[0034] 步骤1.7:确定k7:{k7|0<1-k8-k7ψe_max}、确定λ12:{λ12|0<λ12<1}、确定ε347:{ε347|0<ε347}、确定λ56:{λ56|0<λ56<1}、确定k2:{k2|0
[0035] 步骤1.8:确定λ的范围,使得k8为实数,λ:
[0036]
[0037] 步骤1.9:确定k8的范围,使得满足k5为实数的条件:
[0038]
[0039]
[0040] 步骤1.10:确定k5:
[0041] 步骤1.11:构建优化问题 求解使得k1最小时的ε1、ε2、ε3、ε4、ε5、ε6、ε7,即:
[0042]
[0043] s.t.ε1+ε2=ε12,ε5+ε6=ε56,ε3+ε4+ε7=ε347,
[0044] ε1>0,ε2>0,ε3>0,ε4>0,ε5>0,ε6>0,ε7>0,
[0045] 其中,C2为正常数;
[0046] 步骤1.12:将已确定的参数ki,i=0,…8带入步骤1.4所描述的控制律X、N中;将此控制律X、N作用于误差系统,验证该时变状态反馈控制律是否能够使得跟踪误差渐近收敛,是否能够作为MPC的终端控制律;
[0047] 步骤2:基于MPC的轨迹跟踪控制方法的设计:
[0048] 步骤2.1:设置参考轨迹,设定参考轨迹的初始状态xd(0)和角速度rd,利用虚拟水面船模型生成参考轨迹,设定实际水面船的初始状态为x(0);
[0049] 步骤2.2:设置MPC迭代的时间T/次数Nt、预测时域Tp、采样间隔δ。考虑到实际系统的采样机制,控制输入定义为 系统方程的状态值由四阶-五阶Runge-Kutta算法求解,其中 表示不大于t的最大采样时刻;
[0050] 步骤2.3:设置当前tk时刻MPC约束优化问题的目标函数为:
[0051] 其中,Tp为预测时域,xe(t;tk)为系统在tk时刻对t时刻误差状态的预测,ue(t;tk)为系统在tk时刻对t时刻误差输入的预测,Q为误差状态的加权矩阵,R为输入误差的加权矩阵,P为终端误差状态的加权矩阵,其中,Pe、Qe的具体形式分别为:
[0052]
[0053] 步骤2.4:设置MPC优化问题的状态约束xe∈X、输入约束ue∈U以及终端约束xe(tk+Tp)∈Ω(tk+Tp);
[0054] 步骤2.5:根据模型预测系统未来的动态,求解tk时刻的MPC约束优化问题得到tk时刻预测的最优控制序列,tk时刻优化问题的具体形式为:
[0055]
[0056]
[0057]
[0058]
[0059] 其中,
[0060] 步骤2.6:将第一个最优控制作用于误差系统,直到新的采样时刻tk+1到来时,将新的采样状态作为初始状态构建新时刻tk+1的MPC约束优化问题,滚动迭代直到跟踪控制过程结束。
[0061] 有益效果
[0062] 本发明提出的一种基于模型预测控制的欠驱动水面船轨迹跟踪控制方法,利用Lyapunov直接法和反步法,设计的使得跟踪误差渐近收敛的时变状态反馈控制律,在设计过程中跟踪误差不需要进行坐标变换、设计的Lyapunov函数为整个系统的Lyapunov函数,有着更加明确的物理意义;设计的MPC跟踪控制律将实际输入范围和状态范围作为硬约束,更加符合实际应用;设计的MPC跟踪控制律考虑到了给定的优化指标,可以实现对指定状态的优先跟踪;设计的MPC跟踪控制律考虑了终端约束,能够在满足约束的情况下以较短的预测时域得到使得误差系统稳定(输入状态稳定,Input-to-State-Stability,ISS)的最优控制律,解决欠驱动水面船轨迹跟踪控制中实际约束和给定性能无法满足以及无终端MPC难以保证稳定性的问题。
附图说明
[0063] 图1:Cybership Ⅱ实验船
[0064] 图2:参考轨迹
[0065] 图3:反步法作用下的跟踪轨迹
[0066] 图4:反步法作用下的跟踪误差
[0067] 图5:反步法作用下跟踪误差的范数
[0068] 图6:反步法作用下的控制输入
[0069] 图7:MPC方法作用下的跟踪轨迹
[0070] 图8:MPC方法作用下的跟踪误差
[0071] 图9:MPC方法作用下跟踪误差的范数
[0072] 图10:MPC方法作用下的控制输入
具体实施方式
[0073] 现结合实施例、附图对本发明作进一步描述:
[0074] 本发明提出的一种基于模型预测控制的欠驱动水面船轨迹跟踪控制方法的原理是将跟踪误差模型投影到体坐标系下,利用Lyapunov直接法和反步法设计使得跟踪误差渐近收敛的控制律并结合系统约束获取合适的终端不变集;采样当前时刻系统状态,构建有限时域带终端约束的MPC约束优化问题;求解约束优化问题,得到当前时刻预测的最优控制序列,将第一个最优控制作用于系统;新采样时刻获取新的系统状态并更新MPC约束优化问题,滚动迭代直到跟踪控制过程结束。
[0075] 实施方式主要分为两部分,第一部分是利用Lyapunov直接法和反步法设计使得跟踪误差渐近收敛的控制律;第二部分是将第一部分求得的控制律作为有终端约束MPC的终端控制律,进而选取合适的终端不变集并结合系统模型、系统约束和优化指标构建MPC约束优化问题,具体步骤为:
[0076] 步骤1:终端控制律的设计及相应参数的选取,
[0077] 步骤1.1:假设水面船有着良好的对称性且低速航行,忽略动力学模型中惯性矩阵的非对角项和阻尼矩阵的非对角项及非线性项,考虑参考轨迹是由虚拟水面船模型产生,则实际水面船运动模型 和虚拟水面船运动模型 可分别表示为:
[0078]
[0079]
[0080]
[0081]
[0082]
[0083]
[0084] 其中,x=[x,y,ψ,u,v,r]'为实际水面船的系统状态,xd=[xd,yd,ψd,ud,vd,rd]'为虚拟水面船的系统状态,[x,y,ψ]'为水面船在地面坐标系下的位姿,[u,v,r]'为水面船在载体坐标系下的线速度及角速度,u为实际的控制输入,ud为参考的控制输入。
[0085] 步骤1.2:将误差模型 投影到体坐标系下得到 即:
[0086]
[0087]
[0088]
[0089]
[0090]
[0091]
[0092] 其中,xe=[xe,ye,ψe,ue,ve,re]'为地面坐标系下的跟踪误差在载体坐标系下的投影。
[0093] 步骤1.3:设置水面船的模型参数m11、m22、m33、d11、d22、d33,参照图1,本实施例中使用的模型为CybershipⅡ实验船,该船的驱动机构为尾部的两个推进器,具体参数为m11=25.8、m22=33.8、m33=2.8、d11=0.72、d22=0.89,d33=1.9,最大推力为2N,最大旋转力矩为
1.5N·m。
[0094] 步骤1.4:设置参考轨迹,设定参考轨迹的初始状态xd(0)和角速度rd,利用虚拟水面船模型实现参考轨迹的生成。本实施例中所使用模型的最大前向速度为0.2m/s、最大角速度为0.5236rad/s、最大侧向速度为0.1m/s,本实施例考虑跟踪一圆周轨迹,选取参考角速度rd=0.05rad/s,参考前向速度ud=0.05m/s,参考轨迹的初始状态为xd(0)=[0,0,0,0.05,0,0.05]',参考轨迹参照图2;设定实际水面船的初始状态为x(0)=[-0.0478,-
0.0086,0.0001,0.0553,0.0002,0.0426]'。
[0095] 步骤1.5:选取Lyapunov函数 利用反步法设计可得:在选取合适参数(ki,i=0,…8)的情况下,当实际的控制输入为u=[X,N]'时,能够使得跟踪误差渐近收敛,X、N的具体形式分别为:
[0096]
[0097]
[0098] 其中, 为虚拟角速度误差, 为虚拟角速度, 为虚拟前向速度误差, 为虚拟前向速度。
[0099] 步骤1.6:由步骤1.4可知,选取合适的参数是设计控制律使得跟踪误差渐近收敛的关键部分,定义k0=m22/(k3d22-m22)、k4=m22/(k3m11)、k6=k4-d11/m11+C1、k3=m22/(d22λ)、c=rd(1-k7)ψe_maxm11/d22、
ψe_max=maxψe。
[0100] 步骤1.7:确定k7:{k7|0<1-k8-k7ψe_max}、确定λ12:{λ12|0<λ12<1}、确定ε347:{ε347|0<ε347}、确定λ56:{λ56|0<λ56<1}、确定k2:{k2|0
[0101] 步骤1.8:确定λ的范围,使得k8为实数,λ:
[0102]本实施例中λ=0.9167、正常数C1=1,由此可确定k3=41.4268、k0=11.0105、k4=0.0316、k6=1.0037。
[0103] 步骤1.9:确定k8的范围,使得满足k5为实数的条件:
[0104]本实施例中k8=0.1077。
[0105] 步骤1.10:确定k5: 本实施例中k5=2.2026。
[0106] 步骤1.11:构建优化问题 求解使得k1最小时的ε1、ε2、ε3、ε4、ε5、ε6、ε7,即:
[0107]
[0108] s.t.ε1+ε2=ε12,ε5+ε6=ε56,ε3+ε4+ε7=ε347,
[0109] ε1>0,ε2>0,ε3>0,ε4>0,ε5>0,ε6>0,ε7>0,
[0110] 其中,C2为正常数,本实施例中C2=10,由此可确定k1=85.0826。
[0111] 步骤1.12:将已确定的参数(ki,i=0,…8)带入控制律X、N中并作用于误差系统,在该时变状态反馈控制律作用下的跟踪轨迹参照图3,从中可以看出该控制律有着较好的跟踪效果;跟踪误差参照图4,从中可看出误差系统的状态呈现收敛趋势;跟踪误差的范数参照图5,从中可看出跟踪误差的范数呈现渐近收敛趋势,与设计要求一致;控制输入的范围参照图6,从中可看出实际系统的控制输入在约束范围内。因此,在满足约束的情况下该控制律可使得跟踪误差渐近收敛,能够作为有终端条件MPC的终端控制律。
[0112] 在所述步骤1中,本发明将轨迹跟踪误差投影到体坐标系下,并利用Lyapunov直接法和反步法设计了使得跟踪误差渐近收敛的控制律,并将此控制律作为有终端MPC的终端控制律。
[0113] 步骤2:基于MPC的轨迹跟踪控制方法的实施,
[0114] 步骤2.1:设置参考轨迹,设定参考轨迹的初始状态xd(0)和参考角速度rd,利用虚拟水面船模型实现参考轨迹的生成。本实施例中所使用模型的最大前向速度为0.2m/s、最大角速度为0.5236rad/s、最大侧向速度为0.1m/s,本实施例考虑跟踪一圆周轨迹,选取参考角速度rd=0.05rad/s,参考前向速度ud=0.05m/s,参考轨迹的初始状态为xd(0)=[0,0,0,0.05,0,0.05]',参考轨迹参照图2;设定实际水面船的初始状态为x(0)=[-0.2,-0.2,0,
0,0,0]'。
[0115] 步骤2.2:设置MPC迭代的时间T/次数Nt、预测时域Tp、采样间隔δ,考虑到实际系统的采样机制,控制输入定义为 系统方程的状态值由四阶-五阶Runge-Kutta算法求解,其中 表示不大于t的最大采样时刻。本实施例中Nt=274、Tp=10s、δ=0.5s。
[0116] 步骤2.3:设置当前tk时刻MPC优化问题的目标函数为:
[0117] 其中,xe(t;tk)为tk时刻对t时刻误差状态的预测,ue(t;tk)为tk时刻对t时刻误差输入的预测,Q为状态误差加权矩阵,R为输入误差加权矩阵,P为终端状态误差加权矩阵, 本实施例中R=
diag(0.001,0.001),Qs=diag(0.01,0.005,0.06,0.001,0.005,0.005),Pe、Qe的具体数值分别为:
[0118]
[0119] 步骤2.4:设置MPC优化问题的状态约束xe∈X、输入约束ue∈U以及终端约束xe(tk+Tp)∈Ω(tk+Tp)。本实施例中所使用模型的最大推力为2N,最大旋转力矩为1.5N·m,由此可确定输入约束,输入约束和终端约束分别为:
[0120]
[0121] 步骤2.5:根据模型预测系统未来的动态,求解tk时刻的MPC约束优化问题得到tk时刻预测的最优控制序列,tk时刻优化问题的具体形式为:
[0122]
[0123]
[0124]
[0125]
[0126] 其中,
[0127] 步骤2.6:将第一个最优控制作用于误差系统,直到新的采样时刻tk+1到来时,将新的采样状态作为初始状态构建新时刻tk+1的MPC优化问题,滚动迭代直到跟踪控制过程结束。在模型预测控制的作用下,欠驱动水面船运动轨迹参照图7,从中可看出该控制律有着良好的跟踪性能;跟踪误差参照图8,从中可看出误差状态呈现较快地收敛趋势;跟踪误差的范数参照图9,从中可看出跟踪误差的范数呈现渐近收敛趋势,与设计要求一致;系统控制输入参照图10,从中可看出控制输入皆满足约束。从实施例中可以看出,所设计的基于模型预测控制的欠驱动水面船轨迹跟踪控制方法在满足系统实际约束的情况下以较短的预测时域求解到了保证系统ISS稳定的最优控制律,取得了较好的跟踪效果。
[0128] 在所述步骤2中,结合系统约束和步骤1设计的终端控制律获取了合适的终端不变集,在MPC跟踪控制律的设计中考虑了实际系统的输入约束和状态约束,在MPC跟踪控制律的设计中考虑了指定的优化指标,并且通过构建有限时域带终端约束的MPC约束优化问题得到了使得欠驱动无人船误差系统稳定的最优控制律。