CN112558622B - 一种爬壁六足机器人足端轨迹跟踪控制器的控制方法 - Google Patents

Abstract

本发明公开了一种爬壁六足机器人足端轨迹跟踪控制器的控制方法，该方法包括如下步骤：构造爬壁六足机器人的单腿支链的运动学模型；根据微分运动学求解爬壁六足机器人单腿支链的速度雅克比矩阵；设计基于雅可比矩阵的六足机器人关节角速度控制器；基于李雅普诺夫函数证明关节角速度控制器的稳定性。本发明设计的控制器结合了机器人微分运动学与反馈控制思想，具有收敛速度快，跟踪期望足端轨迹误差小等优点，并具有良好的稳定性，可实现六足机器人全方位运动的轨迹跟踪控制。

Description

一种爬壁六足机器人足端轨迹跟踪控制器的控制方法

技术领域

本发明涉及机器人运动控制领域，具体是涉及一种爬壁六足机器人足端轨迹跟踪控制器的控制方法。

背景技术

六足机器人是移动足式机器人的一种，其拥有六个单腿支链，具有良好的稳定性和灵活性，能够在复杂的非结构化地形中自由行走，因此目前被广泛的研究。

六足机器人的跨步运动主要依赖于单腿支链足端从抬起至落下的完整过程，即足端的轨迹规划过程。在确定了足端运动轨迹之后，如何让六足机器人单腿支链的足端能够准确地依照所规划的目标轨迹运动是首要解决的关键问题。因此，需要保证六足机器人的单腿支链关节空间的协调运动的控制结果能够实现在足端笛卡尔空间上对期望目标轨迹的准确跟踪。

如专著文献《机器人学》所述，在传统的机器人运动控制中，通常使用建立机器人逆运动学模型的方法，通过对机器人笛卡尔空间的目标轨迹求逆解，得到关节空间的理想轨迹。该方法具有以下不足：逆运动学方法属于开环控制，未考虑到误差带来的影响，依赖于关节伺服电机的准确控制，仅在理想情况下可以准确跟踪足端目标轨迹；逆运动学解的多解性与非线性也极大提高了该方法的复杂度；逆运动学方法也未考虑到关节速度等因素的影响。因此学者们又提出了分解速度控制的方法，通过对末端执行器的运动分解为各关节的期望速度，然后对各关节实行速度伺服控制，最终实现准确的轨迹跟踪。设计一种可同时考虑到位姿与速度因素且实现简易的闭环反馈控制器算法，对六足机器人的精确控制具有重要的意义。

发明内容

本发明的要解决的技术问题是提供一种爬壁六足机器人足端轨迹跟踪控制器的控制方法，以达到机器人的足端可精确跟踪给定的期望轨迹的目标，实现期望的跨步运动。

本发明至少通过如下技术方案之一实现。

一种爬壁六足机器人足端轨迹跟踪控制器的控制方法，包括步骤：

S1、构造爬壁六足机器人的单腿支链的运动学模型；

S2、根据微分运动学求解爬壁六足机器人单腿支链的速度雅克比矩阵；

S3、设计基于雅可比矩阵的六足机器人关节角速度控制器；

S4、基于李雅普诺夫函数证明关节角速度控制器的稳定性。

优选的，所述爬壁六足机器人单腿支链包括依次以连杆连接的髋关节(1)、膝关节(2)、踝关节(3)、足关节(4)和足端的吸盘(5)。

优选的，所述步骤S1的具体过程为：

S11、构造六足机器人单腿支链的参考坐标系与足端坐标系，定义单腿参考坐标系{L}的原点为髋关节轴线的中心，以髋关节转动轴线方向为z轴，以沿连接髋关节与踝关节的第一连杆的方向为x轴，y轴由右手定则确定；定义足端坐标系{F}的原点位于吸盘中心，以沿平行足关节转动轴线方向为z轴，以沿连接足关节与足端吸盘的第四连杆的方向为x轴，y轴由右手定则确定；

S12、建立足端坐标系{F}相对于参考坐标系的变换矩阵，确定足端坐标系{F}在初始位姿下相对于单腿参考坐标系{L}的变换矩阵为g_L,F(0)；

S13、根据旋量理论确定单腿支链各关节的运动旋量，在已知初始位形下转动关节的转动轴旋转矢量ω_i与轴线位置矢量r_i的情况下，各转动关节的运动旋量表示为

S14、根据旋量指数映射公式求解单腿支链各关节运动旋量的指数映射，在求得各转动关节的运动旋量情况下，相对应的指数映射表示为

即确定第i个转动关节位姿相对于参考坐标系的位姿转换矩阵，其中，θ_i为第i个转动关节的转动角度，

为关于运动旋量ξ_i的4×4李代数矩阵形式；

S15、根据叠加原理确定六足机器人的单腿支链的运动学模型，通过叠加原理将所得的各关节旋量的指数映射左乘于初始位姿的变换矩阵g_L,F(0)，确定爬壁六足机器人单腿支链运动学模型的齐次变换矩阵为：

其中，R为3×3的足端位姿旋转矩阵，p为3×1的足端位置矢量。

优选的，所述足端坐标系{F}在初始位姿下相对于单腿参考坐标系{L}的变换矩阵为g_L,F(0)为：

其中，L₁、L₂、L₃和L₄分别为第一连杆(1)、第二连杆(2)、第三连杆(3)和第四连杆(4)的长度；Trans(L₁+L₂,0,-(L₃+L₄))表示沿参考坐标系x轴平移L₁+L₂距离并沿z轴平移-(L₃+L₄)距离的平移变换矩阵；

表示绕参考坐标系x轴作

旋转的旋转变化矩阵；

表示绕参考坐标系z轴作

旋转的旋转变化矩阵。

优选的，确定第i个转动关节的运动旋量的指数映射为：

其中，θ_i为第i个转动关节的转动角度，I为3×3的单位矩阵，

为关于ω_i的3×3反对称矩阵，

为关于运动旋量ξ_i的4×4李代数矩阵形式。

优选的，所述步骤S2中，根据微分运动学原理，将足端位置矢量p对时间t求导，确定足端位置矢量p与关节转角θ的微分关系为：

其中，

为p对时间t的微分，即足端广义速度；

为θ对时间t的微分，即关节角速度；

的偏导数矩阵，即单腿支链的速度雅可比矩阵。

优选的，所述单腿支链的速度雅可比矩阵为：

其中，记

θ_i、θ_j、θ_k分别表示第i、j、k个关节转动角度，S_ijk表示第i、j、k个关节转动角度之和的正弦值，C_ijk表示第i、j、k个关节转动角度之和的余弦值。

优选的，所述步骤S3中，定义理想足端位置为p_d，则理想足端位置与实际足端位置的误差表示为e＝p_d-p，根据反馈控制原理，设计基于误差的负反馈控制，并加上前馈控制项，设计关节角速度控制器规律为：

其中，

为理想足端位置的微分，即理想足端速度；K为3×3的对角矩阵，K＝diag(k₁,k₂,k₃)，k₁、k₂与k₃分别为控制器中可调节的x、y、z三维的增益参数；J^T(θ)为J(θ)的转置矩阵；

为J(θ)的伪逆矩阵，表示为：

对关节角速度控制器输出求积分，即可得到第i个转动关节转动角度θ_i。

优选的，所述理想足端位置为p_d为：

其中，t表示时间；p_dx(t)表示x轴方向理想轨迹；p_dy(t)表示y轴方向理想轨迹；p_dz(t)表示z轴方向理想轨迹。

优选的，所述步骤S4中，根据现代控制理论设计正定的李雅普诺夫函数为：

将误差表达式与关节角速度控制器规律代入，确定该李雅普诺夫函数的导数为：

与现有技术相比，本发明具有如下有益效果：

(1)本发明的轨迹跟踪控制器引入了六足机器人单腿支链运动学外环反馈控制，相较于传统的机器人采用运动学逆解方法的开环控制，具有跟踪期望足端轨迹误差小、稳定性好的优点。

(2)本发明的轨迹跟踪控制器通过引入雅可比伪逆矩阵，将六足机器人的速度影响因素作为前馈控制项加入控制器中，提高了跟踪速度，具有轨迹跟踪速度快的优点。

附图说明

图1为本发明所述的爬壁六足机器人足端轨迹跟踪控制器的控制方法的流程示意图；

图2为本发明实施例的爬壁六足机器人单腿支链结构模型图；

图3为本发明实施例的足端轨迹跟踪控制器系统框图；

图4为本发明实施例的爬壁六足机器人单腿支链足端x轴方向上的轨迹跟踪仿真图；

图5为本发明实施例的爬壁六足机器人单腿支链足端y轴方向上的轨迹跟踪仿真图；

图6为本发明实施例的爬壁六足机器人单腿支链足端z轴方向上的轨迹跟踪仿真图；

图7为本发明实施例的爬壁六足机器人单腿支链各关节的转动角度变化仿真图；

图中：1-髋关节；2-膝关节；3-踝关节；4-足关节；5-吸盘。

具体实施方式

为了使本技术领域的人员更好地理解本发明方案，下面结合附图和具体实施例对本发明的发明目的作进一步详细地描述。显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例，但本发明的实施方式并不因此限定于以下实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

如图1所示，本实施例提供了一种爬壁六足机器人足端轨迹跟踪控制器的控制方法，包括以下步骤：

S1、基于旋量理论构造爬壁六足机器人的单腿支链的运动学模型。

其中，所述S1步骤中构造运动学模型的具体过程为：

S11、建立六足机器人单腿支链的参考坐标系与足端坐标系。

所述爬壁六足机器人单腿支链模型结构图如图2所示，包括依次以连杆连接的髋关节1、膝关节2、踝关节3、足关节4共四个转动关节和足端的吸盘5。定义单腿参考坐标系{L}的原点为髋关节轴线的中心，以髋关节转动轴线方向为z轴，以沿连接髋关节与踝关节的第一连杆的方向为x轴，y轴由右手定则确定；定义足端坐标系{F}的原点位于吸盘中心，以沿平行足关节转动轴线方向为z轴，以沿连接足关节与吸盘的第四连杆的方向为x轴，y轴由右手定则确定。

S12、建立足端坐标系相对于参考坐标系的变换矩阵。

根据坐标变换关系确定足端系{F}在初始位姿下相对于单腿参考坐标系{L}的变换矩阵为：

其中，L₁＝0.093m、L₂＝0.145m、L₃＝0.164m和L₄＝0.157m，分别为第一连杆1、第二连杆2、第三连杆3和第四连杆4的长度；Trans(L₁+L₂,0,-(L₃+L₄))表示沿参考坐标系x轴平移L₁+L₂距离并沿z轴平移-(L₃+L₄)距离的平移变换矩阵；

表示绕参考坐标系x轴作

旋转的旋转变化矩阵；

表示绕参考坐标系z轴作

旋转的旋转变化矩阵。

S13、根据旋量理论确定单腿支链各关节的运动旋量。

在初始位形下机器人单腿支链4个转动关节的转动轴的矢量ω_i(i＝1,2,3,4)在单腿参考坐标系{L}表示为：

进一步地，取各个轴线上的一点的位置向量r_i(i＝1,2,3,4)，分别表示为：

更进一步地，根据转动关节运动旋量求解公式

可确定各个转动关节的运动旋量为：

S14、根据旋量指数映射公式求解单腿支链各关节运动旋量的指数映射。

第i个转动关节的运动旋量的指数映射公式为：

其中，θ_i为第i个转动关节的转动角度，I为3×3的单位矩阵，

为关于ω_i的3×3反对称矩阵，

为关于运动旋量ξ_i的4×4李代数矩阵形式。

进一步地，代入步骤S13中已确定的数据，可得第i个转动关节的指数映射矩阵，即第i个转动关节位姿相对于参考坐标系的位姿转换矩阵表示为：

S15、根据叠加原理确定六足机器人的单腿支链的运动学模型。

通过叠加原理将所得的各关节旋量指数映射矩阵左乘于初始位姿变换矩阵，确定爬壁六足机器人单腿运动学模型的齐次变换矩阵为：

其中，足端位姿旋转矩阵R为：

足端位置矢量p为：

进一步地，记足端位置与θ的关系为：T(θ)＝p。

S2、根据微分运动学求解爬壁六足机器人单腿支链的速度雅克比矩阵。

将已确定的足端的位置矢量p对时间t求导，确定p与关节转角θ的微分关系为：

其中，

为p对时间t的微分，即足端广义速度；

为θ对时间t的微分，即关节角速度；J(θ)为3×4的偏导数矩阵，即单腿支链的速度雅可比矩阵为：

其中，为便于表示，记

θ_i、θ_j、θ_k分别表示第i、j、k个关节转动角度，S_ijk表示第i、j、k个关节转动角度之和的正弦值，C_ijk表示第i、j、k个关节转动角度之和的余弦值。

S3、设计基于雅可比矩阵的六足机器人关节角速度控制器。

定义理想足端位置为p_d，考虑到爬壁六足机器人运动的可行性，足端应垂直于水平面方向上先运动后停止，而在水平方向上后运动先停止，保证足端垂直于支撑面抬起和落下，从而避免足端与地面发生摩擦。因此，设计如下的理想轨迹：

其中，exp(·)形式函数表示指数积形式函数；t表示时间；p_dx(t)表示x轴方向理想轨迹；p_dy(t)表示y轴方向理想轨迹；p_dz(t)表示z轴方向理想轨迹。进一步地，理想足端位置与实际足端位置的误差为：e＝p_d-p，因此可根据反馈控制原理，设计基于误差的负反馈控制，并加上前馈控制项，设计如下关节角速度控制器规律：

其中，

为理想足端位置的微分，即理想足端速度；K为3×3的对角矩阵，表示为：K＝diag(k₁,k₂,k₃)，k₁、k₂与k₃分别为控制器中可调节的x、y、z三维的增益参数，实施例中取为K＝diag(15000,20000,50000)；J^T(θ)为J(θ)的转置矩阵；

为J(θ)的伪逆矩阵，表示为：

更进一步地，对关节角速度控制器输出求积分，即可得到第i个转动关节转动角度θ_i。

得到的控制器系统框图如图3所示，其中S表示微分变换，1/S表示积分变换。在整个控制系统中，理想轨迹p_d通过微分变换作用得到的理想足端速度

经过伪逆矩阵

变换得到前馈项控制信号；实际转动角度θ经过正运动学变换T(θ)得到实际轨迹p作为负反馈信号；理想轨迹p_d与p作差得到的误差信号e经过控制增益K放大，再经过雅可比转置矩阵J^T(θ)变换得到反馈项控制信号；前馈项控制信号与反馈项控制信号相加后得到关节角速度

关节角速度通过积分变换作用最终得到关节实际转动角度θ。

S4、基于李雅普诺夫函数证明关节角速度控制器的稳定性，设计李雅普诺夫函数证明控制器具有稳定性。

据现代控制理论设计正定的李雅普诺夫函数为：

更进一步地，将误差表达式与控制规律代入，可确定该李雅普诺夫函数的导数为：

因此，可证明该控制系统是李雅普诺夫渐进稳定的。

根据得到的控制框图进行仿真实验，得到的单腿支链足端在x轴、y轴、z轴方向上的轨迹跟踪仿真结果如图4、图5、图6所示，从图中可看出，三个方向上整体的实际轨迹与理想轨迹几乎完全重合，跟踪结束位置与理想情况一致，说明设计的控制系统具有良好的跟踪性。单腿支链各关节的转动角度变化情况如图7所示，从图中可看出曲线平滑变化，说明控制系统输出具有良好的稳定性。

以上所述，仅为本发明较佳的实施例，但本发明的保护范围并不局限于此。任何熟悉本技术领域的技术人员在本发明所公开的范围内，根据本发明的技术方案及其发明构思加以若干等同替换或改变，都属于本发明的保护范围。

Claims (8)

1.一种爬壁六足机器人足端轨迹跟踪控制器的控制方法，其特征在于，包括步骤：

S1、构造爬壁六足机器人的单腿支链的运动学模型；

S2、根据微分运动学求解爬壁六足机器人单腿支链的速度雅克比矩阵；

单腿支链的速度雅可比矩阵为：

其中，记

θ_i、θ_j、θ_k分别表示第i、j、k个关节转动角度，S_ijk表示第i、j、k个关节转动角度之和的正弦值，C_ijk表示第i、j、k个关节转动角度之和的余弦值，i＝1,2,3,4；L₁、L₂、L₃和L₄分别为第一连杆(1)、第二连杆(2)、第三连杆(3)和第四连杆(4)的长度；

S3、设计基于雅可比矩阵的六足机器人关节角速度控制器：定义理想足端位置为p_d，则理想足端位置与实际足端位置的误差表示为e＝p_d-p，根据反馈控制原理，设计基于误差的负反馈控制，并加上前馈控制项，设计关节角速度控制器规律为：

其中，

为理想足端位置的微分，即理想足端速度；K为3×3的对角矩阵，K＝diag(k₁,k₂,k₃)，k₁、k₂与k₃分别为控制器中可调节的x、y、z三维的增益参数；J^T(θ)为J(θ)的转置矩阵；

为J(θ)的伪逆矩阵，表示为：

对关节角速度控制器输出求积分，即可得到第i个转动关节转动角度θ_i；

S4、基于李雅普诺夫函数证明关节角速度控制器的稳定性。

2.根据权利要求1所述爬壁六足机器人足端轨迹跟踪控制器的控制方法，其特征在于，所述爬壁六足机器人单腿支链包括依次以连杆连接的髋关节(1)、膝关节(2)、踝关节(3)、足关节(4)和足端的吸盘(5)。

3.根据权利要求2所述爬壁六足机器人足端轨迹跟踪控制器的控制方法，其特征在于，所述步骤S1的具体过程为：

S11、构造六足机器人单腿支链的参考坐标系与足端坐标系，定义单腿参考坐标系{L}的原点为髋关节轴线的中心，以髋关节转动轴线方向为z轴，以沿连接髋关节与踝关节的第一连杆的方向为x轴，y轴由右手定则确定；定义足端坐标系{F}的原点位于吸盘中心，以沿平行足关节转动轴线方向为z轴，以沿连接足关节与足端吸盘的第四连杆的方向为x轴，y轴由右手定则确定；

S12、建立足端坐标系{F}相对于参考坐标系的变换矩阵，确定足端坐标系{F}在初始位姿下相对于单腿参考坐标系{L}的变换矩阵为g_L,F(0)；

S13、根据旋量理论确定单腿支链各关节的运动旋量，在已知初始位形下转动关节的转动轴旋转矢量ω_i与轴线位置矢量r_i的情况下，各转动关节的运动旋量表示为

S14、根据旋量指数映射公式求解单腿支链各关节运动旋量的指数映射，在求得各转动关节的运动旋量情况下，相对应的指数映射表示为

即确定第i个转动关节位姿相对于参考坐标系的位姿转换矩阵，其中，θ_i为第i个转动关节的转动角度，

为关于运动旋量ξ_i的4×4李代数矩阵形式；

S15、根据叠加原理确定六足机器人的单腿支链的运动学模型，通过叠加原理将所得的各关节旋量的指数映射左乘于初始位姿的变换矩阵g_L,F(0)，确定爬壁六足机器人单腿支链运动学模型的齐次变换矩阵为：

其中，R为3×3的足端位姿旋转矩阵，p为3×1的足端位置矢量。

4.根据权利要求3所述的爬壁六足机器人足端轨迹跟踪控制器的控制方法，其特征在于，所述足端坐标系{F}在初始位姿下相对于单腿参考坐标系{L}的变换矩阵为g_L,F(0)为：

其中，L₁、L₂、L₃和L₄分别为第一连杆(1)、第二连杆(2)、第三连杆(3)和第四连杆(4)的长度；Trans(L₁+L₂,0,-(L₃+L₄))表示沿参考坐标系x轴平移L₁+L₂距离并沿z轴平移-(L₃+L₄)距离的平移变换矩阵；

表示绕参考坐标系x轴作

旋转的旋转变化矩阵；

表示绕参考坐标系z轴作

旋转的旋转变化矩阵。

5.根据权利要求4任一项所述的爬壁六足机器人足端轨迹跟踪控制器的控制方法，其特征在于，确定第i个转动关节的运动旋量的指数映射为：

其中，θ_i为第i个转动关节的转动角度，I为3×3的单位矩阵，

为关于ω_i的3×3反对称矩阵，

为关于运动旋量ξ_i的4×4李代数矩阵形式。

6.根据权利要求5所述的爬壁六足机器人足端轨迹跟踪控制器的控制方法，其特征在于，所述步骤S2中，根据微分运动学原理，将足端位置矢量p对时间t求导，确定足端位置矢量p与关节转角θ的微分关系为：

其中，

为p对时间t的微分，即足端广义速度；

为θ对时间t的微分，即关节角速度；

为3×4的偏导数矩阵，即单腿支链的速度雅可比矩阵。

7.根据权利要求6所述的爬壁六足机器人足端轨迹跟踪控制器的控制方法，其特征在于，所述理想足端位置为p_d为：

其中，t表示时间；p_dx(t)表示x轴方向理想轨迹；p_dy(t)表示y轴方向理想轨迹；p_dz(t)表示z轴方向理想轨迹。

8.根据权利要求7所述的爬壁六足机器人足端轨迹跟踪控制器的控制方法，其特征在于，所述步骤S4中，根据现代控制理论设计正定的李雅普诺夫函数为：

将误差表达式与关节角速度控制器规律代入，确定该李雅普诺夫函数的导数为：

Images