(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211347880.0 (22)申请日 2022.10.31 (71)申请人 武汉恒新动力科技有限公司 地址 430074 湖北省武汉市东湖新 技术开 发区高新大道999号未来科技城B1栋 401室 (72)发明人 黄麟　余翔　王隐　 (74)专利代理 机构 湖北武汉 永嘉专利代理有限 公司 42102 专利代理师 黄帅 (51)Int.Cl. B25J 9/04(2006.01) B25J 9/16(2006.01) (54)发明名称 欠自由度机械臂奇异性预估与运动控制方 法及机器人 (57)摘要 本发明公开了一种欠自由度机械臂奇异性 预估与运动控制方法及机器人， 该方法包括： 对 欠自由度机械臂建立虚拟轴， 使其转化为拥有虚 拟关节的6轴机械臂； 欠自由度机械臂关节控制 角度与虚拟关节控制角度组成6轴机械臂在关节 空间的6个控制量； 对6轴机械臂进行初始值正解 预解算， 预储存部分正解角度和位姿对应关系； 求逆解时， 利用上述初始值进行筛选并遍历迭代 法求收敛解； 求解6轴机械臂雅可比矩阵计算机 械臂奇异性， 以获取奇异点解集； 运动过程中， 根 据目标位姿， 结合奇异点解集， 规划出机械臂最 优路径点， 再根据路径点求逆解角度， 最后依次 下发各轴角度以完成欠自由度机械臂的运动控 制。 本发明能够完成欠自由度机械臂的奇异性预 估与运动控制。 权利要求书3页 说明书8页 附图4页 CN 115533883 A 2022.12.30 CN 115533883 A 1.一种欠自由度机 械臂奇异性预估与运动控制方法， 其特 征在于， 包括以下步骤： S1、 对欠自由度机械臂连杆模型建立虚拟轴， 使其转化为拥有虚拟关节的6轴机械臂； 原有的欠自由度机械臂关节控制角度与增加的虚拟关节控制角度一起组成6轴机械臂在关 节空间的6个控制量θ1、 θ2、 θ3、 θ4、 θ5、 θ6； 当关节类型为平动关节时， θi取值为平动移动距离， 当关节类型为 转动关节时， θi取值为转动角度， 其中i ＝1,2,3. .,6； S2、 6轴机械臂关节空间控制量θ1、 θ2、 θ3、 θ4、 θ5、 θ6与6轴机械臂位置X、 Y、 Z及姿态 有以下对应关系： 根据上式对6轴机械臂进行初始值正解预解算， 预储存部分正解角度和位姿对应关系， 作为后续求逆解时的初始值输入； 求逆解时， 利用上述初始值进行筛选并遍历迭代法求收 敛解， 收敛解均带入正 解验算， 正 解验算通过即求逆解成功； S3、 求解6轴机械臂的雅可比矩阵J( θ )， 由雅可比矩阵J( θ )及其转置矩阵JT( θ )计算机械 臂奇异性 由该式对欠自由度机械臂可达区间的点位进行奇异性预 估， 以获取奇异点 解集； S4、 运动过程中， 根据欠自由度机械臂目标位姿， 结合奇异点解集， 规划出欠自由度机 械臂最优路径点， 再根据路径点求逆解角度， 最后依 次下发各轴角度以完成欠自由度机械 臂的运动控制。 2.根据权利要求1所述的欠自由度机械臂奇异性预估与运动控制方法， 其特征在于， 所 述S1中的虚拟关节， 加入在欠自由度的N轴机械臂现有关节的起始关节处、 关节连接处或者 机械臂末端； 其中N< 6。 3.根据权利要求2所述的欠自由度机械臂奇异性预估与运动控制方法， 其特征在于， 所 述的虚拟关节是平动关节或者 转动关节。 4.根据权利要求2所述的欠自由度机械臂奇异性预估与运动控制方法， 其特征在于， 虚 拟关节位置处于机械臂末端时， 不影响机械臂末端位置X、 Y、 Z， 仅影响姿态角度 5.根据权利要求1所述的欠自由度机械臂奇异性预估与运动控制方法， 其特征在于， 所 述S2中， 正 解预解算及求逆解包括以下步骤： S21、 根据欠自由度机械臂 实际N轴和实际安全工作区间求得对应各单轴角度极限θmin、 θmax， 对各单轴在 安全工作区间内以步进角度θstep， 进行N层嵌套遍历运动学正解计算， 得到 N轴角度fAngle( θ1， θ2... θN)和机械臂位姿 的映射关系： 构建映射 集： 其中， θi表示第i轴角度， 取θmin到θmax， 步距 θstep； S22、 根据目标位姿 与映射集 比对， 提取满足如下空间位置的fMap值：权　利　要　求　书 1/3 页 2 CN 115533883 A 2|XArmPose‑XMap|≤L |YArmPose‑YMap|≤L |ZArmPose‑ZMap|≤L 即提取位置位于目标位姿周围， 以L为边长的栅格内的映射关系， 作为初次筛选的fMap 结果； S23、 在初次筛选 的fMap中， 分别求出与 的位置X， Y， Z差值和 姿态 差值： ΔX＝|XArmPose‑XMap| ΔY＝|YArmPose‑YMap| ΔZ＝|ZArmPose‑ZMap| 并在姿态上叠加一个加权值 k， 求出初次筛 选的fMap与fArmPose的位姿差值加权和Δf： 在所有位姿差值加权和中取最小的m个值， 得到精筛 选的初始映射关系： S24、 将精筛 选的m个初始映射关系中的θ1， θ2... θN依次进行迭代法求逆解计算： 对于得到的逆解 值分别进行正 解反验算： 当正解结果和目标位姿一 致时， 即确认求得正确的逆解 值： f逆 解( θ1, θ2…θN)。 6.根据权利要求1所述的欠自由度机械臂奇异性预估与运动控制方法， 其特征在于， 所 述S3中， 求 解机械臂雅可比矩阵采用矢量积法， 具体包括： 按列进行雅可比矩阵的计算， 如果关节i 为移动关节， 则雅可比矩阵的第i列为： 如果关节i 为转动关节， 则雅可比矩阵的第i列为： 式中： Ji表示雅可比矩阵的第i列， ×表示矢量积符号， 表示末端执行器坐标原点相 对坐标系{i}的位置矢量在基坐标系{0}中的表示， Zi表示在基坐标系{0}中表示 的坐标系权　利　要　求　书 2/3 页 3 CN 115533883 A 3