本发明涉及auv与海底基站对接过程的导航与引导方法，尤其涉及一种基于测距声通的auv回坞导航方法。
背景技术：
随着人们对海洋资源的开发，自主水下航行器(auv)的使用越来越广泛。然而，auv在水下所携带的能源有限，因此需要对auv回收充电。为了降低成本，人们希望这一充电过程可以在水下直接进行，而非用母船对其回收至岸上进行。
公开号为cn102320362a的中国专利文献公开了一种海底对接装置，用于回收auv进行充电。为了auv能够顺利与海底基站对接，就需要精确的导航算法。
由于电磁波在海水中会急速衰减，无法利用gps在海底进行导航定位。因此在海洋中，主要是依靠声学定位系统进行导航定位，例如超短基线(usbl)、短基线(sbl)和长基线(lbl)等。
公开号为cn104457754a的中国专利文献公开了一种利用长基线进行潜器定位的导航算法，然而利用长基线进行定位的前提是需要提前在潜器航行的区域进行基线的布放，增加了实现的难度和成本。sbl和usbl的使用相比lbl难度虽然有所降低，但这两种定位系统的成本依旧高昂，难以推广应用，因此，就需要一种导航定位精度高，成本低，并适用于auv与海底基站的导航定位方法。
在auv进入视觉导引区后，需要保证在洋流的干扰下，auv的姿态使其始终能够看到海底基站上的导航灯，因此，就需要一种适合auv在洋流干扰下的回坞导引方法。
技术实现要素：
本发明提供了一种基于测距声通的auv回坞导航方法，该导航方法精度高、成本低。
一种基于测距声通的auv回坞导航方法，包括以下步骤：
(1)将auv下潜至与海底基站同样深度后朝海底基站航行；
(2)在朝海底基站航行过程中，以auv的位置为圆心，以测距声通测得的auv与海底基站之间的距离为半径画圆，利用auv处于不同位置时所画圆的交点来估计海底基站的位置；
(3)根据获得的海底基站的位置进行auv的航位推算，对auv进行导航；
(4)当auv与海底基站之间的距离到达预设的距离阈值时，auv按照预设半径的圆形轨迹环绕，环绕过程中结合auv与海底基站之间的距离信息采用扩展卡尔曼滤波算法寻找auv的定位可信的位置；
(5)auv从定位可信的位置朝海底基站航行进入视觉末端导引范围，根据视觉末端的导引进入海底基站。
测距声通为一对带有测距功能的声通(acousticmodem)，一端装在auv(自主水下航行器)上，一端装在海底基站上。auv在行驶过程中不断收到海底基站发来的两者之间的距离信息。
步骤(2)中，估计海底基站位置的方法为：
按照获得距离信息的先后顺序，分别以auv的两个不同时刻的位置p1、p2为圆心，以对应的auv与海底基站之间的距离l1、l2为半径画圆，两圆相交于一点时，交点为海底基站的位置；
若两圆交于两点q1和q2；以下一时刻auv的位置p3为圆心，以对应的auv与海底基站之间的距离l3为半径画圆，此圆与前两圆交于点mi，其中i＝0、1、2、3或4；
i＝0时，读取下一时刻auv的位置代替p3，继续画圆，直至i≠0；
i≠0时，计算mi与q1、q2的距离|miq1|、|miq2|，若|miq1|为最短距离，则q1为海底基站的位置；反之，则q2为海底基站的位置。
获得海底基站的位置后对auv进行导航。
为了保证导航的精度，作为优选，在auv朝海底基站航行过程中，不断重复步骤(2)和(3)对海底基站的位置进行更新。
为了进一步提高导航的精度，当auv航行到距离海底基站一定距离时，采用步骤(4)中更高精度的导航方法对auv进行导航。
步骤(4)中，预设的距离阈值和预设半径根据实际的导航定位效果而定。
步骤(4)中，扩展卡尔曼滤波算法采用的系统状态方程为：
其中，f为系统函数，xk为k时刻的系统状态变量，xk-1为k-1时刻的系统状态变量，vk-1为k-1时刻的状态噪声向量，t是采样时间间隔，u是auv前向速度，v是auv侧向速度，ψ为auv的偏航角，(xauv，yauv)为auv在海底基站所处平面的绝对坐标；
采用的观测方程为：
其中，zk为观测方程，g为量测函数，wk为观测噪声，vsound为声音在海水中传播的速度，(xtra，ytra)为auv发出声学信号时的绝对坐标，(xrec，yrec)为auv收到海底基站返回的应答信号时的绝对坐标。
进一步的，步骤(4)中，寻找auv的定位可信的位置的方法包括：
(4-1)对auv进行状态初始化：
p0＝var(x0)，x0＝e(x0)
其中，p0是初始状态变量x0的协方差矩阵；
(4-2)对auv进行状态更新：
xk，k-1＝f(xk-1)
其中，qk-1为状态噪声的协方差矩阵，pk，k-1为更新过程状态变量协方差矩阵，xk，k-1为更新过程状态变量，pk-1为k-1时刻状态变量协方差矩阵；
(4-3)对auv进行状态校正：
xk＝xk，k-1+gk(tk-g(xk，k-1))
其中，gk为卡尔曼增益矩阵，xk，k-1为更新过程状态变量，tk为最近的一次auv向海底基站发射声学信号和收到应答的时间间隔，rk为k时刻量测噪声的协方差矩阵，pk为k时刻状态变量协方差矩阵；
(4-4)定位误差判断：
构造误差判别式为：
若δ＜γ2，则认为此时auv的定位可信；否则，auv继续按圆轨迹进行环绕航行；
上式中，γ2为设定的阈值常量。γ2根据实际的auv对接成果率而定。
进一步优选的，步骤(4)中，在没有获得观测数据来进行状态校正的间隙，重复执行步骤(4-1)和(4-2)来进行航位推算，获得自身的位置信息进行导航；当获得定位可信的位置后，按照步骤(4-1)-(4-3)进行组合导航。
步骤(5)中，auv驶入视觉末端导引范围后，根据视觉末端的引导进入海底基站，包括：
(5-1)将海底基站的开口朝向调整到与洋流方向相同；
海底基站根据自身携带的声学多普勒流速剖面仪(adcp)测出洋流的方向和大小，同时将海底基站的开口朝向调整到与洋流方向相同；
调整海底基站的开口朝向，可以克服洋流对auv对接所产生的影响，提高auv对接成功率；
(5-2)计算海底基站出现在auv视线范围的临界条件所对应的auv偏航角约束范围；
根据auv头部摄像头的可视角与auv的偏航角计算auv偏航角约束范围，以此作为导引控制过程中auv的偏航角边界；
(5-3)以auv与海底基站的连线作为理想航线进行导引控制，auv的偏航角控制在步骤(5-2)计算得到的auv偏航角约束范围内；
(5-4)auv进入海底基站。
与现有技术相比，本发明的导航方法的优点在于：
(1)利用一对带有测距功能的声通来获得auv与海底基站的相对位置关系，相比于lbl、sbl和usbl水声定位系统，成本降低；
(2)利用距离信息的画圆方法求解auv的绝对位置，方法简单，实时性好；
(3)auv在海底基站前端按圆形轨迹航行，不断搜寻满足误差要求的定位点，定位精度得到提高；
(4)海底基站能通过自动转向克服海流对auv对接的影响，提高了对接成功率。
附图说明
图1为auv画圆定位海底基站的示意图；
图2为auv按照圆形轨迹环绕定位的示意图；
图3为海底基站根据洋流方向调整其开口方向的示意图。
具体实施方式
下面结合附图和实施例对本发明作进一步详细描述。
将一对带有测距功能的声通，一端装在auv上，一端装在海底基站上。海底基站布放位置的gps坐标已知。auv在水面上通过gps获得自身的初始位置后，开始下潜，朝着海底基站的方向驶去。当auv行驶至声学信号覆盖范围后，auv在航行过程中会不断收到海底基站发来的距离信息。
如图1所示，按照获得距离信息的先后顺序，分别取三个auv的位置和对应的距离信息。其中三个位置信息定义为：位置1(p1)、位置2(p2)和位置3(p3)；三个对应的距离信息为：l1、l2和l3；以p1和p2为圆心，半径l1和l2分别画圆，两圆相交于点q1和q2(当q1和q2重合时，则q1(q2)即为海底基站的位置)；以p3为圆心，l3为半径画圆，此圆与之前的两个圆有i个交点(其中，i＝0、1、2、3或4)，第i个交点为点mi，同时假设海底基站的水平面绝对坐标为(0，0)；
若i＝0，则继续读取下一个距离信息来代替位置3的距离和位置信息，继续画圆，直到i不等于0；计算所有交点mi与q1和q2的距离|miq1|和|miq2|。若最短距离为|miq1|中的一个，则q1为海底基站的位置；反之，q2为海底基站的位置；
auv获得基站位置后，利用航位推算，朝着海底基站的方向开去。此过程中，不断重复画圆的方法，进行海底基站位置的更新，保证导航的精度。
如图2所示，当auv航行至海底基站前方区域一定距离(该距离根据实际的导航效果进行确定)时，auv开始按一定半径(该半径根据实际的导航效果进行确定)的圆形轨迹(圆形轨迹的圆心不用确定，确定其半径即可)环绕。
环绕过程中，结合auv与海底基站之间的距离信息采用扩展卡尔曼滤波算法寻找auv的定位可信的位置。
扩展卡尔曼滤波算法采用的系统状态方程为：
其中，f为系统函数，xk为k时刻的系统状态变量，xk-1为k-1时刻的系统状态变量，vk-1为k-1时刻的状态噪声向量，t是采样时间间隔，u是auv前向速度，v是auv侧向速度，ψ为auv的偏航角，(xauv，yauv)为auv在海底基站所处平面的绝对坐标；
采用的观测方程为：
其中，zk为观测方程，g为量测函数，wk为观测噪声，vsound为声音在海水中传播的速度，(xtra，ytra)为auv发出声学信号时的绝对坐标，(xrec，yrec)为auv收到海底基站返回的应答信号时的绝对坐标。
寻找auv的定位可信的位置的方法包括：
(4-1)对auv进行状态初始化：
p0＝var(x0)，x0＝e(x0)
其中，p0是初始状态变量x0的协方差矩阵；
(4-2)对auv进行状态更新：
xk，k-1＝f(xk-1)
其中，qk-1为状态噪声的协方差矩阵，pk，k-1为更新过程状态变量协方差矩阵，xk，k-1为更新过程状态变量，pk-1为k-1时刻状态变量协方差矩阵；
(4-3)对auv进行状态校正：
xk＝xk，k-1+gk(tk-g(xk，k-1))
其中，gk为卡尔曼增益矩阵，xk，k-1为更新过程状态变量，tk为最近的一次auv向海底基站发射声学信号和收到应答的时间间隔，rk为k时刻量测噪声的协方差矩阵，pk为k时刻状态变量协方差矩阵；
(4-4)定位误差判断：
构造误差判别式为：
若δ＜γ2，则认为此时auv的定位可信；否则，auv继续按圆轨迹进行环绕航行；
上式中，γ2为设定的阈值常量。γ2根据实际的auv对接成果率而定。
当auv获得可信的自身定位后，按照步骤(4-1)-(4-3)进行组合导航；在没获得观测数据来进行状态校正的间隙，重复执行步骤(4-1)和(4-2)来进行航位推算，获得自身的位置信息来进行导航。
当auv驶入视觉末端导引范围后，auv的导引方法需要根据洋流方向和大小进行补偿，并按照以下步骤实现：
步骤1、auv驶入视觉导引区后，海底基站根据自身携带的声学多普勒流速剖面仪(adcp)测出洋流的方向和大小，同时将海底基站的开口朝向调整到与洋流方向相同的方向，如图3所示；
步骤2、根据auv头部摄像头的可视角与auv的偏航角计算出海底基站出现在auv视线范围的临界条件所对应的auv偏航角约束范围，以此作为导引控制过程中的偏航角边界；
步骤3、以auv与海底基站的连线作为理想航线进行导引控制，偏航角控制范围保证在步骤2所计算出的边界内；
步骤4、auv进入海底基站。
以上所述的实施例对本发明的技术方案和有益效果进行了详细说明，应理解的是以上所述仅为本发明的具体实施例，并不用于限制本发明，凡在本发明的原则范围内所做的任何修改、补充和等同替换等，均应包含在本发明的保护范围之内。