参考一些已有的工作，在 AirSim 中实现了基于人工势场法（Artificial Potential Field, APF）和 LQR 控制的无人机避障任务仿真。

势函数与势场

势函数

常见的斥力势函数为 $U_{\rm rep}(P)=\begin{cases} \dfrac 12 \eta(\dfrac{1}{d(P,P_{\rm ob})}-\dfrac 1Q)^2, & d(P,P_{\rm ob})\leq Q \\ 0,& d(P,P_{\rm ob})> Q \end{cases}$

常见的引力势函数为 $U_{\rm att}(P)=\dfrac 12\xi d^2(P,P_{\rm goal})$

$P$ 为无人机当前坐标，$P_{\rm ob}$ 为被计算势函数的障碍物的坐标，$Q$ 为障碍物的作用范围，$P_{\rm goal}$ 为目标点的坐标。

计算方法

计算的方法为对势能计算负梯度得到下降方向，以该方向作为加速度的方向，通过加速度-速度-位置的方法实现控制。具体的计算方法：

$$ F_{\rm att}=-\nabla U_{\rm att}(P)=-\xi (P,P_{\rm goal}) $$
$$ F_{\rm rep}^i=-\nabla U_{\rm rep}(P)=\begin{cases} \eta(\dfrac{1}{d(P,P_{\rm ob})} - \dfrac 1Q)\cdot\dfrac{1}{d^3(P,P_{\rm ob})} \cdot(P-P_{\rm ob}), & d(P,P_{\rm ob})\leq Q \\ 0,& d(P,P_{\rm ob})> Q \end{cases} $$
$$ F_{\rm rep}=\sum_i F_{\rm rep}^i $$

程序编写

使用 AirSim 的 Python API 编写仿真程序。程序逻辑如下：

  • 在从当前航路点 $W_a$ 前往目标航路点 $W_b$ 的过程中：
    • 计算矩阵 $A=\dfrac{1}{\Vert W_a-W_b \Vert^2}\left[I_2-(W_a-W_b)(W_a-W_b)^\top\right]$，其中 $I_2$ 是 2 阶单位矩阵。
    • 在每个控制周期 ${\rm d}t$ 内，进行航路点跟踪
      • 计算：
        • $P_t=P-W_b$，$P$ 为无人机当前位置
      • 若未检测到障碍物，则执行航路点跟踪
        • $U_1=K_0P_t+K_1AP_t$，其中 $K_0,K_1\in\mathbb{R}$ 是系数
        • 对 $U_1$ 进行限幅：若 $\vert U_1\vert > U_{tmax}$，则 $U_1\leftarrow U_{tmax}\frac{U_1}{\vert U_1\vert}$
      • 若检测到障碍物，则进行避障
        • $U_1=F_{att}+F_{rep}$
        • 对 $U_1$ 进行限幅：若 $\vert U_1\vert > U_{amax}$，则 $U_1\leftarrow U_{amax}\frac{U_1}{\vert U_1\vert}$
      • $U=\frac{1}{K_2}(U_1-V)$
    • 使用 LQR 执行计算得到的期望加速度
      • 计算期望速度 $V_{next}=V+U\cdot {\rm d}t$
      • 计算期望位置 $P_{next}=U+V_{next}\cdot {\rm d}t$
      • 调用 LQR 控制函数，设置期望的位置和速度分别为 $V_{next}, P_{next}$

避障效果测试

使用 AirSim 自带的 Blocks 环境，通过调整起始位置、设置航路点，进行一个简单的避障仿真测试。飞控模式为 simple_flight。无人机的路径如下图： APF_test

此图为俯视图，无人机从图片下方飞向上方。飞行过程中未发生碰撞（图片左上方显示 Collision Count: 0）。

高速飞行测试

结合毕设的目标（评估飞行平台的计算性能），考虑如何体现计算对该任务的影响。

计算性能会直接影响到计算处理时间。不难想到，若在附近无障碍状态下的飞行速度变快，通过摄像头检测障碍物可用的时间便会变短；若计算时间不足，则无人机可能无法及时检测出障碍物、无法及时作出避障动作。

在上一节中，我们的使用的 $U_{tmax}=3$。我们通过改变无障碍时的速度限幅参数 $U_{tmax}$ 来改变最大幅度限制。当调整 $U_{tmax}=6$ 时，无人机会在飞行过程中与障碍物发生剐蹭。（做这两个视频的时候似乎在用耳机听歌，有一些奇怪的 BGM）


而当 $U_{tmax}=7$ 时，无人机会径直撞上障碍物。


在设想中，算力的提升应该可以允许无人机以更大的速度上限进行无碰撞飞行。在以上实例中，控制周期为 ${\rm d}t=\frac 1f=0.3s$；考虑到算力提升可以支持更高的控制周期，故我尝试调整 ${\rm}t=0.1s$。调整后，无人机确实可以做到以 $U_{amax}=7$ 的条件下无碰撞飞行。然而我在打开 Xbox 录屏程序尝试记录下该过程时，无人机却在飞行过程中发生了碰撞。这说明电脑 CPU 占用导致的算力降低导致了飞行质量的降低，进一步佐证了设想的正确性。在我把 ${\rm d}t$ 调整至 $0.07s$ 后，无人机可在录屏程序开启的前提下实现无障碍飞行了：


接下来，我们尝试将计算程序迁移到其他设备（如 WSL2、树莓派等）上进行硬(软)件在环仿真。

三维 APF 避障

原有的代码基于 LQR + 姿态角控制实现了无人机在水平方向上的避障任务。我采用了几何控制的方法实现了在三维空间中的 APF 避障。

几何控制

输入为期望的无人机位置、姿态、机体角速率 $x_d,R_d,\Omega_d$，输出为无人机的力与力矩 $f,M$。可由 $f, M$ 映射得到四旋翼各个电机的输入 $\sigma_1,\sigma_2,\sigma_3,\sigma_4$，进而通过 AirSim 的电机控制接口client.moveByMotorPWMsAsync进行控制。

避障算法

将原有的 Carrot Chasing 轨迹跟踪及 APF 避障进行了到 3 维的扩展，但并没有局部极小值的处理。在 Blocks 环境中的测试效果如下：

3dAPF