CN103112517B - 一种调节四足机器人身体姿态的方法和装置 - Google Patents

Abstract

一种调节四足机器人身体姿态的方法和装置，针对四足机器人的运动特点，通过姿态传感器测量的姿态角，求出机器人绕支撑腿连线的旋转角度，通过在规划轨迹上增加调节量，达到防止机器人倾斜的目的。其中，本发明的调节量必须与支撑腿的连线垂直。采用本发明的方法和装置，可以不需要精确测量机器人质心的位置，特别适用于四足机器人身体姿态的调节。

Description

一种调节四足机器人身体姿态的方法和装置

技术领域

本发明涉及一种四足机器人的控制方法，特别是调节四足机器人身体姿态的方法和装置。

背景技术

四足机器人作为多足机器人的一种，具有极强的对室外复杂地形的适应能力，近年来成为了多足机器人研究领域的研究热点之一。四足机器人在进行移动时，有其固定的步态，此步态是一个周期性的运动。将机器人的四条腿分为两组，左前腿和右后腿为一组，右前腿和左后腿为一组。这两组腿交替支撑、迈步，使机器人移动。

为了使四足机器人运动，需要预先给定四足机器人的运动轨迹，即所有关节的角度。一般将机器人简化为一个质点，将零力矩点ZMP点设在支撑腿的连线上，然后根据倒立摆模型，规划出质心的运动轨迹，最后产生全身的运动轨迹。但是四足机器人是一个极其复杂的系统，它的精确的质量分布很难测量得到，因此简化的质点的位置不是非常精确。当机器人按照预先设定的轨迹运动时，实际期望合外力的ZMP点不在支撑腿的连线上，机器人会向着期望的ZMP点的一侧倾斜，导致此侧的摆动腿抬不起来，严重影响机器人的运动。即使不使用简化模型，而是使用全身的动力学模型，也是由于同样的问题，即机器人的质量分布很难准确测量，规划出的轨迹对应的期望ZMP点很难在支撑腿的连线上，最后导致机器人的倾斜。

最显而易见的解决方案是通过一定的方式测量机器人的质心，然后通过配重，调节机器人质心的位置，尽量测出机器人准确的质心位置。

给机器人配重并测量质心的方法比较直观，但是会带来很多问题。

1)在进行机器人硬件设计时，由于各个关节的驱动能力的限制，为了提高机器人的灵活性，往往进行机器人硬件的轻量化设计。但是给机器人配重会明显增加机器人的重量，导致机器人的移动能力的下降。

2)机器人的结构比较复杂，测量质心的位置是一个比较繁琐的工作，而且很困难。当机器人运动时，机器人的形态发生变化，质心也就相应变化，这意味着，在某种姿态下配重过后的质心位置满足要求，当运动到另一种姿态时，质心的位置就很难满足要求。

在专利号为200810172220.7的专利中提出了一种防止仿人机器人左右倾斜的运动规划方法和装置，此方法和装置通过姿态传感器测量机器人的姿态，根据测得的姿态调节机器人支撑腿的髋关节，达到防止身体左右倾斜的目的。但是此方法不适用于四足机器人，因为四足机器人的自由度配置、运动步态、控制方法与双足机器人存在很大的区别。比如双足机器人在单脚支撑期时是一个开放链式系统，对机器人支撑腿的髋关节的调节，不会影响其他关节的运动，但是四足机器人是一个闭环链式系统，对机器人的髋关节的调节，会影响其他关节的运动。

在申请号为201210303079.6的专利中公开了一种四足机器人运动控制器，该控制器主要是对四足机器人进行硬件层面上的设计，而没有涉及到任何控制算法。

在申请号为201210054721.1的专利中公开了一种四足机器人的时位控制方法，该方法主要依据传感器信息控制机器人的支撑腿的位置和摆动腿的落脚点，达到控制机器人速度的目的，然而没有涉及如何根据传感器信息控制机器人姿态，达到调节机器人身体姿态的目的。

本发明针对四足机器人的在双腿支撑期时是一个闭环链式系统的特点，根据机器人的实际姿态，调节两条支撑腿相对于机器人驱赶的位置，达到防止机器人倾斜的目的。

发明内容

四足机器人在运动过程中处于双脚支撑期时，如果所期望的合外力的ZMP(zeromomentpoint)点不在两条支撑腿的连线上，机器人会向一侧倾斜，容易导致此侧的摆动腿一直与地面接触，使机器人无法按照预期的轨迹运动或者不稳定。

本发明的第一方面在于一种方法，包括：步骤1：通过姿态传感器，测量出四足机器人每条支撑腿绕x、y、z轴的姿态角，得到四足机器人的姿态矩阵；步骤2：通过姿态矩阵，获取四足机器人绕x轴倾斜的角度ψ，根据角度ψ，求出支撑腿的调节量Δp(t)；步骤3：四足机器人每条支撑腿的参考位置p(t)等于离线规划的轨迹p₀(t)加上实时的调节量Δp(t)。其中步骤1具体包括：测量出了机器人绕x、y、z轴的姿态角分别为γ，β和α，此时机器人的姿态矩阵为：R_xyz(γ,β,α)＝R_z(α)R_Y(β)R_X(γ)；此姿态矩阵在坐标系中的表示为 $R_{o_1}=R_z\left(\mathrm{\θ}\right)R_{\mathrm{xyz}}(\mathrm{\γ},\mathrm{\β},\mathrm{\α})=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right],$ 其中，L为前后腿之间的距离，W为左右腿之间的距离。其中步骤2具体包括：通过姿态矩阵求出四足机器人绕x轴倾斜的角度为调节量Δp(t)＝H(-K_pψ-K_iΔψ)，其中H为坐标系原点到地面的距离，K_p为比例系数、K_i为积分系数。其中步骤3具体包括：调节支撑腿1为左前腿的参考位置为：

$\left\{\begin{array}{c}{p}_x^1\left(t\right)=p_{0x}^1\left(t\right)-\mathrm{\Δp}\left(t\right)\sin \left(\mathrm{\θ}\right)\\ p_y^1\left(t\right)=p_{0y}^1\left(t\right)+\mathrm{\Δp}\left(t\right)\cos \left(\mathrm{\θ}\right)\end{array}\right.$

其中，表示左前腿在x轴方向的参考位置，表示左前腿在x轴方向的离线规划的轨迹，表示左前腿在y轴方向的参考位置，表示左前腿在y轴方向的离线规划的轨迹，采用同样方法，调节另一支撑腿的参考位置。

本发明的第二方面在于一种装置，包括：姿态传感器：用于获取四足机器人每条支撑腿绕x、y、z轴的姿态角，根据姿态角得到四足机器人的姿态矩阵；调节量计算器：用于根据姿态矩阵，求出机器人绕x轴的角度ψ，根据角度ψ，求出支撑腿的调节量Δp(t)；位置调节器：用于获取四足机器人每条支撑腿的参考位置p(t)，其中，参考位置p(t)等于离线规划的轨迹p₀(t)加上实时的调节量Δp(t)。其中姿态传感器具体用于：测量出了机器人绕x、y、z轴的姿态角分别为γ，β和α，此时机器人的姿态矩阵为：R_xyz(γ,β,α)＝R_z(α)R_Y(β)R_X(γ)；此姿态矩阵在坐标系中的表示为 $R_{o_1}=R_z\left(\mathrm{\θ}\right)R_{\mathrm{xyz}}(\mathrm{\γ},\mathrm{\β},\mathrm{\α})=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right],$ 其中，L为前后腿直接的距离，W为左右腿直接的距离。其中调节量计算器具体用于：通过姿态矩阵求出机器人绕x轴倾斜的角度为调节量Δp(t)＝H(-K_pψ-K_iΔψ)，其中H为坐标系原点到地面的距离，K_p为比例系数、K_i为积分系数。其中位置调节器具体用于：

调节左前腿的参考位置为：

$\left\{\begin{array}{c}{p}_x^1\left(t\right)=p_{0x}^1\left(t\right)-\mathrm{\Δp}\left(t\right)\sin \left(\mathrm{\θ}\right)\\ p_y^1\left(t\right)=p_{0y}^1\left(t\right)+\mathrm{\Δp}\left(t\right)\cos \left(\mathrm{\θ}\right)\end{array}\right.$

其中，表示左前腿在x轴方向的参考位置，表示左前腿在x轴方向的离线规划的轨迹，表示左前腿在y轴方向的参考位置，表示左前腿在y轴方向的离线规划的轨迹，调节另一支撑腿(即右后腿)的参考位置。

本发明针对四足机器人的运动特点，通过安装在机器人胸腔中的陀螺仪检测机器人身体的倾斜角度，并根据测量得到的角度调节机器人的两条支撑腿相对于身体的位置，达到防止机器人向一侧倾斜的目的。

附图说明

图1展示了本发明调节四足机器人身体姿态的流程图。

图2展示了本发明通过姿态角获取调节量的示意图。

具体实施方式

本发明针对的四足机器人由一个躯干和四条腿，分别为3左前腿、4右前腿、5右后腿、6左后腿，如图1所示，躯干2中装有姿态传感器1(陀螺仪)，用来测量机器人姿态，每条腿至少有三个自由度，包括一个左右摆动的自由度和两个前后摆动的自由度，用来实现机器人在三维空间中的运动。

将机器人的四条腿分为两组，左前腿和右后腿为一组，右前腿和左后腿为一组，当其中一组腿支撑时，另一组腿迈步，反之亦然。建立机器人的局部笛卡尔坐标系，机器人的前方为x轴方向，机器人左边为y轴方向，竖直向上为z轴方向，o表示坐标系的原点。机器人的四条腿的位置在此坐标系中表示，通过逆运动学可以解出所有关节的伺服参考角度。

通过姿态传感器，测量出了机器人绕x、y、z轴的姿态角分别为γ，β和α，此时机器人的姿态矩阵为

R_xyz(γ,β,α)＝R_z(α)R_Y(β)R_X(γ)。

将γ，β和α输入至平衡控制器，得到调节量Δp(t)，利用调节量对离线规划的轨迹p₀(t)进行修正，得到参考轨迹p(t)；

机器人每条腿的参考轨迹p(t)等于离线规划的轨迹p₀(t)加上实时的调节量Δp(t)。其中p₀(t)是根据机器人的粗略模型生成的，Δp(t)是当机器人倾斜时，为了克服此倾斜的实时调节量。求Δp(t)时，必须考虑四足机器人的特点，即闭环链式系统，在修正规划值时，必须考虑运动学的约束，因此修正量必须与双腿的连线垂直，否者机器人将会出现绕竖直方向上的旋转，导致机器人无法平衡。

图2展示了本发明通过姿态角获取调节量的示意图。首先以机器人的左前腿和右后腿支撑为例，求Δp(t)。如图2所示，前后腿之间的距离为L，左右腿之间的距离为W，坐标原点到地面的垂直距离为H。对于左前腿3，建立坐标系的x轴(标记为x₁)与左前腿和右后腿的连线平行，z轴(标记为z₁)与坐标系Σ_o的z轴重合，y轴(标记为y₁)与x轴、z轴符合建立右手直角坐标系准则。

其中通过姿态传感器，测量出了机器人绕x、y、z轴的姿态角分别为γ，β和α，此时机器人的姿态矩阵为

R_xyz(γ,β,α)＝R_z(α)R_Y(β)R_X(γ)。

此姿态矩阵在坐标系中的表示为

$R_{o_1}=R_z\left(\mathrm{\θ}\right)R_{\mathrm{xyz}}(\mathrm{\γ},\mathrm{\β},\mathrm{\α})=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right].$

通过姿态矩阵求出机器人绕x₁倾斜的角度为为了防止机器人倾斜，需要根据角度ψ，求出支撑脚的调节量。我们采用PI控制器，即本专利提到的平衡控制器，调节量

Δp(t)＝H(-K_pψ-K_iΔψ)，其中

H为坐标系原点到地面的距离，K_p为比例系数、K_i为积分系数。

当支撑腿变为摆动腿后，Δp(t)慢慢的变为0。

左前腿的参考位置为：

$\left\{\begin{array}{c}{p}_x^1\left(t\right)=p_{0x}^1\left(t\right)-\mathrm{\Δp}\left(t\right)\sin \left(\mathrm{\θ}\right)\\ p_y^1\left(t\right)=p_{0y}^1\left(t\right)+\mathrm{\Δp}\left(t\right)\cos \left(\mathrm{\θ}\right)\end{array}\right.$

通过同样的方法，可以求出右后腿的参考位置为：

$\left\{\begin{array}{c}{p}_x^3\left(t\right)=p_{0x}^3\left(t\right)-\mathrm{\Δp}\left(t\right)\sin \left(\mathrm{\θ}\right)\\ p_y^3\left(t\right)=p_{0y}^3\left(t\right)+\mathrm{\Δp}\left(t\right)\cos \left(\mathrm{\θ}\right)\end{array}\right.$

同理，当机器人的右前腿和左后腿支撑时，也能求出其调节量。

通过增加修正量Δp(t)，防止机器人向一边倾斜，保证机器人按照预先步态稳定行走。

本发明通过姿态传感器测量的姿态角，求出机器人绕支撑腿连线的旋转角度，通过在规划轨迹上增加调节量，达到防止机器人倾斜的目的。其中，本发明的调节量必须与支撑腿的连线垂直。采用本发明的方法和装置，可以不需要精确测量机器人质心的位置，特别适用于四足机器人身体姿态的调节。

Claims (2)

1.一种调节四足机器人身体姿势的方法，其特征在于，所述四足机器人包括一个躯干和四条腿，所述四条腿分别为左前腿、右前腿、左后腿和右后腿，所述四条腿分为两组，所述左前腿和所述右后腿为一组，所述右前腿和所述左后腿为一组，当其中一组退支撑时，另一组腿迈步，反之亦然；所述方法包括：

步骤1：通过姿态传感器，测量出四足机器人每条支撑腿绕x、y、z轴的姿态角γ，β和α，得到四足机器人的姿态矩阵，即R_xyz(γ,β,α)＝R_z(α)R_Y(β)R_X(γ)；

此姿态矩阵在坐标系中的表示为

$R_{o_1}=R_z\left(\mathrm{\θ}\right)R_{\mathrm{xyz}}(\mathrm{\γ},\mathrm{\β},\mathrm{\α})=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right],$

其中，L为前后腿之间的距离，W为左右腿之间的距离；

步骤2：通过姿态矩阵，获取四足机器人绕x轴倾斜的角度ψ，根据角度ψ，求出支撑腿的调节量Δp(t)，

通过姿态矩阵求出四足机器人绕x轴倾斜的角度为

调节量Δp(t)＝H(-K_pψ-K_iΔψ)，其中

H为坐标系原点到地面的距离，K_p为比例系数、K_i为积分系数；

步骤3：四足机器人每条支撑腿的参考位置p(t)等于离线规划的轨迹p₀(t)加上实时的调节量Δp(t)，

调节支撑腿1为左前腿的参考位置为：

$\left\{\begin{array}{c}{p}_x^1\left(t\right)=p_{0x}^1\left(t\right)-\mathrm{\Δp}\left(t\right)\sin \left(\mathrm{\θ}\right)\\ p_y^1\left(t\right)=p_{0y}^1\left(t\right)+\mathrm{\Δp}\left(t\right)\cos \left(\mathrm{\θ}\right)\end{array}\right.$

其中，表示左前腿在x轴方向的参考位置，表示左前腿在x轴方向的离线规划的轨迹，表示左前腿在y轴方向的参考位置，表示左前腿在y轴方向的离线规划的轨迹，

采用同样方法，调节另一支撑腿的参考位置。

2.一种调节四足机器人身体姿势的装置，其特征在于，所述四足机器人包括一个躯干和四条腿，所述四条腿分别为左前腿、右前腿、左后腿和右后腿，所述四条腿分为两组，所述左前腿和所述右后腿为一组，所述右前腿和所述左后腿为一组，当其中一组退支撑时，另一组腿迈步，反之亦然；所述装置包括：

姿态传感器：用于获取四足机器人每条支撑腿绕x、y、z轴的姿态角，根据姿态角得到四足机器人的姿态矩阵；

其中姿态传感器具体用于：

测量出了机器人绕x、y、z轴的姿态角分别为γ，β和α，此时机器人的姿态矩阵为：

R_xyz(γ,β,α)＝R_z(α)R_Y(β)R_X(γ)；

此姿态矩阵在坐标系中的表示为

$R_{o_1}=R_z\left(\mathrm{\θ}\right)R_{\mathrm{xyz}}(\mathrm{\γ},\mathrm{\β},\mathrm{\α})=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right],$

其中，L为前后腿之间的距离，W为左右腿之间的距离；

调节量计算器：用于根据姿态矩阵，求出机器人绕x轴的角度ψ，根据角度ψ，求出支撑腿的调节量Δp(t)；

其中调节量计算器具体用于：

通过姿态矩阵求出机器人绕x轴倾斜的角度为

调节量Δp(t)＝H(-K_pψ-K_iΔψ)，其中

H为坐标系原点到地面的距离，K_p为比例系数、K_i为积分系数；

位置调节器：用于获取四足机器人每条支撑腿的参考位置p(t)，其中，参考位置p(t)等于离线规划的轨迹p₀(t)加上实时的调节量Δp(t)；

其中位置调节器具体用于：

调节支撑腿1为左前腿的参考位置为：

$\left\{\begin{array}{c}{p}_x^1\left(t\right)=p_{0x}^1\left(t\right)-\mathrm{\Δp}\left(t\right)\sin \left(\mathrm{\θ}\right)\\ p_y^1\left(t\right)=p_{0y}^1\left(t\right)+\mathrm{\Δp}\left(t\right)\cos \left(\mathrm{\θ}\right)\end{array}\right.$

其中，表示左前腿在x轴方向的参考位置，表示左前腿在x轴方向的离线规划的轨迹，表示左前腿在y轴方向的参考位置，表示左前腿在y轴方向的离线规划的轨迹，调节另一支撑腿的参考位置。