(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210548168.0 (22)申请日 2022.05.18 (71)申请人 佛山汇真科技有限公司 地址 528200 广东省佛山市南海区桂城街 道夏南路12 号天富科技中心4号楼1层 101单元之一 申请人 辽宁中科博研科技有限公司 　 沈阳大学 (72)发明人 张宏伟　韩晓微　石泽亮　汤广全　 曹楠　 (74)专利代理 机构 沈阳优普达知识产权代理事 务所(特殊普通 合伙) 21234 专利代理师 陈曦 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 一种移动操作臂 协同控制方法 (57)摘要 本发明的一种移动操作臂协同控制方法， 其 包括： 建立四轮移动底盘和操作臂的运动学模 型， 联立获得移动操作臂运动学模型； 利用移动 操作臂运动学模型， 完成相机内外参标定； 采集 目标物的位姿轨迹并完成坐标转换与投影平移 处理； 设计视觉伺服跟踪控制器， 实现四轮移动 底盘对目标物投影平移后的轨迹跟踪； 设计PID 和自抗扰视觉伺服抓取控制器， 联立视觉伺服跟 踪控制器构建联合控制系统以对移动操作臂进 行协同控制。 本发明控制方法有效解决了动态抓 取过程中精度低、 响应速度慢、 抗干扰性差的问 题， 使其适用于新材料用智能装 备制备材料过程 的物料精 准取送， 实现了特殊工况条件下的物料 自动控制。 权利要求书5页 说明书11页 附图4页 CN 114986498 A 2022.09.02 CN 114986498 A 1.一种移动操作臂 协同控制方法， 其特 征在于， 包括以下步骤： S1、 建立四轮移动底盘的运动学模型和操作臂的运动学模型， 联立获得移动操作臂 的 运动学模型， 并设定初始参数； S2、 利用移动操作臂运动学模型， 完成相机的内外参标定； S3、 利用相机采集目标物的位姿轨 迹并完成坐标转换与投影平 移处理； S4、 根据四轮移动底盘的运动模型设计基于模型预测控制的视觉伺服跟踪控制器， 以 实现四轮移动底盘对目标物投影平 移后的轨 迹跟踪； S5、 针对操作臂抓取设计PID和自抗扰视觉伺服抓取控制器， 联立视觉伺服跟踪控制器 构建联合控制系统以对移动操作臂进行协同控制。 2.如权利要求1所述的移动操作臂 协同控制方法， 其特 征在于， 步骤S1包括： S11、 建立四轮移动底盘的运动学模型， 四轮移动底盘定义在二维平面XWY平面， 其中W 代表世界坐标系原点， 下式(1)是四轮移动底盘运动学 方程： 上式中(x,y)是四轮移动底盘后轴中心位置坐标； 是四轮移动底盘与世界坐标系X轴 的夹角； l为四轮移动底盘的车轮轴距， v为后轴当前速度， σ 为四轮移动底盘航向与前轮转 向之间的夹角； 定义状态变量为 控制量为u＝[v, σ ]T， 对于在参考轨迹中的任意一点用带 下标的s表示， 把式(1)用f函数表示， 利用泰勒级数展开并忽略高阶项完成线性 化： 定义状态误差量 如下： 对式(3)进行前向欧拉离散化， 其中T为采样周期， k、 k+1分别为当前采样和后一采样时 刻， 整理可 得 式(2)中雅克比矩阵带入上式(4)中可 得离散线性化运动学方程： S12、 利用DH参数法建立操作臂运动学模型， 用cθi来表示cosθi； 用sθi来表示sinθi； cαi‑1权　利　要　求　书 1/5 页 2 CN 114986498 A 2代表cosαi‑1； sαi‑1代表sinαi‑1； 上式中i表示关节号， ai‑1为操作臂的关节轴i ‑1和关节轴i之间公垂线的连杆长度、 αi‑1 为关节轴i ‑1和关节轴i之间的的连杆转角、 di‑1为关节轴i ‑1上的连杆偏距， θi为关节轴i ‑1 和关节轴i公共轴线旋转的关节角； 将UR5操作臂的DH参数表带入式(6)得出操作臂末端坐标系6相对操作臂基座坐标系0 的变换矩阵： S13、 联立四轮移动底盘和操作臂建立移动操作臂运动学模型， 定义M代表四轮移动底 盘坐标系， W代表世界坐标系， S代表目标物坐标系； 在操作臂末端带有二指夹爪， 设四轮移 动底盘坐标系M在世界坐 标系W下的位置 为WPM＝(wxM,wyM,wzM)， 设四轮移动底盘 坐标系M在世 界坐标系W下的姿态角等于 则可得到四轮移动底盘 坐标系M相对于世界坐标系W的转换矩 阵 操作臂基座坐标系0在M坐标系下的坐标为MP0＝(Mx0,My0,Mz0)， 定义Mx0＝e，My0＝0，Mz0＝ h， 于是得到操作臂基座 坐标系0相对于四轮移动底盘坐标系M的变换矩阵： 通过 得到操作臂末端坐标系6在世界坐标系W中的位姿变换矩阵 设操 作臂末端坐标系6相对于世界坐标系W的位置为WP6＝(Wx6,Wy6,Wz6)， 姿态为WΩ6＝(Wα6,Wβ6,W γ6)， 则得到操作臂末端坐标系 6相对于四轮移动底盘坐标系M的位置为：MP6＝(Wx6‑WxM,Wy6 ‑WyM,Wz6‑WzM)； 操作臂基座与四轮移动底盘固连， 得操作臂末端坐标系 6相对于基座的坐标 0P6＝(Wx6‑WxM‑l,Wy6‑WyM,Wz6‑WzM‑h)， 完成坐标变换后利用参数法逆解到关节空间完成位置 逆解； 姿态的逆解就是操作臂六自由度的逆解， 即Ω6＝WΩ6＝(Wα6,Wβ6,Wγ6)， 利用参数法逆 解到关节空间便 完成了姿态逆解。 3.如权利要求2所述的移动操作臂 协同控制方法， 其特 征在于， 步骤S2包括： S21、 内参标定决定相机内部映射关系， 通过从不同的角度拍摄已经打印好的黑白棋盘 获得角点信息， 提取亚像素角点信息， 计算反向投影误差， 完成内参标定； S22、 外参标定即求得相机坐标系相对于操作臂末端的工具坐标系的转换矩阵 权　利　要　求　书 2/5 页 3 CN 114986498 A 3