UKF-based enhancement and ROS implementation of 4-WDDMR localization with advanced control system

Abstract

This article exhibits integration of nonholonomic four-wheeled differential drive mobile robot platform (4-WDDMR) implemented using an Arduino Mega 2560 microcontroller and robot operating system. Particularly, estimating the current situation of the robot navigation system is complex thanks to uncertainty exerted by the robot incorporated with actuators complexities and nonlinearities. An efficient and accurate estimation technique which applies probabilistic algorithm based Unscented Kalman Filter is proposed. The stability of 4-WDDMR Control-Navigation system has been tested and improved based on Lyapunov criterion. The proposed techniques are implemented and evaluated using MATLAB/SIMULINK®. Both practical and simulation results demonstrate the vitality of the proposed Control-Navigation approach. It is believed that the proposed approaches will help to build an autonomous robotics system that can assist humans with their works in daily life basis.

Keywords:

• 4-WDDMR
• localization
• stability
• UKF
• system uncertainty
• ROS

PUBLIC INTEREST STATEMENT

The popularity of using robots as alternatives to human beings in the real world is becoming more common nowadays, especially in industry, medicine, entertainment, and military. These robots need means to think and to mimic the natural behavior of people. In this article, an artificial intelligence method is proposed to give robots thinking ability to find their way in buildings. The method presented is based on the Bees Algorithm which simulates foraging behavior of real honey bees. The proposed method is implemented as a computer program designed to find the shortest path in a building. This path is planned in a way to avoid collision with stationary barriers and moving objects such as other robots.

1. Introduction

Mobile Robot Localization is a key issue in which high dynamic performance control becomes a vital requirement to ensure correct trajectory tracking with high degree of belief (DoB). One of the most efficient technique used to correct the robot localization is the Extended Kalman the Filter (EKF) (Klančar, Teslić, & Škrjanc, 2013; Panich & Afzulpurkar, 2011). The important drawback of EKF is that, it uses a linearized dynamic model, which leads to bias correction and ignoring the robot uncertainty. However, vision sensor is proposed in (Negenborn, 2003), it is aimed to assist the system to be localized correctly at a predefined location, (Al-Araji, Abbod, & Al-Raweshidy, 2012; Zhao et al., 2012) consider robot proprioceptive sensors with as pure measured signals as possible, which enable to determine correctly the robot posture. Small signal stability analysis is taken into consideration to investigate the robot stability when the system uncertainty is exhibited. Higher uncertainty degree in the dynamic and kinematic model prone on the robot, the EKF-Localization system failed to define the real posture and the robot DoB is not more trusted (Thrun, Fox, & Burgard, 2005). To overcome this limitation, Unscented Kalman Filter (UKF) based on Unscented Transform (UT) has been developed as a method to estimate the robot posture (Houshangi & Azizi, 2005). This article introduces a novel 4-WDDMR Control-Navigation approach using UKF-assisted Adaptive Optimal Controller (Adaptive-Linear Quadratic Regulator, Adaptive-LQR). The system stability has been tested and improved based on Lyapunov criterion. The fusion sensors consisted of a gyroscope and noisy odometry sensors were used. It is assumed also, that the 4-WDDMR model is considered highly uncertain. Intensive results have shown that the estimated robot posture based on UKF are more precise than those obtained from EKF in spite of huge uncertainty imposed to the 4-WDDMR. Finally, a practical system implementation based on ROS, the most trending and popular robotic framework in the world (O’Kane, 2014) and Arduino board has been developed. The practical results were coincided with those obtained from MATLAB models and C augmented programs for Adaptive-LQR control technique as well as for UKF. The remaining sections of the paper are as follows: Section II provides details about hardware overview of the mobile robot platform and explains the model of the mobile robot combining the actuators dynamics; Section III discusses mobile robot localization based on UKF as a probabilistic inference solution; Section IV provides the stability of the proposed controller for the kinematic and dynamic systems; Section V exhibits intensive simulation results of UKF performance with high uncertainty of system’s parameters; Section VI explains the system implementation and practical results based ROS system and Arduino board, and section VII provides a conclusion for the presented work.

2. 4-WDDMR modeling

The diagram of the 4-WDDMR, shown in Figure 1, consists of a wheeled robot with four driving wheels mounted with 4-DC geared motors and two channels Hall Effect incremental encoders. The 4-WDDMR platform is implemented with an Arduino Mega 2560 board for low-level robot control.

Figure 1. Diagram of the 4-WDDMR.

The orientation of the robot is determined using the angular velocities of left and right wheels “that is” differential drive system. The wheels in the same side are operated together using (L298N H-Bridge Module). To measure the robot’s orientation angle $\theta$ around the Z-axis, a three-axis gyroscope sensor communicated with Arduino via $I^{2}C$ Bus is used. The Onboard robot system is connected with PC-based Offboard robot system via RF 433 MHz. The parameters of 4-WDDMR and actuators are listed in Table 1.

Table 1. 4-WDDMR model parameters

Symbol	Quantity
$M_c,M_{\omega }[\mathrm{k}\mathrm{g}]$	4-WDDMR and Wheel Mass
$L[\mathrm{c}\mathrm{m}]$	Distance Between the Two Back Wheels
$d[\mathrm{c}\mathrm{m}]$	Distance Between A and C
$r_{\omega }[\mathrm{c}\mathrm{m}]$	Wheels Radius
$I_c[\mathrm{k}\mathrm{g}.{\mathrm{m}}^2]$	4-WDDMR’s Moment of Inertia
$I_{\omega }[[\mathrm{k}\mathrm{g}.{\mathrm{m}}^2]$	Wheel’s Moment of Inertia about the Wheel Axis
$I_m[[\mathrm{k}\mathrm{g}.{\mathrm{m}}^2]$	Wheel’s Moment of Inertia about the Diameter
$R_a[\mathrm{\Omega }]$	Armature Resistance
$F_m[\mathrm{N}.\mathrm{m}]$	Friction Constant
$J_m[\mathrm{N}.\mathrm{m}]$	The Inertia Motor
$C{m}_{\phi }[\nu s.ra{d}^{-1}]$	Back e.m.f Constant
$N$	Gear Ratio
$\nu [m.s^{-1}]$	4-WDDMR Linear Velocity
$\omega [rad.s^{-1}]$	4-WDDMR Angular Velocity
${\omega }_R,{\omega }_L[rad.s^{-1}]$	Wheels Angular Velocities
$u_R,u_L[\nu ]$	Motor’s Nominal Voltages
The total moment of inertia: $I=M_c{d}^2+I_c+4M_{\omega }{L}^2+4I_m$ The total mass: $M=M_c+4M_{\omega }$

As seen from Figure 2, the robot’s location is determined in its environment based on two coordinated frames; namely, Global Frame and Local Frame (Dhaouadi & Abu Hatab, 2013). C is the center of mass of the 4-WDDMR’s and A is the middle point between the two back wheels. Let $P^L={\left[\begin{array}{ccc}{X}_A^L& Y_A^L& \theta \end{array}\right]}^T$ is the robot posture represented in {L} frame and $P^G={\left[\begin{array}{ccc}{X}_A^G& Y_A^G& \theta \end{array}\right]}^T$ is the robot posture represented in {G} frame. It is considered that the wheels do not slide “that is” nonholonomic constraint (Siegwart & Noubakhsh, 2004).

Figure 2. 4-WDDMR coordinates systems.

$x_A\mathrm{\&}{y}_A$ are the coordinates of point A, and $\theta$ is the robot’s orientation angle sensed between the moving direction of $X_G$ (Dhaouadi & Abu Hatab, 2013). The Kinematic of 4-WDDMR can be described as (1):

(1) $\left[\begin{array}{c}{\dot{x}}_C^G\\ {\dot{y}}_C^G\\ {\dot{\theta }}_C^G\end{array}\right]=\left[\begin{array}{cc}cos(\theta )& -dsin(\theta )\\ sin(\theta )& dcos(\theta )\\ 0& 1\end{array}\right]\left[\begin{array}{c}\nu \\ \omega \end{array}\right]=\mathbf{G}\left[\begin{array}{c}\nu \\ \omega \end{array}\right]$(1)

The dynamic model of a nonholonomic 4-WDDMR is given in (2) as follows (Dhaouadi & Abu Hatab, 2013):

(2) $\mathbf{M}(q)\ddot{\mathbf{q}}+\mathbf{V}(q,\dot{q})\dot{\mathbf{q}}+\mathbf{G}(q)+\mathbf{F}(\dot{q})+{\tau }_{\mathbf{d}}=\mathbf{B}(q)\tau -{\mathbf{\Lambda }}^T(q)\lambda$(2)

Where $\mathbf{M}(q)\in {\mathrm{\Re }}^{n\times n}$ is an inertia matrix, must be definite, $\mathbf{V}(q,\dot{q})\in {\mathrm{\Re }}^n$ is the Centripetal and Coriolis matrix, $\mathbf{G}(q)\in {\mathrm{\Re }}^n$ is the Gravitational vector, $\mathbf{F}(\dot{q})\in {\mathrm{\Re }}^n$ is the Friction Surface matrix, ${\tau }_{\mathbf{d}}$ is the bounded unknown disturbance vector, $\mathbf{B}(q)\in {\mathrm{\Re }}^{n\times r}$ is the Input Transformation matrix, $\tau ={\left[\begin{array}{cc}{\tau }_R& {\tau }_L\end{array}\right]}^T\in {\mathrm{\Re }}^r$ is the Input Actuator’s Torques vector which act on the right and left wheels, ${\mathbf{\Lambda }}^T(q)\in {\mathrm{\Re }}^{m\times n}$ is the Kinematic Constraints Associated matrix, $\lambda \in {\mathrm{\Re }}^m$ is the Constraint Forces vector, $n$ is the robot coordinates number, $r$ is the inputs number and $m$is the constraints number. $\mathbf{G}(q)$ is equal to zero since the robot base constrained the horizontal plane. For a real 4-WDDMR, the torque vector $\tau$ is produced by electromechanical actuators. A common and simplified dynamic model of a permanent magnet DC motor with gear train is given as follows (3), (Kelly, Santibanez, & Loria, 2006):

(3) $\tau =N^2\left(\left(\frac{C{m}_{\phi }}{N{R}_a}\right)u-J_m\ddot{q}-\left(f_m+\left(\frac{C{m}_{\phi }^2}{R_a}\right)\right)\dot{q}\right)$(3)

Where $q[\mathrm{r}\mathrm{a}\mathrm{d}]$ is the angular position of the wheel.

Thus, the integrated dynamic model of a 4-WDDMR considering friction in the wheels and having its actuators is obtained by substituting $\tau$ from (3) in (2):

(4) $(N\mathbf{M}(q)+\mathbf{J})\ddot{\mathbf{q}}+N(\mathbf{V}(q,\dot{q})+\mathbf{B})\dot{\mathbf{q}}+N\mathbf{G}(q)+N\mathbf{F}(\dot{q})+{\tau }_{\mathbf{d}}=\mathbf{K}(q)\mathbf{u}-{\mathbf{\Lambda }}^T(q)\lambda$(4)

However, the information of the robot location obtained from the Odometry-based Onboard sensors is subject to different types of accumulated errors and uncertainties over time due to the nature of process. There are two main errors categories: Deterministic and Non-Deterministic (Siegwart & Noubakhsh, 2004). The major position error sources that cannot be detected by Odometry are misalignment of the wheels, unequal wheel diameter, limited resolution during integration and other random errors affected by the environment, this errors represented by ${\sigma }_p$. Thus, the real increment posture of 4-WDDMR $P_k={\left[\begin{array}{ccc}{x}_k& y_k& {\theta }_k\end{array}\right]}^T$ becomes (5):

(5) ${\mathbf{P}}_k={\mathbf{P}}_{k-1}+\mathrm{\Delta }\mathbf{P}+{\sigma }_P$(5)

For this propose, UKF was implemented based on the kinematic model of the robot to determine the real posture precisely (Houshangi & Azizi, 2005).

3. Perception and localization-based UKF

In the present section, a novel approach for localization-based UKF-fusion sensors have been investigated successfully. The problem of navigation can be summarized into answering four questions (Liu, 2010): “Where am I?” which is the Localization problem, “What does the word look like?,” “Where am I going?,” and “How should I get there?”

Accurate Localization is regarded as fulfilling a major precondition of trusty autonomous robots (Thrun et al., 2005). Whenever the robot acts in real world, it needs to be able to deal with errors and uncertainty. In literature, researchers have developed a variety of localization techniques depending on the robot’s perception system “that is” sensors (Thrun et al., 2005).

Probabilistic Inference or Bayesian rule are a common way to deal with this problem by representing the robot’s posture $x_k$ at sample time $k$ with probability distributions $p(x_k)$ (Negenborn, 2003).

Being able to estimate the posterior probability distributions of the 4-WDDMR’s posture is useful for many robotics problem (Negenborn, 2003). For this purpose, Kalman Filter Framework (KF)-based Gaussian approximate solution was implemented as a practically implementable Optimal Recursive State Estimator for a known 4-WDDMR kinematic model (Houshangi & Azizi, 2005). It is proposed that the robot is equipped with encoders and exteroceptive sensors as shown in Figure 3.

Figure 3. Block diagram of fusion sensors.

Nonlinearity errors can lead to poor EKF performance or to divergence (Kandepu, Foss, & Imsland, 2008; Sarkka, 2011). Jacobians does not allow correct state and measurements Covariance matrices propagation in presence of significant dynamic conditions (Rhudy, Gu, & Napolitano, 2013). To overcome such limitations, UKF Algorithm has been developed and verified with the present of uncertainties.

UKF is derived based on UT to capture the posterior mean $\bar{\mathbf{X}}$ and covariance $\mathbf{P}$ to the third order of nonlinear system (Sarkka, 2011). First, a set of Sigma points are voted to express the probability distribution. The nonlinear function is then applied to each Sigma point. Hence, the results provide a set of transformed points with the mean and covariance (Merwe, 2004).

UKF is computationally more costly than EKF, but it reduces estimation errors. One of the advantages of UKF over EKF is that it does not need to drive the Jacobian matrices (Rhudy et al., 2013). In order to apply UKF as a localization algorithm (Sarkka, 2011), a discrete time model for the 4-WDDMR navigation with sampling time $T_s$ is given by (6):

(6) $\left[\begin{array}{c}{x}_k\\ y_k\\ {\theta }_k\end{array}\right]=\frac{T_s{r}_{\omega }}{2}\left[\begin{array}{cc}cos({\theta }_k)& cos({\theta }_k)\\ sin({\theta }_k)& sin({\theta }_k)\\ \frac{1}{L}& -\frac{1}{L}\end{array}\right]\left[\begin{array}{c}{\omega }_{Rk}\\ {\omega }_{Lk}\end{array}\right]+\left[\begin{array}{c}{x}_{k-1}\\ y_{k-1}\\ {\theta }_{k-1}\end{array}\right]$(6)

Where the robot posture $\mathbf{x}={\left[\begin{array}{ccc}{x}_k& y_k& {\theta }_k\end{array}\right]}^T$ is the state vector and the input vector is the angular velocities of the right and left wheels$\mathbf{u}={\left[\begin{array}{cc}{\omega }_{Rk}& {\omega }_{Lk}\end{array}\right]}^T$. The UKF algorithm includes three main stages as shown in Figure 4 (Merwe, 2004).

• Initialize the State Space vector and Covariance matrix (7):

(7) $\begin{array}{rl}& {\chi }_{\mathbf{0}}=[\begin{array}{ccc}{\mathbf{x}}_{\mathbf{0}}& \mathbf{0}& \mathbf{0}\end{array}{]}^T\\ & {\mathbf{P}}_{\mathbf{k}-\mathbf{1}}=\left[\begin{array}{ccc}{\mathbf{P}}_{\mathbf{0}}& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& \mathbf{Q}& \mathbf{0}\\ \mathbf{0}& \mathbf{0}& \mathbf{R}\end{array}\right]\end{array}$(7)

• Action (or Prediction) update:

Figure 4. UKF recursive algorithm.

The following operations are performed on each time ($k$):

1. Form ($2n+1$) weighed Sigma points as follows (8):

(8) ${\chi }_{\mathbf{k}-\mathbf{1}}=[\begin{array}{ccc}{\mathbf{x}}_{\mathbf{k}-\mathbf{1}}& {\mathbf{x}}_{\mathbf{k}-\mathbf{1}}+\gamma \sqrt{{\mathbf{P}}_{\mathbf{k}-\mathbf{1}}}& {\mathbf{x}}_{\mathbf{k}-\mathbf{1}}-\gamma \sqrt{{\mathbf{P}}_{\mathbf{k}-\mathbf{1}}}\end{array}{]}^T$(8)

Where $\gamma$ is a scalar parameter.

• (2) Transfer the Sigma points over the 4-WDDMR Kinematic Model (9):

(9) ${\chi }_{\mathbf{k}}={\mathbf{A}}_{\mathbf{d}}(x_{k-1}){\chi }_{\mathbf{k}-\mathbf{1}}+{\mathbf{B}}_{\mathbf{d}}(x_{k-1}){\mathbf{u}}_{\mathbf{k}}$(9)

• (3) Calculate the predicted Mean ${\stackrel{\mathbf{ˆ}}{\chi }}_{\mathbf{k}}^{-}$ and Covariance ${\mathbf{P}}_{\chi \chi }^{-}$ (10):

(10) $\begin{array}{c}{\hat{\chi }}_{\mathbf{k}}^{-}=\sum _{i=0}^{2n}{\omega }_{\mathbf{m}}^{[\mathbf{i}]}{\chi }_{\mathbf{k}}^{[\mathbf{i}]}\\ P_{\chi \chi }^{-}=\sum _{i=0}^{2n}{\omega }_{\mathbf{c}}^{[\mathbf{i}]}\left({\chi }_{\mathbf{k}}^{[\mathbf{i}]}-{\hat{\chi }}_{\mathbf{k}}^{-}\right){\left({\chi }_{\mathbf{k}}^{[\mathbf{i}]}-{\hat{\chi }}_{\mathbf{k}}^{-}\right)}^T\end{array}$(10)

Where the weighs ${\omega }_{\mathbf{m}}^{[\mathbf{i}]},{\omega }_{\mathbf{c}}^{[\mathbf{i}]}$ defined by the algorithm.

• (4) Propagate Sigma points through the Measurements Model and calculate the predicted Mean ${\hat{\mathbf{z}}}_{\mathbf{k}}$, the predicted Covariance of the measurement ${\mathbf{P}}_{\mathbf{z}\mathbf{z}}$, and the Cross-Covariance of the state and measurement ${\mathbf{P}}_{\chi \mathbf{z}}$(11):

(11) $\begin{array}{c}{\hat{\mathbf{z}}}_{\mathbf{k}}^{-}=\sum _{i=0}^{2n}{\omega }_{\mathbf{m}}^{[\mathbf{i}]}{\mathbf{z}}_{\mathbf{k}}^{[\mathbf{i}]}\\ {\mathbf{P}}_{\mathbf{z}\mathbf{z}}=\sum _{i=0}^{2n}{\omega }_{\mathbf{c}}^{[\mathbf{i}]}\left({\mathbf{z}}_{\mathbf{k}}^{[\mathbf{i}]}-{\hat{\mathbf{z}}}_{\mathbf{k}}\right){\left({\mathbf{z}}_{\mathbf{k}}^{[\mathbf{i}]}-{\hat{\mathbf{z}}}_{\mathbf{k}}\right)}^T\\ {\mathbf{P}}_{\chi \mathbf{z}}=\sum _{i=0}^{2n}{\omega }_{\mathbf{c}}^{[\mathbf{i}]}\left({\chi }_{\mathbf{k}}^{[\mathbf{i}]}-{\hat{\chi }}_{\mathbf{k}}^{-}\right){\left({\mathbf{z}}_{\mathbf{k}}^{[\mathbf{i}]}-{\hat{\mathbf{z}}}_{\mathbf{k}}\right)}^T\end{array}$(11)

• Perception update:

The 4-WDDMR corrects its posture by merging its belief with the probability of making exactly that observation (Kandepu et al., 2008). The UKF gain ${\mathrm{K}}_{\mathbf{k}}$ is chosen to minimize the estimation errors and updated the Mean ${\hat{\chi }}_{\mathbf{k}}$ and Covariance ${\mathbf{P}}_{\chi \chi }$ as follows (12):

(12) $\begin{array}{c}{\mathrm{K}}_{\mathbf{k}}={\mathbf{P}}_{\chi \mathbf{z}}{\mathbf{P}}_{\mathbf{z}\mathbf{z}}^{-\mathbf{1}}\\ {\hat{\chi }}_{\mathbf{k}}={\hat{\chi }}_{\mathbf{k}}^{-}+{\mathrm{K}}_{\mathbf{k}}({\mathbf{z}}_{\mathbf{k}}-{\hat{\mathbf{z}}}_{\mathbf{k}})\\ {\mathbf{P}}_{\chi \chi }={\mathbf{P}}_{\chi \chi }^{-}-{\mathrm{K}}_{\mathbf{k}}{\mathbf{P}}_{\mathbf{z}\mathbf{z}}{\mathbf{K}}_{\mathbf{k}}^{\mathbf{T}}\end{array}$(12)

4. System stability analysis

In the presence of uncertainties and disturbances, which increase the complexity of the control study, it is not always possible to obtain a control law ensuring the stability of the system with respect to precalculated trajectories (Kühne, Lages, & DaSilva, 2005).

Also due to its high nonlinearity and nonholonomic constrains, the control of a 4-WDDMR is very challenging. The state of the art of mobile robot control strategies employed can be found (Wang, Qu, Obeng, & Wu, 2007; Zhao et al., 2012). A complex analytically derived control law, which has significantly high computational accuracy and lead to minimum tracking error of mobile robot for different types of trajectories is proposed in (Al-Araji et al., 2012).

The authors’ previous work (Kass Hanna & Joukhadar, 2015), proposed a simple adaptive quadratic optimal predictive control approach for 4-WDDMR Kinematic Model. A LQR is implemented for the low-level control, which tracks the velocities commends given by the high-level controller. It has been shown that the dynamic control performance is high and robust to system uncertainty (Kass Hanna & Joukhadar, 2015). This section, the stability of the proposed control system studied in (Kass Hanna & Joukhadar, 2015) has been validated mathematically based on Lyapunov Criterion. As seen in Figure 2, the objective is to track a pre-calculated trajectory reference signal ${\left[\begin{array}{ccc}{x}_d& y_d& {\theta }_d\end{array}\right]}^T$ with the desired linear and angular velocities ${\left[{\nu }_d{\omega }_d\right]}^T$ by the robot platform. Therefore, the dynamic equations of tracking error based on the Kinematic Model (1) are as follows (13):

(13) $\begin{array}{rl}& {\dot{e}}_x=-y_d{e}_{\theta }+{\nu }_{d}cos(\theta )-\nu cos({\theta }_d)\\ & {\dot{e}}_y=x_d{e}_{\theta }-{\nu }_{d}sin(\theta )-\nu sin({\theta }_d)\\ & {\dot{e}}_{\theta }=e_{\omega }={\omega }_d-\omega \end{array}$(13)

Where ${\left[\begin{array}{ccc}{e}_x& e_y& e_{\theta }\end{array}\right]}^T$ is the posture error vector of 4-WDDMR with respect to the global frame. The proposed velocities control low is given by (14):

(14) $\begin{array}{c}\nu =K_x{e}_{x}cos({\theta }_d)+K_y{e}_{y}sin({\theta }_d)+{\nu }_d\\ \omega =K_{\theta }{e}_{\theta }+{\omega }_d\end{array}$(14)

$K_x>0,K_y>0,K_{\theta }>0$ are the optimal control gains. It should be noted that the control law was adapted along a desired orientation to avoid losing information about the 4-WDDMR motion (Kass Hanna & Joukhadar, 2015). Considering the following positive Lyapunov function (15):

(15) $E=\frac{1}{2}\left(e_x^2+e_y^2+e_{\theta }^2\right)$(15)

The time derivative of $E$ is given by (16):

(16) $\dot{E}=e_x{\dot{e}}_x+e_y{\dot{e}}_y+e_{\theta }{\dot{e}}_{\theta }$(16)

Using the proposed control law (14), the time derivative of the Lyapunov function become (17):

(17) $\dot{E}=-K_x{e}_x^2{cos}^2({\theta }_d)-K_y{e}_y^2{sin}^2({\theta }_d)-K_{\theta }{e}_{\theta }^2$(17)

The time derivative $\dot{E}$is negative, then, using Lyapunov Criterion, the 4-WDDMR control system is stable (Gutierrez-Arias, Flores-Mena, Morin-Castillo, & Suarez-Ramier, 2011). The aforementioned velocities control law (14) is based on the Kinematic Model. It is necessary to design torque controllers for the 4-WDDMR based on the Dynamic Model. The Dynamic Model (4) can be formulated as follow (18) where $x={\left[\begin{array}{cc}\nu & \omega \end{array}\right]}^T$

(18) $\bar{\mathbf{M}}\dot{x}+\overline{\mathbf{C}\mathbf{x}}=\bar{\mathbf{u}}$(18)

$\bar{\mathbf{M}}$ is the Inertia Mass Matrix and $\bar{\mathbf{C}}$is the Coriolis Matrix satisfies the following skew-symmetric property (Wang, 2012):

(19) ${\mathbf{x}}^{\mathbf{T}}(\bar{\mathbf{M}}-2\bar{\mathbf{C}})\mathbf{x}=\mathbf{0}\hspace{0.17em}\mathrm{\forall }\mathbf{x}\in {\mathrm{\Re }}^n$(19)

The Control vector ${\bar{\mathbf{u}}}_{\mathrm{o}\mathrm{p}\mathrm{t}}$ has to be determined to minimize a selected cost function as follow (20):

(20) ${\bar{\mathbf{u}}}_{\mathbf{o}\mathbf{p}\mathbf{t}}=-{\mathbf{K}}_{\mathbf{o}\mathbf{p}\mathbf{t}}\bar{\mathbf{x}}$(20)

To enhance the dynamic system behavior an integral LQR-based controller is proposed (Kass Hanna & Joukhadar, 2015).

The designed system has a configuration with inherent stability characteristics based Lyapunov approach (Gutierrez-Arias et al., 2011).

If the state vector $\widehat{\stackrel{ˉ}{\mathbf{x}}}$ was estimated then it needed to close the feedback loop of the nonlinear control system. UKF is proposed to compensate the uncertainty in the plant dynamic under control as shown in equation (21).

(21) $\frac{\widehat{.}}{x}=(A-K_{UKF}C-B{K}_{opt})\widehat{\overline{x}}$(21)

Equ. (21) explains the existing of both the adaptive optimal controller gain vector and the UKF correction gain vector. Both have significant effect of the dynamic control performance in case they were not suitably tuned.

This results in precise state estimation of the nonlinear system and enables for high dynamic performance control

5. Simulation results

To demonstrate the effectiveness of the proposed Control-Navigation system, simulation has been performed for a 4-WDDMR using MATLAB/SIMULINK® and C code. The simulation validated by tracking 3DoFs desired flower shape path with $2m$ of diameter. The angular velocities at joint space level and using LQR controller and Cartesian space posture adaptive optimal controller were augmented with the proposed UKF-based localization system have been implemented. The system was tested with high degree of parametric uncertainty.

Figure 5 shows the proposed system’s dynamic performance supported with EKF-Localization system. Figure 5-a was obtained assuming the presence of nondeterministic errors affected by the environment on 4-WDDMR as nonsmooth floor (bumps and holes), as seen the 4-WDDMR tracks the desired path with low DoB. Figure 5-b was conducted with the presence of deterministic errors in 4-WDDMR’s Dynamic and Kinematic parameter, as seen the divergence is very high and the DoB is null (the desired trajectory “blue,” the actual trajectory “red” and the ellipsoids is represent the uncertainty). The same system was reevaluated assuming the similar uncertainties in the 4-WDDMR model but using UKF-Localization system.

Figure 5. EKF-based path correction.

The results in Figure 6 showed that the UKF decreased the drift in 4-WDDMR path and the Control-Navigation system enhanced the 4-WDDMR’s DoB along the trajectory.

Figure 6. UKF-based path correction.

6. Software architecture-based ROS

A software framework for a 4-WDDMR based on ROS is described in this section. ROS is a meta-open source operating system, which is designed for robots and runs on Linux (Quigley et al., 2008). ROS distribution used in this article is ROS Indigo which was released in 22 July 2014.

ROS provides libraries and tools such as hardware abstraction, message passing, device drivers, perception, visualizer, and more to help software developers create robot applications. ROS is built up by stacks, which in turn are built up by packages, every package contains at least one node. ROS-message is a data structure that can be sent between nodes. Both the Adaptive Optimal Control and UKF algorithm are implemented in C++ using ROS.

A diagram of the 4-WDDMR-ROS software architecture is shown in Figure 7. The perception and High Level Control tasks shown (left), the main real time optimal control loop, which communicate via Arduino board with sensors and actuators shown (right). Arduino board communicates with PC-master via ROS serial topic. Figure 8 shows the time response of the wheels angular velocities. It is observed that the low level dynamic system is exponentially converged and shows high dynamic response. The spikes shown at sample instants 220 and 240 s are due to applying sudden breaking signals to examine the robustness of the developed controller.

Figure 7. Schematic of ROS software architecture for 4-WDDMR.

Figure 8. Time response for right and left wheels.

Figure 9 shows high level control dynamic performance and UKF navigation system with an arbitrary predetermined trajectory generated via ROS topic, as seen the robot true orientation angle is coincided with UKF and the gyroscope sensor one, where the odometry one is out of synchronization and not accurate.

Figure 9. 4-WDDMR orientation tracking.

7. Conclusion

This article has presented an ultimate integrated robotics system-based ROS as an advanced robot software environment. The proposed Adaptive Optimal Controller has been shown to be capable to overcome the robot dynamic parameters uncertainties and robustness against parameters mismatch.

The implemented UKF-based system state estimation has proven to be robust against the uncertainties exhibited by robot kinematic parameters. It has been noted that the proposed approach has provided high DoB for the robot localization. The conducted practical results obtained from the proposed approach, which were implemented on ROS were coincided with the simulation results obtained from the developed model built in MATLAB/SIMULINK® and C language. This work will be further developed with UKF and D-SLAM techniques to obtain an autonomous robotic system (Kerl, Sturm, & Cremers, 2013).

Additional information

Funding

The authors received no direct funding for this research.

Notes on contributors

Abdulkader Joukhadar

Abdulkader Joukhadar was born in 1968. He received his Ph.D. degree from Aberdeen University, UK in 2004. Currently, he is an associate professor of Intelligent Control System at the Dept. Mechatronics Engineering, Faculty of Electrical and Electronic Engineering, University of Aleppo. The central theme of his research is in the field of Robotics, Robot Control, Probabilistic Robotics and Intelligent Control Systems, Kalman Filtering, Adaptive Control.

Dalia Kass Hanna

Dalia Kass Hanna was born in 1987. She studied Control Engineering and Automation at the University of Aleppo, Syria. She received her MSc degree in 2016. Since 2011, she is a teacher at the Dept. Mechatronics Engineering, Laboratory of Integrated Mechatronics Systems. Her research interests lie in the fields of Intelligent Mechatronics Systems, Embedded Systems, Robot Localization, SLAM and Intelligent Motion approaches.