轨迹优化 | 基于边界中间值问题(BIVP)的路径平滑求解器(附C++/Python仿真)

0 专栏介绍

🔥课设、毕设、创新竞赛必备！🔥本专栏涉及更高阶的运动规划算法轨迹优化实战，包括：曲线生成、碰撞检测、安全走廊、优化建模(QP、SQP、NMPC、iLQR等)、轨迹优化(梯度法、曲线法等)，每个算法都包含代码实现加深理解

🚀详情：运动规划实战进阶：轨迹优化篇

---

1 最小控制问题一般形式

优化变量$z\in {\mathbb{R}}^m$的最小控制优化问题的一般性数学描述为：
$$\begin{array}{rl}\underset{z(t_0),t=t_k}{\min }& {\int }_{t_0}^{t_k}\mathbf{v}(t{)}^T\mathbf{W}\mathbf{v}(t)+\rho (t_k-t_0)\\ \text{s.t.}& \mathbf{G}(z(t),\cdots ,z^{(s)}(t))⩽0,\forall t\in [t_0,t_k]\\ & z(t)\in \mathcal{C},\forall t\in [t_0,t_k]\\ & z^{[s-1]}(t_i)={\bar{z}}_i,1⩽i⩽k,r_i⩽s\\ & z^{[s-1]}(t_0)={\bar{z}}_0,z^{[s-1]}(t_k)={\bar{z}}_k\end{array}$$
其中定义$z^{[s-1]}(t)\triangleq (z^T,{\dot{z}}^T,\cdots ,z^{(s-1{)}^T}{)}^T$，$\mathbf{v}(t)\triangleq z^{(s)}(t)$。$z(t)$是微分平坦空间的输出轨迹，$T$是轨迹分配的总时长。当 $s=2$时为​​加速度(acceleration)、 $s=3$时为加加速度(jerk)、 $s=4$时为加加加速度(snap)。目标函数期望轨迹高阶微分量$\mathbf{v}(t)$尽可能平滑，有利于减少控制输入的突变，降低机械部件的磨损，平滑的轨迹也有益于舒适性的提升；同时运动时间尽可能短。$\mathbf{G}(z(t),\cdots ,z^{(s)}(t))⩽0$是优化问题的不等式约束，例如约束轨迹符合智能体物理限制。$z(t)\in \mathcal{C}$代表安全性约束，即要求运动轨迹不与障碍发生重叠。$z^{[s-1]}(t_i)={\bar{z}}_i$是优化问题的中间点约束，$z^{[s-1]}(t_0)={\bar{z}}_0$ 与 $z^{[s-1]}(t_k)={\bar{z}}_k$是优化问题的边界条件，约束轨迹的首末各阶状态。

2 边界中间值问题

2.1 问题形式化

当第一节中一般优化问题的约束条件仅保留边界约束和中间点约束时，简化为边界中间值问题（Boundary Intermediate Value Problem, BIVP），描述如下：

$$\begin{array}{rl}\min & {\int }_{t_0}^{t_k}\mathbf{v}(t{)}^T\mathbf{W}\mathbf{v}(t)+\rho (t_k-t_0)\\ \text{s.t.}& z_{[r_i-1]}^{(i-1)}(t_i)={\hat{z}}_i,1⩽i<k,r_i⩽s(8.4)\\ & z_{[s-1]}^{(s-1)}(t_0)={\hat{z}}_0,z_{[s-1]}^{(s-1)}(t_k)={\hat{z}}_k\end{array}$$

如上定义的BIVP最优解$z^{\ast }(t):[t_0,t_k]\to {\mathbb{R}}^n$的充要条件为：

• $z^{\ast }$ 的第$i$段轨迹是 $2s-1$ 次多项式；
• 边界条件与中间点条件成立；
• $z^{\ast }$ 在 $t=t_i$ 处满足 ${\bar{r}}_i-1$ 次连续可微，其中 ${\bar{r}}_i=2s-r_i$；

举例而言，对于 $s=3$ 且 $r_i=1$ 的Minimum jerk过路径点问题，最优性条件要求轨迹是 $n=2s-1=5$ 次多项式，同时在中间点 $t_i$ 处满足四阶连续可微，即路径、速度、加速度、加加速度以及更高次的加加速度在分段点连续

2.2 优化求解

根据 BIVP 最优性原理，最优轨迹$z^{\ast }$包含$2ks$个未知的多项式参数。考虑到边界条件提供$2s$个约束，中间点条件提供$r_i(k-1)$个约束，中间点连续可微条件提供${\bar{r}}_i(k-1)$个约束，共计$2ks$个约束方程，表明 BIVP 的最优轨迹$z^{\ast }$有且只有一条，在应用时可以直接依据边值和中间值约束解析地求解$z^{\ast }$，不需要显式地计算优化问题。

形式化地，设n阶多项式为$z(t)={\sum }_{i=0}^n{p}_i{t}^i$

$$\mathbf{p}={\left[\begin{array}{llll}{p}_0& p_1& \cdots & p_n\end{array}\right]}^T$$

是轨迹参数向量。单个多项式曲线过于简单，一段复杂的轨迹通常需要通过设置中间点将轨迹按时间分为$k$段，每段用最优的 $n=2s-1$ 次多项式曲线表示

$$z_i(t)={\mathbf{p}}_i^T\mathbf{\beta }(t-t_i)，t_{i-1}\le t<t_i$$

其中$\mathbf{\beta }(t)={\left[\begin{array}{llllc}1& t& t^2& \cdots & t^{2s-1}\end{array}\right]}^T$。考虑到平坦变量通常不是一维（例如车辆是二维、无人机是四维），将上式在平坦变量维度上进行堆叠，同时引入时间间隔 $T_i=t_i-t_{i-1}$ ，得到

$${\mathbf{z}}_i(t)={\mathbf{P}}_i^T\mathbf{\beta }(T)，0\le T<T_i$$

其中${\mathbf{P}}_i=\left[\begin{array}{llll}{\mathbf{p}}_{i,1}& {\mathbf{p}}_{i,2}& \cdots & {\mathbf{p}}_{i,m}\end{array}\right]\in {\mathbb{R}}^{2s\times m}$。接着在分段数上堆叠得到完整多项式系数矩阵

$$\mathbf{P}=\left[\begin{array}{l}{\mathbf{P}}_1\\ {\mathbf{P}}_2\\ ⋮\\ {\mathbf{P}}_k\end{array}\right]\in {\mathbb{R}}^{2sk\times m}$$

根据BIVP最优性原理的边值条件得到2s个边值约束方程

$${\mathbf{F}}_0{\mathbf{P}}_1={\mathbf{D}}_0，{\mathbf{E}}_k{\mathbf{P}}_k={\mathbf{D}}_k$$

其中

$${\mathbf{F}}_0={\left[\begin{array}{lll}\mathbf{\beta }(0)& \cdots & {\mathbf{\beta }}^{(s-1)}(0)\end{array}\right]}^T\in {\mathbb{R}}^{s\times 2s}$$

$${\mathbf{E}}_k={\left[\begin{array}{lll}\mathbf{\beta }(T_k)& \cdots & {\mathbf{\beta }}^{(s-1)}(T_k)\end{array}\right]}^T\in {\mathbb{R}}^{s\times 2s}$$

等式右边的边值${\mathbf{D}}_0\in {\mathbb{R}}^{s\times m}$ 与 ${\mathbf{D}}_k\in {\mathbb{R}}^{s\times m}$提前给定。

根据BIVP最优性原理的中间值约束和连续性约束，每个中间点提供$2s$个约束方程：

$$\left[\begin{array}{ll}{\mathbf{E}}_i& {\mathbf{F}}_i\end{array}\right]\left[\begin{array}{c}{\mathbf{P}}_i\\ {\mathbf{P}}_{i+1}\end{array}\right]=\left[\begin{array}{c}{\mathbf{D}}_i\\ 0_{{\vec{r}}_i\times m}\end{array}\right]$$

其中

$${\mathbf{D}}_i\in {\mathbb{R}}^{r_i\times m}$$

$${\mathbf{E}}_i={\left[\mathbf{\beta }\left(T_i\right)\cdots {\mathbf{\beta }}^{\left(r_i-1\right)}\left(T_i\right)\mid \mathbf{\beta }\left(T_i\right)\cdots {\mathbf{\beta }}^{\left({\vec{r}}_i-1\right)}\left(T_i\right)\right]}^T\in {\mathbb{R}}^{2s\times 2s}$$

$${\mathbf{F}}_i={\left[0_{2s\times r_i}\mid -\mathbf{\beta }(0)\cdots -{\mathbf{\beta }}^{\left({\vec{r}}_i-1\right)}(0)\right]}^T\in {\mathbb{R}}^{2s\times 2s}$$

前$r_i$ 行用于中间值约束；后 ${\bar{r}}_i$ 行用于连续性约束。进一步，将所有约束方程堆叠可得：

$$\mathbf{M}=\left[\begin{array}{ccccc}{\mathbf{F}}_0& 0& 0& \cdots & 0\\ {\mathbf{E}}_1& {\mathbf{F}}_1& 0& \cdots & 0\\ 0& {\mathbf{E}}_2& {\mathbf{F}}_2& \cdots & 0\\ ⋮& ⋮& ⋮& \ddots & ⋮\\ 0& 0& 0& \cdots & {\mathbf{F}}_{k-1}\\ 0& 0& 0& \cdots & {\mathbf{E}}_k\end{array}\right]\in {\mathbb{R}}^{2ks\times 2ks}$$
$$\mathbf{D}=\left[\begin{array}{c}{\mathbf{D}}_0\\ {\mathbf{D}}_1\\ 0\\ ⋮\\ {\mathbf{D}}_{k-1}\\ 0\\ {\mathbf{D}}_k\end{array}\right]\in {\mathbb{R}}^{2ks\times m}$$

考虑到$M$是稀疏带状矩阵，可以通过LU分解的方式以线性时间复杂度求解 $\mathbf{M}\mathbf{P}=\mathbf{D}$，而无需进行 $O\left(N^2\right)$ 复杂度的矩阵求逆。需要注意，时间微分约束的阶次越高，多项式轨迹的次数越高，可能造成函数解析区间过小，产生插值两端剧烈振荡的龙格现象(runge phenomenon)。

3 算法仿真

3.1 C++实现

为了可以通用地支持不同维度、边界阶数与中间阶数的BIVP问题求解，我设计了下面的求解器模板类

template <typename problem_type>
struct ProblemTraits
{// 1. problem dimension// 2. boundary_order// 3. intermediate_order
};template <typename problem_type>
class BIVPSolver
{
public:using Traits = ProblemTraits<problem_type>;static constexpr size_t boundary_order = Traits::boundary_order;static constexpr size_t intermediate_order = Traits::intermediate_order;static_assert(intermediate_order <= boundary_order, "intermediate_order cannot exceed boundary_order");using BoundaryState = Eigen::Matrix<double, boundary_order + 1, Traits::problem_dim>;using IntermediateState = Eigen::Matrix<double, intermediate_order + 1, Traits::problem_dim>;using IntermediateStates = std::vector<IntermediateState>;
}

通过ProblemTraits结构体参数化问题特性，将问题维度、边界阶数和中间阶数抽象为类型成员，使求解器能灵活适配车辆二维、无人机四维等不同维度问题场景，以及不同维度最小控制问题的工程需求。利用Eigen矩阵的高效运算能力以及模板编译器推导的特性，可以使求解计算效率达到落地的需求。

接下来看一个使用案例，以二维路径平滑问题为例实例化一个求解器

struct WaypointProblem
{static constexpr size_t problem_dim = 2;         // x, ystatic constexpr size_t boundary_order = 2;      // position, velocity, acclerationstatic constexpr size_t intermediate_order = 0;  // position
};template <>
struct rmp::common::math::ProblemTraits<WaypointProblem>
{static constexpr size_t problem_dim = WaypointProblem::problem_dim;static constexpr size_t boundary_order = WaypointProblem::boundary_order;static constexpr size_t intermediate_order = WaypointProblem::intermediate_order;
};using Solver = rmp::common::math::BIVPSolver<WaypointProblem>;

设置以下的测试用例

std::vector<std::pair<double, double>> waypoints = {{ 0.0, 0.0 }, { 4.0, 2.0 }, { 9.0, 0.5 }, { 5.5, -1.0 }, { 10.0, -4.0 }
};Solver solver;
Solver::BoundaryState start_boundary, end_boundary;
start_boundary.setZero();
end_boundary.setZero();start_boundary(0, 0) = waypoints[0].first;
start_boundary(0, 1) = waypoints[0].second;
start_boundary(1, 0) = 0.0;
start_boundary(1, 1) = 0.0;
start_boundary(2, 0) = 0.0;
start_boundary(2, 1) = 0.0;
end_boundary(0, 0) = waypoints.back().first;
end_boundary(0, 1) = waypoints.back().second;
end_boundary(1, 0) = 0.0;
end_boundary(1, 1) = 0.0;
end_boundary(2, 0) = 0.0;
end_boundary(2, 1) = 0.0;solver.setBoundaryConstraints(start_boundary, end_boundary);Solver::IntermediateStates intermediate_constraints;
for (size_t i = 1; i < waypoints.size() - 1; ++i)
{Solver::IntermediateState constraint;constraint.setZero();constraint(0, 0) = waypoints[i].first;constraint(0, 1) = waypoints[i].second;intermediate_constraints.push_back(constraint);
}
solver.setIntermediateConstraints(intermediate_constraints);double dt = 10.0;
std::vector<double> time_allocation;
double total_time = 0.0;
for (size_t i = 0; i < waypoints.size() - 1; ++i)
{time_allocation.push_back(dt);total_time += dt;
}
solver.setTimeAllocation(time_allocation);Eigen::MatrixXd solution = solver.solve();

得到以下的优化结果

3.2 Python仿真

核心代码如下所示

def solve(self):if not self.is_boundary_constraint_set:raise RuntimeError(f"No valid boundary constraints.")if not self.is_intermediate_constraint_set:raise RuntimeError(f"No valid intermediate constraints.")s = self.params.boundary_constraint_order + 1ri = self.params.intermediate_constraint_order + 1ri_bar = 2 * s - riM = np.zeros((s * self.segments_num * 2, s * self.segments_num * 2))D = np.zeros((s * self.segments_num * 2, self.params.problem_dim))# initial boundaryfor r in range(s):M[r, 0 : 2 * s] = self.beta(0, r)D[r, :] = self.D0[r, :]# intermediate boundaryfor seg in range(self.segments_num - 1):start_row_index = s + 2 * s * segstart_col_index = 2 * s * segfor r in range(ri):M[start_row_index + r, start_col_index : start_col_index + 2 * s] = self.beta(self.time_allocation[seg], r)D[start_row_index + r, :] = self.Di[seg][r, :]start_row_index += rifor r in range(ri_bar):M[start_row_index + r, start_col_index : start_col_index + 2 * s] = self.beta(self.time_allocation[seg], r)M[start_row_index + r,start_col_index + 2 * s : start_col_index + 4 * s,] = -self.beta(0, r)# terminal boundarystart_row_index = (self.segments_num - 1) * s * 2 + sfor r in range(s):M[start_row_index + r, (self.segments_num - 1) * s * 2 :] = self.beta(self.time_allocation[-1], r)D[start_row_index + r, :] = self.Dk[r, :]return np.linalg.inv(M) @ D

完整工程代码请联系下方博主名片获取

---

🔥 更多精彩专栏：

• 《ROS从入门到精通》
• 《Pytorch深度学习实战》
• 《机器学习强基计划》
• 《运动规划实战精讲》
• …

👇源码获取 · 技术交流 · 抱团学习 · 咨询分享 请联系👇