一种基于超声波传感器的无人机反应式避障方法

本发明涉及无人机，具体涉及一种基于超声波传感器的无人机反应式避障方法。

背景技术：

1、随着技术的发展，无人机目前在各个领域有深入的应用。在这些应用中，无人机往往需要在复杂环境中进行作业，因此保障无人机飞行的安全性，防止无人机与障碍物发生碰撞，就显得十分重要。同时由于无人机负载、板载计算资源和电池容量均有限，因此应尽量在无人机上部署质量轻、原理简单可靠、功耗小的传感器，超声波传感器相比于激光雷达、双目相机，能够很好满足这一要求。另外无人机运动较为快速，这也要求避障算法应该具备实时快速的特点，综合前面这些要求，研究基于轻便的超声波传感器的无人机反应式避障算法具有重要意义。

2、现有的很多技术都仅仅使用了超声波传感器基础的测距功能，文献[1]使用一个超声波传感器进行测距，当探测到障碍物时就让机器人停下。这种算法过于简单，遇到障碍物就停下的策略使机器人的运动效率不高，运动轨迹不顺滑，耗能和耗时都较高。文献[2][3]使用了多个超声波传感器测量机器人到墙面的距离，并根据测距结果来调整机器人运动方向，实现机器人的沿墙行走功能。这类算法主要是使机器人与障碍物保持一个较小的固定距离，并没有实现对障碍物的躲避远离，而且这种做法减小了机器人的运动空间，不利于后续的运动规划；另外这类算法所使用的超声波传感器数量较多，往往存在传感器观测数据的冗余浪费。还有一类算法是对超声波传感器一段时间内的测距结果进行收集，对收集的距离数据进行简单处理并采取避障操作，文献[4]将超声波传感器的观测数据映射到栅格地图上，这种方法需要保存大量栅格地图数据，对内存资源占用大；文献[5]通过判断所搜集的测量数据变化趋势来得到障碍物边界，这种方法采样数据较多，所需采样时间也较长，因此无法实时运行，且后续规划轨迹是简单的折线段，没有充分利用无人机的运动学特性，运动效率低下。

技术实现思路

1、针对现有技术中存在的问题，本发明提供一种基于超声波传感器的无人机反应式避障方法，其目的在于：提供一种基于超声波传感器的无人机反应式避障算法，解决现有算法存在的超声波传感器数据利用不充分、障碍物建模不准确、后续的避障策略不顺滑、安全性不足的问题，高效安全地实现无人机对静态障碍物的躲避。

2、本发明采用的技术方案如下：

3、一种基于超声波传感器的无人机反应式避障方法，包括以下步骤：

4、s1：对无人机的超声波传感器设置报警阈值mrange和标志值avaiflag，通过超声波传感器对障碍物进行实时监测和记录，将实时监测的障碍物数据信息形成数据集存储在缓存器中；

5、s2：调用s1中缓存器内的数据集对障碍物进行建模，得到具有空间点位坐标的障碍物表面模型；

6、s3：根据s1中设置的报警阈值mrange与s2中障碍物表面模型的对应数据信息，判断是否进行障碍物平面拟合；

7、s4：将障碍物表面模型加入wallqueue队列中，进行障碍物表面模型去重和危险解除判断；

8、s5：从wallqueue队列中获取需要躲避的障碍物表面，判断无人机当前运动方向相对障碍物表面的位置关系；

9、s6：根据超声波传感器监测到的障碍物数据信息与无人机当前的飞行状况，计算出相应的避障轨迹，无人机根据单个障碍物规划、多个障碍物规划进行障碍躲避。

10、优选的，所述无人机上设置有至少三个超声波传感器，每个超声波传感器设置有对应的报警阈值mrange和标志值avaiflag，所述标志值avaiflag反映超声波传感器的使用状态，标志值avaiflag初始默认为avaiflag＝1，表示对应的超声波传感器为使用状态；当超声波传感器测量数据小于报警阈值mrange且avaiflag＝1时，触发报警并计算出避障轨迹，同时将avaiflag的值置0。

11、优选的额，所述报警阈值mrange的计算原理为：

12、假设无人机摄像头视场角为θ，当视场角边界与障碍物面垂直时，超声波传感器的测量数据小于报警阈值mrange时，表示无人机距离障碍物过近，需要进行躲避；根据几何关系可得，mrange＝h/sinθ，h表示无人机从当前时刻开始，在垂直墙面方向以最大加速度做匀减速运动所飞行的距离；当无人机在垂直于墙面方向的分速度为最大速度时，其中，vmax、amax分别表示无人机的最大速度与最大加速度；由于从触发报警开始，无人机需要采集超声波传感器在δt时间内的测量数据，因此要将δt时间里的运动距离考虑在内，得到mrange的计算公式如下：

13、

14、其中，α表示调节因子，根据具体飞行环境进行设置。

15、优选的，所述s2具体步骤为：

16、从触发报警开始，超声波传感器监测在接下来的δt时间内的障碍物数据信息，结合无人机当前状态信息解算出对应的障碍物表面点的坐标，将对应的障碍物表面点的坐标存储在缓存器中；存储完后，计算缓存器中数据的平均值，若平均值大于报警阈值mrange，则报警解除，此时标志值avaiflag不变，若平均值小于报警阈值mrange，则使用最小二乘法进行平面拟合，得到对应的具有空间点位坐标的障碍物表面模型。

17、优选的，所述s4中障碍物表面模型去重的过程为：

18、若干个超声波传感器会并行生成若干个对应的障碍物表面模型，将若干个障碍物表面模型存放至wallqueue队列中，对每个新加入wallqueue队列中的障碍物表面模型进行去重处理，去重处理具体操作为：

19、计算新加入的障碍物表面模型与队列中已有障碍物表面模型之间的夹角，若计算得到的夹角小于30°，则判断新加入的障碍物表面模型与其夹角小于30°的原有障碍物表面模型属于同一个障碍物表面模型，从而将该新障碍物表面模型从wallqueue队列中剔除；

20、在无人机飞行过程中，还会根据无人机当前位置来计算无人机与队列中障碍物表面模型之间的距离，若计算出的距离大于无人机与该障碍物表面模型的初始距离，说明无人机已经成功躲避并远离该障碍物，则将该障碍物表面模型从wallqueue队列中删除。

21、优选的，所述s4中危险解除判断的过程为：

22、判断无人机当前运动方向是否远离障碍物表面；通过采样无人机沿当前运动方向到达的下一个位置点，分别计算当前位置与下一个位置到障碍物表面的距离，若距离变小，则表示无人机正在靠近障碍物表面，需要规划避障轨迹，启动报警；否则认为无人机远离障碍物表面，障碍物对无人机运动没有威胁，危险解除。

23、优选的，所述避障轨迹的单个障碍物规划过程为：

24、在躲避单个障碍物时，将障碍物作为一个硬约束，将无人机当前运动状态作为初始条件，计算得到二阶运动轨迹；无人机在y′轴方向做匀减速运动，在x′轴方向做匀速运动，以无人机当前位置为机体坐标原点，分别以平行于障碍物表面的方向和垂直于障碍物表面的方向作为x′轴和y′轴，表示当前无人机速度方向与x′轴正方向的夹角，θ表示障碍物表面方向与地面坐标系x轴正方向的夹角，可得机体坐标系的方向向量为：

25、

26、无人机相对于地面坐标系的速度v在机体坐标系上的分量为：

27、

28、在y′轴方向所需加速度为：

29、ay′＝amax·(-1)·sign(vy′)                                         (4)

30、其中，sign()为符号函数，若自变量x为正数，则sign(x)＝1；若x为负数，则sign(x)＝-1；若x＝0，则sign(x)＝0；

31、所以，单个障碍物避障轨迹在机体坐标系下的轨迹方程为：

32、

33、机体坐标系与地面坐标系之间的转换关系为：

34、

35、其中，pos向量表示无人机当前在地面坐标系中的位置；

36、因此单个障碍物避障轨迹在地面坐标系下的方程为：

37、

38、优选的，所述避障轨迹的多个障碍物规划过程为：

39、在躲避多个障碍物轨迹中，无人机在x′轴方向做匀减速运动，等无人机在该方向的分速度等大反向之后，再做匀速运动，而在y′轴方向一直做匀减速运动，设在x′轴方向的加速度为a1，在y′轴方向的加速度为a2；根据x′轴的运动情况，避障轨迹可以分为两个阶段，设第一个阶段的飞行时间为t1第二个阶段的飞行时间为t2,我们可以得到关于a1、a2、t1、t2的关系式如下：

40、

41、同时有：

42、

43、根据式(9)、(10)得：

44、

45、令：

46、

47、根据式(7)得计算出加速度之后，多个障碍物避障轨迹在机体坐标系下的轨迹方程为：

48、

49、在地面坐标系下的轨迹方程为：

50、

51、(14)。

52、优选的，若wallqueue队列中只有当前障碍物表面模型，则无人机根据单个障碍物规划执行避障；若wallqueue队列中除了即将需要躲避的障碍物表面模型外，还有一个正在躲避的障碍物表面模型，则根据单个障碍物规划和多个障碍物规划，先计算相对即将需要躲避的障碍物表面对应的终点位置d和d′:

53、

54、然后根据d与d′相对于正在躲避的障碍物表面的远近关系，选择最优的运动轨迹引导无人机对即将需要躲避的障碍物表面进行躲避，同时保证无人机不会与正在躲避的障碍物表面发生碰撞。

55、优选的，所述s6中，在无人机执行指定避障轨迹时，设置minbuffer值，当minbuffer的值大于报警阈值mrange且avaiflag＝0时，将avaiflag的值置为1，表示对应的超声波传感器与障碍物之间已经达到安全距离，并重新启用对应的超声波传感器。

56、综上所述，由于采用了上述技术方案，本发明的有益效果是：

57、1.发明充分挖掘并利用超声波传感器的单点测距能力和无人机自身的运动实现对障碍物的扫描建模，使得障碍物感知更为简单快速，所需计算量少；

58、2.专门设计了两种二阶的顺滑避障轨迹，更贴合无人机运动学特性，实现无人机更平稳、更顺滑的运动，同时节约能量，减小因轨迹突变带来的机械损坏；

59、3.设计的避障轨迹会使无人机做出一个反弹的动作，能够实现无人机对障碍物的躲避远离，同时为无人机后续飞行提供更高的安全性和更大运动空间；

60、4.设计的多传感器同时感知避障与多障碍物避障策略，能够使无人机适应更复杂的环境，保证无人机飞行安全，不会陷入障碍物构成的死角。