Final Solution

Your Name and Student ID:










Please answer all questions. Make sure to review the Exam Instructions on bCourses before start-ing the exam. Before each answer report the question number you are answering: 3.a.ii means problem 3, question (a), part ii.










Problem: Max Score Score




50



5



5



20



20






Total 100







































































































1 of 9
Available: ——- Due: ——-










Navigation for Parking



Note this problem seems long, but it is a “simple” navigation problem. You can solve it by starting from the yalmip code I have provided to you. The description is long because I want to make sure you understand the model.







We will consider the following simplified geometric model of a car. Note: this model is ‘simpler’ than the kinematic bicycle model used in class.
































































The above figure depicts a car in a global cartesian coordinate space. The vehicle is characterized by its position and heading, [x, y,✓ ], where x and y are the coordinate of the center of the rear-axel and ✓ is the heading relative to the x-axis. The distance between rear and front axel is L.




If the car moves of a distance d while steering its front wheel with an angle δ, we have that the turning radius is

R =


L








(1)
tan δ




















and the turn angle is
d










d




β =
tan δ =


.
(2)
L












R





In the next figure the vehicle and its center of turn (with coordinates xc, yc) are depicted, the steering δ is denoted by ↵ in the next figure.























































2 of 9
Available: ——- Due: ——-



























































































The coordinates of center of turn can be easily computed from the figure as


xc = x R sin ✓
(3)
yc = y + R cos ✓.





The new heading of the car after the turn can be obtained by adding β and original heading ✓. The new coordinates [x+, y+] of the car after the turn are




x+ = xc + R sin(✓ + β)


y+ = yc R cos(✓ + β)
(4)
✓+ =(✓+β).


In conclusion combining (3) and (4) our vehicle model can be rewritten as


xk+1 = xk Rk sin(✓k) + Rk sin(✓k + βk)


yk+1 = yk + Rk cos(✓k) Rk cos(✓k + βk)
(5)
✓k+1 = ✓k + βk





where the vehicle state at time step k is zk = [xk, yk, ✓k] (coordinates of center real-axel and heading angle) and the inputs are uk = [Rk, βk] (turn radius and turn angle). Model (5) will be compactly rewritten as

zk+1 = fcar(zk, uk).
(6)



A few remarks about the model:




A straight line motion corresponds to an infinite turn radius Rk (i.e., 0 steering angle).




Numerically you will observe ‘almost’ straight lines when Rk is very big.




For a given input uk = [Rk, βk] the traveled distance by the car (from eq. 2) at step k is |Rk · βk|.







3 of 9
Available: ——- Due: ——-










If the vehicle goes at constant speed the length of the time steps is nonuniform. This means that at time step k you can choose to travel a short distance |Rk · βk| or a long distance |Rk · βk|. In other words, the model is event-based. Nothing changes in what you have learned and in the code you have worked on in class.







Cars have a min and max steering angle δ 2 [δmin, δmax]. Often δmin is negative and δmax = δmin. This implies that in terms of constraints on the turning radius we have that




R
L
OR R


L
. This “OR” constrains will be simplified later. Steering
tan(δmin)
tan(δmax)
right means R < 0, left R 0.




I have shared with you the function “car plot”. It plots the vehicle trajectory and shows its motion. Try in MATLAB:







car plot([10,−10;pi, pi/4],[0;0;0])







it plots the car trajectory with z0 = [0; 0; 0], u0 = [10; ⇡], u1 = [ 10, ⇡ /4], note that in the second part of the maneuver the car is moving backward (assuming you can use a reverse gear). The front wheels are colored in red.




The syntax is car plot(U,z0) where z0 is a 3 ⇥ 1 vector of initial conditions, U is a 2 ⇥ p matrix collecting a generic input sequence [u1, u2, ...up].




Note that the simulation of the model (6) with z0 = [0; 0; 0], u0 = [10; ⇡], u1 = [ 10, ⇡ /4] gives as output the two states z1, z2. The state sequence z0, z1, z2 are in depicted with the green markings (a point and an arrow) by the function car plot().







In order to show a nice trajectory, the function car plot() plots a number “rep” of points between zi and zi+1. You can change the variable “rep” in the function car plot(). You can decrease it if the animation takes too long, increase it to see smoother curves.







You are asked to use model (5) to formulate and solve a parking problem as a finite-time optimal control problem formulated as follows:






k=N−1












Xk
k ·


)2




min
(R
β




z0,...,zN ,u0,...,uN−1
=0
k




















zk+1 = fcar(zk, uk)
8k = {0, . . . , N
1}


zmin zk zmax
8k = {0, . . . , N}
(7)


umin uk umax
8k = {0, . . . , N
1}


zk(1 : 2) 62Obstacle(m)
8k = {0, . . . , N} and 8m = {1, . . . , Nobs}


z0 = z¯0












zN = z¯N













The vehicle starts from the initial state z¯0. Our goal is to park the vehicle in the terminal state z¯N while minimizing total traveled distance.




Settings:




Assume the steering command is broken and you can steer only to the left (R 0) – this gets rid of the ‘OR’ constraint.




Set Rmin =
L
with L = 2.8 and δmax = 25⇡/180. Set βmin = ⇡ and βmax = ⇡.
tan(δmax)




4 of 9



Available: ——-
 
Due: ——-


The min and max input constraints are umin = [Rmin; βmin] and umax = [+1; βmax].
The state has the constraints of [
20, 10,
2⇡]T z(k) [20, 20, 2⇡]T
Set z¯N = [0; 0; ⇡/2]







I have provided an .m code with all the parameters and three obstacles (Nobs = 3)







Note that the obstacle-avoidance constraint is formulated only for the point representing the coordinates of the center real-axel.







HERE ARE THE QUESTIONS




(a) Use z¯0 = [ 10; 10; 0], N = 3. Look at the the code:“parkingOptimalControl toshare.m” I provided you which does not include obstacle avoidance. Run the code and make sure you can park the car. No deliverables.







(b) Use z¯0 = [ 10; 10; 0]. Submit a code and a plot which shows parking while avoiding the three obstacles given in the code. Use the duality approach method. Submit the final plot generated by the function car plot() and your code.










Note: (1) You can pick the N and the solver you want to solve the problem as fast as you can. I solved it with N = 4 and ipopt (my solution below). You are not allowed to change the constraints or the method to encode obstacle avoidance constraint. (2) You might need to enforce constraints at a number of points p between zi and zi+1. (3) Do the best you can to have “some” obstacle avoidance, it is ok to hit obstacles because the car is modeled as a point mass or because the p is small for computational reasons.







































































































5 of 9
Available: ——- Due: ——-










Show what happens with an N larger than the one used at the previous point, submit the final plot generated by the function car plot().



Code now and solve the same problem in (b) by using the signed-distance formulation of Equation (13) of the document CollisionAvoidanceLatex final.pdf. Submit the final plot generated by the function car plot() and your code. NOTE: same comments as point (b) apply.



(ONLY IF YOU HAVE TIME, FINISH THE OTHER PROBLEMS FIRST) Elaborate on how to get rid of the assumption that the car can steer only to the left (R 0). Do not try to code the “OR constraints” in yalmip or with bigM, you would need an MINLP solver. Think of a simple approach and code it if you have time (submit the code and a plot showing that it works).



















































































































































6 of 9
Available: ——- Due: ——-










2. Examine (and run) the code below







1




2




3




4




5




begin code







G = tf([-5.8 -3.2 9],[0.02 0.98 4.8 9]); normTol = 1e-5;




[A,B] = norm(G,inf,normTol);




[A abs(freqresp(G,B))]




bodemag(G, frd(freqresp(G,B),B), ’ro’) end code




Explain what lines #3, #4, and #5 are doing.






3. Examine (and run) the code below






1




2




3




4




5




6




7




begin code







normTol = 1e-5;




delta = complex(randn,randn);




wBar = exp(2*randn);




D = cnum2sys(delta,wBar);




[freqresp(D,wBar) delta]




[norm(D,inf,normTol) abs(delta)]




bode(D)




end code



Explain what lines #4, #5, #6 and #7 are doing. Hint: Refer to slide 3 in the “WorstCase-UncertainSystemAnalysis” powerpoint file for a reminder about the behavior of cnum2sys







4. Transfer functions of plant P and controller C are given below






P (s) =


1
,
C(s) =


5.8s + 9




s
1
s(0.04s + 1)








(a)
Verify that C stabilizes P in a standard negative-feedback loop configuration
(b)
Consider additive perturbations to P of the form P + , where 2 C is a complex number.


Redraw the uncertain closed-loop system (ie., negative-feedback interconnection of P +


and C) as a positive feedback loop consisting of


and some system G (which depends only


on P and C). Determine G.















(c) Use the commands feedback, norm, freqresp, and basic arithmetic (eg., multiplication, addition, division) to find the smallest (in absolute value) value of such that the feedback connection of P + and C is unstable? Verify the instability using feedback and pole. Hint: Refer to slides 3-7 in the “WorstCaseUncertainSystemAnalysis” powerpoint file.




Duplicate the results in part 4c using a ucomplex uncertain element, and the commands feedback and robuststab (or robstab). Explicitly show that the results are the same. Explain in 2 sentences how the first two output arguments from robuststab (or robstab) confirm your own calculation in part (4c).






7 of 9
Available: ——- Due: ——-










(e) With some of the results from part 4c, use cnum2sys to find a stable transfer function ˆ such that ˆ = | | (where was obtained part 4c) such that the feedback connection

1

of P + ˆ and C is unstable? Verify the instability using feedback and pole.




Duplicate the results in part 4e using a ultidyn uncertain element, and the commands feedback and robuststab (or robstab). Explicitly show that the results are the same. Explain in 2 sentences how the first two output arguments from robuststab confirm your own calculation in part (4e). Note: Recall we established that for robust-stability questions, constant complex-valued uncertainty is equivalent to linear dynamic-system uncertainty.



(g) Now consider multiplicative perturbations to P of the form P (1 + ) , where 2 C is a complex number. Redraw the uncertain closed-loop system (ie., negative-feedback intercon-nection of P (1 + ) and C) as a positive feedback loop consisting of and some system G (which again depends only on P and C, and is di↵erent from that in (4b)). Determine G.




(h) Use the commands feedback, norm, freqresp, and basic arithmetic (eg., multiplication, addition, division) to find the smallest (in absolute value) value of such that the feedback connection of P (1 + ) and C is unstable? Verify the instability using feedback and pole.




Duplicate these results of part (4h) using a ucomplex (or ultidyn) uncertain element, and the commands feedback and robuststab (or robstab).



Consider the same nominal plant and controller data as in the previous problem



P (s) =


1
,
C(s) =


5.8s + 9
s
1
s(0.04s + 1)









In this problem we consider the response to output disturbances, so we will focus on the sensitivity function S := 1 , and variations of that due to uncertainty in the plant behavior.




1+P C




(a) Define


0.667s + 3










Wp :=




























s + 0.003






Show that kWpSk1 1, using the commands feedback and norm.
(b) Make a Bode magnitude (magnitude only) plot of S and


1
(on the same axis) to confirm




1
for all ! 2 R.






Wp
that |S(j!)|














|Wp(j!)|















(c) Consider 40% multiplicative (ie., percentage) uncertainty in P , modeled as Pu := P (1 + 0.4 ), where is any stable linear system, with k k1 1. Based on results from 4g, can you confirm that for all such Pu, the feedback connection of C and Pu is stable? Why?




(d) Again, consider 40% multiplicative (ie., percentage) uncertainty in P , modeled as Pu := P (1 + 0.4 ), where is any stable linear system, with k k1 1. Using commands ultidyn, feedback and wcgain, find the value of




max
Wp


1 + PuC 1
allowable



8 of 9
Available: ——- Due: ——-










and a specific stable linear system ¯ that achieves this worst-case performance.




(e) Plot the Bode-magnitude response of S, for both the nominal plant P , and the “worst-case” Pu on the same axis. Include a legend, and 1 as well. Does the plot confirm the worst-case




Wp

gain computed in part (5c)?




Plot the step response of S, for both the nominal plant P , and the “worst-case” Pu on the same axis. Include a legend. Make the final time TF = 4



Suppose the uncertainty model is not a constant 40% level at all frequencies, but rather it starts at 40%, and grows to 100% at ! = 20, and continues to grow to 400%. This can be modeled as








Pu = P (1 + Wu )
where
Wu =
4s+33.8
(using makeweight(0.4, 20, 400)) and is any stable linear system,
s+84.5


with k
k1 1.


Task 1: Is the closed-loop system robust to this uncertainty model?







Task 2: If the closed-loop is robustly stable, use wcgain to compute the worst-case




gain degradation for


Wp
.
1+PuC






1
Task 3: Plot the step response of S, for both the nominal plant P , and the “worst-case” Pu on the same axis. Include a legend. Make the final time TF = 4
















































































































9 of 9