CN106705954A - 浮球平台载体姿态测量方法 - Google Patents

Abstract

本发明提供一种浮球平台载体姿态测量方法，该方法通过光电传感器测量载体相对球体的转动经过的球壳内表面上的弧长，根据测量信息解算出载体当前时刻转动的转轴方向，然后结合光电传感器的安装位置解算出载体当前时刻转动角度，最后根据四元数乘法计算初始时刻到当前时刻载体转动的四元数，从而解算出该载体当前的姿态。本发明提供的方法结构简单，只需在球体上安装光电传感器，无需在球壳上安装辅助设备，易于实现时间同步；光电传感器只敏感测量平面内的相对位移，所得结果能避免受到球体晃动的影响，准确性得到提高。

Description

浮球平台载体姿态测量方法

技术领域

本发明涉及惯性导航技术领域，特别的涉及一种针对浮球平台的载体姿态测量方法，尤其是能采用光电传感器实现载体的非接触全姿态测量。

背景技术

在惯性导航领域，浮球式惯性平台(简称：浮球平台)属于平台式惯导系统，相比于捷联式导航系统，其能够为载体提供更加安静稳定的测量环境，适用于无源的长航时导航。浮球平台是一种新型惯性平台，多用于远程战略导弹和远程轰炸机的惯性导航中。其功能与传统的惯性导航设备相同，能为载体提供实时的位置、速度和姿态信息。传统的惯性平台为框架式惯性平台，其台体有三个框架支承，台体上安装三个相互正交的陀螺仪及三个相互正交的加速度计，其中陀螺仪用于敏感台体相对惯性空间的转动，通过控制系统，驱动框架转动，使台体在惯性空间保持稳定，在此基础上，三个正交的加速度计可直接测量载体加速度在惯性空间中的分量，通过框架上的编码器测量框架转动，根据框架角解算出载体的姿态。

浮球平台采用液体静压支承系统代替框架平台的框架系统，浮球平台的台体是一个密封的球体，其置于一个与载体固连的球壳中，球体与球壳间充满液体，球体与球壳由液体隔开，球体内部装有液体循环装置，用于产生液体静压支承作用和液体射流反推控制力矩，液体的静压支承能够有效的削弱载体转动对台体姿态的影响，同时也能产生良好的减振性能。由于液体的隔离作用，浮球平台能够实现载体的全姿态测量，目前的浮球平台对于载体姿态的测量采用电容姿态带的方法，在球体表面安装三条相互正交的激励带，在球壳的两个半球咬合处安装一条接收带，在三条激励带上施加不同频率的正弦信号，激励带和接收带间的电容变化规律可影响接收带敏感到的交流电信号，通过设计合理的解算方案，从而得到载体的姿态。但是，这种方法对于交流电信号的检测具有较高的要求，而且在实际工作过程中，电容式姿态测量系统存在如下问题：激励信号与接收信号分别作用在球体和球壳上，时间同步较难，接收的交流电信号噪声大，信号不稳定；球体的晃动会改变激励带和接收带的间距，进而造成电容不规律的改变；接收带相对于激励带发生倾斜时，以及激励带处于接收带交叉位置时都需要引入判断机制，在这些情况下，所需解算方案复杂，运算耗时长。

发明内容

本发明的目的在于提供一种浮球平台载体姿态测量方法，该发明解决了现有技术中电容式姿态测量方法对于交流电信号的检测要求较高；激励信号与接收信号时间同步难，接收的交流电信号噪声大，信号不稳定；电容不规律的改变；接收带相对于激励带发生倾斜时，以及激励带处于接收带交叉位置时所需引入判断机制，解算方案复杂，运算耗时长等问题。

本发明提供了一种浮球平台载体姿态测量方法，能够实现载体姿态的非接触全姿态测量。

进一步地，通过光电传感器测量得到载体转动的线速度。

进一步地，根据线速度和角速度的关系，利用最小二乘估计方法得到载体的转轴方向。

进一步地，根据光电传感器的安装位置计算出各光电传感器测量弧长对应的载体转动角度，将其均值作为载体的转动角度，构造出载体当前时刻的转动四元数。

进一步地，根据四元数乘法得到初始时刻到当前时刻载体的转动四元数，并基于此解算出载体当前的姿态。

相对现有技术，本发明的技术效果：

本发明提供的浮球平台载体姿态测量方法，通过将光电传感器安装于球体表面，敏感球壳相对球体的转动，从而辨识出载体的姿态，该方法通过对由光电传感器检测得到的结果进行处理分析，实现载体的非接触全姿态测量。通过采用光电传感器，该方法不需要复杂的交流电信号检测技术，只需读取光电传感器测量球壳相对球体转动的弧长。即可得到结果。

本发明提供的浮球平台载体姿态测量方法，该方法尤其适用于使用光电传感器的浮球平台载体姿态测量，采用光电传感器后，①只需在球体上安装光电传感器，无需在球壳上安装辅助设备，无需将激励信号与接收信号分别作用在球体和球壳上，易于实现时间同步；②所安装光电传感器只敏感测量平面内的相对位移，从而避免光电传感器受球体晃动影响，导致检测结果不准确。

本发明提供的浮球平台载体姿态测量方法，①所用光电传感器为数字传感器，输出噪声小，且测量数据稳定；②所需解算方案简捷，可直接计算出载体当前转动的四元数，利用四元数乘法计算载体从初始时刻到当前时刻的转动四元数，运算量小，计算误差积累小，实时性好；③安装多个光电传感器可得到冗余的测量数据，通过数据融合可提高姿态测量的精度。

具体请参考根据本发明的浮球平台载体姿态测量方法提出的各种实施例的如下描述，将使得本发明的上述和其他方面显而易见。

附图说明

图1是本发明提供浮球平台载体姿态测量方法的流程示意图；

图2是本发明优选实施例中光电传感器的安装示意图；

图3是本发明优选实施例1中针对解算位和设定姿态位所得俯仰角曲线、偏航角曲线、滚动角曲线对比图以及针对解算位和设定姿态位所得俯仰角、偏航角和滚动角的姿态角偏差曲线图；

图4是本发明优选实施例中载体转轴方向示意图；

图5是本发明优选实施例2中针对解算位和设定姿态位所得俯仰角曲线、偏航角曲线、滚动角曲线对比图以及针对解算位和设定姿态位所得俯仰角、偏航角和滚动角的姿态角偏差曲线图。

具体实施方式

构成本申请的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。

参见图1，本发明提供了一种浮球平台载体姿态测量方法，包括以下步骤：

步骤S100：预设载体初始时刻的姿态获取光电传感器数据，通过光电传感器测量得到载体转动的线速度，采用最小二乘估计方法解算得到当前时刻的载体转轴方向；

步骤S200：根据各光电传感器的安装位置计算出各光电传感器测量所得弧长对应的当前时刻的载体转动角度，将各光电传感器测得的载体转动角度的均值作为载体的转动角度，并构造载体当前时刻的转动四元数；

步骤S300：解算初始时刻到当前时刻的载体转动四元数，并根据该载体转动四元数解算得到载体在当前时刻的姿态。

解算载体姿态时，可采用常规方法由四元数转换为姿态角的方法进行。由(12)式得到初始时刻到当前时刻的载体转动四元数可解算出载体姿态的方向余弦阵，再设定载体转序则可计算出载体的姿态角。

S100和S200构造出当前转动的四元数，S300则利用四元数乘法计算初始时刻到当前时刻的转动四元数，本发明提供方法有别于捷联惯性导航系统中用陀螺测量数据积分的方法，减小了积分产生的误差积累。

本发明提供的方法针对浮球平台载体姿态的非接触测量问题，充分利用光电传感器敏感相对位移的特性。由于光电传感器为数字传感器，因此不涉及电容姿态带方法中复杂的交流电检测技术，简化了载体姿态解算方法。

优选的，光电传感器数量为8个，均为数字传感器，安装在球体表面，利用多个光电传感器的数据，测算结果更准确。

优选的，本发明提供的浮球平台载体姿态测量方法具体包括以下步骤：

步骤1：姿态解算准备；

给定载体初始姿态根据光电传感器的安装位置计算以下结构参数：

R_c0i：球体坐标系中球心C到光电坐标系原点P_i的矢量，i＝1,2,...,m；

M_PEi2C：光电坐标系到球体坐标系的转换矩阵，i＝1,2,...,m；

Δt：测量周期，又称采样周期；

加速度观测矩阵，用于后续的最小二乘估计载体转动角速度。

步骤2：采集光电传感器数据并保存；

读取光电传感器测量数据其中，下标i＝1,2,...,m表示m个光电传感器，上标n表示当前时刻，保存数据；

步骤3：解算当前时刻的载体转轴方向；

根据测量周期Δt得到n-1到n时刻载体转动的线速度在光电坐标系中的表达式：

根据光电坐标系到球体坐标系的转换矩阵M_PEi2C将载体转动线速度转换到球体坐标系中：

另外，载体线速度和角速度满足关系：

其中，R_c0i为球体坐标系中球心C到光电坐标系原点P_i的矢量。将(2)式代入(3)式可得到：假设光电传感器测量得到的线速度为观测值，令考虑测量噪声则可构建载体转动的观测方程：

采用最小二乘估计方法得到载体n-1到n时刻的角速度：

将角速度单位化后得到n-1到n时刻载体转动的转轴方向：

步骤4：解算当前时刻的载体转动角度；

计算n-1到n时刻光电传感器测量球体转动得到的弧长：

该弧长对应的半径由下式计算：

则可得到第i个光电传感器测量得到的载体转动角度：

将m个光电传感器解算出的转动角度进行平均作为n-1到n时刻的转动角度：

步骤5：构造当前时刻的载体转动四元数；

根据n-1到n时载体的转轴方向和转动角度得到n-1到n时载体转动四元数在球体坐标系中的表达式：

步骤6：计算初始时刻到当前时刻的载体转动四元数；

根据四元数乘法可得到从0时刻到n时刻载体转动的四元数：

其中，在n-1时刻已求出，表示n-1时刻到n时刻球壳的转动四元数，对应步骤5得到的但是，中转动向量应该在当前的球壳坐标系中，因此，需要将中的载体转动轴进行变换：

在当前的球壳坐标系中，可表示为：

将(14)式代入(12)式便可得到初始时刻到当前时刻的载体转动四元数

步骤7：解算载体姿态；

由步骤6得到初始时刻到当前时刻的载体转动四元数可解算出载体姿态的方向余弦阵，再设定载体转序则可计算出载体的姿态角。

本发明针对浮球平台载体姿态的非接触测量问题，结合光电传感器敏感相对位移的特性，首先通过光电传感器测量得到载体转动的线速度，根据线速度和角速度的关系，结合m个光电传感器的测量结果，利用最小二乘估计方法得到载体转轴方向，然后根据光电传感器的安装位置计算出各光电传感器测量弧长对应的载体转动角度，将其均值作为载体的转动角度，构造出载体当前时刻转动的四元数，最后根据四元数乘法得到初始时刻到当前时刻载体的转动四元数，基于此可解算出载体当前的姿态。

以下结合具体实施例，对本发明提供的浮球平台载体姿态测量方法做进一步说明，该具体实例中所用浮球平台如图2所示，球体表面均匀分布有8个悬浮垫，悬浮垫的位置为球体内接正方体的8个顶点，在球体坐标系中即为8个卦限的中心位置，光电传感器安装在悬浮垫内，即m＝8，各悬浮垫中心P_i处安装了光电传感器，其具体步骤如下：

步骤1：姿态解算准备；

给定初始的载体姿态，以四元数形式给出，假设初始时刻载体坐标系与球体坐标系重合，则初始载体姿态用四元数表示为：

装载固定参数：R_c0i为球体坐标系中球心C到光电坐标系原点P_i的矢量，根据浮球平台结构设计，R_c0i的长度为416mm，其方向指向球体坐标系每个卦限的中心位置，8个光电传感器原点矢径为：

同时，系数矩阵也可计算得到，该矩阵为24×3矩阵；

根据光电传感器的具体安装位置，光电坐标系到球体坐标系的转换矩阵为：

采样周期Δt＝20ms

开始采集时，光电输出为0，i＝1,2,...,8；

设定壳体转动轨迹：按当前球壳坐标系，绕Z轴以10°/s转动10°，绕Y轴以20°/s转动40°，绕X轴以50°/s转动100°

步骤2：采集光电传感器数据并保存；

读取光电传感器测量数据i＝1,2,...,8，n≥2。以n＝2为例，其读数为：

步骤3：解算当前时刻的载体转轴方向；

根据采样时间和数据计算相邻采样周期内的转动速度，其在光电坐标系中的表达式为以n＝2为例，各光电传感器测量得到的载体转动线速度为：

利用设定好的坐标转换矩阵将光电传感器测量得到的载体转动线速度转换到球体坐标系中各光电传感器的测量线速度可表示为：

构造线速度观测矢量这是一个24维的矢量，根据最小二乘估计得到载体在1-2时刻的转动角速度：

单位化后得到1-2时刻载体转动轴向量：

步骤4：解算转动角度；

根据1，2时刻的数据得到各光电传感器测量出的弧长：

计算各光电传感器测量弧长的半径：

根据弧长和相应的半径即可得到各光电传感器测量得到的载体转动角度：

将8个解算得到的转角进行平均作为载体的转角：

步骤5：得到n-1到n时刻载体转动轴和转动角φⁿ，可得到载体转动的四元数以1-2时刻为例，1-2时刻的载体转动可由四元数表示为：

步骤6：解算当前时刻的载体四元数；

以1-2时刻为例，为初始时刻的四元数，根据步骤1的初始化条件可设为：

步骤3得到1-2时刻的转轴方向：将其转换到2时刻的球壳坐标系中可得：

因此，1-2时刻载体转动在当前球壳坐标系中可表示为：

综上可得0-2时刻载体转动四元数：

步骤7：重复步骤2-步骤6即可得到任意时刻n的载体姿态四元数，进而可得到载体的姿态角，设定转序为“ZYX”，图3给出了采用本发明提供方法得到的解算结果和设定姿态(姿态参数为：绕Z轴以10°/s转动10°，绕Y轴以20°/s转动40°，绕X轴以50°/s转动100°)的对比图，姿态角解算误差小于0.002°，每个采样周期解算耗时0.6ms，具有较好的实时性，本方法能够很好的跟踪载体的转动。

为了验证本发明的适应性，设定转轴方向为俯仰角在[-90°,90°]内任意取一值θ(转轴与XY平面的夹角)，方位角在[0°,360°)内任意取一值(转轴在XY平面的投影与X轴的夹角)，如图4所示，采样周期仍为20ms，每个采样周期内转动角度为[0°,1°]内任意取一值，转动200s，姿态解算结果与设定姿态的对比图如图5所示，姿态解算误差保持在0.04°以内，相对于上一案例中所处理的简单转动，图4所示转动的设定具有随机性，在200s内任意设定转动轨迹，所得结果误差较小，充分说明了本方法具有较好的适应性和测量精度。

本领域技术人员将清楚本发明的范围不限制于以上讨论的示例，有可能对其进行若干改变和修改，而不脱离所附权利要求书限定的本发明的范围。尽管己经在附图和说明书中详细图示和描述了本发明，但这样的说明和描述仅是说明或示意性的，而非限制性的。本发明并不限于所公开的实施例。

通过对附图，说明书和权利要求书的研究，在实施本发明时本领域技术人员可以理解和实现所公开的实施例的变形。在权利要求书中，术语“包括”不排除其他步骤或元素，而不定冠词“一个”或“一种”不排除多个。在彼此不同的从属权利要求中引用的某些措施的事实不意味着这些措施的组合不能被有利地使用。权利要求书中的任何参考标记不构成对本发明的范围的限制。

Claims (7)

1.一种浮球平台载体姿态测量方法，其特征在于，包括以下步骤：

步骤S100：预设载体初始时刻的姿态获取光电传感器数据，通过光电传感器测量得到载体转动的线速度，采用最小二乘估计方法解算得到当前时刻的载体转轴方向；

步骤S200：根据各光电传感器的安装位置计算出各光电传感器测量所得弧长对应的当前时刻的载体转动角度，将各光电传感器测得的载体转动角度的均值作为载体的转动角度，并构造载体当前时刻的转动四元数；

步骤S300：解算初始时刻到当前时刻的载体转动四元数，并根据该载体转动四元数解算得到载体在当前时刻的姿态。

2.根据权利要求1所述的浮球平台载体姿态测量方法，其特征在于，所述步骤S100中采用最小二乘估计方法解算得到当前时刻的载体转轴方向包括以下步骤：

根据测量周期Δt得到n-1到n时刻载体转动的线速度在光电坐标系中的表达式：

$v_i^n=-{\left[\begin{array}{ccc}\frac{{\mathrm{PEX}}_i^n-{\mathrm{PEX}}_i^{n-1}}{\mathrm{\Δ}t}& \frac{{\mathrm{PEY}}_i^n-{\mathrm{PEY}}_i^{n-1}}{\mathrm{\Δ}t}& 0\end{array}\right]}^{\′}---\left(1\right)$

根据光电坐标系到球体坐标系的转换矩阵M_PEi2C将载体转动线速度转换到球体坐标系中：

$v_{ci}^n=M_{PEi2C}\·v_i^n---\left(2\right)$

载体线速度和角速度满足关系：

$v_{ci}^n={\mathrm{\ω}}_c^n\×R_{c0i}---\left(3\right)$

其中，R_c0i为球体坐标系中球心C到光电坐标系原点P_i的矢量，得到：

假设光电传感器测量得到的线速度为观测值，令其中，m为光电传感器的数量，考虑测量噪声构建载体转动观测方程：

$v_c^n=C_{c\mathrm{\ω}}\·{\mathrm{\ω}}_c^n+{\mathrm{\ξ}}_c^n---\left(4\right)$

采用最小二乘估计方法得到载体n-1到n时刻的角速度：

${\mathrm{\ω}}_c^n={({C_{c\mathrm{\ω}}}^{\′}\·C_{c\mathrm{\ω}})}^{-1}\·{C_{c\mathrm{\ω}}}^{\′}\·v_c^n---\left(5\right)$

将角速度单位化后得到n-1到n时刻载体转动的转轴方向：

${\mathrm{\η}}_c^n=\frac{{\mathrm{\ω}}_c^n}{\left|{\mathrm{\ω}}_c^n\right|}---\left(6\right).$

3.根据权利要求1所述的浮球平台载体姿态测量方法，其特征在于，所述步骤S200中解算当前时刻的载体转动角度包括以下步骤：

计算n-1到n时刻光电传感器测量球体转动得到的弧长：

$l_i^{\mathrm{\Δ}n}=\sqrt{{(P_i{\mathrm{EX}}_i^n-P_i{\mathrm{EX}}_i^{n-1})}^2+{(P_i{\mathrm{EY}}_i^n-P_i{\mathrm{EY}}_i^{n-1})}^2}---\left(7\right)$

所得弧长对应的半径由下式计算：

$R_i^n=|R_{c0i}\×{\mathrm{\η}}_c^n|---\left(8\right)$

得到第i个光电传感器测量得到的载体转动角度：

${\mathrm{\φ}}_i^n=\frac{l_i^{\mathrm{\Δ}n}}{R_i^n}---\left(9\right)$

将m个光电传感器解算出的转动角度进行平均作为n-1到n时刻的转动角度：

${\mathrm{\φ}}^n=\frac{\underset{i=1}{\overset{m}{\Σ}}{\mathrm{\φ}}_i^n}{m}---\left(10\right).$

4.根据权利要求3所述的浮球平台载体姿态测量方法，其特征在于，所述步骤S200中构造载体当前时刻的转动四元数包括以下步骤：根据n-1到n时载体的转轴方向和转动角度得到n-1到n时载体转动四元数在球体坐标系中的表达式：

$q_{shell}^n=\[\begin{array}{cc}cos({\mathrm{\φ}}^n/2)& {\mathrm{\η}}_c^n\·\sin ({\mathrm{\φ}}^n/2\end{array})\]---\left(11\right).$

5.根据权利要求1所述的浮球平台载体姿态测量方法，其特征在于，所述步骤S300中解算初始时刻到当前时刻的载体转动四元数包括步骤：根据四元数乘法可得到从0时刻到n时刻载体转动的四元数：

$q_{shell}^{0\_n}=q_{shell}^{0\_n-1}\⊗q_{shell}^{n-1\_n}---\left(12\right)$

其中，在n-1时刻得到，表示n-1时刻到n时刻球壳的转动四元数，将中的载体转动轴进行变换：

${\mathrm{\η}}_s^n={\left(q_{shell}^{0\_n-1}\right)}^{*}\⊗{\mathrm{\η}}_c^n\⊗q_{shell}^{0\_n-1}---\left(13\right)$

在当前的球壳坐标系中，表示为：

$q_{shell}^{n-1\_n}=\[\begin{array}{cc}cos({\mathrm{\φ}}^n/2)& {\mathrm{\η}}_s^n\·sin({\mathrm{\φ}}^n/2)\end{array}\]---\left(14\right)$

将(14)式代入(12)式得到初始时刻到当前时刻的载体转动四元数

6.根据权利要求1所述的浮球平台载体姿态测量方法，其特征在于，所述光电传感器数据包括根据光电传感器的安装位置计算得到的以下结构参数：

R_c0i：球体坐标系中球心C到光电坐标系原点P_i的矢量，i＝1,2,...,m；

M_PEi2C：光电坐标系到球体坐标系的转换矩阵，i＝1,2,...,m；

Δt：测量周期；

加速度观测矩阵，用于后续的最小二乘估计载体转动角速度。

7.根据权利要求1所述的浮球平台载体姿态测量方法，其特征在于，所述光电传感器数量为8个。