Motion Planning of Discrete-time Nonholonomic Systems

Hirohiko AraiBiorobotics Division, Robotics Department

Mechanical Engineering Laboratory, AIST-MITI1-2 Namiki, Tsukuba 305-8564 Japan

harai@mel.go.jp http://www.mel.go.jp/sta�/harai.html

Abstract

The concept of discrete-time nonholonomic sys-

tems, in which the constraints are represented as dif-

ference equations of generalized coordinates, is intro-

duced. Such systems can be seen in the digital con-

trol of continuous-time nonholonomic systems, and in

mechanical systems with repetitive and discontinuous

constraints. A two-wheeled mobile robot and pivoting

manipulation of a polyhedral object are described as

simple examples. The k-step reachable region is de-

�ned as the set of the k-th state which the system can

reach from the initial state, and the reachability of such

systems is discussed. A motion planning method us-

ing the Jacobian matrix of the state with regard to the

input series is proposed.

1 Introduction

Studies on nonholonomic systems have attractedmany researchers in the �eld of advanced robot controlin recent years [1]. Nonholonomic systems in roboticsinvolve kinematic or dynamic constraints representedas nonintegrable di�erential equations. The motionplanning and control of such systems have been dis-cussed in the framework of continuous-time nonlinearsystems. Though the concept of nonholonomy origi-nated in mechanics and has continuous properties in-herently, this paper extends it into discrete-time sys-tems. Discrete nonholonomic constraints formulatedas di�erence equations are introduced based upon ananalogy with continuous nonholonomic constraints.

This extension of nonholonomy allows the follow-ing two applications; digital control of continuous-timenonholonomic systems such as a wheeled vehicle anda space robot, and the control of mechanisms withrepetitive, discontinuous constraints such as a leggedrobot and a multi-�ngered hand. These systems areapparently in quite di�erent classes, yet the concept of

discrete-time nonholonomic systems can provide a uni-�ed theoretical basis for dealing with these systems.

The remainder of this paper is organized as follows.Section 2 introduces the concept of discrete-time non-holonomic systems. Section 3 presents two simple ex-amples of discrete-time nonholonomic systems. Sec-tion 4 de�nes the k-step reachable region which thesystem can reach in k steps, and discusses the reach-ability of the system. Section 5 presents the author'sproposed motion planning method for discrete-timenonholonomic systems.

2 Discrete-time Nonholonomic Sys-tems

Before introducing discrete-time nonholonomic sys-tems, a brief review of the de�nition of continuous-time nonholonomic systems is in order. A mechanicalsystem which has a constraint that cannot be repre-sented as h(q; t) = 0, (q: generalized coordinates, t:time) is called nonholonomic. Wheeled vehicles andspace robots are typical examples of nonholonomicsystems being studied in robotics. In those systems,the constraint can be formulated as a �rst-order dif-ferential equation,

h(q; _q) = 0 (1)

(q: generalized coordinates, _q: generalized velocity)

The state equation is represented as a continuous-timenonlinear system,

_q = g(q;u) (u : input) (2)

The number of inputs u is generally smaller than thenumber of generalized coordinates q. In wheeled vehi-cles and space robots, the constraint (1) is expressedin a Pfa�an form,H(q) _q = 0, and the state equation(2) is a drift-free a�ne system, _q = G(q)u.

1

Equations (1) and (2) are the expressions whenthe state variables change continuously over passageof time. These equations are the starting point fromwhich nonholonomic systems have been investigated,mainly from the viewpoint of continuous-time non-linear systems. Discrete-time nonholonomic systemscan be formulated from an analogy with continuous-time systems. Let us suppose a system whose gen-eralized coordinates change discretely with the dis-crete inputs. The generalized coordinates and the in-puts at the k-th step are qk = (qk1 ; :::; q

kn)T 2 M and

uk = (uk1; :::; ukm)

T 2 , respectively, where m < n.

[Discrete-time Nonholonomic System]If the discrete-time system just described had a con-straint that could not be represented as an algebraicequation,

h(qk; k) = 0 (3)

it would be de�ned as a discrete-time nonholonomicsystem.

Similar to the continuous-time nonholonomic con-straints as di�erential equations, there are discrete-time nonholonomic constraints as di�erence equations.A discrete-time nonlinear system,

qk+1 = g(qk;uk) (4)

is a nonholonomic system if the constraint can be rep-resented as n�m di�erence equations,

h(qk+1; qk) = 0 (5)

and cannot be transformed into Eq. (3). Theremight also be higher order nonholonomic systems inwhich the di�erence equation of the constraint in-cludes qk+2; qk+3; :::. This paper, however, focuses on�rst-order nonholonomic systems represented as Eq.(4) and (5).

Thus we have arti�cially extended the concept ofnonholonomy to discrete-time systems, based on theanalogy with continuous-time systems. The practicalmotivations for this extension include;

� Discrete-time control of a continuous-time non-holonomic system,

� Control of a mechanical system in which the con-straint changes discontinuously and repetitively.

The �rst application is discretization and digitalcontrol of a continuous-time nonholonomic system.The control of a nonholonomic system usually involvescomplicated calculations that require computer con-trol. In most cases, the controller is designed in the

following two steps. First, an analogue controller isdesigned for the continuous-time model. Then, thatcontroller is approximately discretized into a digitalcontroller and applied to the actual system. It ispreferable, however, to directly design a digital con-troller for the exactly discretized model. The e�ect ofdiscretization errors sould be taken into account par-ticularly in the case of long sampling intervals.

In contrast to the vast ammount of literature oncontinuous control of nonholonomic systems, there hasnot been much research reported on digital control.Nonetheless, several researchers have noted its impor-tance. Divelbiss andWen [2] proposed a nonholonomicmotion planning method using a series of piecewise-constant inputs. Monaco, Normand-Cyrot, and theircolleagues [3, 4, 5] intensively studied multirate dig-ital control of nonholonomic systems. Their methodis basically analogue-digital hybrid control. First, theoriginal nonholonomic system is transformed into achained form using a continuous state feedback. Then,piecewise-constant input is applied to the chainedform with multirate sampling. Recently, Heimann [6]also proposed discrete-time open- and closed-loop con-trol for an underactuated manipulator using piecewiseconstant torque inputs.

Here, a continuous-time nonholonomic system (2)is discretized with the sampling interval �T . State qk

of the discretized system is sampled from state q(t) ofthe original continuous system as qk = q(k�T ) (k =0; 1; :::). Piecewise constant inputs, u(t) = uk, areapplied to the continuous system during each intervalk�T < t < (k + 1)�T . Then the state at the nextstep is,

qk+1 = qk +

Z (k+1)�T

k�T

g(q;uk)dt (6)

Since uk and �T are constant, the integral results ina function of qk and uk. Eq. (6) now has the sameform as Eq. (4). It should be noted that input u(t)does not have to be piecewise-constant. It can be acontinuous function parameterized by uk, e.g., quasi-periodical input of interval �T .

Another situation for a discrete-time nonholonomicsystem appears when the constraint changes repeti-tively and discontinuously. Transition of the state in-herently occurs in a discrete way. Such a system canbe seen in a legged robot or a multi-�ngered robothand. The constraint discontinuously changes accord-ing to the contact status between the foot and theground or between the �ngertip and the manipulatedobject. The robot body or the object is driven by therepetitive movement of the legs or �ngers. The motion

2

T

��� ��

�

���

���

Fig. 1: Two-wheeled mobile robot

variables of each leg or �nger (e.g. step distance) arethe discrete inputs, and at the end of each cycle thecon�gurations of the robot body and the legs, or theobject and the �ngers, are the state variables.

Kelly and Murray [7] and Goodwine and Burdick[8] investigated the nonholonomy of a legged robot.Their studies treated the whole system as a collectionof continuous systems connected by the change of theconstraint. In contrast, the author's proposed methoddescribes the motion of the system during each con-straint with a few variables and treats them as discreteinputs. Thus, the problem is simpli�ed to the discretetransition of the state.

3 Examples of Discrete-time Nonholo-nomic Systems

Two simple examples of discrete-time nonholo-nomic systems are presented here. One exampledemonstrates the discretization of a continuous-timenonholonomic system, and the other, a mechanismwith repetitive and discontinuous constraint. Com-mon characteristics of discrete-time nonholonomic sys-tems can be seen in these examples.

3.1 Discretization of a continuous-timenonholonomic system

Discretization of a two-wheeled mobile robot (Fig.1), a classical example of a continuous nonholonomicsystem, is considered here. Inputs, !L and !R, are theangular velocity of the left and the right wheels. Thestate equation of this continuous system is,

0@ _x

_y_�

1A =

0@ d(!R + !L) cos �=2

d(!R + !L) sin �=2d(!R � !L)=l

1A (7)

Inputs, !R and !L, are given piecewise-constant, !kRand !kL, with sampling interval �T . The discrete in-

puts are,

ukv = d�T (!kR + !kL)=2; uk! = d�T (!kR � !kL)=l;

Then, the state equation of the discretized system is,

(When uk! 6= 0)

8<:

xk+1 = xk + fsin(�k + uk!)� sin �kgukv=uk!

yk+1 = yk � fcos(�k + uk!)� cos �kgukv=uk!

�k+1 = �k + uk!

(8)

(When uk! = 0)

8<:

xk+1 = xk + cos �kukvyk+1 = yk + sin �kukv�k+1 = �k

(9)

When inputs ukv and uk! are removed from Eq. (8),the constraint of the system is obtained as a di�erenceequation,

(xk+1 � xk)(cos �k+1 � cos �k)+

(yk+1 � yk)(sin �k+1 � sin �k) = 0 (10)

Since Eq. (9) also satis�es Eq. (10), the constraint isdescribed by this equation for both cases.

Eq. (8) and (9) can be linearized around the originwith regard to the small inputs as follows.

qk+1 =

0@ 1 0 0

0 1 00 0 1

1A qk +

0@ 1 0

0 00 1

1Auk (11)

(qk = (xk; yk; �k)T ; uk = (ukv; uk!)T )

This system does not satisfy the controllability condi-tion of a discrete-time linear system. Hence, the �rstlinear approximation (11) of the system (8), (9) is un-controllable.

Then, let us suppose the following series of controlinputs are applied to the nonlinear system (8) and (9),

u0 = (�; 0)T ! q1 = (�; 0; 0)T

u0 = (�; �)T ; u1 = (�;��)T ! q2 = (0; 4�=�; 0)T

u0 = (0; �)T ! q1 = (0; 0; �)T

where q0 = (0; 0; 0)T . The state can transition to allthree directions of x, y, � using the two inputs, ukvand uk!, as is the case with the continuous-time non-holonomic system. Also from this fact, the constraintcannot be represented as the form of Eq. (3).

3

������

�

�

Fig. 2: Pivoting of an object

N$

�

N%�

�

�

�

��� �� �� NN ��

��� NN ��

��NT

NT

Fig. 3: Model of planar pivoting

3.2 Repetitive and discontinuous con-straint

Aiyama et al. [9] proposed a pivoting manipula-

tion method for manipulating a polyhedral object on aplane without picking it up. The object is inclined andsupported on a vertex, and then it is rotated aroundthis point (Fig. 2). Repeating this motion around thetwo vertices of the bottom edge alternately, the object\walks" on the plane and its position and orientationcan be controlled.

Figure 3 shows the kinematic model of the pivot-ing. Edge AB is �rst rotated around vertex B by theangle ukB, and is next rotated around A by ukA. Thesequence of these two rotations is considered to be onestep of the transition. The state equation is,8<:

xk+1 = xk + l cos �k � l cos(ukB � �k)yk+1 = yk + l sin �k + l sin(ukB � �k)�k+1 = �k + ukA � ukB

(12)

and it has the same form as Eq. (4). The biped robotwith a constant step [10] illustrated in Fig. 4 hasequivalent kinematics.

When inputs ukA and ukB are removed fromEq. (12),the constraint of the system is obtained as a di�erenceequation,

(xk+1�xk�l cos �k)2+(yk+1�yk�l sin �k)2 = l2 (13)

Fig.4: Biped robot equivalent to pivoting

δδ δ

δδ

δ

�δ

δ

��� ��� �θ�

Fig. 5: Motion in each coordinate

This equation also has the same form as Eq. (5).Equation (12) can be linearly approximated around

the origin as follows.

qk+1 =

0@ 1 0 0

0 1 00 0 1

1A qk +

0@ 0 0

0 l1 �1

1Auk (14)

(qk = (xk; yk; �k)T ; uk = (ukA; ukB)

T )

This system is also uncontrollable.Each of the following input series leads, respec-

tively, to the motion of only one coordinate (Fig. 5).

u0 = (�; �)T ; u1 = (��;��)T ! q

2 = (2l(1� cos �); 0; 0)T

u0 = (2�; �)T ; u1 = (0; �)T ! q

2 = (0; 2l sin �; 0)T

u0 = (�; 0) ! q

1 = (0; 0; �)T

where, q0 = (0; 0; 0)T . The system can move in anydirection, though its linear approximation is uncon-trollable.

4 k-Step Reachable Region

This section discusses the properties of the reach-able region of a discrete-time nonholonomic system,i.e., where the system can reach from an initial state.For a continuous-time system, the inputs applied overa period of time can be considered an input series of in-�nite dimensions. The number of inputs to a discrete-time system over a given period of time, however, is

4

�

�

�

�

θ

� �O

�π

��������

�

�

�

�

θ

���O �O

�π

��������

�O��O�O

Fig. 6: k-step reachable region

�nite. This results in the di�erent characteristics ofthe reachable region of a continuous-time system anda discrete-time system.

In the discrete-time system (4), the input series andthe state is related as,

q1 = g(q0;u0) = G1(q0;u0)q2 = g(q1;u1) = G2(q

0;u0;u1)...

......

qk = g(qk�1;uk�1) = Gk(q0;u0; :::;uk�1)

(15)

If ui and uj (i 6= j) are considered to be individualinputs, the number of inputs that a�ect the �nal stateincreases every step. Therefore, the properties of thereachable region depend on the number of steps thesystem passes through. Now, the reachable region re-lated to the number of steps is considered. A k-stepreachable region �(q0; k) from an initial state q0 isde�ned as the set of the k-th state qk reachable fromq0 with admissible inputs.

�(q0; k) (16)

= fqk; qj+1 = g(qj ;uj); uj 2 ; 0 � j � k � 1g

Figure 6 shows 1-step and 2-step reachable regionsfrom the origin (0; 0; 0)T in the pivoting manipulationdescribed in Section 3.2. The 1-step reachable regionis limited to the surface of a cylinder with radius l(Fig. 6(a)). The three generalized coordinates cannotbe transferred to the desired value simultaneously inone step. The 2-step reachable region is the surfaceand the inside of a cylinder with radius 3l (Fig. 6(b)).The state can reach any point within this region in twosteps. Thus, the dimension of the reachable region ofa discrete-time nonholonomic system depends on thenumber of steps.

Next, k-step local reachability is de�ned, followingthe de�nition of local reachability of a continuous-time

system. If the k-step reachable region �(q0; k) frominitial state q0 has an interior within the state spaceM , this system is k-step locally reachable from q0.

Eq. (15), qk = Gk(q0;u0;u1; :::;uk�1), means thatthe k-th state is determined as a nonlinear function ofinput series Uk = (u0

T; :::;uk�1

T)T comprisingm�k

components. The Jacobian matrix, i.e., the partialderivative of Gk with regard to Uk,

Jk =@Gk

@Uk

=

�@Gk

@u0; :::;

@Gk

@uk�1

�(17)

is considered. If the input series is perturbed asUk = Uk0+�Uk, the state reachesGk(q

0;Uk0)+�qk,

where,�qk = Jk�Uk +O(j�Ukj

2)

If the rank of Jk equals the dimension of state spaceM , it is obvious that the state can move in any di-rection ofM , by adding a small perturbation to inputseries Uk. If there is an input series Uk such that

rank Jk = dim M (18)

then the k-step reachable region �(q0; k) has an inte-rior in M , and the system is k-step locally reachable.

Jakubczyk and Sontag [11] de�ned the for-ward/backward accessibility of discrete-time nonlinearsystems. They showed Eq. (18) and equivalent condi-tions using a vector �eld generated from the discrete-time state equation. Though Eq. (18) is a simplecondition, Jacobian matrix Jk relates the modi�ca-tion of the input series to the change of the resultedstate, and this can be useful in motion planning andcontrol.

The Jacobian matrix Jk of a discrete-time linearsystem qk+1 = Aqk+Buk becomes Jk = (Ak�1B; :::;AB;B). This is equivalent to the controllability ma-trix when k = n. If the linear system is controllable,it is also n-step locally reachable. The controllabil-ity index equals the minimum step k that su�ces Eq.(18). Hence, condition (18) is a natural extension ofthe controllability condition of linear systems.

Next, the step local reachability of pivoting ma-nipulation is examined. For Jacobian matrix J2 withregard to 2-step pivoting from origin (0; 0; 0)T ,

j J2JT2 j= l4f3� cos u0A � cos(2u0A) + cos(u0A � 2u1B)

� cos(2(u0A � u1B)) + cos(2u0A � u1B)� cosu1B� cos(2u1B)g

Some input series, e.g. u0A = 0, u1B = 0, makes thepreceding equation zero. However, the rank of J2 isthree otherwise, and the system is thus 2-step locallyreachable.

5

5 Motion Planning

This section describes the author's proposed mo-tion planning method for discrete-time nonholonomicsystems. The planning problem implies the determi-nation of the input series Uk = (u0; : : : ;uk�1) thattransfers the system (4) from initial state q0 to the de-sired state qkd in k steps. From Eq. (15), the plannedinput Uk is a solution of the nonlinear algebraic equa-tion qkd = Gk(q

0;Uk).This equation has the same form as a kinematic

equation of a redundant manipulator if the input se-ries Uk and the desired state qkd are replaced withthe joint angles and the operational coordinates, re-spectively. Therefore, motion planning of a discrete-time nonholonomic system has a structure common tothe inverse kinematics problem of a manipulator. Themotion planning problem can be solved by a similarmethod with the resolved rate motion control of a re-dundant manipulator [12], using the Jacobian matrix,Jk. From the viewpoint of numerical calculation, itcan also be interpreted as a type of Newton-Raphsonmethod.

First, the pseudo-inverse of the Jacobian matrix,J+k = JTk (JkJ

Tk )�1, is calculated for some initial

value of the input series. Then, the perturbation ofthe input series is calculated.

�Uk = J+kKp(q

kd � q

k) + (I � J+k Jk)f (Uk) (19)

The �rst term reduces the error between the k-th stateand the desired state. Kp > 0 is the error feedbackgain. f(Uk) in the second term is a subtask vectorutilizing the redundancy of the inputs. It can performlower priority tasks such as input minimization andobstacle avoidance. Due to the null space matrix, thisterm does not a�ect the k-th state.

The perturbation (19) is added to the input series,Uk Uk + �Uk, and the k-th state is then evalu-ated again for the new input. These calculations arerepeated until the error qkd � q

k decreases su�ciently.Then the input series Uk is obtained (Fig. 7).

The calculation of the Jacobian matrix, Jk, seemsto require a symbolic computation of the compositefunction Gk(q

0;Uk) and its partial derivative. How-ever, the submatrices composing Jk can be simpli�edusing the one-step transition function g(q;u) as fol-lows.

@Gk

@ui=

@g

@q

����qk�1

:::@g

@q

����qi+1

@g

@u

����qi

Therefore, complicated symbolic operations are notnecessary.

��� �

NNN 8T*T

������NNN

NNGSNN

. 8I--,TT-8 �� ��� '

NNN888 '�m

"H�� NNG

TT

�����

���

�

��

Fig.7: Motion planning algorithm

start

end

(a)

start

end

(b)

start

end

(c)

Fig. 8: Planned motion of mobile robot; (a): (0, 0,�=2) ! (5, 10, �=2), (b): (0, 0, 0) ! (0, 10, �), (c):(0, 0, ��=2) ! (0, 10, �=2)

Applying the above method, the motions of thewheeled mobile robot in Section 3.1 are planned.The subtask using the redundant inputs is given asf(Uk) =�(Kvu

0v;K!u

0!; :::;Kvu

k�1v ;K!u

k�1! ). This

minimizes the cost function,Pk�1

i=0 fKv(uiv)2 +

K!(ui!)2g. The planned motions are illustrated in

Fig. 8. The error of the k-th state from the de-sired state converges to zero, Fig. 9 (a), and the costfunction reduces, (b), while iteratively calculating thetrajectory Fig. 8 (a).

The pivoting motions in Section 3.2 are alsoplanned (Fig. 10). The subtask is f(Uk) =�Ku(u0A; u

0B; :::; u

k�1A ; uk�1B ) and the cost function isPk�1

i=0 f(uiA)

2+(uiB)2g. The computer programs for the

planning of the mobile robot and pivoting manipula-tion are almost the same except for the calculations ofthe transition functions and their partial derivatives.Motion planning is achieved by an uni�ed method inboth cases.

6

�

��

���

���

���

� �� �� ��

QXPEHU RI LWHUDWLRQ

_HUURU_A�

�D� 3RVLWLRQ HUURU

���

���

���

�

���

���

� �� �� ��

QXPEHU RI LWHUDWLRQ

�.XA��

�E� &RVW IXQFWLRQ

Fig. 9: Error and cost function

start

end

(a)

start

end

(b)

start

end

(c)

Fig. 10: Planned motion of pivoting; (a): (0, 0, 0) !(3, 6, 0)，(b): (0, 0, �=2) ! (0, 6, �=2), (c): (0, 0,�=2) ! (0, 6, ��=2)

6 Conclusions

The concept of nonholonomy was extended todiscrete-time systems through an analogy withcontinuous-time systems. Potential applications ofthis extension include digital control of continuous-time nonholonomic systems, and control of mecha-nisms with repetitive and discontinuous constraints.The digital control of a two-wheeled vehicle, and piv-oting manipulation of a polyhedral object were shownas examples. In both cases, the constraints are rep-resented as di�erence equations, and the states canmove in any direction in spite of the uncontrollabilityof the linear approximations. The dimension of theregion the state can reach depends on the number ofsteps. The input series to reach the desired state canbe solved in the same way as the inverse kinematics ofa redundant manipulator.

References

[1] I. Kolmanovsky and N. H. McClamroch, \De-velopments in Nonholonomic Control Problems,"IEEE Control Systems, Vol. 15, No. 6, pp. 20{36,1995.

[2] A. Divelbiss and J. Wen, \A Global Approachto Nonholonomic Motion Planning," Proc. 31stIEEE Conf. Decision and Control, pp. 1597{1602,1992.

[3] S. Monaco and D. Normand-Cyrot, \An Intro-duction to Motion Planning Under Multirate Dig-ital Control," Proc. 31st IEEE Conf. Decision andControl, pp. 1780{1785, 1992.

[4] A. Chelouah et al., \Digital Control of Nonholo-nomic Systems: Two Case Studies," Proc. 32ndIEEE Conf. Decision and Control, pp. 2664{2669,1993.

[5] P. Di Giamberadino et al., \Piecewise Continu-ous Control for a Car-Like Robot: Implementa-tion and Experimental Results," Proc. 35th IEEEConf. Decision and Control, pp. 3564{3569, 1996.

[6] B. Heimann, \Feedback Linearization of Un-deractuated Manipulation Systems," Proc. 3rdInt. Conf. Advanced Mechatronics (ICAM'98),pp.163{166, 1998.

[7] S. Kelly and R. Murray, \Geometric Phasesand Robotic Locomotion," J. Robotic Systems,Vol.12, No.6, pp.417{431,1995.

[8] B. Goodwine and J. Burdick, \Trajectory Gen-eration for Kinematic Legged Robots," Proc.1997 IEEE Int. Conf. Robotics and Automation,pp.2689{2696, 1997.

[9] Y. Aiyama et al., \Pivoting: A New Method ofGraspless Manipulation by Robot Fingers," Proc.IROS'93, Vol.1, pp.136{143，1993.

[10] T. Yano et al., \Development of a Self-ContainedWall Climbing Robot with Scanning Type Suc-tion Cups," Proc. IROS'98, pp.249{254, 1998.

[11] B. Jakubczyk and E. Sontag, \Controllabilityof Nonlinear Discrete-time Systems: A Lie-Algebraic Approach," SIAM J. Control and Op-timization, Vol.28, No.1, pp.1{33, 1990.

[12] Y. Nakamura, H. Hanafusa, and T. Yoshikawa,\Task-Priority Based Redundancy Control ofRobot Manipulators," Int. J. Robotics Research,Vol.6, No.2, pp.3{15, 1987.

7