[发明专利]柔性机械臂在线臂形规划方法

权利要求书

1.一种柔性机械臂在线臂形规划方法，其特征在于，包括以下步骤：

S1依据柔性机械臂初始末端位姿和目标末端位姿，通过柔性机械臂逆运动学模型得到柔性机械臂初始臂形和目标臂形；

S2基于快速扩展随机树算法从包含柔性机械臂和障碍物的空间中确定连接初始臂形和目标臂形之间的若干有效臂形序列，构成柔性机械臂的无碰撞路径规划方案，同时生成在线碰撞-自由地图；以柔性机械臂臂形作为待生成随机树中的节点；该步骤包括以下分步骤：

S20对随机树进行初始化；

S21在包含柔性机械臂和障碍物的空间进行采样，得到一个随机节点q_rand；

S22寻找随机树中与随机节点q_rand最近的近节点q_near；

S23基于随机节点q_rand、近节点q_near和设定的扩展距离，得到候选节点q_pre-new；

S24判断候选节点q_pre-new与近节点q_near之间区域是否覆盖柔性机械臂和障碍物；如果候选节点q_pre-new与近节点q_near之间区域没有覆盖柔性机械臂与障碍物，则将该候选节点作为有效节点q_new，加入到随机树中，并进入步骤S25；否则返回步骤S21；

S25判断目标臂形与生成的有效节点q_new是否满足||q_goal-q_new||≥q_err，若不满足，得到连接初始臂形和目标臂形之间的若干有效臂形序列，并以得到的若干臂形序列构成柔性机械臂的无碰撞路径规划方案，同时构建包含连接初始臂形和目标臂形之间的若干有效臂形序列的在线碰撞-自由地图；若满足，进入步骤S26；

S26迭代次数增加1，并判断是否达到最大迭代次数；若是，则无碰撞路径规划失败，返回步骤S20，重新对随机树进行初始化；否则返回步骤S21，同时构建包含节点q_new及其之前生成的有效节点和初始臂形的在线碰撞-自由地图。

2.根据权利要求1所述的柔性机械臂在线臂形规划方法，其特征在于，步骤S1中，所述柔性机械臂逆运动学模型如下：

式中，(X_j,Y_j,Z_j)表示末端位姿；表示柔性机械臂的臂形变量，其中，θ_j为第j段柔性臂的弯曲角，为第j段柔性臂的旋转角；以柔性机械臂的臂形变量表征柔性机械臂臂形。

3.根据权利要求1所述的柔性机械臂在线臂形规划方法，其特征在于，步骤S21中，基于当前臂形、目标臂形、柔性机械臂与障碍物空间信息以及设定的扩展概率，通过SampleFree()函数，在柔性机械臂在线碰撞-自由地图上进行采样，得到一个随机节点q_rand。

4.根据权利要求1所述的柔性机械臂在线臂形规划方法，其特征在于，步骤S22中，基于随机树和步骤S21得到的随机节点q_rand，通过Nearest()函数得到与随机节点q_rand最近的近节点q_near。

5.根据权利要求1所述的柔性机械臂在线臂形规划方法，其特征在于，步骤S23中，基于随机节点q_rand、近节点q_near和设定的扩展距离d，通过Steer()函数，得到位于节点q_rand与节点q_near之间的候选节点q_pre-new。

6.根据权利要求1所述的柔性机械臂在线臂形规划方法，其特征在于，步骤S25中，基于柔性机械臂的无碰撞路径规划方案，通过柔性机械臂逆运动学模型得到柔性机械臂驱动信号。