申请/专利权人：安徽理工大学

申请日：2022-05-05

公开（公告）日：2024-04-16

公开（公告）号：CN115016458B

主分类号：G05D1/43

分类号：G05D1/43;G05D1/242;G05D1/246;G05D1/65;G05D1/633;G05D1/644;G05D109/10

优先权：

专利状态码：有效-授权

法律状态：2024.04.16#授权;2022.09.23#实质审查的生效;2022.09.06#公开

摘要：本发明公开一种基于RRT算法的地洞勘探机器人路径规划方法，首先使用激光传感器深度相机采集地洞环境的障碍物信息，然后对控制器中的信息在MATLAB平台下用激光同步定位与建图框架来搭建地洞勘探下的环境地图；然后用基于蒙特卡洛的改进粒子滤波算法经过贝叶斯重要性采样完成目标跟踪与定位工作；接着利用PTP‑RRT算法产生一条快速搜索随机树节点更少的初步路径，然后使用A*算法在构建好的环境地图上搜索一条确定路径的起点B1与路径终点Bs的初始路径route1，然后提取初始路径的起点、终点与转折点并使用转折点优化方法提取关键路标；然后将坐标离散化，将这些离散点利用高斯函数对数据点集进行函数逼近的拟合来得到曲线Fx，然后与地图中的障碍物进行碰撞检测；删除冗余节点，若一次平滑无碰撞就是最优路径，如果有碰撞，就选择碰撞处的最短转折点增补为关键路标转折点，生成最优平滑路径route2。最终实现了一种基于RRT算法的地洞勘探机器人路径规划方法。

主权项：1.一种基于RRT算法的地洞勘探机器人路径规划方法，其特征在于，包括以下步骤：1采用型号为RPLIDARA3的激光传感器深度相机采集地洞环境的障碍物信息，然后将收集到的障碍物信息数据存储在激光传感器的控制器当中，然后对控制器中存储的信息在MATLAB平台下用激光同步定位与建图Simultaneouslocalizationandmapping，SLAM框架来搭建地洞勘探下的环境地图；鉴于地洞勘探下的环境复杂情况，然后用基于蒙特卡洛的改进粒子滤波算法经过贝叶斯重要性采样完成目标跟踪与定位工作；其具体步骤如下：1.1在整个地洞场地内，将障碍物模拟成粒子，总数为N，然后进行滤波得到系统初始状态的粒子均匀分布，机器人坐标系XOY取原点在两后轮的中轴的中点处，X轴沿机器人车体从后轮指向前轮，Y轴与其平面垂直，坐标系采用右手笛卡尔坐标系；用传感器获得移动机器人的初始位置坐标；1.2按照给定运动方程进行运动，传感器会测量到当前位置，接着把粒子群里的全部粒子带到机器人的运动方程里，得到粒子群的位置，同时，计算此时每个粒子的位置和测量得到的小车位置之间的距离；机器人车体运动时所携带的传感器通常都带有噪声且运动的不确定信息，为此引入噪声wt,可得到以下运动方程：t代表时刻，L是车体长度，x,y表示横纵坐标，表示前轮转角，指令的时间间隔为ΔT，机器人获得相对于坐标系原点的速度V和前轮的转向角α，指令输入为ut＝Vt,αt；1.3采用拓展卡尔曼滤波进行粒子的状态预测，状态方程为k表示采样时刻点，表示k-1时刻的状态矩阵，f表示移动机器人的运动状态转移函数，然后更新粒子的状态，状态方程Kk为卡尔曼增益；为第k时刻采集的粒子索引序列号m,h是移动机器人观测函数，zk是第k时刻移动机器人系统观测方程；1.4将粒子和机器人之间的几何距离按照距离大小给每个粒子添加一个权重ω，权重和距离关系的函数，这里采用高斯分布钟形曲线的右侧，即随距离增大，权重单调递减,得到全部粒子s的权重后进行归一化粒子的加权值；然后将上述更新后的粒子数s重采样得到新的粒子的个数；重要性采样公式为其中采样次数为n,qz表示随机变量z的概率分布，加权的分母是所有重要性权重ωn的和，fzn为采样的每个数据对应的值，E[f]代表整个概率分布的期望；1.5最后输出移动机器人在当前时刻k的位姿信息，用1.6接下来重复上述1.2到1.5步骤，把机器人周围的点综合起来，完成地图的构建；2利用PTP-RRT算法产生一条快速搜索随机树节点更少的初步路径，PTP-RRT路径转折点PathTurningPoint,PTP算法是PTP算法和RRT算法的融合，具体步骤为：利用快速搜索随机树theRapidlyexploringRandomTree，RRT的路径规划算法，在步骤1中搭建的环境地图中产生一条单向快速搜索随机树，接下来通过快速搜索随机树节点的扩展进行路径搜索，使用A*算法在构建好的环境地图上搜索一条确定路径的起点B1与路径终点Bs的初始路径route1，然后提取初始路径的起点、终点与转折点B1，B2，B3……Bi……，Bi+k……Bs，其中i、k均为正整数，其中i+ks；3利用步骤2中产生的路径节点，然后使用路径转折点优化方法提取关键路标方法细化以下步骤：step1：从B1开始判断路径线段是不是与障碍物相碰，若与障碍物相交，则进行step2,若不与障碍物相交，跳转step3；step2：进行用Bi+1代替原来的转折起点Bi成为新的转折起点然后跳转step1；step3：判断Bi+2等不等于Bs，如果等于，略去转折点Bi-1，转折点优化结束，如果不等于，则继续判断路径线段是不是与环境地图中的障碍物相交，若相交，则略去转折点Bi+1,用Bi+2代替原来的转折点成为新的转折点起点跳转到step1，若不相交，则继续下一步step4；step4：Bi+3等不等于Bs，若等于，转折点优化结束，若不等于，则继续寻优，判断是否与地图中的障碍物相交，如果相交，则略去转折点Bi+1、Bi+2,用Bi+3代替原来的转折点成为新的转折起点并跳转step1，若不相交，则跳转step5；……Stepk+1：直到Bi+k＝Bs时，关键路径节点提取结束；k表示步数；4将步骤3中坐标离散化，通过激光雷达对地洞的边缘进行扫描，以机器人朝向的方向为正向基准，以竖直方向为Y轴，以水平方向为X轴建立平面坐标系，节点坐标为xi，yi，i＝1，2，……，c，c为节点坐标总数，然后将这些离散点利用高斯函数对数据点集进行函数逼近的拟合来得到曲线Fx,然后与地图中的障碍物进行碰撞检测；删除冗余节点，最终一次平滑无碰撞就是最优路径route2。

全文数据：

权利要求：

百度查询： 安徽理工大学 基于RRT算法的地洞勘探机器人路径规划方法

免责声明
1、本报告根据公开、合法渠道获得相关数据和信息，力求客观、公正，但并不保证数据的最终完整性和准确性。
2、报告中的分析和结论仅反映本公司于发布本报告当日的职业理解，仅供参考使用，不能作为本公司承担任何法律责任的依据或者凭证。

阅读全文 双屏查看 官方信息 专利公告 收藏专利 下载PDF 下载WORD