Point to Point Path Planning

Slides:



Advertisements
Similar presentations
Nonholonomic Motion Planning: Steering Using Sinusoids R. M. Murray and S. S. Sastry.
Advertisements

Physics 430: Lecture 16 Lagrange’s Equations with Constraints
Forward and Inverse Kinematics CSE 3541 Matt Boggus.
Trajectory Generation
EARS1160 – Numerical Methods notes by G. Houseman
Trajectory Planning.  Goal: to generate the reference inputs to the motion control system which ensures that the manipulator executes the planned trajectory.
EE631 Cooperating Autonomous Mobile Robots Lecture 5: Collision Avoidance in Dynamic Environments Prof. Yi Guo ECE Dept.
Nonholonomic Multibody Mobile Robots: Controllability and Motion Planning in the Presence of Obstacles (1991) Jerome Barraquand Jean-Claude Latombe.
Trajectory Generation How do I get there? This way!
Ch 7.9: Nonhomogeneous Linear Systems
Forward Kinematics.
COMP322/S2000/L23/L24/L251 Camera Calibration The most general case is that we have no knowledge of the camera parameters, i.e., its orientation, position,
Inverse Kinematics Jacobian Matrix Trajectory Planning
Velocities and Static Force
CpE- 310B Engineering Computation and Simulation Dr. Manal Al-Bzoor
Physics 114: Lecture 15 Probability Tests & Linear Fitting Dale E. Gary NJIT Physics Department.
Motion of an object is the continuous change in the position of that object. In this chapter we shall consider the motion of a particle in a straight.
February 21, 2000Robotics 1 Copyright Martin P. Aalund, Ph.D. Computational Considerations.
Motion Control (wheeled robots)
1 CMPUT 412 Motion Control – Wheeled robots Csaba Szepesvári University of Alberta TexPoint fonts used in EMF. Read the TexPoint manual before you delete.
ORDINARY DIFFERENTIAL EQUATION (ODE) LAPLACE TRANSFORM.
Scientific Computing Linear and Quadratic Splines.
An Introduction to Programming and Algorithms. Course Objectives A basic understanding of engineering problem solving process. A basic understanding of.
Robotics Chapter 5 – Path and Trajectory Planning
1 Fundamentals of Robotics Linking perception to action 2. Motion of Rigid Bodies 南台科技大學電機工程系謝銘原.
ME451 Kinematics and Dynamics of Machine Systems
Differential Equations MTH 242 Lecture # 13 Dr. Manshoor Ahmed.
Advanced EM - Master in Physics We have now calculated all the intermediate derivatives which we need for calculating the fields E and B. We.
1 Dynamics Differential equation relating input torques and forces to the positions (angles) and their derivatives. Like force = mass times acceleration.
2010.  A first degree equation is called a linear equation in two variables if it contains two distinct variables.
Trajectory Generation
Optimal Path Planning Using the Minimum-Time Criterion by James Bobrow Guha Jayachandran April 29, 2002.
Camera Calibration Course web page: vision.cis.udel.edu/cv March 24, 2003  Lecture 17.
Basilio Bona DAUIN – Politecnico di Torino
Robotics Chapter 3 – Forward Kinematics
Chapter 5: Integration Section 5.1 An Area Problem; A Speed-Distance Problem An Area Problem An Area Problem (continued) Upper Sums and Lower Sums Overview.
Boyce/DiPrima 10th ed, Ch 7.9: Nonhomogeneous Linear Systems Elementary Differential Equations and Boundary Value Problems, 10th edition, by William E.
Advance Fluid Mechanics
CSCE 441: Computer Graphics Forward/Inverse kinematics
Linear Programming Many problems take the form of maximizing or minimizing an objective, given limited resources and competing constraints. specify the.
Trajectory Generation
Character Animation Forward and Inverse Kinematics
Modeling and Simulation Dr. Mohammad Kilani
INVERSE MANIPULATOR KINEMATICS
Control Design and Analysis of Chained Systems
Differential Equations
Direct Manipulator Kinematics
EE631 Cooperating Autonomous Mobile Robots Lecture: Collision Avoidance in Dynamic Environments Prof. Yi Guo ECE Dept.
4.9 – Antiderivatives.
Constraints: Connectivity and Mobility, Lecture 2
Linear Programming.
Chapter 4 Linear Programming: The Simplex Method
Autonomous Cyber-Physical Systems: Dynamical Systems
Physics 111: Mechanics Lecture 2
CSCE 441: Computer Graphics Forward/Inverse kinematics
RS – Reed Solomon List Decoding.
TransCAD Vehicle Routing 2018/11/29.
3 General forced response
Homework: See Handout. Turn in Lab #1Wednesday
Inverse Kinematics 12/30/2018.
Robotics kinematics: D-H Approach
Robot Kinematics We know that a set of “joint angles” can be used to locate and orientate the hand in 3-D space We know that the joint angles can be combined.
Quadratic Equations and Functions
Synthesis of Motion from Simple Animations
© Sharif University of Technology - CEDRA By: Professor Ali Meghdari
Basilio Bona DAUIN – Politecnico di Torino
Physics 111: Mechanics Lecture 2
Chapter 4 . Trajectory planning and Inverse kinematics
Chapter 7 Inverse Dynamics Control
Presentation transcript:

Point to Point Path Planning Gabriele Minotto, Prof. Mariolino De Cecco

Systems in "chained form" This formulation allows a simple integration of the kinematic equations:

In the case of two inputs, we have the single chain (2,n), which are always controllable and nilpotent. Input u1 is called "generator", variables x1 and x2 are called "base variables".

If u1 is constant the system is linear and completely controllable, but it can work well also with other type of functions, like polynomials. To transform in chained form we must: Change the coordinates, such as x=Φ(q) ; Change inputs, like ν=β(q) . It is possible to demostrate that a non holonomous system with m=2 inputs and n=3,4 generalized coordinates can always be casted in chained form.

1) Monocycle model Where v is the wheel velocity, w the steering velocity. Using the trasformation of coordinate, which provide us the Cartesian coordinates of the unicycle expressed in the body reference system:

and the input transform: we obtain the following chained form:

2) CAR-LIKE model Where u1 is the front wheel velocity and u2 the steering velocity. In this case we use as new coordinates:

and as inputs: and then we obtain the following chained form (defined for δ ≠ π/2 ± kπ, k ϵ Ν):

3) RHOMBIC-LIKE MODEL Here the kinematic model of rhombic-like vehicle, expressed in vehicle’s reference frame.

Where v1 and v2 are the traction speeds, ά1 and ά2 the steering velocities, respectively of front and rear wheels. And the chained form….. IT’S UP TO YOU!!!!!! SUGGESTION: to find a chained form for the rhombic-like vehicle, use the kinematic model of previous slide, defined in vehicle’s reference frame, in order to not complicate too much the computation. Don’t forget the kinematic constraint of velocities!

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 giude the robot from a starting configuration to another one with assigned coordinates. It is possible to use different kind of inputs, like sinusoidal, piecewise constant and polynomial functions.

1) Piecewise constant inputs In this case we divide total time T in subintervals of length Δ where both inputs are constants: It is possible to mantain u1 always constant and take n-1 intervals (k = 1,…,n-1) so that:

where n is the number of generalized coordinates, xf1 and x01 are respectively final and initial values of first coordinates x1. The n-1 constants values (u2,1, u2,2, …, u2,n-1) of u2 are obtained by solving a linear system, computed after integration of the chain and imposition of initial and final conditions. In the case of coincidence between initial and final abscissa’s value, it is necessary to add an intermediate point, and it’s also impossible to carry out manoeuvres with automatic speed inversion. One of this method’s drawbacks is the fact that steering speed may present discontinuities.

2) POLYNOMIAL INPUTS This method, which use polynomial functions as inputs, is characterized by better performances in terms of input functions’ continuity ("smoothness"). But we can increase the degree of polynomial, in order to have more degrees which allow us to avoid eventually present obstacles.

By integrating the chain and imposing initial and final conditions, we will obtain a linear system into the unknowns (c0, c1, …, cn-2): A(T) can be inverted for T≠0. Also in this case if abscissa’s initial and final points coincide we must add an intermediate point, and we cannot carry out manoeuvres with automatic inversion of the speed.

Work in class 1) PATH PLANNING Drive a vehicle from pose [0, 0, 0] to [1, 1, pi/2] in T=1s, where the three coordinates are [x, y, δ]. For a car like add the steering angle that you prefer. Choose monocycle, car-like or rhombic-like model and consider its chained form. Choose piecewise constant or polynomial inputs.

Then: Compute initial and final conditions in the transformed coordinates . Divide the time interval T in n-1 intervals (n equal to the number of generalized coordinates). Find input u1 in explicit form and u2 in symbolic form for the n-1 intervals. Integrate symbolically the chained form equations starting from x1 in order to obtain xi(t), i = 1…n. 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).

Solve for the u2i unknowns. Substitute the u2i values to obtain xi(t). From the coordinates transform equations find the generalized coordinates (x, y, δ for the monocycle and x, y, δ, α for the car-like). From the input transform equations find the two inputs as a function of time. Plot the resulting trajectories/paths in the 2D space and inputs as a function of time.

Once finished the above steps, you can consider the following: Compare the results with other inputs. Add to the polynomial input one order and choose the resulting degree of freedom to change your path (imagine an application of obstacle avoidance).

2) OPTIMIZATION ALGORITHMS A) Minimization of distance from a set of points like obstacles Define Oi = [xi, yi]T i=1…n point obstacles. Define symbolically the polynomial’s parameters. For every Oi choose a Pi on the trajectory (or find the one with minimum distance). Build a functional that uses the distances of Oi from Pi. Minimize this functional. Try to change the degree of polynomial that you choose as degree of freedom. Try to add obstacles and degrees of freedom (as coefficients to the polynomial).

B) Minimization of length of path in the presence of obstacles Choose the right target to minimize in order to find the shortest path that can be performed by vehicle… IT’S UP TO YOU!!!!!!

SOFTWARE SUGGESTED FOR COMPUTATIONS: SUGGESTION: use often Maple Help!

A) Using piecewise functions B) Using polynomial inputs 1) PATH PLANNING MONOCYCLE MODEL A) Using piecewise functions B) Using polynomial inputs

MONOCYCLE MODEL: PATH PLANNING > # Clear and close all previous work. restart: with(plots): with(LinearAlgebra): > # Data Inputs. data := [T = 1, n=3]; Model definition (here we introduce models in matrix form < > or in list form [ ]): > eq_tr := <x__1(t) = -delta(t), x__2(t) = x(t)*cos(delta(t)) + y(t)*sin(delta(t)), x__3(t) = -x(t)*sin(delta(t)) + y(t)*cos(delta(t))>; > eq_v := [v__1(t) = x__3(t)*u__1(t) + u__2(t), v__2(t) = -u__1(t)]; > in_cond := [0, 0, 0]; fi_cond := [1, 1, Pi/2];

1) Initial transformed conditions >in_tr_cond := convert(evalf(subs(x(t)=in_cond[1],y(t)=in_cond[2],delta(t)=in_cond[3],eq_tr)),list); > fi_tr_cond := convert(evalf(subs(x(t)=fi_cond[1],y(t)=fi_cond[2],delta(t)=fi_cond[3],eq_tr)),list); 2) Divide time int. > n_i := subs(data,n-1); > delta_T := subs(data,T/n_i);

A) Using piecewise functions: 3)Input u__1 e u__2 in symbolic form: ‘convert’ operation is useful to extract informations from piecewise functions. It may be useful to convert MATRIX in LIST. > u__1s(t) := (x__f1 - x__i1)/T; #symbolic form > u__1(t) := subs(data,x__f1=rhs(fi_tr_cond[1]), x__i1=rhs(in_tr_cond[1]),u__1s(t)); > u__2(t) := piecewise(0 <= t and t <= delta_T, u__21, delta_T <= t and t <= n_i*delta_T, u__22); Substitute chosen inputs in the chain, with commands previously defined, and integrate all of them (see ‘int’ command).

It may be common to group all of generalized coordinates in an unique list: > # Generalized velocities and positions. > vel_gen := [op(diff_ui(t)), op(diff_u3(t))]; > pos_gen := [op(x__i(t)), simplify(x__i3)]; 4 -5) Constraint initial and final conditions - linear system. Then solve for the u__2i unknowns. It may be common to substitute T in piecewise functions and match them to the final conditions. See ‘solve’ and ‘evalf’ commands.

??? ??? ??? 6) Obtain x__i(t) from u__2 7) Obtain generalized coordinate x, y, delta ??? 8) Find the two inputs ???

9) Plot trajectories & controls See ‘plot’ and ‘display’ commands, in order to plot trajectory in the plane (x,y) and controls in time. Plot solutions:

Compare the results with other inputs. NOTES: third initial and final conditions (related to δ) must not be the same, because equation related to x3(t) is always equal to zero and another setting of piecewise equations must be performed. Remember also previous notes about efficiency of this method.

B) Using polynomial inputs: We can use the previous Maple sheet, simply modifying the input u2. Since the linear system of 2 equations can have more than 2 unknowns (as we said in the related slide), you must choose a ‘first guess’ value for those in excess, in order to solve the system and find the last two coefficients.

9) Plot trajectories & controls

NOTES: if third initial and final conditions are the same, there is only one unknown polynomial constant, because one equation with final conditions is always satisfied and all the other constants must be defined a priori.

2) OPTIMIZATION ALGORITHMS A) Minimization of distance from a set of points like obstacles B) Minimization of length of path in the presence of obstacles

OPTIMIZATION ALGORITHMS In these cases we must define the obstacle’s positions: > Obs_Pos_Data := [ xp__1 = 0.8, yp__1 = 0.8, xp__2 = 0.2, yp__2 = 0.2 , xp__3 = 0.3, yp__3 = 0.7 ]: <%>: And some other data parameters, like n_OP (number of obstacles), ε (minimum distance from path to obstacles), n_P (number of integration substeps for defining the target to minimize), T__step (time at which vehicle must pass between two obstacles).

SUGGESTION: define also u1 as a polynomial input, in order to increase the smoothness of solution. Prepare the Optimization: A) Target is the distance between path and the obstacles’ positions. Bound is the linear system with final conditions. B) Target is the trajectory of vehicle, which length is to minimize. Bounds are the linear system with final conditions and a minimum distance from obstacles ε. See ‘Optimization’ in Maple Help.

A) Minimization of distance from a set of points like obstacles SUGGESTION: in this optimization we want the vehicle passing through two obstacles close toghether at a certain time. For this reason we want to define the constraint of optimization as the euclidean distance between themselves and the car at a determined time: obviously, if we have two couples of obstacles, we will define two time to which vehicle will pass thorugh them, one for the first couple and one for the second couple, and so on. > T__step := subs(data,[T/4, T/4, 2*T/4, 2*T/4, 3*T/4, 3*T/4]):

Position of couples of obstacles, with related passage time. (xp1,yp1) → t = T/4 (xp3,yp3) → t = 3*T/4 (xp2,yp2) → t = 2*T/4

Plot procedure is similar to previous exercise, only with some more features (see ‘plot options’ in maple help). Lindsey Vonn, Audi FIS World Cup - Women's Super Giant Slalom If your computation works well, you could have a vehicle that makes ‘slalom’ between obstacles!!!

RESULTS: in this case we have taken symbolically also initial and final attitudes, in order to increase the smoothness further (to plot obstacles, see ‘pointplot’ command). Improve this work, by passing the vehicle between two obstacles with a trajectory orthogonal to the line which joins them!!

B) SUGGESTION: in order to reduce the computation time, you can define the target with the rectangle method instead of the common integral (see ‘sum’ command). RESULTS (i.c. = [0,0,π/4], f.c. = [1,1,π/4] ):

To verify that path optimization works well, remove obstacle bounds and impose as initial and final conditions respectively [0,0,π/4] and [1,1, π/4]. Result must be a straight path, then without attitude variations:

IT’S UP TO YOU!!!!!! AND THE NEXT STEPS???? -) Car-like model … -) Rhombic-like model … IT’S UP TO YOU!!!!!! SUGGESTION: for the rhombic-like vehicle, remember to work always in the vehicle’s reference frame, and only after the optimization return to the ground!!

BIBLIOGRAFY Prof. Mariolino De Cecco - slides of Robotic Perception and Action course. Luca Baglivo - Navigazione di Veicoli Autonomi: Pianificazione e Controllo di Traiettoria