(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211337039.3 (22)申请日 2022.10.28 (71)申请人 天津大学 地址 300072 天津市南 开区卫津路9 2号 (72)发明人 田文杰　郭旭　王丽娜　张相鹏　 黄田　 (74)专利代理 机构 天津市北洋 有限责任专利代 理事务所 12 201 专利代理师 吴学颖 (51)Int.Cl. B25J 9/00(2006.01) B25J 9/16(2006.01) (54)发明名称 基于内部传感信息的混联机器人运动学标 定方法 (57)摘要 本发明公开了一种基于内部传感信息的混 联机器人运动学标定方法： 混联机器人内部传感 系统的设计、 安装与校准： 混联机器人运动系统 的划分， 内部传感系统的设计与安装， 内部传感 系统的校准； 混联机器人从动运动链结构误差参 数的识别： 主动运动链与从动运动链之间的理想 运动学关系的构建， 从动运动链综合误差模型的 建立， 刀具位姿误差的测量与求解， 从动运动链 结构误差的辨识； 基于从动运动链内部传感系统 实时测量信息的机器人位姿误差补偿： 混联机器 人末端构 件位姿误差的实时预测， 混联机器人驱 动关节补偿量的实时求解， 基于内部传感信息的 混联机器人位姿误差实时补偿 。 本发明可直接基 于机器人内部误差 检测数据完成位姿误差补偿。 权利要求书3页 说明书8页 附图1页 CN 115533876 A 2022.12.30 CN 115533876 A 1.一种基于 内部传感信息的混联机器人运动学标定方法， 所述混联机器人由并联机构 与串联转头构成， 所述并联机构由第一主动伸缩装置(1)、 第二主动伸缩装置(2)、 第三主动 伸缩装置(3)、 从动伸缩装置(4)、 动平台(5)、 第一转动支撑结构(6)、 第二转动支撑结构 (7)、 第一机架轴承座(8)、 第二机架轴承座(9)组成， 所述串 联转头由第一转动部件(11)、 第 二转动部件(12)、 主轴(13)、 刀具(14)构成， 其特 征在于， 标定方法包括以下步骤： 步骤一， 混联机器人内部传感系统的设计、 安装与校准 1)混联机器人运动系统的划分 依据运动学等效原 理， 将所述混联机器人划分为主动 运动链与从动 运动链两个独立的 运动系统； 所述主动运动链由第一主动伸缩装置(1)、 第二主动伸缩装置(2)、 第三主动伸缩 装置(3)、 动平台(5)、 第一转动支撑结构(6)、 第二转动支撑结构(7)、 串联转头构成； 所述 从 动运动链由第一 转动支撑结构(6)、 从动伸缩 装置(4)、 动平台(5)、 串联转头构成； 2)内部传感系统的设计与安装 依据内部传感器所对应的运动 生成元可张成末端运动输出构件可控运动空间的原则， 在从动运动链上设计并安装内部传感系统； 所述内部传感系统由第一圆光栅(21)、 第二圆 光栅(22)、 第三直线光栅(23)、 第四圆光栅、 第五圆光栅构成， 其中， 第一圆光栅(21)安装于 第一转动支撑结构(6)与第一机架轴承座(8)之间的转动副， 第二圆光栅(22)安装于第一转 动支撑结构(6)与从动伸缩装置(4)之间的转动副， 第三直线光栅(23)安装于从动伸缩装置 (4)中的移动副， 第四圆光栅安装于第一转动部件(11)与动平台(5)之间的转动副内部， 第 五圆光栅安装于第一 转动部件(1 1)与第二 转动部件(12)之间的转动副内部； 3)内部传感系统的校准 采用外部测量仪器获取混联机器人运动至作业空间内不同位形处的刀具六维位姿误 差， 利用驱动关节误差传递模型构建误差传递方程组， 并利用正则回归算法求解出机器人 五个驱动关节的零点误差； 将所得零点误差的相反数存储至数控系统中与各个驱动关节对 应的零点偏置变量中， 实现混联机器人的驱动关节零点误差补偿； 在此基础上， 控制混联机 器人各驱动关节运动至零点位置， 并将当前位形下 的各个光栅的读数清零， 定义此时的各 光栅位置为各光 栅的零点位置， 从而实现内部传感系统的零 位校准； 步骤二， 混联机器人从动运动链结构误差参数的识别 1)主动运动链与从动运动链之间的理想运动学关系的构建 采用机构学方法建立无误差状态下主动运动链驱动关节变量与刀具位姿之间的映射 模型p＝f主(q主)， 其中， p为刀具六维位姿向量， q主为主动运动链驱动关节变量， f主为q主与p之 间的映射函数； 采用机构学方法建立无误差状态下从动运动链内部传感系统读数与刀具位 姿之间的映射模型 其中， q从为从动运动链广义驱动关节变量， 其数值由从动运 动链内部传感系统读数获取， 为p与q从之间的映射 函数； 联立上述两个函数关系式， 求解得到无误差状态下主动运动链驱动关节变量q主与从动 运动链广义驱动关节变量q从之间的映射 函数： 2)从动运动链综合 误差模型的建立 依据从动 运动链的误差源是否可被 内部传感系统直接检测， 将从动 运动链中的误差源权　利　要　求　书 1/3 页 2 CN 115533876 A 2分解为广义驱动关节运动误差Δq从与结构误差Δs两类； 所述广义驱动关节运动误差Δq从 是指与内部传感系统测量量值相对应的关节运动误差， 通过从动运动链广义驱动关节变量 的实测值q从,实测与理论值q从,理论作差得到， 即Δq从＝q从,实测‑q从,理论， q从,理论由式(1)计算得到； 所 述结构误差 Δs包括从动运动链的零部件加工误差、 装配误差、 轴线偏差； 采用旋量方法、 POE方法或D ‑H方法， 建立线性 化表达的从动运动链综合 误差模型如下： Δp＝AΔq从+BΔs            (2) 式中， Δp为刀具位姿误差向量， A为广义驱动关节运动误差 映射矩阵， B为结构误差 映 射矩阵； 3)刀具位姿误差的测量与求 解 当机器人运动至任一位形时， 利用激光跟踪仪仪器设备测量主轴上不共线的3个点P1、 P2、 P3的空间位置误差， 分别记为ΔrP,1、 ΔrP,2、 ΔrP,3； 将点Pm(m＝1,2,3)的位置误差ΔrP,m 表示为 ΔrP,m＝Δr+Δθ ×RrP,m,0， m＝1,2,3          (3) 式中， Δr与Δθ分别为刀具的位置与姿态误差向量， rP,m,0为刀尖点指向点Pm的向量， R 为刀具相对于混联机器人机架的姿态 矩阵； 将式(3)中的三个等式联立并改写成矩阵格式， 有 ΔrP＝KΔp               (4) 式中， ΔrP为由三个点的位置误差构成的向量， Δp为刀具位姿误差向量， K为ΔrP与Δp 之间的映射矩阵， I3为三阶单位矩阵； 根据式(4)并采用最小二乘方法， 求解机器人位于当 前位形处的刀具位姿误差向量Δp Δp＝(KTK)‑1KTΔrP         (5) 式中， KT为矩阵K的转置矩阵； 4)从动运动链结构误差的辨识 控制机器人运动至其工作空间中的n个位形， 在每一个位形处， 利用所述刀具位姿误差 的测量与求解方法， 获取刀具 的位姿误差向量Δp， 并记第i个位形处的机器人刀具位姿误 差向量为Δpi(i＝1～n)； 利用n个位形对应的n个从动运动 链综合误差模型(即公式(2))组 集成从动运动链结构误差的辨识方程组： 其中， Δpi为第i个位形处的机器人刀具位姿误差向量， Ai为第i个位形处的从动运动 链 广义驱动关节运动误差映射矩阵， Bi为第i个位形处的从动运动链结构误差映射矩阵， Δ q从,i为第i个位形处的从动运动链广义驱动关节运动误差， 可由从动运动链内部传感系统读 数计算得到； 所选取的n个位形必须使得从动运动链结构误差Δs可辨识， 利用式(6)并采用最小二权　利　要　求　书 2/3 页 3 CN 115533876 A 3