[发明专利]基于改进人工势场法的路径规划导航系统及方法

摘要

本发明公开了一种基于改进人工势场的煤矿救灾机器人路径规划导航系统及方法，将障碍物填充法和扰动势场法相结合，障碍物填充法通过对凹障碍物进行虚拟填充，构造填充势场函数，产生相应的排斥势场函数；扰动势场通过在引力势场中加入扰动势场，改变引力势场函数，使救灾机器人在陷入局部极小点时，自主走出局部极小点；两种方法相结合，可以很好地帮助井下救灾机器人逃离局部极小点，重新规划路径，避免再次陷入其他局部极小点，成功完成救灾救险任务，算法简洁并且有很高的可靠性。

主权项

1.一种基于改进人工势场的煤矿井下救灾机器人的路径规划方法，其特征在于，该规划方法是将扰动势场法和原有的障碍物填充法相结合，帮助机器人逃离局部极小点，重新规划避障路径，避免再次陷入局部极小点；所述局部极小点是指机器人在人工势场法的路径规划方法下，由于引力势场和斥力势场的共同作用，在未到达目标的位置，由于所受势场合力为零，导致陷入某个区域无法继续完成路径规划；所述障碍物填充法是通过救灾机器人的探测装置，对凹形障碍物进行虚拟填充，使其最终形成较为规则的障碍物，避免产生局部极小点；所述扰动势场法是当机器人陷入局部极小点时，通过在总势场函数中引入扰动势场函数，改变总势场函数，帮助井下救灾机器人走出局部极小点；所述路径规划方法当救灾机器人在煤矿井下遇到凹形障碍物时，具体工作步骤如下：步骤一：将已知的煤矿井下全局信息输入到路径规划系统中；步骤二：探测障碍物信息，融合已知的煤矿井下全局信息，构建障碍物斥力势场模型和目标点引力势场模型，对路径进行初步规划；所述障碍物斥力势场模型为：F_att(X)＝‑gradU_att(X)＝‑K_att|X‑X_g|，U_att(X)表示障碍物斥力势场函数，K_att|X‑X_g|表示障碍物斥力势场系数，X表示机器人的位置，X_g表示机器人的目标点位置；所述目标点引力势场函数模型为：其中，K_rep表示引力势场常数，X₀表示障碍物的位置，ρ₀表示障碍物产生的斥力场影响的距离范围；步骤三：救灾任务开始，救灾机器人开始向目标点运行，直到陷入凹形障碍物局部极小点；此时机器人所受合力为零，到达整个规划路径中的势场最低点；步骤四：开始逃离局部极小点，启动扰动势场函数，此时总势场函数改变，机器人出现震荡，暂时逃离局部极小点；所述扰动势场函数为：其中，Kdis是扰动势场常数；ρ(X，Xg)是机器人距离目标点的距离；ρa为机器人是否到达目标点的评判距离；步骤五：为了避免救灾机器人再次陷入凹形障碍物的其他局部极小点，开始进行凹形障碍物填充，填充范围随救灾机器人逃离局部极小点逐步扩大；填充障碍物斥力势场模型为：其中：Klocal表示填充区域斥力势场比例系数；x表示机器人退出局部极小点时的位置，xlocal表示局部极小点的位置；ρl为填充区域的影响范围；步骤六：等待一个扫描周期过后，判断机器人是否已逃离局部极小点，如果仍陷入局部极小点，回到步骤二对现阶段各类模型进行重新建立，重新进行路径规划；如果逃离局部极小点，按照现有势场模型继续进行救灾任务；步骤七：机器人到达目标位置，路径规划结束。