# Practical Position Tracking for 3D Unicycle Model

Authors: Haoyan Lin; He Cai
Journal: Mathematical Problems in Engineering
 (2022)

---

## Abstract

This paper considers practical position tracking for the 3D unicycle model. In contrast to the existing hand position coordinate transformation, a novel modified hand position coordinate transformation is proposed for the 3D unicycle model which can guarantee that the induced velocity transformation matrix is nonsingular for all the possible orientations. By employing the proposed modified hand position coordinate transformation, the nonlinear 3D unicycle position kinematics can be converted into the single integrator form, which enables arbitrary position reference trajectory tracking given any prespecified upper bound for the steady-state tracking error. The control performance is examined by numerical examples.


---

## Body

## 1. Introduction

The guidance and control problem for robotic system has long been a central topic for both the control and robotic communities [1–3], which leads to fruitful results in the literature, for instance, the intelligent neural network-based control approaches [4–6] and the trajectory tracking for bio-inspired smart robots [7–9], just to name a few. Unicycle is widely adopted as a simplified mathematical model for mobile vehicles, such as the differential drive vehicles [10, 11], aircraft-like vehicles [12, 13], underwater vehicles [14], and so on. The control of the unicycle model, including both stabilization and tracking, is a well-known benchmark problem which has attracted extensive attention from both the control and robotic communities. Though appearing deceptively simple, the control of the unicycle model is indeed rather challenging in that such a system cannot be stabilized by any smooth time-invariant feedback due to its nonholonomic constraint [15].The vector of generalized coordinate of a unicycle model consists of both position and orientation, and all the vectors of generalized coordinate constitute the configuration space of the unicycle model. Note that the nonholonomic constraint requires that all the admissible vectors of generalized velocity belong to the null space of the constraint matrix, while the motion in the configuration space is not limited. Therefore, the unicycle model is underactuated for the case of configuration tracking, where the configuration reference trajectory must satisfy the nonholonomic constraint. The local and global configuration tracking for 2D unicycle model has so far been well investigated, which leads to various discontinuous and/or time-varying control schemes (see [16–19] and references therein). On the other hand, if only the position tracking of the unicycle model is of interest, then the number of system inputs and outputs will be equal and thus the unicycle model becomes fully actuated. In [20], a hand position coordinate transformation was proposed to solve the stabilization problem for the 2D unicycle model. A salient feature of the hand position coordination transformation is that the induced velocity transformation matrix is nonsingular for all the possible orientations, through which the nonlinear position kinematics of the unicycle model can be converted into the single integrator form, thus allowing arbitrary position reference trajectory tracking given any prespecified upper bound for the steady-state tracking error, called practical position tracking. Since [20], there have been many results based on this hand position coordination transformation, especially on the formation control of multiple unicycle systems [21–24].The 3D unicycle model is an extension of the 2D unicycle model from planar motion to space motion. Unlike the 2D unicycle model, there have been thus far less results on the motion control of the 3D unicycle model since it is much more complex than that of the 2D unicycle model. In particular, the 3D unicycle model has three Cartesian coordinates and two angle coordinates, while the 2D unicycle model has two Cartesian coordinates and only one angle coordinate. As a result, two extra variables are involved with the nonholonomic constraint for the 3D unicycle model, thus making the kinematic equations for the 3D unicycle model being subject to more complex nonlinearities in terms of the product of sinusoidal functions of the angles. For the 3D unicycle model, the dipolar navigation function-based navigation methodology was invoked in [13] for solving the collision avoidance problem. Later, Wiig et al. [25] studied the same collision avoidance problem by maintaining a constant avoidance angle to the obstacle. Besides, the authors in [12, 26, 27] considered the flocking or formation control problem for a group of 3D unicycle models. In particular, inspired by the work of Pomet et al. [20], a 3D hand position coordinate transformation was proposed by Li et al. [27]. However, the induced velocity transformation matrix of the 3D hand position coordinate transformation would be singular for certain orientations. Therefore, the resulting practical position tracking control shall also suffer from the singularity issues.To address this issue, in this paper, a novel modified hand position coordinate transformation for the 3D unicycle model is proposed, which, different from the hand position coordinate transformation, can guarantee that the induced velocity transformation matrix is nonsingular for all the possible orientations, and thus it makes it possible to achieve practical position tracking for arbitrary position reference trajectory for the 3D unicycle model.In contrast to the existing results, the main contributions of this paper are as follows:(i)
In [12], complex projection operators were designed to achieve local leaderless consensus. In contrast, the control approach proposed in this paper is a simple PD type controller which can achieve global practical position reference tracking.(ii)
The control laws in [13, 26] are discontinuous. Moreover, the nonsmooth analysis of Roussos et al. [13] is based on an approximated scheme and the initial angular values of the agents in [26] should be within some specified range, while the control approach proposed in this paper is continuous and valid for any system initial condition, and the result is proven without any approximation.(iii)
The underwater vehicles considered in [25] are assumed to be passively stable with respect to the motion of rolling, while in this paper, no assumption is imposed on the motion of the 3D unicycle.(iv)
Different from the hand position coordinate transformation proposed in [27] which will be singular at certain angle, the modified hand position coordinate transformation proposed in this paper is singularity free.


## 2. 2D Unicycle Model Revisited

In this section, we will revisit the practical position tracking for the 2D unicycle model, which is given by (to save space, given an angleθ, define cθ≜cosθ and sθ≜sinθ)(1a)x˙=vcθ,(1b)y˙=vsθ,(1c)θ˙=ω,where x,y denotes the Cartesian position of the vehicle center, θ denotes the orientation, v and ω denote the linear and angular velocity input, respectively, and q=x,y,θT denotes the vector of generalized coordinate. As in [10, 28, 29], the nonholonomic constraint of (111) is given by(2)x˙sθ−y˙cθ=0,or in matrix form(3)0=sθ−cθ0x˙y˙θ˙≜A2Dqq˙.By (3), the vector of generalized velocity q˙ must belong to the null space of the constraint matrix A2Dq, while the motion q in the configuration space ℝ3 is free.Suppose the position reference trajectory for 2D unicycle model isrxt,ryt which is differentiable. Then, the practical position tracking problem for 2D unicycle model is given as follows.Problem 1.
(practical position tracking for 2D unicycle model). Given system (1a)–(1c), define the trajectory tracking error as(4)et=xtyt−rxtryt.
Given anyε〉0, design control input v,ω such that(5)limsupt⟶∞et〈ε.
Illustrated by Figure1, the following hand position coordinate transformation was proposed in [20]:(6)xhyh=xy+dcθsθ,where d〉0. Then, it follows that(7)x˙hy˙h=vcθvsθ+d−ωsθωcθ=cθ−dsθsθdcθvω≜Jθ,dvω.
Since detJθ,d=d≠0, the induced velocity transformation matrix Jθ,d is nonsingular for all θ∈ℝ. Thus, we have the following result.Figure 1
2D hand position coordinate transformation.Theorem 1.
By choosing0〈d〈ε, Problem1is solvable by the following control law:(8)vω=J−1θ,dr˙x−αxh−rxr˙y−αyh−ry,for any α〉0.Proof 1.
Submitting (8) into (7) gives(9)x˙hy˙h=r˙x−αxh−rxr˙y−αyh−ry.
Letx˜h=xh−rx and y˜h=yh−ry. Then by (9), we have(10)x˜˙h=−αx˜h,y˜˙h=−αy˜h.
As a result, sinceα〉0, it follows that(11)limt⟶∞x˜ht=0,limt⟶∞y˜ht=0,i.e.,(12)limt⟶∞xht−rxt=0,limt⟶∞yht−ryt=0.
Note that(13)et=xtyt−rxtryt=xhtyht−rxtryt−dcθtsθt.
Therefore, by (12), it follows that(14)limsupt⟶∞et=d〈ε.Remark 1.
Through the hand position transformation, the nonlinear position kinematics (1a) and (1b) have been converted into the form of equation (7). By compensating the nonsingular velocity transformation matrix Jθ,d, equation (7) becomes a single integrator, thus allowing arbitrary hand position reference trajectory tracking, which is equivalent to arbitrary position reference trajectory tracking given any prespecified upper bound for the steady-state tracking error, depending on the design parameter d, while a trade-off on the selection of the distance parameter d lies in that smaller d indicates smaller steady-state tracking error, but possibly larger control inputs v and ω.


## 3. Main Results

In this section, we will present the practical position tracking for 3D unicycle model, which is given by(15a)x˙=vcθcφ,(15b)y˙=vsθcφ,(15c)z˙=vsφ,(15d)θ˙=ωθ,(15e)φ˙=ωφ,where x,y,z denotes the Cartesian position of the vehicle center, θ,φ denotes the orientation, v denotes the linear velocity control input, ωθ,ωφ denotes the angular velocity control inputs, and q=x,y,z,θ,φT denotes the vector of generalized coordinate. Similar to (2), the nonholonomic constraint of (1515151515) is given by(16)x˙cθsφ+y˙sθsφ−z˙cφ=0,or in matrix form(17)0=cθsφsθsφ−cφ00x˙y˙z˙θ˙φ˙,≜A3Dqq˙.By the Lie bracket verification as in [28, 29], it can be verified that the velocity constraint (17) is not integrable.Suppose the position reference trajectory for 3D unicycle model is given byrxt,ryt,rzt which is differentiable. Then, the practical position tracking problem for 3D unicycle model is given as follows.Problem 2.
(practical position tracking for 3D unicycle model). Given system (12), define the trajectory tracking error as(18)et=xtytzt−rxtrytrzt.
Given anyε〉0, design the control input v,ωθ,ωφ such that(19)limsupt⟶∞et〈ε.
Inspired by Pomet et al. [20], the following 3D hand position coordinate transformation was adopted in [27]:(20)xhyhzh=xyz+d1cθcφsθcφsφ,where d1〉0. Then, it follows that(21)x˙hy˙hz˙h=vcθcφvsθcφvsφ+d1−ωθsθcφ−ωφcθsφωθcθcφ−ωφsθsφωφcφ=cθcφ−d1sθcφ−d1cθsφsθcφd1cθcφ−d1sθsφsφ0d1cφvωθωφ≜Jhθ,φ,dvωθωφ.
Since detJhθ,φ,d=d12cφ, the velocity transformation matrix Jhθ,φ,d will be singular at φ=±π/2.
To address this issue, in this paper, we define a modified hand position coordinate transformation as follows:(22)xmymzm=xyz+d1cθcφsθcφsφ+d2cθsθ0,where d1,d2〉0. In geometry, the term d1cθcφ,sθcφ,sφT represents a position offset along the direction of the velocity, while the term d2cθ,sθ,0T represents a position offset along the direction of the projection of the velocity in the XOY plane. A two-step illustration is given in Figure 2.
It follows that(23)x˙my˙mz˙m=vcθcφvsθcφvsφ+d1−ωθsθcφ−ωφcθsφωθcθcφ−ωφsθsφωφcφ+d2−ωθsθωθcθ0=cθcφ−d1sθcφ−d2sθ−d1cθsφsθcφd1cθcφ+d2cθ−d1sθsφsφ0d1cφvωθωφ≜Jmθ,φ,d1,d2vωθωφ.
Since detJmθ,φ,d1,d2=d1d2+d1cφ, the velocity transformation matrix Jmθ,φ,d1,d2 will be nonsingular for all θ,φ∈ℝ as long as 0〈d1〈d2. Then, we have the following result.Figure 2
3D modified hand position coordinate transformation.Theorem 2.
By choosing0〈d1〈d2≤ε/2, Problem2is solvable by the following control law:(24)vωθωφ=Jm−1θ,φ,d1,d2r˙x−αxm−rxr˙y−αym−ryr˙z−αzm−rz,for any α〉0.Proof 2.
Submitting (24) into (23) gives(25)x˙my˙mz˙m=r˙x−αxm−rxr˙y−αym−ryr˙z−αzm−rz.
As a result, similar to the proof of the 2D unicycle case, it follows that(26)limt⟶∞xmt−rxt=0,limt⟶∞ymt−ryt=0,limt⟶∞zmt−rzt=0.
We have(27)et=xtytzt−rxtrytrzt=xmtymtzmt−rxtrytrzt−d1cθcφsθcφsφ−d2cθsθ0,andd1cθcφ+d2cθ2+d1sθcφ+d2sθ2+d12sφ2=d12+d22+2d1d2cφ≤d12+d22+2d1d2=d1+d22.
Therefore, by (26), it follows that(28)limsupt⟶∞et≤d1+d2〈ε.Remark 2.
Similar to the case of the 2D unicycle model, there is a trade-off on the selection of the distance parametersd1,d2. In particular, the smallerd1,d2are, the smaller the steady-state tracking error is, but possibly the larger the magnitude of the control inputs is.Remark 3.
By checking (24) and (25) together, the control law (24) is indeed a PD type controller whereJm−1θ,φ,d1,d2can be viewed as a feedback linearization term. In particular, the vectorsr˙x,r˙y,r˙zTand−αxm−rx,ym−ry,zm−rzTare the proportional part and derivative part of the PD type controller, respectively.


## 4. Numerical Examples

In this section, numerical examples are presented to examine the performance of the proposed control method. Suppose the position reference trajectory is given by(29)rxt=5sint,ryt=5cost,rzt=t.Note that in this case, the orientation reference trajectories can be calculated by(30)θrt=arctanr˙yr˙x=π−modt+π,2πrad,φrt=arctanr˙zr˙x2+r˙y2=arctan15≈0.2rad.Moreover, supposeε=1 and let α=1. In what follows, we examine the control performance for three different cases.Case 1.
Effect of initial configuration.
In this case, we show the control performance of the proposed control method subject to different initial configurations. Letd1=0.1, d2=0.2. Three initial configurations are considered:(1)
IC1:q00=0,5,0,0,1/5T.(2)
IC2:q00=0,0,−1,π/4,π/4T.(3)
IC3:q00=5,1,0,π,π/3T.
The control performance is shown in Figure3. It can be seen that successful practical tracking has been achieved for all these three initial configurations. Moreover, both the transient and steady-state control inputs are of the same magnitude.Figure 3
Control performance subject to different initial configurations. (a) The time profile of ‖e(t)‖. (b) The position trajectory of the 3D unicycle model. (c) The time profiles of θ(t) and φ(t). (d) The time profiles of the control inputs.
(a)(b)(c)(d)Case 2.
Effect of distance parameters.
In this case, we show the control performance of the proposed control method subject to different distance parameters. Letq00=0,0,−1,π/4,π/4T. Three sets of distance parameters are considered:(1)
DP1:d1=0.25, d2=0.5.(2)
DP2:d1=0.1, d2=0.2.(3)
DP3:d1=0.01, d2=0.02.
The control performance is shown in Figure4. It can be seen that smaller distance parameters lead to smaller steady-state tracking errors, yet resulting in larger control magnitude.Figure 4
Control performance subject to different distance parameters. (a) The time profile of ‖e(t)‖. (b) The position trajectory of the 3D unicycle model. (c) The time profiles of θ(t) and φ(t). (d) The time profiles of the control inputs.
(a)(b)(c)(d)Case 3.
Effect of noisy orientation measurements.
In this case, we show the control performance of the proposed control method subject to noisy orientation measurements. Letθmt and φmt be the measured orientations, which are supposed to be θmt=θt+θdt and φmt=φt+φdt with θdt,φdt being the stochastic measurement noises. Suppose θdt,φdt∈−0.05,0.05. Let d1=0.25, d2=0.5, and q00=0,0,−1,π/4,π/4T. The control performance is shown in Figure 5. It can be seen that noisy orientation measurements do not affect much the control performance.Figure 5
Control performance subject to noisy orientation measurements. (a) The time profile of ‖e(t)‖. (b) The position trajectory of the 3D unicycle model. (c) The time profiles of θ(t) and φ(t). (d) The time profiles of the control inputs.
(a)(b)(c)(d)Case 4.
Comparison with the classic hand position coordinate transformation.
The modified hand position coordinate transformation proposed in this paper is global without singularity issues, while the classic hand position coordinate transformation proposed in [27] is not. In this case, we will show this fact by numerical simulations. To this end, we modify the position reference trajectory to be(31)rxt=0,ryt=0,rzt=t.
Supposeε=1. Let α=1, d1=0.25, and d2=0.5. The initial configuration is given by(32)q00=−1,−1,0,0.7,π/3T.
The simulation results for both methods are shown in Figure6, where M-HPCT is short for modified hand position coordinate transformation proposed in this paper and HPCT is short for hand position coordinate transformation proposed in [27]. It can be seen that the though the control input ωφt for the M-HPCT has a large transient at t=8 sec, it is still bounded. However, all the three control inputs for the HPCT method tend to go unbounded since in this case, φrt=π/2, which is singular for the HPCT method.Figure 6
Comparison of the control performance. (a) The time profile of ‖e(t)‖ by M-HPCT. (b) The time profile of ‖e(t)‖ by HPCT. (c) The time profiles of θ(t) and φ(t) by M-HPCT. (d) The time profiles of θ(t) and φ(t) by HPCT. (e) The position trajectory of the 3D unicycle model by M-HPCT. (f) The position trajectory of the 3D unicycle model by HPCT. (g) The time profiles of the control inputs by M-HPCT. (h) The time profiles of the control inputs by HPCT.
(a)(b)(c)(d)(e)(f)(g)(h)


## 5. Conclusion

This paper proposed a novel modified hand position coordinate transformation for the 3D unicycle model. Since the induced velocity transformation matrix is nonsingular for all the possible orientations, the system kinematics can be converted into the single integrator form, which makes it simple to achieve practical position tracking for the 3D unicycle model, while a drawback of the current method might be that the resulting control input may incur large transient for some special types of position reference trajectories, such as Case4 in the numerical example. In the future, it would be interesting to overcome this issue possibly by integrating the modified hand position coordinate transformation with path planning techniques.



---

*Source: 5725917-2022-09-30.xml*