一种四足机器人基于trot步态的平衡控制算法.pdf

(19)国家知识产权局(12)发明专利申请(10)申请公布号CN114721414A(43)申请公布日2022.07.08(21)申请号202210366509.2(22)申请日2022.04.08(71)申请人合肥工业大学地址230000安徽省合肥市包河区屯溪路193号(72)发明人钟华勇吴赶明甄圣超(74)专利代理机构合肥中悟知识产权代理事务所(普通合伙)34191专利代理师张婉(51)Int.Cl.G05D1/08(2006.01)权利要求书3页说明书7页附图3页(54)发明名称一种四足机器人基于trot步态的平衡控制算法(57)摘要本发明公开了一种四足机器人基于trot步态的平衡控制算法，该方法包括以下步骤：S1、设计四足机器人的步态，并给出步态时序图；S2、搭建平衡控制框图，从期望状态的输入到期望关节力矩的输出，根据四足机器人期望状态的导数和当前状态，求得期望的地面反作用力，地面反作用力包括沿x、y轴的摩擦力和沿z轴的支撑力，再根据期望的地面反作用力和四足机器人单腿的雅可比矩阵求得期望的关节力矩的大小。本发明通过提出了一种面对复杂地形的平衡控制算法用以保证四足机器人最基本的平衡控制，解决了四足机器人在任一时刻下都处于自不稳定的状态的问题。CN114721414ACN114721414A权利要求书1/3页1.一种四足机器人基于trot步态的平衡控制算法，其特征在于：该方法包括以下步骤：S1、设计四足机器人的步态，并给出步态时序图；S2、搭建平衡控制框图，从期望状态的输入到期望关节力矩的输出，根据四足机器人期望状态的导数和当前状态，求得期望的地面反作用力，地面反作用力包括沿x、y轴的摩擦力和沿z轴的支撑力，再根据期望的地面反作用力和四足机器人单腿的雅可比矩阵求得期望的关节力矩的大小，再通过四足机器人关节模组的电流环使四足机器人到达期望的位姿并保持平衡；S3、建立四足机器人的动力学模型，并基于此模型结合现代控制理论的知识设计状态方程，四足机器人动力学模型，推导如下：首先，建立了四足机器人的SOLIDWORKS模型：四足机器人由机身和四条腿组成，其中每条腿的机械结构基本是一致的，四足机器人单腿主要由三个关节来驱动，分别为测摆关节、髋关节和膝关节，通过这三个关节的耦合是机器人末端按照研究人员的预期状态工作；其次，以四足机器人移动在水平平面为例介绍平衡控制算法的推导过程，建立了四足机器人的动力学模型：O(x‑y‑z)为世界坐标系，B(x‑y‑z)为机身坐标系，其坐标原点为机身中心位置，世界坐标系与机身坐标系之间的转换矩阵如下：其中，代表四足机器人沿z轴旋转的角度，称为偏航角，θ代表四足机器人沿y轴旋转的角度，称为横滚角，φ代表四足机器人沿x轴旋转的角度，称为俯仰角，由于四足机器人的横滚角θ以及俯仰角φ角度很小，Ry(θ)、Rx(φ)被简化为I3x3的单位矩阵，因此旋转矩阵表示如下：对上式求一阶导得：本文所述的动力学模型是在世界坐标系中建立的，由于是基于四足机器人的trot步态来建立的平衡控制算法，在任一时刻只有两条支撑腿着地，因此以左前腿1和右后腿4为例进行建模，也就是说，四足机器人此时与地面的接触点为p1、p4，具体如下：公式(3)是通过牛顿第二定律建立，将四足机器人看作一个整体，其中为T质心的加速度，m为四足机器人质量，g代表重力加速度，f1＝[fx1fy1fz1]代表地面对左前T腿施加的作用力，f2＝[fx2fy2fz2]代表地面对右后腿施加的作用力，其中fx代表地面对四足机器人沿x方向的摩擦力，而fy代表地面对四足机器人沿y方向的摩擦力，fz代表地面对四2CN114721414A权利要求书2/3页足机器人沿z方向的支撑力；对上式中的公式(4)进行简化得：公式(2)是通过力矩平衡方程建立，I代表四足机器人相对于p1‑p4连线的转动惯量，代表世界坐标系中四足机器人的转向角速度，r1代表p1指向B的矢径，f1＝TT[fx1fy1fz1]代表地面对左前腿施加的作用力，r2代表p4指向B的矢径，f2＝[fx2fy2fz2]代表地面对右后腿施加的作用力，其中fx代表地面对四足机器人沿x方向的摩擦力，而fy代表地面对四足机器人沿y方向的摩擦力，fz代表地面对四足机器人沿z方向的支撑力；通过将四足机器人简化为刚体模型，应用理论力学相关原理建立上式保证四足机器人的平衡；根据现代控制理论，将上式(2)、(3)和(4)写成状态空间表达式的形式如下：将上式变形可得：基于上式求得的地面反作用力fi，求得四足机器人单腿的关节力矩：i＝1，2，R代表机身坐标系相对于世界坐标系的转换矩阵，J代表四足机器人单腿的雅可比矩阵，注意每条腿的雅可比矩阵是不同的，具体表达如下：式中：J11＝0；J12＝l3cos(q2+q3)+l2cos(q2)；J13＝l3c