Modified 2018-05-27 by Andrea Censi
TODO for Jacopo Tani: add operation manual, fix bibliographic references, math formatting, various typos
The following was marked as "todo".
TODO for Jacopo Tani: add operation manual, fix bibliographic references, math formatting, various typos
File book/fall2017_projects/16_navigators/20-navigators-final-report.md.
File book/fall2017_projects/16_navigators/20-navigators-final-report.md
in repo duckietown/docs-fall2017_projects branch master commit f551eedd
last modified by Andrea Censi on 2018-09-02 16:46:23
create_notes_from_elements
in module mcdp_docs.task_markers
.Modified 2018-04-29 by Andrea Censi
Video of the final result:
Modified 2018-03-11 by Jacopo Tani
The objective of this project was to implement a method that allows Duckiebots to reliably navigate any kind of intersection they may encounter when driving through a regular Duckietown.
Motto:
TRANSIBITIS (you shall pass)
What is in scope:
Modified 2018-03-11 by Jacopo Tani
We seek to find a method that allows a Duckiebot to safely navigate an intersection. Duckiebots navigate through regular streets in Duckietown using a lane following method. It includes a localization method but also a position control method. Based on this scheme, the need for a controlled crossing of intersection emerged. The main motivation is therefore based on the safety and the reliability of intersection navigation. Ideally, the vehicle should be able to not only go from exit 1 to exit 2, but also recognize where it is relative to those 2 points (localization). Once the Duckiebot receives information about its current position, it will be able to correct any mistake relative to a precalculated path.
Modified 2018-03-11 by Jacopo Tani
The project starts from the current solution, in which the Duckiebot navigates the intersections in open loop. In this solution, after arriving at an intersection, the Duckiebot uses the AprilTags to know the kind of intersection and the feasible exits. Then it randomly chooses one of them and executes standard commands to navigate. This solution did not include localization implying that the current position of the vehicle was not known during the intersection navigation. Since one of the objectives of the project was to use the controller, that the Controllers group designed, the open loop implementation could not be used any further. For practical reasons, we almost started from scratch, and the new solution would have very few common points with the previous implementation.
So our main improvement to the current solution is the use of the camera to introduce vision during navigation. This allow us to introduce feedback into the system with all the benefits that closed loop control has with respect to open loop, and so regulating the control inputs based on the information of the system state.
Modified 2018-03-11 by Jacopo Tani
As mentioned previously, the drawback of the existing solution is the missing information about the Duckiebot’s position during the navigation. The result is that the system inputs, linear and angular velocities are independent of the system state so that the navigation is not robust and the percentage of failures is high (around 50% for some group members’ Duckiebots for short right turns).
Our main contribution is to use the camera as well as a state estimator to estimate the absolute position of the robot with respect to the intersection. Moreover, we compute a path to the desired exit. The planned path is a cubic spline computed with two control points and directions, placed in the initial and final desired intersection positions.
The localization is done by comparing the images from the camera with a template of the intersection. Namely, edges are detected in the current image and some control points are defined so to minimize the distance between their 2D projections in the image frame and the detected edges. In this way, we can estimate the initial position and localize our robot during the navigation.
Reference paper, from which we took inspiration is J. Barandiaran, D.Borro, “Edge-Based Markerless 3D Tracking of Rigid Objects”, IEEE Conference on Artificial Reality and Telexistence, 2007.
Modified 2018-03-11 by Jacopo Tani
Modified 2018-02-22 by Theodore Koutros
We seek to find a method that allows a Duckiebot to safely navigate an intersection. In particular, we attempt to solve the following problem: Given that the Duckiebot finds itself at rest at an intersection, 1) Initial localization of the rest position, 2) compute a path that guides the Duckiebot to the desired exit while respecting the Duckiebots’ system dynamics and control input limitations, 3) Continuous localization thanks to a state estimator which integrates wheel commands and pose updates from images, the same method of the initial localization is used frame by frame, 4) track the path providing localization feedback to the lane controller, and lastly 5) detect when the maneuver is finished and the Duckiebot finds itself in a regular lane.
Modified 2018-02-28 by tanij
Modified 2018-04-29 by Andrea Censi
Modified 2018-02-22 by Theodore Koutros
Success rate, i.e. the percentage of trials for which the Duckiebot ends up in the desired lane and successfully hands over the control to the lane following controller. A trial is considered to be successful if the Duckiebot is completely inside the desired lane without touching any lane markings. The success rate is evaluated by simply performing the intersection navigation task N times and counting the number of successful trials. Accuracy and precision of final state, i.e. how close is the Duckiebot’s state relative to the desired final state and how repeatable is this. The accuracy and precision of the final state is estimated using the existing lane detection method, and is measured for different initial conditions. Duration, i.e. the average time required for the Duckiebot to cross an intersection and an upper limit (worst-case) on the time required. The average duration is computed by running a series of N experiments.
Modified 2018-04-29 by Andrea Censi
The intersection navigation is started as soon as the Duckiebot is told that it is in front of an intersection. The following functions are then executed (in chronological order): The Duckiebot localizes itself with respect to the intersection, given the intersection type. The Duckiebot waits until it receives a message “turn_type” indicating which exit of the intersection it should take, and a message “go” indicating that the navigation can start. A path is planned that guides the Duckiebot from its current location to the desired intersection exit. The lane following controller, adapted for path tracking, steers the Duckiebot to its final location. During the navigation, the Duckiebot continuously localizes itself and feeds the estimated pose (i.e. the distance from the desired path and the relative orientation error) to the lane following controller to account for disturbances or modelling errors. * The Duckiebot detects when it traversed the intersection, i.e. when it finds itself again in a regular lane, and hands control back to the lane following controller by publishing on the topic “intersection_done”.
It is assumed that: the Duckiebot stops between 0.10m and 0.16m in front of the center of the red stop line, i.e. $d_x \in \lbrack 0.1m,0.16m\rbrack$, has an error of no more than 0.03m with respect to the center of its lane, i.e. $d_y \in \lbrack-0.03m,0.03m\rbrack$, and that the orientation error is smaller than 0.17rad, i.e. $\theta\in\lbrack-0.17rad,0.17rad\rbrack$ (see Fig. 4 for details, all values are with respect to the origin of the Duckiebot’s axle-fixed coordinate frame). a lane following controller exists that takes as inputs the distance from desired path $d$ and the orientation error with respect to the path tangent $\theta$ (see Fig. 5 for details). This is done by the new lane following controller. However, we needed to slightly modify the controller to account for thresholds wheels’ speed.
Modified 2018-02-28 by tanij
Two nodes were developped: “intersection_navigation” and “intersection_localization”. In the following, their functionality and interfaces will be described in detail.
“intersection_navigation”-node
The “intersection_navigation”-node is responsible for the high level logic of navigating the Duckiebot across an intersection, planning paths from the Duckiebot’s initial position to the final position, estimating the Duckiebot’s pose and communicating with the lane following controller. It subscribes to the following topics:
The “intersection_navigation”-node publishes on the following topics:
“intersection_localizer”-node
The “intersection_localization”-node is responsible for localizing the Duckiebot at an intersection. For this purpose, it subscribes to the following topics:
The ”intersection_localizer”-node publishes the following topic:
Modified 2018-02-28 by tanij
There are two main algorithms in our implementation about localization and path planning respectively
The algorithm is composed by the following steps:
\begin{equation} W=min\sum_i (A_iW-l_i)^2 \end{equation}
Where A is the nx3 matrix which contains the n control points, W is the motion vector defined as $W=\lbrack w_z t_x t_y \rbrack^T$ where we can obtain the Rotation matrix from the vector w applying the Rodriguez’s formula. And l_i are the displacements between the projections of the control points and the edges detected in the image.
Then we obtain the new pose from W, which tells us the relative position and orientation between two consecutive poses, and the previous pose.
The path planned to traverse an intersection is a polynomial of order three. The polynomial coefficients are chosen such that the path starts at the Duckiebot’s current pose (i.e. position and orientation) and ends at a desired pose. However, this only defines the coefficients partially. In particular, the orientation of the Duckiebot only determines the direction of the velocity at the initial and final position, but not its magnitude. The magnitude of the initial and final velocity are thus optimized to minimize the curvature of the path. During the optimization, it is also verified that path does not contain any loops and that it satisfies the Duckiebot’s maximum curvature, i.e. only feasible paths are planned.
Modified 2018-03-11 by Jacopo Tani
Modified 2018-06-25 by Andrea Censi
All the experiments are taken in a duckietown with appearence in accord to appearance specifications. It is a Duckietown with 3 and 4-ways intersections and intersection April Tags well visible. We consider an experiment is valid, when the Duckiebot correctly stops at the red line. The term correctly refers to the thresholds defined in section 8 Logical Architecture. We let the Duckiebot navigate Duckietown for 2 runs of 30 minutes randomly choosing the exit to take.
Success rate: Our implementation has success rate of 80% for the upper left turns and for the straight exit, whereas a lower rate of 65 % for the short right turn. The main failures are: right wheel touches the track boundary white line for the upper left turn, left wheel touches the middle dashed line for the straight exit and the right turn. Mainly the lower success rate of the right turn is due to the fact that in such short maneuver the feedback controller cannot compensate, factors as wheels slippage and inaccurate kinematic calibration. However, our implementation improves the current solution, in which the Duckiebots hits a lane marking 50% of the times and often fails in navigating the short right turn.
Accuracy and precision: We define a final state as accurate when the Duckiebot finds itself in lane after the intersection navigation is done. Our results show that 95% of the times that the navigation is concluded the robot detects itself in lane and successfully switches to the lane following control. To note that, if the path is concluded but the robot does not find itself in lane, it slows down and goes straight for 2 seconds. If it finds itself in lane during this time, we hand back the control over to the lane following controller, otherwise the robot stops.
Duration of the intersection: The time is computed from when the Duckiebot arrives at the red line, the fsm mode is at “intersection_coordination”, and the intersection navigation is done, publishing of the topic “intersection_done”. The average time is 17 seconds and the upper limit (worst-case) is 21 seconds.
Moreover, we estimate the accuracy and precision of the estimated pose during traversing the intersection with a visualizer node, which can be run on the laptop. The visualizer node outputs the images from the camera and the edges projections from the estimated current pose.
The time between when the Duckiebot stops at the red line and when it is ready to start the navigation as well as the pose estimation accuracy are the biggest challenges, where mainly our solution may be improved.
In the next section, we give some insights about a possible ways of improvement.
Modified 2018-03-11 by Jacopo Tani
The main improvements can be done about the accuracy of the localization, which will also have a positive impact on the computation time.
Specifically, our localization algorithm is very sensible to the camera calibration. Since the calibration matrices are used in the 2D projection of the control points in the image frame, with not adequately good calibration, the optimization problem will minimize quantities that are affected by offsets. In order to compensate for it we optimize, as described in section 8 Algorithms, over a range of initial conditions, but this increases the computation time.
A solution could be to improve the camera calibration procedure and introducing metrics to evaluate its performance. It would allow to decrease the range of initial positions used in the least squares optimization and so to have benefits on both localization accuracy and computation time.
No questions found. You can ask a question on the website.