JP4664251B2

JP4664251B2 - Autonomous mobile

Info

Publication number: JP4664251B2
Application number: JP2006207329A
Authority: JP
Inventors: 彬陳
Original assignee: Fujitsu Ltd
Current assignee: Fujitsu Ltd
Priority date: 2006-07-31
Filing date: 2006-07-31
Publication date: 2011-04-06
Anticipated expiration: 2026-07-31
Also published as: JP2008033696A

Description

本発明は、自律的に移動する自律移動体に関し、特に、パーティクルフィルタを用いて自律移動体の状態を推定してその移動を制御するという構成を採るときにあって、下位センサによる検出値に基づいてパーティクルフィルタに依らずに直接的に自律移動体を制御するという機構が組み込まれるときにも、パーティクルフィルタによる自律移動体の状態推定を高精度に実行できるようにする自律移動体に関する。 The present invention relates to an autonomous mobile body that moves autonomously, and particularly to a configuration in which the state of an autonomous mobile body is estimated using a particle filter to control the movement, and the detection value by a lower sensor is used. The present invention also relates to an autonomous moving body that enables highly accurate state estimation of an autonomous moving body using a particle filter even when a mechanism for directly controlling the autonomous moving body without using a particle filter is incorporated.

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

ここで、このような下位制御機構と上位制御機構とを統合したアーキテクチャはハイブリッドアーキテクチャと呼ばれている。 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 mobiles, it is common to integrate information from multiple sensors such as encoders, inertial sensors, and vision sensors using Kalman filters. There is a problem that it is difficult to apply in reality.

すなわち、カルマンフィルタによる方法では、自律移動体の構造モデルと運動モデルとに従い、カルマンフィルタが推定した過去の時間での自律移動体の運動量に基づいて、ある決まった時間サンプリングで次の時間での運動状態を予測することになるが、この時間サンプリングは処理データが多い場合に長くなる。 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 wall, the movement is stacked, and the subordinate control mechanism Will try to escape from the stack by selecting a simple action such as deceleration, direction change, etc. At this time, the motion state of the autonomous mobile body predicted by the Kalman filter will be different from the actual situation .

このようなことから、カルマンフィルタを用いてハイブリッドアーキテクチャ構成の自律移動体の最適な行動を制御するという方法は現実には適用しにくいという問題がある。 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 installed in the host control mechanism cannot predict the behavior of the lower control mechanism that quickly grasps and reacts to the situation change in the real environment. The method of controlling behavior is difficult to apply in reality.

このようなことを背景にして、近年、パーティクルフィルタを用いてハイブリッドアーキテクチャ構成の自律移動体の最適な行動を制御するという方法が検討されている（例えば、非特許文献１参照）。
M.Isard and A.Blake,"Condensation-conditional density propagation for visual tracking," IJCV, Vol.29, No.1, pp.5-28, 1998. Against this background, in recent years, 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).
M. Isard and A. Blake, "Condensation-conditional density propagation for visual tracking," IJCV, Vol. 29, No. 1, pp. 5-28, 1998.

パーティクルフィルタもまた、カルマンフィルタと同様に、パーティクルフィルタが推定した現在の自律移動体の運動状態に基づいて次の時間での運動状態を予測するという処理を行う。 Similar 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.

これから、カルマンフィルタによる方法と同じ理由により、パーティクルフィルタによる方法も現実には適用しにくい。 Therefore, for the same reason as the method using the Kalman filter, the method using the particle filter is difficult to actually apply.

ただし、予測される運動の量として、オドメトリ（エンコーダやジャイロによる自己位置推定）を用いるならば、パーティクルフィルタを適用することは可能である。 However, if odometry (self-position estimation by an encoder or gyroscope) is used as the predicted amount of motion, a particle filter can be applied.

しかしながら、オドメトリには不確かさ（曖昧さ）が含まれており、この不確かさを考慮せずにパーティクルフィルタを適用するならば、推定した自己位置の精度が著しく低くなることから、安定した自己位置推定は困難なものとなる。 However, the odometry includes uncertainty (ambiguity), and if the particle filter is applied without taking this uncertainty into account, the accuracy of the estimated self-position will be significantly reduced, so that stable self-position Estimation is difficult.

本発明はかかる事情に鑑みてなされたものであって、ハイブリッドアーキテクチャ構成の自律移動体の行動を制御すべくパーティクルフィルタを適用する際に、オドメトリの情報を直接利用して自律移動体の移動の予測量とすることで、パーティクルフィルタの適用を可能にする場合にあって、オドメトリの情報の不確かさを考慮して、複数センサからの情報を統合することにより、自律移動体の現在位置の推定精度を高めることを実現する新たな技術の提供を目的とする。 The present invention has been made in view of such circumstances, and when applying a particle filter to control the behavior of an autonomous mobile body having a hybrid architecture configuration, the odometry information is directly used to control the movement of the autonomous mobile body. Estimating the current position of an autonomous mobile object by integrating the information from multiple sensors in consideration of the uncertainty of odometry information when applying the particle filter by using the prediction amount The purpose is to provide a new technology to improve accuracy.

この目的を達成するために、本発明の自律移動体は、上位センサによる環境測定を行い、パーティクルフィルタを用いて自律移動体の状態を推定してその移動を制御する上位制御機構と、下位センサによる検出値に基づいて自律移動体の状態を推定してその移動を制御する下位制御機構とを備えるという構成を採るときにあって、（１）下位センサの検出値に基づいて推定した自律移動体の状態変化量を処理対象として、その状態変化量の信頼度分布の上限及び下限を設定する第１の設定手段と、（２）環境測定により推定した各パーティクルにおける自律移動体の状態変化量の信頼度に基づいて、自律移動体の状態変化量の信頼度分布の上限及び下限を設定する第２の設定手段と、（３）自律移動体の状態毎に、第１及び第２の設定手段の設定した上限を統合するとともに、第１及び第２の設定手段の設定した下限を統合する統合手段と、（４）統合手段の統合した上限及び下限に基づいて、自律移動体の状態変化量の信頼度分布を算出する算出手段と、（５）環境測定により推定した各パーティクルにおける自律移動体の状態変化量の信頼度を、算出手段の算出した信頼度分布に基づいて修正する修正手段とを備えるように構成する。 In order to achieve this object, the autonomous mobile body of the present invention measures the environment using a host sensor, estimates the state of the autonomous mobile body using a particle filter, and controls its movement, and the lower sensor (1) Autonomous movement estimated based on the detection value of the low-order sensor, with a low-level control mechanism for estimating the state of the autonomous mobile body based on the detection value of A first setting means for setting an upper limit and a lower limit of a reliability distribution of the state change amount as a processing target, and (2) a state change amount of the autonomous mobile body in each particle estimated by environment measurement Second setting means for setting an upper limit and a lower limit of the reliability distribution of the state change amount of the autonomous mobile body based on the reliability of the autonomous mobile body, and (3) the first and second settings for each state of the autonomous mobile body Means And (4) the state change amount of the autonomous mobile body based on the upper limit and the lower limit integrated by the integration means, and the integration means for integrating the lower limits set by the first and second setting means. A calculation unit that calculates a reliability distribution; and (5) a correction unit that corrects the reliability of the state change amount of the autonomous moving body in each particle estimated by the environmental measurement based on the reliability distribution calculated by the calculation unit. Configure to include.

ここで、以上の各処理手段はコンピュータプログラムでも実現できるものであり、このコンピュータプログラムは、適当なコンピュータ読み取り可能な記録媒体に記録して提供されたり、ネットワークを介して提供され、本発明を実施する際に本発明の自律移動体が搭載するコンピュータにインストールされてＣＰＵなどの制御手段上で動作することにより本発明を実現することになる。 Here, each of the processing means described above can be realized by a computer program, and this computer program is provided by being recorded on an appropriate computer-readable recording medium or provided via a network to implement the present invention. In doing so, the present invention is realized by being installed in a computer mounted on the autonomous mobile body of the present invention and operating on a control means such as a CPU.

このように構成される本発明の自律移動体では、第１の設定手段は、下位センサの検出値に基づいて推定した自律移動体の状態変化量を処理対象として、その状態変化量の信頼度分布の上限及び下限を設定する。例えば、下位センサの検出値を中心にして、その近傍領域に規定の分布を示す状態変化量の集合の信頼度を設定して、その集合の示す信頼度に基づいて上限及び下限を設定するのである。 In the autonomous mobile body of the present invention configured as described above, the first setting means uses the state change amount of the autonomous mobile body estimated based on the detection value of the lower sensor as a processing target, and the reliability of the state change amount. Set the upper and lower limits of the distribution. For example, the reliability of a set of state change amounts showing a prescribed distribution is set around the detection value of the lower sensor, and the upper and lower limits are set based on the reliability indicated by the set. is there.

そして、第２の設定手段は、環境測定により推定した各パーティクルにおける自律移動体の状態変化量の信頼度に基づいて、自律移動体の状態変化量の信頼度分布の上限及び下限を設定する。例えば、各パーティクルの位置する状態変化量空間を中心にして、その近傍領域に含まれるパーティクルの持つ状態変化量の信頼度の平均値を算出することで、各パーティクルについて区間信頼度を設定して、それらの区間信頼度の示す信頼度に基づいて上限及び下限を設定するのである。 And a 2nd setting means sets the upper limit and minimum of the reliability distribution of the state change amount of an autonomous mobile body based on the reliability of the state change amount of the autonomous mobile body in each particle estimated by the environmental measurement. For example, setting the interval reliability for each particle by calculating the average value of the reliability of the state change amount of the particles included in the neighboring area around the state change amount space where each particle is located The upper and lower limits are set based on the reliability indicated by the interval reliability.

このようにして、第１及び第２の設定手段が自律移動体の状態変化量の信頼度分布の上限及び下限を設定すると、統合手段は、自律移動体の状態毎に、第１及び第２の設定手段の設定した上限を統合するとともに、第１及び第２の設定手段の設定した下限を統合する。例えば、第１及び第２の設定手段の設定した上限を重み平均することでそれらの上限を統合するとともに、第１及び第２の設定手段の設定した下限を重み平均することでそれらの下限を統合するのである。 Thus, when the first and second setting means set the upper limit and the lower limit of the reliability distribution of the state change amount of the autonomous mobile body, the integration means first and second for each state of the autonomous mobile body. The upper limits set by the setting means are integrated, and the lower limits set by the first and second setting means are integrated. For example, the upper limits set by the first and second setting means are weighted to integrate the upper limits, and the lower limits set by the first and second setting means are weighted to reduce the lower limits. To integrate.

続いて、算出手段は、統合手段の統合した上限及び下限に基づいて、自律移動体の状態変化量の信頼度分布を算出する。例えば、統合手段の統合した上限及び下限に対してDempster-Shafer 確率モデルを適用することで、自律移動体の状態変化量の信頼度分布を算出するのである。 Subsequently, the calculation unit calculates the reliability 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. For example, the reliability distribution of the state change amount of the autonomous mobile body is calculated by applying the Dempster-Shafer probability model to the upper and lower limits integrated by the integration means.

これを受けて、修正手段は、環境測定により推定した各パーティクルにおける自律移動体の状態変化量の信頼度を、算出手段の算出した信頼度分布に基づいて修正する。例えば、環境測定により推定した各パーティクルにおける自律移動体の状態変化量の信頼度と、算出手段の算出した信頼度分布の持つ該当する信頼度とを乗算することで、環境測定により推定した各パーティクルにおける自律移動体の状態変化量の信頼度を修正するのである。 In response to this, the correcting means corrects the reliability of the state change amount of the autonomous moving body in each particle estimated by the environmental measurement based on the reliability distribution calculated by the calculating means. For example, each particle estimated by the environmental measurement by multiplying the reliability of the state change amount of the autonomous mobile body in each particle estimated by the environmental measurement by the corresponding reliability of the reliability distribution calculated by the calculation means This corrects the reliability of the state change amount of the autonomous mobile body.

このようにして、本発明では、ハイブリッドアーキテクチャ構成の自律移動体の行動を制御すべくパーティクルフィルタを適用する際に、オドメトリの情報を直接利用して自律移動体の移動の予測量とすることで、パーティクルフィルタの適用を可能にする場合にあって、オドメトリの情報の不確かさを考慮して、複数センサからの情報を統合するように処理する。 In this way, in the present invention, when applying a particle filter to control the behavior of an autonomous mobile object having a hybrid architecture configuration, the odometry information is directly used as the predicted amount of movement of the autonomous mobile object. In the case where the application of the particle filter is made possible, in consideration of the uncertainty of the odometry information, processing is performed so as to integrate information from a plurality of sensors.

この構成に従って、本発明によれば、下位センサによる検出値に基づいてパーティクルフィルタに依らずに直接的に自律移動体を制御するという機構が組み込まれるときにも、パーティクルフィルタによる自律移動体の状態推定を高精度に実行することができるようになる。 According to the present invention, according to the present invention, the state of the autonomous mobile body by the particle filter is incorporated even when a mechanism for directly controlling the autonomous mobile body without depending on the particle filter based on the detection value by the lower sensor is incorporated. The estimation can be performed with high accuracy.

以下、実施の形態に従って本発明を詳細に説明する。 Hereinafter, the present invention will be described in detail according to embodiments.

図１に、本発明を具備するハイブリッドアーキテクチャ構成の自律移動体に搭載されることになる上位制御機構１及び下位制御機構２の装置構成例を図示する。 FIG. 1 shows an example of the device configuration of a host control mechanism 1 and a lower control mechanism 2 to be mounted on an autonomous mobile body having a hybrid architecture having the present invention.

この図に示すように、本発明を実現すべく備えられる上位制御機構１は、下位制御機構２から送られてくる下位センサ情報を取得する下位センサ情報取得モジュール１０と、自律移動体の周囲の環境を撮影するステレオカメラ１１と、ステレオカメラ１１の撮影した映像情報を入力するステレオカメラ入力部１２と、ステレオカメラ入力部１２の入力した映像情報に基づいて３次元環境計測（ビジョンの計測）を実行する３次元環境計測部１３と、３次元環境計測部１３の計測結果を使い、パーティクルフィルタの手法に基づいて自律移動体の自己位置を推定する自己位置推定モジュール１４と、自己位置推定モジュール１４の自己位置推定結果に基づいて自律移動体の経路を計画する経路計画モジュール１５とを備える。 As shown in this figure, the upper control mechanism 1 provided to realize the present invention includes a lower sensor information acquisition module 10 that acquires lower sensor information sent from the lower control mechanism 2, and a surrounding area of the autonomous mobile body. Stereo camera 11 that captures the environment, stereo camera input unit 12 that inputs video information captured by stereo camera 11, and three-dimensional environment measurement (vision measurement) based on the video information input by stereo camera input unit 12 The self-position estimation module 14 that estimates the self-position of the autonomous mobile body based on the particle filter technique using the measurement result of the three-dimensional environment measurement section 13 to be executed, the three-dimensional environment measurement section 13, and the self-position estimation module 14 And a route planning module 15 for planning a route of the autonomous mobile body based on the self-position estimation result.

一方、本発明を実現すべく備えられる下位制御機構２は、複数のセンサで構成される下位センサ系２０と、下位センサ系２０のセンサにより検出されたデータを記憶するセンサデータ記憶モジュール２１と、上位制御機構１の経路計画モジュール１５から送られてくる自律移動体の行動情報（自律移動体をどのように動かすのかを指示する情報）を受け取る行動情報受付モジュール２２と、下位センサ系２０のセンサにより検出されたデータに基づいて行動する自律移動体の行動情報（自律移動体をどのように動かすのかを指示する情報）を出力する単純反応処理モジュール２３と、行動情報受付モジュール２２の受け付けた行動情報か単純反応処理モジュール２３の出力する行動情報のいずれか一方を選択する行動選択機構２４と、行動選択機構２４の選択した行動情報に基づいて自律移動体を駆動する自律移動体駆動機構２５とを備える。 On the other hand, the lower-level control mechanism 2 provided to implement the present invention includes a lower-level sensor system 20 composed of a plurality of sensors, a sensor data storage module 21 that stores data detected by the sensors of the lower-level sensor system 20, An action information receiving module 22 that receives action information of the autonomous mobile body (information that indicates how to move the autonomous mobile body) sent from the route planning module 15 of the host control mechanism 1, and a sensor of the lower sensor system 20 The simple reaction processing module 23 that outputs the behavior information of the autonomous mobile body that acts based on the data detected by the information (information that instructs how to move the autonomous mobile body), and the behavior received by the behavior information reception module 22 An action selection mechanism 24 for selecting either information or action information output by the simple reaction processing module 23; And a autonomous moving body drive mechanism 25 for driving the autonomous moving body based on the selected activity information mechanism 24.

下位制御機構２の備える下位センサ系２０は、自律移動体のＸ方向の移動量を検出するエンコーダ２００と、自律移動体のＹ方向の移動量を検出するエンコーダ２０１と、自律移動体の移動方向を検出するジャイロ２０２と、自律移動体と障害物との間の距離を測定する距離センサ２０３などのセンサで構成されている。 The lower sensor system 20 provided in the lower control mechanism 2 includes an encoder 200 that detects the amount of movement of the autonomous mobile body in the X direction, an encoder 201 that detects the amount of movement of the autonomous mobile body in the Y direction, and the direction of movement of the autonomous mobile body. And a sensor such as a distance sensor 203 for measuring the distance between the autonomous mobile body and the obstacle.

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

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

このように構成される上位制御機構１及び下位制御機構２に従って、自律移動体は、障害物との間の距離が近い場合には、下位センサ系２０のセンサデータに基づいて決定された行動情報に従って行動し、障害物との間の距離が遠い場合には、パーティクルフィルタによる自己位置推定結果に基づいて決定された行動情報に従って行動することになる。 In accordance with the upper control mechanism 1 and the lower control mechanism 2 configured as described above, the autonomous mobile body is action information determined based on the sensor data of the lower sensor system 20 when the distance from 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.

図２に、本発明を実現すべく備えられる自己位置推定モジュール１４の装置構成例を図示する。 FIG. 2 shows a device configuration example of the self-position estimation module 14 provided to realize the present invention.

この図に示すように、自己位置推定モジュール１４は、パーティクルフィルタ１４０と、エンコーダ推定信頼度分布処理部１４１と、ジャイロ推定信頼度分布処理部１４２と、ビジョン推定信頼度分布処理部１４３と、上限重み平均算出部１４４と、下限重み平均算出部１４５と、Ｐｌｓ分布生成部１４６と、統合処理部１４７とを備える。 As shown in this figure, the self-position estimation module 14 includes a particle filter 140, an encoder estimated reliability distribution processing unit 141, a gyro estimated reliability distribution processing unit 142, a vision estimated reliability distribution processing unit 143, and an upper limit. A weighted average calculating unit 144, a lower limit weighted average calculating unit 145, a Pls distribution generating unit 146, and an integration processing unit 147 are provided.

このパーティクルフィルタ１４０は、パーティクルのリサンプリングとサンプルの評価を行い、ビジョンによる信頼度の分布であるビジョン信頼度分布１４８を得て、それに基づいて、パーティクルのリサンプリングとサンプルの評価を行うことを繰り返すことで、自律移動体の自己位置を推定する処理を行う。 This particle filter 140 performs resampling of the particles and evaluation of the sample, obtains a vision reliability distribution 148 that is a distribution of reliability by vision, and performs resampling of the particles and evaluation of the sample based on the vision reliability distribution 148. By repeating, the process which estimates the self position of an autonomous mobile body is performed.

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

エンコーダ推定信頼度分布処理部１４１は、エンコーダ２００，２０１により検出された自律移動体の移動量ｕ，ｖを入力として、その入力した移動量ｕ，ｖについての推定信頼度分布を生成して、その生成した推定信頼度分布の上限及び下限を設定し、さらに、入力した自律移動体の移動量ｕ，ｖに基づいて自律移動体の移動方向の変化量θを算出し、その算出した移動方向の変化量θについての推定信頼度分布を生成して、その生成した推定信頼度分布の上限及び下限を設定する。 The encoder estimated reliability distribution processing unit 141 receives the movement amounts u and v of the autonomous mobile body detected by the encoders 200 and 201 as input, and generates an estimated reliability distribution for the input movement amounts u and v. The upper limit and the lower limit of the generated estimated reliability distribution are set, and the change amount θ of the movement direction of the autonomous mobile body is calculated based on the input movement amounts u and v of the autonomous mobile body, and the calculated movement direction An estimated reliability distribution for the amount of change θ is generated, and an upper limit and a lower limit of the generated estimated reliability distribution are set.

ジャイロ推定信頼度分布処理部１４２は、ジャイロ２０２により検出された自律移動体の移動方向の変化量θを入力として、その入力した移動方向の変化量θについての推定信頼度分布を生成して、その生成した推定信頼度分布の上限及び下限を設定する。 The gyro estimated reliability distribution processing unit 142 receives the change amount θ in the movement direction of the autonomous mobile body detected by the gyro 202 and generates an estimated reliability distribution for the input change amount θ in the movement direction. The upper and lower limits of the generated estimated reliability distribution are set.

ビジョン推定信頼度分布処理部１４３は、パーティクルフィルタ１４０により生成されたビジョン信頼度分布１４８を入力として、その入力したビジョン信頼度分布１４８に基づいて、ビジョンにより検出された自律移動体の移動量ｕ，ｖについての推定信頼度分布を生成して、その生成した推定信頼度分布の上限及び下限を設定するとともに、ビジョンにより検出された自律移動体の移動方向の変化量θについての推定信頼度分布を生成して、その生成した推定信頼度分布の上限及び下限を設定する。 The vision estimation reliability distribution processing unit 143 receives the vision reliability distribution 148 generated by the particle filter 140 as an input, and based on the input vision reliability distribution 148, the movement amount u of the autonomous mobile body detected by the vision. , V are generated, an upper limit and a lower limit of the generated estimated reliability distribution are set, and an estimated reliability distribution regarding the change amount θ of the moving direction of the autonomous mobile body detected by the vision And the upper and lower limits of the generated estimated reliability distribution are set.

上限重み平均算出部１４４は、（イ）自律移動体の移動量ｕについて、エンコーダ推定信頼度分布処理部１４１の設定した推定信頼度分布の上限と、ビジョン推定信頼度分布処理部１４３の設定した推定信頼度分布の上限とを重み平均することで統合し、（ロ）さらに、自律移動体の移動量ｖについて、エンコーダ推定信頼度分布処理部１４１の設定した推定信頼度分布の上限と、ビジョン推定信頼度分布処理部１４３の設定した推定信頼度分布の上限とを重み平均することで統合し、（ハ）さらに、自律移動体の移動方向の変化量θについて、エンコーダ推定信頼度分布処理部１４１の設定した推定信頼度分布の上限と、ジャイロ推定信頼度分布処理部１４２の設定した推定信頼度分布の上限と、ビジョン推定信頼度分布処理部１４３の設定した推定信頼度分布の上限とを重み平均することで統合する。 The upper limit weight average calculation unit 144 sets the upper limit of the estimated reliability distribution set by the encoder estimated reliability distribution processing unit 141 and the vision estimated reliability distribution processing unit 143 for (i) the movement amount u of the autonomous moving body. (B) Furthermore, the upper limit of the estimated reliability distribution set by the encoder estimated reliability distribution processing unit 141 for the movement amount v of the autonomous mobile body, and the vision The estimated reliability distribution set by the estimated reliability distribution processing unit 143 is integrated by weighted averaging. (C) Further, an encoder estimated reliability distribution processing unit for the change amount θ in the moving direction of the autonomous mobile body. 141, the upper limit of the estimated reliability distribution set by 141, the upper limit of the estimated reliability distribution set by the gyro estimated reliability distribution processing unit 142, and the setting of the vision estimated reliability distribution processing unit 143. And the estimated reliability distribution limit integration by averaging weights.

下限重み平均算出部１４５は、（イ）自律移動体の移動量ｕについて、エンコーダ推定信頼度分布処理部１４１の設定した推定信頼度分布の下限と、ビジョン推定信頼度分布処理部１４３の設定した推定信頼度分布の下限とを重み平均することで統合し、（ロ）さらに、自律移動体の移動量ｖについて、エンコーダ推定信頼度分布処理部１４１の設定した推定信頼度分布の下限と、ビジョン推定信頼度分布処理部１４３の設定した推定信頼度分布の下限とを重み平均することで統合し、（ハ）さらに、自律移動体の移動方向の変化量θについて、エンコーダ推定信頼度分布処理部１４１の設定した推定信頼度分布の下限と、ジャイロ推定信頼度分布処理部１４２の設定した推定信頼度分布の下限と、ビジョン推定信頼度分布処理部１４３の設定した推定信頼度分布の下限とを重み平均することで統合する。 The lower limit weight average calculation unit 145 sets (a) the lower limit of the estimated reliability distribution set by the encoder estimated reliability distribution processing unit 141 and the vision estimated reliability distribution processing unit 143 for the movement amount u of the autonomous moving body. (B) Furthermore, the lower limit of the estimated reliability distribution set by the encoder estimated reliability distribution processing unit 141 for the movement amount v of the autonomous mobile body, and the vision The estimated reliability distribution processing unit 143 is integrated by weighted averaging with the lower limit of the estimated reliability distribution set by the estimated reliability distribution processing unit 143. 141, the lower limit of the estimated reliability distribution set by 141, the lower limit of the estimated reliability distribution set by the gyro estimated reliability distribution processing unit 142, and the setting of the vision estimated reliability distribution processing unit 143 And the estimated reliability distribution limit integration by averaging weights.

Ｐｌｓ分布生成部１４６は、（イ）自律移動体の移動量ｕについて、上限重み平均算出部１４４の統合した上限と下限重み平均算出部１４５の統合した下限とに対してDempster-Shafer 確率モデルを適用することで、自律移動体の移動量ｕについての信頼度分布を示すＰｌｓ分布を生成し、（ロ）自律移動体の移動量ｖについて、上限重み平均算出部１４４の統合した上限と下限重み平均算出部１４５の統合した下限とに対してDempster-Shafer 確率モデルを適用することで、自律移動体の移動量ｖについての信頼度分布を示すＰｌｓ分布を生成し、（ハ）自律移動体の移動方向の変化量θについて、上限重み平均算出部１４４の統合した上限と下限重み平均算出部１４５の統合した下限とに対してDempster-Shafer 確率モデルを適用することで、自律移動体の移動方向の変化量θについての信頼度分布を示すＰｌｓ分布を生成する。 The Pls distribution generation unit 146 applies (1) a Dempster-Shafer probability model to the upper limit integrated by the upper limit weight average calculation unit 144 and the lower limit integrated by the lower limit weight average calculation unit 145 for the movement amount u of the autonomous mobile body. By applying, a Pls distribution indicating a reliability distribution for the movement amount u of the autonomous mobile body is generated, and (b) the upper limit and lower limit weights integrated by the upper limit weight average calculation unit 144 for the movement amount v of the autonomous mobile body By applying the Dempster-Shafer probability model to the integrated lower limit of the average calculation unit 145, a Pls distribution indicating a reliability distribution for the movement amount v of the autonomous mobile body is generated. By applying the Dempster-Shafer probability model to the upper limit integrated by the upper limit weight average calculation unit 144 and the lower limit integrated by the lower limit weight average calculation unit 145 for the change amount θ in the movement direction, the autonomous mobile body Generating a Pls distribution indicating the reliability distribution for the amount of change in the moving direction theta.

統合処理部１４７は、（イ）パーティクルフィルタ１４０により生成された自律移動体の移動量ｕについてのビジョン信頼度分布１４８と、Ｐｌｓ分布生成部１４６により生成された自律移動体の移動量ｕについてのＰｌｓ分布とを統合することで、パーティクルフィルタ１４０のリサンプリングに用いられる自律移動体の移動量ｕについてのビジョン信頼度分布（ビジョン信頼度分布１４８に代わるもの）を生成し、（ロ）さらに、パーティクルフィルタ１４０により生成された自律移動体の移動量ｖについてのビジョン信頼度分布１４８と、Ｐｌｓ分布生成部１４６により生成された自律移動体の移動量ｖについてのＰｌｓ分布とを統合することで、パーティクルフィルタ１４０のリサンプリングに用いられる自律移動体の移動量ｖについてのビジョン信頼度分布（ビジョン信頼度分布１４８に代わるもの）を生成し、（ハ）さらに、パーティクルフィルタ１４０により生成された自律移動体の移動方向の変化量θについてのビジョン信頼度分布１４８と、Ｐｌｓ分布生成部１４６により生成された自律移動体の移動方向の変化量θについてのＰｌｓ分布とを統合することで、パーティクルフィルタ１４０のリサンプリングに用いられる自律移動体の移動方向の変化量θについてのビジョン信頼度分布（ビジョン信頼度分布１４８に代わるもの）を生成する。 The integrated processing unit 147 performs (a) a vision reliability distribution 148 for the movement amount u of the autonomous moving body generated by the particle filter 140 and a movement amount u of the autonomous moving body generated by the Pls distribution generation unit 146. By integrating the Pls distribution, a vision reliability distribution (an alternative to the vision reliability distribution 148) for the movement amount u of the autonomous moving body used for resampling of the particle filter 140 is generated, and (b) By integrating the vision reliability distribution 148 for the movement amount v of the autonomous mobile body generated by the particle filter 140 and the Pls distribution for the movement amount v of the autonomous mobile body generated by the Pls distribution generation unit 146, The movement amount v of the autonomous moving body used for resampling of the particle filter 140 Vision reliability distribution (alternative to vision reliability distribution 148), and (c) a vision reliability distribution 148 for the change amount θ in the moving direction of the autonomous moving body generated by the particle filter 140; , By integrating the Pls distribution with respect to the change amount θ of the movement direction of the autonomous mobile body generated by the Pls distribution generation unit 146, the change amount θ of the movement direction of the autonomous mobile body used for resampling of the particle filter 140. A vision reliability distribution (alternative to the vision reliability distribution 148) is generated.

次に、図２のように構成される自己位置推定モジュール１４の実行する処理について説明する。 Next, processing executed by the self-position estimation module 14 configured as shown in FIG. 2 will be described.

パーティクルフィルタ１４０は、パーティクルのリサンプリングとサンプルの評価を行い、ビジョンによる信頼度の分布であるビジョン信頼度分布１４８を生成する。 The particle filter 140 performs resampling of the particles and evaluation of the sample, and generates a vision reliability distribution 148 which is a distribution of reliability according to vision.

このビジョン信頼度分布１４８の生成を受けて、エンコーダ推定信頼度分布処理部１４１は、エンコーダ２００，２０１により検出された自律移動体の移動量ｕ，ｖについての推定信頼度分布を生成して、その生成した推定信頼度分布の上限及び下限を設定し、さらに、移動方向の変化量θについての推定信頼度分布を生成して、その生成した推定信頼度分布の上限及び下限を設定する。 In response to the generation of the vision reliability distribution 148, the encoder estimated reliability distribution processing unit 141 generates an estimated reliability distribution for the movement amounts u and v of the autonomous mobile body detected by the encoders 200 and 201. An upper limit and a lower limit of the generated estimated reliability distribution are set, an estimated reliability distribution for the moving direction change amount θ is generated, and an upper limit and a lower limit of the generated estimated reliability distribution are set.

例えば、図３に示すように、エンコーダ２００により検出された自律移動体の移動量ｕの近傍に、三角分布で定義される信頼度分布の集合を想定することで、その移動量ｕについての推定信頼度分布を生成して、その生成した推定信頼度分布の上限及び下限を設定するのである。 For example, as shown in FIG. 3, by assuming a set of reliability distributions defined by a triangular distribution in the vicinity of the movement amount u of the autonomous mobile body detected by the encoder 200, the estimation of the movement amount u is performed. A reliability distribution is generated, and an upper limit and a lower limit of the generated estimated reliability distribution are set.

そして、このビジョン信頼度分布１４８の生成を受けて、ジャイロ推定信頼度分布処理部１４２は、エンコーダ推定信頼度分布処理部１４１と同様の処理に従って、ジャイロ２０２により検出された自律移動体の移動方向の変化量θについての推定信頼度分布を生成して、その生成した推定信頼度分布の上限及び下限を設定する。 In response to the generation of the vision reliability distribution 148, the gyro estimated reliability distribution processing unit 142 follows the same process as the encoder estimated reliability distribution processing unit 141, and the moving direction of the autonomous mobile body detected by the gyro 202 An estimated reliability distribution for the amount of change θ is generated, and an upper limit and a lower limit of the generated estimated reliability distribution are set.

一方、このビジョン信頼度分布１４８の生成を受けて、ビジョン推定信頼度分布処理部１４３は、ビジョン信頼度分布１４８に基づいて、ビジョンにより検出された自律移動体の移動量ｕ，ｖについての推定信頼度分布を生成して、その生成した推定信頼度分布の上限及び下限を設定するとともに、ビジョンにより検出された自律移動体の移動方向の変化量θについての推定信頼度分布を生成して、その生成した推定信頼度分布の上限及び下限を設定する。 On the other hand, in response to the generation of the vision reliability distribution 148, the vision estimated reliability distribution processing unit 143 estimates the movement amounts u and v of the autonomous mobile body detected by the vision based on the vision reliability distribution 148. Generate a reliability distribution, set an upper limit and a lower limit of the generated estimated reliability distribution, and generate an estimated reliability distribution about the amount of change θ in the moving direction of the autonomous mobile body detected by the vision, The upper and lower limits of the generated estimated reliability distribution are set.

例えば、移動量ｕについての推定信頼度分布の上限及び下限の設定処理で説明するならば、（ｕ_i,ｖ_i,θ_i) という３次元空間値を持つパーティクルＳ_i（例えば、ｉ＝１〜５００）のそれぞれについて、パーティクルＳ_iを中心とする近傍空間を定義して、区間〔ｕ_i−ｕ’，ｕ_i＋ｕ’〕の中に入る全てのパーティクルについて、パーティクルＳ_iの近傍空間に入るのか否かをチェックして、近傍空間に入ると判断したパーティクルＳ_iの信頼度の平均値ｍ_iを算出することで、
｛〔ｕ₁−ｕ’，ｕ₁＋ｕ’〕，ｍ₁｝
｛〔ｕ₂−ｕ’，ｕ₂＋ｕ’〕，ｍ₂｝
・
・
｛〔ｕ₅₀₀−ｕ’，ｕ₅₀₀＋ｕ’〕，ｍ₅₀₀｝
という集合を生成して、
上限＝Σｍ_i ただし、ａ∩ｂ≠φの総和を算出する・・・・（１）式
下限＝Σｍ_i ただし、ａ⊆ｂ≠φの総和を算出する・・・・（２）式
という算出式に従って、図４に示すように、ビジョンにより検出された自律移動体の移動量ｕについての推定信頼度分布の上限及び下限を設定するのである。 For example, in the case of the processing for setting the upper limit and lower limit of the estimated reliability distribution for the movement amount u, a particle S _i (for example, i = 1) having a three-dimensional spatial value of (u _i, v _i, θ _i ). ˜500), a neighborhood space centered on the particle S _i is defined, and all particles that fall within the interval [u _i −u ′, u _i + u ′] are included in the neighborhood space of the particle S _i. entering the whether check to the, by calculating the average value m _i of the reliability of particle S _i which is determined to fall near the 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)
Lower limit = Σm _i However, the sum of a⊆b ≠ φ is calculated. As shown in FIG. 4, according to the calculation formula (2), the movement amount u of the autonomous mobile body detected by the vision is calculated. The upper and lower limits of the estimated reliability distribution are set.

ここで、（１）式及び（２）式はDempster-Shafer 確率モデルで使用されている式であり、Dempster-Shafer 確率モデルについて記載する下記の文献では、（１）式についてはＰｌｓ（ｂ）として記載し、（２）式についてはＢｅｌ（ｂ）と記載している。 Here, the formulas (1) and (2) are formulas used in the Dempster-Shafer probability model. In the following document describing the Dempster-Shafer probability model, the formula (1) is expressed as Pls (b) The equation (2) is described as Bel (b).

〔Dempster-Shafer 確率モデルの文献〕
S.Ferson, V.Kreinovich, L.Ginzburg, D.Myers, and K.Sents, "Constructing probability boxes and DempsterShafer structures," Technical report, Sandia National Laboratories, 2003.
エンコーダ推定信頼度分布処理部１４１、ジャイロ推定信頼度分布処理部１４２及びビジョン推定信頼度分布処理部１４３が推定信頼度分布の上限を設定すると、上限重み平均算出部１４４は、自律移動体の移動量ｕについて、それらの設定された推定信頼度分布の上限を重み平均することで統合し、さらに、自律移動体の移動量ｖについて、それらの設定された推定信頼度分布の上限を重み平均することで統合し、さらに、自律移動体の移動方向の変化量θについて、それらの設定された推定信頼度分布の上限を重み平均することで統合する。 [Documents of Dempster-Shafer probabilistic model]
S. Ferson, V. Kreinovich, L. Ginzburg, D. Myers, and K. Sents, "Constructing probability boxes and Dempster Shafer structures," Technical report, Sandia National Laboratories, 2003.
When the encoder estimated reliability distribution processing unit 141, the gyro estimated reliability distribution processing unit 142, and the vision estimated reliability distribution processing unit 143 set the upper limit of the estimated reliability distribution, the upper limit weight average calculating unit 144 moves the autonomous mobile object. For the quantity u, the upper limits of the set estimated reliability distributions are integrated by weighted averaging, and for the movement amount v of the autonomous mobile body, the upper limits of the set estimated reliability distributions are weighted averaged. In addition, the change amount θ in the moving direction of the autonomous mobile body is integrated by weighted averaging the upper limits of the set estimated reliability distributions.

すなわち、自律移動体の移動量ｕについて説明するならば、図５に示すような重み平均演算を行うことで、エンコーダ推定信頼度分布処理部１４１が自律移動体の移動量ｕについて設定した推定信頼度分布の上限と、ビジョン推定信頼度分布処理部１４３が自律移動体の移動量ｕについて設定した推定信頼度分布の上限とを統合するのである。 That is, if the movement amount u of the autonomous mobile body is described, the estimated reliability set by the encoder estimated reliability distribution processing unit 141 for the movement amount u of the autonomous mobile body is performed by performing a weighted average calculation as shown in FIG. The upper limit of the degree distribution and the upper limit of the estimated reliability distribution set by the vision estimated reliability distribution processing unit 143 for the movement amount u of the autonomous mobile body are integrated.

そして、エンコーダ推定信頼度分布処理部１４１、ジャイロ推定信頼度分布処理部１４２及びビジョン推定信頼度分布処理部１４３が推定信頼度分布の下限を設定すると、下限重み平均算出部１４５は、上限重み平均算出部１４４と同様の重み平均演算を実行することで、自律移動体の移動量ｕについて、それらの設定された推定信頼度分布の下限を重み平均することで統合し、さらに、自律移動体の移動量ｖについて、それらの設定された推定信頼度分布の下限を重み平均することで統合し、さらに、自律移動体の移動方向の変化量θについて、それらの設定された推定信頼度分布の下限を重み平均することで統合する。 When the encoder estimated reliability distribution processing unit 141, the gyro estimated reliability distribution processing unit 142, and the vision estimated reliability distribution processing unit 143 set the lower limit of the estimated reliability distribution, the lower limit weight average calculating unit 145 By performing the same weighted average calculation as that of the calculation unit 144, the moving amount u of the autonomous mobile body is integrated by performing weighted averaging of the lower limits of the set estimated reliability distributions. For the movement amount v, the lower limits of the set estimated reliability distributions are integrated by weighted averaging, and the lower limit of the set estimated reliability distribution for the change amount θ in the movement direction of the autonomous mobile body. Are integrated by weighted averaging.

このようにして、自己位置推定モジュール１４は、図６に示すように、エンコーダ２００により検出された自律移動体の移動量ｕについての推定信頼度分布（図中に示すエラーモデル）を生成して、その生成した推定信頼度分布の上限及び下限を設定し、さらに、ビジョン信頼度分布１４８に基づいて、ビジョンにより検出された自律移動体の移動量ｕについての推定信頼度分布を生成して、その生成した推定信頼度分布の上限及び下限を設定して、それらの上限を重み平均により統合するとともに、それらの下限を重み平均により統合するように処理するのである。 In this way, as shown in FIG. 6, the self-position estimation module 14 generates an estimated reliability distribution (error model shown in the figure) for the movement amount u of the autonomous mobile body detected by the encoder 200. Set an upper limit and a lower limit of the generated estimated reliability distribution, and further, based on the vision reliability distribution 148, generate an estimated reliability distribution for the movement amount u of the autonomous mobile body detected by the vision, An upper limit and a lower limit of the generated estimated reliability distribution are set, and these upper limits are integrated by weighted average, and those lower limits are integrated by weighted average.

そして、自己位置推定モジュール１４は、図７に示すように、エンコーダ２０１により検出された自律移動体の移動量ｖについての推定信頼度分布を生成して、その生成した推定信頼度分布の上限及び下限を設定し、さらに、ビジョン信頼度分布１４８に基づいて、ビジョンにより検出された自律移動体の移動量ｖについての推定信頼度分布を生成して、その生成した推定信頼度分布の上限及び下限を設定して、それらの上限を重み平均により統合するとともに、それらの下限を重み平均により統合するように処理するのである。 Then, as shown in FIG. 7, the self-position estimation module 14 generates an estimated reliability distribution for the movement amount v of the autonomous mobile body detected by the encoder 201, and the upper limit of the generated estimated reliability distribution and A lower limit is set, and further, based on the vision reliability distribution 148, an estimated reliability distribution for the movement amount v of the autonomous mobile body detected by the vision is generated, and the upper and lower limits of the generated estimated reliability distribution Are set so that the upper limits thereof are integrated by the weighted average, and the lower limits thereof are integrated by the weighted average.

そして、自己位置推定モジュール１４は、図８に示すように、エンコーダ２００，２０１により検出された自律移動体の移動量ｕ，ｖに基づいて自律移動体の移動方向の変化量θについての推定信頼度分布を生成して、その生成した推定信頼度分布の上限及び下限を設定、さらに、ジャイロ２０２により検出された自律移動体の移動方向の変化量θについての推定信頼度分布を生成して、その生成した推定信頼度分布の上限及び下限を設定し、さらに、ビジョンにより検出された自律移動体の移動方向の変化量θについての推定信頼度分布を生成して、その生成した推定信頼度分布の上限及び下限を設定して、それらの上限を重み平均により統合するとともに、それらの下限を重み平均により統合するように処理するのである。 Then, as shown in FIG. 8, the self-position estimation module 14 estimates the reliability of the change θ in the movement direction of the autonomous mobile body based on the movement amounts u and v of the autonomous mobile body detected by the encoders 200 and 201. A degree distribution, set an upper limit and a lower limit of the generated estimated reliability distribution, and further generate an estimated reliability distribution for the amount of change θ in the moving direction of the autonomous mobile body detected by the gyro 202, The upper and lower limits of the generated estimated reliability distribution are set, and further, an estimated reliability distribution is generated for the change amount θ of the moving direction of the autonomous mobile body detected by the vision, and the generated estimated reliability distribution An upper limit and a lower limit are set, and these upper limits are integrated by weighted average, and those lower limits are integrated by weighted average.

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

すなわち、上限重み平均算出部１４４の統合した上限と下限重み平均算出部１４５の統合した下限とに対して、上述の（１）式を適用することで、図９に示すようなＰｌｓ分布を生成することで、自律移動体の移動量ｕについての信頼度分布を示すＰｌｓ分布と、自律移動体の移動量ｖについての信頼度分布を示すＰｌｓ分布と、自律移動体の移動方向の変化量θについての信頼度分布を示すＰｌｓ分布とを生成するのである。 That is, by applying the above equation (1) to the upper limit integrated by the upper limit weight average calculation unit 144 and the lower limit integrated by the lower limit weight average calculation unit 145, a Pls distribution as shown in FIG. 9 is generated. By doing so, a Pls distribution indicating a reliability distribution for the movement amount u of the autonomous mobile body, a Pls distribution indicating a reliability distribution for the movement amount v of the autonomous mobile body, and a change amount θ in the movement direction of the autonomous mobile body And a Pls distribution indicating a reliability distribution for the.

このＰｌｓ分布生成部１４６によるＰｌｓ分布の生成を受けて、統合処理部１４７は、パーティクルフィルタ１４０により生成されたビジョン信頼度分布１４８と、Ｐｌｓ分布生成部１４６により生成されたＰｌｓ分布とを統合することで、パーティクルフィルタ１４０により生成された自律移動体の移動量ｕについてのビジョン信頼度分布１４８と、パーティクルフィルタ１４０により生成された自律移動体の移動量ｖについてのビジョン信頼度分布１４８と、パーティクルフィルタ１４０により生成された自律移動体の移動方向の変化量θについてのビジョン信頼度分布１４８とを補正する。 Upon receiving the Pls distribution generated by the Pls distribution generation unit 146, the integration processing unit 147 integrates the vision reliability distribution 148 generated by the particle filter 140 and the Pls distribution generated by the Pls distribution generation unit 146. Thus, the vision reliability distribution 148 for the movement amount u of the autonomous moving body generated by the particle filter 140, the vision reliability distribution 148 for the movement amount v of the autonomous moving body generated by the particle filter 140, and the particles The vision reliability distribution 148 for the amount of change θ in the moving direction of the autonomous moving body generated by the filter 140 is corrected.

すなわち、自律移動体の移動量ｕについて説明するならば、図１０に示すような統合演算を行うことで、パーティクルフィルタ１４０により生成された自律移動体の移動量ｕについてのビジョン信頼度分布１４８と、Ｐｌｓ分布生成部１４６により生成された自律移動体の移動量ｕについてのＰｌｓ分布とを統合することで、パーティクルフィルタ１４０により生成された自律移動体の移動量ｕについてのビジョン信頼度分布１４８を補正するのである。 That is, if the movement amount u of the autonomous mobile body is described, a vision reliability distribution 148 about the movement amount u of the autonomous mobile body generated by the particle filter 140 is obtained by performing an integration operation as shown in FIG. The vision reliability distribution 148 for the movement amount u of the autonomous mobile body generated by the particle filter 140 is integrated by integrating the Pls distribution for the movement amount u of the autonomous mobile body generated by the Pls distribution generation unit 146. It is corrected.

このようにして、自己位置推定モジュール１４は、図１１の処理フローに示すように、ステップＳ１０で、パーティクルフィルタ１４０によりパーティクルのリサンプリングとサンプルの評価を行い、ビジョンによる信頼度の分布であるビジョン信頼度分布１４８を生成すると、続いて、ステップＳ１１で、エンコーダ２００，２０１の検出値に基づいて、自律移動体の移動量ｕ，ｖについての推定信頼度分布を生成して、その生成した推定信頼度分布の上限及び下限を設定し、さらに、自律移動体の移動方向の変化量θについての推定信頼度分布を生成して、その生成した推定信頼度分布の上限及び下限を設定する。 In this way, as shown in the processing flow of FIG. 11, the self-position estimation module 14 performs the resampling of the particles and the evaluation of the sample by the particle filter 140 in step S10, and the vision which is the reliability distribution by vision. When the reliability distribution 148 is generated, subsequently, in step S11, based on the detection values of the encoders 200 and 201, an estimated reliability distribution for the movement amounts u and v of the autonomous mobile body is generated, and the generated estimation is performed. An upper limit and a lower limit of the reliability distribution are set, an estimated reliability distribution is generated for the change amount θ in the moving direction of the autonomous mobile body, and an upper limit and a lower limit of the generated estimated reliability distribution are set.

続いて、ステップＳ１２で、ジャイロ２０２の検出値に基づいて、自律移動体の移動方向の変化量θについての推定信頼度分布を生成して、その生成した推定信頼度分布の上限及び下限を設定する。 Subsequently, in step S12, based on the detected value of the gyro 202, an estimated reliability distribution is generated for the change amount θ in the moving direction of the autonomous mobile body, and upper and lower limits of the generated estimated reliability distribution are set. To do.

続いて、ステップＳ１３で、ビジョン信頼度分布１４８に基づいて、自律移動体の移動量ｕ，ｖについての推定信頼度分布を生成して、その生成した推定信頼度分布の上限及び下限を設定し、さらに、自律移動体の移動方向の変化量θについての推定信頼度分布を生成して、その生成した推定信頼度分布の上限及び下限を設定する。 Subsequently, in step S13, based on the vision reliability distribution 148, an estimated reliability distribution for the movement amounts u and v of the autonomous mobile body is generated, and upper and lower limits of the generated estimated reliability distribution are set. Furthermore, an estimated reliability distribution for the amount of change θ in the moving direction of the autonomous mobile body is generated, and an upper limit and a lower limit of the generated estimated reliability distribution are set.

続いて、ステップＳ１４で、自律移動体の移動量ｕについて、設定した推定信頼度分布の上限を重み平均することで統合し、さらに、自律移動体の移動量ｖについて、設定した推定信頼度分布の上限を重み平均することで統合し、さらに、自律移動体の移動方向の変化量θについて、設定した推定信頼度分布の上限を重み平均することで統合する。 Subsequently, in step S14, the upper limit of the set estimated reliability distribution is integrated for the moving amount u of the autonomous moving body by weighted averaging, and further, the set estimated reliability distribution is set for the moving amount v of the autonomous moving body. Are integrated by weighted average, and further, the upper limit of the set estimated reliability distribution is integrated by weighted average for the change amount θ in the movement direction of the autonomous mobile body.

続いて、ステップＳ１５で、自律移動体の移動量ｕについて、設定した推定信頼度分布の下限を重み平均することで統合し、さらに、自律移動体の移動量ｖについて、設定した推定信頼度分布の下限を重み平均することで統合し、さらに、自律移動体の移動方向の変化量θについて、設定した推定信頼度分布の下限を重み平均することで統合する。 Subsequently, in step S15, the moving amount u of the autonomous mobile body is integrated by weighted averaging the lower limit of the set estimated reliability distribution, and further, the set estimated reliability distribution is set for the moving amount v of the autonomous mobile body. Are integrated by weighted average, and further, the lower limit of the set estimated reliability distribution is integrated by weighted average for the change amount θ in the movement direction of the autonomous mobile body.

続いて、ステップＳ１６で、自律移動体の移動量ｕについて、統合した上限と統合した下限とに対してDempster-Shafer 確率モデルを適用することで、自律移動体の移動量ｕについての信頼度分布を示すＰｌｓ分布を生成し、さらに、自律移動体の移動量ｖについて、統合した上限と統合した下限とに対してDempster-Shafer 確率モデルを適用することで、自律移動体の移動量ｖについての信頼度分布を示すＰｌｓ分布を生成し、さらに、自律移動体の移動方向の変化量θについて、統合した上限と統合した下限とに対してDempster-Shafer 確率モデルを適用することで、自律移動体の移動方向の変化量θについての信頼度分布を示すＰｌｓ分布を生成する。 Subsequently, in step S16, by applying the Dempster-Shafer probability model to the integrated upper limit and the integrated lower limit for the movement amount u of the autonomous mobile body, the reliability distribution for the movement amount u of the autonomous mobile body Is generated, and the Dempster-Shafer probability model is applied to the integrated upper limit and the integrated lower limit for the movement amount v of the autonomous mobile body. By generating a Pls distribution indicating a reliability distribution and applying a Dempster-Shafer probability model to the integrated upper limit and the integrated lower limit for the change amount θ in the movement direction of the autonomous mobile object, the autonomous mobile object Pls distribution indicating the reliability distribution for the change amount θ in the moving direction is generated.

続いて、ステップＳ１７で、自律移動体の移動量ｕについて、パーティクルフィルタ１４０により生成したビジョン信頼度分布１４８をＰｌｓ分布で補正し、自律移動体の移動量ｖについて、パーティクルフィルタ１４０により生成したビジョン信頼度分布１４８をＰｌｓ分布で補正し、自律移動体の移動方向の変化量θについて、パーティクルフィルタ１４０により生成したビジョン信頼度分布１４８をＰｌｓ分布で補正して、パーティクルフィルタ１４０による次のリサンプリングにと処理を進めていくことになる。 Subsequently, in step S17, the vision reliability distribution 148 generated by the particle filter 140 is corrected with the Pls distribution for the movement amount u of the autonomous moving body, and the vision generated by the particle filter 140 is corrected for the movement amount v of the autonomous moving body. The reliability distribution 148 is corrected by the Pls distribution, and the vision reliability distribution 148 generated by the particle filter 140 is corrected by the Pls distribution with respect to the change amount θ in the moving direction of the autonomous moving body, and the next resampling by the particle filter 140 is performed. The process will continue.

図１２に、本発明の有効性を検証するために行った実験結果を図示する。 FIG. 12 illustrates the results of an experiment conducted to verify the effectiveness of the present invention.

この実験結果では、ｕ，ｖ，θについてのＰｌｓ分布と、それらのＰｌｓ分布を統合する前のビジョン信頼度分布１４８と、それらのＰｌｓ分布を統合した後のビジョン信頼度分布１４８の一例を図示している。 This experimental result shows an example of Pls distributions for u, v, and θ, vision reliability distribution 148 before integrating those Pls distributions, and vision reliability distribution 148 after integrating those Pls distributions. Show.

この実験結果から分かるように、エンコーダ２００，２０１及びジャイロ２０２の検出結果をビジョン信頼度分布１４８に反映させることで、誤りのピークが抑制されることになるとともに、曖昧さが抑制されることになり、これにより本発明の有効性を検証できた。 As can be seen from the experimental results, by reflecting the detection results of the encoders 200 and 201 and the gyro 202 in the vision reliability distribution 148, an error peak is suppressed and ambiguity is suppressed. Thus, the effectiveness of the present invention was verified.

本発明によれば、分散センサネットワーク及びハイブリッドアーキテクチャの制御方式を用いる自律移動体を対象として、自律移動体の自己位置を推定するために、内部センサによるデッドレコリングと外部センサによるデッドレコリングとを統合して、自律移動体の自己位置を高精度に推定することができるようになる。 According to the present invention, in order to estimate the self-position of an autonomous mobile body for an autonomous mobile body using a distributed sensor network and a hybrid architecture control method, dead recording by an internal sensor and dead recording by an external sensor are performed. Can be integrated to estimate the self-position of the autonomous mobile body with high accuracy.

（付記１）上位センサによる環境測定を行い、パーティクルフィルタを用いて自律移動体の状態を推定してその移動を制御する上位制御機構と、下位センサによる検出値に基づいて自律移動体の状態を推定してその移動を制御する下位制御機構とを備える自律移動体であって、前記下位センサの検出値に基づいて推定した自律移動体の状態変化量を処理対象として、その状態変化量の信頼度分布の上限及び下限を設定する第１の設定手段と、前記環境測定により推定した各パーティクルにおける自律移動体の状態変化量の信頼度に基づいて、自律移動体の状態変化量の信頼度分布の上限及び下限を設定する第２の設定手段と、自律移動体の状態毎に、前記第１及び第２の設定手段の設定した上限を統合するとともに、前記第１及び第２の設定手段の設定した下限を統合する統合手段と、前記統合手段の統合した上限及び下限に基づいて、自律移動体の状態変化量の信頼度分布を算出する算出手段と、前記環境測定により推定した各パーティクルにおける自律移動体の状態変化量の信頼度を、前記算出手段の算出した信頼度分布に基づいて修正する修正手段とを備えることを、特徴とする自律移動体。 (Supplementary note 1) An upper-level control mechanism that measures the environment using a higher-order sensor, estimates the state of the autonomous mobile body using a particle filter, and controls its movement; An autonomous mobile body comprising a lower-level control mechanism that estimates and controls the movement, and the state change amount of the autonomous mobile body estimated based on the detection value of the lower-order sensor is a processing target, and the state change amount is trusted. A first setting means for setting an upper limit and a lower limit of the degree distribution, and a reliability distribution of the state change amount of the autonomous mobile body based on the reliability of the state change amount of the autonomous mobile body in each particle estimated by the environmental measurement And the second setting means for setting the upper limit and the lower limit of the first and the upper limits set by the first and second setting means for each state of the autonomous mobile body, and the first and second settings Integration means for integrating the lower limit set by the stage, calculation means for calculating the reliability distribution of the state change amount of the autonomous mobile body based on the integrated upper limit and lower limit of the integration means, and each estimated by the environmental measurement An autonomous moving body comprising correction means for correcting the reliability of the state change amount of the autonomous moving body in the particles based on the reliability distribution calculated by the calculating means.

（付記２）付記１に記載の自律移動体において、前記第１の設定手段は、前記下位センサの検出値を中心にして、その近傍領域に規定の分布を示す状態変化量の集合の信頼度を設定して、その集合の示す信頼度に基づいて、前記上限及び下限を設定することを、特徴とする自律移動体。 (Supplementary note 2) In the autonomous mobile body according to supplementary note 1, the first setting means has a reliability of a set of state change amounts having a prescribed distribution in a neighborhood region around the detection value of the lower sensor. And an upper limit and a lower limit are set based on the reliability indicated by the set.

（付記３）付記１に記載の自律移動体において、前記第２の設定手段は、各パーティクルの位置する状態変化量空間を中心にして、その近傍領域に含まれるパーティクルの持つ状態変化量の信頼度の平均値を算出することで、各パーティクルについて区間信頼度を設定して、それらの区間信頼度の示す信頼度に基づいて、前記上限及び下限を設定することを、特徴とする自律移動体。 (Supplementary note 3) In the autonomous mobile body according to supplementary note 1, the second setting means has a state change amount space where each particle is located as a center, and the reliability of the state change amount possessed by the particles included in the neighboring region By calculating the average value of degrees, an interval reliability is set for each particle, and the upper limit and the lower limit are set based on the reliability indicated by the interval reliability. .

（付記４）付記１ないし３のいずれか１項に記載の自律移動体において、前記統合手段は、前記第１及び第２の設定手段の設定した上限を重み平均することでそれらの上限を統合するとともに、前記第１及び第２の設定手段の設定した下限を重み平均することでそれらの下限を統合することを、特徴とする自律移動体。 (Appendix 4) In the autonomous mobile body according to any one of appendices 1 to 3, the integration unit integrates the upper limits by weighted average of the upper limits set by the first and second setting units. An autonomous mobile body characterized by integrating the lower limits by weighted averaging the lower limits set by the first and second setting means.

（付記５）付記１ないし４のいずれか１項に記載の自律移動体において、前記算出手段は、前記統合手段の統合した上限及び下限に対してDempster-Shafer 確率モデルを適用することで、自律移動体の状態変化量の信頼度分布を算出することを、特徴とする自律移動体。 (Supplementary note 5) In the autonomous mobile body according to any one of supplementary notes 1 to 4, the calculation means autonomously applies a Dempster-Shafer probability model to the upper and lower limits integrated by the integration means. An autonomous mobile body characterized by calculating a reliability distribution of a state change amount of the mobile body.

（付記６）付記１ないし５のいずれか１項に記載の自律移動体において、前記修正手段は、前記環境測定により推定した各パーティクルにおける自律移動体の状態変化量の信頼度と、前記算出手段の算出した信頼度分布の持つ該当する信頼度とを乗算することで、前記環境測定により推定した各パーティクルにおける自律移動体の状態変化量の信頼度を修正することを、特徴とする自律移動体。 (Appendix 6) In the autonomous mobile body according to any one of appendices 1 to 5, the correction means includes the reliability of the state change amount of the autonomous mobile body in each particle estimated by the environmental measurement, and the calculation means. An autonomous mobile body characterized by correcting the reliability of the state change amount of the autonomous mobile body in each particle estimated by the environmental measurement by multiplying the corresponding reliability of the calculated reliability distribution of .

（付記７）上位センサによる環境測定を行い、パーティクルフィルタを用いて自律移動体の状態を推定してその移動を制御する上位制御機構と、下位センサによる検出値に基づいて自律移動体の状態を推定してその移動を制御する下位制御機構とを備える自律移動体を制御するために用いられる自律移動体制御用プログラムであって、コンピュータを、前記下位センサの検出値に基づいて推定した自律移動体の状態変化量を処理対象として、その状態変化量の信頼度分布の上限及び下限を設定する第１の設定手段と、前記環境測定により推定した各パーティクルにおける自律移動体の状態変化量の信頼度に基づいて、自律移動体の状態変化量の信頼度分布の上限及び下限を設定する第２の設定手段と、自律移動体の状態毎に、前記第１及び第２の設定手段の設定した上限を統合するとともに、前記第１及び第２の設定手段の設定した下限を統合する統合手段と、前記統合手段の統合した上限及び下限に基づいて、自律移動体の状態変化量の信頼度分布を算出する算出手段と、前記環境測定により推定した各パーティクルにおける自律移動体の状態変化量の信頼度を、前記算出手段の算出した信頼度分布に基づいて修正する修正手段として機能させるための自律移動体制御用プログラム。 (Supplementary note 7) A host control mechanism that measures the environment by a host sensor, estimates the state of the autonomous mobile body using a particle filter and controls its movement, and the state of the autonomous mobile body based on the detection value by the lower sensor An autonomous mobile body control program used for controlling an autonomous mobile body provided with a low-order control mechanism that estimates and controls movement thereof, wherein the computer estimates the computer based on a detection value of the low-order sensor A first setting means for setting an upper limit and a lower limit of a reliability distribution of the state change amount, and a reliability of the state change amount of the autonomous mobile body in each particle estimated by the environmental measurement Based on the second setting means for setting an upper limit and a lower limit of the reliability distribution of the state change amount of the autonomous mobile body, and for each state of the autonomous mobile body, the first and first The integration means for integrating the upper limits set by the setting means and the integration means for integrating the lower limits set by the first and second setting means, and the state of the autonomous mobile body based on the integrated upper and lower limits of the integration means A calculation unit that calculates a reliability distribution of the amount of change, and a correction unit that corrects the reliability of the state change amount of the autonomous moving body in each particle estimated by the environmental measurement based on the reliability distribution calculated by the calculation unit Program for autonomous mobile body control.

本発明を具備するハイブリッドアーキテクチャ構成の自律移動体に搭載されることになる上位制御機構及び下位制御機構の装置構成例である。It is an apparatus structural example of the high-order control mechanism and low-order control mechanism which are mounted in the autonomous mobile body of the hybrid architecture structure which comprises this invention. 自己位置推定モジュールの装置構成例である。It is an apparatus structural example of a self-position estimation module. 推定信頼度分布の上限及び下限の設定処理の説明図である。It is explanatory drawing of the setting process of the upper limit and lower limit of estimated reliability distribution. 推定信頼度分布の上限及び下限の設定処理の説明図である。It is explanatory drawing of the setting process of the upper limit and lower limit of estimated reliability distribution. 重み平均演算の説明図である。It is explanatory drawing of a weighted average calculation. 自己位置推定モジュールの実行する処理の説明図である。It is explanatory drawing of the process which a self-position estimation module performs. 自己位置推定モジュールの実行する処理の説明図である。It is explanatory drawing of the process which a self-position estimation module performs. 自己位置推定モジュールの実行する処理の説明図である。It is explanatory drawing of the process which a self-position estimation module performs. Ｐｌｓ分布の生成処理の説明図である。It is explanatory drawing of the production | generation process of Pls distribution. 統合演算の説明図である。It is explanatory drawing of integrated calculation. 自己位置推定モジュールの実行する処理フローである。It is a processing flow which a self-position estimation module performs. 本発明の有効性を検証するために行った実験結果の説明図である。It is explanatory drawing of the experimental result performed in order to verify the effectiveness of this invention. 自律移動体の安定的なナビゲーションを実現するためのハイブリッドアーキテクチャ構成の説明図である。It is explanatory drawing of the hybrid architecture structure for implement | achieving the stable navigation of an autonomous mobile body.

Explanation of symbols

１上位制御機構
２下位制御機構
１０下位センサ情報取得モジュール
１１ステレオカメラ
１２ステレオカメラ入力部
１３３次元環境計測部
１４自己位置推定モジュール
１５経路計画モジュール
２０下位センサ系
２１センサデータ記憶モジュール
２２行動情報受付モジュール
２３単純反応処理モジュール
２４行動選択機構
２５自律移動体駆動機構
２００エンコーダ
２０１エンコーダ
２０２ジャイロ
２０３距離センサ DESCRIPTION OF SYMBOLS 1 High-order control mechanism 2 Low-order control mechanism 10 Low-order sensor information acquisition module 11 Stereo camera 12 Stereo camera input part 13 Three-dimensional environment measurement part 14 Self-position estimation module 15 Path planning module 20 Low-order sensor system 21 Sensor data storage module 22 Behavior information reception Module 23 Simple reaction processing module 24 Action selection mechanism 25 Autonomous moving body drive mechanism 200 Encoder 201 Encoder 202 Gyro 203 Distance sensor

Claims

Measure the environment with the upper sensor, estimate the state of the autonomous mobile body using the particle filter and control its movement, and estimate the state of the autonomous mobile body based on the detection value of the lower sensor An autonomous mobile body comprising a lower control mechanism for controlling movement,
A first setting means for setting an upper limit and a lower limit of a reliability distribution of the state change amount, with the state change amount of the autonomous mobile body estimated based on the detection value of the lower sensor as a processing target;
A second setting means for setting an upper limit and a lower limit of the reliability distribution of the state change amount of the autonomous mobile body based on the reliability of the state change amount 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;
Based on the upper and lower limits integrated by the integrating means, calculating means for calculating a reliability distribution of the state change amount of the autonomous mobile body,
A correction means for correcting the reliability of the state change amount of the autonomous mobile body in each particle estimated by the environmental measurement based on the reliability distribution calculated by the calculation means,
A feature of autonomous mobile.

The autonomous mobile body according to claim 1,
The first setting means sets a reliability of a set of state change amounts indicating a prescribed distribution in a neighboring area around the detection value of the lower sensor, and based on the reliability indicated by the set , Setting the upper and lower limits,
A feature of autonomous mobile.

The autonomous mobile body according to claim 1,
The second setting means calculates an average value of the reliability of the state change amount of the particles included in the vicinity region with the state change amount space where each particle is located as a center, thereby obtaining an interval for each particle. Setting reliability and setting the upper and lower limits based on the reliability indicated by the interval reliability;
A feature of autonomous mobile.

In the autonomous mobile body according to any one of claims 1 to 3,
The integration means integrates the upper limits by weighted averaging the upper limits set by the first and second setting means, and weights averages the lower limits set by the first and second setting means. Integrating those lower bounds with
A feature of autonomous mobile.

In the autonomous mobile body according to any one of claims 1 to 4,
The calculation means calculates the reliability distribution of the state change amount of the autonomous mobile body by applying a Dempster-Shafer probability model to the upper limit and the lower limit integrated by the integration means,
A feature of autonomous mobile.