CN113843799B - 一种四足机器人姿态复位控制方法、装置及存储介质 - Google Patents

Abstract

本发明公开了一种四足机器人姿态复位控制方法，包括获取机器人的角速度、加速度以及姿态角等计算得出机器人的实时位置和实时速度；然后根据四足机器人的期望运动速度、实时速度和实时位置对四足机器人的摆动相的每条腿的足端运动轨迹；以及计算得出四足机器人的每条腿的伺服电机摇臂角度，进而生成伺服控制输出指令控制每条腿的运动状态；当摆动相的每条腿均触地时，将作为摆动相的每条腿立即停止运动并切换至支撑相，将作为支撑相的每条腿切换为摆动相，然后继续获取数据，进行下一周期的姿态控制；直到四足机器人停止运动。本发明还公开了一种四足机器人姿态复位控制装置及存储介质。

Description

一种四足机器人姿态复位控制方法、装置及存储介质

技术领域

本发明涉及四足机器人领域，尤其涉及一种四足机器人姿态复位控制方法和***。

背景技术

仿生足式机器人在工程探险、反恐防爆、军事侦察等领域内具有良好的应用前景，因此，对于仿生足式机器人的研究也越来越热门，其中，由于四足机器人既有超于二足机器人的平稳性又避免了六足机器人机构的冗余和复杂性，其应用的最为广泛。但是，目前的仿生足式机器人是基于舵机的控制***来实现其运动的，其机器人的各个关节的速度不可控，足端运动轨迹不圆滑，同时其运动速度不可根据实际的情况进行变更，对于机器人的姿态复位也无法进行控制。

发明内容

为了克服现有技术的不足，本发明的目的之一在于提供一种四足机器人姿态复位控制方法，其能够控制现有四足机器人的姿态的快速复位。

本发明的目的之二在于提供一种四足机器人姿态复位控制装置，其能够控制现有四足机器人的姿态的快速复位。

本发明的目的之三在于提供一种存储介质，其能够控制现有四足机器人的姿态的快速复位。

本发明的目的之一采用如下技术方案实现：

一种四足机器人姿态复位控制方法，所述姿态复位控制方法包括：

数据解算步骤：获取所述四足机器人的姿态角、加速度和角速度；

矩阵解算步骤：采用运动学正解对所述四足机器人的角速度、姿态角和加速度进行计算得出所述四足机器人的每个腿的足端位置，并结合所述四足机器人的尺寸数据进行里程计计算得出所述四足机器人的实时位置和实时速度；

摆动相计算步骤：获取并根据所述四足机器人的期望运动速度、实时速度与实时位置计算得出所述四足机器人的摆动相的每条腿的足端运动轨迹；同时，采用运动学逆解对所述四足机器人的摆动相的每条腿的足端运动轨迹进行计算得出所述四足机器人的摆动相的每条腿的伺服电机摇臂角度；

支撑相计算步骤：使用VMC三通道控制算法对所述四足机器人的实时速度和实时位置进行计算以得出所述四足机器人的支撑相的各个腿的关节力矩，以及采用伪力矩模型将所述四足机器人的支撑相的每条腿的关节力矩转换为伺服电机期望转速；同时更新四足机器人的支撑相的每条腿的伺服电机摇臂角度；控制步骤：根据所述四足机器人的每条腿的伺服电机摇臂角度和伺服电机的类型生成对应的伺服控制指令并控制四足机器人的每条腿的运动；

检测步骤：当四足机器人的摆动相的每条腿均触地时，将作为摆动相的每条腿立即停止运动并切换为支撑相，将作为支撑相的每条腿切换为摆动相，然后执行数据解算步骤；直到四足机器人停止运动。

进一步地，所述数据解算步骤还包括：通过姿态传感器获取IMU数据，并对IMU数据进行预处理，然后对预处理后的IMU数据进行姿态解算得出所述四足机器人的姿态角和加速度，最后结合所述四足机器人的尺寸数据、姿态角和加速度计算得出所述四足机器人的角速度。

进一步地，所述数据解算步骤还包括：根据所述四足机器人的尺寸数据得出所述四足机器人的数学模型，并根据所述四足机器人的数学模型、姿态角和加速度计算得出四足机器人的角速度；所述四足机器人的数学模型是根据所述四足机器人的尺寸数据转换形成的数学；所述四足机器人的尺寸数据包括机器人的身体的长度、宽度、高度、大腿长度和小腿长度。

进一步地，所述姿态传感器包括三轴陀螺仪和三轴加速度计；所述预处理包括中值滤波处理和/或低通滤波处理。

进一步地，所述矩阵解算步骤具体包括：

首先根据三角函数和余弦定理得出运动学正解表达式如公式(1)：

其中，l为虚拟腿长，L₁为四足机器人的小腿长度，L₂为四足机器人的大腿长度，θ＝[θ₁,θ₂]为电机驱动摇臂角度；

然后对机器人的虚拟腿长l进行分解得出四足机器人每条腿的足端位置p＝[x,z]为：

再根据公式(1)和(2)推算得出电机驱动摇臂角度θ＝[θ₁,θ₂]为：

对公式(3)进行求偏导得出雅可比矩阵：

其中，

最后根据雅可比矩阵将四足机器人的每个腿的关节速度转换为足端移动速度，进而得出四足机器人的每个腿的足端位置。

进一步地，所述摆动相计算步骤之前还包括：

修正步骤：获取外部定位传感器的数据，并将通过里程计计算得出的四足机器人的实时速度和实时位置和外部定位传感器的数据进行融合得出所述四足机器人的实时速度和实时位置；所述外部定位传感器的数据包括通过检测设备检测得出的四足机器人的实时位置和实时速度。

进一步地，所述修正步骤还包括：

通过卡尔曼滤波算法将里程计计算得出的所述四足机器人的实时速度与实时位置与外部定位传感器的检测得到的所述四足机器人的实时速度与实时位置进行融合得出所述四足机器人的实时速度和实时位置。

进一步地，所述修正步骤还包括：获取外部定位传感器的数据并对所述外部定位传感器的数据进行滤波处理；所述滤波处理包括微分处理和低通滤波处理；所述外部定位传感器为激光雷达传感器。

本发明的目的之二采用如下技术方案实现：

一种四足机器人姿态复位控制装置，包括存储器、处理器以及存储在存储器上并在处理器上运行的计算机程序，所述计算机程序为四足机器人姿态复位控制程序，所述处理器执行所述四足机器人姿态复位控制程序时实现如本发明的目的之一采用的一种四足机器人姿态复位控制方法的步骤。

本发明的目的之三采用如下技术方案实现：

一种存储介质，所述存储介质为计算机可读存储介质，其上存储有计算机程序，所述计算机程序为四足机器人姿态复位控制程序，所述四足机器人姿态复位控制程序用于执行如本发明的目的之一采用的一种四足机器人姿态复位控制方法的步骤。

相比现有技术，本发明的有益效果在于：

本发明通过安装于四足机器人的姿态传感器以获取四足机器人的姿态数据，然后将其用于对四足机器人的姿态控制，通过控制四足机器人的支撑相与摆动相的腿的运动状态，以实现四足机器人的运动控制以及姿态复位，解决现有技术中四足机器人无法实现姿态复位的问题。

附图说明

图1为本发明提供的一种四足机器人姿态复位控制方法流程图；

图2为步骤S5的具体流程图。

具体实施方式

下面，结合附图以及具体实施方式，对本发明做进一步描述，需要说明的是，在不相冲突的前提下，以下描述的各实施例之间或各技术特征之间可以任意组合形成新的实施例。

实施例一

本发明应用于四足机器人，其通过对四足机器人用于角步态行走腿进行划分，将处于一条对角线上的两条腿划分为摆动相、另外一条对角线上的腿划分为支撑相，通过来回控制四足机器人的腿在摆动相与支撑相之间的切换，从而实现四足机器人的运动以及姿态复位控制。

本发明提供一优选的实施例，一种四足机器人姿态复位控制方法，如图1所示，包括以下步骤：

步骤S1、获取四足机器人的姿态角、加速度和角速度。

具体地，可通过安装于四足机器人上的姿态传感器的IMU数据，然后对IMU数据进行姿态解算得出四足机器人的姿态角和加速度，再根据姿态角、加速度以及四足机器人的尺寸数据计算得出四足机器人的角速度。

其中，姿态传感器包括三轴陀螺仪和三轴加速度计。对于陀螺仪来说，其测试的绕XYZ轴转动的角速度，可通过对角速度进行积分即可得到对应的角度。

而对于加速度计，测试的是XYZ方向上受到的加速度。在物体静止时，测量的加速度为重力加速度；而当物体倾斜时，根据重力的分力可以粗略地计算得出对应的倾斜角度。当物体在运动时，加速度计测量到的加速度，除了包括重力加速度外，其还可能叠加由于运行产生的加速度。

也即，对于三轴陀螺仪来说，其需要在物体运动时才能实现测量；而对于加速度来说，需要静止才能实现测量。因此，在获取IMU数据后还需要对其进行姿态求解。其中，IMU数据包括角速度和加速度，并且角速度和加速度均为四足机器人的机体坐标系下的数据。通过数据解算对加速度和角速度进行计算以得出四足机器人在地球坐标下的加速度和姿态角等。

另外，为了保证后续数据解算的准确性，本发明还对获取到的IMU数据进行预处理。其中，预处理方式包括中值滤波处理和/或低通滤波处理。

更为具体地，根据四足机器人的尺寸数据得出四足机器人的数学模型，并根据四足机器人的数学模型、四足机器人的姿态角与加速度计算得出四足机器人的角速度。

其中，四足机器人的数学模型是根据四足机器人的实际结构，将其简化后得到的数学模型，该数学模型包括四足机器人的身体部分的长宽高、大腿长度、小腿长度等数据。因此，在实际的使用过程中，根据四足机器人的尺寸数据(比如身体部分的长、宽、高，大腿长度，小腿长度等数据)进行转换得到四足机器人的数学模型。

步骤S2、采用运动学正解对四足机器人的角速度、姿态角和加速度进行计算得出雅可比矩阵，进而根据雅可比矩阵得出四足机器人的每个腿的足端位置。

其中，雅可比矩阵是一阶偏导数以一定方式排列成的矩阵，其行列式为雅可比行列式。雅可比矩阵的重要性在于它体现了一个可微方程与给出点的最优线性逼近。雅可比矩阵类似于多元函数的导数。

更为具体地，步骤S2还包括：

首先根据三角函数和余弦定理得出运动学正解表达式，如公式(1)：

其中，l为虚拟腿长，L₁为四足机器人的小腿长度，L₂为四足机器人的大腿长度，θ＝[θ₁,θ₂]为电机驱动摇臂角度。

然后将工作空间坐标系固定于机器人的机体轴上，对虚拟腿长l进行分解以得到足端位置p＝[x,z]为：

再根据公式(2)和运动学逆解得出以下方程组，也即采用足端位置得出电机驱动摇臂角度：

根据公式(4)求偏导(求取足端位置对电机驱动摇臂角度的偏导)以得出对应的雅可比矩阵：

则有：

其中，

步骤S3、对四足机器人的每个腿的足端位置和数学模型进行里程计计算得出里程计数据。

其中，里程计数据包括四足机器人的实时位置和实时速度。

里程计是一种利用从移动传感器获取的数据来估计物***置随时间变化而改变的方法。该方法被用在多种机器人***(比如轮式机器人或腿式机器人)中用于估计物体的位置与速度。

更为具体地，本发明还对得出的四足机器人的每个腿的足端位置进行滤波处理。其中，滤波处理包括微分处理和低通滤波处理。

步骤S4、获取四足机器人的期望运动速度，并结合四足机器人的实时速度和实时位置对四足机器人的摆动相的每条腿的足端的落足点进行规划，进而计算得出四足机器人的摆动相的每条腿的足端运动轨迹；同时，采用运动学逆解对四足机器人的摆动相的每条腿的足端运动轨迹进行计算得出四足机器人的摆动相的每条腿的伺服电机摇臂角度。

其中，本发明中的摆动相包括四足机器人的两条腿，并且两条腿位于同一对角线上。同理，在四足机器人的另一条对角线上的两条腿为支撑相。其中，摆动相的腿是指处于运动的状态，用于实现四足机器人的运动；而支撑相的腿是静止的，触地，用于支撑四足机器人的站立。本发明中的四足机器人是通过将四足机器人的四条腿中的摆动相与支撑相的腿来回切换以实现四足机器人的运动。摆动相与支撑相的腿并不是固定的，也即，在某一时刻，四足机器人的其中一条对角线上的两条腿为摆动相时，另一条对角线上的两条腿为支撑相；反之，若四足机器人其中一条对象线上的两条腿为支撑相时，另一条对角线上的两条腿为摆动相。当摆动相的两条腿从开始运动到触地，为完成一个步态周期。

也即，在机器人的运动过程中，将作为摆动相的腿与作为支撑相的腿来回切换，以保证机器人的持续移动。比如四足机器人的四条腿为A、B、C、D。其中，A和D处于同一对角线上，B和C处于同一对角线上。当A和D为摆动相时，B和C为支撑相。当B和C为摆动相时，A和D为支撑相。通过摆动相与支撑相的来回切换控制，实现机器人的持续移动。

当获取到顶层设备发送的控制命令后获取四足机器人的期望运动速度，然后根据机器人的期望运动速度、以及机器人的实时速度和位置对四足机器人的摆动相的每条腿的足端的落足点进行规划，从而得出摆动相的两个腿的足端运动轨迹。同时，采用运动学逆解根据四足机器人的摆动相的每条腿的足端运动轨迹计算得出四足机器人的摆动相的每条腿的关节角度。

四足机器人的摆动相，即实现机器人向期望方向的跨腿以保证机器人的持续移动，为保证机器人跨腿时能具有较小的冲击力，并且不会出现拖退的线性，一般采用摆线轨迹减小地面的冲击力。

一般来说，轨迹规划方法有多种，可采用多项式拟合方法，正余弦组合法、最优轨迹算法、摆线规划算法等。其中，摆线规划算法，只要需要给定起点、中点为端点位置作为输入即可实现轨迹的规划。

比如，设定足端当前起点位置为p_s＝[x_s,y_s,z_s]，期望落足点为p_f＝[x_f,y_f,z_f]，跨腿高度为h，中点位置为

采用Raibiert提出的方法计算落点：

其中，T_s为步态周期，λ为支撑相占空比。

通过调节增益参数期望落足点坐标就会随着机器人实时速度的变化为改变，即速度越大步幅越大，速度越小步幅越小，速度为零则为原地踏步。

对于Z_f来说，可直接等于该腿当前的期望高度Z_f＝Z_exp，则摆线轨迹如下：

其中，

则在获取跨腿轨迹后可直接使用各时刻足尖位置采用运动学逆解计算出对应的摇臂角度。

步骤S5、使用VMC三通道控制算法对四足机器人的实时速度和实时位置进行计算以得出四足机器人的支撑相的各个腿的关节力矩，以及采用伪力矩模型将四足机器人的支撑相的每条腿的关节力矩转换为伺服电机期望转速；同时更新四足机器人的支撑相的每条腿的伺服电机摇臂角度。

其中，VMC三通道控制算法是利用遐想的虚拟弹簧构建连接机器人内部作用点或者连接作用点与外部环境，产生相应的虚拟来驱使机器人实现期望的运动。

首先采用VMC三通道控制算法对四足机器人的实时位置与实时速度进行计算得出全局虚拟力，然后采用VMC分解方法将全局虚拟力分解到四足机器人的摆动相的各个腿进而得出各个腿的关节力矩。

其中，VMC分解原则如下：

(1)对于x轴来说：将全局虚拟力分解到前后两组腿中，由于前后腿一起提供前进动力因此可对两腿分别设计独立的x轴反馈控制器，并且将参数调节为：

(2)对于z轴来说：将全局虚拟力分解到前后两组腿中，由于前后两腿同时完成支撑因此可独立设计z轴控制器，并调节参数为

同时对于机体较长那一轴来说希望机器移动时能如下图所示跟随地面角度变化而移动，因此可以在移动时设定：

(3)对于俯仰轴来说：机器人调节俯仰角可简化为增加前后俩组腿的不同高度，因此同理可将俯仰轴控制输出叠加至原始期望高度上，这里与四轴控制算法中在某一悬停油门上设计高度控制器的思想十分类似，因此可以得到：

步骤S6、根据所述四足机器人的每条腿的伺服电机摇臂角度和伺服电机的类型生成对应的伺服控制指令并控制四足机器人的每条腿的运动。由于机器人所采用的伺服电机的类型，比如舵机、无刷电机等。则在转换伺服控制输出命令时的转换方法也不同，因此，在生成伺服控制输出命令时，还需要获取伺服电机的类型。

步骤S7、当检测到四足机器人的摆动相的每条腿均触地时，将作为摆动相的每条腿立即停止运动并将其切换至支撑相，将作为支撑相的每条腿切换为摆动相，然后执行步骤S1，直到四足机器人停止运动或到达指定位置。

也即，本实施例在一个步态周期内，由于支撑相是一直触地的，摆动相根据足端轨迹进行运动。而当摆动相的两条腿触地时将其更变换为支撑相，同时原有作为支撑相的两条腿切换为摆动相，然后进入下一个步态周期继续对机器人进行控制。通过本发明可快速实现四足机器人的姿态复位，从在一个步态周期结束后，快速进入下一个步态周期的控制。

优选地，如图2所示，步骤S3还包括：

步骤S31、获取外部定位设备的检测数据。

步骤S32、对外部定位设备的检测数据进行滤波以得出四足机器人的实时位置和实时速度。

其中，外部定位传感器为激光雷达传感器，用于获取机器人的定位数据，其定位数据为点云。

具体地，对外部定位设备的检测数据进行滤波时，包括对检测数据进行微分处理和低通滤波处理。

步骤S33、采用卡尔曼滤波器将里程计计算得出的四足机器人的实时位置与实时速度与通过外部定位传感器得出的四足机器人的实时位置与实时速度进行融合得出四足机器人的实时速度和实时位置。

由于里程计计算得出的数据可能具有一定的误差，因此，通过采用外部定位设备获取的数据来实现对四足机器人的实时位置与实时速度进行修改，也即，通过卡尔曼滤波器将二者的数据进行融合以得到相对准确机器人的实时速度和位置。

实施例二

基于实施例一提供的一种四足机器人姿态复位控制方法，本发明还提供一种四足机器人姿态复位控制装置，包括存储器、处理器以及存储在存储器上并在处理器上运行的计算机程序，所述计算机程序为四足机器人姿态复位控制程序，所述处理器执行所述四足机器人姿态复位控制程序时实现如实施例一提供的一种四足机器人姿态复位控制方法的步骤。

实施例三

一种存储介质，所述存储介质为计算机可读存储介质，其上存储有计算机程序，所述计算机程序为四足机器人姿态复位控制程序，所述四足机器人姿态复位控制程序用于执行如实施例一提供的一种四足机器人姿态复位控制方法的步骤。

上述实施方式仅为本发明的优选实施方式，不能以此来限定本发明保护的范围，本领域的技术人员在本发明的基础上所做的任何非实质性的变化及替换均属于本发明所要求保护的范围。

Claims (9)

1.一种四足机器人姿态复位控制方法，其特征在于，所述姿态复位控制方法包括：

数据解算步骤：获取所述四足机器人的姿态角、加速度和角速度；根据所述四足机器人的尺寸数据得出所述四足机器人的数学模型，并根据所述四足机器人的数学模型、姿态角和加速度计算得出四足机器人的角速度；所述四足机器人的数学模型是根据所述四足机器人的尺寸数据转换形成的数学模型；所述四足机器人的尺寸数据包括机器人的身体的长度、宽度、高度、大腿长度和小腿长度；

矩阵解算步骤：采用运动学正解对所述四足机器人的角速度、姿态角和加速度进行计算得出所述四足机器人的每个腿的足端位置，并结合所述四足机器人的尺寸数据进行里程计计算得出所述四足机器人的实时位置和实时速度；

摆动相计算步骤：获取并根据所述四足机器人的期望运动速度、实时速度与实时位置计算得出所述四足机器人的摆动相的每条腿的足端运动轨迹；同时，采用运动学逆解对所述四足机器人的摆动相的每条腿的足端运动轨迹进行计算得出所述四足机器人的摆动相的每条腿的伺服电机摇臂角度；

支撑相计算步骤：使用VMC三通道控制算法对所述四足机器人的实时速度和实时位置进行计算以得出所述四足机器人的支撑相的各个腿的关节力矩，以及采用伪力矩模型将所述四足机器人的支撑相的每条腿的关节力矩转换为伺服电机期望转速；同时更新四足机器人的支撑相的每条腿的伺服电机摇臂角度；控制步骤：根据所述四足机器人的每条腿的伺服电机摇臂角度和伺服电机的类型生成对应的伺服控制指令并控制四足机器人的每条腿的运动；

检测步骤：当四足机器人的摆动相的每条腿均触地时，将作为摆动相的每条腿立即停止运动并切换为支撑相，将作为支撑相的每条腿切换为摆动相，然后执行数据解算步骤；直到四足机器人停止运动。

2.根据权利要求1所述的四足机器人姿态复位控制方法，其特征在于，所述数据解算步骤还包括：通过姿态传感器获取IMU数据，并对IMU数据进行预处理，然后对预处理后的IMU数据进行姿态解算得出所述四足机器人的姿态角和加速度，最后结合所述四足机器人的尺寸数据、姿态角和加速度计算得出所述四足机器人的角速度。

3.根据权利要求2所述的四足机器人姿态复位控制方法，其特征在于，所述姿态传感器包括三轴陀螺仪和三轴加速度计；所述预处理包括中值滤波处理和/或低通滤波处理。

4.根据权利要求1所述的四足机器人姿态复位控制方法，其特征在于，所述矩阵解算步骤具体包括：

首先根据三角函数和余弦定理得出运动学正解表达式如公式(1)：

其中，l为虚拟腿长，L₁为四足机器人的小腿长度，L₂为四足机器人的大腿长度，θ＝[θ₁,θ₂]为电机驱动摇臂角度；

然后对机器人的虚拟腿长l进行分解得出四足机器人每条腿的足端位置p＝[x,z]为：

再根据公式(1)和(2)推算得出电机驱动摇臂角度θ＝[θ₁,θ₂]为：

对公式(3)进行求偏导得出雅可比矩阵：

其中，

最后根据雅可比矩阵将四足机器人的每个腿的关节速度转换为足端移动速度，进而得出四足机器人的每个腿的足端位置。

5.根据权利要求1所述的四足机器人姿态复位控制方法，其特征在于，所述摆动相计算步骤之前还包括：

修正步骤：获取外部定位传感器的数据，并将通过里程计计算得出的四足机器人的实时速度和实时位置和外部定位传感器的数据进行融合得出所述四足机器人的实时速度和实时位置；所述外部定位传感器的数据包括通过检测设备检测得出的四足机器人的实时位置和实时速度。

6.根据权利要求5所述的四足机器人姿态复位控制方法，其特征在于，所述修正步骤还包括：

通过卡尔曼滤波算法将里程计计算得出的所述四足机器人的实时速度与实时位置与外部定位传感器的检测得到的所述四足机器人的实时速度与实时位置进行融合得出所述四足机器人的实时速度和实时位置。

7.根据权利要求5所述的四足机器人姿态复位控制方法，其特征在于，所述修正步骤还包括：获取外部定位传感器的数据并对所述外部定位传感器的数据进行滤波处理；所述滤波处理包括微分处理和低通滤波处理；所述外部定位传感器为激光雷达传感器。

8.一种四足机器人姿态复位控制装置，包括存储器、处理器以及存储在存储器上并在处理器上运行的计算机程序，所述计算机程序为四足机器人姿态复位控制程序，其特征在于，所述处理器执行所述四足机器人姿态复位控制程序时实现如权利要求1-7中任一项所述的一种四足机器人姿态复位控制方法的步骤。

9.一种存储介质，所述存储介质为计算机可读存储介质，其上存储有计算机程序，所述计算机程序为四足机器人姿态复位控制程序，其特征在于，所述四足机器人姿态复位控制程序用于执行如权利要求1-7中任意一项所述的一种四足机器人姿态复位控制方法的步骤。

Images