当前位置：首页 > news >正文

基于二次规划的路径规划与速度规划：从MATLAB到C++的实践

news 2026/5/12 20:31:21

基于二次规划(QP)的路径规划和速度规划 matlab代码实现 + 详细文档 picewise jerk path picewise jerk speed 更新: c++版本已完成，qt可视化。

最近在做一个关于路径规划和速度规划的项目，主要基于二次规划（QP）的方法。虽然之前对QP有所了解，但真正实现起来还是遇到了不少问题。好在经过一段时间的努力，终于完成了MATLAB的验证和C++的实现，并且用QT做了一个简单的可视化界面。下面分享一下我的学习和实践过程。

1. 路径规划：从理论到代码

路径规划是自动驾驶和机器人导航中的核心问题。二次规划（QP）是一种常用的方法，它可以通过优化目标函数来找到满足约束条件的最优路径。

QP的基本思想

QP的优化目标通常是路径的平滑性和安全性。比如，我们希望路径尽可能平滑，同时避开障碍物。数学上，QP可以表示为：

\min_{x} \quad \frac{1}{2}x^T Q x + c^T x \\

\text{s.t.} \quad A x \leq b \\

基于二次规划(QP)的路径规划和速度规划 matlab代码实现 + 详细文档 picewise jerk path picewise jerk speed 更新: c++版本已完成，qt可视化。

其中，$x$是决策变量（比如路径点的位置），$Q$是一个正定矩阵，决定了优化的目标函数形式。

MATLAB实现

MATLAB的优化工具箱提供了quadprog函数，可以方便地求解QP问题。下面是一个简单的路径规划示例代码：

% 定义QP问题 n = 10; % 路径点数 Q = eye(n); % 简单的平滑性目标 c = zeros(n,1); % 无偏置项 % 添加约束条件（比如路径必须在某个区域内） A = [ones(1,n); -ones(1,n)]; b = [10; -5]; % 路径必须在y=5到y=10之间 % 求解QP x = quadprog(Q, c, A, b); % 可视化结果 plot(x, 'b-o'); title('QP路径规划结果'); xlabel('点编号'); ylabel('位置');

这段代码虽然简单，但展示了QP的基本用法。实际应用中，约束条件会更复杂，比如需要考虑避障、路径长度等因素。

2. 速度规划：平滑性与实时性

速度规划的目标是为路径上的每个点分配一个合适的速度，使得整体运动尽可能平滑，同时满足安全性和舒适性要求。这里我们采用分段 jerk（加加速度）的方法。

分段 jerk 的意义

Jerk是加速度的变化率，反映的是加速度的平滑性。在实际应用中，过大的jerk会导致乘坐不舒适，因此需要对jerk进行约束。

分段 jerk 的方法是将路径划分为多个小段，在每段内保持jerk恒定。这种方法可以在保证平滑性的同时，降低计算复杂度。

C++实现

由于实际应用中需要实时性，MATLAB的效率可能不够，因此我们用C++重新实现了速度规划算法。下面是一个关键的代码片段：

#include <vector> #include <Eigen/Dense> using namespace Eigen; void computeSpeedProfile(const std::vector<double>& path, std::vector<double>& speed) { int n = path.size(); MatrixXd A(n, n); VectorXd b(n); // 构建QP问题 for (int i = 0; i < n; ++i) { A(i, i) = 2.0; // jerk的权重 if (i > 0) A(i, i-1) = -1.0; if (i < n-1) A(i, i+1) = -1.0; } // 边界条件 b(0) = 0.0; // 初始jerk为0 b(n-1) = 0.0; // 终止jerk为0 // 求解QP LLT<MatrixXd> lltOfA(A); VectorXd x = lltOfA.solve(b); // 速度积分 double v = 0.0; for (int i = 0; i < n; ++i) { v += x(i); // 累积jerk得到加速度 speed[i] = v; } }

这段代码使用了Eigen库来求解线性方程组。虽然QP的求解在这里被简化为一个线性问题，但在实际应用中，可能需要更复杂的约束条件。

3. QT可视化：从代码到界面

为了更直观地展示路径规划和速度规划的结果，我们用QT做了一个简单的可视化界面。下面是一个界面的截图：

!QT可视化界面

QT界面的主要功能

路径显示：用折线图显示规划的路径。
速度显示：用柱状图显示每个点的速度。
参数设置：允许用户调整QP的权重和约束条件。

QT代码示例

下面是一个简单的QT界面代码片段：

#include <QApplication> #include <QWidget> #include <QVBoxLayout> #include <QPlotWidget> int main(int argc, char *argv[]) { QApplication app(argc, argv); QWidget window; QVBoxLayout *layout = new QVBoxLayout(&window); // 创建绘图组件 QPlotWidget *pathPlot = new QPlotWidget; QPlotWidget *speedPlot = new QPlotWidget; // 添加到布局 layout->addWidget(pathPlot); layout->addWidget(speedPlot); // 更新数据 auto updatePlot = [pathPlot, speedPlot]() { // 更新路径和速度数据 // ... }; // 显示窗口 window.show(); return app.exec(); }