[发明专利]一种基于曲柄连杆机构的六自由平台姿态正解方法

权利要求书

1.一种基于曲柄连杆机构的六自由平台姿态正解方法,其特征在于：包括以下步骤：

A.建立平台基座坐标系E₀，根据机械结构确定六个驱动电机或其传动机构的驱动轴与曲柄连接轴承的中心点坐标；

在平台基座坐标系E₀中，以天顶方向为z轴正方向；以六轴驱动电机或减速机旋转轴线所在的平面为z＝0的参考平面；六个曲柄连杆与电机转轴连接处的支撑点构成一个六边形，取六条轴线交点为坐标原点o；以平台正前方为y轴正方向，右手方向为x轴正方向；绕坐标轴ox轴的旋转角度定义为俯仰角θ，绕oy轴的旋转角度定义为滚动角γ，绕oz轴的旋转角度定义为偏航角

定义曲柄和电机或其传动机构的驱动轴连接轴承的中心点为支撑点A_i，其中i＝1,...,6，六个支撑点构成一个六边形A₁A₂A₃A₄A₅A₆；根据机械结构确定六个支撑点的坐标A_i(x_Ai,y_Ai,z_Ai)，其中，i＝1,...,6；

B.根据曲柄当前角位置，确定曲柄与连杆连接处的轴承中心坐标；

定义与支撑点A_i连接的曲柄LA_i处于坐标系E₀参考平面内时为初始位置,其中坐标系E₀参考平面为xoy平面，记的LA_i初始位置相量记LA_i当前时刻位置与初始位置的夹角α_i,其中i＝1～6,且以逆时针为正；

记曲柄与连杆连接处的轴承中心点D_i；联结连杆与上平台连接处的支撑轴承的中心点为B_i，其中i＝1,...,6，曲柄LA_i的旋转轴为矢量

曲柄初始位置及旋转矢量p_i在参考平面分布，其中p_i＝[x_Ai,y_Ai,z_Ai]；

根据运动学知识中，任何矢量围绕p_i旋转角度α_i的旋转矩阵表达式推导得到：曲柄LA_i旋转后的矢量的表达式为：进而可以得到D_i点的坐标(x_di,y_di,z_di)：

C.根据连杆的长度、上平台各个支撑点的几何关系，得到平台和支撑点的一组线性方程；

定义上平台各支撑轴承球心中心点为：B_i，其中i＝1,...,6，B_i在坐标系E₀中的坐标为(x_bi,y_bi,z_bi)；B_i共有六个点，这六个点构成六边形B₁B₂B₃B₄B₅B₆；

记六边形长边长度|B₁B₂|＝|B₃B₄|＝|B₅B₆|＝l_b，短边长度|B₂B₃|＝|B₄B₅|＝|B₆B₁|＝h_b，三条长边的延长线交于三点C_b1、C_b2、C_b3，这三个点在坐标系E₀中的坐标为(x_cj,y_cj,z_cj)，其中j＝1,2,3，为一个等边三角形；

三角形的边C_bjC_bk的长度为：|C_bjC_bk|＝2h_b+l_b，其中j＝1,2,3k＝1,2,3且j≠k，B_i、D_i与相邻的C_bj的可以构成一个三角形，设B_i、D_i之间的连杆为LB_i，其矢量表达式为长度为L_B，则根据B_i、D_i、C_bj的相互关系可以得到：

因为|C_bjB_i|＝h_b，设

可以得到：

进而得到关于C_b1、C_b2、C_b3三个点坐标(x_cj,y_cj,z_cj)的六个方程，其中j＝1,2,3；

再根据|C_bjC_bk|＝L_C，其中j＝1,2,3，k＝1,2,3且j≠k，可以列写三角形的边长公式，得到关于(x_cj,y_cj,z_cj)的另外三个方程，其中，j＝1,2,3；上述九个方程，构成了关于x_c1、y_c1、z_c1、x_c2、y_c2、z_c2、x_c3、y_c3、z_c3这九个未知数的方程组；

D.求解方程组得到上平台的姿态和位置；

利用求解非线性方程组的方法求解步骤C中的九个方程构成的方程组，可以得到x_c1、y_c1、z_c1、x_c2、y_c2、z_c2、x_c3、y_c3、z_c3这九个未知数；

定义与上平台固联的上平台坐标系E_b：E_b的参考平面，即Z_b＝0平面取上平台六个支撑轴承球心点B_i所在平面，其中i＝1,...,6；原点O_b取为上平台几何中心；

记六个曲柄处于水平位置，即α_i＝0，i＝1,2,...,6时上平台处于初始状态，初始状态下o_bx_b轴与平台基座坐标系E₀中ox轴平行；o_by_b轴与平台基座坐标系E₀中oy轴平行；此时，E_b坐标系原点距离E₀坐标系原点的高度为Z_b0；上平台的三个轴向相量在E₀坐标系中的表达式为：

o_bx_b＝(x_c2-x_c3,y_c2-y_c3,z_c2-z_c3)

o_bz_b＝o_bx_b×o_by_b；

定义E_b与E₀之间的姿态变换关系：E_b由E₀按3-1-2的顺序依次逆时针旋转坐标轴，即先沿oz轴转动角度再沿转动后的ox′轴转动角度θ，最后沿转动后的oy″轴转动角度γ，θ、γ为姿态角；得到三次转动的姿态余弦阵分别为：

E₀到E_b的姿态变换余弦阵为：

六自由度平台还有三个平移运动的自由度，定义的三轴平移矢量T’＝[Δx,Δy,Δz]’，初始时刻E_b与E₀坐标系原点的高度差为z_b0，所以两坐标系之间的平移矢量修正为：T＝[Δx Δy Δz+z_bo]′

平台进行六自由度运动后，两坐标系间的坐标变换阵CT为：

平台上任意一点B在E_b坐标系中的坐标(x_b,y_b,z_b)经三轴平移和三轴旋转之后，对应E₀中对应的坐标(x,y,z)为：上式中(x_bo,y_bo,z_bo)是E_b坐标系原点o_b在E₀中的表达式；

由于平移运动不影响矢量的方向，得到E_b坐标系中各坐标轴的轴向矢量在E₀坐标系中的表达式为：

将上平台的三个轴向相量在E₀坐标系中的表达式与上述三个表达式对比，可以得到三轴姿态角θ、γ的表达式：

将上平台原点o_b在E_b坐标系中的坐标(0,0,0)代入公式

可以得到其在E₀中的坐标(Δx₀，Δy₀，Δz₀+z_bo)，又因为o_b为中心，求得三轴姿态角θ、γ、以及Δx_o、Δy_o、Δz_o即平台当前的姿态和位置：