关键点采样的思路

关键点采样使用时间步长 ts 来在初始轨迹方程中取点。

在上一步的初始轨迹生成后，轨迹方程保存在 gl_traj 中，ego 的关键点采样就是选取合适的 ts，在轨迹方程中代入 ts ，选取一定量的关键点，用于后续的 B 样条拟合。

ts的选取

初始步长的确定

采样步长的初始值使用参数步长 (pp_.ctrl_pt_dist) 以及参数最大速度 (pp_.max_vel_) 来确定：

// ts 初始数值先使用参数控制点距离与最大速度参数来设定
double ts = (start_pt - local_target_pt).norm() > 0.1 ? pp_.ctrl_pt_dist / pp_.max_vel_ * 1.2 : pp_.ctrl_pt_dist / pp_.max_vel_ * 5;

动态确定最终合适的步长

初始步长不一定是最合适的，可能会有采样点比较密集或者稀疏的问题，因此需要再进行一次动态确定最终步长的过程，在 ego 中是使用了一个 do，while 循环，在进行循环之前，先把初始的 ts 进行放大：

ts *= 1.5; // ts will be divided by 1.5 in the next  将最初的时间间隔放大，避免因为初始 ts 过小导致采样点很密集

之后在每次循环中，逐步减小 ts ，然后判断根据当前 ts 采样到的点是否满足要求，不满足就继续降低 ts：

// 以下循环用于找到合适的时间步 ts，保证分割点合适，不会太过密集，同时也控制数量需要超大于等于7个，用于后续的B样条拟合
do
{ts /= 1.5;  // 每次循环时将 ts 除以 1.5，逐渐找到合适的时间步point_set.clear();    // 每次修改时间步后，清除点flag_too_far = false;Eigen::Vector3d last_pt = gl_traj.evaluate(0);  // 获取轨迹起点// 根据当前的时间步 来循环采点，判断是否需要进一步缩小 ts 来采点for (t = 0; t < time; t += ts){Eigen::Vector3d pt = gl_traj.evaluate(t); // 计算 t 时刻的轨迹位置// 检查相邻的采样点距离是否超限if ((last_pt - pt).norm() > pp_.ctrl_pt_dist * 1.5){flag_too_far = true;  // 距离过远时，置位标志，进一步缩小 ts break;}last_pt = pt;point_set.push_back(pt);}
} while (flag_too_far || point_set.size() < 7); // To make sure the initial path has enough points.

根据最终的出循环条件，找到一个距离足够远，且数量大于等于 7 个的采样点的 ts。

之后将此 ts 对应的起点，终点的速度，加速度记录下来：

t -= ts;  // 回退到最后一个采样点的时刻(因为上面循环中有最后一个 t += ts)
// 记录轨迹起点，终点的速度，加速度信息
start_end_derivatives.push_back(gl_traj.evaluateVel(0));
start_end_derivatives.push_back(local_target_vel);
start_end_derivatives.push_back(gl_traj.evaluateAcc(0));
start_end_derivatives.push_back(gl_traj.evaluateAcc(t));

python代码验证

编写代码来验证效果的话，只需要在上一篇的基础上，添加一下总时间，以及动态计算即可，然后把点给画出来看看效果：

import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
class PolynomialTraj:"""实现EGO-Planner中描述的五次多项式轨迹生成"""def __init__(self, coeffs, total_time):self.coeffs = coeffs  # 多项式系数 (3x6矩阵)self.total_time = total_time  # 轨迹总时间@staticmethoddef 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: 轨迹总时间返回:PolynomialTraj对象"""# 构造约束矩阵Ct = total_timeC = np.zeros((6, 6))# t=0时刻的约束C[0, 5] = 1  # 位置: x(0) = c0C[1, 4] = 1  # 速度: v(0) = c1C[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):# 构造约束向量BB = 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 PolynomialTraj(coeffs, total_time)def evaluate(self, time):"""计算指定时刻的位置"""t = np.clip(time, 0, self.total_time)# 计算位置 (x(t) = c5*t^5 + c4*t^4 + c3*t^3 + c2*t^2 + c1*t + c0)pos = np.zeros(3)pos[0] = np.dot(self.coeffs[0], [t **5, t** 4, t ** 3, t ** 2, t, 1])pos[1] = np.dot(self.coeffs[1], [t **5, t** 4, t ** 3, t ** 2, t, 1])pos[2] = np.dot(self.coeffs[2], [t **5, t** 4, t ** 3, t ** 2, t, 1])return posdef evaluate_full(self, time):"""计算指定时刻的位置、速度和加速度"""t = np.clip(time, 0, self.total_time)pos = np.zeros(3)vel = np.zeros(3)acc = np.zeros(3)# 计算位置pos[0] = np.dot(self.coeffs[0], [t **5, t** 4, t ** 3, t ** 2, t, 1])pos[1] = np.dot(self.coeffs[1], [t **5, t** 4, t ** 3, t ** 2, t, 1])pos[2] = np.dot(self.coeffs[2], [t **5, t** 4, t ** 3, t ** 2, t, 1])# 计算速度vel[0] = np.dot(self.coeffs[0], [5 * t **4, 4 * t** 3, 3 * t ** 2, 2 * t, 1, 0])vel[1] = np.dot(self.coeffs[1], [5 * t **4, 4 * t** 3, 3 * t ** 2, 2 * t, 1, 0])vel[2] = np.dot(self.coeffs[2], [5 * t **4, 4 * t** 3, 3 * t ** 2, 2 * t, 1, 0])# 计算加速度acc[0] = np.dot(self.coeffs[0], [20 * t **3, 12 * t** 2, 6 * t, 2, 0, 0])acc[1] = np.dot(self.coeffs[1], [20 * t **3, 12 * t** 2, 6 * t, 2, 0, 0])acc[2] = np.dot(self.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_accif distance < critical_distance:# 短距离：仅加速即可到达目标return np.sqrt(distance / max_acc) * 2  # 修正系数使结果更合理else:# 长距离：加速→匀速→减速time_acc_dec = 2 * max_vel / max_acc  # 加速和减速时间之和time_constant = (distance - critical_distance) / max_vel  # 匀速时间return time_acc_dec + time_constant
def find_optimal_ts_and_sample(traj, ctrl_pt_dist, min_point_num=7):"""动态调整采样时间步ts，生成满足条件的采样点集参数：traj: PolynomialTraj对象ctrl_pt_dist: B样条控制点基准间距（m）min_point_num: 最小采样点数量返回：optimal_ts: 最优采样时间步（s）point_set: 采样点集（Nx3）sample_times: 采样时刻（s）"""# 初始ts猜测（先放大1.5倍，后续逐步缩小）initial_ts_guess = traj.total_time / 5  # 初始预估5个点optimal_ts = initial_ts_guess * 1.5  # 预放大flag_too_far = Truepoint_set = []sample_times = []# 循环调整ts直到满足条件while flag_too_far or len(point_set) < min_point_num:optimal_ts /= 1.5  # 每次缩小1.5倍point_set.clear()sample_times.clear()flag_too_far = False# 起点初始化last_pt = traj.evaluate(0.0)point_set.append(last_pt)sample_times.append(0.0)# 按当前ts采样t = optimal_tswhile t < traj.total_time + 1e-6:  # 允许微小误差，避免漏采终点current_pt = traj.evaluate(t)# 检查相邻点距离是否超限dist_between = np.linalg.norm(current_pt - last_pt)if dist_between > ctrl_pt_dist * 1.5:flag_too_far = Truebreak  # 超限，退出当前采样point_set.append(current_pt)sample_times.append(t)last_pt = current_ptt += optimal_ts# 确保最后一个点是轨迹终点final_pt = traj.evaluate(traj.total_time)if np.linalg.norm(final_pt - point_set[-1]) > 1e-3:point_set.append(final_pt)sample_times.append(traj.total_time)# 转换为numpy数组point_set = np.array(point_set)sample_times = np.array(sample_times)return optimal_ts, point_set, sample_times# *********************************** 动态寻找 ts 并生成采样点 ************************************
def visualize_trajectory(traj, point_set, sample_times, optimal_ts, ctrl_pt_dist):"""可视化轨迹和采样点"""# 生成高密度轨迹点用于绘制连续曲线dense_times = np.linspace(0, traj.total_time, 1000)dense_points = np.array([traj.evaluate(t) for t in dense_times])# 创建3D图形fig = plt.figure(figsize=(15, 10))# 3D轨迹图ax3d = fig.add_subplot(111, projection='3d')ax3d.plot(dense_points[:, 0], dense_points[:, 1], dense_points[:, 2],'gray', linewidth=1, label='traj')ax3d.scatter(point_set[:, 0], point_set[:, 1], point_set[:, 2],c='red', s=50, zorder=5, label='sample')ax3d.scatter(point_set[0, 0], point_set[0, 1], point_set[0, 2],c='green', s=100, marker='o', label='start')ax3d.scatter(point_set[-1, 0], point_set[-1, 1], point_set[-1, 2],c='blue', s=100, marker='*', label='end')ax3d.set_xlabel('X (m)')ax3d.set_ylabel('Y (m)')ax3d.set_zlabel('Z (m)')ax3d.set_title('3D traj')ax3d.legend()plt.tight_layout()plt.show()
def ego_ts_main():# 轨迹参数设置start_pt = np.array([0.0, 0.0, 0.0])  # 起点位置start_vel = np.array([0.1, 0.1, 0.0])  # 起点速度start_acc = np.array([0.0, 0.0, 0.0])  # 起点加速度end_pt = np.array([8.0, 4.0, 2.0])  # 终点位置end_vel = np.array([0.1, 0.1, 0.0])  # 终点速度end_acc = np.array([0.0, 0.0, 0.0])  # 终点加速度max_vel = 2.0  # 最大速度max_acc = 1.0  # 最大加速度ctrl_pt_dist = 0.8  # 控制点基准间距# 计算轨迹距离和时间distance = np.linalg.norm(end_pt - start_pt)total_time = calculate_trajectory_time(distance, max_vel, max_acc)print(f"轨迹距离: {distance:.2f}m, 计算得到总时间: {total_time:.2f}s")# 生成多项式轨迹traj = PolynomialTraj.one_segment_traj_gen(start_pt, start_vel, start_acc,end_pt, end_vel, end_acc,total_time)# 动态确定最优采样时间步并采样optimal_ts, point_set, sample_times = find_optimal_ts_and_sample(traj, ctrl_pt_dist)print(f"最优采样时间步: {optimal_ts:.4f}s, 采样点数量: {len(point_set)}")# 可视化结果visualize_trajectory(traj, point_set, sample_times, optimal_ts, ctrl_pt_dist)