CN103884868B - 一种六维加速度采集方法 - Google Patents

Abstract

一种六维加速度采集方法，涉及一种加速度采集方法。为了解决现有六维加速度采集方法复杂的问题。它包括：建立16加速度计检测空间模型：所述模型呈圆柱体，8个两轴加速度计平均分两组，每4个两轴加速度计相互之间成90度设置且形成所述圆柱体的顶面或底面；顶面的两轴加速度计与底面的两轴加速度计的位置相互对应，且顶面与底面之间的距离为L，所述圆柱体的半径为R；每个两轴加速度计的两个敏感轴方向垂直；按所述模型，将两轴加速度计安装在待测机器人末端上，且以待测机器人末端的中心轴线为所述模型的中心轴；利用两轴加速度计测得待测机器人末端在载体坐标系下的线加速度和角加速度。它用于采集待测机器人末端的六维加速度。

Description

一种六维加速度采集方法

技术领域

本发明涉及一种加速度采集方法，特别涉及一种六维加速度采集方法。

背景技术

近年来，机器人的人机交互能力得到越来越广泛的重视，具有多感知能力、高载荷自重比的机器人成为机器人发展的一个重要的方向。通过机器人结构的变化降低机器人重量是提高机器人载荷自重比的一种主要措施，但随着机器人重量的降低，机器人不可避免的将出现柔性，使得机器人具有典型的柔性的特点。机器人柔性会引起机器人控制困难，引起机器人末端振动等问题。除此之外，高性能的机器人控制器要求能够实现机器人的加速度控制。

机器人末端六维加速度传感器是机械人构成及其控制的核心部件，能够直接影响着机械臂整体的控制精度。机械人末端六维加速度传感器的设计增强了机械臂的人机交互能力，保证了在人机交互中人的安全。机器人末端六维加速度传感器的设计加大的扩展了机器人的任务范围，将在服务型机器人、空间机器人以及特种操作机器人等得到广泛的应用。

但现有六维加速度传感器结构复杂，在采集六维加速度的过程中操作复杂。

发明内容

本发明的目的是为了解决现有六维加速度采集方法复杂的问题，本发明提供一种六维加速度采集方法。

本发明的一种六维加速度采集方法，所述采集方法是基于8个两轴加速度计实现的，所述方法包括如下步骤：

步骤一：建立16加速度计检测空间模型：

所述模型呈圆柱体，4个两轴加速度计相互之间成90度设置且形成所述圆柱体的顶面；另4个两轴加速度计相互之间成90度设置且形成所述圆柱体的底面；顶面的4个两轴加速度计与底面的4个两轴加速度计的位置相互对应，且顶面与底面之间的距离为L，所述圆柱体的半径为R；每个两轴加速度计的两个敏感轴方向垂直；

步骤二：按照步骤一建立的空间模型，将8个两轴加速度计安装在待测机器人末端上，且以待测机器人末端的中心轴线为所述空间模型的中心轴；

步骤三：利用所述8个两轴加速度计测得待测机器人末端在载体坐标系下的线加速度和角加速度；所述载体坐标系为以待测机器人末端的中心轴线为Z轴，载体坐标系的Y轴与X轴和Z轴满足右手坐标系。

所述两轴加速度计的两个敏感轴中的任意一个敏感轴方向与Z轴相同。

在步骤三中，利用所述8个两轴加速度计测得待测机器人末端在载体坐标系下的线加速度和角加速度的方法为：

${\stackrel{\·\·}{p}}_{bx}=\frac{1}{2}(A_{71}-A_{31})$

${\stackrel{\·\·}{p}}_{by}=\frac{1}{2}(A_{11}-A_{51})$

${\stackrel{\·\·}{p}}_{bz}=\frac{1}{2}(A_{32}+A_{72})=\frac{1}{2}(A_{12}+A_{52})$

${\stackrel{\·}{w}}_{bx}=\frac{1}{2L}(A_{61}-A_{51})+\frac{1}{4R}(A_{32}-A_{72})$

${\stackrel{\·}{w}}_{by}=\frac{1}{2L}(A_{31}-A_{41})+\frac{1}{4R}(A_{52}-A_{12})$

${\stackrel{\·}{w}}_{bz}=\frac{1}{4R}(A_{11}+A_{51}+A_{31}+A_{71})$

$w_{bx}{w}_{by}=\frac{1}{4R}(A_{11}+A_{51}-A_{31}-A_{71})$

$w_{by}{w}_{bz}=\frac{1}{4R}(A_{32}-A_{72})-\frac{1}{2L}(A_{61}-A_{51})$

$w_{bx}{w}_{bz}=\frac{1}{2L}(A_{31}-A_{41})-\frac{1}{4R}(A_{52}-A_{12})$

$w_{bx}^2+w_{by}^2=\frac{1}{L}(A_{12}-A_{22})=\frac{1}{L}(A_{32}-A_{42})=\frac{1}{L}(A_{52}-A_{62})=\frac{1}{L}(A_{72}-A_{82});$

其中，和分别为在载体坐标系X轴、Y轴和Z轴方向的角加速度， 和分别为在载体坐标系X轴、Y轴和Z轴方向的线加速度，A_mn为两轴加速度计的输出值，m的取值为[1，8]的整数，表示第m个两轴加速度计，n的取值为1或2，表示两轴加速度计的第n个轴；w_bx、w_by和w_bz分别为在载体坐标系X轴、Y轴和Z轴方向的角速度。

本发明的有益效果在于，本发明具有结构模块化、紧凑化、多感知能力等优点，克服了传统的无陀螺惯性导航结构复杂的缺点。仅需测量传感器的分布圆半径以及传感器布置的轴向高度即可，后期处理计算简单，可以直接测量出三个平动的线性加速度和三个旋转的角加速度以及基座的角速度。在地面环境中，通过对比重力加速度，可以实现传感器的倾角检测。可以广泛的应用于服务机器人、医疗机器人、空间机器人。同时还能够应用于导弹等飞行器的惯性导航。具有结构简单、成本低廉等特点。随着航空航天、机器人、生物医疗等领域向高、精、尖方向发展，全面感知物体的运动信息变得越来越重要，多维加速度传感器因其能够感知更为丰富的运动信息，从而将广泛应用在航空航天、医疗、军事、体育、娱乐等领域。另外，十六加速度计圆柱形模型构型加工简单，对于具有长圆柱形形结构的物体设置可以不需要加工机械支撑件，仅需要安装在圆柱形外壳即可，安装难度容易，且具有较多的冗余量，可以提高无陀螺惯性测量组合的可靠性。

附图说明

图1为具体实施方式一所述的模型的原理示意图。

图2为具体实施方式三中六维加速度传感器的测量原理示意图。

具体实施方式

具体实施方式一：结合图1说明本实施方式，本实施方式所述的一种六维加速度采集方法，所述采集方法是基于8个两轴加速度计实现的，所述方法包括如下步骤：

步骤一：建立16加速度计检测空间模型：

所述模型呈圆柱体，4个两轴加速度计相互之间成90度设置且形成所述圆柱体的顶面；另4个两轴加速度计相互之间成90度设置且形成所述圆柱体的底面；顶面的4个两轴加速度计与底面的4个两轴加速度计的位置相互对应，且顶面与底面之间的距离为L，所述圆柱体的半径为R；每个两轴加速度计的两个敏感轴方向垂直；

步骤二：按照步骤一建立的空间模型，将8个两轴加速度计安装在待测机器人末端上，且以待测机器人末端的中心轴线为所述空间模型的中心轴；

步骤三：利用所述8个两轴加速度计测得待测机器人末端在载体坐标系下的线加速度和角加速度；所述载体坐标系为以待测机器人末端的中心轴线为Z轴，载体坐标系的Y轴与X轴和Z轴满足右手坐标系。

具体实施方式二：本实施方式是对具体实施方式一所述的一种六维加速度采集方法的进一步限定，所述两轴加速度计的两个敏感轴中的任意一个敏感轴方向与Z轴相同。

具体实施方式三：本实施方式是对具体实施方式一所述的一种六维加速度采集方法的进一步限定，

在步骤三中，利用所述8个两轴加速度计测得待测机器人末端在载体坐标系下的线加速度和角加速度的方法为：

${\stackrel{\·\·}{p}}_{bx}=\frac{1}{2}(A_{71}-A_{31})$

${\stackrel{\·\·}{p}}_{by}=\frac{1}{2}(A_{11}-A_{51})$

${\stackrel{\·\·}{p}}_{bz}=\frac{1}{2}(A_{32}+A_{72})=\frac{1}{2}(A_{12}+A_{52})$

${\stackrel{\·}{w}}_{bx}=\frac{1}{2L}(A_{61}-A_{51})+\frac{1}{4R}(A_{32}-A_{72})$

${\stackrel{\·}{w}}_{by}=\frac{1}{2L}(A_{31}-A_{41})+\frac{1}{4R}(A_{52}-A_{12})$

${\stackrel{\·}{w}}_{bz}=\frac{1}{4R}(A_{11}+A_{51}+A_{31}+A_{71})$

$w_{bx}{w}_{by}=\frac{1}{4R}(A_{11}+A_{51}-A_{31}-A_{71})$

$w_{by}{w}_{bz}=\frac{1}{4R}(A_{32}-A_{72})-\frac{1}{2L}(A_{61}-A_{51})$

$w_{bx}{w}_{bz}=\frac{1}{2L}(A_{31}-A_{41})-\frac{1}{4R}(A_{52}-A_{12})$

$w_{bx}^2+w_{by}^2=\frac{1}{L}(A_{12}-A_{22})=\frac{1}{L}(A_{32}-A_{42})=\frac{1}{L}(A_{52}-A_{62})=\frac{1}{L}(A_{72}-A_{82});$

其中，和分别为在载体坐标系X轴、Y轴和Z轴方向的角加速度， 和分别为在载体坐标系X轴、Y轴和Z轴方向的线加速度，A_mn为两轴加速度计的输出值，m的取值为[1，8]的整数，表示第m个两轴加速度计，n的取值为1或2，表示两轴加速度计的第n个轴；w_bx、w_by和w_bz分别为在载体坐标系X轴、Y轴和Z轴方向的角速度。

对于一般的旋转刚体，如图2所示，在惯性坐标系Σ₀和载体坐标系Σ_b中存在有一旋转刚体，对于刚体上的固定点i，图2中p_i和p_b分别表示的是地心(惯性坐标系原点)指向点i和载体坐标系的向量，r_i为载体坐标系原点指向点i的向量在惯性坐标系下表示。根据哥氏定理，i点的速度为

${\stackrel{\·}{p}}_i={\stackrel{\·}{r}}_i+w_b\×p_i---\left(2\right)$

其中w_b表示载体坐标系下的角速度；

对(2)取微分可得

${\stackrel{\·\·}{p}}_i={\stackrel{\·\·}{r}}_i+2w_b\×{\stackrel{\·}{r}}_i+{\stackrel{\·}{w}}_b\×r_i+w_b\×(w_b\×r_i)---\left(3\right)$

其中为科式加速度，是由载体坐标系的转动，牵连运动和相对运动的相互影响造成的，w_b×(w_b×r_i)为向心加速度，为切向加速度。

由于点i在刚体上是固定点，即所以(3)可以写成

${\stackrel{\·\·}{p}}_i={\stackrel{\·}{w}}_b\×r_i+w_b\×(w_b\×r_i)---\left(4\right)$

点i在惯性系下表示为

p_i＝p_b+r_i(5)

对(5)做二阶微分可得

${\stackrel{\·\·}{p}}_i={\stackrel{\·\·}{p}}_b+{\stackrel{\·\·}{r}}_i---\left(6\right)$

将(4)代入到(6)可得

${\stackrel{\·\·}{p}}_i={\stackrel{\·\·}{p}}_b+g+{\stackrel{\·}{w}}_b\×r_i+w_b\×(w_b\×r_i)---\left(7\right)$

由于加速度计的输出值为作用在加速度计上的单位质量对应的惯性力(比力specificforce)，u_i表示的是敏感轴的安装方向，所以加速度计的输出为

$a_i=u_i^T{\stackrel{\·\·}{p}}_i=\[\begin{array}{cc}{u}_i^T& -u_i^T{R}_i\end{array}\]\left[\begin{array}{c}{\stackrel{\·\·}{p}}_b+g\\ {\stackrel{\·}{w}}_b\end{array}\right]+u_i^T\{w_b\×(w_b\×r_i)\}---\left(8\right)$

其中：

${\stackrel{\·\·}{p}}_i={\stackrel{\·\·}{p}}_b+g+{\stackrel{\·}{w}}_b\×r_i+w_b\×(w_b\×r_i)$

R_i＝(r_i×)(9)

u_i＝[u_ix,u_iy,u_iz]^T

w_b＝(w_bxw_byw_bz)^T为载体坐标系相对于惯性坐标系的角速度，令w_b的反对称矩阵(w_b×)为 ${\mathrm{\Ω}}_b=\left[\begin{array}{ccc}0& -w_{\mathrm{bz}}& w_{\mathrm{by}}\\ w_{\mathrm{bz}}& 0& -w_{\mathrm{bx}}\\ -w_{\mathrm{by}}& w_{\mathrm{bx}}& 0\end{array}\right],$ 那么 ${\mathrm{\Ω}}_b^2=\left[\begin{array}{ccc}-w_{by}^2-w_{bz}^2& w_{bx}{w}_{by}& w_{bx}{w}_{bz}\\ w_{bx}{w}_{by}& -w_{bx}^2-w_{bz}^2& w_{by}{w}_{bz}\\ w_{bx}{w}_{bz}& w_{by}{w}_{bz}& -w_{bx}^2-w_{by}^2\end{array}\right]$

假设存在线加速度计构型矩阵J₁＝[u₁u₂…u_N]和角加速度构型矩阵 $J_2=\[\begin{array}{cccc}-R_1^T{u}_1& -R_2^T{u}_2& ...& -R_N^T{u}_N\end{array}\],$ 当有N个敏感轴时(8)可以写成

$\left[\begin{array}{c}{a}_1\\ .\\ .\\ .\\ a_N\end{array}\right]=\[\begin{array}{cc}{J}_1^T& J_2^T\end{array}\]\left[\begin{array}{c}{\stackrel{\·\·}{p}}_b+g\\ {\stackrel{\·}{w}}_b\end{array}\right]+\left[\begin{array}{c}{u}_1^T{\mathrm{\Ω}}_b^2{r}_1\\ .\\ .\\ .\\ u_2^T{\mathrm{\Ω}}_b^2{r}_2\end{array}\right]---\left(10\right)$

令 $J=\[\begin{array}{cc}{J}_1^T& J_2^T\end{array}\],$ J为构型矩阵。如果想要求出六维加速度，只要满足矩阵J可逆。也就是说rank(J)＝6。这样，加速度计的数量必须满足N≥6。因此，想要完整的计算出六维加速度，至少需要六个单轴加速度计，通过合理的设计加速度计的位置和敏感轴方向，使得构型矩阵J可逆。

令 $J=\[\begin{array}{cc}{J}_1^T& J_2^T\end{array}\],$ J为构型矩阵；rank(J)＝6；加速度计的数量N≥6；使得构型矩阵J可逆，确定各个加速度计在待测机器人末端上的位置和敏感轴方向。

相对于六加速度计、九加速度计和十二加速度计，十六加速度计圆柱形布置的以及GDOP指标的改善较大，能够大幅度的提高六维加速度的计算精度，采用多传感器数据融合的方法提高导航精度。而且加速度计敏感方向均与载体坐标系坐标轴平行，有利于无陀螺惯性测量组合的工程实现，除此之外，十六加速度计圆柱形构型加工简单，对于具有长圆柱形结构的物体设置可以不需要加工机械支撑件，仅需要安装在圆柱形外壳即可，安装难度容易，且具有较多的冗余量，可以提高无陀螺惯性测量组合的可靠性。

为线加速度几何精度影响因子，表示线加速度计的安装误差对载体线加速度测量精度的影响程度，、为角加速度几何精度影响因子，表示线加速度计的安装误差对载体角加速度测量精度的影响程度，GDOP(GeometricDilutionofPrecision几何精度因子)在卫星导航中用于表示卫星的分布误差对地面导航精度的影响，在这里用来表示线加速度计的安装误差对于加速度求解结果的影响程度。

所以本实施方式中，选择加速度计的数量N＝16，

$r_i=\left[\begin{array}{cccccccccccccccc}R& R& R& R& 0& 0& 0& 0& -R& -R& -R& -R& 0& 0& 0& 0\\ 0& 0& 0& 0& R& R& R& R& 0& 0& 0& 0& -R& -R& -R& -R\\ 0& 0& L& L& 0& 0& L& L& 0& 0& L& L& 0& 0& L& L\end{array}\right]---\left(11\right)$

$u_i=\left[\begin{array}{cccccccccccccccc}0& 0& 0& 0& -1& 0& -1& 0& 0& 0& 0& 0& 1& 0& 1& 0\\ 1& 0& 1& 0& 0& 0& 0& 0& -1& 0& -1& 0& 0& 0& 0& 0\\ 0& 1& 0& 1& 0& 1& 0& 1& 0& 1& 0& 1& 0& 1& 0& 1\end{array}\right]---\left(12\right)$

$A_{11}={\stackrel{\·\·}{p}}_{by}+R{\stackrel{\·}{w}}_{bz}+{\mathrm{Rw}}_{bx}{w}_{by}$

$A_{12}={\stackrel{\·\·}{p}}_{bz}-R{\stackrel{\·}{w}}_{by}+{\mathrm{Rw}}_{bx}{w}_{bz}$

$A_{21}={\stackrel{\·\·}{p}}_{by}-L{\stackrel{\·}{w}}_{bx}+R{\stackrel{\·}{w}}_{bz}+{\mathrm{Lw}}_{by}{w}_{bz}+{\mathrm{Rw}}_{bx}{w}_{by}$

$A_{22}={\stackrel{\·\·}{p}}_{bz}-R{\stackrel{\·}{w}}_{by}-L(w_{bx}^2+w_{by}^2)+{\mathrm{Rw}}_{bx}{w}_{bz}$

$A_{31}=-{\stackrel{\·\·}{p}}_{bx}+R{\stackrel{\·}{w}}_{bz}-{\mathrm{Rw}}_{bx}{w}_{by}$

$A_{41}=-{\stackrel{\·\·}{p}}_{bx}+R{\stackrel{\·}{w}}_{bz}-L{\stackrel{\·}{w}}_{by}-{\mathrm{Lw}}_{bx}{w}_{bz}-{\mathrm{Rw}}_{bx}{w}_{by}$

$A_{42}={\stackrel{\·\·}{p}}_{bz}+R{\stackrel{\·}{w}}_{bx}-L(w_{bx}^2+w_{by}^2)+{\mathrm{Rw}}_{by}{w}_{bz}$

$A_{51}=-{\stackrel{\·\·}{p}}_{by}+R{\stackrel{\·}{w}}_{bz}+{\mathrm{Rw}}_{bx}{w}_{by}$

$A_{52}={\stackrel{\·\·}{p}}_{bz}+R{\stackrel{\·}{w}}_{by}-{\mathrm{Rw}}_{bx}{w}_{bz}$

$A_{61}=-{\stackrel{\·\·}{p}}_{by}+L{\stackrel{\·}{w}}_{bx}+R{\stackrel{\·}{w}}_{bz}-{\mathrm{Lw}}_{by}{w}_{bz}+{\mathrm{Rw}}_{bx}{w}_{by}$

$A_{62}={\stackrel{\·\·}{p}}_{bz}+R{\stackrel{\·}{w}}_{by}-L(w_{bx}^2+w_{by}^2)-{{\mathrm{Rw}}_{bx}}^0{w}_{bz}$

$A_{71}={\stackrel{\·\·}{p}}_{bx}+R{\stackrel{\·}{w}}_{bz}-{\mathrm{Rw}}_{bx}{w}_{by}$

$A_{72}={\stackrel{\·\·}{p}}_{bz}-R{\stackrel{\·}{w}}_{bx}-{\mathrm{Rw}}_{by}{w}_{bz}$

$A_{81}={\stackrel{\·\·}{p}}_{bx}+R{\stackrel{\·}{w}}_{bz}+L{\stackrel{\·}{w}}_{by}+{\mathrm{Lw}}_{bx}{w}_{bz}-{\mathrm{Rw}}_{bx}{w}_{by}$

$A_{82}={\stackrel{\·\·}{p}}_{bz}-R{\stackrel{\·}{w}}_{bx}-L(w_{bx}^2+w_{by}^2)-{\mathrm{Rw}}_{by}{w}_{bz}---\left(13\right).$

具体实施方式四：本实施方式是对具体实施方式三所述的一种六维加速度采集方法的进一步限定，载体坐标系X轴、Y轴和Z轴方向的角速度的求取方法为：

利用积分法或开方法或微分法或对数法求取载体坐标系X轴、Y轴和Z轴方向的角速度的值。

在无陀螺惯导中，由于加速度计的输出中存在角速度的交叉乘积项，这就为角速度的计算提供了很多方法，主要有积分法、开方法、微分法或对数法。

1.积分法

对角加速度的结果直接积分便可以得到角速度的结果，主要的计算方法为

$\left[\begin{array}{c}{w}_{bx}\left(t\right)\\ w_{by}\left(t\right)\\ w_{bz}\left(t\right)\end{array}\right]=\left[\begin{array}{c}{w}_{bx}(t-T)+{\∫}_{t-T}^t{\stackrel{\·}{w}}_{bx}\left(t\right)dt\\ w_{by}(t-T)+{\∫}_{t-T}^t{\stackrel{\·}{w}}_{by}\left(t\right)dt\\ w_{bz}(t-T)+{\∫}_{t-T}^t{\stackrel{\·}{w}}_{bz}\left(t\right)dt\end{array}\right]---\left(14\right)$

从式(14)中可以看出，通过角加速度直接积分计算角速度使得角速度计算结果存在较大的累积误差，因此不能长时间使用。

2.开方法

由于加速度计的输出中存在角速度的交叉乘积项，因此三个不同的角速度交叉项的乘积除以每个加速度乘积项的平方可以得出角速度的平方项，对角速度平方项直接开放便可以得到角速度的绝对值，角速度符号的确定可以通过对角加速度的积分得到。这样可以消除加速度计累积误差的影响，但计算量较大，需要进行符号判断，且在角速度接近0时误差较大。

3.微分法

以z轴为例，根绝微分的定义可以得出

${\stackrel{\·}{w}}_{bx}\left(t\right)=\frac{d\left(w_{bx}\right(t\left)\right)}{dt}\≈\frac{{\mathrm{\Δw}}_{bx}\left(t\right)}{T}$

${\stackrel{\·}{w}}_{by}\left(t\right)=\frac{d\left(w_{by}\right(t\left)\right)}{dt}\≈\frac{{\mathrm{\Δw}}_{by}\left(t\right)}{T}---\left(15\right)$

其中T为采样周期。

而加速度交叉乘积项

w_bx(t-T)w_by(t-T)＝(w_bx(t)-△w_bx(t))(w_by(t)-△w_by(t))(16)

将(15)代入到(16)中可得

$\begin{array}{c}{w}_{bx}(t-T)w_{by}(t-T)=\left(w_{bx}\right(t)-{\mathrm{\Δw}}_{bx}(t\left)\right)\left(w_{by}\right(t)-{\mathrm{\Δw}}_{by}(t\left)\right)\\ =\left(w_{bx}\right(t)-T{\stackrel{\·}{w}}_{bx}(t\left)\right)\left(w_{by}\right(t)-T{\stackrel{\·}{w}}_{by}(t\left)\right)\\ =w_{bx}\left(t\right)w_{by}\left(t\right)-{\mathrm{Tw}}_{bx}\left(t\right){\stackrel{\·}{w}}_{by}\left(t\right)-T{\stackrel{\·}{w}}_{bx}\left(t\right)w_{by}\left(t\right)+T^2{\stackrel{\·}{w}}_{bx}\left(t\right){\stackrel{\·}{w}}_{by}\left(t\right)\\ =w_{bx}\left(t\right)w_{by}\left(t\right)-T{\stackrel{\·}{w}}_{by}\left(t\right)\frac{w_{bx}\left(t\right)w_{bz}\left(t\right)}{{w_0}_{bz}\left(t\right)}-T{\stackrel{\·}{w}}_{bx}\left(t\right)\frac{{w_0}_{by}\left(t\right){w_0}_{bz}\left(t\right)}{{w_0}_{bz}\left(t\right)}+T^2{\stackrel{\·}{w}}_{bx}\left(t\right){\stackrel{\·}{w}}_{by}\left(t\right)\end{array}---\left(17\right)$

由此可以得到z轴角速度的估计值为

${\hat{w}}_{bz}\left(t\right)=\frac{T{\stackrel{\·}{w}}_{by}\left(t\right)w_{bx}\left(t\right)w_{bz}\left(t\right)+T{\stackrel{\·}{w}}_{bx}\left(t\right)w_{by}\left(t\right)w_{bz}\left(t\right)}{w_{bx}\left(t\right)w_{by}\left(t\right)-w_{bx}(t-T)w_{by}(t-T)+T^2{\stackrel{\·}{w}}_{bx}\left(t\right){\stackrel{\·}{w}}_{by}\left(t\right)}$

同理可以得到和

${\hat{w}}_{bx}\left(t\right)=\frac{T{\stackrel{\·}{w}}_{bz}\left(t\right)w_{bx}\left(t\right)w_{by}\left(t\right)+T{\stackrel{\·}{w}}_{by}\left(t\right)w_{bx}\left(t\right)w_{bz}\left(t\right)}{w_{by}\left(t\right)w_{bz}\left(t\right)-w_{by}(t-T)w_{bz}(t-T)+T^2{\stackrel{\·}{w}}_{by}\left(t\right){\stackrel{\·}{w}}_{bz}\left(t\right)}$

${\hat{w}}_{by}\left(t\right)=\frac{T{\stackrel{\·}{w}}_{bz}\left(t\right)w_{bx}\left(t\right)w_{by}\left(t\right)+T{\stackrel{\·}{w}}_{bx}\left(t\right)w_{by}\left(t\right)w_{bz}\left(t\right)}{w_{bx}\left(t\right)w_{bz}\left(t\right)-w_{bx}(t-T)w_{bz}(t-T)+T^2{\stackrel{\·}{w}}_{bx}\left(t\right){\stackrel{\·}{w}}_{bz}\left(t\right)}$

该方法有以下的特点：与积分法相比，消除了累积误差的影响；与开方法相比，计算量小，不需要进行符号判断。但其微分应用近似的方法，所以当采样周期较大时，存在较大的误差。

4.对数法

由于加速度计的输出中存在角速度的交叉乘积项，对角速度的交叉乘积项取对数可得

lg(w_bx(t)w_by(t))＝lg(w_bx(t))+lg(w_by(t))

lg(w_by(t)w_bz(t))＝lg(w_by(t))+lg(w_bz(t))

lg(w_bx(t)w_bz(t))＝lg(w_bx(t))+lg(w_bz(t))(18)所以

$\lg \left(w_{bx}\right(t\left)\right)=\frac{1}{2}\left(\lg \right(w_{bx}\left(t\right)w_{by}\left(t\right))-\lg (w_{by}\left(t\right)w_{bz}\left(t\right))+\lg (w_{bx}\left(t\right)w_{bz}\left(t\right)\left)\right)$

$\lg \left(w_{by}\right(t\left)\right)=\frac{1}{2}\left(\lg \right(w_{bx}\left(t\right)w_{by}\left(t\right))+\lg (w_{by}\left(t\right)w_{bz}\left(t\right))-\lg (w_{bx}\left(t\right)w_{bz}\left(t\right)\left)\right)$

$\lg \left(w_{bz}\right(t\left)\right)=\frac{1}{2}(-\lg (w_{bx}\left(t\right)w_{by}\left(t\right))+\lg (w_{by}\left(t\right)w_{bz}\left(t\right))+\lg (w_{bx}\left(t\right)w_{bz}\left(t\right)\left)\right)---\left(19\right)$

由此可得

$w_{by}\left(t\right)={10}^{\left(\frac{1}{2}\right(\lg \left(w_{bx}\left(t\right)w_{by}\left(t\right)\right)+\lg \left(w_{by}\left(t\right)w_{bz}\left(t\right)\right)-\lg \left(w_{bx}\left(t\right)w_{bz}\left(t\right)\right)\left)\right)}$

$w_{bz}\left(t\right)={10}^{\left(\frac{1}{2}\right(-\lg \left(w_{bx}\left(t\right)w_{by}\left(t\right)\right)+\lg \left(w_{by}\left(t\right)w_{bz}\left(t\right)\right)+\lg \left(w_{bx}\left(t\right)w_{bz}\left(t\right)\right)\left)\right)}---\left(20\right)$

该方法也需要对加速度的符号进行判断。

为保证六维加速度传感器长时间工作的要求，优先选择微分法计算载体坐标系的角速度。

Claims (4)

1.一种六维加速度采集方法，其特征是在于，所述采集方法是基于8个两轴加速度计实现的，所述方法包括如下步骤：步骤一：建立16加速度计检测空间模型：

所述模型呈圆柱体，4个两轴加速度计相互之间成90度设置且形成所述圆柱体的顶面；另4个两轴加速度计相互之间成90度设置且形成所述圆柱体的底面；顶面的4个两轴加速度计与底面的4个两轴加速度计的位置相互对应，且顶面与底面之间的距离为L，所述圆柱体的半径为R；每个两轴加速度计的两个敏感轴方向垂直；

步骤二：按照步骤一建立的空间模型，将8个两轴加速度计安装在待测机器人末端上，且以待测机器人末端的中心轴线为所述空间模型的中心轴；

步骤三：利用所述8个两轴加速度计测得待测机器人末端在载体坐标系下的线加速度和角加速度；所述载体坐标系为以待测机器人末端的中心轴线为Z轴，载体坐标系的Y轴与X轴和Z轴满足右手坐标系。

2.根据权利要求1所述的一种六维加速度采集方法，其特征在于，所述两轴加速度计的两个敏感轴中的任意一个敏感轴方向与Z轴相同。

3.根据权利要求2所述的一种六维加速度采集方法，其特征在于，

在步骤三中，利用所述8个两轴加速度计测得待测机器人末端在载体坐标系下的线加速度和角加速度的方法为：

其中，和分别为在载体坐标系X轴、Y轴和Z轴方向的角加速度，和分别为在载体坐标系X轴、Y轴和Z轴方向的线加速度，A_mn为两轴加速度计的输出值，m的取值为[1，8]的整数，表示第m个两轴加速度计，n的取值为1或2，表示两轴加速度计的第n个轴；w_bx、w_by和w_bz分别为在载体坐标系X轴、Y轴和Z轴方向的角速度。

4.根据权利要求3所述的一种六维加速度采集方法，其特征在于，

载体坐标系X轴、Y轴和Z轴方向的角速度的求取方法为：

利用积分法或开方法或微分法或对数法求取载体坐标系X轴、Y轴和Z轴方向的角速度的值。