[发明专利]水下机器人自主定位与节点地图构建方法

摘要

本发明涉及水下机器人导航技术领域，旨在提供一种水下机器人自主定位与节点地图构建方法。本发明提出一种新型的节点定位与水下机器人自主定位方法，即在移动节点只能获得自身与随机节点的位置关系时，对所有节点进行定位，获得节点地图并同时对AUV进行定位。该方法能够确定较大范围内随机布放的节点位置，获得节点地图；并保证AUV的位置坐标误差在一个较小的范围内。本发明只需获得AUV在每个最优估计位置上与未知节点的距离即可，无需获得二者之间的角度关系；相较于传统方法，只利用声呐进行测距，大大降低测量成本。能满足在大范围海域内的工作需求，可保证自身定位误差，以及保证节点地图的节点定位平均误差小。

主权项

1.提供一种水下机器人自主定位与节点地图构建方法，包括以下步骤：(1)通过水下机器人AUV搭载的传感器获取其自身角度、距离参数，用于计算移动误差，并作出下一时刻的位置预测：上式中的递归公式是利用贝叶斯滤波的方式得到机器人在t时刻的位姿的后验概率；式中是t时刻AUV的预测位置，X_t‑1是t‑1时刻的AUV的真实位置，P(·)表示随机变量的概率密度函数，P(·|·)表示条件概率密度函数；利用粒子滤波器对后验概率进行近似估计，在t时刻时每个粒子位置的后验概率分布满足：上式中，为第j(j＝1，2，…，m)个粒子在t时刻的位置，U_t‑1是t‑1时刻的AUV的控制量；(2)在状态预测方程中加入观测值Z_t‑1，利用观测量修正AUV的预测值得到更为精确的概率分布并作为提议分布位置：式中表示在已知t时刻AUV的预测位置与t时刻AUV的观测位置时AUV在t时刻的真实位置的概率分布情况；(3)由于观测存在误差，其观测位置亦与X_t‑1，U_t‑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在每个时刻采集的随机信标节点的信息，进行节点位置计算；假设已知节点a_k与未知节点x_j之间的测距误差为w_kj，测量距离为d_kj，真实距离为d(a_k，x_j)；那么就有：||a_k‑x_j||²＝d(a_k，x_j)，d(a_k，x_j)+w_kj＝d_kj    (1‑8)基于实际的测量情况，测量误差利用正态分布表示，即：式中表示测量方差；由于每次测量之间是独立的，所以利用每次测量信息得到的X的最大似然函数L表示为：其似然估计表示为：所以利用最大似然估计结果建立数学规划模型：将上述式子松弛为半定规划得到：其中定义e_i∈Rⁿ代表R^n×n中第i列，并且定义对称矩阵0∈R^d是一个全零向量；I_d是d阶单位阵；Z_{(1：d，1：d)}表示矩阵Z的左上角d阶主子阵，矩阵之间的点乘代表每个元素的乘积之和A·B＝∑_ijA_ijB_ij，Z≥0表示对称矩阵Z是半定矩阵；其中矩阵Z∈R^(d+n)×(d+n)能分解成子矩阵：式中X表示未知节点的坐标位置，Y表示一个矩阵块，无特殊意义；(9)对水下机器人进行自主定位与节点地图构建：A、在AUV初始位置周围布放4～6个已知位置的节点，对AUV的初期运动进行导航；将通过步骤(6)获得的AUV的最优估计位置视为已知节点位置a_k，即AUV初始位置为a₁，下一时刻为a₂，以此类推；对依次扫描到的未知节点进行编号并视为x_j，即AUV扫描测距到的第一个未知节点x₁，下一个为x₂，以此类推；在a_k所测得AUV距未知节点x_j的距离为d_kj；在移动中不断检测所剩粒子，当剩余粒子少于初始粒子数50％时按步骤(7)所述内容进行重采样；B、在初始已知位置节点的定位下，AUV通过一段时间的移动获得足量的关于未知节点x_j的距离信息d_kj；按照步骤(8)中公式(1‑13)进行求解，解得半定矩阵Z；矩阵Z中的X分量即为未知节点位置，即X＝[x₁，x₂，…，x_j]，将求得的这部分节点的位置加入到地图中，实现节点地图构建；C、根据前一步骤B所构建的地图，利用该步骤中所求的节点位置对AUV进行后续导航，实现水下机器人的自主定位；同时利用通信，获得AUV与所有能扫描到的未知节点的位置；D、重复步骤B、C，直至节点地图构建完成，或者水下机器人到达目标位置。