一种欠驱动水下机器人远距离精确自主搭载对接控制方法
一种欠驱动水下机器人远距离精确自主搭载对接控制方法
【技术领域】
[0001]本发明属于水下机器人对接回收领域，尤其涉及一种欠驱动水下机器人远距离精确自主搭载对接控制方法。
【背景技术】
[0002]目前，自主水下机器人得到了世界各国的高度重视，在军事和民用领域发挥了特别关键的作用。自主水下机器人搭载了能源、控制、探测系统，无须人工干预就能完成海洋环境信息采集、水中目标探测搜索、海洋科学考察等自主作业。由于载体结构限制，自主水下机器人携带的能源是有限的，无法长期在水下作业，因此载体释放与回收影响了自主水下机器人的作业效率。而能源与信息的水下交换技术为延长自主水下机器人作业时间提供了有效的解决手段。而实现能源与信息的水下交换，其前提是首先要完成自主水下机器人与基站的自主搭载对接。
[0003]为了完成自主搭载对接，需要知道水下机器人与基站的准确的相对位置，并且该信息能够持续的传递给水下机器人，即引导，然后机器人根据引导信息调整运动状态自主完成与基站的搭载对接。目前，应用的水下引导方法有光学引导与坐落式声学引导。光学引导相对简单，但是对水下能见度要求较高，并且由于光在水中的衰减，引导距离十分有限。坐落式水声引导相对来说作用距离稍远一些，并且对于水质的要求不那么强，但是引导的精度随着距离的增加下降很快，容易受到噪声的干扰，并且要求工作在较深的开阔水域，另外水下机器人载体的运动状态对引导的精度影响很大，要求水下机器人具有悬停定位的能力。然而，大部分水下机器人出于任务使命的需要，一般都设计为欠驱动形式，即配置一个主推、水平舵与垂直舵，能够实现进退、俯仰与转艏控制，而对于横移与升沉无法进行直接控制(一般水下机器人都忽略横滚控制)。另外实际的水下机器人作业范围较大，因此现有的方法难以实现水下机器人与基站的自主对接。本发明主要解决欠驱动水下机器人在较远的距离与基站的精确自主搭载对接问题。
【发明内容】
[0004]本发明的目的是一种利用声学引导信息、全自主方式欠驱动水下机器人远距离精确自主搭载对接控制方法。
[0005]本发明一种欠驱动水下机器人远距离精确自主搭载对接控制方法，包括以下步骤，
[0006]步骤一:航渡过程:基于预知的基站位姿信息，控制水下机器人航行至对接起始点，预知的基站位姿信息为基站上搭载对接装置大地坐标系下GPS玮度、GPS经度、相对水平面深度、横滚角、俯仰角及艏向角；
[0007]步骤二:粗对准过程:控制水下机器人按照预定的航线进行航行，航行过程中调整姿态，保证速度、深度和艏向控制稳定，利用水下机器人的水下声学超短基线定位系统对基站引导定位信息进行收集，直到引导定位信息数量大于门限值，并且引导定位信息连续稳定；
[0008]步骤三:精对准过程:对稳定的基站引导定位信息进行处理，完成水下机器人载体坐标系下基站位置的精确估计，根据基站引导定位信息实时地对水下机器人的航线、航行姿态进行微调，直到水下机器人与基站横向和垂向偏差满足搭载对接要求；
[0009]步骤四:对接过程:水下机器人进入到基站内后载体运动速度迅速下降至0，此时水下机器人主推持续工作，待基站完成对水下机器人的夹持动作后，主推关闭；
[0010]步骤五:退出过程:水下机器人和基站完成能量与信息交换后，基站松开水下机器人，水下机器人开始倒车，在后退距离L后，停止倒车，转入到其他作业。
[0011 ]本发明一种欠驱动水下机器人远距离精确自主搭载对接控制方法，还可以包括:
[0012]1、还包括步骤六，水下机器人出现异常状态时，返回步骤一控制水下机器人重新进行对接操作，或者回到待命点待命。
[0013]2、还包括步骤六，水下机器人对于自身状态进行持续的检测，当异常状态出现时，根据异常状态的类别采取相应的措施，异常状态包括系统超时、机器人离开作业区、剩余能源不足、目标点不可达、偏离航线过远、机器人无法移动、传感器故障和执行器故障;对于对接过程中出现的异常状态，采取水下机器人上浮至水面等待命令，若水下机器人无法退出基站时，先停止倒车，启动正车重新对接，然后再次倒车。
[0014]3、所述的步骤二中，水下声学超短基线定位系统采集基站定位信息后，对得到的连续的基站定位信息先进行时间补偿，然后进行姿态补偿，最后进行坐标变换，将基站定位信息转化为大地坐标系的基站定位信息；采用滑动窗口的方法判断基站定位信息是否稳定。
[0015]4、所述的水下机器人头部安装有水下声学超短基线定位系统，水下机器人中部搭载了电池舱和控制舱，水下机器人尾部装有惯导系统、天线、垂直舵、水平舵及主推装置。
[0016]5、所述的基站包括水声信标、喇叭口、捕获笼和基座，喇叭口位于捕获龙的一侧，捕获笼安装在基座上方，水声信标安装在喇叭口上。
[0017]6、所述的对接过程中水下机器人朝着喇叭口中心航行，在喇叭口的导向作用下进入到捕获笼内。
[0018]有益效果:
[0019]本发明涉及的一种欠驱动水下机器人远距离精确自主搭载对接控制方法，可以在离基站较远时进行搭载对接的准备工作，第一可以保证欠驱动水下机器人具有足够的航行距离进行姿态调整，第二可以具有足够的时间对水声定位信息进行处理分析，消除噪声信号干扰，提高基站定位的精度。因此，本发明能够解决欠驱动水下机器人进行搭载对接过程时，应用现有光学引导或坐落式水声引导所面临的距离短、水质要求高、要求悬停定位能力等不足。实践表明，本发明便于工程实现，并且成功率高。
【附图说明】
[0020]图1水下机器人与基站示意图；
[0021]图2—种基于声学引导信息的水下机器人自主搭载对接控制方法流程图；
[0022]图3水下机器人自主搭载对接实例。
【具体实施方式】
[0023]下面将结合附图对本发明做进一步详细说明。
[0024]发明的目的是提供一种可以实现欠驱动水下机器人借助于水声定位信息，无须任何人工干预，从较远的距离以全自主的方式，完成与基站的精确对准直至与基站搭载对接的控制方法。
[0025]本发明的目的是这样实现的:
[0026](1)航渡，根据预知的大地坐标系下基站位姿信息，控制水下机器人水下航行至对接起始点，在该点水下机器人正对基站且具有足够的航行距离。
[0027](2)粗对准，控制水下机器人按照预定的航线航行，航行过程中不断收集水下声学超短基线定位系统提供的基站定位信息，直到基站定位信息持续稳定。
[0028](3)精对准，对持续稳定的基站定位信息进行处理，完成水下机器人载体坐标系下基站位置的精确估计，然后实时地对水下机器人的航线、航行姿态进行微调，直到水下机器人与基站横向和垂向偏差满足搭载对接要求。
[0029](4)对接，水下机器人进入到基站内后迅速停止，这时水下机器人主推持续工作，保证水下机器人与基站紧密接触，待基站完成对水下机器人的夹持动作后，主推关闭。
[0030](5)退出，基站松开水下机器人后，水下机器人开始倒车，在后退足够的距离后，停止倒车，转入到其他作业。
[0031](6)异常处理，水下机器人与基站精对准后而未实现对接时，控制水下机器人重新进行(1)、(2)、(3)步，或者回到待命点待命。
[0032]一种欠驱动水下机器人远距离精确自主搭载对接控制方法，基于预先知道的基站位姿信息以及水下声学超短基线定位系统的引导信息，水下机器人经过航渡、粗对准、精对准、对接、退出以及异常处理等过程，完成水下机器人与基站的自主搭载对接。
[0033]预先知道的基站位姿信息:基站上搭载对接装置大地坐标系下GPS玮度、GPS经度、相对于水平面的深度、横滚角、俯仰角及首向角
[0034]USBL系统的引导信息是:水下机器人相对于基站上水声信标的纵向距离、横向距离、垂向距离及斜距，是基于水声定位方法获得的3维测距信息。
[0035]航渡过程:根据预知的大地坐标系下基站位姿信息，控制水下机器人水下航行至对接起始点，在该点水下机器人正对基站且具有足够的航行距离。
[0036]粗对准过程:控制水下机器人按照预定的航线航行，航行过程中不断收集水下声学超短基线定位系统提供的基站定位信息，直到基站定位信息持续稳定。
[0037]精对准过程:对持续稳定的基站定位信息进行处理，完成水下机器人载体坐标系下基站位置的精确估计，然后实时地对水下机器人的航线、航行姿态进行微调，直到水下机器人与基站横向和垂向偏差满足搭载对接要求。
[0038]对接过程:水下机器人进入到基站内后迅速停止，这时水下机器人主推持续工作，保证水下机器人与基站紧密接触，待基站完成对水下机器人的夹持动作后，主推关闭。
[0039]退出过程:基站松开水下机器人后，水下机器人开始倒车，在后退足够的距离后，停止倒车，转入到其他作业。
[0040]异常处理过程:水下机器人与基站精对准后而未实现对接时，控制水下机器人重新进行航渡、粗对准、精对准等过程，或者回到待命点待命。
[0041 ]图1中自主水下机器人载体头部装有声学 定位基阵1，中部搭载了电池舱2和控制舱3，尾部装有惯导系统4、天线5和垂直舵6、水平舵7及主推装置8，基站上装有水声信标9、喇叭口 10、捕获笼11、基座12。对接时水下机器人朝着喇叭口中心航行，在喇叭口的导向作用下依靠惯性进入到捕获笼内。本发明的目的是提供一种可以实现水下机器人借助于水声定位信息，无须任何人工干预，以全自主的方式，完成与基站的对准直至与基站搭载对接的控制方法。
[0042]由于结构限制，水下机器人一般都是欠驱动的，即一般只有进退、俯仰、转艏三个自由度的控制能力，而不能直接进行横移和升沉控制，因此对接时不能采取悬停定位的方式，只能在航行过程中不断调整水下机器人姿态，间接的消除横向和垂向偏差，以一种完全动态、自主的方式实现准确的对接。另外由于水下机器人机动性较差，需要离开基站较远的距离开始进行调整，而距离增加以及机器人的姿态对与水声定位的精度影响。因此，水下机器人的自主搭载对接是一个十分复杂的问题。
[0043]根据水下机器人的自主搭载对接的复杂性，将整个对接过程划分为若干阶段，每个阶段实现特定的目标，最终完成水下机器人与基站的搭载对接。结合图2，具体实施步骤如下:
[0044](1)航渡。对接过程开始点距离基站lkm左右，为了尽量充分利用该距离，要求水下机器人航行到该点时已经位于粗对准航线上，并且艏向与航线方向基本一致。因此航渡时，根据起始点位置及航线方向，首先确定一个过渡点，然后控制机器人向该点航行，到达该点后水下机器人按照过渡点至起始点的航线以路径跟踪的方式航行。路径跟踪控制方法有多种，为了实现水下机器人快速进入航线并且稳定的跟踪航线，本发明采用限制艏向的方法，即水下机器人的目标艏向取值范围为航线方向左右偏转30°。这样可以避免机器人艏向与航线方向相差较大时引起的机器人轨迹震荡，提高路径跟踪控制效果。
[0045](2)粗对准。水声定位系统在距离较远时给出的引导信息是不稳定的，同时信息连续性与精度较差，此时水声引导还未建立。因此，控制机器人按照朝向基站的航线航行。在航行过程中，水下对航行姿态进行调节，并接收水声定位系统提供的基站定位信息，同时完成以下处理:首先，判断定位信息的连续性，只有对时间连续的定位信息才能采取有效的数据处理手段;能够得到连续的定位信息之后，对其进行处理，先进行时间补偿，然后进行姿态补偿，最后进行坐标变换，将定位系统提供的基站相对位置信息转化为大地坐标系的定位信息;采用滑动窗口的方法，考察一段时间内基站定位信息的分布情况。当定位信息集中在一定范围内时，可以认为粗对准完成。
[0046](3)精对准。完成粗对准后进入精对准状态，此时基站定位信息连续稳定，可利用该信息对水下机器人进行引导。为了克服定位信息中噪声信号的干扰，需要对定位信息进行滤波，将处理得到的定位信息作为引导信息。精对准过程中，由于深度控制响应较慢，稳定时间长，因此首先对深度进行调整，使机器人保持对基站深度定位信息的跟踪状态。然后根据基站的水平面定位信息和已知的朝向，动态规划精对准航线，水下机器人通过调整艏向保持对该航线的跟踪状态。
[0047](4)对接。在精对准的最后阶段，保持水下机器人航行状态，当距离基站小于一定的距离时，进行对接测试。对接测试首先根据距离信息判断机器人是否在接近基站，如果否则对接失败，转入其他处理；如果是，则判断机器人速度是否小于一个小值并持续一定时间，条件成立则认为对接成功。对接成后，基站完成对水下机器人的夹持，并进行能量及信息交换等处理。
[0048](5)退出。能量与信息交换完成后，基站放松对水下机器人的夹持，水下机器人开始倒车，同时开始退出测试，当检测到水下机器人已经退出基站后停止倒车或执行其他任务，或者检测到退出失败时，转入异常处理。
[0049](6)异常处理。水下机器人对于自身状态进行持续的检测，当异常状态出现时，异常处理程序根据异常的类别采取相应的措施。异常状态包括系统超时、机器人离开作业区、剩余能源不足、目标点不可达、偏离航线过远、机器人无法移动、传感器故障、执行器故障等。对于对接过程中可能出现的异常，可采取水下机器人上浮至水面等待命令，若水下机器人无法退出基站时，先停止倒车，启动正车重新对接，然后再次尝试倒车。
[0050]下面结合实例具体说明自主对接控制过程，如图3:
[0051 ]图3中A点为水下机器人入水点，B点为待命点，D点为开始对接点，F点为已知的基站位置，G点为最后水声引导给出的基站位置，r多边形为划定的水下机器人工作区域，p折线(C-D-F)为规划的路径，t曲线为水下机器人航行的轨迹。水下机器人入水后首先进行航渡，即由A点向B点航行，航行至B点后向⑶航线航行，上线后按照航线航行至开始对接点D ；由D点开始粗对准，到E点时完成粗对准，开始精对准;精对准开始后，水下机器人离开预定航线，按照实时规划的航线航行，调整运动姿态，使机器人正对基站;到达G点后成功对接，完成能量与信息交换后退出，上浮至水面。
【主权项】
1.一种欠驱动水下机器人远距离精确自主搭载对接控制方法，其特征在于:包括以下步骤， 步骤一:航渡过程:基于预知的基站位姿信息，控制水下机器人航行至对接起始点，预知的基站位姿信息为基站上搭载对接装置大地坐标系下GPS玮度、GPS经度、相对水平面深度、横滚角、俯仰角及艏向角； 步骤二:粗对准过程:控制水下机器人按照预定的航线进行航行，航行过程中调整姿态，保证速度、深度和艏向控制稳定，利用水下机器人的水下声学超短基线定位系统对基站引导定位信息进行收集，直到引导定位信息数量大于门限值，并且引导定位信息连续稳定； 步骤三:精对准过程:对稳定的基站引导定位信息进行处理，完成水下机器人载体坐标系下基站位置的精确估计，根据基站引导定位信息实时地对水下机器人的航线、航行姿态进行微调，直到水下机器人与基站横向和垂向偏差满足搭载对接要求； 步骤四:对接过程:水下机器人进入到基站内后载体运动速度迅速下降至0，此时水下机器人主推持续工作，待基站完成对水下机器人的夹持动作后，主推关闭； 步骤五:退出过程:水下机器人和基站完成能量与信息交换后，基站松开水下机器人，水下机器人开始倒车，在后退距离L后，停止倒车，转入到其他作业。2.根据权利要求1所述的一种欠驱动水下机器人远距离精确自主搭载对接控制方法，其特征在于:还包括步骤六，水下机器人出现异常状态时，返回步骤一控制水下机器人重新进行对接操作，或者回到待命点待命。3.根据权利要求1所述的一种欠驱动水下机器人远距离精确自主搭载对接控制方法，其特征在于:还包括步骤六，水下机器人对于自身状态进行持续的检测，当异常状态出现时，根据异常状态的类别采取相应的措施，异常状态包括系统超时、机器人离开作业区、剩余能源不足、目标点不可达、偏离航线过远、机器人无法移动、传感器故障和执行器故障;对于对接过程中出现的异常状态，采取水下机器人上浮至水面等待命令，若水下机器人无法退出基站时，先停止倒车，启动正车重新对接，然后再次倒车。4.根据权利要求1所述的一种欠驱动水下机器人远距离精确自主搭载对接控制方法，其特征在于:所述的步骤二中，水下声学超短基线定位系统采集基站定位信息后，对得到的连续的基站定位信息先进行时间补偿，然后进行姿态补偿，最后进行坐标变换，将基站定位信息转化为大地坐标系的基站定位信息；采用滑动窗口的方法判断基站定位信息是否稳定。5.根据权利要求1所述的一种欠驱动水下机器人远距离精确自主搭载对接控制方法，其特征在于:所述的水下机器人头部安装有水下声学超短基线定位系统，水下机器人中部搭载了电池舱和控制舱，水下机器人尾部装有惯导系统、天线、垂直舵、水平舵及主推装置。6.根据权利要求1所述的一种欠驱动水下机器人远距离精确自主搭载对接控制方法，其特征在于:所述的基站包括水声信标、喇叭口、捕获笼和基座，喇叭口位于捕获龙的一侧，捕获笼安装在基座上方，水声信标安装在喇叭口上。7.根据权利要求1所述的一种欠驱动水下机器人远距离精确自主搭载对接控制方法，其特征在于:所述的对接过程中水下机器人朝着喇叭口中心航行，在喇叭口的导向作用下进入到捕获笼内。
【专利摘要】本发明公开了一种欠驱动水下机器人远距离精确自主搭载对接控制方法。航渡过程：基于预知基站位姿信息，控制水下机器人航行至对接起始点；粗对准过程：航行过程中采集基站定位信息，直到基站定位信息稳定；精对准过程：实时地对水下机器人的航线、航行姿态进行微调，直到水下机器人与基站横向和垂向偏差满足搭载对接要求；对接过程：水下机器人进入到基站内后迅速停止，水下机器人主推持续工作，待基站完成对水下机器人的夹持动作后，主推关闭；退出过程：水下机器人和基站完成能量与信息交换后，基站松开水下机器人后，水下机器人开始倒车。本发明无须任何人工干预，从较远的距离以全自主的方式，完成与基站的精确对准直到对接。
【IPC分类】G05D1/10
【公开号】CN105487548
【申请号】CN201510889730
【发明人】李岳明, 张国成, 刘涛, 万磊, 王庆, 吴海波, 张树迪, 蒋龙杰, 刘奕辉
【申请人】哈尔滨工程大学
【公开日】2016年4月13日
【申请日】2015年12月7日
【技术领域】
[0001]本发明属于水下机器人对接回收领域，尤其涉及一种欠驱动水下机器人远距离精确自主搭载对接控制方法。
【背景技术】
[0002]目前，自主水下机器人得到了世界各国的高度重视，在军事和民用领域发挥了特别关键的作用。自主水下机器人搭载了能源、控制、探测系统，无须人工干预就能完成海洋环境信息采集、水中目标探测搜索、海洋科学考察等自主作业。由于载体结构限制，自主水下机器人携带的能源是有限的，无法长期在水下作业，因此载体释放与回收影响了自主水下机器人的作业效率。而能源与信息的水下交换技术为延长自主水下机器人作业时间提供了有效的解决手段。而实现能源与信息的水下交换，其前提是首先要完成自主水下机器人与基站的自主搭载对接。
[0003]为了完成自主搭载对接，需要知道水下机器人与基站的准确的相对位置，并且该信息能够持续的传递给水下机器人，即引导，然后机器人根据引导信息调整运动状态自主完成与基站的搭载对接。目前，应用的水下引导方法有光学引导与坐落式声学引导。光学引导相对简单，但是对水下能见度要求较高，并且由于光在水中的衰减，引导距离十分有限。坐落式水声引导相对来说作用距离稍远一些，并且对于水质的要求不那么强，但是引导的精度随着距离的增加下降很快，容易受到噪声的干扰，并且要求工作在较深的开阔水域，另外水下机器人载体的运动状态对引导的精度影响很大，要求水下机器人具有悬停定位的能力。然而，大部分水下机器人出于任务使命的需要，一般都设计为欠驱动形式，即配置一个主推、水平舵与垂直舵，能够实现进退、俯仰与转艏控制，而对于横移与升沉无法进行直接控制(一般水下机器人都忽略横滚控制)。另外实际的水下机器人作业范围较大，因此现有的方法难以实现水下机器人与基站的自主对接。本发明主要解决欠驱动水下机器人在较远的距离与基站的精确自主搭载对接问题。
【发明内容】
[0004]本发明的目的是一种利用声学引导信息、全自主方式欠驱动水下机器人远距离精确自主搭载对接控制方法。
[0005]本发明一种欠驱动水下机器人远距离精确自主搭载对接控制方法，包括以下步骤，
[0006]步骤一:航渡过程:基于预知的基站位姿信息，控制水下机器人航行至对接起始点，预知的基站位姿信息为基站上搭载对接装置大地坐标系下GPS玮度、GPS经度、相对水平面深度、横滚角、俯仰角及艏向角；
[0007]步骤二:粗对准过程:控制水下机器人按照预定的航线进行航行，航行过程中调整姿态，保证速度、深度和艏向控制稳定，利用水下机器人的水下声学超短基线定位系统对基站引导定位信息进行收集，直到引导定位信息数量大于门限值，并且引导定位信息连续稳定；
[0008]步骤三:精对准过程:对稳定的基站引导定位信息进行处理，完成水下机器人载体坐标系下基站位置的精确估计，根据基站引导定位信息实时地对水下机器人的航线、航行姿态进行微调，直到水下机器人与基站横向和垂向偏差满足搭载对接要求；
[0009]步骤四:对接过程:水下机器人进入到基站内后载体运动速度迅速下降至0，此时水下机器人主推持续工作，待基站完成对水下机器人的夹持动作后，主推关闭；
[0010]步骤五:退出过程:水下机器人和基站完成能量与信息交换后，基站松开水下机器人，水下机器人开始倒车，在后退距离L后，停止倒车，转入到其他作业。
[0011 ]本发明一种欠驱动水下机器人远距离精确自主搭载对接控制方法，还可以包括:
[0012]1、还包括步骤六，水下机器人出现异常状态时，返回步骤一控制水下机器人重新进行对接操作，或者回到待命点待命。
[0013]2、还包括步骤六，水下机器人对于自身状态进行持续的检测，当异常状态出现时，根据异常状态的类别采取相应的措施，异常状态包括系统超时、机器人离开作业区、剩余能源不足、目标点不可达、偏离航线过远、机器人无法移动、传感器故障和执行器故障;对于对接过程中出现的异常状态，采取水下机器人上浮至水面等待命令，若水下机器人无法退出基站时，先停止倒车，启动正车重新对接，然后再次倒车。
[0014]3、所述的步骤二中，水下声学超短基线定位系统采集基站定位信息后，对得到的连续的基站定位信息先进行时间补偿，然后进行姿态补偿，最后进行坐标变换，将基站定位信息转化为大地坐标系的基站定位信息；采用滑动窗口的方法判断基站定位信息是否稳定。
[0015]4、所述的水下机器人头部安装有水下声学超短基线定位系统，水下机器人中部搭载了电池舱和控制舱，水下机器人尾部装有惯导系统、天线、垂直舵、水平舵及主推装置。
[0016]5、所述的基站包括水声信标、喇叭口、捕获笼和基座，喇叭口位于捕获龙的一侧，捕获笼安装在基座上方，水声信标安装在喇叭口上。
[0017]6、所述的对接过程中水下机器人朝着喇叭口中心航行，在喇叭口的导向作用下进入到捕获笼内。
[0018]有益效果:
[0019]本发明涉及的一种欠驱动水下机器人远距离精确自主搭载对接控制方法，可以在离基站较远时进行搭载对接的准备工作，第一可以保证欠驱动水下机器人具有足够的航行距离进行姿态调整，第二可以具有足够的时间对水声定位信息进行处理分析，消除噪声信号干扰，提高基站定位的精度。因此，本发明能够解决欠驱动水下机器人进行搭载对接过程时，应用现有光学引导或坐落式水声引导所面临的距离短、水质要求高、要求悬停定位能力等不足。实践表明，本发明便于工程实现，并且成功率高。
【附图说明】
[0020]图1水下机器人与基站示意图；
[0021]图2—种基于声学引导信息的水下机器人自主搭载对接控制方法流程图；
[0022]图3水下机器人自主搭载对接实例。
【具体实施方式】
[0023]下面将结合附图对本发明做进一步详细说明。
[0024]发明的目的是提供一种可以实现欠驱动水下机器人借助于水声定位信息，无须任何人工干预，从较远的距离以全自主的方式，完成与基站的精确对准直至与基站搭载对接的控制方法。
[0025]本发明的目的是这样实现的:
[0026](1)航渡，根据预知的大地坐标系下基站位姿信息，控制水下机器人水下航行至对接起始点，在该点水下机器人正对基站且具有足够的航行距离。
[0027](2)粗对准，控制水下机器人按照预定的航线航行，航行过程中不断收集水下声学超短基线定位系统提供的基站定位信息，直到基站定位信息持续稳定。
[0028](3)精对准，对持续稳定的基站定位信息进行处理，完成水下机器人载体坐标系下基站位置的精确估计，然后实时地对水下机器人的航线、航行姿态进行微调，直到水下机器人与基站横向和垂向偏差满足搭载对接要求。
[0029](4)对接，水下机器人进入到基站内后迅速停止，这时水下机器人主推持续工作，保证水下机器人与基站紧密接触，待基站完成对水下机器人的夹持动作后，主推关闭。
[0030](5)退出，基站松开水下机器人后，水下机器人开始倒车，在后退足够的距离后，停止倒车，转入到其他作业。
[0031](6)异常处理，水下机器人与基站精对准后而未实现对接时，控制水下机器人重新进行(1)、(2)、(3)步，或者回到待命点待命。
[0032]一种欠驱动水下机器人远距离精确自主搭载对接控制方法，基于预先知道的基站位姿信息以及水下声学超短基线定位系统的引导信息，水下机器人经过航渡、粗对准、精对准、对接、退出以及异常处理等过程，完成水下机器人与基站的自主搭载对接。
[0033]预先知道的基站位姿信息:基站上搭载对接装置大地坐标系下GPS玮度、GPS经度、相对于水平面的深度、横滚角、俯仰角及首向角
[0034]USBL系统的引导信息是:水下机器人相对于基站上水声信标的纵向距离、横向距离、垂向距离及斜距，是基于水声定位方法获得的3维测距信息。
[0035]航渡过程:根据预知的大地坐标系下基站位姿信息，控制水下机器人水下航行至对接起始点，在该点水下机器人正对基站且具有足够的航行距离。
[0036]粗对准过程:控制水下机器人按照预定的航线航行，航行过程中不断收集水下声学超短基线定位系统提供的基站定位信息，直到基站定位信息持续稳定。
[0037]精对准过程:对持续稳定的基站定位信息进行处理，完成水下机器人载体坐标系下基站位置的精确估计，然后实时地对水下机器人的航线、航行姿态进行微调，直到水下机器人与基站横向和垂向偏差满足搭载对接要求。
[0038]对接过程:水下机器人进入到基站内后迅速停止，这时水下机器人主推持续工作，保证水下机器人与基站紧密接触，待基站完成对水下机器人的夹持动作后，主推关闭。
[0039]退出过程:基站松开水下机器人后，水下机器人开始倒车，在后退足够的距离后，停止倒车，转入到其他作业。
[0040]异常处理过程:水下机器人与基站精对准后而未实现对接时，控制水下机器人重新进行航渡、粗对准、精对准等过程，或者回到待命点待命。
[0041 ]图1中自主水下机器人载体头部装有声学 定位基阵1，中部搭载了电池舱2和控制舱3，尾部装有惯导系统4、天线5和垂直舵6、水平舵7及主推装置8，基站上装有水声信标9、喇叭口 10、捕获笼11、基座12。对接时水下机器人朝着喇叭口中心航行，在喇叭口的导向作用下依靠惯性进入到捕获笼内。本发明的目的是提供一种可以实现水下机器人借助于水声定位信息，无须任何人工干预，以全自主的方式，完成与基站的对准直至与基站搭载对接的控制方法。
[0042]由于结构限制，水下机器人一般都是欠驱动的，即一般只有进退、俯仰、转艏三个自由度的控制能力，而不能直接进行横移和升沉控制，因此对接时不能采取悬停定位的方式，只能在航行过程中不断调整水下机器人姿态，间接的消除横向和垂向偏差，以一种完全动态、自主的方式实现准确的对接。另外由于水下机器人机动性较差，需要离开基站较远的距离开始进行调整，而距离增加以及机器人的姿态对与水声定位的精度影响。因此，水下机器人的自主搭载对接是一个十分复杂的问题。
[0043]根据水下机器人的自主搭载对接的复杂性，将整个对接过程划分为若干阶段，每个阶段实现特定的目标，最终完成水下机器人与基站的搭载对接。结合图2，具体实施步骤如下:
[0044](1)航渡。对接过程开始点距离基站lkm左右，为了尽量充分利用该距离，要求水下机器人航行到该点时已经位于粗对准航线上，并且艏向与航线方向基本一致。因此航渡时，根据起始点位置及航线方向，首先确定一个过渡点，然后控制机器人向该点航行，到达该点后水下机器人按照过渡点至起始点的航线以路径跟踪的方式航行。路径跟踪控制方法有多种，为了实现水下机器人快速进入航线并且稳定的跟踪航线，本发明采用限制艏向的方法，即水下机器人的目标艏向取值范围为航线方向左右偏转30°。这样可以避免机器人艏向与航线方向相差较大时引起的机器人轨迹震荡，提高路径跟踪控制效果。
[0045](2)粗对准。水声定位系统在距离较远时给出的引导信息是不稳定的，同时信息连续性与精度较差，此时水声引导还未建立。因此，控制机器人按照朝向基站的航线航行。在航行过程中，水下对航行姿态进行调节，并接收水声定位系统提供的基站定位信息，同时完成以下处理:首先，判断定位信息的连续性，只有对时间连续的定位信息才能采取有效的数据处理手段;能够得到连续的定位信息之后，对其进行处理，先进行时间补偿，然后进行姿态补偿，最后进行坐标变换，将定位系统提供的基站相对位置信息转化为大地坐标系的定位信息;采用滑动窗口的方法，考察一段时间内基站定位信息的分布情况。当定位信息集中在一定范围内时，可以认为粗对准完成。
[0046](3)精对准。完成粗对准后进入精对准状态，此时基站定位信息连续稳定，可利用该信息对水下机器人进行引导。为了克服定位信息中噪声信号的干扰，需要对定位信息进行滤波，将处理得到的定位信息作为引导信息。精对准过程中，由于深度控制响应较慢，稳定时间长，因此首先对深度进行调整，使机器人保持对基站深度定位信息的跟踪状态。然后根据基站的水平面定位信息和已知的朝向，动态规划精对准航线，水下机器人通过调整艏向保持对该航线的跟踪状态。
[0047](4)对接。在精对准的最后阶段，保持水下机器人航行状态，当距离基站小于一定的距离时，进行对接测试。对接测试首先根据距离信息判断机器人是否在接近基站，如果否则对接失败，转入其他处理；如果是，则判断机器人速度是否小于一个小值并持续一定时间，条件成立则认为对接成功。对接成后，基站完成对水下机器人的夹持，并进行能量及信息交换等处理。
[0048](5)退出。能量与信息交换完成后，基站放松对水下机器人的夹持，水下机器人开始倒车，同时开始退出测试，当检测到水下机器人已经退出基站后停止倒车或执行其他任务，或者检测到退出失败时，转入异常处理。
[0049](6)异常处理。水下机器人对于自身状态进行持续的检测，当异常状态出现时，异常处理程序根据异常的类别采取相应的措施。异常状态包括系统超时、机器人离开作业区、剩余能源不足、目标点不可达、偏离航线过远、机器人无法移动、传感器故障、执行器故障等。对于对接过程中可能出现的异常，可采取水下机器人上浮至水面等待命令，若水下机器人无法退出基站时，先停止倒车，启动正车重新对接，然后再次尝试倒车。
[0050]下面结合实例具体说明自主对接控制过程，如图3:
[0051 ]图3中A点为水下机器人入水点，B点为待命点，D点为开始对接点，F点为已知的基站位置，G点为最后水声引导给出的基站位置，r多边形为划定的水下机器人工作区域，p折线(C-D-F)为规划的路径，t曲线为水下机器人航行的轨迹。水下机器人入水后首先进行航渡，即由A点向B点航行，航行至B点后向⑶航线航行，上线后按照航线航行至开始对接点D ；由D点开始粗对准，到E点时完成粗对准，开始精对准;精对准开始后，水下机器人离开预定航线，按照实时规划的航线航行，调整运动姿态，使机器人正对基站;到达G点后成功对接，完成能量与信息交换后退出，上浮至水面。
【主权项】
1.一种欠驱动水下机器人远距离精确自主搭载对接控制方法，其特征在于:包括以下步骤， 步骤一:航渡过程:基于预知的基站位姿信息，控制水下机器人航行至对接起始点，预知的基站位姿信息为基站上搭载对接装置大地坐标系下GPS玮度、GPS经度、相对水平面深度、横滚角、俯仰角及艏向角； 步骤二:粗对准过程:控制水下机器人按照预定的航线进行航行，航行过程中调整姿态，保证速度、深度和艏向控制稳定，利用水下机器人的水下声学超短基线定位系统对基站引导定位信息进行收集，直到引导定位信息数量大于门限值，并且引导定位信息连续稳定； 步骤三:精对准过程:对稳定的基站引导定位信息进行处理，完成水下机器人载体坐标系下基站位置的精确估计，根据基站引导定位信息实时地对水下机器人的航线、航行姿态进行微调，直到水下机器人与基站横向和垂向偏差满足搭载对接要求； 步骤四:对接过程:水下机器人进入到基站内后载体运动速度迅速下降至0，此时水下机器人主推持续工作，待基站完成对水下机器人的夹持动作后，主推关闭； 步骤五:退出过程:水下机器人和基站完成能量与信息交换后，基站松开水下机器人，水下机器人开始倒车，在后退距离L后，停止倒车，转入到其他作业。2.根据权利要求1所述的一种欠驱动水下机器人远距离精确自主搭载对接控制方法，其特征在于:还包括步骤六，水下机器人出现异常状态时，返回步骤一控制水下机器人重新进行对接操作，或者回到待命点待命。3.根据权利要求1所述的一种欠驱动水下机器人远距离精确自主搭载对接控制方法，其特征在于:还包括步骤六，水下机器人对于自身状态进行持续的检测，当异常状态出现时，根据异常状态的类别采取相应的措施，异常状态包括系统超时、机器人离开作业区、剩余能源不足、目标点不可达、偏离航线过远、机器人无法移动、传感器故障和执行器故障;对于对接过程中出现的异常状态，采取水下机器人上浮至水面等待命令，若水下机器人无法退出基站时，先停止倒车，启动正车重新对接，然后再次倒车。4.根据权利要求1所述的一种欠驱动水下机器人远距离精确自主搭载对接控制方法，其特征在于:所述的步骤二中，水下声学超短基线定位系统采集基站定位信息后，对得到的连续的基站定位信息先进行时间补偿，然后进行姿态补偿，最后进行坐标变换，将基站定位信息转化为大地坐标系的基站定位信息；采用滑动窗口的方法判断基站定位信息是否稳定。5.根据权利要求1所述的一种欠驱动水下机器人远距离精确自主搭载对接控制方法，其特征在于:所述的水下机器人头部安装有水下声学超短基线定位系统，水下机器人中部搭载了电池舱和控制舱，水下机器人尾部装有惯导系统、天线、垂直舵、水平舵及主推装置。6.根据权利要求1所述的一种欠驱动水下机器人远距离精确自主搭载对接控制方法，其特征在于:所述的基站包括水声信标、喇叭口、捕获笼和基座，喇叭口位于捕获龙的一侧，捕获笼安装在基座上方，水声信标安装在喇叭口上。7.根据权利要求1所述的一种欠驱动水下机器人远距离精确自主搭载对接控制方法，其特征在于:所述的对接过程中水下机器人朝着喇叭口中心航行，在喇叭口的导向作用下进入到捕获笼内。
【专利摘要】本发明公开了一种欠驱动水下机器人远距离精确自主搭载对接控制方法。航渡过程：基于预知基站位姿信息，控制水下机器人航行至对接起始点；粗对准过程：航行过程中采集基站定位信息，直到基站定位信息稳定；精对准过程：实时地对水下机器人的航线、航行姿态进行微调，直到水下机器人与基站横向和垂向偏差满足搭载对接要求；对接过程：水下机器人进入到基站内后迅速停止，水下机器人主推持续工作，待基站完成对水下机器人的夹持动作后，主推关闭；退出过程：水下机器人和基站完成能量与信息交换后，基站松开水下机器人后，水下机器人开始倒车。本发明无须任何人工干预，从较远的距离以全自主的方式，完成与基站的精确对准直到对接。
【IPC分类】G05D1/10
【公开号】CN105487548
【申请号】CN201510889730
【发明人】李岳明, 张国成, 刘涛, 万磊, 王庆, 吴海波, 张树迪, 蒋龙杰, 刘奕辉
【申请人】哈尔滨工程大学
【公开日】2016年4月13日
【申请日】2015年12月7日
最新回复(0)