本发明属于机器人控制技术领域，特别涉及一种适用于坦克动力舱装配的六自由度并联调姿机构逆解方法。
背景技术：
随着中国军事技术的崛起，军工企业也引来了新一代技术革新。在坦克动力舱装配中，发动机与综合传动箱配合面的轴孔对接的智能化过程，一直是众多工程师们正在解决的工作重点及难点，传统的对接过程费时费力，安装的合格率较低，同时对工人的安全也可能造成一定的伤害，六自由度并联调姿机构可以提高了坦克动力舱装配过程的效率和精度。
传统的装配方法自动化程度低、生产效率低下、质量不够稳定、对不同部件适应性差。
现有文献研究的并联调姿机构主要分为3个定位器和6个定位器来进行位姿控制，还没有出现通过4定位器进行位姿并联调姿机构的相关研究。
技术实现要素：
本发明的目的是提供一种适用于坦克动力舱装配的六自由度并联调姿机构逆解方法，解决了基于4定位器的并联调姿机构逆解方法定位的技术问题。
为实现上述目的，本发明采用以下技术方案：
一种适用于坦克动力舱装配的六自由度并联调姿机构逆解方法，包括如下步骤：
步骤1：建立六自由度并联调姿平台，六自由度并联调姿平台为矩形调姿平台，其设有第一工艺球、第二工艺球、第三工艺球、第四工艺球、动平台、底部平台、第一定位器、第二定位器、第三定位器和第四定位器，第一定位器、第二定位器、第三定位器和第四定位器均设在底部平台上；四个定位器与四个球铰中小固定；
步骤2：建立六自由度并联调姿平台的数学模型，在数学模型中设定端点a1代表第一定位器，端点a2代表第二定位器，端点a3代表第三定位器，端点a4代表第四定位器，端点b1代表第一工艺球、端点b2代表第二工艺球、端点b3代表第三工艺球、端点b4代表第四工艺球，以端点a1、端点a2、端点a3和端点a4为顶点建立四边形a1a2a3a4，在底部平台设立笛卡尔坐标系o{x，y，z}，o{x，y，z}的原点位于四边形a1a2a3a4的中心点上，o{x，y，z}的x轴正方向定义为沿端点a1指向端点a2的方向，o{x，y，z}的z轴正向垂直于底座平面；
以端点b1、端点b2、端点b3和端点b4为顶点建立四边形b1b2b3b4，建立调姿系统机构坐标系p{x,y,z}，p{x,y,z}的原点位于四边形b1b2b3b4的中心点上，p{x,y,z}的x轴正向定义为沿端点b1指向端点b2的方向，p{x,y,z}的z轴正向垂直于四边形b1b2b3b4所在平面；
步骤3：通过以下公式计算出工艺球的球绞点在定位器参考点中的矢量di：
其中
步骤4：第一工艺球、第二工艺球、第三工艺球和第四工艺球相对于第一定位器、第二定位器、第三定位器和第四定位器的距离分别为d1,d2,d3,d4，其d1,d2,d3,d4为根据六自由度并联调姿平台的大小尺寸获得的已知量，端点a1～a4与b1～b4的坐标，在逆运算的实际工程中均为已知量；
在初始状态下向量ai与向量bi均可以很容易得到，对应第一定位器、第二定位器、第三定位器和第四定位器的向量a1～a4与向量b1～b4均可作为已知条件，p向量[pxpypz]为坦克动力舱装配调姿平台的位置坐标；
步骤5：根据一下公式计算各个定位器轴向驱动位移：
li,j＝diej,i＝1,2,3,4,j＝1,2,3；其中j表示定位器的第j个驱动轴，i表示第i个定位器，即第一定位器、第二定位器、第三定位器和第四定位器中的任意一个，ej为定位器各驱动的单位方向矢量，ej的取值为以下公式计算得出：
ex＝[1,0,0]t,ey＝[0,1,0]t,ez＝[0,0,1]t；其中t为矩阵的转置。
步骤6：分别计算出第一定位器、第二定位器、第三定位器和第四定位器在x,y,z轴上的运动情况：
第一定位器在x,y,z方向上的运动为l1(x,y,z)＝ro*b1+p-a1；
第二定位器在x,y,z方向上的运动为l2(x,y,z)＝ro*b2+p-a2；
第三定位器在x,y,z方向上的运动为l3(x,y,z)＝ro*b3+p-a3；
第四定位器在x,y,z方向上的运动为l4(x,y,z)＝ro*b4+p-a4；
步骤7：根据步骤2到步骤6的结论得出底部平台上的第一定位器、第二定位器、第三定位器和第四定位器分别在空间坐标系中x，y，z三个方向上的理论轴向位移。
所述位姿表示为所述动平台的位置和姿态，姿态通过围绕所述坐标系p{x,y,z}的x,y,z轴的转动所表示。
本发明所述的一种适用于坦克动力舱装配的六自由度并联调姿机构逆解方法，解决了基于4定位器的并联调姿机构逆解方法定位的技术问题，降低了生产成本，提高安全性，本发明采用4定位器的并联调姿机构逆解方法，比现有文献研究的并联调姿机构主要分为3个定位器更加准确，比6个定位器的成本更加低廉，安全性更高，本发明是利用空间向量的内在关系进行研究，大大降低了算法的复杂度，便于计算理解，在matlab上得到了验证。
附图说明
图1是六自由度并联调姿机构的结构示意图；
图2是本发明的调姿平台的数学模型；
图3是本发明的调姿平台单一支路矢量链路图；
图中：第一定位器1、第二定位器2、第三定位器3、第四定位器4、第一工艺球5、第二工艺球6、第三工艺球7、第四工艺球8、动平台9。
具体实施方式
如图1-3所示的一种适用于坦克动力舱装配的六自由度并联调姿机构逆解方法，包括如下步骤：
步骤1：建立六自由度并联调姿平台，六自由度并联调姿平台为矩形调姿平台，其设有第一工艺球5、第二工艺球6、第三工艺球7、第四工艺球8、动平台9、底部平台、第一定位器1、第二定位器2、第三定位器3和第四定位器4，第一定位器1、第二定位器2、第三定位器3和第四定位器4均设在底部平台上；第一定位器1、第二定位器2、第三定位器3和第四定位器4上均设有一个伸缩杆，第一定位器1的伸缩杆上、第二定位器2的伸缩杆上、第三定位器3的伸缩杆上和第四定位器4的伸缩杆上分别铰接有第一工艺球5、第二工艺球6、第三工艺球7和第四工艺球8，第一工艺球5、第二工艺球6、第三工艺球7和第四工艺球8还均与动平台9铰接；
如图1所示，第一工艺球5、第二工艺球6、第三工艺球7和第四工艺球8分别故设在第一定位器1、第二定位器2、第三定位器3和第四定位器4的上边，第一工艺球5、第二工艺球6、第三工艺球7和第四工艺球8均与动平台9铰接。
步骤2：建立六自由度并联调姿平台的数学模型，在数学模型中设定端点a1代表第一定位器1，端点a2代表第二定位器2，端点a3代表第三定位器3，端点a4代表第四定位器4，端点b1代表第一工艺球5、端点b2代表第二工艺球6、端点b3代表第三工艺球7、端点b4代表第四工艺球8，以端点a1、端点a2、端点a3和端点a4为顶点建立四边形a1a2a3a4，在底部平台设立笛卡尔坐标系o{x，y，z}，o{x，y，z}的原点位于四边形a1a2a3a4的中心点上，o{x，y，z}的x轴正方向定义为沿端点a1指向端点a2的方向，o{x，y，z}的z轴正向垂直于底座平面；
以端点b1、端点b2、端点b3和端点b4为顶点建立四边形b1b2b3b4，建立调姿系统机构坐标系p{x,y,z}，p{x,y,z}的原点位于四边形b1b2b3b4的中心点上，p{x,y,z}的x轴正向定义为沿端点b1指向端点b2的方向，p{x,y,z}的z轴正向垂直于四边形b1b2b3b4所在平面；
步骤3：通过以下公式计算出工艺球的球绞点在底部笛卡尔坐标系o中的矢量di：
其中
步骤4：第一工艺球5、第二工艺球6、第三工艺球7和第四工艺球8相对于第一定位器1、第二定位器2、第三定位器3和第四定位器4的距离分别为d1,d2,d3,d4，其d1,d2,d3,d4为根据六自由度并联调姿平台的大小尺寸获得的已知量，端点a1～a4与b1～b4的坐标，在逆运算的实际工程中均为已知量；
如图3所示，ai是底部坐标原点o到定位器初始位置的空间向量；bi上部坐标系原点p到球铰的空间向量，在初始状态下向量ai与向量bi均可以很容易得到，对应第一定位器1、第二定位器2、第三定位器3和第四定位器4的向量a1～a4与向量b1～b4均可作为已知条件，p向量[pxpypz]为坦克动力舱装配调姿平台的位置坐标；
步骤5：根据一下公式计算各个定位器轴向驱动位移：
li,j＝diej,i＝1,2,3,4,j＝1,2,3；其中j表示定位器的第j个驱动轴，i表示第i个定位器，即第一定位器1、第二定位器2、第三定位器3和第四定位器4中的任意一个，ej为定位器各驱动的单位方向矢量，ej的取值为以下公式计算得出：
ex＝[1,0,0]t,ey＝[0,1,0]t,ez＝[0,0,1]t；其中t为矩阵的转置。
步骤6：分别计算出第一定位器1、第二定位器2、第三定位器3和第四定位器4在x,y,z轴上的运动情况：
第一定位器1在x,y,z方向上的运动为
其中l是一个向量，表示在x轴或者y轴或者z轴上的移动距离，a1～a4和b1～b4就是ai和bi在四个支路上的矢量，p是调姿平台的位置。
第二定位器2在x,y,z方向上的运动位移为
第三定位器3在x,y,z方向上的运动位移为
第四定位器4在x,y,z方向上的运动位移为
步骤7：根据步骤2到步骤6的结论得出底部平台上的第一定位器1、第二定位器2、第三定位器3和第四定位器4分别在空间坐标系中x，y，z三个方向上的理论轴向位移。
所述位姿表示为所述动平台9的位置和姿态，姿态通过围绕所述坐标系p{x,y,z}的x,y,z轴的转动所表示。
六自由度并联调姿机构正解方法中的未知量作为已知量放进逆解表达式中，通过逆解方法求解，验证结果是否与正解方法中的已知量相等或相近，若相等或相近即为有效。
六自由度并联调姿机构正解与逆解是相对的，,在已知输入构件位置的条件下,求解出构件位置和姿态,称为并联调姿机构正解分析；反之,当已知并联调姿机构的位置和姿态,求解输入件的位置,为并联调姿机构位置反解分析,也就是逆解。根据实际情况来看，两者都需要。
本发明的定位精度如表1所示：
表1
本发明所述的一种适用于坦克动力舱装配的六自由度并联调姿机构逆解方法，解决了基于4定位器的并联调姿机构逆解方法定位的技术问题，降低了生产成本，提高安全性，本发明采用4定位器的并联调姿机构逆解方法，比现有文献研究的并联调姿机构主要分为3个定位器更加准确，比6个定位器的成本更加低廉，安全性更高，本发明是利用空间向量的内在关系进行研究，大大降低了算法的复杂度，便于计算理解，在matlab上得到了验证。