改良IFDS扰动流场三维避障规划算法实现

参考文献

[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)

不同之处：

1. 扰动矩阵的合成变化了，从连乘变为连加：
   $$\overline{\mathbf{M}}(\mathbf{P})=\prod _{w=1}^W{\mathbf{M}}_{\mathbf{\omega }}(\mathbf{P})\to \overline{\mathbf{M}}=\sum _{k=1}^K{\omega }_k{\mathbf{M}}_k$$
2. 扰动矩阵增加了切向矩阵：
   $${\mathbf{M}}_k=\mathbf{I}-\frac{{\mathbf{n}}_k{\mathbf{n}}_k^T}{{\left|{\Gamma }_k\right|}^{1/{\rho }_k}{\mathbf{n}}_k^T{\mathbf{n}}_k}+\frac{{\mathbf{t}}_k{\mathbf{n}}_k^T}{{\left|{\Gamma }_k\right|}^{1/{\sigma }_k}\Vert {\mathbf{t}}_k\Vert \Vert {\mathbf{n}}_k\Vert }$$
   其中$-\frac{{\mathbf{n}}_k{\mathbf{n}}_k^T}{{|{\Gamma }_k|}^{1/{\rho }_k}{\mathbf{n}}_k^T{\mathbf{n}}_k}$为排斥矩阵（IFDS只有它），$\frac{{\mathbf{t}}_k{\mathbf{n}}_k^T}{{|{\Gamma }_k|}^{1/{\sigma }_k}\Vert {\mathbf{t}}_k\Vert \Vert {\mathbf{n}}_k\Vert }$为切向矩阵（IIFDS的关键）。

tk如何计算？
定义两互相正交且与nk正交向量tk1,tk2:
$$\{\begin{array}{l}{t}_{k,1}={\left[\frac{\partial {\Gamma }_k}{\partial y},-\frac{\partial {\Gamma }_k}{\partial x},0\right]}^T\\ {\mathbf{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}$$
将n,tk1,tk2作为新坐标系，则与n垂直平面上的任意单位向量可以表为：
$${\mathbf{t}}_k^{\prime }={\left[\cos {\theta }_k,\sin {\theta }_k,0\right]}^T$$
将tk’通过坐标转换矩阵转换到xoy标准坐标系下：
$${\mathbf{t}}_k={\mathbf{\Omega }}_T^I{\mathbf{t}}^{\prime }{}_k$$
转换矩阵${\mathbf{\Omega }}_T^I$求解：转换矩阵

与IFDS区别

IFDS绕物体的流线往往只能在一个平面，IIFDS增加切向矩阵通过参数变动可以绕物体各个方位流动：

相较于IFDS:

对于动态障碍，IIFDS依旧具有相当亮眼的表现：