1 時空解耦運動規劃

在自主移動系統的運動規劃體系中，時空解耦的遞進式架構因其高效性與工程可實現性被廣泛采用。這一架構將復雜的高維運動規劃問題分解為路徑規劃與速度規劃兩個層次化階段：路徑規劃階段基于靜態環境約束生成無碰撞的幾何軌跡；速度規劃階段則在此基礎上引入時間維度，通過動態障礙物預測、運動學約束建模等為機器人或車輛賦予時間軸上的運動規律。這種解耦策略雖在理論上犧牲了時空聯合優化的最優性，卻通過分層降維大幅提升了復雜場景下的計算效率，使其成為自動駕駛、服務機器人等實時系統的經典方案。

2 PJSO速度規劃原理

2.1 優化變量

分段加加速度優化(Piecewise Jerk Speed Optimizer, PJSO)算法是常用的縱向速度優化方式，核心原理是用三次多項式表示s-t圖中每個時間區間$\left[t_k,t_{k+1}\right)$的速度曲線，并約束每個區間加加速度相等，其中$k=0,1,?,n?2$， 可取為路徑規劃的路點數量。PJSO算法的優化變量為

$$\mathbf{x}={\left[\begin{array}{ccccccccccc}l{s}_0& {\dot{s}}_0& {\ddot{s}}_0& ?& s_k& {\dot{s}}_k& {\ddot{s}}_k& ?& s_{n?1}& {\dot{s}}_{n?1}& {\ddot{s}}_{n?1}\end{array}\right]}_{3n\times 1}^T$$

2.2 代價函數

代價函數可設計為

$$\begin{gathered} J=\sum _{i=0}^{n?2}\left(w_s{\left(s_i?s_{i,\mathrm{r}\mathrm{e}\mathrm{f}}\right)}^2+w_v{\left({\dot{s}}_i?{\dot{s}}_{i,\mathrm{r}\mathrm{e}\mathrm{f}}\right)}^2+w_a{\ddot{s}}_i^2+w_j{s}_i^{(3)2}\right) \\ +w_{s,\mathrm{e}\mathrm{n}\mathrm{d}}{\left(s_{n?1}?s_{\mathrm{e}\mathrm{n}\mathrm{d}}\right)}^2+w_{v,\mathrm{e}\mathrm{n}\mathrm{d}}{\left({\dot{s}}_{n?1}?{\dot{s}}_{\mathrm{e}\mathrm{n}\mathrm{d}}\right)}^2+w_{a,\mathrm{e}\mathrm{n}\mathrm{d}}{\left({\ddot{s}}_n?{\ddot{s}}_{\mathrm{e}\mathrm{n}\mathrm{d}}\right)}^2 \end{gathered}$$

通過

$$s_i^{(3)}=\frac{{\ddot{s}}_{i+1}?{\ddot{s}}_i}{\Delta t}$$

隱式地消除加加速度變量，代入代價函數并消除常數項可化簡為

$$\begin{array}{rl}J=& w_s\sum _{i=0}^{n?2}{s}_i^2+w_{s,\mathrm{e}\mathrm{n}\mathrm{d}}{s}_{n?1}^2?2w_s\sum _{i=0}^{n?2}{s}_{i,\mathrm{r}\mathrm{e}\mathrm{f}}{s}_i?2w_{s,\mathrm{e}\mathrm{n}\mathrm{d}}{s}_{\mathrm{e}\mathrm{n}\mathrm{d}}{s}_{n?1}\\ & +w_v\sum _{i=0}^{n?2}{\dot{s}}_i^2+w_{v,\mathrm{e}\mathrm{n}\mathrm{d}}{\dot{s}}_{n?1}^2?2w_v\sum _{i=0}^{n?2}{\dot{s}}_{i,\mathrm{r}\mathrm{e}\mathrm{f}}{\dot{s}}_i?2w_{v,\mathrm{e}\mathrm{n}\mathrm{d}}{\dot{s}}_{\mathrm{e}\mathrm{n}\mathrm{d}}{\dot{s}}_{n?1}\\ & +\left(w_a+\frac{w_j}{\Delta t^2}\right){\ddot{s}}_0^2+\left(w_a+w_{a,\mathrm{e}\mathrm{n}\mathrm{d}}+\frac{w_j}{\Delta t^2}\right){\ddot{s}}_{n?1}^2?2w_{a,\mathrm{e}\mathrm{n}\mathrm{d}}{\ddot{s}}_{\mathrm{e}\mathrm{n}\mathrm{d}}{\ddot{s}}_{n?1}\\ & +\left(w_a+\frac{2w_j}{\Delta t^2}\right)\sum _{i=1}^{n?2}{\ddot{s}}_i^2?\frac{2w_j}{\Delta t^2}\sum _{i=0}^{n?2}{\ddot{s}}_i{\ddot{s}}_{i+1}\end{array}$$

從而得到核矩陣

$$\mathbf{P}={?\begin{array}{ccc}{\mathbf{P}}_s& & \\ & {\mathbf{P}}_{\dot{s}}& \\ & & {\mathbf{P}}_{\ddot{s}}\end{array}?}_{3n\times 3n}$$

和偏置向量

$$\mathbf{q}={?\begin{array}{c}?2w_s{s}_{0,\mathrm{r}\mathrm{e}\mathrm{f}}\\ ?\\ ?2w_s{s}_{n?2,\mathrm{r}\mathrm{e}\mathrm{f}}\\ ?2w_{s,\mathrm{e}\mathrm{n}\mathrm{d}}{s}_{\mathrm{e}\mathrm{n}\mathrm{d}}\\ ?2w_v{\dot{s}}_{0,\mathrm{r}\mathrm{e}\mathrm{f}}\\ ?\\ ?2w_v{\dot{s}}_{n?2,\mathrm{r}\mathrm{e}\mathrm{f}}\\ ?2w_{v,\mathrm{e}\mathrm{n}\mathrm{d}}{\dot{s}}_{\mathrm{e}\mathrm{n}\mathrm{d}}\\ 0\\ ?\\ 0\\ ?2w_{a,\mathrm{e}\mathrm{n}\mathrm{d}}{\ddot{s}}_{\mathrm{e}\mathrm{n}\mathrm{d}}\end{array}?}_{3n\times 1}$$

2.3 約束條件

根據運動學公式可得運動學等式約束

$$\{\begin{array}{l}{\dot{s}}_{i+1}={\dot{s}}_i+{\ddot{s}}_i\Delta t+\frac{1}{2}{j}_i\Delta t^2={\dot{s}}_i+\frac{1}{2}{\ddot{s}}_i\Delta t+\frac{1}{2}{\ddot{s}}_{i+1}\Delta t\\ s_{i+1}=s_i+{\dot{s}}_i\Delta t+\frac{1}{2}{\ddot{s}}_i\Delta t^2+\frac{1}{6}{j}_i\Delta t^3=s_i+{\dot{s}}_i\Delta t+\frac{1}{3}{\ddot{s}}_i\Delta t^2+\frac{1}{6}{\ddot{s}}_{i+1}\Delta t^3\end{array}$$

約束初始狀態

$${\dot{s}}_0={\dot{s}}_{\mathrm{i}\mathrm{n}\mathrm{i}\mathrm{t}},{\dot{s}}_0={\dot{s}}_{\mathrm{i}\mathrm{n}\mathrm{i}\mathrm{t}},{\ddot{s}}_0={\ddot{s}}_{\mathrm{i}\mathrm{n}\mathrm{i}\mathrm{t}}$$

此外還需保證各階狀態量滿足不等式約束

$$s_{\min }?s_i?s_{\max },{\dot{s}}_{\min }?{\dot{s}}_i?{\dot{s}}_{\max },{\ddot{s}}_{\min }?{\ddot{s}}_i?{\ddot{s}}_{\max },{\ddot{s}}_{\min }^{(3)}\Delta t?{\ddot{s}}_{i+1}?{\ddot{s}}_i?{\ddot{s}}_{\max }^{(3)}\Delta t$$

從而得到約束矩陣

$$\mathbf{A}={?\begin{array}{c}{\mathbf{A}}_{\mathrm{i}\mathrm{n}\mathrm{i}\mathrm{t},3\times 3n}\\ {\mathbf{A}}_{\mathrm{b}\mathrm{o}\mathrm{u}\mathrm{n}\mathrm{d},3n+\left(n?1\right)\times 3n}\\ {\mathbf{A}}_{\mathrm{s}\mathrm{y}\mathrm{s},2\left(n?1\right)\times 3n}\end{array}?}_{6n\times 3n},\mathbf{l}={?\begin{array}{c}{\mathbf{l}}_{\mathrm{i}\mathrm{n}\mathrm{i}\mathrm{t}}\\ {\mathbf{l}}_{\mathrm{b}\mathrm{o}\mathrm{u}\mathrm{n}\mathrm{d}}\\ {\mathbf{l}}_{\mathrm{s}\mathrm{y}\mathrm{s}}\end{array}?}_{6n\times 1},\mathbf{u}={?\begin{array}{c}{\mathbf{u}}_{\mathrm{i}\mathrm{n}\mathrm{i}\mathrm{t}}\\ {\mathbf{u}}_{\mathrm{b}\mathrm{o}\mathrm{u}\mathrm{n}\mathrm{d}}\\ {\mathbf{u}}_{\mathrm{s}\mathrm{y}\mathrm{s}}\end{array}?}_{6n\times 1}$$

2.4 二次規劃形式

綜上所述，可得到PJSO算法的二次規劃形式

$$\begin{gathered} \underset{\mathbf{x}}{\min }J={\mathbf{x}}^T\mathbf{P}\mathbf{x}+{\mathbf{q}}^T\mathbf{x} \\ \mathrm{s}.\mathrm{t}.\mathbf{l}?\mathbf{A}\mathbf{x}?\mathbf{u} \end{gathered}$$

3 算法仿真

3.1 ROS C++仿真

核心代碼如下所示：

bool PiecewiseJerkVelocityPlanner::plan(const Points3d& waypoints, VelocityProfile& velocity_profile)
{int dim = static_cast<int>(waypoints.size());std::vector<double> s_ref(dim, 0.0);for (int i = 1; i < dim; ++i){s_ref[i] =s_ref[i - 1] + std::hypot(waypoints[i].x() - waypoints[i - 1].x(), waypoints[i].y() - waypoints[i - 1].y());}x_max_ = s_ref.back();// construct QP ProblemEigen::MatrixXd P, A;std::vector<c_float> l, u, q;_calKernel(dim, P);_calOffset(dim, s_ref, q);_calAffineConstraint(dim, A);_calBoundary(dim, s_ref, l, u);std::vector<c_float> P_data;std::vector<c_int> P_indices;std::vector<c_int> P_indptr;int ind_P = 0;for (int col = 0; col < P.cols(); ++col){P_indptr.push_back(ind_P);for (int row = 0; row <= col; ++row){P_data.push_back(P(row, col));P_indices.push_back(row);ind_P++;}}P_indptr.push_back(ind_P);std::vector<c_float> A_data;std::vector<c_int> A_indices;std::vector<c_int> A_indptr;int ind_A = 0;for (int col = 0; col < A.cols(); ++col){A_indptr.push_back(ind_A);for (int row = 0; row < A.rows(); ++row){double data = A(row, col);if (std::fabs(data) > kMathEpsilon){A_data.push_back(data);A_indices.push_back(row);++ind_A;}}}A_indptr.push_back(ind_A);// solveOSQPWorkspace* work = nullptr;OSQPData* data = reinterpret_cast<OSQPData*>(c_malloc(sizeof(OSQPData)));OSQPSettings* settings = reinterpret_cast<OSQPSettings*>(c_malloc(sizeof(OSQPSettings)));osqp_set_default_settings(settings);settings->verbose = false;settings->warm_start = true;data->n = 3 * dim;data->m = 6 * dim;data->P = csc_matrix(data->n, data->n, P_data.size(), P_data.data(), P_indices.data(), P_indptr.data());data->A = csc_matrix(data->m, data->n, A_data.size(), A_data.data(), A_indices.data(), A_indptr.data());data->q = q.data();data->l = l.data();data->u = u.data();osqp_setup(&work, data, settings);osqp_solve(work);auto status = work->info->status_val;if ((status < 0) || (status != 1 && status != 2)){R_DEBUG << "failed optimization status: " << work->info->status;return false;}// parsefor (int i = 0; i < dim; ++i){velocity_profile.push(dt_ * i, work->solution->x[i], work->solution->x[dim + i], work->solution->x[2 * dim + i]);}// Cleanuposqp_cleanup(work);c_free(data->A);c_free(data->P);c_free(data);c_free(settings);return true;
}

這里設置初始速度$v_0=1.0m/s$，初始加速度$a_0=0m/s^2$；終點速度$v_0=0.0m/s$，終點加速度$a_0=0m/s^2$

3.2 Python仿真

核心代碼如下所示：

def process(self, path: List[Point3d]) -> List[Dict]:velocity_profiles = []path_segs, path_refs = PathPlanner.getPathSegments(path)for i, path_seg in enumerate(path_segs):s_ref = path_refs[i]n = max(len(path_refs[i]),round(self.r * (self.dx_max ** 2 + s_ref[-1] * self.ddx_max) \/ (self.dx_max * self.ddx_max * self.dt)))s_ref = np.linspace(0, s_ref[-1], n)'''min x^T P x + q^T xs.t. l <= Ax <= u'''P = sparse.csc_matrix(self.calKernel(n))q = self.calOffset(n, s_ref)A = sparse.csc_matrix(self.calAffineConstraint(n))l, u = self.calBoundary(n, s_ref)solver = osqp.OSQP()solver.setup(P, q, A, l, u, verbose=False)res = solver.solve()sol = res.xvelocity_profiles.append({"time": [self.dt * i for i in range(n)],"path": PathPlanner.pathInterpolation(path_seg, n),"distance": sol[:n].tolist(),"velocity": sol[n:2 * n].tolist(),"acceleration": sol[2 * n:3 * n].tolist(),})return velocity_profiles

對于如下圖所示的倒車路徑

規劃的速度和加速度曲線如下所示，中間速度為0處為換擋點

---

🔥 更多精彩專欄：

• 《ROS從入門到精通》
• 《Pytorch深度學習實戰》
• 《機器學習強基計劃》
• 《運動規劃實戰精講》
• …

👇源碼獲取 · 技術交流 · 抱團學習 · 咨詢分享 請聯系👇