JP5763986B2 - MOBILE BODY AND METHOD FOR CONTROLLING MOBILE BODY - Google Patents
MOBILE BODY AND METHOD FOR CONTROLLING MOBILE BODY Download PDFInfo
- Publication number
- JP5763986B2 JP5763986B2 JP2011145585A JP2011145585A JP5763986B2 JP 5763986 B2 JP5763986 B2 JP 5763986B2 JP 2011145585 A JP2011145585 A JP 2011145585A JP 2011145585 A JP2011145585 A JP 2011145585A JP 5763986 B2 JP5763986 B2 JP 5763986B2
- Authority
- JP
- Japan
- Prior art keywords
- data
- distance sensor
- distance
- map data
- moving body
- 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.)
- Expired - Fee Related
Links
- 238000000034 method Methods 0.000 title claims description 55
- VJYFKVYYMZPMAB-UHFFFAOYSA-N ethoprophos Chemical compound CCCSP(=O)(OCC)SCCC VJYFKVYYMZPMAB-UHFFFAOYSA-N 0.000 title 2
- 238000005259 measurement Methods 0.000 claims description 89
- 230000009467 reduction Effects 0.000 claims description 37
- 230000008569 process Effects 0.000 claims description 29
- 230000036544 posture Effects 0.000 description 28
- 230000007246 mechanism Effects 0.000 description 16
- 238000012545 processing Methods 0.000 description 15
- 238000009434 installation Methods 0.000 description 13
- 230000000694 effects Effects 0.000 description 6
- 238000003860 storage Methods 0.000 description 5
- 238000004891 communication Methods 0.000 description 4
- 230000006870 function Effects 0.000 description 4
- 238000012790 confirmation Methods 0.000 description 2
- 238000001514 detection method Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000011946 reduction process Methods 0.000 description 2
- 230000008859 change Effects 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 238000005336 cracking Methods 0.000 description 1
- 230000001678 irradiating effect Effects 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 238000002360 preparation method Methods 0.000 description 1
- 239000000725 suspension Substances 0.000 description 1
- 230000008719 thickening Effects 0.000 description 1
Images
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Description
本発明は、周囲の環境を計測することで自身の位置・姿勢を認識しながら移動する移動体および移動体の制御方法に関する。 The present invention relates to a moving body that moves while recognizing its own position and orientation by measuring the surrounding environment, and a method for controlling the moving body.
移動体が環境内を効率的に移動するためには、環境内における位置・姿勢の推定（以下、位置姿勢推定）が不可欠である。移動体自身の位置姿勢推定を行う移動体に関する背景技術として、特許文献１と特許文献２が挙げられる。特許文献１には、環境中に設置された複数の反射体を計測することで、路面の傾斜や凹凸による移動体の傾きを検出し、自身の位置を正確に求める技術ついての開示がある。特許文献２には、環境の地図と車体の移動情報を用いて、センサで実測した場合に得られるセンサデータを予測し、この予測によるセンサデータと実測によるセンサデータの比較によって、位置推定誤りが生じていると判定された場合は、その場での旋回を行い、旋回後にセンサが遮断されてない状態で得られるセンサデータと地図とを用いて位置推定を行うことで、位置推定誤り状態から復帰させる技術について記載されている。 In order for a moving body to move efficiently in the environment, estimation of the position / posture in the environment (hereinafter, position / posture estimation) is indispensable. Patent documents 1 and patent documents 2 are mentioned as background art about a mobile object which performs position and orientation estimation of the mobile object itself. Japanese Patent Application Laid-Open No. 2004-133867 discloses a technique for measuring a plurality of reflectors installed in the environment to detect the inclination of a moving body due to road surface inclination or unevenness and accurately determine its own position. In Patent Document 2, sensor data obtained when actually measured by a sensor is predicted using an environment map and vehicle body movement information, and a position estimation error is detected by comparing the sensor data obtained by this prediction with the sensor data obtained by actual measurement. If it is determined that it has occurred, perform a turn on the spot and perform position estimation using sensor data and a map obtained when the sensor is not shut off after the turn. It describes the technology to restore.
特許文献１に示される技術は、再帰反射体と正反射体の既知の反射体を所定の複数場所に設け、移動体のセンサから発射したレーザ光が反射体で反射して戻る正反射光と再帰反光を検出して、移動体の傾きと位置を検出している。しかしながら、通常の環境に事前に再帰反射体と正反射体の専用の反射体を設置することは、準備に手数がかかってコスト的、実用的に困難である。また検出の演算が複雑でありこの点からもコストを要していた。 The technology disclosed in Patent Document 1 is a method of providing a regular reflection light in which a laser beam emitted from a sensor of a moving body is reflected by a reflector and returned from a predetermined number of places where a known retroreflector and a regular reflector are provided. The tilt and position of the moving object are detected by detecting recursive reflection. However, it is difficult to install retrospective reflectors and regular reflectors in a normal environment in advance because of the time and effort required for preparation. In addition, the calculation for detection is complicated, and cost is also required from this point.
特許文献２に示される技術は、通常の環境中の一般の物体の幾何形状をセンサによって計測し、計測結果と予め保持する地図とを比較（マッチング）して移動体の地図上での位置・姿勢を推定し、その推定結果に基いて移動体が移動するようにしている。しかし、移動体は、サスペンションなどの移動機構のため、路面の凹凸や加速、減速、停止時に揺れることでセンサが傾くことがある。センサが傾くと、環境中の物体の幾何形状の本来の計測点（場所）を見失う恐れがあり、地図データと物体の計測点とマッチングがとれず、計測誤りが生じて位置姿勢推定が困難となる恐れがある。特許文献２にはこの点について考慮されていない。 The technique disclosed in Patent Document 2 measures the geometry of a general object in a normal environment with a sensor, compares (matches) the measurement result with a map stored in advance, and compares the position of the moving object on the map. The posture is estimated, and the moving body moves based on the estimation result. However, since the moving body is a moving mechanism such as a suspension, the sensor may tilt when the road surface is uneven, accelerated, decelerated, or shaken when stopped. If the sensor is tilted, the original measurement point (location) of the geometrical shape of the object in the environment may be missed, the map data and the measurement point of the object cannot be matched, and a measurement error occurs, making it difficult to estimate the position and orientation There is a fear. Patent Document 2 does not consider this point.
本発明は、従来技術の問題点にかんがみ、センサの傾きに因む計測誤りの影響を低減して、より正確な位置・姿勢の推定が行える、移動体および移動体の制御方法を提供することを目的とする。 In view of the problems of the prior art, the present invention provides a moving body and a moving body control method capable of reducing the influence of measurement errors due to sensor inclination and more accurately estimating the position and posture. Objective.
上記課題を解決するため、本発明は、距離センサにより移動体の周囲の環境を計測し、計測した距離データと予め保持する地図データとを比較することにより移動体の前記地図データ上での位置と姿勢を推定し、その推定結果に基づいて目的地に移動する移動体において、
前記距離センサの距離データを環境の幾何形状データに変換する距離センサ制御部と、
計測点のセンサからの距離に応じて、前記距離センサ制御部で得られた幾何形状データと地図データの一方のマッチング対象のサイズを設定する距離センサ誤差低減部と、
前記距離センサ誤差低減部で設定されたマッチング対象のサイズで幾何形状データと地図データをマッチング処理して移動体の位置と姿勢を推定する位置姿勢推定部を備えたことを特徴とする。
In order to solve the above-mentioned problem, the present invention measures the environment around the moving body with a distance sensor, and compares the measured distance data with map data stored in advance to determine the position of the moving body on the map data. In a moving object that moves to the destination based on the estimation result,
A distance sensor control unit for converting the distance data of the distance sensor into the geometric data of the environment;
A distance sensor error reducing unit that sets the size of one of the matching objects of the geometric shape data and the map data obtained by the distance sensor control unit according to the distance from the sensor of the measurement point;
A position / orientation estimation unit for estimating the position and orientation of a moving body by performing a matching process on geometric shape data and map data with a matching target size set by the distance sensor error reduction unit is provided.
また、上記に記載の移動体において、前記距離センサ誤差低減部は、計測点のセンサからの距離が長いほどマッチング対象のサイズを大きく設定することを特徴とする。 In the moving body described above, the distance sensor error reduction unit sets the size of the matching object to be larger as the distance from the sensor of the measurement point is longer.
また、上記に記載の移動体において、前記距離センサ誤差低減部は、幾何形状データをなす計測点毎に、マッチング対象のサイズを設定することを特徴とする。 In the moving body described above, the distance sensor error reduction unit sets a size of a matching target for each measurement point forming the geometric shape data.
また、上記に記載の移動体において、前記距離センサ誤差低減部は、前記距離センサ制御部で得られた幾何形状データのマッチング対象のサイズを設定することを特徴とする。 Further, in the moving body described above, the distance sensor error reduction unit sets a size of a matching target of geometric shape data obtained by the distance sensor control unit.
また、上記に記載の移動体において、前記距離センサ誤差低減部は、前記距離センサ制御部で得られた幾何形状データのマッチング対象の形状をセンサの誤差に合わせて楕円形状に設定することを特徴とする。 Further, in the moving body described above, the distance sensor error reduction unit sets the shape of the matching target of the geometric shape data obtained by the distance sensor control unit to an elliptical shape according to the sensor error. And
また、上記に記載の移動体において、前記距離センサ誤差低減部は、前記地図データのマッチング対象のサイズを設定することを特徴とする。 Further, in the mobile body described above, the distance sensor error reduction unit sets a size of a matching target of the map data.
また、上記に記載の移動体において、前記距離センサ誤差低減部は、前記地図データのマッチング対象である物体存在画素の範囲を設定することを特徴とする。 In the moving object described above, the distance sensor error reduction unit sets a range of object existence pixels that are matching targets of the map data.
また、上記に記載の移動体において、前記距離センサ誤差低減部は、移動体の周囲の環境の床面による計測誤差を含むと考えられる計測点を幾何形状データから除去することを特徴とする。 In the moving body described above, the distance sensor error reduction unit removes measurement points that are considered to include a measurement error due to the floor surface of the environment around the moving body from the geometric shape data.
上記課題を解決するため、本発明は、距離センサにより移動体の周囲の環境を計測し、計測した距離データと予め保持する地図データとを比較することにより移動体の前記地図データ上での位置と姿勢を推定し、その推定結果に基づいて目的地に移動する移動体の制御方法において、
距離センサ制御部により距離センサの距離データを環境の幾何形状データに変換し、
距離センサ誤差低減部により前記で得られた幾何形状データと地図データの一方のマッチング対象のサイズを計測点のセンサからの距離に応じて設定し、
位置姿勢推定部により前記で設定されたマッチング対象のサイズで幾何形状データと地図データをマッチング処理して移動体の位置と姿勢を推定することを特徴とする。
In order to solve the above-mentioned problem, the present invention measures the environment around the moving body with a distance sensor, and compares the measured distance data with map data stored in advance to determine the position of the moving body on the map data. In the control method of the moving object that moves to the destination based on the estimation result,
The distance sensor control unit converts the distance data of the distance sensor into the geometric data of the environment,
The distance sensor error reduction unit sets the size of one of the geometric shape data and the map data obtained above according to the distance from the sensor of the measurement point,
The position and orientation estimation unit estimates the position and orientation of the moving object by matching the geometric shape data and the map data with the size of the matching target set as described above.
また、上記に記載の移動体の制御方法において、前記距離センサ誤差低減部により、計測点のセンサからの距離が長いほどマッチング対象のサイズを大きく設定することを特徴とする。 In the above-described method for controlling a moving body, the distance sensor error reduction unit sets the size of the matching target to be larger as the distance from the sensor of the measurement point is longer.
また、上記に記載の移動体の制御方法において、前記距離センサ誤差低減部により、幾何形状データをなす計測点毎に、マッチング対象のサイズを設定することを特徴とする。 Further, in the above-described method for controlling a moving body, the distance sensor error reduction unit sets a size of a matching target for each measurement point forming geometric shape data.
また、上記に記載の移動体の制御方法において、前記距離センサ誤差低減部により、前記距離センサ制御部で得られた幾何形状データのマッチング対象のサイズを設定することを特徴とする。 In the control method for a moving body described above, the distance sensor error reduction unit sets a size of a matching target of geometric shape data obtained by the distance sensor control unit.
また、上記に記載の移動体の制御方法において、前記距離センサ誤差低減部により、前記距離センサ制御部で得られた幾何形状データのマッチング対象の形状をセンサの誤差に合わせて楕円形状に設定されることを特徴とする。 Further, in the above-described moving body control method, the distance sensor error reduction unit sets the shape to be matched of the geometric shape data obtained by the distance sensor control unit to an elliptical shape according to the sensor error. It is characterized by that.
また、上記に記載の移動体の制御方法において、前記距離センサ誤差低減部により、前記地図データのマッチング対象のサイズを設定することを特徴とする。 Moreover, in the control method of the moving body described above, the distance sensor error reduction unit sets the size of the matching target of the map data.
また、上記に記載の移動体の制御方法において、前記距離センサ誤差低減部により、前記地図データのマッチング対象である物体存在画素の範囲を設定することを特徴とする。 In the moving body control method described above, the distance sensor error reduction unit sets a range of object existing pixels that are targets of matching of the map data.
また、上記に記載の移動体の制御方法において、前記距離センサ誤差低減部により、移動体の周囲の環境の床面による計測誤差を含むと考えられる計測点を幾何形状データから除去することを特徴とする。 In the method for controlling a moving body described above, the distance sensor error reduction unit removes from the geometric shape data measurement points that are considered to include measurement errors due to the floor surface of the environment around the moving body. And
本発明によれば、センサの傾きに因む計測誤りの影響を低減して、より正確な位置姿勢推定が行える。 According to the present invention, it is possible to reduce the influence of measurement errors caused by the tilt of the sensor and perform more accurate position and orientation estimation.
以下、図面を用いて本発明の実施例について説明する。移動体の具体例としてはロボットを用いている。 Embodiments of the present invention will be described below with reference to the drawings. A robot is used as a specific example of the moving body.
また、前述の説明では、各部の間で出力、入力、受信、送信されたり、参照されたり、記憶されたり、設定される情報、データ、信号等を説明によっては、異なる表現としているが、その文言によって、限定されるものではない。例えば、「値（設定値、検出値、補正値、出力値など）」として、説明していても、データにのみ限定されるものではなく、情報、信号として扱い、表現しても同様の意味を表すものとする。これは、以下の説明においても、同様とする。 In the above description, information, data, signals, and the like that are output, input, received, transmitted, referenced, stored, and set between the units are expressed differently depending on the description. It is not limited by the wording. For example, even if it is described as “value (setting value, detection value, correction value, output value, etc.)”, it is not limited only to data, but it may be treated and expressed as information or a signal with the same meaning . The same applies to the following description.
また、本明細書において、「計測」として、説明をしている。しかし、この「計測」に限定されるものではなく、一般に「測定」として、説明されること、「測定」を行う構成としても、若しくは、表現しても同様の意味を表すものとする。 In the present specification, “measurement” is described. However, the present invention is not limited to this “measurement”, and is generally described as “measurement”, and it is assumed that the same meaning is expressed or expressed as a configuration for performing “measurement”.
ここでは、まず本実施例で想定するロボットの機能の構成・主な処理内容について述べ、続いて、ロボットのより具体的なハードウェア・ソフトウェア構成と、これらの動作について述べる。 Here, the configuration and main processing contents of the robot function assumed in this embodiment will be described first, followed by a more specific hardware / software configuration of the robot and the operation thereof.
ロボットの機能の構成を図１に示す。ロボット（移動体）０１０１は、コントローラ部０１０２、距離センサ部０１０３、移動機構部０１０９を有している。コントローラ部０１０２は、距離センサ制御部０１０４、距離センサ誤差低減部０１０５、位置姿勢推定部０１０６、経路計画部０１０７、移動機構制御部０１０８、距離センサ設置データ０１１０、距離センサスポット光サイズデータ０１１１、地図データ０１１２、経路データ０１１３から構成される。各データは後述するように記憶装置に保持されている。また、ここでは図示していないが、各部を支持する筐体、電源・配線など、各部が連携し、動作するために必要なものは備わっているものとする。
The configuration of the robot function is shown in FIG. The robot (moving body) 0101 has a
まず、ロボット０１０１が自動走行する際に行われる処理について述べる。ロボット０１０１は、距離センサ制御部０１０４により距離センサ部０１０３を制御し、ロボット０１０１から周囲の環境中の物体までの距離と方向からなる距離データを、計測する。ここでは距離センサ部０１０３として、レーザ距離センサを用いるものとする。
First, processing performed when the
このレーザ距離センサは、レーザを物体に照射してから、照射したレーザが物体によって反射してセンサに返ってくるまでの時間を計測することで、センサから環境中の物体までの距離の計測を行うレーザ照射部（図示せず）を備えている。このレーザ照射部を一定の回転角毎に回転させながら計測することで、一定の回転角度範囲内にある物体までの距離の計測（以下、スキャン）が可能であるとする。このスキャンにより、スキャンによってレーザがなす平面（以下、スキャン面）上にある物体のまでの距離と方向が得られる。 This laser distance sensor measures the distance from the sensor to the object in the environment by measuring the time from when the laser is irradiated to the object until the irradiated laser is reflected by the object and returns to the sensor. A laser irradiation unit (not shown) is provided. It is assumed that measurement (hereinafter referred to as scanning) of a distance to an object within a certain rotation angle range is possible by measuring the laser irradiation unit while rotating it at every constant rotation angle. By this scanning, a distance and direction to an object on a plane (hereinafter referred to as a scanning plane) formed by the laser by scanning can be obtained.
これらのデータの組を距離データと呼ぶものとする。この距離データの１つ１つは距離と方向のデータから位置のデータに変換することができる。このようにして距離データを位置のデータに変換したものを幾何形状データと呼ぶものとする。また、幾何形状データをなす１つ１つの位置のデータは、レーザスポット光が環境中の物体に到達した位置、つまりは点の位置を表すがこの点を計測点と呼ぶものとする。ここで、このレーザ距離センサのスキャン面が床面に平行となるようにロボットに取り付けられているものとすると、レーザ距離センサのスキャン面の高さでの幾何形状がデータとして得られることとなる。 These data sets are called distance data. Each of the distance data can be converted from distance and direction data into position data. The data obtained by converting the distance data into the position data in this way is referred to as geometric data. Further, the data of each position constituting the geometric shape data represents the position where the laser spot light reaches the object in the environment, that is, the position of the point, but this point is called a measurement point. Here, assuming that the scanning surface of the laser distance sensor is attached to the robot so as to be parallel to the floor surface, the geometric shape at the height of the scanning surface of the laser distance sensor is obtained as data. .
上記のようにして、距離センサ制御部０１０４によって距離データから得られた幾何形状データは、距離センサ誤差低減部０１０５に送られる。距離センサ誤差低減部０１０５には、予め距離センサ設置データ０１１０と距離センサスポット光サイズデータ０１１１が読み込まれているものとする。
As described above, the geometric shape data obtained from the distance data by the distance
距離センサ設置データ０１１０とは、ロボットに対する距離センサ部０１０３の位置・姿勢と床面からの高さを示すデータである。また、距離センサスポット光サイズデータ０１１１とは、距離センサから環境中の物体に向けて照射されるレーザなどのスポット光が距離に応じてどのように広がるかを示すデータである。より具体的には、ここでは、センサからの計測距離毎のスポット光の直径が記録されているものとする。これらのデータを用いて、距離センサ誤差低減部０１０５では、まずスポット光割れセンサデータの除去が行われ、続いて、距離データをなす距離値毎に後述の位置姿勢推定のマッチング判定基準となるマッチングウインドウサイズの設定が行われる。
The distance
まず、スポット光割れセンサデータの除去について述べる。 First, the removal of spot light crack sensor data will be described.
スポット光割れセンサデータの除去は、前述の距離センサ設置データ０１１０と距離センサスポット光サイズデータ０１１１を用いて行われる。より具体的には、センサが傾いてないとの仮定のもと、距離センサ部０１０３より照射されたレーザスポット光が距離に応じて広がって床面と交わる際の距離を求め、幾何形状データをなす多数の計測点のうち、この距離よりも遠い位置にある計測点の幾何形状データについては、床面などを誤計測しているものとみなして、幾何形状データから除去する。
The removal of the spot light crack sensor data is performed using the distance
続いて、幾何形状データをなす計測点毎にマッチングウインドウサイズ（マッチング対象のサイズ）の設定が行われる。まず、ここでのマッチングとしては、画像として表現された幾何形状データと、予めスキャン面の高さでの環境の幾何形状を画像として扱えるようにした地図データ０１１２を重ね合せたときに、物体が存在することを示す画素（以下、物体存在画素）同士が最も重なり合うときの地図データ上での幾何形状データの位置・姿勢を探索によって求める方式を想定する。また、マッチングウインドウ（マッチング対象）とは、幾何形状データの画像における物体存在画素（幾何形状データをなす計測点に相当）を中心に設けられる枠を指す。このマッチングウインドウは、幾何形状データと地図データとを重ね合わせたときのマッチングの判定基準として用いられる。
Subsequently, a matching window size (matching target size) is set for each measurement point forming the geometric shape data. First, as the matching here, when the geometric shape data expressed as an image and the
具体的には、幾何形状データをなす計測点に設けたマッチングウインドウと重なる範囲にある地図データの画素に、物体存在画素が含まれる場合、その計測点は地図データに対してマッチしたとみなすというふうに用いられる。よって、ウインドウサイズが大きくなるほど、幾何形状データをなす計測点と地図データとを重ね合わせたときのずれが許容されることとなる。距離センサ誤差低減部０１０５では、幾何形状データをなす計測点のセンサからの距離が大きい（長い）ほど、マッチングウインドウサイズも大きく設定する。
Specifically, if an object existence pixel is included in the pixel of the map data in the range that overlaps the matching window provided at the measurement point forming the geometric shape data, the measurement point is regarded as matching with the map data. Used in a fussy manner. Therefore, the larger the window size, the larger the deviation is allowed when the measurement points forming the geometric shape data and the map data are superimposed. In the distance sensor
距離センサ部０１０３が傾いた状態（スキャン面が床面に対して平行でない状態）で計測を行うと、センサが傾いてない状態（スキャン面が床面に対して平行な状態）での計測に比べて大きな距離値が得られる。今、センサが傾いてない状態で得られる幾何形状データのマッチングを前提として、床面に平行かつ傾いてないセンサのスキャン面に対応するように地図データが作られているとする。この状況において、マッチングウインドウサイズが距離によらず一定の大きさだとすると、センサが傾いてないときには地図データに重なり合う計測点であっても、センサが傾いているときには、地図データに重なりにくく、つまりはマッチしなくなる。これは、計測点のセンサからの距離が大きいほど計測点のずれが大きくなるため、マッチングしづらくなる。
When measurement is performed with the
これに対して、本実施例１は前述の通り、幾何形状データをなす計測点のセンサからの距離が大きいほど、マッチングウインドウサイズが大きくなるように設定すると、本来、地図データと重なり合うべき計測点が、地図データ上で本来重なり合うべき点に対して少々ずれても、ウインドウサイズの範囲内に収まり、つまりはマッチする可能性が上がり、センサの傾きに伴うマッチング誤りが低減されることとなる。以上のようにして、マッチングウインドウサイズの大きさの変更によって、マッチングの判定基準を緩める設定を行い、距離センサの傾きにより計測に誤差があってもマッチング誤差の低減、つまりは位置姿勢推定の誤りを低減することができる。 On the other hand, in the first embodiment, as described above, when the distance from the sensor of the measurement point forming the geometric shape data is set to be larger as the matching window size is larger, the measurement point that should originally overlap with the map data is set. However, even if it slightly deviates from the point that should originally overlap on the map data, it falls within the window size range, that is, the possibility of matching increases, and the matching error due to the tilt of the sensor is reduced. As described above, by setting the matching window size to be changed, the matching criterion is relaxed, and even if there is an error in measurement due to the inclination of the distance sensor, the matching error is reduced, that is, the position and orientation estimation error Can be reduced.
距離に応じてマッチングウインドウのサイズが設定された幾何形状データは、位置姿勢推定部０１０６に送られる。位置姿勢推定部０１０６では、予め地図データ０１１２が読み込まれており、幾何形状データと地図データ０１１２とのマッチングが行われ、幾何形状データと地図データ０１１２とが重なる割合が最大のときの幾何形状データの位置・姿勢が求まる。地図データ０１１２上に定められた座標系における幾何形状データの始点の位置と姿勢つまりは距離センサ部０１０３の始点の位置と姿勢が求められる。ロボットの位置・姿勢を表す際にはロボットの筐体のどこを基準としてもよいため、ここでは求められた距離センサ部０１０３の位置・姿勢をもってロボット０１０１の位置・姿勢とする。
The geometric shape data in which the size of the matching window is set according to the distance is sent to the position /
経路計画部０１０７は、ロボットの通りうる通路の長さや方向が記録された経路グラフデータ０１１３を予め読み込んでおき、位置姿勢推定部０１０６で得られた現在のロボットの位置・姿勢をもとに目的地までの経路、および経路と現在位置とのずれを求める。
The
そして、移動機構制御部０１０８では、ロボットと追従しようとしている経路とのずれを小さくするように移動機構部０１０９の制御する。すなわち、車輪の回転速度やステアリングの切れ角などを求めてモータなどへの指示が行われる。これらにより、ロボットの経路への追従、ひいては目的地までの自動走行が実現される。以上が、ロボットを構成する機能毎に行われる処理の概要となる。
Then, the movement
続いて、まず、ロボットのハードウェアとソフトウェアの構成について述べ、ロボットの距離センサが傾いた状態で計測することで得られた幾何形状データを用いて、位置姿勢推定を行う例を通して、ハードウェアとソフトウェア全体の処理の流れについて述べる。 Next, the configuration of the hardware and software of the robot will be described first. Through the example of estimating the position and orientation using the geometric shape data obtained by measuring the robot with the robot's distance sensor tilted, The flow of the entire software process is described.
ロボットのハードウェアとこれに格納されるソフトウェアの構成を図２に示す。ロボット０２０１（０１０１に相当）は、位置姿勢推定コントローラ０２０２、レーザ距離センサ０２０７（０１０３に相当）、移動機構０２０８（０１０９に相当）、ディスプレイ０２０９、入力機器０２１０、これらの機器同士が通信するための通信線０２０５より構成される。なお、図２では処理の流れに直接関わる要素のみを表記しており、各要素の動作に必要な電源等は当然備わっているものとする。
FIG. 2 shows the configuration of the robot hardware and the software stored therein. A robot 0201 (corresponding to 0101) is a position /
レーザ距離センサ０２０７には、前述の距離センサ部０１０４で挙げたレーザ距離と同じ方式のセンサを用いるものとする。ここでは、例として、レーザ距離センサ０２０７がスキャンする角度範囲を１８０度とし、この角度範囲において０．５度毎にレーザを照射し、物体までの距離を計測することを想定するが、スキャンする角度範囲やレーザを照射する角度の刻み幅、距離の最大計測範囲などは異なっていてもよい。
As the
図３にロボットに取り付けられたレーザ距離センサにより、環境中の物体を計測する様子を示す。同図は環境とロボット０３０５（図２のロボット０２０１に相当）を上から見下ろした様子を表す平面図である。また、環境中の物体０３０１の壁面は床面に対して垂直であり、凹凸がないものとする。例として図中の位置・姿勢にあるロボット０３０５がレーザ距離センサ０３０６（図２のレーザ距離センサ０２０７に相当）により１８０度の角度範囲０３０９をスキャンしたとすると、幾何形状データ０３０８（ドットで示す）、赤外線を面状に物体に照射することで画素毎の物体までの距離計測が可能なデプスカメラなどであってもよい。
FIG. 3 shows how an object in the environment is measured by a laser distance sensor attached to the robot. This figure is a plan view showing the environment and the robot 0305 (corresponding to the
移動機構部０２０８としては、図３に示されるロボット０３０５を想定する。同ロボット０３０５には、前部にキャスタが、また後部に駆動輪が備えられており、駆動輪の回転角速度の差を制御することで直進・旋回を行えるようになっているものとする。本実施例ではこのような移動機構の使用を想定しているが、同様に環境内を移動する効果が得られるのであれば移動機構の方式は異なっていてもよい。例えば、無限軌道を備える車両、脚を備えるロボット、船舶、航空機、飛行船など他の移動機構であってもよい。また、本実施例においては、ロボットが自動走行するようになっているが、人が搭乗してロボットを操縦するようになっていてもよいし、また、搭乗せずに遠隔からの通信によって人が操縦するようになっていてもよい。
As the moving
また、位置姿勢推定コントローラ０２０２は、プロセッサ０２０３、メモリ０２０４、記憶装置０２０６、ＯＳ０２１１、ＢＩＯＳ読み込みやＯＳの起動を行うコントローラ初期化プログラム０２１２、レーザ距離センサ０２０７から距離データを取得するレーザ距離センサ制御プログラム０２１３、幾何形状データの中から無効なデータを除去すると共に、マッチングウインドウの大きさを設定する、レーザ距離センサ誤差低減プログラム０２１４、幾何形状データと地図データ０２２０とのマッチングによって、位置・姿勢を算出する位置姿勢推定プログラム０２１５、経路グラフデータ０２２１をもとに目的地に辿り着くための経路を算出する経路計画プログラム０２１６、経路に沿って車体が移動するように車輪の回転速度などを算出する移動機構制御プログラム０２１７から構成される。
The position /
また、記憶装置０２０６には、ロボットに対する距離センサの設置位置・姿勢や床面からの高さを表すレーザ距離センサ設置データ０２１８、レーザ距離センサより照射されるレーザスポット光が距離に応じて広がる際の距離毎の直径を表すレーザ距離センサスポット光サイズデータ０２１９、位置姿勢推定プログラム０２１５がマッチングに用いる地図データ０２２０、環境内でロボットが通る経路の長さや方向を表す経路グラフデータ０２２１が記録されている。
Further, in the
なお、図２のプログラムやデータは、メモリにロードされた上で、プロセッサにより処理されることを想定しているが、これと同様の効果が得られるのであれば、実装は異なっていてもよい。例えば、ＦＰＧＡ（Ｆｉｅｌｄ Ｐｒｏｇｒａｍｍａｂｌｅ Ｇｒｉｄ Ａｒｒａｙ）やＣＰＬＤ（Ｃｏｍｐｌｅｘ Ｐｒｏｇｒａｍｍａｂｌｅ Ｌｏｇｉｃ Ｄｅｖｉｃｅ）などのプログラマブルなハードウェアで以上の処理を実現してもよい。なお、プログラムやデータは、ＣＤ−ＲＯＭ等の記憶媒体から移してもよいし、ネットワーク経由で他の装置からダウンロードしてもよい。 It is assumed that the program and data in FIG. 2 are loaded into the memory and then processed by the processor, but the implementation may be different as long as the same effect can be obtained. . For example, the above processing may be realized by programmable hardware such as FPGA (Field Programmable Grid Array) or CPLD (Complex Programmable Logic Device). Note that the program and data may be transferred from a storage medium such as a CD-ROM or downloaded from another device via a network.
また、プロセッサや記憶装置、移動機構など、ロボット０２０１を構成する各デバイスは、ここでは有線の通信線０２０５により互いに通信することを想定しているが、無線であってもよく、また、通信が可能であれば、コントローラ０２０２、ディスプレイ０２０８、入力装置０２１０の各デバイスが物理的に遠隔にあってもよい。また、以上のハードウェアやソフトウェアは、実施形態に応じて、取捨選択してもよい。
In addition, although it is assumed here that each device constituting the
続いて、ロボット０２０１で行われる処理の流れについて述べる。起動されたコントローラ０２０２の処理の流れを図４に示す。
Next, the flow of processing performed by the
コントローラ０２０２が起動されると（０４０１）、コントローラ初期化プログラム０２１２により、ＯＳ０２１１の読み込み、各プログラム０２１２〜０２１７の起動が行われる（０４０２）。次いで、目的地の設定を終了するかどうかの確認画面がディスプレイ０２０９に表示される。ロボット０２０１に自動走行を行わせる場合、操作者は目的地の設定を行うことを入力装置０２１０により選択する（０４０３）。もし、ロボット０２０１の自動走行を行わないのであれば終了を選択する。この場合、直ちにプログラムは終了となる（０４０４）。今、ロボットに自動走行を行わせるため、操作者が目的地の設定を行う方を選択したとすると、処理０４０５に処理が進む。
When the
続いて、操作者は、ディスプレイ０２０９に表示される搬送先の候補のリスト上で目的地を確認し、入力装置０２１０により目的地を設定する（０４０５）。
次に、レーザ距離センサ誤差低減プログラム０２１４により、レーザ距離センサ設置データ０２１８が読み込まれる（０４０６）。このデータには前述のとおり、ロボット０２０１に対するレーザ距離センサ０２０７の設置位置・姿勢や床面からの高さが記録されている。
Subsequently, the operator confirms the destination on the list of transport destination candidates displayed on the
Next, the laser distance
次に、同じくレーザ距離センサ誤差低減プログラム０２１４により、レーザ距離センサスポット光サイズデータ０２１９が読み込まれる（０４０７）。このデータには前述のとおり、距離センサから環境中の物体に向けて照射されるレーザなどのスポット光が、距離に応じてどのように広がるかが記録されている。ここでは、具体的には、センサからの計測距離毎のスポット光の直径が記録されている。
Next, the laser distance sensor spot
次に、位置姿勢推定プログラム０２１５により、地図データ０２２０が読み込まれる（０４０８）。なお、ここでの地図データ０２２０は画像データとなっており、画素毎に環境中の物体の有無が記録されているものとする。
Next,
次に、経路計画プログラム０２１６により、経路グラフデータ０２２１が読み込まれる（０４０９）。なお、ここでの経路グラフデータ０２２１には、環境内でロボットが通る経路の長さや方向が記録されているものとする。
Next, the
続いて、レーザ距離センサ制御プログラム０２１３によりレーザ距離センサ０２０７から距離データを取得する（０４１０）。距離データをなす距離と方向より位置が求められ、物体の存在の有無を画素に記録して画像とした幾何形状データに変換される。
Subsequently, distance data is acquired from the
レーザ距離センサ制御プログラム０２１３により変換された幾何形状データは、距離センサ誤差低減プログラム０２１４に送られる。レーザ距離センサ誤差低減プログラム０２１４には前述の通り、レーザ距離センサ設置データ０２１８とレーザ距離センサスポット光サイズデータ０２１９が読み込まれている。これらのデータを用いることで、レーザ距離センサ誤差低減プログラム０２１４では、まずスポット光割れセンサデータの除去が行われる（０４１１）。
The geometric shape data converted by the laser distance
ここでは、レーザ距離センサ０２０７が傾いてないとの仮定のもと、レーザ距離センサ０２０７より照射されたレーザスポット光が距離に応じて広がって床面と交わる際の距離（以下、床面交差距離）を求め、幾何形状データをなす各計測点のうち、センサから計測点までの距離が床面交差距離よりも大きい計測点については、床面などを誤計測しているものとみなして、幾何形状データから除去する。
Here, on the assumption that the
具体的には、今、レーザ距離センサ設置データ０２１８として、ロボット０２０１に対するレーザ距離センサ０２０７の設置位置・姿勢と、レーザ距離センサ０２０７の床面からの設置高さが、図５のような状態にあることが記録されており、かつ、レーザスポット光が距離に応じて図５のように広がることが記録されているとする。この場合、レーザスポット光０５０４は、図中の点０５０６で床面０５０５と交わることがわかる。このレーザスポット光０５０４と、床面０５０５とが交わる点０５０６までのセンサからの距離０５０７が床面交差距離である。
Specifically, as the laser distance
図５のレーザスポット光０５０４によって環境中の物体０５０８までの距離を計測する場合、このレーザスポット光０５０４は図５のとおり、床面０５０５と交差するため、計測値には一定の誤差が含まれることとなる。このため、この計測点のデータは除去するものとする。
When the distance to the
続いて、計測点毎に幾何形状データへのマッチングウインドウサイズの設定が行われる（０４１２）。処理０４１２において、幾何形状データをなす計測点のセンサからの距離が大きいほど、幾何形状データのマッチングウインドウ（マッチング対象）のサイズを大きく設定しておくことで、この後の処理０４１４でのマッチング誤りを抑制し、正確な位置・姿勢の算出（推定）が行われる。
Subsequently, the matching window size is set to the geometric shape data for each measurement point (0412). In
ここでのマッチングウインドウの前提として、次の処理０４１４の位置姿勢推定で行われるマッチングについてまず述べる。ここでのマッチングとは、幾何形状データと地図データ０２２０を重ね合せて、物体存在画素同士が最も重なり合うときの地図データ０２２０上での幾何形状データの位置・姿勢を探索によって求めることである。地図データ０２２０への幾何形状データのマッチングの様子を図６に示す。
As a premise of the matching window here, the matching performed in the position and orientation estimation of the
図６の地図データ０６０１（図２の地図データ０２２０に相当）は、図３の環境に対応している。ここで、地図データの座標系０６０８上でのロボットの位置・姿勢を求める処理について述べる。ロボットが図６の位置・姿勢にて計測した際に得られる幾何形状データ０６０２が、前回の位置姿勢推定結果０６０６を中心とする図中の点線による一定の領域内において取りうるすべての位置・姿勢で、地図データ０６０１に対して重ね合わせ、物体存在画素が重なる割合を算出する処理を行っていく。これにより、例えば、幾何形状データ０６０３の始点の位置・姿勢（上方にずれている）０６０７では、地図データ０６０１と重なる割合は低いため、解の候補から外され、重なる割合がより高くなる幾何形状データ０６０２の始点の位置・姿勢０６０４解として求められることとなる。求められた座標系０６０８上での幾何形状データの位置・姿勢をロボットの位置・姿勢とする。
The
ここで、前述の物体存在画素が重なっているかどうかの判定基準について、図７および図７の一部を拡大して示す図９を用いて説明する。今、レーザ距離センサ０７２０（図２のレーザ距離センサ０２０７に相当）が、傾いてない状態で計測して得られる幾何形状データ０７０２を、地図データ０７０１にマッチングする場合を考える。マッチングにおける探索時の位置・姿勢の解の候補として、幾何形状データ０７０２が０７０３の位置・姿勢で計測されたものとして地図データ０７０１と重ね合わせられたとする。このとき、幾何形状データ０７０２と地図データ０７０１とが重ね合わせられた範囲０７０４を拡大した図を０７０５に示す。
Here, a criterion for determining whether or not the above-described object-existing pixels are overlapped will be described with reference to FIG. 7 and FIG. 9 showing an enlarged part of FIG. Consider a case where the
拡大図０７０５において、ハッチングがかかっている画素０７０７は、幾何形状データ０７０２をなす計測点である。つまりは、ハッチングがかかっている画素０７０７が、幾何形状データ０７０２の物体存在画素である。また、白い画素０７０８と黒い画素０７０６は、地図データ０７０１の画素をなす部分であり、白い画素０７０８は物体が存在していないことを示しており、黒い画素０７０６は物体が存在することを示している。つまりは、黒い画素０７０６が地図データ０７０１の物体存在画素である。
In the
幾何形状データ０７０２が図７の位置・姿勢にあるとき、例えば計測点の上記物体存在画素０７０７は、地図データ０７０１の物体存在画素０７０６重なっているため、マッチしたと判定される。マッチングウインドウサイズとは、幾何形状データ０７０２の物体存在画素（ハッチング部分）を中心に一定の大きさを持つ枠である。幾何形状データ０７０２の物体存在画素０７０７では、マッチングウインドウサイズはこの計測点０７０７の画素と同じ大きさとしている。
When the
今、レーザ距離センサ０７２０が傾いた状態で計測を行ったとすると、センサからの距離が短い計測点よりも、距離が長い計測点ほど、センサが傾いてないときの距離との差が大きくなる。このセンサの傾きを考慮せずに、幾何形状データを地図データにマッチングしようとすると、センサからの距離が長い計測点、つまりは幾何形状データの物体存在画素のうち、センサからの距離が長い物体存在画素ほど地図データにマッチしなくなる。図７を例とすると、幾何形状データ０７０２の物体存在画素０７０７の周辺の拡大図０７０５は、センサが傾いた状態で計測が行われたとしても、センサ０７２０から距離が短い計測点部分０７０４であるため、傾いてないときの計測点との距離の差は小さいため、マッチングウインドウサイズが小さくても、幾何形状データ０７０２の物体存在画素０７０７は地図データ０７０１の物体存在画素０７０６にマッチする。
If the measurement is performed with the
これに対して、センサが傾いた状態で計測が行われた場合、センサから遠い計測点ほど、傾いてないときの計測点との距離の差は大きくなるため、マッチングウインドウサイズが小さいままだと、幾何形状データ０７０２の物体存在画素０７０７は地図データ０７０１の物体存在画素０７０６にマッチしなくなる。
On the other hand, when measurement is performed with the sensor tilted, the difference between the distance from the measurement point when the sensor is not tilted increases as the measurement point is farther from the sensor. The
例えば、計測点がセンサ０７２０から遠く離れ位置にある場合について説明する。センサが傾いてない状態では、幾何形状データ０７０２と地図データ０７０１とが重なり合う範囲０７１１において、センサが傾いてないときは、０７１０のように幾何形状データ０７０２が地図データ０７０１にマッチする。しかし、センサが傾いているときは０７０９のように、幾何形状データ０７０２が実際のセンサから物体までの水平での距離（センサが傾いてないとき）よりも大きくでるため、地図データ０７０１にマッチしなくなる。このマッチしない部分を拡大したものを０７１５に示す。０７１５では、マッチングウインドウサイズが画素単位の１×１で小さく設定されている。幾何形状データ０７０２の物体存在画素０７１３（ハッチング部分）が、地図データ０７０１の物体存在画素０７１４（黒い画素）に重なっておらず、マッチしていないことがわかる。
For example, a case where the measurement point is far from the
このマッチング誤りに対しては、マッチングウインドウサイズの拡大で対応する。具体的には、拡大部分０７１６における、幾何形状データ０７０２の物体存在画素０７１９（ハッチング部分）の周囲に設けたマッチングウインドウを、破線の０７１７で示したようにサイズを３×３に大きく設定する。そして、大きく設定されたサイズのマッチングウインドウ０７１７の範囲に、地図データ０７０１の物体存在画素０７１９がある場合はマッチしたと判定するようにする。
This matching error is dealt with by expanding the matching window size. Specifically, the size of the matching window provided around the object existence pixel 0719 (hatched portion) of the
これにより、センサの傾きに因む、幾何形状データの地図データに対するずれが許容され、マッチング誤りが低減される。以上のように処理０４１２において、レーザ距離センサ誤差低減プログラム０２１４によって、幾何形状データをなす計測点のセンサからの距離が大きくなるほど、幾何形状データのマッチングウインドウのサイズを大きく設定する処理が行われる。例えば、計測点のセンサからの距離が大きくなるに従って、サイズが（１×１）→（２×２）→（３×３）のように設定される。なお、このマッチングウインドウサイズは、幾何形状データをなす計測点のセンサからの距離に単純に比例させてもよいが、予め設定された距離の閾値を用いて、この距離の閾値を越えたらマッチングウインドウサイズを一定としてもよい。これにより、マッチングウインドウサイズが大きくなりすぎることでのマッチング誤りを低減するようにしてもよい。
Thereby, the deviation of the geometric shape data from the map data due to the inclination of the sensor is allowed, and the matching error is reduced. As described above, in the
また、マッチングウインドウの形状は、ここでは単純に縦横比が同じ正方形としているが、形状は問わない。例えば、丸や長方形でもよい。また、レーザ距離センサによる計測の誤差の特性を考慮してもよい。例えば図１１のように、レーザ距離センサのレーザスポット光の始点から幾何形状データをなす計測点とを結ぶ直線１１０１に沿い、かつ計測点１１０４の誤差の範囲を表す楕円１１０３をマッチングウインドウとして設けるものとし、レーザ距離センサの奥行き方向の計測誤差より楕円の軸１１０２方向の大きさ１１０６を設定し、また、レーザスポット光の広がりやレーザスポット光を投光する方位の制御に因む計測誤差より楕円の軸１１０７方向の大きさ１１０５を設定し、マッチングウインドウとしてもよい。もちろん、楕円でなく、レーザ距離センサのレーザスポット光の始点から幾何形状データをなす計測点とを結ぶ直線１１０１に沿う四角形などであってもよい。
In addition, the shape of the matching window is simply a square having the same aspect ratio here, but the shape is not limited. For example, it may be a circle or a rectangle. In addition, characteristics of measurement errors by the laser distance sensor may be taken into consideration. For example, as shown in FIG. 11, an
このように、レーザ距離センサ誤差低減プログラム０２１４により、処理０４１１と０４１２でマッチング誤りの低減処理が行われる。
As described above, the matching error reduction process is performed in the
続いて、位置姿勢推定プログラム０２１５によって、幾何形状データと地図データ０２２０とのマッチングにもとづく位置・姿勢の推定が行われる（０４１４）。ここでは前述の通り、画像状に表現された幾何形状データと、同じく画像状に表現された地図データ０２２０を重ね合わせ、幾何形状データの中で物体存在画素が、地図データ０２２０上で物体存在画素と重なる割合が最大となるときの、地図データ０２２０上での幾何形状データの位置・姿勢、つまりはレーザ距離センサ０２０７の位置・姿勢が求められる。ここではこのレーザ距離センサ０２０７の位置・姿勢をロボット０２０１の位置・姿勢とする。なお、マッチングの手法としては、同様に地図データに対する幾何形状データの位置・姿勢を求める効果が得られるのであればマッチングの方式は異なっていてもよい。
Subsequently, the position /
次にロボットが目的地に到着しているか否かの判定を行う（０４１５）。これは、前述の位置姿勢推定で得られた位置が、目的地の座標から一定の距離内に入っているかどうかの判定によって行う。位置姿勢推定で得られた位置と目的地との距離が閾値以下の場合は、目的地に到着したものと判定され、処理０４０５に戻り、次の目的地までの自動走行まで指示待ちとなる。位置姿勢推定で得られた位置と目的地との距離が閾値より大きい場合は、目的地に到着していないものと判定され処理０４１７に処理が進む。 Next, it is determined whether or not the robot has arrived at the destination (0415). This is performed by determining whether or not the position obtained by the above-described position and orientation estimation is within a certain distance from the coordinates of the destination. When the distance between the position obtained by the position and orientation estimation and the destination is equal to or smaller than the threshold value, it is determined that the destination has arrived, and the process returns to process 0405 to wait for an instruction until automatic travel to the next destination. If the distance between the position obtained by the position and orientation estimation and the destination is larger than the threshold, it is determined that the destination has not arrived, and the process proceeds to process 0417.
ロボットが目的地にまだ到着してない場合、経路計画プログラム０２１６によって、現在位置・姿勢をもとに、目的地までの移動経路が求められる。図３の環境において、移動開始位置０３０２にあるロボット０２０１の目的地として、０３０７が設定されたとすると、経路計画プログラム０２１６は、起動時に予め読み込んだ経路グラフデータ０２２１を用いて経路探索を行い、局所的移動経路０３０４が求められる（０４１７）。
If the robot has not yet arrived at the destination, the route planning program 0216 obtains a movement route to the destination based on the current position / posture. In the environment of FIG. 3, if 0307 is set as the destination of the
続いて、移動機構制御プログラム０２１７により、経路計画で得られた経路に車体を移動させるための車輪の回転速度が算出され、また、車輪の回転速度が所定の値となるように移動機構０２０８のモータに対する電流値の指示が出される（０４１６）。
Subsequently, the moving
ロボットが移動していき、目的地に到着したと判定されたならば（０４１５）、前述の通り、目的地の設定確認の処理０４０３に戻る。以上が、ロボットが通常動作を行っている際の処理の流れとなる。
If it is determined that the robot has moved and has arrived at the destination (0415), the process returns to the destination
実施例１では、処理０４１２において、幾何形状データをなす計測点のセンサからの距離が大きいほど、幾何形状データへのマッチングウインドウ（マッチング対象）のサイズを大きく設定しておくことで、処理０４１３でのマッチング誤りを抑制し、センサの傾きの位置姿勢推定への影響を低減する方式について述べた。ここでは同様の効果を得るための別の方式について述べる。
In the first embodiment, in the
本実施例２では、実施例１のように幾何形状データへのマッチングウインドウサイズ（マッチング対象のサイズ）を大きくすることはせず、代わりに地図データのマッチング対象である物体存在画素の範囲（サイズ）を大きくする。つまり、低解像度化して物体存在画素を大きくする（膨張処理あるいは太線化処理する）ことで、計測点がずれた幾何形状データでも、地図データの物体存在画素にマッチし易くすることで、マッチング誤りを低減している。より具体的には、解像度の異なる地図データを複数準備しておき、幾何形状データをなす計測点のセンサからの距離が大きいほど、低解像度な（大きな）地図データを選択して、選択された地図データと幾何形状データのマッチングを行うようにする。 In the second embodiment, the matching window size (the size of the matching target) to the geometric shape data is not increased as in the first embodiment. Instead, the range (size) of the object existence pixel that is the matching target of the map data. ). In other words, by reducing the resolution and enlarging the object existence pixels (expansion processing or thickening processing), even if the geometric shape data has shifted measurement points, it is easy to match the object existence pixels in the map data, thereby matching errors. Is reduced. More specifically, a plurality of map data having different resolutions are prepared, and the larger the distance from the sensor of the measurement point forming the geometric shape data, the lower resolution (large) map data is selected and selected. Matching between map data and geometric data is performed.
図８およびその一部を拡大して示す図１０を用いて、解像度の異なる地図データを用いての、マッチング誤りを防ぐ様子を示す。センサが傾いていないならば、幾何形状データは０８０１のように地図データにマッチするが、センサが傾いた状態では、０８０２のように地図データに対してずれる。これは実施例１の説明で、図７を参照して説明したとおりである。ここで、ずれた０８０２の幾何形状データが、低解像度な地図データに対してマッチする様子を０８０４に示す。 FIG. 8 and FIG. 10 showing an enlarged part thereof show how a matching error is prevented using map data having different resolutions. If the sensor is not tilted, the geometric shape data matches the map data as 0801. However, when the sensor is tilted, it is shifted from the map data as 0802. This is the description of the first embodiment, as described with reference to FIG. Here, 0804 shows how the shifted 0802 geometric data matches low-resolution map data.
０８０４において、白い画素０８０６は地図データの物体が存在しない画素、黒い画素０８０７は地図データの物体存在画素、ハッチングがかかっている画素０８０５は幾何形状データの物体存在画素である。もとの地図データの物体存在画素は、図７の０７１９のように１画素の幅の線であったが、ここでは地図データを低解像度化として３画素の幅の線に変更されている。この画素の変更により、幾何形状データのウインドウサイズが１画素であっても、物体存在画素０８０５は地図データの物体存在画素０８０７に重なり、マッチしたと判定されることとなる。
In 0804, a
以上の処理を用いれば、本実施例２は、実施例１と同様にセンサの傾きのマッチングへの影響を低減することが可能となる。本実施例２の動作フローとしては、図４で処理０４０８、０４１２、０４１４を変更すればよい。
If the above processing is used, the second embodiment can reduce the influence on the matching of the tilt of the sensor as in the first embodiment. As the operation flow of the second embodiment, the
まず、処理０４０８において、解像度の異なる複数の地図データの読み込みを行う。解像度の異なる（マッチング対象のサイズの異なる）地図データとしては、実際の画素数を減らしてもよいし、画素数はそのままに、物体存在画素のある領域を膨張させて実質的な解像度を落としてもよい。処理０４１２を図４のフローから省き、処理０４１４の処理を変更する。すなわち、変更した処理０４１４では、センサからの距離が大きい計測点のマッチングには、複数の地図データから低解像度な地図データを選択した上で、マッチングを行うようにする。このように実施例２では、レーザ距離センサ誤差低減プログラム０２１４により、変更された処理０４１４でマッチング誤りの低減処理が行われる。
First, in
センサの傾きによって生じうるマッチング誤りに対し、実施例１では幾何形状データのマッチングウインドウサイズ（マッチング対象のサイズ）の変更により対応し、実施例２では低解像度にすることにより、地図データのマッチング対象のサイズ変更により対応する。 In the first embodiment, a matching error that may occur due to the tilt of the sensor is dealt with by changing the matching window size (the size of the matching target) of the geometric shape data. In the second embodiment, the map data is matched by reducing the resolution. Correspond by changing the size.
両実施例は効果が同じであるが、実施例１では、マッチングの都度、ウインドウサイズが変わり、このウインドウサイズの範囲でマッチしているか否かの判定を行う必要がある。幾何形状データにセンサから遠く離れた計測点が多く含まれるほど、大きなウインドウサイズでのマッチング判定を行うこととなり、計算量が増えることとなるが、実施例２のように複数の地図データを準備する必要がないため、使用するメモリ量は少なくて済む。 Both embodiments have the same effect, but in the first embodiment, the window size changes each time matching is performed, and it is necessary to determine whether or not the window size matches. The more measurement points that are far from the sensor are included in the geometric shape data, the larger the matching determination is performed with a large window size, which increases the amount of calculation. However, a plurality of map data is prepared as in the second embodiment. Since there is no need to do this, the amount of memory used is small.
一方、実施例２では、解像度の異なる地図データをマッチングの都度生成するのであれば計算量が増えることとなるが、予め複数の解像度の異なる地図データを用意しておけば、計算量は少なくなり、処理速度を速めることができる。 On the other hand, in the second embodiment, if map data with different resolutions is generated each time matching is performed, the amount of calculation increases. However, if a plurality of map data with different resolutions are prepared in advance, the amount of calculation is reduced. , Processing speed can be increased.
なお、幾何形状データと地図データの両方のマッチング対象のサイズを変更しても良いが、サイズ設定の制御が複雑となるので、一方のみで行うのが好ましい。 Note that the size of the matching target of both the geometric shape data and the map data may be changed. However, since the size setting control is complicated, it is preferable to perform only one.
０１０１、０２０１…ロボット、移動体、０１０２…コントローラ部、０１０３…距離センサ部、０１０４…距離センサ制御部、０１０５…距離センサ誤差低減部、０１０６…位置姿勢推定部、０１０７…経路計画部、０１０８…移動機構制御部、０１０９…移動機構制御部、０１１０…距離センサ設置データ、０１１１…距離センサスポット光サイズデータ、０１１２…地図データ、０１１３…経路グラフデータ、０７１７、０８０７…マッチング対象。 0101, 0201 ... Robot, moving object, 0102 ... Controller unit, 0103 ... Distance sensor unit, 0104 ... Distance sensor control unit, 0105 ... Distance sensor error reduction unit, 0106 ... Position and orientation estimation unit, 0107 ... Path planning unit, 0108 ... Moving mechanism control unit, 0109 ... Moving mechanism control unit, 0110 ... Distance sensor installation data, 0111 ... Distance sensor spot light size data, 0112 ... Map data, 0113 ... Route graph data, 0717, 0807 ... Matching target.
Claims (6)
前記距離センサの距離データを環境の幾何形状データに変換する距離センサ制御部と、
計測点の前記距離センサからの距離に応じて、前記距離センサ制御部で得られた前記幾何形状データと前記地図データの一方のマッチング対象のサイズを設定し、前記計測点の前記距離センサからの距離が長いほど前記マッチング対象のサイズを大きく設定する距離センサ誤差低減部と、
前記距離センサ誤差低減部で設定されたマッチング対象のサイズで前記幾何形状データと前記地図データをマッチング処理して前記移動体の位置と姿勢を推定する位置姿勢推定部を備えたことを特徴とする移動体。 The environment around the moving object is measured by the distance sensor, and the position and orientation of the moving object on the map data are estimated by comparing the measured distance data with the map data stored in advance, and based on the estimation result In the moving body that moves to the destination
A distance sensor control unit for converting the distance data of the distance sensor into the geometric data of the environment;
In accordance with the distance from the range sensor measurement points, to set the size of one of the matching target of the map data and the geometry data obtained by the distance sensor control unit, from the distance sensor of the measuring points A distance sensor error reduction unit that sets a larger size of the matching target as the distance is longer ;
Characterized by comprising a position and orientation estimation unit for estimating the position and orientation of the moving body the geometry data and the map data by matching the size of said distance sensor matching target set by the error reducing unit Moving body.
前記距離センサの距離データを環境の幾何形状データに変換する距離センサ制御部と、
計測点の前記距離センサからの距離に応じて、前記距離センサ制御部で得られた前記幾何形状データと前記地図データの一方のマッチング対象のサイズを設定し、前記距離センサ制御部で得られた前記幾何形状データのマッチング対象の形状を前記距離センサの誤差に合わせて楕円形状に設定する距離センサ誤差低減部と、
前記距離センサ誤差低減部で設定されたマッチング対象のサイズで前記幾何形状データと前記地図データをマッチング処理して前記移動体の位置と姿勢を推定する位置姿勢推定部を備えたことを特徴とする移動体。 The environment around the moving object is measured by the distance sensor, and the position and orientation of the moving object on the map data are estimated by comparing the measured distance data with the map data stored in advance, and based on the estimation result In the moving body that moves to the destination
A distance sensor control unit for converting the distance data of the distance sensor into the geometric data of the environment;
According to the distance of the measurement point from the distance sensor, the size of one of the matching objects of the geometric shape data and the map data obtained by the distance sensor control unit is set, and obtained by the distance sensor control unit A distance sensor error reducing unit that sets a shape to be matched of the geometric shape data to an elliptical shape in accordance with an error of the distance sensor;
A position / orientation estimation unit that estimates the position and orientation of the moving body by performing a matching process on the geometric shape data and the map data with a matching target size set by the distance sensor error reduction unit. Moving body.
前記距離センサの距離データを環境の幾何形状データに変換する距離センサ制御部と、
計測点の前記距離センサからの距離に応じて、前記距離センサ制御部で得られた前記幾何形状データと前記地図データの一方のマッチング対象のサイズを設定し、前記移動体の周囲の環境の床面による計測誤差を含むと考えられる計測点を前記幾何形状データから除去する距離センサ誤差低減部と、
前記距離センサ誤差低減部で設定されたマッチング対象のサイズで前記幾何形状データと前記地図データをマッチング処理して前記移動体の位置と姿勢を推定する位置姿勢推定部を備えたことを特徴とする移動体。 The environment around the moving object is measured by the distance sensor, and the position and orientation of the moving object on the map data are estimated by comparing the measured distance data with the map data stored in advance, and based on the estimation result In the moving body that moves to the destination
A distance sensor control unit for converting the distance data of the distance sensor into the geometric data of the environment;
According to the distance of the measurement point from the distance sensor, the size of one of the geometric shape data and the map data obtained by the distance sensor control unit is set, and the environment floor around the moving body is set. A distance sensor error reducing unit that removes measurement points considered to include measurement errors due to a surface from the geometric shape data;
A position / orientation estimation unit that estimates the position and orientation of the moving body by performing a matching process on the geometric shape data and the map data with a matching target size set by the distance sensor error reduction unit. Moving body.
距離センサ制御部により距離センサの距離データを環境の幾何形状データに変換し、The distance sensor control unit converts the distance data of the distance sensor into the geometric data of the environment,
距離センサ誤差低減部により前記で得られた幾何形状データと地図データの一方のマッチング対象のサイズを計測点の前記距離センサからの距離に応じて設定し、前記計測点の前記距離センサからの距離が長いほど前記マッチング対象のサイズを大きく設定し、The size of one of the geometric shape data and the map data obtained by the distance sensor error reduction unit is set according to the distance from the distance sensor of the measurement point, and the distance of the measurement point from the distance sensor The longer the is, the larger the size of the matching target is set,
位置姿勢推定部により前記で設定されたマッチング対象のサイズで前記幾何形状データと前記地図データをマッチング処理して前記移動体の位置と姿勢を推定することを特徴とする移動体の制御方法。A method for controlling a moving body, wherein the position and orientation of the moving body is estimated by performing a matching process on the geometric shape data and the map data with the size of the matching target set as described above by a position and orientation estimation unit.
距離センサ制御部により距離センサの距離データを環境の幾何形状データに変換し、The distance sensor control unit converts the distance data of the distance sensor into the geometric data of the environment,
距離センサ誤差低減部により前記で得られた幾何形状データと地図データの一方のマッチング対象のサイズを計測点の前記距離センサからの距離に応じて設定し、前記距離センサ制御部で得られた前記幾何形状データのマッチング対象の形状を前記距離センサの誤差に合わせて楕円形状に設定し、The size of one of the geometric shape data and the map data obtained by the distance sensor error reduction unit is set according to the distance from the distance sensor of the measurement point, and the distance sensor control unit obtains the size Set the shape to be matched in the geometric data to an elliptical shape according to the error of the distance sensor,
位置姿勢推定部により前記で設定されたマッチング対象のサイズで前記幾何形状データと前記地図データをマッチング処理して前記移動体の位置と姿勢を推定することを特徴とする移動体の制御方法。A method for controlling a moving body, wherein the position and orientation of the moving body is estimated by performing a matching process on the geometric shape data and the map data with the size of the matching target set as described above by a position and orientation estimation unit.
距離センサ制御部により距離センサの距離データを環境の幾何形状データに変換し、The distance sensor control unit converts the distance data of the distance sensor into the geometric data of the environment,
距離センサ誤差低減部により前記で得られた幾何形状データと地図データの一方のマッチング対象のサイズを計測点の前記距離センサからの距離に応じて設定し、前記移動体の周囲の環境の床面による計測誤差を含むと考えられる計測点を前記幾何形状データから除去し、The size of one of the geometric shape data and the map data obtained by the distance sensor error reduction unit is set according to the distance from the distance sensor of the measurement point, and the floor surface of the environment around the moving body The measurement points that are considered to contain measurement errors due to are removed from the geometric data,
位置姿勢推定部により前記で設定されたマッチング対象のサイズで前記幾何形状データと前記地図データをマッチング処理して前記移動体の位置と姿勢を推定することを特徴とする移動体の制御方法。A method for controlling a moving body, wherein the position and orientation of the moving body is estimated by performing a matching process on the geometric shape data and the map data with the size of the matching target set as described above by a position and orientation estimation unit.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2011145585A JP5763986B2 (en) | 2011-06-30 | 2011-06-30 | MOBILE BODY AND METHOD FOR CONTROLLING MOBILE BODY |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2011145585A JP5763986B2 (en) | 2011-06-30 | 2011-06-30 | MOBILE BODY AND METHOD FOR CONTROLLING MOBILE BODY |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2013012128A JP2013012128A (en) | 2013-01-17 |
| JP5763986B2 true JP5763986B2 (en) | 2015-08-12 |
Family
ID=47685940
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2011145585A Expired - Fee Related JP5763986B2 (en) | 2011-06-30 | 2011-06-30 | MOBILE BODY AND METHOD FOR CONTROLLING MOBILE BODY |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP5763986B2 (en) |
Families Citing this family (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2015055906A (en) * | 2013-09-10 | 2015-03-23 | 株式会社日立産機システム | Position detection device for outputting control command to travel control means of moving body and moving body system |
| JP6548452B2 (en) * | 2015-05-26 | 2019-07-24 | 株式会社日立産機システム | Map generation apparatus and map generation method |
| JP6647125B2 (en) * | 2016-04-15 | 2020-02-14 | 株式会社日立産機システム | Position identification system |
| JP6711138B2 (en) | 2016-05-25 | 2020-06-17 | 村田機械株式会社 | Self-position estimating device and self-position estimating method |
| CN113677412B (en) * | 2019-04-08 | 2023-10-10 | 索尼集团公司 | Information processing device, information processing method, and program |
| DE102019129600B4 (en) * | 2019-11-04 | 2023-11-02 | Evitado Technologies GmbH | Portable sensor system |
Family Cites Families (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP5141507B2 (en) * | 2008-08-25 | 2013-02-13 | 村田機械株式会社 | Autonomous mobile device |
| JP2010238217A (en) * | 2009-03-09 | 2010-10-21 | Yaskawa Electric Corp | Self-localization method of mobile robot and the mobile robot |
| JP5216690B2 (en) * | 2009-06-01 | 2013-06-19 | 株式会社日立製作所 | Robot management system, robot management terminal, robot management method and program |
| JP5304534B2 (en) * | 2009-08-21 | 2013-10-02 | トヨタ自動車株式会社 | Self-position estimation apparatus, self-position estimation method and program |
-
2011
- 2011-06-30 JP JP2011145585A patent/JP5763986B2/en not_active Expired - Fee Related
Also Published As
| Publication number | Publication date |
|---|---|
| JP2013012128A (en) | 2013-01-17 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP5763986B2 (en) | MOBILE BODY AND METHOD FOR CONTROLLING MOBILE BODY | |
| JP5278283B2 (en) | Autonomous mobile device and control method thereof | |
| US9939529B2 (en) | Robot positioning system | |
| JP3865121B2 (en) | Vehicle obstacle detection device | |
| JP4241651B2 (en) | Mobile device | |
| US9242378B2 (en) | System and method for determing necessity of map data recreation in robot operation | |
| US10747229B2 (en) | Environment arrangement robot and control program thereof | |
| JP5105596B2 (en) | Travel route determination map creation device and travel route determination map creation method for autonomous mobile body | |
| JP5276931B2 (en) | Method for recovering from moving object and position estimation error state of moving object | |
| JP5802423B2 (en) | Moving body | |
| JP6074205B2 (en) | Autonomous mobile | |
| JP5800613B2 (en) | Position / posture estimation system for moving objects | |
| JP2010079869A (en) | Autonomous moving apparatus | |
| JP2011027598A (en) | Environment recognition device and moving body with the same | |
| JP6348971B2 (en) | Moving body | |
| JP2017120551A (en) | Autonomous traveling device | |
| JP2022530246A (en) | Simultaneous execution of self-position estimation and environmental map creation | |
| JP2021163455A (en) | Position estimation system | |
| JP2018155694A (en) | Concave obstacle detection device and method | |
| JP4745159B2 (en) | Mobile robot | |
| CN113490973B (en) | Information processing device and mobile robot | |
| JP2010026727A (en) | Autonomous moving device | |
| JP6895074B2 (en) | Object detection system and object detection program | |
| JP6192506B2 (en) | Moving body | |
| JP5644673B2 (en) | Perimeter monitoring device, perimeter monitoring method, and driving support device |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20140207 |
|
| RD02 | Notification of acceptance of power of attorney |
Free format text: JAPANESE INTERMEDIATE CODE: A7422 Effective date: 20141027 |
|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20141128 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20141202 |
|
| A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20150128 |
|
| 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: 20150609 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20150612 |
|
| R150 | Certificate of patent or registration of utility model |
Ref document number: 5763986 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
| LAPS | Cancellation because of no payment of annual fees |