CN112505667A - 一种二维声呐基阵运动姿态自校准方法 - Google Patents

Abstract

本发明提供一种二维声呐基阵运动姿态自校准方法，首先对声呐基阵接收到的回波信号进行脉冲压缩处理，分别估计“声亮点”距离基阵各顶点的整数倍时延及小数倍时延，结合二维基阵孔径建立非线性超定方程组。针对获得的非线性超定方程组构造目标函数，利用梯度法迭代求解方程组的最小二乘解，从而获得当前采样位置处的二维基阵中心、平移误差以及四顶点位置。利用系统旋转矩阵再次建立非线性超定方程组，迭代求解获得姿态误差。本方法通过对回波信号的时延估计和基阵结构建立几何方程，能够实现二维基阵运动姿态的六自由度运动误差联合自校准过程，具有不依赖于昂贵的外部辅助设备、时延估计精确高、算法实时性良好等技术优势。

Description

一种二维声呐基阵运动姿态自校准方法

技术领域

本发明涉及一种二维声呐基阵运动姿态自校准方法，属于声呐信号处理领域。

背景技术

成像声呐是目前最常用的水下地形地貌探测技术手段，已经发展出了多个技术分支，并且受到了学术机构和声呐设备厂商的广泛研究和关注。常见的成像声呐设备主要包括侧扫声呐、多波束测深声呐以及合成孔径声呐等。这些成像声呐设备通过发射和接收声信号，实现对探测区域的二维或三维成像过程。声呐基阵载体的运动姿态估计与补偿是声呐成像、成图后处理的重要因素，准确的姿态估计与补偿是稳定成图、图像拼接的重要保障。载体在三维空间内的走航会产生六个自由度的运动误差，包括：横移、纵移、升沉和横摇、纵摇、艏偏。其中，沿水平轴、航迹轴和深度轴平移的横移、纵移和升沉偏差，称为位置误差；绕水平轴、航迹轴和深度轴旋转的横摇、纵摇和艏向偏差，称为姿态误差。具有不同基阵结构和探测原理的成像声呐对于六自由度误差的敏感度不同，例如对于单线阵多波束测深声呐，横摇是影响成像效果的主要因素，纵摇和艏偏主要影响的是成像的均匀性，横移、纵移和升沉的位置误差主要影响成图的空间归位，因此认为单线阵多波束测深声呐成像效果对横摇敏感，而对于其他因素的影响稍弱，可以进行运动误差成图后补偿。又例如对于单基元合成孔径声呐，或者沿某一方向布置的多子阵接收系统，其成像效果主要受位置误差影响，在某些维度的姿态旋转仅影响接收阵元的指向性和发射波束的空间归位，并不影响阵元的空间位置，即对回波的声程差没有影响。随着成像声呐技术研究的不断发展深入，二维声呐基阵被逐步的应用到成像系统中来，能够提供更好的阵增益以及提升系统测绘效率，新机理、新设备逐渐涌现，例如二维多波束图像声呐、多波束合成孔径声呐等。

因此，对于各种成像声呐的探测新机理，六自由度的运动误差联合估计就显得尤为重要，精细化的探测技术对三维空间内的运动误差均较为敏感。现有的成像声呐运动估计技术分为两种：硬件估计和软件估计。其中，硬件估计主要指基于姿态仪、罗经、GPS等外部运动估计辅助设备的估计方法，其优点是估计实时性好、精度上限高，缺点是价格昂贵、占用空间大，并且无法在水下小空间密闭舱体使用，限制了水下无人环境下的应用；软件估计主要是在合成孔径声呐系统中，通过目标回波自聚焦的方式进行运动姿态估计，继而进行成像补偿，其优点是成本节约，不需要额外增加外部辅助设备，缺点是现有方法不完全适用于二维声呐基阵结构。

针对现有方法的不足之处，为了更好的估计二维声呐基阵的运动姿态误差，本发明公开了一种二维声呐基阵运动姿态自校准方法，通过对回波信号的时延估计和基阵结构建立几何方程组，通过高斯迭代方法求解非线性超定方程组。能够实现二维基阵运动姿态的六自由度联合自校准过程，具有不依赖于昂贵的外部辅助设备、时延估计精确高、算法实时性良好等技术优势。

发明内容

本发明的目的是为了配合多波束合成孔径声呐探测新机理，实现二维基阵运动姿态的六自由度联合自校准过程，目的是不依赖于昂贵的外部辅助设备，提升时延估计精度，保障算法的实时性。

本发明的目的是这样实现的：步骤如下：

步骤一：选取二维基阵的三顶点位置处的接收基元，对接收到的回波信号进行脉冲压缩处理，选取探测区域的三个“声亮点”目标，分别估计目标至顶点位置处的整数倍时延以及小数倍时延，计算目标斜距；

步骤二：利用获得的目标斜距以及二维基阵的接收孔径，根据几何关系建立非线性超定方程组并构造目标函数，通过梯度法求解非线性超定方程组的最小二乘解；

步骤三：根据上述步骤中获得的最小二乘解估计出基阵的几何中心及其四顶点坐标，从而计算基阵的平移误差；

步骤四：利用系统旋转矩阵和上述步骤中估计得到的顶点坐标和平移误差，再次建立非线性超定方程组，通过求解获得基阵的姿态误差，从而联合估计系统六自由度运动误差。

本发明还包括这样一些结构特征：

1.步骤一具体为：系统选取线性调频信号作为探测信号，发射信号为：

其中，N为发射信号脉宽，f₀为信号中心频率，f_s为系统采样率，n为采样序号；接收信号时延点数可以表示为t+Δt，其中整数倍时延点数t可以由相关峰计算得到，接收信号形式为：

经脉冲压缩处理后的相关函数表示为：

对互相关信号R(m)提取出其相位P，则在m＝N时刻的时延点数估计残差有如下关系：

求解一元二次方程根可得时延估计残差点数Δt，即为小数倍时延：

2.步骤二具体为：利用获得的目标斜距以及二维基阵的接收孔径，根据几何关系建立非线性超定方程组；基于“声亮点”的运动误差自校准方法利用上一次基阵所在位置估计当前基阵所在位置，因此首先估计独立亮点目标到基阵三顶点位置的时延；设定探测区域存在三个独立亮点E(x₁,y₁,z₁)，F(x₂,y₂,z₂)和G(x₃,y₃,z₃)，载体初始位置P₀处选取三个位置已知的顶点A₀(x₀₁,y₀₁,z₀₁)，B₀(x₀₂,y₀₂,z₀₂)和C₀(x₀₃,y₀₃,z₀₃)，载体偏移位置P₁处选取三个位置需要估计的顶点A₁(x₁₁,y₁₁,z₁₁)，B₁(x₁₂,y₁₂,z₁₂)和C₁(x₁₃,y₁₃,z₁₃)，二维基阵航迹向实孔径d_y和水平向实孔径d_x已知；

在载体初始位置P₀处，根据相关函数可以确定载体在偏移位置P₁处时亮点E至A₁、B₁和C₁三顶点处的距离

和

并且有距离关系式：

对于亮点E和F也具有关系式：

对于二维面阵结构的接收阵，其任意三顶点位于直角三角形上，有如下关系：

根据上述建立关系方程组F(ξ)为：

3.步骤二中的构造目标函数，通过梯度法求解非线性超定方程组的最小二乘解；目标函数形如：

梯度法迭代按照以下步骤进行：

(1)给定初始位置处的基阵三顶点坐标作为迭代初始值，设定ξ^k为第k次迭代获得的估计值；

(2)计算目标函数G(ξ^k)的值，如果G(ξ^k)＜ε则认为此时的ξ^k为所求最小二乘解，否则需要对迭代解进行修正，对于目标函数G(ξ^k)的梯度可以通过对ξ求偏导获得；

(3)重复修正迭代解

直到满足收敛精度(G₁ ^k+1,G₂ ^k+1,…,G₁₂ ^k+1)＜ε，终止计算。

4.步骤三具体为：根据上述步骤中获得的最小二乘解估计出基阵的几何中心及其四顶点坐标，从而计算基阵的平移误差；通过上述梯度迭代方法，估计出载体在P₁位置处的三顶点坐标

和

并且计算得到基阵中心O'和第四个顶点位置

5.步骤四具体为：利用系统旋转矩阵和上述步骤中估计得到的顶点坐标和平移误差，再次建立非线性超定方程组，通过求解获得基阵的姿态误差；其中旋转矩阵定义为M_{＜α,β,γ＞}，且有如下关系式：

上述约束条件为具有3个未知数和9个方程的非线性超定方程组，同样可以用上述梯度法迭代搜索最小二乘解，由此估计出载体的姿态误差(Δα,Δβ,Δγ)。

与现有技术相比，本发明的有益效果是：

1、通过最小二乘法估计二维基阵的六自由度联合误差，相对于单自由度的误差估计具有更好的工程适用性。

2、通过整数倍估计和小数倍估计结合的方式，提高目标至基阵的时延估计精度，从而提升六自由度联合估计的准确性。

3、采用针对目标回波的软件运动误差估计方法，利用二维基阵接收到的回波信号及其几何关系，实现载体运动姿态的自校准过程。避免了硬件估计带来的高成本和对于装载空间的要求，适用于水下无人小空间的工程化应用场景。

附图说明

图1六自由度联合误差估计示意图

图2载体运动误差轨迹及其估计结果示意图

图3载体位置估计结果与预设值之间的偏差

图4载体姿态估计结果与预设值之间的偏差

图5运动误差估计与补偿之前的成像结果

图6运动误差估计与补偿之后的成像结果

具体实施方式

下面结合附图与具体实施方式对本发明作进一步详细描述。

如图1中所示，本发明涉及一种二维声呐基阵运动姿态自校准方法，包含以下步骤：

步骤(1)：选取二维基阵的三顶点位置处的接收基元，对接收到的回波信号进行脉冲压缩处理，选取探测区域的三个“声亮点”目标，分别估计目标至顶点位置处的整数倍时延以及小数倍时延，计算目标斜距。

步骤(2)：利用获得的目标斜距以及二维基阵的接收孔径，根据几何关系建立非线性超定方程组并构造目标函数，通过梯度法求解非线性超定方程组的最小二乘解。

步骤(3)：根据上述步骤中获得的最小二乘解估计出基阵的几何中心及其四顶点坐标，从而计算基阵的平移误差。

步骤(4)：利用系统旋转矩阵和上述步骤中估计得到的顶点坐标和平移误差，再次建立非线性超定方程组，通过求解获得基阵的姿态误差，从而联合估计系统六自由度运动误差。

步骤(1)中，选取二维基阵的三顶点位置处的接收基元，对接收到的回波信号进行脉冲压缩处理，选取探测区域的三个“声亮点”目标，如图1所示。在三维空间内，选取三个亮点位置，估计这些亮点与二维基阵的其中三个顶点之间的距离，从而确定基阵位置。基于“声亮点”的六自由度运动误差联合估计方法利用上一次基阵所在位置估计当前基阵所在位置，因此首先估计独立亮点目标到基阵三顶点位置的时延。设定探测区域存在三个独立亮点E(x₁,y₁,z₁)，F(x₂,y₂,z₂)和G(x₃,y₃,z₃)，载体初始位置P₀处选取三个位置已知的顶点A₀(x₀₁,y₀₁,z₀₁)，B₀(x₀₂,y₀₂,z₀₂)和C₀(x₀₃,y₀₃,z₀₃)，载体偏移位置P₁处选取三个位置需要估计的顶点A₁(x₁₁,y₁₁,z₁₁)，B₁(x₁₂,y₁₂,z₁₂)和C₁(x₁₃,y₁₃,z₁₃)，二维基阵航迹向实孔径d_y和水平向实孔径d_x已知。对于离散线性调频信号，发射信号可以表示为下式：

其中，N为发射信号脉宽，f₀为信号中心频率，f_s为系统采样率，n为采样序号。当发射信号经目标反射回接收基元时，其时延点数可以表示为t+Δt，其中整数倍时延点数t可以由相关峰计算得到，因此只考虑其时延点数估计残差Δt，信号形式为：

对接收信号进行匹配滤波处理，可以得到离散信号表达式如下所示，可以观察到当时刻m＝N+Δt时，R(m)取得幅值最大值且此时的相位项为0。由此，可以利用信号相关峰处的相位值求解时延点数残差Δt，由于点数残差Δt恒小于一个采样点对应的时间门，因此其相位变化处于主值区间内不存在相位模糊问题。

对互相关信号R(m)提取出其相位P，则在m＝N时刻的时延点数估计残差有如下关系：

求解一元二次方程根可得时延估计残差点数Δt：

方程具有两个平方根，根据约束条件|Δt|＜1可以确定其中的有效解，因此利用相关峰及其时延点数估计残差可以计算得到精确的目标与阵元之间的时延值τ，根据声速可以计算获得独立亮点至接收阵元之间的斜距l。

步骤(2)中，利用获得的目标斜距以及二维基阵的接收孔径，根据几何关系建立非线性超定方程组并构造目标函数，通过梯度法求解非线性超定方程组的最小二乘解。

如图1所示，在载体初始位置P₀处，可以根据亮点E至A₀、B₀和C₀三顶点处的距离唯一确定其坐标位置，根据相关函数可以确定载体在偏移位置P₁处时亮点E至A₁、B₁和C₁三顶点处的距离

和

并且有距离关系式：

类似的，对于亮点E和F也具有关系式：

此外，对于二维面阵结构的接收阵，其任意三顶点位于直角三角形上，有如下关系：

为了计算方便，将上述方程组改写成如下形式：

上述方程组具有三个顶点坐标的9个未知数和12个方程的限定条件，并且限定条件为二次方程组，因此其解析解可能不存在，需要通过求解非线性超定方程组最小二乘解的方式，求取三顶点坐标。对于非线性超定方程组的解法，首先需要将其转换成恰定方程组，之后再通过迭代法求解未知数。构造目标函数g(x)，利用梯度法求解函数的最小二乘解：

从某一点出发，将函数在该点下降最快的方向作为搜索方向，则其负梯度方向为该点下降最快的方向。由此可以将问题转化为一组沿负梯度方向搜索目标函数最优解的问题。对于目标函数G(ξ)，寻找使其达到最小值的ξ，即为所寻找的最小二乘解，其中未知数ξ＝(x₁₁,y₁₁,z₁₁,x₁₂,y₁₂,z₁₂,x₁₃,y₁₃,z₁₃)。设定迭代门限ε，当进行第k此迭代时，该组解ξ^k能够满足G(ξ^k)＜ε，则终止迭代。

梯度法迭代按照以下步骤进行：

(1)给定初始位置处的基阵三顶点坐标ξ⁰＝(x₀₁,y₀₁,z₀₁,x₀₂,y₀₂,z₀₂,x₀₃,y₀₃,z₀₃)，作为迭代初始值，设定ξ^k为第k次迭代获得的估计值。

(2)计算目标函数G(ξ^k)的值，如果G(ξ^k)＜ε则认为此时的ξ^k为所求最小二乘解，否则需要对迭代解进行如下修正：

对于目标函数G(ξ^k)的梯度可以通过对ξ求偏导获得：

其中，

H控制收敛常数。

(3)重复修正迭代解

直到满足收敛精度(G₁ ^k+1,G₂ ^k+1,…,G₁₂ ^k+1)＜ε，终止计算。

步骤(3)中，根据上述步骤中获得的最小二乘解估计出基阵的几何中心及其四顶点坐标，从而计算基阵的平移误差。通过上述步骤中的梯度迭代方法，可以估计出载体在P₁位置处的三顶点坐标

和

并且可以计算得到基阵中心和第四个顶点位置：

步骤(4)中，利用系统旋转矩阵和上述步骤中估计得到的顶点坐标和平移误差，再次建立非线性超定方程组，通过求解获得基阵的姿态误差。对于载体中心，其位置不受姿态变化的影响，因此其位置的变化反映了载体的平移误差，由此可以获得载体的位置误差(Δx,Δy,Δz)。根据二维基阵的四个顶点位置，结合设计的基阵阵元间距，可以步进求取所有接收基元的实际空间位置。此外，还可以通过三维空间变换逆矩阵求取载体的姿态误差，其中P_r0 ^-1为初始位置P_r0的广义逆。

M_{＜α,β,γ＞}＝(P_r1-P_Δ)×P_r0 ^-1

通过旋转矩阵M_{＜α,β,γ＞}＝M_αN_βQ_γ的定义，可以获得约束关系式：

上述约束条件为具有3个未知数和9个方程的非线性超定方程组，同样可以用上述梯度法迭代搜索最小二乘解，由此可以估计出载体的姿态误差(Δα,Δβ,Δγ)。

下面结合本申请方法的一种具体应用仿真对本申请进行更详细的阐述：选取二维多波束合成孔径声呐系统基阵结构，系统信号中心频率f₀＝150kHz，信号带宽B＝10kHz，脉冲宽度τ＝10ms，发射阵元实孔径尺寸D＝10cm。水平向阵元数目32，阵元间距5mm，航迹向阵元数目4，阵元间距11cm。设定载体运动速度为0.2m/s，理论运动轨迹应为沿航迹向的匀速直线运动。在理论运动轨迹上叠加误差曲线，设定载体的位置误差按照幅值为1m的正弦波变化，姿态误差按照幅值为10°的正弦波变化。利用本申请提出的二维基阵姿态自校准方法估计载体的运动轨迹及运动偏差，载体运动轨迹如图2所示，以基阵中心点位置作为载体的运动轨迹。可以观察到，经过最小二乘估计后的载体中心位置与预设存在运动误差时的载体中心位置吻合度较好，能够准确的反映载体的实际位置。

利用本申请提出的二维基阵姿态自校准方法联合估计六个自由度的位置误差和姿态误差，算法估计结果与预设结果之间的偏差如图3和图4所示。六自由度误差随着航迹向采样位置变化，位置误差的估计偏差保持在厘米量级，姿态误差的估计偏差保持在0.05°范围，算法仿真结果证明了基于六自由度联合误差估计算法的有效性。此外，还利用具有8个运算核心的DSP器件TMS320C6678进行了迭代算法仿真，CPU工作在单核、主频1GHz情况下，运动误差估计算法运行时间约为毫秒量级，经器件并行化处理后会获得一定的加速度比，因此计算时间会进一步减少，保证了联合估计算法在工程应用条件下的运算实时性。

图4和图5分别展示了仿真硬件姿态估计方法精度不足时的成像效果和经本申请中提出的自校准方法成像结果，给定仿真条件，载体以0.5m/s的速度航行，在理论运动轨迹上叠加误差曲线，设定载体的位置误差按照幅值为1m的正弦波变化，姿态误差按照幅值为10°的正弦波变化。设定30个独立点目标处于深度值15m的平面上，目标间距20cm，利用多波束合成孔径声呐成像算法对探测区域进行三维成像。首先，模拟硬件补偿精度不足情况下的成像结果，将预设的载体位置精度范围控制在2cm范围内，将姿态精度控制在0.2°范围内，对姿态信息进行截断。图4与图5对比可以发现，虽然目标所在区域成像效果基本正确，未发生严重的图像散焦，但目标个数和位置发生混叠，成像效果未达到理论效果。经本申请中提出的自校准方法联合估计六自由度运动误差，对获得的运动误差补偿后的成像效果显示目标位置和间距与理论预设值相符，并且目标位置的能量聚集程度较好，达到了预期效果，证明了本申请提出方法的有效性。

本申使用最小二乘估计方法计算二维基阵的六自由度联合误差，相对于单自由度的误差估计具有更好的工程适用性。通过整数倍估计和小数倍估计结合的方式，提高目标至基阵的时延估计精度，从而提升六自由度联合估计的准确性。采用针对目标回波的软件运动误差估计方法，利用二维基阵接收到的回波信号及其几何关系，实现载体运动姿态的自校准过程。避免了硬件估计带来的高成本和对于装载空间的要求限制，适用于水下无人小空间的工程化应用场景。

综上，本发明公开了一种二维声呐基阵运动姿态自校准方法。首先对声呐基阵接收到的回波信号进行脉冲压缩处理，分别估计“声亮点”距离基阵各顶点的整数倍时延及小数倍时延，结合二维基阵孔径建立非线性超定方程组。针对获得的非线性超定方程组构造目标函数，利用梯度法迭代求解方程组的最小二乘解，从而获得当前采样位置处的二维基阵中心、平移误差以及四顶点位置。利用系统旋转矩阵再次建立非线性超定方程组，迭代求解获得姿态误差。本方法通过对回波信号的时延估计和基阵结构建立几何方程，能够实现二维基阵运动姿态的六自由度运动误差联合自校准过程，具有不依赖于昂贵的外部辅助设备、时延估计精确高、算法实时性良好等技术优势。

Claims (6)

1.一种二维声呐基阵运动姿态自校准方法，其特征在于：步骤如下：

步骤一：选取二维基阵的三顶点位置处的接收基元，对接收到的回波信号进行脉冲压缩处理，选取探测区域的三个“声亮点”目标，分别估计目标至顶点位置处的整数倍时延以及小数倍时延，计算目标斜距；

步骤二：利用获得的目标斜距以及二维基阵的接收孔径，根据几何关系建立非线性超定方程组并构造目标函数，通过梯度法求解非线性超定方程组的最小二乘解；

步骤三：根据上述步骤中获得的最小二乘解估计出基阵的几何中心及其四顶点坐标，从而计算基阵的平移误差；

步骤四：利用系统旋转矩阵和上述步骤中估计得到的顶点坐标和平移误差，再次建立非线性超定方程组，通过求解获得基阵的姿态误差，从而联合估计系统六自由度运动误差。

2.根据权利要求1所述的一种二维声呐基阵运动姿态自校准方法，其特征在于：步骤一具体为：系统选取线性调频信号作为探测信号，发射信号为：

其中，N为发射信号脉宽，f₀为信号中心频率，f_s为系统采样率，n为采样序号；接收信号时延点数可以表示为t+Δt，其中整数倍时延点数t可以由相关峰计算得到，接收信号形式为：

经脉冲压缩处理后的相关函数表示为：

对互相关信号R(m)提取出其相位P，则在m＝N时刻的时延点数估计残差有如下关系：

求解一元二次方程根可得时延估计残差点数Δt，即为小数倍时延：

3.根据权利要求2所述的一种二维声呐基阵运动姿态自校准方法，其特征在于：步骤二具体为：利用获得的目标斜距以及二维基阵的接收孔径，根据几何关系建立非线性超定方程组；基于“声亮点”的运动误差自校准方法利用上一次基阵所在位置估计当前基阵所在位置，因此首先估计独立亮点目标到基阵三顶点位置的时延；设定探测区域存在三个独立亮点E(x₁,y₁,z₁)，F(x₂,y₂,z₂)和G(x₃,y₃,z₃)，载体初始位置P₀处选取三个位置已知的顶点A₀(x₀₁,y₀₁,z₀₁)，B₀(x₀₂,y₀₂,z₀₂)和C₀(x₀₃,y₀₃,z₀₃)，载体偏移位置P₁处选取三个位置需要估计的顶点A₁(x₁₁,y₁₁,z₁₁)，B₁(x₁₂,y₁₂,z₁₂)和C₁(x₁₃,y₁₃,z₁₃)，二维基阵航迹向实孔径d_y和水平向实孔径d_x已知；

在载体初始位置P₀处，根据相关函数可以确定载体在偏移位置P₁处时亮点E至A₁、B₁和C₁三顶点处的距离

和

并且有距离关系式：

对于亮点E和F也具有关系式：

对于二维面阵结构的接收阵，其任意三顶点位于直角三角形上，有如下关系：

根据上述建立关系方程组F(ξ)为：

4.根据权利要求3所述的一种二维声呐基阵运动姿态自校准方法，其特征在于：步骤二中的构造目标函数，通过梯度法求解非线性超定方程组的最小二乘解；目标函数形如：

梯度法迭代按照以下步骤进行：

(1)给定初始位置处的基阵三顶点坐标作为迭代初始值，设定ξ^k为第k次迭代获得的估计值；

(2)计算目标函数G(ξ^k)的值，如果G(ξ^k)＜ε则认为此时的ξ^k为所求最小二乘解，否则需要对迭代解进行修正，对于目标函数G(ξ^k)的梯度可以通过对ξ求偏导获得；

(3)重复修正迭代解

直到满足收敛精度(G₁ ^k+1,G₂ ^k+1,…,G₁₂ ^k+1)＜ε，终止计算。

5.根据权利要求4所述的一种二维声呐基阵运动姿态自校准方法，其特征在于：步骤三具体为：根据上述步骤中获得的最小二乘解估计出基阵的几何中心及其四顶点坐标，从而计算基阵的平移误差；通过上述梯度迭代方法，估计出载体在P₁位置处的三顶点坐标

和

并且计算得到基阵中心O'和第四个顶点位置

6.根据权利要求5所述的一种二维声呐基阵运动姿态自校准方法，其特征在于：步骤四具体为：利用系统旋转矩阵和上述步骤中估计得到的顶点坐标和平移误差，再次建立非线性超定方程组，通过求解获得基阵的姿态误差；其中旋转矩阵定义为M_{＜α,β,γ＞}，且有如下关系式：

上述约束条件为具有3个未知数和9个方程的非线性超定方程组，同样可以用上述梯度法迭代搜索最小二乘解，由此估计出载体的姿态误差(Δα,Δβ,Δγ)。

Images