CN101052929A - 倒立二轮行走型机器人及其控制方法 - Google Patents

Abstract

一种倒立二轮行走型机器人，其包括：车体；两个车轮，其同轴配置于车体；驱动装置，其分别驱动车轮；第一状态检测机构，其检测车体的倾斜角度以及倾斜角速度中的至少一个；第二状态检测机构；其检测车轮的旋转角度以及旋转角速度中的至少一个；车体受拘束识别机构，其检测车体的旋转是否受到拘束；和控制机构，其决定向所述驱动装置的指令值。根据车体受拘束识别机构的检测结果，控制机构决定使用于车体旋转的转矩的比例变化的指令值。

Description

倒立二轮行走型机器人及其控制方法

技术领域

本发明涉及进行货物搬运作业的机器人中使用同轴二轮车的倒立二轮行走型机器人及其控制方法，更详细地说，是涉及机器人姿势受拘束时的倒立二轮行走型机器人及其控制方法。

背景技术

作为进行货物搬运作业的机器人，一般都是三轮型或四轮型等必须以三点以上进行接触来保持稳定。但是，关于三轮型、四轮型等由于具有面积和难以转小弯，因而正在研究基于倒立摆模型进行动作的同轴二轮车型的机器人。

作为同轴二轮车型的移动机器人，公知例如专利文献1、专利文献2中记载的机器人。

图13是表示上述专利文献1中记载的现有的同轴二轮车型的移动机器人。

专利文献1中记载的同轴二轮车型移动机器人包括：车体104，其可以转动地支撑于车轴101上，该车轴101在其两端具有一对车轮102、103；安装于车体104的车轮驱动用电动机105；向车轮驱动用电动机105发出动作指令的控制计算机106；和检测车体104的倾斜度的角度检测机构107。并进行状态反馈控制，其中以短时间间隔采样由角度检测机构107检测出的车体104的倾斜角度，将车体104的倾斜角度作为状态变量输入值，将反馈增益K作为系数，基于预先在控制计算机106内设定的控制输入计算式，计算车轮驱动用电动机105的控制转矩。由控制计算机106对车轮驱动用电动机105作出指令进行与所述算出的控制转矩相当的动作，来维持倒立的姿势控制。

图14是表示上述专利文献2中记载的载人型的同轴二轮车型移动机器人的图。

专利文献2中记载的载人型的同轴二轮车型移动机器人通过根据姿势感知传感器的输出控制同轴配于平台201上的左右驱动轮，由此来进行用于保持前后方向平衡的姿势控制和行走控制。而且包括：在驱动轮202的前方侧以及后方侧接地的辅助轮203；和使辅助轮203伸缩的辅助轮驱动部204。而且，由于具有障碍物探测传感器205和根据行走速度以及姿势的控制状态来驱动辅助轮驱动部204而使辅助轮203伸缩的控制电路，所以当存在容易导致跌倒的障碍物时，由于辅助轮203伸出提高了稳定性，所以可以减少跌倒的危险性。

专利文献1：日本特许2530652号

专利文献2：日本特开2004-74814

在上述专利文献1中公开的技术中，经常进行状态反馈控制。

而且，当同轴二轮车的车体104和车轮102、103可以共同无拘束地相对旋转时，在车轮驱动用电动机105产生的转矩用于车轮102、103的旋转，与此同时，反作用的转矩作用于车体104，从而可维持整体倒立姿势控制。

但是，若车体104的一部分与墙壁或地面接触，而从外部受到不能继续旋转的拘束力时，则车体104不旋转，不仅如此，拘束力带来的转矩被传递到车轮102、103。由此，与没有拘束力的情况相比，给车轮102、103带来过大的转矩，从而存在车轮旋转增加、行走速度急剧上升的问题。

在上述的专利文献2中公开的技术中，由于具有当由障碍物探测传感器205探测到障碍物时驱动辅助轮驱动部204使辅助轮203伸缩的机构，所以当辅助轮203伸出时，左右的驱动轮202和辅助轮203三点接地，从而可以减少危险性。

但是，在该技术中，采用的结构是通过识别障碍物而仅伸出辅助轮203，速度和姿势依赖于人的操纵。在该结构中同样存在下述问题：当人失去平衡而使得包含平台的主体部与地面接触时，或包含平台201的主体部与墙壁等接触而受到不能旋转的拘束力时，与专利文献1的情况同样对驱动轮202施加了过大的转矩，使得行走速度急剧上升。在该情况下，存在下述问题：虽然通过人的操纵来发送停止指令等来避免危险，但在操作依赖于人这一点上给用户增加了负担。

发明内容

本发明的目的在于解决上述现有的问题，提供一种在进行倒立控制的同时进行移动的同轴二轮车的车体的一部分与地面或墙壁接触而受到拘束力时，可以维持姿势控制和行走状态并且不会给用户带来负担的安全的倒立二轮行走型机器人及其控制方法。

为了达到上述目的，本发明按照下述构成。

根据本发明的第一方式，提供一种倒立二轮行走型机器人，其包括：

车体；

两个车轮，其同轴配置于所述车体；

驱动装置，其分别驱动所述车轮；

第一状态检测机构，其检测所述车体的倾斜角度以及倾斜角速度中的至少一个；

第二状态检测机构；其检测所述车轮的旋转角度以及旋转角速度中的至少一个；

车体受拘束识别机构，其将所述第一状态检测机构或第二状态检测机构检测出的信息作为输入，检测向所述车体的倾斜方向的旋转是否受到拘束；和

控制机构，其决定向所述驱动装置的指令值，通过所决定的指令值切换多个行走控制方法来驱动所述驱动装置而进行所述车体的行走控制，

根据所述车体受拘束识别机构的检测结果，所述控制机构决定对切换所述多个行走控制方法的所述驱动装置的指令值，通过所决定的所述指令值驱动所述驱动装置，进行所述车体的行走控制来维持行走状态。

根据本发明的第十方式，提供一种倒立二轮行走型机器人的控制方法，其包括下述步骤：

检测车体的倾斜角度以及倾斜角速度中至少一个作为第一状态检测信息；

检测所述车轮的旋转角度以及旋转角速度中至少一个作为第二状态检测信息；

将所述第一状态检测信息或所述第二状态检测信息作为输入，检测所述车体向倾斜方向的旋转是否被拘束来作为车体受拘束识别信息；

基于上述所检测出的车体受拘束识别信息，控制机构切换多个行走控制方法，决定向分别对同轴配置于所述车体的两个车轮进行驱动的驱动装置的指令值，根据所决定的所述指令值切换多个行走控制方法，驱动所述驱动装置，进行所述车体的行走控制来维持行走状态。

根据本发明的构成，在倒立地一边进行姿势控制一边进行行走的倒立二轮行走型机器人的车体的一部分与地面或墙壁等接触使旋转受到拘束的情况下，通过车体受拘束识别机构或车体受拘束识别动作识别车体的旋转方向的拘束。然后根据所识别的拘束状态使例如用于车体旋转的转矩的比例变化，从而不会对车轮施加过大的转矩来维持姿势控制，可以避免行走速度急剧上升等的问题。

由此，根据本发明的构成，在一边进行倒立控制一边进行移动的同轴二轮车型的倒立二轮行走型机器人中，即使在车体的一部分与地面或墙壁接触而是旋转受到拘束的情况下，也可以维持姿势控制或行走状态，不会给用户带来负担，可以安全地进行移动。

附图说明

本发明的上述和其他的目的以及特征通过与添加的附图的优选实施方式相关联的下面的记述能够明确。在这些附图中：

图1A是本发明的第一实施方式中的倒立二轮行走型机器人的倾斜状态下的侧视图；

图1B是本发明的第一实施方式中的倒立二轮行走型机器人的控制部等的框图；

图2是本发明的第一实施方式中的倒立二轮行走型机器人的倾斜状态下的主视图；

图3是本发明的第一实施方式中的倒立二轮行走型机器人的移动构成的说明图；

图4是本发明的第一实施方式中的倒立二轮行走型机器人的机构参数图；

图5是本发明的第一实施方式中的倒立二轮行走型机器人的控制框图；

图6是本发明的第一实施方式中的倒立二轮行走型机器人的回旋控制模式图；

图7是本发明的第一实施方式中的行走控制整体图；

图8是本发明的第二实施方式中的倒立二轮行走型机器人的流程图；

图9是本发明的第一实施方式中的倒立二轮行走型机器人的接触状态图；

图10是本发明的第二实施方式中的倒立二轮行走型机器人的流程图；

图11是本发明的第三实施方式中的倒立二轮行走型机器人的车体倾斜地与地面接触时的说明图；

图12是本发明的第一实施方式中的倒立二轮行走型机器人的流程图；

图13是现有的专利文献1中记载的同轴二轮车型的移动机器人的图；

图14是现有的专利文献2中记载的载人型的同轴二轮车型移动机器人的图。

具体实施方式

在继续本发明的描述之前，在添加附图中对相同部件赋予相同的参考符号。

下面，在说明本发明的实施方式之前，对本发明的各方式进行说明。

本发明的方式，提供一种倒立二轮行走型机器人，其包括：

车体；

两个车轮，其同轴配置于所述车体；

驱动装置，其分别驱动所述车轮；

第一状态检测机构，其检测所述车体的倾斜角度以及倾斜角速度中的至少一个；

第二状态检测机构，其检测所述车轮的旋转角度以及旋转角速度中的至少一个；

车体受拘束识别机构，其将所述第一状态检测机构或第二状态检测机构检测出的信息作为输入，检测向所述车体的倾斜方向的旋转是否受到拘束；和

控制机构，其决定给所述驱动装置的指令值，通过所决定的指令值切换多个行走控制方法驱动所述驱动装置而进行所述车体的行走控制，

根据所述车体受拘束识别机构的检测结果，所述控制机构决定向切换所述多个行走控制方法的所述驱动装置的指令值，通过所决定的所述指令值驱动所述驱动装置，进行所述车体的行走控制来维持行走状态。

本发明的第二方式，根据第一方式中记载的倒立二轮行走型机器人，在所述车体具有检测对所述车体的接触力的至少一个接触传感器，并且所述车体受拘束识别机构将由所述接触传感器检测出的接触信息作为输入，来识别车体受拘束状态。

本发明的第三方式，根据第一或第二方式中记载的倒立二轮行走型机器人，在车体受拘束时，切换成在使所述车轮旋转驱动的转矩中加入因所述车体的拘束力而产生的转矩量的控制方法。

本发明的第四方式，根据第一或第二方式中记载的倒立二轮行走型机器人，在车体受拘束时，切换成使对所述车体的倾斜角度以及倾斜角速度的反馈停止的控制。

本发明的第五方式，根据第一方式中记载的倒立二轮行走型机器人，在车体受拘束时，切换成下述的控制：使对所述车体的倾斜角度以及倾斜角速度的反馈停止，并且仅继续所述车体的行走。

本发明的第六方式，根据第一或第二方式中记载的倒立二轮行走型机器人，在车体受拘束时复位所述车轮的旋转角速度的误差的积分值。

本发明的第七方式，根据第三方式中记载的倒立二轮行走型机器人，在车体受拘束时复位所述车轮的旋转角速度的误差的积分值。

本发明的第八方式，根据第四方式中记载的倒立二轮行走型机器人，在车体受拘束时复位所述车轮的旋转角速度的误差的积分值。

本发明的第九方式，根据第五方式中记载的倒立二轮行走型机器人，在车体受拘束时复位所述车轮的旋转角速度的误差的积分值。

本发明的第十方式，根据倒立二轮行走型机器人的控制方法，其包括下述步骤：

检测车体的倾斜角度以及倾斜角速度至少一个作为第一状态检测信息；

检测所述车轮的旋转角度以及旋转角速度至少一个作为第二状态检测信息；

将所述第一状态检测信息或所述第二状态检测信息作为输入，检测所述车体向倾斜方向的旋转是否被拘束来作为车体受拘束识别信息；

基于上述所检测出的车体受拘束识别信息，控制机构切换多个行走控制方法，决定给分别对同轴配置于所述车体的两个车轮进行驱动的驱动装置的指令值，根据所决定的所述指令值切换多个行走控制方法，而驱动所述驱动装置，进行所述车体的行走控制来维持行走状态。

下面，参照附图对本发明的实施方式进行说明。

(第一实施方式)

下面参照附图对本发明的第一实施方式中的倒立二轮行走型机器人及其控制方法进行说明。

图1A表示本发明的第一实施方式中的倒立二轮行走型机器人的倾斜状态下的侧视图，图2表示倾斜状态下的主视图。相对于机器人的车体1，将两个车轮2a、2b大体配置于同一轴上。这里，车体1对着图中箭头A所示的行进方向，左车轮为2a，右车轮为2b。另外，图2是从车体1的行进方向观察的主视图。作为在车轮2a、2b和车体1之间分别独立产生驱动力的驱动装置的一个例子而发挥作用的执行机构3a、3b被安装于车体1的车轮2a、2b的附近，与各车轮2a、2b相连。另外，为了进行说明，作为执行机构3a、3b的一个例子为电动机。另外，在电动机3a、3b安装有减速器4a、4b，也可以构成上述驱动装置的一个例子。另外，在电动机3a、3b安装有编码器5a、5b，其作为第二状态检测机构的一个例子发挥作用，用于检测上述车轮2a、2b的旋转角度以及旋转角速度中的至少一个作为第二状态检测信息，可以测量车体1和车轮2a、2b的旋转角度。以一定采样时间(例如每隔1msec)测量旋转角度，并用其差值除以采样时间，来求得旋转角速度，可以将此作为第二状态检测信息。

另外，作为对以车体1的车轴方向为法线的垂直平面内的姿势进行检测的姿势传感器6的一个例子的两个传感器，即，作为检测车体1的倾斜角度以及倾斜角速度的至少一个作为第一状态检测信息的第一状态检测机构的一个例子而发挥作用的单轴陀螺仪6a和双轴加速度传感器6b被安装于车体1的上部。单轴陀螺仪6a安装于车体1上，以车轴方向为法线的垂直平面可以检测车轴方向的倾斜角速度作为第一状态检测信息，可以检测以车体1的车轴方向为法线的垂直平面内的倾斜角速度(用圆弧6a1表示)。另外，双轴加速度传感器6b被安装成可以检测以车轴方向为法线的垂直平面的两个规定方向(6b1、6b2)的加速度，可以检测重力方向(6b3)。作为第一状态检测信息的车体1的姿势角度(倾斜角度)，是通过将单轴陀螺仪6a的积分值和来自双轴加速度传感器6b的值分别用控制计算机部9施加到高通滤波器、低通滤波器，并将其用控制计算机部9合成而求得。

另外，在车体1的前后的下部设有接触传感器10a、10b，可以分别测量其他物体等对车体1的前后下部的接触力作为接触信息。

在车体1的内部，搭载有用于驱动电动机3a、3b的电动机驱动器7a、7b以及电池8，和作为控制机构以及车体受拘束识别机构的一个例子发挥作用的控制计算机部9。如图1B所示，控制计算机部9由A/D转换器9a和D/A转换器9b以及编码计数器部9c和运算部9d构成，编码器5a、5b的值通过编码计数器部9c被输入到运算部9d。另外，单轴陀螺仪6a和双轴加速度传感器6b、接触传感器10a、10b的值，通过A/D转换器9a被输入到运算部9d。在运算部9d中，根据所输入的来自接触传感器10a、10b的值，检测向上述车体1的倾斜方向的旋转是否被拘束，可以生成车体受拘束识别信息。该生成的车体受拘束识别信息例如是通过接触传感器10a、10b的输入而判断的对车体1的拘束力的信息(参照第一实施方式)、通过接触传感器10a、10b的输入而判断的有无接触的信息(参照第二实施方式)、车体1的倾斜角度信息(参照第三实施方式)等，如后面所述，该信息可以在切换控制方法时使用。控制计算机部9的运算部9d根据输入到运算部9d的值计算姿势控制以及行走控制所需的转矩，通过D/A转换器9b对电动机驱动器7a、7b发出转矩指令值。电动机驱动器7a、7b根据指令值驱动电动机3a、3b。另外，从输入值到输出值的计算方法在后面描述。

接着，通过图3对本构成的倒立二轮行走型机器人的控制方法进行说明。图3是从垂直上方观察机器人的图。在本机构中，分成以车轴方向为法线的垂直平面(B-B’面)内的姿势以及该垂直平面内的水平方向速度相关的并行控制11(姿势控制+前后移动控制)和回旋方向的动作相关回旋控制12这两个控制来考虑。前者是按照跟踪并行方向的

【编号1】

速度ν

的方式进行姿势控制和行走控制来维持行走状态的。后者是按照跟踪回旋方向的

【编号2】

角速度ω

的方式进行控制的。如后面所述，通过合并两个控制，可以进行移动控制(行走控制)来维持行走状态。

接着，对该移动控制按照顺序进行说明。

首先，利用表示机构参数的图4对并行控制11进行说明。另外，由于是以车轴方向为法线的垂直平面内进行动作，所以作为二维模型来处理。如图4所示，如下所述来设定参数。分别将车轮2a、2b、电动机3a、3b、减速器4a、4b、编码器5a、5b看作一个，并设为车轮2、电动机3、减速器4、编码器5。另外，将车体1的重心设为车体重心13，将车轮2的车轴设为车轴14，将车轮2的重心设为车轮重心15。

M_b：机器人车体1的质量[kg]

M_w：车轮2的质量(两个车轮的合计)[kg]、

I_b：机器人车体1的绕车体重心13的惯性矩[kgm²]

I_w：车轮2的绕车轮重心15的惯性矩(两个车轮的合计)[kgm²]

I_m：电动机3的惯性矩[kgm²]

r：车轮2的半径[m]

L_g：从车轴14到机器人的车体重心13的距离[m]

【编号3】

μ_s

：电动机3和减速器4和车轮2之间的粘性摩擦系数[Nm/(rad/s)]

【编号4】

μ_g

：车轮2和地面800之间的粘性摩擦系数[Nm/(rad/s)]

【编号5】

τ_t

：电动机3的转矩常数[Nm/A]

【编号6】

η

：减速器4的齿轮部减速比

g：重力加速度

【编号7】

φ

：机器人车体1的倾斜度[rad]

【编号8】

θ

：车轮2对地面800的旋转角[rad]

u：输入到电动机3的电流[A]

另外，如下所述对符号进行定义。

T：动能，U：势能，D：摩擦损失

z₁：车体重心高度，z₂：车轮重心高度，s₁：车体重心并进位置，

s₂：车轮并进位置，

这里，使用拉格朗日法导出倒立二轮行走型机器人的运动方程式。

拉格朗日的运动方程式

【编号9】

$\frac{d}{\mathrm{dt}}\left(\frac{\∂T}{\∂\stackrel{\·}{\mathrm{\φ}}}\right)-\frac{\∂T}{\∂\mathrm{\φ}}+\frac{\∂U}{\∂\mathrm{\φ}}+\frac{\∂D}{\∂\mathrm{\φ}}=Q_1$ (式1)

【编号10】

$\frac{d}{\mathrm{dt}}\left(\frac{\∂T}{\∂\stackrel{\·}{\mathrm{\θ}}}\right)-\frac{\∂T}{\∂\mathrm{\θ}}+\frac{\∂U}{\∂\mathrm{\θ}}+\frac{\∂D}{\∂\mathrm{\θ}}=Q_2$ (式2)

如下所述表示各值及其微分值。

【编号11】

s₁＝rθ+L_g sinφ                                            (式3)

【编号12】

s₂＝rθ                                                     (式4)

【编号13】

${\stackrel{\·}{s}}_1=r\stackrel{\·}{\mathrm{\θ}}+L_g\stackrel{\·}{\mathrm{\φ}}\cos \mathrm{\φ}$ (式5)

【编号14】

${\stackrel{\·}{s}}_2=r\stackrel{\·}{\mathrm{\θ}}$ (式6)

【编号15】

z₁＝r+L_g cosφ                                              (式7)

【编号16】

z₂＝r                                                       (式8)

【编号17】

${\stackrel{\·}{z}}_1=-L_g\stackrel{\·}{\mathrm{\φ}}\sin \mathrm{\φ}$ (式9)

【编号18】

${\stackrel{\·}{z}}_2=0$ (式10)

各能量

【编号19】

$T=\frac{1}{2}{M}_w({\stackrel{\·}{s}}_2^2+{\stackrel{\·}{z}}_2^2)+\frac{1}{2}{M}_b({\stackrel{\·}{s}}_1^2+{\stackrel{\·}{z}}_1^2)+\frac{1}{2}{I}_w{\stackrel{\·}{\mathrm{\θ}}}^2+\frac{1}{2}{I}_b{\stackrel{\·}{\mathrm{\φ}}}^2+\frac{1}{2}{I}_m{\mathrm{\η}}^2{(\stackrel{\·}{\mathrm{\θ}}-\stackrel{\·}{\mathrm{\φ}})}^2$ (式11)

【编号20】

U＝M_wgr+M_bg(r+lcosφ)                                (式12)

【编号21】

$D=\frac{1}{2}({\mathrm{\μ}}_s{(\stackrel{\·}{\mathrm{\θ}}-\stackrel{\·}{\mathrm{\φ}})}^2+u_g{\stackrel{\·}{\mathrm{\θ}}}^2)$ (式13)

作为简单的模型，将施加到车体1的外力设为0的情况

【编号22】

Q₁＝-ητ_tu                                          (式14)

【编号23】

Q₂＝ητ_tu                                           (式15)

由此，动能T是

【编号24】

$T=\frac{1}{2}{M}_w\left(r^2{\stackrel{\·}{\mathrm{\θ}}}^2\right)+\frac{1}{2}{M}_b(r^2{\stackrel{\·}{\mathrm{\θ}}}^2+2r\stackrel{\·}{\mathrm{\θ}}{L}_g\stackrel{\·}{\mathrm{\φ}}\cos \mathrm{\φ}+L_g^2{\stackrel{\·}{\mathrm{\φ}}}^2)+\frac{1}{2}{I}_w{\stackrel{\·}{\mathrm{\θ}}}^2+\frac{1}{2}{I}_b{\stackrel{\·}{\mathrm{\φ}}}^2+\frac{1}{2}{I}_m{\mathrm{\η}}^2{(\stackrel{\·}{\mathrm{\θ}}-\stackrel{\·}{\mathrm{\φ}})}^2$

(式16)。

关于机器人车体1的倾斜度φ，利用拉格朗日法导出运动方程式。

【编号25】

$M_b=(r\stackrel{\·\·}{\mathrm{\θ}}{L}_g\cos \mathrm{\φ}+L_g^2\stackrel{\·\·}{\mathrm{\φ}})+I_b\stackrel{\·\·}{\mathrm{\φ}}-I_m{\mathrm{\η}}^2(\stackrel{\·\·}{\mathrm{\θ}}-\stackrel{\·\·}{\mathrm{\φ}})-M_b\mathrm{gl}\sin \mathrm{\φ}-\left({\mathrm{\μ}}_s(\stackrel{\·}{\mathrm{\θ}}-\stackrel{\·}{\mathrm{\φ}})\right)$

$=(M_b{L}_g^2+I_b+I_m{\mathrm{\η}}^2)\stackrel{\·\·}{\mathrm{\φ}}+(M_{b}r{L}_g\cos \mathrm{\φ}-I_m{\mathrm{\η}}^2)\stackrel{\·\·}{\mathrm{\θ}}-M_b\mathrm{gl}\sin \mathrm{\φ}-{\mathrm{\μ}}_s\stackrel{\·}{\mathrm{\θ}}+{\mathrm{\μ}}_s\stackrel{\·}{\mathrm{\φ}}=-\mathrm{\η}{\mathrm{\τ}}_{t}u$

(式17)

关于车轮2对地面800的旋转角θ，利用拉格朗日法导出运动方程式。

【编号26】

$M_w\left(r^2\stackrel{\·\·}{\mathrm{\θ}}\right)+M_b(r^2\stackrel{\·\·}{\mathrm{\θ}}+r{L}_g\stackrel{\·\·}{\mathrm{\φ}}\cos \mathrm{\φ}-r{L}_g{\stackrel{\·}{\mathrm{\φ}}}^2\sin \mathrm{\φ})+I_w\stackrel{\·\·}{\mathrm{\θ}}+I_m{\mathrm{\η}}^2(\stackrel{\·\·}{\mathrm{\θ}}-\stackrel{\·\·}{\mathrm{\φ}})+({\mathrm{\μ}}_s(\stackrel{\·}{\mathrm{\θ}}-\stackrel{\·}{\mathrm{\φ}})+u_g\stackrel{\·}{\mathrm{\θ}})$

$=(M_b{\mathrm{rL}}_g\cos \mathrm{\φ}-I_m{\mathrm{\η}}^2)\stackrel{\·\·}{\mathrm{\φ}}+((M_b+M_w)r^2+I_w+I_m{\mathrm{\η}}^2)\stackrel{\·\·}{\mathrm{\θ}}-M_{b}r{L}_g{\stackrel{\·}{\mathrm{\φ}}}^2\sin \mathrm{\φ}+{\mathrm{\μ}}_s\stackrel{\·}{\mathrm{\θ}}$

$-{\mathrm{\μ}}_s\stackrel{\·}{\mathrm{\φ}}+u_g\stackrel{\·}{\mathrm{\θ}}=\mathrm{\η}{\mathrm{\τ}}_{t}u$

(式18)

若合并(式17)和(式18)，则：

【编号27】

$(M_{b}r{L}_g\cos \mathrm{\φ}+M_b{L}_g^2+I_b)\stackrel{\·\·}{\mathrm{\φ}}+((M_b+M_w)r^2+I_w+M_{b}r{L}_g\cos \mathrm{\φ})\stackrel{\·\·}{\mathrm{\θ}}$

$-M_b\mathrm{gl}\sin \mathrm{\φ}-M_{b}r{L}_g{\stackrel{\·}{\mathrm{\φ}}}^2\sin \mathrm{\φ}+u_g\stackrel{\·}{\mathrm{\θ}}=0$

(式19)。

由此，在

【编号28】

φ＝0， $\stackrel{\·}{\mathrm{\φ}}=0$

附近近似两个式(式17)和(式19)，成为

【编号29】

(sinφ＝φ，cosφ＝1， ${\stackrel{\·}{\mathrm{\φ}}}^2=0$ )

【编号30】

$(M_b{L}_g^2+I_b+{\mathrm{\η}}^2{I}_m)\stackrel{\·\·}{\mathrm{\φ}}+(M_b{\mathrm{rL}}_g\cos \mathrm{\φ}-{\mathrm{\η}}^2{I}_m)\stackrel{\·\·}{\mathrm{\θ}}-M_b\mathrm{gl\φ}-{\mathrm{\μ}}_s\stackrel{\·}{\mathrm{\θ}}+{\mathrm{\μ}}_s\stackrel{\·}{\mathrm{\φ}}=-\mathrm{\η}{\mathrm{\τ}}_{t}u$ (式20)

$(M_{b}r{L}_g+M_b{L}_g^2+I_b)\stackrel{\·\·}{\mathrm{\φ}}+((M_b+M_w)r^2+M_{b}r{L}_g+I_w)\stackrel{\·\·}{\mathrm{\θ}}-M_b\mathrm{gl\φ}+u_g\stackrel{\·}{\mathrm{\θ}}=0$ (式21)。

这里，可以将变量设为

【编号31】

$\left(\begin{array}{cc}{a}_{11}& a_{12}\\ a_{21}& a_{22}\end{array}\right)=\left(\begin{array}{cc}{M}_b{L}_g^2+I_b+{\mathrm{\η}}^2{I}_m& M_{b}r{L}_g\cos \mathrm{\φ}-{\mathrm{\η}}^2{I}_m\\ M_{b}r{L}_g+M_b{L}_g^2+I_b& (M_b+M_w)r^2+M_{b}r{L}_g+I_w\end{array}\right)$ (式22)

Δ＝a₁₁a₂₂-a₁₂a₂₁                                    (式23)

$\left(\begin{array}{cc}{a}_1& a_2\\ a_3& a_4\\ a_5& a_6\end{array}\right)=\frac{1}{\mathrm{\Δ}}\left(\begin{array}{cc}(a_{22}-a_{12})M_{b}g{L}_g& (a_{11}-a_{21})M_{b}g{L}_g\\ -{\mathrm{\μ}}_s{a}_{22}& {\mathrm{\μ}}_s{a}_{21}\\ {\mathrm{\μ}}_s{a}_{22}+{\mathrm{\μ}}_g{a}_{12}& -({\mathrm{\μ}}_s{a}_{21}+{\mathrm{\μ}}_g{a}_{11})\end{array}\right)$ (式24)

【编号32】

$\left(\begin{array}{c}{b}_1\\ b_2\end{array}\right)=\frac{1}{\mathrm{\Δ}}\left(\begin{array}{c}-a_{22}{\mathrm{\η\τ}}_t\\ a_{21}{\mathrm{\η\τ}}_t\end{array}\right)$ (式25)

复原成

【编号33】

$\frac{d}{\mathrm{dt}}\left(\begin{array}{c}\mathrm{\φ}\\ \stackrel{\·}{\mathrm{\φ}}\\ \stackrel{\·}{\mathrm{\θ}}\end{array}\right)=\stackrel{\·}{x}=\left(\begin{array}{ccc}0& 1& 0\\ a_1& a_3& a_5\\ a_2& a_4& a_6\end{array}\right)\left(\begin{array}{c}\mathrm{\φ}\\ \stackrel{\·}{\mathrm{\φ}}\\ \stackrel{\·}{\mathrm{\θ}}\end{array}\right)+\left(\begin{array}{c}0\\ b_1\\ b_2\end{array}\right)u=\mathrm{Ax}+\mathrm{Bu}$ (式26)

【编号34】

$A=\left(\begin{array}{ccc}0& 1& 0\\ a_1& a_3& a_5\\ a_2& a_4& a_6\end{array}\right)B=\left(\begin{array}{c}0\\ b_1\\ b_2\end{array}\right)x=\left(\begin{array}{c}\mathrm{\φ}\\ \stackrel{\·}{\mathrm{\φ}}\\ \stackrel{\·}{\mathrm{\θ}}\end{array}\right)$ (式27)

的状态方程式的形状。另外，

【编号35】

$x=\left(\begin{array}{c}\mathrm{\φ}\\ \stackrel{\·}{\mathrm{\φ}}\\ \stackrel{\·}{\mathrm{\θ}}\end{array}\right)$

是状态矢量。

以上的模型化由于是状态矢量的各值为0的近似模型，所以为了考虑与前后方向的位置(速度指令)相关的跟踪性，对该模型构成跟踪阶梯状的目标输入的模型的控制系统。另外，将目标输入设为一定速度目标值。

【编号36】

速度 $y(=\stackrel{\·}{\mathrm{\θ}})$

为可观测，并表示为

【编号37】

$y=\stackrel{\·}{\mathrm{\θ}}=\mathrm{Cx}=\left(\mathrm{0,0,1}\right)x.$

【编号38】为了使

速度y(t)

没有稳定偏差地跟踪阶梯状的

【编号39】

目标角速度 ${\stackrel{\·}{\mathrm{\θ}}}_d=\mathrm{\ν}/r$

，而设为采用了积分器的控制系统。另外，在图5中表示此时的控制方框。这里，是用于使

【编号40】

K(1×3)、G(1×1)

的闭环系统稳定的反馈矢量。

在该构成中，

【编号41】

$z=\∫({\stackrel{\·}{\mathrm{\θ}}}_d-\stackrel{\·}{\mathrm{\θ}})\mathrm{dt}$

【编号42】

$\stackrel{\·}{z}=-\mathrm{Cx}+{\stackrel{\·}{\mathrm{\θ}}}_d$ (式28)

控制输入表示为

【编号43】

u＝-Kx+Gz                                                (式29)

这里，设In为3行3列的单位行列，设Im＝1，

【编号44】

$\stackrel{\·}{u}=-K\stackrel{\·}{x}+G\stackrel{\·}{z}=-K(\mathrm{Ax}+\mathrm{Bu})+G(-\mathrm{Cx}+{\mathrm{\θ}}_d)$

$=-\left[\begin{array}{cc}K& G\end{array}\right]\left[\begin{array}{cc}A& B\\ C& 0\end{array}\right]\left[\begin{array}{c}x\\ u\end{array}\right]+{\mathrm{G\θ}}_d$

(式30)

通过(式26)和(式30)

【编号45】

$\left[\begin{array}{c}\stackrel{\·}{x}\\ \stackrel{\·}{u}\end{array}\right]=\left[\begin{array}{cc}{I}_n& 0\\ -K& -G\end{array}\right]\left[\begin{array}{cc}A& B\\ C& 0\end{array}\right]\left[\begin{array}{c}x\\ u\end{array}\right]+\left[\begin{array}{c}0\\ G\end{array}\right]{\stackrel{\·}{\mathrm{\θ}}}_d$ (式31)

【编号46】

y＝[C 0][x^T u^T]^T                                         (式32)

这里，常数值

【编号47】

$\left[\begin{array}{c}x(\∞)\\ u(\∞)\end{array}\right]=-{\left[\begin{array}{cc}A& B\\ C& 0\end{array}\right]}^{-1}{\left[\begin{array}{cc}{I}_n& 0\\ -K& -G\end{array}\right]}^{-1}\left[\begin{array}{c}0\\ G\end{array}\right]{\stackrel{\·}{\mathrm{\θ}}}_d=-{\left[\begin{array}{cc}A& B\\ C& 0\end{array}\right]}^{-1}\left[\begin{array}{cc}{I}_n& 0\\ -G^{-1}K& -G^{-1}\end{array}\right]\left[\begin{array}{c}0\\ G\end{array}\right]{\stackrel{\·}{\mathrm{\θ}}}_d={\left[\begin{array}{cc}A& B\\ C& 0\end{array}\right]}^{-1}\left[\begin{array}{c}0\\ I_m\end{array}\right]{\stackrel{\·}{\mathrm{\θ}}}_d$

(式33)

【编号48】

y(∞)＝[C 0][x(∞)^Tu(∞)^T]^T                              (式34)

另外，

【编号49】

$y(\∞)=\left[\begin{array}{cc}C& 0\end{array}\right]{\left[\begin{array}{cc}x{(\∞)}^T& u{(\∞)}^T\end{array}\right]}^T=\left[\begin{array}{cc}C& 0\end{array}\right]{\left[\begin{array}{cc}A& B\\ C& 0\end{array}\right]}^{-1}\left[\begin{array}{c}0\\ I_m\end{array}\right]{\stackrel{\·}{\mathrm{\θ}}}_d={\stackrel{\·}{\mathrm{\θ}}}_d$ (式35)，

可知没有稳定偏差。

接着，考虑由常数值而来的误差系统。

【编号50】若作为

x_e＝x-x(∞)，u_e＝u-u(∞)， $e=\stackrel{\·}{\mathrm{\θ}}-{\stackrel{\·}{\mathrm{\θ}}}_d$

进行公式变形，则成为

【编号51】

$\left[\begin{array}{c}{\stackrel{\·}{x}}_e\\ {\stackrel{\·}{u}}_e\end{array}\right]=\left[\begin{array}{c}\stackrel{\·}{x}\\ \stackrel{\·}{u}\end{array}\right]=\left[\begin{array}{cc}{I}_n& 0\\ -K& -G\end{array}\right]\left[\begin{array}{cc}A& B\\ C& 0\end{array}\right]\left[\begin{array}{c}x\\ u\end{array}\right]+\left[\begin{array}{c}0\\ G\end{array}\right]{\stackrel{\·}{\mathrm{\θ}}}_d$

$=\left[\begin{array}{cc}{I}_n& 0\\ -K& -G\end{array}\right]\left[\begin{array}{cc}A& B\\ C& 0\end{array}\right]\left[\begin{array}{c}{x}_e+x(\∞)\\ u_e+u(\∞)\end{array}\right]+\left[\begin{array}{c}0\\ G\end{array}\right]{\stackrel{\·}{\mathrm{\θ}}}_d$

$=\left[\begin{array}{cc}{I}_n& 0\\ -K& -G\end{array}\right]\left[\begin{array}{cc}A& B\\ C& 0\end{array}\right]\{\left[\begin{array}{c}{x}_e\\ u_e\end{array}\right]+{\left[\begin{array}{cc}A& B\\ C& 0\end{array}\right]}^{-1}\begin{array}{c}\left[\begin{array}{c}0\\ I_m\end{array}\right]\end{array}{u}_r\}+\left[\begin{array}{c}0\\ G\end{array}\right]{\stackrel{\·}{\mathrm{\θ}}}_d$

$=\left[\begin{array}{cc}{I}_n& 0\\ -K& -G\end{array}\right]\left[\begin{array}{cc}A& B\\ C& 0\end{array}\right]\left[\begin{array}{c}{x}_e\\ u_e\end{array}\right]$

$=\left[\begin{array}{cc}A& B\\ 0& 0\end{array}\right]\left[\begin{array}{c}{x}_e\\ u_e\end{array}\right]+\left[\begin{array}{cc}0& 0\\ -K& -G\end{array}\right]\left[\begin{array}{cc}A& B\\ C& 0\end{array}\right]\left[\begin{array}{c}{x}_e\\ u_e\end{array}\right]$

$=\left[\begin{array}{cc}A& B\\ 0& 0\end{array}\right]\left[\begin{array}{c}{x}_e\\ u_e\end{array}\right]-\left[\begin{array}{c}0\\ I_m\end{array}\right]\left[\begin{array}{cc}K& G\end{array}\right]\left[\begin{array}{cc}A& B\\ C& 0\end{array}\right]\left[\begin{array}{c}{x}_e\\ u_e\end{array}\right]$

(式36)

【编号52】设

$H=\left[\begin{array}{cc}K& G\end{array}\right]\left[\begin{array}{cc}A& B\\ C& 0\end{array}\right]$ (式37)

【编号53】设

$w=-H\left[\begin{array}{c}{x}_e\\ u_e\end{array}\right]$ (式38)

【编号54】则

$\left[\begin{array}{c}{\stackrel{\·}{x}}_e\\ {\stackrel{\·}{u}}_e\end{array}\right]=\left[\begin{array}{cc}A& B\\ 0& 0\end{array}\right]\left[\begin{array}{c}{x}_e\\ u_e\end{array}\right]-\left[\begin{array}{c}0\\ I_m\end{array}\right]H\left[\begin{array}{c}{x}_e\\ u_e\end{array}\right]=\left[\begin{array}{cc}A& B\\ 0& 0\end{array}\right]\left[\begin{array}{c}{x}_e\\ u_e\end{array}\right]+\left[\begin{array}{c}0\\ I_m\end{array}\right]w$ (式39)

将出力设为e，

【编号55】则

e＝[C 0][x_e ^T u_e ^T]^T                                    (式40)。

看作是将w的状态反馈加到(式39)、(式40)的调节器。由此，通过最佳调节法或极点配置法、及其他各种设计法来导出该误差系统的反馈增益H。若使用该增益，则可以设计

【编号56】

偏差e(t)

的过渡响应误差少的最佳伺服系统。另外，通过(式37)

【编号57】

$\left[\begin{array}{cc}K& G\end{array}\right]=H{\left[\begin{array}{cc}A& B\\ C& 0\end{array}\right]}^{-1}=[k_1,k_2,k_3,k_4]$ (式41)。

由此，

【编号58】使用

增益[k1，k2，k3，k4]，

并向电动机3输出。

【编号59】若将

转矩指令值τ₁设为

【编号60】

τ₁＝ητ_tu＝-ητ_t[k₁，k₂，k₃，k₄]x₁                    (式42)

，则可以同时实现倒立的姿势控制和速度控制。

其中，

【编号61】

$x_1=\left(\begin{array}{c}\mathrm{\φ}\\ \stackrel{\·}{\mathrm{\φ}}\\ \stackrel{\·}{\mathrm{\θ}}\\ z_1\end{array}\right)$ (式43)

【编号62】

$z_1=-\∫({\stackrel{\·}{\mathrm{\θ}}}_d-\stackrel{\·}{\mathrm{\θ}})\mathrm{dt}=-z$ (式44)。

另外，

【编号63】

k₁＜0，k₂＜0，k₃＜0，k₄＜0。

在本构成中，由于车轮2有两个，所以若向各车轮2(2a、2b)输出0.5倍的

【编号64】

转矩指令值τ₁，

则可进行并行方向的姿势控制和速度控制。设此时的各车轮2a、2b的

【编号65】

转矩指令值为左车轮τ_1L、右车轮τ_2R，

则

【编号66】

τ_1L＝0.5τ₁                                (式45)

τ_1R＝0.5τ₁                                (式46)。

接着，利用回旋控制模式图的图6对回旋控制12进行说明。

从上面观察机器人车体1，设机器人车体1的车轮2间的长度为

【编号67】

L_w

设机器人车体1在水平面上的移动速度为

【编号68】

ν

设左车轮2a的旋转速度为

【编号69】

ν_L

设右车轮2b的旋转速度为

【编号70】

ν_R

设回旋半径为R，设回旋速度为

【编号71】

ω。

另外，设左车轮2a旋转角速度为

【编号72】

${\stackrel{\·}{\mathrm{\θ}}}_L$

设右车轮2b的旋转角速度为

【编号73】

${\stackrel{\·}{\mathrm{\θ}}}_R$

则

【编号74】

$\left(\begin{array}{c}\mathrm{\ν}\\ \mathrm{\ω}\end{array}\right)=\left(\begin{array}{cc}\frac{r}{2}& \frac{r}{2}\\ \frac{-r}{L_w}& \frac{r}{L_w}\end{array}\right)\left(\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_L\\ {\stackrel{\·}{\mathrm{\θ}}}_R\end{array}\right)$ (式47)。

【编号75】设

回旋速度目标值为ω_d

【编号76】对与

实际的回旋速度ω

之差进行反馈控制(PD控制等)，

【编号77】决定

回旋方向的转矩τ₂。

【编号78】

回旋方向的转矩τ₂

被看作左右车轮2a、2b上产生的转矩差。在左右车轮2a、2b上，分别通过将其回旋方向的转矩的0.5倍加到可以算出回旋方向的目标转矩的两车轮2a、2b上或从该两车轮2a、2b减去回旋方向的转矩的0.5倍，从而可以实现回旋方向的控制。

在图7中表示整合了并行控制11和回旋控制12的控制框图。

若将对左轮2a、右轮2b指令的转矩指令值设为

【编号79】

τ_L、τ_R，

则

【编号80】

$\left(\begin{array}{c}{\mathrm{\τ}}_L\\ {\mathrm{\τ}}_R\end{array}\right)=\left(\begin{array}{cc}\frac{1}{2}& -\frac{1}{2}\\ \frac{1}{2}& \frac{1}{2}\end{array}\right)\left(\begin{array}{c}{\mathrm{\τ}}_1\\ {\mathrm{\τ}}_2\end{array}\right)$ (式48)

如上所述，通过决定转矩，上述机器人可以一边维持倒立姿势，一边进行回旋以及直线行走。

如图1A所示，在车体1从地面800离开的情况下，若根据以上描述的控制规则，则可以一边维持倒立姿势一边进行行走。

接着，在第一实施方式中表示下述情况：如图8所示，以车体1的一部分在旋转方向受到拘束的情况为例，车体1倾斜，使得车体下部与地面800接触。假设车体1和地面800的摩擦力为0，在从车轴14水平移动了

【编号81】

垂直阻力F₁

的接触点作为拘束力产生了

【编号82】

距离r₁。

接触传感器10b(由于接触传感器10a也一样，所以以接触传感器10b为代表进行说明。)可以测量其他物体或地面800或墙壁等对车体1的接触力，通过接触力传感器10b，检测

【编号83】

垂直方向的拘束力F₁，

根据几何学的条件，

【编号84】

距离r₁

也已知。此时，在车体1产生由拘束力而引起的

【编号85】

转矩τ_a＝F₁r₁。

另外，位置偏移量为

【编号86】

s₃＞0

的状态时，考虑图8所示的

【编号87】

(倾斜角φa＜0)

产生接触的情况。此时，设使车轮2正转的方向(机器人车体1在图8中向右方向移动，车体1绕车轴14进行左旋转的方向)为正，基于(式42)的电动机3的转矩指令值为

【编号88】

${\mathrm{\τ}}_1=\mathrm{\η}{\mathrm{\τ}}_t[-k_1,{-k}_2,-k_3,-k_4]x_1=-\mathrm{\η}{\mathrm{\τ}}_t(k_1\mathrm{\φ}+k_2\stackrel{\·}{\mathrm{\φ}}+k_3\stackrel{\·}{\mathrm{\θ}}+k_4{z}_1)$ (式49)。

【编号89】由于

垂直阻力F₁＞0，

所以倾斜方向的旋转受到拘束，

【编号90】

φ＝φ_a、 $\stackrel{\·}{\mathrm{\φ}}=0.$

此时，

【编号91】

${\mathrm{\τ}}_1=-\mathrm{\η}{\mathrm{\τ}}_t(k_1{\mathrm{\φ}}_a+k_3\stackrel{\·}{\mathrm{\θ}}+k_4{z}_1)$ (式50)。

【编号92】由于

施加到车体1的转矩τ_b

是来自电动机3的转矩的

【编号93】

反作用力(-τ₁)

与

【编号94】

接触引起的转矩τ_d

之和，所以

【编号95】

${\mathrm{\τ}}_b={\mathrm{\τ}}_a-{\mathrm{\τ}}_1=F_1{r}_1+\mathrm{\η}{\mathrm{\τ}}_t(k_1{\mathrm{\φ}}_a+k_3\stackrel{\·}{\mathrm{\θ}}+k_4{z}_1)$ (式51)。

【编号96】

τ_b根据 Z₁的值，能成为

【编号97】

τ_b＞0、τ_b＜0

中的任意一种。

在(式51)中，当

【编号98】

τ_b＞0

时，由于转矩以进行右旋转的方式作用于车体1，所以若车轴14右旋转，接触被消除，可返回到倒立状态。但是，当

【编号99】

τ_b＜0

时，左旋转的转矩施加于车体1，在车轮2作为反力而产生向右移动的旋转转矩。此时，车体由于受到

【编号100】

外力F₁

的拘束，所以，通过

【编号101】

外力F₁

而产生的

【编号102】

转矩τ_a＝F₁r₁

作为向车轮2旋转转矩也被传递到车轮2。因此，在车轮2上施加多余的转矩，产生车轮2过度旋转现象，导致陷入失控状态。

因此，在本发明的上述第一实施方式的机器人控制方法中，通过接触力传感器10b探测接触信息，比较所探测到的接触力方向和

【编号103】

转矩τ₁，

在其旋转方向相反的情况下，将向电动机3的

【编号104】

转矩指令值τ₁

设为加上由接触力产生的

【编号105】

转矩量τ_a＝F₁r₁

而得到

【编号106】

${\mathrm{\τ}}_1=-\mathrm{\η}{\mathrm{\τ}}_t(k_1{\mathrm{\φ}}_a+k_3\stackrel{\·}{\mathrm{\θ}}+k_4{z}_1)+F_1{r}_1$ (式52)。

由此，根据本发明的上述第一实施方式的机器人的控制方法，通过根据(式52)算出电动机3的转矩，从而可以消除来自地面800的接触力带来的影响，不会给用户带来负担，可以实现安全的倒立二轮行走型机器人。

另外，当在本构成中于车体1和地面800之间产生

【编号107】

摩擦力F₂

时，如图9所示，若补偿由其

【编号108】

合力F₃

产生的旋转的反方向的转矩进行行走的话，则会得到同样的效果

【编号109】

${\mathrm{\τ}}_1=-\mathrm{\η}{\mathrm{\τ}}_t(k_1{\mathrm{\φ}}_a+k_3\stackrel{\·}{\mathrm{\θ}}+k_4{z}_1)+F_3{r}_3$ (式53)

另外，也可以不用接触传感器而使用干扰观测器来推定干扰力，利用该推定出的接触力进行与(式52)、(式53)同样的控制。

这里，通过图12对使用上述的第一实施方式中的切换控制的行走动作流程进行说明。

在初始状态下(步骤S1)，机器人车体1倾斜停止。作为初始设定，

【编号110】

(z₁ ν ω)＝(0，0，0)

(＝步骤S2)。接着，进行(式50)所示的倒立停止

【编号111】

$({\stackrel{\·}{\mathrm{\θ}}}_d=\mathrm{\ν}/r=0)$

控制(步骤S3)而站起来。在倒立停止控制(步骤S3)的稳定时间(例如5秒左右)后，判断有无车体1对地面800等的接触(步骤S5)。步骤S5中没有接触的情况下，

【编号112】进行

速度(νω)

設定(步骤S8)，之后进行(式49)所示的倒立行走控制(步骤S9)维持行走状态，然后返回到步骤S5。

当在步骤S5中有接触的情况下，

【编号113】进行

速度(νω)

设定(步骤S6)，之后进行(式52或式53)所示的倒立行走控制(步骤S7)维持行走状态，然后返回到步骤S5。

另外，在步骤S6和步骤S8中，由于在接地时和不接地时，都补偿其差值转矩，所以在任意情况下都进行同样的行走。

如上所述，从初始的车体1移动停止与地面800接地的状态，首先通过进行倒立停止控制(式(49)、步骤S2～S3)，到倒立位置为止使车体1站起来，维持倒立状态(停止行走的状态)。接着，在经过规定时间(稳定时间)倒立状态稳定(步骤S4)之后，由接触传感器10a、10b开始有无接触的检测(步骤S5)。若在步骤S5中没有接触(换句话说车体受拘束)，则从倒立停止控制过渡到倒立行走控制(式(49)、步骤S8～S9)维持行走状态，之后由接触传感器10a、10b再次进行有无接触的检测(步骤S5)。当在步骤S5中检测到接触(换句话说车体受拘束)的情况下，从倒立停止控制移至倒立行走控制(式(52)或式(53)、步骤S6～S7)维持行走状态，之后由接触传感器10a、10b再次进行有无接触的检测(步骤S5)。

这样，通过适当切换倒立停止控制(步骤S3)、倒立行走控制(步骤S9)、倒立行走控制(步骤S7)，同时进行二轮行走，从而即使与车体1产生接触，通过根据接触状态(即，车体受拘束状态)使用于车体旋转的转矩的比例变化，从而不会对车轮2施加过大的转矩来维持姿势控制，可以防止行走速度急剧上升的所谓的失控现象，不会给用户带来负担，可以安全地继续行走。

(第二实施方式)

与第一实施方式相同，用图8来说明本发明的第二实施方式。在第一实施方式中，设置接触传感器来探测接触力，而在第二实施方式中，设接触传感器10b为仅探测有无接触而不进行力测量的传感器的情况、或即使是可以进行力测量的传感器也仅利用有无接触的情况，叙述其控制方法。

当车体1的一部分与地面800等接触的情况下，车体1可以不旋转，车体1通过两个车轮2a、2b和至少1点的三点以上与地面800接触。在通过以往的独立二轮驱动由辅助轮等接地的货车的移动控制中，通过PID控制等对车轮2的旋转速度进行行走控制维持行走状态。为此，在该第二实施方式中，由于在接地时也可看作同样的构成，所以在接地的情况下，被构成为

【编号114】

${\mathrm{\τ}}_1=k_5(\stackrel{\·}{\mathrm{\θ}}-{\stackrel{\·}{\mathrm{\θ}}}_d)+k_6\∫(\stackrel{\·}{\mathrm{\θ}}-{\stackrel{\·}{\mathrm{\θ}}}_d)\mathrm{dt}=k_5(\stackrel{\·}{\mathrm{\θ}}-{\stackrel{\·}{\mathrm{\θ}}}_d)+k_6{z}_1$ (式54)

所示的对旋转速度的大致PI控制的速度控制。由此，在存在接触的情况下，切换(式54)的控制进行控制，在没有接触的情况下切换(式50)的控制进行控制。

利用图10对使用第二实施方式的切换控制的行走动作流程进行说明。在初始状态(步骤S11)下，机器人车体1倾斜停止。作为初始设定，

【编号115】

(z₁ ν ω)＝(0，0，0)

(＝步骤S12)。接着，进行(式50)所示的倒立停止

【编号116】

$({\stackrel{\·}{\mathrm{\θ}}}_d=\mathrm{\ν}/r=0)$

控制(步骤S13)而站起来。在倒立停止控制(步骤S13)的稳定时间(例如5秒左右)后(步骤S14)，判断有无接触(步骤S15)。如果没有接触(步骤S15)，则进行

【编号117】

速度(ν ω)

设定(步骤S20)，之后进行(式49)所示的倒立行走控制(步骤S21)维持行走状态。作为从基于(式49)的倒立行走控制(步骤S21)开始向接触时的基于(式54)的速度控制(步骤S18)的控制方法过渡的条件，设置积分项复位历史位bt1。所谓积分项是指速度差的

【编号118】

积分Z₁

(车体1的平衡相对于车体的当前位置被破坏等产生位置偏移时的位置偏移量)。然后，在倒立行走控制(步骤S21)中，判断有无接触(步骤S22)。

当在步骤S22中车体1没有与地面800等接触的情况下，将积分项复位历史位bt1复位(bt1＝0)(步骤S26)后(即不进行积分项的复位)，返回到步骤S20。

当在步骤S22中车体1与地面800等接触的情况下，判断是否对积分项复位历史位bt1进行复位，即积分项复位历史位bt1是否等于1(步骤S23)。仅在积分项复位历史位bt1＝0时(接触后不进行积分项的复位时)，将

【编号119】

积分项Z₁

设为

【编号120】

复位Z₁＝0

(步骤S27)后(复位积分项，且残留表示复位了积分项的历史信息后)，返回到步骤S20。对积分项进行复位是用于：使接触前产生的位置偏移(速度差的

【编号121】

积分Z₁

)一次为0并成为接触时的PI控制。这样，通过复位积分项，可以消除接触前的位置偏移量，仅通过接触后的车体1的位置偏移量来进行动作控制，所以可以防止车体1的失控现象。

当在到步骤S23中积分项复位历史位bt1＝1时，判断有无接触(步骤S24)。

当在步骤S24中车体1没有与地面800等接触的情况下，将积分项复位历史位bt1复位(bt1＝0)(步骤S28)后(即不进行积分项的复位)，返回到步骤S20。

在步骤S24中车体1与地面800等持续接触的情况下(bt1＝1)，若在稳定时间(例如1秒)(步骤S25)以内，则返回到步骤S20，继续倒立行走(步骤S21)。

另一方面，若经过稳定时间(例如1秒)，则从倒立行走(步骤S21)移至速度控制(步骤S30～S32)。

在步骤S15中车体1与地面800等接触的情况下或在步骤S25中经过稳定时间时，

【编号122】进行

速度(ν ω)

设定(步骤S30)，之后判断是否为

【编号123】

(ν ω)＝(0，0)

(步骤S31)。在“是”的情况下判断为无法站起并结束(步骤S19)。在步骤S31中判断为“否”，即、

【编号124】

(ν ω)≠(0，0)

的情况下，通过(式54)所示的速度控制(步骤S32)进行行走，之后返回到步骤S14，判断速度控制(步骤S32)是否经过稳定时间(例如5秒左右)(步骤S14)。

如上所述，根据上述第二实施方式，在控制计算机部9的控制下，可以进行以下所述的动作控制。

即，从初始的车体1移动停止与地面800接地的状态，首先通过进行倒立停止控制(式(49)、步骤S12～S13)，使车体1在倒立位置站起，维持倒立状态(停止行走的状态)。接着，在经过规定时间(稳定时间)倒立状态稳定(步骤S14)之后，由接触传感器10a、10b开始有无接触的检测(步骤S15)。若在步骤S15中没有接触(换句话说车体受拘束)，则从倒立停止控制移至倒立行走控制(式(49)、步骤S20～S21)维持行走状态。当在步骤S15中检测到接触(换句话说车体受拘束)的情况下，从倒立停止控制移至行走控制(步骤S30～S32)。接着，在经过规定时间(稳定时间)稳定进行速度控制(步骤S14)之后，由接触传感器10a、10b再次开始有无接触的检测(步骤S15)，当在步骤S15中没有接触(若站起成功)，则移至倒立行走控制(式(49)、步骤S20～S21)维持行走状态。若在步骤S15中有接触(若站起成功)，直接继续速度控制(步骤S30～S32)。在进行速度控制(步骤S30～S32)时，在速度以及加速度为0的情况下，车体1与物体或地面800等接触时等，判断为车体1没有站起的情况，停止车体1的站起驱动，并设为设为接地状态。

另外，当在倒立行走控制(式(49)、步骤S20～S21)中有接触时(步骤S22)，在复位接触时为止的位置偏移量使其为0之后，仅通过接触时的位置偏移量进行动作控制，所以可以防止车体1的失控现象。而且，在倒立行走控制时，在复位接触时为止的位置偏移量使其为0之后，经过稳定时间还继续接触的情况下，判断为车体1的倒立状态被破坏，可以按照从倒立行走控制切换为速度控制而恢复到倒立状态的方式进行动作控制。

这样，通过一边适当切换倒立停止控制(步骤S13)、倒立行走控制(步骤S21)、速度控制(步骤S32)，一边进行倒立二轮行走，从而即使与车体1产生接触，通过根据接触状态(即车体受拘束状态)改变用于车体旋转的转矩的比例，从而不会对车轮2施加过大的转矩来维持姿势控制，可以方式速度急剧上升等所谓的失控，不会给用户带来负担，可以安全地进行行走。

(第三实施方式)

在第二实施方式中，通过实际的接触传感器10a、10b的输入检测有无接触来切换控制方法，而在本发明的第三实施方式中，在得知行走的地面800是平坦的面的情况下，没有接触传感器，通过车体1的倾斜角度进行控制方法的切换，并在车体1的倾斜角度倾斜规定角度以上时进行切换控制。即，如图11所示，车体1倾斜并与地面800接触的情况下的车体1的倾斜角为

【编号125】

φ＞φ_b＞0 φ＜φ_c＜0

车体1倾斜不与地面800接触时，即

【编号126】

(φ_b≥φ≥φ_c)

时，进行(式49)所示的倒立行走控制维持行走状态，另一方面，当车体1倾斜与地面800接触时，即

【编号127】

(φ＞φ_b或φ＜φ_c)

时，进行基于(式54)的控制。

这里，作为一个例子如图11所示，若相对于倒立停止或行走状态(单点划线所示的I)如倾斜位置(由虚线所示的II或III)那样倾斜10°，则在假设为车体1与地面800接触时，考虑5°的容许值，若倾斜9.5°以上，则只要判断为车体1与地面800接触即可。即在

【编号128】

φ＞φ_b＝9.5°＞0

或者

【编号129】

φ＜φ_c＝-9.5°＜0

时，只要判断为车体1与地面800接触即可。由此，当车体1倾斜不与地面800接触时，即在

【编号130】

(φ_b＝9.5°≥φ≥φ_c＝-9.5°)

时，进行(式49)所示的倒立行走控制维持行走状态，另一方面，在车体1倾斜与地面800接触时，即在

【编号131】

(φ＞φ_b＝9.5°又はφ＜φ_c＝-9.5°)

时，进行基于(式54)的速度控制。

如上所述，通过根据车体1的倾斜角度切换控制方法，同时进行行走，从而即使在平坦的面产生接触，也不会失控且不会给用户带来负担，可安全地继续行走。

另外，通过适当组合上述各实施方式中的任意的实施方式，可以起到各自所具有的效果。

(工业上的可利用性)

在本发明的倒立二轮行走型机器人及其控制方法中，提供了一种倒立二轮行走型机器人及其控制方法，即使在与地面或避免等接触而使旋转受拘束的情况下，可以维持姿势控制或行走状态，且不会给用户带来负担，可以安全地移动。

虽然参照附图并与优选的实施方式相关联地充分记载了本发明，但对于该技术熟练的人来说，可以推导出各种变形或修正。这些变形或修正在不脱离本申请附加的权利要求的范围的情况下，应该理解为包含于本申请。

Claims (10)

1.一种倒立二轮行走型机器人，其包括：

车体；

两个车轮，其同轴配置于所述车体；

驱动装置，其分别驱动所述车轮；

第一状态检测机构，其检测所述车体的倾斜角度以及倾斜角速度中的至少一个；

第二状态检测机构；其检测所述车轮的旋转角度以及旋转角速度中的至少一个；

车体受拘束识别机构，其将由所述第一状态检测机构或第二状态检测机构检测出的信息作为输入，检测向所述车体的倾斜方向的旋转是否受到拘束；和

控制机构，其决定向所述驱动装置的指令值，通过所决定的指令值切换多个行走控制方法来驱动所述驱动装置而进行所述车体的行走控制，

根据所述车体受拘束识别机构的检测结果，所述控制机构决定切换所述多个行走控制方法的对所述驱动装置的指令值，通过所决定的所述指令值驱动所述驱动装置，进行所述车体的行走控制来维持行走状态。

2.根据权利要求1所述的倒立二轮行走型机器人，其特征在于，

在所述车体具有检测对所述车体的接触力的至少一个接触传感器，并且所述车体受拘束识别机构将由所述接触传感器检测出的接触信息作为输入，来识别车体受拘束状态。

3.根据权利要求1或2所述的倒立二轮行走型机器人，其特征在于，

在车体受拘束时，切换成在使所述车轮旋转驱动的转矩中加入了由所述车体受到的拘束力所产生的转矩量的控制方法。

4.根据权利要求1或2所述的倒立二轮行走型机器人，其特征在于，

在车体受拘束时，切换成使对所述车体的倾斜角度以及倾斜角速度的反馈停止的控制。

5.根据权利要求4所述的倒立二轮行走型机器人，其特征在于，

在车体受拘束时，切换成下述的控制：使对所述车体的倾斜角度以及倾斜角速度的反馈停止，并且仅继续所述车体的行走。

6.根据权利要求1或2所述的倒立二轮行走型机器人，其特征在于，

在车体受拘束时复位所述车轮的旋转角速度的误差的积分值。

7.根据权利要求3所述的倒立二轮行走型机器人，其特征在于，

在车体受拘束时复位所述车轮的旋转角速度的误差的积分值。

8.根据权利要求4所述的倒立二轮行走型机器人，其特征在于，

在车体受拘束时复位所述车轮的旋转角速度的误差的积分值。

9.根据权利要求5所述的倒立二轮行走型机器人，其特征在于，

在车体受拘束时复位所述车轮的旋转角速度的误差的积分值。

10.一种倒立二轮行走型机器人的控制方法，其包括下述步骤：

检测车体的倾斜角度以及倾斜角速度至少一个作为第一状态检测信息；

检测所述车轮的旋转角度以及旋转角速度至少一个作为第二状态检测信息；

将所述第一状态检测信息或所述第二状态检测信息作为输入，检测所述车体向倾斜方向的旋转是否被拘束来作为车体受拘束识别信息；

基于上述所检测出的车体受拘束识别信息，控制机构切换多个行走控制方法，决定给分别对同轴配置于所述车体的两个车轮进行驱动的驱动装置的指令值，根据所决定的所述指令值切换多个行走控制方法，驱动所述驱动装置，进行所述车体的行走控制来维持行走状态。

Images