本发明涉及一种自主式水下移动平台北斗定位与巡航优化方法，属于机器人智能控制的技术领域。
背景技术：
随着海洋开发方面日趋激烈的竞争，对具有自主导航能力的水下机器人的要求越来越高，定位是机器人实现自主导航的关键，同时定位与路径优化是实现自主移动机器人的核心问题，是机器人感知能力和智能水平的体现。机器人根据传感器提供周围环境信息和位置估计，逐步确定自身位姿。路径的优化方法有多种：如可视图法、自由空间法、单元分解法和人工势场法等，这个几种方法适用于确定环境中的路径规划，目前，PSO算法在很多实际问题中取得成功应用，包括从多峰非线性函数和多目标优化等问题,到动态系统的跟踪与优化。
水下定位过程中，由于环境比较恶劣，需要多个水面定位浮标，同时水下移动平台绝对位置标定时，需要再在水底恶劣的环境中维持稳定。水下平台巡航过程中需要重新进行绝对位置标定，通过浮标进行计算，计算量较大。
为克服上述限制，本发明设计了一种新的优化方法，用以解决水下移动平台巡航过程中路径优化及系统校正。
技术实现要素：
发明目的：针对现有技术中存在的问题，本发明提供一种自主式水下移动平台北斗定位与巡航优化方法。
技术方案：一种自主式水下移动平台北斗定位与巡航优化方法，包含自主式水下移动平台绝对位置标定和自主式水下移动平台相对位置定位系统优化校正两大步骤；
所述绝对位置标定步骤中基于北斗定位浮标计算获取自主式水下移动平台初始化参数，所述初始化参数包括自主式水下移动平台空间坐标、北斗精确授时；
所述自主式水下移动平台相对位置定位系统优化校正步骤中基于粒子群(PSO)优化自主式水下移动平台航迹路径，校正相对定位系统误差；
自主式水下移动平台绝对位置标定包括以下步骤：
(1)水下控制模块无线接收水面控制平台操作指令，从数据存储器中读取定位指令，向水面4个北斗智能浮标同步发送定位请求；
(2)北斗定位接收机接收北斗卫星定位数据，解析定位数据；
(3)信号处理电路对卫星信号做时延和误差估计，计算出浮标准确位置参数；
(4)北斗智能浮标通过超声波向水下控制模块发送各浮标位置参数和实时时间参数；
(5)超声波接收器接收水面北斗浮标的数据，使用渡越时间法计算出超声波信号在水中传输时间ti，进行机器人水下空间位置参数计算；
(6)北斗智能浮标获取北斗卫星定位数据并进行坐标系转换，将大地坐标系转换为空间直角坐标系，得到定位浮标各自空间坐标(oi,pi,qi)；
(7)4个浮标中选择1号浮标作为参考浮标，建立方程组：
其中vs为水介质中速度；
(8)由已知条件，根据方程式组求解出自主式水下移动平台空间坐标(o,p,q,)和水介质中速度vs；
实现自主式水下移动平台相对位置定位系统校正包括以下步骤：
(1)建立自主式水下移动平台运动模型：
其中状态向量st是标识机器人在t时刻的位置参数，表示机器人运动方位角，ωt表示机器人角加速度，ut表示机器人外部控制输入参数，δt表示机器人在水下移动过程中的随机噪声，h是状态转换模型；
(2)建立路径状态预测方程：
Δt为机器人移动时间，lt-1,t为t-1时刻到t时刻的航行距离，δm和δn分别为水平和垂直运动方向随机噪声；
(3)自主式水下移动平台接收外部控制指令，执行循环巡航任务，在直接坐标系中，自主式水下移动平台的出发点为S，自主式水下移动平台区域循环巡航路线为10mx10m的正方形，并在四个转角处转90°，巡航路径分成4段直线，从出发点S开始，下面的拐弯点依次为S1、S2和S3；
(4)对S-S1、S1-S2、S2-S3和S3-S四段巡航路径进行N等分，S-S1路径点集合为A＝{A0,A1,A2,...,An,An+1}，其余三段的集合分别为B＝{B0,B1,B2,...,Bn,Bn+1}、C＝{C0,C1,C2,...,Cn,Cn+1}和D＝{D0,D1,D2,...,Dn,Dn+1}，一系列的路径集合点构成了巡航路径，一个粒子代表一条路径，每一段粒子的向量就是指巡航路径的集合点距离该段起点的直线距离，一段路径中N各粒子组成一个群体；
(4)建立粒子群方程和适应度函数，对粒子参数初始化，设置学习因子c1和c2，将惯性权重w分配给每粒粒子，根据t-1时刻的粒子位置参数zt-1，预测t时刻的粒子位置zt；
(5)根据北斗绝对位置标定参数(oi,pi,qi)和上一步预测粒子状态参数zt，由贝叶斯法则，对每个粒子的权值进行更新；
(6)通过粒子群方程迭代运算，计算出巡航路径群体中各粒子的最小的适应度函数值f，比较适应值，找到路径点最优解，更新个体和全局最优值；
(7)根据最优解，比较粒子预测位置参数与最优点位置参数，两者距离若大于设定的阈值kij，对状态预测方程中向量进行校正，代入式5、式6，调整运动方位角和角加速度ωt，经多次迭代后，若小于设定阈值kij，且最优解不再变化，则认为算法达到最优收敛。
本发明采用上述技术方案，具有以下有益效果：
1、水下移动平台移动起点绝对位置标定确定，在移动过程中，只有到集合点时进行标定，在集合点之间移动过程中无需标定。
2、进过多次迭代，对大于阈值部分粒子进行优化校正，计算得出全局最优值。
综上所述，本发明能够满足面向水下自主巡航的要求。
附图说明
图1为本发明的方法总体流程图；
图2为本发明的绝对位置标定示意图；
图3为本发明的相对位置定位系统优化校正流程图。
具体实施方式
下面结合具体实施例，进一步阐明本发明，应理解这些实施例仅用于说明本发明而不用于限制本发明的范围，在阅读了本发明之后，本领域技术人员对本发明的各种等价形式的修改均落于本申请所附权利要求所限定的范围。
自主式水下移动平台北斗定位与巡航优化方法，包含自主式水下移动平台绝对位置标定和自主式水下移动平台相对位置定位系统优化校正两大步骤；
所述绝对位置标定步骤中基于北斗定位浮标计算获取自主式水下移动平台初始化参数，所述初始化参数包括自主式水下移动平台空间坐标、北斗精确授时；
所述自主式水下移动平台相对位置定位系统优化校正步骤中基于粒子群(PSO)优化自主式水下移动平台航迹路径，校正相对定位系统误差；
自主式水下移动平台绝对位置标定包括以下步骤：
(1)水下控制模块无线接收水面控制平台操作指令，从数据存储器中读取定位指令，向水面4个北斗智能浮标同步发送定位请求；
(2)北斗定位接收机接收北斗卫星定位数据，解析定位数据；
(3)信号处理电路对卫星信号做时延和误差估计，计算出浮标准确位置参数；
(4)北斗智能浮标通过超声波向水下控制模块发送各浮标位置参数和实时时间参数；
(5)超声波接收器接收水面北斗浮标的数据，使用渡越时间法计算出超声波信号在水中传输时间ti，进行机器人水下空间位置参数计算；
(6)北斗智能浮标获取北斗卫星定位数据并进行坐标系转换，将大地坐标系转换为空间直角坐标系，得到定位浮标各自空间坐标(oi,pi,qi)；
(7)4个浮标中选择1号浮标作为参考浮标，建立方程组：
其中vs为水介质中速度；
(8)由已知条件，根据方程式组求解出自主式水下移动平台空间坐标(o,p,q,)和水介质中速度vs；
实现自主式水下移动平台相对位置定位系统校正包括以下步骤：
(1)建立自主式水下移动平台运动模型：
其中状态向量st是标识机器人在t时刻的位置参数，表示机器人运动方位角，ωt表示机器人角加速度，ut表示机器人外部控制输入参数，δt表示机器人在水下移动过程中的随机噪声，h是状态转换模型；
(2)建立路径状态预测方程：
Δt为机器人移动时间，lt-1,t为t-1时刻到t时刻的航行距离，δm和δn分别为水平和垂直运动方向随机噪声；
(3)自主式水下移动平台接收外部控制指令，执行循环巡航任务，在直接坐标系中，自主式水下移动平台的出发点为S，自主式水下移动平台区域循环巡航路线为10mx10m的正方形，并在四个转角处转90°，巡航路径分成4段直线，从出发点S开始，下面的拐弯点依次为S1、S2和S3；
(4)对S-S1、S1-S2、S2-S3和S3-S四段巡航路径进行N等分，S-S1路径点集合为A＝{A0,A1,A2,...,An,An+1}，其余三段的集合分别为B＝{B0,B1,B2,...,Bn,Bn+1}、C＝{C0,C1,C2,...,Cn,Cn+1}和D＝{D0,D1,D2,...,Dn,Dn+1}，一系列的路径集合点构成了巡航路径，一个粒子代表一条路径，每一段粒子的向量就是指巡航路径的集合点距离该段起点的直线距离，一段路径中N各粒子组成一个群体；
(4)建立粒子群方程和适应度函数，对粒子参数初始化，设置学习因子c1和c2，将惯性权重w分配给每粒粒子，根据t-1时刻的粒子位置参数zt-1，预测t时刻的粒子位置zt；
(5)根据北斗绝对位置标定参数(oi,pi,qi)和上一步预测粒子状态参数zt，由贝叶斯法则，对每个粒子的权值进行更新；
(6)通过粒子群方程迭代运算，计算出巡航路径群体中各粒子的最小的适应度函数值f，比较适应值，找到路径点最优解，更新个体和全局最优值；
(7)根据最优解，比较粒子预测位置参数与最优点位置参数，两者距离若大于设定的阈值kij，对状态预测方程中向量进行校正，代入式5、式6，调整运动方位角和角加速度ωt，经多次迭代后，若小于设定阈值kij，且最优解不再变化，则认为算法达到最优收敛。
基于粒子群水下导航路径优化校正中，一个粒子代表一条路径，一条路径由一系列路径关键点构成，粒子的各维向量实际含义是路径上的关键点距离终点(即起点S)直连线的距离，巡航路径分成4段直线，从出发点S开始，下面的拐弯点依次为S1、S2和S3，一段路径点集合为：g＝(g0,g1,...,gN)，每一点为巡航路线等分点，N个粒子组成一个群体，粒子群算法通过下面公式对粒子操作：
vid(t+1)＝wvid(t)+c1r1(mid-xid(t))+c2r2(mgd-xid(t))式8
xid(t+1)＝xid(t)+vid(t+1)式9
相关参数有：i表示粒子个数，这里指路径点个数N，d表示向量维数，为路径位置参数，mi表示个体极值，mg为全局极值，r1和r2为[0,1]之间的随机数，w表示惯性权重，c1和c2是学习因子，设置c1＝c2＝2；
适应度函数：
f＝(xi-xs)2+(yi-ys)2式10
(xs，ys)为起点S位置参数
将惯性权重分配给每粒粒子，每个粒子表示为：
粒子群优化算法分下面几个步骤：
第一步：初始化种群，为N个路径粒子分配一个权重，将权重初始化为1/N；
第二步:根据公式进行递推迭代运算，根据t-1时刻的粒子位置参数，预测t时刻的粒子位置；
第三步：根据北斗绝对位置标定参数和上一步预测粒子状态参数，由贝叶斯法则，对每个粒子的权值进行更新，得到
第四步：通过迭代运算，计算出群体中各粒子的最小的适应度函数值，比较适应值，找到路径点最优解，更新个体和全局最优值，第一个解叫做个体极值pBest，另一个极值是整个种群的最优解，是全局极值gBest；
第五步：根据最优解，比较粒子预测位置参数与最优点位置参数，两者距离若大于设定的阈值，对状态预测方程中向量进行校正，代入式5、式6，调整运动方位角和角加速度，经多次迭代后，若小于设定阈值，且最优解不再变化，则认为算法达到最优收敛。