2. 中国科学院机器人与智能制造创新研究院,辽宁 沈阳 110016;
3. 中国科学院大学,北京 100049
2. Institutes for Robotics and Intelligent Manufacturing, Chinese Academy of Sciences, Shenyang 110016, China;
3. University of Chinese Academy of Sciences, Beijing 100049, China
水下机器人是探索海洋的重要工具,其主要分为自主水下机器人和遥控水下机器人。为综合自主水下机器人和遥控水下机器人的特点并克服各自的不足,研究人员提出了自主/遥控水下机器人(ARV),它可以在自主、半自主、遥控等不同模式下作为自主水下机器人和遥控水下机器人执行搜索、观察以及作业等任务。鉴于其独特的特点和技术优势,ARV的研发和在海洋观测、探测中的应用成为近年来国际上水下机器人发展的热点[1-2]。
由于ARV既可由操作人员遥控,又具备一定程度的智能和自主。因此,如何在其工作过程中融合人的感知、控制与ARV的自主感知、控制,以实现操作人员遥控和ARV自主控制协调的共享控制,一方面通过ARV的自主控制降低操作人员操作ARV的复杂性、保障其安全,另一方面通过操作人员的监控和辅助提高ARV对环境和任务变化相适应的工作能力,是ARV的高效应用需要研究和解决的重要问题[3-7]。
本文针对ARV在二维水平面运动进行环境探索和目标观察,提出一种基于行为的共享控制方法。针对环境探索,设计了遥控行为和自主避障行为;针对目标观察,设计了人机协同路径跟踪控制行为。通过基于优先级的行为组织和融合方法,实现了ARV基于设计的行为以“人主机辅”模式执行环境探索继而以“机主人辅”模式执行目标观察过程的有效共享控制。
1 控制结构为了便于设计和实现基于行为的共享控制方法,本文采用一种模块化的基于行为的控制结构,由相互串联的行为管理模块、行为模块、行为融合模块、运动控制模块组成。行为管理模块根据控制任务打开或关闭要求的行为模块中的行为,即设置行为是否参与执行给定的控制任务;行为模块为设计的ARV控制行为的集合,各行为的输入为传感器感知信息以及操作人员的控制信息,各行为输出为ARV的期望艏向角ψc和期望前向速度vc;行为融合模块负责融合行为模块中各个行为输出的ψc和vc,并将融合结果输出给运动控制模块;运动控制模块根据行为融合模块输出的ψc和vc,计算ARV舵、推进器等执行机构控制指令,使ARV实现期望的运动。在本文中,运动控制模块采用基于反馈线性化的方法进行ARV的前向速度控制、采用混合模糊P+ID控制方法进行ARV的首向角控制[8]。
近海底环境探索和目标搜寻是ARV所执行的重要任务。执行任务过程中,操作人员需实时控制ARV按操作人员的意图对未知环境进行探索和搜寻感兴趣的目标,因此本文设计了ARV的操作人员遥控行为,操作人员通过控制手柄实时控制ARV的运动速度和方向。由于ARV也具备通过传感器如声呐感知周边环境并基于感知信息进行自主运动控制能力,为进一步保障环境探索过程中ARV在操作人员遥控下的航行安全,本文设计了ARV的自主避障行为。通过遥控行为和自主避障行为的融合,实现环境探索任务的共享控制。
2.1 遥控行为遥控行为实现操作人员通过控制手柄实时控制ARV的航行速度和首向角。该行为的输入为控制手柄在x和y方向的控制输入量Jx和Jy,其中Jx控制ARV的首向角,Jy控制ARV的航行速度,并且Jx∈[−1,1],Jy∈[0,1]。该行为的输出为ARV的期望首向角ψc和期望的航行速度vc。
ψc和vc分别按下式计算:
$ {\psi _c}\left( t \right) = \left\{ {\begin{array}{*{20}{c}} {\psi \left( t \right) + {k_\psi }\dfrac{{{J_x}\left( t \right) - {J_{xd}}}}{{1 - {J_{xd}}}}}\text{,}&{{J_x}\left( t \right) \geqslant {J_{xd}}}\text{,}\\ {{\psi _c}\left( {t - 1} \right)}\text{,}&{{\rm{}}\left| {{J_x}\left( t \right)} \right| < {J_{xd}}}\text{,}\\ {\psi \left( t \right) + {k_\psi }\dfrac{{{J_x}\left( t \right) + {J_{xd}}}}{{1 - {J_{xd}}}}}\text{,}&{{J_x}\left( t \right) \leqslant - {J_{xd}}}\text{,} \end{array}} \right. $ | (1) |
$ {v_c}\left( t \right) = \frac{{{J_y}\left( t \right) - {J_{yd}}}}{{1 - {J_{yd}}}}{v_{\max}}\text{。} $ | (2) |
其中:
由于ARV具有基于声呐等传感器检测环境障碍物并基于感知信息进行自主运动控制能力,因此本文设计ARV自主避障行为,辅助操作人员通过遥控行为控制ARV过程中避免与环境中的障碍物发生碰撞,进一步保障ARV的航行安全。
自主避障行为的输入为ARV和障碍物的当前位置
$ {\psi _c} = {\rm{atan}}\left( {{y_v} - {y_o},{x_v} - {x_o}} \right), $ | (3) |
其中
$ {v_c} = \left\{ {\begin{array}{*{20}{c}} 0\text{,}&{{l_o} \geqslant {l_{\max}}}\text{,}\\ {{k_o}\dfrac{{{l_{\max}} - {l_o}}}{{{l_{\max}} - {l_{\min}}}}{v_{\max}}}\text{,}&{{l_{\min}} < {l_o} < {l_{\max}}}\text{,}\\ {{v_{\max}}}\text{,}&{{l_o} \leqslant {l_{\min}}}\text{。} \end{array}} \right. $ | (4) |
其中:lo=‖Pv-Po‖为ARV与障碍物之间的距离;lmax>0为障碍物对ARV航行安全产生影响的最大距离;lmin>0为保障ARV与障碍物之间安全的距离阈值;ko>0为比例系数;lmax,lmin,ko为行为中的设计参数。
若环境中存在多个障碍物,则自主避障行为的输出为各个障碍物对ARV影响的综合:
$ \left\{ {\begin{array}{*{20}{c}} {{\psi _c} = \angle \left( {\displaystyle\mathop \sum \nolimits_{i = 1}^n {{{V}}_{ci}}} \right)}\text{,}\\ {{v_c} =\left|\left| \displaystyle\mathop \sum \nolimits_{i = 1}^n {{{V}}_{ci}}\right|\right| \;if \;{v_c} > {v_{\max}}{v_c} = {v_{\max}}}\text{。} \end{array}} \right. $ | (5) |
其中:Vci为自主避障行为输出的ARV对第i个障碍物避障的速度向量,其值为vci、方向为ψci;n为环境中对ARV航行安全产生影响的障碍物数量。
2.3 行为融合环境探索任务的共享控制通过遥控行为和自主避障行为的融合实现,即在控制结构的行为融合模块融合遥控行为输出的ψc,vc以及自主避障行为输出的ψc和vc,得到ARV运动控制模块输入的ψc和vc。为了保障ARV的安全,本文设计自主避障行为与遥控行为相比具有高优先级,并设计基于行为优先级的融合方法。
考虑到如下的共享控制要求:1)当ARV与障碍物之间的距离lo小于设定的ARV与障碍物之间安全距离阈值lmin时,自主避障行为需抑制遥控行为,控制ARV自主避障以保障其安全;2)当ARV与障碍物之间的距离lo大于障碍物对ARV航行安全产生影响的最大距离lmax时,自主避障行为无输出,ARV仅在操作人员遥控下运动;3)当ARV与障碍物之间的距离lo大于lmin但小于lmax时,自主避障行为发挥部分作用,对操作人员遥控的ARV运动施加影响,使其远离障碍物。针对该共享控制要求,参考文献[9],本文设计行为输出融合方法如下:
$ {{{V}}_{cf}} = {{{V}}_{ch}}\frac{{{\alpha _h}}}{{{\alpha _f}}} + {{{V}}_{cl}}\frac{{{\alpha _l}\left( {1 - {\alpha _h}} \right)}}{{{\alpha _f}}}, $ | (6) |
其中:∠Vch=ψch,‖Vch‖=vch,∠Vcl=ψcl,‖Vcl‖=vcl,∠Vcf=ψcf,‖Vcf‖=vcf,下标h、l和f分别表示高优先级行为(自主避障行为)、低优先级行为(遥控行为)、和融合后的行为输出结果;
$ {\alpha _h} = \left\{ {\begin{array}{*{20}{c}} 0\text{,}&{{l_o} \geqslant {l_{\max}}}\text{,}\\ {\dfrac{{{l_{\max}} - {l_o}}}{{{l_{\max}} - {l_{\min}}}}}\text{,}&{{l_{\min}} < {l_o} < {l_{\max}}}\text{,}\\ 1\text{,}&{{l_o} \leqslant {l_{\min}}}\text{。} \end{array}} \right. $ | (7) |
αl>0为低优先级行为输出结果加权系数的调节参数(当αl>1时,增加融合结果中遥控行为输出的权重;当αl<1时,增加融合结果中自主避障行为的输出的权重),αl可根据操作人员操作ARV的技术水平以及期望的ARV运动对操作人员遥控的依从度来进行设置。通过式(6)和式(7),即实现了上述的共享控制要求。
3 目标观察共享控制近海底环境探索和寻找到感兴趣的目标后,ARV对目标进行围绕观察也是ARV所需执行的重要任务。由于ARV具有自主运动控制能力,且其控制精度要高于操作人员的遥控。因此,本文设计路径跟踪控制行为实现ARV自主精确的跟踪期望的目标围绕观察路径。然而,由于ARV的自主感知和理解能力限制,其难以在目标观察过程中根据环境、目标的信息和操作人员的观察意图自主调整路径。因此,本文在自主路径跟踪控制中融入操作人员对路径参数及路径跟踪运动的控制,即设计人机协同路径跟踪控制行为,实现在围绕目标观察过程中,以ARV自主路径跟踪控制为主、操作人员调节路径参数及路径跟踪运动为辅助的共享控制,提高ARV执行目标观察路径跟踪过程中对环境、目标以及观测要求变化的适应能力。
3.1 超椭圆路径为了便于对不同形状和尺度目标的围绕观察,本文设计ARV执行超椭圆路径围绕目标进行观察。超椭圆是介于椭圆和矩形之间的一类曲线,具有良好的几何性质,形状可以在椭圆和矩形之间连续变化,其参数方程[10]:
$ \left\{ {\begin{array}{*{20}{c}} {x = a \cdot {\rm{sign}}\left( {\cos\left( t \right)} \right){{\left| {\cos\left( t \right)} \right|}^{\frac{2}{n}}}}\text{,}\\ {y = b \cdot {\rm{sign}}\left( {\sin\left( t \right)} \right){{\left| {\sin\left( t \right)} \right|}^{\frac{2}{n}}}}\text{。} \end{array}} \right. $ | (8) |
其中:
自主路径跟踪控制的输入为ARV需要跟踪的路径P(即超椭圆路径的参数a,b和n),ARV的位置速度等状态信息,输出为ARV的期望艏向角ψc和期望的航行速度vc。
在路径跟踪控制中,vc为操作人员给定的ARV沿路径运动的速度。下面说明本文自主路径跟踪控制中ψc的计算。
由于大多数的水下机器人为欠驱动,因此本文考虑欠驱动ARV的路径跟踪控制。令S为预规划的水平面航行路径P上的运动参考点,该路径在参考点S处的单位切向量T和单位法向量N,在该点处张成直角坐标系SF。ARV以重心G为原点的载体坐标系为Gxyz,绕载体坐标系的z轴旋转漂角β,得到ARV的速度坐标系W,ψWS为从SF坐标系到W坐标系的旋转欧拉角。ARV跟踪路径P,即其速度坐标系以期望速度vc跟踪运动坐标系SF(vc方向跟踪T方向);因此,路径跟踪误差包括速度跟踪误差vc–vt,和在SF坐标系中表示的位置跟踪误差,ARV的重心位置G在SF坐标系中的位置向量
ARV路径跟踪,即控制其速度vt趋于期望速度vc,同时使位置误差和角度误差全局渐进收敛到零。对于欠驱动ARV,不能直接将路径跟踪位置误差用于闭环反馈控制。因此引入趋近角:
$ {\psi _a} = - {\rm{sign}}\left( {{v_t}} \right){\psi _l}\left( {\frac{{2.0}}{{1.0 + {e^{ - {k_N}{l_N}}}}} - 1.0} \right), $ | (9) |
其中
趋近角ψa跟踪误差可以表示为:
$ \begin{split} {\psi _a}\left( t \right) - &{\psi _{WS}}\left( t \right) = {\psi _a}\left( t \right) - \left( {\psi \left( t \right) + \beta \left( t \right) - {\psi _{SF}}\left( t \right)} \right)= \\ &\left( {{\psi _a}\left( t \right) + {\psi _{SF}}\left( t \right) - \beta \left( t \right)} \right) - \psi \left( t \right), \end{split} $ | (10) |
其中:ψSF为SF坐标系T向量在大地坐标系中的角度,因此将
$ {\psi _c}\left( t \right) = {\psi _a}\left( t \right) + {\psi _{SF}}\left( t \right) - \beta \left( t \right)\text{。} $ | (11) |
ARV通过自主路径跟踪控制跟踪超椭圆路径对目标进行围绕观察过程中,操作人员可通过ARV搭载的摄像机等传感器反馈信息实时获得目标的观察信息。因此,操作人员可根据目标观察要求实时调节超椭圆路径以及ARV的路径跟踪运动,实现可根据环境、目标、以及观察需要相应变化的更好目标观察。
本文针对该要求,在上述ARV自主路径跟踪控制基础上,设计人机协同路径跟踪控制行为实现目标观察的共享控制。该行为的输入为ARV需要跟踪的路径P(即超椭圆路径的参数a,b和n,由操作人员根据获取的目标信息确定)、ARV的位置速度等状态信息、以及操作人员通过控制手柄输出的信号Jx和Jy(Jx∈[-1,1],Jy∈[0,1]),行为的输出为ARV的期望首向角ψc和期望的航行速度vc。
设计vc由操作人员通过Jy来实时调节:
$ {v_c}\left( t \right) = \frac{{{J_y}\left( t \right) - {J_{yd}}}}{{1 - {J_{yd}}}}{v_{\max}}; $ | (12) |
设计Jx来实时调节超椭圆路径的a和b参数,即相应的调节ARV与目标的观察距离,当Jx>0时ARV远离目标,当Jx<0时ARV接近目标:
$ a\left( t \right) = \left\{ {\begin{array}{*{20}{c}} {{a_0} + {k_a}\dfrac{{{J_x}\left( t \right) - {J_{xd}}}}{{1 - {J_{xd}}}}}\text{,}&{{J_x}\left( t \right) \geqslant {J_{xd}}}\text{,}\\ {{a_0}}\text{,}&{\left| {{J_x}\left( t \right)} \right| < {J_{xd}}}\text{,}\\ {{a_0} + {k_a}\dfrac{{{J_x}\left( t \right) + {J_{xd}}}}{{1 - {J_{xd}}}}}\text{,}&{{J_x}\left( t \right) \leqslant - {J_{xd}}}\text{,} \end{array}} \right. $ | (13) |
$ b\left( t \right) = \left\{ {\begin{array}{*{20}{c}} {{b_0} + {k_b}\dfrac{{{J_x}\left( t \right) - {J_{xd}}}}{{1 - {J_{xd}}}}}\text{,}&{{J_x}\left( t \right) \geqslant {J_{xd}}}\text{,}\\ {{b_0}}\text{,}&{\left| {{J_x}\left( t \right)} \right| < {J_{xd}}}\text{,}\\ {{b_0} + {k_b}\dfrac{{{J_x}\left( t \right) + {J_{xd}}}}{{1 - {J_{xd}}}}}\text{,}&{{J_x}\left( t \right) \leqslant - {J_{xd}}}\text{。} \end{array}} \right. $ | (14) |
其中:a0,b0为该行为执行时a和b参数的初始值;ka>0,kb>0分别为调节a和b参数变化的增益系数。当a和b参数调节后,该行为由自主路径跟踪控制算法实时计算ψc(t):
$ {\psi _c}\left( t \right) = {\psi _a}\left( t \right) + {\psi _{SF}}\left( t \right) - \beta \left( t \right)\text{。} $ | (15) |
不同于环境探索通过融合遥控行为和自主避障行为来实现共享控制(即共享控制在行为间实现),在目标围绕观察中本文通过人机协同路径跟踪控制一个行为实现了ARV围绕目标观察运动的共享控制(即共享控制在行为内实现)。
4 行为综合管理与融合针对环境探索和目标观察的共享控制需求,本文设计了自主避障行为、遥控行为、和人机协同路径跟踪控制行为,它们置于控制结构的行为模块中。为了对行为模块中各行为的输出进行融合,以上述自主避障行为和遥控行为融合的方法为基础,设计控制结构的行为融合模块中的行为融合方法。
行为融合模块中,对行为模块中的各行为赋予优先级并按优先级从高到底进行顺序排序。以ARV进行环境探索、对目标进行观察、完成任务后返航这一完整作业过程为例,行为优先级从高到低分别为自主避障行为、人机协同路径跟踪控制行为、遥控行为、返航行为(返航行为控制ARV自主返回到期望位置
基于行为的优先级,利用式(6)对各行为的输出进行融合。在自主避障行为中,αh由式(7)进行计算,而其他行为的αh和αl则由操作人员在行为管理模块中根据任务要求进行设置:当人机协同路径跟踪控制行为和遥控行为的αh均设置为0时,则行为融合模块融合自主避障行为和返航行为的输出,ARV自主返航;当遥控行为的αh设置为1时,则其将抑制返航行为,进而融合模块融合自主避障行为和人机协同路径跟踪控制行为输出,实现ARV进行环境探索的共享控制;当路径跟踪行为的αh设置为1时,则其将抑制遥控行为和返航行为的输出,进而融合模块融合自主避障行为和人机协同路径跟踪控制行为的输出,实现ARV进行目标观察的共享控制同时通过自主避障行为保障其安全。
因此,通过行为管理模块对行为融合模块中各行为αh和αl的设置,实现了对行为模块中行为的协调和其输出的融合,使ARV基于设计的行为执行不同的任务。
5 计算机仿真为了对本文提出的基于行为的ARV共享控制方法进行验证,研发了ARV共享控制计算机仿真研究环境。仿真环境主要包括ARV动力学仿真、ARV共享控制、障碍物和目标仿真、以及视景显示4个模块。其中,ARV的动力学模型采用REMUS AUV的六自由度动力学模型[11],ARV的遥控输入来自操作人员控制的图马斯特T.16000M操作杆。
为了验证基于行为的ARV共享控制方法的有效性,本文在仿真环境中进行了仿真实验。仿真中任务分为3个阶段:第1阶段为环境探索,操作人员使用“人主机辅”模式进行环境探索以寻找目标;第2阶段为目标观察,操作人员在发现目标后使用“机主人辅”模式进行观察;第3阶段为返航,ARV在返航行为和自主避障行为的控制下返回出发点。
仿真中ARV的控制周期为0.1 s,仿真区域大小为900×600 m,障碍物为直径40 m的圆形,观察的目标为150×100 m的矩形。基于行为的ARV共享控制方法中的参数设置为:vmax=2.5 m/s,控制手柄阈值Jxd=0.25,Jyd=0.05;遥控行为中
仿真结果如图5所示。图中A点为出发点,黑色圆形代表障碍物,矩形表示目标。曲线为ARV的运动路径,其中AB段为环境探索阶段,BC段为目标观察阶段,CA段为返航阶段。
在环境探索阶段,ARV在“人主机辅”模式下安全的穿过障碍物区域并到达了目标附近,较好实现了操作人员的控制意图。该阶段的仿真时间为0~433.1 s,图6为式(6)中环境探索阶段遥控行为输出Vcl的权值
目标观察阶段开始观察目标的仿真时间段为500~1 000 s,操作人员通过控制手柄修改路径参数,控制手柄x轴的输入信号Jx和ARV到目标的距离变化如图7所示。操作人员输入的路径参数分为3部分,第1部分Jx<Jxd,此时
由图5和图7可以看出,ARV在“机主人辅”模式下观察目标时可以适应目标的形状,同时可以根据操作人员输入的路径参数及时改变ARV的运动轨迹,以保持ARV与目标间相对稳定的距离,便于操作人员对目标进行观察。此外,操作人员在“机主人辅”模式下观察目标时输入的控制信号比较简单,有效降低了操作人员操作ARV的复杂性。
6 结 语针对ARV在近海底环境探索和目标观察中的共享控制需求,采用基于行为的控制思想,提出一种基于行为的ARV共享控制方法。基于设计的遥控行为、自主避障行为及其融合,实现了“人主机辅”模式执行环境探索过程的有效共享控制;基于设计的人机协同路径跟踪控制行为,实现了“机主人辅”模式执行目标观察过程的有效共享控制。通过设计的基于优先级的行为组织和融合方法,实现了ARV基于设计的行为执行环境探索继而目标观察过程的有效共享控制。基于构建的水下机器人共享控制仿真环境对设计的共享控制方法进行的仿真实验,验证了提出的共享控制方法的有效性。
[1] |
李一平, 李硕, 张艾群. 自主/遥控水下机器人研究现状[J]. 工程研究-跨学科视野中的工程, 2016, 8(2): 217-222. |
[2] |
XIANG X, NIU Z, LAPIERRE L, et al. Hybrid underwater robotic vehicles: the state-of-the-art and future trends[J]. Hkie Transactions, 2015, 22(2): 103-116. DOI:10.1080/1023697X.2015.1038322 |
[3] |
REED S, WOOD J, HAWORTH C. The detection and disposal of IED devices within harbor regions using AUVs, smart ROVs and data processing/fusion technology[C]//Waterside Security Conference. 2011.
|
[4] |
WANG Y, SHI Z, WANG C, et al. Human-robot mutual trust in (semi) autonomous underwater robots[J]. Studies in Computational Intelligence, 2014, 115-137. |
[5] |
SPENCER D A, YUE W. SLQR suboptimal human-robot collaborative guidance and navigation for autonomous underwater vehicles[C]. American Control Conference. 2015.
|
[6] |
HENRIKSEN E H, SCHJOLBERG I, GJERSVIK T B, et al. Adaptable joystick control system for underwater remotely operated vehicles[J]. IFAC-Papersonline, 2016, 49(23): 167-172. DOI:10.1016/j.ifacol.2016.10.338 |
[7] |
GARCÍA J C, PÉREZ J, MENEZES P, et al. A control architecture for hybrid underwater intervention systems[C]//IEEE International Conference on Systems. 2017.
|
[8] |
田宇, 张艾群, 李伟. 基于混合模糊P+ID控制的欠驱动AUV路径跟踪控制及仿真[J]. 系统仿真学报, 2012, 24(5): 78-82+7. TIAN Y, ZHANG A, LI W. Path-following control and simulation of underactuated autonomous underwater vehicles based on fuzzy P plus ID control[J]. Journal of System Simulation, 2012, 24(5): 78-82+7. |
[9] |
CARRERAS M, BATLLE J, RIDAO P. Hybrid coordination of reinforcement learning-based behaviors for AUV control[C]//Proceedings 2001 IEEE/RSJ International Conference on Intelligent Robots and Systems, 2001.
|
[10] |
WEISSTEIN, ERIC W. Superellipse[DB/OL]. MathWorld-A Wolfram Web Resource, 2016.
|
[11] |
PRESTERO T. Verification of a six-degree of freedom simulation model for the REMUS autonomous underwater vehicle[D]. Massachusetts Institute of Technology-Woods Hole Oceanographic Institution, 2001. https://www.researchgate.net/publication/37913361_Verification_of_a_six-degree_of_freedom_simulation_model_for_the_REMUS_autonomous_underwater_vehicle
|