[发明专利]一种海洋机器人轨迹跟踪控制结构的设计方法

摘要

本发明公开了一种海洋机器人轨迹跟踪控制结构的设计方法，所述的结构包括滚动时域优化控制模块、动力学估计模块、运动学估计模块、纵向转换控制模块、艏摇方向转换控制模块和海洋机器人。本发明通过滚动时域优化控制模块对海洋机器人的动态进行在线滚动优化和反馈校正，避免了海洋机器人产生超出约束范围的动态，提高了整个轨迹跟踪结构的控制性能，并降低了跟踪误差，可以高效的控制海洋机器人的输出动态。本发明通过采用估计模块对由海洋机器人动力学建模不确定性与环境中带来外部扰动组成的集总不确定性进行估计，之后把估计值作为补偿来抵消，从而实现消除扰动，有效的降低了控制结构的计算负荷，提高了控制结构的抗干扰能力，控制性能良好。

主权项

1.一种海洋机器人轨迹跟踪控制结构的设计方法，其特征在于：所述的结构包括滚动时域优化控制模块、动力学估计模块、运动学估计模块、纵向转换控制模块、艏摇方向转换控制模块和海洋机器人；所述的滚动时域优化控制模块包括速度组合预测模块、位置预测模块和优化选择模块，所述的位置预测模块输入端与速度组合预测模块的输出端相连，优化选择模块的输入端与位置预测模块的输出端相连；所述滚动时域优化控制模块内部的位置预测模块的输入端分别与海洋机器人的输出端和运动学估计模块的输出端相连；所述动力学估计模块的输入端分别与纵向转换控制模块的输出端、艏摇方向转换控制模块的输出端和海洋机器人的输出端相连；所述运动学估计模块的输入端与海洋机器人的输出端相连；纵向转换控制模块的输入端分别与滚动时域优化控制模块内部的优化选择模块的输出端、动力学估计模块的输出端和海洋机器人的输出端相连；艏摇方向转换控制模块的输入端分别与滚动时域优化控制模块内部的优化选择模块的输出端、动力学估计模块的输出端和海洋机器人的输出端相连；所述的海洋机器人的运动学和动力学模型用下式表示：x、y、ψ分别代表地球坐标系下的海洋机器人的横坐标、纵坐标、航向角；u、v、r分别代表本体坐标系下的海洋机器人的纵向速度、横向速度和艏摇角速度；v_x、v_y分别代表地球坐标系下的海洋环境中的x、y方向的海流速度；m_u、m_v、m_r分别代表海洋机器人纵向、横向方向上的质量以及艏摇角方向上的惯量；τ_u、τ_r分别表示海洋机器人纵向的推力和艏摇角方向上的力矩，由于海洋机器人是欠驱动系统，没有横向推力，所以τ_v＝0；τ_wu、τ_wv、τ_wr分别为海洋机器人的纵向速度、横向速度、艏摇角速度方向上外界干扰；d₁₁、d₂₂、d₃₃分别是海洋机器人在纵向速度、横向速度、艏摇角速度方向上的阻尼参数；σ_u、σ_r分别表示输入到海洋机器人的纵向和艏摇角方向上的控制电压信号；b_u、b_r分别表示海洋机器人在纵向电压和推力以及艏摇角方向电压和力矩的比例系数；所述的设计方法，包括以下步骤：A、滚动时域优化控制模块的设计滚动时域优化控制模块的输入信号为海洋机器人的地球坐标系下的横坐标x、纵坐标y、航向角ψ以及运动学估计模块输出的海洋机器人在地球坐标系下受到的海流和本体横向上的扰动观测值滚动时域优化控制模块的输出信号包括期望的艏摇角速度r和期望的纵向速度u；在滚动时域优化控制模块的数学仿真中,连续模型不可直接用于预测，因此需要将其进行离散化；其中海洋机器人的连续模型表示为：其中，取采样时间为T，根据连续模型(2)式，得如下离散状态空间模型，即滚动时域优化控制模块中预测方程表示为：其中x(k+1)、y(k+1)、ψ(k+1)分别代表k+1时刻时的地球坐标系下的海洋机器人的横坐标、纵坐标、航向角；u(k)、r(k)分别代表k时刻时的本体坐标系下的海洋机器人的纵向速度、艏摇角速度；x(k)、y(k)分别表示k时刻时地球坐标系下的海洋机器人的横坐标、纵坐标；因此构造代价函数表示为：其中N_p是预测时域；J、x_p、y_p分别表示预测与虚拟目标的距离误差、虚拟目标坐标x、虚拟目标坐标y，i＝1、2、3、…；因此所述的滚动时域优化控制模块通过(4)式，将海洋机器人的轨迹跟踪问题转化为求解如下的优化问题P：其中，U₁、U₂分别为u、r的约束条件；结合式(3)最终求得如下在线滚动时域优化决策得到最优期望速度和航向序列：u^*(k)＝(u(k+1),u(k+2),···,u(k+n))；r^*(k)＝(r(k+1),r(k+2),···,r(k+n))式中，n是预测步数，将最优期纵向速度和望艏摇角速度序列的第一组元素u(k+1)、r(k+1)作用于海洋机器人，更新海洋机器人的状态及轨迹，并进行迭代；B、动力学估计模块的设计动力学估计模块的输入信号为海洋机器人在地球坐标系下的横坐标x、纵坐标y、航向角ψ，本体坐标系下的速度信号u、v、r，纵向转换控制模块输出的控制电压信号σ_u以及艏摇方向转换控制模块输出的控制电压信号σ_r；动力学估计模块的输出信号是海洋机器人在纵向、艏摇方向上产生的不确定性扰动的估计值所设计的动力学估计模块表示为：式中：其中是位置x方向的观测值，是位置y方向的观测值，是航向角ψ的观测值；其中是纵向速度u的观测值，是横向速度ν的观测值，是艏摇角方向速度r的观测值；其中是纵向不确定性的观测值，是横向不确定性的观测值，是艏摇方向不确定性的观测值；是η₁的估计值，η₁是海洋机器人的位置姿态信息，表示为η₁＝[x y ψ]^T；是ν₁的观测值，ν₁是含约束无人船的速度信号，表示为ν₁＝[u v r]^T；R是旋转矩阵；σ₁是控制输入电压信号，表示为σ₁＝[σ_u 0 σ_r]^T；M是惯性矩阵；K₁、K₂、K₃是扰动估计模块的增益矩阵；C、运动学估计模块的设计运动学估计模块的输入信号为海洋机器人在地球坐标系下的位置和航向信号x、y、ψ与本体坐标系下的纵向速度信号u；运动学估计模块的输出信号包括对海洋机器人观测的x方向海流与横向速度造成的扰动的观测值y方向海流与横向速度造成的扰动的观测值所设计的速度扰动估计模块表示为：式中：其中是位置x方向的观测值，是位置y方向的观测值；其中是x方向海流与横向速度造成的扰动的估计值，是y方向海流与横向速度造成的扰动的估计值；是η₂的估计值，η₂是海洋机器人的位置姿态信息，表示为η₂＝[x y]^T；ν₂是海洋机器人的纵向速度信息，表示为ν₂＝u；J是旋转矩阵；K₄、K₅是运动学估计模块的增益矩阵；D、纵向转换控制模块的设计纵向转换控制模块的输入信号是海洋机器人纵向速度的期望值u_r、海洋机器人实际纵向速度u、动力学估计模块输出的纵向不确定性扰动的估计值在不考虑电机特性的情况下，海洋机器人的速度的响应模型为：其中，f_u代表纵向不确定性及外部扰动；b_u代表控制增益系数；σ_u代表纵向控制输入电压信号；其中f_u已经由动力学估计模块估计得到；因此所设计的纵向转换控制模块表示为：其中，k₁代表纵向线性控制规律控制增益系数；σ_u代表纵向的控制电压信号；b_u代表纵向控制电压信号和纵向推力的比例系数；E、艏摇方向转换控制模块的设计艏摇方向转换控制模块的输入信号是海洋机器人艏摇角速度的期望值r_r、海洋机器人实际艏摇角速度r、动力学估计模块输出的艏摇方向不确定性扰动的估计值在不考虑电机特性的情况下，海洋机器人的速度的响应模型为：其中，f_r代表艏摇角方向的不确定性及外部扰动；b_u代表控制增益系数；σ_r代表艏摇角方向控制输入电压信号；其中f_r已经由动力学估计模块估计得到；因此所设计的艏摇方向转换控制模块表示为：式中，k₂代表艏摇方向线性控制规律控制增益系数；σ_r代表艏摇角方向控制电压信号；b_r代表艏摇角方向的控制电压信号和艏摇角方向的力矩的比例系数。