一种基于路径规划下的水下仿生机器人自主避障方法

本发明涉及水下机器人避障，尤其是涉及一种基于路径规划下的水下仿生机器人自主避障方法。

背景技术：

1、当前存在的水质监测方法主要通过采集当地水源带回进行分析，或者通过设立固定监测点对水源进行监测，这些方法往往需要耗费大量人力物力成本，且得到的结果往往滞后于当前实际水质，具有一定的局限性。

2、当前机器人技术蓬勃发展，已经在工业、医疗、服务等多个领域展现出成熟应用的效果。路径规划作为机器人技术中的重要组成部分，已经在各个领域得到广泛应用。通过智能的路径规划算法，机器人能够高效、灵活地规划自身的运动路径，避开障碍物，实现安全、快速的导航。

3、此外，随着计算机技术的发展，机器人的避障算法有较大的发展，rrt算法作为其中一种典型的算法广泛应用于机器人避障中。针对rrt算法产生路径可优化程度大，随机节点生成存在偏离目标点等缺陷，karaman等人提出了rrt*算法，通过寻找周围节点优化路径，能够减少路径代价，但优化完的路径还存在优化空间(karamans,frazzolie.sampling-based algorithms for optimal motion planning[j].international journal ofrobotics research,2011.)；kang等人提出rrt-connect算法，通过起始点与终点同时拓展随机树，能够大大减少搜寻路径时间，但搜寻完的路径仍然存在较大的优化空间(kang jg,lim d w,chol y,et llmproved rrt-connect algorithm based on triangularinequality for robot path planningljl.sensors,2021.21(2):333-343.)；杜传胜等人提出同根双向扩展的贪心rrt路径规划算法，能够在短时间内寻找到一条通过狭小空间的路径，但其生成的路径还存在优化空间(杜传胜,高焕兵,侯宇翔等.同根双向扩展的贪心rrt路径规划算法[j].计算机工程与应用,2023,59(21):312-318.)；nasir等人提出了rrt*-smart，在寻找完初始路径后在障碍旁产生信标，增加启发式思想，在后续迭代中能够更好的优化路径代价，但该算法寻找初始路径的效率并未得到提升，效率降低(nasir j,islam f,malik u,et al.rrt*-smart:a rapid convergence implementation of rrt*，[j].international journal of advanced robotic systems，2013，10(7)：299-.)；jeongi b等人提出了q-rrt*算法，根据选取父节点思想能够很好的减少路径代价以及迭代速度，但寻找初始路径的效率低(jeong i b，lee s j，kim j h.quick-rrt*：triangularinequality-based implementation of rrt*with improved initial solution andconvergence rate[j].expert systems with applications，2019，123(jun.)：82-90.)。基于以上研究，如何提出一种效率高、且优化代价低的基于路径规划下的水下仿生机器人自主避障方法是当前很重要的课题。

技术实现思路

1、本发明所要解决的技术问题是提供一种优化效率高、且优化代价低的基于路径规划下的水下仿生机器人自主避障方法。

2、本发明所采用的技术方案是，一种基于路径规划下的水下仿生机器人自主避障方法，该方法包括下列步骤：

3、s1、通过上位机选取仿生机器人进行游动的三维路径点，根据所述的三维路径点生成一条初始路径，将所述原始路径下发给仿生机器人，使其按照该初始轨迹进行游动；

4、s2、获取水下仿生机器人当前所处位置，并由水下仿生机器人建立实时地图；

5、s3、当识别到有障碍物在上位机下发的初始路径中，根据水下仿生机器人当前所处位置，确定路径起始点为xstart，路径终点为xgoal，以路径起始点xstart为中心，围绕路径起始点xstart在所述实时地图中随机生成n个节点：x1～xn；

6、s4、通过欧式距离对n个节点中的每个节点与路径终点xgoal的距离进行计算，其表达式为：其中(xi，yi，zi)表示n个节点中任意一个节点的坐标，(xgoal，ygoal，zgoal)表示路径终点xgoal的坐标；

7、s5、采用轮盘赌算法从所述的n个节点中选取一个节点作为生成的随机点xrand，具体过程为：

8、s5.1、根据步骤s4得到的每个节点对应的欧氏距离，计算出每个节点对应的启发函数，表达式为：

9、s5.2、根据步骤s5.1得到的启发函数，计算每个节点被选中的概率，其表达式为：其中，∑m∈nηim(t)表示n个节点的概率总和；所述的概率pij(t)对应着轮盘的一个区间；

10、s5.3、随机生成一个数值，判断所述数值处于哪一个轮盘区间，所述数值所处的轮盘区间对应的节点即为生成的随机节点xrand；

11、s6、选取离所述生成的随机点xrand最近的一个节点作为备选节点xnearest，沿备选节点xnearest向随机点xrand的方向上，选取距离备选节点xnearest固定长度s的位置作为新节点xnew，并选取备选节点xnearest的父节点xrand作为新节点xnew的父节点，将生成的新节点xnew与其父节点xrand进行连接，若这条连接线未与障碍物发生碰撞，则将xnearest删去，生成新路径：xrand→xnew；若这条连接线与障碍物发生碰撞，则将新节点xnew与节点xnearest进行连接，若新节点xnew与节点xnearest连接的这条连接路径未与障碍物发生碰撞，则xnearest与xnew相连得到一条新路径；若这条路径与障碍物发生碰撞，则随机再生成一个新的节点xrand重复步骤s6；其中，新节点xnew生成的表达式为：

12、

13、s7、通过人工势场法对生成新节点xnew进行修正得到的新节点为xrenew；

14、s8、以新节点xrenew作为路径起始点，返回步骤s2继续进行迭代，直到最终生成的新节点xrenew在路径终点xgoal附近时，直接与路径终点相连，完成水下避障轨迹；

15、s9、完成水下避障后继续按照上位机下发的初始路径游动，当游动过程中继续检测到障碍物时，返回s2继续开启新的避障路径规划。

16、本发明的有益效果是：采用上述一种基于路径规划下的水下仿生机器人自主避障方法，通过轮盘赌算法选取出随机节点，并根据改进的rrt算法来规划出避障路径，该方法能够以较少的时间寻找到一条代价低的避障路径，其优化效率高、且优化代价低。

17、作为优选，在步骤s1中，建立实时地图的具体过程为：通过所述水下仿生机器人的双目视觉相机来采集所述水下仿生机器人周围的水下图像，从而建立实时地图。

18、作为优选，在步骤s7中，通过人工势场法对生成新节点xnew进行修正得到的新节点为xrenew的具体过程为：通过人工势场法，在合力f(q)的影响下，新生成的节点xnew进行一定偏移得到的新节点为xrenew，所述的合力f(q)的表达式为：f(q)＝fatt(q)+frep(q)；其中，fatt(q)表示引力势场的负梯度作用力函数，uatt(q)表示引力势场的势场函数，η表示正比例增益系数；ρ(q，qg)为矢量，大小为所述水下航行器位置q和目标点qg之间的欧式距离|q-qg|，方向为机器人指向终点的方向；frep(q)表示斥力势场的负梯度作用力函数，k为正比例系数；ρ0为常数。