CN112631277B - 一种四足机器人站姿转换的平衡控制方法及系统 - Google Patents

Abstract

本发明公开了一种四足机器人站姿转换的平衡控制方法及系统，该方法包括：基于三阶倒立摆新构型构建动力学模型；构建状态空间方程；构建四足机器人控制器；获取四足机器人实际肩关节、髋关节和踝关节的角度和角速度；结合肩关节和髋关节的期望角度得到肩关节、髋关节、踝关节的反馈角度和反馈角速度；得到对应的实际控制力矩；更新肩关节和髋关节的期望角度，实现站姿转换。该系统包括：动力学方程模块、状态空间方程模块、控制器模块、实际姿态模块、反馈模块、实际控制力矩模块和姿态更新模块。本发明只需控制较少的关节即可实现整套动作的控制。本发明作为一种四足机器人站姿转换的平衡控制方法及系统，可广泛应用于机器人控制领域。

Description

一种四足机器人站姿转换的平衡控制方法及系统

技术领域

本发明属于机器人控制领域，尤其涉及一种四足机器人站姿转换的平衡控制方法及系统。

背景技术

四足机器人可以很好地适应非结构化的恶劣环境，可满足军事侦察、资源探勘以及抢险救灾等多种用途需求。但其在非结构化作业环境下，有时候需要根据不同的地形，切换不同的站姿步态。然而，四足机器人大多是点足接触，接触面积小，在站立姿态转换的过程保持平衡较为困难，目前对于四足机器人的站姿平衡控制方法，多是涉及机器人在不同的行进步态下的四足平衡状态，很少涉及站姿转换过程中(四足到双足的转换)的平衡控制。现有的对于四足机器人的站姿转换方法，结合了传统的坐标转换和ZMP(零力矩点法)，但是ZMP在计算的时候局限于平面接触面，当机器人在松软、崎岖的地形工作时，ZMP的运用较为有限，且机器人构型复杂，传统的坐标转换方法需要大量的计算，不利于下一步的控制器设计。

发明内容

为了解决上述技术问题，本发明的目的是提供一种四足机器人站姿转换的平衡控制方法及系统，通过把找到的关系路径作为特征来学习目标关系，实现知识图谱补全的方法。

本发明所采用的第一技术方案是：一种四足机器人站姿转换的平衡控制方法，包括以下步骤：

基于预设的三阶倒立摆新构型构建动力学模型；

根据动力学模型得到状态空间方程并简化，得到简化后的状态空间方程；

基于简化后的状态空间方程构建四足机器人控制器；

基于传感器获取四足机器人实际肩关节、髋关节和踝关节的角度和角速度；

将实际肩关节、髋关节和踝关节三个关节的角度和角速度结合肩关节和髋关节的期望角度输入到控制器，并得到肩关节、髋关节、踝关节的反馈角度和反馈角速度；

根据肩关节、髋关节、踝关节的反馈角度和角速度，得到对应的实际控制力矩；

根据对应的实际控制力矩和按时间更新肩关节和髋关节的期望角度，实现站姿转换。

进一步，所述预设的三阶倒立摆新构型包括模拟四足机器人前肢的上摆、模拟躯干的中摆和模拟后肢的下摆。

进一步，所述基于预设的三阶倒立摆新构型构建动力学模型并得到状态空间方程这一步骤，其具体包括：

根据预设的三阶倒立摆新构型的上摆的受力情况和肩关节处的力矩平衡方程，得到上摆动力学模型；

根据预设的三阶倒立摆新构型的中摆的力学方程和髋关节处的力矩平衡方程，得到中摆动力学模型；

根据预设的三阶倒立摆新构型的下摆的力学方程，得到下摆动力学模块。

进一步，所述上摆动力学模型表达式如下：

上式中，M_m表示中摆的质量，J_m表示中摆的理想转动惯量，θ_m表示髋关节对应的角度，F₂表示髋关节处产生的横向力，T_act2表示在髋关节处施加的主动力矩。

进一步，所述中摆动力学模型表达式如下：

上式中，M_l表示下摆的质量，J_l表示下摆的理想转动惯量，θ_l表示踝关节对应的角度。

进一步，所述下摆动力学模型表达如下：

上式中，状态变量为θ_u，θ_m，θ_l，

系统输入为T_act1和T_act2。其中

节对应的角速度，

表示髋关节对应的角速度，

表示踝关节对应的角速度，P_u1，P_u2，P_m1，P_m2，P_m3，P_m4，P_m5，P_l1，P_l2，P_l3，P_l4，P_l5表示根据上述的动力学方程联立得出的对应状态变量和输入的权值。系统输出为θ_c，表示系统组合质心相对于踝关节的夹角，K₁，K₂，K₃表示根据组合质心公式，三个角度分别对应的权值。

进一步，所述状态空间方程表达式如下：

下式中，状态变量为θ_u，θ_m，θ_l，

系统输入为T_act1和T_act2。其中

表示肩关节对应的角速度，

表示髋关节对应的角速度，

表示踝关节对应的角速度，P_u1，P_u2，P_m1，P_m2，P_m3，P_m4，P_m5，P_l1，P_l2，P_l3，P_l4，P_l5表示根据上述的动力学方程联立得出的对应状态变量和输入的权值。系统输出为θ_c，表示系统组合质心相对于踝关节的夹角，K₁，K₂，K₃表示根据组合质心公式，三个角度分别对应的权值。

进一步，所述简化后的状态空间方程表达式如下：

上式中，x₁＝[θ_u θ_m θ_l]^T为角度值，

为角速度值，u表示系统输入，K＝[K₃ K₂ K₁]，

表示角速度值，

表示角加速度值，

进一步，所述控制器的控制信号表达式如下：

上式中，K₁₁和K₁₂表示状态反馈增益，calibration_matrix为针对误差e的修正矩阵。

本发明所采用的第二技术方案是：一种四足机器人站姿转换的平衡控制方法系统，包括以下模块：

动力学方程模块，用于基于预设的三阶倒立摆新构型构建动力学模型；

状态空间方程模块，用于根据动力学模型得到状态空间方程并简化，得到简化后的状态空间方程；

控制器模块，用于基于简化后的状态空间方程构建四足机器人控制器；

实际姿态模块，用于基于传感器获取四足机器人实际肩关节、髋关节和踝关节的角度和角速度；

反馈模块，用于将实际肩关节、髋关节和踝关节三个关节的角度和角速度结合肩关节和髋关节的期望角度输入到控制器，并得到肩关节、髋关节、踝关节的反馈角度和反馈角速度；

实际控制力矩模块，用于根据肩关节、髋关节、踝关节的反馈角度和角速度，得到对应的实际控制力矩；

姿态更新模块，用于根据对应的实际控制力矩和按时间更新肩关节和髋关节的期望角度，实现站姿转换。

本发明方法及系统的有益效果是：本发明本发明将四足机器人的站姿转换过程简化为了一种特殊的三阶倒立摆模型，与传统的坐标转换方法相比，简化了动力学分析。分阶段设计控制器，可以有效地自由配置倒立摆的摆起方式，同时当倒立摆处于稳定状态时，对于外界的未知有界扰动有着很好的鲁棒性。

附图说明

图1是本发明具体实施例一种四足机器人站姿转换的平衡控制方法的步骤流程图；

图2是本发明具体实施例一种四足机器人站姿转换的平衡控制系统的结构框图；

图3是本发明具体实施例预设的三阶倒立摆新构型的示意图；

图4是本发明具体实施例站姿转换的控制流程图；

图5是本发明具体实施例预设的三阶倒立摆新构型与实际四足机器人的站姿映射图。

具体实施方式

下面结合附图和具体实施例对本发明做进一步的详细说明。对于以下实施例中的步骤编号，其仅为了便于阐述说明而设置，对步骤之间的顺序不做任何限定，实施例中的各步骤的执行顺序均可根据本领域技术人员的理解来进行适应性调整。

如图1所示，本发明提供了一种四足机器人站姿转换的平衡控制方法，该方法包括以下步骤：

S1、基于预设的三阶倒立摆新构型构建动力学模型；

S2、根据动力学模型得到状态空间方程并简化，得到简化后的状态空间方程；

S3、基于简化后的状态空间方程构建四足机器人控制器；

S4、基于传感器获取四足机器人实际肩关节、髋关节和踝关节的角度和角速度；

S5、将实际肩关节、髋关节和踝关节三个关节的角度和角速度结合肩关节和髋关节的期望角度输入到控制器，并得到肩关节、髋关节、踝关节的反馈角度和反馈角速度；

S6、根据肩关节、髋关节、踝关节的反馈角度和角速度，得到对应的实际控制力矩；

S7、根据对应的实际控制力矩和按时间更新肩关节和髋关节的期望角度，实现站姿转换。

具体地，整个实现过程分为两个部分，首先，对三阶倒立摆新构型进行运动学/动力学分析，建立数学模型；然后，根据相应的控制目的，对已建立的数学模型进行相应的控制器设计，对设计好的控制器进行性能分析。

进一步作为本方法的优选实施例，所述预设的三阶倒立摆新构型包括模拟四足机器人前肢的上摆、模拟躯干的中摆和模拟后肢的下摆。

具体地，参照图3，利用三阶倒立摆新构型来近似四足机器人站姿转换的运动过程，该模型由三个部分组成，上摆模拟四足机器人的(一对)前肢，中摆模拟躯干，下摆模拟(一对)后肢。整个倒立摆系统的输入部分安装在肩关节和髋关节(可以是电机或是液压驱动器，根据现实场景中四足机器人的具体驱动方式而定)，下摆的底部(踝关节)通过旋转关节与地面铰接。

进一步作为本方法优选实施例，所述基于预设的三阶倒立摆新构型构建动力学模型这一步骤，其具体包括：

根据预设的三阶倒立摆新构型的上摆的受力情况和肩关节处的力矩平衡方程，得到上摆动力学模型；

根据预设的三阶倒立摆新构型的中摆的力学方程和髋关节处的力矩平衡方程，得到中摆动力学模型；

根据预设的三阶倒立摆新构型的下摆的力学方程，得到下摆动力学模块。

进一步作为本发明的优选实施例，所述上摆动力学模型表达式如下：

上式中，M_u表示上摆的质量，l_u表示上摆长度，l_m表示中摆长度，l_l表示下摆长度，J_u表示上摆的理想转动惯量，θ_u表示肩关节对应的角度，F₁表示肩关节处产生的横向力，T_act1表示在肩关节处施加的主动力矩，g表示表示地球重力加速度，

表示肩关节对应的角加速度，

表示表示髋关节对应的角加速度，

表示表示踝关节对应的角加速度。

进一步作为本发明优选实施例，所述中摆动力学模型表达式如下：

上式中，M_m表示中摆的质量，J_m表示中摆的理想转动惯量，θ_m表示髋关节对应的角度，F₂表示髋关节处产生的横向力，T_act2表示在髋关节处施加的主动力矩。

进一步作为本发明优选实施例，所述下摆动力学模型表达如下：

具体地，θ_u以y_u轴负半轴的逆时针方向为正角，θ_m以y_m轴正半轴的顺时针方向为正角，θ_l以y_l轴正半轴的顺时针方向为正角。根据小角近似原理，即tanθ_c＝x_c/y_c≈θ_c。在四足机器人的站姿转换过程中，θ_m由90°变化成0°，θ_u和θ_l在经过小幅变化后，仍归于0°，在抗扰动平衡控制中，系统经过未知扰动影响后，θ_c保持稳定在0°。

整理式(1)、(2)、(3)可得状态空间方程：

上式中，状态变量为θ_u，θ_m，θ_l，

系统输入为T_act1和T_act2。其中

表示肩关节对应的角速度，

表示髋关节对应的角速度，

表示踝关节对应的角速度，P_u1，P_u2，P_m1，P_m2，P_m3，P_m4，P_m5，P_l1，P_l2，P_l3，P_l4，P_l5表示根据上述的动力学方程联立得出的对应状态变量和输入的权值。系统输出为θ_c，表示系统组合质心相对于踝关节的夹角，K₁，K₂，K₃表示根据组合质心公式，三个角度分别对应的权值。

进一步作为本方法的优选实施例，所述简化后的状态空间方程表达式如下：

上式中，x₁＝[θ_u θ_m θ_l]^T为角度值，

为角速度值，u表示系统输入，K＝[K₃ K₂ K₁]，

表示角速度值，

表示角加速度值，

将x_1d定义为期望的输入角度，以e来表示期望值和实际值之间的误差。

针对误差e，加入修正矩阵，设计以下控制器：

进一步作为本方法的优选实施例，所述控制器的控制信号表达式如下：

具体地，其中K₁₁和K₁₂是状态反馈增益(此时的状态变量是x₂和e)，可以用常规的控制算法求得。calibration_matrix为针对误差e的修正矩阵，使得该控制器可以根据需求自由配置θ_u和θ_m的变化过程，对于θ_l必须设置为0°，映射到现实中，下摆相当于四足机器人的支撑后肢，不宜做角度上的变化。

参照图3、图4和图5，在初始状态下，前/后肢停留在地面上以模拟四足动物四足站立的情况。通过在肩关节和髋关节处施加主动扭矩，在四足机器人的站姿转换过程中调整模型的组合质心(CoM)与地面的夹角，控制该模型达到前肢抬离地面，而后肢不抬起的双足站立平衡状态。利用传感器(光电编码器、IMU等)获取四足机器人肩关节、髋关节、踝关节的角度和角速度，并同肩关节和髋关节的期望角度输入其对应的力矩控制器，根据肩关节、髋关节、踝关节的反馈角度和角速度，获取实际的控制力矩。按时间更新肩关节和髋关节的期望角度，实现四足机器人站姿转换过程中运动姿态的自由配置。另外，在平衡阶段，根据相应的控制力矩将四足机器人的CoM稳定在踝关节的正上方，实现四足机器人在有界扰动状态下的平衡控制，以保证系统在双足站立的条件下受到未知的有界扰动后能保持平衡稳定。

本发明根据四足机器人实际的应用需求，建立了三阶倒立摆新构型。此新构型与传统的倒立摆模型(即小车-倒立摆模型)不同，大大简化了动力学/运动学分析，丰富了运动过程的选择，传统模型只是把倒立摆立起来不让它们倒下；根据新构型进行数学建模，整个系统只需要两个输入(即两个驱动)，只需要控制四足机器人的肩关节和髋关节即可实现整套动作，需要的状态变量也就较少；本发明的控制器好处在于，可以让工作人员根据实际的需求，自由地调整机器人的运动过程，并非单一地只是让四足机器人完成四足站立到双足站立。

如图2所示，一种四足机器人站姿转换的平衡控制方法系统，包括以下模块：

动力学方程模块，用于基于预设的三阶倒立摆新构型构建动力学模型；

状态空间方程模块，用于根据动力学模型得到状态空间方程并简化，得到简化后的状态空间方程；

控制器模块，用于基于简化后的状态空间方程构建四足机器人控制器；

实际姿态模块，用于基于传感器获取四足机器人实际肩关节、髋关节和踝关节的角度和角速度；

反馈模块，用于将实际肩关节、髋关节和踝关节三个关节的角度和角速度结合肩关节和髋关节的期望角度输入到控制器，并得到肩关节、髋关节、踝关节的反馈角度和反馈角速度；

实际控制力矩模块，用于根据肩关节、髋关节、踝关节的反馈角度和角速度，得到对应的实际控制力矩；

姿态更新模块，用于根据对应的实际控制力矩和按时间更新肩关节和髋关节的期望角度，实现站姿转换。

上述系统实施例中的内容均适用于本方法实施例中，本方法实施例所具体实现的功能与上述系统实施例相同，并且达到的有益效果与上述系统实施例所达到的有益效果也相同。

以上是对本发明的较佳实施进行了具体说明，但本发明创造并不限于所述实施例，熟悉本领域的技术人员在不违背本发明精神的前提下还可做作出种种的等同变形或替换，这些等同的变形或替换均包含在本申请权利要求所限定的范围内。

Claims (9)

1.一种四足机器人站姿转换的平衡控制方法，其特征在于，包括以下步骤：

基于预设的三阶倒立摆新构型构建动力学模型；

根据动力学模型得到状态空间方程并简化，得到简化后的状态空间方程；

基于简化后的状态空间方程构建四足机器人控制器；

所述控制器的控制信号表达式为：

K₁₁和K₁₂表示状态反馈增益，calibration_matrix为针对误差e的修正矩阵，u表示系统输入，

为角速度值，x_1d表示期望的输入角度，P_u1，P_u2，P_m1，P_m2，P_m3，P_m4，P_m5，P_l1，P_l4，P_l5表示对应状态变量和输入的权值，

表示肩关节对应的角速度，

表示髋关节对应的角速度，

表示踝关节对应的角速度；

基于传感器获取四足机器人实际肩关节、髋关节和踝关节的角度和角速度；

将实际肩关节、髋关节和踝关节三个关节的角度和角速度结合肩关节和髋关节的期望角度输入到控制器，并得到肩关节、髋关节、踝关节的反馈角度和反馈角速度；

根据肩关节、髋关节、踝关节的反馈角度和角速度，得到对应的实际控制力矩；

根据对应的实际控制力矩和按时间更新肩关节和髋关节的期望角度，实现站姿转换。

2.根据权利要求1所述一种四足机器人站姿转换的平衡控制方法，其特征在于，所述预设的三阶倒立摆新构型包括模拟四足机器人前肢的上摆、模拟躯干的中摆和模拟后肢的下摆。

3.根据权利要求2所述一种四足机器人站姿转换的平衡控制方法，其特征在于，所述基于预设的三阶倒立摆新构型构建动力学模型这一步骤，其具体包括：

根据预设的三阶倒立摆新构型的上摆的受力情况和肩关节处的力矩平衡方程，得到上摆动力学模型；

根据预设的三阶倒立摆新构型的中摆的力学方程和髋关节处的力矩平衡方程，得到中摆动力学模型；

根据预设的三阶倒立摆新构型的下摆的力学方程，得到下摆动力学模块。

4.根据权利要求3所述一种四足机器人站姿转换的平衡控制方法，其特征在于，所述上摆动力学模型表达式如下：

上式中，M_u表示上摆的质量，l_u表示上摆长度，l_m表示中摆长度，l_l表示下摆长度，J_u表示上摆的理想转动惯量，θ_u表示肩关节对应的角度，F₁表示肩关节处产生的横向力，T_act1表示在肩关节处施加的主动力矩，g表示表示地球重力加速度，

表示肩关节对应的角加速度，

表示表示髋关节对应的角加速度，

表示表示踝关节对应的角加速度。

5.根据权利要求4所述一种四足机器人站姿转换的平衡控制方法，其特征在于，所述中摆动力学模型表达式如下：

上式中，M_m表示中摆的质量，J_m表示中摆的理想转动惯量，θ_m表示髋关节对应的角度，F₂表示髋关节处产生的横向力，T_act2表示在髋关节处施加的主动力矩。

6.根据权利要求5所述一种四足机器人站姿转换的平衡控制方法，其特征在于，所述下摆动力学模型表达如下：

上式中，M_l表示下摆的质量，J_l表示下摆的理想转动惯量，θ_l表示踝关节对应的角度。

7.根据权利要求6所述一种四足机器人站姿转换的平衡控制方法，其特征在于，所述状态空间方程表达式如下：

上式中，状态变量为θ_u，θ_m，θ_l，

系统输入为T_act1和T_act2，其中

表示肩关节对应的角速度，

表示髋关节对应的角速度，

表示踝关节对应的角速度，P_u1，P_u2，P_m1，P_m2，P_m3，P_m4，P_m5，P_l1，P_l2，P_l3，P_l4，P_l5表示对应状态变量和输入的权值，系统输出为θ_c，表示系统组合质心相对于踝关节的夹角，K₁，K₂，K₃表示根据组合质心公式，三个角度分别对应的权值。

8.根据权利要求7所述一种四足机器人站姿转换的平衡控制方法，其特征在于，所述简化后的状态空间方程表达式如下：

上式中，x₁＝[θ_u θ_m θ_l]^T为角度值，

为角速度值，u表示系统输入，K＝[K₃K₂ K₁]，

表示角速度值，

表示角加速度值，

9.一种四足机器人站姿转换的平衡控制系统，其特征在于，包括以下模块：

动力学方程模块，用于基于预设的三阶倒立摆新构型构建动力学模型；

状态空间方程模块，用于根据动力学模型得到状态空间方程并简化，得到简化后的状态空间方程；

控制器模块，用于基于简化后的状态空间方程构建四足机器人控制器，所述控制器的控制信号表达式为：

K₁₁和K₁₂表示状态反馈增益，calibration_matrix为针对误差e的修正矩阵，u表示系统输入，

为角速度值，P_u1，P_u2，P_m1，P_m2，P_m3，P_m4，P_m5，P_l1，P_l4，P_l5表示对应状态变量和输入的权值，

表示肩关节对应的角速度，

表示髋关节对应的角速度，

表示踝关节对应的角速度；

实际姿态模块，用于基于传感器获取四足机器人实际肩关节、髋关节和踝关节的角度和角速度；

反馈模块，用于将实际肩关节、髋关节和踝关节三个关节的角度和角速度结合肩关节和髋关节的期望角度输入到控制器，并得到肩关节、髋关节、踝关节的反馈角度和反馈角速度；

实际控制力矩模块，用于根据肩关节、髋关节、踝关节的反馈角度和角速度，得到对应的实际控制力矩；

姿态更新模块，用于根据对应的实际控制力矩和按时间更新肩关节和髋关节的期望角度，实现站姿转换。

Images