11(2)-AirSim+四旋翼仿真-AirSim中实现人工势场法避障_k

之前已经在python中实现了人工势场法避障的仿真，以及人工势场法避障和航路点跟踪的融合，但是避障部分并没有考虑到实际情况下的动力学模型、障碍物检测等，在将代码迁移至AirSim中进行仿真时也遇到了很多问题。这里对遇到的问题和解决方案进行记录，详细源码见Github：https://github.com/Kun-k/airsim_python/blob/main/code_airsim/airsim_tracking_and_avoid.py

1.障碍物检验

由于障碍物检测部分不是本人直接负责，我也一直没有拿到障碍物检测部分的接口，这里使用AirSim中提供的物体识别接口，通过物体检测代替障碍物检测，选择易被识别的物体作为障碍物。

物体识别部分参考 https://microsoft./AirSim/object_detection/#usage-example ，代码示意如下：

camera_name = "0"
image_type = airsim.ImageType.Scene
client.simSetDetectionFilterRadius(camera_name, image_type, 80 * 100) # in [cm]
client.simAddDetectionFilterMeshName(camera_name, image_type, "Cylinder_*") 
client.simGetDetections(camera_name, image_type)

以上代码中，simSetDetectionFilterRadius用于设置识别半径，单位为厘米；simAddDetectionFilterMeshName为设置希望识别的物体类型和名称，如"Cylinder_*"表示名称前缀为Cylinder_的圆柱体，在UE中设置障碍物时需特别注意物体名称；simGetDetections为获取识别到的物体信息，其返回一个储存物体信息的列表。

详细物体信息如下，这里使用Cylinder.relative_pose.position中的信息作为检测到的障碍物信息。

Cylinder: <DetectionInfo> {   'box2D': <Box2D> {   'max': <Vector2r> {   'x_val': 617.025634765625,
    'y_val': 583.5487060546875},
    'min': <Vector2r> {   'x_val': 485.74359130859375,
    'y_val': 438.33465576171875}},
    '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}},
    'geo_point': <GeoPoint> {   'altitude': 16.979999542236328,
    'latitude': 32.28772183970703,
    'longitude': 34.864785008379876},
    'name': 'Cylinder9_2',
    'relative_pose': <Pose> {   'orientation': <Quaternionr> {   'w_val': 0.9929741621017456,
    'x_val': 0.0038591264747083187,
    'y_val': -0.11333247274160385,
    'z_val': 0.03381215035915375},
    'position': <Vector3r> {   'x_val': 4.400000095367432,
    'y_val': 0.29999998211860657,
    'z_val': 1.0199999809265137}}}

需注意的是，这里检测到的位置信息是在机体坐标系下，后续计算中使用的信息为全局坐标系下的信息，需要进行转换：

state = get_state(client)
yaw = -state['orientation'][2]
Rz = np.array([[math.cos(yaw), math.sin(yaw)],
               [-math.sin(yaw), math.cos(yaw)]])
pos_obstacle = cylinder.relative_pose.position
P_search = (np.array([pos_obstacle.x_val, pos_obstacle.y_val])).dot(Rz.T)

之后已[0,0]为当前位置，P_search为障碍物位置，可进行人工势场计算。

2.设置无人机飞行朝向

这部分涉及到的姿态角解算、LQR方法原理等内容，可以查看之前的文章。

这里使用的四旋翼模型共有5个相机，包括三个前侧、一个后侧、一个下方相机，分布如下图所示：

受到相机分布的限制，无人机不能对周围任意角度的障碍物进行实时检测，为确保无人机能够时刻获取前方障碍物信息，需使其飞行朝向与机头朝向一致。

在AirSim中，提供的无人机控制接口有以下几类：

1. 位置控制，输入期望无人机前往的位置，可设置其偏航模式，使无人机保持机头朝向飞行方向。但避障算法中迭代相对较快、要求实时性好、误差小，使用位置控制的效果很差。
2. 速度控制，输入期望无人机飞行的速度和持续时间，也可以设置偏航模式，但存在和位置控制类似的问题，在避障中使用速度控制的效果很差。
3. 姿态角控制，通过改变无人机的姿态角，使其做出我们期望的动作，通过加速度进行姿态角解算。是在之前的算法中一直使用的方法，在不改变无人机偏航角时的控制效果很好，改变偏航角后效果很差。
4. 电机控制，是更底层的控制，在实际使用中没有涉及到。

这里依然使用姿态角控制，并针对改变偏航角后控制效果差这一问题，设计解决方案。在指令更新频率低、动作幅度小的情况下，使用姿态角控制，以其速度方向解算偏航角，得到的效果较好；但在加快指令频率，或增大动作幅度时，无人机趋于失控，无法得到理想的无人机状态。

分析原因，在使用加速度解算期望的姿态角时，偏航角是作为已知条件的，需根据无人机传感器获取的速度信息计算偏航角，而速度信息中存在一定的误差，也具有时效性，会导致计算得的姿态角不准确。为减小误差的影响，可以使用闭环控制。

这里使用了之前已经实现过的LQR方法，算法大致流程如下：

1. 根据实际问题选择参数矩阵 Q , R Q,R Q,R；
2. 根据方程 P = Q + A T P A − A T P B ( R + B T P B ) − 1 B T P A P=Q+A^TPA-A^TPB(R+B^TPB)^{-1}B^TPA P=Q+ATPA−ATPB(R+BTPB)−1BTPA，计算矩阵 P P P；
3. 计算反馈矩阵 K = ( R + B T P B ) − 1 B T P A K=(R+B^TPB)^{-1}B^TPA K=(R+BTPB)−1BTPA；
4. 计算控制量 u = − K X ( k ) u=-KX(k) u=−KX(k)

其中状态变量 X X X包括期望的无人机速度和位置信息，系统输入为加速度。

3.参数设置

这里的参数与之前实现的人工势场法+航路点规划中的参数基本一致，不同参数下的效果也不再赘述，只说明迭代时间dt这一参数。在之前的仿真中，没有考虑到传感器识别物体所需的时间，故dt可以设置得很小。而在AirSim的仿真中，摄像头进行物体识别需一定的时间，这一时间与计算机的算力也是有关系的，我在运行中得到的结果是，每个相机进行一次物体识别，需用时约0.11s，这一时间是不可忽略的。

这里建议在无人机执行命令的语句后，不要加.join()，即命令不阻塞，且迭代时间dt略大于识别用时。这样可以使无人机飞行和相机识别同时进行，且无人机每个时刻都能获取命令，不至于产生停顿。

4.仿真飞行效果

改进前的避障效果

改进后的避障效果