[发明专利]一种基于附加编码器的并联六轴机器人速度正解方法

权利要求书

1.一种基于附加编码器的并联六轴机器人速度正解方法，其特征在于，

所述六轴机器人包括静平台、动平台、虎克铰组件、关节组件和电缸组件；

所述关节组件包括：

轴承座；

交叉轴壳体，所述交叉轴壳体转动设置在所述轴承座上；

轴耳，所述轴耳转动设置在所述交叉轴壳体上，所述轴耳在所述交叉轴壳体上的旋转轴线与所述交叉轴壳体在所述轴承座上的旋转轴线相交且相互垂直，在所述轴耳上设置有两个轴承孔，所述两个轴承孔关于所述轴耳在所述交叉轴壳体上的旋转轴线对称分布，所述两个轴承孔的轴线相互平行且位于同一水平面上，所述轴承孔的轴线与所述交叉轴壳体在所述轴承座上的旋转轴线相互垂直；

所述虎克铰组件安装在所述静平台上，所述关节组件安装在所述动平台上，所述电缸组件的一端安装在所述虎克铰组件上，所述电缸组件的另一端通过承力销轴转动连接在所述关节组件的所述轴耳上，所述承力销轴转动设置在所述轴耳的所述轴承孔内；

在所述静平台上安装有六个所述虎克铰组件，所述六个虎克铰组件分别为第一虎克铰组件、第二虎克铰组件、第三虎克铰组件、第四虎克铰组件、第五虎克铰组件、第六虎克铰组件，在所述动平台上设置有三个所述关节组件，所述三个关节组件分别为第一关节组件、第二关节组件、第三关节组件，所述静平台与所述动平台之间设置有六个所述电缸组件，所述六个电缸组件分别为第一电缸组件、第二电缸组件、第三电缸组件、第四电缸组件、第五电缸组件、第六电缸组件；

所述第二电缸组件、所述第三电缸组件、所述第二虎克铰组件和所述第三虎克铰组件以及所述第一关节组件形成第一控制组件，所述第四电缸组件、所述第五电缸组件、所述第四虎克铰组件、所述第五虎克铰组件以及所述第二关节组件形成第二控制组件，所述第六电缸组件、所述第一电缸组件、所述第六虎克铰组件、所述第一虎克铰组件、以及所述第三关节组件形成第三控制组件；

所述速度正解方法包括：

S10：在机器人的第一控制组件上安装五个角度编码器以读取关节角度值和关节角速度值，包括如下子步骤：

S101：在第一控制组件的第二虎克铰组件的虎克铰上基座和虎克铰下基座上均安装一个角度编码器，记为第一角度编码器和第二角度编码器，用于测量第二虎克铰组件的固定轴部和摆动轴部分别相对于虎克铰下基座和虎克铰上基座的旋转角度和旋转角速度，分别记为jθ₁、jθ₂和

S102：在第一控制组件的第三虎克铰组件的虎克铰上基座上安装一角度编码器，记为第三角度编码器，用于测量摆动轴部相对于虎克铰组件的虎克铰上基座的旋转角度和旋转角速度，记为jθ₃和

S103：在第一控制组件的关节组件内部安装一角度编码器，记为第四角度编码器，用于测量关节组件的轴耳的轴部相对于交叉轴壳体的角度值和角速度值，记为jθ₄和

S104：在动平台上与第一控制组件连接的位置处安装一角度编码器，记为第五角度编码器，用于测量动平台相对于关节组件的旋转角度值和旋转角速度值，记为jθ₅和

S20：构建并联六轴机器人的运动学元素；

B_i,i＝1～6：虎克铰组件原点，将B_i,i＝1～6点设置在虎克铰组件的十字轴中心；

{O}：基坐标系，在静平台上建立直角坐标系{O}，将坐标系的原点O设置在B₁～B₆确定的平面上，且位于B₁～B₆确定的圆的圆心位置，y轴方向设置在OB₁,OB₂线段的角平分线位置，此时六个虎克铰组件相对于y轴对称，z轴设置朝上，x轴根据右手定则即可自动确定；

R_b：表示B₁～B₆确定的圆的半径，称为“虎克铰虚拟圆半径”；

^OOB₁～^OOB₆：^OOB_i：虎克铰位置向量，具体表示以基坐标系的原点为起点，第i个虎克铰原点B_i,i＝1～6为终点的向量，参考坐标系为基坐标系；

β_i，i＝1～6：虎克铰偏置角，表示^OOB_i与基坐标系{O}的Y轴的角度；

R_Q：称为“交叉轴虚拟圆半径”，表示三个交叉轴原点确定的圆的半径值；

Q_i，i＝1～3：轴耳原点，轴耳零件上与U_i，i＝1～3对应重合的点，设为Q_i，i＝1～3；

{Q_i}，i＝1～3：表示轴耳坐标系。坐标系原点固连在轴耳原点Q_i，{Q_i}的z轴沿轴耳的轴部轴线方向，y轴沿销轴铰链两点的连线方向，x轴根据右手定则确定；

U_i：交叉轴原点，关节组件的两个圆柱面的轴线行成一个交点，设该点为U_i，i＝1～3；

{U_i}和i＝1～3：分别表示交叉轴坐标系，坐标系原点固连在交叉轴零件原点U_i，{U_i}的z轴沿轴耳的轴部轴线方向，y轴沿中心轴轴线的方向，x轴根据右手定则确定。

{P}：动平台坐标系，{P}的原点P位于U_i，i＝1～3确定的平面上，且位于三个U_i确定的圆的圆心位置。设置{P}的y轴设置为：其中U₂在y轴的负方向，U₁,U₃相对于y轴对称，z轴朝上，x轴可用右手定则确定。如图13所示。

轴耳原点的分布角度

^OOP：动平台的位置，表示动平台坐标系{P}的原点P相对于静平台坐标系原点O的位置向量；

^OR_P：动平台的姿态，表示动平台坐标系{P}相对于静平台坐标系{O}的旋转矩阵；

A₁～A₆：销轴铰链中心，在轴耳零件1(轴耳零件2，轴耳零件3)中，两个销轴铰链中心点A₂(点A₄，点A₆)和点A₃(点A₅，点A₁)点相对于{Q₁}({Q₂}，{Q₃})的Y轴对称布置，将点A₂(点A₄，点A₆)和点A₃(点A₅，点A₁)放置在{Q₁}({Q₂}，{Q₃})的XY平面上，用QAX表示A₂和A₃在{Q₁}的X轴方向的绝对偏移值，QAY表示A₂和A₃在{Q₁}的Y轴方向的绝对偏移值；

^ΟΒΑ_i：表示电缸位置向量，以基坐标系{O}为参考系；

Q₁'：表示轴耳零件的速度瞬心；

S30：从电缸组件自带的控制系统读出电缸的长度，记为l1～l6，用l1～l6表示^ΟΒΑ₁～^ΟΒΑ₆的模，设l1＝|^ΟΒΑ₁|，...，l6＝|^ΟΒΑ₆|，从电缸组件自带的控制系统读出电缸组件中的活塞杆相对于缸体的伸缩速度v_Li,i＝1～6；

S40：在第二虎克铰组件的十字轴上，以十字轴中心B₂为原点建立虎克铰坐标系{B2}，在第三虎克铰组件的十字轴上，以十字轴中心B₃为原点建立虎克铰坐标系{B3}；

S50：在第二虎克铰组件的十字轴上，建立电缸坐标系{L2}，其原点与{B2}的原点重合，在第三虎克铰组件的虎克铰上基座上，建立电缸坐标系{L3}，其原点与{B3}的原点重合；

S60：计算第二虎克铰组件的中心B₂与第三虎克铰组件的中心B₃的距离b₂₃；

S70：以{O}为参考系，求解{B2}、{B3}坐标系的位姿；

S80：以{O}为参考系，求解{L2}、{L3}坐标系的位姿；

S90：以{O}为参考系，求解A₂、A₃点的位置；

S100：求解{Q₁}相对于{O}的位姿，用^OT_Q1表示；

S110：求解{U₁}相对于{O}的位姿，用^OT_U1表示；

S120：求解{P₁}相对于{O}的位姿，用^OT_P1表示；

S130：求解动平台的位姿，即动平台的位置^OOP和动平台的位姿^OR_P；

S140：以{O}为参考坐标系，求解第二虎克铰组件和第三虎克铰组件的十字轴相对于静平台的角速度向量，记作^OW_B2,O和^OW_B3,O；

S150：以{O}为参考坐标系，求解第二电缸组件和第三电缸组件的缸体相对于静平台的角速度向量，记作^OW_L2,O和^OW_L3,O；

S160：求解销轴铰链中心A₂，A₃的线速度向量，以{O}为参考坐标系，用^OV_A2和^OV_A3表示；

S170：求解轴耳零件上的轴耳原点Q₁点的线速度^OV_Q1，以及轴耳的角速度向量^OW_Q1,O；

S180：求解第一关节组件的交叉轴的角速度，以{O}为参考系，记作^OW_U1,O；

S190：求解动平台的角速度，以{O}为参考系，即^OW_P,O；

S200：求解动平台中心的线速度，以{O}为参考系，即^OV_P。