CN113485398B - 一种轮式双足机器人姿态控制方法 - Google Patents

Abstract

本发明公开了一种轮式双足机器人姿态控制方法，通过建立低维单轮‑倒立质心摆模型，优化控制质心与轮轴相对位置，从而实现轮式稳定与移动能力；建立双足机器人全身动力学模型，依据机器人参考位姿求解全身动力学补偿控制量和调节量，实现腿式高维非线性系统的位姿控制。该方法将轮式双足机器人姿态控制与平衡控制解耦，通过建立低维模型进而快速优化平衡控制量，通过高维模型精确控制机器人形态位姿，因此方法结合了快、准的优点，使轮式双足机器人控制更加方便快捷并具备稳定性。

Description

一种轮式双足机器人姿态控制方法

技术领域

本发明属于双足机器人技术领域，尤其是一种轮式双足机器人姿态控制方法。

背景技术

双足机器人是具有人类外形、双腿等特征的智能机器人，也是人们对机器人形象设想的完美具体表现形式。它可以实现拟人的灵巧行走动作，对未知复杂环境的适应能力强，相较于专业机器人需要对环境进行改造的情况双足机器人基本不需要并同时代替人工劳动完成更加复杂工作任务等。轮子可以赋予机械更敏捷快速的移动能力，其运动速度远超腿足生物，是提升机器人速度的有效方式之一。因此轮式双足机器人不仅具备仿人机器人的灵巧拟人作业能力和环境适应性，还拥有轮式移动的快速性。而机器人具备多学科交叉融合的特点，需要机电、计算机、仿生等多领域技术支撑，多国都在大力研究中，而我国也将智能机器人发展作为规划目标，因此，轮式双足机器人具有重要的研究意义与应用价值。

现有轮式双足机器人自由度配置较为简单，如图1依次所示，早期轮式机器人在平衡车基础上增加人形躯体，但也只是刚性一体结构，根本不具备双足运动能力，如ALTER-EGO 机器人所示；近期，出现了腿部连接的轮足形态，但腿部自由度仅有一个，腿部只能进行升缩的功能，如ASCENTO机器人所示；腾讯公司的机器人采用菱形腿设计能够使腿部具有伸缩和摆动的能力，但已经偏离仿人机器人形态；而波士顿动力公司的Handle机器人具备人腿外形以及庞大上身和手臂，但腿部不具备侧摆能力，虽然该机器人工作灵活能够实现跳跃、滑行和搬动货物的能力，但该公司并未公开其方法，属于商业机密。

现有技术中存在的不足集中在以下两点：

1、轮式双足机器人平衡稳定控制问题。相比较两轮的平衡车两自由度、低质心、一体化结构，轮式双足机器人增加了更多的关节自由度，质心相较轮轴距离增大，并且两轮通过双腿连接在上身处使得相对位置的可变性增大，控制难度加大。

2、轮式双足机器人位置和姿态控制问题。相比较传统具有平脚掌的双足机器人，轮式双足机器人不能通过脚掌与脚踝的运动配合调整自身姿态，踩在轮子上的固有不平衡性导致在此基础上的传统控制方法在轮足机器人上的不适用性。

针对上文提到的现有技术问题，传统做法是上身形态固定不动，从而将整个系统简化为单一的平衡车控制，但是这不符合轮式双足机器人快速移动、灵巧作业的要求，当上身姿态需要变换的时候平衡车控制的稳定性将降低，更不要说手臂的协同操作等更复杂行为；也有建立机器人全模型进行优化控制的做法，但对于有限的计算机资源，这一个过程是非常耗时的，并且由于轮式运动的高速性，实时快速的控制成为必不可少、无可替代的部分，这样才能保障机器人的安全性。

发明内容

为了解决现有技术中存在的不足，本发明提出了一种轮式双足机器人姿态控制方法，为轮式建立与整个系统解耦的轮-倒立质心摆模型，通过低维系统优化控制可以实现快速实时性极高的稳定性能；为双足机器人建立全身动力学模型通过动力学补偿实现精确的位姿控制。

本发明所采用的技术方案如下：

一种轮式双足机器人姿态控制方法，分别构造姿态控制器和平衡控制器；所述姿态控制器包括姿态解算和全身动力学补偿控制，姿态解算接收给定上身躯干的参考姿态矩阵 R_ref、躯干相对轮轴的参考位置p_ref以及状态估计得到的机器人状态；姿态解算进行姿态解算后经全身动力学补偿控制单元得到姿态控制量τ_leg，即机器人腿部各关节驱动力矩，从而控制身体姿态；

所述平衡控制器接收给定轮式双足机器人移动的参考速度v_ref、状态估计得到的机器人质心相对轮轴的位置d_com以及质心相对世界坐标系的实际速度v_com，形成反馈控制率得到平衡调节控制量τ_wheel，即轮子的驱动力矩，从而调整机器人的运动平衡。

进一步，平衡控制器的构造方法为：

S1、将当前机器人状态下上身简化为质心刚体，底部为轮子刚体，两个刚体之间通过无质量的结构连接形成轮-倒立质心摆模型；

S2、设平衡控制器的优化变量ε，建立控制目标函数及优化的约束条件；

S3、在每个控制周期内，根据机器人状态得到轮-倒立质心摆状态，经过优化计算最终优化量ε中的力矩值，即平衡控制器的调节量τ_wheel。

进一步，轮-倒立质心摆模型的动力学方程表示为：

其中，ξ_WIP是轮-倒立质心摆模型的状态变量，分别是状态变量一阶微分和二阶微分，M(ξ_WIP)是由状态变量决定的系统惯量矩阵，/>是由状态变量及其一阶微分决定的科氏力、重力项矩阵，B是驱动力矩选择矩阵，J_wheel是轮子边缘与地面接触点相对世界坐标系的雅克比矩阵，(·)^T是矩阵中的转置运算，λ_ext是该接触点的外力。因此整个系统的输入为轮子的驱动力矩τ_wheel。

进一步，设平衡控制器的优化变量为目标函数表示为：

其中，w₁,w₂,w₃均是优化权重系数，β是距离调节增益系数，J_com是质心相对世界坐标系的雅克比矩阵而是其一阶微分，/>是轮子驱动力矩的上一次调节量。

进一步，平衡控制器的约束包括动力学方程约束、轮子驱动能力约束和接触点的摩擦锥约束。

进一步，动力学约束为：

轮子驱动约束：τ_min和τ_max分别是电机输出的最小力矩值与最大力矩值；

接触点摩擦锥约束：其中μ为摩擦因数。

进一步，姿态控制器的构造方法为：

S1、设全身动力学模型的系统状态变量χ_WBD，使用拉格朗日动力学构建全身动力学方程；

S2、将上身的位置和姿态参考加速度输入PD控制器，由PD控制器输入的六个量和组成/>因此得到全身动力学补偿控制器；/>是系统状态变量参考值的二阶微分；

S3、为了增加驱动关节的姿态控制精度，在前馈力矩基础上增加关节层的PD控制器得到最终的姿态控制器控制量τ_leg。

进一步，全身动力学模型的系统状态变量为χ_WBD＝[[x_b,y_b,z_b,r_b,p_b,ya_b]^T；q_leg]，其中， x_b和r_b分别是上身躯干质心处世界坐标系下的x方向位置和角度，y_b和p_b分别是上身躯干质心处世界坐标系下的y方向位置和角度，z_b和ya_b分别是上身躯干质心处世界坐标系下的 z方向位置和角度，q_leg包含两条腿各关节的角度；

全身动力学方程表示为：

其中，是系统状态变量的一阶微分，D(χ_WBD)是由机器人状态变量决定的系统惯量矩阵，/>是由状态变量及其一阶微分决定的科氏力矩阵，G(χ_WBD)是重力项矩阵，Φ是腿部驱动力矩选择矩阵即前6行元素为零，τ_leg为腿部各关节驱动力矩，/>分别是右、左腿末端相对世界坐标系的雅克比矩阵，/>和/>分别是右、左腿末端与地面接触点收到的外部作用力。

进一步，利用PD控制器得到的六个量表示为：

其中，分别是上身躯干质心处世界坐标系下的x方向、y方向和z方向加速度的参考值，/>分别是上身躯干质心处世界坐标系下的x方向、y方向和z 方向角加速度的参考值；kp_tor和kd_tor均为PD控制增益参数矩阵，/>分别是身躯干质心处世界坐标系下的x方向、y方向和z方向的速度，/>分别是身躯干质心处世界坐标系下的x方向、y方向和z方向的角速度；且为了避免引入噪声和误差，故设置两条腿各关节的角加速度参考值/>

得到全身动力学补偿控制器，表示为：

其中，是系统状态变量参考值；/>表示腿部关节驱动力矩恢复矩阵，/>表示通过系统参考状态求得的前馈力矩。

进一步，姿态控制器控制量τ_leg表示为：

其中，kp_leg和kd_leg为腿部PD控制增益参数矩阵。

本发明的有益效果：

本发明的方法针对现有技术中存在的不足，为轮式建立与整个系统解耦的轮-倒立质心摆模型，利用轮-倒立质心摆简化模型和腿式全身动力学模型的组合；通过模型优化控制运动速度和轮式平衡稳定；通过基于参考位姿和反馈状态的全身动力学补偿控制腿部上身姿态。在实时稳定轮式运动的同时能够精确控制机器人上身位置、姿态。通过低维系统优化控制，可以实现快速实时性极高的稳定性能；为双足机器人建立全身动力学模型通过动力学补偿实现精确的位姿控制。

附图说明

图1是现有双足机器人示意图；

图2是本发明双足机器人姿态控制框图示意图；

图3是本发明轮式双足机器人动力学模型示意图；

图4是本发明轮式双足机器人平衡控制的轮-倒立质心摆模型示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用于解释本发明，并不用于限定本发明。

一种轮式双足机器人姿态控制系统，包括姿态控制器和平衡控制器；所述姿态控制器包括姿态解算单元和全身动力学补偿控制单元，姿态解算单元接收给定上身躯干的参考姿态矩阵R_ref(包括俯仰pitch、翻滚roll和偏航yaw角度)、躯干相对轮轴的参考位置p_ref以及状态估计得到的机器人状态；姿态解算单元进行姿态解算后经全身动力学补偿控制单元 (WBD-C)得到姿态控制量τ_leg，即机器人腿部各关节驱动力矩，从而控制身体姿态。

所述平衡控制器接收给定轮式双足机器人移动的参考速度v_ref、状态估计得到的机器人质心相对轮轴的位置d_com以及质心相对世界坐标系的实际速度v_com，形成反馈控制率得到平衡调节控制量τ_wheel，即轮子的驱动力矩，从而调整机器人的运动平衡，当参考速度v_ref＝0时为静止平衡态。

平衡控制器的构造方法：带有轮子的双足机器人刚体动力学系统如图3所示，当上身运动不剧烈即缓慢变换时，轮以上部分可以简化为图4所示的轮-倒立质心摆模型，这个模型的几何构型并不是固定的，而是有当前机器人状态得到的。该模型由两个刚体组成，一个是由当前机器人状态下上身简化的质心刚体，具有质量为m_com、惯量矩阵为I_com的动力学参数；另一个是轮子刚体，同样具有质量为m_wheel和惯量为I_wheel信息。两个刚体通过无质量的结构连接形成轮-倒立质心摆模型(Wheel-Inverted Center of Mass Pendulum:W-ICoMP)，这个模型显然在不控制的情况下无法站立稳定。

通过状态估计和运动学可以获得质心刚体相对轮轴的位置速度信息，通过轮子电机的码盘信息获得角度和转速信息。设轮-倒立质心摆模型状态变量为ξ_WIP＝[x_com；z_com；θ_cw；θ_wheel]，其中，x_com,z_com,θ_cw,θ_wheel分别为质心在世界坐标系的x方向的位置、z方向的位置、质心与轮轴连线相对竖直方向的角度以及轮子转动的角度。设分别是上述物理量的一阶微分速度量，二阶微分加速度量表示为/>则状态变量的一阶微分和二阶微分形式分别为/>利用拉格朗日动力学可以推导出该轮-倒立质心摆模型动力学方程为：

其中，M(ξ_WIP)是由状态变量决定的系统惯量矩阵，是由状态变量及其一阶微分决定的哥氏力、重力项矩阵，B是驱动力矩选择矩阵，J_wheel是轮子边缘与地面接触点相对世界坐标系的雅克比矩阵，(·)^T是矩阵中的转置变换，λ_ext是该接触点的外力。因此整个系统的输入为轮子的驱动力矩τ_wheel，即平衡调节控制器的控制量，而这个量是由接下来的模型优化根据当前机器人状态和参考值得到的。

设平衡控制器的优化变量为建立控制目标函数。整个平衡控制器的目的就是控制质心处的运动速度趋向参考速度v_ref，在这里我们通过控制质心相对轮轴的水平(x方向)距离d_com间接控制实际行进速度v_com。这就像平衡车的原理：人身体前倾往前走、后倾往后走，而倾斜的越厉害行进的速度就越快。因此参考的水平距离/>为：

其中，α是速度调节增益系数。另外在优化过程中要求轮子的驱动力矩τ_wheel尽量小，且两次调节之间调节量跳变要小，则优化目标函数为

其中，w₁,w₂,w₃均是优化权重系数，β是距离调节增益系数，J_com是质心相对世界坐标系的雅克比矩阵而是其一阶微分，/>是轮子驱动力矩的上一次调节量。优化的约束包括动力学方程约束、轮子驱动能力约束和接触点的摩擦锥约束。

动力学约束为：

轮子驱动约束：τ_min和τ_max分别是电机输出的最小力矩值与最大力矩值；

接触点摩擦锥约束：其中μ为摩擦因数。

每个控制周期，都会根据机器人状态得到轮-倒立质心摆状态，经过优化计算最终优化量ε中的力矩值就是平衡控制器的调节量τ_wheel。到此平衡控制器结束。

姿态控制器的构造方法为：由于轮子以上结构具有多连杆串联结构特点，因此姿态的控制需要更精确的建模。如图3所示，设全身动力学模型的系统状态变量为χ_WBD＝[[x_b,y_b,z_b,r_b,p_b,ya_b]^T；q_leg]，分别是上身躯干质心处世界坐标系下的x方向位置x_b和角度r_b、y方向位置y_b和角度p_b、z方向位置z_b和角度ya_b，q_leg包含两条腿各关节的角度。同样各变量的一阶微分即速度和二阶微分即加速度分别表示为系统状态变量的一阶微分和二阶微分表示为/>则使用拉格朗日动力学构建全身动力学方程：

其中，D(χ_WBD)是由机器人状态变量决定的系统惯量矩阵，是由状态变量及其一阶微分决定的哥氏力矩阵，G(χ_WBD)是重力项矩阵，Φ是腿部驱动力矩选择矩阵即前6 行元素为零，τ_leg为腿部各关节驱动力矩，/>分别是右、左腿末端相对世界坐标系的雅克比矩阵，/>和/>分别是右、左腿末端与地面接触点收到的外部作用力。

当给定上身参考位置和姿态/>时，/>表示三维向量，由于双脚是着地的，所以通过逆运动学的姿态解算可以计算求解出腿部各关节的参考角度为/>之后通过与上一控制周期的参考值数值微分可求出关节的参考速度/>因此得到模型的参考状态变量的参考值/>及其一阶微分变量/>形式，而对于其二阶微分量/>需要进一步详细说明。由于对腿部关节角度的再次微分会引入噪声和误差，因此设置/>对于上身的位置和姿态参考加速度通过PD控制器得到：

其中，kp_tor和kd_tor为PD控制增益参数矩阵。所以由PD控制器输入的六个量和组成。因此可以得到全身动力学补偿控制器：

其中，表示腿部关节驱动力矩恢复矩阵，/>表示通过系统参考状态求得的前馈力矩。为了增加驱动关节的姿态控制精度，在前馈力矩基础上增加关节层的PD控制器得到最终的姿态控制器控制量τ_leg(即腿部各关节的驱动力矩)：

其中，kp_leg和kd_leg为腿部PD控制增益参数矩阵。姿态控制至此结束。

单次控制周期中平衡控制器和姿态控制器的调节量分别给轮子电机和腿部关节电机发送力矩指令，完成一次调控。

以上实施例仅用于说明本发明的设计思想和特点，其目的在于使本领域内的技术人员能够了解本发明的内容并据以实施，本发明的保护范围不限于上述实施例。所以，凡依据本发明所揭示的原理、设计思路所作的等同变化或修饰，均在本发明的保护范围之内。

Claims (8)

1.一种轮式双足机器人姿态控制方法，其特征在于，分别构造姿态控制器和平衡控制器；所述姿态控制器包括姿态解算和全身动力学补偿控制，姿态解算接收给定上身躯干的参考姿态矩阵R_ref、躯干相对轮轴的参考位置p_ref以及状态估计得到的机器人状态；进行姿态解算后经全身动力学补偿控制单元得到姿态控制量τ_leg，即机器人腿部各关节驱动力矩，从而控制身体姿态；

所述平衡控制器接收给定轮式双足机器人移动的参考速度v_ref、状态估计得到的机器人质心相对轮轴的位置d_com以及质心相对世界坐标系的实际速度v_com，形成反馈控制率得到平衡调节控制量τ_wheel，即轮子的驱动力矩，从而调整机器人的运动平衡；

平衡控制器的构造方法为：

S1、将当前机器人状态下上身简化为质心刚体，底部为轮子刚体，两个刚体之间通过无质量的结构连接形成轮-倒立质心摆模型；

S2、设平衡控制器的优化变量ε，建立控制目标函数及优化的约束条件；设平衡控制器的优化变量为目标函数表示为：

其中，w₁,w₂,w₃均是优化权重系数，β是距离调节增益系数，J_com是质心相对世界坐标系的雅克比矩阵而是其一阶微分，/>是轮子驱动力矩的上一次调节量；

S3、在每个控制周期内，根据机器人状态得到轮-倒立质心摆状态，经过优化计算最终优化量ε中的力矩值，即平衡控制器的调节量τ_wheel。

2.根据权利要求1所述的一种轮式双足机器人姿态控制方法，其特征在于，轮-倒立质心摆模型的动力学方程表示为：

其中，ξ_WIP是轮-倒立质心摆模型的状态变量，分别是状态变量一阶微分和二阶微分，M(ξ_WIP)是由状态变量决定的系统惯量矩阵，/>是由状态变量及其一阶微分决定的科氏力、重力项矩阵，B是驱动力矩选择矩阵，J_wheel是轮子边缘与地面接触点相对世界坐标系的雅克比矩阵，(·)^T是矩阵中的转置运算，λ_ext是接触点的外力；因此整个系统的输入为轮子的驱动力矩τ_wheel。

3.根据权利要求1所述的一种轮式双足机器人姿态控制方法，其特征在于，平衡控制器的约束包括动力学方程约束、轮子驱动能力约束和接触点的摩擦锥约束。

4.根据权利要求3所述的一种轮式双足机器人姿态控制方法，其特征在于，动力学约束为：M(ξ_WIP)是由状态变量决定的系统惯量矩阵，是由状态变量及其一阶微分决定的科氏力、重力项矩阵，B是驱动力矩选择矩阵，J_wheel是轮子边缘与地面接触点相对世界坐标系的雅克比矩阵；

轮子驱动约束：τ_min和τ_max分别是电机输出的最小力矩值与最大力矩值；

接触点摩擦锥约束：其中μ为摩擦因数，λ_ext为接触点的外力。

5.根据权利要求1所述的一种轮式双足机器人姿态控制方法，其特征在于，姿态控制器的构造方法为：

S1、设全身动力学模型的系统状态变量χ_WBD，使用拉格朗日动力学构建全身动力学方程；

S2、将上身的位置和姿态参考加速度输入PD控制器，由PD控制器输入的六个量和角加速度参考值组成/>因此得到全身动力学补偿控制器；/>是系统状态变量参考值的二阶微分；

S3、为了增加驱动关节的姿态控制精度，在前馈力矩基础上增加关节层的PD控制器得到最终的姿态控制器控制量τ_leg。

6.根据权利要求5所述的一种轮式双足机器人姿态控制方法，其特征在于，全身动力学模型的系统状态变量为χ_WBD＝[[x_b,y_b,z_b,r_b,p_b,ya_b]^T；q_leg]，其中，x_b和r_b分别是上身躯干质心处世界坐标系下的x方向位置和角度，y_b和p_b分别是上身躯干质心处世界坐标系下的y方向位置和角度，z_b和ya_b分别是上身躯干质心处世界坐标系下的z方向位置和角度，q_leg包含两条腿各关节的角度；

全身动力学方程表示为：

其中，是系统状态变量的一阶微分，D(χ_WBD)是由机器人状态变量决定的系统惯量矩阵，/>是由状态变量及其一阶微分决定的科氏力矩阵，G(χ_WBD)是重力项矩阵，Φ是腿部驱动力矩选择矩阵即前6行元素为零，τ_leg为腿部各关节驱动力矩，/>分别是右、左腿末端相对世界坐标系的雅克比矩阵，/>和/>分别是右、左腿末端与地面接触点收到的外部作用力。

7.根据权利要求6所述的一种轮式双足机器人姿态控制方法，其特征在于，利用PD控制器得到的六个量表示为：

其中，分别是上身躯干质心处世界坐标系下的x方向、y方向和z方向加速度的参考值，/>分别是上身躯干质心处世界坐标系下的x方向、y方向和z方向角加速度的参考值；kp_tor和kd_tor均为PD控制增益参数矩阵，/>分别是身躯干质心处世界坐标系下的x方向、y方向和z方向的速度，/>分别是身躯干质心处世界坐标系下的x方向、y方向和z方向的角速度；且为了避免引入噪声和误差，故设置两条腿各关节的角加速度参考值/>

得到全身动力学补偿控制器，表示为：

其中，是系统状态变量参考值；/>表示腿部关节驱动力矩恢复矩阵，/>表示通过系统参考状态求得的前馈力矩。

8.根据权利要求7所述的一种轮式双足机器人姿态控制方法，其特征在于，姿态控制器控制量τ_leg表示为：

其中，kp_leg和kd_leg为腿部PD控制增益参数矩阵，为腿部各关节的参考角度，/>为腿部各关节的参考角度速度/>腿部各关节的角度速度。