table of Contents

In this topic, we will introduce how to build a ROS chassis using mecanum wheels. With reference to the standard ROS chassis, the ROS chassis should be able to receive the control commands on the topic "cmd_vel" sent by the navigation node (the speed of the car in X, Y, and Z directions and the angular speed in the three directions), and the speed of the chassis wheels The information estimates the position of the chassis and publishes it on the Odometry topic.

The whole topic is divided into four parts:

1. First introduce the hardware structure of the car,

2. Next, introduce the kinematic model of the Mecanum wheel,

3. Then use the remote control to control the mecanum wheel to move back and forth, left and right, to achieve remote control,

4. Finally, write a ROS node to transfer the data of the "cmd_vel" topic to the chassis.

All the codes are placed in Github , welcome to download and leave a message.

1. Chassis hardware block diagram

The main task of the chassis is to receive instructions from the host computer (that is, the instructions of the ROS node), control the speed of each motor, and upload the information of sensors and actuators such as IMU, ultrasonic, and motor speed on the chassis to the chassis node in ROS. Here we involve sensors and actuators respectively: 2.4G remote control receiver, IMU, ultrasonic sensor, 4-way DJI M3508 motor and driver. Among them, the remote control is used to assist the remote control chassis and switch control methods, the ultrasonic is used to make up for the blind area of the lidar, and the IMU is used to measure the attitude and heading angle of the chassis (in some chassis, IMU information is also used to make the closed loop of the corner control)

Figure 1-1 Hardware block diagram of the car

The actual hardware of the car is shown in the figure below. Define the motor number in the upper left corner as #1, the lower left as #2, the lower right as #3, and the upper right as #4. The front is the X direction, and the horizontal direction is the Y direction (the same as the coordinate system in ROS).

Figure 1-2 The actual hardware of the car

The development task of the chassis is realized in four steps:

The first step is to open up the communication link to read the sensor (DBUS receiving, ~~IMU data reading, ultrasonic data reading~~ ) and motor driver (C620) data.

Step 2: Use PI controller to realize the speed closed-loop control of the motor.

The third step: use the mecanum wheel speed inverse solution model to realize the speed control in the three directions of X/Y/W.

The fourth step: perfect the host computer to send and receive instructions protocol, upload IMU, ultrasonic, motor speed information.

Next, we first introduce the kinematics model of the Mecanum wheel. This kinematics model is mainly to facilitate our speed in the three directions of X, Y, W and the speed of the four motors (V1, V2, V3, V4) Establish a mapping relationship between. When controlling, the navigation module gives the speed in the X and Y directions (m/s) and the rotation speed W (rad/s) of the car. When publishing the odometer, we need to read the speed or rotation of the four motors To calculate the position coordinates and heading angle of the trolley.

2. Introduction to Mecanum Wheel Model

First of all, let's analyze how the mecanum wheel chassis realizes front and rear, left and right, and rotational movements from a macro perspective.

a. Forward and backward movement b. Left and right movement

c. 45 degree diagonal movement d. Rotation movement

Figure 2-1 Schematic diagram of the kinematics model of Mecanum wheel

The mecanum wheel is installed in a "meter" shape, as shown in Figure 1. Suppose we need to control the following movements of the car:

Move forward, then four wheels need to be turned forward
When moving to the left, the left front and right rear wheels need to be turned backward, and the left and right front wheels must be turned forward
When moving forward to the right, the front left and right wheels rotate forward
When rotating counterclockwise, left front and left back rotate backward, right front and right back rotate forward

In the reference [1] blog, a classic tutorial explaining the mecanum wheel has been given, but the catch is that the pictures in this tutorial can’t be displayed because of unknown reasons, so refer to the tutorial on the official website of DJI. Finished.

First, define the left and right sides of the trolley as the Y direction, where leftward is positive; the front and rear are the X direction, where the front is positive; the trolley rotates counterclockwise as positive ${w_v}$ . Assuming that the speed of the car is $\ rm {v = [}} {{\ rm {v}} _ x} \; {{\ rm {v}} _ y} \; {w_v} {\ rm {]}$ , the Mecanum wheel is labeled from the upper left corner to the upper right corner as N=1,2,3,4. (Xn, Yn) is the coordinates of the nth wheel relative to the origin, and the speed is ${{\ rm {v}} _ n} {\ rm {= [}} {{\ rm {v}} _ {xn}} \; {{\ rm {v}} _ {yn}} {\ rm { }} \; {w_n} {\ rm {]}}$ expressed by.

Each wheat wheel hub is installed parallel to the X axis (usually installed in a meter self-shape), r is the radius of the hub, and the deflection in the counterclockwise direction is positive. $\theta _N$ It is the angle between the axis of the grounding roller of the wheat wheel n and the X axis [that is, the offset installation angle of the roller, the size is 45°]. The roller is the one inclined on the Mecanum wheel. The speeds of the 4 wheels are respectively ${{\ rm {v}} _ n} {\ rm {= [}} {{\ rm {v}} _ {xn}} \; {{\ rm {v}} _ {yn}} {\ rm { }} \; {w_n} {\ rm {]}}$ . After defining these, we begin to derive the kinematic model of the Mecanum wheel.

Figure 2-2 Hardware block diagram of the car

2.1 Inverse kinematics model

Knowing the speed of the four wheels to $\Omega {\rm{ = [}}{w_1},{w_2},{w_3},{w_4}{{\rm{]}}^T}$ solve the speed of the car ${\ rm {v = [}} {{\ rm {v}} _ x} \; {{\ rm {v}} _ y} \; {w_v} {{\ rm {]}} ^ T}$ , that is, to find a 4x3 matrix R such that it satisfies:

$\Omega {\rm{ = }}\frac{1}{{\rm{r}}}RV$ （2.1）

Assuming that the car body is a rigid body, the horizontal and vertical speed components of the center of each wheat wheel ${{\ rm {v}} _ n} {\ rm {= [}} {{\ rm {v}} _ {xn}} \; {{\ rm {v}} _ {yn}} {\ rm { ]}}$ are equal to the horizontal and vertical speed components of the car ${\ rm {v = [}} {{\ rm {v}} _ x} \; {{\ rm {v}} _ y} {\ rm {]}}$ . In addition, because the car body has a rotation speed ${w_v}$ , it ${w_v}$ will also generate a speed component (horizontal and vertical) on each wheat wheel ${{\ rm {v}} _ n} {\ rm {= [}} {{\ rm {v}} _ {xn}} \; {{\ rm {v}} _ {yn}} {\ rm { ]}}$

$\ begin {array} {l} {{\ rm {v}} _ {xrn}} = {\ rm {- Yn}} \ cdot {w_n} \\ {{\ rm {v}} _ {yrn}} = X {\ rm {n}} \ cdot {w_n} \ end {array}$ （2.2）

Therefore, in general, the speed components of each wheat wheel in the horizontal and vertical directions are:

$\ begin {array} {l} {{\ rm {v}} _ {xn}} {\ rm {=}} {{\ rm {v}} _ x} {\ rm {+}} {{\ rm { v}} _ {xrn}} = {{\ rm {v}} _ x} {\ rm {-}} {{\ rm {Y}} _ n} \ cdot {w_n} \\ {{\ rm {v} } _ {yn}} {\ rm {=}} {{\ rm {v}} _ y} {\ rm {+}} {{\ rm {v}} _ {yrn}} = {{\ rm {v }} _ y} {\ rm {+}} {X_n} \ cdot {w_n} \ end {array}$ （2.3）

At this point, we have found the relationship between ${\ rm {v = [}} {{\ rm {v}} _ x} \; {{\ rm {v}} _ y} \; {w_v} {\ rm {]}}$ the speed ${{\ rm {v}} _ n} {\ rm {= [}} {{\ rm {v}} _ {xn}} \; {{\ rm {v}} _ {yn}} {\ rm { ]}}$ of the car body and the speed on each wheat wheel , and then we only need to find the relationship between the speed of each wheel ${w_n}$ and ${v_n}$ the construction is complete. The figure below shows the velocity vector relationship of the upper right wheel at a certain moment.

Figure 2-3 Hardware block diagram of the car

The resultant velocity vector Vn in Figure 2-3 is the current actual movement velocity of wheat wheel n, which is synthesized by velocity components Vxn and Vyn. It can also be seen in the figure that Vn is synthesized from the hub center speed Aî=ωn·r of the wheat wheel n and the center speed Bû of the grounded free roller ( the center speed B of the free roller changes with the speed vector Vn ). So there is the equation:

${w_n} r = {{\ rm {v}} _ {xn}} {\ rm {+}} {{\ rm {v}} _ {in}} * \ tan {\ theta _n}$ （2.4）

Transformed into:

$\begin{array}{l} {w_n} = \frac{1}{r}[({{\rm{v}}_x}{\rm{ - }}{{\rm{Y}}_n} \cdot {w_n}){\rm{ + }}({{\rm{v}}_y}{\rm{ + }}{X_n} \cdot {w_n})\tan {\theta _n}]\\ \;\;\;\;\; = \frac{1}{r}[{{\rm{v}}_x}{\rm{ + }}{{\rm{v}}_y}\tan {\theta _n}{\rm{ + }}({X_n}\tan {\theta _n} - {{\rm{Y}}_n}) \cdot {w_n}] \end{array}$ （2.5）

For 4 wheels:

$\left[ {\begin{array}{*{20}{c}} {{w_1}}\\ {{w_2}}\\ {{w_3}}\\ {{w_4}} \end{array}} \right]{\rm{ = }}\frac{1}{{\rm{r}}}\left[ {\begin{array}{*{20}{c}} 1&{\tan {\theta _1}}&{{X_1}\tan {\theta _1} - {{\rm{Y}}_1}}\\ 1&{\tan {\theta _2}}&{{X_2}\tan {\theta _2} - {{\rm{Y}}_2}}\\ 1&{\tan {\theta _3}}&{{X_3}\tan {\theta _3} - {{\rm{Y}}_3}}\\ 1&{\tan {\theta _4}}&{{X_4}\tan {\theta _4} - {{\rm{Y}}_4}} \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {{{\rm{v}}_x}}\\ {{{\rm{v}}_y}}\\ {{{\rm{v}}_z}} \end{array}} \right]\;$ （2.6）

which is: $\Omega {\rm{ = }}\frac{1}{{\rm{r}}}RV$

According to the definition of θn in the first figure , θ1 and θ3 are -45°, and θ2 and θ4 are +45° , so the R matrix is:

${\rm{R = }}\left[ {\begin{array}{*{20}{c}} 1&{ - 1}&{ - {X_1} - {{\rm{Y}}_1}}\\ 1&1&{{X_2} - {{\rm{Y}}_2}}\\ 1&{ - 1}&{ - {X_3} - {{\rm{Y}}_3}}\\ 1&1&{{X_4} - {{\rm{Y}}_4}} \end{array}} \right]$ （2.7）

Usually the chassis we design is axially symmetric, that is, the distance from the origin to the four wheels in the coordinate system is equal. Therefore, the absolute values of the horizontal and vertical coordinates of each wheel are equal. Let K=abs(Xn)+abs(Yn). According to the sign of the quadrant of each wheel coordinate, the matrix can be simplified to:

${\rm{R = }}\left[ {\begin{array}{*{20}{c}} 1&{ - 1}&{ - {\rm{K}}}\\ 1&1&{ - {\rm{K}}}\\ 1&{ - 1}&{\rm{K}}\\ 1&1&{\rm{K}} \end{array}} \right]$ （2.8）

In addition, considering the linear combination of the two unit vectors of î and û, it just forms the entire vector space in the XOY plane (of course, the speed cannot be infinite, but the direction must be omnidirectional). The reason for the movement. Note that Bû is always perpendicular to the axis of the roller, because the roller must roll in a direction perpendicular to its axis of rotation, just like Aî is always perpendicular to the axis of the motor (parallel to the Y axis).

2.2 Positive kinematics model

Positive kinematics refers to knowing the speed of each motor and finding the speed of the car body. In mathematical form is to find a 3x4 matrix [F] to satisfy:

$\;{\rm{F}}\Omega {\rm{r = }}V$ （2.9）

The method is to provide the matrix of the optimal solution through the least square method [F] , starting from the inverse kinematics equation,

$\Omega {\rm{ = }}\frac{1}{{\rm{r}}}RV$ （2.10）

Multiply both sides by the transpose of [R] at the same time to get:

${R^{\rm{T}}}\Omega {\rm{ = }}\frac{1}{{\rm{r}}}{R^{\rm{T}}}RV$ （2.11）

Multiply both sides by the inverse of [R]'[R] at the same time to get:

${({R^{\rm{T}}}R)^{ - 1}}{R^{\rm{T}}}\Omega {\rm{ = }}\frac{1}{{\rm{r}}}V$ （2.12）

The right side is reduced to [V] to get :

${({R^{\rm{T}}}R)^{ - 1}}{R^{\rm{T}}}\Omega {\rm{r = }}V$ （2.13）

This gives [F]:

$\; {\ rm {F}} \ Omega {\ rm {r =}} V \ ;, {\ rm {F =}} {({R ^ {\ rm {T}}} R) ^ {- 1 }} {R ^ {\ rm {T}}} $$ （2.14）

Using the simplified [R] matrix in inverse kinematics , it is easy to find [F] :

note:

$V$ The unit of speed in formula 2.1 is m/s, and $\Omega$ the unit of rotation speed is rad/s (radians per second). The feedback speed of DJI C620 motor encoder is rpm (rotation per minute), which needs to be converted ( $\Omega$ * 2 * $\pi$ = rpm/60)

3 Chassis software framework

In the previous chapter, we introduced the basic framework of the Mecanum wheel chassis and the kinematic model of the Mecanum wheel. Here, we use the remote control of DJI to control the chassis to move forward, backward, left and right.

The chassis is based on STM32F1, and the chassis can be divided into the following functional modules:

C620 ESC CAN drive module, receiving motor encoder feedback data through CAN interrupt
Receive remote control data of DBUS protocol
Set speed PID closed loop
Upload the rotation angle, speed and temperature of 4 motors to the host computer

Specifically, first use the serial port 1 to receive the data from the receiver, and then use the mecanum wheel kinematics model to convert the speed in the three directions of X, Y, and W into the target speed (RPM) of the four motors and send it to PI Control as the target speed, and then the PI controller receives the target speed of the four wheels to achieve speed closed-loop control, calculates the control value, and transmits it to the motor through the CAN bus. Finally, the status data upload module regularly uploads the motor, IMU, and ultrasonic data on the chassis to the host computer.

Wiring instructions:

For connection, you only need to connect the CAN bus of the C620 ESC to the STM32, the serial port 1 receiving pin is connected to the receiver (an external inverter is required between the receiver and the STM32), and the serial port 2 is connected through a USB to serial line Just go to the host computer.

Note : (The SW1 of DJI remote control needs to be switched to CL or HL to be effective, which means the remote control is in control, and the OFF position means to cancel the remote control and it is controlled by the host computer)

The code is not posted, and interested friends download it ( code of the chassis ). At this point, we can basically use the remote control to remotely control the Mecanum wheel chassis to realize the three directions of movement of front, back and spin. Finally, paste an effect picture.

4 references

[1]: DJI Forum, https://bbs.robomaster.com/thread-3960-1-1.html

[2]: 《Kinematic Analysis of Four-Wheel Mecanum Vehicle》

[3]: Code address: https://github.com/RuPingCen/mick_robot_chasiss

Next: ROS chassis production-Mecanum wheel model (below)

For friends who have running problems, please email or leave a message to discuss [email protected]

Making of ROS Mecanum Wheel Chassis (Part 1)