Txs for the log. So it appears that the bendy ruler object avoidance is the cause of the stops at waypoints (even those that don’t require a pivot).
I need to investigate more but I think what is happening is that the vehicle reaches the waypoint, then passes the next waypoint to bendy ruler and waits for a response. Bendy Ruler runs asyncronously so it can take a second or so to respond (depending upon CPU load) and during this period the vehicle stops. Once the response arrives it starts on its way again.
From a quick test master acts the same but I haven’t tested vs 4.1.3. This at least highlights that scurves corners still end up being stops if OA path planning is enabled (e.g. OA_TYPE is non-zero).
That makes sense, Randy. I did notice some “stuttering” at waypoints under 4.1 with BR enabled and never connected the dots. With S-Curve, it’s quite pronounced. I will get the mower out today and try disabling OA for comparison’s sake.
I’m happy to test with any variation of fence-based OA enabled/disabled as we move forward.
EDIT:
Can confirm - disabling OA removes the delay at non-pivoted waypoints. Pivoted waypoints still seem to be approached overly cautiously, even with fairly aggressive ACCEL values (up to 10m/s/s). It almost seems as if the vehicle gets a little “indecisive” as the waypoint approaches, slowing excessively and creeping up on it, then finally engaging the pivot turn. Maybe we can relax the “waypoint achieved” determination?
Also, it’d be really interesting and probably necessary at some point if you have some documentation (or could write a bit more of a primer) on the position controller and how the PSC* parameters should be tuned for navigation.
I’ll do some more comparisons vs master of the vehicle’s behaviour during a pivot turn discuss more with Leonard especially re the pre-pivot part.
Yes, I totally agree about the PSC_ parameters although I also hope that we can find a combination that works for almost all vehicles I’ve got a small improvement to “limit handling” that I’m working on that may help a bit too.
I’ve updated the binaries linked at the top of this discussion with a new version (with suffix “12Jan2022”) that includes:
SCurve “jerk time” shortened from 1 sec to 0.1 sec. I hope this will reduce the delays we’re seeing before pivot turns
Position controller velocity limits to reduce overshoot. This should make tuning easier by ensuring even if the gains are too high, impossibly high velocities are never requested
Position Controller + Simple avoidance fixes and enhancements. The vehicle should properly backup now if it gets too close to a fence. I’ve also tentatively enabled AVOID_BEHAVE=0 (Slide) allowing the vehicle to turn a bit to get around obstacles.
By the way, I played around with the PSC_* tuning at various speeds in the simulator and especially with the new “Position Controller velocity limits” the results can be subtle
PSC_POS_P (converts position error to desired velocity)
higher values increase accuracy of driving along the path especially when close to the line
very high values on Ackermann vehicle leads to circling as small errors are exaggerated into large forward-back speed changes but also turns. On skid steering vehicles high values don’t cause many problems but maybe lead to some wobbling on the path or unhelpful random turns when very close to the waypoints
very low values lead to smoother driving but the vehicle ends up off the path
PSC_VEL_P (converts velocity errors into speed changes and turns)
very high values lead to oscillation on the path much like high PSC_POS_P
very low values lead to an offset off the path
BTW you can monitor the this controller in real-time by setting GCS_PID_MASK = 64 (or 128). Sadly this is a 2D controller in North-East frame so you can only look at Northern or Easterly performance not both at the same time.
PSC_VEL_I
very high values lead to oscillation on the path
very low values lead to a consistent offset off the path
my guess is that this should always be some ratio of PSC_VEL_P (like 20% of PSC_VEL_P)
PSC_VEL_D
higher values slow the vehicle’s return to the path but keep the vehicle driver straighter which looks nice.
very high values lead to unhelpful turns in the wrong direction at waypoints
very low values (like zero) aren’t too bad but probably lead to more wavering across the line
FYI, I’ve updated the links again just now to “12Jan2022b” based on another discussion with Leonard about the “Position Controller velocity limits”. This most recent change is quite minor though and doesn’t change any of the advice or discussion above.
It’s running to the limit of TURN_MAX_G, but still overshoots sharp corners as well as slowing for the corner. However, it’s either turning too late, or not slowing enough.
Increasing the ACCEL and DECEL MAX doesn’t seem to affect much, but maybe reduces the slowing slightly (at the cost of more overshoot).
Bringing WP_ACCEL to 1 (or seemingly changing any of the other params to 1) gives much smoother nav, but still slows a lot for the sharp corner.
I’ve tried bumping up the WP_RADIUS and the ATC_DECEL_MAX, and they help a little on making corners faster, but it still slows. It’s not getting close to TURN_MAX_G, so theoretically should be able to turn faster.
@rmackay9, huge improvements here! Using 12Jan2022b, there were few to no delays at any waypoints, and things went fairly smoothly once I got the tuning sorted a bit (and there’s probably some more improvement to be had in my tuning).
Pivot turns were nicely controlled with no undue delay, and course control between pivoted waypoints was generally very good.
Where I encountered the most difficulty was in smoothing a somewhat aggressive “lurch” at non-pivoted waypoints, where the steering rate would pretty rapidly increase, resulting in an overshoot of the desired course and subsequent oscillation (depicted most clearly at points 3-4 and 6-7 in the image below).
I tried various values for PSC_POS_P and PSC_VEL_P ranging from 0.1 to 0.5, finally settling on 0.4 for each as the best I could manage. Reducing the P terms really just made the correction take longer rather than reducing the magnitude of the overshoot as I’d hoped.
I resorted to trying to limit the steering “aggression” by reducing ATC_TURN_MAX_G, ATC_STR_ACC_MAX, ATC_STR_RAT_MAX, and finally ATC_STR_RAT_SMAX and ATC_STR_RAT_FF. I did achieve a little bit better result when I pinned ATC_STR_RAT_SMAX to its lower limit of 10, but nothing eliminated the lurch/overshoot behavior at non-pivoted waypoints even when the value reductions made pivot turns painfully slow.
I do still have MOT_STR_THR_MIX set to its max of 1.0 - maybe that should be reduced?
A review of the log (rather than just eyeballing the real-time tuning graph) shows frequent/consistent overshoots of the desired rate. Maybe it’s as simple as reducing the steering rate FF term (and possibly using a non-zero P value).
Txs as always for testing. I’m very relieved that the delays at pivot waypoints is gone because that issue has been hanging around for a while. I might try removing the change I made earlier which froze “spline time” during segment’s slow-down phase because it seems that wasn’t the core problem… maybe I’ll give you a special firmware that allows turning that on/off so we can be sure we don’t take a step backwards.
For “non-pivoted lurch” hopefully we can determine if it’s a lower level control issue or an SCurve path shape issue. I’m not sure if MOT_STR_THR_MIX will have much impact… it’s possible but I suspect 1.0 is fine.
After reviewing the log, I’d like to try reducing ATC_STR_RAT_FF from 0.3 to 0.2 and then increasing the P term to 0.1 or more. I think FF is blindly too aggressive, and a little proportional feedback might be all that’s needed. I will let you know how that goes.
The SCurve path is created using a max acceleration which is the minimum of ATC_ACCEL_MAX, ATC_DECEL_MAX, ATC_TURN_MAX_G*9.81 and WP_ACCEL. It’s done this way because the SCurves path planner can only accept a single acceleration so to be sure that we don’t ask the vehicle to do anything it is not physically capable of we need to use the minimum.
In general setting any of these accelerations to a low value will cause the short-cut at the corners to be larger but the maximum distance from the waypoint at the corner will never be larger than WP_RADIUS so sometimes it will need to reduce the speed into the corner to keep the acceleration below the limit and within WP_RADIUS. … so to avoid the vehicle slowing down set all accelerations to be low and WP_RADIUS to be large.
When it comes to the vehicle not staying on the path, this will be caused by the limitations of the vehicle or the tuning of the position controller. Note that the simulated vehicle’s limitations that are not changed with the parameters mentioned so far. E.g. increasing ATC_ACCEL_MAX does not change the underlying physical limitations of the simulated vehicle… it just changes what the attitude controller will try to get the vehicle to do. I think the simulated Ackermann steering rover’s maximum acceleration is about 1m/s/s.
@rmackay9, Just reporting that I loaded the latest alpha today finally. I only had about 30 minutes to test before dark, but I only had to change one PSC_ parameter to get a decent 1st AUTO run. The change was to set PSC_VEL_D to 0.
I did have a little delay during pivot turns, but it was minor and I may be able to change the pivot rate or other parameters and overcome it. (I had OA_TYPE=0, by the way.)
At this point, I have nothing to contribute to the cause, but wanted you to know I am at least on the bandwagon!
I was correct about my steering FF term being the cause of the “lurches” at waypoints. I re-tuned the steering and eliminated the rate overshoots. Things are behaving much better now.
I’m having a little trouble avoiding an overshoot of non -pivoted waypoints. It’s minor, but the path never cuts inside the turn as expected but rather overshoots and comes back. I’ve tried altering the accel, max-G, and radius values all over the place, and I can never get rid of that overshoot.
Just checking this is a typo in the post and not in the code - TURN_MAX_G should be multiplied by 9.81, not divided in order to get an acceleration in m/s/s like ATC_ACCEL etc.
Can you point me to the section where the code decides that strong text now is the time to initiate the turn?
Yes, that was a typo. It should be ATC_TURN_MAX_G multiplied by 9.81. I’ve fixed it above txs.
The way SCurves cause the vehicle to turn is the current leg’s scurve’s output and the next leg’s scurve’s output are added together. So the turn starts when the current leg enters it’s braking stage. The exact code is (I think) here in SCurve::advance_target_along_track().
I’ve updated the binaries at the top of the discussion with just one improvement:
WP_OPTIONS added (temporarily) to allow testing the SCurve slow down behaviour
0 = “normal” behaviour (e.g. scurve time scaling is always enabled)
1 = scurve time scaling disabled during slow down
While investigating the delay-during-pivot-turns issue we added this special behaviour to disable “scurve time scaling” as the vehicle slows down to stop at a waypoint. Later on we found out that the biggest cause of the problem was actually the “SCurve Jerk time” setting was too low. So basically I want to remove the special behaviour if it’s not useful.
@Yuri_Rage no pressure of course but if you have a chance to test pivot turns with this parameter set at 0 and 1 that would be great. I don’t think you’ll notice difference but let’s see!
This branch has also been rebased on master.
UPDATE: I tested on my AION robotics rover:
I couldn’t consistently see any difference in behaviour between WP_OPTIONS=0 vs 1
changing PSC_VEL_D to 0, 0.1 or 0.3 didn’t make much difference to straight line performance
I don’t have an RTK GPS on my vehicle though so the vehicle’s performance is quite variable. Sometimes it seems rock solid as it travels in a straight line between waypoints and other times it wavers on the path. I guess I need a better GPS so I can more easily distinguish between what is an estimation issue and what is a control issue.
I think I’m seeing part of my problem (or at least something I’m missing) - in order to fully diagnose things, I need to look at desired vs achieved turn rate as well as desired N and E velocities, ideally with a square course. There’s a combination of what the system WANTS the vehicle to do, versus what the vehicle can achieve. My problems may be a result of perfect planning but poor execution.
May be time for an improvement request to implement GCS_PID_MASK1 / GCS_PID_MASK2 so you can look at two things at once.
