Differential Drive Control Plugin#

The Differential Drive Control plugin provides a means for driving differential drive robots/ego vehicles. Differential drive is a drive mechanism commonly used in mobile robots where two wheels are mounted on a common axis and each is capable of being driven forward or backward independently.

Differential drive robots often have wheel encoders that precisely track the rotation of each wheel. The rotation of each wheel can be used both as feedback for a closed loop controller to precisely control the robot motion, and for dead reckoning.

The differential drive control plugin of the SVL Simulator, subscribes to control velocity values in the form of geometry_msgs/Twist message types (see definition), implements a PID controller to control each wheel, and publishes an odometric pose in the form of a nav_msgs/Odometry message (see definition).

Wheel Control#

Setup#

To be able to control the wheels on the robot/ego vehicle, the control plugin needs to be able to locate the GameObjects for each drive wheel in Unity. To maintain generality and avoid restricting the control plugin to a particular robot model the path from the main body to each wheel is defined as a customizable parameter which can be edited through the simulator's web user interface.

The figure below shows the expanded hierarchy for the LG CLOi robot as seen in the Unity Editor. The path to each of the wheels can be followed through the tree and entered into the web user interface.

cloi_tree

Sensor configuration for the Differential Drive Control plugin in the web user interface is seen below.

diffdrive

The path to the two wheels are defined under LeftWheelLinkPath and RightWheelLinkPath.

Calculating wheel motion#

The incoming control command expected by the plugin has a linear (\(V\)) and an angular velocity (\(\omega\)) component. Using this input, as well as the wheel separation (\(l\)) and wheel radius (\(R\)) the rotation of each wheel can be computed as follows.

\[ \omega_r = (V + \omega . l/2)/R \]
\[ \omega_l = (V - \omega . l/2)/R \]

The angular velocities \(\omega_l\) and \(\omega_r\) are the rotation rates of the left and right wheels which are applied to their respective articulated bodies in Unity.

Note that the control plugin only expects an \(x\) component on the commanded linear velocity and a \(z\) component for the commanded angular velocity.

Calculating the odometry pose#

The odometry pose is calculated relative to the starting position of the robot. The yaw angle of the robot is calculated through an internal IMU, and is used alongside the rotation of each wheel as observed by the wheel odometers to calculate the pose of the robot/ego vehicle at each instance. The odometric pose of the robot is expected to drift over time as it does not account for wheel slippage which can be significant.

The pose is published using a nav_msgs/Odometry message type. Since the odometry pose cannot estimate changes in elevation, the \(z\) component of the pose is not needed and is often left as 0. Here we are using this field to convey the yaw angle of the robot.