CN112327899B - 一种可变构型的四足机器人运动控制方法及系统 - Google Patents

Abstract

本发明公开了一种可变构型的四足机器人运动控制方法及系统，方法包括以下步骤：规划四足机器人的步态；基于机器人的微分运动学，建立以机器人当前的关节位置、速度作为输入的四足机器人的整数规划模型；基于冗余自由度，实现对机器人各种构型之间的切换；基于当前构型规划机器人的运动，规划支撑相、摆动相的足端期望位置，实现四足机器人在多构型下的全方位移动，在面对复杂地形时稳定性和适应性显著增加。

Description

一种可变构型的四足机器人运动控制方法及系统

技术领域

本发明属于四足机器人控制方法技术领域，具体涉及一种可变构型的四足机器人运动控制方法及系统。

背景技术

这里的陈述仅提供与本发明相关的背景技术，而不必然地构成现有技术。

具有冗余自由度的四足机器人相对与传统四足机器人有较强的地形适应和越障能力，可以利用改变构型适应在复杂地形运行。现有技术中，例如，东京工业大学研发的TITAN-VIII机器人利用昆虫构型成功的实现较强的地形性和稳定性。但是目前的四足机器人无法实现多构型的切换和多构型的运动控制，使得环境适应性降低。同时大部分使用静步态运动，运动速度缓慢，没有动步态运行效率更高、更快。

发明人认为，对于具有冗余结构的四足机器人，其运动学逆解存在多解情况，通常的处理方式是先指定某个关节的取值，将其简化为三自由度进行求解，主要采用的还是传统的控制方法，没有将冗余自由度与变构型运动控制结合起来，充分利用冗余自由度的优势。

发明内容

针对现有技术存在的不足，本发明的目的是提供一种可变构型的四足机器人运动控制方法及系统，为实现四足机器人各个构型的有效控制，利用微分运动学方程，建立机器人腿部关节空间与操作空间之间的运动学关联，获得在期望位置下对应的腿部关节空间变化；利用冗余自由度，改变机器人的构型，通过规划机器人躯干姿态与足端的轨迹，实现具有冗余自由度和多构型随机切换的情况下进行全方位控制；利用基于足底力的触底检测和时间规划的状态机用来规划落足点与四足机器人各个腿的运动状态。

为了实现上述目的，本发明是通过如下的技术方案来实现：

第一方面，本发明的技术方案提供了一种可变构型的四足机器人运动控制方法，包括以下步骤：

利用微分运动学方程，建立以四足机器人的足端位置、速度作为输入的四足机器人的整数规划模型，以建立机器人腿部关节空间与操作空间之间的运动学关联，并获得在期望位置下对应的腿部关节空间变化；

基于冗余自由度，实现对机器人各种构型之间的切换，并避免初始构型处于奇异点；

根据当前构型规划机器人的运动，规划机器人躯干姿态与速度，利用基于足底力的触底检测和时间规划的状态机规划落足点与四足机器人各个腿的运动状态，按照规划的轨迹运动。

第二方面，本发明的技术方案还提供了一种具有冗余自由度可变构型的四足机器人运动控制系统，包括以下模块，各模块之间级联动作：

第一模块，被配置为建立以四足机器人的足端位置、速度作为输入的四足机器人的整数规划模型；

第二模块，被配置为基于冗余自由度，实现对机器人各种构型之间的切换；

第三模块，被配置为根据当前构型规划并执行机器人的运动。

上述本发明的技术方案的有益效果如下：

1)本发明通过微分运动学，建立了具有冗余关节的关节空间与操作空间的联系，大大增加了冗余自由度的利用效率，通过基于二次整数规划的最优化方法实现冗余自由度机器人的运动学求解，利用微分运动学建立关节空间与操作空间的关系，同时增加了四足机器人的运动控制的稳定性与鲁棒性。

2)本发明将通过变轨迹规划来实现多构型的四足机器人的运动控制；改变四足机器人的初始位置，对四足机器人运动构型切换，同时借助新构型下轨迹规划，实现各个构型下稳定的运动控制；规划四足机器人的支撑相轨迹来实现对机器人运动速度、躯干姿态角的控制；规划四足机器人的摆动相的轨迹来间接对速度的控制；基于时间状态与触地情况的状态机实现支撑相与摆动相的切换；实现四足机器人的多构型运动，针对不同场景适用该场景下的最优构型，增加四足机器人对于环境的适应性与稳定性。

附图说明

构成本发明的一部分的说明书附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。

图1是本发明根据一个或多个实施方式的四足机器人运动控制方法的控制框图，

图2是本发明根据一个或多个实施方式的四足机器人的坐标系定义说明示意图，

图3是本发明根据一个或多个实施方式的构型切换仿真截图

图4是本发明根据一个或多个实施方式的对四足机器人变构型后爬坡、过低矮障碍、侧向冲击的仿真过程。

为显示各部位位置而夸大了互相间间距或尺寸，示意图仅作示意使用。

具体实施方式

应该指出，以下详细说明都是例示性的，旨在对本发明提供进一步的说明。除非另有指明，本发明使用的所有技术和科学术语具有与本发明所属技术领域的普通技术人员通常理解的相同含义。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本发明的示例性实施方式。如在这里所使用的，除非本发明另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合；

为了方便叙述，本发明中如果出现“上”、“下”、“左”“右”字样，仅表示与附图本身的上、下、左、右方向一致，并不对结构起限定作用,仅仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的设备或元件必须具有特定的方位，以特定的方位构造和操作，因此不能理解为对本发明的限制。

术语解释部分：本发明中的术语“安装”、“相连”、“连接”、“固定”等术语应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或为一体；可以是机械连接，也可以是电连接，可以是直接连接，也可以是通过中间媒介间接相连，可以是两个元件内部连接，或者两个元件的相互作用关系，对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明的具体含义。

构形：本发明中的构型具体指的是机器人的结构或姿态，例如，机器人在第一姿态的形状称为第一构型，机器人在第二姿态的形状被称为第二构型。

正如背景技术所介绍的，针对现有技术存在的不足，本发明的目的是提供一种可变构型的四足机器人运动控制方法及系统，为实现四足机器人各个构型的有效控制，利用微分运动学方程，建立机器人腿部关节空间与操作空间之间的运动学关联，获得在期望位置下对应的腿部关节空间变化；利用冗余自由度，改变机器人的构型，通过规划机器人躯干姿态与足端的轨迹，实现具有冗余自由度和多构型随机切换的情况下进行全方位控制。利用基于足底力的触底检测和时间规划的状态机用来规划落足点与四足机器人各个腿的运动状态。

实施例1

本发明的一种典型实施方式中，本实施例公开了具有冗余自由度可变构型的四足机器人的运动控制方法，需要说明的是，本实施例中的，四足机器人是一种目前较为通用的机器人，例如专利号为ZL 2016100065414的发明公开的电驱动四足仿生机器人，其前躯干的两侧分别设有前腿，后躯干的两侧分别设有后腿.，图2中，本实施例将其简化为具有躯干以及四肢的模型。

本实施例中提及的方法，具体步骤如下：

(1)规划四足机器人的步态；

以trot步态作为运动步态，每个步态周期包含两足支撑、两足摆动两个状态，在保证足端工作空间约束满足的前提下，通过调整机器人的落足点来调节机器人躯干的平衡，保证机器人的躯干质心投影尽可能在两支撑腿足端连线的中间。

(2)基于四足机器人的微分运动学方程，建立以四足机器人的足端位置、速度作为输入的四足机器人的整数规划模型；具体过程如下：

①建立如图2所示的坐标系：

躯干坐标系∑_B：此坐标系固定在机器人的躯干上，原点位于机器人躯干质心，X_B指向躯干正前方，Y_B指向机器人左方，Z_B垂直于躯干向上；

腿部坐标系∑₀：此坐标系固定在机器人的侧摆关节上，原点位于机器人腿部侧摆关节质心，X₀指向躯干正前方，Y₀指向机器人左方，Z₀垂直于侧摆关节向上；

足端坐标系∑_E：此坐标系固定在机器人的腿部足端上，原点位于机器人腿部足端中心，X_e、Y_e、Z_e指向和DH参数法规定的方向相同；

②基于机器人的微分运动学方程，建立以足端位置、速度作为输入的二次整数规划：

利用DH参数法根据建立的坐标系求解出机器人的正运动学。对于每一个由4个自由度组成的腿模型，关节空间速度与任务空间速度之间的关系可以通过分析雅克比矩阵描述出来。四足机器人的腿部关节速度的(4×1)向量为

足端对应的速度为ν_e＝[ν_x ν_y ν_z]，分析雅克比矩阵为J，则微分运动学方程为

当已知足端的初始位置q(0)时，关节位置通过速度对时间的积分可以计算，即

对于积分进行离散化处理，基于欧拉积分法，给定一个时间间隔Δt，对于t_k时刻的关节位置和速度已知，则t_k+1＝t_k+Δt时刻的关节位置为

利用二次整数规划，将以上的微分运动学方程转化为线性最优化问题，求解满足约束函数的二次型目标函数最小的解：

其中的H∈R^4×4是对称正定的各个关节的权值矩阵，利用拉格朗日乘数法，求解出最优解为

简化方程为

于是

为增大跟随的效果，引入足端的位置偏差作为反馈校正后

W∈R^4×4是常数正定增益矩阵，Δp∈R^3×1是足端的期望位置与反馈位置之差。由此

四足机器人足端的关节空间解可以求出，由期望的足端轨迹计算出关节角的变化。

(3)基于冗余自由度，实现对机器人各种构型之间的切换；

利用微分运动学中，关节空间的运动学求解在同一位置的解不唯一。由微分运动学的性质知道机器人的运动与机器人的初始位置有很大关系。利用四足机器人的冗余自由度，可以实现将四足机器人的构型切换。如图3和图4所示，图3示出了站立姿态下的构型和半蹲姿态下的构型之间的变化过程，图4由上到下由左到右分别示出了坡面向右下方倾斜第一角度时的姿态、坡面向右下方倾斜第二角度时的姿态(这个地方是到达了坡顶以后的运动)、坡面向左下方倾斜第一角度时的姿态、进入低矮处前的姿态，在低矮处中的姿态、走出低矮处后的姿态、侧向冲击前的姿态、侧向冲击中的姿态和侧向冲击后的姿态，其中，坡面向右下方倾斜第一角度时的姿态、坡面向右下方倾斜第二角度时的姿态、坡面向左下方倾斜第一角度时的姿态均属于一种构型，进入低矮处前的姿态，在低矮处中的姿态、走出低矮处后的姿态均属于一种构型，侧向冲击前的姿态、侧向冲击中的姿态和侧向冲击后的姿态均属于一种构型。

机器人的站立高度和步高分别设置为变量H_height＝const1、H_step＝const2，其中const1，const2是常量，根据不同的构型改变不同的站高和步高，不同的构型切换后，利用新的高度设置重新规划足端的轨迹规划。

对于初始构型稳定性判断，针对腿部的初始位置是否存在奇异点，使用雅克比矩阵的条件数来表示当前的姿态与机械臂关节冗余之间的距离：

由奇异值分解得到的雅可比矩阵

的最大奇异值和最小奇异值为:δ_max、δ_min。应该尽量避免四足机器人处于奇异点位置，当远离奇异点的位置时微分运动学计算可以产生期望的轨迹。根据机器人在不同的构型下的条件数的大小，判断是否处于奇异点的位置。当机器人位于奇异点时，即条件数远远大于设定的稳定差时的位置跟随误差较大。当判断腿型奇异点时，可以重新规划四足机器人的位置。

(4)基于当前构型规划机器人的运动，计算得到支撑相、摆动相的足端期望位置；

①机器人躯干姿态与速度的规划：

对于四足机器人的运动控制分解为支撑相的控制和摆动相的控制，将Trot步态运动的四足运动简化为倒立摆运动，步态周期为T，其中摆动相和支撑相的周期分别为T_s、T_c，通过支撑相控制四足机器人躯干的期望俯仰角φ_{d_pitch}、横滚角为φ_{d_roll}和期望的速度v_dx、v_dy，反馈的实际俯仰角φ_pitch、横滚角为φ_roll，支撑相的起始点坐标为(x_c0(0),y_c0(0),z_c0(0))，支撑相在t_k时刻的轨迹近似为：

z_c(t_k)＝z_c0

其中，k_{p_roll}为横滚角比例参数、k_{d_roll}为横滚角微分参数，k_{p_pitch}为俯仰角比例参数，k_{d_pitch}为俯仰角微分参数，调整上述参数使得能够跟随上期望的姿态角。

②机器人落足点的规划：

摆动相控制四足机器人的落足点，使用三次曲线规划在空中的运动轨迹。机器人的运行速度通过在摆动相落足点上引入关于速度偏差量的函数调节，因此落地点作为

假设四足机器人摆动相的抬脚足端坐标为(x_s(0),y_s(0),z_s(0))，落脚点坐标(x_s(T_s),y_s(T_s),z_s(T_s))，对应的速度为

和

在轨迹最高点的高度、速度为z_s(0)+H_step、0，期望的速度为v_d，机器人抬起的步高设置为H_step，使用三次曲线规划四足机器人摆动相的运行轨迹。

摆动相的足端轨迹方程为：

此处需要说明的是，在对摆动相进行的足端轨迹进行规划时，由于使用三次曲线规划，计算步骤较少，计算量大为减轻，有利于降低对应计算模块的计算强度，但是仍然能够取得相对准确的路径规划数值。

通过时间和触地检测状态机相结合来决定每条腿是处于摆动相或者支撑相，按照规划的轨迹运动。运动轨迹通过二次整形规划求解出的最优关节角来伺服，实现四足机器人在各个构型下的期望运动。

本发明通过微分运动学，建立了具有冗余关节的关节空间与操作空间的联系，大大增加了冗余自由度的利用效率，通过基于二次整数规划的最优化方法实现冗余自由度机器人的运动学求解，利用微分运动学建立关节空间与操作空间的关系，同时增加了四足机器人的运动控制的稳定性与鲁棒性。

本发明将通过变轨迹规划来实现多构型的四足机器人的运动控制。改变四足机器人的初始位置，对四足机器人运动构型切换，同时借助新构型下轨迹规划，实现各个构型下稳定的运动控制。规划四足机器人的支撑相轨迹来实现对机器人运动速度、躯干姿态角的控制。规划四足机器人的摆动相的轨迹来间接对速度的控制。基于时间状态与触地情况的状态机实现支撑相与摆动相的切换。实现四足机器人的多构型运动，针对不同场景适用该场景下的最优构型，增加四足机器人对于环境的适应性与稳定性。

实施例2

本发明的一种典型实施方式中，本实施例公开了一种具有冗余自由度可变构型的四足机器人运动控制系统，包括以下模块，各模块之间级联动作：

第一模块，被配置为建立以四足机器人的足端位置、速度作为输入的四足机器人的整数规划模型；

第二模块，被配置为基于冗余自由度，实现对机器人各种构型之间的切换；

第三模块，被配置为根据当前构型规划并执行机器人的运动。

各模块之间级联动作，所述级联动作关系为，当第n模块动作完成后，第n+1模块投入动作。

n为大于等于1的整数。

当然，上述第一模块、第二模块、第三模块对应于实施例1中的步骤(2)、步骤(3)、步骤(4)，上述模块与对应的步骤所实现的示例和应用场景相同，但不限于上述实施例一所公开的内容。需要说明的是，上述模块作为系统的一部分可以在诸如一组计算机可执行指令的计算机系统中执行。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (9)

1.一种可变构型的四足机器人运动控制方法，其特征在于，包括以下步骤：

规划四足机器人的运动步态，且以trot步态作为四足机器人的运动步态，保证机器人的躯干质心投影在两支撑腿足端连线的中间；

利用微分运动学方程，建立以四足机器人的足端位置、速度作为输入的四足机器人的整数规划模型，以建立机器人腿部关节空间与操作空间之间的运动学关联，并获得在期望位置下对应的腿部关节空间变化；

基于冗余自由度，实现对机器人各种构型之间的切换，并避免初始构型处于奇异点；

根据当前构型规划机器人的运动，规划机器人躯干姿态与速度，利用基于足底力的触底检测和时间规划的状态机规划落足点与四足机器人各个腿的运动状态，按照规划的轨迹运动。

2.如权利要求1所述的可变构型的四足机器人运动控制方法，其特征在于，建立四足机器人的整数规划模型时，具体包括以下步骤：

对四足机器人的躯干、腿部和足端分别建立坐标系；

基于机器人的微分运动学方程，建立以足端位置、速度作为输入的二次整数规划。

3.如权利要求1所述的可变构型的四足机器人运动控制方法，其特征在于，机器人各种构型之间切换时，机器人的站立高度和步高分别设置为第一变量和第二变量，根据相异的构型改变不同的站高和步高，构型切换后，利用新的高度设置重新规划足端的轨迹规划。

4.如权利要求1或3 所述的可变构型的四足机器人运动控制方法，其特征在于，对初始构型进行稳定性判断；进行稳定性判断时，使用雅克比矩阵的条件数来表示当前的姿态与机械臂关节冗余之间的距离，由雅可比矩阵得到最大奇异值和最小奇异值，避免四足机器人处于奇异值对应的位置；当判断腿型奇异值对应的位置时，重新规划四足机器人的位置。

5.如权利要求1或3所述的可变构型的四足机器人运动控制方法，其特征在于，对初始构型进行稳定性判断；进行稳定性判断时，根据机器人在不同的构型下的条件数的大小，判断是否处于奇异点的位置；当机器人位于奇异点时，条件数远大于设定的稳定差时的位置；当判断腿型处于奇异点时，重新规划四足机器人的位置。

6.如权利要求1所述的可变构型的四足机器人运动控制方法，其特征在于，基于当前构型规划机器人的运动时，对机器人躯干的姿态与速度、对机器人落足点均进行规划。

7.如权利要求6所述的可变构型的四足机器人运动控制方法，其特征在于，对机器人躯干的姿态与速度进行规划时，包括以下步骤：

把对于四足机器人的运动控制分解为对于支撑相的控制和对于摆动相的控制；

将步态运动的四足运动简化为倒立摆运动，并规划四足机器人的支撑相与摆动相，通过支撑相控制四足机器人躯干的期望俯仰角、期望横滚角和期望速度，并反馈的实际俯仰角、实际横滚角和实际速度。

8.如权利要求7所述的可变构型的四足机器人运动控制方法，其特征在于，使用所述摆动相控制四足机器人的落足点，使用三次曲线规划在空中的运动轨迹；机器人的运行速度通过在摆动相落足点上引入关于速度偏差量的函数调节；通过时间和触地检测状态机相结合来决定每条腿处于摆动相或者支撑相，按照规划的轨迹运动。

9.一种可变构型的四足机器人运动控制系统，其特征在于，包括以下模块：

第一模块，被配置为规划四足机器人的运动步态，且以trot步态作为四足机器人的运动步态，保证机器人的躯干质心投影在两支撑腿足端连线的中间；

建立以四足机器人的足端位置、速度作为输入的四足机器人的整数规划模型；

第二模块，被配置为基于冗余自由度，实现对机器人各种构型之间的切换；

第三模块，被配置为根据当前构型规划并执行机器人的运动。

Images