Update path_follow to use heading

[Ezward]

The current path_follow algorithm searches for the nearest waypoint to it's current position, then chooses the next waypoint in the path (the target waypoint) and uses those two points to create a line; it then determines which side of the line it is on and how far from the line it is and uses that as the input to a PID that decides which direction to turn in and how much to turn. Throttle is held constant (based on configuration).

Note that the input to the PID is the cross-track error. The vehicle's current heading is not considered.

Modify the code to keep an estimate of the vehicle's heading. Use this heading estimate to calculate the heading error relative to the line connecting the vehicle and the target waypoint (the next waypoint after the nearest waypoint, as described above). Use that heading error as the input to the PID that controls steering. So for instance, if the car's heading would take it to the left of the target waypoint, then we would want to turn right to point at the target waypoint. If the car's heading would take it to the right of the target waypoint, then we would want to turn left to point at the target waypoint. As long as we are always pointing at the target waypoint and we are making progress, then we will achieve the waypoint.

[TCIII]

@Ezward,

An excellent plan. I believe that the inclusion of Heading in the steering solution will help to maintain vehicle tracking on long straightaways and help to prevent off track drifting.

TCIII

[TCIII]

@Ezward,

I am TCIII and I have a Traxxas E-Maxx Truck Chassis test vehicle running the DC main branch and using the path_follow.py template that employs input from a Sparkfun NEO-M9N GPS module with an actve 32db gain antenna.

I will be available to test and validate any code branches that add velocity, heading, and IMU input to the GPS CTE input mix.

Regards, TCIII

[aldenyue]

@Ezward Hi Edward, me and group of students at UCSD are using Donkey Car and would like to help out with this issue. We plan on using a Sparkfun NEO-M9N GPS module to obtain heading.

[TCIII]

@aldenyue,

Besides the Sparkfun NEO-M9N on my Traxxas E-Maxx test vehicle, I have a Robo HAT MM1 that includes a mpu9250 IMU.

I tested it recently and all of the sensors are fully functional.

TCIII

[TCIII]

@aldenyue,

Have you made any progress adding Heading to the DC path_follow template?

TCIII

[aldenyue]

@TCIII

We are trying to figure out how to get accurate heading from our Sparkfun NEO-M9N. Have you head any success in getting accurate heading measurements? We are thinking of fusing sensor data from our GPS and a separate IMU.

Alden

[TCIII]

@aldenyue,

From the UBlox NEO-M9N Integration Manual "The NEO-M9N firmware does not support the dead reckoning position fix type." I take this to mean that you cannot determine "Heading" (dead reckoning) when using the NEO-M9N.

Sparkfun does have a "dead -reckoning" GPS available, however it utilizes the older NEO-M8U GPS which is somewhat inferior to the NEO-M9N. "The NEO-M8U takes advantage of u-blox's Untethered Dead Reckoning (UDR) technology."

I was expecting to integrate an IMU with the GPS position data since we already have the NEO-M9N GPSs. My GPS testbed vehicle is equipped with a mpu9250 IMU mounted on a Robo HAT MM1 M4 besides the Sparkfun NEO-M9N GPS.

I am not a Python coder, but do understand Python structure enough to make on the fly corrections to python programs when there are obvious errors.

I am a System Engineer and will be glad to work with you to integrate IMU sensor fusion into the present GPS path_follow template.

Here is a very good tutorial on improving lateral control when employing a basic CTE algorithm.

A little research into GPS + IMU sensor fusion:

1) Link 2) Link 3) The whole shebang 4) Link

Observations: Most GPS +IMU sensor fusion seem to implement a Kalman Filter. The GPS location is a Gaussian distribution about the vehicles actual location.

TCIII

[Ezward]

@TCIII

We are trying to figure out how to get accurate heading from our Sparkfun NEO-M9N. Have you head any success in getting accurate heading measurements? We are thinking of fusing sensor data from our GPS and a separate IMU.

Alden

@aldenyue You might try estimating heading using the last two or so positions you get from the gps. The NMEA sentences also contain a heading estimate; see the GPRMC/GNRMC sentence that we decode to get the (x,y) position; http://aprs.gids.nl/nmea/#rmc It also provides a field called "True course" or "Track made good in degrees True"; For a ground vehicle this is equivalent to heading (provided you are not drifting!). Note there is also "Speed over ground in knots" for velocity if you care and want to do some closed loop speed control. Note that this course over ground is only as good as the GPS hardware that estimates it; so for a GPS that does not have an integrated IMU then it may be of similar accuracy and latency to the using the last two known GPS points.

This can be made much better by integrating an IMU, especially if it has a magnetometer and you can isolate the magnetometer from the variable magnetic field generated by the motor and servo (hint; put it on a mast).

In either of these cases you may want to interpolate changes in pose (position and heading) between GPS fixes by using a kinematics model. So if you know the velocity and steering angle you can estimate where the car will be. There are primitives for this in this branch; https://github.com/autorope/donkeycar/blob/921-gps-logger/donkeycar/parts/kinematics.py

I would probably start with the heading and velocity estimate from the GPS NMEA sentence or calculate them from the last two known gps points, then implement the algorithm to point the car at the target waypoint (so make the difference between the car's heading and the angle to waypoint to be zero), then implement the IMU so you can do faster updates.

If you decide go use Track made good and/or Speed over ground, then this is the code the parses the NMEA sentence; https://github.com/autorope/donkeycar/blob/a1e003c7a9bf7954b56684baec7b8b1717c28d66/donkeycar/parts/gps.py#L182 It currently parses out the position only, but could be modified to return heading and velocity in meters/sec based on the content of the NMEA sentence.

[TCIII]

@Ezward, How do you figure that "The NMEA sentences also contain a heading estimate; see the GPRMC/GNRMC sentence that we decode to get the (x,y) position;"? When: "The NEO-M9N firmware does not support the dead reckoning position fix type."

TCIII

[TCIII]

@aldenyue,

Any progress this week?

TCIII

[aldenyue]

@TCIII

Sorry but we won't be able to make any more progress :(. Our group doesn't have enough time this quarter to figure out heading for our final project, so we're pivoting to a different idea that we'll be able to finish.

Alden

[Ezward]

@Ezward, How do you figure that "The NMEA sentences also contain a heading estimate; see the GPRMC/GNRMC sentence that we decode to get the (x,y) position;"? When: "The NEO-M9N firmware does not support the dead reckoning position fix type."

TCIII

It's explained in the comment; at the worst it can just use the last two gps readings to estimate course over ground.

[Ezward]

Note that the kinematics models have been landed in the main branch at https://github.com/autorope/donkeycar/blob/main/donkeycar/parts/kinematics.py. The Bicycle model has been tested with the path_follow template and works well. The Unicycle model is not well tested in the real world but has unit tests.