一种基于粒子群算法的AUV导航用磁力计标定方法技术

本发明专利技术涉及一种基于粒子群算法的AUV导航用磁力计标定方法，通过分析导致磁力计出现误差的各种因素，建立一体化的磁力计误差参数模型；针对椭球拟合法现场标定的缺点，建立一个多约束的磁力计误差方程组，充分利用多个约束条件使得标定结果更加准确；针对AUV难以满足全姿态及椭球拟合法线性化误差这两个问题，使用粒子算法代替非线性求解，更加快速的得到磁力计的所有误差参数。本发明专利技术采用粒子群算法求解标定参数，解决椭球拟合法存在线性化误差的问题，保证AUV在难以实现全方位姿态的条件下完成标定任务。完成标定任务。完成标定任务。

【技术实现步骤摘要】
一种基于粒子群算法的AUV导航用磁力计标定方法


[0001]本专利技术属于惯性测量单元磁力计标定，涉及一种基于粒子群算法的AUV导航用磁力计标定方法。

技术介绍

[0002]海洋约占地球表面的71％，海洋不仅是21世纪人类可持续发展的宝贵财富，也是国际政治博弈的重要舞台。伴随着世界经济、政治格局的加速调整以及人类对资源需求的大幅增长，海权竞争再度成为国际焦点。中国是个地理大国，同时也是个海洋大国，中国拥有面积约300万平方公里的海域，约占中国陆地面积的1/3。因此开发海洋资源，维系外贸通道，保护海洋权益是中国的必要战略要求。
[0003]自主式水下航行器(Autonomous Underwater Vehicle，AUV)是无人水下航行器(Unmanned underwater vehicle，UUV)的一种，具有高度智能化、完全自主、安全可靠、体积较小和费用较低等特点，这些特点使其在水下作战和作业方面更加灵活，日益受到发达国家军事海洋技术部门的重视，是当前海洋工程领域研究的热点之一。要使AUV完成预定的任务，离不开水下导航技术，惯性导航是水下导航中常用的方法，惯性导航是指利用陀螺仪和加速度计等惯性传感器实现自主导航任务，但惯性系统自身器件的累积误差会随着时间发散，无法满足AUV在水下长时间工作时精度和性能方面的要求。因此需要加入其它信息对导航进行修正，磁力计利用地磁场信息对航向进行修正，可以消除累积惯性导航的误差。加速度计和陀螺仪经过标定后，零偏和随机游走等参数在比较长一段时间内都无需考虑；对于磁力计，其易受周围磁场环境的影响，所以每次更换场景时都需要对其参数重新进行标定，以使其能够精确的测量AUV运行过程中的磁场强度。因此，磁力计的参数标定属于工程上必不可少的一步，其标定结果对后续导航等任务影响非常大。
[0004]当前AUV现场标定磁力计的方法存在两种分类：与姿态无关的标定和与姿态有关的标定，其中与姿态无关的标定比另一种适应性更好，其方法的实现主要依赖于以下性质：
[0005]性质1：一定范围内，局部地磁场矢量的模值为一个常数。
[0006]性质2：航姿系统及其载体原地转动时，重力矢量与地磁场矢量的点积是一个常数，即点积不变性质。
[0007]性质3：地磁矢量和重力矢量之间的交叉角是恒定的，且在某一范围内地磁矢量与重力矢量的夹角为一个常数。
[0008]磁力计最为常用的一种标定方法是椭球拟合法，该标定方法与姿态无关，主要思路是将磁力计测量的磁场数据进行椭球拟合，然后根据性质1进行标定。传统椭球拟合法的实现思路如下：
[0009]性质1的数学表述为：
[0010][0011]式中，R为当地磁场参考值；B
b
为载体系理想地磁矢量；为载体系磁力计测量地
磁矢量；K和b为待标定系数。
[0012]将上式展开可得：
[0013][0014]整理上式得：
[0015][0016]令K
T
K＝L可得：
[0017][0018]当L为正定矩阵时，上式可看成关于的椭球二次型，将其继续展开得：
[0019][0020]式中，为载体系下磁力计测量值，a
i
(i＝1,2,...,10)是标定参数的中间变量，已知有M个磁场测量值，两者结合，可得线性方程组：
[0021]AX＝β
[0022]其中：
[0023][0024][0025][0026]通常采用最小二乘法进行求解：
[0027]X
LS
＝(A
T
A)A
T
β
[0028]利用上式求解标定参数得到L和b：
[0029][0030][0031]L为对称正定矩阵，再对其进行Cholesky分解，求解得到K阵。至此标定参数K和b求解完成。
[0032]利用椭球拟合法标定磁力计虽然不需要任何外部信息，但其需要全方位的标准姿
态，对于AUV来说实现非常困难。同时，该方法对误差模型进行线性化处理求解误差参数，会使模型存在线性化误差，对参数的解算产生影响。

技术实现思路

[0033]要解决的技术问题
[0034]为了避免现有技术的不足之处，本专利技术提出一种基于粒子群算法的AUV导航用磁力计标定方法，克服椭球拟合法标定技术的不足。解决现有模型存在线性化误差的问题，保证AUV在难以实现全方位姿态的条件下完成标定任务。本专利技术可以求解出磁力计误差模型的全部参数，更快更可靠地实现现场磁力计标定任务。
[0035]技术方案
[0036]一种基于粒子群算法的AUV导航用磁力计标定方法，其特征在于步骤如下：
[0037]步骤1、构造多约束磁力计误差方程组：
[0038][0039]式中，为磁力计测量值，K和b为待标定系数，R为当地磁场参考值，为加速度计测量值，B
b
为载体系下地磁矢量的理想输出，θ为加速度输出矢量和磁场矢量B
b
的夹角；
[0040]步骤2：数据采集，确定优化函数
[0041]忽略加速度计测量误差，采集加速度计和磁力计多组数据i＝1,2,3,...,s，代入式1，令λ＝cosθ得：
[0042][0043]式中，为磁力计测量值，为加速度计测量值；
[0044]得到最后的优化函数：
[0045][0046]步骤3：利用粒子群算法求解标定系数K和b。
[0047]所述步骤3利用粒子群算法求解优化函数，得到标定结果K和b的过程如下：
[0048]步骤(1)初始化。
[0049]设定需要估计的状态量x：
[0050]x＝[K
11 K
12 K
13 K
21 K
22 K
23 K
31 K
32 K
33 b
1 b
2 b
3 λ][0051]被估计的状态量x是由磁力计参数K和b的各个分量构成，种群初始化数量为100个个体；
[0052]步骤(2)计算适应度
[0053]公式3的优化函数为多目标优化问题即K和b，公式2所示的两目标方程均是由对地磁矢量的约束条件得到的，需要求解的是目标方程的最小值，两者所占权重相同，权重系数各取0.5，最终得到适应度函数为：
[0054][0055]式中，i＝1,2,3,...,s为采样点个数；i＝1,2,3,...,s为采样点个数；
[0056]步骤(3)适应度更新
[0057]对每个粒子，将其适应度值与所经历的最好位置的适应度值进行比较，若比最好位置的适应度值好，则将其作为当前的个体最优位置；对每个粒子，将其适应度值与全局所经历的最好位置的适应度值进行比较，若比全局所经历的最好位置的适应度值好，则将其作为当前的全局最优位置；
[0058]步骤(4)位置和速度更新
[0059]根据下式调整粒子的位置和速度...

【技术保护点】

【技术特征摘要】
1.一种基于粒子群算法的AUV导航用磁力计标定方法，其特征在于步骤如下：步骤1、构造多约束磁力计误差方程组：式中，为磁力计测量值，K和b为待标定系数，R为当地磁场参考值，为加速度计测量值，B
b
为载体系下地磁矢量的理想输出，θ为加速度输出矢量和磁场矢量B
b
的夹角；步骤2：数据采集，确定优化函数忽略加速度计测量误差，采集加速度计和磁力计多组数据i＝1,2,3,...,s，代入式1，令λ＝cosθ得：式中，为磁力计测量值，为加速度计测量值；得到最后的优化函数：步骤3：利用粒子群算法求解标定系数K和b。2.根据权利要求1所述基于粒子群算法的AUV导航用磁力计标定方法，其特征在于：所述步骤3利用粒子群算法求解优化函数，得到标定结果K和b的过程如下：步骤(1)初始化。设定需要估计的状态量x：x＝[K
11 K
12 K
13 K
21 K
22 K
23 K
31 K
32 K
33 b
1 b
2 b
3 λ]被估计的状态量x是由磁力计参数K和b的各个分量构成，种群初始化数量为100个个体；步骤(2)计算适应度公式3的优化函数为多目标优化问题即K和b，公式2所示的两目标方程均是由对地磁矢量的约束条件得到的，需要求解的是目标方程的最小值，两者所占权重相同，权重系数各取0.5，最终得到适应度函数为：式中，为采样点个数；为采样点个数；步骤(3)适应度更新对每个粒子，将其适应度值与所经历的最好位置的适应度值进行比较，若比最好位置的适应...

【专利技术属性】
技术研发人员：张福斌，梁泽旸，高剑，潘光，彭星光，张立川，陈依民，杨慧博，
申请(专利权)人：西北工业大学，
类型：发明
国别省市：