[发明专利]自主式水下移动平台北斗定位与巡航优化方法

权利要求书

1.一种自主式水下移动平台北斗定位与巡航优化方法，其特征在于，包含自主式水下移动平台绝对位置标定和自主式水下移动平台相对位置定位系统优化校正两大步骤；

所述绝对位置标定步骤中基于北斗定位浮标计算获取自主式水下移动平台初始化参数，所述初始化参数包括自主式水下移动平台空间坐标、北斗精确授时；

所述自主式水下移动平台相对位置定位系统优化校正步骤中基于粒子群优化自主式水下移动平台航迹路径，校正相对定位系统误差；

自主式水下移动平台绝对位置标定包括以下步骤：

(1)水下控制模块无线接收水面控制平台操作指令，从数据存储器中读取定位指令，向水面4个北斗智能浮标同步发送定位请求；

(2)北斗定位接收机接收北斗卫星定位数据，解析定位数据；

(3)信号处理电路对卫星信号做时延和误差估计，计算出浮标准确位置参数；

(4)北斗智能浮标通过超声波向水下控制模块发送各浮标位置参数和实时时间参数；

(5)超声波接收器接收水面北斗浮标的数据，使用渡越时间法计算出超声波信号在水中传输时间t_i，进行机器人水下空间位置参数计算；

(6)北斗智能浮标获取北斗卫星定位数据并进行坐标系转换，将大地坐标系转换为空间直角坐标系，得到定位浮标各自空间坐标(o_i,p_i,q_i)；

(7)4个浮标中选择1号浮标作为参考浮标，建立方程组：

其中v_s为水介质中速度；

(8)由已知条件，根据方程式组求解出自主式水下移动平台空间坐标(o,p,q,)和水介质中速度v_s；

实现自主式水下移动平台相对位置定位系统校正包括以下步骤：

(1)建立自主式水下移动平台运动模型：

其中状态向量s_t是标识机器人在t时刻的位置参数，表示机器人运动方位角，ω_t表示机器人角加速度，u_t表示机器人外部控制输入参数，δ_t表示机器人在水下移动过程中的随机噪声，h是状态转换模型；

(2)建立路径状态预测方程：

Δt为机器人移动时间，l_t-1,t为t-1时刻到t时刻的航行距离，δ_m和δ_n分别为水平和垂直运动方向随机噪声；

(3)自主式水下移动平台接收外部控制指令，执行循环巡航任务，在直接坐标系中，自主式水下移动平台的出发点为S，自主式水下移动平台区域循环巡航路线为10m x10m的正方形，并在四个转角处转90°，巡航路径分成4段直线，从出发点S开始，下面的拐弯点依次为S1、S2和S3；

(4)对S-S1、S1-S2、S2-S3和S3-S四段巡航路径进行N等分，S-S1路径点集合为A＝{A₀,A₁,A₂,...,A_n,A_n+1}，其余三段的集合分别为B＝{B₀,B₁,B₂,...,B_n,B_n+1}、C＝{C₀,C₁,C₂,...,C_n,C_n+1}和D＝{D₀,D₁,D₂,...,D_n,D_n+1}，一系列的路径集合点构成了巡航路径，一个粒子代表一条路径，每一段粒子的向量就是指巡航路径的集合点距离该段起点的直线距离，一段路径中N个粒子组成一个群体；

(4)建立粒子群方程和适应度函数，对粒子参数初始化，设置学习因子c₁和c₂，将惯性权重w分配给每粒粒子，根据t-1时刻的粒子位置参数z_t-1，预测t时刻的粒子位置z_t；

(5)根据北斗绝对位置标定参数(o_i,p_i,q_i)和上一步预测粒子状态参数z_t，由贝叶斯法则，对每个粒子的权值进行更新；

(6)通过粒子群方程迭代运算，计算出巡航路径群体中各粒子的最小的适应度函数值f，比较适应值，找到路径点最优解，更新个体和全局最优值；

(7)根据最优解，比较粒子预测位置参数与最优点位置参数，两者距离若大于设定的阈值k_ij，对状态预测方程中向量进行校正，代入式5、式6，调整运动方位角和角加速度ω_t，经多次迭代后，若小于设定阈值k_ij，且最优解不再变化，则认为算法达到最优收敛。