Openpilot: Account for Toyota's loose steeringRatio around center and adjust Kp on angle, add Kd PID term

[zorrobyte]

Give the age of this issue and recent changes in controls. If the issue still affects the latest release, it can be reopened or a new issue opened. https://github.com/commaai/openpilot/issues/300

I'm a new OP user with a '19 Prius Prime. Less than 600 miles, so alignment, etc is nominal. Running stock latest stable OP, autotune variables settle at:
/data/params/d {"steerRatio": 16.329651149572307, "carFingerprint": "TOYOTA PRIUS 2017", "stiffnessFactor": 0.5, "angleOffsetAverage": 1.0404464786796497} {"angle_model_bias": 0.3890739310530656}

Prius Prime steering is rather twitchy and while I don't have experience with OP on other vehicles, @zeeexsixare 's tuning pdf makes me believe that this behavior is undesirable https://github.com/commaai/openpilot/pull/464

[zorrobyte]

Another '19 Prius Prime extreme see-saw though that same turn as previously reported.

Clean data, good weather, ran 0510 stock 17:33:13 - 17:36:13 and especially 253 -- two turns 5e7462b4437e3372|2019-04-16--17-30-13&seekTime=193

https://community.comma.ai/cabana/?route=5e7462b4437e3372%7C2019-04-16--17-30-13&max=7&url=https%3A%2F%2Fchffrprivate.blob.core.windows.net%2Fchffrprivate3%2Fv2%2F5e7462b4437e3372%2F7e75afc578e2867b4be8d0b96414b655_2019-04-16--17-30-13

I can replicate that test loop anytime, let me know if it's needed or want to collab.

[ErichMoraga]

Stepping away from stock code, let me know in Discord how you like my latest VSR port.

[mtourangeau]

I’ll try it.

[zorrobyte]

@andyh2 -- For when ya'll pickup a Prius Prime, sir.

@energee @zorrobyte This sounds like its related to the stiction issue with prius, are you already aware of this?

zorrobyte
@energee could you elaborate by what you mean by stiction?

Additional feedback: tried Grenbys feed forward fork last night and had a buttery smooth straits and turning experience on the same test loop, that curve which has always been choppy. It was the first time I’ve ever felt “on rails”, giving me hope this issue can be resolved within upstream OP

csouers
Static friction. Need to apply so much torque to break free and allow movement.

zorrobyte
Hm, I can't say I've really had much experience or have heard much about Prius's actuators. What I can say is that OP on Prius Prime constantly adjusts steering and is very overactive. It's almost like the planner doesn't plan lateral control ahead and instead respond to every single little change the vision system picks up.

Another interesting point is that the curve above that I use to test; OP can clearly see the lines but just seesaws desired angle seemingly for no reason, sending the car bouncing all over the place.

Desired angle should just hold steady, vision sees the curvature of the road, it's constant and predictable. The "white line" in the HUD holds steady around the curve, while the "Green Line" bounces around and seems to be related to desired angle/lateral control.

Even with model specific implementation issues, I'd imagine desired angle is "upstream" of vehicle specific lateral control? I could be wrong.

Even with model specific implementation issues, I'd imagine desired angle is "upstream" of vehicle specific lateral control? I could be wrong.
This is the specific commit of a community fork that I tried in which I experienced my first "on rails" OP experience (and I've tried several other forks, 058, wiggly), it was magic, it felt confident and controlled. It oversteered to the right by the end of the curve, but the code could give OP dev some hints at future enhancements https://github.com/Gernby/openpilot/commit/0aeb717d1c347cde54f1c550718fcd0a0db4305d

Erich
Right, you mentioned it basically tried to kill you. Not a cool tradeoff...
https://discordapp.com/channels/469524606043160576/534769856617381918/567892440682594322

zorrobyte
That may be true, yes - but it did prove that a curve could be made in any capacity by software changes, instead of assuming the Prius itself has limited/faulty hardware/actuators. I don't "daily" that code, no and I only test in a controlled environment under certain conditions.

I think that's fair.

Considering experiences with @ErichMoraga 's code which is fantastic for straight line driving, in my limited experiences, it almost seems as if OP, in general, could plan lateral control better. Opening the "Engineering UI" shows the "Desired Angle" is constantly being tweaked realtime and is hyperactive, in my opinion. This just could be the state of OP in current versions, I haven't been around long enough to know if this is expected behavior.

https://discordapp.com/channels/469524606043160576/524594418628558878/568082954748821509 -- onwards for additional discussion around the perdictive model, etc.

[energee]

IRC, Comma has a Prius which should have the same EPS as Prius Prime, @ErichMoraga can probably confirm.

[zorrobyte]

Some additional community chatter https://discordapp.com/channels/469524606043160576/524594418628558878/568101145902579734
```
@Gernby After the discovery last weekend that the PID / PIF values from latcontrol.py are almost never sent to the Toyota's EPS without being clipped by downstream logic, it's clear that the steering issue really isn't caused by laggy or sticky actuators. We just need to improve the tuning of the PID values, and provide feedback into latcontrol when the requested torque is being clipped. Otherwise, the actuators are just going to be controlled entirely by riding the maximum extents of a validation, which sucks.
The reason why Flag, VSR, Resonant, and the rate-based feedforward improves straight line driving so much for the Prius is because they allow the torque requests to be metered well enough that it stays within the validation boundaries.

foglem
Sounds hopeful!

Gernby
Actually, I shouldn't use the term "clipped" to describe what the Toyota validation logic is doing, since it doesn't just reduce the requested torque value. Sometimes it actually increases the requested torque, since there are limits to the rate of change in both directions from sample to sample.```

[zeeexsixare]

The stock Highlander tuning uses RAV4 parameters and is very choppy even on the straights at low speed <30mph. Turns are just as bad. Increased speed helped, but the final fix was to edit the PI parameters. I suggest using my fork's Highlander params from #464 just to see if the significantly different params give any improvement, or worsen it.

By the way, I strongly feel that the stiction/lag problem is primarily lag. You can compare timestamps of CAN data showing torque CMD and actual steering angle. The other reason I feel that it is NOT stiction is that the stock LDA controller can implement very finely controlled steering with very low torque such as when the LDA triggers, or when the stock camera detects a curve ahead and assists in anticipation. The torque is so low, it's almost imperceptible. But it's there.

[legonigel]

The variable steer ratio should not cause any issues or oscillations. Almost all modern cars have this, and this is why the steer ratio learner was added in 0.5.10. Variable steer ratios are only a problem at large steer angles.

We did try our prius and found we have a similar twtichiness. We are trying to develop a solution, but it might take some time.

[zorrobyte]

@legonigel There's a sensor delay of almost a second. Kp applies torque CMD, there may be a 0.2 delay, and that's cumulative (but seemingly accounted for w/ steerActuatorDelay). PI loop keeps getting fed the steering angle that's about a second laggy, Kp keeps applying force, overshoots, repeats == oscillation.

Meaning, oscillation due to almost a second delay that's unaccounted for in the PID loop for the steering sensor

5e7462b4437e3372|2019-04-24--17-26-01&seekTime=294

We can either DIY our own non-laggy steering sensor (hard, would solve root problem), or account for the angle_steers delay in the PI loop (easy). https://controlguru.com/dead-time-is-the-how-much-delay-variable/
https://www.automation.com/library/articles-white-papers/pid-tuning-loop-control/how-to-control-a-process-with-long-dead-time

Also, not sure why the 20Hz PID loop constraint was removed, but slowing down the PID loop could lead to reducing Kp overshoot
https://github.com/commaai/openpilot/blame/devel/selfdrive/controls/lib/latcontrol.py#L31
"# no greater than dt mpc + dt, to prevent too high extraps"

058 performance was better as lower Kp/Ki could be used due to a lower steeringRatio. With these settings, I can get perfect cornering performance at the expense of straight lines: https://github.com/ErichMoraga/openpilot/commit/7759129f3bf37fbddaefccdc3b4ed5b828b774de#diff-93874011152ca529debcfce500178461R86

Another potential avenue would be to set steerRateCost to 2.0 and use very high Kp, Ki values; as the overshoot correct frequency becomes high enough not to cause more than 0.4~ degree sway, but it kills cornering performance.

Lastly, @Gernby created an "Active noise cancellation" workaround to feed back oscillations, leading to what some have called, "Tesla like steering" in straight lines.
https://github.com/Gernby/openpilot/commit/bec9ea6f9dcd9f30b102b1a94a3b0b34302c6c17

The issue here though is that the oscillation frequency changes with speed, so a linear function would need to be written to calculate oscPeriod @ vEgo

This impacts most Toyota vehicles, at least the Rav 4 & Prius. Likely not the CHR and Camry due to a better quality angle sensor.

Sample non-zero steady state oscillation periods:
Prius
0.4 Kp
4.26 sec @ 11.85 vEgo
5.664 sec @ 17.72 vEgo

0.175 Kp
6.422 sec @ 17.93 vEgo

1.2 Kp
3.86 sec @ 18 vEgo

Rav 4
0.3 Kp
2.311 sec @ 19.21 vEgo
1.9 sec @ 30.79 vEgo

I joined OP about three weeks ago, have never heard of PID tuning, or any of this jazz, yet here I am kicking ass and taking names. I love this community <3

[ErichMoraga]

Let's replace the sensor!

[zorrobyte]

Hold my beer

[zorrobyte]

!!!!!!
EDIT: With changes in OP 0.5.12, the following information is possibly moot. I personally believe that any additional, intentional backlash in the EPS system is suboptimal, but one must consider real world feasibility of dropping a steering column, and the potential long term impacts this could have on your vehicle. I personally don't anticipate any problem with this mod, and will be keeping mine installed, but the word from comma is, don't swap out potentially safety critical parts in your car. YMMV.
!!!!!!

Root cause analysis:
There's a rubber damper between the EPS motor and steering shaft on most Toyotas (Camry should be excluded, EPS is on the steering rack). It introduces 0.4mm of backlash or more.

I 3D printed a solid replacement. Steering response is improved.

Before:

This one is really bad:

After:

Video of the "Dampner":
https://youtu.be/jloNX67sLR8

Video of my highly scientific, professional testing:
https://youtu.be/njIcvTiDFYI

Here's my Fusion 360 source for the solid replacement part. I designed it with a 0.1mm oversize for an interference fit. The previous bushing is made of polypropene, PETG should be more than able to handle permanent installation. DM me on Slack zorrobyte and I'll print and mail you one.
https://a360.co/2L3KbsS

------- Technical analysis
PI control loops are sensitive to deadtime and must have accurate sensor sources. The observed PI overshoot, overcorrect, repeat behavior, herein defined as the "oscillation" is directly attributed to the EPS motor not being directly attached to the steering shaft. Small angle adjustments needed for straight line performance is impossible due to backlash. This behavior is also apparent when cornering as OP may need to turn the wheel negative, positive in which the steering is allowed to "float", causing "ping pong" and other undesired behavior.

Effective EPS output, especially output that can be sensed and applied to the PI loop for proper control vastly enhances the OP experience on Toyota.

[zorrobyte]

!!!!!!
EDIT: With changes in OP 0.5.12, the following information is possibly moot. I personally believe that any additional, intentional backlash in the EPS system is suboptimal, but one must consider real world feasibility of dropping a steering column, and the potential long term impacts this could have on your vehicle. I personally don't anticipate any problem with this mod, and will be keeping mine installed, but the word from comma is, don't swap out potentially safety critical parts in your car. YMMV.
!!!!!!

[zeeexsixare]

This is incredible work! It's been dogging us for forever and I don't think anyone here tore down the assembly to find the root cause. Looks like you have some photos; any way we can get a full step-by-step teardown set of photos and work instruction? Would be great to see if it's minimally invasive like disconnecting DSU or if it's brutal like an engine teardown.

[ErichMoraga]

Have you seen the YouTube video? This is the same cogwheel I showed everyone 2 years ago in Slack, but was not going to tear apart my car when it was new.

[zeeexsixare]

I haven't, no. Linky? Your car is old now, OK to tear apart :)

[ErichMoraga]

The YouTube link is a few posts up (https://youtu.be/jloNX67sLR8), and yeah, I'll be posting a step-by-step on the mod, but I don't believe Highlanders have this cogwheel "damper". The exact p/n in the Prime is Steering Column Damper [multi-piece cogwheel] p/n 45254-28040, which I found in vehicles like...
2013-18 Avalon
2017-18 C-HR
2016-18 Prius
2017+ Prime
2012-14 Camry
2020 Corolla

...and surely the Toyota w/ EPS on the steering column (vs. rack) have something similar.

[Gernby]

I agree that the OEM bushing in the steering column probably contributes to poor OP steering performance. However, the only "delay" that it could cause would be in the "free play" region. Once the steering rotates enough to close the gap, the delay associated with the bushing would be 0.

[zorrobyte]

Yes, until reverse torque is applied which happens frequently with the PI control scheme, which is to be expected in this application. The math of the small amount of time the rather high inertia EPS motor is freewheeling, the delay from a forward/reverse torque, and the sensitivity/reaction time of the PI controller is beyond me, but noticeably affects practical performance.

There's also backlash/elasticity in the steering shaft, thanks to two U-joints which is unfortunate.

I wish I was smarter and could approach this scientifically, and I could be very wrong indeed. Alas, I'm just some idiot hacking his CAN bus and installing 3D printed parts in a car with less than 2,000 miles 😆

[Gernby]

@zorrobyte This is great work, so I hope my comment didn't come across as a criticism. I just wanted to point out that there are multiple "stacked" factors contributing to this issue. There's not going to be a magic bullet, and software workarounds are almost certainly going to require trade-offs in other performance categories.

[Gernby]

@zorrobyte The free play in the steering column obviously causes a dead-zone in the reported steering angle. That seems like a good opportunity for a dead-zone adjustment in the logic. I recall you saying that you observed benefit from a deadzone into PID, but I assume that the deadzone value you used was a positive number. If so, that would tend to cause under steer AND oversteer.

It seems that the case of a mechanical deadzone might require a negative logical deadzone. Basically, if there is 1.0 degree of free play in the sensor data, then -0.5 degrees of logical deadzone might be beneficial. However, without having a Toyota to test with, I can only guess. If that doesn't turn out to be beneficial, then my next guess would be that a negative deadzone should be applied only when desired angle is closer to center than actual.

[zorrobyte]

@pd0wm Partially, if not totally solved with replacing the EPS Damper & tuning params based on steeringAngle https://youtu.be/rHp__gzwzlY Great results on Rav 4 without hardware modifications: https://discordapp.com/channels/469524606043160576/534769493734588447/574467722100998165
0.4+/- degree oscillations in corners, +/- 0.5mm in straights.
https://github.com/zorrobyte/openpilot -- devel_modStockPID branch. My code changes are arguably minimal.

Compare this to previous screenshots of Grafana data ^^

Note: You MUST revert the steeringRatio in your data/params/d/LiveParameters to around 14 for Prius (13.40 preferable) 18.25 for Rav 4. I based my tunings at low SR, most stock OP SRs settle at 20-22 (from other forks, stock OP), drastically overscaling my PID values.

I also added a derivative term to your PI control. I only use it for extreme angles at torque limits to smooth out OP, that and Proportional on Measurement (self.proportional_on_measurement = False # http://brettbeauregard.com/blog/2017/06/introducing-proportional-on-measurement/) could be worth looking into for further experimentation.

Just in case, this representative of typical stock OP lateral control. Even though it's on "Gernby's" fork, his custom functionality should be disabled, spare bugs or implementation woes.

[zorrobyte]

Due to news of Comma switching out PID for INDI Control (which could have unique features as auto tuning of params, being resistant to wind, etc) in 0.5.12 (check out livestream, 8 min mark) https://www.pscp.tv/w/1OyKApqkPyzxb?t=1s -- I'm closing this issue.

Fingers crossed we get some cool features to play with:
https://arxiv.org/pdf/1701.07254.pdf
https://fenix.tecnico.ulisboa.pt/downloadFile/563345090414617/RA_EduardoLSSilva_69916.pdf

@pd0wm Looking forward to your implementation!

👍