CN117193070A - 一种四足机器人落足与机体协调控制方法 - Google Patents

Abstract

本发明提供一种四足机器人落足与机体协调控制方法，所述方法包括：获取目标区域内的高程信息，并根据所述高程信息计算平衡落足点，分别计算新落足和当前支撑足的运动学参数，并根据新落足和当前支撑足的运动学参数计算当前位姿下的压力中心；计算所述平衡落足点的平面位置，根据所述平衡落足点的平面位置和所述当前支撑足的运动学参数计算稳定压力中心；从而利用落足点和相关的机体稳定的融合信息，在保证安全落足的同时，机器人身体进行协调适应。解决了现有技术中机器人跨越大尺寸障碍物时稳定性较差的问题，从而提高了足式机器人在跨越障碍物时的落足稳定性，进而提高了足式机器人在复杂地形的通过性能。

Description

一种四足机器人落足与机体协调控制方法

技术领域

本发明涉及机器人控制技术领域，尤其涉及一种四足机器人落足与机体协调控制方法。

背景技术

四足机器人相比于传统的轮式、履带式车辆，可通过选择离散的摆动腿落足点，因此可适应高原、山地等复杂的地形。目前，四足机器人主要依靠落足点实现机器人的平衡，当遇到较大尺寸的障碍物时，在机器人越障时，机器人会通过选择安全的落足点实现对机器人的跨越，然而，当安全的落足点超出稳定的控制范围时，即机器人在大跨步越障时，机器人的动态稳定性将大大降低，甚至会发生倾覆的危险。

有鉴于此，提供一种用于四足机器人落足与机体协调的控制方法，以解决机器人跨越大尺寸障碍物时稳定性较差的问题，从而提高足式机器人在跨越障碍物时的落足稳定性，进而提高足式机器人在复杂地形的通过性能，就成为本领域技术人员亟待解决的问题。

发明内容

本发明提供一种四足机器人落足与机体协调控制方法，以解决现有技术中机器人跨越大尺寸障碍物时稳定性较差的问题，从而提高足式机器人在跨越障碍物时的落足稳定性，进而提高足式机器人在复杂地形的通过性能。

本发明提供一种四足机器人落足与机体协调控制方法，包括：

获取目标区域内的高程信息，并根据所述高程信息计算平衡落足点；

分别计算新落足和当前支撑足的运动学参数，并根据新落足和当前支撑足的运动学参数计算当前位姿下的压力中心；

计算所述平衡落足点的平面位置，根据所述平衡落足点的平面位置和所述当前支撑足的运动学参数计算稳定压力中心；

基于所述压力中心与稳定压力中心之间的偏移量构建机身控制模型，并通过所述机身控制模型计算足部作用力；

以所述足部作用力作为输入，构建协调控制模型，并通过所述协调控制模型计算当前位姿下的关节运动参数，以使足式机器人的控制装置根据所述关节运动参数输出控制指令，所述控制指令用于控制所述足式机器人的腿部关节动作。

根据本发明提供的一种四足机器人落足与机体协调控制方法，获取目标区域内的高程信息，并根据所述高程信息计算平衡落足点，具体包括：

获取目标区域内的高程信息，并基于所述高程信息生成高程图；

根据所述高程信息计算目标区域内的高程标准差；

在所述高程标准差小于预设的标准差阈值、且所述高程信息大于预设的高程阈值的情况下，则计算平衡落足点。

根据本发明提供的一种四足机器人落足与机体协调控制方法，其特征在于，基于所述目标落足点位置，分别计算新落足和当前支撑足的运动学参数，并根据新落足和当前支撑足的运动学参数计算当前位姿下的压力中心，具体包括：

利用机器人腿部运动学算法和采集的惯性数据，计算当前支撑足的竖直方向力和平面位置，以及新落足的竖直方向力和平面位置；

根据当前支撑足的竖直方向力和平面位置，以及新落足的竖直方向力和平面位置，计算得到当前位姿下的压力中心。

根据本发明提供的一种四足机器人落足与机体协调控制方法，利用第一表达式和第二表达式，计算当前位姿下的压力中心的x、y方向位置；

所述第一表达式为：

所示第二表达式为：

式中，F_zi为第i个支撑足的竖直方向力，F_zσj为第j个新落足的竖直方向力，p_σxj、p_σyj为第j个新落足的x、y方向位置，p_ix、p_iy为第i个当前支撑足的x、y方向位置，p′_{cop_x}、p′_{cop_y}为压力中心的x、y方向位置，n_σ为新落足数量，n_c为当前支撑足数量。

根据本发明提供的一种四足机器人落足与机体协调控制方法，计算所述平衡落足点的平面位置，根据所述平衡落足点的平面位置和所述当前支撑足的运动学参数计算稳定压力中心，具体包括：

建立目标优化函数；

设定约束条件，通过求解满足约束条件的所述目标优化函数，得到目标落足点的期望竖直方向力以及当前支撑足的期望竖直方向力；

根据所述目标落足点的期望竖直方向力以及当前支撑足的期望竖直方向力，计算稳定压力中心。

根据本发明提供的一种四足机器人落足与机体协调控制方法，所述目标优化函数为：

minF(x)＝(Ax-b)^TS(Ax-b)+αx^TWx+β‖x-x^*‖

其中，

式中，M_m、F_m为机体虚拟合力矩矢量、虚拟合力矢量；m为机器人质量；S为加权矩阵，W为半正定对称矩阵，α、β为整定因子；x^*为上一时刻的优化解；r_i为第i个足部相对机身质心在世界坐标系下的向量，包含平衡落足点以及当前支撑点。

根据本发明提供的一种四足机器人落足与机体协调控制方法，利用第三表达式计算机体虚拟合力矩矢量和虚拟合力矢量，所述第三表达式为：

式中，M_m、F_m为机体虚拟合力矩矢量、虚拟合力矢量；k_p为正定增益矩阵；q为机身实际的位姿矢量；q_d为机身期望的位姿矢量；k_v为正定微分系数矩阵；为机身实际的位姿速度矢量，/>为机身期望的位姿速度矢量，k_ff为正定速度前馈系数矩阵，m为机器人质量，g为重力加速度。

本发明提供的四足机器人落足与机体协调控制方法，通过获取目标区域内的高程信息，并根据所述高程信息计算平衡落足点，分别计算新落足和当前支撑足的运动学参数，并根据新落足和当前支撑足的运动学参数计算当前位姿下的压力中心；计算所述平衡落足点的平面位置，根据所述平衡落足点的平面位置和所述当前支撑足的运动学参数计算稳定压力中心；从而利用落足点和相关的机体稳定的融合信息，在保证安全落足的同时，机器人身体进行协调适应。

进一步地，本发明所提供的四足机器人落足与机体协调控制方法通过基于所述压力中心与稳定压力中心之间的偏移量构建机身控制模型，并通过所述机身控制模型计算足部作用力；本发明通过将压力中心结合进控制算法，并计算机体的反馈控制信息，从而提升了机体的稳定性，使得本发明可适用于不同的步态进行大尺寸越障。进而以所述足部作用力作为输入，构建协调控制模型，并通过所述协调控制模型计算当前位姿下的关节运动参数，以使足式机器人的控制装置根据所述关节运动参数输出控制指令，以控制所述足式机器人的腿部关节按照得到的关节运动参数完成相应的动作，从而使得机器人能够安全通过大尺寸障碍物，提高了机器人复杂地形的通过性能。

综上所述，本发明所提供的四足机器人落足与机体协调控制方法解决了现有技术中机器人跨越大尺寸障碍物时稳定性较差的问题，从而提高了足式机器人在跨越障碍物时的落足稳定性，进而提高了足式机器人在复杂地形的通过性能。

附图说明

为了更清楚地说明本发明或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本发明提供的四足机器人落足与机体协调控制方法的流程示意图之一；

图2是本发明提供的四足机器人落足与机体协调控制方法的流程示意图之二；

图3是本发明提供的四足机器人落足与机体协调控制方法的流程示意图之三；

图4是本发明提供的四足机器人落足与机体协调控制方法的力学原理图之一；

图5是本发明提供的四足机器人落足与机体协调控制方法的力学原理图之二；

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将结合本发明中的附图，对本发明中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

下面结合图1-图5描述本发明的四足机器人落足与机体协调控制方法。

在一种具体实施方式中，如图1所示，本发明提供的一种四足机器人落足与机体协调控制方法包括以下步骤：

S110：获取目标区域内的高程信息，并根据所述高程信息计算平衡落足点；该目标区域是指落足周围一定范围内的区域，这个范围可以根据实际应用要求确定，例如周围5-10cm的范围内，也就是说，基于落足区域附近的高程信息来计算安全的平面落足点位置。

S120：分别计算新落足和当前支撑足的运动学参数，并根据新落足和当前支撑足的运动学参数计算当前位姿下的压力中心。也就是说，根据新的落足点计算压力中心点，新的落足点是指下一位姿即将落地的行走足，当前支撑足是指当前位姿处于支撑状态的行走足。应当理解的是，由于足式机器人是多个行走足轮流作为支撑足和落足的，因此，新落足可以是任意一个行走足，支撑足也可以是任意一个行走足，并不特指与某一个行走足，不应理解为对某个行走足的特殊限定。

S130：计算所述平衡落足点的平面位置，根据所述平衡落足点的平面位置和所述当前支撑足的运动学参数计算稳定压力中心；其中，运动学参数可以包括竖直方向力、腿部正向运动学参数、关节角度向量以及足端位置等与行走足的运动有关的参数，平面位置可以根据在平面上的x方向、y方向的位置确定，应当理解的是，x、y方向为平面直角坐标系的x轴和y轴方向。

S140：基于所述压力中心与稳定压力中心之间的偏移量，根据压力中心点的变化构建机身控制模型，并通过所述机身控制模型计算足部作用力；应当理解的是，该机身控制模型具体为数学模型，通过求解数学模型计算足部作用力。

S150：以所述足部作用力作为输入，构建协调控制模型，并通过所述协调控制模型计算当前位姿下的关节运动参数，以使足式机器人的控制装置根据所述关节运动参数输出控制指令，所述控制指令用于控制所述足式机器人的腿部关节动作。该协调控制模型具体为数学模型，通过求解数学模型计算关节运动参数，机器人的控制装置根据得到的关节运动参数生成相应的控制指令，以便控制机器人的行走足完成所需的动作。其中，关节运动参数可以包括关节角度向量、关节角速度向量以及足端期望力等。

当安全的落足点超出稳定的控制范围时，即机器人在大跨步越障时，机器人的动态稳定性将大大降低，甚至会发生倾覆的危险。为了克服这一缺陷，本发明实施例充分考虑安全的落足点与机体的信息，针对安全的地形区域，在保证安全落足的同时，机器人身体进行协调控制，既保证机器人在跨越大尺寸障碍物时能够安全通过障碍物，又提高机器人跨越大尺寸障碍物的动态稳定性与通过效率。

在一些实施例中，如图2所示，获取目标区域内的高程信息，并根据所述高程信息计算平衡落足点，具体包括以下步骤：

S210：获取目标区域内的高程信息，并基于所述高程信息生成高程图；具体地，可以利用机载的感知传感器建立落足区域附近高程信息。

S220：根据所述高程信息计算目标区域内的高程标准差；

S230：在所述高程标准差小于预设的标准差阈值、且所述高程信息大于预设的高程阈值的情况下，则计算平衡落足点。

在一个具体使用场景中，根据所述高程信息计算平衡落足点时，利用机载的感知传感器建立落足区域附近高程信息x，而后根据高程信息计算安全的平面落足点位置。其中，在计算平面落足点位置时包括以下步骤：

S11：计算地形的起伏程度，该起伏程度以地形的高程标准差表示：

式中，σ为高程标准差，N为高程网格的数量，x_i为第i个网格的高程值，为N个网格的高程平均值；

S12：设定地形起伏信息阈值σ^*，地形的高程阈值x^*，当前起伏信息σ小于设定阈值σ^*，且x>x^*时，认为为落足安全区域，记录安全落足区域的高程值x_σ以及网格数量c_x、c_y；

S13：根据局部地图的原点O、高程信息x_σ及地图网格尺寸Δc，计算安全的落足点p_σ：

p_σ＝[O_x+c_xΔc,O_y+c_yΔc,x_σ]^T

其中，O_x、O_y为局部地图原点O的元素；

S14：计算安全的平面落足点位置p_σx、p_σy：

p_σx＝O_x+c_xΔc

p_σy＝O_y+c_yΔc

在步骤S120中，基于所述目标落足点位置，分别计算新落足和当前支撑足的运动学参数，并根据新落足和当前支撑足的运动学参数计算当前位姿下的压力中心，如图3所示，具体包括以下步骤：

S310：利用机器人腿部运动学算法和采集的惯性(IMU)数据，计算当前支撑足的竖直方向力和平面位置，以及新落足的竖直方向力和平面位置；

S320：根据当前支撑足的竖直方向力和平面位置，以及新落足的竖直方向力和平面位置，计算得到当前位姿下的压力中心。也就是说，根据当前支撑足和新落足的竖直方向力和平面位置，计算新的压力中心。

在一个具体使用场景中，如图4所示，新落足和当前支撑足的运动学参数计算当前位姿下的压力中心，具体包括以下步骤：

S21：利用腿部运动学与IMU信息，计算当前支撑足的平面位置p_ix、p_iy：

p_i＝R×FK(θ_i)

式中，FK为腿部的正向运动学，θ_i为第i个支撑腿的关节角度向量，p_i为第i个支撑腿的足端位置，p_ix、p_iy为p_i向量的前两个元素。

S22：根据当前支撑足和新落足的竖直方向力和平面位置，计算新的压力中心：

式中，F_zi为第i个支撑足的竖直方向力，F_zσj为第j个新落足的竖直方向力，p_σxj、p_σyj为第j个新落足的x、y方向位置，p_ix、p_iy为第i个当前支撑足的x、y方向位置，p′_{cop_x}、p′_{cop_y}为压力中心的x、y方向位置，n_σ为新落足数量，n_c为当前支撑足数量。

在步骤S130中，计算所述平衡落足点的平面位置，根据所述平衡落足点的平面位置和所述当前支撑足的运动学参数计算稳定压力中心，具体包括：

建立目标优化函数；

设定约束条件，通过求解满足约束条件的所述目标优化函数，得到目标落足点的期望竖直方向力以及当前支撑足的期望竖直方向力；

根据所述目标落足点的期望竖直方向力以及当前支撑足的期望竖直方向力，计算稳定压力中心。

具体地，如图5所示，在计算稳定的压力中心p_{cop_x}、p_{cop_y}时，首先建立机身虚拟伺服控制器(即机身控制模型)，通过机身虚拟伺服控制器计算机体虚拟合力矩矢量和虚拟合力矢量，并通过目标优化方法分配到足部，虚拟伺服控制器的表达式为：

式中，M_m、F_m为机体虚拟合力矩矢量、虚拟合力矢量；k_p为正定增益矩阵；q为机身实际的位姿矢量；q_d为机身期望的位姿矢量；k_v为正定微分系数矩阵；为机身实际的位姿速度矢量，/>为机身期望的位姿速度矢量，k_ff为正定速度前馈系数矩阵，m为机器人质量，g为重力加速度。

进一步地，通过设定约束条件，建立目标优化函数F(x)，求解满足约束条件，且使目标函数获得最小值的一组解，目标优化函数如下：

min F(x)＝(Ax-b)^TS(Ax-b)+αx^TWx+β‖x-x^*‖

其中，

式中，S为加权矩阵,W为半正定对称矩阵,α、β为整定因子.x^*为上一时刻的优化解,r_i为第i个足部相对机身质心在世界坐标系下的向量，包含平衡落足点以及当前支撑点。

上述设定的约束条件具体为：

上式中，为第i接触点的法向力；/>为第i接触点的切向力；μ为滑动摩擦系数。

通过优化求解，求解出包含平衡落足点j的期望竖直方向力F_zsj以及当前支撑足i的期望竖直方向力F_zdi。

式中，F_zsj为第j个平衡落足的竖直方向期望力，F_zdi为第i个支撑足的竖直方向期望力，p_sxj、p_syj为第j个新落足的x、y方向位置，n_s为平衡落足的数量。

平衡落足点的计算如下：

式中，T_s为支撑周期，z₀为机器人高度，v_x、v_y为机器人实际的前向速度、侧向速度，v_xd、v_yd为机器人期望的前向速度、侧向速度。

在步骤S140中，计算足部作用力时，将压力中心产生的偏移量作为输入前述机身虚拟伺服控制器，并通过目标优化方法分配到足部，前述机身虚拟伺服控制器为数学模型，具体为：

其中，

上式中，q′_d为机身新的期望位姿矢量，为机身新的期望位姿速度矢量，k_{cop_x}、k_{cop_y}为整定系数，/>分别为q_d的位姿元素，/>分别为/>的位姿速度元素，/>为x、y方向位姿的变化量。

上式的sign和min函数表示：

其中，a,b表示函数里的参数。

这样，利用以上数学模型通过优化求解方法，即可求解新的足部作用力。

在步骤S150中，以新的足部作用力作为输入，建立作为关节控制器的协调控制模型，该协调控制模型的表达式为：

式中：K_pθ、K_vθ为控制器刚度、阻尼系数矩阵；θ_d为期望的关节角度向量；θ为实际的关节角度向量；为期望的关节角速度向量；/>为实际的关节角速度向量；τ_ff＝-J^TF_d为关节力前馈项；F_d为足端期望力；J为关节力雅可比矩阵；u为关节控制器输入。

在上述具体实施方式中，本发明提供的一种四足机器人落足与机体协调控制方法，通过获取目标区域内的高程信息，并根据所述高程信息计算平衡落足点，分别计算新落足和当前支撑足的运动学参数，并根据新落足和当前支撑足的运动学参数计算当前位姿下的压力中心；计算所述平衡落足点的平面位置，根据所述平衡落足点的平面位置和所述当前支撑足的运动学参数计算稳定压力中心；从而利用落足点和相关的机体稳定的融合信息，在保证安全落足的同时，机器人身体进行协调适应。

进一步地，本发明所提供的一种四足机器人落足与机体协调控制方法通过基于所述压力中心与稳定压力中心之间的偏移量构建机身控制模型，并通过所述机身控制模型计算足部作用力；本发明通过将压力中心结合进控制算法，并计算机体的反馈控制信息，从而提升了机体的稳定性，使得本发明可适用于不同的步态进行大尺寸越障。进而以所述足部作用力作为输入，构建协调控制模型，并通过所述协调控制模型计算当前位姿下的关节运动参数，以使足式机器人的控制装置根据所述关节运动参数输出控制指令，以控制所述足式机器人的足部关节按照得到的关节运动参数完成相应的动作，从而使得机器人能够安全通过大尺寸障碍物，提高了机器人复杂地形的通过性能。

综上所述，本发明所提供的一种四足机器人落足与机体协调控制方法解决了现有技术中机器人跨越大尺寸障碍物时稳定性较差的问题，从而提高了足式机器人在跨越障碍物时的落足稳定性，进而提高了足式机器人在复杂地形的通过性能。

最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (7)

1.一种四足机器人落足与机体协调控制方法，其特征在于，包括：

获取目标区域内的高程信息，并根据所述高程信息计算平衡落足点；

分别计算新落足和当前支撑足的运动学参数，并根据新落足和当前支撑足的运动学参数计算当前位姿下的压力中心；

计算所述平衡落足点的平面位置，根据所述平衡落足点的平面位置和所述当前支撑足的运动学参数计算稳定压力中心；

基于所述压力中心与稳定压力中心之间的偏移量构建机身控制模型，并通过所述机身控制模型计算足部作用力；

以所述足部作用力作为输入，构建协调控制模型，并通过所述协调控制模型计算当前位姿下的关节运动参数，以使足式机器人的控制装置根据所述关节运动参数输出控制指令，所述控制指令用于控制所述足式机器人的腿部关节动作。

2.根据权利要求1所述的四足机器人落足与机体协调控制方法，其特征在于，获取目标区域内的高程信息，并根据所述高程信息计算平衡落足点，具体包括：

获取目标区域内的高程信息，并基于所述高程信息生成高程图；

根据所述高程信息计算目标区域内的高程标准差；

在所述高程标准差小于预设的标准差阈值、且所述高程信息大于预设的高程阈值的情况下，则计算平衡落足点。

3.根据权利要求1所述的四足机器人落足与机体协调控制方法，其特征在于，基于所述目标落足点位置，分别计算新落足和当前支撑足的运动学参数，并根据新落足和当前支撑足的运动学参数计算当前位姿下的压力中心，具体包括：

利用机器人腿部运动学算法和采集的惯性数据，计算当前支撑足的竖直方向力和平面位置，以及新落足的竖直方向力和平面位置；

根据当前支撑足的竖直方向力和平面位置，以及新落足的竖直方向力和平面位置，计算得到当前位姿下的压力中心。

4.根据权利要求3所述的四足机器人落足与机体协调控制方法，其特征在于，利用第一表达式和第二表达式，计算当前位姿下的压力中心的x、y方向位置；

所述第一表达式为：

所示第二表达式为：

式中，F_zi为第i个支撑足的竖直方向力，F_zσj为第j个新落足的竖直方向力，p_σxj、p_σyj为第j个新落足的x、y方向位置，p_ix、p_iy为第i个当前支撑足的x、y方向位置，p^′ _{cop_x}、p^′ _{cop_y}为压力中心的x、y方向位置，n_σ为新落足数量，n_c为当前支撑足数量。

5.根据权利要求1所述的四足机器人落足与机体协调控制方法，其特征在于，计算所述平衡落足点的平面位置，根据所述平衡落足点的平面位置和所述当前支撑足的运动学参数计算稳定压力中心，具体包括：

建立目标优化函数；

设定约束条件，通过求解满足约束条件的所述目标优化函数，得到目标落足点的期望竖直方向力以及当前支撑足的期望竖直方向力；

根据所述目标落足点的期望竖直方向力以及当前支撑足的期望竖直方向力，计算稳定压力中心。

6.根据权利要求5所述的四足机器人落足与机体协调控制方法，其特征在于，所述目标优化函数为：

min F(x)＝(Ax-b)^TS(Ax-b)+αx^TWx+β‖x-x^*‖

其中，

式中，M_m、F_m为机体虚拟合力矩矢量、虚拟合力矢量；m为机器人质量；S为加权矩阵，W为半正定对称矩阵，α、β为整定因子；x^*为上一时刻的优化解；r_i为第i个足部相对机身质心在世界坐标系下的向量，包含平衡落足点以及当前支撑点。

7.根据权利要求6所述的四足机器人落足与机体协调控制方法，其特征在于，利用第三表达式计算机体虚拟合力矩矢量和虚拟合力矢量，所述第三表达式为：

式中，M_m、F_m为机体虚拟合力矩矢量、虚拟合力矢量；k_p为正定增益矩阵；q为机身实际的位姿矢量；q_d为机身期望的位姿矢量；k_v为正定微分系数矩阵；为机身实际的位姿速度矢量，/>为机身期望的位姿速度矢量，k_ff为正定速度前馈系数矩阵，m为机器人质量，g为重力加速度。