六轴机器人的反力逆解方法和六轴机器人与流程
本发明涉及工业机器人技术领域，特别是涉及一种六轴机器人的反力逆解方法和六轴机器人。
背景技术：
并联机器人相较于串联机器人具有刚度大，精度高，负载自重比较大的特点，在定位平台，仿真设备，娱乐设备等领域已有广泛的应用，其在自动化加工应用场景的潜力很大。
在目前的学术和商用领域，绝大多数六轴机器人采用6-6ups或者6-6upu构型。
6-6ups表示在静平台上设置6个虎克铰关节，在动平台上设置6个球绞关节。静平台上的每个虎克铰关节和动平台上的虎克绞关节中间均通过电缸连接，电缸的活塞杆可以伸缩。6个电缸的活塞杆伸缩可导致动平台的位置和姿态变化，共6个自由度。
6-6upu表示在静平台上设置6个虎克铰关节，在动平台上设置6个虎克铰关节。静平台上的每个虎克铰关节和动平台上的每个虎克绞关节中间均通过电缸连接，电缸的活塞杆可以伸缩。6个电缸的活塞杆伸缩可导致动平台的位置和姿态变化，共6个自由度。
为了研制新型并联机器人的控制系统，实现并联机器人的基本运动，反力逆解方法是首先需要解决的问题，反力逆解方法可用于六轴机器人开发前的运动学仿真，指导零部件的选型。当给定工作位置和负载，通过反力求解算法可计算出电缸的推力值或拉力值，检验电缸的选型是否正确。
但现有的商业运动学和动力学仿真软件为闭源软件，设计人员无法了解其运行机理。
技术实现要素：
基于此，本发明的目的在于，提供一种六轴机器人的反力逆解方法和六轴机器人，可用于该六轴机器人开发前的运动学仿真，并指导零部件的选型。
第一方面，本发明提供了一种六轴机器人的反力逆解方法，所述六轴机器人包括静平台(100)、动平台(200)、虎克铰组件(300)、关节组件(400)和电缸组件(500)；
所述关节组件(400)包括：
轴承座(420)；
交叉轴壳体(430)，所述交叉轴壳体(430)转动设置在所述轴承座(420)上；
轴耳(433)，所述轴耳(433)转动设置在所述交叉轴壳体(430)上，所述轴耳(433)在所述交叉轴壳体(430)上的旋转轴线与所述交叉轴壳体(430)在所述轴承座(420)上的旋转轴线相交且相互垂直，在所述轴耳(433)上设置有两个轴承孔(441)，所述两个轴承孔(441)关于所述轴耳(433)在所述交叉轴壳体(430)上的旋转轴线对称分布，所述两个轴承孔(441)的轴线相互平行且位于同一水平面上，所述轴承孔(441)的轴线与所述交叉轴壳体(430)在所述轴承座(420)上的旋转轴线相互垂直；
所述虎克铰组件安装在所述静平台(100)上，所述关节组件(400)安装在所述动平台(200)上，所述电缸组件(500)的一端安装在所述虎克铰组件(300)上，所述电缸组件(500)的另一端通过承力销轴(410)转动连接在所述关节组件(400)的所述轴耳(433)上，所述承力销轴(410)转动设置在所述轴耳(433)的所述轴承孔(441)内；在所述静平台(100)上安装有六个所述虎克铰组件(300)，所述六个虎克铰组件(300)分别为第一虎克铰b1、第二虎克铰、第三虎克铰、第四虎克铰、第五虎克铰、第六虎克铰，在所述动平台(200)上设置有三个所述关节组件(400)；
所述方法包括如下步骤：
s1：建立基于该六轴机器人的运动学模型，在静平台100上建立直角坐标系{o}为基坐标系，在动平台200上建立直角坐标系{p}为动平台坐标系；
s2：获取末端动平台坐标系相对于静平台基坐标系的给定位置oop和给定姿态值orp；
s3：获取虎克铰虚拟圆半径rb，虎克铰位置偏置角βi，i＝1～6，通过rb乘以绕z轴旋转角度矩阵计算公式再乘以y轴单位向量，得到虎克铰位置向量oob1～oob6；
s4：根据交叉轴虚拟圆半径rq，轴耳原点分布角度
s5：根据所述给定位置oop和所述给定姿态orp，根据所述轴耳位置向量ppqi，i＝1～3和所述虎克铰位置向量oob1～oob6，再根据机器人学理论中的坐标旋转定理和向量的基本运算定理，得到虎克铰-轴耳向量obqi,i＝1～6；
s6：根据所述给定姿态orp，odi向量分布角度，再根据机器人学理论中的坐标旋转定理，通过orp乘以绕z轴旋转角度矩阵计算公式再乘以y轴单位向量，得到交叉轴横轴向量odi，i＝1～3；
s7：根据虎克铰-轴耳向量obq3和obq2，通过向量的叉乘obq3×(-obq2)，求模||obq3×(-obq2)||，除法计算后，得到分量ozq1，根据分量ozqi，获取分量oyqi和分量oxqi，根据分量ozqi、分量oyqi和分量oxqi，得到orqi，i＝1～3；
s8：计算使用x绝对偏移值qax和y绝对偏移值qay为元素，构建以{q}坐标系为参考系的轴耳-旋转中心向量qjqai，i＝1～6,j＝1～3；
s9：根据轴耳坐标系旋转矩阵orqi，i＝1～3，轴耳-旋转中心向量qjqai，i＝1～6,j＝1～3，将orqi乘以qqai得到以基坐标系{o}为参考系的轴耳-旋转中心向量oqai，i＝1～6；
s10：根据轴耳-旋转中心向量qqai，i＝1～6和虎克铰-轴耳向量obqi,i＝1～6，再使用向量的基本运算，得到电缸位置向量obai,i＝1～6；
s11：获得交叉轴受力偏角θ1～θ3，；
s12：根据向量的点乘积原理，获得电缸-轴耳受力偏角γ1～γ6；
s13：构建成由15个方程构成的方程组及其矩阵形式fin＝a*fb；
s14：从系数矩阵a中计算包含反力fa1～fa6在内的15个未知数向量fb，从fb中提取出第7至12项，得到fa1～fa6，即每个电缸的反力值。
第二方面，本发明提供了一种六轴机器人，包括：
至少一个存储器以及至少一个处理器；
所述存储器，用于存储一个或多个程序；
当所述一个或多个程序被所述至少一个处理器执行，使得所述至少一个处理器实现如本发明第一方面所述的六轴机器人的反力逆解方法的步骤。
为了更好地理解和实施，下面结合附图详细说明本发明。
附图说明
图1为实施例所述的六轴机器人的结构示意图；
图2为实施例所述的六轴机器人的爆炸示意图；
图3为实施例所述的虎克铰组件与静平台的连接示意图；
图4为实施例所述的关节组件、电缸组件、动平台的连接示意图；
图5为实施例所述的虎克铰组件在静平台上的分布示意图；
图6为实施例所述的虎克铰组件的爆炸示意图；
图7为实施例所述的虎克铰组件的固定轴系的连接示意图；
图8为实施例所述的虎克铰组件的摆动轴系的连接示意图；
图9为实施例所述的关节组件在动平台上的分布示意图；
图10为实施例所述的关节组件的爆炸示意图；
图11为实施例所述的关节组件的组装示意图；
图12为实施例所述的交叉轴壳体的内部示意图；
图13为实施例所述的轴耳的结构示意图；
图14为实施例所述的关节组件的承力销轴轴系的连接示意图；
图15为实施例所述的电缸组件的结构示意图；
图16为虎克铰原点定义的示意图；
图17为构建基坐标系的示意图；
图18为虎克铰位置向量的示意图；
图19为构建交叉轴原点的示意图；
图20为构建动平台坐标系的示意图；
图21为交叉轴横轴向量的示意图；
图22为轴耳原点的示意图；
图23为虎克铰-轴耳向量的示意图；
图24为轴耳坐标系的示意图；
图25为轴耳原点的分布角度示意图；
图26为动平台坐标系的原点相对于静平台坐标系原点的位置向量的示意图；
图27为销轴铰链中心的示意图；
图28为反力逆解的示意图；
图29为交叉轴-动平台力矩向量和交叉轴受力偏角的示意图；
图30-32为轴耳径向分力、轴耳-交叉轴力矩向量、电缸反力值和电缸-轴耳受力偏角的示意图；
图33-34为本发明的六轴机器人的反力逆解方法的应用案例示意图；
附图标记：100、静平台；200、动平台；300、虎克铰组件；310、虎克铰下基座；311、第一圆锥滚子轴承；312、十字轴；313、第一端盖；314、第一平垫片；315、第一弹簧垫片；316、第一螺栓；320、虎克铰上座；321、第二圆锥滚子轴承；322、第二端盖；323、第二平垫片；324、第二弹簧垫片；325、第二螺栓；400、关节组件；410、承力销轴；411、孔用弹性挡圈；412、滚针轴承；413、轴用弹性挡圈；420、轴承座；421、第三圆锥滚子轴承；422、中心轴；423、第三端盖；430、交叉轴壳体；431、第四圆锥滚子轴承；432、第五圆锥滚子轴承；433、轴耳；434、锁紧螺母；435、平垫圈；436、止动垫圈；437、第一通孔；438、第二通孔；439、凸台；440、圆柱部；441、轴承孔；500、电缸组件；510、缸体；520、活塞杆；530、减速机；540、电机。
具体实施方式
针对背景技术中的技术问题，本发明提供一种六轴机器人以及其逆解方法和装置，参见图1与图2，六轴机器人包括静平台100、动平台200、六个虎克铰组件300、三个关节组件400、六个电缸组件500。其中，虎克铰组件安装在静平台100上，关节组件400安装在动平台200上，电缸组件500的一端安装在虎克铰组件300上，电缸组件500的另一端通过承力销轴410转动连接在关节组件400上。
机器人的构型可认为是由关节的布置，包括关节类型、关节数量及装配尺寸参数。本实施例所述的六轴机器人使用上述零部件按照特定的装配方式可形成一种6-3upu构型(表示支腿构型为：虎克铰关节-平动关节-交叉轴关节)。上述零部件可形成的关节类型和数量为：
(1)与静平台100固连的每个虎克铰组件300均有2个旋转关节，以实现电缸组件500的缸体510相对于静平台100有2个旋转自由度，共12个旋转自由度，旋转轴如图3中j1～j12所示；
(2)每个电缸组件500有1个平动关节，实现电缸组件500的活塞杆520相对于电缸组件500的缸体510有一个平移自由度，共6个平移自由度，移动轴如图4中的j13～j18所示；
(3)关节组件400的轴耳433底部上有2个单轴旋转关节，以实现每个轴耳433相对于连接的2个活塞杆520各有1个旋转自由度，共6个旋转自由度，旋转轴为图4中的j19～j24；
(4)每个关节组件400有2个旋转关节，一个旋转关节是可实现关节组件400的轴耳433相对于关节组件400交叉轴壳体430的单轴旋转，共3个自由度，旋转轴如图4中的j25～j27所示；另一个旋转关节是可实现关节组件400的交叉轴壳体430相对于动平台200单轴旋转，共3个自由度，旋转轴为图4中的j28～j30所示。
参见图5，每个虎克铰组件300均通过螺栓固定安装在静平台100上。在组装过程中，六个虎克铰组件300需要按照图5的分布来安装在静平台100上。下面为了清楚地解释六个虎克铰组件300在静平台100上的分布，将六个虎克铰组件300分别定义为第一虎克铰、第二虎克铰、第三虎克铰、第四虎克铰、第五虎克铰、第六虎克铰。具体地，定义虎克铰组件300的十字轴312的两旋转轴的交点为虎克铰旋转中心，在本实施例中，第一虎克铰的虎克铰旋转中心为b1，第二虎克铰的虎克铰旋转中心为b2，第三虎克铰的虎克铰旋转中心为b3，第四虎克铰的虎克铰旋转中心为b4，第五虎克铰的虎克铰旋转中心为b5，第六虎克铰的虎克铰旋转中心为b6。虎克铰旋转中心b1～b6布置在一个圆心为o、半径为rb的虚拟圆上。从虚拟圆的圆心o上延伸出三条射线，分别是射线y1、射线y2、射线y3。射线y1与射线y2的夹角、射线y1与射线y3的夹角、射线y2与射线y3的夹角均相等，均为120度。虎克铰旋转中心b1与虎克铰旋转中心b2关于射线y1对称。虎克铰旋转中心b3与虎克铰旋转中心b4关于射线y2对称。虎克铰旋转中心b5与虎克铰旋转中心b6关于射线y3对称。第一虎克铰的十字轴312的固定轴部的轴线与射线y1成30度夹角。第二虎克铰的十字轴312的固定轴部的轴线与射线y1成30度夹角。第三虎克铰的十字轴312的固定轴部的轴线与射线y1成30度夹角。第四虎克铰的十字轴312的固定轴部的轴线与射线y1成90度夹角。第五虎克铰的十字轴312的固定轴部的轴线与射线y1成90度夹角。第六虎克铰的十字轴312的固定轴部的轴线与射线y1成30度夹角。
六个虎克铰组件300的结构完全相同，下面具体描述其中一个虎克铰组件300的具体结构，参见图6至图8，虎克铰组件300由固定轴系和摆动轴系组成。固定轴系是实现虎克铰组件300的十字轴312相对于虎克铰组件300的虎克铰下基座310的相对转动，以十字轴312的固定轴部的轴线为旋转轴。摆动轴系是实现虎克铰组件300的虎克铰上座320相对于虎克铰组件300的十字轴312的摆动轴部的相对转动，以十字轴312的摆动轴部的轴线为旋转轴。
参见图6与图7，固定轴系包括虎克铰下基座310、第一圆锥滚子轴承311、十字轴312、第一端盖313、第一平垫片314、第一弹簧垫片315、第一螺栓316。其中，虎克铰下基座310通过螺栓固定安装在静平台100上。在虎克铰下基座310上安装两个第一圆锥滚子轴承311，两个第一圆锥滚子轴承311同轴设置。第一圆锥滚子轴承311的外圈与虎克铰下基座310紧配合，第一圆锥滚子轴承311的外圈与虎克铰下基座310不能相对转动。十字轴312的固定轴部的两端分别安装在两个第一圆锥滚子轴承311内，且十字轴312的固定轴部与第一圆锥滚子轴承311的内圈紧配合，十字轴312的固定轴部与第一圆锥滚子轴承311的内圈不能相对转动。在第一圆锥滚子轴承311远离虎克铰下基座310的一侧依次设置有第一端盖313、第一平垫片314、第一弹簧垫片315、第一螺栓316，第一螺栓316依次穿过第一弹簧垫片315、第一垫片、第一端盖313后与十字轴312的固定轴部螺纹连接，并且第一螺栓316的头部抵压第一弹簧垫片315，第一端盖313相抵于第一圆锥滚子轴承311的内圈的端面。
参见图6与图8，摆动轴系包括虎克铰上座320、第二圆锥滚子轴承321、第二端盖322、第二平垫片323、第二弹簧垫片324、第二螺栓325。虎克铰上座320用于安装电缸组件500。在虎克铰上座320上安装有两个第二圆锥滚子轴承321，两个第二圆锥滚子轴承321同轴设置。第二圆锥滚子轴承321的外圈与虎克铰上座320紧配合，第二圆锥滚子轴承321的外圈与虎克铰上座320不能相对转动。第二圆锥滚子轴承321的内圈与十字轴312的摆动轴部紧配合，十字轴312的摆动轴部与第二圆锥滚子轴承321的内圈不能相对转动。在第二圆锥滚子轴承321远离虎克铰上座320的一侧依次设置有第二端盖322、第二平垫片323、第二弹簧垫片324、第二螺栓325，第二螺栓325依次穿过第二弹簧垫片324、第二垫片、第二端盖322后与十字轴312的摆动轴部螺纹连接，并且第而螺栓的头部抵压第二弹簧垫片324，第二端盖322相抵于第二圆锥滚子轴承321的内圈的端面。
参见图9，每个关节组件400均通过螺栓固定安装在动平台200上。在组装过程中，三个关节组件400需要按照图9的分布来安装在动平台200上。下面为了清楚地解释三个关节组件400在动平台200上的分布，将三个关节组件400分别定义为第一关节、第二关节、第三虎关节。定义关节组件400的中心轴422的轴线与关节组件400的轴耳433的圆柱部440的轴线的交点为关节中心，在本实施例中，第一关节的关节中心为q1，第二关节的关节中心为q2，第三关节的关节中心为q3。关节中心q1～q3布置在一个圆心为p、半径为rq的虚拟圆上，其中，rq要小于rb。三个关节组件400等间隔地分布在圆心为p的虚拟圆上，也就是说，连线pq1与连线pq2成120度夹角，连线pq1与连线pq3成120度夹角，连线pq2与连线pq3成120度夹角。并且，第一关节的中心轴422的轴线与连线pq1相互垂直，第二关节的中心轴422的轴线与连线pq2相互垂直，第三关节的中心轴422的轴线与连线pq3相互垂直。
三个关节组件400的结构完全相同，下面具体描述其中一个关节组件400的具体结构，参见图10至图14，关节组件400由水平轴系、垂直轴系、承力销轴410轴系组成。水平轴系是实现关节组件400的交叉轴壳体430相对于关节组件400的轴承座420的转动，垂直轴系是实现关节组件400的轴耳433相对于关节组件400的交叉轴壳体430的转动。承力销轴410轴系是实现关节组件400的轴耳433相对于电缸组件500的活塞杆520的转动。
参见图10与图11，水平轴系包括轴承座420、第三圆锥滚子轴承421、中心轴422、第三端盖423。其中，轴承座420通过螺栓固定安装在动平台200上。在轴承座420上安装有两个第三圆锥滚子轴承421，两个第三圆锥滚子轴承421同轴设置。第三圆锥滚子轴承421的外圈与轴承座420紧配合，第三圆锥滚子轴承421的外圈与轴承座420不能相对转动。中心轴422的两端分别安装在两个第三圆锥滚子轴承421内，且中心轴422与第三圆锥滚子轴承421的内圈紧配合，中心轴422与第三圆锥滚子轴承421的内圈不能相对转动。在中心轴422的两端处分别设置有一个第三端盖423，第三端盖423螺栓连接在轴承座420上，第三端盖423将第三圆锥滚子轴承421限定在轴承座420内。
参见图10至图13，垂直轴系包括交叉轴壳体430、第四圆锥滚子轴承431、第五圆锥滚子轴承432、轴耳433、锁紧螺母434、平垫圈435、止动垫圈436。交叉轴壳体430具有第一通孔437、第二通孔438，第一通孔437的轴线与第二通孔438的轴线相交且相互垂直。第一通孔437与中心轴422紧配合，交叉轴壳体430与中心轴422同步运动。在第二通孔438内设置有凸台439，凸台439从第二通孔438的内壁向第二通孔438的内腔延伸。在第二通孔438内安装有第四圆锥滚子轴承431、第五圆锥滚子轴承432，第四圆锥滚子轴承431、第五圆锥滚子轴承432分别位于凸台439的两侧，第四圆锥滚子轴承431与第五圆锥滚子轴承432同轴设置。第四圆锥滚子轴承431的外圈与第二通孔438的内壁紧配合，第四圆锥滚子轴承431的外圈与交叉轴壳体430不能相对转动，并且第四圆锥滚子轴承431的外圈的端面相抵于凸台439的端面。第五圆锥滚子轴承432的外圈与第二通孔438的内壁紧配合，第五圆锥滚子轴承432的外圈与交叉轴壳体430不能相对转动，并且第五圆锥滚子轴承432的外圈的端面相抵于凸台439的另一端面。在轴耳433上设置有圆柱部440，该圆柱部440依次穿过第五圆锥滚子轴承432、第四圆锥滚子轴承431。在圆柱部440的端部螺纹连接有锁紧螺母434，锁紧螺母434抵压第四圆锥滚子轴承431的内圈的端面。在轴耳433与第五圆锥滚子轴承432之间设置有平垫圈435，该平垫圈435相抵于第五圆锥滚子轴承432的内圈，以减少轴耳433与第五圆锥滚子轴承432的接触面积。在锁紧螺母434与第四圆锥滚子轴承431之间设置有止动垫圈436，以防止锁紧螺母434松动。另外，在轴耳433上还设置有两个轴承孔441，两个轴承孔441关于圆柱部440的轴线对称分布，两个轴承孔441的轴线相互平行且位于同一水平面上，轴承孔441的轴线与中心轴422的轴线相互垂直。
参见图10、图11、图14，承力销轴410轴系包括承力销轴410、孔用弹性挡圈411、滚针轴承412、轴用弹性挡圈413。其中，孔用弹性挡圈411安装在轴耳433的轴承孔441内。滚针轴承412安装在轴耳433的轴承孔441中，滚针轴承412的外圈与轴承孔441紧配合，滚针轴承412的外圈与轴耳433不能相对转动。滚针轴承412的外圈相抵于孔用弹性挡圈411，通过孔用弹性挡圈411来限定滚针轴承412的位置。承力销轴410安装在滚针轴承412内，承力销轴410与滚针轴承412的内圈紧配合，承力销轴410与滚针轴承412的内圈不能相对转动。承力销轴410还穿设在电缸组件500的活塞杆520上。在承力销轴410的两端均开设有挡圈槽，在每个挡圈槽内安装一个轴用弹性挡圈413，两个轴用弹性挡圈413分别位于电缸组件500的活塞杆520的两侧，并且轴用弹性挡圈413相抵于活塞杆520，以防止承力销轴410在轴耳433或电杆组件的活塞杆520上轴向窜动。
参见图15，电缸组件500包括缸体510、活塞杆520、减速机530、电机540。由于电缸组件500采用现有技术，所以在此不再论述电缸组件500的具体结构以及缸体510、活塞杆520、减速机530、电机540之间的连接关系、位置关系。但是，需要注意的是，在组装过程中，活塞杆520端部的叉架的销轴孔的轴线需要与虎克铰组件300的十字轴312的摆动轴部的轴线相互平行。
相对于现有技术，本发明实施例所述的六轴机器人具有如下有益效果：
1、通过伺服驱动器驱动电缸组件500的电机540旋转，电缸里的丝杆机构，可将电机540的旋转运动转化为活塞杆520的平行移动。整机仅有这6个可主动运动的平动关节，对这6个关节施加运动可导致动平台200产生6个自由度的运动，包括沿x轴、y轴、z轴的平移和绕x轴、绕y轴、绕z轴的转动；
2、关节组件400，可以增大动平台200的工作空间范围，例如位置范围、姿态翻转范围；
3、本实施例所述的关节组件400是由常规的传动零件和机加件装配而成，降低制造成本；
4、本实施例所述的关节组件400为模块化结构，降低六轴机器人的装配复杂度；
5、采用本实施例所述的关节组件400，可以让六轴机器人吊挂使用。
针对上述实施例中的六轴机器人，本发明还提供了一种六轴机器人的反力逆解方法，该逆解方法主要包括几何元素及运动学元素构建、位置逆解和速度逆解的步骤。
下面分别介绍上述步骤：
一、几何元素及运动学元素构建的步骤，该步骤用于定义本发明的六轴机器人的反力逆解方法中所需要用到的几何元素以及构建运动学元素：
建立虎克铰原点bi,i＝1～6。如图16所示，将虎克铰原点bi,i＝1～6点分别设置在六个虎克铰组件300的十字轴中心。
建立基坐标系{o}：在静平台100上建立直角坐标系{o}，将直角坐标系的原点o设置在虎克铰原点b1～b6确定的平面上，且位于b1～b6确定的圆的圆心位置，y轴方向设置在ob1,ob2线段的角平分线位置，此时六个虎克铰组件300相对于y轴对称，z轴设置朝上，x轴根据右手定则即可自动确定，最终的直角坐标系{o}如图17所示。
rb：表示b1～b6确定的圆的半径，如图17所示，称为“虎克铰虚拟圆半径”。
oob1～oob6：oobi：虎克铰位置向量，如图18所示，具体表示以基坐标系的原点o为起点，第i个虎克铰原点bi,i＝1～6为终点的向量，参考坐标系为基坐标系{o}。
βi，i＝1～6：虎克铰偏置角，如图18所示，表示虎克铰位置向量oobi与基坐标系{o}的y轴的角度。
ui：交叉轴原点，如图19所示，关节组件400的两个圆柱面的轴线形成一个交点，设该点为ui，i＝1～3。
{ui}和orui，i＝1～3：分别表示“交叉轴坐标系”和“交叉轴坐标系{ui}相对于基坐标系{o}的旋转矩阵”。建立方法如下速度逆解方法中的“步骤2.1所示”。
rq：称为“交叉轴虚拟圆半径”，如图20所示，表示三个交叉轴原点确定的圆的半径值。
{p}：动平台坐标系。如图20所示，在动平台200上建立直角坐标系{p}，{p}的原点p位于ui，i＝1～3确定的平面上，且位于三个ui确定的圆的圆心位置，直角坐标系{p}的y轴如图所示，其中u2在y轴的负方向，u1,u3相对于y轴对称，z轴朝上，x轴可用右手定则确定。
odi，i＝1～3：称作交叉轴横轴向量，如图21所示，以基坐标系{o}为参考系。
δi，i＝1～3：odi向量分布角度，如图21所示，表示交叉轴横轴向量odi与动平台坐标系{p}的y轴的角度，
qi，i＝1～3：轴耳原点。如图21和图22所示，为轴耳433上与ui，i＝1～3对应重合的点。
obqi,i＝1～6：虎克铰-轴耳向量，如图23所示，以基坐标系{o}为参考系。
{qi}，i＝1～3和orqi，i＝1～3：分别表示“轴耳坐标系”及“轴耳坐标系相对于基坐标系{o}的旋转矩阵”。坐标系原点固连在轴耳原点qi，{qi}的xyz三个轴如图24所示，具体求解方法按“逆位置算法”中的步骤5。
ppqi：如图25所示，表示“以动平台坐标系{p}为参考系的轴耳原点在动平台200上分布的位置向量”。
oop：给定位置。如图26所示，表示动平台坐标系{p}的原点p相对于静平台坐标系原点o的位置向量，此项为已知项。
orp：给定姿态。如图26所示，表示动平台坐标系{p}相对于静平台坐标系{o}的旋转矩阵，此项为已知项。
a1～a6：销轴铰链中心。如图26所示和图27所示，在三个轴耳433中，两个承力销轴410的铰链中心点a2(点a4，点a6)和点a3(点a5，点a1)点相对于{q1}({q2}，{q3})的y轴对称布置，将点a2(点a4，点a6)和点a3(点a5，点a1)放置在{q1}({q2}，{q3})的xy平面上，用qax表示a1和a2在{q1}的x轴方向的绝对偏移值，称作“x绝对偏移值”，用qay表示a1和a2在{q1}的y轴方向的绝对偏移值，称作“y绝对偏移值”。
f＝[fxfyfz]：外部负载力向量，如图29所示，表示动平台坐标系{p}原点受到的外部负载力矢量，其中，fx，fy，fzf分别表示动平台坐标系{p}原点受到的外部负载力矢量在动平台坐标系{p}的x轴，y轴和z轴的分量。
n＝[nxnynz]：外部负载力矩向量，如图29所示，表示动平台200受到的外部负载力矩矢量，其中nx，ny，nz表示动平台受到的外部负载力矩矢量在{p}的x轴，y轴和z轴的分量。
fz1，fz2，fz3：交叉轴径向分力，表示动平台200分别在轴耳原点q1，q2，q3受到三个关节组件400所施加的一个分力，方向沿{qi}坐标系的y轴。
fz1'，fz2'，fz3'：轴耳径向分力，如图30-32所示，表示三个轴耳433受到的三个关节组件400所施加的一个分力。
t1，t2，t3：交叉轴-动平台力矩向量，如图29所示，表示动平台200分别在轴耳原点q1，q2，q3点受到三个关节组件400所施加的合力矩。
t1'，t2'，t3'：轴耳-交叉轴力矩向量，如图30-32所示，表示三个轴耳433受到的三个关节组件400所施加的一个力矩。
fa1～fa6:电缸反力值，如图30-32所示，表示电缸对轴耳的合力值。
γ1～γ6：电缸-轴耳受力偏角，如图30-32所示，表示力fai与一个轴耳433上的两个承力销轴410的旋转中心点连线的夹角。
θ1～θ3：交叉轴受力偏角，如图29所示，表示fzi与ppqi的夹角。
fin：总负载向量。具体表达式为：
fin＝[-fx-fy-fz-nx-ny-nz000000000]t
fb：内部力向量。具体表达式为：
fb＝[f1f2f3fz1fz2fz3fa1fa2fa3fa4fa5fa6t1t2t3]t。
a：系数矩阵。
二、位置逆解算法的流程
步骤1：计算向量oob1～oob6。根据“虎克铰虚拟圆半径”rb，虎克铰位置偏置角βi，i＝1～6，再根据机器人学理论中的坐标旋转定理，通过“rb”乘以“绕z轴旋转角度矩阵计算公式”再乘以“y轴单位向量”，得到虎克铰位置向量oob1～oob6。
步骤2：计算向量ppqi，i＝1～3。根据“交叉轴虚拟圆半径”rq，轴耳原点分布角度
步骤3：计算虎克铰-轴耳向量obqi,i＝1～6。根据已知量“给定位置”oop，已知量“给定姿态”orp，步骤2得到的轴耳位置位置向量ppqi，i＝1～3，上述得到的虎克铰位置向量oob1～oob6，再根据机器人学理论中的坐标旋转定理和向量的基本运算定理，通过以下公式，得到虎克铰-轴耳向量obqi,i＝1～6。
obq1＝oop+orp*ppq3-oob1
obq2＝oop+orp*ppq1-oob2
obq3＝oop+orp*ppq1-oob3
obq4＝oop+orp*ppq2-oob4
obq5＝oop+orp*ppq2-oob5
obq6＝oop+orp*ppq3-oob6
步骤4：计算交叉轴横轴向量odi，i＝1～3，按以下方式得到：根据已知量“给定姿态”orp，odi向量分布角度，再根据机器人学理论中的坐标旋转定理，通过“orp”乘以“绕z轴旋转角度矩阵计算公式”再乘以“y轴单位向量”，得到交叉轴横轴向量odi，i＝1～3。
步骤5：计算orqi，i＝1～3。根据机器人学的运动学理论基础，可表示为orqi＝[oxqioyqiozqi]，oxqi，oyqi，ozqi为直角坐标系的分轴，且符合右手定则，故需要分别求取的是：[oxq1oyq1ozq1]，[oxq2oyq2ozq2]，[oxq3oyq3ozq3]，共9项因子。根据虎克铰-轴耳向量obq3和obq2，通过向量的叉乘obq3×(-obq2)，求模||obq3×(-obq2)||，除法计算后，可得到分量ozq1。根据上一步得到的ozq1和上述得到的交叉轴横轴向量od1，通过向量的叉乘ozq1×od1，求模||ozq1×od1||，除法计算后，可得到分量oyq1。根据上述得到的z分量和y分量，根据右手定则，可得到分量oxq1。oxq2oxq3oyq2oyq3ozq2ozq3同理如下所示：
步骤6：计算qjqai，i＝1～6,j＝1～3，使用x绝对偏移值qax和y绝对偏移值qay为元素，构建“以{q}坐标系为参考系的轴耳-旋转中心向量”qjqai，i＝1～6,j＝1～3：
q3qa1＝q1qa3＝q2qa5＝[-qax-qay0]t
q1qa2＝q2qa4＝q3qa6＝[qaxqay0]t
步骤7：计算oqai，i＝1～6。使用上述得到的轴耳坐标系旋转矩阵orqi，i＝1～3，上述得到的轴耳-旋转中心向量qjqai，i＝1～6,j＝1～3，根据机器人学运动学理论中的坐标转换定理，将orqi乘以qjqai可计算得到“以基坐标系{o}为参考系的轴耳-旋转中心向量”oqai，i＝1～6。
oqa1＝orq3*q3qa1
oqa2＝orq1*q1qa2
oqa3＝orq1*q1qa3
oqa4＝orq2*q2qa4
oqa5＝orq2*q2qa5
oqa6＝orq3*q3qa6
步骤8：求解obai,i＝1～6，使用上述得到的以基坐标系{o}为参考系的轴耳-旋转中心向量qjqai，i＝1～6,j＝1～3和上述得到的虎克铰-轴耳向量obqi,i＝1～6，再使用向量的基本运算，可用以下公式得到电缸位置向量，即为位置逆解：
obai＝obqi+oqai
其中i＝1～6。
三、反力逆解算法的流程
步骤1：求解θ1～θ3。使用上述求得的轴耳坐标系相对于基坐标系{o}的旋转矩阵的y分量oyqi，已知量给定姿态orp，上述求得的以动平台坐标系{p}为参考系的轴耳原点在动平台上分布的位置向量ppqi，根据向量的点乘积原理，获得交叉轴受力偏角θ1～θ3。
步骤2：求解γ1～γ6。使用上述求得的轴耳坐标系相对于基坐标系{o}的旋转矩阵的x分量oxqi，上述求得的电缸位置向量οβαi，根据向量的点乘积原理，获得电缸-轴耳受力偏角γ1～γ6。
步骤3：构建系数矩阵a。根据动平台200和三个轴耳433的空间力系的平衡方程，再根据线性方程组的整理方式，可等价地构建成由15个方程构成的方程组及其矩阵形式fin＝a*fb。其中fin为总负载向量，fb为内部力向量，a为系数矩阵，系数矩阵a是一个15*15的矩阵，具体值为如下所示。式中，a(i,j:k)表示a的第i行，第j至k列。
a(1,7:15)＝[000000000]
a(2,7:15)＝[000000000]
α(3,1:6)＝[000sinθ1sinθ2sinθ3]
a(3,7:15)＝[000000000]
a(6,1:6)＝[-rqrqrq000]
a(6,7:15)＝[000000cosθ1cosθ2cosθ3]
a(7,1:6)＝[100000]
a(7,7:15)＝[0cosγ2cosγ3000000]
a(8,1:6)＝[000-100]
a(8,7:15)＝[0sinγ2sinγ3000000]
a(9,1:6)＝[000000]
a(9,7:15)＝[0cosγ2·(qay)-sinγ2·(qax)cosγ3·(qay)+sinγ3·(qax)000100]
a(10,1:6)＝[0-10000]
a(10,7:15)＝[000cosγ4cosγ50000]
a(11,1:6)＝[0000-10]
a(11,7:15)＝[000sinγ4sinγ50000]
a(12,1:6)＝[000000]
a(12,7:15)＝[000cosγ4·(qay)-sinγ4·(qax)cosγ5·(qay)+sinγ5·(qax)0010]
a(13,1:6)＝[00-1000]
a(13,7:15)＝[cosγ10000cosγ6000]
a(14,1:6)＝[00000-1]
a(14,7:15)＝[sinγ10000sinγ6000]
a(15,1:6)＝[000000]
a(15,7:15)＝[(qay)·cosγ1+sinγ1·(qax)0000(qay)·cosγ6-sinγ6·(qax)001]
步骤4：计算包含反力fa1～fa6在内15个未知数向量fb。按线性代数理论中的求逆矩阵方法下式计算。
fb＝a-1*fin
从fb中提取出fa1～fa6即第7至12项，即为电缸反力值。
相对于现有技术，本发明实施例所述的六轴机器人的反力逆解方法可用于该六轴机器人开发前的运动学仿真，并指导零部件的选型。例如，当给定工作位置和负载，通过反力逆解方法可计算出电缸的推力值或拉力值，检验电缸的选型是否正确，反力逆解方法计算出的关键承力位置的受力大小和方向，直接决定了机械或传动零件(轴承等)的型号规格。
本发明还给出了根据上述六轴机器人的反力逆解方法的应用案例，如图33所示，六轴机器人可进行机加工操作，例如磨削，铣削。如图33所示，力传感器安装到六轴机器人的动平台上，主轴安装到力传感器上，主轴上安装有刀具(砂轮或铣刀)。当工作时，刀具按一定的轨迹对工件进行机加工，工件会对刀具有一个反作用力，最终通过力传感器传递到动平台，力传感器可以读取到所受到的负载力大小f和负载力矩大小n。
将本发明中的反力逆解方法内置到并联机器人控制系统软件中，将力f和负载力矩n输入到控制系统中，经本发明中的反力逆解方法，可得到各个电缸的受力大小，将其作为前馈量输入到电机伺服中，实现并联机器人的前馈控制，达到加工过程中减小振动的效果。
参照图34，上述应用包括如下步骤：
通过设置在动平台(200)上的力传感器，获取动平台上的工件受到加工刀具的负载力大小f和负载力矩大小n；
调用上述的反力逆解方法，得到六轴机器人每个电缸的受力大小；
将所述每个电缸的受力大小作为前馈量输出至电机伺服系统，使得电机伺服系统根据每个电缸的受力大小控制电缸组件运动。
本发明还提供了一种六轴机器人，包括：
至少一个存储器以及至少一个处理器；
所述存储器，用于存储一个或多个程序；
当所述一个或多个程序被所述至少一个处理器执行，使得所述至少一个处理器实现如本发明上述的六轴机器人的反力逆解方法的步骤。
以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。
- 还没有人留言评论。精彩留言会获得点赞！