(57)【要約】      （修正有）【課題】走行中の姿勢変更時に生じる車輪への荷重抜けを抑制して転倒を防ぐために，鉛直方向における重心位置変化を抑制する姿勢安定制御装置と姿勢安定制御装置を備えた直列2輪移動機構を提供する。【解決手段】2個の車輪４１，４２を有し、車輪を走行方向に回転する走行駆動機構と，車輪を操舵する操舵駆動機構と，姿勢角度を検出する姿勢角度検出部６０を備えた直列2輪移動機構と，走行駆動機構を制御する走行制御部３０と，操舵駆動機構を制御する操舵制御部を備え，各制御部に指令を与える上位制御部２０を備えた直列2輪移動機構システムにおいて，脚の開脚角度を変更可能な開脚駆動部と，この開脚駆動部を制御する開脚制御部を備え，走行中に直列2移動機構の鉛直方向における重心位置変化を抑制するように開脚駆動部の開脚角度を制御することを特徴とする姿勢安定制御装置と制御装置を備える直列2輪移動機構。【選択図】図３
2017043222.tif 000002
【特許請求の範囲】
【請求項１】
少なくとも２個の車輪を直列的に配置可能な構成の移動装置において，前記車輪の少なくとも１個を走行方向に回転駆動する走行駆動機構と，前記走行駆動機構を制御する走行制御部と，前記車輪の少なくとも１個を操舵する操舵駆動機構と，前記操舵駆動機構を制御する操舵制御部と，鉛直方向成分を有する方向に構成物の一部を移動させて重心位置を変更する重心位置変更機構と，前記重心位置変更機構を制御する重心位置制御部とを有し，前記重心位置変更機構は，或る姿勢から他の姿勢に姿勢が変化したときに，前記或る姿勢で構成される鉛直方向の重心位置に，前記他の姿勢で構成される鉛直方向の重心位置を近付けるように前記重心位置変更機構を制御することを特徴とする移動装置。
【請求項２】
請求項1記載の移動装置において，前記重心位置変更機構は，前記車輪に接続される脚の開脚角度を変更可能な開脚駆動部として構成され，重心位置制御部は開脚制御部として構成されることを特徴とする移動装置。
【請求項３】
請求項２記載の移動装置において，前記走行制御部，前記操舵制御部。前記開脚制御部は，上記制御部から制御指令を受けることを特徴とする移動装置。
【請求項４】
請求項２記載の移動装置において，姿勢角度を検出する姿勢角度検出部を有し，前記開脚制御部は前記姿勢角度に基づいて開脚制御することを特徴とする移動装置。
【請求項５】
請求項４記載の移動装置において，前記開脚制御部における開脚目標角度が，前記姿勢角度検出部で検出される進行方向に対して垂直方向の姿勢角度を基に，前記上位制御部で算出される開脚角度指令に対応する鉛直方向の重心位置に対して，この重心位置の変化を抑制するように算出されることを特徴とする移動装置。
【請求項６】
請求項１記載の移動装置において， 前記開脚制御部における開脚目標角度の算出方法が前記姿勢角度検出部で検出される進行方向に対して垂直方向の姿勢角度，角速度および角加速度の条件により異なり，所定の重心位置変化を抑制するように開脚目標角度が算出されることを特徴とする移動装置。 
【請求項７】
請求項１記載の移動装置において，進行方向における上体姿勢の倒れ角度を変更可能な上体駆動部とこの上体駆動部を制御する上体制御部を備え，走行中に鉛直方向における重心位置変化を抑制するように，上体角度を制御することを特徴とする移動装置。
【請求項８】
請求項４記載の移動装置において，前記上体制御部における上体目標角度が，前記姿勢角度検出部で検出される進行方向に対して垂直方向の姿勢角度を基に，前記上位制御部で算出される上体角度指令に対応する鉛直方向の重心位置に対して，この重心位置の変化を抑制するように算出されることを特徴とする移動装置。
【請求項９】
請求項４記載の移動装置において，前記上体制御部における上体目標角度の算出方法が前記姿勢角度検出部で検出される進行方向に対して垂直方向の姿勢角度，角速度および角加速度の条件により異なり，所定の重心位置変化を抑制するように上体目標角度が算出されることを特徴とする移動装置。
【請求項１０】
請求項４記載の移動装置において，前記開脚制御部における開脚目標角度と上体制御部における上体目標角度が，重み係数により分配されていることを特徴とする移動装置。
【請求項１１】
少なくとも２個の車輪を走行方向に直列的に配置可能な車輪の少なくとも１個を走行方向に回転し，前記車輪の少なくとも１個を操舵し，或る姿勢から他の姿勢に姿勢が変化したときに，前記或る姿勢で構成される鉛直方向の重心位置に，前記他の姿勢で構成される鉛直方向の重心位置を近付けるように，所定構造物を鉛直方向成分の移動を有するように移動させて重心位置を変更する移動装置の制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は，移動装置及び移動装置の制御方法に係り，特に，自転車やバイクのように回転方向が一致するように車輪が一直線上に配置されており走行中に転倒することなく姿勢を変更可能な姿勢安定制御に好適な移動装置及び移動装置の制御方法に関する。
【背景技術】
【０００２】
複数の車輪を備えて自転車やバイクのように例えば各車輪の回転方向が一致するように一列的に車輪が配置された移動装置（以下，直列2輪移動機構と称する）がある。この移動装置は，例えば移動ロボット等に利用される。直列2輪移動機構は進行方向と垂直な方向（ロール方向）に対する姿勢制御が必要であり，姿勢安定制御手法として走行車輪を操舵する手法がある。この手法では，車輪操舵により直列2輪移動機構のロール方向に生じる遠心力を用いて姿勢安定化を図る。そのため，直列2輪移動機構が所望の経路に従って走行する場合，常にロール方向の姿勢を変更しながら走行することになる。すなわち，緩やかな経路変化の場合，車輪操舵角度が小さくロール方向の姿勢変化も小さいが，急激な経路変化の場合，車輪操舵角度が大きくロール方向の姿勢変化も大きい。
【０００３】
このように，走行中の姿勢変化により生じる荷重抜けを抑制するために，重心位置変化を抑制することは非常に重要である。従来，このような直列2輪移動機構に関する重心位置の調整技術，制御技術は，例えば，特開2005-271815号公報（特許文献２）に示すように提案されているように，自律走行型二輪車では，前輪と後輪が水平方向に揺動可能なスイングアームで連結されており，水平方向の重心位置をスイングアームで調整して安定な姿勢制御を実現するものが知られている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開2005-271815号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
しかしながら，上記の従来技術では，走行中において水平方向のみの重心位置を制御できるものの，鉛直方向における重心位置変化が考慮されていない。
【０００６】
車輪操舵による姿勢安定制御手法の場合，車輪に荷重が働いていることが重要であるが，先述のように姿勢を変更しながら走行するために車輪への荷重が抜け易い。特に，ロール方向に倒れた姿勢で旋回走行している状態から起き上がる方向の姿勢変化の場合である。直列2輪移動機構の重心が鉛直方向において上向きに移動してロール方向の姿勢角加速度に応じた力が直列2輪移動機構に上向きに働くため，姿勢変化時に走行車輪への荷重が小さくなる。このとき，抜け荷重が大きいと，車輪操舵による力が直列2輪移動機構に働かなくなり走行中に直列2輪移動機構が転倒することになる。
【０００７】
本発明の目的は，走行中において姿勢変化時に生じる荷重抜けを防止するために，鉛直方向における重心位置を調整可能な開脚部を有し，姿勢変化時において重心位置変化を抑制することが可能な移動装置及び移動装置の制御方法を提供することにある。
【課題を解決するための手段】
【０００８】
上記目的を達成するために，請求項１に係る発明では，少なくとも２個の車輪を直列的に配置可能な構成の移動装置において，前記車輪の少なくとも１個を走行方向に回転駆動する走行駆動機構と，前記走行駆動機構を制御する走行制御部と，前記車輪の少なくとも１個を操舵する操舵駆動機構と，前記操舵駆動機構を制御する操舵制御部と，鉛直方向成分を有する方向に構成物の一部を移動させて重心位置を変更する重心位置変更機構と，前記重心位置変更機構を制御する重心位置制御部とを有し，前記重心位置変更機構は，或る姿勢から他の姿勢に姿勢が変化したときに，前記或る姿勢で構成される鉛直方向の重心位置に，前記他の姿勢で構成される鉛直方向の重心位置を近付けるように前記重心位置変更機構を制御することを特徴とする移動装置を構成する。
【０００９】
請求項２記載の発明は，前記重心位置変更機構は，前記車輪に接続される脚の開脚角度を変更可能な開脚駆動部として構成され，重心位置制御部は開脚制御部として構成されることを特徴とする。
【００１０】
請求項３記載の発明は，前記走行制御部，前記操舵制御部。前記開脚制御部は，上記制御部から制御指令を受けることを特徴とする。
【００１１】
請求項４記載の発明は，姿勢角度を検出する姿勢角度検出部を有し，前記開脚制御部は前記姿勢角度に基づいて開脚制御することを特徴とする。
【００１２】
請求項５記載の発明は，上記開脚制御部における開脚目標角度が，上記姿勢角度検出部で検出される進行方向に対して垂直方向の姿勢角度を基に，上位制御部で算出される開脚角度指令に対応する鉛直方向の重心位置に対して，この重心位置の変化を抑制するように算出されることを特徴とする。
【００１３】
請求項６記載の発明は，上記開脚制御部における開脚目標角度の算出方法が姿勢角度検出部で検出される進行方向に対して垂直方向の姿勢角度，角速度および角加速度の条件により異なり，所定の重心位置変化を抑制するように開脚目標角度が算出されることを特徴とする。 
請求項７記載の発明は，進行方向における上体姿勢の倒れ角度を変更可能な上体駆動部とこの上体駆動部を制御する上体制御部を備え，走行中に鉛直方向における重心位置変化を抑制するように，上体角度を制御することを特徴とする。
【００１４】
請求項８記載の発明は，上記上体制御部における上体目標角度が，上記姿勢角度検出部で検出される進行方向に対して垂直方向の姿勢角度を基に，上記上位制御部で算出される上体角度指令に対応する鉛直方向の重心位置に対して，この重心位置の変化を抑制するように算出されることを特徴とする。
【００１５】
請求項９記載の発明は，上記上体制御部における上体目標角度の算出方法が上記姿勢角度検出部で検出される進行方向に対して垂直方向の姿勢角度，角速度および角加速度の条件により異なり，所定の重心位置変化を抑制するように上体目標角度が算出されることを特徴とする。
【００１６】
請求項１０記載の発明は，上記開脚制御部における開脚目標角度と上体制御部における上体目標角度が，重み係数により分配されていることを特徴とする。
【発明の効果】
【００１７】
本発明によれば，姿勢変化に伴う鉛直方向における重心位置変化を抑制することで，走行車輪に生じる荷重抜けを防止できる。従って，姿勢変化を伴う走行中においても姿勢を安定化が可能となる。
【図面の簡単な説明】
【００１８】
【図１】第1の実施例に係る直列2輪移動機構1の側面図である。
【図２】第1の実施例に係る直列2輪移動機構1の簡略モデルである。
【図３】第1の実施例に係る直列2輪移動機構システムの概略図である。
【図４】前輪部と後輪部の構成を示す概略図である。
【図５】開脚部の構成を示す概略図である。
【図６】第1の実施例に係る開脚目標角度の算出フローチャートである。
【図７】第2の実施例に係る直列2輪移動機構1の側面図である。
【図８】第2の実施例に係る直列2輪移動機構1の簡略モデルである。
【図９】第2の実施例に係る直列2輪移動機構システムの概略図である。
【図１０】上体部の構成を示す概略図である。
【図１１】第2の実施例に係る上体目標角度の算出フローチャートである。
【図１２】第2の実施例に係る開脚目標角度と上体目標角度の算出フローチャートである。
【発明を実施するための形態】
【００１９】
以下，図面を用いて実施例を説明する。
【実施例１】
【００２０】
本発明に関する姿勢安定制御装置の第1実施例として，まず，図1に直列2輪移動機構1の側面図を示す。胴体2の下に脚3aと3bが配置され，それぞれの脚先に走行車輪7aと7bが取り付けられている。走行車輪7a，7bは回転方向が一致するように一列に配置されている。また，各走行車輪7a，7bには車輪走行用駆動部6a，6bが取り付けられており，走行車輪の回転すなわち走行速度を調整可能である。さらに，各走行車輪7a，7bにはそれぞれ車輪操舵用駆動部4a，4bが取り付けられており，走行方向に対する走行車輪の操舵角度が調整可能である。図中の脚3aと3bは胴体2近傍で接続部を介して連動しており，開脚駆動部8により脚が開脚し，その開脚角度を調整可能である。ｘ軸，ｙ軸，ｚ軸周りの回転をそれぞれピッチ，ロール（バンク），ヨーと定義する。
【００２１】
本実施例では，走行車輪7a，7bの回転により操舵がない状態で各走行車輪7aから7bの方向，あるいは，各走行車輪7bから7aの方向に移動する。ｙ軸が移動方向である。この走行車輪7a，7bは常にｙ軸方向に間隔を持って配置されていても良く，或いは，所定の条件下で走行車輪7a，7bがｘ軸方向に揃っていて，その状態からｙ軸方向に間隔を持って配置されるように広げられても良い。
【００２２】
また，本実施例では，走行車輪7a，7bにはそれぞれ車輪走行用駆動部6a，6bが取り付けられているが，走行車輪7a，7bの一方のみに車輪走行用駆動部6a，6bいずれかが取り付けられるようにしても良い。例えば，走行車輪7a，7bのうちの操舵されない走行車輪にのみに車輪走行用駆動部6a，6bのいずれかを取り付けても良い。
【００２３】
図2に直列2輪移動機構1の簡略モデルを示す。図において，直列2輪移動機構1の重心200が走行車輪7a，7bの接地面から機体に沿って距離202の位置に，鉛直方向においては重心位置207に存在し，走行車輪7aは走行方向に対して操舵角度201だけ回転されている。また，脚3aと3bは開脚角度203だけ開脚され，走行車輪7aと7bは接地距離（ホイールベース長）204だけ離れる。さらに，直列2輪移動機構1はロール方向に傾いた角度を表すバンク角度206だけ傾いた状態になっている。
【００２４】
直列2輪移動機構システム100を図3に示す。図において，直列2輪移動機構システム100は主に上位制御部20，走行制御部30および直列2輪移動機構1で構成される。上位制御部20，走行制御部30は直列2輪移動機構1と一体的に構成してもよく，あるいは，上位制御部20，走行制御部30と直列2輪移動機構1とは無線等で遠隔的に接続しても良い。また，走行制御部30は走行速度補償器31，操舵補償器32および開脚補償器33から成る。ここで，走行制御部30は計算機で構成され，走行速度補償器31，操舵補償器32および開脚補償器33の各機能は計算機上でソフトウエアとして演算が実行されてもよい。
【００２５】
直列2輪移動機構40は，車輪操舵用駆動部4a（車輪操舵用駆動部4b），車輪走行用駆動部6a（車輪走行用駆動部6b）と走行車輪7a（走行車輪7b）からなる前輪部41，車輪操舵用駆動部4b(車輪操舵用駆動部4a)，車輪走行用駆動部6b（車輪走行用駆動部6a）と走行車輪7b（走行車輪7a）からなる後輪部42，開脚駆動部8と脚3a，3bからなる開脚部49および姿勢角度検出部60で構成される（胴2の記載は省略）。さらに，図4に示すように前輪部41と後輪部42は共に，車輪走行用駆動部6a（車輪走行用駆動部6b）としての走行駆動装置43，走行モータ44，走行エンコーダ45，車輪操舵用駆動部4 a (車輪操舵用駆動部4 b)としての操舵駆動装置46，操舵モータ47，操舵エンコーダ48でそれぞれ構成され，図5に示すように開脚部49は，開脚駆動部8としての開脚駆動装置50，開脚モータ51，開脚エンコーダ52で構成される。本実施例では，走行速度，操舵角度，開脚角度を検出するためにそれぞれ走行エンコーダ45，操舵エンコーダ48，開脚エンコーダ52を用いているが，別の検出手段を用いても良い。また，力学モデルを用いて推定しても良い。
【００２６】
上位制御部20では，図示されていないステレオカメラやレーザ等で構成される障害物検出部の検出結果を基に，直列2輪移動機構1が走行するのに適した経路が決定され，走行速度指令ｖｒｅｆ，姿勢（バンク）角度指令θｒｅｆ，開脚角度指令α０が算出され，それぞれ走行速度補償器31，操舵補償器32，開脚補償器33に出力される。
【００２７】
走行速度補償器31では，直列2輪移動機構1の走行速度制御が実施される。走行速度指令ｖｒｅｆと前輪部41と後輪部42の走行エンコーダ45でそれぞれ検出した走行速度ｖＦ，ｖＲを基に次式を用いて走行出力電圧ＶｖＦ，ＶｖＲが算出される。
【００２８】
【数１】
2017043222.tif 000003
【００２９】
式中のＫｖｍｎ（m=F，R，n=f，p，d，i）は走行制御ゲインを表す。算出された走行出力電圧ＶｖＦ，ＶｖＲが前輪部41と後輪部42の走行駆動装置43にそれぞれ入力され，増幅された電圧が走行モータ44にそれぞれ印加される。すなわち，走行速度指令ｖｒｅｆに対する偏差ｅｖＦ，ｅｖＲに基づいて比例，微分，積分のフィードバックを掛けて制御する。走行モータ44の回転角度が走行エンコーダ45により検出され，走行速度補償器31にフィードバックされる。走行制御ゲインを調整することで走行速度指令ｖｒｅｆに対して所望の追従性能を調整できる。
【００３０】
操舵補償器32では，直列2輪移動機構1の姿勢制御（バンク角度制御）が実施される。バンク角度指令θｒｅｆと姿勢角度検出部60により検出された姿勢角度（バンク角度）θを基に，次式を用いて走行中の姿勢安定化を図る車輪操舵角度出力ＶｓＦ，ＶｓＲが算出される。
【００３１】
【数２】
2017043222.tif 000004
【００３２】
式中のＫｓｍｎ（m=F，R，n=f，p，d，i）は操舵制御ゲインを表し，ＶｓＦ０，ＶｓＲ０は走行車輪7a，7bの操舵中央を表すオフセット出力電圧である。算出された操舵角度出力が図4に示す前輪部41と後輪部42の操舵駆動装置46にそれぞれ入力され，増幅された車輪操舵角度出力ＶｓＦ，ＶｓＲが操舵モータ47に印加される。すなわちバンク角度指令θｒｅｆに対する偏差ｅsに基づいて比例，微分，積分のフィードバックを掛けて制御する。操舵モータ47の回転角度が操舵エンコーダ48によりそれぞれ検出され，操舵補償器にフィードバックされる。数式2に表す操舵制御ゲイン調整することで所望の姿勢角度を達成できる。本実施例では，後述するが走行中に直列2輪移動機構の開脚角度が変化するため，開脚角度に応じて操舵制御ゲインＫｓｍｎ（m=F，R，n=f，p，d，i）は変化する。また，開脚角度が変化すると前輪部41と後輪部42で回転方向が一致する角度に偏差が生じるが，開脚角度αを基にＶｓＦ０，ＶｓＲ０を算出して調整可能である。さらに，数式2において，操舵エンコーダ48で検出された回転角度は用いられていないが，算出される車輪操舵角度出力ＶｓＦ，ＶｓＲとの偏差に対してフィードバック制御を実施することで，精度よく走行車輪を操舵可能である。
【００３３】
開脚補償器33では直列2輪移動機構1における脚3a，3bが成す開脚角度の制御が実施される。本実施例では，上位制御部20で算出された開脚角度指令α０に対応する鉛直方向における重心位置207が変化しないように開脚角度を変更する。すなわち，開脚角度指令α０に相当する重心位置207Ｈを算出し，バンク角度θが変化したとしても，その重心位置207Ｈを維持する開脚角度を開脚目標角度αｒｅｆとして算出するものである。
【００３４】
姿勢角度検出部60で検出されたバンク角度θを基に開脚目標角度αｒｅｆが算出され，開脚エンコーダ52で検出された開脚角度αを基に，次式を用いて開脚出力電圧ＶＣが算出される。
【００３５】
【数３】
2017043222.tif 000005
【００３６】
式中のＨは重心位置207，ｈ０は開脚しても変化しない部分の総合距離，ＬＷは脚3a，3bの長さ208 である。また， Ｋｃｎ（n=f，p，d，i）は開脚制御ゲインを表す。算出された開脚出力電圧ＶＣが開脚部の開脚駆動装置50に入力され，増幅された出力電圧が開脚モータ51に印加される。開脚角度αが開脚エンコーダ52により検出され，操舵補償器32と開脚補償器33にフィードバックされる。開脚補償器33により開脚角度αを変更するため，走行中において，鉛直方向における重心位置偏差ｄＨは抑制されるが，ホイールベース長204は変化する。先述した通り操舵補償器32では，開脚エンコーダ52で検出した開脚角度αを基にホイールベース長204を算出して，走行制御ゲインを変更する。
【００３７】
本実施例の開脚補償器33の1つの変形例を説明する。開脚補償器33を除いた部分は同じであるため，説明は省略する。変形例では，開脚補償器33の状態変数SWcに応じて，開脚目標角度αｒｅｆの算出方法を変更する。初期の状態変数SWcはOFFである。図6に開脚補償器33における開脚目標角度αｒｅｆの算出フローチャートを示す。
【００３８】
まず，フロー301において，開脚補償器33の状態変数SWcを確認する。この状態変数SWcは初期値がONとなっており前回のフローで，フロー307においてバンク角加速度の絶対値が閾値Bより大きいと判断された場合に状態変数SWcはOFFに設定されている。ONならばフロー307に遷移し，OFFならばフロー302に遷移する。フロー302では，バンク角度θと角速度（ｄθ/ｄｔ）の積θ×（ｄθ/ｄｔ）を算出して，正負を判断する。すなわち，直列2輪移動機構1がバンクした姿勢から起き上がる方向に姿勢が変化するか，もしくは倒れる方向に姿勢が変化するかを判断する。起き上がる方向に姿勢が変化する場合（負の場合）はフロー303に遷移して，倒れる方向に姿勢が変化する場合（正の場合）はフロー304に遷移する。フロー303では，図中に示す数式Ｘ１の2つの不等式を同時に満たすかどうかを判断する。式中のＦは算出時における機体に生じる鉛直上向きの力を表し，ＦＰは１サンプリング前の力を表す。すなわち，1サンプリング前は閾値Ａより小さいが，現時点で閾値Ａを越えているかどうかを判断し，機体に生じる鉛直上向きの力が閾値を越えるタイミングを判断する。数式Ｘ１を満たす場合はフロー305に遷移して，満たさない場合はフロー304に遷移する。フロー305では状態変数SWcはONとなり，フロー306に遷移する。フロー306では，数式Ｘ２により，保持する開脚角度αｓ（検出値），バンク角度θｓ（検出値），鉛直方向における重心位置ＨＳが設定され，フロー309に遷移する。すなわち，変形例では，検出された開脚角度αｓ（検出値），検出されたバンク角度θｓ（検出値）に基づいて重心位置ＨＳを算出して，その算出された重心位置ＨＳを維持するように開脚目標角度αｒｅｆを算出する。フロー307では，バンク角加速度の絶対値が閾値Bより大きいかどうかを判断する。大きい場合はフロー309に遷移し，小さい場合はフロー308に遷移する。フロー308では，状態変数SWcはOFFに設定され，フロー304に遷移する。
【００３９】
フロー304では，開脚目標角度αｒｅｆが数式Ｙ１に従って算出される。ここでは，開脚補償器33による重心位置変化ｄＨが小さく，姿勢安定化に対する影響が小さいと判断するため，開脚目標角度αｒｅｆは上位制御部20から出力された開脚角度指令α０と同じである。フロー309では，開脚補償器33による重心位置変化ｄＨが大きく姿勢安定化に対する影響が大きいため，抑制が必要である。ここでは，数式Ｙ２に従って開脚目標角度αｒｅｆが算出される。算出された開脚目標角度αｒｅｆと開脚エンコーダ52で検出した開脚角度αを基に開脚出力電圧Ｖcを算出する。算出方法は先述した方法と同じである。
【００４０】
以上のように，走行中において直列2輪移動機構1のバンク角度に応じて開脚角度を制御して鉛直方向における重心位置変化を抑制することで，走行車輪への荷重抜けによる転倒防止が可能となる。したがって，走行車輪操舵により安定した姿勢制御を実現できる。本実施例では，各補償器において，基本的なフィードバック補償器とフィードフォーワード補償器で構成したが，状態フィードバックの別の補償器で構成しても同様の結果が得られる。
【実施例２】
【００４１】
本発明に関する姿勢安定制御装置の他の実施例を説明する。該実施例は第１の実施例と異なる部分を説明する。よって，説明されない部分は原則的に第１の実施例と同様である。第2実施例として，まず，図7に直列2輪移動機構1の側面図を示す。図において，胴2に上体駆動部9が取り付けられており，ピッチ方向の上体回転角度（ピッチ角度）を調整することができる。図7の簡略モデルを図8に示す。図において，重心200は車輪7a，7bの接地面から鉛直方向における重心位置207に存在し，また，重心200はピッチ方向に上体回転角度209だけ傾いている。図7，8において，他の部分に関しては実施例1と同じであるため，説明は省略する。
【００４２】
本実施例における直列2輪移動機構システム101を図9に示す。実施例1との主な相違点は走行制御部30に上体補償器34が存在して，直列2輪移動機構1に上体部53が存在することである。図10に示すように上体部53は上体駆動装置54，上体モータ55，上体エンコーダ56で構成される。図9における他の部分は同じであるため，ここでは説明は省略する。
【００４３】
上位制御部20において，図示されていない障害物検出部の検出結果を基に，直列2輪移動機構1の適した走行経路が算出され，上体角度指令（ピッチ角度指令）φ０が上体補償器に出力される。
【００４４】
走行制御部30を構成する上体補償器34では，走行中におけるピッチ角度制御が実施される。上位制御部20で算出された上体角度指令φ０に対応する鉛直方向における重心位置207の位置変化を抑制できるように上体角度を調整する。すなわち，姿勢角度検出部60で検出されたバンク角度θを基に上体目標角度φｒｅｆが算出され，同じく姿勢角度検出部60で検出されたピッチ角度φを基に次式を用いて上体出力電圧Ｖｕが算出される。
【００４５】
【数４】
2017043222.tif 000006
【００４６】
式中のＨは重心位置207，ｈ１はピッチ角度に依存せずに変化しない部分の総合距離，Ｌｕは上体の長さである。また， Ｋｕｎ（n=f，p，d，i）は上体制御ゲインを表す。算出された上体出力電圧Ｖｕが上体部の上体駆動装置54に入力され，増幅された出力電圧が上体モータ55に印加される。上体角度が上体エンコーダ56により検出され，上体補償器34にフィードバックされる。数式4の上体制御ゲインを調整することで所望のピッチ角度を達成できる。上体は走行車輪7a，7bのどちら側に傾けても鉛直方向の重心位置は変更可能であるが，車輪を操舵している側に傾けることが望ましい。逆側に傾けると走行車輪の荷重抜けを誘発することがある。
【００４７】
数式4では上体エンコーダ56で検出された上体角度は用いられていないが，姿勢角度検出部60で検出されたピッチ角度の代用として用いられても良い。上体補償器34によりピッチ角度φを変更するため，走行中において，鉛直方向における重心位置偏差ｄＨは抑制される。
【００４８】
本実施例の上体補償器34において，第1の変形例を説明する。
第1の変形例では，上体補償器34の状態変数SWuに応じて，上体目標角度φｒｅｆの算出方法が変更される。初期の状態変数SWuはOFFである。図11に上体補償器34における上体目標角度φｒｅｆの算出フローチャートを示す。
【００４９】
まず，フロー401において，開脚補償器34の状態変数SWuを確認する。ONならばフロー407に遷移し，OFFならばフロー402に遷移する。フローにおいて，バンク角度θ，角速度（ｄθ/ｄｔ），バンク角加速度（ｄ２θ/ｄｔ２）を検出および算出して，各姿勢条件に従ってフローが遷移する。フロー404では，上体目標角度φｒｅｆが数式Ｙ３に従って算出される。このフローでは，上位制御部20で算出された上体指令角度φ０と同じである。フロー406では，状態変数SWuがONに設定された時のピッチ角度φＳ，バンク角度θｓおよび鉛直方向における重心位置Ｈｓが保持されて，フロー409に遷移する。フロー409では，上体目標角度φｒｅｆが数式Ｙ４に従って算出される。図11に示すフローチャートに従って算出された上体目標角度φｒｅｆと姿勢角度検出部60で検出したピッチ角度φを基に上体出力電圧Ｖｕを算出する。算出方法は先述した方法と同じである。
【００５０】
続いて，第2の変形例を説明する。ここでは，第1の変形例に対する変更点のみを説明する。第2の変形例では，開脚補償器33と上体補償器34を共に用いて，鉛直方向における重心位置変化が抑制される。開脚目標角度αｒｅｆと上体目標角度φｒｅｆは次式で算出される。
【００５１】
【数５】
2017043222.tif 000007
数式中のＰは開脚目標角度と上体目標角度の重み付け係数であり，実現できる姿勢の範囲内において任意に設定可能である。Ｐ＝０の時，第1の変形例と同じである。
【００５２】
また，各姿勢状態に従って開脚目標角度αｒｅｆと上体目標角度φｒｅｆの算出方法を変更する場合の算出フローチャートを図12に示す。フロー504，506，509が異なっており，フロー504では，開脚目標角度αｒｅｆと上体目標角度φｒｅｆが数式Ｙ５に従って算出される。このフローでは，上位制御部20で算出された開脚角度指令α０と上体指令角度φ０と同じである。フロー506では，状態変数SWuがONに設定された時の開脚角度αｓ，ピッチ角度φｓ，バンク角度θｓおよび鉛直方向における重心位置ＨＳが保持されて，フロー509に遷移する。フロー509では，開脚目標角度αｒｅｆと上体目標角度φｒｅｆが数式Ｙ６に従って算出される。
【００５３】
以上のように，走行中において直列2輪移動機構1のバンク角度に応じて上体角度もしくは開脚角度と上体角度の両方を制御して鉛直方向における重心位置変化を抑制することで，走行車輪への荷重抜けを防止することができる。すなわち，走行中における姿勢変化時の転倒防止が可能となる。したがって，走行車輪操舵により安定した姿勢制御を実現できる。本実施例では，各補償器において，基本的なフィードバック補償器とフィードフォーワード補償器で構成したが，状態フィードバックの別の補償器で構成しても同様の結果が期待できる。
【符号の説明】
【００５４】
1・・・直列2輪移動機構
2・・・胴
3a，3b・・・脚
4a，4b・・・車輪操舵用駆動部
6a，6b・・・車輪走行用駆動部
7a，7b・・・走行車輪
8・・・開脚駆動部
9・・・上体駆動部
20・・・上位制御部
30・・・走行制御部
31・・・走行速度補償器
32・・・操舵補償器
33・・・開脚補償器
34・・・上体補償器
41・・・前輪部
42・・・後輪部
43・・・走行駆動装置
44・・・走行モータ
45・・・走行エンコーダ
46・・・操舵駆動装置
47・・・操舵モータ
48・・・操舵エンコーダ
49・・・開脚部
50・・・開脚駆動装置
51・・・開脚モータ
52・・・開脚エンコーダ
53・・・上体部
54・・・上体駆動装置
55・・・上体モータ
56・・・上体エンコーダ
60・・・姿勢角度検出部
200・・・機構1の重心
201・・・車輪の操舵角度
202・・・機構に沿った接地面と重心位置の距離
203・・・開脚角度
204・・・ホイールベース長
206・・・バンク角度
207・・・鉛直方向における重心位置
208・・・脚の長さ
209・・・上体角度（ピッチ角度）
301〜309・・・実施例1の1変形例の各フロー
401〜409・・・実施例2の第1変形例の各フロー
501〜509・・・実施例2の第2変形例の各フロー
【図１】
2017043222.tif 000008
【図２】
2017043222.tif 000009
【図３】
2017043222.tif 000010
【図４】
2017043222.tif 000011
【図５】
2017043222.tif 000012
【図６】
2017043222.tif 000013
【図７】
2017043222.tif 000014
【図８】
2017043222.tif 000015
【図９】
2017043222.tif 000016
【図１０】
2017043222.tif 000017
【図１１】
2017043222.tif 000018
【図１２】
2017043222.tif 000019