CN112147890B - 一种基于Acrobot模型的四足机器人对角支撑静平衡控制方法 - Google Patents

Abstract

本发明公开了一种基于Acrobot模型的四足机器人对角支撑静平衡控制方法，对四足机器人的对角支撑模型进行简化，降低了平衡控制的复杂度，引入Acrobot欠驱动倒立摆模型，很好的解决了身体质心会偏离转轴位置这一问题；最后对于Acrobot这一复杂的非线性系统，通过线性二次型调节器在平衡点处进行稳定控制，通过仿真分析与模型建立，将对角支撑模型简化为一个虚拟的欠驱动二阶倒立摆Acrobot模型，进一步进行对角支撑静平衡控制。

Description

一种基于Acrobot模型的四足机器人对角支撑静平衡控制 方法

技术领域

本发明属于机器人控制技术领域，具体涉及一种基于Acrobot模型的四足机器人对角支撑静平衡控制方法。

背景技术

四足机器人的平衡控制一直是机器人领域的研究热点，目前四足机器人平衡控制的研究主要着眼于动态平衡控制，而目前主流的动平衡控制方法存在各自的不足之处。基于SLIP(Spring Loaded Inverted Pendulum)动平衡控制方法可以完成四足机器人的动步态下的平衡控制，但是为了保持动态平衡，必须通过较高频率的切换支撑腿与摆动腿。此外，当机器人的机体姿态出现偏转时，SLIP模型需要调整摆动腿落足点的位置，该方法限制了机器人的运动效率和对复杂环境的适应能力。基于惯性轮倒立摆的静平衡控制方法的上摆质心位于上摆旋转轴处，但是其与四足机器人的机体状态不符，因此该控制模型存在误差，此外，这种静平衡控制方法是通过对角支撑腿髋关节力矩输出进行平衡控制，依据对角支撑腿髋关节不同轴的机体机构，进行静平衡控制时四足机器人会出现机体航向偏转的问题。

发明内容

本发明所要解决的技术问题在于针对上述现有技术中的不足，提供一种基于Acrobot模型的四足机器人对角支撑静平衡控制方法，通过建立四足机器人的对角支撑动力学模型，基于LQR控制方法推导出静平衡控制方法，并在仿真与实物上验证了算法的可行性。

本发明采用以下技术方案：

一种基于Acrobot模型的四足机器人对角支撑静平衡控制方法，包括以下步骤：

S1、建立四足机器人身体坐标系(x_b,y_b,z_b)描述身体翻转姿态，x轴沿对角支撑髋关节连线方向，z轴沿竖直向上方向，建立四足机器人对角支撑腿坐标系

和

用于描述支撑腿倾角，坐标系方向与身体系相同，四足机器人的对角支撑看作两个上摆耦合的倒立摆模型；

S2、通步骤S1得到的两个倒立摆模型，在两对角支撑腿坐标系yz平面内进行投影，得到一个欠驱动二阶倒立摆Acrobot模型，确定两个Acrobot倒立摆模型之间的耦合关系；

S3、当对角两支撑腿髋关节采取相同控制时，对步骤S2得到的耦合关系进行解耦，将四足机器人对角支撑虚拟腿简化为单个Acrobot倒立摆模型，根据最简的对角支撑模型确定最简模型的拉格朗日动力学方程，并在平衡区进行线性化处理，根据状态变量将线性化的动力学方程转化为系统的状态方程；

S4、根据步骤S3建立的系统状态方程，选取角度与角速度作为系统的状态变量，并采用基于LQR的状态反馈控制方法对四足机器人的对角支撑进行静平衡控制，设置实数矩阵Q和实数矩阵R，输入原系统的系统矩阵与输入矩阵，得到系统的状态反馈矩阵K，根据系统状态反馈控制器得到输出力矩τ，确定对角支撑腿髋关节平衡控制的力矩输出ⁱτ，i＝1,2，完成平衡控制。

具体的，步骤S1中，对四足机器人的对角支撑模型进行简化，引入Acrobot欠驱动倒立摆模型；通过线性二次型调节器在平衡点处进行稳定控制，四足机器人在对角支撑状态下的静平衡控制为平衡区的稳定控制，对平衡区的稳定控制进行建模，针对欠驱动二阶倒立摆模型，运用拉格朗日动力学方法建立对角支撑最简动力学方程。

具体的，步骤S2中，两个Acrobot倒立摆的耦合关系为：

¹θ₁+¹θ₂＝²θ₁+²θ₂

其中，i＝1为前支撑腿，i＝2为后支撑腿；¹θ₁为对应前支撑腿的虚拟腿在支撑腿坐标系yz平面投影与z轴夹角，¹θ₂为对应后支撑腿的虚拟腿在支撑腿坐标系yz平面投影与z轴夹角，²θ₁为身体坐标系z轴在对应前支撑腿坐标系yz平面平面投影与虚拟腿投影的夹角，²θ₂为身体坐标系z轴在对应后支撑腿坐标系yz平面平面投影与虚拟腿投影的夹角。

具体的，步骤S3中，当对角两支撑腿髋关节采取相同的控制时，即¹τ＝²τ，将两条支撑虚拟腿简化为一条虚拟腿，即¹θ₁＝²θ₁,¹θ₂＝²θ₂，把四足机器人对角支撑简化模型简化为单个Acrobot倒立摆模型，在四足机器人最简模型中，倾角θ₁为虚拟腿在投影面的投影与支撑腿坐标系z轴夹角，即θ₁＝¹θ₁＝²θ₁，倾角θ₂为身体坐标系z轴与虚拟腿在投影面的投影夹角，即θ₂＝¹θ₂＝²θ₂，力矩τ为驱动关节的输出力矩，即τ/2＝¹τ＝²τ，选取

作为四足机器人对角支撑最简模型的系统的状态变量，对四足机器人对角支撑模型的动力学方程进行线性化处理，得到线性化后的动力学方程，选取关节力矩τ作为系统的控制变量，将线性化的动力学方程转化为系统的状态方程。

进一步的，四足机器人对角支撑最简模型的动力学方程为：

其中，

^bC＝m₂L₁l₂,^bD＝(m₁l₁+m₂L₁)g,^bE＝m₂gl₂，c₂＝cosθ₂,s₂＝sinθ₂,s₁₂＝sin(θ₁+θ₂)，k为髋关节减速器减速比，m₁为两对角支撑腿的质量，m₂为身体与两对角摆动腿的质量，I₁为两对角支撑腿绕质心x轴转动惯量，I₂为身体与两对角摆动腿的绕质心x轴转动惯量，I_M为髋关节驱动电机转动惯量，L₁为虚拟腿的长度，l₁为虚拟腿质心到足底的距离，l₂为身体与摆动腿质心到髋关节轴连线在z向距离。

更进一步的，选取

作为对角支撑最简模型的状态变量，选取驱动关节τ作为系统控制变量，将线性化动力学方程转化为系统的状态方程如下：

u＝τ

0_2×1＝[0 0]

其中，A为系统状态转移矩阵，b为系统控制量矩阵，u为控制量。

具体的，采用基于LQR的状态反馈控制方法，完成状态反馈矩阵K的计算后，最简模型的状态反馈控制输出为τ＝-Kx，则四足机器人对角支撑静平衡控制方法下，为进行平衡控制对角支撑腿髋关节的力矩输出为：

其中，

与现有技术相比，本发明至少具有以下有益效果：

本发明一种基于Acrobot模型的四足机器人对角支撑静平衡控制方法，对四足机器人对角支撑模型进行简化，得到了四足机器人对角支撑的最简模型，根据拉格朗日动力学方法建立四足机器人运动学模型，通过设置不同的坐标系对四足机器人运动学模型进行表示，实现四足机器人对角支撑静平衡控制方法的可行性。

进一步的，针对对角支撑问题，引入Acrobot倒立摆模型，并将对角支撑模型简化为Acrobot倒立摆，一方面Acrobot倒立摆控制方面有着比较成熟的控制理论，另一方面只研究倒立摆平衡区的稳定，可以简化对角支撑问题。

进一步的，对于对角支撑可以看作是两个耦合的倒立摆，解决其中的耦合问题有利于模型的简化。

进一步的，将两个对角的倒立摆进一步进行简化，得到一个虚拟的倒立摆，然后对该倒立摆进行建模控制。

进一步的，运用拉格朗日动力学方法建立四足机器人对角支撑最简模型的动力学方程。

进一步的，在Acrobot倒立摆平衡点附近，忽略的动力学模型中的二阶小量，对动力学模型进行线性化处理，将一个非线性问题转换为线性问题。

进一步的，运用线性二次型调节器(LQR)计算系统的状态反馈矩阵，进而计算出控制的力矩，LQR是一种成熟的状态空间设计发，易于构成闭环最优控制，计算起来十分方便，可靠。

综上所述，本发明在保持对角支撑的状态下实现对四足机器人的平衡控制，对摆动腿的落足点位置没有额外限制，此外这种模型还充分考虑了机器人身体质心偏离转轴位置的情况，其上摆质心可以通过调整模型参数进行设置。

下面通过附图和实施例，对本发明的技术方案做进一步的详细描述。

附图说明

图1为四足机器人对角支撑状态图；

图2为四足机器人对角支撑简化模型图；

图3为四足机器人对角支撑前后支撑腿Acrobot模型图；

图4为四足机器人对角支撑最简模型静平衡控制角位移图；

图5为四足机器人对角支撑最简模型静平衡控制角速度图；

图6为四足机器人对角支撑最简模型静平衡控制力矩图。

具体实施方式

本发明提供了一种基于Acrobot模型的四足机器人对角支撑静平衡控制方法，对四足机器人的对角支撑模型进行简化，降低了平衡控制的复杂度，引入了Acrobot欠驱动倒立摆模型，很好的解决了身体质心会偏离转轴位置这一问题；最后对于Acrobot这一复杂的非线性系统，通过线性二次型调节器(LQR)在平衡点处进行稳定控制，通过仿真分析与模型建立，将对角支撑模型简化为一个虚拟的欠驱动二阶倒立摆Acrobot模型，进一步来进行对角支撑静平衡控制。

本发明一种基于Acrobot模型的四足机器人对角支撑静平衡控制方法，包括以下步骤：

S1、建立四足机器人身体坐标系(x_b,y_b,z_b)描述身体翻转姿态，x轴沿对角支撑髋关节连线方向，z轴沿竖直向上方向，建立四足机器人对角支撑腿坐标系

和

描述支撑腿倾角，坐标系方向与身体系相同，四足机器人的对角支撑看作两个上摆耦合的欠驱动Acrobot倒立摆模型；

请参阅图1，为了研究四足机器人对角支撑下的平衡控制问题，选定两对角摆动腿沿竖直方向抬起并且保持关节锁定，用于减小扰动，两对角支撑腿未驱动质心移动的状态，进行静平衡控制。

在这种状态下，将四足机器人两条支撑腿看作是与身体固连在一起，即与身体简化为同一个刚体。由于对角支撑静平衡是通过髋关节的驱动对四足机器人机体进行平衡控制，所以四足机器人的对角支撑可以看作是两个上摆耦合的欠驱动Acrobot倒立摆模型。

针对欠驱动二阶倒立摆，包括摇起区与平衡区，四足机器人在对角支撑状态下的静平衡控制不涉及摆起控制，只针对平衡区的稳定控制进行建模；针对欠驱动二阶倒立摆模型，运用拉格朗日动力学方法建立对角支撑最简动力学方程。

S2、在两对角支撑腿坐标系yz平面内进行投影，得到一个欠驱动二阶倒立摆Acrobot模型，根据两个Acrobot倒立摆的耦合关系和最简模型的拉格朗日函数L确定四足机器人对角支撑最简模型的动力学方程；

两个Acrobot倒立摆的耦合关系为：

¹θ₁+¹θ₂＝²θ₁+²θ₂

请参阅图2，为四足机器人对角支撑简化模型，其中，i＝1为前支撑腿，i＝2为后支撑腿。¹θ₁为对应前支撑腿的虚拟腿在支撑腿坐标系yz平面投影与z轴夹角，¹θ₂为对应后支撑腿的虚拟腿在支撑腿坐标系yz平面投影与z轴夹角，²θ₁为身体坐标系z轴在对应前支撑腿坐标系yz平面平面投影与虚拟腿投影的夹角，²θ₂为身体坐标系z轴在对应后支撑腿坐标系yz平面平面投影与虚拟腿投影的夹角，¹τ为对应前支撑腿髋关节的输出力矩，²τ为对应后支撑腿髋关节的输出力矩。

S3、当对角两支撑腿髋关节采取相同控制时，将四足机器人对角支撑简化模型简化为单个Acrobot倒立摆模型，根据最简模型的拉格朗日函数L确定四足机器人对角支撑最简模型的动力学方程，进行线性化处理，将线性化的动力学方程转化为系统的状态方程；

当对角两支撑腿髋关节采取相同的控制时，即¹τ＝²τ，将两条支撑虚拟腿简化为一条虚拟腿，即¹θ₁＝²θ₁,¹θ₂＝²θ₂，即把四足机器人对角支撑简化模型进一步简化为单个Acrobot倒立摆模型，如图3所示。

在四足机器人最简模型中，倾角θ₁为虚拟腿在投影面的投影与支撑腿坐标系z轴夹角，即θ₁＝¹θ₁＝²θ₁，倾角θ₂为身体坐标系z轴与虚拟腿在投影面的投影夹角，即θ₂＝¹θ₂＝²θ₂，力矩τ为驱动关节的输出力矩，即τ/2＝¹τ＝²τ。

首先计算出四足机器人对角支撑最简模型的总势能P和总动能K，进一步得到最简模型的拉格朗日函数L，将拉格朗日函数L代入得到四足机器人对角支撑最简模型的动力学方程：

在倒立摆平衡点附近，忽略动力学模型中的二阶小量，对四足机器人对角支撑模型的动力学方程进行线性化处理，得到线性化后的动力学方程。

选取

作为四足机器人对角支撑最简模型的系统的状态变量，选取关节力矩τ作为系统的控制变量，将线性化的动力学方程转化为系统的状态方程

如下：

u＝τ

S4、采用基于LQR的状态反馈控制方法对四足机器人的对角支撑进行静平衡控制，输入原系统的系统矩阵与输入矩阵，根据控制要求设置实数矩阵Q和实数矩阵R，得到状态反馈矩阵K。

完成状态反馈矩阵K的计算后，最简模型的状态反馈控制输出为τ＝-Kx，则四足机器人对角支撑静平衡控制方法下，为进行平衡控制对角支撑腿髋关节的力矩输出为：

其中，

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。通常在此处附图中的描述和所示的本发明实施例的组件可以通过各种不同的配置来布置和设计。因此，以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

请参阅图4、图5和图6，对四足机器人对焦支撑最简模型静平衡控制效果进行精确的仿真，采用四阶龙格-库塔算法可以省去求解微分方程的复杂过程，同时保持较高的精度。在该静平衡控制下仿真中，四足机器人对角支撑最简模型的状态响应以及控制力矩如图4、图5和图6所示。

通过仿真结果可以看出，在四足机器人机体存在初始翻转角的的情况下，通过静平衡控制方法，可以使得四足机器人控制机体回复到零位并保持稳定，该仿真结果验证了四足机器人对角支撑静平衡控制方法的可行性。

综上所述，本发明一种基于Acrobot模型的四足机器人对角支撑静平衡控制方法，对四足机器人对角支撑模型进行简化，得到了四足机器人对角支撑的最简模型，根据拉格朗日动力学方法建立四足机器人对角支撑的动力学模型，并应用LQR最优控制方法计算状态反馈矩阵，最后对该最简模型进行仿真，仿真结果验证了静平衡控制方法的可行性。此外，在四足机器人实验平台上也取得了较理想的结果。

以上内容仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明权利要求书的保护范围之内。

Claims (6)

1.一种基于Acrobot模型的四足机器人对角支撑静平衡控制方法，其特征在于，包括以下步骤：

S1、建立四足机器人身体坐标系(x_b,y_b,z_b)描述身体翻转姿态，x轴沿对角支撑髋关节连线方向，z轴沿竖直向上方向，建立四足机器人对角支撑腿坐标系

和

用于描述支撑腿倾角，坐标系方向与身体坐标系相同，四足机器人的对角支撑看作两个上摆耦合的倒立摆模型；

S2、通过步骤S1得到的两个倒立摆模型，在两对角支撑腿坐标系yz平面内进行投影，得到一个Acrobot欠驱动倒立摆模型，确定两个Acrobot欠驱动倒立摆模型之间的耦合关系；

S3、当对角两支撑腿髋关节采取相同控制时，对步骤S2得到的耦合关系进行解耦，将四足机器人对角支撑虚拟腿简化为单个Acrobot欠驱动倒立摆模型，根据对角支撑最简动力学方程确定拉格朗日动力学方程，并在平衡区进行线性化处理，根据状态变量将线性化的动力学方程转化为系统的状态方程，当对角两支撑腿髋关节采取相同的控制时，即¹τ＝²τ，将两条支撑虚拟腿简化为一条虚拟腿，即¹θ₁＝²θ₁,¹θ₂＝²θ₂，把四足机器人对角支撑最简模型简化为单个Acrobot倒立摆模型，在四足机器人对角支撑最简模型中，倾角θ₁为虚拟腿在投影面的投影与支撑腿坐标系z轴夹角，即θ₁＝¹θ₁＝²θ₁，倾角θ₂为身体坐标系z轴与虚拟腿在投影面的投影夹角，即θ₂＝¹θ₂＝²θ₂，力矩τ为驱动关节的输出力矩，即τ/2＝¹τ＝²τ，选取

作为四足机器人对角支撑最简模型的系统的状态变量，对四足机器人对角支撑最简模型的动力学方程进行线性化处理，得到线性化后的动力学方程，选取关节力矩τ作为系统的控制变量，将线性化的动力学方程转化为系统的状态方程；

S4、根据步骤S3建立的系统状态方程，选取角度与角速度作为系统的状态变量，并采用基于LQR的状态反馈控制方法对四足机器人的对角支撑进行静平衡控制，设置实数矩阵Q和实数矩阵R，输入原系统的系统矩阵与输入矩阵，得到系统的状态反馈矩阵K，根据系统状态反馈控制器得到输出力矩τ，确定对角支撑腿髋关节平衡控制的力矩输出ⁱτ，i＝1,2，完成平衡控制。

2.根据权利要求1所述的基于Acrobot模型的四足机器人对角支撑静平衡控制方法，其特征在于，步骤S1中，对四足机器人的对角支撑模型进行简化，引入Acrobot欠驱动倒立摆模型；通过线性二次型调节器在平衡点处进行稳定控制，四足机器人在对角支撑状态下的静平衡控制为平衡区的稳定控制，对平衡区的稳定控制进行建模，针对欠驱动二阶倒立摆模型，运用拉格朗日动力学方法建立对角支撑最简动力学方程。

3.根据权利要求1所述的基于Acrobot模型的四足机器人对角支撑静平衡控制方法，其特征在于，步骤S2中，两个Acrobot倒立摆的耦合关系为：

¹θ₁+¹θ₂＝²θ₁+²θ₂

其中，i＝1为前支撑腿，i＝2为后支撑腿；¹θ₁为对应前支撑腿的虚拟腿在支撑腿坐标系yz平面投影与z轴夹角，¹θ₂为对应后支撑腿的虚拟腿在支撑腿坐标系yz平面投影与z轴夹角，²θ₁为身体坐标系z轴在对应前支撑腿坐标系yz平面平面投影与虚拟腿投影的夹角，²θ₂为身体坐标系z轴在对应后支撑腿坐标系yz平面平面投影与虚拟腿投影的夹角。

4.根据权利要求1所述的基于Acrobot模型的四足机器人对角支撑静平衡控制方法，其特征在于，四足机器人对角支撑最简模型的动力学方程为：

其中，

^bC＝m₂L₁l₂,^bD＝(m₁l₁+m₂L₁)g,^bE＝m₂gl₂，c₂＝cosθ₂,s₂＝sinθ₂,s₁₂＝sin(θ₁+θ₂)，k为髋关节减速器减速比，m₁为两对角支撑腿的质量，m₂为身体与两对角摆动腿的质量，I₁为两对角支撑腿绕质心x轴转动惯量，I₂为身体与两对角摆动腿的绕质心x轴转动惯量，I_M为髋关节驱动电机转动惯量，L₁为虚拟腿的长度，l₁为虚拟腿质心到足底的距离，l₂为身体与摆动腿质心到髋关节轴连线在z向距离。

5.根据权利要求4所述的基于Acrobot模型的四足机器人对角支撑静平衡控制方法，其特征在于，选取

作为四足机器人对角支撑最简模型的状态变量，选取驱动关节τ作为系统控制变量，将线性化动力学方程转化为系统的状态方程如下：

u＝τ

0_2×1＝[0 0]

其中，A为系统状态转移矩阵，b为系统控制量矩阵，u为控制量。

6.根据权利要求1所述的基于Acrobot模型的四足机器人对角支撑静平衡控制方法，其特征在于，采用基于LQR的状态反馈控制方法，完成状态反馈矩阵K的计算后，四足机器人对角支撑最简模型的状态反馈控制输出为τ＝-Kx，则四足机器人对角支撑静平衡控制方法下，为进行平衡控制对角支撑腿髋关节的力矩输出为：

其中，

ⁱθ₁为对应支撑腿的虚拟腿在支撑腿坐标系yz平面投影与z轴夹角，ⁱθ₂为身体坐标系z轴在对应支撑腿坐标系yz平面投影与虚拟腿投影的夹角。

Images