一种基于极坐标系的AUV曲线运动状态下的协同定位方法.pdf

摘要
申请专利号：	CN201410195910.X	申请日：	2014.05.09
公开号：	CN103968838A	公开日：	2014.08.06
当前法律状态：	授权	有效性：	有权
法律详情：	授权\|\|\|实质审查的生效IPC(主分类):G01C 21/16申请日:20140509\|\|\|公开
IPC分类号：	G01C21/16; G01C21/20	主分类号：	G01C21/16
申请人：	哈尔滨工程大学
发明人：	高伟; 史宏洋; 刘博; 杨建; 于春阳; 张丽丽; 张亚; 梁宏; 兰海钰; 夏建忠
地址：	150001 黑龙江省哈尔滨市南岗区南通大街145号哈尔滨工程大学科技处知识产权办公室
优先权：
专利代理机构：		代理人：
PDF下载：	PDF下载

内容摘要

本发明提供的是一种基于极坐标系的AUV曲线运动状态下的协同定位方法。其中主AUV装备精度较高的惯性测量单元，而子AUV装备较低精度的IMU。首先，子AUV通过融合自身的惯性测量单元测量载体运动信息，并测定相对于主AUV的距离和方位信息，最后利用扩展卡尔曼滤波来实现协同定位。本发明与传统的基于笛卡尔坐标系的算法相比，在AUV做曲线运动时具有较高的定位精度。

权利要求书

权利要求书
1. 一种基于极坐标系的AUV曲线运动状态下的协同定位方法，其特征在于，包括以下步骤：
步骤一：主、子AUV开始协同导航，实时采集子AUV惯性测量单元测得的加速度和角速度信息，并积分得到子AUV的速度和航向信息；
步骤二：采集子AUV相对于主AUV的距离和方位信息；
采集的子AUV与主AUV之间的相对距离为：
R=(r)2+(r1)2-2rr1cos(θ-θ1)]]>
其中：R为主AUV和子AUV之间的相对距离；r1和θ1为主AUV在极坐标系下的半径和极角；r和θ为子AUV在极坐标系下的半径和极角；
采集的子AUV与主AUV之间的相对方位角为：
当rcosθ-r1cosθ1≤0时，有：

其中：φ为子AUV和主AUV之间的相对方位角；为子AUV的航向角；
当rcosθ-r1cosθ1＞0时，有：

步骤三：结合步骤一中得到的子AUV的速度和航向信息，以及步骤二中得到的子AUV相对于主AUV的距离和方位信息，利用扩展卡尔曼滤波器，对子AUV的位置进行估计，实现子AUV的导航定位；
所涉及的系统状态向量为：

其中：为主AUV的航向角；
所涉及的系统离散状态方程为：
S^k-=Φk,k-1S^k-1+Tk-1Uk-1+Gk-1Wk-1]]>
其中：和分别为k，k-1时刻的状态预测值和状态估计值；Uk-1为k-1时刻的系统输入；Tk-1为k-1时刻对应的输入矩阵；Φk,k-1为状态转移矩阵；Gk-1为k-1时刻系统的噪声驱动矩阵；W(t)为噪声矩阵：
系统的输入向量：
Uk-1＝[vk-1 ωk-1]T
对应的输入矩阵：
Tk-1=000001]]>
系统的噪声矩阵：

其中：分别对应于k-1时刻主AUV在极坐标下的半径，极角和航向误差，分别对应于k-1时刻子AUV在极坐标下的半径，极角和航向误差；
系统的状态转移矩阵为：

其中：T为采样周期；
所涉及的系统量测方程为：
Zk=HkS^k-+Vk]]>
其中：Zk为k时刻系统的量测向量；Hk为k时刻系统的量测矩阵；Vk为量测噪声；
系统的量测向量为：
Zk＝[Rk φk]T
当rcosθ-r1cosθ1≤0时，量测矩阵为：
Hk=rk(rk-r1,k)cos(θk-θ1,k)Rkrkr1,ksin(θk-θ1,k)Rk0-r1,kcos(θk-θ1,k)Rk2rk(rk-r1,k)cos(θk-θ1,k)Rk21;]]>
当rcosθ-r1cosθ1＞0时，量测矩阵为：
Hk=rk(rk-r1,k)cos(θk-θ1,k)Rkrkr1,ksin(θk-θ1,k)Rk0-r1,kcos(θk-θ1,k)Rk2-rk(rk-r1,k)cos(θk-θ1,k)Rk2-1.]]>

说明书

说明书一种基于极坐标系的AUV曲线运动状态下的协同定位方法
技术领域
本发明涉及的是一种在曲线运动状态下基于极坐标系的AUV曲线运动状态下的协同定位方法。
背景技术
未来是海洋文明的世纪，海洋领域的探索将成为世界的发展趋势，因此水面船艇作业、应用的研究具有广阔的前景。自主水下载体(Autonomous Underwater Vehicle,AUV)实际上是一种水下无人平台，以其体积小、机动性强和成本低等优势成为研究热点；可以执行各种导航和水下地理勘察等复杂任务[1]。随着人类进一步开发认识海洋，AUV研究、应用的理论价值和实际意义更为凸显。AUV与常规的船艇比较，易于操作、灵活快捷，可以经受住各种复杂环境的考验。AUV在允许侦测区域进行隐蔽及公开作业，要充分利用传感器信息，这就需要多AUV进行协同定位。目前，大多数的AUV协同定位系统都是基于笛卡尔坐标系建立的，且其运动状态大都为匀速直航，而极坐标系是针对曲线运动的，所以提出了一种基于极坐标系的协同定位算法，另外由于在AUV作曲线运动时，单纯的距离信息不能够很好的描述AUV的运动状态，因此提出了一种以距离加方位信息作为观测量的协同导航定位算法并进行了可观性分析。
发明内容
本发明的目的在于提供一种提高精度的基于极坐标系的AUV曲线运动状态下的新协同定位方法。
本发明的目的是这样实现的：
步骤一：主、子AUV开始协同导航，实时采集子AUV惯性测量单元测得的加速度和角速度信息，并积分得到子AUV的速度和航向信息；
步骤二：采集子AUV相对于主AUV的距离和方位信息；
采集的子AUV与主AUV之间的相对距离为：
R=(r)2+(r1)2-2rr1cos(θ-θ1)]]>
其中：R为主AUV和子AUV之间的相对距离；r1和θ1为主AUV在极坐标系下的半径和极角；r和θ为子AUV在极坐标系下的半径和极角；
采集的子AUV与主AUV之间的相对方位角为：
当rcosθ-r1cosθ1≤0时，有：

其中：φ为子AUV和主AUV之间的相对方位角；为子AUV的航向角；
当rcosθ-r1cosθ1＞0时，有：

步骤三：结合步骤一中得到的子AUV的速度和航向信息，以及步骤二中得到的子AUV相对于主AUV的距离和方位信息，利用扩展卡尔曼滤波器，对子AUV的位置进行估计，实现子AUV的导航定位；
所涉及的系统状态向量为：

其中：为主AUV的航向角；
所涉及的系统离散状态方程为：
S^k-=Φk,k-1S^k-1+Tk-1Uk-1+Gk-1Wk-1]]>
其中：和分别为k，k-1时刻的状态预测值和状态估计值；Uk-1为k-1时刻的系统输入；Tk-1为k-1时刻对应的输入矩阵；Φk,k-1为状态转移矩阵；Gk-1为k-1时刻系统的噪声驱动矩阵；W(t)为噪声矩阵：
系统的输入向量：
Uk-1＝[vk-1 ωk-1]T
对应的输入矩阵：
Tk-1=000001]]>
系统的噪声矩阵：

其中：分别对应于k-1时刻主AUV在极坐标下的半径，极角和航向误差，分别对应于k-1时刻子AUV在极坐标下的半径，极角和航向误差；
系统的状态转移矩阵为：

其中：T为采样周期；
所涉及的系统量测方程为：
Zk=HkS^k-+Vk,]]>
其中：Zk为k时刻系统的量测向量；Hk为k时刻系统的量测矩阵；Vk为量测噪声；
系统的量测向量为：
Zk＝[Rk φk]T，
当rcosθ-r1cosθ1≤0时，量测矩阵为：
Hk=rk(rk-r1,k)cos(θk-θ1,k)Rkrkr1,ksin(θk-θ1,k)Rk0-r1,kcos(θk-θ1,k)Rk2rk(rk-r1,k)cos(θk-θ1,k)Rk21;]]>
当rcosθ-r1cosθ1＞0时，量测矩阵为：
Hk=rk(rk-r1,k)cos(θk-θ1,k)Rkrkr1,ksin(θk-θ1,k)Rk0-r1,kcos(θk-θ1,k)Rk2-rk(rk-r1,k)cos(θk-θ1,k)Rk2-1.]]>
本发明的有益效果在于：
(1)与传统的基于笛卡尔坐标系下的系统动力学模型相比，极坐标系下的系统动力学模型更适用于AUV作曲线运动的状态下。
(2)与传统的仅用相对距离信息作为观测量的算法相比，采用距离加方位信息作为观测量时能够更好的反映曲线运动下AUV的运动状态，从而使得定位结果更精确。
(3)采用李导数对系统的可观测性进行了分析，为协同定位提供了先决条件，进而有助于定位精度的进一步提高。
附图说明
图1是本发明的方法流程图；
图2是AUV的估计模型；
图3是主AUV和子AUV的真实运动轨迹；
图4是基于传统算法时的子AUV定位误差曲线；
图5是基于传统算法时的子AUV的位置估计曲线；
图6是基于传统算法时的子AUV的位置估计曲线的末端放大图；
图7是基于新算法时的子AUV定位误差曲线；
图8是基于新算法时的子AUV的位置估计曲线；
图9是基于新算法时的子AUV的位置估计曲线的末端放大图。
具体实施方式
下面将结合附图对本发明作进一步的详细说明。
基于极坐标系的AUV曲线运动状态下的新协同定位算法，包括以下几个步骤：
步骤一：采集惯性测量单元(Inertial Measurement Unit,IMU)测量的加速度、和角速度信息。
步骤二：利用IMU测得的加速度信息和角速度信息积分得到载体AUV的速度和航向信息，并利用该信息建立传统的笛卡尔坐标系下AUV的系统方程。
步骤三：利用极坐标系和步骤二的笛卡尔坐标系之间的转换关系将步骤二中AUV的系统方程转换到极坐标下。
步骤四：采集子AUV相对于主AUV的距离和方位信息。
步骤五：基于步骤三和步骤四建立扩展卡尔曼滤波(Extend Kalman Filter,EKF)的状态方程和量测方程，对子AUV的位置进行估计，以实现定位。
利用IMU测得的加速度信息和角速度信息积分得到载体AUV的速度和航向信息，并利用该信息建立传统的笛卡尔坐标系下AUV的系统方程为：

其中：vi和ωi分别为第i个水下自主载体(Autonomous Underwater Vehicle,AUV)的速度和加速度，为第i个AUV航向角，为第i个AUV航向角的一阶导数。由于AUV作曲线运动，则ωi≠0。
利用极坐标系和步骤二的笛卡尔坐标系之间的转换关系将步骤二中AUV的系统方程转换到极坐标下为：

其中：ri和θi分别为第i个AUV在极坐标系下的半径和极角。
采集的子AUV相对于主AUV的距离和方位信息为：
(1)以两个AUV的协同导航定位系统为例，采集的子AUV与主AUV之间的相对距离为：
R=(r)2+(r1)2-2rr1cos(θ-θ1)]]>
其中：R为主AUV和子AUV之间的相对距离；r1和θ1为主AUV在极坐标系下的半径和极角；r和θ为子AUV在极坐标系下的半径和极角。
(2)采集的子AUV与主AUV之间的相对方位角为：
当rcosθ-r1cosθ1≤0时，有：

其中：φ为子AUV和主AUV之间的相对方位角；为子AUV的航向角。
当rcosθ-r1cosθ1＞0时，有：

基于步骤三和步骤四建立EKF的状态方程和量测方程包括：
(1)EKF算法的状态方程：
系统的状态变量为：
其中：为主AUV的航向角。
系统的离散状态方程为：
S^k-=Φk,k-1S^k-1+Tk-1Uk-1+Gk-1Wk-1]]>
其中：和分别为k，k-1时刻的状态预测值和状态估计值；Uk-1为k-1时刻的系统输入；Tk-1为k-1时刻对应的输入矩阵；Φk,k-1为状态转移矩阵；Gk-1为k-1时刻系统的噪声驱动矩阵；W(t)为噪声矩阵。
系统的输入向量：
Uk-1＝[vk-1 ωk-1]T
对应的输入矩阵：
Tk-1=000001]]>
系统的噪声矩阵：

其中：分别对应于k-1时刻主AUV在极坐标下的半径，极角和航向误差，分别对应于k-1时刻子AUV在极坐标下的半径，极角和航向误差。
系统的状态转移矩阵为：

其中：T为采样周期。
(2)EKF算法的离散量测方程：
Zk=HkS^k-+Vk,]]>
其中：Zk为k时刻系统的量测向量；Hk为k时刻系统的量测矩阵；Vk为量测噪声。
系统的量测向量为：
Zk＝[Rk φk]T
当rcosθ-r1cosθ1≤0时，量测矩阵为：
Hk=rk(rk-r1,k)cos(θk-θ1,k)Rkrkr1,ksin(θk-θ1,k)Rk0-r1,kcos(θk-θ1,k)Rk2rk(rk-r1,k)cos(θk-θ1,k)Rk21]]>
当rcosθ-r1cosθ1＞0时，量测矩阵为：
Hk=rk(rk-r1,k)cos(θk-θ1,k)Rkrkr1,ksin(θk-θ1,k)Rk0r1,kcos(θk-θ1,k)Rk2-rk(rk-r1,k)cos(θk-θ1,k)Rk2-1]]>
通过EKF算法对子AUV的位置进行估计，以实现定位。
本发明提供的是一种基于极坐标系的自主水下载体(Autonomous Underwater Vehicle,AUV)曲线运动状态下的新的协同定位算法。其中主AUV装备精度较高的惯性测量单元(Inertial Measurement Unit,IMU)，而子AUV装备较低精度的IMU。首先子AUV通过融合自身的IMU测量信息以及相对于主AUV的距离和方位信息并利用扩展卡尔曼滤波(ExtendKalman Filter,EKF)来实现协同定位。然后对从可观测性分析的角度进一步说明了该协同定位系统性能的改善。最后从仿真角度验证了该协同定位算法的有效性并获得了较高的定位性能。本发明与传统的基于笛卡尔坐标系的算法相比，具有较高的定位精度，同时比航位推算算法和只融合距离测量信息的算法的精度较。
本发明的目的是提供一种针对AUV的曲线运动状态提供一种基于极坐标系的协同定位算法。首先在普通模型的基础上建立极坐标下的系统动力学模型；然后建立主AUV和子AUV之间的相对距离模型和相对方位角模型；在上述基础上利用卡尔曼滤波对系统子AUV进行定位；最后利用李导数对该非线性系统进行了可观测性分析，分析了本发明的优势。
本发明包括以下几个步骤：
步骤一：采集惯性测量单元(Inertial Measurement Unit,IMU)测量的加速度和速度信息；
步骤二、建立传统的笛卡尔坐标系下AUV的系统动力学方程；
步骤三、利用极坐标系和传统的笛卡尔坐标系之间的转换关系建立极坐标系下AUV的系统动力学方程；
步骤四、采集需要与子AUV自身测量信息进行融合的相对与主AUV距离和方位信息；
步骤五、选取状态变量建立扩展卡尔曼滤波(Extend Kalman Filter,EKF)的状态方程和量测方程，对子AUV的位置进行估计，以实现定位；
步骤六、根据极坐标系下建立的系统状态方程和量测方程，利用李导数对该非线性系统进行可观测性分析。
本发明的一种基于极坐标系的AUV曲线运动状态下的新协同定位算法，流程图如图1所示，包括以下几个步骤：
步骤一：采集惯性测量单元(Inertial Measurement Unit,IMU)测量的加速度、速度和角速度信息。
步骤二：建立传统的笛卡尔坐标系下第i个AUV的系统动力学方程为：

其中：vi和ωi分别为第i个水下自主载体(Autonomous Underwater Vehicle,AUV)的速度和加速度，为第i个AUV航向角，为第i个AUV航向角的一阶导数。由于AUV作曲线运动，则ωi≠0。
步骤三：利用极坐标系和传统的笛卡尔坐标系之间的转换关系建立极坐标系下第i个AUV的系统动力学方程为：

其中：ri和θi分别为第i个AUV在极坐标系下的半径和极角。
步骤四：建立需要与子AUV自身测量信息进行融合的相对与主AUV距离和方位模型包括：
(1)以两个AUV的协同导航定位系统为例，建立子AUV与主AUV之间的相对距离模型为：
R=(r)2+(r1)2-2rr1cos(θ-θ1)---(3)]]>
其中：R为主AUV和子AUV之间的相对距离；r1和θ1为主AUV在极坐标系下的半径和极角；r和θ为子AUV在极坐标系下的半径和极角。
(2)根据图2可建立子AUV与主AUV之间的相对方位角模型为：
当rcosθ-r1cosθ1≤0时，有：

其中：φ为子AUV和主AUV之间的相对方位角；为子AUV的航向角。
当rcosθ-r1cosθ1＞0时，有：

步骤五：选取状态变量建立基于EKF滤波算法的系统状态方程和量测方程。
(1)建立EKF算法的状态方程：
系统的状态变量为：

其中：为主AUV的航向角。
系统的离散状态方程为：
S^k-=Φk,k-1S^k-1+Tk-1Uk-1+Gk-1Wk-1---(7)]]>
其中：和分别为k，k-1时刻的状态预测值和状态估计值；Uk-1为k-1时刻的系统输入；Tk-1为k-1时刻对应的输入矩阵；Φk,k-1为状态转移矩阵；Gk-1为k-1时刻系统的噪声驱动矩阵；W(t)为噪声矩阵。
系统的输入向量：
Uk-1＝[vk-1 ωk-1]T (8)
对应的输入矩阵：
Tk-1=000001---(9)]]>
系统的噪声矩阵：

其中：分别对应于k-1时刻主AUV在极坐标下的半径，极角和航向误差，分别对应于k-1时刻子AUV在极坐标下的半径，极角和航向误差。
系统的状态转移矩阵为：

其中：T为采样周期。
(2)建立EKF算法的离散量测方程：
Zk=HkS^k-+Vk---(12)]]>
其中：Zk为k时刻系统的量测向量；Hk为k时刻系统的量测矩阵；Vk为量测噪声。
系统的量测向量为：
Zk＝[Rk φk]T (13)
当rcosθ-r1cosθ1≤0时，量测矩阵为：
Hk=rk(rk-r1,k)cos(θk-θ1,k)Rkrkr1,ksin(θk-θ1,k)Rk0-r1,kcos(θk-θ1,k)Rk2rk(rk-r1,k)cos(θk-θ1,k)Rk21---(14)]]>
当rcosθ-r1cosθ1＞0时，量测矩阵为：
Hk=rk(rk-r1,k)cos(θk-θ1,k)Rkrkr1,ksin(θk-θ1,k)Rk0r1,kcos(θk-θ1,k)Rk2-rk(rk-r1,k)cos(θk-θ1,k)Rk2-1---(15)]]>
仿真时间2500秒，在相同的仿真条件下，分别对基于传统的笛卡尔坐标系和采用极坐标下的模型进行，为了更好的说明本发明的优势，在两种方法下分别对航推、基于位置观测信息的EKF算法和基于位置和方位角的EKF算法进行可比较。
实施效果：图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可知，对与曲线运动来说，本发明提能够获得更高的精度。
步骤六：利用李导数对非线性系统进行可观测性分析。
建立非线性的状态方程为：

当rcosθ-r1cosθ1≤0时，建立非线性的量测方程为：

可观测性矩阵为：

其中：a1＝r-r1cos(θ-θ1)；a1＝rr1sin(θ-θ1)；b2＝-r2+rr1cos(θ-θ1)＝-ra1；c1=h12f1-a12f1-a1a2f2;]]>c2=h12r2f2-a1a2f1-a22f2;]]>c3=h12(b2f2-b1f1);]]>d1=-h12rf2-2a1b1f1-2a1b2f2;]]>d2=-h12rf1-2a2b1f1-2a2b2f2;]]>d3=-h12(a2f2+a1f1).]]>
当rcosθ-r1cosθ1＞0时，建立非线性的量测方程为：

可观测性矩阵为：

其中：a1＝r-r1cos(θ-θ1)；a1＝rr1sin(θ-θ1)；b2＝r2-rr1cos(θ-θ1)＝ra1；c1=h12f1-a12f1-a1a2f2;]]>c2=h12r2f2-a1a2f1-a22f2;]]>c3=h12(b2f2-b1f1);]]>d1=h12rf2-2a1b1f1-2a1b2f2;]]>d2=-h12rf1-2a2b1f1-2a2b2f2;]]>d3=h12(a2f2+a1f1).]]>
通过分析可知，当r＝r1,θ＝θ1，即主AUV和子AUV的位置重合时，系统整体不可观；当主AUV和子AUV之间的相对距离不变时，距离观测子块即O1不可观；当主AUV和子AUV之间的相对方位角不变时，距离观测子块即O2不可观；当主AUV和子AUV之间的相对距离和相对方位角都不变时，整个系统不可观，所以为了提高系统的定位精度，保证系统的可观测性，应该保证主AUV和子AUV之间的相对距离或相对方位角为变化的。
从以上实施例不难看出，相对于传统的在笛卡尔坐标系下的协同定位算法，本发明提供方法能够获得更高的精度。

资源描述

《一种基于极坐标系的AUV曲线运动状态下的协同定位方法.pdf》由会员分享，可在线阅读，更多相关《一种基于极坐标系的AUV曲线运动状态下的协同定位方法.pdf（22页珍藏版）》请在专利查询网上搜索。

1、(10)申请公布号 CN 103968838 A (43)申请公布日 2014.08.06 CN 103968838 A (21)申请号 201410195910.X (22)申请日 2014.05.09 G01C 21/16(2006.01) G01C 21/20(2006.01) (71)申请人哈尔滨工程大学地址 150001 黑龙江省哈尔滨市南岗区南通大街 145 号哈尔滨工程大学科技处知识产权办公室 (72)发明人高伟史宏洋刘博杨建于春阳张丽丽张亚梁宏兰海钰夏建忠 (54) 发明名称一种基于极坐标系的 AUV 曲线运动状态下的协同定位方法 (57) 摘要。

2、本发明提供的是一种基于极坐标系的 AUV 曲线运动状态下的协同定位方法。其中主 AUV 装备精度较高的惯性测量单元，而子 AUV 装备较低精度的 IMU。首先，子 AUV 通过融合自身的惯性测量单元测量载体运动信息，并测定相对于主 AUV 的距离和方位信息，最后利用扩展卡尔曼滤波来实现协同定位。本发明与传统的基于笛卡尔坐标系的算法相比，在 AUV 做曲线运动时具有较高的定位精度。 (51)Int.Cl. 权利要求书 2 页说明书 11 页附图 8 页 (19)中华人民共和国国家知识产权局 (12)发明专利申请权利要求书2页说明书11页附图8页 (10)申请。

3、公布号 CN 103968838 A CN 103968838 A 1/2 页 2 1. 一种基于极坐标系的 AUV 曲线运动状态下的协同定位方法，其特征在于，包括以下步骤：步骤一：主、子 AUV 开始协同导航，实时采集子 AUV 惯性测量单元测得的加速度和角速度信息，并积分得到子 AUV 的速度和航向信息；步骤二：采集子 AUV 相对于主 AUV 的距离和方位信息；采集的子 AUV 与主 AUV 之间的相对距离为：其中： R 为主 AUV 和子 AUV 之间的相对距离； r1和 1为主 AUV 在极坐标系下的半径和极角； r 和为子 AUV 在。

4、极坐标系下的半径和极角；采集的子 AUV 与主 AUV 之间的相对方位角为：当 rcos-r1cos1 0 时，有：其中：为子 AUV 和主 AUV 之间的相对方位角；为子 AUV 的航向角；当 rcos-r1cos1 0 时，有：步骤三：结合步骤一中得到的子AUV的速度和航向信息，以及步骤二中得到的子AUV相对于主 AUV 的距离和方位信息，利用扩展卡尔曼滤波器，对子 AUV 的位置进行估计，实现子 AUV 的导航定位；所涉及的系统状态向量为：其中：为主 AUV 的航向角；所涉及的系统离散状态方程为：其中：和分别为k， k。

5、-1时刻的状态预测值和状态估计值； Uk-1为k-1时刻的系统输入； Tk-1为 k-1 时刻对应的输入矩阵； k,k-1为状态转移矩阵； Gk-1为 k-1 时刻系统的噪声驱动矩阵； W(t) 为噪声矩阵：系统的输入向量： Uk-1 vk-1 k-1T 对应的输入矩阵：系统的噪声矩阵：权利要求书 CN 103968838 A 2 2/2 页 3 其中：分别对应于k-1时刻主AUV在极坐标下的半径，极角和航向误差，分别对应于 k-1 时刻子 AUV 在极坐标下的半径，极角和航向误差；系统的状态转移矩阵为：其中： T 为采样周期；所涉及的。

6、系统量测方程为：其中： Zk为 k 时刻系统的量测向量； Hk为 k 时刻系统的量测矩阵； Vk为量测噪声；系统的量测向量为： Zk Rk kT 当 rcos-r1cos1 0 时，量测矩阵为：当 rcos-r1cos1 0 时，量测矩阵为：权利要求书 CN 103968838 A 3 1/11 页 4 一种基于极坐标系的 AUV 曲线运动状态下的协同定位方法技术领域 0001 本发明涉及的是一种在曲线运动状态下基于极坐标系的 AUV 曲线运动状态下的协同定位方法。背景技术 0002 未来是海洋文明的世纪，海洋领域的探索将成为世界的发展趋势，因此水。

7、面船艇作业、应用的研究具有广阔的前景。自主水下载体(Autonomous Underwater Vehicle,AUV) 实际上是一种水下无人平台，以其体积小、机动性强和成本低等优势成为研究热点；可以执行各种导航和水下地理勘察等复杂任务 1。随着人类进一步开发认识海洋， AUV 研究、应用的理论价值和实际意义更为凸显。 AUV与常规的船艇比较，易于操作、灵活快捷，可以经受住各种复杂环境的考验。AUV 在允许侦测区域进行隐蔽及公开作业，要充分利用传感器信息，这就需要多 AUV 进行协同定位。目前，大多数的 AUV 协同定位系统都是基于笛卡尔坐标系建立的，且。

8、其运动状态大都为匀速直航，而极坐标系是针对曲线运动的，所以提出了一种基于极坐标系的协同定位算法，另外由于在 AUV 作曲线运动时，单纯的距离信息不能够很好的描述 AUV 的运动状态，因此提出了一种以距离加方位信息作为观测量的协同导航定位算法并进行了可观性分析。发明内容 0003 本发明的目的在于提供一种提高精度的基于极坐标系的 AUV 曲线运动状态下的新协同定位方法。 0004 本发明的目的是这样实现的： 0005 步骤一：主、子 AUV 开始协同导航，实时采集子 AUV 惯性测量单元测得的加速度和角速度信息，并积分得到子 AUV 的速度和航向信息； 000。

9、6 步骤二：采集子 AUV 相对于主 AUV 的距离和方位信息； 0007 采集的子 AUV 与主 AUV 之间的相对距离为： 0008 0009 其中： R 为主 AUV 和子 AUV 之间的相对距离； r1和 1为主 AUV 在极坐标系下的半径和极角； r 和为子 AUV 在极坐标系下的半径和极角； 0010 采集的子 AUV 与主 AUV 之间的相对方位角为： 0011 当 rcos-r1cos1 0 时，有： 0012 0013 其中：为子 AUV 和主 AUV 之间的相对方位角；为子 AUV 的航向角； 0014 当 rcos-r1cos1 0 时。

10、，有：说明书 CN 103968838 A 4 2/11 页 5 0015 0016 步骤三：结合步骤一中得到的子 AUV 的速度和航向信息，以及步骤二中得到的子 AUV相对于主AUV的距离和方位信息，利用扩展卡尔曼滤波器，对子AUV的位置进行估计，实现子 AUV 的导航定位； 0017 所涉及的系统状态向量为： 0018 0019 其中：为主 AUV 的航向角； 0020 所涉及的系统离散状态方程为： 0021 0022 其中：和分别为k， k-1时刻的状态预测值和状态估计值； Uk-1为k-1时刻的系统输入； Tk-1为 k-1 时刻对应的输入。

11、矩阵； k,k-1为状态转移矩阵； Gk-1为 k-1 时刻系统的噪声驱动矩阵； W(t) 为噪声矩阵： 0023 系统的输入向量： 0024 Uk-1 vk-1 k-1T 0025 对应的输入矩阵： 0026 0027 系统的噪声矩阵： 0028 0029 其中：分别对应于k-1时刻主AUV在极坐标下的半径，极角和航向误差，分别对应于 k-1 时刻子 AUV 在极坐标下的半径，极角和航向误差； 0030 系统的状态转移矩阵为： 0031 0032 其中： T 为采样周期； 0033 所涉及的系统量测方程为：说明书 CN 103968838 A 5 3/。

12、11 页 6 0034 0035 其中： Zk为 k 时刻系统的量测向量； Hk为 k 时刻系统的量测矩阵； Vk为量测噪声； 0036 系统的量测向量为： 0037 Zk Rk kT， 0038 当 rcos-r1cos1 0 时，量测矩阵为： 0039 0040 当 rcos-r1cos1 0 时，量测矩阵为： 0041 0042 本发明的有益效果在于： 0043 (1) 与传统的基于笛卡尔坐标系下的系统动力学模型相比，极坐标系下的系统动力学模型更适用于 AUV 作曲线运动的状态下。 0044 (2) 与传统的仅用相对距离信息作为观测量的算法相比，采用距离加方位信。

13、息作为观测量时能够更好的反映曲线运动下 AUV 的运动状态，从而使得定位结果更精确。 0045 (3) 采用李导数对系统的可观测性进行了分析，为协同定位提供了先决条件，进而有助于定位精度的进一步提高。附图说明 0046 图 1 是本发明的方法流程图； 0047 图 2 是 AUV 的估计模型； 0048 图 3 是主 AUV 和子 AUV 的真实运动轨迹； 0049 图 4 是基于传统算法时的子 AUV 定位误差曲线； 0050 图 5 是基于传统算法时的子 AUV 的位置估计曲线； 0051 图 6 是基于传统算法时的子 AUV 的位置估计曲线的末端放大图； 0052。

14、图 7 是基于新算法时的子 AUV 定位误差曲线； 0053 图 8 是基于新算法时的子 AUV 的位置估计曲线； 0054 图 9 是基于新算法时的子 AUV 的位置估计曲线的末端放大图。具体实施方式 0055 下面将结合附图对本发明作进一步的详细说明。 0056 基于极坐标系的 AUV 曲线运动状态下的新协同定位算法，包括以下几个步骤：说明书 CN 103968838 A 6 4/11 页 7 0057 步骤一：采集惯性测量单元(Inertial Measurement Unit,IMU)测量的加速度、和角速度信息。 0058 步骤二：利用 IMU 测得的加速。

15、度信息和角速度信息积分得到载体 AUV 的速度和航向信息，并利用该信息建立传统的笛卡尔坐标系下 AUV 的系统方程。 0059 步骤三：利用极坐标系和步骤二的笛卡尔坐标系之间的转换关系将步骤二中 AUV 的系统方程转换到极坐标下。 0060 步骤四：采集子 AUV 相对于主 AUV 的距离和方位信息。 0061 步骤五：基于步骤三和步骤四建立扩展卡尔曼滤波(Extend Kalman Filter,EKF) 的状态方程和量测方程，对子 AUV 的位置进行估计，以实现定位。 0062 利用 IMU 测得的加速度信息和角速度信息积分得到载体 AUV 的速度和航向信息，并利用该。

16、信息建立传统的笛卡尔坐标系下 AUV 的系统方程为： 0063 0064 其中： vi和 i分别为第 i 个水下自主载体 (Autonomous Underwater Vehicle,AUV) 的速度和加速度，为第 i 个 AUV 航向角，为第 i 个 AUV 航向角的一阶导数。由于 AUV 作曲线运动，则 i 0。 0065 利用极坐标系和步骤二的笛卡尔坐标系之间的转换关系将步骤二中 AUV 的系统方程转换到极坐标下为： 0066 0067 其中： ri和 i分别为第 i 个 AUV 在极坐标系下的半径和极角。 0068 采集的子 AUV 相对于主 AU。

17、V 的距离和方位信息为： 0069 (1)以两个AUV的协同导航定位系统为例，采集的子AUV与主AUV之间的相对距离为： 0070 0071 其中： R 为主 AUV 和子 AUV 之间的相对距离； r1和 1为主 AUV 在极坐标系下的半径和极角； r 和为子 AUV 在极坐标系下的半径和极角。 0072 (2) 采集的子 AUV 与主 AUV 之间的相对方位角为： 0073 当 rcos-r1cos1 0 时，有： 0074 说明书 CN 103968838 A 7 5/11 页 8 0075 其中：为子 AUV 和主 AUV 之间的相对方位角；为子 A。

18、UV 的航向角。 0076 当 rcos-r1cos1 0 时，有： 0077 0078 基于步骤三和步骤四建立 EKF 的状态方程和量测方程包括： 0079 (1)EKF 算法的状态方程： 0080 系统的状态变量为： 0081 其中：为主 AUV 的航向角。 0082 系统的离散状态方程为： 0083 0084 其中：和分别为k， k-1时刻的状态预测值和状态估计值； Uk-1为k-1时刻的系统输入； Tk-1为 k-1 时刻对应的输入矩阵； k,k-1为状态转移矩阵； Gk-1为 k-1 时刻系统的噪声驱动矩阵； W(t) 为噪声矩阵。 0085 系统的。

19、输入向量： 0086 Uk-1 vk-1 k-1T 0087 对应的输入矩阵： 0088 0089 系统的噪声矩阵： 0090 0091 其中：分别对应于 k-1 时刻主 AUV 在极坐标下的半径，极角和航向误差，分别对应于 k-1 时刻子 AUV 在极坐标下的半径，极角和航向误差。 0092 系统的状态转移矩阵为： 0093 0094 其中： T 为采样周期。 0095 (2)EKF 算法的离散量测方程： 0096 说明书 CN 103968838 A 8 6/11 页 9 0097 其中： Zk为 k 时刻系统的量测向量； Hk为 k 时刻系统的量测矩阵；。

20、Vk为量测噪声。 0098 系统的量测向量为： 0099 Zk Rk kT 0100 当 rcos-r1cos1 0 时，量测矩阵为： 0101 0102 当 rcos-r1cos1 0 时，量测矩阵为： 0103 0104 通过 EKF 算法对子 AUV 的位置进行估计，以实现定位。 0105 本发明提供的是一种基于极坐标系的自主水下载体 (Autonomous Underwater Vehicle,AUV) 曲线运动状态下的新的协同定位算法。其中主 AUV 装备精度较高的惯性测量单元 (Inertial Measurement Unit,IMU)，而子 AUV 装备较低精度。

21、的 IMU。首先子 AUV 通过融合自身的 IMU 测量信息以及相对于主 AUV 的距离和方位信息并利用扩展卡尔曼滤波 (Extend Kalman Filter,EKF)来实现协同定位。然后对从可观测性分析的角度进一步说明了该协同定位系统性能的改善。最后从仿真角度验证了该协同定位算法的有效性并获得了较高的定位性能。本发明与传统的基于笛卡尔坐标系的算法相比，具有较高的定位精度，同时比航位推算算法和只融合距离测量信息的算法的精度较。 0106 本发明的目的是提供一种针对 AUV 的曲线运动状态提供一种基于极坐标系的协同定位算法。首先在普通模型的基础上建立极坐标下的系统动力学模。

22、型；然后建立主 AUV 和子 AUV 之间的相对距离模型和相对方位角模型；在上述基础上利用卡尔曼滤波对系统子 AUV 进行定位；最后利用李导数对该非线性系统进行了可观测性分析，分析了本发明的优势。 0107 本发明包括以下几个步骤： 0108 步骤一：采集惯性测量单元 (Inertial Measurement Unit,IMU) 测量的加速度和速度信息； 0109 步骤二、建立传统的笛卡尔坐标系下 AUV 的系统动力学方程； 0110 步骤三、利用极坐标系和传统的笛卡尔坐标系之间的转换关系建立极坐标系下 AUV 的系统动力学方程； 0111 步骤四、采集需。

23、要与子 AUV 自身测量信息进行融合的相对与主 AUV 距离和方位信息； 0112 步骤五、选取状态变量建立扩展卡尔曼滤波(Extend Kalman Filter,EKF)的状态方程和量测方程，对子 AUV 的位置进行估计，以实现定位；说明书 CN 103968838 A 9 7/11 页 10 0113 步骤六、根据极坐标系下建立的系统状态方程和量测方程，利用李导数对该非线性系统进行可观测性分析。 0114 本发明的一种基于极坐标系的 AUV 曲线运动状态下的新协同定位算法，流程图如图 1 所示，包括以下几个步骤： 0115 步骤一：采集惯性测量单元(。

24、Inertial Measurement Unit,IMU)测量的加速度、速度和角速度信息。 0116 步骤二：建立传统的笛卡尔坐标系下第 i 个 AUV 的系统动力学方程为： 0117 0118 其中： vi和 i分别为第 i 个水下自主载体 (Autonomous Underwater Vehicle,AUV) 的速度和加速度，为第 i 个 AUV 航向角，为第 i 个 AUV 航向角的一阶导数。由于 AUV 作曲线运动，则 i 0。 0119 步骤三：利用极坐标系和传统的笛卡尔坐标系之间的转换关系建立极坐标系下第 i 个 AUV 的系统动力学方。

25、程为： 0120 0121 其中： ri和 i分别为第 i 个 AUV 在极坐标系下的半径和极角。 0122 步骤四：建立需要与子 AUV 自身测量信息进行融合的相对与主 AUV 距离和方位模型包括： 0123 (1)以两个AUV的协同导航定位系统为例，建立子AUV与主AUV之间的相对距离模型为： 0124 0125 其中： R 为主 AUV 和子 AUV 之间的相对距离； r1和 1为主 AUV 在极坐标系下的半径和极角； r 和为子 AUV 在极坐标系下的半径和极角。 0126 (2) 根据图 2 可建立子 AUV 与主 AUV 之间的相对方位角模型为： 01。

26、27 当 rcos-r1cos1 0 时，有： 0128 0129 其中：为子 AUV 和主 AUV 之间的相对方位角；为子 AUV 的航向角。 0130 当 rcos-r1cos1 0 时，有： 0131 说明书 CN 103968838 A 10 8/11 页 11 0132 步骤五：选取状态变量建立基于 EKF 滤波算法的系统状态方程和量测方程。 0133 (1) 建立 EKF 算法的状态方程： 0134 系统的状态变量为： 0135 0136 其中：为主 AUV 的航向角。 0137 系统的离散状态方程为： 0138 0139 其中：和分别为k，。

27、k-1时刻的状态预测值和状态估计值； Uk-1为k-1时刻的系统输入； Tk-1为 k-1 时刻对应的输入矩阵； k,k-1为状态转移矩阵； Gk-1为 k-1 时刻系统的噪声驱动矩阵； W(t) 为噪声矩阵。 0140 系统的输入向量： 0141 Uk-1 vk-1 k-1T (8) 0142 对应的输入矩阵： 0143 0144 系统的噪声矩阵： 0145 0146 其中：分别对应于k-1时刻主AUV在极坐标下的半径，极角和航向误差，分别对应于 k-1 时刻子 AUV 在极坐标下的半径，极角和航向误差。 0147 系统的状态转移矩阵为： 0148 0149 其。

28、中： T 为采样周期。 0150 (2) 建立 EKF 算法的离散量测方程： 0151 0152 其中： Zk为 k 时刻系统的量测向量； Hk为 k 时刻系统的量测矩阵； Vk为量测噪声。说明书 CN 103968838 A 11 9/11 页 12 0153 系统的量测向量为： 0154 Zk Rk kT (13) 0155 当 rcos-r1cos1 0 时，量测矩阵为： 0156 0157 当 rcos-r1cos1 0 时，量测矩阵为： 0158 0159 仿真时间 2500 秒，在相同的仿真条件下，分别对基于传统的笛卡尔坐标系和采用极坐标下的模型进行。

29、，为了更好的说明本发明的优势，在两种方法下分别对航推、基于位置观测信息的 EKF 算法和基于位置和方位角的 EKF 算法进行可比较。 0160 实施效果：图 3 给出了主 AUV，子 AUV 的真实轨迹；图 4 给出基于传统算法时的子 AUV 定位误差曲线，其中： EKF1 表示观测信息为距离时在传统算法下的 EKF 估计轨迹， EKF2 表示观测信息为距离加方位时在传统算法下的 EKF 估计轨迹，图 4 中的子图为基于 EKF2 算法的全局放大图；图 5 给出了基于传统算法时的子 AUV 的位置估计曲线，其中： EKF1 表示观测信息为距离时在传统算法下的。

30、 EKF 估计误差， EKF2 表示观测信息为距离加方位时在传统算法下的 EKF 估计误差，为了更加清楚明了的比较航推、 EKF1 和 EKF2 三种算法的定位结果，对其定位轨迹的末端进行放大，如图 6 所示；图 7 给出了基于本发明的子 AUV 定位误差曲线，其中： EKF1表示观测信息为距离时在本发明下的EKF估计轨迹， EKF2表示观测信息为距离加方位时在本发明下的 EKF 估计轨迹，图 7 中的子图为基于 EKF2 算法的全局放大图；图 8 给出了基于本发明的子 AUV 的位置估计曲线，其中： EKF1 表示观测信息为距离时在本发明下的 EKF 估计误。

31、差， EKF2 表示观测信息为距离加方位时在本发明下的 EKF 估计误差，为了更加清楚明了的比较航推、 EKF1 和 EKF2 三种算法的定位结果，对其定位轨迹的末端进行放大，如图 9 所示。通过图 4 和图 7 可以看出，无论是在传统算法还是在本发明提出的算法下，本发明提出的距离加方位观测法都能够获得较高的精度，同时比较图 4 和图 7 可知，对与曲线运动来说，本发明提能够获得更高的精度。 0161 步骤六：利用李导数对非线性系统进行可观测性分析。 0162 建立非线性的状态方程为： 0163 0164 当 rcos-r1cos1 0 时，建立非线性的量测方程为。

32、：说明书 CN 103968838 A 12 10/11 页 13 0165 0166 可观测性矩阵为： 0167 0168 其中： a1 r-r1cos(-1) ； a1 rr1sin(-1) ； b2 -r2+rr1cos(-1) -ra1； 0169 当 rcos-r1cos1 0 时，建立非线性的量测方程为： 0170 0171 可观测性矩阵为： 0172 0173 其中： a1 r-r1cos(-1) ； a1 rr1sin(-1) ； b2 r 2-rr 1cos(-1) ra1； 0174 通过分析可知，当 r r1, 1，即主 AUV 和子 AUV 的。

33、位置重合时，系统整体不可观；当主 AUV 和子 AUV 之间的相对距离不变时，距离观测子块即 O1不可观；当主 AUV 和子 AUV之间的相对方位角不变时，距离观测子块即O2不可观；当主AUV和子AUV之间的相对距说明书 CN 103968838 A 13 11/11 页 14 离和相对方位角都不变时，整个系统不可观，所以为了提高系统的定位精度，保证系统的可观测性，应该保证主 AUV 和子 AUV 之间的相对距离或相对方位角为变化的。 0175 从以上实施例不难看出，相对于传统的在笛卡尔坐标系下的协同定位算法，本发明提供方法能够获得更高的精度。说明。

34、书 CN 103968838 A 14 1/8 页 15 图 1 图 2 说明书附图 CN 103968838 A 15 2/8 页 16 图 3 说明书附图 CN 103968838 A 16 3/8 页 17 图 4 说明书附图 CN 103968838 A 17 4/8 页 18 图 5 说明书附图 CN 103968838 A 18 5/8 页 19 图 6 说明书附图 CN 103968838 A 19 6/8 页 20 图 7 说明书附图 CN 103968838 A 20 7/8 页 21 图 8 说明书附图 CN 103968838 A 21 8/8 页 22 图 9 说明书附图 CN 103968838 A 22 。

展开阅读全文