Evolve the GPS path follow template

[Ezward]

We have implemented a template that used GPS to record a path and follow it. This is the issue #991 Add gps part and integrate into path_follow template describing the work and what is left to do. This is the branch that implements it 921-gps-logger. As noted in issue #991, there are three steps for getting this merged into the main branch. The first of these steps is included below. As these are merged the list will be updated with testing for the subsequent PRs.

1. [x] Test refactor of complete.py template (The DNN template)

   • Success criteria; The refactored complete.py should work the same as the current main branch (no new functionality was added, this is just a refactor).
   • Any instructions in docs.donkeycar.com should work. If there are discrepancies then we should open an issue to either change the code or update the docs at the time we merge this branch into main.
   • Report any bugs (add issues in github)
   • Work with developer to fix bugs and close issues.
   • do this for the other 2 stages are described in issue #991 as PRs become available.

2. Experiment with various GPS receiver technologies

   • compare RTK, RTK with dead reckoning and non-RTK, non-RTK with dead reckoning gps receivers. See this article for how to setup RTK for this purpose
   • how many laps (how much time) can you go with non-RTK gps before drift inteferes?
   • Which technologies perform better in a time trial?
   • what is the highest performing setup?
   • what is the most cost effective setup?

3. Improve path follow 1

   • The current algorithm uses cross-track error and a PID to turn towards line to follow; no heading is involved.
   • How do optimal PID values vary with velocity?
   • What recommendations can we make for starting PID values and for a process to identify optimal values?
   • Observe the behavior of the vehicle if it moves too far way from line to follow; the vehicle will get stuck in a tight turn an never achieve the line to follow. Why does this happen? What changes could be made to recover from this state? See https://github.com/autorope/donkeycar/issues/1044 for a related project.
   • What happens if the path to follow crosses itself? What changes can be made to reliably drive on such paths? See https://github.com/autorope/donkeycar/issues/1043 for a related project.

4. Improve path follow 2

   • The current algorithm uses cross-track error and a PID to turn towards line to follow; no heading is involved.
   • How could we add in heading?
     • software only (delta of current and previous GPS location).
     • Use heading provided by GPS (see GPRMC NMEA message's field 8, Track made good in degrees GPRMC)
     • hardware 6 axis acc/gyro like MPU6500
       • This can provide a relative heading at a high refresh rate; so another method that provides the absolute heading would be needed, then this can be used to provide a more accurate heading estimate in between updating the absolute heading.
     • hardware 9 axis acc/gyro/mag like MPU9250 (see https://quadmeup.com/mpu6000-vs-mpu6050-vs-mpu6500/ for comparison)
     • wheel encoders and a kinematic model.
       • Like 6 axis acc/gyro, this can be use to provide high frequency updates, but should be paired with a more accurate heading source.
       • Note there is code for kinetics and speed control in parts/tachometer.py, parts/odometer.py, parts/kinematics.py, parts/velocity.py, but it needs work. See https://github.com/autorope/donkeycar/issues/948#issuecomment-1272369801
   • How do these complement GPS? How would data from these sources be use in conjunction with GPS positions?
   • Implement one or more of these sources of heading and add this to the vehicles current pose estimate. (x, y) -> (x, y, heading). Then modify the path follow algorithm to heading error as input to the PID rather than cross-track error. See https://github.com/autorope/donkeycar/issues/1048 and https://github.com/autorope/donkeycar/issues/1060 and https://github.com/autorope/donkeycar/issues/1092 for related projects.

5. Improve path follow 3

   • Once we have heading, we can use more sophisticated path follow algorithms, like pure pursuit.
   • implement pure pursuit using one or more of the technologies identified in Improve path follow 1&2 and using a constant velocity.
   • Ideally make this the path follow logic 'pluggable' so that in configuration we can specify a python file that contains the logic, so individuals can develop their own algorithms and use them without having to fork the repo. We do something similar for game controllers where we can configure the code to look in the mycar folder for a custom controller implementation.
   • how does pure pursuit compare to cross-track error; what are benefits and how are failure modes different?

6. Pure Pursuit with waypoint velocity

   • Rather than each waypoint being a simple (x,y) coordinate, add a target velocity to achieve at that waypoint (x, y, velocity).
   • If wheel encoders are available, then closed loop velocity control can be added to achieve these target velocities.
   • In the absence of wheel encoders, a software only approach where velocity is calculated from the current and previous pose estimate.
   • develop a control algorithm to accelerate/decelerate to achieve the target velocity at each waypoint.
   • how precise does the velocity estimate and control need to be to deliver benefits?
   • compare this to cross-track/pid and constant velocity pure pursuit;
     • which can achieve lower (faster) lap times?

7. Pre-calculate an optimal trajectory

   • given a map that indicates the driveable area (perhaps as paths for the left and right borders of the track), calculate an optimal path for the vehicle to follow.
   • try minimizing distance traveled in one lap
   • try minimizing curvature of path in one lap
   • try maximizing single-point velocity that can be achieved.
   • The TUM racing team github has several repositories that implement trajectory optimization. This one global_racetrajectory_optimization and GraphBasedLocalTrajectoryPlanner look the most interesting.

[TCIII]

@Ezward,

An excellent path forward in order to integrate both heading and IMU functions and possibly wheel encoders with GPS be it just GPS or GPS + RTK. I presently have a Traxxas E-Maxx Truck chassis running a Rpi 3B+/Robo HAT MM1 M4 configuration. The Robo HAT has a built in MPU9250 at hex address 0x69. Unfortunately it will be difficult to add wheel encoders to my E-Maxx. However I do have a large 6WD chassis with wheel encoders and a small 6WD chassis with motor wheel encoders. The 6WD chassis are not fast, but I have used both wheel encoders and GPS to validate the path_follow.py template from the DC 921-gps-logger branch.

Here is a link to a video of my Traxxas E-Max running a Sparkfun NEO-M9N GPS with an active antenna on a ground plane. At the time the video was shot the E-Maxx batteries were running low and a cloud cover was coming in as the E-Maxx had performed better during previous runs.

Regards, TCIII

[Ezward]

IMU pose estimation and IMU/GPS fusion resources:

• https://pypi.org/project/droneposelib/. dronepose lib is a python library that will make pose estimates based on IMU data.
• https://pypi.org/project/imusensor/ library to make reading the IMU easy. Look at the acknowledgements at the bottom of the page.
• https://github.com/micropython-IMU/micropython-fusion micro-python library implements Madgwick
• https://github.com/xioTechnologies/Fusion AHRS fusion in C with Python bindings. Includes some error rejection.
• https://github.com/smahajan07/sensor-fusion Python fuse GPS with IMU for position and orientation estimation
• https://ahrs.readthedocs.io/en/latest/index.html Collection of Python AHRS (Attitude and Heading Reference Systems) methods for combining IMU and GPS etc.