-Interactive lesson on Path Control-
Prof. Mariolino De Cecco
Studenti:
Luca Pietropoli 196766
Alessandro Motta 198089
Riccardo Andreoni 195367
Marius Chiriac 203543
INTRODUCTION
This project consists in an interactive lesson focused on explaining different methods for the path following problem.
The interaction is implemented allowing the user to intervene directly on the lesson deciding on the course of it according to his choices. Our aim was to make it intuitive but not trivial, dealing with complex topics in a simplified way.
Two kind of wheeled mobile robots are considered: a rhombic like vehicle (RLV) and a car like vehicle (CLV).
Two path following controllers, based on feedback laws derived at a kinematic level with geometrical inspiration, are developed to steer the kinematic model of a RLV along a desired path:
- Arc Path Following controller (APF)
- Alonzo Kelly modified controller (AKM)
The APF controller is implemented also on the CLV to steer it along a desired path.
VEHICLES KINEMATIC
Rhombic like vehicle
RLVs distinguish themselves from the typical unicycle and car like vehicles, by allowing to control the linear speed, vi, and the orientation, θi, of each wheel, with i є {Rear; Front}. That is because RLVs are a set of nonholonomic vehicles which enable a decoupling between the vehicle’s orientation and velocity vector, creating a controllable sideslip angle, β.
The mathematical equations of a kinematic model for a RLV relate the temporal variations of the vehicle pose with the desired total speed of the vehicle. It is considered the state vector q=[xc,yc,θm] as a representation of the vehicle pose in the frame {I}, with (xc,yc) the coordinates of the center of the vehicle and θm the orientation of the vehicle. In addiction it is considered v as the vehicle’s total speed and β as the controllable sideslip angle of the vehicle.
The kinematic model that we have used allows the simulation of the vehicle motion directly through the desired total speed of the vehicle, instead of imposing an individual linear speed for each wheel:
Car like vehicle
A 3-wheeled vehicle consists of a fixed axle rear wheels and steerable front wheel. The main difference with RLV is that, for this kind of vehicle, it is not possible to control both the orientation θi of each wheel i є {Rear; Front}.
The mathematical equations of a kinematic model for a CLV, as the ones for an RLV, relate the temporal variations of the vehicle pose with the desired total speed of the vehicle. It is considered the state vector q= [xR, yR, δ] as a representation of the vehicle pose in the absolute frame, with (xR, yR) the coordinates of the mean rear axle point (Pv) and δ is the angle that measures the orientation of the vehicle with respect to the axis. Also it is considered α as the steering angle of the front wheel measured in relation to the axis of longitudinal symmetry of the vehicle
There are several ways to obtain the kinematic model of this kind of vehicle. For the purposes of this lesson it has been used a method that consider the rotation of the vehicle around its center of instantaneous rotation (ICR). The result is the model that you can see displayed:
PATH FOLLOWING CONTROLLERS
Arc Path Following Controller (APF) for RLV
The key idea of this controller is that, since the vehicle is free to define its Instantaneous Center of Rotation (ICR), it is in any case possible to compute the generalized circular path that steers the vehicle from the current pose to the desired one. From the figure it is possible to understand better this concept.
The following are the steps to follow for implementing this controller:
- Knowing the desired point to reach (Pd) and the desired orientation (θd), firstly it’s calculated the desired ICRd, the desired radius of curvature of the two wheels, all of that expressed in the mobile reference frame {M}, and the control laws, one for each wheel. With the equations below it’s possible compute all these values, where T = [Tx, Ty] is the vector P – C expressed in {M}, θe = θm – θd, and (F)M = [M/2 ,0], and (R)M = [-M/2 ,0], are the coordinates of the two wheels expressed in {M};
- Compute β= 〖tan^(-1) (〗〖〖〖ICRd〗_y〗_M/〖〖ICRd〗_x〗_M 〗), found by simply geometrical considerations;
- Calculate the temporal variation of the vehicle pose [x’c,y’c,θ’m], knowing velocity v and using the equations of the kinematic model;
- Multiply this temporal variation for the time interval to find the new pose of the vehicle and repeat the computations (it will be changed the vector C-P and θe compared to the precedent step).
This controller, like the one for the 3WV vehicle and the AKM, enables the introduction of a lookahead distance, i.e. it is not mandatory that P is the orthogonal projection of C on the path. This represent an extra degree of freedom in the controller that can be exploited to achieve smoother and better path following performances.
Arc Path Following Controller (APF) for CLV
The key idea of this controller is that, since the vehicle in free to define its Instantaneous Center of Rotation (ICR), it is in any case possible to compute the generalized circular path that steers the vehicle from the current pose to the desired one.
The following are the steps to follow for implementing this controller:
- Compute the ICR, by firstly defining ϒ as the angle between the vector Pv-Pd and the vehicle orientation. Respect to the mobile frame, the center of the two rear wheels has coordinates (0,0), the point Pd has coordinates (xd,yd) and the ICR has coordinates (0,r), where r is the only unknown. Since the coordinates of Pd are known, it’s possible find ϒ. After, by considering the triangle of vertices Pv, the mid-point of Pv-P and the ICR is easy to find r.
- It’s possible to find the desired ICR also in a graphical way. In fact, it is the point that results from the intersection between the orthogonal from the mid-point of the vector Pv-Pd and the orthogonal to the vehicle orientation passing through Pv.
- Once r is defined, calculate the angle α and, using the equations of the kinematic model, compute the temporal variation of the vehicle pose
- Multiply this temporal variation for the time interval to find the new pose of the vehicle and repeat the computations (it will be changed the vector Pv-Pd and δ compared to the precedent step).
This controller, like the ones for the RLV vehicle and the AKM, enables the introduction of a lookahead distance, i.e. it is not mandatory that P is the orthogonal projection of C on the path.
Alonzo Kelly Modified Controller (AKM) for RLV
This controller enables the formulation of a path following controller that only requires as input the desired pose of the vehicle at the path, [Px, Py, θd], and upon the control of β e ωm, outputs the values of vF, vR, θF, θR that enable the vehicle to converge and follow the desired path.
The following are the steps to follow for implementing this controller:
- Compute the control laws for β and ω_m. The control law for β is used to direct the vehicle’s velocity towards the desired path point, as depicted in the figure. The control law for ω_m is responsible for orientating the vehicle with the desired orientation at the path, where tn the simulation step and k_(ω_m ) a positive gain that tunes the vehicle’s response rate to orientation variations
- Use the following equations to pass from [ωm, β, v] to [vF, vR, θF, θR], where θi is the wheel’s orientation and vi is the wheel’s speed, with i є {R, F}.
- Being known β, θm, θF, θR and using the equations of the kinematic model find the temporal variation of the vehicle pose [x’c,y’c,θ’m].
- Multiply this temporal variation for the time interval to find the new pose of the vehicle and repeat the computations (it will be changed the vector C-P and orientation θm compared to the precedent step).
Another degree of freedom inherent to this controller is the choice of the path point to follow, like in the APF controller. Indeed it is not mandatory to chosen as desired point P the orthogonal projection of the C on the path (so the closest one, as you can see from the figure). Thought, by introducing an offset on the path point to be followed, the performance of the controller can be significantly improved.
DEVELOPMENT IN UNITY
As explained in the introduction, our implementation wants to be an interactive lesson where the student can both consult the theoretical aspects and play with parameters to understand better the concepts.
We developed a state machine to manage this lesson. For all the theory and parameters scenes and for the scene where the student can select by the different controllers, by clicking the RETURN button there is the possibility to listen the audio explanation.
In the initial scene a student has the possibility to select one of the two vehicles if he wants to better understand the kinematic model or he can press the start button if he wants to choose one of the controllers.
In the scene about the controllers the student can select one of them and, after the selection, for each controller it will open a new scene where he can press 3 different buttons:
- THEORY: to have a full explanation about how the controller works
- PARAMETERS: to set the different parameters for the animation part
- CHANGE CONTROLLER: to return to the scene of the controller in order to choose a different one
In the parameters scene the student can set the different parameter to run the animation and see how a certain controller works. He can change the initial pose of the vehicle (xc,yc,θm), the vehicle’s total speed, the lookahead distance, the positive gain k_(ω_m )(parameter only present in the AKM controller) and the trajectory. In particular, the trajectory can be construct by composing three different functions (straight line, sine, arctangent) and by selecting, for each function, the length and the multiplier.
After the parameters setting, the student can see how actually the controller works by clicking on the animation button. In each animation scene he has also the possibility to click on the CHANGE CONTROLLER button to change controller to make some comparisons using the same parameters setting.
REFERCENS of the project
[1] Nuno Silva, Luca Baglivo, Alberto Vale, Mariolino De Cecco, Four Path Following Controllers for Rhombic Like Vehicles, IEEE International Conference, ICRA 2013.
[2] Luca Baglivo, Autonomous Vehicles Navigation, Trajectory Planning and Control, Notes fot the “Space Robotics” course in Aerospace Engeneering.
[3] Danwei Wang, Trajectory planning for a four-wheel-steering vehicle, IEEE 2001.
[4] Bruno Siciliano, Lorenzo Sciavicco, Luigi Villani, Giuseppe Oriolo, Robotics
Modelling, Planning and Control, Springer 2009.
[5] Terry Norton, Learning C# by Developing Games with Unity 3D, Packt Publishing 2013.
[6] Will Goldstone, Unity 3.x Game Development Essentials, Packt Publishing 2011.