ego-planner中生成初始轨迹的核心思路
ego中由于是不依赖建图，因此其在生成初始轨迹时，就采用的是另外的思路：位置采用五阶多项式的方式，然后再求导得到速度，加速度的方程，这样就可以保证初始轨迹的加速度，速度，位置是连续的。
位置：x(t) = c₅t⁵ + c₄t⁴ + c₃t³ + c₂t² + c₁t + c₀
速度：vₓ(t) = 5c₅t⁴ + 4c₄t³ + 3c₃t² + 2c₂t + c₁
加速度：aₓ(t) = 20c₅t³ + 12c₄t² + 6c₃t + 2c₂
以上的公式也比较容易求解。（PS：文章只是解析其原理，并不会苛求与代码完全对应）
列举线性方程
列出线性方程：C·coeff = B
以上的 c0 - c5 参数都是 coeff 矩阵的系数，那么之后的核心问题就转化为了以上的 coeff 矩阵各个系数的求解，通俗一点说，就是求解出一套 coeff 矩阵的系数，满足全时域的约束条件。
其中 B 矩阵就是约束向量，表示轨迹起点坐标，终点坐标，起点速度，终点速度，起点加速度，终点加速度。即：
B = [start_pt.x, start_vel.x, start_acc.x, end_pt.x, end_vel.x, end_acc.x]ᵀ
C矩阵就是上面求导方程中除去系数的部分，如下所示：
| 行号 | 对应约束条件 | 矩阵行向量（按[c₅, c₄, c₃, c₂, c₁, c₀]顺序） | 数学依据（代入多项式公式） |
|---|---|---|---|
| 0 | t=0 时，x (0)=start_pt.x | [0, 0, 0, 0, 0, 1] | x(0) = c₅·0⁵ + ... + c₀ = c₀ → 1·c₀ = start_pt.x |
| 1 | t=0 时，vₓ(0)=start_vel.x | [0, 0, 0, 0, 1, 0] | vₓ(0) = 5c₅·0⁴ + ... + c₁ = c₁ → 1·c₁ = start_vel.x |
| 2 | t=0 时，aₓ(0)=start_acc.x | [0, 0, 0, 2, 0, 0] | aₓ(0) = 20c₅·0³ + ... + 2c₂ = 2c₂ → 2·c₂ = start_acc.x |
| 3 | t=T 时，x (T)=end_pt.x | [T⁵, T⁴, T³, T², T, 1] | x(T) = c₅T⁵ + ... + c₀ → T⁵·c₅ + ... + 1·c₀ = end_pt.x |
| 4 | t=T 时，vₓ(T)=end_vel.x | [5T⁴, 4T³, 3T², 2T, 1, 0] | vₓ(T) = 5c₅T⁴ + ... + c₁ → 5T⁴·c₅ + ... + 1·c₁ = end_vel.x |
| 5 | t=T 时，aₓ(T)=end_acc.x | [20T³, 12T², 6T, 2, 0, 0] | aₓ(T) = 20c₅T³ + ... + 2c₂ → 20T³·c₅ + ... + 2·c₂ = end_acc.x |
由于C是满秩矩阵（6 个约束彼此线性无关，没有一行能由其他行推导得出），方程组有且仅有唯一解 —— 这意味着：存在且仅存在一组系数，能满足所有运动学约束。
求解方程组 C·coeff = B，就是找到唯一的coeff，使得所有约束条件同时成立。
传入的时间 t 就是轨迹的总时间。
之后求解这个方程组就可以得到多项式的系数，ego 使用的是 QR 分解的方法。对应代码：
PolynomialTraj PolynomialTraj::one_segment_traj_gen(const Eigen::Vector3d &start_pt, const Eigen::Vector3d &start_vel, const Eigen::Vector3d &start_acc,
const Eigen::Vector3d &end_pt, const Eigen::Vector3d &end_vel, const Eigen::Vector3d &end_acc,
double t)
{
Eigen::MatrixXd C = Eigen::MatrixXd::Zero(6, 6), Crow(1, 6);
Eigen::VectorXd Bx(6), By(6), Bz(6);
C(0, 5) = 1;
C(1, 4) = 1;
C(2, 3) = 2;
Crow << pow(t, 5), pow(t, 4), pow(t, 3), pow(t, 2), t, 1;
C.row(3) = Crow;
Crow << 5 * pow(t, 4), 4 * pow(t, 3), 3 * pow(t, 2), 2 * t, 1, 0;
C.row(4) = Crow;
Crow << 20 * pow(t, 3), 12 * pow(t, 2), 6 * t, 2, 0, 0;
C.row(5) = Crow;
Bx << start_pt(0), start_vel(0), start_acc(0), end_pt(0), end_vel(0), end_acc(0);
By << start_pt(1), start_vel(1), start_acc(1), end_pt(1), end_vel(1), end_acc(1);
Bz << start_pt(2), start_vel(2), start_acc(2), end_pt(2), end_vel(2), end_acc(2);
Eigen::VectorXd Cofx = C.colPivHouseholderQr().solve(Bx);
Eigen::VectorXd Cofy = C.colPivHouseholderQr().solve(By);
Eigen::VectorXd Cofz = C.colPivHouseholderQr().solve(Bz);
vector<double> cx(6), cy(6), cz(6);
for (int i = 0; i < 6; i++)
{
cx[i] = Cofx(i);
cy[i] = Cofy(i);
cz[i] = Cofz(i);
}
PolynomialTraj poly_traj;
poly_traj.addSegment(cx, cy, cz, t);
return poly_traj;
}
轨迹总时间的计算
在 ego 中，轨迹总时间的计算分为两个方案，是根据距离与最大速度，最大加速度约束来计算的，分界点就是临界距离的计算，这里就写作 d_critical 吧，当然 ego 的代码里没有这样取名字，它直接用了问号表达式，文章只是解析原理，不用太深究完全对应代码。
根据匀加速运动的规律，当无人机起点与终点的距离小于临界距离时，无人机是不需要进入匀速阶段就能到达位置的，当距离大于临界距离时，才会需要进入匀速阶段，最终呈现为：匀加速->匀速->匀减速 三阶段来达到目标位置。
因此可以看到其核心是是否会需要进入匀速阶段，这里将其分为两部分，短距离与长距离。如下图所示，当然，ego在代码中没有考虑先加速不匀速后减速的情况，这里先按照他的代码逻辑来画了：
临界距离满足：v_max² = 2 * a_max * d_critical
可以得到：d_critical = v_max² / a_max
临界距离也可以理解为不需要经过匀速阶段能达到的最大距离。
对应的 ego 代码为：
// 根据距离计算轨迹总时间
double dist = (start_pt - local_target_pt).norm();
// 如果是短距离，则时间计算就根据匀加速运动(从静止开始以最大加速度 a_max 加速到最大速度 v_max)公式： d = vmax^2 / a_max
// 如果求解出来的加速到最大能达到的距离大于当前距离，说明不需要经历匀速阶段，此时距离公式就为 d = 0.5 * a_max * T^2， 这里代码求解时间简化为 T = sqrt(d / a_max)
// 如果距离比较远时，需要经历匀加速，匀速，匀减速三个阶段，因此就求解三个阶段的时间，相加得到最终时间
double time = pow(pp_.max_vel_, 2) / pp_.max_acc_ > dist ? sqrt(dist / pp_.max_acc_) : (dist - pow(pp_.max_vel_, 2) / pp_.max_acc_) / pp_.max_vel_ + 2 * pp_.max_vel_ / pp_.max_acc_;
短距离时间计算
短距离就是临界距离以内，则看上图就可以得到时间公式：
d = 0.5 * (a_max * T) * T
正常求解出来的 T = sqrt(2*d / a_max)
但需要注意的是，ego 代码中是 T = sqrt(d / a_max) ，不知道是不是它做的简化操作。后续的学习中如果发现原因了再更新。
长距离时间计算
长距离计算就分为三个阶段来计算。
匀加速：T1 = v_max / a_max
匀减速：T3 = T1 (因为对称)
匀速：d2 = d - 2 * d1，
d2 = v_max * T2，可得 T2 = d2 / v_max = (d - v_max² / a_max) / v_max。
总距离就 T1 + T2 + T3 即可，对应 ego 的代码。
python 代码验证
这里编写了一个python代码来验证：
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
class PolynomialTraj:
"""实现EGO-Planner中描述的五次多项式轨迹生成"""
@staticmethod
def one_segment_traj_gen(start_pt, start_vel, start_acc,
end_pt, end_vel, end_acc, total_time):
"""
生成单段五次多项式轨迹，满足论文中6个运动学约束条件
参数:
start_pt: 起点位置 [x, y, z]
start_vel: 起点速度 [vx, vy, vz]
start_acc: 起点加速度 [ax, ay, az]
end_pt: 终点位置 [x, y, z]
end_vel: 终点速度 [vx, vy, vz]
end_acc: 终点加速度 [ax, ay, az]
total_time: 轨迹总时间
返回:
多项式系数 (3x6矩阵)，每行对应x, y, z方向的系数 [c5, c4, c3, c2, c1, c0]
"""
# 构造约束矩阵C，如论文中描述的五次多项式约束形式
t = total_time
C = np.zeros((6, 6))
# t=0时刻的约束
C[0, 5] = 1 # 位置: x(0) = c0
C[1, 4] = 1 # 速度: v(0) = c1
C[2, 3] = 2 # 加速度: a(0) = 2*c2
# t=T时刻的约束
C[3] = [t ** 5, t ** 4, t ** 3, t ** 2, t, 1] # 位置
C[4] = [5 * t ** 4, 4 * t ** 3, 3 * t ** 2, 2 * t, 1, 0] # 速度
C[5] = [20 * t ** 3, 12 * t ** 2, 6 * t, 2, 0, 0] # 加速度
# 求解三个方向的多项式系数
coeffs = np.zeros((3, 6))
for i in range(3):
# 构造约束向量B，对应论文中的边界条件
B = np.array([
start_pt[i], # 起点位置
start_vel[i], # 起点速度
start_acc[i], # 起点加速度
end_pt[i], # 终点位置
end_vel[i], # 终点速度
end_acc[i] # 终点加速度
])
# 求解线性方程组 C·coeff = B，得到系数
coeffs[i] = np.linalg.solve(C, B)
return coeffs
@staticmethod
def evaluate(coeffs, time):
"""计算指定时刻的位置、速度和加速度"""
pos = np.zeros(3)
vel = np.zeros(3)
acc = np.zeros(3)
t = time
# 计算位置 (x(t) = c5*t^5 + c4*t^4 + c3*t^3 + c2*t^2 + c1*t + c0)
pos[0] = np.dot(coeffs[0], [t ** 5, t ** 4, t ** 3, t ** 2, t, 1])
pos[1] = np.dot(coeffs[1], [t ** 5, t ** 4, t ** 3, t ** 2, t, 1])
pos[2] = np.dot(coeffs[2], [t ** 5, t ** 4, t ** 3, t ** 2, t, 1])
# 计算速度 (v(t) = 5c5*t^4 + 4c4*t^3 + 3c3*t^2 + 2c2*t + c1)
vel[0] = np.dot(coeffs[0], [5 * t ** 4, 4 * t ** 3, 3 * t ** 2, 2 * t, 1, 0])
vel[1] = np.dot(coeffs[1], [5 * t ** 4, 4 * t ** 3, 3 * t ** 2, 2 * t, 1, 0])
vel[2] = np.dot(coeffs[2], [5 * t ** 4, 4 * t ** 3, 3 * t ** 2, 2 * t, 1, 0])
# 计算加速度 (a(t) = 20c5*t^3 + 12c4*t^2 + 6c3*t + 2c2)
acc[0] = np.dot(coeffs[0], [20 * t ** 3, 12 * t ** 2, 6 * t, 2, 0, 0])
acc[1] = np.dot(coeffs[1], [20 * t ** 3, 12 * t ** 2, 6 * t, 2, 0, 0])
acc[2] = np.dot(coeffs[2], [20 * t ** 3, 12 * t ** 2, 6 * t, 2, 0, 0])
return pos, vel, acc
def calculate_trajectory_time(distance, max_vel, max_acc):
"""
根据论文中的方法计算轨迹总时间
短距离: 仅加速阶段
长距离: 加速→匀速→减速阶段
"""
# 计算临界距离：达到最大速度所需的距离
critical_distance = (max_vel ** 2) / max_acc
if distance < critical_distance:
# 短距离：仅加速即可到达目标
return np.sqrt(distance / max_acc)
else:
# 长距离：加速→匀速→减速
time_acc_dec = 2 * max_vel / max_acc # 加速和减速时间之和
time_constant = (distance - critical_distance) / max_vel # 匀速时间
return time_acc_dec + time_constant
def ego_planner_init_traj_main():
# 1. 设置轨迹参数
start_pt = np.array([0.0, 0.0, 0.0]) # 起点位置
start_vel = np.array([0.5, 0.0, 0.2]) # 起点速度
start_acc = np.array([0.1, 0.1, 0.0]) # 起点加速度
end_pt = np.array([8.0, 4.0, 2.0]) # 终点位置
end_vel = np.array([0.5, 0.3, 0.0]) # 终点速度
end_acc = np.array([0.0, 0.1, -0.1]) # 终点加速度
max_vel = 2.0 # 最大速度，符合论文中的动力学约束
max_acc = 1.0 # 最大加速度，符合论文中的动力学约束
# 2. 计算距离和轨迹总时间
distance = np.linalg.norm(end_pt - start_pt)
total_time = calculate_trajectory_time(distance, max_vel, max_acc)
print(f"轨迹距离: {distance:.2f}m")
print(f"计算得到的轨迹总时间: {total_time:.2f}s")
# 3. 生成五次多项式轨迹
coeffs = PolynomialTraj.one_segment_traj_gen(
start_pt, start_vel, start_acc,
end_pt, end_vel, end_acc,
total_time
)
# 4. 采样轨迹并验证
time_points = np.linspace(0, total_time, 1000)
pos_list = []
vel_list = []
acc_list = []
for t in time_points:
pos, vel, acc = PolynomialTraj.evaluate(coeffs, t)
pos_list.append(pos)
vel_list.append(vel)
acc_list.append(acc)
pos_array = np.array(pos_list)
vel_array = np.array(vel_list)
acc_array = np.array(acc_list)
# 5. 验证边界条件是否满足（检查误差）
print("\n边界条件验证:")
print(f"起点位置误差: {np.linalg.norm(pos_array[0] - start_pt):.6f}m")
print(f"终点位置误差: {np.linalg.norm(pos_array[-1] - end_pt):.6f}m")
print(f"起点速度误差: {np.linalg.norm(vel_array[0] - start_vel):.6f}m/s")
print(f"终点速度误差: {np.linalg.norm(vel_array[-1] - end_vel):.6f}m/s")
print(f"起点加速度误差: {np.linalg.norm(acc_array[0] - start_acc):.6f}m/s²")
print(f"终点加速度误差: {np.linalg.norm(acc_array[-1] - end_acc):.6f}m/s²")
# 6. 可视化轨迹（3D路径和各方向的位置、速度、加速度曲线）
fig = plt.figure(figsize=(15, 12))
# 3D轨迹图
ax1 = fig.add_subplot(221, projection='3d')
ax1.plot(pos_array[:, 0], pos_array[:, 1], pos_array[:, 2], 'b-')
ax1.scatter(start_pt[0], start_pt[1], start_pt[2], c='g', s=100, label='start')
ax1.scatter(end_pt[0], end_pt[1], end_pt[2], c='r', s=100, label='end')
ax1.set_xlabel('X (m)')
ax1.set_ylabel('Y (m)')
ax1.set_zlabel('Z (m)')
ax1.set_title('3Dtraj')
ax1.legend()
# 位置曲线
ax2 = fig.add_subplot(222)
ax2.plot(time_points, pos_array[:, 0], 'r-', label='X')
ax2.plot(time_points, pos_array[:, 1], 'g-', label='Y')
ax2.plot(time_points, pos_array[:, 2], 'b-', label='Z')
ax2.set_xlabel('time (s)')
ax2.set_ylabel('x (m)')
ax2.set_title('x-time')
ax2.legend()
# 速度曲线
ax3 = fig.add_subplot(223)
ax3.plot(time_points, vel_array[:, 0], 'r-', label='X')
ax3.plot(time_points, vel_array[:, 1], 'g-', label='Y')
ax3.plot(time_points, vel_array[:, 2], 'b-', label='Z')
ax3.axhline(y=max_vel, color='k', linestyle='--', label='v_max')
ax3.axhline(y=-max_vel, color='k', linestyle='--')
ax3.set_xlabel('time (s)')
ax3.set_ylabel('vel (m/s)')
ax3.set_title('vel-time')
ax3.legend()
# 加速度曲线
ax4 = fig.add_subplot(224)
ax4.plot(time_points, acc_array[:, 0], 'r-', label='X')
ax4.plot(time_points, acc_array[:, 1], 'g-', label='Y')
ax4.plot(time_points, acc_array[:, 2], 'b-', label='Z')
ax4.axhline(y=max_acc, color='k', linestyle='--', label='a_max')
ax4.axhline(y=-max_acc, color='k', linestyle='--')
ax4.set_xlabel('time (s)')
ax4.set_ylabel('acc (m/s²)')
ax4.set_title('acc-time')
ax4.legend()
plt.tight_layout()
plt.show()