买专利卖专利找龙图腾，真高效！ 查专利查商标用IPTOP,全免费！专利年费监控用IP管家,真方便！

申请/专利权人：哈尔滨工程大学

摘要：本发明公开了一种海底地形等深线辅助导航路径规划方法，属于水下地形辅助导航与定位领域。本发明通过研究等深线地形匹配特性，从等深线先验海图中提取等深线疏密及等深线平滑度作为海底地形适配性，并根据海底地形适配性完成地形适配区建模；采用双向RRT*进行全局路径规划，在随机树扩展中，根据所处位置的等深线法线方向进行随机树节点筛选；在计算花费中，将AUV所处位置的地形适配性作为奖励函数考虑在计算范围内，寻求最优的规划路径；最后对规划好的路径利用贝塞尔曲线进行优化，满足AUV实际航行中的性能。

主权项：1.一种海底地形等深线辅助导航路径规划方法，其特征在于：具体步骤如下：步骤1：根据等深线匹配特性进行地形适配性划分；步骤1.1：根据先验海图生成等深线图，根据网格划分标准，将等深线图划分为M个等深线子地图；步骤1.2：进行环境建模，根据每个等深线子地图中的等深线密度与等深线平滑度进行环境建模；步骤1.3：针对等深线密度，通过计算每个子地图区域内的等深线数量nc，利用深线数量来表示等深线密度；等深线平滑度是将等深线子地图中每条等深线离散得到等深线点，对等深线进行平滑处理，将得到的等深线进行离散得到平滑后的等深线点，计算等深线路径点距离平滑曲线的最小值并取平均值作为该子地图内的地形平滑度ξ，其计算公式为： 式中，xc,yc为等深线离散得到的等深线点，xo,yo为对等深线优化后得到的离散等深线点，N为子地图中离散等深线点的数量；步骤1.4：将子地图区域内的等深线进行地形适配性划分，将等深线数量、等深线平滑度划分为适配、一般、不适配三个等级；步骤2：根据步骤1得到的地形适配性划分与匹配特性完成地形适配区建模；其中等深线疏密为地形适配性主影响因素，等深线平滑度为次影响因素，综合考虑每个等深线子地图中的等深线疏密与等深线平滑度，完成先验海图地形适配区建模；步骤3：利用双向RRT*进行全局路径规划；步骤3.1：进行双向RRT*全局路径规划，根据路径的起点zinit与终点zgoal初始化两棵随机树分别为Ta和Tb，其中Ta以zgoal为目标进行扩展，Tb以zinit为目标进行扩展；步骤3.2：在路径搜索过程中两棵随机树同时进行扩展，在整个搜索空间内采用随机采样生成一个节点zrand，从树的节点中寻找到距离zrand最近的节点znear，并从节点znear向zrand延长步长p后的得到新节点znew，并对新节点进行避碰检测，合格后作为新节点加入随机树中，反之则删除；在进行地形匹配定位中，针对等深线地形辅助导航匹配特性，需要考虑AUV进入地形区域的方向，进行有效节点筛选，其中从znear到znew的方向为路径规划得到的AUV航行，需要寻找到与此位置等深线法线方向角度差在ρ内的随机树节点，保留在角度差范围内的随机树节点，其余节点则删除，在保证匹配效果基础上，提高路径规划的效率；步骤3.3：计算随机树节点中距离znew最近的节点为zneighbor，计算从zinit到zneighbor的花费，以及从zneighbor到znew的欧式距离，并将距离随机树节点znew最近的节点zneighbor设为其父节点zparent；在进行地形辅助导航中，AUV所处位置的地形适配性会对定位结果产生影响，根据每个节点所在的位置x*,y*以及建立的地形适配性模型，得到节点位置的地形适配性，并将地形适配性设定为奖励函数，将奖励函数考虑到路径成本中，实现地形辅助导航中的最优路径；步骤3.4：在双向RRT*算法中，根据路径代价最小进行父节点重选，计算从zinit到附近相连节点znear的花费以及znear到znew的欧式距离，同时考虑节点的奖励函数，寻找到成本最小的节点znear作为znew的父节点zparent；步骤3.5：根据寻找到的随机数父节点进行节点重新布线，计算从zinit到节点znew的花费以及znear到znew的欧式距离，同时考虑节点的奖励函数，通过节点重新布线保证所有节点到初始点花费最小；步骤3.6：两棵随机树继续扩展，直到两个树生成的znew节点之间的距离小于步长阈值时，将两棵随机树Ta和Tb连通；步骤3.7：进行新的随机树节点生成，直至达到设定的迭代次数阈值；步骤4：对规划后的路径进行轨迹曲线优化；在得到全局规划的路径后，利用贝塞尔曲线进行最终路径优化，保证得到的路径能够满足AUV的实际性能要求。

全文数据：

权利要求：

百度查询： 哈尔滨工程大学 一种海底地形等深线辅助导航路径规划方法