CN114734445A - 一种四足机器人动态适应负载的控制方法 - Google Patents

Abstract

本发明属于机器人运动控制技术领域，具体涉及一种四足机器人动态适应负载的控制方法。该方法包括：针对机器人的负载参数进行估计，所述机器人的负载参数包括机器人的机身质量及机身质心位置；基于估计获取的机器人的负载参数，建立机器人的虚拟伺服力与力矩；将虚拟伺服力与力矩优化分配至各支撑腿，并通过关节映射实现负载的动态适应。本发明通过建立机器人负载自适应稳定控制器实现对外界负载的抗扰动，从而提升机器人在复杂路面下的通过性能。

Description

一种四足机器人动态适应负载的控制方法

技术领域

本发明属于机器人运动控制技术领域，具体涉及一种四足机器人动态适应负载的控制方法。

背景技术

四足机器人相比于传统的轮式、履带式车辆，可通过选择离散的摆动腿落足点，因此可适应高原、山地等复杂的地形，目前，四足机器人主要针对已知负载做一些算法上的控制，当机器人需要搭载外界未知负载时：1)重新采用第三方测量工具进行标定；2)依靠自身的控制器鲁棒性进行负载适应。前者可标定出机器人的负载参数，进而输入至动态适应控制器中，但是效率较低；后者针对大的负载，且负载分配不均时，超出机器人的调整范围，机器人将变得不稳定，甚至发生倾覆的危险。部分高校及研究机构虽开展了机器人负载参数自适应整定方面的研究，一方面未考虑机器人腿足连杆质量的影响，另一方面针对如何进一步适应负载的控制研究较为欠缺。

发明内容

(一)要解决的技术问题

本发明要解决的技术问题是：如何提供一种四足机器人动态适应负载的控制方法。

(二)技术方案

为解决上述技术问题，本发明提供一种四足机器人动态适应负载的控制方法，所述方法包括如下步骤：

步骤1：针对机器人的负载参数进行估计，所述机器人的负载参数包括机器人的机身质量及机身质心位置；

步骤2：基于估计获取的机器人的负载参数，建立机器人的虚拟伺服力与力矩；

步骤3：将虚拟伺服力与力矩优化分配至各支撑腿，并通过关节映射实现负载的动态适应。

其中，所述步骤1包括：

步骤11：在机器人的机身原点处建立动力学方程；

步骤12：在机器人站立平衡条件下，建立力平衡方程，估计机器人的机身质量；

步骤13：在机器人站立平衡条件下，建立力矩平衡方程，采用最小二乘法估计机器人的机身质心位置。

其中，所述步骤2中，包括：

步骤21：根据估计的负载参数建立机器人高度方向的虚拟伺服力；

步骤22：根据估计的机身质心位置建立机器人转动方向的虚拟伺服力矩。

其中，所述步骤3中，将虚拟伺服力与力矩优化分配至各支撑腿，并通过关节映射实现负载的动态适应；具体包括：

步骤31：建立虚拟伺服力/力矩与足端力之间的映射关系；

步骤32：建立虚拟伺服力/力矩力分配的目标优化函数，求解满足约束的一组最优解；

步骤33：建立底层柔顺伺服控制器。

其中，所述步骤11中，以机器人为研究对象，在机身原点处建立动力学方程，具体如下：

其中，

式中，F_i为第i条腿的地面作用力矢量，G_i,j为第i条腿第j个关节连杆的重力矢量，m_i,j为第i条腿第j个关节连杆的质量，I_i,j为第i条腿第j个关节连杆的转动惯量，

为第i条腿第j个关节连杆质心的平动加速度，

为第i条腿第j个关节的角加速度，G_t为含负载的机身重力矢量，m_t为含负载的机身质量，I_t为含负载的机身转动惯量，

为机身姿态角加速度，

为含负载的机身平动加速度，F_G为作用在机身原点的合力矢量，M_G为作用在机身原点的合力矩矢量，r_i为足相对机身原点的位置矢量，r_{b_com}为机身质心相对机身原点的位置矢量，r_{i,j_com}为腿足各关节连杆质心相对机身原点的位置矢量。

其中，所述步骤12中，在机器人站立平衡条件下，合力为零，即F_G＝0，估计负载质量以及含负载的机身质量，得到：

m_L＝G_{t_z}/g-m_b

G_{t_z}/g＝m_L+m_b

式中，G_{t_z}为G_t的z元素，g＝-9.812为重力加速度，m_b为不含负载的机身质量，m_L为机身质量，G_{t_z}/g为含负载的机身质量。

其中，所述步骤13中，在机器人站立平衡条件下，合力矩为零，即M_G＝0，机器人的机身质心位置表达如下：

式中，R为机身姿态变换矩阵，r_{b_com_b}为含负载机身质心相对机身原点的位置矢量；

选取N组不同姿态下的站立平衡状态，采用最小二乘法求解r_{b_com_b}，计算如下：

其中，所述步骤21中，根据估计的负载参数建立机器人虚拟伺服力，机身在平动方向被简化为具有弹簧和阻尼的虚拟元件，在相同的阻尼比和自然频率下，平动刚度、阻尼与质量成正比，将负载重力作为前馈，考虑了负载的虚拟力建立如下：

式中，F_m为机体虚拟力矢量；k_pp为位置正定增益矩阵；p为机身实际的位置矢量；p_d为机身期望的位置矢量；k_pd为位置正定微分系数矩阵；

为机身实际的平动速度矢量，

为机身期望的平动速度矢量。

其中，所述步骤22中，根据估计的机身质心位置建立机器人虚拟伺服力矩，机身在转动方向被简化为具有弹簧和阻尼的虚拟元件，在相同的阻尼比和自然频率下，旋转刚度、阻尼与转动惯量成正比，将负载重力产生的力矩作为前馈，考虑了负载的虚拟力矩建立如下：

其中，

式中，M_m为机体虚拟力矩矢量；

为姿态正定增益矩阵；

为机身实际的姿态矢量；

为机身期望的姿态矢量；

为姿态正定微分系数矩阵；

为机身实际的转动速度矢量，

为机身期望的转动速度矢量，

为不含负载的机身旋转刚度、阻尼系数矩阵。

其中，所述步骤31中，建立虚拟伺服力/力矩与足端力之间的映射关系，建立如下：

所述步骤32中，建立虚拟伺服力/力矩力分配的目标优化函数，求解满足约束的一组最优解，目标优化函数如下：

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

其中，

式中，S为加权矩阵,W为半正定对称矩阵,α、β为整定因子.x^*为上一时刻的优化解；

约束条件如下：

上式中，

为第i接触点的法向力；

为第i接触点的切向力；μ为滑动摩擦系数；

所述步骤33中，以分配足部的力作为输入，建立关节柔顺伺服控制器，设计如下所示：

式中：K_pθ、K_vθ为控制器刚度、阻尼系数矩阵；θ_d为期望的关节角度向量；θ为实际的关节角度向量；

为期望的关节角速度向量；

为实际的关节角速度向量；τ_ff＝-J^TF_d为关节力前馈项；F_d为足端期望力；J为关节力雅可比矩阵；u为关节柔顺伺服控制器输入。

(三)有益效果

本发明充分考虑机器人各连杆的质量影响，通过建立机器人动态适应负载的控制器实现对外界负载的自适应，提高四足机器人可变负载下的通过性，通过负载参数估计进一步建立负载适应控制器，从而实现机器人对未知负载的动态稳定适应。

与现有技术相比较，本发明具备如下有益效果：

(1)本发明采用地面力信息及机器人已知的物理参数能够精确估计机器人机身的质量与质心位置。

(2)本发明根据估计的负载信息，建立虚拟伺服力/力矩前馈输入，能够有效提升机器人对负载的动态响应性能。

(3)本发明采用虚拟伺服与底层柔顺控制方法，可提高机器人可变负载下的稳定性与柔顺性。

(4)本发明可快速实现对外界负载的自适应，提高四足机器人行走效率。

附图说明

图1为四足机器人动态适应负载方法实现的步骤流程图；

图2是负载参数估计示意图；

图3是四足机器人负载动态适应控制器。

具体实施方式

为使本发明的目的、内容、和优点更加清楚，下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。

为解决上述技术问题，本发明提供一种四足机器人动态适应负载的控制方法，如图1所示，所述方法包括如下步骤：

步骤1：针对机器人的负载参数进行估计，所述机器人的负载参数包括机器人的机身质量及机身质心位置；

步骤2：基于估计获取的机器人的负载参数，建立机器人的虚拟伺服力与力矩；

步骤3：将虚拟伺服力与力矩优化分配至各支撑腿，并通过关节映射实现负载的动态适应。

其中，所述步骤1包括：

步骤11：在机器人的机身原点处建立动力学方程；

步骤12：在机器人站立平衡条件下，建立力平衡方程，估计机器人的机身质量；

步骤13：在机器人站立平衡条件下，建立力矩平衡方程，采用最小二乘法估计机器人的机身质心位置。

其中，如图3所示，所述步骤2中，包括：

步骤21：根据估计的负载参数建立机器人高度方向的虚拟伺服力；

步骤22：根据估计的机身质心位置建立机器人转动方向的虚拟伺服力矩。

其中，如图3所示，所述步骤3中，将虚拟伺服力与力矩优化分配至各支撑腿，并通过关节映射实现负载的动态适应；具体包括：

步骤31：建立虚拟伺服力/力矩与足端力之间的映射关系；

步骤32：建立虚拟伺服力/力矩力分配的目标优化函数，求解满足约束的一组最优解；

步骤33：建立底层柔顺伺服控制器。

其中，所述步骤11中，如图2所示，以机器人为研究对象，在机身原点处建立动力学方程，具体如下：

其中，

式中，F_i为第i条腿的地面作用力矢量，G_i,j为第i条腿第j个关节连杆的重力矢量，m_i,j为第i条腿第j个关节连杆的质量，I_i,j为第i条腿第j个关节连杆的转动惯量，

为第i条腿第j个关节连杆质心的平动加速度，

为第i条腿第j个关节的角加速度，G_t为含负载的机身重力矢量，m_t为含负载的机身质量，I_t为含负载的机身转动惯量，

为机身姿态角加速度，

为含负载的机身平动加速度，F_G为作用在机身原点的合力矢量，M_G为作用在机身原点的合力矩矢量，r_i为足相对机身原点的位置矢量，r_{b_com}为机身质心相对机身原点的位置矢量，r_{i,j_com}为腿足各关节连杆质心相对机身原点的位置矢量。

其中，所述步骤12中，在机器人站立平衡条件下，合力为零，即F_G＝0，估计负载质量以及含负载的机身质量，得到：

m_L＝G_{t_z}/g-m_b

G_{t_z}/g＝m_L+m_b

式中，G_{t_z}为G_t的z元素，g＝-9.812为重力加速度，m_b为不含负载的机身质量，m_L为机身质量，G_{t_z}/g为含负载的机身质量。

其中，所述步骤13中，在机器人站立平衡条件下，合力矩为零，即M_G＝0，机器人的机身质心位置表达如下：

式中，R为机身姿态变换矩阵，r_{b_com_b}为含负载机身质心相对机身原点的位置矢量；

选取N组不同姿态下的站立平衡状态，采用最小二乘法求解r_{b_com_b}，计算如下：

其中，所述步骤21中，根据估计的负载参数建立机器人虚拟伺服力，机身在平动方向被简化为具有弹簧和阻尼的虚拟元件，在相同的阻尼比和自然频率下，平动刚度、阻尼与质量成正比，将负载重力作为前馈，考虑了负载的虚拟力建立如下：

式中，F_m为机体虚拟力矢量；k_pp为位置正定增益矩阵；p为机身实际的位置矢量；p_d为机身期望的位置矢量；k_pd为位置正定微分系数矩阵；

为机身实际的平动速度矢量，

为机身期望的平动速度矢量。

其中，所述步骤22中，根据估计的机身质心位置建立机器人虚拟伺服力矩，机身在转动方向被简化为具有弹簧和阻尼的虚拟元件，在相同的阻尼比和自然频率下，旋转刚度、阻尼与转动惯量成正比，将负载重力产生的力矩作为前馈，考虑了负载的虚拟力矩建立如下：

其中，

式中，M_m为机体虚拟力矩矢量；

为姿态正定增益矩阵；

为机身实际的姿态矢量；

为机身期望的姿态矢量；

为姿态正定微分系数矩阵；

为机身实际的转动速度矢量，

为机身期望的转动速度矢量，

为不含负载的机身旋转刚度、阻尼系数矩阵。

其中，所述步骤31中，建立虚拟伺服力/力矩与足端力之间的映射关系，建立如下：

所述步骤32中，建立虚拟伺服力/力矩力分配的目标优化函数，求解满足约束的一组最优解，目标优化函数如下：

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

其中，

式中，S为加权矩阵,W为半正定对称矩阵,α、β为整定因子.x^*为上一时刻的优化解；

约束条件如下：

上式中，

为第i接触点的法向力；

为第i接触点的切向力；μ为滑动摩擦系数；

所述步骤33中，以分配足部的力作为输入，建立关节柔顺伺服控制器，设计如下所示：

式中：K_pθ、K_vθ为控制器刚度、阻尼系数矩阵；θ_d为期望的关节角度向量；θ为实际的关节角度向量；

为期望的关节角速度向量；

为实际的关节角速度向量；τ_ff＝-J^TF_d为关节力前馈项；F_d为足端期望力；J为关节力雅可比矩阵；u为关节柔顺伺服控制器输入。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。

Claims (10)

1.一种四足机器人动态适应负载的控制方法，其特征在于，所述方法包括如下步骤：

步骤1：针对机器人的负载参数进行估计，所述机器人的负载参数包括机器人的机身质量及机身质心位置；

步骤2：基于估计获取的机器人的负载参数，建立机器人的虚拟伺服力与力矩；

步骤3：将虚拟伺服力与力矩优化分配至各支撑腿，并通过关节映射实现负载的动态适应。

2.如权利要求1所述的四足机器人动态适应负载的控制方法，其特征在于，所述步骤1包括：

步骤11：在机器人的机身原点处建立动力学方程；

步骤12：在机器人站立平衡条件下，建立力平衡方程，估计机器人的机身质量；

步骤13：在机器人站立平衡条件下，建立力矩平衡方程，采用最小二乘法估计机器人的机身质心位置。

3.如权利要求2所述的四足机器人动态适应负载的控制方法，其特征在于，所述步骤2中，包括：

步骤21：根据估计的负载参数建立机器人高度方向的虚拟伺服力；

步骤22：根据估计的机身质心位置建立机器人转动方向的虚拟伺服力矩。

4.如权利要求3所述的四足机器人动态适应负载的控制方法，其特征在于，所述步骤3中，将虚拟伺服力与力矩优化分配至各支撑腿，并通过关节映射实现负载的动态适应；具体包括：

步骤31：建立虚拟伺服力/力矩与足端力之间的映射关系；

步骤32：建立虚拟伺服力/力矩力分配的目标优化函数，求解满足约束的一组最优解；

步骤33：建立底层柔顺伺服控制器。

5.如权利要求4所述的四足机器人动态适应负载的控制方法，其特征在于，所述步骤11中，以机器人为研究对象，在机身原点处建立动力学方程，具体如下：

其中，

式中，F_i为第i条腿的地面作用力矢量，G_i,j为第i条腿第j个关节连杆的重力矢量，m_i,j为第i条腿第j个关节连杆的质量，I_i,j为第i条腿第j个关节连杆的转动惯量，

为第i条腿第j个关节连杆质心的平动加速度，

为第i条腿第j个关节的角加速度，G_t为含负载的机身重力矢量，m_t为含负载的机身质量，I_t为含负载的机身转动惯量，

为机身姿态角加速度，

为含负载的机身平动加速度，F_G为作用在机身原点的合力矢量，M_G为作用在机身原点的合力矩矢量，r_i为足相对机身原点的位置矢量，r_{b_com}为机身质心相对机身原点的位置矢量，r_{i,j_com}为腿足各关节连杆质心相对机身原点的位置矢量。

6.如权利要求5所述的四足机器人动态适应负载的控制方法，其特征在于，所述步骤12中，在机器人站立平衡条件下，合力为零，即F_G＝0，估计负载质量以及含负载的机身质量，得到：

m_L＝G_{t_z}/g-m_b

G_{t_z}/g＝m_L+m_b

式中，G_{t_z}为G_t的z元素，g＝-9.812为重力加速度，m_b为不含负载的机身质量，m_L为机身质量，G_{t_z}/g为含负载的机身质量。

7.如权利要求6所述的四足机器人动态适应负载的控制方法，其特征在于，所述步骤13中，在机器人站立平衡条件下，合力矩为零，即M_G＝0，机器人的机身质心位置表达如下：

式中，R为机身姿态变换矩阵，r_{b_com_b}为含负载机身质心相对机身原点的位置矢量；

选取N组不同姿态下的站立平衡状态，采用最小二乘法求解r_{b_com_b}，计算如下：

8.如权利要求7所述的四足机器人动态适应负载的控制方法，其特征在于，所述步骤21中，根据估计的负载参数建立机器人虚拟伺服力，机身在平动方向被简化为具有弹簧和阻尼的虚拟元件，在相同的阻尼比和自然频率下，平动刚度、阻尼与质量成正比，将负载重力作为前馈，考虑了负载的虚拟力建立如下：

式中，F_m为机体虚拟力矢量；k_pp为位置正定增益矩阵；p为机身实际的位置矢量；p_d为机身期望的位置矢量；k_pd为位置正定微分系数矩阵；

为机身实际的平动速度矢量，

为机身期望的平动速度矢量。

9.如权利要求8所述的四足机器人动态适应负载的控制方法，其特征在于，所述步骤22中，根据估计的机身质心位置建立机器人虚拟伺服力矩，机身在转动方向被简化为具有弹簧和阻尼的虚拟元件，在相同的阻尼比和自然频率下，旋转刚度、阻尼与转动惯量成正比，将负载重力产生的力矩作为前馈，考虑了负载的虚拟力矩建立如下：

其中，

式中，M_m为机体虚拟力矩矢量；

为姿态正定增益矩阵；

为机身实际的姿态矢量；

为机身期望的姿态矢量；

为姿态正定微分系数矩阵；

为机身实际的转动速度矢量，

为机身期望的转动速度矢量，

为不含负载的机身旋转刚度、阻尼系数矩阵。

10.如权利要求9所述的四足机器人动态适应负载的控制方法，其特征在于，所述步骤31中，建立虚拟伺服力/力矩与足端力之间的映射关系，建立如下：

所述步骤32中，建立虚拟伺服力/力矩力分配的目标优化函数，求解满足约束的一组最优解，目标优化函数如下：

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

其中，

式中，S为加权矩阵,W为半正定对称矩阵,α、β为整定因子.x^*为上一时刻的优化解；

约束条件如下：

上式中，

为第i接触点的法向力；

为第i接触点的切向力；μ为滑动摩擦系数；

所述步骤33中，以分配足部的力作为输入，建立关节柔顺伺服控制器，设计如下所示：

式中：K_pθ、K_vθ为控制器刚度、阻尼系数矩阵；θ_d为期望的关节角度向量；θ为实际的关节角度向量；

为期望的关节角速度向量；

为实际的关节角速度向量；τ_ff＝-J^TF_d为关节力前馈项；F_d为足端期望力；J为关节力雅可比矩阵；u为关节柔顺伺服控制器输入。

Images