EGO-Planner: An ESDF-Free Gradient-Based Local Planner for Quadrotors
EGO-Planner: An ESDF-free Gradient-based Local Planner for Quadrotors
https://github.com/ZJU-FAST-Lab/ego-planner
摘要
- 欧氏符号距离场(Euclidean Signed Distance Field,ESDF)常用于估计梯度大小和方向
- 轨迹规划只在ESDF很小的子空间进行,更新整个ESDF不必要
- 本文提出ESDF-free的基于梯度的规划框架
- 罚函数的碰撞项基于对比有碰撞的轨迹和无碰撞的引导路径,只有轨迹碰撞新的障碍物时规划器才提取必要的障碍物信息
- 若轨迹是动力学不可行的,则延长轨迹的时间
引言
构建ESDF的方式有两种:
- 增量式全局更新(incremental global updating)
- 批量式局部计算(batch local calculation)
二者都没有考虑轨迹本身,不能单独直地接服务于轨迹优化
本文提出了ESDF-free基于梯度的局部规划框架(ESDF-free Gradient-based lOcal planning framework,EGO),包含
- 基于梯度的样条优化器
- 对比轨迹和无碰撞的引导路径
- 在有碰撞的轨迹上施加力并生成估计的梯度以使轨迹远离障碍物
- 轨迹会在附近的障碍物之间反弹几次,并稳定在安全区域内
- 只计算必要的梯度,避免计算和局部轨迹无关的梯度
- 随后的改进过程
- 若轨迹不符合动力学约束,则进入改进过程
- 给轨迹分配更长的时间,产生新的B样条
- 新轨迹拟合之前的轨迹,在轴向和径向(axial and radial directions)上使用不同的惩罚,以增强鲁棒性
主要贡献:
- 提出新的基于梯度的四旋翼无人机局的部规划方法,其直接从障碍物评估和投影梯度信息
- 提出轻量级的轨迹改进方法,使用各向异性(anisotropic)误差惩罚生成更平滑的轨迹
- 把上述方法集成到四旋翼无人机系统中并开源了软件
相关工作
主要分为两部分:
- 基于梯度的运动规划
- 把局部轨迹生成建模为无约束的非线性优化问题
- 常依赖ESDF
- 欧式符号距离场ESDF
- 常用于从带噪声的传感器参数中构造物体
- 常包含冗余信息
避碰力估计
优化变量为控制点 Q \mathbf{Q} Q,每个控制点独立拥有自己的环境信息
- 不考虑碰撞,给出满足初末状态约束的B样条曲线 Φ \Phi Φ
- 一次迭代中检测到的每一个碰撞,生成一条无碰撞轨迹 Γ \Gamma Γ
- 碰撞段的每个控制点 Q i \mathbf{Q}_i Qi在障碍物表面分配一个锚点 p i j \mathbf{p}_{i j} pij和相应的排斥力方向 v i j \mathbf{v}_{i j} vij
- 省略了下表的每个 { p , v } \{\mathbf{p}, \mathbf{v}\} {p,v}对只对应一个特定的控制点,其生成过程如算法1和图3所示
- 从 Q i \mathbf{Q}_i Qi到第j个障碍物的距离定义为
d i j = ( Q i − p i j ) ⋅ v i j d_{i j}=\left(\mathbf{Q}_i-\mathbf{p}_{i j}\right) \cdot \mathbf{v}_{i j} dij=(Qi−pij)⋅vij
- 为了避免轨迹离开障碍物前重复生成 { p , v } \{\mathbf{p}, \mathbf{v}\} {p,v}对,本文认为只有对所有 j j j满足 d i j > 0 d_{i j}>0 dij>0时 Q i \mathbf{Q}_i Qi原本所在的障碍物是才是新发现的
- 基于ESDF的规划器容易陷入如下图的局部最优而无法避碰,故需要无碰撞的初始轨迹
基于梯度的轨迹优化
问题定义
轨迹建模为均匀B样条曲线 Φ \Phi Φ,其次数为 p b p_b pb, N c N_c Nc个控制点为 { Q 1 , Q 2 , … , Q N c } \left\{\mathbf{Q}_1, \mathbf{Q}_2, \ldots, \mathbf{Q}_{N_c}\right\} {Q1,Q2,…,QNc},节点向量为 { t 1 , t 2 , … , t M } \left\{t_1, t_2, \ldots, t_M\right\} {t1,t2,…,tM},且满足 M = N c + p b M=N_c+p_b M=Nc+pb(B样条固有的性质)。
由于是均匀B样条,故节点区间 Δ t = t m + 1 − t m \Delta t=t_{m+1}-t_m Δt=tm+1−tm相等,则速度、加速度和加加速度可表示为
V i = Q i + 1 − Q i Δ t , A i = V i + 1 − V i Δ t , J i = A i + 1 − A i Δ t \mathbf{V}_i=\frac{\mathbf{Q}_{i+1}-\mathbf{Q}_i}{\Delta t}, \mathbf{A}_i=\frac{\mathbf{V}_{i+1}-\mathbf{V}_i}{\Delta t}, \mathbf{J}_i=\frac{\mathbf{A}_{i+1}-\mathbf{A}_i}{\Delta t} Vi=ΔtQi+1−Qi,Ai=ΔtVi+1−Vi,Ji=ΔtAi+1−Ai
优化问题表示为
min Q J = λ s J s + λ c J c + λ d J d \min _{\mathbf{Q}} J=\lambda_s J_s+\lambda_c J_c+\lambda_d J_d QminJ=λsJs+λcJc+λdJd
其中等是右侧三项依次表示平滑性惩罚、碰撞惩罚和可行性惩罚
平滑性惩罚
只取轨迹的几何信息,在没有时间积分的情况下惩罚加速度和加加速度的平方
J s = ∑ i = 1 N c − 1 ∥ A i ∥ 2 2 + ∑ i = 1 N c − 2 ∥ J i ∥ 2 2 J_s=\sum_{i=1}^{N_c-1}\left\|\mathbf{A}_i\right\|_2^2+\sum_{i=1}^{N_c-2}\left\|\mathbf{J}_i\right\|_2^2 Js=i=1∑Nc−1∥Ai∥22+i=1∑Nc−2∥Ji∥22
碰撞惩罚
定义安全距离 s f s_f sf并惩罚 d i j < s f d_{i j}<s_f dij<sf的控制点,本文设计了二次连续可微的罚函数 j c j_c jc并在 d i j d_{ij} dij减小时减小其斜率
j c ( i , j ) = { 0 ( c i j ≤ 0 ) c i j 3 ( 0 < c i j ≤ s f ) 3 s f c i j 2 − 3 s f 2 c i j + s f 3 ( c i j > s f ) c i j = s f − d i j , \begin{aligned}j_c(i, j) & = \begin{cases}0 & \left(c_{i j} \leq 0\right) \\c_{i j}^3 & \left(0<c_{i j} \leq s_f\right) \\3 s_f c_{i j}^2-3 s_f^2 c_{i j}+s_f^3 & \left(c_{i j}>s_f\right)\end{cases} \\c_{i j} & =s_f-d_{i j},\end{aligned} jc(i,j)cij=⎩ ⎨ ⎧0cij33sfcij2−3sf2cij+sf3(cij≤0)(0<cij≤sf)(cij>sf)=sf−dij,
其中 j c ( i , j ) j_c(i, j) jc(i,j)源于 Q i \mathbf{Q}_i Qi上的 { p , v } j \{\mathbf{p}, \mathbf{v}\}_j {p,v}j对
- 每个 Q i \mathbf{Q}_i Qi的成本独立评估,故发现更多障碍物的控制点有更高的轨迹形变权重
- 第 i i i个控制点的成本增加值为 j c ( Q i ) = ∑ j = 1 N p j c ( i , j ) j_c\left(\mathbf{Q}_i\right)=\sum_{j=1}^{N_p} j_c(i, j) jc(Qi)=∑j=1Npjc(i,j),其中 N p N_p Np为 Q i \mathbf{Q}_i Qi的 { p , v } j \{\mathbf{p}, \mathbf{v}\}_j {p,v}j对数量
总成本为
J c = ∑ i = 1 N c j c ( Q i ) J_c=\sum_{i=1}^{N_c} j_c\left(\mathbf{Q}_i\right) Jc=i=1∑Ncjc(Qi)
总成本关于 Q i \mathbf{Q}_i Qi的导数为
∂ J c ∂ Q i = ∑ i = 1 N c ∑ j = 1 N p v i j { 0 ( c i j ≤ 0 ) − 3 c i j 2 ( 0 < c i j ≤ s f ) − 6 s f c i j + 3 s f 2 ( c i j > s f ) \frac{\partial J_c}{\partial \mathbf{Q}_i}=\sum_{i=1}^{N_c} \sum_{j=1}^{N_p} \mathbf{v}_{i j}\left\{\begin{array}{lr}0 & \left(c_{i j} \leq 0\right) \\-3 c_{i j}^2 & \left(0<c_{i j} \leq s_f\right) \\-6 s_f c_{i j}+3 s_f^2 & \left(c_{i j}>s_f\right)\end{array}\right. ∂Qi∂Jc=i=1∑Ncj=1∑Npvij⎩ ⎨ ⎧0−3cij2−6sfcij+3sf2(cij≤0)(0<cij≤sf)(cij>sf)
可行性惩罚
限制轨迹的高阶导数 ∣ Φ r ( k ) ( t ) ∣ < Φ r , max ( k ) \left|\Phi_r^{(k)}(t)\right|<\Phi_{r, \max }^{(k)} Φr(k)(t) <Φr,max(k),其中 r ∈ { x , y , z } r \in\{x, y, z\} r∈{x,y,z}表示每个维度
罚函数的表达式为
J d = ∑ i = 1 N c w v F ( V i ) + ∑ i = 1 N c − 1 w a F ( A i ) + ∑ i = 1 N c − 2 w j F ( J i ) J_d=\sum_{i=1}^{N_c} w_v F\left(\mathbf{V}_i\right)+\sum_{i=1}^{N_c-1} w_a F\left(\mathbf{A}_i\right)+\sum_{i=1}^{N_c-2} w_j F\left(\mathbf{J}_i\right) Jd=i=1∑NcwvF(Vi)+i=1∑Nc−1waF(Ai)+i=1∑Nc−2wjF(Ji)
其中各项函数
F ( C ) = ∑ r = x , y , z f ( c r ) F(\mathbf{C})=\sum_{r=x, y, z} f\left(c_r\right) F(C)=r=x,y,z∑f(cr)
f ( c r ) = { a 1 c r 2 + b 1 c r + c 1 ( c r ≤ − c j ) ( − λ c m − c r ) 3 ( − c j < c r < − λ c m ) 0 ( − λ c m ≤ c r ≤ λ c m ) ( c r − λ c m ) 3 ( λ c m < c r < c j ) a 2 c r 2 + b 2 c r + c 2 ( c r ≥ c j ) f\left(c_r\right)=\left\{\begin{array}{lr}a_1 c_r^2+b_1 c_r+c_1 & \left(c_r \leq-c_j\right) \\\left(-\lambda c_m-c_r\right)^3 & \left(-c_j<c_r<-\lambda c_m\right) \\0 & \left(-\lambda c_m \leq c_r \leq \lambda c_m\right) \\\left(c_r-\lambda c_m\right)^3 & \left(\lambda c_m<c_r<c_j\right) \\a_2 c_r^2+b_2 c_r+c_2 & \left(c_r \geq c_j\right)\end{array}\right. f(cr)=⎩ ⎨ ⎧a1cr2+b1cr+c1(−λcm−cr)30(cr−λcm)3a2cr2+b2cr+c2(cr≤−cj)(−cj<cr<−λcm)(−λcm≤cr≤λcm)(λcm<cr<cj)(cr≥cj)
其中 c r ∈ C ∈ { V i , A i , J i } c_r \in \mathbf{C} \in\left\{\mathbf{V}_i, \mathbf{A}_i, \mathbf{J}_i\right\} cr∈C∈{Vi,Ai,Ji}
数值求解
上述定义的问题有如下特点:
- 目标函数根据新发现的障碍物自适应变化
- 目标函数近似二次函数
采用梯度信息近似逆Hessian矩阵的拟牛顿法求解
L-BFGS算法平衡了重启损失(loss of restart)和逆Hessian矩阵的估计精度,该算法求解无约束优化问题
min x ∈ R n f ( x ) \min _{\mathbf{x} \in \mathbb{R}^n} f(\mathbf{x}) x∈Rnminf(x)
每步更新为
x k + 1 = x k − α k H k ∇ f k \mathbf{x}_{k+1}=\mathbf{x}_k-\alpha_k \mathbf{H}_k \nabla \mathbf{f}_k xk+1=xk−αkHk∇fk
H k + 1 = V k T H k V k + ρ k s k s k T \mathbf{H}_{k+1}=\mathbf{V}_k^T \mathbf{H}_k \mathbf{V}_k+\rho_k \mathbf{s}_k \mathbf{s}_k^T Hk+1=VkTHkVk+ρkskskT
其中 ρ k = ( y k T s k ) − 1 , V k = I − ρ k y k s k T , s k = x k + 1 − x k , y k = ∇ f k + 1 − ∇ f k \rho_k=\left(\mathbf{y}_k^T \mathbf{s}_k\right)^{-1}, \mathbf{V}_k=\mathbf{I}-\rho_k \mathbf{y}_k \mathbf{s}_k^T, \mathbf{s}_k=\mathbf{x}_{k+1}-\mathbf{x}_k, \mathbf{y}_k=\nabla \mathbf{f}_{k+1}-\nabla \mathbf{f}_k ρk=(ykTsk)−1,Vk=I−ρkykskT,sk=xk+1−xk,yk=∇fk+1−∇fk
此处不精确计算 H k \mathbf{H}_k Hk,更新过程满足双循环更新方法,具有线性的时间和空间复杂度。Barzilai-Borwein步的权重作为初始逆Hessian矩阵 H k 0 \mathbf{H}_k^0 Hk0
Barzilai-Borwein (BB) method也是梯度下降方法的一种,他主要是通过近似牛顿方法来实现更快的收敛速度,同时避免计算二阶导数带来的计算复杂度。
H k 0 = s k − 1 T y k − 1 y k − 1 T y k − 1 I or s k − 1 T s k − 1 s k − 1 T y k − 1 I \mathbf{H}_k^0=\frac{\mathbf{s}_{k-1}^T \mathbf{y}_{k-1}}{\mathbf{y}_{k-1}^T \mathbf{y}_{k-1}} \mathbf{I} \text { or } \frac{\mathbf{s}_{k-1}^T \mathbf{s}_{k-1}}{\mathbf{s}_{k-1}^T \mathbf{y}_{k-1}} \mathbf{I} Hk0=yk−1Tyk−1sk−1Tyk−1I or sk−1Tyk−1sk−1Tsk−1I
时间重分配和轨迹改进
本节主要解决轨迹不可行的问题
首先计算超出极限值的比例
r e = max { ∣ V i , r / v m ∣ , ∣ A j , r / a m ∣ , ∣ J k , r / j m ∣ 3 , 1 } r_e=\max \left\{\left|\mathbf{V}_{i, r} / v_m\right|, \sqrt{\left|\mathbf{A}_{j, r} / a_m\right|}, \sqrt[3]{\left|\mathbf{J}_{k, r} / j_m\right|}, 1\right\} re=max{∣Vi,r/vm∣,∣Aj,r/am∣,3∣Jk,r/jm∣,1}
之后重新计算新均匀B样条轨迹 Φ f \Phi_f Φf的节点区间
Δ t ′ = r e Δ t \Delta t^{\prime}=r_e \Delta t Δt′=reΔt
新轨迹 Φ f \Phi_f Φf要保持和原轨迹 Φ s \Phi_s Φs相同的形状和控制点数量,由光滑性、可行性和曲线拟合组成的罚函数为
min Q J ′ = λ s J s + λ d J d + λ f J f \min _{\mathbf{Q}} J^{\prime}=\lambda_s J_s+\lambda_d J_d+\lambda_f J_f QminJ′=λsJs+λdJd+λfJf
由于拟合后的曲线已经无碰撞,故设计:
- 低惩罚权重的轴向位移以放松平滑性调节限制
- 高惩罚权重的径向位移以避免碰撞
为了实现这一点,本文采用下图所示的椭球度量
则轴向位移 d a d_a da和径向位移 d r d_r dr为
d a = ( Φ f − Φ s ) ⋅ Φ ˙ s ∥ Φ ˙ s ∥ , d r = ∥ ( Φ f − Φ s ) × Φ ˙ s ∥ Φ ˙ s ∥ ∥ \begin{aligned}& d_a=\left(\boldsymbol{\Phi}_f-\boldsymbol{\Phi}_s\right) \cdot \frac{\dot{\mathbf{\Phi}}_s}{\left\|\dot{\boldsymbol{\Phi}}_s\right\|}, \\& d_r=\left\|\left(\boldsymbol{\Phi}_f-\boldsymbol{\Phi}_s\right) \times \frac{\dot{\boldsymbol{\Phi}}_s}{\left\|\dot{\boldsymbol{\Phi}}_s\right\|}\right\|\end{aligned} da=(Φf−Φs)⋅ Φ˙s Φ˙s,dr= (Φf−Φs)× Φ˙s Φ˙s
则拟合惩罚为
J f = ∫ 0 1 [ d a ( α T ′ ) 2 a 2 + d r ( α T ′ ) 2 b 2 ] d α J_f=\int_0^1\left[\frac{d_a\left(\alpha T^{\prime}\right)^2}{a^2}+\frac{d_r\left(\alpha T^{\prime}\right)^2}{b^2}\right] \mathrm{d} \alpha Jf=∫01[a2da(αT′)2+b2dr(αT′)2]dα
后面没细看了,整个框架流程如下图算法2