Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Interpolate desired angle between MPC updates #643

Open
wants to merge 2 commits into
base: devel
from

Conversation

Projects
None yet
2 participants
@Gernby
Copy link
Contributor

commented May 15, 2019

This enhancement is part of a large group of enhancements included in a different PR, and is an update to this PR from 2 months ago.

The existing lateral control logic does not interpolate between MPC updates, which creates a significant amount of noise in controls. Also, since controls and planning run in separate threads without any synchronization, the number and offset of control cycles per MPC update is irregular, creating additional noise.

This enhancement eliminates both of those noise sources by interpolating between multiple points using system time. Below are examples of standard vs interpolated values over a 2 second period.

Standard values
non-interpolated

Interpolated values
interpolated

Note that I have altered the calculation of MPC times to compensate for the effective time delay caused by interpolation. In the sketch below, the top image shows how interpolating from previous angle to the new angle creates a delay of 0.05 seconds before the new desired angle is requested. However, by reducing the first time offset to half the length of an MPC cycle, the phase is corrected.

If you prefer a different approach, please let me know.

image

Gernby added some commits May 15, 2019

@pd0wm

This comment has been minimized.

Copy link
Contributor

commented May 16, 2019

Thanks for looking into this. My 2 cents:

  1. You don't want to apply the 25ms offset. The values at the beginning of each timestep are what MPC assumes the car will exectue.
  2. MPC assumes the control signal stays constant during the whole timestep. However, the control signal (often called u in control theory) in MPC is not angle, it is rate: https://github.com/commaai/openpilot/blob/devel/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c#L116

Therefore the correct way compute intermediate angles is to take the angle at the beginning of the timestep and add the rate multiplied by elapsed time.

0.5.12 will be released very soon, which should make the packet received times very easy to access. This means this change can be made without any changes to pathplanner.

@Gernby

This comment has been minimized.

Copy link
Contributor Author

commented May 16, 2019

Thanks for the prompt feedback! I understand what you're saying, and I can see how INDI will improve steering performance immensely. However, since the current logic to convert desired angle to torque isn't aware of the MPC's assumptions, leaving out the time offset results in a slight under-steer. That under-steer goes away completely with the 25ms time adjustment.

That said, if the 25ms Offset is a blocking issue for the PR, I would not hesitate to remove it. The main reason why I added the offset was to make it most compatible with existing CP.steerActuatorDelay values.

@Gernby

This comment has been minimized.

Copy link
Contributor Author

commented May 17, 2019

@pd0wm, it occurred to me last night that your statements about computing desired angle using step + rate sounds very similar to my original interpolation logic.

I agree that the interpolation could be performed in latcontrol.py with the same result, but it seems slightly less performant to do those calculations / conversions ~5 times per MPC cycle instead of once per MPC cycle. It also seems that converting and interpolating the angles inside pathplanner would make it easier to access accurate desired angles for future purposes outside of latcontrol.py (auto-tuning, etc.)

@Gernby

This comment has been minimized.

Copy link
Contributor Author

commented May 17, 2019

@pd0wm , I read through our DM's from last month about desired rate, and really think you're making the same assumptions I made 6 months ago. It took a good bit of testing and feedback from the community to realize that interpolating between desired angles (using desired rate) definitely caused a delay.

The reason why I eventually converted the desired angle to an array of desired angles was to avoid over-interpolation from irregular process timing. Prior to 0.5.10, the MPC cycles alternated between 30ms and 70ms, so over-interpolation occurred on every other update. The refactoring in 0.5.10 improved that greatly, but the MPC updates still vary between 4 to 6 control cycles.

@Gernby

This comment has been minimized.

Copy link
Contributor Author

commented May 17, 2019

Also, as a reminder, I have submitted multiple PR's for latcontrol, which have all been proven beneficial by the community.

arne182 added a commit to arne182/openpilot that referenced this pull request May 18, 2019

arne182 added a commit to arne182/openpilot that referenced this pull request May 18, 2019

arne182 added a commit to arne182/openpilot that referenced this pull request May 18, 2019

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.