JetRacer ROS AI Kit Tutorial VII: Robot Odometer Calibration

From Waveshare Wiki
Jump to: navigation, search

JetRacer ROS Kit User Guide

Introduction

  • The robot can output odometer information for learning how far the robot has traveled and how much the robot has turned. However, there may be errors between the odometer and the actual information output by the default program. We can calibrate the odometer by adjusting the parameters so as to realize higher accuracy.
  • This step can be skipped if the accuracy requirements are not high for the first time, and the following functions can be realized normally without calibration.

Step 1: Calibrate the Linear Velocity

  • Connect the robot by SSH to open the terminal, and enter the following command to enable the chassis node.
  • 【Note: the chassis node of the robot should not be enabled repeatedly, otherwise, it will go wrong. If the chassis node has not been closed, you do not need to enable it again.】
roslaunch jetracer jetracer.launch        #Enable the robot chassis node

JetRacer Odometer Calibration01.png

  • Press "Ctrl + Alt + t" to open the new terminal in the Ubuntu virtual terminal, and enter the following command to enable the keyboard to control the topic nodes.
roslaunch jetracer keyboard.launch     

Jetracer Robot Odometer Calibration03.png

  • Press the "I" key on the keyboard to control the robot to move forward, and observe whether the robot moves in a straight line. If the robot is biased to one side, it can be calibrated in the following way.
  • Open a new terminal again and run the following command to start the rqt_reconfig tool
rosrun rqt_reconfigure rqt_reconfigure 

JetRacer Topics with ROS03.png

  • Adjust the value of servo_bias and modify the initial offset of the servo until the robot can move in a straight line.
  • Dynamic parameter adjustment will only affect the running effect at this time, the parameters will not be saved in the parameter file, and will not affect the next startup. Parameters need to be saved to the jetracer_ros/cfg/jetracer.cfg file.
vi ~/catkin_ws/src/jetracer_ros/cfg/jetracer.cfg

Modify the servo_bias line, modify the fifth parameter to the value just set and save it.
Jetracer Robot Odometer Calibration05.png

Step 2: Linear Velocity Calibration

  • The linear calibration startup program has already started the robot chassis node by default, please close the robot chassis node that was started earlier, and keep the "robot master node" running, otherwise an error will be prompted after running the command.
  • Open a terminal in Jetson Nano and run the following commands to enable the program of linear velocity calibration.
roslaunch jetracer calibrate_linear.launch

Jetracer robot odometer.png

  • Do not close after enabling, otherwise you will not see the calibration option in the virtual machine command box.
  • Run the following commands in the virtual machine and open the dynamic parameter configure interface. If the program of linear velocity calibration runs normally, you can see the "calibrate_linear" option.
rosrun rqt_reconfigure rqt_reconfigure

Jetracer Robot Odometer Calibration.png

  • Among which:
    • Test_distance: the test distance, the default is 1m.
    • Speed is the linear speed of the robot.
    • Tolerance is the error to reach the goal. If the error is too small, it will shake at the target position, otherwise, the error of reaching the target point will be very large.
    • odom_linear_scale_correction is the odometer scaling, and the final corrected value is the linear speed calibration parameter.
    • Start_test starts robot calibration.
  • Put the robot on the ground, mark the robot position, check start_test, the robot starts to move, wait for the robot to stop, and measure the robot movement distance. Record the ratio of the movement distance divided by the target distance, and multiply odom_linear_scale_correction by this ratio as a new parameter. Repeat the test until the actual movement distance is 1m.
  • For example, if the movement is set to 1m and the actual operation is 1.2m, the ratio is 1.2, and the odom_linear_scale_correction is updated to 1x1.2, that is, 1.2 to start the test again. If the effect is satisfactory, the linear calibration parameter is 1.2. If the actual movement is 1.1m, then the odom_linear_scale_correction is updated to 1.2* 1.1 is 1.32. Repeat this until you are satisfied with the effect.

Robot Odometer Calibration03.png

  • Enter the following command in jetson nano, open the catkin_ws/src/jetracer_ros/launch/jetracer.launch file, and modify the value of linear_correction to the odom_linear_scale_correction calibration parameter obtained from the calibration just now. Start the calibration procedure again to actually walk a distance of 1m.
vi ~/catkin_ws/src/jetracer_ros/launch/jetracer.launch       #Modify calibration parameters

Jetracer Robot Odometer Calibration04.png

Step 2: Angular Velocity Calibration

  • Since the rotation angle of the servo is not equal to the steering angle of the tire, we need to collect the steering angle of the tire under different servo angles, and then use a polynomial to fit the relationship between the two.
  • The calibration process of the servos is more complicated. If the requirements are not high, you can skip this step and not re-calibrate.
  • The following trinomial fitting is used in the program, where x represents the steering angle of the tire, y represents the rotation angle of the servo, and servo_bias is the linear calibration parameter. Under normal circumstances, this item should be 0. a,b,c, and d are fitting parameters.
y = a*x^3 + b*x^2 + c*x + d + servo_bias.
  • Before calibrating, you need to unplug the motor wire to keep the motor in a free state. At the same time, it is necessary to modify the parameters so that the servo can directly control its steering, that is, y = x. Therefore, a, b, d, servo_bias are all set to 0, and c is set to 1.
  • Open the jetracer_ros/cfg/jetracer.cfg file and set servo_bias to 0. Modify the servo_bias line, modify the fifth parameter to 0 and save it.
vi ~/catkin_ws/src/jetracer_ros/cfg/jetracer.cfg

Jetracer Robot Odometer Calibration05.png

  • Open jetracer_ros/launch/jetracer.launc file and set "coefficient_a, coefficient_b, coefficient_d" to 0, and set coefficient_a to 1.
vi ~/catkin_ws/src/jetracer_ros/launch/jetracer.launch

Jetracer Robot Odometer Calibration06.png

  • Connect the robot by SSH, open the terminal, and enter the following commands to enable the chassis node of the robot.
  • 【Note: Do not enable the chassis node of the robot repeatedly, otherwise it will be in error. If you have not close the chassis node, you do not need to enable it again.】
roslaunch jetracer jetracer.launch      #Start the robot chassis node
  • Enable the topic publishing function by the command of "rqt_publisher" in the virtual machine, choose "/cmd_vel" to add more topic information.
rosrun rqt_publisher rqt_publisher

Jetracer Robot Odometer Calibration07.png

  • Set the value of "/cmd_vel.augular.z" as -1 and publish. Turn the servo to left and push the JetRacer slowly. Test the turning diameter of the car and record it.
  • Set /cmd_vel.augular.z to [-1, -0.9, -0.8, -0.7, -0.6, -0.5, -0.4, -0.3, -0.2, -0.1, 0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1], and measure and record the turning diameter of the car at different values.
  • Open the program in the virtual machine, and replace the turning diameters at different angles with the actual data just obtained, where the R array is the actual turning diameter, and the corresponding steering gear angle is the y array.
vi ~/catkin_ws/src/jetracer_ros/scripts/servo_calibratin.py

Jetracer Robot Odometer Calibration08.png

  • Run the following command to calibrate the servo.
python ~/catkin_ws/src/jetracer_ros/scripts/servo_calibratin.py    
  • After the program runs, the fitting curve is shown in the figure below, the red line is the fitting result, and the green line is the collected data.

Jetracer Robot Odometer Calibration09.png

  • After the program runs, the output result is shown in the following figure;

Jetracer Robot Odometer Calibration10.png

  • The last line is the fitting parameter, corresponding to the parameters a, b, c, and d, respectively.
  • Save parameters to the startup file.
vi ~/catkin_ws/src/jetracer_ros/launch/jetracer.launch
  • Note: The output in the program is scientific notation and needs to be converted to floating point numbers before saving it to a file.

Jetracer Robot Odometer Calibration11.png

  • After normal calibration, the car should be able to drive in a straight line, and servo-bias is directly 0 without modification. If the car travels in a straight line, you can slightly adjust the servo-bias parameter or directly adjust the d parameter.