参考文献
[1]. Wu J , Wang H , Li N , et al. Formation Obstacle Avoidance: A Fluid-Based Solution[J]. IEEE Systems Journal, 2019, PP(99):1-12.
基础算法
移步此博客:IFDS
Improved IFDS(IIFDS)
不同之处:
- 扰动矩阵的合成变化了,从连乘变为连加:
M ‾ ( P ) = ∏ w = 1 W M ω ( P ) → M ‾ = ∑ k = 1 K ω k M k \overline{\boldsymbol{M}}(\boldsymbol{P})=\prod_{w=1}^{W} \boldsymbol{M}_{\boldsymbol{\omega}}(\boldsymbol{P}) \to \overline{\boldsymbol{M}}=\sum_{k=1}^{K} \omega_{k} \boldsymbol{M}_{k} M(P)=w=1∏WMω(P)→M=k=1∑KωkMk - 扰动矩阵增加了切向矩阵:
M k = I − n k n k T ∣ Γ k ∣ 1 / ρ k n k T n k + t k n k T ∣ Γ k ∣ 1 / σ k ∥ t k ∥ ∥ n k ∥ \boldsymbol{M}_{k}=\boldsymbol{I}-\frac{\boldsymbol{n}_{k} \boldsymbol{n}_{k}^{T}}{\left|\Gamma_{k}\right|^{1 / \rho_{k}} \boldsymbol{n}_{k}^{T} \boldsymbol{n}_{k}}+\frac{\boldsymbol{t}_{k} \boldsymbol{n}_{k}^{T}}{\left|\Gamma_{k}\right|^{1 / \sigma_{k}}\left\|\boldsymbol{t}_{k}\right\|\left\|\boldsymbol{n}_{k}\right\|} Mk=I−∣Γk∣1/ρknkTnknknkT+∣Γk∣1/σk∥tk∥∥nk∥tknkT
其中 − n k n k T ∣ Γ k ∣ 1 / ρ k n k T n k -\frac{\boldsymbol{n}_{k} \boldsymbol{n}_{k}^{T}}{\left|\Gamma_{k}\right|^{1 / \rho_{k}} \boldsymbol{n}_{k}^{T} \boldsymbol{n}_{k}} −∣Γk∣1/ρknkTnknknkT为排斥矩阵(IFDS只有它), t k n k T ∣ Γ k ∣ 1 / σ k ∥ t k ∥ ∥ n k ∥ \frac{\boldsymbol{t}_{k} \boldsymbol{n}_{k}^{T}}{\left|\Gamma_{k}\right|^{1 / \sigma_{k}}\left\|\boldsymbol{t}_{k}\right\|\left\|\boldsymbol{n}_{k}\right\|} ∣Γk∣1/σk∥tk∥∥nk∥tknkT为切向矩阵(IIFDS的关键)。
tk如何计算?
定义两互相正交且与nk正交向量tk1,tk2:
{ t k , 1 = [ ∂ Γ k ∂ y , − ∂ Γ k ∂ x , 0 ] T t k , 2 = [ ∂ Γ k ∂ x ∂ Γ k ∂ z , ∂ Γ k ∂ y ∂ Γ k ∂ z , − ( ∂ Γ k ∂ x ) 2 − ( ∂ Γ k ∂ y ) 2 ] T \left\{\begin{array}{l} t_{k, 1}=\left[\frac{\partial \Gamma_{k}}{\partial y},-\frac{\partial \Gamma_{k}}{\partial x}, 0\right]^{T} \\ \boldsymbol{t}_{k, 2}=\left[\frac{\partial \Gamma_{k}}{\partial x} \frac{\partial \Gamma_{k}}{\partial z}, \frac{\partial \Gamma_{k}}{\partial y} \frac{\partial \Gamma_{k}}{\partial z},-\left(\frac{\partial \Gamma_{k}}{\partial x}\right)^{2}-\left(\frac{\partial \Gamma_{k}}{\partial y}\right)^{2}\right]^{T} \end{array}\right. ⎩⎪⎪⎨⎪⎪⎧tk,1=[∂y∂Γk,−∂x∂Γk,0]Ttk,2=[∂x∂Γk∂z∂Γk,∂y∂Γk∂z∂Γk,−(∂x∂Γk)2−(∂y∂Γk)2]T
将n,tk1,tk2作为新坐标系,则与n垂直平面上的任意单位向量可以表为:
t k ′ = [ cos θ k , sin θ k , 0 ] T \boldsymbol{t}_{k}^{\prime}=\left[\cos \theta_{k}, \sin \theta_{k} ,0\right]^{T} tk′=[cosθk,sinθk,0]T
将tk’通过坐标转换矩阵转换到xoy标准坐标系下:
t k = Ω T I t ′ k \boldsymbol{t}_{k}=\boldsymbol{\Omega}_{T}^{I} \boldsymbol{t}^{\prime}{ }_{k} tk=ΩTIt′k
转换矩阵 Ω T I \boldsymbol{\Omega}_{T}^{I} ΩTI求解:转换矩阵
与IFDS区别
IFDS绕物体的流线往往只能在一个平面,IIFDS增加切向矩阵通过参数变动可以绕物体各个方位流动:
相较于IFDS:
对于动态障碍,IIFDS依旧具有相当亮眼的表现: