1、本文网址:http:/www.ship- J.中国舰船研究,2024,19(1):145157.JIA J H,HUANG Y,ZHAO W T,et al.Unmanned sailboat trajectory tracking method based on model predictivecontrolJ.Chinese Journal of Ship Research,2024,19(1):145157(in Chinese).基于模型预测控制的无人帆船轨迹跟踪方法扫码阅读全文贾金恒1,2,3,黄琰*2,3,赵文涛2,3,孙朝阳2,3,颜闽秀11 沈阳化工大学 信息工程学院,辽宁 沈
2、阳 1101422 中国科学院沈阳自动化研究所 机器人学国家重点实验室,辽宁 沈阳 1100163 中国科学院 机器人与智能制造创新研究院,辽宁 沈阳 110169摘 要:目的目的面向无人帆船在水面、水下跨域异构海洋机器人的协同作业场景,提出基于模型预测控制(MPC)的无人帆船轨迹跟踪方法。方法方法针对“海鸥”号无人帆船,建立其动力学模型及运动学模型,通过分析无人帆船动力学特点及执行器约束条件,构建 MPC 目标函数及系统约束条件,将无人帆船的轨迹跟踪问题转化为优化问题,并利用 Matlab 软件开展仿真实验验证。结果结果仿真结果表明,与“帆舵”分离的PID 轨迹跟踪控制方法相比,所提出的无人
3、帆船“帆舵”联合 MPC 控制方法更便于添加约束条件,其在风向变化的情况下能以更小的轨迹跟踪误差来更快地收敛于指定轨迹,且其可以实现逆风折线航行。结论结论研究成果可为无人帆船的帆、舵控制提供新的思路,提高其轨迹跟踪能力,进一步为无人帆船与 AUV 的高效协同作业提供技术保障。关键词:无人帆船;轨迹跟踪;模型预测控制;帆舵联合控制;Matlab中图分类号:U664.82文献标志码:ADOI:10.19693/j.issn.1673-3185.03162 Unmanned sailboat trajectory tracking methodbased on model predictive co
4、ntrolJIA Jinheng1,2,3,HUANG Yan*2,3,ZHAO Wentao2,3,SUN Zhaoyang2,3,YAN Minxiu11 College of Information Engineering,Shenyang University of Chemical Technology,Shenyang 110142,China2 State Key Laboratory of Robotics,Shenyang Institute of Automation Chinese Academy of Sciences,Shenyang 110016,China3 In
5、stitutes for Robotics and Intelligent Manufacturing,Chinese Academy of Sciences,Shenyang 110169,ChinaAbstract:ObjectiveAn unmanned sailboat trajectory tracking method based on model predictive control(MPC)is proposed for the collaborative operation of unmanned sailboats in surface/underwater cross-d
6、omainheterogeneous marine robotics scenarios.MethodsUsing the unmanned sailboat Seagull,we establish itsdynamics and kinematics models,analyze its dynamic characteristics and actuator constraints,construct theMPC objective function and system constraints,convert the trajectory tracking problem of th
7、e unmanned sail-boat into an optimization problem and carry out simulation and experimental validation using Matlab software.ResultsThe simulation results show that compared with the sail-rudder separate PID trajectory trackingcontrol method,the proposed sail-rudder joint MPC control method is more
8、convenient for adding constraints.It can converge to the specified trajectory faster with smaller trajectory tracking error under a changing winddirection and realize upwind zigzag sailing.ConclusionThe results of this study can provide new ideas forthe sail and rudder control of unmanned sailboats,
9、improve the trajectory tracking ability and provide a furthertechnical guarantee for the efficient collaborative operation of unmanned sailboats and AUVs.Key words:autonomous sailboats;track tracking;model predictive control;joint sail and rudder control;Matlab收稿日期:20221102 修回日期:20230110 网络首发时间:2024
10、0118 17:40基金项目:国家自然科学基金资助项目(42276198);国家自然科学基金青年科学基金资助项目(42206197,41906173);辽宁省博士科研启动基金计划项目(1645783741584);机器人学国家重点实验室基金资助项目(2020-Z14)作者简介:贾金恒,男,1997 年生,硕士生。研究方向:无人帆船航行控制。E-mail:黄琰,男,1984 年生,博士,研究员。研究方向:海洋机器人技术。E-mail:赵文涛,男,1989 年生,博士,副研究员。研究方向:机器人智能控制及协同作业。E-mail:*通信作者:黄琰 第 19 卷 第 1 期中 国 舰 船 研 究Vol
11、.19 No.12024 年 2 月Chinese Journal of Ship ResearchFeb.2024 0 引言近年来,随着水下无人装备技术和水声通信定位技术的发展,利用水面无人艇(USV)作为自主水下航行器(AUV)的通信定位中继,进而实现脱离母船的无人化协同作业,已经成为一种新的技术趋势1。与依靠燃油作为动力的常规 USV 相比,无人帆船主要依靠风力驱动航行,且可利用太阳能对载荷电池充电,因此具备长时间航行的优点2-3,非常适合与 AUV 协同开展深海调查长期任务(图 1)。在这种典型场景中,无人帆船和 AUV的协同航行问题可以转化为无人帆船对 AUV 声学信标示位轨迹的跟踪
12、问题。岸上基站卫星图 1无人帆船与 AUV 协同作业示意图Fig.1 Collaborative operation for unmanned sailboat and AUV s1s2目前,无人帆船的航行控制算法普遍采用“帆舵”分离控制策略4-6,即采用比例积分微分(proportion intergration differentiation,PID)算法实现舵控调节帆船的航向,并利用速度极曲线7(图 2(a)调节帆角以获得当前视风下的最大航速。该方法适用于无人帆船的大范围巡航过程,其控制策略相对简单,可以保证航速最佳且便于参数整定。然而,上述研究中的 PID 控制器是单输入/单输出的控制
13、器,其在无人帆船的轨迹跟踪过程中未考虑船帆所提供的横向力和艏摇力矩,亦未考虑因速度极曲线的对称性及在同一航向、风速、风向时所带来的同一航速(非最大航速,轨迹跟踪时不要求航速维持最大值)下对应 2 个不同帆角的工况(如图 2(b),正顺风时在和这 2 个帆角下均可获得同一非最大前向力 F),故在此基础上设计的控制器无法保证所得帆角为最佳帆角,将导致帆船需较长的航行时间才能到达稳定状态态(稳态),以及轨迹跟踪精度较差等问题。综Ls1Ds1上,现有控制方法无法解决“帆舵”分离控制所带来的同一速度下对应不同帆角和船帆产生的艏摇力矩对艏向的影响等问题,故需开展基于动力学模型的“帆舵”联合轨迹跟踪控制算法
14、研究。图 2(b)中,和分别为帆产生的升力与阻力。0306090120150180(a)速度极曲线8(b)正顺风帆受力艏向舵风向S1S2F2102402703003304 m/s3 m/s2 m/s1 m/sLS1DS1图 2速度极曲线及正顺风帆受力图Fig.2 Velocity polar curve and force on the positive smooth sail 目前,国内外关于无人帆船轨迹跟踪的文献成果较少,因此本文主要调研了常规 USV 的轨迹跟踪方法。Peng 等8设计了一种基于卡尔曼滤波器的 USV 轨迹跟踪控制器,通过实时更新不确定的船模参数估计值,从而可以保证 US
15、V 的跟踪误差状态变量能够收敛至靠近原点任意小的领域之内;Dong 等9针对欠驱动 USV 在水平面上的轨迹跟踪问题,提出了一种基于状态反馈的反步控制算法,其通过应用改进后的反步法对 USV 系统进行轨迹跟踪并取得了显著效果;张凯10设计了一种基于可变边界层的 USV 滑模轨迹跟踪控制器,该方法利用滑模控制原理结合 USV 运动学神经动态模型设计了 USV 轨迹跟踪控制系统的实际控制律,以减少控制器的计算复杂度,并利用数值仿真验证了控制器设计方法的有效性;吴青等11基于模型预测控制(model predictive control,MPC)方法设计了一种考虑各种约束和环境干扰的船舶146“无人
16、船艇自主性技术”专辑第 19 卷轨迹跟踪控制器,并实现了对控制器跟踪性能的仿真测试;王浩等12设计了一种基于准无限时域MPC 控制的轨迹跟踪控制器,通过应用准无限时域模型预测控制理论,将终端惩罚函数融入到线性模型预测控制方法中,从而使 USV 可以较快地收敛于目标轨迹。相较于常规的 USV,海上时变的风力是帆船的唯一动力源,因此,无人帆船控制面临驱动力不恒定、存在航向死区等问题13,对于无人帆船研究层面而言,其具有动力学模型复杂、约束条件多等难点。为此,基于中国科学院沈阳自动化研究所研制的“海鸥”号海洋观测无人帆船,本文拟建立面向航行控制的三自由度动力学模型,设计一种可实现“帆舵”联合控制的模
17、型预测控制 MPC算法,并将通过仿真实验证明该算法的有效性。1 无人帆船模型建立“海鸥”号无人帆船主要由船体、翼帆、方向舵和龙骨组成,如图 3 所示,在航行过程中,无人帆船的龙骨及舵位于水面以下,而帆及其搭载的通信设备则位于水面以上。帆通信设备龙骨舵(a)三维结构图(b)海试图图 3“海鸥”号无人帆船三维结构图及海试图Fig.3 The 3D structural diagram and trial image for Seagull un-manned sailboat为描述无人帆船的全局运动情况,需建立大地坐标系(惯性坐标系)及体坐标系(运动坐标系),2 种坐标系定义如下:1)大地坐标系
18、OXYZ 固定于地球表面,将其作为基准参考系统,坐标系建立于海平面上。选择海上任一点为坐标原点 O(通常可选择在时间 t=0时刻的船舶重心位置),设定 X 轴指向正北方向,Y 轴指向正东方向,Z 轴指向地心,且该坐标固定不动。2)体坐标系 Gxyz 固定于无人帆船上,选择帆船重心位置为原点 G,设定 x 轴指向船首方向,y 轴指向右舷,z 轴指向龙骨。分析不同的运动特性时,可以建立 3 自由度与 6 自由度的无人帆船运动模型14,针对无人帆船水平面上的轨迹跟踪控制,作出如下理想化假设:1)仅考虑帆船在水平面的运动,即纵荡、横荡和艏摇,忽略垂荡、横摇和纵摇这 3 个自由度;2)无人帆船关于大地坐
19、标系的 XOZ 平面对称,船体不提供艏摇力矩;3)无人帆船的重心、浮心均在 z 轴上;4)桅杆位于无人帆船的重心上方,故船帆主要提供前向力;5)舵位于船体中心线上,其主要提供艏摇力矩;6)龙骨位于无人帆船重心下方,其主要提供横向力。为简化分析,将帆、舵和龙骨都视为均质薄板,合力直接作用于薄板一点,该点即为流体作用中心点;在该点上,薄板上分布的所有力及其产生的总力矩均可用单个力及其产生的力矩予以表示。sr 180,180twvtwLsDsLrDrLkDk基于上述假设,得到如图 4 所示的无人帆船坐标系及受力分析图。其中:G 为帆船重心;为帆角;为舵角;,为艏向角;为真风向;为真风速;和,和,和分
20、别为帆、舵、龙骨所产生的升力和阻力。rsxy东北北XvtwtwLsDsLkLrDkDrG东YO图 4无人帆船坐标系及受力分析图Fig.4 Coordinate system and force analysis diagram of unmannedsailboat第 1 期贾金恒等:基于模型预测控制的无人帆船轨迹跟踪方法147本文将基于日本船舶操纵运动数学模型研究小组(ship manoeuvring mathematical model group,MMG)提出的分离建模方法,应用经验公式15及计算流体动力学(CFD)方法16计算模型参数,进而采用实验数据来验证模型及参数的正确性。分别计算
21、风作用于船帆的力和力矩,以及舵、龙骨、船体与水流的力矩,即可得到三自由度无人帆船数学模型:x=ucosvsin y=usin+vcos=r u=(p1+p3)vr+Xh+Xs(p1 p2)1 v=(p1 p2)ur+Yk+Yh+Yr+Ys(p1 p3)1 r=(p2+p3)uv+Ns+NrNd(p5 p4)1(1)Xh,XsYk,Yh,Yr,YsNsNrNd=p12r|r|式中:x,y 分别为帆船北向位置与东向位置;u,v 分别为前向、横向速度;r 为艏摇角速度;分别为船体、船帆的前向力;分别为龙骨、船体、舵和帆的横向力;,分别为帆、舵的艏摇力矩;,为艏摇阻尼,其中p12为偏航阻尼系数;p1,
22、p2,p3,p4,p5均为帆船模型参数,具体详见表 1。式(1)中的帆、舵、龙骨、船体受力和力矩均根据参考文献 15 进行了简化处理。表 1 帆船的模型参数Table 1 Parameters for the unmanned sailboat model参数数值整船质量p1/kg230前向附加质量系数p223横向附加质量系数p3117.01艏摇附加质量系数p4172.91艏摇转动惯量p5/(kgm2)44.91舵的纵向位置p6/m1.2帆的面积p7/m21.6舵的面积p8/m20.048龙骨面积p9/m20.127空气密度p10/(kgm3)1.2海水密度p11/(kgm3)1 025偏航阻
23、尼系数p12100船体阻力系数p135.825船体阻力系数p149.567船体阻力系数p152.13舵的升阻系数p162.2帆的升阻系数p171.8龙骨升阻系数p181.5帆的纵向位置p19/m0.29YsNs为考虑“帆舵”联合,本文建模时引入了帆的横向力及艏摇力矩,其中帆对艏摇力矩的影响尤为重要。图 5 所示为下文 3.2 节曲线轨迹跟踪仿真工况下帆、舵产生的艏摇力矩对比图,可以看出,帆产生的艏摇力矩不仅更为平缓,可以防止舵轴因转动力矩过大而产生断裂,而且帆产生的艏摇力矩幅值约为舵的 50%,故其对船体转向的作用十分明显。在此工况下使用“帆舵”分离控制时,舵需克服帆带来的力矩扰动,所以增加了
24、轨迹跟踪误差以及舵的工作强度。因此,考虑“帆舵”联合作用的帆船模型将更贴近实船应用场景,通过在设计控制器时有机结合帆与舵,充分利用帆、舵的联合力矩进行艏向控制,即可有效避免轨迹跟踪过程中的大超调问题。0100200300400500600时间/s2520151050510152025艏摇力矩/(Nm1)帆艏摇力矩舵艏摇力矩图 5帆、舵产生的艏摇力矩对比Fig.5 Comparison of yaw moment due to the effect of sail andrudder Xs=Lssinaw DscosawLs=0.5p10p7v2awCLsDs=0.5p10p7v2awCDsva
25、w=v2awu+v2awvvawu=vtwcos(tw)+uvawv=vtwsin(tw)+vaw=arctan2(vawv,vawu)s=awsCLs=0.5p17sin(2s)CDs=0.5p171cos(2s)XhYk,Yh,Yr,Ys,Ns,Nr以帆提供的前向力即为例,其中,帆的升力,帆的阻力,帆的视风速(其中,视风速在前进方向的分量,横向的分量);帆的视风向;帆的攻角;帆的升力系数;帆的阻力系数。模型中其他参数的具体数值含义详见表 1,的详细推导过程见文后附录。2 模型预测控制器(MPC)设计MPC 采用了滚动优化的方法来实现最优控制,该方法不依赖于系统精确模型,可以有效应对无人帆船
26、结构参数变化及复杂海气界面外部环境带来的模型不确定性问题,通过将舵角、帆角等操纵性约束加入到控制过程中,即可用于无人帆船的轨迹跟踪控制器设计。基于式(1)的三自148“无人船艇自主性技术”专辑第 19 卷由度帆船模型,本文将应用“帆舵”联合的控制理念结合 MPC 方法(未改进)进行控制器设计(图 6),从而解决了“帆舵”分离控制中存在的帆舵角度非最优、漂角(航向角与艏向角之差)偏大,以及轨迹跟踪过程中达到稳定状态态的时间较长、超调较大、跟踪精度较低等问题。目标函数风速风向预测输出初始状态参考轨迹+帆船状态信息帆舵联合的线性误差预测模型考虑帆对横向力及艏摇力影响帆角 s舵角 r帆舵约束优化求解帆
27、舵联合的 MPC 控制器的“帆舵”联合控制帆船模型图 6无人帆船轨迹跟踪 MPC 原理图Fig.6 Schematics of trajectory tracking MPC for unmanned sailboat 2.1 模型离散化X=x,y,u,v,rTU=s,rTsrs式(1)的无人帆船三自由度模型为非线性模型,为便于控制器设计,本节将采用近似线性化方法对其进行线性化处理17。在传统控制方法中仅通过控制舵角来控制航向,为了更精确地跟踪轨迹,“帆舵”联合控制不仅考虑了艏向,还考虑了位置和速度,可避免出现较大的跟踪误差及漂角,因此,设定帆船状态量,控制量,其中与是相互耦合的关系,除了可以
28、影响前向速度之外,二者还将共同影响帆船的横向速度及艏向,进而改变帆船的位置。f(X,U)f(XR,UR)f(X,U)整个系统可表示为,采用下标 R 代表轨迹参考量,则参考系统为。将在参考点进行泰勒展开,忽略高阶项,得X=f(XR,UR)+fX?X=XRU=UR(XXR)+fU?X=XRU=UR(UUR)(2)XR式(2)减去,得XXR=X=fX?X=XRU=UR(XXR)+fU?X=XRU=UR(UUR)=AX+BU(3)X=XXRXR=xR,yR,R,uR,vR,rRTUUR=U式中:,为帆船实际状态与参考系统状态量的偏差,其中;,为帆船实际控制量与参考系统输入控制量的偏差;A,B 均为状态
29、矩阵。结合式(1)的无人帆船的三自由度模型,可得状态矩阵 A,B 分别为:A=00vsin()ucos()cos()sin()000vcos()usin()sin()cos()00000010000(p1 p3)rp1 p2(p1 p3)vp1 p2000(p1 p2)rp1 p30(p1 p2)up1 p3000(p2 p3)vp4 p5(p2 p3)up4 p5p12(|r|+sign(r)r)p4 p5B=000000p7p10p17v2awsin(2saw)2(p1 p2)p5p11sin(2r)u|u|p9p7p10p17v2awcos(2saw)2(p1 p3)p8p11p16v2
30、arcos(2r+ar)2(p1 p3)p7p10p17p19v2arcos(2saw)2(p5 p4)|p6|p8p11p16v2arcos(2r+ar)2(p4 p5)第 1 期贾金恒等:基于模型预测控制的无人帆船轨迹跟踪方法149采用文献 18 中使用的近似离散化方法(前向欧拉法)对式(3)进行离散化,得Ak=I+T ABk=T B(4)X(k+1)=AkX(k)+BkU(k)Y(k)=CkX(k)(5)AkBkX(k)X(k+1)U(k)CkY(k)Y(k)=Y(k)YR(k)Y(k)YRCk=I66式中:和均为离散化后的线性时变矩阵;I 为单位矩阵;T 为离散时间步长(即控制周期);
31、k 为任 意 次 采 样 次 数;,分 别 为 第 k 和k+1 次采样时帆船实际位置与期望位置的差值;为第 k 次采样时帆船实际帆角、舵角与期望帆角、舵角的差值(本文期望帆角和舵角均设为0);为单位矩阵;为离散系统输出,即,其中,为实际输出轨迹,为参考轨迹。鉴于无人帆船航行过程中风对帆船的影响不容忽视,其将导致帆船出现较大的漂角,以及将在仅控制位置或仅控制艏向时出现跟踪精度低、跟踪误差及跟踪超调较大等问题,本文在传统无人帆船艏向控制的基础上进行了优化,增加了位置控制及速度控制,并且以无人帆船的位置信息、艏向及速度作为系统输出,所以式(5)中。2.2 未来状态预测U(k)=U(k)U(k1)(
32、k)=X(k)U(k1)TU(k1)X(k)本节将采用增量式状态空间模型,亦即将作为控制输入的增量,以消除因外部干扰以及模型不精确带来的稳态静差19。为了便于添加约束,定义新的系统状态变量(因第 k 次采样时采用了上一时刻的控制量计算结果,而状态量由于使用了传感器采集数据,因此其为实时状态),故新的状态空间模型为(k+1)=Ak(k)+BkU(k)Y(k)=Ck(k)(6)其中:Ak=AkBk026I2,Bk=BkI2,Ck=Ck032NP假设在预测时域内,风速和风向不发生变化,为简化运算,假定如下:Ak+i=Ak,i=1,2,.,NPBk+i=Bk,i=1,2,.,NP(7)由式(6)和式(
33、7)可知,预测输出为P(k)=k(k)+kH(k)(8)其中:P(k)=Y(k+1)|k)Y(k+2)|k).Y(k+NC)|k).Y(k+NP)|k),k=CkAkCkA2k.CkANCk.CkANPkH(k)=U(k|k)U(k+1)|k).U(k+NC1)|k)k=CkBk000CkAkBkCkBk00.CkANC1kBkCkANC2kBkCkBkCkANCkBkCkANC1kBkCkAkBk.CkANP1kBkCkANP2kBkCkANPNCkBkP(k)NPH(k)NCNC NPkkY(k+NP)|k)k+NPU(k+NC1|k)k+NC1式中:为第 k 次采样时在预测时域内的预测输
34、出;为第 k 次采样时在控制时域内的预测输入增量,通常情况下;和均为系统矩阵;为第 k 次采样时预测第次采样时的系统输出值;为第 k 次采样时预测第次采样时的输入增量值,其他参数同理。2.3 目标函数设计J(k)为保证无人帆船能以尽可能小的控制增量来精准跟踪目标轨迹,以无人帆船状态量偏差及其控制量增量来构建目标函数:J(k)=NP1i=1Y(k+i|k)TQY(k+i|k)+NC1j=0U(k+j|k)TRU(k+j|k)(9)式中,Q 和 R 均为权重矩阵,可以根据控制需求予以更改。式(9)右侧第 1 项体现了无人帆船在“帆舵”联合控制器作用下的跟踪效果,第 2 项体现了无人帆船运动是否平稳
35、,即在满足跟踪任务的条件下尽量保证相对较小的控制增量(表示对帆、舵的调整幅度较小),从而在一定程度上减少了系统能耗。结合式(6)和式(8)对式(9)进行变换并去掉150“无人船艇自主性技术”专辑第 19 卷H(k)J(H(k)与无关的项,即可得到标准二次型目标函数:J(H(k)=12H(k)TMkH(k)+GkH(k)(10)Mk=2(TkQek+Re)Gk=2TkQek(k)Qe=diag(Q1,Q2,.,QNP)Re=diag(R1,R2,.,RNC)Q1=Q2=QNPR1=R2=RNC式中,其中,,均采用 diag 函数提取矩阵中的对角线元素,且权重矩阵,。NP无人帆船的帆、舵受限于机械
36、性能影响,其运动范围及速率均存在一定限制,因此需考虑控制量及控制增量的阈值约束。在第 k 次采样时的预测时域内,约束条件可以表示为:Umin H(k)Umax(11)UminWH(k)+U(k1)Umax(12)其中,W=100110.0111NCNCUmin,UmaxUmin,UmaxU(k1)上式中:均表示输入增量约束;均为输入约束;W 为下三角矩阵;为上一时刻的帆角、舵角控制量。综上所述,基于MPC 的无人帆船轨迹跟踪可以描述为式(13)的最优值问题20:argminU(k)J(H(k)s.t.Umin H(k)Umax,UminWH(k)+U(k1)Umax(13)2.4 反馈校正H(
37、k)U(k1)+U(k|k)在每一个控制周期内对式(13)进行二次规划(QP)求解,即可得到控制时域 NC内的输入增量,将作为第 k+1 次采样时无人帆船的帆角、舵角输入,进入下一个控制周期后,循环上述优化过程,直至完成轨迹跟踪过程21。上述过程中,式(13)是基于“帆舵”联合控制的预测值表达式,可以有效地避免“帆舵”分离控制中,因速度极曲线的对称及在同一航向、风速、风向时同一非最大航速对应 2 个不同帆角工况导致的轨迹跟踪过程出现巨大超调的问题(详见 3.1 节的仿真结果)。除了控制艏向之外,该控制器还引入了位置及速度控制,可以避免帆船出现较大的漂角及仅控制位置或仅控制艏向时所出现的跟踪精度
38、低、跟踪误差及跟踪超调较大等问题。3 仿真及结果分析为验证本文基于 MPC 的无人帆船轨迹跟踪控制器的有效性,本节搭建了 Matlab 仿真平台,用以观察其跟踪航迹误差并开展跟踪性能分析。仿真工况如表 2 所示。首先将开展顺风状态下直线和曲线轨迹的跟踪仿真实验,以及逆风状态下的“之”字形折线航行实验,然后将所得的仿真结果与舵、帆分离控制的 PID 控制算法进行对比验证。表 2 仿真工况设计Table 2 Simulation working condition design工况风向航行要求目的工况1顺风,风向变化跟踪直线轨迹验证跟踪直线能力工况2顺风,风向变化跟踪曲线轨迹验证帆船转弯能力工况3
39、逆风,风向固定折线航行验证逆风航行能力 仿真过程中,依据“海鸥”号无人帆船的帆角、舵角以及角速率限制,需对模型预测控制器的控制量、控制增量进行如下约束:10/s10/s H(k)10/s10/s9045WH(k)+U(k1)9045(14)同时,仿真过程中应遵循以下权重设置原则:1)顺风,横向偏移小。顺风状况下,首先保证较小的横向偏移,故设计如表 3 所示的权重设计优先级。其中,优先级越高,代表权重系数更大。表 3 顺风时小横向偏移的权重设计Table 3 Weight design for small lateral offset under downwindconditions目标权重设置
40、xyuvr横向偏移一级优先三级优先四级优先 五级优先前向偏移二级优先三级优先 2)顺风,横向偏移大。在顺风状况下,如果横向偏移较大,为了更快到达参考轨迹,可以将期望艏向设置为当前位置的指向期望位置,并增加艏向权重。3)逆风。逆风时仅进行艏向控制,因此应给予较大的艏向权重。此外,为避免产生较大的漂角,还需设置横向速度权重,令期望横向速度为 0,并在转弯处增加横向速度权重,从而使帆船实现更迅速第 1 期贾金恒等:基于模型预测控制的无人帆船轨迹跟踪方法151地转弯操作。3.1 工况 1Ts=t/T该工况将主要验证无人帆船在存在初始误差工况下的直线轨迹跟踪能力,设定仿真步长 T=0.1 s,仿真时长
41、t=300 s,仿真采样步数,设置参考轨迹如下:if k 0.2R=atan2(yyR,xRx);elseR=0;end(15)k=1,.,Ts y式中:采样次数;为帆船实际位置与参考点的东向位置偏差。180+30sin(0.02kt)(x0,y0,0,u0,v0,r0)=(0,0,0,1,0,0)NP=30NC=4设定风速为 6 m/s,风向为,帆 船 初 始 状 态 为,预测时域,控制时域,则权重系数如下:if y/yR 0.2R=2eye(2);Q=diag(2,8,6,5,0,0);elseR=2eye(2);Q=diag(2,8,2,5,0,0);end(16)eye(2)式中,函数
42、的作用是生成一个大小为2 的单位矩阵。上述仿真过程中,当横向误差较大时,需改变期望艏向并使其指向目标轨迹点,同时增加艏向权重以更快地到达目标轨迹,结果如图 7 所示。在顺风跟踪轨迹点时,若跟踪误差小于 10 m,即认为属于稳定状态。由图 7 可知,本文“帆舵”联合的 MPC 线性模型预测控制器与“帆舵”分离的 PID 控制器均能使无人帆船跟踪给定的直线轨迹,其航向及控制量均能够到达稳定的状态(图 7(c),帆角和舵角可以满足控制量约束,且帆角和舵角转速可满足控制增量约束(图 7(d)。由图 7(a)和图 7(b)可知,存在初始误差时的风向变化工况下,MPC 控制器相对于 PID 控制器在跟踪直
43、线轨迹时的误差幅值更小(约 10 m),且可更快地收敛于给定轨迹。由于速度极曲线的对称 300 200 1000100200300东向位置/m0100200300400500600北向位置/m目标轨迹变风 PID变风 MPC500500204060801001005005005205405605806000510152025跟踪误差/m变风 PID变风 MPC(a)跟踪轨迹(c)艏向角、航向角、风向(d)帆角、舵角(b)与目标轨迹点之间的距离050100150200250300时间/s60402002040角度/()PID 艏向角PID 航向角MPC 艏向角MPC 航向角05010015020
44、0250300时间/s050100150200250300时间/s050100150200250300时间/s050100150200250300时间/s140160180200220风向/()风向角度10050050100帆角/()变风 PID变风 MPC30201001020舵角/()变风 PID变风 MPC图 7直线轨迹跟踪结果Fig.7 Straight-line trajectory tracking results152“无人船艇自主性技术”专辑第 19 卷性,帆船在获得同一前向力时会得到 2 个不同的帆角,这就造成了在最初时刻 PID 控制所求出的帆角并非最佳的帆角(由图 7(d
45、)可知,最初二者所求解得到的帆角在 2 个不同的方向),所以导致到达稳定状态的时间较长;而 MPC 因其具有一定的预测能力,最终提前约 50 s 达到了稳定状态。此外,图 7(a)和图 7(b)中,PID 控制到达稳定状态之前存在一个较大的超调。3.2 工况 2该工况将主要验证无人帆船跟随轨迹转弯的能力,参考轨迹设置如下:xR=2kT;yR=10sin(0.05kT);uR=uref;if y/yR 0.2R=atan2(yyR,xRx);elseR=ref;end(17)uref=sqrt(xR(k+1)2+(yR(k+1)2)xR(k+1)=xR(k+1)xR(k)yR(k+1)=yR(k
46、+1)yR(k)式 中:,其 中,为相邻 2 次采样时间点参考轨迹点的北向距离,为相邻 2 次采样时间点参考轨迹点的东向ref=atan2(xR(k+1)2+(yR(k+1)2)距离;。180+30sin(0.1kt)(x0,y0,0,u0,v0,r0)=(0,0,0,1,0,0)风向为,初始状态,权重系数设置如下:if(y/yR 0.2)&(k 1.6uR=1.6uR 0uR=0R=2eye(2)Q=diag(0,0,10,0,1,0)Q=diag(1,0,10,1,5,0)该工况将主要验证使用 MPC 算法时无人帆船逆风折线航行的能力,设定帆船的初始状态为,风 向 为 0(即北向来风),风
47、速为 6 m/s;设定目标轨迹点朝正北方向移动,移动速度为 0.8 m/s。本节设计了这 2 种不同方向的期望艏向进行折线航行:初始期望艏向 40,设置左右走廊,当目标轨迹东向位置达到 25 m 时,期望艏向变为40;当目标轨迹东向位置达到25 m 时,期望艏向变为 40。假定期望横向速度始终为 0,期望前向速度的设定规则如下:令,若,则取值;若,则取值。权重系数的设定规则为:;当东向位置绝对值大于 25 m 时,则。在此工况下,帆船到达走廊边界进行折向航行时,为减少帆船的转弯时间,需增加艏向权重;为防止帆船产生较大的漂角,需设置期望横向速度为 0,并保留横向速度权重;同时,为避免其他干扰,应
48、设置其他项的权重为 0。逆风航行的仿真结果如图 9 所示。由图可知,无人帆船的北向跟踪误差在 2.2 m 内波动,跟踪效果良好;帆角、舵角及实际艏向角、航向角均呈周期性变化,符合逆风折线航行时的状态量变化趋势。由图 9(b)可以看出,当稳定时,逆风状态比顺风状态的漂角更大,逆风漂角约 8.7,顺风漂角则在 3以内(图 7(c)和图 8(c),这是由于逆风状态下帆船产生了图 9(d)中较为稳定且速度相对较大的横向速度所致。综上所述,本文所提出的“帆舵”联合 MPC控制器是有效的,其比 PID 控制方法的收敛速度更快、跟踪误差更小、轨迹跟踪性能更好,且能实现逆风折线航行。1501005005010
49、0150东向位置/m050100150200北向位置/m走廊边界目标轨迹实际轨迹050100150200250300050100150200250300时间/s50050角度/()期望艏向角艏向角航向角采样时间/s1012北向跟踪误差/m北向跟踪误差60402002040帆角/()帆角050100150200250300时间/s(c)帆角、舵角(a)轨迹(b)艏向角、航向角及北向跟踪误差(d)前向速度、横向速度050100150200250300时间/s050100150200250300时间/s050100150200250300时间/s201001020舵角/()舵角1.01.21.41.
50、61.8前向速度/(ms1)前向速度0.20.100.10.2横向向速度/(ms1)横向速度图 9逆风航行的仿真结果Fig.9 Simulation results of traveling against the wind 4 结语面向无人帆船与 AUV 的协同作业场景,无人帆船需完成对预定轨迹的跟踪,考虑帆舵执行器的约束条件,为解决“帆舵”分离控制下轨迹跟踪过程中无法解决速度极曲线的对称性,以及同航向、风速、风向时同一非最大航速对应不同帆角导致的跟踪误差大、动态响应慢、帆舵角度非154“无人船艇自主性技术”专辑第 19 卷最优等问题,本文设计了无人帆船的“帆舵”联合 MPC 控制器,基于“