考虑洋流影响的基于因子图的主从式AUV协同定位方法

考虑洋流影响的基于因子图的主从式auv协同定位方法
技术领域
1.本发明属于自主水下航行器(autonomous underwater vehicle，auv)协同定位技术领域，具体涉及考虑洋流影响的基于因子图的主从式auv协同定位方法。


背景技术：

2.自主水下航行器(autonomous underwater vehicle，auv)是人类探索海洋世界的重要途径，扮演着越来越重要的角色，在海洋资源勘探、侦查监视、反潜等民用和军事领域得到了很好的应用。与单体auv相比，auv之间互相协作往往能高效的完成更复杂、更艰巨的水下任务。通常情况下，主从式auv协同作业是一种既能兼顾性能又能考虑系统成本的选择。因此，主从式auv协同系统也成为研究者的主要研究对象。auv精确的位置信息是完成水下任务的重要前提，直接决定着任务完成的效果。因此，对auv协同定位方法的研究是主从式auv协同定位系统的关键。
3.水下环境与陆地环境不同，电磁波在水下快速衰减，因此基于全球定位系统(global positioning system，gps)等无线电信号的常规定位方法不适用于水下环境，这大大增加水下主从式auv协同定位的难度。另一方面，由于水下环境的随机性和复杂性，主从式auv协同定位系统的定位性能受到外部等多种因素的制约。洋流就是影响主从式auv协同定位精度的重要因素之一。洋流影响auv的航位推算造成auv自身定位误差，同时，由于洋流的存在也会导致auv之间的测距误差。随着误差的积累，最终导致auv定位的失败。因此，研究一种考虑洋流影响的主从式auv协同定位方法是必要的。
4.在已发表的文章中，如2012年在《西北工业大学学报》第30卷第4期的房新鹏、严卫生、李俊兵发表的《基于距离量测的主从式auv协同定位系统观测性研究》和2017年在《哈尔滨工业大学学报》第49卷第9期的于飞，范世伟，李倩，郝强，姜畔发表的《主从式usv协同定位系统性能分析》文章中通过研究主从式auv协同定位系统的可观测性分析协同定位系统的定位性能。但是，这些方法忽略了auv实际工作时洋流对auv定位性能的影响。2011年在《火力与指挥控制》第36卷第11期的张永清、张福斌、张立川发表的《基于距离量测的主从式auv协同定位方法》和2014年在《兵工学报》第35卷第11期的徐博，肖永平，高伟，刘亚龙，杨建发表的《主从式无人水面艇协同定位滤波方法与实验验证》文章，这些方法均是基于扩展卡尔曼滤波(extended kalman filter，ekf)提出的方法，由于存在一阶泰勒线性化近似带来的误差，进而影响auv的定位精度。因子图(factor graph，fg)相比于ekf具有计算复杂度小、分布式估计、扩展性好的优势。因子图也被用于多auv协同定位中，如2021年公开的孙骞、张国昌、唐忠的一篇名为《一种基于旋转矩阵的因子图协同定位方法》(专利申请号：cn202011552599.1)的专利介绍了一种增强协同定位系统鲁棒性的方法。但是，该方法没有考虑未知洋流对主从式auv协同定位的影响。


技术实现要素：

5.本发明的目的在于解决主从式auv协同定位中洋流对auv定位的影响而提供了考
虑洋流影响的基于因子图的主从式auv协同定位方法。
6.考虑洋流影响的基于因子图的主从式auv协同定位方法，包括以下步骤：
7.步骤1：建立考虑洋流影响的主从式auv协同定位系统的状态方程；
8.步骤2：建立考虑洋流影响的主从式auv协同定位系统的量测方程；
9.步骤3：建立考虑洋流影响的主从式auv协同定位系统的因子图模型；
10.步骤4：根据步骤3建立的系统因子图模型，计算待定位的从auv时间更新后的东向位置、北向位置、东向洋流速度和北向洋流速度；
11.步骤5：根据步骤3建立的系统因子图模型，计算待定位的从auv量测更新后的东向位置、北向位置、东向洋流速度和北向洋流速度；
12.步骤6：通过步骤4和步骤5，计算待定位的从auv的东向位置、北向位置、东向洋流速度和北向洋流速度。
13.进一步地，所述步骤1建立的考虑洋流影响的主从式auv协同定位系统状态方程为：
14.xk＝x
k-1
v
x,k-1
t dx
k-1
cosθ
k-1-dy
k-1
sinθ
k-1
15.yk＝y
k-1
v
y,k-1
t dx
k-1
sinθ
k-1
dy
k-1
cosθ
k-1
[0016][0017][0018]
其中，x
k-1
、y
k-1
和xk、yk分别为k-1时刻和k时刻待定位的从auv在地理坐标系东向和北向的位置分量；v
x,k-1
、v
y,k-1
和v
x,k
、v
y,k
分别为k-1时刻和k时刻洋流在地理坐标系东向和北向的速度分量；dx
k-1
,dy
k-1
,θ
k-1
分别为待定位的从auv在载体坐标系右向位移增量、前向位移增量和航向角，分别由从auv搭载的多普勒计程仪和罗经系统给出；和为洋流过程噪声；t是采样周期。
[0019]
进一步地，所述步骤2建立考虑洋流影响的主从式auv协同定位系统的量测方程具体为：
[0020][0021]
其中，为k时刻主auv的东向位置和北向位置，由主auv搭载的高精度组合导航系统给出；δk是符合零均值高斯白噪声的距离量测噪声。
[0022]
进一步地，所述步骤3因子图模型包含x
k-1
、y
k-1
、xk、yk、v
x,k-1
、v
y,k-1
、v
x,k
、v
y,k
、dk变量节点和a
k-1,k
、b
k-1,k
、c
k-1,k
、d
k-1,k
、ek、函数节点，函数节点和的约束函数表示他们分别连接的x
k-1
、y
k-1
、v
x,k-1
、v
y,k-1
和dk变量节点的概率密度函数。
[0023]
进一步地，所述步骤4计算待定位的从auv时间更新后的东向位置、北向位置、东向洋流速度和北向洋流速度具体为：
[0024]
计算k时刻待定位的从auv时间更新后的东向位置均值和北向位置均值
[0025][0026]
[0027][0028][0029]
其中：b1＝cos(μ
θ,k-1
) μ
θ,k-1
sin(μ
θ,k-1
)，b2＝-sin(μ
θ,k-1
) μ
θ,k-1
cos(μ
θ,k-1
)，b3＝-a1μ
θ,k-1
。
[0030]
b4＝sin(μ
θ,k-1
)-μ
θ,k-1
cos(μ
θ,k-1
)，b5＝cos(μ
θ,k-1
) μ
θ,k-1
sin(μ
θ,k-1
)，b6＝-a2μ
θ,k-1
。
[0031]
其中：为k时刻东向位置均值和北向位置均值的方差，为k-1时刻计算的从auv的东向位置均值，为k-1时刻计算的从auv的北向位置均值，为k-1时刻的东向位置均值和北向位置均值的方差，为k-1时刻东向洋流速度均值，为北向洋流速度均值，为k-1时刻计算的东向洋流速度均值和北向洋流速度均值的方差，为auv搭载的多普勒计程仪输出的右向速度均值，为auv搭载的多普勒计程仪输出的前向速度均值，为右向速度均值和前向速度均值的方差，μ
θ,k-1
为罗经系统输出的从auv的航向角均值，罗经系统输出的从auv的航向角均值的方差。
[0032]
计算东向洋流速度均值和北向洋流速度均值
[0033][0034]
进一步地，所述步骤5建立计算待定位的从auv量测更新后的东向位置、北向位置、东向洋流速度和北向洋流速度具体为：
[0035]
(1)计算k时刻待定位的从auv量测更新后的东向位置均值和方差
[0036][0037][0038]
其中：为k时刻主auv东向位置，为k时刻主auv北向位置，t为采样周期，为k时刻待定位从auv与主auv之间的距离均值，测距精度；
[0039]k22
＝0，k
44
＝(δy)2/(δx)3，，
[0040]
其中δy、δx、k1、k2、k3、k4、k
12
、k
13
、k
14
、k
23
、k
24
、k
34
具体为：k1＝-δy/δx,k2＝-t，k3＝-t(δy)/δx，k12
＝0，k
23
＝0，k
24
＝0，
[0041]
(2)计算k时刻待定位的从auv量测更新后的北向位置均值和方差
[0042][0043][0044]
其中，参数s
11
、s
22
、s
33
、s
44
、r1、r2、r3、r4具体为：
[0045]s33
＝0，s
44
＝(δx1)2/(δy1)3，，
[0046]
其中δx1、δy1、s1、s2、s3、s4、s
12
、s
13
、s
14
、s
23
、s
24
、s
34
具体为：
[0047]
s1＝δx1/δy1，s2＝t(δx1)/δy1，s3＝-t，s
13
＝0，s
23
＝0，s
34
＝0
[0048]
计算
[0049][0050][0051]
其中，n
11
、n
22
、n
33
、n
44
、l1、l2、l3、l4具体为：
[0052]n11
＝0，n
44
＝(δy)2/(t(δx)3)，)，
[0053]
其中，n1、n2、n3、n4、n
12
、n
13
、n
14
、n
23
、n
24
、n
34
具体为：
[0054]
n1＝-1/t,n2＝-δy/(t(δx))，n3＝-δy/δx，n
12
＝0，n
13
＝0，n
14
＝0，
[0055]
计算
[0056][0056][0057]
其中h
11
、h
22
、h
33
、h
44
、q1、q2、q3、q4具体为：
[0058]h22
＝0，h
44
＝(δx1)2/(t(δy1)3)，)，
[0059]
其中，h1、h2、h3、h4、h
12
、h
13
、h
14
、h
23
、h
24
、h
34
具体为：
[0060]
h1＝-δx1/(t(δy1)),h2＝-1/t，h3＝-δx1/δy1，h
12
＝0，h
23
＝0，h
24
＝0，进一步地，所述步骤6具体为：
[0061]
k时刻东向位置均值北向位置均值及其方差为：
[0062][0063][0064]
其中，分别为步骤4计算出k时刻的时间更新后的东向位置均值、北向位置均值和它们的方差，分别为步骤5计算出的k时刻主从auv量测更新后的东向位置均值、北向位置均值和它们的方差。
[0065]
k时刻东向洋流速度均值北向洋流速度均值及它们的方差为：
[0066][0067][0068]
其中，分别为步骤4计算出的k时刻时间更新后的东向洋流速度均值、北向洋流速度均值和它们的方差，分别为步骤5计
算出的k时刻主从auv量测更新后的东向洋流速度均值、北向洋流速度均值和它们的方差。
[0069]
本发明的有益效果在于：
[0070]
本发明考虑了海水洋流对auv定位精度的影响，将东向洋流速度、北向洋流速度引入系统因子图中，参与从auv的定位计算，提高待定位从auv的定位精度。为了验证本发明的有益效果，将本发明的考虑洋流影响的基于因子图的主从式auv协同定位方法与忽略洋流影响的基于因子图的主从式auv协同定位方法，和考虑洋流影响的基于ekf的主从式auv协同定位方法进行仿真对比。考虑洋流影响的基于因子图的主从式auv协同定位方法与忽略洋流对auv定位影响的方法相比，可以有效地提高auv定位的精度。另外，与考虑洋流影响的基于ekf的主从式auv协同定位方法相比，该方法由于保留了泰勒展开的二阶项而获得了更高的定位精度。
附图说明
[0071]
图1为本发明考虑洋流影响的主从式auv协同定位系统的因子图模型图。
[0072]
图2为本发明忽略洋流影响的基于fg的主从式auv协同定位方法、考虑洋流影响的基于ekf的主从式auv协同定位方法、考虑洋流影响的基于fg的主从式auv协同定位方法的平均定位精度比较图。
具体实施方式
[0073]
下面结合附图对本发明做进一步描述。
[0074]
一种考虑洋流影响的基于因子图的多auv协同定位方法，包括如下步骤：
[0075]
步骤1：建立考虑洋流影响的主从式auv协同定位系统的状态方程，如下所示：
[0076]
xk＝x
k-1
v
x,k-1
t dx
k-1
cosθ
k-1-dy
k-1
sinθ
k-1
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1)
[0077]
yk＝y
k-1
v
y,k-1
t dx
k-1
sinθ
k-1
dy
k-1
cosθ
k-1
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(2)
[0078][0079][0080]
其中，x
k-1
、y
k-1
和xk、yk分别为k-1时刻和k时刻待定位的从auv在地理坐标系东向和北向的位置分量，是待计算的量。v
x,k-1
、v
y,k-1
和v
x,k
、v
y,k
分别为k-1时刻和k时刻洋流在地理坐标系东向和北向的速度分量，是待计算的量。dx
k-1
,dy
k-1
,θ
k-1
分别为待定位的从auv在载体坐标系右向位移增量、前向位移增量和航向角，可分别由从auv搭载的多普勒计程仪和罗经系统给出。和为洋流过程噪声，是具有零均值的高斯白噪声序列。t是采样周期。
[0081]
步骤2：建立考虑洋流影响的主从式auv协同定位系统的量测方程。本发明将主从auv之间的距离信息作为量测信息，则系统量测方程为：
[0082][0083]
其中，为k时刻主auv的东向位置和北向位置，可由主auv搭载的高精度组合导航系统给出。δk是符合零均值高斯白噪声的距离量测噪声。本发明涉及的主从式auv协同定位系统是包含1个从auv和1个主auv的系统。
[0084]
步骤3：建立考虑洋流影响的主从式auv协同定位系统的因子图模型。该因子图模型包含x
k-1
、y
k-1
、xk、yk、v
x,k-1
、v
y,k-1
、v
x,k
、v
y,k
、dk变量节点和a
k-1,k
、b
k-1,k
、c
k-1,k
、d
k-1,k
、ek、函数节点，如附图1所示。函数节点a
k-1,k
、b
k-1,k
、c
k-1,k
、d
k-1,k
、ek的约束函数分别如式(1)-(5)所示，函数节点和的约束函数表示他们分别连接的x
k-1
、y
k-1
、v
x,k-1
、v
y,k-1
和dk变量节点的概率密度函数。
[0085]
步骤4：结合步骤3建立的系统因子图模型，计算待定位的从auv时间更新后的东向位置、北向位置、东向洋流速度和北向洋流速度。
[0086]
k时刻待定位的从auv时间更新后的东向位置均值北向位置均值及它们的方差可根据k-1时刻计算的从auv的东向位置均值北向位置均值及它们的方差k-1时刻计算的东向洋流速度均值北向洋流速度均值及它们的方差从auv搭载的多普勒计程仪输出的右向速度均值前向速度均值及它们的方差和罗经系统输出的从auv的航向角均值μ
θ,k-1
及其方差计算。由于系统状态方程是非线性方程，为了保持变量的高斯特性，非线性的状态方程需泰勒展开。为了保证更高的精度要求，本发明均采用二阶泰勒展开。具体地，计算的公式如下所示：
[0087][0088][0089]
其中，参数a1、b1、b2、b3可根据以下公式计算：
[0090][0091]
b2＝-sin(μ
θ,k-1
) μ
θ,k-1
cos(μ
θ,k-1
)，b3＝-a1μ
θ,k-1
。
[0092]
具体地，计算的公式如下所示：
[0093][0093][0094]
其中，参数a2、b4、b5、b6可根据以下公式计算：
[0095]
b4＝sin(μ
θ,k-1
)-μ
θ,k-1
cos(μ
θ,k-1
)，b5＝cos(μ
θ,k-1
) μ
θ,k-1
sin(μ
θ,k-1
)，b6＝-a2μ
θ,k-1
。
[0096]
另外，根据k-1时刻的东向洋流速度均值及其方差和过程噪声的方差可计算时间更新后的东向洋流速度均值及其方差根据k-1时刻的北向洋流速度均值及其方差和过程噪声的方差可计算时间更新后的北向洋流速度均值及其方差及其方差和的计算公式如下所示：
[0097][0098]
步骤5：结合步骤3建立的系统因子图模型，计算待定位的从auv量测更新后的东向位置、北向位置、东向洋流速度和北向洋流速度。
[0099]
k时刻待定位的从auv量测更新后的东向位置均值北向位置均值及它们的方差可根据步骤4计算出的及他们的方差及他们的方差k时刻主auv东向位置北向位置采样周期t，和k时刻待定位从auv与主auv之间的距离均值及测距精度计算。由于系统量测方程是非线性方程，为了保持变量的高斯特性，非线性的量测方程需泰勒展开。为了保证更高的精度要求，本发明均采用二阶泰勒展开。具体地，计算的公式如下所示：
[0100][0101][0102]
其中，参数k
11
、k
22
、k
33
、k
44
、m1、m2、m3、m4可根据以下公式计算：
[0103]k22
＝0，k
44
＝(δy)2/(δx)3，，
[0104]
其中，参数δy、δx、k1、k2、k3、k4、k
12
、k
13
、k
14
、k
23
、k
24
、k
34
可根据以下公式计算：k1＝-δy/δx,k2＝-t，k3＝-t(δy)/δx，k
12
＝0，k
23
＝0，k
24
＝0，
[0105]
具体地，计算的公式如下所示：
[0106][0107][0108]
其中，参数s
11
、s
22
、s
33
、s
44
、r1、r2、r3、r4可根据以下公式计算：
[0109]s33
＝0，s
44
＝(δx1)2/(δy1)3，
[0110]
其中，参数δx1、δy1、s1、s2、s3、s4、s
12
、s
13
、s
14
、s
23
、s
24
、s
34
可根据以下公式计算：s1＝δx1/δy1，s2＝t(δx1)/δy1，s3＝-t，s
13
＝0，s
23
＝0，s
34
＝0。
[0111]
另外，根据步骤4计算出的及他们的方差k时刻主auv东向位置北向位置采样周期t，和k时刻待定位从auv与主auv之间的距离均值及测距精度可计算k时刻东向洋流速度均值北向洋流速度均值及其方差具体地，计算的公式如下所示：
[0112][0113][0114]
其中，参数n
11
、n
22
、n
33
、n
44
、l1、l2、l3、l4可根据以下公式计算：
[0115]n11
＝0，n
44
＝(δy)2/(t(δx)3)，)，
[0116]
其中，参数n1、n2、n3、n4、n
12
、n
13
、n
14
、n
23
、n
24
、n
34
可根据以下公式计算：
[0117]
n1＝-1/t,n2＝-δy/(t(δx))，n3＝-δy/δx，n
12
＝0，n
13
＝0，n
14
＝0，
[0118]
具体地，计算的公式如下所示：
[0119][0120][0121]
其中，参数h
11
、h
22
、h
33
、h
44
、q1、q2、q3、q4可根据以下公式计算：
[0122]h22
＝0，h
44
＝(δx1)2/(t(δy1)3)，)，
[0123]
其中，参数h1、h2、h3、h4、h
12
、h
13
、h
14
、h
23
、h
24
、h
34
可根据以下公式计算：
[0124]
h1＝-δx1/(t(δy1)),h2＝-1/t，h3＝-δx1/δy1，h
12
＝0，h
23
＝0，h
24
＝0，
[0125]
步骤6：通过步骤4和步骤5，计算待定位的从auv的东向位置、北向位置、东向洋流速度和北向洋流速度。
[0126]
待定位的从auv的东向位置、北向位置、东向洋流速度和北向洋流速度的计算公式如下所示。k时刻东向位置均值北向位置均值及其方差为：
[0127][0128][0129]
其中，分别为步骤4计算出k时刻的时间更新后的东向位置均值、北向位置均值和它们的方差，分别为步骤5计算出的k时刻主从auv量测更新后的东向位置均值、北向位置均值和它们的方差。
[0130]
k时刻东向洋流速度均值北向洋流速度均值及它们的方差为：
[0131][0132][0133]
其中，分别为步骤4计算出的k时刻时间更新后的东向洋流速度均值、北向洋流速度均值和它们的方差，分别为步骤5计算出的k时刻主从auv量测更新后的东向洋流速度均值、北向洋流速度均值和它们的方差。
[0134]
以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。