一种AUV海底地形匹配的动态路径规划方法.pdf

(19)中华人民共和国国家知识产权局(12)发明专利申请(10)申请公布号CN108871351A(43)申请公布日2018.11.23(21)申请号201810870169.0(22)申请日2018.08.02(71)申请人哈尔滨工程大学地址150001黑龙江省哈尔滨市南岗区南通大街145号哈尔滨工程大学科技处知识产权办公室(72)发明人李晔徐硕马腾丛正贡雨森王汝鹏(51)Int.Cl.G01C21/30(2006.01)G01C21/32(2006.01)G06Q10/04(2012.01)权利要求书3页说明书8页附图3页(54)发明名称一种AUV海底地形匹配的动态路径规划方法(57)摘要本发明公开了一种AUV海底地形匹配的动态路径规划方法，属于水下航行器导航技术领域。包括环境建模，离线路径规划和在线路径重规划。环境模型通过地形信息和地形来源置信度构建，其中地形特征用地形标准差表示。离线路径规划基于快速搜索随机树算法，改进选择新节点和父节点算法，通过轮盘赌和马氏距离来处理地形信息，以避免低地形信息区域。在AUV沿着离线路径航行，检测到地形信息改变时，在线重规划通过判断地形改变，寻找局部导航最优节点和重优化全局路径等方法实现了对地形变化的判断并保证了导航精度。本发明适用于长航程、高精度的AUV路径规划，特别是当地形信息改变时，通过实时修改路径以保证导航精度和路径距离。CN108871351ACN108871351A权利要求书1/3页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下界表示为：2CN108871351A权利要求书2/3页因此，令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的总成本，从而找到总成本最小时的节点