CN109814386B - 基于无模型外环补偿的机器人轨迹跟踪自抗扰控制方法 - Google Patents

Abstract

本发明涉及一种基于无模型外环补偿的机器人轨迹跟踪自抗扰控制方法，步骤如下：步骤一：建立全方位移动机器人系统动力学模型；步骤二：根据动力学模型设计扩张状态观测器；步骤三：根据解析加速度控制方法设计自抗扰控制器；自抗扰控制器由两部分组成，一部分用于补偿系统的总扰动，另一部分用于机器人的轨迹跟踪控制，引入比例微分反馈，并根据伪雅可比矩阵估计值得出无模型自适应控制器，将无模型自适应控制加在扩张状态观测器中。

Description

基于无模型外环补偿的机器人轨迹跟踪自抗扰控制方法

技术领域

本发明涉及全方位移动机器人控制领域，尤其涉及一种基于无模型外环补偿的全方位移动机器人轨迹跟踪自抗扰控制。

背景技术

全方位移动机器人是一种满足完整约束条件，并在平面内具有三个自由度的机器人，即可以同时且独立的进行平移和旋转运动。它能够通过狭窄的通道、直角弯道，能够在需要精确定位和高精度轨迹跟踪的场合进行自身位姿的细微调整，且能够实现零曲率半径运动。但由于全方位移动机器人系统是典型的非线性、强耦合的时变多输入多输出系统，很难建立准确的系统模型，因此其轨迹跟踪控制成为机器人研究领域的一个具有挑战性的热点问题。

针对全方位移动机器人的轨迹跟踪问题，国内外的研究人员们进行了大量的研究。日本佐贺大学学者研究了解析加速度控制方法、比例积分微分(PID)控制方法、随机模糊伺服法和模糊模型法四种经典控制方法(会议：SecondInternationalConferenceonKnowledge-Based Intelligent Electronic Systems Second International Conferenceon Knowledge-Based Intelligent Electronic Systems；著者：WatanabeK；出版年月：1998；文章题目：Controlofanomnidirectional mobilerobot；页码：51-60)；美国俄亥俄大学学者研究了一种将轨迹进行线性化的控制方法，为全方位移动机器人设计了非线性控制器。(期刊：Robotics andAutonomous Systems；著者： YongLiu,J.JimZhu,RobertL.WilliamsII,JianhuaWu；出版年月：2008；文章题目： Omni-directionalmobile robotcontrollerbasedontrajectory linearization；页码：461-479)。康奈尔大学学者研究了一种针对理想的全方位移动机器人的近似最佳的控制算法，并利用仿真验证了算法的有效性(期刊：Robotics&Autonomous Systems；著者：T.Kalmár-Nagy,R. D’Andrea,P.Ganguly；出版年月：2004；文章题目：Near-optimal dynamictrajectory generation andcontrolofanomnidirectionalvehicle；页码：47-64)；台湾地区 中国文化大学学者提出了一种平滑切换自适应滑模控制器，使其在存在结构化和非结构化不确定性的情况下处理全方位移动机器人的跟踪任务。(期刊：IEEE Transactions on Control Systems Technology；著者：Jeng-TzeHuang,TranVanHung,Ming-LeiTseng；出版年月：2015；文章题目：SmoothSwitching RobustAdaptive Control forOmnidirectional Mobile Robots；页码：1986-1993)。日本立命馆大学的学者将自抗扰控制用于全方位移动机器人，使用扰动观测器来估计总扰动。(会议： IEEE ISR2013；著者：ChaoRen,ShugenMa；出版年月：2013；文章题目：Analysis andcontrol ofanomnidirectionalmobilerobot)。

发明内容

本发明的目的是提供一种可以实现动力学模型不确定和外部扰动同时存在的条件下对全方位移动机器人精确控制的全方位移动机器人轨迹跟踪自抗扰控制方法。本发明首先给出全方位移动机器人系统的动力学模型；然后采用扩张状态观测器ESO估计动力学模型的不确定性以及系统外部扰动；接着根据解析加速度控制方法RAC设计自抗扰控制器，并将扩张状态观测器估计出来的扰动信息加入到自抗扰控制器中。由于扩张状态观测器和自抗扰控制器的设计都是基于动力学模型信息的，当模型信息不准确时，尤其是当惯性矩阵与输入矩阵不准确时，扩张状态观测器和自抗扰控制器的实际应用效果会受到影响，所以最后还需要在扩张状态观测器和自抗扰控制器组成的自抗扰控制ADRC系统的外环，加入无模型自适应控制MFAC进行补偿。技术方案如下：

一种基于无模型外环补偿的机器人轨迹跟踪自抗扰控制方法，步骤如下：

步骤一：建立全方位移动机器人系统动力学模型

定义世界坐标系{W}和移动坐标系{M}，基于拉格朗日方程建立全方位移动机器人的动力学模型，并用一个未知矩阵表示系统的总扰动，包括机器人系统的未建模部分、参数的不确定性和外部扰动等，得到世界坐标系下含有未知总扰动的全方位移动机器人动力学模型：

式中，q＝[x y θ]^T表示世界坐标系下机器人的位姿，[·]^T表示矩阵的转置，[·]^-1表示矩阵的逆，x、y和θ分别表示三个自由度的方向，M∈R^3×3表示一个惯性矩阵，∈表示集合间的“属于”关系，R^3×3表示3行3列的实数矩阵，F∈R^3×1表示系统总扰动，B∈R^3×3表示输入矩阵，u∈R^3×1表示控制输入。

步骤二：根据动力学模型即公式(1)设计扩张状态观测器

定义采样时间为T，第k个时刻机器人的位姿为q(k)，第k个时刻的控制输入为u(k)，状态变量x₁(k)＝q(k),x₂(k)＝(q(k)-q(k-1))/T,x₃(k)＝f(k),f(k)表示摩擦力以及未建模动态,设z_i(k),i＝1,2,3,为状态变量x_i(k)的估计值，并令

则状态扩张观测器可设计为：

其中，β_i,i＝1,2,3,为扩张状态观测器的增益矩阵，

ω_o为扩张状态观测器的带宽且ω_o＞0，是扩张状态观测器唯一一个需要调节的参数，由于z₃(k)是x₃(k)的估计值，故总扰动的估计值

步骤三：根据解析加速度控制方法设计自抗扰控制器

自抗扰控制器由两部分组成，一部分用于补偿系统的总扰动，另一部分用于机器人的轨迹跟踪控制。

令e(k)＝q_d(k+1)-q(k)，则自抗扰控制器输出为 u(k)＝B-¹M[(qd(k+1)-2qd(k)+qd(k-1))/T²+K_pe(k)+K_d(e(k)-e(k-1))/T-z₃(k)]

(3)

其中，qd(k)为k时刻的机器人期望位姿，

ω_c表示自抗扰控制器带宽，ξ表示阻尼比。

步骤四：在外环加入无模型自适应控制进行补偿

步骤一至步骤三组成自抗扰控制系统，此自抗扰控制系统可以被视为一个新的非线性系统，并转化为如下基于紧格式动态线性化CFDL的数据模型，记为：

Δq(k+1)＝Φ_c(k)Δu_MFAC(k) (4)

其中，u_MFAC(k)为在k时刻的无模型自适应控制方法的输入，Φ_c(k)为系统的伪雅可比矩阵PJM，且对于任意时刻k是有界的，表示为

且Δq(k+1)＝q(k+1)-q(k)，Δu_MFAC(k)＝u_MFAC(k)-u_MFAC(k-1)。

接下来设计伪雅可比矩阵的估计算法：

其中，η∈(0,2]为步长因子；||·||表示矩阵的2范数；

表示伪雅可比矩阵的估计值。采用如下所示

的重置算法以提高伪雅可比矩阵的估计稳定性：

如果

或

或

如果

或

其中，

是

的初值，b₁、b₂、α为正常数，且满足α≥1，b₂＞2b₁(2α+1)。

引入比例微分反馈，并根据伪雅可比矩阵估计值得出无模型自适应控制器的表达式：

其中，ρ∈(0,1]为步长因子。

将公式(5)-(8)所述的无模型自适应控制加在自抗扰控制系统的外环上，即将无模型自适应控制加在扩张状态观测器中，即将公式(8)加入到公式(2)中，得到：

原有的自抗扰控制器(3)不改变，即

u(k)＝B^-1M[(q_d(k+1)-2q_d(k)+q_d(k-1))/T²+K_pe(k)+K_d(e(k)-e(k-1))/T-z₃(k)]

(10)。

针对全方位移动机器人的轨迹跟踪控制中存在的动力学模型不确定及外部扰动问题，本发明采用基于无模型外环补偿的全方位移动机器人轨迹追踪自抗扰控制进行研究。将基于紧格式的无模型自适应控制模块化，并将其嵌入到自抗扰控制中，使得在无法精确建模的情况下，尤其是当惯性矩阵与输入矩阵不准确时，既能不破坏原有的控制系统又能改善控制系统的品质，使两种控制方法可以优势互补，达到机器人轨迹精确追踪的目的。

附图说明

图1是本发明所设计的控制系统的结构框图；

图2是本发明中全方位移动机器人的坐标系框架示意图；

图3是M^-1B和B^-1M为原来的80％时全方位移动机器人正方形轨迹跟踪控制仿真效果图，图中：

a是平面轨迹曲线；

b是各方向轨迹追踪曲线；

c是本发明所设计的方法与自抗扰控制方法相比各方向轨迹跟踪误差变化曲线；

d是本发明所设计的方法与无模型自适应控制方法相比各方向轨迹跟踪误差变化曲线；

e是控制输入变化曲线；

图4是M^-1B和B^-1M为原来的50％时全方位移动机器人正方形轨迹跟踪控制仿真效果图，图中：

a是平面轨迹曲线；

b是各方向轨迹追踪曲线；

c是本发明所设计的方法与自抗扰控制方法相比各方向轨迹跟踪误差变化曲线；

d是本发明所设计的方法与无模型自适应控制方法相比各方向轨迹跟踪误差变化曲线；

e是控制输入变化曲线。

具体实施方式

基于无模型外环补偿的全方位移动机器人轨迹追踪自抗扰控制方法。该方案首先给出全方位移动机器人系统的动力学模型；然后采用扩张状态观测器估计动力学模型的不确定性及系统外部扰动；接着根据解析加速度控制方法设计控制器，并将观测器估计出来的扰动信息加入到控制器中；最后将基于紧格式的无模型自适应控制加入到已经建立的自抗扰控制系统的外环。

本发明将基于模型的控制方法和不需要模型的控制方法进行结合，使得在无法精确建模的情况下，既能不破坏原有的控制系统，又能改善控制系统的品质。换句话说，就是将自抗扰控制和无模型自适应控制模块化，将不同的控制方法进行模块化组合，实现自抗扰控制与无模型自适应控制之间优势互补的工作机制。

本发明所设计的控制系统的结构框图如图1所示,虚线框内为自抗扰控制系统。具体实施的细化步骤如下所示：

步骤一：建立全方位移动机器人系统动力学模型

定义世界坐标系{W}和移动坐标系{M}，基于拉格朗日方程建立全方位移动机器人的动力学模型，并用一个未知矩阵表示系统的总扰动，包括机器人系统的未建模部分、参数的不确定性和外部扰动等，得到世界坐标系下含有未知总扰动的全方位移动机器人动力学模型：

式中，q＝[x y θ]^T表示世界坐标系下机器人的位姿，[·]^T表示矩阵的转置，[·]^-1表示矩阵的逆，x、y和θ分别表示三个自由度的方向，M∈R^3×3表示一个惯性矩阵，∈表示集合间的“属于”关系，R^3×3表示3行3列的实数矩阵，F∈R^3×1表示系统总扰动，B∈R^3×3表示输入矩阵，u∈R^3×1表示控制输入。

步骤二：根据动力学模型即公式(1)设计扩张状态观测器

定义采样时间为T，第k个时刻机器人的位姿为q(k)，第k个时刻的控制输入为u(k)，状态变量x₁(k)＝q(k),x₂(k)＝(q(k)-q(k-1))/T,x₃(k)＝f(k),f(k)表示摩擦力以及未建模动态，则机器人系统的状态空间描述为：

设z_i(k),i＝1,2,3,为状态变量x_i(k)的估计值，并令

则：

其中，β_i,i＝1,2,3,为扩张状态观测器的增益矩阵，

ω_o为扩张状态观测器的带宽且ω_o＞0，是扩张状态观测器唯一一个需要调节的参数，由于z₃(k)是x₃(k)的估计值，故总扰动的估计值

步骤三：根据解析加速度控制方法设计自抗扰控制器

自抗扰控制器由两部分组成，一部分用于补偿系统的总扰动，另一部分用于机器人的轨迹跟踪控制；

总扰动的补偿部分设计如下：

u₁(k)＝-B^-1Mz₃(k) (4)

令e(k)＝q_d(k+1)-q(k)，则机器人的轨迹跟踪部分设计如下：

u₂(k)＝B^-1M[(q_d(k+1)-2q_d(k)+q_d(k-1))/T²+K_pe(k)+K_d(e(k)-e(k-1))/T] (5)

其中，q_d(k)为k时刻的机器人期望位姿，

ω_c表示自抗扰控制器带宽，ξ表示阻尼比。

由公式(4)和(5)可得自抗扰控制器输出为：

u(k)＝B^-1M[(qd(k+1)-2qd(k)+qd(k-1))/T²+K_pe(k)+K_d(e(k)-e(k-1))/T-z₃(k)]

(6)

步骤四：在外环加入无模型自适应控制进行补偿

步骤一至步骤三组成自抗扰控制系统，此自抗扰控制系统被视为一个新的非线性系统，记为：

q(k+1)＝f(q(k),q(k-1),…,q(k-n_q),u_MFAC(k),u_MFAC(k-1),…,u_MFAC(k-n_u)) (7)

其中，u_MFAC(k)为在k时刻的无模型自适应控制方法的输入，n_q和n_u是两个正整数，f(…)∈R³表示非线性函数。

将公式(7)转化为如下基于紧格式动态线性化CFDL的数据模型：

Δq(k+1)＝Φ_c(k)Δu_MFAC(k) (8)

Φ_c(k)为系统的伪雅可比矩阵PJM，且对于任意时刻k是有界的，表示为

且Δq(k+1)＝q(k+1)-q(k)，Δu_MFAC(k)＝u_MFAC(k)-u_MFAC(k-1)，选择紧格式动态线性化方法作为动态线性化方法，因为其计算量小并且结构简单。

接下来设计伪雅可比矩阵的估计算法：

其中，η∈(0,2]为步长因子；||·||表示矩阵的2范数；

表示伪雅可比矩阵的估计值。采用如下所示

的重置算法以提高伪雅可比矩阵的估计稳定性：

如果

或

或

如果

或

其中，

是

的初值，b₁、b₂、α为正常数，且满足α≥1，b₂＞2b₁(2α+1)。

引入比例微分反馈，并根据伪雅可比矩阵估计值得出无模型自适应控制器的表达式：

其中，ρ∈(0,1]为步长因子。

将公式(9)-(12)所述的无模型自适应控制加在自抗扰控制系统的外环上，这里为了得到更好的控制效果，将无模型自适应控制加在扩张状态观测器中，即将公式(12)加入到公式(3) 中，得到：

原有的自抗扰控制器(6)不改变，即

u(k)＝B^-1M[(q_d(k+1)-2q_d(k)+q_d(k-1))/T²+K_pe(k)+K_d(e(k)-e(k-1))/T-z₃(k)]

(14)

为验证本发明所设计的控制算法的有效性，以MATLAB作为仿真平台，以三轮全方位移动机器人(图1所示)为控制对象进行了全方位移动机器人轨迹跟踪控制仿真实验的验证。下面结合仿真实验和附图，在控制系统中存在模型不确定、参数不准确以及外部扰动的情况下，对本发明提出的全方位移动机器人的轨迹跟踪控制方法做出详细说明。

如图1所示，仿真中全方位移动机器人的任务为按照给定轨迹在平面上运动。仿真中各参数取值如下：机器人质量为35千克，车轮半径为0.06米，x、y方向的相关粘滞摩擦系数为0.5，θ方向的相关粘滞摩擦系数为0.2，x、y方向的相关库伦摩擦系数为40，θ方向的相关库伦摩擦系数为1，以机器人中心为轴的转动惯量为1.35千克·米²，以电机轴为旋转轴的转动惯量为3.15×10^-5千克·米²，接触半径为0.1915米，电机反向电动势常数为 0.029，电机力矩常数为0.029牛顿·米/安培，减速比185.7，电机电阻0.61欧姆。本发明方法中控制器各参数：状态观测器带宽ω_o＝30，控制器带宽ω_c＝6，阻尼比ξ＝4，λ＝μ＝1，ρ＝η＝0.5，b₁＝0.2，b₂＝0.5，α＝10，k_p＝35，k_d＝15，仿真时间为 40秒，采样频率为200赫兹。

由于正方形轨迹有四个直角拐角，可以看作是有一个突加外部扰动，具有较高的追踪难度，所以本方案中选取正方形轨迹为参考轨迹，正方形参考轨迹为关于时间t的函数，t的单位为秒，如下：

θ_d[弧度]＝0.35t

同时，将公式(3)中u前面的系数M^-1B和公式(6)中B^-1M均调整为原来的80％，则正方形轨迹的仿真结果如图3所示。从图3(a)和图3(b)可以看出，采用本发明的控制系统在模型不确定、参数不准确以及外部扰动的条件下，具有良好的跟踪性能，基本可以准确的跟踪期望轨迹；同时，从图3(c)和图3(d)可以看出，系统的跟踪误差很小，在正方形轨迹的四个拐角处，有较大的误差，但系统很快就进行了调节，并且可以看出本发明所提方法的误差比只用ADRC方法和只用MFAC方法时的误差都小，达到了使两种方法优势互补的效果；图 3(e)显示了控制电压随时间的变化曲线。

将公式(3)中u前面的系数M^-1B和公式(6)中B^-1M均调整为原来的50％，正方形轨迹的仿真结果如图4所示。可以看出，在没有任何参数重调的情况下实现了同样优越的控制性能。

经过上述分析，证明了本发明算法的有效性。

Claims (1)

1.一种基于无模型外环补偿的机器人轨迹跟踪自抗扰控制方法，步骤如下：

步骤一：建立全方位移动机器人系统动力学模型

定义世界坐标系{W}和移动坐标系{M}，基于拉格朗日方程建立全方位移动机器人的动力学模型，并用一个未知矩阵表示系统的总扰动，包括机器人系统的未建模部分、参数的不确定性和外部扰动等，得到世界坐标系下含有未知总扰动的全方位移动机器人动力学模型：

式中，q＝[x y θ]^T表示世界坐标系下机器人的位姿，[·]^T表示矩阵的转置，[·]^-1表示矩阵的逆，x、y和θ分别表示三个自由度的方向，M∈R^3×3表示一个惯性矩阵，∈表示集合间的“属于”关系，R^3×3表示3行3列的实数矩阵，F∈R^3×1表示系统总扰动，B∈R^3×3表示输入矩阵，u∈R^3×1表示控制输入；

步骤二：根据动力学模型即公式(1)设计扩张状态观测器

定义采样时间为T，第k个时刻机器人的位姿为q(k)，第k个时刻的控制输入为u(k)，状态变量x₁(k)＝q(k),x₂(k)＝(q(k)-q(k-1))/T,x₃(k)＝f(k),f(k)表示摩擦力以及未建模动态,设z_i(k),i＝1,2,3,为状态变量x_i(k)的估计值，并令

则状态扩张观测器可设计为：

其中，β_i,i＝1,2,3,为扩张状态观测器的增益矩阵，

ω_o为扩张状态观测器的带宽且ω_o＞0，是扩张状态观测器唯一一个需要调节的参数，由于z₃(k)是x₃(k)的估计值，故总扰动的估计值

步骤三：根据解析加速度控制方法设计自抗扰控制器

自抗扰控制器由两部分组成，一部分用于补偿系统的总扰动，另一部分用于机器人的轨迹跟踪控制；

令e(k)＝q_d(k+1)-q(k)，则自抗扰控制器输出为u(k)＝B^-1M[(q_d(k+1)-2q_d(k)+q_d(k-1))/T²+K_pe(k)+K_d(e(k)-e(k-1))/T-z₃(k)](3)

其中，q_d(k)为k时刻的机器人期望位姿，

ω_c表示自抗扰控制器带宽，ξ表示阻尼比；

步骤四：在外环加入无模型自适应控制进行补偿

步骤一至步骤三组成自抗扰控制系统，此自抗扰控制系统可以被视为一个新的非线性系统，并转化为如下基于紧格式动态线性化CFDL的数据模型，记为：

Δq(k+1)＝Φ_c(k)Δu_MFAC(k) (4)

其中，u_MFAC(k)为在k时刻的无模型自适应控制方法的输入，Φ_c(k)为系统的伪雅可比矩阵PJM，且对于任意时刻k是有界的，表示为

且Δq(k+1)＝q(k+1)-q(k)，Δu_MFAC(k)＝u_MFAC(k)-u_MFAC(k-1)；

接下来设计伪雅可比矩阵的估计算法：

其中，η∈(0,2]为步长因子；||·||表示矩阵的2范数；

表示伪雅可比矩阵的估计值；采用如下所示

的重置算法以提高伪雅可比矩阵的估计稳定性：

其中，

是

的初值，b₁、b₂、α为正常数，且满足α≥1，b₂＞2b₁(2α+1)；

引入比例微分反馈，并根据伪雅可比矩阵估计值得出无模型自适应控制器的表达式：

其中，ρ∈(0,1]为步长因子；

将公式(5)-(8)所述的无模型自适应控制加在自抗扰控制系统的外环上，即将无模型自适应控制加在扩张状态观测器中，即将公式(8)加入到公式(2)中，得到：

原有的自抗扰控制器(3)不改变，即

u(k)＝B^-1M[(q_d(k+1)-2q_d(k)+q_d(k-1))/T²+K_pe(k)+K_d(e(k)-e(k-1))/T-z₃(k)] (10)。

Images