JP7065069B2 - 移動体の制御装置 - Google Patents

Description

本発明は、操縦者の操縦操作に応じて移動する移動体の制御装置に関する。

従来、この種の移動体の制御装置としては、例えば特許文献１に見られるものが知られている。この装置では、操縦者が移動体を操縦しているときに、移動体と障害物との距離が所定値以下になると、反発力と摩擦力とを設定し、これらの反発力及び摩擦力に応じて、移動体の前進速度とヨーレートとを、操縦者の操縦操作に応じたものから修正することで、移動体が障害物に接触するのを回避するようにしている。

特開平７－１１０７１１号公報

しかしながら、特許文献１に見られる技術では、移動体と障害物との距離が前記所定値以下になっても、該距離が該所定値に近い状況では、設定される反発力と摩擦力とが小さいため、上記距離が所定値以下になった直後では、移動体の前進速度とヨーレートを十分に修正できず、ひいては、障害物に対する移動体の回避動作が遅れ易い。

また、移動体が、特許文献１に記載されている車椅子のように、操縦者が搭乗する移動体である場合には、上記距離が所定値以下になった直後では、移動体の移動動作が設定される反発力や摩擦力の影響を受け難いため、操縦者は、障害物に接近し過ぎていることを体感的に認識し難い。このため、特に移動体の移動環境に、バーチャルウォール等の仮想的な障害物が設定される場合、該仮想的な障害物と移動体とが過剰に接近することが頻繁に生じやすい。

本発明はかかる背景に鑑みてなされたものであり、操縦者による操縦操作に応じて移動体を移動させているときに、移動体が障害物に接近し過ぎるのを適切に防止しつつ、該移動体の移動を円滑に行わせることを可能とする制御装置を提供することを目的とする。

本発明の移動体の制御装置の第１の態様は、上記の目的を達成するために、操縦者の操縦操作に応じて移動する移動体の制御装置であって、
前記操縦者の操縦操作に基づく前記移動体の移動指令である基本移動指令を生成する基本移動指令生成部と、
前記移動体の移動環境に存在し、又は該移動環境に設定された障害物と前記移動体との位置関係に応じて、前記基本移動指令を修正する移動指令修正部と、
前記移動指令修正部により修正された基本移動指令である修正後移動指令に応じて前記移動体の移動制御を行う移動制御部とを備えており、
前記移動指令修正部は、前記障害物と前記移動体との距離が第１所定値以下になったとき、前記移動体に、前記障害物から離れる方向のヨーレートを付加的に発生させるように前記基本移動指令を修正するように構成されていると共に、前記距離の減少に対する前記ヨーレートの大きさの増加率を、前記距離が前記第１所定値に近いほど、大きくするように前記基本移動指令を修正するように構成されていることを特徴とする（第１発明）。

なお、本発明における「障害物」は、移動体と実際に接触する可能性のある物体に限らず、移動体の移動環境に仮想的に設定された障害物（バーチャールウォール等の障害物）であってもよい。

また、本発明において、移動体に「付加的に発生させる」ヨーレートというのは、移動体の実際の運動によって発生するヨーレートを、操縦者の操縦操作に応じて規定される移動体の運動によって発生するヨーレートから変化させるヨーレート（操縦者の操縦操作に応じて規定される移動体の運動によって発生するヨーレートからの変化分のヨーレート）を意味する。

また、「前記距離の減少に対する前記ヨーレートの大きさの増加率」というのは、障害物と移動体との距離の単位減少量当たりの、前記ヨーレート（移動体に付加的に発生させるヨーレート）の増加量を意味する。

上記第１発明によれば、障害物と移動体との距離が第１所定値以下になったとき、障害物と移動体との距離の減少に対する、ヨーレート（移動体に付加的に発生させるヨーレートであって、移動体が障害物から離れる方向のヨーレート）の増加率が、前記距離が第１所定値に近いほど、大きくなるように前記基本移動指令が修正される。

このため、前記距離が第１所定値以下になった直後に、移動体に発生するヨーレートを、移動体が障害物から離れる方向に速やかに増加させることが可能となる。ひいては、前記距離が第１所定値以下になった直後から移動体が障害部から離れる方向に円滑に旋回し得るようになって、障害物へのさらなる接近を速やかに回避することが可能となる。

また、移動体が障害物にさらに接近すると、前記ヨーレートの増加率が小さくなるので、障害物に近い位置での移動体の挙動変化を抑制することが可能となる。
よって、第１発明によれば、操縦者による操縦操作に応じて移動体を移動させているときに、移動体が障害物に接近し過ぎるのを適切に防止しつつ、該移動体の移動を円滑に行わせることが可能となる。

上記第１発明では、前記移動指令修正部は、前記移動体に付加的に発生させるヨーレートを、前記距離と、前記障害物に対する前記移動体の向きと、前記移動体の移動速度とに応じて変化させるように前記基本移動指令を修正するように構成されていることが好ましい（第２発明）。

これによれば、移動体に付加的に発生させるヨーレートを、障害物と移動体との距離と、障害物に対する移動体の向きと、移動体の移動速度とを反映させて制御できるため、障害物に移動体が接近し過ぎるのを効果的に防止し得るように、上記ヨーレートを制御することが可能となる。

上記第１発明又は第２発明では、前記移動指令修正部は、さらに、前記距離が前記第１所定値以下になったとき、前記移動体の代表点の移動速度を、前記基本移動指令に応じて規定される移動速度である基本移動速度よりも絶対値が小さい移動速度に制限するように前記基本移動指令を修正するように構成されていると共に、前記距離の減少に対する前記移動速度の制限度合いの増加率を、前記距離が前記第１所定値に近いほど、大きくするように前記基本移動指令を修正するように構成されていることが好ましい（第３発明）。

なお、第３発明において、移動体の「移動速度の制限度合い」というのは、前記基本移動速度と、前記制限後の移動速度との差の大きさの、ある基準値（例えば、前記基本移動速度の大きさ）に対する大小度合いを意味する。
また、「前記距離の減少に対する前記移動速度の制限度合いの増加率」というのは、障害物と移動体との距離の単位減少量当たりの、前記移動速度の制限度合いの増加量を意味する。

上記第３発明によれば、障害物と移動体との距離が第１所定値以下の距離になったとき、障害物と移動体との距離の減少に対する移動速度（移動体の代表点の移動速度）の制限度合いの増加率が、前記距離が第１所定値に近いほど、大きくなるように前記基本移動指令が修正される。

このため、前記距離が第１所定値以下になった直後に、移動体の代表点の移動速度の制限度合いを速やかに増加させることが可能となる。ひいては、前記距離が第１所定値以下になった直後から、移動体の代表点の移動速度を速やかに減速させ、障害物へのさらなる接近を速やかに抑制することが可能となる。

また、移動体が障害物にさらに接近すると、前記移動速度の制限度合いの増加率が小さくなるので、障害物に近い位置での移動体の挙動変化を抑制することが可能となる。

上記第３発明では、前記移動指令修正部は、前記距離が前記第１所定値以下になったとき、前記距離に応じた前記移動速度の制限度合いを示す第１制限度合い指標値を設定する処理と、前記障害物に対する前記移動体の向きに応じた前記移動速度の制限度合いを示す第２制限度合い指標値を設定する処理とを実行し、該第１制限度合い指標値及び該第２制限度合い指標値に応じて前記移動体の代表点の移動速度を制限するように構成されていると共に、前記距離が前記第１所定値よりも小さい第２所定値以下の距離である場合に設定する前記第２制限度合い指標値により示される制限度合いを第Ａ制限度合い、前記距離が前記第２所定値よりも大きい距離である場合に設定する前記第２制限度合い指標値により示される制限度合いを第Ｂ制限度合いと定義したとき、前記移動体の向きの可変範囲のうちの少なくとも一部の範囲において、前記第Ａ制限度合いが前記第Ｂ制限度合いよりも大きくなるように前記第２制限度合い指標値を設定するように構成されていることが好ましい（第４発明）。

なお、第４発明において、「第１制限度合い指標値」は、前記距離に応じた前記移動速度の制限度合いに対して単調変化する（単調増加又は単調減少）する指標値を意味し、「第２制限度合い指標値」は、前記障害物に対する前記移動体の向きに応じた前記移動速度の制限度合いに対して単調変化する（単調増加又は単調減少）する指標値を意味する。

上記第４発明によれば、障害物に対する移動体の向きの可変範囲のうちの少なくとも一部の範囲において、前記第Ａ制限度合いが前記第Ｂ制限度合いよりも大きくなるので、障害物と移動体との距離が第２所定値以下の距離である場合には、該距離が第２所定値よりも大きい距離である場合よりも、移動体の向きの幅広い範囲で、移動体の代表点の移動速度の制限度合いを強めにするように、第２制限度合い指標値を設定できる。

このため、移動体の向きの幅広い範囲で、移動体が前記第２所定値の距離よりも障害物に接近するのを効果的に抑制することが可能となる。
上記第１～第４発明の移動体の制御装置は、前記移動体が、前記操縦者が搭乗する搭乗部を有する搭乗型移動体である場合に好適である（第５発明）。すなわち、搭乗型移動体では、障害物と移動体との距離が第１所定値以下になった直後に、移動体が障害物から離反する方向に旋回しようとしたり、あるいは、速やかに減速されるので、障害物が、実体物であるか、仮想的なものであるかによらずに、操縦者は、移動体が障害物に接近し過ぎたことを体感的に容易に認識することが可能となる。従って、操縦者は、障害物へのさらなる接近を回避するように移動体を操縦することを速やかに実施することが可能となる。

また、本発明の移動体の制御装置の第２の態様は、操縦者の操縦操作に応じて移動する移動体の制御装置であって、
前記操縦者の操縦操作に基づく前記移動体の移動指令である基本移動指令を生成する基本移動指令生成部と、
前記移動体の移動環境に存在し、又は該移動環境に設定された障害物と前記移動体との位置関係に応じて、前記基本移動指令を修正する移動指令修正部と、
前記修正された基本移動指令である修正後移動指令に応じて前記移動体の移動制御を行う移動制御部とを備えており、
前記移動指令修正部は、前記障害物と前記移動体との距離が第１所定値以下の距離になったとき、前記移動体の代表点の移動速度を、前記基本移動指令に応じて規定される移動速度である基本移動速度よりも絶対値が小さい移動速度に制限するように前記基本移動指令を修正するように構成されていると共に、前記距離の減少に対する前記移動速度の制限度合いの増加率を、前記距離が前記第１所定値に近いほど、大きくするように前記基本移動指令を修正するように構成されていることを特徴とする（第６発明）。

なお、上記第６発明において、移動体の「移動速度の制限度合い」の意味と、「前記距離の減少に対する前記移動速度の制限度合いの増加率」の意味とは、前記第３発明と同じである。

上記第６発明によれば、前記第３発明と同様に、障害物と移動体との距離が第１所定値以下になった直後に、移動体の代表点の移動速度の制限度合いを速やかに増加させることが可能となる。ひいては、前記距離が第１所定値以下になった直後から、移動体の代表点の移動速度を速やかに減速させ、障害物へのさらなる接近を速やかに抑制することが可能となる。

また、移動体が障害物にさらに接近すると、前記移動速度の制限度合いの増加率が小さくなるので、障害物に近い位置での移動体の挙動変化を抑制することが可能となる。
よって、第６発明によれば、操縦者による操縦操作に応じて移動体を移動させているときに、移動体が障害物に接近し過ぎるのを適切に防止しつつ、該移動体の移動を円滑に行わせることが可能となる。

上記第６発明では、前記移動指令修正部は、前記第４発明と同じ態様を採用し得る（第７発明）。これにより第４発明と同様の効果を得ることが可能となる。

上記第６～第７発明の移動体の制御装置は、前記移動体が、前記操縦者が搭乗する搭乗部を有する搭乗型移動体である場合に好適である（第８発明）。すなわち、搭乗型移動体では、障害物と移動体との距離が第１所定値以下になった直後に、移動体が障害物から離反する方向に旋回しようとしたり、あるいは、速やかに減速されるので、障害物が、実体物であるか、仮想的なものであるかによらずに、操縦者は、移動体が障害物に接近し過ぎたことを体感的に容易に認識することが可能となる。従って、操縦者は、障害物へのさらなる接近を回避するように移動体を操縦することを速やかに実施することが可能となる。

本発明の実施形態の移動体としての倒立振子型車両の斜視図。 実施形態の倒立振子型車両の側面図。 実施形態の倒立振子型車両の制御に係る構成を示すブロック図。 図３に示す移動制御部の要部の処理を示すブロック線図。 図３に示す移動制御部の処理に用いる倒立振子モデルを説明するための図。 図４に示す重心ずれ推定部の処理を示すブロック線図。 図４に示す速度制限係数設定部及びヨーレート指令設定部の処理に関する説明図。 図４に示す速度制限係数設定部の処理を示すブロック線図。 図８に示す第１制限率設定部の処理を説明するためのグラフ。 図１０Ａ及び図１０Ｂは、図８に示す第２制限率設定部の処理を説明するためのグラフ。 図１１Ａ及び図１１Ｂは、図８に示す第２制限率設定部の処理を説明するためのグラフ。 図１２Ａ及び図１２Ｂは、図８に示す第２制限率設定部の処理を説明するためのグラフ。 図１３Ａ及び図１３Ｂは、図８に示す第２制限率設定部の処理を説明するためのグラフ。 図４に示すヨーレート指令設定部の処理を示すブロック線図。 図１５Ａは、図１４に示すヨーレート基本値設定部の処理を説明するためのグラフ、図１５Ｂは図１４に示す速度係数設定部の処理を説明するためのグラフ。

本発明の一実施形態を図１～図１５Ｂを参照して以下に説明する。図１及び図２に示すように、本実施形態の移動体１は、例えば、搭乗型の移動体としての倒立振子型車両である。以降、本実施形態の説明では、移動体１を車両１という。この車両１は、基体２と、床面上を全方向に移動可能な移動動作部３と、移動動作部３の後方に配置された尾輪４と、車両１の操縦者が搭乗する搭乗部５とを備える。なお、「床面」は通常の意味での床面に限らず、地面、路面等であってもよい。

移動動作部３は、本実施形態では、例えば特開２０１５－０９３６５１号公報等に見られる如き車輪状の移動動作部である。この移動動作部３は、その構成を概略的に説明すると、図２に示す円環状の芯体６（以下、環状芯体６という）と、この環状芯体６の円周方向（軸心周り方向）に等角度間隔で並ぶようにして該環状芯体６に装着された複数の円環状のローラ７とを備え、下端部に位置するローラ７を介して床面上に接地される。そして、環状芯体６の回転駆動と、ローラ７の回転駆動との両方又は一方を行うことで、移動動作部３は、床面上を全方向に移動することが可能である。かかる移動動作部３の詳細な構成は、特開２０１５－０９３６５１号公報等にて詳細に説明されているので、本明細書での詳細な説明は省略する。

以降の説明では、図１及び図２に示す如く、車両１の前後方向をＸ軸方向、車両１の左右方向をＹ軸方向、上下方向（鉛直方向もしくはほぼ鉛直な方向）をＺ軸方向とする３軸直交座標系を想定し、Ｘ軸方向、Ｙ軸方向、Ｚ軸方向は、特に記載のない限り、当該座標系の各座標軸方向を意味する。この場合、Ｘ軸方向（車両１の前後方向）は、床面上で起立させた状態の移動動作部３の環状芯体６を回転駆動したときに該移動動作部３が転動する方向、Ｙ軸方向（車両１の左右方向）は、起立状態の移動動作部３の環状芯体６の軸心の方向である。なお、Ｘ軸の正方向は、車両１の前方向、Ｙ軸の正方向は車両１の左方向、Ｚ軸の正方向は上方向である。また、Ｘ軸周りの方向、Ｙ軸周りの方向、Ｚ軸周りの方向をそれぞれ、ロール方向、ピッチ方向、ヨー方向と称する。

基体２には、上記移動動作部３が組み付けられている。該基体２は、床面に接地させた移動動作部３の下部を除く部分の周囲を覆うように設けられている。そして、この基体２に移動動作部３の環状芯体６が、その軸心周りで回転自在に支持されている。この場合、基体２は、移動動作部３の環状芯体６の軸心を支点として、ピッチ方向に傾動自在であると共に、移動動作部３と共に床面に対して傾くことで、移動動作部３の接地部を支点として、ロール方向に傾動自在である。

また、基体２の内部には、移動動作部３を移動させる駆動力を発生するアクチュエータ８が搭載されている。このアクチュエータ８は、環状芯体６を回転駆動するアクチュエータとしての電動モータ８ａと、各ローラ７を回転駆動するアクチュエータとしての電動モータ８ｂとから構成される。そして、電動モータ８ａ，８ｂは、それぞれ図示を省略する動力伝達機構を介して環状芯体６、各ローラ７に回転駆動力を付与する。この動力伝達機構としては、例えば、特開２０１５－０９３６５１号公報等にて説明されている公知の構造のものを採用し得る。

なお、移動動作部３は、上記の構造と異なる構造のものであってもよい。例えば、移動動作部３及びその駆動系の構造として、ＰＣＴ国際公開公報ＷＯ／２００８／１３２７７８、あるいは、ＰＣＴ国際公開公報ＷＯ／２００８／１３２７７９に記載された構造のものを採用し得る。

また、基体２には、操縦者が搭乗する搭乗部５が組み付けられている。この搭乗部５は、操縦者が着座するシートにより構成されており、基体２と一体に傾動し得るように該基体２の上端部に固定されている。

基体２には、さらに搭乗部５に着座した操縦者がその足を載せる一対の足載せ部９，９と、操縦者が把持する一対の把持部１０，１０とが組み付けられている。足載せ部９，９は、基体２の両側部の下部に突設されている。なお、図１及び図２では、一方側（右側）の足載せ部９の図示は省略されている。

また、把持部１０，１０は、搭乗部５の両側にＸ軸方向（前後方向）に延在して配置されたバー状のものであり、それぞれ、基体２から延設されたロッド１１を介して基体２に固定されている。そして、把持部１０，１０のうちの一方の把持部１０（図では右側の把持部１０）には、車両１の操縦操作用の操作器としてのジョイスティック１２がピッチ方向及びロール方向に揺動し得るように取り付けられている。

尾輪４は、例えばオムニホイール（登録商標）、もしくは自在キャスター等により構成され、アーム１５を介して基体２又は移動動作部３に連結されている。アーム１５は、その先端部が移動動作部３の環状芯体６の軸心周りに揺動し得るように基体２又は移動動作部３に軸支されており、基体２の後方に向かって延設されている。なお、アーム１５は、基体２に設けられたストッパ１６，１６の間の範囲内で揺動することが可能である。そして、尾輪４は、移動動作部３の後方で床面に接地して、該移動動作部３の移動に応じて床面上を移動し、もしくは転回し得るようにアーム１５の後端部に取り付けられている。なお、尾輪４は、バネ等により、床面に押し付けられる方向に付勢されていてもよい。

図１及び図２での図示は省略したが、本実施形態の車両１の基体２には、該車両１の動作制御（移動動作部３の移動制御）のための構成として、図３に示すように、制御装置２０と、基体２の３軸方向の加速度を検出する加速度センサ５０と、３軸周りの角速度を検出する角速度センサ５１と、電動モータ８ａの回転速度を検出する回転速度センサ５２ａと、電動モータ８ｂの回転速度を検出する回転速度センサ５２ｂとが搭載されている。回転速度センサ５２ａ，５２ｂは、例えば、ロータリエンコーダ、レゾルバ等により構成され得る。

制御装置２０は、例えば、マイクロコンピュータもしくはプロセッサ、メモリ、インターフェース回路等を含む一つ以上の電子回路ユニットにより構成される。この制御装置２０には、ジョイスティック１２の出力（操作信号）と、加速度センサ５０、角速度センサ５１及び回転速度センサ５２ａ，５２ｂの各検出信号とが入力される。また、制御装置２０は、図示しない外部のサーバ等と無線通信を行うことが可能である。

そして、制御装置２０は、実装されたハードウェア構成及びプログラム（ソフトウェア構成）の一方又は両方により実現される一つの機能として、慣性センサとしての加速度センサ５０及び角速度センサ５１のそれぞれの検出信号から、ストラップダウン方式等の公知の慣性航法手法により車両１の自己位置を計測したり、基体２の傾斜角度（＝搭乗部５の傾斜角度）を計測する機能を有する。

なお、車両１の自己位置の計測処理では、慣性航法の手法に限らず、例えばＧＮＳＳ（Global Navigation Satellite System）を用いてもよく、あるいは、車両１に搭載された外界センサ（カメラ、測距センサ）等の検出情報や車両１の移動環境に設置されたカメラの撮影情報を用いてもよい。また、車両１の位置を計測する処理を外部のサーバで実行してもよい。

また、制御装置２０は、外部のサーバと通信を行うことで、該サーバから車両１の周囲に存在する障害物の位置を示す障害物情報を取得することが可能である。なお、障害物は、車両１と実際に接触する可能性のある物体（静止物体もしくは移動物体）に限らず、車両１の進入禁止領域の境界等に仮想的に設定される仮想壁（バーチャルウォール）等の仮想的な障害物であってもよい。また、障害物情報は、例えば、車両１に搭載された外界センサ（カメラ、測距センサ）等の検出情報や車両１の移動環境に設置されたカメラの撮影情報等を用いて、制御装置２０自身が認識し得るようにしてもよい。

制御装置２０はさらに、アクチュエータ８としての電動モータ８ａ，８ｂを制御することで移動動作部３の移動動作を制御する移動制御部２１としての機能を備えている。この移動制御部２１は、後述する演算処理を実行することで、移動動作部３の移動速度（詳しくは、Ｘ軸方向の並進速度とＹ軸方向の並進速度との組）の目標値である目標速度を逐次算出し、移動動作部３の実際の移動速度を、該目標速度に一致させるように電動モータ８ａ，８ｂの回転速度の目標値を決定する。そして、移動制御部２１は、電動モータ８ａ，８ｂの回転速度を、移動動作部３の目標速度に応じて規定される目標値にフィードバック制御することで、移動動作部３の実際の移動速度を目標速度に一致させるように制御する。

次に、上記移動制御部２１による処理をさらに詳細に説明する。移動制御部２１は、図４に示すように、その主要な機能部として、ジョイスティック１２から入力される操作信号により示される該ジョイスティック１２の前後方向の揺動量（ピッチ方向の回転量）Ｊs_x及び左右方向の揺動量（ロール方向の回転量）Ｊs_yの組を車両１の移動のための速度指令に変換する操作指令変換部３１と、車両１の全体重心（以降、車両系全体重心という）の目標速度Ｖb_cmd_xyを決定する重心目標速度決定部３２と、車両系全体重心の実際の速度Ｖbを推定する重心速度推定部３３と、車両系全体重心の速度Ｖbを目標速度Ｖb_cmd_xyに追従させつつ、搭乗部５の姿勢（基体２の姿勢）を安定化するように移動動作部３の目標速度Ｖw1_cmd_xyを決定する姿勢制御演算部３４と、車両系全体重心の後述する重心ずれ量Ｏfst_xyを推定する重心ずれ推定部３５と、該重心ずれ量Ｏfst_xyに起因する後述する重心ずれ影響量Ｖofs_xyを算出する重心ずれ影響量算出部３６とを備える。

なお、本実施形態では、車両系全体重心の速度が、本発明における「移動体の代表点の移動速度」に相当し、車両系全体重心の目標速度Ｖb_cmd_xyが本発明における移動指令に相当する。そして、重心目標速度決定部３２は、本発明における基本移動指令生成部及び移動指令修正部としての機能を含む。

そして、移動制御部２１は、これらの各機能部の処理を所定の演算処理周期で逐次実行することで、移動動作部３の目標速度Ｖw1_cmd_xyを決定する。なお、本実施形態の説明において、搭乗部５の姿勢（基体２の姿勢）を安定化するというのは、車両系全体重心を倒立振子質点のようにバランスさせることを意味する。

また、本実施形態の説明では、添え字“_x”を付する参照符号は、車両１をＹ軸方向から見た場合（ＺＸ平面に投影して見た場合）の車両１の挙動に関する状態量もしくはパラメータを意味し、添え字“_y”を付する参照符号は、車両１をＸ軸方向から見た場合（ＹＺ平面に投影して見た場合）の車両１の挙動に関する状態量もしくはパラメータを意味する。

そして、添え字“_xy”を付する参照符号は、車両１をＸ軸方向から見た場合の状態量もしくはパラメータと、車両１をＹ軸方向から見た場合の状態量もしくはパラメータとの組を意味する。例えば、移動動作部３の目標速度Ｖw1_cmd_xyは、車両１をＹ軸方向から見た場合の移動動作部３の目標速度Ｖw1_cmd_x（すなわち、Ｘ軸方向の目標速度）と、車両１をＸ軸方向から見た場合の移動動作部３の目標速度Ｖw1_cmd_y（すなわち、Ｙ軸方向の目標速度）との組を意味する。

ここで、移動制御部２１の各機能部の処理を具体的に説明する前に、その処理の基礎となる事項を説明しておく。車両系全体重心の動力学的な挙動（詳しくは、Ｙ軸方向から見た挙動と、Ｘ軸方向から見た挙動）は、近似的に、図５に示すような倒立振子モデルの挙動により表現される。移動制御部２１の処理のアルゴリズムは、この挙動を基礎として構築されている。

なお、図５では、Ｙ軸方向から見た場合の倒立振子モデルと、Ｘ軸方向から見た場合の倒立振子モデルとを併せて図示するために、Ｙ軸方向から見た場合の変数の参照符号には括弧を付さず、Ｘ軸方向から見た場合の変数の参照符号に括弧を付している。

Ｙ軸方向から見た車両系全体重心の挙動を表す倒立振子モデルは、Ｙ軸方向と平行な回転軸心を有して床面上を輪転自在な仮想的な車輪６１_x（以降、仮想車輪６１_xという）と、該仮想車輪６１_xの回転中心から延設されて、該仮想車輪６１_xの回転軸周りに（Ｙ軸周り方向に）揺動自在なロッド６２_xと、このロッド６２_xの先端部（上端部）である基準部Ｐs_xに連結された質点Ｇa_xとを備える。

この倒立振子モデルでは、質点Ｇa_xの運動が、Ｙ軸方向から見た車両系全体重心の運動に相当し、鉛直方向に対するロッド６２_xの傾斜角度θb_x（Ｙ軸周り方向の傾斜角度）が、搭乗部５（又は基体２）のＹ軸周り方向の傾斜角度に一致する。また、移動動作部３のＸ軸方向の並進運動が、仮想車輪６１_xの輪転によるＸ軸方向の並進運動に相当する。
そして、仮想車輪６１_xの半径ｒ_xと、基準部Ｐs_x及び質点Ｇa_xの床面からの高さｈ_xとは、あらかじめ設定された既定値（一定値）である。

同様に、Ｘ軸方向から見た車両系全体重心の挙動を表す倒立振子モデルは、Ｘ軸方向と平行な回転軸心を有して床面上を輪転自在な仮想的な車輪６１_y（以降、仮想車輪６１_yという）と、該仮想車輪６１_yの回転中心から延設されて、該仮想車輪６１_yの回転軸周りに（Ｘ軸周り方向に）揺動自在なロッド６２_yと、このロッド６２_yの先端部（上端部）である基準部Ｐs_yに連結された質点Ｇa_yとを備える。

この倒立振子モデルでは、質点Ｇa_yの運動が、Ｘ軸方向から見た車両系全体重心の運動に相当し、鉛直方向に対するロッド６２_yの傾斜角度θb_y（Ｘ軸周り方向の傾斜角度）が、搭乗部５（又は基体２）のＸ軸周り方向の傾斜角度に一致する。また、移動動作部３のＹ軸方向の並進運動が、仮想車輪６１_yの輪転によるＹ軸方向の並進運動に相当する。

そして、仮想車輪６１_yの半径ｒ_yと、基準部Ｐs_y及び質点Ｇa_yの床面からの高さｈ_yとは、あらかじめ設定された既定値（一定値）である。なお、Ｘ軸方向で見た基準部Ｐs_y及び質点Ｇa_yの床面からの高さｈ_yは、Ｙ軸方向で見た基準部Ｐs_x及び質点Ｇa_xの床面からの高さｈ_xと同じである。そこで、以降、ｈ_x＝ｈ_y＝ｈとおく。

ここで、Ｙ軸方向から見た場合の上記基準部Ｐs_xと質点Ｇa_xとの位置関係について補足すると、基準部Ｐs_xの位置は、搭乗部５に搭乗（着座）した操縦者が、該搭乗部５に対して予め定められた中立姿勢のまま不動であると仮定した場合における車両系全体重心の位置に相当している。

従って、この場合には、質点Ｇa_xの位置は、基準部Ｐs_xの位置に一致する。このことは、Ｘ軸方向から見た場合の上記基準部Ｐs_yと質点Ｇa_yとの位置関係ついても同様である。

ただし、実際には、搭乗部５に搭乗した操縦者が、その上体等を搭乗部５（又は基体２）に対して動かすことで、実際の車両系全体重心のＸ軸方向の位置及びＹ軸方向の位置は、一般には、それぞれ基準部Ｐs_x，Ｐs_yの位置から水平方向にずれることとなる。このため、図５では、質点Ｇa_x，Ｇa_yの位置をそれぞれ基準部Ｐs_x，Ｐs_yの位置からずらした状態で示している。

上記のような倒立振子モデルで表現される車両系全体重心の挙動は、次式（１ａ），（１ｂ），（２ａ），（２ｂ）により表現される。式（１ａ），（１ｂ）は、Ｙ軸方向で見た挙動、式（２ａ），（２ｂ）は、Ｘ軸方向で見た挙動を表している。

Ｖb_x＝Ｖw1_x＋ｈ・ωb_x ……（１ａ）
dＶb_x／dt＝(ｇ／ｈ)・(θb_x・(ｈ－ｒ_x)＋Ｏfst_x)＋ωz・Ｖb_y ……（１ｂ）
Ｖb_y＝Ｖw1_y＋ｈ_y・ωb_y ……（２ａ）
dＶb_y／dt＝(ｇ／ｈ)・(θb_y・(ｈ－ｒ_y)＋Ｏfst_y)－ωz・Ｖb_x ……（２ｂ）

ここで、Ｖb_xは、車両系全体重心のＸ軸方向の速度（並進速度）、θb_xは搭乗部５（又は基体２）のＹ軸周り方向の傾斜角度、Ｖw1_xは、仮想車輪６１_xのＸ軸方向の移動速度（並進速度）、ωb_xはθb_xの時間的変化率（＝ｄθb_x／dt）、Ｏfst_xは車両系全体重心のＸ軸方向の位置（質点Ｇa_xのＸ軸方向の位置）の、前記基準部Ｐs_xの位置からのＸ軸方向のずれ量、Ｖb_yは、車両系全体重心のＹ軸方向の速度（並進速度）、Ｖw1_yは、仮想車輪６１_yのＹ軸方向の移動速度（並進速度）、θb_yは搭乗部５（又は基体２）のＸ軸周り方向の傾斜角度、ωb_yはθb_yの時間的変化率（＝ｄθb_y／dt）、Ｏfst_yは車両系全体重心のＹ軸方向の位置（質点Ｇa_yのＹ軸方向の位置）の、前記基準部Ｐs_yの位置からのＹ軸方向のずれ量である。また、ωzは車両１の旋回時のヨーレート（ヨー軸周り方向の角速度）、ｇは重力加速度定数である。

なお、式（１ａ），（１ｂ），（２ａ），（２ｂ）では、θb_x、ωb_xの正方向は、車両系全体重心がＸ軸の正方向（前向き）に傾く方向、θb_y、ωb_yの正方向は、車両系全体重心がＹ軸の正方向（左向き）に傾く方向である。また、ωzの正方向は、車両１を上方から見た場合に、反時計回り方向である。

本実施形態では、移動制御部２１の処理のアルゴリズムは、上記のように車両系全体重心の基準部Ｐs_x，Ｐs_yからのずれ量と、遠心力とを考慮した車両系全体重心の挙動モデル（倒立振子モデル）に基づいて構築されている。

以上を前提として、移動制御部２１の処理をより具体的に説明する。図４を参照して、移動制御部２１は、各演算処理周期において、まず、操作指令変換部３１の処理と、重心速度推定部３３の処理とを実行する。

操作指令変換部３１は、ジョイスティック１２のピッチ方向の揺動量（前方への揺動量又は後方への揺動量）に応じて、Ｘ軸方向での移動動作部３の移動速度（並進速度）の基本指令値である基本速度指令Ｖjs_xを決定し、ジョイスティック１２のロール方向の揺動量（左方向への揺動量又は右方向への揺動量）に応じて、Ｙ軸方向での移動動作部３の移動速度（並進速度）の基本指令値である基本速度指令Ｖjs_yを決定する。

この場合、Ｘ軸方向の基本速度指令Ｖjs_xの向き（極性）は、ピッチ方向でのジョイスティック１２の揺動の方向（前方向又は後方向）に応じて決定され、基本速度指令Ｖjs_xの大きさは、ピッチ方向でのジョイスティック１２の揺動量に応じて決定される。同様に、Ｙ軸方向の基本速度指令Ｖjs_yの向き（極性）は、ロール方向でのジョイスティック１２の揺動の方向（左方向又は右方向）に応じて決定され、基本速度指令Ｖjs_yの大きさは、ロール方向でのジョイスティック１２の揺動量に応じて決定される。

補足すると、本実施形態では、基本速度指令Ｖjs_xyを設定するための操作器として、ジョイスティック１２を例示した。ただし、基本速度指令Ｖjs_xyの設定操作を行うための操作器はジョイスティック１２以外の操作器であってもよい。例えば、操縦者が所持するスマートフォン等の携帯端末と通信し得るように制御装置２０が構成されている場合には、該携帯端末を、基本速度指令Ｖjs_xyの設定操作のための操作器として利用することも可能である。

前記重心速度推定部３３は、前記式（１ａ），（２ａ）に表される運動学的な関係式に基づいて、車両系全体重心の速度Ｖbの推定値Ｖb_estm1_xyを算出する。具体的には、図４に示す如く、移動動作部３の実際の並進速度Ｖw1_act_xyの値と、搭乗部５の傾斜角度θb_xyの実際の時間的変化率（傾斜角速度）ωb_act_xyに、車両系全体重心の高さｈを乗じてなる値とから、次式（３ａ），（３ｂ）により車両系全体重心の速度Ｖbの推定値Ｖb_estm1_xyが算出される。

Ｖb_estm1_x＝Ｖw1_act_x＋ｈ・ωb_act_x ……（３ａ）
Ｖb_estm1_y＝Ｖw1_act_y＋ｈ・ωb_act_y ……（３ｂ）

この場合、上記演算におけるＶw1_act_x，Ｖw1_act_yの値としては、本実施形態では、前回の演算処理周期で姿勢制御演算部３４により決定された移動動作部３の目標速度Ｖw1_cmd_x，Ｖw1_cmd_y（前回値）が用いられる。ただし、例えば、電動モータ８ａ，８ｂのそれぞれの回転速度を回転速度センサ５２ａ，５２ｂにより検出し、それらの検出値から推定したＶw1_act_x，Ｖw1_act_yの最新値を式（３ａ），（３ｂ）の演算に用いてもよい。

また、ωb_act_x，ωb_act_yの値としては、本実施形態では、加速度センサ５０及び角速度センサ５１の検出信号に基づく搭乗部５の傾斜角度θb_x，θb_yの計測値の時間的変化率の最新値（換言すれば、ωb_act_x，ωb_act_yの計測値の最新値）が用いられる。

移動制御部２１は、上記の如く操作指令変換部３１及び重心速度推定部３３の処理を実行した後、次に、図４に示す重心ずれ推定部３５の処理を実行することで、前記重心ずれ量Ｏfst_xyの推定値である重心ずれ量推定値Ｏfst_estm_xyを決定する。なお、重心ずれ推定部３５に関する以降の説明では、重心速度推定部３３により算出された車両系全体重心の速度の推定値Ｖb_estm1_x，Ｖb_estm1_yをそれぞれ第１推定値Ｖb_estm1_x，Ｖb_estm1_yと称する。

重心ずれ推定部３５の処理は、本実施形態では、例えば図６のブロック線図で示す如く実行される。なお、図６は、重心ずれ量推定値Ｏfst_estm_xyのうちのＸ軸方向の重心ずれ量推定値Ｏfst_estm_xの決定処理を代表的に表している。

図６の処理を具体的に説明すると、重心ずれ推定部３５は、加速度センサ５０及び角速度センサ５１の検出信号から得られた搭乗部５のＹ軸周り方向の実際の傾斜角度θb_act_xの計測値（最新値）及び車両１の実際のヨーレートωz_actの計測値（最新値）と、重心速度推定部３３により算出された車両系全体重心のＹ軸方向の速度の第１推定値Ｖb_estm1_y（最新値）と、前回の演算処理周期で決定したＸ軸方向の重心ずれ量推定値Ｏfst_estm_x（前回値）とを用いて、前記式（１ｂ）の右辺の演算処理を演算部３５ａで実行することにより、車両系全体重心のＸ軸方向の並進加速度の推定値DVb_estm_xを算出する。

さらに重心ずれ推定部３５は、車両系全体重心のＸ軸方向の並進加速度の推定値DVb_estm_xを積分する処理を演算部３５ｂで実行することにより、車両系全体重心のＸ軸方向の速度の第２推定値Ｖb_estm2_xを算出する。

次いで、重心ずれ推定部３５は、車両系全体重心のＸ軸方向の速度の第２推定値Ｖb_estm2_x（最新値）と、第１推定値Ｖb_estm1_x（最新値）との偏差を算出する処理を演算部３５ｃで実行する。
さらに、重心ずれ推定部３５は、この偏差に所定値のゲイン（－Ｋｐ）を乗じる処理を演算部３５ｄで実行することにより、Ｘ軸方向の重心ずれ量推定値Ｏfst_estm_xの最新値を決定する。

Ｙ軸方向の重心ずれ量推定値Ｏfst_estm_yの決定処理も上記と同様に実行される。具体的には、この決定処理を示すブロック線図は、図６中の添え字“_x”と“_y”とを入れ替え、加算器３５ｅへの入力の一つである図中右側の加速度成分（遠心力によって発生する加速度成分）の符号“＋”を“－”に置き換えることによって得られる。

移動制御部２１は、次に、図４に示す重心ずれ影響量算出部３６の処理を実行することによって、重心ずれ影響量Ｖofs_xyを算出する。重心ずれ影響量Ｖofs_xyは、後述する姿勢制御演算部３４において、車両系全体重心の位置が倒立振子モデルにおける前記基準部Ｐs_xyの位置からずれることを考慮せずにフィードバック制御を行った場合の車両系全体重心の目標速度に対する実際の速度のずれを表す。

この重心ずれ影響量算出部３６は、新たに決定された重心ずれ量推定値Ｏfst_estm_xyの各成分に、(Ｋth_xy／（ｈ-ｒ_xy))／Ｋvb_xyという値を乗じることにより、重心ずれ影響量Ｖofs_xyを算出する。

なお、Ｋth_xyは、後述する姿勢制御演算部３４の処理において、搭乗部５の傾斜角度をゼロに近づけるように機能する操作量成分を決定するためのゲイン値である。また、Ｋvb_xyは、後述する姿勢制御演算部３４の処理において、車両系全体重心の目標速度Ｖb_cmd_xyと該車両系全体重心の速度の第１推定値Ｖb_estm1_xyとの偏差をゼロに近づけるように機能する操作量成分を決定するためのゲイン値である。

移動制御部２１は、次に、図４に示す重心目標速度決定部３２の処理を実行することによって、各演算処理周期毎の車両系全体重心の目標速度Ｖb_cmd_xy（以降、重心目標速度Ｖb_cmd_xyという）を決定する。

この場合、重心目標速度決定部３２は、まず、操作指令変換部３１により決定された基本速度指令Ｖjs_xy（最新値）と、重心ずれ影響量算出部３６により決定された重心ずれ影響量Ｖofs_xy（最新値）とから、重心目標速度Ｖb_cmd_xyの基本値Ｖ1_xyを決定する。該基本値Ｖ1_xyは、ジョイスティック１２の操作と、操縦者の上体の動きに伴う重心ずれ量推定値Ｏfst_estm_xyとに応じた車両系全体重心の速度の要求値に相当するものである。以降、基本値Ｖ1_xyを重心速度基本要求値Ｖ1_xyという。

具体的には、重心目標速度決定部３２は、重心ずれ影響量Ｖofs_xyに対する不感帯処理とリミット処理とを処理部３２ａで実行することで、重心速度基本要求値Ｖ1_xyのうちの重心ずれ量推定値Ｏfst_estm_xyに応じた成分としての重心速度加算量Vb_cmd_by_ofs_xyを決定する。

さらに詳細には、本実施形態では、重心目標速度決定部３２は、Ｘ軸方向の重心ずれ影響量Ｖofs_xの大きさがゼロ近辺の所定の範囲である不感帯域内の値（比較的ゼロに近い値）である場合には、Ｘ軸方向の重心速度加算量Vb_cmd_by_ofs_xをゼロにする。

また、重心目標速度決定部３２は、Ｘ軸方向の重心ずれ影響量Ｖofs_xの大きさが不感帯域内から逸脱した値である場合には、Ｘ軸方向の重心速度加算量Vb_cmd_by_ofs_xを、Ｖofs_xと同極性で、その大きさが、Ｖofs_xの大きさの増加に伴い大きくなるように決定 する。ただし、重心速度加算量Vb_cmd_by_ofs_xの値は、所定の上限値（＞０）と下限値（≦０）との間の範囲内に制限される。Ｙ軸方向の重心速度加算量Vb_cmd_by_ofs_yの決定処理も上記と同様である。

次いで、重心目標速度決定部３２は、操作指令変換部３１により決定された基本速度指令Ｖjs_xyの各成分に重心速度加算量Vb_cmd_by_ofs_xyの各成分を加え合わせる処理を処理部３２ｂで実行することで、重心速度基本要求値Ｖ1_xyを算出する。すなわち、Ｖ1_x＝Ｖjs_x＋Vb_cmd_by_ofs_x、Ｖ1_y＝Ｖjs_y＋Vb_cmd_by_ofs_yという処理によって、重心速度基本要求値Ｖ1_xy（Ｖ1_x，Ｖ1_yの組）を決定する。

補足すると、本実施形態の車両１では、ジョイスティック１２の操作と、操縦者の上体の動き（体重移動）に伴う重心ずれ量Ｏfst_xyの変化とが車両１の操縦操作に相当する。また、重心速度基本要求値Ｖ1_xyが、本発明における基本移動指令に相当する。そして、重心目標速度決定部３２のうち、処理部３２ａ及び３２ｂの処理が、本発明における基本移動指令生成部の処理に相当する。

重心目標速度決定部３２は、さらに、外部のサーバ等から取得した障害物情報と、自己位置の計測値とから特定される車両１と障害物との位置関係に応じて、重心速度基本要求値Ｖ1_xyに制限を加えるための速度制限係数α_xy（α_x、α_yの組）を速度制限係数設定部３２ｃで設定すると共に、車両１に付加的に発生させるヨーレートの指令値としてのヨーレート指令ωz_cmdをヨーレート指令設定部３２ｄで設定する。

速度制限係数α_xyのＸ軸方向成分及びＹ軸方向成分は、重心速度基本要求値Ｖ1_xyのＸ軸方向成分及びＹ軸方向成分に各々制限を加えるために該重心速度基本要求値Ｖ1_xyのＸ軸方向成分及びＹ軸方向成分に各々乗じる係数であり、“０”から“１”までの範囲内の値である。

上記速度制限係数設定部３２ｃには、障害物と車両１との位置関係を示す情報として、車両１と障害物との間の距離ｄと、障害物に対する車両１の向きを示す方位角φとが入力される。これらの距離ｄ及び方位角φは、外部のサーバ等から取得された障害物情報と、自己位置の計測値とから特定される。

ここで、図７を参照して、本実施形態では、上記距離ｄは、車両１の代表点Ｐａ（例えば移動動作部３の接地点等）と障害物との間の最短距離、方位角φは、距離ｄの方向に対する車両１の前後方向（Ｘ軸方向）の傾斜角度（ヨー方向での傾斜角度）である。なお、図７では、車両１を模式化して記載している。

この場合、上記方位角φは、車両１の前後方向（Ｘ軸方向）が距離ｄの方向に一致し、且つ、車両１の正面前方（Ｘ軸の正方向）に障害物が存在する場合にゼロである。また、車両１の正面前方の方向が、距離ｄの方向から反時計回り方向に傾いている場合の方位角φの極性が正、距離ｄの方向から時計回り方向に傾いている場合の方位角φの極性が負である。従って、方位角φは、－１８０°と＋１８０°との間の範囲の角度である。なお、図７に示す例では、φ＞０である。

速度制限係数設定部３２ｃは、入力された距離ｄ及び方位角φから、図８のブロック線図で示す処理を実行することで、速度制限係数α_xyを決定する。具体的には、速度制限係数設定部３２ｃは、入力された距離ｄに応じて第１制限率β(ｄ)を設定する処理を第１制限率設定部３２c1で実行すると共に、入力された方位角φに応じて第２制限率β(φ)を設定する処理を第２制限率設定部３２c2で実行する。

第１制限率β(ｄ)及び第２制限率β(φ)は、それぞれ、重心速度基本要求値Ｖ1_xyに対する重心目標速度Ｖb_cmd_xの制限度合いの要求を示すパラメータであり、本実施形態では“０”から“１”までの範囲内の値に設定される。そして、β(ｄ)，β(φ)のそれぞれは、その値が大きいほど（“１”に近いほど）、重心目標速度Ｖb_cmd_xyを重心速度基本要求値Ｖ1_xyよりも“０”に近づけることの要求が高い（Ｖ1_xyに対するＶb_cmd_xyの制限度合いを大きくすることの要求が高い）ことを表す。

第１制限率設定部３２c1は、上記第１制限率β(ｄ)を、速度制限係数設定部３２ｃに入力された距離ｄから、あらかじめ作成されたマップもしくは演算式により、例えば図９のグラフで示す形態で設定する。すなわち、距離ｄが所定値ｄ１よりも大きい場合（車両１が障害物から十分に離れている場合）には、第１制限率β(ｄ)は、ゼロに設定される。そして、距離ｄが所定値ｄ１と所定値ｄ２（＜ｄ１）との間の範囲内である場合と、距離ｄが所定値ｄ２と所定値ｄ３（＜ｄ２）との間の範囲内である場合とでは、距離ｄの減少に伴い、第１制限率β(ｄ)が“０”から“１”まで増加するように設定される。そして、距離ｄが所定値ｄ３以下である場合には、第１制限率β(ｄ)は“１”に維持される

この場合、ｄ１～ｄ２の範囲では、ｄ２～ｄ３の範囲よりも、距離ｄの減少に対する第１制限率β(ｄ)の増加率（距離ｄの単位減少量当たりのβ(ｄ)の増加量）が大きくなるように、第１制限率β(ｄ)が設定される。従って、ｄ１～ｄ３の範囲では、距離ｄの減少に対する第１制限率β(ｄ)の増加率が距離ｄの減少に伴い、小さくなるように第１制限率β(ｄ)が設定される。

補足すると、上記の如く設定される第１制限率β(ｄ)が本発明における第１制限度合い指標値に相当する。また、距離ｄの所定値ｄ１が本発明における第１所定値に相当する。

第２制限率設定部３２c2が設定する第２制限率β（φ）は、Ｘ軸方向で前向きの（正極性の）重心速度基本要求値Ｖ1_xを制限するための第２制限値率β（φ）_fd_xと、Ｘ軸方向で後ろ向きの（負極性の）重心速度基本要求値Ｖ1_xを制限するための第２制限値率β（φ）_bk_xと、Ｙ軸方向で左向きの（正極性の）重心速度基本要求値Ｖ1_yを制限するための第２制限率β（φ）_L_yと、Ｙ軸方向で右向きの（負極性の）重心速度基本要求値Ｖ1_yを制限するための第２制限値率β（φ）_R_yとからなる。

そして、第２制限率設定部３２c2は、これらの第２制限率β（φ）_fd_x，β（φ）_bk_x，β（φ）_L_y，β（φ）_R_yを、速度制限係数設定部３２ｃに入力された方位角φと距離ｄとから、あらかじめ作成されたマップもしくは演算式により設定する。

具体的には、第２制限率設定部３２c2は、距離ｄが前記所定値ｄ３よりも大きい場合（換言すれば、前記第１制限率β(ｄ)が“１”よりも小さい値に設定される場合）には、β（φ）_fd_x，β（φ）_bk_x，β（φ）_L_y，β（φ）_R_yを、それぞれ、基本的には、例えば図１０Ａ、図１１Ａ、図１２Ａ，図１３Ａのグラフで示す関数形態で方位角φに応じて設定する。ただし、第２制限率β（φ）_fd_x，β（φ）_bk_x，β（φ）_L_y，β（φ）_R_yは、いずれも、“０”よりも若干大きい所定値が下限値β（φ）_lowとされる。

この場合、前向きの重心速度基本要求値Ｖ1_x（＞０）に対する第２制限率β（φ）_fd_xは、基本的には、図１０Ａのグラフで示す如く、方位角φが－９０°～＋９０°の範囲では、φが０°に近づくに伴い、“０”から“１”まで増加し、－９０°～－１８０°の範囲及び＋９０°～＋１８０°の範囲では、“０”になるという関数形態で設定される。ただし、第２制限率β（φ）_fd_xは、当該関数形態で設定された値が所定の下限値β（φ）_lowよりも小さい場合には、該下限値β（φ）_lowに制限される。

また、後ろ向きの重心速度基本要求値Ｖ1_x（＜０）に対する第２制限率β（φ）_bk_xは、基本的には、図１１Ａのグラフで示す如く、方位角φが－９０°～＋９０°の範囲では、“０”になり、－９０°～－１８０°範囲及び＋９０°～＋１８０°の範囲では、方位角φの大きさ（絶対値）が大きくなるに伴い、“０”から“１”まで増加するという関数形態で設定される。ただし、第２制限率β（φ）_bk_xは、当該関数形態で設定された
値が所定の下限値β（φ）_lowよりも小さい場合には、該下限値β（φ）_lowに制限される。

また、左向きの重心速度基本要求値Ｖ1_y（＞０）に対する第２制限率β（φ）_L_yは、基本的には、図１２Ａのグラフで示す如く、方位角φが０°～－１８０°の範囲では、φが－９０°に近づくに伴い、“０”から“１”まで増加し、０°以上の範囲では、“０”になるという関数形態で設定される。ただし、第２制限率β（φ）_L_yは、当該関数形態で設定された値が所定の下限値β（φ）_lowよりも小さい場合には、該下限値β（φ）_lowに制限される。

また、右向きの重心速度基本要求値Ｖ1_y（＜０）に対する第２制限率β（φ）_R_yは、基本的には、図１３Ａのグラフで示す如く、方位角φが０°～＋１８０°の範囲では、φが＋９０°に近づくに伴い、“０”から“１”まで増加し、０°以下の範囲では、“０”になるという関数形態で設定される。ただし、第２制限率β（φ）_R_yは、当該関数形態で設定された値が所定の下限値β（φ）_lowよりも小さい場合には、該下限値β（φ）_lowに制限される。

また、第２制限率設定部３２c2は、距離ｄが前記所定値ｄ３以下である場合（換言すれば、前記第１制限率β（ｄ）が“１”に設定される場合）には、第２制限率β（φ）_fd_x，β（φ）_bk_x，β（φ）_L_y，β（φ）_R_yを、それぞれ、基本的には、例えば図１０Ｂ、図１１Ｂ、図１２Ｂ，図１３Ｂのグラフで示す関数態様で方位角φに応じて設定する。

この場合、前向きの重心速度基本要求値Ｖ1_x（＞０）に対する第２制限率β（φ）_fd_xは、基本的には、図１０Ｂのグラフで示す如く、方位角φが－９０°～＋９０°の範囲では、 “１”になり、－９０°～－１８０°範囲及び＋９０°～＋１８０°の範囲では、φの大きさ（絶対値）が小さくなるに伴い、“０”から“１”まで増加するという関数形態で設定される。ただし、第２制限率β（φ）_fd_xは、当該関数形態で設定された値が所定の下限値β（φ）_lowよりも小さい場合には、該下限値β（φ）_lowに制限される。

また、後ろ向きの重心速度基本要求値Ｖ1_x（＜０）に対する第２制限率β（φ）_bk_xは、基本的には、図１１Ｂのグラフで示す如く、方位角φの大きさ（絶対値）が“０”から大きくなるに伴い、 “０”から“１”まで増加するという関数形態で設定される。ただし、第２制限率β（φ）_bk_xは、当該関数形態で設定された値が所定の下限値β（φ）_lowよりも小さい場合には、該下限値β（φ）_lowに制限される。

また、左向きの重心速度基本要求値Ｖ1_y（＞０）に対する第２制限率β（φ）_L_yは、基本的には、図１２Ｂのグラフで示す如く、方位角φが０°～－１８０°の範囲では、“１”になり、０°～＋１８０°の範囲では、φと＋９０°との差の大きさ（絶対値）が大きくなるに伴い、“０”から“１”まで増加するという関数形態で設定される。ただし、第２制限率β（φ）_L_yは、当該関数形態で設定された値が所定の下限値β（φ）_lowよりも小さい場合には、該下限値β（φ）_lowに制限される。

また、右向きの重心速度基本要求値Ｖ1_y（＜０）に対する第２制限率β（φ）_R_yは、基本的には、図１３Ｂのグラフで示す如く、方位角φが０°～＋１８０°の範囲では、“１”になり、０°～－１８０°の範囲では、φと－９０°との差の大きさ（絶対値）が大きくなるに伴い、“０”から“１”まで増加するという関数形態で設定される。ただし、第２制限率β（φ）_R_yは、当該関数形態で設定された値が所定の下限値β（φ）_lowよりも小さい場合には、該下限値β（φ）_lowに制限される。

距離ｄが前記所定値ｄ３以下である場合には、第２制限率β（φ）_fd_x，β（φ）_bk_x，β（φ）_L_y，β（φ）_R_yが上記の如く方位角φに応じて設定されるので、ｄ≦ｄ３である場合の第２制限率β（φ）_fd_x，β（φ）_bk_x，β（φ）_L_y，β（φ）_R_yのそれぞれは、ｄ＞ｄ３である場合よりも、“１”もしくはそれに近い値になるφの範囲が拡大する（換言すれば、下限値β(φ)_lowもしくはそれに近い値になるφの範囲が縮小する）。

補足すると、上記の如く設定される第２制限率β(φ)が本発明における第２制限度合い指標値に相当する。また、距離ｄに関する所定値ｄ３が本発明における第２所定値に相当する。

速度制限係数設定部３２ｃは、上記の如く第１制限率β(d)及び第２制限率β（φ）（β（φ）_fd_x，β（φ）_bk_x，β（φ）_L_y，β（φ）_R_y）を決定した後、第１制限率β(ｄ)と、第２制限値β（φ）とを掛け合わせる処理を処理部３２c3で実行することにより、Ｘ軸方向及びＹ軸方向のそれぞれの速度に関する合成制限率β1_xy（β1_x，β1_yの組）を算出する。

具体的には、処理部３２c3では、Ｘ軸方向の重心速度基本要求値Ｖ1_xが前向きの速度である場合には、β1_x＝β(d)・β（φ）_fd_xという演算により、Ｘ軸方向の速度に関する合成制限率β1_xが算出され、Ｘ軸方向の重心速度基本要求値Ｖ1_xが後向きの速度である場合には、β1_x＝β(d)・β（φ）_bk_xという演算により、Ｘ軸方向の速度に関する合成制限率β1_xが算出される。

また、Ｙ軸方向の重心速度基本要求値Ｖ1_yが左向きの速度である場合には、β1_y＝β(d)・β（φ）_L_yという演算により、Ｙ軸方向の速度に関する合成制限率β1_yが算出され、Ｙ軸方向の重心速度基本要求値Ｖ1_yが右向きの速度である場合には、β1_y＝β(d)・β（φ）_R_yという演算により、Ｙ軸方向の速度に関する合成制限率β1_yが算出される。なお、処理部３２c3で算出される合成制限率β1_xyは、暫定値であるので、以降、暫定合成制限率β1_xyという。

次いで、速度制限係数設定部３２ｃは、暫定合成制限率β1_xyに対するリミット処理をレートリミッタ３２c4で施すことによって、前記速度制限係数α_xyを決定するために実際に用いる合成制限率である確定合成制限率β2_xyを決定する。このレートリミッタ３２c4で実行するリミット処理は、確定合成制限率β2_xyの時間的変化率（単位時間当たりの変化量）を制限するための処理である。

該リミット処理では、移動制御部２１の今回の（現在の）演算処理周期で算出された暫定合成制限率β1_xyと、前回の演算処理周期で決定された確定合成制限率β2_xy_pとの各成分毎の偏差Δβ_xy（Δβ_x＝β1_x－β2_x_p、Δβ_y＝β1_y－β2_y_p）の絶対値が、各々、所定の上限値Δβmax_xy以下の範囲に収まっているか否かが判断される。

そして、今回の（現在の）演算処理周期での確定合成制限率β2_xyのうちのβ2_xは、Δβ_xyのうちのΔβ_xの絶対値が上限値Δβmax_x以下の範囲に収まっているか否かに応じて次のように決定される。すなわち、Δβ_xの絶対値が上限値Δβmax_x以下の範囲に収まっている場合には、処理部３２c3で算出された暫定合成制限率β1_xが、そのまま今回の演算処理周期での確定合成制限率β2_xとして決定される。

また、Δβ_xの絶対値が上限値Δβmax_xよりも大きい場合には、今回の演算処理周期での確定合成制限率β2_xは、前回の演算処理周期での確定合成制限率β2_x_pに、Δβmax_x又は－Δβmax_xを加算した値に制限される。より詳しくは、Δβ_x＜－Δβmax_xである場合には、β2_x＝β2_x_p－Δβmax_xという演算により、今回の演算処理周期での確定合成制限率β2_xが決定され、Δβ_x＞Δβmax_xである場合には、β2_x＝β2_x_p＋Δβmax_xという演算により、今回の演算処理周期での確定合成制限率β2_xが決定される。

また、今回の（現在の）演算処理周期での確定合成制限率β2_xyのうちのβ2_yは、Δβ_xyのうちのΔβ_yの絶対値が上限値Δβmax_y以下の範囲に収まっているか否かに応じて上記と同様に決定される。上記のように確定合成制限率β2_xyが決定されることで、該確定合成制限率β2_xyは、急激な変化が抑制されつつ、暫定合成制限率β1_xyに収束していくように決定される。

次いで、速度制限係数設定部３２ｃは、上記の如く決定した確定合成制限率β2_xyの各成分を“１”から減算する処理を処理部３２c5で実行することにより、前記速度制限係数α_xyを決定する。すなわち、α_x＝１－β2_x、α_y＝１－β2_yという演算により速度制限係数α_xyの各成分が決定される。

従って、速度制限係数α_xyのうちのα_xは、確定合成制限率β2_xが大きいほど（換言すれば、Ｘ軸方向の重心速度基本要求値Ｖ1_xに対する重心目標速度Ｖb_cmd_xの制限度合いを大きくすることの要求が高いほど）、小さくなるように（“０”に近づくように）決定される。

同様に、速度制限係数α_xyのうちのα_yは、確定合成制限率β2_yが大きいほど（換言すれば、Ｙ軸方向の重心速度基本要求値Ｖ1_yに対する重心目標速度Ｖb_cmd_yの制限度合いを大きくすることの要求が高いほど）、小さくなるように（“０”に近づくように）決定される。

ヨーレート指令設定部３２ｄには、障害物と車両１との位置関係を示す情報として、車両１と障害物との間の距離ｄと、障害物に対する車両１の向きを示す方位角φとが入力されることに加えて、さらに、重心速度推定部３３で算出された車両系全体重心のＸ軸方向の速度推定値Ｖb_estm1_xが入力される。そして、ヨーレート指令設定部３２ｄは、入力された方位角φ、距離ｄ及び車両系全体重心のＸ軸方向の速度Ｖb_xの推定値Ｖb_estm1_xから、図１４のブロック線図で示す処理を実行することで、ヨーレート指令ωz_cmdを決定する。

具体的には、ヨーレート指令設定部３２ｄは、入力された方位角φに応じて、ヨーレート指令ωz_cmdの基本値であるヨーレート基本値ωz_cmd1を設定する処理をヨーレート基本値設定部３２d1で実行する。この場合、ヨーレート基本値設定部３２d1は、ヨーレート基本値ωz_cmd1を、ヨーレート指令設定部３２ｄに入力された方位角φから、あらかじめ作成されたマップもしくは演算式により、例えば図１５Ａのグラフで示す形態で設定する。すなわち、方位角φが＋９０°よりも若干大きい所定値＋φ１から＋１８０°までの範囲である場合、あるいは、－φ１から－１８０°までの範囲である場合には、ヨーレート基本値ωz_cmd1は“０に設定される。

また、方位角φが０°よりも若干大きい所定値＋φ２から＋φ１までの範囲である場合には、方位角φが＋φ２に近づくに伴い、ヨーレート基本値ωz_cmd1が正方向（上方から見て反時計回り方向）に増加するように設定される。また、方位角φが－φ２から－φ１までの範囲である場合には、方位角φが－φ２に近づくに伴い、ヨーレート基本値ωz_cmd1が負方向（上方から見て時計回り方向）に増加するように設定される。

さらに、方位角が－φ２～＋φ２の範囲である場合には、ヨーレート基本値ωz_cmd1は、－φ２に対応する負の値から＋φ２に対応する正の値まで連続的に変化するように設定される。これは、方位角φがゼロ近辺で変化するときに、ヨーレート基本値ω_cmd1が不連続的に変化するのを防止するためである。

上記のようにヨーレート基本値ωz_cmd1を方位角φに応じて設定することで、ωz_cmd1は、－φ１～＋φ１の範囲で、車両１の前方の向きを障害物から離す方向の角速度を車両１に発生させるように設定される。

ヨーレート指令設定部３２ｄは、さらに、入力された距離ｄから前記第１制限率β(ｄ)を設定する処理を、第１制限率設定部３２d2で設定すると共に、入力された速度推定値Ｖb_estm1_xから速度係数ｋ(Vb_x)を設定する処理を速度係数設定部３２d3で実行する。この場合、第１制限率設定部３２d2の処理は、前記速度制限係数設定部３２ｃの第１制限率設定部３２c1の処理と同じである。従って、距離ｄがｄ１～ｄ３の範囲では、距離ｄの減少に対する第１制限率β(ｄ)の増加率が距離ｄの減少に伴い、小さくなるように第１制限率β(ｄ)が設定される。

なお、ヨーレート指令設定部３２ｄで設定する第１制限率β(ｄ)の距離ｄに対する変化のパターンは、速度制限係数設定部３２ｃで設定する第１制限率β(ｄ)の距離ｄに対する変化のパターンと完全に一致していなくてもよい。例えばｄ１～ｄ３の範囲でのβ(ｄ)の増加率の大きさ等が、ヨーレート指令設定部３２ｄで設定する第１制限率β(ｄ)と、速度制限係数設定部３２ｃで設定する第１制限率β(ｄ)とで異なっていてもよい。

前記速度係数ｋ(Vb_ｘ)は、“０”から“１”までの範囲内の値であり、速度係数設定部３２d3は、この速度係数ｋ(Vb_x)を、入力された速度推定値Ｖb_estm1_xから、あらかじめ作成されたマップもしくは演算式により、例えば図１５Ｂのグラフで示す形態で設定する。

具体的には、速度推定値Ｖb_estm1_xが所定値Ｖ１以下であるときには、速度係数ｋ(Vb_x)は“０”に設定され、所定値Ｖ１から所定値Ｖ２（＞Ｖ１）の範囲では、速度推定値Ｖb_estm1_x の増加に伴い、速度係数ｋ(Vb_x)は“０”から“１”まで増加するように設定される。そして、所定値Ｖ２以上では、速度係数ｋ(Vb_x)は“１”に維持される。

ヨーレート指令設定部３２ｄは、上記の如く方位角φに応じて設定したヨーレート基本値ωz_cmd1に、距離ｄに応じて設定した第１制限率β(d)と、速度推定値Ｖb_estm1_xに応じて設定した速度係数ｋ(Vb_x)とを乗じる処理を処理部３２d4で実行することで、ヨーレート指令ωz_cmdを決定する。

図４の説明に戻って、重心目標速度決定部３２は、上記のように決定されたヨーレート指令ωz_cmdに、車両１の移動動作部３の接地部と尾輪４の接地部との間の距離Ｌを乗じる処理を処理部３２ｅで実行することによって、ヨーレート指令ωz_cmdのヨーレートを車両１に付加的に発生させるために必要な車両系全体重心のＹ軸方向の速度Ｖ4_y（以降、ヨーレート付加用速度Ｖ4_yという）を算出する。

また、重心目標速度決定部３２は、上記の如く重心速度基本要求値Ｖ1_xyと、速度制限係数α_xyを決定した後、Ｖ1_xyの各成分にα_xyの各成分を乗じる処理を処理部３２ｆで実行することにより、重心目標速度Ｖb_cmd_xyの第１暫定値としての第１暫定重心目標速度Ｖ2_xyを算出する。すなわち、Ｖ2_x＝Ｖ1_x・α_x、Ｖ2_y＝Ｖ1_y・α_yという演算により、第１暫定重心目標速度Ｖ2_xyの各成分が算出される。

次いで、重心目標速度決定部３２は、第１暫定重心目標速度Ｖ2_xyに対するリミット処理を処理部３２ｇで施すことによって、重心目標速度Ｖb_cmd_xyの第２暫定値としての第２暫定重心目標速度Ｖ3_xyを決定する。この処理部３２ｇでのリミット処理は、移動動作部３のアクチュエータ８としての電動モータ８ａ，８ｂのそれぞれの回転速度が所定の許容範囲から逸脱しないようにすると共に、Ｘ軸方向の重心目標速度Ｖb_cmd_xを、ゼロ近辺の負の所定値又はゼロを下限値とする範囲に制限するように第２暫定重心目標速度Ｖ3_xyを決定する処理である。

このリミット処理では、処理部３２ｆで算出された第１暫定重心目標速度Ｖ2_x，Ｖ2_yの組が、例えば、Ｖ2_xの値を縦軸、Ｖ2_yの値を横軸とする座標系上で所定の領域内に在る場合には、第１暫定重心目標速度Ｖ2_xyがそのまま第２暫定重心目標速度Ｖ3_ xyとして決定される。

また、処理部３２ｆで算出された第１暫定重心目標速度Ｖ2_x，Ｖ2_yの組が、上記座標系上の所定の領域から逸脱している場合には、該所定の領域の境界上のＶ2_x，Ｖ2_yの値の組に制限したものが、第２暫定重心目標速度Ｖ3 _xyとして決定される。

次いで、重心目標速度決定部３２は、第2暫定重心目標速度Ｖ3_xyに、前記ヨーレート付加用速度Ｖ4_yを加える処理を処理部３２ｈで実行することで、重心目標速度Ｖb_cmd_xyを決定する。具体的には、Ｘ軸方向の第2暫定重心目標速度Ｖ3_xがそのまま、Ｘ軸方向の重心目標速度Ｖb_cmd_xとして決定されると共に、Ｙ軸方向の第2暫定重心目標速度Ｖ3_yにヨーレート付加用速度Ｖ4_yを加えたもの（＝Ｖ3_y＋Ｖ4_y）がＹ軸方向の重心目標速度Ｖb_cmd_yとして決定される。

補足すると、本実施形態の車両１では、重心目標速度決定部３２のうち、速度制限係数設定部３２ｃ、ヨーレート指令設定部３２ｄ、及び処理部３２ｅ，３２ｆ，３２ｈの処理が本発明における移動指令修正部の処理に相当する。

以上の如く重心目標速度決定部３２の処理を実行した後、移動制御部２１は、次に、姿勢制御演算部３４の処理を実行する。この姿勢制御演算部３４は、図４のブロック線図で示す処理によって、搭乗部５及び基体２の姿勢を安定化するように、移動動作部３の目標速度Ｖw1_cmd_xyを決定する。

より詳しくは、姿勢制御演算部３４は、まず、前記重心目標速度Ｖb_cmd_xyの各成分から、重心ずれ影響量Ｖofs_xyの各成分を減じる処理を演算部３４ａで実行することにより、重心ずれ補償後目標速度Ｖb_cmpn_cmd_xy（重心ずれの影響を補償してなる目標速度）を決定する。

次いで、姿勢制御演算部３４は、移動動作部３の接地点の並進加速度の目標値である目標並進加速度DVw1_cmd_xyのうちのＸ軸方向の目標並進加速度DVw1_cmd_xと、Ｙ軸方向の目標並進加速度DVw1_cmd_yとをそれぞれ次式（４ａ），（４ｂ）の演算により算出する。この演算処理が、図４に示す姿勢制御演算部３４のうち、上記演算部３４ａと、積分演算を行う積分演算部３４ｂとを除く演算部の処理である。

DVw1_cmd_x＝Ｋvb_x・(Ｖb_cmpn_cmd_x－Ｖb_estm1_x)
－Ｋth_x・θb_act_x－Ｋw_x・ωb_act_x ……（４ａ）
DVw1_cmd_y＝Ｋvb_y・(Ｖb_cmpn_cmd_y－Ｖb_estm1_y)
－Ｋth_y・θb_act_y－Ｋw_y・ωb_act_y ……（４ｂ）

式（４ａ），（４ｂ）におけるＫvb_xy、Ｋth_xy、Ｋw_xyはあらかじめ設定された所定のゲイン値である。また、式（４ａ）の右辺の第１項は、車両系全体重心のＸ軸方向の重心ずれ補償後目標速度Ｖb_cmpn_cmd_x（最新値）と車両系全体重心の速度の第１推定値Ｖb_estm1_x（最新値）との偏差に応じたフィードバック操作量成分、第２項は、搭乗部５のＹ軸周り方向の実際の傾斜角度θb_act_xの計測値（最新値）に応じたフィードバック操作量成分、第３項は、搭乗部５のＹ軸周り方向の実際の傾斜角速度ωb_act_xの計測値（最新値）応じたフィードバック操作量成分である。そして、Ｘ軸方向の目標並進加速度DVw1_cmd_xは、これらのフィードバック操作量成分の合成操作量として算出される。

同様に、式（４ｂ）の右辺の第１項は、車両系全体重心のＹ軸方向の重心ずれ補償後目標速度Ｖb_cmpn_cmd_y（最新値）と車両系全体重心の速度の第１推定値Ｖb_estm1_y（最新値）との偏差に応じたフィードバック操作量成分、第２項は、搭乗部５のＸ軸周り方向の実際の傾斜角度θb_act_yの計測値（最新値）に応じたフィードバック操作量成分、第３項は、搭乗部５のＸ軸周り方向の実際の傾斜角速度ωb_act_yの計測値（最新値）に応じたフィードバック操作量成分である。そして、Ｙ軸方向の目標並進加速度DVw1_cmd_yは、これらのフィードバック操作量成分の合成操作量として算出される。なお、前記式（４ａ），（４ｂ）はそれぞれ、次式（４ａ）’、（４ｂ）’に書き換えることができる。

DVw1_cmd_x＝Ｋvb_x・(Ｖb_cmd_x－Ｖb_estm1_x)
－Ｋth_x・(Ofst_estm_x／(ｈ－ｒ_x)＋θb_act_x)
－Ｋw_x・ωb_act_x ……（４ａ）’
DVw1_cmd_y＝Ｋvb_y・(Ｖb_cmd_y－Ｖb_estm1_y)
－Ｋth_y・(Ofst_estm_y／(ｈ－ｒ_y)＋θb_act_y)
－Ｋw_y・ωb_act_y ……（４ｂ）’

この場合、式（４ａ）’、（４ｂ）’の右辺第２項は、Ｘ軸方向及びＹ軸方向における実際の車両系全体重心の位置が、移動動作部３の接地部の直上の位置になるようにするためのフィードバック操作量成分としての意味を持つ。

次いで、姿勢制御演算部３４は、積分演算部３４ｂによって、目標並進加速度DVw1_cmd_xyの各成分を積分することによって、移動動作部３の目標速度Ｖw1_cmd1_xy（最新値）を決定する。

移動制御部２１で移動動作部３の目標速度Ｖw1_cmd1_xyを決定する処理は、各演算処理周期において、以上の如く実行される。この処理によって、移動動作部３の目標速度Ｖw1_cmd1_xyは、車両１の搭乗部５及び基体２の姿勢を安定に保ちつつ、車両系全体重心の移動速度（水平方向の移動速度）を重心目標速度Ｖb_cmd_xyに収束させることを実現し得るように決定される。そして、移動制御部２１は、このように決定された目標速度Ｖw1_cmd_xyに移動動作部３の実際の移動速度を追従させるように、電動モータ８ａ，８ｂをフィードバック制御する。

以上説明した実施形態によれば、車両１の操作器としてのジョイスティック１２の操作に応じて設定される前記基本速度指令Ｖjs_xyと、操縦者の上体の動き（体重移動）に伴なう前記重心ずれ影響量Ｖofs_xy（または、重心ずれ量Ｏfst_xy）とに応じて、車両１の代表点の目標速度の基本値としての重心速度基本要求値Ｖ1_xyが決定される。そして、基本的には、重心速度基本要求値Ｖ1_xyを重心目標速度Ｖb_cmd_xyとして設定し、この重心目標速度Ｖb_cmd_xyを実現するように、移動動作部３の移動動作が制御される。このため、操縦者は、操作器の操作（ジョイスティック１２の操作）と、操縦者の上体の動き（体重移動）とのうちの一方又は両方によって、車両１を操縦することができる。

ただし、車両１と障害物との間の距離ｄが所定値ｄ１以下になると、前記したように速度制限係数α_xyが設定されると共に、ヨーレート指令ωz_cmdが設定される。そして、速度制限係数α_xyに応じて、重心目標速度Ｖb_cmd_xyが重心速度基本要求値Ｖ1_xyに対して制限されると共に、ヨーレート指令ωz_cmdのヨーレートを車両１に付加的に発生させるためのヨーレート付加用速度Ｖ4_yが重心目標速度Ｖb_cmd_xyに付加される。

この場合、速度制限係数α_xyの設定用の第１制限率β(ｄ)が、前記した如く図９のグラフで示す如き形態で距離ｄに応じて設定されるため、距離ｄが所定値ｄ１以下になった直後に、第１制限率β(ｄ)が速やかに増加する（ひいては、速度制限係数α_xyのＸ軸方向成分及びＹ軸方向成分）が速やかに小さくなる）。

このため、重心目標速度Ｖb_cmd_xyの各成分が重心速度基本要求値Ｖ1_xyの各成分よりも絶対値が小さい速度に速やかに制限される。従って、距離ｄが所定値ｄ１以下になった直後に、車両１が速やかに減速して、障害物にさらに接近するのを適切に抑制することができる。

また、速度制限係数α_xyは、距離ｄと、障害物に対する車両１の方位角φとに応じて設定されるので、距離ｄと方位角φとに適した態様で、重心目標速度Ｖb_cmd_xyの各成分が重心速度基本要求値Ｖ1_xyの各成分に対して制限することができる。

さらに、方位角φに応じて設定される第２制限率β(φ)は、距離ｄが所定値ｄ３よりも小さい場合（すなわち、車両１が障害物の近くまで移動した場合）には、距離ｄが所定値ｄ３よりも大きい場合よりも、方位角φの幅広い範囲で、“１”もしくはそれに近い値に設定される。このため、車両１の方位角φの幅広い範囲で、車両１が障害物にさらに接近することを抑制する効果を高めることができる。

一方で、距離ｄが所定値ｄ３よりも大きい場合には、第２制限率β(φ)が“１”もしくはそれに近い値に設定される方位角φの範囲が、距離ｄが所定値ｄ３以下である場合に比して小さくなるので、操縦者が車両１を動かし得る方向の自由度を高めることができる。

また、ヨーレート指令ωz_cmdの設定用の第１制限率β(ｄ)が、前記した如く図９のグラフで示す如き形態で距離ｄに応じて設定されるため、距離ｄが所定値ｄ１以下になった直後に、ヨーレート指令ωz_cmdの大きさが、車両１が障害物から離れる方向で速やかに増加する。このため、距離ｄが所定値ｄ１以下になった直後に、車両１を障害物から離す方向のヨーレートが車両１に付加的に発生するように移動動作部３の移動制御が行われる。これにより、車両１が障害物に接近しにくくなって、該障害物にさらに接近するのを適切に抑制することができる。

また、ヨーレート指令ωz_cmdは、距離ｄ及び方位角φに加えて、Ｘ軸方向での車両系重心の速度推定値Ｖb_estm1_x（車両１の代表点の速度）に応じて設定されるため、車両１が障害物に接近する際の様々な動作状態に対して、障害物に対するさらなる接近を抑制する上で、好適なヨーレート指令ωz_cmdを設定して、車両１の移動を制御することができる。

さらに、前記実施形態では、距離ｄが所定値ｄ１以下になると、上記のように、車両系全体重心の速度が速やかに制限され、また、ヨーレート指令ωz_cmdのヨーレートが車両１に付加的に速やかに発生するように移動動作部３の移動制御が行われるので、操縦者は、障害物が実体物であるか仮想的なものであるかによらずに、車両１が障害物に接近し過ぎたことを速やかに体感的に認識することができる。このため、操縦者は、車両１が障害物に近づき過ぎないように車両１の移動操縦を行うことが適切に実施できる。

なお、本発明は、以上説明した実施形態に限定されるものではない。以下に他の実施形態をいくつか説明する。前記実施形態では、重心目標速度Ｖb_cmd_xyの元となる重心速度基本要求値Ｖ1_xyは、操縦者による操作器の操作（ジョイスティック１２の操作）と、操縦者の上体の動き（体重移動）とに応じて設定される。ただし、重心速度基本要求値Ｖ1_xyは、操縦者による操作器の操作（ジョイスティック１２の操作）と、操縦者の上体の動き（体重移動）とのうちのいずれか一方だけに応じて設定してもよい。

また、前記実施形態では、車両１と障害物との間の距離ｄに応じた第１制限率β(d)と、障害物に対する車両１の方位角φに応じた第２制限率β(φ)_xyとを各別に決定した上で、それらを統合した合成制限率（暫定合成制限率）β1_xyを決定した。ただし、例えば、距離ｄと方位角φとから、マップ等を用いて、直接的に合成制限率β1_x，β1_yのそれぞれ（あるいは、１－β1_x及び１－β1_yのそれぞれ）を決定するようにしてもよい。

前記実施形態では、車両１の代表点として車両系全体重心を用いたが、該代表点は、例えば移動動作部３の代表点であってもよい。また、前記実施形態では、移動体として倒立振子型の車両１を例示したが、本発明における移動体は、例えば、電動車椅子、電動カート等の移動体であってもよく、さらには、操縦者が搭乗しないタイプの移動体（操縦者が遠隔操縦を行う移動体）であってもよい。また、移動体の搭乗部は、操縦者が着座可能な搭乗部に限らず、例えば、操縦者が起立して搭乗し得るように構成されていてもよい。

１…車両（移動体）、５…搭乗部、２０…制御装置、２１…移動制御部、３２…重心目標速度決定部（基本移動指令生成部、移動指令修正部）。

Claims (5)

1. 操縦者の操縦操作に応じて移動する移動体の制御装置であって、
   前記操縦者の操縦操作に基づく前記移動体の移動指令である基本移動指令を生成する基本移動指令生成部と、
   前記移動体の移動環境に存在し、又は該移動環境に設定された障害物と前記移動体との位置関係に応じて、前記基本移動指令を修正する移動指令修正部と、
   前記移動指令修正部により修正された基本移動指令である修正後移動指令に応じて前記移動体の移動制御を行う移動制御部とを備えており、
   前記移動指令修正部は、前記障害物と前記移動体との距離が第１所定値以下になったとき、前記移動体に、前記障害物から離れる方向のヨーレートを付加的に発生させるように前記基本移動指令を修正するように構成されていると共に、前記距離の減少に対する前記ヨーレートの大きさの増加率を、前記距離が前記第１所定値に近いほど、大きくするように前記基本移動指令を修正するように構成されていることを特徴とする移動体の制御装置。
2. 請求項１記載の移動体の制御装置において、
   前記移動指令修正部は、前記移動体に付加的に発生させるヨーレートを、前記距離と、前記障害物に対する前記移動体の向きと、前記移動体の移動速度とに応じて変化させるように前記基本移動指令を修正するように構成されていることを特徴とする移動体の制御装置。
3. 請求項１又は２記載の移動体の制御装置において、
   前記移動指令修正部は、さらに、前記距離が前記第１所定値以下になったとき、前記移動体の代表点の移動速度を、前記基本移動指令に応じて規定される移動速度である基本移動速度よりも絶対値が小さい移動速度に制限するように前記基本移動指令を修正するように構成されていると共に、前記距離の減少に対する前記移動速度の制限度合いの増加率を、前記距離が前記第１所定値に近いほど、大きくするように前記基本移動指令を修正するように構成されていることを特徴とする移動体の制御装置。
4. 請求項３記載の移動体の制御装置において、
   前記移動指令修正部は、
   前記距離が前記第１所定値以下になったとき、前記距離に応じた前記移動速度の制限度合いを示す第１制限度合い指標値を設定する処理と、前記障害物に対する前記移動体の向きに応じた前記移動速度の制限度合いを示す第２制限度合い指標値を設定する処理とを実行し、該第１制限度合い指標値及び該第２制限度合い指標値に応じて前記移動体の代表点の移動速度を制限するように構成されていると共に、
   前記距離が前記第１所定値よりも小さい第２所定値以下の距離である場合に設定する前記第２制限度合い指標値により示される制限度合いを第Ａ制限度合い、前記距離が前記第２所定値よりも大きい距離である場合に設定する前記第２制限度合い指標値により示される制限度合いを第Ｂ制限度合いと定義したとき、前記移動体の向きの可変範囲のうちの少なくとも一部の範囲において、前記第Ａ制限度合いが前記第Ｂ制限度合いよりも大きくなるように前記第２制限度合い指標値を設定するように構成されていることを特徴とする移動体の制御装置。
5. 請求項１～４のいずれか１項に記載の移動体の制御装置において、
   前記移動体は、前記操縦者が搭乗する搭乗部を有する搭乗型移動体であることを特徴とする移動体の制御装置。

Images