Quintic polynomials planning
Motion planning with quintic polynomials.
It can calculate 2D path, velocity, and acceleration profile based on quintic polynomials.
Quintic polynomials for one dimensional robot motion
We assume a one-dimensional robot motion \(x(t)\) at time \(t\) is formulated as a quintic polynomials based on time as follows:
\(a_0, a_1. a_2, a_3, a_4, a_5\) are parameters of the quintic polynomial.
It is assumed that terminal states (start and end) are known as boundary conditions.
Start position, velocity, and acceleration are \(x_s, v_s, a_s\) respectively.
End position, velocity, and acceleration are \(x_e, v_e, a_e\) respectively.
So, when time is 0.
Then, differentiating the equation (1) with t,
So, when time is 0,
Then, differentiating the equation (3) with t again,
So, when time is 0,
so, we can calculate \(a_0, a_1, a_2\) with eq. (2), (4), (6) and boundary conditions.
\(a_3, a_4, a_5\) are still unknown in eq (1).
We assume that the end time for a maneuver is \(T\), we can get these equations from eq (1), (3), (5):
From eq (7), (8), (9), we can calculate \(a_3, a_4, a_5\) to solve the linear equations: \(Ax=b\)
We can get all unknown parameters now.
Quintic polynomials for two dimensional robot motion (x-y)
If you use two quintic polynomials along x axis and y axis, you can plan for two dimensional robot motion in x-y plane.
It is assumed that terminal states (start and end) are known as boundary conditions.
Start position, orientation, velocity, and acceleration are \(x_s, y_s, \theta_s, v_s, a_s\) respectively.
End position, orientation, velocity, and acceleration are \(x_e, y_e. \theta_e, v_e, a_e\) respectively.
Each velocity and acceleration boundary condition can be calculated with each orientation.
\(v_{xs}=v_scos(\theta_s), v_{ys}=v_ssin(\theta_s)\)
\(v_{xe}=v_ecos(\theta_e), v_{ye}=v_esin(\theta_e)\)