技术领域
[0001] 本
发明属于地面模拟
航天器在微重力环境下的伺服控制领域，涉及一种中性
浮力机器人自抗扰姿态与轨迹控制方法，特别涉及一种带有执行器饱和的中性浮力机器人在复杂
水环境中姿态与轨迹自抗扰控制方法。
背景技术
[0002] 地面模拟与验证微重力环境下的航天任务是航天领域研究的重要组成部分。由于中性浮力能够长时间持续提供六
自由度微重力环境，因此国内外航天局将其作为模拟空间运动的地面试验手段，用于对航天员的训练、空间机构的对接与组装以及非合作目标的抓取等。但是由于控制力间的耦合以及机器人在水中运动过程中受到水的粘性阻力等导致中性浮力机器人系统模型中具有很强的非线性等不确定因素；同时该中性浮力机器人的执行机构中存在饱和问题。因此，需要研究控制方案来确保复杂非线性中性浮力机器人在执行器未饱和的条件下实现良好的姿态与轨迹控制。
[0003] 自抗扰控制技术是上世纪九十年代由中国科学院数学与系统科学研究院的韩京清研究员及其领导的研究小组创立发展的，其既继承和发扬了经典控制的观念，同时又吸收了现代控制理论的思想。自抗扰控制技术的核心思想为：将系统中的未建模动态以及未知外部干扰当作系统的总和扰动，被实时地估计并补偿到
控制器中，从而实现了动态系统的动态反
馈线性化。随着广大学者对自抗扰控制技术理论方面的充分研究，自抗扰控制技术已被广泛应用于
电机控制、
飞行器控制、轧
钢、
发电厂、坦克炮控系统等机器人领域。
[0004] 工程设备中所用到的执行器均会因外界工作环境以及自身额定
电压等因素的制约，使得执行器输出有一定的上下限，即存在执行器饱和问题。如果忽略执行器饱和问题，可能会使系统性能降低甚至会对设备造成一定的损害。为了避免执行器过饱和现象的发生，当前常用的方法为将饱和控制器用凸包的形式表示，估计出所研究系统的吸引域，即保证位于该吸引域内的所有系统初始状态，在控制器作用下均能够收敛到原点或有界区域。
发明内容
[0005] 本发明的技术解决问题是：针对中性浮力机器人系统，为克服
现有技术在水下环境抗干扰能力的不足以及执行器饱和导致的系统性能降低的
缺陷，根据期望姿态以及运动轨迹对中性浮力机器人系统模型进行变换，得到其
跟踪误差系统模型，同时考虑系统中的复杂非线性和执行器饱和问题，采用自抗扰控制技术实现精准快速地姿态与轨迹控制。
[0006] 为实现上述目的，本发明的技术解决方案为：
[0007] 一种中性浮力机器人姿态与轨迹自抗扰控制方法，包括以下步骤：
[0008] 1)建立带有执行器饱和的中性浮力机器人系统动力学模型；
[0009] 2)利用原系统模型，构建带有执行器饱和的中性浮力机器人姿态与轨迹跟踪误差系统；
[0010] 3)设计跟踪微分器，并为初始跟踪误差安排过渡过程避免初始时刻输出超调，进一步获取跟踪误差
信号的微分信号；
[0011] 4)设计扩张状态观测器，并实时估计系统中的非线性不确定项；
[0012] 5)非线性不确定项的估计值补偿到跟踪误差状态反馈中建立复合控制器；
[0013] 6)考虑执行器饱和问题，建立凸优化问题，求解跟踪误差系统的吸引域，即得到中性浮力机器人系统的安全运动范围。
[0014] 第1)步：构建带有执行器饱和的中性浮力机器人系统动力学模型
[0015] 建立地理
坐标系Oxnynzn： 分别指中性浮力机器人在Oxn、Oyn和Oyz方向的
位置； 分别指中性浮力机器人的
横滚角，
俯仰角以及
偏航角。建立中性浮力机器人体坐标系Oxbybzb： 为中性浮力机器人线速度向量， 为中性浮力机器人
角速度向量。根据Oxnynzn与Oxbybzb的关系，建立如下等式：
[0016]
[0017] 式中， J(η)为运动系数矩阵。
[0018] 构建中性浮力机器人系统在体坐标系下的动力学模型：
[0019]
[0020] 式中，τ为系统控制输入，τm为系统控制输入的上限值，M为惯性
质量矩阵，C(v)为
科里奥利力矩阵，D(v)为中性浮力机器人在水中受到的黏性阻力，g(η)为负浮力系数，sat(τ)是对控制输入的饱和约束函数，其表达式为：
[0021]
[0022] 式中，sign(·)为符号函数。
[0023] 联立式(1)和(2)，可得带有执行器饱和的中性浮力机器人系统动力学模型：
[0024]
[0025] 式中，Mη(η)＝J-T(η)MJ-1(η)， Dη(η,v)＝J-T(η)D(v)J-1(η)，gη(η)＝J-T(η)g(η)。
[0026] 第2)步：构建带有执行器饱和的中性浮力机器人姿态与轨迹跟踪误差系统[0027] 设中性浮力机器人系统的期望姿态与轨迹为ηd，则根据式(3)可得姿态与轨迹的跟踪误差系统为：
[0028]
[0029] 令x1(t)＝η-ηd， 跟踪误差系统(4)可以进一步简化为如下
状态空间表达式的形式：
[0030]
[0031] 式中，
[0032] B0为参数可调的对角矩阵。
[0033] 第3)步：设计跟踪微分器
[0034] 一方面为了避免初始时刻姿态与轨迹的跟踪误差x1(t)过大而造成系统输出超调；另一方面为了获取轨迹跟踪误差x1(t)的微分信号为设计控制器做准备，设计跟踪微分器如下所示：
[0035]
[0036] 其中，x1(t)是跟踪微分器的
输入信号，v1(t)和v2(t)是跟踪微分器的
输出信号，并且v1(t)是x1(t)的跟踪信号，v2(t)是x1(t)的近似微分信号，r和h分别是跟踪微分器的速度因子和滤波因子，fhan(v1(t)-x1(t),v2(t),r,h)的表达式为：
[0037]
[0038] 在此，我们忽略跟踪微分器对信号造成的误差，即假设x1(t)＝v1(t)，x2(t)＝v2(t)。
[0039] 第4)步：设计扩张状态观测器，估计系统中的非线性不确定项
[0040] 由于系统中的非线性不确定项d(t)中含有饱和函数sat(τ)，因此该非线性不确定项存在微分无法求解的问题。为了解决此问题，设饱和函数sat(τ)的近似函数 如下所示：
[0041]
[0042] 式中， ε0为正常数。
[0043] 该饱和函数 连续可导，并且满足 和dsat(τ)/dτ≤1。
[0044] 因此，原跟踪误差系统(5)改写为：
[0045]
[0046] 式中，
[0047] 针对跟踪误差系统(7)设计扩张状态观测器形式如下所示：
[0048]
[0049] 式中， 是扩张状态观测器的输出状态， 是跟踪误差状态x1(t)的估计误差，α＝[α1,α2,α3]T是扩状态观测器的可调增益，系数矩阵 和分别为：
[0050]
[0051] 该扩张状态观测器是一个动态过程，它只用了跟踪误差系统(7)中的输入输出信息。扩张状态观测器的输出状态 之所以能够实时估计出系统中的非线性不确定项其根本原因是不管非线性不确定项 是什么形式，只要其影响系统的实际输出，就可以从输出信息中提炼出非线性不确定项的实时作用量。
[0052] 综上所述，利用设计的扩张状态观测器可以实时的估计出系统中众多未建模动态、非线性不确定项等外部干扰，增强了中性浮力机器人系统的
稳定性和鲁棒性。
[0053] 第5)步：复合控制器设计
[0054] 为了保证精准的姿态与轨迹跟踪控制，本发明中控制器采用跟踪误差状态反馈控制，并将扩张状态观测器估计出的非线性不确定项实时补偿到控制器中。进而，无论系统是确定的还是不确定的，是线性的还是非线性，是时不变的还是时变的，都可以将原系统转换为确定的线性时不变积分
串联型系统。跟踪误差系统(7)中的控制器设计为：
[0055]
[0056] 式中，K＝[k1,k2]是控制器(9)的可调控制增益。
[0057] 第6)步：考虑执行器饱和问题，求解跟踪误差系统的吸引域
[0058] 为了避免控制输入超过执行器的饱和上限而损坏执行器，本发明将饱和控制器sat(τ)用凸包的方式表示，并求解中性浮力机器人跟踪误差系统的吸引域。
[0059] 饱和函数sat(τ)的凸包形式为：
[0060]
[0061] 式中，0≤δi≤1， Ei为由0或1组成的对角矩阵， H∈R6×12并满12
足线性饱和区域Ξ(H)＝{x(t)∈R :|Hx(t)|≤1}。
[0062] 将饱和控制器(10)代入跟踪误差系统(7)，得
[0063]
[0064] 式中， e3(t)是扩张状态观测器对非线性不确定项的估计误差，即， 并且通过调节扩张状态观测器的增益α可以保证估计误差e3(t)有界。本发明设|e3(t)|≤M1，则根据 可知
因此系统(11)可改写为：
[0065]
[0066] 式中，
[0067] 设椭球表达式为 其中，Φ∈R12×12是正定矩阵，ρ是正数。并针对该跟踪误差系统(12)设定两个椭球区域 和 分别满足如下条件：
[0068]
[0069]
[0070]
[0071] 再给出两个形状参考集 和 分别满足 和其中，γ表示干扰抑制水平。设θ＝γ2，Θ＝Φ-1，X1＝H1Θ和X2＝H2Θ，并设定ρ1和κ的值，可通过求解如下凸优化问题(13)，一方面可以求出最小的干扰抑制水平γ；
另一方面可以求出正定矩阵Φ并求出跟踪误差系统的吸引域。
[0072]
[0073] s.t.a)
[0074] b)
[0075] c)
[0076] d)
[0077] e)
[0078] 与现有技术相比，本发明具有以下有益效果：
[0079] 本发明在考虑执行器饱和问题的情况下，首先根据期望的姿态与运动轨迹将中性浮力机器人系统动力学模型转化为跟踪误差系统形式；通过跟踪微分器为初始跟踪误差安排过渡过程避免初始时刻输出超调，进一步获取跟踪误差信号的微分信号；扩张状态观测器实时估计跟踪误差系统中的非线性不确定项并补偿到跟踪误差状态组成的反馈控制器中，避免内外干扰等非线性因素对系统造成不利影响，提高系统的鲁棒性，便于精准地姿态与轨迹跟踪控制；通过凸包方法处理系统中的执行器饱和问题，并求解出系统的吸引域，即，中性浮力机器人系统的安全运动范围。尤其是利用跟踪微分器安排过渡过程，避免了因初始误差过大而造成的输出超调现象，对中性浮力机器人起到一定的保护作用；通过扩张状态观测器实时估计系统中的非线性不确定项并补偿到跟踪误差反馈控制器中，克服了非线性不确定项对系统造成的不利影响，提高系统的鲁棒性，便于实现中性浮力机器人的姿态与轨迹跟踪控制；考虑执行器饱和问题，求解出系统安全工作的区域，避免因执行器输入电压超过其饱和上限而损坏。本发明提出的控制策略对带有执行器饱和的中性浮力机器人系统有良好的控制效果，并且可以广泛应用于带有执行器饱的其他非线性系统中。
附图说明
[0080] 图1为本发明中性浮力机器人自抗扰姿态与轨迹控制方法
流程图。
[0081] 图2为带有执行器饱和的中性浮力机器人自抗扰控制方
框图。
具体实施方式
[0082] 下面结合附图和具体
实施例对本发明做进一步说明：
[0083] 本发明如流程图1所示，通过以下步骤实现带有执行器饱和的中性浮力机器人系统姿态与轨迹跟踪控制：
[0084] 第一步：构建带有执行器饱和的中性浮力机器人系统动力学模型
[0085] 建立地理坐标系Oxnynzn： 分别指中性浮力机器人在Oxn、Oyn和Oyz方向的位置； 分别指中性浮力机器人的横滚角，俯仰角以及偏航角。建立中性浮力机器人体坐标系Oxbybzb： 为中性浮力机器人线速度向量， 为中性浮力机器人角速度向量。根据Oxnynzn与Oxbybzb的关系，建立如下等式：
[0086]
[0087] 式中， J(η)为运动系数矩阵。
[0088] 构建中性浮力机器人系统在体坐标系下的动力学模型：
[0089]
[0090] 式中，τ为系统控制输入，τm为系统控制输入的上限值，M为惯性质量矩阵，C(v)为科里奥利力矩阵，D(v)为中性浮力机器人在水中受到的黏性阻力，g(η)为负浮力系数，sat(τ)是对控制输入的饱和约束函数，其表达式为：
[0091]
[0092] 式中，sign(·)为符号函数。
[0093] 由式(14)可得
[0094]
[0095] 对式(14)进行微分，可得
[0096]
[0097] 将式(16)和(17)带入式(15)可得带有执行器饱和的中性浮力机器人系统动力学模型：
[0098]
[0099] 式中，Mη(η)＝J-T(η)MJ-1(η)， Dη(η,v)＝J-T(η)D(v)J-1(η)，gη(η)＝J-T(η)g(η)。
[0100] 第二步：构建带有执行器饱和的中性浮力机器人姿态与轨迹跟踪误差系统[0101] 设中性浮力机器人系统的期望姿态与轨迹为 本实例中，选取 和
[0102] 根据式(18)可得姿态与轨迹的跟踪误差系统为：
[0103]
[0104] 令x1(t)＝η-ηd， 则跟踪误差系统(19)可以进一步简化为如下状态空间表达式的形式：
[0105]
[0106] 式中，B0为参数可调的对角矩阵。
[0107] 第三步：设计跟踪微分器
[0108] 一方面为了避免初始时刻姿态与轨迹的跟踪误差x1(t)过大而造成系统输出超调；另一方面为了获取轨迹跟踪误差x1(t)的微分信号为设计控制器做准备，设计跟踪微分器如下所示：
[0109]
[0110] 式中，x1(t)是跟踪微分器的输入信号，v1(t)和v2(t)是跟踪微分器的输出信号，并且v1(t)是x1(t)的跟踪信号，v2(t)是x1(t)的近似微分信号，r和h分别是跟踪微分器的速度因子和滤波因子，fhan(v1(t)-x1(t),v2(t),r,h)的表达式为：
[0111]
[0112] 为了保证跟踪微分器即能够快速地跟踪信号又能起到良好的滤波效果，本实例中选取速度因子与滤波因子分别为：r＝[100 100 100 100 100 100]T和h＝[0.1 0.1 0.1 0.1 0.1 0.1]T。
[0113] 在此，我们忽略跟踪微分器对信号造成的误差，即假设x1(t)＝v1(t)，x2(t)＝v2(t)。
[0114] 第四步：设计扩张状态观测器，估计系统中的非线性不确定项
[0115] 为了解决饱和函数sat(τ)导致非线性不确定项d(t)无法求微分的问题，设饱和函数sat(τ)的近似函数 如下所示：
[0116]
[0117] 式中， ε0为正常数。
[0118] 该饱和函数 连续可导，并且满足 和dsat(τ)/dτ≤1。
[0119] 因此，原跟踪误差系统(20)改写为：
[0120]
[0121] 式中，
[0122] 针对跟踪误差系统(22)设计扩张状态观测器形式如下所示：
[0123]
[0124] 式中， 是扩张状态观测器的输出状态， 是跟踪误差状态x1(t)的估计误差，α＝[α1,α2,α3]T是扩状态观测器的可调增益，系数矩阵 和分别为：
[0125]
[0126] 为了实现对非线性不确定项的实时估计，本实例中对扩张状态观测器的可调增益选取为：
[0127] α1＝diag{100 100 100 100 100 100}，
[0128] α2＝diag{2000 2000 2000 2000 2000 2000}，
[0129] α3＝diag{300 300 300 300 300 300}。
[0130] 该扩张状态观测器是一个动态过程，它只用了跟踪误差系统(22)中的输入输出信息。扩张状态观测器的输出状态 之所以能够实时估计出系统中的非线性不确定项其根本原因是不管非线性不确定项 是什么形式，只要其影响系统的实际输出，就可以从输出信息中提炼出非线性不确定项的实时作用量。
[0131] 综上所述，利用设计的扩张状态观测器可以实时的估计出系统中众多未建模动态、非线性不确定项等外部干扰，增强了中性浮力机器人系统的稳定性和鲁棒性。
[0132] 第五步：复合控制器设计
[0133] 为了保证精准的姿态与轨迹跟踪控制，本发明中控制器采用跟踪误差状态反馈控制，并将扩张状态观测器估计出的非线性不确定项实时补偿到控制器中。进而，无论系统是确定的还是不确定的，是线性的还是非线性，是时不变的还是时变的，都可以将原系统转换为确定的线性时不变积分串联型系统。跟踪误差系统(22)中的控制器设计为：
[0134]
[0135] 式中，K＝[k1,k2]是控制器(24)的可调控制增益。为了保证中性浮力机器人实现快速精准的姿态与轨迹跟踪控制，本实例中选取参数可调的对角矩阵B0为：B0＝diag{20 20 20 10 10 10}
[0136] 控制增益为：k1＝diag{10 10 15 2 1.8 3}和k2＝diag{1 1 3 1 1.5 2}。
[0137] 第六步：考虑执行器饱和问题，求解跟踪误差系统的吸引域
[0138] 为了避免控制输入超过执行器的饱和上限而损坏执行器，本发明将饱和控制器sat(τ)用凸包的方式表示，并求解中性浮力机器人跟踪误差系统的吸引域。
[0139] 饱和函数sat(τ)的凸包形式为：
[0140]
[0141] 式中，0≤δi≤1， Ei为由0或1组成的对角矩阵， H∈R6×12并满足线性饱和区域Ξ(H)＝{x(t)∈R12:|Hx(t)|≤1}。
[0142] 将饱和控制器(25)代入跟踪误差系统(22)，得
[0143]
[0144] 式中， e3(t)是扩张状态观测器对非线性不确定项的估计误差，即， 并且通过调节扩张状态观测器的增益α可以保证估计误差e3(t)有界。本发明设|e3(t)|≤M1，则根据 可知
因此系统(26)可改写为：
[0145]
[0146] 式中，
[0147] 设椭球表达式为 其中，Φ∈R12×12是正定矩阵，ρ是正数。并针对该跟踪误差系统(22)设定两个椭球区域 和 分别满足如下条件：
[0148]
[0149]
[0150]
[0151] 并给出两个形状参考集 和 分别满足 和2 -1
其中，γ表示干扰抑制水平。设θ＝γ ，Θ＝Φ ，X1＝H1Θ和X2＝H2Θ，并选取ρ1＝0.01和κ＝0.05，通过求解如下凸优化问题(28)，即可求出最小的干扰抑制水平γ又可以求出正定矩阵Φ并求出跟踪误差系统的吸引域。
[0152]
[0153] s.t.a)
[0154] b)
[0155] c)
[0156] d)
[0157] e)
[0158] 以上，仅为本发明的较佳实施例，并非仅限于本发明的实施范围，凡依本发明
专利范围的内容所做的等效变化和修饰，都应为本发明的技术范畴。
