In this assignment you will implement a set-point control algorithm for wheeled mobile robots. The control objective is for the robot to asymptotically converge to a reference target with coordinates (xr, yr). You will need to modify parts from two les: 1. main.m: This is the main MATLAB le that executes and operates the simulator. 2. controller.m: In this le you will insert the vehicles controller. 1 Part I: Set-point (waypoint) control In the rst part of this assignment you will apply a smooth feedback control law that guarantees the asymptotic convergence of the robot from any initial pose (x, y, ) to a reference point with coordinates (xr, yr). For this part, you will need to implement the feedback control law to the simulator and observe the response of the robot for dierent initial conditions. The kinematics of the mobile robot are described by the Dubins model. The robot kinematics are: x = v cos y = v sin = where: x: Position (North) y: Position (West) : Heading (Yaw) v: Advancement : Angular velocity 1 1.1 Part I: Procedures The following step describe the step to apply the control law to the simulator: 1. For starters you should dene the initial pose of the vehicle. Open the main simulator le main.m. The initial pose (x0, y0, 0) is set at line 14 of the le (variables: x_0, y_0 and psi_0). Use non-zero initial coordinates i.e. x0, y0 6= 0. 2. In the same le, you should also determine the reference point's coordinates (xr, yr). These coordinates are dened in line 21 of the main.m le. Make sure that (xr, yr) = (0, 0). The variable psi_r does not play any role in this assignment. 3. At this point you should insert the feedback control law. To do so, open the le controller.m. The inputs arguments of the controller function are (x, y, , xr, yr, r, Rt). You should disregard r and Rt for this assignment. 4. You will need to design the control signals (v, ). These variables are the output arguments v, omega of the controller.m le at lines 23, 24. You should adjust these variables such as the control signals will be: v = K cos = K cos sin + K sin where K, K are positive gains and . = q (x xr) 2 + (y yr) 2 t = atan2 (yr y, xr x) = t 5. Tune the gains K, K such that the vehicle converges to the target. 6. After the implementation of the controller, you will have to validate your design. Setting the string plot_real_2D at main.m to `yes', will generate an animation of the mobile robot. To plot the pose of the robot at every time sample in the 2D space, activate the plot_instances 2 string. If the simulated points are too many, you can downsample the output response by increasing the Tdn (line 58) variable. With a value of one will, no downsapling takes place. A value of two would be more than enough. Finally, plots of the the pose response and the control inputs will be generated if you activate the plot_pose and plot_control strings. 1.2 Part I: What to submit 1. Show the response of the vehicle for dierent initial conditions that lie in a circle. Keep the initial heading always to zero (0 = 0). The coordinates of the initial position that you should use are: x i 0 = 10 cos [(i 1) /4] y i 0 = 10 sin [(i 1) /4] for i = 1, 2, . . . , 5 2. For each initial condition use the gure that is generated when the plot_instances is set to `yes'. Make sure to adjust appropriately the simulation time Tsim such that the robot has enough time to converge to the target. Use the same plot to show the response of the vehicle for the dierent initial conditions. Just leave Fig.5 open and run the main.m for a dierent initial condition. 3. For at least one case, show the change in the sign of v. To support your results provide both the plot of v, with respect to time as well as the pose of the vehicle in the 2D plot in space for dierent time instances (plot_instances=`yes'). Justify why this change takes place. 2 Part II: Waypoint control with positive advancement In this part you will modify the controller that you implemented in Part I such that the robot has only positive advancement, v. To achieve that, the vehicle will execute a pure rotation until || where (0 /2]. The pure rotation takes place with a constant magnitude angular velocity 0. When the reference target is in the Line-Of-Sight (LOS) of the mobile robot || , the controller should apply the feedback law described in Part I. 3 2.1 Part II: Procedures 1. Start by commenting the controller outputs that you put together in Part I. In this Part of the assignment, you should modify the previous controller based on the pseudocode of Table 1. 2. Use dierent values of the constant and observe what happens. The variable 0 is a design parameter. For the given simulation just set 0 = 2. 2.2 Part II: What to submit 1. Explain clearly the eect of the design constant to the vehicle's motion. Provide illustrative plots to support your justication. To show the eect of nd a suitable initial position and heading. Set the reference location to the origin of the Cartesian plane. function set-point control outputs: v, 1: q (x xr) 2 + (y yr) 2 2: t atan2 (yr y, xr x) 3: t 4: if cos() < cos() then 5: if sin() 0 then 6: (v, ) (0, 0) 7: else 8: (v, ) (0, 0) 9: end if 10: else 11: v K cos 12: K cos sin + K sin 13: end if Table 1: Pseudocode with the modied feedback that guarantees a positive advancement (v > 0)