JP6894595B2

JP6894595B2 - 移動体

Info

Publication number: JP6894595B2
Application number: JP2018061776A
Authority: JP
Inventors: 和宏久野
Original assignee: Equos Research Co Ltd
Current assignee: Equos Research Co Ltd
Priority date: 2018-03-28
Filing date: 2018-03-28
Publication date: 2021-06-30
Anticipated expiration: 2038-03-28
Also published as: CN111164533A; US20200241548A1; EP3779632A4; JP2019175074A; WO2019188866A1; EP3779632A1

Description

本発明は、移動体に関し、特に、対象者が旋回動作した場合の大回りな随行動作を抑制できる移動体に関するものである。

特許文献１には、移動する追従対象２に対して、斜め前方かつ相対的な所定位置を維持して追従動作を行う移動ロボット１が開示されている。移動ロボット１は、追従対象２の向きをカメラ等によって検出し、その検出された追従対象２の向きに応じて当該移動ロボット１の目標位置を決定する。移動ロボット１は、決定された目標位置に対する移動経路を算出し、移動する。

特開２００８−２３４４０４号公報

しかしながら、移動ロボット１が追従対象２の前方において追従動作する場合、追従対象２が旋回動作をすると、移動ロボット１は追従対象２の挙動に比べて大回りに追従してしまう。特に、移動ロボット１が追従対象２の真正面ではなく、左右どちらかにシフトした位置で前方追従する場合には、大回りの度合いは一層大きくなる。例えば、特許文献１のように、移動ロボット１が追従対象２の右前方において追従動作する場合、追従対象２が左旋回すると、移動ロボット１の左旋回は非常に大回りになってしまう。かかる課題は、移動ロボット１の目標位置が追従対象２から離れているほど顕著なものとなる。

本発明は、上述した問題点を解決するためになされたものであり、対象者が旋回動作した場合の大回りな随行動作を抑制できる移動体を提供することを目的としている。

この目的を達成するために本発明の移動体は、移動手段を備え、その移動手段により対象者に随行して移動するものであり、前記対象者を測距する測距手段と、その測距手段により測距された複数の測距データに基づいて前記対象者の位置と向きとを算出する位置向き算出手段と、その位置向き算出手段による算出結果に基づいて前記対象者に対する当該移動体の移動目標を算出する移動目標算出手段と、当該移動体の向きと前記位置向き算出手段により算出された前記対象者の向きとがなす角度偏差に基づいて、前記移動目標を前記対象者側へシフトする移動目標シフト手段と、その移動目標シフト手段によりシフトされた移動目標へ当該移動体が移動するように前記移動手段を制御する移動制御手段とを備えている。

なお、操舵角算出手段が操舵角の算出に用いる目標経路上の中間点としては、必ずしも当該移動体の位置と移動目標との中点である必要はなく、その中点近傍の目標経路上の点でも良いし、或いはその中点近傍の目標経路に近似する点であっても良い。

本発明の移動体によれば、対象者を測距した複数の測距データに基づいて対象者の位置と向きとが算出され、その算出結果に基づいて対象者に対する当該移動体の移動目標が算出される。また、当該移動体の向きと前記対象者の向きとがなす角度偏差に基づいて、前記移動目標が対象者側へシフトされる。この対象者側へシフトされた移動目標へ当該移動体が移動するように移動手段が制御されるので、対象者が旋回動作した場合の大回りな随行動作を抑制することができる。

（ａ）は、移動体の側面図であり、（ｂ）は、移動体の上面図である。移動体の移動目標を示す図である。（ａ）は移動体の制御目標を示す図であり、（ｂ）は、ユーザがその場で左回転した場合の制御目標とシフト制御目標との位置をそれぞれ示す図である。移動体の電気的構成を示すブロック図である。移動体のフローチャートである。（ａ）は移動体の制御目標を示す図であり、（ｂ）は、角度偏差に応じたシフト制御目標のｘｕ軸方向の位置を示すグラフである。（ａ）は移動体の操舵指令値の算出を説明するための図であり、（ｂ）は、横偏差に応じた操舵指令値の補正量を示すグラフである。ユーザがその場で左回転した場合の制御目標とシフト制御目標とによる移動軌跡をそれぞれ示す図である。

以下、本発明の好ましい実施形態について、添付図面を参照して説明する。まず、図１を参照して、本実施形態における移動体１の構成を説明する。図１（ａ）は、移動体１の側面図であり、図１（ｂ）は、移動体１の上面図である。移動体１は、ユーザＨ（対象者）の右前方にて、ユーザＨに対し適切な位置に移動して、ユーザＨに随行できる装置として機能する。

図１に示す通り、移動体１は、主に略円柱状の外装２と、その外装２の内部に配設され移動体１の各部を制御する制御部１０と、測距センサ１６と、車輪１７とを有する。測距センサ１６は、外装２の上部に配置され、レーザ光を全方位（３６０度）に対して照射することで、測距センサ１６と物体との距離を検知（測距）する装置である。測距センサ１６は、角度０．２５度毎に検出された対象物との距離を、その角度に対応付けて制御部１０へ送信する。また、測距センサ１６は上下方向に移動可能に構成され、予め測距センサ１６からのレーザ光がユーザＨの肩周辺に照射されるよう、測距センサ１６の上下方向の位置が適宜設定される。以下、測距センサ１６から検知される距離および角度のことを「測距データ」と称す。

車輪１７は、外装２の下部における左右一対に対向して設けられる車輪である。図１（ｂ）に示すように、左右の車輪１７間の幅はｄとされる。左右の車輪１７それぞれにはモータ（図示せず）が接続され、後述する駆動部１８（図４参照）からの制御信号に基づいてモータを駆動させることで、移動体１が移動される。

左右のモータを、同じ出力で正転および逆転させることで移動体１の前方移動および後方移動を行い、また、モータを差動させることで、移動体１の移動方向の変更を行う。ここで移動体１は、車輪１７が設けられる左右方向へは直接移動できないので、かかる左右方向への移動には移動方向の変更が必要となる。即ち、移動体１は、非ホロノミックな拘束条件を有する車輪１７及び駆動部１８（移動部）によって移動するものである。

次に、図２，図３を参照して、移動体１の移動目標Ｔ１および制御目標Ｔ２について説明する。図２は移動体１の移動目標Ｔ１を示す図であり、図３（ａ）は移動体１の制御目標Ｔ２を示す図であり、図３（ｂ）は、ユーザＨがその場で左回転した場合の制御目標Ｔ２とシフト制御目標Ｔ２’との位置をそれぞれ示す図である。

図２に示す通り、移動体１は、ユーザＨの右前方に設定される移動目標Ｔ１を随行しながら移動する。本実施形態では、移動体１の移動目標Ｔ１は、ユーザＨの前側「０．６ｍ」かつ右側「０．３ｍ」の位置とされる。また、移動体１は測距センサ１６から測距される測距データＭＰを、移動体１の位置Ｐｒを原点（０，０）、移動体１の向きＤｒを９０度とし、移動体１における水平方向をｘｒ軸，そのｘｒ軸に直交する垂直方向をｙｒ軸とした「移動体座標系」と、ユーザＨの位置Ｐｕを原点（０，０）、ユーザＨの向きＤｕを９０度とし、ユーザＨの水平方向をｘｕ軸，そのｘｕ軸に直交する垂直方向をｙｕ軸とした「ユーザ座標系」とに基づいて処理することで、移動制御が行われる。

ところで、ユーザＨが旋回した場合、移動体１も合わせて旋回する必要がある。ここで、移動体１と移動目標Ｔ１との距離は比較的小さいので、移動体１の旋回半径も小さくなる。ここで、移動体１は、左右の車輪１７を差動させることで旋回するので、小さな旋回半径による急旋回をしようとしても十分に旋回できず、ユーザＨへの随行が遅れてしまう虞がある。

そこで、図３（ａ）に示すように、移動目標Ｔ１をｙｕ軸方向へＹｓだけ移動（シフト）した制御目標Ｔ２が設定され、移動体１は該制御目標Ｔ２を目標として移動制御される。本実施形態では、移動目標Ｔ１のシフト量ＹｓはユーザＨの速度Ｖｕに基づいて算出され、具体的には、数式１に基づいて算出される。

数式１において、ΔＴは所定の時間間隔であり「１秒間」が例示される。即ち、制御目標Ｔ２は、移動目標Ｔ１にユーザＨが１秒間進む距離が加算された位置である。該制御目標Ｔ２を移動目標として移動体１が移動することで、移動体１と移動目標との距離を確保することができる。従って、移動体１が移動目標Ｔ１を移動目標とする場合よりも、移動体１の旋回半径を大きくすることができる。よって、ユーザＨが急激に旋回したとしても、移動体１は大きな旋回半径に沿って緩やか旋回すれば良いので、ユーザＨへの随行遅れを抑制することができる。

ここで、ユーザＨが回転または旋回した場合の移動体１の移動動作を説明する。まず、ユーザＨが右回転または右旋回した場合は、ユーザＨが移動体１の位置する方向へ回転または旋回するので、かかる回転または旋回に伴う制御目標Ｔ２の変化は小さくて済む。従って、制御目標Ｔ２に従って移動する移動体１の軌跡（以下「移動軌跡」と略す）は、全体的に小回りとなる。

一方で、ユーザＨが左回転または左旋回した場合、ユーザＨは右前方に位置する移動体１とは逆方向へ回転または旋回する。即ちユーザＨは、移動体１から離れる方向へ回転または旋回するので、かかる回転または旋回に伴う制御目標Ｔ２の変化は、ユーザＨが右回転または右旋回した場合と比較しても大きくなる。これによって、移動軌跡は全体的に大回りとなってしまう。

また、移動体１はユーザＨの右前方を随行しているので、ユーザＨの右前方に位置する移動体１において、ユーザＨが同一の角度で左旋回または右旋回した場合、旋回後のユーザＨの向きＤｕと移動体１の向きＤｒとの角度偏差は、ユーザＨが左旋回をした場合の方が小さくなる。詳細は図７で後述するが、移動体１を操舵するための操舵指令値ωは、移動体１の向きＤｒと制御目標Ｔ２に基づく位置との角度偏差に応じた値であり、上述した通り、制御目標Ｔ２は、移動目標Ｔ１に基づいて算出され、更に移動目標Ｔ１はユーザＨの向きＤｕに基づいて算出されるので、移動体１の向きＤｒとユーザＨの向きＤｕとの角度偏差が小さいほど、操舵指令値ωが小さくなる。従って、ユーザＨが右旋回した場合よりも、左旋回した場合の方が操舵指令値ωが小さくなるので、その分、移動体１は大回りな移動となってしまう。

そこで本実施形態では、移動体１の向きＤｒによる角度Δθに基づいて、制御目標Ｔ２をｘｕ軸方向におけるユーザＨ側にＸｓだけ移動（シフト）した、シフト制御目標Ｔ２’が算出され、かかるシフト制御目標Ｔ２’を目標として、移動体１が移動制御される。ここで、図３（ｂ）を参照して、ユーザＨがその場で左回転した場合の、制御目標Ｔ２とシフト制御目標Ｔ２’との位置の比較を行う。

図３（ｂ）は、ユーザＨがその場で左回転した場合の制御目標Ｔ２とシフト制御目標Ｔ２’との位置をそれぞれ示す図である。図３（ｂ）において、ユーザＨは向きＤａから、向きＤｂ→向きＤｃ→向きＤｄ・・・と、その場で左回転を行い、ユーザＨは向きＤｄ以降も左回転を続けている。また、図３（ｂ）は説明のため、ユーザＨがその場で左回転した場合でも、シフト量Ｙｓを０ではない一定値に設定したものを図示している。

かかるユーザＨの回転によって、ユーザの向きＤａに対応する制御目標Ｔ２ａが算出され、制御目標Ｔ２ａからシフト制御目標Ｔ２ａ’が算出される。なお、図３（ｂ）において、ユーザＨの向きＤａと、移動体１の向きＤｒは一致しているので、制御目標Ｔ２ａとシフト制御目標Ｔ２ａ’とは同一の位置とされる。

順次、ユーザＨの向きＤｂから制御目標Ｔ２ｂが算出され、ユーザＨの向きＤｂと移動体１の向きＤｒとに基づいてシフト量Ｘｓｂが算出され、この制御目標Ｔ２ｂとシフト量Ｘｓｂとからシフト制御目標Ｔ２ｂ’が算出される。同様に、ユーザＨの向きＤｃおよび移動体１の向きＤｒから制御目標Ｔ２ｃ、シフト量Ｘｓｃ及びシフト制御目標Ｔ２ｃ’が算出され、ユーザＨの向きＤｄおよび移動体１の向きＤｒから制御目標Ｔ２ｄ、シフト量Ｘｓｄ及びシフト制御目標Ｔ２ｄ’が算出される。

シフト制御目標Ｔ２ｂ’〜Ｔ２ｄ’は、制御目標Ｔ２ｂ〜Ｔ２ｄよりもｘｕ軸方向に、即ち、ユーザＨの正面側にシフトされるので、ユーザＨの位置Ｐｕとシフト制御目標Ｔ２ｂ’〜Ｔ２ｄ’との距離は、位置Ｐｕと制御目標Ｔ２ｂ〜Ｔ２ｄとの距離よりも小さくなる。従って、シフト制御目標Ｔ２ｂ’〜Ｔ２ｄ’による分布は、制御目標Ｔ２ｂ〜Ｔ２ｄによる分布よりもユーザＨ側となる。これにより、シフト制御目標Ｔ２ｂ’〜Ｔ２ｄ’に基づく移動軌跡は、制御目標Ｔ２ｂ〜Ｔ２ｄに基づく移動軌跡よりも即ち小回りとできるので、ユーザＨが左回転した場合でも、移動体１が大回りすることを抑制できる。

次に、図４を参照して、移動体１の電気的構成について説明する。図４は、移動体１の電気的構成を示すブロック図である。移動体１は制御部１０を有し、その制御部１０はＣＰＵ１１と、フラッシュＲＯＭ１２と、ＲＡＭ１３とを有し、これらはバスライン１４を介して入出力ポート１５にそれぞれ接続されている。入出力ポート１５には、更に、測距センサ１６と、駆動部１８とが接続されている。

ＣＰＵ１１は、バスライン１４により接続された各部を制御する演算装置である。フラッシュＲＯＭ１２は、ＣＰＵ１１により実行されるプログラムや固定値データ等を格納した書き換え可能な不揮発性の記憶装置であり、制御プログラム１２ａが記憶される。ＣＰＵ１１によって制御プログラム１２ａが実行されると、図５のメイン処理が実行される。

ＲＡＭ１３は、ＣＰＵ１１が制御プログラム１２ａの実行時に各種のワークデータやフラグ等を書き換え可能に記憶するためのメモリであり、測距センサ１６から測距された測距データＭＰが記憶される測距データメモリ１３ａと、ユーザＨの位置Ｐｕが記憶されるユーザ位置メモリ１３ｂと、ユーザＨの速度Ｖｕが記憶されるユーザ速度メモリ１３ｃと、ユーザＨの向きＤｕが記憶されるユーザ向きメモリ１３ｄと、移動体１の位置Ｐｒが記憶される移動体位置メモリ１３ｅと、移動体１の向きＤｒが記憶される移動体向きメモリ１３ｆと、移動体１の移動目標Ｔ１が記憶される目標位置メモリ１３ｇと、移動体１の位置Ｐｒと移動目標Ｔ１とのｘｕ軸方向の偏差である横偏差ΔＸｔ（図７（ａ）参照）が記憶される横偏差メモリ１３ｈと、シフト制御目標Ｔ２’が記憶される制御目標位置メモリ１３ｉと、中点角度メモリ１３ｊと、中点角度前回値メモリ１３ｋと、移動体１を操舵するための操舵指令値ωが記憶される操舵指令値メモリ１３ｍとがそれぞれ設けられる。

中点角度メモリ１３ｊは、移動体１の位置Ｐｒとシフト制御目標Ｔ２’とを結ぶ移動体１の目標経路Ｒ（図７（ａ）参照）における中点Ｔｃと移動体１の位置Ｐｒとを結ぶ線分と、移動体１の向きＤｒとのなす角の角度Δθ_ｔが記憶されるメモリであり、中点角度前回値メモリ１３ｋは、その角度Δθ_ｔの前回値Δθ_ｔ０が記憶されるメモリである。

本実施形態において、ユーザ位置メモリ１３ｂ，ユーザ向きメモリ１３ｄは上述した移動体座標系に基づく値とされ、移動体位置メモリ１３ｅ、移動体向きメモリ１３ｆ、目標位置メモリ１３ｇ、制御目標位置メモリ１３ｉ、中点角度メモリ１３ｊ及び中点角度前回値メモリ１３ｋはユーザ座標系に基づく値とされる。

駆動部１８は、移動体１を移動動作させるための装置であり、車輪１７（図１参照）および車輪１７の駆動源となるモータ（図示せず）等から構成される。制御部１０から制御信号が駆動部１８に入力されると、入力された制御信号に基づいてモータが回転し、該モータの回転が動力となって車輪１７が駆動し、移動体１を動作させる。

次に、図５〜図７を参照して、移動体１のＣＰＵ１１で実行されるメイン処理を説明する。図５は、移動体１のメイン処理のフローチャートである。メイン処理は移動体１の電源投入直後に実行される。メイン処理はまず、測距センサ１６から取得した測距データＭＰを測距データメモリ１３ａに保存する（Ｓ１）。

Ｓ１の処理の後、測距データメモリ１３ａの測距データＭＰに基づいて、ユーザＨの位置Ｐｕ、ユーザＨの速度Ｖｕ及びユーザＨの向きＤｕを移動体座標系に基づいて算出し、それぞれユーザ位置メモリ１３ｂ、ユーザ速度メモリ１３ｃ及びユーザ向きメモリ１３ｄに保存する（Ｓ２）。測距データＭＰは移動体１を基準とした値であるので、ユーザＨの位置Ｐｕ、ユーザＨの速度Ｖｕ及びユーザＨの向きＤｕは、上述した移動体座標系に基づいて算出される。

Ｓ２の処理の後、ユーザ位置メモリ１３ｂ及びユーザ向きメモリ１３ｄに記憶される、移動体座標系によるユーザＨの位置Ｐｕ及び向きＤｕを座標変換することで、ユーザ座標系における移動体１の位置Ｐｒ及び移動体１の向きＤｒとを算出し、それぞれ移動体位置メモリ１３ｅ及び移動体向きメモリ１３ｆに保存する（Ｓ３）。

Ｓ３の処理の後、ユーザ座標系における移動目標Ｔ１を算出し、目標位置メモリ１３ｇに保存する（Ｓ４）。Ｓ４の処理の後、移動体位置メモリ１３ｅの移動体の位置Ｐｒと目標位置メモリ１３ｇの移動目標Ｔ１との、ｘｕ軸方向の偏差である横偏差ΔＸｔを算出し、横偏差メモリ１３ｈへ保存する（Ｓ５）。

Ｓ５の処理の後、目標位置メモリ１３ｇに記憶される移動目標Ｔ１と、ユーザ速度メモリ１３ｃに記憶されるユーザＨの速度Ｖｕとから、制御目標Ｔ２を算出する（Ｓ６）。具体的には、まず、上述した数式１によりユーザＨの速度Ｖｕからシフト量Ｙｓ（図３参照）が算出され、移動目標Ｔ１に対して、該シフト量Ｙｓを移動目標Ｔ１のｙｕ軸方向に加算した位置が、制御目標Ｔ２とされる。

Ｓ６の処理の後、移動体向きメモリ１３ｆの移動体１の向きＤｒに基づいて制御目標Ｔ２をシフトすることで、シフト制御目標Ｔ２’を算出し、制御目標位置メモリ１３ｉへ保存する（Ｓ７）。かかるＳ７の処理による、シフト制御目標Ｔ２’の算出について図６を参照して説明する。

図６（ａ）は移動体１の制御目標を示す図であり、図６（ｂ）は、角度偏差Δθに応じたシフト制御目標Ｔ２’のｘｕ軸方向の位置Ｘ_Ｔ２’を示すグラフである。図６（ａ）に示すように、制御目標Ｔ２を、移動体１の向きＤｒとｙｕ軸とのなす角である角度偏差Δθに基づいてシフトすることで、シフト制御目標Ｔ２’が算出される。シフト制御目標Ｔ２’のｘｕ軸方向における位置Ｘ_Ｔ２’は、制御目標Ｔ２のｘｕ軸方向の位置Ｘ_Ｔ２と角度偏差Δθとによって、以下の数式２で決定される。

なお、数式２におけるα_１，β_１，θ_ｍａｘは係数であり、実験によって予め算出される値である。かかる数式２で算出された位置Ｘ_Ｔ２’が、制御目標位置メモリ１３ｉに記憶される。

ここで、図６（ｂ）を参照して、角度偏差Δθと位置Ｘ_Ｔ２’との関係について説明する。図６（ｂ）に示すように、シフト制御目標Ｔ２’のｘｕ軸方向における位置Ｘ_Ｔ２’は、シグモイド曲線に従って、角度偏差Δθの増加に伴いなだらかに減少する。位置Ｘ_Ｔ２’と制御目標Ｔ２のｘｕ軸方向における位置Ｘ_Ｔ２との偏差が、図６（ａ）におけるシフト量Ｘｓに該当する。これにより、シフト制御目標Ｔ２’が制御目標Ｔ２よりもユーザＨ側となるので、ユーザＨがその場で左回転したり左旋回した場合も、移動体１が大回りに移動することを抑制できる。

また、シフト量Ｘｓが、シグモイド曲線に従って角度偏差Δθの増加に伴い、なだらかに変化するので、角度偏差Δθが小さい場合は、即ち、ユーザＨの向きＤｕが僅かに変化した場合は、過敏にシフト量Ｘｓが増加しない。これによって、ユーザＨの向きＤｕの僅かな変化によって、移動体１は急激に方向転換することはないので、移動体１の挙動を安定させることができ、更に、ユーザＨにとって、より自然な移動体１の移動動作を実現できる。

図５に戻る。Ｓ７の処理によって、制御目標Ｔ２は、ユーザＨ側のシフト制御目標Ｔ２’へシフトされるので、ユーザＨが左回りや左旋回した場合に移動体１が大回りに移動されるのを抑制できる。本実施形態では、更に移動体１の大回りな移動を抑制するため、かかるシフト制御目標Ｔ２’へシフトに加え、移動体１を操舵するための操舵指令値ωを、移動体１の位置Ｐｒと移動目標Ｔ１との横偏差ΔＸｔに応じて補正する。かかる操舵指令値ωの補正について、図５と図７とを参照して説明する。

まず図５において、Ｓ７の処理の後、移動体位置メモリ１３ｅの移動体１の位置Ｐｒと、制御目標位置メモリ１３ｉのシフト制御目標Ｔ２’とから、位置Ｐｒからシフト制御目標Ｔ２’への目標経路Ｒを算出し（Ｓ８）、目標経路Ｒと、移動体向きメモリ１３ｆの移動体１の向きＤｒとのなす角度θｔを算出し、中点角度メモリ１３ｊへ保存する（Ｓ９）。Ｓ９の処理の後、中点角度メモリ１３ｊの角度θｔと、中点角度前回値メモリ１３ｋの角度θｔの前回値Δθ_ｔ０とから、操舵指令値ωを算出する（Ｓ１０）。かかる操舵指令値ωを、横偏差メモリ１３ｈの横偏差ΔＸｔに基づいて補正し、補正された操舵指令値ωを操舵指令値メモリ１３ｍに保存する（Ｓ１１）。かかるＳ８〜Ｓ１１の処理による操舵指令値ωの算出および補正について、図７を参照して説明する。

図７（ａ）において、移動体１の位置Ｐｒから、図５のＳ７の処理で算出されたシフト制御目標Ｔ２’へ移動体１が移動するための目標経路Ｒが算出される（図５のＳ８）。本実施形態では、目標経路Ｒは、位置Ｐｒとシフト制御目標Ｔ２’とを結ぶＳ字状の曲線とされ、目標経路Ｒの曲線形状は、移動体１が安定して位置Ｐｒからシフト制御目標Ｔ２’へ移動できる程度とされる。操舵指令値の算出として、まず、目標経路Ｒに沿って移動体１を移動させる目標地点として、目標経路Ｒの中点Ｔｃが選択され、かかる中点Ｔｃに向けて移動体１を移動するための操舵指令値ω１が算出される。具体的には、操舵指令値ω１は、中点Ｔｃと移動体１の向きＤｒとのなす角である角度Δθ_ｔと、Δθ_ｔの前回値Δθ_ｔ０と、左右の車輪１７の幅ｄとから、数式３にて算出される。

ここで、Ｋ_ｐ１，Ｋ_ｐ２，Ｋ_ｄは係数であり、実験によって予め算出される値である。かかる操舵指令値ω１によって、移動体１はシフト制御目標Ｔ２’へ向かって移動できるが、本実施形態では、操舵指令値ω１へ更に、横偏差ΔＸｔに基づく横偏差項ｆ（ΔＸｔ）で補正した操舵指令値ωに基づいて、移動体１を操舵する。具体的には、横偏差項ｆ（ΔＸｔ）は数式４で算出され、横偏差項ｆ（ΔＸｔ）及び操舵指令値ω１による、操舵指令値ωは数式５で算出される。

ここで、α_２，β_２，ω_ｍａｘは係数であり、実験によって予め算出される値である。図５のＳ１１の処理では、数式５による操舵指令値ωが操舵指令値メモリ１３ｍに記憶される。

図７（ｂ）に示すように、横偏差項ｆ（ΔＸｔ）はシグモイド曲線に従って、横偏差ΔＸｔの増加に伴ってなだらかに増加する。具体的には、横偏差ΔＸｔに応じて、横偏差項ｆ（ΔＸｔ）は大きな値が設定される。横偏差ΔＸｔの絶対値が大きくなるほど、移動体１と移動目標Ｔ１との距離が離れるほどに、横偏差項ｆ（ΔＸｔ）が大きくなり、横偏差ΔＸｔの絶対値が小さいほど、横偏差項ｆ（ΔＸｔ）は小さくなる。

横偏差ΔＸｔの絶対値が大きい場合は、移動体１と移動目標Ｔ１とのｘｕ軸方向の偏差が大きい場合であり、移動体１と、移動目標Ｔ１や移動目標Ｔ１に基づいて算出されるシフト制御目標Ｔ２’とが離れている場合である。この場合、横偏差項ｆ（ΔＸｔ）は大きな値が設定されることで、操舵指令値ωが大きくなる。これによって、移動体１とシフト制御目標Ｔ２’とが離れている場合は、大きな操舵指令値ωによって、移動体１をより早くシフト制御目標Ｔ２’側へ移動できるので、移動体１は、より小回りに移動できる。

一方で、横偏差ΔＸｔの絶対値が小さい場合は、移動体１とシフト制御目標Ｔ２’との偏差が小さい場合であるので、ユーザＨ側に向けて操舵する必要はない。この場合に、横偏差項ｆ（ΔＸｔ）の値が小さく設定されることで、移動体１がユーザＨ側へ向けて操舵されることはないので、移動体１の操舵に関する不安定な動作が抑制され、移動体１の挙動を安定させることができる。

図５に戻る。Ｓ１１の処理の後、操舵指令値メモリ１３ｍの操舵指令値ωに基づいて、駆動部１８を動作させ、移動体１を移動させる（Ｓ１３）。これにより、制御目標Ｔ２より小回りとなったシフト制御目標Ｔ２’に対して、更にシフト制御目標Ｔ２’側へ大きく操舵されるので、移動体１の移動軌跡をより一層小回りとできる。かかるここで、ユーザＨがその場で左回転した場合の、シフト制御目標Ｔ２’および操舵指令値ωによる移動体１の移動軌跡について、図８を参照して説明する。

図８は、ユーザＨが左回転した場合の制御目標Ｔ２とシフト制御目標Ｔ２’との移動軌跡をそれぞれ示す図である。図８も説明のため、ユーザＨがその場で左回転した場合でも、シフト量Ｙｓを０ではない一定値に設定したものを図示している。図８においても図３（ｂ）と同様に、ユーザＨは向きＤａから向きＤｂ→向きＤｃ→向きＤｄ・・・と、その場で左回転を行い、ユーザＨは向きＤｄ以降も左回転を続ける。ユーザの向きＤａから制御目標Ｔ２ａ及びシフト制御目標Ｔ２ａ'が算出され、同様に、ユーザの向きＤｂ〜Ｄｄから制御目標Ｔ２ａ〜Ｔ２ｄ及びシフト制御目標Ｔ２ａ’〜Ｔ２ｄ’が算出される。

図８に示すように、従来の制御目標Ｔ２ａ〜Ｔ２ｄによる移動体１の移動は、ユーザＨが右回りに回転した場合よりも大回りな制御目標Ｔ２ａ〜Ｔ２ｄに沿うように操舵される。従って、移動体１の制御目標Ｔ２ａ〜Ｔ２ｄへの移動による移動軌跡Ｑｂも、大回りな軌跡となる。

これに対して、本実施形態の移動体１は、制御目標Ｔ２ａ〜Ｔ２ｄよりも、小回りなシフト制御目標Ｔ２ａ’〜Ｔ２ｄ’が設定され、更に、移動体１の位置Ｐｒと移動目標Ｔ１との横偏差ΔＸｔによる横偏差項ｆ（ΔＸｔ）が加えられた操舵指令値ωに基づいて移動される。これにより、移動体１は小回りなシフト制御目標Ｔ２ａ’〜Ｔ２ｄ’よりも、ユーザＨ側に沿って移動されるので、かかる移動による移動軌跡Ｑは、シフト制御目標Ｔ２ａ’〜Ｔ２ｄ’よりも更に小回りとできる。

従って、本実施形態の移動体１は、ユーザＨが左回転した場合や左旋回した場合でも、シフト制御目標Ｔ２ａ’〜Ｔ２ｄ’と、横偏差項ｆ（ΔＸｔ）とによる操舵指令値ωに基づいて移動されるので、移動体１が大回りとなるのを抑制できる。これにより、移動体１は、ユーザＨが左回転した場合や左旋回した場合でも、随行遅れをすることなく、適切にユーザＨへ随行移動できる。

以上、実施形態に基づき本発明を説明したが、本発明は上述した実施形態に何ら限定されるものではなく、本発明の趣旨を逸脱しない範囲内で種々の改良変更が可能であることは容易に推察できるものである。

上記実施形態では、目標経路Ｒの中点Ｔｃに基づいて操舵指令値ω１を算出する構成とした。しかし、必ずしもこれに限られるものではなく、目標経路Ｒ上の中点Ｔｃに近傍する位置や、中点Ｔｃ近傍の目標経路Ｒに近似する位置等の目標経路Ｒの中間点に基づいて、操舵指令値ω１を算出する構成としても良い。また、目標経路Ｒの変曲点や鞍点、極大点や極小点等の目標経路Ｒの特徴を表す位置や、それら位置の近傍の位置に基づいて操舵指令値ω１を算出する構成としても良い。また、その時点での移動体１の速度Ｖｒや角速度と、シフト制御目標Ｔ２’に到達した時点において目標とされる移動体１の速度や角速度とに応じて、操舵指令値ω１を算出する構成としても良い。

上記実施形態では、ユーザＨの位置Ｐｕ、速度Ｖｕ及び向きＤｕを測距センサ１６の測距データに基づいて算出した。しかし、必ずしもこれに限られるものではなく、測距センサ１６の代わりにカメラを搭載し、該カメラで取得された画像に基づいて、ユーザＨの位置Ｐｕ、速度Ｖｕ及び向きＤｕを算出する構成としても良い。

上記実施形態では、図６（ｂ），図７（ｂ）において、位置Ｘ_Ｔ２’や横偏差項ｆ（ΔＸｔ）をシグモイド曲線に従って変化させる構成とした。しかし、必ずしもこれに限られるものではなく、位置Ｘ_Ｔ２’や横偏差項ｆ（ΔＸｔ）をガウス曲線等、他の形状の曲線に基づいて変化させる構成としても良い。

上記実施形態では、横偏差ΔＸｔを移動体の位置Ｐｒと移動目標Ｔ１との、ｘｕ軸方向の偏差とした。しかし、必ずしもこれに限られるものではなく、横偏差ΔＸｔを移動体の位置Ｐｒとシフト制御目標Ｔ２’との、ｘｕ軸方向の偏差としても良い。

上記実施形態では、移動体１は、ユーザＨの右前方を随行しながら移動する構成とした。しかし、必ずしもこれに限られるものではなく、移動体１をユーザＨの左前方を随行しながら移動する構成としても良い。

上記実施形態に挙げた数値は一例であり、他の数値を採用することは当然可能である。

１移動体
１６測距センサ（測距手段）
１７車輪（移動手段および移動部の一部）
１８駆動部（移動手段および移動部の一部）
Ｈユーザ（対象者）
Ｒ目標経路
Ｔ１移動目標（移動目標、第１移動目標）
Δθ 角度偏差
Ｔ２制御目標（移動目標、第２移動目標）
Ｔ２’ シフト制御目標（移動目標）
Ｔｃ中点（中間点）
ΔＸｔ横偏差
Ｓ２位置向き算出手段、速度算出手段
Ｓ４移動目標算出手段、第１目標算出手段
Ｓ７移動目標シフト手段、第２目標算出手段
Ｓ１１操舵角補正手段
Ｓ１２移動制御手段

Claims

移動手段を備え、その移動手段により対象者に随行して移動する移動体において、
前記対象者を測距する測距手段と、
その測距手段により測距された複数の測距データに基づいて前記対象者の位置と向きとを算出する位置向き算出手段と、
その位置向き算出手段による算出結果に基づいて前記対象者に対する当該移動体の移動目標を算出する移動目標算出手段と、
当該移動体の向きと前記位置向き算出手段により算出された前記対象者の向きとがなす角度偏差に基づいて、前記移動目標を前記対象者側へシフトする移動目標シフト手段と、
その移動目標シフト手段によりシフトされた移動目標へ当該移動体が移動するように前記移動手段を制御する移動制御手段とを備えていることを特徴とする移動体。
前記測距手段により測距された複数の測距データに基づいて前記対象者の速度を算出する速度算出手段を備え、
前記移動目標算出手段は、前記位置向き算出手段により算出された前記対象者の位置と向きとに基づいて当該移動体の第１移動目標を算出する第１目標算出手段と、その第１目標算出手段により算出された第１移動目標と前記速度算出手段により算出された前記対象者の速度とに基づいて当該移動体の第２移動目標を算出する第２目標算出手段とを備え、
前記移動目標シフト手段は、その第２移動目標を前記移動目標として前記対象者側へシフトするものであることを特徴とする請求項１記載の移動体。
前記移動目標シフト手段は、前記位置向き算出手段により算出された前記対象者の向きに直交する方向の前記対象者側へ前記移動目標をシフトするものであることを特徴とする請求項１又は２に記載の移動体。
前記移動目標シフト手段は、前記角度偏差とシグモイド曲線とに基づいて前記移動目標のシフト量を算出するものであることを特徴とする請求項１から３のいずれかに記載の移動体。
当該移動体の位置と前記移動目標シフト手段によりシフトされた移動目標とに基づいて、当該移動体の目標経路を算出する目標経路算出手段と、
その目標経路算出手段により算出された目標経路上の中間点に基づいて前記移動手段の操舵角を算出する操舵角算出手段と、
前記移動目標算出手段により算出された移動目標または前記移動目標シフト手段によりシフトされた移動目標と、当該移動体の位置との横偏差に基づいて、前記操舵角算出手段により算出された操舵角を補正する操舵角補正手段とを備えていることを特徴とする請求項１から４のいずれかに記載の移動体。
前記操舵角補正手段は、前記横偏差とシグモイド曲線とに基づいて前記操舵角を補正するものであることを特徴とする請求項５記載の移動体。
前記移動制御手段は、当該移動体を前記対象者の右前方または左前方において随行移動させるものであることを特徴とする請求項１から６のいずれかに記載の移動体。
前記移動手段は、非ホロノミック系の移動部を介して当該移動体を移動するものであることを特徴とする請求項１から７のいずれかに記載の移動体。