CN110244714A - 基于滑模控制的机器人单腿摆动相双闭环控制方法 - Google Patents

Abstract

本发明公开了一种基于滑模控制的机器人单腿摆动相双闭环控制方法，建立四足机器人的运动学模型；根据四足机器人的运动学模型，建立四足机器人摆动相动力学模型；设计四足机器人摆动相轨迹；设计运动学、动力学双闭环控制器，实现机器人关节角度的跟踪。本发明将外环的输出作为内环的参考输入，可以对轨迹的变化及时做出反应，响应速度很快且可以实现无差跟踪。

Description

基于滑模控制的机器人单腿摆动相双闭环控制方法

技术领域

本发明属于机器人控制技术，具体为一种基于滑模控制的机器人单腿摆动相双闭环控制方法。

背景技术

从上世纪末开始，世界上很多国家及研究机构都对四足机器人的研究拥有极大兴趣，目前较为成功的要属波士顿动力公司研发的BIGDOG四足机器人，其具有相当强的地形适应性和复杂环境的抗扰动能力。但是，相关研究资料并没有公开，因此其控制算法一直不为人所知。目前，对于四足机器人的控制问题，主要分为以下三类：

(1)基于静态稳定的控制方法

步态轨迹规划方法是足式机器人通常采用的基于静态稳定的控制方法，由此进行腿部关节的协调运动。但是这种方法机器人速度不够快，在处理动步态行走的四足仿生机器人系统时有很多困难，仅仅依靠上述运动控制方法很难实现复杂的运动要求与自适应性。

(2)基于生物神经调节机制和行为特性的控制方法

随着神经生物学与相关学科的深入研究，出现了一种新的解决低速条件下多足机器人多关节运动祸合问题的思路，即模仿生物中枢神经结构的CPG仿生控制。研究人员构造了由多个神经振荡器组成的CPG神经元网络，腿部的关节通常受一个神经元控制。但是对于复杂环境情况，CPG神经元网络具有很大局限性，无法满足复杂环境下的运动控制。

(3)基于动力学模型的控制方法

基于动力学模型的控制方法在诸如机械臂等工业机器人上取得了很好的效果。但相比较机械臂而言，足式机器人由于通常具有基座浮动(足式机器人的机体或上平台)、腿部关节欠驱动或冗余驱动等特点，这些都使基于动力学模型的控制方法在四足仿生机器人上的应用受到限制，相关的研究方法也很少。

发明内容

本发明的目的在于提出了一种基于滑模控制的机器人单腿摆动相双闭环控制方法。

实现本发明的技术解决方案为：一种基于滑模控制的机器人单腿摆动相双闭环控制方法，具体步骤为：

步骤1、建立四足机器人的运动学模型；

步骤2、根据四足机器人的运动学模型，建立四足机器人摆动相动力学模型；

步骤3、设计四足机器人摆动相轨迹；

步骤4、设计运动学、动力学双闭环控制器，实现机器人关节角度的跟踪。

优选地，步骤1建立四足机器人运动学模型的具体方法为：根据D-H坐标系建立规则，建立四足机器人各足在机器人质心坐标系下的位置与各关节角度的关系模型，对该关系模型求关于各个关节角的偏微分，得到雅可比矩阵，对雅可比矩阵求逆得到逆雅可比矩阵。

优选地，四足机器人各足在机器人质心坐标系下的位置与各关节角度的关系模型为：

其中，a₁,a₂,a₃是腿各个关节连杆的长度，θ₁,θ₂,θ₃是各关节的旋转角度，b,w分别为机器人机身的长度、宽度的一半，h为机器人的高度。

优选地，得到的雅可比矩阵为：

优选地，步骤2根据四足机器人足端位置与关节角度之间的关系，建立四足机器人摆动相动力学模型的具体方法为：

步骤2-1、根据各杆件的动能和势能，确定拉格朗日函数为所有杆件动能与势能之差：

L＝K-P

其中，K为系统动能，P为系统势能；

步骤2-2、根据下式，确定关节力矩τ与各关节角度θ、角速度之间的关系：

2-3、根据各关节力矩，确定四足机器人摆动相动力学模型为：

其中，M_ij、C_ij为关于m₀、m₁、m₂、m₃、θ_i、的质量矩阵，C_ij为关于m₀、m₁、m₂、m₃、θ_i、的哥氏力和向心力矩阵，θ_i为各个关节的旋转角，为角速度，为关节的角加速度，G_j为重力分量，τ_i为各关节的力矩，m₀为机器人机身质量，m₁,m₂,m₃为各连杆质量。

优选地，步骤3设计的四足机器人摆动相轨迹为：

其中，x为足端在机身坐标系下的实际位置，z为足端在机身坐标系下的高度，S为单腿步长，T为整个周期，其中摆动相只占一半周期，h为腿摆动过程中最高点到机身的距离，H为机器人机身高度。

优选地，步骤4中设计的运动学、动力学双闭环控制器分别为：

外环运动学控制器为：

其中，为足端期望速度，e＝x_d-x，x_d为足端期望位置，x为足端实际位置，K为控制器比例系数；

内环动力学控制器为：

其中，其中k_di为控制器的比例常数，s为滑模函数，λ_i＞0,i＝1,2,3λ_i为常系数。

优选地，滑模函数为：

其中，θ(t)为期望角度，θ_d(t)为实际角度，为误差的时间导数。

本发明与现有技术相比，其显著优点为：本发明克服了运动学逆解引起的误差累积问题，并且对于外界扰动具有抗扰动能力，有较强鲁棒性。

下面结合附图对本发明做进一步详细的描述。

附图说明

图1是四足机器人单腿D-H坐标图。

图2是规划的理想摆动相轨迹图。

图3是运动学、动力学双闭环控制结构图。

图4是规划的摆动相轨迹和从任意位置开始的实际轨迹图。

图5是关节1的期望角度和实际角度曲线对比图。

图6是关节2的期望角度和实际角度曲线对比图。

图7是关节3的期望角度和实际角度曲线对比图。

具体实施方式

一种基于滑模控制的机器人单腿摆动相双闭环控制方法，具体步骤为：

步骤1、建立四足机器人的运动学模型；

根据机器人的D-H坐标系建立规则，建立四足机器人各足在机器人质心坐标系下的位置与各关节角度的关系模型。四足机器人各条腿的D-H坐标系如图1所示。其中，躯干坐标系{O}在躯干的几何中心，O_x指向机器人的前进方向，O_z指向重心的反方向，O_y指向由右手法则确定。在躯干的四个角分别建立四个过渡坐标系(k＝1,2,3,4，表示各腿的序号)。

建立机器人的D-H坐标系后，机器人单腿各连杆D-H参数分别如表1所示：

表1

连杆i	α<sub>i-1</sub>扭角	关节角	杆件长度
				1	0	θ<sub>1</sub>	0
2	90°	θ<sub>2</sub>	a<sub>1</sub>
				3	0	θ<sub>3</sub>	a<sub>2</sub>
4	0	0	a<sub>3</sub>

如果将固连在机器人躯干四个角的过渡坐标看作基坐标系，则机器人足相对于过渡坐标系的位姿与通用串联机械手中末端执行器相对于基座的位姿分析完全相同。在图1所示的机器人D-H坐标系中，不考虑各腿序号的影响，从机器人髋关节到足端的4个连杆坐标系{O¹,O²,O³,O⁴}可简写为{1},{2},{3},{4}。其中，连杆坐标系{i}相对于{i-1}的位姿关系可用连杆变换^i-1T_i来描述。

根据D-H坐标表，坐标系{i}和坐标系{i-1}之间的变换关系为：

^i-1T_i＝Rot(x,α_i-1)×Trans(x,a_i-1)×Rot(z,θ_i) (1.1)

其中，Rot(x,α_i-1)表示绕x_i-1轴旋转α_i-1角度，Trans(x,a_i-1)表示沿x_i-1轴平移a_i-1，Rot(z,θ_i)表示绕z_i轴旋转θ_i角度。

定义躯干质心坐标系{B}，其坐标系方向与躯干坐标系{O}一致，坐标系原点位于机器人躯干的几何中心，此时，假设过渡坐标系(i＝1,2,3,4)在躯干质心坐标系{B}下的坐标为：[x_ref y_ref z_ref]^T，则各足端在以躯干中心原点为坐标系的位置与各关节角之间关系为：

其中，a₁,a₂,a₃是腿各个关节连杆的长度，θ₁,θ₂,θ₃是各关节的旋转角度，b,w分别为机器人机身的长度、宽度的一半，h为机器人的高度。

对式1.2求关于各个关节角的偏微分，则机器人各足在躯干坐标系下的正微分运动学模型为：

其中，J为雅可比矩阵，具体为：

在实际情况中，该雅可比矩阵满秩，因此逆雅可比矩阵可以通过对J的求逆获得。

步骤2、根据四足机器人的运动学模型，建立四足机器人摆动相动力学模型；

根据四足机器人的运动学模型，连杆坐标系{1},{2},{3},{4}的原点表示为P_org1,P_org2,P_org3,P_org4，则各坐标原点为：

首先，根据各关节在机身坐标系下的位置，求出各杆件的重力势能为：

根据各连杆的位置函数，并对位置函数求关于时间的导数，获得各连杆质心的运动速度，对速度求积分，求得各杆件的动能为：

其中，θ_i，i＝1,2,3为各个关节的角度，为角速度，为关节的角加速度，，m₀为机器人机身质量，m₁,m₂,m₃为各连杆质量。

采用拉格朗日法对四足机器人进行动力学分析，确定拉格朗日函数为：

L＝K-P (2.11)

式中，L是拉格朗日函数，K为系统动能，P为系统势能，即

L＝E_k1+E_k2+E_k3-E_p1-E_p2-E_p3 (2.12)

对上式采用拉格朗日函数算子，求出大小腿各关节力矩

其中，θ＝[θ₁ θ₂ θ₃]^T为各个关节的旋转角，τ＝[τ₁ τ₂ τ₃]^T，为各个关节的力矩。经计算化简，四足机器人单腿摆动相的动力学模型如下：

其中，M_ij为质量矩阵，C_ij为哥氏力和向心力矩阵，G_j为重力分量，τ_i为各关节的力矩，具体地：

M₁₂＝M₁₃＝M₂₁＝M₃₁＝0

C₂₆＝-a₂a₃m₂sinθ₃

C₁₁＝C₁₂＝C₁₃＝C₁₆＝C₂₂＝C₂₄＝C₂₅＝C₃₃＝C₃₄＝C₃₅＝C₃₆＝0

将式(2.14)简写为：

其中，θ＝[θ₁ θ₂ θ₃]^T为各个关节的旋转角，为各个关节的角速度，为各个关节的角加速度，τ＝[τ₁ τ₂ τ₃]^T，为各个关节的力矩，M为质量矩阵，C为哥氏力和向心力矩阵，G为重力矩阵。

步骤3、设计四足机器人摆动相轨迹；

步态是各腿在时间和空间上的运动顺序，平整地面上多为周期步态。当腿与地面接触时，腿的状态为支撑相，腿在空中摆动时状态为摆动相。对角小跑步态(Trot步态)是稳定性极高的动步态，其对角线上的腿运动一致。

设定对角小跑步态占空比为0.5，即每个时刻都有两条腿处于着地状态，另外两条腿处于空中摆动状态。

Trot步态时，对角线上两足运动状态一致，支撑相两足相对于各自髋部的速度是一致的，足端轨迹采用多项式形式，四足机器人摆动相轨迹如式(3.1)、(3.2)所示,Trot步态足端轨迹如图2所示。

其中，X_trot为trot步态足端在机身坐标系下的x方向的坐标，Z_trot为trot步态足端在机身坐标系下的高度，S为单腿步长，T为整个周期，其中摆动相只占一般周期，h为腿摆动过程中最高点到机身的距离，H为机器人机身高度。

4.运动学、动力学双闭环控制器

步骤4、设计运动学、动力学双闭环控制器，实现机器人关节角度的跟踪。

设计外环运动学控制器：

控制目标是在给定足端运动轨迹的情况下，产生相应的关节角度使得实际的足端位置能够跟踪给定输入。要解决这一问题，采用末端执行器位置的期望值与实际值之间的误差来进行控制目标，令：

e＝x_d-x (4.1)

在雅可比矩阵J(θ)为非奇异方阵的情况下，设计控制器为：

其中，e＝x_d-x为期望位置与实际位置之间的误差。

下面，证明该控制器可以使得误差e收敛到0。

根据微分运动学(1.3)，对式(4.1)求导，得

其中：

为逆雅可比矩阵。

其中：

J₁₁＝0

J₁₂＝cos(θ₁)/(a₁+a₃cos(θ₂+θ₃)+a₂cos(θ₂))

J₁₃＝sin(θ₁)/(a₁+a₃cos(θ₂+θ₃)+a₂cos(θ₂))

J₂₁＝sin(θ₂+θ₃)/(a₂sin(θ₃))

J₂₂＝(cos(θ₂+θ₃)sin(θ₁))/(a₂sin(θ₃))

J₂₃＝-(cos(θ₂+θ₃)cos(θ₁))/(a₂sin(θ₃))

J₃₁＝-(a₃sin(θ₂+θ₃)+a₂sin(θ₂))/(a₂a₃sin(θ₃))

J₃₂＝-(sin(θ₁)(a₃cos(θ₂+θ₃)+a₂cos(θ₂)))/(a₂a₃sin(θ₃))

J₃₃＝(cos(θ₁)(a₃cos(θ₂+θ₃)+a₂cos(θ₂)))/(a₂a₃sin(θ₃))

将式(4.3)代入式(4.4),得到线性系统

当K为一个正定矩阵，则系统是渐进稳定的，误差沿轨迹趋于零，收敛速度取决于矩阵K的特征值，特征值越大，收敛速度越快。

设计内环动力学控制器

由于四足机器人通常是以关节角度进行动力学建模的，通过设计执行机构施加的关节扭矩τ，实现关节角度和关节角速度的跟踪。

由于四足机器人四条腿之间动力学特性相同，因此在这里只需要讨论单腿的位置跟踪问题，并将其推广到四足即可。

考虑四足机器人单腿三关节，其动力学方程为

其中，θ∈R³是表示关节变量的向量，τ∈R³是执行机构施加的关节扭矩向量，M(θ)∈R^3×3为对称正定惯性矩阵，为哥氏力和离心力向量，G(θ)∈R³为重力向量。

针对式(4.6)，取误差定义

其中，λ_i＞0,i＝1,2,3。

滑模函数为：

设计内环动力学控制器为：

其中，k_di＞0,i＝1,2,3。

根据李雅普诺夫定理，证明该控制结构的稳定性。

设计Lyapunov函数为

则有

将控制律式(4.9)代入上式，得

由于为一个反对称矩阵，则有

其中， λ_Dmax分别为K_d、M的最小和最大特征值。

不等式方程的解为

从而可得，当t→∞时，滑模函数s趋近于零，即且指数收敛，收敛的精度取决于参数μ的值。

通过本发明建立如图3所示的控制模型，实现规划腿部运动轨迹的跟踪。图4中，虚线轨迹是规划的摆动轨迹，实线轨迹是运用本发明设计的位置-角度双闭环控制器实际的跟踪曲线，在跟踪的前半部分，存在一定的跟踪误差，但是后半阶段的跟踪曲线完全实现了给定轨迹的跟踪。图5、6、7中，虚线为外环的位置控制器根据给定的足端轨迹生成的关节角度变化曲线，这一关节值作为内环动力学控制器的给定参考输入，图中的实线即为内环滑模控制器的实际角度曲线，可以看到实际的角度基本上可以跟踪给定的输入角度信号。

本发明基于四足机器人的动力学模型对其进行控制，基于四足机器人的运动轨迹，设计了位置-角度双闭环控制器，在运动学外环，设计了前馈及比例控制器，在动力学内环，设计了滑模控制器。本发明能够实现多关节机器人给定轨迹的跟踪，本发明将外环的输出作为内环的参考输入，可以对轨迹的变化及时做出反应，响应速度很快且可以实现无差跟踪。

Claims (8)

1.一种基于滑模控制的机器人单腿摆动相双闭环控制方法，其特征在于，具体步骤为：

步骤1、建立四足机器人的运动学模型；

步骤2、根据四足机器人的运动学模型，建立四足机器人摆动相动力学模型；

步骤3、设计四足机器人摆动相轨迹；

步骤4、设计运动学、动力学双闭环控制器，实现机器人关节角度的跟踪。

2.根据权利要求1所述的基于滑模控制的机器人单腿摆动相双闭环控制方法，其特征在于，步骤1建立四足机器人运动学模型的具体方法为：根据D-H坐标系建立规则，建立四足机器人各足在机器人质心坐标系下的位置与各关节角度的关系模型，对该关系模型求关于各个关节角的偏微分，得到雅可比矩阵，对雅可比矩阵求逆得到逆雅可比矩阵。

3.根据权利要求2所述的基于滑模控制的机器人单腿摆动相双闭环控制方法，其特征在于，四足机器人各足在机器人质心坐标系下的位置与各关节角度的关系模型为：

z_ref＝-h

其中，a₁,a₂,a₃是腿各个关节连杆的长度，θ₁,θ₂,θ₃是各关节的旋转角度，b,w分别为机器人机身的长度、宽度的一半，h为机器人的高度。

4.根据权利要求2所述的基于滑模控制的机器人单腿摆动相双闭环控制方法，其特征在于，得到的雅可比矩阵为：

5.根据权利要求1所述的基于滑模控制的机器人单腿摆动相双闭环控制方法，其特征在于，步骤2根据四足机器人足端位置与关节角度之间的关系，建立四足机器人摆动相动力学模型的具体方法为：

步骤2-1、根据各杆件的动能和势能，确定拉格朗日函数为所有杆件动能与势能之差：

L＝K-P

其中，K为系统动能，P为系统势能；

步骤2-2、根据下式，确定关节力矩τ与各关节角度θ、角速度之间的关系：

2-3、根据各关节力矩，确定四足机器人摆动相动力学模型为：

其中，M_ij、C_ij为关于m₀、m₁、m₂、m₃、θ_i、的质量矩阵，C_ij为关于m₀、m₁、m₂、m₃、θ_i、的哥氏力和向心力矩阵，θ_i为各个关节的旋转角，为角速度，为关节的角加速度，G_j为重力分量，τ_i为各关节的力矩，m₀为机器人机身质量，m₁,m₂,m₃为各连杆质量。

6.根据权利要求1所述的基于滑模控制的机器人单腿摆动相双闭环控制方法，其特征在于，步骤3设计的四足机器人摆动相轨迹为：

其中，x为足端在机身坐标系下的实际位置，z为足端在机身坐标系下的高度，S为单腿步长，T为整个周期，其中摆动相只占一半周期，h为腿摆动过程中最高点到机身的距离，H为机器人机身高度。

7.根据权利要求1所述的基于滑模控制的机器人单腿摆动相双闭环控制方法，其特征在于，步骤4中设计的运动学、动力学双闭环控制器分别为：

外环运动学控制器为：

其中，为足端期望速度，e＝x_d-x，x_d为足端期望位置，x为足端实际位置，K为控制器比例系数；

内环动力学控制器为：

其中，其中k_di为控制器的比例常数，s为滑模函数，λ_i＞0,i＝1,2,3λ_i为常系数。

8.根据权利要求7所述的基于滑模控制的机器人单腿摆动相双闭环控制方法，其特征在于，滑模函数为：

其中，θ(t)为期望角度，θ_d(t)为实际角度，为误差的时间导数。