# What concepts does Rover code use to steer between 2 waypoints and stay on track

My main question relates to driving an autonomous differential drive rover between 2 waypoints and staying on the track between the two waypoints

I have built an autonomous grass mowing machine for my garden. We wrote the code from the ground up. I have used pixhawk in the past but could not get it to work for our needs. Its written in C++ Arduino code. Coding is not my speciality.

It is set up to mow lanes that are 17 inches apart due to the blade size. All the software does is drives our chassis up a lane to a waypoint and then makes a 180 degree turn and drives down the next lane to the next waypoint which is located 17 inches over to one side of the first waypoint.

It uses a pair Ublox C94 M8P RTK gps with a reliable AHRS compass (with very little distortion due to magnetic or ferrous material). The vehicle follows waypoints by calculating the difference between the target heading (maths and the gps) and the actual heading (compass) and it uses the difference to send a value in a drive command to the motor controller that alters the speeds to the 2 driving wheels. this variable is called turnSpeed. the chassis is differentially driven.

After many months of work the mower drives well. The heading errors are generally below 5 degrees. However if the mower for some reasons gets off the straight line track between the 2 waypoints the code does not do a good job of getting the mower back on the straight line track quickly as it is aiming at a distant waypoint. In short, it is good at heading to a distant point but not good at getting on the straight line track between the 2 way points and then heading to the point

In order to solve this we started calculating a cross track distance error. And we use a set point of 0 cross track error and a PID to try to get the vehicle back on the straight tract between the two waypoints.

The problem now is that we have 2 inputs to the motor command variable turnSpeed. One from the heading error and the other from the PID. So this is getting real confusing.

Normally I would use the heading error variable if the heading error is greater than 2 degrees. To get the PID tuned I eased off the heading error so that the heading error only works if the heading error is more than Â±20 degrees. I set Ki and Kd to zero and increased Kp until the mower went in to a stable oscillation.
I then timed the period of the oscillation for 1 complete wavelength it was 6 seconds.

I then used the following

New Kp= Kp*0.60
New Ki = Period/2
New Kd = Period/8

this gave Kp=5
ki= 3
Kd= 0.7

before this I did many other runs and got poor control response. After running this the mower is still in an oscilation.

I would really appreciate some help here.

1 what are the general mathematical concepts that need to be employed to drive a differential drive robot along a straight path between 2 waypointsand minimize the cross track error.

2 Can we expect our method to work when with have 2 inputs to the same variable, heading error and the PID

3 Do you have a rule of thumb method to tune the PID

Any help on this will be appreciated

regards

Max

PS Max, you don;t have to be sorry to be new on the site. Welcome welcome, thirst for knowledge and understanding is what we do here!

I have built an autonomous grass mowing machine for my garden. We write the code from the ground up. Its basically C++ Arduino code. Coding is not my speciality.

It is set up to mow lanes that are 17 inches apart due to the blade size. All the software does is drives our chassis up a lane to a waypoint and then makes a 180 degree turn and drives down the next lane to the next waypoint which is located 17 inches over to one side of the first waypoint.

It uses a pair Ublox C94 M8P RTK gps with a reliable AHRS compass (with very little distortion due to magnetic or ferrous material). The vehicle follows waypoints by calculating the difference between the target heading (maths and the gps) and the actual heading (compass) and it uses the difference to send a value in a drive command to the motor controller that alters the speeds to the 2 driving wheels. this variable is called turnSpeed. the chassis is differentially driven.

After many months of work the mower drives well. The heading errors are generally below 5 degrees. However if the mower for some reason gets off the straight line track between the 2 waypoints the code does not do a good job of getting the mower back on the straight line track quickly as it is aiming at a distant waypoint. In short, it is good at heading to a distant point but not good at getting on the straight line track between the 2 way points and then heading to the point

In order to solve this we started calculating a cross track distance error. And we use a set point of 0 cross track error and use a PID to try to get the vehicle back on the straight tract between the two waypoints.

The problem now is that we have 2 inputs to the motor command variable turnSpeed. One from the heading error and the other from the PID. So this is getting real confusing.

Normally I would use the heading error variable if the heading error is greater than 2 degrees. To get the PID tuned I eased off the heading error so that the heading error only works if the heading error is more than Â±20 degrees. I set Ki and Kd to zero and increased Kp until the mower went in to a stable oscillation.
I then timed the period of the oscillation for 1 complete wavelength it was 6 seconds.

I then used the following

New Kp= Kp*0.60
New Ki = Period/2
New Kd = Period/8

this gave Kp=5
ki= 3
Kd= 0.7

before this I did many other runs and got poor control response. After running this the mower is still in an oscilation.

I would really appreciate some help here.

1 what are the general concepts that need to be employed to drive a robot along a straight path between 2 waypoints. Especially to get back on. This is my main question. You seems to have cracked this very well
track if it gets off track

2 Can we expect our method to work when with have 2 inputs to the same variable, heading error and the PID

3 Do you have a rule of thumb method to tune the PID

Any help on this will be appreciated

regards

Max

PS Max, you don;t have to be sorry to be new on the site. Welcome welcome, thirst for knowledge and understanding is what we do here!

2 Likes

In a shorter request which modules of the Ardupilot Rover2 code concentrate specifically on steering in auto mode.
Would really appreciate a short conceptual pointer to the correct mathematics to differential drive steering between and on a track between 2 waypoints

Hi Max,

Iâ€™ve written a wiki page here on Roverâ€™s L1 navigation controller and it includes a link to the original paper that it was based upon. The L1 controller was originally written for Plane and itâ€™s been inherited by Rover. Leonard Hall and I are thinking of rewriting it in a few months but what we have for now is whatâ€™s described on that wiki page.

ArduPilot (Rover and Copter anyway) have layered controllers. So at the top is the navigation controller (i.e. L1) and this outputs a lateral acceleration which is then converted into a desired turn rate and passed into a turn rate controller (a PID controller).

1 Like

I hope this is the correct area (newbe). My question is for trolling motor application (vectored thrust). What I cant find is a setting for what I call â€śreturn to original calculated headingâ€ť. In other words you have a heading from way point â€ś1â€ť to â€ś2â€ť. Now it seams to vector to next way point from current location. I would like for it to return to the calculated heading if it becomes off coarse (wind, current, way point overshoot) then resume to way point.
I hope this makes sense.

I donâ€™t quite understand unfortunately. The way the navigation works is that the vehicle will try to get back on the line between the waypoints. It may not point directly at the next waypoint especially if there is a current pushing it off the line.

If Auto is interrupted (by switching the vehicle to another mode and then back to Auto) it will probably pick up from where it is currently. It wonâ€™t go back to the line between waypoints.

Hope this gets close to an answerâ€¦

I found my answer in your previous post on L1 calculations â€śhttp://ardupilot.org/dev/docs/rover-L1.htmlâ€ť. Sorry I didnâ€™t read first. Working much better now. Very good write up by the way. I found that Rover defaulted to 6 seconds on navl1-period. This was to long.

I like your video of your boat in loiter mode in a strong current, this was what made me pursue my project with my trolling motor. It is winter hear in Wisconsin so I am not on the water. I am doing testing with an RC truck I converted with Pixhawk.

New Questions:
I am using a 320A ESC for the main drive motor and a rear wiper motor from a mini van for the steering. I am using a arduino to convert the RC PWM signal from Pixhawk, so I can incorporate a ten turn pot for steering position feedback. This all works using a regular RC transmitter and receiver and bench testing with Pixhawk. Has any progress been made on multiple types of motor drives in Rover (RC-PWM and DC motor drive PWM at same time)? Do you know if its possible to close a position loop with analog input on a Pixhawk, so I can eliminate the arduino in my steering control?

This is my trolling motor sketch. I hope you can read it ok.

1 Like

Nice use of the wiper motor.

It should be possible to use RC-PWM output for servo control on one channel with DC motor PWM on another channel. The issue may be the â€śoutput bankâ€ť restrictions on Pixhawks (and some other flight controllers). In particular on a Pixhawk1 or Cube, I think the first four outputs must use the same type of output. but 5 and 6 can be different from the first four. This isnâ€™t documented but itâ€™s on my list to add. Also on my list is a page describing how to setup the brushed motor output for both Copters and Rovers.

Re adding a PID controller into AP to support closed loop steering control - I havenâ€™t heard of this before so perhaps opening an issue on the issues list would be good. I donâ€™t think itâ€™s something that I will personally be able to look at any time soon but perhaps another developer will. If a few users show up asking for this then itâ€™s more likely I could prioritise it and do it myself. Sorry, itâ€™s just there are so many requests I canâ€™t do them all â€¦ of course another developer might do it though.

I have started an issue for my problem #10172. https://github.com/ArduPilot/ardupilot/issues/10172
I hope it conforms to the policies for posting issues. I also am changing from a DC PWM drive to a ESC. Havenâ€™t re-coded my Arduino yet but this should work satisfactory. I found an issue already proposed for supporting the DC drive I was using (2 PWM input, fwd/rev) that seams to not be moving forward so another ESC for steering may be a better solution. Thank You very much for your input on this mater