CN104634346B - 基于光纤陀螺的光电平台姿态检测方法 - Google Patents

Abstract

本发明提出一种基于光纤陀螺的光电平台姿态检测方法，采用正交安装的三只光纤陀螺及正交安装的三只加速度计组成IMU组件，IMU组件安装在光电平台上，且IMU组件能够绕光电平台俯仰轴和方位轴转动上，跟随俯仰轴运动。姿态测量系统通过采样IMU的输出信号，解算得到IMU的空间方位角度、姿态角度。再通过光电稳定平台的方位轴和俯仰轴的测角机构，测出姿态测量系统相对光电稳定平台的相对空间关系，计算获得光电稳定平台的方位角度、姿态角度。

Description

基于光纤陀螺的光电平台姿态检测方法

技术领域

本发明属于光电检测领域，是一种基于光纤陀螺的光电平台姿态检测方法。该方法利用安装在光电平台上的惯性测量器件组成的测量单元，实现军用光电平台的姿态检测。

背景技术

随着信息技术、智能弹药、远程打击等技术的发展，各种光电平台系统已经进入了信息化阶段。因此，对武器系统的打击精度、攻击距离、信息共享、反应时间、机动能力都提出更高的要求。传统光电平台系统，只在停下来进行目标观察瞄准时能保障精度，而动态情况下的精度难以满足指标要求。因此，为这些光电平台系统提供姿态信息，配备姿态测量系统是一个重要的解决方案。

光电平台必须要了解自身载体的方位角、俯仰角和侧倾角等姿态信息，然后才能为光电系统观察目标、确定目标位置和方位、锁定目标，提供精确的修正量；尤其是间接瞄准观察目标时，它可以根据前方观察者、后方使用者、目标位置等信息，调整平台的姿态，间接测量出目标的地理信息。

通过姿态测量系统精确测出光电平台在运动中的方位角度、姿态角度等信息，使观察人员能够获得良好的观瞄、操作能力，进行目标锁定。同时，可以将光电平台的运动数据发送到其它信息单元，用于进行各种补偿解算，提高观瞄精度。

姿态测量及为观瞄线相结合具有明显优势：首先，高精度陀螺稳定的光电平台漂移量小；其次，动态稳定性能好，可以实现自动目标跟踪功能，观瞄装置可以在行进中稳定的跟踪目标。我国该项技术的发展与国外还有较大的差距，因此，需要提出新的姿态测量方法，为新型光电稳定平台在各种载体平台的发展提供原理分析和技术支撑。

发明内容

本发明针对光电稳定平台的姿态检测和在未来装备中的使用问题，面向光电稳定平台的姿态检测需求，提供一种基于光纤陀螺的光电平台姿态检测方法。

本发明的技术方案为：

所述一种基于光纤陀螺的光电平台姿态检测方法，其特征在于：采用正交安装的三只光纤陀螺及正交安装的三只加速度计组成IMU组件，IMU组件安装在光电平台上，且IMU组件能够绕光电平台方位轴和俯仰轴转动，并用过以下步骤进行光电平台姿态检测：

步骤1：对IMU组件进行初始对准：

控制IMU组件绕光电平台方位轴转动，得到IMU组件转动0度，90度，180度和270度时刻的输出：0度位置三个陀螺输出和三个加速度计输出90度位置三个陀螺输出和三个加速度计输出180度位置三个陀螺输出和三个加速度计输出270度位置三个陀螺输出和三个加速度计输出并得到IMU组件当前姿态和方位角度为：

俯仰角度：

倾斜角度：

方位角度：

其中L是光电平台所处的纬度，g为重力加速度，w_ie为地球的自转角速率；

建立IMU组件的初始捷联矩阵

并对四元数矩阵Q＝[q₀ q₁ q₂ q₃]进行初始化：

q₀＝cos(α/2)cos(β/2)cos(γ/2)-sin(α/2)sin(β/2)sin(γ/2)，

q₁＝cos(α/2)sin(β/2)cos(γ/2)+sin(α/2)cos(β/2)sin(γ/2)，

q₂＝cos(α/2)cos(β/2)sin(γ/2)-sin(α/2)sin(β/2)cos(γ/2)，

q₃＝-sin(α/2)cos(β/2)cos(γ/2)+cos(α/2)sin(β/2)sin(γ/2)；

在初始对准后，光电稳定平台的方位角α₂、俯仰角度为β₂、倾斜角度γ₂与IMU组件的方位角α、俯仰角度为β、倾斜角度γ对应相同；

步骤2：采用以下步骤对初始对准后的IMU组件进行方位和姿态测量：

步骤2.1：根据三个陀螺在当前采样周期的测量数据ω_e2＝[ω_e2x ω_e2y ω_e2z]以及在上一采样周期的测量数据ω_e1＝[ω_e1x ω_e1y ω_e1z]，得到角度增量E1，E2：

E1＝(ω_e1-ω_in)×h，E2＝(ω_e2-ω_in)×h

其中h为采样周期，ω_in通过以下公式计算：

V_x为光电平台相对大地坐标系的东向运动速度，V_y为光电平台相对大地坐标系的北向运动速度，R为地球半径；

步骤2.2：根据角度增量E1，E2计算向量

得到四元数增量其中向量模值

步骤2.3：根据得到的四元数增量Qh更新四元数：

Q_k＝Q_k-1·Qh

其中Q_k-1为上一采样周期得到的四元数，Q_k为当前采样周期更新后的四元数，Q_k＝[q_0k q_1k q_2k q_3k]；

步骤2.4：根据更新后的四元数Q_k得到当前采样周期IMU组件的捷联矩阵：

步骤2.5：根据当前采样周期IMU组件的捷联矩阵得到当前采样周期IMU组件的方位角度和姿态角度：

俯仰角度：

倾斜角度：

方位角度：

其中

步骤3：采用以下步骤测量光电平台方位和姿态测量：

步骤3.1：由光电平台得到当前采样周期IMU组件相对光电平台的方位角度α_1k、俯仰角度β_1k，计算从IMU组件到光电平台的姿态转换矩阵

进一步得到当前采样周期光电平台相对大地坐标系的捷联矩阵

步骤3.2：根据当前采样周期光电平台相对大地坐标系的捷联矩阵Cb_k得到当前采样周期光电平台的方位角度和姿态角度：

俯仰角度：β_2k＝sin^-1(Cb_k(3,2))，

倾斜角度：

方位角度：

其中

有益效果

本发明使用安装在光电平台俯仰轴上的惯性测量器件组成的测量单元，结合光电稳定平台的测角装置，实现光电稳定平台的姿态精确检测。

附图说明

图1是本发明中姿态测量方法的原理框图。

具体实施方式

下面结合具体实施例描述本发明：

采用正交安装的三只光纤陀螺及正交安装的三只加速度计组成IMU组件，IMU组件安装在光电平台上，且IMU组件能够绕光电平台俯仰轴和方位轴转动上，跟随俯仰轴运动。姿态测量系统通过采样IMU的输出信号，解算得到IMU的空间方位角度、姿态角度。再通过光电稳定平台的方位轴和俯仰轴的测角机构，测出姿态测量系统相对光电稳定平台的相对空间关系，计算获得光电稳定平台的方位角度、姿态角度。

具体光电平台姿态检测步骤如下：

步骤1：对IMU组件进行初始对准：

光纤陀螺姿态测量系统在进行姿态测量解算之前，系统需要进行初始对准。初始对准采用四位置初始对准的方式进行，四位置对准通过0度和180度，90度和270度位置对应数据相减，可以有效减少开机重复性的误差影响和零位偏置误差。

在四位置初始对准过程中，采用机械结构的限位方式来提高转动定位精度和位置归零重复精度，即到位的最终结果是由结构来保证，电机在初始对准过程中起驱动作用和较低精度的到位控制作用。这样可以降低转位控制的要求，以及提高转位重复精度，以满足系统初始对准指标的要求。

控制IMU组件绕光电平台方位轴转动，得到IMU组件转动0度，90度，180度和270度时刻的输出：0度位置三个陀螺输出和三个加速度计输出90度位置三个陀螺输出和三个加速度计输出180度位置三个陀螺输出和三个加速度计输出270度位置三个陀螺输出和三个加速度计输出通过得到的陀螺和加速度计数据，得到IMU组件当前姿态和方位角度为：

俯仰角度：

倾斜角度：

方位角度：

其中L是光电平台所处的纬度，g为重力加速度，w_ie为地球的自转角速率；

建立IMU组件的初始捷联矩阵

并对四元数矩阵Q＝[q₀ q₁ q₂ q₃]进行初始化：

q₀＝cos(α/2)cos(β/2)cos(γ/2)-sin(α/2)sin(β/2)sin(γ/2)，

q₁＝cos(α/2)sin(β/2)cos(γ/2)+sin(α/2)cos(β/2)sin(γ/2)，

q₂＝cos(α/2)cos(β/2)sin(γ/2)-sin(α/2)sin(β/2)cos(γ/2)，

q₃＝-sin(α/2)cos(β/2)cos(γ/2)+cos(α/2)sin(β/2)sin(γ/2)；

在初始对准后，光电稳定平台的方位角α₂、俯仰角度为β₂、倾斜角度γ₂与IMU组件的方位角α、俯仰角度为β、倾斜角度γ对应相同。

初始对准完成，光纤陀螺姿态测量装置就可以进行动态的姿态测量工作。

步骤2：采用以下步骤对初始对准后的IMU组件进行方位和姿态测量：

当光电平台运动时，四元数将随着IMU相对惯性空间的运动而运动，通过获得实时的四元数，就可以获得实时的IMU组件捷联矩阵，这样就可以解算出IMU组件的方位角度、俯仰角度、倾斜角度。四元数的更新采用二子样算法，即通过IMU上的三轴光纤陀螺，感应到IMU相对惯性空间的角速度变化量，结合光电平台的运动速度，采用二子样解算，可以确定实时的四元数。

步骤2.1：根据三个陀螺在当前采样周期的测量数据ω_e2＝[ω_e2x ω_e2y ω_e2z]以及在上一采样周期的测量数据ω_e1＝[ω_e1x ω_e1y ω_e1z]，得到角度增量E1，E2：

E1＝(ω_e1-ω_in)×h，E2＝(ω_e2-ω_in)×h

其中h为采样周期，ω_in通过以下公式计算：

V_x为光电平台相对大地坐标系的东向运动速度，V_y为光电平台相对大地坐标系的北向运动速度，R为地球半径；通过里程计测得光电稳定平台的运动速度为V，由此得到V_x＝-Vcos(β_2(k-1))sin(α_2(k-1))，V_y＝Vcos(β_2(k-1))cos(α_2(k-1))，其中β_2(k-1)为光电平台上一采样周期的俯仰角度，α_2(k-1)为光电平台上一采样周期的方位角度；

步骤2.2：根据角度增量E1，E2计算向量

得到四元数增量其中向量模值

步骤2.3：根据得到的四元数增量Qh更新四元数：

Q_k＝Q_k-1·Qh

其中Q_k-1为上一采样周期得到的四元数，Q_k为当前采样周期更新后的四元数，Q_k＝[q_0k q_1k q_2k q_3k]；

步骤2.4：根据更新后的四元数Q_k得到当前采样周期IMU组件的捷联矩阵：

因此，每进行一次四元数迭代，就可以获得一次新的四元数，根据四元数和捷联矩阵的对应关系，求解出方位角度和姿态角度。通过以下公式求取实时的IMU组件的方位角度和姿态角度：

步骤2.5：根据当前采样周期IMU组件的捷联矩阵得到当前采样周期IMU组件的方位角度和姿态角度：

俯仰角度：其中为矩阵的第3行第2列的元素值；

倾斜角度：

方位角度：

其中

步骤3：采用以下步骤测量光电平台方位和姿态测量：

光电稳定平台姿态的解算，是在已知IMU组件的方位角度、姿态角度的情况下，通过IMU组件相对车体的即时位置关系求取得到。

步骤3.1：由光电平台得到当前采样周期IMU组件相对光电平台的方位角度α_1k、俯仰角度β_1k，计算从IMU组件到光电平台的姿态转换矩阵

进一步得到当前采样周期光电平台相对大地坐标系的捷联矩阵

步骤3.2：根据当前采样周期光电平台相对大地坐标系的捷联矩阵Cb_k得到当前采样周期光电平台的方位角度和姿态角度：

俯仰角度：β_2k＝sin^-1(Cb_k(3,2))，

倾斜角度：

方位角度：

其中

Claims (1)

1.一种面向复合材料铺层单元数控下料工序的展开轮廓线修正方法，其特征在于：

包括以下步骤：

步骤1：通过以下步骤1.a对不闭合的铺层单元S^P展开轮廓C^U进行封闭化处理，得到封闭的展开轮廓C^C，所述封闭的展开轮廓C^C由直线段L^L、圆弧段L^C和样条线L^S组成；并通过以下步骤1.b对展开轮廓C^C中圆弧段L^C和样条线L^S进行离散，分别用直线段连接相邻离散点，得到只由直线段组成的封闭展开轮廓

步骤1.a：铺层单元S^P经CAD展开软件展开得到展开轮廓C^U，对于展开轮廓C^U中所有不闭合处的两个线段端点，均采用直线段连接相应的两个线段端点，得到新的展开轮廓C^C；

步骤1.b：对展开轮廓C^C中圆弧段L^C和样条线L^S进行离散，得到圆弧段L^C和样条线L^S离散点分别为用直线段分别连接圆弧段L^C和样条线L^S中的相邻离散点，分别得到近似逼近圆弧段L^C和样条线L^S的多边形L^PS与展开轮廓C^C中直线段L^L共同构成新的展开轮廓

步骤2：采用以下步骤2.a和步骤2.b删除展开轮廓线中在同一直线段上的点，得到新的展开轮廓线

步骤2.a：以展开轮廓线C^LU中任一连接点P_i(i＝3,4…I)为开始点，沿同一方向对展开轮廓C^LU进行搜索，判断搜索方向上连接点P_i(3≤i≤I)到线段P_i-2P_i-1的距离是否满足条件

$D^{P_{i-2}{P}_{i-1}}=\frac{(y_{i-2}-y_{i-1})x_i-(x_{i-2}-x_{i-1})y_i+x_{i-2}{y}_{i-1}-x_{i-1}{y}_{i-2}}{\sqrt{{(y_{i-2}-y_{i-1})}^2+{(x_{i-2}-x_{i-1})}^2+{(x_{i-2}{y}_{i-1}-x_{i-1}{y}_{i-2})}^2}}=0$

公式中对应点坐标为P_i(x_i,y_i)、P_i-1(x_i-1,y_i-1)、P_i-2(x_i-2,y_i-2)；满足上述条件的连接点P_i为无效连接点P_i ^N，不满足上述条件的连接点P_i为有效连接点P_i ^P；

步骤2.b：若连接点P_i为无效连接点P_i ^N，则将线段P_i-1P_i的终点P_i(x_i,y_i)设置为线段P_i-₂P_i-1的终点P_i-1(x_i-1,y_i-1)，采用线段P_i-2P_i替换原展开轮廓C^LU中的线段P_i-2P_i-1和P_i-1P_i；若连接点P_i为有效连接点P_i ^P，则将P_i+1(i≤I)作为新的开始点，重复步骤2.a、2.b继续进行判断，直到i＝I为止，此时得到新的展开轮廓为C^NU；

步骤3采用以下步骤3.a和步骤3.b判断步骤2得到的新的展开轮廓线中连接点P_j(2≤j≤J)的凹凸性，删除展开轮廓C^NU中所有凹点，得到新的展开轮廓

步骤3.a；以展开轮廓线C^NU中任一连接点P_j(j＝2,3,…,J)为开始点进行搜索，开始点两个相邻边分别为P_j-1P_j、P_jP_j+1，方向向量分别为 其中对应点坐标为P_j(x_j,y_j)、P_j+1(x_j+1,y_j+1)、P_j-1(x_j-1,y_j-1)，与之间的夹角为θ，同时垂直于和的向量为则有 当搜索方向为沿轮廓线顺时针方向D^c时，若sinθ＞0，则连接点P_j为凸点，若sinθ＜0，则连接点P_j为凹点；当搜索方向为沿轮廓线逆时针方向D^ac时，若sinθ＞0，则连接点P_j为凹点，若sinθ＜0，则连接点P_j为凸点；

步骤3.b：如果开始点P_j为凸点，判断连接点P_j+1的凸凹性，若P_j+1为凸点，则将P_j+1作为新的开始点，若P_j+1为凹点，则将线段P_j+1P_j+2的终点P_j+2(x_j+2,y_j+2)设置为线段P_jP_j+1的终点P_j+1(x_j+1,y_j+1)，采用线段P_jP_j+2替换原展开轮廓C^NU中的线段P_jP_j+1和P_j+1P_j+2，将P_j+2作为新的开始点继续搜索判断剩余点；如果开始点P_j为凹点，则舍弃当前连接点P_j，重新选择开始点P_j+1并进行凹凸性判断，直到判断的开始点为凸点为止，后续的处理方法与前述开始点为凸点的处理方法相同；得到处理后的展开轮廓为