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

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