Rover S-Curves alpha testing

ArduRover Rover 4.1
1

image
image1609×689 111 KB

I’m wondering if anyone would be interested in helping me test Rover S-Curves (PR is here)?

A fair bit of work has gone into it already but there are a few known issues (see below). Still, I would like to get some feedback to confirm that we are on the right track. In particular I would like to know:

a) Does it travel in a straight line to the next waypoint?
b) Do you like how it corners?
c) How is the pivot turn compared with master?
d) any other feedback re performance or issues found

Here are the changes:

  1. In Auto mode S-Curves are used to plan a path (i.e. target position, velocity and acceleration at each moment) to the next two waypoints including the corner. The vehicle should slightly cut the corner instead of passing through the waypoint and then turning. The amount that it cuts the corner will increase as the vehicle speed increases but it will never be more than WP_RADIUS.
  2. The L1 controller (e.g. NAVL1_ parameters) have been replaced with a position controller (e.g. PSC_POS_P, PSC_VEL_FF/P/I/D parameters) to keep the vehicle on the path to the next waypoint. If the vehicle wobbles or strays from the path it will most likely be the PSC_VEL_P/I/D values that are the problem.
  3. Pivot turns have been improved a little bit by tightening the completion requirement. They may also perform better because the vehicle will only start the pivot once it has arrived at the waypoint while previously it would start pivoting at up to WP_RADIUS from the waypoint.

Some warnings and known issues

  • Guided mode may act a little differently than Auto because it uses the position controller directly but it can be forced to use S-Curves by setting GUID_OPTIONS = 64. SCurves can’t handle fast position target updates which is often what guided mode is used for.

Auto, Guided, RTL and SmartRTL have been affected by this change but other modes including Manual, Acro, Follow, Loiter should be unaffected so if anything odd happens please switch into these modes to retake control.

I’ve placed some pre-build binaries below and I’m happy to provide others if people would like to test.

8 Likes
2

Awesome! I’m out of town for a few days right now but plan to download and test as soon as I can (hopefully by this weekend).

2 Likes
3

Where to get the sources to compile this myself? Are the changes already in ardurover master?

4

@count74,

The changes are in this PR and here is a direct link to the branch.

5

Awesome :slightly_smiling_face: Sadly we have winter conditions here in Norway, no further tests using boats for months… Keep up the good work!

1 Like
6

Compiled, uploaded and it’s raining…:confused:

2 Likes
7

is only Cube Orange suitable, or can I try it on Pixhawk?

8

Hi @McKey,

I’m happy to provide pre-built binaries for any boards people want to test with. In the top discussion above I’ve placed a link to a Pixhawk1 binary. Thanks for giving it a try!

9

Keen to try this - will probably only be able to give it a go in simulation at the moment.

I have a question: will the vehicle follow the same path regardless of speed (up to some sort of g limit)?

Are you basing the path on geometry or turn rate / lateral acceleration?

I think it is important to be able to make the vehicle follow the same path at all speeds. Does the vehicle follow a circular path through the corner (thus it would be possible to have a constant turn radius, and vary the turn-in point depending on the angle between the current and next waypoint?

One thing I’ve noticed with L1 is that it tends to turn in hard, then can take a while to track to the path, rather than setting itself up on a steady curve to intercept the new path.

10

I built the SITL target today and unfortunately got called away from the desk just as I was connecting the test case in MP.

I intend to live test tomorrow on the mower with a Cube Orange.

1 Like
11

Hi @mroberts,

I think speed will have less of an effect on the path but will still have some effect. For example at lower speeds the SCurves will plan the turn so it passes very close to the waypoint. At higher speed it will turn earlier but the vehicle’s maximum distance from the waypoint will never be more than WP_RADIUS so the vehicle may slow down if necessary to keep the lateral acceleration low.

I have also seen that L1 responds very differently at low speeds compared to high speeds and I think this is improved with this PR because it also replaces the L1 controller with a Position Controller that is similar to what we use in Copter. I suspect that if there are performance problems it will be in this area rather than in the SCurve path planning.

12

Hi @rmackay9 I really like the consistent turn radius.

I can see that it slows down if the corner is too tight, but I don’t yet understand how to tweak that - I think it would be good to understand when / why it decides to slow, because it looks like it’s being quite conservative on the speed.

Here’s a sample mission in sim. You can see at WP 3 the effect of increasing WP RADIUS. It’s going into that corner at 5m/s and slows to 2m/s through it. Also of note is how the radius changes, and it takes a while to get back on track afterwards, compared to WP 2.

However, it also slows to 2m/s for WPs 4-8 - a far gentler corner.

image
image1073×716 216 KB

I like the uniform turning radius through WP2, but I think the slow down is excessive from 4-8.

From a mission planning perspective, I’d like to be able to define a nominal turn radius and a maximum g, and have the vehicle follow that radius at all speeds, until it hits turn_max_g, then slow to maintain the g.

For ground rovers in particular, confidence, repeatability and predictability I think are much more important than in air vehicles - mowers need to ensure coverage, and road-based vehicles need to stay on the road.

2 Likes
13

@mroberts,

I don’t have a full answer immediately but one oddity of SCurves is that they will slow down if waypoints are placed too close together. There’s a built in assumption that the vehicle must be able to stop by the time it reaches the “next next” waypoint.

BTW, sometimes people add lots of waypoints in order to draw a curve. We could potentially add spline waypoint support to provide another way to produce a curve with fewer waypoints.

14

For boats that I care about I guess the most critical ability is precision in passing through or arriving at points. For pure bathymetric mapping then the thoughts shared for rovers above are valid. Not very important driving through open waters, but boats will also travel along water edges or islands where path precision is really important (we likely need to undress and swim to save a boat caught in an obstacle).
But in general, getting as close as possible to a point is paramount. The boats in mind (fisherman’s bait boat that drops tackle at and/or feeds the fish at a particular underwater feature) typically travel slow. Like 1-2 m/s usually at the lower end. I understand the comments that the slow speed will underhelp getting close. As will a narrow WP radius. That reminds me of other discussions in these forums where the advice was to avoid setting the radius too short as it could impact cornering significantly. Ideally, our boats should aim for a wp radius of no more than 0.5 meters (depending on GPS precision, probably). Are the current concerns of a short wp radius still valid for S-curve, or does S-curve instead open an opinion to shorten it?
These boats, at least the models using skid steering, are usually highly capable of doing pivot turns on the spot as well. Ie cornering at the point seems like a more appropriate ability than a consistent turn radius (might not need to be a contradiction).

15

Spline support would be awesome. I often mow around curved landscaping edges, and my waypoint paths are sort of clunky to avoid hitting anything. Spline waypoints would likely help a lot there.

Downloaded and installed the CubeOrange binary above, and it hangs at boot time. The speaker plays its usual happy tones, then one long, high pitched beep, then reboots and does the same…endlessly.

UPDATE:
I found the issue. SERVOx_FUNCTION=-1 does not work to define GPIO pins for this branch. Instead,
SERVO_GPIO_MASK defines those. Ok, now maybe I can drive…

Initial impressions are positive. As far as the planned vs actual path, it’s about on par with previous L1 performance, if slightly better after a little PSC_VEL* tuning. Pivot turn behavior is decidedly worse. It sort of ratchets through the turns in 2-3 increments, and often overshoots, causing an oscillation leaving the turn. ATC_STR_ANG_P changes do not seem to affect that behavior. It’s also pausing at nearly every waypoint, which is something it tended to do before but now seems exaggerated.

I have a persistent unhealthy AHRS warning that was not previously present, and I can’t figure out why just yet. A look at the logs may reveal that.

Overall, it’s perfectly usable, and I think we are headed for a huge improvement. The position controller will likely prove easier to tune than the L1 controller, and turn performance will almost certainly improve over previous.

I will get some video footage and retrieve a log file or two later today.

image
image694×692 185 KB

Please let me know if there are specific test cases you’d like to see. The waypoints above are representative of the kind of geometry I tend to mow, so that’s what I used.

Unrelated to S-Curve, but perhaps a newly minted “feature” of 4.2 - I have MIS_OPTIONS=0, but every time I reboot, the waypoint mission that I had loaded seems to disappear. It’s become quite frustrating! (I do run a script that’s capable of clearing missions, but I think I’ve ruled that out as causal)

Some video footage from today’s testing. A review of the log shows some pretty high yaw innovation values, possibly accounting for the yaw offset seen in the log viewer footage. I may need to recalibrate everything after all the firmware changes today.

FOLLOW UP:

Some follow ups as I get ready to do a little more S-Curve exploration today:

I cleared the AHRS fault with a full 3-axis re-calibration, which hopefully improves the yaw innovation values as well.

POSSIBLE BUG REPORT:
I conclusively proved that scripting is not the cause of the waypoints disappearing between boot cycles. Something in this branch deletes all waypoints upon reboot regardless of MIS_OPTIONS value.

With no changes other than disabling steering acceleration limiting (ATC_STR_ACCEL_MAX=0 from a value of 100 deg/s/s/), I went from Mr. Toad’s Wild Ride at this sharp pivot, to a very controlled slight overshoot of the heading. The mid-turn pauses are still present.

image image

POSSIBLE BUG REPORT:
Disabling acceleration limiting (ATC_ACCEL_MAX=0), however, resulted in Internal Errors 20000000.

Increasing ATC_STR_ANG_P to a ridiculously large value of 5 (from 1.7) along with increasing ATC_TURN_MAX_G to an impossible to achieve value of 5 (effectively taking it out of the loop) seemed to improve performance a bit, with less (but still some) stutter-stepping in the turns. I want to keep exploring the relationship between those values when I can. The last path that the mower tracked before a servo malfunction was very nice!

Pivots seem to a be less “pure” than with the L1 controller. The outside wheel turns faster than the inside wheel, resulting in a positive turn radius (as witnessed in the last pic I posted of the zoomed in sharp turn).

Here’s the best I achieved:

image
image749×1227 95.3 KB

But…I burned out a drive servo, so that’s all I can do today. I won’t blame the new firmware for it - that servo has seen plenty of use and abuse over the past year. Replacements should arrive early next week.

2 Likes
16

Hi @Yuri_Rage,

Thanks very much for the testing and feedback including the video.

I guess maybe the issue with the pivot turns is that it is not actually using the pivot turn controller but is instead falling back to the position controller. Maybe when your servo is replaced you could try adding a delay to the waypoint commands? The “delay” is the first field in the command.

Some warnings and known issues
Skid-steering vehicles will not pivot at a corner unless the waypoint command’s “delay” field (the first field) is set to 1 (or higher). I suspect I should change this behaviour so it is like master and pivots at any corner where the angle is at least WP_PIVOT_ANGLE.

I thought this might be confusing/annoying to have to add the “delay” but I also thought it might be interesting to see how the position controller did without the use of the pivot controller. I think we can safely assume the answer is, “not well” so I’ll modify the branch so that it always checks the angle of the next corner and uses the pivot controller if it is > WP_PIVOT_ANGLE.

Re the other two issues with SERVO_GPIO_MASK and disappearing missions, I’ll have a play and see if i can reproduce these. I suspect the SERVO_GPIO_MASK issue has been fixed in master and I’ll just need to rebase this PR.

Thanks again.

1 Like
17

Thanks for the reply, Randy! I did have WP_PIVOT_DELAY=1, but I did not have each waypoint delay set as you suggest. Will do that next!

I’d also suggest that “not well” is more like “not bad,” but not quite up to expectation :slight_smile:

I feel like I was really making progress before the servo fried. The tuning is quite different from previous, and I was just starting to figure it out when disaster struck. I REALLY like the oscillation recovery after the pivot that induced it, and I think a better tune will leverage the new controller well!

And a late edit - I wanted to send you a .bin log for the better tuned results, but my session was cut short. The only log I have is probably several hours of power-on time, long periods of inactivity, multiple restarts, and a few promising results sprinkled in. I’ll get some good data soon!

1 Like
18

This is awesome to see. Everything I see above looks great except I was concerned about the pivot turns, since that is something that is working super well, in my opinion, with the current pivot controller. SCurves with the old pivot controller is looking promising. I will try to get my vehicle out and give this a try as well, but I can’t expect that I will add anything to Yuri’s analysis since our vehicles are so similar. But I would like to join in the fun!

2 Likes
19

I’ve updated the binaries linked in the description at the top to a new 21-Dec-2021 version of the SCurves feature. The changes vs the 14-Dec-2021 version are:

  • Vehicle always pivots at corners with angle > WP_PIVOT_ANGLE (no need to set each waypoint’s “delay” field)
  • Rebased on master (should fix the SERVOx_FUNCTION=-1 hang issue)
1 Like
20

What is the preferred/intended method to define GPIO pins? Should I be using the SERVO_GPIO_MASK or individual SERVOx_FUNCTION params?

On one hand, the bitmask is convenient, but on the other, the servo function params are a bit easier to read and maintain. Is there a plan to deprecate one of them?