Motion Control

Motion control is all about making the robot move. This is done by creating a reference for the desired motion, given in the robot local coordinates. Typically, a linear and angular velocity. The forward kinematics then transforms these references to the required wheel velocity references. The wheel velocity references are sent to the controller board, which actuates the wheels. The actual wheel velocities are found using wheel encoders. This data is available from the controller board, and is sent to the mechanical odometry.

The figure below illustrates how this is implemented in ROS 2.

Figure: Motion control communication diagram.

Motion Reference

The motion references for the uiabot is linear and angular velocities, given in the local coordinate frame. Generating these references are done by publishing the desired linear and angular velocity in a geometry_msgs/Twist message to the /cmd_vel topic.

The figure above shows this by using the built-in teleop_twist_keyboard node, that converts keyboard input to linear and angular velocity. Another solution would be to create a node that generates a trajectory and publishes a geometry_msgs/Twist message to the /cmd_vel topic.

Note

Command to run the teleop_twist_keyboard node:

ros2 run teleop_twist_keyboard teleop_twist_keyboard

Inverse Kinematics

To transform the linear and angular velocity references to wheel amgular velocity references we need the inverse kinematics of the robot. The following figure defines the robot local coordinate system, and positive linear wheel velocites.

Figure: Robot definition.

Each wheel has the linear velocities \(\dot{x_r}\) and \(\dot{x_l}\), corresponding to right and left wheels, respectively. As shown in the figure below, the wheel angular velocity is denoted by \(\dot{\phi}\), and wheel radius by \(r\).

Figure: Wheel definition.

The linear velocities for the wheels can be written as.

(1)\[\begin{split}\dot{x}_{r} = \dot{\phi}_{r}r \\ \dot{x}_{l} = \dot{\phi}_{l}r\end{split}\]

Considering pure translational velocity

We first consider that the robot has pure translational velocity. Since we are looking at the robot in the local coordinate frame, the linear velocity is composed of a velocities along the x- and y-axis. However, assuming zero slip, which means \(\dot{y}=0\). The linear velocity is only \(\dot{x}\). For the robot to have pure translation velocity, both wheels will need to have the same angular velocities. This yields the following equation.

(2)\[\begin{split}\begin{align} \dot{x}_{r} = \dot{x}_{l} = \dot{x} \\ \Rightarrow \dot{\phi}_{r}r = \dot{\phi}_{l}r = \dot{x} \\ \Rightarrow \dot{\phi}_{r} = \dot{\phi}_{l} = \frac{\dot{x}}{r} \end{align}\end{split}\]

Considering pure angular velocity

We then consider that the robot has pure rotational velocity. The robot rotational velocity is denoted as \(\dot{\theta}\). For the robot to have pure rotational velocity, the wheels will need to have opposite angular velocites.

(3)\[\dot{\phi}_{r} = -\dot{\phi}_{l}\]

Assuming no lateral movement, the wheels must follow a circle, which yields the following equations for the right (4) and left wheel (5).

(4)\[\dot{\phi}_{r}r = \frac{L}{2}\dot{\theta} \Rightarrow \dot{\phi}_{r} = \frac{L}{2}\frac{\dot{\theta}}{r}\]

(5)\[\dot{\phi}_{l}r = -\frac{L}{2}\dot{\theta} \Rightarrow \dot{\phi}_{l} = -\frac{L}{2}\frac{\dot{\theta}}{r}\]

Combining the velocities

Since both the definintion of the linear (6) and angular velocites are valid in vector-space, these can be added together for the right and left wheel in equation (6) and (7). This yields expressions for the wheel angular velocities, \(\dot{\phi}_r\) and \(\dot{\phi}_l\), given the linear and angular robot velocites, \(\dot{x}\) and \(\dot{\theta}\).

(6)\[\dot{\phi}_{r} = \frac{\dot{x}}{r} + \frac{L}{2}\frac{\dot{\theta}}{r}\]

(7)\[\dot{\phi}_{l} = \frac{\dot{x}}{r} - \frac{L}{2}\frac{\dot{\theta}}{r}\]

The inverse kinematics can then be written in matrix form as.

(8)\[\begin{split}\begin{bmatrix}\dot{\phi}_r \\ \dot{\phi}_l \end{bmatrix} = \frac{1}{r} \begin{bmatrix}1 & \frac{L}{2} \\ 1 & -\frac{L}{2} \end{bmatrix}\begin{bmatrix}\dot{x} \\ \dot{\theta}\end{bmatrix}\end{split}\]

The inverse kinematics of the robot is implemented in the control node.

Note

Command to run the control node:

ros2 run uiabot control

Forward Kinematics

The forward kinematics is used to transform wheel angular velocities, \(\dot{\phi}_r\) and \(\dot{\phi}_l\), to robot angular and linear velocities, \(\dot{x}\) and \(\dot{\theta}\), given in the local coordinate frame. Based on equation (8) from the inverse kinematics, we can solve for \(\dot{x}\) and \(\dot{\theta}\). Which gives the following equation.

(9)\[\begin{split}\begin{bmatrix}\dot{x} \\ \dot{\theta}\end{bmatrix} = \begin{bmatrix}\frac{r}{2} & \frac{r}{2} \\ \frac{L}{2} & -\frac{L}{2}\end{bmatrix} \begin{bmatrix}\dot{\phi}_r \\ \dot{\phi}_l \end{bmatrix}\end{split}\]

The forward kinematics of the robot is implemented in the mechanical_odometry node.

Note

Command to run the mechanical_odometry node:

ros2 run uiabot mechanical_odometry

Motor Controller

On the uiabot, the motors are driven by a ODrive controller board, which is connected to the Jetson Nano through USB. To make the ODrive available on the ROS 2 network, an interface node is created: odrive-ros2. This node makes it possible to publish velocity references to each wheel.

Note

Command to run the odrive_ros2 node:

ros2 run odrive_ros2 odrive_ros2