CN104703871A - 倒立型移动体及其控制方法 - Google Patents

Abstract

被进行倒立控制的倒立型移动体具备：第一传感器，检测绕着从俯仰轴倾斜了第一预定角度的轴的角速度；第二传感器，检测绕着从俯仰轴倾斜了第二预定角度的轴的角速度；第三传感器，检测绕着俯仰轴的角速度；俯仰轴加速度检测部，检测俯仰轴的加速度；滚转轴加速度检测部，检测滚转轴的加速度；横摆轴加速度检测部，检测横摆轴的加速度；及控制部，基于检测出的角速度来进行倒立控制。控制部基于第一角速度、第二角速度、第三角速度的相互关系，使特定的安全功能起动，该第一角速度是基于由第一传感器及第二传感器检测出的角速度而算出的绕着俯仰轴的角速度，该第二角速度是第三传感器检测出的绕着俯仰轴的角速度，该第三角速度是基于检测出的加速度而算出的绕着俯仰轴的角速度。

Description

倒立型移动体及其控制方法

技术领域

本发明涉及倒立型移动体及其控制方法，尤其是涉及基于传感器检测出的角速度而对倒立型移动体进行倒立控制的技术。

背景技术

在倒立二轮车等倒立型移动体中，即使在系统发生了异常的情况下也能够安全地使搭乘者下车的情况是重要的事项之一。倒立型移动体基于来自传感器的输出来进行倒立控制。因此，高精度地检测传感器的故障及发生了故障的传感器并抑制基于来自发生了故障的传感器的输出的倒立控制的情况在确保安全性方面非常重要。

专利文献1中公开了一种将电源、传感器电子板和控制处理器的组合汇总到一起而成的动力基体以三重冗余构成的车辆。该车辆对从三重冗余传感器分别供给的数据进行比较，由此检测传感器的故障。

然而，如专利文献1公开的车辆那样，当简单地以同一结构将传感器设为多个冗余时，存在成本增大的问题。例如为了检测正交三轴的角速度，在传感器电子板中对3个正交三轴分别搭载陀螺仪传感器的情况下，传感器电子板为三重冗余，因此陀螺仪传感器总计需要9个(3×3)。即，存在成本增大的问题。另外，在专利文献2中公开了一种根据由相互斜交配置的4个陀螺仪计测出的角速度来算出正交三轴的角速度的惯性基准装置。

专利文献

专利文献1：日本特表2003-517340号公报

专利文献2：日本特开2009-204419号公报

发明内容

发明要解决的课题

本发明基于上述的见解而作出，目的在于提供一种不降低故障检测精度而能够减少成本的倒立型移动体及其控制方法。

用于解决课题的方案

本发明的第一方案的倒立型移动体被进行倒立控制，所述倒立型移动体具备：第一传感器，检测绕着在与所述倒立型移动体的横摆轴正交的平面上从所述倒立型移动体的俯仰轴倾斜了第一预定角度的轴的角速度；第二传感器，检测绕着在与所述倒立型移动体的横摆轴正交的平面上从所述倒立型移动体的俯仰轴倾斜了第二预定角度的轴的角速度；第三传感器，检测绕着所述倒立型移动体的俯仰轴的角速度；俯仰轴加速度检测部，检测所述倒立型移动体的俯仰轴的加速度；滚转轴加速度检测部，检测所述倒立型移动体的滚转轴的加速度；横摆轴加速度检测部，检测所述倒立型移动体的横摆轴的加速度；及控制部，基于所述第一传感器、所述第二传感器及所述第三传感器分别检测出的角速度来进行所述倒立控制，所述控制部基于第一角速度、第二角速度、第三角速度的相互关系，使特定的安全功能起动，该第一角速度是基于由所述第一传感器及所述第二传感器分别检测出的角速度而算出的绕着所述倒立型移动体的俯仰轴的角速度，该第二角速度是所述第三传感器检测出的绕着所述倒立型移动体的俯仰轴的角速度，该第三角速度是基于由所述俯仰轴加速度检测部、所述滚转轴加速度检测部及所述横摆轴加速度检测部分别检测出的加速度而算出的绕着所述倒立型移动体的俯仰轴的角速度。

本发明的第二方案的控制方法是倒立型移动体的控制方法，基于第一传感器、第二传感器、第三传感器分别检测出的角速度来进行倒立控制，该第一传感器检测绕着在与所述倒立型移动体的横摆轴正交的平面上从所述倒立型移动体的俯仰轴倾斜了第一预定角度的轴的角速度，该第二传感器检测绕着在与所述倒立型移动体的横摆轴正交的平面上从所述倒立型移动体的俯仰轴倾斜了第二预定角度的轴的角速度，该第三传感器检测绕着所述倒立型移动体的俯仰轴的角速度，所述控制方法中，检测所述倒立型移动体的俯仰轴的加速度、所述倒立型移动体的滚转轴的加速度及所述倒立型移动体的横摆轴的加速度，基于第一角速度、第二角速度、第三角速度的相互关系，使特定的安全功能起动，该第一角速度是基于由所述第一传感器及所述第二传感器分别检测出的角速度而算出的绕着所述倒立型移动体的俯仰轴的角速度，该第二角速度是所述第三传感器检测出的绕着所述倒立型移动体的俯仰轴的角速度，该第三角速度是基于检测出的所述倒立型移动体的俯仰轴、滚转轴及横摆轴各自的所述加速度而算出的绕着所述倒立型移动体的俯仰轴的角速度。

发明效果

根据上述的本发明的各方案，能够提供不降低故障检测精度而能够减少成本的倒立型移动体及其控制方法。

附图说明

图1是表示实施方式的倒立二轮车的简要结构的图。

图2是表示实施方式的控制装置的结构的框图。

图3是表示实施方式的传感器所检测的角速度及加速度的图。

图4是表示在实施方式中检测、算出的X轴、Y轴及Z轴的加速度的图。

图5是表示在实施方式中检测、算出的角速度的图。

图6是表示实施方式的传感器故障检测处理的流程图。

图7是表示实施方式的加速度及角速度的转化的一例的图。

具体实施方式

参照图1，说明本发明的实施方式的倒立二轮车1。图1是表示本发明的实施方式的倒立二轮车1的简要结构的图。

倒立二轮车1通过传感器来检测搭乘于踏脚板3的搭乘者使载荷沿倒立二轮车1的前后方向作用时的倒立二轮车1向前后方向的姿态角(俯仰角)，基于该检测结果，以维持倒立二轮车1的倒立状态的方式控制驱动左右车轮2的电动机。即，倒立二轮车1控制对左右车轮2进行驱动的电动机，使得当搭乘于踏脚板3的搭乘者使载荷向前方作用而使倒立二轮车1向前方倾斜时，以维持倒立二轮车1的倒立状态的方式向前方加速，当搭乘者使载荷向后方作用而使倒立二轮车1向后方倾斜时，以维持倒立二轮车1的倒立状态的方式向后方加速。倒立二轮车1为了确保控制的稳定性，将控制电动机的控制系统双重化。

另外，上述的电动机的控制由搭载于倒立二轮车1的控制装置10进行。接着，参照图2，说明控制装置10。

接着，参照图2，对本发明的实施方式的控制装置10的结构进行说明。参照图2，是表示本发明的实施方式的控制装置10的结构的框图。

控制装置10具有微型控制器11、12(以下，也称为“微控制器”)、逆变器13～16、电动机17、18及传感器19～22。

控制装置10为了确保倒立二轮车1的控制的稳定性，而形成双重化为1系的系统100和2系的系统200的双重系统。即，在通常时由两方的系统100、200进行倒立二轮车1的控制，在一方的系统检测出异常的情况下，由另一方的系统进行控制以使倒立二轮车1安全停止。1系的系统100包含微控制器11、逆变器13、14及传感器19～21。2系的系统200包含微控制器12、逆变器15、16及传感器22。

以下，将倒立二轮车1的滚转轴也称为X轴，将倒立二轮车1的俯仰轴也称为Y轴，将倒立二轮车1的横摆轴也称为Z轴而进行说明。在1系的系统100中，传感器19和传感器20以各自的检测轴分别从俯仰轴及滚转轴在与横摆轴正交的平面上呈45°的角度相向的方式配置。即，传感器19和传感器20以各自的检测轴相对于俯仰轴对称地向不同的方向倾斜45°的方式配置。而且，在1系的系统100中，传感器21以其检测轴与横摆轴一致的方式配置。在2系的系统200中，传感器22以其检测轴与俯仰轴一致的方式配置。

微控制器11、12分别是基于从传感器19～21及传感器22分别输出的角速度信号而如上述那样以使倒立二轮车1维持倒立状态的方式控制电动机17、18的ECU(Engine Control Unit)。微控制器11、12分别具有CPU(Central Processing Unit)及存储部，通过执行存储在存储部的程序，来执行本实施方式中的微控制器11、12各自的处理。即，在微控制器11、12各自的存储部所存储的程序包含用于使CPU执行本实施方式的微控制器11、12各自的处理的代码。另外，存储部构成为包含例如能够存储该程序、CPU的处理所利用的各种信息的任意的存储装置。存储装置例如是存储器等。

微控制器11生成对电动机17进行控制的指令值，并向逆变器13输出。而且，微控制器11生成对电动机18进行控制的指令值，并向逆变器14输出。在此，微控制器11基于从传感器19、20输出的角速度信号来算出倒立二轮车1的姿态角，基于算出的姿态角而生成以维持倒立二轮车1的倒立状态的方式控制电动机17、18的指令值。

具体而言，微控制器11根据从传感器19、20分别输出的角速度信号表示的角速度，算出绕着俯仰轴的角速度。微控制器11通过对算出的绕着俯仰轴的角速度进行积分来算出倒立二轮车1的前后方向的姿态角(俯仰角)，并基于算出的姿态角(俯仰角)，生成以维持倒立二轮车1的倒立状态的方式控制电动机17、18的指令值。在此，从传感器19、20分别输出的角速度信号表示的角速度如上述那样成为绕着从俯仰轴倾斜了45°的轴的角速度。因此，微控制器11如后述那样，通过对这些角速度进行旋转矩阵计算来算出绕着俯仰轴的角速度，并基于算出的绕着俯仰轴的角速度来算出倒立二轮车1的前后方向的姿态角(俯仰角)。

而且，微控制器11根据从传感器19、20分别输出的角速度信号表示的角速度，算出绕着滚转轴的角速度。微控制器11通过对算出的绕着滚转轴的角速度进行积分来算出倒立二轮车1的左右方向的姿态角(滚转角)，并基于算出的姿态角(滚转角)，生成以使倒立二轮车1转弯的方式控制电动机17、18的指令值。在此，从传感器19、20分别输出的角速度信号表示的角速度也成为绕着从滚转轴倾斜了45°的轴的角速度。因此，微控制器11如后述那样通过对这些角速度进行旋转矩阵计算，算出绕着滚转轴的角速度，并基于算出的绕着滚转轴的角速度，算出倒立二轮车1的左右方向的姿态角(滚转角)。

而且，微控制器11可以基于从传感器21输出的角速度信号表示的绕着横摆轴的角速度，进行任意的倒立二轮车1的控制。例如，微控制器11为了防止倒立二轮车1的急剧的转弯，可以在判断为从传感器21输出的角速度信号表示的角速度超过预定的角速度的情况下，生成以避免使倒立二轮车1以预定的角速度以上的角速度转弯的方式控制电动机17、18的指令值。

微控制器12生成对电动机17进行控制的指令值，并向逆变器15输出。而且，微控制器12生成对电动机18进行控制的指令值，并向逆变器16输出。在此，微控制器12基于从传感器22输出的角速度信号来算出倒立二轮车1的姿态角，并基于算出的姿态角，生成以维持倒立二轮车1的倒立状态的方式控制电动机17、18的指令值。

具体而言，微控制器12通过对从传感器22输出的角速度信号表示的绕着俯仰轴的角速度进行积分来算出倒立二轮车1的前后方向的姿态角(俯仰角)，并基于算出的姿态角(俯仰角)，生成以维持倒立二轮车1的倒立状态的方式控制电动机17、18的指令值。

逆变器13基于从微控制器11输出的指令值来进行PWM(PulseWidth Modulation)控制，由此生成进行按照指令值的电动机控制的驱动电流，并向电动机17供给。逆变器14基于从微控制器11输出的指令值来进行PWM控制，由此生成进行按照指令值的电动机控制的驱动电流，并向电动机18供给。逆变器15基于从微控制器12输出的指令值进行PWM控制，由此生成进行按照指令值的电动机控制的驱动电流，并向电动机17供给。逆变器16基于从微控制器12输出的指令值进行PWM控制，由此生成进行按照指令值的电动机控制的驱动电流，并向电动机18供给。

电动机17、18分别为双重卷绕的电动机。电动机17基于从逆变器13供给的驱动电流和从逆变器15供给的驱动电流而被驱动。通过驱动电动机17，倒立二轮车1的左侧的车轮2旋转。电动机18基于从逆变器14供给的驱动电流和从逆变器16供给的驱动电流而被驱动。通过驱动电动机18，倒立二轮车1的右侧的车轮2旋转。

接着，参照图3，分别说明传感器19～22。图3是表示传感器19～22分别检测的角速度及加速度的图。在此，传感器19～21分别包含能够检测1轴的角速度和2轴的加速度的复合型传感器芯片。即，传感器19～21分别作为陀螺仪传感器及加速度传感器发挥功能。而且，传感器22是检测1轴的角速度的传感器芯片。即，传感器22作为陀螺仪传感器发挥功能。

传感器19如上述那样检测绕着从俯仰轴及滚转轴分别倾斜了45°的轴的角速度ω0，生成表示检测出的角速度ω0的角速度信号而向微控制器11输出。而且，传感器19检测从俯仰轴及滚转轴分别倾斜了45°的轴向的加速度Acc_left，生成表示检测出的加速度Acc_left的倾斜轴加速度信号而向微控制器11输出。角速度ω0的检测轴与加速度Acc_left的检测轴正交配置。而且，传感器19检测Z轴的加速度AccZ_0，生成表示检测出的加速度AccZ_0的Z轴加速度信号而向微控制器11输出。

传感器20如上述那样检测绕着从俯仰轴及滚转轴分别倾斜了45°的轴的角速度ω1，生成表示检测出的角速度ω1的角速度信号而向微控制器11输出。而且，传感器20检测从俯仰轴及滚转轴分别倾斜了45°的轴的加速度Acc_right，生成表示检测出的加速度Acc_right的倾斜轴加速度信号而向微控制器11输出。角速度ω1的检测轴与加速度Acc_right的检测轴正交配置。即，角速度ω0的检测轴与加速度Acc_right的检测轴为同一轴，角速度ω1的检测轴与加速度Acc_left的检测轴为同一轴。而且，传感器20检测Z轴的加速度AccZ'_0，生成表示检测出的加速度AccZ_0的Z轴加速度信号而向微控制器11输出。

传感器21检测绕着横摆轴的角速度ω2，生成表示检测出的角速度ω2的角速度信号而向微控制器11输出。而且，传感器21检测X向的加速度AccX_0，生成表示检测出的加速度AccX_0的X轴加速度信号而向微控制器11输出。而且，传感器21检测Y轴方向的加速度AccY_0，生成表示检测出的加速度AccY_0的Y轴加速度信号而向微控制器11输出。

传感器22检测绕着俯仰轴的角速度ω3，生成表示检测出的角速度ω3的角速度信号而向微控制器12输出。

接着，参照图4，说明X轴、Y轴及Z轴各自的加速度的计算方法。图4是表示在本实施方式中检测、算出的X轴、Y轴及Z轴的加速度的图。

如上所述，传感器21的2个加速度的检测轴分别与X轴及Y轴一致。而且，传感器19、20的1个加速度的检测轴与Z轴一致。因此，如式(1)～(3)所示，微控制器11将X轴及Y轴各自的加速度AccX、AccY设为传感器21检测出的X轴及Y轴各自的加速度AccX_0、AccY_0，将Z轴的加速度AccZ设为传感器19检测出的Z轴的加速度AccZ_0或传感器20检测出的Z轴的加速度AccZ'_0。另外，在本实施方式中，说明使用传感器19检测出的Z轴的加速度AccZ_0作为Z轴的加速度AccZ的情况。

[数学式1]

AccX＝AccX_0…(1)

AccY＝AccY_0…(2)

AccZ＝AccZ_0…(3)

即，如式(1)所示，微控制器11将从传感器21输出的X轴加速度信息表示的X轴的加速度AccX_0设为X轴的加速度AccX。而且，如式(2)所示，微控制器11将从传感器21输出的Y轴加速度信息表示的Y轴的加速度AccY_0设为Y轴的加速度AccY。而且，如式(3)所示，微控制器11将从传感器19输出的Z轴加速度信息表示的Z轴的加速度AccZ_0设为Z轴的加速度AccZ。

另外，如后所述，微控制器11也算出用于检测与传感器19～21的加速度检测相关的故障的比较所用的加速度AccX'_0、AccY'_0。在此，如上述那样，传感器19、20各自的1个加速度的检测轴相对于俯仰轴及滚转轴分别成45°的角度。因此，如式(4)所示，微控制器11通过对传感器19、20分别检测出的加速度Acc_left、Acc_right各自的X轴分量的加速度进行合成来算出X轴的加速度AccX'_0。

[数学式2]

$\begin{array}{c}{\mathrm{AccX}}^{\text{'}}\_0=\\ \frac{1}{\sqrt{2}}(\mathrm{Acc}0\_\mathrm{left}+\mathrm{Acc}0\_\mathrm{right})\end{array}...\left(4\right)$

具体而言，如式(4)所示，将传感器19检测出的加速度Acc0_left与传感器20检测出的加速度Acc0_right的总和(相加值)除以2的平方根，由此算出X轴的角速度AccX'_0。即，微控制器11将从传感器19输出的倾斜轴加速度信息表示的加速度Acc0_left与从传感器20输出的倾斜轴加速度信息表示的加速度Acc0_right的总和除以2的平方根，来算出X轴的角速度AccX'_0。

而且，如式(5)所示，微控制器11通过对传感器19、20分别检测出的加速度Acc_left、Acc_right各自的Y轴分量的加速度进行合成，由此算出Y轴方向的加速度AccY'_0。

[数学式3]

$\begin{array}{c}{\mathrm{AccX}}^{\text{'}}\_0=\\ \frac{1}{\sqrt{2}}(-\mathrm{Acc}0\_\mathrm{left}+\mathrm{Acc}0\_\mathrm{right})\end{array}...\left(5\right)$

具体而言，如式(5)所示，将传感器19检测出的加速度Acc0_left与传感器20检测出的加速度Acc0_right的差值(相减值)除以2的平方根，由此算出Y轴的角速度AccY'_0。即，微控制器11将从传感器19输出的倾斜轴加速度信息表示的加速度Acc0_left与从传感器20输出的倾斜轴加速度信息表示的加速度Acc0_right的差值除以2的平方根，来算出Y轴的角速度AccY'_0。

接着，参照图5，说明滚转轴及俯仰轴的角速度的计算方法。图5是表示在本实施方式中检测、算出的角速度的图。

如上所述，1系的系统100中的传感器19、20各自的角速度的检测轴相对于俯仰轴及滚转轴分别呈45°的角度。因此，如式(6)所示，微控制器11通过对传感器19、20分别检测出的角速度ω0、ω1各自的滚转轴分量的角速度进行合成来算出绕着滚转轴的角速度Roll_0。

[数学式4]

$\mathrm{Roll}\_0=\frac{1}{\sqrt{2}}({\mathrm{\ω}}_0-{\mathrm{\ω}}_1)...\left(6\right)$

具体而言，如式(6)所示，将传感器19检测出的角速度ω0与传感器20检测出的角速度ω1的差值(相减值)除以2的平方根，由此算出在1系的系统100中检测的绕着滚转轴的角速度Roll_0。即，微控制器11将从传感器19输出的角速度信息表示的角速度ω0与从传感器20输出的角速度信息表示的角速度ω1的差值除以2的平方根，来算出绕着滚转轴的角速度Roll_0。

而且，如式(7)所示，微控制器11通过对传感器19、20分别检测出的角速度ω0、ω1各自的俯仰轴分量的角速度进行合成来算出绕着俯仰轴的角速度Pitch_0。

[数学式5]

$\mathrm{Pitch}\_0=\frac{1}{\sqrt{2}}({\mathrm{\ω}}_0+{\mathrm{\ω}}_1)...\left(7\right)$

具体而言，如式(7)所示，将传感器19检测出的角速度ω0与传感器20检测出的角速度ω1的总和(相加值)除以2的平方根，由此算出在1系的系统100中检测的绕着俯仰轴的角速度Pitch_0。即，微控制器11将从传感器19输出的角速度信息表示的角速度ω0与从传感器20输出的角速度信息表示的角速度ω1的总和除以2的平方根，来算出绕着俯仰轴的角速度Pitch_0。

如上所述，2系的系统200的传感器22的角速度的检测轴与俯仰轴一致。因此，如式(8)所示，微控制器11将绕着俯仰轴的角速度Pitch_1设为传感器22检测出的角速度ω3。

[数学式6]

Pitch_1＝ω3…(8)

即，如式(8)所示，微控制器11将在2系的系统200中检测的绕着俯仰轴的角速度Pitch_1设为从传感器22输出的角速度信息表示的角速度ω3。

另外，如后述那样，微控制器11也算出与用于检测传感器19、20、22的角速度检测相关的故障的比较所用的角速度Pitch_Acc。如式(9)所示，微控制器11基于X轴、Y轴及Z轴的加速度AccX、AccY、AccZ而近似地算出绕着俯仰轴的角速度Pitch_Acc。

[数学式7]

$\begin{array}{c}\mathrm{Pitch}\_\mathrm{Acc}=\\ -\arcsin (\mathrm{AccX}/\sqrt{{\mathrm{AccX}}^2+{\mathrm{AccY}}^2+{\mathrm{AccZ}}^2})\end{array}...\left(9\right)$

具体而言，如式(9)所示，微控制器11算出X轴的加速度AccX除以X轴的加速度AccX的平方、Y轴的加速度AccY的平方及Z轴的加速度AccZ的平方的总和(相加值)的平方根所得到的值的反正弦函数的值作为绕着俯仰轴的角速度Pitch_Acc。

在本实施方式中，基于如以上说明那样得到的绕着俯仰轴的角速度Pitch_0、Pitch_1、Pitch_Acc，进行与绕着俯仰轴的角速度检测相关的传感器故障的检测及故障传感器的确定。这样一来，除了基于由传感器19、20、22检测出的角速度而得到的绕着俯仰轴的角速度Pitch_0、Pitch_1之外，还利用基于加速度而得到的绕着俯仰轴的角速度Pitch_Acc追加性地进行验证，由此能够提高故障传感器的确定精度。

接着，参照图6，说明本发明的实施方式的控制装置10的传感器故障检测处理。参照图6，是表示本发明的实施方式的控制装置10的传感器故障检测处理的流程图。

在本实施方式的传感器故障检测中，也利用如上述那样基于加速度而得到的绕着俯仰轴的角速度Pitch_Acc。因此，首先，微控制器11在利用该绕着俯仰轴的角速度Pitch_Acc来判断与绕着俯仰轴的角速度检测相关的传感器故障之前，开始检测与加速度检测相关的传感器故障的处理(S1)。

微控制器11基于从传感器19、20分别输出的倾斜轴加速度信息表示的加速度Acc0_left、Acc0_right，算出X轴的加速度AccX'_0。并且，微控制器11对从传感器21输出的X轴加速度信息表示的X轴的加速度AccX与算出的X轴的加速度AccX'_0进行比较，判断各个加速度AccX、AccX'_0在预定范围内是否一致(S2)。在此，“在预定范围内一致”可以是完全一致的情况，也可以是虽然值稍有不同但其差比预定值小的情况(包括完全一致)。

在判断为加速度AccX、AccX'_0在预定范围内不一致的情况下(S2为“是”)，在传感器19或传感器20、或者传感器21中，判断为产生了从俯仰轴倾斜45°的轴或X轴的加速度无法正常检测的故障(S3)。在这种情况下，微控制器11对于倒立二轮车1，使与加速度检测相关的异常所对应的预定的安全功能起动。例如微控制器11、12以使倒立二轮车1停止的方式进行制动控制。具体而言，微控制器11向微控制器12输出通知传感器的故障的信号，并且与接受到该信号的微控制器12一起以使倒立二轮车1停止的方式生成指令值而向逆变器13、14输出。而且，也可以进行发出警告音的警告或施加物理性的制动的制动控制。

在判断为加速度AccX、AccX'_0在预定范围内一致的情况下(S2为“否”)，微控制器11基于从传感器19、20分别输出的倾斜轴加速度信息表示的加速度Acc0_left、Acc0_right，算出Y轴的加速度AccY'_0。并且，微控制器11对从传感器21输出的Y轴加速度信息表示的Y轴的加速度AccY与算出的Y轴的加速度AccY'_0进行比较，判断各个加速度AccY、AccY'_0在预定范围内是否一致(S4)。

在判断为加速度AccY、AccY'_0在预定范围内不一致的情况下(S3为“是”)，在传感器19或传感器20、或者传感器21中，判断为发生了从俯仰轴倾斜45°的轴或Y轴的加速度无法正常检测的故障(S5)。在这种情况下，微控制器11对于倒立二轮车1，使与加速度检测相关的异常所对应的预定的安全功能起动。例如微控制器11如上述那样使倒立二轮车1停止等。

在判断为加速度AccY、AccY'_0在预定范围内一致的情况下(S4为“否”)，微控制器11对从传感器19输出的Z轴加速度信息表示的Z轴的加速度AccZ与从传感器20输出的Z轴加速度信息表示的Z轴的加速度AccZ'_0进行比较，判断各个加速度AccZ、AccZ'_0在预定范围内是否一致(S6)。

在判断为加速度AccZ、AccZ'_0在预定范围内不一致的情况下(S6为“是”)，在传感器19或传感器20中，判断为发生了Z轴方向的加速度无法正常检测的故障(S7)。在这种情况下，微控制器11对于倒立二轮车1，使与加速度检测相关的异常所对应的预定的安全功能起动。例如微控制器11如上述那样使倒立二轮车1停止等。

在判断为加速度AccZ、AccZ'_0在预定范围内一致的情况下(S6为“否”)，判断为基于传感器19～21的加速度检测正常进行。因此，微控制器11开始检测与绕着俯仰轴的角速度检测相关的传感器故障的处理(S8)。

微控制器11基于从传感器19、20分别输出的角速度信息表示的角速度ω0、ω1，算出绕着俯仰轴的角速度Pitch_0。而且，微控制器12将从传感器22输出的角速度信息向微控制器11输出。微控制器11对算出的绕着俯仰轴的角速度Pitch_0与从微控制器12输出的角速度信息表示的绕着俯仰轴的角速度Pitch_1进行比较，判断各个角速度Pitch_0、Pitch_1在预定范围内是否一致(S9)。

在判断为角速度Pitch_0、Pitch_1在预定范围内一致的情况下(S9为“否”)，在传感器19、20中正常进行角速度检测，能得到正常的绕着俯仰轴的角速度Pitch_0、Pitch_1(S10)。因此，微控制器11、12分别维持倒立二轮车1的倒立控制。即，微控制器11基于算出的绕着俯仰轴的角速度Pitch_0，以维持倒立二轮车1的倒立状态的方式生成指令值并向逆变器13、14输出。而且，微控制器12基于从传感器22输出的角速度信息表示的绕着俯仰轴的角速度Pitch_1，以维持倒立二轮车1的倒立状态的方式生成指令值并向逆变器15、16输出。由此，微控制器11、12对倒立二轮车1进行倒立控制。

在判断为角速度Pitch_0、Pitch_1在预定范围内不一致的情况下(S9为“是”)，在传感器19或传感器20中，判断为存在发生了从俯仰轴倾斜了45°的轴或俯仰轴的角速度无法正常检测的故障的可能性。在这种情况下，微控制器11基于从传感器21输出的X轴加速度信息及Y轴加速度信息分别表示的X轴的加速度AccX及Y轴的加速度AccY和从传感器19输出的Z轴加速度信息表示的Z加速度AccZ，算出绕着俯仰轴的角速度Pitch_Acc。并且，微控制器11对绕着俯仰轴的角速度Pitch_0与绕着俯仰轴的角速度Pitch_Acc的差值和绕着俯仰轴的角速度Pitch_1与绕着俯仰轴的角速度Pitch_Acc的差值进行比较，判定角速度Pitch_1与角速度Pitch_Acc的差值是否大于角速度Pitch_0与角速度Pitch_Acc的差值(S11)。

在角速度Pitch_1与角速度Pitch_Acc的差值大于角速度Pitch_0与角速度Pitch_Acc的差值的情况下(S11为“是”)，判断为在2系的系统200的传感器21中发生了俯仰轴的角速度无法正常检测的故障(S12)。

在这种情况下，微控制器11使2系的系统200中的与角速度检测相关的异常所对应的预定的安全功能起动。例如微控制器11基于如上述那样得到的绕着俯仰轴的角速度Pitch_0以维持倒立二轮车1的倒立且如上述那样使倒立二轮车1停止的方式进行控制等。具体而言，微控制器11以使倒立二轮车1停止的方式生成指令值而向逆变器13、14输出。而且，微控制器11可以切断从2系的系统200的逆变器15、16向电动机17、18的输出。这通过在例如逆变器15、16与电动机17、18之间具备继电器电路而对继电器电路输出将逆变器15、16与电动机17、18的连接切断的信号来实现。而且，微控制器11向微控制器12输出通知传感器的故障的信号，微控制器12根据该信号，可以如上述那样向继电器电路输出信号等，抑制来自2系的系统200的电动机17、18的控制。而且，由此，在确定出一方的系统的故障的情况下，即便没有使倒立二轮车1立即停止，也能够根据另一方的系统来维持倒立控制。

在角速度Pitch_1与角速度Pitch_Acc的差值不大于角速度Pitch_0与角速度Pitch_Acc的差值的情况下(S11为“否”)，判断为在1系的系统100的传感器19或传感器20中发生了从俯仰轴倾斜45°的轴的角速度无法正常检测的故障(S13)。

在这种情况下，微控制器12使1系的系统100的与角速度检测相关的异常所对应的预定的安全功能起动。例如微控制器12基于如上所述得到的绕着俯仰轴的角速度Pitch_1以维持倒立二轮车1的倒立且如上述那样使倒立二轮车1停止的方式进行控制等。具体而言，微控制器11向微控制器12输出通知传感器的故障的信号。微控制器12根据该信号，以使倒立二轮车1停止的方式生成指令值而向逆变器15、16输出。而且，微控制器12可以切断从1系的系统100的逆变器13、14向电动机17、18的输出。这通过例如在逆变器13、14与电动机17、18之间具备继电器电路而对继电器电路输出将逆变器13、14与电动机17、18的连接切断的信号来实现。而且，微控制器11也可以如上述那样向继电器电路输出信号等，抑制来自1系的系统100的电动机17、18的控制。而且，由此，在确定出一方的系统的故障的情况下，即便没有使倒立二轮车1立即停止，也能够根据另一方的系统来维持倒立控制。

在此，在上述处理中，可以在一次的S12或S13的判断中确定该判断，也可以在预定的时间内使同一判断持续的情况下确定该判断。例如如图7所示，在传感器19或传感器20中发生从俯仰轴倾斜45°的轴的角速度无法正常检测的故障，基于传感器19、20分别检测出的角速度ω0、ω1而算出的绕着俯仰轴的角速度Pitch_0比正常时大(图7的4415[msec])。在这种情况下，进行步骤S13的判断。并且，在预定的时间为20msec的情况下，在步骤S13的判断持续了20msec时(图7的4435[msec])，确定步骤S13的判断。

另外，在确定步骤S13的判断后，进而，在要确定传感器19或传感器20中的任一个是否发生故障的情况下，微控制器11只要对绕着俯仰轴的角速度ω2进行旋转矩阵计算，算出从俯仰轴倾斜了45°的2个轴各自的分量的角速度，将算出的角速度分别与角速度ω0、ω1进行比较，确定检测该差进一步增大的角速度ω0或ω1的传感器作为故障传感器即可。

如以上说明那样，根据本实施方式，根据由配置成“ハ字”型的2个传感器19、20检测出的角速度，对俯仰轴及滚转轴的角速度分别进行合成而能够导出，并且通过由另一个传感器22检测出的俯仰轴的角速度的比较，能够进行传感器的故障及发生了故障的传感器的检测。由此，与分别将俯仰轴设为检测轴而检测角速度的3个传感器所构成的系统相比，具有相同的故障诊断能力，并且也能够检测滚转轴的角速度。即，不需要将滚转轴设为检测轴来检测角速度的传感器，能够减少成本。

然而，在向角速度乘以旋转矩阵时，由此得到的角速度的分辨度下降。例如在上述的例子中，角速度的检测轴倾斜45°，因此由其得到的俯仰轴的角速度为0.7倍左右的值。由此，微小的变化量有时作为大的变化量而得到。因此，在未对此作出考虑的情况下，以分辨度存在差别的角速度进行比较验证，因此存在故障检测精度下降的问题。

相对于此，在本实施方式中，根据基于由传感器19、20分别检测出的角速度而算出的绕着俯仰轴的角速度、传感器22检测出的绕着俯仰轴的角速度、基于由传感器19、21分别检测出的加速度而算出的绕着俯仰轴的角速度的相互关系，对传感器的异常(故障)进行诊断。由此，也进行基于根据分辨度未进一步下降的加速度而算出的角速度的比较验证，由此确定1系的传感器19、20和2系的传感器22中的哪一个传感器发生了故障，因此不降低精度而能够确定故障传感器。即，根据本实施方式，不降低故障检测精度而能够减少成本。

另外，在本实施方式中，传感器19、20、22中的1个传感器22以将俯仰轴设为检测轴而检测角速度的方式配置，因此关于传感器22不进行旋转矩阵计算，而能够进行俯仰轴的角速度检测。由此，能够简化相应的处理，减少处理时间。

另外，本发明并不局限于上述实施方式，在不脱离宗旨的范围内能够适当变更。

在上述实施方式中，例示了作为控制对象的倒立型移动体是倒立二轮车的情况，但车轮数并不局限于此。另外，在车轮数不同的情况下，不言而喻，与之对应的结构要素(逆变器、电动机)数也发生变更。

在上述实施方式中，例示了将传感器19、20的检测轴设为相对于俯仰轴彼此对称地向不同方向倾斜了同一角度(45°)的轴的情况，但并不局限于该角度。也可以是例如45°以外的角度，传感器19和传感器20的检测轴可以分别从俯仰轴倾斜不同的角度。在这种情况下，只要通过进行与该角度相应的旋转矩阵计算来提取俯仰轴分量及滚转轴分量即可。

在上述实施方式中，例示了将传感器19检测出的Z轴的加速度AccZ_0设为Z轴的加速度AccZ的情况，但也可以将传感器20检测出的Z轴的加速度AccZ'_0设为Z轴的加速度AccZ。在这种情况下，在步骤S6中，只要对从传感器20输出的Z轴加速度信息表示的Z轴的加速度AccZ与从传感器19输出的Z轴加速度信息表示的Z轴的加速度AccZ_0进行比较即可。

在上述实施方式中，说明了作为与角速度检测相关的传感器故障检测而进行了在步骤S9、S11中说明的异常诊断的情况，但只要是根据基于传感器19、20检测出的角速度的角速度Pitch_0、传感器22检测出的角速度Pitch_1和基于传感器19、21检测出的加速度的角速度Pitch_Acc的相互关系来进行传感器的异常诊断即可，并不局限于此。例如，可以进行各个角速度Pitch_0、Pitch_1、Pitch_Acc彼此在预定范围内是否一致的多数决定比较。

附图标记说明

1 倒立二轮车

10 控制装置

11、12 微控制器

13、14、15、16 逆变器

17、18 电动机

19、20，21，22 传感器

100、200 系统

Claims (9)

1.一种倒立型移动体，被进行倒立控制，所述倒立型移动体具备：

第一传感器，检测绕着在与所述倒立型移动体的横摆轴正交的平面上从所述倒立型移动体的俯仰轴倾斜了第一预定角度的轴的角速度；

第二传感器，检测绕着在与所述倒立型移动体的横摆轴正交的平面上从所述倒立型移动体的俯仰轴倾斜了第二预定角度的轴的角速度；

第三传感器，检测绕着所述倒立型移动体的俯仰轴的角速度；

俯仰轴加速度检测部，检测所述倒立型移动体的俯仰轴的加速度；

滚转轴加速度检测部，检测所述倒立型移动体的滚转轴的加速度；

横摆轴加速度检测部，检测所述倒立型移动体的横摆轴的加速度；及

控制部，基于所述第一传感器、所述第二传感器及所述第三传感器分别检测出的角速度来进行所述倒立控制，

所述控制部基于第一角速度、第二角速度、第三角速度的相互关系，使特定的安全功能起动，所述第一角速度是基于由所述第一传感器及所述第二传感器分别检测出的角速度而算出的绕着所述倒立型移动体的俯仰轴的角速度，所述第二角速度是所述第三传感器检测出的绕着所述倒立型移动体的俯仰轴的角速度，所述第三角速度是基于由所述俯仰轴加速度检测部、所述滚转轴加速度检测部及所述横摆轴加速度检测部分别检测出的加速度而算出的绕着所述倒立型移动体的俯仰轴的角速度。

2.根据权利要求1所述的倒立型移动体，其中，

所述控制部包含基于所述第一传感器及所述第二传感器分别检测出的角速度来进行所述倒立控制的第一控制部和基于所述第三传感器检测出的角速度来进行所述倒立控制的第二控制部，

在判断为所述第三传感器异常时，所述控制部通过所述第一控制部和所述第二控制部中的所述第一控制部来进行所述倒立型移动体的倒立控制，

在判断为所述第一传感器或所述第二传感器异常时，所述控制部通过所述第一控制部和所述第二控制部中的所述第二控制部来进行所述倒立型移动体的倒立控制。

3.根据权利要求1或2所述的倒立型移动体，其中，

所述安全功能是使所述倒立型移动体停止的制动功能。

4.根据权利要求1～3中任一项所述的倒立型移动体，其中，

所述第一传感器还检测从所述倒立型移动体的俯仰轴倾斜了第二预定角度的轴的加速度，

所述第二传感器还检测从所述倒立型移动体的俯仰轴倾斜了第一预定角度的轴的加速度，

所述倒立型移动体具有所述横摆轴加速度检测部作为第一横摆轴加速度检测部，并且还具有检测所述倒立型移动体的横摆轴的加速度的第二横摆轴加速度检测部，

所述控制部进行：

基于由所述第一传感器及所述第二传感器分别检测出的加速度而算出的所述倒立型移动体的俯仰轴的加速度与所述俯仰轴加速度检测部检测出的所述倒立型移动体的俯仰轴的加速度的比较；

基于由所述第一传感器及所述第二传感器分别检测出的加速度而算出的所述倒立型移动体的滚转轴的加速度与所述滚转轴加速度检测部检测出的所述倒立型移动体的滚转轴的加速度的比较；及

所述第一横摆轴加速度检测部检测出的所述倒立型移动体的横摆轴的加速度与所述第二横摆轴加速度检测部检测出的所述倒立型移动体的横摆轴的加速度的比较，

所述控制部基于这些比较结果，使所述安全功能起动。

5.根据权利要求4所述的倒立型移动体，其中，

所述第一传感器具有所述第一横摆轴加速度检测部，

所述第二传感器具有所述第二横摆轴加速度检测部，

所述倒立型移动体具有第四传感器，所述第四传感器具有所述俯仰轴加速度检测部及所述滚转轴加速度检测部。

6.根据权利要求2所述的倒立型移动体，其中，

在所述第一角速度与所述第二角速度在预定范围内不一致时，所述控制部对第一差值和第二差值进行比较，所述第一差值是所述第一角速度与所述第三角速度的差值，所述第二差值是所述第二角速度与所述第三角速度的差值，

在所述第二差值大于所述第一差值时，判断为所述第三传感器异常，

在所述第二差值不大于所述第一差值时，判断为所述第一传感器或所述第二传感器异常。

7.根据权利要求6所述的倒立型移动体，其中，

在预定的时间内所述第一角速度与所述第二角速度在预定范围内不一致且所述第一差值与所述第二差值的比较结果持续一致时，所述控制部判断为所述第一传感器或所述第二传感器、或者所述第三传感器异常。

8.根据权利要求1～7中任一项所述的倒立型移动体，其中，

所述第一预定角度及所述第二预定角度分别为相对于俯仰轴彼此对称地向不同的方向倾斜了同一角度的角度。

9.一种控制方法，是倒立型移动体的控制方法，基于第一传感器、第二传感器、第三传感器分别检测出的角速度来进行倒立控制，所述第一传感器检测绕着在与所述倒立型移动体的横摆轴正交的平面上从所述倒立型移动体的俯仰轴倾斜了第一预定角度的轴的角速度，所述第二传感器检测绕着在与所述倒立型移动体的横摆轴正交的平面上从所述倒立型移动体的俯仰轴倾斜了第二预定角度的轴的角速度，所述第三传感器检测绕着所述倒立型移动体的俯仰轴的角速度，

所述控制方法中，

检测所述倒立型移动体的俯仰轴的加速度、所述倒立型移动体的滚转轴的加速度及所述倒立型移动体的横摆轴的加速度，

基于第一角速度、第二角速度、第三角速度的相互关系，使特定的安全功能起动，所述第一角速度是基于由所述第一传感器及所述第二传感器分别检测出的角速度而算出的绕着所述倒立型移动体的俯仰轴的角速度，所述第二角速度是所述第三传感器检测出的绕着所述倒立型移动体的俯仰轴的角速度，所述第三角速度是基于检测出的所述倒立型移动体的俯仰轴、滚转轴及横摆轴各自的所述加速度而算出的绕着所述倒立型移动体的俯仰轴的角速度。