2.4 Dynamic Model of a Mobile System With Constraints
The kinematic model only describes static transformation of some robot velocities (pseudo velocities) to the velocities expressed in global coordinates. However, the dynamic motion model of the mechanical system includes dynamic properties such as system motion caused by external forces and system inertia. Using Lagrange formulation, which is especially suitable to describe mechanical systems [14], the dynamic model reads
ddt(∂L∂q˙k)−∂L∂qk+∂P∂q˙k+gk+τdk=fk
(2.46)
where index k describes the general coordinates qk (k = 1, …, n), L defines the Lagrangian (difference between kinetic and potential energy of the system), P is the power dissipation function due to friction and damping in the system, gk are the forces due to gravitation, τdk are the system disturbances, and fk are the general forces (external influences to the system) related to the general coordinate qk. Eq. (2.46) is valid only for a nonconstrained system, that is, for the system without constraints that has n DOFs and no velocity constraints.
For systems with motion constraints the dynamic motion equations are given using Lagrange multipliers [15] as follows:
ddt(∂L∂q˙k)−∂L∂qk+∂P∂q˙k+gk+τdk=fk−∑mj=1λjajk
(2.47)
where m is the number of linearly independent motion constraints, λj is the Lagrange multiplier associated with the jth constraint relation, and ajk (j = 1, …, m, k = 1, …, n) are coefficients of the constraints.
The final set of equations consists of n + m differential and algebraic equations (n Lagrange equations and m constraints equations) with n + m unknowns (n general coordinates qk and m Lagrange multipliers λj). Equations are differential in generalized coordinates and algebraic regarding Lagrange multipliers.
A dynamic model (2.47) of some mechanical system with constraints can be expressed in matrix form as follows:
M(q)q¨+V(q,q˙)+F(q˙)+G(q)+τd=E(q)u−AT(q)λ
(2.48)
where the meaning of matrices is described in Table 2.1.
Vector of generalized coordinates (dimension n × 1)
M(q)
Positive-definite matrix of masses and inertia (dimension n × n)
V(q,q˙)
Vector of Coriolis and centrifugal forces (dimension n × 1)
F(q˙)
Vector of friction and dumping forces (dimension n × 1)
G(q)
Vector of forces and torques due to gravity (dimension n × 1)
τd
Vector of unknown disturbances including unmodeled dynamics (dimension n × 1)
E(q)
Transformation matrix from actuator space to generalized coordinate space (dimension n × r)
u
Input vector (dimension r × 1)
AT(q)
Matrix of kinematic constraint coefficients (dimension m × n)
λ
Vector of constraint forces (Lagrange multipliers) (dimension m × 1)
2.4.1 State-Space Representation of the Dynamic Model of a Mobile System With Constraints
In the sequel a state-space dynamic model for a system with m kinematic constraints is derived. Then partial linearization of the system state-space description is performed using a nonlinear feedback relation [6] that enables the second-order kinematic model to be obtained.
A dynamic system with m kinematic constraints is described by
M(q)q¨+V(q,q˙)+F(q˙)+G(q)=E(q)u−AT(q)λ
(2.49)
where the influence of unknown disturbances and unmodeled dynamics from Eq. (2.48) is left out. The kinematic motion model is given by
q˙=S(q)v(t)
(2.50)
The dynamic model (2.49) and kinematic model (2.50) can be joined to a single state-space description. A unified description of nonholonomic and holonomic constraints can be found in [16], where holonomic constraints are expressed in differential (with velocities) form like nonholonomic constraints.
Due to shorter notation the dependence on q is omitted in the following equations (e.g. M(q) or S(q) is written as M or S, respectively). A time derivative of Eq. (2.50) is
q¨=S˙v+Sv˙
(2.51)
Inserting into Eq. (2.49) and expressing generalized coordinates q with pseudo velocities v results in
MS˙v+MSv˙+V+F+G=Eu−ATλ
(2.52)
The presence of Lagrangian multipliers λ due to motion constraints can be eliminated by considering relation AS = 0 and its transform STAT = 0. Multiplying Eq. (2.52) by ST therefore gives a reduced dynamical model:
STMS˙v+STMSv˙+STV+STF+STG=STEu
(2.53)
where Lagrange multipliers λ are eliminated. Introducing M˜=STMS, V˜=STMS˙v+ST(V+F+G), and E˜=STE gives a more transparent notation,
M˜v˙+V˜=E˜u
(2.54)
from where a vector of pseudo accelerations v˙ is expressed:
v˙=M˜−1(E˜u−V˜)
(2.55)
If condition detSTE≠0 is true (which in most of realistic examples is) then from Eq. (2.54) the system input can be expressed as
u=E˜−1(M˜v˙+V˜)
(2.56)
By extending the state vector with pseudo velocities x = [qTvT]T and presenting the system in nonlinear form x˙=f(x)+g(x)u (expression f(x) contains nonlinear dependence from states) the state-space representation of the system reads
x˙=⎡⎣⎢⎢Sv−M˜−1V˜⎤⎦⎥⎥+⎡⎣⎢⎢0n×rM˜−1E˜⎤⎦⎥⎥u
(2.57)
where r is the number of inputs in vector u and state vector x has the dimension (2n − m) × 1.
Using the inverse model (2.56) the required system input can be calculated for the desired pseudo accelerations of the system. Applying these inputs to Eq. (2.57) the resulting model becomes
where uz are pseudo accelerations of the system. Relation (2.56) can therefore be used for calculating the required predicted system inputs. These inputs can be used to control the system without feedback (open-loop control) or they are better used as feedforward control in combination with some feedback control.
2.4.2 Kinematic and Dynamic Model of Differential Drive Robot
The differential drive vehicle in Fig. 2.3 has two wheels usually powered by electric motors. Let us assume that the vehicle mass center coincides with its geometric center. The mass of the vehicle without the wheels is mv, and the mass of the wheels is mw. The vehicle moves on the plane and its moment of inertia around the z-axis is Jv (z is in the normal direction to the plane), and the moment of inertia for the wheels is Jw.
In practice usually the mass of the wheels is much smaller than the mass of the casing; therefore a common mass m and inertia J can be used. The vehicle is described by three generalized coordinates q = [x, y, φ] and its input is the torque on the left and the right wheel, τl and τr, respectively (Fig. 2.18).
which means that the reachable motion directions are columns of the kinematic matrix
S=⎡⎣⎢⎢⎢⎢cos(φ(t))sin(φ(t))0001⎤⎦⎥⎥⎥⎥
(2.59)
and the constraint coefficient matrix is
A=[−sinφcosφ0]
(2.60)
Dynamic Model
The dynamic model is derived by Lagrange formulation:
ddt(∂L∂q˙k)−∂L∂qk+∂P∂q˙k=fk−∑mj=1λjajk
(2.61)
where unknown disturbance τdk from relation (2.47) is omitted. Similarly the forces and the torques due to the gravitation gk are not present because the vehicle drives on the plane where the potential energy is constant (without loss of generality WP = 0 can be assumed).
Overall, kinetic energy of the system reads
WK=m2(x˙2+y˙2)+J2φ˙2
(2.62)
The potential energy is WP = 0, so the Lagrangian is
L=WK−WP=m2(x˙2+y˙2)+J2φ˙2
(2.63)
Additionally damping and friction at the wheels rotation can be neglected (P = 0). Forces and torques in Eq. (2.61) are
ddt(∂L∂x˙)=mx¨ddt(∂L∂y˙)=mÿddt(∂L∂φ˙)=Jφ¨
(2.64)
and
∂L∂x=0∂L∂y=0∂L∂φ=0
(2.65)
According to Lagrange formulation (2.61) the following differential equations are obtained:
mx¨−λ1sinφ=Fxmÿ+λ1cosφ=FyJφ¨=M
(2.66)
where the resultant force on the left and the right wheel is F=1r(τr+τl) and r is the wheel radius. The force in the x-axis direction is Fx=1r(τr+τl)cosφ and in the y-axis it is Fy=1r(τr+τl)sinφ. Torque on the vehicle is obtained by M=L2r(τr−τl), where L is the distance among the wheels. Therefore, the final model is
The common state-space model (2.57) that includes the kinematic and dynamic model is determined by matrices
M˜=V˜=E˜=⎡⎣⎢m00J⎤⎦⎥⎡⎣⎢00⎤⎦⎥1r⎡⎣⎢⎢1L21−L2⎤⎦⎥⎥
and according to Eq. (2.57) the system can be written in the state-space form x=f(x)+g(x)u˙, where the state vector is x = [qT, vT]T. The resulting model is
The inverse system model is obtained by taking into account Eq. (2.56) as
⎡⎣⎢τrτl⎤⎦⎥=⎡⎣⎢⎢v˙mr2+ω˙JrLv˙mr2−ω˙JrL⎤⎦⎥⎥
(2.69)
Using the inverse model the required input torques to the wheels can be obtained from the known robot velocities and accelerations. The obtained input torques can be applied in open-loop control or better as a feedforward part in a closed-loop control.
Example 2.9
Derive the kinematic and dynamic model for a vehicle with differential drive in Fig. 2.19. Express the models with the coordinates of the mass center (xc, yc), which are distance d away from the geometric center (x, y).
Solution
Considering the transformation between the geometric and mass center x=xc−dcosφ and y=yc−dsinφ and their time derivatives x˙=x˙c+dφ˙sinφ and y˙=y˙c−dφ˙cosφ and inserting the derivatives to kinematic model (Fig. 2.3), the final kinematic model and kinematic constraint for the mass center are obtained:
Derive the kinematic and dynamic model for a vehicle in Example 2.9 where the mass center and geometric center are not the same. Express the models using coordinates of the geometric center (x, y), which could be more appropriate in case that the mass center of the vehicle is changing due to a varying load.
Solution
The kinematic model and kinematic constraint for the geometric center are obtained:
Using Matlab calculate the required torques so that the mobile robot from Example 2.9 will drive along the reference trajectory xr=1.1+0.7sin(2π/30), yr=0.9+0.7sin(4π/30) without using sensors (open-loop control). In the simulation calculate robot torques using the inverse dynamic model, and depict system trajectory. Robot parameters are m = 0.75 kg, J = 0.001 kg m2, L = 0.075 m, r = 0.024 m, and d = 0.01 m.
Solution
The inverse robot model considering relation (2.56) is