动手学运动规划: 2.1.c 基于5次多项式的参数方程曲线代码解析

最新推荐文章于 2024-10-30 16:47:45 发布

自动驾驶小白说

最新推荐文章于 2024-10-30 16:47:45 发布

阅读量1.2k

点赞数 38

分类专栏：动手学运动规划文章标签：算法自动驾驶

本文链接：https://blog.csdn.net/2403_86993842/article/details/142578702

版权

动手学运动规划专栏收录该内容

22 篇文章 0 订阅

订阅专栏

醉后不知天在水，满船清梦压星河。— 唐温如

🏰代码及环境配置：请参考0.2 环境配置和代码运行 | 动手学运动规划!

本节提供了基于5次多项式的参数方程曲线的代码测试

python3 tests/curves/quintic_polynomial.py

2.1.c.1 基于5次多项式的参数方程代码实现

在tests/curves/quintic_polynomial.py 我们定义了QuinticPolynomial , 它实现了5次多项式的参数方程的参数求解.

class 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.
        self.a0 = xs
        self.a1 = vxs
        self.a2 = axs / 2.0

        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,
            ]
        )
        x = np.linalg.solve(A, b)

        self.a3 = x[0]
        self.a4 = x[1]
        self.a5 = x[2]

我们需要给出多项式在起点和终点的状态约束: $x_s,v_s,a_s$ 和 $x_e,v_e,a_e$ , 求解过程上一节解释过.

求解之后, QuinticPolynomial提供了相关接口来计算5次多项式的0阶, 1阶, 2阶导数:

    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**5
        )

        return xt

    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**4
        )

        return xt

    def calc_second_derivative(self, t):
        xt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t**2 + 20 * self.a5 * t**3

        return xt

    def calc_third_derivative(self, t):
        xt = 6 * self.a3 + 24 * self.a4 * t + 60 * self.a5 * t**2

        return xt

2.1.c.2 基于5次多项式的参数方程曲线代码测试

在main函数中, 我们给出多项式在起点和终点的状态约束: $x_s,v_s,a_s$ 和 $x_e,v_e,a_e$ . 调用了quintic_polynomials_planner , 它求解了x,y的参数方程, 并将参数方程的结果插值提取出来用于可视化.

正如上节所述,因为无法确定曲线结果是否有效, 所以这里采取了对总时间T采样的方式, 寻找是否存在有效轨迹.

def quintic_polynomials_planner(
    sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_accel, max_jerk, dt
):
		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)
        
        for t in np.arange(0.0, T + dt, dt):
	        ...			
	        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 *= -1
	            ra.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 *= -1
	            rj.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!!")
	            break