Receding-Horizon Vision Guidance with Smooth Trajectory Blending in the Field of View of Mobile Robots

Abstract

Applying computer vision to mobile robot navigation has been studied for over two decades. One of the most challenging problems for a vision-based mobile robot involves accurately and stably tracking a guide path in the robot limited field of view under high-speed manoeuvres. Pure pursuit controllers are a prevalent class of path tracking algorithms for mobile robots, while their performance is rather limited to relatively low speeds. In order to cope with the demands of high-speed manoeuvres, a multi-loop receding-horizon control framework, including path tracking, robot control, and drive control, is proposed in this paper. This is done within the vision guidance of differential-driving wheeled mobile robots (DWMRs). Lamé curves are used to synthesize a trajectory with $G^2$-continuity in the field of view of the mobile robot for path tracking, from its current posture towards the guide path. The platform twist—point velocity and angular velocity—is calculated according to the curvature of the Lamé-curve trajectory, then transformed into actuated joint rates by means of the inverse-kinematics model; finally, the motor torques needed by the driving wheels are obtained based on the inverse-dynamics model. The whole multi-loop control process, initiated from Lamé-curve blending to computational torque control, is conducted iteratively by means of receding-horizon guidance to robustly drive the mobile robot manoeuvring close to the guide path. The results of numerical simulation show the effectiveness of our approach.

1. Introduction

Mobility is a distinct characteristic separating mobile robots from their base-fixed counterparts [1]. Navigation is an essential issue to realize the motion of mobile robots, which brings about three basic questions: (a) where am I? (b) where am I heading for? and (c) how do I get there? The third question pertains to path-guidance control [2]. If the position reference is not available temporarily, the navigation system typically enters a mode known as dead reckoning to maintain an estimate of the position [3]. Otherwies guidance involves obtaining a trajectory for a mobile robot from its position and orientation in order to follow a given path [4].

Given paths, or guide paths, the target to be continuously followed by mobile robots can be generated in various possible ways: path planning on a map (environment model) [2,5,6]; a parametric reference in a fixed coordinate frame [7,8,9,10,11]; and a physical landmark (wire, tape, stripe) on the ground [12,13,14,15]. Recently, computer vision has been used to measure the posture of mobile robots when tracking guide pathways. One approach resorts to a ceiling-mounted camera overlooking a mobile robot with specific colored labels on its platform [16,17,18]; another approach relies on an onboard camera looking down at a physical pathway (colored stripe) on the ground from the perspective of the robot [12,14,15].

For the first approach, the stationary cameras fixed in the environment have a large field of view, but at the cost of measurement precision. For example, a high-resolution camera overhead provides a positioning measurement value with a resolution of 14.9 mm for a robot with dimensions of 365 × 419 mm [17]. In this sense, cameras mounted on robots in the second approach are preferred for applications requiring a high accuracy.

Line tracking via an onboard camera is perhaps the prospective vision navigation approach mostly used in industrial environments, since it offers real-time, precise and reliable performance. The computational load relevant to this kind of vision perception is mitigated by coding or learning the specific perceptual knowledge that is expected to extract from the environment [15,19,20,21]. Moreover, gluing colored stripes on the floor is inexpensive, compared not only with laying down a wire under the ground, but also with setting up a plurality of retro reflectors for a laser guidance system [22].

However, the limited field of view of onboard cameras brings great difficulties to the path tracking of differential-driving wheeled mobile robots (DWMRs) in high-speed manoeuvres, especially when only kinematic models [12,14,17] or pure pursuit controllers [23,24,25] are used. In order to cope with the demands of high-speed manoeuvres, dynamic models are used to describe the inherent characteristics of correlating actuation with motion in a DWMR. These models are normally force-driven, second-order differential equations relating wheel forces to robot acceleration [13,18,26]. When motor dynamics is included, robot models evolve to composite models. In these models, the platform twist (A vector array that includes point velocity and rigid-body angular velocity.) can be associated with the time-derivative of motor-armature voltage [8], or with the duty ratio of pulse-width modulation (PWM) of motor voltage [27], or even with the motor current [28].

Controller design is influenced by system models of mobile robots. Pure pursuit controllers are a prevalent class of path tracking algorithms for mobile robots, while their performance is rather limited to relatively low speeds. Hence, the receding-horizon strategy is used to develop a model-based predictive active yaw control implementation for pure-pursuit tracking at high speeds [25]. The strategy is a model-based feedback control solution in terms of the limited sampling and control period, in which an optimal control trajectory is generated for the initial state, and updated at each sampled instant. This strategy can be combined with a sequential convex programming method to develop a closed-loop guidance control scheme that robustly drives a spacecraft maneuvering close to the target [29]. This strategy can also be used to control mobile robots to locate sensor nodes in unknown wireless sensor networks [30]. The strategy has proven to be effective to enhance the scheme quality and reduce the computational burden by dividing the optimization horizon into smaller time windows [31].

The underlying motivation of this paper lies in the need for combining the receding-horizon strategy with Lamé-curve blending for vision guidance in the limited field of view of mobile robots. Our main contribution is twofold. On the one hand, a multi-loop receding-horizon control framework is outlined for vision guidance of mobile robots. Herein, cascaded control loops of path tracking, robot control and drive control are developed based on kinematics and dynamics models. On the other hand, Lamé-curve blending is proposed for kinematic path tracking to generate a trajectory with $G^2$-continuity to blend its current posture with a certain point of the guide path.

The balance of the paper is organized as follows: the problem description and the receding-horizon control framework for vision guidance of mobile robots are addressed in Section 2; the kinematics and dynamics models are formulated for DWMRs in Section 3; the kinematic path tracking technique based on Lamé-curve blending is proposed in Section 4; simulation results are reported in Section 5, while conclusions and recommendations for future research work are given in Section 6.

2. Problem Formulation

2.1. Vision Guidance

The problem under study consists in guiding a DWMR through a given path by means of a vision guidance system. An onboard camera is assumed to be mounted on the front of the DWMR, looking down obliquely at colored stripes on the ground, as shown in Figure 1.

Colored stripes in the robot field of view are recognized as guide paths. A position error $e_d$ and an orientation error $e_{\theta }$ of the DWMR are defined with respect to a guide path, as shown in Figure 2. Moreover, $e_d$ denotes the x-coordinate of the intersection of the guide path with the $X_k$-axis, $e_{\theta }$ referring to the angular error between the tangent of the guide path and the $Y_k$-axis. Fields of view 1, 2 and 3 in Figure 2 represent a sequence of vision samplings at three consecutive instants, as the robot moves. Since the field of view is relatively small, namely, 0.4 × 0.3 m, a guide path can be approximated as a straight line by means of least squares. The linearization error increases with a decrease in the radius of curvature, as shown in

Table 1. Linearization error.

Curvature [m]	2	1.8	1.6	1.4	1.2	1.0
Error [${10}^{-3}$ m]	5.06	5.63	6.34	7.25	8.48	10.20
Relative error [%]	0.253	0.313	0.396	0.518	0.707	1.02

.

When high-curvature arcs are used for guide paths, the tangents of their different sections undergo a significant change in the field of view. In these cases, an approximation method based on binary-tree guidance window partition can be used to replace the curve with a series of piecewise lines at any given approximation accuracy [32]. Hence, only straight paths are considered as the target of tracking control in the field of view, as shown in Figure 3.

In Figure 3, point C, the centre of mass of the platform, indicates the position of the DWMR, while the angle $e_{\theta }$ defines its orientation error with respect to the path. Path tracking can be converted into a trajectory-planning problem from the current pose (point C along the Y-axis) to a target pose (point $T_k$ along the path). It is noteworthy that path is distinguished from trajectory here. The former is the colored stripe marked on the floor, which serves to guide the robot; the latter is the curve traced by a reference point of the platform. We choose this point at the platform centre of mass.

2.2. Methodology Overview

Electric motors are the actuators of choice in most mobile robots. A typical motion control system includes a feedback subsystem with three cascaded loops: the outer position loop; the intermediate velocity loop; and the inner current loop. Nowadays, many commercial off-the-shelf motor drives provide the complete control capability for all three loops. Dynamic path-tracking can be implemented by interfacing the torque output of the inverse-dynamics model with the current-loop control of the motor drives, in a hierarchical control framework based on kinematics and dynamics models, as shown in Figure 4.

This control framework combines vision detection with cascaded control loops of path tracking, robot control and drive control, meanwhile facilitating the integration of kinematics and dynamics models with commercial off-the-shelf motor drives. First, colored stripes on the ground are recognized as guide paths, and then robot posture errors are obtained by vision detection, the errors being the vision input. Second, path tracking is used to eliminate these errors by controlling the robot twist, as the first-layer output. Third, the inverse-kinematics model transforms the twist into the actuated joint-rate vector, as the second-layer output. Fourth, the desired torque is calculated from the inverse-dynamics model for the motor drives, as the third-layer output. Finally, torque control is automatically implemented in the current loop by motor drives, which drive the mobile robot manoeuvring close to the guide path.

In this control framework, the path recognition approach based on vision detection can be found in a previous paper [32], while joint rate and torque instructions are not difficult to obtain if kinematics and dynamics models are both available. In this sense, the key issue in our vision guidance study is how to drive the mobile robot from the current pose to a target pose. Lamé-curve blending is proposed for kinematic path tracking in the field of view by means of a receding-horizon model.

The receding-horizon strategy is implemented as explained next. In each sampling period, the path image is updated by the vision guidance system as the initial state of path tracking, as shown in Figure 3. In each control period, the Lamé-curve blending technique is used to plan a trajectory with $G^2$-continuity to approach the guide path. In general, several control periods may be needed to complete the process of trajectory tracking. However, only the current robot twist, determined by the trajectory curvature, is used as the control output to the inverse-kinematics model of the robot at the current instant. When the next control period comes, a new Lamé trajectory is generated according to the corresponding new path image, thereby determining a new robot twist as the next control output.

Apparently, the receding-horizon strategy can mitigate the deviations of the actual motion of mobile robots from the predefined trajectory, thereby facilitating the implementation of the kinematics path tracking in the closed-loop vision guidance framework. The sections below focus on both the kinematics and dynamics models of a DWMR, and the kinematic path tracking approach based on Lamé-curve blending.

3. Mobile Robot Modeling

As shown in Figure 5, the platform of a DWMR is depicted as a T-shaped rigid body. Two coaxial wheels are coupled to the platform by means of revolutes of axes passing through points $O_1$ and $O_2$. Let C be the centre of mass of the platform. Point M is the midpoint of segment $\overline{O_1{O}_2}$. Moreover, let the position vectors of C, M, $O_1$ and $O_2$ in an inertial frame be denoted by $\mathbf{c}$, $\mathbf{m}$, ${\mathbf{o}}_1$ and ${\mathbf{o}}_2$, respectively. Additionally, let $\omega$ be the scalar angular velocity of the platform about a vertical axis. In order to proceed with the kinematic analysis of this system, we define a moving frame $\mathcal{F}$, of axes $X,Y,Z$, attached to the platform, with Z pointing in the upward vertical direction. Unit vectors $\mathbf{i},\mathbf{j},\mathbf{k}$ are defined parallel to the X-, Y-, Z-axes, respectively.

3.1. Kinematics

Let the radius of the actuated wheels be r, their angular displacements being ${\theta }_1$ and ${\theta }_2$. The velocity ${\dot{\mathbf{o}}}_i$ of point $O_i$, under pure-rolling, for $i=1,2$, is given by

$${\dot{\mathbf{o}}}_i=r{\dot{\theta }}_i\mathbf{j}i=1,2$$

(1)

Furthermore, the velocity of C can now be expressed in two-dimensional form as

$$\dot{\mathbf{c}}={\dot{\mathbf{o}}}_i+\omega \mathbf{E}(\mathbf{c}-{\mathbf{o}}_i),i=1,2$$

(2)

where $\mathbf{E}$ is defined as an orthogonal matrix rotating two-dimensional vectors through an angle of ${90}^{\circ }$ counterclockwise [33], i.e.,

$$\mathbf{E}\equiv \left[\begin{array}{cc}0& -1\\ 1& 0\end{array}\right]$$

Let the distance between the two actuated wheels be $2l$. Substituting Equation (1) into Equation (2) and subtracting sidewise the latter from the former, we obtain

$$r({\dot{\theta }}_1-{\dot{\theta }}_2)+2\omega l=0$$

(3)

Hence, the scalar angular velocity of the platform is derived from Equation (3) as

$$\omega =\frac{r}{2l}({\dot{\theta }}_2-{\dot{\theta }}_1)$$

(4)

The angular velocities of the two actuated wheels are, therefore,

$$\begin{array}{cc}\hfill {\mathbf{\omega }}_1& =-{\dot{\theta }}_1\mathbf{i}+\omega \mathbf{k}=\left[\begin{array}{cc}-\mathbf{i}-(r/2l)\mathbf{k}& (r/2l)\mathbf{k}\end{array}\right]\dot{\mathbf{\theta }}\hfill \\ \hfill {\mathbf{\omega }}_2& =-{\dot{\theta }}_2\mathbf{i}+\omega \mathbf{k}=\left[\begin{array}{cc}-(r/2l)\mathbf{k}& -\mathbf{i}+(r/2l)\mathbf{k}\end{array}\right]\dot{\mathbf{\theta }}\hfill \end{array}$$

(5)

with the two-dimensional vector $\dot{\mathbf{\theta }}$ of actuated joint rates defined as

$$\dot{\mathbf{\theta }}\equiv \left[\begin{array}{c}{\dot{\theta }}_1\\ {\dot{\theta }}_2\end{array}\right]$$

(6)

Point P is defined at the front of the platform. Let the distance between C and M be a, that between C and P being b. The velocity $\dot{\mathbf{c}}$ of point C can be obtained in terms of ${\dot{\theta }}_1$ and ${\dot{\theta }}_2$ as well, upon substitution of Equation (1) into Equation (2), and addition of Equation (2) for $i=1$ to its counterpart for $i=2$, thus obtaining

$$\dot{\mathbf{c}}=\frac{ar}{2l}({\dot{\theta }}_1-{\dot{\theta }}_2)\mathbf{i}+\frac{r}{2}({\dot{\theta }}_1+{\dot{\theta }}_2)\mathbf{j}$$

(7)

Further, the planar twist $\mathbf{t}$ of the platform is defined as a three-dimensional array:

$$\mathbf{t}\equiv \left[\begin{array}{c}\omega \\ \dot{\mathbf{c}}\end{array}\right]$$

the forward kinematics model of the platform then being expressed as

$$\mathbf{t}=\mathbf{T}\dot{\mathbf{\theta }}$$

(8)

with the 3 × 2 matrix $\mathbf{T}$ defined as

$$\mathbf{T}\equiv \left[\begin{array}{cc}-r/\left(2l\right)& r/\left(2l\right)\\ (ar/2l)\mathbf{i}+(r/2)\mathbf{j}& -(ar/2l)\mathbf{i}+(r/2)\mathbf{j}\end{array}\right]$$

In order to derive the inverse kinematics of the platform, both sides of Equation (7) are dot-multiplied by $\mathbf{j}$, thereby obtaining

$${\dot{\mathbf{c}}}^T\mathbf{j}=\frac{r}{2}({\dot{\theta }}_1+{\dot{\theta }}_2)$$

(9)

The scalar equations of the inverse kinematics are readily derived by combining Equations (4) and (9), namely,

$${\dot{\theta }}_1=\frac{v_c}{r}-\frac{l}{r}\omega ,{\dot{\theta }}_2=\frac{v_c}{r}+\frac{l}{r}\omega$$

(10)

where the longitudinal speed is $v_c\equiv {\dot{\mathbf{c}}}^T\mathbf{j}$, the speed of the centre of mass.

3.2. Dynamics

Within the Newton-Euler formulation applied to multibody systems, we distinguish three rigid bodies composing the DWMR, as shown in Figure 5. The $6\times 6$ inertia dyad of the two actuated wheels are denoted by ${\mathbf{M}}_1$ and ${\mathbf{M}}_2$, with a similar notation for their six-dimensional twists. Since the platform undergoes planar motion, its $3\times 3$ counterpart inertia dyad ${\mathbf{M}}_3^{\prime }$ is introduced here, while the platform twist becomes, correspondingly, the three-dimensional vector ${\mathbf{t}}_3^{\prime }$. Besides, the transformation matrices of the three moving bodies are expressed in a similar way. These relate the body twists with the vector of actuated joint rates $\dot{\mathbf{\theta }}$, introduced in Equation (6), i.e.,

$${\mathbf{t}}_i={\mathbf{T}}_i\dot{\mathbf{\theta }},i=1,2$$

(11)

and

$${\mathbf{t}}_3^{\prime }={\mathbf{T}}_3^{\prime }\dot{\mathbf{\theta }}$$

(12)

where, from Equations (1), (5) and (8), and considering that the two wheels undergo general six-degree-of -freedom motion,

$$\begin{array}{cc}\hfill {\mathbf{T}}_1& =\left[\begin{array}{cc}-\mathbf{i}-r/\left(2l\right)\mathbf{k}& r/\left(2l\right)\mathbf{k}\\ r\mathbf{j}& \mathbf{0}\end{array}\right],\hfill \\ \hfill {\mathbf{T}}_2& =\left[\begin{array}{cc}-r/\left(2l\right)\mathbf{k}& -\mathbf{i}+r/\left(2l\right)\mathbf{k}\\ \mathbf{0}& r\mathbf{j}\end{array}\right],\hfill \\ \hfill {\mathbf{T}}_3^{\prime }& =\frac{r}{2}\left[\begin{array}{cc}-1/l& 1/l\\ (a/l)\mathbf{i}+\mathbf{j}& (-a/l)\mathbf{i}+\mathbf{j}\end{array}\right]\hfill \end{array}$$

(13)

By means of the natural orthogonal complement [34], the generalized dynamics model is derived from the Newton-Euler equations for the DWMR, which leads to

$$\mathbf{I}\ddot{\mathbf{\theta }}=-\mathbf{C}\dot{\mathbf{\theta }}+\mathbf{\tau }+\mathbf{\delta }+\mathbf{\gamma }$$

(14)

where $\mathbf{I}$ is the positive definite 2 × 2 generalized inertia matrix, $\mathbf{C}\dot{\mathbf{\theta }}$ being the 2-dimensional vector of Coriolis and centrifugal-force terms. Furthermore, $\mathbf{\tau }$, $\mathbf{\delta }$ and $\mathbf{\gamma }$ denote the 2-dimensional vectors of generalized active, dissipative, and gravity forces, respectively. Moreover, the twist $\mathbf{t}$ is a linear transformation of the independent generalized speeds $\dot{\mathbf{\theta }}$, as per Equation (8).

The $2\times 2$ generalized inertia matrix $\mathbf{I}$ is obtained as

$$\mathbf{I}={\mathbf{I}}_w+{\mathbf{I}}_p=\sum _{i=1}^2{\mathbf{T}}_i^T{\mathbf{M}}_i{\mathbf{T}}_i+{\left({\mathbf{T}}_3^{\prime }\right)}^T{\mathbf{M}}_3^{\prime }{\mathbf{T}}_3^{\prime }$$

(15)

where ${\mathbf{I}}_w$ and ${\mathbf{I}}_p$ are the inertia matrices of the two actuated wheels and of the platform, respectively.

The $2\times 2$ matrix $\mathbf{C}$ of Coriolis and centrifugal force is derived below:

$$\begin{array}{cc}\hfill \mathbf{C}=& \sum _{i=1}^2{\mathbf{T}}_i^T{\mathbf{M}}_i{\dot{\mathbf{T}}}_i+{\left({\mathbf{T}}_3^{\prime }\right)}^T{\mathbf{M}}_3^{\prime }{\dot{\mathbf{T}}}_3^{\prime }+\sum _{i=1}^2{\mathbf{T}}_i^T{\mathbf{W}}_i{\mathbf{M}}_i{\mathbf{T}}_i+{\left({\mathbf{T}}_3^{\prime }\right)}^T{\mathbf{W}}_3^{\prime }{\mathbf{M}}_3^{\prime }{\mathbf{T}}_3^{\prime }\hfill \end{array}$$

(16)

where ${\mathbf{W}}_i$, for $i=1,2$, and ${\mathbf{W}}_3^{\prime }$ are, correspondingly, $6\times 6$ and $3\times 3$ angular-velocity dyads, defined as

$${\mathbf{M}}_i\equiv \left[\begin{array}{cc}{\mathbf{I}}_i& {\mathbf{O}}_3\\ {\mathbf{O}}_3& m_i{\mathbf{1}}_3\end{array}\right],{\mathbf{W}}_i\equiv \left[\begin{array}{cc}{\mathsf{\Omega }}_i& {\mathbf{O}}_3\\ {\mathbf{O}}_3& {\mathbf{O}}_3\end{array}\right],{\mathbf{W}}_3^{\prime }\equiv \left[\begin{array}{cc}{\omega }_3& {\mathbf{0}}_2^T\\ {\mathbf{0}}_2& {\mathbf{O}}_2\end{array}\right]$$

with ${\mathbf{I}}_i$ and $m_i$ denoting the $3\times 3$ moment-of-inertia matrix and the mass of the ith body, while ${\mathsf{\Omega }}_i$ is the $3\times 3$ angular velocity matrix, the cross-product matrix of the angular velocity vector ${\mathbf{\omega }}_{\mathbf{i}}$, i.e., ${\mathsf{\Omega }}_i=\mathrm{CPM}\left({\mathbf{\omega }}_{\mathbf{i}}\right)$, defined such that for any $\mathbf{r}\in {\mathbb{R}}^3$, ${\mathbf{\omega }}_{\mathbf{i}}\times \mathbf{r}={\mathsf{\Omega }}_i\mathbf{r}$. Besides, ${\mathbf{O}}_3$, ${\mathbf{O}}_2$, ${\mathbf{0}}_2$ and ${\mathbf{1}}_3$ are the $3\times 3$ and $2\times 2$ zero matrices, the two-dimensional zero vector, and the $3\times 3$ identity matrix, respectively.

In order to expand the foregoing matrices, we let $I_x$, $I_y$ and $I_z$ be the three principal moments of inertia of the two actuated wheels, respectively. Moreover, $I_3$ denotes the principal moment of inertia of the platform at its centre of mass. Therefore,

$${\mathbf{I}}_1={\mathbf{I}}_2=\left[\begin{array}{ccc}{I}_x& 0& 0\\ 0& I_y& 0\\ 0& 0& I_z\end{array}\right],{\mathbf{M}}_3^{\prime }=\left[\begin{array}{cc}{I}_3& {\mathbf{0}}_2^T\\ {\mathbf{0}}_2& m_3{\mathbf{1}}_2\end{array}\right]$$

(17)

where, ${\mathbf{1}}_2$ is the $2\times 2$ identity matrix; because of wheel symmetry, $I_x=I_y$.

Substitution of Equations (13) and (17) into Equation (15) leads to

$${\mathbf{I}}_w=\left[\begin{array}{cc}{H}_1& H_2\\ H_2& H_1\end{array}\right],{\mathbf{I}}_p=\left[\begin{array}{cc}{H}_3& -H_3\\ -H_3& H_3\end{array}\right]$$

(18)

where $H_i$, for $i=1,2,3$, is given below:

$$H_1=I_x+\frac{1}{2}{\left(\frac{r}{l}\right)}^2{I}_z+m_1{r}^2,H_2=-\frac{1}{2}{\left(\frac{r}{l}\right)}^2{I}_z,H_3={\left(\frac{r}{2l}\right)}^2(I_3+m_3{a}^2)$$

(19)

The coefficient matrix $\mathbf{C}$ is expanded in a similar way. Since the inertia dyad ${\mathbf{M}}_i$ and the angular-velocity dyad ${\mathbf{W}}_i$ are block-diagonal matrices, the triad $\{\mathbf{i},\mathbf{j},\mathbf{k}\}$ will not appear in matrices ${\mathbf{T}}_i^T{\mathbf{M}}_i{\mathbf{T}}_i$, ${\mathbf{T}}_i^T{\mathbf{M}}_i{\dot{\mathbf{T}}}_i$ and ${\mathbf{T}}_i^T{\mathbf{W}}_i{\mathbf{M}}_i{\mathbf{T}}_i$. Consequently, when the time-history of the actuated joint rates is given, the torque requirements at the different actuated joints can be determined.

4. Path Tracking

4.1. Tracking Target

As shown in Figure 3, kinematic path tracking can be regarded as the generation of a trajectory for the mobile robot to travel along, from its current pose to the target pose. Several types of curves can be used here to blend the trajectory with the Y-axis at points C and $T_k$ of Figure 3. Compared with circular arcs and ellipses, Lamé curves can provide the full trajectory with $G^2$-continuity, meaning that position, tangent and curvature are all continuous along the trajectory, including the blending points.

As shown in Figure 6, the trajectory, on which a robot moves toward the target pose on the prescribed path, can be obtained by curve transformation. C${}_1$ is a standard Lamé curve, intersecting the X- and Y-axes at points $A_1$ and $B_1$, respectively. The difference of the orientation angles at these two points is a right angle, namely, the angle formed by the tangents at point $E_1$. Point $M_k$ is the intersection of the path with the Y-axis. Moreover, $\angle C{M}_k{T}_k=2\psi =\pi -e_{\theta }$, and $l_2=l_1+b$, as shown in Figure 3 ($l_1$ and b) and Figure 6 ($l_2$). Affine transformations are used to convert Lamé curve C${}_1$ into the trajectory, curve $C{T}_k$, by mapping points $B_1$, $E_1$ and $A_1$ into points C, $M_k$ and $T_k$, respectively, while keeping the smoothness of the Lamé curve.

The cubic Lamé curve, the lowest order of this curve family with a variable curvature, is chosen as the trajectory for the DWMR. This curve is defined as [26]

$${\left(\frac{x}{g_x}\right)}^3+{\left(\frac{y}{g_y}\right)}^3=1$$

(20)

where $0\le x\le g_x,0\le y\le g_y$, while $g_x$ and $g_y$ are one-half of the side lengths of the circumscribing rectangle.

The explicit parametric equations of this curve can be expressed in terms of a parameter $\varphi$ ($0\le \varphi <\pi /2$) as

$$x\left(\varphi \right)=\frac{g_x}{{(1+{tan}^3\varphi )}^{1/3}},y\left(\varphi \right)=\frac{g_{y}tan\varphi }{{(1+{tan}^3\varphi )}^{1/3}}$$

(21)

where $\varphi$ is the angle that the position vector of an arbitrary point on the curve makes with the X-axis, as shown in Figure 6. In our case, we use a symmetric Lamé curve, which means $g_x=g_y$.

4.2. Trajectory Blending

As shown in Figure 7, Lamé curve C${}_1$ is defined by the above equation. Affine transformations are implemented on Lamé curve C${}_1$ to conform the derived shape and location in frame $\mathcal{F}$. In this way, a smooth trajectory is generated, tangent to both the Y-axis and the path at points C and $T_k$, the tangent lines intersecting at point $M_k$. The homogeneous coordinates of the Lamé curve C${}_j$ (j = 1, 2, 3, 4) and the trajectory are stored in arrays ${\mathbf{p}}_j$ and ${\mathbf{p}}_5$. The rotation, the homogeneous scaling and displacement matrices are $\mathbf{R}\left(\phi \right)$, $\mathbf{S}(s_x,s_y)$ and $\mathbf{D}$. The first two matrices are given below:

$$\mathbf{R}\left(\phi \right)\equiv \left[\begin{array}{ccc}cos\phi & -sin\phi & 0\\ sin\phi & cos\phi & 0\\ 0& 0& 1\end{array}\right],\mathbf{S}(s_x,s_y)\equiv \left[\begin{array}{ccc}{S}_x& 0& 0\\ 0& S_y& 0\\ 0& 0& 1\end{array}\right]$$

The corresponding transformation procedure follows:

Step 1:

Rotate Lamé curve C${}_1$ through $\pi /4$ around point C counterclockwise to obtain Lamé curve C${}_2$. Hence, the parametric equation of Lamé curve C${}_2$ can be obtained by using the rotation transformation from that of curve C${}_1$, i.e., ${\mathbf{p}}_2=\mathbf{R}(\pi /4){\mathbf{p}}_1$, where ${\mathbf{p}}_1$ and ${\mathbf{p}}_2$ are the position vectors of an arbitrary point on the parametric equations of Lamé curves C${}_1$ and C${}_2$, respectively.

Step 2:

Conform Lamé curve C${}_2$ to curve C${}_3$ in order to make the difference of the orientation angles $\angle A_3{E}_3{B}_3=2\psi$. Since $y_3=y_2$ and $x_3=x_{2}tan\psi$, the parametric equation of Lamé curve C${}_3$ can be obtained by using the scaling transformation from that of curve C${}_2$, i.e., ${\mathbf{p}}_3={\mathbf{S}}_{\psi }(tan\psi ,1){\mathbf{p}}_2$, as shown in Figure 8, with ${\mathbf{p}}_3$ defined as the position vector of an arbitrary point on Lamé curve C${}_3$. For this curve, two tangent lines stemming from points $A_3$ and $B_3$ meet at point $E_3$ (coinciding with $E_2$), at an angle $\angle A_3{E}_3{B}_3=\angle C{M}_k{T}_k$.

Step 3:

Isotropically scale (An isotropic planar scaling is a resizing of a planar figure by means of identical scalar factors in two orthogonal directions.) Lamé curve C${}_3$ to curve C${}_4$ according to the distance $l_2$. Letting the scaling factor be $k_1$, and the length of segment $\overline{B_3{E}_3}$ be $l_0$, $k_1=l_2/l_0$. Hence, the parametric equation of Lamé curve C${}_4$ can be obtained by using the isotropically scaling transformation from that of curve C${}_3$, i.e., ${\mathbf{p}}_4={\mathbf{S}}_k(k_1,k_1){\mathbf{p}}_3$, with ${\mathbf{p}}_4$ defined as the position vector of an arbitrary point on Lamé curve C${}_4$. It is noteworthy that isotropic scaling changes the size of Lamé curve C${}_3$ but preserves its shape, i.e., the length of segment $\overline{B_4{E}_4}$ is changed to $l_2$, while $\angle A_4{E}_4{B}_4$ remains equal to $\angle A_3{E}_3{B}_3$. In this step, Lamé curve C${}_4$ has the size and shape required by the final trajectory.

Step 4:

Displace Lamé curve C${}_4$ to the final trajectory, making points $B_4$, $E_4$ and $A_4$ coincide with points C, $M_k$ and $T_k$, respectively. Store the homogeneous coordinates of points $B_j$, $E_j$, $A_j$, C, $M_k$ and $T_k$ into arrays ${\mathbf{b}}_j$, ${\mathbf{e}}_j$, ${\mathbf{a}}_j$, ${\mathbf{c}}_k$, ${\mathbf{m}}_k$ and ${\mathbf{t}}_k$, respectively. Define five homogeneous coordinate matrices: ${\mathbf{N}}_j$ = [${\mathbf{b}}_j$ ${\mathbf{e}}_j$ ${\mathbf{a}}_j$], for $i=1,2,3,4$, and ${\mathbf{N}}_5$ = [${\mathbf{c}}_k$${\mathbf{m}}_k$${\mathbf{t}}_k$], whose vector blocks are all three dimensional — their entries are the homogeneous coordinates of the corresponding points in the platform frame $\mathcal{F}$. Then, find a homogeneous displacement matrix $\mathbf{D}$ satisfying ${\mathbf{N}}_5=\mathbf{D}{\mathbf{N}}_4$; hence, $\mathbf{D}={\mathbf{N}}_5{\mathbf{N}}_4^{-1}$. Finally, the homogeneous coordinates of the trajectory are calculated as the product of these affine transformations starting from Lamé curve C${}_1$, i.e.,

$${\mathbf{p}}_5={\mathbf{N}}_5{\mathbf{N}}_4^{-1}{\mathbf{S}}_k(k_1,k_1){\mathbf{S}}_{\psi }(tan\psi ,1)\mathbf{R}(\pi /4){\mathbf{p}}_1$$

(22)

with matrix ${\mathbf{N}}_4$ given by

$${\mathbf{N}}_4={\mathbf{S}}_k(k_1,k_1){\mathbf{S}}_{\psi }(tan\psi ,1)\mathbf{R}(\pi /4){\mathbf{N}}_1$$

(23)

where matrices ${\mathbf{N}}_1$ and ${\mathbf{N}}_5$ are obtained from the homogeneous coordinates of the starting points $B_1$, $E_1$ and $A_1$ on curve C${}_1$, and the desired points C, $M_k$ and $T_k$ on the final trajectory. These matrices are

$${\mathbf{N}}_1=\left[\begin{array}{ccc}0& 1& 1\\ 1& 1& 0\\ 1& 1& 1\end{array}\right],{\mathbf{N}}_5=\left[\begin{array}{ccc}0& 0& l_{2}sin{e}_{\theta }\\ 0& l_2& l_2(1+cos{e}_{\theta })\\ 1& 1& 1\end{array}\right]$$

(24)

In order to simplify the calculation, steps 3 and 4 are combined to obtain the homogeneous coordinates of the trajectory from Lamé curve C${}_3$ directly, i.e.,

$${\mathbf{p}}_5={\mathbf{N}}_5{\mathbf{N}}_3^{-1}{\mathbf{S}}_{\psi }(tan\psi ,1)\mathbf{R}(\pi /4){\mathbf{p}}_1$$

(25)

with matrix ${\mathbf{N}}_3$ given by

$${\mathbf{N}}_3={\mathbf{S}}_{\psi }(tan\psi ,1)\mathbf{R}(\pi /4){\mathbf{N}}_1$$

(26)

According to Equations (24)–(26), the composite affine transformation matrix is calculated as

$${\mathbf{T}}_5\equiv {\mathbf{N}}_5{\mathbf{N}}_3^{-1}{\mathbf{S}}_{\psi }(tan\psi ,1)\mathbf{R}(\pi /4)=\left[\begin{array}{ccc}0& -l_3& l_3\\ l_2& -l_4& l_4\\ 0& 0& 1\end{array}\right]$$

(27)

with $l_3$ and $l_4$ defined as

$$l_3=l_{2}sin{e}_{\theta },l_4=l_{2}cos{e}_{\theta }$$

(28)

4.3. Tracking Scheme

Let the curvature of Lamé curve Cj and the trajectory be ${\kappa }_j$ and ${\kappa }_5$, respectively. When the DWMR moves along the trajectory at a velocity $v_c$, the robot twist is constrained by

$$\omega =-{\kappa }_5{v}_c$$

(29)

Based on Equations (10) and (29), when the DWMR moves along the trajectory, the actuated joint rates follow in terms of ${\kappa }_5$:

$${\dot{\theta }}_1=\frac{v_c}{r}(1+l{\kappa }_5),{\dot{\theta }}_2=\frac{v_c}{r}(1-l{\kappa }_5)$$

(30)

If the DWMR tracks the trajectory at a velocity $v_c\left(t\right)$, the time-derivatives of the actuated joint rates are

$${\ddot{\theta }}_1=\frac{{\dot{v}}_c}{r}(1+l{\kappa }_5)+\frac{v_c}{r}l{\dot{\kappa }}_5,{\ddot{\theta }}_2=\frac{{\dot{v}}_c}{r}(1-l{\kappa }_5)-\frac{v_c}{r}l{\dot{\kappa }}_5$$

(31)

As shown in Figure 4, when the actuated joint rates and accelerations are obtained, the desired torques can be readily calculated based on the inverse dynamics model of Equation (14). It is noteworthy that although path tracking is first implemented by means of the Lamé curve blending technique at the level of kinematic control, it is finally converted into the torque control at the dynamic level. Moreover, this multi-loop vision guidance framework is carried out in the receding-horizon mode, implying multi-step planning but one-step control.

5. Simulation Tests

In order to validate the foregoing technique based on curve blending for DWMRs, numerical simulation tests are conducted in the context of geometric trajectory planning, kinematic trajectory tracking and dynamic torque computation. The geometric, kinematic and inertial parameters of the DWMR at hand are given in

Table 2. Geometric and kinematic parameters.

r [m]	l [m]	a [m]	b [m]	${\mathit{v}}_{\mathit{c}}$ [m/s]
0.08	0.2	0.18	0.42	0.5

and

Table 3. Inertial parameters.

$m_1$	2 kg	$I_x$	0.0064 kg·m${}^2$	$I_y$	0.0032 kg·m${}^2$
$m_2$	2 kg	$I_z$	0.0032 kg·m${}^2$	$I_3$	104 kg·m${}^2$
$m_3$	200 kg	$H_4$	0.0192 kg·m${}^2$	$H_5$	8.8581 kg·m${}^2$

. Moreover, the friction coefficient is $\beta$ = 2 Ns/rad, the rated torque of the onboard motors being ${\tau }_r$ = 20 Nm.

The posture errors are detected as $e_{\theta }={30}^{\circ }$ and $l_2$ = 1.6 m. The trajectory from the current posture of the DWMR to the guide path is obtained by means of different curves: one is a circular arc, and the other is an affine transformed Lamé curve. The trajectory curvature and its time-derivative are illustrated in Figure 9 and Figure 10, respectively. In Figure 9, the curvature $\kappa$ of the circular arc has a positive step change from 0 to 0.16 m${}^{-1}$ at the starting point, another negative step change from 0.16 to 0 m${}^{-1}$ at the ending point, and a constant value on the trajectory between these two points. Consequently, one positive and negative impulse occur on the curvature rate of change (RoC) $\dot{\kappa }$ for the circular arc at these two end-points. On the contrary, in Figure 10, the curvature ${\kappa }_5$ of the Lamé curve has a smooth increasing change from 0 m${}^{-1}$ at the starting point, to 0.218 m${}^{-1}$ at the middle point, and then has a smooth decreasing change to 0 m${}^{-1}$ at the ending point. As a result, the curvature RoC $\dot{{\kappa }_5}$ of the Lamé curve still remains a smooth curve, changing from an initial finite positive value, going across the 0 value, and reaching a terminal finite negative value.

For a constant velocity, $v_c$ = 0.5 m/s, the actuated joint rates and their time-derivatives on different trajectories are calculated based on Equations (30) and (31), as shown in Figure 11 and Figure 12, respectively. In Figure 11, the actuated joint rate ${\dot{\theta }}_1$ of the left wheel has a positive step change from 6.25 to 6.5 rad·s${}^{-1}$ at the starting point, another negative step change from 6.5 to 6.25 rad·s${}^{-1}$ at the ending point, and a constant value on the trajectory between these two points. Consequently, one positive and negative impulse occur on the angular acceleration ${\ddot{\theta }}_1$ of the left wheel for the circular arc at these two end-points. On the contrary, in Figure 12, the actuated joint rate ${\dot{\theta }}_1$ of the left wheel on the Lamé curve has a smooth increasing change from 6.25 rad·s${}^{-1}$ at the starting point, to 6.523 rad·s${}^{-1}$ at the middle point, and then has a smooth decreasing change to 6.25 rad·s${}^{-1}$ at the ending point. As a result, the angular acceleration ${\ddot{\theta }}_1$ of the left wheel for the Lamé curve remains smooth, changing from an initial finite positive value, going across the 0 value, and reaching a terminal finite negative value. From Figure 10 and Figure 12, it is apparent that the trajectory curvature of the Lamé curve and the actuated joint rates of the two wheels on the curve are significantly flat in the middle section.

Fianlly, the required torques of the onboard motors of the two driving wheels are computed for the DWMR to move along different trajectories, as shown in Figure 13 and Figure 14, respectively. In Figure 13, the motor torque of the left wheel has a positive impulse at the starting point, another negative impulse at the ending point, and a constant value on the trajectory between these two points. It is noteworthy that the amplitude of the torque impulse is more than 50 Nm, exceeding the rated torque of the onboard motors. On the contrary, in Figure 14, the motor torque of the left wheel for the Lamé curve remains smooth, changing from an initial finite positive value, going across the 0 value, and reaching a terminal finite negative value. The maximum amplitude of the motor torque is always kept below the rated torque.

Moreover, the simulation results reflect the stepwise processing result of the multi-loop vision guidance framework. In the first step, a Lamé-curve trajectory is generated by kinematic path tracking, with its shape as shown in Figure 7, and pertinent parameters, as shown in Figure 10. In the second step, the actuated joint rates and angular accelerations are calculated based on the inverse-kinematics model, as shown in Figure 12. In the third step, the required wheel torques are computed based on the inverse-dynamics model, as shown in Figure 14.

6. Conclusions

Applying computer vision to mobile robot navigation has been studied for more than two decades. Cameras can be fixed on the ceiling with a large field of view, or mounted on the mobile robot with a high recognition accuracy. The limited field of view of onboard cameras brings great difficulties to path tracking of DWMRs in high-speed manoeuvres. In order to cope with the demands of the latter, a multi-loop receding-horizon control framework, including path tracking, robot control, and drive control, is proposed for vision guidance of mobile robots. Lamé curves are used to synthesize a trajectory with $G^2$-continuity in the field of view of the mobile robot for path tracking, from its current posture towards the guide path. The whole multi-loop control process, from Lamé-curve blending to computational torque control, is carried out iteratively by means of the receding-horizon strategy. In simulation tests, a circular arc and a Lamé curve are, respectively, used as the blending trajectory for the vision guidance approach. The comparison of trajectory curvature, actuated joint rates and motor torques shows the effectiveness of the vision guidance approach based on Lamé-curve blending. In future research work, our guidance control approach will probably be experimentally integrated with a practical vision guidance system, and further validated on a vision-guided mobile robot running in a shop-floor environment.