Simple Discrete-time Model of Mobile Robot for Localization, SLAM, and Dead Reckoning – Fusion of Engineering, Control, Coding, Machine Learning, and Science

In this robotics and control engineering tutorial, we explain how to develop a simple discrete-time model of a mobile robot that can be used for the development of algorithms for localization, Simultaneous Localization And Mapping (SLAM), and dead reckoning.

We develop a discrete-time kinematics model and we explain several important aspects of this model. This model is used in our other tutorials for the development of robotics algorithms. This is arguably the simplest possible model of a mobile robot. For simplicity and clarity of this tutorial, we focus on a differential drive mobile robot introduced in our previous tutorial given here. Consequently, we strongly suggest to the reader of this tutorial to first thoroughly read and understand the material presented in our previous tutorial.

Although simple, the model developed in this tutorial can still be used in practical applications and the modeling ideas presented in this tutorial can be used as an inspiration to develop models of robots with various configurations.

We use two approaches to develop the discrete-time kinematics model. Both approaches result in the same discrete-time model. The first approach is based on the direct discretization of the continuous-time model of the robot kinematics. The second approach uses simple trigonometry and physical intuition to develop the model. The YouTube tutorial accompanying this tutorial is given below.

Continuous-Time Model of Mobile Robot

The figure below shows a graphical representation of a differential drive robot introduced in our previous tutorial given here.

Figure 1: Graphical representation of a differential drive robot.

The robot consists of the robot base with two active (actuated) wheels and a third passive caster wheel. The third passive caster wheel provides the stability of the robotic platform (passive means that is not actively actuated, however, the wheel can still move around two perpendicular axes). This third wheel is not shown on the graph for clarity. We introduce the following quantities:

Inertial coordinate system $X$ – $Y$ (denoted by uppercase letters $X$ and $Y$ ). This coordinate system is fixed.
Body coordinate system $X_{B}$ – $Y_{B}$ (denoted by uppercase letters $X_{B}$ and $Y_{B}$ ). The body coordinate system is also called the body frame. This coordinate system is rigidly fixed to the robot body or robot base. Its center is at the point $B$ . Usually the point $B$ is the center of the mass or the center of the symmetry of the robot body or the robot base. The body coordinate system moves together with the robot.
The coordinates $x$ and $y$ are horizontal and vertical positions of the body coordinate system $X_{B}$ – $Y_{B}$ with respect to the inertial coordinate system $X$ – $Y$ .
The angle $\theta$ is the angle that the body coordinate system makes with the $X$ axis of the inertial coordinate system. That is, the angle $\theta$ is the rotation of the body coordinate system with respect to the inertial coordinate system. The angle $\theta$ is called the orientation angle or angular orientation. This angle is also called the bearing angle (or simply bearing) or heading angle (or simply heading). Since the body coordinate system is firmly attached to the robot body the angle $\theta$ is the rotation of the robot body.
$v$ is the instantaneous velocity of the center of the body frame.
$\omega$ is the instantaneous angular velocity of the robot.

Next, we need to define what is a robot pose. The (robot) pose is the vector $\mathbf{z}$ defined by

(1) $\begin{align*}\mathbf{z}=\begin{bmatrix}x \\ y \\ \theta \end{bmatrix}\end{align*}$

The (robot) pose completely defines the position (location) and orientation of the robot with respect to the fixed inertial frame $X$ – $Y$ . From a purely geometrical point of view, the pose defines the location and orientation of the body frame $X_{B}$ – $Y_{B}$ with respect to the inertial frame $X$ – $Y$ . The pose is also called the kinematic state. The reason for this will be explained later on.

The precise knowledge of the robot pose is important for a number of reasons. First of all, it determines the precise location and orientation of the robot in space. Secondly, the robot pose is used for feedback control. Finally, robot pose information is used in other robotic algorithms, such as path planning, obstacle avoidance, and SLAM algorithms.

In our previous tutorial given here, we derived the continuous-time kinematics model describing the robot motion

(2) $\begin{align*}\dot{x}&=v\cos(\theta) \\\dot{y}&=v\sin(\theta) \\\dot{\theta}&=\omega\end{align*}$

This model relates the instantaneous velocity $v$ and the current orientation angle $\theta$ of the robot with the change of the robot pose in the inertial coordinate system. Here, it is very important to understand that this model is developed by using only kinematics reasoning without taking into account the robot dynamics (mass and forces). In the sequel, we develop the robot model by discretizing this kinematics model by using the forward Euler method. Of course, the forward Euler method is only one among a number of discretization approaches. The main reason for using the forward Euler method is its simplicity and ease of implementation. On the other hand, to achieve a relatively good accuracy of the model obtained by using the forward Euler method, we need to select a small discretization step. Also, the forward Euler method might suffer from the stability issues. Some alternative approaches that achieve better modeling accuracy are the trapezoidal discretization method, backward Euler method (which has better accuracy and stability properties than the forward Euler method), trapezoidal implicit method, and the Runge-Kutta method. However, such methods result in mathematically complex models that might significantly increase the computational burden of robotics algorithms.

Derivation of the Robot Model by Using Discretization

Here, we obtain the robot model by using the forward Euler discretization. By using the forward Euler method, we discretize the time derivatives of $x$ , $y$ , and $z$ in the model (2), as follows

(3) $\begin{align*}\dot{x} & \approx \frac{x_{k+1}-x_{k}}{h} \\\dot{y} & \approx \frac{y_{k+1}-y_{k}}{h} \\\dot{\theta} & \approx \frac{\theta_{k+1}-\theta_{k}}{h} \end{align*}$

where $h$ is the discretization step, $k$ is the discrete-time instant, and $x_{k}$ and $y_{k}$ are the horizontal and vertical coordinates of the center of the robot body frame at the discrete-time instant $k$ , that is, $x_{k}=x(kh)$ and $y_{k}=y(kh)$ . Similarly, $\theta_{k}$ is the orientation of the robot body frame at the discrete-time instant $k$ , that is, $\theta_{k}=\theta(kh)$ . Next, by using the forward Euler method and by substituting equations (3) in (2), and by replacing the approximation symbols with equality symbols, we obtain

(4) $\begin{align*} \frac{x_{k+1}-x_{k}}{h} & = v_{k}\cos(\theta_{k}) \\ \frac{y_{k+1}-y_{k}}{h} & =v_{k}\sin(\theta_{k}) \\ \frac{\theta_{k+1}-\theta_{k}}{h} & = \omega_{k}\end{align*}$

where $v_{k}$ is the instantaneous velocity at the discrete-time instant $k$ , and $\omega_{k}$ is the instantaneous angular velocity at the discrete-time instant $k$ . Here, it is implicitly assumed that the instantaneous velocity $v_{k}$ and angular velocity $\omega_{k}$ are constant between the discrete-time instants $k$ and $k+1$ .

From (4), we obtain the discretized kinematics model of the robot

(5) $\begin{align*}x_{k+1}& =x_{k}+v_{k}h\cos(\theta_{k}) \\y_{k+1}& =y_{k}+v_{k}h\sin(\theta_{k}) \\\theta_{k+1}& =\theta_{k}+\omega_{k}h \end{align*}$

Next, we need to observe the following

The product $v_{k}h$ is the distance traveled in the direction of the velocity vector $v_{k}$ (the velocity vector is actually determined by the intensity given by $v_{k}$ and direction determined by the angle $\theta_{k}$ ). It should be kept in mind that the velocity vector is constant between the time instant $k$ and $k+1$ . The distance traveled between the time instant $k$ and $k+1$ is denoted by

(6) $\begin{align*}s_{k}=v_{k}h\end{align*}$
Since $\omega_{k}$ is the angular velocity, the product $\omega_{k}h$ is the rotation angle of the robot body between the discrete-time instant $k$ and $k+1$ . We denote this angle by

(7) $\begin{align*}\Delta \theta_{k}=\omega_{k}h \end{align*}$

Finally, by using (6) and (7), the final discretized model of the robot kinematics takes the following form

(8) $\begin{align*}x_{k+1}& =x_{k}+s_{k}\cos(\theta_{k}) \\y_{k+1}& =y_{k}+s_{k}\sin(\theta_{k}) \\\theta_{k+1}& =\theta_{k}+\Delta \theta_{k}\end{align*}$

Derivation of the Robot Model by Using Trigonometry and Physical Intuition

First, let us analyze the discrete-time model (8) obtained by using the forward Euler method. If we ignore the last equation in (8), the resulting two equations

(9) $\begin{align*}x_{k+1}& =x_{k}+s_{k}\cos(\theta_{k}) \\y_{k+1}& =y_{k}+s_{k}\sin(\theta_{k}) \end{align*}$

describe the translation of the robot body along the direction of the instantaneous velocity vector, where the orientation angle $\theta_{k}$ remains constant. On the other hand, if we ignore the first two equations in (8), and if we only consider the last equation

(10) $\begin{align*}\theta_{k+1}& =\theta_{k}+\Delta \theta_{k}\end{align*}$

then, this equation describes the rotation for the angle of $\Delta \theta_{k}$ . That is, the motion is decomposed (decoupled) into translation and rotation. This is a very well-known fact in kinematics and dynamics of motion of rigid bodies in a plane. Namely, the complex motion of a body in a plane from one pose to another can be decomposed into translation and rotation.

Let us not use this principle to rederive the kinematics model (8) by using a more intuitive approach that does not explicitly rely upon the model discretization.

We formally decompose the motion of the robot from the starting point (at the discrete-time $k$ ) to the endpoint (at the discrete-time instant $k+1$ ) as follows

From the starting point $A$ (location at the discrete-time instant $k$ ) to the endpoint $B$ (location at the discrete-time instant $k+1$ ), the robot moves along a straight line. That is, during the motion from the point $A$ to the point $B$ , the orientation angle $\theta_{k}$ is constant, and the position coordinates $x$ and $y$ are changing.
At the point $B$ , the orientation angle $\theta_{k}$ is changed, such that the orientation becomes $\theta_{k+1}$ .

This is just a formal mathematical decomposition of the motion, since in practice, the actual motion between the two points might not look like this. This is an approximation of the motion that enables us to mathematically analyze the geometry and kinematics of motion. However, if the time interval $h$ between the time instants $k$ and $k+1$ is relatively small this formal decomposition relatively accurately approximates the true motion. Also, it is important to keep in mind that at the time instants $k$ and $k+1$ the locations and orientations correspond to the real locations and orientations of the robot. What happens in between the time instants $k$ and $k+1$ cannot be exactly modeled unless a more complex mathematical model is used.

To repeat, from the mathematical point of view, the motion is decomposed into a series of straight-line motions and orientation changes. Let us illustrate this with the example shown in the figure below.

Figure 2: Illustration of the main assumptions on the robotic motion.

The goal is to move from the point $A$ to the point $C$ by visiting the intermediate point $B$ . The first step, while we are still in the point $A$ , is to adjust the orientation $\theta_{1}$ , such that the robot can move along the straight line from the point $A$ to the point $B$ . This is shown in the figure below.

Figure 3: Step 1- orientation is adjusted.

In step 2, we move from the point A to the point $B$ along the straight line for the distance $s_{1}$ . This is shown in the figure below.

Figure 4: Step 2- move along the straight line from A to B.

In step 3, we adjust the orientation of the robot for the angle $\theta_{2}$ such that we can move along the straight line from the point $B$ to the point $C$ .

Figure 5: Step 3- adjust the orientation in the point $B$ .

In step 4, we move along the straight line from the point $B$ to the point $C$ for the distance $s_{2}$ .

Figure 6: Step 4- move along the straight line from B to C.

Next, we develop the kinematics model of the robot that mathematically describes this motion. Consider the figure shown below.

Figure 7: Diagram for developing the kinematics model. This diagram shows the translation.

Here, we remind the reader that $x_{k}$ and $y_{k}$ denote the $X$ and $Y$ coordinates of the center of the robot body at the discrete time instant $k$ . That is,

(11) $\begin{align*}x_{k}=x(t=kh),\;\; y_{k}=y(t=kh)\end{align*}$

where $h$ is the discretization step. That is, we look at the location of the robot $(x(t),y(t))$ at the discrete-time instants $t=0,h,2h,3h,\ldots, kh,\ldots$ . Similarly, we have previously defined $\theta_{k}$ as follows

(12) $\begin{align*}\theta_{k}=\theta(t=kh)\end{align*}$

From the discrete-time instant $k$ to the discrete-time instant $k+1$ , the robot travels from the start to the end position shown in the figure above. During that time interval, the robot had traveled the distance of $s_{k}$ . This is precisely the distance introduced in (6). Under the assumption that the orientation $\theta_{k}$ remains constant, from the geometry shown in Figure 7 above, we have

(13) $\begin{align*}x_{k+1}=x_{k}+s_{k}\cos(\theta_{k}) \\y_{k+1}=y_{k}+s_{k}\sin(\theta_{k})\end{align*}$

These equations describe the location of the robot at the time instant $k+1$ , under the assumption that the robot was at the location $(x_{k},y_{k})$ at the discrete time instant $k$ and that it had traveled the distance of $s_{k}$ between the discrete-time instants $k$ and $k+1$ . In addition to these equations, we need to add an equation describing the change of the orientation angle $\theta$ . For that purpose, consider the figure below.

Figure 8: Diagram for developing the kinematics model. This diagram shows the rotation.

In the endpoint, we perform the rotation for the angle $\Delta \theta_{k}$ that is first introduced in equation (7). That is, the final orientation at the discrete-time instant $k+1$ is

(14) $\begin{align*}\theta_{k+1}=\theta_{k}+\Delta \theta_{k}\end{align*}$

By combining (13) and (14), we obtain the final kinematics model of the mobile robot

(15) $\begin{align*}x_{k+1}& =x_{k}+s_{k}\cos(\theta_{k}) \\y_{k+1}& =y_{k}+s_{k}\sin(\theta_{k}) \\\theta_{k+1}& =\theta_{k}+\Delta \theta_{k}\end{align*}$

That is precisely the kinematics model (8) obtained by using discretization.

Important Remarks About the Derived Model

Here, for clarity, we repeat the derived model

(16) $\begin{align*}x_{k+1}& =x_{k}+s_{k}\cos(\theta_{k}) \\y_{k+1}& =y_{k}+s_{k}\sin(\theta_{k}) \\\theta_{k+1}& =\theta_{k}+\Delta \theta_{k}\end{align*}$

Usually, the traveled distance $s_{k}$ and the change of the angle $\Delta \theta_{k}$ are directly or indirectly measured by using sensors, such as wheel encoders, accelerometers, gyroscopes, compasses, etc. The process of obtaining these measurements is called odometry. Often in the robotics literature, you will see that $s_{k}$ and $\Delta \theta_{k}$ are used as known inputs. That is, although these quantities are actually measured outputs of the model, they are actually considered as the inputs! These inputs are measured at the discrete time instant $k+1$ . That is, at the end of the movement. This means that the model is written as follows

(17) $\begin{align*}x_{k+1}& =x_{k}+u_{k,1}\cos(\theta_{k}) \\y_{k+1}& =y_{k}+u_{k,1}\sin(\theta_{k}) \\\theta_{k+1}& =\theta_{k}+u_{k,2}\end{align*}$

where $u_{k,1}$ and $u_{k,2}$ are the measurements of $s_{k}$ and $\Delta \theta_{k}$ , respectively, that is

(18) $\begin{align*}u_{k,1}& =s_{k} \\u_{k,2}& =\Delta \theta_{k}\end{align*}$

Continuous-Time Model of Mobile Robot

Derivation of the Robot Model by Using Discretization

Derivation of the Robot Model by Using Trigonometry and Physical Intuition

Important Remarks About the Derived Model

admin

You might also like

Interface Encoder with STM32 Microcontroller and Read Encoder Angle Measurements

Correct Explanation of State Observers for State Estimation of State-Space Models with Python Simulations

How To Approximate Expectations and Integrals of Functions by Using the Monte Carlo Method