TorBorve / mpc_local_planner

A nonlinear MPC used to control an autonomous car.
MIT License
41 stars 9 forks source link

how about rear wheel steering? #11

Closed chan-yuu closed 1 month ago

chan-yuu commented 1 month ago

I changed this code:

  1. urdf:

from

  <!-- <xacro:rear_wheel name="rl" x="${-half_wheelbase}" y="${half_rear_track_width}" z="0" flip="1" />
  <xacro:rear_wheel name="rr" x="${-half_wheelbase}" y="${-half_rear_track_width}" z="0" flip="0" />
  <xacro:front_wheel name="fl" x="${half_wheelbase}" y="${half_front_track_width}" z="0" flip="1" />
  <xacro:front_wheel name="fr" x="${half_wheelbase}" y="${-half_front_track_width}" z="0" flip="0" /> -->

to:

  <xacro:front_wheel name="fl" x="${-half_wheelbase}" y="${half_rear_track_width}" z="0" flip="1" />
  <xacro:front_wheel name="fr" x="${-half_wheelbase}" y="${-half_rear_track_width}" z="0" flip="0" />
  <xacro:rear_wheel name="rl" x="${half_wheelbase}" y="${half_front_track_width}" z="0" flip="1" />
  <xacro:rear_wheel name="rr" x="${half_wheelbase}" y="${-half_front_track_width}" z="0" flip="0" />
  1. yaml: from: steering_ratio: -17.3 to: steering_ratio: -17.3 That worked, but the control effect was not good when the car turning: image image Maybe I still have a few things I need to fix?
TorBorve commented 1 month ago

Hi,

  1. The urdf you are referring to is this the audibot simulator? That is a seperate repository for simulating a car. This repo is only for path following with a car. This repo needs information about the base_line of the car (length between the rear and front tires) and it needs information about the steering angles.
  2. I guess you should have written from 17.3, to -17.3? A negative steering ratio does not make sense unless the steering angle in your simulator is defined as positive anti clockwise.

I am not sure what you mean when you say the steering is not good. On the pictures it seems like the car is following the path and the planning horizion seems reasonable. What is your issue exactly?

chan-yuu commented 1 month ago

Yes, it's the audibot simulator's urdf and I didn't modify the base _ line.I have now switched the vehicle to rear wheel steering, so turning left should result in a negative turn angle. So I think it is -17.3. I mean, there's a lot of tracking error when the car turns! This is the tracking error of the front wheel steering: image It's obviously smaller than before.

chan-yuu commented 1 month ago

Is it because the current URDF for rear wheel steering has been changed incorrectly?

  <xacro:front_wheel name="fl" x="${-half_wheelbase}" y="${half_rear_track_width}" z="0" flip="1" />
  <xacro:front_wheel name="fr" x="${-half_wheelbase}" y="${-half_rear_track_width}" z="0" flip="0" />
  <xacro:rear_wheel name="rl" x="${half_wheelbase}" y="${half_front_track_width}" z="0" flip="1" />
  <xacro:rear_wheel name="rr" x="${half_wheelbase}" y="${-half_front_track_width}" z="0" flip="0" />

I swapped the front and rear wheels.

TorBorve commented 1 month ago

Oh, I see. It makes sens that it does not work that well when the car has rear wheel steering. This is because the model of the car (bicycle model) assumes that the front wheels are turning. In your case the model is correct if you assume the car is reversing. Perhaps you can set the path tracking vel to a negative number in the .yaml file? I'm not sure it it will allow this. If you do that then rear axel in the mpc becomes the front axel in the simulator. Thus you need to update the frame of the "rear axel" to now be the front axel. Hope this helps.

chan-yuu commented 1 month ago

oh! good idea. I will try it out.

chan-yuu commented 1 month ago

I solved it. Thank you very much for your help!

  1. Modify URDF

Firstly, the front and rear wheels were replaced, but attention should be paid to the differential in the simulation, which needs to be modified in this way

  <xacro:front_wheel name="fr" x="${-half_wheelbase}" y="${half_rear_track_width}" z="0" flip="1" />
  <xacro:front_wheel name="fl" x="${-half_wheelbase}" y="${-half_rear_track_width}" z="0" flip="0" />
  <xacro:rear_wheel name="rl" x="${half_wheelbase}" y="${half_front_track_width}" z="0" flip="1" />
  <xacro:rear_wheel name="rr" x="${half_wheelbase}" y="${-half_front_track_width}" z="0" flip="0" />
  1. Replace the axle

Secondly, the 'rear wheels' have been set to the current front wheels, which are "base_link_rear"

  <link name="base_link_rear">
    <inertial>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <mass value="${body_mass}"/>
      <inertia ixx="${body_mass/12 * (body_width*body_width + body_length*body_length)}" ixy="0" ixz="0" iyy="${body_mass/12 * (body_length*body_length + body_depth*body_depth)}" iyz="0" izz="${body_mass/12 * (body_width*body_width + body_depth*body_depth)}"/>
    </inertial>
  </link>

  <joint name="base_link_rear_joint" type="fixed">
    <origin xyz="1.325 0 0" rpy="0 0 0"/>
    <parent link="base_link"/>
    <child link="base_link_rear"/>
  </joint>

add it to URDF and then set yaml: car_frame: "base_link_rear"

  1. Modify the steering ratio

Finally, the angle conversion relationship was replaced with a negative number: steering_ratio: -17.3 image

TorBorve commented 1 month ago

Fantastic! I am happy to help.