Point to Point Path Planning
Pianificazione di Traiettoria punto-punto
Appunti per il corso di “Robotic Perception and Action”
per Ingegneria Meccatronica
1. Point to Point Steering
1.1. Systems in “chained form”.
This formulation allows a simple integration of the kinematics equations.
For a two input model (many mobile robots have such a configuration) we have:
If is constant the system is linear and completely controllable.
To transform in chained form it is enough to:
1. Change coordinates
2. Change inputs .
It is possible to demonstrate that a non holonomous system with input and generalized coordinates can always be casted in chained form.
1.1.1. Monocycle model
The monocycle model is the following:
Were v1 is the wheel velocity, v2 is the steering velocity.
We can use the coordinates transform (it’s interesting to note that the new variables and are simply the Cartesian coordinates of the unicycle expressed in the body reference system):
and the input transform
we obtain:
1.1.2. Car like model
For the car like model:
we can use:
and:
so that the system assumes the chained form:
Note that the coordinates transform s defined only for
1.2. Methods to steer a vehicle Point to Point starting from the chained form
It occurs that also the chained system is not holonomous but in this case you can use appropriate procedures to obtain classes of solutions to the problem of motion planning of point-to-point, or trajectories (motion laws) that guide the robot from a starting configuration to a configuration of arrival assigned.
It is possible to use different kind of inputs:
· sinusoidal inputs
· inputs piecewise constant
· polynomial inputs
1.2.1. Inputs piecewise constant
Divide the total time T is sub intervals of lengthwhere both inputs are constants:
with
it is possible to maintain always constant and take intervals (-1) so that:
and:
where:
· is the number of generalized coordinates
· and are the final and initial value of the first coordinate
The constant values () of are obtained solving a linear system.
In the case in which the abscissa at the point of departure and arrival coincide it is necessary to add an intermediate point, it is also not possible to carry out manoeuvres with automatic reversal of the speed.
One of the drawbacks of this method is the fact that the steering speed may present discontinuities.
1.2.2. polynomial inputs
An algorithm similar to that of the input piecewise constant, but with better performance in the level of continuity ("smoothness") of the input functions, it is to use polynomial inputs.
By choosing for the two inputs:
for the total time you can choose (but not necessarily):
integrating the chained for equations and imposing the initial and final conditions, you will obtain a linear system in the unknowns ():
can be inverted for
Also in this case if the abscissa at the point of departure and arrival coincide it is necessary to add an intermediate point, it is also not possible to carry out manoeuvres with automatic reversal of the speed.
2. Work in class
Steer a vehicle from pose [0, 0, 0] to [1, 1, pi/2] in T = 1 second, where the three coordinates are [x, y, d]. For a car like add the steering angle that you prefer.
You can follow the steps:
1. choose monocycle or car like model and consider its chained form
2. choose piecewise constant or polynomial inputs
then (for the piecewise constant, but it is similar for the polynomial inputs):
1. compute the transformed coordinates initial and final conditions
2. divide the time interval T in n-1 intervals (n equal to the number of generalized coordinates)
3. find input u1 in explicit form and u2 in symbolic form for the n-1 intervals
4. integrate symbolically the chained form equations starting from x1 in order to obtain xi(t), i = 1…n
5. constrain with the initial and final conditions to find a linear system in the symbolic piecewise constant u2 inputs (polynomial coefficients in the other case)
6. solve for the u2i unknowns
7. insert the u2i values to obtain xi(t)
8. from the coordinates transform equations find the generalized coordinates (x, y, d for the monocycle and x, y, d, a for the car like)
9. from the input transform equations find the two inputs as a function of time
10. plot the resulting trajectories/paths in the 2D space and inputs as a function of time
when you have finished the above steps, you can consider the following:
1. to compare with the other inputs
2. to add to the polynomial input one order and choose the resulting degree of freedom to change your path (imagine an application of obstacle avoidance)
3. then, with the added order try to find by means of an optimization routine that leads the path as far as possible from a couple of obstacles that you can model as two simple points (two point obstacles are a good example to evaluate your algorithm as far as the result shall lead to a trajectory orthogonal to the line made by the two obstacles points and passing close to the mid point). See the following paragraph:
2.1. minimization of distance from a set of point like obstacles
define Oi = [xi, yi]T i=1…n point obstacles
then follow the following steps (indicatively):
1. define numerically the parameter of the polinomial
2. for every Oi find Pi on the trajectory with minimum distance
3. build a functional that uses the minimum distances of Oi from Pi
4. maximize this functional
try to change the parameter of the polynomial that you choose as degree of freedom
try to add obstacles and degree of freedom (coefficient to the polynomial)
6