ardupilot开发 --- 避障篇

1. 飞机避障的类型

• 空中防碰撞ADSB，主要是防止与其他飞行器的碰撞；
  https://ardupilot.org/copter/docs/common-ads-b-receiver.html#common-ads-b-receiver
• 避障，防止与一般障碍物的碰撞，如天花板、地板、树木、墙壁，电线杆等低空障碍物；

2. 实现避障必要的传感器

• ADSB 接收器（ADSB receivers）
• 距离传感器、接近传感器。如雷达、深度相机、声呐、测距仪等可测距传感器（Rangefinders or Proximity Sensors or Realsense Depth Camera）

3. 路径规划和避障

避开接近传感器检测到的障碍物以及地面站设置的围栏；

3.1 传感器选择

• Proximity Sensors 接近传感器
• Rangefinders 测距仪
• Realsense Depth Camera 深度相机（如Realsense 深度相机）

用户经常将测距仪与接近传感器混淆。测距仪通常是一维传感器，可以提供距离非常窄的光线的距离。接近传感器通常是360激光雷达和其他FOV更宽的传感器的名称。两类传感器使用的设置和参数不同。

3.1.1 Proximity Sensors 接近传感器

ArduPilot最多支持4个接近传感器。
360度激光雷达通常作为接近传感器用于物体规避，但多个测距仪传感器或立体深度相机也可用于接近检测。
选定一个接近传感器然后配置飞控并测试：
https://ardupilot.org/copter/docs/common-proximity-landingpage.html#common-proximity-landingpage

3.2 避障类型

3.2.1 简单避障模式

Copter支持水平和向上的简单物体回避；
简单目标规避可以使用地理围栏和围栏信标作为接近传感器，可在AVOID_ENABLE参数设置。

算法介绍：

proximity sensor source 只能有一个：

• 要么是一个360deg的雷达；
• 要么是9个测距仪（rangefinders）；
• 简单避障：简单避障的原理
• 源码位置：The AP_Proximity 和 AC_Avoidance.

简单避障用于哪些飞行模式？

• Horizontal object avoidance works in AltHold and Loiter modes.
• Upward object avoidance works in LOITER, ALTHOLD modes only.
• 可不可以用在auto模式？？待验证！！
• 哪个参数控制是水平躲避还是垂直躲避？？待验证！！

简单避障模式支持的传感器类型（避障信息来源）：

• 360雷达 360 degree lidar （2D雷达）；
• 测距仪 Rangefinders（单线测距，本质是2D信息）；
• 还可以将围栏作为障碍物如Geo-Fences and Fence Beacons ，在AVOID_ENABLE 中配置；
• 对于新的（其他）2D距离传感器，只要能够将障碍物信息通过Mavlink 消息Distance Sensor 传递给飞控即可；例如传感器与机载计算机连接，机载计算机通过MavRos发送Distance Sensor消息给飞控使用；原理可参考：https://ardupilot.org/dev/docs/code-overview-object-avoidance.html#providing-distance-sensor-messages-to-ardupilot
• 对于3D 避障，3D障碍物检测传感器将障碍物信息通过Mavlink 消息 OBSTACLE_DISTANCE_3D 传递给飞控即可让飞机实现简单避障功能！

3.2.2 基于BendyRuler路径规划的避障

• 适用于哪些飞行模式？
  For Object Avoidance in AUTO and GUIDED modes in Copter and Rover, see Object Avoidance with Bendy Ruler or Object Avoidance with Dijkstra’s
• 原理：BendyRuler Path Planning Around Obstacles and Fences
• 简述
  适用于auto、guided、RTL模式；
  BendyRuler算法在车辆的多个方向上进行探测，寻找空地，然后尝试选择足够开阔的方向，同时将车辆驶向最终目的地。
  

3.2.3 基于Dijkstra’s路径规划的避障

• 适用于哪些飞行模式？
  For Object Avoidance in AUTO and GUIDED modes in Copter and Rover, see Object Avoidance with Bendy Ruler or Object Avoidance with Dijkstra’s
• 原理：Dijkstra’s Path Planning Around Fences
• 简述
  Dijkstra在围栏和隔离区周围进行路径规划。这种众所周知的算法在内部建立了一个根据围栏和隔离区计算的“安全区域”列表，然后找到到达目的地的最短路径。
  

3.2.4 基于Dijkstra’s with BendyRule的避障

融合Dijkstras和BendyRuler，支持路径规划。BendyRuler不能保证路径最短，可以称为本地计划者。尽管Dijkstra让我们能够以最短的路径在复杂的围栏周围导航，但由于计算复杂性，它无法用于避免基于邻近度的障碍物。因此，这两种算法的优点被结合在一起使用。该方法从传统的Dijkstra规划飞行路径之间所有围栏周围的最短路径开始。在此路径之间，如果检测到任何基于接近度的障碍物，导航将切换到BendyRuler。如果传感器附近没有障碍物，车辆将恢复正常的Dijkstra导航。

3.3 飞控参数配置

不同的传感器类型配置方式不相同，总共有3种类型：

• Proximity Sensors
• Rangefinders
• Realsense Depth Camera

3.3.1 以Proximity Sensors 为例

以RPLidar A2 雷达为例；
最多可接4个接近传感器（雷达）；

如何接线：

串口连接，建议接串口1，因为串口1能够提供最高1.5A的电流；
注意：飞控的串口TX连接到雷达的RX，飞控串口的RX连接到雷达的TX ！！

驱动参数：

• SERIAL1_PROTOCOL = 11 Lidar360
• SERIAL1_BAUD = 115 for A2 ，256 for S1 or A3
• PRX1_TYPE = 5 RPLidarA2
• PRX1_ORIENT = 0：装在飞机顶部，1：装在飞机底部
• 如果使用的是串口1,2，还应该关闭“flow control”
  BRD_SER1_RTSCTS = 0

忽略区域配置：

• 最多可配置6个区域（sectors ）
• PRX1_IGN_ANG1~PRX1_IGN_ANG6
  Proximity sensor ignore angle
  
• PRX1_IGN_WID1~PRX1_IGN_WID6
  Proximity sensor ignore width
  

感知范围设置：

• 传感器的探测距离
  PRX1_MAX 和 PRX1_MIN
  默认为0，设为0即使用传感器厂商提供的数据（驱动中自动检测获取）
• 开启避障的最小高度
  PRX_ALT_MIN
  默认值为1m

避障控制：

• 开启避障
  AVOID_ENABLE = 7 (“All”)
  
• 设置某个RC通道作为避障开关
  RC10_OPTION = 40 Proximity Avoidance Enable
• 开启避障的最低高度（需要安装向下的距离传感器rangefinder）
  为了避免一些具有垂直视野（2.5D或3D传感器）把地面当做障碍物。
  AVOID_ALT_MIN = 0~6 m
  This feature requires a valid Downward Facing Rangefinder reading to works???

飞行模式参数：

• 定点模式
• • 避障动作
    AVOID_BEHAVE = 0 Slide，1 Stop
• • 开始避障动作的距离
    AVOID_MARGIN = 1 ~ 10 m
• • 以一定的速度远离障碍物
    AVOID_BACKUP_SPD = 0 ~2 m/s 0时关闭此功能
    如果避障动作设为slide则此参数要设置为0！
• • 避障动作的最大加速度
    AVOID_ACCEL_MAX = 0~9 m/s^2
• 定高模式
• • AVOID_DIST_MAX
• • AVOID_ANGLE_MAX
    Max lean angle used to avoid obstacles while in non-GPS modes
    
• auto、guided 模式
  路径规划算法选择：
• • 禁用路径规划，即不进行避障？
    OA_TYPE = 0
• • BendyRuler 路径规划
    OA_TYPE = 1
    BendyRuler Path Planning Around Obstacles and Fences，适用在auto，guided模式
• • Dijkstra’s 路径规划
    OA_TYPE = 2
    Dijkstra’s Path Planning Around Fences，适用在auto，guided模式
• • Dijkstra’s with BendyRuler
    OA_TYPE = 3
    Dijkstra’s with BendyRuler Path Planning Around Obstacles and Fences，适用在auto，guided模式

路径规划算法参数：

• BendyRuler
  1）算法探测距离
  OA_BR_LOOKAHEAD = 1~100m
  默认值为15，典型值为5，太大会导致飞机会过于谨慎，不会进入有很多障碍物的区域；
  2）飞机应远离障碍物的距离
  OA_MARGIN_MAX = 0.1~100m
  默认值为5，典型值为2；
  3）路径规划类型
  OA_BR_TYPE = 1 水平搜索路径，2 在四个方向上搜索路径：直向目的地、垂直向上、垂直向下和向后；
• Dijkstra’s
  1）车辆应远离围栏和隔离区的距离
  OA_MARGIN_MAX = 0.1~100m
• Dijkstra’s with BendyRule
  参数与BendyRuler 、Dijkstra’s共用；

高级配置：

1）对障碍物数据库obstacle database的配置；
可以不做改动，默认值即可！

• OA_DB_SIZE: the maximum number of obstacles that can be tracked
• OA_DB_EXPIRE: the number of seconds after an obstacle disappears from view that it is removed from the database
• OA_DB_QUEUE_SIZE: the buffer size between the lidar and obstacle database. Normally this can be left at the default value
• OA_DB_OUTPUT: controls whether tracked objects are visible on the GCS as small airplanes
• OA_DB_ALT_MIN: OADatabase will reject obstacle’s if vehicle’s altitude above home is below this parameter, in a 3 meter radius around home. This can be useful if your sensor is picking up the ground as obstacles while taking off

2）数据过滤
避免将数据噪声当成障碍物
PRX_FILT = 0~20 Hz 越小噪声越小
3）忽略地面
对于低空避障，以及在起飞和降落时，有时传感器会将下面的地面作为障碍物。这可能很危险，因为飞机会试图“避开”这些障碍物（如果启用了回避功能）。通过设置 PRX_IGN_GND=1，我们尝试检测并忽略地面附近的任何障碍物。

4. 避障测试

简单避障模式的特点：
In ALTHOLD mode, the aggregate threat is translated into an attempt to add lean input into the pilot’s commands to move the Copter away from the aggregate obstructions. The pilot can still overcome these additions to his command inputs and fly into a object, if determined to do so.

In LOITER, either stopping in front of the object or a “sliding” algorithm is used to avoid it. “Sliding” involves slowing and slightly yawing as the vehicle approaches the object. For example, if Copter approaches a fence line at an angle, it will “slide along” the fence as the operator pushes the Copter toward the fence. Head-on approaches would stop, even if the pilot continues to “pushes” it forward.

5. 开发

• 任何能够向Ardupilot提供DISTANCE_SENSOR消息的距离传感器都可以让飞机实现避障功能！！
• 或提供OBSTACLE_DISTANCE_3D 消息实现3D避障！！
• Ardupilot支持DISTANCE_SENSOR和OBSTACLE_DISTANCE MAVLink消息，前者携带单个距离，后者携带一组距离；
• This page describes how the object avoidance feature works and how “proximity sensors” should provide data into ArduPilot.

5.1 Loiter模式避障原理

• AP_Proximity library 收集了原始的障碍物距离信息；
• 这些原始的距离信息被整合，最后只需要保存8个扇区内的最近距离和角度（默认情况下，将车辆周围的区域划分为8个扇区，但可以根据不同的传感器使用不同数量的扇区）；
• 每个扇区宽45度，扇区0指向车辆前方，扇区1向右前方，以此类推。根据这些扇区内障碍物的最近距离和角度，可以在飞机周围建立一个虚拟围栏（使用2D矢量保存）；
  
• AC_Avoidance library 根据上述虚拟围栏，来调整Loiter controller生成的desired velocity vectors（搜索“avoid->adjust_velocity”进行查找）。Loiter’s desired velocity 则来自 RC input 或地面站的控制。

5.2 AltHold模式避障原理

参考：https://ardupilot.org/dev/docs/code-overview-object-avoidance.html#avoidance-in-althold

5.3 Providing Distance Sensor messages to ArduPilot

参考：https://ardupilot.org/dev/docs/code-overview-object-avoidance.html#providing-distance-sensor-messages-to-ardupilot

5.3.1 举个栗子

5.3.2 基于ROS的避障？

参考：Future Steps

6. 疑问与解答

• 问题1：当OA_TYPE = 1时，定高定点的避障是怎么样的行为？？失效还是只是做简单的避障？？
• 问题2：当OA_TYPE = 0时，auto模式下的避障是怎么样的行为？？失效还是只是做简单的避障？？
• 问题3：定点下的简单避障分水平躲避和垂直躲避，如何设置？？哪个参数控制的？？
  没找到这个参数。
  先水平躲避不行再向上躲避？

OA_TYPE 参数的含义实际上是路径规划的类型。
为了回答以上几个问题，去实现简单避障模式的.h和.cpp文件中寻找OA_TYPE 的用法：
AC_Avoid.h中没找到这个参数，只找到下面参数：

说明简单避障模式根本就没使用OA_TYPE 这个参数，问题1得到答案！

在AP_OAPathPlanner.h中，包含了：

#include "AP_OABendyRuler.h"
#include "AP_OADijkstra.h"

可知AP_OAPathPlanner.h是实现路径规划避障的实现类。
在AP_OAPathPlanner.h中找到了参数OA_TYPE ：

    // enumerations for _TYPE parameter
    enum OAPathPlanTypes {
        OA_PATHPLAN_DISABLED = 0,
        OA_PATHPLAN_BENDYRULER = 1,
        OA_PATHPLAN_DIJKSTRA = 2,
        OA_PATHPLAN_DJIKSTRA_BENDYRULER = 3,
    };

和另一个相关的枚举：

    // path planner responsible for a particular result
    enum OAPathPlannerUsed : uint8_t {
        None = 0,
        BendyRulerHorizontal,
        BendyRulerVertical,
        Dijkstras
    };

在.cpp中查找OA_PATHPLAN_DISABLED并联系上下文就可以回答问题2：

再看看是谁调用了这个mission_avoidance（）？

进去看看：

原来如此，如果OA_TYPE = 0，则不会进行路径规划，也不会进行避障！！问题2得到回答。

不过以上都只是根据代码看出来的结果，实际还是得测试才知道真正的结果！

至于问题3…
从类AC_Avoid.h中的类AC_Avoid可知，避障动作必定与这几个关键词有关：“BehaviourType”、“BEHAVIOR_SLIDE”、“_behavior”，在AC_Avoid.cpp中搜索这几个关键词即可找到答案：
可知总共在两种情况下使用：

• fence
• proximity sensor
  我们关注的是后者，即adjust_velocity_proximity()函数中引用的_behavior部分代码：
  
  都是调用了limit_velocity_3D()函数，真实头大！！只能看看传递进去的参数有什么不同了…

7. 定高定点的避障实现代码

7.1 定点

已知AC_Avoid.h中的类AC_Avoid是实现简单避障模式的类。
先搜索哪个文件包含了AC_Avoid.h：

那必然在AC_Loiter.cpp中引用了避障类，在AC_Loiter.cpp中搜索类关键字“AC_Avoid”：

找到结果！

7.2 定高

先找到定高模式坐在的cpp，并在该cpp中搜索类关键字“AC_Avoid”：

也找到结果！

点赞、收藏、关注哟！