知財求人 - 知財ポータルサイト「IP Force」
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】特許公報(B2)
(11)【特許番号】
(24)【登録日】2024-07-16
(45)【発行日】2024-07-24
(54)【発明の名称】車両位置制御装置および自律走行装置
(51)【国際特許分類】
G05D 1/43 20240101AFI20240717BHJP
G01C 21/28 20060101ALI20240717BHJP
【ＦＩ】
G05D1/43
G01C21/28
【請求項の数】
14
(21)【出願番号】P 2020180670
(22)【出願日】2020-10-28
(65)【公開番号】P2022071611
(43)【公開日】2022-05-16
【審査請求日】2023-09-20
(73)【特許権者】
【識別番号】000005049
【氏名又は名称】シャープ株式会社
(74)【代理人】
【識別番号】100103034
【弁理士】
【氏名又は名称】野河 信久
(74)【代理人】
【識別番号】100065248
【弁理士】
【氏名又は名称】野河 信太郎
(74)【代理人】
【識別番号】100159385
【弁理士】
【氏名又は名称】甲斐 伸二
(74)【代理人】
【識別番号】100163407
【弁理士】
【氏名又は名称】金子 裕輔
(74)【代理人】
【識別番号】100166936
【弁理士】
【氏名又は名称】稲本 潔
(74)【代理人】
【識別番号】100174883
【弁理士】
【氏名又は名称】冨田 雅己
(72)【発明者】
【氏名】藤原 陽一
【審査官】影山 直洋
(56)【参考文献】
【文献】国際公開第２０１３／００２０６７（ＷＯ，Ａ１）
【文献】特開２０２０－１１８４５１（ＪＰ，Ａ）
(58)【調査した分野】(Int.Cl.，ＤＢ名)
Ｇ０５Ｄ １／４３
Ｇ０１Ｃ ２１／２８
(57)【特許請求の範囲】
【請求項1】
自律走行する自律走行装置の走行位置を制御する車両位置制御装置であって、自律走行装置を走行させる車輪と、
自律走行装置の周辺の画像を撮影するカメラと、
自律走行装置の走行中に、車輪から得た車輪移動量と、カメラで撮影された画像から得たカメラ移動量のうち、自律走行装置の自己位置推定に利用する移動量を決定する移動量処理部と、
自律走行装置の周辺の空間領域内に存在する物体を検出し、検出された物体に関する障害物情報を取得する障害物検出部と、
前記決定された移動量と、前記取得された障害物情報とを利用して、自律走行装置の現在の位置を推定する自己位置推定部とを備え、
前記移動量処理部が、前記車輪移動量から判定された自律走行装置の静動判定結果と前記カメラ移動量から判定された自律走行装置の静動判定結果とに基づいて、車輪移動量とカメラ移動量のうち、どちらの移動量を利用するかを決定し、
前記移動量処理部で選択された現在の移動量に関する情報を、前記移動量処理部から取得し、取得された現在の移動量に関する情報に対応させて、自己位置推定処理を制御するための動作制御信号を、前記自己位置推定部に出力する移動量切替検出部をさらに備え、前記自己位置推定部は、前記動作制御信号に基づいて、自己位置推定処理を制御し、
前記移動量切替検出部が、取得された現在の移動量に関する情報が、カメラ移動量から車輪移動量に切り替えられたことを検出した場合、前記自己位置推定部に、自己位置推定処理をリセットすることを示す動作制御信号を出力し、前記自己位置推定部は、自己位置推定処理で積算された自己位置のずれをリセットすることを特徴とする車両位置制御装置。
【請求項2】
前記移動量処理部が、前記車輪の回転数を取得する回転数取得部と、
前記取得された車輪の回転数を利用して、前記車輪移動量を取得する車輪移動量取得部と、
前記カメラによって撮影された画像の画像データを利用して、前記カメラ移動量を取得するカメラ移動量取得部と、
前記車輪移動量を利用して、自律走行装置が移動中かあるいは停止中かを示す静動判定を行う第１停止判定部と、
前記カメラ移動量を利用して、自律走行装置が移動中かあるいは停止中かを示す静動判定を行う第２停止判定部と、
前記第１停止判定部と前記第２停止判定部で行われた２つの静動判定の結果から、自律走行装置の自己位置推定に利用する移動量を選択する移動量選択部と、
前記移動量選択部によって選択された移動量を、前記自己位置推定部に出力する選択移動量出力部とを備え、
前記移動量選択部が、前記車輪移動量による静動判定の結果と前記カメラ移動量による静動判定の結果が一致している場合は、車輪移動量を、自己位置推定に利用する移動量に選択し、前記車輪移動量による静動判定の結果と前記カメラ移動量による静動判定の結果が一致していない場合は、カメラ移動量を、自己位置推定に利用する移動量に選択し、前記自己位置推定部が、前記選択移動量出力部から出力された移動量と、前記障害物情報とを利用して自己位置推定を行うことを特徴とする請求項１に記載の車両位置制御装置。
【請求項3】
前記車輪移動量取得部が取得する車輪移動量は、車輪に関する情報から求めた自律走行装置の移動速度である車輪速度であり、前記カメラ移動量取得部が取得するカメラ移動量は、前記画像データから抽出された複数の特徴点に関する情報から求めた自律走行装置の移動速度であるカメラ速度であることを特徴とする請求項２に記載の車両位置制御装置。
【請求項4】
前記移動量処理部が、前記車輪移動量取得部によって取得された車輪移動量のうち、所定の時間分の複数の車輪移動量を保存する車輪移動量履歴保存部と、前記カメラ移動量取得部によって取得されたカメラ移動量の取得時刻を監視し、前記カメラ移動量の取得時刻とほぼ同じ時刻に取得された車輪移動量が、前記車輪移動量履歴保存部から前記第１停止判定部に出力されるように、前記車輪移動量履歴保存部を制御する時刻監視部とをさらに備えることを特徴とする請求項２に記載の車両位置制御装置。
【請求項5】
前記移動量処理部が、前記カメラ移動量取得部が前記カメラ移動量を取得するために、前記画像データから抽出された複数の特徴点に関する情報を利用する場合に、前記特徴点の数を取得するカメラ特徴点監視部と、前記取得された特徴点の数に基づいて、前記取得されたカメラ移動量を無効化するカメラ移動量無効部とをさらに備え、前記カメラ移動量無効部が、前記取得された特徴点の数が所定の判定比較値よりも小さい場合に、前記カメラ移動量を無効化し、前記移動量選択部がカメラ移動量を選択しないようにすることを特徴とする請求項２に記載の車両位置制御装置。
【請求項6】
前記移動量処理部が、自律走行装置に加えられた加速度と角速度を計測する慣性計測装置と、前記慣性計測装置で計測された加速度と角速度からなる慣性情報を取得する慣性情報取得部と、前記取得された慣性情報に基づいて、前記取得されたカメラ移動量を無効化するカメラ移動量無効部とをさらに備え、前記カメラ移動量無効部が、前記取得された加速度と角速度の少なくともどちらかが、所定の判定比較値よりも大きい場合に、前記カメラ移動量を無効化し、前記移動量選択部がカメラ移動量を選択しないようにすることを特徴とする請求項２に記載の車両位置制御装置。
【請求項7】
前記移動量処理部が、前記車輪速度を監視する車輪速度監視部をさらに備え、前記車輪速度監視部が監視する前記車輪速度がゼロとなり、車輪の停止状態を検出した後の所定期間において、前記車輪速度の変化量が、所定のしきい値を超えた場合は、前記移動量選択部が、前記車輪速度を、自己位置推定に利用する移動量として選択することを特徴とする請求項３に記載の車両位置制御装置。
【請求項8】
前記移動量処理部が、前記車輪速度を監視する車輪速度監視部と、自律走行装置の走行動作を実行する動作実行部と、動作実行部が実行する走行動作の活動状態に基づいて、カメラ移動量の停止判定をする基準となる停止判定値を変更するカメラ移動量判定基準変更部とをさらに備え、前記動作実行部が実行する走行動作の活動状態が活動中であるときに、前記車輪速度が所定の停止判定値よりも小さくなって、前記第１停止判定部による静動判定によって自律走行装置が停止中と判定された場合に、カメラ移動量が選択されないようにするために、カメラ移動量の停止判定をする基準となる停止判定値を、所定量だけ大きく設定し、前記車輪移動量を自己位置推定に利用する移動量に選択する範囲を広げることを特徴とする請求項３に記載の車両位置制御装置。
【請求項9】
自律走行装置が静止した位置で、自律走行装置を回転させる定置旋回制御部をさらに備え、前記移動量切替検出部が、取得された現在の移動量に関する情報がカメラ移動量から車輪移動量に切り替えられたことを検出した場合、前記定置旋回制御部に、定置旋回要求信号を出力し、前記定置旋回制御部は、走行を再開する前に、自律走行装置を定置旋回させる
ことを特徴とする請求項１に記載の車両位置制御装置。
【請求項10】
自律走行装置の車体が持ち上げられたことを検出する車体持上検出部をさらに備え、車体が持ち上げられたことを検出した場合には、前記自己位置推定部が、前記カメラ移動量を利用して、自己位置の推定を行うことを特徴とする請求項１に記載の車両位置制御装置。
【請求項11】
前記車体持上検出部が、自律走行装置と走行面との距離を測定する走行面距離測定センサと、自律走行装置が走行面を走行している状態と自律走行装置の車体が持ち上げられた状態で、接点の開閉状態が変化する持上検出スイッチのうち、少なくともどちらか一方または両方から構成されることを特徴とする請求項１０に記載の車両位置制御装置。
【請求項12】
請求項１から請求項１１のいずれかに記載された車両位置制御装置を備えたことを特徴とする自律走行装置。
【請求項13】
自律走行装置に取り付けられた車両位置制御装置の車両位置制御方法であって、カメラで撮影された画像を利用して、前記画像から抽出された複数の特徴点に関する情報から求めたカメラ移動量を取得するカメラ移動量取得ステップと、
車輪の回転数を利用して、前記車輪に関する情報から求めた車輪移動量を取得する車輪移動量取得ステップと、
前記取得されたカメラ移動量と車輪移動量を利用して、自律走行装置が停止状態にあるかまたは移動状態にあるかを判定する自律走行装置の静動判定処理を行う停止判定ステップと、
前記静動判定処理の結果に基づいて、車輪移動量とカメラ移動量のうち、自律走行装置の自己位置推定処理に利用する移動量を選択する移動量選択ステップと、
自律走行装置の周辺に存在する障害物に関する障害物情報を取得する障害物情報取得ステップと、
前記選択された移動量と、前記取得された障害物情報と、所定の地図情報とを利用して、自律走行装置の現在の位置を推定する自己位置推定ステップと、
前記推定された現在の位置に関する情報から、自律走行装置の姿勢を調整する車両姿勢制御ステップと、
前記移動量選択ステップで選択された現在の移動量に関する情報を取得し、取得された現在の移動量に関する情報に対応させて、自己位置推定処理を制御するための動作制御信号を、前記自己位置推定ステップで出力する移動量切替検出ステップとからなり、
前記自己位置推定ステップにおいて、前記動作制御信号に基づいて、自己位置推定処理を制御し、
前記移動量切替検出ステップにおいて、取得された現在の移動量に関する情報が、カメラ移動量から車輪移動量に切り替えられたことを検出した場合、前記自己位置推定ステップにおいて、自己位置推定処理をリセットすることを示す動作制御信号を出力し、自己位置推定処理で積算された自己位置のずれをリセットすることを特徴とする車両位置制御装置の車両位置制御方法。
【請求項14】
自律走行する自律走行装置の走行位置を制御する車両位置制御装置であって、
自律走行装置を走行させる車輪と、
自律走行装置の周辺の画像を撮影するカメラと、
自律走行装置の走行中に、車輪から得た車輪移動量と、カメラで撮影された画像から得たカメラ移動量のうち、自律走行装置の自己位置推定に利用する移動量を決定する移動量処理部と、
自律走行装置の周辺の空間領域内に存在する物体を検出し、検出された物体に関する障害物情報を取得する障害物検出部と、
前記決定された移動量と、前記取得された障害物情報とを利用して、自律走行装置の現在の位置を推定する自己位置推定部とを備え、
前記移動量処理部が、前記車輪移動量から判定された自律走行装置の静動判定結果と前記カメラ移動量から判定された自律走行装置の静動判定結果とに基づいて、車輪移動量とカメラ移動量のうち、どちらの移動量を利用するかを決定し、
前記移動量処理部が、
前記車輪の回転数を取得する回転数取得部と、
前記取得された車輪の回転数を利用して、前記車輪移動量を取得する車輪移動量取得部と、
前記カメラによって撮影された画像の画像データを利用して、前記カメラ移動量を取得するカメラ移動量取得部と、
前記車輪移動量を利用して、自律走行装置が移動中かあるいは停止中かを示す静動判定を行う第１停止判定部とを備え、
前記車輪移動量取得部が取得する車輪移動量は、車輪に関する情報から求めた自律走行装置の移動速度である車輪速度であり、
前記カメラ移動量取得部が取得するカメラ移動量は、前記画像データから抽出された複数の特徴点に関する情報から求めた自律走行装置の移動速度であるカメラ速度であり、
前記移動量処理部が、前記車輪速度を監視する車輪速度監視部と、自律走行装置の走行動作を実行する動作実行部と、動作実行部が実行する走行動作の活動状態に基づいて、カメラ移動量の停止判定をする基準となる停止判定値を変更するカメラ移動量判定基準変更部とをさらに備え、
前記動作実行部が実行する走行動作の活動状態が活動中であるときに、前記車輪速度が所定の停止判定値よりも小さくなって、前記第１停止判定部による静動判定によって自律走行装置が停止中と判定された場合に、カメラ移動量が選択されないようにするために、カメラ移動量の停止判定をする基準となる停止判定値を、所定量だけ大きく設定し、前記車輪移動量を自己位置推定に利用する移動量に選択する範囲を広げることを特徴とする車両位置制御装置。
【発明の詳細な説明】
【技術分野】
【0001】
この発明は、車両位置制御装置および自律走行装置に関し、車両の位置を推定する機能を有する車両位置制御装置および車両位置制御装置を備えた自律走行装置に関する。
【背景技術】
【0002】
今日、荷物を搬送する搬送用ロボットや、建物内および建物周辺の敷地内の状況を監視する監視用ロボットなど、自律的に移動する自律走行装置が利用されている。
このような自律走行装置は、地図情報を利用し、移動距離や走行速度などの走行情報を測定し、測定した走行情報を用いて、地図上の現在の自己位置を推定しながら、所定の経路を走行する。
このような自律走行装置は、地図情報を利用し、移動距離や走行速度などの走行情報を測定し、測定した走行情報を用いて、地図上の現在の自己位置を推定しながら、所定の経路を走行する。
【0003】
たとえば、従来から、LIDAR（ライダー：Light Detection and Ranging、あるいはLaser Imaging Detection and Ranging）によるSLAM（スラム：Simultaneous Localization and Mapping）技術を使ってあらかじめ地図を作成し、その後、自分が地図上のどこにいるのかを推定して自律走行するロボットが利用されている。
【0004】
ここで、自律走行装置の地図上での自己位置を推定する技術として、パーティクルフィルタを使用した方式が良く知られている。
パーティクルフィルタは適者生存の原理に基づく動的最大値探索法であり、複数のパーティクルと、地図上に存在する所定の障害物との位置関係から、ロボットの自己位置を推定することができる。
パーティクルフィルタは適者生存の原理に基づく動的最大値探索法であり、複数のパーティクルと、地図上に存在する所定の障害物との位置関係から、ロボットの自己位置を推定することができる。
【0005】
図２９に、LIDARを用いて自己位置を推定する従来の自律走行装置（ロボット）の概略構成ブロック図を示す
自律走行装置（ロボット）１００は、主として、自律走行装置を走行させる2つ以上の車輪１０１、移動量算出部１０２、ＬＩＤＡＲ１０３、障害物検出部１０４、自己位置推定部１０５、地図情報１０６、車体姿勢制御部１０７とから構成される。
車輪１０１には、車輪の回転数を計数するエンコーダーが取り付けられている。
自律走行装置（ロボット）１００は、主として、自律走行装置を走行させる2つ以上の車輪１０１、移動量算出部１０２、ＬＩＤＡＲ１０３、障害物検出部１０４、自己位置推定部１０５、地図情報１０６、車体姿勢制御部１０７とから構成される。
車輪１０１には、車輪の回転数を計数するエンコーダーが取り付けられている。
【0006】
移動量算出部１０２は、エンコーダーで検出された車輪の回転数と、車輪の直径から、左右の車輪の移動量を算出する。
左右の車輪の移動量の平均から、ロボットの直進方向の移動量を算出し、左右の車輪の移動量の差分と車輪の間隔から、ロボットの旋回角度を算出する。
これにより、ロボットがどの方向にどれだけ移動したかを算出することができる。
車輪の回転数から算出したロボットの移動量は、車輪の直径の誤差、車輪間隔の誤差、車輪のすべり等の要因から、実際の動きに対して徐々にずれていく。
そこで、自己位置推定部１０５は、以下に述べるLIDARが測定した障害物情報と、地図上の障害物情報との比較による自己位置の推定を行う。
左右の車輪の移動量の平均から、ロボットの直進方向の移動量を算出し、左右の車輪の移動量の差分と車輪の間隔から、ロボットの旋回角度を算出する。
これにより、ロボットがどの方向にどれだけ移動したかを算出することができる。
車輪の回転数から算出したロボットの移動量は、車輪の直径の誤差、車輪間隔の誤差、車輪のすべり等の要因から、実際の動きに対して徐々にずれていく。
そこで、自己位置推定部１０５は、以下に述べるLIDARが測定した障害物情報と、地図上の障害物情報との比較による自己位置の推定を行う。
【0007】
ＬＩＤＡＲ１０３は、所定の障害物検知領域内の２次元空間または３次元空間にレーザーを出射し、障害物検知領域内にある障害物の複数の測点までの距離を測定する装置である。ＬＩＤＡＲ１０３は、主に、レーザーの発光部、受光部、およびレーザー走査装置から構成される。
ＬＩＤＡＲ１０３を構成する発光部からレーザーを出射した後、障害物によって反射された反射光を受光部で検出し、出射時刻と受光時刻との時間差から、障害物までの距離（障害物情報）を算出する。
障害物検出部１０４は、LIDARの測距データ（障害物情報）から、２次元平面上の障害物の配置を求める。
ＬＩＤＡＲ１０３を構成する発光部からレーザーを出射した後、障害物によって反射された反射光を受光部で検出し、出射時刻と受光時刻との時間差から、障害物までの距離（障害物情報）を算出する。
障害物検出部１０４は、LIDARの測距データ（障害物情報）から、２次元平面上の障害物の配置を求める。
【0008】
自己位置推定部１０５は、車輪によって検出したロボットの移動量と、ＬＩＤＡＲ１０３による障害物情報と、実際の地図情報１０６から得た障害物情報とを利用して、ロボットの地図上の姿勢（座標＋方位）を推定する。
車体姿勢制御部１０７は、推定されたロボットの姿勢に関する情報を利用して、ロボットの位置を制御する。
車体姿勢制御部１０７は、推定されたロボットの姿勢に関する情報を利用して、ロボットの位置を制御する。
【0009】
図３０Aに、自律走行装置（ロボット）１００の姿勢と、障害物１１０の配置の一実施例の説明図を示す。
ロボット１００は、図３０Aの紙面の右方向に進行するものとする。
パーティクルフィルタを使用する方式では、ロボット１００の姿勢の真値がわからない状況で、以下の手順により、ロボットの現在位置を推定する。
ロボット１００は、図３０Aの紙面の右方向に進行するものとする。
パーティクルフィルタを使用する方式では、ロボット１００の姿勢の真値がわからない状況で、以下の手順により、ロボットの現在位置を推定する。
【0010】
最初にロボット１００の現在位置と思われる座標の周辺に、パーティクルをランダムにばらまく。各パーティクルは、ロボットの姿勢（座標＋方位）の候補を表している。
図３０Bに、自律走行装置（ロボット）１００の姿勢と障害物１１０の配置の説明図におけるパーティクルの配置の例を示す。
図３０Bでは、６つのパーティクル１１１を示しており、各パーティクル１１１には、番号（１から６）が付与されているとする。
図３０Bに、自律走行装置（ロボット）１００の姿勢と障害物１１０の配置の説明図におけるパーティクルの配置の例を示す。
図３０Bでは、６つのパーティクル１１１を示しており、各パーティクル１１１には、番号（１から６）が付与されているとする。
【0011】
地図に登録された障害物と各パーティクルとの位置関係から、各パーティクルから見える障害物の位置が算出できる。この算出された障害物の位置の情報と、実際にロボットのLIDARによって測定された障害物情報の一致度を評価することで、各パーティクルのもっともらしさ（尤度）が算出される。
一致度の高いパーティクルほど、もっともらしさ（尤度）が大きい。
一致度の高いパーティクルほど、もっともらしさ（尤度）が大きい。
【0012】
次に、尤度が所定値以下のパーティクルを取り除き、尤度が大きいパーティクルの周辺に、パーティクルをランダムに再散布する。
その後、ロボットを少し動かす。
車輪の回転数からこの時のロボットの移動量を算出する。パーティクルはロボットの姿勢の候補であるので、ロボットの移動量を全パーティクルに適用することで移動後のロボットの姿勢の候補が算出される。ロボットを動かしたことでロボットのLIDARによって測定される障害物の情報が変化し、パーティクルを合わせて動かしたことで、パーティクルから見える障害物の位置も変化する。この時、ロボットの姿勢真値に近いパーティクルでは、移動後における地図上の障害物の配置と観測データが整合するが、ロボットの姿勢真値から外れたパーティクルでは、移動後における障害物の配置と観測データが整合しない。
その後、ロボットを少し動かす。
車輪の回転数からこの時のロボットの移動量を算出する。パーティクルはロボットの姿勢の候補であるので、ロボットの移動量を全パーティクルに適用することで移動後のロボットの姿勢の候補が算出される。ロボットを動かしたことでロボットのLIDARによって測定される障害物の情報が変化し、パーティクルを合わせて動かしたことで、パーティクルから見える障害物の位置も変化する。この時、ロボットの姿勢真値に近いパーティクルでは、移動後における地図上の障害物の配置と観測データが整合するが、ロボットの姿勢真値から外れたパーティクルでは、移動後における障害物の配置と観測データが整合しない。
【0013】
移動後に各パーティクルの尤度を再評価し、尤度が低い、すなわちロボットの姿勢真値との差が大きいパーティクルを取り除き、尤度が大きい、すなわちロボットの姿勢真値に近いと推定されるパーティクルの周辺に、パーティクルをランダムに再散布する。
これにより、ロボットの動きと障害物の変化が合わないパーティクルが削除されていく。
これにより、ロボットの動きと障害物の変化が合わないパーティクルが削除されていく。
【0014】
以上の処理を繰り返すことで、適者生存の原理によって、もっともらしい位置に散布されたパーティクルの密度が高まっていく。
そこで、以上の処理を繰り返しながら、各パーティクルを尤度に基づき加重平均した位置情報を求め、この位置情報を、ロボットの姿勢（座標＋方位）とする。
説明用に簡略化したため、図３０Bではパーティクルは6個しかないが、実際は数百～数千のパーティクルを使用することで統計的に精度を確保している。
そこで、以上の処理を繰り返しながら、各パーティクルを尤度に基づき加重平均した位置情報を求め、この位置情報を、ロボットの姿勢（座標＋方位）とする。
説明用に簡略化したため、図３０Bではパーティクルは6個しかないが、実際は数百～数千のパーティクルを使用することで統計的に精度を確保している。
【0015】
また、カメラで撮影された画像を利用して、自律走行装置の自己位置を推定する技術も利用されている。たとえば、カメラで撮影された画像を利用して自律走行装置の移動量を検出するVisual OdometryやVisual SLAMと呼ばれる手法がある。
これらの手法では、カメラで撮影された画像内から特徴点を抽出し、画像内の特徴点の動きからカメラの動き、すなわちロボット本体の動きを推定する。これを用いれば車輪の状態に関係なく自己位置を推定することが可能である。
これらの手法では、カメラで撮影された画像内から特徴点を抽出し、画像内の特徴点の動きからカメラの動き、すなわちロボット本体の動きを推定する。これを用いれば車輪の状態に関係なく自己位置を推定することが可能である。
【0016】
さらに、特許文献１には、車両の前後にそれぞれカメラを備え、車速検出器で車速パルスをカウントし、所定の時間の前後におけるカメラからの各画像をもとに、撮影された同じ特徴物とその変位量を検出することにより、その時間における車両の走行距離を算出し、算出した走行距離と、その走行距離に対応する上記時間における車速パルスに基づいて、所定の計算式で、車速パルス１パルスあたりの真の単位距離を算出して補正するナビゲーション装置が記載されている。
【0017】
また、特許文献２には、車輪速センサと、車両の先端と後端に前方カメラと後方カメラを備え、走行中に、路面上の同一の特徴物を撮影し、前方カメラで撮影した特徴物の車両との相対位置と車輪速センサからの車輪速パルスカウンタ値を取得した後、直進して、後方カメラで撮影した特徴物の車両との相対位置と車輪速センサからの車輪速パルスカウンタ値を取得して、これらの取得された情報から、車輪速パルスカウンタ値当りの移動距離を算出して、自車両の位置を推定するための車両の走行距離を算出する車両制御装置が記載されている。
【先行技術文献】
【特許文献】
【0018】
【文献】特開２００８－８２９２５号公報
【文献】国際公開２０１８／１７３９０７ Ａ１
【発明の概要】
【発明が解決しようとする課題】
【0019】
しかし、以上のような従来の自己位置推定技術においては、移動量に伴う障害物の見え方の変化に基づきパーティクルの尤度を見積もっているため、移動量があまりに不正確だと尤度の見積もりが不正確になる。
その結果、削除するパーティクルが不正確になり、実際とは異なる位置のパーティクル密度が高まり、結果として自己位置推定位置が誤ってしまうという問題点があった。
その結果、削除するパーティクルが不正確になり、実際とは異なる位置のパーティクル密度が高まり、結果として自己位置推定位置が誤ってしまうという問題点があった。
【0020】
また、小型ロボットにおいては、持ち上げて位置移動されたり、走行中に障害物に乗り上げたりすることがある。
持ち上げて位置移動された場合、車輪が回転していないため移動されているのに移動していないことになってしまう。
一方、走行中に障害物に乗り上げた場合、車輪が空転するため移動していないのに移動していることになってしまう。
その結果、車輪による移動量が不正になって自己位置推定が誤ることになるという問題点があった。
持ち上げて位置移動された場合、車輪が回転していないため移動されているのに移動していないことになってしまう。
一方、走行中に障害物に乗り上げた場合、車輪が空転するため移動していないのに移動していることになってしまう。
その結果、車輪による移動量が不正になって自己位置推定が誤ることになるという問題点があった。
【0021】
一旦自己位置が不正になるとロボット単体では復帰できず、ロボットの現在位置に対してパーティクルの散布を手動で行う必要があった。この操作は煩雑でユーザーにとって負荷が大きいものであった。
【0022】
また、従来のカメラで撮影された画像を利用した自己位置推定技術では、カメラで撮影された画像から算出する移動量は、特に前後方向の移動量の精度が低く、縮尺が合わなくなる場合があった。単眼カメラで移動量を算出する場合にはこの傾向が特に顕著になる。
撮影された画像を利用する場合、画面全体が白壁になったりカメラに外光が差し込んだりする状況では、画面内に特徴点がなくなり移動量が算出できないという問題点があった。
さらに、素早い動きや地面の凹凸で画像がぶれると時間的に前後する特徴点の対応関係がわからなくなり移動量が算出できなくなることがあり、カメラで撮影された画像を利用して自己位置を推定するのが難しい場合も多かった。
撮影された画像を利用する場合、画面全体が白壁になったりカメラに外光が差し込んだりする状況では、画面内に特徴点がなくなり移動量が算出できないという問題点があった。
さらに、素早い動きや地面の凹凸で画像がぶれると時間的に前後する特徴点の対応関係がわからなくなり移動量が算出できなくなることがあり、カメラで撮影された画像を利用して自己位置を推定するのが難しい場合も多かった。
【0023】
そこで、この発明は、以上のような事情を考慮してなされたものであり、従来のようにLIDARを用いて自律走行装置の自己位置を推定する場合や、カメラで撮影された画像を利用して自律走行装置の自己位置を推定する場合の問題点を解消し、たとえば、自律走行装置の車体が持ち上げられて移動させられたり、車輪が空転したりして、車輪が地面を離れ自律走行装置の移動量が正確に把握できないような状態の場合でも、自律走行装置の自己位置の推定を可能とし、自律走行を継続させることのできる自律走行装置を提供することを目的とする。
【課題を解決するための手段】
【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】
また、この発明は、自律走行装置に取り付けられた車両位置制御装置の車両位置制御方法であって、カメラで撮影された画像を利用して、前記画像から抽出された複数の特徴点に関する情報から求めたカメラ移動量を取得するカメラ移動量取得ステップと、車輪の回転数を利用して、前記車輪に関する情報から求めた車輪移動量を取得する車輪移動量取得ステップと、前記取得されたカメラ移動量と車輪移動量を利用して、自律走行装置が停止状態にあるかまたは移動状態にあるかを判定する自律走行装置の静動判定処理を行う停止判定ステップと、前記静動判定処理の結果に基づいて、車輪移動量とカメラ移動量のうち、位置推定処理に利用する移動量を選択する移動量選択ステップと、自律走行装置の周辺に存在する障害物に関する障害物情報を取得する障害物情報取得ステップと、前記選択された移動量と、前記取得された障害物情報と、所定の地図情報とを利用して、自律走行装置の現在の位置を推定する位置推定ステップと、前記推定された現在の位置に関する情報から、自律走行装置の姿勢を調整する車両姿勢制御ステップとからなる車両位置制御装置の車両位置制御方法を提供するものである。
【発明の効果】
【0053】
この発明によれば、移動量処理部が、車輪移動量から判定された自律走行装置の静動判定結果とカメラ移動量から判定された自律走行装置の静動判定結果とに基づいて、車輪移動量とカメラ移動量のうち、どちらの移動量を利用するかを決定し、決定された移動量と、取得された障害物情報とを利用して、自律走行装置の現在の位置を推定するので、精度が高く、安定した自律走行装置の位置の推定を可能とし、自律走行を継続させることができる。
たとえば、自律走行装置の車体が持ち上げられて移動させられたり、車輪が空転したりして、車輪が地面を離れ自律走行装置の移動量が正確に把握できないような状態になった場合でも、安定した自律走行装置の位置の推定ができる。
たとえば、自律走行装置の車体が持ち上げられて移動させられたり、車輪が空転したりして、車輪が地面を離れ自律走行装置の移動量が正確に把握できないような状態になった場合でも、安定した自律走行装置の位置の推定ができる。
【図面の簡単な説明】
【0054】
【図1】この発明の自律走行装置の一実施例の概略構成ブロック図である。
【図2】この発明の自律走行装置の移動量処理部の一実施例の構成ブロック図である。
【図3】この発明の自律走行装置の移動量処理部の一実施例の構成ブロック図である。
【図4】この発明の自律走行装置の移動量処理部の一実施例の構成ブロック図である。
【図5】この発明の自律走行装置の移動量処理部の一実施例の構成ブロック図である。
【図6】この発明の自律走行装置の移動量処理部の一実施例の構成ブロック図である。
【図7】この発明の自律走行装置の移動量処理部の一実施例の構成ブロック図である。
【図8】この発明の自律走行装置の移動量処理部の一実施例の構成ブロック図である。
【図9】この発明における車輪速度とカメラ速度との関係と、移動量の選択速度領域の一実施例の説明図である。
【図10】この発明における車輪速度とカメラ速度を利用した移動量判定選択情報の一実施例の説明図である。
【図11】この発明において、車両が停止した場合における車輪速度とカメラ速度の変化の一実施例の説明図である。
【図12】この発明において、車輪速度が大きく変化した場合における車輪速度とカメラ速度の変化の一実施例の説明図である。
【図13】この発明の自律走行装置が停止中において採用する移動量の選択判定領域の一実施例の説明図である。
【図14】この発明の自律走行装置が活動中において採用する移動量の選択判定領域の一実施例の説明図である。
【図15】この発明の自律走行装置が低速移動中の場合に、採用する移動量の選択判定領域の一実施例の説明図である。
【図16】この発明の自律走行装置が低速移動中の場合に、採用する移動量の選択判定領域の一実施例の説明図である。
【図17】この発明において、車輪とカメラの移動量と、定置旋回等との関係を示したタイムチャートの一実施例の説明図である。
【図18】この発明の自律走行装置の第２の実施形態の概略構成ブロック図である。
【図19】この発明の自律走行装置の第３の実施形態の概略構成ブロック図である。
【図20】この発明の自律走行装置の第４の実施形態の概略構成ブロック図である。
【図21A】この発明の自律走行装置に取り付けられた走行面距離測定センサの一実施例の説明図である。
【図21B】この発明の自律走行装置に取り付けられた走行面距離測定センサの一実施例の説明図である。
【図22A】この発明の自律走行装置に取り付けられた持上検出スイッチの一実施例の説明図である。
【図22B】この発明の自律走行装置に取り付けられた持上検出スイッチの一実施例の説明図である。
【図23】この発明の自律走行装置の車両位置制御処理の一実施例の概略フローチャートである。
【図24】この発明の移動量処理部における移動量判定選択処理の一実施例のフローチャートである。
【図25】この発明の移動量処理部における移動量判定選択処理の一実施例のフローチャートである。
【図26】この発明の移動量処理部における移動量判定選択処理の一実施例のフローチャートである。
【図27】この発明の移動量処理部における移動量判定選択処理の一実施例のフローチャートである。
【図28】この発明の移動量処理部における移動量判定選択処理の一実施例のフローチャートである。
【図29】従来におけるLIDARを用いて自己位置を推定する自律走行装置の概略構成ブロック図である。
【図30A】自律走行装置の姿勢と、障害物の配置の一実施例の説明図である。
【図30B】自律走行装置の姿勢と障害物の配置の説明図におけるパーティクルの配置の一実施例の説明図である。
【発明を実施するための形態】
【0055】
以下、図面を使用して本発明の実施の形態を説明する。なお、以下の実施例の記載によって、この発明が限定されるものではない。
この発明の車両位置制御装置は、所定の走行経路を自律走行する自律走行装置に備えられ、自律走行装置の走行位置を制御する装置であり、自律走行装置の現在位置や周辺の物体の情報を取得して自己位置を推定し、自律走行装置の位置や進行方向の補正をする。
この発明の車両位置制御装置は、所定の走行経路を自律走行する自律走行装置に備えられ、自律走行装置の走行位置を制御する装置であり、自律走行装置の現在位置や周辺の物体の情報を取得して自己位置を推定し、自律走行装置の位置や進行方向の補正をする。
【0056】
＜＜第１の実施形態＞＞
＜車両位置制御装置および自律走行装置の構成＞
図１に、この発明の自律走行装置の一実施例の概略構成ブロック図を示す。
図１において、この発明の自律走行装置１００は、主に、車体と、2つ以上の車輪と、カメラとを備え、所定の経路情報に基づいて、障害物を避けながら、自律的に移動する機能を有する車両である。
また、自律走行装置１００は、移動機能に加えて、輸送機能、監視機能、掃除機能、通報機能などの種々の機能を備えてもよい。
＜車両位置制御装置および自律走行装置の構成＞
図１に、この発明の自律走行装置の一実施例の概略構成ブロック図を示す。
図１において、この発明の自律走行装置１００は、主に、車体と、2つ以上の車輪と、カメラとを備え、所定の経路情報に基づいて、障害物を避けながら、自律的に移動する機能を有する車両である。
また、自律走行装置１００は、移動機能に加えて、輸送機能、監視機能、掃除機能、通報機能などの種々の機能を備えてもよい。
【0057】
図１では、自律走行装置１００（以下、ロボット、車両とも呼ぶ）の主要な構成を示したものであり、特に、自律走行装置に備えられる車両位置制御装置に関する部分の構成を示したものである。
図１において、この発明の自律走行装置１００は、主として、車輪１、移動量処理部２、ＬＩＤＥＲ３、障害物検出部４、自己位置推定部５、地図情報６、車体姿勢制御部７、カメラ１１を備える。
ここで、移動量処理部２、自己位置推定部５、車体姿勢制御部７が、車両位置制御装置の主要な構成要素である。
図１において、この発明の自律走行装置１００は、主として、車輪１、移動量処理部２、ＬＩＤＥＲ３、障害物検出部４、自己位置推定部５、地図情報６、車体姿勢制御部７、カメラ１１を備える。
ここで、移動量処理部２、自己位置推定部５、車体姿勢制御部７が、車両位置制御装置の主要な構成要素である。
【0058】
車輪１は、自律走行装置を走行させる部材である。
自律移動ロボットで良く使用される差動駆動方式では、2つの駆動輪を持ち駆動輪の速度差によって旋回を行う。差動駆動方式においては、ロボットを自立させるためにキャスターなどの従動輪が設けられることが多い。この他、自動車と同じように前輪の操舵角が変化して向きを変える構造もある。
また、差動駆動方式では、図示しないエンコーダーを、駆動輪の左輪と右輪にそれぞれ設け、車輪の回転数や回転方向、回転位置、回転速度等を計測する。エンコーダーは、後述する回転数取得部に相当する。自動車と同じ駆動方式の場合は、エンコーダーを駆動輪の回転速度と操舵角の測定に使用する。
自律移動ロボットで良く使用される差動駆動方式では、2つの駆動輪を持ち駆動輪の速度差によって旋回を行う。差動駆動方式においては、ロボットを自立させるためにキャスターなどの従動輪が設けられることが多い。この他、自動車と同じように前輪の操舵角が変化して向きを変える構造もある。
また、差動駆動方式では、図示しないエンコーダーを、駆動輪の左輪と右輪にそれぞれ設け、車輪の回転数や回転方向、回転位置、回転速度等を計測する。エンコーダーは、後述する回転数取得部に相当する。自動車と同じ駆動方式の場合は、エンコーダーを駆動輪の回転速度と操舵角の測定に使用する。
【0059】
カメラ１１は、自律走行装置の周辺の画像を撮影し、撮影された画像の画像データを出力するものである。
カメラ１１としては、たとえば、自律走行装置の前方を撮影する固定カメラでもよく、あるいは、水平方向に３６０度回転し、自律走行装置の周囲全体を撮影できる可動カメラを利用してもよい。
また、自律走行装置の周辺を撮影するカメラとして、たとえば、車体の側面に、左方向、右方向、後方をそれぞれ撮影する固定カメラを備えてもよい。
カメラ１１で撮影された画像から得られた画像データ（以下、カメラ画像とも呼ぶ）は、移動量処理部２に与えられる。
カメラ１１としては、たとえば、自律走行装置の前方を撮影する固定カメラでもよく、あるいは、水平方向に３６０度回転し、自律走行装置の周囲全体を撮影できる可動カメラを利用してもよい。
また、自律走行装置の周辺を撮影するカメラとして、たとえば、車体の側面に、左方向、右方向、後方をそれぞれ撮影する固定カメラを備えてもよい。
カメラ１１で撮影された画像から得られた画像データ（以下、カメラ画像とも呼ぶ）は、移動量処理部２に与えられる。
【0060】
ＬＩＤＥＲ（ライダー：Light Detection and Ranging、あるいはLaser Imaging Detection and Ranging）３は、車両の現在位置から、車体の進行方向の所定の空間内に存在する物体や路面までの距離を測定する装置であり、車体の前方のほぼ中央付近に配置されることが多い。
ＬＩＤＡＲは、進行方向の所定の領域にレーザーを出射する発光部と、物体によって反射されたレーザーを受光する受光部と、レーザーの出射方向を２次元的あるいは３次元的に変化させる走査制御部とから構成される。
ＬＩＤＡＲは、進行方向の所定の領域にレーザーを出射する発光部と、物体によって反射されたレーザーを受光する受光部と、レーザーの出射方向を２次元的あるいは３次元的に変化させる走査制御部とから構成される。
【0061】
発光部から所定の障害物検知領域内の２次元空間または３次元空間にレーザーを出射した後、物体によって反射された反射光を受光部で検出し、たとえば、出射時刻と受光時刻との時間差から、受光距離を算出する。この算出された受光距離から、物体（障害物）までの距離が検出される。
また、ＬＩＤＡＲ３は、走査制御部によって、レーザーの出射方向を変化させることによって、障害物検知領域内の複数の測点における距離を測定する。
ＬＩＤＡＲ３で測定された距離情報は、障害物検出部４に与えられる。
また、ＬＩＤＡＲ３は、走査制御部によって、レーザーの出射方向を変化させることによって、障害物検知領域内の複数の測点における距離を測定する。
ＬＩＤＡＲ３で測定された距離情報は、障害物検出部４に与えられる。
【0062】
障害物検出部４は、自律走行装置の周辺の空間領域内に存在する物体（たとえば、建物、柱、壁、突起物、人体など）を検出し、検出された物体に関する情報（以下、障害物情報と呼ぶ）を取得する部分である。
障害物検出部４は、主として、上記したＬＩＤＥＲ３によって測定された物体までの距離を利用して、物体（障害物）を検出する。
ＬＩＤＡＲ３によって、反射光が受光された複数の測点までの距離が算出されるので、距離が算出された測点の位置情報から、測点の方向に障害物が存在することが検出され、その障害物の大きさ、位置、形状、障害物までの距離が取得される。
障害物検出部４は、主として、上記したＬＩＤＥＲ３によって測定された物体までの距離を利用して、物体（障害物）を検出する。
ＬＩＤＡＲ３によって、反射光が受光された複数の測点までの距離が算出されるので、距離が算出された測点の位置情報から、測点の方向に障害物が存在することが検出され、その障害物の大きさ、位置、形状、障害物までの距離が取得される。
【0063】
また、障害物検出部４としては、車体の周辺に存在する物体までの距離を測定する超音波センサや、車体の側面に設けられたバンパーに物体が接触したことを検出するバンパースイッチを備えてもよく、ＬＩＤＡＲ３に加え、超音波センサ、およびバンパースイッチのいずれかから取得した情報を利用して、物体（障害物）を検出してもよい。
障害物検出部４によって検出された物体（障害物）に関する情報（障害物情報）は、自己位置推定部５に与えられ、自律走行装置の自己位置を推定するのに用いられる。
障害物検出部４によって検出された物体（障害物）に関する情報（障害物情報）は、自己位置推定部５に与えられ、自律走行装置の自己位置を推定するのに用いられる。
【0064】
移動量処理部２は、自律走行装置の走行中に、車輪から得た車輪移動量と、カメラで撮影された画像から得たカメラ移動量のうち、自律走行装置の自己位置推定に利用する移動量を決定する部分である。
移動量処理部２では、まず、カメラ１１から出力されたカメラ画像と、車輪１から取得した走行情報を利用して、自律走行装置の移動量を取得する。
以下の実施例では、カメラ画像から取得された移動量をカメラ移動量と呼び、車輪１から取得された移動量を、車輪移動量と呼ぶ。
移動量処理部２では、まず、カメラ１１から出力されたカメラ画像と、車輪１から取得した走行情報を利用して、自律走行装置の移動量を取得する。
以下の実施例では、カメラ画像から取得された移動量をカメラ移動量と呼び、車輪１から取得された移動量を、車輪移動量と呼ぶ。
【0065】
また、移動量処理部２において、取得されたカメラ移動量と車輪移動量とを利用して、自律走行装置の静動判定（停止判定処理とも呼ぶ）を行う。
さらに、移動量処理部２は、車輪移動量から判定された自律走行装置の静動判定結果と、カメラ移動量から判定された自律走行装置の静動判定結果とに基づいて、車輪移動量とカメラ移動量のうち、どちらの移動量を利用するかを決定し、自己位置推定に利用する移動量を選択する処理（移動量選択処理とも呼ぶ）を行う。
選択された移動量は、自己位置推定部５に出力される。
選択された移動量は、自己位置推定部５で、自律走行装置の自己位置を推定するのに用いられる。
さらに、移動量処理部２は、車輪移動量から判定された自律走行装置の静動判定結果と、カメラ移動量から判定された自律走行装置の静動判定結果とに基づいて、車輪移動量とカメラ移動量のうち、どちらの移動量を利用するかを決定し、自己位置推定に利用する移動量を選択する処理（移動量選択処理とも呼ぶ）を行う。
選択された移動量は、自己位置推定部５に出力される。
選択された移動量は、自己位置推定部５で、自律走行装置の自己位置を推定するのに用いられる。
【0066】
以下の実施例では、自律走行装置の移動量として、移動速度を取得するものとする。
カメラ移動量は、自律走行装置に取り付けられたカメラで撮影された画像（カメラ画像）の画像データから抽出された複数の特徴点に関する情報から求めた自律走行装置の移動速度であり、カメラ速度（V2）とも呼ぶ。
カメラ移動量（カメラ速度：V2）は、後述するカメラ移動量取得部によって取得される。
カメラ移動量は、自律走行装置に取り付けられたカメラで撮影された画像（カメラ画像）の画像データから抽出された複数の特徴点に関する情報から求めた自律走行装置の移動速度であり、カメラ速度（V2）とも呼ぶ。
カメラ移動量（カメラ速度：V2）は、後述するカメラ移動量取得部によって取得される。
【0067】
車輪移動量は、車輪に関する情報から求めた自律走行装置の移動速度であり、特に、自律走行装置の車輪の回転数から算出された移動速度であり、車輪速度（V1）とも呼ぶ。
車輪移動量（車輪速度：V1）は、後述する車輪移動量取得部によって取得される。
移動量処理部２の詳細な構成や機能については、後述するいくつかの実施例で説明する。
車輪移動量（車輪速度：V1）は、後述する車輪移動量取得部によって取得される。
移動量処理部２の詳細な構成や機能については、後述するいくつかの実施例で説明する。
【0068】
自己位置推定部５は、移動量処理部２によって決定された移動量と、障害物検出部４によって取得された障害物情報とを利用して、自律走行装置の現在の位置を推定する部分であり、推定した位置情報や、自律走行装置の進行方向や移動速度など、自律走行装置の姿勢を制御するのに必要な情報（位置推定情報）を、車体姿勢制御部７に与える。
上記した障害物情報と、移動量処理部２によって選択された移動量（カメラ移動量または車輪移動量のいずれか）に加え、実際の位置が示された地図情報６を利用して、自律走行装置の現在の位置を推定し、現在位置のずれを検出する。
自己位置推定部５において実行される処理は、上記したようなパーティクルフィルタを使用した従来技術を利用すればよいので、詳細な説明は省略する。
上記した障害物情報と、移動量処理部２によって選択された移動量（カメラ移動量または車輪移動量のいずれか）に加え、実際の位置が示された地図情報６を利用して、自律走行装置の現在の位置を推定し、現在位置のずれを検出する。
自己位置推定部５において実行される処理は、上記したようなパーティクルフィルタを使用した従来技術を利用すればよいので、詳細な説明は省略する。
【0069】
地図情報６は、自律走行装置が実際に走行する経路等が示された地図であり、ROMなどの記憶素子や、ハードディスク等の記憶装置に予め記憶される。
地図情報６は、上記したように、自己位置推定部５で実行される自律走行装置の現在位置の推定処理で使用される。
地図情報６は、上記したように、自己位置推定部５で実行される自律走行装置の現在位置の推定処理で使用される。
【0070】
車体姿勢制御部７は、自己位置推定部５から出力される位置推定情報を利用して、自律走行装置の姿勢（進行方向や移動速度など）を調整し、変更する部分である。
車体姿勢制御部７において実行される処理も、従来技術を利用すればよいので、詳細な説明は省略する。
車体姿勢制御部７において実行される処理も、従来技術を利用すればよいので、詳細な説明は省略する。
【0071】
また、自律走行装置１００は、図示しない制御部を備え、移動量処理部２やＬＩＤＥＲ３などの各構成要素の動作を制御する。
制御部は、主として、ＣＰＵ，ＲＯＭ，ＲＡＭ，Ｉ／Ｏコントローラ，タイマー等からなるマイクロコンピュータによって実現される。
ＣＰＵは、ＲＯＭ等に予め格納された制御プログラムに基づいて、各種ハードウェアを有機的に動作させて、この発明の走行機能、障害物検出機能、自己位置推定機能などを実行する。
制御部は、主として、ＣＰＵ，ＲＯＭ，ＲＡＭ，Ｉ／Ｏコントローラ，タイマー等からなるマイクロコンピュータによって実現される。
ＣＰＵは、ＲＯＭ等に予め格納された制御プログラムに基づいて、各種ハードウェアを有機的に動作させて、この発明の走行機能、障害物検出機能、自己位置推定機能などを実行する。
【0072】
＜自律走行装置の車両位置制御処理＞
図２３に、この発明の自律走行装置に取り付けられた車両位置制御装置による車両位置制御処理の一実施例の概略フローチャートを示す。
自律走行装置の車両位置制御装置は、自律走行をしている状態において、車輪およびカメラ画像から移動量を取得し、さらに障害物情報を取得し、取得した移動量と障害物情報等を利用して現在の位置を推定し、推定した位置情報と地図情報とを利用して、車両姿勢を制御する。
図２３に、この発明の自律走行装置に取り付けられた車両位置制御装置による車両位置制御処理の一実施例の概略フローチャートを示す。
自律走行装置の車両位置制御装置は、自律走行をしている状態において、車輪およびカメラ画像から移動量を取得し、さらに障害物情報を取得し、取得した移動量と障害物情報等を利用して現在の位置を推定し、推定した位置情報と地図情報とを利用して、車両姿勢を制御する。
【0073】
図２３のステップS1において、カメラ移動量取得処理を行う。
ここでは、後述するカメラ移動量取得部２２が、カメラで撮影された画像を利用して、画像から抽出された複数の特徴点に関する情報から求めたカメラ移動量を取得する。
ステップS2において、車輪移動量取得を行う。
ここでは、後述する車輪移動量取得部２１が、回転数取得部２０によって取得された車輪の回転数を利用して、車輪に関する情報から求めた車輪移動量を取得する。
ここでは、後述するカメラ移動量取得部２２が、カメラで撮影された画像を利用して、画像から抽出された複数の特徴点に関する情報から求めたカメラ移動量を取得する。
ステップS2において、車輪移動量取得を行う。
ここでは、後述する車輪移動量取得部２１が、回転数取得部２０によって取得された車輪の回転数を利用して、車輪に関する情報から求めた車輪移動量を取得する。
【0074】
ステップS3において、停止判定処理を行う。
ここでは、後述する第１停止判定部２３と第２停止判定部２４が、取得されたカメラ移動量と車輪移動量を利用して、それぞれの所定の判定基準に基づいて、自律走行装置の静動判定処理を行う。すなわち、自律走行装置が停止状態にあるかまたは移動状態にあるかを判定する。
ここでは、後述する第１停止判定部２３と第２停止判定部２４が、取得されたカメラ移動量と車輪移動量を利用して、それぞれの所定の判定基準に基づいて、自律走行装置の静動判定処理を行う。すなわち、自律走行装置が停止状態にあるかまたは移動状態にあるかを判定する。
【0075】
ステップS4において、移動量選択処理を行う。
ここでは、後述する移動量選択部２５が、静動判定処理の結果に基づいて、車輪移動量とカメラ移動量のうち、位置推定処理に利用する移動量を選択する。
ステップS5において、選択移動量出力処理を行う。
ここでは、後述する選択移動量出力部２６が、選択された移動量（車輪移動量とカメラ移動量のうちどちらか）を、自己位置推定部５に出力する。
ここでは、後述する移動量選択部２５が、静動判定処理の結果に基づいて、車輪移動量とカメラ移動量のうち、位置推定処理に利用する移動量を選択する。
ステップS5において、選択移動量出力処理を行う。
ここでは、後述する選択移動量出力部２６が、選択された移動量（車輪移動量とカメラ移動量のうちどちらか）を、自己位置推定部５に出力する。
【0076】
ステップS6において、障害物情報取得処理を行う。
ここでは、障害物検出部４が、ＬＩＤＥＲ３等によって測定された物体までの距離情報を利用して、自律走行装置の周辺に存在する障害物を検出し、その障害物に関する障害物情報を取得する。
取得された障害物情報は、自己位置推定部５に出力される。
ここでは、障害物検出部４が、ＬＩＤＥＲ３等によって測定された物体までの距離情報を利用して、自律走行装置の周辺に存在する障害物を検出し、その障害物に関する障害物情報を取得する。
取得された障害物情報は、自己位置推定部５に出力される。
【0077】
ステップS7において、位置推定処理を行う。
ここでは、自己位置推定部５が、自己位置推定部５に入力された選択移動量と、障害物検出部４によって取得された障害物情報と、所定の地図情報６とを利用して、自律走行装置の現在の位置を推定する。推定された現在の位置に関する情報が、車両姿勢制御部７に出力される。
ここでは、自己位置推定部５が、自己位置推定部５に入力された選択移動量と、障害物検出部４によって取得された障害物情報と、所定の地図情報６とを利用して、自律走行装置の現在の位置を推定する。推定された現在の位置に関する情報が、車両姿勢制御部７に出力される。
【0078】
ステップS8において、車両姿勢制御処理を行う。
ここでは、車両姿勢制御部７が、推定された現在の位置に関する情報から、自律走行装置の姿勢、たとえば、自律走行装置の進行方向や移動速度を調整または変更する。
ここでは、車両姿勢制御部７が、推定された現在の位置に関する情報から、自律走行装置の姿勢、たとえば、自律走行装置の進行方向や移動速度を調整または変更する。
【0079】
上記したステップS1からステップS5までの処理（カメラ移動量取得処理、車輪移動量取得処理、停止判定処理、移動量選択処理、選択移動量出力処理）は、移動量処理部２によって行われる。
【0080】
＜移動量処理部の実施例１＞
（移動量処理部の構成）
図２に、この発明の自律走行装置の移動量処理部の一実施例の構成ブロック図を示す。
図２において、移動量処理部２は、主として、回転数取得部２０、車輪移動量取得部２１、カメラ移動量取得部２２、第１停止判定部２３、第２停止判定部２４、移動量選択部２５、選択移動量出力部２６を備える。
（移動量処理部の構成）
図２に、この発明の自律走行装置の移動量処理部の一実施例の構成ブロック図を示す。
図２において、移動量処理部２は、主として、回転数取得部２０、車輪移動量取得部２１、カメラ移動量取得部２２、第１停止判定部２３、第２停止判定部２４、移動量選択部２５、選択移動量出力部２６を備える。
【0081】
回転数取得部２０は、車輪の回転数を取得するものであり、エンコーダーに相当する。
取得した車輪の回転数は、車輪移動量取得部２１に与えられる。
差動駆動方式の場合は、左右の車輪に取り付けられたエンコーダーから、それぞれの回転数を取得し、回転数を平均したものを、車輪移動量取得部２１に出力する。
取得した車輪の回転数は、車輪移動量取得部２１に与えられる。
差動駆動方式の場合は、左右の車輪に取り付けられたエンコーダーから、それぞれの回転数を取得し、回転数を平均したものを、車輪移動量取得部２１に出力する。
【0082】
車輪移動量取得部２１は、回転数取得部２０によって取得された車輪の回転数を利用して、車輪の移動量（車輪移動量）を取得する部分である。
車輪移動量として移動速度を取得する場合、車輪の回転数と、車輪の直径と、測定時間とを用いて、自律走行装置の移動速度に相当する車輪移動量（車輪速度：V1）を計算する。
取得された車輪移動量は、第１停止判定部２３と選択移動量出力部２６に与えられる。
車輪移動量として移動速度を取得する場合、車輪の回転数と、車輪の直径と、測定時間とを用いて、自律走行装置の移動速度に相当する車輪移動量（車輪速度：V1）を計算する。
取得された車輪移動量は、第１停止判定部２３と選択移動量出力部２６に与えられる。
【0083】
カメラ移動量取得部２２は、カメラによって撮影された画像の画像データ（カメラ画像）を利用して、カメラの移動量（カメラ移動量）を取得する部分である。
カメラ移動量として移動速度を取得する場合、カメラ画像に含まれる所定の複数の特徴点を抽出し、特徴点の動き（変位量）や、カメラの光学モデル、カメラの回転を計測するIMUなどを用いて、自律走行装置の移動速度に相当するカメラ移動量（カメラ速度：V2）を計算する。
カメラ移動量として移動速度を取得する場合、カメラ画像に含まれる所定の複数の特徴点を抽出し、特徴点の動き（変位量）や、カメラの光学モデル、カメラの回転を計測するIMUなどを用いて、自律走行装置の移動速度に相当するカメラ移動量（カメラ速度：V2）を計算する。
【0084】
抽出される特徴点は、カメラ画像の中に存在する物体の輪郭や角部分に相当する画素であり、回転や拡大・縮小しても変わらず検出できるものである。複数ある特徴点の配置と、その時間変化から逆算してカメラの動きを推定することができる。
取得されたカメラ移動量は、第２停止判定部２４と選択移動量出力部２６に与えられる。
取得されたカメラ移動量は、第２停止判定部２４と選択移動量出力部２６に与えられる。
【0085】
第１停止判定部２３は、自律走行装置の静動判定をする部分であり、車輪移動量（車輪速度：V1）を利用して、自律走行装置が移動中かあるいは停止中かを判定する部分である。
静動判定は、予め記憶されている停止判定値（速度）V10を利用して行う。
停止判定値（速度）V10は、ゼロではなく、ギアの遊び等により停止していてもわずかな動きが検出される場合があるため、小さな数値が設定される。
車輪移動量である車輪速度V1が、停止判定値V10よりも小さい場合（V1＜V10）、車輪による判断では自律走行装置が停止中であると判定し、それ以外の場合は、車輪による判断では自律走行装置が移動中であると判定する。
この判定結果は、移動量選択部２５に与えられる。
静動判定は、予め記憶されている停止判定値（速度）V10を利用して行う。
停止判定値（速度）V10は、ゼロではなく、ギアの遊び等により停止していてもわずかな動きが検出される場合があるため、小さな数値が設定される。
車輪移動量である車輪速度V1が、停止判定値V10よりも小さい場合（V1＜V10）、車輪による判断では自律走行装置が停止中であると判定し、それ以外の場合は、車輪による判断では自律走行装置が移動中であると判定する。
この判定結果は、移動量選択部２５に与えられる。
【0086】
第２停止判定部２４は、第１停止判定部２３と同様に、自律走行装置の静動判定をする部分であり、カメラ移動量（カメラ速度：V2）を利用して、自律走行装置が移動中かあるいは停止中かを判定する部分である。
静動判定は、予め記憶されている停止判定値（速度）V20を利用して行う。
停止判定値（速度）V20は、ゼロではなく、わずかな明度の変化や被写体の動き等により停止していてもわずかな動きが検出される場合があるため、小さな数値が設定される。
カメラ移動量であるカメラ速度V2が、停止判定値V20よりも小さい場合（V1＜V20）、カメラによる判断では自律走行装置が停止中であると判定し、それ以外の場合は、カメラによる判断では自律走行装置が移動中であると判定する。
この判定結果は、移動量選択部２５に与えられる。
静動判定は、予め記憶されている停止判定値（速度）V20を利用して行う。
停止判定値（速度）V20は、ゼロではなく、わずかな明度の変化や被写体の動き等により停止していてもわずかな動きが検出される場合があるため、小さな数値が設定される。
カメラ移動量であるカメラ速度V2が、停止判定値V20よりも小さい場合（V1＜V20）、カメラによる判断では自律走行装置が停止中であると判定し、それ以外の場合は、カメラによる判断では自律走行装置が移動中であると判定する。
この判定結果は、移動量選択部２５に与えられる。
【0087】
移動量選択部２５は、第１停止判定部２３と第２停止判定部２４で行われた２つの静動判定の結果から、自律走行装置の自己位置推定に利用する移動量を選択する部分である。
車輪移動量とカメラ移動量のうち、どちらの移動量を自己位置推定処理に利用するのが適切かを判断し、適切と判断されたほうの移動量を、自己位置推定に利用する移動量として選択する。
たとえば、判断基準となる後述するような移動量判定選択情報を予め設定記憶しておき、この判断基準に基づいて、車輪移動量とカメラ移動量のうち、いずれかの移動量を選択する。
選択された移動量に関する情報は、選択移動量出力部２６に与えられる。
車輪移動量とカメラ移動量のうち、どちらの移動量を自己位置推定処理に利用するのが適切かを判断し、適切と判断されたほうの移動量を、自己位置推定に利用する移動量として選択する。
たとえば、判断基準となる後述するような移動量判定選択情報を予め設定記憶しておき、この判断基準に基づいて、車輪移動量とカメラ移動量のうち、いずれかの移動量を選択する。
選択された移動量に関する情報は、選択移動量出力部２６に与えられる。
【0088】
自律走行装置の通常の走行状態では車輪の静動判定とカメラの静動判定は一致するはずである。
しかし、自律走行装置を持ち上げて移動した時には、車輪の静動判定は停止中で、カメラの静動判定は移動中という状態になる。
一方、車輪が空転している時には、車輪の静動判定は移動中で、カメラの静動判定は停止中という状態になる。
しかし、自律走行装置を持ち上げて移動した時には、車輪の静動判定は停止中で、カメラの静動判定は移動中という状態になる。
一方、車輪が空転している時には、車輪の静動判定は移動中で、カメラの静動判定は停止中という状態になる。
【0089】
一般的に、車輪移動量による静動判定の結果とカメラ移動量による静動判定の結果が一致している場合は、車輪移動量のほうがカメラ移動量よりも信頼性が高いと考えられるので、絶対的な移動量を計測できる車輪移動量を、自己位置推定に利用する移動量に選択する。
一方、車輪移動量による静動判定の結果とカメラ移動量による静動判定の結果が一致していない場合は、車輪移動量の信頼性が低いと考えられるので、カメラ移動量を、自己位置推定に利用する移動量に選択する。
一方、車輪移動量による静動判定の結果とカメラ移動量による静動判定の結果が一致していない場合は、車輪移動量の信頼性が低いと考えられるので、カメラ移動量を、自己位置推定に利用する移動量に選択する。
【0090】
図９に、車輪速度とカメラ速度との関係と、移動量の選択速度領域の一実施例の説明図を示す。
図１０に、車輪速度とカメラ速度を利用した移動量判定選択情報の一実施例の説明図を示す。
図９において、横軸に車輪速度V1を示し、縦軸にカメラ速度V2を示し、２つの停止判定値（V10、V20）によって区別される移動量の選択速度領域（A1、A2、B1、B2）を示している。
図１０に、車輪速度とカメラ速度を利用した移動量判定選択情報の一実施例の説明図を示す。
図９において、横軸に車輪速度V1を示し、縦軸にカメラ速度V2を示し、２つの停止判定値（V10、V20）によって区別される移動量の選択速度領域（A1、A2、B1、B2）を示している。
【0091】
図９において、車輪速度V1が、停止判定値V10よりも小さい場合（V1＜V10）、自律走行装置は停止中と判断され、それ以外の場合（V1≧V10）は、自律走行装置は移動中と判断されるものとする。
また、カメラ速度V2が、停止判定値V20よりも小さい場合（V2＜V20）、自律走行装置は停止中と判断され、それ以外の場合（V2≧V20）は、自律走行装置は移動中と判断されるものとする。
以下に示すように、選択速度領域A1とA2は、車輪移動量を選択する範囲であり、選択速度領域B1とB2は、カメラ移動量を選択する範囲である。
また、カメラ速度V2が、停止判定値V20よりも小さい場合（V2＜V20）、自律走行装置は停止中と判断され、それ以外の場合（V2≧V20）は、自律走行装置は移動中と判断されるものとする。
以下に示すように、選択速度領域A1とA2は、車輪移動量を選択する範囲であり、選択速度領域B1とB2は、カメラ移動量を選択する範囲である。
【0092】
図１０において、２つの移動量のうちいずれかを選択するための判断基準となる移動量判定選択情報を示しており、ここでは、図９の選択速度領域に対応した４つの基準を示している。
図１０の移動量判定選択情報では、取得移動量である車輪速度V1とカメラ速度V2の条件に対応づけて、選択すべき使用移動量が、予め設定されている。
図１０の移動量判定選択情報では、取得移動量である車輪速度V1とカメラ速度V2の条件に対応づけて、選択すべき使用移動量が、予め設定されている。
【0093】
たとえば、図１０の基準１の「V1＜V10」と「V2＜V20」が満たされる場合と、基準２の「V1≧V10」と「V2≧V20」が満たされる場合では、車輪移動量とカメラ移動量のうち、車輪移動量（車輪速度V1）を選択する。
基準１の「V1＜V10」と「V2＜V20」で示す領域は、図９の選択速度領域のA1に相当する。
基準２の「V1≧V10」と「V2≧V20」で示す領域は、図９の選択速度領域のA2に相当する。
これらの選択速度領域（A1、A2）では、２つの静動判定結果がほぼ一致していると考えて、信頼性の高い車輪移動量（車輪速度V1）を選択する。
基準１の「V1＜V10」と「V2＜V20」で示す領域は、図９の選択速度領域のA1に相当する。
基準２の「V1≧V10」と「V2≧V20」で示す領域は、図９の選択速度領域のA2に相当する。
これらの選択速度領域（A1、A2）では、２つの静動判定結果がほぼ一致していると考えて、信頼性の高い車輪移動量（車輪速度V1）を選択する。
【0094】
また、図１０の基準３の「V1＜V10」と「V2≧V20」が満たされる場合と、基準４の「V1≧V10」と「V2＜V20」が満たされる場合では、車輪移動量とカメラ移動量のうち、カメラ移動量（カメラ速度V2）を選択する。
基準３の「V1＜V10」と「V2≧V20」で示す領域は、図９の選択速度領域のB1に相当する。
基準４の「V1≧V10」と「V2＜V20」で示す領域は、図９の選択速度領域のB2に相当する。
これらの選択速度領域（B1、B2）では、２つの静動判定結果が一致していないので、車輪移動量の信頼性が低いと考えて、カメラ移動量（カメラ速度V2）を選択する。
基準３の「V1＜V10」と「V2≧V20」で示す領域は、図９の選択速度領域のB1に相当する。
基準４の「V1≧V10」と「V2＜V20」で示す領域は、図９の選択速度領域のB2に相当する。
これらの選択速度領域（B1、B2）では、２つの静動判定結果が一致していないので、車輪移動量の信頼性が低いと考えて、カメラ移動量（カメラ速度V2）を選択する。
【0095】
以上のように、通常の走行状態では、２つの静動判定結果がほぼ一致しており、車輪による移動量（車輪速度V1）が使用されるので、従来と同様の精度での自己位置推定が行うことができる。
一方、車体が持ち上げられたり、車輪が空転したりした場合は、２つの静動判定結果が一致しなくなり、車輪による移動量の精度が著しく低くなるので、カメラ画像による移動量（カメラ速度V2）を使用するものとし、カメラ移動量（カメラ速度V2）を利用して自己位置推定処理を継続することによって、自己位置推定が破たんすることを防ぐことができる。
一方、車体が持ち上げられたり、車輪が空転したりした場合は、２つの静動判定結果が一致しなくなり、車輪による移動量の精度が著しく低くなるので、カメラ画像による移動量（カメラ速度V2）を使用するものとし、カメラ移動量（カメラ速度V2）を利用して自己位置推定処理を継続することによって、自己位置推定が破たんすることを防ぐことができる。
【0096】
選択移動量出力部２６は、車輪移動量とカメラ移動量のうち、移動量選択部２５によって選択された移動量を、自己位置推定部５に出力する部分である。
すなわち、移動量選択部２５によって、車輪移動量（車輪速度V1）が選択された場合は、車輪移動量（車輪速度V1）を自己位置推定部５に出力し、カメラ移動量（カメラ速度V2）が選択された場合は、カメラ移動量（カメラ速度V2）を自己位置推定部５に出力する。
上記した自己位置推定部５が、この選択移動量出力部２６から出力された移動量と、障害物情報とを利用して自己位置推定を行う。
すなわち、移動量選択部２５によって、車輪移動量（車輪速度V1）が選択された場合は、車輪移動量（車輪速度V1）を自己位置推定部５に出力し、カメラ移動量（カメラ速度V2）が選択された場合は、カメラ移動量（カメラ速度V2）を自己位置推定部５に出力する。
上記した自己位置推定部５が、この選択移動量出力部２６から出力された移動量と、障害物情報とを利用して自己位置推定を行う。
【0097】
以上のように、実施例１では、車輪移動量による静動判定の結果とカメラ移動量による静動判定の結果が一致している場合は、車輪移動量を、自己位置推定に利用する移動量に選択し、車輪移動量による静動判定の結果とカメラ移動量による静動判定の結果が一致していない場合は、カメラ移動量を、自己位置推定に利用する移動量に選択する。
したがって、たとえば、一時的にロボットを持ち上げて移動したり、障害物に乗り上げてロボットの車輪が空転したりして車輪移動量が不正になった場合でも、カメラ移動量で補間することにより、安定した自己位置推定を継続することが可能になる。
したがって、たとえば、一時的にロボットを持ち上げて移動したり、障害物に乗り上げてロボットの車輪が空転したりして車輪移動量が不正になった場合でも、カメラ移動量で補間することにより、安定した自己位置推定を継続することが可能になる。
【0098】
（移動量処理部の移動量判定選択処理）
図２４に、移動量処理部における移動量判定選択処理の一実施例のフローチャートを示す。
図２４のステップS11において、カメラ移動量取得部２２が、カメラ移動量（カメラ速度V2）を取得する。
ステップS12において、車輪移動量取得部２１が、車輪移動量（車輪速度V1）を取得する。
図２４に、移動量処理部における移動量判定選択処理の一実施例のフローチャートを示す。
図２４のステップS11において、カメラ移動量取得部２２が、カメラ移動量（カメラ速度V2）を取得する。
ステップS12において、車輪移動量取得部２１が、車輪移動量（車輪速度V1）を取得する。
【0099】
以下のステップS13からステップS19において、図１０の移動量判定選択情報に示した判断基準に基づいて、移動量を選択する。
ステップS13において、V1＜V10の場合、ステップS14に進み、そうでない場合は、ステップS17に進む。
ステップS14において、V2＜V20の場合、ステップS15に進み、そうでない場合は、ステップS16に進む。
ステップS17において、V2＜V20の場合、ステップS19に進み、そうでない場合は、ステップS18に進む。
ステップS15とステップS18において、車輪移動量（車輪速度V1）を、選択する。
ステップS16とステップS19において、カメラ移動量（カメラ速度V2）を、選択する。
以上のようにして、移動量の停止判定と選択処理が行われる。
ステップS13において、V1＜V10の場合、ステップS14に進み、そうでない場合は、ステップS17に進む。
ステップS14において、V2＜V20の場合、ステップS15に進み、そうでない場合は、ステップS16に進む。
ステップS17において、V2＜V20の場合、ステップS19に進み、そうでない場合は、ステップS18に進む。
ステップS15とステップS18において、車輪移動量（車輪速度V1）を、選択する。
ステップS16とステップS19において、カメラ移動量（カメラ速度V2）を、選択する。
以上のようにして、移動量の停止判定と選択処理が行われる。
【0100】
＜移動量処理部の実施例２＞
（移動量処理部の構成）
図３に、この発明の自律走行装置の移動量処理部の一実施例の構成ブロック図を示す。
図３の移動量処理部は、図２の移動量処理部と比較して、車輪移動量履歴保存部３１と時刻監視部３２が追加される点が異なり、その他の構成は、図２の移動量処理部と同じ構成を有する。
（移動量処理部の構成）
図３に、この発明の自律走行装置の移動量処理部の一実施例の構成ブロック図を示す。
図３の移動量処理部は、図２の移動量処理部と比較して、車輪移動量履歴保存部３１と時刻監視部３２が追加される点が異なり、その他の構成は、図２の移動量処理部と同じ構成を有する。
【0101】
車輪移動量履歴保存部３１は、車輪移動量取得部２１によって取得された車輪移動量のうち、所定の時間分の複数の車輪移動量を保存するバッファである。
車輪移動量（車輪速度V1）は、回転数から即時的に算出することができ、たとえば、数十ミリ秒程度の一定時間間隔で取得される。
しかし、カメラ移動量（カメラ速度V2）は、カメラ画像を利用して、特徴点の抽出や連続する画像間の比較等の複雑な処理が必要なので、カメラ移動量の計算に比較的長時間を必要とし、得られたカメラ画像によって算出にかかる時間も変化する。
車輪移動量（車輪速度V1）は、回転数から即時的に算出することができ、たとえば、数十ミリ秒程度の一定時間間隔で取得される。
しかし、カメラ移動量（カメラ速度V2）は、カメラ画像を利用して、特徴点の抽出や連続する画像間の比較等の複雑な処理が必要なので、カメラ移動量の計算に比較的長時間を必要とし、得られたカメラ画像によって算出にかかる時間も変化する。
【0102】
したがって、一般的に、カメラ移動量が取得される間隔は一定間隔ではなく、カメラ移動量が取得される時刻は、車輪移動量の取得時刻よりも遅延する。
もし、第１停止判定部２３と第２停止判定部２４において、取得時刻が一致しない車輪移動量とカメラ移動量を利用して、静動判定をした場合には、正確な静動判定ができない。
もし、第１停止判定部２３と第２停止判定部２４において、取得時刻が一致しない車輪移動量とカメラ移動量を利用して、静動判定をした場合には、正確な静動判定ができない。
【0103】
そこで、第１停止判定部２３と第２停止判定部２４での停止判定で、時刻が一致した車輪移動量とカメラ移動量を利用して静動判定をするために、バッファ３１に、所定時間分の複数の車輪移動量（車輪速度V1）の履歴を保存しておく。
【0104】
また、時刻監視部３２は、カメラ移動量取得部２２によって取得されたカメラ移動量の取得時刻（以下、計算時刻T2と呼ぶ）を監視する部分であり、カメラ移動量の取得された時刻とほぼ同じ時刻に取得された車輪移動量（車輪速度V1）が、バッファ３１から第１停止判定部２３に出力されるように、バッファ３１を制御する。
【0105】
たとえば、時刻監視部３２が、カメラ移動量取得部２２から、カメラ移動量の取得された時刻（計算時刻T2）を取得し、その計算時刻T2の車輪移動量（車輪速度V1）が、バッファ３１に保存されているか否かを、バッファ３１に問い合わせる。
バッファ３１に、計算時刻T2の車輪移動量（車輪速度V1）が保存されている場合は、その車輪移動量（車輪速度V1）を、バッファ３１から第１停止判定部２３に出力する。
同時に、計算時刻T2に取得されたカメラ移動量を、第２停止判定部２４に出力する。
バッファ３１に、計算時刻T2の車輪移動量（車輪速度V1）が保存されている場合は、その車輪移動量（車輪速度V1）を、バッファ３１から第１停止判定部２３に出力する。
同時に、計算時刻T2に取得されたカメラ移動量を、第２停止判定部２４に出力する。
【0106】
これにより、同じ時刻に取得された車輪移動量（車輪速度V1）とカメラ移動量（カメラ速度V2）が、それぞれ、第１停止判定部２３と第２停止判定部２４に出力される。
時刻の一致した車輪移動量とカメラ移動量を利用して、静動判定を行うことにより、より精度の高い移動量の選択が可能となり、さらに、自己位置の推定の精度も向上させることができる。
時刻の一致した車輪移動量とカメラ移動量を利用して、静動判定を行うことにより、より精度の高い移動量の選択が可能となり、さらに、自己位置の推定の精度も向上させることができる。
【0107】
なお、カメラ移動量の計算における遅延時間量がさらに大きくなった場合（たとえば、数秒～十数秒程度以上の時間遅延）、現実の自律走行装置の動きに対して実時間性が成立しなくなり、自己位置推定の制御が不可能になる。
このような場合にはカメラ移動量の計算をリセットし、強制的に、選択する移動量を、車輪移動量（車輪速度V1）に切り替えることが好ましい。車輪移動量（車輪速度V1）を使用することで、自己位置推定処理を安定化することができる。
このような場合にはカメラ移動量の計算をリセットし、強制的に、選択する移動量を、車輪移動量（車輪速度V1）に切り替えることが好ましい。車輪移動量（車輪速度V1）を使用することで、自己位置推定処理を安定化することができる。
【0108】
また、カメラ移動量を使用している状態で、カメラ移動量の計算がリセットされた場合には、管理担当者等に、自己位置エラーを通知して、自己位置推定処理を停止してもよい。
これにより、不安定な自己位置に基づいて自律走行装置が動作することによって、事故が発生する危険を低減することができる。
これにより、不安定な自己位置に基づいて自律走行装置が動作することによって、事故が発生する危険を低減することができる。
【0109】
以上のように、実施例２では、取得された車輪移動量のうち、所定の時間分の複数の車輪移動量を保存しておき、取得されたカメラ移動量の取得時刻を監視し、カメラ移動量の取得時刻とほぼ同じ時刻に取得された車輪移動量が第１停止判定部に出力されるようにするので、ほぼ同一時刻のデータによって静動判定を行うことができ、精度の高い安定した自己位置推定を継続することが可能になる。
【0110】
（移動量処理部の移動量判定選択処理）
図２５に、移動量処理部における移動量判定選択処理の一実施例のフローチャートを示す。
図２５において、図２４のステップと同じ処理をするステップには、同じ番号を付与している。
図２５に、移動量処理部における移動量判定選択処理の一実施例のフローチャートを示す。
図２５において、図２４のステップと同じ処理をするステップには、同じ番号を付与している。
【0111】
図２５のステップS12において、車輪移動量取得部２１が、車輪移動量（車輪速度V1）を取得する。このとき、一定時間分の車輪移動量が、取得時刻と対応付けて、バッファ３１に保存される。
ステップS31において、カメラ移動量取得部２２が、カメラ移動量（カメラ速度V2）と、そのカメラ移動量を取得した時刻（計算時刻T2）とを対応付けて取得する。
ステップS32において、計算時刻T2の車輪移動量の有無を、バッファ３１に問い合わせる。
バッファ３１では、この問合せに対して、保存されている車輪移動量の中に、計算時刻T2に対応づけられた車輪移動量が存在するか否かをチェックする。
ステップS31において、カメラ移動量取得部２２が、カメラ移動量（カメラ速度V2）と、そのカメラ移動量を取得した時刻（計算時刻T2）とを対応付けて取得する。
ステップS32において、計算時刻T2の車輪移動量の有無を、バッファ３１に問い合わせる。
バッファ３１では、この問合せに対して、保存されている車輪移動量の中に、計算時刻T2に対応づけられた車輪移動量が存在するか否かをチェックする。
【0112】
ステップS33において、計算時刻T2の車輪移動量が存在する場合は、ステップS35に進み、そうでない場合は、ステップS34に進む。
ステップS35において、バッファ３１から、計算時刻T2の車輪移動量（車輪速度V1）を第１停止判定部２３に出力し、ステップS13に進む。このとき、計算時刻T2のカメラ移動量（カメラ速度V2）も、第２停止判定部２４に出力される。
ステップS34において、カメラ移動量取得部２２におけるカメラ移動量の計算処理をリセットし、ステップS15に進む。
ステップS35において、バッファ３１から、計算時刻T2の車輪移動量（車輪速度V1）を第１停止判定部２３に出力し、ステップS13に進む。このとき、計算時刻T2のカメラ移動量（カメラ速度V2）も、第２停止判定部２４に出力される。
ステップS34において、カメラ移動量取得部２２におけるカメラ移動量の計算処理をリセットし、ステップS15に進む。
【0113】
以下のステップS13からステップS19において、図２４と同様に、図１０の移動量判定選択情報に示した判断基準に基づいて、移動量を選択する。
すなわち、ステップS15とステップS18において、車輪移動量（車輪速度V1）を、選択する。
ステップS16とステップS19において、カメラ移動量（カメラ速度V2）を、選択する。
以上のようにして、移動量の停止判定と選択処理が行われる。
すなわち、ステップS15とステップS18において、車輪移動量（車輪速度V1）を、選択する。
ステップS16とステップS19において、カメラ移動量（カメラ速度V2）を、選択する。
以上のようにして、移動量の停止判定と選択処理が行われる。
【0114】
＜移動量処理部の実施例３＞
（移動量処理部の構成）
図４に、この発明の自律走行装置の移動量処理部の一実施例の構成ブロック図を示す。
図４の移動量処理部は、図２の移動量処理部と比較して、カメラ特徴点監視部３５と、カメラ移動量無効部３６が追加される点が異なり、その他の構成は、図２の移動量処理部と同じ構成を有する。
（移動量処理部の構成）
図４に、この発明の自律走行装置の移動量処理部の一実施例の構成ブロック図を示す。
図４の移動量処理部は、図２の移動量処理部と比較して、カメラ特徴点監視部３５と、カメラ移動量無効部３６が追加される点が異なり、その他の構成は、図２の移動量処理部と同じ構成を有する。
【0115】
カメラ特徴点監視部３５は、カメラ移動量取得部２２がカメラ移動量を取得するために、カメラで撮影された画像の画像データから抽出された複数の特徴点に関する情報を利用する場合に、特徴点の数を取得する部分である。
特徴点の数（以下、SPとも呼ぶ）は、取得されたカメラ移動量の信頼性に関係する。
特徴点の数（以下、SPとも呼ぶ）は、取得されたカメラ移動量の信頼性に関係する。
【0116】
上記したように、カメラ移動量取得部２２は、カメラで撮影されたカメラ画像に含まれる物体の複数の特徴点を利用して、カメラ移動量を取得するが、特徴点の数が多いほうが、正確なカメラ移動量を取得することができる。
抽出される特徴点の数は、撮影される空間の状況によって変化し、たとえば、撮影されたカメラ画像の画面全体が白飛びしたり被写体ブレがひどかったりする場合には、抽出される特徴点が少なくなる。特徴点が少なくなると、特徴点の動きを追跡するのが困難になる場合があり、特徴点の動き等を利用して計算されるカメラ移動量が不正確になる可能性がある。
抽出される特徴点の数は、撮影される空間の状況によって変化し、たとえば、撮影されたカメラ画像の画面全体が白飛びしたり被写体ブレがひどかったりする場合には、抽出される特徴点が少なくなる。特徴点が少なくなると、特徴点の動きを追跡するのが困難になる場合があり、特徴点の動き等を利用して計算されるカメラ移動量が不正確になる可能性がある。
【0117】
カメラ移動量無効部３６は、カメラ特徴点監視部３５によって取得された特徴点の数に基づいて、取得されたカメラ移動量（カメラ速度V2）を無効化する部分である。
カメラ移動量を無効化する基準として、特徴点の数SPと比較する判定比較値SP0を予め記憶しておく。
取得された特徴点の数SPと、判定比較値SP0とを比較し、特徴点の数SPが、所定の判定比較値SP0よりも小さい場合（SP＜SP0）に、取得されたカメラ移動量（カメラ速度V2）を無効化し、移動量選択部２５がカメラ移動量（カメラ速度V2）を選択しないようにする。
カメラ移動量を無効化する基準として、特徴点の数SPと比較する判定比較値SP0を予め記憶しておく。
取得された特徴点の数SPと、判定比較値SP0とを比較し、特徴点の数SPが、所定の判定比較値SP0よりも小さい場合（SP＜SP0）に、取得されたカメラ移動量（カメラ速度V2）を無効化し、移動量選択部２５がカメラ移動量（カメラ速度V2）を選択しないようにする。
【0118】
カメラ移動量の無効化は、カメラ移動量（カメラ速度V2）を採用しないことを意味し、言いかえれば、カメラ移動量取得部２２によるカメラ移動量の計算をリセットすることを意味する。
SP＜SP0であった場合、たとえば、カメラ移動量無効部３６から、移動量選択部２５あるいは、選択移動量出力部２６に対して、カメラ移動量を選択しないこと、あるいはカメラ移動量を出力しないことを意味する無効化信号を与える。
無効化信号を受け取った移動量選択部２５は、カメラ移動量（カメラ速度V2）の選択をやめ、車輪移動量（車輪速度V1）に切り替え、選択移動量出力部２６からは、車輪移動量（車輪速度V1）が出力される。
このように、不正確になった可能性があるカメラ移動量を無効化することによって、自己位置推定処理が不安定になることを防ぐことができる。
SP＜SP0であった場合、たとえば、カメラ移動量無効部３６から、移動量選択部２５あるいは、選択移動量出力部２６に対して、カメラ移動量を選択しないこと、あるいはカメラ移動量を出力しないことを意味する無効化信号を与える。
無効化信号を受け取った移動量選択部２５は、カメラ移動量（カメラ速度V2）の選択をやめ、車輪移動量（車輪速度V1）に切り替え、選択移動量出力部２６からは、車輪移動量（車輪速度V1）が出力される。
このように、不正確になった可能性があるカメラ移動量を無効化することによって、自己位置推定処理が不安定になることを防ぐことができる。
【0119】
また、カメラ移動量を使用している状態で、特徴点数SPが、所定の判定比較値SP0よりも小さくなった場合には、管理担当者等に、自己位置エラーを通知して、自己位置推定処理を停止してもよい。
これにより、不安定な自己位置に基づいて自律走行装置が動作することによって、事故が発生する危険を低減することができる。
これにより、不安定な自己位置に基づいて自律走行装置が動作することによって、事故が発生する危険を低減することができる。
【0120】
以上のように、取得された特徴点の数が所定の判定比較値よりも小さい場合に、カメラ移動量を無効化し、移動量選択部がカメラ移動量を選択しないようにするので、カメラ移動量の信頼性が悪くなる状況でも、精度の高い安定した自己位置推定を継続することができる。
【0121】
（移動量処理部の移動量判定選択処理）
図２６に、移動量処理部における移動量判定選択処理の一実施例のフローチャートを示す。
図２６において、図２４や図２５のステップと同じ処理をするステップには、同じ番号を付与している。
図２６に、移動量処理部における移動量判定選択処理の一実施例のフローチャートを示す。
図２６において、図２４や図２５のステップと同じ処理をするステップには、同じ番号を付与している。
【0122】
図２６のステップS12において、車輪移動量取得部２１が、車輪移動量（車輪速度V1）を取得する。
ステップS41において、カメラ移動量取得部２２が、カメラ移動量（カメラ速度V2）と、特徴点数SPとを取得する。
ステップS41において、カメラ移動量取得部２２が、カメラ移動量（カメラ速度V2）と、特徴点数SPとを取得する。
【0123】
ステップS42において、取得された特徴点の数SPと、判定比較値SP0とを比較する。
ステップS43において、SP＜SP0であった場合、ステップS34に進み、そうでない場合は、ステップS44に進む。
ステップS43において、SP＜SP0であった場合、ステップS34に進み、そうでない場合は、ステップS44に進む。
【0124】
ステップS44において、車輪移動量（車輪速度V1）とカメラ移動量（カメラ速度V2）を取得し、ステップS13に進む。
ステップS34において、カメラ移動量取得部２２におけるカメラ移動量の計算処理をリセットし、ステップS15に進む。
ステップS34において、カメラ移動量取得部２２におけるカメラ移動量の計算処理をリセットし、ステップS15に進む。
【0125】
以下のステップS13からステップS19において、図２４と同様に、図１０の移動量判定選択情報に示した判断基準に基づいて、移動量を選択する。
すなわち、ステップS15とステップS18において、車輪移動量（車輪速度V1）を、選択する。
ステップS16とステップS19において、カメラ移動量（カメラ速度V2）を、選択する。
以上のようにして、移動量の停止判定と選択処理が行われる。
すなわち、ステップS15とステップS18において、車輪移動量（車輪速度V1）を、選択する。
ステップS16とステップS19において、カメラ移動量（カメラ速度V2）を、選択する。
以上のようにして、移動量の停止判定と選択処理が行われる。
【0126】
＜移動量処理部の実施例４＞
（移動量処理部の構成）
図５に、この発明の自律走行装置の移動量処理部の一実施例の構成ブロック図を示す。
図５の移動量処理部は、図２の移動量処理部と比較して、慣性計測装置４１と、慣性情報取得部４２と、カメラ移動量無効部３６が追加される点が異なり、その他の構成は、図２の移動量処理部と同じ構成を有する。
ここでは、慣性計測装置４１によって、車体に加えられた衝撃を計測し、衝撃の大きさに基づいて、適切な移動量を選択するようにして、自己位置推定の精度を向上させる。
（移動量処理部の構成）
図５に、この発明の自律走行装置の移動量処理部の一実施例の構成ブロック図を示す。
図５の移動量処理部は、図２の移動量処理部と比較して、慣性計測装置４１と、慣性情報取得部４２と、カメラ移動量無効部３６が追加される点が異なり、その他の構成は、図２の移動量処理部と同じ構成を有する。
ここでは、慣性計測装置４１によって、車体に加えられた衝撃を計測し、衝撃の大きさに基づいて、適切な移動量を選択するようにして、自己位置推定の精度を向上させる。
【0127】
慣性計測装置４１（IMU: Inertial Measurement Unit）は、自律走行装置の車体に加えられた衝撃に対応する加速度と角速度を計測するものである。
たとえば、３軸のジャイロと３方向の加速度計によって、３次元の加速度VAと角速度VBを計測する。加速度VAと角速度VBを、慣性情報と呼ぶ。
慣性情報取得部４２は、慣性計測装置４１で計測された慣性情報（加速度VAと角速度VB）を取得する部分である。取得された加速度VAと角速度VBは、カメラ移動量無効部３６に与えられる。
たとえば、３軸のジャイロと３方向の加速度計によって、３次元の加速度VAと角速度VBを計測する。加速度VAと角速度VBを、慣性情報と呼ぶ。
慣性情報取得部４２は、慣性計測装置４１で計測された慣性情報（加速度VAと角速度VB）を取得する部分である。取得された加速度VAと角速度VBは、カメラ移動量無効部３６に与えられる。
【0128】
たとえば、車体が乱暴に地面に置かれて所定値を超える加速度が計測された場合や、車体が素早く振り回されて所定値を超える加速度や角速度が計測された場合には、カメラ画像に含まれる物体のブレが大きくなり、正確なカメラ移動量を取得することが困難となり、自己位置推定が不安定になる場合がある。
そこで、計測された加速度と角速度が、所定の判定比較値よりも大きい場合には、カメラ画像のブレが大きいと判定し、カメラの移動量を無効化する。
そこで、計測された加速度と角速度が、所定の判定比較値よりも大きい場合には、カメラ画像のブレが大きいと判定し、カメラの移動量を無効化する。
【0129】
カメラ移動量無効部３６は、慣性情報取得部４２によって取得された慣性情報（加速度VAと角速度VB）に基づいて、取得されたカメラ移動量（カメラ速度V2）を無効化する部分である。
カメラ移動量を無効化する基準として、加速度VAと比較する判定比較値VA0と、角速度VBと比較する判定比較値VB0とを予め記憶しておく。
カメラ移動量を無効化する基準として、加速度VAと比較する判定比較値VA0と、角速度VBと比較する判定比較値VB0とを予め記憶しておく。
【0130】
たとえば、取得された加速度VAと、判定比較値VA0とを比較し、加速度VAが、判定比較値VA0よりも大きい場合（VA＞VA0）、取得されたカメラ移動量（カメラ速度V2）を無効化する。
また、取得された角速度VBと、判定比較値VB0とを比較し、角速度VBが、判定比較値VA0よりも大きい場合（VB＞VB0）も、取得されたカメラ移動量（カメラ速度V2）を無効化する。
すなわち、カメラ移動量無効部３６が、取得された加速度VAと角速度VBの少なくともどちらかが、所定の判定比較値よりも大きい場合に、カメラ移動量を無効化し、移動量選択部２５がカメラ移動量を選択しないようにする。
また、取得された角速度VBと、判定比較値VB0とを比較し、角速度VBが、判定比較値VA0よりも大きい場合（VB＞VB0）も、取得されたカメラ移動量（カメラ速度V2）を無効化する。
すなわち、カメラ移動量無効部３６が、取得された加速度VAと角速度VBの少なくともどちらかが、所定の判定比較値よりも大きい場合に、カメラ移動量を無効化し、移動量選択部２５がカメラ移動量を選択しないようにする。
【0131】
上記したように、カメラ移動量の無効化は、カメラ移動量（カメラ速度V2）を採用しないようにし、カメラ移動量取得部２２によるカメラ移動量の計算をリセットすることを意味し、カメラ移動量無効部３６から、移動量選択部２５あるいは、選択移動量出力部２６に対して、カメラ移動量を選択しないこと、あるいはカメラ移動量を出力しないことを意味する無効化信号を与える。
【0132】
無効化信号を受け取った移動量選択部２５は、カメラ移動量（カメラ速度V2）の選択をやめ、車輪移動量（車輪速度V1）に切り替え、選択移動量出力部２６からは、車輪移動量（車輪速度V1）が出力される。
このように、不正確になった可能性があるカメラ移動量を無効化することによって、自己位置推定処理が不安定になることを防ぐことができる。
このように、不正確になった可能性があるカメラ移動量を無効化することによって、自己位置推定処理が不安定になることを防ぐことができる。
【0133】
また、カメラ移動量を使用している状態で、上記のように大きな衝撃が加えられ、かなり大きな加速度または角速度が計測された場合には、管理担当者等に、自己位置エラーを通知して、自己位置推定処理を停止してもよい。
これにより、不安定な自己位置に基づいて自律走行装置が動作することによって、事故が発生する危険を低減することができる。
これにより、不安定な自己位置に基づいて自律走行装置が動作することによって、事故が発生する危険を低減することができる。
【0134】
以上のように、実施例４では、自律走行装置に加えられた衝撃に対応する加速度と角速度を計測して、前記取得された加速度と角速度の少なくともどちらかが、所定の判定比較値よりも大きい場合に、カメラ移動量を無効化し、移動量選択部がカメラ移動量を選択しないようにするので、衝撃によって画像がぶれることによって、有効なカメラ移動量を取得できないような状況でも、精度の高い安定した自己位置推定を継続することができる。
【0135】
（移動量処理部の移動量判定選択処理）
図２７に、移動量処理部における移動量判定選択処理の一実施例のフローチャートを示す。
図２７において、図２４のステップと同じ処理をするステップには、同じ番号を付与している。
図２７に、移動量処理部における移動量判定選択処理の一実施例のフローチャートを示す。
図２７において、図２４のステップと同じ処理をするステップには、同じ番号を付与している。
【0136】
図２７のステップS51において、慣性情報取得部４２が、慣性計測装置IMUから、慣性情報（加速度VAと角速度VB）を取得する。
ステップS52において、取得された加速度VAと判定比較値VA0とを比較し、取得された角速度VBと判定比較値VB0とを比較する。
ステップS53において、加速度VA＞VA0であった場合、ステップS34に進み、そうでない場合は、ステップS54に進む。
ステップS54において、角速度VB＞VB0であった場合、ステップS34に進み、そうでない場合は、ステップS55に進む。
ステップS52において、取得された加速度VAと判定比較値VA0とを比較し、取得された角速度VBと判定比較値VB0とを比較する。
ステップS53において、加速度VA＞VA0であった場合、ステップS34に進み、そうでない場合は、ステップS54に進む。
ステップS54において、角速度VB＞VB0であった場合、ステップS34に進み、そうでない場合は、ステップS55に進む。
【0137】
ステップS55において、車輪移動量（車輪速度V1）とカメラ移動量（カメラ速度V2）を取得し、ステップS13に進む。
ステップS34において、カメラ移動量取得部２２におけるカメラ移動量の計算処理をリセットし、ステップS15に進む。
ステップS34において、カメラ移動量取得部２２におけるカメラ移動量の計算処理をリセットし、ステップS15に進む。
【0138】
以下のステップS13からステップS19において、図２４と同様に、図１０の移動量判定選択情報に示した判断基準に基づいて、移動量を選択する。
すなわち、ステップS15とステップS18において、車輪移動量（車輪速度V1）を、選択する。
ステップS16とステップS19において、カメラ移動量（カメラ速度V2）を、選択する。
以上のようにして、移動量の停止判定と選択処理が行われる。
すなわち、ステップS15とステップS18において、車輪移動量（車輪速度V1）を、選択する。
ステップS16とステップS19において、カメラ移動量（カメラ速度V2）を、選択する。
以上のようにして、移動量の停止判定と選択処理が行われる。
【0139】
＜移動量処理部の実施例５＞
（移動量処理部の構成）
図６に、この発明の自律走行装置の移動量処理部の一実施例の構成ブロック図を示す。
図６の移動量処理部は、図２の移動量処理部と比較して、車輪速度監視部５１が追加される点が異なり、その他の構成は、図２の移動量処理部と同じ構成を有する。
ここでは、車輪移動量である車輪速度V1を監視し、自律走行装置が停止した時を検出して、停止時の誤判定を防止するために、適切な移動量を選択するようにして、自己位置推定の精度を向上させる。
（移動量処理部の構成）
図６に、この発明の自律走行装置の移動量処理部の一実施例の構成ブロック図を示す。
図６の移動量処理部は、図２の移動量処理部と比較して、車輪速度監視部５１が追加される点が異なり、その他の構成は、図２の移動量処理部と同じ構成を有する。
ここでは、車輪移動量である車輪速度V1を監視し、自律走行装置が停止した時を検出して、停止時の誤判定を防止するために、適切な移動量を選択するようにして、自己位置推定の精度を向上させる。
【0140】
車輪速度監視部５１は、車輪の走行速度である車輪速度V1を監視する部分であり、車輪移動量取得部２１から出力される車輪移動量である車輪速度V1を取得する。
また、車輪速度監視部５１が監視する車輪速度V1がゼロとなり、車輪の停止状態を検出した後の所定期間において、移動量選択部２５が、車輪速度V1を、自己位置推定に利用する移動量として選択する。
車輪速度監視部５１は、取得された車輪速度V1によって、車輪の状態をチェックし、停止状態の場合には、カメラ移動量（カメラ速度V2）を採用せず、車輪移動量（車輪速度V1）を選択するように、移動量選択部２５に、所定の制御信号を出力する。
また、車輪速度監視部５１が監視する車輪速度V1がゼロとなり、車輪の停止状態を検出した後の所定期間において、移動量選択部２５が、車輪速度V1を、自己位置推定に利用する移動量として選択する。
車輪速度監視部５１は、取得された車輪速度V1によって、車輪の状態をチェックし、停止状態の場合には、カメラ移動量（カメラ速度V2）を採用せず、車輪移動量（車輪速度V1）を選択するように、移動量選択部２５に、所定の制御信号を出力する。
【0141】
通常、自律走行装置では、上下方向のカメラの視界を確保して鮮明な画像を得るために、カメラは、車輪から十分に離れた自律走行装置の上方部分に設置されることが多い。
しかし、左右の車輪で駆動され、上下に長さのある、例えば筒状の形状をした自律走行装置においては、カメラが上方部分に設置された場合、車輪の接地面を起点にして、カメラが前後に揺れやすい構成になっている。
しかし、左右の車輪で駆動され、上下に長さのある、例えば筒状の形状をした自律走行装置においては、カメラが上方部分に設置された場合、車輪の接地面を起点にして、カメラが前後に揺れやすい構成になっている。
【0142】
このために、自律走行装置が停止する時に、カメラが前後に揺れることによって、カメラ画像から計算したカメラ速度として、異常な速度を計測してしまう場合がある。
図１１に、車両が停止した場合における車輪速度とカメラ速度の変化の一実施例の説明図を示す。
図１１において、自律走行装置が停止する場合、車輪の減速ともに、次第に、車輪速度V1がゼロとなる。車輪の減速ともに、カメラ速度も、同様に減少する。
しかし、車輪速度V1がゼロになった直後の一定期間、カメラ速度として、図１１に示すような異常な速度が計測される場合がある。
図１１に、車両が停止した場合における車輪速度とカメラ速度の変化の一実施例の説明図を示す。
図１１において、自律走行装置が停止する場合、車輪の減速ともに、次第に、車輪速度V1がゼロとなる。車輪の減速ともに、カメラ速度も、同様に減少する。
しかし、車輪速度V1がゼロになった直後の一定期間、カメラ速度として、図１１に示すような異常な速度が計測される場合がある。
【0143】
すなわち、車輪が停止しているにもかかわらず、カメラが移動していると判断されるので、車体が持ち上げ移動されているという誤判定がされる場合がある。
また、車輪が停止した後、所定の時間が経過すれば、カメラの前後の揺れは収まり、カメラも停止した状態にある。
また、車輪が停止した後、所定の時間が経過すれば、カメラの前後の揺れは収まり、カメラも停止した状態にある。
【0144】
したがって、車輪の停止状態を検出した場合、その停止直後の所定期間では、カメラ速度を移動量として採用しないことが好ましいので、その所定期間では、車輪速度V1を移動量として選択するようにする。
これにより、車体の持ち上げ移動がされたという誤判定を防止することができる。
これにより、車体の持ち上げ移動がされたという誤判定を防止することができる。
【0145】
上記した所定期間は、予め設定記憶しておけばよいが、自律走行装置の構造やカメラの取付位置等によって、カメラの揺れが収まる時間が異なるので、設定すべき適切な所定期間も異なる。
たとえば、所定期間として１秒が予め設定された場合、車輪の停止後の１秒間は、カメラ移動量（カメラ速度V2）を選択しないことを意味する無効化信号を、車輪速度監視部５１から移動量選択部２５に出力する。
無効化信号を受け取った移動量選択部２５は、カメラ移動量（カメラ速度V2）の選択をやめ、車輪移動量（車輪速度V1）に切り替え、選択移動量出力部２６からは、車輪移動量（車輪速度V1）が出力される。
たとえば、所定期間として１秒が予め設定された場合、車輪の停止後の１秒間は、カメラ移動量（カメラ速度V2）を選択しないことを意味する無効化信号を、車輪速度監視部５１から移動量選択部２５に出力する。
無効化信号を受け取った移動量選択部２５は、カメラ移動量（カメラ速度V2）の選択をやめ、車輪移動量（車輪速度V1）に切り替え、選択移動量出力部２６からは、車輪移動量（車輪速度V1）が出力される。
【0146】
所定期間（１秒）が経過した後に、無効化信号の出力を停止する。
これにより、移動量選択部２５は、もとの移動量の選択処理に戻る。
なお、所定期間（１秒）、カメラ移動量（カメラ速度V2）を選択しないことを意味する無効化信号を出力する代わりに、車輪移動量（車輪速度V1）を選択することを意味する制御信号を、移動量選択部２５に出力してもよい。
これにより、移動量選択部２５は、もとの移動量の選択処理に戻る。
なお、所定期間（１秒）、カメラ移動量（カメラ速度V2）を選択しないことを意味する無効化信号を出力する代わりに、車輪移動量（車輪速度V1）を選択することを意味する制御信号を、移動量選択部２５に出力してもよい。
【0147】
また、減速と加速が頻繁に繰り返される場合、車輪速度V1が大きく変化する状態が繰り返されている場合も、車体が持ち上げ移動されているという誤判定がされる場合がある。
図１２に、車輪速度V1が大きく変化した場合における車輪速度とカメラ速度の変化の一実施例の説明図を示す。
すなわち、車輪速度V1が大きく変化し車体の加速度が大きい場合は車体が前後に揺れていると考えられ、車輪速度V1がゼロに近づいたとき、その直後に、カメラ速度に異常に大きな変化が発生する場合がある。
図１２に、車輪速度V1が大きく変化した場合における車輪速度とカメラ速度の変化の一実施例の説明図を示す。
すなわち、車輪速度V1が大きく変化し車体の加速度が大きい場合は車体が前後に揺れていると考えられ、車輪速度V1がゼロに近づいたとき、その直後に、カメラ速度に異常に大きな変化が発生する場合がある。
【0148】
このように、カメラ速度に異常に大きな変化が発生した場合も、車輪速度V1がゼロとなった直後の所定期間、カメラ速度を移動量として採用しないことが好ましい。
したがって、前記車輪速度監視部５１が監視する車輪速度V1がゼロとなり、車輪の停止状態を検出した後の所定期間において、その所定時間内における車輪速度V1の変化量が、所定のしきい値を超えた場合は、カメラ速度を無視し、その所定期間内では、車輪速度V1を、自己位置推定に利用する移動量として選択するようにする。
これにより、自律走行装置が加減速を繰り返している場合に、車体が持ち上げ移動されているという誤判定がされるのを防止することができる。
したがって、前記車輪速度監視部５１が監視する車輪速度V1がゼロとなり、車輪の停止状態を検出した後の所定期間において、その所定時間内における車輪速度V1の変化量が、所定のしきい値を超えた場合は、カメラ速度を無視し、その所定期間内では、車輪速度V1を、自己位置推定に利用する移動量として選択するようにする。
これにより、自律走行装置が加減速を繰り返している場合に、車体が持ち上げ移動されているという誤判定がされるのを防止することができる。
【0149】
以上のように、実施例５では、監視する車輪速度がゼロとなり、車輪の停止状態を検出した後の所定期間において、移動量選択部が、車輪速度を自己位置推定に利用する移動量として選択するので、カメラ移動量であるカメラ速度は採用されず、静止直後にカメラが前後に揺れて異常なカメラ速度を計測してしまう状況でも、車体が持ち上げ移動されたと誤判定されることを防止し、精度の高い安定した自己位置推定を継続することができる。
また、監視する前記車輪速度がゼロとなり、車輪の停止状態を検出した後の所定期間において、車輪速度の変化量が、所定のしきい値を超えた場合は、移動量選択部が、車輪速度を自己位置推定に利用する移動量として選択するので、カメラ移動量であるカメラ速度は採用されず、加減速を繰り返し、車輪速度が大きく変化するような状況でも、車体が持ち上げ移動されたと誤判定されることを防止し、精度の高い安定した自己位置推定を継続することができる。
また、監視する前記車輪速度がゼロとなり、車輪の停止状態を検出した後の所定期間において、車輪速度の変化量が、所定のしきい値を超えた場合は、移動量選択部が、車輪速度を自己位置推定に利用する移動量として選択するので、カメラ移動量であるカメラ速度は採用されず、加減速を繰り返し、車輪速度が大きく変化するような状況でも、車体が持ち上げ移動されたと誤判定されることを防止し、精度の高い安定した自己位置推定を継続することができる。
【0150】
（移動量処理部の移動量判定選択処理）
図２８に、移動量処理部における移動量判定選択処理の一実施例のフローチャートを示す。
図２８において、図２４のステップと同じ処理をするステップには、同じ番号を付与している。
図２８に、移動量処理部における移動量判定選択処理の一実施例のフローチャートを示す。
図２８において、図２４のステップと同じ処理をするステップには、同じ番号を付与している。
【0151】
図２８のステップS61において、車輪速度監視部５１が、車輪速度V1を取得する。
上記したように、車輪移動量が車輪速度V1であるので、車輪移動量取得部２１から出力される車輪移動量である車輪速度V1を取得する。
上記したように、車輪移動量が車輪速度V1であるので、車輪移動量取得部２１から出力される車輪移動量である車輪速度V1を取得する。
【0152】
ステップS62において、車輪速度V1を利用して、車輪の状態をチェックする。
ここでは、車輪が停止状態にあるかまたは走行状態にあるかをチェックする。
車輪速度V1がゼロであれば、停止状態であり、それ以外の場合は、走行状態である。
ステップS63において、車輪が停止状態（V1＝0）の場合、ステップS64に進み、そうでない場合は、ステップS67に進む。
ここでは、車輪が停止状態にあるかまたは走行状態にあるかをチェックする。
車輪速度V1がゼロであれば、停止状態であり、それ以外の場合は、走行状態である。
ステップS63において、車輪が停止状態（V1＝0）の場合、ステップS64に進み、そうでない場合は、ステップS67に進む。
【0153】
ステップS64において、所定期間を計測するための監視タイマーTMを起動させる。
ステップS65において、所定期間が経過し、監視タイマーTMがタイムアウトした場合は、ステップS67に進み、タイムアウトしていない場合は、ステップS66に進む。
ステップS65において、所定期間が経過し、監視タイマーTMがタイムアウトした場合は、ステップS67に進み、タイムアウトしていない場合は、ステップS66に進む。
【0154】
ステップS66において、移動量として、車輪移動量（車輪速度V1）を選択し、ステップS65に戻る。
ここでは、輪速度監視部５１から移動量選択部２５に、カメラ移動量（カメラ速度V2）を選択しないことを意味する無効化信号を出力する。
あるいは、車輪移動量（車輪速度V1）を選択することを意味する制御信号を、移動量選択部２５に出力してもよい。
これにより、監視タイマーTMに設定された所定期間において、カメラ移動量（カメラ速度V2）が採用されず、車輪移動量（車輪速度V1）が使用される。
ここでは、輪速度監視部５１から移動量選択部２５に、カメラ移動量（カメラ速度V2）を選択しないことを意味する無効化信号を出力する。
あるいは、車輪移動量（車輪速度V1）を選択することを意味する制御信号を、移動量選択部２５に出力してもよい。
これにより、監視タイマーTMに設定された所定期間において、カメラ移動量（カメラ速度V2）が採用されず、車輪移動量（車輪速度V1）が使用される。
【0155】
ステップS67において、所定期間が経過し、監視タイマーTMがタイムアウトしたので、車輪移動量（車輪速度V1）とカメラ移動量（カメラ速度V2）を取得し、ステップS13に進む。
【0156】
以下のステップS13からステップS19において、図２４と同様に、図１０の移動量判定選択情報に示した判断基準に基づいて、移動量を選択する。
すなわち、ステップS15とステップS18において、車輪移動量（車輪速度V1）を、選択する。
ステップS16とステップS19において、カメラ移動量（カメラ速度V2）を、選択する。
以上のようにして、移動量の停止判定と選択処理が行われる。
すなわち、ステップS15とステップS18において、車輪移動量（車輪速度V1）を、選択する。
ステップS16とステップS19において、カメラ移動量（カメラ速度V2）を、選択する。
以上のようにして、移動量の停止判定と選択処理が行われる。
【0157】
＜移動量処理部の実施例６＞
（移動量処理部の構成）
図７に、この発明の自律走行装置の移動量処理部の一実施例の構成ブロック図を示す。
図７の移動量処理部は、図２の移動量処理部と比較して、車輪速度監視部５１と、動作実行部６１と、カメラ移動量判定基準変更部６２が追加される点が異なり、その他の構成は、図２の移動量処理部と同じ構成を有する。
ここでは、自律走行装置が走行している場合（以下、活動中とも呼ぶ）、車輪速度V1が停止判定値V10よりも小さくなって、車輪が停止判定された場合に、カメラ移動量を選択する基準となる停止判定値V20を変更する。
これによって、車体が持ち上げ移動されているという誤判定がされるのを防止する。
（移動量処理部の構成）
図７に、この発明の自律走行装置の移動量処理部の一実施例の構成ブロック図を示す。
図７の移動量処理部は、図２の移動量処理部と比較して、車輪速度監視部５１と、動作実行部６１と、カメラ移動量判定基準変更部６２が追加される点が異なり、その他の構成は、図２の移動量処理部と同じ構成を有する。
ここでは、自律走行装置が走行している場合（以下、活動中とも呼ぶ）、車輪速度V1が停止判定値V10よりも小さくなって、車輪が停止判定された場合に、カメラ移動量を選択する基準となる停止判定値V20を変更する。
これによって、車体が持ち上げ移動されているという誤判定がされるのを防止する。
【0158】
車輪速度監視部５１は、図６と同様に、車輪の走行速度である車輪速度V1を監視する部分であり、車輪移動量取得部２１から出力される車輪移動量である車輪速度V1を取得する。取得された車輪速度V1は、カメラ移動量判定基準変更部６２に与えられる。
【0159】
動作実行部６１は、自律走行装置の走行動作を実行する部分である。
ここでは、自律走行装置が走行し何らかの活動をしている状態と、走行を停止し活動していない状態を区別するための情報（活動状況情報）が、動作実行部６１から、出力されるものとする。
活動状況情報は、カメラ移動量判定基準変更部６２に出力される。
たとえば、自律走行装置が走行し活動をしている状態の場合に、活動状況情報として、走行動作の活動状態が活動中であることを示す情報（活動中：ST1）が出力されるものとする。
一方、自律走行装置が停止している状態の場合に、活動状況情報として、走行動作の活動状態が停止中であることを示す情報（停止中：ST0）が出力されるものとする。
ここでは、自律走行装置が走行し何らかの活動をしている状態と、走行を停止し活動していない状態を区別するための情報（活動状況情報）が、動作実行部６１から、出力されるものとする。
活動状況情報は、カメラ移動量判定基準変更部６２に出力される。
たとえば、自律走行装置が走行し活動をしている状態の場合に、活動状況情報として、走行動作の活動状態が活動中であることを示す情報（活動中：ST1）が出力されるものとする。
一方、自律走行装置が停止している状態の場合に、活動状況情報として、走行動作の活動状態が停止中であることを示す情報（停止中：ST0）が出力されるものとする。
【0160】
カメラ移動量判定基準変更部６２は、動作実行部６１が実行する走行動作の活動状態に基づいて、カメラ移動量の停止判定をする基準となる停止判定値V20を変更する部分である。
動作実行部６１から出力される自律走行装置が活動中であるかあるいは停止中であるかの活動状況情報によって、図９に示したカメラ速度V2の停止判定値V20を変更する。
動作実行部６１から出力される自律走行装置が活動中であるかあるいは停止中であるかの活動状況情報によって、図９に示したカメラ速度V2の停止判定値V20を変更する。
【0161】
この実施例では、動作実行部６１が実行する走行動作の活動状態が活動中であるときに、車輪速度が所定の停止判定値よりも小さくなって、第１停止判定部２３による静動判定によって自律走行装置が停止中と判定された場合に、カメラ移動量が選択されないようにするために、カメラ移動量の停止判定をする基準となる停止判定値を、所定量だけ大きく設定し、車輪移動量を自己位置推定に利用する移動量に選択する範囲を広げるようにする。
【0162】
ここで、停止中のカメラ速度V2の停止判定値をV20とし、走行動作の活動状態が活動中であるときに、静動判定によって自律走行装置が停止中と判定された場合のカメラ速度V2の停止判定値を、V20よりも大きなV20’とする（V20＜V20’）。
また、自律走行装置が活動中のときに静動判定によって車輪が停止したと判断される場合に、停止判定値V20を変更するようにし、車輪速度監視部５１から与えられた車輪速度V1がゼロでなくても、車輪速度V1＜V10を満たしている場合にだけ、カメラ速度V2の停止判定値を、V20よりも大きなV20’に変更する。
また、自律走行装置が活動中のときに静動判定によって車輪が停止したと判断される場合に、停止判定値V20を変更するようにし、車輪速度監視部５１から与えられた車輪速度V1がゼロでなくても、車輪速度V1＜V10を満たしている場合にだけ、カメラ速度V2の停止判定値を、V20よりも大きなV20’に変更する。
【0163】
カメラ移動量判定基準変更部６２は、走行動作の活動状態に基づいて、停止中の停止判定値V20を第２停止判定部２４に与えるか、または変更後のカメラ速度V2の停止判定値V20’を、第２停止判定部２４に与える。
あるいは、第２停止判定部２４が、すでに、停止中のカメラ速度V2の停止判定値V20と、変更後のカメラ速度V2の停止判定値V20’を予め記憶している場合は、カメラ移動量判定基準変更部６２は、停止中または停止判定値を変更することを要求する情報を、第２停止判定部２４に与えるだけでもよい。
あるいは、第２停止判定部２４が、すでに、停止中のカメラ速度V2の停止判定値V20と、変更後のカメラ速度V2の停止判定値V20’を予め記憶している場合は、カメラ移動量判定基準変更部６２は、停止中または停止判定値を変更することを要求する情報を、第２停止判定部２４に与えるだけでもよい。
【0164】
自律走行装置が活動中は、通常、時速3～4km程度の所定の速度で走行している場合が多い。
ただし、活動中においても、障害物を検出した場合、急に停止することがある。
このように急に停止することで、車輪速度V1はゼロになるが、上記したように、カメラ速度が異常に変化したために、持ち上げ移動がされたと誤判定される場合がある。
ただし、活動中においても、障害物を検出した場合、急に停止することがある。
このように急に停止することで、車輪速度V1はゼロになるが、上記したように、カメラ速度が異常に変化したために、持ち上げ移動がされたと誤判定される場合がある。
【0165】
ところで、悪意のない管理担当者によって、実際に行われる持ち上げ移動は、自律走行装置を一時停止してから行うことが多く、自律走行装置の活動中に不意に持ち上げられることは少ない。
そこで、自律走行装置の活動中の一時停止によって、持ち上げ移動がされたという誤判定を回避するために、活動中の状態では、カメラ移動量の停止判定をする基準となる停止判定値V20を、大きくしてもよい。
そこで、自律走行装置の活動中の一時停止によって、持ち上げ移動がされたという誤判定を回避するために、活動中の状態では、カメラ移動量の停止判定をする基準となる停止判定値V20を、大きくしてもよい。
【0166】
図１３に、自律走行装置が停止中において採用する移動量の選択判定領域の一実施例の説明図を示す。
図１４に、自律走行装置が活動中において採用する移動量の選択判定領域の一実施例の説明図を示す。
図１３に示す選択判定領域は、図９に示した選択判定領域と同じものである。
図１３では、領域A1とA2が、車輪移動量を使用する領域であり、領域B1とB2が、カメラ移動量を使用する領域である。
図１４に、自律走行装置が活動中において採用する移動量の選択判定領域の一実施例の説明図を示す。
図１３に示す選択判定領域は、図９に示した選択判定領域と同じものである。
図１３では、領域A1とA2が、車輪移動量を使用する領域であり、領域B1とB2が、カメラ移動量を使用する領域である。
【0167】
図１４では、車輪速度V1が、車輪速度の停止判定値V10よりも小さい場合（V1＜V10）において、カメラ速度V2の停止判定値を、V20よりも大きなV20’に設定したものを示している。
図１４において、領域A1とA2に加えて、領域A3も、車輪移動量を使用する領域となる。また、図１４では、領域B1とB2が、カメラ移動量を使用する領域であるが、領域B1は、領域A3の分だけ減少することになる。
図１４において、領域A1とA2に加えて、領域A3も、車輪移動量を使用する領域となる。また、図１４では、領域B1とB2が、カメラ移動量を使用する領域であるが、領域B1は、領域A3の分だけ減少することになる。
【0168】
動作実行部６１から、自律走行装置が停止中であることを示す情報（停止中：ST0）が出力されている場合、カメラ移動量判定基準変更部６２は、図１３に示した選択判定領域を使用するものと判断し、通常どおり、判定基準として、カメラ速度V2の停止判定値V20を使用するように、V20を、第２停止判定部２４に与える。
【0169】
一方、動作実行部６１から、自律走行装置が活動中であることを示す情報（活動中：ST1）が出力されている場合に、カメラ移動量判定基準変更部６２は、図１４に示した選択判定領域を使用するものと判断し、車輪速度監視部５１から与えられた車輪速度V1が、車輪速度の停止判定値V10よりも小さい場合（V1＜V10）において、判定基準として、カメラ速度V2の停止判定値V20’を使用するように、V20’を、第２停止判定部２４に与える。
【0170】
第２停止判定部２４は、V20’が与えられた場合、車輪速度V1が車輪速度の停止判定値V10よりも小さい場合（V1＜V10）において、取得されたカメラ速度V2が、カメラ速度V2の停止判定値V20’よりも小さければ、図１４の領域A3またはA1に該当するので、移動量として、車輪移動量（車輪速度V1）を使用すべきと判断される。
このように、自律走行装置の活動中において、カメラ速度V2の停止判定値を大きくすることによって、持ち上げ移動がされたという誤判定を回避することが可能となる。
このように、自律走行装置の活動中において、カメラ速度V2の停止判定値を大きくすることによって、持ち上げ移動がされたという誤判定を回避することが可能となる。
【0171】
以上のように、実施例６では、走行動作の活動状態が活動中であるときに、カメラ移動量の停止判定をする基準となる停止判定値を、所定量だけ大きく設定し、車輪移動量を自己位置推定に利用する移動量に選択する範囲を広げるので、たとえば、活動中に、障害物を検出して急停止した状況でも、車体が持ち上げ移動されたと誤判定されることを防止し、精度の高い安定した自己位置推定を継続することができる。
【0172】
＜移動量処理部の実施例７＞
（移動量処理部の構成）
図８に、この発明の自律走行装置の移動量処理部の一実施例の構成ブロック図を示す。
図８の移動量処理部は、図２の移動量処理部と比較して、車輪速度監視部５１と、車輪移動量判定基準変更部７１と、カメラ速度監視部７２とが追加される点が異なり、その他の構成は、図２の移動量処理部と同じ構成を有する。
（移動量処理部の構成）
図８に、この発明の自律走行装置の移動量処理部の一実施例の構成ブロック図を示す。
図８の移動量処理部は、図２の移動量処理部と比較して、車輪速度監視部５１と、車輪移動量判定基準変更部７１と、カメラ速度監視部７２とが追加される点が異なり、その他の構成は、図２の移動量処理部と同じ構成を有する。
【0173】
ここでは、走行中の車輪速度V1が通常の停止判定値V10よりもわずかに大きい速度で、自律走行装置が低速移動している状態で、カメラ速度が停止判定された場合に、車輪移動量を選択する基準となる停止判定値V10を変更する。
これによって、カメラ速度が停止状態と判定されているにもかかわらず、車輪速度V1が停止状態と判定されないことで、車輪が空転しているという誤判定がされるのを防止する。
これによって、カメラ速度が停止状態と判定されているにもかかわらず、車輪速度V1が停止状態と判定されないことで、車輪が空転しているという誤判定がされるのを防止する。
【0174】
車輪速度監視部５１は、図６と同様に、車輪の走行速度である車輪速度V1を監視する部分であり、車輪移動量取得部２１から出力される車輪移動量である車輪速度V1を取得する。取得された車輪速度V1は、車輪移動量判定基準変更部７１に与えられる。
【0175】
カメラ速度監視部７２は、カメラの移動速度であるカメラ速度V2を監視する部分であり、カメラ移動量取得部２１から出力されるカメラ移動量であるカメラ速度V2を取得する。取得されたカメラ速度V2は、車輪移動量判定基準変更部７１に与えられる。
カメラ移動量であるカメラ速度V2は、上記したように、カメラ画像から取得される。複数のカメラ画像における画像の変化が小さい場合、正確なカメラ速度V2が取得できず、カメラ速度V2が停止判定値V20よりも小さくなって、カメラ速度V2が停止判定されてしまう場合がある。
カメラ移動量であるカメラ速度V2は、上記したように、カメラ画像から取得される。複数のカメラ画像における画像の変化が小さい場合、正確なカメラ速度V2が取得できず、カメラ速度V2が停止判定値V20よりも小さくなって、カメラ速度V2が停止判定されてしまう場合がある。
【0176】
車輪移動量判定基準変更部７１は、監視されている車輪速度V1とカメラ速度V2に基づいて、車輪移動量（車輪速度V1）の停止判定をする基準となる停止判定値V10を変更する部分である。
この実施例では、自律走行装置が所定値以下の低速走行をしている状態で、カメラ速度が所定の停止判定値よりも小さくなって、第２停止判定部による静動判定によって自律走行装置が停止中と判定された場合に、カメラ移動量が選択されないようにするために、車輪移動量の停止判定をする基準となる停止判定値を、所定量だけ大きく設定し、車輪移動量を自己位置推定に利用する移動量に選択する範囲を広げるようにする。
この実施例では、自律走行装置が所定値以下の低速走行をしている状態で、カメラ速度が所定の停止判定値よりも小さくなって、第２停止判定部による静動判定によって自律走行装置が停止中と判定された場合に、カメラ移動量が選択されないようにするために、車輪移動量の停止判定をする基準となる停止判定値を、所定量だけ大きく設定し、車輪移動量を自己位置推定に利用する移動量に選択する範囲を広げるようにする。
【0177】
図１５に、自律走行装置が低速移動中の場合に、採用する移動量の選択判定領域の一実施例の説明図を示す。
図９では、車輪速度V1の停止判定値をV10とし、４つの選択判定領域（A1、A2、B1、B2）を設定していたが、図１５では、新たに、選択判定領域A4を設ける。
図１５において、変更後の車輪速度V1の停止判定値を、V10よりも大きなV10’とし（V10＜V10’）、車輪速度V1が、V10とV10’との間で、カメラ速度V2が停止判定値V20よりも小さい領域A4も、車輪移動量（車輪速度V1）を選択する領域とする。
図９では、車輪速度V1の停止判定値をV10とし、４つの選択判定領域（A1、A2、B1、B2）を設定していたが、図１５では、新たに、選択判定領域A4を設ける。
図１５において、変更後の車輪速度V1の停止判定値を、V10よりも大きなV10’とし（V10＜V10’）、車輪速度V1が、V10とV10’との間で、カメラ速度V2が停止判定値V20よりも小さい領域A4も、車輪移動量（車輪速度V1）を選択する領域とする。
【0178】
すなわち、自律走行装置が低速走行中のときに、カメラ速度V2によって停止したと判断される場合に、停止判定値V10を変更するようにし、カメラ速度監視部７２から与えられたカメラ速度V2がゼロでなくても、カメラ速度V2＜停止判定値V20を満たしている場合にだけ、車輪速度V1の停止判定値をV10よりも大きなV10’に変更する。
ここで、低速走行とは、車輪速度V1が、V10’よりも小さい走行を意味するものとする。
ここで、低速走行とは、車輪速度V1が、V10’よりも小さい走行を意味するものとする。
【0179】
車輪移動量判定基準変更部７１は、変更後の車輪速度V1の停止判定値V10’を、第１停止判定部２３に与える。
あるいは、第１停止判定部２３が、すでに、変更前の車輪速度V1の停止判定値V10と、変更後の車輪速度V1の停止判定値V10’とを予め記憶している場合は、車輪移動量判定基準変更部７１は、停止判定値V10を変更することを要求する情報を、第１停止判定部２３に与えるだけでもよい。
あるいは、第１停止判定部２３が、すでに、変更前の車輪速度V1の停止判定値V10と、変更後の車輪速度V1の停止判定値V10’とを予め記憶している場合は、車輪移動量判定基準変更部７１は、停止判定値V10を変更することを要求する情報を、第１停止判定部２３に与えるだけでもよい。
【0180】
自律走行装置の車輪速度V1がゼロではないが、自律走行装置がかなり低い速度で移動している場合、上記のように、複数のカメラ画像における画像の変化が小さいと、正確なカメラ速度V2が取得できず、カメラ速度V2が停止判定されてしまう場合がある。
【0181】
この場合、２つの停止判定における車輪とカメラの静動判定の結果が不一致となり、車輪速度の静動判定では、車輪速度V1が停止判定値V10よりも大きいために、移動中と判定され、カメラ速度の静動判定では、カメラ速度V2が停止判定値V20よりも小さいために、停止中と判定され、車輪が空転しているという誤判定がされる場合がある。
【0182】
そこで、このような車輪が空転しているという誤判定がされることを回避するために、低速走行中（V1＜V10’）で、カメラ速度V2が停止判定された場合（V2＜V20）において、車輪速度V1の停止判定値を、V10よりも大きなV10’に変更する。
車輪速度V1の停止判定値を大きくすることにより、車輪速度V1が、変更前の停止判定値V10よりも大きいが停止判定値V10’よりも小さいような低速走行をしている場合において、2つの静動判定を一致させることが可能となり、車輪が空転しているという誤判定がされないようにできる。
車輪速度V1の停止判定値を大きくすることにより、車輪速度V1が、変更前の停止判定値V10よりも大きいが停止判定値V10’よりも小さいような低速走行をしている場合において、2つの静動判定を一致させることが可能となり、車輪が空転しているという誤判定がされないようにできる。
【0183】
また、車輪が空転しているという誤判定がされることを回避するために、次のような移動量の選択判定領域を利用してもよい。
図１６に、自律走行装置が低速移動中の場合に、採用する移動量の選択判定領域の一実施例の説明図を示す。
図１６では、カメラ速度V2が停止判定された場合（V2＜V20）において、車輪速度V1とカメラ速度V2の差が、所定閾値Vdよりも小さい場合に、車輪移動量（車輪速度V1）を選択するものとする。
すなわち、図１６において、車輪速度V1＞V10であり、カメラ速度V2＜V20であり、V1－V2＜Vdを満たす選択判定領域A5を設け、取得された車輪速度V1とカメラ速度V2が、この選択判定領域A5に含まれる場合、車輪移動量（車輪速度V1）を選択する。
これにより、低速走行をしている状態で、カメラ速度V2が停止判定された場合でも、車輪が空転しているという誤判定がされることを回避することができ、車輪移動量（車輪速度V1）を用いて、自己位置推定を継続することができる。
図１６に、自律走行装置が低速移動中の場合に、採用する移動量の選択判定領域の一実施例の説明図を示す。
図１６では、カメラ速度V2が停止判定された場合（V2＜V20）において、車輪速度V1とカメラ速度V2の差が、所定閾値Vdよりも小さい場合に、車輪移動量（車輪速度V1）を選択するものとする。
すなわち、図１６において、車輪速度V1＞V10であり、カメラ速度V2＜V20であり、V1－V2＜Vdを満たす選択判定領域A5を設け、取得された車輪速度V1とカメラ速度V2が、この選択判定領域A5に含まれる場合、車輪移動量（車輪速度V1）を選択する。
これにより、低速走行をしている状態で、カメラ速度V2が停止判定された場合でも、車輪が空転しているという誤判定がされることを回避することができ、車輪移動量（車輪速度V1）を用いて、自己位置推定を継続することができる。
【0184】
以上のように、実施例７では、自律走行装置が所定値以下の低速走行をしている状態で、車輪移動量の停止判定をする基準となる停止判定値を、所定量だけ大きく設定し、車輪移動量を自己位置推定に利用する移動量に選択する範囲を広げるので、たとえば、低速走行をしている状態で撮影される画像の変化が小さすぎる状況において、車輪が空転しているという誤判定がされることを防止し、精度の高い安定した自己位置推定を継続することができる。
【0185】
【0186】
移動量切替検出部１３は、移動量処理部２で行った選択判断に基づいて、自己位置推定部５に対して、動作制御信号を与える部分である。
すなわち、移動量切替検出部１３は、移動量処理部２で選択された現在の移動量に関する情報を、移動量処理部２から取得し、取得された現在の移動量に関する情報に対応させて、自己位置推定処理を制御するための動作制御信号を、自己位置推定部５に出力する。
自己位置推定部５は、この動作制御信号に基づいて、自己位置推定処理を制御する。
すなわち、移動量切替検出部１３は、移動量処理部２で選択された現在の移動量に関する情報を、移動量処理部２から取得し、取得された現在の移動量に関する情報に対応させて、自己位置推定処理を制御するための動作制御信号を、自己位置推定部５に出力する。
自己位置推定部５は、この動作制御信号に基づいて、自己位置推定処理を制御する。
【0187】
この実施例では、特に、後述するように、移動量切替検出部１３が、取得された現在の移動量に関する情報が、カメラ移動量から車輪移動量に切り替えられたことを検出した場合、自己位置推定部５に、自己位置推定処理をリセットすることを示す動作制御信号を出力し、自己位置推定部５は、自己位置推定処理で積算された自己位置のずれをリセットする。
【0188】
移動量処理部２で行った選択判断は、取得した２つの移動量（車輪移動量、カメラ移動量）のうち、どちらを使用するかの判断であり、移動量処理部２によって選択された現在の移動量（車輪移動量あるいはカメラ移動量のいずれか）に関する情報を、移動量処理部２から取得する。
【0189】
自己位置推定部５では、上記したパーティクルフィルタを利用して自己位置推定処理が行われるが、以下に示すように、動作制御信号に基づいて、パーティクルフィルタを利用した処理を行うか、パーティクルフィルタを利用した処理を停止するか、あるいは、パーティクルフィルタを利用した処理をリセットする。
すなわち、移動量処理部２から自己位置推定部５に与えられる選択移動量にかかわらず、移動量切替検出部１３から自己位置推定部５に与えられる動作制御信号によって、自己位置推定部５の処理を制御する。
すなわち、移動量処理部２から自己位置推定部５に与えられる選択移動量にかかわらず、移動量切替検出部１３から自己位置推定部５に与えられる動作制御信号によって、自己位置推定部５の処理を制御する。
【0190】
まず、移動量切替検出部１３が、移動量処理部２から選択された現在の移動量が車輪移動量であるという情報を取得している場合、車輪速度V1を利用した自己位置推定が行われるが、パーティクルフィルタを利用した自己位置推定を行うことを示す動作制御信号（制御信号C1）が、移動量切替検出部１３から自己位置推定部５に与えられる。
【0191】
また、移動量切替検出部１３が、移動量処理部２から選択された現在の移動量がカメラ移動量であるという情報を取得している場合、カメラ速度V2を利用した自己位置推定が行われるが、パーティクルフィルタを利用した自己位置推定を行わないことを示す動作制御信号（停止信号C2）が、移動量切替検出部１３から自己位置推定部５に与えられる。
【0192】
さらに、移動量切替検出部１３が移動量処理部２から取得した現在の移動量が、カメラ移動量から車輪移動量に切り替えられたことを検出した場合、パーティクルフィルタを利用した自己位置推定処理をリセットすることを意味する動作制御信号（リセット信号C3）が、移動量切替検出部１３から自己位置推定部５に与えられる。
【0193】
このような動作制御信号によって、自己位置推定処理を制御するのは、以下に示すように、パーティクルフィルタを利用した自己位置推定のずれを補正し、さらに、カメラ移動量を利用して行った自己位置推定で積算された自己位置のずれをリセットして、パーティクルフィルタを正しい位置に収束させるようにするためである。
【0194】
現在の移動量が車輪移動量であるという情報を取得している場合、自己位置推定部５では、選択された車輪速度V1を利用しパーティクルフィルタによる自己位置推定処理が行われる。
しかし、車体が手で持ち上げられた場合には、車輪が空転するので、カメラ移動量を利用して、自己位置推定処理が行われる。
しかし、車体が手で持ち上げられた場合には、車輪が空転するので、カメラ移動量を利用して、自己位置推定処理が行われる。
【0195】
一般的に、車体が手で持ち上げられた場合には、車体が上下に動く。
車体が上下に動くと、LIDARの走査面も上下に動き、通常走行時とは異なる高さの障害物を検出してしまう。
地図は通常走行時のLIDARから得たデータを使用して作成されるため、異なる高さの障害物情報によりパーティクルフィルタの更新を行うと、間違った位置にパーティクルフィルタが収束してしまう可能性がある。
したがって、カメラ移動量を使用している期間は、パーティクルフィルタによる自己位置推定を停止し、カメラによる移動量（カメラ移動量）のみを利用して、自己位置を更新する。
車体が上下に動くと、LIDARの走査面も上下に動き、通常走行時とは異なる高さの障害物を検出してしまう。
地図は通常走行時のLIDARから得たデータを使用して作成されるため、異なる高さの障害物情報によりパーティクルフィルタの更新を行うと、間違った位置にパーティクルフィルタが収束してしまう可能性がある。
したがって、カメラ移動量を使用している期間は、パーティクルフィルタによる自己位置推定を停止し、カメラによる移動量（カメラ移動量）のみを利用して、自己位置を更新する。
【0196】
しかし、カメラによる移動量は車輪による移動量とは精度が異なり、精度が低い場合が多い。特に、カメラが前後に移動する時のカメラ速度の検出精度は、一般的にあまり高くない。このため、カメラ移動量を使用している間に地図上の自己位置がずれてしまう可能性がある。
すなわち、カメラ移動量を使用している間に、自己位置の誤差が積算されてしまう場合がある。積算された自己位置の誤差を解消するためには、パーティクルフィルタをリセットする必要がある。
すなわち、カメラ移動量を使用している間に、自己位置の誤差が積算されてしまう場合がある。積算された自己位置の誤差を解消するためには、パーティクルフィルタをリセットする必要がある。
【0197】
そこで、移動量切替検出部１３が移動量処理部２から取得した現在の移動量が、カメラ移動量から車輪移動量に切り替えられたことを検出した場合、自己位置推定部５において、パーティクルフィルタをリセットする。
ここで、パーティクルフィルタのリセットとは、現在の自己位置推定座標の周辺の所定の範囲に、パーティクルを散布する操作である。
このように、パーティクルフィルタの収束状態をリセットして、パーティクルフィルタを広めに再散布することで、カメラ移動量を使用している間に積算された誤差をリセットして、パーティクルフィルタを正しい位置に収束させることができる。
ここで、パーティクルフィルタのリセットとは、現在の自己位置推定座標の周辺の所定の範囲に、パーティクルを散布する操作である。
このように、パーティクルフィルタの収束状態をリセットして、パーティクルフィルタを広めに再散布することで、カメラ移動量を使用している間に積算された誤差をリセットして、パーティクルフィルタを正しい位置に収束させることができる。
【0198】
以上のように、第２の実施形態によれば、取得された現在の移動量に関する情報が、カメラ移動量から車輪移動量に切り替えられたことを検出した場合、自己位置推定部は、自己位置推定処理で積算された自己位置のずれをリセットするので、カメラ移動量を使用している場合に積算された自己位置のずれを解消することができ、精度の高い安定した自己位置推定を継続することができる。
【0199】
＜＜第３の実施形態＞＞
図１９に、この発明の自律走行装置の第３の実施形態の概略構成ブロック図を示す。
図１９では、図１８に示した自律走行装置の構成に加えて、さらに、定置旋回制御部１４を設ける。
定置旋回制御部１４は、自律走行装置が静止した位置で、自律走行装置を回転させる部分である。
この実施例では、移動量切替検出部１３が、取得された現在の移動量に関する情報がカメラ移動量から車輪移動量に切り替えられたことを検出した場合、定置旋回制御部１４に、定置旋回要求信号を出力する。
移動量切替検出部１３から定置旋回制御部１４に対して、定置旋回要求信号が与えられた場合に、定置旋回制御部１４が、走行を再開する前に、自律走行装置を定置旋回させる。
図１９に、この発明の自律走行装置の第３の実施形態の概略構成ブロック図を示す。
図１９では、図１８に示した自律走行装置の構成に加えて、さらに、定置旋回制御部１４を設ける。
定置旋回制御部１４は、自律走行装置が静止した位置で、自律走行装置を回転させる部分である。
この実施例では、移動量切替検出部１３が、取得された現在の移動量に関する情報がカメラ移動量から車輪移動量に切り替えられたことを検出した場合、定置旋回制御部１４に、定置旋回要求信号を出力する。
移動量切替検出部１３から定置旋回制御部１４に対して、定置旋回要求信号が与えられた場合に、定置旋回制御部１４が、走行を再開する前に、自律走行装置を定置旋回させる。
【0200】
図１８に示した第２の実施形態では、パーティクルフィルタを正しい位置に収束させるために、移動量切替検出部１３が移動量処理部２から取得した現在の移動量が、カメラ移動量から車輪移動量に切り替えられたことを検出した場合、パーティクルフィルタをリセットした。
ここでは、さらに、カメラ移動量から車輪移動量に切り替えられたことを検出した場合に、走行を再開する前に、車両を定置旋回させて、周囲の障害物の位置を検出することにより、より正確に、パーティクルフィルタを正しい位置に収束させる。
定置旋回は、360度全方位の地形を確認することができるので、パーティクルフィルタを正しい位置に収束させるのに有効な動作である。
ここでは、さらに、カメラ移動量から車輪移動量に切り替えられたことを検出した場合に、走行を再開する前に、車両を定置旋回させて、周囲の障害物の位置を検出することにより、より正確に、パーティクルフィルタを正しい位置に収束させる。
定置旋回は、360度全方位の地形を確認することができるので、パーティクルフィルタを正しい位置に収束させるのに有効な動作である。
【0201】
図１７に、車輪とカメラの移動量と、定置旋回等との関係を示したタイムチャートの一実施例の説明図を示す。
図１７において、最初、車両は通常走行（R1）していたが、車輪移動量とカメラ移動量との選択切替が行われ、時刻T1において、車輪移動量P2からカメラ移動量P1に切り替えられ、時刻T3において、カメラ移動量P1から車輪移動量P2に切り替えられるものとする。
時刻T1から時刻T3までの間は、たとえば、持ち上げ移動（R2）されている状態で、カメラ速度V2によって自己位置推定が行われる。
図１７において、最初、車両は通常走行（R1）していたが、車輪移動量とカメラ移動量との選択切替が行われ、時刻T1において、車輪移動量P2からカメラ移動量P1に切り替えられ、時刻T3において、カメラ移動量P1から車輪移動量P2に切り替えられるものとする。
時刻T1から時刻T3までの間は、たとえば、持ち上げ移動（R2）されている状態で、カメラ速度V2によって自己位置推定が行われる。
【0202】
また、パーティクルフィルタの更新P3について、カメラ移動量P1に切り替えられた時刻T1のときに、停止され、カメラ移動量P1から車輪移動量P2に切り替えられた時刻T3のときに、再開されるものとする。
さらに、第２の実施形態で説明したように、車輪移動量P2からカメラ移動量P1に切り替えられたとき、時刻T2でパーティクルフィルタをリセットする信号が、移動量切替検出部１３から、自己位置推定部５に与えられ、時刻T3において、自己位置推定部５で利用される移動量が、車輪移動量P2に切り替えられる。
さらに、第２の実施形態で説明したように、車輪移動量P2からカメラ移動量P1に切り替えられたとき、時刻T2でパーティクルフィルタをリセットする信号が、移動量切替検出部１３から、自己位置推定部５に与えられ、時刻T3において、自己位置推定部５で利用される移動量が、車輪移動量P2に切り替えられる。
【0203】
パーティクルフィルタをリセットする信号を受信した自己位置推定部５では、パーティクルフィルタをリセットする処理、たとえば、現在の推定自己位置周辺へのパーティクルの再散布が行われる。
その後、移動量切替検出部１３から定置旋回制御部１４に、定置旋回要求信号が出力され、時刻T3から時刻T4までの一定時間（たとえば、十数秒程度）、車両を定置旋回（R3）させる。
定置旋回している状態において、たとえば所定角度旋回する毎にパーティクルフィルタを更新する処理が行われる。
その後、車両は、通常走行（R1）に戻る。
その後、移動量切替検出部１３から定置旋回制御部１４に、定置旋回要求信号が出力され、時刻T3から時刻T4までの一定時間（たとえば、十数秒程度）、車両を定置旋回（R3）させる。
定置旋回している状態において、たとえば所定角度旋回する毎にパーティクルフィルタを更新する処理が行われる。
その後、車両は、通常走行（R1）に戻る。
【0204】
上記のように、定置旋回をすることにより、走行を再開する前に周囲の障害物配置からパーティクルフィルタを収束させることができるので、自己位置の精度を向上させることができる。
なお、上記では一定時間定置旋回を実行したが、リセットにより広範囲に散布したパーティクルが一定の範囲に収束するまで旋回をし続けることも可能である。この時にタイムアウト時間や最大回転回数の制限を設け、パーティクルが収束しない場合は、管理担当者等に、自己位置エラーを通知して、自己位置推定処理を停止してもよい。
なお、上記では一定時間定置旋回を実行したが、リセットにより広範囲に散布したパーティクルが一定の範囲に収束するまで旋回をし続けることも可能である。この時にタイムアウト時間や最大回転回数の制限を設け、パーティクルが収束しない場合は、管理担当者等に、自己位置エラーを通知して、自己位置推定処理を停止してもよい。
【0205】
以上のように、第３の実施形態によれば、取得された現在の移動量に関する情報がカメラ移動量から車輪移動量に切り替えられたことを検出した場合、定置旋回制御部が、走行を再開する前に、自律走行装置を定置旋回させるので、定置旋回をすることにより自律走行装置の周辺にある障害物を検出することで、カメラ移動量を使用している場合に積算された自己位置のずれを解消することができ、精度の高い安定した自己位置推定を継続することができる。
【0206】
＜＜第４の実施形態＞＞
図２０に、この発明の自律走行装置の第４の実施形態の概略構成ブロック図を示す。
図２０では、図１８に示した自律走行装置の構成に加えて、さらに、車体持上検出部１５を設ける。
車体持上検出部１５は、自律走行装置の車体が持ち上げられたことを検出する部分である。
車体が持ち上げられたことを検出した場合には、自己位置推定部５が、カメラ移動量を利用して、自己位置の推定を行う。
図２０に、この発明の自律走行装置の第４の実施形態の概略構成ブロック図を示す。
図２０では、図１８に示した自律走行装置の構成に加えて、さらに、車体持上検出部１５を設ける。
車体持上検出部１５は、自律走行装置の車体が持ち上げられたことを検出する部分である。
車体が持ち上げられたことを検出した場合には、自己位置推定部５が、カメラ移動量を利用して、自己位置の推定を行う。
【0207】
車体持上検出部１５としては、たとえば、車体に取り付けられた走行面距離測定センサ１５aや、持上検出スイッチ１５bを利用することができる。
車体持上検出部１５は、走行面距離測定センサ１５aと、持上検出スイッチ１５bのうち、少なくともどちらか一方または両方から構成してもよい。
車体持上検出部１５は、走行面距離測定センサ１５aと、持上検出スイッチ１５bのうち、少なくともどちらか一方または両方から構成してもよい。
【0208】
車体が持ち上げられたことを、車体に取り付けられたセンサやスイッチを利用することで、直接的に持上状態を検出することができるため、カメラの揺れ等による持上状態の誤検出をすることがなく、精度の高い持上状態の検出が可能になる。
なお、第４の実施形態においても、図１９と同様に、定置旋回制御部１４を備えてもよい。
なお、第４の実施形態においても、図１９と同様に、定置旋回制御部１４を備えてもよい。
【0209】
図２１Ａと図２１Ｂに、この発明の自律走行装置に取り付けられた走行面距離測定センサの一実施例の説明図を示す。
図２１Ａに示すように、走行面距離測定センサ１５aは、自律走行装置の車体の下部に固定設置される。
走行面距離測定センサ１５aは、自律走行装置と走行面との距離を測定する部材であり、たとえば、光学的に距離を測定する測距センサや、超音波センサ等を利用すればよい。
走行面距離測定センサ１５aは、自律走行装置の車体の下部に、１つまたは複数個、固定設置される。
図２１Ａに示すように、走行面距離測定センサ１５aは、自律走行装置の車体の下部に固定設置される。
走行面距離測定センサ１５aは、自律走行装置と走行面との距離を測定する部材であり、たとえば、光学的に距離を測定する測距センサや、超音波センサ等を利用すればよい。
走行面距離測定センサ１５aは、自律走行装置の車体の下部に、１つまたは複数個、固定設置される。
【0210】
図２１Ａは、自律走行装置の車輪１が走行面に接地して、走行面を走行している状態を示しており、走行面距離測定センサ１５aによって、車体と走行面の距離L1が、直接測定される。
距離L1は、たとえば、予め設定された最小距離Lminと、最大距離Lmaxと比較される。
この距離L1が、所定の距離範囲内（Lmin＜L1＜Lmax）であれば、車体持上検出部１５によって、車体が持ち上げられていないことが検出される。
距離L1は、たとえば、予め設定された最小距離Lminと、最大距離Lmaxと比較される。
この距離L1が、所定の距離範囲内（Lmin＜L1＜Lmax）であれば、車体持上検出部１５によって、車体が持ち上げられていないことが検出される。
【0211】
図２１Ｂは、自律走行装置の車輪１が走行面に接地しておらず、車体が持ち上げられている状態を示しており、走行面測距センサ１５aによって、車体と走行面の距離L2が測定される。
車体が持ち上げられているので、距離L2は、上記した距離L1よりも大きい。
この距離L2が、予め設定された所定の距離Lmaxよりも大きい場合（Lmax＜L2）、車体持上検出部１５によって、車体が持ち上げられていることが検出される。
車体が持ち上げられていることが検出された場合は、車体移動量は信頼性がなくなるので、カメラ移動量が選択され、自己位置推定部５は、カメラ移動量を利用して、自己位置の推定を行う。
車体が持ち上げられているので、距離L2は、上記した距離L1よりも大きい。
この距離L2が、予め設定された所定の距離Lmaxよりも大きい場合（Lmax＜L2）、車体持上検出部１５によって、車体が持ち上げられていることが検出される。
車体が持ち上げられていることが検出された場合は、車体移動量は信頼性がなくなるので、カメラ移動量が選択され、自己位置推定部５は、カメラ移動量を利用して、自己位置の推定を行う。
【0212】
図２２Ａと図２２Ｂに、この発明の自律走行装置に取り付けられた持上検出スイッチの一実施例の説明図を示す。
持上検出スイッチ１５bは、自律走行装置が走行面を走行している状態と自律走行装置の車体が持ち上げられた状態で、接点の開閉状態が変化するスイッチである。
図２２Ａに示すように、たとえば、持上検出スイッチ１５bは、押圧板１５cと組合わせて、自律走行装置の車体の下部に、固定設置される。
持上検出スイッチ１５bは、押しボタンスイッチであり、押しボタン部分が、押圧板１５cによって押し下げられるように設置される。
押圧板１５cは、走行面を走行している場合と車体が持ち上げられた場合では、その位置が垂直方向に移動するように、自律走行装置の車体の下部に設置される。
持上検出スイッチ１５bは、自律走行装置が走行面を走行している状態と自律走行装置の車体が持ち上げられた状態で、接点の開閉状態が変化するスイッチである。
図２２Ａに示すように、たとえば、持上検出スイッチ１５bは、押圧板１５cと組合わせて、自律走行装置の車体の下部に、固定設置される。
持上検出スイッチ１５bは、押しボタンスイッチであり、押しボタン部分が、押圧板１５cによって押し下げられるように設置される。
押圧板１５cは、走行面を走行している場合と車体が持ち上げられた場合では、その位置が垂直方向に移動するように、自律走行装置の車体の下部に設置される。
【0213】
図２２Ａは、自律走行装置の車輪１が走行面に接地して、走行面を走行している状態を示しており、持上検出スイッチ１５bの押しボタン部分と、押圧板１５cとは接触せずに、押しボタン部分が押し下げられていないものとする。
押しボタン部分が押し下げられていない場合、車体持上検出部１５によって、車体が持ち上げられていないことが検出される。
押しボタン部分が押し下げられていない場合、車体持上検出部１５によって、車体が持ち上げられていないことが検出される。
【0214】
図２２Ｂは、自律走行装置の車輪１が走行面に接地しておらず、車体が持ち上げられている状態を示しており、押圧板１５cが下方に移動し、持上検出スイッチ１５bの押しボタン部分と押圧板１５cとが接触し、押しボタン部分が押し下げられた状態になる。
押しボタン部分が押し下げられたことが検出されると、車体持上検出部１５によって、車体が持ち上げられていることが検出され、カメラ移動量が選択される。
自己位置推定部５は、選択されたカメラ移動量を利用して、自己位置の推定を行う。
押しボタン部分が押し下げられたことが検出されると、車体持上検出部１５によって、車体が持ち上げられていることが検出され、カメラ移動量が選択される。
自己位置推定部５は、選択されたカメラ移動量を利用して、自己位置の推定を行う。
【0215】
図２２Ａの場合、赤外線等を利用した高価な走行面距離測定センサ１５aと比較して、安価な部材であるスイッチを利用して、車体の持ち上げを検出することができ、車体持上検出部１５のコストを下げることができる。
【0216】
以上のように、第４の実施形態によれば、車体持上検出部（走行面距離測定センサ１５a、あるいは持上検出スイッチ１５b）によって、自律走行装置の車体が持ち上げられたことを検出し、車体が持ち上げられたことを検出した場合には、自己位置推定部がカメラ移動量を利用して自己位置の推定を行うので、車体が持ち上げられたことを確実に検出でき、車体が持ち上げられた状況でも、安定した自己位置推定を継続することができる。
【符号の説明】
【0217】
１ 車輪、
２ 移動量処理部、
３ ＬＩＤＥＲ、
４ 障害物検出部、
５ 自己位置推定部、
６ 地図情報、
７ 車体姿勢制御部、
１１ カメラ、
１３ 移動量切替検出部、
１４ 定置旋回制御部、
１５ 車体持上検出部、
１５ａ 走行面距離測定センサ、
１５ｂ 持上検出スイッチ、
１５ｃ 押圧板、
２０ 回転数取得部、
２１ 車輪移動量取得部、
２２ カメラ移動量取得部、
２３ 第１停止判定部、
２４ 第２停止判定部、
２５ 移動量選択部、
２６ 選択移動量出力部、
３１ 車輪移動量履歴保存部、
３２ 時刻監視部、
３５ カメラ特徴点監視部、
３６ カメラ移動量無効部、
４１ 慣性計測装置、
４２ 慣性情報取得部、
５１ 車輪速度監視部、
６１ 動作実行部、
６２ カメラ移動量判定基準変更部、
７１ 車輪移動量判定基準変更部、
７２ カメラ速度監視部、
１００ 自律走行装置、
１０１ 車輪、
１０２ 移動量算出部、
１０３ ＬＩＤＥＲ、
１０４ 障害物検出部、
１０５ 自己位置推定部、
１０６ 地図情報、
１０７ 車体姿勢制御部、
１１０ 障害物、
１１１ パーティクル
２ 移動量処理部、
３ ＬＩＤＥＲ、
４ 障害物検出部、
５ 自己位置推定部、
６ 地図情報、
７ 車体姿勢制御部、
１１ カメラ、
１３ 移動量切替検出部、
１４ 定置旋回制御部、
１５ 車体持上検出部、
１５ａ 走行面距離測定センサ、
１５ｂ 持上検出スイッチ、
１５ｃ 押圧板、
２０ 回転数取得部、
２１ 車輪移動量取得部、
２２ カメラ移動量取得部、
２３ 第１停止判定部、
２４ 第２停止判定部、
２５ 移動量選択部、
２６ 選択移動量出力部、
３１ 車輪移動量履歴保存部、
３２ 時刻監視部、
３５ カメラ特徴点監視部、
３６ カメラ移動量無効部、
４１ 慣性計測装置、
４２ 慣性情報取得部、
５１ 車輪速度監視部、
６１ 動作実行部、
６２ カメラ移動量判定基準変更部、
７１ 車輪移動量判定基準変更部、
７２ カメラ速度監視部、
１００ 自律走行装置、
１０１ 車輪、
１０２ 移動量算出部、
１０３ ＬＩＤＥＲ、
１０４ 障害物検出部、
１０５ 自己位置推定部、
１０６ 地図情報、
１０７ 車体姿勢制御部、
１１０ 障害物、
１１１ パーティクル
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】
【図15】
【図16】
【図17】
【図18】
【図19】
【図20】
【図21A】
【図21B】
【図22A】
【図22B】
【図23】
【図24】
【図25】
【図26】
【図27】
【図28】
【図29】
【図30A】
【図30B】
