JP4696088B2

JP4696088B2 - Autonomous moving body and moving method

Info

Publication number: JP4696088B2
Application number: JP2007073473A
Authority: JP
Inventors: 彬陳
Original assignee: Fujitsu Ltd
Current assignee: Fujitsu Ltd
Priority date: 2007-03-20
Filing date: 2007-03-20
Publication date: 2011-06-08
Anticipated expiration: 2027-03-20
Also published as: JP2008234350A

Description

この発明は、上位センサによる環境測定を行い、パーティクルフィルタを用いて自律移動体の状態を推定して当該自律移動体の移動を制御する上位制御機構と、下位センサによる検出値から自律移動体の状態を推定して当該自律移動体の移動を制御する下位制御機構とを備える自律移動体およびその移動方法に関し、特に、自律移動体の現在位置の推定精度を向上させることができる自律移動装置および移動方法に関するものである。 This invention measures the environment by a host sensor, estimates the state of the autonomous mobile body using a particle filter and controls the movement of the autonomous mobile body, and detects the autonomous mobile body from the detection value of the lower sensor. The present invention relates to an autonomous mobile body including a lower-level control mechanism that estimates the state and controls the movement of the autonomous mobile body, and its moving method, and in particular, an autonomous mobile device capable of improving the estimation accuracy of the current position of the autonomous mobile body and It relates to the movement method.

複雑な環境下で自律移動体（ロボット）が安定的なナビゲーションを実現するためには、図１４に示すように、外界情報を感知する分散センサネットワークと、各々のセンサ入力に応じて単純的な基本行動を選択する機構（下位制御機構）と、各センサの情報を統合して処理することによって最適な行動を選択する機構（上位制御機構）という３つが必要である。 In order to realize a stable navigation by an autonomous mobile body (robot) in a complex environment, as shown in FIG. 14, a distributed sensor network that senses external world information, and a simple response according to each sensor input. Three mechanisms are necessary: a mechanism for selecting a basic action (lower control mechanism) and a mechanism for selecting an optimal action by integrating and processing information from each sensor (upper control mechanism).

ここで、このような下位制御機構と上位制御機構とを統合したアーキテクチャはハイブリッドアーキテクチャと呼ばれている。自律移動体における従来の研究では、エンコーダや慣性センサやビジョンセンサなどの複数のセンサの情報をカルマンフィルタ（Kalman Filter）を用いて統合することが一般的であるが、この方法はハイブリットアーキテクチャ構成の自律移動体に適用しにくいという問題がある。 Here, an architecture in which such a low-level control mechanism and a high-level control mechanism are integrated is called a hybrid architecture. In conventional research on autonomous mobile bodies, it is common to integrate information from multiple sensors such as encoders, inertial sensors, and vision sensors using a Kalman filter, but this method is autonomous in a hybrid architecture configuration. There is a problem that it is difficult to apply to a moving body.

すなわち、カルマンフィルタによる方法では、自律移動体の構造モデルと運動モデルとに従い、カルマンフィルタが推定した過去の時間での自律移動体の運動量に基づいて、ある決まった時間サンプリングで次の時間での運動状態を予測することになるが、この時間サンプリングは処理データが多い場合に長くなる。 That is, in the method using the Kalman filter, the motion state at the next time with a certain time sampling based on the momentum of the autonomous mobile body at the past time estimated by the Kalman filter according to the structure model and the motion model of the autonomous mobile body. This time sampling becomes longer when there is a large amount of processing data.

一方、下位制御機構は、常に短い時間サンプリングで自律移動体と障害物との接近状態を確認しており、自律移動体が障害物や壁などに接近し過ぎるとその動きはスタックされ、下位制御機構は、減速、方向変更などの単純行動を選択して、スタックから脱出することを試みることになるが、この際、カルマンフィルタが予測した自律移動体の運動状態は実際の状況とは異なるものとなる。このようなことから、カルマンフィルタを用いてハイブリッドアーキテクチャ構成の自律移動体の最適な行動を制御するという方法は現実には適用しにくいという問題がある。 On the other hand, the subordinate control mechanism always confirms the approaching state of the autonomous mobile body and the obstacle with short time sampling, and if the autonomous mobile body gets too close to the obstacle or the wall, the movement is stacked, and the subordinate control The mechanism will select simple actions such as deceleration and direction change and attempt to escape from the stack, but the motion state of the autonomous mobile body predicted by the Kalman filter is different from the actual situation. Become. For this reason, there is a problem that the method of controlling the optimum behavior of an autonomous mobile body having a hybrid architecture configuration using a Kalman filter is difficult to apply in practice.

つまり、上位制御機構に実装しているカルマンフィルタが、実環境の状況変化を素早く把握して反応する下位制御機構の挙動を予測できないことから、カルマンフィルタを用いてハイブリッドアーキテクチャ構成の自律移動体の最適な行動を制御するという方法は現実には適用しにくいのである。 In other words, the Kalman filter implemented in the host control mechanism cannot quickly predict the behavior of the lower control mechanism that reacts by quickly grasping the situation change in the real environment. The method of controlling behavior is difficult to apply in reality.

また、パーティクルフィルタ（Particle Filter）を用いてハイブリッドアーキテクチャ構成の自律移動体の最適な行動を制御するという方法が検討されている（例えば、非特許文献１参照）。パーティクルフィルタもまた、カルマンフィルタと同様にパーティクルフィルタが推定した現在の自律移動体の運動状態に基づいて次の時間での運動状態を予測するという処理を行う。これらの理由から、カルマンフィルタによる方法と同じ理由により、パーティクルフィルタによる方法も適用しにくい。 Further, a method of controlling the optimum behavior of an autonomous mobile body having a hybrid architecture configuration using a particle filter has been studied (for example, see Non-Patent Document 1). Similarly to the Kalman filter, the particle filter also performs a process of predicting the motion state at the next time based on the current motion state of the autonomous mobile body estimated by the particle filter. For these reasons, it is difficult to apply the particle filter method for the same reason as the Kalman filter method.

さらに、自律移動体の移動量を計測するオドメトリ（エンコーダ）には不確かさ（曖昧さ）が含まれており、この不確かさを考慮せずにパーティクルフィルタを適用するならば、推定した自己位置の精度が著しく低くなることから、安定した自己位置測定は困難なものとなる。 Furthermore, the odometry (encoder) that measures the amount of movement of an autonomous mobile body contains uncertainty (ambiguity). If a particle filter is applied without taking this uncertainty into account, the estimated self-location Since the accuracy is significantly lowered, stable self-position measurement becomes difficult.

そこで、近年では、ハイブリッドアーキテクチャ構成の自律移動体の行動を制御すべくパーティクルフィルタを適用する際に、オドメトリの情報を直接利用して自律移動体の移動の予測量とすることで、パーティクルフィルタの適用を可能にする場合にあって、オドメトリの情報の不確かさを考慮して、複数のセンサからの情報を統合することにより、自律移動体の現在位置の推定精度を高める技術が提案されている。 Therefore, in recent years, when applying a particle filter to control the behavior of an autonomous mobile body with a hybrid architecture configuration, the odometry information is directly used as the predicted amount of movement of the autonomous mobile body. A technology has been proposed to improve the estimation accuracy of the current position of an autonomous mobile body by integrating the information from multiple sensors in consideration of the uncertainty of odometry information when it is applicable. .

M.Isard and A.Blake,“Condensation-conditional density propagation for visual tracking," IJCV, Vol.29, No.1, pp.5-28, 1998.M. Isard and A. Blake, “Condensation-conditional density propagation for visual tracking,” IJCV, Vol. 29, No. 1, pp. 5-28, 1998.

しかしながら、上述した従来の技術では、複数のセンサからの情報を統合し、自律移動体の現在位置の推定精度を高める場合に、各センサにおける状態推定値の確率分布に対して上限、下限（確率分布の上限と下限とがセンサの推定値の不確実さを意味する）をもうけ、パーティクルフィルタの状態推定値の確率分布を収束させているが、かかる上限と下限とを事前の評価データに基づいて設定しているため、センサの推定値の不確実さをうまく捕らえることができず、自律移動体の現在位置の推定精度が低下するという問題があった。 However, in the above-described conventional technology, when information from a plurality of sensors is integrated to improve the estimation accuracy of the current position of the autonomous mobile body, the upper and lower limits (probability) The upper and lower limits of the distribution mean the uncertainty of the sensor estimates), and the probability distribution of the state estimates of the particle filter is converged, but these upper and lower limits are based on prior evaluation data Therefore, there is a problem that the uncertainty of the estimated value of the sensor cannot be caught well and the estimation accuracy of the current position of the autonomous mobile body is lowered.

この発明は、上述した従来技術による問題点を解消するためになされたものであり、自律移動体の現在位置の推定精度を向上させることができる自律移動装置および自律移動方法を提供することを目的とする。 The present invention has been made to solve the above-described problems caused by the prior art, and an object thereof is to provide an autonomous mobile device and an autonomous mobile method that can improve the estimation accuracy of the current position of the autonomous mobile body. And

上述した課題を解決し、目的を達成するため、本発明は、上位センサによる環境測定を行い、パーティクルフィルタを用いて自律移動体の状態を推定して当該自律移動体の移動を制御する上位制御機構と、下位センサによる検出値から自律移動体の状態を推定して当該自律移動体の移動を制御する下位制御機構とを備える自律移動体であって、前記下位制御機構は、自律移動体の運動状態を予測するカルマンフィルタを備え、前記カルマンフィルタを用いて前記下位センサの検出値から自律移動体の期待値および分散を推定し、自律移動体の状態変化量の確率分布の上限および下限を設定する第１の設定手段と、前記環境測定により推定した各パーティクルにおける自律移動体の状態変化の信頼度に基づいて、自律移動体の状態変化の確率分布の上限および下限を設定する第２の設定手段と、自律移動体の状態毎に、前記第１および第２の設定手段の設定した上限を統合すると共に、前記第１および第２の設定手段の設定した下限を統合する統合手段と、前記統合手段の統合した上限および下限に基づいて、自律移動体の状態変化量の確率分布を算出する算出手段と、前記環境測定により推定した各パーティクルにおける自律移動体の状態変化の信頼度を、前記算出手段の算出した確率分布に基づいて修正する修正手段と、を備えることを特徴とする。 In order to solve the above-described problems and achieve the object, the present invention performs high-level control in which environmental measurement is performed by a high-order sensor, the state of the autonomous mobile body is estimated using a particle filter, and the movement of the autonomous mobile body is controlled. An autonomous mobile body comprising a mechanism and a lower-level control mechanism for controlling the movement of the autonomous mobile body by estimating the state of the autonomous mobile body from the detection value of the lower sensor, A Kalman filter for predicting a motion state, using the Kalman filter to estimate an expected value and variance of the autonomous mobile body from detection values of the lower sensors, and to set an upper limit and a lower limit of the probability distribution of the state change amount of the autonomous mobile body Based on the first setting means and the reliability of the state change of the autonomous mobile body in each particle estimated by the environmental measurement, the probability change of the state change of the autonomous mobile body A second setting means for setting an upper limit and a lower limit of the first setting means, and an upper limit set by the first setting means and the second setting means for each state of the autonomous mobile body. An integration unit that integrates the set lower limit, a calculation unit that calculates a probability distribution of the state change amount of the autonomous mobile body based on the upper limit and the lower limit integrated by the integration unit, and an autonomous in each particle estimated by the environmental measurement Correction means for correcting the reliability of the state change of the moving body based on the probability distribution calculated by the calculation means.

また、本発明は、上記発明において、前記統合手段は、前記第１および第２の設定手段の設定した上限を重み平均することで各上限を統合すると共に、前記第１および第２の設定手段の設定した下限を重み平均することで各下限を統合することを特徴とする。 Further, the present invention is the above invention, wherein the integrating means integrates the upper limits by weighted averaging the upper limits set by the first and second setting means, and the first and second setting means. Each of the lower limits is integrated by weighted averaging the lower limits set in (1).

また、本発明は、上記発明において、前記算出手段によって算出された確率分布に基づいて、前記下位制御機構に設定されている自律移動体の位置および姿勢の情報を修正する情報修正手段をさらに備えたことを特徴とする。 In the above invention, the present invention further includes information correction means for correcting the position and orientation information of the autonomous mobile body set in the lower control mechanism based on the probability distribution calculated by the calculation means. It is characterized by that.

また、本発明は、上位センサによる環境測定を行い、パーティクルフィルタを用いて自律移動体の状態を推定して当該自律移動体の移動を制御する上位制御機構と、下位センサによる検出値から自律移動体の状態を推定して当該自律移動体の移動を制御する下位制御機構とを備える自律移動体の移動方法であって、前記下位制御機構は、自律移動体の運動状態を予測するカルマンフィルタを備え、前記カルマンフィルタを用いて前記下位センサの検出値から自律移動体の期待値および分散を推定し、自律移動体の状態変化量の確率分布の上限および下限を設定する第１の設定工程と、前記環境測定により推定した各パーティクルにおける自律移動体の状態変化の信頼度に基づいて、自律移動体の状態変化の確率分布の上限および下限を設定する第２の設定工程と、自律移動体の状態毎に、前記第１および第２の設定工程により設定した上限を統合すると共に、前記第１および第２の設定工程により設定した下限を統合する統合工程と、前記統合工程において統合した上限および下限に基づいて、自律移動体の状態変化量の確率分布を算出する算出工程と、前記環境測定により推定した各パーティクルにおける自律移動体の状態変化の信頼度を、前記算出工程により算出した確率分布に基づいて修正する修正工程と、を含んだことを特徴とする。 The present invention also provides a host control mechanism for measuring the environment by a host sensor, estimating the state of the autonomous mobile body using a particle filter, and controlling the movement of the autonomous mobile body, and autonomously moving from the detection value of the lower sensor. A method for moving an autonomous mobile body comprising a lower-level control mechanism that estimates the state of the body and controls the movement of the autonomous mobile body, wherein the lower-level control mechanism includes a Kalman filter that predicts the motion state of the autonomous mobile body The first setting step of estimating the expected value and variance of the autonomous mobile body from the detection values of the lower sensors using the Kalman filter, and setting the upper and lower limits of the probability distribution of the state change amount of the autonomous mobile body, Set the upper and lower bounds for the probability distribution of state change of autonomous mobile objects based on the reliability of state change of autonomous mobile objects for each particle estimated from environmental measurements Integration of the second setting step and the upper limit set by the first and second setting steps and the lower limit set by the first and second setting steps for each state of the autonomous mobile body A step of calculating a probability distribution of the state change amount of the autonomous mobile body based on the upper limit and the lower limit integrated in the integration step, and the reliability of the state change of the autonomous mobile body in each particle estimated by the environmental measurement And a correction step of correcting the degree based on the probability distribution calculated by the calculation step.

また、本発明は、上記発明において、前記統合工程は、前記第１および第２の設定工程において設定した上限を重み平均することで各上限を統合すると共に、前記第１および第２の設定工程において設定した下限を重み平均することで各下限を統合することを特徴とする。 Further, the present invention is the above invention, wherein the integration step integrates the upper limits by weighted averaging the upper limits set in the first and second setting steps, and the first and second setting steps. Each lower limit is integrated by weighted averaging of the lower limits set in step (1).

また、本発明は、上記発明において、前記算出工程によって算出された確率分布に基づいて、前記下位制御機構に設定されている自律移動体の位置および姿勢の情報を修正する情報修正工程をさらに含んだことを特徴とする。 In the above invention, the present invention further includes an information correction step of correcting the position and orientation information of the autonomous mobile body set in the lower control mechanism based on the probability distribution calculated in the calculation step. It is characterized by that.

本発明によれば、カルマンフィルタを用いて下位センサの検出値から自律移動体の期待値および分散を推定し、自律移動体の状態変化量の確率分布の上限および下限を設定し、環境測定により推定した各パーティクルにおける自律移動体の状態変化の信頼度に基づいて、自律移動体の状態変化の確率分布の上限および下限を算出し、自律移動体の状態毎に各上限および下限を統合し、統合した上限および下限に基づいて、自律移動体の状態変化量の確率分布を算出し、環境計測により推定した各パーティクルにおける自律移動体の状態変化の信頼度を、算出した確率分布に基づいて修正するので、自律移動体の現在位置の推定精度を高めることができる。 According to the present invention, the expected value and variance of the autonomous mobile body are estimated from the detection values of the lower sensors using the Kalman filter, the upper and lower limits of the probability distribution of the state change amount of the autonomous mobile body are set, and estimated by environmental measurement Calculate the upper and lower limits of the probability distribution of the state change of the autonomous mobile body based on the reliability of the state change of the autonomous mobile body in each particle, and integrate the upper and lower limits for each state of the autonomous mobile body The probability distribution of the state change amount of the autonomous mobile body is calculated based on the upper limit and the lower limit, and the reliability of the state change of the autonomous mobile body in each particle estimated by the environmental measurement is corrected based on the calculated probability distribution. Therefore, the estimation accuracy of the current position of the autonomous mobile body can be increased.

本発明によれば、第１および第２の設定手段の設定した上限を重み平均することで各上限を統合すると共に、第１および第２の設定手段の設定した下限を重み平均することで各下限を統合するので、確率分布の上限および下限を精度よく設定することができる。 According to the present invention, each upper limit is integrated by weighted averaging the upper limits set by the first and second setting means, and each weighted average is calculated by averaging the lower limits set by the first and second setting means. Since the lower limits are integrated, the upper and lower limits of the probability distribution can be set with high accuracy.

本発明によれば、確率分布に基づいて、下位制御機構に設定されている自律移動体の位置および姿勢の情報を修正するので、自律移動体の移動制御を精度よく実施することができる。 According to the present invention, since the information on the position and orientation of the autonomous mobile body set in the lower control mechanism is corrected based on the probability distribution, the autonomous mobile body can be accurately controlled.

以下に添付図面を参照して、この発明に係る自律移動体および移動方法の好適な実施の形態を詳細に説明する。 Exemplary embodiments of an autonomous mobile body and a movement method according to the present invention will be explained below in detail with reference to the accompanying drawings.

以下において、本実施例にかかる自律移動体（ロボット）について詳細に説明する。図１は、本実施例にかかる自律移動体の構成の一例を示す図である。同図に示すように、この自律移動体１０は、車輪１１と、モータ１２と、エンコーダ１３と、ＣＰＵ１４と、移動量算出部１５と、上位制御機構１００と、下位制御機構２００とを備えて構成される。 Hereinafter, the autonomous mobile body (robot) according to the present embodiment will be described in detail. FIG. 1 is a diagram illustrating an example of the configuration of the autonomous mobile body according to the present embodiment. As shown in FIG. 1, the autonomous mobile body 10 includes wheels 11, a motor 12, an encoder 13, a CPU 14, a movement amount calculation unit 15, a higher control mechanism 100, and a lower control mechanism 200. Composed.

このうち、モータ１２は、下位制御機構２００からの制御信号に応答して車輪１１を回転させるモータであり、エンコーダ１３は、車輪の回転数等から自律移動体１０の移動量を測定する手段である。 Among these, the motor 12 is a motor that rotates the wheel 11 in response to a control signal from the lower control mechanism 200, and the encoder 13 is a means for measuring the amount of movement of the autonomous mobile body 10 from the rotational speed of the wheel. is there.

ＣＰＵ１４は、各種の処理手順を規定したプログラムや制御データを格納するための内部メモリを有し、これらによって種々の処理を実行する制御手段であり、特に、本発明に密接に関連するものとしては、環境のグローバル地図の情報（以下、地図情報と表記する）を保持しており、上位制御機構１００からの要求に応答して地図情報を上位制御機構１００に出力する。 The CPU 14 has an internal memory for storing programs that define various processing procedures and control data, and is a control means for executing various processes by using these. Particularly, the CPU 14 is closely related to the present invention. In addition, information on the global map of the environment (hereinafter referred to as map information) is held, and the map information is output to the host control mechanism 100 in response to a request from the host control mechanism 100.

移動量算出部１５は、エンコーダ１３、ジャイロ（ジャイロスコープ；図示略）、距離センサ（図示略）などから入力される自律移動体の移動量、自律移動体の向きなどに関する情報を基にして、自律移動体１０の移動量、移動方向の変化量などを算出し、算出した情報を上位制御機構１００、下位制御機構２００に出力する手段である。ステレオカメラ１６は、複数のカメラから構成され、各カメラによって撮影した自律移動体１０の周囲の環境を撮影する。ステレオカメラ１６は、撮影した映像の情報（以下、映像情報と表記する）を上位制御機構１００に出力する。 The movement amount calculation unit 15 is based on information about the movement amount of the autonomous mobile body, the orientation of the autonomous mobile body, and the like input from the encoder 13, a gyroscope (not shown), a distance sensor (not shown), and the like. This is a means for calculating the amount of movement of the autonomous mobile body 10, the amount of change in the direction of movement, and the like, and outputting the calculated information to the upper control mechanism 100 and lower control mechanism 200. The stereo camera 16 is composed of a plurality of cameras, and images the environment around the autonomous mobile body 10 captured by each camera. The stereo camera 16 outputs information of the captured video (hereinafter referred to as video information) to the host control mechanism 100.

上位制御機構１００は、ステレオカメラ１６から入力される環境情報、下位制御機構２００から入力される下位センサ（エンコーダ、ジャイロ、距離センサ）の情報を基にして、自律移動体の経路計画などを行う手段である。下位制御機構２００は、上位制御機構１００から出力される制御情報に基づいて、自律移動体１０の移動制御などを行う手段である。 The host control mechanism 100 performs route planning of the autonomous mobile body based on the environment information input from the stereo camera 16 and the information of the lower sensors (encoder, gyroscope, distance sensor) input from the lower control mechanism 200. Means. The lower control mechanism 200 is means for performing movement control of the autonomous mobile body 10 based on the control information output from the higher control mechanism 100.

ここで、上位制御機構１００および下位制御機構２００の構成について説明する。図２は、上位制御機構１００および下位制御機構２００の構成を示す機能ブロック図である。同図の左側に示すように、上位制御機構１００は、下位センサ情報取得モジュール１１０と、ステレオカメラ１２０（図１に示したステレオカメラ１６に対応）と、ステレオカメラ入力部１３０と、３次元環境計測部１４０と、自己位置推定モジュール１５０と、経路計画モジュール１６０と、を備えて構成される。 Here, the configurations of the upper control mechanism 100 and the lower control mechanism 200 will be described. FIG. 2 is a functional block diagram showing configurations of the upper control mechanism 100 and the lower control mechanism 200. As shown on the left side of the figure, the host control mechanism 100 includes a lower sensor information acquisition module 110, a stereo camera 120 (corresponding to the stereo camera 16 shown in FIG. 1), a stereo camera input unit 130, and a three-dimensional environment. A measurement unit 140, a self-position estimation module 150, and a route planning module 160 are provided.

このうち、下位センサ情報取得モジュール１１０は、下位制御機構２００から送られてくる下位センサ情報（下位センサ系２１０が検出した各検出値、カルマンフィルタ２７０に推定された自律移動体１０の期待値と分散の情報等）を取得する手段である。ステレオカメラ入力部１３０は、ステレオカメラ１２０が撮影した映像情報を３次元環境計測部１４０に入力する手段である。 Among these, the lower sensor information acquisition module 110 receives the lower sensor information sent from the lower control mechanism 200 (the detected values detected by the lower sensor system 210, the expected value of the autonomous mobile body 10 estimated by the Kalman filter 270, and the variance). This is a means for acquiring information etc.). The stereo camera input unit 130 is means for inputting video information captured by the stereo camera 120 to the three-dimensional environment measurement unit 140.

３次元環境計測部１４０は、ステレオカメラ入力部１３０から入力された映像情報に基づいて３次元環境計測（ビジョンの計測）を実行する手段である。例えば、３次元環境計測部１４０は、映像情報から特徴点（床や壁など）およびステレオカメラの位置関係から三角法を用いて自律移動体１０から各特徴点までの距離を算出し、ビジョンを計測する。 The three-dimensional environment measurement unit 140 is a unit that performs three-dimensional environment measurement (vision measurement) based on video information input from the stereo camera input unit 130. For example, the 3D environment measurement unit 140 calculates the distance from the autonomous mobile body 10 to each feature point using trigonometry from the positional information of the feature point (floor, wall, etc.) and the stereo camera from the video information, and measures the vision. To do.

自己位置推定モジュール１５０は、３次元環境計測部１４０の測定結果を使用し、パーティクルフィルタ（Particle Filter）の手法に基づいて自律移動体の自律位置を推定する手段である。経路計画モジュール１６０は、自己位置推定モジュール１５０の自己位置推定結果に基づいて自律移動体の経路を計画する手段である。 The self-position estimation module 150 is means for estimating the autonomous position of the autonomous mobile body based on the particle filter technique using the measurement result of the three-dimensional environment measurement unit 140. The route planning module 160 is a means for planning the route of the autonomous mobile body based on the self-position estimation result of the self-position estimation module 150.

一方、図２の右側に示すように、下位制御機構２００は、下位センサ系２１０と、センサデータ記憶モジュール２２０と、行動情報受付モジュール２３０と、単純反応処理モジュール２４０と、行動選択機構２５０と、自律移動体駆動機構２６０と、カルマンフィルタ２７０とを備えて構成される。 On the other hand, as shown on the right side of FIG. 2, the lower control mechanism 200 includes a lower sensor system 210, a sensor data storage module 220, a behavior information reception module 230, a simple reaction processing module 240, a behavior selection mechanism 250, An autonomous mobile drive mechanism 260 and a Kalman filter 270 are provided.

下位制御機構２００の備える下位センサ系２１０は、自律移動体１０のＸ、Ｙ方向の移動量と向きの変化θを検出するエンコーダ２１１，２１２（図１のエンコーダ１３に対応する）と、自律移動体１０の移動方向を検出するジャイロ２１３と、自律移動体１０の距離を測定する距離センサ２１４などのセンサで構成されている。 The lower-level sensor system 210 included in the lower-level control mechanism 200 includes encoders 211 and 212 (corresponding to the encoder 13 in FIG. 1) that detect movement amounts and direction changes θ of the autonomous mobile body 10 and autonomous movement. A gyro 213 that detects the moving direction of the body 10 and a sensor such as a distance sensor 214 that measures the distance of the autonomous moving body 10 are configured.

ここで、本発明では、ジャイロ２１３を使って自律移動体１０の移動方向を検出するという方法を用いる他に、エンコーダ２１１の検出する移動量とエンコーダ２１２の検出する移動量とに基づいて自律移動体１０の移動方向を検出するという手法を用いる。 Here, in the present invention, in addition to using the method of detecting the movement direction of the autonomous mobile body 10 using the gyro 213, autonomous movement is performed based on the movement amount detected by the encoder 211 and the movement amount detected by the encoder 212. A method of detecting the moving direction of the body 10 is used.

また、行動選択機構２５０は、距離センサ２１４が自律移動体１０と障害物との間の距離が近いことを検出する場合には、単純反応処理モジュール２４０の出力する行動情報を選択して自律移動体駆動機構２６０に渡し、距離センサ２１４が自律移動体１０と障害物との間の距離が遠いことを検出する場合には、行動情報受付モジュール２３０の受け付けた行動情報を選択して自律移動体駆動機構２６０に渡すように処理する。 In addition, when the distance sensor 214 detects that the distance between the autonomous mobile body 10 and the obstacle is short, the behavior selection mechanism 250 selects the behavior information output by the simple reaction processing module 240 and moves autonomously. When the distance sensor 214 detects that the distance between the autonomous mobile body 10 and the obstacle is long, the behavior information received by the behavior information reception module 230 is selected and the autonomous mobile body Processing is performed so as to pass to the drive mechanism 260.

自律移動体駆動機構２６０は、行動選択機構２５０から出力される行動情報を取得した場合に、取得した行動情報に従って自律移動体１０を駆動させるべく、モータ１２（図１参照）に制御信号を出力する。 When the autonomous mobile body driving mechanism 260 acquires the behavior information output from the behavior selection mechanism 250, the autonomous mobile body driving mechanism 260 outputs a control signal to the motor 12 (see FIG. 1) to drive the autonomous mobile body 10 according to the acquired behavior information. To do.

カルマンフィルタ２７０は、予め設定された自律移動体１０の構造モデルと運動モデルとに従い、推定した過去の時間での自律移動体１０の運動量に基づいて、ある決まった時間サンプリングで次の時間での運動状態を予測する。特に、本実施例では、カルマンフィルタ２７０は、エンコーダ２１１，２１２およびジャイロ２１３の検出値から自律移動体１０の期待値（自律移動体１０の移動量ｕ、ｖおよび変化量θの期待値）および分散（自律移動体１０の移動量ｕ、ｖおよび変化量θの分散）を推定して、上位制御機構１００に出力する。 The Kalman filter 270 follows the preset structure model and motion model of the autonomous mobile body 10, and based on the estimated momentum of the autonomous mobile body 10 in the past time, the motion at the next time with a certain time sampling. Predict the state. In particular, in this embodiment, the Kalman filter 270 determines the expected values of the autonomous mobile body 10 (the expected values of the movement amounts u and v of the autonomous mobile body 10 and the change amount θ) and the variance from the detection values of the encoders 211 and 212 and the gyro 213 (Dispersion of the movement amounts u and v of the autonomous mobile body 10 and the change amount θ) is estimated and output to the host control mechanism 100.

また、カルマンフィルタ２７０は、エンコーダ２１１，２１２の検出値（ジャイロ２１３の検出値を含まない）から自律移動体の期待値（自律移動体１０の移動量ｕ、ｖおよび変化量θの期待値）および分散（自律移動体１０の移動量ｕ、ｖおよび変化量θの分散）を推定して上位制御機構１００に出力する。本実施例において利用されるカルマンフィルタは、周知技術のカルマンフィルタと同様である。 Further, the Kalman filter 270 calculates the expected value of the autonomous mobile body (the expected values of the movement amounts u and v of the autonomous mobile body 10 and the variation θ) from the detection values of the encoders 211 and 212 (not including the detection value of the gyro 213). The variance (the variance of the movement amounts u and v of the autonomous mobile body 10 and the variation θ) is estimated and output to the host control mechanism 100. The Kalman filter used in the present embodiment is the same as a well-known Kalman filter.

図２に示される上位制御機構１００および下位制御機構２００に従って、自律移動体１０は、障害物との間の距離が近い場合には、下位センサ系２１０のセンサデータに基づいて決定された行動情報に従って行動し、障害物との間の距離が遠い場合には、パーティクルフィルタによる自己位置推定結果に基づいて決定された行動情報に従って行動することになる。 In accordance with the upper control mechanism 100 and the lower control mechanism 200 shown in FIG. 2, the autonomous mobile body 10 determines the behavior information determined based on the sensor data of the lower sensor system 210 when the distance to the obstacle is short. If the distance to the obstacle is far, the user will act according to behavior information determined based on the self-position estimation result by the particle filter.

続いて、本発明を実現すべく備えられる自己位置推定モジュール１５０の構成例について説明する。図３は、本実施例にかかる自己位置推定モジュール１５０の構成例を示す図である。同図に示すように、この自己位置推定モジュール１５０は、パーティクルフィルタ３００と、エンコーダ・ジャイロ推定確率分布処理部３０１と、エンコーダ推定確率分布処理部３０２と、ビジョン推定確率分布処理部３０３と、上限重み平均算出部３０４と、下限重み平均算出部３０５と、Ｐｌｓ分布生成部３０６と、統合処理部３０７とを備えて構成される。 Next, a configuration example of the self-position estimation module 150 provided to realize the present invention will be described. FIG. 3 is a diagram illustrating a configuration example of the self-position estimation module 150 according to the present embodiment. As shown in the figure, the self-position estimation module 150 includes a particle filter 300, an encoder / gyro estimation probability distribution processing unit 301, an encoder estimation probability distribution processing unit 302, a vision estimation probability distribution processing unit 303, and an upper limit. A weighted average calculating unit 304, a lower limit weighted average calculating unit 305, a Pls distribution generating unit 306, and an integration processing unit 307 are configured.

このうち、パーティクルフィルタ３００は、パーティクル（自律移動体１０のＸ軸方向の移動量ｕ、自律移動体１０のＹ軸方向の移動量ｖ、自律移動体１０の移動方向の変化量θ）のリサンプリング（予測処理）、サンプリングの評価（更新処理）を行い、ビジョンによる信頼度の分布であるビジョン確率分布を得て、このビジョン確率分布に基づいて、パーティクルのリサンプリングとサンプリングの評価を行うことを繰り返すことで、自律移動体の自己位置を推定する処理を行う。 Among these, the particle filter 300 is configured to reduce particles (the movement amount u of the autonomous mobile body 10 in the X-axis direction, the movement amount v of the autonomous mobile body 10 in the Y-axis direction, and the change amount θ of the movement direction of the autonomous mobile body 10). Sampling (prediction processing), sampling evaluation (update processing), obtaining vision probability distribution, which is the distribution of reliability by vision, and performing particle resampling and sampling evaluation based on this vision probability distribution By repeating the above, the process of estimating the self position of the autonomous mobile body is performed.

ここで、ビジョン確率分布３０８は、横軸がパーティクルの位置を示し、縦軸が信頼度を示すものであって、パーティクルフィルタ３００により、移動量ｕ（前回処理時の位置からのＸ軸方向の移動量）、移動量ｖ（前回処理時の位置からＹ軸方向の移動量）、変化量θ（前回処理時の移動方向からの変化量）のそれぞれについて生成されることになる。 Here, in the vision probability distribution 308, the horizontal axis indicates the position of the particle, and the vertical axis indicates the reliability. The particle filter 300 causes the movement amount u (in the X-axis direction from the position at the previous processing). (Movement amount), movement amount v (movement amount in the Y-axis direction from the position at the time of the previous processing), and change amount θ (change amount from the movement direction at the time of the previous processing).

エンコーダ・ジャイロ推定確率分布処理部３０１は、エンコーダ２１１，２１２、およびジャイロ２１３により検出された検出値に基づいてカルマンフィルタ２７０に推定された自律移動体１０の移動量ｕ、移動量ｖ、変化量θの期待値および分散を取得し、取得した移動量ｕ、移動量ｖ、変化量θの期待値についての推定確率分布を生成し、移動量ｕ、移動量ｖ、変化量θの分散に基づいて、推定確率分布の上限および下限を設定する。 The encoder / gyro estimated probability distribution processing unit 301 includes the movement amount u, the movement amount v, and the change amount θ of the autonomous moving body 10 estimated by the Kalman filter 270 based on the detection values detected by the encoders 211 and 212 and the gyro 213. Are obtained, and an estimated probability distribution is generated for the obtained expected values of the movement amount u, the movement amount v, and the change amount θ, and based on the variance of the movement amount u, the movement amount v, and the change amount θ. Set the upper and lower limits of the estimated probability distribution.

エンコーダ・ジャイロ推定確率分布処理部３０１は、カルマンフィルタ２７０によって推定された移動量ｕについての分散から、移動量ｕについての推定確率分布の上限・下限を算出する（移動量ｕについての分散に対して予め設定された値からガウス分布を求め、このガウス分布を積分することによって、推定確率分布の上限・下限を算出する）。移動量ｖおよび変化量θについても同様に、各分散からガウス分布を求め、かかるガウス分布を積分することによって、推定確率分布の上限・下限（移動量ｖに対応する推定確率分布の上限・下限、変化量θに対応する推定確率分布の上限・下限）を算出する。 The encoder / gyro estimated probability distribution processing unit 301 calculates the upper and lower limits of the estimated probability distribution for the moving amount u from the variance for the moving amount u estimated by the Kalman filter 270 (with respect to the variance for the moving amount u). The upper and lower limits of the estimated probability distribution are calculated by obtaining a Gaussian distribution from preset values and integrating the Gaussian distribution). Similarly, for the movement amount v and the change amount θ, a Gaussian distribution is obtained from each variance, and the Gaussian distribution is integrated to obtain the upper and lower limits of the estimated probability distribution (the upper and lower limits of the estimated probability distribution corresponding to the movement amount v). , An upper limit / lower limit of the estimated probability distribution corresponding to the change amount θ.

エンコーダ推定確率分布処理部３０２は、エンコーダ２１１，２１２により検出された検出値に基づいてカルマンフィルタ２７０により推定された自律移動体１０の移動方向の変化量θの期待値および分散を取得し、取得した移動方向の変化量θの期待値についての推定確率分布を生成し、変化量θの分散に基づいて、推定確率分布の上限および下限を設定する。 The encoder estimated probability distribution processing unit 302 acquires the expected value and variance of the change amount θ in the movement direction of the autonomous mobile body 10 estimated by the Kalman filter 270 based on the detection values detected by the encoders 211 and 212, and acquires An estimated probability distribution for the expected value of the change amount θ in the moving direction is generated, and an upper limit and a lower limit of the estimated probability distribution are set based on the variance of the change amount θ.

また、エンコーダ推定確率分布処理部３０２は、カルマンフィルタ２７０によって推定された変化量θについての分散から、変化量θについての推定確率分布の上限・下限を算出する（変化量θについての分散に対して予め設定された値からガウス分布を求め、このガウス分布を積分することによって、推定確率分布の上限・下限を算出する）。 Further, the encoder estimated probability distribution processing unit 302 calculates the upper and lower limits of the estimated probability distribution for the change amount θ from the variance for the change amount θ estimated by the Kalman filter 270 (with respect to the variance for the change amount θ). The upper and lower limits of the estimated probability distribution are calculated by obtaining a Gaussian distribution from preset values and integrating the Gaussian distribution).

ビジョン推定確率分布処理部３０３は、パーティクルフィルタ３００により生成されたビジョン確率分布３０８を取得し、取得したビジョン確率分布３０８に基づいて、ビジョンにより検出された自律移動体の移動量ｕ、ｖについての推定信頼度分布を生成して、その生成した推定信頼度分布の上限および下限を設定すると共に、ビジョンにより検出された自律移動体１０の移動方向の変化量θについての推定信頼度分布を生成して、その生成した推定信頼度分布の上限および下限を設定する。 The vision estimation probability distribution processing unit 303 acquires the vision probability distribution 308 generated by the particle filter 300, and based on the acquired vision probability distribution 308, the movement amounts u and v of the autonomous moving body detected by the vision are obtained. An estimated reliability distribution is generated, an upper limit and a lower limit of the generated estimated reliability distribution are set, and an estimated reliability distribution is generated for the change amount θ of the movement direction of the autonomous mobile body 10 detected by the vision. Then, an upper limit and a lower limit of the generated estimated reliability distribution are set.

上限重み平均算出部３０４は、自律移動体１０の移動量ｕについて、エンコーダ・ジャイロ推定確率分布処理部３０１の設定した推定確率分布の上限と、ビジョン推定確率分布処理部３０３の設定した推定信頼度分布の上限とを重み平均することで統合する。 The upper limit weight average calculation unit 304 sets the upper limit of the estimated probability distribution set by the encoder / gyro estimated probability distribution processing unit 301 and the estimated reliability set by the vision estimated probability distribution processing unit 303 for the movement amount u of the autonomous mobile body 10. Integration by weighted averaging with the upper limit of the distribution.

また、上限重み平均算出部３０４は、自律移動体１０の移動量ｖについて、エンコーダ・ジャイロ推定確率分布処理部３０１の設定した推定確率分布の上限と、ビジョン推定確率分布処理部３０３の設定した推定確率分布の上限とを重み平均することで統合する。 The upper limit weight average calculation unit 304 also sets the upper limit of the estimated probability distribution set by the encoder / gyro estimated probability distribution processing unit 301 and the estimation set by the vision estimated probability distribution processing unit 303 for the movement amount v of the autonomous mobile body 10. Integration is performed by weighted averaging with the upper limit of the probability distribution.

また、上限重み平均算出部３０４は、自律移動体１０の移動方向の変化量θについて、エンコーダ・ジャイロ推定確率分布処理部３０１の設定した推定確率分布の上限と、エンコーダ推定確率分布処理部３０２の設定した推定確率分布の上限と、ビジョン推定確率分布処理部３０３が設定した推定確率分布の上限とを重み平均することで統合する。 Further, the upper limit weight average calculating unit 304 sets the upper limit of the estimated probability distribution set by the encoder / gyro estimated probability distribution processing unit 301 and the encoder estimated probability distribution processing unit 302 for the change amount θ in the moving direction of the autonomous mobile body 10. The upper limit of the set estimated probability distribution and the upper limit of the estimated probability distribution set by the vision estimated probability distribution processing unit 303 are integrated by weighted averaging.

下限重み平均算出部３０５は、自律移動体１０の移動量ｕについて、エンコーダ・ジャイロ推定確率分布処理部３０１の設定した推定確率分布の下限と、ビジョン推定確率分布処理部３０３の設定した推定確率分布の下限とを重み平均することで統合する。 The lower limit weight average calculation unit 305 sets the lower limit of the estimated probability distribution set by the encoder / gyro estimated probability distribution processing unit 301 and the estimated probability distribution set by the vision estimated probability distribution processing unit 303 for the movement amount u of the autonomous mobile body 10. Are integrated by weighted averaging with the lower limit of.

また、下限重み平均算出部３０５は、自律移動体１０の移動量ｖについて、エンコーダ・ジャイロ推定確率分布処理部３０１の設定した推定確率分布の下限と、ビジョン推定確率分布処理部３０３の設定した推定確率分布の下限とを重み平均することで統合する。 The lower limit weight average calculation unit 305 also sets the lower limit of the estimated probability distribution set by the encoder / gyro estimated probability distribution processing unit 301 and the estimation set by the vision estimated probability distribution processing unit 303 for the movement amount v of the autonomous mobile body 10. Integration by weighted averaging of the lower limit of the probability distribution.

また、下限重み平均算出部３０５は、自律移動体１０の移動方向の変化量θについて、エンコーダ・ジャイロ推定確率分布処理部３０１の設定した推定確率分布の下限と、エンコーダ推定確率分布処理部３０２の設定した推定確率分布の下限と、ビジョン推定確率分布処理部３０３の設定した推定確率分布の下限とを重み平均することで統合する。 Further, the lower limit weight average calculation unit 305 determines the lower limit of the estimated probability distribution set by the encoder / gyro estimated probability distribution processing unit 301 and the encoder estimated probability distribution processing unit 302 for the change amount θ in the moving direction of the autonomous moving body 10. The lower limit of the set estimated probability distribution and the lower limit of the estimated probability distribution set by the vision estimated probability distribution processing unit 303 are integrated by weighted averaging.

Ｐｌｓ分布生成部３０６は、各パーティクルｓ_i＝（ｕ_i，ｖ_i，θ_i）に対して、下記処理を行い、信頼度（Ｐｌｓ）_iを計算することによって、Ｐｌｓ分布を生成する。自律移動体１０の移動量ｕについて、上限重み平均算出部３０４の統合した上限と下限重み平均算出部３０５の統合した下限とに対してDempster-Shafer確率モデルを適用することで、自律移動体１０の真の移動量ｕが区間[ｕ_i−ｕ’，ｕ_i＋ｕ’]に属する信頼度を示す

を計算する。 The Pls distribution generation unit 306 generates the Pls distribution by performing the following processing on each particle s _i = (u _i , v _i , θ _i ) and calculating the reliability (Pls) _i . By applying the Dempster-Shafer probability model to the upper limit integrated by the upper limit weight average calculation unit 304 and the lower limit integrated by the lower limit weight average calculation unit 305 for the movement amount u of the autonomous mobile body 10, the autonomous mobile body 10 Indicates the reliability that the true movement amount u belongs to the section [u _i −u ′, u _i + u ′]

Calculate

また、Ｐｌｓ分布生成部３０６は、自律移動体１０の移動量ｖについて、上限重み平均算出部３０４の統合した上限と下限重み平均算出部３０５の統合した下限とに対してDempster-Shafer確率モデルを適用することで、自律移動体１０の真の移動量ｖが区間[ｖ_i−ｖ’，ｖ_i＋ｖ’]に属する信頼度を示す

を計算する。 In addition, the Pls distribution generation unit 306 calculates a Dempster-Shafer probability model for the movement amount v of the autonomous mobile body 10 with respect to the upper limit integrated by the upper limit weight average calculation unit 304 and the lower limit integrated by the lower limit weight average calculation unit 305. By applying, the true moving amount v of the autonomous mobile body 10 indicates the reliability that belongs to the section [v _i −v ′, v _i + v ′].

Calculate

また、Ｐｌｓ分布生成部３０６は、自律移動体１０の移動方向の変化量θについて、上限重み平均算出部３０４の統合した上限と下限重み平均算出部３０５の統合した下限とに対してDempster-Shafer確率モデルを適用することで、自律移動体１０の真の移動方向の変化量θが区間[θ_i−θ’，θ_i＋θ’]に属する信頼度を示す

を計算する。 In addition, the Pls distribution generation unit 306 performs Dempster-Shafer with respect to the upper limit integrated by the upper limit weight average calculation unit 304 and the lower limit integrated by the lower limit weight average calculation unit 305 for the change amount θ in the movement direction of the autonomous mobile body 10. By applying the probability model, the degree of reliability θ in the true movement direction of the autonomous mobile body 10 indicates the reliability belonging to the section [θ _i −θ ′, θ _i + θ ′].

Calculate

ただし、ｕ’，ｖ’，θ’は、事前に決めたパラメータである。
ここで、

とする。 However, u ′, v ′, and θ ′ are parameters determined in advance.
here,

And

統合処理部３０７は、パーティクルフィルタ３００により生成された自律移動体１０の移動量（ｕ，ｖ，θ）についてのビジョン確率分布３０８と、Ｐｌｓ分布生成部３０６により生成された自律移動体の移動量（ｕ，ｖ，θ）についてのＰｌｓ分布生成部３０６とを統合することで、パーティクルフィルタ３００のリサンプリングに用いられる自律移動体の移動量（ｕ，ｖ，θ）についてのビジョン確率分布（ビジョン確率分布３０８に代わるもの）を生成する。 The integration processing unit 307 includes the vision probability distribution 308 for the movement amount (u, v, θ) of the autonomous mobile body 10 generated by the particle filter 300 and the movement amount of the autonomous mobile body generated by the Pls distribution generation unit 306. By integrating the Pls distribution generation unit 306 for (u, v, θ), the vision probability distribution (vision) for the movement amount (u, v, θ) of the autonomous moving body used for resampling of the particle filter 300 In place of the probability distribution 308).

つぎに、図３のように構成される自己位置推定モジュール１５０の実行する処理について説明する。パーティクルフィルタ３００は、パーティクルのリサンプリングとサンプリングの評価を行い、ビジョンによる信頼度の分布であるビジョン確率分布３０８を生成する。 Next, processing executed by the self-position estimation module 150 configured as shown in FIG. 3 will be described. The particle filter 300 performs resampling of the particles and evaluation of the sampling, and generates a vision probability distribution 308 that is a reliability distribution by vision.

このビジョン確率分布３０８の生成を受けて、エンコーダ・ジャイロ推定確率分布処理部３０１は、エンコーダ２１１，２１２により検出された移動量ｕ、ｖ（移動量の期待値ｕ、ｖ）についての推定確率分布を生成し、移動量の分散から推定確率分布の上限、下限を設定する。また、エンコーダ・ジャイロ推定確率分布処理部３０１は、移動方向の変化量θ（変化量の期待値θ）についての推定確率分布を生成し、変化量の分散から推定確率分布の上限、下限を設定する。 In response to the generation of the vision probability distribution 308, the encoder / gyro estimated probability distribution processing unit 301 estimates the estimated probability distribution for the movement amounts u and v (expected values u and v of the movement amount) detected by the encoders 211 and 212. And the upper and lower limits of the estimated probability distribution are set from the variance of the movement amount. Also, the encoder / gyro estimated probability distribution processing unit 301 generates an estimated probability distribution for the change amount θ in the moving direction (expected value θ of change amount), and sets the upper and lower limits of the estimated probability distribution from the variance of the change amount. To do.

図４は、推定確率分布の生成を説明するための図であり、図５は、上限・下限の設定を説明するための図である。図４に示すように、エンコーダ２１１，２１２により検出された自律移動体１０の移動量ｕの近傍に三角分布で定義される確率分布の集合を想定することで、その移動量ｕについての推定確率分布を生成する。 FIG. 4 is a diagram for explaining generation of an estimated probability distribution, and FIG. 5 is a diagram for explaining setting of upper and lower limits. As shown in FIG. 4, by assuming a set of probability distributions defined by a triangular distribution in the vicinity of the movement amount u of the autonomous mobile body 10 detected by the encoders 211 and 212, an estimated probability for the movement amount u. Generate a distribution.

また、図５に示すように、分散が±αである場合には、期待値（移動量ｕ）から−αの位置におけるガウス分布を求め、期待値から＋αの位置におけるガウス分布を求める。各位置におけるガウス分布は予め設定されているものとする。そして、各ガウス分布を積分することによって、推定確率分布の上限および下限を算出することができる。 As shown in FIG. 5, when the variance is ± α, a Gaussian distribution at the position −α is obtained from the expected value (movement amount u), and a Gaussian distribution at the position + α is obtained from the expected value. It is assumed that the Gaussian distribution at each position is set in advance. Then, by integrating each Gaussian distribution, the upper and lower limits of the estimated probability distribution can be calculated.

一方、ビジョン確率分布３０８の生成を受けて、エンコーダ推定確率分布処理部３０２は、エンコーダ・ジャイロ推定確率分布処理部３０１と同様の処理に従って、エンコーダ２１１、２１２により検出された自律移動体１０の移動方向の変化量θについての推定確率分布を生成すると共に、分散に対応するガウス分布を積分することによって、推定確率分布の上限および下限を算出する。 On the other hand, in response to the generation of the vision probability distribution 308, the encoder estimated probability distribution processing unit 302 moves the autonomous mobile body 10 detected by the encoders 211 and 212 according to the same processing as the encoder / gyro estimated probability distribution processing unit 301. An upper limit and a lower limit of the estimated probability distribution are calculated by generating an estimated probability distribution for the direction change amount θ and integrating the Gaussian distribution corresponding to the variance.

一方、ビジョン確率分布３０８の生成を受けて、ビジョン推定確率分布処理部３０３は、ビジョン確率分布３０８に基づいて、ビジョンにより検出された自律移動体１０の移動量ｕ、ｖについての推定確率分布を生成して、その生成した推定確率分布の上限および下限を設定すると共に、ビジョンにより検出された自律移動体１０の移動方向の変化量θについての推定確率分布を生成して、その生成した推定確率分布の上限および下限を設定する。 On the other hand, upon receiving the generation of the vision probability distribution 308, the vision estimation probability distribution processing unit 303 calculates the estimated probability distribution for the movement amounts u and v of the autonomous mobile body 10 detected by the vision based on the vision probability distribution 308. Generating an upper limit and a lower limit of the generated estimated probability distribution, and generating an estimated probability distribution for the change amount θ of the moving direction of the autonomous mobile body 10 detected by the vision, and generating the generated estimated probability Set the upper and lower limits of the distribution.

例えば、移動量ｕについての推定確率分布の上限および下限の設定処理で説明するならば、（ｕ_i，ｖ_i，θ_i）という３次元空間値を持つパーティクルＳ_i（例えば、ｉ＝１〜５００）のそれぞれについてパーティクルＳ_iを中心とする近傍空間を定義して、区間[ｕ_i−ｕ’，ｕ_i＋ｕ’]の中に入る全てのパーティクルについて、パーティクルＳ_iの近傍空間に入るのか否かをチェックして、近傍空間に入ると判定したパーティクルＳ_iの信頼度の平均値ｍ_iを算出することで、
[[ｕ₁−ｕ’，ｕ₁＋ｕ’]，ｍ₁]
[[ｕ₂−ｕ’，ｕ₂＋ｕ’]，ｍ₂]
〜
[[ｕ₅₀₀−ｕ’，ｕ₅₀₀＋ｕ’]，ｍ₅₀₀]
という集合を生成して、
上限＝Σｍ_i ただし、ａ∩ｂ≠φの総和を算出する
・・・（１）式
下限＝Σｍ_i ただし、ａ⊆ｂ≠φの総和を算出する
・・・（２）式
という算出式に従って、図６に示すように、ビジョンにより検出された自律移動体１０の移動量ｕについての推定確率分布の上限および下限を設定する。図６は、ビジョンにより検出された自律移動体の移動量ｕについての推定確率分布の上限・下限の設定手法を説明するための図である。 For example, if described in the process of setting upper and lower limits of the estimated probability distribution regarding the movement amount _{_{u, (u i, v i}} , θ i) particles S _i having a three-dimensional space value of (e.g., i =. 1 to 500), a neighborhood space centered on the particle S _i is defined, and whether all particles that fall within the interval [u _i −u ′, u _i + u ′] enter the neighborhood space of the particle S _i . By checking whether or not and calculating the average value m _i of the reliability of the particles S _i determined to enter the neighboring space,
[[u ₁ −u ′, u ₁ + u ′], m ₁ ]
[[u ₂ −u ′, u ₂ + u ′], m ₂ ]
~
[[u ₅₀₀ −u ′, u ₅₀₀ + u ′], m ₅₀₀ ]
Generate a set called
Upper limit = Σm _{i where} the sum of a∩b ≠ φ is calculated (1) Formula lower limit = Σm _{i where} the sum of a⊆b ≠ φ is calculated ... According to the formula (2) As shown in FIG. 6, the upper limit and the lower limit of the estimated probability distribution for the movement amount u of the autonomous mobile body 10 detected by the vision are set. FIG. 6 is a diagram for explaining a method for setting an upper limit and a lower limit of the estimated probability distribution for the movement amount u of the autonomous mobile body detected by the vision.

ここで、（１）式および（２）式は、Dempster-Shafer確率モデルで使用されている式であり、Dempster-Shafer確率モデルについて記載する下記の文献では、（１）式についてはＰｌｓ（ｂ）として記載し、（２）式についてはＢｅｌ（ｂ）と記載している。
（Dempster-Shafer確率モデルの文献）
S.Ferson, V.Kreinovich, L.Ginzburg, D.Myers, and K.Sents, "Constructing probability boxes and DempsterShafer structures," Tecnical report, Sandia National Laboratories, 2003 Here, the expressions (1) and (2) are expressions used in the Dempster-Shafer probability model. In the following document describing the Dempster-Shafer probability model, the expression (1) is expressed as Pls (b ) And (2) is described as Bel (b).
(Dempster-Shafer probability model literature)
S. Ferson, V. Kreinovich, L. Ginzburg, D. Myers, and K. Sents, "Constructing probability boxes and Dempster Shafer structures," Tecnical report, Sandia National Laboratories, 2003

エンコーダ・ジャイロ推定確率分布処理部３０１、エンコーダ推定確率分布処理部３０２およびビジョン推定確率分布処理部３０３が推定確率分布の上限を設定すると、上限重み平均算出部３０４は、自律移動体１０の移動量ｕについて、それらの設定された推定確率分布の上限を重み平均することで統合し、さらに、自律移動体１０の移動量ｖについて、それらの設定された推定確率分布の上限を重み平均することで統合し、さらに、自律移動体１０の移動方向の変化量θについて、それらの設定された推定確率分布の上限を重み平均することで統合する。 When the encoder / gyro estimated probability distribution processing unit 301, the encoder estimated probability distribution processing unit 302, and the vision estimated probability distribution processing unit 303 set the upper limit of the estimated probability distribution, the upper limit weight average calculating unit 304 moves the movement amount of the autonomous mobile body 10 For u, the upper limit of the set estimated probability distribution is integrated by weighted average, and further, the upper limit of the set estimated probability distribution is weighted averaged for the movement amount v of the autonomous mobile body 10. Further, the change amount θ in the moving direction of the autonomous mobile body 10 is integrated by weighted averaging the upper limits of the set estimated probability distributions.

まず、自律移動体１０の移動量ｕについて説明する。図７は、重み平均演算を説明するための図である。図７に示すような重み平均演算を行うことで、エンコーダ・ジャイロ推定確率分布処理部３０１が自律移動体１０の移動量ｕについて設定した推定確率分布の上限と、ビジョン推定確率分布処理部３０３が自律移動体１０の移動量ｕについて設定した推定確率分布の上限とを統合する。 First, the movement amount u of the autonomous mobile body 10 will be described. FIG. 7 is a diagram for explaining the weighted average calculation. By performing the weighted average calculation as shown in FIG. 7, the upper limit of the estimated probability distribution set by the encoder / gyro estimated probability distribution processing unit 301 for the movement amount u of the autonomous mobile body 10 and the vision estimated probability distribution processing unit 303 The upper limit of the estimated probability distribution set for the movement amount u of the autonomous mobile body 10 is integrated.

そして、エンコーダ・ジャイロ推定確率分布処理部３０１、エンコーダ推定確率分布処理部３０２およびビジョン推定確率分布処理部３０３が推定確率分布の下限を設定すると、下限重み平均算出部３０５は、上限重み平均算出部３０４と同様の重み平均演算を実行することで、自律移動体１０の移動量ｕについて、それらの設定された推定確率分布の下限を重み平均することで統合し、さらに、自律移動体１０の移動量ｖについて、それらの設定された推定確率分布の下限を重み平均することで統合し、さらに、自律移動体の移動方向の変化量θについて、それらの設定された推定確率分布の下限を重み平均することで統合する。 When the encoder / gyro estimated probability distribution processing unit 301, the encoder estimated probability distribution processing unit 302, and the vision estimated probability distribution processing unit 303 set the lower limit of the estimated probability distribution, the lower limit weight average calculating unit 305 By performing the same weighted average calculation as in 304, the movement amount u of the autonomous mobile body 10 is integrated by performing weighted averaging of the lower limits of the set estimated probability distributions, and the movement of the autonomous mobile body 10 For the amount v, the lower limit of the set estimated probability distribution is integrated by weighted average, and the lower limit of the set estimated probability distribution for the amount of change θ in the movement direction of the autonomous mobile body is weighted averaged. To integrate.

図８は、移動量ｕの上限および下限の統合を説明する図である。同図に示すように、自己位置推定モジュール１５０は、エンコーダ２１１，２１２、ジャイロ２１３により検出された自律移動体１０の移動量ｕについての推定確率分布（図中に示すエラーモデル）を生成して、その生成した推定確率分布の上限および下限を設定し、さらに、ビジョン確率分布３０８に基づいて、ビジョンにより検出された自律移動体１０の移動量ｕについての推定確率分布を生成して、その生成した推定確率分布の上限および下限を設定して、それらの上限を重み平均により統合すると共に、それらの下限を重み平均により統合するように処理する。 FIG. 8 is a diagram for explaining integration of the upper limit and the lower limit of the movement amount u. As shown in the figure, the self-position estimation module 150 generates an estimated probability distribution (error model shown in the figure) for the movement amount u of the autonomous mobile body 10 detected by the encoders 211 and 212 and the gyro 213. Then, an upper limit and a lower limit of the generated estimated probability distribution are set, and further, based on the vision probability distribution 308, an estimated probability distribution for the movement amount u of the autonomous mobile body 10 detected by the vision is generated, and the generation The upper and lower limits of the estimated probability distribution are set, and the upper limits are integrated by weighted average, and the lower limits are integrated by weighted average.

図９は、移動量ｖの上限および下限の統合を説明する図である。同図に示すように、自己位置推定モジュール１５０は、エンコーダ２１１，２１２、ジャイロ２１３により検出された自律移動体１０の移動量ｖについての推定確率分布を生成して、その生成した推定確率分布の上限および下限を設定し、さらに、ビジョン確率分布３０８に基づいて、ビジョンにより検出された自律移動体１０の移動量ｖについての推定確率分布を生成して、その生成した推定確率分布の上限および下限を設定して、それらの上限を重み平均で統合すると共に、それらの下限を重み平均により統合するように処理する。 FIG. 9 is a diagram illustrating the integration of the upper limit and the lower limit of the movement amount v. As shown in the figure, the self-position estimation module 150 generates an estimated probability distribution for the movement amount v of the autonomous mobile body 10 detected by the encoders 211 and 212 and the gyro 213, and the generated estimated probability distribution An upper limit and a lower limit are set, and further, an estimated probability distribution for the movement amount v of the autonomous mobile body 10 detected by the vision is generated based on the vision probability distribution 308, and the upper and lower limits of the generated estimated probability distribution Is set so that the upper limits thereof are integrated by the weighted average, and the lower limits thereof are integrated by the weighted average.

図１０は、変化量θの上限および下限の統合を説明する図である。同図に示すように、自己位置推定モジュール１５０は、エンコーダ２１１，２１２，ジャイロ２１３により検出された自律移動体の移動方向の変化量θについての推定確率分布を生成して、その生成した推定確率分布の上限および下限を設定、さらに、エンコーダ２１１，２１２により検出された自律移動体１０の移動方向の変化量θについての推定確率分布を生成して、その生成した推定確率分布の上限および下限を設定し、さらに、ビジョンにより検出された自律移動体１０の移動方向の変化量θについての推定確率分布を生成して、その生成した推定確率分布の上限および下限を設定してそれらの上限を重み平均により統合すると共に、それらの下限を重み平均により統合するように処理する。 FIG. 10 is a diagram illustrating the integration of the upper limit and the lower limit of the change amount θ. As shown in the figure, the self-position estimation module 150 generates an estimated probability distribution for the change amount θ of the moving direction of the autonomous mobile body detected by the encoders 211 and 212 and the gyro 213, and the generated estimated probability An upper limit and a lower limit of the distribution are set, and further, an estimated probability distribution for the change amount θ in the moving direction of the autonomous mobile body 10 detected by the encoders 211 and 212 is generated, and the upper and lower limits of the generated estimated probability distribution are set. In addition, an estimated probability distribution is generated for the moving direction change amount θ of the autonomous mobile body 10 detected by the vision, and an upper limit and a lower limit of the generated estimated probability distribution are set, and the upper limit is weighted. The integration is performed by averaging, and the lower limits thereof are processed so as to be integrated by weighted averaging.

続いて、Ｐｌｓ分布生成部３０６は、上限重み平均算出部３０４の統合した上限と、下限重み平均算出部３０５の統合した下限とに対して、上述の（１）式を適用することにより、Dempster-Shafer確率モデルを適用することで、自律移動体１０の移動量ｕについての確率分布を示すＰｌｓ分布と、自律移動体１０の移動量ｖについての確率分布を示すＰｌｓ分布と、自律移動体の移動方向の変化量θについての確率分布を示すＰｌｓ分布とを生成する。 Subsequently, the Pls distribution generation unit 306 applies the above-described equation (1) to the upper limit integrated by the upper limit weight average calculation unit 304 and the lower limit integrated by the lower limit weight average calculation unit 305, whereby Dempster By applying the -Shafer probability model, the Pls distribution indicating the probability distribution for the movement amount u of the autonomous mobile body 10, the Pls distribution indicating the probability distribution for the movement amount v of the autonomous mobile body 10, and the autonomous mobile body A Pls distribution indicating a probability distribution for the change amount θ in the movement direction is generated.

図１１は、Ｐｌｓ分布の生成を説明する図である。上限重み平均算出部３０４の統合した上限と下限重み平均算出部３０５の統合した下限とに対して、上述の（１）式を適用することで、図１１に示すようなＰｌｓ分布を生成することで、自律移動体１０の移動量ｕについての確率分布を示すＰｌｓ分布と、自律移動体１０の移動量ｖについての確率分布を示すＰｌｓ分布と、自律移動体１０の移動方向の変化量θについての確率分布を示すＰｌｓ分布とを生成する。 FIG. 11 is a diagram for explaining the generation of the Pls distribution. The Pls distribution as shown in FIG. 11 is generated by applying the above equation (1) to the upper limit integrated by the upper limit weight average calculation unit 304 and the lower limit integrated by the lower limit weight average calculation unit 305. Thus, the Pls distribution indicating the probability distribution for the movement amount u of the autonomous mobile body 10, the Pls distribution indicating the probability distribution for the movement amount v of the autonomous mobile body 10, and the change amount θ of the movement direction of the autonomous mobile body 10. Pls distribution indicating the probability distribution of

このＰｌｓ分布生成部３０６によりＰｌｓ分布の生成を受けて、統合処理部３０７は、パーティクルフィルタ３００により生成されたビジョン確率分布３０８と、Ｐｌｓ分布生成部３０６により生成されたＰｌｓ分布とを統合（乗算）することで、パーティクルフィルタ３００により生成された自律移動体１０の移動量（ｕ，ｖ，θ）についてビジョン確率分布３０８を修正する。 Upon receiving the Pls distribution generated by the Pls distribution generation unit 306, the integration processing unit 307 integrates (multiplies) the vision probability distribution 308 generated by the particle filter 300 and the Pls distribution generated by the Pls distribution generation unit 306. Thus, the vision probability distribution 308 is corrected with respect to the movement amount (u, v, θ) of the autonomous mobile body 10 generated by the particle filter 300.

統合処理部３０７は、統合した結果得られるビジョン確率分布３０８を経路設計モジュール１６０に出力する。経路設計モジュールは、ビジョン確率分布３０８に基づいて、自律移動体１０の位置および姿勢などを算出する。そして、経路計画モジュール１６０は、算出した位置および姿勢から自律移動体１０を目標起軌道に戻すための制御経路情報を生成し、生成した制御経路情報とビジョン確率分布３０８の情報を下位制御機構２００に出力する。 The integration processing unit 307 outputs the vision probability distribution 308 obtained as a result of the integration to the route design module 160. The route design module calculates the position and posture of the autonomous mobile body 10 based on the vision probability distribution 308. Then, the route planning module 160 generates control route information for returning the autonomous mobile body 10 to the target starting trajectory from the calculated position and posture, and uses the generated control route information and information of the vision probability distribution 308 as the lower control mechanism 200. Output to.

下位制御機構２００は、上位制御機構１００から出力される制御経路情報に従って自律移動体駆動機構２６０が自律移動体を移動させる。また、下位制御機構２００は、ビジョン確率分布３０８から自律移動体１０の期待値および分散を求め、下位制御機構２００に含まれるカルマンフィルタの誤差を調整する。例えば、カルマンフィルタ２７０が、既に設定されてある自律移動体１０の位置および姿勢を、上位制御機構１００から入力される確率分布から推定される自律移動体１０の位置および姿勢に修正する。 In the lower control mechanism 200, the autonomous mobile body driving mechanism 260 moves the autonomous mobile body according to the control path information output from the upper control mechanism 100. Further, the lower control mechanism 200 obtains the expected value and variance of the autonomous mobile body 10 from the vision probability distribution 308, and adjusts the error of the Kalman filter included in the lower control mechanism 200. For example, the Kalman filter 270 corrects the position and orientation of the autonomous mobile body 10 that has already been set to the position and orientation of the autonomous mobile body 10 estimated from the probability distribution input from the host control mechanism 100.

つぎに、本実施例にかかる自律移動体１０の処理手順について説明する。図１２および図１３は、本実施例にかかる自律移動体の処理手順を示すフローチャートである。同図に示すように、上位制御機構１００は、ＫＦ（Kalman Filter；エンコーダ＋ジャイロ）により推定された自律移動体１０の現在状態の期待値（移動量ｕ、ｖ、変化量θの期待値）および分散（移動量ｕ、ｖ、変化量θの分散）を下位制御機構２００から取得し（ステップＳ１０１）、ＫＦ（エンコーダ）により推定された自律移動体１０の現在状態の期待値（変化量θの期待値）および分散（変化量θの分散）を取得する（ステップＳ１０２）。 Below, the process sequence of the autonomous mobile body 10 concerning a present Example is demonstrated. 12 and 13 are flowcharts illustrating the processing procedure of the autonomous mobile body according to the present embodiment. As shown in the figure, the host control mechanism 100 is the expected value of the current state of the autonomous mobile 10 estimated by KF (Kalman Filter; encoder + gyro) (expected values of the movement amounts u and v and the change amount θ). And the variance (the variance of the movement amounts u and v and the change amount θ) are obtained from the lower control mechanism 200 (step S101), and the expected value (the change amount θ of the current state of the autonomous mobile body 10 estimated by the KF (encoder). Expected value) and variance (variance of variation θ) are acquired (step S102).

そして、上位制御機構１００は、ＫＦによって推定された自律移動体１０の現在状態の周辺で、一様ランダムサンプリングを行うと共に、サンプルセットを初期化し（ステップＳ１０３）、自律移動体１０の現在状態の確率分布（ビジョン確率分布）を生成する（ステップＳ１０４）。 Then, the host control mechanism 100 performs uniform random sampling around the current state of the autonomous mobile body 10 estimated by the KF, initializes a sample set (step S103), and sets the current state of the autonomous mobile body 10 A probability distribution (vision probability distribution) is generated (step S104).

また、上位制御機構１００は、確率分布に対してランダムサンプリングを行い、サンプルセットを生成し（ステップＳ１０５）、ＫＦ（エンコーダ＋ジャイロ）により推定された自律移動体１０の現在状態の期待値および分散を下位制御機構２００から取得し（ステップＳ１０６）、ＫＦ（エンコーダ）により推定された自律移動体１０の現在状態の期待値および分散を下位制御機構２００から取得し（ステップＳ１０７）、自律移動体の状態変化を計算する（ステップＳ１０８）。 Further, the host control mechanism 100 performs random sampling on the probability distribution, generates a sample set (step S105), and the expected value and variance of the current state of the autonomous mobile body 10 estimated by KF (encoder + gyro) Is acquired from the lower-level control mechanism 200 (step S106), and the expected value and variance of the current state of the autonomous mobile body 10 estimated by the KF (encoder) are acquired from the lower-level control mechanism 200 (step S107). The state change is calculated (step S108).

そして、上位制御機構１００は、自律移動体１０の状態変化に基づいてサンプルの状態を遷移し（ステップＳ１０９）、各サンプルに対して、ステレオカメラ１２０によって観測した環境情報と地図情報とを照合し、尤度を計算する（ステップＳ１１０）。 Then, the host control mechanism 100 changes the state of the sample based on the state change of the autonomous mobile body 10 (step S109), and collates the environment information observed by the stereo camera 120 with the map information for each sample. The likelihood is calculated (step S110).

続いて、上位制御機構１００は、ビジョンによる確率分布を生成し（ステップＳ１１１）、ビジョンによる確率分布から自律移動体１０の状態の期待値と分散とを生成し（ステップＳ１１２）、自律移動体１０の期待値および分散に基づいて、現在位置姿勢から目標軌道に戻す制御経路を生成し、下位制御機構に出力する（ステップＳ１１３）。 Subsequently, the host control mechanism 100 generates a probability distribution by vision (step S111), generates an expected value and a variance of the state of the autonomous mobile body 10 from the probability distribution by vision (step S112), and the autonomous mobile body 10 Based on the expected value and variance, a control path for returning the current position and orientation to the target trajectory is generated and output to the lower control mechanism (step S113).

また、上位制御機構１００は、自律移動体の期待値および分散を下位制御機構２００に出力し、下位制御機構２００に保持されている期待値および分散（あるいは、自律移動体１０の位置および姿勢）を修正する（ステップＳ１１４）。 The host control mechanism 100 outputs the expected value and variance of the autonomous mobile body to the lower control mechanism 200, and the expected value and variance held in the lower control mechanism 200 (or the position and orientation of the autonomous mobile body 10). Is corrected (step S114).

そして、上位制御機構１００は、各サンプルに対して、自律移動体１０の真の状態がこのサンプル周辺に含まれている信念度を生成し、真の状態の仮説と、この仮説に対する信念度のペアの集合を生成し（ステップＳ１１５）、各サンプルに対して、自律移動体１０の真の状態がこのサンプルの周辺に含まれている信念度を生成し、真の状態の仮説と、この仮説に対する信念度のペアとなる仮説集合を生成する（ステップＳ１１６）。 Then, for each sample, the host control mechanism 100 generates a belief degree that the true state of the autonomous mobile body 10 is included in the vicinity of this sample, and the hypothesis of the true state and the belief degree for this hypothesis A set of pairs is generated (step S115), and for each sample, a belief degree in which the true state of the autonomous mobile body 10 is included in the vicinity of the sample is generated. A hypothesis set that is a pair of belief degrees is generated (step S116).

上位制御機構１００は、仮説集合に対して積分を行い、自律移動体１０の状態の確率分布の第１の上限と下限とを生成し（ステップＳ１１７）、ＫＦ（エンコーダ＋ジャイロ）により推定された期待値と分散から自律移動体１０の状態の確率分布の第２の上限と下限とを生成し（ステップＳ１１８）、ＫＦ（エンコーダ）により推定された期待値と分散から自律移動体１０の状態の確率分布の第３の上限と下限とを生成し（ステップＳ１１９）、各上限と下限のペアを統合する（ステップＳ１２０）。 The host control mechanism 100 performs integration on the hypothesis set, generates the first upper limit and lower limit of the probability distribution of the state of the autonomous mobile body 10 (step S117), and is estimated by KF (encoder + gyro). A second upper limit and a lower limit of the probability distribution of the state of the autonomous mobile body 10 are generated from the expected value and variance (step S118), and the state of the autonomous mobile body 10 is estimated from the expected value and variance estimated by the KF (encoder). A third upper limit and a lower limit of the probability distribution are generated (step S119), and each pair of the upper limit and the lower limit is integrated (step S120).

そして、上位制御機構１００は、統合した上限と下限に対して微分処理を行い、自律移動体１０の真の状態の属する数値空間、およびこの区間に属する信念度を生成（Ｐｌｓ分布を生成）し（ステップＳ１２１）、ビジョンによる確率分布とＰｌｓ分布とを乗算した確率分布を生成し（ステップＳ１２２）、かかる確率分布によって確率分布を更新（代入）し（ステップＳ１２３）、ステップＳ１０５に移行する。 Then, the host control mechanism 100 performs differential processing on the integrated upper and lower limits, and generates a numerical space to which the true state of the autonomous mobile body 10 belongs and a belief degree belonging to this section (generates a Pls distribution). (Step S121), a probability distribution obtained by multiplying the probability distribution by vision and the Pls distribution is generated (Step S122), the probability distribution is updated (substituted) by the probability distribution (Step S123), and the process proceeds to Step S105.

上述してきたように、本実施例にかかる自律移動体１０は、カルマンフィルタを用いて下位センサの検出値から自律移動体１０の期待値および分散を推定し、自律移動体１０の状態変化量の確率分布の上限および下限を設定し、環境測定により推定した各パーティクルにおける自律移動体１０の状態変化の信頼度に基づいて、自律移動体１０の状態変化の確率分布の上限および下限を算出し、自律移動体１０の状態毎に各上限および下限を統合し、統合した上限および下限に基づいて、自律移動体１０の状態変化量の確率分布を算出し、環境計測により推定した各パーティクルにおける自律移動体１０の状態変化の信頼度を、算出した確率分布に基づいて修正するので、自律移動体１０の現在位置の推定精度を高めることができる。 As described above, the autonomous mobile body 10 according to the present embodiment estimates the expected value and variance of the autonomous mobile body 10 from the detection values of the lower sensors using the Kalman filter, and the probability of the state change amount of the autonomous mobile body 10 The upper and lower limits of the distribution are set, the upper and lower limits of the probability distribution of the state change of the autonomous mobile body 10 are calculated based on the reliability of the state change of the autonomous mobile body 10 at each particle estimated by the environmental measurement, and the autonomous Each upper limit and lower limit are integrated for each state of the moving body 10, the probability distribution of the state change amount of the autonomous moving body 10 is calculated based on the integrated upper limit and lower limit, and the autonomous moving body in each particle estimated by environment measurement Since the reliability of the 10 state changes is corrected based on the calculated probability distribution, it is possible to improve the estimation accuracy of the current position of the autonomous mobile body 10.

また、本実施例にかかる自律移動体１０は、カルマンフィルタによって推定される自律移動体１０の期待値および分散から確率分布の上限・下限を設定しているので、確率分布の上限・下限にかかる制約条件が追加され、自律移動体１０の現在位置推定を効率よく実施することができる。 Moreover, since the autonomous mobile body 10 according to the present embodiment sets the upper and lower limits of the probability distribution from the expected value and the variance of the autonomous mobile body 10 estimated by the Kalman filter, the restrictions on the upper and lower limits of the probability distribution Conditions are added, and the current position of the autonomous mobile body 10 can be estimated efficiently.

さて、これまで本発明の実施例について説明したが、本発明は上述した実施例以外にも、特許請求の範囲に記載した技術的思想の範囲内において種々の異なる実施例にて実施されてもよいものである。 Although the embodiments of the present invention have been described so far, the present invention may be implemented in various different embodiments in addition to the above-described embodiments within the scope of the technical idea described in the claims. It ’s good.

また、本実施例において説明した各処理のうち、自動的におこなわれるものとして説明した処理の全部または一部を手動的におこなうこともでき、あるいは、手動的におこなわれるものとして説明した処理の全部または一部を公知の方法で自動的におこなうこともできる。 In addition, among the processes described in this embodiment, all or part of the processes described as being performed automatically can be performed manually, or the processes described as being performed manually can be performed. All or a part can be automatically performed by a known method.

この他、上記文書中や図面中で示した処理手順、制御手順、具体的名称、各種のデータやパラメータを含む情報については、特記する場合を除いて任意に変更することができる。 In addition, the processing procedure, control procedure, specific name, and information including various data and parameters shown in the above-described document and drawings can be arbitrarily changed unless otherwise specified.

また、図示した各装置の各構成要素は機能概念的なものであり、必ずしも物理的に図示のように構成されていることを要しない。すなわち、各装置の分散・統合の具体的形態は図示のものに限られず、その全部または一部を、各種の負荷や使用状況などに応じて、任意の単位で機能的または物理的に分散・統合して構成することができる。 Each component of each illustrated device is functionally conceptual and does not necessarily need to be physically configured as illustrated. In other words, the specific form of distribution / integration of each device is not limited to that shown in the figure, and all or a part thereof may be functionally or physically distributed or arbitrarily distributed in arbitrary units according to various loads or usage conditions. Can be integrated and configured.

以上のように、本発明にかかる自律移動体および移動方法は、分散センサネットワークおよびハイブリッドアーキテクチャの制御方式を用いる自律移動体に有用であり、特に、自律移動体の自己位置を高精度に推定する必要がある場合に適している。 As described above, the autonomous mobile body and the moving method according to the present invention are useful for an autonomous mobile body using a distributed sensor network and a hybrid architecture control method, and in particular, estimate the self-position of the autonomous mobile body with high accuracy. Suitable when necessary.

本実施例にかかる自律移動体の構成の一例を示す図である。It is a figure which shows an example of a structure of the autonomous mobile body concerning a present Example. 上位制御機構および下位制御機構の構成を示す機能ブロック図である。It is a functional block diagram which shows the structure of a high-order control mechanism and a low-order control mechanism. 本実施例にかかる自己位置推定モジュールの構成例を示す図である。It is a figure which shows the structural example of the self-position estimation module concerning a present Example. 推定確率分布の生成を説明するための図である。It is a figure for demonstrating the production | generation of an estimation probability distribution. 上限・下限の設定を説明するための図である。It is a figure for demonstrating the setting of an upper limit and a minimum. ビジョンにより検出された自律移動体の移動量ｕについての推定確率分布の上限・下限の設定手法を説明するための図である。It is a figure for demonstrating the setting method of the upper limit and lower limit of the estimation probability distribution about the movement amount u of the autonomous mobile body detected by the vision. 重み平均演算を説明するための図である。It is a figure for demonstrating a weight average calculation. 移動量ｕの上限および下限の統合を説明する図である。It is a figure explaining the integration of the upper limit and the lower limit of the movement amount u. 移動量ｖの上限および下限の統合を説明する図である。It is a figure explaining the integration of the upper limit and the lower limit of the movement amount v. 変化量θの上限および下限の統合を説明する図である。It is a figure explaining integration of the upper limit and lower limit of variation | change_quantity (theta). Ｐｌｓ分布の生成を説明する図である。It is a figure explaining the production | generation of Pls distribution. 本実施例にかかる自律移動体の処理手順を示すフローチャート（１）である。It is a flowchart (1) which shows the process sequence of the autonomous mobile body concerning a present Example. 本実施例にかかる自律移動体の処理手順を示すフローチャート（２）である。It is a flowchart (2) which shows the process sequence of the autonomous mobile body concerning a present Example. 従来の技術を説明するための図である。It is a figure for demonstrating the prior art.

Explanation of symbols

１０自律移動体（ロボット）
１１車輪
１２モータ
１３，２１１，２１２エンコーダ
１４ＣＰＵ
１５移動量算出部
１００上位制御機構
１１０下位センサ情報取得モジュール
１２０ステレオカメラ
１３０ステレオカメラ入力部
１４０３次元環境計測部
１５０自己位置推定モジュール
１６０経路計画モジュール
２００下位制御機構
２１０下位センサ系
２１３ジャイロ
２１４距離センサ
２２０センサデータ記憶モジュール
２３０行動情報受付モジュール
２４０単純反応処理モジュール
２５０行動選択機構
２６０自律移動体駆動機構
２７０カルマンフィルタ
３００パーティクルフィルタ
３０１エンコーダ・ジャイロ推定確率分布処理部
３０２エンコーダ推定確率分布処理部
３０３ビジョン推定確率分布処理部
３０４上限重み平均算出部
３０５下限重み平均算出部
３０６Ｐｌｓ分布生成部
３０７統合処理部 10 Autonomous mobile object (robot)
11 Wheel 12 Motor 13, 211, 212 Encoder 14 CPU
15 Movement amount calculation unit 100 Upper control mechanism 110 Lower sensor information acquisition module 120 Stereo camera 130 Stereo camera input unit 140 Three-dimensional environment measurement unit 150 Self-position estimation module 160 Path planning module 200 Lower control mechanism 210 Lower sensor system 213 Gyro 214 Distance Sensor 220 Sensor data storage module 230 Action information reception module 240 Simple reaction processing module 250 Action selection mechanism 260 Autonomous moving body drive mechanism 270 Kalman filter 300 Particle filter 301 Encoder / gyro estimation probability distribution processing section 302 Encoder estimation probability distribution processing section 303 Vision estimation Probability distribution processing unit 304 Upper limit weight average calculation unit 305 Lower limit weight average calculation unit 306 Pls distribution generation unit 307 Integration processing unit

Claims

Measures the environment using a host sensor, estimates the state of the autonomous mobile body using a particle filter and controls the movement of the autonomous mobile body, and estimates the state of the autonomous mobile body from the detection value of the lower sensor An autonomous mobile body comprising a lower control mechanism for controlling the movement of the autonomous mobile body,
The lower control mechanism includes a Kalman filter that predicts the motion state of the autonomous mobile body,
First setting means for estimating an expected value and variance of the autonomous mobile body from the detection values of the lower sensors using the Kalman filter, and setting an upper limit and a lower limit of a probability distribution of the state change amount of the autonomous mobile body;
A second setting means for setting an upper limit and a lower limit of the probability distribution of the state change of the autonomous mobile body based on the reliability of the state change of the autonomous mobile body in each particle estimated by the environmental measurement;
Integrating means for integrating the upper limits set by the first and second setting means for each state of the autonomous mobile body, and integrating the lower limits set by the first and second setting means;
Calculation means for calculating a probability distribution of the state change amount of the autonomous mobile body based on the upper limit and the lower limit integrated by the integration means;
Correction means for correcting the reliability of the state change of the autonomous mobile body in each particle estimated by the environmental measurement based on the probability distribution calculated by the calculation means;
An autonomous mobile body characterized by comprising:

The integrating means integrates the upper limits by weighted averaging the upper limits set by the first and second setting means, and weighted averages the lower limits set by the first and second setting means. The autonomous mobile body according to claim 1, wherein the lower limits are integrated.

The information correction means for correcting the position and orientation information of the autonomous mobile body set in the subordinate control mechanism based on the probability distribution calculated by the calculation means is further provided. 2. The autonomous mobile body according to 2.

Measures the environment using a host sensor, estimates the state of the autonomous mobile body using a particle filter and controls the movement of the autonomous mobile body, and estimates the state of the autonomous mobile body from the detection value of the lower sensor A method for moving an autonomous mobile body comprising a lower control mechanism for controlling the movement of the autonomous mobile body,
The lower control mechanism includes a Kalman filter that predicts the motion state of the autonomous mobile body,
A first setting step of estimating an expected value and variance of an autonomous mobile body from detection values of the lower sensors using the Kalman filter, and setting an upper limit and a lower limit of a probability distribution of the state change amount of the autonomous mobile body;
A second setting step for setting an upper limit and a lower limit of the probability distribution of the state change of the autonomous mobile body based on the reliability of the state change of the autonomous mobile body in each particle estimated by the environmental measurement;
Integrating the upper limits set by the first and second setting steps for each state of the autonomous mobile body, and integrating the lower limits set by the first and second setting steps;
Based on the upper limit and the lower limit integrated in the integration step, a calculation step of calculating a probability distribution of the state change amount of the autonomous mobile body,
A correction step of correcting the reliability of the state change of the autonomous mobile body in each particle estimated by the environmental measurement based on the probability distribution calculated by the calculation step;
The movement method characterized by including.

The integration step integrates the upper limits by weighted averaging the upper limits set in the first and second setting steps, and weight-averages the lower limits set in the first and second setting steps. The moving method according to claim 4, wherein the lower limits are integrated.

5. The information correction step of correcting the position and orientation information of the autonomous mobile body set in the lower control mechanism based on the probability distribution calculated by the calculation step. 5. The moving method according to 5.