辅导案例-CMPT 419/983-Assignment 2
CMPT 419/983 Assignment 2 Due date: October 28, 2019 Submit zip file to CourSys 1) Sequential Quadratic Programming. Use Sequential Quadratic Programming to solve the NLP, introduced in class. You may use any software to solve the quadratic subproblems. (The solution will be given in cvx). minimize sin() sin(2) subject to −1 ≤ ≤ 1 −1 ≤ ≤ 1 a) Construct a quadratic subproblem that minimizes the quadratic approximation of the objective, subject to linearization of the constraints, centred around a given iterate . b) Write code to solve the above quadratic subproblem using cvx. c) Write code to solve the entire NLP, starting from several different initial points . Make a plot showing several sequences of {} starting these initial points. 2) Differential Flatness. Consider the following simple model of the car (that is slightly more complex than the one introduced in class: ̇ = cos ̇ = sin ̇ = ̇ = The states consist of the position (, ), the heading , and the longitudinal speed . The controls are the turn rate and the longitudinal acceleration . a) Show that the system is differentially flat by letting = (, ), and deriving the functions and such that (, , , ) = ൫, ̇, … , ()൯, (, ) = ൫, ̇, … , ()൯. b) Consider a maneuver given by the following initial condition at = 0 and final condition at = = 10: ⎣ ⎢ ⎢ ⎡ (0) (0) (0) (0)⎦ ⎥ ⎥ ⎤ = ൦ 0 0 0 1 ൪ , ⎣ ⎢ ⎢ ⎡ () () () ()⎦ ⎥ ⎥ ⎤ = ⎣ ⎢ ⎢ ⎢ ⎡ 0 0 2 1⎦ ⎥ ⎥ ⎥ ⎤ Using the basis functions () = 1, ଵ() = , ଶ = ଶ, ଷ = ଷ, ସ = ସ, we parameterize the flat outputs as follows: () = () ଷ ୀ () = ଵ() ଷ ୀ Write a system of equations in terms of the coefficients {} and {ଵ} such that the initial and final conditions are satisfied. c) Solve the above systems of equations using a software of your choice to obtain and plot the state and control trajectories. 3) Multiple Shooting with Casadi. Consider a system involving an inverted pendulum on a cart. For this system, the state is (, , , ), representing the position of the cart, speed of the cart, angle of the pendulum, and angular speed of the pendulum, respectively. The initial state at time = 0 is (0,0,0,0). The problem parameters are as follows: = 0.5 kg is the mass of the cart, = 0.2 kg is the mass of the pendulum, = 0.3 m is the half-length of the pendulum, = 0.006 kg∙m2 is the moment of inertial of the pendulum. We would like to apply a force () such that at time = 5 s, the cart is stopped at the origin, with the pendulum being stationary at the upright position, represented by the state (0,0, , 0). At the same time, we would like to minimize the control effort, given by the integral ∫ ଶ()ହ௧ୀ , while ensuring that the position of the cart stays within 1 m of the origin, || ≤ 1. The control is also limited by |()| ≤ 0.2 at all times. The equations of motion are given by ( + )̈ + ̇ + ̈ cos − ̇ଶ sin = ( + ଶ)̈ + sin = −̈ cos where = 0.1 N/m/s is the coefficient of friction, and = 9.81 m/s/s is the acceleration due to gravity on Earth. In terms of a first-order ODE, we have ̇ = , ̇ = . The rest of the system dynamics, for (, ), can be implicitly written as ቂ + cos cos + ଶ ቃ ቂ ̇ ̇ቃ = − + ଶ sin + − sin ൨ a) Write down the associated optimal control problem. b) Discretize the optimal control problem using the multiple shooting method and write down the resulting nonlinear program. Use = 50 time intervals, forward Euler integration, and left endpoint first order integration (the same schemes as presented in the lecture). c) Using the Casadi toolbox, which can be found at https://github.com/casadi/casadi/wiki, solve the nonlinear program, and plot ((), (), (), ()), and (). A good starting point is the direct multiple shooting example at https://github.com/casadi/casadi/wiki/examples. 4) Reachability Analysis. Consider a kinematic model of a fixed-wing aircraft flying in the presence of wind, given as follows: ̇ = cos + ௫ ̇ = sin + ௬ ̇ = + ఏ The state (, , ) consists of - and - position, and heading . The aircraft flies at a constant speed = 0.3 km/s, and controls its turn rate , with || ≤ 0.5 rad/s. The effect of wind is modeled by the disturbance = ൫௫ , ௬ , ఏ൯ with |௫|, ห௬ห ≤ 50 m/s, and |ఏ| ≤ 0.005 rad/s. The aircraft aims to arrive at its next waypoint at = 2 km, = 2 km, = ଷగ ଼ , with an acceptable tolerance of 200 m in distance and గ ଵ in heading. We now proceed to compute all possible initial states and times that can reach the setpoint within tolerance at time = 0. a) What is the target set representing the acceptable set of states for reaching the waypoint within the tolerances? What is a suitable function (, , ) such that (, , ) ≤ 0 ⇔ (, , ) ∈ ? b) The value function (, , , ) representing the set of states from which the aircraft can reach the target set within seconds is the solution to the HJI variational inequality min ൜ (, , , ) + min ௨∈ max ௗ∈ (, , , )ୃ(, , , , ) , () − (, )ൠ = 0 where (, , , , ) is given by the system dynamics. Determine the optimal and optimizes the min-max expression. That is find an analytic expression for ∗ and ∗, where ∗(, , , ) = arg min ௨∈ max ௗ∈ ∇(, , , )ୃ(, , , , ) ∗(, , , ) = arg max ௗ∈ ∇(, , , )ୃ(, , , ∗, ) and ∈ and ∈ represents the bounds on the control and disturbances. c) Compute (, , , ) from = −10 to = 0 using the helperOC toolbox, which can be found at https://github.com/HJReachability/helperOC. A good starting point is tutorial.m. Use the visSetIm and proj functions to visualize the following: ( = −10, , , ), ( = −5, , , ), ( = −10, , , = 0), ( = −10, , = 0, ). For your computation, use the following recommended grid bounds and resolution: ∈ [−2,5] with 45 grid points, ∈ [−2.5,4] with 45 grid points, ∈ [−, ] with 35 grid points.