阅读说明：本技术 一种接近信标轨迹下的水下机器人定位方法 (Underwater robot positioning method under beacon track approaching condition ) 是由 冀大雄 方文巍 于 2019-11-04 设计创作，主要内容包括：本发明涉及水下机器人定位技术,旨在提供一种接近信标轨迹下的水下机器人定位方法。包括：在保持水下机器人匀速率运动且速率大小已知的情况下,按照设定测量周期计算得到当前位移方向与当前观测距离方向之间的夹角；然后通过强化学习和训练,获得艏向角的调整策略并用于调整艏向角,使水下机器人朝向接近信标的方向运动；在接近信标的过程中,利用扩展卡尔曼滤波位置估计方程计算水下机器人的位置以实现定位。本发明在初始估计误差较大的情况下,相比于圆形轨迹,收敛速度更快。用强化学习的方法来调整艏向角,使水下机器人做接近信标的运动。避免了复杂的调整角度规则的制定。不需要准确的初始位置估计值,运算量小,计算简单,定位稳定可靠。(The invention relates to the underwater robot positioning technology, and aims to provide an underwater robot positioning method close to a beacon track. The method comprises the following steps: under the condition that the underwater robot is kept moving at a uniform speed and the speed is known, calculating according to a set measurement period to obtain an included angle between the current displacement direction and the current observation distance direction; then, obtaining an adjustment strategy of the heading angle through reinforcement learning and training, and adjusting the heading angle to enable the underwater robot to move towards the direction close to the beacon; and in the process of approaching the beacon, calculating the position of the underwater robot by using an extended Kalman filtering position estimation equation to realize positioning. Under the condition of larger initial estimation error, the method has higher convergence speed compared with a circular track. And adjusting the heading angle by using a reinforcement learning method to enable the underwater robot to move close to the beacon. The complex formulation of the angle regulation rule is avoided. The method does not need an accurate initial position estimation value, has small calculation amount, simple calculation and stable and reliable positioning.)

一种接近信标轨迹下的水下机器人定位方法

技术领域

本发明涉及水下机器人定位技术，特别涉及一种接近信标轨迹下的水下机器人定位 方法。

背景技术

水下机器人在水下执行一段时间任务后，累积误差会随着时间增大。这时候，一般需要浮出水面，接收GPS信号重新定位。对于深水作业的水下机器人，浮出水面需要 消耗大量能量，且隐蔽性不好。

单信标测距定位技术由于安装简单、成本低，成为近几年水下定位技术新的发展方 向。但是单个测距信息不能完全确定水下机器人的位置，因此需要设计一定的轨迹和相应的算法来实现定位。圆形轨迹和扩展卡尔曼滤波算法是常用的机动轨迹和滤波算法。 其中，扩展卡尔曼滤波是一个非线性的滤波器，对初始估计值要求较高，当滤波初始估 计值误差较大时，往往会导致收敛速度慢，甚至发散。在初始估计误差较大时，采用圆 形轨迹，收敛速度慢。

发明内容

本发明要解决的技术问题是，克服现有技术中的不足，提供一种接近信标轨迹下的 水下机器人定位方法。该方法是在单信标测距系统下，通过调整艏向角，使水下机器人向信标运动，同时在该轨迹中用扩展卡尔曼滤波进行位置估计，实现定位。

为解决技术问题，本发明的解决方案是：

提供一种接近信标轨迹下的水下机器人定位方法，包括以下步骤：

(1)在保持水下机器人匀速率运动且速率大小已知的情况下，按照设定测量周期，以单个声信标测量水下机器人与信标之间的距离值，以水下机器人自带的罗盘测量艏向角，由前一个时刻的位移距离和前一时刻的观测距离，和当前时刻的观测距离，通过已 知三边长度的余弦定理计算得到当前位移方向与当前观测距离方向之间的夹角；然后通 过强化学习和训练，获得艏向角的调整策略；

(物理定义中匀速运动是指速度大小和方向不变，本发明所述匀速率是指速度大小 不变，但通过调整艏向角的角度改变方向。)

(观测距离方向并不可测量。但通过三边长度已知的余弦定理，可以求出当前位移方向与观测距离方向的夹角。观测距离即由信标测量得到的水下机器人与信标之间的 距离值，观测距离也就是量测距离，由于水声测距带有噪声，会与实际距离存在误差。)

(2)利用强化学习训练得到的策略来调整艏向角，使水下机器人朝向接近信标的方向运动；

(3)在接近信标的过程中，利用扩展卡尔曼滤波位置估计方程计算水下机器人的位置以实现定位。

本发明中，所述步骤(1)具体包括：

首先，建立8行×16列的R表格，表格内容是对应于各情况下的平均奖励值，表 示选择相应动作时对接近信标的有利程度；其中，

行名包括下述八种情况：情况1：观测距离变大，变化量大于一半的位移距离且位移方向与观测距离方向的夹角变大；情况2：观测距离变大，变化量小于等于一半的位 移距离，且位移方向与观测距离方向的夹角变大；情况3：观测距离变小，变化量小于 一半的位移距离，且位移距离与观测距离方向的夹角变大；情况4：观测距离变小，变 化量大于等于一半的位移距离，且位移距离与观测距离方向的夹角变大；情况5：观测 距离变大，变化量大于一半的位移距离，且位移方向与观测距离方向的夹角变小；情况 6：观测距离变大，变化量小于等于一半的位移距离，且位移方向与观测距离方向的夹 角变小；情况7：观测距离变小，变化量小于一半的位移距离，且位移距离与观测距离 方向的夹角变小；情况8：观测距离变小，变化量大于等于一半的位移距离，且位移距 离与观测距离方向的夹角变小；

列名包括下述十六个调整动作：顺时针旋转15°、30°、45°、60°、75°、90°、 105°、120°和逆时针旋转15°、30°、45°、60°、75°、90°、105°、120°；

其次，以经过强化学习训练的R表格作为艏向角的调整策略，具体训练的过程包括：

(1.1)初始化R表格，确定训练次数；

(1.2)设置水下机器人的起始位置后，使其保持匀速率运动；在第一个时刻里，水下机器人朝任意方向移动；

(1.3)按设定时间间隔，测量当前时刻的观测距离r(t)、上一时刻的观测距离r(t-1)、上一时刻的位移距离d(t-1)和用已知三边长度下的余弦定理，计算位移方向 与当前观测距离方向之间的夹角α；如果r(t)＜r(t-1)且夹角α小于45°，保持直行； 否则将水下机器人的行进方向逆时针旋转5°后直行，并测量获得下一个时刻的观测距 离、位移和夹角数据；

(1.4)由连续三个时刻的观测距离和两次位移距离的信息，计算出后两次观测距离 的差值r(t+1)-r(t)和两次夹角的差值α2-α1，并在细分后进行归类；

(1.5)以ε-greedy的探索策略选取设计好的十六个动作；执行动作后，根据观测距离变化情况获得对应奖励值Re；当观测距离减小时Re＝+1，当观测距离不变或变大时 Re＝-1；并更新R表格；更新公式为：

R(s，a)＝(R(s，a)×(N(s，a)-1)+Re)/N(s，a)

s为情况s1～s8中的一个，a为动作a1～a16，具体为得到奖励值Re前，所归类的情况s，和所执行的动作a；

N(s，a)为训练过程中N(S，A)表格中的一项，表示在情况s下执行了动作a的次 数；N(S，A)是一个8x16的表格；更新规则为N(s，a)＝N(s，a)+1；

R(s，a)为训练过程中R(S，A)表格中的一项，表示在情况s状态下执行动作a获 得奖励值的平均值，R(S，A)是一个8x16的表格；

(1.6)当观测距离小于D；本轮训练结束，返回步骤(1.3)；当训练达到设定次 数后结束训练，保留训练结果R表格。

(ε-greedy的探索策略是现有技术，常用于强化学习的一种探索策略，以ε概率随机 选取一个动作(包括使得R值最大的动作)，以1-ε的概率选择R值最大的动作，这样 在训练过程中，相比于随机策略，R值最大的动作被更多的选取，被更充分的训练。)

本发明中，所述步骤(2)包括：

(2.1)按设定时间间隔，测量当前时刻的观测距离r(t+1)、上一时刻的观测距离 r(t)、上一时刻的位移距离d(t)和用已知三边长度下的余弦定理，计算位移方向与当 前观测距离方向之间的夹角α2；如果r(t+1)＜r(t)且夹角α2小于45°，保持直行；否 则将水下机器人的行进方向逆时针旋转5°后直行，并测量获得下一个时刻的观测距离、 位移和夹角数据；

(2.2)调整艏向角后，继续直行，并重新进行测量和计算；重复以上过程，直到水下机器人到信标的距离小于设定的距离值D；然后围绕信标方向随机地运动一段设定时 间T，定位结束。

本发明中，所述步骤(3)包括：

(1)设定扩展卡尔曼滤波位置估计方程为：

P(k+1|k)＝P(k|k)+Q

K(k+1)＝P(k+1|k)·H^T(k+1)·[H(k+1)·P(k+1|k)·H^T(k+1)+R]^-1

P(k+1|k+1)＝[I-K(k+1)·H(k+1)]·P(k+1|k)

上式中，k表示k时刻，X(k)＝[x_k y_k]^T上标“T”表示向量或者矩阵的转置x_k， yk分别是水下机器人在k时刻以信标为原点的东向和北向坐标；字母上带“^”符号的 表示该状态量的预测值或估计值；

方程式中，

是k时刻的状态估计值，

是k+1时刻的状态预测值，

是根据观测值校正后的k+1时刻的状态估计值；

U(k)＝[v·sinθ_k v·cosθ_k]^T，θ_k为k时刻的水下机器人的艏向角，v为水下机器人的航速；

I为单位矩阵P(k|k)是k时刻的误差的协方差矩阵；P(k+1|k)是k+1时刻预测的 误差协方差矩阵；P(k+1|k+1)是量测更新后的协方差矩阵；

V(k+1)为k+1时刻过程噪声，W(k+1)为k+1时刻观测噪声，满足零均值高斯分 布，Q是过程噪声的方差，R是观测噪声的方差；

Z(k+1)＝h(X(k+1))+W(k+1)，是即k+1时刻观测到的带有噪声的水下机器与信 标之间的距离值；

即k+1时刻水下机器人到信标的距离；

即k+1时刻，通过状态预测量计算的距离；

H(k+1)是将

通过泰勒展开，保留一阶项的方式，线性化后得到的雅克比矩阵，

与现有技术相比，本发明的有益效果是：

1.本发明在初始估计误差较大的情况下，相比于圆形轨迹，收敛速度更快。

2.本发明用强化学习的方法来调整艏向角，使水下机器人做接近信标的运动。避免了复杂的调整角度规则的制定。

3.本发明不需要准确的初始位置估计值，运算量小，计算简单，定位稳定可靠。

附图说明

图1是本发明中信标测距和计算夹角的示意图。

图2是用于八种情况分类的测量示意图。

图3是R表格训练的流程图。

具体实施方式

本发明利用单信标测距导航，在水下机器人初始位置未知的情况下，通过使水下机 器人接近信标运动，并用扩展卡尔曼滤波进行位置估计实现定位的。接近信标轨迹下的扩展卡尔曼滤波具有较快的收敛速度，且对初始估计误差大小不敏感。在初始位置未知 的情况下，做接近信标的运动需要一定的艏向角调整策略。本发明通过强化学习的方法 训练得到艏向角调整策略的结果。

本发明的控制策略以R表格形式存储，通过查询表格选择某一情况下较为合适的角 度进行调整。

R表格的结构示例如下：

R(S，A)	a1	a2	a3	...	a16
						s1	R(s1，a1)	R(s1，a2)	R(s1，a3)	...	R(s1，a16)
s2	R(s2，a1)	R(s2，a2)	R(s2，a3)	...	R(s2，a16)
						s3	R(s3，a1)	R(s3，a2)	R(s3，a3)	...	R(s3，a16)
s4	R(s4，a1)	R(s4，a2)	R(s4，a3)	...	R(s4，a16)
						s5	R(s5，a1)	R(s5，a2)	R(s5，a3)	...	R(s5，a16)
s6	R(s6，a1)	R(s6，a2)	R(s6，a3)	...	R(s6，a16)
						s7	R(s7，a1)	R(s7，a2)	R(s7，a3)	...	R(s7，a16)
s8	R(s8，a1)	R(s8，a2)	R(s8，a3)	...	R(s8，a16)

表格中，S为情况集合S＝＜s1，s2，s3，s4....s8＞，A为动作集合A＝＜a1，a2，a3...a16＞。

R表格的训练过程示例如下：

例如假设某次得到的R值训练表格如上所示(为清楚展示内容，只显示了表格的一部分)，有标记的是该行值最大的一格。

那么当训练完成后，根据表格挑选动作，选择值最大的所对应的动作。

当情况归为s1时候，选择的动作就是a8，情况归为s2的时候动作就是a1。

如果是训练的时候，用ε-greedy随机探索策略时候，有标记的格子所对应的动作被选取的概率大于其他动作，更新R(S，A)表格还需要用的N(S，A)表格(为清楚展 示内容，只显示了表格的一部分)。

比如说情况s1下，执行动作a1后，观测距离减小了，则Re＝+1。

更新N(s1，a1)＝N(s1，a1)+1＝8

更新R(s1，a1)＝(R(s1，a1)*(N(s1，a1)-1)+Re)/N(s1，a1)＝(-0.22*(8-1)+1)/8＝-0.0675。

如图2所示，图中只有7个测量的量(即3+2+2)：三个时刻的观测距离值(无方 向)、这三个时刻间的两个位移距离，以及这三个时刻间的两个位移方向。按照距离变 化(变大变小，变大程度与位移/2之间的大小，变小程度与位移/2大小可分为4种)，以 及位移与距离夹角的角度(变大与变小)变化来划分出的4x2＝8种情况，夹角α是通过 三边测距余弦定理测得的。

图2中，后两次观测距离的差值tenzhen1与两次夹角的差值tenzhen2是两个用于情 况划分的标志量，α1是前一时刻位移距离d(t-1)与观测距离r(t-1)的夹角，通过d (t-1)，r(t-1)，r(t)三个距离长度，通过余弦定理计算得到，α2是旋转5°后，位移距离d (t)与观测距离r(t)之间的夹角，d是位移距离值。水下机器人做匀速率运动，则d＝d (t)＝d(t-1)。tezhen1＝r(t+1)-r(t)，tezhen2＝α2-α1；情况分类的依据是，根据tezhen1 大于等于d/2、小于d/2且大于等于0、小于等于0且大于-d/2、小于等于-d/2将所有情 况划分为四种情况。再根据tezhen2是否大于0，对这四种情况继续细分，这样所有情 况一共被分为八种。为了更好的描述八种情况的分类依据，用以下情况分类表示意。

情况分类表

情况集合S	分类依据
		s1	r(t+1)＞r(t)+d(t)/2，α2≥α1
s2	r(t)＜r(t+1)≤r(t)+d(t)/2，α2≥α1
		s3	r(t)-d(t)/2＜r(t+1)≤r(t)，α2≥α1
s4	r(t+1)≤r(t)-d(t)/2，α2≥α1
		s5	r(t+1)＞r(t)+d(t)/2，α2＜α1
s6	r(t)＜r(t+1)≤r(t)+d(t)/2，α2＜α1
		s7	r(t)-d(t)/2＜r(t+1)≤r(t)，α2＜α1
s8	r(t+1)≤r(t)-d(t)/2，α2＜α1

动作集合A里面的a1，a2...a8分别为顺时针旋转15°，30°...120°。a9，a10...a16分别为逆时针旋转15°，30°，...120°。

艏向角调整在t+1时刻调整，调整角度为a1，a2....a16中的一个。训练过程中动作选取策略为ε-greedy策略。ε是一个0到1之间的数，越大说明采取随机动作的概率越 大，这里设置为0.9，使得在训练过程中可以充分探索各个动作。调整前，先生成一个0 到1之间的随机数，若这个随机数大于ε，则随机选取A集合中的一个动作。小于ε则 根据表格，通过情况的分类找到在表格对应的行名，查找到这一行中值最大的那一格， 选取那一格列名所对应的动作。

选取动作后，执行该调整角度的动作，并直行。测得直行后，水下机器人到信标的距离r(t+2)，计算奖励值，用来作为一个判断该情况下采取该动作好坏的一个信号。 目的是使得水下机器人做接近信标的运动。因此当距离变小时，奖励值为+1，距离变大 时，奖励值为-1。即当r(t+2)≤r(t+1)时，奖励值Re＝+1；当r(t+2)≥r(t+1)时， 奖励值Re＝-1。后时刻比前时刻距离小的话奖励值为正。目的是为了接近信标，距离减 小是好的，给正的奖励值。奖励值用于更新R表格，表格的更新使艏向角调整策略也会 改变。更新的公式为R(s，a)＝(R(s，a)×(N(s，a)-1)+Re)/N(s，a).s为情况s1～s8中的 一个，a为动作al～a16，具体为得到奖励值Re前，所归类的情况s，和所执行的动作a。

N(s，a)为N(S，A)表格中的一项，表示训练过程中情况s下执行了动作a的 次数。N(S，A)是一个8x16的表格。更新规则为N(s，a)＝N(s，a)+1。

R(s，a)为R(S，A)表格中的一项，表示情况s下执行动作a获得奖励值的平 均值。R(S，A)是一个8x16的表格。

本发明在动作选取策略上采取了ε-greedy随机策略，在训练过程中，每个动作都会 被充分选择，这样R(s，a)的值会趋于该值的数学期望值。

训练过程如图3流程图所示。

步骤1：初始化R表格为8x16的一张空表格。设置训练次数

步骤2：设置水下机器人的起始位置。

在每个时刻t测得观测距离r(t)，结合上一个时刻的位移d(t-1)，和上一个时 刻的观测距离r(t-1)，如图1所示，进行判断是否要调整角度。若r(t)＜r(t-1)，且夹 角α小于45°，保持直行。

步骤3：不然先逆时针旋转5度，直行，获得下一个时刻的观测距离和位移r(t+1)。

步骤4：如图2所示。由三个时刻的观测距离和两个位移信息，计算特征1和2，由 特征将所有情况归为八类。对每一个情况按ε-greedy策略选取动作。选取动作后，执行 该动作，直行，并获得下一个时刻的距离r(t+2)。

步骤5：距离变小，奖励值Re为+1，距离变大，奖励值Re为-1。获得奖励值Re， 并更新表格。

步骤6：当观测距离小于D。本轮训练结束，返回步骤2。训练达到设定次数，训 练结束。保留训练结果R表格。

扩展卡尔曼滤波位置估计方程：

P(k+1|k)＝P(k|k)+Q

K(k+1)＝P(k+1|k)·H^T(k+1)·[H(k+1)·P(k+1|k)·H^T(k+1)+R]^-1

P(k+1|k+1)＝[I-K(k+1)·H(k+1)]·P(k+1|k)

上式中，k表示k时刻，X(k)＝[x_k y_k]^T上标“T”表示向量或者矩阵的转置x_k， yk分别是水下机器人在k时刻以信标为原点的东向和北向坐标；字母上带“^”符号的 表示该状态量的预测值或估计值。方程中

是k时刻的状态估计值，

是 k+1时刻的状态预测值，

是根据观测值校正后的k+1时刻的状态估计值，

U(k)＝[v·sinθ_k v·cosθ_k]^Tθ_k为k时刻的水下机器人的艏向角，v为水下机器人的航速；

I为单位矩阵P(k|k)是k时刻的误差的协方差矩阵；P(k+1|k)是k+1时刻预测的 误差协方差矩阵；P(k+1|k+1)是量测更新后的协方差矩阵。

V(k+1)为k+1时刻过程噪声，W(k+1)为k+1时刻观测噪声，满足零均值高斯分 布，Q是过程噪声的方差，R是观测噪声的方差；

Z(k+1)＝h(X(k+1))+W(k+1)，是即k+1时刻观测到的带有噪声的水下机器与信 标之间的距离值；

即k+1时刻水下机器人到信标的距离。

即k+1时刻，通过状态预测量计算的距离。

H(k+1)是将通过泰勒展开，保留一阶项的方式，线性化后得到的雅克比矩阵，

根据R表格，对于情况s_i(i＝1，2，3...，8)，比较R(s_i，a1)，R(s_i，a2)，R(s_i， a3)...R(s_i，a16)的值，选则其中最大R值所对应的动作，作为需要调整的角度，使水下 机器人向信标运动。在水下机器人向信标运动过程中，用扩展卡尔曼滤波进行位置估计。

水下机器人匀速率运动，在每个时刻测得到单信标的观测距离。根据速度信息，可获得两个时刻之间的位移信息。在开始时刻，向任意方向运动。由两个时刻测得的到信 标的观测距离r(t-1)、r(t)和这两个时刻之间的位移距离d(t-1)，构成一个三角形。 进行一次判定，若r(t)＜r(t-1)，且三角形中边d(t-1)与边r(t-1)之间夹角小于 45度，则保持直行。否则，在当前时刻逆时针旋转5°并直行；进入下一个时刻，测得 下一个时刻到单信标的观测距离r(t+1)和下一个时刻与当前时刻之间的位移距离d(t)。

由三个时刻的到信标的距离和三个时刻间的两个位移距离信息可以组成两个三角 形。对这两个三角形根据距离变化和夹角变化分为八种情况。对每一种情况，根据R表格，选择一个调整的角度。R表格为强化学习训练的结果。调整角度后，直行，获得下 一个时刻到信标的距离r(t+2)。

重复以上步骤，直到水下机器人到信标的距离小于D。之后在信标附近运动一段时间T，定位结束。

实施例：

对本发明提供的一种接近信标轨迹下的水下机器人定位方法进行仿真分析。

水下机器人实际起始位置为(400m，-300m)，扩展卡尔曼滤波估计的起始位置为(200m，-100m)。水下机器人做2m/s的匀速率运动。测距周期为1s。

过程噪声方差

量测噪声方差V＝[1]m

每个时刻测得观测距离，结合位移信息，判断是否进行角度调整。需要调整角度时， 对情况进行分类，并根据R表格做角度调整。直到观测距离小于2m时，再在信标附近 做一段时间运动。