[发明专利]一种基于模型预测控制的水下机器人路径跟踪控制方法

摘要

本发明提出一种基于模型预测控制的水下机器人路径跟踪控制方法，属于水下机器人运动控制技术领域。该方法首先根据水下机器人的运动学与动力学模型得到离散时间预测模型，然后转化为回归模型，并基于递推最小二乘法在线估计参数；利用上述模型对目标路径点跟踪问题确定优化目标函数及约束条件，采用投影梯度法求取最优控制输入序列，然后对水下机器人执行当前时刻的最优控制输入，并通过不断迭代，使得水下机器人到达每个目标路径点。本方法能够在水下机器人上高效在线实施路径点跟踪控制，且能够处理控制输入的约束，能够在模型存在不确定性和外界干扰的情况下保持一定鲁棒性。

主权项

1.一种基于模型预测控制的水下机器人路径跟踪控制方法，其特征在于，该方法包括以下步骤：1)构建预测模型；具体步骤如下：1‑1)建立水下机器人的运动学与动力学模型；水下机器人连续时间下的运动学与动力学模型表达式如下：其中，η＝[x,y,ψ]^T是水平面坐标系中水下机器人的x轴坐标，y轴坐标和艏向角，v＝[u,v,r]^T是水下机器人本体坐标系中水下机器人的前进速度、侧向速度和偏航角速度，和分别表示上述两个变量的时间导数；J,M,C,D矩阵具有以下形式：其中，d₁(u)＝X_u‑X_u|u||u|,d₂＝Y_v‑Y_uvu‑Y_v|v||v|,d₃＝Y_r‑Y_uru‑Y_r|r||r|,d₄＝N_v‑N_uvu‑N_v|v||v|,d₅＝N_r‑N_uru‑N_r|r||r|；m是水下机器人的质量，I_zz是绕z轴的转动惯量，是附加质量系数，X_u、X_u|u|、Y_v、Y_uv、Y_v|v|、Y_r、Y_ur、Y_r|r|、N_v、N_uv、N_v|v|、N_r、N_ur、N_r|r|是阻力系数；水下机器人三个自由度上的动力及力矩为：其中，ξ是推进器的推力，δ是方向舵的偏航角；μ＝[ξ,δ]T是两个控制输入，Yuuδ、Nuuδ是耦合系数；1‑2)确定控制输入的约束条件，表达式如下：0≤ξ≤ξmax,‑δmax≤δ≤δmax  (3)其中ξmax是推进器的最大推力，δmax是方向舵偏航角的最大幅度；1‑3)将水下机器人的运动学与动力学模型转化为离散时间预测模型；将式(1)和(2)离散化，得到如下离散时间预测模型：其中，Δt＞0为离散化的采样时间间隔，即采样时间区间[ti,ti+1]的长度；i表示第i个采样时刻；将式(4)记为一个通用形式的离散时间预测模型：χ(i+1)＝f(χ(i),μ(i))  (5)其中，系统状态变量为:χ(i)＝[ηT(i),vT(i)]T＝[xT(i),yT(i),ψT(i),uT(i),vT(i),rT(i)]T，控制输入为:μ(i)＝[ξ(i),δ(i)]T；2)模型反馈校正；具体步骤如下：2‑1)推导参数回归模型；将式(5)中包含状态变量和控制输入的耦合项视为独立的新变量，各耦合项前面的系数单独表示出来，得到如下线性回归模型：χ(i+1)＝Φ(χ(i),μ(i))Θ  (6)其中Φ(χ(i),μ(i))是i时刻状态变量和控制输入组合而成的回归矩阵，记为Φ(i)；Θ是式(5)模型中所有参数组成的回归系数向量；2‑2)基于递推最小二乘法在线估计回归模型的参数；对如式(6)所示的回归模型，在第i时刻，采用带有遗忘因子的递推最小二乘法在线估计参数，计算表达式如下：其中，为i时刻参数Θ的估计值，初值由离线辨识得到，P(0)＝α²I，10⁵≤α²≤10¹⁰，遗忘因子0.95≤λ≤0.995；3)基于步骤1)和步骤2)确定的模型，对水下机器人目标路径点跟踪进行在线优化求解；具体步骤如下：3‑1)基于视线导航策略确定优化目标函数；假设水下机器人当前位置坐标为p＝(x,y)，设定的一系列目标路径点为{p1,…,pk,pk+1,…},其中当前目标路径点为第k个路径点pk＝(xk,yk)；视线导航策略中，水下机器人艏向角与视线导航角之间的偏差为零，即||ψ‑ψ_LOS||→0，水下机器人前进速度u被控制到一个稳定速度u_d，即||u‑u_d||→0；||ψ‑ψ_LOS||采用反余弦的向量夹角形式表示，即视线向量与水下机器人艏向向量之间的夹角，如式(8)所示：路径点跟踪的最终目标是使水下机器人达到稳定状态，即ψ＝ψLOS，u＝ud以及v＝r＝0；对应这一目标稳态，从式(1)推导出稳态控制输入为：μd＝[ξd,δd]T＝[Xuud‑Xu|u|ud|ud|,0]T令当前时刻t0＝0，系统的优化目标函数定义为：其中，i表示第i个采样时刻，整数N是预测时间序列长度，正实数ai和bi是平衡角度跟踪误差和速度跟踪误差的权重系数，正实数矩阵Ri＝diag(ri1,ri2)是平衡两个控制输入之间的权重系数矩阵；将式(8)带入式(9)，将优化目标函数中关于系统状态χ(i)和控制输入μ(i)的两部分分开表示如下：其中，l(μ(i))＝ri1(ξ(i)‑ξd)2+ri2δ2(i)系统状态和控制输入满足的以下约束条件：3‑2)判断当前时刻系统状态下优化目标函数J的值是否接近于零：如果是，则在当前时刻采用稳态控制输入μd作为当前时刻的控制输入，并在采样时间区间(t0,t0+Δt]内保持不变，然后进入步骤3‑7)；如果否，则执行步骤3‑3)进行具体的优化求解；3‑3)推导优化目标函数对控制输入变量的偏导数：将状态变量{χ(i),i＝1,…,N}和控制输入{μ(i),i＝0,…,N‑1}联立作为优化变量，通过引入拉格朗日乘子{λ(i),i＝1,…,N}，将包含等式约束的优化目标函数式(10)转化为：J关于{μ(0),…,μ(N‑1)}的偏导数如下式：由状态变量{χ(i),i＝1,…,N}的最优性必要条件，得出如下递推公式：因此，根据式(13)求得{λ(i),i＝1,…,N}，然后代入式(12)，求得优化目标函数对控制输入变量序列的偏导数3‑4)确定当前时刻控制输入序列的初始可行解：在当前时刻t0，对t0‑1时刻求得的最优控制输入序列进行延拓，作为初始可行解，即：其中，为t₀‑1时刻求得的最优控制输入序列，为当前t₀时刻控制输入序列的初始可行解；在路径跟踪开始时刻，水下机器人处于静止状态，此时控制输入序列的初始可行解为全零序列；3‑5)采用梯度投影法迭代求解：从初始可行解开始迭代，在当前可行解处根据式(12)和(13)求取负梯度方向，采用阿米霍Armijo规则确定迭代步长，再将得到的解投影到式(11)中不等式定义的可行集内，得到下一个可行解；对于第一步迭代：其中，s⁰是由Armijo规则确定的迭代步长，[·]⁺表示到式(11)约束集的投影；重复上述迭代过程，直到前后两次迭代的解使得J的下降幅度小于设定的阈值ε∈(0,0.01)，或者迭代步数达到设定的最大值Mmax，迭代终止，得到最优控制输入序列{μ*(0),…,μ*(N‑1)}；3‑6)在当前时刻，对水下机器人施加控制输入μ(t)＝μ*(0),t＝(t0,t0+Δt]，该控制量在采样时间区间(t0,t0+Δt]内保持不变；3‑7)在下一时刻，将该时刻作为新的当前时刻，重新返回步骤2‑2)。