CN112985380A - 基于非完整测量向量的航姿解算方法 - Google Patents

Abstract

本发明属于航姿解算技术领域，尤其基于非完整测量向量的航姿解算方法，包括当传感器故障或干扰造成a_b或m_b不完整时，重力加速度向量与地磁向量在物体坐标系中的测量值与相应向量在参考坐标系中的坐标值满足三维笛卡尔坐标变换关系，该变换为等积变换，即变换不改变向量的模值以及不同向量间的点积；本发明航姿解算仅使用向量传感器数据不需要陀螺仪等其它传感器，且计算过程不包含迭代运算，这使得解算方法的实施成本低且计算过程耗时短。

Description

基于非完整测量向量的航姿解算方法

技术领域

本发明涉及航姿解算技术领域，尤其涉及基于非完整测量向量的航姿解算方法。

背景技术

在石油定向钻井过程中需要检测井眼轨迹，以确保按预定轨迹向预定目标点钻进。检测井眼轨迹需要在多点进行航姿解算，航姿参数包括坐标变换矩阵、欧拉角、四元数等，它描述了物体坐标系在某一参考坐标系中的角位置。检测井眼轨迹时的航姿解算是指利用装载于钻柱测量单元的加速度计以及磁传感器测量重力加速度向量以及地磁向量，并利用向量测量值解算得到一组欧拉角。

向量法是一种基本航姿解算方法。它利用空间向量在参考坐标系与物体坐标系中坐标值计算航姿参数。空间向量在参考坐标系中坐标值是已知的，因此向量在物体坐标系中的测量效果是影响向量法应用的关键。在一些应用中，环境对测量造成的干扰以及传感器故障会导致向量测量值的一个或多个分量严重失真，不能用于航姿解算，造成测量向量不完整，从而使传统向量法无法使用。因此，解决非完整测量向量条件下航姿解算问题具有重要的实际意义。针对这一问题，文献1（范光第，蒲文学，赵国山.磁力随钻测斜仪轴向磁干扰校正方法[J]. 石油钻探技术,2017,45(4):121-126.）利用模值约束估计缺失分量进而完成航姿解算，这种方法不能确定航姿参数唯一解，且只能用于缺失一个分量的情况。文献2（SOO Y B, SUNG H M, HYUNWOOK W. Sensor-fault tolerant attitude determinationusing two-stage estimator[J]. Advances in Space Research, 2019, 63, 3632-3645.）利用陀螺测量信息复原向量测量缺失信息，这种方法需要使用陀螺传感器，一方面增加了应用成本，另一方面陀螺测得的角速度信息仅含有相对航姿信息且存在累积误差，不宜长时间使用。

为解决上述问题，本发明提出基于非完整测量向量的航姿解算方法；本发明提供了一种仅使用加速度计及磁传感器测量信息，在重力加速度向量或地磁向量测量分量缺失条件下实现航姿解算的方法， 该方法可用于测量向量缺失一个或两个分量的情况，且具有成本低、计算简单、实用性强的优点。

发明内容

为解决上述问题，本发明提供了基于非完整测量向量的航姿解算方法，包括以下方法步骤：设重力加速度向量a在参考坐标系中的坐标值为：a_r=[a_rx,a_ry,a_rz]^T，在物体坐标系中坐标值即加速度计测量值为：a_b=[a_bx,a_by,a_bz]^T；地磁向量m在参考坐标系中的坐标值为：m_r=[m_rx,m_ry,m_rz]^T，在物体坐标系中坐标值即磁传感器测量值为：m_b=[m_bx,m_by,m_bz]^T，以上各向量分量的角标x,y,z分别表示向量的x轴分量、y轴分量及z轴分量；首先计算参考坐标系中的一组单位正交基向量r_x,r_y,r_z：

计算物体坐标系中对应的单位正交基向量b_x,b_y,b_z：

最后计算坐标变换矩阵C：

将C转变为一组欧拉角，即航向角Ψ、俯仰角θ、横滚角γ，计算公式如下：

式中c _ij表示c的第i行第j列元素；

当传感器故障或干扰造成a_b或m_b不完整时，重力加速度向量与地磁向量在物体坐标系中的测量值与相应向量在参考坐标系中的坐标值满足三维笛卡尔坐标变换关系，该变换为等积变换，即变换不改变向量的模值以及不同向量间的点积，具体关系式可写为：

其中，符号

表示相应向量的模值，式（11）与（12）为模值约束关系式，（13）为点积约束关系式；a_r与m_r为已知量，因此式（11）、（12）及（13）右端已知，从而构成对测量向量值的约束条件。

所述重力加速度向量与地磁向量在航姿解算过程中具有对等的计算关系，且同一向量的不同分量也具有对等的计算关系。

假设m_bz缺失，航姿解算方法如下：

将m_b分量代入模值约束关系式（12），并取平方可得：

将m_b、a_b分量带入点积约束关系式（13）可得：

设

为m_bz的估计值，分别由式（14）、（15）可得：

式（16）无法唯一确定

，而式（17）的不足是当a_bz绝对值接近零时将导致较大的计算误差；为此，将两式结合使用，利用式（16）确定

的绝对值，利用式（17）确定

的符号，即：

上式中sign为符号函数，其功能是取得自变量的符号；

首先判断式（18）中根号下取值是否为负，若为负数说明干扰或测量误差影响较大，不能求解；

若能求解则将a_bz的绝对值与阈值T₁比较，当|a_bz|≥T₁时，按式（18）确定符号，否则

的符号分别取正负两种情况；得到

后首先构建磁传感器测量向量，然后进行航姿解算。

当m_by与m_bz缺失，航姿解算方法如下：

由式（14）、（15）构成方程组，m_by与m_bz为未知量，其解分别为

与

，求解方程组可得：

式中

△为二次方程根的判别式，为进一步分析解的情况，将向量分量代入式（21）可得：

注意式（22）中出现未知量，不能求解计算但可用于理论分析；

由式(22)可知判别式的理论值为非负数，采取以下两项措施构建缺失分量的实用解算方法：第一，设置一负数阈值T₂，由式（21）计算判别式，若△<T₂，说明干扰或测量误差影响较大，不能求解；第二，与无法求解条件对应并避免较大计算误差造成无效解，设置阈值T₃，若a_by ²+a_bz ²<T₃，则方程不做求解；否则将判别式值取为其绝对值然后求解。

优选的，当a_by ²+a_bz ²>T₂时，满足该条件的解对称分布于由a_b与锥面中心轴确定平面的两侧，对应缺失分量的两组解。

优选的，当地磁向量测量估计值处于由a_b与锥面中心轴确定的平面内时才能满足与a_b的夹角要求时，对应缺失分量存在唯一解的情况。

优选的，所述重力加速度向量和所述地磁向量任意一个或两个分量缺失的情况下，均适用于航姿解算方法进行计算。

本发明的上述技术方案具有如下有益的技术效果：

1、本发明不仅可以在缺失一个分量情况下使用，还可处理缺失两个分量的情况，且在多数应用条件下可将解的数量限定为至多两组，特别是在一些情况下可求得唯一解;

2、本发明对航姿解算存在唯一解、两组解、无法求解及干扰或误差过大造成不能求解的情况进行区分可以给出全面的求解分析，有效提升解算方法的实用性，并便于与其它方法结合使用;

3、本发明航姿解算仅使用向量传感器数据不需要陀螺仪等其它传感器，且计算过程不包含迭代运算，这使得解算方法的实施成本低且计算过程耗时短。

附图说明

图1为m_bz缺失时航姿解算方法流程图；

图2为m_by与m_bz缺失时航姿解算方法流程图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明了，下面结合具体实施方式并参照附图，对本发明进一步详细说明。应该理解，这些描述只是示例性的，而并非要限制本发明的范围。此外，在以下说明中，省略了对公知结构和技术的描述，以避免不必要地混淆本发明的概念。

如图1-2所示，本发明提出的基于非完整测量向量的航姿解算方法，设重力加速度向量a在参考坐标系中的坐标值为：a_r=[a_rx,a_ry,a_rz]^T，在物体坐标系中坐标值即加速度计测量值为：a_b=[a_bx,a_by,a_bz]^T；地磁向量m在参考坐标系中的坐标值为：m_r=[m_rx,m_ry,m_rz]^T，在物体坐标系中坐标值即磁传感器测量值为：m_b=[m_bx,m_by,m_bz]^T，以上各向量分量的角标x,y,z分别表示向量的x轴分量、y轴分量及z轴分量，当测量值a_b与m_b完整时，航姿解算过程如下：

首先计算参考坐标系中的一组单位正交基向量r_x,r_y,r_z：

然后计算物体坐标系中对应的单位正交基向量b_x,b_y,b_z：

最后计算坐标变换矩阵C：

将C转变为一组欧拉角，即航向角Ψ、俯仰角θ、横滚角γ，计算公式如下：

式中c ^ij表示c的第i行第j列元素；

当传感器故障或干扰造成a_b或m_b不完整时，无法使用上述方法解算航姿参数。为此，首先利用模值约束与点积约束条件估计缺失分量值然后进行航姿解算。重力加速度向量与地磁向量在物体坐标系中的测量值与相应向量在参考坐标系中的坐标值满足三维笛卡尔坐标变换关系，该变换为等积变换，即变换不改变向量的模值以及不同向量间的点积，具体关系式可写为：

其中，符号

表示相应向量的模值，式（11）与（12）为模值约束关系式，（13）为点积约束关系式。在应用中，a_r与m_r为已知量，因此式（11）、（12）及（13）右端已知，从而构成对测量向量值的约束条件。

本发明给出了缺失一个及两个分量情况下的航姿解算方法，在下文中将针对m_b缺失m_bz及缺失m_by与m_bz两种情况展开说明。需指出的是，重力加速度向量与地磁向量在航姿解算过程中具有对等的计算关系，同一向量的不同分量亦具有对等的计算关系，因此，下文针对具体情况的解算方法具有一般性，可推广至其它分量缺失情况。

1、缺失一个分量的情况

假设m_bz缺失，航姿解算方法如下：

将m_b分量代入模值约束关系式（12），并取平方可得：

将m_b、a_b分量带入点积约束关系式（13）可得：

设

为m_bz的估计值，分别由式（14）、（15）可得：

式（16）无法唯一确定

，而式（17）的不足是当a_bz绝对值接近零时将导致较大的计算误差；为此，将两式结合使用，利用式（16）确定

的绝对值，利用式（17）确定

的符号，即：

上式中sign为符号函数，其功能是取得自变量的符号。在应用中上述做法仍存在缺陷。首先，由于受到干扰或测量误差影响，式（18）中根号下取值可能为负；其次，理论上当a_bz非零时，式（17）的符号是准确的，但在应用中受测量误差与噪声的影响，当重力加速度向量在物体坐标系z轴分量绝对值接近零时，其测量值a_bz的符号具有随机性，这将导致

估计错误进而导致航姿解算错误。为此，在构建实用算法时，首先判断式（18）中根号下取值是否为负，若为负数说明干扰或测量误差影响较大，不能求解；若能求解则将a_bz的绝对值与阈值T₁比较，当|a_bz|≥T₁时，按式（18）确定符号，否则

的符号分别取正负两种情况。得到

后首先构建磁传感器测量向量，然后进行航姿解算。综上所述，在缺失

情况下进行航姿解算的方法如图1所示。

由上述方法可知，当a_bz的绝对值小于阈值时航姿解算存在两个解，分别对应

符号取正负的两种情况。现对这一问题做进一步说明，在应用中a_bz的绝对值小于阈值的情况对应于重力加速度向量在物体坐标系z轴分量为零的情况，由式（15）可知，此时点积约束无法对

构成有效约束。从几何关系角度看，当向量模值不变时，点积约束意味着向量间夹角不变，重力加速度向量在物体坐标系z轴分量为零说明该向量处于物体坐标系xy平面内，此时地磁向量在物体坐标系z轴分量的符号为正或负两种情况下，其与重力加速度向量的夹角相同。由此可知，当重力加速度向量在物体坐标系z轴分量为零时，点积约束无法发挥作用，只能利用模值约束估计缺失分量，从而导致航姿解算解不唯一。应当指出，在应用中阈值T₁取值较小，在大多数情况下航姿解算可取得唯一解。

2、缺失两个分量的情况

假设m_by与m_bZ缺失，航姿解算方法如下：

由式（14）、（15）构成方程组，m_by与m_bZ为未知量，其解分别为

与

，求解方程组可得：

式中△为二次方程根的判别式，为进一步分析解的情况，将向量分量代入式（21）可得：

注意式（22）中出现未知量，不能求解计算但可用于理论分析。

由式(22)可知判别式的理论值为非负数，但在应用中受到干扰或测量误差影响，判别式的计算值可能为负值。此外，由式（19）至式（22）可知：在理论上，当a_by与a_bz不全为零时方程组存在一组或两组实数解，否则方程无法求解。但在工程应用中应考虑到当a_by与a_bz均接近零时，求解结果会存在较大误差。基于上述分析，采取以下两项措施构建缺失分量的实用解算方法：第一，设置一负数阈值T ₂，由式（21）计算判别式，若△<T ₂，说明干扰或测量误差影响较大，不能求解；第二，与无法求解条件对应并避免较大计算误差造成无效解，设置阈值T ₃，若a_by ²+a_bz ²<T₃，则方程不做求解；否则将判别式值取为其绝对值然后求解。综上所述，在缺失m_by与m_bz情况下进行航姿解算的方法如图2所示。

图2中，

与

分别是式（19）、（20）的第一组解与第二组解，当判别式为零时，两组解相等，可求得唯一解，由此可知，若不考虑较大干扰或误差影响的情况，缺失分量的求解计算可分为无法求解、两组解、唯一解三种情况。无法求解对应于a_by与a_bz均为零的情况，由式（15）可知，此时点积约束已失去作用，缺失分量的解仅受模值约束，由式（14）可知，缺失分量的平方和为一常数，满足此条件的无穷多解构成物体坐标系yz平面内以原点为圆心的圆，与此相对应的地磁向量测量估计值

构成了物体坐标系中以x轴为中心轴，原点为顶点的圆锥面，因此，无法求解的情况并非无解，而是无法确定有限个数解。当a_by ²+a_bz ²>T₂时，点积约束发挥作用，具体表现为在地磁向量测量估计值分布于上述圆锥面的基础上进一步要求其与a_b夹角为某一固定值，显然，满足该条件的解必然对称分布于由a_b与锥面中心轴确定平面的两侧，即对应缺失分量的两组解。唯一解是两组解的一种特殊情况，只有当地磁向量测量估计值处于由a_b与锥面中心轴确定的平面内时才能满足与a_b的夹角要求时，即为缺失分量存在唯一解的情况。还应指出，理论上判别式存在取零值的情况，但在实际应用中受各种因素影响，判别式计算值为零的概率很低，因此，当缺失两个测量分量时无法求解与唯一解均为特殊情况，在多数情况下可获得两组航姿解算结果。

为验证上述航姿解算方法的效果，给出计算实例，基本过程是：首先应用完整向量测量数据进行航姿解算，得到欧拉角参考值，然后去除磁传感器测量数据中的相关分量，采用本发明方法进行航姿解算，得到欧拉角的解算值，最后对欧拉角参考值与解算值进行对比。实验中参考坐标系为东北天坐标系，物体坐标系为传感器坐标系，设置阈值T ₁₌|a_r|cos(89)=0.171，T ₂ =-10 ⁷ ，T ₃ =2T ₁ ² =0.059，具体算例如下。

实施例1

在参考坐标系下a_r=[0,0,9.8] ^T ，m_r= [-3627,29344,44065] ^T ，加速度计完整测量向量a_b=[-4.85,-1.72,8.35] ^T ，磁传感器完整测量向量m_b= [44236,20482,-21503] ^T 。以上各量中，加速度向量单位为米/秒²，磁向量单位为纳特。由式（1）至式（10）可得航向角参考值为：65.85度，俯仰角参考值为：29.64度，横滚角参考值为：-11.61度。

去除m_bz，使地磁测量向量缺失一个分量。航姿计算过程如下：

（1）m_bx ²+m_by ²=2.3763×10⁹，

，因此，

（2）由式（18）可得

=-20968

（3）a_bz=8.35，因为|a_bz|≥T ₁，所以构建地磁测量向量的估计值 [44236,20482,-20968] ^T

（4）由式（1）至式（10）可得航向角解算值为：65.93度，俯仰角解算值为：29.64度，横滚角解算值为：-11.61度。

对比参考值与解算值可知，航向角误差为0.08度，俯仰角与横滚角误差为0度。

实施例2

在参考坐标系下a_r=[0,0,9.8] ^T ，m_r= [-3627,29344,44065] ^T ,加速度计完整测量向量 a_b=[2.62,-9.43,0.12] ^T ，磁传感器完整测量向量m_b= [9408,48577,19600] ^T 。以上各量中，加速度向量单位为米/秒²，磁向量单位为纳特。由式（1）至式（10）可得航向角参考值为：54.47度，俯仰角参考值为：-15.53度，横滚角参考值为：-89.27度。

去除m_bz使地磁测量向量缺失一个分量。航姿计算过程如下：

（1）m_bx ²+m_by ²=2.4482×10⁹，

，因此，

（2）由式（18）可得

=19176

（3）abz=0.12，因为|abz|<T₁，所以构建地磁测量向量的估计值[9408,48577,19176] ^T 与[9408,48577,-19176] ^T

（4）由式（1）至式（10）并分别使用以上两个地磁测量向量的估计值可得第一组解航向角解算值为：54.58度，俯仰角解算值为：-15.53度，横滚角解算值为：-89.27度；第二组解航向角解算值为：137.03度，俯仰角解算值为：-15.53度，横滚角解算值为：-89.27度。

对比参考值与解算值可知，第一组解与参考值对应，其航向角误差为0.11度，俯仰角与横滚角误差为0度。

实施例3

在参考坐标系下a_r=[0,0,9.8] ^T ，m_r= [-3627,29344,44065] ^T ，加速度计完整测量向量 a_b=[9.40,-2.62,-1.00] ^T ，磁传感器完整测量向量m_b= [-34476,31212,25604] ^T 。以上各量中，加速度向量单位为米/秒²，磁向量单位为纳特。由式（1）至式（10）可得航向角参考值为：71.66度，俯仰角参考值为：-73.39度，横滚角参考值为：-110.96度。

去除m_by与m_bz，使地磁测量向量缺失两个分量。航姿计算过程如下：

（1）由式（21）可得△=1.1446×10⁹，因此，△>T ₂

（2）a_by ²+a_bz ²=7.85，因此，a_by ²+a_bz ²>T ₃

（3）对△取绝对值

（4）由式（19）、（20）可得

=40263,

=2496，

=31627,

=25042

（5）构建地磁测量向量的估计值 [-34476,40263,2496] ^T 与[-34476,31627,25042] ^T

（6）由式（1）至式（10）并分别使用以上两个地磁测量向量的估计值可得第一组解航向角解算值为：121.11度，俯仰角解算值为：-73.39度，横滚角解算值为：-110.96度；第二组解航向角解算值为：72.98度，俯仰角解算值为：-73.39度，横滚角解算值为：-110.96度

对比参考值与解算值可知，第二组解与参考值对应，其航向角误差为1.32度，俯仰角与横滚角误差为0度。

本发明具有以下技术效果：

1、同时采用模值约束与点积约束条件估计缺失分量值，提供了一种在非完整测量向量条件下进行航姿解算的新技术方案。该方案不仅可以在缺失一个分量情况下使用，还可处理缺失两个分量的情况，且在多数应用条件下可将解的数量限定为至多两组，特别是在一些情况下可求得唯一解;

2、对航姿解算存在唯一解、两组解、无法求解及干扰或误差过大造成不能求解的情况进行区分可以给出全面的求解分析，有效提升解算方法的实用性，并便于与其它方法结合使用;

3、航姿解算仅使用向量传感器数据不需要陀螺仪等其它传感器，且计算过程不包含迭代运算。这使得解算方法的实施成本低且计算过程耗时短。

以上显示和描述了本发明的基本原理、主要特征和本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的仅为本发明的优选例，并不用来限制本发明，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。

Claims (4)

1.基于非完整测量向量的航姿解算方法，其特征在于，包括以下方法步骤：设重力加速度向量a在参考坐标系中的坐标值为：a_r=[a_rx,a_ry,a_rz]^T，在物体坐标系中坐标值即加速度计测量值为：a_b=[a_bx,a_by,a_bz]^T；地磁向量m在参考坐标系中的坐标值为：m_r=[m_rx,m_ry,m_rz]^T，在物体坐标系中坐标值即磁传感器测量值为：m_b=[m_bx,m_by,m_bz]^T，以上各向量分量的角标x,y,z分别表示向量的x轴分量、y轴分量及z轴分量；

首先计算参考坐标系中的一组单位正交基向量r_x,r_y,r_z：

计算物体坐标系中对应的单位正交基向量b_x,b_y,b_z：

最后计算坐标变换矩阵C：

将C转变为一组欧拉角，即航向角Ψ、俯仰角θ、横滚角γ，计算公式如下：

式中c _ij表示c的第i行第j列元素；

当传感器故障或干扰造成a_b或m_b不完整时，重力加速度向量与地磁向量在物体坐标系中的测量值与相应向量在参考坐标系中的坐标值满足三维笛卡尔坐标变换关系，该变换为等积变换，即变换不改变向量的模值以及不同向量间的点积，具体关系式可写为：

其中，符号

表示相应向量的模值，式（11）与（12）为模值约束关系式，（13）为点积约束关系式；a_r与m_r为已知量，因此式（11）、（12）及（13）右端已知，从而构成对测量向量值的约束条件；

所述重力加速度向量与地磁向量在航姿解算过程中具有对等的计算关系，且同一向量的不同分量也具有对等的计算关系；

假设m_bz缺失，航姿解算方法如下：

将m_b分量代入模值约束关系式（12），并取平方可得：

将m_b、a_b分量带入点积约束关系式（13）可得：

设

为m_bz的估计值，分别由式（14）、（15）可得：

式（16）无法唯一确定

，而式（17）的不足是当a_bz绝对值接近零时将导致较大的计算误差；为此，将两式结合使用，利用式（16）确定

的绝对值，利用式（17）确定

的符号，即：

上式中sign为符号函数，其功能是取得自变量的符号；

首先判断式（18）中根号下取值是否为负，若为负数说明干扰或测量误差影响较大，不能求解；

若能求解则将a_bz的绝对值与阈值T ₁比较，当|a_bz|≥T₁时，按式（18）确定符号，否则

的符号分别取正负两种情况；得到

后首先构建磁传感器测量向量，然后进行航姿解算；

当m_by与m_bz失，航姿解算方法如下：

由式（14）、（15）构成方程组，m_by与m_bz为未知量，其解分别为

与

，求解方程组可得：

式中△为二次方程根的判别式，为进一步分析解的情况，将向量分量代入式（21）可得：

注意式（22）中出现未知量，不能求解计算但可用于理论分析；

由式(22)可知判别式的理论值为非负数，采取以下两项措施构建缺失分量的实用解算方法：第一，设置一负数阈值T ₂，由式（21）计算判别式，若△<T ₂，说明干扰或测量误差影响较大，不能求解；第二，与无法求解条件对应并避免较大计算误差造成无效解，设置阈值T ₃，若a_by ²+a_bz ²<T₃，则方程不做求解；否则将判别式值取为其绝对值然后求解。

2.根据权利要求1所述的基于非完整测量向量的航姿解算方法，其特征在于，当a_by ²+a_bz ²>T₂时，满足该条件的解对称分布于由a_b与锥面中心轴确定平面的两侧，对应缺失分量的两组解。

3.根据权利要求1所述的基于非完整测量向量的航姿解算方法，其特征在于，当地磁向量测量估计值处于由a_b与锥面中心轴确定的平面内时才能满足与a_b的夹角要求时，对应缺失分量存在唯一解的情况。

4.根据权利要求1所述的基于非完整测量向量的航姿解算方法，其特征在于，所述重力加速度向量和所述地磁向量任意一个或两个分量缺失的情况下，均适用于权利要求1所述的基于非完整测量向量的航姿解算方法进行计算。

Images