特許 7294179 自律移動体

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】

(24)【登録日】2023-06-12

(45)【発行日】2023-06-20

(54)【発明の名称】自律移動体

(51)【国際特許分類】

   G05D 1/02 20200101AFI20230613BHJP

【ＦＩ】

G05D1/02 X

【請求項の数】 2

(21)【出願番号】P 2020025182

(22)【出願日】2020-02-18

(65)【公開番号】P2021131601

(43)【公開日】2021-09-09

【審査請求日】2022-05-18

(73)【特許権者】

【識別番号】000003218

【氏名又は名称】株式会社豊田自動織機

(74)【代理人】

【識別番号】100105957

【弁理士】

【氏名又は名称】恩田 誠

(74)【代理人】

【識別番号】100068755

【弁理士】

【氏名又は名称】恩田 博宣

(72)【発明者】

【氏名】山村 憲司

【審査官】今井 貞雄

(56)【参考文献】

【文献】特開昭６０－１１２１１０（ＪＰ，Ａ）

【文献】特開平１－２９６３１８（ＪＰ，Ａ）

【文献】特開２０１０－９２２７９（ＪＰ，Ａ）

(58)【調査した分野】(Int.Cl.，ＤＢ名)

Ｇ０５Ｄ １／０２

(57)【特許請求の範囲】

【請求項1】

モータの駆動により移動する移動体と、
前記移動体の一点から延びる指令ベクトルの大きさを速度指令値、前記指令ベクトルの向きを方向指令値とし、前記移動体の速度が前記速度指令値に追従し、前記移動体の移動方向が前記方向指令値に追従するように制御を行う走行制御部と、
１回の制御周期の間に前回の制御周期での前記指令ベクトルから変更可能な前記指令ベクトルの範囲を変更可能範囲とすると、
前記変更可能範囲を加味せずに目標位置に基づいて算出される目標速度指令値及び目標方向指令値からなる目標指令ベクトルを導出する目標指令導出部と、
前記目標指令ベクトルが前記変更可能範囲内か否かを判定する判定部と、
前記目標指令ベクトル及び、前回の制御周期での前記指令ベクトルに基づいて、前記変更可能範囲内の制限速度指令値及び制限方向指令値からなる制限指令ベクトルを導出する制限指令導出部と、
前記目標指令ベクトルが前記変更可能範囲内の場合、前記目標指令ベクトルを前記指令ベクトルとし、前記目標指令ベクトルが前記変更可能範囲外の場合、前記制限指令ベクトルを前記指令ベクトルとする指令ベクトル決定部と、を備える自律移動体。

【請求項2】

前記制限指令導出部は、前記指令ベクトルを前記目標指令ベクトルに最短で変更させることができる最短変更優先ベクトル、前記指令ベクトルの前記方向指令値を前記目標指令ベクトルの前記目標方向指令値に最短で変更させることができる方向変更優先ベクトル、及び前記変更可能範囲内で前記速度指令値が最も低くなる減速優先ベクトルの少なくともいずれかに基づき前記制限指令ベクトルを導出する請求項１に記載の自律移動体。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、自律移動体に関する。

【背景技術】

【0002】

特許文献１に記載されているように、自律移動体は、移動体と、制御装置と、を備える。移動体は、複数の車輪と、車輪を回転させるためのモータと、モータを駆動させるモータドライバと、を備える。制御装置は、移動体が目標位置に向けて移動するように走行指令を導出する。走行指令は、速度指令値と方向指令値とを含む。移動体の速度が速度指令値に追従し、移動体の移動方向が方向指令値に追従するようにモータの回転数が制御される。制御装置は、所定の制御周期毎に走行指令を導出する。これにより、自律移動体は自律的に移動する。

【先行技術文献】

【特許文献】

【0003】

【文献】特開２０１９－９５２５６号公報

【発明の概要】

【発明が解決しようとする課題】

【0004】

目標位置が変更になると、速度指令値や方向指令値が大きく変更される場合がある。しかしながら、モータの応答性やモータの出力等、モータの性能による制限によって、１回の制御周期の間に移動体の速度や移動方向を走行指令に追従させることができない場合がある。即ち、移動体を円滑に移動させることができない場合がある。

【0005】

本発明の目的は、移動体を円滑に移動させることができる自律移動体を提供することにある。

【課題を解決するための手段】

【0006】

上記課題を解決する自律移動体は、モータの駆動により移動する移動体と、前記移動体の一点から延びる指令ベクトルの大きさを速度指令値、前記指令ベクトルの向きを方向指令値とし、前記移動体の速度が前記速度指令値に追従し、前記移動体の移動方向が前記方向指令値に追従するように制御を行う走行制御部と、１回の制御周期の間に前回の制御周期での前記指令ベクトルから変更可能な前記指令ベクトルの範囲を変更可能範囲とすると、前記変更可能範囲を加味せずに目標位置に基づいて算出される目標速度指令値及び目標方向指令値からなる目標指令ベクトルを導出する目標指令導出部と、前記目標指令ベクトルが前記変更可能範囲内か否かを判定する判定部と、前記目標指令ベクトル及び、前回の制御周期での前記指令ベクトルに基づいて、前記変更可能範囲内の制限速度指令値及び制限方向指令値からなる制限指令ベクトルを導出する制限指令導出部と、前記目標指令ベクトルが前記変更可能範囲内の場合、前記目標指令ベクトルを前記指令ベクトルとし、前記目標指令ベクトルが前記変更可能範囲外の場合、前記制限指令ベクトルを前記指令ベクトルとする指令ベクトル決定部と、を備える。

【0007】

目標指令ベクトルが変更可能範囲内の場合、目標指令ベクトルが指令ベクトルとなる。目標指令ベクトルが変更可能範囲外の場合、制限指令ベクトルが指令ベクトルとなる。従って、指令ベクトルは、変更可能範囲内のベクトルとなる。１回の制御周期で指令ベクトルが変更可能範囲を超えて変更されることが抑制される。１回の制御周期で指令ベクトルが過剰に変更されることが抑制されるため、移動体を円滑に移動させることができる。

【0008】

上記自律移動体について、前記制限指令導出部は、前記指令ベクトルを前記目標指令ベクトルに最短で変更させることができる最短変更優先ベクトル、前記指令ベクトルの前記方向指令値を前記目標指令ベクトルの前記目標方向指令値に最短で変更させることができる方向変更優先ベクトル、及び前記変更可能範囲内で前記速度指令値が最も低くなる減速優先ベクトルの少なくともいずれかに基づき前記制限指令ベクトルを導出してもよい。

【発明の効果】

【0009】

本発明によれば、移動体を円滑に移動させることができる。

【図面の簡単な説明】

【0010】

【図1】自律移動体及び追尾対象を示す概略図。

【図2】自律移動体を示すブロック図。

【図3】センサの検出範囲を模式的に示す図。

【図4】指令ベクトルの模式図。

【図5】制御装置が行う処理を示すフローチャート。

【図6】変更可能範囲を示す図。

【図7】制限指令ベクトルの導出方法を説明するための図。

【発明を実施するための形態】

【0011】

以下、自律移動体の一実施形態について説明する。
図１に示すように、自律移動体１０は、車両２０と、車両２０に搭載されたセンサ３１と、車両２０に搭載された制御装置３２と、を備える。車両２０は、車体２１と、複数の車輪２２と、車両２０を移動させるための駆動機構２３と、を備える。車両２０は、制御装置３２に制御されることで、登録された追尾対象Ｔを追尾するように自律移動する移動体である。自律移動体１０は、追尾対象Ｔとの離間距離が所定の範囲内となるように移動する。

【0012】

本実施形態の車輪２２は、オムニホイール、メカナムホイール、オムニボールなどの全方向移動車輪である。車輪２２は、３つ以上設けられている。車輪２２の回転数、及び、回転方向が制御されることで、車両２０は、車体２１の向きを維持した状態での全方向への移動、車体２１の向きを変更しながらの移動、移動しない状態での車体２１の向きの変更が可能である。車両２０は、操舵装置を備えず、複数の車輪２２同士の回転数の差と回転方向の違いを利用して移動方向を変更するものである。なお、上記した「全方向」とは、車両２０が移動する路面上での移動方向を示す。

【0013】

図２に示すように、駆動機構２３は、車輪２２を回転させるためのモータ２４と、モータ２４を駆動させるモータドライバ２５と、エンコーダ２６と、を備える。なお、図示は省略するが、モータ２４及びモータドライバ２５は、車輪２２の数と同数設けられる。これにより、各車輪２２の回転数と回転方向を独立して制御することが可能である。エンコーダ２６は、車輪２２毎に個別に設けられている。

【0014】

モータドライバ２５には、指令回転数を示す情報と指令回転方向を示す情報を含む指令が制御装置３２から入力される。モータドライバ２５は、指令回転数に追従するようにモータ２４の回転数を制御する。モータドライバ２５は、指令回転方向とモータ２４の回転方向が一致するようにモータ２４を制御する。

【0015】

エンコーダ２６は、例えば、モータ２４の回転軸の回転量に基づいたパルス信号を出力するインクリメンタル型のエンコーダである。エンコーダ２６は、モータ２４の回転軸の回転数を検出する。モータドライバ２５は、エンコーダ２６の検出結果から、モータ２４の回転数、及び回転方向を認識可能である。車両２０は、モータ２４の駆動による車輪２２の回転によって移動する移動体である。

【0016】

センサ３１としては、制御装置３２に車両２０の周辺に存在する物体を認識させることができ、かつ、自律移動体１０から物体までの距離を測定できるものが用いられる。物体は、追尾対象Ｔ及び障害物を含む。

【0017】

本実施形態では、センサ３１として、LIDAR：Laser Imaging Detection and Rangingを用いている。LIDARは、レーザーを周辺に照射し、レーザーが当たった部分によって反射された反射光を受光することで周辺環境を認識可能な距離計である。本実施形態のLIDARとしては、水平方向の照射角度を変更しながらレーザーを照射する二次元距離計が用いられる。

【0018】

図３に示すように、センサ３１のレーザーの照射範囲は、例えば、水平方向への照射可能角度θ１＝２７０°の範囲である。照射可能角度θ１の中央を基準軸Ｂとすると、レーザーの照射範囲は基準軸Ｂ±１３５°の範囲である。センサ３１のレーザーの照射範囲は、中心角２７０°の扇形となる。レーザーの照射範囲は、センサ３１による物体の検出を行える検出範囲ともいえる。自律移動体１０は、基準軸Ｂが追尾対象Ｔを向くように移動する。レーザーは、２７０°の範囲を一定角度置きに照射されていく。一定角度は、角度分解能に応じた角度である。

【0019】

レーザーが照射された物体における当該レーザーが当たった部分を測定点とすると、センサ３１は、センサ３１から測定点までの距離をレーザー照射から反射光を受光するまでの時間から算出し、レーザーの照射角度に対応付けて距離情報として、制御装置３２に出力するものである。詳述すると、センサ３１は、レーザー照射後、反射光を受光すると、レーザー照射から反射光を受光するまでの時間からセンサ３１から測定点までの距離を算出し、算出した距離をレーザーの照射角度に対応付けて距離情報として、制御装置３２に出力する。従って、制御装置３２は、センサ３１から受信した距離情報から車両２０の周囲に物体が存在することを判断することができ、物体が存在する場合、物体までの距離と照射角度とを把握できる。なお、距離情報は、測定点の座標といえる。測定点の座標は、基準軸Ｂの延びる方向をＸ軸、水平方向のうちＸ軸に直交する軸をＹ軸とする直交座標系の座標である。座標は、センサ３１を原点とする直交座標系の測定点の位置を表しているといえる。なお、本実施形態では、センサ３１を直交座標系の原点としているが、車体２１における水平方向の中心位置等、自律移動体１０の任意の位置を原点とすることができる。

【0020】

制御装置３２は、ＣＰＵやＧＰＵ等のプロセッサ３３と、ＲＡＭ及びＲＯＭ等からなる記憶部３４と、を備える。記憶部３４には、自律移動体１０を動作させるためのプログラムが記憶されている。記憶部３４は、処理をプロセッサ３３に実行させるように構成されたプログラムコードまたは指令を格納しているといえる。記憶部３４、すなわち、コンピュータ可読媒体は、汎用または専用のコンピュータでアクセスできるあらゆる利用可能な媒体を含む。制御装置３２は、ASIC：Application Specific Integrated CircuitやFPGA：Field Programmable Gate Array等のハードウェア回路によって構成されていてもよい。処理回路である制御装置３２は、コンピュータプログラムに従って動作する１つ以上のプロセッサ、ASICやFPGA等の１つ以上のハードウェア回路、或いは、それらの組み合わせを含み得る。また、自律移動体１０は、制御装置３２が読み取り可能な補助記憶装置を備えていてもよく、補助記憶装置に各種プログラムが記憶されていてもよい。

【0021】

制御装置３２は、センサ３１の検出結果から車両２０の周囲に存在する物体を検出する。制御装置３２は、複数の距離情報のうち、距離の差が所定範囲内の値が所定範囲内の照射角度に亘って存在する場合、それら複数の距離情報をクラスタ化して点群とする。制御装置３２は、点群を物体と判断する。制御装置３２は、点群の座標から物体の水平方向の寸法を導出する。

【0022】

制御装置３２は、検出された物体から追尾対象Ｔを認識する。追尾対象Ｔの情報は、制御装置３２の記憶部３４に記憶されている。追尾対象Ｔの情報とは、例えば、追尾対象Ｔの形状や追尾対象Ｔの水平方向の寸法である。制御装置３２は、登録された追尾対象Ｔの情報に適合する物体を追尾対象Ｔと判定する。制御装置３２は、追尾対象Ｔと判定した物体とは異なる物体を障害物と判定する。

【0023】

制御装置３２は、車両２０を制御するための各種指令を導出する。制御装置３２は、指令ベクトルを導出し、この指令ベクトルに従って車両２０を走行させる。指令ベクトルは、車両２０の一点から延びるベクトルである。本実施形態において、車両２０の一点は、水平方向における車両２０の中心ＣＰである。

【0024】

図４に指令ベクトルＶＶの一例を示す。指令ベクトルＶＶの大きさＶは速度指令値を示し、指令ベクトルＶＶの向きθは方向指令値を示している。指令ベクトルの向きθは、Ｘ軸と、指令ベクトルＶＶとがなす角の角度である。制御装置３２は、所定の制御周期毎に指令ベクトルＶＶを導出する。以下、制御装置３２が車両２０を移動させる際の制御について説明する。

【0025】

図５に示すように、ステップＳ１において、制御装置３２は、目標位置に基づいて算出される目標速度指令値及び目標方向指令値からなる目標指令ベクトルを導出する。目標位置は、車両２０の現在位置、車両２０の周辺環境、追尾対象Ｔの現在位置等によって設定される。本実施形態の車両２０は、追尾対象Ｔを追尾するため、車両２０の周囲に障害物が存在しない場合、目標位置は追尾対象Ｔから離間した位置であって追尾対象Ｔとの離間距離が所定の範囲内となる位置に設定される。車両２０の周囲に障害物が存在している場合であり、追尾対象Ｔを追尾する際に障害物に接触したり、障害物に過剰に接近するおそれがある場合、制御装置３２は障害物を回避できる位置に目標位置を設定する。制御装置３２は、例えば、車両２０の速度から目標指令ベクトルの目標速度指令値を算出する。制御装置３２は、車両２０を加速させる場合、車両２０の現在の速度よりも高い値を目標速度指令値とする。制御装置３２は、車両２０の速度を維持する場合、車両２０の現在の速度と同様の値を目標速度指令値とする。制御装置３２は、車両２０を減速させる場合、車両２０の現在の速度よりも低い値を目標速度指令値とする。なお、目標指令ベクトルの目標速度指令値は、種々の手法で導出することができる。例えば、目標指令ベクトルの目標速度指令値は、車両２０から目標位置までの距離が長いほど大きな値としてもよい。制御装置３２は、車両２０の移動方向が目標位置を向くように目標指令ベクトルの目標方向指令値を算出する。ステップＳ１の処理を行うことで、制御装置３２は目標指令導出部を備えているといえる。

【0026】

次に、ステップＳ２において、制御装置３２は、目標指令ベクトルと、前回の制御周期での指令ベクトルとを比較し、目標指令ベクトルが変更可能範囲内か否かを判定する。変更可能範囲とは、１回の制御周期の間に前回の制御周期での指令ベクトルから変更可能な指令ベクトルの範囲である。変更可能範囲は、前回の制御周期での指令ベクトルから導出される。

【0027】

図６に示すように、指令ベクトルは、車両２０の中心ＣＰを原点とする極座標系の極座標として捉えることができる。詳細にいえば、指令ベクトルは、車両２０の中心ＣＰを原点、Ｘ軸を始線、指令ベクトルとＸ軸とのなす角を偏角、原点からの指令ベクトルの大きさを動径とする極座標系における極座標として捉えることができる。前回の制御周期での指令ベクトルＶＶ０の大きさをＶ０、向きをθ０とすると、前回の制御周期での指令ベクトルＶＶ０の極座標は（Ｖ０，θ０）と捉えることができる。図４に示す指令ベクトルＶＶの極座標は（Ｖ，θ）と捉えることができる。

【0028】

変更可能範囲Ａは、上記極座標系における前回の制御周期での指令ベクトルＶＶ０の極座標（Ｖ０，θ０）を中心とする円形の範囲である。変更可能範囲Ａの半径ｒは、前回の制御周期での指令ベクトルＶＶ０の極座標（Ｖ０，θ０）を始点とするベクトルで定義され、当該ベクトルの大きさは、例えば、車両２０の加速度［ｍ／ｓ^２］×制御周期［ｓ］である。変更可能範囲Ａの半径ｒは、前回の制御周期での指令ベクトルＶＶ０の極座標（Ｖ０，θ０）を極座標系の原点とした場合に、１回の制御周期の間に変化可能な速度［ｍ／ｓ］の上限を表しているといえる。なお、本実施形態において、車両２０の加速度は一定である。車両２０の加速度の上限値は、モータ２４の性能によって定まる。車両２０の加速度は、加速度の上限値以下の範囲で適宜設定することができる。

【0029】

上記したように、指令ベクトルは、車両２０の中心ＣＰを原点とする極座標系での極座標として捉えることができる。一方で、変更可能範囲Ａは、前回の制御周期での指令ベクトルＶＶ０の極座標（Ｖ０，θ０）を始点とするベクトルで定義されている。変更可能範囲Ａについても、車両２０の中心ＣＰを原点とする極座標系での極座標として捉えるため、以下の方法で、車両２０の中心ＣＰを原点とする変更可能範囲Ａの極座標を導出する。

【0030】

図６及び図７に示すように、前回の制御周期での指令ベクトルＶＶ０の大きさＶ０は既知の値であり、変更可能範囲Ａの半径ｒも既知の値である。前回の制御周期での指令ベクトルＶＶ０と、前回の制御周期での指令ベクトルＶＶ０から変更可能範囲Ａの円周上に延びるベクトルとのなす角の角度をθｎとする。余弦定理を用いて、角度θｎを任意の値とした場合における車両２０の中心ＣＰから変更可能範囲Ａの円周上までのベクトルＶＶｒの大きさＶｒを導出することができる。同様に、余弦定理を用いて、角度θｎを任意の値とした場合における車両２０の中心ＣＰから変更可能範囲Ａの円周上まで延びるベクトルＶＶｒと、前回の制御周期での指令ベクトルＶＶ０とのなす角の角度θｘを導出することができる。前回の制御周期での指令ベクトルＶＶ０の向きθ０は既知の値であるため、向きθ０と角度θｘから、車両２０の中心ＣＰから変更可能範囲Ａの円周上までのベクトルＶＶｒの向きθｒを導出することができる。即ち、前回の制御周期での指令ベクトルＶＶ０と、変更可能範囲Ａの半径ｒから、角度θｎを任意の値とした場合における車両２０の中心ＣＰから変更可能範囲Ａの円周上までのベクトルＶＶｒの極座標（Ｖｒ，θｒ）を導出することができる。角度θｎを所定角度毎に変更して、車両２０の中心ＣＰから変更可能範囲Ａの円周上までのベクトルＶＶｒを導出することで、車両２０の中心ＣＰを原点とする変更可能範囲Ａの円周上の極座標を導出することができる。仮に、角度θｎを１°毎に変更した場合、変更可能範囲Ａの円周上の極座標が３６０箇所得られ、当該極座標を円周とする変更可能範囲Ａを導出できる。変更可能範囲Ａは、前回の制御周期での指令ベクトルＶＶ０からの速度指令値の変更量が大きいほど方向指令値の変更量が小さくなるように設定された範囲である。

【0031】

変更可能範囲Ａを導出するための半径ｒと角度θｎは予め設定されており、記憶部３４や補助記憶装置に記憶されている。制御装置３２は、半径ｒと角度θｎを用いて前回の制御周期での指令ベクトルＶＶ０から変更可能範囲Ａを導出可能である。制御装置３２は、目標指令ベクトルと変更可能範囲Ａとを比較し、目標指令ベクトルが変更可能範囲Ａ内に含まれているかの判定を行う。なお、変更可能範囲Ａの円周上は変更可能範囲Ａ内とみなす。

【0032】

図５に示すように、制御装置３２は、ステップＳ２の判定結果が肯定の場合、ステップＳ４の処理を行う。制御装置３２は、ステップＳ２の判定結果が否定の場合、ステップＳ３の処理を行う。ステップＳ２の処理を行うことで、制御装置３２は判定部を備えているといえる。

【0033】

ステップＳ３において、制御装置３２は、目標指令ベクトル及び、前回の制御周期での指令ベクトルＶＶ０に基づいて、変更可能範囲Ａ内での制限速度指令値及び制限方向指令値からなる制限指令ベクトルを導出する。制限指令ベクトルは、車両２０の中心ＣＰを原点とした極座標系の極座標として捉えた場合、極座標が変更可能範囲Ａ内に位置するベクトルである。目標指令ベクトルが変更可能範囲Ａ外となる場合、目標指令ベクトルが前回の制御周期の指令ベクトルＶＶ０から大きく変更されたといえる。このように、目標指令ベクトルが変更可能範囲Ａ外となる場合、前回の制御周期から目標位置が大きく変更された場合といえる。例えば、前回の制御周期での指令ベクトルＶＶ０の向きθ０に、障害物が存在している場合がある。すると、障害物を回避するために、目標位置が大きく変更される場合がある。

【0034】

図７に示す例を用いて、制限指令ベクトルの導出態様について説明する。図７に示す例では、ステップＳ１で導出された目標指令ベクトルＶＶ１が変更可能範囲Ａ外に位置している。

【0035】

制限指令ベクトルを導出する際に、制御装置３２は、最短変更優先ベクトルＶＶ２、方向変更優先ベクトルＶＶ３、及び減速優先ベクトルＶＶ４を導出する。最短変更優先ベクトルＶＶ２は、指令ベクトルを目標指令ベクトルＶＶ１に最短で変更させることができるベクトルである。なお、「最短」とは、指令ベクトルを目標指令ベクトルＶＶ１にするまでの時間、言い換えれば、目標指令ベクトルＶＶ１が変更可能範囲Ａ内となるまでの時間が最も短いことを示す。最短変更優先ベクトルＶＶ２は、車両２０の中心ＣＰを原点とする極座標系において前回の制御周期の指令ベクトルＶＶ０の極座標と目標指令ベクトルＶＶ１の極座標とを繋ぐ線分Ｌ１と変更可能範囲Ａの円周との交点Ｐ１を極座標とするベクトルである。なお、線分Ｌ１は、最短変更優先ベクトルＶＶ２を指令ベクトルとして、前回の制御周期での指令ベクトルＶＶ０から目標指令ベクトルＶＶ１に指令ベクトルを連続的に変化させた場合の軌跡と捉えることができる。線分Ｌ１から把握できるように、線分Ｌ１に沿って指令ベクトルを変更することで、最も短い軌跡で指令ベクトルが目標指令ベクトルＶＶ１に変更されることがわかる。

【0036】

方向変更優先ベクトルＶＶ３は、指令ベクトルの方向指令値を目標指令ベクトルＶＶ１の目標方向指令値に最短で変更させることができるベクトルである。方向変更優先ベクトルＶＶ３は、車両２０の中心ＣＰを原点とする極座標系において前回の制御周期の指令ベクトルＶＶ０の極座標から目標指令ベクトルＶＶ１に延びる垂線Ｌ２と変更可能範囲Ａの円周との交点Ｐ２を極座標とするベクトルである。なお、垂線Ｌ２は、方向変更優先ベクトルＶＶ３を指令ベクトルとして、前回の制御周期での指令ベクトルＶＶ０から目標指令ベクトルＶＶ１に指令ベクトルを連続的に変化させた場合の軌跡と捉えることができる。垂線Ｌ２から把握できるように、垂線Ｌ２に沿って指令ベクトルを変更することで、最も短い軌跡で指令ベクトルの方向指令値が目標指令ベクトルＶＶ１の目標方向指令値に変更されることがわかる。

【0037】

減速優先ベクトルＶＶ４は、変更可能範囲Ａ内で速度指令値が最も低くなるベクトルである。減速優先ベクトルＶＶ４は、前回の制御周期での指令ベクトルＶＶ０と変更可能範囲Ａの円周との交点Ｐ３を極座標とするベクトルである。減速優先ベクトルＶＶ４は、前回の制御周期での指令ベクトルＶＶ０の方向指令値と向きが同一なベクトルである。

【0038】

制御装置３２は、最短変更優先ベクトルＶＶ２、方向変更優先ベクトルＶＶ３、及び減速優先ベクトルＶＶ４に基づき制限指令ベクトルを導出する。一例として、障害物を回避する場合に制限指令ベクトルを導出する導出態様について説明する。

【0039】

制御装置３２は、車両２０の現在位置から障害物までの距離と、車両２０の速度から、減速をしなくても障害物を回避できる場合には最短変更優先ベクトルＶＶ２を制限指令ベクトルとして選択する。車両２０の現在位置から障害物までの距離と、車両２０の速度から、減速しなければ障害物を回避できないおそれがあったり、車両２０が障害物に過剰に近づくおそれがある場合には減速優先ベクトルＶＶ４を制限指令ベクトルとして選択する。車両２０の現在位置から障害物までの距離と、車両２０の速度から、減速優先ベクトルＶＶ４ほどの減速は不要であるが、最短変更優先ベクトルＶＶ２よりは減速を行う必要がある場合、方向変更優先ベクトルＶＶ３を制限指令ベクトルとして選択する。また、制御装置３２は、最短変更優先ベクトルＶＶ２、方向変更優先ベクトルＶＶ３、及び減速優先ベクトルＶＶ４から複数のベクトルを選択し、選択したベクトルを合成したものを制限指令ベクトルとして選択してもよい。選択したベクトルを合成する際に、選択したベクトルに重み付けをし、合成を行ってもよい。制限指令ベクトルは、前回の制御周期の指令ベクトルＶＶ０の向きと同一の向き、あるいは、前回の制御周期の指令ベクトルＶＶ０よりも目標指令ベクトルＶＶ１の向きとの差が小さいベクトルとなる。

【0040】

上記したように、制御装置３２は、目標指令ベクトルＶＶ１及び、前回の制御周期での指令ベクトルＶＶ０に基づいて、変更可能範囲Ａ内での制限速度指令値及び制限方向指令値からなる制限指令ベクトルを導出する。詳細にいえば、目標指令ベクトルＶＶ１、前回の制御周期での指令ベクトルＶＶ０、及び変更可能範囲Ａから最短変更優先ベクトルＶＶ２、方向変更優先ベクトルＶＶ３、及び減速優先ベクトルＶＶ４を導出し、これらのベクトルから制限指令ベクトルを導出している。最短変更優先ベクトルＶＶ２、方向変更優先ベクトルＶＶ３、及び減速優先ベクトルＶＶ４に基づき制限指令ベクトルを導出する態様には、最短変更優先ベクトルＶＶ２、方向変更優先ベクトルＶＶ３、及び減速優先ベクトルＶＶ４そのものを制限指令ベクトルとして選択する態様が含まれる。また、最短変更優先ベクトルＶＶ２、方向変更優先ベクトルＶＶ３、及び減速優先ベクトルＶＶ４を合成することで得られたベクトルを制限指令ベクトルとする態様も含まれる。

【0041】

上記したように、制限指令ベクトルは、目標指令ベクトル及び、前回の制御周期での指令ベクトルに基づいて導出される。一方で、目標指令ベクトルは、変更可能範囲を加味せずに目標位置に基づいて導出されている。

【0042】

制限指令ベクトルの導出態様の一例について説明したが、制限指令ベクトルの導出態様は、目標指令ベクトルＶＶ１、前回の制御周期での指令ベクトルＶＶ０、センサ３１により検出した追尾対象Ｔや障害物との位置関係、車両２０の速度等、種々の要素によって種々のものが考えられる。上記した例では、障害物を回避する場合について説明したが、障害物を回避する必要がない場合には、上記した導出態様とは異なる態様で制限指令ベクトルが導出される。例えば、制御装置３２が追尾対象Ｔを検出できなくなり、その後、追尾対象Ｔを再度検出した場合、追尾対象Ｔの位置によっては、回避する必要のある障害物が存在しないにも関わらず、目標位置が大きく変更される場合がある。制御装置３２は、車両２０に障害物を回避させる必要がないため、減速を考慮せずに制限指令ベクトルを導出することができる。ステップＳ３の処理を行うことで、制御装置３２は制限指令導出部を備えているといえる、
ステップＳ４において、制御装置３２は、指令ベクトルを決定する。制御装置３２は、目標指令ベクトルが変更可能範囲Ａ内の場合、目標指令ベクトルを指令ベクトルとする。制御装置３２は、目標指令ベクトルが変更可能範囲Ａ外の場合、制限指令ベクトルを指令ベクトルとする。ステップＳ４の処理を行うことで、制御装置３２は、指令ベクトル決定部を備えているといえる。

【0043】

ステップＳ５において、制御装置３２は、指令ベクトルに従い車両２０を移動させる。制御装置３２は、指令ベクトルの速度指令値に従った速度で、指令ベクトルの方向指令値に従った方向に移動するように、モータ２４の指令回転数及び指令回転方向を演算する。制御装置３２は、複数のモータ２４毎に個別に指令回転数及び指令回転方向を演算し、モータドライバ２５に指令回転数及び指令回転方向を含む指令を出力する。モータドライバ２５は、指令回転数及び指令回転方向に従ってモータ２４を制御する。これにより、指令ベクトルに従い車両２０は移動する。ステップＳ５の処理を行うことで、制御装置３２は走行制御部を備えているといえる。

【0044】

ステップＳ６において、制御装置３２は、指令ベクトルを保存する。指令ベクトルは、例えば、ＲＡＭや補助記憶装置に記憶される。ステップＳ６で保存された指令ベクトルは、次回の制御周期で、前回の制御周期での指令ベクトルとして、変更可能範囲Ａの導出や制限指令ベクトルの導出に用いられる。

【0045】

本実施形態の作用について説明する。
目標指令ベクトルが変更可能範囲Ａ内の場合、目標指令ベクトルが指令ベクトルとされる。目標指令ベクトルは、車両２０が目標位置に向かうように導出されているため、目標指令ベクトルが指令ベクトルとなると、車両２０は目標位置に向けて移動する。

【0046】

１回の制御周期の間に、目標位置が大きく変更となった場合、目標指令ベクトルが変更可能範囲Ａ外となる場合がある。この場合、制限指令ベクトルが指令ベクトルとされる。制限指令ベクトルは、変更可能範囲Ａ内のベクトルであり、前回の制御周期での指令ベクトルから目標指令ベクトルに近づくように導出されている。従って、制御周期が経過する毎に、制限指令ベクトルは、目標指令ベクトルに近付いていく。制限指令ベクトルは、指令ベクトルを急激に目標指令ベクトルにせずに、指令ベクトルを段階的に目標指令ベクトルにするためのベクトルといえる。指令ベクトルが急激に変更された場合であっても、モータ２４の応答性やモータ２４の出力により、１回の制御周期の間に実際に変化できる速度や移動方向は制限される。このため、１回の制御周期で指令ベクトルを過剰に変化させても、車両２０が指令ベクトルに追従することができずに、車両２０を所望の通りに制御できないおそれがある。また、指令ベクトルが急激に変化した場合、車輪２２の滑りや車両２０の振動の原因となるおそれがある。本実施形態のように、目標指令ベクトルが変更可能範囲Ａ外の場合には、変更可能範囲Ａ内の制限指令ベクトルを導出し、当該制限指令ベクトルを指令ベクトルとすることで、指令ベクトルを緩やかに変化させることができる。

【0047】

本実施形態の効果について説明する。
（１）制御装置３２は、目標指令ベクトルが変更可能範囲Ａ内の場合には目標指令ベクトルを指令ベクトルとし、目標指令ベクトルが変更可能範囲Ａ外の場合には制限指令ベクトルを指令ベクトルとしている。このため、１回の制御周期で指令ベクトルが変更可能範囲Ａを超えて変更されることが抑制される。１回の制御周期で指令ベクトルが過剰に変更されることが抑制される。従って、車両２０を円滑に移動させることができる。

【0048】

（２）制御装置３２は、最短変更優先ベクトルＶＶ２、方向変更優先ベクトルＶＶ３、及び減速優先ベクトルＶＶ４を導出している。制御装置３２は、最短変更優先ベクトルＶＶ２、方向変更優先ベクトルＶＶ３、及び減速優先ベクトルＶＶ４に基づき制限指令ベクトルを導出する。制御装置３２は、状況に合わせた制限指令ベクトルを導出することができる。例えば、指令ベクトルを目標指令ベクトルにすることを優先したい場合、最短変更優先ベクトルＶＶ２そのものや、最短変更優先ベクトルＶＶ２を用いて合成したベクトルを制限指令ベクトルとする等、優先したい事項に合わせた制限指令ベクトルを導出可能である。

【0049】

実施形態は、以下のように変更して実施することができる。本実施形態及び以下の変形例は、技術的に矛盾しない範囲で互いに組み合わせて実施することができる。
○制御装置３２は、制限指令ベクトルを導出する際に最短変更優先ベクトルＶＶ２のみを導出してもよい。この場合、最短変更優先ベクトルＶＶ２が制限指令ベクトルとなる。制御装置３２は、制限指令ベクトルを導出する際に方向変更優先ベクトルＶＶ３のみを導出してもよい。この場合、方向変更優先ベクトルＶＶ３が制限指令ベクトルとなる。制御装置３２は、最短変更優先ベクトルＶＶ２、方向変更優先ベクトルＶＶ３及び減速優先ベクトルＶＶ４のうち２つを導出するようにしてもよい。この場合、導出される２つのベクトルのいずれか、あるいは、２つのベクトルを合成することにより得られるベクトルが制限指令ベクトルとなる。

【0050】

○制御装置３２は、制限指令ベクトルを導出する際に、最短変更優先ベクトルＶＶ２、方向変更優先ベクトルＶＶ３及び減速優先ベクトルＶＶ４を導出しなくてもよい。制限指令ベクトルは、変更可能範囲Ａ内のベクトルであり、前回の制御周期での指令ベクトルから目標指令ベクトルに近付くように変化しているベクトルであればよい。

【0051】

○変更可能範囲Ａの大きさは適宜変更してもよい。また、変更可能範囲Ａは、円形の範囲に限られず、多角形の範囲や楕円形の範囲であってもよい。
○目標指令ベクトルが変更可能範囲Ａ内の場合であっても、制限指令ベクトルを導出してもよい。制御装置３２は、目標指令ベクトルが変更可能範囲Ａ内の場合には目標指令ベクトルを指令ベクトル、目標指令ベクトルが変更可能範囲Ａ外の場合には制限指令ベクトルを指令ベクトルとできればよく、目標指令ベクトルを指令ベクトルとする場合であっても、制限指令ベクトルを導出してもよい。

【0052】

○センサ３１として用いられるLIDARとして、水平方向及び鉛直方向の両方の照射角度を変更しながらレーザーを照射する三次元距離計を用いてもよい。
○センサ３１として、ステレオカメラを用いてもよい。ステレオカメラは、複数のカメラによって周辺環境を撮像することで得られた視差画像から周辺環境を制御装置３２に認識させる。視差画像は、同一の特徴点について複数のカメラによって撮像を行った場合に、カメラ間で生じる画素差を示すものである。特徴点は、物体のエッジなど視差が得られる部分、即ち、撮像された画像の各画素において輝度が変化する画素である。制御装置３２は、ステレオカメラの眼間距離、焦点距離、視差画像などを用いてセンサ３１から特徴点までの距離及び方位を求めることができる。なお、センサ３１としては、ミリ波レーダーを用いることもできる。

【0053】

○センサ３１は、LIDARとステレオカメラなど、複数のセンサを組み合わせたものであってもよい。即ち、センサ３１は、単数のセンサによって構成されていてもよいし、複数のセンサによって構成されていてもよい。

【0054】

○自律移動体１０は、経路を追従するものであってもよい。この場合、制御装置３２は、経路を生成し、経路に沿って車両２０が移動するように制御を行う。目標位置は、経路上の位置となる。自律移動体１０が経路を追従するものであっても、障害物が存在する場合など、車両２０の周辺環境によっては目標位置が大きく変化し得る。この場合、実施形態と同様の制御を行うことで、車両２０を円滑に移動させることができる。また、自律移動体１０としては、追尾対象Ｔを追尾するモードと、経路を追従するモードとを切り替え可能なものであってもよい。

【0055】

○追尾対象Ｔの検出は、追尾対象Ｔにセンサ３１により検出できる目印などを設けることで行われてもよい。また、追尾対象Ｔの検出は、センサ３１とは別の検出部によって行われてもよい。

【0056】

○駆動機構２３としては、速度指令値に追従するように車両２０の速度を制御することができれば、どのような構成のものでもよい。
○車輪として、全方向移動車輪以外の車輪、即ち、車輪の回転軸線方向への移動を許容しない車輪を用いたものであってもよい。例えば、車輪として全方向移動車輪以外の車輪を用いる場合、車輪を２つにして二輪速度差制御を行ってもよい。

【0057】

○移動体は、モータの駆動により移動するものであればよく、多足歩行ロボット等でもよい。
○走行制御部、目標指令導出部、判定部、制限指令導出部、指令ベクトル決定部は、それぞれ別々の制御装置であってもよい。

【0058】

○制御装置３２は、点群だけでなく、１つの距離情報を物体と認識してもよい。

【符号の説明】

【0059】

Ａ…変更可能範囲、１０…自律移動体、２０…移動体としての車両、２４…モータ、３２…制御装置。

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】