(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210395720.7 (22)申请日 2022.04.15 (71)申请人 伯朗特机 器人股份有限公司 地址 523791 广东省东莞 市大朗镇沙步村 沙富路83号 (72)发明人 郭鹏　 (74)专利代理 机构 广州骏思知识产权代理有限 公司 44425 专利代理师 龙婷 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 一种基于附加编码器的并联六轴机器人速 度正解方法 (57)摘要 本发明涉及一种基于附加编码器的并联六 轴机器人速度正解方法和六轴机器人， 本发明提 供的并联六轴机器人速度正解方法在机器人的 特定关节处安装了五个角度编码器， 可以额外读 取5个部位的角度值和角速度值， 同时结合给定 并联六轴机器人的结构参数以及任意时刻 的电 缸活塞杆的伸长量以及伸缩速度， 来求解动平台 任意时刻的线速度和角速度。 因为通过五个编码 器可以获得更多的信息， 可以降低求解正解的难 度， 并且本发明在运算过程中， 只有初等的矩阵 乘法运算， 即只有简易的乘法和加法运算， 所 以 计算效率得到有效提高。 上述方法还便于使用C/ C++等编程语言重构， 重构以后即可运行控制系 统中。 权利要求书7页 说明书14页 附图14页 CN 114589700 A 2022.06.07 CN 114589700 A 1.一种基于附加编码器的并联 六轴机器人速度正 解方法， 其特 征在于， 所述六轴机器人包括静平台、 动平台、 虎克铰组件、 关节组件和电缸组件； 所述关节组件 包括： 轴承座； 交叉轴壳体， 所述交叉轴壳体转动设置在所述轴承座上； 轴耳， 所述轴耳转动设置在所述交叉轴壳体上， 所述轴耳在所述交叉轴壳体上的旋转 轴线与所述交叉轴壳体在所述轴承座上的旋转轴线相交且相互垂直， 在所述轴耳上设置有 两个轴承孔， 所述两个轴承孔关于所述轴 耳在所述交叉轴壳体上 的旋转轴线对称分布， 所 述两个轴承孔的轴线相互平行且位于同一水平面上， 所述轴承孔的轴线与所述交叉轴壳体 在所述轴承座上的旋转轴线相互垂直； 所述虎克铰组件安装在所述静平台上， 所述关节组件安装在所述动平台上， 所述电缸 组件的一端安装在所述虎克铰组件上， 所述电缸 组件的另一端通过承力销轴转动连接在所 述关节组件的所述轴耳 上， 所述承力 销轴转动设置在所述轴耳的所述轴承孔内； 在所述静平台上安装有六个所述虎克铰组件， 所述六个虎克铰组件分别为第 一虎克铰 组件、 第二虎克铰组件、 第三虎克铰组件、 第四虎克铰组件、 第五虎克铰组件、 第六虎克铰组 件， 在所述动平台上设置有三个所述关节组件， 所述三个关节组件分别为第一关节组件、 第 二关节组件、 第三关节组件， 所述静平台与所述动平台之 间设置有 六个所述电缸组件， 所述 六个电缸组件分别为第一电缸组件、 第二电缸组件、 第三电缸组件、 第四电缸组件、 第五电 缸组件、 第六电缸组件； 所述第二电缸组件、 所述第三电缸组件、 所述第二虎克铰组件和所述第三虎克铰组件 以及所述第一关节组件形成第一控制组件， 所述第四电缸组件、 所述第五电缸 组件、 所述第 四虎克铰组件、 所述第 五虎克铰组件以及所述第二关节组件形成第二控制组件， 所述第六 电缸组件、 所述第一电缸组件、 所述第六虎克铰组件、 所述第一虎克铰组件、 以及所述第三 关节组件形成第三控制组件； 所述速度正 解方法包括： S10： 在机器人的第一控制组件上安装五个角度编码器以读取关节角度值和关节角速 度值， 包括如下子步骤： S101： 在第 一控制组件的第二虎克铰组件的虎克铰上基座和虎克铰下基座上均安装一 个角度编码器， 记为第一角度编码器和第二角度编码器， 用于测 量第二虎克铰组件的固定 轴部和摆动轴部分别相对于虎克铰下基座和虎克铰上基座的旋转角度和旋转角速度， 分别 记为j θ1、 j θ2和 S102： 在第一控制组件的第三虎克铰组件的虎克铰上基座上安装一角度编码器， 记为 第三角度编 码器， 用于测量摆动轴部相对于虎克铰组件的虎克铰上基座的旋转角度和旋转 角速度， 记为j θ3和 S103： 在第一控制组件的关节组件内部安装一角度编码器， 记为第四角度编码器， 用于 测量关节组件的轴耳的轴部相对于交叉轴壳体的角度值和角速度值， 记为j θ4和 S104： 在动平台上与第一控制组件连接的位置处安装一角度编码器， 记为第五角度编权　利　要　求　书 1/7 页 2 CN 114589700 A 2码器， 用于测量动平台相对于关节组件的旋转角度值和旋转角速度值， 记为j θ5和 S20： 构建并联 六轴机器人的运动学 元素； Bi,i＝1～6： 虎克铰组件原点， 将Bi,i＝1～6点设置在虎克铰组件的十 字轴中心； {O}： 基坐标系， 在静平台上建立直角坐标系{O}， 将 坐标系的原点O设置在B1～B6确定的 平面上， 且位于B1～B6确定的圆的圆心位置， y轴方向设置在OB1,OB2线段的角平分线位置， 此时六个虎克铰组件相对于y轴对称， z轴设置朝上， x轴根据右手定则即可自动确定； Rb： 表示B1～B6确定的圆的半径， 称为 “虎克铰虚拟圆半径 ”； OOB1～OOB6：OOBi： 虎克铰位置向量， 具体表示以基坐标系的原点为起点， 第i个虎克铰原 点Bi,i＝1～6为终点的向量， 参 考坐标系为基坐标系； βi， i＝1～6： 虎克铰偏置角， 表示OOBi与基坐标系{O}的Y轴的角度； RQ： 称为“交叉轴虚拟圆半径 ”， 表示三个交叉轴原点确定的圆的半径值； Qi， i＝1～3： 轴耳原点， 轴耳零件上与Ui， i＝1～3对应重合的点， 设为 Qi， i＝1～3； {Qi}， i＝1～3： 表示轴耳坐标系。 坐标系原点固连在轴耳原点Qi， {Qi}的z轴沿轴耳的轴 部轴线方向， y轴沿销轴铰链两点的连线方向， x轴根据右手定则确定； Ui： 交叉轴原点， 关节组件的两个圆柱面的轴线行成一个交点， 设该点 为Ui， i＝1～3； {Ui}和i＝1～3： 分别表示交叉轴坐标系， 坐标系 原点固连在交叉轴零件原点Ui， {Ui}的 z轴沿轴耳的轴部轴线方向， y轴沿中心轴 轴线的方向， x轴根据右手定则确定 。 {P}： 动平台坐标系， {P}的原点P 位于Ui， i＝1～3确定的平面上， 且位于三个Ui确定的圆 的圆心位置。 设置{P}的y轴设置为： 其中U2在y轴的负方向， U1,U3相对于y轴对称， z轴朝上， x 轴可用右手定则确定 。 如图13所示。 轴耳原点的分布角度 OOP： 动平台的位置， 表示动平台坐标系{ P}的原点P相对于静平台坐标系原点 O的位置向 量； ORP： 动平台的姿态， 表示动平台坐标系{P}相对于静平台坐标系{O}的旋转矩阵； A1～A6： 销轴铰链中心， 在轴 耳零件1(轴 耳零件2， 轴 耳零件3)中， 两个销轴铰链中心点 A2(点A4， 点A6)和点A3(点A5， 点A1)点相对于{Q1}({Q2}， {Q3})的Y轴对称布置， 将点A2(点A4， 点 A6)和点A3(点A5， 点A1)放置在{Q1}({Q2}， {Q3})的XY平面上， 用QAX表示A2和A3在{Q1}的X轴方 向的绝对偏移值， QAY表示A2和A3在{Q1}的Y轴方向的绝对偏移值； ΟΒΑi： 表示电缸位置向量， 以基坐标系{O}为 参考系； Q1'： 表示轴耳零件的速度瞬心； S30： 从电缸组件自带的控制系统读出电缸的长度， 记为l1～l6， 用l1～l6表示ΟΒΑ1 ～ΟΒΑ6的模， 设l1＝|ΟΒΑ1|， ...， l6＝|ΟΒΑ6|， 从电缸组件自带的控制系统读出电缸 组件中的活塞杆相对于缸体的伸缩速度vLi,i＝1～6； S40： 在第二虎克铰组件的十字轴上， 以十字轴中心B2为原点建立虎克铰坐标系{B 2}， 在 第三虎克铰组件的十 字轴上， 以十 字轴中心B3为原点建立虎克铰坐标系{B3}； S50： 在第二虎克铰组件的十字轴上， 建立电缸坐标系{L2}， 其原点与{B2}的原点重合， 在第三虎克铰组件的虎克铰上基座上， 建立电缸坐标系{L3}， 其原点与 {B3}的原点重合；权　利　要　求　书 2/7 页 3 CN 114589700 A 3