## The Problem (TL;DR) ##
My robot used to travel in straight lines between point A and point B. Now it doesn’t. I want to figure out why and how I can make it do that again by forcing the local path planner to follow the global path as closely as possible. I've done my best to summarize the problem here, but if you don't want to read an essay skip to the solution to this ROS question where I've re-stated the problem more clearly, and the work-around.
-----------
## The Problem (full explanation) ##
If a robot travels directly between two points and there are no obstacles in the way, then its trajectory between those points should be a perfectly straight line (in theory). My robot used to behave like this, as shown below:
And now it doesn't, it consistently follows a bowed/curved trajectory every time. The radius of the curve changes but otherwise the trajectory is identical. The final stretch back to the starting point also has a funny kink it. Example:
Now before I go any further, I do understand that this problem is largely aesthetic and doesn't matter. And I know that I could work around this by adding more goal points and breaking the longer lines down into a series of shorter ones. However the fact remains that I can't currently explain this behaviour and I'd like to solve the problem if only for my own education.
So I would like to correct this behaviour and answer a few questions
Here is all the relevant system information:
## System Information ##
- **OS:** Ubuntu 14.04 (laptop), ROS Indigo Igloo (robot)
- **Robot:** Clearpath Robotics Jackal UGV (real, not simulated)
- **Sensors:** IMU, wheel encoders, high accuracy RTK GPS
- **Localization:** package used is [robot_localization](http://wiki.ros.org/robot_localization). There are two instances of ekf_localization_node (local and global). IMU and encoder data is fused in the (local) /odom frame. IMU, encoder and GPS data is fused in the (global) /map frame - this is the frame /move_base uses for navigation. I can provide the specific launch files if requested.
The output from the global EKF is always accurate and doesn't drift. The local output accumulates error over time and tends to drift quite badly by the end of a run.
- **Navigation:** I use the [jackal_navigation](http://wiki.ros.org/jackal_navigation) package (version 2.2.2) to set up my navigation stack. I use the [odom_navigation_demo.launch](https://github.com/jackal/jackal/blob/indigo-devel/jackal_navigation/launch/odom_navigation_demo.launch) with the following parameter changes to [local_base_planner](http://wiki.ros.org/base_local_planner) and [costmap_2d](http://wiki.ros.org/costmap_2d):
1. xy_goal_tolerance increased to 0.35 m
2. heading_scoring = true (false by default)
3. Costmap size increased to 120x120 m
4. All instances of any "global_frame" parameter have been changed to /map.
5. All map (local and global) "update_frequency" parameters reduced from 20 Hz to 10 Hz
6. Reduced "controller_frequency" and "planner_frequency" parameters from 20 HZ to 10 Hz
To the best of my knowledge, these are the only parameters that have changed between the two graphs shown above.
The full navigation parameter files are [here](https://drive.google.com/open?id=0B1KZT92BcdVNZ2xVdXN6QlBycFk)
Currently, in all cases the global path is perfectly straight, but the robot never seems to follow it directly. I've tried tweaking the various trajectory scoring parameters in the [local_base_planner](http://wiki.ros.org/base_local_planner) config files but I haven't found any combination that provides a better outcome than the default values, possibly because I don't fully understand what each scoring parameter is changing.
- **Frame tree:** [tree](https://drive.google.com/open?id=0B1KZT92BcdVNYlZTT1JUc0VBYW8)
- **Node graph:** [node_graph](https://drive.google.com/open?id=0B1KZT92BcdVNSy1sNm4ybmNTWnc)
- **Localization launch file:** [localization.launch](https://drive.google.com/open?id=0B1KZT92BcdVNX29lbHY1V3dQVVU)
- **Example bagfile:** [Bagfile for 2nd graph](https://drive.google.com/open?id=0B1KZT92BcdVNOVhqWDhhcldPWFU)
## Possible Causes ##
I have several working theories on the cause of this problem:
1. **The wheels on one side of my robot slip.** If the wheels on one side weren't engaging properly this could explain why the robot leans to one side. However if this were the cause I would expect the robot to consistently pull to one side and one side only, which is not the case. In addition, when I manually drive the robot in straight lines it doesn't noticeably drift. The reason I'm considering this is because I have already had an issue with a wheel slipping on one axle, which has been replaced.
2. **The navigation parameters for local_base_planner are not tuned.** Which could cause the local base planner to not follow the global path properly. See the next section for questions I'd like to answer that may help with this.
3. **Trajectories are being calculated in the /odom frame.** I'm sure [base_local_planner](http://wiki.ros.org/base_local_planner) does this by default. If this were the case, and the /odom frame had drifted relative to /map, then a straight trajectory in the /odom frame would translate to an angled path in the /map frame. This could explain the drift I'm seeing. I've done my best to replace every mention of "/odom" in the config files with "/map" to try and force /move_base to do everything in the /map frame but it hasn't improved the trajectory.
Over the coming days I'll be trying a few other things like fusing GPS data in the /odom frame and periodically re-zeroing to match the /map frame. But I'm not sure they'll fix the problem I wasn't doing them previously when the trajectory was straighter.
## Questions I Want to Answer ##
1. In the trajectory scoring parameters for [base_local_planner](http://wiki.ros.org/base_local_planner) the explanation for *pdist_scale* says "The weighting for how much the controller should stay close to the path it was given" Does this refer to the global path or some other path?
2. What parameter dictates which reference frame local_base_planner calculates trajectories in? I'm pretty sure it's *global_frame_id* but I've set this to /map and it didn't improve the trajectory at all.
3. In general, what parameter changing/tuning advice do people have to make the local path adhere more closely to the global path?
Thank you in advance to anyone who can help answer these questions. I apologize profusely for the lengthy essay, but there's just no short way to convey this kind of problem *and* the context needed to solve it.
## Updates to the Problem ##
**UPDATE 1:** I'll post all new information in its own section for clarity. I have tried two solutions, both have offered only marginal improvements to the trajectory of the robot. However I have also discovered another possible cause.
First, I tried to periodically re-set the odometry in the /odom frame to match the odometry in the /map frame. I didn't complete a full run before the localization completely bugged out and started its "I'm hopelessly lost so I'll rotate on the spot endlessly" behaviour. Here is the trajectory for comparison: [Actual Trajectory (odom resetting) ](https://drive.google.com/open?id=0B1KZT92BcdVNa2Z5RVFsV1VBQkk) and here is what all the local/global/gps odometry streams look like relative to their internal reference frame (i.e. in X and Y terms, not UTM Northing/Easting): [Internal Odometry (odom resetting)](https://drive.google.com/open?id=0B1KZT92BcdVNd0FoR1pNOXNQXzQ)
Second, I tried to fuse GPS data in the /odom frame to (in theory) make it identical to the /map frame odometry and therefore just as accurate. It did offer slight improvement: [Actual Trajectory (gps fused in odom)](https://drive.google.com/open?id=0B1KZT92BcdVNZExwVzduU1NYOGM). What is worrying though is that the local data in the /odom frame and the global data in the /map frame were still different: [Internal Odometry (gps fused in odom)](https://drive.google.com/open?id=0B1KZT92BcdVNd0Y1QTR1MjlnX3c). I'm still investigating this.
In addition, I discovered that the data coming from the wheel encoders (in the topic /jackal_velocity_controller/odom) is at a very large angle to all the other data:
This data is fused in both my local (/odom) and global (/map) EKFs. Based on my experience with fusing GPS data in the global reference frame, if you fuse two data streams that are at angles to one another, you are going to have serious problems. Both the local and global EKFs are fusing the X and Y velocities from this data, and because these velocities will be at an angle to all the other sensor data this is quite likely to be what is pulling the local EKF off course. Unfortunately, it looks like this was also happening when the data for the very first graph (the one with nice straight lines) was recorded. Yet it obviously didn't affect the trajectory in that case, which is confusing.
The topic with this data in it - /jackal_velocity_controller/odom is set up by Clearpath and is published by a ROS node that runs by default on the UGV. However, as I said I have replaced one axle on the robot so it's possible that there was a encoder calibration process that I messed with when I replaced it. Or perhaps this is normal and the ekf_localization node accounts for this angular difference at some point, I don't know enough about how the source code works to say. I'm continuing to look into all this, but any advice or explanation on this particular issue would be very welcome.
**UPDATE 2:** I ran a rough test to confirm the behaviour of the odometry in /jackal_velocity_controller/odom. I started the UGV at a central point and then drove it in a (roughly) straight line in each of the cardinal directions as show below (re-booting the UGV between each run to reset the encoder odometry.
As you can see, with one exception the encoder odometry reports odometry starting in the positive 'X' direction. So even if the robot is travelling in a different direction (say, positive 'Y' direction), the encoder velcities I'm fusing will be showing it moving in a positive 'X' direction. But this doesn't seem to be having a significant effect on the local EKF which still confuses me. Obviously there is something critical that I'm missing about the way the EKF handles the data.
**UPDATE 3:** In response to @ahendrix and @MarkyMark2012 comments:
@MarkyMark2012: to the best of my knowledge, the only thing I have changed (in the localization/navigation packages) is the parameter changes I've listed in section "System Information > Navigation" above.
@ahendrix. It sounds like the EFK isn't handling the odometry data quite the way I think it does. To clarify, this is the relevant code in my launch file for both the local and global EKFs:
[false, false, false,
false, false, false,
true, true, true,
false, false, true,
false, false, false]
And the resulting trajectory (for a single pass, this one is straighter than most) looks like this:
I'm fusing the X/Y velocities, and my understanding is that in this case the encoder data is indicating that the X velocity is large and positive (moving to the right) while the Y velocity is small and positive. All other data indicates that the robot is moving up which contrasts with the direction the encoder velocity is implying. So in theory, this will encourage the EKF to propagate the position estimate to the right, following the encoder data. This doesn't seem to be consistent though, and like I said elsewhere, the encoder data has been doing this for as long as I've been recording bagfiles, but didn't affect it previously.
Regarding tuning the PID controller, that's a really good suggestion, but there doesn't seem to be any explicit PID parameters defined for the [/move_base](http://wiki.ros.org/move_base?distro=kinetic) or [/local_base_planner](http://wiki.ros.org/base_local_planner) packages. The closet equivalent seems to be the *pdist_scale*, *gdist_scale* and *occdist_scale* parameters in the /local_base_planner package, is that what you're referring to?
I'm running some more tests today to further investigate the issue so I'll try to update this again soon.
**UPDATE 4:** I tried increasing *pdist_scale* in the [/local_base_planner](http://wiki.ros.org/base_local_planner) package to 1000, which according to the [navigation tuning guide](http://wiki.ros.org/navigation/Tutorials/Navigation%20Tuning%20Guide) should force the local plan to follow the global plan more closely. As you can see it didn't help, if anything making the trajectory worse: [odometry_with_pdist=1000](https://drive.google.com/open?id=0B1KZT92BcdVNLXVaS2REWTIwTjg)
Looking at the simulated data in RViz I did notice that the local path seems to stick a little closer to the global path, and the global path still points straight at the end goal point. But the robot still seems to drift. Other than the wheel encoders messing with the EKF I'm running out of leads to chase.
**UPDATE 5:** Just realized I had also altered the various map/controller/planner refresh frequencies from 20 Hz to 10 Hz. I did that many months ago to reduce warning messages about the controller/planner not meeting its refresh rate target. I'm not sure how this could affect the performance of the robot's trajectory but I'm not taking any chances so I'm looking into it now.
**UPDATE 6:** Based on [this](http://answers.ros.org/question/209287/base_local_planner-does-not-follow-the-global-plan-accurately/) ROS question I've slowly reverted some of my navigation parameter changes to try and make things run a bit more smoothly. I've changed the format of my charts to make them a bit more readable and added a "desired" trajectory to better show the drift.The changes (accumulative and in order) were:
1. Reduced the map size (120x120 -> 80x80). [Result](https://drive.google.com/open?id=0B1KZT92BcdVNRkh5ckxtcUtjTlU). No significant improvement.
2. Increase all map/controller/planner frequencies (10Hz -> 20Hz). [Result](https://drive.google.com/open?id=0B1KZT92BcdVNcHhsTGRxc1RLWVE). No significant improvement.
3. Reduced speed (1m/s -> 0.5m/s) and increased EKF freq (20Hz -> 30Hz). [Result](https://drive.google.com/open?id=0B1KZT92BcdVNTUkwMkJ6R3dHUG8). Biggest improvement is from reducing the max speed but waiting for a slow robot gets annoying quickly.
Many of the frequency changes were to get rid of warning messages about the map missing its update rate (because of my massively increased map size). Either way it doesn't seem to be the silver bullet I (briefly) hoped it might be.
**UPDATE 7:** I couldn't find information coming from the encoders directly (I think that Clearpath software handles it and gives the result to ROS) but I have been looking at the command signal coming from the [/move_base](http://wiki.ros.org/move_base?distro=kinetic) node, which is published to the /cmd_vel topic:
The top graph shows the actual vs desired trajectory of the robot, the key part here is obviously the blue line curving when it should follow the red (Sorry about the poor presentation). The bottom graph shows the content of /cmd_vel during this part of the trajectory. They key part here is the angular component - a positive value commands the robot to turn *left* and a negative value commands it to turn *right*. The large spike is where the robot is turning the first 90 degree corner.
Honestly this doesn't feel that helpful, because it looks like the robot is largely following the commands it is send. i.e. sharp left turn, straight, then slow left turn. I'm not sure if this means that the problem is happening downstream of the [/move_base](http://wiki.ros.org/move_base?distro=kinetic) node or not.
↧