CN103968838A

CN103968838A - Co-location method of AUVs (Autonomous Underwater Vehicles) in curvilinear motion state based on polar coordinate system

Info

Publication number: CN103968838A
Application number: CN201410195910.XA
Authority: CN
Inventors: 高伟; 史宏洋; 刘博�; 杨建�; 于春阳; 张丽丽; 张亚; 梁宏; 兰海钰; 夏建忠
Original assignee: Harbin Engineering University
Current assignee: Harbin Engineering University
Priority date: 2014-05-09
Filing date: 2014-05-09
Publication date: 2014-08-06
Anticipated expiration: 2034-05-09
Also published as: CN103968838B

Abstract

The invention provides a coordinated positioning method based on a polar coordinate system in the AUV curve motion state. Among them, the main AUV is equipped with a high-precision inertial measurement unit, while the sub-AUV is equipped with a low-precision IMU. First, the sub-AUV measures the motion information of the carrier by fusing its own inertial measurement unit, and determines the distance and orientation information relative to the main AUV. Finally, the extended Kalman filter is used to achieve co-location. Compared with the traditional algorithm based on the Cartesian coordinate system, the present invention has higher positioning accuracy when the AUV moves in a curve.

Description

A Co-location Method Based on Polar Coordinate System for AUV in Curved Motion State

技术领域technical field

本发明涉及的是一种在曲线运动状态下基于极坐标系的AUV曲线运动状态下的协同定位方法。The invention relates to a collaborative positioning method in a curved motion state of an AUV based on a polar coordinate system in a curved motion state.

背景技术Background technique

未来是海洋文明的世纪，海洋领域的探索将成为世界的发展趋势，因此水面船艇作业、应用的研究具有广阔的前景。自主水下载体(Autonomous Underwater Vehicle,AUV)实际上是一种水下无人平台，以其体积小、机动性强和成本低等优势成为研究热点；可以执行各种导航和水下地理勘察等复杂任务[1]。随着人类进一步开发认识海洋，AUV研究、应用的理论价值和实际意义更为凸显。AUV与常规的船艇比较，易于操作、灵活快捷，可以经受住各种复杂环境的考验。AUV在允许侦测区域进行隐蔽及公开作业，要充分利用传感器信息，这就需要多AUV进行协同定位。目前，大多数的AUV协同定位系统都是基于笛卡尔坐标系建立的，且其运动状态大都为匀速直航，而极坐标系是针对曲线运动的，所以提出了一种基于极坐标系的协同定位算法，另外由于在AUV作曲线运动时，单纯的距离信息不能够很好的描述AUV的运动状态，因此提出了一种以距离加方位信息作为观测量的协同导航定位算法并进行了可观性分析。The future is the century of marine civilization, and the exploration of the marine field will become the development trend of the world. Therefore, the research on the operation and application of surface ships has broad prospects. Autonomous Underwater Vehicle (AUV) is actually an underwater unmanned platform, which has become a research hotspot due to its advantages of small size, strong mobility and low cost; it can perform various navigations and underwater geographical surveys, etc. Complex tasks [1]. As humans further develop and understand the ocean, the theoretical value and practical significance of AUV research and application are more prominent. Compared with conventional boats, AUV is easy to operate, flexible and fast, and can withstand the test of various complex environments. AUVs operate covertly and openly in areas that allow detection. To make full use of sensor information, this requires multiple AUVs to coordinate positioning. At present, most of the AUV collaborative positioning systems are established based on the Cartesian coordinate system, and most of their motion states are straight and constant speed, while the polar coordinate system is for curved motion, so a collaborative positioning system based on the polar coordinate system is proposed. In addition, when the AUV is moving in a curve, the pure distance information cannot describe the motion state of the AUV well, so a collaborative navigation positioning algorithm using distance plus azimuth information as the observation is proposed and the observability is carried out. analyze.

发明内容Contents of the invention

本发明的目的在于提供一种提高精度的基于极坐标系的AUV曲线运动状态下的新协同定位方法。The purpose of the present invention is to provide a new collaborative positioning method based on the polar coordinate system under the curved motion state of the AUV with improved accuracy.

本发明的目的是这样实现的：The purpose of the present invention is achieved like this:

步骤一：主、子AUV开始协同导航，实时采集子AUV惯性测量单元测得的加速度和角速度信息，并积分得到子AUV的速度和航向信息；Step 1: The main and sub-AUVs start cooperative navigation, collect the acceleration and angular velocity information measured by the sub-AUV inertial measurement unit in real time, and integrate to obtain the speed and heading information of the sub-AUV;

步骤二：采集子AUV相对于主AUV的距离和方位信息；Step 2: Collect the distance and orientation information of the sub-AUV relative to the main AUV;

采集的子AUV与主AUV之间的相对距离为：The relative distance between the collected sub-AUV and the main AUV is:

$R R = = \sqrt{{((r r))}^{22} + + {(({r r}_{11}))}^{22} - - 22 {rr rr}_{11} cos cos ((θ θ - - {θ θ}_{11}))}$

其中：R为主AUV和子AUV之间的相对距离；r₁和θ₁为主AUV在极坐标系下的半径和极角；r和θ为子AUV在极坐标系下的半径和极角；Wherein: R is the relative distance between the main AUV and the sub-AUV; _r1 and _θ1 are the radius and polar angle of the main AUV in the polar coordinate system; r and θ are the radius and polar angle of the sub-AUV in the polar coordinate system;

采集的子AUV与主AUV之间的相对方位角为：The relative azimuth between the collected sub-AUV and the main AUV is:

当rcosθ-r₁cosθ₁≤0时，有：When rcosθ-r ₁ cosθ ₁ ≤0, there are:

其中：φ为子AUV和主AUV之间的相对方位角；为子AUV的航向角；Where: φ is the relative azimuth between the sub-AUV and the main AUV; is the heading angle of the sub-AUV;

当rcosθ-r₁cosθ₁＞0时，有：When rcosθ-r ₁ cosθ ₁ >0, there are:

步骤三：结合步骤一中得到的子AUV的速度和航向信息，以及步骤二中得到的子AUV相对于主AUV的距离和方位信息，利用扩展卡尔曼滤波器，对子AUV的位置进行估计，实现子AUV的导航定位；Step 3: Combining the speed and heading information of the sub-AUV obtained in step 1, and the distance and orientation information of the sub-AUV relative to the main AUV obtained in step 2, use the extended Kalman filter to estimate the position of the sub-AUV, Realize the navigation and positioning of the sub-AUV;

所涉及的系统状态向量为：The system state vectors involved are:

其中：为主AUV的航向角；in: heading angle of the main AUV;

所涉及的系统离散状态方程为：The discrete state equation of the system involved is:

${\overset{^^}{S S}}_{k k}^{- -} = = {Φ Φ}_{k k,, k k - - 11} {\overset{^^}{S S}}_{k k - - 11} + + {T T}_{k k - - 11} {U u}_{k k - - 11} + + {G G}_{k k - - 11} {W W}_{k-1 k-1}$

其中：和分别为k，k-1时刻的状态预测值和状态估计值；U_k-1为k-1时刻的系统输入；T_k-1为k-1时刻对应的输入矩阵；Φ_k,k-1为状态转移矩阵；G_k-1为k-1时刻系统的噪声驱动矩阵；W(t)为噪声矩阵：in: and are the state prediction value and state estimation value at time k and k-1, respectively; U _k-1 is the system input at time k-1; T _k-1 is the input matrix corresponding to time k-1; Φ _{k, k-1} is the state transition matrix; G _k-1 is the noise driving matrix of the system at time k-1; W(t) is the noise matrix:

系统的输入向量：The input vector of the system:

U_k-1＝[v_k-1 ω_k-1]^T U _k-1 ＝[v _k-1 ω _k-1 ] ^T

对应的输入矩阵：The corresponding input matrix:

${T T}_{k k - - 11} = = [\begin{matrix} 00 & 00 \\ 00 & 00 \\ 00 & 11 \end{matrix}]$

系统的噪声矩阵：The noise matrix of the system:

其中：分别对应于k-1时刻主AUV在极坐标下的半径，极角和航向误差，分别对应于k-1时刻子AUV在极坐标下的半径，极角和航向误差；in: Corresponding to the radius, polar angle and heading error of the main AUV in polar coordinates at time k-1, Corresponding to the radius, polar angle and heading error of the sub-AUV in polar coordinates at time k-1, respectively;

系统的状态转移矩阵为：The state transition matrix of the system is:

其中：T为采样周期；Where: T is the sampling period;

所涉及的系统量测方程为：The system measurement equations involved are:

${Z Z}_{k k} = = {H h}_{k k} {\overset{^^}{S S}}_{k k}^{- -} + + {V V}_{k k},,$

其中：Z_k为k时刻系统的量测向量；H_k为k时刻系统的量测矩阵；V_k为量测噪声；Among them: Z _k is the measurement vector of the system at time k; H _k is the measurement matrix of the system at time k; V _k is the measurement noise;

系统的量测向量为：The measurement vector of the system is:

Z_k＝[R_k φ_k]^T，Z _k = [R _k φ _k ] ^T ,

当rcosθ-r₁cosθ₁≤0时，量测矩阵为：When rcosθ-r ₁ cosθ ₁ ≤0, the measurement matrix is:

${H h}_{k k} = = [\begin{matrix} \frac{{r r}_{k k} (({r r}_{k k} - - {r r}_{11,, k k})) cos cos (({θ θ}_{k k} - - {θ θ}_{11,, k k}))}{{R R}_{k k}} & \frac{{r r}_{k k} {r r}_{11,, k k} sin sin (({θ θ}_{k k} - - {θ θ}_{11,, k k}))}{{R R}_{k k}} & 00 \\ \frac{- - {r r}_{11,, k k} cos cos (({θ θ}_{k k} - - {θ θ}_{11,, k k}))}{{R R}_{k k}^{22}} & \frac{{r r}_{k k} (({r r}_{k k} - - {r r}_{11,, k k})) cos cos (({θ θ}_{k k} - - {θ θ}_{11,, k k}))}{{R R}_{k k}^{22}} & 11 \end{matrix}];;$

当rcosθ-r₁cosθ₁＞0时，量测矩阵为：When rcosθ-r ₁ cosθ ₁ >0, the measurement matrix is:

${H h}_{k k} = = [\begin{matrix} \frac{{r r}_{k k} (({r r}_{k k} - - {r r}_{11,, k k})) cos cos (({θ θ}_{k k} - - {θ θ}_{11,, k k}))}{{R R}_{k k}} & \frac{{r r}_{k k} {r r}_{11,, k k} sin sin (({θ θ}_{k k} - - {θ θ}_{11,, k k}))}{{R R}_{k k}} & 00 \\ \frac{- - {r r}_{11,, k k} cos cos (({θ θ}_{k k} - - {θ θ}_{11,, k k}))}{{R R}_{k k}^{22}} & - - \frac{{r r}_{k k} (({r r}_{k k} - - {r r}_{11,, k k})) cos cos (({θ θ}_{k k} - - {θ θ}_{11,, k k}))}{{R R}_{k k}^{22}} & - - 11 \end{matrix}] . .$

本发明的有益效果在于：The beneficial effects of the present invention are:

(1)与传统的基于笛卡尔坐标系下的系统动力学模型相比，极坐标系下的系统动力学模型更适用于AUV作曲线运动的状态下。(1) Compared with the traditional system dynamics model based on the Cartesian coordinate system, the system dynamics model in the polar coordinate system is more suitable for the AUV in the state of curved motion.

(2)与传统的仅用相对距离信息作为观测量的算法相比，采用距离加方位信息作为观测量时能够更好的反映曲线运动下AUV的运动状态，从而使得定位结果更精确。(2) Compared with the traditional algorithm that only uses relative distance information as the observation quantity, using the distance plus azimuth information as the observation quantity can better reflect the motion state of the AUV under the curve motion, thus making the positioning result more accurate.

(3)采用李导数对系统的可观测性进行了分析，为协同定位提供了先决条件，进而有助于定位精度的进一步提高。(3) The observability of the system is analyzed by using the Lie derivative, which provides a prerequisite for co-location, which in turn helps to further improve the positioning accuracy.

附图说明Description of drawings

图1是本发明的方法流程图；Fig. 1 is method flowchart of the present invention;

图2是AUV的估计模型；Figure 2 is the estimation model of AUV;

图3是主AUV和子AUV的真实运动轨迹；Figure 3 is the real trajectory of the main AUV and sub-AUV;

图4是基于传统算法时的子AUV定位误差曲线；Fig. 4 is the sub-AUV positioning error curve when based on the traditional algorithm;

图5是基于传统算法时的子AUV的位置估计曲线；Fig. 5 is the position estimation curve of the sub-AUV when based on the traditional algorithm;

图6是基于传统算法时的子AUV的位置估计曲线的末端放大图；FIG. 6 is an enlarged view of the end of the position estimation curve of the sub-AUV based on the traditional algorithm;

图7是基于新算法时的子AUV定位误差曲线；Fig. 7 is the sub-AUV positioning error curve based on the new algorithm;

图8是基于新算法时的子AUV的位置估计曲线；Fig. 8 is the position estimation curve of the sub-AUV based on the new algorithm;

图9是基于新算法时的子AUV的位置估计曲线的末端放大图。FIG. 9 is an enlarged view of the end of the position estimation curve of the sub-AUV based on the new algorithm.

具体实施方式Detailed ways

下面将结合附图对本发明作进一步的详细说明。The present invention will be further described in detail below in conjunction with the accompanying drawings.

基于极坐标系的AUV曲线运动状态下的新协同定位算法，包括以下几个步骤：The new co-location algorithm based on the polar coordinate system under the AUV curve motion state includes the following steps:

步骤一：采集惯性测量单元(Inertial Measurement Unit,IMU)测量的加速度、和角速度信息。Step 1: Collect acceleration and angular velocity information measured by an inertial measurement unit (Inertial Measurement Unit, IMU).

步骤二：利用IMU测得的加速度信息和角速度信息积分得到载体AUV的速度和航向信息，并利用该信息建立传统的笛卡尔坐标系下AUV的系统方程。Step 2: Use the acceleration information and angular velocity information measured by the IMU to integrate the velocity and heading information of the carrier AUV, and use this information to establish the system equation of the AUV in the traditional Cartesian coordinate system.

步骤三：利用极坐标系和步骤二的笛卡尔坐标系之间的转换关系将步骤二中AUV的系统方程转换到极坐标下。Step 3: Using the conversion relationship between the polar coordinate system and the Cartesian coordinate system in step 2, the system equation of the AUV in step 2 is transformed into polar coordinates.

步骤四：采集子AUV相对于主AUV的距离和方位信息。Step 4: Collect the distance and orientation information of the sub-AUV relative to the main AUV.

步骤五：基于步骤三和步骤四建立扩展卡尔曼滤波(Extend Kalman Filter,EKF)的状态方程和量测方程，对子AUV的位置进行估计，以实现定位。Step 5: Establish the state equation and measurement equation of the Extended Kalman Filter (EKF) based on Step 3 and Step 4, and estimate the position of the sub-AUV to achieve positioning.

利用IMU测得的加速度信息和角速度信息积分得到载体AUV的速度和航向信息，并利用该信息建立传统的笛卡尔坐标系下AUV的系统方程为：The acceleration information and angular velocity information measured by the IMU are used to integrate the velocity and heading information of the carrier AUV, and this information is used to establish the system equation of the AUV in the traditional Cartesian coordinate system as follows:

其中：v_i和ω_i分别为第i个水下自主载体(Autonomous Underwater Vehicle,AUV)的速度和加速度，为第i个AUV航向角，为第i个AUV航向角的一阶导数。由于AUV作曲线运动，则ω_i≠0。Where: v _i and ω _i are the velocity and acceleration of the i-th autonomous underwater vehicle (AUV), respectively, is the heading angle of the ith AUV, is the first derivative of the heading angle of the ith AUV. Since the AUV moves in a curve, ω _i ≠0.

利用极坐标系和步骤二的笛卡尔坐标系之间的转换关系将步骤二中AUV的系统方程转换到极坐标下为：Using the conversion relationship between the polar coordinate system and the Cartesian coordinate system in step 2, the system equation of the AUV in step 2 is converted to polar coordinates as:

其中：r_i和θ_i分别为第i个AUV在极坐标系下的半径和极角。Among them: r _i and θ _i are the radius and polar angle of the ith AUV in the polar coordinate system, respectively.

采集的子AUV相对于主AUV的距离和方位信息为：The distance and orientation information of the collected sub-AUV relative to the main AUV is:

(1)以两个AUV的协同导航定位系统为例，采集的子AUV与主AUV之间的相对距离为：(1) Taking the cooperative navigation and positioning system of two AUVs as an example, the relative distance between the collected sub-AUV and the main AUV is:

其中：R为主AUV和子AUV之间的相对距离；r₁和θ₁为主AUV在极坐标系下的半径和极角；r和θ为子AUV在极坐标系下的半径和极角。Among them: R is the relative distance between the main AUV and the sub-AUV; r ₁ and θ ₁ are the radius and polar angle of the main AUV in the polar coordinate system; r and θ are the radius and polar angle of the sub-AUV in the polar coordinate system.

(2)采集的子AUV与主AUV之间的相对方位角为：(2) The relative azimuth between the collected sub-AUV and the main AUV is:

其中：φ为子AUV和主AUV之间的相对方位角；为子AUV的航向角。Where: φ is the relative azimuth between the sub-AUV and the main AUV; is the heading angle of the sub-AUV.

基于步骤三和步骤四建立EKF的状态方程和量测方程包括：Establishing the state equation and measurement equation of EKF based on steps 3 and 4 includes:

(1)EKF算法的状态方程：(1) The state equation of the EKF algorithm:

系统的状态变量为：The state variables of the system are:

其中：为主AUV的航向角。in: Heading angle of the main AUV.

系统的离散状态方程为：The discrete state equation of the system is:

其中：和分别为k，k-1时刻的状态预测值和状态估计值；U_k-1为k-1时刻的系统输入；T_k-1为k-1时刻对应的输入矩阵；Φ_k,k-1为状态转移矩阵；G_k-1为k-1时刻系统的噪声驱动矩阵；W(t)为噪声矩阵。in: and are the state prediction value and state estimation value at time k and k-1, respectively; U _k-1 is the system input at time k-1; T _k-1 is the input matrix corresponding to time k-1; Φ _{k, k-1} is the state transition matrix; G _k-1 is the noise driving matrix of the system at time k-1; W(t) is the noise matrix.

系统的输入向量：The input vector of the system:

U_k-1＝[v_k-1 ω_k-1]^T U _k-1 ＝[v _k-1 ω _k-1 ] ^T

对应的输入矩阵：The corresponding input matrix:

系统的噪声矩阵：The noise matrix of the system:

其中：分别对应于k-1时刻主AUV在极坐标下的半径，极角和航向误差，分别对应于k-1时刻子AUV在极坐标下的半径，极角和航向误差。in: Corresponding to the radius, polar angle and heading error of the main AUV in polar coordinates at time k-1, Corresponding to the radius, polar angle and heading error of sub-AUV in polar coordinates at time k-1, respectively.

系统的状态转移矩阵为：The state transition matrix of the system is:

其中：T为采样周期。Where: T is the sampling period.

(2)EKF算法的离散量测方程：(2) Discrete measurement equation of EKF algorithm:

${Z Z}_{k k} = = {H h}_{k k} {\overset{^^}{S S}}_{k k}^{- -} + + {V V}_{k k},,$

其中：Z_k为k时刻系统的量测向量；H_k为k时刻系统的量测矩阵；V_k为量测噪声。Among them: Z _k is the measurement vector of the system at time k; H _k is the measurement matrix of the system at time k; V _k is the measurement noise.

系统的量测向量为：The measurement vector of the system is:

Z_k＝[R_k φ_k]^T Z _k ＝[R _k φ _k ] ^T

${H h}_{k k} = = [\begin{matrix} \frac{{r r}_{k k} (({r r}_{k k} - - {r r}_{11,, k k})) cos cos (({θ θ}_{k k} - - {θ θ}_{11,, k k}))}{{R R}_{k k}} & \frac{{r r}_{k k} {r r}_{11,, k k} sin sin (({θ θ}_{k k} - - {θ θ}_{11,, k k}))}{{R R}_{k k}} & 00 \\ \frac{- - {r r}_{11,, k k} cos cos (({θ θ}_{k k} - - {θ θ}_{11,, k k}))}{{R R}_{k k}^{22}} & \frac{{r r}_{k k} (({r r}_{k k} - - {r r}_{11,, k k})) cos cos (({θ θ}_{k k} - - {θ θ}_{11,, k k}))}{{R R}_{k k}^{22}} & 11 \end{matrix}]$

${H h}_{k k} = = [\begin{matrix} \frac{{r r}_{k k} (({r r}_{k k} - - {r r}_{11,, k k})) cos cos (({θ θ}_{k k} - - {θ θ}_{11,, k k}))}{{R R}_{k k}} & \frac{{r r}_{k k} {r r}_{11,, k k} sin sin (({θ θ}_{k k} - - {θ θ}_{11,, k k}))}{{R R}_{k k}} & 00 \\ \frac{{r r}_{11,, k k} cos cos (({θ θ}_{k k} - - {θ θ}_{11,, k k}))}{{R R}_{k k}^{22}} & - - \frac{{r r}_{k k} (({r r}_{k k} - - {r r}_{11,, k k})) cos cos (({θ θ}_{k k} - - {θ θ}_{11,, k k}))}{{R R}_{k k}^{22}} & - - 11 \end{matrix}]$

通过EKF算法对子AUV的位置进行估计，以实现定位。The position of the sub-AUV is estimated by the EKF algorithm to achieve positioning.

本发明提供的是一种基于极坐标系的自主水下载体(Autonomous Underwater Vehicle,AUV)曲线运动状态下的新的协同定位算法。其中主AUV装备精度较高的惯性测量单元(Inertial Measurement Unit,IMU)，而子AUV装备较低精度的IMU。首先子AUV通过融合自身的IMU测量信息以及相对于主AUV的距离和方位信息并利用扩展卡尔曼滤波(ExtendKalman Filter,EKF)来实现协同定位。然后对从可观测性分析的角度进一步说明了该协同定位系统性能的改善。最后从仿真角度验证了该协同定位算法的有效性并获得了较高的定位性能。本发明与传统的基于笛卡尔坐标系的算法相比，具有较高的定位精度，同时比航位推算算法和只融合距离测量信息的算法的精度较。The invention provides a new cooperative positioning algorithm based on a polar coordinate system under the curved motion state of an autonomous underwater vehicle (AUV) (Autonomous Underwater Vehicle, AUV). Among them, the main AUV is equipped with a high-precision inertial measurement unit (Inertial Measurement Unit, IMU), while the sub-AUV is equipped with a low-precision IMU. First, the sub-AUV realizes co-location by fusing its own IMU measurement information and the distance and orientation information relative to the main AUV and using the Extended Kalman Filter (EKF). Then the performance improvement of the co-location system is further illustrated from the perspective of observability analysis. Finally, the effectiveness of the co-location algorithm is verified from the simulation point of view and a high positioning performance is obtained. Compared with the traditional algorithm based on the Cartesian coordinate system, the present invention has higher positioning accuracy, and at the same time, the accuracy is lower than that of the dead reckoning algorithm and the algorithm that only fuses the distance measurement information.

本发明的目的是提供一种针对AUV的曲线运动状态提供一种基于极坐标系的协同定位算法。首先在普通模型的基础上建立极坐标下的系统动力学模型；然后建立主AUV和子AUV之间的相对距离模型和相对方位角模型；在上述基础上利用卡尔曼滤波对系统子AUV进行定位；最后利用李导数对该非线性系统进行了可观测性分析，分析了本发明的优势。The purpose of the present invention is to provide a collaborative positioning algorithm based on a polar coordinate system for the curved motion state of the AUV. First, the system dynamics model in polar coordinates is established on the basis of the common model; then the relative distance model and relative azimuth model between the main AUV and the sub-AUV are established; on the basis of the above, the system sub-AUV is positioned using Kalman filtering; Finally, the observability analysis of the nonlinear system is carried out by using the Lie derivative, and the advantages of the present invention are analyzed.

本发明包括以下几个步骤：The present invention comprises the following steps:

步骤一：采集惯性测量单元(Inertial Measurement Unit,IMU)测量的加速度和速度信息；Step 1: collecting acceleration and velocity information measured by an inertial measurement unit (Inertial Measurement Unit, IMU);

步骤二、建立传统的笛卡尔坐标系下AUV的系统动力学方程；Step 2, establishing the system dynamics equation of the AUV under the traditional Cartesian coordinate system;

步骤三、利用极坐标系和传统的笛卡尔坐标系之间的转换关系建立极坐标系下AUV的系统动力学方程；Step 3, using the conversion relationship between the polar coordinate system and the traditional Cartesian coordinate system to establish the system dynamics equation of the AUV under the polar coordinate system;

步骤四、采集需要与子AUV自身测量信息进行融合的相对与主AUV距离和方位信息；Step 4, collecting relative and main AUV distance and orientation information that needs to be fused with the sub-AUV's own measurement information;

步骤五、选取状态变量建立扩展卡尔曼滤波(Extend Kalman Filter,EKF)的状态方程和量测方程，对子AUV的位置进行估计，以实现定位；Step 5. Select the state variable to establish the state equation and measurement equation of the Extended Kalman Filter (EKF), and estimate the position of the sub-AUV to achieve positioning;

步骤六、根据极坐标系下建立的系统状态方程和量测方程，利用李导数对该非线性系统进行可观测性分析。Step 6. According to the system state equation and measurement equation established in the polar coordinate system, the observability analysis of the nonlinear system is carried out by using the Lie derivative.

本发明的一种基于极坐标系的AUV曲线运动状态下的新协同定位算法，流程图如图1所示，包括以下几个步骤：A new collaborative positioning algorithm based on the polar coordinate system of the present invention under the AUV curve motion state, the flow chart is shown in Figure 1, including the following steps:

步骤一：采集惯性测量单元(Inertial Measurement Unit,IMU)测量的加速度、速度和角速度信息。Step 1: Collect acceleration, velocity and angular velocity information measured by an inertial measurement unit (Inertial Measurement Unit, IMU).

步骤二：建立传统的笛卡尔坐标系下第i个AUV的系统动力学方程为：Step 2: Establish the system dynamics equation of the i-th AUV in the traditional Cartesian coordinate system as:

步骤三：利用极坐标系和传统的笛卡尔坐标系之间的转换关系建立极坐标系下第i个AUV的系统动力学方程为：Step 3: Use the conversion relationship between the polar coordinate system and the traditional Cartesian coordinate system to establish the system dynamics equation of the i-th AUV in the polar coordinate system as:

步骤四：建立需要与子AUV自身测量信息进行融合的相对与主AUV距离和方位模型包括：Step 4: Establish the relative and main AUV distance and orientation model that needs to be fused with the sub-AUV's own measurement information, including:

(1)以两个AUV的协同导航定位系统为例，建立子AUV与主AUV之间的相对距离模型为：(1) Taking the cooperative navigation and positioning system of two AUVs as an example, the relative distance model between the sub-AUV and the main AUV is established as follows:

$R R = = \sqrt{{((r r))}^{22} + + {(({r r}_{11}))}^{22} - - 22 {rr rr}_{11} cos cos ((θ θ - - {θ θ}_{11}))} - - - - - - ((33))$

(2)根据图2可建立子AUV与主AUV之间的相对方位角模型为：(2) According to Figure 2, the relative azimuth model between the sub-AUV and the main AUV can be established as:

步骤五：选取状态变量建立基于EKF滤波算法的系统状态方程和量测方程。Step 5: Select the state variables to establish the system state equation and measurement equation based on the EKF filter algorithm.

(1)建立EKF算法的状态方程：(1) Establish the state equation of the EKF algorithm:

系统的状态变量为：The state variables of the system are:

其中：为主AUV的航向角。in: Heading angle of the main AUV.

系统的离散状态方程为：The discrete state equation of the system is:

${\overset{^^}{S S}}_{k k}^{- -} = = {Φ Φ}_{k k,, k k - - 11} {\overset{^^}{S S}}_{k k - - 11} + + {T T}_{k k - - 11} {U u}_{k k - - 11} + + {G G}_{k k - - 11} {W W}_{k-1 k-1} - - - - - - ((77))$

系统的输入向量：The input vector of the system:

U_k-1＝[v_k-1 ω_k-1]^T (8)U _k-1 ＝[v _k-1 ω _k-1 ] ^T (8)

对应的输入矩阵：The corresponding input matrix:

${T T}_{k k - - 11} = = [\begin{matrix} 00 & 00 \\ 00 & 00 \\ 00 & 11 \end{matrix}] - - - - - - ((99))$

系统的噪声矩阵：The noise matrix of the system:

系统的状态转移矩阵为：The state transition matrix of the system is:

其中：T为采样周期。Where: T is the sampling period.

(2)建立EKF算法的离散量测方程：(2) Establish the discrete measurement equation of the EKF algorithm:

${Z Z}_{k k} = = {H h}_{k k} {\overset{^^}{S S}}_{k k}^{- -} + + {V V}_{k k} - - - - - - ((1212))$

系统的量测向量为：The measurement vector of the system is:

Z_k＝[R_k φ_k]^T (13)Z _k ＝[R _k φ _k ] ^T (13)

${H h}_{k k} = = [\begin{matrix} \frac{{r r}_{k k} (({r r}_{k k} - - {r r}_{11,, k k})) cos cos (({θ θ}_{k k} - - {θ θ}_{11,, k k}))}{{R R}_{k k}} & \frac{{r r}_{k k} {r r}_{11,, k k} sin sin (({θ θ}_{k k} - - {θ θ}_{11,, k k}))}{{R R}_{k k}} & 00 \\ \frac{- - {r r}_{11,, k k} cos cos (({θ θ}_{k k} - - {θ θ}_{11,, k k}))}{{R R}_{k k}^{22}} & \frac{{r r}_{k k} (({r r}_{k k} - - {r r}_{11,, k k})) cos cos (({θ θ}_{k k} - - {θ θ}_{11,, k k}))}{{R R}_{k k}^{22}} & 11 \end{matrix}] - - - - - - ((1414))$

${H h}_{k k} = = [\begin{matrix} \frac{{r r}_{k k} (({r r}_{k k} - - {r r}_{11,, k k})) cos cos (({θ θ}_{k k} - - {θ θ}_{11,, k k}))}{{R R}_{k k}} & \frac{{r r}_{k k} {r r}_{11,, k k} sin sin (({θ θ}_{k k} - - {θ θ}_{11,, k k}))}{{R R}_{k k}} & 00 \\ \frac{{r r}_{11,, k k} cos cos (({θ θ}_{k k} - - {θ θ}_{11,, k k}))}{{R R}_{k k}^{22}} & - - \frac{{r r}_{k k} (({r r}_{k k} - - {r r}_{11,, k k})) cos cos (({θ θ}_{k k} - - {θ θ}_{11,, k k}))}{{R R}_{k k}^{22}} & - - 11 \end{matrix}] - - - - - - ((1515))$

仿真时间2500秒，在相同的仿真条件下，分别对基于传统的笛卡尔坐标系和采用极坐标下的模型进行，为了更好的说明本发明的优势，在两种方法下分别对航推、基于位置观测信息的EKF算法和基于位置和方位角的EKF算法进行可比较。The simulation time is 2500 seconds. Under the same simulation conditions, the models based on the traditional Cartesian coordinate system and the polar coordinates are respectively carried out. In order to better illustrate the advantages of the present invention, under the two methods, the aerodynamic, The EKF algorithm based on position observation information is compared with the EKF algorithm based on position and azimuth.

实施效果：图3给出了主AUV，子AUV的真实轨迹；图4给出基于传统算法时的子AUV定位误差曲线，其中：EKF1表示观测信息为距离时在传统算法下的EKF估计轨迹，EKF2表示观测信息为距离加方位时在传统算法下的EKF估计轨迹，图4中的子图为基于EKF2算法的全局放大图；图5给出了基于传统算法时的子AUV的位置估计曲线，其中：EKF1表示观测信息为距离时在传统算法下的EKF估计误差，EKF2表示观测信息为距离加方位时在传统算法下的EKF估计误差，为了更加清楚明了的比较航推、EKF1和EKF2三种算法的定位结果，对其定位轨迹的末端进行放大，如图6所示；图7给出了基于本发明的子AUV定位误差曲线，其中：EKF1表示观测信息为距离时在本发明下的EKF估计轨迹，EKF2表示观测信息为距离加方位时在本发明下的EKF估计轨迹，图7中的子图为基于EKF2算法的全局放大图；图8给出了基于本发明的子AUV的位置估计曲线，其中：EKF1表示观测信息为距离时在本发明下的EKF估计误差，EKF2表示观测信息为距离加方位时在本发明下的EKF估计误差，为了更加清楚明了的比较航推、EKF1和EKF2三种算法的定位结果，对其定位轨迹的末端进行放大，如图9所示。通过图4和图7可以看出，无论是在传统算法还是在本发明提出的算法下，本发明提出的距离加方位观测法都能够获得较高的精度，同时比较图4和图7可知，对与曲线运动来说，本发明提能够获得更高的精度。Implementation effect: Figure 3 shows the real trajectory of the main AUV and sub-AUV; Figure 4 shows the positioning error curve of the sub-AUV based on the traditional algorithm, where: EKF1 represents the EKF estimated trajectory under the traditional algorithm when the observation information is distance, EKF2 represents the EKF estimated trajectory under the traditional algorithm when the observation information is distance plus azimuth. The sub-graph in Fig. 4 is the global enlarged map based on the EKF2 algorithm; Fig. 5 shows the position estimation curve of the sub-AUV based on the traditional algorithm. Among them: EKF1 indicates the EKF estimation error under the traditional algorithm when the observation information is distance, and EKF2 indicates the EKF estimation error under the traditional algorithm when the observation information is distance plus azimuth. The positioning result of the algorithm is enlarged at the end of its positioning track, as shown in Figure 6; Figure 7 provides the sub-AUV positioning error curve based on the present invention, wherein: EKF1 represents the EKF under the present invention when the observation information is distance Estimated trajectory, EKF2 represents the EKF estimated trajectory under the present invention when the observation information is distance plus azimuth, the sub-graph in Figure 7 is a global enlarged view based on the EKF2 algorithm; Figure 8 provides the position estimation based on the sub-AUV of the present invention Curve, wherein: EKF1 represents the EKF estimation error under the present invention when the observation information is distance, and EKF2 represents the EKF estimation error under the present invention when the observation information is distance plus azimuth, in order to more clearly compare aerial push, EKF1 and EKF2 The positioning results of the three algorithms are enlarged at the end of the positioning trajectory, as shown in Figure 9. As can be seen from Fig. 4 and Fig. 7, no matter in the traditional algorithm or under the algorithm proposed by the present invention, the distance plus azimuth observation method proposed by the present invention can obtain higher precision, and it can be known by comparing Fig. 4 and Fig. 7 simultaneously that For curved motion, the present invention can obtain higher precision.

步骤六：利用李导数对非线性系统进行可观测性分析。Step 6: Use Lie derivatives to analyze the observability of the nonlinear system.

建立非线性的状态方程为：The nonlinear state equation is established as:

当rcosθ-r₁cosθ₁≤0时，建立非线性的量测方程为：When rcosθ-r ₁ cosθ ₁ ≤0, the nonlinear measurement equation is established as:

可观测性矩阵为：The observability matrix is:

其中：a₁＝r-r₁cos(θ-θ₁)；a₁＝rr₁sin(θ-θ₁)；b₂＝-r²+rr₁cos(θ-θ₁)＝-ra₁； $c_{1} = h_{1}^{2} f_{1} - a_{1}^{2} f_{1} - a_{1} a_{2} f_{2};$ $c_{2} = h_{1}^{2} r^{2} f_{2} - a_{1} a_{2} f_{1} - a_{2}^{2} f_{2};$ $c_{3} = h_{1}^{2} (b_{2} f_{2} - b_{1} f_{1});$ $d_{1} = - h_{1}^{2} r f_{2} - 2 a_{1} b_{1} f_{1} - 2 a_{1} b_{2} f_{2};$ $d_{2} = - h_{1}^{2} r f_{1} - 2 a_{2} b_{1} f_{1} - 2 a_{2} b_{2} f_{2};$ $d_{3} = - h_{1}^{2} (a_{2} f_{2} + a_{1} f_{1}) .$ Where: a ₁ =rr ₁ cos(θ-θ ₁ ); a ₁ =rr ₁ sin(θ-θ ₁ ); b ₂ =-r ² +rr ₁ cos(θ-θ ₁ )=-ra ₁ ; $c_{1} = h_{1}^{2} f_{1} - a_{1}^{2} f_{1} - a_{1} a_{2} f_{2};$ $c_{2} = h_{1}^{2} r^{2} f_{2} - a_{1} a_{2} f_{1} - a_{2}^{2} f_{2};$ $c_{3} = h_{1}^{2} (b_{2} f_{2} - b_{1} f_{1});$ $d_{1} = - h_{1}^{2} r f_{2} - 2 a_{1} b_{1} f_{1} - 2 a_{1} b_{2} f_{2};$ $d_{2} = - h_{1}^{2} r f_{1} - 2 a_{2} b_{1} f_{1} - 2 a_{2} b_{2} f_{2};$ $d_{3} = - h_{1}^{2} (a_{2} f_{2} + a_{1} f_{1}) .$

当rcosθ-r₁cosθ₁＞0时，建立非线性的量测方程为：When rcosθ-r ₁ cosθ ₁ >0, the nonlinear measurement equation is established as:

可观测性矩阵为：The observability matrix is:

其中：a₁＝r-r₁cos(θ-θ₁)；a₁＝rr₁sin(θ-θ₁)；b₂＝r²-rr₁cos(θ-θ₁)＝ra₁； $c_{1} = h_{1}^{2} f_{1} - a_{1}^{2} f_{1} - a_{1} a_{2} f_{2};$ $c_{2} = h_{1}^{2} r^{2} f_{2} - a_{1} a_{2} f_{1} - a_{2}^{2} f_{2};$ $c_{3} = h_{1}^{2} (b_{2} f_{2} - b_{1} f_{1});$ $d_{1} = h_{1}^{2} r f_{2} - 2 a_{1} b_{1} f_{1} - 2 a_{1} b_{2} f_{2};$ $d_{2} = - h_{1}^{2} r f_{1} - 2 a_{2} b_{1} f_{1} - 2 a_{2} b_{2} f_{2};$ $d_{3} = h_{1}^{2} (a_{2} f_{2} + a_{1} f_{1}) .$ Where: a ₁ =rr ₁ cos(θ-θ ₁ ); a ₁ =rr ₁ sin(θ-θ ₁ ); b ₂ =r ² -rr ₁ cos(θ-θ ₁ )=ra ₁ ; $c_{1} = h_{1}^{2} f_{1} - a_{1}^{2} f_{1} - a_{1} a_{2} f_{2};$ $c_{2} = h_{1}^{2} r^{2} f_{2} - a_{1} a_{2} f_{1} - a_{2}^{2} f_{2};$ $c_{3} = h_{1}^{2} (b_{2} f_{2} - b_{1} f_{1});$ $d_{1} = h_{1}^{2} r f_{2} - 2 a_{1} b_{1} f_{1} - 2 a_{1} b_{2} f_{2};$ $d_{2} = - h_{1}^{2} r f_{1} - 2 a_{2} b_{1} f_{1} - 2 a_{2} b_{2} f_{2};$ $d_{3} = h_{1}^{2} (a_{2} f_{2} + a_{1} f_{1}) .$

通过分析可知，当r＝r₁,θ＝θ₁，即主AUV和子AUV的位置重合时，系统整体不可观；当主AUV和子AUV之间的相对距离不变时，距离观测子块即O₁不可观；当主AUV和子AUV之间的相对方位角不变时，距离观测子块即O₂不可观；当主AUV和子AUV之间的相对距离和相对方位角都不变时，整个系统不可观，所以为了提高系统的定位精度，保证系统的可观测性，应该保证主AUV和子AUV之间的相对距离或相对方位角为变化的。Through analysis, it can be seen that when r=r ₁ , θ=θ ₁ , that is, when the positions of the main AUV and the sub-AUV coincide, the system as a whole is not observable; when the relative distance between the main AUV and the sub-AUV remains unchanged, the distance observation sub-block is O ₁ is not observable; when the relative azimuth between the main AUV and the sub-AUV is constant, the distance observation sub-block that is _O2 is not observable; when the relative distance and relative azimuth between the main AUV and the sub-AUV are constant, the whole system is not observable, Therefore, in order to improve the positioning accuracy of the system and ensure the observability of the system, the relative distance or relative azimuth between the main AUV and sub-AUV should be guaranteed to be variable.

从以上实施例不难看出，相对于传统的在笛卡尔坐标系下的协同定位算法，本发明提供方法能够获得更高的精度。It is not difficult to see from the above embodiments that, compared with the traditional co-location algorithm in the Cartesian coordinate system, the method provided by the present invention can obtain higher precision.

Claims

1. the colocated method under the AUV curvilinear motion state based on polar coordinate system, is characterized in that, comprises the following steps:

Step 1: main and sub AUV starts collaborative navigation, acceleration and angular velocity information that the sub-AUV Inertial Measurement Unit of Real-time Collection records, and integration obtains speed and the course information of sub-AUV;

Step 2: gather sub-AUV with respect to the distance and bearing information of main AUV;

The sub-AUV gathering and the relative distance between main AUV are:

R = \sqrt{{(r)}^{2} + {(r_{1})}^{2} - 2 {rr}_{1} \cos (θ - θ_{1})}

Wherein: R is the relative distance between main AUV and sub-AUV; r ₁and θ ₁be radius and the polar angle of main AUV under polar coordinate system; R and θ are radius and the polar angle of sub-AUV under polar coordinate system;

The sub-AUV gathering and the relative bearing between main AUV are:

As rcos θ-r ₁cos θ ₁≤ 0 o'clock, have:

Wherein: φ is the relative bearing between sub-AUV and main AUV; course angle for sub-AUV;

As rcos θ-r ₁cos θ ₁during > 0, have:

Step 3: the speed of the sub-AUV obtaining in integrating step one and course information, and the sub-AUV obtaining in step 2 is with respect to the distance and bearing information of main AUV, utilize extended Kalman filter, the position of antithetical phrase AUV is estimated, realizes the navigator fix of sub-AUV;

Related system state vector is:

Wherein: it is the course angle of main AUV;

Related system discrete state equations is:

{\hat{S}}_{k}^{-} = Φ_{k, k - 1} {\hat{S}}_{k - 1} + T_{k - 1} U_{k - 1} + G_{k - 1} W_{k-1}

Wherein: with be respectively k, k-1 status predication value and state estimation value constantly; U _k-1for k-1 system input constantly; T _k-1for k-1 input matrix corresponding to the moment; Φ _{k, k-1}for state-transition matrix; G _k-1during for k-1, the noise of etching system drives matrix; W (t) is noise matrix:

The input vector of system:

U _k-1＝[v _k-1ω _k-1] ^T

Corresponding input matrix:

T_{k - 1} = [\begin{matrix} 0 & 0 \\ 0 & 0 \\ 0 & 1 \end{matrix}]

The noise matrix of system:

Wherein: correspond respectively to the k-1 radius of main AUV under polar coordinates constantly, polar angle and course error, correspond respectively to the k-1 radius of sub-AUV under polar coordinates constantly, polar angle and course error;

The state-transition matrix of system is:

Wherein: T is the sampling period;

Related system measurements equation is:

Z_{k} = H_{k} {\hat{S}}_{k}^{-} + V_{k}

Wherein: Z _kthe measurement of etching system vector during for k; H _kthe measurement matrix of etching system during for k; V _kfor measurement noise;

The measurement vector of system is:

Z _k＝[R _kφ _k] ^T

As rcos θ-r ₁cos θ ₁≤ 0 o'clock, measurement matrix was:

H_{k} = [\begin{matrix} \frac{r_{k} (r_{k} - r_{1, k}) \cos (θ_{k} - θ_{1, k})}{R_{k}} & \frac{r_{k} r_{1, k} \sin (θ_{k} - θ_{1, k})}{R_{k}} & 0 \\ \frac{- r_{1, k} \cos (θ_{k} - θ_{1, k})}{R_{k}^{2}} & \frac{r_{k} (r_{k} - r_{1, k}) \cos (θ_{k} - θ_{1, k})}{R_{k}^{2}} & 1 \end{matrix}];

As rcos θ-r ₁cos θ ₁during > 0, measurement matrix is:

H_{k} = [\begin{matrix} \frac{r_{k} (r_{k} - r_{1, k}) \cos (θ_{k} - θ_{1, k})}{R_{k}} & \frac{r_{k} r_{1, k} \sin (θ_{k} - θ_{1, k})}{R_{k}} & 0 \\ \frac{- r_{1, k} \cos (θ_{k} - θ_{1, k})}{R_{k}^{2}} & - \frac{r_{k} (r_{k} - r_{1, k}) \cos (θ_{k} - θ_{1, k})}{R_{k}^{2}} & - 1 \end{matrix}] .