EN Nonlinear Control and Planning in Robotics Lecture 2: System Models January 28, 2015

Transcription

1 EN Nonlinear Control and Planning in Robotics Lecture 2: System Models January 28, 25 Prof: Marin Kobilarov. Constraints The configuration space of a mechanical sysetm is denoted by Q and is assumed to be an n- dimensional manifold, locally isomorphic to R n (we ll say exactly what this means in a future lecture. A configuration is denoted by q Q. We first introduce the notion of constraints: holonomic (or geometric: h i (q =, i =,..., k restrict possible motions to a n k dimensional sub-manifold (think hypersurface embedded in a larger ambient space linear (Pfaffian nonholonomic (or kinematic: linear in the velocities a T i (q q =, i =,..., k, or A T (q q = in matrix form Nonholonomic constraints are not integrable, i.e. it is not possible to find k functions h i such that q h i (q = a i (q, i =,..., k If one can find such functions then the constraint is holonomic, i.e. a T i (q(t q(tdt = h i (q(t T q(tdt = h i (q + c, where c is a constant of integration. Holonomic constraints are inherently different than nonholonomic. If a(q T q = can be integrated to obtain h(q = c, then the motion is restricted to lie on a level surface (a leaf of h corresponding to the constant c obtained by the initial condition c = h(q. Practically speaking, once the system is on the surface, it cannot escape. Consider a single constraint a(q T q =. When the constraint is nonholonomic the instantaneous motion (velocity is allowed in all directions except for a(q (i.e. to an n -dimensional space. But it could still be possible to reach any configuration in Q. So the system will leave the surface. Example. The unicycle. The canonical example of a nonholonomic system is the unicycle (a.k.a. the rolling disk. The configuration is q = (x, y, θ denoting position (x, y and orientation θ. There is one constraint, i.e. the unicycle must move in the direction in which it is pointing: ẋ sin θ ẏ cos θ =, or ẏ = tan θ, ẋ

2 We have a(q = sin θ cos θ The feasible velocities are then contained in the null space of A(q = a(q, i.e. cos θ null(a T (q = span sin θ, This system starts at configuration q = (x, y, θ and can reach any desired final configuration q f = (x f, y f, θ f. The simplest strategy is first to rotate so that the disk points to (x f, y f, then move forward until (x f, y f is reached, then turn in place until the orientation reaches θ f. Draw a picture of the motion in the the configuration space. More generally, let us denote the allowed directions of motion by vectors g j, i.e. or in matrix form. a i (q T g j (q =, i =,..., k, j =,..., n k A T (qg(q =. The feasible trajectories of the mechanical system are the solutions of q = m g j (qv j = G(qv, j= where v(t R m, m = n k, are called reduced velocities or pseudovelocities We will be concerned with two classes of models. Kinematic models assume that v can be directly controlled. Dynamic models require the derviation of another differential equation determining the evolution of v. For kinematic systems the question of controllability is equivalent to nonholonomy..2 Dynamics How do we obtain ẋ = f(t, x, u for dynamical systems? We will focus on mechanical systems with equations of motion derived through a Lagrangian approach, which is often sufficient for most systems of interest in robotics..2. Holonomic Underactuated Systems Let q R n denote generalized coordinates. Assume that the system has a Lagrangian L(q, q = 2 qt M(q q V (q, with inertia matrix M(q > and potential energy V (q. The system is subject to external forces f ext (q, q R n and control inputs τ R m. 2

3 The equations of motion in terms of the Lagrangian (i.e.the Euler-Lagrange equations are given by d dt ql q L = f ext (q, q + B(qτ, where B(q R n m is a matrix mapping from m control inputs to the forces/torques acting on the generalized coordinates q. This equation is obtained from Lagrange-d Alembert variational principle δ t L(q, qdt + The actual equations take the form t [f ext (q, q + B(qτ] T δq(t =. M(q q + b(q, q = B(qτ, ( where b(q, q = Ṁ(q q 2 q( q T M(q q + q V (q f ext (q, q. The system is written in control form in terms of the state x = (q, q as ( ( q ẋ = f(x + g(xu = M(q + b(q, q M(q B(q Example 2. 2-dof manipulator. Consider a 2 dof-manipulator subject to gravity with the following parameters: Description Notation Length of link # l Length of link #2 l 2 Distance to COM of link # l c Distance to COM of link #2 l c2 link # mass m link #2 mass m 2 link # inertia I link #2 inertia I 2 gravity acceleration g The mass matrix is [ m l M(q = c + m 2[l 2 + l2 2c + 2l l c2 cos q 2 ] + I + I 2 m 2 (lc2 2 + l ] l c2 cos q 2 + I 2 m 2 (lc2 2 + l l c2 cos q 2 + I 2 m 2 lc2 2 + I, 2 u while the bias term is [ m2 l b(q, q = l c2 sin(q 2 q 2 m 2 l l c2 sin(q 2 [ q + q 2 ] m 2 l l c2 sin(q 2 q ] ( [m l q+ c + m 2 l ]g sin(q + m 2 l c2 g sin(q + q 2 m 2 l c2 g sin(q + q 2, For fully actuated manipulator we have B(q = I. For actuation only at the first joint we have [ ] B(q =. 3

4 Example 3. Simplified model of a boat in 2D, with two rear propellers. denoted by q = (x, y, θ. The mass matrix is given by m M(q = m. J The configuration is while the bias is b(q, q = R(θD( q q, where tha matrix D( q denotes drag terms and R(θ is the rotation matrix cos θ sin θ R(θ = sin θ cos θ which transforms forces from body-fixed to spatial frame. The control martix is B(q = R(θ, r r where the constant r > denotes the distance between each thruster and central axis..2.2 Nonholonomic Systems Assume that the system has a Lagrangian L(q, q = 2 qt K(q q V (q, with inertia matrix K(q > and potential energy V (q. The system is subject to external forces f ext (q, q and control inputs τ R m. The Euler-Lagrange equations take the form d dt ql q L = A(qλ + f ext (q, q + S(qτ, where S(q R n m is a matrix mapping from m control inputs to the forces/torques acting on the generalized coordinates q and where λ R k is a vector of Lagrange multipliers. The term A(qλ should be understood as a force which counters any motion in directions spanned by A(q. This equation is obtained from the Lagrange-d Alembert variational principle δ t L(q, qdt + t [f ext (q, q + S(qτ] T δq(t =, subject to both A(q T q = and A(q T δq(t =, i.e. the variations are restricted as well. The actual equations take the form K(q q + n(q, q = A(qλ + S(qτ, (2 A T (q q =, (3 4

5 where n(q, q = K(q q 2 q( q T K(q q + q V (q The Lagrange multipliers can be eliminated by first noting that A T (qg(q = and multiplying (2 by G T (q to obtain a reduced set of m = n k differential equations G T (q(k(q q + n(q, q = G T S(qτ. A standard assumption will be that det(g(q T S(q or that all feasible directions are controllable. The final equations are then expressed as where using the notation q = G(qv, (4 M(q v + b(q, v = B(qτ, (5 M(q = G T (qk(qg(q > b(q, v = G T K(qĠ(qv + GT (qn(q, G(qv Ġ(qv = B(q = G T (qs(q m ( g i (q T v i G(qv. i= For nonholonomic systems, we would normally assume an isomorphism between pseudo-accelerations a = v and control inputs τ, i.e. any acceleration a can be achieved by setting τ = B(q (M(qa + b(q, v. That is why often in nonholonomic control we take a as the (virtual control input, i.e. u a and express the control system in terms of the state x = (q, v ẋ = f(x + g(xu = ( G(qv ( + I m Example 4. Unicycle. The configuration is q = (x, y, θ with mas m, moment of inertia J, driving force τ, steering force τ 2. The general dynamic model K(q q + n(q, q = A(qλ + S(qτ, u. takes the form m m J ẍ ÿ θ = sin θ cos θ λ + cos θ sin θ ( τ τ 2, 5

6 We have G(q = S(q, G T (qs(q = I 2, and GT (qbġ(q =, from which we obtain the reduced mass matrix and bias M(q = The complete equations of motion are [ m J ], b(q, q =. ẋ = cos θv ẏ = sin θv θ = v 2 m v = τ J v 2 = τ 2, which can be put in a standard form, for x = (x, y, θ, v, v 2 ẋ = f(x + g(xτ. Example 5. Simple car models. A common way to model a car for control purposes is to employ the bycycle model, i.e. collapse each pair of wheels to a single wheel at the center of their axle. The generalized coordinates are q = (x, y, θ, φ, where φ is the steering angle. We have the constraints ẋ sin θ ẏ cos θ = front wheel (6 ẋ sin(θ + φ ẏ cos(θ + φ θl cos φ = rear wheel (7 For the real-wheel drive we have while for the front-wheel drive we have A kinematic model is given by G(q = G(q = cos θ sin θ l tan φ cos θ cos φ sin θ cos φ l sin φ q = G(qu, where the inputs are u rear drive velocity, u 2 - steering. A dynamic model includes the dynamics of v which is the same as the unicycle. 6