JP7155686B2 - Autonomous mobile body position estimation device - Google Patents

Autonomous mobile body position estimation device Download PDF

Info

Publication number
JP7155686B2
JP7155686B2 JP2018130898A JP2018130898A JP7155686B2 JP 7155686 B2 JP7155686 B2 JP 7155686B2 JP 2018130898 A JP2018130898 A JP 2018130898A JP 2018130898 A JP2018130898 A JP 2018130898A JP 7155686 B2 JP7155686 B2 JP 7155686B2
Authority
JP
Japan
Prior art keywords
self
mobile body
autonomous mobile
estimator
value
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
JP2018130898A
Other languages
Japanese (ja)
Other versions
JP2020009252A (en
Inventor
晋悟 服部
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Toyota Industries Corp
Original Assignee
Toyota Industries Corp
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Toyota Industries Corp filed Critical Toyota Industries Corp
Priority to JP2018130898A priority Critical patent/JP7155686B2/en
Publication of JP2020009252A publication Critical patent/JP2020009252A/en
Application granted granted Critical
Publication of JP7155686B2 publication Critical patent/JP7155686B2/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Landscapes

  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Description

本発明は、自律移動体の位置推定装置に関する。 The present invention relates to a position estimation device for an autonomous mobile body.

自律移動体の位置推定装置としては、例えば特許文献１に記載されているように、地図の構築及び使用を行う同時位置推定地図構築（ＳＬＡＭ）を用いて、自己位置の推定を行う技術が知られている。 As a position estimating device for an autonomous mobile body, for example, as described in Patent Document 1, a technique for estimating a self-position using simultaneous localization map construction (SLAM) that constructs and uses a map is known. It is

特開２０１４－２２２５５０号公報JP 2014-222550 A

しかしながら、上記従来技術においては、自律移動体の周囲環境によっては自己位置推定結果にずれが生じることがあるため、自律移動体の自己位置を高精度に推定することが困難である。 However, in the conventional technology described above, it is difficult to estimate the self-position of the autonomous mobile body with high accuracy because the self-position estimation result may deviate depending on the surrounding environment of the autonomous mobile body.

本発明の目的は、自律移動体の自己位置の推定精度を向上させることができる自律移動体の位置推定装置を提供することである。 An object of the present invention is to provide a position estimation device for an autonomous mobile body that can improve the accuracy of self-position estimation of the autonomous mobile body.

本発明の一態様は、自律移動体の位置を推定する自律移動体の位置推定装置であって、自己位置推定技術を用いて自律移動体の自己位置を推定する自己位置推定器と、自律移動体に具備されたセンサを有し、自律移動体の自己位置を検出する自己位置検出部と、自己位置推定器により得られた自律移動体の位置推定値と自己位置検出部により得られた自律移動体の位置検出値とを比較して、自律移動体の位置推定値の外れ値を抽出して除去する外れ値除去部と、外れ値除去部により外れ値が除去された自律移動体の位置推定値に基づいて、自律移動体の自己位置を最終的に推定する最終自己位置推定部とを備える。 One aspect of the present invention is an autonomous mobile body position estimation device for estimating the position of an autonomous mobile body, comprising: a self-position estimator for estimating the self-position of an autonomous mobile body using a self-position estimation technique; A self-position detection unit that has a sensor provided on the body and detects the self-position of the autonomous mobile body, an estimated position value of the autonomous mobile body obtained by the self-position estimator, and an autonomy obtained by the self-position detection unit An outlier remover that compares the position detection value of the mobile body and extracts and removes an outlier from the estimated position value of the autonomous mobile body, and the position of the autonomous mobile body from which the outlier is removed by the outlier remover A final self-position estimator for finally estimating the self-position of the autonomous mobile body based on the estimated value.

このような位置推定装置においては、自己位置推定器により得られた自律移動体の位置推定値と自己位置検出部により得られた自律移動体の位置検出値との比較によって、自律移動体の位置推定値の外れ値が抽出されて除去される。そして、その外れ値が除去された自律移動体の位置推定値に基づいて、自律移動体の自己位置が最終的に推定される。このように自律移動体の自己位置を最終的に推定するときには、自律移動体の位置推定値の外れ値が使用されないため、自律移動体の自己位置が大きくずれることが防止される。これにより、自律移動体の自己位置の推定精度が向上する。 In such a position estimation device, the position estimation value of the autonomous mobile body obtained by the self-position estimator and the position detection value of the autonomous mobile body obtained by the self-position detection unit are compared to determine the position of the autonomous mobile body. Outliers in the estimates are extracted and removed. Then, the self-position of the autonomous mobile body is finally estimated based on the estimated position value of the autonomous mobile body from which the outlier is removed. When the self-position of the autonomous mobile body is finally estimated in this way, the outliers of the position estimation values of the autonomous mobile body are not used, so that the self-position of the autonomous mobile body is prevented from deviating greatly. This improves the accuracy of estimating the self-position of the autonomous mobile body.

外れ値除去部は、自己位置検出部により今回得られた自律移動体の位置検出値と自己位置検出部により前回得られた自律移動体の位置検出値との相対位置と、自己位置推定器により今回得られた自律移動体の位置推定値と自己位置推定器により前回得られた自律移動体の位置推定値との相対位置との差分に関する計算式を計算し、計算式の計算値が閾値よりも大きいときに、今回得られた自律移動体の位置推定値を外れ値として除去してもよい。このような構成では、自律移動体の位置推定値の外れ値の抽出精度が高くなるため、自律移動体の自己位置の推定精度が更に向上する。 The outlier remover removes the relative position between the position detection value of the autonomous mobile body obtained this time by the self-position detection part and the position detection value of the autonomous mobile body obtained last time by the self-position detection part, and the self-position estimator Calculate the difference between the position estimation value of the autonomous mobile body obtained this time and the position estimation value of the autonomous mobile body obtained last time by the self-position estimator and the relative position, and is large, the position estimation value of the autonomous mobile body obtained this time may be removed as an outlier. With such a configuration, the accuracy of extracting an outlier in the position estimation value of the autonomous mobile body is increased, so the accuracy of estimating the self-position of the autonomous mobile body is further improved.

閾値は、自律移動体の周囲環境に応じて変更されてもよい。このような構成では、自律移動体の周囲環境に応じた最適な閾値が設定されるため、自律移動体の位置推定値の外れ値の抽出精度が更に高くなる。これにより、自律移動体の自己位置の推定精度が一層向上する。 The threshold may be changed according to the surrounding environment of the autonomous mobile body. In such a configuration, since an optimum threshold value is set according to the surrounding environment of the autonomous mobile body, the extraction accuracy of outliers in the position estimation value of the autonomous mobile body is further increased. This further improves the estimation accuracy of the self-position of the autonomous mobile body.

自己位置推定器は、自律移動体の自己位置として自律移動体の二次元位置及び向きを推定し、自己位置検出部は、自律移動体の自己位置として自律移動体の二次元位置及び向きを検出してもよい。このような構成では、自律移動体の二次元位置だけでなく自律移動体の向きも考慮されるため、自律移動体の位置推定値の外れ値の抽出精度が一層高くなる。これにより、自律移動体の自己位置の推定精度がより一層向上する。 The self-position estimator estimates the two-dimensional position and orientation of the autonomous mobile body as the self-position of the autonomous mobile body, and the self-position detector detects the two-dimensional position and orientation of the autonomous mobile body as the self-position of the autonomous mobile body. You may In such a configuration, not only the two-dimensional position of the autonomous mobile body but also the orientation of the autonomous mobile body is taken into consideration, so that the outlier extraction accuracy of the position estimation value of the autonomous mobile body is further enhanced. This further improves the estimation accuracy of the self-position of the autonomous mobile body.

計算式は、自律移動体の二次元位置及び向きに関する変数を加算する式であってもよい。このような構成では、自律移動体の二次元位置及び向きの要素を含んだ１つの計算式を用いるので、自律移動体の二次元位置及び向きを総合的に判断して、自律移動体の位置推定値の外れ値を抽出することが可能となる。また、閾値の数が１つだけで済む。 The calculation formula may be a formula for adding variables related to the two-dimensional position and orientation of the autonomous mobile body. In such a configuration, since one calculation formula including elements of the two-dimensional position and orientation of the autonomous mobile body is used, the two-dimensional position and orientation of the autonomous mobile body are comprehensively judged, and the position of the autonomous mobile body It becomes possible to extract the outliers of the estimated values. Also, only one threshold is required.

本発明によれば、自律移動体の自己位置の推定精度を向上させることができる。 ADVANTAGE OF THE INVENTION According to this invention, the estimation precision of the self-position of an autonomous mobile body can be improved.

本発明の一実施形態に係る自律移動体の位置推定装置を示す概略構成図である。1 is a schematic configuration diagram showing a position estimation device for an autonomous mobile body according to an embodiment of the present invention; FIG. 図１に示された自己位置算出部により実行される自己位置算出処理手順の詳細を示すフローチャートである。2 is a flowchart showing details of a self-position calculation processing procedure executed by a self-position calculation unit shown in FIG. 1; 図１に示された外れ値除去部により実行される外れ値除去処理手順の詳細を示すフローチャートである。2 is a flowchart showing details of an outlier removal processing procedure executed by an outlier removal unit shown in FIG. 1; 図１に示された最終自己位置推定部により実行される自己位置推定処理手順の詳細を示すフローチャートである。2 is a flowchart showing details of a self-position estimation processing procedure executed by a final self-position estimation unit shown in FIG. 1; 図３に示された外れ値除去処理手順の変形例を示すフローチャートである。FIG. 4 is a flowchart showing a modification of the outlier removal processing procedure shown in FIG. 3; FIG.

以下、本発明の実施形態について図面を参照して詳細に説明する。なお、図面において、同一または同等の要素には同じ符号を付し、重複する説明を省略する。 BEST MODE FOR CARRYING OUT THE INVENTION Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings. In the drawings, the same or equivalent elements are denoted by the same reference numerals, and overlapping descriptions are omitted.

図１は、本発明の一実施形態に係る自律移動体の位置推定装置を示す概略構成図である。図１において、本実施形態の位置推定装置１は、自律移動体であるフォークリフト２の自動運転を実施する際に、フォークリフト２の位置を推定する装置である。位置推定装置１は、フォークリフト２に搭載されている。 FIG. 1 is a schematic configuration diagram showing a position estimation device for an autonomous mobile body according to one embodiment of the present invention. In FIG. 1, a position estimation device 1 of this embodiment is a device that estimates the position of a forklift 2, which is an autonomous mobile body, when the forklift 2 is automatically driven. A position estimation device 1 is mounted on a forklift 2 .

位置推定装置１は、第１自己位置推定器３と、第２自己位置推定器４と、ホイールエンコーダ５と、ジャイロセンサ６と、コントローラ７と、表示器８とを備えている。 The position estimation device 1 includes a first self-position estimator 3 , a second self-position estimator 4 , a wheel encoder 5 , a gyro sensor 6 , a controller 7 and a display 8 .

第１自己位置推定器３は、ＳＬＡＭ（Simultaneous Localization andMapping）を用いて、フォークリフト２の自己位置を推定する。ＳＬＡＭは、センサデータ及び地図データを使って自己位置推定を行う自己位置推定技術である。ＳＬＡＭは、センサとしてレーザレンジスキャナー等を利用して、自己位置推定と環境地図の作成とを同時に行う。ＳＬＡＭでは、建物の有無によって自己位置推定結果にずれが生じる。従って、ＳＬＡＭは、屋内での推定精度が屋外での推定精度よりも高い自己位置推定技術である。第１自己位置推定器３は、フォークリフト２の自己位置を絶対位置として推定する。 The first self-position estimator 3 estimates the self-position of the forklift 2 using SLAM (Simultaneous Localization and Mapping). SLAM is a self-localization technique that performs self-localization using sensor data and map data. SLAM uses a laser range scanner or the like as a sensor to simultaneously estimate its own position and create an environment map. In SLAM, the self-position estimation result varies depending on the presence or absence of buildings. Therefore, SLAM is a self-localization technique whose indoor estimation accuracy is higher than outdoor estimation accuracy. The first self-position estimator 3 estimates the self-position of the forklift 2 as an absolute position.

第２自己位置推定器４は、ＧＮＳＳ（Global NavigationSatellite System）を用いて、フォークリフト２の自己位置を推定する。ＧＮＳＳは、衛星を使って自己位置推定を行う自己位置推定技術である。ＧＮＳＳでは、衛星の状態によって自己位置推定結果にずれが生じる。従って、ＧＮＳＳは、屋外での推定精度が屋内での推定精度よりも高い自己位置推定技術である。第２自己位置推定器４は、フォークリフト２の自己位置を絶対位置として推定する。 The second self-position estimator 4 estimates the self-position of the forklift 2 using GNSS (Global Navigation Satellite System). GNSS is a self-localization technology that uses satellites to self-locate. In GNSS, a deviation occurs in the self-position estimation result depending on the state of the satellite. Therefore, GNSS is a self-localization technique whose outdoor estimation accuracy is higher than indoor estimation accuracy. A second self-position estimator 4 estimates the self-position of the forklift 2 as an absolute position.

第１自己位置推定器３及び第２自己位置推定器４は、フォークリフト２の自己位置としてフォークリフト２の二次元位置及び向きを推定する。ここでは、第１自己位置推定器３及び第２自己位置推定器４により推定された自己位置を位置推定値と称する。第１自己位置推定器３及び第２自己位置推定器４による自己位置推定は、単位時間（例えば数ｍＳ）毎に行われる。 A first self-position estimator 3 and a second self-position estimator 4 estimate the two-dimensional position and orientation of the forklift 2 as the self-position of the forklift 2 . Here, the self-positions estimated by the first self-position estimator 3 and the second self-position estimator 4 are referred to as position estimation values. Self-position estimation by the first self-position estimator 3 and the second self-position estimator 4 is performed every unit time (for example, several milliseconds).

ホイールエンコーダ５は、フォークリフト２のホイールに取り付けられ、フォークリフト２の速度を計測するセンサである。ジャイロセンサ６は、フォークリフト２の車体等に取り付けられ、フォークリフト２の角速度を計測するセンサである。 The wheel encoder 5 is a sensor that is attached to the wheel of the forklift 2 and measures the speed of the forklift 2 . The gyro sensor 6 is a sensor that is attached to the vehicle body of the forklift 2 or the like and measures the angular velocity of the forklift 2 .

コントローラ７は、ＣＰＵ、ＲＡＭ、ＲＯＭ及び入出力インターフェース等により構成されている。コントローラ７は、第１自己位置推定器３及び第２自己位置推定器４の自己位置推定結果とホイールエンコーダ５及びジャイロセンサ６の計測値とを入力し、所定の処理を行い、フォークリフト２の自己位置を推定する。コントローラ７は、自己位置算出部１０と、外れ値除去部１１と、最終自己位置推定部１２とを有している。 The controller 7 is composed of a CPU, a RAM, a ROM, an input/output interface, and the like. The controller 7 inputs the self-position estimation results of the first self-position estimator 3 and the second self-position estimator 4 and the measured values of the wheel encoder 5 and the gyro sensor 6, performs predetermined processing, Estimate location. The controller 7 has a self-position calculator 10 , an outlier remover 11 , and a final self-position estimator 12 .

自己位置算出部１０は、ホイールエンコーダ５及びジャイロセンサ６の計測値に基づいて、フォークリフト２の自己位置を算出する。ホイールエンコーダ５、ジャイロセンサ６及び自己位置算出部１０は、フォークリフト２に具備されたセンサを有し、フォークリフト２の自己位置を検出する自己位置検出部１３を構成する。自己位置検出部１３は、フォークリフト２の自己位置としてフォークリフト２の二次元位置及び向きを検出する。ここでは、自己位置検出部１３により検出された自己位置を位置検出値と称する。 The self-position calculator 10 calculates the self-position of the forklift 2 based on the measured values of the wheel encoder 5 and the gyro sensor 6 . The wheel encoder 5 , the gyro sensor 6 , and the self-position calculator 10 have sensors provided in the forklift 2 , and constitute a self-position detector 13 that detects the self-position of the forklift 2 . The self-position detector 13 detects the two-dimensional position and orientation of the forklift 2 as the self-position of the forklift 2 . Here, the self-position detected by the self-position detector 13 is called a position detection value.

外れ値除去部１１は、第１自己位置推定器３及び第２自己位置推定器４により得られたフォークリフト２の位置推定値と自己位置検出部１３により得られたフォークリフト２の位置検出値とを比較して、フォークリフト２の位置推定値の外れ値を抽出して除去する。 The outlier remover 11 removes the position estimation value of the forklift 2 obtained by the first self-position estimator 3 and the second self-position estimator 4 and the position detection value of the forklift 2 obtained by the self-position detector 13. By comparison, outliers in the forklift 2 position estimate are extracted and removed.

最終自己位置推定部１２は、第１自己位置推定器３及び第２自己位置推定器４によりそれぞれ得られた２つのフォークリフト２の位置推定値に基づいて、フォークリフト２の自己位置を最終的に推定する。このとき、最終自己位置推定部１２は、外れ値除去部１１により外れ値が除去されたフォークリフト２の位置推定値に基づいて、フォークリフト２の自己位置を最終的に推定する。また、最終自己位置推定部１２は、フォークリフト２の周囲環境に基づいて第１自己位置推定器３及び第２自己位置推定器４の重み係数を設定し、その重み係数を用いてフォークリフト２の自己位置を最終的に推定する。 The final self-position estimator 12 finally estimates the self-position of the forklift 2 based on the two position estimation values of the forklift 2 respectively obtained by the first self-position estimator 3 and the second self-position estimator 4. do. At this time, the final self-position estimator 12 finally estimates the self-position of the forklift 2 based on the position estimation value of the forklift 2 from which the outliers have been removed by the outlier remover 11 . Further, the final self-position estimating unit 12 sets the weighting factors of the first self-position estimator 3 and the second self-position estimator 4 based on the surrounding environment of the forklift 2, and uses the weighting factors to determine the self-position of the forklift 2. Estimate the final position.

表示器８は、最終自己位置推定部１２によって最終的に推定されたフォークリフト２の自己位置を表示する。表示器８は、例えばフォークリフト２の二次元位置及び向きを画面表示する。 The display 8 displays the self-position of the forklift 2 finally estimated by the final self-position estimator 12 . The display 8 displays, for example, the two-dimensional position and orientation of the forklift 2 on the screen.

図２は、自己位置算出部１０により実行される自己位置算出処理手順の詳細を示すフローチャートである。図２において、自己位置算出部１０は、まずホイールエンコーダ５及びジャイロセンサ６の計測値を取得する（手順Ｓ１０１）。 FIG. 2 is a flowchart showing the details of the self-position calculation processing procedure executed by the self-position calculator 10. As shown in FIG. In FIG. 2, the self-position calculator 10 first acquires the measured values of the wheel encoder 5 and the gyro sensor 6 (step S101).

続いて、自己位置算出部１０は、ホイールエンコーダ５及びジャイロセンサ６の計測値に基づいて、フォークリフト２の位置検出値を算出する（手順Ｓ１０２）。具体的には、自己位置算出部１０は、ホイールエンコーダ５により計測されたフォークリフト２の速度を積分することにより、フォークリフト２の二次元位置（ＸＹ座標の位置）を求める。また、自己位置算出部１０は、ジャイロセンサ６により計測されたフォークリフト２の角速度を積分することにより、フォークリフト２の向きを求める。続いて、自己位置算出部１０は、フォークリフト２の位置検出値を外れ値除去部１１に出力する（手順Ｓ１０３）。 Subsequently, the self-position calculator 10 calculates the position detection value of the forklift 2 based on the measured values of the wheel encoder 5 and the gyro sensor 6 (step S102). Specifically, the self-position calculator 10 obtains the two-dimensional position (XY coordinate position) of the forklift 2 by integrating the speed of the forklift 2 measured by the wheel encoder 5 . The self-position calculator 10 also obtains the orientation of the forklift 2 by integrating the angular velocity of the forklift 2 measured by the gyro sensor 6 . Subsequently, the self-position calculator 10 outputs the detected position value of the forklift 2 to the outlier remover 11 (step S103).

図３は、外れ値除去部１１により実行される外れ値除去処理手順の詳細を示すフローチャートである。図３において、外れ値除去部１１は、まず第１自己位置推定器３及び第２自己位置推定器４により得られたフォークリフト２の位置推定値を取得する（手順Ｓ１１１）。 FIG. 3 is a flowchart showing the details of the outlier removal processing procedure executed by the outlier removal unit 11. As shown in FIG. In FIG. 3, the outlier remover 11 first acquires the position estimation value of the forklift 2 obtained by the first self-position estimator 3 and the second self-position estimator 4 (step S111).

続いて、外れ値除去部１１は、第１自己位置推定器３及び第２自己位置推定器４により今回得られたフォークリフト２の位置推定値と第１自己位置推定器３及び第２自己位置推定器４により前回得られたフォークリフト２の位置推定値との相対位置（ｘ，ｙ，θ）を算出する（手順Ｓ１１２）。前回得られたフォークリフト２の位置推定値は、単位時間前に推定されたフォークリフト２の自己位置であり、メモリに記憶されている。 Subsequently, the outlier removing unit 11 removes the position estimation value of the forklift 2 obtained this time by the first self-position estimator 3 and the second self-position estimator 4, the first self-position estimator 3 and the second self-position estimation The relative position (x, y, .theta.) of the forklift 2 obtained last time is calculated by the device 4 (step S112). The previously obtained position estimation value of the forklift 2 is the self-position of the forklift 2 estimated a unit time ago, and is stored in the memory.

相対位置（ｘ，ｙ，θ）は、例えば第１自己位置推定器３により得られた位置推定値と第２自己位置推定器４により得られた位置推定値との平均値として算出してもよいし、或いは第１自己位置推定器３により得られた位置推定値及び第２自己位置推定器４により得られた位置推定値のうち前回算出された値に近いほうを採用してもよい。 The relative position (x, y, θ) may be calculated as an average value of the position estimation value obtained by the first self-position estimator 3 and the position estimation value obtained by the second self-position estimator 4, for example. Alternatively, of the position estimation value obtained by the first self-position estimator 3 and the position estimation value obtained by the second self-position estimator 4, whichever is closer to the value calculated last time may be adopted.

続いて、外れ値除去部１１は、自己位置算出部１０により得られたフォークリフト２の位置検出値を取得する（手順Ｓ１１３）。続いて、外れ値除去部１１は、自己位置算出部１０により今回得られたフォークリフト２の位置検出値と自己位置算出部１０により前回得られたフォークリフト２の位置検出値との相対位置（ｘｄ，ｙｄ，θｄ）を算出する（手順Ｓ１１４）。前回得られたフォークリフト２の位置検出値は、単位時間前に検出されたフォークリフト２の自己位置であり、メモリに記憶されている。 Subsequently, the outlier remover 11 acquires the position detection value of the forklift 2 obtained by the self-position calculator 10 (step S113). Subsequently, the outlier removing unit 11 determines the relative position (x d , y d , θ d ) are calculated (step S114). The previously obtained position detection value of the forklift 2 is the self-position of the forklift 2 detected a unit time ago, and is stored in the memory.

続いて、外れ値除去部１１は、フォークリフト２の周囲環境に基づいて、閾値δｘ，δｙ，δθを設定する（手順Ｓ１１５）。閾値δｘ，δｙは、フォークリフト２の二次元位置（ｘ軸位置，ｙ軸位置）に係る閾値である。閾値δθは、フォークリフト２の向きに係る閾値である。フォークリフト２の周囲環境は、最終自己位置推定部１２により前回推定されたフォークリフト２の自己位置の環境である。なお、フォークリフト２の周囲環境は、環境地図を作成するＳＬＡＭを用いる第１自己位置推定器３によって把握することができる。 Subsequently, the outlier removing unit 11 sets thresholds δx, δy, δθ based on the surrounding environment of the forklift 2 (step S115). The thresholds δx and δy are thresholds related to the two-dimensional position (x-axis position, y-axis position) of the forklift 2 . The threshold δθ is a threshold relating to the orientation of the forklift 2 . The surrounding environment of the forklift 2 is the environment of the self-position of the forklift 2 previously estimated by the final self-position estimation unit 12 . The surrounding environment of the forklift 2 can be grasped by the first self-localization estimator 3 using SLAM for creating an environment map.

例えば、フォークリフト２が雨等で滑りやすい屋外を走行する場合、ホイールエンコーダ５は滑りの影響が大きいため、ホイールエンコーダ５の計測値に誤差が出やすいが、ジャイロセンサ６は滑りの影響が少ないため、ジャイロセンサ６の計測値に誤差が出にくい。従って、屋外では、屋内に比べて閾値δｘ，δｙを大きく（緩く）設定する。例えば、屋外では、閾値δｘ，δｙを１ｍに設定し、屋内では、閾値δｘ，δｙを０．５ｍに設定する。閾値δθについては、屋外と屋内とで等しくする。例えば、屋外及び屋内において、閾値δθを０．３ｒａｄに設定する。このように閾値δｘ，δｙ，δθは、フォークリフト２の周囲環境に応じて変更される。 For example, when the forklift 2 travels outdoors where it is slippery due to rain or the like, the wheel encoder 5 is greatly affected by slippage, and errors tend to occur in the measurement values of the wheel encoder 5. However, the gyro sensor 6 is less affected by slippage. , the measurement value of the gyro sensor 6 is less likely to have an error. Therefore, outdoors, the threshold values δx and δy are set larger (loosely) than indoors. For example, the thresholds δx and δy are set to 1 m outdoors, and the thresholds δx and δy are set to 0.5 m indoors. The threshold Δθ is made equal between outdoors and indoors. For example, the threshold value δθ is set to 0.3 rad outdoors and indoors. The thresholds δx, δy, δθ are thus changed according to the surrounding environment of the forklift 2 .

また、フォークリフト２が例えば冷蔵庫の内外のような温度差が大きい屋内及び屋外を走行する場合には、ジャイロセンサ６の計測値に誤差が出やすい。このため、屋内では、屋外に比べて閾値δθを大きく（緩く）設定する。 Further, when the forklift 2 travels indoors and outdoors, such as inside and outside a refrigerator, where there is a large temperature difference, errors in the measurement values of the gyro sensor 6 are likely to occur. Therefore, indoors, the threshold Δθ is set larger (loosely) than outdoors.

続いて、外れ値除去部１１は、手順Ｓ１１４で算出された相対位置（ｘｄ，ｙｄ，θｄ）と手順Ｓ１１２で算出された（ｘ，ｙ，θ）との差分に関する計算式ｆ（ｘ），ｆ（ｙ），ｆ（θ）を計算する（手順Ｓ１１６）。計算式ｆ（ｘ），ｆ（ｙ），ｆ（θ）は、下記式で表される。

Figure 0007155686000001
Subsequently , the outlier removing unit 11 uses a calculation formula f( x), f(y), and f(θ) are calculated (step S116). Calculation formulas f(x), f(y), and f(θ) are represented by the following formulas.
Figure 0007155686000001

続いて、外れ値除去部１１は、上記式にも示されるように、計算式ｆ（ｘ），ｆ（ｙ），ｆ（θ）の計算値がそれぞれ閾値δｘ，δｙ，δθよりも大きいかどうかを判断する（手順Ｓ１１７）。 Subsequently, the outlier removal unit 11 determines whether the calculated values of the calculation formulas f(x), f(y), and f(θ) are larger than the thresholds δx, δy, and δθ, respectively, as shown in the above formulas. It is determined whether or not (step S117).

外れ値除去部１１は、計算式ｆ（ｘ），ｆ（ｙ），ｆ（θ）の計算値が閾値δｘ，δｙ，δθよりも大きいと判断したときは、第１自己位置推定器３及び第２自己位置推定器４により今回得られたフォークリフト２の位置推定値を外れ値として除去する（手順Ｓ１１８）。このとき、外れ値除去部１１は、計算式ｆ（ｘ），ｆ（ｙ），ｆ（θ）の計算値の少なくとも１つが閾値δｘ，δｙ，δθよりも大きいとき、つまり上記式の条件を１つでも満たすときは、今回得られたフォークリフト２の位置推定値を外れ値とみなす。 When the outlier removing unit 11 determines that the calculated values of the calculation formulas f(x), f(y), and f(θ) are larger than the thresholds δx, δy, δθ, the first self-position estimator 3 and The position estimation value of the forklift 2 obtained this time by the second self-position estimator 4 is removed as an outlier (step S118). At this time, when at least one of the calculated values of the calculation formulas f(x), f(y), and f(θ) is larger than the threshold values δx, δy, and δθ, the outlier removing unit 11 satisfies the conditions of the above formulas. If even one condition is satisfied, the position estimation value of the forklift 2 obtained this time is regarded as an outlier.

外れ値除去部１１は、計算式ｆ（ｘ），ｆ（ｙ），ｆ（θ）の計算値の全てが閾値δｘ，δｙ，δθよりも大きくない、つまり上記式の条件を全て満たさないと判断したときは、第１自己位置推定器３及び第２自己位置推定器４により今回得られたフォークリフト２の位置推定値を最終自己位置推定部１２に出力する（手順Ｓ１１９）。 The outlier removing unit 11 requires that all the calculated values of the formulas f(x), f(y), and f(θ) are not greater than the thresholds δx, δy, and δθ, that is, satisfy all the conditions of the above formulas. When determined, the position estimation value of the forklift 2 obtained this time by the first self-position estimator 3 and the second self-position estimator 4 is output to the final self-position estimator 12 (step S119).

図４は、最終自己位置推定部１２により実行される自己位置推定処理手順の詳細を示すフローチャートである。ここでは、最終自己位置推定部１２は、第１自己位置推定器３及び第２自己位置推定器４によりそれぞれ得られた２つのフォークリフト２の位置推定値を融合し、線形最小分散推定法を利用してフォークリフト２の自己位置を最終的に推定する。 FIG. 4 is a flowchart showing the details of the self-position estimation processing procedure executed by the final self-position estimation unit 12. As shown in FIG. Here, the final self-position estimator 12 fuses two position estimation values of the forklift 2 respectively obtained by the first self-position estimator 3 and the second self-position estimator 4, and uses a linear minimum variance estimation method. Then, the self-position of the forklift 2 is finally estimated.

図４において、最終自己位置推定部１２は、まず第１自己位置推定器３により得られた最新のフォークリフト２の位置推定値（ｘｓ，ｙｓ，θｓ）と、第２自己位置推定器４により得られた最新のフォークリフト２の位置推定値（ｘｇ，ｙｇ，θｇ）とを取得する（手順Ｓ１２１）。 In FIG. 4 , the final self-position estimating unit 12 first calculates the latest position estimation value (xs, ys, θs) of the forklift 2 obtained by the first self-position estimator 3 and the The latest estimated position value (xg, yg, θg) of the forklift 2 is obtained (step S121).

続いて、最終自己位置推定部１２は、フォークリフト２の周囲環境に基づいて、第１自己位置推定器３の誤差分散（σｓｘ２，σｓｙ２，σｓθ２）と、第２自己位置推定器４の誤差分散（σｇｘ２，σｇｙ２，σｇθ２）とを設定する（手順Ｓ１２２）。フォークリフト２の周囲環境は、最終自己位置推定部１２により前回推定されたフォークリフト２の自己位置の環境である。また、誤差分散とは、重み係数のことである。誤差分散σｓｘ２，σｓｙ２及び誤差分散σｇｘ２，σｇｙ２は、フォークリフト２の二次元位置（ｘ軸位置，ｙ軸位置）に係る重み係数である。誤差分散σｓθ２及び誤差分散σｇθ２は、フォークリフト２の向きに係る重み係数である。 Subsequently, the final self-position estimator 12 calculates the error variance (σsx 2 , σsy 2 , σsθ 2 ) of the first self-position estimator 3 and the Error variances (σgx 2 , σgy 2 , σgθ 2 ) are set (step S122). The surrounding environment of the forklift 2 is the environment of the self-position of the forklift 2 previously estimated by the final self-position estimation unit 12 . Also, the error variance is a weighting factor. The error variances σsx 2 , σsy 2 and the error variances σgx 2 , σgy 2 are weighting factors related to the two-dimensional position (x-axis position, y-axis position) of the forklift 2 . The error variance σs θ 2 and the error variance σg θ 2 are weighting factors related to the orientation of the forklift 2 .

このとき、屋内では、ＳＬＡＭの推定精度がＧＮＳＳの推定精度よりも高いため、第１自己位置推定器３の誤差分散を第２自己位置推定器４の誤差分散よりも低く設定する。例えば、第１自己位置推定器３の誤差分散を４とし、第２自己位置推定器４の誤差分散を３６とする。屋外では、ＧＮＳＳの推定精度がＳＬＡＭの推定精度よりも高いため、第２自己位置推定器４の誤差分散を第１自己位置推定器３の誤差分散よりも低く設定する。例えば、第１自己位置推定器３の誤差分散を３６とし、第２自己位置推定器４の誤差分散を４とする。また、屋内と屋内との中間では、第１自己位置推定器３の誤差分散と第２自己位置推定器４の誤差分散とを等しく設定する。例えば、第１自己位置推定器３及び第２自己位置推定器４の誤差分散を何れも２５とする。なお、屋内と屋内との中間とは、例えば屋根はないが建物の近くで柱が存在しているような場所である。 At this time, since the estimation accuracy of SLAM is higher indoors than the estimation accuracy of GNSS, the error variance of first self-position estimator 3 is set lower than the error variance of second self-position estimator 4 . For example, assume that the error variance of the first self-position estimator 3 is 4, and the error variance of the second self-position estimator 4 is 36. Outdoors, the GNSS estimation accuracy is higher than the SLAM estimation accuracy, so the error variance of the second self-position estimator 4 is set lower than the error variance of the first self-position estimator 3 . For example, let the error variance of the first self-position estimator 3 be 36 and the error variance of the second self-position estimator 4 be 4. Between indoors and indoors, the error variance of the first self-position estimator 3 and the error variance of the second self-position estimator 4 are set equal. For example, assume that the error variances of the first self-position estimator 3 and the second self-position estimator 4 are both 25. Note that the middle between indoors is, for example, a place where there is no roof but there are pillars near the building.

続いて、最終自己位置推定部１２は、第１自己位置推定器３により得られた最新のフォークリフト２の位置推定値（ｘｓ，ｙｓ，θｓ）、第２自己位置推定器４により得られた最新のフォークリフト２の位置推定値（ｘｇ，ｙｇ，θｇ）、第１自己位置推定器３の誤差分散（σｓｘ２，σｓｙ２，σｓθ２）及び第２自己位置推定器４の誤差分散（σｇｘ２，σｇｙ２，σｇθ２）を用いて、下記式により最終的なフォークリフト２の自己位置（ｘｅｓｔ，ｙｅｓｔ，θｅｓｔ）を推定する。

Figure 0007155686000002
Subsequently, the final self-position estimator 12 obtains the latest position estimation values (xs, ys, θs) of the forklift 2 obtained by the first self-position estimator 3, the latest of the forklift 2 (xg, yg, θg), the error variance of the first self-position estimator 3 (σsx 2 , σsy 2 , σsθ 2 ) and the error variance of the second self-position estimator 4 (σgx 2 , σgy 2 , σgθ 2 ), the final self-position (x est , y est , θ est ) of the forklift truck 2 is estimated by the following equation.
Figure 0007155686000002

以上のように本実施形態にあっては、第１自己位置推定器３及び第２自己位置推定器４により得られたフォークリフト２の位置推定値と自己位置検出部１３により得られたフォークリフト２の位置検出値との比較によって、フォークリフト２の位置推定値の外れ値が抽出されて除去される。そして、その外れ値が除去されたフォークリフト２の位置推定値に基づいて、フォークリフト２の自己位置が最終的に推定される。このようにフォークリフト２の自己位置を最終的に推定するときには、フォークリフト２の位置推定値の外れ値が使用されないため、フォークリフト２の自己位置が大きくずれることが防止される。これにより、フォークリフト２の自己位置の推定精度が向上する。その結果、フォークリフト２の自動運転時に、フォークリフト２の安定した走行が可能となる。 As described above, in this embodiment, the position estimation value of the forklift 2 obtained by the first self-position estimator 3 and the second self-position estimator 4 and the position of the forklift 2 obtained by the self-position detector 13 Outliers in the position estimate of the forklift 2 are extracted and removed by comparison with the position detection values. Then, the self-position of the forklift 2 is finally estimated based on the position estimation value of the forklift 2 from which the outlier is removed. When the self-position of the forklift 2 is finally estimated in this way, the outliers of the position estimation values of the forklift 2 are not used, so that the self-position of the forklift 2 is prevented from deviating significantly. This improves the accuracy of estimating the self-position of the forklift 2 . As a result, the forklift 2 can travel stably when the forklift 2 is automatically operated.

また、本実施形態では、自己位置検出部１３により今回得られたフォークリフト２の位置検出値と自己位置検出部１３により前回得られたフォークリフト２の位置検出値との相対位置（ｘｄ，ｙｄ，θｄ）と、第１自己位置推定器３及び第２自己位置推定器４により今回得られたフォークリフト２の位置推定値と第１自己位置推定器３及び第２自己位置推定器４により前回得られたフォークリフト２の位置推定値との相対位置（ｘ，ｙ，θ）との差分に関する計算式ｆ（ｘ），ｆ（ｙ），ｆ（θ）が計算され、計算式ｆ（ｘ），ｆ（ｙ），ｆ（θ）の計算値が閾値δｘ，δｙ，δθよりも大きいときに、今回得られたフォークリフト２の位置推定値が外れ値として除去される。従って、フォークリフト２の位置推定値の外れ値の抽出精度が高くなるため、フォークリフト２の自己位置の推定精度が更に向上する。 In the present embodiment, the relative position (x d , y d , θ d ), the position estimation value of the forklift 2 obtained this time by the first self-position estimator 3 and the second self-position estimator 4, and the previous position estimation value obtained by the first self-position estimator 3 and the second self-position estimator 4 Calculation formulas f(x), f(y), f(θ) relating to the difference between the obtained estimated position value of the forklift 2 and the relative position (x, y, θ) are calculated, and the calculation formula f(x) , f(y) and f(θ) are larger than the threshold values δx, δy and δθ, the currently obtained position estimation value of the forklift 2 is removed as an outlier. Therefore, the accuracy of extracting an outlier in the position estimation value of the forklift 2 is improved, and the accuracy of estimating the self-position of the forklift 2 is further improved.

また、本実施形態では、閾値δｘ，δｙ，δθは、フォークリフト２の周囲環境に応じて変更される。従って、フォークリフト２の周囲環境に応じた最適な閾値δｘ，δｙ，δθが設定されるため、フォークリフト２の位置推定値の外れ値の抽出精度が更に高くなる。これにより、フォークリフト２の自己位置の推定精度が一層向上する。 Further, in this embodiment, the thresholds δx, δy, δθ are changed according to the surrounding environment of the forklift 2 . Therefore, since the optimum thresholds δx, δy, δθ are set according to the surrounding environment of the forklift 2, the accuracy of extracting outliers in the estimated position of the forklift 2 is further improved. This further improves the estimation accuracy of the self-position of the forklift 2 .

また、本実施形態では、第１自己位置推定器３及び第２自己位置推定器４は、フォークリフト２の自己位置としてフォークリフト２の二次元位置及び向きを推定し、自己位置検出部１３は、フォークリフト２の自己位置としてフォークリフト２の二次元位置及び向きを検出する。このようにフォークリフト２の二次元位置だけでなくフォークリフト２の向きも考慮されるため、フォークリフト２の位置推定値の外れ値の抽出精度が一層高くなる。これにより、フォークリフト２の自己位置の推定精度がより一層向上する。 Further, in the present embodiment, the first self-position estimator 3 and the second self-position estimator 4 estimate the two-dimensional position and orientation of the forklift 2 as the self-position of the forklift 2, and the self-position detector 13 detects the forklift 2 detects the two-dimensional position and orientation of the forklift 2 as its self-position. Since not only the two-dimensional position of the forklift 2 but also the orientation of the forklift 2 is taken into consideration in this way, the accuracy of extracting outliers in the estimated position of the forklift 2 is further enhanced. This further improves the accuracy of estimating the self-position of the forklift 2 .

図５は、図３に示された外れ値除去処理手順の変形例を示すフローチャートである。図５において、外れ値除去部１１は、図３に示されるフローチャートと同様に、手順Ｓ１１１～Ｓ１１４を実行する。 FIG. 5 is a flow chart showing a modification of the outlier removal processing procedure shown in FIG. In FIG. 5, the outlier remover 11 executes steps S111 to S114 in the same manner as in the flowchart shown in FIG.

外れ値除去部１１は、手順Ｓ１１４を実行した後、フォークリフト２の周囲環境に基づいて、閾値δ及び重み係数α，β，γを設定する（手順Ｓ１１５Ａ）。重み係数αは、フォークリフト２のｘ軸位置に係る重み係数である。重み係数βは、フォークリフト２のｙ軸位置に係る重み係数である。重み係数γは、フォークリフト２の向きに係る重み係数である。このとき、例えば雨等で滑りやすい屋外では、屋内に比べて重み係数α，βを小さく設定する。 After executing step S114, the outlier remover 11 sets the threshold δ and the weighting factors α, β, γ based on the surrounding environment of the forklift 2 (step S115A). A weighting factor α is a weighting factor related to the x-axis position of the forklift 2 . A weighting factor β is a weighting factor related to the y-axis position of the forklift 2 . A weighting factor γ is a weighting factor related to the orientation of the forklift 2 . At this time, the weighting coefficients α and β are set smaller than indoors, for example, outdoors where it is slippery due to rain or the like.

続いて、外れ値除去部１１は、手順Ｓ１１４で算出された相対位置（ｘｄ，ｙｄ，θｄ）と手順Ｓ１１２で算出された（ｘ，ｙ，θ）との差分に関する計算式ｆを計算する（手順Ｓ１１６Ａ）。計算式ｆは、下記式で表される。計算式ｆは、フォークリフト２の二次元位置及び向きに関する変数を加算する式である。

Figure 0007155686000003
Subsequently, the outlier removing unit 11 calculates a calculation formula f regarding the difference between the relative position (x d , y d , θ d ) calculated in step S114 and (x, y, θ) calculated in step S112. Calculate (step S116A). The calculation formula f is represented by the following formula. The calculation formula f is a formula for adding variables related to the two-dimensional position and orientation of the forklift 2 .
Figure 0007155686000003

続いて、外れ値除去部１１は、上記式にも示されるように、計算式ｆの計算値が閾値δよりも大きいかどうかを判断する（手順Ｓ１１７Ａ）。外れ値除去部１１は、計算式ｆの計算値が閾値δよりも大きいと判断したときは、第１自己位置推定器３及び第２自己位置推定器４により今回得られたフォークリフト２の位置推定値を外れ値として除去する（手順Ｓ１１８）。 Subsequently, the outlier removal unit 11 determines whether the calculated value of the calculation formula f is greater than the threshold value δ, as also shown in the above formula (step S117A). When the outlier removal unit 11 determines that the calculated value of the calculation formula f is larger than the threshold value δ, the position estimation of the forklift 2 obtained this time by the first self-position estimator 3 and the second self-position estimator 4 The value is removed as an outlier (step S118).

外れ値除去部１１は、計算式ｆの計算値が閾値δよりも大きくないと判断したときは、第１自己位置推定器３及び第２自己位置推定器４により今回得られたフォークリフト２の位置推定値を最終自己位置推定部１２に出力する（手順Ｓ１１９）。 When the outlier removal unit 11 determines that the calculated value of the calculation formula f is not larger than the threshold value δ, the position of the forklift 2 currently obtained by the first self-position estimator 3 and the second self-position estimator 4 is The estimated value is output to the final self-position estimation unit 12 (step S119).

本変形例においては、フォークリフト２の二次元位置及び向きの要素を含んだ１つの計算式ｆを用いるので、フォークリフト２の二次元位置及び向きを総合的に判断して、フォークリフト２の位置推定値の外れ値を抽出することが可能となる。また、閾値の数が１つだけで済む。 In this modification, since one calculation formula f including elements of the two-dimensional position and orientation of the forklift 2 is used, the two-dimensional position and orientation of the forklift 2 are comprehensively judged, and the estimated position of the forklift 2 is outliers can be extracted. Also, only one threshold is required.

なお、本発明は、上記実施形態には限定されない。例えば上記実施形態では、ＳＬＡＭを用いてフォークリフト２の自己位置を推定する第１自己位置推定器３と、ＧＮＳＳを用いてフォークリフト２の自己位置を推定する第２自己位置推定器４とを使用しているが、特にその形態には限られず、第１自己位置推定器３及び第２自己位置推定器４の何れか一方のみを使用してもよい。この場合には、最終自己位置推定部１２は、第１自己位置推定器３及び第２自己位置推定器４によりそれぞれ得られた２つのフォークリフト２の位置推定値を融合しなくて済む。また、使用する自己位置推定技術としては、特にＳＬＡＭまたはＧＮＳＳには限られない。 In addition, this invention is not limited to the said embodiment. For example, in the above embodiment, the first self-position estimator 3 that estimates the self-position of the forklift 2 using SLAM and the second self-position estimator 4 that estimates the self-position of the forklift 2 using GNSS are used. However, the form is not particularly limited, and either one of the first self-position estimator 3 and the second self-position estimator 4 may be used. In this case, the final self-position estimating unit 12 does not have to fuse the two position estimation values of the forklift 2 respectively obtained by the first self-position estimator 3 and the second self-position estimator 4 . Also, the self-localization technique used is not particularly limited to SLAM or GNSS.

また、上記実施形態では、ホイールエンコーダ５によりフォークリフト２の速度を計測し、ジャイロセンサ６によりフォークリフト２の角速度を計測しているが、使用するセンサとしては、特にそれには限られない。例えば、フォークリフト２の速度を計測するセンサとしては、ロータリーエンコーダ等を使用してもよく、フォークリフト２の角速度を計測するセンサとしては、操舵角センサまたは操舵ポテンショメータ等を使用してもよい。また、フォークリフト２の二次元位置及び向きをセンサにより直接検出してもよい。 In the above embodiment, the wheel encoder 5 measures the speed of the forklift 2 and the gyro sensor 6 measures the angular velocity of the forklift 2, but the sensors used are not particularly limited to them. For example, a sensor for measuring the speed of the forklift 2 may be a rotary encoder or the like, and a sensor for measuring the angular velocity of the forklift 2 may be a steering angle sensor or a steering potentiometer. Also, the two-dimensional position and orientation of the forklift 2 may be directly detected by a sensor.

また、上記実施形態では、最終自己位置推定部１２は、第１自己位置推定器３及び第２自己位置推定器４によりそれぞれ得られた２つのフォークリフト２の位置推定値を融合し、線形最小分散推定法を利用してフォークリフト２の自己位置を最終的に推定しているが、２つのフォークリフト２の位置推定値を融合する場合には、例えばカルマンフィルタを利用してフォークリフト２の自己位置を最終的に推定してもよい。 Further, in the above embodiment, the final self-position estimator 12 fuses the two position estimation values of the forklift 2 respectively obtained by the first self-position estimator 3 and the second self-position estimator 4, and obtains the linear minimum variance Although the self-position of the forklift 2 is finally estimated using the estimation method, when the position estimation values of the two forklifts 2 are fused, the self-position of the forklift 2 is finally estimated using, for example, a Kalman filter. can be estimated to

さらに、上記実施形態では、フォークリフト２の位置を推定しているが、本発明は、フォークリフト以外の産業車両あるいは移動ロボット等といった自律移動体にも適用可能である。 Furthermore, in the above embodiment, the position of the forklift 2 is estimated, but the present invention can also be applied to autonomous moving bodies such as industrial vehicles and mobile robots other than forklifts.

１…位置推定装置、２…フォークリフト（自律移動体）、３…第１自己位置推定器（自己位置推定器）、４…第２自己位置推定器（自己位置推定器）、５…ホイールエンコーダ（センサ）、６…ジャイロセンサ（センサ）、１１…外れ値除去部、１２…最終自己位置推定部、１３…自己位置検出部。 1... Position estimation device, 2... Forklift (autonomous moving body), 3... First self-position estimator (self-position estimator), 4... Second self-position estimator (self-position estimator), 5... Wheel encoder ( sensor), 6... Gyroscope (sensor), 11... Outlier remover, 12... Final self-position estimator, 13... Self-position detector.

Claims (4)

自律移動体の位置を推定する自律移動体の位置推定装置であって、
自己位置推定技術を用いて前記自律移動体の自己位置を推定する第１自己位置推定器と、
前記第１自己位置推定器とは異なる自己位置推定技術を用いて前記自律移動体の自己位置を推定する第２自己位置推定器と、
前記自律移動体に具備されたセンサにより計測された前記自律移動体の速度及び角速度に基づいて、前記自律移動体の自己位置を検出する自己位置検出部と、
前記第１自己位置推定器及び前記第２自己位置推定器により得られた前記自律移動体の位置推定値と前記自己位置検出部により得られた前記自律移動体の位置検出値とを比較して、前記第１自己位置推定器及び前記第２自己位置推定器により得られた前記自律移動体の位置推定値の外れ値を抽出して除去する外れ値除去部と、
前記外れ値除去部により前記外れ値が除去された前記自律移動体の位置推定値に基づいて、線形最小分散推定法またはカルマンフィルタを利用して、前記自律移動体の自己位置を最終的に推定する最終自己位置推定部とを備え、
前記外れ値除去部は、前記自己位置検出部により今回得られた前記自律移動体の位置検出値と前記自己位置検出部により前回得られた前記自律移動体の位置検出値との相対位置と、前記第１自己位置推定器及び前記第２自己位置推定器により今回得られた前記自律移動体の位置推定値と前記第１自己位置推定器及び前記第２自己位置推定器により前回得られた前記自律移動体の位置推定値との相対位置との差分に関する計算式を計算し、前記計算式の計算値が閾値よりも大きいときに、前記今回得られた自律移動体の位置推定値を前記外れ値として除去し、
前記今回得られた前記自律移動体の位置推定値と前記前回得られた前記自律移動体の位置推定値との相対位置は、前記第１自己位置推定器により得られた前記自律移動体の位置推定値と前記第２自己位置推定器により得られた前記自律移動体の位置推定値との平均値、或いは前記第１自己位置推定器により得られた前記自律移動体の位置推定値及び前記第２自己位置推定器により得られた前記自律移動体の位置推定値のうち前回推定された前記自律移動体の自己位置に近いほうの値を用いて算出され、
前記最終自己位置推定部は、前記自律移動体の周囲環境に基づいて前記第１自己位置推定器及び前記第２自己位置推定器の誤差分散を設定し、前記第１自己位置推定器により得られた前記自律移動体の位置推定値と前記第２自己位置推定器により得られた前記自律移動体の位置推定値と前記第１自己位置推定器及び前記第２自己位置推定器の誤差分散とに基づいて、前記自律移動体の自己位置を最終的に推定する自律移動体の位置推定装置。 An autonomous mobile body position estimation device for estimating the position of an autonomous mobile body,
A first self-position estimator that estimates the self-position of the autonomous mobile body using a self-position estimation technique;
a second self-position estimator that estimates the self-position of the autonomous mobile body using a self-position estimation technique different from that of the first self-position estimator;
A self-position detection unit that detects the self-position of the autonomous mobile body based on the speed and angular velocity of the autonomous mobile body measured by a sensor provided on the autonomous mobile body;
Comparing the position estimation value of the autonomous mobile body obtained by the first self-position estimator and the second self-position estimator with the position detection value of the autonomous mobile body obtained by the self-position detection unit , an outlier removal unit that extracts and removes outliers of the position estimation values of the autonomous mobile body obtained by the first self-position estimator and the second self-position estimator ;
Based on the position estimation value of the autonomous mobile body from which the outlier is removed by the outlier removing unit, the self-position of the autonomous mobile body is finally estimated using a linear minimum variance estimation method or a Kalman filter . and a final self-position estimation unit ,
The outlier removing unit includes a relative position between the position detection value of the autonomous mobile body obtained this time by the self-position detection unit and the position detection value of the autonomous mobile body obtained last time by the self-position detection unit; The position estimation value of the autonomous mobile body obtained this time by the first self-position estimator and the second self-position estimator and the position estimation value obtained last time by the first self-position estimator and the second self-position estimator Calculating a formula for the difference between the position estimated value of the autonomous mobile body and the relative position, and when the calculated value of the formula is larger than a threshold, the position estimated value of the autonomous mobile body obtained this time is out of the way remove as a value,
The relative position between the position estimation value of the autonomous mobile body obtained this time and the position estimation value of the autonomous mobile body obtained last time is the position of the autonomous mobile body obtained by the first self-position estimator. An estimated value and an average value of the position estimation value of the autonomous mobile body obtained by the second self-position estimator, or the position estimation value of the autonomous mobile body obtained by the first self-position estimator and the first 2 Calculated using the value closer to the self-position of the autonomous mobile body estimated last time out of the estimated position values of the autonomous mobile body obtained by the self-position estimator,
The final self-position estimator sets error variances of the first self-position estimator and the second self-position estimator based on the surrounding environment of the autonomous mobile body, and obtains by the first self-position estimator position estimation value of the autonomous mobile body obtained by the second self-position estimator and the error variance of the first self-position estimator and the second self-position estimator A position estimating device for an autonomous mobile body that finally estimates the self-position of the autonomous mobile body based on the above. 前記閾値は、前記自律移動体の周囲環境に応じて変更される請求項１記載の自律移動体の位置推定装置。 The position estimation device for an autonomous mobile body according to claim 1 , wherein the threshold is changed according to the surrounding environment of the autonomous mobile body. 前記第１自己位置推定器及び前記第２自己位置推定器は、前記自律移動体の自己位置として前記自律移動体の二次元位置及び向きを推定し、
前記自己位置検出部は、前記自律移動体の自己位置として前記自律移動体の二次元位置及び向きを検出する請求項１または２記載の自律移動体の位置推定装置。 The first self-position estimator and the second self-position estimator estimate the two-dimensional position and orientation of the autonomous mobile body as the self-position of the autonomous mobile body,
The position estimation device for an autonomous mobile body according to claim 1 or 2 , wherein the self-position detection unit detects a two-dimensional position and orientation of the autonomous mobile body as the self-position of the autonomous mobile body. 前記計算式は、前記自律移動体の二次元位置及び向きに関する変数を加算する式である請求項３記載の自律移動体の位置推定装置。 4. The position estimation device for an autonomous mobile body according to claim 3 , wherein said calculation formula is a formula for adding variables relating to the two-dimensional position and orientation of said autonomous mobile body.
JP2018130898A 2018-07-10 2018-07-10 Autonomous mobile body position estimation device Active JP7155686B2 (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
JP2018130898A JP7155686B2 (en) 2018-07-10 2018-07-10 Autonomous mobile body position estimation device

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP2018130898A JP7155686B2 (en) 2018-07-10 2018-07-10 Autonomous mobile body position estimation device

Publications (2)

Publication Number Publication Date
JP2020009252A JP2020009252A (en) 2020-01-16
JP7155686B2 true JP7155686B2 (en) 2022-10-19

Family

ID=69151865

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2018130898A Active JP7155686B2 (en) 2018-07-10 2018-07-10 Autonomous mobile body position estimation device

Country Status (1)

Country Link
JP (1) JP7155686B2 (en)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP7322799B2 (en) * 2020-05-01 2023-08-08 株式会社豊田自動織機 Self-localization device

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2011210121A (en) 2010-03-30 2011-10-20 Fuji Soft Inc Program for robot and program for information-processing device
JP2017107425A (en) 2015-12-10 2017-06-15 カシオ計算機株式会社 Autonomous mobile device, autonomous moving method and program
WO2018020588A1 (en) 2016-07-26 2018-02-01 日産自動車株式会社 Self-position estimation method and self-position estimation device

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2011210121A (en) 2010-03-30 2011-10-20 Fuji Soft Inc Program for robot and program for information-processing device
JP2017107425A (en) 2015-12-10 2017-06-15 カシオ計算機株式会社 Autonomous mobile device, autonomous moving method and program
WO2018020588A1 (en) 2016-07-26 2018-02-01 日産自動車株式会社 Self-position estimation method and self-position estimation device

Also Published As

Publication number Publication date
JP2020009252A (en) 2020-01-16

Similar Documents

Publication Publication Date Title
EP2495079B1 (en) Slip detection apparatus and method for a mobile robot
US8452535B2 (en) Systems and methods for precise sub-lane vehicle positioning
Jo et al. GPS-bias correction for precise localization of autonomous vehicles
KR101738750B1 (en) Method and apparatus for robust localization in outdoor environments
KR101439921B1 (en) Slam system for mobile robot based on vision sensor data and motion sensor data fusion
US9122278B2 (en) Vehicle navigation
CN110554376A (en) Radar range finding method for vehicles
KR20170118040A (en) Method and device for the estimation of car egomotion from surround view images
KR20180030597A (en) A control method of a travel control device and a travel control device
JP7284268B2 (en) Methods for Identifying Integrity Ranges
JP2010224755A (en) Moving object and position estimating method of the same
Hoang et al. Multi-sensor perceptual system for mobile robot and sensor fusion-based localization
Kang et al. On-road vehicle localization with GPS under long term failure of a vision sensor
JP6539129B2 (en) Vehicle position estimation device, steering control device using the same, and vehicle position estimation method
EP2527943A1 (en) Vehicle navigation
JP7155686B2 (en) Autonomous mobile body position estimation device
CN110637209A (en) Method, apparatus, and computer-readable storage medium having instructions for estimating a pose of a motor vehicle
Li et al. Visual-inertial odometry with online calibration of velocity-control based kinematic motion models
Kimura et al. Vehicle localization by sensor fusion of LRS measurement and odometry information based on moving horizon estimation
JP2020008461A (en) Autonomous moving body location estimation device
CN110095793A (en) A kind of automatic Pilot low speed sweeper localization method adaptive based on tire radius
Seyr et al. Proprioceptive navigation, slip estimation and slip control for autonomous wheeled mobile robots
CN112697153A (en) Positioning method of autonomous mobile device, electronic device and storage medium
JP7040308B2 (en) Travel control device and travel control method for automatic guided vehicles
Zhang et al. Cumulative error estimation from noisy relative measurements

Legal Events

Date Code Title Description
A621 Written request for application examination

Free format text: JAPANESE INTERMEDIATE CODE: A621

Effective date: 20201020

A131 Notification of reasons for refusal

Free format text: JAPANESE INTERMEDIATE CODE: A131

Effective date: 20210629

A977 Report on retrieval

Free format text: JAPANESE INTERMEDIATE CODE: A971007

Effective date: 20210630

A601 Written request for extension of time

Free format text: JAPANESE INTERMEDIATE CODE: A601

Effective date: 20210824

A521 Request for written amendment filed

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20211026

A131 Notification of reasons for refusal

Free format text: JAPANESE INTERMEDIATE CODE: A131

Effective date: 20220301

A521 Request for written amendment filed

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20220428

TRDD Decision of grant or rejection written
A01 Written decision to grant a patent or to grant a registration (utility model)

Free format text: JAPANESE INTERMEDIATE CODE: A01

Effective date: 20220906

A61 First payment of annual fees (during grant procedure)

Free format text: JAPANESE INTERMEDIATE CODE: A61

Effective date: 20220919

R151 Written notification of patent or utility model registration

Ref document number: 7155686

Country of ref document: JP

Free format text: JAPANESE INTERMEDIATE CODE: R151