自动驾驶规划 – 5次多项式拟合

简介

自动驾驶运动规划中会用到各种曲线，主要用于生成车辆的轨迹，常见的轨迹生成算法，如贝塞尔曲线，样条曲线，以及apollo EM Planner的五次多项式曲线，城市场景中使用的是分段多项式曲线，在EM Planner和Lattice Planner中思路是，都是先通过动态规划生成点，再用5次多项式生成曲线连接两个点（虽然后面的版本改动很大，至少lattice planner目前还是这个方法）。

在这里可以看出5次多项式的作用，就是生成轨迹，这里的轨迹不一定是车行驶的轨迹，比如S—T图中的线，是用来做速度规划的。 如下图：

在apollo里面用到了，3-5次多项式，

cubic_polynomial_curve1d，三次多项式曲线
quartic_polynomial_curve1d，四次多项式曲线-
quintic_polynomial_curve1d，五次多项式曲线

3次多项式

$f(x) = C_0 + C_1x + C_2x^2+ C_3x^3 —-(1)$
对x求导====>
$f'(x) = C_1 + 2C_2x+ 3C_3x^2$
$f(x) = 2C_2+ 6C_3x$
因为是连接两个已知点，所以两个轨迹点的信息是已知的。
如已知，当x=0，代如得：
$f(0) = C_0 ； f'(0) = C_1；f”(0) =2C_2$

所以
$C_0 = f(0)；C_1 = f'(0)； C_2 = f”(0)/2$
将终点 (X_end，f(X_end))带入得到C_3。

$C_3 = (f(x_p)-C_0-C_1x_p-C_2x_p^2)/x_p^3$

（1）式的所有未知参数都求出，代入x可以得到两点间的轨迹。

apollo中3次多项式的计算

// cubic_polynomial_curve1d.ccCubicPolynomialCurve1d::CubicPolynomialCurve1d(const double x0, const double dx0, const double ddx0, const double x1, const double param) {ComputeCoefficients(x0, dx0, ddx0, x1, param);param_ = param;start_condition_[0] = x0;start_condition_[1] = dx0;start_condition_[2] = ddx0;end_condition_ = x1;}void CubicPolynomialCurve1d::ComputeCoefficients(const double x0, const double dx0, const double ddx0, const double x1, const double param) {DCHECK(param > 0.0);const double p2 = param * param;const double p3 = param * p2;coef_[0] = x0;coef_[1] = dx0;coef_[2] = 0.5 * ddx0;coef_[3] = (x1 - x0 - dx0 * param - coef_[2] * p2) / p3;}

5次多项式

$f(x) = C_0 + C_1x + C_2x^2+ C_3x^3+C_4x^4+C_5x^5 —-(3-1)$
对x求导====>
$f'(x) = C_1 + 2C_2x+ 3C_3x^2+4C_4x^3+5C_5x^4—-(3-2)$

$f”(x) = 2C_2+ 6C_3x+12C_4x^2+20C_5x^3—-(3-3)$
因为是连接两个已知点，所以两个轨迹点的信息是已知的。
如已知，当x=0，代如得：
$f(0) = C_0 ； f'(0) = C_1；f”(0) =2C_2$

所以
$C_0 = f(0)；C_1 = f'(0)； C_2 = f”(0)/2$
将终点 (x_e，f(x_e))带入 (3-1) (3-2) (3-3)。

得到
$f(x_e) = C_0 + C_1x_e + C_2x_e^2+ C_3x_e^3+C_4x_e^4+C_5x_e^5 —-(3-4)$
对x求导====>
$f'(x) = C_1 + 2C_2x_e+ 3C_3x_e^2+4C_4x_e^3+5C_5x_e^4—-(3-5)$

$f”(x) = 2C_2+ 6C_3x_e+12C_4x_e^2+20C_5x_e^3—-(3-6)$

联立可得
$C_3 = \frac{10(f(x_p)-1/2*f”(0)x_p^2-f'(0)x_p-f(0))-4(f'(x_p)-f”(0)x_p-f'(0))x_p+1/2*(f”(x_p)-f”(0))x_p^2}{x_p^3}$

$C_4 = \frac{-15(f(x_p)-1/2*f”(0)x_p^2-f'(0)x_p-f(0))+7(f'(x_p)-f”(0)x_p-f'(0))x_p-1/2*(f”(x_p)-f”(0))x_p^2}{x_p^4}$

$C_5 = \frac{6(f(x_p)-1/2*f”(0)x_p^2-f'(0)x_p-f(0))-3(f'(x_p)-f”(0)x_p-f'(0))x_p+1/2*(f”(x_p)-f”(0))x_p^2}{x_p^5}$

apollo中5次多项式的计算

void QuinticPolynomialCurve1d::ComputeCoefficients(const double x0, const double dx0, const double ddx0, const double x1,const double dx1, const double ddx1, const double p) {CHECK_GT(p, 0.0);coef_[0] = x0;coef_[1] = dx0;coef_[2] = ddx0 / 2.0;const double p2 = p * p;const double p3 = p * p2;// the direct analytical method is at least 6 times faster than using matrix// inversion.const double c0 = (x1 - 0.5 * p2 * ddx0 - dx0 * p - x0) / p3;const double c1 = (dx1 - ddx0 * p - dx0) / p2;const double c2 = (ddx1 - ddx0) / p;coef_[3] = 0.5 * (20.0 * c0 - 8.0 * c1 + c2);coef_[4] = (-15.0 * c0 + 7.0 * c1 - c2) / p;coef_[5] = (6.0 * c0 - 3.0 * c1 + 0.5 * c2) / p2;}

5次多项式拟合

import mathimport matplotlib.pyplot as pltimport numpy as np# parameterMAX_T = 100.0# maximum time to the goal [s]MIN_T = 5.0# minimum time to the goal[s]show_animation = Trueclass QuinticPolynomial:def __init__(self, xs, vxs, axs, xe, vxe, axe, time):# calc coefficient of quintic polynomial# See jupyter notebook document for derivation of this equation.# 根据初始状态c0 c1 c2self.a0 = xs # x(t)self.a1 = vxs # v(t)self.a2 = axs / 2.0 # a(t)A = np.array([[time ** 3, time ** 4, time ** 5],[3 * time ** 2, 4 * time ** 3, 5 * time ** 4],[6 * time, 12 * time ** 2, 20 * time ** 3]])b = np.array([xe - self.a0 - self.a1 * time - self.a2 * time ** 2,vxe - self.a1 - 2 * self.a2 * time,axe - 2 * self.a2])# Ax=b 求解xx = np.linalg.solve(A, b)# 计算c3 c4 c5self.a3 = x[0]self.a4 = x[1]self.a5 = x[2]# 根据时间计算点x(t)def calc_point(self, t):xt = self.a0 + self.a1 * t + self.a2 * t ** 2 + \ self.a3 * t ** 3 + self.a4 * t ** 4 + self.a5 * t ** 5return xt# 计算v(t)def calc_first_derivative(self, t):xt = self.a1 + 2 * self.a2 * t + \ 3 * self.a3 * t ** 2 + 4 * self.a4 * t ** 3 + 5 * self.a5 * t ** 4return xt# 计算a(t)def calc_second_derivative(self, t):xt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t ** 2 + 20 * self.a5 * t ** 3return xt# 计算jerk(t)def calc_third_derivative(self, t):xt = 6 * self.a3 + 24 * self.a4 * t + 60 * self.a5 * t ** 2return xtdef quintic_polynomials_planner(sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_accel, max_jerk, dt):"""quintic polynomial plannerinputs_x: start x position [m]s_y: start y position [m]s_yaw: start yaw angle [rad]sa: start accel [m/ss]gx: goal x position [m]gy: goal y position [m]gyaw: goal yaw angle [rad]ga: goal accel [m/ss]max_accel: maximum accel [m/ss]max_jerk: maximum jerk [m/sss]dt: time tick [s]returntime: time resultrx: x position result listry: y position result listryaw: yaw angle result listrv: velocity result listra: accel result list"""vxs = sv * math.cos(syaw)# 起点速度在x方向分量vys = sv * math.sin(syaw)# 起点速度在y方向分量vxg = gv * math.cos(gyaw)# 终点速度在x方向分量vyg = gv * math.sin(gyaw)# 终点速度在y方向分量axs = sa * math.cos(syaw)# 起点加速度在x方向分量ays = sa * math.sin(syaw)# 终点加速度在y方向分量axg = ga * math.cos(gyaw)# 起点加速度在x方向分量ayg = ga * math.sin(gyaw)# 终点加速度在y方向分量time, rx, ry, ryaw, rv, ra, rj = [], [], [], [], [], [], []for T in np.arange(MIN_T, MAX_T, MIN_T):xqp = QuinticPolynomial(sx, vxs, axs, gx, vxg, axg, T)yqp = QuinticPolynomial(sy, vys, ays, gy, vyg, ayg, T)time, rx, ry, ryaw, rv, ra, rj = [], [], [], [], [], [], []for t in np.arange(0.0, T + dt, dt):time.append(t)rx.append(xqp.calc_point(t))ry.append(yqp.calc_point(t))vx = xqp.calc_first_derivative(t)vy = yqp.calc_first_derivative(t)v = np.hypot(vx, vy)yaw = math.atan2(vy, vx)rv.append(v)ryaw.append(yaw)ax = xqp.calc_second_derivative(t)ay = yqp.calc_second_derivative(t)a = np.hypot(ax, ay)if len(rv) >= 2 and rv[-1] - rv[-2] < 0.0:a *= -1ra.append(a)jx = xqp.calc_third_derivative(t)jy = yqp.calc_third_derivative(t)j = np.hypot(jx, jy)if len(ra) >= 2 and ra[-1] - ra[-2] < 0.0:j *= -1rj.append(j)if max([abs(i) for i in ra]) <= max_accel and max([abs(i) for i in rj]) <= max_jerk:print("find path!!")breakif show_animation:# pragma: no coverfor i, _ in enumerate(time):plt.cla()# for stopping simulation with the esc key.plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None])plt.grid(True)plt.axis("equal")plot_arrow(sx, sy, syaw)plot_arrow(gx, gy, gyaw)plot_arrow(rx[i], ry[i], ryaw[i])plt.title("Time[s]:" + str(time[i])[0:4] +" v[m/s]:" + str(rv[i])[0:4] +" a[m/ss]:" + str(ra[i])[0:4] +" jerk[m/sss]:" + str(rj[i])[0:4],)plt.pause(0.001)return time, rx, ry, ryaw, rv, ra, rjdef plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):# pragma: no cover"""Plot arrow"""if not isinstance(x, float):for (ix, iy, iyaw) in zip(x, y, yaw):plot_arrow(ix, iy, iyaw)else:plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw),fc=fc, ec=ec, head_width=width, head_length=width)plt.plot(x, y)def main():sx = 10.0# start x position [m]sy = 10.0# start y position [m]syaw = np.deg2rad(10.0)# start yaw angle [rad]sv = 1.0# start speed [m/s]sa = 0.1# start accel [m/ss]gx = 30.0# goal x position [m]gy = -10.0# goal y position [m]gyaw = np.deg2rad(20.0)# goal yaw angle [rad]gv = 1.0# goal speed [m/s]ga = 0.1# goal accel [m/ss]max_accel = 1.0# max accel [m/ss]max_jerk = 0.5# max jerk [m/sss]dt = 0.1# time tick [s]time, x, y, yaw, v, a, j = quintic_polynomials_planner(sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_accel, max_jerk, dt)if show_animation:# pragma: no coverplt.plot(x, y, "-r")plt.subplots()plt.plot(time, [np.rad2deg(i) for i in yaw], "-r")plt.xlabel("Time[s]")plt.ylabel("Yaw[deg]")plt.grid(True)plt.subplots()plt.plot(time, v, "-r")plt.xlabel("Time[s]")plt.ylabel("Speed[m/s]")plt.grid(True)plt.subplots()plt.plot(time, a, "-r")plt.xlabel("Time[s]")plt.ylabel("accel[m/ss]")plt.grid(True)plt.subplots()plt.plot(time, j, "-r")plt.xlabel("Time[s]")plt.ylabel("jerk[m/sss]")plt.grid(True)plt.show()if __name__ == '__main__':main()

上图模拟了车辆曲线的拟合过程的轨迹，箭头代表方向。

过程的速度

感觉5次多项式和贝塞尔曲线还是挺像的，有时间写一篇五次多项式、贝塞尔曲线、样条曲线的对比。

还有一个问题就是为啥要用5次多项式，而不是4次，6次更高阶或者低阶呢？下回复习一下老王的讲解。
参考：

https://blog.csdn.net/qq_36458461/article/details/110007656
https://zhuanlan.zhihu.com/p/567375631

自动驾驶规划 – 5次多项式拟合

简介

3次多项式

5次多项式

5次多项式拟合

最新关注

热文推荐

四叉树图像模糊（C代码及实现思路）

『C++成长记』C++入门——命名空间&缺省参数

用SQL语句进行数据库查询(复杂查询)

【6s965-fall2022】神经架构搜索 Neural Architecture Search

阿里P8晒1月工资条，看完真的狠狠扎心了…

sql server相关学习sql语句

自动驾驶规划 – 5次多项式拟合

简介

3次多项式

5次多项式

5次多项式拟合

相关文章

最新关注

热文推荐