Hi ArduPilot community! I am really excited to announce that I have successfully passed the midterm evaluation for my GSoC project. I would like to thank my mentors @rmackay9 and @peterbarker for their valuable feedback and all their help on the project. This blog post is to share our progress on the project.
I have spend around six weeks working on the project. During this period, I worked on developing algorithm to make the vehicle approach towards the docking station and correct its heading while docking (so that the vehicle always docks head-on to the target).
Summary
The new DOCK mode will automatically maneuver the vehicle towards a visual docking target. Currently, we have added support for sending the information of docking target through a camera attached to a companion computer. The companion computer would detect the aruco_marker/apriltag on the docking station and calculate the distance and angle towards the target. It will then send mavlink message to the autopilot containing that information about the target. These messages will be handled by the AC_PrecLand library (the same library which we use to perform precision landing in copter) which will estimate the position of docking target w.r.t. EKF origin. This location will we used by our algorithm in DOCK mode which will maneuver the vehicle towards the dock.
The above image shows a rover and a docking station in a simulated environment (Gazebo sim).
Key highlights
The key highlights of the new DOCK mode are as follows:--
Reverse Docking: We allow the camera to be mounted at the front or the back of the vehicle. The user can mount the camera at the back of the vehicle and set the PLND_ORIENT (newly added parameter which sets the orientation of camera mounted on vehicle body) to 4 (i.e., YAW_180 rotation). This makes the rover to dock in reverse direction. The video in the beginning of this blog post shows a rover performing automatic docking in reverse direction in SITL environment.
-
Heading correction: This is the feature I spent most of my time working on. This feature makes it possible for the vehicle to correct its heading while approaching to the target. With this, even if we switch to the DOCK mode when we are at an angle to the docking target, the vehicle corrects its path and docks head on to the target. I will be discussing about this feature in more detail in later part of this blog post.
-
Scaled slowdown: In order to give the vehicle more time to align itself to the docking target, we have implemented scaled slowdown during docking process. This slows down the vehicle based on two factors:-
- How far is the vehicle from the dock?
- How much is it misaligned from the dock?
These two values help us decide how much should we slow down the vehicle in order to make the docking process mode smooth and precise. To make sure the vehicle doesn’t get stuck, we slow down the vehicle only till a certain speed.
The user can also set the DOCK_SPEED parameter to change the speed limit during the docking process (note that this is the maximum speed with which the vehicle can move in DOCK mode)
Heading Correction
Getting the value of slant of the docking station from the camera sensor is susceptible to noise. The other factors which can influence the accuracy of the reading (of slant of docking station) are:- the distance to the docking station (from very far away we get really crazy values), the resolution of camera etc. We generally set the camera resolution to lower values to get higher fps from the camera and increase the speed of marker detection but this leads to decreased ability to estimate the rotation of marker. Hence, we decided to do it in a different way.We added a parameter named DOCK_DIR which will tell the direction from where the user wants the vehicle to dock at the target. For example, the value of DOCK_DIR set to 180.0 tells the algorithm that the user wants the vehicle to dock to the target while moving towards south (i.e., the docking station is facing towards north). It is a reliable way since it makes the process independent of the camera sensor and hence the factors I mentioned above would not interfere with the process. The only assumption is, the docking station is stationary (which is a good assumption for now).
We have made the heading correction feature optional and it can be enabled by setting DOCK_HDG_CORR_EN parameter to 1. There is another parameter named DOCK_HDG_CORR_WT (heading correction weight) which describes how aggressively vehicle tries correcting its path as it approaching towards the target. A higher value of this parameter will make the vehicle make more efforts to correct its path quickly. We algorithm we use behind path correction is as follows.
Instead of passing the docking target directly to vehicle's position controller, we pass a target which is between the vehicle and the docking station along the line of approach.
The value of DOCK_HDG_CORR_WT parameter tells where should be the position target be on the line of approach at a given time instance. For example, if the DOCK_HDG_CORR_WT parameter is set to 0.75, the position target would be 75% of the distance from vehicle to the docking target along line of approach away from the docking target. As the vehicle tries to approach the target, this target moves towards the real docking target (because the distance of vehicle to dock along line of approach decreases) which brings the vehicle on the line of approach.
The above images explains how the vehicle comes on the line of approach by following the position targets along line of approach.
The above image shows the path of vehicle for different values of DOCK_HDG_CORR_WT parameter.
Note that while correcting the path, the vehicle might lose the marker from its sight. The algorithm works in such a way that we are always able to retrieve the target back. During the window we don’t have target in sight, the vehicle proceeds based on most recent target vector of docking station w.r.t. EKF origin. This value can be used since the dock is stationary and its position with respect to EKF origin can be assumed same throughout the window.
I have also added an autotest for this feature and we expect to get this mode merged into the master soon (after reviews from community).
Note that, the parameter names mentioned above might get changed based on discussions with my project mentors.
What next?
I have also finished building my test rover. Thanks to my mentors and the funding team for providing the funding for the hardware components required to build the rover. I will soon begin tuning it and start testing the new mode. Here is an image of my rover.
I will also be testing the feature on boats (in SITL for now) for various situations (tides and waves etc.) and fix the issues arising while testing.
Please feel free to comment your views on my project. Your feedback is always welcome
