CN104848818A - Stewart平台姿态测量装置及测量方法 - Google Patents

Abstract

本发明公开了一种Stewart平台姿态测量装置，动平台坐标系的三个坐标轴轴向分别为X、Y、Z，在Stewart平台的动平台上通过四个垫块安装有四个三轴向加速度传感器，其中两个三轴向加速度传感器以动平台坐标系的坐标原点O_b对称并分别位于X轴的两侧，另外两个三轴向加速度传感器以X轴为对称轴对称安装。本发明还公开了一种Stewart平台姿态测量装置采用的测量方法，通过公式求得动平台的坐标原点O_b相对于静平台的坐标原点O_I的加速度分量和动平台旋转角速度，即可获得动平台坐标系相对于静平台坐标系的位置和姿态参数。通过本发明无需复杂计算就可以直接获得Stewart平台的形心坐标、旋转角度和旋转方向参数。

Description

Stewart平台姿态测量装置及测量方法

技术领域

本发明涉及一种Stewart平台姿态测量装置及测量方法，尤其涉及一种采用全加速度传感器实现六自由度Stewart平台形心位置、旋转角度和旋转方向的姿态参数测量的测量装置及测量方法，属于运动模拟器姿态测量领域。

背景技术

Stewart平台又称为并联平台机构，它是由动平台、静平台、铰链和六个驱动杆组成，可以实现6个自由度的转动。由于这种机构具有结构简单、动态性能好、刚度大、承载能力强等优点，在机器人、机床制造行业、汽车运动模拟器、航天运载工具模拟器、空间对接机构、潜艇救援对接器及高速铁路无砟轨道板等方面具有极高的应用价值。

Stewart平台的位置姿态控制精度是衡量其工作质量优劣和性能高低的主要指标，成为并联驱动平台应用研究中的关键技术之一，平台控制算法的核心内容是位置正解和位置反解，即已知动平台位姿求解驱动杆长(反解)，或已知各驱动杆长求解动平台位姿(正解)。测量出动平台位置姿态六参数(x,y,z,α,β,γ)^T，即可求解反解；相反已知杆长变化可正解出位姿6参数。一般来说正解多采用数值分析法，比如牛顿—辛卜森法迭代求解计算量大、速度慢、不能闭环控制，相比之下反解速度快可实现实时控制。

目前对平台位置和姿态的精确控制常用方法主要有两种。一是机电法，通过测量电机的角位移或油缸的伸缩线位移进行反馈控制，该方案的优点是成本低，便于实现，缺点是半闭环控制，精度低；二是光电法，通过单个或两个摄像头获取三维定位信息，其优点是精度高，但是造价高，可定位范围小，对应用现场也有较高要求。这两种方法都具有较强的局限性，由于位置和姿态的精确控制是并联平台机构得以实现其应用价值的必要前提，因而对其进行高精度的动态位姿测量就具有十分重要的意义，基于实时控制的强烈需求，本发明提出一种基于无陀螺捷联惯导系统测量方法，采用4只三轴加速度传感器直接测量Stewart平台位姿参数的新方法。

无陀螺捷联惯导是指惯性测量组合中不使用陀螺仪，而利用线加速度计得到惯性测量的全部参数，适用于战术导弹、智能炮弹中等利用加速度计代替陀螺实现制导的领域。此理念国外早在1965年就提出，但此后近20年时间里一直停顿，主要原因是陀螺技术的飞速发展。进入20世纪90年代，随着计算机技术以及MEMS水平的提高，中国国内外对采用全加速度计制作惯性测量的研究方案越来越重视，使其重获新生。1982年Shmuel J.Merhav提出采用旋转或振动加速度计组成无陀螺惯性测量组件，提出从加速度计的输出信号中分离线加速度和角速度的方法；1991年Algrain断言最少需要六个加速度计可测量物体的线加速度和角加速度；1999年Lee求出了利用6个加速度计测量物体旋转运动的解法，并将卡尔曼滤波应用在导航系统中；2000年Kirill详细研究了基于六加速度计配置的算法。国内的研究刚刚起步，主要进行此方向研究的有哈尔滨工程大学、哈尔滨工业大学、北京理工大学等少数高校，但目前研究内容主要涉及到不同应用领域加速度计配置方案的研究，及相应角速度的优化算法等，还没有具体应用方面的报道。通过调研，国内外学者在无陀螺捷联惯导加速度计方案配置方面，主要采用的是六加速度配置方案，细分又有好几种，但都是由加速度计输出的比力中解出载体角加速度，再经积分得到角速度。主要缺点有些是由于角速度项由平方根计算得到，无法确定角速度方向；或者计算量大，且角速度的误差随时间累积，或者对安装的精度要求太高，实际应用困难，由于以上原因六加速度计方式不是很理想的方式。九加速度计配置方案是一种改进方式，利用加速度计输出的冗余信息改进角速度解算算法从而提高精度，通过对角加速度积分确定角速度符号消除角速度符号的不确定性，对角速度平方项开方确定数值抑制迭代误差，但具体算法为见公布，且从文献上看传感器的安装位置配置不适合应用于Stewart这类动平台上。

发明内容

本发明的目的就在于为了解决上述问题而提供一种采用全加速度传感器实现Stewart平台姿态测量装置及测量方法。

本发明通过以下技术方案来实现上述目的：

一种Stewart平台姿态测量装置，所述Stewart平台包括动平台和静平台，所述动平台和所述静平台之间通过六个球铰分别与六根可伸缩的连杆相连；设动平台坐标系{O_b}的三个坐标轴轴向分别为X、Y、Z，其坐标原点O_b是所述动平台的6个球铰的机械绞点组成的圆周的圆心，设静平台坐标系{O_I}的坐标原点O_I是所述静平台的6个球铰的机械绞点组成的圆周的圆心；在所述动平台上安装有四个垫块，在四个所述垫块上一一对应地安装有四个三轴向加速度传感器，其中，所述第一三轴向加速度传感器和所述第四三轴向加速度传感器分别安装于对应的所述垫块的上表面，所述第二三轴向加速度传感器和所述第三三轴向加速度传感器分别安装于对应的所述垫块的侧表面；四个所述三轴向加速度传感器的三个敏感轴方向均为x、y、z，其中，第一三轴向加速度传感器与第四三轴向加速度传感器以坐标原点O_b对称并分别位于X轴的两侧，第二三轴向加速度传感器与第三三轴向加速度传感器以X轴为对称轴对称安装，所述第一三轴向加速度传感器的x、y、z轴分别与所述动平台的X、Y、Z轴平行，所述第一三轴向加速度传感器、所述第二三轴向加速度传感器、所述第三三轴向加速度传感器、所述第四三轴向加速度传感器的包含安装方向的坐标分别为：(x₁、-y₁、z₁)、(-x₂、y₂、z₂)、(-x₂、-y₂、z₂)、(-x₁、y₁、z₁)。

具体地，所述三轴向加速度传感器为灵敏度为100mv/g、量程为50g、精度为0.0001g、频响范围为0.5-4.5KHz的低频三轴向加速度传感器。

一种Stewart平台姿态测量装置采用的测量方法，包括以下步骤：

(1)将四个所述三轴向加速度传感器的输出简化成(b₁，b₂……b₁₂)，其中(b₁、b₂、b₃)是第一三轴向加速度传感器的x、y、z三个方向输出值，(b₄、b₅、b₆)是第二三轴向加速度传感器的x、y、z三个方向输出值，(b₇、b₈、b₉)是第三三轴向加速度传感器的x、y、z三个方向输出值，(b₁₀、b₁₁、b₁₂)是第四三轴向加速度传感器的x、y、z三个方向输出值；

(2)根据下面公式(1)推导求解：

$\left\{\begin{array}{c}{a}_1+z_1{a}_5+y_1{a}_6-x_1(a_8+a_9)-y_1{a}_{10}+z_1{a}_{11}=b_1\\ a_2-z_1{a}_4+x_1{a}_6+y_1(a_7+a_9)+x_1{a}_{10}+z_1{a}_{12}=b_2\\ a_3-y_1{a}_4-x_1{a}_5-z_1(a_7+a_8)+x_1{a}_{11}-y_1{a}_{12}=b_3\\ -a_3-y_2{a}_4-x_2{a}_5+z_2(a_7+a_8)+x_2{a}_{11}-y_1{a}_{12}=b_4\\ -a_1-z_2{a}_5+y_2{a}_6-x_2(a_8+a_9)-y_2{a}_{10}-z_2{a}_{11}=b_5\\ a_2-z_2{a}_4-x_2{a}_6-y_2(a_7+a_9)-x_2{a}_{10}+z_2{a}_{12}=b_6\\ a_3-y_2{a}_4+x_2{a}_5-z_2(a_7+a_8)-x_2{a}_{11}-y_2{a}_{12}=b_7\\ a_1+z_2{a}_5+y_2{a}_6+x_2(a_8+a_9)-y_2{a}_{10}+z_2{a}_{11}=b_8\\ a_2-z_2{a}_4-x_2{a}_6+y_2(a_7+a_9)-x_2{a}_{10}+z_2{a}_{12}=b_9\\ -a_2+z_1{a}_4+x_1{a}_6+y_1(a_7+a_9)+x_1{a}_{10}-z_1{a}_{12}=b_{10}\\ a_1+z_1{a}_5-y_1{a}_6+x_1(a_8+a_9)+y_1{a}_{10}+z_1{a}_{11}=b_{11}\\ a_3+y_1{a}_4+x_1{a}_5-z_1(a_7+a_8)-x_1{a}_{11}+y_1{a}_{12}=b_{12}\end{array}\right.---\left(1\right)$

式(1)中，未知变量a₁、a₂、a₃、a₄、a₅、a₆、a₇、a₈、a₉、a₁₀、a₁₁、a₁₂分别为一一对应的A_x、A_y、A_z、ω_xω_y、ω_xω_z、ω_yω_z，求解式(1)可推导出平台旋转参数，如以下公式(2)～(13)所示，其中，A_x、A_y、A_z分别是动平台的坐标原点O_b相对于静平台的坐标原点O_I在x、y、z三个方向的加速度分量，分别是动平台旋转角速度在x、y、z三个方向的角加速度，通过该角加速度可快速判断出旋转角方向，分别是动平台旋转角速度在x、y、z三个方向的幅值平方，通过该幅值平方可快速计算出旋转角度大小，ω_xω_y、ω_xω_z、ω_yω_z为误差分析参数，由 ω_xω_y、ω_xω_z、ω_yω_z九个变量准确获得在x、y、z三个方向的动平台旋转角速度ω_x、ω_y、ω_z的幅值和旋转方向：

$A_x=\frac{1}{2}\left\{\right[1+\frac{Z_1(1-\frac{X_2}{X_1})}{(Z_2-Z_1)}]b_1+\frac{Z_1(1+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_2-Z_1)}{b}_5+\frac{Z_1(-1+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_2-Z_1)}{b}_8+[1+\frac{Z_1(1+\frac{X_2}{X_1})}{(Z_2-Z_1)}\left]b_{11}\right\}---\left(2\right)$

$A_y=\frac{1}{2}\left\{\right[1+\frac{Z_1(1-\frac{X_2}{X_1})}{(Z_2-Z_1)}]b_2-\frac{Z_1(3+\frac{y_1}{y_2}+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_2-Z_1)}{b}_6+\frac{Z_1(\frac{2y_1}{y_2}+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_2-Z_1)}{b}_9-[1+\frac{Z_1(1+\frac{X_2}{X_1})}{(Z_2-Z_1)}\left]b_{10}\right\}---\left(3\right)$

$A_z=\frac{1}{2}\left\{\right[1+\frac{Z_1(1-\frac{X_2}{X_1})}{(Z_2-Z_1)}]b_3+\frac{Z_1(1+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_2-Z_1)}{b}_4+\frac{Z_1(-1+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_2-Z_1)}{b}_7+[1+\frac{Z_1(1+\frac{X_2}{X_1})}{(Z_2-Z_1)}\left]b_{12}\right\}---\left(4\right)$

${\stackrel{\·}{\mathrm{\ω}}}_x=\frac{1}{4}[\frac{(1-\frac{X_2}{X_1})}{(Z_2-Z_1)}{b}_2-\frac{1}{y_2}{b}_4-\frac{(3+\frac{y_1}{y_2}+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_2-Z_1)}{b}_6-\frac{1}{y_2}{b}_7+\frac{(\frac{2y_1}{y_2}+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_2-Z_1)}{b}_9-\frac{(1+\frac{X_2}{X_1})}{(Z_2-Z_1)}{b}_{10}]---\left(5\right)$

${\stackrel{\·}{\mathrm{\ω}}}_y=\frac{1}{4}\left[\begin{array}{c}\frac{(1-\frac{X_2}{X_1})}{(Z_1-Z_2)}{b}_1-\frac{1}{x_1}{b}_3+\frac{y_1}{x_1{y}_2}{b}_4+\frac{(1+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_1-Z_2)}{b}_5+\frac{y_1}{x_1{y}_2}{b}_7\\ +\frac{(-1+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_1-Z_2)}{b}_8+\frac{(1+\frac{X_2}{X_1})}{(Z_1-Z_2)}{b}_{11}+\frac{1}{x_1}{b}_{12}\end{array}\right]---\left(6\right)$

${\stackrel{\·}{\mathrm{\ω}}}_z=\frac{1}{4}[\frac{1}{x_1}{b}_2+\frac{1}{y_2}{b}_5+\frac{y_1}{x_1{y}_2}{b}_6-\frac{y_1}{x_1{y}_2}{b}_9+\frac{1}{y_2}{b}_8+\frac{1}{x_1}{b}_{10}]---\left(7\right)$

${\mathrm{\ω}}_x^2=\frac{1}{4}\left[\begin{array}{c}\frac{1}{x_1}{b}_1+\frac{(1-\frac{X_2}{X_1})}{(Z_2-Z_1)}{b}_3+\frac{(1+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_2-Z_1)}{b}_4-\frac{y_1}{x_1{y}_2}{b}_5-\frac{1}{y_2}{b}_6+\frac{(-1+\frac{x_1{y}_2}{x_1{y}_2})}{(Z_2+Z_1)}{b}_7\\ -\frac{y_1}{x_1{y}_2}{b}_8+\frac{1}{y_2}{b}_9-\frac{1}{x_1}{b}_{11}+\frac{(1+\frac{x_2}{x_1})}{(Z_2-Z_1)}{b}_{12}\end{array}\right]---\left(8\right)$

${\mathrm{\ω}}_y^2=\frac{1}{4}\left[\begin{array}{c}-\frac{1}{x_1}{b}_1+\frac{(1-\frac{X_2}{X_1})}{(Z_2-Z_1)}{b}_3+\frac{(1+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_2-Z_1)}{b}_4+\frac{y_1}{x_1{y}_2}{b}_5+\frac{1}{y_2}{b}_6+\frac{(-1+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_2+Z_1)}{b}_7\\ +\frac{y_1}{x_1{y}_2}{b}_8-\frac{1}{y_2}{b}_9+\frac{1}{x_1}{b}_{11}+\frac{(1+\frac{x_2}{x_1})}{(Z_2-Z_1)}{b}_{12}\end{array}\right]---\left(9\right)$

${\mathrm{\ω}}_z^2=\frac{1}{4}\left[\begin{array}{c}-\frac{1}{x_1}{b}_1-\frac{(1-\frac{X_2}{X_1})}{(Z_2-Z_1)}{b}_3-\frac{(1+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_2-Z_1)}{b}_4+\frac{y_1}{x_1{y}_2}{b}_5-\frac{1}{y_2}{b}_6-\frac{(-1+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_2+Z_1)}{b}_7\\ +\frac{y_1}{x_1{y}_2}{b}_8+\frac{1}{y_2}{b}_9+\frac{1}{x_1}{b}_{11}-\frac{(1+\frac{x_2}{x_1})}{(Z_2-Z_1)}{b}_{12}\end{array}\right]---\left(10\right)$

${\mathrm{\ω}}_x{\mathrm{\ω}}_y=\frac{1}{4}[\frac{1}{x_1}{b}_2-\frac{1}{y_2}{b}_5+\frac{y_1}{x_1{y}_2}{b}_6-\frac{1}{y_2}{b}_8-\frac{y_1}{x_1{y}_2}{b}_9+\frac{1}{x_1}{b}_{10}]---\left(11\right)$

${\mathrm{\ω}}_x{\mathrm{\ω}}_z=\frac{1}{4}\left[\begin{array}{c}\frac{(1-\frac{X_2}{X_1})}{(Z_1-Z_2)}{b}_1+\frac{1}{x_1}{b}_3-\frac{y_1}{x_1{y}_2}{b}_4+\frac{(1+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_1-Z_2)}{b}_5-\frac{y_1}{x_1{y}_2}{b}_7\\ +\frac{(-1+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_1-Z_2)}{b}_8+\frac{(1+\frac{X_2}{X_1})}{(Z_1-Z_2)}{b}_{11}-\frac{1}{x_1}{b}_{12}\end{array}\right]---\left(12\right)$

${\mathrm{\ω}}_y{\mathrm{\ω}}_z=-\frac{1}{4}\left[\begin{array}{c}\frac{(1-\frac{X_2}{X_1})}{(Z_2-Z_1)}{b}_2+\frac{1}{y_2}{b}_4-\frac{(3+\frac{y_1}{y_2}+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_2-Z_1)}{b}_6+\frac{1}{y_2}{b}_7\\ +\frac{(\frac{2y_1}{y_2}+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_2-Z_1)}{b}_9-\frac{(1+\frac{X_2}{X_1})}{(Z_2-Z_1)}{b}_{10}\end{array}\right]---\left(13\right)$

上面公式中的x₁、y₁、z₁、x₂、y₂、z₂分别为对应三轴向加速度传感器相对于动平台的坐标原点O_b的距离，可自定义其数值；

通过上述公式(2)-(13)求得A_x、A_y、A_z、ω_x、ω_y、ω_z，即可获得动平台坐标系相对于静平台坐标系的位置和姿态参数，即动平台的坐标原点O_b即形心在静平台坐标系中的位置坐标P＝(x,y,z)和旋转参数摇摆角α、纵摇角β及偏摆角γ。

本发明的有益效果在于：

本发明通过在动平台上配置四个三轴向加速度传感器，并对其两两对称安装，在能够测量Stewart平台姿态的前提下，大大简化了测量装置的结构，既便于安装，又便于测量Stewart平台姿态；通过本发明的测量方法，无需复杂计算就可以直接获得Stewart平台的形心坐标、旋转角度和旋转方向参数，从而实现平台的全闭环控制，具有计算速度快、测量精度高、动态特性(响应速度快、工作频响范围宽)好、响应速度快的优点。

附图说明

图1是本发明所述Stewart平台姿态测量装置的结构示意图；

图2是本发明所述第一三轴向加速度传感器和第一垫块的立体结构示意图；

图3是本发明所述Stewart平台的动平台和静平台的坐标示意图。

具体实施方式

下面结合附图对本发明作进一步说明：

如图1和图2所示，本发明所述Stewart平台包括动平台5和静平台(与本发明的创新方案无关，所以图中未示出)，动平台5和静平台之间通过六个球铰分别与六根可伸缩的连杆(图中未示出)相连，动平台5的六个球铰分别为第一球铰A1、第二球铰A2、第三球铰A3、第四球铰A4、第五球铰A5、第六球铰A6；本发明所述Stewart平台姿态测量装置包括四个三轴向加速度传感器，即第一三轴向加速度传感器AC1、第二三轴向加速度传感器AC2、第三三轴向加速度传感器AC3和第四三轴向加速度传感器AC4；设动平台5的坐标系{O_b}的三个坐标轴轴向分别为X、Y、Z，其坐标原点O_b是动平台5的6个球铰的机械绞点组成的圆周的圆心，设静平台坐标系{O_I}的坐标原点O_I是静平台的6个球铰的机械绞点组成的圆周的圆心；在动平台5上安装有四个垫块，即第一垫块1、第二垫块2、第三垫块3、第四垫块4，第一三轴向加速度传感器AC1、第二三轴向加速度传感器AC2、第三三轴向加速度传感器AC3和第四三轴向加速度传感器AC4分别一一对应地安装在第一垫块1、第二垫块2、第三垫块3和第四垫块4上，其中，第一三轴向加速度传感器AC1和第四三轴向加速度传感器AC4分别安装于第一垫块1和第四垫块4的上表面，第二三轴向加速度传感器AC2和第三三轴向加速度传感器AC3分别安装于第二垫块2和第三垫块3的侧表面；四个三轴向加速度传感器的三个敏感轴方向均为x、y、z，其中，第一三轴向加速度传感器AC1与第四三轴向加速度传感器AC4以坐标原点O_b对称并分别位于X轴的两侧，第二三轴向加速度传感器AC2与第三三轴向加速度传感器AC3以X轴为对称轴对称安装，第一三轴向加速度传感器AC1的x、y、z轴分别与动平台5的X、Y、Z轴平行，第一三轴向加速度传感器AC1、第二三轴向加速度传感器AC2、第三三轴向加速度传感器AC3、第四三轴向加速度传感器AC4的包含安装方向的坐标分别为：(x₁、-y₁、z₁)、(-x₂、y₂、z₂)、(-x₂、-y₂、z₂)、(-x₁、y₁、z₁)，这里的x₁、y₁、z₁、x₂、y₂、z₂分别为对应三轴向加速度传感器相对于动平台5的坐标原点O_b的距离，可自定义其数值。上述PCB公司的三轴向加速度传感器为灵敏度为100mv/g、量程为50g、精度为0.0001g、频响范围为0.5-4.5KHz的低频三轴向加速度传感器。

图1中，第一球铰A1和第二球铰A2连线中心位置取点，从动平台的坐标原点O_b(即形心)出射经过此点的射线设为动平台5的X轴正向，从动平台的坐标原点O_b出射垂直于动平台5平面的射线设为Z轴正向，右手法则可以找到动平台5的Y轴正向；动平台5绕X、Y、Z轴转动角度分别用摇摆角α、纵摇角β及偏摆角γ描述，动平台的坐标原点O_b在静平台坐标系中的位置坐标用(x,y,z)表示，即动平台的坐标原点O_b在静平台坐标系中的位置坐标P＝(x,y,z)。

结合图1-图3，本发明所述Stewart平台姿态测量装置采用的测量方法，其测量原理是将无陀螺捷联惯导思想应用于Stewart平台位姿测量上，即采用线加速度计测量物体的角加速度，线加速度a同角加速度的关系通过两点间的线加速度即可求出物体某个方向的角加速度，因此在刚体上合理安装适当数量的加速度计，对加速度计输出信息进行处理，便可确定刚体在三维空间中的运动情况。

图2是动、静平台坐标示意图，定义静平台坐标系I和动平台坐标系b，M为动平台坐标系内一点。M点在静平台坐标系I的矢径等于M点在动平台坐标系b的矢径与动平台坐标系b相对于静平台坐标系I的矢径的矢量和，R＝R_I+r_b，对此式求二阶导数，可以得出动平台坐标系中M点加速度a用静平台坐标系I的坐标表示方程为：

$\mathrm{\α}=A_I+\stackrel{\·\·}{r_b}+\stackrel{\·}{\mathrm{\ω}}\×r+2\mathrm{\ω}\×\stackrel{\·}{r_b}+\mathrm{\ω}\×(\mathrm{\ω}\×r)---\left(14\right)$

其中，A_I是动平台5的坐标原点O_b相对于静平台的坐标原点O_I的加速度，即是M点相对动平台坐标系b的加速度；ω×(ω×r)是欧拉加速度(Euleracceleration)；是科氏加速度(Coriolis acceleration)；ω×(ω×r)是向心加速度(Centripetal acceleration)。若M点是固定在动平台坐标系b的，因此所以有关和的项可以舍去，故式(14)表示为以下式(15)：

$\mathrm{\α}=A_I+\stackrel{\·}{\mathrm{\ω}}\×r+\mathrm{\ω}\×(\mathrm{\ω}\×r)---\left(15\right)$

式(15)用矩阵表示为下式(16)：

$\mathrm{\α}=A_I+F\left(\stackrel{\·}{\mathrm{\ω}}\right)r+F{\left(\mathrm{\ω}\right)}^{2}r---\left(16\right)$

其中：

$F\left(m\right)\left|\begin{array}{ccc}0& -m_z& m_y\\ m_z& 0& -m_x\\ -m_y& -m_x& 0\end{array}\right|$

设三轴向加速度传感器的输出加速度值为a_MS，三轴向加速度传感器的敏感轴为θi＝[θx，θy，θz]，a_MS＝α·θi。

线性加速度传感器通常只有一个敏感方向，因此式(16)可以用以下标量表示：

$\begin{array}{c}A{C}_i={\mathrm{\θ}}_x(A_x-r_x{\mathrm{\ω}}_y^2-r_x{\mathrm{\ω}}_z^2+r_y{\mathrm{\ω}}_x{\mathrm{\ω}}_y+r_z{\mathrm{\ω}}_x{\mathrm{\ω}}_z+r_z{\stackrel{\·}{\mathrm{\ω}}}_y-r_y{\stackrel{\·}{\mathrm{\ω}}}_z)\\ +{\mathrm{\θ}}_y(A_y-r_y{\mathrm{\ω}}_x^2-r_y{\mathrm{\ω}}_z^2+r_x{\mathrm{\ω}}_x{\mathrm{\ω}}_y+r_z{\mathrm{\ω}}_y{\mathrm{\ω}}_z+r_x{\stackrel{\·}{\mathrm{\ω}}}_z-r_z{\stackrel{\·}{\mathrm{\ω}}}_x)\\ +{\mathrm{\θ}}_z(A_z-r_z{\mathrm{\ω}}_y^2-r_z{\mathrm{\ω}}_x^2+r_x{\mathrm{\ω}}_x{\mathrm{\ω}}_z+r_y{\mathrm{\ω}}_y{\mathrm{\ω}}_z+r_y{\stackrel{\·}{\mathrm{\ω}}}_x{r}_x{\stackrel{\·}{\mathrm{\ω}}}_y)\end{array}---\left(17\right)$

式(17)可以看出敏感轴方向、加速度、角速度和角加速度合成了三轴向加速度传感器的输出。由于从欧拉加速度和向心加速度都可以分离出物体的旋转，但两者各有优缺点。采用欧拉加速度计算的优点是很小的角速度和旋转方向都能分辨出，缺点是必须两次积分角加速度才能获得角度，这导致了误差累积，使长时间测量时会产生较大的误差；而向心加速度是角速度的平方值，取平方根后只需积分一次就可以获得旋转角度，优点是计算速度快时间累积误差小，缺点是无法分辨小的旋转角度和选转方向，所以结合两者优点，一般采用前者判断旋转方向，后者直接计算旋转角度。

将图1所示4个三轴向加速度传感器的敏感轴输出、坐标位置及敏感方向带入式(17)，并转换成矩阵表示：

$\left|\begin{array}{c}A{C}_{1x}\\ A{C}_{1y}\\ A{C}_{1z}\\ A{C}_{2x}\\ A{C}_{2y}\\ A{C}_{2z}\\ A{C}_{3x}\\ A{C}_{3y}\\ A{C}_{3z}\\ A{C}_{4x}\\ A{C}_{4y}\\ A{C}_{4z}\end{array}\right|=\begin{array}{c}\left|\begin{array}{cccccccccccc}1& 0& 0& 0& z_1& -y_1& 0& -x_1& -x_1& y_1& z_1& 0\\ 0& 1& 0& -z_1& 0& x_1& -y_1& 0& -y_1& x_1& 0& z_1\\ 0& 0& 1& y_1& -x_1& 0& -z_1& -z_1& 0& 0& x_1& y_1\\ 0& 0& -1& -y_2& x_2& 0& z_2& z_2& 0& 0& -x_2& -y_2\\ -1& 0& 0& 0& -z_2& y_2& 0& x_2& x_2& -y_2& -z_2& 0\\ 0& 1& 0& -z_2& 0& x_2& -y_2& 0& -y_2& x_2& 0& z_2\\ 0& 0& 1& y_3& -x_3& 0& -z_3& -z_3& 0& 0& x_3& y_3\\ 1& 0& 0& 0& z_3& -y_3& 0& -x_3& -x_3& y_3& z_3& 0\\ 0& 1& 0& -z_3& 0& x_3& -y_3& 0& -y_3& x_3& 0& z_3\\ 0& -1& 0& z_4& 0& -x_4& y_4& 0& y_4& -x_4& 0& -z_4\\ 1& 0& 0& 0& z_4& -y_4& 0& -x_4& -x_4& y_4& z_4& 0\\ 0& 0& 1& y_4& -x_4& 0& -z_4& -z_4& 0& 0& x_4& y_4\end{array}\right|\end{array}\·\left|\begin{array}{c}{A}_x\\ A_y\\ A_z\\ {\stackrel{\·}{\mathrm{\ω}}}_x\\ {\stackrel{\·}{\mathrm{\ω}}}_y\\ {\stackrel{\·}{\mathrm{\ω}}}_z\\ {\mathrm{\ω}}_x^2\\ {\mathrm{\ω}}_y^2\\ {\mathrm{\ω}}_z^2\\ {\mathrm{\ω}}_x{\mathrm{\ω}}_y\\ {\mathrm{\ω}}_x{\mathrm{\ω}}_y\\ {\mathrm{\ω}}_x{\mathrm{\ω}}_z\end{array}\right|---\left(18\right)$

根据上述推论，本发明所述Stewart平台姿态测量装置采用的测量方法，包括以下步骤：

(1)将四个所述三轴向加速度传感器的输出简化成(b₁，b₂……b₁₂)，其中(b₁、b₂、b₃)是第一三轴向加速度传感器的x、y、z三个方向输出值，(b₄、b₅、b₆)是第二三轴向加速度传感器的x、y、z三个方向输出值，(b₇、b₈、b₉)是第三三轴向加速度传感器的x、y、z三个方向输出值，(b₁₀、b₁₁、b₁₂)是第四三轴向加速度传感器的x、y、z三个方向输出值；

(2)将式(18)转换成式(1)：

$\left\{\begin{array}{c}{a}_1+z_1{a}_5+y_1{a}_6-x_1(a_8+a_9)-y_1{a}_{10}+z_1{a}_{11}=b_1\\ a_2-z_1{a}_4+x_1{a}_6+y_1(a_7+a_9)+x_1{a}_{10}+z_1{a}_{12}=b_2\\ a_3-y_1{a}_4-x_1{a}_5-z_1(a_7+a_8)+x_1{a}_{11}-y_1{a}_{12}=b_3\\ -a_3-y_2{a}_4-x_2{a}_5+z_2(a_7+a_8)+x_2{a}_{11}-y_1{a}_{12}=b_4\\ -a_1-z_2{a}_5+y_2{a}_6-x_2(a_8+a_9)-y_2{a}_{10}-z_2{a}_{11}=b_5\\ a_2-z_2{a}_4-x_2{a}_6-y_2(a_7+a_9)-x_2{a}_{10}+z_2{a}_{12}=b_6\\ a_3-y_2{a}_4+x_2{a}_5-z_2(a_7+a_8)-x_2{a}_{11}-y_2{a}_{12}=b_7\\ a_1+z_2{a}_5+y_2{a}_6+x_2(a_8+a_9)-y_2{a}_{10}+z_2{a}_{11}=b_8\\ a_2-z_2{a}_4-x_2{a}_6+y_2(a_7+a_9)-x_2{a}_{10}+z_2{a}_{12}=b_9\\ -a_2+z_1{a}_4+x_1{a}_6+y_1(a_7+a_9)+x_1{a}_{10}-z_1{a}_{12}=b_{10}\\ a_1+z_1{a}_5-y_1{a}_6+x_1(a_8+a_9)+y_1{a}_{10}+z_1{a}_{11}=b_{11}\\ a_3+y_1{a}_4+x_1{a}_5-z_1(a_7+a_8)-x_1{a}_{11}+y_1{a}_{12}=b_{12}\end{array}\right.---\left(1\right)$

式(1)中，未知变量a₁、a₂、a₃、a₄、a₅、a₆、a₇、a₈、a₉、a₁₀、a₁₁、a₁₂分别为一一对应的A_x、A_y、A_z、ω_xω_y、ω_xω_z、ω_yω_z，求解式(1)可推导出平台旋转参数，如以下公式(2)～(13)所示，其中，A_x、A_y、A_z分别是动平台的坐标原点O_b相对于静平台的坐标原点O_I在x、y、z三个方向的加速度分量，分别是动平台旋转角速度在x、y、z三个方向的角加速度，通过该角加速度可快速判断出旋转角方向，分别是动平台旋转角速度在x、y、z三个方向的幅值平方，通过该幅值平方可快速计算出旋转角度大小，ω_xω_y、ω_xω_z、ω_yω_z为误差分析参数，由 ω_xω_y、ω_xω_z、ω_yω_z九个变量准确获得在x、y、z三个方向的动平台旋转角速度ω_x、ω_y、ω_z的幅值和旋转方向：

$A_x=\frac{1}{2}\left\{\right[1+\frac{Z_1(1-\frac{X_2}{X_1})}{(Z_2-Z_1)}]b_1+\frac{Z_1(1+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_2-Z_1)}{b}_5+\frac{Z_1(-1+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_2-Z_1)}{b}_8+[1+\frac{Z_1(1+\frac{X_2}{X_1})}{(Z_2-Z_1)}\left]b_{11}\right\}---\left(2\right)$

$A_y=\frac{1}{2}\left\{\right[1+\frac{Z_1(1-\frac{X_2}{X_1})}{(Z_2-Z_1)}]b_2-\frac{Z_1(3+\frac{y_1}{y_2}+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_2-Z_1)}{b}_6+\frac{Z_1(\frac{2y_1}{y_2}+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_2-Z_1)}{b}_9-[1+\frac{Z_1(1+\frac{X_2}{X_1})}{(Z_2-Z_1)}\left]b_{10}\right\}---\left(3\right)$

$A_z=\frac{1}{2}\left\{\right[1+\frac{Z_1(1-\frac{X_2}{X_1})}{(Z_2-Z_1)}]b_3+\frac{Z_1(1+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_2-Z_1)}{b}_4+\frac{Z_1(-1+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_2-Z_1)}{b}_7+[1+\frac{Z_1(1+\frac{X_2}{X_1})}{(Z_2-Z_1)}\left]b_{12}\right\}---\left(4\right)$

${\stackrel{\·}{\mathrm{\ω}}}_x=\frac{1}{4}[\frac{(1-\frac{X_2}{X_1})}{(Z_2-Z_1)}{b}_2-\frac{1}{y_2}{b}_4-\frac{(3+\frac{y_1}{y_2}+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_2-Z_1)}{b}_6-\frac{1}{y_2}{b}_7+\frac{(\frac{2y_1}{y_2}+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_2-Z_1)}{b}_9-\frac{(1+\frac{X_2}{X_1})}{(Z_2-Z_1)}{b}_{10}]---\left(5\right)$

${\stackrel{\·}{\mathrm{\ω}}}_y=\frac{1}{4}\left[\begin{array}{c}\frac{(1-\frac{X_2}{X_1})}{(Z_1-Z_2)}{b}_1-\frac{1}{x_1}{b}_3+\frac{y_1}{x_1{y}_2}{b}_4+\frac{(1+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_1-Z_2)}{b}_5+\frac{y_1}{x_1{y}_2}{b}_7\\ +\frac{(-1+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_1-Z_2)}{b}_8+\frac{(1+\frac{X_2}{X_1})}{(Z_1-Z_2)}{b}_{11}+\frac{1}{x_1}{b}_{12}\end{array}\right]---\left(6\right)$

${\stackrel{\·}{\mathrm{\ω}}}_z=\frac{1}{4}[\frac{1}{x_1}{b}_2+\frac{1}{y_2}{b}_5+\frac{y_1}{x_1{y}_2}{b}_6-\frac{y_1}{x_1{y}_2}{b}_9+\frac{1}{y_2}{b}_8+\frac{1}{x_1}{b}_{10}]---\left(7\right)$

${\mathrm{\ω}}_x^2=\frac{1}{4}\left[\begin{array}{c}\frac{1}{x_1}{b}_1+\frac{(1-\frac{X_2}{X_1})}{(Z_2-Z_1)}{b}_3+\frac{(1+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_2-Z_1)}{b}_4-\frac{y_1}{x_1{y}_2}{b}_5-\frac{1}{y_2}{b}_6+\frac{(-1+\frac{x_1{y}_2}{x_1{y}_2})}{(Z_2+Z_1)}{b}_7\\ -\frac{y_1}{x_1{y}_2}{b}_8+\frac{1}{y_2}{b}_9-\frac{1}{x_1}{b}_{11}+\frac{(1+\frac{x_2}{x_1})}{(Z_2-Z_1)}{b}_{12}\end{array}\right]---\left(8\right)$

${\mathrm{\ω}}_y^2=\frac{1}{4}\left[\begin{array}{c}-\frac{1}{x_1}{b}_1+\frac{(1-\frac{X_2}{X_1})}{(Z_2-Z_1)}{b}_3+\frac{(1+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_2-Z_1)}{b}_4+\frac{y_1}{x_1{y}_2}{b}_5+\frac{1}{y_2}{b}_6+\frac{(-1+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_2+Z_1)}{b}_7\\ +\frac{y_1}{x_1{y}_2}{b}_8-\frac{1}{y_2}{b}_9+\frac{1}{x_1}{b}_{11}+\frac{(1+\frac{x_2}{x_1})}{(Z_2-Z_1)}{b}_{12}\end{array}\right]---\left(9\right)$

${\mathrm{\ω}}_z^2=\frac{1}{4}\left[\begin{array}{c}-\frac{1}{x_1}{b}_1-\frac{(1-\frac{X_2}{X_1})}{(Z_2-Z_1)}{b}_3-\frac{(1+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_2-Z_1)}{b}_4+\frac{y_1}{x_1{y}_2}{b}_5-\frac{1}{y_2}{b}_6-\frac{(-1+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_2+Z_1)}{b}_7\\ +\frac{y_1}{x_1{y}_2}{b}_8+\frac{1}{y_2}{b}_9+\frac{1}{x_1}{b}_{11}-\frac{(1+\frac{x_2}{x_1})}{(Z_2-Z_1)}{b}_{12}\end{array}\right]---\left(10\right)$

${\mathrm{\ω}}_x{\mathrm{\ω}}_y=\frac{1}{4}[\frac{1}{x_1}{b}_2-\frac{1}{y_2}{b}_5+\frac{y_1}{x_1{y}_2}{b}_6-\frac{1}{y_2}{b}_8-\frac{y_1}{x_1{y}_2}{b}_9+\frac{1}{x_1}{b}_{10}]---\left(11\right)$

${\mathrm{\ω}}_x{\mathrm{\ω}}_z=\frac{1}{4}\left[\begin{array}{c}\frac{(1-\frac{X_2}{X_1})}{(Z_1-Z_2)}{b}_1+\frac{1}{x_1}{b}_3-\frac{y_1}{x_1{y}_2}{b}_4+\frac{(1+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_1-Z_2)}{b}_5-\frac{y_1}{x_1{y}_2}{b}_7\\ +\frac{(-1+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_1-Z_2)}{b}_8+\frac{(1+\frac{X_2}{X_1})}{(Z_1-Z_2)}{b}_{11}-\frac{1}{x_1}{b}_{12}\end{array}\right]---\left(12\right)$

${\mathrm{\ω}}_y{\mathrm{\ω}}_z=-\frac{1}{4}\left[\begin{array}{c}\frac{(1-\frac{X_2}{X_1})}{(Z_2-Z_1)}{b}_2+\frac{1}{y_2}{b}_4-\frac{(3+\frac{y_1}{y_2}+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_2-Z_1)}{b}_6+\frac{1}{y_2}{b}_7\\ +\frac{(\frac{2y_1}{y_2}+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_2-Z_1)}{b}_9-\frac{(1+\frac{X_2}{X_1})}{(Z_2-Z_1)}{b}_{10}\end{array}\right]---\left(13\right)$

上面公式中的x₁、y₁、z₁、x₂、y₂、z₂分别为对应三轴向加速度传感器相对于动平台的坐标原点O_b的距离，可自定义其数值；

通过上述公式(2)-(13)求得A_x、A_y、A_z、ω_x、ω_y、ω_z，即可获得动平台坐标系相对于静平台坐标系的位置和姿态参数，即动平台的坐标原点O_b即形心在静平台坐标系中的位置坐标P＝(x,y,z)和旋转参数摇摆角α、纵摇角β及偏摆角γ。

理论上完整描述一个物体在空间的运动，需要6个独立参量，即3个描述质心运动的参数和3个描述绕质心转动的参量。这6个独立参量至少需要6个测量元件来测量。针对Stewart六自由度平台的位姿测量，本发明提出了4个三轴向加速度传感器配置方案，并推导出利用加速度计输出信号获得Stewart六自由度平台的位姿的测量算法，由于计算量小，通过数据采集系统和测试软件，除了能很快获得描述载体运动所需的6个独立参量外，还能准确判断旋转方向，且具有冗余信息可以计算载体姿态角速度部分误差，并对其进行补偿提高系统测量精度。本发明给出了具体的加速度配置方法，并推导出姿态参数解析公式，可以为Stewart六自由度平台的位姿测量提供一种新的途径，也可为其他多自由度平台的位姿测量提供参考。

上述实施例只是本发明的较佳实施例，并不是对本发明技术方案的限制，只要是不经过创造性劳动即可在上述实施例的基础上实现的技术方案，均应视为落入本发明专利的权利保护范围内。

Claims (3)

1.一种Stewart平台姿态测量装置，所述Stewart平台包括动平台和静平台，所述动平台和所述静平台之间通过六个球铰分别与六根可伸缩的连杆相连；其特征在于：设动平台坐标系{O_b}的三个坐标轴轴向分别为X、Y、Z，其坐标原点O_b是所述动平台的6个球铰的机械绞点组成的圆周的圆心，设静平台坐标系{O_I}的坐标原点O_I是所述静平台的6个球铰的机械绞点组成的圆周的圆心；在所述动平台上安装有四个垫块，在四个所述垫块上一一对应地安装有四个三轴向加速度传感器，其中，所述第一三轴向加速度传感器和所述第四三轴向加速度传感器分别安装于对应的所述垫块的上表面，所述第二三轴向加速度传感器和所述第三三轴向加速度传感器分别安装于对应的所述垫块的侧表面；四个所述三轴向加速度传感器的三个敏感轴方向均为x、y、z，其中，第一三轴向加速度传感器与第四三轴向加速度传感器以坐标原点O_b对称并分别位于X轴的两侧，第二三轴向加速度传感器与第三三轴向加速度传感器以X轴为对称轴对称安装，所述第一三轴向加速度传感器的x、y、z轴分别与所述动平台的X、Y、Z轴平行，所述第一三轴向加速度传感器、所述第二三轴向加速度传感器、所述第三三轴向加速度传感器、所述第四三轴向加速度传感器的包含安装方向的坐标分别为：(x₁、-y₁、z₁)、(-x₂、y₂、z₂)、(-x₂、-y₂、z₂)、(-x₁、y₁、z₁)。

2.根据权利要求1所述的Stewart平台姿态测量装置，其特征在于：所述三轴向加速度传感器为灵敏度为100mv/g、量程为50g、精度为0.0001g、频响范围为0.5-4.5KHz的低频三轴向加速度传感器。

3.一种如权利要求1或2所述的Stewart平台姿态测量装置采用的测量方法，其特征在于：包括以下步骤：

(1)将四个所述三轴向加速度传感器的输出简化成(b₁，b₂……b₁₂)，其中(b₁、b₂、b₃)是第一三轴向加速度传感器的x、y、z三个方向输出值，(b₄、b₅、b₆)是第二三轴向加速度传感器的x、y、z三个方向输出值，(b₇、b₈、b₉)是第三三轴向加速度传感器的x、y、z三个方向输出值，(b₁₀、b₁₁、b₁₂)是第四三轴向加速度传感器的x、y、z三个方向输出值；

(2)根据下面公式(1)推导求解：

$\left\{\begin{array}{c}{a}_1+z_1{a}_5+y_1{a}_6-x_1(a_8+a_9)-y_1{a}_{10}+z_1{a}_{11}=b_1\\ a_2-z_1{a}_4+x_1{a}_6+y_1(a_7+a_9)+x_1{a}_{10}+z_1{a}_{12}=b_2\\ a_3-y_1{a}_4-x_1{a}_5-z_1(a_7+a_8)+x_1{a}_{11}-y_1{a}_{12}=b_3\\ -a_3-y_2{a}_4-x_2{a}_5+z_2(a_7+a_8)+x_2{a}_{11}-y_1{a}_{12}=b_4\\ -a_1-z_2{a}_5+y_2{a}_6-x_2(a_8+a_9)-y_2{a}_{10}-z_2{a}_{11}=b_5\\ a_2-z_2{a}_4-x_2{a}_6-y_2(a_7+a_9)-x_2{a}_{10}+z_a{a}_{12}=b_6\\ a_3-y_2{a}_4+x_2{a}_5-z_2(a_7+a_8)-x_2{a}_{11}-y_2{a}_{12}=b_7\\ a_1+z_2{a}_5+y_2{a}_6+x_2(a_8+a_9)-y_2{a}_{10}+z_2{a}_{11}=b_8\\ a_2-z_2{a}_4-x_2{a}_6+y_2(a_7+a_9)-x_2{a}_{10}+z_2{a}_{12}=b_9\\ -a_2+z_1{a}_4+x_1{a}_6+y_1(a_7+a_9)+x_1{a}_{10}-z_1{a}_{12}=b_{10}\\ a_1+z_1{a}_5-y_1{a}_6+x_1(a_8+a_9)+y_1{a}_{10}+z_1{a}_{11}=b_{11}\\ a_3+y_1{a}_4+x_1{a}_5-z_1(a_7+a_8)-x_1{a}_{11}+y_1{a}_{12}=b_{12}\end{array}\right.---\left(1\right)$

式(1)中，未知变量a₁、a₂、a₃、a₄、a₅、a₆、a₇、a₈、a₉、a₁₀、a₁₁、a₁₂分别为一一对应的A_x、A_y、A_z、求解式(1)可推导出平台旋转参数，如以下公式(2)～(13)所示，其中，A_x、A_y、A_z分别是动平台的坐标原点O_b相对于静平台的坐标原点O_I在x、y、z三个方向的加速度分量，分别是动平台旋转角速度在x、y、z三个方向的角加速度，通过该角加速度可快速判断出旋转角方向，分别是动平台旋转角速度在x、y、z三个方向的幅值平方，通过该幅值平方可快速计算出旋转角度大小，ω_xω_y、ω_xω_z、ω_yω_z为误差分析参数，由 ω_xω_y、ω_xω_z、ω_yω_z九个变量准确获得在x、y、z三个方向的动平台旋转角速度ω_x、ω_y、ω_z的幅值和旋转方向：

$A_x=\frac{1}{2}\left\{\right[1+\frac{Z_1(1-\frac{x_2}{x_1})}{(Z_2-Z_1)}]b_1+\frac{Z_1(1+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_2-Z_1)}{b}_5+\frac{Z_1(-1+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_2-Z_1)}{b}_8+[1+\frac{z_1(1+\frac{x_2}{x_1})}{(Z_2-Z_1)}\left]b_{11}\right\}---\left(2\right)$

$A_y=\frac{1}{2}\left\{\right[1+\frac{Z_1(1-\frac{x_2}{x_1})}{(Z_2-Z_1)}]b_2+\frac{Z_1(3+\frac{y_1}{y_2}+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_2-Z_1)}{b}_6+\frac{Z_1(\frac{{2y}_1}{y_2}+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_2-Z_1)}{b}_9-[1+\frac{Z_1(1+\frac{x_2}{x_1})}{(Z_2-Z_1)}\left]b_{10}\right\}---\left(3\right)$

$A_z=\frac{1}{2}\left\{\right[1+\frac{Z_1(1-\frac{x_2}{x_1})}{(Z_2-Z_1)}]b_3+\frac{Z_1(1+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_2-Z_1)}{b}_4+\frac{Z_1(-1+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_2-Z_1)}{b}_7+[1+\frac{Z_1(1+\frac{x_2}{x_1})}{(Z_2-Z_1)}\left]b_{12}\right\}---\left(4\right)$

${\stackrel{\·}{\mathrm{\ω}}}_x=\frac{1}{4}[\frac{(1-\frac{x_2}{x_1})}{(Z_2-Z_1)}{b}_2-\frac{1}{y_2}{b}_4-\frac{(3+\frac{y_1}{y_2}+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_2-Z_1)}{b}_6-\frac{1}{y_2}{b}_7+\frac{(\frac{{2y}_1}{y_2}+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_2-Z_1)}{b}_9-\frac{(1+\frac{x_2}{x_1})}{(Z_2-Z_1)}{b}_{10}]---\left(5\right)$

${\stackrel{\·}{\mathrm{\ω}}}_y=\frac{1}{4}\left[\begin{array}{c}\frac{(1-\frac{x_2}{x_1})}{(Z_1-Z_2)}{b}_1-\frac{1}{x_1}{b}_3+\frac{y_1}{x_1{y}_2}{b}_4+\frac{(1+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_1-Z_2)}{b}_5+\frac{y_1}{x_1{y}_2}{b}_7\\ +\frac{(-1+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_1-Z_2)}{b}_8+\frac{(1+\frac{x_2}{x_1})}{(Z_1-Z_2)}{b}_{11}+\frac{1}{x_1}{b}_{12}\end{array}\right]---\left(6\right)$

${\stackrel{\·}{\mathrm{\ω}}}_z=\frac{1}{4}[\frac{1}{x_1}{b}_2+\frac{1}{y_2}{b}_5+\frac{y_1}{x_1{y}_2}{b}_6-\frac{y_1}{x_1{y}_2}{b}_9+\frac{1}{y_2}{b}_8+\frac{1}{x_1}{b}_{10}]---\left(7\right)$

${\mathrm{\ω}}_x^2=\frac{1}{4}\left[\begin{array}{c}\frac{1}{x_1}{b}_1+\frac{(1-\frac{x_2}{x_1})}{(Z_2-Z_1)}{b}_3+\frac{(1+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_2-Z_1)}{b}_4-\frac{y_1}{x_1{y}_2}{b}_5-\frac{1}{y_2}{b}_6+\frac{(-1+\frac{x_1{y}_2}{x_1{y}_2})}{(Z_2-Z_1)}{b}_7\\ -\frac{y_1}{x_1{y}_2}{b}_8+\frac{1}{y_2}{b}_9-\frac{1}{x_1}{b}_{11}+\frac{(1+\frac{x_2}{x_1})}{(Z_2-Z_1)}{b}_{12}\end{array}\right]---\left(8\right)$

${\mathrm{\ω}}_y^2=\frac{1}{4}\left[\begin{array}{c}-\frac{1}{x_1}{b}_1+\frac{(1-\frac{x_2}{x_1})}{(Z_2-Z_1)}{b}_3+\frac{(1+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_2-Z_1)}{b}_4-\frac{y_1}{x_1{y}_2}{b}_5+\frac{1}{y_2}{b}_6+\frac{(-1+\frac{x_1{y}_2}{x_1{y}_2})}{(Z_2-Z_1)}{b}_7\\ +\frac{y_1}{x_1{y}_2}{b}_8-\frac{1}{y_2}{b}_9+\frac{1}{x_1}{b}_{11}+\frac{(1+\frac{x_2}{x_1})}{(Z_2-Z_1)}{b}_{12}\end{array}\right]---\left(9\right)$

${\mathrm{\ω}}_z^2=\frac{1}{4}\left[\begin{array}{c}-\frac{1}{x_1}{b}_1-\frac{(1-\frac{x_2}{x_1})}{(Z_2-Z_1)}{b}_3-\frac{(1+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_2-Z_1)}{b}_4+\frac{y_1}{x_1{y}_2}{b}_5-\frac{1}{y_2}{b}_6-\frac{(-1+\frac{x_1{y}_2}{x_1{y}_2})}{(Z_2-Z_1)}{b}_7\\ +\frac{y_1}{x_1{y}_2}{b}_8+\frac{1}{y_2}{b}_9+\frac{1}{x_1}{b}_{11}-\frac{(1+\frac{x_2}{x_1})}{(Z_2-Z_1)}{b}_{12}\end{array}\right]---\left(10\right)$

${\mathrm{\ω}}_x{\mathrm{\ω}}_y=\frac{1}{4}[\frac{1}{x_1}{b}_2-\frac{1}{y_2}{b}_5+\frac{y_1}{x_1{y}_2}{b}_6-\frac{1}{y_2}{b}_8-\frac{y_1}{x_1{y}_2}{b}_9+\frac{1}{x_1}{b}_{10}]---\left(11\right)$

${\mathrm{\ω}}_x{\mathrm{\ω}}_z=\frac{1}{4}\left[\begin{array}{c}\frac{(1-\frac{x_2}{x_1})}{(Z_1-Z_2)}{b}_1+\frac{1}{x_1}{b}_3-\frac{y_1}{x_1{y}_2}{b}_4+\frac{(1+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_1-Z_2)}{b}_5-\frac{y_1}{x_1{y}_2}{b}_7\\ +\frac{(-1+\frac{x_2{y}_1}{x_1{y}_2})}{(Z_1-Z_2)}{b}_8+\frac{(1+\frac{x_2}{x_1})}{(Z_1-Z_2)}{b}_{11}-\frac{1}{x_1}{b}_{12}\end{array}\right]---\left(12\right)$

${\mathrm{\ω}}_y{\mathrm{\ω}}_z=-\frac{1}{4}\left[\begin{array}{c}\frac{(1-\frac{x_2}{x_1})}{(z_2-z_1)}{b}_2+\frac{1}{y_2}{b}_4-\frac{(3+\frac{y_1}{y_2}+\frac{x_2{y}_1}{x_1{y}_2})}{(z_2-z_1)}{b}_6+\frac{1}{y_2}{b}_7\\ +\frac{(\frac{{2y}_1}{y_2}+\frac{x_2{y}_1}{x_1{y}_2})}{(z_2-z_1)}{b}_9-\frac{(1+\frac{x_2}{x_1})}{(z_2-z_1)}{b}_{10}\end{array}\right]---\left(13\right)$

上面公式中的x₁、y₁、z₁、x₂、y₂、z₂分别为对应三轴向加速度传感器相对于动平台的坐标原点O_b的距离，可自定义其数值；

通过上述公式(2)-(13)求得A_x、A_y、A_z、ω_x、ω_y、ω_z，即可获得动平台坐标系相对于静平台坐标系的位置和姿态参数，即动平台的坐标原点O_b即形心在静平台坐标系中的位置坐标P＝(x,y,z)和旋转参数摇摆角α、纵摇角β及偏摆角γ。