CN112558051A - 基于ukf的光电吊舱稳定平台被动测距方法 - Google Patents

Abstract

基于UKF的光电吊舱稳定平台被动测距方法，包括以下步骤：1)、定义吊舱稳定平台的各项参数；2)、获得测量值，设置状态量和状态以及测量方程；3)、基于坐标系变换一步递推的UKF被动测距算法；4)、相对运动量与状态量的相互转换，得到预测值和预测方差矩阵，实现被动测距。本发明通过对稳定平台结构的分析，得到了视线角和角速率与框架角，吊舱姿态，转动角速率之间的关系，在这个基础上提出了适合光电吊舱唯测角稳定平台的状态方程和运动方程，并提出基于坐标变换的一步递推的UKF被动测距算法。

Description

基于UKF的光电吊舱稳定平台被动测距方法

技术领域

本发明涉及被动测距技术领域，具体涉及基于UKF的光电吊舱稳定平台被动测距方法。

背景技术

已有方法在光电吊舱稳定平台进行被动定位应用有很大的局限性，其角度测量是吊舱与目标在惯性系下的视线角，但是在稳定平台中对视线的测量是通过平台的框架角，以及视线相对于光轴的角误差得到的，使得在光电吊舱稳定平台在被动定位应用中存在有很大的局限性。

发明内容

本发明要解决的技术问题是，克服现有技术存在的上述缺陷，提供一种解决了机载光电吊舱稳定平台对于目标只能定向无法进行无源测距的问题的基于UKF的光电吊舱稳定平台被动测距方法。

本发明解决其技术问题所采用的技术方案是：基于UKF的光电吊舱稳定平台被动测距方法，包括以下步骤：

1)、定义吊舱稳定平台的各项参数：做以下定义：I为惯性坐标系，B为吊舱坐标系，O为稳定平台的光轴坐标系；所述B的X轴为吊舱的轴线，其Y轴为垂直于吊舱的垂直轴面，其Z轴为垂直于吊舱的水平轴面；

所述I到所述B通过绕惯性坐标轴Y，X，Z转动得到的欧拉角分别为：ψ,θ,φ，在所述B下吊航的惯性角速度的分量分别为：p,q,r，通过平台的角速率陀螺测量所述O下光轴的惯性角速率分别为：q_O,r_O；

X'_O为光轴在平面X_BZ_B的投影，对于所述O的原点为稳定平台的光心，X轴为光轴，Z轴在平面X_BZ_B内并垂直于X'_O，Y轴按右手法则给出；光轴相对于吊舱的框架角包括俯仰和偏航，其中俯仰φ_z为X_O与X'_O的夹角；所述I的原点位于吊舱的质心，X轴指向东，Y轴指向北，Z轴垂直向上，则XY平面为水平面；

吊舱与目标相对位移为r，分量r_h为r在XY平面内的投影，r_h与X的夹角η为光轴在所述I下的偏航角，r与r_h的夹角ε为光轴在所述I下的俯仰角，目标加速度在所述O下每个坐标轴的投影分量分别为：a_tz,a_ty,a_tx，其对应的吊舱的加速度分量分别为：a_mx,a_my,a_mz；

2)、获得测量值，设置状态量和状态以及测量方程：在稳定平台中，得到的测量值包括：光轴相对于吊舱的框架角分别为：φ_y,φ_z，光轴在所述O下的惯性角速率分别为：p_O,q_O,r_O；所述B下的惯性角速率p,q,r，吊舱对于所述I的姿态分别为：ψ,θ,φ,吊舱的加速度a^I _mx,a^I _my,a^I _mz；稳定平台的状态矢量的计算公式为：

状态矢量在GSC下的状态量的计算公式为：

使测量值为Z，其计算公式为：

其中W为测量误差矩阵；

3)、基于坐标系变换一步递推的UKF被动测距算法：假设状态向量x为n维随机变量，并已知其均值

和方差P_xx可以通过2n+1个加权点来近似这个随机变量的分布：

W_i＝1/2(n+κ)，i＝1，2，...，n

W_i+n＝1/2(n+κ)，i＝1，2，...，n

式中：k为自然数，可以起到调节高阶矩的作用并减小预测误差，其值班x分布的不同而不同，如果假设x服从正态分布，则取n+k＝3，

为矩阵(n+k)P_xx的均方根的第i行或者第i列，对于该矩阵均方根的求解可以利用AR分解或Cholesky分解得到，W_i为与第i个点相对应的权重；

4)、相对运动量与状态量的相互转换，得到预测值和预测方差矩阵，实现被动测距：吊舱与目标相对运动的状态量为惯性系统下X,Y,Z方向的位移和速度：X_c＝[x y z v_xv_y v_z]′，状态方程为：X_C(k+1)＝ΦX_C(k)+BU，其中：

其中T为采样时间，I3为三阶单位矩阵，a^I _t,a^I _m,分别为吊舱与目标在所述I下的加速度，其中吊舱的加速度由吊舱的惯性设备测量得到；使光轴相对于惯性坐标系的俯仰和偏航分别为ε和η，吊舱相对于所述I的转动欧拉角为θ和φ，计算公式为：

和

其中

所述I下的运动状态量和GMSC下的状态量有以下转换关系：

(1)从GMSC到惯性直角坐标系统下的状态量变换：h_mc：x→x_c，其中：

x＝r cosεcosη

y＝r cosεsinη

z＝rsinε

(2)、从所述I下的状态量到GMSC的变换：h_cm：x_c→x，其中：

通过非线性转移函数表示为：h(χ_i(k))＝h_cm(Φ(h_mc(χ_i(k))))，之后再通过点集经加权平均和协方差得到预测值和预测方差矩阵，再利用UKF进行滤波，从而实现被动测距。

进一步的，所述步骤1)的p为0,ψ为0。

进一步的，所述步骤3)的变换过程如下：

(1)、将每个点代入非线性函数得到相应函数值，形成变换后的点集：z_i＝h(χ_i)；

(2)、非线性函数的期望由变换后的点集经加权平均得到：

(3)、求取协方差的公式经过相应加权可以得到非线性函数的协方差：

进一步的，所述步骤3)，当目标近似为匀速时a^I _m＝0。

进一步的，所述步骤4)，当要用光轴在所述B下实际测量的状态量φ_r和φ_z与光轴相对于所述I下的俯仰和偏航ε和η，可通过以下公式进行变

换：

本发明的有益效果是，通过对稳定平台结构的分析，得到了视线角和角速率与框架角，吊舱姿态，转动角速率之间的关系，在这个基础上提出了适合光电吊舱唯测角稳定平台的状态方程和运动方程，该方法可以检测到传统方法不能发现的弱小目标，在检测精度上有很明显的提高。

具体实施方式

下面结合实施例对本发明的具体实施方式作进一步详细说明：

基于UKF的光电吊舱稳定平台被动测距方法的实施例，包括以下步骤：

1)、定义吊舱稳定平台的各项参数：做以下定义：I为惯性坐标系，B为吊舱坐标系，O为稳定平台的光轴坐标系；B的X轴为吊舱的轴线，其Y轴为垂直于吊舱的垂直轴面，其Z轴为垂直于吊舱的水平轴面；

I到B通过绕惯性坐标轴Y，X，Z转动得到的欧拉角分别为：ψ,θ,φ，在B下吊航的惯性角速度的分量分别为：p,q,r，通过平台的角速率陀螺测量O下光轴的惯性角速率分别为：q_O,r_O；

X'_O为光轴在平面X_BZ_B的投影，对于O的原点为稳定平台的光心，X轴为光轴，Z轴在平面X_BZ_B内并垂直于X'_O，Y轴按右手法则给出；光轴相对于吊舱的框架角包括俯仰和偏航，其中俯仰φ_z为X_O与X'_O的夹角；I的原点位于吊舱的质心，X轴指向东，Y轴指向北，Z轴垂直向上，则XY平面为水平面；

吊舱与目标相对位移为r，分量r_h为r在XY平面内的投影，r_h与X的夹角η为光轴在I下的偏航角，r与r_h的夹角ε为光轴在I下的俯仰角，目标加速度在O下每个坐标轴的投影分量分别为：a_tz,a_ty,a_tx，其对应的吊舱的加速度分量分别为：a_mx,a_my,a_mz；当吊舱的横滚控制做得很好，可以近似认为p为0,ψ为0。

2)、获得测量值，设置状态量和状态以及测量方程：在稳定平台中，得到的测量值包括：光轴相对于吊舱的框架角分别为：φ_y,φ_z，光轴在O下的惯性角速率分别为：p_O,q_O,r_O；B下的惯性角速率p,q,r，吊舱对于I的姿态分别为：ψ,θ,φ,吊舱的加速度a^I _mx,a^I _my,a^I _mz；稳定平台的状态矢量的计算公式为：

状态矢量在GSC下的状态量的计算公式为：

使测量值为Z，其计算公式为：

其中W为测量误差矩阵；3)、基于坐标系变换一步递推的UKF被动测距算法：假设状态向量x为n维随机变量，并已知其均值

和方差P_xx可以通过2n+1个加权点来近似这个随机变量的分布：

W_i＝1/2(n+κ),i＝1，2，...，n

W_i+n＝1/2(n+κ)，i＝1，2，...，n

式中：k为自然数，可以起到调节高阶矩的作用并减小预测误差，其值班x分布的不同而不同，如果假设x服从正态分布，则取n+k＝3，

为矩阵(n+k)P_xx的均方根的第i行或者第i列，对于该矩阵均方根的求解可以利用AR分解或Cholesky分解得到，W_i为与第i个点相对应的权重；当目标近似为匀速时a^I _m＝0；变换过程如下：

(1)、将每个点代入非线性函数得到相应函数值，形成变换后的点集：z_i＝h(χ_i)；

(2)、非线性函数的期望由变换后的点集经加权平均得到：

(3)、求取协方差的公式经过相应加权可以得到非线性函数的协方差：

4)、相对运动量与状态量的相互转换，得到预测值和预测方差矩阵，实现被动测距：吊舱与目标相对运动的状态量为惯性系统下X,Y,Z方向的位移和速度：X_c＝[x y z v_xv_y v_z]′，状态方程为：X_c(k+1)＝ΦX_C(k)+BU，其中：

其中T为采样时间，I3为三阶单位矩阵，a^I _t,a^I _m,分别为吊舱与目标在I下的加速度，其中吊舱的加速度由吊舱的惯性设备测量得到；使光轴相对于惯性坐标系的俯仰和偏航分别为ε和η，吊舱相对于I的转动欧拉角为θ和φ，计算公式为：

和

其中

，当要用光轴在B下实际测量的状态量φ_r和φ_z与光轴相对于I下的俯仰和偏航ε和η，可通过以下公式进行变换：

I下的运动状态量和GMSC下的状态量有以下转换关系：

(1)从GMSC到惯性直角坐标系统下的状态量变换：h_mc：x→x_c，其中：

x＝r cosεcosη

y＝r cosεsinη

z＝r sinε

(2)、从I下的状态量到GMSC的变换：h_vm：xc_→x，其中：

通过非线性转移函数表示为：h(χ_i(k))＝h_cm(Φ(h_mc(χ_i(k))))，之后再通过点集经加权平均和协方差得到预测值和预测方差矩阵，再利用UKF进行滤波，从而实现被动测距。

说明书中未详细说明的内容属于本领域技术人员熟知的现有技术。

以上仅是本发明的优选实施方式，应当指出，对于本领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应当视为在本发明的保护范围之内。

Claims (5)

1.基于UKF的光电吊舱稳定平台被动测距方法，其特征在于，包括以下步骤：

1)、定义吊舱稳定平台的各项参数：做以下定义：I为惯性坐标系，B为吊舱坐标系，O为稳定平台的光轴坐标系；所述B的X轴为吊舱的轴线，其Y轴为垂直于吊舱的垂直轴面，其Z轴为垂直于吊舱的水平轴面；

所述I到所述B通过绕惯性坐标轴Y，X，Z转动得到的欧拉角分别为：ψ,θ,φ，在所述B下吊航的惯性角速度的分量分别为：p,q,r，通过平台的角速率陀螺测量所述O下光轴的惯性角速率分别为：q_O,r_O；

X'_O为光轴在平面X_BZ_B的投影，对于所述O的原点为稳定平台的光心，X轴为光轴，Z轴在平面X_BZ_B内并垂直于X'_O，Y轴按右手法则给出；光轴相对于吊舱的框架角包括俯仰和偏航，其中俯仰φ_z为X_O与X'_O的夹角；所述I的原点位于吊舱的质心，X轴指向东，Y轴指向北，Z轴垂直向上，则XY平面为水平面；

吊舱与目标相对位移为r，分量r_h为r在XY平面内的投影，r_h与X的夹角η为光轴在所述I下的偏航角，r与r_h的夹角ε为光轴在所述I下的俯仰角，目标加速度在所述O下每个坐标轴的投影分量分别为：a_tz,a_ty,a_tx，其对应的吊舱的加速度分量分别为：a_mx,a_my,a_mz；

2)、获得测量值，设置状态量和状态以及测量方程：在稳定平台中，得到的测量值包括：光轴相对于吊舱的框架角分别为：φ_y,φ_z，光轴在所述O下的惯性角速率分别为：p_O,q_O,r_O；所述B下的惯性角速率p,q,r，吊舱对于所述I的姿态分别为：ψ,θ,φ,吊舱的加速度a^I _mx,a^I _my,a^I _mz；稳定平台的状态矢量的计算公式为：

状态矢量在GSC下的状态量的计算公式为：

使测量值为Z，其计算公式为：

其中W为测量误差矩阵；

3)、基于坐标系变换一步递推的UKF被动测距算法：假设状态向量x为n维随机变量，并已知其均值

和方差P_xx可以通过2n+1个加权点来近似这个随机变量的分布：

W_i＝1/2(n+κ),i＝1，2，...，n

W_i+n＝1/2(n+κ)，i＝1，2，…，n

式中：k为自然数，可以起到调节高阶矩的作用并减小预测误差，其值班x分布的不同而不同，如果假设x服从正态分布，则取n+k＝3，

为矩阵(n+κ)P_xx的均方根的第i行或者第i列，对于该矩阵均方根的求解可以利用AR分解或Cholesky分解得到，W_i为与第i个点相对应的权重；

4)、相对运动量与状态量的相互转换，得到预测值和预测方差矩阵，实现被动测距：吊舱与目标相对运动的状态量为惯性系统下X,Y,Z方向的位移和速度：X_c＝[x y z v_x v_yv_z]′，状态方程为：X_c(k+1)＝ΦX_c(k)+BU，其中：

其中T为采样时间，I3为三阶单位矩阵，a^I _t,a^I _m,分别为吊舱与目标在所述I下的加速度，其中吊舱的加速度由吊舱的惯性设备测量得到；使光轴相对于惯性坐标系的俯仰和偏航分别为ε和η，吊舱相对于所述I的转动欧拉角为θ和φ，计算公式为：

和

其中

所述I下的运动状态量和GMSC下的状态量有以下转换关系：

(1)从GMSC到惯性直角坐标系统下的状态量变换：h_mc：x→x_c，其中：

x＝rcosεcosη

y＝rcosεsinη

z＝rsinε

(2)、从所述I下的状态量到GMSC的变换：h_cm：x_c→x，其中：

通过非线性转移函数表示为：h(χ_i(k))＝h_cm(Φ(h_mc(χ_i(k))))，之后再通过点集经加权平均和协方差得到预测值和预测方差矩阵，再利用UKF进行滤波，从而实现被动测距。

2.根据权利要求1所述的基于UKF的光电吊舱稳定平台被动测距方法，其特征在于，所述步骤1)的p为0,ψ为0。

3.根据权利要求1所述的基于UKF的光电吊舱稳定平台被动测距方法，其特征在于，所述步骤3)的变换过程如下：

(1)、将每个点代入非线性函数得到相应函数值，形成变换后的点集：z_i＝h(χ_i)；

(2)、非线性函数的期望由变换后的点集经加权平均得到：

(3)、求取协方差的公式经过相应加权可以得到非线性函数的协方差：

4.根据权利要求1所述的基于UKF的光电吊舱稳定平台被动测距方法，其特征在于，所述步骤3)，当目标近似为匀速时a^I _m＝0。

5.根据权利要求1所述的基于UKF的光电吊舱稳定平台被动测距方法，其特征在于，所述步骤4)，当要用光轴在所述B下实际测量的状态量φ_r和φ_z与光轴相对于所述I下的俯仰和偏航ε和η，可通过以下公式进行变换：