JP6782903B2

JP6782903B2 - Self-motion estimation system, control method and program of self-motion estimation system

Info

Publication number: JP6782903B2
Application number: JP2015255056A
Authority: JP
Inventors: 正裕友納
Original assignee: Chiba Institute of Technology
Current assignee: Chiba Institute of Technology
Priority date: 2015-12-25
Filing date: 2015-12-25
Publication date: 2020-11-11
Anticipated expiration: 2035-12-25
Also published as: JP2017117386A

Description

本発明は、自己運動推定システム、自己運動推定システムの制御方法及びプログラムに関する。 The present invention relates to a self-motion estimation system, a control method and a program of the self-motion estimation system.

近年、外界の状況をリアルタイムに判断しながら、自らの運動を自律的に制御可能な自律移動ロボットの開発が急速に進められている。 In recent years, the development of autonomous mobile robots capable of autonomously controlling their own movements while judging the situation of the outside world in real time has been rapidly promoted.

そしてこのための技術として、ロボットに外界を認識するためのカメラなどのセンサを搭載し、センサから得られる情報をロボット自らがリアルタイムに解析することで、ロボットを取り巻く外界の３次元地図を作製しつつ自らの運動状態を推定する技術が開発されている（例えば特許文献１、非特許文献１、非特許文献２参照）。 As a technology for this, the robot is equipped with sensors such as a camera to recognize the outside world, and the robot itself analyzes the information obtained from the sensors in real time to create a three-dimensional map of the outside world surrounding the robot. At the same time, a technique for estimating one's own motion state has been developed (see, for example, Patent Document 1, Non-Patent Document 1, and Non-Patent Document 2).

特開２００８−１９７８８４号公報Japanese Unexamined Patent Publication No. 2008-197884

J.Zhang and S.Singh:Visual-lidar Odometry and Mapping, Low-drift, Robust, and Fast, ICRA2015,pp.2174-2181,2015.J.Zhang and S.Singh: Visual-lidar Odometry and Mapping, Low-drift, Robust, and Fast, ICRA2015, pp.2174-2181, 2015. M.Tomono:Robust 3DSLAM with a Stereo Camera Based on an Edge-Point ICP Algorithm, Proc. of ICRA2009,pp.4306-4311,2009.M.Tomono: Robust 3DSLAM with a Stereo Camera Based on an Edge-Point ICP Algorithm, Proc. Of ICRA2009, pp.4306-4311, 2009.

しかしながらこの場合、ロボットは休みなく膨大な情報を取り込んで高速に処理する必要があるため、これらの膨大な情報をいかに効率的に処理できるかが重要であり、しかも、いかに正確に外界の状況及び自己の運動状態を推定できるかが、より高度な自律移動制御を実現するための鍵となる。 However, in this case, since the robot needs to take in a huge amount of information without rest and process it at high speed, it is important how efficiently the huge amount of information can be processed, and how accurately the situation of the outside world and how accurately it can be processed. Being able to estimate one's own motor state is the key to realizing more advanced autonomous movement control.

またこのような技術は、例えば自転車や自動車、人をも含むロボット以外の広義の移動体に適用して、自律移動制御以外の様々な分野（例えば地図作成の支援など）への応用も期待されている。 In addition, such technology is expected to be applied to mobile objects in a broad sense other than robots including bicycles, automobiles, and humans, and to be applied to various fields other than autonomous movement control (for example, support for map creation). ing.

本発明はこのような点を鑑みてなされたものであり、より効率的かつ正確に外界を観測しつつリアルタイムに自己の運動状態を推定可能な自己運動推定システムを提供することを一つの目的とする。 The present invention has been made in view of these points, and one object of the present invention is to provide a self-motion estimation system capable of estimating one's own motion state in real time while observing the outside world more efficiently and accurately. To do.

一つの側面に係る自己運動推定システムは、移動体に搭載され、外界の存在物との相対的な位置の変化を検出することにより、前記移動体の運動状態を推定する自己運動推定システムであって、前記移動体に搭載される画像センサによって所定周期で撮影される外界の明暗画像を取得する明暗画像取得部と、前記移動体に搭載される距離センサによって所定周期で計測される、前記画像センサの撮影範囲に含まれる外界の各点までの複数の距離情報を有する距離画像を取得する距離画像取得部と、前記明暗画像からエッジを検出するエッジ検出部と、第１タイミングで取得した前記距離画像と前記明暗画像とを照合し、前記明暗画像におけるエッジの位置に対応する前記距離画像内の距離情報を、エッジ位置距離情報として特定するエッジ位置距離情報特定部と、前記第１タイミングにおける前記エッジ位置距離情報と、前記第１タイミングから第２タイミングまでの間の所定時間における前記移動体の推定移動量と、を用いて算出される、前記エッジ位置距離情報の前記第２タイミングにおける明暗画像内での透視変換による投影位置が、前記第２タイミングで取得される前記明暗画像から検出されるエッジの位置に近づくように、前記移動体の前記推定移動量を算出する移動量算出部と、を備える。 The self-motion estimation system according to one aspect is a self-motion estimation system that is mounted on a moving body and estimates the motion state of the moving body by detecting a change in position relative to an object in the outside world. The image is measured in a predetermined cycle by a light / dark image acquisition unit that acquires a light / dark image of the outside world captured by an image sensor mounted on the moving body and a distance sensor mounted on the moving body. A distance image acquisition unit that acquires a distance image having a plurality of distance information to each point in the outside world included in the imaging range of the sensor, an edge detection unit that detects an edge from the bright and dark image, and the acquisition at the first timing. The edge position distance information specifying unit that collates the distance image with the light and dark image and specifies the distance information in the distance image corresponding to the edge position in the light and dark image as the edge position distance information, and the first timing. The brightness and darkness of the edge position and distance information at the second timing calculated by using the edge position and distance information and the estimated movement amount of the moving body in a predetermined time between the first timing and the second timing. A movement amount calculation unit that calculates the estimated movement amount of the moving body so that the projection position by the fluoroscopic conversion in the image approaches the position of the edge detected from the light and dark image acquired at the second timing. , Equipped with.

その他、本願が開示する課題、及びその解決方法は、発明を実施するための形態の欄の記載、及び図面の記載等により明らかにされる。 In addition, the problems disclosed in the present application and the solutions thereof will be clarified by the description in the column of the mode for carrying out the invention, the description in the drawings, and the like.

本発明によれば、より効率的かつ正確に外界を観測しつつリアルタイムに自己の運動状態を推定可能な自己運動推定システムを提供することが可能となる。 According to the present invention, it is possible to provide a self-motion estimation system capable of estimating a self-motion state in real time while observing the outside world more efficiently and accurately.

本実施形態に係る移動体及び自己運動推定システムの構成を示す図である。It is a figure which shows the structure of the moving body and self-motion estimation system which concerns on this embodiment. 本実施形態に係る明暗画像を示す図である。It is a figure which shows the light-dark image which concerns on this embodiment. 本実施形態に係るエッジ画像を示す図である。It is a figure which shows the edge image which concerns on this embodiment. 本実施形態に係る距離画像を示す図である。It is a figure which shows the distance image which concerns on this embodiment. 本実施形態に係るＲＧＢＤ特徴ペアを示す図である。It is a figure which shows the RGBD feature pair which concerns on this embodiment. 本実施形態に係る自己運動推定システムの処理の流れを示すフローチャートである。It is a flowchart which shows the process flow of the self-motion estimation system which concerns on this embodiment.

本明細書および添付図面の記載により、少なくとも以下の事項が明らかとなる。 The description of the present specification and the accompanying drawings will clarify at least the following matters.

本発明の実施形態に係る自己運動推定システム１００について、図面を参照しながら説明する。 The self-motion estimation system 100 according to the embodiment of the present invention will be described with reference to the drawings.

本実施形態に係る自己運動推定システム１００は、例えば自律移動ロボットのような移動体１０００に搭載され、移動体１０００から観察される外界の存在物の相対的な位置の変化を検出することにより、移動体１０００の運動状態を推定する機能を有する装置である。 The self-motion estimation system 100 according to the present embodiment is mounted on a moving body 1000 such as an autonomous mobile robot, and detects a change in the relative position of an object in the outside world observed from the moving body 1000. It is a device having a function of estimating the motion state of the moving body 1000.

ここでの運動とは、移動体１０００の位置姿勢の時系列を意味する。移動体１０００の位置姿勢とは、たとえば、移動体１０００が３次元空間を移動する場合は、所定の３次元座標系での位置座標および方位角である。 The motion here means a time series of the position and posture of the moving body 1000. The position / orientation of the moving body 1000 is, for example, the position coordinates and the azimuth in a predetermined three-dimensional coordinate system when the moving body 1000 moves in a three-dimensional space.

なお詳細は後述するが、本実施形態に係る自己運動推定システム１００は、移動体１０００の移動中にリアルタイムに周囲の３次元地図の生成も行っている。 Although details will be described later, the self-motion estimation system 100 according to the present embodiment also generates a three-dimensional map of the surroundings in real time while the moving body 1000 is moving.

＜移動体＞
本実施形態に係る移動体１０００は、例えば自動運転車、ヒューマノイドロボット、パーソナルモビリティなど、自らの判断で自らの運動を自律的に制御することが可能な装置である。 <Mobile>
The mobile body 1000 according to the present embodiment is a device capable of autonomously controlling its own movement at its own discretion, such as an autonomous driving vehicle, a humanoid robot, and a personal mobility.

図１に示すように、移動体１０００は、自己運動推定システム１００、距離センサ２００、画像センサ３００、運動制御部４００、アクチュエータ５００を備えて構成される。 As shown in FIG. 1, the moving body 1000 includes a self-motion estimation system 100, a distance sensor 200, an image sensor 300, a motion control unit 400, and an actuator 500.

画像センサ３００は、外界の光を検出するセンサである。本実施形態に係る画像センサ３００は、レンズを通して集光した外界の光を、２次元平面状に配置した複数の受光素子に照射し、画素ごとにＲＧＢ(Red,Green,Blue)の各色の明るさを検出することによりカラー画像を取得可能なカメラである。もちろん画像センサ３００は単色（白黒）のカメラであっても良いし、赤外線や紫外線などの可視光以外の電磁波を検知するカメラであっても良い。画像センサ３００は、所定周期（例えば0.1秒周期ないし0.01秒周期）に外界の様子を撮影する。なお周期は移動体１０００の移動速度に応じて選べばよい。例えば0.1秒より遅い場合も0.01秒より速い場合もありうる。画像センサ３００がタイミングｔで外界を撮影して得られた画像を明暗画像Ｉｔとも記す。明暗画像Ｉｔの一例を図２に模式的に示す。 The image sensor 300 is a sensor that detects light in the outside world. The image sensor 300 according to the present embodiment irradiates a plurality of light receiving elements arranged in a two-dimensional plane with the light of the outside world collected through the lens, and the brightness of each color of RGB (Red, Green, Blue) for each pixel. It is a camera that can acquire a color image by detecting the brightness. Of course, the image sensor 300 may be a monochromatic (black and white) camera, or may be a camera that detects electromagnetic waves other than visible light such as infrared rays and ultraviolet rays. The image sensor 300 captures the state of the outside world at a predetermined cycle (for example, a cycle of 0.1 second to a cycle of 0.01 second). The cycle may be selected according to the moving speed of the moving body 1000. For example, it may be slower than 0.1 seconds or faster than 0.01 seconds. The image obtained by the image sensor 300 photographing the outside world at the timing t is also referred to as a light / dark image It. An example of the light / dark image It is schematically shown in FIG.

距離センサ２００は、計測範囲内（計測対象となる視野の範囲内）に存在する複数の地点までの距離をレーザ光により計測するセンサである。本実施形態に係る距離センサ２００は、一例として、計測対象地点に向けて出射したレーザパルスが往復する時間を計測することで計測対象地点までの距離を計測し、計測した距離をレーザパルスの出射方向を示す情報と共に出力可能な３Ｄレーザスキャナである。レーザスキャナはレーザ光を機械的に回転させて複数の計測対象地点までの距離を逐次的に計測するため、一般に、広い視野を確保できる。レーザスキャナには、往復時間計測を用いるものの他に、レーザ光の送信信号と受信信号の位相差を用いたり、レーザ光の出射点と受光点間の距離に基づく三角法を用いるものがあり、距離センサ２００はそれらのどれでもよい。またレーザスキャナの他に、複数の受光素子を用いて複数地点の距離を同時に測定する距離画像カメラを用いてもよい。ここでは、代表としてレーザスキャナを扱うこととする。 The distance sensor 200 is a sensor that measures the distances to a plurality of points existing within the measurement range (within the range of the visual field to be measured) by laser light. As an example, the distance sensor 200 according to the present embodiment measures the distance to the measurement target point by measuring the reciprocating time of the laser pulse emitted toward the measurement target point, and emits the measured distance as the laser pulse. It is a 3D laser scanner that can output with information indicating the direction. Since the laser scanner mechanically rotates the laser beam to sequentially measure the distances to a plurality of measurement target points, a wide field of view can generally be secured. Laser scanners include those that use round-trip time measurement, those that use the phase difference between the transmission signal and reception signal of the laser light, and those that use the trigonometry based on the distance between the emission point and the light reception point of the laser light. The distance sensor 200 may be any of them. In addition to the laser scanner, a distance image camera that simultaneously measures the distances of a plurality of points using a plurality of light receiving elements may be used. Here, a laser scanner will be treated as a representative.

そして本実施形態に係る距離センサ２００は、距離の計測範囲（視野の範囲）が画像センサ３００の撮影範囲（視野の範囲）の一部あるいは全部を含むように取り付け位置が調整されて移動体１０００に装着されている。つまり本実施形態に係る距離センサ２００は、画像センサ３００の撮影範囲に含まれる外界の各点までの距離が計測されるような向きに移動体１０００に装着されている。 The distance sensor 200 according to the present embodiment is adjusted in mounting position so that the distance measurement range (field of view range) includes a part or all of the shooting range (field of view range) of the image sensor 300, and the moving body 1000 It is attached to. That is, the distance sensor 200 according to the present embodiment is mounted on the moving body 1000 in a direction in which the distance to each point in the outside world included in the photographing range of the image sensor 300 is measured.

そして、画像センサ３００及び距離センサ２００は、画像センサ３００が撮影するタイミングと、その撮影範囲内の各点までの距離を距離センサ２００が計測するタイミングとが揃うように、撮影タイミング及び計測タイミングが調整されている。ここでのタイミングは、実際の測定時刻とは別のものであり、データの通し番号に相当する離散時刻である。なお、各データの測定時刻は自己運動推定の要求精度に応じて合せる必要があるが、高精度が要求されない場合は厳密に合せなくてもよい。実際の測定時刻は別途記録しておき、データの歪み補正（後述）に用いてもよい。 Then, in the image sensor 300 and the distance sensor 200, the shooting timing and the measurement timing are set so that the timing at which the image sensor 300 shoots and the timing at which the distance sensor 200 measures the distance to each point within the shooting range are aligned. It has been adjusted. The timing here is different from the actual measurement time, and is a discrete time corresponding to the serial number of the data. The measurement time of each data needs to be adjusted according to the required accuracy of self-motion estimation, but it is not necessary to exactly adjust it when high accuracy is not required. The actual measurement time may be recorded separately and used for data distortion correction (described later).

自己運動推定システム１００は、移動体１０００に搭載され、外界の存在物との相対的な位置の変化を所定時間ごとに検出することにより、移動体１０００の運動状態を移動体１０００の移動中に推定する。詳細は後述する。 The self-motion estimation system 100 is mounted on the mobile body 1000, and detects a change in the position relative to an object in the outside world at predetermined time intervals to change the motion state of the mobile body 1000 during the movement of the mobile body 1000. presume. Details will be described later.

運動制御部４００は、自己運動推定システム１００により推定された移動体１０００の所定時間毎の移動量や、距離センサ２００により得られる距離測定データ（後述）から作成される３次元地図を用いて、移動体１０００を移動させるためのアクチュエータ５００を駆動する。 The motion control unit 400 uses a three-dimensional map created from the movement amount of the moving body 1000 estimated by the self-motion estimation system 100 at predetermined time intervals and the distance measurement data (described later) obtained by the distance sensor 200. The actuator 500 for moving the moving body 1000 is driven.

アクチュエータ５００は、移動体１０００を移動させるためのモータやバルブ、エンジンなどの動力発生装置である。 The actuator 500 is a power generator such as a motor, a valve, or an engine for moving the moving body 1000.

＜自己運動推定システムの構成＞
自己運動推定システム１００は、図１に示すように、距離画像取得部１１１、距離測定データ記憶部１１２、明暗画像取得部１２１、エッジ検出部１２２、特徴ペア生成部１３１、特徴ペア記憶部１３２、移動量算出部１４１、移動量記憶部１４２、３次元地図生成部１５０を備える。 <Structure of self-motion estimation system>
As shown in FIG. 1, the self-motion estimation system 100 includes a distance image acquisition unit 111, a distance measurement data storage unit 112, a light / dark image acquisition unit 121, an edge detection unit 122, a feature pair generation unit 131, and a feature pair storage unit 132. It includes a movement amount calculation unit 141, a movement amount storage unit 142, and a three-dimensional map generation unit 150.

なお、自己運動推定システム１００は、不図示のＣＰＵ（Central Processing Unit）、ＲＡＭ(Random Access Memory)やＲＯＭ(Read Only Memory)、ハードディスク装置などの記憶装置、各種のデータ入出力インタフェース等の機器を有するコンピュータにより構成される。そしてＣＰＵが、記憶装置に記憶されているプログラムを実行することにより、本実施形態に係る自己運動推定システム１００が有する各種機能が実現される。 The self-motion estimation system 100 includes a CPU (Central Processing Unit), a RAM (Random Access Memory), a ROM (Read Only Memory), a storage device such as a hard disk device, and various data input / output interfaces (not shown). It consists of a computer that has. Then, when the CPU executes the program stored in the storage device, various functions of the self-motion estimation system 100 according to the present embodiment are realized.

また自己運動推定システム１００は、データ入出力インタフェースを通じて、距離センサ２００や画像センサ３００などの外部機器からデータを取得する。またプログラムがＣＤＲＯＭやＤＶＤ等の記録媒体に記録されている場合には、これらの記録媒体からプログラムを記憶装置にロードすることができる。あるいは、プログラムが他のコンピュータに格納されている場合には、インターネット等の通信ネットワークを介してこのコンピュータからプログラムを記憶装置にダウンロードすることができる。 Further, the self-motion estimation system 100 acquires data from an external device such as a distance sensor 200 or an image sensor 300 through a data input / output interface. When the program is recorded on a recording medium such as a CDROM or a DVD, the program can be loaded into the storage device from these recording media. Alternatively, if the program is stored in another computer, the program can be downloaded from this computer to the storage device via a communication network such as the Internet.

距離画像取得部１１１は、移動体１０００に搭載される距離センサ２００によって所定周期（例えば0.1秒毎）で計測される物理量（レーザ光の往復時間や位相差、角度など）から算出される、画像センサ３００の撮影範囲（視野）に含まれる外界の各計測点までの距離情報とレーザ光の向きからなる距離測定データを取得し、この距離測定データから距離画像Ｐｔを生成する。距離画像Ｐｔは、たとえば、レーザ光の向きから得られる計測点の方位角(θ,φ)を画像上の画素位置に対応づけ、距離センサ２００から計測点までの直線距離を画素値として生成する。この方位角と距離情報から計測点の３次元位置の座標値(x,y,z)も計算できる。以下では、このような３次元位置で表された計測点（３Ｄ点）の集合も便宜的に距離画像と呼ぶ。このような距離画像では、同じ画素位置（方位角）に距離の異なる複数の３Ｄ点が登録されてもよい。なお、距離センサ２００が出力する計測点の距離、方位角、および、そこから得られる位置は、すべて距離センサ２００に付随した３次元座標系での値であり、自己運動を推定する３次元座標系（世界座標系）とは異なる。 The distance image acquisition unit 111 is an image calculated from physical quantities (reciprocating time of laser light, phase difference, angle, etc.) measured at a predetermined cycle (for example, every 0.1 seconds) by a distance sensor 200 mounted on the moving body 1000. Distance measurement data consisting of distance information to each measurement point in the outside world included in the imaging range (field of view) of the sensor 300 and the direction of the laser beam is acquired, and a distance image Pt is generated from this distance measurement data. In the distance image Pt, for example, the azimuth angle (θ, φ) of the measurement point obtained from the direction of the laser beam is associated with the pixel position on the image, and the linear distance from the distance sensor 200 to the measurement point is generated as the pixel value. .. From this azimuth and distance information, the coordinate values (x, y, z) of the three-dimensional position of the measurement point can also be calculated. In the following, a set of measurement points (3D points) represented by such three-dimensional positions will also be referred to as a distance image for convenience. In such a distance image, a plurality of 3D points having different distances may be registered at the same pixel position (azimuth). The distance, azimuth, and position obtained from the measurement points output by the distance sensor 200 are all values in the three-dimensional coordinate system attached to the distance sensor 200, and are three-dimensional coordinates for estimating self-motion. It is different from the system (world coordinate system).

距離画像Ｐｔの計測タイミングは、距離測定データの測定時刻を用いて、距離画像取得部１１１によって適宜生成される。例えば距離センサ２００が３６０度回転可能なセンサである場合、０度の位置から３６０度の位置までの１回転の間は同じ計測タイミングとなるように、生成される。つまり、１周分または１回の撮影分のデータを同じ離散時刻で扱う。離散時刻は順序を表す整数であり、添え字のtである。 The measurement timing of the distance image Pt is appropriately generated by the distance image acquisition unit 111 using the measurement time of the distance measurement data. For example, when the distance sensor 200 is a sensor that can rotate 360 degrees, it is generated so that the measurement timing is the same during one rotation from the 0 degree position to the 360 degree position. That is, the data for one lap or one shooting is handled at the same discrete time. Discrete time is an integer representing the order and is the subscript t.

なお以下の説明では、距離画像Ｐｔに含まれる各点を３次元座標値(x,y,z)で表したものを３Ｄ点と記し，この３Ｄ点を用いて処理の詳細を説明する。ここで、時刻ｔにおけるi番目の３Ｄ点をp_tiと表す。pは３次元座標値であり、添え字のtは計測タイミングを表し、添え字のiは識別番号である。距離画像Ｐｔの一例を図４に示す。図４に示す距離画像Ｐｔは、図２に示した明暗画像Ｉｔを撮影したタイミングｔと同じタイミングｔで距離を計測した結果を表す。なお図４において、各点のドットの大きさは各点までの距離を表しており、距離センサ２００からの距離が近いほど、ドットのサイズが大きくなるように表現されている。ただし、各点までの距離をドットの大きさで表すこの表現はここでの便宜上のものである。 In the following description, each point included in the distance image Pt is represented by a three-dimensional coordinate value (x, y, z) as a 3D point, and the details of the process will be described using the 3D point. Here, the i-th 3D point at time t is represented as p _ti . p is a three-dimensional coordinate value, the subscript t represents the measurement timing, and the subscript i is the identification number. An example of the distance image Pt is shown in FIG. The distance image Pt shown in FIG. 4 represents the result of measuring the distance at the same timing t as when the light / dark image It shown in FIG. 2 was captured. In FIG. 4, the size of the dots at each point represents the distance to each point, and the closer the distance from the distance sensor 200, the larger the dot size. However, this expression, which expresses the distance to each point by the size of dots, is for convenience here.

距離測定データ記憶部１１２は、距離センサ２００が計測した距離測定データを、計測タイミングと対応付けて記憶する。詳細は後述するが、記憶測定データ記憶部１１２が記憶する距離測定データは、３次元地図生成部１５０が３次元地図を作成する際に用いられる。 The distance measurement data storage unit 112 stores the distance measurement data measured by the distance sensor 200 in association with the measurement timing. Although the details will be described later, the distance measurement data stored in the storage measurement data storage unit 112 is used when the three-dimensional map generation unit 150 creates a three-dimensional map.

明暗画像取得部１２１は、移動体１０００に搭載される画像センサ３００によって所定周期（例えば0.1秒毎）で撮影される外界の明暗画像Ｉｔを取得する。明暗画像Ｉｔには、撮影タイミングを表す情報と、明暗画像Ｉｔ内の各点の２次元座標を表す情報と、各点の明るさを示す情報と、が含まれる。撮影タイミングは、明暗画像取得部１２１が、例えば時刻情報を用いて生成する。 The light / dark image acquisition unit 121 acquires the light / dark image It of the outside world taken at a predetermined cycle (for example, every 0.1 seconds) by the image sensor 300 mounted on the moving body 1000. The light / dark image It includes information indicating the shooting timing, information indicating the two-dimensional coordinates of each point in the light / dark image It, and information indicating the brightness of each point. The shooting timing is generated by the light / dark image acquisition unit 121 using, for example, time information.

距離画像取得部１１１が生成する計測タイミングと、明暗画像取得部１２１が生成する撮影タイミングは、同じタイミングであれば同じ値になるように適宜生成される。 The measurement timing generated by the distance image acquisition unit 111 and the shooting timing generated by the light / dark image acquisition unit 121 are appropriately generated so as to have the same value if they are the same timing.

ここで、本実施形態に係る自己運動推定システム１００の他の構成要素及び処理内容について順に説明する前に、説明を補足する。 Here, the description will be supplemented before the other components and the processing contents of the self-motion estimation system 100 according to the present embodiment are described in order.

まず、以下の説明では、簡単のため、距離センサ２００の座標系と、移動体１０００の座標系を同一視する。そしてその座標系は、x軸が移動体１０００の前方、y軸が左方、z軸が上方とする。これは説明の都合上定めたものであり、実際は、全体で一貫性がとれていれば座標軸をどのように定めてもよい。 First, in the following description, for the sake of simplicity, the coordinate system of the distance sensor 200 and the coordinate system of the moving body 1000 are equated. In the coordinate system, the x-axis is in front of the moving body 1000, the y-axis is on the left, and the z-axis is on the top. This is set for convenience of explanation, and in fact, the coordinate axes may be set in any way as long as they are consistent as a whole.

また明暗画像Ｉｔの内部校正、および距離センサ２００と画像センサ３００との間の外部校正はできているとする。 Further, it is assumed that the internal calibration of the light / dark image It and the external calibration between the distance sensor 200 and the image sensor 300 have been completed.

内部校正とレンズの歪み補正を合わせた行列をＡとする。また距離センサ２００と画像センサ３００の相対位置は固定であり、距離センサ２００の座標系から画像センサ３００の座標系への変換行列をＴ_ｌｃ＝[Ｒ|Ｔ]とする（Ｒは回転行列、Ｔは並進ベクトル）。 Let A be the matrix that combines the internal calibration and the distortion correction of the lens. Further, the relative positions of the distance sensor 200 and the image sensor 300 are fixed, and the transformation matrix from the coordinate system of the distance sensor 200 to the coordinate system of the image sensor 300 is T _lc = [R | T] (R is a rotation matrix, T is a translation vector).

また、時刻tの距離画像をＰｔ、明暗画像をＩｔとする。そして時刻tの距離画像Ｐｔと明暗画像Ｉｔとの組を、フレームＦｔ＝(Ｐｔ,Ｉｔ)と呼ぶ。時刻tの移動体１０００の姿勢をrtとする。また距離画像Ｐｔと画像エッジ点（後述）を対応づけたものをＲＧＢＤ特徴ペアと呼ぶ。 Further, the distance image at time t is Pt, and the light / dark image is It. The set of the distance image Pt at the time t and the light / dark image It is called a frame Ft = (Pt, It). Let rt be the posture of the moving body 1000 at time t. Further, the association between the distance image Pt and the image edge point (described later) is called an RGBD feature pair.

以下、自己運動推定システム１００の他の構成要素について説明する。 Hereinafter, other components of the self-motion estimation system 100 will be described.

エッジ検出部１２２は、明暗画像Ｉｔからエッジを検出する。具体的には、エッジ検出部１２２は、例えばCanny法（詳しくは、J. Canny: “A Computational Approach to Edge Detection,” IEEE Trans. on PAMI, Vol. 8, No. 6, pp.679-698 (1986)を参照）を用いて明暗画像Ｉｔからエッジを抽出する。そしてエッジ検出部１２２は、画像の勾配方向（エッジの法線方向）で微分値の絶対値が極大になる点を求めることで、太さ１画素のエッジ画像Ｅｔを生成する。エッジ画像Ｅｔは、エッジのある画素ｑ_ｔ，ｉが１、エッジのない画素ｑ_ｔ，ｉが０の値をとる。図２に示した明暗画像Ｉｔから生成されたエッジ画像Ｅｔの例を図３に示す。なお、明暗画像Ｉｔから抽出したエッジ点ｑ_ｔ，ｉの集合をＱｔとする。各エッジ点は画像の勾配方向から求めた法線ベクトルをもつ。 The edge detection unit 122 detects an edge from the light / dark image It. Specifically, the edge detection unit 122 uses, for example, the Canny method (for details, J. Canny: “A Computational Approach to Edge Detection,” IEEE Trans. On PAMI, Vol. 8, No. 6, pp.679-698. (See (1986)) is used to extract edges from the light-dark image It. Then, the edge detection unit 122 generates an edge image Et having a thickness of 1 pixel by finding a point at which the absolute value of the differential value becomes maximum in the gradient direction (edge normal direction) of the image. Edge image Et, the pixel _{q t} with _{edges, i} is 1, an edge-free pixel _{q t, i} has a value of 0. An example of the edge image Et generated from the light / dark image It shown in FIG. 2 is shown in FIG. Let Qt be the set of edge points qt and _i extracted from the light and dark image It. Each edge point has a normal vector obtained from the gradient direction of the image.

特徴ペア生成部１３１は、自己運動推定システム１００が同一のタイミング（第１タイミング）で取得した距離画像Ｐｔとエッジ画像Ｅｔとを照合し、エッジ画像Ｅｔにおけるエッジの位置に対応する距離画像Ｐｔ内の３Ｄ点を、エッジ位置距離情報として特定する。 The feature pair generation unit 131 collates the distance image Pt acquired by the self-motion estimation system 100 at the same timing (first timing) with the edge image Et, and in the distance image Pt corresponding to the position of the edge in the edge image Et. 3D point of is specified as edge position distance information.

具体的には、特徴ペア生成部１３１は、３Ｄ点ｐ_ｔ，ｉ∈Ｐｔを、エッジ画像Ｅｔに式（１）で透視投影し、対応する画素ｑ_ｔ，ｉを求める。ここで、各点は同次座標で表し、ｐ_ｔ，ｉ＝（Ｘ，Ｙ，Ｚ，１）^Ｔ、ｑ_ｔ，ｉ＝（ｘ，ｙ，１）^Ｔである。 Specifically, the feature pair generation unit 131 perspectively projects the 3D points pt _{, i} ∈ Pt onto the edge image Et by the equation (1), and obtains the corresponding pixels q _{t, i} . Here, each point is represented by homogeneous coordinates, and pt _{, i} = (X, Y, Z, 1) ^T , q _{t, i} = (x, y, 1) ^T.

ｗ_ｉｑ_ｔ，ｉ＝ＡＴ_ｌｃｐ_ｔ，ｉ（１）
なおｗ_ｉはスケールである。そして、エッジ画像Ｅｔのｑ_ｔ，ｉでの画素値が１ならば、ｐ_ｔ，ｉとｑ_ｔ，ｉを対応づけ、ＲＧＢＤ特徴ペアとする。その画素値が０ならば何もしない。時刻ｔにおけるＲＧＢＤ特徴ペアの集合をＣｔとする。 w _i q _{t, i} = AT _{lc pt} _{, i} (1)
It should be noted that _{w i} is a scale. Then, if the pixel value of the edge image Et at q _{t, i} is 1, pt _{, i} and q _{t, i} are associated with each other to form an RGBD feature pair. If the pixel value is 0, nothing is done. Let Ct be the set of RGBD feature pairs at time t.

図３に示すｑ_ｔ，ｉと図４に示すｐ_ｔ，ｉがＲＧＢＤ特徴ペアとして対応する。ＲＧＢＤ特徴ペアの集合の一例を図５に示す。３Ｄ点ｐ_ｔ，ｉがエッジ位置距離情報である。 The q _{t and i} shown in FIG. 3 and the pt _{and i} shown in FIG. 4 correspond as RGBD feature pairs. An example of a set of RGBD feature pairs is shown in FIG. The 3D points pt _{and i} are edge position distance information.

そして特徴ペア生成部１３１は、ＲＧＢＤ特徴ペアの集合Ｃｔを特徴ペア記憶部１３２に記憶する。 Then, the feature pair generation unit 131 stores the set Ct of RGBD feature pairs in the feature pair storage unit 132.

なお、距離センサ２００が回転型の場合は、３Ｄ点群が時々刻々と逐次的にとれる。このため、移動体１０００が移動しながら３Ｄ点群を取得すると、その形状は歪むが、この歪みの補正は公知の技術より補正することができる。 When the distance sensor 200 is a rotary type, a 3D point cloud can be obtained from moment to moment. Therefore, when the moving body 1000 acquires a 3D point cloud while moving, its shape is distorted, and the correction of this distortion can be corrected by a known technique.

移動量算出部１４１は、上記第１タイミングにおけるエッジ位置距離情報と、第１タイミングから第２タイミングまでの間の所定時間における移動体１０００の推定移動量と、を用いて算出される、エッジ位置距離情報の第２タイミングにおける明暗画像内での透視変換による投影位置が、第２タイミングで取得される明暗画像Ｉｔから検出されるエッジの位置に近づくように、移動体１０００の推定移動量を算出する。 The movement amount calculation unit 141 calculates the edge position using the edge position distance information in the first timing and the estimated movement amount of the moving body 1000 in a predetermined time between the first timing and the second timing. The estimated movement amount of the moving body 1000 is calculated so that the projection position of the distance information in the light / dark image at the second timing approaches the position of the edge detected from the light / dark image It acquired at the second timing. To do.

具体的には、移動量算出部１４１は、時刻ｔ-１（第１タイミング）におけるＲＧＢＤ特徴ペアの集合をＣｔ-１とし、時刻ｔ-１（第１タイミング）から時刻ｔ（第２タイミング）までの移動体１０００の推定移動量をｍｔ＝［Ｒｔ｜Ｔｔ］としたときに、この推定移動量ｍｔを用いてＣｔ-１の各３Ｄ点ｐ_{ｔ-１，ｉ}を、式（２）によってエッジ画像Ｅｔに投影する。 Specifically, the movement amount calculation unit 141 sets Ct-1 as the set of RGBD feature pairs at time t-1 (first timing), and from time t-1 (first timing) to time t (second timing). When the estimated movement amount of the moving body 1000 up to is mt = [Rt | Tt], each 3D point pt _{-1, i} of Ct-1 is set by the equation (2) using this estimated movement amount mt. Project to the edge image Et.

なお、図１に記載するように、移動量算出部１４１は、特徴ペア記憶部１３２からＣｔ−１を読み出して利用する。 As shown in FIG. 1, the movement amount calculation unit 141 reads and uses Ct-1 from the feature pair storage unit 132.

ｗ’_ｉｑ_ｔ，ｉ＝ＡＴ_ｌｃ［Ｒｔ｜Ｔｔ］ｐ_{ｔ-１，ｉ} （２）
そして移動量算出部１４１は、Ｅｔの中でｑ_ｔ，ｉに最も近いエッジ点ｅ_ｔ，ｉを、ｐ_{ｔ-１，ｉ}に対応づける。ただしこの際、対応づけの精度を上げるために、原画像Ｉ_ｔ-１とＩ_ｔにおいて、ｑ_{ｔ-１，ｉ}とｅ_ｔ，ｉをそれぞれ中心とする局所画像のコサイン相関あるいは正規化相関を計算して、相関値が閾値を超えた場合に、ｅ_ｔ，ｉをｐ_{ｔ-１，ｉ}に対応づけると良い。 _{_{_{w 'i q t, i =}}} AT lc [Rt | Tt] p t-1, i (2)
Then, the movement amount calculation unit 141 associates the edge points _{et, i} closest to q _{t, i} in Et with pt _{-1, i} . However at this time, in order to increase the accuracy of the correspondence, in the original image I _t-1 and _{_{I t, q t-1,}} i and e _t, cosine correlation or normalized correlation of the local _{image i} respectively centered When the correlation value exceeds the threshold value by calculation, it is preferable to associate _{et and i} with _{pt-1, i} .

そして移動量算出部１４１は、こうして求めた対応づけに対して、式（３）を最小化する推定移動量ｍｔを求める。このとき移動量算出部１４１は、ｍｔの初期値を０として計算する。 Then, the movement amount calculation unit 141 obtains the estimated movement amount mt that minimizes the equation (3) with respect to the correspondence obtained in this way. At this time, the movement amount calculation unit 141 calculates the initial value of mt as 0.

なお、ｎ_ｔ，ｉはｅ_ｔ，ｉの法線ベクトルである。Ｇ_t(mt)の最小化は、準ニュートン法やレーベンバーグ・マーカート法、ガウス・ニュートン法、共役勾配法など種々の方法を用いて行うことが可能である。 Note that _{nt and i} are normal vectors of et _{and i} . Minimization of G _t (mt) can be performed by using various methods such as the quasi-Newton method, the Levenberg-Marquardt method, the Gauss-Newton method, and the conjugate gradient method.

移動量算出部１４１は、この対応づけと投影誤差最小化の処理を、Ｇ_t(mt)の値が所定値以下に収束するまで交互に繰り返す。これは、３Ｄ点−２Ｄ点対応に基づいて点対線のＩＣＰ(Iterative Closest Point)を画像空間で行うことに相当する。 The movement amount calculation unit 141 alternately repeats the process of associating and minimizing the projection error until the value of G _t (mt) converges to a predetermined value or less. This corresponds to performing a point-to-line ICP (Iterative Closest Point) in the image space based on a 3D point-2D point correspondence.

ＩＣＰは２つの点群データの位置合せをする方法である。点対点のＩＣＰでは、２つの点群データの対応点間のユークリッド距離の二乗和を誤差関数とする。点対線のＩＣＰでは、どちらかの点が法線ベクトルをもつ場合に適用でき、対応点間の垂線距離の和を誤差関数とする。疎らな点群では対応点の位置が一致することはまれであり、滑らかな形状をもつ環境ならば、垂線距離を用いた誤差関数の方が安定性が高く、最小化計算において極値に陥らずによい解に到達しやすい利点がある（ＩＣＰについては、例えばP. J. Besl and N. D. Mckay: “A Method of Registration of 3-D Shapes,” IEEE Trans. on PAMI, Vol. 14, No. 2,pp. 239-256, 1992.を参照）。 ICP is a method of aligning two point cloud data. In point-to-point ICP, the sum of squares of the Euclidean distance between the corresponding points of the two point cloud data is used as the error function. In point-to-line ICP, it can be applied when either point has a normal vector, and the sum of the perpendicular distances between the corresponding points is used as an error function. In a sparse point cloud, the positions of the corresponding points rarely match, and in an environment with a smooth shape, the error function using the perpendicular distance is more stable and falls into an extreme value in the minimization calculation. There is an advantage that it is easy to reach a good solution without (For ICP, for example, PJ Besl and ND Mckay: “A Method of Registration of 3-D Shapes,” IEEE Trans. On PAMI, Vol. 14, No. 2, pp . 239-256, 1992.).

そして移動量算出部１４１は、推定移動量ｍｔを移動量記憶部１４２に記憶する。 Then, the movement amount calculation unit 141 stores the estimated movement amount mt in the movement amount storage unit 142.

移動量記憶部１４２には、上記のようにして所定時間毎に算出された移動体１０００の推定移動量が記憶される。 The movement amount storage unit 142 stores the estimated movement amount of the moving body 1000 calculated at predetermined time intervals as described above.

自己運動推定システム１００は、これらの推定移動量を順次積算することにより、移動体１０００の移動経路を求めることが可能となる。また推定移動量を第１タイミングから第２タイミングまでの間の時間（所定時間）で除算することにより、移動体１０００の速度も求めることが可能となる。 The self-motion estimation system 100 can obtain the movement path of the moving body 1000 by sequentially integrating these estimated movement amounts. Further, by dividing the estimated movement amount by the time (predetermined time) between the first timing and the second timing, the speed of the moving body 1000 can also be obtained.

３次元地図生成部１５０は、移動体記憶部１４２に順次蓄積されていく移動体１０００の推定移動量を、所定時間毎に積算することで、各時点における移動体１０００の位置を特定しつつ、各位置で取得した距離測定データあるいは距離画像Ｐｔを取得距離情報記憶部１１２から読み出して、移動体１０００の位置姿勢に基づいて座標変換をしながら重ね合わせることにより、３次元地図を生成する（数式としては、後述する式（８）座標変換の式と同じ）。 The three-dimensional map generation unit 150 integrates the estimated movement amount of the moving body 1000 sequentially accumulated in the moving body storage unit 142 at predetermined time intervals, thereby specifying the position of the moving body 1000 at each time point. A three-dimensional map is generated by reading the distance measurement data or the distance image Pt acquired at each position from the acquired distance information storage unit 112 and superimposing them while performing coordinate conversion based on the position and orientation of the moving body 1000 (formula). Is the same as the equation (8) coordinate conversion equation described later).

そして、運動制御部４００は、移動体１０００が自律移動する場合は、移動量記憶部１４２に所定時間毎に順次蓄積されていく推定移動量や、所定時間毎に完成していく３次元地図を用いて、移動体１０００の自律走行を行う。 Then, when the moving body 1000 moves autonomously, the motion control unit 400 displays an estimated movement amount that is sequentially accumulated in the movement amount storage unit 142 at predetermined time intervals and a three-dimensional map that is completed at predetermined time intervals. It is used to autonomously travel the moving body 1000.

このように本実施形態に係る自己運動推定システム１００は、距離センサ２００から得られる３Ｄ点群と、画像センサ３００から得られる明暗画像Ｉｔから抽出した画像エッジ点群と、を対応づけ、画像空間でＩＣＰを行うことにより、リアルタイムで移動体１０００の運動状態を推定することができる。このような態様によって、自己運動推定システム１００によれば、より効率的かつ正確に外界を観測しつつリアルタイムに自己の運動状態を推定することが可能となる。 As described above, the self-motion estimation system 100 according to the present embodiment associates the 3D point cloud obtained from the distance sensor 200 with the image edge point cloud extracted from the light / dark image It obtained from the image sensor 300, and forms an image space. By performing ICP in, the motion state of the moving body 1000 can be estimated in real time. According to the self-motion estimation system 100, it is possible to estimate the self-motion state in real time while observing the outside world more efficiently and accurately.

例えば、本実施形態に係る自己運動推定システム１００は、距離画像Ｐｔと明暗画像Ｉｔのエッジ点を用いて、上記のようにＩＣＰを行っている。多くの場合、１枚の明暗画像Ｉｔの中からは数多くのエッジ点を抽出できるため、非特許文献１のように画像のコーナ点を用いる一般的な方法に比べて、処理が格段に安定する。また画像エッジ点は法線をもつので、点対線のＩＣＰが可能となり安定性も増す。 For example, the self-motion estimation system 100 according to the present embodiment performs ICP as described above by using the edge points of the distance image Pt and the light / dark image It. In many cases, many edge points can be extracted from one bright / dark image It, so that the processing is much more stable than the general method using the corner points of the image as in Non-Patent Document 1. .. Further, since the image edge point has a normal line, ICP of point-to-line line is possible and stability is increased.

上述したように、本実施形態に係る自己運動推定システム１００は、明暗画像Ｉｔから特徴抽出（エッジ検出）を行うようにし、３Ｄ点群からは特徴抽出を行わないので、処理が単純で効率がよい。また、リアルタイムで使える距離センサ２００は一般に解像度が低いので、遠方での計測点は疎らになり、そのような計測点からの特徴抽出は不安定になりやすいが、本実施形態に係る自己運動推定システム１００は、３Ｄ特徴（３Ｄ点群から抽出した特徴）を用いないので、遠方で３Ｄ点群が疎になって特徴抽出が不安定になるような問題も発生しない。 As described above, the self-motion estimation system 100 according to the present embodiment performs feature extraction (edge detection) from the light / dark image It and does not perform feature extraction from the 3D point cloud, so that the processing is simple and efficient. Good. Further, since the distance sensor 200 that can be used in real time generally has a low resolution, measurement points at a distance become sparse, and feature extraction from such measurement points tends to be unstable. However, self-motion estimation according to the present embodiment. Since the system 100 does not use 3D features (features extracted from the 3D point cloud), there is no problem that the 3D point cloud becomes sparse and the feature extraction becomes unstable in the distance.

また自己運動推定システム１００は、３Ｄ点と２Ｄ点の対応しか用いないので、レーザセンサのみを用いる方法の場合に必要になる３Ｄ点同士の対応づけのためのデータ管理も不要にできる。 Further, since the self-motion estimation system 100 uses only the correspondence between the 3D points and the 2D points, it is possible to eliminate the data management for associating the 3D points, which is necessary in the case of the method using only the laser sensor.

さらに自己運動推定システム１００は、平面上のテクスチャなどの画像特徴は得られても３Ｄ特徴が得られない場合であっても、問題なく動作することが可能である。 Further, the self-motion estimation system 100 can operate without any problem even when image features such as textures on a plane are obtained but 3D features cannot be obtained.

なお、本実施形態に係る自己運動推定システム１００は、距離センサ２００として３Ｄレーザスキャナを用いて距離画像Ｐｔを得ている。そのため、遠方の地形まで精度よく計測できるため、とくに屋外での大規模な地図構築に適している。 The self-motion estimation system 100 according to the present embodiment uses a 3D laser scanner as the distance sensor 200 to obtain a distance image Pt. Therefore, it can accurately measure distant terrain, and is particularly suitable for large-scale outdoor map construction.

＜自己運動推定システムの処理の流れ＞
次に、本実施形態に係る自己運動推定システム１００の処理の流れを、図６に示すフローチャートに従って説明する。 <Processing flow of self-motion estimation system>
Next, the processing flow of the self-motion estimation system 100 according to the present embodiment will be described with reference to the flowchart shown in FIG.

まず、自己運動推定システム１００は、距離画像Ｐｔ及び明暗画像Ｉｔを取得する（S1000、S1010）。図６では、便宜上、距離画像Ｐｔを明暗画像Ｉｔよりも先に取得するように記載しているが、自己運動推定システム１００は、距離画像Ｐｔと明暗画像Ｉｔを並列して取得する。 First, the self-motion estimation system 100 acquires the distance image Pt and the light / dark image It (S1000, S1010). In FIG. 6, for convenience, it is described that the distance image Pt is acquired before the light / dark image It, but the self-motion estimation system 100 acquires the distance image Pt and the light / dark image It in parallel.

次に、自己運動推定システム１００は、明暗画像Ｉｔからエッジ点を検出する（S1020）。エッジ点の検出は、公知のアルゴリズムを用いることが可能であり、本実施形態では、たとえば、Canny法を用いる。 Next, the self-motion estimation system 100 detects an edge point from the light / dark image It (S1020). A known algorithm can be used to detect the edge point, and in this embodiment, for example, the Canny method is used.

そして自己運動推定システム１００は、距離画像Ｐｔの各３Ｄ点ｐ_ｔ，ｉを、エッジ画像Ｅｔに式（１）で透視投影することで、対応する画素ｑ_ｔ，ｉを求め、ＲＧＢＤ特徴ペアの集合Ｃｔを求める（S1030）。 Then, the self-motion estimation system 100 obtains the corresponding pixels qt _{and i} by perspectively projecting the 3D points pt _{and i} of the distance image Pt onto the edge image Et by the equation (1), and obtains the corresponding pixels qt _{and i} , and obtains the corresponding pixels qt _{and i} . Find the set Ct (S1030).

そして自己運動推定システム１００は、時刻ｔ-１から時刻ｔまでの移動体１０００の推定移動量をｍｔとしたときに、この推定移動量ｍｔを所定の初期値から順次更新しながら、式（２）によって、時刻ｔ−１の３Ｄ点ｐ_{ｔ-１，ｉ}をＥｔに投影し、式（３）の値を最小にするような推定移動量ｍｔを求める（S1040）。 Then, when the estimated movement amount of the moving body 1000 from the time t-1 to the time t is mt, the self-motion estimation system 100 sequentially updates the estimated movement amount mt from a predetermined initial value, and the equation (2) ), The 3D points pt _{-1, i} at time t-1 are projected onto Et, and the estimated movement amount mt that minimizes the value of the equation (3) is obtained (S1040).

以上の処理を繰り返し実行することによって、本実施形態に係る自己運動推定システム１００は、より効率的かつ正確に外界を観測しつつリアルタイムに自己の運動状態を推定することができる。 By repeatedly executing the above processing, the self-motion estimation system 100 according to the present embodiment can estimate the self-motion state in real time while observing the outside world more efficiently and accurately.

＝＝その他の実施形態＝＝
以下に、自己運動推定システム１００のその他の実施形態を説明する。以下に記載する各態様により、より一層効率的かつ正確に自己運動状態を推定することが可能となる。 == Other embodiments ==
Hereinafter, other embodiments of the self-motion estimation system 100 will be described. Each aspect described below makes it possible to estimate the self-motion state more efficiently and accurately.

＜取得周期が異なる場合＞
上述したように、時刻tの距離画像Ｐｔと明暗画像Ｉｔとの組を、フレームＦｔ＝(Ｐｔ,Ｉｔ)と呼ぶが、一般的に画像センサ３００の方が距離センサ２００よりも取得周期が短い。この場合、明暗画像取得部１２１が第１時間毎に明暗画像Ｉｔを取得するとすると、距離画像取得部１１１は、第１時間の整数倍（ｎ倍）の第２時間毎に距離画像Ｐｔを取得する。 <When the acquisition cycle is different>
As described above, the pair of the distance image Pt at the time t and the light / dark image It is called the frame Ft = (Pt, It), but the image sensor 300 generally has a shorter acquisition cycle than the distance sensor 200. .. In this case, assuming that the light / dark image acquisition unit 121 acquires the light / dark image It every first hour, the distance image acquisition unit 111 acquires the distance image Pt every second hour, which is an integral multiple (n times) of the first hour. To do.

このような場合、距離画像Ｐｔが空のフレームが生じるが、本実施形態に係る自己運動推定システム１００は以下のような処理を行うことにより、同じ３Ｄ点群を複数の明暗画像Ｉｔと位置合わせできる。 In such a case, a frame in which the distance image Pt is empty occurs, but the self-motion estimation system 100 according to the present embodiment aligns the same 3D point group with a plurality of light and dark images It by performing the following processing. it can.

つまり、本実施形態に係る自己運動推定システム１００は、第１実施形態の処理において、明暗画像Ｉ_ｔ−１及び距離画像Ｐ_ｔ−１の双方を取得するタイミングを第１タイミングとすると共に、新たな明暗画像Ｉ_{ｔ＋ｋ−１}（k=0,…,n-1）を取得する第１時間毎に到来するタイミングを第２タイミングとして、第１時間毎に、推定移動量を算出する。 That is, in the self-motion estimation system 100 according to the present embodiment, in the processing of the first embodiment, the timing of acquiring both the light / dark image It _-1 and the distance image _Pt-1 is set as the first timing, and is newly added. The estimated movement amount is calculated for each first hour, with the timing at which the bright / dark image It _{+ k-1} (k = 0, ..., N-1) is acquired every first hour as the second timing.

またこのとき自己運動推定システム１００は、明暗画像取得部１２１が明暗画像Ｉ_{ｔ＋ｋ−１}を取得する上記第１時間毎に、第１タイミングｔ−１で取得した距離画像Ｐ_ｔ−１と、第１タイミングｔ−１から第２タイミングｔ＋ｋ−１までの上記推定移動量の累積値とを用いて算出される、第２タイミングｔ＋ｋ−１における距離画像Ｐ_ｔ−１の透視変換による投影位置と、第２タイミングにおける明暗画像Ｉ_{ｔ＋ｋ−１}と、を照合し、明暗画像Ｉ_{ｔ＋ｋ−１}におけるエッジの位置に対応する距離画像Ｐ_ｔ−１内の距離情報を、エッジ位置距離情報として特定する
より具体的には、画像センサ３００の取得周期が距離センサ２００の取得周期の１／ｎ倍の場合、１つの３Ｄ点群Ｐｔ-１に対して、ｎ枚の明暗画像Ｉ_ｔ-１，Ｉ_ｔ…Ｉ_{ｔ＋ｎ-２}が対応するが、自己運動推定システム１００は、Ｉ_{ｔ＋ｋ-１}（k=0,…,n-1）までの推定移動量が得られているとして、Ｉ_ｔ＋ｋの位置合せを次のように行う。 At this time, the self-motion estimation system 100 includes the distance image P _t-1 acquired at the first timing t-1 and the first distance image P _t-1 acquired at the first timing t-1 every first hour when the light / dark image acquisition unit 121 acquires the light / dark image It _{+ k-1} . The projected position of the distance image P _t-1 at the second timing t + k-1 calculated by using the cumulative value of the estimated movement amount from the first timing t-1 to the second timing t + k-1 and the projection position by the perspective conversion. The light-dark image It _{+ k-1 at} the second timing is collated, and the distance information in the distance image P _t-1 corresponding to the edge position in the light-dark image It _{+ k-1} is specified as the edge position distance information. the, if acquisition period of the image sensor 300 is 1 / n times the period of acquiring the distance sensor 200, for one 3D point cloud Pt-1, n pieces of light-and-dark image _{I _t-1,} I _t ... It _{+ n-2} corresponds, but the self-motion estimation system 100 sets the alignment of It _{+ k} as follows, assuming that the estimated movement amount up to It _{+ k-1} (k = 0, ..., N-1) is obtained. Do as follows.

まず自己運動推定システム１００は、時刻ｔ-１からｔ＋ｋまでの推定移動量をｍ_ｔ＋ｋ＝［Ｒ_ｔ＋ｋ｜Ｔ_ｔ＋ｋ］とした場合に、ｍ_{ｔ＋ｋ-１}を用いて、式（４）のように、Ｐ_ｔ-１の３Ｄ点ｐ_{ｔ-１，ｉ}をエッジ画像Ｅ_{ｔ＋ｋ-１}に投影する。 Ego-motion estimation system 100 First, the estimated amount of movement from the time t-1 to _{_{t + k m t + k =}} | when the _{_{[R t + k T t +}} k], using the _{m t + k-1,} as shown in Equation (4) , P _t-1 3D points pt _{-1, i} are projected onto the edge image _{Et + k-1} .

そして、自己運動推定システム１００は、第１実施形態と同様に、Ｅ_{ｔ＋ｋ-１}の画素値に基づいて、ｑ_{ｔ＋ｋ-１，ｉ}がエッジ点ならば、ｐ_{ｔ-１，ｉ}に対応づけてＲＧＢＤ特徴ペアとする。 The ego-motion estimation system 100, as in the first embodiment, based on the pixel values of _{_{E t + k-1, q}} t + k-1, i is if edge point, in association with the p _{t-1, i} Let it be an RGBD feature pair.

次に、自己運動推定システム１００は、ｍ_ｔ＋ｋを用いて、式（５）のように、ｐ_{ｔ-１，ｉ}をＥ_ｔ＋ｋに投影する。 Next, ego-motion estimation system ₁₀₀ uses the _{m t + k,} as shown in equation (5), projects a p _{t-1, i} to _{E t + k.}

そして自己運動推定システム１００は、Ｅ_ｔ＋ｋの中でｑ_{ｔ＋ｋ，ｉ}に最も近いエッジ点ｅ_{ｔ＋ｋ，ｉ}をｐ_{ｔ-１，ｉ}に対応づける。
この際自己運動推定システム１００は、第１実施形態と同様に、原画像Ｉ_{ｔ＋ｋ-１}とＩ_ｔ＋ｋにおいて、ｑ_{ｔ＋ｋ-１，ｉ}とｅ_ｔ，ｉをそれぞれ中心とする局所画像のコサイン相関あるいは正規化相関を計算して相関値が閾値を超えた場合に、ｅ_{ｔ＋ｋ，ｉ}をｐ_{ｔ-１，ｉ}に対応づける。なお相関計算にはｑ_{ｔ-１，ｉ}を用いてもよいが、ｑ_{ｔ＋ｋ-１，ｉ}の方が視点変化による画像の歪みを小さくできる。 The ego-motion estimation system 100 _associates _{q t + k} in _{E t + _k,} the closest edge point _{e t + k} to _{_i, i} to p _{t-1, i.}
At this time, the self-motion estimation system 100 has a cosine correlation of local images centered on q _{t + k-1, i} and _{et, i} in the original images It _{+ k-1} and It _{+ k} , respectively, as in the first embodiment. When the normalized correlation is calculated and the correlation value exceeds the threshold value, _{et + k, i is associated} with _{pt-1, i} . Although q _{t-1, i} may be used for the correlation calculation, q _{t + k-1, i} can reduce the distortion of the image due to the change in the viewpoint.

次に自己運動推定システム１００は、第１実施形態と同様に、式（６）を最小化する移動量ｍ_ｔ＋ｋを求める。ｍ_ｔ＋ｋの初期値はｍ_{ｔ＋ｋ-１}とする。 Next, the self-motion estimation system 100 obtains a movement amount mt _{+ k} that minimizes the equation (6), as in the first embodiment. The initial value of the m _{t + k} is the _{m t + k-1.}

このような、対応づけと投影誤差の最小化の処理を、自己運動推定システム１００は交互に繰り返し行う。 The self-motion estimation system 100 alternately repeats such a process of associating and minimizing the projection error.

このような態様によって、同じ３Ｄ点群に対して複数の明暗画像を位置合せできるようになるため、画像センサ３００の取得周期が距離センサ２００の取得周期より短くても対応することが可能となる。これらの特長により、取得周期が長い、低性能で安価な３Ｄレーザスキャナを用いることも可能となる。 In such an aspect, since a plurality of bright and dark images can be aligned with respect to the same 3D point cloud, it is possible to deal with the acquisition cycle of the image sensor 300 even if it is shorter than the acquisition cycle of the distance sensor 200. .. These features make it possible to use a low-performance, inexpensive 3D laser scanner with a long acquisition cycle.

＜画像の階層化＞
次に、本実施形態に係る自己運動推定システム１００は、明暗画像Ｉｔを階層化して処理することも可能である。 <Image hierarchy>
Next, the self-motion estimation system 100 according to the present embodiment can process the light / dark image It in a layered manner.

この場合、自己運動推定システム１００は、処理の効率化とＩＣＰの極値回避のために、明暗画像Ｉｔを階層化して位置合せを行う。階層の数は画像サイズを考慮して適宜決めればよいが、ここでは、例えば１００万画素程度の明暗画像Ｉｔを２段階で処理することとし、１／４縮小画像（解像度が相対的に低い第１明暗画像）と原画像（解像度が相対的に高い第２明暗画像）の２つを用いる。 In this case, the self-motion estimation system 100 stratifies and aligns the light and dark images It in order to improve the processing efficiency and avoid the extreme values of ICP. The number of layers may be appropriately determined in consideration of the image size, but here, for example, a bright / dark image It of about 1 million pixels is processed in two stages, and a 1/4 reduced image (the resolution is relatively low). Two images (1 light and dark image) and an original image (second light and dark image with relatively high resolution) are used.

具体的には、自己運動推定システム１００は、まず、１／４縮小画像に対して、第１実施形態に記載した方法で位置合せを行うことで、推定移動量ｍｔ（あるいはｍｔ＋ｋ）を求める。 Specifically, the self-motion estimation system 100 first obtains an estimated movement amount mt (or mt + k) by aligning a 1/4 reduced image with the method described in the first embodiment.

そして、この１／４縮小画像を用いて算出した推定移動量ｍｔ（あるいはｍｔ＋ｋ）を、ＩＣＰの初期値として、原画像を用いて、推定移動量ｍ’ｔ（あるいはｍ’ｔ＋ｋ）を求める。 Then, the estimated movement amount mt (or mt + k) calculated using this 1/4 reduced image is used as the initial value of ICP, and the estimated movement amount m't (or m't + k) is obtained using the original image.

より具体的に説明すると、エッジ検出部１２２は、１／４縮小画像である第１明暗画像、及び原画像である第２明暗画像のそれぞれについてエッジを検出する。 More specifically, the edge detection unit 122 detects edges for each of the first light-dark image which is a 1/4 reduced image and the second light-dark image which is an original image.

そして特徴ペア生成部１３１は、時刻ｔ−１（第１タイミング）で取得した距離画像と第１明暗画像とを照合し、第１明暗画像におけるエッジの位置に対応する距離画像内の３Ｄ点を、第１エッジ位置距離情報として特定する。 Then, the feature pair generation unit 131 collates the distance image acquired at time t-1 (first timing) with the first light-dark image, and determines a 3D point in the distance image corresponding to the position of the edge in the first light-dark image. , First edge position Specify as distance information.

そして移動量算出部１４１は、第１タイミングにおける第１エッジ位置距離情報と、時刻ｔ−１（第１タイミング）から時刻ｔ（第２タイミング）までの所定時間における移動体１０００の推定移動量ｍｔと、を用いて算出される、第１エッジ位置距離情報の時刻ｔ（第２タイミング）における明暗画像内での透視変換による投影位置が、時刻ｔ（第２タイミング）で取得される第１明暗画像におけるエッジの位置に近づくように、推定移動量ｍｔを算出する。 Then, the movement amount calculation unit 141 uses the first edge position / distance information at the first timing and the estimated movement amount mt of the moving body 1000 at a predetermined time from the time t-1 (first timing) to the time t (second timing). And, the projected position by the perspective conversion in the light-dark image at the time t (second timing) of the first edge position distance information calculated by using, is acquired at the time t (second timing). The estimated movement amount mt is calculated so as to approach the position of the edge in the image.

そして次に、特徴ペア生成部１３１は、距離画像と第２明暗画像とを照合し、第２明暗画像におけるエッジの位置に対応する距離画像内の３Ｄ点を、第２エッジ位置距離情報として特定する。 Next, the feature pair generation unit 131 collates the distance image with the second light-dark image, and specifies the 3D point in the distance image corresponding to the edge position in the second light-dark image as the second edge position distance information. To do.

そして移動量算出部１４１は、上記第１明暗画像を用いて算出された推定移動量ｍｔを初期値として、第２エッジ位置距離情報と、時刻ｔ−１（第１タイミング）から時刻ｔ（第２タイミング）までの所定時間における移動体１０００の推定移動量ｍ’ｔと、を用いて算出される、第２エッジ位置距離情報の時刻ｔ（第２タイミング）における明暗画像内での透視変換による投影位置が、時刻ｔ（第２タイミング）で取得される第２明暗画像におけるエッジの位置に近づくように、推定移動量ｍ’ｔを算出する。 Then, the movement amount calculation unit 141 uses the estimated movement amount mt calculated using the first light-dark image as an initial value, and sets the second edge position / distance information and the time t-1 (first timing) to the time t (first timing). By fluoroscopic conversion in the light-dark image at time t (second timing) of the second edge position distance information calculated using the estimated movement amount m't of the moving body 1000 in the predetermined time up to (2 timing). The estimated movement amount m't is calculated so that the projected position approaches the position of the edge in the second light-dark image acquired at the time t (second timing).

この方法は、いわゆるピラミッド探索に似ているが、対応点を求めるだけでなく縮小画像でＩＣＰ全体を行う点が異なる。その利点は、縮小画像では画素数が少ないので対応点の探索範囲が小さくて済み、また原画像では移動量のよい初期値が縮小画像により得られているので、対応点の探索範囲も移動量の探索範囲も非常に小さくて済むことである。これにより処理効率と極値回避性を向上することができる。 This method is similar to the so-called pyramid search, except that it not only finds the corresponding points but also performs the entire ICP on the reduced image. The advantage is that the search range of the corresponding point is small because the number of pixels is small in the reduced image, and the search range of the corresponding point is also the moving amount because the initial value with a good movement amount is obtained from the reduced image in the original image. The search range of is also very small. As a result, processing efficiency and extreme value avoidance can be improved.

なお、階層の数が３以上の場合は、同様の処理を繰り返し行えばよい。例えば１／８縮小画像と１／４縮小画像と原画像との３つを用いて３段階で処理する場合は、初めに解像度が最も低い１／８縮小画像（第１明暗画像）を用いて推定移動量を算出し、次いで解像度が２番目に低い１／４縮小画像（第２明暗画像）を用いて推定移動量を算出し、最後に解像度が最も高い原画像（第３明暗画像）を用いて推定移動量を算出する。このように、Ｎ（Ｎは２以上の自然数）階層で処理する場合は、各階層の明暗画像を解像度が低い順に用いて、推定移動量をＮ回算出する。 When the number of layers is 3 or more, the same process may be repeated. For example, when processing a 1/8 reduced image, a 1/4 reduced image, and an original image in three stages, first use the 1/8 reduced image (first bright and dark image) having the lowest resolution. The estimated movement amount is calculated, then the estimated movement amount is calculated using the 1/4 reduced image (second light-dark image) having the second lowest resolution, and finally the original image (third light-dark image) having the highest resolution is calculated. Use to calculate the estimated movement amount. In this way, when processing in N (N is a natural number of 2 or more) layers, the estimated movement amount is calculated N times by using the bright and dark images of each layer in ascending order of resolution.

＜移動量の予測＞
次に、本実施形態に係る自己運動推定システム１００は、移動量を推定する処理を以下のように改良することも可能である。つまり、過去のフレームで算出した推定移動量を用いて、ＩＣＰの探索範囲を狭める。 <Forecast of movement amount>
Next, the self-motion estimation system 100 according to the present embodiment can also improve the process of estimating the movement amount as follows. That is, the search range of the ICP is narrowed by using the estimated movement amount calculated in the past frame.

具体的には、移動量算出部１４１は、所定時間毎に推定移動量を算出する際に、直近に算出した移動体１０００の推定移動量を予測値として用いるようにする。 Specifically, the movement amount calculation unit 141 uses the most recently calculated estimated movement amount of the moving body 1000 as a predicted value when calculating the estimated movement amount at predetermined time intervals.

このような態様によって、例えば明暗画像Ｉｔ中に、隣接する走行レーンを走る車などの移動物体が写り込んでいる場合や、環境内に目印となる大きな物体（ランドマーク）が少なく、また、繰り返し構造（同じ形状が繰り返し続く構造物）が続く場合であっても、ＩＣＰの探索範囲を狭めることが可能となり、処理効率を向上させ、また対応誤りを減らすことが可能である。 Depending on such an aspect, for example, when a moving object such as a car running in an adjacent traveling lane is reflected in the light / dark image It, there are few large objects (landmarks) that serve as landmarks in the environment, and the process is repeated. Even when the structure (structure in which the same shape continues repeatedly) continues, the search range of ICP can be narrowed, the processing efficiency can be improved, and the correspondence error can be reduced.

つまり、ＩＣＰの探索には、対応点の探索と移動量の探索の２つがあるが、まず、対応点の探索範囲は次のように狭められる。 That is, there are two types of ICP search, the search for the corresponding point and the search for the amount of movement. First, the search range for the corresponding point is narrowed as follows.

すなわち、移動量の予測値によって、移動体１０００の次の位置を予測する。その予測位置に基づいて、３Ｄ点群を次フレームの明暗画像に投影して、その近傍で対応点候補を探索する。予測によって３Ｄ点は真の対応点の近くに投影されるので、予測値がない場合よりもはるかに狭い範囲を探索すればすむ。 That is, the next position of the moving body 1000 is predicted by the predicted value of the moving amount. Based on the predicted position, the 3D point cloud is projected onto the bright / dark image of the next frame, and the corresponding point candidate is searched in the vicinity thereof. The prediction projects the 3D point near the true corresponding point, so you only have to search a much narrower range than if there were no predicted value.

一方、移動量の探索では、予測値を初期値として、式（３）または式（６）の最適化を行う。予測により初期値が真の解に近づくので、極値に陥る可能性が低くなる。 On the other hand, in the search for the amount of movement, the equation (3) or the equation (6) is optimized with the predicted value as the initial value. Since the initial value approaches the true solution by prediction, the possibility of falling into an extreme value is reduced.

移動量の予測により、移動物体の問題は次のように解決される。 By predicting the amount of movement, the problem of moving objects is solved as follows.

つまり、移動体１０００と静止物体および移動体１０００と移動物体の相対速度の差が一定量より大きければ、移動物体の対応点は予測による探索範囲からはずれるため、対応点候補とならない。そのため、誤った対応づけで位置がずれることを防ぐことができる。 That is, if the difference between the relative velocities of the moving body 1000 and the stationary object and the moving body 1000 and the moving object is larger than a certain amount, the corresponding point of the moving object deviates from the search range by prediction, so that it is not a corresponding point candidate. Therefore, it is possible to prevent the position from being displaced due to incorrect association.

また繰り返し構造の問題は以下のように解決される。予測による対応点の探索範囲が繰り返し間隔より小さければ、誤対応点は探索範囲からはずれるため、対応点候補とならない。そのため、誤った対応づけで位置がずれることを防ぐことができる。 The problem of repeating structure is solved as follows. If the search range of the corresponding points by prediction is smaller than the repetition interval, the erroneous corresponding points are out of the search range and are not candidates for the corresponding points. Therefore, it is possible to prevent the position from being displaced due to incorrect association.

＜複数フレームの統合＞
次に、本実施形態に係る自己運動推定システム１００は、以下のようにして過去Nフレームの距離画像を用いて距離画像Ｐｔを強化することで、よりロバストに運動推定を行うことも可能である。 <Integration of multiple frames>
Next, the self-motion estimation system 100 according to the present embodiment can perform motion estimation more robustly by strengthening the distance image Pt using the distance image of the past N frames as follows. ..

つまり、距離画像取得部１１１は、距離センサ２００から新たな距離画像Ｐｔを取得した際に、過去に取得した距離画像Ｐｔ−ｋ（k=1,…,N）と、過去の距離画像Ｐｔ−ｋを取得した時点から現在までの移動体１０００の推定移動量と、を用いて、過去の距離画像Ｐｔ−ｋに含まれる３Ｄ点の位置を現在の距離センサ２００の座標系に変換し、変換後の３Ｄ点を新たな距離画像Ｐｔに追加するようにする。 That is, when the distance image acquisition unit 111 acquires a new distance image Pt from the distance sensor 200, the distance image Pt-k (k = 1, ..., N) acquired in the past and the past distance image Pt- Using the estimated movement amount of the moving body 1000 from the time when k is acquired to the present, the position of the 3D point included in the past distance image Pt-k is converted into the coordinate system of the current distance sensor 200 and converted. The latter 3D point is added to the new distance image Pt.

このような態様によって、よいランドマークがなく位置合せの手がかりとなる３Ｄ点が少ししかとれない環境の中で、３Ｄ点群の解像度が低い距離センサ２００を用いるような場合であっても、複数フレームを統合することにより、そのような３Ｄ点を増やすことが可能となる。 According to such an aspect, even when the distance sensor 200 having a low resolution of the 3D point cloud is used in an environment where there are no good landmarks and only a few 3D points can be obtained as clues for alignment, a plurality of distance sensors 200 are used. By integrating the frames, it is possible to increase such 3D points.

そのため、時刻ｔ-１のフレームの３Ｄ点群だけでなく、過去のいくつかの３Ｄ点群を統合して用いる。 Therefore, not only the 3D point cloud of the frame at time t-1 but also some past 3D point clouds are integrated and used.

これにより距離画像Ｐｔの点密度が高くなり、ランドマークが小さかったり、遠方で疎な場合でも、そこに当たる３Ｄ点の数が増えて、ＩＣＰの誤差を小さくしたり、極値を回避する可能性が高くなる。 As a result, the point density of the distance image Pt becomes high, and even if the landmarks are small or sparse in the distance, the number of 3D points that hit them increases, which may reduce the ICP error or avoid extreme values. Will be higher.

このために、式（７）のようにして、過去Ｎフレームの３Ｄ点群を統合した局所地図（過去の３Ｄ点を追加した距離画像Ｐ^Ｎ _ｔ-１）を生成し、これを用いてＩＣＰを行う。まず、世界座標系での局所地図Ｐ’^Ｎ _ｔ-１を、普通の地図と同様に３Ｄ点群を移動体１０００の姿勢に沿って配置して生成する。 For this purpose, a local map (distance image ^PN _t-1 to which the past 3D points are added) that integrates the 3D point cloud of the past N frames is generated as in the equation (7), and ICP is used using this. I do. First, a local map ^P'N _t-1 in the world coordinate system is generated by arranging a 3D point cloud along the posture of the moving body 1000 in the same manner as an ordinary map.

さらに、式（９）のようにして、Ｐ’^Ｎ _ｔ-１を時刻ｔ−１の距離センサ２００の座標系に変換し、ＩＣＰを行う。 Further, as the equation (9), to convert the P ^'N _t-1 to the coordinate system of the distance sensor 200 at time t-1, perform the ICP.

本実施形態ではＮ＝５としているが、この限りではなく、移動体１０００の速度などを考慮して決めればよい。 In this embodiment, N = 5 is set, but the present invention is not limited to this, and it may be determined in consideration of the speed of the moving body 1000 and the like.

以上、本実施形態に係る自己運動推定システム１００について説明したが、本実施形態に係る自己運動推定システム１００によれば、より効率的かつ正確に外界を観測しつつリアルタイムに自己の運動状態を推定することが可能となる。 The self-motion estimation system 100 according to the present embodiment has been described above, but the self-motion estimation system 100 according to the present embodiment estimates the self-motion state in real time while observing the outside world more efficiently and accurately. It becomes possible to do.

例えば、本実施形態に係る自己運動推定システム１００は、３Ｄ特徴（距離画像Ｐｔから抽出されるエッジ点などの特徴点）を抽出することなく、３Ｄ点群と画像エッジ点群の位置合せによって運動推定する。 For example, the self-motion estimation system 100 according to the present embodiment moves by aligning the 3D point cloud and the image edge point cloud without extracting 3D features (feature points such as edge points extracted from the distance image Pt). presume.

このため、例えば低密度・低速な３Ｄレーザスキャナ（距離センサ２００）を用いた場合には利用できる３Ｄ点が少なくなるが、このような場合であっても、位置合せの精度を低下させずに済む。 Therefore, for example, when a low-density, low-speed 3D laser scanner (distance sensor 200) is used, the number of 3D points that can be used decreases, but even in such a case, the alignment accuracy is not reduced. I'm done.

また本実施形態に係る自己運動推定システム１００は、レーザスキャナ（距離センサ２００）とカメラ（画像センサ３００）を組合せて用いているが、このことは、取得周期の観点からも重要である。 Further, the self-motion estimation system 100 according to the present embodiment uses a laser scanner (distance sensor 200) and a camera (image sensor 300) in combination, which is also important from the viewpoint of the acquisition cycle.

つまり、機械駆動の３Ｄスキャナでは、カメラほど取得周期を短くすることは一般的に困難である。そのため、仮に、３Ｄ点群だけを用いて制御を行う場合には、取得周期が長いとフレーム間の移動距離が増えてしまい、データの対応づけが難しくなる。
とくに、移動体１０００が単調な環境を走行する場合のように、目立ったランドマークが少なく、また、繰り返し構造があるような場合に顕著である。フレーム間の移動距離が大きいほど予測誤差は大きくなり、はずれる可能性も高くなる。 That is, in a mechanically driven 3D scanner, it is generally difficult to shorten the acquisition cycle as much as a camera. Therefore, if control is performed using only the 3D point cloud, if the acquisition cycle is long, the moving distance between frames will increase, and it will be difficult to associate the data.
In particular, it is remarkable when there are few conspicuous landmarks and there is a repeating structure, such as when the moving body 1000 travels in a monotonous environment. The larger the moving distance between frames, the larger the prediction error and the higher the possibility of deviation.

しかしながら、本実施形態に係る自己運動推定システム１００は、レーザスキャナ（距離センサ２００）に加えて、カメラ（画像センサ３００）を組合せて用いることにより、取得周期を短く保ち、正確に位置合せをすることが可能である。さらに本実施形態に係る自己運動推定システム１００は、移動量の予測を取り入れることで、より正確な位置合わせを行うこともできる。 However, the self-motion estimation system 100 according to the present embodiment keeps the acquisition cycle short and accurately aligns by using the camera (image sensor 300) in combination with the laser scanner (distance sensor 200). It is possible. Further, the self-motion estimation system 100 according to the present embodiment can perform more accurate alignment by incorporating the prediction of the movement amount.

本実施形態に係る自己運動推定システム１００は、３Ｄレーザスキャナ（距離センサ２００）とカメラ（画像センサ３００）を用いて、３次元６自由度の運動推定を行い、３Ｄ地図を構築する。この自己運動推定システム１００では、まず、同一フレームにおいて３Ｄレーザスキャナの３Ｄ点群と画像エッジ点群を対応づけてＲＧＢＤ特徴ペアを生成し、次に、時刻ｔ-１のＲＧＢＤ特徴ペアと時刻ｔの画像エッジ点を対応づけて、画像空間でＩＣＰを行うことにより、ロボット（移動体１０００）の移動量を求める。さらに、移動量の予測値によって探索範囲を狭め、また、複数フレームの点群を統合して点群密度を増やすことで、移動物体や単調環境・繰り返し構造による移動量推定の障害に対処することもできる。 The self-motion estimation system 100 according to the present embodiment uses a 3D laser scanner (distance sensor 200) and a camera (image sensor 300) to estimate motion with 3D and 6 degrees of freedom, and constructs a 3D map. In this self-motion estimation system 100, first, the 3D point cloud of the 3D laser scanner and the image edge point cloud are associated with each other in the same frame to generate an RGBD feature pair, and then the RGBD feature pair at time t-1 and the time t are generated. The amount of movement of the robot (moving body 1000) is obtained by performing ICP in the image space by associating the image edge points of. Furthermore, by narrowing the search range according to the predicted value of the movement amount and increasing the point cloud density by integrating the point groups of multiple frames, it is possible to deal with the obstacle of the movement amount estimation due to the moving object and the monotonous environment / repeating structure. You can also.

なお上述した実施の形態は本発明の理解を容易にするためのものであり、本発明を限定して解釈するためのものではない。本発明はその趣旨を逸脱することなく変更、改良され得るとともに、本発明にはその等価物も含まれる。 It should be noted that the above-described embodiment is for facilitating the understanding of the present invention, and is not for limiting and interpreting the present invention. The present invention can be modified and improved without departing from the spirit thereof, and the present invention also includes an equivalent thereof.

例えば、移動体１０００は、自らの判断で自らの運動を自律的に制御することが可能な装置であるが、人間や他の制御装置からの操作に従って移動する機能を有していても良い。さらには人間が３Ｄレーザスキャナ（距離センサ２００）、カメラ（画像センサ３００）及び自己運動推定システム１００を携帯して移動することで、人間の移動経路を推定する形態であってもよい。 For example, the moving body 1000 is a device capable of autonomously controlling its own movement at its own discretion, but may have a function of moving according to an operation from a human being or another control device. Further, a human moving path may be estimated by carrying a 3D laser scanner (distance sensor 200), a camera (image sensor 300), and a self-motion estimation system 100.

また移動体１０００が、複数の画像センサ３００を備える場合には、明暗画像取得部１２１は、これらの画像センサ３００により撮影された明暗画像をそれぞれ保持するようにしても良い。 Further, when the moving body 1000 includes a plurality of image sensors 300, the light / dark image acquisition unit 121 may hold the light / dark images taken by these image sensors 300, respectively.

例えば移動体１０００が第１画像センサ３００Ａ及び第２画像センサ３００Ｂの２つの画像センサ３００を備える場合には、明暗画像取得部１２１は、第１画像センサ３００Ａにより撮影された第１撮影範囲の第１明暗画像と、第２画像センサ３００Ｂにより撮影された第２撮影範囲の第２明暗画像と、を持つようにする。 For example, when the moving body 1000 includes two image sensors 300 of the first image sensor 300A and the second image sensor 300B, the light / dark image acquisition unit 121 is the first in the first shooting range shot by the first image sensor 300A. It has one light-dark image and a second light-dark image in the second shooting range taken by the second image sensor 300B.

この場合エッジ検出部１２２は、第１明暗画像及び第２明暗画像それぞれから第１エッジ画像Ｅ１ｔ及び第２エッジ画像Ｅ２ｔを生成し、特徴ペア生成部１３１は、距離画像Ｐｔの各３Ｄ点ｐ_ｔ，ｉを、第１画像センサ３００Ａと距離センサ２００との相対位置で決まる変換行列T_ｌｃ１及び第２画像センサ３００Ｂと距離センサ２００との相対位置で決まる変換行列T_ｌｃ２に基づいて、これらのエッジ画像Ｅ１ｔ及びＥ２ｔに透視投影することで、対応する画素ｑ_ｔ，ｉを求め、ＲＧＢＤ特徴ペアの集合Ｃｔを求めるようにする。さらに、移動量算出部１４１は、距離画像Ｐｔ−１の各３Ｄ点ｐ_{ｔ−１，ｉ}を、エッジ画像Ｅ１ｔ及びＥ２ｔに透視投影する際にこれらの変換行列T_ｌｃ１及びT_ｌｃ２を用いるようにする。この際、式（３）および式（６）は、エッジ画像Ｅ１ｔ及びＥ２ｔに対して計算した値を合計すればよい。 In this case the edge detector 122 from each of the first contrast images and second contrast images to generate a first edge image E1t and second edge image E2T, characterized pair generation unit 131, the 3D point of the range image Pt _{p t the i,} based on the transformation matrix _{T lc2} determined by the relative positions of the transformation matrix _{T lc1} and the second image sensor 300B and the distance sensor 200 which is determined by the relative positions of the first image sensor 300A and the distance sensor 200, these edges By perspectively projecting onto the images E1t and E2t, the corresponding pixels qt _{and i} are obtained, and the set Ct of the RGBD feature pairs is obtained. Further, the movement amount calculation unit 141 uses these transformation matrices T _lc1 and T _lc2 when perspectively projecting the 3D points pt _{-1, i} of the distance image Pt-1 onto the edge images E1t and E2t. To do. At this time, in the equations (3) and (6), the values calculated for the edge images E1t and E2t may be summed.

このような態様によって、自己運動推定システム１００は、より広範囲の外界の存在物の位置変化に基づいて自己の運動状態を推定することができるため、運動状態の推定精度を向上させることが可能となる。 According to such an aspect, the self-motion estimation system 100 can estimate the self-motion state based on the position change of the existence in the outer world in a wider range, so that the estimation accuracy of the motion state can be improved. Become.

１００自己運動推定システム
１１１距離画像取得部
１１２距離測定データ記憶部
１２１明暗画像取得部
１２２エッジ検出部
１３１特徴ペア生成部
１３２特徴ペア記憶部
１４１移動量算出部
１４２移動量記憶部
１５０３次元地図生成部
２００距離センサ
３００画像センサ
４００運動制御部
５００アクチュエータ
１０００移動体 100 Self-motion estimation system 111 Distance image acquisition unit 112 Distance measurement data storage unit 121 Bright / dark image acquisition unit 122 Edge detection unit 131 Feature pair generation unit 132 Feature pair storage unit 141 Movement amount calculation unit 142 Movement amount storage unit 150 Three-dimensional map generation Unit 200 Distance sensor 300 Image sensor 400 Motion control unit 500 Actuator 1000 Moving object

Claims

It is a self-motion estimation system that is mounted on a moving body and estimates the motion state of the moving body by detecting a change in position relative to an object in the outside world.
A light / dark image acquisition unit that acquires a light / dark image of the outside world taken at a predetermined cycle by an image sensor mounted on the moving body, and a light / dark image acquisition unit.
A distance image acquisition unit that acquires a distance image having a plurality of distance information to each point in the outside world included in the imaging range of the image sensor, which is measured by a distance sensor mounted on the moving body at a predetermined cycle.
An edge detection unit that detects edges from the bright and dark images,
Edge position distance information specifying unit that collates the distance image acquired at the first timing with the light / dark image and specifies the distance information in the distance image corresponding to the edge position in the light / dark image as edge position distance information. When,
The edge position / distance information of the edge position / distance information calculated by using the edge position / distance information at the first timing and the estimated movement amount of the moving body at a predetermined time between the first timing and the second timing. The estimated movement amount of the moving body is calculated so that the projection position by the fluoroscopic conversion in the light-dark image at the second timing approaches the position of the edge detected from the light-dark image acquired at the second timing. Movement amount calculation unit and
A self-motion estimation system characterized by being equipped with.

The self-motion estimation system according to claim 1.
The light-dark image acquisition unit acquires the light-dark image every first hour.
The distance image acquisition unit acquires the distance image every second hour , which is an integral multiple of two or more of the first hour.
The movement amount calculation unit sets the timing of acquiring both the light-dark image and the distance image as the first timing, and sets the timing of arrival every first hour of acquiring a new light-dark image as the second timing. As the timing, the estimated movement amount is calculated for each of the first hours.
The edge position distance information specifying unit includes the distance image acquired at the first timing and the first timing to the second timing every time the light / dark image acquisition unit acquires the bright / dark image. The projected position at the second timing by the perspective conversion of the distance image, which is calculated by using the cumulative value of the estimated movement amount of the above, is collated with the bright / dark image at the second timing, and the bright / dark image is used. A self-motion estimation system characterized in that distance information in the distance image corresponding to an edge position is specified as edge position distance information.

The self-motion estimation system according to claim 1 or 2.
The light-dark image includes a first light-dark image having a relatively low resolution and a second light-dark image having a relatively high resolution.
The edge detection unit detects an edge for each of the first light and dark image and the second light and dark image, and then detects an edge.
The edge position / distance information specifying unit collates the distance image acquired at the first timing with the first light / dark image, and obtains the distance information in the distance image corresponding to the position of the edge in the first light / dark image. The distance in the distance image corresponding to the position of the edge in the second light and dark image is collated with the distance image acquired at the first timing while specifying as the first edge position distance information. Identify the information as the second edge position distance information,
The movement amount calculation unit is calculated by using the first edge position / distance information at the first timing and the estimated movement amount of the moving body at the predetermined time from the first timing to the second timing. The estimation so that the projection position of the first edge position / distance information in the light / dark image at the second timing approaches the edge position in the first light / dark image acquired at the second timing. After calculating the movement amount, using the calculated estimated movement amount as an initial value, the second edge position / distance information in the first timing and the moving body in the predetermined time from the first timing to the second timing. The projected position of the second edge position / distance information by the perspective conversion in the light / dark image at the second timing, which is calculated by using the estimated movement amount of the second light / dark image, is acquired at the second timing. A self-motion estimation system characterized in that the estimated movement amount is calculated so as to approach the position of the edge in.

The self-motion estimation system according to any one of claims 1 to 3.
The movement amount calculation unit is a self-motion estimation system characterized in that, when calculating the estimated movement amount at predetermined time intervals, the most recently calculated estimated movement amount of the moving body is used as a predicted value.

The self-motion estimation system according to any one of claims 1 to 4.
When a new distance image is acquired from the distance sensor, the distance image acquisition unit includes a distance image acquired in the past and an estimated movement amount of the moving body from the time when the past distance image is acquired to the present. , Is used to project the position of the distance information included in the past distance image to the current position, and the projected distance information is added to the new distance image.

The self-motion estimation system according to any one of claims 1 to 5.
The image sensor is configured to include a first image sensor that shoots in the first shooting range and a second image sensor that shoots in the second shooting range.
The light-dark image acquired by the light-dark image acquisition unit is characterized by including a first light-dark image captured by the first image sensor and a second light-dark image captured by the second image sensor. Self-motion estimation system.

It is a control method of a self-motion estimation system that is mounted on a moving body and estimates the motion state of the moving body by detecting a change in position relative to an object in the outside world.
The self-motion estimation system acquires bright and dark images of the outside world taken at a predetermined cycle by an image sensor mounted on the moving body.
The self-motion estimation system acquires a distance image having a plurality of distance information to each point in the outside world included in the imaging range of the image sensor, which is measured by a distance sensor mounted on the moving body at a predetermined cycle. ,
The self-motion estimation system detects edges from the light-dark image and
The self-motion estimation system collates the distance image acquired at the first timing with the light-dark image, and specifies the distance information in the distance image corresponding to the edge position in the light-dark image as edge position distance information. And
The self-motion estimation system is calculated by using the edge position / distance information at the first timing and the estimated movement amount of the moving body at a predetermined time between the first timing and the second timing. The projection position of the edge position / distance information in the light / dark image at the second timing is closer to the position of the edge detected from the light / dark image acquired at the second timing. A control method of a self-motion estimation system, which comprises calculating the estimated movement amount.

A self-motion estimation system consisting of a computer mounted on a moving body and detecting a change in position relative to an object in the outside world to estimate the motion state of the moving body.
A procedure for acquiring bright and dark images of the outside world taken at a predetermined cycle by an image sensor mounted on the moving body, and
A procedure for acquiring a distance image having a plurality of distance information to each point in the outside world included in the shooting range of the image sensor, which is measured by a distance sensor mounted on the moving body at a predetermined cycle.
The procedure for detecting edges from the bright and dark images and
A procedure for collating the distance image acquired at the first timing with the light-dark image and specifying the distance information in the distance image corresponding to the edge position in the light-dark image as edge position distance information.
The edge position / distance information of the edge position / distance information calculated by using the edge position / distance information at the first timing and the estimated movement amount of the moving body at a predetermined time between the first timing and the second timing. The estimated movement amount of the moving body is calculated so that the projection position by the fluoroscopic conversion in the light-dark image at the second timing approaches the position of the edge detected from the light-dark image acquired at the second timing. Procedure and
A program to execute.