[发明专利]基于惯性单元和激光跟踪仪动态位姿测量装置和方法

权利要求书

1.一种基于惯性单元和激光跟踪仪动态位姿测量装置，其特征是，由激光靶和激光跟踪仪构成，激光标靶内部由视觉成像单元、惯性测量单元、倾角传感单元测量单元和数据处理单元组成，激光跟踪仪发出干涉激光，干涉激光锁定激光靶上的带切口的角锥棱镜，干涉激光在棱镜中发生镜面反射，一部分干涉光在反射之后返回至激光跟踪仪，激光跟踪仪通过干涉测距原理和球坐标与直角坐标的转换关系得到角锥棱镜反射中心，另一部分光通过角锥棱镜的切口，经过相机镜头，在相机的靶面上形成光斑，激光跟踪仪发出一部分干涉激光通过后，视觉成像单元中的数字相机将实时采集的图像发送给数据处理单元，惯性测量单元和倾角传感单元测量单元分别不断地将自身输出的角速度和角度发送至数据处理单元，数据处理单元对采集到的数据进行姿态解算得到方位角、俯仰角、滚转角三个姿态自由度，连同激光跟踪仪输出的位置自由度，最终得到激光靶的六自由度。

2.一种基于惯性单元和激光跟踪仪动态位姿测量方法，其特征是，利用激光跟踪仪通过跟踪定位激光靶上的角锥棱镜，得到棱镜反射中心在跟踪仪坐标系下的位置坐标，来自激光跟踪仪的一部分光通过激光靶上的角锥棱镜的切口，经过相机镜头，在相机的靶面上形成光斑，利用激光跟踪仪视觉成像单元中的数字相机将实时采集的光斑图像发送给激光跟踪仪数据处理单元，激光跟踪仪惯性测量单元和倾角传感单元测量单元分别不断地将自身输出的角速度和角度发送至数据处理单元，数据处理单元对采集到的数据进行姿态解算得到方位角、俯仰角、滚转角三个姿态自由度，连同激光跟踪仪输出的位置自由度，最终得到激光靶的六自由度。

3.如权利要求2所述的基于惯性单元和激光跟踪仪动态位姿测量方法，其特征是，在数据处理单元中通过解算，得到目标某时刻相对于初始位置和初始姿态时的位置及姿态变化。当确定好初始姿态，通过相应的转换关系求得每次当前最新的欧拉角。

4.如权利要求2或3所述的基于惯性单元和激光跟踪仪动态位姿测量方法，其特征是，数据处理的具体步骤是，n系(O_nX_nY_nZ_n)为大地坐标系，将大地坐标系的原点建立在跟踪仪上；b(O_bX_bY_bZ_b)系为载体坐标系，也就是惯性测量单元的自身轴系，原点位于激光靶内的惯性测量单元中心，θ、ψ分别为b系在n系下的滚转角、俯仰角及方位角，惯性单元的使用捷联惯导理论进行位姿解算时，实际上就是求载体坐标系在大地坐标系下的位姿，设q＝(q₀,q₁,q₂,q₃)为与旋转矩阵等价的姿态四元数，根据四元数的运算法则推导出以下关系：

其中为四元数乘法运算，为陀螺仪相对大地坐标系的的观测值，上式化简为

即为姿态更新的四元数微分方程

其中的获取按下式进行

和分别是地球自转速率和位置速率，且

式中，V_N V_E,L为导航计算的最新值；

对四元数微分方程采用一阶毕卡算法：

其中

式中，(△θ_x,△θ_y,△θ_z)为陀螺仪在采样时间[t_k,t_k+1]间隔内相对导航坐标系的角增量，从而得到每次迭代得到的四元数，将更新得到的四元数转化为欧拉角形式：

当确定好初始姿态，即确定好q(t₀)，求得每次当前最新的欧拉角。

5.如权利要求2所述的基于惯性单元和激光跟踪仪动态位姿测量方法，其特征是，在物体运动的过程中，激光靶标内部的数据处理单元会根据惯性传感单元的输出量，运用离散卡尔曼间接滤波算法，对惯性传感单元的输出量与倾角传感单元的输出量进行融合，具体地，定义：

[φ_I p_I]为惯性测量单元估计的姿态和位置；[φ_S p_S]^T为激光标靶测量的姿态和位置；

[φ p]^T为真实的姿态和位置；△φ_S为由激光标靶加速运动引起的姿态误差。