JP5530771B2 - System and method for detecting turning angle, direction, and position of moving object - Google Patents
System and method for detecting turning angle, direction, and position of moving object Download PDFInfo
- Publication number
- JP5530771B2 JP5530771B2 JP2010062212A JP2010062212A JP5530771B2 JP 5530771 B2 JP5530771 B2 JP 5530771B2 JP 2010062212 A JP2010062212 A JP 2010062212A JP 2010062212 A JP2010062212 A JP 2010062212A JP 5530771 B2 JP5530771 B2 JP 5530771B2
- Authority
- JP
- Japan
- Prior art keywords
- data
- moving body
- distance
- azimuth
- fourier transform
- 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 18
- 238000001514 detection method Methods 0.000 claims description 24
- 230000001131 transforming effect Effects 0.000 claims description 3
- 230000033001 locomotion Effects 0.000 description 19
- 239000004744 fabric Substances 0.000 description 6
- 238000010586 diagram Methods 0.000 description 4
- 238000005259 measurement Methods 0.000 description 4
- 238000007689 inspection Methods 0.000 description 2
- 230000010354 integration Effects 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 238000005070 sampling Methods 0.000 description 2
- 230000003595 spectral effect Effects 0.000 description 2
- 238000009987 spinning Methods 0.000 description 2
- 238000012360 testing method Methods 0.000 description 2
- 238000012795 verification Methods 0.000 description 2
- 238000009825 accumulation Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000008054 signal transmission Effects 0.000 description 1
Description
本発明は、移動体に搭載した距離計の測定データを用いてフーリエ変換処理をすることにより、移動体の旋回角度・方位・位置を検出するシステムおよび方法に関する。 The present invention relates to a system and method for detecting a turning angle, azimuth, and position of a moving body by performing Fourier transform processing using measurement data of a distance meter mounted on the moving body.
特許文献１には、移動体の位置および方位を自動検出する装置が開示されている。この装置はＧＰＳ受信機と速度センサとジャイロを備えている。通常はＧＰＳデータから移動体の位置と方位を検出することができる。また、ＧＰＳデータを受信できない状況でも、移動体の走行中、速度センサからのデータを積分処理することにより移動体の移動距離を演算し、ジャイロからの角速度を積分処理することにより移動体の方位からの旋回角度を演算し、これら移動距離と旋回角度に基づき、移動体の現在の位置と方位を検出するようになっている。 Patent Document 1 discloses an apparatus that automatically detects the position and orientation of a moving body. This device includes a GPS receiver, a speed sensor, and a gyro. Usually, the position and direction of the moving body can be detected from the GPS data. Even when GPS data cannot be received, the moving distance of the moving object is calculated by integrating the data from the speed sensor while the moving object is running, and the moving direction of the moving object is calculated by integrating the angular velocity from the gyro. Is calculated, and the current position and direction of the moving body are detected based on the moving distance and the turning angle.
先行技術文献として参考のために特許文献２を挙げる。この特許文献２では、対象物を撮影し、そのビデオ画像データを空間周波数でフーリエ変換し、このフーリエ変換データに基づき対象物の動きを検出する。 Patent Document 2 is cited as a prior art document for reference. In this patent document 2, an object is photographed, the video image data is Fourier transformed at a spatial frequency, and the movement of the object is detected based on the Fourier transform data.
特許文献１の位置・方位検出システムでは、高価なジャイロを必要とする。また、積分処理の際の誤差の蓄積により旋回角度の検出精度が低下する可能性もある。
また、移動体の旋回指令量から旋回角度を推測し、移動指令量により移動距離を推測して位置・方位情報を得る技術もあるが、これは移動体の滑りにより大きな誤差が生じる可能性がある。
The position / orientation detection system disclosed in Patent Document 1 requires an expensive gyro. In addition, there is a possibility that the detection accuracy of the turning angle is lowered due to accumulation of errors during the integration process.
There is also a technology for estimating the turning angle from the turning command amount of the moving body and obtaining the position / orientation information by estimating the moving distance based on the moving command amount, but this may cause a large error due to the sliding of the moving body. is there.
なお、特許文献２では空間周波数でフーリエ変換したデータを用いる点で本願発明と共通であるが、対象物の動きを検出することを目的とする点および原データとしてビデオ画像データを用いる点が本願発明と異なり、さらにフーリエ変換データの利用の仕方も本願発明と異なるものである。 Note that Patent Document 2 is common to the present invention in that data that is Fourier-transformed at a spatial frequency is used. However, the purpose of this invention is to detect motion of an object and to use video image data as original data. Unlike the invention, the method of using the Fourier transform data is also different from the present invention.
本発明は、上記課題を解決するため、移動体の旋回角度を検出するシステムにおいて、
（ア）移動体に搭載され、所定の走査角度範囲にわたって周囲の対象物までの距離データを生成する距離計と、
（イ）上記距離データ又はこの距離データに基づき演算された上記周囲対象物の二次元マップデータを、空間周波数領域においてフーリエ変換するフーリエ変換手段と、
（ウ）移動体が第１の方位から第２の方位へと旋回する際に、上記第１の方位での上記フーリエ変換データの所定次数の成分と、上記第２の方位での上記フーリエ変換データの上記所定次数の成分の位相差に基づき、移動体の旋回角度を演算する旋回角度演算手段と、
を備えたことを特徴とする。
In order to solve the above problems, the present invention provides a system for detecting a turning angle of a moving body.
(A) a distance meter mounted on a moving body and generating distance data to surrounding objects over a predetermined scanning angle range;
(A) Fourier transform means for Fourier transforming the distance data or the two-dimensional map data of the surrounding object calculated based on the distance data in a spatial frequency domain;
(C) When the moving body turns from the first azimuth to the second azimuth, the component of the predetermined order of the Fourier transform data in the first azimuth and the Fourier transform in the second azimuth A turning angle calculating means for calculating a turning angle of the moving body based on the phase difference of the predetermined order component of the data;
It is provided with.
上記システムによれば、比較的安価な距離計からの測定データを用いて移動体の旋回角度を検出することができる。
また、この旋回角度検出に際して、積分処理をしないので誤差が蓄積されることがなく、旋回指令量を用いないので移動体のすべりに伴う誤差も生じず、比較的高い検出精度が得られる。
According to the above system, the turning angle of the moving body can be detected using measurement data from a relatively inexpensive distance meter.
In addition, since the integration process is not performed when the turning angle is detected, no error is accumulated, and since no turning command amount is used, no error is caused due to slipping of the moving body, and relatively high detection accuracy is obtained.
好ましくは、上記フーリエ変換データの上記所定次数の成分が、三次成分である。この三次成分は旋回角度との相関性が高いので、旋回角度の検出精度をより高めることができる。 Preferably, the component of the predetermined order of the Fourier transform data is a third-order component. Since this tertiary component has a high correlation with the turning angle, the detection accuracy of the turning angle can be further increased.
本発明の移動体の方位検出システムは、上記旋回角度検出システムを含み、さらに、この旋回角度検出システムで検出された旋回角度と既知の第１の方位に基づき、移動体の第２の方位を演算する方位演算手段を備えている。
これにより、移動体の第２方位すなわち現在方位を検出することができる。
The moving body direction detection system of the present invention includes the turning angle detection system described above, and further determines the second direction of the moving body based on the turning angle detected by the turning angle detection system and the known first direction. An azimuth calculating means for calculating is provided.
Thereby, the 2nd azimuth | direction, ie, the present azimuth | direction, of a moving body is detectable.
本発明の移動体の位置・方位検出システムは、上記移動体の方位検出システムを含み、上記走査角度範囲が上記距離計を通る基準軸線を中心として広がっており、さらに、移動体が第１の位置から第２の位置へ上記基準軸線に沿って直線的に移動する際に、上記フーリエ変換手段で演算された第１の位置でのフーリエ変換データの一次成分と、上記第２の位置でのフーリエ変換データの一次成分の振幅差に基づき、移動体の移動距離を演算する移動距離演算手段と、上記演算された移動距離と、移動体の既知の第１の位置とこの第１位置での既知の方位に基づき、移動体の第２の位置を演算する位置演算手段とを備えている。
これによれば、移動体の第２方位すなわち現在方位のみならず、第２位置すなわち現在位置をも距離データを用いて検出することができる。
The mobile object position / orientation detection system of the present invention includes the above-mentioned mobile object orientation detection system, wherein the scanning angle range extends around a reference axis passing through the distance meter, and the mobile object is a first object. When linearly moving from the position to the second position along the reference axis, the primary component of the Fourier transform data at the first position calculated by the Fourier transform means and the second position Based on the amplitude difference of the primary component of the Fourier transform data, the moving distance calculating means for calculating the moving distance of the moving object, the calculated moving distance, the known first position of the moving object, and the first position Position calculating means for calculating the second position of the moving body based on the known orientation.
According to this, not only the second azimuth of the moving body, that is, the current azimuth, but also the second position, that is, the current position can be detected using the distance data.
移動体の旋回角度検出方法は、移動体が第１の方位から第２の方位へと旋回する際の旋回角度を検出するものであって、
（ア）移動体に搭載された距離計により、移動体が上記第１の方位にある時に所定の走査角度範囲にわたって周囲の対象物までの距離データを生成し、移動体が上記第２の方位にある時に上記走査角度範囲にわたって周囲対象物までの距離データを生成し、
（イ）上記第１、第２の方位での距離データ又はこの距離データに基づき演算された上記周囲対象物の二次元マップデータを、それぞれ空間周波数領域においてフーリエ変換し、
（ウ）上記第１方位でのフーリエ変換データの所定次数の成分と上記第２方位でのフーリエ変換データの上記所定次数の成分の位相差に基づき、移動体の旋回角度を演算する
ことを特徴とする。
The turning angle detection method of the moving body is to detect the turning angle when the moving body turns from the first direction to the second direction,
(A) A distance meter mounted on the moving body generates distance data to surrounding objects over a predetermined scanning angle range when the moving body is in the first orientation, and the moving body has the second orientation. Generating distance data to surrounding objects over the scan angle range when
(A) The distance data in the first and second directions or the two-dimensional map data of the surrounding object calculated based on the distance data is Fourier-transformed in the spatial frequency domain,
(C) calculating a turning angle of the moving body based on a phase difference between the predetermined order component of the Fourier transform data in the first direction and the predetermined order component of the Fourier transform data in the second direction. And
好ましくは、上記フーリエ変換データの上記所定次数の成分が、三次成分である。 Preferably, the component of the predetermined order of the Fourier transform data is a third-order component.
本発明の移動体の方位検出方法は、上記移動体の旋回角度検出方法を用いて検出した移動体の旋回角度と、既知の上記第１方位とに基づき、上記第２方位を検出する。 The moving body azimuth detecting method of the present invention detects the second azimuth based on the turning angle of the moving body detected using the moving body turning angle detecting method and the known first azimuth.
本発明の移動体の位置・方位検出方法は、上記移動体の方位検出方法を用いて移動体の方位を検出するとともに、移動体の位置を検出する方法であって、
上記走査角度範囲が、上記距離計を通る基準軸線を中心として広がっており、
移動体が第１の位置から第２の位置へ上記基準軸線に沿って直線的に移動する際に、上記距離計により、移動体が上記第１の位置にある時に上記走査角度範囲にわたって周囲の対象物までの距離データを生成し、移動体が上記第２の位置にある時に上記走査角度範囲にわたって周囲対象物までの距離データを生成し、
上記第１、第２の位置での距離データ又はこの距離データに基づき演算された上記周囲対象物の二次元マップデータを、それぞれ空間周波数領域においてフーリエ変換し、
上記第１の位置でのフーリエ変換データの一次成分と、上記第２の位置でのフーリエ変換データの一次成分の振幅差に基づき、移動体の移動距離を演算し、
上記の演算された移動距離と、既知の第１の位置とこの第１位置での既知の方位に基づき、移動体の第２の位置を演算する。
The mobile object position / orientation detection method of the present invention is a method for detecting the orientation of a mobile object using the above-mentioned mobile object orientation detection method, and detecting the position of the mobile object,
The scanning angle range extends around a reference axis passing through the distance meter;
When the moving body linearly moves along the reference axis from the first position to the second position, the distance meter causes the surroundings to be moved over the scanning angle range when the moving body is at the first position. Generating distance data to an object, generating distance data to surrounding objects over the scanning angle range when the moving body is at the second position,
The distance data at the first and second positions or the two-dimensional map data of the surrounding object calculated based on the distance data is Fourier-transformed in the spatial frequency domain,
Based on the amplitude difference between the primary component of the Fourier transform data at the first position and the primary component of the Fourier transform data at the second position, the moving distance of the moving object is calculated,
Based on the calculated moving distance, the known first position, and the known orientation at the first position, the second position of the moving body is calculated.
本発明によれば、安価な距離計を用いて、負担の軽いフーリエ変換処理により、移動体の旋回角度、方位、位置を比較的高い精度で検出することができる。 According to the present invention, it is possible to detect the turning angle, azimuth, and position of a moving body with relatively high accuracy by an inexpensive distance meter and a light Fourier transform process.
以下、本発明の一実施形態に係わるクローラロボット１０（移動体）の位置・方位検出システムについて、図面を参照しながら説明する。
説明の便宜上、図１に示すようにクローラロボット１０を基準として、その前後に延びる座標軸をＹ軸とし、左右に延びる座標軸をＸ軸とする。クローラロボット１０のＹ軸に沿って延びる中心軸線（基準軸線）を符号１０ａで示す。
A crawler robot 10 (moving body) position / orientation detection system according to an embodiment of the present invention will be described below with reference to the drawings.
For convenience of explanation, with reference to the crawler robot 10 as shown in FIG. 1, a coordinate axis extending forward and backward is defined as a Y axis, and a coordinate axis extending left and right is defined as an X axis. A central axis (reference axis) extending along the Y axis of the crawler robot 10 is denoted by reference numeral 10a.
本実施形態のクローラロボット１０は、軽量小型であり、前後方向に細長いボデイ１１と、このボデイ１１の左右に設けられた一対のクローラ１２とを備えている。
上記ボデイ１１には、上記中心軸線１０ａ上に位置するビデオカメラ１５とレーザー距離計１６が搭載されるとともに、送受信機１７が搭載されている。
The crawler robot 10 of this embodiment is light and small, and includes a body 11 that is elongated in the front-rear direction and a pair of crawlers 12 provided on the left and right sides of the body 11.
The body 11 is equipped with a video camera 15 and a laser distance meter 16 positioned on the central axis 10a and a transceiver 17.
他方、基地局２０はパソコンを主要構成としており、送受信機２１と、制御・演算部２２（図２参照）を備えるとともに、ディスプレイとリモートコントローラ（いずれも図示せず）を備えている。送受信機１７，２１間では、無線または有線による信号伝送が行われる。これら構成は周知であるので詳細な説明を省略する。 On the other hand, the base station 20 has a personal computer as a main component, and includes a transmitter / receiver 21, a control / calculation unit 22 (see FIG. 2), and a display and a remote controller (both not shown). Between the transceivers 17 and 21, signal transmission by radio or wire is performed. Since these structures are well known, detailed description thereof is omitted.
ビデオカメラ１５で撮影された映像は基地局２０のディスプレイで表示される。操作者は、ディスプレイの映像を見ながらリモートコントローラを操作してクローラロボット１２を移動させる。 The video imaged by the video camera 15 is displayed on the display of the base station 20. The operator moves the crawler robot 12 by operating the remote controller while viewing the video on the display.
上記レーザー距離計１６は、クローラロボット１０の前方および左右にわたって、より正確には上記中心軸線１０ａを中心に角度範囲Θにわたって、水平面上を走査して距離測定を行う。この角度Θは好ましくは１８０〜２４０°であり、本実施形態では２００°である。なお、距離計測は、角度Θを均等に角度分割した１２８のサンプリングポイントで行うようになっている。 The laser distance meter 16 scans a horizontal plane over the front, left and right of the crawler robot 10, more precisely over the angle range Θ around the central axis 10 a, and performs distance measurement. This angle Θ is preferably 180 to 240 °, and 200 ° in the present embodiment. The distance measurement is performed at 128 sampling points obtained by equally dividing the angle Θ.
図２に示すように、上記基地局２０の制御・演算部２２は後述するルーチンを実行し、実質的に、マップ作成手段２４と、ＦＦＴ２５（高速フーリエ変換手段：フーリエ変換手段）と、移動距離演算手段２６と、位置演算手段２７と、旋回角度演算手段２８と、方位演算手段２９とを備えている。 As shown in FIG. 2, the control / calculation unit 22 of the base station 20 executes a routine to be described later, and substantially includes a map creation means 24, an FFT 25 (fast Fourier transform means: Fourier transform means), and a moving distance. Calculation means 26, position calculation means 27, turning angle calculation means 28, and azimuth calculation means 29 are provided.
上記構成をなすクローラロボット１０を家屋の床下点検に用いた場合の位置・方位検出システムの作用について説明する。床下には布基礎１００（周囲対象物）が配置されている。操作者は、クローラロボット１０を移動させ、ディスプレイに表示された映像をみながら床下の状態を点検する。 The operation of the position / orientation detection system when the crawler robot 10 having the above-described configuration is used for an underfloor inspection of a house will be described. A cloth foundation 100 (surrounding object) is disposed under the floor. The operator moves the crawler robot 10 and checks the state under the floor while watching the video displayed on the display.
本実施形態では、クローラロボット１０の移動制御は、中心軸線１０ａに沿った前後の直線的移動と、超信地旋回（位置を変えずにその場での旋回）に限定する。
クローラロボット１０が図３に示す第１の位置・方位にある場合を例にとって説明する。図３の状態では、クローラロボット１０は、布基礎１００において２つの壁面１０１が直角に交差した隅部１０２の近傍に配置されている。クローラロボット１０の中心軸線１０ａが上記隅部１０２を通り、２つの壁面１０１と同角度をなして交差している。
In the present embodiment, the movement control of the crawler robot 10 is limited to the linear movement before and after the central axis 10a and the super-spinning (turning on the spot without changing the position).
A case where the crawler robot 10 is in the first position / orientation shown in FIG. 3 will be described as an example. In the state of FIG. 3, the crawler robot 10 is disposed in the vicinity of a corner 102 where two wall surfaces 101 intersect at a right angle in the cloth foundation 100. The central axis 10a of the crawler robot 10 passes through the corner 102 and intersects the two wall surfaces 101 at the same angle.
上記クローラロボット１０が直線的移動または超信地旋回をした場合、クローラロボット１０は上記第１の位置・方位から第２の位置・方位に至る。上記クローラロボット１０が図３に示すように直線移動する際には、第１の方位と第２の方位が等しく、第１の位置から第２の位置へと変化する。図７に示すように超信地旋回する際には、第１の位置と第２の位置が等しく、第１の方位から第２の方位へと変化する。 When the crawler robot 10 moves linearly or performs a super-turn, the crawler robot 10 reaches the second position / orientation from the first position / orientation. When the crawler robot 10 moves linearly as shown in FIG. 3, the first azimuth and the second azimuth are equal and change from the first position to the second position. As shown in FIG. 7, when making a super turn, the first position and the second position are the same, and the first direction changes to the second direction.
クローラロボット１０が上記第１の位置・方位にある時、レーザー距離計１６により、角度Θにわたりレーザー距離計１６から壁面１０１までの距離データを得る。上記制御・演算部２２ではこの距離データに基づき、図４（Ａ）、図８（Ａ）に実線で示すような壁面１０１のＸ，Ｙ座標における二次元マップデータを作成する。 When the crawler robot 10 is in the first position / orientation, the laser distance meter 16 obtains distance data from the laser distance meter 16 to the wall surface 101 over an angle Θ. Based on the distance data, the control / calculation unit 22 creates two-dimensional map data in the X and Y coordinates of the wall surface 101 as shown by the solid lines in FIGS. 4 (A) and 8 (A).
クローラロボット１０が上記第２の位置・方位にある時にも同様にして距離データを得、二次元マップデータを作成する。
図３に矢印で示すように、クローラロボット１０を中心軸線１０ａに沿って直線的に移動させた場合、例えば布基礎１００の隅部１０２から遠ざかるように後退させた場合には、壁面１０１のマップは図４（Ａ）に破線で示すようになる。また、超信地旋回させた場合のマップは図８（Ａ）に破線で示すようになる。
Similarly, when the crawler robot 10 is in the second position / orientation, distance data is obtained and two-dimensional map data is created.
When the crawler robot 10 is linearly moved along the central axis 10a as shown by an arrow in FIG. 3, for example, when the crawler robot 10 is moved away from the corner 102 of the cloth foundation 100, a map of the wall surface 101 is used. Is indicated by a broken line in FIG. Further, the map in the case of a super-revolution is shown by a broken line in FIG.
上記制御・演算部２２における二次元マップ作成処理は、図２のマップ作成手段２４に相当する。上記の壁面１０１のマップはクローラロボット１０が第１位置・方位にある時および第２位置・方位にある時、基地局２０のディスプレイに表示される。 The two-dimensional map creation process in the control / calculation unit 22 corresponds to the map creation means 24 in FIG. The map of the wall surface 101 is displayed on the display of the base station 20 when the crawler robot 10 is in the first position / orientation and when it is in the second position / orientation.
上記制御・演算部２２では、上記第１の位置・方位でのマップデータおよび第２の位置・方位でのマップデータをそれぞれ空間周波数においてフーリエ変換し、記憶する。このフーリエ変換処理は、図２のＦＦＴ２５に相当する。このフーリエ変換処理は、クローラロボット１０が各位置・方位にある時に実行してもよいし、クローラロボットの移動終了後に行ってもよい。 In the control / calculation unit 22, the map data at the first position / orientation and the map data at the second position / orientation are Fourier-transformed at the spatial frequency and stored. This Fourier transform process corresponds to the FFT 25 in FIG. This Fourier transform process may be performed when the crawler robot 10 is in each position and orientation, or may be performed after the movement of the crawler robot is completed.
上記制御・演算手段２２は、上記リモートコントローラからの操作情報（直線的移動か超信地旋回かの情報）に応答して以下のルーチンを実行する。
クローラロボット１０が直線的移動例えば上述したように後退動作をする際には、制御・演算手段２２は第１の位置・方位でのフーリエ変換データから図４（Ｂ）に実線で示す一次成分（周波数が「１」の成分）を抽出するとともに、第２の位置・方位でのフーリエ変換データから図４（Ｂ）に破線で示す一次成分を抽出する。なお、図４（Ｂ）において横軸は、位相（または周期）を表し、縦軸は振幅を表す。横軸の数字は０〜１２７のサンプリングポイントを示す。
The control / calculation means 22 executes the following routine in response to operation information from the remote controller (information on linear movement or super turning).
When the crawler robot 10 moves linearly, for example, as described above, the control / calculation means 22 obtains a primary component (shown by a solid line in FIG. 4B) from the Fourier transform data at the first position / orientation. Component having a frequency of “1”), and a primary component indicated by a broken line in FIG. 4B is extracted from Fourier transform data at the second position / orientation. In FIG. 4B, the horizontal axis represents the phase (or period), and the vertical axis represents the amplitude. The numbers on the horizontal axis indicate 0 to 127 sampling points.
さらに、上記第１の位置・方位での一次成分と第２の位置・方位での一次成分の振幅の差ΔＷを演算し、この振幅差ΔＷから直線移動距離を演算する。上記一次成分の振幅差ΔＷと直線移動距離は、図５に示す検証試験結果に示すように相関係数０．９９８３のリニアな関係を有している。この演算処理は、図２の移動距離演算手段２６に相当する。 Further, the difference ΔW in amplitude between the primary component at the first position / orientation and the primary component at the second position / orientation is calculated, and the linear movement distance is calculated from the amplitude difference ΔW. The amplitude difference ΔW of the first-order component and the linear movement distance have a linear relationship with a correlation coefficient of 0.9983 as shown in the verification test result shown in FIG. This calculation process corresponds to the movement distance calculation means 26 of FIG.
参考までに、上記直線移動の際の、第１の位置・方位および第２の位置・方位でのフーリエ変換データの三次成分を図４（Ｃ）に実線および破線で示す。この三次成分でも振幅差ΔＷ’が生じるが、一次成分の振幅差ΔＷより遥かに小さい。
本発明者は、所定距離だけ直線的に移動した時の一次〜四次のスペクトル成分の振幅差を測定し、図６のようなデータを得た。このデータからも、一次成分の振幅差が最大であることが理解できる。
そこで、本実施形態では上述したように、一次成分の振幅差ΔＷを用いて移動距離を演算したのである。
For reference, the cubic component of the Fourier transform data at the first position / orientation and the second position / orientation during the linear movement is shown by a solid line and a broken line in FIG. The amplitude difference ΔW ′ occurs even with this third-order component, but is much smaller than the amplitude difference ΔW of the first-order component.
The inventor measured the amplitude difference between the first to fourth spectral components when moving linearly by a predetermined distance, and obtained data as shown in FIG. It can be understood from this data that the amplitude difference of the primary component is the maximum.
Therefore, in this embodiment, as described above, the movement distance is calculated using the amplitude difference ΔW of the primary component.
次に、上記第１の位置・方位と上記演算された直線移動距離に基づき、第２の位置すなわち現在の位置を演算する。上記第１の位置・方位は最初にクローラロボット１０を床下に置く時に操作者が入力されるか、制御・演算部２２で演算された位置・方位であり、既知である。この処理は図２の位置演算手段２７に相当する。 Next, the second position, that is, the current position is calculated based on the first position / orientation and the calculated linear movement distance. The first position / orientation is a position / orientation that is input by an operator when the crawler robot 10 is first placed under the floor or is calculated by the control / calculation unit 22 and is known. This processing corresponds to the position calculation means 27 in FIG.
上述したようにして制御・演算部２２は第２の位置を検出する。なお、直線的移動の場合、第２の方位は既知の第１の方位と等しいので、制御・演算部２２は現在の位置・方位（第２の位置・方位）を検出したことになる。この位置・方位は、次の移動（直線的移動または超信地旋回）に際して、第１の位置・方位として提供される。
クローラロボット１０を前進する場合も、上記と同様の演算により現在の位置・方位（第２の位置・方位）を検出することができる。
As described above, the control / calculation unit 22 detects the second position. In the case of linear movement, since the second azimuth is equal to the known first azimuth, the control / calculation unit 22 has detected the current position / azimuth (second position / azimuth). This position / orientation is provided as the first position / orientation in the next movement (linear movement or super-spinning).
Even when the crawler robot 10 moves forward, the current position / orientation (second position / orientation) can be detected by the same calculation as described above.
クローラロボット１０が超信地旋回の場合には、制御・演算手段２２は第１の位置・方位でのフーリエ変換データから図８（Ｃ）に実線で示す三次成分（周波数が「３」の成分、換言すれば角度Θで３周期となる空間周波数をもつ成分）を抽出するとともに、第２の位置・方位でのフーリエ変換データから図８（Ｃ）に破線で示す三次成分を抽出する。 When the crawler robot 10 is in a super turn, the control / calculation means 22 uses the third-order component (the component whose frequency is “3”) shown by the solid line in FIG. In other words, a component having a spatial frequency of three periods at an angle Θ) is extracted, and a tertiary component indicated by a broken line in FIG. 8C is extracted from Fourier transform data at the second position / orientation.
さらに、上記第１の位置・方位での三次成分と第２の位置・方位での三次成分の位相差αを演算し、この位相差αから旋回角度を演算する。上記三次成分の位相差αと旋回角度は、図９に示す検証試験結果に示すように相関係数０．９９７９のリニアな関係を有している。この演算処理は、図２の旋回角度演算手段２８に相当する。 Further, the phase difference α between the tertiary component at the first position / orientation and the tertiary component at the second position / orientation is calculated, and the turning angle is calculated from the phase difference α. The third-order component phase difference α and the turning angle have a linear relationship with a correlation coefficient of 0.9979 as shown in the verification test results shown in FIG. This calculation process corresponds to the turning angle calculation means 28 of FIG.
参考までに、上記超信地旋回の際の、第１の位置・方位および第２の位置・方位でのフーリエ変換データの一次成分を図８（Ｂ）に実線および破線で示す。この一次成分での位相差α’は三次成分の位相差αより小さい。
本発明者は、所定角度だけ超信地旋回時の一次〜四次のスペクトル成分の位相差を測定し、図１０のようなデータを得た。このデータからも、三次成分の位相差が最大であることが理解できる。
そこで、本実施形態では上述したように、三次成分の振幅差αを用いて旋回角度を演算したのである。
For reference, the primary components of the Fourier transform data at the first position / orientation and the second position / orientation at the time of the above super turn are shown by a solid line and a broken line in FIG. The phase difference α ′ at the first order component is smaller than the phase difference α at the third order component.
The present inventor measured the phase difference of the primary to quaternary spectral components during the turning of the superstrate by a predetermined angle and obtained data as shown in FIG. From this data, it can be understood that the phase difference of the third-order component is the maximum.
Therefore, in this embodiment, as described above, the turning angle is calculated using the amplitude difference α of the tertiary component.
次に、既知の第１の方位と上記演算された旋回角度に基づき、第２の方位すなわち現在の方位を演算する。この処理は、図２の方位演算手段２９に相当する。
上述したようにして制御・演算部２２は第２の方位を検出する。なお、超信地旋回の場合、第２の位置は第１の位置と等しいので、制御・演算部２２は、現在の位置・方位（第２の位置・方位）を検出したことになる。
Next, based on the known first azimuth and the calculated turning angle, the second azimuth, that is, the current azimuth is calculated. This process corresponds to the azimuth calculating means 29 in FIG.
As described above, the control / calculation unit 22 detects the second orientation. In addition, in the case of super turning, since the second position is equal to the first position, the control / calculation unit 22 has detected the current position / azimuth (second position / azimuth).
本発明は上記実施例に制約されず、種々の態様を採用することができる。例えば、上記実施形態では距離データからＸ，Ｙ軸の２次元マップを作成し、このマップデータをフーリエ変換したが、距離データを直接フーリエ変換してもよい。 The present invention is not limited to the above-described embodiments, and various aspects can be adopted. For example, in the above embodiment, a two-dimensional map of the X and Y axes is created from the distance data, and the map data is Fourier transformed. However, the distance data may be directly Fourier transformed.
上記実施形態では旋回角度演算のためにフーリエ変換データの三次成分を用いたが、一次、二次、四次以降の成分を用いてもよいし、２以上の成分に基づいて演算された旋回角度を評価し、平均化処理して最終的な旋回角度を得てもよい。 In the above embodiment, the tertiary component of the Fourier transform data is used for the calculation of the turning angle. However, the primary, secondary, quaternary and subsequent components may be used, and the turning angle calculated based on two or more components. May be evaluated and averaged to obtain a final turning angle.
上記実施形態では直線的移動または超信地旋回の前後における移動距離、旋回角度、位置、方位を演算したが、これら演算を移動の過程で所定時間間隔毎に行なってもよい。 In the above embodiment, the movement distance, the turning angle, the position, and the direction before and after the linear movement or the super-sense turn are calculated. However, these calculations may be performed at predetermined time intervals during the movement.
本実施形態では、距離計からの距離データだけに基づいて、移動体の旋回角度、移動距離、方位、位置の演算を行ったが、例えばＧＰＳと併用し、ＧＰＳの電波が届かない場所で本実施形態の検出システムを作動させるようにしてもよい。
また、本実施形態の検出システムと、ジャイロ、速度センサを含む位置・方位検出システムとの併用も可能である。
In the present embodiment, the turning angle, moving distance, direction, and position of the moving object are calculated based only on the distance data from the distance meter. You may make it operate the detection system of embodiment.
Further, the detection system of the present embodiment can be used in combination with the position / orientation detection system including the gyroscope and the speed sensor.
本発明は、床下点検ロボット等の移動体の位置・方位の検出システムとして用いることができる。 The present invention can be used as a position / orientation detection system for a moving body such as an underfloor inspection robot.
１０ クローラロボット（移動体）
１６ 距離計
２４ マップ作成手段
２５ ＦＦＴ（フーリエ変換手段）
２６ 移動距離演算手段
２７ 位置演算手段
２８ 旋回角度演算手段
２９ 方位演算手段
10 Crawler robot (moving body)
16 Distance meter 24 Map creation means 25 FFT (Fourier transform means)
26 Moving distance calculating means 27 Position calculating means 28 Turning angle calculating means 29 Direction calculating means
Claims (4)
（ア）移動体に搭載され、所定の走査角度範囲にわたって周囲の対象物までの距離データを生成する距離計と、
（イ）上記距離データ又はこの距離データに基づき演算された上記周囲対象物の二次元マップデータを、空間周波数領域においてフーリエ変換するフーリエ変換手段と、
（ウ）移動体が第１の方位から第２の方位へと旋回する際に、上記第１の方位での上記フーリエ変換データの三次成分と、上記第２の方位での上記フーリエ変換データの三次成分の位相差に基づき、移動体の旋回角度を演算する旋回角度演算手段と、
を備えたことを特徴とする移動体の旋回角度検出システム。 In a system for detecting the turning angle of a moving object,
(A) a distance meter mounted on a moving body and generating distance data to surrounding objects over a predetermined scanning angle range;
(A) Fourier transform means for Fourier transforming the distance data or the two-dimensional map data of the surrounding object calculated based on the distance data in a spatial frequency domain;
(C) When the moving body turns from the first azimuth to the second azimuth, the third order component of the Fourier transform data in the first azimuth and the Fourier transform data in the second azimuth A turning angle calculating means for calculating a turning angle of the moving body based on the phase difference of the tertiary component;
A turning angle detection system for a moving body.
（ア）移動体に搭載され、所定の走査角度範囲にわたって周囲の対象物までの距離データを生成する距離計と、
（イ）上記距離データ又はこの距離データに基づき演算された上記周囲対象物の二次元マップデータを、空間周波数領域においてフーリエ変換するフーリエ変換手段と、
（ウ）移動体が第１の方位から第２の方位へと旋回する際に、上記第１の方位での上記フーリエ変換データの所定次数の成分と、上記第２の方位での上記フーリエ変換データの上記所定次数の成分の位相差に基づき、移動体の旋回角度を演算する旋回角度演算手段と、
（エ）上記旋回角度検出システムで検出された旋回角度と既知の第１の方位に基づき、移動体の第２の方位を演算する方位演算手段と、
を備え、上記距離計の所定の走査角度範囲が上記距離計を通る基準軸線を中心として広がっており、さらに、
（オ）移動体が第１の位置から第２の位置へ上記基準軸線に沿って直線的に移動する際に、上記フーリエ変換手段で演算された第１の位置でのフーリエ変換データの一次成分と、上記第２の位置でのフーリエ変換データの一次成分の振幅差に基づき、移動体の移動距離を演算する移動距離演算手段と、
（カ）上記演算された移動距離と、移動体の既知の第１の位置とこの第１位置での既知の方位に基づき、移動体の第２の位置を演算する位置演算手段と、
を備えたことを特徴とする移動体の位置・方位検出システム。 In a system for detecting the position and orientation of a moving object,
(A) a distance meter mounted on a moving body and generating distance data to surrounding objects over a predetermined scanning angle range;
(A) Fourier transform means for Fourier transforming the distance data or the two-dimensional map data of the surrounding object calculated based on the distance data in a spatial frequency domain;
(C) When the moving body turns from the first azimuth to the second azimuth, the component of the predetermined order of the Fourier transform data in the first azimuth and the Fourier transform in the second azimuth A turning angle calculating means for calculating a turning angle of the moving body based on the phase difference of the predetermined order component of the data;
(D) azimuth calculating means for calculating the second azimuth of the mobile body based on the turning angle detected by the turning angle detection system and the known first azimuth;
A predetermined scanning angle range of the distance meter extends around a reference axis passing through the distance meter, and
(E) When the moving body linearly moves from the first position to the second position along the reference axis, the primary component of the Fourier transform data at the first position calculated by the Fourier transform means And a moving distance calculating means for calculating the moving distance of the moving body based on the amplitude difference of the primary component of the Fourier transform data at the second position,
(F) Position calculating means for calculating the second position of the moving body based on the calculated moving distance, the known first position of the moving body and the known azimuth at the first position;
A position / orientation detection system for a moving object.
（ア）移動体に搭載された距離計により、移動体が上記第１の方位にある時に所定の走査角度範囲にわたって周囲の対象物までの距離データを生成し、移動体が上記第２の方位にある時に上記走査角度範囲にわたって周囲対象物までの距離データを生成し、
（イ）制御・演算手段により、上記第１、第２の方位での距離データ又はこの距離データに基づき演算された上記周囲対象物の二次元マップデータを、それぞれ空間周波数領域においてフーリエ変換し、さらに、上記第１方位でのフーリエ変換データの三次成分と上記第２方位でのフーリエ変換データの三次成分の位相差に基づき、移動体の旋回角度を演算する
ことを特徴とする移動体の旋回角度検出方法。 In a method of detecting a turning angle when a moving body turns from a first direction to a second direction,
(A) A distance meter mounted on the moving body generates distance data to surrounding objects over a predetermined scanning angle range when the moving body is in the first orientation, and the moving body has the second orientation. Generating distance data to surrounding objects over the scan angle range when
(B) The distance data in the first and second directions or the two-dimensional map data of the surrounding object calculated based on the distance data is Fourier-transformed in the spatial frequency domain by the control / calculation means , Furthermore, the turning angle of the moving object is calculated based on the phase difference between the third order component of the Fourier transform data in the first direction and the third order component of the Fourier transform data in the second direction. Angle detection method.
移動体に搭載された距離計により、移動体が上記第１の方位にある時に、上記距離計を通る基準軸線を中心として広がる走査角度範囲にわたって周囲の対象物までの距離データを生成し、移動体が上記第２の方位にある時に上記走査角度範囲にわたって周囲対象物までの距離データを生成し、
制御・演算手段が、
（ア）上記第１、第２の方位での距離データ又はこの距離データに基づき演算された上記周囲対象物の二次元マップデータを、それぞれ空間周波数領域においてフーリエ変換し、
（イ）上記第１方位でのフーリエ変換データの所定次数の成分と上記第２方位でのフーリエ変換データの上記所定次数の成分の位相差に基づき、移動体の旋回角度を演算し、
（ウ）上記演算された移動体の旋回角度と、既知の上記第１方位とに基づき、上記第２方位を演算し、
（エ）移動体が第１の位置から第２の位置へ上記基準軸線に沿って直線的に移動する際に、上記距離計により、移動体が上記第１の位置にある時に上記走査角度範囲にわたって周囲の対象物までの距離データを生成し、移動体が上記第２の位置にある時に上記走査角度範囲にわたって周囲対象物までの距離データを生成し、
（オ）上記第１、第２の位置での距離データ又はこの距離データに基づき演算された上記周囲対象物の二次元マップデータを、それぞれ空間周波数領域においてフーリエ変換し、
（カ）上記第１の位置でのフーリエ変換データの一次成分と、上記第２の位置でのフーリエ変換データの一次成分の振幅差に基づき、移動体の移動距離を演算し、
（キ）上記の演算された移動距離と、既知の第１の位置とこの第１位置での既知の方位に基づき、移動体の第２の位置を演算する
ことを特徴とする移動体の位置・方位検出方法。 In a method for detecting the position and orientation of a moving object,
When the moving body is in the first orientation, the distance data mounted on the moving body generates distance data to surrounding objects over a scanning angle range that extends around the reference axis passing through the distance meter , and moves Generating distance data to surrounding objects over the scan angle range when the body is in the second orientation;
Control / calculation means
(A) The distance data in the first and second directions or the two-dimensional map data of the surrounding object calculated based on the distance data is Fourier-transformed in the spatial frequency domain,
(A) Based on the phase difference between the predetermined order component of the Fourier transform data in the first azimuth and the predetermined order component of the Fourier transform data in the second azimuth, the turning angle of the moving object is calculated ,
(C) Calculate the second azimuth based on the calculated turning angle of the moving body and the known first azimuth,
(D) When the moving body is linearly moved from the first position to the second position along the reference axis, the range of the scanning angle is obtained when the moving body is at the first position by the distance meter. Generating distance data to surrounding objects over the scan angle range when the moving body is in the second position,
(E) The distance data at the first and second positions or the two-dimensional map data of the surrounding object calculated based on the distance data is Fourier-transformed in the spatial frequency domain,
(F) calculating the moving distance of the moving body based on the amplitude difference between the primary component of the Fourier transform data at the first position and the primary component of the Fourier transform data at the second position;
(G) Calculate the second position of the moving body based on the calculated moving distance, the known first position, and the known orientation at the first position.
A method for detecting the position / orientation of a moving object.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2010062212A JP5530771B2 (en) | 2010-03-18 | 2010-03-18 | System and method for detecting turning angle, direction, and position of moving object |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2010062212A JP5530771B2 (en) | 2010-03-18 | 2010-03-18 | System and method for detecting turning angle, direction, and position of moving object |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2011196761A JP2011196761A (en) | 2011-10-06 |
| JP5530771B2 true JP5530771B2 (en) | 2014-06-25 |
Family
ID=44875190
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2010062212A Expired - Fee Related JP5530771B2 (en) | 2010-03-18 | 2010-03-18 | System and method for detecting turning angle, direction, and position of moving object |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP5530771B2 (en) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104635734A (en) * | 2014-12-09 | 2015-05-20 | 华北电力大学 | Method for tracking trajectories of tracked robots |
| CN104734633B (en) * | 2015-01-28 | 2017-04-19 | 秦皇岛博硕光电设备股份有限公司 | Test contact for solar cell module |
Family Cites Families (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP3279064B2 (en) * | 1994-06-13 | 2002-04-30 | 三菱電機株式会社 | Spacecraft attitude determination device |
| JP2002074577A (en) * | 2000-08-29 | 2002-03-15 | Fujitsu Ltd | Inter-vehicle communication system and device therefor |
| JP2009216470A (en) * | 2008-03-07 | 2009-09-24 | Fujitsu Ten Ltd | Radar system and vehicle control system |
-
2010
- 2010-03-18 JP JP2010062212A patent/JP5530771B2/en not_active Expired - Fee Related
Also Published As
| Publication number | Publication date |
|---|---|
| JP2011196761A (en) | 2011-10-06 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US7436522B2 (en) | Method for determining the 3D coordinates of the surface of an object | |
| JP5459678B2 (en) | Mobile image tracking device | |
| Kim et al. | Developing accurate long-distance 6-DOF motion detection with one-dimensional laser sensors: Three-beam detection system | |
| Nienaber et al. | A comparison of low-cost monocular vision techniques for pothole distance estimation | |
| JP4660387B2 (en) | Apparatus and method for correcting position information of moving body, computer-readable recording medium storing computer program for controlling the apparatus | |
| Liu et al. | An external parameter calibration method for multiple cameras based on laser rangefinder | |
| US11426876B2 (en) | Information processing apparatus, information processing method, and program | |
| JP2013221799A (en) | Shape measurement device and shape measurement method | |
| Sels et al. | Three-dimensional full-field vibration measurements using a handheld single-point laser Doppler vibrometer | |
| Chou et al. | Encoder-camera-ground penetrating radar sensor fusion: Bimodal calibration and subsurface mapping | |
| US20130021452A1 (en) | Three-dimensional distance measurement apparatus and method therefor | |
| JP5669195B2 (en) | Surface shape measuring device and surface shape measuring method | |
| JP5530771B2 (en) | System and method for detecting turning angle, direction, and position of moving object | |
| KR101403377B1 (en) | Method for calculating 6 dof motion of object by using 2d laser scanner | |
| Chou et al. | Mirror-assisted calibration of a multi-modal sensing array with a ground penetrating radar and a camera | |
| CN103455038B (en) | A kind of electronic equipment and the method for adjustment direction | |
| KR101359649B1 (en) | obstacle detection sensor | |
| JP2011053165A (en) | Device and method for detecting position of moving carriage of trackless type | |
| JP2015075415A (en) | Ultrasonic flaw detection apparatus and ultrasonic flaw detection method | |
| hyeuk Lee et al. | Elbow detection for localization of a mobile robot inside pipeline using laser pointers | |
| CN103185567B (en) | Electronic apparatus and method for measuring distance | |
| JP5230354B2 (en) | POSITIONING DEVICE AND CHANGED BUILDING DETECTION DEVICE | |
| TWI748234B (en) | Three-dimensional reconstruction device, three-dimensional reconstruction system, three-dimensional reconstruction method, and recording medium for recording three-dimensional reconstruction program | |
| JP5605491B2 (en) | Chassis dynamometer | |
| JP2007085825A (en) | Apparatus for estimating arrival direction |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20130130 |
|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20131212 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20131217 |
|
| A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20140131 |
|
| 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: 20140415 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20140421 |
|
| R150 | Certificate of patent or registration of utility model |
Ref document number: 5530771 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
| LAPS | Cancellation because of no payment of annual fees |