CN107589749A - 水下机器人自主定位与节点地图构建方法 - Google Patents
水下机器人自主定位与节点地图构建方法 Download PDFInfo
- Publication number
- CN107589749A CN107589749A CN201710851937.3A CN201710851937A CN107589749A CN 107589749 A CN107589749 A CN 107589749A CN 201710851937 A CN201710851937 A CN 201710851937A CN 107589749 A CN107589749 A CN 107589749A
- Authority
- CN
- China
- Prior art keywords
- mrow
- msub
- mtd
- auv
- msubsup
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明涉及水下机器人导航技术领域，旨在提供一种水下机器人自主定位与节点地图构建方法。本发明提出一种新型的节点定位与水下机器人自主定位方法，即在移动节点只能获得自身与随机节点的位置关系时，对所有节点进行定位，获得节点地图并同时对AUV进行定位。该方法能够确定较大范围内随机布放的节点位置，获得节点地图；并保证AUV的位置坐标误差在一个较小的范围内。本发明只需获得AUV在每个最优估计位置上与未知节点的距离即可，无需获得二者之间的角度关系；相较于传统方法，只利用声呐进行测距，大大降低测量成本。能满足在大范围海域内的工作需求，可保证自身定位误差，以及保证节点地图的节点定位平均误差小。
Description
技术领域
本发明属于水下机器人导航技术领域，设计一种利用水下机器人(AutonomousUnderwater Vehicle，AUV；以下简称AUV)对无线传感器网络节点进行定位并同时确定自身位置的方法。
背景技术
随着海洋的开发，无线传感器网络技术被广泛的应用于海洋生态状况监控、海洋地质信息采集以及航行器定位等领域中。由于布设成本所限，无法对网络中每个节点进行事先预设以精确布置，但实际使用中有要求所有节点位置已知，因此解决无线传感器网络节点定位问题是无线传感器网络应用的关键。
为了解决此问题，研究人员提出包括凸优化、MDS-MAP、DV-Hop等多种传统的无线传感器网络节点定位方法，利用信标间的通信关系构建整体无线传感器网络节点位置关系。这种方法在通信质量高的环境下具有较高的准确度，但由于海洋中的特殊环境导致各种通信干扰，使得节点定位问题面临两难情况：1)各节点间在保证足够的通信质量下需求较高的通信成本；2)由于无线传感器网络节点数量多，无法满足较高的通信成本。这些直接导致这些传统的无线传感器网络节点定位方法无法直接用于海洋中。
目前为了解决上述问题，研究人员提出引入即时定位与地图构建(simultaneouslocalization and mapping，SLAM；以下简称SLAM)方法对节点进行定位。即引入移动节点(如AUV)，采用SLAM方法对移动节点进行即时定位，利用移动节点获取随机节点相对于自身的距离和角度，对无线传感器网络节点进行定位，得到节点地图，再利用地图对移动节点进行后续定位。这种方法从理论上基本是可行的，但是在实际使用情况中移动节点只能获得自身与随机节点的位置关系而无法获得角度关系，直接导致无法确定随机节点位置，导致方法整体不可行。
发明内容
本发明要解决的技术问题是，克服现有技术中的不足，提供一种水下机器人自主定位与节点地图构建方法。
为解决技术问题，本发明的解决方案是：
提供一种水下机器人自主定位与节点地图构建方法，包括以下步骤：
(1)通过水下机器人AUV搭载的传感器获取其自身角度、距离参数，用于计算移动误差小，并作出下一时刻的位置预测：
上式中的递归公式是利用贝叶斯滤波的方式得到机器人在t时刻的位姿的后验概率；式中是t时刻AUV的预测位置，Xt-1是t-1时刻的AUV的真实位置，P(·)表示随机变量的概率密度函数，P(·|·)表示条件概率密度函数(后文P(·|·)均表示条件概率，不再赘述)；
利用粒子滤波器对后验概率进行近似估计，在t时刻时每个粒子位置的后验概率分布满足：
上式中，为第j(j＝1,2,…,m)个粒子在t时刻的位置，Ut-1是t-1时刻的AUV的控制量；
(2)在状态预测方程中加入观测值Zt-1，利用观测量修正AUV的预测值得到更为精确的概率分布并作为提议分布位置：
式中表示在已知t时刻AUV的预测位置与t时刻AUV的观测位置时AUV在t时刻的真实位置的概率分布情况；
(3)由于观测存在误差，其观测位置亦与Xt-1，Ut-1相关，由观测获得目标分布为：
(4)由于每个粒子的位姿不一，则其重要程度也会不一致，因此在对粒子的预测位姿进行更新的同时，对t时刻第j个粒子计算其权重
(5)得到每个粒子在加入观测值后的一个权重之后，根据其权重大小，将其中权重小于0.4的粒子丢弃，并对剩下的粒子进行归一化：
式中是归一化后的例子权重，M是剩下粒子数；
(6)将粒子归一化之后，对粒子进行加权求和得到AUV的最优估计位置：
式中是最优估计位置，第j(j＝1,2,…,m)个粒子在t时刻的位置；
同时利用声呐或超声波测量扫描周围所有未知节点的位置，获得AUV与所有能扫描到的未知节点的距离；
(7)由于在步骤(5)中丢弃了部分粒子，会导致随着计算过程中粒子数目逐渐减少；为保证粒子的有效数目，设定判断变量N，N＝m/2，m为初始粒子总数；当M＜N时则表明粒子数太少，需要对机器人的位置进行重采样；重采样过程为：
粒子数：M→m；粒子分布：即粒子分布不变；
式中表示重采样前M个粒子的分布情况，表示重采样后N个粒子的分布情况；
(8)利用凸优化方法，根据AUV在每个时刻采集的随机信标节点的信息，进行节点位置计算；
假设已知节点ak与未知节点xj之间的测距误差为wkj，测量距离为dij，真实距离为d(ak,xj)；那么就有：
基于实际的测量情况，测量误差利用正态分布表示，即：
式中表示测量方差；
由于每次测量之间是独立的，所以利用每次测量信息得到的X的最大似然函数L表示为：
其似然估计表示为：
所以利用最大似然估计结果建立数学规划模型：
将上述式子松弛为半定规划得到：
maximize α
subject to Z(1:d,1:d)＝Id
Z≥0
其中定义ei∈Rn代表Rn×n中第i列，并且定义对称矩阵0∈Rd是一个全零向量；Id是d阶单位阵；Z(1:d,1:d)表示矩阵Z的左上角d阶主子阵，矩阵之间的点乘代表每个元素的乘积之和A·B＝ΣijAijBij，Z≥0表示对称矩阵Z是半定矩阵；
其中矩阵Z∈R(d+n)×(d+n)能分解成子矩阵：
式中X表示未知节点的坐标位置，Y表示一个矩阵块，无特殊意义；
(9)对水下机器人进行自主定位与节点地图构建：
A、在AUV初始位置周围布放4～6个已知位置的节点，对AUV的初期运动进行导航；将通过步骤(6)获得的AUV的最优估计位置视为已知节点位置ak，即AUV初始位置为a1，下一时刻为a2，以此类推；对依次扫描到的未知节点进行编号并视为xj，即AUV扫描测距到的第一个未知节点x1，下一个为x2，以此类推；在ak所测得AUV距未知节点xj的距离为dkj；在移动中不断检测所剩粒子，当剩余粒子少于初始粒子数50％时按步骤(7)所述内容进行重采样；
B、在初始已知位置节点的定位下，AUV通过一段时间的移动获得足量的关于未知节点xj的距离信息dkj；按照步骤(8)中公式(1-13)进行求解，解得半定矩阵Z；矩阵Z中的X分量即为未知节点位置(X＝[x1,x2,…,xj])，将求得的这部分节点的位置加入到地图中，实现节点地图构建；
C、根据前一步骤B所构建的地图，利用该步骤中所求的节点位置对AUV进行后续导航，实现水下机器人的自主定位；同时利用通信，获得AUV与所有能扫描到的未知节点的位置；
D、重复步骤B、C，直至节点地图构建完成，或者水下机器人到达目标位置。
发明原理描述：
本发明提出一种新型的节点定位与水下机器人自主定位方法，即在移动节点只能获得自身与随机节点的位置关系时，对所有节点进行定位，获得节点地图并同时对AUV进行定位。该方法能够确定较大范围内随机布放的节点位置，获得节点地图；并保证AUV的位置坐标误差在一个较小的范围内。
与现有技术相比，本发明的有益效果在于：
(1)本发明只需获得AUV在每个最优估计位置上与未知节点的距离即可，无需获得二者之间的角度关系；相较于传统方法，只利用声呐进行测距，大大降低测量成本。
(2)本发明考虑到AUV测距所用的声呐具有一定测量范围，不能同时获得AUV自身与大范围海域内所有未知节点的距离；提出利用AUV自主定位与节点地图构建同步，满足在大范围海域内的工作需求。
(3)本发明在AUV移动存在较大误差时仍有较好的AUV自主定位的准确度；通过此方法仿真得到AUV在不大于移动值×20％的移动误差的情况下，仍可保证自身定位误差远小于移动值×1％。
(4)在AUV测量存在较大误差时仍有较好的节点地图的准确度；通过此方法仿真得到AUV在不大于测量值×10％的测量误差下，仍可保证节点地图的节点定位平均误差小于海域面积尺度的1％。
附图说明
图1是本发明t时刻对AUV位置估计示意图；
图2是本发明初始位置上的示意图；
图中圆圈代表随机的节点，三角代表人为布放的节点，AUV的初始位置位于(0.3,0.3)附近。
图3是本发明整体运算的流程图。
具体实施方式
下面以在海域中随机分布一个由若干位置未知的节点所所构成的节点网络，利用AUV进行节点地图构建并同时对AUV进行自主定位的例子来说明本发明的方法。
(1)在AUV初始位置上人为布置4个位置已知的节点，此4个节点呈正方形排列(具体见图2)。AUV通过此4个节点，利用极大似然法求解AUV最初阶段的观测位置。在初始时生成200个粒子，所有粒子的位置与AUV最初阶段的观测位置相同。
由于公式(1-1)～(1-7)给出的是一般的计算形式，具体计算形式与其设计的概率分布和AUV运动有关。下面结合实例说明公式(1-1)～(1-7)中变量、函数及概率密度函数的具体含义：
假定AUV满足的运动方程为：
Xt＝f(Xt-1,Ut-1)+wt
其中Xt＝(x,y)是t时刻AUV的位置；f()代表运动函数；Ut-1＝(v,θ)是t-1时刻AUV的控制量，其中v是速度，θ是前进方向；wt是白噪声，满足：
E[Wt]＝0
式中Wt是t时刻噪声，可以通过AUV自身控制误差得出。
由上文叙述可知，在利用概率密度函数的方式预估AUV的位置时，是用每个粒子去预估机器人所在位置的可能性，同时对每一种可能性进行评估，其具体方式如下：
t时刻每个粒子所代表的AUV的位置变化可以近似分解为x-y方向坐标上的运动：
其中为第j(j＝1,2,…,m)个粒子在t时刻的位置，wxt是t时刻x方向的噪声，wyt是t时刻y方向的噪声。
在每个时刻的位置上利用AUV所携带的声呐或超声波检测其能够检测到的若干已知节点的距离信息。
假定位置已知的节点为A＝{Ai}，Ai＝(xi,yi)，当t＝1时i＝1,…,4。利用AUV获得的AUV距离这些节点的距离分别为di；假定AUV此时位置为利用极大似然法可知：
通过上式解出的作为AUV的观测位置。
观测位置与真实位置之差近似服从一维正态分布，即计算每个粒子的权值
得到每个粒子在加入观测值之后的一个权重之后，根据其权重大小，丢弃一部分其中权重较小的粒子，并对剩下的粒子进行归一化：
将粒子归一化之后，即可对粒子进行加权求和得到AUV的最优估计位置：
在已知AUV的最优估计位置之后，AUV检测所有未知的节点距离信息，所检测到的信息中AUV每个时刻的位置为其中k＝t；i为未知节点代号，假定AUV在每个采集位置上可以特异的识别出此距离信息源于哪一个未知节点；dkj为AUV在ak位置上采集到的AUV与第i个节点间的距离。
在AUV每个位置上检测剩余粒子数，当剩余粒子数目少于初始粒子数一半时，对粒子进行重采样。设现有M个粒子，初始粒子数为m，生成m-M个0～1个随机数，设所构成的随机数列为K。将现有的M个粒子根据权重在0～1上分配空间，根据随机数列K中的随机数落在每个空间中的数量，在每个空间所对应的粒子位置上生成相应数量的新粒子，以此保证粒子分布不变。
(2)AUV采集到对于某部分未知信标位置的足够的信息后或者移动到预设的位置时，将所采集到的距离信息代入凸优化模型进行求解：
maximize α
subject to Z(1:d,1:d)＝Id
Z≥0
把利用模型解得的若干个(r个)节点位置加入到A＝{Ai}中，Ai＝(xi,yi)，i＝1,…,4+r，实现对节点地图数据的更新，并重复(1)(2)过程直至节点地图构建完成或水下机器人到达目标位置。
Claims (1)
1.提供一种水下机器人自主定位与节点地图构建方法，包括以下步骤：
(1)通过水下机器人AUV搭载的传感器获取其自身角度、距离参数，用于计算移动误差小，并作出下一时刻的位置预测：
<mrow>
<mi>P</mi>
<mrow>
<mo>(</mo>
<msub>
<mover>
<mi>X</mi>
<mo>^</mo>
</mover>
<mi>t</mi>
</msub>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mo>&Integral;</mo>
<mi>P</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>X</mi>
<mrow>
<mi>t</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>)</mo>
</mrow>
<mi>P</mi>
<mrow>
<mo>(</mo>
<msub>
<mover>
<mi>X</mi>
<mo>^</mo>
</mover>
<mi>t</mi>
</msub>
<mo>|</mo>
<msub>
<mi>X</mi>
<mrow>
<mi>t</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>)</mo>
</mrow>
<msub>
<mi>dX</mi>
<mrow>
<mi>t</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>1</mn>
<mo>-</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
</mrow>
上式中的递归公式是利用贝叶斯滤波的方式得到机器人在t时刻的位姿的后验概率；式中是t时刻AUV的预测位置，Xt-1是t-1时刻的AUV的真实位置，P(·)表示随机变量的概率密度函数，P(·|·)表示条件概率密度函数；
利用粒子滤波器对后验概率进行近似估计，在t时刻时每个粒子位置的后验概率分布满足：
<mrow>
<msubsup>
<mi>S</mi>
<mi>t</mi>
<mrow>
<mo>&lsqb;</mo>
<mi>j</mi>
<mo>&rsqb;</mo>
</mrow>
</msubsup>
<mo>~</mo>
<mi>P</mi>
<mrow>
<mo>(</mo>
<msub>
<mover>
<mi>X</mi>
<mo>^</mo>
</mover>
<mi>t</mi>
</msub>
<mo>|</mo>
<msub>
<mi>X</mi>
<mrow>
<mi>t</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>,</mo>
<msub>
<mi>U</mi>
<mrow>
<mi>t</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>)</mo>
</mrow>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>1</mn>
<mo>-</mo>
<mn>2</mn>
<mo>)</mo>
</mrow>
</mrow>
上式中，为第j(j＝1,2,…,m)个粒子在t时刻的位置，Ut-1是t-1时刻的AUV的控制量；
(2)在状态预测方程中加入观测值Zt-1，利用观测量修正AUV的预测值得到更为精确的概率分布并作为提议分布位置：
<mrow>
<mi>P</mi>
<mrow>
<mo>(</mo>
<msub>
<mover>
<mi>X</mi>
<mo>~</mo>
</mover>
<mi>t</mi>
</msub>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mi>P</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>X</mi>
<mi>t</mi>
</msub>
<mo>|</mo>
<msub>
<mover>
<mi>X</mi>
<mo>^</mo>
</mover>
<mi>t</mi>
</msub>
<mo>,</mo>
<msub>
<mi>Z</mi>
<mi>t</mi>
</msub>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mi>P</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>X</mi>
<mi>t</mi>
</msub>
<mo>|</mo>
<msub>
<mi>X</mi>
<mrow>
<mi>t</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>,</mo>
<msub>
<mi>U</mi>
<mrow>
<mi>t</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>,</mo>
<msub>
<mi>Z</mi>
<mi>t</mi>
</msub>
<mo>)</mo>
</mrow>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>1</mn>
<mo>-</mo>
<mn>3</mn>
<mo>)</mo>
</mrow>
</mrow>
式中表示在已知t时刻AUV的预测位置与t时刻AUV的观测位置时AUV在t时刻的真实位置的概率分布情况；
(3)由于观测存在误差，其观测位置亦与Xt-1，Ut-1相关，由观测获得目标分布为：
<mrow>
<mi>Q</mi>
<mrow>
<mo>(</mo>
<msub>
<mover>
<mi>X</mi>
<mo>~</mo>
</mover>
<mi>t</mi>
</msub>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mi>Q</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>X</mi>
<mi>t</mi>
</msub>
<mo>|</mo>
<msub>
<mover>
<mi>X</mi>
<mo>^</mo>
</mover>
<mi>t</mi>
</msub>
<mo>,</mo>
<msub>
<mi>Z</mi>
<mi>t</mi>
</msub>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mi>Q</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>X</mi>
<mi>t</mi>
</msub>
<mo>|</mo>
<msub>
<mi>X</mi>
<mrow>
<mi>t</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>,</mo>
<msub>
<mi>U</mi>
<mrow>
<mi>t</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>,</mo>
<msub>
<mi>Z</mi>
<mi>t</mi>
</msub>
<mo>)</mo>
</mrow>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>1</mn>
<mo>-</mo>
<mn>4</mn>
<mo>)</mo>
</mrow>
</mrow>
(4)由于每个粒子的位姿不一，则其重要程度也会不一致，因此在对粒子的预测位姿进行更新的同时，对t时刻第j个粒子计算其权重
(5)得到每个粒子在加入观测值后的一个权重之后，根据其权重大小，将其中权重小于0.4的粒子丢弃，并对剩下的粒子进行归一化：
<mrow>
<msubsup>
<mover>
<mi>w</mi>
<mo>^</mo>
</mover>
<mi>t</mi>
<mrow>
<mo>&lsqb;</mo>
<mi>j</mi>
<mo>&rsqb;</mo>
</mrow>
</msubsup>
<mo>=</mo>
<mfrac>
<msubsup>
<mi>w</mi>
<mi>t</mi>
<mrow>
<mo>&lsqb;</mo>
<mi>j</mi>
<mo>&rsqb;</mo>
</mrow>
</msubsup>
<mrow>
<msubsup>
<mi>&Sigma;</mi>
<mrow>
<mi>j</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
<mi>M</mi>
</msubsup>
<msubsup>
<mi>w</mi>
<mi>t</mi>
<mrow>
<mo>&lsqb;</mo>
<mi>j</mi>
<mo>&rsqb;</mo>
</mrow>
</msubsup>
</mrow>
</mfrac>
<mo>,</mo>
<mrow>
<mo>(</mo>
<mi>M</mi>
<mo>&le;</mo>
<mi>m</mi>
<mo>)</mo>
</mrow>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>1</mn>
<mo>-</mo>
<mn>6</mn>
<mo>)</mo>
</mrow>
</mrow>
式中是归一化后的例子权重，M是剩下粒子数；
(6)将粒子归一化之后，对粒子进行加权求和得到AUV的最优估计位置：
<mrow>
<msub>
<mover>
<mi>X</mi>
<mo>^</mo>
</mover>
<mi>t</mi>
</msub>
<mo>=</mo>
<msubsup>
<mi>&Sigma;</mi>
<mrow>
<mi>j</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
<mi>M</mi>
</msubsup>
<msubsup>
<mover>
<mi>w</mi>
<mo>^</mo>
</mover>
<mi>t</mi>
<mrow>
<mo>&lsqb;</mo>
<mi>j</mi>
<mo>&rsqb;</mo>
</mrow>
</msubsup>
<msubsup>
<mi>S</mi>
<mi>t</mi>
<mrow>
<mo>&lsqb;</mo>
<mi>j</mi>
<mo>&rsqb;</mo>
</mrow>
</msubsup>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>1</mn>
<mo>-</mo>
<mn>7</mn>
<mo>)</mo>
</mrow>
</mrow>
式中是最优估计位置，第j(j＝1,2,…,m)个粒子在t时刻的位置；
同时利用声呐或超声波测量扫描周围所有未知节点的位置，获得AUV与所有能扫描到的未知节点的距离；
(7)由于在步骤(5)中丢弃了部分粒子，会导致随着计算过程中粒子数目逐渐减少；为保证粒子的有效数目，设定判断变量N，N＝m/2，m为初始粒子总数；当M＜N时则表明粒子数太少，需要对机器人的位置进行重采样；重采样过程为：
粒子数：M→m；粒子分布：即粒子分布不变；
式中表示重采样前M个粒子的分布情况，表示重采样后N个粒子的分布情况；
(8)利用凸优化方法，根据AUV在每个时刻采集的随机信标节点的信息，进行节点位置计算；
假设已知节点ak与未知节点xj之间的测距误差为wkj，测量距离为dij，真实距离为d(ak,xj)；那么就有：
||ak-xj||2＝d(ak,xj)，d(ak,xj)+wkj＝dkj (1-8)
基于实际的测量情况，测量误差利用正态分布表示，即：
<mrow>
<msub>
<mi>w</mi>
<mrow>
<mi>k</mi>
<mi>j</mi>
</mrow>
</msub>
<mo>~</mo>
<mi>N</mi>
<mrow>
<mo>(</mo>
<mn>0</mn>
<mo>,</mo>
<msubsup>
<mi>&sigma;</mi>
<mrow>
<mi>k</mi>
<mi>j</mi>
</mrow>
<mn>2</mn>
</msubsup>
<mo>)</mo>
</mrow>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>1</mn>
<mo>-</mo>
<mn>9</mn>
<mo>)</mo>
</mrow>
</mrow>
式中表示测量方差；
由于每次测量之间是独立的，所以利用每次测量信息得到的X的最大似然函数L表示为：
<mrow>
<mi>L</mi>
<mo>=</mo>
<mo>&Pi;</mo>
<mfrac>
<mn>1</mn>
<mrow>
<mn>2</mn>
<msqrt>
<mi>&pi;</mi>
</msqrt>
<msub>
<mi>&sigma;</mi>
<mrow>
<mi>k</mi>
<mi>j</mi>
</mrow>
</msub>
</mrow>
</mfrac>
<msup>
<mi>e</mi>
<mrow>
<mo>(</mo>
<mo>-</mo>
<mfrac>
<mn>1</mn>
<mrow>
<mn>2</mn>
<msubsup>
<mi>&sigma;</mi>
<mrow>
<mi>k</mi>
<mi>j</mi>
</mrow>
<mn>2</mn>
</msubsup>
</mrow>
</mfrac>
<msup>
<mrow>
<mo>(</mo>
<mrow>
<msub>
<mi>d</mi>
<mrow>
<mi>k</mi>
<mi>j</mi>
</mrow>
</msub>
<mo>-</mo>
<mi>d</mi>
<mrow>
<mo>(</mo>
<mrow>
<msub>
<mi>a</mi>
<mi>k</mi>
</msub>
<mo>,</mo>
<msub>
<mi>x</mi>
<mi>j</mi>
</msub>
</mrow>
<mo>)</mo>
</mrow>
</mrow>
<mo>)</mo>
</mrow>
<mn>2</mn>
</msup>
<mo>)</mo>
</mrow>
</msup>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>1</mn>
<mo>-</mo>
<mn>10</mn>
<mo>)</mo>
</mrow>
</mrow>
其似然估计表示为：
<mrow>
<mtable>
<mtr>
<mtd>
<mrow>
<mi>X</mi>
<mo>=</mo>
<mi>arg</mi>
<munder>
<mi>max</mi>
<mi>x</mi>
</munder>
<mi>L</mi>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>=</mo>
<mi>arg</mi>
<munder>
<mi>min</mi>
<mi>x</mi>
</munder>
<mrow>
<mo>(</mo>
<mi>&Sigma;</mi>
<mfrac>
<mn>1</mn>
<msubsup>
<mi>&sigma;</mi>
<mrow>
<mi>k</mi>
<mi>j</mi>
</mrow>
<mn>2</mn>
</msubsup>
</mfrac>
<msup>
<mrow>
<mo>(</mo>
<mrow>
<msub>
<mi>d</mi>
<mrow>
<mi>k</mi>
<mi>j</mi>
</mrow>
</msub>
<mo>-</mo>
<mi>d</mi>
<mrow>
<mo>(</mo>
<mrow>
<msub>
<mi>a</mi>
<mi>k</mi>
</msub>
<mo>,</mo>
<msub>
<mi>x</mi>
<mi>j</mi>
</msub>
</mrow>
<mo>)</mo>
</mrow>
</mrow>
<mo>)</mo>
</mrow>
<mn>2</mn>
</msup>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
</mtable>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>1</mn>
<mo>-</mo>
<mn>11</mn>
<mo>)</mo>
</mrow>
</mrow>
所以利用最大似然估计结果建立数学规划模型：
<mrow>
<mtable>
<mtr>
<mtd>
<mrow>
<mi>max</mi>
<mi>i</mi>
<mi>m</mi>
<mi>i</mi>
<mi>z</mi>
<mi>e</mi>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>&Sigma;</mo>
<mfrac>
<mn>1</mn>
<msubsup>
<mi>&sigma;</mi>
<mrow>
<mi>k</mi>
<mi>j</mi>
</mrow>
<mn>2</mn>
</msubsup>
</mfrac>
<msub>
<mi>&epsiv;</mi>
<mrow>
<mi>k</mi>
<mi>j</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>s</mi>
<mi>u</mi>
<mi>b</mi>
<mi>j</mi>
<mi>e</mi>
<mi>c</mi>
<mi>t</mi>
<mi> </mi>
<mi>t</mi>
<mi>o</mi>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>|</mo>
<mo>|</mo>
<msub>
<mi>a</mi>
<mi>k</mi>
</msub>
<mo>-</mo>
<msub>
<mi>x</mi>
<mi>j</mi>
</msub>
<mo>|</mo>
<msup>
<mo>|</mo>
<mn>2</mn>
</msup>
<mo>-</mo>
<msubsup>
<mi>d</mi>
<mrow>
<mi>k</mi>
<mi>j</mi>
</mrow>
<mn>2</mn>
</msubsup>
<mo>=</mo>
<msub>
<mi>&epsiv;</mi>
<mrow>
<mi>k</mi>
<mi>j</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>1</mn>
<mo>-</mo>
<mn>12</mn>
<mo>)</mo>
</mrow>
</mrow>
将上述式子松弛为半定规划得到：
<mrow>
<mtable>
<mtr>
<mtd>
<mrow>
<mi>max</mi>
<mi>i</mi>
<mi>m</mi>
<mi>i</mi>
<mi>z</mi>
<mi>e</mi>
</mrow>
</mtd>
<mtd>
<mi>&alpha;</mi>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>s</mi>
<mi>u</mi>
<mi>b</mi>
<mi>j</mi>
<mi>e</mi>
<mi>c</mi>
<mi>t</mi>
<mi> </mi>
<mi>t</mi>
<mi>o</mi>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>Z</mi>
<mrow>
<mo>(</mo>
<mn>1</mn>
<mo>:</mo>
<mi>d</mi>
<mo>,</mo>
<mn>1</mn>
<mo>:</mo>
<mi>d</mi>
<mo>)</mo>
</mrow>
</msub>
<mo>=</mo>
<msub>
<mi>I</mi>
<mi>d</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow></mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mover>
<mi>A</mi>
<mo>&OverBar;</mo>
</mover>
<mrow>
<mi>k</mi>
<mi>j</mi>
</mrow>
</msub>
<mo>&CenterDot;</mo>
<mi>Z</mi>
<mo>=</mo>
<msubsup>
<mi>d</mi>
<mrow>
<mi>k</mi>
<mi>j</mi>
</mrow>
<mn>2</mn>
</msubsup>
<mo>+</mo>
<msub>
<mi>&epsiv;</mi>
<mrow>
<mi>k</mi>
<mi>j</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow></mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mo>&Sigma;</mo>
<mrow>
<mo>&ForAll;</mo>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>,</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
<mo>&Element;</mo>
<mover>
<mi>E</mi>
<mo>&OverBar;</mo>
</mover>
</mrow>
</msub>
<msubsup>
<mi>&epsiv;</mi>
<mrow>
<mi>k</mi>
<mi>j</mi>
</mrow>
<mn>2</mn>
</msubsup>
<mo>&le;</mo>
<mi>&alpha;</mi>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow></mrow>
</mtd>
<mtd>
<mrow>
<mi>Z</mi>
<mo>&GreaterEqual;</mo>
<mn>0</mn>
</mrow>
</mtd>
</mtr>
</mtable>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>1</mn>
<mo>-</mo>
<mn>13</mn>
<mo>)</mo>
</mrow>
</mrow>
其中定义ei∈Rn代表Rn×n中第i列，并且定义对称矩阵0∈Rd是一个全零向量；Id是d阶单位阵；Z(1:d,1:d)表示矩阵Z的左上角d阶主子阵，矩阵之间的点乘代表每个元素的乘积之和A·B＝∑ijAijBij，Z≥0表示对称矩阵Z是半定矩阵；
其中矩阵Z∈R(d+n)×(d+n)能分解成子矩阵：
<mrow>
<mi>Z</mi>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mi>I</mi>
</mtd>
<mtd>
<mi>X</mi>
</mtd>
</mtr>
<mtr>
<mtd>
<msup>
<mi>X</mi>
<mi>T</mi>
</msup>
</mtd>
<mtd>
<mi>Y</mi>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>1</mn>
<mo>-</mo>
<mn>14</mn>
<mo>)</mo>
</mrow>
</mrow>
式中X表示未知节点的坐标位置，Y表示一个矩阵块，无特殊意义；
(9)对水下机器人进行自主定位与节点地图构建：
A、在AUV初始位置周围布放4～6个已知位置的节点，对AUV的初期运动进行导航；将通过步骤(6)获得的AUV的最优估计位置视为已知节点位置ak，即AUV初始位置为a1，下一时刻为a2，以此类推；对依次扫描到的未知节点进行编号并视为xj，即AUV扫描测距到的第一个未知节点x1，下一个为x2，以此类推；在ak所测得AUV距未知节点xj的距离为dkj；在移动中不断检测所剩粒子，当剩余粒子少于初始粒子数50％时按步骤(7)所述内容进行重采样；
B、在初始已知位置节点的定位下，AUV通过一段时间的移动获得足量的关于未知节点xj的距离信息dkj；按照步骤(8)中公式(1-13)进行求解，解得半定矩阵Z；矩阵Z中的X分量即为未知节点位置(X＝[x1,x2,…,xj])，将求得的这部分节点的位置加入到地图中，实现节点地图构建；
C、根据前一步骤B所构建的地图，利用该步骤中所求的节点位置对AUV进行后续导航，实现水下机器人的自主定位；同时利用通信，获得AUV与所有能扫描到的未知节点的位置；
D、重复步骤B、C，直至节点地图构建完成，或者水下机器人到达目标位置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710851937.3A CN107589749B (zh) | 2017-09-19 | 2017-09-19 | 水下机器人自主定位与节点地图构建方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710851937.3A CN107589749B (zh) | 2017-09-19 | 2017-09-19 | 水下机器人自主定位与节点地图构建方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN107589749A true CN107589749A (zh) | 2018-01-16 |
| CN107589749B CN107589749B (zh) | 2019-09-17 |
Family
ID=61047465
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201710851937.3A Active CN107589749B (zh) | 2017-09-19 | 2017-09-19 | 水下机器人自主定位与节点地图构建方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN107589749B (zh) |
Cited By (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108337639A (zh) * | 2018-05-02 | 2018-07-27 | 鹿建银 | 一种基于牛顿插值的蒙特卡罗移动节点定位算法 |
| CN108769996A (zh) * | 2018-04-18 | 2018-11-06 | 网易（杭州）网络有限公司 | 用于定位的信标节点布局方法及定位方法 |
| CN109541606A (zh) * | 2018-09-30 | 2019-03-29 | 天津大学 | 单水声信标测距辅助的水下同步定位与构图方法 |
| CN111203849A (zh) * | 2020-01-08 | 2020-05-29 | 浙江大学 | 一种移动机器人抓取作业***及控制方法 |
| CN112163284A (zh) * | 2020-11-02 | 2021-01-01 | 西北工业大学 | 一种水下航行器操稳性因果分析方法 |
| CN112505625A (zh) * | 2021-02-04 | 2021-03-16 | 杭州电子科技大学 | 基于通信规划机制的分布式优化控制的信号源定位方法 |
| CN113804188A (zh) * | 2021-09-06 | 2021-12-17 | 浙江大学 | 一种基于随机分布节点的水下机器人导航方法 |
| CN114765781A (zh) * | 2022-06-14 | 2022-07-19 | 西南科技大学 | 面向应急机器人的通信感知地图构建方法 |
| WO2023184223A1 (zh) * | 2022-03-30 | 2023-10-05 | 中国电子科技集团公司信息科学研究院 | 基于脑启发空间编码机制的机器人自主定位方法和装置 |
Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US4829304A (en) * | 1986-05-20 | 1989-05-09 | Harris Corp. | Map-aided navigation system employing TERCOM-SITAN signal processing |
| CN102402225A (zh) * | 2011-11-23 | 2012-04-04 | 中国科学院自动化研究所 | 一种实现移动机器人同时定位与地图构建的方法 |
| WO2013002067A1 (ja) * | 2011-06-29 | 2013-01-03 | 株式会社日立産機システム | 移動ロボット、及び移動体に搭載される自己位置姿勢推定システム |
| CN103487047A (zh) * | 2013-08-06 | 2014-01-01 | 重庆邮电大学 | 一种基于改进粒子滤波的移动机器人定位方法 |
| CN103644903A (zh) * | 2013-09-17 | 2014-03-19 | 北京工业大学 | 基于分布式边缘无味粒子滤波的同步定位与地图构建方法 |
| CN105606104A (zh) * | 2016-03-17 | 2016-05-25 | 北京工业大学 | 基于航向辅助分布式slam的机器人自主导航方法 |
| CN105865449A (zh) * | 2016-04-01 | 2016-08-17 | 深圳杉川科技有限公司 | 基于激光和视觉的移动机器人的混合定位方法 |
| CN106248077A (zh) * | 2016-07-06 | 2016-12-21 | 北京理工大学 | 一种基于粒子滤波的可见光组合定位***和方法 |
| CN106598052A (zh) * | 2016-12-14 | 2017-04-26 | 南京阿凡达机器人科技有限公司 | 一种基于环境地图的机器人安防巡检方法及其机器人 |
| CN106885574A (zh) * | 2017-02-15 | 2017-06-23 | 北京大学深圳研究生院 | 一种基于重跟踪策略的单目视觉机器人同步定位与地图构建方法 |
-
2017
- 2017-09-19 CN CN201710851937.3A patent/CN107589749B/zh active Active
Patent Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US4829304A (en) * | 1986-05-20 | 1989-05-09 | Harris Corp. | Map-aided navigation system employing TERCOM-SITAN signal processing |
| WO2013002067A1 (ja) * | 2011-06-29 | 2013-01-03 | 株式会社日立産機システム | 移動ロボット、及び移動体に搭載される自己位置姿勢推定システム |
| CN102402225A (zh) * | 2011-11-23 | 2012-04-04 | 中国科学院自动化研究所 | 一种实现移动机器人同时定位与地图构建的方法 |
| CN103487047A (zh) * | 2013-08-06 | 2014-01-01 | 重庆邮电大学 | 一种基于改进粒子滤波的移动机器人定位方法 |
| CN103644903A (zh) * | 2013-09-17 | 2014-03-19 | 北京工业大学 | 基于分布式边缘无味粒子滤波的同步定位与地图构建方法 |
| CN105606104A (zh) * | 2016-03-17 | 2016-05-25 | 北京工业大学 | 基于航向辅助分布式slam的机器人自主导航方法 |
| CN105865449A (zh) * | 2016-04-01 | 2016-08-17 | 深圳杉川科技有限公司 | 基于激光和视觉的移动机器人的混合定位方法 |
| CN106248077A (zh) * | 2016-07-06 | 2016-12-21 | 北京理工大学 | 一种基于粒子滤波的可见光组合定位***和方法 |
| CN106598052A (zh) * | 2016-12-14 | 2017-04-26 | 南京阿凡达机器人科技有限公司 | 一种基于环境地图的机器人安防巡检方法及其机器人 |
| CN106885574A (zh) * | 2017-02-15 | 2017-06-23 | 北京大学深圳研究生院 | 一种基于重跟踪策略的单目视觉机器人同步定位与地图构建方法 |
Non-Patent Citations (1)
| Title |
|---|
| 侯荣波等: "基于ORB-SLAM的室内机器人定位和三维稠密地图构建", 《计算机应用》 * |
Cited By (14)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108769996B (zh) * | 2018-04-18 | 2022-03-25 | 杭州易现先进科技有限公司 | 用于定位的信标节点布局方法及定位方法 |
| CN108769996A (zh) * | 2018-04-18 | 2018-11-06 | 网易（杭州）网络有限公司 | 用于定位的信标节点布局方法及定位方法 |
| CN108337639B (zh) * | 2018-05-02 | 2019-07-26 | 巢湖学院 | 一种基于牛顿插值的蒙特卡罗移动节点定位算法 |
| CN108337639A (zh) * | 2018-05-02 | 2018-07-27 | 鹿建银 | 一种基于牛顿插值的蒙特卡罗移动节点定位算法 |
| CN109541606A (zh) * | 2018-09-30 | 2019-03-29 | 天津大学 | 单水声信标测距辅助的水下同步定位与构图方法 |
| CN111203849A (zh) * | 2020-01-08 | 2020-05-29 | 浙江大学 | 一种移动机器人抓取作业***及控制方法 |
| CN112163284A (zh) * | 2020-11-02 | 2021-01-01 | 西北工业大学 | 一种水下航行器操稳性因果分析方法 |
| CN112163284B (zh) * | 2020-11-02 | 2021-11-05 | 西北工业大学 | 一种水下航行器操稳性因果分析方法 |
| CN112505625A (zh) * | 2021-02-04 | 2021-03-16 | 杭州电子科技大学 | 基于通信规划机制的分布式优化控制的信号源定位方法 |
| CN113804188A (zh) * | 2021-09-06 | 2021-12-17 | 浙江大学 | 一种基于随机分布节点的水下机器人导航方法 |
| CN113804188B (zh) * | 2021-09-06 | 2024-02-09 | 浙江大学 | 一种基于随机分布节点的水下机器人导航方法 |
| WO2023184223A1 (zh) * | 2022-03-30 | 2023-10-05 | 中国电子科技集团公司信息科学研究院 | 基于脑启发空间编码机制的机器人自主定位方法和装置 |
| CN114765781A (zh) * | 2022-06-14 | 2022-07-19 | 西南科技大学 | 面向应急机器人的通信感知地图构建方法 |
| CN114765781B (zh) * | 2022-06-14 | 2022-08-19 | 西南科技大学 | 面向应急机器人的通信感知地图构建方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN107589749B (zh) | 2019-09-17 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107589749A (zh) | 水下机器人自主定位与节点地图构建方法 | |
| CN109540151B (zh) | 一种基于强化学习的auv三维路径规划方法 | |
| Cao et al. | Target search control of AUV in underwater environment with deep reinforcement learning | |
| CN111240319B (zh) | 室外多机器人协同作业***及其方法 | |
| CN101413806B (zh) | 一种实时数据融合的移动机器人栅格地图创建方法 | |
| CN107462892B (zh) | 基于多超声传感器的移动机器人同步定位与地图构建方法 | |
| CN102359784B (zh) | 一种室内移动机器人自主导航避障***及方法 | |
| CN102915465B (zh) | 一种基于移动生物刺激神经网络的多机器人联合编队方法 | |
| WO2017028653A1 (zh) | 一种移动机器人室内自建地图的方法和*** | |
| JP7339029B2 (ja) | モーション認識モデルを用いた自体運動推定装置及び方法並びにモーション認識モデルトレーニング装置及び方法 | |
| CN202216696U (zh) | 基于信息融合的煤矿救灾机器人导航装置 | |
| CN102915039A (zh) | 一种仿动物空间认知的多机器人联合目标搜寻方法 | |
| CN108829918B (zh) | 面向海洋现象的智能浮标组网仿真方法及*** | |
| CN110095120A (zh) | 自治水下航行器在海洋环流下的生物启发自组织映射路径规划方法 | |
| Xiong et al. | A scheme on indoor tracking of ship dynamic positioning based on distributed multi-sensor data fusion | |
| CN103984981A (zh) | 基于高斯过程模型的建筑物环境传感器测点优化方法 | |
| CN113238232B (zh) | 面向海洋静态目标的自主水下航行器***目标搜索方法 | |
| Lan et al. | Improved RRT algorithms to solve path planning of multi-glider in time-varying ocean currents | |
| CN104049639A (zh) | 一种基于支持向量回归机的无人艇抗浪涌控制装置和方法 | |
| CN112857370A (zh) | 一种基于时序信息建模的机器人无地图导航方法 | |
| Yan et al. | Real-world learning control for autonomous exploration of a biomimetic robotic shark | |
| CN117669008B (zh) | 一种基于深度学习的地基沉降预测方法及*** | |
| Yu et al. | A deep-learning-based strategy for kidnapped robot problem in similar indoor environment | |
| Toledo et al. | Map building with ultrasonic sensors of indoor environments using neural networks | |
| Wang et al. | Wireless sensor networks for underwater localization: A survey |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |