HardPurus 发布的文章

Frenet坐标系:[$s$,$\dot{s}$,$\ddot{s}$,l,$\dot{l}$,$\ddot{l}$,${l}'$,${l}''$]

Cartesian坐标系:[$\vec{x} $,$v_{x}$,$a_{x}$,$\theta_{x}$,$k_{x}$]

 

$s$：Frenet纵坐标

$\dot{s}$=$\frac{ds}{dt} $：Frenet纵坐标对时间的导数，纵向速度

$\ddot{s}$=$\frac{\dot{ds}}{dt} $：纵向加速度

$l$：Frenet横坐标

$\dot{l}$=$\frac{dl}{dt}$：Frenet横向速度

$\ddot{l}$=$\frac{d\dot{l}}{dt}$：Frenet横向加速度

${l}'$=$\frac{dl}{ds}$：Frenet横向坐标对纵向坐标的导数

${l}''$=$\frac{\dot{dl}}{ds}$：Frenet横向坐标对纵向坐标的二阶导

$\vec{x} $：Cartesian坐标系下的一个向量

$\theta_{x}$：Cartesian坐标系下的朝向

$k_{x}$=$\frac{d{\theta_{x}}}{ds}$：曲率

$v_{x}$=$\left \| \dot{\vec{x} } \right \| _{2}$：Cartesian坐标系下的线速度

$a_{x}$=$\frac{dv_{x}}{dt}$：Cartesian坐标系下的加速度

推导过程：

笛卡尔坐标系与Frenet坐标系的转换

  

使用五阶多项式来表示随时间变化的位置：
$s(t)=c_{1} t^{4}+c_{2} t^{3}+c_{3} t^{2}+c_{4} t+c_{5}$

可以推导出:

速度(一阶导数):
$v(t)=\frac{d s}{d t}=4 c_{1} t^{3}+3 c_{2} t^{2}+2 c_{3} t+ c_{4}$

加速度(二阶导数)
$a(t)=\frac{d v}{d t}=12c_{1} t^{2}+6 c_{2} t+2 c_{3}$
约束条件:
1.初始条件：

在初始时刻$t_0$和终止时刻$t_1$分别设置约束：

$t_0=0$

初始位置：$s(t_0)=c_5=s_0$
初始速度：$v(t_0)=c_4=v_0$
初始加速度：$a(t_0)=2c_3=a_0$

 

2.终止条件约束：

终止位置：$s(t_1)=c_1t_1^4+c_2t_1^3+c_3t_1^2+c_4t_1+c_5=s_1$
终止速度：$v(t_1)=4c_1t_1^3+3c_2t_1^2+2c_3t_1+c_4=v_1$
终止加速度：$a(t_1)=12c_1t_1^2+6c_2t_1+2c_3=a_1$
 

方程组求解：

6个方程，可以解出6个系数$c_1$到$c_6$：
$c_5=s_0$（直接得出)
$c_4=v_0$   (直接得出)
$2c_3=a_0\Rightarrow c_3=\frac{a_0}{2} $
 

对于剩余的三个系数$c_1, c_2$,需要解以下矩阵方程：
$\begin{bmatrix}
  t_1^4& t_1^3\\
  4t_1^3&  3t_1^2\\
\end{bmatrix}\begin{bmatrix}
 c_1\\
 c_2\\
\end{bmatrix}=\begin{bmatrix}
 s_1-c_3t_1^2-c_4t_1-c_5\\
 v_1-2c_3t_1-c_4\\
\end{bmatrix}$


使用 LAPACK 库的 LU 分解法，解方程
python代码示例：

import numpy as np

def solve_quartic_coefficients(t1, s0, v0, a0, s1, v1, a1):
    """
    求解四次多项式 s(t) = c1*t^4 + c2*t^3 + c3*t^2 + c4*t + c5 的系数
    输入参数：
        t1: 终止时间
        s0, v0, a0: 初始位置、速度、加速度
        s1, v1, a1: 终止位置、速度、加速度
    返回：
        系数列表 [c1, c2, c3, c4, c5]
    """
    # 直接已知的系数
    c5 = s0
    c4 = v0
    c3 = a0 / 2

    # 构建矩阵 A 和向量 b
    A = np.array([
        [t1**4, t1**3],
        [4*t1**3, 3*t1**2]
    ])

    # 终止时刻的加速度方程：12*c1*t1^2 + 6*c2*t1 + 2*c3 = a1
    # 速度方程：4*c1*t1^3 + 3*c2*t1^2 + 2*c3*t1 + c4 = v1
    # 位移方程：c1*t1^4 + c2*t1^3 + c3*t1^2 + c4*t1 + c5 = s1
    b = np.array([
        s1 - c3*t1**2 - c4*t1 - c5,
        v1 - 2*c3*t1 - c4
    ])

    # 解线性方程组 Ax = b
    c1, c2 = np.linalg.solve(A, b)

    return [c1, c2, c3, c4, c5]

# 示例参数（与五次多项式示例一致）
t1 = 1.0
s0, v0, a0 = 0.0, 0.0, 0.0
s1, v1, a1 = 1.0, 0.0, 0.0

# 求解系数
coefficients = solve_quartic_coefficients(t1, s0, v0, a0, s1, v1, a1)
print("系数 c1 到 c5:", coefficients)

# 验证终止条件
t = t1
c1, c2, c3, c4, c5 = coefficients

s = c1*t**4 + c2*t**3 + c3*t**2 + c4*t + c5
v = 4*c1*t**3 + 3*c2*t**2 + 2*c3*t + c4
a = 12*c1*t**2 + 6*c2*t + 2*c3

print(f"验证 t={t}: s={s}, v={v}, a={a}")

 

 

 

 

 

使用五阶多项式来表示随时间变化的位置：
$s(t)=c_{1} t^{5}+c_{2} t^{4}+c_{3} t^{3}+c_{4} t^{2}+c_{5} t+c_{6}$

可以推导出:

速度(一阶导数):
$v(t)=\frac{d s}{d t}=5 c_{1} t^{4}+4 c_{2} t^{3}+3 c_{3} t^{2}+2 c_{4} t+c_{5}$

加速度(二阶导数)
$a(t)=\frac{d v}{d t}=20 c_{1} t^{3}+12 c_{2} t^{2}+6 c_{3} t+2 c_{4}$
约束条件:
1.初始条件：

在初始时刻$t_0$和终止时刻$t_1$分别设置约束：

$t_0=0$

初始位置：$s(t_0)=c_6=s_0$
初始速度：$v(t_0)=c_5=v_0$
初始加速度：$a(t_0)=2c_4=a_0$

 

2.终止条件约束：

终止位置：$s(t_1)=c_1t_1^5+c_2t_1^4+c_3t_1^3+c_4t_1^2+c_5t_1+c_6=s_1$
终止速度：$v(t_1)=5c_1t_1^4+4c_2t_1^3+3c_3t_1^2+2c_4t_1+c_5=v_1$
终止加速度：$a(t_1)=20c_1t_1^3+12c_2t_1^2+6c_3t_1+2c_4=a_1$
 

方程组求解：

6个方程，可以解出6个系数$c_1$到$c_6$：
$c_6=s_0$（直接得出)
$c_5=v_0$   (直接得出)
$2c_4=a_0\Rightarrow c_4=\frac{a_0}{2} $
 

对于剩余的三个系数$c_1, c_2, c_3$,需要解以下矩阵方程：
$\begin{bmatrix}
  t_1^5& t_1^4& t_1^3\\
  5t_1^4&  4t_1^3& 3t_1^2\\
  20t_1^3&  12t_1^2& 6t_1
\end{bmatrix}\begin{bmatrix}
 c_1\\
 c_2\\
 c_3
\end{bmatrix}=\begin{bmatrix}
 s_1-c_4t_1^2-c_5t_1-c_6\\
 v_1-2c_4t_1-c_5\\
 a_1-2c_4
\end{bmatrix}$


使用 LAPACK 库的 LU 分解法，解方程
python代码示例：

import numpy as np

def solve_quintic_coefficients(t1, s0, v0, a0, s1, v1, a1):
    """
    求解五阶多项式 s(t) = c1*t^5 + c2*t^4 + c3*t^3 + c4*t^2 + c5*t + c6 的系数
    输入参数：
        t1: 终止时间
        s0, v0, a0: 初始位置、速度、加速度
        s1, v1, a1: 终止位置、速度、加速度
    返回：
        系数列表 [c1, c2, c3, c4, c5, c6]
    """
    # 直接已知的系数
    c6 = s0
    c5 = v0
    c4 = a0 / 2

    # 构建矩阵 A 和向量 b
    A = np.array([
        [t1**5,      t1**4,      t1**3],
        [5*t1**4,    4*t1**3,    3*t1**2],
        [20*t1**3,   12*t1**2,   6*t1]
    ])

    b = np.array([
        s1 - c4*t1**2 - c5*t1 - c6,
        v1 - 2*c4*t1 - c5,
        a1 - 2*c4
    ])

    # 解线性方程组 Ax = b
    c1, c2, c3 = np.linalg.solve(A, b)

    return [c1, c2, c3, c4, c5, c6]

# 示例参数（与之前数学示例一致）
t1 = 1.0
s0, v0, a0 = 0.0, 0.0, 0.0
s1, v1, a1 = 1.0, 0.0, 0.0

# 求解系数
coefficients = solve_quintic_coefficients(t1, s0, v0, a0, s1, v1, a1)
print("系数 c1 到 c6:", coefficients)

# 验证终止条件
t = t1
c1, c2, c3, c4, c5, c6 = coefficients

s = c1*t**5 + c2*t**4 + c3*t**3 + c4*t**2 + c5*t + c6
v = 5*c1*t**4 + 4*c2*t**3 + 3*c3*t**2 + 2*c4*t + c5
a = 20*c1*t**3 + 12*c2*t**2 + 6*c3*t + 2*c4

print(f"验证 t={t}: s={s}, v={v}, a={a}")

 

 

 

 

 

  1. 1
  2. 2