CN115951696A - 一种四足机器人路面姿态自适应方法 - Google Patents

Abstract

本发明属于机器人技术领域，公开了一种路面姿态自适应方法。包括根据当前时刻触地足端坐标得到滚转角俯仰角偏移量；根据滚转角俯仰角偏移量和当前时刻机身姿态角得到下一时刻期望机身姿态角；根据滚转角俯仰角偏移量、当前时刻机身高度与机身长度得到下一时刻期望机身高度；根据当前时刻落足点位置坐标确定下一时刻期望落足点位置坐标；将期望机身姿态角输入到MPC中得到初始地面反作用力；用全身阻抗控制对期望机身姿态角、期望机身高度和落足点位置坐标进行处理得到期望关节角度角速度角加速度；根据初始地面反作用力、角度角速度角加速度得到期望关节力矩以控制四足机器人运动。能使四足机器人自动调整机身姿态和高度，克服平坦路面的局限性。

Description

一种四足机器人路面姿态自适应方法

技术领域

本发明属于机器人技术领域，特别涉及一种四足机器人路面姿态自适应方法。

背景技术

四足机器人能在复杂的路面上行走，目前得到了广泛的关注。为了使四足机器人适应复杂的路面，需要对四足机器人进行控制。

现有技术中，公开了一种基于模型预测控制和零空间投影的四足机器人控制方法。该方法首先忽略四足机器人腿部转动惯量，将复杂的动力学模型简化成单刚体动力学模型，接着假设机身俯仰角和滚转角时刻都等于零，基于偏航角对非线性模型进行离散化，然后基于离散模型应用模型预测控制根据定义好的期望机身位置、速度、角度以及角速度求出四条腿的地面反作用力。随后，将期望关节角度、角速度和角加速度带入到全身浮基动力学模型中，根据求解好的地面反作用力，求出最终的期望电机输出力矩，最后应用比例微分控制使电机输出期望力矩。由于前述方法通过假设机身俯仰角和滚转角始终为零而建立的动力学离散模型，所以无论在何种地形下，机身始终和水平面平行，因此适应性不强；在路面和水平面成一定角度时，如斜坡或者楼梯，例如，在上坡或者上楼梯时，原始方法迫使机身下压到和水平面平行，从而使得前腿过度弯曲而后退过度拉伸，电机输出可能超过力矩限制损坏电机，而且机身震动会随之变大，损伤机器人内外部部件。同时，原始方法足端轨迹规划算法应用三次贝塞尔权限，根据起始点和最终点规划出一条摆动曲线，摆动腿跟随此曲线完成一次摆动过程，存在的问题是此算法仅适用于与水平面平行的路面，而当路面与水平面呈一定角度时，因为没有腿部压力传感器，四足机器人无法得知摆动腿已完成摆动过程，而且在上坡或者上楼梯时，机身和水平面平行，所以前腿会提前触地而算法此时认为摆动腿还未触底，这就导致前腿会以非常大的速度接触地面，从而使整体结构震荡损伤机器人。

发明内容

为了解决上述问题，本发明提供了一种四足机器人路面姿态自适应方法，其包括：

根据当前时刻四足机器人与路面接触的触地足端坐标，得到滚转角偏移量和俯仰角偏移量；根据所述俯仰角偏移量、所述滚转角偏移量和当前时刻的机身姿态角，得到下一时刻的期望机身姿态角；根据所述俯仰角偏移量、所述滚转角偏移量、当前时刻四足机器人的机身高度与机身长度，得到下一时刻的期望机身高度；根据当前时刻的落足点位置坐标确定下一时刻的期望落足点位置坐标，所述期望落足点位置坐标中的z轴分量是根据当前时刻触底足坐标的z轴分量和期望机身姿态角得到；将所述期望机身姿态角输入到MPC模型预测控制中，得到初始地面反作用力；利用全身阻抗控制对所述期望机身姿态角、期望机身高度和所述期望落足点位置坐标进行处理得到期望关节角度、期望关节角速度与期望关节角加速度；根据所述初始地面反作用力、期望关节角度、期望关节角速度与期望关节角加速度，得到期望关节力矩；基于所述期望关节力矩，控制所述四足机器人运动。

在如上所述的方法中，可选地，所述根据当前时刻四足机器人与路面接触的触地足端坐标，得到滚转角偏移量和俯仰角偏移量，包括：根据当前时刻四足机器人与路面接触的触地足端坐标，确定构成虚拟操作平面的3个触地足端坐标；根据所述3个触地足端坐标得到滚转角偏移量和俯仰角偏移量。

在如上所述的方法中，可选地，所述根据当前时刻四足机器人与路面接触的触地足端坐标，确定构成虚拟操作平面的3个触地足端坐标，包括：若当前时刻所述四足机器人的触地足端个数等于3，则将当前时刻的所述触地足端坐标确定为所述触地足端坐标；若当前时刻所述四足机器人的触地足端个数小于3，则将上一时刻构成虚拟操作平面的3个触地足端坐标确定为当前时刻构成虚拟操作平面的3个所述触地足端坐标；若当前时刻所述四足机器人的触地足端个数等于4，则将当前时刻4个触地足端坐标中的3个确定为3个所述触地足端坐标。

在如上所述的方法中，可选地，所述根据所述3个触地足端坐标得到滚转角偏移量和俯仰角偏移量，包括：根据所述3个触地足端坐标得到初始滚转角偏移量和初始俯仰角偏移量；对所述初始滚转角偏移量和初始俯仰角偏移量分别进行归一化处理，得到所述滚转角偏移量和所述俯仰角偏移量。

在如上所述的方法中，可选地，所述对所述初始滚转角偏移量和初始俯仰角偏移量分别进行归一化处理，得到所述滚转角偏移量和所述俯仰角偏移量，包括：采用Sigmoid函数对所述初始滚转角偏移量和初始俯仰角偏移量分别进行处理，得到所述滚转角偏移量和所述俯仰角偏移量。

在如上所述的方法中，可选地，所述根据所述俯仰角偏移量、所述滚转角偏移量和当前时刻的机身姿态角，得到下一时刻的期望机身姿态角，包括：

期望机身姿态角

的计算公式为：

式中，φ_t、θ_t、ψ_t分别表示当前时刻机身姿态角中的滚转角、俯仰角和偏航角，

表示滚转角偏移量，

表示俯仰角偏移量。

在如上所述的方法中，可选地，所述根据所述俯仰角偏移量、所述滚转角偏移量、当前时刻四足机器人的机身高度与机身长度，得到下一时刻的期望机身高度，包括：

期望机身高度

的计算公式为：

式中，h表示当前时刻机身高度，L表示机身长度，

表示滚转角偏移量，

表示俯仰角偏移量。

在如上所述的方法中，可选地，所述根据当前时刻的落足点位置坐标确定下一时刻的期望落足点位置坐标，所述期望落足点位置坐标中的z轴分量是根据当前时刻触底足坐标的z轴分量和期望机身姿态角得到，包括：

期望落足点位置坐标

表示为：

其中，

的计算公式为：

式中，i表示第i个触地足，

为当前时刻触地足坐标的z轴分量，φ_t、θ_t分别表示当前时刻机身姿态角中的滚转角和俯仰角，

表示滚转角偏移量，

表示俯仰角偏移量。

在如上所述的方法中，可选地，所述基于所述期望关节力矩，控制所述四足机器人运动，包括：在运动过程中，采用五次贝塞尔曲线来规划摆动腿的轨迹；在所述五次贝塞尔曲线中，第一控制点位于初始点沿四足机器人的前进方向上，第二控制点位于初始点和最终点在x轴方向上的三分之一位置，第第三个控制点位于初始点和最终点在x轴方向上的三分之二位置，所述初始点坐标为当前时刻的触地足端位置坐标，所述最终点坐标为期望落足点位置坐标。

在如上所述的方法中，可选地，所述根据所述初始地面反作用力、期望关节角度、期望关节角速度与期望关节角加速度，得到期望关节力矩，包括：利用二次规划，对所述期望角加速度和所述初始地面反作用力进行优化，得到优化角加速度和优化地面反作用力；采用浮基动力学模型，对所述优化角加速度和优化地面反作用力进行处理，得到期望关节力矩。

本发明实施例提供的技术方案带来的有益效果是：

1、能够使四足机器人在特殊路面实时生成虚拟操作平面，自动调整机身姿态和高度，克服原始算法只针对于平坦路面的局限性。

2、进后的落足点位置和摆动腿轨迹避免了原始算法在斜坡路面或者楼梯上，前腿触地力过大的问题，达到了保护机器人，避免过大的反作用力损伤结构的目的。

3、足机器人可以在斜坡或者楼梯上自由上下或者转弯，机动性得到了极大的提升。

附图说明

图1为本发明实施例提供的一种四足机器人路面姿态自适应方法的流程示意图。

具体实施方式

下面将参考附图并结合实施例来详细说明本发明。

参见图1，本发明实施例提供了一种四足机器人路面姿态自适应方法，其包括以下步骤：

步骤101，根据当前时刻四足机器人与路面接触的触地足端坐标，得到滚转角偏移量和俯仰角偏移量。

步骤102，根据俯仰角偏移量、滚转角偏移量和当前时刻的机身姿态角，得到下一时刻的期望机身姿态角。

步骤103，根据俯仰角偏移量、滚转角偏移量、当前时刻四足机器人的机身高度与机身长度，得到下一时刻的期望机身高度。

步骤104，根据当前时刻的落足点位置坐标确定下一时刻的期望落足点位置坐标，期望落足点位置坐标中的z轴分量是根据当前时刻触底足坐标的z轴分量和期望机身姿态角得到。

步骤105，将期望机身姿态角输入到MPC模型预测控制中，得到初始地面反作用力。

步骤106，利用全身阻抗控制对期望机身姿态角、期望机身高度和期望落足点位置坐标进行处理得到望关节角度、期望关节角速度与期望关节角加速度。

步骤107，根据初始地面反作用力、期望关节角度、期望关节角速度与期望关节角加速度，得到期望关节力矩。

步骤108，基于期望关节力矩，控制四足机器人运动。

为了克服现有的算法无法使四足机器人应对斜坡或楼梯等复杂路面，本发明提供一种基于虚拟操作平面的四足机器人在上下楼梯时机身姿态自适应算法。该方法基于机身处的姿态传感器，构建出一个四足机器人与足端接触的虚拟操作平面，计算出机身坐标系和虚拟平面三轴角度偏移量，随后将偏移量作为模型预测控制中机身的期望角度以求解改进后的地面反作用力；接着将偏移量再带入到全身阻抗控制中，作为机身期望角度的增量，随后将当前时刻摆动腿足端触地相对于世界坐标系下的位置中z方向的数据，作为下一个摆动周期当前摆动腿在世界坐标系下期望落足点的z轴位置。最后，使用零空间投影的方法，将第一个任务优先级定义为机身姿态，第二个任务优先级定义为机身位置，第三个任务优先级定义为落足点位置，得到期望关节角度、角速度和角加速度，再根据模型预测控制器结算出来的地面反作用力，带入到全身浮基动力学表达式中，求得最后的关节力矩。该处机身位置是机身XYZ方向位置，其中，Z方向位置即是机身高度。

下面对各步骤涉及的内容进行详细说明：

对于四足机器人来说，由于腿部质量相对于机身来说是很小的一部分，因此可以忽略腿部转动惯量，得到简化的单钢体动力学模型：

其中，p、

ω、f_i、c_g分别为世界坐标系下的机身位置、机身线加速度、机身角速度和地面反作用力、重力加速度。m为机身质量，应用中可以忽略腿部质量。I为世界坐标系下机身转动惯量，n_c是四足机器人和路面接触的触地足的总接触数。r_i是与地面第i个接触点的位置坐标。

姿态角包括：滚转角φ、俯仰角θ和偏航角ψ，假设滚转角和俯仰角都足够小，可以得到以下简化后的坐标系转换：

_gI≈R_z(ψ)_BIR_z(ψ)^T  (1-2)

其中，下角标g和B分别表示世界坐标系(ground)和机身坐标系(body)，

为机身坐标系下的机身角速度，R_z(ψ)是将世界坐标系下的角速度转换到机身坐标系下的转换矩阵。

假设机器人状态接近于控制指令轨迹，那么可以用偏航角ψ来对整个系统进行线性化。综上所述，四足机器人简化后的离散模型可以表示为：

x(k+1)＝A_kx(k)+B_kf(k)+g  (1-3)

其中，

为机器人状态，Θ为世界坐标系下机身角度，

为机身线速度，f为地面反作用力，g为重力加速度，k、k+1表示当前时刻以及下一时刻。

其中，Δt为时间间隔，n表示触地足个数，r_n表示第n个触地足的坐标，1_3×3为单位阵，R_z(ψ_k)为绕当前时刻k偏航角的旋转矩阵，公式如下：

为了能够使四足机器人行走在特殊路面，如楼梯，斜坡，自动调整机身姿态和高度，克服现有技术中的算法只针对于平坦路面的局限性，本方法设计了实时的虚拟操作平面。具体如下：

四足机器人的前腿分为左前腿和右前腿，后腿分为左后腿和右后腿，相应地，触地足(落地足)分为左前足与右前足以及左后足和右后足，在运动时采用对角步态，即：左前腿和右后腿为一组，右前腿和左后腿为一组，两组交替摆动。

判断当前触地足的个数n，基于触地足的个数确定构建虚拟操作平面的3个触地足端坐标。考虑到在机器人爬楼梯的过程中，由于X轴速度矢量方向和世界坐标系的X轴方向产生偏移或由于自身踏空，从而导致两足、三足和四足站立的三种情况发生，本方案考虑这三种情况。

在四足机器人上楼梯过程中，四只足可能处于不同的位置，即P_i＝[x_i，y_i，z_i]^T都不相同：x₁≠x₂≠x₃≠x₄，y₁≠y₂≠y₃≠y₄，z₁≠z₂≠z₃≠z₄，i＝1、2、3、4。

由于机身姿态中的俯仰角在实时变化，而机身高度始终为定值，因此重心会向机身后方略微偏移，如此导致后腿将承担更多的支撑作用，所以在四足支撑的情况下，始终选取左后足和右后足为虚拟操作平面三点中的两点。由于前腿主要起到试探的作用，因此可能出现两条前腿不在同一级楼梯的情况。例如：左前足相对于右前足处在上一级台阶上，即x₁＞x₂，z₁＞z₂。为了使机身俯仰角变化量不至于过大，选取两个前足端中x轴和z轴数值较小的为第三个点。最终由这三个点构成虚拟操作平面。在双足支撑的情况下，选取上一时刻的虚拟操作平面为当前时刻的虚拟操作平面，直到三足或四足支撑的情况重新出现。在三足支撑的情况下，选取该三个点构成虚拟操作平面。

需要说明的是：虚拟操作平面，其中z轴和重力矢量平行，x轴与重力矢量垂直并且与机器人速度方向一致，坐标轴中心和世界坐标系中心重合。当四足机器人在楼梯或者斜坡上行走时，虚拟平面的z轴和机器人的足端所构成的平面的法向量平行，x轴和机器人坐标系在足端所构成的平面上的投影平行，坐标原点和世界坐标系原点重合，因此只考虑虚拟坐标系的相对旋转运动而可以忽略平移运动。

基于前述内容，确定了构成虚拟操作平面的三个触地足坐标P_i＝[x_i，y_i，z_i]^T，然后基于三个触地足坐标计算初始滚转角偏移量Δφ_t和初始俯仰角偏移量Δθt，具体计算公式如下：

其中，n_f和n_h分别表示当前前腿和后腿触地的个数，x_i，y_i，z_i分别表示足端相对于世界坐标系下的三轴位置。俯仰角和滚转角相对于世界坐标系的初始偏移量Δθ_t，Δφ_t。

为保证每个时刻偏移量不会过大，对初始化滚转角偏移量和俯仰角偏移量进行归一化处理，得到滚转角偏移量和俯仰角偏移量，优选地，引入Sigmoid函数进行约束。具体计算公式如下：

根据俯仰角偏移量

和滚转角偏移量

计算期望机身姿态角

和期望机身高度

具体计算公式如下：

其中，φ_t、θ_t、φ_t分别表示当前时刻的滚转角、俯仰角和偏航角，h为当前机身相对于世界坐标系的高度，L为机身长度。

将期望机身姿态角输入到MPC(Model Predictive Control，模型预测控制)中计算初始的地面反作用力f_r。需要说明的是：关于MPC以及MPC的处理过程，可以为现有技术中的MPC以及MPC的处理过程，本实施例对此不进行限定。本方法中是对MPC的输入进行了设计，将原先的机身姿态角替换为期望机身姿态角。处理时，MPC将俯仰角和滚转角假设为0度，针对偏航角将单钢体动力学模型做线性化处理，所以MPC的期望状态里只有偏航角。代价函数总共输入为12维向量：[XYZ速度、XYZ线速度、0、0、偏航角、0、0、偏航角速度]。

MPC用来计算地面反作用力。因为只考虑地面反作用力而不考虑关节力矩，MPC并不需要知道腿部的运动学表达式。模型预测控制包含需要被控制的系统模型，在每一次迭代中，从当前状态开始，基于定义好的状态轨迹约束和控制输入约束，再依照给定的预测长度，寻找最优的控制序列和相对应的状态轨迹。因为这个过程在每一次迭代中都会重复，所以只采取求得的控制序列中的第一个控制量输入给系统，然后系统进入下一个状态，之后不断重复这个过程。给定预测长度K，MPC问题可以通过以下表达式来描述：

s.t.x_i+1＝A_ix_i+B_iu_i，i＝0…K-1(2-2)

D_iu_i，i＝0…K-1(2-4)

其中Q_i和R_i为对角半正定矩阵，表示权重。C_i、c_i 和

表示控制输入的不等约束，D_i是选择矩阵，用来选择没有和地面接触的腿对应的力，u_i为控制量，即触地足端反作用力。x_i+1，ref是i+1时刻的参考轨迹，而x_i+1是+1时刻的传感器输出的机身状态。公式(2-2)表示的等式约束用来将离地腿的力设置为0，公式(2-3)表示的不等约束用来设置以下和地面接触的腿的10个不等约束。

f_min≤f_z≤f_max  (2-5)

-μf_z≤±f_x≤μf_z  (2-6)

-μf_z≤±f_y≤μf_z  (2-7)

上述约束函数限制了z方向上的最大和最小力，同时限制了摩擦锥的正方棱锥估计。

参考轨迹只考虑x、y方向上的速度，x、y方向上的位置，z方向上的位置，偏航和偏航率。所有参数都由机器人操控着直接给出，除了x、y方向上的位置和偏航角，这三个参数由合适的速度计算得到。其余的参数都设置为0。在实际操作中，参考轨迹非常短，通常在0.3到0.5秒之间，然后每0.03到0.05秒重新计算以确保简化的动力学模型在扰动下足够准确。

在对系统离散化的过程中，公式(1-4)中B矩阵只和足部与地面接触位置r_n和ψ有关，因此B的估计值

可以由参考轨迹和落足点位置计算得到的ψ和r_i求得，其中n为期望轨迹离散点个数。类似的，整个参考轨迹的A矩阵的估计值

可以由ψ的平均值计算得到。A和B都被转换到了零阶保持离散时间模型中：

上式允许将整个四足机器人简化模型用离散时间的形式来表达：

上式对四足机器人动力学模型的估计只有当机器人能够按照参考轨迹运动是才准确，当遇到较大扰动的时候，比如因地形影响，会导致

不准确。MPC能够在每次迭代中重新计算参考轨迹，因此会保证在扰动情况下模型的准确度。n为期望轨迹离散点个数。

注意到公式(2-1)的优化问题过于复杂，计算量会随着MPC预测长度、状态的数量和约束量呈三次方增长，因此需要对问题进行降维以简化计算。通过直接移除动力学约束和状态轨迹约束，并将它们直接添加到代价函数中。进一步的，此方法同样移除了不重要的等式约束公式(2-4)，从而使得优化问题简化到只优化与地面接触的腿的足端反作用力。简化后的动力学表达式称为浓缩表达式：

X＝A_qpx₀+B_qpU  (2-10)

其中，A_qp，B_qp是求解二次规划时状态矩阵Condensed构造方式，A_qp包含单位阵在内的所有MPC预测序列，B_qp为半正定矩阵，主对角线右侧全为零，

是状态，

是控制输入，k为预测长度，n为与地面接触腿的个数。目标函数旨在最小化和参考轨迹之间的加权最小二乘偏差：

J(U)＝||A_qpx₀+B_qpU-x_ref||_L+||U||_K  (2-11)

其中，

是状态偏差的加权对角阵，

是反作用力偏差加权对角阵，U和X是整个预测长度内所有的控制输入和状态。x₀为系统初始状态。定义K＝α1_3nk。优化问题可以重写为：

其中，C为约束矩阵，H为Hessian矩阵，g为仿射函数约束项：

整体程序执行过程为：首先得到离散系统的状态矩阵，接着根据预测长度初始化浓缩状态矩阵和约束矩阵，然后将所有矩阵带入到二次规划求解器qpOASES中构建二次规划问题，最后计算足端反作用力并将输出向量的头12个元素作为机器人的最终足端力带入到后续计算中。

下面对摆动腿落足点规划及摆动轨迹规划进行详细说明：

四足机器人的落足点决定了其摆动腿在下一支撑相中足部的具体位置，首先需要保证在摆动的过程中，腿部按照设计好的轨迹摆动(五次贝塞尔曲线)，而且要同时保证机身的姿态稳定。下一时刻摆动腿期望落足点由以下公式给出：

其中，

p_s，i表示第i条摆动腿的髋关节相对于机身坐标系的位置，可以表示为

可由转换矩阵和机身位置计算得出：

其中，

为当前时刻机身的位置，R_z(ψ_k)是机身坐标系转换到腿部坐标系的转换矩阵，ψ_k为当前偏航角，I_i为第i个摆动腿髋关节在机身坐标系中的位置。

p_s是Raibert启发式算法，它约束了腿部落地瞬间的角度和离地瞬间的角度，并且确保这两个角度在任意一个步态周期内相等。p_s可由以下公式给出

其中，t_stance为支撑相的相位时间，v为当前机器人的线速度，v_cmd为速度指令，增益k为正数，可以取0.03。考虑虚拟操作平面，期望速度在X轴和Y轴上的分量修正为

下角标rc表示遥控器输入值。需要说明的是：引入Sigmoid函数进行约束，Δθ_t可以调整为

Δφ_t可以调整为

p_c为离心位置，可由以下公式给出：

其中，h为抬腿高度，可以提前定义，g为重力加速度，v为当前机器人的线速度，ω_cmd为角速度指令。p_c的主要作用是确保在整个摆动周期中，四足机器人机身按照角速度指令移动，同时确保机器人整体稳定不会摔倒。将当前时刻摆动腿足端触地相对于世界坐标系下的位置中z方向的数据，作为下一个摆动周期当前摆动腿在世界坐标系下期望落足点的z轴位置。因此公式(3-1)中，

分量重新定义为

其中，

为当前时刻此摆动腿在世界坐标系下z轴位置。需要说明的是：引入Sigmoid函数进行约束，Δθ_t可以调整为

Δφ_t可以调整为

考虑到在爬楼梯过程中，机器人摆动腿可能存在与紧挨楼梯的情况，从而使得下一时刻的摆动会与楼梯产生碰撞而无法成功的完成一次摆动相。采用五次贝塞尔曲线来规划摆动腿的轨迹，其中第一个控制点p₁位于初始点p₀沿机器人前进方向，即x轴的后方。第二个控制点p₂位于初始点和最终点p_f，在x轴方向上的三分之一位置，用来定义整个轨迹的最高高度，可由p₂＝f(h_step，p₀，p_f)函数来表示，h_step为定义好的摆动腿最大腾空高度。第三个控制点p₃位于初始点和最终点在x轴方向上的三分之二位置，且在z轴负方向上，作用为使得落地点处的轨迹曲线足够平滑，从而尽可能使得z方向上的速度为0以减少落地瞬间由碰撞产生的力，达到减小机体损伤的目的。初始点为上一时刻的落足点位置，最终点为期望落足点位置

下面对全身阻抗控制进行详细说明：

全身阻抗控制(WholeBody Impulse Control，WBIC)是基于零空间投影的全身阻抗控制。需要说明的是：关于全身阻抗控制的处理技术，可以为现有技术中的全身阻抗控制的处理技术，本实施例对此不进行限定。本方法中是基于滚转角偏移量和俯仰角偏移量，对相应的设计进行了改进。引入Sigmoid函数进行约束，下述公式中的Δθ_t可以调整为

Δφ_t可以调整为

基于前述确定的构成虚拟操作平面的三个触地足坐标得出当前虚拟操作平面和世界坐标系关于x和y轴的角度偏移量，随即求出俯仰角和滚转角偏量ΔΔθ_t与Δφ_t。定义期望机身姿态角：

再定义四足机器人广义角度

其中，

为机身姿态角，

为所有腿部的关节角。为保证每个时刻偏移量不会过大，引入归一化处理，因此可以将前述的Δφ_t与Δθ_t替换成

与

迭代规则为：

其中，J_i|i-1为第i个任务雅可比矩阵在第i-1个任务雅可比矩阵零空间上的投影。其定义为

J_i|i-1＝J_iN_i-1

N_i-1＝N₀N_1|0…N_i-1|i-2  (4-5)

投影矩阵N定义为

初始状态

其中，e_i为第i个任务x_i和参考轨迹x_ref之间的误差，即e_i＝x_ref-x_i。特别的，考虑第二个任务优先级(i＝2)，即期望机身位置，由于虚拟操作平面的作用，机身会以质心为中心，旋转Δθ_t，Δφ_t，从而使得后腿处于过度弯曲的状态，因此需要调整机身高度使机器人适应于当前虚拟平面来避免腿部过度弯曲导致的力矩超限。定义

其中

的计算公式如下：

式中，h为当前机身相对于世界坐标系的高度，L为机身长度，Δφ_t，Δθ_t分别为基于虚拟操作平面的滚转角和俯仰角偏移量。

为第i个任务的加速度指令：

式中，K_p和K_d是任务层面的位置和速度反馈增益。

为动态一致伪逆运算。接着，关节角度指令可以通过公式(4-2)求得的角度变化量带入到下式中求解达到：

式中，j为当前时刻，接着应用二次规划，在以下等式约束和不等约束下，通过最小化代价函数，计算松弛变量，最后对期望角加速度和地面反作用力进行优化：

Wf_r≥0

其中，

为MPC计算得出的足端反作用力(初始地面反作用力)，S_f＝[0_6×6I_n×n]为选择矩阵(I_n×n是n维单位阵)，n为关节数，J_c ^T为与地面接触的雅可比矩阵的转置，Q₁，Q₂为参数对角阵，δ_f，

为机身加速度和反作用力的松弛变量，

为浮基动力学模型，该模型为现有的模型，本发明实施例并未对模型进行改进设计。最后，结合上述QP(Quadratic Programming，二次规划)最优算法得出的优化后的加速度

和反作用力f_r，可以求出电机的前馈力矩：

其中，τ_f为机身力矩，因为前驱动系统机身无法直接控制，因此τ_f＝0，τ_j为关节力矩。

由技术常识可知，本发明可以通过其它的不脱离其精神实质或必要特征的实施方案来实现。因此，上述公开的实施方案，就各方面而言，都只是举例说明，并不是仅有的。所有在本发明范围内或在等同于本发明的范围内的改变均被本发明包含。

Claims (10)

1.一种四足机器人路面姿态自适应方法，其特征在于，包括：

根据当前时刻四足机器人与路面接触的触地足端坐标，得到滚转角偏移量和俯仰角偏移量；

根据所述俯仰角偏移量、所述滚转角偏移量和当前时刻的机身姿态角，得到下一时刻的期望机身姿态角；

根据所述俯仰角偏移量、所述滚转角偏移量、当前时刻四足机器人的机身高度与机身长度，得到下一时刻的期望机身高度；

根据当前时刻的落足点位置坐标确定下一时刻的期望落足点位置坐标，所述期望落足点位置坐标中的z轴分量是根据当前时刻触底足坐标的z轴分量和期望机身姿态角得到；

将所述期望机身姿态角输入到MPC模型预测控制中，得到初始地面反作用力；

利用全身阻抗控制对所述期望机身姿态角、期望机身高度和所述期望落足点位置坐标进行处理得到期望关节角度、期望关节角速度与期望关节角加速度；

根据所述初始地面反作用力、期望关节角度、期望关节角速度与期望关节角加速度，得到期望关节力矩；

基于所述期望关节力矩，控制所述四足机器人运动。

2.根据权利要求1所述的方法，其特征在于，所述根据当前时刻四足机器人与路面接触的触地足端坐标，得到滚转角偏移量和俯仰角偏移量，包括：

根据当前时刻四足机器人与路面接触的触地足端坐标，确定构成虚拟操作平面的3个触地足端坐标；

根据所述3个触地足端坐标得到滚转角偏移量和俯仰角偏移量。

3.根据权利要求2所述的方法，其特征在于，所述根据当前时刻四足机器人与路面接触的触地足端坐标，确定构成虚拟操作平面的3个触地足端坐标，包括：

若当前时刻所述四足机器人的触地足端个数等于3，则将当前时刻的所述触地足端坐标确定为所述触地足端坐标；

若当前时刻所述四足机器人的触地足端个数小于3，则将上一时刻构成虚拟操作平面的3个触地足端坐标确定为当前时刻构成虚拟操作平面的3个所述触地足端坐标；

若当前时刻所述四足机器人的触地足端个数等于4，则将当前时刻4个触地足端坐标中的3个确定为3个所述触地足端坐标。

4.根据权利要求2所述的方法，其特征在于，所述根据所述3个触地足端坐标得到滚转角偏移量和俯仰角偏移量，包括：

根据所述3个触地足端坐标得到初始滚转角偏移量和初始俯仰角偏移量；

对所述初始滚转角偏移量和初始俯仰角偏移量分别进行归一化处理，得到所述滚转角偏移量和所述俯仰角偏移量。

5.根据权利要求4所述的方法，其特征在于，所述对所述初始滚转角偏移量和初始俯仰角偏移量分别进行归一化处理，得到所述滚转角偏移量和所述俯仰角偏移量，包括：

采用Sigmoid函数对所述初始滚转角偏移量和初始俯仰角偏移量分别进行处理，得到所述滚转角偏移量和所述俯仰角偏移量。

6.根据权利要求1所述的方法，其特征在于，所述根据所述俯仰角偏移量、所述滚转角偏移量和当前时刻的机身姿态角，得到下一时刻的期望机身姿态角，包括：

期望机身姿态角

的计算公式为：

式中，φ_t、θ_t、ψ_t分别表示当前时刻机身姿态角中的滚转角、俯仰角和偏航角，

表示滚转角偏移量，

表示俯仰角偏移量。

7.根据权利要求1所述的方法，其特征在于，所述根据所述俯仰角偏移量、所述滚转角偏移量、当前时刻四足机器人的机身高度与机身长度，得到下一时刻的期望机身高度，包括：

期望机身高度

的计算公式为：

式中，h表示当前时刻机身高度，L表示机身长度，

表示滚转角偏移量，

表示俯仰角偏移量。

8.根据权利要求1所述的方法，其特征在于，所述根据当前时刻的落足点位置坐标确定下一时刻的期望落足点位置坐标，所述期望落足点位置坐标中的z轴分量是根据当前时刻触底足坐标的z轴分量和期望机身姿态角得到，包括：

期望落足点位置坐标

表示为：

其中，

的计算公式为：

式中，i表示第i个触地足，

为当前时刻触地足坐标的z轴分量，φ_t、θ_t分别表示当前时刻机身姿态角中的滚转角和俯仰角，

表示滚转角偏移量，

表示俯仰角偏移量。

9.根据权利要求1所述的方法，其特征在于，所述基于所述期望关节力矩，控制所述四足机器人运动，包括：

在运动过程中，采用五次贝塞尔曲线来规划摆动腿的轨迹；

在所述五次贝塞尔曲线中，第一控制点位于初始点沿四足机器人的前进方向上，第二控制点位于初始点和最终点在x轴方向上的三分之一位置，第第三个控制点位于初始点和最终点在x轴方向上的三分之二位置，所述初始点坐标为当前时刻的触地足端位置坐标，所述最终点坐标为期望落足点位置坐标。

10.根据权利要求1所述的方法，其特征在于，所述根据所述初始地面反作用力、期望关节角度、期望关节角速度与期望关节角加速度，得到期望关节力矩，包括：

利用二次规划，对所述期望角加速度和所述初始地面反作用力进行优化，得到优化角加速度和优化地面反作用力；

采用浮基动力学模型，对所述优化角加速度和优化地面反作用力进行处理，得到期望关节力矩。

Images