CN101943912B - 一种消除双足仿人机器人上身姿态晃动的控制方法 - Google Patents

Abstract

本发明提供了一种消除双足仿人机器人上身姿态晃动的控制方法，所述机器人具有腰部及姿态传感器，所述方法包括如下步骤：生成所述机器人的规划步态；通过所述姿态传感器测量所述机器人上身的倾角及角速度；根据所述倾角及角速度计算所述腰部的关节的修正量；根据所述修正量调节所述关节，以消除机器人上身姿态晃动。该方法只是调节腰关节，不影响机器人的着地时间，在机器人整个行走工程或者作业过程中均起作用。

Description

一种消除双足仿人机器人上身姿态晃动的控制方法

技术领域

本发明涉及双足机器人行走或者作业的控制方法，特别是双足机器人行走或者作业时消除上身晃动的控制方法。

背景技术

仿人机器人(以下简称机器人)跟人一样，是靠两条腿的行走实现移动的。它的双腿结构跟人类似，较传统的轮式和履带式机器人有更好的机动性，尤其是在凹凸不平的地面、楼梯以及与地面仅有离散不连续的接触点的场合更体现出优越性。但是双足机器人有本质不稳定的特点，容易摔倒。

为了使机器人行走，需要给定机器人的行走轨迹，即动态步态，包括关节机器人各个关节的角度。机器人的动态步态是一种固有的、周期的运动，是依据双足机器人整体动力学产生的。由于约束条件的耦合性和动力学方程的复杂性，动态步态计算需要一个优化过程。因此，动态步态一般通过离线计算方法来实现。也就是说，动态步态一般是在假设双足机器人模型和周围环境已知的情况下生成的。实际上，双足机器人行走现实环境不可能与设定的环境和条件完全相同，由于机器人周围环境的变化或产生了未知状况，如果机器人机械地按照预先规划好的动态步态完全执行，不对所规划的动态步态进行实时修正和控制，很可能会产生不稳定甚至摔倒等异常现象。因此，必须根据当前的环境信息和机器人当前的自身状态，对规划的动态步态进行修正，进行实时步态控制，克服环境的改变与不确定性，使机器人能在实际环境中稳定行走。

仿人机器人的步行周期分为单脚支撑期和双脚支撑期，单脚支撑期主要进行摆腿运动，双脚支撑期主要进行移腰运动，因此机器人上身的运动不论在其前后方向还是左右方向，其速度都不是恒定的。再加上由于机器人向前移动时腿的摆动，脚着地时对机器人的冲击，造成了机器人上身的剧烈晃动，影响到整个机器人的稳定性；同时，机器人的上肢在作业时，由于负载或者手臂的作业姿态等的变化、扰动，都会造成上身的晃动，在很大程度上影响了机器人手臂作业的稳定性。

申请号为200810171980.6的中国专利中公开了一种调节仿人机器人上身姿态的方法，该方法根据姿态传感器测出的姿态倾角，调节仿人机器人的髋关节，保持上身姿态到规划值。该方法调节髋关节，对机器人摆动腿的着地时间影响很大，并且该方法建立的模型不适用于具有腰部的仿人机器人上身姿态的控制。该方法的计算公式采用积分算法，当机器人实际的姿态偏离规划的姿态时，就调节髋关节，直到二者相等，该方法无法消除上身的晃动。

在论文“Realization of Stretch-legged Walking of the Humanoid Robot”中，仿人机器人的膝关节的运动事先给定，仿人机器人腰关节的作用是给机器人下肢增加自由度，以满足下肢逆运动学的求解。

该方法的腰关节的两个自由度的目的是补充减少的膝关节的自由度，通过逆运动学求解出腰关节的角度，实现机器人的直膝行走，在论文中没有提及用腰关节给上身消除晃动的算法。

发明内容

针对现有技术中的上述技术问题，本发明的目的在于建立了具有腰部的仿人机器人上身的模型，对机器人的上身进行控制，消除上身的晃动。

本发明通过如下的技术方案实现。

一种消除双足仿人机器人上身姿态晃动的控制方法，所述机器人具有腰部及姿态传感器，其中，所述方法包括如下步骤：

生成所述机器人的规划步态，其中规划步态包含了机器人上身的规划姿态，姿态是指机器人上身在前后及/或左右方向上与铅垂线的夹角；

通过所述姿态传感器测量所述机器人上身的倾角及角速度；

根据所述倾角及角速度计算所述腰部的关节的修正量；

根据所述修正量调节所述关节，以消除机器人上身姿态晃动。

根据以上技术方案所述的方法，其中所述腰部的关节具有俯仰和滚转两个自由度。

根据以上技术方案所述的方法，其中所述修正量采用如下方法计算：

Δθ_w＝K_p*(θ_{body_ref}-θ_body)+K_v/(s+a)*ω

Δθ_w是机器人腰关节的修正量，K_p是比例反馈系数，K_v是速度反馈系数，θ_{body_ref}是机器人上身的规划姿态，θ_body是机器人上身的实际姿态，ω是机器人上身的实际角速度，s是拉普拉斯变换的变量，a是低通滤波系数，a通过实验结果选择，如a＝20.0。

经过离散化得到实时修正量Δθ_w(nT)计算如下：

$\left\{\begin{array}{c}y\left(\mathrm{nT}\right)=y\left((n-1)T\right)+K_{v}T(\mathrm{\ω}\left(\mathrm{nT}\right)+\mathrm{\ω}\left((n-1)T\right))/2---\left(1\right)\\ \mathrm{\Δ}{\mathrm{\θ}}_w\left(\mathrm{nT}\right)=K_p({\mathrm{\θ}}_{\mathrm{body}\_\mathrm{ref}}\left(\mathrm{nT}\right)-{\mathrm{\θ}}_{\mathrm{body}}\left(\mathrm{nT}\right)-y\left(\mathrm{nT}\right))---\left(2\right)\end{array}\right.$

其中n的取值是1，2，3……，θ_{body_ref}(nT)是在nT时刻机器人上身的规划姿态，即规划的机器人上身在前后及/或左右方向上与铅垂线的夹角，这个角度预先给定，θ_body(nT)是在nT时刻机器人上身的实际姿态，即机器人上身在前后及/或左右方向上与铅垂线的实际夹角，可以通过陀螺仪测量得到，ω(nT)是机器人在nT时刻上身的角速度，可以通过角速度计测量得到。T是计算机伺服控制系统的采样周期。K_p是比例反馈系数，K_v是速度反馈系数，腰上两个关节的系数根据实验分别调节，如K_p、K_v在前后方向上分别为0.3、0.2，左右方向上分别为0.4、0.35。ω(nT)和ω((n-1)T)分别是机器人在nT和(n-1)T时刻上身的角速度，y(nT)和y((n-1)T)分别是nT和(n-1)T时刻上身角速度经过滤波后的变量

根据以上技术方案所述的方法，其中所述动态步态通过离线计算方法实现。

根据以上技术方案所述的方法，其中所述姿态传感器是陀螺仪和加速度计。

本发明的上述和/或其它方面还通过提供一种双足仿人机器人来实现，所述机器人包括：

腰部，其具有关节；

姿态传感器，其用于测量所述机器人的上身的倾角及角速度；

步态生成部，其用于生成所述机器人的规划步态；

修正量计算部，其根据所述倾角及角速度计算所述腰部的关节的修正量；

关节调节部，其根据所述修正量调节所述关节，以消除机器人上身姿态晃动。

与现有技术相比，本发明能够取得如下有益的技术效果：

1.建立了具有要关节的仿人机器人上身模型，通过对腰关节的控制，消除上身晃动，达到机器人的稳定行走或者双臂作业的要求。

2.既结合了动态步态，保证实现行走步态，又能保持动态稳定。

3.只调节腰关节，不影响摆动腿的着地时间。

附图说明

图1根据本发明的控制器结构图。

图2是根据本发明的上身姿态调整原理图。

具体实施方式

图1示出根据本发明的机器人控制器结构图。它由离线步态规划θ_w0(t)和起反馈作用的Δθ_w(t)构成。步行机器人包括姿态传感器1、上身2、腰关节3(包括俯仰和滚转两个自由度)、下身4及其连接件。速度反馈用到的角速度值需经过低通滤波器5的滤波处理。

机器人实时行走或者作业时，机器人腰关节的参考角度θ_w(t)等于离线规划的动态步态θ_w0(t)加上实时修正量Δθ_w(t)。其中，θ_w0(t)一般是在给定机器人模型和周围环境已知的情况下生成的。Δθ_w(t)是当机器人上身晃动时，为了消除晃动时腰关节的实时修正量。

经过实验，机器人的上身可以简化成一个低阻尼的二阶系统，容易振荡。为了提高响应速度和减小振荡，采用PD控制。

腰关节实时修正量Δθ_w(t)如下：

Δθ_w＝K_p*(θ_{body_ref}-θ_body)+K_v/(s+a)*ω

Δθ_w是机器人腰关节的修正量，K_p是比例反馈系数，K_v是速度反馈系数，θ_{body_ref}是机器人上身的规划姿态，θ_body是机器人上身的实际姿态，ω是机器人上身的实际角速度，s是拉普拉斯变换的变量，a是低通滤波系数，a通过实验结果选择，如a＝20.0。

经过离散化得到实时修正量Δθ_w(nT)计算如下：

$\left\{\begin{array}{c}y\left(\mathrm{nT}\right)=y\left((n-1)T\right)+K_{v}T(\mathrm{\ω}\left(\mathrm{nT}\right)+\mathrm{\ω}\left((n-1)T\right))/2---\left(1\right)\\ \mathrm{\Δ}{\mathrm{\θ}}_w\left(\mathrm{nT}\right)=K_p({\mathrm{\θ}}_{\mathrm{body}\_\mathrm{ref}}\left(\mathrm{nT}\right)-{\mathrm{\θ}}_{\mathrm{body}}\left(\mathrm{nT}\right)-y\left(\mathrm{nT}\right))---\left(2\right)\end{array}\right.$

其中n的取值是1，2，3……，θ_{body_ref}(nT)是在nT时刻机器人上身的规划姿态，即规划的机器人上身在前后及/或左右方向上与铅垂线的夹角，这个角度预先给定，θ_body(nT)是在nT时刻机器人上身的实际姿态，即机器人上身在前后及/或左右方向上与铅垂线的实际夹角，可以通过陀螺仪测量得到，ω(nT)是机器人在nT时刻上身的角速度，可以通过角速度计测量得到。T是计算机伺服控制系统的采样周期。所谓计算机伺服控制系统，即控制机器人的各个关节精确地跟随给定目标值运动的控制系统。K_p是比例反馈系数，K_v是速度反馈系数，腰上两个关节的系数根据实验分别调节，如K_p、K_v在前后方向上分别为0.3、0.2，左右方向上分别为0.4、0.35。ω(nT)和ω((n-1)T)分别是机器人在nT和(n-1)T时刻上身的角速度，y(nT)和y((n-1)T)分别是nT和(n-1)T时刻上身角速度经过滤波后的变量

本发明的实施例还可构造成消除其它能够步行移动的机器人或行进装置的上身姿态晃动的控制。同时，这些能够步行移动的机器人或行进装置也并不限于具有两个腿部的形式。

因此，本领域技术人员应当理解，本发明并不仅限于本文描述或例示的方法或者构造、应用或使用的细节。实际上，任意合适的变型或替代性实施例都应包含在本发明的精神和范围内。

Claims (6)

1.一种消除双足仿人机器人上身姿态晃动的控制方法，所述机器人具有腰部、姿态传感器，以及计算机伺服控制系统，其特征在于，所述方法包括如下步骤：

生成所述机器人的规划步态；

通过所述姿态传感器测量所述机器人上身的倾角及角速度；

根据所述倾角及角速度计算所述腰部的关节的修正量；

根据所述修正量调节所述关节，以消除机器人上身姿态晃动；

所述腰部的关节具有俯仰和滚转两个自由度；

所述修正量采用如下方法计算：

Δθ_w(t)＝K_p*(θ_{body_ref}-θ_body)+K_v/(s+a)*ω

其中Δθ_w是机器人腰关节的修正量，K_p是比例反馈系数，K_v是速度反馈系数，θ_{body_ref}是机器人上身的规划姿态，θ_body是机器人上身的实际姿态，ω是机器人上身的实际角速度，s是拉普拉斯变换的变量，a是低通滤波系数；

经过离散化得到实时修正量Δθ_w(nT)计算如下：

$\left\{\begin{array}{cc}y\left(\mathrm{nT}\right)=y\left((n-1)T\right)+K_{v}T(\mathrm{\ω}\left(\mathrm{nT}\right)+\mathrm{\ω}\left((n-1)T\right))/2& \left(1\right)\\ {\mathrm{\Δ\θ}}_w\left(\mathrm{nT}\right)=K_p({\mathrm{\θ}}_{\mathrm{body}\_\mathrm{ref}}\left(\mathrm{nT}\right)-{\mathrm{\θ}}_{\mathrm{body}}\left(\mathrm{nT}\right)-y\left(\mathrm{nT}\right))& \left(2\right)\end{array}\right.$

其中n取自然数，θ_{body_ref}(nT)是在nT时刻机器人上身的规划姿态，θ_body(nT)是在nT时刻机器人上身的实际姿态，ω(nT)是机器人在nT时刻上身的角速度，T是计算机伺服控制系统的采样周期，K_p是比例反馈系数，K_v是速度反馈系数，ω(nT)和ω((n-1)T)分别是机器人在nT和(n-1)T时刻上身的角速度，y(nT)和y((n-1)T)分别是nT和(n-1)T时刻上身角速度经过滤波后的变量。

2.根据权利要求1所述的方法，其特征在于，所述规划步态通过离线计算方法实现。

3.根据权利要求1-2之一所述的方法，其特征在于，所述姿态传感器是陀螺仪和加速度计。

4.一种双足仿人机器人，包括：

腰部，其具有关节；

姿态传感器，其用于测量所述机器人的上身的倾角及角速度；

计算机伺服控制系统，其用于控制机器人的关节跟随给定目标值运动；

步态生成部，其用于生成所述机器人的规划步态；

修正量计算部，其根据所述倾角及角速度计算所述腰部的关节的修正量；

关节调节部，其根据所述修正量调节所述关节，以消除机器人上身姿态晃动；

所述腰部的关节具有俯仰和滚转两个自由度；

所述修正量采用如下方法计算：

Δθ_w(t)＝K_p*(θ_{body_ref}-θ_body)+K_v/(s+a)*ω

其中Δθ_w是机器人腰关节的修正量，K_p是比例反馈系数，K_v是速度反馈系数，θ_{body_ref}是机器人上身的规划姿态，θ_body是机器人上身的实际姿态，ω是机器人上身的实际角速度，s是拉普拉斯变换的变量，a是低通滤波系数；

经过离散化得到实时修正量Δθ_w(nT)计算如下：

$\left\{\begin{array}{cc}y\left(\mathrm{nT}\right)=y\left((n-1)T\right)+K_{v}T(\mathrm{\ω}\left(\mathrm{nT}\right)+\mathrm{\ω}\left((n-1)T\right))/2& \left(1\right)\\ {\mathrm{\Δ\θ}}_w\left(\mathrm{nT}\right)=K_p({\mathrm{\θ}}_{\mathrm{body}\_\mathrm{ref}}\left(\mathrm{nT}\right)-{\mathrm{\θ}}_{\mathrm{body}}\left(\mathrm{nT}\right)-y\left(\mathrm{nT}\right))& \left(2\right)\end{array}\right.$

其中n取自然数，θ_{body_ref}(nT)是在nT时刻机器人上身的规划姿态，θ_body(nT)是在nT时刻机器人上身的实际姿态，ω(nT)是机器人在nT时刻上身的角速度，T是计算机伺服控制系统的采样周期，K_p是比例反馈系数，K_v是速度反馈系数，ω(nT)和ω((n-1)T)分别是机器人在nT和(n-1)T时刻上身的角速度，y(nT)和y((n-1)T)分别是nT和(n-1)T时刻上身角速度经过滤波后的变量。

5.根据权利要求4所述的机器人，其特征在于，所述步态生成部通过离线计算方法生成所述步态。

6.根据权利要求4-5之一所述的机器人，其特征在于，所述姿态传感器是陀螺仪和加速度计。