JP2020009252A - Autonomous moving body location estimation device - Google Patents
Autonomous moving body location estimation device Download PDFInfo
- Publication number
- JP2020009252A JP2020009252A JP2018130898A JP2018130898A JP2020009252A JP 2020009252 A JP2020009252 A JP 2020009252A JP 2018130898 A JP2018130898 A JP 2018130898A JP 2018130898 A JP2018130898 A JP 2018130898A JP 2020009252 A JP2020009252 A JP 2020009252A
- Authority
- JP
- Japan
- Prior art keywords
- self
- autonomous mobile
- forklift
- value
- estimator
- 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.)
- Granted
Links
- 238000001514 detection method Methods 0.000 claims abstract description 25
- 239000000284 extract Substances 0.000 claims abstract description 3
- 238000000034 method Methods 0.000 claims description 16
- 230000014509 gene expression Effects 0.000 claims description 4
- 238000005516 engineering process Methods 0.000 abstract description 3
- 230000008030 elimination Effects 0.000 abstract 2
- 238000003379 elimination reaction Methods 0.000 abstract 2
- 238000012545 processing Methods 0.000 description 7
- 238000005259 measurement Methods 0.000 description 6
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000010276 construction Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
Images
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
Description
本発明は、自律移動体の位置推定装置に関する。 The present invention relates to a position estimation device for an autonomous mobile object.
自律移動体の位置推定装置としては、例えば特許文献1に記載されているように、地図の構築及び使用を行う同時位置推定地図構築(SLAM)を用いて、自己位置の推定を行う技術が知られている。 As a position estimating apparatus for an autonomous mobile object, for example, as described in Patent Literature 1, a technique for estimating a self-position using a simultaneous position estimation map construction (SLAM) for constructing and using a map is known. Have been.
しかしながら、上記従来技術においては、自律移動体の周囲環境によっては自己位置推定結果にずれが生じることがあるため、自律移動体の自己位置を高精度に推定することが困難である。 However, in the above-described related art, the self-position estimation result may be shifted depending on the surrounding environment of the autonomous mobile body, and thus it is difficult to estimate the self-position of the autonomous mobile body with high accuracy.
本発明の目的は、自律移動体の自己位置の推定精度を向上させることができる自律移動体の位置推定装置を提供することである。 An object of the present invention is to provide an autonomous moving object position estimating device that can improve the accuracy of estimating the position of the autonomous moving object.
本発明の一態様は、自律移動体の位置を推定する自律移動体の位置推定装置であって、自己位置推定技術を用いて自律移動体の自己位置を推定する自己位置推定器と、自律移動体に具備されたセンサを有し、自律移動体の自己位置を検出する自己位置検出部と、自己位置推定器により得られた自律移動体の位置推定値と自己位置検出部により得られた自律移動体の位置検出値とを比較して、自律移動体の位置推定値の外れ値を抽出して除去する外れ値除去部と、外れ値除去部により外れ値が除去された自律移動体の位置推定値に基づいて、自律移動体の自己位置を最終的に推定する最終自己位置推定部とを備える。 One aspect of the present invention is an autonomous mobile position estimating device for estimating the position of an autonomous mobile object, comprising: a self-position estimator for estimating the self-position of the autonomous mobile object using a self-position estimation technique; A self-position detecting unit that has a sensor provided on the body and detects a self-position of the autonomous moving object, a position estimation value of the autonomous moving object obtained by the self-position estimator, and an autonomous position obtained by the self-position detecting unit An outlier removing unit that extracts and removes outliers of the position estimation value of the autonomous moving object by comparing the detected position value of the moving object with the position of the autonomous moving object from which the outliers have been removed by the outlier removing unit A final self-position estimating unit for finally estimating the self-position of the autonomous mobile body based on the estimated value.
このような位置推定装置においては、自己位置推定器により得られた自律移動体の位置推定値と自己位置検出部により得られた自律移動体の位置検出値との比較によって、自律移動体の位置推定値の外れ値が抽出されて除去される。そして、その外れ値が除去された自律移動体の位置推定値に基づいて、自律移動体の自己位置が最終的に推定される。このように自律移動体の自己位置を最終的に推定するときには、自律移動体の位置推定値の外れ値が使用されないため、自律移動体の自己位置が大きくずれることが防止される。これにより、自律移動体の自己位置の推定精度が向上する。 In such a position estimating device, the position of the autonomous moving body is compared by comparing the position estimated value of the autonomous moving body obtained by the self position estimator with the position detected value of the autonomous moving body obtained by the self position detecting unit. Outliers of the estimated value are extracted and removed. Then, the self-position of the autonomous moving object is finally estimated based on the position estimation value of the autonomous moving object from which the outlier has been removed. As described above, when the self-position of the autonomous mobile body is finally estimated, an outlier of the estimated position value of the autonomous mobile body is not used, so that the self-position of the autonomous mobile body is prevented from being largely shifted. Thereby, the estimation accuracy of the self-position of the autonomous mobile body is improved.
外れ値除去部は、自己位置検出部により今回得られた自律移動体の位置検出値と自己位置検出部により前回得られた自律移動体の位置検出値との相対位置と、自己位置推定器により今回得られた自律移動体の位置推定値と自己位置推定器により前回得られた自律移動体の位置推定値との相対位置との差分に関する計算式を計算し、計算式の計算値が閾値よりも大きいときに、今回得られた自律移動体の位置推定値を外れ値として除去してもよい。このような構成では、自律移動体の位置推定値の外れ値の抽出精度が高くなるため、自律移動体の自己位置の推定精度が更に向上する。 The outlier removing unit calculates the 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. Calculate the calculation formula for the difference between the position estimation value of the autonomous moving body obtained this time and the relative position of the position estimation value of the autonomous moving body obtained last time by the self-position estimator. May also be removed as an outlier. In such a configuration, the accuracy of extracting an outlier of the estimated position value of the autonomous mobile body is increased, and thus the accuracy of estimation of 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. In such a configuration, since the optimal threshold value is set according to the surrounding environment of the autonomous mobile body, the accuracy of extracting an outlier of the estimated position value of the autonomous mobile body is further improved. Thereby, the estimation accuracy of the self-position of the autonomous mobile body is further improved.
自己位置推定器は、自律移動体の自己位置として自律移動体の二次元位置及び向きを推定し、自己位置検出部は、自律移動体の自己位置として自律移動体の二次元位置及び向きを検出してもよい。このような構成では、自律移動体の二次元位置だけでなく自律移動体の向きも考慮されるため、自律移動体の位置推定値の外れ値の抽出精度が一層高くなる。これにより、自律移動体の自己位置の推定精度がより一層向上する。 The self-position estimator estimates the two-dimensional position and orientation of the autonomous mobile as the self-position of the autonomous mobile, and the self-position detector detects the two-dimensional position and orientation of the autonomous mobile as the self-position of the autonomous mobile. May be. In such a configuration, since not only the two-dimensional position of the autonomous mobile object but also the orientation of the autonomous mobile object is considered, the accuracy of extracting an outlier of the estimated position value of the autonomous mobile object is further improved. Thereby, the estimation accuracy of the self-position of the autonomous mobile body is further improved.
計算式は、自律移動体の二次元位置及び向きに関する変数を加算する式であってもよい。このような構成では、自律移動体の二次元位置及び向きの要素を含んだ1つの計算式を用いるので、自律移動体の二次元位置及び向きを総合的に判断して、自律移動体の位置推定値の外れ値を抽出することが可能となる。また、閾値の数が1つだけで済む。 The calculation formula may be a formula that adds variables related to the two-dimensional position and orientation of the autonomous mobile object. In such a configuration, since one calculation formula including elements of the two-dimensional position and orientation of the autonomous mobile is used, the two-dimensional position and orientation of the autonomous mobile are comprehensively determined, and the position of the autonomous mobile is determined. Outliers of the estimated value can be extracted. Also, only one threshold is required.
本発明によれば、自律移動体の自己位置の推定精度を向上させることができる。 ADVANTAGE OF THE INVENTION According to this invention, the estimation accuracy of the self-position of an autonomous mobile body can be improved.
以下、本発明の実施形態について図面を参照して詳細に説明する。なお、図面において、同一または同等の要素には同じ符号を付し、重複する説明を省略する。 Hereinafter, embodiments of the present invention will be described in detail with reference to the drawings. In the drawings, the same or equivalent elements have the same reference characters allotted, and overlapping description will be omitted.
図1は、本発明の一実施形態に係る自律移動体の位置推定装置を示す概略構成図である。図1において、本実施形態の位置推定装置1は、自律移動体であるフォークリフト2の自動運転を実施する際に、フォークリフト2の位置を推定する装置である。位置推定装置1は、フォークリフト2に搭載されている。 FIG. 1 is a schematic configuration diagram illustrating an apparatus for estimating the position of an autonomous moving object according to an embodiment of the present invention. In FIG. 1, a position estimating device 1 of the present embodiment is a device for estimating the position of a forklift 2 when performing automatic operation of a forklift 2 which is an autonomous moving body. The position estimating device 1 is mounted on a forklift 2.
位置推定装置1は、第1自己位置推定器3と、第2自己位置推定器4と、ホイールエンコーダ5と、ジャイロセンサ6と、コントローラ7と、表示器8とを備えている。
The position estimation device 1 includes a first self-position estimator 3, a second self-position estimator 4, a
第1自己位置推定器3は、SLAM(Simultaneous Localization andMapping)を用いて、フォークリフト2の自己位置を推定する。SLAMは、センサデータ及び地図データを使って自己位置推定を行う自己位置推定技術である。SLAMは、センサとしてレーザレンジスキャナー等を利用して、自己位置推定と環境地図の作成とを同時に行う。SLAMでは、建物の有無によって自己位置推定結果にずれが生じる。従って、SLAMは、屋内での推定精度が屋外での推定精度よりも高い自己位置推定技術である。第1自己位置推定器3は、フォークリフト2の自己位置を絶対位置として推定する。 The first self-position estimator 3 estimates the self-position of the forklift 2 using Simultaneous Localization and Mapping (SLAM). SLAM is a self-position estimation technique for performing self-position estimation using sensor data and map data. The SLAM uses a laser range scanner or the like as a sensor to simultaneously perform self-position estimation and create an environmental map. In SLAM, the result of self-location estimation varies depending on the presence or absence of a building. Therefore, SLAM is a self-position estimation technique in which the estimation accuracy indoors is higher than the estimation accuracy outdoors. The first self-position estimator 3 estimates the self-position of the forklift 2 as an absolute position.
第2自己位置推定器4は、GNSS(Global NavigationSatellite System)を用いて、フォークリフト2の自己位置を推定する。GNSSは、衛星を使って自己位置推定を行う自己位置推定技術である。GNSSでは、衛星の状態によって自己位置推定結果にずれが生じる。従って、GNSSは、屋外での推定精度が屋内での推定精度よりも高い自己位置推定技術である。第2自己位置推定器4は、フォークリフト2の自己位置を絶対位置として推定する。 The second self-position estimator 4 estimates the self-position of the forklift 2 using a GNSS (Global Navigation Satellite System). GNSS is a self-position estimation technology that performs self-position estimation using satellites. In the GNSS, the self-position estimation result varies depending on the state of the satellite. Therefore, GNSS is a self-position estimation technology in which the estimation accuracy outdoors is higher than the estimation accuracy indoors. The second self-position estimator 4 estimates the self-position of the forklift 2 as an absolute position.
第1自己位置推定器3及び第2自己位置推定器4は、フォークリフト2の自己位置としてフォークリフト2の二次元位置及び向きを推定する。ここでは、第1自己位置推定器3及び第2自己位置推定器4により推定された自己位置を位置推定値と称する。第1自己位置推定器3及び第2自己位置推定器4による自己位置推定は、単位時間(例えば数mS)毎に行われる。 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. Here, the self-position estimated by the first self-position estimator 3 and the second self-position estimator 4 is referred to as a position estimation value. The 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).
ホイールエンコーダ5は、フォークリフト2のホイールに取り付けられ、フォークリフト2の速度を計測するセンサである。ジャイロセンサ6は、フォークリフト2の車体等に取り付けられ、フォークリフト2の角速度を計測するセンサである。
The
コントローラ7は、CPU、RAM、ROM及び入出力インターフェース等により構成されている。コントローラ7は、第1自己位置推定器3及び第2自己位置推定器4の自己位置推定結果とホイールエンコーダ5及びジャイロセンサ6の計測値とを入力し、所定の処理を行い、フォークリフト2の自己位置を推定する。コントローラ7は、自己位置算出部10と、外れ値除去部11と、最終自己位置推定部12とを有している。
The
自己位置算出部10は、ホイールエンコーダ5及びジャイロセンサ6の計測値に基づいて、フォークリフト2の自己位置を算出する。ホイールエンコーダ5、ジャイロセンサ6及び自己位置算出部10は、フォークリフト2に具備されたセンサを有し、フォークリフト2の自己位置を検出する自己位置検出部13を構成する。自己位置検出部13は、フォークリフト2の自己位置としてフォークリフト2の二次元位置及び向きを検出する。ここでは、自己位置検出部13により検出された自己位置を位置検出値と称する。
The self-
外れ値除去部11は、第1自己位置推定器3及び第2自己位置推定器4により得られたフォークリフト2の位置推定値と自己位置検出部13により得られたフォークリフト2の位置検出値とを比較して、フォークリフト2の位置推定値の外れ値を抽出して除去する。
The
最終自己位置推定部12は、第1自己位置推定器3及び第2自己位置推定器4によりそれぞれ得られた2つのフォークリフト2の位置推定値に基づいて、フォークリフト2の自己位置を最終的に推定する。このとき、最終自己位置推定部12は、外れ値除去部11により外れ値が除去されたフォークリフト2の位置推定値に基づいて、フォークリフト2の自己位置を最終的に推定する。また、最終自己位置推定部12は、フォークリフト2の周囲環境に基づいて第1自己位置推定器3及び第2自己位置推定器4の重み係数を設定し、その重み係数を用いてフォークリフト2の自己位置を最終的に推定する。
The final self-
表示器8は、最終自己位置推定部12によって最終的に推定されたフォークリフト2の自己位置を表示する。表示器8は、例えばフォークリフト2の二次元位置及び向きを画面表示する。
The
図2は、自己位置算出部10により実行される自己位置算出処理手順の詳細を示すフローチャートである。図2において、自己位置算出部10は、まずホイールエンコーダ5及びジャイロセンサ6の計測値を取得する(手順S101)。
FIG. 2 is a flowchart illustrating details of the self-position calculation processing procedure executed by the self-
続いて、自己位置算出部10は、ホイールエンコーダ5及びジャイロセンサ6の計測値に基づいて、フォークリフト2の位置検出値を算出する(手順S102)。具体的には、自己位置算出部10は、ホイールエンコーダ5により計測されたフォークリフト2の速度を積分することにより、フォークリフト2の二次元位置(XY座標の位置)を求める。また、自己位置算出部10は、ジャイロセンサ6により計測されたフォークリフト2の角速度を積分することにより、フォークリフト2の向きを求める。続いて、自己位置算出部10は、フォークリフト2の位置検出値を外れ値除去部11に出力する(手順S103)。
Next, the self-
図3は、外れ値除去部11により実行される外れ値除去処理手順の詳細を示すフローチャートである。図3において、外れ値除去部11は、まず第1自己位置推定器3及び第2自己位置推定器4により得られたフォークリフト2の位置推定値を取得する(手順S111)。
FIG. 3 is a flowchart illustrating details of the outlier removal processing procedure executed by the
続いて、外れ値除去部11は、第1自己位置推定器3及び第2自己位置推定器4により今回得られたフォークリフト2の位置推定値と第1自己位置推定器3及び第2自己位置推定器4により前回得られたフォークリフト2の位置推定値との相対位置(x,y,θ)を算出する(手順S112)。前回得られたフォークリフト2の位置推定値は、単位時間前に推定されたフォークリフト2の自己位置であり、メモリに記憶されている。
Subsequently, the
相対位置(x,y,θ)は、例えば第1自己位置推定器3により得られた位置推定値と第2自己位置推定器4により得られた位置推定値との平均値として算出してもよいし、或いは第1自己位置推定器3により得られた位置推定値及び第2自己位置推定器4により得られた位置推定値のうち前回算出された値に近いほうを採用してもよい。 The relative position (x, y, θ) may be calculated as, for example, 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. Alternatively, one 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 which is closer to the previously calculated value may be adopted.
続いて、外れ値除去部11は、自己位置算出部10により得られたフォークリフト2の位置検出値を取得する(手順S113)。続いて、外れ値除去部11は、自己位置算出部10により今回得られたフォークリフト2の位置検出値と自己位置算出部10により前回得られたフォークリフト2の位置検出値との相対位置(xd,yd,θd)を算出する(手順S114)。前回得られたフォークリフト2の位置検出値は、単位時間前に検出されたフォークリフト2の自己位置であり、メモリに記憶されている。
Subsequently, the
続いて、外れ値除去部11は、フォークリフト2の周囲環境に基づいて、閾値δx,δy,δθを設定する(手順S115)。閾値δx,δyは、フォークリフト2の二次元位置(x軸位置,y軸位置)に係る閾値である。閾値δθは、フォークリフト2の向きに係る閾値である。フォークリフト2の周囲環境は、最終自己位置推定部12により前回推定されたフォークリフト2の自己位置の環境である。なお、フォークリフト2の周囲環境は、環境地図を作成するSLAMを用いる第1自己位置推定器3によって把握することができる。
Subsequently, the
例えば、フォークリフト2が雨等で滑りやすい屋外を走行する場合、ホイールエンコーダ5は滑りの影響が大きいため、ホイールエンコーダ5の計測値に誤差が出やすいが、ジャイロセンサ6は滑りの影響が少ないため、ジャイロセンサ6の計測値に誤差が出にくい。従って、屋外では、屋内に比べて閾値δx,δyを大きく(緩く)設定する。例えば、屋外では、閾値δx,δyを1mに設定し、屋内では、閾値δx,δyを0.5mに設定する。閾値δθについては、屋外と屋内とで等しくする。例えば、屋外及び屋内において、閾値δθを0.3radに設定する。このように閾値δx,δy,δθは、フォークリフト2の周囲環境に応じて変更される。
For example, when the forklift 2 travels outdoors, which is slippery due to rain or the like, the
また、フォークリフト2が例えば冷蔵庫の内外のような温度差が大きい屋内及び屋外を走行する場合には、ジャイロセンサ6の計測値に誤差が出やすい。このため、屋内では、屋外に比べて閾値δθを大きく(緩く)設定する。
When the forklift 2 travels indoors and outdoors with a large temperature difference, such as inside and outside of a refrigerator, an error tends to occur in the measurement value of the
続いて、外れ値除去部11は、手順S114で算出された相対位置(xd,yd,θd)と手順S112で算出された(x,y,θ)との差分に関する計算式f(x),f(y),f(θ)を計算する(手順S116)。計算式f(x),f(y),f(θ)は、下記式で表される。
続いて、外れ値除去部11は、上記式にも示されるように、計算式f(x),f(y),f(θ)の計算値がそれぞれ閾値δx,δy,δθよりも大きいかどうかを判断する(手順S117)。
Subsequently, the
外れ値除去部11は、計算式f(x),f(y),f(θ)の計算値が閾値δx,δy,δθよりも大きいと判断したときは、第1自己位置推定器3及び第2自己位置推定器4により今回得られたフォークリフト2の位置推定値を外れ値として除去する(手順S118)。このとき、外れ値除去部11は、計算式f(x),f(y),f(θ)の計算値の少なくとも1つが閾値δx,δy,δθよりも大きいとき、つまり上記式の条件を1つでも満たすときは、今回得られたフォークリフト2の位置推定値を外れ値とみなす。
When the
外れ値除去部11は、計算式f(x),f(y),f(θ)の計算値の全てが閾値δx,δy,δθよりも大きくない、つまり上記式の条件を全て満たさないと判断したときは、第1自己位置推定器3及び第2自己位置推定器4により今回得られたフォークリフト2の位置推定値を最終自己位置推定部12に出力する(手順S119)。
The
図4は、最終自己位置推定部12により実行される自己位置推定処理手順の詳細を示すフローチャートである。ここでは、最終自己位置推定部12は、第1自己位置推定器3及び第2自己位置推定器4によりそれぞれ得られた2つのフォークリフト2の位置推定値を融合し、線形最小分散推定法を利用してフォークリフト2の自己位置を最終的に推定する。
FIG. 4 is a flowchart illustrating details of the self-position estimation processing procedure executed by the final self-
図4において、最終自己位置推定部12は、まず第1自己位置推定器3により得られた最新のフォークリフト2の位置推定値(xs,ys,θs)と、第2自己位置推定器4により得られた最新のフォークリフト2の位置推定値(xg,yg,θg)とを取得する(手順S121)。
In FIG. 4, the final self-
続いて、最終自己位置推定部12は、フォークリフト2の周囲環境に基づいて、第1自己位置推定器3の誤差分散(σsx2,σsy2,σsθ2)と、第2自己位置推定器4の誤差分散(σgx2,σgy2,σgθ2)とを設定する(手順S122)。フォークリフト2の周囲環境は、最終自己位置推定部12により前回推定されたフォークリフト2の自己位置の環境である。また、誤差分散とは、重み係数のことである。誤差分散σsx2,σsy2及び誤差分散σgx2,σgy2は、フォークリフト2の二次元位置(x軸位置,y軸位置)に係る重み係数である。誤差分散σsθ2及び誤差分散σgθ2は、フォークリフト2の向きに係る重み係数である。
Subsequently, based on the surrounding environment of the forklift 2, the final self-
このとき、屋内では、SLAMの推定精度がGNSSの推定精度よりも高いため、第1自己位置推定器3の誤差分散を第2自己位置推定器4の誤差分散よりも低く設定する。例えば、第1自己位置推定器3の誤差分散を4とし、第2自己位置推定器4の誤差分散を36とする。屋外では、GNSSの推定精度がSLAMの推定精度よりも高いため、第2自己位置推定器4の誤差分散を第1自己位置推定器3の誤差分散よりも低く設定する。例えば、第1自己位置推定器3の誤差分散を36とし、第2自己位置推定器4の誤差分散を4とする。また、屋内と屋内との中間では、第1自己位置推定器3の誤差分散と第2自己位置推定器4の誤差分散とを等しく設定する。例えば、第1自己位置推定器3及び第2自己位置推定器4の誤差分散を何れも25とする。なお、屋内と屋内との中間とは、例えば屋根はないが建物の近くで柱が存在しているような場所である。 At this time, since the SLAM estimation accuracy is higher than the GNSS estimation accuracy indoors, the error variance of the first self-position estimator 3 is set lower than the error variance of the second self-position estimator 4. For example, the error variance of the first self-position estimator 3 is set to 4, and the error variance of the second self-position estimator 4 is set to 36. Since the GNSS estimation accuracy is higher than the SLAM estimation accuracy outdoors, 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, assume that the error variance of the first self-position estimator 3 is 36, and the error variance of the second self-position estimator 4 is 4. In the middle 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 to be equal. For example, the error variances of the first self-position estimator 3 and the second self-position estimator 4 are both set to 25. In addition, the middle between indoors and indoors is, for example, a place where there is no roof but there is a pillar near a building.
続いて、最終自己位置推定部12は、第1自己位置推定器3により得られた最新のフォークリフト2の位置推定値(xs,ys,θs)、第2自己位置推定器4により得られた最新のフォークリフト2の位置推定値(xg,yg,θg)、第1自己位置推定器3の誤差分散(σsx2,σsy2,σsθ2)及び第2自己位置推定器4の誤差分散(σgx2,σgy2,σgθ2)を用いて、下記式により最終的なフォークリフト2の自己位置(xest,yest,θest)を推定する。
以上のように本実施形態にあっては、第1自己位置推定器3及び第2自己位置推定器4により得られたフォークリフト2の位置推定値と自己位置検出部13により得られたフォークリフト2の位置検出値との比較によって、フォークリフト2の位置推定値の外れ値が抽出されて除去される。そして、その外れ値が除去されたフォークリフト2の位置推定値に基づいて、フォークリフト2の自己位置が最終的に推定される。このようにフォークリフト2の自己位置を最終的に推定するときには、フォークリフト2の位置推定値の外れ値が使用されないため、フォークリフト2の自己位置が大きくずれることが防止される。これにより、フォークリフト2の自己位置の推定精度が向上する。その結果、フォークリフト2の自動運転時に、フォークリフト2の安定した走行が可能となる。
As described above, in the present 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 forklift 2 obtained by the self-
また、本実施形態では、自己位置検出部13により今回得られたフォークリフト2の位置検出値と自己位置検出部13により前回得られたフォークリフト2の位置検出値との相対位置(xd,yd,θd)と、第1自己位置推定器3及び第2自己位置推定器4により今回得られたフォークリフト2の位置推定値と第1自己位置推定器3及び第2自己位置推定器4により前回得られたフォークリフト2の位置推定値との相対位置(x,y,θ)との差分に関する計算式f(x),f(y),f(θ)が計算され、計算式f(x),f(y),f(θ)の計算値が閾値δx,δy,δθよりも大きいときに、今回得られたフォークリフト2の位置推定値が外れ値として除去される。従って、フォークリフト2の位置推定値の外れ値の抽出精度が高くなるため、フォークリフト2の自己位置の推定精度が更に向上する。
In the present embodiment, the relative position (x d , y d ) between the position detection value of the forklift 2 obtained by the self-
また、本実施形態では、閾値δx,δy,δθは、フォークリフト2の周囲環境に応じて変更される。従って、フォークリフト2の周囲環境に応じた最適な閾値δx,δy,δθが設定されるため、フォークリフト2の位置推定値の外れ値の抽出精度が更に高くなる。これにより、フォークリフト2の自己位置の推定精度が一層向上する。 In the present embodiment, the threshold values δx, δy, δθ are changed according to the surrounding environment of the forklift 2. Therefore, since the optimal thresholds δx, δy, δθ are set according to the surrounding environment of the forklift 2, the accuracy of extracting the outlier of the estimated position of the forklift 2 is further improved. As a result, the accuracy of estimating the position of the forklift 2 is further improved.
また、本実施形態では、第1自己位置推定器3及び第2自己位置推定器4は、フォークリフト2の自己位置としてフォークリフト2の二次元位置及び向きを推定し、自己位置検出部13は、フォークリフト2の自己位置としてフォークリフト2の二次元位置及び向きを検出する。このようにフォークリフト2の二次元位置だけでなくフォークリフト2の向きも考慮されるため、フォークリフト2の位置推定値の外れ値の抽出精度が一層高くなる。これにより、フォークリフト2の自己位置の推定精度がより一層向上する。
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-
図5は、図3に示された外れ値除去処理手順の変形例を示すフローチャートである。図5において、外れ値除去部11は、図3に示されるフローチャートと同様に、手順S111〜S114を実行する。
FIG. 5 is a flowchart showing a modification of the outlier removal processing procedure shown in FIG. In FIG. 5, the
外れ値除去部11は、手順S114を実行した後、フォークリフト2の周囲環境に基づいて、閾値δ及び重み係数α,β,γを設定する(手順S115A)。重み係数αは、フォークリフト2のx軸位置に係る重み係数である。重み係数βは、フォークリフト2のy軸位置に係る重み係数である。重み係数γは、フォークリフト2の向きに係る重み係数である。このとき、例えば雨等で滑りやすい屋外では、屋内に比べて重み係数α,βを小さく設定する。
After executing step S114, the
続いて、外れ値除去部11は、手順S114で算出された相対位置(xd,yd,θd)と手順S112で算出された(x,y,θ)との差分に関する計算式fを計算する(手順S116A)。計算式fは、下記式で表される。計算式fは、フォークリフト2の二次元位置及び向きに関する変数を加算する式である。
続いて、外れ値除去部11は、上記式にも示されるように、計算式fの計算値が閾値δよりも大きいかどうかを判断する(手順S117A)。外れ値除去部11は、計算式fの計算値が閾値δよりも大きいと判断したときは、第1自己位置推定器3及び第2自己位置推定器4により今回得られたフォークリフト2の位置推定値を外れ値として除去する(手順S118)。
Subsequently, the
外れ値除去部11は、計算式fの計算値が閾値δよりも大きくないと判断したときは、第1自己位置推定器3及び第2自己位置推定器4により今回得られたフォークリフト2の位置推定値を最終自己位置推定部12に出力する(手順S119)。
When the
本変形例においては、フォークリフト2の二次元位置及び向きの要素を含んだ1つの計算式fを用いるので、フォークリフト2の二次元位置及び向きを総合的に判断して、フォークリフト2の位置推定値の外れ値を抽出することが可能となる。また、閾値の数が1つだけで済む。 In this modified example, since one calculation formula f including the two-dimensional position and direction elements of the forklift 2 is used, the two-dimensional position and direction of the forklift 2 are comprehensively determined, and the position estimation value of the forklift 2 is determined. Outliers can be extracted. Also, only one threshold is required.
なお、本発明は、上記実施形態には限定されない。例えば上記実施形態では、SLAMを用いてフォークリフト2の自己位置を推定する第1自己位置推定器3と、GNSSを用いてフォークリフト2の自己位置を推定する第2自己位置推定器4とを使用しているが、特にその形態には限られず、第1自己位置推定器3及び第2自己位置推定器4の何れか一方のみを使用してもよい。この場合には、最終自己位置推定部12は、第1自己位置推定器3及び第2自己位置推定器4によりそれぞれ得られた2つのフォークリフト2の位置推定値を融合しなくて済む。また、使用する自己位置推定技術としては、特にSLAMまたはGNSSには限られない。
Note that the present invention is not limited to the above embodiment. For example, in the above embodiment, a first self-position estimator 3 for estimating the self-position of the forklift 2 using SLAM and a second self-position estimator 4 for estimating the self-position of the forklift 2 using GNSS are used. However, the present invention is not particularly limited to this mode, and only one of the first self-position estimator 3 and the second self-position estimator 4 may be used. In this case, the final self-
また、上記実施形態では、ホイールエンコーダ5によりフォークリフト2の速度を計測し、ジャイロセンサ6によりフォークリフト2の角速度を計測しているが、使用するセンサとしては、特にそれには限られない。例えば、フォークリフト2の速度を計測するセンサとしては、ロータリーエンコーダ等を使用してもよく、フォークリフト2の角速度を計測するセンサとしては、操舵角センサまたは操舵ポテンショメータ等を使用してもよい。また、フォークリフト2の二次元位置及び向きをセンサにより直接検出してもよい。
Further, in the above embodiment, the speed of the forklift 2 is measured by the
また、上記実施形態では、最終自己位置推定部12は、第1自己位置推定器3及び第2自己位置推定器4によりそれぞれ得られた2つのフォークリフト2の位置推定値を融合し、線形最小分散推定法を利用してフォークリフト2の自己位置を最終的に推定しているが、2つのフォークリフト2の位置推定値を融合する場合には、例えばカルマンフィルタを利用してフォークリフト2の自己位置を最終的に推定してもよい。
Further, in the above embodiment, the final self-
さらに、上記実施形態では、フォークリフト2の位置を推定しているが、本発明は、フォークリフト以外の産業車両あるいは移動ロボット等といった自律移動体にも適用可能である。 Further, in the above embodiment, the position of the forklift 2 is estimated, but the present invention is also applicable to an autonomous mobile body such as an industrial vehicle or a mobile robot other than the forklift.
1…位置推定装置、2…フォークリフト(自律移動体)、3…第1自己位置推定器(自己位置推定器)、4…第2自己位置推定器(自己位置推定器)、5…ホイールエンコーダ(センサ)、6…ジャイロセンサ(センサ)、11…外れ値除去部、12…最終自己位置推定部、13…自己位置検出部。 DESCRIPTION OF SYMBOLS 1 ... Position estimation device, 2 ... Forklift (autonomous mobile body), 3 ... 1st self-position estimator (self-position estimator), 4 ... 2nd self-position estimator (self-position estimator), 5 ... Wheel encoder ( Sensor), 6: gyro sensor (sensor), 11: outlier removal unit, 12: final self-position estimating unit, 13: self-position detecting unit.
Claims (5)
自己位置推定技術を用いて前記自律移動体の自己位置を推定する自己位置推定器と、
前記自律移動体に具備されたセンサを有し、前記自律移動体の自己位置を検出する自己位置検出部と、
前記自己位置推定器により得られた前記自律移動体の位置推定値と前記自己位置検出部により得られた前記自律移動体の位置検出値とを比較して、前記自律移動体の位置推定値の外れ値を抽出して除去する外れ値除去部と、
前記外れ値除去部により前記外れ値が除去された前記自律移動体の位置推定値に基づいて、前記自律移動体の自己位置を最終的に推定する最終自己位置推定部とを備える自律移動体の位置推定装置。 An autonomous mobile position estimating device that estimates the position of the autonomous mobile object,
A self-position estimator for estimating the self-position of the autonomous mobile body using a self-position estimation technique,
A self-position detecting unit that has a sensor provided in the autonomous moving body and detects a self-position of the autonomous moving body,
The position estimation value of the autonomous mobile body obtained by the self-position estimator is compared with the position detection value of the autonomous mobile body obtained by the self-position detection unit, and the position estimation value of the autonomous mobile body is calculated. An outlier removing unit that extracts and removes outliers,
A final self-position estimating unit for finally estimating the self-position of the autonomous mobile based on the position estimated value of the autonomous mobile from which the outlier has been removed by the outlier removing unit. Position estimation device.
前記自己位置検出部は、前記自律移動体の自己位置として前記自律移動体の二次元位置及び向きを検出する請求項2または3記載の自律移動体の位置推定装置。 The self-position estimator estimates the two-dimensional position and orientation of the autonomous mobile as the self-position of the autonomous mobile,
The position estimation device for an autonomous mobile according to claim 2, wherein the self-position detector detects a two-dimensional position and an orientation of the autonomous mobile as a self-position of the autonomous mobile.
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 true JP2020009252A (en) | 2020-01-16 |
JP7155686B2 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) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2021176052A (en) * | 2020-05-01 | 2021-11-04 | 株式会社豊田自動織機 | Self-position estimating device |
Citations (3)
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 |
-
2018
- 2018-07-10 JP JP2018130898A patent/JP7155686B2/en active Active
Patent Citations (3)
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 |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2021176052A (en) * | 2020-05-01 | 2021-11-04 | 株式会社豊田自動織機 | Self-position estimating device |
Also Published As
Publication number | Publication date |
---|---|
JP7155686B2 (en) | 2022-10-19 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US9234758B2 (en) | Machine positioning system utilizing position error checking | |
KR101439921B1 (en) | Slam system for mobile robot based on vision sensor data and motion sensor data fusion | |
ES2706123T3 (en) | Estimation of the positions of a device and of at least one objective in an environment | |
CN110554376A (en) | Radar range finding method for vehicles | |
US20200200537A1 (en) | Dynamic gyroscope bias offset compensation | |
WO2017150106A1 (en) | In-vehicle device and estimation method | |
US9122278B2 (en) | Vehicle navigation | |
JP2011065308A (en) | Autonomous moving system and autonomous mobile device | |
CN107782304B (en) | Mobile robot positioning method and device, mobile robot and storage medium | |
AU2012209016B2 (en) | Inclination detection systems and methods | |
JP2022031266A (en) | Self-position estimation device, control method, program, and storage medium | |
US20170261996A1 (en) | Location and mapping device and method | |
WO2021112074A1 (en) | Information processing device, control method, program, and storage medium | |
Hoang et al. | Multi-sensor perceptual system for mobile robot and sensor fusion-based localization | |
JP5164645B2 (en) | Method and apparatus for repetitive calculation control in Kalman filter processing | |
JP2023164553A (en) | Position estimation device, estimation device, control method, program and storage medium | |
US10976426B2 (en) | Apparatus and method for ascertaining object kinematics of a movable object | |
JP2020008461A (en) | Autonomous moving body location estimation device | |
JP2007333652A (en) | Positioning system and navigation system | |
CN110637209A (en) | Method, apparatus, and computer-readable storage medium having instructions for estimating a pose of a motor vehicle | |
CN110095793A (en) | A kind of automatic Pilot low speed sweeper localization method adaptive based on tire radius | |
JP2020009252A (en) | Autonomous moving body location estimation device | |
KR101435106B1 (en) | Localization system of under water robots | |
CN112697153A (en) | Positioning method of autonomous mobile device, electronic device and storage medium | |
CN109612464B (en) | Multi-algorithm enhanced indoor navigation system and method based on IEZ framework |
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 |