(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211271688.8 (22)申请日 2022.10.18 (71)申请人 南京航空航天大 学 地址 210016 江苏省南京市秦淮区御道街 29号南京航空航天大 学 申请人 天奇自动化工程股份有限公司 (72)发明人 钱晓明　楼佩煌　郭大宏　 (74)专利代理 机构 江苏圣典律师事务所 32 237 专利代理师 于瀚文 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 一种基于快速拓展随机树改进的机械臂运 动规划方法 (57)摘要 本发明提供了一种基于快速拓展随机树改 进的机械臂运动规划方法， 包括： 确定目标和障 碍物位置； 然后根据机械臂模型和障碍物模型建 立三维障碍物碰撞检测模型； 根据机械臂模型确 定机械臂的目标位姿； 最后根据机械臂初始位 姿、 目标位置、 障碍物位置、 碰撞检测模 型和机械 臂目标位姿， 通过基于快速拓展随机树改进的机 械臂运动规划方法， 确定无碰撞运动路径， 并控 制机械臂运动至目标位姿。 本申请通过采用基于 概率的目标偏置策略、 回归过滤机制、 步长调整 策略对传统快速拓展随机树进行改进， 能够有效 提高快速 拓展随机树 算法的规划效率。 权利要求书4页 说明书11页 附图3页 CN 115533912 A 2022.12.30 CN 115533912 A 1.一种基于快速 拓展随机树改进的机 械臂运动规划方法， 其特 征在于， 包括以下步骤： 步骤1， 对目标和障碍物进行定位， 确定目标位置和障碍物位置； 步骤2， 对机 械臂和障碍物进行建模， 建立机 械臂和障碍物的模型； 步骤3， 根据机 械臂和障碍物的模型， 建立 三维碰撞检测模型； 步骤4， 根据机 械臂模型和目标位置， 确定 机械臂的目标位姿； 步骤5， 获取机械臂的初始位姿， 根据机械臂的初始位姿、 目标位置、 障碍物位置、 三维 碰撞检测模型和机械臂的末端位姿， 利用基于快速拓展随机树改进的机械臂 运动规划方法 对运动路径进行规划， 确定无碰撞路径； 步骤6， 控制机 械臂按照运动路径运动至目标位姿。 2.如权利要求1所述的方法， 其特征在于， 步骤1中， 使用机械臂双目视觉系统对目标和 障碍物进行定位， 确定目标位置和障碍物位置 。 3.如权利要求2所述的方法， 其特征在于， 步骤4中， 根据机械臂模型使用逆向运动学分 析， 得到机械臂的目标位姿， 具体是采用封闭解法对 逆向运动学进行求 解， 包括： 其中， px、 py、 pz、 qx、 qy、 qz、 rx、 ry、 rz、 sx、 sy、 sz9个变量， 对应以上六个矩阵权　利　要　求　书 1/4 页 2 CN 115533912 A 2相乘后对应的数值； 是机械臂末端坐标系到 机械臂基座 坐标系的变换关系； 为具体的变换矩阵； 首先对关节角 θ1、 关节角 θ5、 关节角 θ6进行求解， 对公式(1)进行变换 得到： 机械臂基座坐标系中的关节角为θ1， 最右端是机械臂末端坐标系(x5， y5， z5)， 其对应的 关节角为θ6； 六个连杆坐标系分别对应机械臂 的六个连杆， 其中最左端是机械臂基座坐标系， 最右 端是机械臂末端坐标系， 机械臂末端在机械臂基座坐标系下的位姿由关节变量和连杆结构 决定； 根据公式(2)左右两边矩阵的第三行第四列对应相等 求出关节角 θ1： m1、 n1为中间变量， 使用d6、 ry、 sy、 rx、 sx， 计算得出； 连杆偏距d4表示关节轴4前后的连 杆公垂线投影到关节轴4上后它 们之间的距离； 连杆长度ai‑1表示关节轴i ‑1和关节轴i 之间的公垂线的距离， 则连杆长度ai则表示关节 轴i与关节轴 i+1之间的公垂线的长度， 假设关节轴 i‑1与关节轴 i之间的公垂线在关节轴 i 上的垂足为点m， 关节轴i与关节轴 i+1之间的公垂线在关节轴i上的垂足为点n， 则点m， n之 间的距离即为连杆偏距di； 假设关节轴3与关节轴4之间的公垂线在关节轴4上的垂足为点 p， 关节轴4与关节轴5之间的公垂线在关节轴4上的垂足为点q， 则连杆偏距 d4为点p， q之间 的距离； 其中m1＝d6ry‑sy， n1＝d6rx‑sx； 连杆偏距d6表示关节轴6前后的连 杆公垂线投影到关节轴6上后它 们之间的距离； 根据公式(2)左右两边矩阵的第三行第三列对应相等 求出关节角 θ5： θ5＝±arccos(rxsinθ1‑rycosθ1)        (4) 根据公式(2)左右两边矩阵的第三行第一列对应相等 求出关节角 θ6： θ6＝atan2(m6， n6)‑atan2(si nθ5， 0)       (5) 其中m6＝pxsinθ1‑pycosθ1， n6＝qxsinθ1‑qycosθ1； m6、 n6是中间参数； 然后对关节角 θ2、 关节角 θ3、 关节角 θ4进行求解， 对公式(1)进行变换 得到： 根据公式(6)左右两边矩阵的第 一行第四列以及第 二行第四列分别对应相等求出关节 角 θ3和 θ2： 关节角 θ2表示一个连杆2的状态； 连杆i表示 关节轴i与关节轴i+1之间的机械臂； 关节角 θi表示关节轴i前后的公垂线绕其旋转的角度；权　利　要　求　书 2/4 页 3 CN 115533912 A 3