一种复杂环境下机器人避障的路径优化方法及系统

专利主权项内容

1.一种复杂环境下机器人避障的路径优化方法及系统，其特征在于，包括以下步骤：步骤1：当发布目标点后，全局路径规划器计算全局路径，全局路径上的坐标点集合为{(x, y), (x, y), (x, y)..., (x, y)}；112233nn步骤2：将障碍物坐标进行分类，分别存入A缓冲区和B缓冲区，分类方法如下：从n＝1开始计算出步骤1获得的全局路径上相邻两个路径点坐标(x, y)和(x, y)的直线方程为y＝kx+b，其中k为该直线斜率，b为该直线的纵截距，计算出垂直与该段路径，且经过这两个路径点的两条直线，取两条直线间的所有障碍物点坐标为{(x, y), (x, y), (x, y)..., (x, y)}；故障点到路径的距离差值参数mun值：nnn+1n+1o1o1o2o2o3o3onon将mun值小于0所对应的障碍物坐标(x, y)放入缓冲区A，mun值大于0所对应的障碍物坐标(x, y)放入缓冲区B；onononon步骤3：若A和B缓冲区都不为空，则表示路径点在两个障碍物之间，将该路径点反馈给代价地图算法，标记为不可通过，执行步骤4；若A和B缓冲区有一个不为空，则执行步骤5；步骤4：重复步骤1～步骤3，重复次数为路径的长度(单位：米)取整；在限定次数内A和B缓冲区都不为空，则目标点不可达；步骤5：若A和B缓冲区有一个不为空，则根据步骤2中所使用的全局路径上相邻两个路径点坐标(x, y)和(x, y)计算X轴移动的分量Δx和y轴移动分量Δy，计算过程如下：nnn+1n+1计算步骤1中获得的全局路径与X轴夹角的弧度rad：计算A、B缓冲区中的障碍物坐标点{(x, y), (x, y), (x, y)..., (x, y)}与坐标点(x, y)的距离：o1o1o2o2o3o3ononnn取d最小值为d＝min(d)；nminn全局路径需要移动的距离distance＝d-d，其中d为设定的障碍物与车体的安全距离；safeminsafeX轴移动分量为：Δx＝|distance×sin(rad)|；Y轴移动分量为：Δy＝|distance×cos(rad)|；步骤6：若A缓冲区不为空，B缓冲区为空，优化后的路径点坐标如下：优化后X轴坐标优化后Y轴坐标y＝y-Δy；newn步骤7：若A缓冲区为空，B缓冲区不为空，优化后的路径点坐标如下：优化后X轴坐标优化后Y轴坐标y＝y+Δy；newn步骤8：重复步骤2～步骤7，直到遍历完所全局路径上所有坐标点{(x, y), (x, y), (x, y)..., (x, y)}，从而得到最优全局路径；112233nn步骤9：若存在最优全局路径，将最优全局路径发布给局部路径规划器，得到局部路径。