[发明专利]一种AUV海底地形匹配的动态路径规划方法

摘要

本发明公开了一种AUV海底地形匹配的动态路径规划方法，属于水下航行器导航技术领域。包括环境建模，离线路径规划和在线路径重规划。环境模型通过地形信息和地形来源置信度构建，其中地形特征用地形标准差表示。离线路径规划基于快速搜索随机树算法，改进选择新节点和父节点算法，通过轮盘赌和马氏距离来处理地形信息，以避免低地形信息区域。在AUV沿着离线路径航行，检测到地形信息改变时，在线重规划通过判断地形改变，寻找局部导航最优节点和重优化全局路径等方法实现了对地形变化的判断并保证了导航精度。本发明适用于长航程、高精度的AUV路径规划，特别是当地形信息改变时，通过实时修改路径以保证导航精度和路径距离。

主权项

1.一种AUV海底地形匹配的动态路径规划方法，其特征在于，包含以下步骤：步骤一：构建海底的环境模型；对于海底的物理环境地图，首先将其网格化，从而变成对于点(i,j)的高度为h(i,j)的网格地图；然后提取每个点的导航能力，这里用地形标准差σT来描述导航能力，表示为：其中m和n表示网格地图的尺寸大小，是平均高度；若网格点(i,j)的地形越复杂，则改点σ_T的值越大，即该点越适合用来匹配导航，因此，用地形标准差σ_T来表示网格点的导航能力NAV；步骤二：进行离线RRT*路径规划，首先用起始点zinit初始化随机树T＝(V,E)，这里V表示随机树中的节点，E表示连接这些节点的边缘；步骤三：利用采样函数，在自由空间中随机产生一个节点zrand；然后在随机树中找到距离节点zrand最近的节点znearest，暂时作为新节点znew的父节点；步骤四：根据AUV的运动能力u，在zrand和znearest的连线上，找到新节点znew，并计算从znearest运动到znew的成本和从起始节点zinit运动到znew的成本；寻找新节点znew时，考虑其导航能力，本发明改进了轮盘赌中最小圆和最大椭圆的大小，其中最小圆的半径取决于AUV的运动特点，即半径为：其中，Rs和δ表示旋转半径和垂直舵角，L表示AUV的长度，m′表示AUV的无量纲质量，N′v,N′r,N′δ,Y′v,Y′r,Y′δ表示AUV的各种水动力系数；在TAN中，地形匹配结果的置信度也会影响其值；因此，置信度由地形匹配过程中测量噪声的协方差矩阵表示，节点间的距离用马氏距离表示；测量噪声的协方差矩阵算用Cramer‑Rao下界,即CRLB,来估计；当AUV在节点znew地形匹配过程中，测量公式为：h＝g(x)+w                                 (3)其中，h代表地形测量的高度，x代表AUV的位置和姿态，g(x)代表预测的地形，w表示测量噪声与分布N(0,1)；然后计算地形匹配概率函数：其中，N表示测量点个数，H(i)表示匹配结果，σe表示由每个测量点的方差组成的对角矩阵，并且gi(x)测量点i的预测地形；因此费雪信息J表示为Cramer‑Rao下界表示为：因此，令x＝[a,b]，其中a和b表示AUV在节点znew的位置，则使用CRLB来估计协方差矩阵的下界为：对于最大椭圆，根据公式(7)，令：结合公式(8)，则节点znew和节点znearest之间的马氏距离为：其中，a1和b1表示节点znearest的位置，最大距离是s，最大椭圆的非标准方程为：在确定了搜索区域后，利用遗传算法找到新节点znew；为了找到在搜索区域中具有最佳导航能力的新节点znew，需要改进遗传算法的适应度函数；对于在搜索区域中具有地形标准差σT的点(r,β)，其适应度函数表示为σT·cosβ，这样新节点znew沿着znearest和zrand之间的连接移动会更好；步骤五：如果znearest和znew之间有障碍物，则删掉新节点znew，返回步骤三，重新寻找新节点；反之，将znew加入到随机树T中，执行步骤六；步骤六：以r为半径，以znew为圆心，圈出所有距离znew小于r的节点{zi}，并计算从起点，沿着随机树的路径，分别经过这些节点到达znew的总成本，从而找到总成本最小时的节点zi，将其作为znew的父节点，并连接zi和znew；若半径r的圆形区域中没有节点，则取znearest作为znew的父节点；改进RRT*中通常计算马氏距离作为成本；由公式(9)，znew和圆形区域内(半径为r){zi}节点之间的马氏距离为从而选出马氏距离D(xi,xnew)最小的节点zi，作为znew的父节点；步骤七：如果znew距离目标节点zgoal小于一定的值，则将zgoal作为新节点返回步骤六，并将zgoal加入随机树中，则离线全局路线规划算法完成，进行下一步；否则，返回步骤三；步骤八：若某节点的地形发生改变，则进行在线路径重规划；首先AUV从起点，即节点zinit，开始执行任务，沿着RRT*计划的离线路径航行；初始化为变量j为1，其表示AUV所在的节点位置；步骤九：如果节点zj是目标节点zgoal，则则执行步骤十五，AUV到达终点和在线重规划方法结束，否则转到步骤十；步骤十：当AUV到达节点zj时，根据测量的测深数据与先验图匹配的可能性，确定节点zj上是否存在地形变化；如果存在地形变化，请转至步骤十一，否则转至步骤十四；具体为：根据公式(3)中的地形匹配概率函数，似然函数L表示为：并且，似然L由决定以便选择阈值；如果匹配网格中似然L的最大值低于阈值，则地形会发生变化；步骤十一：搜索节点zj周围具有最佳地形导航能力的节点zp；具体为：当节点zj地形改变后，设计搜索区域为与节点zj+1连线对称的半圆，其半径取决于AUV的运动能力，并利用遗传算法的轮盘赌方法，在搜索区域内寻找具有最佳导航能力的新节点zp；步骤十二：通过RRT*找到局部最优路径连接节点zp和节点zj+1；步骤十三：将局部路径插入节点zj和节点zj+1之间的全局路径，并通过重新优化新路径获得最终路径；具体为：判断将局部路径插入节点zj和节点zj+1之间的全局路径是不是最优路径，若果不是最优路径，将局部路径插入节点zj和节点zj+2之间的全局路径，从而找到最优的全局路径；步骤十四：变量j变为j+1，然后转到步骤九；步骤十五：程序结束。