一种基于非线性观测器的自主水下航行器运动控制方法
【专利摘要】一种基于非线性观测器的自主水下航行器运动控制方法，涉及自主水下航行器(AUV)运动控制技术领域。本发明是为了增强自主水下航行器对环境扰动的鲁棒性，提供控制系统的动态性能指标。包括以下步骤：步骤1、建立自主水下航行器的六自由度数学模型；步骤2、设计跟踪微分器获取期望位置信息的跟踪信息以及微分信息；步骤3、设计非线性观测器利用传感器测量得到的位置信息观测出自主水下航行器的速度、干扰状态信息；步骤4、设计控制器利用跟踪微分器以及非线性观测器获得的信息得到执行机构所需控制量；步骤5、执行机构作用于受控对象自主水下航行器，使自主水下航行器运动到设定的期望位置。本发明适用于自主水下航行器运动控制。
【专利说明】
一种基于非线性观测器的自主水下航行器运动控制方法
技术领域
[0001] 本发明涉及自主水下航行器(AUV)运动控制技术领域。
【背景技术】
[0002] 自主水下航行器可以在水下承担情报收集、侦察、反潜、水下武器对抗、应急救生 与打捞、危险条件下作业等多种任务，被广泛应用于军事、海洋环境勘测、海底资源开发等 领域。自主水下航行器在水下自治工作，具有水下活动范围大、机动性好、安全、结构简单等 优点，成为当前世界各国海洋开发、国防工业部门、和海洋装备的最重要研究方向之一。
[0003] 自主水下航行器要实现在水下自治式工作，其最主要的设计部分为控制系统的设 计，高性能高精度的运动控制是提升自主水下航行器行为能力的重要指标之一。随着对自 主水下航行器控制系统性能指标的要求越来越高，自主水下航行器已经从单纯的反馈控制 发展为非线性、鲁棒、智能控制等先进控制方法的阶段。自主水下航行器是一个强耦合、存 在模型参数摄动、易受外界环境干扰力影响的强非线性系统，需要采用高效的控制策略方 能满足其对高精度控制性能指标的需求。非线性观测器是提高耦合、扰动系统鲁棒性的有 效手段，已被广泛应用于船舶动力定位系统、磁悬浮列车悬浮系统、机器人关节伺服控制系 统等，以实现尚速、尚精度的跟踪控制、定位控制等目标。非线性观测器利用有效的测量状 态，实现状态反馈控制中所需的不可测状态的观测，估计环境干扰力并对环境干扰力进行 补偿，以提升系统的控制性能。同时，非线性观测器对由测量元件引入的测量噪声还具有较 好的滤波效果。
[0004] 因此，设计一种基于非线性观测器的自主水下航行器运动控制方法具有重要的作 用价值。
【发明内容】
[0005] 本发明是为了增强自主水下航行器对环境扰动的鲁棒性，提供控制系统的动态性 能指标，从而提供一种基于非线性观测器的自主水下航行器运动控制方法。
[0006] -种基于非线性观测器的自主水下航行器运动控制方法，它包括以下步骤：
[0007] 步骤一、建立自主水下航行器的六自由度数学模型；
[0008] 自主水下航行器的数学模型建立在北东坐标系和运动坐标系下，在北东坐标系下 定义其位置矢量，运动坐标系下定义其广义速度矢量;建立在两坐标系下的运动学和动力 学数学模型为：
[? = J{Q)V …
[0009] ⑴
[MV = -Cpii(V)V - r. {V)V - 1){V)V - g{?) + b + T
[0010] 其中，n为定义在固定坐标系下自主水下航行器的位置矢量矩阵，V为定义在运动 坐标系下自主水下航行器的广义速度矢量矩阵，J( ? )为自主水下航行器在北东坐标系下 的速度与运动坐标系下的广义速度之间的转换矩阵，M为惯性矩阵，Crb(V)为刚体科里奥利 向心力矩阵，Ca(V)为流体产生的科里奥利向心力矩阵，D(V)为阻尼力矩阵，g( Θ )为重力和 浮力产生的阿基米德回复力和力矩矩阵，b为环境干扰力矩阵，τ为控制输入；
[0011] 步骤二、利用跟踪微分器获取期望位置信息的跟踪信息以及微分信息；
[0012] 步骤三、利用非线性Luenberger观测器利用传感器测量得到的位置信息观测出自 主水下航行器的速度和干扰状态信息；
[0013] 步骤四、控制器利用跟踪微分器以及非线性Luenberger观测器获得的信息得到执 行机构所需控制量；
[0014] 步骤五、执行机构作用于受控对象自主水下航行器，使自主水下航行器运动到设 定的期望位置，完成一次自主水下航行器运动控制。
[0015] 步骤二至四为基于非线性Luenberger观测器的自主水下航行器定深控制步骤，具 体为：
[0016]步骤Α、跟踪微分器对期望深度指令进行微分，得到期望指令的跟踪信息X和微分 信息V;
[0017] 步骤Β、非线性Luenberger观测器利用传感器测量得到的位置信息τι观测出航行器 在运动坐标系下的广义速度V;
[0018] 步骤C、三阶非线性扩张状态观测器利用非线性Luenberger观测器观测到的深度 信息i观测出航行器的深度21以及定义在北东坐标系下的下潜速度4;
[0019] 步骤D、将跟踪微分器获得的期望深度跟踪信息X和微分信息V分别与三阶非线性 扩张状态观测器观测到的深度信息21以及定义在北东坐标系下的下潜速度4的差值作为 控制器的比例和微分输入，经非线性PD控制率获得输出控制信号U0;把非线性Luenberger 观测器观测到的下潜速度信息％作为二阶非线性扩张状态观测器的输入，观测出航行器在 下潜方向的总扰动；
[0020] 步骤E、将控制信号Uo与总扰动的差值作为最后升降舵的控制输入u。
[0021 ] 步骤B中所述的非线性Luenberger观测器为：
[0022]
(2)
[0023] 其中，β为非线性扩张观测器的可调参数;g(0)为重力加速度分量;片为对η的观测 值，#为对V的观测值，^为对环境干扰b的观测值4 = /7 -々为位置观测误差；
[0024] J)为饱和函数，其具体形式如下：
[0025]
(3)
[0026I α和δ为饱和函数的待设计参数。
[0027]步骤C中，三阶非线性扩张状态观测器为：
[0028]
(4)
[0029] 其中，e为下潜深度观测误差；；i为非线性Luenberger观测器观测出的下潜位移状 态，Z1为对I的观测值，Z2为对為的观测值;Z3为对下潜方向所受总和干扰的估计值;P 1、β2、β3 为非线性扩张观测器的可调参数。
[0030] 击3Hn由一阶韭錄桦扩张状态观测器为：
[0031]
(5)
[0032] 其中，ev为下潜速度观测误差；#为非线性Luenberger观测器观测出的下潜速度状 态，Zn为对#的观测值，Z22为对下潜方向总干扰的观测值;βη、β12为非线性扩张观测器的可 调参数。
[0033]步骤四中所述的控制器为非线性ro控制器：
[0034] u〇 = kPfal(ei,ap,5k)+kdfal(e2,ad,5k) (6)
[0035] 其中，ap、ad、5k为饱和函数的待设计参数，ei = χ-ζι，e2 = v_Z2, kP、kd为控制器的比 例和微分设计参数。
[0036]步骤E中最后升降舵的控制输入u为：
[0037] u = u〇-Z22/b (7)
[0038] 其中，UQ为纵向速度控制常量;Z22为非线性Luenberger观测器对自主水下航行器 定深控制方向总扰动的观测值，b为环境干扰力矩阵。
[0039] 本发明通过非线性Luenberger观测器实现对自主水下航行器速度、总扰动的准确 观测，通过控制率对总扰动的实时补偿，使受控对象成为积分串联型系统，不仅提高了控制 系统的动态性能指标，而且增强了系统对环境扰动、模型参数摄动的鲁棒性。
【附图说明】
[0040]图1是基于非线性Luenberger观测器系统的自主水下航行器垂直面控制系统结构 示意图；
[0041 ]图2是控制系统的控制流程示意图；
[0042]图3是非线性Luenberger观测器对自主水下航行器下潜位移、纵倾角实际值与观 测值曲线仿真示意图；
[0043]图4是非线性Luenberger观测器对自主水下航行器下潜速度、纵倾角速度实际值 与观测值曲线仿真示意图；
[0044]图5是非线性Luenberger观测器对两坐标系下自主水下航行器下潜速度观测值曲 线仿真不意图；
[0045]图6是非线性Luenberger观测器对自主水下航行器垂荡方向总和干扰观测值曲线 仿真不意图；
[0046]图7是PID和基于非线性Luenberger观测器的自抗扰控制下自主水下航行器升降 舵舵角曲线仿真示意图；
[0047]图8是PID和基于非线性Luenberger观测器的自抗扰控制下自主水下航行器下潜 位移响应曲线仿真不意图；
[0048]图9是非线性Luenberger观测器对含测量噪声影响的测量值滤波效果曲线仿真示 意图；
【具体实施方式】
【具体实施方式】 [0049] 一、一种基于非线性观测器的自主水下航行器运动控制方法，本发 明以自主水下航行器定深控制为研究对象，提出了一种基于非线性观测器的自主水下航行 器运动控制方法。该控制系统中非线性Luenberger观测器不仅可以对系统的全维状态进行 观测，而且可以估计出系统的总和扰动，同时对由测量元件引入的测量噪声有着较好的滤 波效果。
[0050] 下面结合附图对本发明作进一步详细说明。
[0051 ] (1 )、考虑实际受控对象的特点，建立自主水下航行器的数学模型；
[0052]自主水下航行器是一个空间的六自由度运动载体，为方便描述其运动，其数学模 型通常建立在北东坐标系和运动坐标系下。在北东坐标系下定义其位置矢量，运动坐标系 下定义其广义速度矢量。建立在两坐标系下的运动学和动力学数学模型为：
[0053]
CO
[0054] 其中，η为定义在固定坐标系下自主水下航行器的位置矢量矩阵，V为定义在运动 坐标系下自主水下航行器的广义速度矢量矩阵，J( Θ)为自主水下航行器在北东坐标系下 的速度与运动坐标系下的广义速度之间的转换矩阵，M为惯性矩阵，Crb(V)为刚体科里奥利 向心力矩阵，Ca(V)为流体产生的科里奥利向心力矩阵，D(V)为阻尼力矩阵，g( Θ )为重力和 浮力产生的阿基米德回复力和力矩矩阵，b为环境干扰力矩阵，τ为控制输入。
[0055] (2)、建立基于非线性Luenberger观测器的自主水下航行器定深控制系统设计方 案；
[0056]本发明中，自主水下航行器的位置矢量由传感器测量得到。控制系统的基本控制 结构及控制系统工作流程图如图1和2所示:跟踪微分器对期望深度指令进行微分，得到期 望指令的跟踪信息X和微分信息V;非线性Luenberger观测器利用传感器测量得到的位置信 息η观测出航行器在运动坐标系下的广义速度V;三阶非线性扩张状态观测器利用非线性 Luenberger观测器观测到的深度信息?观测出航行器的深度Z 1以及定义在北东坐标系下的 下潜速度免；将跟踪微分器获得的期望深度跟踪信息X和微分信息V分别与三阶非线性扩张 状态观测器观测到的深度信息 21以及定义在北东坐标系下的下潜速度免的差值作为控制 器的比例和微分输入，经非线性PD控制率获得输出控制信号uo;把非线性Luenberger观测 器观测到的下潜速度信息兔作为二阶非线性扩张状态观测器的输入，观测出航行器在下潜 方向的总扰动;将控制信号UO与总扰动的差值作为最后升降舵的控制输入u。
[0057] (3)、非线性Luenberger观测器的设计：
[0058] 设计非线性Luenberger观测器的目的是为了观测出航行器在北东坐标系下的下 潜速度信息以及深度控制方向的总扰动。要想观测出航行器在深度控制方向的总扰动，首 先要获得航行器在运动坐标系下的广义下潜速度信息。
[0059] a、为获得航行器在运动坐标系下的广义下潜速度信息，对自主水下航行器的运动 数学模型（1)设计如下形式的非线性Luenberger观测器：
[0060]
(2)
[0061 ] 其中，为对η的观测值，#为对V的观测值，I为对环境干扰b的观测值，= 77-巧为 位置观测误差。>/(象A而为饱和函数，其具体形式如下：
[0062]
(3)：
[0063] α和δ为饱和函数的待设计参数。
[0064]非线性Luenberger观测器(2)利用系统的标称模型信息重构了原系统，当々趋于零 时，即可实现对原系统位置、广义速度以及环境干扰的观测。
[0065]附图3和4给出了非线性Luenberger观测器对自主水下航行器下潜位移、纵倾角、 下潜速度以及纵倾角速度的实际值与观测值曲线，附图9为含测量噪声影响的位置测量值 及其经非线性Luenberger观测器滤波后的观测值曲线。可见，非线性Luenberger观测器不 仅可以较准确的实现对自主水下航行器位置、广义速度状态的实时观测，还具有较好的滤 波效果。
[0066] b、为获得控制器所需自主水下航行器在北东坐标系下的实际下潜速度，设计三阶 非线性扩张状杰观测器：
[0067]
(4)
[0068] 其中，S为非线性Luenberger观测器观测出的下潜位移状态，Z1为对i:的观测值，Z2 为对為的观测值。可见，三阶非线性扩张状态观测器观测出的速度状态与位置状态是定义 在北东坐标系下的。
[0069]附图5给出了三阶非线性扩张状态观测器和非线性Luenberger观测器对固定坐标 系中下潜速度也和运动坐标系中下潜速度化的观测值，可见，两者是完全不同的，三阶非线 性扩张状态观测器的观测值兔可以作为设计控制器所需的速度反馈状态。
[0070] c、未知扰动是影响被控对象控制系统性能的一个重要因素，如能对系统的扰动进 行观测并进行实时补偿，将会大大提高系统的控制性能。非线性扩张状态观测器可以将作 用于系统的外界干扰、未建模扰动以及系统的已建模确知部分归结为系统的总扰动进行观 测，将观测到的总扰动进行实时补偿，便可以将复杂被控系统转变为简单的积分串联型系 统。为获得航行器在定深控制方向的总扰动，设计如下形式的二阶非线性扩张状态观测器：
[0071]
(5)
[0072] 其中，4为非线性Luenberger观测器观测出的下潜速度(运动坐标系）状态，Z11为 对众的观测值，Z 22为对下潜方向总干扰的观测值。由二阶非线性扩张状态观测器(5)可得： 只要观测误差ev趋于零，Z 11便趋向于#,Z22趋向于下潜方向的总扰动。
[0073]附图6为三阶非线性扩张状态观测器和二阶非线性扩张状态观测器对自主水下航 行器下潜方向总扰动的观测曲线，在150-200秒加入幅值为250N的白噪声信号来模拟外界 环境干扰对自主水下航行器的影响，可见，两观测器对总扰动的观测值有着很大的差别，二 阶非线性扩张状态观测器对垂荡方向总扰动的观测比较精确。
[0074] (4)控制器的设计：
[0075] 设计非线性ro控制器：
[0076] u〇 = kPfal(ei,ap,5k)+kdfal(e2,ad,5k) (6)
[0077] 其中，ap、ad、5k为饱和函数的待设计参数，ei = χ-ζι，e2 = v_Z2, kP、kd为控制器的比 例和微分设计参数。为实现系统对非线性Luenberger观测器观测总扰动的补偿，最终升降 舵所获得的控制输入为：
[0078] u = u〇-Z22/b (7)
[0079]其中，Z22为非线性Luenberger观测器对自主水下航行器定深控制方向总扰动的观 测值，b为环境干扰力参数。
[0080] 附图7和8分别给出了传统PID控制和基于非线性Luenberger观测器的非线性PD控 制下自主水下航行器的升降舵舵角和下潜深度响应曲线。
[0081 ]由附图7可见，基于非线性Luenberger观测器的控制方法相对于传统PID控制，自 主水下航行器升降舵舵角的幅度明显减小，有利于减小执行机构的磨损。
[0082]由附图8可见，对自主水下航行器垂直面的运动控制，基于非线性Luenberger观测 器的控制方法相对于传统PID控制，系统超调明显减小，对外界环境扰动能够做出较快的相 应。控制系统中通过加入非线性Luenberger观测器，有利于改善控制系统的动态性能指标， 增强了系统对外界环境扰动的鲁棒性。
【主权项】
1. 一种基于非线性观测器的自主水下航行器运动控制方法，其特征是：它包括W下步 骤： 步骤一、建立自主水下航行器的六自由度数学模型； 自主水下航行器的数学模型建立在北东坐标系和运动坐标系下，在北东坐标系下定义 其位置矢量，运动坐标系下定义其广义速度矢量;建立在两坐标系下的运动学和动力学数 学模型为：(1) 其中，η为定义在固定坐标系下自主水下航行器的位置矢量矩阵，V为定义在运动坐标 系下自主水下航行器的广义速度矢量矩阵，J( Θ )为自主水下航行器在北东坐标系下的速 度与运动坐标系下的广义速度之间的转换矩阵，Μ为惯性矩阵，Crb(V)为刚体科里奥利向屯、 力矩阵，Ca(V)为流体产生的科里奥利向屯、力矩阵，D(V)为阻尼力矩阵，g(0)为重力和浮力 产生的阿基米德回复力和力矩矩阵，b为环境干扰力矩阵，τ为控制输入； 步骤二、利用跟踪微分器获取期望位置信息的跟踪信息W及微分信息； 步骤Ξ、利用非线性Luenberger观测器利用传感器测量得到的位置信息观测出自主水 下航行器的速度和干扰状态信息； 步骤四、控制器利用跟踪微分器W及非线性Luenberger观测器获得的信息得到执行机 构所需控制量； 步骤五、执行机构作用于受控对象自主水下航行器，使自主水下航行器运动到设定的 期望位置，完成一次自主水下航行器运动控制。2. 根据权利要求1所述的一种基于非线性观测器的自主水下航行器运动控制方法，其 特征在于步骤二至四为基于非线性观测器的自主水下航行器定深控制步骤，具体为： 步骤A、跟踪微分器对期望深度指令进行微分，得到期望指令的跟踪信息X和微分信息 V; 步骤B、非线性Luenberger观测器利用传感器测量得到的位置信息η观测出航行器在运 动坐标系下的广义速度V; 步骤C、S阶非线性扩张状态观测器利用非线性Luenberger观测器观测到的深度信息S 观测出航行器的深度ziW及定义在北东坐标系下的下潜速度取; 步骤D、将跟踪微分器获得的期望深度跟踪信息X和微分信息V分别与Ξ阶非线性扩张 状态观测器观测到的深度信息ziW及定义在北东坐标系下的下潜速度的差值作为控制 器的比例和微分输入，经非线性PD控制率获得输出控制信号UQ;把非线性Luenberger观测 器观测到的下潜速度信息成作为二阶非线性扩张状态观测器的输入，观测出航行器在下潜 方向的总扰动； 步骤E、将控制信号U0与总扰动的差值作为最后升降舱的控制输入U。3. 根据权利要求2所述的一种基于非线性Luenberger观测器的自主水下航行器运动控 制方法，其特征在于步骤B中所述的非线性Luenberger观测器为：(2) 其中，β为非线性扩张观测器的可调参数;g(e)为重力加速度分量;々为对η的观测值，F 为对V的观测值，i为对环境干扰b的观测值，巧=C 々为位置观测误差； .知/化α，巧为饱和函数，其具体形式如下：(3)： α和δ为饱和函数的待设计参数。4. 根据权利要求3所述的一种基于非线性观测器的自主水下航行器运动控制方法，其 特征在于步骤C中，Ξ阶非线性扩张状态观测器为：(斗） 其中，e为下潜深度观测误差；S为非线性Luenberger观测器观测出的下潜位移状态，Ζ1 为对；的观测值，Z2为对勾的观测值，Z3为对下潜方向所受总和干扰的估计值;01、防、03为非 线性扩张观测器的可调参数。5. 根据权利要求4所述的一种基于非线性观测器的自主水下航行器运动控制方法，其 特征在于步骤D中，二阶非线性扩张状态观测器为：") 其中，ev为下潜速度观测误差；#为非线性Luenberger观测器观测出的下潜速度状态， Z1功对#的观测值，Z22为对下潜方向总干扰的观测值;βιι、β?2为非线性扩张观测器的可调 参数。6. 根据权利要求5所述的一种基于非线性观测器的自主水下航行器运动控制方法，其 特征在于步骤四中所述的控制器为非线性PD控制器： u〇 = kpfal(ei,ap,Sk)+kdfal(e2,ad,Sk) (6) 其中，ap、ad、Sk为饱和函数的待设计参数，el = χ-Zl，e2 = v-Z2，kp、kd为控制器的比例和 微分设计参数。7. 根据权利要求6所述的一种基于非线性观测器的自主水下航行器运动控制方法，其 特征在于步骤E中最后升降舱的控制输入U为： U = U〇-Z22/b (7) 其中，U0为纵向速度控制常量;Z22为非线性观测器对自主水下航行器定深控制方向总 扰动的观测值，b为环境干扰力矩阵。
【文档编号】G05B13/04GK105843233SQ201610221521
【公开日】2016年8月10日
【申请日】2016年4月11日
【发明人】李娟 , 孔明, 陈兴华, 徐健, 王宏健, 张伟
【申请人】哈尔滨工程大学