Differential drive kinematics and odometry for a mobile robot using TwinCAT

1.   Introduction

Robotics is a critical component of industrial development and automation. It enables significant growth in manufacturing efficiency, productivity, precision and quality, while also increasing safety when performing tasks that would be dangerous for a human operator to perform.

Automation solutions incorporating mobile robots in industry have grown in popularity in the last decades. This is due to an increasing demand for flexible solutions. Autonomous mobile robots are used in multiple areas, such as industrial automation, logistics and warehouse management, health care and services and many other industrial and non-industrial applications ^[1,2].

The basics of mobile robotics can be divided into locomotion, perception, localization and planning and navigation. Locomotion is required for the robot to move in its working environment. It relies on suitable motion control schemes that generate the necessary control actions to the motors' drivers, based on set points and feedback data ^[1,2,3,4].

There are many works focused on the analysis of kinematic models of differential drive vehicles ^[5,6,7,8,9]. Additionally, multiple studies show methods to test the accuracy of these models and how to modify them to reduce errors in odometry ^{[10,11,12,13]}. On the other hand, robot path planning and optimization either in static or dynamic environments represent key issues. Path planning strategies are often classified as (ⅰ) analytical, (ⅱ) enumerative, (ⅲ) meta-heuristic and (ⅳ) evolutionary ^[14]. In particular, the evolutionary methods can be regarded as optimization procedures, and many algorithms have been proposed ^{[15,16,17,18,19,20,21,22]}.

A quality control system is necessary to ensure that the robot can move with high accuracy, which is very important for industrial environments. Motion control systems for mobile robots with various drive types utilizing Beckhoff's TwinCAT automation software have been addressed in a small number of works. For instance, in ^[23] a new remote operated vehicle (ROV) control system based on TwinCAT was developed. The solution allowed the connection of functional modules and hardware and/or software modifications, while reducing the time needed for maintenance. A graphical user interface (GUI) was also created for real-time data monitoring and controlling the ROV. In ^[24] a heavy-duty omni-directional Mecanum robot was presented, together with its control system and simulation design. The control system combined a Beckhoff module and the Robot Operating System (ROS) for low-level motion execution and high-level intelligent navigation tasks, respectively. In addition, a virtual simulation environment was validated. In ^[25] real-time sensor fusion for smooth position feedback of a heavy-duty field robot was addressed. The robot autonomous driving system was based on a Beckhoff hardware platform running TwinCAT. The programming was performed in a MATLAB/Simulink environment, while the functional modules were built on a target TwinCAT system. The robot used wireless communication between the real-time hardware and the host PC. In ^[26] a solution for a network of real-time-capable modules was derived and illustrated on a prototype of a mobile robot using TwinCAT CNC. In ^[27] a novel wheel-legged robotic system was developed. The robot control software was implemented using TwinCAT. In ^[28] a path planning and navigation control system was developed for a 12 m length driverless electric bus. The TwinCAT software system was utilized for path planning and trajectory tracking controller, while ensuring real-time update and synchronization rate. However, to the best of the authors' knowledge, none of these TwinCAT implementations has addressed motion accuracy testing and odometry of the robot, nor motion correction in order to obtain minimum errors. On the other hand, the proposed approach is flexible, completely modular and extendable to accommodate different sensors and actuators, as well as other robotic differential drive platforms utilizing TwinCAT 3.

The present paper proposes a motion control scheme for a low-cost differential drive mobile robot. The robot locomotion platform consists of a differential drive setup using two wheels powered by Beckhoff motors and a free caster wheel. The motion control is developed using Beckhoff's TwinCAT 3 automation software in an industrial PC (IPC). The system is tested and experimentally tuned to achieve excellent performance. Field tests of the robot performing linear and angular trajectories reveal errors below 0.02 and 0.03%, respectively, after tuning its motion parameters. The proposed approach can be extended and applied to other robotic differential drive platforms utilizing TwinCAT 3, being a cost-effective solution for real-world applications.

The paper is organized as follows: Section 2 describes the robot and its hardware; Section 3 presents the development of the motion control system, the kinematics of the robot and the testing and correction of the odometry. Finally, Section 4 outlines the main conclusions.

2.   Robot overview

The mobile robot uses a differential drive platform with two independently driven wheels and one free caster wheel. Each of the driven wheels is equipped with an AS1050-0120 Beckhoff motor with an incorporated incremental encoder. The resolution of the encoders is 1.8° or 200 increments per revolution. The motors are coupled with a gearbox with a gear ratio of 5. The robotic platform is also equipped with an anthropomorphic robot arm, which is not addressed in this paper. A picture of the robot and the two independently driven wheels can be seen in Figure 1, along with their respective Beckhoff stepper motors and gearboxes ^[29].

The electrical cabinet can be seen in Figure 2. It contains three different power supplies, one with 48 V, one with 24 V and one with 5 V:

1) The 48 V power supply is used to power 5 EtherCAT terminals (3 EL7047 and 2 EL7041) that connect to 3 stepper motors used in the robot arm and 2 stepper motors used for the robot's locomotion.

2) The 24 V power supply is used to power the EtherCAT terminal EL9800, which supplies 2 EtherCAT EL7037 modules that connect to 2 other stepper motors used in the robot arm. It also powers their encoders, the robot's IPC, the EtherCAT coupler EK1100 and the input (EL1008) and output terminals (EL2008).

3) The 5 V power supply is used for three of the encoders in the robot arm and for the two encoders used for locomotion.

Figure 3 presents a schema of the electrical power used in the robot. The IPC used in this robot is the C6015-0010 model from Beckhoff, running with Windows 10 as its operating system and with TwinCAT 3 installed. The robot is also equipped with an emergency button that, when activated, turns off all circuits that send power to the motors, while keeping the command circuits on, including the IPC and the EtherCAT communication.

3.   Motion control system

The software used for the control and command of the robotic system is TwinCAT 3. TwinCAT 3 is an automation software solution for PC-based control that can turn a PC with EtherCAT communication into a real time controller with multiple subsystems running simultaneously. It consists of a XAR (eXtended Automation Runtime) that runs the already programmed control modules and the XAE (eXtended Automation Engineering) that, using Microsoft Visual Studio, allows the user to setup, modify and program the control system to be used.

TwinCAT 3 uses a number of modules that communicate with one another to form a complete controller. There are modules for the system configuration (SYSTEM), motion control (MOTION), PLC programming (PLC), safety (SAFETY), C++ programming (C++), analytics (ANALYTICS) and I/O configuration (I/O). This paper does not make use of the SAFETY, C++ or ANALYTICS modules.

When configuring the system, the software in the I/O module recognized all connected EtherCAT devices. Furthermore, the motor drivers were configured, which included entering multiple motor properties, such as maximum current, nominal voltage, number of steps per revolution and encoder resolution. The different movement axes were configured in the MOTION module. This module is responsible for the numerical control of the movement axes. It can use point-to-point control to generate the signals needed to send to the appropriate I/O device. It has several tabs where different fields must be configured correctly. The PLC module implements the system logic and is responsible for the overall control of the robot's motion system. This module is programmable according to the IEC 61131-3 international standard for programmable PLCs, which supports the following programming languages: ladder diagram (LD), function block diagram (FBD), sequential function chart (SFC), structured text (ST) and instruction list (IL). In the case of this paper, every program in the PLC module was developed in ST.

The robot used in this paper was also simultaneously used for the development of an autonomous navigation system using ROS. As such, the objective was for TwinCAT to receive velocity commands from the ROS system while simultaneously sending odometry information to the ROS environment. This information flow is shown in Figure 4.

The ROS Navigation Stack uses the move_base package. This package generates the following commands: linear velocity in x (V_x), linear velocity in y (V_y) and angular velocity in θ (V_θ). These directions are relative to the robot's current position, in accordance with Figure 5. This specific robot can only have velocities in the x and θ directions since it is a differential drive robot. Because there are no other possible velocities that a differential drive robot can take, V_x and V_θ are also respectively known as linear and angular velocity.

3.1.   Differential drive kinematics

Because TwinCAT 3 controls the movement of both wheel axes independently, the robot's linear and angular velocities must be converted into velocities for the left and right wheels. Furthermore, odometry information about the robot must be obtained using information from the encoders.

TwinCAT's MOTION module was configured with information about the motors and transmission systems used. As a result, it can use wheel velocities specified in mm/s and automatically read information from encoders in mm. As such, it is only necessary to transform V_x and V_θ into V_R (velocity of right wheel) and V_L (velocity of left wheel), and 𝛿_R and 𝛿_L into 𝛿_x, 𝛿_y and 𝛿_θ, where 𝛿 means the variation of position between each control cycle.

In differential drives, linear and angular velocities are given as functions of the velocities of the wheels, according to ^[5]:

where B is the axial distance between the driven wheels. Adding and rearranging Eqs (1) and (2) results in

Since the increments in position of each wheel can be obtained in mm from the encoder data in TwinCAT, linear and angular displacements of the robot are given by

The length of the red arc in Figure 6 is given by 𝛿_x (the size is exaggerated for purposes of easier understanding). The linear distance, d, between the two points (green line) can be obtained with the trigonometric relations:

By adding the previous two equations, Eq (9) is obtained:

This equation, however, can only be used if the robot did not move in a straight line between two cycles of the program. If 𝛿_θ is 0, then Eq (10) is used instead:

The position of the robot in relation to its starting frame can be obtained over time by adding the linear and angular displacements between two cycles of the program (k + 1 and k) to the last calculated position of the robot, using

The angle used to calculate x and y is the average value of the angular position in each of the two cycles to reduce errors. Initial position is initialized to (0, 0, 0). Since usually these cycles are calculated at very high frequencies, the distance between two points in an arc given by Eq (9) can be approximated by the length of the arc itself with negligible error. Lower cycle frequency and higher linear and angular velocities result in a higher error when using this approximation. Herein, the cycle time of the program is 10 ms. At the robot's maximum linear velocity (V_x) of 0.3 m/s and maximum angular velocity (V_x) of 0.7 rad/s, the length of the arc traveled between each cycle (𝛿_x) is 3 mm, and the angular displacement (𝛿_θ) is 0.007 rad. Using these values in Eq (9) results in a linear distance traveled (d) of approximately 2.999994 mm. This means that, in this case, the maximum error obtainable from the use of this approximation is around 0.0002%.

3.2.   Implementation in TwinCAT 3

The kinematics was implemented in a program developed in ST language in TwinCAT. It was necessary to take the velocity commands and make the wheels move at the respective wheel velocities until a new velocity command is obtained. To achieve this, the MC_MoveVelocity function block was used. This function block starts a continuous movement with specified velocity and direction. However, this function block cannot receive a null velocity as an input, which means that an additional function block must be used for when the velocity of any of the wheels is zero. As such, the MC_Halt function block was used for these cases, as it stops an axis's movement with a defined breaking ramp.

Since the MC_MoveVelocity function block only takes velocity inputs larger than 0 and a forward or negative direction value, IF conditions are written for all nine possible combinations of positive, null, and negative velocity values of each wheel. The velocity command variables V_x and V_θ and the odometry variables x, y and θ are all set as global variables to allow them to be written and read by ROS. The program also reads the current velocity of each wheel and calculates the linear and angular velocities the robot is moving at currently, as these are necessary odometry data for ROS. The odometry is calculated using the difference between the absolute values of the encoders in the current and last cycles. During the first cycle of the program, odometry values are set to (0, 0, 0), the motors are powered, and their axes are reset.

Because the MC_MoveVelocity and MC_Halt function blocks require a rising edge signal of the Execute Boolean input, it is also necessary to add the following lines present in Figure. These lines feed the mentioned function blocks an input of "Execute = FALSE". This allows the robot's movement to be flexible and able to change velocities at any moment.

4.   Odometry testing and correction

In order to minimize the trajectory errors, the values of the robot parameters need to be properly determined. This problem is challenging, but a deep analysis is out of the scope of this paper. As such, herein, an empirical tuning approach was adopted based on experimental testing and error analysis, by comparing the actual odometry trajectories with those recorded by an external measurement system. Naturally, this method is simple, but it does not guarantee optimal robot behavior. Different methods, namely, parameters' optimization based on evolutionary algorithms, can be an effective means to address the issue ^[30,31].

The quality, and magnitude of the errors, of the implemented odometry were tested by comparing the odometry values with the real trajectory of the robot. As such, a pen was attached to the robotic platform at the robot's rotation axis. A component to attach the pen to the robot in the position of its frame was modeled and 3D printed (Figure 8). Its geometry only allows the pen to move in the vertical axis, ensuring that it can stay in contact with the ground while the robot moves. A spring was inserted inside to guarantee that there is tension between the pen's ballpoint and the ground.

To make the robot perform specific movements that can be repeated, another TwinCAT program was also written in ST. This program includes the odometry functionalities already implemented, but the robot movements are specified for a set distance and relative to the position at the start of the movement. To achieve this, the program utilizes the MC_MoveRelative function block. It can be used to perform linear, circular or rotational trajectories.

The quality of the linear odometry was tested by measuring the length of the line drawn when instructing the robot to move 1 m forward (Figure 9). This test was done at a linear velocity of 0.025 m/s and repeated three times. The results of the linear odometry tests are shown in Table 1.

The results of the linear odometry test show that the robot's odometry was giving values lower than the measured values by around 1.03%. To counteract this error, the scaling factor parameter of the motor's encoder was changed in TwinCAT. This scaling factor specifies how much each wheel moves in mm per increment of the encoder. The scaling factor is given by:

where SF is the scaling factor, D is the diameter of the wheel, i is the gear ratio of the transmission system, and e is the number of encoder increments per rotation.

To correct the linear odometry of the robot, it is necessary to divide the scaling factor by (1 + %Error):

Since e and i are constant, any changes to the SF can also be interpreted as changes to the wheel diameter considered. The original SF used in this robot was 0.015340 mm/Inc (mm per increment of the encoder). This was obtained using a wheel diameter of 100 mm in Eq (14). In this case, using Eq (15) a corrected SF of 0.015498 mm/Inc is obtained. This equates to considering a wheel diameter of around 101.0 mm. The previous linear odometry tests were repeated using the corrected scaling factor. The results are shown in Table. We verify now an average error of around 0.02%, or 0.2 mm, which is within the resolution of the ruler used to measure the line length.

Angular odometry tests were performed after the correction of the linear odometry with the new scaling factor. The robot was programmed to perform an in-place rotation of 360°. A line parallel to the wheel's initial and final positions was drawn on a piece of paper on the floor. The angle between these two lines was measured and compared to the angle given by the robot's odometry. This test was done at an angular velocity of 0.182 rad/s and repeated three times. The results are summarized in Table.

Angular displacement is given by Eq (6). Considering that the linear odometry is already corrected, angular odometry can only be corrected by changing the value of the distance between wheels (B), which is inversely proportional to the angular displacement. It can be corrected by:

In this case, the measuring error was considered too large in comparison with the odometry error to take any conclusions. Because of this, a new series of tests was conducted by commanding the robot to perform a 1080° in-place rotation with the same angular velocity of 0.182 rad/s. This larger movement results in an increased odometry error, while maintaining similar measurement errors, which allows for a more accurate percentual error to be calculated. Results of these tests are shown in Table 4.

By applying the average error obtained from these tests to Eq (16) using the original value of B of 550 mm, a corrected distance between wheels of 545.38 mm is determined. The same angular odometry tests were repeated using the corrected value of B. The results are shown in Table 5.

In a 1080° trajectory, the results now indicate an average error of 0.03°, or approximately 0.03%, which is within the repeatability errors of the tests. As a result, the value of 545.38 mm for the corrected distance between wheels is accepted.

When using different values for the velocities, the odometry errors appeared to be similar due to low wheel slippage occurring in the testing environment.

5.   Conclusions

A motion control system for a low-cost differential drive mobile robot was proposed, based on Beckhoff's TwinCAT 3 automation software running on an IPC. An empirical tuning approach based on experimental testing and error analysis, by comparing the odometry trajectories with those measured with an external measurement system, was implemented. The approach led to good robot performance, with errors below 0.02 and 0.03% while performing linear and angular trajectories, respectively. The method is simple, but it does not guarantee optimal robot behavior. Further work will address parameter optimization, namely, the adoption of evolutionary algorithms, and additional tests to further assess the accuracy of the system, including verifying the accuracy of each wheel individually and testing differences between clockwise and counterclockwise movement. The proposed approach is flexible and modular, and it can be extended, adjusted and used in other robotic differential drive based solutions utilizing TwinCAT 3.

Conflict of interest

The authors declare there is no conflict of interest.