欠驱动自主水下航行器移动式回收控制及视景仿真

谢天奇, 李晔, 姜言清,2, 庞硕, 张文君, 徐雪峰

(1.哈尔滨工程大学 水下机器人技术重点实验室, 黑龙江 哈尔滨 150001; 2.河南省水下智能装备重点实验室, 河南 郑州 450015; 3.中国船舶航海保障技术实验室, 天津 300131)

自主水下航行器(autonomous underwater vehicle,AUV)目前被广泛应用于水下勘测、矿产勘探和海洋测绘等水下作业任务[1]。因此,关于水下航行器的研究也吸引了众多研究人员的关注,水下自主回收技术就是其中一个研究热点[2]。由于AUV的航行时间一直受到其电池容量的限制,借助自主回收技术AUV可以实现随时自动充电,这是一种提高AUV任务执行时间的替代方案;且在水下自主回收技术的帮助下,水下航行器在回收成功后可以采取有线方式高速传输数据。另外,与固定式回收技术相比,移动式回收具有更大的灵活性。例如,水面无人艇可以作为AUV的运输和支持平台,与AUV一同协作完成更复杂更耗时的任务。

由于AUV水下自主回收技术具有重大的研究意义和实用价值,所以水下自主回收技术的理论研究和试验正大力开展[3-7]。文献[3]介绍了一种基于双目摄像机的可靠视觉定位方法,该方法根据双目摄像机中标记点的图像数量在单目和双目组合定位方法之间切换,并进行了水下回收试验,试验证明了AUV可以由不同的起点出发进入固定漏斗形回收站。文献[4]中提出了一种自动布放和回收系统,可满足2种布放回收方案,即移动式与固定式USV布放和回收AUV,海上固定式回收回收试验已证明此系统可用于自主布放和回收AUV。文献[5]提出了一种基于视觉的移动式回收方法,用于在浅水中由一个AUV回收另一个AUV,外场试验验证了提出的视觉导航方法的可靠性。文献[6]和文献[7]中通过仿真验证了AUV对接回收过程中轨迹规划方法的合理性,但移动式对接轨迹规划方法仍然难以解决轨迹规划最优性和实时计算之间的矛盾。文献[8-10]中通过仿真研究了在海流影响的情况下的AUV轨迹跟踪控制问题,但由于文献[10]中期望前向速度公式设计错误导致当AUV位于目标点前方时控制器发散。本文针对文献[10]的不足提出了新的期望速度公式保证AUV顺利完成对接任务。另外,拟通过利用半物理仿真验证AUV移动式对接轨迹规划方法的实时计算能力并通过视景仿真直观地展示移动式回收过程、检验控制算法是否合理。

本文介绍了欠驱动AUV移动式回收任务的流程,设计了一种新的前向速度导引律,并采用基于模型的轨迹跟踪方法来控制欠驱动AUV自主完成移动式回收任务。另外,为了直观展现移动式回收装置与欠驱动AUV的移动式回收过程,本文搭建了半物理仿真系统并利用视景仿真软件Vega实时显示移动式回收场景。

1 任务流程和规划系统

结合图1说明欠驱动AUV完成移动式回收任务的流程。移动式回收任务较为复杂,本文令回收装置随载体作相对简单的匀速直线运动,速度为Ud,AUV上携带超短基线声学定位系统和摄像头,通过声学定位和光视觉定位可得到AUV和回收装置之间的相对位置关系,若此时相对位置关系满足回收成功条件,AUV被固定在回收装置中随载体一起运动,那么此时AUV可断电停止;若未完成回收任务,则可结合船位推算单元计算回收装置的运动方向;然后通过规划制导系统[11]得到下一时刻的期望位置;最后通过控制器和操纵执行机构完成对AUV的自动控制。

图1 移动式回收任务流程

2 轨迹跟踪控制

2.1 建立轨迹跟踪误差方程

六自由度AUV建模可参考文献[11],由于通过推进器、水平舵和垂直舵控制的欠驱动AUV不具备控制横滚角的能力,所以轨迹跟踪控制方法中将横滚角相关项视作模型不确定影响,利用五自由度模型推导控制输入并对六自由度AUV进行控制,其五自由度运动学模型为:

(1)

式中:x、y和z代表大地坐标系下的惯性坐标;θ和ψ是俯仰角和艏向角;u、v、w代表艇体坐标系下前向、侧向和垂向的速度分量;q和r代表俯仰角速度和转艏角速度。

动力学模型:

(2)

|(v2+w2)1/2||r|+Yrur+Y0u2+Yvuv+

(3)

|(v2+w2)1/2||q|+Z0u2+Zwuw+

Zw|w|w|(v2+w2)1/2|+Zww|w(v2+w2)1/2|+

(4)

|(v2+w2)1/2|q+Mwuw+Mw|w|w|(v2+w2)1/2|+

M|w|u|w|+Mww|w(v2+w2)1/2|+M0u2+Mvvv2-

(5)

N|v|r|(v2+w2)1/2|r+N0u2+Nvuv+

Nv|v|v|(v2+w2)1/2|+Nvwvw+Nu2δru2δr)×

(6)

其中:

(7)

(8)

(9)

式中:m是AUV的质量;Ix、Iy和Iz是惯性矩;Xqq、Xrr、Xvr等均为水动力参数;推进力τu,水平舵角δs和垂直舵角δr是可提供的控制输入。

轨迹跟踪误差定义为[10]：

ex=(x-xd)cosγd+(y-yd)sinγd

(10)

ey=-(x-xd)sinγd+(y-yd)cosγd

(11)

ez=z-zd

(12)

对上述轨迹跟踪误差进行微分可得:

(13)

(14)

(15)

2.2 运动学控制器设计

新导引律设计为:

(Δcos(θ-α))

(16)

θd=λ+α

(17)

ψd=γd-φ-χ

(18)

定理1如果能保证u、ψ、θ收敛到他们的期望值附近,那么使用上述导引律 (16)～(18), 可保证位置跟踪误差能够收敛到0附近的一个闭集中。

证明定义:

ue=u-ud

(19)

ψe=ψ-ψd

(20)

θe=θ-θd

(21)

将式 (16)～(21) 代入式 (13)～ (15) 中可得:

(22)

(23)

(24)

式中:

Γx=cosχ(cosψe-1)/ψe+sinχsinψe/ψe

(25)

Γy=cosχsinψe/ψe-sinχ(cosψe-1)/ψe

(26)

Γz=cosλsinθe/θe+sinλ(cosθe-1)/θe

(27)

因为不等式 |sinx/x|≤1和|(cosx-1)/x|<0.73对于任意x都成立, 所以Γx、Γy和Γz的上界为1.73。

(28)

(29)

式中k1是正常数。

将式 (28)～(29) 代入式 (22), 可得

(30)

选择李雅普诺夫函数:

V=(ex2+ey2+ez2)/2

(31)

结合式(23) ～(24)和(30)可求其时间微分为:

(32)

定义:

(33)

(34)

(35)

则式 (32) 变为:

‖E‖‖N‖=-‖E‖(‖E‖‖M‖-‖N‖)

(36)

因此,在有限的时间内跟踪误差向量E的范数有界:

‖E‖≤‖N‖/‖M‖≤λmax(N)/λmin(M)

(37)

式中λmin为M的最小特征值;λmax为N的最大特征值。另外, 当u、ψ、θ收敛到他们的期望值附近时,参数可以被选择可保证位置跟踪误差能够收敛到0附近的一个闭集中。

证毕。

2.3 动力学控制器设计

动力学控制器设计分为2部分,即速度控制器和姿态控制器。

1)速度控制器设计。

为保证速度跟踪误差ue稳定, 选择李雅普诺夫函数:

(38)

结合式 (19) 对其微分:

(39)

如果选择控制输入τu为:

(40)

式中k3为正常数。

则式 (39) 变为:

(41)

2)姿态控制器设计。

为保证速度跟踪误差ψe和θe稳定,选择李雅普诺夫函数:

(42)

结合式 (20)～(21) 对其微分:

(43)

(44)

(45)

式中k4和k5为正常数。

因为虚拟变量rd和qd不是真实控制输入,定义误差变量为:

re=r-rd

(46)

qe=q-qd

(47)

将式 (44)～(47) 代入式 (37) 可得:

(48)

为使误差变量re和qe稳定, 选择如下李雅普诺夫函数:

V3=V2+(re2+qe2)/2

(49)

结合式 (46)～(47) 对其微分可得:

(50)

如果选择控制输入δr和δs为:

(51)

(52)

式中k6和k7为正常数。

则式 (50)变为:

(53)

根据李雅普诺夫稳定性理论可知系统全局一致稳定。

3 半物理仿真系统

如图2所示,半物理仿真系统由规划控制计算机、传输计算机、仿真计算机和以太网组成[12]。

图2 半物理仿真系统

图3 规划控制界面

规划控制计算机承担最大的计算任务,负责计算移动式回收装置的位姿信息,通过TCP网络与传输计算机通信,接收来自仿真计算机的AUV位姿信息,并据此完成规划控制计算,最后将控制指令通过TCP网络通信和传输计算机传递给仿真计算机。

仿真计算机由欠驱动AUV的操纵性仿真模块和基于Vega的视景仿真模块组成,可以模拟欠驱动AUV六自由度运动并将移动式回收装置和AUV的位姿实时展示在建立的海洋环境中。仿真计算机中的软件程序均是在MFC框架下编写,可通过TCP网络与传输计算机通信,接收传输计算机发送过来的回收装置位姿、推进器推力和舵角指令,然后基于欠驱动AUV六自由度运动学及动力学方程内部解算出AUV当前的位置和姿态,反馈给规划控制计算机,使得规划控制计算机根据获得的状态信息完成下一步的解算。同时还需要将当前时刻的AUV位姿信息和回收装置位姿信息通过Vega视景模块不断刷新显示。

传输计算机是半物理仿真系统中的实物部分,为集成了奔腾Ⅲ处理器的PC104工作板,其上安装了实时嵌入式操作系统Vxworks5.5,可通过装有Tornado2.2软件的宿主机进行交叉编译,并将生成的可执行文件下载到传输计算机处理器中。传输计算机主要用于水下航行器的信息中转,实际应用中此处理器可采集各传感器数据供规划控制计算机使用和向下发送控制指令,且存有最基本的控制程序,当规划控制计算机失灵时传输计算机能保证AUV浮出水面等待救援。

3.1 规划控制部分

本部分的用户界面由Matlab软件设计,如图所示,可设置的参数为AUV的初始位置、姿态、速度和移动式回收任务执行的总时间,任务结束时可选择输出基于回转圆的轨迹规划结果和AUV与移动式回收装置的实际轨迹曲线。当鼠标点击“开始”按钮,程序开始执行,先创建全局变量、读入用户界面的位姿、速度与时间设置;再利用了定时器timer函数以固定时间间隔进行计算;使用tcpip函数与传输计算机建立通信并使用fscanf函数和fwrite函数进行数据读写;另外采用自编写的规划函数planning.m和控制函数controller.m函数生成规划指令和控制指令。

3.2 实物传输部分

PC104工作板搭载了嵌入式实时操作系统Vxworks5.5,共包含3个任务,即主循环任务、与规划控制计算机的通信任务和与仿真计算机的通信任务。主循环任务使用了看门狗定时器和信号量实现固定时间间隔进行计算;与规划控制计算机的通信任务中传输计算机作为服务器,规划控制计算机作为客户端;与仿真计算机的通信任务中传输计算机作为客户端,仿真计算机作为服务器。

3.3 运动仿真部分

本文采用实验室在MFC框架下编写的欠驱动水下航行器操纵性仿真软件[13],软件输入为推进器推力、垂直舵角和水平舵角,模块输出为水下航行器的位姿及速度,六自由度运动仿真流程如图4所示。

图4 运动仿真流程

3.4 视景仿真部分

为了直观展现移动式回收装置与欠驱动AUV的水下回收过程,首先进行仿真环境建模,包括海底地形、自主水下航行器、母艇与回收装置,另外在回收装置上加了5个圆球用来模拟光源[14-15]。需要特别注意的是为了突出展现回收装置以及AUV终端对准的情况,在显示时利用Vega下的图形化工具Lynx对母艇进行了缩小操作。在海底地形、水下航行器等视景仿真模型建立完成后,通过Lynx图形界面将模型导入并进行三维场景渲染,借助Lynx图形界面对三维场景进行观察和修改,并为水下航行器和母艇添加player。

图5为欠驱动AUV移动式回收的视景仿真场景,借此可以直观且清晰地观察整个移动式回收过程。为了更好展示整个回收过程,还设置多个观察者视角,包括俯视、侧视、鸟瞰以及2个模拟摄像机的观察者,然后设置了相应的通道,这样就可以在同一个画面中展现出不同的观察者视角。图中间部分为鸟瞰视角,一直处于AUV的后上方,且随AUV移动。侧视视角和俯视视角处于图像左侧,与母艇固定且随母艇移动。当AUV距离回收装置较远时,主要通过鸟瞰视角观察AUV运动情况,当AUV距离回收装置较近时则可以搭配侧视和俯视视角观察AUV与移动式回收装置的相对位置关系。右上角的2个画面用来模拟AUV前端携带的摄像机的拍摄图像,拍摄到的光点可用于光视觉定位。

图5 移动式回收任务仿真场景

最后,除了借助Lynx图形界面进行基本设置以外,还需要通过操纵性仿真软件与Vega模块联合驱动整个视景场景的运行。基于Vega的视景仿真模块每个时间步都会接收来自操纵性仿真软件的AUV和回收装置的位姿信息,通过vgPosVec函数和vgPos函数对AUV和回收装置的player进行重新赋值,并且更新图5中的文字信息,文字信息的内容是回收装置的位置、艏向和AUV的位姿信息。

4 仿真研究

首先对采用不同期望前向速度导引律的轨迹跟踪控制方法进行仿真与比较,文献[10]中采用的期望前向速度导引律(图中标注为控制器1)为:

(54)

轨迹跟踪控制方法对比仿真中采用的期望轨迹方程为:

(55)

仿真结果如图6所示,2个控制器的艏向控制都是正确的,但从图7可以看出,由控制器1控制的AUV的相对位置跟踪误差却越来越大,即当AUV的初始位置位于期望位置前方时,由图8可知方程(54)的结果明显不合理。

图6 轨迹跟踪控制方法对比

图7 相对位置误差

图8 期望前向速度和实际前向速度

然后进行移动式回收任务的半物理仿真实验,回收装置随载体作相对简单的匀速直线运动,假设它的起点为(0,0,-50),运动方向为X轴正向,速度为Ud=1.0 m/s,AUV最大规划速度为ur=1.5 m/s,回收成功条件为纵向垂向距离差小于1 m,艏向角和俯仰角度差小于30°。规划导引系统可参考文献[11], 基于回转圆的轨迹规划方法以仿真实验或外场试验测得的AUV最小回转半径、最小回转周期和最大航行速度作为AUV的运动约束,利用几何知识将欠驱动AUV移动式对接任务的归航轨迹规划为直线与回转圆曲线的组合,采用多个时间步逼近的形式实时预测欠驱动AUV移动式对接任务的最终对接位置和姿态并生成AUV的期望跟踪轨迹。仿真实验表明相比于基于最优控制理论的轨迹规划方法,该方法计算简单、能够满足实时性要求且规划结果更加稳定;相比于固定式对接任务中的点对点制导方法,该方法能够引导欠驱动AUV更快地进入作直线运动的对接装置中。规划导引系统在深度方向采用视线法导引律,在水平面上采用纯跟踪导引律、基于回转圆的轨迹规划方法和视线法导引律相结合的方法,由于纯跟踪导引律和视线法导引律的计算结果随AUV和回收装置位置的变化而变化,所以仿真图中只绘制了由基于回转圆的轨迹规划方法规划出的轨迹。其参数选为d1=100 m,d2=80 m,d3=8 m,R=13.3 m,T=55.8 s,控制器中的参数选为k1=1,k2=0.06,k3=2,k4=2,k5=3,k6=5,k7=4,Δ=8,下面通过3组仿真对AUV移动式回收过程进行说明。

第1组仿真:AUV的起点为(-50,100,-30),艏向角为0。如图9所示,AUV接近移动式回收装置的轨迹分为2部分,第1部分是AUV沿基于回转圆的轨迹规划方法规划出的一段直线移动;第2部分为当AUV进入回收装置正后方矩形区域时由视线法导引从回收装置后方追赶并完成回收任务。

图9 第1组仿真规划的轨迹与实际轨迹

第2组仿真:AUV的起点为(200,200,-30),艏向角为0。如图10所示,AUV接近移动式回收装置的轨迹分为3部分,第1部分是AUV由纯跟踪制导方法导引先做回转运动使其前进方向朝向移动式回收装置,再沿他们间的连线靠近移动式回收装置;第2部分是AUV沿基于回转圆的轨迹规划方法规划出的2段圆弧及一段直线移动;第3部分为当AUV进入回收装置正后方矩形区域时由视线法导引从回收装置后方追赶并完成回收任务。

图10 第2组仿真规划的轨迹与实际轨迹

第3组仿真:AUV的起点为(200,0,-30),艏向角为0。如图11所示,AUV接近移动式回收装置的轨迹分为3部分,第1部分是AUV由纯跟踪制导方法导引先做回转运动使其前进方向朝向移动式回收装置,再沿他们间的连线靠近移动式回收装置;第2部分是AUV沿基于回转圆的轨迹规划方法规划出的2段圆弧移动;第3部分为当AUV进入回收装置正后方矩形区域时由视线法导引从回收装置后方追赶并完成回收任务。

由上述3组仿真实验可以看出欠驱动AUV由不同位置出发均能顺利完成移动式回收任务。本仿真系统能够验证AUV移动式对接规划控制方法,便于直观展示移动式回收过程,并能够与工控机通信验证嵌入式程序是否合理,可有效地节约外场试验时间。

5 结论

1)设计了一种新的前向速度导引律,并采用基于模型的轨迹跟踪方法来控制欠驱动AUV,并完成了AUV由不同位置出发的移动式回收仿真实验。

2)搭建了半物理仿真系统及视景仿真系统实时显示欠驱动AUV水下自主对接回收的过程,验证了所用方法的可行性和实时性。

本文只涉及到了移动式回收装置随载体做匀速直线运动的情况,接下来会继续研究移动式回收装置随载体做匀速圆周运动或跟踪矩形轨迹的情况。