特開 2025-7158 自律移動体

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

(12)【公報種別】公開特許公報(A)

(11)【公開番号】P2025007158

(43)【公開日】2025-01-17

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

(51)【国際特許分類】

   G05D 1/43 20240101AFI20250109BHJP

【ＦＩ】

G05D1/02 H

【審査請求】未請求

【請求項の数】1

【出願形態】ＯＬ

(21)【出願番号】P 2023108368

(22)【出願日】2023-06-30

(71)【出願人】

【識別番号】000003207

【氏名又は名称】トヨタ自動車株式会社

(74)【代理人】

【識別番号】100103894

【弁理士】

【氏名又は名称】家入 健

(72)【発明者】

【氏名】松野 喜幸

【テーマコード（参考）】

5H301

【Ｆターム（参考）】

5H301AA01

5H301AA10

5H301BB14

5H301CC03

5H301CC06

5H301CC09

5H301DD01

5H301DD15

5H301GG08

5H301HH01

5H301HH02

(57)【要約】

【課題】自律移動体が推定する最初の自己位置姿勢を所定位置姿勢に一致させる。
【解決手段】初期パーティクル生成部７は、複数の初期パーティクルの位置を所定位置に乱数を加えたものとすると共に、複数の初期パーティクルの位置の算術平均が所定位置に一致するように、複数の初期パーティクルの位置を設定する。初期パーティクル生成部７は、複数の初期パーティクルから複数の初期パーティクル対を生成し、各初期パーティクル対毎に、当該初期パーティクル対に属する２つの初期パーティクルの姿勢を所定姿勢に乱数を加えたものとすると共に、２つの初期パーティクルの姿勢の算術平均が所定姿勢に一致するように、２つの初期パーティクルの姿勢を設定する。以上の構成によれば、自律移動体が推定する最初の自己位置姿勢を所定位置姿勢に一致させることができる。
【選択図】図２

【特許請求の範囲】

【請求項1】

パーティクルフィルタを用いて自己位置姿勢を推定する自己位置姿勢推定部と、
複数の初期パーティクルを生成する初期パーティクル生成部と、
を含み、
所定位置及び所定姿勢となるように位置及び姿勢が調整された、
自律移動体であって、
前記初期パーティクル生成部は、
前記複数の初期パーティクルの位置を所定位置にノイズを加えたものとすると共に、前記複数の初期パーティクルの位置の算術平均が所定位置に一致するように、前記複数の初期パーティクルの位置を設定し、
前記複数の初期パーティクルから複数の初期パーティクル対を生成し、各初期パーティクル対毎に、当該初期パーティクル対に属する２つの初期パーティクルの姿勢を所定姿勢にノイズを加えたものとすると共に、前記２つの初期パーティクルの姿勢の算術平均が所定姿勢に一致するように、前記２つの初期パーティクルの姿勢を設定する、
自律移動体。

【発明の詳細な説明】

【技術分野】

【0001】

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

【背景技術】

【0002】

特許文献１は、パーティクルフィルタを用いて自己位置を推定する技術を開示している。具体的には、赤外線の照射範囲から外れたパーティクルを積極的に除去することで、自己位置の推定誤差が減少するとしている。

【先行技術文献】

【特許文献】

【0003】

【特許文献1】特開２００９－１７６０３１号公報

【発明の概要】

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

【0004】

ところで、オペレータは、自律移動体が移動を開始するに際し、自律移動体の位置姿勢が所定位置姿勢となるように、自律移動体を移動させたり自律移動体を回転させたりするなどして、自律移動体の位置姿勢を調整する。ここで、調整の精度としては、典型的には、位置に関して言えば１センチメートル未満であり、姿勢に関して言えば３度未満である。

【0005】

一方、自律移動体の自己位置姿勢推定にパーティクルフィルタを用いる場合、自律移動体は、次ステップのパーティクルの予測、予測したパーティクル毎の尤度計算、尤度に基づくリサンプリングをこの記載順に繰り返しながら、自己位置姿勢推定を行う。ここで、自律移動体は、最初のステップのために複数の初期パーティクルを生成する必要がある。複数の初期パーティクルの位置姿勢には、上記の所定位置姿勢にランダムなノイズが乗ったものが採用される。そして、自律移動体は、複数の初期パーティクルの位置姿勢を算術平均することで、最初の自己位置姿勢を推定する。

【0006】

従って、オペレータが自律移動体の位置姿勢を高い精度で調整したところで、自律移動体が推定した最初の自己位置姿勢は、オペレータによる調整後の現実の位置姿勢から若干ずれてしまう。このときのずれ量は、ランダムなノイズが正規分布に従っている場合は分散を大きく設定したときに、また、パーティクルの数が少ないときに、特に顕著となる。

【0007】

本発明の目的は、自律移動体が推定する最初の自己位置姿勢を所定位置姿勢に一致させる技術を提供することにある。

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

【0008】

本開示によれば、
パーティクルフィルタを用いて自己位置姿勢を推定する自己位置姿勢推定部と、
複数の初期パーティクルを生成する初期パーティクル生成部と、
を含み、
所定位置及び所定姿勢となるように位置及び姿勢が調整された、
自律移動体であって、
前記初期パーティクル生成部は、
前記複数の初期パーティクルの位置を前記所定位置にノイズを加えたものとすると共に、前記複数の初期パーティクルの位置の算術平均が所定位置に一致するように、前記複数の初期パーティクルの位置を設定し、
前記複数の初期パーティクルから複数の初期パーティクル対を生成し、各初期パーティクル対毎に、当該初期パーティクル対に属する２つの初期パーティクルの姿勢を所定姿勢にノイズを加えたものとすると共に、前記２つの初期パーティクルの姿勢の算術平均が所定姿勢に一致するように、前記２つの初期パーティクルの姿勢を設定する、
自律移動体が提供される。

【発明の効果】

【0009】

本開示によれば、自律移動体が推定する最初の自己位置姿勢を所定位置姿勢に一致させることができる。

【図面の簡単な説明】

【0010】

【図1】自立型移動ロボットの斜視図である。

【図2】初期パーティクル生成フローである。

【図3】パーティクルの姿勢の説明図である。

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

【0011】

図１には、自律移動型ロボット１の斜視図を示している。図１に示すように、自律移動型ロボット１は、ロボット本体２、２つの駆動輪３、２つの従動輪４、二次元LiDAR装置５、制御部６を含む。

【0012】

２つの駆動輪３及び２つの従動輪４は、ロボット本体２の下面に回転自在に配置されている。制御部６は、図示しないモータを制御することで、２つの駆動輪３を個別に回転させる。これにより、自律移動型ロボット１は、前進、後進、旋回自在となっている。

【0013】

二次元LiDAR装置５は、ロボット本体２の側面に設けられており、水平面内にレーザ光を出射し、その反射光を受光することで、自律移動型ロボット１の周辺環境の点群データを生成する。

【0014】

制御部６は、中央演算処理器としてのCPU６ａ（Central Processing Unit）と、読み書き自由のRAM６ｂ（Random Access Memory）、読み出し専用のROM６ｃ（Read Only Memory）、読み書き自由のSSD６ｄ（Solid state drive）を備えている。そして、CPU６ａがROM６ｃやSSD６ｄに記憶されている制御プログラムを読み出して実行することで、制御プログラムは、CPU６ａなどのハードウェアを、初期パーティクル生成部７、自己位置姿勢推定部８、自律移動制御部９、として機能させる。

【0015】

自己位置姿勢推定部８は、パーティクルフィルタを用いて自己位置姿勢を推定する。パーティクルフィルタを用いた自己位置姿勢の推定は、次ステップのパーティクルの予測、予測したパーティクル毎の尤度計算、尤度に基づくリサンプリングをこの記載順に繰り返しながら、自己位置姿勢推定を行う。

【0016】

パーティクルは、位置データと姿勢データを含む位置姿勢データである。位置データは、自律移動型ロボット１のサービス環境におけるXY座標データである。位置データは、X座標データとY座標データを含む。姿勢データは、自律移動型ロボット１の方位角データである。自己位置姿勢推定部８は、各ステップ毎に、複数のパーティクルの位置データを算術平均することで、自律移動型ロボット１の位置を推定する。同様に、自己位置姿勢推定部８は、各ステップ毎に、複数のパーティクルの姿勢データを算術平均することで、自律移動型ロボット１の姿勢を推定する。

【0017】

初期パーティクル生成部７は、最初のステップのために複数の初期パーティクルを生成する。

【0018】

自律移動制御部９は、自己位置姿勢推定部８によって推定された自己位置姿勢と、SSD６ｄに記憶されている地図データと、に基づいて、自律移動型ロボット１の自律移動を制御する。

【0019】

次に、図２及び図３を参照しつつ、初期パーティクル生成部７を詳しく説明する。図２は、初期パーティクル生成フローである。図３は、姿勢の説明図である。説明の便宜上、初期パーティクル生成部７が生成する複数の初期パーティクルの個数をN個とする。

【0020】

なお、前述したように、オペレータは、自律移動型ロボット１が移動を開始するに際し、自律移動型ロボット１の位置姿勢が所定位置姿勢となるように、自律移動型ロボット１を移動させたり自律移動型ロボット１を回転させたりするなどして、自律移動型ロボット１の位置姿勢を調整する。ここで、調整の精度としては、典型的には、位置に関して言えば１センチメートル未満であり、姿勢に関して言えば３度未満である。所定位置姿勢は、所定位置と所定姿勢を含む。所定位置は、所定X座標と所定Y座標から成る。上記の初期パーティクル生成フローは、オペレータが自律移動型ロボット１の位置姿勢を調整した後に実行してもよく、所定位置姿勢が予め定められていれば、オペレータが自律移動型ロボット１の位置姿勢を調整する前に実行してもよい。

【0021】

まず、初期パーティクル生成部７は、複数の初期パーティクルの位置を設定する（S100）。具体的には、初期パーティクル生成部７は、乱数を生成し、１番目の初期パーティクルのX座標及びY座標を所定X座標及び所定Y座標に乱数を加えたものとする。次に、初期パーティクル生成部７は、新たに乱数を生成し、２番目の初期パーティクルのX座標及びY座標を所定X座標及び所定Y座標に乱数を加えたものとする。初期パーティクル生成部７は、更に、上記の位置設定を３番目から（Ｎ－１）番目の初期パーティクルに対して繰り返す。次に、初期パーティクル生成部７は、Ｎ番目の初期パーティクルのX座標を以下の式（１）に基づいて設定する。

【0022】

【数1】

【0023】

上記式（１）において、X(N)はＮ番目の初期パーティクルのX座標であり、X(i)はｉ番目の初期パーティクルのX座標であり、X0は所定X座標である。上記式（１）によれば、下記式（２）のように、複数の初期パーティクルのX座標の算術平均が所定X座標に一致することになる。

【0024】

【数2】

【0025】

複数の初期パーティクルのY座標についても同様に設定する。

【0026】

次に、初期パーティクル生成部７は、複数の初期パーティクルの姿勢を設定する（S100-S220）。

【0027】

ここで、図３を参照されたい。図３において、ｔは姿勢（orientation）を示している。ｔは、何れもマイナスπからプラスπまでの範囲となるように正規化されている。例えば、ｔ４が１６０であり、ｔ６が－１６０であるとして、ｔ４とｔ６を単純に算術平均すると（ｔ４＋ｔ６）／２でゼロとなる。しかし、本来であれば、あくまで視覚的には、ｔ４とｔ６の平均はｔ５となるべきである。従って、複数の初期パーティクルの姿勢を算術平均するに際し、複数の初期パーティクルの姿勢をそれぞれ単位円上の単位ベクトル[sin(t),cos(t)]に変換し、変化後の複数の単位ベクトルの総和を逆変換することで下記式（３）のように、姿勢に関する算術平均を行うものとする。下記式（３）において、tiはｉ番目の初期パーティクルの姿勢を示し、taveは算術平均された姿勢を示している。本実施形態の初期パーティクル生成部７は、複数の初期パーティクルの姿勢を設定するに際し、下記式（３）のtaveが所定姿勢t0に一致するように、複数の初期パーティクルの姿勢を設定する。

【0028】

【数3】

【0029】

以下、初期パーティクル生成部７は、複数の初期パーティクルの姿勢を設定するに際し、複数の初期パーティクルから複数の初期パーティクル対を生成し、各初期パーティクル対毎に、当該初期パーティクル対に属する２つの初期パーティクルの姿勢を所定姿勢にノイズを加えたものとすると共に、２つの初期パーティクルの姿勢の算術平均が所定姿勢に一致するように、２つの初期パーティクルの姿勢を設定する。即ち、初期パーティクル生成部７は、図２のステップS140からS210において、各初期パーティクル対毎に姿勢設定を行っている。

【0030】

まず、初期パーティクル生成部７は、複数の初期パーティクルの個数Nが奇数であるか判定し（S110）、判定結果がYESの場合（S110:YES）、N番目の姿勢を所定姿勢とし（S120）、判定結果がNOの場合（S110:NO）処理をステップS130に進める。次に、初期パーティクル生成部７は、変数ｉを１に設定する（S130）。

【0031】

次に、初期パーティクル生成部７は、マイナスπからプラスπの範囲内で乱数を生成する（S140）。乱数は、一様乱数であってもよく正規乱数であってもよい。

【0032】

次に、初期パーティクル生成部７は、ステップS140で生成した乱数の絶対値がπ／２未満であるか判定し（S150）、判定結果がYESの場合（S150:YES）は処理をS160に進め、NOの場合（S150:NO）は処理をS190に進める。

【0033】

ステップS160において、初期パーティクル生成部７は、ｉ番目の初期パーティクルの姿勢を所定姿勢t0にステップS140で生成した乱数を加えたものとする。ステップS170において、初期パーティクル生成部７は、（ｉ＋１）番目の初期パーティクルの姿勢を所定姿勢t0からステップS140で生成した乱数を引いたものとする。ここで、図３を参照されたい。t0は所定姿勢であり、t1はｉ番目の初期パーティクルの姿勢であり、t9は（ｉ＋１）番目の初期パーティクルの姿勢である。ステップS160及びステップS170によれば、ｉ番目の初期パーティクルの姿勢と（ｉ＋１）番目の初期パーティクルの姿勢を算術平均すると、付加された乱数が互いに相殺されるので、当該算術平均を所定姿勢であるt0に一致させることができる。図２に戻り、初期パーティクル生成部７は、変数ｉを二回インクリメントし（S180）、処理をS220に進める。

【0034】

仮に、ステップS140で生成した乱数の絶対値がπ／２以上であるとき、上記のステップS160及びステップS170を実行すると、以下の問題が発生する。即ち、図３において、ｉ番目の初期パーティクルの姿勢がt3であり、（ｉ＋１）番目の初期パーティクルの姿勢がt7であるとする。この場合、t3とt7の算術平均は、本来的にはt0としたところ、上記式（３）によればt5となってしまう。そこで、ステップS190において、初期パーティクル生成部７は、ｉ番目の初期パーティクルの姿勢を所定姿勢t0にステップS140で生成した乱数を加えたものとする一方で、ステップS200において、初期パーティクル生成部７は、（ｉ＋１）番目の初期パーティクルの姿勢をｉ番目の初期パーティクルの姿勢にπを加えたものとする。図３において、ｉ番目の初期パーティクルの姿勢はt3であり、（ｉ＋１）番目の初期パーティクルの姿勢はt8である。ステップS190及びステップS200によれば、ｉ番目の初期パーティクルの姿勢と（ｉ＋１）番目の初期パーティクルの姿勢は、上記式（３）の右辺にて互いに相殺されてなかったものとされる。図２に戻り、初期パーティクル生成部７は、変数ｉを二回インクリメントし（S210）、処理をS220に進める。

【0035】

ステップS220において、初期パーティクル生成部７は、すべての初期パーティクルの姿勢の設定が完了したか判定し（S220）、判定結果がYESの場合(S220:YES)、処理を終了し、判定結果がNOの場合（S220:NO）、処理をステップS140に戻す。

【0036】

上記の実施形態は、以下の特徴を有する。
自律移動型ロボット１（自律移動体）は、パーティクルフィルタを用いて自己位置姿勢を推定する自己位置姿勢推定部８と、複数の初期パーティクルを生成する初期パーティクル生成部７と、を含む。自律移動型ロボット１は、所定位置及び所定姿勢となるように位置及び姿勢が調整される。
初期パーティクル生成部７は、複数の初期パーティクルの位置を所定位置に乱数（ノイズ）を加えたものとすると共に、複数の初期パーティクルの位置の算術平均が所定位置に一致するように、複数の初期パーティクルの位置を設定する。
初期パーティクル生成部７は、複数の初期パーティクルから複数の初期パーティクル対を生成し、各初期パーティクル対毎に、当該初期パーティクル対に属する２つの初期パーティクルの姿勢を所定姿勢に乱数（ノイズ）を加えたものとすると共に、２つの初期パーティクルの姿勢の算術平均が所定姿勢に一致するように、２つの初期パーティクルの姿勢を設定する。以上の構成によれば、自律移動体が推定する最初の自己位置姿勢を所定位置姿勢に一致させることができる。

【0037】

本実施形態において、初期パーティクル生成部７は、複数の初期パーティクルから複数の初期パーティクル対を生成し、各初期パーティクル対毎に、２つの初期パーティクルの姿勢を設定している。これは、姿勢がマイナスπからプラスπまでの範囲となるように正規化されているからに他ならない。仮に、複数の初期パーティクルの個数が３つであり、所定姿勢が０であり、１番目及び２番目の姿勢が１７９であるとき、３番目の姿勢をどのような値にしたとしても、これら３つの初期パーティクルの姿勢の算術平均を所定姿勢に一致させることはできない。なぜなら、３番目の姿勢もマイナスπからプラスπまでの範囲となるように正規化されており、－３５８と設定することができないからである。これに対し、本実施形態では、各初期パーティクル対毎に、２つの初期パーティクルの姿勢を設定している。このように２つずつ、初期パーティクルの姿勢を設定することで、複数の初期パーティクル全体の姿勢の算術平均を確実に所定姿勢とすることができる。

【符号の説明】

【0038】

１ 自律移動型ロボット
７ 初期パーティクル生成部
８ 自己位置姿勢推定部

【図1】

【図2】

【図3】