(19)【発行国】日本国特許庁(JP)
【公報種別】再公表特許(A1)
(11)【国際公開番号】WO2014061057
(43)【国際公開日】20140424
【発行日】20160905
(54)【発明の名称】倒立型移動体及びその制御方法
(51)【国際特許分類】
   B62K 17/00 20060101AFI20160808BHJP
   B62K 3/00 20060101ALI20160808BHJP
【FI】
   !B62K17/00
   !B62K3/00
【審査請求】有
【予備審査請求】未請求
【全頁数】22
【出願番号】2014541818
(21)【国際出願番号】JP2012006614
(22)【国際出願日】20121016
(11)【特許番号】5880726
(45)【特許公報発行日】20160309
(81)【指定国】 AP(BW,GH,GM,KE,LR,LS,MW,MZ,NA,RW,SD,SL,SZ,TZ,UG,ZM,ZW),EA(AM,AZ,BY,KG,KZ,RU,TJ,TM),EP(AL,AT,BE,BG,CH,CY,CZ,DE,DK,EE,ES,FI,FR,GB,GR,HR,HU,IE,IS,IT,LT,LU,LV,MC,MK,MT,NL,NO,PL,PT,RO,RS,SE,SI,SK,SM,TR),OA(BF,BJ,CF,CG,CI,CM,GA,GN,GQ,GW,ML,MR,NE,SN,TD,TG),AE,AG,AL,AM,AO,AT,AU,AZ,BA,BB,BG,BH,BN,BR,BW,BY,BZ,CA,CH,CL,CN,CO,CR,CU,CZ,DE,DK,DM,DO,DZ,EC,EE,EG,ES,FI,GB,GD,GE,GH,GM,GT,HN,HR,HU,ID,IL,IN,IS,JP,KE,KG,KM,KN,KP,KR,KZ,LA,LC,LK,LR,LS,LT,LU,LY,MA,MD,ME,MG,MK,MN,MW,MX,MY,MZ,NA,NG,NI,NO,NZ,OM,PA,PE,PG,PH,PL,PT,QA,RO,RS,RU,RW,SC,SD,SE,SG,SK,SL,SM,ST,SV,SY,TH,TJ,TM,TN,TR,TT,TZ,UA,UG,US,UZ,VC
(71)【出願人】
【識別番号】000003207
【氏名又は名称】トヨタ自動車株式会社
【住所又は居所】愛知県豊田市トヨタ町1番地
(74)【代理人】
【識別番号】100103894
【弁理士】
【氏名又は名称】家入 健
(72)【発明者】
【氏名】中山 貴裕
【住所又は居所】愛知県豊田市トヨタ町1番地 トヨタ自動車株式会社内
(72)【発明者】
【氏名】木田 祐介
【住所又は居所】愛知県豊田市トヨタ町1番地 トヨタ自動車株式会社内
(72)【発明者】
【氏名】平 哲也
【住所又は居所】愛知県豊田市トヨタ町1番地 トヨタ自動車株式会社内
(72)【発明者】
【氏名】尾藤 浩司
【住所又は居所】愛知県豊田市トヨタ町1番地 トヨタ自動車株式会社内
【テーマコード(参考)】
3D212
【Fターム(参考)】
3D212BB06
3D212BB08
3D212BB12
3D212BB25
3D212BB44
3D212BB53
3D212BB66
3D212BB74
3D212BB85
3D212BB94
(57)【要約】
倒立制御される倒立型移動体であって、ピッチ軸から第1の所定角度傾斜させた軸周りの角速度を検出する第1のセンサと、ピッチ軸から第2の所定角度傾斜させた軸周りの角速度を検出する第2のセンサと、ピッチ軸周りの角速度を検出する第3のセンサと、ピッチ軸の加速度を検出するピッチ軸加速度検出部と、ロール軸の加速度を検出するロール軸加速度検出部と、ヨー軸の加速度を検出するヨー軸加速度検出部と、検出した角速度に基づいて倒立制御を行う制御部を備える。制御部は、第1のセンサ及び第2のセンサが検出した角速度に基づいて算出したピッチ軸周りの第1角速度と、第3のセンサが検出したピッチ軸周りの第2角速度と、検出した加速度に基づいて算出したピッチ軸周りの第3角速度の相互関係に基づいて、特定の安全機能を発動する。
【特許請求の範囲】
【請求項1】
倒立制御される倒立型移動体であって、
前記倒立型移動体のヨー軸に直交する平面上で前記倒立型移動体のピッチ軸から第1の所定角度傾斜させた軸周りの角速度を検出する第1のセンサと、
前記倒立型移動体のヨー軸に直交する平面上で前記倒立型移動体のピッチ軸から第2の所定角度傾斜させた軸周りの角速度を検出する第2のセンサと、
前記倒立型移動体のピッチ軸周りの角速度を検出する第3のセンサと、
前記倒立型移動体のピッチ軸の加速度を検出するピッチ軸加速度検出部と、
前記倒立型移動体のロール軸の加速度を検出するロール軸加速度検出部と、
前記倒立型移動体のヨー軸の加速度を検出するヨー軸加速度検出部と、
前記第1のセンサ、前記第2のセンサ、及び前記第3のセンサのそれぞれが検出した角速度に基づいて前記倒立制御を行う制御部と、を備え、
前記制御部は、前記第1のセンサ及び前記第2のセンサのそれぞれで検出した角速度に基づいて算出した前記倒立型移動体のピッチ軸周りの第1角速度と、前記第3のセンサが検出した前記倒立型移動体のピッチ軸周りの第2角速度と、前記ピッチ軸加速度検出部、前記ロール軸加速度検出部、及び前記ヨー軸加速度検出部のそれぞれで検出した加速度に基づいて算出した前記倒立型移動体のピッチ軸周りの第3角速度の相互関係に基づいて、特定の安全機能を発動する、
倒立型移動体。
【請求項2】
前記制御部は、前記第1のセンサ及び前記第2のセンサのそれぞれが検出した角速度に基づいて前記倒立制御を行う第1の制御部と、前記第3のセンサが検出した角速度に基づいて前記倒立制御を行う第2の制御部と含み、
前記制御部は、前記第3のセンサが異常であると判断した場合、前記第1の制御部及び前記第2の制御部のうち、前記第1の制御部によって前記倒立型移動体の倒立制御を行い、
前記制御部は、前記第1のセンサ又は前記第2のセンサが異常であると判断した場合、前記第1の制御部及び前記第2の制御部のうち、前記第2の制御部によって前記倒立型移動体の倒立制御を行う、
請求項1に記載の倒立型移動体。
【請求項3】
前記安全機能は、前記倒立型移動体を停止させる制動機能である、
請求項1又は2に記載の倒立型移動体。
【請求項4】
前記第1のセンサは、さらに前記倒立型移動体のピッチ軸から第2の所定角度傾斜させた軸の加速度を検出し、
前記第2のセンサは、さらに前記倒立型移動体のピッチ軸から第1の所定角度傾斜させた軸の加速度を検出し、
前記倒立型移動体は、前記ヨー軸加速度検出部を第1のヨー軸加速度検出部として有するとともに、さらに前記倒立型移動体のヨー軸の加速度を検出する第2のヨー軸加速度検出部を有し、
前記制御部は、
前記第1のセンサ及び前記第2のセンサのそれぞれで検出した加速度に基づいて算出した前記倒立型移動体のピッチ軸の加速度と、前記ピッチ軸加速度検出部が検出した前記倒立型移動体のピッチ軸の加速度の比較、
前記第1のセンサ及び前記第2のセンサのそれぞれで検出した加速度に基づいて算出した前記倒立型移動体のロール軸の加速度と、前記ロール軸加速度検出部が検出した前記倒立型移動体のロール軸の加速度の比較、及び、
前記第1のヨー軸加速度検出部が検出した前記倒立型移動体のヨー軸の加速度と、前記第2のヨー軸加速度検出部が検出した前記倒立型移動体のヨー軸の加速度の比較を行い、
それらの比較結果に基づいて、前記安全機能を発動する、
請求項1乃至3のいずれか1項に記載の倒立型移動体。
【請求項5】
前記第1のセンサは、前記第1のヨー軸加速度検出部を有し、
前記第2のセンサは、前記第2のヨー軸加速度検出部を有し、
前記倒立型移動体は、前記ピッチ軸加速度検出部及び前記ロール軸加速度検出部を有する第4のセンサを有する、
請求項4に記載の倒立型移動体。
【請求項6】
前記制御部は、前記第1角速度と前記第2角速度が所定範囲内で一致しない場合、前記第1角速度と前記第3角速度の差分値となる第1差分値と、前記第2角速度と前記第3角速度の差分値となる第2差分値を比較し、
前記第2差分値が前記第1差分値よりも大きい場合、前記第3のセンサが異常であると判断し、
前記第2差分値が前記第1差分値よりも大きくない場合、前記第1のセンサ又は前記第2のセンサが異常であると判断する、
請求項2に記載の倒立型移動体。
【請求項7】
前記制御部は、所定の時間の間、前記第1角速度と前記第2角速度が所定範囲内で一致せず、かつ、前記第1差分値と前記第2差分値の比較結果が一致し続ける場合、前記第1のセンサ又は前記第2のセンサ、もしくは、前記第3のセンサが異常であると判断する、
請求項6に記載の倒立型移動体。
【請求項8】
前記第1の所定角度及び前記第2の所定角度のそれぞれは、相互にピッチ軸を対称として異なる方向に同一角度傾斜した角度となる、
請求項1乃至7のいずれか1項に記載の倒立型移動体。
【請求項9】
倒立型移動体のヨー軸に直交する平面上で前記倒立型移動体のピッチ軸から第1の所定角度傾斜させた軸周りの角速度を検出する第1のセンサと、前記倒立型移動体のヨー軸に直交する平面上で前記倒立型移動体のピッチ軸から第2の所定角度傾斜させた軸周りの角速度を検出する第2のセンサと、前記倒立型移動体のピッチ軸周りの角速度を検出する第3のセンサのそれぞれが検出した角速度に基づいて倒立制御を行う倒立型移動体の制御方法であって、
前記倒立型移動体のピッチ軸の加速度、前記倒立型移動体のロール軸の加速度、及び前記倒立型移動体のヨー軸の加速度を検出し、
前記第1のセンサ及び前記第2のセンサのそれぞれで検出した角速度に基づいて算出した前記倒立型移動体のピッチ軸周りの第1角速度と、前記第3のセンサが検出した前記倒立型移動体のピッチ軸周りの第2角速度と、前記検出した前記倒立型移動体のピッチ軸、ロール軸、及びヨー軸のそれぞれの加速度に基づいて算出した前記倒立型移動体のピッチ軸周りの第3角速度の相互関係に基づいて、特定の安全機能を発動する、
制御方法。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、倒立型移動体及びその制御方法に関し、特に、センサが検出した角速度に基づいて倒立型移動体を倒立制御する技術に関する。
【背景技術】
【0002】
倒立二輪車等の倒立型移動体において、システムに異常が発生した場合でも安全に搭乗者が降車できるようにすることは、重要な事項の1つである。倒立型移動体は、センサからの出力に基づいて倒立制御を行っている。そのため、センサの故障及び故障したセンサを精度良く検出し、故障したセンサからの出力に基づいた倒立制御を抑止することが、安全性を担保する上で非常に重要となってくる。
【0003】
特許文献1には、電源とセンサーエレクトロニクスボードと制御プロセッサの組み合わせが一まとめにされたパワーベースが三重余剰で構成された車両が開示されている。この車両は、三重の余剰センサのそれぞれから供給されるデータを比較することで、センサの故障を検出するようにしている。
【0004】
しかしながら、特許文献1に開示の車両のように、単純に同一構成でセンサを複数余剰としてしまうと、コストが増大してしまうという問題がある。例えば直交三軸の角速度を検出するために、センサーエレクトロニクスボードにおいて3つの直交三軸のそれぞれに対してジャイロセンサを搭載するようにした場合には、センサーエレクトロニクスボードが三重余剰であるため、合計で9つ(3×3)もジャイロセンサが必要となってしまう。すなわち、コストが増大してしまうという問題がある。なお、特許文献2には、相互に斜交配置された4つのジャイロで計測した角速度から、直交三軸の角速度を算出する慣性基準装置が開示されている。
【先行技術文献】
【特許文献】
【0005】
【特許文献1】特表2003−517340号公報
【特許文献2】特開2009−204419号公報
【発明の概要】
【発明が解決しようとする課題】
【0006】
本発明は、上述した知見に基づいてなされたものであって、故障検出精度を低下させることなく、コストを低減することができる倒立型移動体及びその制御方法を提供することを目的とする。
【課題を解決するための手段】
【0007】
本発明の第1の態様にかかる倒立型移動体は、倒立制御される倒立型移動体であって、前記倒立型移動体のヨー軸に直交する平面上で前記倒立型移動体のピッチ軸から第1の所定角度傾斜させた軸周りの角速度を検出する第1のセンサと、前記倒立型移動体のヨー軸に直交する平面上で前記倒立型移動体のピッチ軸から第2の所定角度傾斜させた軸周りの角速度を検出する第2のセンサと、前記倒立型移動体のピッチ軸周りの角速度を検出する第3のセンサと、前記倒立型移動体のピッチ軸の加速度を検出するピッチ軸加速度検出部と、前記倒立型移動体のロール軸の加速度を検出するロール軸加速度検出部と、前記倒立型移動体のヨー軸の加速度を検出するヨー軸加速度検出部と、前記第1のセンサ、前記第2のセンサ、及び前記第3のセンサのそれぞれが検出した角速度に基づいて前記倒立制御を行う制御部と、を備え、前記制御部は、前記第1のセンサ及び前記第2のセンサのそれぞれで検出した角速度に基づいて算出した前記倒立型移動体のピッチ軸周りの第1角速度と、前記第3のセンサが検出した前記倒立型移動体のピッチ軸周りの第2角速度と、前記ピッチ軸加速度検出部、前記ロール軸加速度検出部、及び前記ヨー軸加速度検出部のそれぞれで検出した加速度に基づいて算出した前記倒立型移動体のピッチ軸周りの第3角速度の相互関係に基づいて、特定の安全機能を発動するものである。
【0008】
本発明の第2の態様にかかる制御方法は、倒立型移動体のヨー軸に直交する平面上で前記倒立型移動体のピッチ軸から第1の所定角度傾斜させた軸周りの角速度を検出する第1のセンサと、前記倒立型移動体のヨー軸に直交する平面上で前記倒立型移動体のピッチ軸から第2の所定角度傾斜させた軸周りの角速度を検出する第2のセンサと、前記倒立型移動体のピッチ軸周りの角速度を検出する第3のセンサのそれぞれが検出した角速度に基づいて倒立制御を行う倒立型移動体の制御方法であって、前記倒立型移動体のピッチ軸の加速度、前記倒立型移動体のロール軸の加速度、及び前記倒立型移動体のヨー軸の加速度を検出し、前記第1のセンサ及び前記第2のセンサのそれぞれで検出した角速度に基づいて算出した前記倒立型移動体のピッチ軸周りの第1角速度と、前記第3のセンサが検出した前記倒立型移動体のピッチ軸周りの第2角速度と、前記検出した前記倒立型移動体のピッチ軸、ロール軸、及びヨー軸のそれぞれの加速度に基づいて算出した前記倒立型移動体のピッチ軸周りの第3角速度の相互関係に基づいて、特定の安全機能を発動するものである。
【発明の効果】
【0009】
上述した本発明の各態様によれば、故障検出精度を低下させることなく、コストを低減することができる倒立型移動体及びその制御方法を提供することができる。
【図面の簡単な説明】
【0010】
【図1】実施の形態にかかる倒立二輪車の概要構成を示す図である。
【図2】実施の形態にかかる制御装置の構成を示すブロック図である。
【図3】実施の形態にかかるセンサが検出する角速度及び加速度を示す図である。
【図4】実施の形態で検出・算出されるX軸、Y軸、及びZ軸の加速度を示す図である。
【図5】実施の形態で検出・算出される角速度を示す図である。
【図6】実施の形態にかかるセンサ故障検出処理を示すフローチャートである。
【図7】実施の形態にかかる加速度及び角速度の遷移の一例を示す図である。
【発明を実施するための形態】
【0011】
図1を参照して、本発明の実施の形態にかかる倒立二輪車1について説明する。図1は、本発明の実施の形態にかかる倒立二輪車1の概要構成を示す図である。
【0012】
倒立二輪車1は、ステッププレート3に搭乗した搭乗者が、倒立二輪車1の前後方向に荷重を作用させた際における、前後方向への倒立二輪車1の姿勢角(ピッチ角)をセンサで検出し、この検出結果に基づいて、倒立二輪車1の倒立状態を維持するように左右の車輪2を駆動するモータを制御する。すなわち、倒立二輪車1は、ステッププレート3に搭乗した搭乗者が前方に荷重を作用させて倒立二輪車1を前方に傾斜させると、倒立二輪車1の倒立状態を維持するように前方に加速し、搭乗者が後方に荷重を作用させて倒立二輪車1を後方に傾斜させると、倒立二輪車1の倒立状態を維持するように後方に加速するように、左右の車輪2を駆動するモータを制御する。倒立二輪車1は、制御の安定性を確保するために、モータを制御する制御系が2重化されている。
【0013】
なお、上述のモータの制御は、倒立二輪車1に搭載された制御装置10によって行われる。次に、図2を参照して制御装置10について説明する。
【0014】
続いて、図2を参照して、本発明の実施の形態にかかる制御装置10の構成について説明する。図2を参照して、本発明の実施の形態にかかる制御装置10の構成を示すブロック図である。
【0015】
制御装置10は、マイクロコントローラ11、12(以下、「マイコン」とも呼ぶ)、インバータ13〜16、モータ17、18、及びセンサ19〜22を有する。
【0016】
制御装置10は、倒立二輪車1の制御の安定性を確保するために、1系のシステム100と2系のシステム200とに二重化された二重系システムとなっている。すなわち、通常時には両方のシステム100、200によって倒立二輪車1の制御を行い、片方のシステムで異常が検出された場合には、他方のシステムによって倒立二輪車1を安全停止させるように制御する。1系のシステム100は、マイコン11、インバータ13、14、及びセンサ19〜21を含む。2系のシステム200は、マイコン12、インバータ15、16、及びセンサ22を含む。
【0017】
以下、倒立二輪車1のロール軸をX軸とも呼び、倒立二輪車1のピッチ軸をY軸とも呼び、倒立二輪車1のヨー軸をZ軸とも呼んで説明する。1系のシステム100では、センサ19とセンサ20は、それぞれの検出軸が、ピッチ軸及びロール軸のそれぞれからヨー軸と直交する平面上で45°の角度をなして対向するように配置されている。すなわち、センサ19とセンサ20は、それぞれの検出軸が、ピッチ軸に対して対称的となるように異なる向きに45°傾斜するように配置される。また、1系のシステム100では、センサ21は、その検出軸がヨー軸と一致するように配置されている。2系のシステム200では、センサ22は、その検出軸がピッチ軸と一致するように配置されている。
【0018】
マイコン11、12のそれぞれは、センサ19〜21及びセンサ22のそれぞれから出力される角速度信号に基づいて、上述したように、倒立二輪車1が倒立状態を維持するようにモータ17、18を制御するECU(Engine Control Unit)である。マイコン11、12のそれぞれは、CPU(Central Processing Unit)及び記憶部を有し、記憶部に格納されたプログラムを実行することによって、本実施の形態におけるマイコン11、12のそれぞれとしての処理を実行する。すなわち、マイコン11、12のそれぞれの記憶部に格納されるプログラムは、本実施の形態におけるマイコン11、12のそれぞれにおける処理を、CPUに実行させるためのコードを含む。なお、記憶部は、例えば、このプログラムや、CPUにおける処理に利用される各種情報を格納することができる任意の記憶装置を含んで構成される。記憶装置は、例えば、メモリ等である。
【0019】
マイコン11は、モータ17を制御する指令値を生成し、インバータ13に出力する。また、マイコン11は、モータ18を制御する指令値を生成し、インバータ14に出力する。ここで、マイコン11は、センサ19、20から出力される角速度信号に基づいて倒立二輪車1の姿勢角を算出し、算出した姿勢角に基づいて倒立二輪車1の倒立状態を維持するようにモータ17、18を制御する指令値を生成する。
【0020】
具体的には、マイコン11は、センサ19、20のそれぞれから出力される角速度信号が示す角速度から、ピッチ軸周りの角速度を算出する。マイコン11は、算出したピッチ軸周りの角速度を積分することで倒立二輪車1の前後方向の姿勢角(ピッチ角)を算出し、算出した姿勢角(ピッチ角)に基づいて倒立二輪車1の倒立状態を維持するように、モータ17、18を制御する指令値を生成する。ここで、センサ19、20のそれぞれから出力される角速度信号が示す角速度は、上述したようにピッチ軸から45°傾斜した軸周りにおける角速度となる。そのため、マイコン11は、後述するように、それらの角速度に対して回転行列計算を行うことで、ピッチ軸周りの角速度を算出し、算出したピッチ軸周りの角速度に基づいて倒立二輪車1の前後方向の姿勢角(ピッチ角)を算出する。
【0021】
また、マイコン11は、センサ19、20のそれぞれから出力される角速度信号が示す角速度から、ロール軸周りの角速度を算出する。マイコン11は、算出したロール軸周りの角速度を積分することで倒立二輪車1の左右方向の姿勢角(ロール角)を算出し、算出した姿勢角(ロール角)に基づいて倒立二輪車1を旋回させるように、モータ17、18を制御する指令値を生成する。ここで、センサ19、20のそれぞれから出力される角速度信号が示す角速度は、ロール軸から45°傾斜した軸周りにおける角速度ともなる。そのため、マイコン11は、後述するように、それらの角速度に対して回転行列計算を行うことで、ロール軸周りの角速度を算出し、算出したロール軸周りの角速度に基づいて倒立二輪車1の左右方向の姿勢角(ロール角)を算出する。
【0022】
また、マイコン11は、センサ21から出力される角速度信号が示すヨー軸周りの角速度に基づいて、任意の倒立二輪車1の制御を行うようにしてもよい。例えば、マイコン11は、倒立二輪車1の急激な旋回を防止するために、センサ21から出力される角速度信号が示す角速度が所定の角速度を超えていると判断した場合に、それ以上の角速度で倒立二輪車1が旋回しないように、モータ17、18を制御する指令値を生成するようにしてもよい。
【0023】
マイコン12は、モータ17を制御する指令値を生成し、インバータ15に出力する。また、マイコン12は、モータ18を制御する指令値を生成し、インバータ16に出力する。ここで、マイコン12は、センサ22から出力される角速度信号に基づいて倒立二輪車1の姿勢角を算出し、算出した姿勢角に基づいて倒立二輪車1の倒立状態を維持するようにモータ17、18を制御する指令値を生成する。
【0024】
具体的には、マイコン12は、センサ22から出力される角速度信号が示すピッチ軸周りの角速度を積分することで倒立二輪車1の前後方向の姿勢角(ピッチ角)を算出し、算出した姿勢角(ピッチ角)に基づいて倒立二輪車1の倒立状態を維持するように、モータ17、18を制御する指令値を生成する。
【0025】
インバータ13は、マイコン11から出力された指令値に基づいてPWM(Pulse Width Modulation)制御を行うことで、指令値に従ったモータ制御を行う駆動電流を生成し、モータ17に供給する。インバータ14は、マイコン11から出力された指令値に基づいてPWM制御を行うことで、指令値に従ったモータ制御を行う駆動電流を生成し、モータ18に供給する。インバータ15は、マイコン12から出力された指令値に基づいてPWM制御を行うことで、指令値に従ったモータ制御を行う駆動電流を生成し、モータ17に供給する。インバータ16は、マイコン12から出力された指令値に基づいてPWM制御を行うことで、指令値に従ったモータ制御を行う駆動電流を生成し、モータ18に供給する。
【0026】
モータ17、18のそれぞれは、二重巻のモータである。モータ17は、インバータ13から供給される駆動電流と、インバータ15から供給される駆動電流とに基づいて駆動される。モータ17を駆動することによって、倒立二輪車1の左側の車輪2が回転する。モータ18は、インバータ14から供給される駆動電流と、インバータ16から供給される駆動電流とに基づいて駆動される。モータ18を駆動することによって、倒立二輪車1の右側の車輪2が回転する。
【0027】
次に、図3を参照してセンサ19〜22のそれぞれについて説明する。図3は、センサ19〜22のそれぞれが検出する角速度及び加速度を示す図である。ここで、センサ19〜21のそれぞれは、1軸の角速度と2軸の加速度を検出可能な複合型センサチップを含む。すなわち、センサ19〜21のそれぞれは、ジャイロセンサ及び加速度センサとして機能する。また、センサ22は、1軸の角速度を検出するセンサチップである。すなわち、センサ22は、ジャイロセンサとして機能する。
【0028】
センサ19は、上述の通り、ピッチ軸及びロール軸のそれぞれと45°傾斜した軸周りの角速度ω0を検出し、検出した角速度ω0を示す角速度信号を生成してマイコン11に出力する。また、センサ19は、ピッチ軸及びロール軸のそれぞれと45°傾斜した軸向の加速度Acc_leftを検出し、検出した加速度Acc_leftを示す傾斜軸加速度信号を生成してマイコン11に出力する。角速度ω0の検出軸と加速度Acc_leftの検出軸は直交するように配置される。また、センサ19は、Z軸の加速度AccZ_0を検出し、検出した加速度AccZ_0を示すZ軸加速度信号を生成してマイコン11に出力する。
【0029】
センサ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に出力する。
【0030】
センサ21は、ヨー軸周りの角速度ω2を検出し、検出した角速度ω2を示す角速度信号を生成してマイコン11に出力する。また、センサ21は、X向の加速度AccX_0を検出し、検出した加速度AccX_0を示すX軸加速度信号を生成してマイコン11に出力する。また、センサ21は、Y軸方向の加速度AccY_0を検出し、検出した加速度AccY_0を示すY軸加速度信号を生成してマイコン11に出力する。
【0031】
センサ22は、ピッチ軸周りの角速度ω3を検出し、検出した角速度ω3を示す角速度信号を生成してマイコン12に出力する。
【0032】
続いて、図4を参照して、X軸、Y軸、及びZ軸のそれぞれにおける加速度の算出方法について説明する。図4は、本実施の形態において検出・算出されるX軸、Y軸、及びZ軸の加速度を示す図である。
【0033】
上述したように、センサ21は、その2つの加速度の検出軸のそれぞれが、X軸及びY軸と一致している。また、センサ19、20は、その1つの加速度の検出軸が、Z軸と一致している。そのため、マイコン11は、式(1)〜(3)に示すように、X軸及びY軸のそれぞれの加速度AccX、AccYを、センサ21が検出したX軸及びY軸のそれぞれの加速度AccX_0、AccY_0とし、Z軸の加速度AccZを、センサ19が検出したZ軸の加速度AccZ_0又はセンサ20が検出したZ軸の加速度AccZ'_0とする。なお、本実施の形態では、Z軸の加速度AccZとして、センサ19が検出したZ軸の加速度AccZ_0を使用する場合について説明する。
【0034】
【数1】
【0035】
すなわち、マイコン11は、式(1)に示すように、センサ21から出力されたX軸加速度情報が示すX軸の加速度AccX_0を、X軸の加速度AccXとする。また、マイコン11は、式(2)に示すように、センサ21から出力されたY軸加速度情報が示すY軸の加速度AccY_0を、Y軸の加速度AccYとする。また、マイコン11は、式(3)に示すように、センサ19から出力されたZ軸加速度情報が示すZ軸の加速度AccZ_0を、Z軸の加速度AccZとする。
【0036】
また、マイコン11は、後述するように、センサ19〜21の加速度検出に関する故障を検出するための比較に用いられる加速度AccX'_0、AccY'_0も算出する。ここで、上述したように、センサ19、20のそれぞれは、その1つの加速度の検出軸が、ピッチ軸及びロール軸のそれぞれに対して45°の角度をなしている。そのため、マイコン11は、式(4)に示すように、X軸の加速度AccX'_0を、センサ19、20のそれぞれが検出した加速度Acc_left、Acc_rightのそれぞれにおけるX軸成分の加速度を合成することで算出する。
【0037】
【数2】
【0038】
具体的には、X軸の角速度AccX'_0は、式(4)に示すように、センサ19が検出した加速度Acc0_leftとセンサ20が検出した加速度Acc0_rightの合計値(加算値)から、2の平方根を除算することで算出される。すなわち、マイコン11は、センサ19から出力された傾斜軸加速度情報が示す加速度Acc0_leftと、センサ20から出力された傾斜軸加速度情報が示す加速度Acc0_rightの合計値から、2の平方根を除算して、X軸の角速度AccX'_0を算出する。
【0039】
また、マイコン11は、式(5)に示すように、Y軸方向の加速度AccY'_0を、センサ19、20のそれぞれが検出した加速度Acc_left、Acc_rightのそれぞれにおけるY軸成分の加速度を合成することで算出する。
【0040】
【数3】
【0041】
具体的には、Y軸の角速度AccY'_0は、式(5)に示すように、センサ19が検出した加速度Acc0_leftとセンサ20が検出した加速度Acc0_rightの差分値(減算値)から、2の平方根を除算することで算出される。すなわち、マイコン11は、センサ19から出力された傾斜軸加速度情報が示す加速度Acc0_leftと、センサ20から出力された傾斜軸加速度情報が示す加速度Acc0_rightの差分値から、2の平方根を除算して、Y軸の角速度AccY'_0を算出する。
【0042】
続いて、図5を参照して、ロール軸及びピッチ軸における角速度の算出方法について説明する。図5は、本実施の形態において検出・算出される角速度を示す図である。
【0043】
上述したように、1系のシステム100におけるセンサ19、20のそれぞれは、その角速度の検出軸が、ピッチ軸及びロール軸のそれぞれに対して45°の角度をなしている。そのため、マイコン11は、式(6)に示すように、ロール軸周りの角速度Roll_0を、センサ19、20のそれぞれが検出した角速度ω0、ω1のそれぞれにおけるロール軸成分の角速度を合成することで算出する。
【0044】
【数4】
【0045】
具体的には、1系のシステム100において検出されるロール軸周りの角速度Roll_0は、式(6)に示すように、センサ19が検出した角速度ω0とセンサ20が検出した角速度ω1の差分値(減算値)から、2の平方根を除算することで算出される。すなわち、マイコン11は、センサ19から出力された角速度情報が示す角速度ω0と、センサ20から出力された角速度情報が示す角速度ω1の差分値から、2の平方根を除算して、ロール軸周りの角速度Roll_0を算出する。
【0046】
また、マイコン11は、式(7)に示すように、ピッチ軸周りの角速度Pitch_0を、センサ19、20のそれぞれが検出した角速度ω0、ω1のそれぞれにおけるピッチ軸成分の角速度を合成することで算出する。
【0047】
【数5】
【0048】
具体的には、1系のシステム100において検出されるピッチ軸周りの角速度Pitch_0は、式(7)に示すように、センサ19が検出した角速度ω0とセンサ20が検出した角速度ω1の合計値(加算値)から、2の平方根を除算することで算出される。すなわち、マイコン11は、センサ19から出力された角速度情報が示す角速度ω0と、センサ20から出力された角速度情報が示す角速度ω1の合計値から、2の平方根を除算して、ピッチ軸周りの角速度Pitch_0を算出する。
【0049】
上述したように、2系のシステム200におけるセンサ22は、その角速度の検出軸がピッチ軸と一致している。そのため、マイコン11は、式(8)に示すように、ピッチ軸周りの角速度Pitch_1を、センサ22が検出した角速度ω3とする。
【0050】
【数6】
【0051】
すなわち、マイコン11は、式(8)に示すように、2系のシステム200において検出されるピッチ軸周りの角速度Pitch_1を、センサ22から出力された角速度情報が示す角速度ω3とする。
【0052】
また、マイコン11は、後述するように、センサ19、20、22の角速度検出に関する故障を検出するための比較に用いられる角速度Pitch_Accも算出する。マイコン11は、式(9)に示すように、ピッチ軸周りの角速度Pitch_Accを、X軸、Y軸、及びZ軸の加速度AccX、AccY、AccZに基づいて近似的に算出する。
【0053】
【数7】
【0054】
具体的には、マイコン11は、式(9)に示すように、X軸の加速度AccXの二乗、Y軸の加速度AccYの二乗、及びZ軸の加速度AccZの二乗の合計値(加算値)の平方根を、X軸の加速度AccXから除算した値の逆正弦関数の値を、ピッチ軸周りの角速度Pitch_Accとして算出する。
【0055】
本実施の形態では、以上に説明したようにして得られたピッチ軸周りの角速度Pitch_0、Pitch_1、Pitch_Accに基づいて、ピッチ軸周りの角速度検出に関するセンサ故障の検出、及び、故障センサの特定を行う。このように、センサ19、20、22によって検出された角速度に基づいて得られたピッチ軸周りの角速度Pitch_0、Pitch_1に加えて、加速度に基づいて得られたピッチ軸周りの角速度Pitch_Accも利用して追加的に検証を行うことで、故障センサの特定精度を向上することを可能としている。
【0056】
続いて、図6を参照して、本発明の実施の形態にかかる制御装置10のセンサ故障検出処理について説明する。図6を参照して、本発明の実施の形態にかかる制御装置10のセンサ故障検出処理を示すフローチャートである。
【0057】
本実施の形態におけるセンサ故障検出では、上述したように加速度に基づいて得られたピッチ軸周りの角速度Pitch_Accも利用する。そのため、まず、マイコン11は、そのピッチ軸周りの角速度Pitch_Accを用いて、ピッチ軸周りの角速度検出に関するセンサ故障を判断する前に、加速度検出に関するセンサ故障を検出する処理を開始する(S1)。
【0058】
マイコン11は、センサ19、20のそれぞれから出力された傾斜軸加速度情報が示す加速度Acc0_left、Acc0_rightに基づいて、X軸の加速度AccX'_0を算出する。そして、マイコン11は、センサ21から出力されたX軸加速度情報が示すX軸の加速度AccXと、算出したX軸の加速度AccX'_0とを比較し、それぞれの加速度AccX、AccX'_0が所定範囲内で一致するか否かを判断する(S2)。ここで、「所定範囲内で一致」とは、完全一致する場合としてもよく、少し値が異なっていてもその差が所定値よりも小さい場合(完全一致を含む)としてもよい。
【0059】
加速度AccX、AccX'_0が所定範囲内で一致していないと判断した場合(S2:YES)、センサ19又はセンサ20、もしくは、センサ21において、ピッチ軸と45°傾斜する軸又はX軸の加速度が正常に検出できない故障が発生していると判断する(S3)。この場合、マイコン11は、倒立二輪車1に対して、加速度検出に関する異常に応じた所定の安全機能を発動する。例えばマイコン11、12は、倒立二輪車1を停止させるように制動制御を行う。具体的には、マイコン11は、マイコン12にセンサの故障を通知する信号を出力するとともに、その信号を受けたマイコン12とともに倒立二輪車1を停止させるように指令値を生成してインバータ13、14に出力する。また、警告音を発しての警告や、物理的なブレーキを掛ける制動制御を行うようにしてもよい。
【0060】
加速度AccX、AccX'_0が所定範囲内で一致していると判断した場合(S2:NO)、マイコン11は、センサ19、20のそれぞれから出力された傾斜軸加速度情報が示す加速度Acc0_left、Acc0_rightに基づいて、Y軸の加速度AccY'_0を算出する。そして、マイコン11は、センサ21から出力されたY軸加速度情報が示すY軸の加速度AccYと、算出したY軸の加速度AccY'_0とを比較し、それぞれの加速度AccY、AccY'_0が所定範囲内で一致するか否かを判断する(S4)。
【0061】
加速度AccY、AccY'_0が所定範囲内で一致していないと判断した場合(S3:YES)、センサ19又はセンサ20、もしくは、センサ21において、ピッチ軸と45°傾斜する軸又はY軸の加速度が正常に検出できない故障が発生していると判断する(S5)。この場合、マイコン11は、倒立二輪車1に対して、加速度検出に関する異常に応じた所定の安全機能を発動する。例えばマイコン11は、上述したように倒立二輪車1を停止させる等する。
【0062】
加速度AccY、AccY'_0が所定範囲内で一致していると判断した場合(S4:NO)、マイコン11は、センサ19から出力されたZ軸加速度情報が示すZ軸の加速度AccZと、センサ20から出力されたZ軸加速度情報が示すZ軸の加速度AccZ'_0とを比較し、それぞれの加速度AccZ、AccZ'_0が所定範囲内で一致するか否かを判断する(S6)。
【0063】
加速度AccZ、AccZ'_0が所定範囲内で一致していないと判断した場合(S6:YES)、センサ19又はセンサ20において、Z軸方向の加速度が正常に検出できない故障が発生していると判断する(S7)。この場合、マイコン11は、倒立二輪車1に対して、加速度検出に関する異常に応じた所定の安全機能を発動する。例えばマイコン11は、上述したように倒立二輪車1を停止させる等する。
【0064】
加速度AccZ、AccZ'_0が所定範囲内で一致していると判断した場合(S6:NO)、センサ19〜21による加速度検出が正常に行われていると判断する。そのため、マイコン11は、ピッチ軸周りの角速度検出に関するセンサ故障を検出する処理を開始する(S8)。
【0065】
マイコン11は、センサ19、20のそれぞれから出力された角速度情報が示す角速度ω0、ω1に基づいて、ピッチ軸周りの角速度Pitch_0を算出する。また、マイコン12は、センサ22から出力された角速度情報をマイコン11に出力する。マイコン11は、算出したピッチ軸周りの角速度Pitch_0と、マイコン12から出力された角速度情報が示すピッチ軸周りの角速度Pitch_1とを比較し、それぞれの角速度Pitch_0、Pitch_1が所定範囲内で一致するか否かを判断する(S9)。
【0066】
角速度Pitch_0、Pitch_1が所定範囲内で一致していると判断した場合(S9:NO)、センサ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を倒立制御する。
【0067】
角速度Pitch_0、Pitch_1が所定範囲内で一致していないと判断した場合(S9:YES)、センサ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)。
【0068】
角速度Pitch_1と角速度Pitch_Accの差分値が、角速度Pitch_0と角速度Pitch_Accの差分値よりも大きい場合(S11:YES)、2系のシステム200のセンサ21においてピッチ軸の角速度が正常に検出できない故障が発生していると判断する(S12)。
【0069】
この場合、マイコン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を即座に停止させなくても、他方の系から倒立制御を維持することも可能である。
【0070】
角速度Pitch_1と角速度Pitch_Accの差分値が、角速度Pitch_0と角速度Pitch_Accの差分値よりも大きくない場合(S11:NO)、1系のシステム100のセンサ19又はセンサ20においてピッチ軸と45°傾斜する軸の角速度が正常に検出できない故障が発生していると判断する(S13)。
【0071】
この場合、マイコン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を即座に停止させなくても、他方の系から倒立制御を維持することも可能である。
【0072】
ここで、上記処理では、一度のS12又はS13の判断で、その判断を確定するようにしてもよく、所定の時間の間、同一の判断が継続した場合に、その判断を確定するようにしてもよい。例えば図7に示すように、センサ19又はセンサ20においてピッチ軸と45°傾斜する軸の角速度が正常に検出できない故障が発生し、センサ19、20のそれぞれが検出した角速度ω0、ω1に基づいて算出されるピッチ軸周りの角速度Pitch_0が正常時より大きくなってしまったとする(図7の4415[msec])。この場合、ステップS13の判断が行われることになる。そして、所定の時間を20msecとした場合、ステップS13の判断が20msec継続したときに(図7の4435[msec])、ステップS13の判断を確定する。
【0073】
なお、ステップS13の判断の確定後に、さらに、センサ19又はセンサ20のうち、いずれが故障しているかを特定したい場合には、マイコン11は、ピッチ軸周りの角速度ω2に対して回転行列計算を行い、ピッチ軸と45°傾斜した2つの軸のそれぞれの成分の角速度を算出し、算出した角速度のそれぞれと角速度ω0、ω1とを比較して、その差がより大きくなる角速度ω0又はω1を検出するセンサを故障センサとして特定するようにすればよい。
【0074】
以上に説明したように、本実施の形態によれば、「ハの字」型に配置した2つのセンサ19、20によって検出した角速度から、ピッチ軸及びロール軸の角速度のそれぞれを合成して導出可能とするとともに、もう1つのセンサ22によって検出したピッチ軸の角速度による比較によって、センサの故障及び故障したセンサの検出を可能としている。これによれば、それぞれがピッチ軸を検出軸として角速度を検出する3つのセンサで構成されるシステムと比較して、同じ故障診断能力を有するとともに、ロール軸の角速度も検出することができる。すなわち、ロール軸を検出軸として角速度を検出するセンサが不要となり、コストを低減することができる。
【0075】
しかしながら、角速度に回転行列を掛けている場合には、それによって得られる角速度の分解能が下がってしまうという。例えば上述の例では角速度の検出軸が45°傾斜しているため、それから得られるピッチ軸の角速度は0.7倍程度の値となる。よって、微小の変化量が大きな変化量として得られてしまう場合がある。そのため、これに対する考慮のない場合には、分解能に差のある角速度で比較検証が行われることになるため、故障検出精度が低下してしまうという問題がある。
【0076】
それに対して、本実施の形態では、センサ19、20のそれぞれで検出した角速度に基づいて算出したピッチ軸周りの角速度と、センサ22が検出したピッチ軸周りの角速度と、センサ19、21のそれぞれで検出した加速度に基づいて算出したピッチ軸周りの角速度の相互関係に基づいて、センサの異常(故障)を診断するようにしている。これによれば、さらに分解能が下がっていない加速度から算出した角速度による比較検証も行うことで、1系のセンサ19、20と、2系のセンサ22のうち、いずれのセンサが故障しているかを特定するようにしているため、精度を落とすことなく、故障センサを特定することができる。すなわち、本実施の形態によれば、故障検出精度を低下させることなく、コストを低減することができる。
【0077】
また、本実施の形態では、センサ19、20、22のうち、1つのセンサ22は、ピッチ軸を検出軸として角速度を検出するように配置されているため、センサ22に関しては回転行列計算を行うことなく、ピッチ軸の角速度検出が可能である。よって、その分の処理を簡易にし、処理時間を低減することができる。
【0078】
なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。
【0079】
上記実施の形態では、制御対象とする倒立型移動体が倒立二輪車である場合について例示したが、車輪数は、これに限られない。なお、車輪数が異なる場合、言うまでもなく、それに対応する構成要素(インバータ、モータ)数も変更される。
【0080】
上記実施の形態では、センサ19、20の検出軸が、相互にピッチ軸を対称として異なる方向に同一の角度(45°)傾斜した軸とした場合について例示したが、この角度に限られない。例えば45°以外の角度であってもよく、センサ19とセンサ20の検出軸のそれぞれがピッチ軸から異なる角度で傾斜していてもよい。この場合、その角度に応じた回転行列計算を行うことでピッチ軸成分及びロール軸成分を抽出すればよいからである。
【0081】
上記実施の形態では、センサ19が検出したZ軸の加速度AccZ_0を、Z軸の加速度AccZとした場合について例示したが、センサ20が検出したZ軸の加速度AccZ'_0を、Z軸の加速度AccZとしてもよい。この場合、ステップS6において、センサ20から出力されたZ軸加速度情報が示すZ軸の加速度AccZと、センサ19から出力されたZ軸加速度情報が示すZ軸の加速度AccZ_0とを比較するようにすればよい。
【0082】
上記実施の形態では、角速度検出に関するセンサ故障検出として、ステップS9、S11で説明した異常診断を行う場合について説明したが、センサ19、20が検出した角速度に基づいた角速度Pitch_0と、センサ22が検出した角速度Pitch_1と、センサ19、21が検出した加速度に基づいた角速度Pitch_Accの相互関係に基づいてセンサの異常診断を行うものであれば、これに限られない。例えば、それぞれの角速度Pitch_0、Pitch_1、Pitch_Accが相互に所定範囲内で一致するか否かによる多数決比較を行うようにしてもよい。
【符号の説明】
【0083】
1 倒立二輪車
10 制御装置
11、12 マイコン
13、14、15、16 インバータ
17、18 モータ
19、20、21、22 センサ
100、200 システム
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【国際調査報告】