(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210582484.X (22)申请日 2022.05.26 (71)申请人 哈尔滨工业大 学 地址 150001 黑龙江省哈尔滨市南岗区西 大直街92号 (72)发明人 杨国财　马正航　纪军红　龙沁　 李志奇　史士财　 (74)专利代理 机构 哈尔滨华夏松花江知识产权 代理有限公司 23213 专利代理师 岳昕 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 一种基于末端测量反馈的机械臂关节轨迹 规划方法 (57)摘要 一种基于末端测量反馈的机械臂关节轨迹 规划方法， 涉及机械臂轨迹规划技术领域， 针对 现有规划方法仅适用于结构化环 境， 并且拖动示 教的方式需要人为辅助的问题， 本申请不需要已 知的离散点， 直接根据期望位置规划机械臂的关 节轨迹， 因此不局限于几何信息的已知的结构化 环境； 本申请在线自主规划机械臂的关节轨迹， 不需要人为拖动示教， 减小了人工工作量， 并提 高了效率及自主性； 本申请实现了轨迹的加加速 度连续， 提高了轨迹平滑性， 减小了执行器运行 负担。 权利要求书2页 说明书6页 附图11页 CN 114952838 A 2022.08.30 CN 114952838 A 1.一种基于末端测量反馈的机 械臂关节轨 迹规划方法， 其特 征在于包括以下步骤： 步骤一： 获取目标坐标系相对于机械臂末端坐标系的位置数据d和姿态数据 并利用d 和 得到目标坐标系相对于 机械臂末端坐标系的位姿矩阵eTt； 步骤二： 获取机械臂关节角度矢量ql， 并根据机械臂关节角度矢量ql得到机械臂末端坐 标系相对于 机械臂基座 坐标系的位姿矩阵bTe； 步骤三： 根据目标坐标系相对于机械臂末端坐标系的位姿矩阵eTt以及机械臂末端坐标 系相对于机械臂基座坐标系的位姿矩阵bTe得到目标坐标系相对于机械臂基座坐标系的位 姿矩阵bTt， 基于目标坐标系相对于机械臂基座坐标系的位姿矩阵bTt并采用逆运动学得到 关节期望位置 矢量qd； 步骤四： 基于关节期望位置 矢量qd， 规划得到 机械臂关节参 考轨迹； 步骤五： 机 械臂关节控制器跟踪机 械臂关节参 考轨迹， 驱动机 械臂运动。 2.根据权利要求1所述的一种基于末端测量反馈的机械臂关节轨迹规划方法， 其特征 在于所述目标坐标系相对于机械臂末端坐标系的位置数据d和姿态数据 通过机械臂末端 安装的测量传感器得到 。 3.根据权利要求1所述的一种基于末端测量反馈的机械臂关节轨迹规划方法， 其特征 在于所述目标坐标系相对于 机械臂末端坐标系的位姿矩阵eTt表示为： 其中， d表示目标坐标系原点在机械臂末端坐标系下的位置矢量， 表示目标坐标系相 对于机械臂末端坐标系的姿态矢量， wz2mt rx()表示 位姿矢量 转换至位姿矩阵的函数。 4.根据权利要求1所述的一种基于末端测量反馈的机械臂关节轨迹规划方法， 其特征 在于所述机 械臂关节角度矢量ql通过关节位置传感器测量得到 。 5.根据权利要求1所述的一种基于末端测量反馈的机械臂关节轨迹规划方法， 其特征 在于所述机 械臂末端坐标系相对于 机械臂基座 坐标系的位姿矩阵bTe表示为： bTe＝forwardk inematics(ql) 其中， ql表示机械臂关节角度矢量， forwardkinematics()表示机械臂的正运动学函 数。 6.根据权利要求1所述的一种基于末端测量反馈的机械臂关节轨迹规划方法， 其特征 在于所述关节期望位置 矢量qd表示为： qd＝inversekinematics(bTt) bTt＝bTeeTt 其中， inversekin ematics()表示机械臂的逆运动学函数，bTt表示目标坐 标系相对 于机 械臂基座坐标系的位姿矩阵，eTt表示目标坐标系相对于机械臂末端坐标系的位姿矩阵，bTe 表示机械臂末端坐标系相对于 机械臂基座 坐标系的位姿矩阵。 7.根据权利要求1所述的一种基于末端测量反馈的机械臂关节轨迹规划方法， 其特征 在于所述机械臂关节参考轨迹包括： 关节参考加加速度矢量 关节参考加速度矢量 关 节参考速度矢量 和关节参 考位置矢量qr。 8.根据权利要求7所述的一种基于末端测量反馈的机械臂关节轨迹规划方法， 其特征 在于所述 步骤四的具体步骤为：权　利　要　求　书 1/2 页 2 CN 114952838 A 2步骤四一： 设定规划参数k， ξ和ωn， 所述规划参数k， ξ和ωn均为正定的对角矩阵； 步骤四二： 根据设定 的规划参数k， ξ和ωn， 得到位置环反馈增益Kp、 速度环反馈增益Kv 以及加速度环增益Ka： 步骤四三： 根据位置环反馈增益Kp、 速度环反馈增益Kv、 加速度环增益Ka以及关节期望 位置矢量qd， 规划关节参 考加加速度矢量 步骤四四： 根据关节参考加加速度矢量 得到关节参考加速度矢量 关节参考速度 矢量 以及关节参 考位置矢量qr。 9.根据权利要求8所述的一种基于末端测量反馈的机械臂关节轨迹规划方法， 其特征 在于所述 位置环反馈增益Kp、 速度环反馈增益Kv以及加速度环增益Ka表示为： Kp＝kωn2 Kv＝ωn2+2kξ ωn Ka＝k+2 ξ ωn。 10.根据权利要求9所述的一种基于末端测量反馈的机械臂关节轨迹规划方法， 其特征 在于所述关节参 考加加速度矢量 表示为： 所述关节参 考加速度矢量 关节参考速度矢量 和关节参 考位置矢量qr表示为： 其中， ∫ dt 表示积分。权　利　要　求　书 2/2 页 3 CN 114952838 A 3