CN111813110A - 一种蛇形机器人路径跟随自抗扰控制方法 - Google Patents

Abstract

本发明公开一种蛇形机器人路径跟随自抗扰控制方法，通过航向角自抗扰控制器的航向角扩张状态观测器与关节角自抗扰控制器的关节角扩张状态观测器分别估计蛇形机器人的航向角的模型未知部分和外部未知扰动在内的航向角总扰动以及关节角模型的未知部分和外部未知扰动在内的关节角总扰动，然后通过对应的控制量补偿航向角总扰动、关节角总扰动对控制系统造成的影响，从而实现蛇形机器人的路径跟随控制。本发明对模型信息依赖度小；当蛇形机器人运动过程中受到时变或恒值外部未知扰动时能很快恢复到稳定状态，并跟随期望路径运动；不仅控制精度高，而且鲁棒性强抗扰性高。

Description

一种蛇形机器人路径跟随自抗扰控制方法

技术领域

本发明属于机器人路径跟随问题的控制技术领域，特别是涉及一种蛇形机器人路径跟随自抗扰控制方法。

背景技术

路径跟随控制是指移动机器人在没有时间约束下到达空间上的给定曲线，是移动机器人实现其他运动控制任务的基础。移动机器人的路径跟随控制问题广泛见于各种工业、商业和军事科学研究中，所以研究移动机器人的路径跟随控制具有重要的科学研究意义和实际应用价值。蛇形机器人能够像生物蛇一样通过身体结构的变换协调配合来实现多运动步态能力，可以适应复杂多变和具有挑战性的运动环境，因而可以在一些不规则的区域运动，例如沼泽、沙滩、草地、崎岖不平的山路等。蛇形机器人与履带式机器人、腿式机器人、轮式机器人等传统的移动机器人相比具有更强的环境适应性和功能执行能力，可以实现工业或军事任务中的复杂且危险的任务。蛇形机器人是一个多自由度的高度耦合的欠驱动非线性系统，其动力学模型相当复杂，摩擦系数未知使得动力学模型存在不确定性，而且在实际应用中还会受到外部未知扰动的影响，这给蛇形机器人的路径跟随控制设计带来了巨大的挑战，严重阻碍了蛇形机器人的实际工程应用与发展。在蛇形机器人系统存在模型不确定及外部未知扰动的影响下，如何实现高精度强鲁棒性的路径跟随控制一直是研究的热点问题。

有关蛇形机器人的路径跟随控制问题，国内外的研究人员进行了很多的研究。挪威科技大学Pettersen等人创新性地提出陆地蛇形机器人的简化模型，在假设摩擦系数精确已知的条件下设计了一种应用视线导引律(LOS,Line-of-Sight)的直线路径跟随控制器(期刊：IEEE Transactions on Control Systems Technology；著者：

IdarU.Haugstuen,Kristin Y.Pettersen,et al；出版年月：2012年1月；文章题目：PathFollowing Control of Planar Snake Robots Using a Cascaded Approach；页码：111-126)，但在实际的工程应用中准确测量摩擦系数的值是很难实现的；印度理工学院德里分校研究者Joyjit Mukherjee等人在摩擦系数未知的条件下基于滑模控制(SMC,SlidingMode Control)研究了平面蛇形机器人的速度控制和路径跟随控制问题(期刊：IEEERobotics and Automation Letters；著者：J.Mukherjee,S.Mukherjee,and I.N.Kar；出版年月：2017年1月；文章题目：Sliding Mode Control of Planar Snake Robot withUncertainty Using Virtual Holonomic Constraints；页码：1077-1084)，但是只有受摩擦系数影响产生的不确定动力学存在上界的情况下控制系统才能达到稳定状态；内华达大学申延涛等人设计了一种新颖的自适应控制器，可以用来估计未知和时变的摩擦力系数以实现精确的蛇形机器人路径跟随控制(期刊：IEEE Robotics and Automation Letters；著者：Gang Wang,Weixin Yang,Yantao Shen,et al；出版年月：2018年10月；文章题目：Adaptive Path Following of Underactuated Snake Robot on Unknown and VariedFrictions Ground:Theory and Validations；页码：4273-4280)，此控制方案没有考虑外部扰动的影响，而在实际运动环境中存在未知扰动的概率很大；中国科学院大学张安翻等人基于反馈控制技术研究鳗鱼机器人的曲线路径跟随控制问题，并利用仿真进行了验证(期刊：Robotics and Autonomous Systems；著者：AnFan Zhang,Shugen Ma,Bin Li,etal；出版年月：2018年6月；文章题目：Curved Path Following Control for Planar EelRobots；页码：129-139)，此控制技术采用的摩擦系数是已知的，也没有设计控制量补偿外部未知扰动的影响，使其很难真正应用到实际工程中。

分析已有文献可知，未知摩擦系数和外部未知扰动是目前影响蛇形机器人路径跟随控制的主要问题。准确测量摩擦系数并计算摩擦力的值是很难实现的，而且准确测量外部未知扰动也是不现实的，这些不确定因素使得蛇形机器人的路径跟随控制成为一项具有挑战性的工作，亟需一种既不需要依赖摩擦系数又能抵抗外部扰动的蛇形机器人路径跟随控制技术。

发明内容

为了克服现有控制技术的不足，本发明针对蛇形机器人路径跟随控制的问题，而一种基于扩张状态观测器的蛇形机器人路径跟随自抗扰控制方法，采用基于扩张状态观测器ESO(ESO,Extended State Observer)的自抗扰控制技术实现蛇形机器人的高精度、强鲁棒的路径跟随控制，其是利用自抗扰控制技术把系统中的不确定因素定义为系统总扰动，利用扩张状态观测器对总扰动的实时估计，然后通过控制输入实现扰动补偿，从而有效解决上述问题。本发明的方案设计简单，对系统模型依赖度低，而且抗扰性好鲁棒性强，更适应于实际工程应用。

本发明是这样实现的，

一种蛇形机器人路径跟随自抗扰控制方法，通过航向角自抗扰控制器的航向角扩张状态观测器与关节角自抗扰控制器的关节角扩张状态观测器分别估计蛇形机器人的航向角的模型未知部分和外部未知扰动在内的航向角总扰动以及关节角模型的未知部分和外部未知扰动在内的关节角总扰动，然后通过对应的控制量补偿航向角总扰动、关节角总扰动对控制系统造成的影响，从而实现蛇形机器人的路径跟随控制；

所述航向角自抗扰控制器根据蛇形机器人的实时的航向参考角输出关节偏离量作为步态模式生成器的输入，所述步态模式生成器输出关节参考角度作为所述关节角自抗扰控制器的输入，所述关节角自抗扰控制器输出关节驱动力到控制对象：

所述的航向角自抗扰控制器输出的关节偏离量如下：

所述的步态模式生成器输出蜿蜒步态，表示如下：

φ_ref,j＝αsin(ωt+(j-1)δ)+φ₀，

F₁代表航向角动力学系统总扰动，

是F₁的估计值，D₁表示航向角动力学外部未知扰动，K_p1＝ω_c1 ²，K_d1＝2ω_c1，ω_c1是航向角自抗扰控制器带宽，e₁＝θ_ref-θ，θ_ref为蛇形机器人实时的航向参考角，B₁＝λ₂v_t，v_t∈R是蛇形机器人的切向速度，θ是蛇形机器人当前的航向角，v_θ∈R表示航向角速度，λ₁是航向角速度对航向角加速度的映射标量参数，λ₂是关节坐标与前进速度对航向角加速度的映射标量参数；N为蛇形机器人的连杆的数量；

φ＝(φ₁,φ₂,Lφ_N-1)∈R^N-1，为蛇形机器人当前的关节角向量，t表示时间；

φ_ref,j表示第j个关节参考角，α和ω分别表示步态模式生成器的蜿蜒步态关节运动的幅值和角频率，δ表示关节运动的相位差；

所述的关节角自抗扰控制器输出的关节驱动力，表示如下：

其中，e₂＝φ_ref-φ，φ_ref是通过步态模式生成器得到的参考关节角，

ω_c2是关节角自抗扰控制器带宽，

φ＝(φ₁,φ₂,Lφ_N-1)∈R^N-1，为蛇形机器人当前的关节角向量，v_φ∈R^N-1表示关节角速度向量，F₂代表关节角动力学系统总扰动，

是F₂的估计值，D₂表示关节角动力学外部未知扰动，m为蛇形机器人的连杆质量，

是蛇形机器人前进的推力系数，表示关节坐标与连杆速度到推进力的映射，c_t,c_n分别为蛇形机器人切向和法向的摩擦力系数，l为连杆长度，A、D为常数矩阵；

其中，所述的蛇形机器人实时的航向参考角表示如下：

其中，

是路径跟踪误差，Δ>0是前向距离的设计参数。

其中，所述的路径跟踪误差

式中，p_y为蛇形机器人质心坐标之纵坐标。

本发明针对蛇形机器人直线路径跟随控制中存在模型不确定性及外部未知扰动的问题，采用自抗扰控制技术研究蛇形机器人的直线路径跟随控制，准确地估计蛇形机器人因摩擦系数测量不准确导致的模型未知部分和外部未知扰动在内的总扰动，然后在控制器中通过控制量以补偿总扰动对控制系统造成的影响。

本发明的特点及有益效果如下：

对系统模型信息的依赖度小。此控制方法不依赖系统模型信息中的摩擦系数，是由于扩张状态观测器能准确估计总扰动。所以当摩擦系数测量不准确时此发明所设计的蛇形机器人路径跟随自抗扰控制方法仍然能实现很好的控制效果。

本发明设计的控制方法抗扰性高。当蛇形机器人运动过程中受到时变或恒值外部未知扰动时能很快恢复到稳定状态，并跟随期望路径运动。所以本发明所设计的蛇形机器人路径跟随自抗扰控制方法不仅控制精度高，而且鲁棒性强抗扰性高。

附图说明

图1是本发明所涉及的蛇形机器人运动学参数示意图；

图2是本发明所涉及的蛇形机器人控制系统的结构原理框图；

图3.1-a--3.1-d是摩擦系数精确已知时的仿真结果，图中：

3.1-a是路径跟随误差曲线；3.1-b是航向角误差曲线；3.1-c是关节角误差曲线；3.1-d是扰动F2估计值和真实值曲线；

图3.2-a--3.2-d是摩擦系数未知时的仿真结果，图中：

3.2-a是路径跟随误差曲线；3.2-b是航向角误差曲线；

3.2-c是关节角误差曲线；3.2-d是扰动F2估计值和真实值曲线；

图4.1-a---4.1-f是存在外部恒值扰动的仿真结果，图中：

4.1-a是路径跟随误差曲线；4.1-b是航向角误差曲线；4.1-c是关节角误差曲线；4.1-d是前向速度曲线；4.1-e是扰动F1估计值和真实值曲线；4.1-f是扰动F2估计值和真实值曲线；

图4.2-a--4.2-f是存在外部时变扰动的仿真结果，图中：

4.2-a是路径跟随误差曲线；4.2-b是航向角误差曲线；4.2-c是关节角误差曲线；4.2-d是前向速度曲线；4.2-e是扰动F1估计值和真实值曲线；4.2-f是扰动F2估计值和真实值曲线。

具体实施方式

下面将结合具体仿真方案和附图对本发明的技术方案进行详细说明。

本发明设计自抗扰控制器(ADRC,Active Disturbance Rejection Controller)，将系统动力学中含摩擦力的项当做模型未知部分处理，并用扩张状态观测器估计包括模型未知部分和外部未知扰动在内的总扰动，然后设计控制量补偿，从而实现基于自抗扰控制技术的蛇形机器人的路径跟随控制。

其具体的实现步骤如下：

步骤一：建立蛇形机器人系统的数学模型。

本发明所涉及的蛇形机器人运动学参数如图1所示，蛇形机器人由N-1个驱动关节链接N个连杆组成，连杆长度为l，连杆质量为m，(p_x,p_y)为蛇形机器人质心坐标，θ_j,j∈{1,L,N}为第j个连杆的角度，φ_j＝θ_j-θ_j+1,j∈{1,L,N-1}表示第j个关节的角度，且φ＝(φ₁,φ₂,L,φ_N-1)∈R^N-1为关节角向量，

表示蛇形机器人航向角。全局坐标系x-y和坐标系t-n的坐标系原点是固定且一致的，t轴与x轴之间的角度θ是蛇形机器人当前的航向角。根据牛顿运动定律推导得到下面的蛇形机器人数学模型：

其中，公式(1a)-(1d)为蛇形机器人运动学模型，公式(1e)-(1h)为蛇形机器人动力学模型。v_φ∈R^N-1表示关节角速度向量；v_θ∈R表示航向角速度；v_t∈R，v_n∈R分别为机器人切向速度和法向速度；c_t,c_n分别为蛇形机器人切向和法向的摩擦力系数，

是蛇形机器人前进的推力系数，表示关节坐标与连杆速度到推进力的映射；u∈R^N-1表示关节驱动力；λ₁,λ₂是和航向角相关的旋转动力学参数，其中，λ₁是航向角速度对航向角加速度的映射标量参数，λ₂是关节坐标与前进速度对航向角加速度的映射标量参数，D₁,D₂分别表示航向角动力学外部未知扰动和关节角动力学外部未知扰动。

其中还涉及到一些常数矩阵，e＝[1 L 1]∈R^N，

为了简化控制器设计，消除身体形状的变化引起的蛇形机器人航向和侧向运动，进行如下坐标变化：

其中，

可以推导得到蛇形机器人面向控制的数学模型如下：

其中，

步骤二：根据蛇形机器人简化模型中航向角动力学方程和关节角动力学方程分别设计扩张状态观测器。

航向角ESO的设计

航向角动力学模型(公式3e)为：

令

且φ_ref,j＝αsin(ωt+(j-1)δ)+φ₀，则航向角动力学模型可写为：

其中，

将上述方程重新写为：

其中，

F₁代表航向角动力学系统总扰动，包括模型未知部分和外部未知扰动D₁。

首先定义状态变量x₁＝θ，

x₃为扩张状态变量，且x₃＝-F₁，则航向角动力学系统的状态空间表达式为：

定义

为x_i的估计值，设计航向角动力学系统的扩张状态观测器方程为：

其中，β₁₁,β₁₂,β₁₃是ESO的增益，其大小为β₁₁＝3ω_o1,β₁₂＝3ω_o1 ²,β₁₃＝ω_o1 ³，ω_o1是观测器带宽，且ω_o1>0。观测器增益大小与观测器观测效果和稳定性有关，合理的观测器增益大小能有效地提升系统控制性能。

为了抑制传统的航向角ESO的初始微分峰值现象，对传统的航向角ESO进行了如下改进设计：

其中，a₁,a₂,a₃为观测器增益，其大小为a₁＝a₂＝a₃＝ω_o1，

是F₁的估计值，而x₃＝-F₁，所以航向角动力学系统总扰动估计为

关节角ESO的设计

蛇形机器人关节角动力学模型(3d)如下：

其中，

将上式重新写为：

其中，

φ＝(φ₁,φ₂,Lφ_N-1)∈R^N-1，F₂代表关节角动力学系统总扰动，包括模型未知部分和外部未知扰动D₂。

设计关节角动力学系统的扩张状态观测器，首先定义状态变量z₁＝φ∈R^N-1，

z₃为扩张状态变量，且z₃＝-F₂，则关节角动力学系统的状态空间表达式为：

定义

为z_i的估计值，设计关节角动力学系统的扩张状态观测器方程为：

其中，β₂₁＝diag(3ω_o2)∈R^N-1×N-1，β₂₂＝diag(3ω_o2 ²)∈R^N-1×N-1，β₂₃＝diag(ω_o2 ³)∈R^N-1×N-1，β₂₁,β₂₂,β₂₃是观测器增益，ω_o2是观测器带宽，ω_o2>0且其取值大小与观测器性能有关。

为了抑制传统关节角ESO的初始微分峰值现象，对传统关节角ESO进行了如下改进设计：

其中，b₁,b₂,b₃为观测器增益，其大小为b₁＝b₂＝b₃＝diag(ω_o2)∈R^N-1×N-1，

是F₂的估计值，而z₃＝-F₂，所以关节角动力学系统总扰动为：

令

则关节角观测器误差方程为：

令

则得到ESO误差方程：

令A＝a₁+a₂+a₃，B＝a₁a₂+a₁a₃+a₂a₃，C＝a₁a₂a₃，可得上式(18)的特征方程为：

λ³+Aλ²+Bλ+C＝0 (19)

由霍尔维茨定理得，特征方程的全部特征根均具有负实部的充要条件是A>0,C>0,AB-C>0。由前述内容可知，该条件全部满足，所以式(18)表示系统的零解

是全局渐进稳定的。

考虑关节角动力学系统总扰动F₂时，系统存在稳态误差。假设

F_2max为常数且F_2max>0。则当系统处于稳态时，可得：

计算稳态误差可得：

综上，关节角ESO是有界输入有界输出稳定的。

航向角ESO稳定性分析与关节角ESO稳定性分析相同，令

假设

同理可得：

因此，航向角ESO也是有界输入有界输出稳定的。

步骤三：蛇形机器人路径跟随自抗扰控制系统设计，是根据步骤二中扩张状态观测器估计的总扰动设计航向角自抗扰控制器和关节角自抗扰控制器。

本发明所涉及的蛇形机器人控制系统的结构框图如图2所示。控制系统主要包括五个部分：视线导引律、航向角自抗扰控制器、步态模式生成器、关节角自抗扰控制器、控制对象。重点是航向角自抗扰控制器和关节自抗扰控制器的设计。

视线导引律

为了使蛇形机器人能收敛到期望直线路径(x轴)，应用视线导引律法则得到蛇形机器人实时的航向参考角：

其中，

是路径跟踪误差；Δ>0是前向距离的设计参数，与收敛到期望路径的速率有关，合理设置Δ的大小可以使得蛇形机器人以合适的速率收敛到期望路径并跟随期望路径运动。

步态模式生成器

本发明所研究的蛇形机器人运动步态选取常用的蜿蜒步态

φ_ref,j＝αsin(ωt+(j-1)δ)+φ₀ (24)

作为各关节参考角度。其中，φ_ref,j表示第j个关节参考角度，α和ω分别表示蜿蜒步态关节运动的幅值和角频率，δ表示关节运动的相位差，φ₀表示关节偏移量，用来控制蛇形机器人航向。

航向角自抗扰控制器的设计

航向角自抗扰控制器设计包含两部分内容：一部分是补偿实时估计总扰动部分，另一部分是路径跟随部分。其中，补偿实时估计总扰动部分控制器设计为：

路径跟随部分控制器设计为：

其中，e₁＝θ_ref-θ，

θ_ref是通过视线导引律得到的参考航向角，K_p1＝ω_c1 ²，K_d1＝2ω_c1，ω_c1是航向角控制器带宽。

所以航向角自抗扰控制器为：

由式(25)可知，只要航向角ESO能精确地估计模型未知部分和外部扰动，则设计的控制器就能够补偿这些扰动，因而本发明所设计的航向角控制系统具有很强的抗扰性和鲁棒性。

关节角自抗扰控制器的设计

关节角控制器设计包含两部分内容：一部分是补偿实时估计总扰动部分，另一部分是轨迹追踪部分。其中，补偿实时估计总扰动部分设计为：

轨迹追踪部分控制器设计为：

其中，e₂＝φ_ref-φ，

φ_ref是通过步态模式生成器得到的参考关节角。

ω_c2是关节角控制器带宽。

所以关节角自抗扰控制器为：

由式(28)可知，只要关节角ESO能精确地估计模型未知部分和外部扰动，就能使所设计的控制器补偿这些扰动，因此我们所设计的关节角自抗扰控制器具有很好的抗扰性和鲁棒性。

蛇形机器人路径跟随自抗扰控制系统稳定性分析

首先根据方向角误差

改写路径跟随误差

和侧向速度

的动力学：

利用关系式

则有

其中，

令

系统的模型可写为如下形式：

其中，

证明式(33c)系统稳定

定理1：假设关节角动力学系统扰动

是有界的，则对于

存在一个常向量ρ＝[ρ₁,ρ₂,L,ρ_2N-2]且ρ_i>0使得在有限时间内η的每一个元素|η_i|<ρ_i(i＝1,2,L,2N-2)。

证明：解方程(33c)得

又知，

令

则有

又A_φmin≤A_φ≤A_φmax，有

因为A_φ是Hurwitz的，因此存在一个有限时间T₂，使得对于

有

对

有

又知

则当T＝max{T₁,T₂,T₃}时，对

有

令

则

对于所有的t>T有

其中，

因此，系统关节角输出是有界稳定的。

证明式(33b)系统稳定

由定理1知，

又因为A_θ是Hurwitz的，所以对

存在一个常向量ζ＝[ζ₁,ζ₂]且ζ_i>0使得在有限时间内ξ的每一个元素满足

|[ξ(t)]_i|≤ζ_i (41)

因此，系统航向角输出是有界稳定的。

证明式(33a)系统稳定

证明式(33a)闭环系统的稳定性，先证明式(33a)的标称系统是渐近稳定的。式(33a)的标称系统如下：

其中，

选取李雅普诺夫函数为：

m>0，则有

根据v_t∈[V_max,V_min]，X≤|X|，可推导得

引入变量

则有

根据不等式

和

可以得到

选择

其中

又因为

所

以α>1，则

所以(33a)的标称系统在平衡点处是渐近稳定的，且平衡点为

由矩阵

导出的2-范数满足：

由前面γ的表达式可知γ的上下界为：

将上式代入式(48)可得

其中，

由(41)可知ξ是有界的，所以

是有界的，系统(33a)的标称系统是渐近稳定的。综合以上分析，可知蛇形机器人系统(33)是有界输入有界输出稳定的。

下面，通过仿真验证对本发明蛇形机器人路径跟随自抗扰控制方法说明。

以MATLAB R2016a作为仿真平台在Windows 10系统的笔记本电脑上运行，以蛇形机器人的数学模型(1a-1h)为控制对象进行蛇形机器人直线路径跟随自抗扰控制的仿真验证。仿真中将基于扩张状态观测器的自抗扰控制器(ADRC，包括航向角自抗扰控制器和关节角自抗扰控制器)与基于模型的控制器(BMC，包括基于模型的航向角控制器和基于模型的关节角控制器)进行对比。仿真分为(a)、(b)两组，分别验证所设计的扩张状态观测器对模型未知部分和外部未知扰动的估计效果，从而验证路径跟随自抗扰控制器的控制效果。

仿真中涉及到的参数如下：蛇形机器人连杆数N＝7，单连杆长度l＝0.151m，单连杆质量m＝0.8kg；地面摩擦力系数c_t＝0.5，c_n＝3；旋转动力学参数λ₁＝0.5，λ₂＝20；视线导引律参数Δ＝1.4；蜿蜒运动步态参数α＝0.045m，ω＝120°/s，δ＝60°；航向角控制器带宽ω_c1＝0.3rad/s，航向角观测器带宽ω_o1＝0.6rad/s；关节角控制器带宽ω_c2＝4rad/s，关节角观测器带宽ω_o2＝12rad/s；仿真时间t＝200s，采样时间ΔT＝0.01s。各初始值为:φ＝0，θ＝90°，p_x＝0m，p_y＝1m，v_θ＝0°，v_φ＝0rad/s，v_t＝0.02m/s，v_n＝0m/s。

(a)验证关节角ESO对模型未知部分的估计效果

蛇形机器人蜿蜒向前运动是由于受到各向异性摩擦力的原因，c_t,c_n分别为蛇形机器人切向和法向的摩擦力系数。蛇形机器人在复杂的运动环境中摩擦系数是很难准确测得的而且是时变的，而摩擦系数c_t,c_n对蛇形机器人的运动控制是非常重要的。本发明设计的关节角自抗扰控制器没有用到摩擦系数，而是将这一部分当做模型未知部分，作为总扰动的一部分利用ESO进行估计并在控制器中补偿。仿真中关节角控制器分别是ADRC和BMC，航向角控制器都使用基于模型的航向角控制器。

仿真设置两种对比：(1)摩擦系数c_t,c_n精确已知，控制器和控制对象中均为c_t＝1.0c_t,c_n＝1.0c_n；(2)摩擦系数c_t,c_n未知，即摩擦系数测量不准确，控制器用c_t＝1.631c_t,c_n＝1.631c_n模拟摩擦系数测量不准确。关节角控制器增益均为K_p2＝20,K_d2＝5。

以上两组仿真结果分别如图3.1和图3.2所示。从仿真结果图3.1我们可以看出，在摩擦系数精确已知时本发明所设计的关节角自抗扰控制器(没用到摩擦系数)和基于模型的控制器(控制器的准确性依赖于摩擦系数)能实现同等的控制效果，即关节角误差、路径跟随误差和航向角误差的瞬态响应和稳态误差都几乎相等；由于蛇形机器人采用蜿蜒步态，所以各关节角变化趋势相似，故仿真中用关节角4的误差图反应关节角的变化情况(下同)。图3.1-d说明ESO的估计扰动效果很好。图3.1表明不依赖模型信息的ADRC也能实现和需要精确已知模型信息的BMC同等的控制效果。从图3.2中可以直观地看出，ADRC的瞬态性能明显优于BMC，而且从关节角误差图3.2-c可知ARDC稳态控制精度远远优于BMC，即在摩擦系数测定不准确的情况下，ADRC展现出了明显的优势。图3.2-d表明在一定的误差范围内ESO估计的扰动F2能跟上真实值。

此外，由于基于模型的关节角控制器(BMC)设计需要利用模型信息(即需要摩擦系数c_t,c_n)，其控制性能依赖系统模型的精确性，图3.2-c中当摩擦系数测定不精确时，BMC的控制效果与图3.1-c中利用精确模型的BMC的效果相比，在瞬态响应和稳态误差方面，摩擦系数的不精确都导致了BMC性能的明显恶化：瞬态响应超调增大，振荡增强，到达稳态时间增加为图3.1-c的5倍左右；稳态误差峰值增大为图3.1-c的3倍左右。由此可见，BMC只能在模型精确已知的条件下才能保证较为优异的控制性能。反之，观察图3.1-c和图3.2-c的ADRC发现，摩擦系数的测定是否准确对ADRC性能的影响较小，这是由于该算法将有关摩擦系数的动力学模型项都看作系统总扰动的一部分，而不需要摩擦系数信息，因此，不论摩擦系数信息准确与否，都被ESO当作总扰动的一部分进行估计并在控制器中被补偿。通过观察图3.2-a和图3.2-b可知，在摩擦系数未知的情况下，由于BMC需要摩擦系数信息而ADRC不需要模型信息，所以ADRC的超调量、瞬态响应和到达稳态的时间均明显小于BMC。

综上，BMC依赖精确的模型信息，而ADRC对模型是否准确并不敏感，考虑实际环境，无法精确获取系统的模型信息，因此，ADRC更具有实际应用优势。

(b)验证ESO对外部未知扰动的估计效果

分两种情况验证ESO对外部未知扰动的估计效果，一种是当系统达到稳态时加入恒值扰动；另一种是当系统达到稳态时加入时变扰动。

1)当系统达到稳态时加入恒值扰动

为模拟这种情况，在仿真时间100～110s中加入恒值扰动：

D₁＝0.03，D₂＝[0.6 0.6 0.6 0.6 0.6 0.6]^T

其中D₁为航向角动力学外部未知恒值扰动，D₂为关节角动力学外部未知恒值扰动。其他仿真参数同上。仿真结果如图4.1所示。

从图4.1-a和4.1-b可以看出当控制系统达到稳态后加入恒值扰动，ADRC不论是瞬态响应还是超调量均明显优于BMC，而且可以明显看出ADRC对扰动的抵抗力很高，鲁棒性很强。这是由于ADRC中将这一部分外部恒值扰动作为总扰动的一部分，用ESO进行估计并在控制器中加以补偿，从图4.1-e和图4.1-f可以看出，ESO的估计扰动效果十分优越，所以ADRC的控制性能明显优于BMC。图4.1-c表明加入外部扰动时对关节角的影响甚小，但从图3.2-c中可以看到当摩擦系数测定不准确时，本发明所设计关节角ADRC的控制性能明显优于BMC。图4.1-d表明在整个控制系统运行过程中，蛇形机器人的切向速度始终大于0。综上所述，ADRC在抵抗外部恒值扰动方面的性能远远优于BMC。

2)当系统达到稳态时加入时变扰动

为模拟这种情况，在仿真时间100～110s中加入外部时变扰动：

其中D₁为航向角动力学外部未知时变扰动，D₂为关节角动力学外部未知时变扰动。其他仿真参数同上，仿真结果如图4.2所示。

从仿真4.2-a和4.2-b可以看出，ADRC的超调量小于BMC且重新恢复到稳定状态时间也小于BMC，很明显ADRC抗扰性更高，鲁棒性更强。这是由于ADRC中将外部时变扰动作为总扰动的一部分，用ESO进行估计并在控制器中加以补偿，从图4.2-e和图4.2-f可以看出，ESO的估计扰动效果很好，所以ADRC的控制性能明显优于BMC。图4.2-d表明在整个控制系统运行过程中，蛇形机器人的切向速度始终大于0，即蛇形机器人一直在运动前进。图4.2-c表明加入外部扰动时对关节角的影响较小，但从第一组仿真可知当摩擦系数测定不准确时，本发明所设计ADRC能补偿由于模型信息未知带来的影响。综上所述，当存在外部时变扰动时ADRC的控制性能明显优于BMC。

通过以上两组仿真对比，可以得出结论：ADRC与BMC相比，抗扰性高，鲁棒性强，控制精度高。在实际应用中，由于蛇形机器人的运动环境复杂，存在很多的不确定性，所以ADRC具有更高的实际应用价值。

经过上述分析，验证了本发明所设计的蛇形机器人路径跟随自抗扰控制方法的有效性。

Claims (3)

1.一种蛇形机器人路径跟随自抗扰控制方法，其特征在于，通过航向角自抗扰控制器的航向角扩张状态观测器与关节角自抗扰控制器的关节角扩张状态观测器分别估计蛇形机器人的航向角的模型未知部分和外部未知扰动在内的航向角总扰动以及关节角模型的未知部分和外部未知扰动在内的关节角总扰动，然后通过对应的控制量补偿航向角总扰动、关节角总扰动对控制系统造成的影响，从而实现蛇形机器人的路径跟随控制：

所述航向角自抗扰控制器根据蛇形机器人的实时的航向参考角输出关节偏离量作为步态模式生成器的输入，所述步态模式生成器输出关节参考角度作为所述关节角自抗扰控制器的输入，所述关节角自抗扰控制器输出关节驱动力到控制对象；

所述的航向角自抗扰控制器输出的关节偏离量如下：

所述的步态模式生成器输出蜿蜒步态，表示如下：

φ_ref,j＝αsin(ωt+(j-1)δ)+φ₀，

F₁代表航向角动力学系统总扰动，

是F₁的估计值，D₁表示航向角动力学外部未知扰动，K_p1＝ω_c1 ²，K_d1＝2ω_c1，ω_c1是航向角自抗扰控制器带宽，e₁＝θ_ref-θ，θ_ref为蛇形机器人实时的航向参考角，B₁＝λ₂v_t，v_t∈R是蛇形机器人的切向速度，θ是蛇形机器人当前的航向角，v_θ∈R表示航向角速，λ₁是航向角速度对航向角加速度的映射标量参数，λ₂是关节坐标与前进速度对航向角加速度的映射标量参数；N为蛇形机器人的连杆的数量；

φ＝(φ₁,φ₂,Lφ_N-1)∈R^N-1，为蛇形机器人当前的关节角向量，

t表示时间；

φ_ref,j表示第j个关节参考角，α和ω分别表示步态模式生成器的蜿蜒步态关节运动的幅值和角频率，δ表示关节运动的相位差；

所述的关节角自抗扰控制器输出的关节驱动力表示如下：

其中，e₂＝φ_ref-φ，φ_ref是通过步态模式生成器得到的参考关节角，

ω_c2是关节角自抗扰控制器带宽，

φ＝(φ₁,φ₂,Lφ_N-1)∈R^N-1，为蛇形机器人当前的关节角向量，v_φ∈R^N-1表示关节角速度向量，F₂代表关节角动力学系统总扰动，

是F₂的估计值，D₂表示关节角动力学外部未知扰动，m为蛇形机器人的连杆质量，

是蛇形机器人前进的推力系数，表示关节坐标与连杆速度到推进力的映射，c_t,c_n分别为蛇形机器人切向和法向的摩擦力系数，l为连杆长度，A、D为常数矩阵；

2.根据权利要求1所述的蛇形机器人路径跟随自抗扰控制方法，其特征在于，所述的蛇形机器人实时的航向参考角表示如下：

其中，

是路径跟踪误差，Δ>0是前向距离的设计参数。

3.根据权利要求2所述的蛇形机器人路径跟随自抗扰控制方法，其特征在于，其中，所述的路径跟踪误差

式中，p_y为蛇形机器人质心坐标之纵坐标。

Images