[发明专利]一种四足机器人基于trot步态的平衡控制算法

权利要求书

1.一种四足机器人基于trot步态的平衡控制算法，其特征在于：该方法包括以下步骤：

S1、设计四足机器人的步态，并给出步态时序图；

S2、搭建平衡控制框图，从期望状态的输入到期望关节力矩的输出，根据四足机器人期望状态的导数和当前状态，求得期望的地面反作用力，地面反作用力包括沿x、y轴的摩擦力和沿z轴的支撑力，再根据期望的地面反作用力和四足机器人单腿的雅可比矩阵求得期望的关节力矩的大小，再通过四足机器人关节模组的电流环使四足机器人到达期望的位姿并保持平衡；

S3、建立四足机器人的动力学模型，并基于此模型结合现代控制理论的知识设计状态方程，四足机器人动力学模型，推导如下：

首先，建立了四足机器人的SOLIDWORKS模型：

四足机器人由机身和四条腿组成，其中每条腿的机械结构基本是一致的，四足机器人单腿主要由三个关节来驱动，分别为测摆关节、髋关节和膝关节，通过这三个关节的耦合是机器人末端按照研究人员的预期状态工作；

其次，以四足机器人移动在水平平面为例介绍平衡控制算法的推导过程，建立了四足机器人的动力学模型：

O(x-y-z)为世界坐标系，B(x-y-z)为机身坐标系，其坐标原点为机身中心位置，世界坐标系与机身坐标系之间的转换矩阵如下：

其中，代表四足机器人沿z轴旋转的角度，称为偏航角，θ代表四足机器人沿y轴旋转的角度，称为横滚角，φ代表四足机器人沿x轴旋转的角度，称为俯仰角，由于四足机器人的横滚角θ以及俯仰角φ角度很小，R_y(θ)、R_x(φ)被简化为I_3x3的单位矩阵，因此旋转矩阵表示如下：

对上式求一阶导得：

本文所述的动力学模型是在世界坐标系中建立的，由于是基于四足机器人的trot步态来建立的平衡控制算法，在任一时刻只有两条支撑腿着地，因此以左前腿1和右后腿4为例进行建模，也就是说，四足机器人此时与地面的接触点为p1、p4，具体如下：

公式(3)是通过牛顿第二定律建立，将四足机器人看作一个整体，其中为质心的加速度，m为四足机器人质量，g代表重力加速度，f₁＝[f_x1 f_y1 f_z1]^T代表地面对左前腿施加的作用力，f₂＝[f_x2 f_y2 f_z2]^T代表地面对右后腿施加的作用力，其中f_x代表地面对四足机器人沿x方向的摩擦力，而f_y代表地面对四足机器人沿y方向的摩擦力，f_z代表地面对四足机器人沿z方向的支撑力；

对上式中的公式(4)进行简化得：

公式(2)是通过力矩平衡方程建立，I代表四足机器人相对于p1-p4连线的转动惯量，代表世界坐标系中四足机器人的转向角速度，r₁代表p1指向B的矢径，f₁＝[f_x1 f_y1 f_z1]^T代表地面对左前腿施加的作用力，r₂代表p4指向B的矢径，f₂＝[f_x2 f_y2 f_z2]^T代表地面对右后腿施加的作用力，其中f_x代表地面对四足机器人沿x方向的摩擦力，而f_y代表地面对四足机器人沿y方向的摩擦力，f_z代表地面对四足机器人沿z方向的支撑力；

通过将四足机器人简化为刚体模型，应用理论力学相关原理建立上式保证四足机器人的平衡；

根据现代控制理论，将上式(2)、(3)和(4)写成状态空间表达式的形式如下：

将上式变形可得：

基于上式求得的地面反作用力f_i，求得四足机器人单腿的关节力矩：

i＝1，2，R代表机身坐标系相对于世界坐标系的转换矩阵，J代表四足机器人单腿的雅可比矩阵，注意每条腿的雅可比矩阵是不同的，具体表达如下：

式中：

J₁₁＝0；

J₁₂＝l₃cos(q₂+q₃)+l₂cos(q₂)；

J₁₃＝l₃cos(q₂+q₃)；

J₂₁＝l₃cos(q₁)cos(q₂+q₃)+l₂cos(q₁)cos(q₂)-l₁sin(q₁)；

J₂₂＝-l₃sin(q₁)sin(q₂+q₃)-l₂sin(q₁)sin(q₂)；

J₂₃＝-l₃sin(q₁)sin(q₂+q₃)；

J₃₁＝l₃sin(q₁)cos(q₂+q₃)+l₂cos(q₂)sin(q₁)+l₁cos(q₁)；

J₃₂＝l₃cos(q₁)sin(q₂+q₃)+l₂cos(q₁)sin(q₂)；

J₃₃＝l₃cos(q₁)sin(q₂+q₃)；

其中，l₁代表髋关节的偏置位移，l₂代表大腿长度，l₃代表小腿长度，q₁代表跨摆关节旋转角度，q₂代表髋关节旋转角度，q₃代表膝关节的旋转角度；

根据上述内容，求得四足机器人支撑腿的每个关节的输出力矩，如果让四足机器人的每个关节工作在电流环，即使得四足机器人在保持平衡的同时到达研究员设定的期望位姿；

S4、根据所提出的平衡控制算法，在MATLAB/SIMULINK中进行模型的仿真。

2.根据权利要求1所述的一种四足机器人基于trot步态的平衡控制算法，其特征在于：所述设计四足机器人的步态具体包括：

基于四足机器人的运动，设计了四足机器人的步态周期T，摆动相的摆动周期T/2，支撑相的支撑周期T/2；

trot步态是自然界四足动物的常用步态，是以四足机器人的左前腿(LF)、右后腿(RH)为一组，标记为第一组，以右前腿(RF)、左后腿(LH)为另外一组，标记为第二组，在一个步态周期T内，描述如下：

在0-T/2时间内，第一组的LF、RH工作在摆动相，而第二组的RF、LH工作在支撑相，当时间来到T/2时，第一组的两条腿落地，第二组的两条腿开始抬起，即在T/2-T时间内，第一组的LF、RH工作在支撑相，而第二组的RF、LH工作在摆动相，如此反复循环，实现四足机器人的前进运动。