Navigation and Trajectory Control for Autonomous Vehicles Presented by: Mithun Chowdhury Tesfaye Asmera Mengesha Matricola No. 160758 Matricola No. 160827 Student of Mechatronics Engineering Student of Mechatronics Engineering University of Trento, Italy University of Trento, Italy UNIVERSITY OF TRENTO 2015, Italy
35
Embed
Navigation and Trajectory Control for Autonomous Robot/Vehicle (mechatronics)
This document is posted to help you gain knowledge. Please leave a comment to let me know what you think about it! Share it to your friends and learn new things together.
Transcript
Navigation and Trajectory Control forAutonomous Vehicles
Presented by:Mithun Chowdhury Tesfaye Asmera MengeshaMatricola No. 160758 Matricola No. 160827Student of Mechatronics Engineering Student of Mechatronics EngineeringUniversity of Trento, Italy University of Trento, Italy
UNIVERSITY OF TRENTO
2015, Italy
Question 1:
Introduce mobile robot designing with considerations about the interrelation existing within:
Task Operational environments Chosen kinematic model Admissible
path/trajectory planning Low-level control High-level control (overall strategy)
Solution:
The design of Mobile robot requires different level of consideration as its mobile base moves
through the environment. Complement to manipulation, the robot arm is fixed but moves objects
in the workspace by imparting force to them. In mobile robotics, the environment is fixed and the
robot moves by imparting force to the environment. Mobile robots have mobile base which
allows it to move freely in the environment.
Tasks and operational environment: Usually the mobile robot task and operational
environment is two completely related terms that goes together. Identification of required task to
be executed is the first step in design of mobile robot. The tasks can be a combination of high
level tasks and low level tasks depending on the application area. The tasks are accomplished
carrying out high level procedures executed concatenating with simpler low level execution. The
execution of mobile robot task is in operational environment, which could be structured
environment or unstructured environment in which there exists no obstacles or static obstacles or
moving objects. Depending on the tasks to be completed and the operational environments a
robot suitable for that application area is designed. This helps for selection of kinematic model of
the robot. So, after identifying the robot and the tasks, task planning should be developed. With
the robot moving in the operational environment the path which fulfills the needed criteria has to
be planned. Path planning is one of the key steps in mobile robot design for execution of the task
in a given operational environment. Trajectory is admissible trajectory if it can be a solution of
kinematic model. Admissible trajectory is the extension of admissible path with introduction of
time law. Planning produces a set of inputs for control loops. The control loop employs high
level control to execute high level tasks and meet the requirement of the robot. It is purely
kinematics based. In the mean while low level control is adopted for low level tasks.
Kinematic model choice: kinematics is the basic study of how the mechanical system of robot
behaves. In mobile robotics, we need to understand the mechanical behavior of the robot both in
order to design appropriate mobile robots for tasks and to understand how to create control
software for an instance of mobile robot hardware.
The kinematic model of a wheeled mobile robot must satisfy the non-holonomic constraints
imposed due to rolling without slipping condition. Denoting input vector fields by {g1,...,gm.} a
basis of N(AT
(q)), the admissible trajectories for the mechanical system can then be
characterized as the solutions of the non-linear dynamical system.
( ) ( )1
mgj q uj G q uq
j
, m = n – k
Where q Rn is the state vector and u = [u1…um]
T IR
m is the input vector. The choice of the
input vector fields g1 (q),...,gm(q) is not unique. Correspondingly, the components of u may
have different meanings. In general, it is possible to choose the basis of N (AT(q)) in such a way
that the uj have a physical interpretation. In any case, vector u may not be directly related to
the actual control inputs that are generally forces and torques.
The holonomy or nonholonomy of constraints can be established by analyzing the controllability
properties of the associated kinematic model.
Admissible path/trajectory planning: admissible path is the path which fits to the desired
kinematic model whereas admissible trajectory is the extension of the path with associated time.
The path/trajectory is said to be Admissible path or trajectory if it is the solution of kinematic
model. With the robot moving in the operational environment the path which fulfills the needed
criteria has to be planned. Now, we can define Admissible Path as a solution of the differential
system corresponding to the kinematic model of the mobile robot, with some initial and final
given conditions. In other words, robot must meet the boundary condition (interpolation of the
assigned points and continuity of the desired degree) and also satisfy the non-holonomic
constraints at all points.
Planning: for execution of specific robotic task involves the consideration of motion planning
algorithm. In wheeled mobile robot path and trajectory (path with associated time law) planning
involves the admission flat output y and its derivative to certain order. The goal of non-
holonomic motion planning is to provide collision-free admissible paths in the configuration
space of the mobile robot system. Many kinematic models of mobile robots, including the
unicycle and the bicycle, exhibits a property known as differential flatness, that is particularly
relevant in planning problems. A non-linear dynamic system
( ) ( )*X f x G x u
is said to be differentially flat if there exists a set of output y, called flat outputs, such that the
state x and the control inputs u can be expressed algebraically as a function of y and its time
derivatives up to certain order:
( )
( , , ,.... )r
x x y y y y
( )
( , , ,.... )r
u u y y y y
Based on the assignment of output trajectory to flat output y, the associated trajectory of the state x and
history of control inputs u are uniquely determined. In fact Cartesian coordinates of unicycle and bicycle
mobile robot is flat output.
Path planning problem can be solved efficiently whenever the mobile robot admits the set of flat output.
The path can be planned by using interpolation scheme considering the satisfaction of boundary
condition. Approaches of path planning:
Planning via Cartesian polynomials
Planning via the chained form
Planning via the parameterized inputs
For example in the following considered vehicle is a unicycle that must perform various parking
manoeuvores.
Figure 1. These results shows two parking manoeuvres planned via cubic Cartesian polynomials; with the
same forward motion.
After the path q, q [qi, qf] has been determined the it is possible to choose a timing law q = q(t) with
which the robot should follow it. For example in the case of unicycle if the driving velocity and steering
velocity is subjected to the bounds,
max( )v t v
max( )t
It is necessary if the velocities along the planned path are admissible. The timing law is rewritten by
normalizing the time variable as follows, /t T with T = tf- ti
1
( ) ( )ds
v t v sdt T
, 1
( ) ( )ds
t sdt T
Increasing the duration of trajectory, T, reduces the uniformity of v and .
High level control: High level control purely kinematics-based, with velocity commands for wheeled
mobile robots (WMR). In WMR it is dedicated to generate reference motion (VL and VR), display user
interface with real-time visualization as well as a simulation environment. The objective of high level
control in mobile robots can be summarized as:
Use sensor data to create model of the world
Use model to form a sequence of behaviors which will achieve the desired goal
Execute the plan
Low-level control loops: These loops that are integrated in the hardware or software architecture, accepts
as input a reference value for the wheel angular speed that is reproduced as accurately as possible by
standard regulation actions (e.g., PID controllers). The low-level control layer is in charge of the
execution of the high-level velocity commands.
PID controller for motor velocity
PID controller for robot drive system
Overall strategy: The overall strategy is to implement the control of the mobile robot based on a two
level hierarchy in which the high level is dedicated to generating the inputs that are required to execute a
planned path. WL and WR are the wheel angular velocity required at that instant of time, while the low
level control is dedicated to executing the velocity commands coming from high level control. One good
example available in literatures is the wheeled mobile robot superMario. SuperMARIO is a two-wheel
differentially-driven vehicle, which more kinematically stable but equivalent form of a unicycle model.
The high level control is dedicated in path planning and generating reference velocities while the low
level control, can be the PID controls the wheel motor.
Figure 2. Overall strategy of mobile robot design
Question 2:Explain the interrelations within:Position and/or velocity constraints; (non)holonomicity; (non)integrability; (non)involutivity; in-stantaneous motion directions; local/global accessibility and manoeuving.
Solution:In mobile robotics, we need to understand the mechanical behavior of the robot both in order todesign appropriate mobile robots for tasks and to understand how to create control software for aninstance of mobile robot hardware. It is subject to kinematic constraints that reduce in general itslocal mobility, while leaving intact the possibility of reaching arbitrary configurations by appro-priate maneuvers. The motion of the system that is represented by the evolution of configurationspace over time may be subject to constraints. Position constraint reduces the positions that can bereached by the robot in the configuration space, i.e. they are related to the generalized coordinates.Thus, these constraints reduce the configuration space of the system. Velocity constraints reduce aset of generalized velocities that can be attained at each configuration.
In the robotics community, when describing the path space of a mobile robot, often the conceptof holonomy is used. The term holonomic has broad applicability to several mathematical areas,including differential equations, functions and constraint expressions. In mobile robotics, the termrefers specifically to the kinematic constraints of the robot chassis. A holonomic robot is a robotthat has zero non-holonomic kinematic constraints. Conversely, a non-holonomic robot is a robotwith one or more non-holonomic kinematic constraints.
Let, q = [q1 ...q2 ...qn], i = 1, ...,k < N with qgeneralized coordinates and εRN and hi : C→ Rare of class C∞
order (smooth) and independent.A kinematic constraint is a holonomic constraint if it can be expressed in the form∑hi = H(q) = 0
iKinematic constraints formulated via differential relations (constant in velocity) are holonomic ifthey are integrable A(q, q) = 0. In the of holonomic systems, we obtain N differential Pfaffianconstraints. Holonomic constraints reduce the space of accessible configuration from n to n-k be-cause the motion of the mechanical system in configuration space is confined to a particular levelsurface of the scalar functions Thus, they can be characterized as position constraints. Holonomicconstraints are generally the result of mechanical interconnections between the various bodies ofthe system. A mobile robot with no constraints is holonomic. I A mobile robot capable of onlytranslations is also holonomic.
Constraints that involve generalized coordinates and velocities are called kinematic constraints.Kinematic constraints are generally linear in generalized velocities. If kinematic constraint isnot integrable, it is called non-holonomic. Thus, non-integrablity of kinematic constraint implies
non-holonomic nature of that constraint. Non-holonomic constraints reduce the mobility of themechanical system in a completely different way with respect to holonomic constraints. Theseconstraints involve generalized coordinates and velocities and constrain the instantaneous admis-sible motion of the mechanical system by reducing the set of generalized velocities that can beattained at each configuration. Non-holonomic constraints are non integrable. One of the test thatgives us information the system nonholomicity is the involutibvity test obtained via Lie algebraand Froboenius involutivity theorem.
Definition 1: For two vector fields f and g, the Lie bracket is a third vector field defined by:
[ f ,g](q) =∂g∂q
f (q)− ∂ f∂q
g(q)
It is obvious that [ f , g] =−[ f , g] = 0 for constant vector fields f and g.
Definition 2: A distribution ∆is involutive if it is closed under Lie bracket operation, that is,if
g1ε∆ and g2ε∆⇒ [g1,g2]ε∆
For a distribution ∆ = span{g1 ..., g2}, where gi is the basis of N(AT (q)), the distribution is invo-lutive if it is closed under Lie brackets.
Frobenius’s theorem: A regular distribution is integrable if and only if it is involutive.
The mobile robot maneuverability is the overall degrees of freedom that a robot can manipulate. Itcan also be defined in terms of mobility and steerability. Maneuverability includes both the degreesof freedom that the robot manipulates directly through wheel velocity and the degrees of freedomthat it indirectly manipulates by changing the steering configuration and moving.
7
Question 3:Compute the accessibility space for the following representative kinematic models after havingre-obtained them from the original kinematic equations
• Unicycle (ideal -single wheel) (U)
• Car-like front-driven (FD); or alternatively:
• Car-like rear-driven (RD)
• Rhombic-like vehicles (RLV)
Solution:A. Unicycle (ideal - single wheel) (U):A unicycle is a vehicle with a single orientable wheel.
Figure 1:
Its configuration can be described by three generalized coordinates: the Cartesian coordinates (x,y)of the contact point with the ground, measured in a fixed reference frame; angle θ characterizingthe orientation of the disk with respect to the x axis.The configuration vector is thereforeq = [xyθ ]T . The kinematic constraint of unicycle is derivedfrom the rolling without slipping condition, which forces the unicycle not to move in the directionorthogonal to the sagittal axis of the vehicle. The angular velocity of the disk around the verticalaxis instead is unconstrained.
.x sinθ−
.y cosθ = 0
=⇒[
sinθ −cosθ 0] .
q= 0
This equation is nonholonomic, because it implies no loss of accessibility in the configurationspace. Now, here the number of generalized coordinate, n = 3 and the number of non-holonomic
8
constraints, k = 1. So the number of null space of kinematic constraints in the case of unicycle,m = n− k = 2. Thus the matrix:
G(q) = [g1(q) g2(q)] =
cosθ 0sinθ 0
0 1
,where, columns g1(q) and g2(q) are for eachq a basis of the null space of the matrix associatedwith the Pfaffian constraint.
The kinematic model of the unicycle is then
.q=
.x.y.
θ
=
cosθ
sinθ
0
v+
001
ω,
where the input v is the driving velocity and ω is the wheel angular speed around the vertical axis.The Lie bracket of the two input vector field is
[g1,g2](q) =
sinθ
−cosθ
0
,that is linearly independent from g1(q), g2(q). The Accessibility space, S= rank
[g1 g2 [g1,g2]
]=
3. This indicates that there is no loss in the accessible space and the constraint is non-holonomic.
9
Figure 2:
B. Car-like front-driven (FD):A bicycle vehicle consists of an oreintable wheel and a fixed wheel.
The generalized coordinates can be expressed by the following equation:
q =[
x y θ Φ
]T, where (x,y) are the cartesian coordinates of the contact point between
the rear wheel and the ground, and θ is the steering angle of the front wheel with respect of thevehicle. The kinematic model is derived from the two pure rolling constraints. The velocity of thecentre of the front wheel is zero in the direction orthogonal to the wheel itself, while the velocityof the centre of the rear wheel is zero in the direction orthogonal to the sagittal axis of the vehicle.Now using the rigid body constraint,
x f = x+ lcosθ
y f = y+ lsinθ
where, l is the distance between the wheels and (x f ,y f ) is the cartesian position of the centre ofthe front wheel. So two constraints are following:
.x sinθ−
.y cosθ = 0 and
.x f sin(θ +Φ)− .
y f cos(θ +Φ) = 0
⇒ .x sin(θ +Φ)−
.y cos(θ +Φ)− l
.θ cosΦ = 0
The matrix associated with the Pfaffian constraints is then:
AT (q) =
[sinθ −cosθ 0 0
sin(θ +Φ) −cos(θ +Φ) −lcosΦ 0
]withrank 2
10
Here the number of generalized coordinate, n = 4 and the number of non-holonomic constraints,k = 2. So the number of null space of kinematic constraints in the case of unicycle, m = n−k = 2.Thus the matrix,
G(q) = [g1(q) g2(q)] =
cosθcosΦ 0sinθsinΦ 0sin(Φ/l) 0
0 1
where, columns g1(q) and g2(q) are for eachq a basis of the null space of the matrix associatedwith the Pfaffian constraint. The kinematic model of the unicycle is then,
.q=
.x.y.
θ.
Φ
=
cosθcosΦ
sinθsinΦ
sin(Φ/l)
0
v+
0001
ω
where, the input v is the driving velocity of front wheel and ω is the steering velocity. Now the Liebracket,
g3(q) = [g1,g2](q) =
cosθcosΦ
sinθsinΦ
−cos(Φ/l)
0
g(q) = [g1,g3](q) =
−sin(θ/l)
cos(θ/l)
00
both linearly independent from g1(q) and g2(q). So, the Accessibility space, S= rank
[g1 g2 g3 g4
]=
4.
11
C. car like rear-driven (RD)
The robot is rear driven for a possible generalized coordinates of [ ] T where (x, y) are
the Cartesian coordinates of the contact point between the rear wheel and the ground (i.e., of the
rear wheel Centre), θ is the orientation of the vehicle with respect to the x axis, and φ is the
steering angle of the front wheel with respect to the vehicle.
Front wheel center is
and
Applying rolling without slipping condition on the front axle/wheels,
( ) ( )
Substituting the above equations,
( ) ( ) ( ) ( )
Solving for gives,
The complete kinematic model is given by,
[
]
[
]
v + [
]
This is the complete kinematic model of Car like rear driven (RD) mobile robot where v is the
driving velocity of rear wheel whereas is the steering velocity.
So from the computed result g3(q) and g4(q) vector fields are independent of g1(q) and g2(q).
This implies that the accessible space = Rank[g1,g2,g3,g4] = 4
D. rhombic like vehicles (RLV)
In the configuration of RLV using the assumption of rolling without slipping condition the
system is subjected to the following constraints:
( ) ( )
( ) ( )
Using rigid body constraints, can be obtained as the function of center mass .thus
[ ], C (q) = 0
[ ( ) ( ) ( ) ( )
] [
]= 0
is the basis of {N(C (q))} = ∑ ( )
Null space of C(q) :g1(q) =
[ ( )
( )
]
, g2(q)=
[ ( )
( )
]
[ ( )
( )
]
+
[ ( )
( )
]
This is the kinematic model of RLV
Accessible space = rank [g1, g2,[g1,g2]]
[ ]
= [
( ( ) ( ) )
( )
]
Since [g1,g2] is independent of g1 and g2, accessible space = 3.
Advantages of pfaffian constraint:
Pfaffian constraints are linear in generalized velocities:
( )
This gives us the advantages of expressing the kinematic model as the combination of null space
of the constraint: in this case the kinematic model is drifless system.
Question 4:
MATLAB exercise - path/trajectory planning and potentialsMotion planning: It is the process of breaking down a desired movement task into discrete motionsthat satisfy movement constraints and possibly optimize some aspect of the movement as well asavoid obstacles. In the original package given to us in order to move the robot from a given initialpoint to goal point, a motion planning algorithm is developed for random environment map andminimum time path is computed using constrained optimization. The motion planning algorithmgenerates path/trajectory based on polynomial interpolation of waypoints from starting position togoal position. The minimum time path is computed by constrained optimization of the path gener-ated based on the waypoints randomly chosen. This optimization algorithm provides the optimalwaypoints between starting position and goal. But, the package doesn’t consider about the pres-ence of obstacles when making the environment and so, the final path/trajectory is always straightline connecting the two points.
Modification:
• the mapping environment is structured
• known position of obstacles are randomly imposed.
0 5 10 15 20 25 30 35
−5
0
5
10
15
20
25
−1
−0.8
−0.6
−0.4
−0.2
0
0.2
0.4
0.6
0.8
1
Figure: Environment map with obstacles, indicating the starting point and end point
15
0 5 10 15 20 25 30 35
−5
0
5
10
15
20
25
0
0.05
0.1
0.15
0.2
0.25
0.3
0.35
0.4
0.45
Figure: Search note
0 5 10 15 20 25 300
5
10
15
−1
−0.8
−0.6
−0.4
−0.2
0
0.2
0.4
0.6
0.8
1
Figure: Optimal points on the map
0 5 10 15 20 25 30 35
−5
0
5
10
15
20
25
0
0.05
0.1
0.15
0.2
0.25
0.3
0.35
0.4
0.45
Figure: Optimal path
16
Question 04 (Extension of Path Planning)Iterative artificial potential field (APF) algorithm APF algorithm is built upon new potentialfunctions based on the distances from obstacles, destination point and start point. The algorithmuses the potential field values iteratively to find the optimum points in the workspace in order toform the path from start to destination. The number of iterations depends on the size and shape ofthe workspace.
In this algorithm the map is assumed to be captured by camera mounted on the robot. For thesake of simulation environmental map is generated by using customized function and changed intoenvironmental map. The obstacles repel the robot with the magnitude inversely proportional tothe distances. The goal attracts the robot. The resultant potential, accounting for the attractiveand repulsive components is measured and used to move the robot. five distances are measured atspecific angles to to compute the repulsive potential. These are are forward ,left side right side, leftdiagonal and forward right diagonal.
Our method involves using a simple potential functions; the workspace is discretized into a grid ofrectangular cells where each cell is marked as an obstacle or a non-obstacle. At each cell potentialfunction is evaluated based on the distances from the starting point, destination and goal. Thesevalues are used to find the optimum points along the entire path. We find these points iterativelyuntil there are enough points that path can be determined as a consecutive sequence of these pointsbeginning from the start location and ending at the destination.
each cell by its coordinates c = [x, y]. Algorithm begins calculating the potential function for everyempty cell in the workspace. UT otal(c) = UStart(c) +UEnd(c)UObs(c) It is important to note thatthe distance of the cell from the start point is being used. The individual functions are expressed asUStart(c) = α/D(c, start)UEnd(c) = α/D(c, End)UObs(c) = β/D(c,Obs)D(c, start) is the distance of the cell c from the start point D(c, End) is the distance of cell c fromthe end point D(c, Obs) is the distance of cell c from the Obstacle point
α and β are positive constant to change the path behavior. As it can be observed from potentialfunction by increasing alpha we put more emphasis on the distance from the start and end points.So having large value of alpha results in shorter path but the path might be close to the obstacle.By increasing beta we put more emphasis on the distance from obstacles and it means that selectinglarge value for gives us a longer path with bigger distance from the obstacles. For this simulationresult in the documentation unit value of alpha and beta. Pseudo code for finding the midpoint LetN be the number of available cells, Evaluate all these cells. A = Sorted array of all cells values.Binary Search( 1, N , A );
Binary Search( i, j ,A)If ( i == j )return A[ i + 1]T = A[ ( i + j ) / 2]If ( by using simple BFS, Is end point reachable from start point using cells with larger value thanT ? )Binary Search( i, (( i + j ) / 2 ) - 1 , A )ElseBinary Search( ( i + j ) / 2, j, A )Pseudo code for finding the pathInputs = Start, Destination, WorkspaceOutput = Collision free pathFunction Find Path (Start, End, Workspace)If ¡ Endpoints are close enough and there is a collision free straight line connecting them ¿Return Segment( Start, End );Else
[ MidPoint, First Workspace, Second Workspace] = Find MidPoint( Start, End, Workspace )
1
First Path = Find Path ( Start, MidPoint, First Workspace )Second Path=FindPath( MidPoint, End, Second Workspace )Return Merge( First Path, Second Path )Function Find MidPoint( Start, Destination, Workspace )Evaluate all the cells in the WorkspaceA = Sorted array of all available cells in Workspace in descending orderN = Length of AT = Binary Search( 1, N , A )Midpoint = Cell having the value TFirst Workspace = Start point clusterSecond Workspace = End point clusterReturn ( Midpoint, First Workspace, Second Workspace )
Figure 1: environmental map
Figure 2: simulation result with appropriate value of alpha and beta
Figure 3: simulation result with appropriate value of alpha and beta
2
Figure 4: smaller safe distance and imbalance between repulsive and attractive force results incollision of robot with obstacles
Figure 5: larger α value results in longer time toconverge to the goal
Figure 6: simulation result with larger value of β
In conclusion the result demonstrates that the appropriate value of α and β should be selectedto achieve the desired path. The thread off between this parameters , attractive and repulsivepotential values results the shortest path to goal avoiding the obstacles.
3
Question 5:
5.1 Produce a “sufficiently smooth” path planning and trajectory?
Trajectory planning involves path planning with association of timing law.
Assuming the trajectory to be planned q (t), for t ∈ [ti , tf], that leads a car like robot from an initial
Configuration q(ti ) = qi to a final configuration q(tf) = qf in the absence of obstacles. The trajectory
q(t) can be broken down into a geometric path q(s), with dq(s)/ds = 0 for any value of s, and a
timing law s=s(t), with the parameters varying between s(ti)=si and s(tf)=sf in a monotonic fashion,
i.e., with s(t) ≥0, for t ∈[ti, tf]. A possible choice for s is the arc length along the path; in this
case, it would be si = 0 and sf =L, where L is the length of the path.
Using space time separation:
�� = 𝑑𝑞
𝑑𝑡=
𝑑𝑞
𝑑𝑠�� = 𝑞′��
And the non-holonomic constraints of the rear driven car like robot is given by
𝐴(𝑞) ∗ �� = 𝑞′��
Geometrically admissible paths can be explicitly defined as the solutions of the nonlinear system
𝑞′ �� = 𝐺(𝑞) ∗ �� , 𝑢 = ��(𝑡)��
In this project the path planning via Cartesian polynomial is adopted. The problem of planning can
be solved by interpolating the initial values xi and yi and the final values xf and yf of the flat
outputs x, y. The expression for other states and input trajectory depend only on the values of
output trajectory and its derivative up to third order. In order to guarantee it’s exact
Reproducibility, the Cartesian trajectory should be three times differentiable almost everywhere.
Cubic polynomial function is given as shown below,
5.3 Define a proper output function for this MIMO non holonomic system and motivate?The RLV kinematic model,
.x.y.
θ.
φ
=
cosθ
sinθtanφ
l0
v+
0001
w
This has the form of.x= f (x)+G(x)u in which f (x) = 0
The dynamic system is differentially flat since it is possible to re-write it as follows:
.θ=
..y .x− ..
x.y(√
.x2
+.y2)2 =
..y..x− ..
x.y
.x2
+.y2
.φ=
l√
.x2
+.y2(...
y .x− ...
x.y)(
.x2
+.y2)−3(
..y .x− ..
x.y)(
.x..x +
.y..y)
(.x2
+.y2)3
+ l2(..
y .x− ..
x.y)2
v =±√
.x +
.y
w =.
φ
So, the states.
θ ,.
φ and the inputs v and w can be expressed as the function of x and y and theirderivatives. This implies the output function are x and y.
z =
[xy
]
25
5.4. Apply the input output linearization feedback by computing the linearizing feedback via lie
algebra from new coordinates z. rewrite the equation in the old set of generalized coordinates q.
For the car-like robot model, the natural output choice for the trajectory tracking task is
𝑧 = [𝑥𝑦]
The linearization algorithm starts by computing
𝑧 = [𝑐𝑜𝑠𝜃 0𝑠𝑖𝑛𝜃 0
] 𝑣 = 𝐴(𝜃)𝑣
At least one input appears in both components of ��, so that A(𝜃) is the actual decoupling matrix
of the system. Since this matrix is singular, static feedback fails to solve the input-output
linearization and decoupling problem.
A possible way to circumvent this problem is to redefine the system output as
,
With ∆≠ 0. The differentiation of new output results in
�� = 𝐴(𝜃, ∅) [𝑣𝜔
]
Since 𝑑𝑒𝑡𝐴(𝜃, ∅) = ∆/𝑐𝑜𝑠∅ ≠ 0 we can set �� = 𝑟 (as an auxiliary input value) and solve for the
input v as, 𝑣 = 𝐴−1(𝜃, ∅)𝑟
Setting �� = [𝑧1
𝑧2
]=[
𝑟1𝑟2
] = R 𝑅 = 𝐴(𝜃, ∅) [𝑣𝜔
]
Computing the linearizing feedback using lie algebra
𝑢 =1
𝐿𝑔𝐿𝑓𝑟−1ℎ(𝑥)
[−𝐿𝑓𝑟−1ℎ(𝑥) + 𝑣]
By replacing the f(x) = 0, V = R, r = Relative degree = 1
𝑢 =1
𝐿𝑔ℎ(𝑥) [𝑅] 𝐿𝑔ℎ(𝑥) =
𝜕ℎ
𝜕𝑥𝑔 =
𝜕𝑧
𝜕𝑥𝑔 Where 𝑔 =
[ 𝑐𝑜𝑠𝜗𝑠𝑖𝑛𝜗𝑡𝑎𝑛∅
𝑙
0 ]
∂ z∂x
=
[∂ z1∂x
∂ z1∂y
∂ z1∂θ
∂ z1∂φ
∂ z2∂x
∂ z2∂y
∂ z2∂θ
∂ z2∂φ
]=
[1 0 −(lsinθ +4sinθ +φ)) −4sin(θ +φ)
0 1 lcosθ +4cos(θ +φ) 4cos(θ +φ)
]
∂ z∂x
g =
[cosθ − tanφ(sinθ + 4sin(θ+φ)
l ) −4sin(θ +φ)
sinθ + tanφ
(cosθ + 4sin(θ+φ)
l
)4cos(θ +φ)
]
U = A−1(θ ,φ)R = A−1(θ ,φ)
[r1
r2
]=
[vw
]The closed loop system defined in the transformed coordinates (z1,z2,θ ,φ) is
.z1= r1
.z2= r2
.θ=
sinφ [cos(θ +φ)r1 + sin(θ +φ)r2]
l
.φ=−
[cos(θ +φ)sinφ
l+
sin(θ +φ)
4
]r1−
[sin(θ +φ)sinφ
l− cos(θ +φ)
4
]r2
where,.
θ=vtanφ
l and φ = wThe equations in the old set of generalized coordinates:
.x= vcosθ = (r1cos(θ +φ)+ r2sin(θ +φ)cosφ)cosθ
.y= vsinθ = (r1cos(θ +φ)+ r2sin(θ +φ)cosφ)sinθ
.θ=
vtanφ
l= (r1cos(θ +φ)+ r2sin(θ +φ)cosφ)
tanφ
l
.φ= w =−
[cos(θ +φ)sinφ
l+
sin(θ +φ)
4
]r1−
[sin(θ +φ)sinφ
l− cos(θ +φ)
4
]r2
27
5.5 Introduce Perturbation and try to set control minimizing e(t ) = yd(t)-y(t). Explain why your
Control and optimization is working properly.
The answer considers the problem of tracking a given Cartesian trajectory with the car-like robot
using feedback control.
Reference trajectory generation
Assume that a feasible and smooth desired output trajectory is given in terms of the Cartesian
position of the car rear wheel;
𝑧𝑑(𝑡) = ⌊𝑥𝑑 = 𝑥𝑑(𝑡)
𝑦𝑑 = 𝑦𝑑(𝑡)⌋ 𝑡 ≥ 𝑡0 Where 𝑧𝑑(𝑡) is desired/reference trajectory
This natural way of specifying the motion of a car-like robot has an appealing property. In fact,
from this information we are able to derive the corresponding time evolution of the remaining
coordinates (state trajectory) as well as of the associated input commands (input trajectory).
Let us assume 𝑧(𝑡) = ⌊𝑥 = 𝑥(𝑡)
𝑦 = 𝑦(𝑡)⌋ is the robot trajectory, then the error between desired trajectory
and the trajectory tracker error is given by
𝑒(𝑡) = ⌊𝑥𝑑(𝑡) − 𝑥(𝑡)
𝑦𝑑(𝑡) − 𝑦(𝑡)⌋
For demonstration of the perturbation model four different reference trajectories (linear, elliptic,
circular and sinusoidal) is selected and the controller for minimizing the error is designed.
The controller design for trajectory tracking is based on the linearization of the dynamic system
without ignoring the non- linearities in the system. These non-linearities are very important to be
ignored. But by selecting proper outputs (x,y in this case) the non-linearities are canceled between
Input and Output by feedback Linearization.
So, Input- Output feedback linearization is deployed to find the controller that minimizes the error.
Input output linearization algorithm starts with
�� = ⌊����
⌋ = ⌊𝑐𝑜𝑠𝜃 0𝑠𝑖𝑛𝜃 0
⌋ [𝑣𝜔
] = 𝐴(𝜃) [𝑣𝜔
]
Since the decoupling matrix is singular the output is defined as follows to circumvent the problem.
𝑧 = [𝑧1
𝑧2] = [
𝑥 + 𝑙𝑐𝑜𝑠𝜃 + ∆cos (𝜃 + ∅)
𝑦 + 𝑙𝑠𝑖𝑛𝜃 + ∆sin(𝜃 + ∅)], ∆≠ 0
�� = [��1
��2] = [
𝑟1
𝑟2] From this formulation the control that minimizes the perturbation error is given
by
[𝑣𝜔
] = 𝐴−1(𝜃, ∅) [𝑟1
𝑟2] Where A(𝜃, ∅) is decoupling matrix.
This control input solves the trajectory tracking problem is computing by choosing
𝑟1 = ��𝑑1 + 𝐾𝑝1(𝑧𝑑1 − 𝑧1) = ��𝑑 + 𝐾𝑝1(𝑥𝑑 − 𝑥)
𝑟2 = ��𝑑2 + 𝐾𝑝2(𝑧𝑑2 − 𝑧2) = ��𝑑 + 𝐾𝑝2(𝑦𝑑 − 𝑦)
A nonlinear internal dynamics which does not affect the input output behavior may be left in the
closed-loop system. If the internal dynamics are stable i.e. the states remain bounded during
tracking (implies stability in the BIBO) the trajectory tracking problem is supposed to be solved.
Similarly the states (𝜃, ∅) associated with internal dynamics are bounded along the nominal output
trajectory.
The output trajectory tracking error converges to zero due to system degree/order of two.
−5 0 5−10
−8
−6
−4
−2
0
2
4
6
8
10Trajectory Tracking
X
Y
Robot TrajectoryReference Trajectory
Figure: Ellipse Trajectory Tracking
0 2 4 6 8 10
−1
0
1
2
3
4
5
6
Trajectory Tracking Errors
Time[s]
Leng
th[m
]
Error on XError on Y
Figure: Ellipse Trajectory Tracking Errors
30
0 2 4 6 8 100
1
2
3
4
5
6
7
8
9
10Trajectory Tracking
X
Y
Robot TrajectoryReference Trajectory
Figure: Line Trajectory Tracking
0 2 4 6 8 10
−3
−2
−1
0
1
2
3
Trajectory Tracking Errors
Time[s]
Leng
th[m
]
Error on XError on Y
Figure: Line Trajectory Tracking Errors
31
−5 0 5−5
−4
−3
−2
−1
0
1
2
3
4
5Trajectory Tracking
X
Y
Robot TrajectoryReference Trajectory
Figure: Circle Trajectory Tracking Errors
0 2 4 6 8 10
−1
0
1
2
3
4
5
6
Trajectory Tracking Errors
Time[s]
Leng
th[m
]
Error on XError on Y
Figure: Circle Trajectory Tracking Errors
32
0 2 4 6 8 10−1
−0.8
−0.6
−0.4
−0.2
0
0.2
0.4
0.6
0.8
1Trajectory Tracking
X
Y
Robot TrajectoryReference Trajectory
Figure: Sinusoid Trajectory Tracking
0 2 4 6 8 10
−3
−2
−1
0
1
2
3
Trajectory Tracking Errors
Time[s]
Leng
th[m
]
Error on XError on Y
Figure: Sinusoid Trajectory Tracking
33
Question 6:
Introduce the concept of “zero dynamics” and “constrained dynamics”, explain?
Solution:Zero Dynamics: A geometric task for a robot may be encoded into a set of outputs in such away that the zeroing of the outputs is asymptotically equivalent to achieving the task, whetherthe task be asymptotic convergence to an equilibrium point, a surface, or a time trajectory. For asystem modeled by ordinary differential equations (in particular, without impact of dynamics), themaximal internal dynamics of the system that is compatible with the output being identically zerois called the zero dynamics.Consider a nonlinear system whose relative degree r is strictly less than n. The single-input single-output system:
x = f (x)+g(x)u and y = h(x)
Assume that a local coordinate transformation has placed this system into its normal form:.z1= z2
..z2= z3
....zr−1= zr
.zr= b(ξ ,η)+a(ξ ,η)u
.η= q(ξ ,η)
and the output is given byy = z1
Equations which describe the system in the new coordinates can be more conveniently representedas follows:
η =[
zr+1 zr+2 . . . zn
]T; ξ =
[z1 z2 . . . zr
]T
We can show that if x0 is such that f (x0) = 0 and h(x0) = 0, then the first r new coordinates of zi are0 at x0. In order to have the output y(t) of the system identically zero, necessarily the initial statemust be such that ξ (0) = 0 , whereas η(0) = η0 at x0. So if x0 were an equilibrium point for thesystem (in the original coordinate frame), it’s corresponding point (ξ ,η) = (0,0) is an equilibriumpoint for the system in the new coordinates. Which means that, 0 = b(0,0)+a(0,0)u.With these observations, according to the value of η0, the input must be set equal to the followingfunction u(t)= b(0,η(t))
a(0,η(t)) , where η(t) denotes the solution of the differential equation.
η= q(0,η(t)).This correspond to the dynamics describing the internal behavior of the system when input andinitial conditions have been chosen in such a way as to constrain the output to remain identicallyzero. These dynamics, which are rather important in many instances, are called the zero dynamicsof the system.
34
The concept of constrained dynamics describes the fact that we choose the initial condition and theinput so that we can constrain the output identically to zero.
6.1 Do it exist a “strictly normal form” for the generalized (be concise: one short-sentence)?It is not exist as the jacobian matrix is non-singular.
6.2 Why the trajectory has to be C∞ (or sufficiently smooth) in respect to the generalized coor-dinates?To avoid the non-continuous curvature behavior the trajectory has to be planned in sufficientlysmooth way with respect to generalized coordinates. Lack of smoothness results in physicallyunachievable maneuvering and motion inversion. This forces the robot to move forward and back-ward. To overcome this problem the trajectory has to be sufficiently smooth.
6.3 Why do not apply the state-space feedback realization (SSFL)?Rear driven Car like mobile robot system has four state variables(x,y,θ ,φ) while the input com-mands are v and w (driving velocity and steering velocity).due this ranking difference betweeninput commands and state variable of system it is impossible to use state-space feedback realiza-tion (SSFL).For a system of x = f (x)+g(x)u ,The necessary and sufficient condition for existence of state feedback linearization is g(x) matrixhave rank n and involute around x0. In fact car like mobile robots are drift less system, whichimplies that f (x) = 0. For car like mobile robots in general rank g(x0) = 2, which is not the sameas the rank of the system. Therefore SSFL is not possible.