[发明专利]一种基于改进式RRT算法的机械臂自主避障方法

权利要求书

1.一种基于改进式RRT算法的机械臂自主避障方法，其特征在于，包括以下步骤：

S1:利用D-H法对六轴机械臂位姿进行建模；

S2：建立基于gazebo的六轴机械臂工作环境，工作环境中随机生成形状不规则障碍物；

S3：利用改进式RRT算法完成机械臂的运动规划；

S4：最后对所得的路径利用三次Hermite曲线平滑处理；

所述S3利用改进式RRT算法完成机械臂的运动规划，具体步骤如下：

S31：确定机械臂工作起点、目标点、障碍物位置；

S32：设初始随机树G₁，G₂；

初始G₁：以起点作为G₁根节点及父节点；

初始G₂：以终点作为G₂根节点及父节点；

S33：确定搜索时长或者最大搜索次数，搜索步长；

搜索步长选取公式：r＝γ[(logn)/n]^1/d

其中：d表示状态空间的维度，n表示树中节点个数，γ是由环境确定的常数；

S34：在无障碍物状态空间中确定固定采样点；

在起点与目标点间引入采样固定点p_fix，分为两种情况：

第一种情况：当起点和目标点有障碍物时；

p_init(x_i,y_i,z_i)与p_goal(x_g,y_g,z_g)连线中点有障碍物时，以中点为圆心，为初始半径画圆，并画一条垂直于p_init与p_goal连线的直线求圆与直线的交点；将得到的两个交点进行碰撞检测，若仍碰撞则增加半径长度，直至某一交点碰撞检测通过，则将该交点作为采样固定点，其中x_i，y_i，z_i表示起点p_init的xyz轴，i是起点的索引单位，g是目标点的索引单位；

第二种情况：当起点和目标点无障碍物时；

p_init(x_i,y_i,z_i)与p_goal(x_g,y_g,z_g)连线中点无障碍物时，中点位置为采样固定点

S35：从G₁中找到距离采集点最近的节点；

遍历G1所有节点，找到与采集点的欧氏距离最短的节点q_nearest

其中Distant表示欧式距离；

S36：从q_nearest向采集点扩展一段距离；

当距离大于步长时：从最近点出发，向采集点延申一个步长的距离；

当距离小于步长时：采集点为扩展点；

S37：碰撞检测；

S371：使用长方体包络障碍物，并得到包络后的解析函数描述；

S372：使用机械臂最大直径膨胀障碍物；

S373：将机械臂骨架化；

S374：将膨胀后的障碍物表面的点和机械臂位置关系表示为函数形式进行碰撞检测；

S38：将检测通过的点，纳入G₁中，并重复S35至S38；若检测不通过，则以q_nearest(x_n，y_n，z_n)为中点，以2r两倍步长的距离为直径画圆，以45°为间隔在z＝z_n的圆域边上采集8个节点，其中节点的坐标为：

x_e＝x_n+rcosθ

y_e＝y_n+rsinθ

z_e＝z_n

其中e为圆域边上节点的索引，n为最近点q_nearest的索引；

然后分别对8个节点进行碰撞检测，选择无碰撞且距离p_fix最近的点作为下一步的扩展方向，并向前扩展一步至q_new；当算法扩展至q_new后，重复S35至S38，直到G₁子节点与p_fix距离小于某个阈值，结束G₁的扩展；

S39：进行G₂的扩展，G₂与G₁扩展步骤相同。