J Intell Robot Syst

DOI 10.1007/s10846-016-0391-7

CrossMark

Waypoint Following for Differentially Driven Wheeled Robots with Limited Velocity Perturbations

Asymptotic and Practical Stabilization Using Transverse Function Approach

Dariusz Pazderski

Received: 2 March 2016 / Accepted: 21 April 2016

© The Author(s) 2016. This article is published with open access at Springerlink.com

Abstract In this paper a unified motion control strategy dedicated for the waypoint following task realized by a differentially driven robot is presented. It is assumed that the vehicle moves with limited velocities and accelerations in order to reduce excessive slip and skid effects. In order to include operational constraints, a motion planner is combined with a universal stabilizer taking advantage of transverse functions. To improve tracking precision translated transverse functions are deployed and a new adaptive technique for the controller tuning is proposed. During the motion planning stage an auxiliary trajectory connecting points in the configuration space and satisfying assumed phase constraints is generated. The resulting motion execution system has been implemented on a laboratory-scale skid-steering mobile robot, which served as platform for experimental validation of presented algorithms.

Keywords Motion control • Transverse functions • Systems on Lie groups

Dariusz Pazderski (I)

Poznan University of Technology, ul. Piotrowo 3a, 61-138, Poznan, Poland

e-mail: dariusz.pazderski@put.poznan.pl

1 Introduction

Motion control constitutes one of fundamental problems encountered in robotics. In particular, this issue is challenging when a robot subject to nonintegrable differential constraints is taken into account. In such a case there are restrictions imposed on its velocities and accelerations which hinder to change the robot configuration in an arbitrarily way.

A well known example of such systems are differentially driven wheeled robots equipped with two or more fixed wheels. In particular, the vehicles with the skid-steering locomotion can be seen as robust and universal platforms well suited for many applications, [24]. Such vehicles are governed by a highly uncertain dynamics describing a wheels-ground interaction phenomenon. In robotics literature various attempts to model similar systems have been presented. Most formal methods based on explicit description of interaction forces can be effectively used for simulation and open-loop control, [25]. Alternatively, a combination between kinematic and dynamic models can be investigated, [4, 10, 24]. Another approach is based on kinematic approximation of nonitegrable velocity constraints, [3, 9, 18].

In this paper we consider a local kinematic model of a skid-steering vehicle assuming that extensive slip is an undesirable phenomenon which can be regarded as a non significant perturbation when the vehicle

Published online: 28 June 2016

Ö Springer

moves with relatively low velocities and accelerations.

The main control task discussed here is practically oriented. It is assumed that the robot moves through the prescribed way-points defined in a configuration space, [11]. In order to solve this problem we propose to combine a motion planner with a universal feedback tracking controller. Such a solution allows one to define a suitable reference motion taking into account desired velocity profiles, path curvatures, slip limitations, etc. Moreover, by decreasing of initial configuration errors a transient response of a closed-loop controller can be reduced significantly.

The motion controller proposed here is based on transverse functions (TFs) introduced by Morin and Samson [12]. The unique property of this control paradigm is to ensure practically stable tracking with non zero error of any (smooth enough) reference trajectory defined in a configuration space. Basically, a controller using TFs can be conveniently designed for systems on Lie group [13]. Then a nonholonomic system can be linearised approximately in a global domain. In [14] and [ 17] it is shown that in some cases asymptotic stability can be achieved when generalized transverse functions are used.

So far not many papers address implementation and practical issues of this control method. In fact only few results have been reported [2, 7, 18]. In [20] selected properties of a controller taking advantage of TFs are thoroughly investigated.

In this paper, motivated by ideas presented in [14, 17, 20], we are focused on control solution for asymptotic tracking and regulation in a quite uniform way. Basically, we extend preliminary results considered in [19] where it is shown that generalized TFs provide much better performance of the controller during execution of typical motion tasks. We investigate conditions of asymptotic convergence for the tracking case and introduce a new adaptation method to decrease size of a transverse function without violating the transversality condition. The formal stability proof of the proposed methods are given. Moreover, it is shown how to improve convergent rate by increasing the size of convergence set.

The motion planner used in the proposed control architecture is based on polynomial segments to connect subsequent way-points. Recently, a similar approach taking advantage of Bezier curves has been studied in [23]. In order to ensure continuous transitions between path segments, tangent and curvature

conditions at waypoints are investigated. Then, using time scaling, an almost admissible trajectory is calculated assuming the desired bounds imposed on linear and angular velocities.

Summarizing, the main purposes of this paper can be itemized as follows:

- presentation of the motion controller using a translated transverse function with an adaptation law (a stability proof is given),

- illustration of the control system designed for motion control of a differentially driven wheeled vehicle (possibly using the skid-steering locomotion) in a practical application,

- experimental verification of the controller robustness to limited skid/slip effects and other unmod-elled dynamics.

The paper is organized as follows. In Section II the control paradigm taking advantage of transverse functions is recalled. The structure of the decoupling controller is shown and possibility of asymptotic stabilization is discussed. A method of stabilizing of augmented dynamics is proposed. Next section is focused on design of control solution for a differentially driven robot. The approximated kinematic model of this robot is outlined. Then the controller using transverse functions for the given system is presented in details. The stability analysis is formally given for the selected control tasks. Next, a motion planner taking advantage of a path planning algorithm and design of velocity profile is presented. In Section IV implementation of the control system is given and results of experiments are shown. Section V concludes the paper.

2 Decoupling of Nonholonomic Systems on Lie Group Using Transverse Functions

2.1 Preliminary and notation

Assume that G denotes a Lie group with group operation gh e G, where g,h e G. The inverse element of g, denoted by g-1 e G, satisfies gg-1 = g-1g = e, with e being neutral (identity) element of group G. The fundamental group diffeomorphisms include: left translation lg : G ^ G,h ^ gh, right translation rg : G ^ G,h ^ hg and conjugation : G ^ G,h ^ lg(rg-1 (h)) = r -\ (lg(h)) = ghg-1. Differentials

of these maps are defined by: dlg(h) := jkh(h), drg(h) := dhrg(h) and d^g(h) := dhghg-1, respectively.

For the given Lie group G associated Lie algebra g can be defined. It consists of vector fields V1,V2,...,Vi,... on G which are left-invariant under left translation, namely

Wg,h e G, dlg(h)Vi(h) = Vi(gh).

Assume that a basis of g is constituted by n independent vector fields X1,X2,..., Xn e g, where n = dim G. Applying vector-matrix notation this basis can be defined by X := [X1 X2 ... Xn] e Rnxn. Accordingly, one can express any vector field V e g evaluated at g e G in the Lie algebra basis as: V = J21=1 Xi (g) Vi = X (g) V, where v = [V1 ... Vn]T e Rn. Noticing that each vector field in basis X is left invariant, from Eq. 1 follows that

Wg, h e G, dlg(h)X(h) = X(gh).

The other important differential operator is the adjoint operator Ad : G x g ^ g defined by

Ad(g)V := d^g(e)V, where V e g. To simplify notation the following conjugation of Ad can be used:

AdX(g) = X(e)-1Ad(g)X(e).

2.2 Control System on Lie Group

Consider m input small-time locally controllable (STLC) system defined on G

g = ^ Xi(g)ui,

where g e G denotes configuration, Xi,X2,..., Xm e g are left-invariant control vector fields and u1,u2,...,um denote inputs. Since the given system is STLC one can define the following basis of g

X ■ = [X1 X2 ... Xm Xm+1 . . . Xn\,

where Xm+1, ...,Xn are properly chosen (first and higher order) Lie brackets of control vector fields X1; X2 ..., Xm. Equivalently, using Eq. 4 one can rewrite Eq. 3 as follows (cf. [17])

g = X(g)Cu,

where C := [I 0]T e R"xm, with I e Rmxm being identity matrix, and u = [u1 u2 ... um]T e Rm.

Additionally, we consider a perturbed structure of control system (5) given by

g = X(g) (Cu + d),

where d e Rn is a bounded function. Here, we recall that if d is the exogenous term independent on input u and system (3) is STLC it is possible to compensate d, at least approximately, by applying an appropriate sequence of control input u. The quality of this approximation can be made arbitrary high assuming unbounded frequency of u.

2.3 Transverse Function

Now we recall definition of a transverse function determined on a torus and consider its translation on a Lie group.

Definition 1 (cf. [13]) Let f : Tp ^ G, where p > n — m be a smooth function defined on p dimensional torus satisfying

Wa e Tp, rankM(a) = n while

M(a) ■=

Xi(f(a))X2(f(a)) ... Xm(f(a))-

^Rnx(m+p) (8)

Wa e Tp, f(a) e Bs(e),

where Bs(e) denotes the n-dimensional ball with radius s and centre e. This function is transversal to control vector fields Xm+1; ...,Xn and is called

transverse function (TF) for system (3).

Notice that when p = n — m the transversality condition given by Eq. 7 can be written in a more convenient way using basis X. Correspondingly, it can be assumed that

df(a) da

■= X(f(a))A (a)

where A (a) := [A~T ATT]T e Rnx(n—m) is the matrix composed of columns being derivative of f expressed

in X, while A1 e Rmx(n—m) and A2 e Rin—m)x(n—m).

Using Eq. 10 and recalling definition of C allows one to rewrite matrix M as follows

M(a) = X(f(a))C(a),

where C(a) := [C — A(a)] e Rnxn. Since X is the full rank matrix and p = n — m, the condition given by Eq. 7 is ensured when C(a) is invertible. Next, computing determinant of C one has: det C = det A2. As a result for p = n — m the transversality condition is given by

Va e Tn-m, det Ä2(a) = 0.

Formula (12) can be used explicitly to verify if f is a transverse function for the chosen set of parameters, [20].

Selection of a transverse function for the given driftless affine system is not unique. On a Lie group one can find a family of transverse function using the left translation.

Proposition 1 (Family of transverse functions on Lie group) Assume that f e G satisfies transversality condition, expressed by Eqs. 9 and 12, and h is an arbitrary element on G. Then

f := hf

is also the transverse function. Correspondingly, one can say that f is the translated transverse function.

Proof Taking time derivative of Eq. 13 yields in f = dlh(f )f + df(h)h. (14)

Since f = X(f )A(a)a, Eq. 14 becomes

f = dlh(f)X(f)A(a)a + drf-(h)h. (15)

Then taking advantage of left invariance of vector fields in basis X and applying Eq. 13 we have:

dlh(f )X(f ) = X(hf) = X(f). As a result we get

f = X(f)A(a)a + drf-(h)h. (16)

Following Eq. 16 one can easily conclude that

f = X(f)Ä(a). da

Since A is such that Eq. 12 is satisfied from assumption and f = hf is bounded for any bounded h e G it follows that f is the transverse function. □

Remark 1 The considered construction of transverse function f gives possibility to freely translate the prototype of transverse function f (typically centered at neutral element e, [16]) on a Lie group without affecting the transversality condition. Additionally, in particular cases one can also employ another method based on dilation to shape the transverse function in order to adjust radius of neighborhood where this function evolves. However, the latter approach is dedicated for affine systems with homogenous vector fields including the chained form (which can be seen as an approximation of many practically motivated nonholonomic kinematics) - see also [6,13, 20].

Now investigate derivative (15) assuming that

h = X(h)vh, (18)

where vh e R" and rewrite formula (16) as follows (see Appendix A.2 - Eq. 103)

f = X(f)(A(a)a + AdX(f-1)vh). (19)

The following result is motivated by proposition of generalized transverse functions (GTFs) introduced in [17]. Let us assume that fr(ar) : T"-m ^ G isanele-ment on Lie group and let h = f-l(ar) e G. In such a case the translated transverse function (13) becomes

f = fr(ar) f (a).

It is assumed that time derivative of f r satisfies

fr = far = X(fr(ar))Ar(ar)ar, (21)

where Ar e R"x("-m). Correspondingly, f can be computed as follows (the details can be found in Appendix A.2 - Eq. 106)

f = X(f)(A(a)a - AdX(f-l)Ar{ar)a^ . (22) 2.4 Companion System and Tracking Control

The fundamental property of the control method taking advantage of the transverse functions approach is based on the STLC concept. Basically, it is possible to decouple approximately a STLC system in spite of kinematic constraints and an additive drift. In the considered case one can apply this technique to recover an almost linear system.

Assume that z e G denotes auxiliary configuration and suppose that for t > 0, z(t) and configuration

g(t) evolve in similar way, such that g(t) e Bs(z(t)), where Br(p) = {g e G : dist(g, p) < r}. Let the distance between elements, z and g, be determined by transverse function f. Then defining an error on Lie group G one can write: z—1g := f. Alternatively, it can be found that

z = gf-1.

Taking time derivative of z and following analysis presented in [17] (see also Appendix A.3 for details -Eq. 112) we consider the companion system defined as

Z = X(z)AdX(f)(c(a)U+d+AdX(f—1)Ar(ar)ct^ ,

where u ■= [uT a

e Rn is the extended input.

Since X(z)AdA(f)C(a) e Rn

is the invertible

matrix one concludes that z can be controlled arbitrarily using bounded input U. Thus, Eq. 24 becomes an unconstrained system evolving on G. Consequently, a motion control problem considered for system (6) can be solved indirectly by applying an appropriate control solution defined for Eq. 24.

Following this idea, one can consider a bounded continuous reference trajectory gr e G for Eq. 24 which satisfies

gr = X(gr)ur,

where ur e Rn is a bounded reference input. Notice that components ur 1, ..., urm correspond to admissible inputs, while urm+1, ..., urn are extended inputs with respect to system (5). To quantify tracking error on Lie group G one can define

Z ■= gr Z.

Taking time derivative of Eq. 26 and using Eq. 24 the following open-loop dynamics can be developed (cf. Appendix A.4 - Eq. 117)

Z = X(Z)AdX(f)(cu + d - AdX(g-1)ur

+AdX(f-l)Ar(ar)àr) , (27)

where g ■= g-1 g is the tracking error expressed with respect to configuration g.

The controller which linearises system (27) can be formulated as follows.

Proposition 2 (Decoupling controller) Applying the following control law

U = C—1AdX(f—^(X©-11w + AdX(Z)ur

—AdX(f)d — Ar(ar)a (28)

where w e Rn is a new input, to Eq. 24 gives fundamental decoupled linear system

Z = w.

Using Eq. 28 with an appropriative stabilizing feedback w = Z(z) one can ensure that limi —z(t) = e. For example one can select w := -Kz, where K e R"x" denotes a positive definite gain matrix.

2.5 Augmented Dynamics

Application of TFs for control purposes introduces an additional dynamics governing evolution of extended state variable a. To be more specific, we take into account that

I -AiA-1 0 -A. '

and define

^c (a,ar) := A—\a)Ad%—mf—1 (a,ar)), (31) where

AdX—m(f—1 (a, ar)) := [0 I] AdX(f—1 (a,ar))

denotes last n — m rows of matrix AdX. Recalling definition of H, using Eqs. 30 and 32 one can rewrite part of Eq. 28 defining the augmented dynamics as follows

a = (a, ar)(X(Z)—1w + AdX(Z)ur

—AdX(f(a, ar))d — Ar(ar)ar) . (33)

Time evolution and stability of Eq. 33 is essential for control. In particular, it determines the way how instantaneous directions in the phase space are approximated by kinematics (6). We consider this issue more thoroughly by investigating dynamics (33) when auxiliary tracking error Z becomes negligible. Then, noticing that w = 0 and AdX(Z,)\~Z=e = I one can consider the following zero dynamics on manifold Z = e

a = — £2a (a, ar) (ur — AdX(f)d — Ar(ar)ar) (34)

It can be concluded that when system (24) evolves along with kinematic constraints imposed on Eq. 6 the motion approximation is no longer necessary. In such a case it should be expected that zero dynamics (34) tends to an equilibrium. However, such a possibility is conditional and it is dependent on terms ur, d, ar and the assumed form of transverse function f.

Latter on, in order to simplify considerations, we investigate the nominal condition assuming that drift d is negligible and the reference trajectory is admissible, namely uri = 0 for i = m + 1,...,n. Next, transverse function f is defined by Eq. 20, such that Wa e Tn—m, f (a) = f (ar)\ar =a. Accordingly, defining

a ar a

one concludes that

(a ^ 0) ^ (f ^ e).

Computing time derivative of Eq. 35 and using Eq. 34 with d = 0 allows one to define the following dynamics

a = fia(a, ar)ur + (I — fia(a, ar)Ar(ar))ar. (37)

In order to investigate (37) in a neighbourhood of a = 0 one computes

lim ^a(a, ar) ^ lim A—1(ar — a)

*ÄdX_m(f 1 (ar -a, ar))

= Ä-l(ar)ÄdX_m(e)

= Ä-l(ar)[0 I]ÄdX(e) = Ä-l(ar)[0 I

Further, taking into account (38) it can be shown that

I - &a(a, ar)Är(ar)\a=0 = 0.

Similarly, recalling that ur is the reference input defining an admissible trajectory one gets: ^a(a, ar)Ar(ar)\a=0 ur = 0. Consequently, a = 0 is the equilibrium point of dynamics (37).

To simplify analysis, it is assumed that ur is a slow time-varying function and ar = 0. Next, we consider the Taylor's linear approximation of Eq. 37 with ar = 0 and postulate that there exists a* e Tn—m such that

Ùa(ur) = —Qa(ar — â, ar)ur e R(n-mXn-m) (40)

da ä=0,ar =a*

becomes the Hurwitz matrix. This guarantees that dynamics (37) with ar = 0 is locally asymptotically stable at a = 0. Following the concept discussed in [17] we propose to chose ar = a* based on reference signal ur .In order to ensure a continuous transition of ar the following simple adaptation rule can be formulated

àr = -kal (ar - a*),

where ka1 >

0 and a* is dependent on ur. In the case when a* is constant one can easily show that ar exponentially converges to a* with convergence rate dependent on kai. Taking into account result (39) and noticing that ar ^ 0 one concludes that the second term in Eq. 37 can be regarded as a vanishing perturbation. As a result a tends to 0, at least locally. Recalling Eq. 40 one can find that eigenvalues of are linearly scaled by input ur. Hence, the convergence rate of a is dependent on reference motion (cf. [17]).

In the case when ur = 0, namely the point stabilization is considered, the reference trajectory cannot be used to maintain asymptotic stability of dynamics (37). To be more specific, this dynamics is then reduced as follows

a = (I - &a(a, ar)Är(ar))ar.

In order to stabilize (42) using auxiliary input ar one can consider the following preliminary proposition.

Proposition 3 (Basic stabilizer of augmented dynamics) Applying the following rule

à r = -ka2 (I - &a(a, ar)Är(ar)) â

with ka2 > 0 being a positive constant, to dynamics (42) ensures that a asymptotically converges to the following set

ra = {<5 e Tn—m : (I — i2a(ar — a,ar)Ar(ar))a= 0} .

Proof Consider the following positive definite function Va = 1 a^a. Using Eq. 43 in Eq. 42 gives

a = —kaP(ar,a )P T(ar,a )a, (45)

P(ar, a) := I — (ar — a, ar) Ar(ar). (46)

Next, computing time derivative of V and taking advantage of Eq. 45 one obtains

V = -kaaT P(ar,a )P T(ar,a )a.

Since P(ar,a)PT(ar,a) is the positive semi-definite matrix it follows that Va < 0. Hence Va tends to critical points such that Va = 0. Clearly, from Eq. 47 one has: Va = 0 for He Ta. □

Remark 2 The considered proposition can be seen as a preliminary idea which could be modified for a particular system. Specifically, it is not guaranteed that a converges do zero as a result of critical points which are dependent on matrix P(ar,a). One of possible solutions is to relax asymptotic stability requirement and to select a transverse function to ensure that P(ar,a) is the full rank matrix. Another proposition of asymptotic stabilization can be formulated for chained systems, [14].

Remark 3 It is worth noting that in the case of tracking of an admissible trajectory the adaptation algorithm based on Proposition 3 does not ensure that ar goes to a*. As a result the dynamics (37) may become locally unstable when ur = 0. To overcome this issue, one can combine two approaches assuming that ka1 goes to zero when ur ^ 0, while ka2 ^ 0 for a higher reference input. Introducing a continuous non-decreasing switching function p such that: V^ e [0,f1), p(^) = 0, V^ e [&, <x>), pg) = 1, and V^ > 0, § > 0, while 0 < < f2, one can consider the following proposition:

ar = -p(\\ur W)ka1 (ar - a*)

-(1 - p(\\ur W)ka2 P(ar, a )a.

Using this adaptation law one can continuously (or even smoothly) switch stabilization strategies specifically defined for the regulation and tracking case. Simultaneously, it is guaranteed that error Z is not affected. Since Z = gf—1 it follows that tracking (regulation) error g is bounded even if the augmented dynamics is not asymptotically stable. Concurrently, when a ^ 0 it follows that g ^ e.

3 Control Design for a Differentially Driven Robot

3.1 Vehicle Kinematics

Consider a planar robot equipped with a differentially drive mechanism illustrated in Fig. 1. It is assumed that wheels on left- and right-hand side of the vehicle are mechanically coupled and their angular velocities, ml and mr , respectively, are controlled by two independent motorized servos. Consequently, um :=

can be regarded as a kinematic input

of the system.

Let g := [x y 0 ]T e R2 x S1 be configuration of the vehicle body, where x, y stand for position coordinates and 0 denotes the vehicle orientation determined in the inertial frame. Assume that v = \vx Vy\ e R2 and w e R are linear and angular velocities, respectively, expressed in the local frame fixed to the robot body. It is well known that for a wheeled skid-steering locomotion velocity transmission between wheels and a moving platform can be significantly affected by slip phenomena. In order to cope with this issue, firstly we introduce a nominal kinematic drive model based on the symmetric two-wheeled nonholonomic robot, which is defined by u := [vx w1^ = Wuw, where W e R2x2 is a matrix with constant coefficients dependent geometric parameters (wheels radius and the distance between the wheels). Secondly, we introduce auxiliary functions aw and av determining slip quantities and consider the following skid-steering drive kinematics

u = H(av, am)Wua

Fig. 1 Kinematics of differentially driven skid-steering robot

where H(av, am) := diag [a aai e R .

It is important to note that lateral velocity vy is related to a skid phenomenon which cannot be modelled explicitly based on a pure kinematic description. The similar remark concerns also slip functions av and aa which are governed by the vehicle dynamics including wheel-ground interaction forces. Following this property, it can be stated that a skid-steering kinematics cannot be globally defined without modelling of a slip/skid dynamics, [8, 18].

However, in spite of this issue, one can consider an approximated kinematic model of such a vehicle assuming that the slip dynamics is neglected. This model is local by nature and it can be justified when the following conditions are met (cf. [3]): mass distribution of the vehicle is uniform, the local frame is fixed at centre of mass of the vehicle (cf. Fig. 1) and the vehicle moves with relatively low velocities and limited accelerations (the latter assumption can be interpreted as the condition of stable and almost stationary motion). Following this simplification, it is assumed that:

- slip functions av and aa are known and bounded,

- lateral slip effect is small - as a result Wt, supt | vy (t) | < Vy, where Vy is a positive small constant.

The kinematics of the considered vehicle can be written in standard form (6) with n = 3 and m = 2, where

X(g) = [X1 X2 X3](g) =

cos 0 0 sin 0 sin 0 0 - cos 0 0 1 0

u = [vx wy e R2 denotes input and d = [0 0 - 1]Tv,

is the drift term which is considered as an exogenous disturbance (notice that vy is a response of the robot lateral dynamics for the given velocities vx and a, and constrained forces exerted by the wheels). It is well known that the considered kinematics can be described on Lie group G = SE(2) with neutral element e = [0 0 0]T and group operation given by

where R (0) e SO (2) is the planar rotation matrix and g, h e G. The inverse element of g e G satisfies

RT (0) 0 0 1

Further, operator ÄdX (g) can be computed as fol-

ÄdX(g) =

cos 0 y sin 0 0 1 0 - sin 0 x cos 0

Next, using Eq. 49 in Eq. 6 one obtains the following equivalent control system

g = X(g)(CH(av, am)Wum + d).

From Eq. 55 it can be concluded that kinematics of differentially driven robot in the presence of limited slip/skid phenomena robot can be treated as a

perturbed kinematics of the nominal symmetric two-wheeled nonholonomic vehicle. We take advantage of this assumption for the controller design.

3.2 Task Description

In the sequel, we will develop motion control and planning algorithms dedicated for the waypoint following motion task. It is assumed that the sequence of prescribed waypoint configurations to be realized by the robot is generated by a high level planner or human operator. We are focused on a feedback control strategy driving the robot through subsequent waypoints.

We propose a two-stage algorithm composed of a universal closed-loop stabilizer and a trajectory planner utilizing of polynomial splines. At the feedback control level tracking and stabilization problems must be considered while issues of acceleration/velocity constraints and skid/slip limiting are considered during the trajectory planning stage. Additionally, we take into account infeasible reference segments taking advantage the motion controller using TFs.

3.3 Motion Controller

3.3.1 Transverse Function and Augmented Dynamics

gh := g +

R (0) 0 0 1

The tracking controller is designed based on proposition discussed in Section 2.4. The transverse function

for system (5) with basis (50) can be selected as follows (cf. [13])

f(a) =

£1 sin a 1 £1£2sin2a £2 cos a

with £1 e R+ and s2 e (0,n). Simultaneously we define a similar component

fr(ar) =

£r1 sin ar 4£1r£2r sin2ar £2r cos ar

eG, (57)

where £r1 and £r2 e R. To simplify notation we introduce coefficients k£1 and k£2 e R such that £r1 = K£1 £1 and £r2 = K£2 £2.

The basic terms associated with derivatives of f (a) and fr(ar), respectively, are given as follows

A(a) =

cos fe'f + sin fef f

sin fd-â - cos fôia

Ar(ar) =

cos fre'f + sin fre'f sin fre'dt - cos fredf

Next, computing the translated transverse function f = fx fy fe]T using components (56)-(57) and formula (20) with Eqs. 52 and 53 one obtains

fx(a,ar) = s\ (cos(KS2£2cosar)(sina — kS1 sinar)

+ 4 £2 sin(KS2 £2 cos ar)(sin2a — kS1 kS2 sin2ar)^ ,

fy(a,ar) = £1 (— sin(KS2 S2 cos ar)(sin a — kS1 sin ar)

+ 4£2cos(KS2£2 cosar)(sin2a — kS1 kS2 sin2ar)^ ,

fe(a,ar) = £2 (cosa — kS2 cosar}. (62)

that selected nonlinear terms can be approximated as follows

Va e S1, cos fe ^ cos(s2cos a) « 1 and sin fe

(56) ~

= sin(s2cos a) « s2

cos a = fe. (63)

Similarly, since kS2 < 1 one has

Var e S1, cos fe ~ 1 and sin fe ~ fe. (64)

Using these approximations in Eqs. 58, 59, and 6062 allows one to simplify considerably formal analysis of properties of the tracking controller defined by Eq. 28.

3.3.2 Stability ofAugmented Dynamics

Now we investigate asymptotic stabilization along admissible persistently exciting reference trajectories. In order to do this we assume that kS1 = kS2 = 1 and consider given by Eq. 40. e R. We search for a feasible selection of a* for which dynamics (37) is locally asymptotically stable. Making detailed analysis (cf. Appendix B.1 - Eq. 126) one can show that when s2 is small enough becomes

(ur 1S2 sin ar + ur2S1 cos ar)

£1 £2

2^J + ur2£2

£1 £2

cos(ar - ç),

where ç := atan2( —£2ur 1, —£1ur 2) while atan2 (■, ■) stands for the four-quadrant inverse tangent function. From Eq. 65 it follows that < 0 for ar = a* selected such that cos(a* — ç) > 0. Moreover, one can introduce optimal value of a* computed in order to minimize . Clearly, \ar achieves minimum at

ar = a*pt, where

a*pt = atan2(-£2ur1, -£1ur2).

Remark 4 The transverse function described by Eqs. 60-62 is well defined even for large values of S2 (recalling that this parameter linearly scales component fe e S1 in practice it is not expected to select s2 higher than n). However, when parameters s2 < S, where S > 0 is a positive constant, is made small enough, based on Eqs. 56 and 57 one can conclude

In order to illustrate a feasible selection of a* in a more general case (without assumption that s2 is small) a numerical analysis has been conducted assuming that ur 1 = cos j and ur2 = sin j, where j e (—n,n]. The results presented in Fig. 2 confirms that the simplified formula (66) can be applied even for a relatively high value of s2. In such a case the feasible set of parameter a* is reduced when j

Fig. 2 Interpretation of local stability of dynamics (37). Selected ratio s\/s2: 0.1 (the left column), 1 (the middle column), 10 (the right column). Gray areas: feasible sets of parameter a*, thick blue line: a*opt computed from Eqs. 66, thick dashed red line: a*pt computed numerically

Parameter 62 = 0.1

a" 0 -1 -2 -3

Parameter £2 = 1.5

approaches ± f (namely, for the reference motion with almost zero longitudinal velocity, ur 1 ~ 0).

Next, we consider stability of dynamics (37) for the regulation case when ur — 0. Based on Proposition 3 we design a stabilizing feedback in order to attenuate regulation error ga without decreasing values of parameters e\ and e2 of the transverse function.

Proposition 4 Let

a r = ka2 a,

where ka2 > 0 is a positive coefficient, be the adaptation rule. Applying Eq. 67 to Eq. 37 for the considered unicycle-like system ensures that a = 0 is locally stable equilibrium point for ur = 0.

Proof Recalling Eq. 42 in the considered case and substituting Eq. 67 yields in

a = -ka1 P(ar, a)a,

where P(ar, a) = (1 — Qa(ar — a, ar)Ar(ar)) e R. In order to determine stability of dynamics (67) one can consider sign of P in some vicinity of aa = 0. From Eq. 36 it is clear that when translated transverse function f is chosen in a typical way, namely Ke1 = kS2 = 1, it converges to neutral element when a ^ 0.

In such a case it is not sufficient to apply linear tools to analyse dynamics (67).

However, assuming that at least one coefficient Ke. (i = 1, 2) is different from 1, P can converge to non-zero value at a = 0. Recalling Eqs. 31 and 32 in the considered case one gets: P(ar, 0) = 1 — A—1(ar)AdX(f—1 \a=o)Ar(ar). Correspondingly, it can be proved (the details are given in Appendix B.2 - Eq. 134) that for e2 < e, where e > 0 is a positive

constant, Kei e relation is met

[0,1] and ks, ks

Var e S1, P(ar, 0) > 0.

1 the following

Thus one easily concludes that dynamics (68) is locally exponentially stable at aa = 0.

A more complicated case is found when Kei = 1 (i = 1, 2) and P(ar, 0) = 0. Then a higher order approximation of term P(ar, a) at neighbourhood of a = 0 can be studied. Recalling that for e2 < e, A2 « 2£i£2 - cf. Eq. 120 - one can write

P(ar,a)\a=0 = 1 - ^ (ÄdXif-1)\a

+da ÄdX(f-h)\ât=0 aa +2 da2 ÄdX(f ~l)\à=0 a2)

^Är(ar) + O(â3)

Computing terms in Eq. 70 thoroughly (cf. Appendix B.3) one obtains relations Eqs. 137 and 138. Hence, formula (70) can be simplified as follows

P(ar,a )\a=0 = —ek J2 AdX(f—1)\a=0Ar(ar)&2

(1=8) (1 — ie|(2sin4 ar +sin2 ar — 1))a2. (71)

Using Eq. 71 in Eq. 68 gives a = —ka1 — 1 e2(2sin4 ar + sin2 ar — 1)^ a3,

It can be shown that for e2 < 1, War e S1, 1 — 1 e2(2 sin4 ar + sin2 ar — 1) > 0. Thus, it yields that a = 0 is the locally asymptotically stable equilibrium point. □

Remark 5 The convergence rate ensured by the stabilizer formulated in Proposition 4 is dependent on selection of parameters kS1 and kS2. In the case when asymptotic stabilization is considered, namely Ke1 = Ke2 = 1, the convergence rate is polynomial. Thus one has to expect that regulation errors tend to zero slowly. Otherwise, at least locally, the convergence is exponential and regulation time can be reduced significantly.

Remark 6 In order to obtain the proper stabilization of the augmented dynamics in the regulation and tracking cases one can consider the hybrid tuning outlined in Remark 3. To be more specific formula (48) can be rewritten as follows

ar = —p(\\ur \\)ka1 (ar —a*) — (1—p(\\ur \\)ka2a, (73)

with p being the continuous switching function discussed formerly.

For simplicity's sake, we consider the trajectory planner assuming that the reference lateral skid is zero, namely the reference motion is defined by Eq. 25 with ur3 = 0. Then, any slip affecting the vehicle motion is treated as a bounded disturbance which should be partially compensated by the closed-loop controller as well as by imposing bounds on desired velocities.

To generate an admissible trajectory while reducing the initial tracking error a simple local planner, which utilizes polynomial splines and takes into account prescribed velocity and acceleration bounds is proposed. The planning process consists of executing the same procedure for every subsequent pair of waypoints, beginning from the initial robot configuration, which is treated as the zeroth waypoint. This procedure is now described.

Assume that subsequent waypoint configurations are denoted by gd(k~\) and gd(k) e G, while Tk is the planned time of reaching kth waypoint. Trajectory gr(t) e G must be planned to satisfy the following conditions:

A1 connectivity: gr (0) = qd(k~1), gr (Tk) = qd(k),

where Tk < to is a bounded time period A2 admissibility: Wt e [0, Tk], gr(t) is a solution

of Eq. 25 with ur3 = 0 A3 boundedness of reference input ur: it is assumed that Wt > 0, \vrx\ < U\ and \ar\ < U2, where Ui and U2 are positive bounds A4 continuity of reference input: Wt e [0, Tk], ur1 and ur2 e C1

In order to satisfy above conditions trajectory gr (t) is designed in two stages. Firstly, a suitable path qd (s) taking into account assumptions A1 and A2 is found. Secondly, a time scaling procedure is used to determine path parametrization s (t) e [0, 1].

3.4.1 Path Design

3.4 Motion Planning Algorithm

The considered universal controller is designed to solve the tracking problem for any bounded reference trajectory with continuous input ur. However, in the given application it is assumed that efficient and natural motions are first employed. Hence, purely non admissible reference path/trajectory segments can be used conditionally when a specific kind of motion is required.

Let gd be an auxiliary configuration on the Lie group G considered between two points:

gd = g—(k)gd(k-\). (74)

Next, define a path gd (s) parametrized by s e [0, 1] such that

gd (0) = qd and qd (1) = e. (75)

gd(s) = gd(k)gd (s). (76)

connects subsequent points gd(k—1) and gd(k). To simplify the notation we define

gd (s) ■= p>d (s) Qd (s)

where pd(s) := [xd (s) yd (s)] denotes position variables.

Path connecting subsequent waypoints is constructed base on a 5th-order polynomial y (s,a) = j25=o ais', where a = [ao a1 ... a5]T e R6 denotes coefficients determining its shape. The first and second order derivatives of y (s,a) are denoted by Y'(s, a) and y" (s, a), respectively. In the considered planning method it is assumed that the position path satisfies: pd (s) := \y(s,ax) y (s, ay)~\T, where ax and ay e R6 are parameters, which can be computed for a given set of boundary conditions. As a result of Eq. 75 and assumption A1 we have:

pd (0) = [xd yd ]T

and pd (1) = 0.

Considering assumption A2 and Fig. 3 one can conclude that a vector tangent to the path pd(s) at s = 0 and s = 1 should be properly oriented in order to meet the following tangent conditions

Pd (0) = V cos t

V0 sin Qd\T

and p'd (1) = [vf 0T , (79)

segment tends to zero at waypoint configurations. This assumption can be represented as follows:

p'd (0) = p'd (1) = 0.

Taking into account (78)-(80) one can compute

polynomial parameters as follows: axT ayT =

S—1Y, where S e R12x12 is the constant invert-ible regression matrix, while Y := [p~T (0) p~T

(1) pT(0) pT(1) 0]T e R12. Having polynomial parameters one can obtain pd (s) for s e [0,1] and compute other path-related variables.

Recalling admissibility condition for the path, the orientation variable and auxiliary inputs values along the path can be computed using differential flatness. Consequently, the orientation satisfies

0d (s) = atan2 (ßyd (s), fzx'd (s)),

where ^ e {—1, 1} is a parameter defining motion strategy (forward \x := 1 or backward \x := — 1). Tangent velocity along the path is given as

Vd(s) =\p'd (s)|| , (82)

while nominal angular velocity satisfies

Md (s) = y (s) x'd (s) — x"d (s) y'd (s)) /vl (s), (83)

where Vn = 0.

The path planning process ends by computing path coordinates in the given inertial frame referring to Eq. 76.

3.4.2 Time Parametrization

with v0 and vf being positive tuning parameters.

In order to guarantee continuity of the reference angular velocity, the path curvature should be properly shaped. In order to facilitate construction of a complex path consisting with more than one segment it is assumed that the curvature of a local path

Fig. 3 A path segment connecting two waypoints defined in the local coordinate frame

J0d(s) -**

The velocity profile is designed taking into account tangent velocity on the path pd(s). It is assumed that nominal velocity vn changes monotonically in the assumed time horizon t > 0 and satisfies

V" (t) — vn (0)

t e [0, T] vn (t) ■=

+ Vn (0),

0- cos( TO)

where vn (0) < U1 and vn(r ) < U1 are desired positive values - cf. Fig. 4. Integrating Eq. 84 one can calculate the following distance

1 r „W, (vn(T) + Vn(0) It = I Vn (t) dt = T

Equation 85 can be used to find time t for the assumed path length. It gives possibility to recognize path

Fig. 4 Velocity profile proposed for trajectory planning

fragments where the reference vehicle should start or stop gradually. From Eq. 85 one has

Vn (T) + Vn (0)' Let ts > 0 be an increasing function such that

dts dt

0. In order to calculate s(ts) we compare nominal velocity (84) with tangent velocity (82) and introduce the following scaling function (ts) := ""fy). For

vd (s (ts ))

the given vn (ts) we scale value of angular velocity given by Eq. 83 and check if the following inequality holds (cf. assumption A4): ^fe) \ad (s (ts))\ < U2. Correspondingly, we define

\ad(s(ts))\

H(h) := ^i(ts)-

In order to ensure a velocity saturation in a smooth way we introduce the following formula

&(ts) := h(ts)/tanhfefe)).

and calculate ts(t) = f^- 1 (Z)dç, and s (ts) = ¡0 jrjjjdç. Finally, the reference configuration is given by gr(t) := gd(s(ts(t))) while the reference velocities become

Vr (t) = ß

Vn (ts(t))

Ï3 (ts(t)y

Wr (t) =

(ts(t))wd(s (ts(t)))

H3 (ts(t))

Recall that parameter \x in Eq. 88 is used to determine the sign of linear velocity.

Remark 7 As a result of the smooth saturation introduced by Eq. 87 variable f3 > 1 for any > 0. Thus recalling (88) the given scaling procedure provides that \vr\ is less than vd for any non zero angular velocity ar. Consequently \vr \ can achieve vd only on a straight segment. This algorithm provides a more conservative generation of vr than in the case of a

typical non-smooth scaling used for example in [19]. Simultaneously, it guarantees a smoother transition of reference signals.

3.4.3 Extension to Non-Admissible Case

In spite of preferring almost admissible reference segments there are motions tasks where approximation of non-feasible directions could be applied with a higher priority. Thus, the considered motion planner should be able to generate also infeasible segments. For simplicity reasons, we investigate a linear interpolation between the successive waypoints in terms of position and orientation. Then it is assumed that velocity vr computed on the given path is profiled in order to avoid rapid transition between the segments. This velocity is propagated on directions defined by a reference frame which are not subjected to nonholonomic constraints. As a result the planner generates independent components vrx, vry and ar which are used to determine input ur.

4 Experimental Studies

The presented control system has been implemented in C++ language using KSISframework and ROS Hydro middlewares. The skid-steering platform RoKSIS used in experiments is presented in Fig. 5. The robot is driven by two Maxon motors governed by EPOS2 drivers connected to the on-board PC via

Fig. 5 Experimental four-wheeled SSMR robot RoKSIS with wheelbase width of 0.405 m and axes within 0.52 m

CAN bus. The vehicle is localized using the high accuracy vision system Optitrack consisting of ten cameras Prime 13W, [1]. To improve robustness of the localization system and overcome wireless communication drop-outs a sensory fusion between data provided by cameras and an internal predictor supported by a gyroscope is realized. The whole system was integrated using ROS complemented by a specialized KSIS software framework providing: automatically verified system composition, building blocks for motion control/planning algorithm implementations and push-based low-latency communication between critical controller components (see [5] for details).

Kinematic model (49) has been identified experimentally. Vehicle motion has been analysed in stationary conditions (assuming that wheels velocities are describedby slow time-varying functions) along trajectories with various curvatures. Based on this analysis it was noticed that the slip functions are almost constant parameters given by: av = 1.1 and am = 0.66. Moreover, for considered paths lateral velocity vy approaches zero which indicates that the disturbance term in Eq. 51 is not significant. The given kinematic slip model and measurement data from the gyroscope sensor are used to update input matrix H present in model (49). Then is possible to partially compensate disturbances coming from non extensive longitudinal slip and to control the robot at kinematics level.

To investigate performance of the presented algorithm experimental tests were conducted extensively. In this section only selected results are presented.

The task studied in experiments E1 and E2 well corresponds to a typical motion scenario, namely the robot is supposed to move through a set of eight way-points along with almost admissible segments computed by the local planner. The detailed parameters

of the nominal motion plan is collected in Table 1. The reference motion on each segment is described by parameters Vd and Vm corresponding to nominal velocity at the given waypoint and nominal velocity in the middle of the preceding segment, respectively. At the beginning of each segment the reference motion is re-planned based on current state of the vehicle including its posture and linear velocity. Parameters v0 and vf of the planner are selected based on Euclidean distance between successive waypoints as follows: v0 = vf = 1.5||pd(0)||, where pd is defined by Eq. 77. The upper bound of the angular reference velocity is limited to 0.6 rad/s.

Gains of the motion controller Eq. 28 with Eq. 73 have been chosen as follows: K = —I, e1 = 0.3, s2 = 0.3, kSi = kS2 = 1, kai = 2, ka2 = 1 and a* is computed based on relation (66). Frequency of the control loop has been set to fs = 100 Hz.

In experiment E1 only forward reference motion is planned while in experiment E2 there are two segments with backward motion imposed. Reference and obtained position paths together with waypoints are presented in Figs. 6a and 7a. The vehicle starts from initial waypoint marked by P0. Additionally, in brackets time of reaching the corresponding way-point is given. Basically, it can be observed that the robot path resembles the reference path closely especially when the reference motion is properly planned (in the sense that reference velocities are properly selected to the given path curvature). It turns out that the most difficult segment is planned between way-points P4 and P5 where the reference linear velocity is increased significantly along the curve. It can be seen that in both experiments there is a noticeable deviation from a reference motion along the considered segment.

Table 1 Motion nominal scenario considered in experiments E1 and E2

Waypoint Xd [m] yd [m] Od [rad] Vd Vm Motion strategy ¡i

Pi 0 0 0 0 0.2 Ex. 1: +, Ex. 2: +

P2 1 0 n/2 0.4 0.4 Ex. 1: +, Ex. 2: -

P3 2.5 0 n/2 0 0.2 Ex. i: +, Ex. 2: -

P4 2 0.5 3/4n 0.4 0.4 Ex. i: +, Ex. 2: +

P5 1 1 n 0.8 0.8 Ex. i: +, Ex. 2: +

P6 2 0.5 n 0.4 0.4 Ex. i: +, Ex. 2: +

P7 2 0.5 n 0.4 0.4 Ex. i: +, Ex. 2: +

P8 2 0.5 n/2 0.4 0.4 Ex. i: +, Ex. 2: +

Fig. 6 Results of experiment E1

2,[m] ;(57.8) ■ (56.7) : (55.4) !

; f 'b :........k..j........j......... P5 »4 (52.8)

ïo) (62.1); ; /'„;, 1 ; (i9.3): x [ml 1 1

i i i i ^30.7) 8) ,

(a) Waypoints, position paths: measured (solid line), reference (dashed line). In brackets time values are given.

50 25 0 -25 -50

40 t[;

0.2 0.1 0

-0.1 -0.2

JiJ.Wjf^

40 t [s]

(b) Position tracing error: x (solid (c) Orientation tracking error 0 in [rad] line), y (dashed line) in [mm]

A .—! \ 'A ^.J ------i-J

40 t [si

0.05 0

-0.05 -0.1 -0.15

40 t [si

(d) Internal variables: a (solid line), ar (e) Estimated lateral velocity vy in [m/s] (dashed line)

-0.5 -1

40 t [s]

(f) Linear reference velocity vr in [m/s] (g) Angular reference velocity

[rad/s]

0.4 0.2 0 -0.2 -0.4

40 t [si

(h) Linear velocity tracking error vx — vr (i) Angular velocity tracking error uj—ujr in [m/s] m ' " ' '

From Figs. 6b, c and 7b, c it follows that the position tracking errors do not exceed 60 mm while the orientation tracking error becomes less than 8.6 deg. Clearly, there is a correspondence between the tracking errors and velocities presented in Figs. 6e-g and 7e-g. In particular, a rapid change of the errors can be noticed when lateral velocity increases. This issue can be interpreted as a response of the closed-loop system to a significant kinematic disturbance from the skid

dynamics. It can be observed that during execution of segment P4 — P5 first the robot starts skidding and then orientation error increasing. The similar effect is not seen along segment P7 — P8 as a result of smaller value of the reference linear velocity imposed in the planning phase.

Taking into account Figs. 6f-g and 7f-g one can observe a continuous transition of the reference velocities between the successive segments. These signals

Fig. 7 Results of experiment E2

y |m| |(57.8) |(56.7) ;

(a) Waypoints, position paths: measured (solid line), reference (dashed line). In brackets time values are given.

(b) Position tracing error: x (solid (c) Orientation tracking error § in [rad] line), y (dashed line) in [mm]

(d) Internal tables: a (solid line), ar (e) Estimated lateral velocity vv in [m/s] (dashed line)

(f) Linear reference velocity vr in [m/s] (g) Angular reference velocity ujr in

[rad/s]

0.4 0.2 0 -0.2 -0.4

(h) Linear velocity tracking error vx —vr (i) Angular velocity tracking error oj—uir

in [m/s]

in [rad/s]

are profiled in order to satisfy the assumed constraints. In particular, it can be noticed that value of linear velocity decreases when value of angular becomes significant. This is due to the continuous scaling introduced by Eq. 88. Analysing Figs. 6h-i and 7h-i it can be concluded that velocities of the robot, estimated based on the posture measurements, correspond to reference values that confirms

an acceptable tracking precision and show that the impact of longitudinal and angular slips is properly compensated (at least when the motion conditions are in the safe region, when there slip-skid dynamics is stable).

Basically, the performance of the controller is not dependent on selected motion strategy (forward/backward) assuming a continuous transition

between segments. It is worth noting that this property is not obvious when a controller based on TFs is used. This is due to augmented dynamics and its stability which is investigated in Section 3.3.2. However, in the given experiments the desired value of ar is computed based on reference signals using formula (67). As a result the augmented dynamics evolves near asymptotically stable equilibrium point. Actually, from Figs. 6d and 7d it can be observed that a ~ 0 while ar changes its value according to the desired motion. Consequently, no oscillatory response of the controller is observed and the considered motion task is realized with a quite high precision and robustness (notice that the selected values of parameters e\ and e2 are not small - anyway norm of f approaches zero as a result of the stability of the augmented dynamics).

In order to illustrate performance of the controller in a neighbourhood of the constant point the kinematic controller is not switched-off at stop waypoint P8. It can be seen from Fig. 6b, c that at the final control stage when reference signal ur generated by the planner goes to zero some residual configuration error exists. As indicated from Fig. 8a, b the proposed adaptation rule attenuates this error asymptotically. However, the convergence rate is only polynomial which makes regulation time to be significant. It can be noticed from Fig. 8d that the closed-loop system

evolves on the zero dynamics defined at z = e. Auxiliary error a tends to zero while ar is increasing function generated by the integral adaptive rule (67). In such a case the steady state cannot be achieved in practice. The robot makes slight manoeuvres at the desired point (see control inputs depicted in Fig. 8e, f) similarly as it can be seen for others asymptotic smooth stabilizers, cf. [14, 21, 22].

In the next experiment, E3, non admissible reference path is designed from initial waypoint P0 to final waypoint P1. It is assumed that maximum velocity on the path is restricted to 0.1 m/s. In this case the parameter k81 is changed from 1 to 2/3, hence no asymptotic stability is ensured. Other parameters of the controller are selected as in experiments E1 and E2. Taking into account results presented in Fig. 9 one can notice that the robot motion along the reference linear segment is executed employing an approximation of infeasible directions (mainly in the lateral direction). The tracking errors - cf. Fig. 9a, b - are bounded according to the assumed parameters of the transverse function. It is interesting to analyse evolution of a and ar illustrated in Fig. 9d. It can be seen that when the reference motion stops a converges to zero. Simultaneously orientation error approaches zero that well corresponds to the given stability proof (recall that e2 = e2r which indicates that fe = 0 for a = 0). However, recalling

Fig. 8 Results of experiment E1 - the stabilization phase

100 150 i[s] 100 150 i[s]

(a) Position regulation error: x (solid (b) Orientation regulation error 6 in [rad] line), y (dashed line) in [mm]

0.004 0.002 0

t [s] 100 150 t [s]

(c) Internal variables: a (solid line), ar (d) Norm of auxiliary tracking error ||S| (dashed line)

0.01 0.005 0

-0.005 -0.01

0.04 0.02 0

-0.02 -0.04

(e) Estimated line^ velocity vx in [m/s] (f) Measured angular velocity to in [rad/s]

Fig. 9 Results of experiment E3

r ,! : : 2/[ml: : : ;

Pi K----yS N.

: : : x [m] 1 1

-1 -0.8 -0.6 -0.4 -0.2 0 0.2 0.4

(a) Waypoints, position paths: measured (solid line), reference (dashed line).

j i 1 1 1 i

V / "V

0 20 40 60 80 t [s]

(b) Position tracking error: x (solid line), (c) Orientation tracking error § in [rad] y (dashed line) in [mm]

.»¡. a!........

0.2 0.15 0.1 0.05 0

(d) Intern^ wiables: a (solid line), ar (e) Norm of auxiliary tracking error ||z|| (dashed line)

0.2 0 -0.2

20 40 60 80

-0.5 -1

•A ! : :

i ■ :

20 40 60 80

(f) Linear velocity: vx (solid line), vrx (g) Angular velocity: u (solid line), ur (dashed line), vry (dotted line) in [m/s] (dashed line) in [rad/s]

that s2 = n/3 one cannot longer assume that formulas (63) and (64) are justified. In such a case it is not guaranteed that P(ar, 0) in Eq. 70 is positive for any ar. Correspondingly, a = 0 may be no asymptotically stable equilibrium point for any ar. This case is met in the given experiment - it can be observed that a increases at 80th sec and after that it converges to zero while ar tends to some constant. Surprisingly, position errors are also well attenuated - cf. Fig. 9, however this effect results from a very specific case but not a general property (notice that for k81 < 1 one cannot guarantee that fx and fy ^ 0).

Comparing the stabilization phases in E1 and E3 it is clear that practical stabilization at the constant point

using the same adaptation method allows one to obtain the steady state stage. Hence, the regulation time is limited considerably.

5 Conclusions

In this paper the control solution for the waypoint following problem is designed for the differentially driven vehicle (including the skid-steering structure). In order to cope with constraints imposed on robot velocities and accelerations the considered method takes advantage of the motion planner and the closed-loop universal stabilizer.

The motion controller is based on the transverse functions approach and is responsible for control of the vehicle in the presence of nonholonomic constraints. In comparison to previous works in this paper much importance is attached to asymptotic tracking and stabilization which are required for an efficient execution of typical motion tasks (cf. [19]). The asymptotic convergence in the nominal (unperturbed) case is ensured by a proper selection of internal variable in order to stabilize the zero dynamics. The given proposition of adaptation rules are formally proved. It is shown that the proposed translated TFs allow one to freely tune the controller which increases a design flexibility. Basically, it is possible to come from practical to asymptotic stabilization without violating structure of the controller or the transverse function. Additionally, the controller is able to generate oscillatory manoeuvres which can be useful for example to execute the parallel parking task while ensuring the desired posture precision at the end.

The given control structure defined at kinematic level can be effectively implemented in practice. In spite of simplifications which are made to describe the vehicle model using the wheeled skid-steering locomotion, based on experimental results it can be stated that the proposed kinematic control approach is justified for motion control in a limited range of velocities when the reference motion is properly designed. Simultaneously, for the given application the proposed integration of a real time planner with a closed-loop controller seems to be crucial. This structure allows one to include additional operational constraints induced from the vehicle dynamics which are not formally covered by the feedback motion controller.

The conducted experiments once more indicate that a good performance of motion control requires a suitable precise and low-latency measurement system supported by estimation techniques. This is an in important issue in mobile robotics in comparison to manipulation robotics where motion in the task space can be typically estimated based on internal configuration variables. It is worth mentioning that currently new set of sensors like Optitrack vision system give possibility to improve control quality noticeably. Additionally, the considered control strategy is scalable in the sense that the controller can be specifically adjusted in order to relax accuracy requirement.

It allows one to apply this method also when lower precise localization measurements are available (cf. results reported in [19]).

In future one can consider application of the proposed control method for car-like robots. In such a case the possibility of approximation of infeasible motions is even more important property than for unicycle-like vehicles since it can facilitate movements in a cluttered environment considerably. Similarly, the issue of asymptotic stabilization (or almost asymptotic stabilization from a practical point of view) can be addressed further in order to improve the regulation time.

Open Access This article is distributed under the terms of the Creative Commons Attribution 4.0 International License (http:// creativecommons.org/licenses/by/4.0/), which permits unrestricted use, distribution, and reproduction in any medium, provided you give appropriate credit to the original author(s) and the source, provide a link to the Creative Commons license, and indicate if changes were made.

Appendix A: Computations for a General Case

A.1 Selected Differential Relations on Lie Groups (cf. [15])

Assume that gi,g2, g3 e G and gi = X(g)vi.

dlglg2 (g3) = dlgi (g2g3)dlg2 (g3), drglg2 (g3) = drg2 (g3gi)drgi (g3),

(dlgi g))-1 =

(drgi (g2))-1 =

drg-i (gi) =

drg-i (e) =

Ad (gi) =

Ad(gi)Ad(g2) Ad(gi )X(e)

dl -i (gig2), dr -i (g2gi), Ad(gi)dlg-i (gi), dlg-i (e)Ad(gi),

dlgi (g-l)dr -i (e) g1

drg-1(g1)dlg1(e), g1

Ad(gig2),

X(e)AdX (gi), -X(g-i)AdX(gi )vi

Another useful relationship

drgig2 (g-i)X(g-i) = X(g2)AdX((gi g2)-i) (99)

can be proved as follows

drgig2(g1-1)X(g1-1)(9o0)drg2(e)drgl (g-h)X(g-h) ^ drg2 (e)Ad(g-1)

xd/gi (g-1)X(g-1) = dr g2 (e)Ad(g-l)X(e) ^ dlg2(e)Ad(g-l)Ad(g-l)X(e) (<=} dlg2(e)Ad((gig2)-1)X(e)

{9=] dlg2 (e)X(e)AdX((gig2)-1)

= X(g2)AdX((gig2)-1).

A.2 Derivative of a Translated Transverse Function Substituting relation Eq. 18 to Eq. 16 one obtains f = X(f)A(a)a + drf(k)X(k)vh. (101)

Computing the second term in Eq. 101 yields in

drf(h)X(h)vh drh-1f(h)X(h)vh

= X(f)AdX(f-1)vh. (102)

Using Eq. 102 in Eq. 101 gives f = X(f)A(a)a + X(f)AdX(f-1)vh. (103)

Now assuming that h = f .-1 and recalling time derivative of h one has

h = X(h)vh = df--1 (= -X(Jr-l)AdX(Jr)Arar.

Comparing terms in Eq. 104 gives vh = -AdX(~fr)Arar. (105)

Using this result in Eq. 103 one has

A.3 Derivation of the Companion System

Taking time derivative of Eq. 23 yields in d

z = dlg(f-1)-f-1 + drf-1 (g)g. (107)

Considering the first term in Eq. 107 and recalling Eq. 19 one has

dlf -1)dtf-1 -dlg(f-1)X(f-1)AdX(f)

x (A(a)a + AdX(f-1)vh)

(2)=23) -X(z)AdX(f)

x (.A(a)a+AdX(f-1)vh).

Next, using definition of control system (6) in the first term in Eq. 107 yields in

drf-1 (g)g = drf-1 (g)X(g) (Cu + d)

i2=] drg-1z(g)X(g) (Cu + d)

(=9) X(z)AdX(f) (Cu + d). (109) Combining Eqs. 108 and 109 in Eq. 107 gives

Z = X(z)AdX(f)(Cu+d-A(a)a-AdX(f-1)vh) .

Next, recalling definitions of the extended input u and the square matrix C given in Eq. 11, one has

Z = X(z)AdX(f)(C(a)u + d - AdX(f-1)vh) .

In the case when h = f r-1 and vh is given by Eq. 105, Eq. 111 can be rewritten as follows

z = X(z)AdX(f)

x (C(a)u + d + AdX(f -1)AdX(fr)Ara^ .

Next, recalling Eqs. 96 and 20 one obtains dynamics (24).

f = X(f){A(a)à - AdX(f-i)AdX(fr)Ar(a)à^ . A.4 Tracking Dynamics

Next, taking into account (96) and recalling (20) one obtains dynamics (22).

Taking time derivative of Eq. 26 one has

z = dl 1 z + drz(gr )—gr

The first term in Eq. 113 becomes

dl- z ^ dl-i X(z)AdX(f)

Eqs. 63 and 64 are justified. Then term A(a) in Eq. 58 can be approximated as follows

x ^C u + d — Ad

(2)=26) X(Z)AdX(f)

x (C U + d — Ad

(f—1)vh) (f—1)vh)

fe d a

dfx fa — f

da fe da

Moreover, the last element in A becomes

while the second term satisfies

, dfx 7 dfy (56) / d (98) A2 = f6 — = Ê1Ê2 (<

drz(g—1 )dg;X = —drz(g—1)X(g—1)AdX(gr)ur da

dfy (56)

[ cos2 a — 2 cos 2a^

(==) —drgrZ (g— 1)X(g—1)AdX(gr)ur (==) —X(Z)AdX(z—1)AdX(gr)ur (96=26) —X(Z)AdX(Z—1)ur. (115)

= 2£1£2.

Term Adf (f 1 ) can be computed based on Eqs. 54 and 53 as follows

Applying Eqs. 114 and 115 to Eq. 113 yields in

Z = X(Z)AdX(f)(c u + d — AdX(f—1)vh)

—X(Z)AdX(Z—1)ur.

Adf (f 1) = [sin fe - fx cos fe - fy sin fe cos fe].

Taking into account derivative of Adf one obtains

Next, assuming that Eqs. 20 and 105 hold one concludes that

z = X(z)Adf(f ) (cu + d + Adf(f-l)Arar)

-X(z)Adf(Z~l)ur. (117)

Appendix B: Computations for the Controller Designed for a Unicycle-Like Kinematics in the Nominal Condition (Case Study)

B.1 Analysis of Augmented Dynamics for Asymptotic Tracking

Computing derivative of Uaur and recalling (31) one has

d / d * \ X -1

-ur =( — A2 (ar — a))AdX(f 1(a,ar))ur

+A—1(a)(daAdX(f—1(a,ar)))ur

Latter on, in order to simplify computations, it is assumed that e2 is a small coefficient and relations

— AdX(f—1(ar — a, ar)

f cos fe K1 — sm fef

where Ki(a,ar) := - (f cos fe + f sin fe + {-fx sin fe + fy cos e) ff). Recalling that

derivative djaur has to be evaluated at a = 0 one can further simplify calculations. Basically, at a = 0 transverse function f = e (for the given parametrization). Hence

dAdX(f~\ar — a ,ar)) da

dfe dfx o da

35 _ J

from Eq. 17 one con-

cludes that f

da a=o

using Eqs. 50 and 119 gives

= —X(e)A(a). Consequently,

dfx r dfy , dfe

---fe —- and —-

da da da

Recalling that dA2 = 0, using Eqs. i23 and i24 in

Eq. ii8 and evaluating djaur at a = 0 allows one to write

^a —

-\{ dfe dfx -dfy\ \

ur = A- I--ur 1 + I--+ f e- I ur2 I .

r 2 y da r y da da J ]

Computing derivative f and substituting its components to Eq. 125 and recalling that ar = a + a « a one obtains

= A- 1(ur 162 sin ar+ur261 cos ar(1+1 62 cos 2ar)).

Recalling that e2 is small, it follows that Var e S1, 1 + 1 e2 cos 2ar « 1. Additionally using Eq. 120 one can define relation (65).

B.2 Analysis of the Augmented Dynamics for the Regulation Case and Non Asymptotic Convergence

Consider the following approximation of transverse function (60)-(62) at a = 0 assuming that e2 is a small coefficient and using relations Eqs. 63 and 64

fx |a=0 = -i ^(i - Kei ) sin ar + -f k-2 (i - Kei k-2) cos ar sin2ar

= -i(i - k-( ) sin ar + i-(-^k^ (i - k-( k-2) cos ar sin2ar

fy la=0 = -i-2 ^ k-2 (i - k-( ) cos ar sin ar + ^ (i - k-( k-2 ) sin2ar)

= 4-i-2 (i - 2k-2 + K-iK-2) sin2ar, fe la=0 = -2(i - k-2) cos ar.

(128) (i29)

Recalling Eq. 59 and computing f from Eq. 57 one can obtain

Ar(ar)

k-( ei cos ar -k-2 e2 sin ar i k-( K-2-i-2

Recalling Eq. 129 it follows that for a small

e2 one can assume that sin fe |a=0 ~ fe |,5=0 and

cos fe |a=o ~ 1. Thus Adf(f 1) given by Eq. 121 can be approximated as follows

AdX(f-1) « [ fe -(fx + fyfe) 1 ]. (131)

Using Eqs. 128 and 129 one can easily obtain

(fyfe)la=0 = 1616^(1 - Ke2) (1 - 2K62 + Ke1 ^62)

x sin2ar cos ar. X -i

Next, computing Adf (f i |a=0)Ar(ar) one has

X(f 1la=0)Ar(ar) = -i-2 (k-( (i - k-2) cos2 ar + k-2(i - k-( ) sin2 a^

10/ ^ \ O 1

+ 8eie2K-2 ( i + 2k-2 (i - k-( ) + k-2 (k-( - 2) I sin 2ar + 5k-( k-2e—

) + O(e3).

(2 • 2 i k-( cos ar + k-2 sin ar - 2 k-( k-2

Assuming that k-( and k-2 e [0, i] and kS( k-2 < i

it follows that Ke, cos2 a,

r + k-2 sin ar 2 K-i K-2 e

[0, 2). Thus one concludes that at a = 0

AdX (f-i|a=0)Ar(ar) < 2eie2 = A2. As a result

Var e s1 , P(ar, 0) = i-A2(ar-a)-iAdX(f -i)Ar(ar)| > 0. (i34)

B.3 Analysis of the Augmented Dynamics for the Regulation Case and Asymptotic Convergence

Here, it is assumed that k61 = k62 = 1. Firstly, we compute first and second derivative of Eq. 131 and evaluate them at a = 0 as follows

Secondly, performing detailed computation using a symbolic solver one obtains:

— AdX (f-1) da 1 (f )

Ar(ar) = 0,

Adf(f-1)

Ar(ar)

-2s1 s2 (l - e2(2sin4 ar + sin2 ar - 1)) . (138)

References

1. Optitrack, optical motion tracking solutions (2015). www. optitrack.com

2. Artus, G., Morin, P., Samson, C.: Tracking of an omnidirectional target with a unicycle-like robot: control design and experimental results. Research Report INRIA 4849, INRIA (2003)

3. Caracciolo, L., De Luca, A., Iannitti, S.: Trajectory tracking control of a four-wheel differentially driven mobile robot. In: Proc. IEEE Int. Conf. Robot. Autom., pp. 2632-2638. Detroit (1999)

4. Cholewinski, M., Arent, K., Mazur, A.: Towards practical implementation of an artificial force method for control of the mobile platform Rex. In: Szewczyk, R., Zielinski, C., Kaliczynska, M. (eds.) Recent Advances in Automation, Robotics and Measuring Techniques, Adv. Intell. Syst. Comput., vol. 267, pp. 353-363. Springer International Publishing (2014)

5. Gawron, T.: Ksis framework - control system middleware

(2015). http://ksisframework.tomaszgawron.pl

6. Hermes, H.: Nilpotent and high-order approximations of vector field systems. SIAM Rev. 33, 238-264 (1991)

7. Kozlowski, K., Pazderski, D.: Stabilization of two-wheel mobile robot using smooth control laws - experimental study. In: Proc. IEEE Int. Conf. on Robotics and Automation, pp. 3387-3392. Orlando (2006)

8. Lewis, A.D.: When is a mechanical control system kinematic? In: Proc. 38th IEEE Conf. on Decision and Control, pp. 1162-1167. Phoenix (1999)

9. Mandow, A., Martinez, J.L., Morales, J., Blanco, J.L., Garcia-Cerezo, A., Gonzalez, J.: Experimental kinematics for wheeled skid-steer mobile robots. In: Proc. IEEE/RSJ Int. Conf. of Intelligent Robots and Systems (IROS), pp. 1222-1227 (2007)

10. Mazur, A., Cholewinski, M.: Implementation of factitious force method for control of 5R manipulator with skid-steering platform REX. Bull. Pol. Ac.: Tech. 64(1), 71-80

(2016)

11. Michalek, M., Kozlowski, K.: Motion planning and feedback control for a unicycle in a way point following task: The VFO approach. Int. J. Appl. Math. Comput. Sci. 19(4), 533-545 (2009)

12. Morin, P., Samson, C.: A characterization of the Lie Algebra Rank Condition by transverse periodic functions. SIAM J. Control Optim. 40, 1227-1249 (2001)

13. Morin, P., Samson, C.: Practical stabilization of driftless systems on Lie groups: The transverse function approach. IEEE Trans. Autom. Control 48(9), 1496-1508 (2003)

14. Morin, P., Samson, C.: Practical and asymptotic stabilization of chained systems by the transverse function control approach. SIAM J. Control Optim. 43(1), 32-57 (2004)

15. Morin, P., Samson, C.: Trajectory tracking for nonholo-nomic systems. Theoretical background and applications. Rapport de recherche RR-6464, INRIA (2008). http://hal. inria.fr/inria-00260694

16. Morin, P., Samson, C.: Transverse function control of a class of non-invariant driftless systems. application to vehicles with trailers. In: Proc. 47th IEEE Conf. on Decision and Control, pp. 4312-4319 (2008)

17. Morin, P., Samson, C.: Control of nonholonomic mobile robots based on the transverse function approach. IEEE Trans. Robot. 25(5), 1058-1073 (2009)

18. Pazderski, D., Kozlowski, K.: Trajectory tracking of under-actuated skid-steering robot. In: Proceedings of the American Control Conference, pp. 3506-3511 (2008)

19. Pazderski, D., Kozlowski, K.: Motion control of a skid-steering robot using transverse function approach - experimental evaluation. In: Proc. 10th Int. Workshop on Robot Motion and Control (RoMoCo), pp. 72-77. Poznan (2015)

20. Pazderski, D., Waskowicz, D., Kozlowski, K.: Motion control of vehicles with trailers using transverse function approach. Int. J. Intell. Syst. 77(3-4), 457-479 (2015)

21. Pomet, J.: Explicit design of time varying stabilization control laws for a class of controllable systems without drifts. Syst. Control Lett. 18(2), 147-158 (1992)

22. Samson, C.: Velocity and torque feedback control of a nonholonomic cart. Advanced Robot Control, pp. 125-151. Birkhauser (1991)

23. Simba, K., Uchiyama, N., Sano, S.: Real-time trajectory generation for mobile robots in a corridor-like space using Beszier curves. In: Proc. IEEE/SICE Int. Symp. System Integration, pp. 37-41 (2013)

24. Trojnacki, M.: Dynamics model of a four-wheeled mobile robot for control applications - a three case study. In: Filev, D., Jablkowski, J., Kacprzyk, J., Krawcza, M., Popchev, I., Rutkowski, L., Sgurev, V., Sotirova, E., Szynkarczyk, P. (eds.) Intelligent Systems'2014, Adv. Intell. Syst. Comput., vol. 323, pp. 99-116. Springer, London (2014)

25. Zadarnowska, K., Ratajczak, A.: Task-priority motion planning of wheeled mobile robots subject to slipping. In: Kozlowski, K. (ed.) Robot Motion and Control 2011, Lecture Notes in Control and Inform. Sci., vol. 422, pp. 75-85. Springer, London (2012)

Dariusz Pazderski is with the Chair of Control and Systems Engineering of the Poznan University of Technology. He received his M.Sc. degree in robotics in 2002 from the Faculty of Electrical Engineering and his Ph.D. degree in 2007 from the Faculty of Computing Science and Management of the Poznan University of Technology. His research interests is mainly focused on mobile robotics and include continuous and discontinuous nonlinear control, robust and adaptive control, motion planning, sensor systems, and various applications of mobile robots. The results of his research have been published in national and international conference and journal papers.