轨迹优化 | 基于边界值问题(BVP)的无约束路径平滑(附ROS C++/Python仿真)

0 专栏介绍

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

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

1 边界值问题建模

在轨迹优化 | 基于边界中间值问题(BIVP)的路径平滑求解器(附C++/Python仿真)中，我们介绍过边界中间值问题（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\\ & z_{[s-1]}^{(s-1)}(t_0)={\hat{z}}_0,z_{[s-1]}^{(s-1)}(t_k)={\hat{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)$

将BIVP的中间值约束移除将得到更特殊的边界值问题(Boundary Value Problem, BVP):

$$\begin{gathered} \underset{\mathbf{z}\left(t\right),T}{\min }{\int }_{t_0}^T\mathbf{v}{\left(t\right)}^T\mathbf{Wv}\left(t\right)+\rho T \\ \mathrm{s}.\mathrm{t}.{\mathbf{z}}^{\left[s-1\right]}\left(0\right)={\bar{\mathbf{z}}}_0,{\mathbf{z}}^{\left[s-1\right]}\left(T\right)={\bar{\mathbf{z}}}_T \end{gathered}$$

其仍然满足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$；

所以最优解满足无中间值条件的约束方程

$$\left[\begin{array}{c}{\mathbf{F}}_0\\ {\mathbf{E}}_k\end{array}\right]{\mathbf{P}}_1=\left[\begin{array}{c}{\mathbf{D}}_0\\ {\mathbf{D}}_k\end{array}\right]$$

对于BVP问题来说，可以更进一步写出矩阵的结构。对于单个微分平坦维度有

$$\mathbf{d}={\mathbf{A}}_F\left(T\right)\mathbf{p}$$

其中$\mathbf{p}={\left[\begin{array}{cccc}{p}_0& p_1& \cdots & p_n\end{array}\right]}^T\in {\mathbb{R}}^{2s}$， $\mathbf{d}=\left[\begin{array}{cccccc}{z}_0& \cdots & z_0^{\left(s-1\right)}& z_T& \cdots & z_T^{\left(s-1\right)}\end{array}\right]\in {\mathbb{R}}^{2s}$

$${\mathbf{A}}_F\left(t\right)=\left[\begin{array}{cc}\mathbf{E}& \mathbf{O}\\ \mathbf{F}\left(t\right)& \mathbf{G}\left(t\right)\end{array}\right]$$

各分块矩阵形式为

$$\begin{array}{rl}{\mathbf{E}}_{ij}& =\{\begin{array}{l}\left(i-1\right)!\mathrm{if}i=j\\ 0\mathrm{if}i\ne j\end{array},\\ {\mathbf{F}}_{ij}\left(t\right)& =\{\begin{array}{l}\frac{\left(j-1\right)!}{\left(j-i\right)!}{t}^{j-i}\mathrm{if}i⩽j\\ 0\mathrm{if}i>j\end{array},\\ {\mathbf{G}}_{ij}\left(t\right)& =\frac{\left(s+j-1\right)!}{\left(s+j-i\right)!}{t}^{s+j-i}\end{array}$$

为了避免大规模矩阵求逆，在应用中也可以直接采用逆矩阵版本

$$\mathbf{p}={\mathbf{A}}_B\left(T\right)\mathbf{d}$$

其中

$${\mathbf{A}}_B\left(t\right)=\left[\begin{array}{cc}\mathbf{U}& \mathbf{O}\\ \mathbf{V}\left(t\right)& \mathbf{W}\left(t\right)\end{array}\right]$$

各分块矩阵形式为

U i j = { 1 ( i − 1 ) !    i f i = j 0 i f i ≠ j , V i j ( t ) = ∑ k = 0 s − max ⁡ { i , j } ( − 1 ) k ( s i + k ) ( 2 s − j − k − 1 s − 1 ) ( j − 1 ) ! ( − 1 ) i t s + i − j W i j ( t ) = ∑ k = 0 s − max ⁡ { i , j } ( s − k − 1 i − 1 ) ( 2 s − j − k − 1 s − 1 ) ( j − 1 ) ! ( − 1 ) i + j t s + i − j \begin{aligned}\boldsymbol{U}_{ij}&=\begin{cases} \frac{1}{\left( i-1 \right) !}\,\, \mathrm{if} i=j\\ 0 \mathrm{if} i\ne j\\\end{cases}, \\\boldsymbol{V}_{ij}\left( t \right) &=\frac{\sum_{k=0}^{s-\max \left\{ i,j \right\}}{\left( -1 \right) ^k\left( \begin{array}{c} s\\ i+k\\\end{array} \right) \left( \begin{array}{c} 2s-j-k-1\\ s-1\\\end{array} \right)}}{\left( j-1 \right) !\left( -1 \right) ^it^{s+i-j}}\\\\\boldsymbol{W}_{ij}\left( t \right) &=\frac{\sum_{k=0}^{s-\max \left\{ i,j \right\}}{\left( \begin{array}{c} s-k-1\\ i-1\\\end{array} \right) \left( \begin{array}{c} 2s-j-k-1\\ s-1\\\end{array} \right)}}{\left( j-1 \right) !\left( -1 \right) ^{i+j}t^{s+i-j}}\end{aligned} Uij​Vij​(t)Wij​(t)​={(i−1)!1​ifi=j0ifi=j​,=(j−1)!(−1)its+i−j∑k=0s−max{i,j}​(−1)k(si+k​)(2s−j−k−1s−1​)​=(j−1)!(−1)i+jts+i−j∑k=0s−max{i,j}​(s−k−1i−1​)(2s−j−k−1s−1​)​​

对平坦变量的各个维度应用上述公式即可求解BVP最优轨迹

2 算法仿真

2.1 ROS C++仿真

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

struct WaypointProblem
{
  static constexpr size_t problem_dim = 2;         // x, y
  static constexpr size_t boundary_order = 2;      // position, velocity, accleration
  static 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>;

接着根据路径点约束设置求解器参数并求解即可

bool BIVPOptimizer::optimize(const Points3d& waypoints)
{
  solver_->reset();
  setBoundaryConstraints(waypoints.front(), waypoints.back());
  setIntermediateConstraints(waypoints);
  setTimeAllocation(waypoints);
  solution_ = std::move(solver_->solve());
  return true;
}

2.2 Python仿真

核心代码如下所示：

def optimize(self, waypoints: List[Point3d]) -> None:
	solver_params = SolverParameters(
	    problem_dim=2,  # x, y
	    boundary_constraint_order=2,  # position, velocity, accleration
	    intermediate_constraint_order=0,  # position
	)
	self.solver = BIVPSolver(solver_params)
	
	# boundary
	start_boundary = [
	    SecondOrderBVPState(x=waypoints[0].x(), dx=0.0, ddx=0.0),  # x
	    SecondOrderBVPState(x=waypoints[0].y(), dx=0.0, ddx=0.0),  # y
	]
	goal_boundary = [
	    SecondOrderBVPState(x=waypoints[-1].x(), dx=0.0, ddx=0.0),  # x
	    SecondOrderBVPState(x=waypoints[-1].y(), dx=0.0, ddx=0.0),  # y
	]
	self.solver.setBoundaryConstraints(start_boundary, goal_boundary)
	
	# intermediate
	intermediate_constraints = []
	for i in range(1, len(waypoints) - 1):
	    constraints = [ZeroOrderBVPState(x=waypoints[i].x()), ZeroOrderBVPState(x=waypoints[i].y())]  # position
	    intermediate_constraints.append(constraints)
	self.solver.setIntermediateConstraints(intermediate_constraints)
	
	# time allocation
	time_allocation = [self.duration_per_segment] * (len(waypoints) - 1)
	self.solver.setTimeAllocation(time_allocation)
	
	# solve
	self.solution = self.solver.solve()

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

🔥 更多精彩专栏：

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