11(3)-AirSim+四旋翼仿真-AirSim中人工势场法方法改进

在11(2)中已经将人工势场法避障代码迁移至AirSim中，但是有一些待解决问题，这里一并说明后续的解决方案。

1.障碍物检验

之前使用了AirSim中提供的目标识别接口，主要问题在于

1. 受到可搜索目标种类的限制，无法识别的物体不会被视作障碍物；
2. 检测距离受限，在距离小于3米时难以识别。

在拿到了障碍物检测接口后，对相关部分进行替换，以上问题得到较好的解决。这部分内容不是本人负责，相关代码可参考：https://github.com/Kun-k/airsim_python/tree/main/ObstacleDetection。该接口可实时检测障碍物，并返回box3D类型的障碍物信息，如下所示：

'box3D': <Box3D> {   'max': <Vector3r> {   'x_val': 4.900000095367432,
'y_val': 0.7999999523162842,
'z_val': 0.5199999809265137},
'min': <Vector3r> {   'x_val': 3.8999998569488525,
'y_val': -0.19999998807907104,
'z_val': 1.5199999809265137}},

box3D类型提供了8个顶点信息，8个顶点可组成一个立方体，进而得到想要的障碍物信息。

这里取用了$x-y$平面内的4个顶点计算斥力。需注意以下两点：

1. 检测障碍物时的距离阈值，与避障时的距离阈值定义不同，故计算斥力时需再次确认障碍物是否在距离阈值内。
2. 直接获取的障碍物信息是在机体坐标系下的信息，需变换至全局坐标系再进行计算。

2.无人机速率和偏航控制

在之前使用LQR方法进行控制，能够较好地控制无人机跟踪目标状态，实现无人机机头朝向与飞行方向的一致。但为了进行速度、加速度限幅，目标状态的速度和加速度并不匹配，控制误差也较大，对无人机的速度限制效果一般。

参考三维航路点跟踪时使用的控制器限幅方法，通过当前速度对控制器输出进行限制，进行如下调整：

# 无障碍物时，三维航路点跟踪的控制器输出
U1 = np.array((K0 * Pt_matrix + K1 * A.dot(Pt_matrix)).T)[0]
if np.linalg.norm(U1, ord=np.inf) > Ul_track:
    U1 = U1 * Ul_track / np.linalg.norm(U1, ord=np.inf)
U = -(U1 + V_curr) / K2      

# 有障碍物时的控制器输出
U = Fatt + Frep
if np.linalg.norm(U, ord=np.inf) > Ul_avoid:
    U = Ul_avoid * U / np.linalg.norm(U, ord=np.inf)
U = (U - V_curr) / K2    # 改进部分

3.改进局部极小值判断方法

考虑下图所示的情况，无人机由轨迹1-2切换至轨迹2-3时，在航路点2处先减速再加速。

此时速度较小，如加速过程中检测到障碍物，会认为满足航路点检测条件，产生不必要的避障动作。

解决方法为，只在加速运动的过程中进行局部极小值检验。

4.实现效果

改进APF方法与RRT方法结合进行避障飞行

RRT+APF无人机避障飞行