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

权利要求书

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，直至节点地图构建完成，或者水下机器人到达目标位置。