DPMPC-Planner：复杂静态环境与动态障碍物下的无人机实时轨迹规划框架

文献信息

标题：DPMPC-Planner: A real-time UAV trajectory planning framework for complex static environments with dynamic obstacles
作者：Zhefan Xu, Di Deng, Yiping Dong, Kenji Shimada
单位：Carnegie Mellon University, Mechanical Engineering Department
发表：IEEE International Conference on Robotics and Automation (ICRA), 2022

一、研究背景与关键科学问题

1.1 研究背景

随着自主无人机在工业领域的应用日益广泛，复杂结构化环境中的在线轨迹生成对安全性和自主性至关重要。在这类场景中，无人机需要在杂乱环境中导航至目标，并确保与人类共存时的安全。

当前轨迹规划面临三大核心挑战：

复杂环境结构：需要考虑静态障碍物的复杂构型
动态障碍物：环境中存在移动的人类和机器人
不确定性：包括测量噪声和不可预测的移动障碍物行为带来的不确定性

1.2 现有方法的局限性

静态环境轨迹规划方法：

现有方法（如Raptor、Ego-planner等）依赖 occupancy map 或 ESDF map 等特定地图表示
这些方法在静态环境导航中证明了实时性能
根本缺陷：无法可靠地编码动态障碍物信息

动态障碍物避障方法：

基于模型预测控制（MPC）和几何障碍物表示的方法（如文献[8][9][10]）提供了可靠解决方案
局限：在轨迹生成中未考虑复杂静态环境结构

1.3 核心挑战

两大主要挑战使动态环境中的安全轨迹规划问题尚未被完美解决：

挑战	描述
环境复杂性	考虑无人机动力学的高维优化问题计算代价高昂
实时性要求	算法需同时考虑静态和动态障碍物，具备足够快的运行速度

1.4 本文目标

提出一种名为动态多项式模型预测控制（DPMPC） 的轨迹规划框架，实现复杂静态环境与动态障碍物共存的场景下的安全无人机导航。

二、研究方法与算法原理

2.1 系统框架概述

DPMPC框架包含四个核心模块：

全局路径规划器：输入目标，生成高层无碰撞航点路径
地图模块：提供碰撞检测功能
动态障碍物表示：获取障碍物位姿、包围盒及估计速度
DPMPC规划器：核心规划模块

双层规划架构：

复制代码

全局路径规划器 → 静态层（迭代走廊收缩算法） → 动态层（机会约束MPC） → 最终轨迹

2.2 静态层：基于迭代走廊收缩的轨迹生成

2.2.1 走廊边界框初始化

为轨迹上的每个采样位置定义走廊边界框尺寸 CboxC_{\text{box}}Cbox：

不要求走廊边界框本身无碰撞
初始尺寸可设为合理值（如0.5m）
采样位置通过对连续轨迹以 ΔT\Delta TΔT 分辨率划分获得

2.2.2 多项式轨迹优化

目标函数：最小化轨迹的snap（位置的四阶导数）

对于 N+1N+1N+1 个航点，整个轨迹由穿过相邻航点的分段连续多项式段构成：

σtraj(t)={1σtraj(t),2σtraj(t),...,Nσtraj(t)}\sigma_{\text{traj}}(t) = \{1\sigma{\text{traj}}(t), 2\sigma{\text{traj}}(t), \ldots, N\sigma{\text{traj}}(t)\}σtraj(t)={1σtraj(t),2σtraj(t),...,Nσtraj(t)}

每段轨迹表示为：

nσtraj(t)=[∑i=0dci,xnti∑i=0dci,ynti∑i=0dci,znti],t∈[tn,tn+1]n\sigma{\text{traj}}(t) = \begin{bmatrix} \sum_{i=0}^{d} c_{i,x}^n t^i \\ \sum_{i=0}^{d} c_{i,y}^n t^i \\ \sum_{i=0}^{d} c_{i,z}^n t^i \end{bmatrix}, \quad t \in [t_n, t_{n+1}]nσtraj(t)= ∑i=0dci,xnti∑i=0dci,ynti∑i=0dci,znti ,t∈[tn,tn+1]

其中 tnt_ntn 为到达第 nnn 个航点的时刻，ddd 为多项式阶数。

约束条件：

轨迹必须经过所有航点（式3b）
连接点处保持连续性（式3c-3d）
轨迹样本位置满足边界框约束（式3e-3f）

该优化问题可表述为二次规划（QP）问题。

2.2.3 迭代走廊收缩算法

算法流程：

复制代码

算法1：迭代走廊收缩
1: M → 静态地图
2: Cbox → 走廊边界框尺寸
3: Vd → 期望速度
4: α → 0.9  (用户定义的收缩因子)
5: Tcollision → true
6: while Tcollision do:
7:   Popt → initPolyOptimization(Vd)
8:   Popt.addCorridorConstraints(Cbox, ΔT)
9:   σtraj → Popt.solve()
10:  Tcollision → M.checkCollision(σtraj, ΔT)
11:  Cbox → α · Cbox
12: return σtraj

关键特性：

收缩因子 α\alphaα 是调优参数，通常范围为0.5到1
通过迭代缩小走廊边界框，逐步收紧约束
虽然需要多次重启优化，但QP formulation保证了实时性能
实验中通常只需5-6次迭代即可收敛

2.3 动态层：机会约束模型预测控制

2.3.1 MPC问题 formulation

MPC有两个核心需求：

(a) 跟踪静态轨迹
(b) 避开动态障碍物

优化问题：

min⁡x1:N,u1:N∑k=1N−1∥x¨k−x¨kref∥2+∥uk∥2\min_{x_{1:N}, u_{1:N}} \sum_{k=1}^{N-1} \|\ddot{x}_k - \ddot{x}_k^{\text{ref}}\|^2 + \|u_k\|^2x1:N,u1:Nmink=1∑N−1∥x¨k−x¨kref∥2+∥uk∥2

约束条件：

初始状态约束：x0=x(t0)x_0 = x(t_0)x0=x(t0)
动力学约束：xk=f(xk−1,uk−1)x_k = f(x_{k-1}, u_{k-1})xk=f(xk−1,uk−1)
碰撞约束（机会约束）：Pr⁡(xk∈Ci)≥δ,∀i∈Io\Pr(x_k \in \mathcal{C}_i) \geq \delta, \forall i \in \mathcal{I}_oPr(xk∈Ci)≥δ,∀i∈Io
控制输入约束：umin⁡≤u≤umax⁡u_{\min} \leq u \leq u_{\max}umin≤u≤umax

2.3.2 机会约束处理

碰撞几何建模：

机器人碰撞几何：半径为 rrr 的球体
障碍物碰撞几何：包围盒的最小外接椭球（由半轴长度 a,b,ca, b, ca,b,c 参数化）

高斯假设下的碰撞概率推导：

对于第 iii 个障碍物在第 kkk 步的碰撞概率：

Pr⁡(pˉkr−pˉkor+ro≤1)≥δ\Pr\left(\frac{\bar{p}_k^r - \bar{p}_k^o}{r + r_o} \leq 1\right) \geq \deltaPr(r+ropˉkr−pˉko≤1)≥δ

其中 ror_oro 为障碍物椭球相关参数。

由于该方程需要对高斯分布进行椭球区域积分，无解析解，采用两步近似：

第一步：坐标变换

将椭球变换为球体，消除 QcQ_cQc：

pˉkr=Qc1/2pkr,pˉko=Qc1/2pko\bar{p}_k^r = Q_c^{1/2} p_k^r, \quad \bar{p}_k^o = Q_c^{1/2} p_k^opˉkr=Qc1/2pkr,pˉko=Qc1/2pko

第二步：线性化近似

将碰撞条件线性化，得到近似确定性形式：

C~k,i,approx={xk∣akT(pkr−pko)≤1}\tilde{C}_{k,i,\text{approx}} = \{x_k | a_k^T(p_k^r - p_k^o) \leq 1\}C~k,i,approx={xk∣akT(pkr−pko)≤1}

其中 ak=pˉkr−pˉko∥pˉkr−pˉko∥a_k = \frac{\bar{p}_k^r - \bar{p}_k^o}{\|\bar{p}_k^r - \bar{p}_k^o\|}ak=∥pˉkr−pˉko∥pˉkr−pˉko

最终确定性约束：

akT(pkr−pko)−1≥erf−1(1−2δ)2akTΣkaka_k^T(p_k^r - p_k^o) - 1 \geq \text{erf}^{-1}(1 - 2\delta) \sqrt{2a_k^T \Sigma_k a_k}akT(pkr−pko)−1≥erf−1(1−2δ)2akTΣkak

这提供了机会约束的解析表达式，支持实时最优控制问题求解。

2.3.3 动态障碍物表示

动态障碍物位置和不确定性通过线性模型传播：

pk+1o=pko+vko(tk+1−tk)p_{k+1}^o = p_k^o + v_k^o(t_{k+1} - t_k)pk+1o=pko+vko(tk+1−tk)

Σk+1o=Σko+Σv,ko(tk+1−tk)\Sigma_{k+1}^o = \Sigma_k^o + \Sigma_{v,k}^o(t_{k+1} - t_k)Σk+1o=Σko+Σv,ko(tk+1−tk)

其中 vkov_k^ovko 和 Σv,ko\Sigma_{v,k}^oΣv,ko 分别为第 kkk 步障碍物的速度及其不确定性。

2.4 时间目标跟踪方法

2.4.1 障碍物相遇条件

障碍物相遇条件基于两个要求定义：

角度条件 ：机器人移动方向与障碍物到机器人方向之间的夹角 θ<90∘\theta < 90^\circθ<90∘
距离条件：机器人到障碍物的距离小于预设阈值

2.4.2 避障目标选择算法

当机器人接近障碍物时，需要选择静态轨迹上的一个特殊位置作为绕行目标（detour goal）。

两个候选目标：

候选目标	选择起点	选择依据
第一个 pnearp_{\text{near}}pnear	轨迹上离障碍物最近的点	沿轨迹的后续点中，满足距离阈值且角度 ≥90∘\geq 90^\circ≥90∘
第二个 pavoidp_{\text{avoid}}pavoid	当前机器人位置	沿轨迹的点中，满足距离阈值且角度 ≥90∘\geq 90^\circ≥90∘

最终选择：选择距离当前机器人轨迹距离更远的候选点

避障目标必须满足 θ>90∘\theta > 90^\circθ>90∘，确保机器人远离障碍物。

2.4.3 时间目标构造

获取避障目标后，为MPC跟踪构造时间目标：

σtg=[σ1(ta),...,σN−n(ta),σN−n+1(ta),...,σN(ta+nΔT)]\sigma_{\text{tg}} = [\sigma_1(t_a), \ldots, \sigma_{N-n}(t_a), \sigma_{N-n+1}(t_a), \ldots, \sigma_N(t_a+n\Delta T)]σtg=[σ1(ta),...,σN−n(ta),σN−n+1(ta),...,σN(ta+nΔT)]

其中：

NNN 为MPC预测时域
前 N−nN-nN−n 个点重复避障目标，确保机器人能避开障碍物
后 nnn 个点提供从避障目标开始的静态轨迹，实现平滑过渡

参数选择 ：n∈[1,3]n \in [1, 3]n∈[1,3] 为合理设置

三、主要创新点与学术贡献

创新点一：双层规划架构

提出创新的双层DPMPC规划架构，充分结合：

静态地图（如Octomap）：表示复杂环境结构
几何障碍物表示：实时处理动态障碍物

架构优势：

层次	功能	输入	输出
静态层	迭代走廊收缩 + 多项式优化	航点、静态地图	无碰撞静态轨迹
动态层	机会约束MPC + 时间目标跟踪	静态轨迹、动态障碍物信息	最终反应式轨迹

创新点二：迭代走廊收缩算法

针对静态环境提出计算高效的轨迹生成方法：

通过迭代缩小走廊边界框，逐步收紧约束
基于QP formulation，计算效率高
通常只需5-6次迭代即可收敛
多项式阶数选择7或8，平衡实时性与轨迹平滑性

创新点三：机会约束MPC处理不确定性

采用机会约束scheme处理测量和障碍物感知不确定性：

将碰撞概率约束转化为确定性约束
基于高斯假设和线性化近似，推导解析表达式
与确定性约束相比，轨迹保守性更低
在不同不确定性水平下保持高成功率

创新点四：时间目标跟踪方法

实现动态障碍物的实时避障：

避障目标选择算法从两个视角评估候选点
时间目标构造：前段重复避障目标，后段接入静态轨迹
实现从避障到静态轨迹跟踪的平滑过渡
避免了返回原轨迹的困难

四、实验设计与结果分析

4.1 实验设置

软件环境：

PX4 Gazebo 仿真环境
ROS（机器人操作系统），C++实现
Intel Core i7-10750H CPU@2.6GHz

静态地图：Octomap

优化求解器：

静态层：Quadprog++（二次规划）
动态层：Acado Toolkit（MPC实现）

关键参数：

参数	值
多项式阶数	7或8
MPC预测时域	N = 20
时间步长	0.1s
收缩因子 α\alphaα	0.9
避障目标参数 nnn	1-3

4.2 仿真环境

三种不同规模的仿真环境：

环境	尺寸 (m³)	动态障碍物信息
Maze	20 × 10 × 3	8人 + 5机器人
Tunnel	15 × 15 × 6	6人 + 5机器人
Forest	40 × 40 × 4	10人 + 2机器人

动态障碍物遵循预定义的分段线性轨迹，均匀分布在环境中。

4.3 实验结果

4.3.1 性能对比

在Forest环境中不同不确定性水平下的性能比较：

方法	不确定性	最小距离 dmin⁡d_{\min}dmin	轨迹长度	时间	成功率
DPMPC (本文)	0.25Σ	1.66	74.51	21.67s	100%
	1Σ	1.63	75.48	24.07s	100%
	4Σ	1.55	78.21	24.39s	83.3%
确定性DPMPC	0.25Σ	1.42	77.51	22.05s	100%
	1Σ	1.35	76.71	24.43s	69.2%
	4Σ	0.92	80.20	28.48s	46.1%
无时间目标DPMPC	0.25Σ	1.65	84.30	28.89s	100%
	1Σ	1.62	87.71	29.45s	100%
	4Σ	1.58	88.12	32.80s	80.5%

关键发现：

提出的DPMPC在所有不确定性水平下保持最大平均最小距离、最短轨迹长度和最少时间
确定性方法成功率随不确定性增加急剧下降（4Σ时仅46.1%）
无时间目标方法执行轨迹时间长，难以平滑返回原静态轨迹

4.3.2 实时性能

各组件计算时间：

静态层：仅需少量计算时间
动态层（无障碍物）：满足实时性能
动态层（遇障碍物）：计算时间略有增加，但仍保证实时性能

QP求解效率使迭代走廊收缩算法运行快速；线性化机会约束相比文献[20]方法显著提高优化效率。

4.3.3 避障行为分析

时间障碍物距离变化对比：

蓝色虚线左侧：遇障到绕行过程
右侧：成功绕行后返回静态轨迹过程

与确定性版本对比：

提出的DPMPC平均最小障碍物距离更大
直方图显示提出方法更远离碰撞区域

保守性分析：

确定性DPMPC简单放大障碍物（如3Σ区域）可提高安全性
但轨迹更保守，因为障碍物未来状态的不确定性随预测时域线性增长
机会约束方法避免了这一问题

五、算法流程

复制代码

算法2：避障目标选择
1: σtraj → 静态轨迹
2: pr → 机器人位置, po → 障碍物位置
3: Δ → 距离阈值
4: pnear → nearestPosInTraj(po, σtraj)
5: σnear → trajStartFromPos(pnear)
6: for ptraj in σnear do:
7:   Ðnear → trajDistance(pnear, ptraj)
8:   if Ðnear ≥ Δ and θnear,p ≥ π/2 then:
9:     pnear_avoid → ptraj
10:    break
11: σr → trajStartFromPos(pr)
12: for ptraj in σr do:
13:   Ðo → distance(po, ptraj)
14:   if Ðo ≥ Δ and θo,pr ≥ π/2 then:
15:     pavoid → ptraj
16:    break
17: pavoid → argmaxTrajDist(pnear_avoid, pavoid, pr)
18: return pavoid

六、总结与展望

6.1 研究总结

本文提出DPMPC-Planner框架，实现复杂静态环境与动态障碍物共存的无人机安全导航。

核心贡献：

创新点	描述	效果
双层规划架构	结合静态地图与几何障碍物表示	同时处理静态和动态障碍物
迭代走廊收缩算法	QP formulation + 逐步收紧约束	高效生成无碰撞静态轨迹
机会约束MPC	处理测量和感知不确定性	不同不确定性水平下保持高成功率
时间目标跟踪	避障目标选择 + 轨迹平滑过渡	实时避障并平滑返回原轨迹

性能优势：

高成功率（100% @ 0.25Σ, 1Σ; 83.3% @ 4Σ）
实时性能保障
轨迹长度和时间最优
与障碍物保持安全距离

6.2 未来工作

在真实实验中实现基于机载视觉的动态障碍物检测
将整个系统部署到实际无人机平台