(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210832949.2 (22)申请日 2022.07.14 (71)申请人 中国科学院沈阳自动化研究所 地址 110016 辽宁省沈阳市沈河区南塔街 114号 (72)发明人 刘金国　张鑫　周浩　 (74)专利代理 机构 沈阳科苑专利商标代理有限 公司 210 02 专利代理师 王倩 (51)Int.Cl. B25J 9/16(2006.01) B25J 18/02(2006.01) (54)发明名称 一种大型重载液压机械臂的增量式运动控 制方法 (57)摘要 本发明属于大型重载机器人领域， 具体 说是 一种大型重载液压机械臂的增量式运动控制方 法， 包括以下步骤： 建立液压机械臂三维几何模 型； 分别设置初始位置、 第一过渡位置、 第二过渡 位置和最佳抓捕位置； 并分别对初始位置、 第一 过渡位置和第二过渡位置时的三维几何模型进 行投影； 根据液压机械臂三维几何模型以及投 影， 获取液压机械臂从初始位置到最佳抓捕位置 各关节增量角以及伸缩关节增量长度； 根据各关 节的增量角以及伸缩关节的增量长度， 实现液压 机械臂对目标物 体的抓捕。 本发 明所提出的增量 式运动控制方法， 无需对形变进行复杂的建模和 参数辨识， 可以对液压机械臂在当前构型一定的 笛卡尔空间邻域内进行精准的位置控制， 确保抓 捕任务的成功率。 权利要求书4页 说明书13页 附图3页 CN 115070769 A 2022.09.20 CN 115070769 A 1.一种大 型重载液压机械臂的增量式运动控制方法， 其特 征在于， 包括以下步骤： 1)根据实际的液压 机械臂建立液压 机械臂的三维几何模型； 2)分别设置初始位置、 第一过渡位置、 第二过渡位置和最佳抓捕位置； 并分别 对初始位 置、 第一过渡位置和第二过渡位置时的三维几何模型进行投影， 分别获取三维几何模型在 初始位置正 面投影、 第一过渡位置水平投影和第二过渡位置正 面投影； 3)根据液压机械臂的三维几何模型以及初始位置正面投影、 第一过渡位置水平投影和 第二过渡位置正面投影， 获取液压机械臂从初始 位置到最佳抓捕位置的回转关节J1的增量 角、 俯仰关节J2的增量角、 回转关节J3的增量角、 伸缩关节J4的增量长度和回转关节J5的增 量角； 4)根据求解出的各关节的增量角以及伸缩关节的增量长度， 通过控制器实现液压机械 臂对目标物体的抓捕。 2.根据权利要求1所述的一种大型重载液压机械臂的增量式运动控制方法， 其特征在 于， 所述步骤1)， 具体为： 所述液压 机械臂， 包括： 液压臂、 吊具以及视 觉系统； 其中， 所述液压臂， 包括： 顺次连接的基座、 主臂、 大臂以及伸展臂； 分别建立液压臂、 吊具以及视觉系统 的模型， 并进行组合得到三维几何模型， 包括以下 步骤： (1‑1)建立液压臂模型， 其中， 所述基座与主臂一端通过回转关节J1连接， 主臂另一端 与大臂一端通过俯仰关节J2连接， 大臂另一端与伸展臂通过俯仰关节J3连接， 伸展臂中间 位置设有伸缩关节J4； 对液压臂建立模 型， 分别得到： 回转关节J1、 俯仰关节J2、 俯仰关节J3 和伸缩关节J4； 所述伸缩关节J4距离J3关节轴线l距离处设有拉线传感器， 用于读取的伸展臂的伸长 长度； (1‑2)对吊具建立模型： 所述吊具的俯仰关节J6所在连杆固接于伸展臂的末端， 俯仰关 节J6底部与回转关节J5顶部固连， 回转关节J5底部固接有夹爪， 建立吊具模型， 得到： 回转 关节J5、 俯仰关节J6和夹爪模型； (1‑3)所述视觉系统， 包括： 视觉标靶和相机， 所述相机安装在吊具底部某一端的末端， 视觉标靶安装在目标物体上， 在吊取目标物体时， 以使相机获取视 觉靶标在相机中的位姿； (1‑4)定义液压机械臂的部分关节零位： 俯仰关节 J2、 回转关节 J3和伸缩关节 J4位于同 一水平面； 回转关节J5将吊具顶面与伸缩关节J4的连杆调整至平行状态， 且俯仰关节J6将 吊具调至平行于水平面。 3.根据权利要求1所述的一种大型重载液压机械臂的增量式运动控制方法， 其特征在 于， 所述正面投影为， 以回转关节J2和J3轴线的垂直面基准面进行投影； 所述水平投影为， 以当液压 机械臂位于零 位时回转关节J2和J3的轴线所在水平面 为基准面进行投影。 4.根据权利要求1所述的一种大型重载液压机械臂的增量式运动控制方法， 其特征在 于， 所述第一 过渡位置为： 基于初始位置， 回转关节J3的抓捕角度为θ3T时液压机械臂所处位 置； 所述抓捕角度 θ3T为在液压 机械臂抓捕目标物体时 回转关节J3的角度； 所述第二过渡位置为： 基于第一过渡位置的液压机械臂位置， 使液压机械臂吊具位于权　利　要　求　书 1/4 页 2 CN 115070769 A 2目标物体正上 方位置； 所述最佳抓捕位置为： 基于第二过渡位置的液压机械臂位置， 使液压机械臂距离目标 物体正上 方设定高度dr位置， 高度dr用于液压 机械臂对目标物体进行抓捕。 5.根据权利要求1所述的一种大型重载液压机械臂的增量式运动控制方法， 其特征在 于， 所述步骤3)， 具体为： (1)对液压机械臂在初始位置进行正面投影， 并确定液压机械臂的第一过渡位置， 获取 俯仰关节J2轴线到吊具中心在水平面投影的长度； (2)对液压机械臂在第一过渡位置进行水平面投影， 此时， 吊具位于目标物体的正上 方， 确定的液压机械臂的第二过渡位置， 根据俯仰关节J2轴线到吊具中心在水平面投影的 长度以及相机此时获取的视觉靶标在相机中的位姿， 获取回转关节J1的增量角Δθ1、 当前 伸缩关节J4的增量长度Δd4c和回转关节J5的增量角Δ δ； (3)对液压机械臂在第二过渡位置进行正面投影， 确定的液压机械臂的最佳抓捕位置， 并获取俯仰关节J2的增 量角Δθ2、 回转关节J3的增 量角Δθ3和当前J4的增 量长度Δd4g， 以 及回转关节J5的增量角。 6.根据权利要求5所述的一种大型重载液压机械臂的增量式运动控制方法， 其特征在 于， 所述步骤(1)， 具体为： 当液压机械臂处于初始位置时， 对其在正面进行投影， 设定回转关节J3的抓捕角度为 θ3T， 并通过俯仰关节J6将吊具调至水平状态， 确定第一过渡位置； 在第一过渡位置上， 视觉系统进行图像采集， 并得到目标物体的视觉靶标在相机坐标 系{C}中的位姿(cx1，cy1，cz1，cε0)； 其中，cε0为在相机坐标系{C}的z方向的欧拉角； 得到液压机械臂位于第一过渡位置时， 俯仰关节J2轴 线到吊具中心在水平面投影的长 度， 即： 其中， L’11表示液压机械臂位于第一过渡位置时俯仰关节J2距离回转关节J5在液压机 械臂正投影面上的距离， 为L’11与水平面的夹角。 7.根据权利要求5所述的一种大型重载液压机械臂的增量式运动控制方法， 其特征在 于， 所述步骤(2)， 具体为： 2‑1)对位于第一过渡位置的液压机械臂进行水平面投影， 得到目标物体的视觉靶标T1 在相机坐标系{C} 中的位置{cx1，cy1，cz1}和在z方向的欧拉角cε0； 2‑2)获取当液压机械臂位于第一过渡位置时J1的轴线到吊具中心在水平面上的距离 D1， 即： D1＝L1.pro‑a1 其中，oxcom用于补偿在oX方向的误差， (xc0，cy0)为目标物体的中心T0在相机坐标系{C}中 的二维坐标，cε0为z方向的欧拉角； 液压机械臂从第一过渡位置运动到第二过渡位置时J4的增量长度为：权　利　要　求　书 2/4 页 3 CN 115070769 A 3