CN114383631A - 基于最小二乘、泰勒展开和综合残差组合的实时标定方法 - Google Patents

Abstract

本发明公开了基于最小二乘、泰勒展开和综合残差组合的实时标定方法，磁传感器数据通过使用一阶泰勒级数展开解算地磁YZ平面关系式，得到YZ综合残差表达式；利用最小二乘法对初始点和真实点之间的修正量进行求解，最后进行组合解算。本发明提出的基于最小二乘、泰勒展开和综合残差组合的实时标定方法，借助卫星导航所得航向角数据，通过磁传感器数据观测，运用了泰勒级数展开式，结合最小二乘法和综合残差进行求解，该标定技术能够有效滤掉飞行过程中实时产生的误差，所需数据量小，解算时间短，标定精度高，能够做到实时标定。

Description

基于最小二乘、泰勒展开和综合残差组合的实时标定方法

技术领域

本发明涉及姿态测量技术，特别涉及一种基于最小二乘、泰勒展开和综合残差组合的实时标定方法。

背景技术

姿态测量技术应用价值巨大，正确解算姿态信息，减小外界误差的影响，可以有效提高传感器测试结果的可靠性、精确性。

传感器进行姿态测量常用有陀螺仪、加速度计、地磁传感器等，本专利着重研究地磁传感器标定技术。磁传感器标定技术，常用椭圆拟合和十二位置磁罗盘进行标定。椭圆拟合通过X轴、Y轴数据画椭圆轨迹，求解的二次曲面方程，即为地磁场测量表达式，利用两者变换关系求解地磁场表达式系数，从而得到传感器X轴和Y轴的标定系数。十二位置磁罗盘标定，由天向磁场推导三轴磁场的零位，标度因子和安装误差的公式，不需要精确的磁北向基准，不需要双轴位置转台，仅仅依靠一个铝制六面体，实现装配后的磁罗盘三轴零位、标度因子和安装误差的标定。上述两种标定技术只能标定固定误差，且精度较低。

目前应用较多的是椭球拟合标定方法。通过求解椭球曲面方程系数，得到传感器标定系数，加其他约束条件，如基于最小二乘法的椭球拟合标定方法、基于最大似然估计的椭球拟合标定方法等等，可以进一步提高标定精度。迭代算法补偿法，针对导航系统中磁传感器标定方法存在的计算时间过长和误差较大等问题提出，通过三轴磁传感器测量过程中的误差来源，给出了磁传感器标定与测量误差的参数化数学模型，应用迭代算法确定相关误差参数。遗传算法标定补偿方法，主要应用于地磁匹配导航中，解决搜索空间过大的问题，遗传算法在匹配区域搜索可能的航迹，用相关性度量作为适应度函数，以定向而非随机方式实现变异，从而获得更高的导航定位。还有人提出了窗口滑动滤波器，对旋转飞行物体进行实时标定，该方案所需数据量巨大，且标定校准精度不高。

磁传感器在进行地面标定之后，对其自身误差、安装误差、环境误差的标定补偿能够有效的保证磁传感器在该使用环境中的测量解算数据精度。然而，在实际的飞行过程中，周围环境在不断变化，磁传感器可能会随着时间变化、或者飞行体内部元器件的影响产生误差，也有可能会有突然出现磁性物质影响其测量值，等等。因此，研究一种基于最小二乘、泰勒展开和综合残差组合的实时标定技术，是保证飞行体在实际飞行过程中，实测数据的准确性和解算数据的精确性的重要手段，对于传感器实际飞行过程中进行姿态解算具有重要的意义。

发明内容

本发明目的是：针对实际飞行过程中，传感器随时间和飞行过程中不定因素影响产生误差，导致姿态解算数据不准确，本发明提供一种基于最小二乘、泰勒展开和综合残差组合的实时标定方法。

本发明的技术方案是：

基于最小二乘、泰勒展开和综合残差组合的实时标定方法，包括：

S 1、磁传感器数据通过使用一阶泰勒级数展开解算地磁YZ平面关系式，得到YZ综合残差表达式；

S2、利用最小二乘法对初始点和真实点之间的修正量进行求解，最后进行组合解算。

具体的，步骤S1中，使用一阶泰勒级数展开解算地磁YZ平面关系式，在实际的飞行过程中，航向角确定，借助卫星导航所得的俯仰角可信，载体坐标系选择前上右，借助航向角和俯仰角理想值得到飞行过程中X轴的可信值，求取磁传感器的YZ平面测得量的可信值；

地磁坐标系转换为地理坐标系，转换关系表示为：

地理坐标系转换为载体坐标系，转换关系表示为：

将地理坐标系和航向角组合解算，得到载体坐标系各轴数据和姿态角的关系为：

求解磁传感器YZ平面测得量的可信值为式(4)：

其中，MagX_B为将飞行过程中可信的航向角和俯仰角代入式(3)中，解算出的磁传感器X轴的可信值，B为该地的理论地磁场值；

磁传感器YZ平面的预测量和传感器单个Y轴、Z轴值之间的关系为：

式中，MagY,MagZ为需要求解的状态量，MagYZ为通过式(4)得到的已知可信测得量；进行空中实时标定时，有一个起始点v，基于这个起始点将式 (5)进行一阶泰勒级数展开就会得到式(6)：

在式(6)中，h.o.t是高阶泰勒级数项；空中飞行阶段，认为每一时刻的状态量与前面时刻的状态量都满足式(6)；

定义如下矢量：

式(9)中U_v叫做方向余弦矢量，表示Y轴和Z轴在YZ面的方向矢量，将式(7)和式(8)代入式(6)中，若令地磁传感器YZ面的测得量减去初始预测量的差值叫做YZ综合残差，用δMagYZ_v表示，省略高阶项后式(6)写成：

δMagYZ_v＝U_v·dMagyz_v (9)

式(9)是对YZ面的测得量做一次观测得到的结果，做m次迭代并线性化得到线性方程组：

将上述方程组写成矩阵的形式得到

δMagYZ＝H·dMagyz_v (11)

式中，δMagYZ＝[δMagYZ_v1,δMagYZ_v2,...δMagYZ_vm]^T；

具体的，步骤S2中，利用最小二乘法求式(11)解得：

dMagyz_v＝(H^TH)^-1H^T(δMagYZ) (12)

式(12)中，dMagyz_v是需要更新的地磁传感器Y轴和Z轴的状态变化量，通过最小二乘法求解一次线性化地磁传感器各轴初始点和真实点之间的修正量，再用修正量来更新初始点，得到修正后的解为：

Mag_{Y_Z}＝Magyz_v+dMagyz_v (13)

即为组合解算记结果。

本发明的优点是：

本发明提出的基于最小二乘、泰勒展开和综合残差组合的实时标定方法，借助卫星导航所得航向角数据，通过磁传感器数据观测，运用了泰勒级数展开式，结合最小二乘法和综合残差进行求解，该标定技术能够有效滤掉飞行过程中实时产生的误差，所需数据量小，解算时间短，标定精度高，能够做到实时标定。

附图说明

下面结合附图及实施例对本发明作进一步描述：

图1为本发明所述的地磁坐标系转换为地理坐标系；

图2为本发明所述的地理坐标系转换为载体坐标系；

图3为本发明所述的地理坐标系到载体坐标系流程图；

图4为本发明所述的地磁分量

B_nx、

B_nz关系图。

具体实施方式

本发明基于最小二乘、泰勒展开和综合残差组合的实时标定方法，通过使用一阶泰勒级数展开解算地磁YZ平面关系式，得到YZ综合残差表达式，再利用最小二乘法对初始点和真实点之间的修正量进行求解，最后进行组合解算。利用综合残差进行泰勒展开，结合最小二乘法解算，由线性化后的修正量更新不可信的预测量(初始点)，得到实时标定后的数据，从而保证飞行过程中解算数据的准确性。

在磁传感器数据信息解算中，采用与卫星导航位置解算相似的解算方式。借助卫星导航所得解得的数据，对磁传感进行空中实时标定。在实际的飞行过程中，航向角确定，借助卫星导航所得的俯仰角可信，载体坐标系选择前上右，借助航向角和俯仰角理想值得到飞行过程中X轴的可信值，求取磁传感器的 YZ平面测得量的可信值。

地磁坐标系转换为地理坐标系，如图1所示，转换关系表示为：

地理坐标系转换为载体坐标系如图2所示，转换流程图如图3所示，转换关系表示为：

将地理坐标系和航向角组合解算，如图4所示，可得到载体坐标系各轴数据和姿态角的关系为：

求解磁传感器YZ平面测得量的可信值为式(4)：

其中，MagX_B为将飞行过程中可信的航向角和俯仰角代入式(3)中，解算出的磁传感器X轴的可信值，B为该地的理论地磁场值；

磁传感器YZ平面的预测量和传感器单个Y轴、Z轴值之间的关系为：

式中，MagY,MagZ为需要求解的状态量，MagYZ为通过式(4)得到的已知可信测得量；进行空中实时标定时，有一个起始点v，基于这个起始点将式 (5)进行一阶泰勒级数展开就会得到式(6)：

在式(6)中，h.o.t是高阶泰勒级数项；空中飞行阶段，认为每一时刻的状态量与前面时刻的状态量都满足式(6)；

定义如下矢量：

式(9)中U_v叫做方向余弦矢量，表示Y轴和Z轴在YZ面的方向矢量，将式(7)和式(8)代入式(6)中，若令地磁传感器YZ面的测得量(理论值)减去初始预测量的差值叫做YZ综合残差，用δMagYZ_v表示，省略高阶项后式(6)写成：

δMagYZ_v＝U_v·dMagyz_v (9)

式(9)是对YZ面的测得量做一次观测得到的结果，做m次迭代并线性化得到线性方程组：

将上述方程组写成矩阵的形式得到

δMagYZ＝H·dMagyz_v (11)

式中，δMagYZ＝[δMagYZ_v1,δMagYZ_v2,...δMagYZ_vm]^T；

具体的，步骤S2中，利用最小二乘法求式(11)解得：

dMagyz_v＝(H^TH)^-1H^T(δMagYZ) (12)

式(12)中，dMagyz_v是需要更新的地磁传感器Y轴和Z轴的状态变化量，通过最小二乘法求解一次线性化地磁传感器各轴初始点和真实点之间的修正量，再用修正量来更新初始点，得到修正后的解为：

Mag_{Y_Z}＝Magyz_v+dMagyz_v (13)

即为组合解算记结果。

上述实施例只为说明本发明的技术构思及特点，其目的在于让熟悉此项技术的人能够了解本发明的内容并据以实施，并不能以此限制本发明的保护范围。凡根据本发明主要技术方案的精神实质所做的修饰，都应涵盖在本发明的保护范围之内。

Claims (3)

1.基于最小二乘、泰勒展开和综合残差组合的实时标定方法，其特征在于，包括：

S1、磁传感器数据通过使用一阶泰勒级数展开解算地磁YZ平面关系式，得到YZ综合残差表达式；

S2、利用最小二乘法对初始点和真实点之间的修正量进行求解，最后进行组合解算。

2.根据权利要求1所述的基于最小二乘、泰勒展开和综合残差组合的实时标定方法，其特征在于，步骤S1中，使用一阶泰勒级数展开解算地磁YZ平面关系式，在实际的飞行过程中，航向角确定，借助卫星导航所得的俯仰角可信，载体坐标系选择前上右，借助航向角和俯仰角理想值得到飞行过程中X轴的可信值，求取磁传感器的YZ平面测得量的可信值；

地磁坐标系转换为地理坐标系，转换关系表示为：

地理坐标系转换为载体坐标系，转换关系表示为：

将地理坐标系和航向角组合解算，得到载体坐标系各轴数据和姿态角的关系为：

求解磁传感器YZ平面测得量的可信值为式(4)：

其中，MagX_B为将飞行过程中可信的航向角和俯仰角代入式(3)中，解算出的磁传感器X轴的可信值，B为该地的理论地磁场值；

磁传感器YZ平面的预测量和传感器单个Y轴、Z轴值之间的关系为：

式中，MagY,MagZ为需要求解的状态量，MagYZ为通过式(4)得到的已知可信测得量；进行空中实时标定时，有一个起始点v，基于这个起始点将式(5)进行一阶泰勒级数展开就会得到式(6)：

在式(6)中，h.o.t是高阶泰勒级数项；空中飞行阶段，认为每一时刻的状态量与前面时刻的状态量都满足式(6)；

定义如下矢量：

式(9)中U_v叫做方向余弦矢量，表示Y轴和Z轴在YZ面的方向矢量，将式(7)和式(8)代入式(6)中，若令地磁传感器YZ面的测得量减去初始预测量的差值叫做YZ综合残差，用δMagYZ_v表示，省略高阶项后式(6)写成：

δMagYZ_v＝U_v·dMagyz_v (9)

式(9)是对YZ面的测得量做一次观测得到的结果，做m次迭代并线性化得到线性方程组：

将上述方程组写成矩阵的形式得到：

δMagYZ＝H·dMagyz_v (11)

式(11)中，δMagYZ＝[δMagYZ_v1,δMagYZ_v2,...δMagYZ_vm]^T；

3.根据权利要求2所述的基于最小二乘、泰勒展开和综合残差组合的实时标定方法，其特征在于，步骤S2中，利用最小二乘法求式(11)解得：

dMagyz_v＝(H^TH)^-1H^T(δMagYZ) (12)

式(12)中，dMagyz_v是需要更新的地磁传感器Y轴和Z轴的状态变化量，通过最小二乘法求解一次线性化地磁传感器各轴初始点和真实点之间的修正量，再用修正量来更新初始点，得到修正后的解为：

Mag_{Y_Z}＝Magyz_v+dMagyz_v (13)

即为组合解算记结果。

Images