[发明专利]基于直接参数化法的全驱动船舶轨迹跟踪控制方法和装置

权利要求书

1.一种基于直接参数化法的全驱动船舶轨迹跟踪控制方法，其特征在于，包括以下步骤：

步骤S1、根据北东坐标系和船体固定坐标系建立船舶的运动学模型和动力学模型，根据所述船舶的运动学模型和动力学模型得到全驱动船舶的二阶数学模型；其中，北东坐标系用于描述全驱动船舶的位置和姿态角，船体固定坐标系用于描述全驱动船舶运动时的线速度和角速度；

步骤S2、根据所述全驱动船舶的非线性误差模型，得到动态增益观测器对复合扰动进行估计；

步骤S3、根据所述全驱动船舶的二阶数学模型和动态增益观测器估计复合扰动设计轨迹跟踪控制器，所述控制器由基于动态增益观测器的补偿控制器和误差反馈控制器两部分构成，其中，误差反馈控制器是由直接参数化方法所设计；

其中，

步骤S1中，对全驱动船舶进行建模，得到运动学模型和动力学模型的形式如下：

其中，η＝[x,y,ψ]为全驱动船舶在北东参考坐标系下的实际位置误差(x,y)和艏摇角误差ψ组成的向量；R(ψ)表示从船体固定坐标系到北东坐标系的转换矩阵，并且满足R^T(ψ)＝R^-1(ψ)；α＝[u,v,r]为全驱动船舶在船体坐标系下纵荡速度误差u、横荡速度误差v和艏摇角速度误差r组成的向量；M＝M₁+M₂是由刚体惯性矩阵M₁和水动力附加质量矩阵M₂所组成的惯性矩阵；C(α)为科里奥利向心力矩阵；D(α)＝D₀(α)+ΔD(α)由线性水动力系数矩阵D₀(α)和非线性水动力系数矩阵ΔD(α)所组成的水动力系数矩阵；τ＝[τ_u,τ_v,τ_r]为控制输入纵向推力τ_u、横向推力τ_v和艏摇力矩τ_r组成的向量；τ_w＝[τ_wu,τ_wv,τ_wr]为船体固定坐标系下外界环境因素产生的干扰；

步骤S1中，

将运动学模型和动力学模型转化成全驱动船舶的二阶数学模型，具体表达式如下：

其中，φ由M系统惯性矩阵、R(ψ)转换矩阵和位置姿态误差导数向量组成，

将全驱动船舶的二阶数学模型转化成二阶系统形式，具体表达式如下：

其中，为系统矩阵，为输入矩阵，非线性项为系统内部不确定项和外界扰动共同作用产生的复合扰动，u为系统输入；

二阶系统形式中的系统矩阵输入矩阵非线性项和系统输入u具有如下形式：

将系统矩阵输入矩阵复合扰动将其简写为系统矩阵A₂，A₁，A₀，输入矩阵B，复合扰动ξ；

步骤S2中，设船舶期望的运动位姿为η_d和实际的运动位姿η_p，期望轨迹连续且二阶导数项存在，即有W₁(t)＝η_p-η_d，得到非线性误差模型如下：

其中，g(t)＝-ξ，T＝Bu；

建立全驱动船舶的非线性误差模型，使得误差模型收敛至平衡点，即W₁(t)＝W₂(t)＝0；

定义中间变量c如下：

其中，为复合扰动g(t)的估计值，Q为动态增益观测器的增益系数；

建立关于W₂(t)的动态方程如下：

复合扰动的估计值的动态方程为：

步骤S3中，全驱动船舶的轨迹跟踪控制器u表达式如下：

u＝u_c+u_f (9)

其中，u_c是补偿控制器，u_f是误差反馈控制器；

设计基于动态增益观测器的补偿控制器，用来抵消船舶所遭受的复合扰动；

τ_w由一阶马尔可夫随机过程得到，即：

其中，b∈R³为北东坐标系下船舶遭受的外部环境扰动，T_c为时间常数对角阵，n∈R³为零均值高斯白噪声向量，ρ为n的幅值矩阵；

通过动态增益观测器估计复合扰动，即非线性项ξ，补偿控制器u_c具体表达式如下：

设计误差反馈控制器u_f，即：

其中，和分别是误差反馈控制器的增益，通过自由参数矩阵给出；β为外部输入信号。

2.一种基于直接参数化法的全驱动船舶轨迹跟踪控制装置，其特征在于，包括以下步骤：

建模模块，用于船舶的运动学模型和动力学模型，得到全驱动船舶的二阶数学模型；

估计模块，用于根据全驱动船舶的非线性误差模型，得到动态增益观测器对复合扰动进行估计；

跟踪控制模块，用于根据全驱动船舶的二阶数学模型和动态增益观测器估计复合扰动设计轨迹跟踪控制器，其中，所述控制器由基于动态增益观测器的补偿控制器和误差反馈控制器两部分构成，其中，误差反馈控制器是由直接参数化方法所设计；

其中，建模模块根据北东坐标系和船体固定坐标系建立船舶的运动学模型和动力学模型，其中，北东坐标系用于描述全驱动船舶的位置和姿态角，船体固定坐标系用于描述全驱动船舶运动时的线速度和角速度；

船舶的运动学模型和动力学模型为：

其中，η＝[x,y,ψ]为全驱动船舶在北东参考坐标系下的实际位置误差(x,y)和艏摇角误差ψ组成的向量；R(ψ)表示从船体固定坐标系到北东坐标系的转换矩阵，并且满足R^T(ψ)＝R^-1(ψ)；α＝[u,v,r]为全驱动船舶在船体坐标系下纵荡速度误差u、横荡速度误差v和艏摇角速度误差r组成的向量；M＝M₁+M₂是由刚体惯性矩阵M₁和水动力附加质量矩阵M₂所组成的惯性矩阵；C(α)为科里奥利向心力矩阵；D(α)＝D₀(α)+ΔD(α)由线性水动力系数矩阵D₀(α)和非线性水动力系数矩阵ΔD(α)所组成的水动力系数矩阵；τ＝[τ_u,τ_v,τ_r]为控制输入纵向推力τ_u、横向推力τ_v和艏摇力矩τ_r组成的向量；τ_w＝[τ_wu,τ_wv,τ_wr]为船体固定坐标系下外界环境因素产生的干扰；

将运动学模型和动力学模型转化成全驱动船舶的二阶数学模型，具体表达式如下：

其中，φ由M系统惯性矩阵、D₀(α)线性水动力阻尼系数矩阵、R(ψ)转换矩阵和位置姿态误差导数向量组成，

将全驱动船舶的二阶数学模型转化成二阶系统形式，具体表达式如下：

其中，为系统矩阵，为输入矩阵，非线性项为系统内部不确定项和外界扰动共同作用产生的复合扰动，u为系统输入；

二阶系统形式中的系统矩阵输入矩阵非线性项和系统输入u具有如下形式：

将系统矩阵输入矩阵复合扰动将其简写为系统矩阵A₂，A₁，A₀，输入矩阵B，复合扰动ξ。