[发明专利]一种狭水道水面无人平台自主航行航路规划方法及装置

权利要求书

1.一种狭水道水面无人平台自主航行航路规划方法，其特征在于，包括以下步骤：

步骤S101：在狭水道航行前，在航海电子地图上确定作业区域，规划初始的全局航线；

步骤S102：所述无人平台装载所述作业区域及所述初始的全局航线，并建立无人平台二维平面直角坐标系，该坐标系以无人平台的中心为原点，以东向为x轴，以真北向为y轴；所述无人平台将所述初始的全局航线映射到所述无人平台二维平面直角坐标系中；

步骤S103：所述无人平台航行中探测到障碍物，将所述障碍物的坐标转换到所述无人平台二维平面直角坐标系；

步骤S104：对所述无人平台二维平面直角坐标系进行栅格地图建模，基于障碍物及所述初始的全局航线的坐标确定对应的栅格值，构建以所述无人平台为中心的水面水下航行环境局部栅格地图；

步骤S105：基于所述水面水下航行环境局部栅格地图，进行碰撞分析并实时动态调整航线；

步骤S105：基于所述水面水下航行环境局部栅格地图，进行碰撞分析并实时动态调整航线，包括：

在栅格地图中，沿着航行方向查找该航线上是否存在航路点与障碍物重合栅格，若存在航路点与障碍物重合栅格，在当前航线上选取距离该航路点与障碍物重合栅格最近的航线栅格作为新的航路点栅格，替换原来的航路点与障碍物重合栅格，并按照新的航线航行，同时在栅格地图中继续查找该航线上是否存在航线与障碍物重合栅格，若存在航线与障碍物重合栅格，则需重新规划航线来规避障碍物；

所述重新规划航线包括：

在栅格地图中，沿着航行方向查找航线与障碍物相交的第一个交点栅格和最后一个交点栅格，沿航行方向的反方向查找到距离第一个交点栅格1.5倍无人平台长度的栅格，将此栅格作为一个起点栅格；沿航行方向查找到距离最后一个交点栅格1.5倍无人平台长度的栅格，将此栅格也作为一个结束栅格，在栅格地图中使用搜索算法搜索出从起点栅格到结束点栅格之间的若干自由栅格；

所述搜索算法为A*算法改进型，其搜索方向为0°、90°、180°和270°四个方向，从起点栅格开始搜索其四个方向上相邻的栅格，并计算所述相邻的栅格与起点栅格的距离，执行循环操作，即每次都从优先选出距离最小的栅格作为下一个遍历的节点，直到到达结束栅格为止；再从结束栅格开始，反过来按照距离最短规则找到起点栅格，由此就搜索出了由若干自由栅格构成的一条新航线；

搜索算法的公式为：

式中，G(i,j)表示栅格(i，j)到起点栅格的距离；G(i_p,j_p)表示栅格(i，j)的前一栅格到起点栅格的距离；G(i_s,j_s)表示起点栅格(i_s,j_s)的距离值为0；表示当栅格(i_n,j_n)为障碍物栅格、航路点与障碍物重合栅格或航线与障碍物重合栅格时，其到起点栅格(i_s,j_s)的距离为无穷大，即不能通行；

将组成新航线的自由栅格的行列坐标转换成无人平台二维平面直角坐标系中的坐标点；

沿着航行方向，对坐标点两两之间计算斜率，剔除斜率相同的中间坐标点；对余下的坐标点两两之间计算距离，当距离小于1.5倍无人平台长度时剔除该两个坐标点，取此两点的中点作为新的坐标点；将获取的坐标点插入到无人平台二维平面直角坐标系中，构成新的作业航线。

2.如权利要求1所述的狭水道水面无人平台自主航行航路规划方法，其特征在于，所述无人平台将所述初始的全局航线映射到所述无人平台二维平面直角坐标系中，包括：

所述无人平台由组合导航定位系统获取自身的大地坐标位置，结合预先制定的航线中航路点经纬度，通过大地坐标与无人平台二维平面直角坐标系的转换将航线映射到无人平台二维平面直角坐标系中。