一种航向角速度局部最优的避障控制方法与流程

本发明涉及机器人避障路径规划，特别涉及一种航向角速度局部最优的避障控制方法。

背景技术：

1、移动机器人在抢险救灾、战争对抗、自动化农业等多个领域已有广泛应用。其中，机器人的避障路径规划为机器人控制算法中的重要部分，它需要同时考虑机器人自身的机动条件和外部环境的干扰和阻碍，最后生成一条引导机器人安全躲避障碍物到达预设目标位置的路径。目前，路径规划方法根据环境信息获取方式的不同一般分为两类，即全局规划方法和局部规划方法。

2、全局路径规划算法是指在机器人执行路径规划任务前就已经获取了任务所在地的数字地图等环境信息，并在无人系统执行任务过程中，利用这些先验信息和机载传感器的本地信息进行路径规划并优化的方法。基于全局信息的路径规划方法往往能够得到近似全局最优或者全局最优的规划路径，现有的全局路径规划方法，有改进a*算法、d*算法、蚁群算法、萤火虫算法等。其中，改进a*算法、d*算法，为基于启发式目标距离函数的网格地图方法，这类算法虽然能够实现动态规划路径以应对动态复杂环境，但往往容易陷入局部极小值而得不到最优路径，并且需要建立栅格地图作为环境感知数据才能够完成机器人避障路径规划，数据运算量较大，传感器精度要求较高；蚁群算法、萤火虫算法，为基于群体智能仿生的路径规划方法，是通过设计特定的轨迹评价函数，利用“信息素”或“亮度”等信号标记进行循环迭代得到最优规划路径，这类方法所需的收敛时间较长，往往需要较长时间进行多步骤计算才能得到路径规划结果，并且不能够保证解算结果在某一短时间范围内一定收敛，非常占用计算资源和时间。

3、局部路径规划算法是指机器人只能通过机载的有限距离感知传感器获取自身位置附近的未知地图信息，并根据环境中存在的障碍信息结合自身运动姿态生成规划路径的一种控制方法。在实际应用中，机器人的任务区域环境在短时间往往无法提前准确获知，例如废墟搜救等场景下，此时全局路径规划方法将不再适用。现有的局部路径规划方法，很难快速计算得到局部最优的机器人运动路径，容易陷入局部极小值或不动点，并且往往需要依赖多种昂贵的传感器设备获取周边环境的稠密点云信息和图像信息作为规划控制器的信息依据。现有的局部路径规划方法包括人工势场法、地图随机撒点路径生成方法、基于深度学习模型的路径规划方法等；其中，基于随机抽样原理的地图随机撒点路径生成方法主要包括 rrt、rrt*、prm 等，通过在地图中撒点并连接起来形成路径的方式，利用剪枝优化来得到无人系统的无障碍安全行进轨迹，但这种方法往往需要占用较大的存储空间且需要较长的计算时间；人工势场法（artificial potential field，apf）是通过分别对无人系统和障碍物周围施加虚拟斥力场和引力场来控制无人系统的速度或加速度，依靠较少的算力就可以实现无人系统避障路径规划，但该方法所规划的路径毛刺较多且容易陷入势场中的平衡点导致机器人运动停止；基于深度学习模型的路径规划方法是通过深度学习模型对理论上最优路径规划策略和结果进行近似的控制方法，如pbnn、binn等，该方法虽然泛化能力强，但仍无法保证其控制结果的可解释性与安全性。

技术实现思路

1、为解决上述问题，本发明提供了一种航向角速度局部最优的避障控制方法，设计了基于机器人运动状态的二次凸优化函数，通过控制lyapunov函数（control lyapunovfunction，clf）和一种控制障碍函数（control barrier function，cbf）作为约束条件进行解算，仅需要一个单线旋转雷达实时计算获取距离机器人最近的障碍点即可完成路径规划，并且在避障过程中仅需要改变极小的行驶方向即可完成避障，避免多余的能量损失。

2、本发明提供了一种航向角速度局部最优的避障控制方法，具体技术方案如下：

3、s1：基于当前机器人设备的本体形状、感知范围以及防止碰撞范围，进行数学建模；

4、s2：对目标区域内的障碍物进行数学建模，基于障碍点与该机器人设备的相对位置关系，通过最近障碍点与机器人设备坐标位置的欧式距离，获取最近障碍点；

5、s3：构建机器人设备的运动学模型，并设置初始参数，所述初始参数包括初始位置、初始方向和初始速度；

6、s4：基于当前机器人的目标搜索航向角度和搜索速度大小，构建目标导航点；

7、s5：根据所述运动学模型，基于设计的目标导航点，构建机器人设备的避障控制函数，通过所述避障控制函数驱动机器人设备运动。

8、进一步的，步骤s1中，所述数学建模具体如下：

9、以当前机器人设备的运动坐标作为原点，将初始时刻的速度方向为正轴方向构建平面直角坐标系；

10、通过机器人设备中心设置的单点旋转扫描测距雷达，建立以当前机器人设备中心为圆心，半径为的圆形感知区域，同时构建半径为的圆形区域为当前机器人设备的防撞保护区。

11、进一步的，所述障碍物为当前机器人设备对应的感知范围内的物体轮廓点的集合；

12、步骤s2中，所述最近障碍点的表示如下：

13、

14、其中，表示当前机器人设备的数学建模中t时刻下的圆心位置，表示时刻第个障碍物的点集合中第个障碍点，和分别表示时刻该障碍物点的轴坐标和轴坐标值，，是第个障碍物集合中的障碍物点的总数，为一个不为零的正整数，m表示半径的圆形感知区域内障碍物的个数。

15、进一步的，步骤s3中，所述机器人设备的运动学模型，具体如下：

16、

17、其中，是机器人设备在时刻的运动状态增量；

18、和分别表示时刻机器人设备的轴坐标和轴坐标的微分导数；

19、表示时刻机器人设备的运动速度；表示时刻机器人设备运动速度的模长；是时刻机器人设备的运动航向角的弧度值；和分别表示时刻机器人设备的航向角微分导数和航向角速率。

20、进一步的，步骤s4中，当前机器人设备在时刻的所述目标导航点，表示如下：

21、

22、其中，表示当前机器人设备的数学建模中t时刻下的圆心位置，即当前机器人设备的位置，表示时刻小机器人设备与最近障碍物点之间的距离因子，和分别是时刻机器人设备和最近障碍物点的轴坐标值，表示目标搜索航向角度，表示目标搜索速度大小，取值范围为，为当前机器人设备最大速度的模长。

23、进一步的，步骤s3和步骤s4中，还构建了巡航速度模长控制器，具体表示如下：

24、

25、其中，表示正实数速度模长大小控制因子，，是机器人设备坐标点与最近的障碍物点之间的欧式距离，是时刻机器人设备的运动航向角的弧度值，表示机器人设备坐标点与目标导航点之间的欧式距离。

26、进一步的，步骤s5之前，还包括构建二次规划控制器，获取航向角速度的局部最优解，具体表示如下：

27、

28、其中，为二次规划的到的局部最优解，表示二次规划的松弛变量，表示包含输入控制量和松弛变量的一列四行矩阵向量，表示四行四列的单位矩阵，表示惩罚因子，为控制lyapunov函数，为控制障碍函数，表示控制lyapunov函数的约束上界衰减率；表示控制障碍函数的约束下界衰减率，是机器人设备在时刻的运动状态矢量。

29、进一步的，所述控制lyapunov函数的关系表示如下：

30、

31、其中，和分别代表目标点的横坐标和纵坐标，表示时刻机器人设备的运动航向角的弧度值，表示机器人设备在时刻的所述目标导航点。

32、进一步的，所述控制障碍函数的关系表示如下：

33、

34、其中，和分别代表机器人设备质心的横坐标和纵坐标，和分别表示时刻最近障碍物点的轴坐标和轴坐标值，是时刻机器人设备坐标点与最近的障碍物点之间的欧式距离。

35、进一步的，步骤s5中，所述避障控制函数，具体表示如下：

36、

37、其中，表示时刻机器人设备的运动状态量，表示时刻机器人设备的局部最优避障航向角速度。

38、本发明的有益效果如下：

39、1、本发明以控制lyapunov函数和控制障碍函数作为约束条件，解算基于仿射机器人运动状态的二次凸优化函数，实现了在环境未知的情况下，仅需要在机器人设备上搭载一个单线旋转扫描雷达，获取最近障碍物点的相对距离和角度，即可通过一轮求解在短时间内快速计算得到所路径规划控制点路径规划，相较于现有技术无需安装高精度传感器获取有限感知范围内的所有地图信息并浪费计算资源建立栅格地图，降低了机载处理设备的算力和传感器测量精度要求。

40、2、在位置且感知距离有限的条件下，可选定任一角度作为机器人探索方向，使得探索时避障的航向角变化率是局部最优的，实现了机器人仅需要改变很小的航向角就能够安全避开障碍物，减小了能量损失。

41、3、机器人避障时的轨迹与障碍物边缘保持某一稳定数值距离，保证避障过程绝对安全，提升了机器人传感器使用效率，避免机器人过于靠近障碍物造成的信号遮挡和测量距离较小问题。