ego(2)---初始軌跡生成后的關鍵點采樣

在初始的多項式軌跡生成后，是要經過一個關鍵點采樣，使用關鍵點來進行后續的 B 樣條曲線擬合的。即：

初始多項式擬合->關鍵點采樣->B樣條擬合

關鍵點采樣的思路

關鍵點采樣使用時間步長 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)