基于声光联合导引重型AUV动/静态目标自主对接方法

文档序号:25541706发布日期:2021-06-18 20:37阅读:202来源:国知局
基于声光联合导引重型AUV动/静态目标自主对接方法

本发明涉及水下机器人技术领域,尤其涉及一种基于声光联合导引的水下机器人的动态及静态目标自主对接方法。



背景技术:

随着人类对海洋领域探索的不断深入,无缆水下机器人(autonomousunderwatervehicle,auv)作为海洋探测的有效工具,发挥着越来越重要的作用。当前绝大多数auv使用电池作为能源,需要保障船舶及人员回收充电,回收过程繁琐且成本较高。由此auv水下自主回收需求应运而生,水下自主回收的关键是水下自主对接,通过与对接装置对接,可实现水下数据传输和能源补给,极大的提高了auv的工作范围及和时间。现有验证的对接技术存在以下三点问题:

一、对接技术验证的auv大多数长度小于5米,重量小于1000kg,长度短重量轻便于航行控制及布放回收,但受制于体积尺寸,携带探测设备和电池的重量及尺寸有严格限制,尤其是声学探测设备及高精度惯性导航设备,作业用途及范围受到限制。重型auv克服了轻型auv搭载设备受限等问题,但由于外形尺寸更大,具有惯性大、长时滞等特点,控制难度更高。

二、与固定目标对接需要提前在海底布放装置并完成相关电缆的铺设,auv的作业范围受到装置布放地域及数量的限制。若能实现auv与动态目标的对接,便可实现auv与auv自主对接、auv与无人水面艇自主对接、auv与海底基站自主对接,进一步拓展了auv作业范围及用途。与静态固定目标对接相比,动态目标对接装置位置姿态实时变化,对目标位置解算及路径跟踪要求更高,对接难度更大。

三、在水下执行任务过程中,auv应能自主判断是否成功进入对接装置,根据对接装置命令执行相应动作,若对接失败应具备再次对接功能,若出现与对接装置卡住或其他故障情况,还应具备完整的故障处理流程。



技术实现要素:

为了克服现有方法的不足,本发明要解决的技术问题是声学光学联合导引目标定位,重型auv动态目标对接路径规划及航行控制,对接成功判断及故障处理等问题。

本发明为实现上述目的所采用的技术方案是:基于声光联合导引重型auv动/静态目标自主对接方法,包括以下步骤:

1)基阵位置搜索阶段:auv搜寻目标上的基阵发出的声学定位数据,确定目标位置;

2)向对接航线起点航行阶段:根据目标位置规划对接航线,向对接航线起点航行;

3)远距离导引阶段:根据目标位置调整对接航线,使auv沿对接航线航行;

4)末端距离导引阶段:使用声学定位数据,提高目标位置更新频率,当收到光学定位数据时,则使用光学导引使auv到达目标位置。

所述确定目标位置具体为:

x_d=-(cos(h)(cos(p)*x_z-sin(p)(sin(r)*y_z+cos(r)*z_z))-sin(h)(cos(r)*y_z-sin(r)*z_z))

y_d=-(sin(h)(cos(p)*x_z-sin(p)(sin(r)*y_z+cos(r)*z_z))+cos(h)(cos(r)*y_z-sin(r)*z_z))

z_d=sin(p)*x_z+cos(p)*(sin(r)*y_z+cos(r)*z_z)

其中,h、p、r分别表示auv的航向、纵倾和横滚角度,x_z、y_z和z_z分别为auv在载体坐标系下的北向、东向和垂向距离,x_d、y_d和z_d分别为目标在大地坐标系下的东向、北向和垂向距离。

所述根据目标位置调整对接航线,使auv沿对接航线航行,具体为:根据auv当前航迹偏航角与目标航迹偏航角的差值,得到航向偏差;将偏差与auv当前航向求和得到目标航向,根据目标航向更新对接航线,auv沿更新后的对接航线航行。

所述根据auv当前航迹偏航角与目标航迹偏航角的差值,得到航向偏差,具体为:

其中,δhdis为航迹偏航角,kp、ki、kd分别为比例系数、积分系数、微分系数,δdis为当前auv与目标航线距离值;

航向偏差δh′如下:

δh′=δhdock-δhdis

其中,δh′为航向偏差,由auv当前航向与目标轴线夹角δhdock与航迹偏航角δhdis计算得出。

步骤4)中,当auv获取到光学导引数据后,使用光学导引数据更新目标位置。

本发明具有以下有益效果及优点:

1.本发明提出了一套自主性高的重型auv对接方法,经过实际试验验证,该对接方法具有对接成功率高、实用性强的特点;

2.本发明所提出的试验方法可同时满足固定(静态)目标和移动(动态)目标的对接需求;

3.本发明提出一套完整的对接试验方法,包括目标搜索、目标位置解算、自主对接路径规及导引、对接完成判断,具有实施性高、可靠性高的特点。

附图说明

图1a为定位设备安装示意图——可移动对接装置;

图1b为定位设备安装示意图——对接auv;

图2为声学数据定义图;

图3为光学导引灯标安装示意图;

图4为声光联合导引切换策略图;

图5为本发明自主对接航线规划方法示意图;

图6为本发明的航迹偏航角示意图;

图7为声学目标搜索示意图。

具体实施方式

为了克服现有方法的不足,本发明要解决的技术问题是声学光学联合导引目标定位,重型auv动态目标对接路径规划及航行控制,对接成功判断及故障处理等问题。

本发明为实现上述目的所采用的技术方案是:一种基于声学、光学联合定位导引方式,面向重型无缆水下机器人与动态或静态中的对接装置实现自主对接的方法:

利用水声定位设备搜索、确定可移动对接装置位置;

水声定位设备将定位信息及对接装置航行姿态数据发送给auv;

根据水声定位设备特性筛选、滤波、解算可移动对接装置位置;

根据声学及光学定位数据更新对接装置位置;

自主规划对接航线,根据声学、光学定位数据实时调整对接航线;

自主判断对接完成状态,规划再次对接或执行相应处理。

通过auv航行搜索获取声学定位数据,对声学、光学数据筛选并解算确定目标位置,接入导引阶段后使用声光联合导引,实时更新目标位置;

根据目标位置解算结果,进行自主路径规划,并根据对接完成情况决定接下来的航行行为;

按照对接阶段划分由三部分组成,向对接航线起点航行阶段、远距离导引阶段和末端导引阶段,根据不同阶段采取不同的导引策略及控制方法。

本实施例的对接目标可以为对接装置,对接装置为现有技术,可以是框架结构,尾部导流罩边缘上设有光学灯标,可以采用专利(申请号:201711329091.3,公布号cn109921233a)“一种水下对接装置”的结构。

如图1a~图1b所示,一种基于带有定位功能的水声通讯的设备,包括水声通讯设备基阵,搭载于移动对接装置上,用于测得auv相对于移动对接装置的位置关系,将此关系信息及对接装置姿态信息通过水声通信发送给auv;水声通讯设备信标,搭载于auv上,用于与基阵进行通讯并接收定位信息。

一种基于光学的导引定位设备,包括安装在对接装置导流罩边缘上的光学灯标,用于对装置入口边缘示位;与auv纵轴平行安装的水下光学摄像机,用于确定导流罩姿态及位置信息。

圆柱状细长体auv,长度为8米,空气中重量约为1.5吨,属于重型auv。为满足对接航行控制需求,正常航速时采用主推加鳍舵控制,在低速时在主推加鳍舵基础上,增加艏、艉部垂向推进器和艏部横向推进器的控制,垂向推进器用于垂直面(纵倾、深度)调整,横向推进器用于水平面调整。

一种基于声学、光学的动态目标位置搜索解算及定位方法。

按照对接auv与对接装置相对距离及对接阶段不同,目标定位方式和方法进行了相应设计,由①目标位置搜索阶段、②向对接航线起点航行阶段、③远距离导引阶段、④末端距离导引阶段四部分组成。

目标位置搜索阶段,作用为搜寻目标(对接装置上的基阵)声学信号、确定目标位置,包括目标筛选、位置解算及滤波三个部分。声学数据筛选基于水声定位原理和实际试验结果,为保证定位精度,优先筛选信标与基阵中心连线角度较小的数据。目标位置筛选、解算方法如下:

声学数据定义如图2所示,其中srange为基阵到信标的细线距离,点abco构成平面为基阵在信标上的投影,depth为基阵到投影面的距离,点o为基阵轴线与投影面交点,range为点o到点b的距离,range反映的是信标在投影平面与基阵的轴向距离,经过多次试验验证及分析发现,range越小表示信标与基阵平面夹角越小,数据可信度越高,因此本发明选择range值对数据进行筛选。

其中:f_search用于对获取的声学进行筛选、解算以及滤波,由cal_target1、cal_target2以及滤波函数组成,其中n代表获取的有效声学数据数量,range0、range[i](筛选阈值)代表声学数据筛选标准,选取小于筛选标准的声学数据进行位置解算。获取声学数据后,cal_target1与cal_target2函数对目标位置进行筛选和解算(包括公式(1)~(3)),区别为cal_target1用于声学数据较少的情况,当数据个数小于3组,只使用range0对数据进行筛选,选取小于range0的数据;当数据个数大于等于3组,使用cal_target2函数进行解算,在cal_target2函数中,使用多组range数据进行筛选,其中range[i]<range[i+1],优先选取满足小于range[min]的一组数据解算目标位置。

在向对接航线航向阶段使用声学定位,由于实际动态对接中,收到声学信号位置与对接航线起点距离会达到几百甚至上千米,同时对接装置位置也在不断变化中,因此在向对接航线航行过程中,需要根据对接装置的位置更新对接航线起点和终点。目标更新策略为:每收到5组声学数据后,进行中位值滤波,去掉两组极值后取平均后结算目标位置。

第三阶段为远距离导引阶段,即进入对接航线后,根据声学解算数据调整对接航线,沿对接航线航行。

完成目标位置解算后,为防止毛刺影响解算精度,使用滤波函数对数据进行中位值平均滤波,剔除毛刺点。获取对接装置与对接auv的相对位置数据,为进一步提高目标定位精度,本发明结合auv纵倾、横滚和航向等姿态数据,将声学定位数据进行坐标变换

x_d=-(cos(h)(cos(p)*x_z-sin(p)(sin(r)*y_z+cos(r)*z_z))-sin(h)(cos(r)*y_z-sin(r)*z_z))(1)

y_d=-(sin(h)(cos(p)*x_z-sin(p)(sin(r)*y_z+cos(r)*z_z))+cos(h)(cos(r)*y_z-sin(r)*z_z))(2)

z_d=sin(p)*x_z+cos(p)*(sin(r)*y_z+cos(r)*z_z)(3)

其中h、p、r为对接auv的航向、纵倾和横滚角度,x_z、y_z和z_z为载体(auv)坐标系下的北向、东向和垂向距离,计算结果x_d、y_d和z_d为大地坐标系下的东向、北向和垂向距离。

完成变换后,结合auv位置推算目标在大地坐标系下位置,并规划对接航线。

第四阶段为末端导引阶段,由于距离对接装置距离较近,提高使用声学数据解算目标位置更新频率。当获取光学导引数据后,改为使用光学导引数据更新目标位置。光学导引安装灯标如图3所示,在对接装置导流罩边缘安装导引灯,对接auv通过视觉方式获取对接装置入口的方向、姿态、auv艏部与对接装置入口处位置偏差等数据。考虑到在实际对接任务中受光线、水质等因素影响,存在无法获取有效光学导引数据的情况,因此设计了声光联合导引切换策略,即auv在接近对接装置后,如图4所示,若光学数据有效,则使用光学数据进行导引,否则使用声学数据完成末端导引。

一种面向动态对接的自主对接航线规划方法。

完成目标位置解算,根据目标位置及航向规划对接航线,对接航线起点和终点随着对接装置位置变化进行实时调整,对接航线规划如图5所示。①获取目标位置后,进行自主路径规划,确定对接航线的起点终点;②向对接航线起点航行,在实际对接中,完成对接路径规划到对接航线起点位置距离较远,且位置在不断变化中,auv根据得到的目标位置变化信息,实时调整对接航线,并向调整后的对接航线起点航行;③远距离导引,到达对接航线起点后,沿对接航线位置闭环航行;④末端导引,接近目标位置后,提高目标更新速率,使用声学和光学联合导引方式;⑤执行与对接装置对接动作,若完成对接,根据对接装置命令执行下一步动作,若未能完成对接,则重新规划对接路径再次对接。

一种基于声学、光学目标位置解算结果的动态对接路径跟随方法。

按照对接阶段划分由三部分组成,向对接航线起点航行阶段、远距离导引阶段和末端导引阶段。前两个阶段使用光学数据导引修正目标位置及航向,由于光学作用距离的限制,在末端使用声光联合导引。

向对接航线起点航行阶段,auv规划从当前位置向对接航线起点的航行行为,航行控制方法为:

其中δh为当前航向与目标航向差值,函数(包括pid控制策略和航向偏差δh′计算方法)计算当前位置与目标位置连线与当前航线的夹角。在向对接航线起点航行过程中,根据声学信号对目标位置lont和latt(即目标位置经、纬度)进行解算,滤波后更新目标位置。计算出航向误差值后,按照航向pid控制方法进行水平面航行控制。

如图6所示,到达对接航线起点后,与向对接起点位置闭环航行控制策略不同,远距离导引阶段与末端导引阶段均按航迹闭环控制策略航行。航迹闭环控制策略为,在航向pid控制方法的基础上,增加航迹闭环pid控制策略:

其中δhdis为航迹偏航角,δdis为与目标航线距离值。根据与目标航线距离值,解算出航迹偏航角δhdis,由此航向偏差δh′计算方法如下:

δh′=δhdock-δhdis

其中,δh′为航向偏差,由与对接装置入口轴线夹角δhdock与航迹偏差角δdis计算得出。计算出航向误差值后,按照航向pid控制方法进行水平面航行控制。在动态对接过程中,受对接装置自身位置变化和海流等因素影响,对接装置位置及对接装置入口朝向不断变化,对接auv完成新的对接航线更新后,按照新的与目标航线距离δdis′解算向及航迹偏差,实现对接auv向对接航线的跟随,进而实现动态对接。

实施例

在实际对接过程中,对接auv首先搜索目标位置,搜索方案如图7所示,auv沿①~⑤顺序搜索,a表示远距离搜索路径长度,b表示近距离环绕搜索边长的一半,c表示目标实际位置与预知位置的最大偏差。进入对接使命后,开始沿规划的对接路径航行,当auv开始沿搜索使命航行后,搜索使命结束判断循环运行,判断的两个结束条件是:(1)完成全部路径搜索但未获取满足条件的声学信号;(2)获取满了足条件的声学信号。首先是判断是在未获取声学数据的情况下否完成全部的搜索路径,若未完成则判断当前路径是否到达终点,若到达则下达下一条搜索路径,否则继续搜索。

完成目标搜索后,按面向动态对接的自主对接航线规划方法,规划对接航线,并向对接航线起点航行。在向起点的航行过程中,由于对接装置位置在不断变化中,因此需要根据对接装置的位置更新对接航线起点和终点。每收到5组声学数据后,进行中位值滤波,去掉两组极值后取平均后结算目标位置。

到达对接航线起点后,进入远距离导引阶段,根据声学解算数据调整对接航线,沿对接航线航行。远距离导引过程中,每获取十组声学数据,解算目标位置,根据目标位置更新对接航线,更新策略如下:

①range值若有小于15米数据个数大于0,使用range值小于15米数据解算目标位置并更新对接航线,否则进入②;

②若range值小于30米数据个数大于0,使用range值小于30米数据解算目标位置并更新对接航线,否则进入③;

③若range值小于50米数据个数大于5,筛选出range值较小的五组数据进行中位值滤波,去掉两组极值点后解算目标位置,若数据个数小于5大于0,则进行中位值滤波,按比例去掉极值点后解算目标位置。若没有range值小于50米数据,则进入④;

④若range值没有小于50米的数据,则筛选出range值较小的五组数据进行中位值滤波,去掉两组极值点后解算目标位置。

接近对接装置,即进入末端导引阶段后,由于距离对接装置距离较近,使用声学数据解算目标位置更新频率提高,若获取数据range值小于5米数据,则直接使用数据更新目标位置;若数据range值大于5米小于20米,则每三组数据取均值后更新目标位置。当获取光学导引数据后,改为使用光学导引数据更新目标位置。

若对接auv完成对接,进入对接装置中,则根据对接装置发送的指令完成电机停转、充电通讯等相应动作;若未能完成对接,则判断未能完成对接原因,若错过对接装置,则重新规划,执行再次对接,若与对接装置卡住,则执行脱困处理,并通过上报水面控制平台。

当前第1页1 2 
网友询问留言 已有0条留言
  • 还没有人留言评论。精彩留言会获得点赞!
1