CN101296838B - 防翻倒控制装置 - Google Patents

Abstract

本发明提供一种防翻倒控制装置，该防翻倒控制装置具有：左右自由倾斜的自行车机器人(A)；将检测轴向着实质上前后方向安装在自行车机器人(A)上的角速度传感器(7)；将旋转轴向着实质上前后方向安装在本体上的电动机(9)；检测电动机(9)的旋转位置或旋转速度的旋转传感器(10)；以及与电动机(9)的转轴连接的惯性转子(8)，用电动机(9)使惯性转子(8)旋转，利用随着惯性转子(8)的旋转而产生的反作用转矩，来校正自行车机器人(A)的倾斜。具有：根据角速度传感器(7)的角速度输出ω₁和给予电动机(9)的转矩指令τ₀来对相对于平衡状态的倾斜角进行推定的倾斜角推定单元，并使用倾斜角推定值，来校正自行车机器人(A)的倾斜。

Description

防翻倒控制装置

技术领域

本发明涉及一种防翻倒控制装置，平衡控制成例如两轮车或两足步行机器人那样左右自由倾斜的本体不翻倒。

背景技术

在专利文献1中，提出了一种两轮车行驶玩具，该玩具具有：转向部；能够利用转向部进行转向的前轮；后轮；根据前轮(把手)的方向相应摇动的飞轮；驱动飞轮的第1驱动部；以及驱动后轮的第2驱动部。在该两轮车的情况下，根据前轮的方向，相应改变飞轮的方向，通过这样，利用飞轮产生的陀螺效应，在行驶中不容易翻倒。

但是，在前述两轮车行驶玩具的情况下，由于根据前轮的方向，不过只相应改变飞轮的方向，因此存在的问题是，如果是通常行驶中，即使能够通过把手操作来防止翻倒，但在停止时或极慢速行驶时，很难仅通过把手操作来取得平衡，不能有效地防止翻倒。

在专利文献2中，提出了一种防止翻倒的倒立控制玩具，该玩具将利用倾斜检测传感器检测出的倾斜输入控制电路，用控制电路驱动电动机，用电动机使惯性大的旋转体旋转，沿着要校正倾斜的方向的反方向提高旋转体的转速，产生反作用力偶，通过这样来防止翻倒。在该倒立控制玩具的情况下，由于通过控制旋转体的旋转，来取得平衡，因此即使是停止时或极慢速前进时，也能够防止翻倒。

在前述倒立控制玩具的情况下，作为对玩具的倾斜进行检测的倾斜检测传感器，是采用通过用受光元件来接受从发光元件送出后被地面反射的光、来检测倾斜的光传感器。但是，实际上不容易正确测定倾斜。即，在使用发光元件及受光元件的倾斜传感器的情况下，若对光要进行反射的地面是平坦的，则没有问题，但在地面有凹凸、或两侧部不存在地面的情况下(渡过宽度窄的桥上的情况等)，就不能正确地检测倾斜。

再有，在前述倒立控制玩具的情况下，将笔直站立的状态的受光量作为标准值，通过取得差分，来检测倾斜，但是笔直站立的状态(垂直方向)不一定是能够取得平衡的状态。例如，在玩具的重心位置从中心位置向左右偏移时，或受到侧风时，相对于垂直方向稍微倾斜的状态是能够倾斜的状态，尽管应该将该能够取得平衡的状态(角度)作为标准位置，但由于在上述的方法中是将垂直方向作为标准位置，因此不能保持平衡，有翻倒的可能性。

另外，作为检测本体倾斜的方法，也可以用角速度传感器检测角速度，并将该检测值进行积分，从而推定倾斜。但是，在将角速度输出进行积分的方法中，发生的问题是，噪声或偏置产生累积，不能持续进行倾斜角推定及防翻倒控制。再有，作为其它的倾斜检测装置，还有使用重块的倾斜传感器，在这种情况下具有的缺点是，也不能检测相对于平衡状体的倾斜，而且响应性差，不能瞬时检测倾斜。

专利文献1：特开2003-190654号公报

专利文献2：特开平11-47454号公报

发明内容

因此，本发明的目的在于提供一种防翻倒控制装置，该防翻倒控制装置能够正确推定相对于能够取得平衡的状态的倾斜角，而且噪声或偏置不累积，并且能够持续进行倾斜角推定及防翻倒控制。

为了达到上述目的，本发明是防翻倒控制装置，具有：左右自由倾斜的本体；将检测轴向着实质上前后方向安装在前述本体上的角速度传感器；将旋转轴向着实质上前后方向安装在前述本体上的电动机；检测前述电动机的旋转位置或旋转速度的旋转传感器；以及与前述电动机的转轴连接的惯性转子，用前述电动机使惯性转子旋转，利用随着前述惯性转子的旋转而产生的反作用转矩，来校正前述本体的倾斜，其中，前述防翻倒控制装置具有：根据前述角速度传感器的角速度输出ω₁和给予前述电动机的转矩指令τ₀、来对前述本体相对于平衡状态的倾斜角进行推定的倾斜角推定单元，并且使用由前述倾斜角推定单元推定的倾斜角推定值，来校正前述本体的倾斜。

本发明有关的防翻倒控制装置的动作原理虽与专利文献2相同，是用电动机使惯性转子旋转，利用随着前述惯性转子的旋转而产生的反作用转矩，来校正前述本体的倾斜，但校正时必须高精度地检测倾斜角。在本发明中，不是用传感器直接检测倾斜角，另外也不是将角速度传感器的角速度输出进行积分来求出。即，根据角速度传感器的角速度输出ω₁和给予电动机的转矩指令τ₀，来推定倾斜角。这里，所谓倾斜角，是相对于由重力产生的转矩、因弯道行驶而产生的离心力、以及由侧风等而产生的干扰转矩的总和成为零的平衡状态下的本体姿势的偏差角度。使用倾斜角推定值，来控制惯性转子的旋转，反复进行电动机转矩控制，使得倾斜角收敛于0。例如，对于本体的平衡轴，当从本体正面来看，倾斜角为左边方向时，为了维持平衡姿势，从本体正面来看，使惯性转子向左转方向加速。反之，当从本体正面来看，倾斜角为右边方向时，为了维持平衡姿势，从本体正面来看，使惯性转子向右转方向加速。

在本发明中，为了检测本体的倾斜角，由于没有使用倾斜角检测传感器，因此即使在地面有凹凸时，或者像平衡木那样在两侧部不存在地面时，也能够正确推定倾斜。另外，由于不需要将角速度传感器的角速度输出进行积分，因此即使在角速度传感器的输出中含有噪声或偏置时，也能够持续进行倾斜角推定，能够持续进行防翻倒控制。再有，与以往的使用重块的倾斜传感器相比，响应性远比它好，能够高精度地推定倾斜。这样，在本发明中，由于能够高精度地推定本体相对于平衡轴的倾斜角，而且响应性好，因此能够根据该倾斜角，相应高精度地控制对电动机施加的转矩。利用由电动机对惯性转子所施加的转矩的反作用转矩，能够向防止本体翻倒的方向高精度地控制本体的倾斜角。其结果，能够制成即使是停止状态或极慢速前进状态也不倒的不倒体。

根据理想的实施形态，最好具有：使用从目标倾斜角减去前述倾斜角推定值的倾斜角偏差信号，生成倾斜角速度指令ω₂的倾斜角速度指令生成单元；以及使用从前述倾斜角速度指令ω₂减去前述角速度传感器的角速度输出ω₁的倾斜角速度偏差信号ω₂-ω₁，生成给予前述电动机的转矩指令τ₀的转矩指令生成单元。在本发明中，首先设定目标倾斜角，从该目标倾斜角减去倾斜角推定值，求出倾斜角偏差信号，根据该偏差信号，生成相对于本体的倾斜角速度指令ω₂。接着，使用从倾斜角速度指令ω₂减去角速度传感器的角速度输出ω₁的倾斜角速度偏差信号ω₂-ω₁，能够生成给予电动机的转矩指令τ₀。

根据理想的实施形态，最好具有：根据倾斜角推定值，推定想要推倒本体的外部转矩的外部转矩推定单元；以及使用外部转矩推定值τ₃向外部转矩被抵消的方向，来校正转矩指令τ₀的转矩校正单元。所谓外部转矩，是因本体相对于平衡轴产生倾斜而对本体施加的重力、以及因干扰而产生的倾斜方向的转矩。由于通过用前馈控制来补偿外部转矩，即使在倾斜角环及倾斜角速度环的响应频率低的情况下，也能够持续进行防翻倒控制，因此能够进行稳定的控制。

根据理想的实施形态，最好具有：使用电动机的旋转速度、而且向着旋转速度减少的方向生成目标倾斜角的目标倾斜角生成单元。由于能够利用重力转矩，放出惯性转子具有的角动量，因此电动机的旋转速度不会饱和，能够持续进行控制。

能够将本发明有关的防翻倒控制装置用于自立行驶两轮车。这种两轮车具有：转向部；能够利用转向部进行转向的前轮；后轮；驱动后轮的后轮驱动部；以及支持前轮及后轮能够自由旋转的车架，通过将本发明用于该两轮车的防翻倒控制，能够提供行驶中当然不翻倒、即使在停止时或极慢速前进时也不翻倒的两轮车。另外，可以仅在停止时或极慢速前进时，使用本发明的防翻倒控制，在行驶中不使惯性转子旋转，而通过操作转向部来取得平衡。

如上所述，根据本发明，由于根据角速度传感器的角速度输出及电动机转矩指令，来推定相对于平衡状态的倾斜角，因此与以往的使用倾斜角检测传感器的情况不同，即使地面有凹凸，或者像平衡木那样在周围不存在地面，或者地面有一些倾斜，也能够正确推定相对于平衡状态的倾斜角。另外，由于不需要将角速度传感器的角速度输出进行积分，因此即使在角速度传感器的输出中含有噪声或偏置时，也能够持续进行倾斜角推定，能够持续进行防翻倒控制。再有，与以往的使用重块的倾斜传感器相比，响应性远比它好，能够高精度地推定倾斜。因而，能够高精度地控制对电动机转矩所施加的转矩，能够实现即使在停止状态或极慢速前进状态下如不翻倒的防翻倒控制装置。

附图说明

图1为采用本发明有关的防翻倒控制装置的自行车机器人的一个实施形态的立体图。

图2为自行车机器人的侧面图。

图3为自行车机器人的控制方框图。

图4为从自行车机器人的正面来看的模型图。

图5为施加干扰时利用陀螺传感器的本体角速度测定值。

图6为施加干扰时的电动机转矩指令。

图7为施加干扰时的本体倾斜角推定值。

标号说明

A  自行车机器人(本体)

1  转向用把手(转向部)

2  前轮

3  后轮

4  后轮驱动电动机(后轮驱动部)

5  车架

6  假人

7  陀螺传感器(角速度传感器)

8  惯性转子

9  平衡用电动机

10 编码器(旋转传感器)

11 控制基板

12 电池

20 计数器部

21 电动机速度计算部

22 目标倾斜角生成部

23 A/D部

24 倾斜角速度计算部

25 倾斜角推定部

26 校正转矩指令生成部

27 目标倾斜角速度生成部

28  转矩指令生成部

29  电动机转矩指令电压计算部

30  D/A部

具体实施方式

以下，参照附图说明本发明的理想实施形态。

(第1实施形态)

图1～图3所示为将本发明有关的防翻倒控制装置用于自行车机器人的第1实施形态。

该自行车机器人A具有：转向用把手1；能够利用转向用把手1进行转向的前轮2；后轮3；驱动后轮3的后轮驱动电动机4；支持前轮2及后轮3能够自由旋转的车架5；以及骑在车架5上的假人6。测定倾斜角速度的陀螺传感器(角速度传感器)7将它的检测轴向着自行车A的实质上前后方向，安装在车架5上。在假人6的胸部安装惯性转子8、驱动惯性转子8的平衡用电动机9、以及测定平衡用电动机9的旋转角度的编码器10。惯性转子8及电动机9的转轴也向着自行车A的实质上前后方向安装。这里，所谓实质上前后方向，也可以相对于严格的前后方向上下偏离若干角度。在假人6的后背，装备有控制平衡用电动机9的控制基板11及电池12。控制基板11上，装有驱动电动机9的驱动器、A/D转换器、D/A转换器、计数器、控制器等。

通常行驶中，通过操作把手1来取得平衡，防止翻倒。另外，在停止状态或极慢速前进状态下，由于仅操作把手1难以取得平衡，因此利用驱动惯性转子8时的反作用进行控制，以便取得平衡。

自行车机器人A利用图3所示的控制方框进行控制。该控制方框是控制基板11上放置的方框的一个例子。首先，在计数器部20中，对编码器10的输出脉冲进行计数。在电动机速度计算部21中，将计数器部20的输出变换为旋转角度之后，进行微分，求出平衡用电动机9的旋转速度。也可以装有除去干扰用的LPF(低通滤波器)。

目标倾斜角生成部22对平衡用电动机9的旋转速度乘以比例系数，在平衡用电动机9的旋转速度从自行车正面来看是左转时，使得求出目标倾斜角为从自行车正面来看是右边方向，而在平衡用电动机9的旋转速度从自行车正面来看是右转时，使得求出目标倾斜角为从自行车正面来看是左边方向。另外，也可以追加积分器，使得惯性转子8没有残留定常旋转。

另外，在A/D部23中，测定陀螺传感器7的角速度输出。在倾斜角速度计算部24中，对角速度输出乘以变换系数，计算倾斜角速度ω₁。

在倾斜角推定部25中，根据倾斜角速度ω₁及电动机转矩指令τ₂，计算基于包含自行车本体(惯性转子以外的部分)及惯性转子8的系统的倾斜角方向的运动方程式导出的、用后述的(式18)表示的倾斜角，再为了使其具有适当的推定速度，使环路稳定，串联增加一阶延迟要素，通过这样计算出倾斜角推定值。具体来说，对使用(式18)的计算值，串联增加作为一阶延迟要素的例如1/(0.1S+1)，但不限于此，可以增加成为适当的推定速度那样的任意延迟要素。这里，所谓倾斜角，是相对于由重力产生的转矩、因弯道行驶而产生的离心力、以及由侧风等而产生的干扰转矩的总和成为零的平衡状态下的本体姿势的偏差角度。

在校正转矩指令生成部26中，对倾斜角推定值乘以变换系数，计算出对自行车作用的外部转矩推定值，生成校正转矩(＝外部转矩推定值)τ₃。

在目标倾斜角速度生成部27中，对目标倾斜角与倾斜角推定值的偏差乘以比例增益，生成目标倾斜角速度ω₂。

在转矩指令生成部28中，对于目标倾斜角速度ω₂与倾斜角速度ω₁的偏差，生成例如利用PI控制的转矩指令τ₀。在电动机转矩指令电压计算部29中，对于将转矩指令τ₀与校正转矩τ₃相加的电动机转矩指令τ₂，乘以变换系数，生成指令电压。最后，在D/A部30中，对驱动器输出指令电压，控制平衡用电动机9的旋转。

这里，对于用(式18)表示的推定倾斜角的计算式的导出方法，在下面进行说明。

图4所示为从前方来看包含惯性转子8的自行车机器人A的模型，其中，O₁为触地点，O₂为惯性转子中心，m₁为本体质量，m₂为惯性转子质量，I₁为本体绕O₁的惯性矩，I₂为惯性转子绕O₂的惯性矩，θ₁为本体相对于垂直轴的倾斜角度，θ₂为惯性转子相对于本体的倾斜角度，τ₁为作用于本体的绕O₁的干扰转矩，τ₂为作用于惯性转子的电动机转矩，l_G为从O₁到本体重心位置的距离，l为从O₁到O₂的距离，g为重力加速度。首先，利用拉格朗日方程式，导出运动方程式。将自行车本体(惯性转子以外的部分)与惯性转子8合在一起的整个的动能T及位能U如下所示。

[数学式1]

$T=\frac{1}{2}{I}_1{{\stackrel{.}{\mathrm{\θ}}}_1}^2+\frac{1}{2}{I}_2{({\stackrel{\·}{\mathrm{\θ}}}_1+{\stackrel{\·}{\mathrm{\θ}}}_2)}^2+\frac{1}{2}{m}_2{l}^2{{\stackrel{\·}{\mathrm{\θ}}}_1}^2$ ……(式1)

U＝(m₁l_G+m₂l)gcosθ₁……(式2)

利用广义坐标及广义速度的微分量如下所示。

[数学式2]

$\frac{\∂T}{\∂{\stackrel{\·}{\mathrm{\θ}}}_1}=I_1{\stackrel{\·}{\mathrm{\θ}}}_1+I_2({\stackrel{\·}{\mathrm{\θ}}}_1+{\stackrel{\·}{\mathrm{\θ}}}_2)+m_2{l}^2{\stackrel{\·}{\mathrm{\θ}}}_1$ ……(式3)

$\frac{\∂T}{\∂{\stackrel{\·}{\mathrm{\θ}}}_2}=I_2({\stackrel{\·}{\mathrm{\θ}}}_1+{\stackrel{\·}{\mathrm{\θ}}}_2)$ ……(式4)

$\frac{\∂T}{\∂{\mathrm{\θ}}_1}=0$ ……(式5)

$\frac{\∂T}{\∂{\mathrm{\θ}}_2}=0$ ……(式6)

$\frac{\∂U}{\∂{\mathrm{\θ}}_1}=-(m_1{l}_G+m_{2}l)g\sin {\mathrm{\θ}}_1$ ……(式7)

$\frac{\∂U}{\∂{\mathrm{\θ}}_2}=0$ ……(式8)

将(式3)～(式8)代入拉格朗日方程式(式9)及(式10)。

[数学式3]

$\frac{d}{\mathrm{dt}}\left(\frac{\∂T}{\∂{\stackrel{\·}{\mathrm{\θ}}}_1}\right)-\frac{\∂T}{\∂{\mathrm{\θ}}_1}+\frac{\∂U}{\∂{\mathrm{\θ}}_1}={\mathrm{\τ}}_1$ ……(式9)

$\frac{d}{\mathrm{dt}}\left(\frac{\∂T}{\∂{\stackrel{\·}{\mathrm{\θ}}}_2}\right)-\frac{\∂T}{\∂{\mathrm{\θ}}_2}+\frac{\∂U}{\∂{\mathrm{\θ}}_2}={\mathrm{\τ}}_2$ ……(式10)

其结果，作为运动方程式，得到下面的(式11)及(式12)。

[数学式4]

$I_1{\stackrel{\·\·}{\mathrm{\θ}}}_1+I_2({\stackrel{\·\·}{\mathrm{\θ}}}_1+{\stackrel{\·\·}{\mathrm{\θ}}}_2)+m_2{l}^2{\stackrel{\·\·}{\mathrm{\θ}}}_1-(m_1{l}_G+m_{2}l)g\sin {\mathrm{\θ}}_1={\mathrm{\τ}}_1$ ……(式11)

$l_2({\stackrel{\·\·}{\mathrm{\θ}}}_1+{\stackrel{\·\·}{\mathrm{\θ}}}_2)={\mathrm{\τ}}_2$ ……(式12)

若将(式12)加以变形，则成为(式13)。

[数学式5]

${\stackrel{\·\·}{\mathrm{\θ}}}_2=\frac{{\mathrm{\τ}}_2}{I_2}-{\stackrel{\·\·}{\mathrm{\θ}}}_1$ ……(式13)

将它代入(式11)，若用θ₁近似sinθ₁，则得到下式。

[数学式6]

$(I_1+m_2{l}^2){\stackrel{\·\·}{\mathrm{\θ}}}_1-(m_1{l}_G+m_{2}l)g{\mathrm{\θ}}_1={\mathrm{\τ}}_1-{\mathrm{\τ}}_2$ ……(式14)

根据(式14)，本体的运动与惯性转子8的角度及角速度无关。

-本体倾斜角的推定-

本体倾斜角虽然也可以利用陀螺传感器7的输出的积分求得，但是由于偏差产生累积，从而不正确，所以必须用别的方法来求。因此，用模型的运动方程式，根据陀螺传感器7输出的本体倾斜角速度测定值及电动机转矩，来推定现在的倾斜角。若将运动方程式(式14)加以变形，则成为下式。

[数学式7]

${\mathrm{\θ}}_1+\frac{{\mathrm{\τ}}_1}{(m_1{l}_G+m_{2}l)g}=\frac{{\mathrm{\τ}}_2+(I_1+m_2{l}^2){\stackrel{\·\·}{\mathrm{\θ}}}_1}{(m_1{l}_G+m_{2}l)g}$ ……(式15)

另外，若设陀螺传感器7输出的本体倾斜角速度测定值为ω₁，则

[数学式8]

${\stackrel{\·\·}{\mathrm{\θ}}}_1\≅{\stackrel{\·}{\mathrm{\ω}}}_1$ ……(式16)

另外，在有干扰转矩τ₁时的视在平衡倾斜角度为

[数学式9]

$-\frac{{\mathrm{\τ}}_1}{(m_1{l}_G+m_{2}l)g}$ ……(式17)

因而，现在倾斜角度相对于视在平衡倾斜角度的偏差可根据(式15)用下式推定。

[数学式10]

${\widetilde{\mathrm{\θ}}}_1\≡{\mathrm{\θ}}_1-(-\frac{{\mathrm{\τ}}_1}{(m_1{l}_G+m_{2}l)g})\≅\frac{{\mathrm{\τ}}_2+(I_1+m_2{l}^2){\stackrel{\·}{\mathrm{\ω}}}_1}{(m_1{l}_G+m_{2}l)g}$ ……(式18)

式中，为了使其具有适当的推定速度，使环路稳定，也可以串联增加一阶延迟要素。

-外部转矩前馈-

利用由(式18)推定的偏差角度，补偿外部转矩。作为下式，与转矩相加。

[数学式11]

${\widetilde{\mathrm{\τ}}}_2=(m_1{l}_G+m_{2}l)g{\widetilde{\mathrm{\θ}}}_1$ ……(式19)

若设下式，即

[数学式12]

${\mathrm{\τ}}_2={\widehat{\mathrm{\τ}}}_2+{\widetilde{\mathrm{\τ}}}_2$ ……(式20)

则由于运动方程式(式14)成为

[数学式13]

$(I_1+m_2{l}^2){\stackrel{\·\·}{\mathrm{\θ}}}_1=-{\widehat{\mathrm{\τ}}}_2$ ……(式21)

因此能够补偿外部转矩。

-目标倾斜角度生成-

以将运动方程式第2式(式13)进行积分的形式，将惯性转子8的旋转速度*进行累积。由于电动机的旋转速度有限制，因此必须利用位置控制进行补偿，使得利用重力转矩放出存储的旋转速度。因此，如下述那样决定目标倾斜角。

在利用重力转矩放出旋转速度期间，假定设倾斜角为一定，则由于

[数学式14]

${\stackrel{\·\·}{\mathrm{\θ}}}_1=0$ ……(式22)

因此运动方程式(式14)及(式13)分别成为(式23)及(式24)。

[数学式15]

${\mathrm{\τ}}_2={\mathrm{\τ}}_1+(m_1{l}_G+m_{2}l)g{\mathrm{\θ}}_1=(m_1{l}_G+m_{2}l)g{\widetilde{\mathrm{\θ}}}_1$ ……(式23)

${\stackrel{..}{\mathrm{\θ}}}_2=\frac{{\mathrm{\τ}}_2}{I_2}=\frac{(m_1{l}_G+m_{2}l)g{\widetilde{\mathrm{\θ}}}_1}{I_2}$ ……(式24)

在想要以时间T_A放出存储的旋转速度*时，由于需要的角加速度为

[数学式16]

${\stackrel{..}{\mathrm{\θ}}}_2=-\frac{{\stackrel{.}{\mathrm{\θ}}}_2}{T_A}$ ……(式25)

因此，将(式24)与(式25)进行比较，可求出下式。

[数学式17]

${\widetilde{\mathrm{\θ}}}_1=-\frac{I_2{\stackrel{.}{\mathrm{\θ}}}_2}{T_A(m_1{l}_G+m_{2}l)g}$ ……(式26)

因而，作为位置环的目标值(目标倾斜角)，只要设定(式27)即可。

[数学式18]

${\mathrm{\θ}}_r=-\frac{I_2{\stackrel{.}{\mathrm{\θ}}}_2}{T_A(m_1{l}_G+m_{2}l)g}$ ……(式27)

放出时间T_A例如只要设定为T_A＝1sec等即可。

由于原理上没有残留定常偏差，因此倾斜角推定部25为了生成目标倾斜角度，不需要积分要素，但实际上，有时惯性转子8残留有低速的定常旋转。这可以认为D/A偏置是形成的原因。虽然维持原样没有问题，但若对目标倾斜角度的生成部分追加时间常数为10秒左右的积分器，则能够消除低速的定常旋转。

图5～图7所示为对于基于上述原理的包含惯性转子的自行车机器人的稳定性进行测定的结果。图5～图7所示为对于没有施加干扰的状态下的自行车机器人用手指从横向推按本体而施加干扰时的响应，图5是表示由陀螺传感器测定的本体角速度，图6是表示电动机转矩指令(额定转矩/3V)，图7是表示本体倾斜角推定值。另外，采样时间为1ms。

由图7可知，施加干扰之前的倾斜角推定值稳定维持在±0.05deg以内，能够维持稳定的平衡状态。再有可知，即使加上干扰时，也迅速收敛于稳定位置。根据该实验结果可以确认，本实施形态有关的自行车机器人能够停止而且不倒，即使对于干扰(包含定常阶跃状干扰)也能够应对。

以下，列出本发明的效果。

(1)由于不对陀螺传感器7的角速度输出进行积分，基于模型来推定倾斜角，因此即使在陀螺传感器7的输出中含有噪声或偏置时，也能够持续进行倾斜角推定，能够持续进行自行车的防翻倒控制。因而，能够制成即使是停止状态或极慢速前进状态也不倒的自行车。

(2)能够根据陀螺传感器7的输出来推定倾斜角，利用由平衡用电动机对惯性转子8施加的转矩的反作用，控制自行车的倾斜角。因而，能够制成即使是停止状态或极慢速前进状态也不倒的自行车。

(3)关于倾斜角推定，由于求出相对于平衡位置的倾斜角，因此除了重力转矩，再加上有弯道行驶中的离心力等干扰转矩时，也始终能够推定根据相对于平衡位置的倾斜角度产生的外部转矩，因此能够计算出将其抵消的校正转矩，即使在有干扰转矩时，也能够保持本体平衡。

(4)由于通过用前馈控制来补偿外部转矩，即使在倾斜角环、倾斜角速度环的响应频率低时，也能够持续进行防翻倒控制，因此能够进行稳定的控制。

(5)由于生成目标倾斜角度，以防止惯性转子的旋转速度的饱和，因此能够在电动机旋转速度饱和之前使倾斜角变化，利用重力转矩放出惯性转子8具有的角动量。因而，能够制成即使是停止状态或极慢速前进状态也能够持续进行防翻倒控制的控制装置。

若更详细叙述，则如下所述。

在从自行车正面来看倾斜角是左边方向时，为了维持其姿势，从自行车正面来看必须向左转方向使惯性转子8加速。在从自行车正面来看倾斜角是右边方向时，为了维持其姿势，从自行车正面来看必须向右转方向使惯性转子8加速。利用该方法，在电动机旋转速度增大时，能够主动使姿势倾斜，利用重力转矩放出惯性转子8具有的角动量，降低电动机旋转速度。通过将惯性转子8安装在转轴上，由于电动机旋转速度达到饱和之前的时间有余量，因此能够进行这样的控制。

关于目标倾斜角生成，是对电动机旋转速度乘以比例系数求得，在电动机旋转速度从自行车正面来看是左转时，使得目标倾斜角为从自行车正面来看是右边方向，而在电动机旋转速度从自行车正面来看是右转时，使得目标倾斜角为从自行车正面来看是左边方向。另外，由于还追加积分器，因此没有D/A转换器的偏置等原因而残留定常旋转的情况。

在上述实施形态中，说明了自行车机器人的防翻倒控制，但本发明不仅限定于此，也可以适用于例如专利文献2所示那样的倒立玩具或两足步行机器人等的防翻倒控制。即，在两足步行机器人的情况下，通过推定相对于平衡轴的倾斜角，能够始终实现稳定的步行。再有，本发明也能够适用于摩托车等两轮车暂时停止时的防翻倒控制。另外，是用(式18)表示推定倾斜角偏差的计算式，但这只不过是表示一个例子，利用对象模型的倾斜角偏差的推定式可能与(式18)不同。

Claims (6)

1.一种防翻倒控制装置，具有：

左右自由倾斜的本体；

安装在所述本体上的角速度传感器，所述角速度传感器的检测轴向着所述本体的实质上前后方向；

安装在所述本体上的电动机，所述电动机的旋转轴向着所述本体的实质上前后方向；

检测所述电动机的旋转位置或旋转速度的旋转传感器；以及

与所述电动机的转轴连接的惯性转子，

用所述电动机使惯性转子旋转，利用随着所述惯性转子的旋转而产生的反作用转矩来校正所述本体的倾斜，其特征在于，

所述防翻倒控制装置具有：根据所述角速度传感器的角速度输出ω₁和给予所述电动机的转矩指令τ₀来对所述本体相对于平衡状态的倾斜角进行推定的倾斜角推定单元，并且使用由所述倾斜角推定单元推定的倾斜角推定值来校正所述本体的倾斜。

2.如权利要求1所述的防翻倒控制装置，其特征在于，还具有：

使用从目标倾斜角减去所述倾斜角推定值的倾斜角偏差信号来生成倾斜角速度指令ω₂的倾斜角速度指令生成单元；以及

使用从所述倾斜角速度指令ω₂减去所述角速度传感器的角速度输出ω₁的倾斜角速度偏差信号ω₂-ω₁来生成给予所述电动机的转矩指令τ₀的转矩指令生成单元。

3.如权利要求2所述的防翻倒控制装置，其特征在于，还具有：

根据所述倾斜角推定值来推定想要推倒所述本体的外部转矩的外部转矩推定单元；以及

使用外部转矩推定值τ₃向外部转矩被抵消的方向来校正所述转矩指令τ₀的转矩校正单元。

4.如权利要求2或3所述的防翻倒控制装置，其特征在于，还具有：

使用所述电动机的旋转速度、而且向着所述旋转速度减少的方向来生成所述目标倾斜角的目标倾斜角生成单元。

5.如权利要求1至3的任一项所述的防翻倒控制装置，其特征在于，

所述本体是两轮车，该两轮车具有：

转向部；

能够利用所述转向部进行转向的前轮；

后轮；

驱动所述后轮的后轮驱动部；以及

支持所述前轮及后轮能够自由旋转的车架。

6.如权利要求4所述的防翻倒控制装置，其特征在于，

所述本体是两轮车，该两轮车具有：

转向部；

能够利用所述转向部进行转向的前轮；

后轮；

驱动所述后轮的后轮驱动部；以及

支持所述前轮及后轮能够自由旋转的车架。

Images