[发明专利]基于贝塞尔拟合的UUV海底地形跟踪路径滚动生成方法

权利要求书

1.一种基于贝塞尔拟合的UUV海底地形跟踪路径滚动生成方法，其特征是，包括如下步骤：

步骤一：初始化参数设置：设置UUV海底地形跟踪的期望航行高度h_p(单位：米)，设置UUV距海底的安全高度h_safe(单位：米)，设置垂直面的XEZ固定坐标系原点的经纬度lon₀(单位：度)和lat₀(单位：度)，设置任务终点经纬度lon_E(单位：度)和lat_E(单位：度)，设置局部跟踪路径离散精度k，设置最大上浮纵倾角θ_max(单位：度)，设置局部跟踪路径最大跟踪时间T_max(单位：秒)；

步骤二：UUV地形跟踪开始，进行信息测量：UUV由配置的惯性导航仪测量得到当前的经纬度lon_uuv、lat_uuv(单位：度)，由UUV配置的深度计测量得到当前深度z_uuv(单位：米)，由配置的姿态传感器测量得到当前纵倾角θ(单位：度)，由配置的测高声纳测量得到当前距离海底的高度h_det(单位：米)，由配置的前视多波束声呐测量得到地形探测点集

步骤三：解算UUV在固定坐标系下的垂直面坐标(x_uuv,z_uuv)；

步骤四：由地形探测点集解算出局部地形点集A；

步骤五：由局部地形点集A进行定高仿射，解算出局部地形仿射点集P；

步骤六：由局部地形仿射点集P进行贝塞尔拟合生成局部跟踪路径L(t)；

步骤七：将局部跟踪路径L(t)进行离散化，生成UUV局部跟踪路径点集Π；

步骤八：UUV采用通用的视线路径跟踪控制方法顺序地对生成的局部跟踪路径点集Π进行局部跟踪；

步骤九：UUV判断是否有与海底地形发生碰撞的风险，如果有碰撞风险转步骤十，否则转步骤十一；

步骤十：UUV进行避碰响应：此时UUV停止路径跟踪控制，以最大上浮纵倾角θ_max(单位：度)紧急上浮避碰，然后转步骤九；

步骤十一：UUV判断是否到达任务终点：判断方法为UUV当前的经纬度是否到达任务终点经纬度lon_E和lat_E，如果到达任务终点转步骤十四，否则转步骤十二；

步骤十二：UUV判断是否完成局部跟踪路径跟踪：判断方法为UUV到达局部跟踪路径点集的最后一个路径点，

如果完成局部跟踪路径跟踪转步骤二，否则转步骤十三；

步骤十三：UUV判断局部跟踪路径的跟踪是否超时：判断方法为UUV的局部跟踪路径的跟踪时间超过规定最大跟踪时间T_max，如果超时转步骤二，否则转步骤八；

步骤十四：UUV海底地形跟踪结束。

2.根据权利要求1所述的基于贝塞尔拟合的UUV海底地形跟踪路径滚动生成方法，其特征是，所述步骤三中UUV在固定坐标系下的垂直面坐标(x_uuv,z_uuv)的解算方法为：

x_uuv＝(lon_uuv-lon₀)·1852·60，其中x_uuv的单位为米；

z_uuv就是深度计测量得到的UUV当前深度，单位为米。