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

权利要求书

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

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

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

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

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

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

步骤六：由局部地形仿射点集P进行多项式拟合生成局部跟踪路径z(x)；

步骤七：UUV采用通用的视线路径跟踪控制方法对生成的局部跟踪路径z(x)进行局部跟踪；

步骤八：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当前深度，单位为米。

3.根据权利要求1所述的基于多项式拟合的UUV海底地形跟踪路径滚动生成方法，还包括：所述步骤四中地形探测点集解算出局部地形点集A的方法为：

步骤四(A)：由地形探测点集提取有效地形探测点集提取方法为：将地形探测点集中测得的UUV与海底地形之间的距离d_i≠0的有效探测波束得到的地形点提取出来，得到有效地形探测点集其中地形探测点集为：(i＝1,2,…n)，表示前视波束声呐第i条波束的地形探测点信息，n表示表示前视多波束声呐波束数目，有效地形探测点集(j＝1,2,…m,j≤i,m≤n)，m表示有效探测波束得到的地形点的数目；

步骤四(B)：由有效地形探测点集解算局部地形点集A，解算方法为：

A＝{A₁,A₂,…A_m}，其中A_j＝[x_j,z_j]，且有