JP6664162B2 - Autonomous flying robot - Google Patents
Autonomous flying robot Download PDFInfo
- Publication number
- JP6664162B2 JP6664162B2 JP2015151739A JP2015151739A JP6664162B2 JP 6664162 B2 JP6664162 B2 JP 6664162B2 JP 2015151739 A JP2015151739 A JP 2015151739A JP 2015151739 A JP2015151739 A JP 2015151739A JP 6664162 B2 JP6664162 B2 JP 6664162B2
- Authority
- JP
- Japan
- Prior art keywords
- voxel
- wind speed
- value
- flying robot
- voxels
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 description 42
- 238000012545 processing Methods 0.000 description 34
- 238000011156 evaluation Methods 0.000 description 29
- 238000001514 detection method Methods 0.000 description 26
- 238000004891 communication Methods 0.000 description 13
- 238000010586 diagram Methods 0.000 description 12
- 230000014509 gene expression Effects 0.000 description 10
- 238000003384 imaging method Methods 0.000 description 10
- 238000000926 separation method Methods 0.000 description 10
- 230000006870 function Effects 0.000 description 7
- 230000003287 optical effect Effects 0.000 description 4
- RZVHIXYEVGDQDX-UHFFFAOYSA-N 9,10-anthraquinone Chemical compound C1=CC=C2C(=O)C3=CC=CC=C3C(=O)C2=C1 RZVHIXYEVGDQDX-UHFFFAOYSA-N 0.000 description 3
- 238000012544 monitoring process Methods 0.000 description 3
- 239000000523 sample Substances 0.000 description 3
- 238000007796 conventional method Methods 0.000 description 2
- 238000012937 correction Methods 0.000 description 2
- 230000007423 decrease Effects 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000002474 experimental method Methods 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 230000005856 abnormality Effects 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 230000012447 hatching Effects 0.000 description 1
- 238000010191 image analysis Methods 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 230000004044 response Effects 0.000 description 1
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Description
本発明は、移動空間内を自律的に飛行する自律飛行ロボットに関する。 The present invention relates to an autonomous flying robot that flies autonomously in a moving space.
従来、現在位置から所定の移動目標位置に至る移動経路を算出し、当該移動経路に沿うよう自律的に飛行可能な自律飛行ロボットが提案されている。例えば、特許文献１には、賊等の移動物体から所定の離間距離だけ離れた位置に移動目標位置を設定し、現在位置から当該移動目標位置に至る移動経路を生成し、当該移動経路に沿うよう飛行制御される自律飛行ロボットが開示されている。 2. Description of the Related Art Conventionally, an autonomous flying robot that calculates a moving path from a current position to a predetermined moving target position and can fly autonomously along the moving path has been proposed. For example, in Patent Literature 1, a movement target position is set at a position separated by a predetermined separation distance from a moving object such as a thief, a movement path from the current position to the movement target position is generated, and the moving path is along the movement path. An autonomous flying robot that is flight controlled is disclosed.
当該従来技術では、生成した移動経路と現在の自己位置との差異が小さくなるようモータの出力（回転数）を制御することによって、移動経路に追従するよう自律飛行している。そのため、移動空間内に風が生じていた場合、風に抗して機体が正しく進むよう各モータの出力が制御される。例えば、横風の場合は、当該横風に流されないよう逆らう方向に対してモータ出力が大きくなるよう飛行制御される。 In the related art, the autonomous flight is performed so as to follow the moving path by controlling the output (rotation speed) of the motor so that the difference between the generated moving path and the current self-position is reduced. Therefore, when wind is generated in the moving space, the output of each motor is controlled so that the aircraft advances properly against the wind. For example, in the case of a crosswind, flight control is performed so that the motor output increases in the direction against which the wind is not swept.
しかしながら、このような飛行制御の場合、突風が生じたときに対応が追い付かず障害物に衝突してしまう恐れがあった。また、急に風向きが変化した場合も、機体の傾斜する方向が前後方向や左右方向に大きく変化して、機体動揺してしまい、障害物に衝突してしまう恐れがあった。 However, in the case of such a flight control, when a gust occurs, the response cannot catch up, and there is a risk of colliding with an obstacle. In addition, even when the wind direction changes suddenly, the direction in which the aircraft inclines greatly changes in the front-rear direction or the left-right direction, and the aircraft may be shaken to collide with an obstacle.
そこで、本発明は、風速が大きいほど障害物などの飛行禁止エリアから大きく離れるような移動経路を設定することにより、自律飛行ロボットが風によって飛行禁止エリアに進入する危険性を少なくすることを目的とする。 Therefore, an object of the present invention is to reduce the risk that an autonomous flying robot will enter a no-fly area by wind by setting a movement path that is farther away from a no-fly area such as an obstacle as the wind speed is higher. And
本発明の１つの態様は、移動空間内を自律飛行する自律飛行ロボットであって、前記移動空間内を複数のボクセルに分割し、前記ボクセルの各々について飛行禁止エリアか否かを示す属性及び自律移動ロボットの移動の障害となる度合いを示すボクセルコスト値を関連付けて記憶した記憶部と、前記移動空間における前記自律飛行ロボットの自己位置を推定する位置推定手段と、前記移動空間における風速を計測する風測定手段と、前記属性および前記ボクセルコスト値に基づいて、前記飛行禁止エリアを通過せずに移動目標位置に到達するまでに通過する前記ボクセルの前記ボクセルコスト値を加算した総コスト値が最も小さくなる移動経路を探索する経路探索手段と、前記移動経路に沿って移動するよう制御する移動制御手段と、を有し、前記経路探索手段は、前記飛行禁止エリアの近傍に前記風速が大きいほど広い範囲に存在する前記ボクセルの前記ボクセルコスト値を大きな値に更新して前記移動経路を探索することを特徴とする自律飛行ロボットである。
One aspect of the present invention is an autonomous flying robot that autonomously flies in a moving space, divides the moving space into a plurality of voxels, and an attribute indicating whether each of the voxels is a no-fly area and an autonomous autonomous robot. A storage unit in which a voxel cost value indicating a degree of obstruction of movement of the mobile robot is stored in association, a position estimating unit for estimating the self-position of the autonomous flying robot in the moving space, and measuring a wind speed in the moving space Wind measurement means, based on the attribute and the voxel cost value, the total cost value obtained by adding the voxel cost value of the voxel that passes before reaching the movement target position without passing through the no-fly area is the most has a route searching means for searching a small and that moving path, and movement control means for controlling so as to move along the travel path, the The autonomous flight characterized in that the route search means searches the movement route by updating the voxel cost value of the voxel present in a wider range as the wind speed increases near the no-fly area to a larger value. Be a robot.
ここで、前記風測定手段は、前記移動空間における風向を更に測定し、前記経路探索手段は、前記風向を参照し、前記範囲に存在するボクセルのうちの風上側よりも風下側のボクセルの前記ボクセルコスト値を大きな値に更新することが好適である。 Here, the wind measuring unit further measures a wind direction in the moving space, and the route searching unit refers to the wind direction, and the voxel of a voxel that is more leeward than a windward side of voxels present in the range. It is preferable to update the voxel cost value to a large value .
また、前記経路探索手段は、現在から過去の所定期間における最大瞬間風速が大きいほど前記範囲に存在するボクセルの前記ボクセルコスト値を大きな値に更新することが好適である。 Further, it is preferable that the route search means updates the voxel cost value of voxels existing in the range to a larger value as the maximum instantaneous wind speed in a predetermined period from now to the past is larger.
また、前記経路探索手段は、現在から過去の所定期間内における最大瞬間風速と該所定期間における平均風速との比から突風率を求め、該突風率が大きくなるほど前記範囲に存在するボクセルの前記ボクセルコスト値を大きな値に更新することが好適である。
Further, the route searching means obtains a gust rate from a ratio of a maximum instantaneous wind speed within a predetermined period from the present to the past and an average wind speed during the predetermined period, and the voxel of the voxels present in the range as the gust rate increases becomes larger. It is preferable to update the cost value to a large value .
本発明によれば、移動空間内に生じている風によって、飛行禁止エリアへの意図しない侵入の危険性を少なくすることができる自律飛行ロボットを提供することができる。 According to the present invention, it is possible to provide an autonomous flying robot that can reduce the risk of unintended intrusion into a no-fly zone due to wind generated in a moving space.
本発明の実施形態における自律飛行ロボット１は、図１の概観図に示すように、クアッドロータ型の小型無人ヘリコプタである。なお、本発明の適用範囲は、クアッドロータ型の小型無人ヘリコプタに限定されるものではなく、シングルロータ型の小型無人ヘリコプタについても同様に適用することができる。 The autonomous flying robot 1 according to the embodiment of the present invention is a quadrotor-type small unmanned helicopter as shown in the schematic view of FIG. The scope of application of the present invention is not limited to a quad rotor small unmanned helicopter, but can be similarly applied to a single rotor small unmanned helicopter.
自律飛行ロボット１は、図２のシステム構成図に示すように、外部の警備センタ１００や管理装置１０２と通信し、移動空間内に存在する所定の移動物体を目標対象物Ｍとして追跡し、当該目標対象物Ｍに対して所定の機能を発揮するように構成されている。目標対象物Ｍとなる移動物体は、例えば、監視領域内に侵入した人物（賊等）や車両等である。本実施形態では、所定の機能として、目標対象物Ｍを撮像する機能を例に説明するが、特に限定されるものではなく、目標対象物Ｍに対して音を発したり、発光による警告を行ったりする等の機能であってもよい。 As shown in the system configuration diagram of FIG. 2, the autonomous flying robot 1 communicates with an external security center 100 or management device 102, tracks a predetermined moving object existing in a moving space as a target object M, and It is configured to perform a predetermined function with respect to the target object M. The moving object serving as the target object M is, for example, a person (pirate or the like) or a vehicle that has entered the monitoring area. In the present embodiment, a function of imaging the target object M will be described as an example of the predetermined function, but the present invention is not particularly limited thereto, and emits a sound to the target object M or issues a warning by emitting light. Function.
警備センタ１００と管理装置１０２とはインターネット等の情報通信網１１０を介して情報伝達可能に接続される。また、自律飛行ロボット１と管理装置１０２は、無線通信等によって情報伝達可能に接続される。 The security center 100 and the management device 102 are connected so as to be able to transmit information via an information communication network 110 such as the Internet. The autonomous flying robot 1 and the management device 102 are connected so as to be able to transmit information by wireless communication or the like.
警備センタ１００は、管理装置１０２を介して自律飛行ロボット１と通信を行い、自律飛行ロボット１によって撮像された目標対象物Ｍの撮像画像を受信する。警備センタ１００は、撮像画像に対して画像処理を行い、警備センタ１００にて異常監視している管理者等（図示しない）に警告を発するような機能を備えていてもよい。また、管理装置１０２から目標対象物Ｍの位置（座標）に関する情報を受信し、当該目標対象物Ｍと自律飛行ロボット１によって撮像された撮像画像とを関連付けて管理するというような機能を備えてもよい。 The security center 100 communicates with the autonomous flying robot 1 via the management device 102, and receives an image of the target object M captured by the autonomous flying robot 1. The security center 100 may have a function of performing image processing on the captured image and issuing a warning to an administrator (not shown) or the like who is monitoring the abnormality at the security center 100. In addition, a function is provided for receiving information on the position (coordinates) of the target object M from the management device 102 and managing the target object M in association with an image captured by the autonomous flying robot 1. Is also good.
管理装置１０２は、地面や壁面等に設置された固定型の目標対象物検出センサ１０４（１０４ａ，１０４ｂ・・・）、風速計１０５（１０５ａ，１０５ｂ・・・）を備え、目標対象物Ｍの位置を検知する。目標対象物検出センサ１０４は、例えば、レーザセンサとすることができる。レーザセンサは、一定の角度サンプル間隔の角度毎にレーザを二次元的にスキャンすることによって、地面（又は床面）から一定の高さの水平面における検知範囲内に存在する物体（障害物）との距離情報を極座標値として取得する。レーザセンサは、放射状にレーザ光である探査信号を走査し、物体に反射して戻ってきた探査信号を受信して、送信と受信の時間差から物体までの距離を算出し、レーザセンサの設置位置の座標及び探査信号を送信した方向と算出した距離から当該物体の位置の極座標値を求め、当該極座標値から３次元の直交座標値（Ｘt，Ｙt，Ｚt）を求める。管理装置１０２は、目標対象物検出センサ１０４によって求められた物体の位置を目標対象物Ｍの位置として自律飛行ロボット１へ送信する。自律飛行ロボット１は、目標対象物Ｍの位置を受信すると、その位置に基づいて自らの移動経路を算出し、当該移動経路に沿って移動する。なお、管理装置１０２は、レーザセンサの検知範囲が重複する領域に存在する目標対象物Ｍの同一性を検証することで複数のレーザセンサの検知範囲に渡る目標対象物Ｍの追跡を行う。すなわち、レーザセンサ１０４ａの検知範囲に存在する目標対象物Ｍがレーザセンサ１０４ｂの検知範囲に移動したとしても、管理装置１０２は、当該目標対象物Ｍが同一の物体であると判定することができる。 The management device 102 includes a fixed target object detection sensor 104 (104a, 104b...) And an anemometer 105 (105a, 105b. Detect position. The target object detection sensor 104 can be, for example, a laser sensor. The laser sensor scans the laser two-dimensionally at every angle of a certain angular sample interval, and detects objects (obstacles) existing within the detection range in the horizontal plane at a certain height from the ground (or floor). Is obtained as polar coordinate values. The laser sensor scans the probe signal, which is a laser beam, radially, receives the probe signal that has been reflected back to the object, calculates the distance to the object from the time difference between transmission and reception, and installs the laser sensor. obtains a polar coordinate value from the coordinates and search signal distance calculated with the transmitted direction of the position of the object, an orthogonal coordinate value of the three-dimensional from the polar coordinate values (X t, Y t, Z t) obtained. The management device 102 transmits the position of the object obtained by the target object detection sensor 104 to the autonomous flying robot 1 as the position of the target object M. When receiving the position of the target object M, the autonomous flying robot 1 calculates its own moving path based on the position and moves along the moving path. The management device 102 tracks the target objects M over the detection ranges of the plurality of laser sensors by verifying the identity of the target objects M existing in the area where the detection ranges of the laser sensors overlap. That is, even if the target object M existing in the detection range of the laser sensor 104a moves to the detection range of the laser sensor 104b, the management device 102 can determine that the target object M is the same object. .
また、管理装置１０２は、地面や壁面等に設置された固定型の風速計１０５（風測定手段）を複数備え、移動空間内の各位置における風速（風の強さ）を測定することができる。本実施形態では、風速計１０５は、図２に表すようにプロペラ等の回転子を利用し、プロペラの回転数により風速を換算する機械式の風速計１０５を利用している。しかし、これに限らず、超音波方式やレーザードップラー方式などを利用した電気式の風速計を用いてもよい。また、風速計１０５を自律飛行ロボット１自身が搭載してもよい。この場合、自らの移動速度などの移動制御に係る情報を考慮して風速計１０５の出力を補正し、実際の風速に近い値を求めることが好適である。さらに、自律飛行ロボット１が風速計１０５を備えずに、自律飛行ロボット１の無風時におけるモータ４の制御指令値とこの場合における移動距離・移動速度の値とを予め測定しておき、現時点のモータ４の制御指令値と実際の移動距離・移動速度の値とを求め、両者を比較することにより、現時点の風速を推測してもよい。管理装置１０２は、風速計１０５によって求められた風速を自律飛行ロボット１へ送信する。自律飛行ロボット１は、風速を受信（または計測）すると、後述するように記憶部８に風速情報８２として記憶し、風速の大きさに基づいて移動経路を算出し、当該移動経路に沿って移動する。 Further, the management device 102 includes a plurality of fixed anemometers 105 (wind measuring means) installed on the ground or a wall surface, and can measure the wind speed (wind strength) at each position in the moving space. . In the present embodiment, the anemometer 105 uses a rotor such as a propeller as shown in FIG. 2, and uses a mechanical anemometer 105 that converts the wind speed based on the number of revolutions of the propeller. However, the invention is not limited thereto, and an electric anemometer using an ultrasonic method, a laser Doppler method, or the like may be used. The anemometer 105 may be mounted on the autonomous flying robot 1 itself. In this case, it is preferable to correct the output of the anemometer 105 in consideration of information related to movement control such as its own moving speed, and to obtain a value close to the actual wind speed. Further, the autonomous flying robot 1 is not provided with the anemometer 105, and the control command value of the motor 4 and the value of the moving distance / moving speed in this case without the wind of the autonomous flying robot 1 are measured in advance. The control command value of the motor 4 and the actual moving distance / moving speed values may be obtained, and the two values may be compared to estimate the current wind speed. The management device 102 transmits the wind speed obtained by the anemometer 105 to the autonomous flying robot 1. Upon receiving (or measuring) the wind speed, the autonomous flying robot 1 stores the wind speed information 82 in the storage unit 8 as described later, calculates a moving route based on the magnitude of the wind speed, and moves along the moving route. I do.
以下、図１の概観図及び図３の機能ブロック図を参照して、自律飛行ロボット１の構成及び機能について説明する。 Hereinafter, the configuration and functions of the autonomous flying robot 1 will be described with reference to the schematic diagram of FIG. 1 and the functional block diagram of FIG.
自律飛行ロボット１は、図１に示すように、４枚のロータ（プロペラ）２（２ａ〜２ｄ）を一平面上に有する。各ロータ２は、バッテリ（二次電池：図示しない）により駆動されるモータ４（４ａ〜４ｄ）を用いて回転させられる。一般的に、シングルロータ型のヘリコプタでは、メインロータによって発生する反トルクをテールロータが生み出すモーメントで相殺することによって方位角を保っている。一方、自律飛行ロボット１のようなクアッドロータ型のヘリコプタでは、前後・左右で異なる方向に回転するロータ２を用いることで反トルクの相殺を行っている。そして、各ロータ２の回転数（ｆａ〜ｆｄ）を制御することにより、様々な機体の移動や姿勢の調節を行うことができる。例えば、機体をヨー方向に回転させたいときは、前後のロータ２ａ、２ｃと左右ロータ２ｄ、２ｂの回転数に差を与えればよい。 As shown in FIG. 1, the autonomous flying robot 1 has four rotors (propellers) 2 (2a to 2d) on one plane. Each rotor 2 is rotated using a motor 4 (4a to 4d) driven by a battery (secondary battery: not shown). Generally, in a single-rotor helicopter, the azimuth is maintained by canceling the anti-torque generated by the main rotor with the moment generated by the tail rotor. On the other hand, in a quad-rotor helicopter such as the autonomous flying robot 1, the anti-torque is canceled by using the rotor 2 rotating in different directions in front, rear, left and right. By controlling the number of rotations (fa to fd) of each rotor 2, it is possible to perform various movements and adjustments of the attitude of the body. For example, when it is desired to rotate the body in the yaw direction, a difference may be given between the rotational speeds of the front and rear rotors 2a and 2c and the left and right rotors 2d and 2b.
撮像部３は、例えばレンズなどの光学系および所定画素（例えば６４０×４８０画素）のＣＣＤやＣＭＯＳなどの２次元アレイ素子を有する二次元イメージセンサで構成され、移動空間の撮像画像を所定の時間間隔で取得するいわゆるカラーカメラである。本実施形態では、撮像部３は、その光軸が自律飛行ロボット１の正面方向を撮像するよう筐体部分に設置され、かつ、水平面（ＸＹ平面）から予め定めた俯角θにより斜め下方の空間を視野角φにおいて撮像するよう設置されている。取得した撮像画像は後述する制御部７に出力され、制御部７により記憶部８に記憶されたり、後述する通信部９を介して管理装置１０２に送信されたりする。 The imaging unit 3 includes a two-dimensional image sensor having, for example, an optical system such as a lens and a two-dimensional array element such as a CCD or a CMOS having predetermined pixels (for example, 640 × 480 pixels). This is a so-called color camera obtained at intervals. In the present embodiment, the imaging unit 3 is installed in a housing part so that its optical axis captures an image in the front direction of the autonomous flying robot 1, and a space obliquely lower by a predetermined depression angle θ from a horizontal plane (XY plane). At a viewing angle φ. The acquired captured image is output to the control unit 7 described later, stored in the storage unit 8 by the control unit 7, or transmitted to the management device 102 via the communication unit 9 described below.
距離検出センサ５は、自律飛行ロボット１の周囲に存在する障害物と自律飛行ロボット１との間の距離を検出し、センサ検出範囲内に存在する障害物の相対的な位置を取得するセンサである。本実施形態では、距離検出センサ５としてマイクロ波センサを備える。マイクロ波センサは、空間にマイクロ波を放出し、その反射波を検知することによって、自律飛行ロボット１の周囲にある物体を探知し、その物体までの距離を求める。距離検出センサ５は、例えば、自律飛行ロボット１の前方に向けて設け、目標対象物Ｍまでの距離を測定するために用いることができる。また、距離検出センサ５は、例えば、自律飛行ロボット１の下部に下向きに設け、地面との距離（高度）を測定するために用いることもできる。また、距離検出センサ５は、例えば、撮像部３の光軸方向に向けて設け、撮像部３の撮像対象物である目標対象物Ｍまでの距離を測定するために用いることができる。 The distance detection sensor 5 is a sensor that detects a distance between an obstacle existing around the autonomous flying robot 1 and the autonomous flying robot 1 and obtains a relative position of the obstacle existing within the sensor detection range. is there. In the present embodiment, a microwave sensor is provided as the distance detection sensor 5. The microwave sensor emits microwaves into space and detects reflected waves, thereby detecting an object around the autonomous flying robot 1 and obtaining a distance to the object. The distance detection sensor 5 is provided, for example, toward the front of the autonomous flying robot 1 and can be used to measure the distance to the target object M. Further, the distance detection sensor 5 can be provided, for example, downward below the autonomous flying robot 1 and used to measure the distance (altitude) from the ground. In addition, the distance detection sensor 5 is provided, for example, in the optical axis direction of the imaging unit 3 and can be used to measure a distance to a target object M which is an imaging target of the imaging unit 3.
位置検出センサ６は、自律飛行ロボット１の現在位置を取得するためのセンサである。位置検出センサ６は、例えば、ＧＮＳＳ(Global Navigation Satellite System)等の航法衛星（人工衛星）から送信される電波（航法信号）を受信する。位置検出センサ６は、複数の航法衛星（人工衛星）から送信される航法信号を受信して制御部７へ入力する。なお、位置検出センサ６は、レーザスキャナ、ジャイロセンサ、電子コンパス、気圧センサ等の他のセンサを用いて既知の従来技術により自己位置を得るための情報を取得するものとしてもよい。 The position detection sensor 6 is a sensor for acquiring the current position of the autonomous flying robot 1. The position detection sensor 6 receives a radio wave (navigation signal) transmitted from a navigation satellite (artificial satellite) such as a GNSS (Global Navigation Satellite System), for example. The position detection sensor 6 receives navigation signals transmitted from a plurality of navigation satellites (artificial satellites) and inputs the navigation signals to the control unit 7. The position detection sensor 6 may acquire information for obtaining its own position by a known conventional technique using other sensors such as a laser scanner, a gyro sensor, an electronic compass, and a barometric pressure sensor.
通信部９は管理装置１０２との間で、例えば無線ＬＡＮや携帯電話回線等により無線通信するための通信モジュールである。本実施形態では、撮像部３によって取得した撮像画像を通信部９により管理装置１０２に送信し、当該撮像画像を管理装置１０２から警備センタ１００に送信することにより、警備員等が遠隔から侵入者を監視することを可能にする。また、通信部９は、管理装置１０２から目標対象物Ｍの位置（座標：Ｘt，Ｙt，Ｚt）を受信することにより、後述するような移動経路の設定を可能にする。 The communication unit 9 is a communication module for performing wireless communication with the management device 102 by, for example, a wireless LAN or a mobile phone line. In the present embodiment, the captured image acquired by the imaging unit 3 is transmitted to the management device 102 by the communication unit 9, and the captured image is transmitted from the management device 102 to the security center 100, so that a security person or the like can remotely enter the intruder. To be able to monitor. In addition, the communication unit 9 receives a position (coordinates: X t , Y t , Z t ) of the target object M from the management device 102, thereby enabling setting of a movement route to be described later.
記憶部８は、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）、ＨＤＤ（Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ）等の情報記憶装置である。記憶部８は、各種プログラムや各種データを記憶し、制御部７との間でこれらの情報を入出力する。各種データには、目標対象物位置８１、風速情報８２、ボクセル情報８３、移動空間グラフ情報８４、各種パラメータ８５等の制御部７の各処理に用いられる情報、各センサ等の出力値及び撮像画像等が含まれる。 The storage unit 8 is an information storage device such as a ROM (Read Only Memory), a RAM (Random Access Memory), and a HDD (Hard Disk Drive). The storage unit 8 stores various programs and various data, and inputs and outputs these information with the control unit 7. The various data include information used for each processing of the control unit 7 such as a target object position 81, wind speed information 82, voxel information 83, moving space graph information 84, various parameters 85, output values of each sensor, and a captured image. Etc. are included.
目標対象物位置８１は、管理装置１０２から受信した目標対象物Ｍの位置情報（座標：Ｘt，Ｙt，Ｚt）である。本実施形態では、目標対象物位置８１を目標対象物Ｍの足元位置、すなわち目標対象物Ｍが地面（床面）に接している位置とする。制御部７は、通信部９を介して目標対象物Ｍの位置を受信すると目標対象物位置８１として記憶部８に記憶させる。なお、管理装置１０２は、目標対象物Ｍの位置（座標：Ｘt，Ｙt，Ｚt）の時間変化に基づいて目標対象物Ｍの姿勢（例えば、目標対象物Ｍの正面方向を示した情報）を推定し、目標対象物位置８１として記憶してもよい。 The target object position 81 is position information (coordinates: Xt , Yt , Zt ) of the target object M received from the management device 102. In the present embodiment, the target object position 81 is a foot position of the target object M, that is, a position where the target object M is in contact with the ground (floor surface). When receiving the position of the target object M via the communication unit 9, the control unit 7 stores the position of the target object M in the storage unit 8 as the target object position 81. Note that the management device 102 indicates the posture of the target object M (for example, the front direction of the target object M) based on the time change of the position (coordinates: Xt , Yt , Zt ) of the target object M. Information) may be estimated and stored as the target object position 81.
風速情報８２は、管理装置１０２から受信した風速の大きさを表す情報である。本実施形態では、移動空間に複数の風速計１０５を設置し、各風速計１０５の設置位置と各風速計１０５によって測定された風速値とを対応付けて記憶している。しかし、これに限らず、移動空間に一つの風速計１０５を設置し、当該風速計１０５によって測定された風速値を、当該移動空間における風速の大きさとみなしてもよい。また、風速計１０５を自律飛行ロボット１が搭載している場合は、搭載した風速計１０５によって測定された風速値と自己位置とを対応付けて記憶する。 The wind speed information 82 is information indicating the magnitude of the wind speed received from the management device 102. In the present embodiment, a plurality of anemometers 105 are installed in the moving space, and the installation positions of the anemometers 105 and the wind speed values measured by the anemometers 105 are stored in association with each other. However, the present invention is not limited to this, and one anemometer 105 may be installed in the moving space, and the wind speed value measured by the anemometer 105 may be regarded as the magnitude of the wind speed in the moving space. When the autonomous flying robot 1 is equipped with the anemometer 105, the wind speed measured by the anemometer 105 and the own position are stored in association with each other.
ボクセル情報８３は、移動空間をボクセル空間として複数のボクセルに分割して移動空間の障害物の構造等を表した情報であり、予め管理者等によって設定され記憶部８に記憶され、また、後述するように位置推定手段７１及び経路探索手段７３にて更新される情報である。本実施形態では、移動空間を所定の大きさ（例えば１５ｃｍ×１５ｃｍ×１５ｃｍ）のボクセルに等分割し、各ボクセルの識別子であるボクセルＩＤと、移動空間におけるボクセルの位置（三次元座標）と、ボクセル属性と、ボクセルコスト値とを対応付けてボクセル情報８３として記憶する。ボクセル属性には、飛行禁止エリアに相当するボクセルを「占有ボクセル」と定義して、自律飛行ロボット１が移動できない空間とする。なお飛行禁止エリアには、例えば建造物等の障害物に相当するエリア、飛行が許可された敷地外のエリア、飛行が許可された高度よりも高いエリアなどが挙げられる。そして、占有ボクセルの近くに存在する空間に位置するボクセルを「近接ボクセル」、それ以外の自由に飛行可能なエリアに位置するボクセルを「自由ボクセル」として定義する。 The voxel information 83 is information that represents the structure of obstacles in the moving space by dividing the moving space into a plurality of voxels as a voxel space, is set in advance by an administrator or the like, is stored in the storage unit 8, and is described later. The information is updated by the position estimating means 71 and the route searching means 73 so as to perform the processing. In the present embodiment, the moving space is equally divided into voxels of a predetermined size (for example, 15 cm × 15 cm × 15 cm), a voxel ID which is an identifier of each voxel, a position of the voxel in the moving space (three-dimensional coordinates), The voxel attribute and the voxel cost value are stored as voxel information 83 in association with each other. In the voxel attribute, a voxel corresponding to the no-fly area is defined as an “occupied voxel” and is a space in which the autonomous flying robot 1 cannot move. The no-fly area includes, for example, an area corresponding to an obstacle such as a building, an area outside the site where the flight is permitted, an area higher than the altitude where the flight is permitted, and the like. Then, voxels located in a space existing near the occupied voxel are defined as “close voxels”, and voxels located in other freely flyable areas are defined as “free voxels”.
各ボクセルには、後述する経路探索手段７３にて移動経路を生成する際に利用できるよう、占有度を示すボクセルコスト値が関連付けて登録される。ボクセルコスト値は、占有ボクセルにおいて最大値Ｃmaxをとり、占有ボクセルからの距離が大きくなるほど小さな値となるように設定される。例えば、あるボクセル（評価ボクセル）のボクセルコスト値は、ボクセルコスト値＝Ｃmax × ｅｘｐ｛−λ×Ｒ｝…式（１）の計算式により算出することが好適である。ここでλは実験によって求めたパラメータとし、Ｒは評価ボクセルに最も近い占有ボクセルからの距離とする。そして、予め定めた閾値以上のボクセルコスト値を有するボクセルを「近接ボクセル」とする。また、ボクセルコスト値が当該閾値よりも小さいボクセルを「自由ボクセル」とし、自由ボクセルとみなされたボクセルのボクセルコスト値を０と設定する。 A voxel cost value indicating an occupancy is registered in association with each voxel so that the voxel cost value can be used when a travel route is generated by a route search unit 73 described later. The voxel cost value is set so as to take the maximum value C max in the occupied voxel and to decrease as the distance from the occupied voxel increases. For example, it is preferable that the voxel cost value of a certain voxel (evaluation voxel) is calculated by the following formula (1): voxel cost value = C max × exp {−λ × R}. Here, λ is a parameter obtained by experiment, and R is a distance from the occupied voxel closest to the evaluation voxel. Then, a voxel having a voxel cost value equal to or greater than a predetermined threshold value is defined as a “proximal voxel”. Further, a voxel whose voxel cost value is smaller than the threshold value is set as a “free voxel”, and a voxel cost value of a voxel regarded as a free voxel is set to 0.
移動空間グラフ情報８４は、ボクセル情報８３に基づいて作成された３次元グラフ情報である。具体的には、ボクセル情報８３に基づいて、各ボクセルの中心位置をノードとし、当該ノードに隣接するノード間を連結した線分をエッジとしたグラフ構造からなる情報である。図４は移動空間グラフ情報８４のグラフ構造を説明する図であり、移動空間におけるボクセルの一部（２７個）を切り欠いたものである。図４において符号Ｂで表す個々の立方体はボクセルを表すものである。また、これらのボクセルＢの中心位置にある黒又はハッチングにて塗りつぶした球はノードであり、ノード間を連結する点線で表示する線分はエッジである。なお、移動空間グラフ情報８４におけるエッジの重みとして、隣接するノード間の距離に基づいて求められる後述する距離コストＣＭが設定されているものとする。 The moving space graph information 84 is three-dimensional graph information created based on the voxel information 83. Specifically, based on the voxel information 83, the information has a graph structure in which the center position of each voxel is a node and a line segment connecting nodes adjacent to the node is an edge. FIG. 4 is a diagram for explaining the graph structure of the moving space graph information 84, in which a part (27) of voxels in the moving space is cut out. Each cube represented by the symbol B in FIG. 4 represents a voxel. The spheres filled with black or hatching at the center position of these voxels B are nodes, and the dotted line connecting the nodes is an edge. It is assumed that a distance cost CM which will be described later, which is obtained based on the distance between adjacent nodes, is set as the weight of the edge in the moving space graph information 84.
また、記憶部８には、各種パラメータ８５として離間距離等も記憶される。離間距離は、目標対象物Ｍに追従飛行するにあたって、自律飛行ロボット１と目標対象物Ｍとの水平面における維持すべき相対距離である。離間距離は、自律飛行ロボット１の管理者等によって予め設定される。自律飛行ロボット１を用いて所定の目標対象物Ｍを監視する場合、目標対象物Ｍに近づき、より詳細な撮像画像を取得できる必要がある。しかし、侵入者などの敵対する目標対象物Ｍから攻撃を受けないようにするためには一定距離以上離間する必要がある。そのため、本実施形態の自律飛行ロボット１は、目標対象物Ｍの詳細な撮像画像を取得でき、かつ、当該目標対象物Ｍから攻撃を受け難い距離に離間距離を予め、当該離間距離を保ちつつ追従飛行するように制御される。離間距離は、例えば３ｍとして設定される。 The storage unit 8 also stores the separation distance and the like as various parameters 85. The separation distance is a relative distance to be maintained in the horizontal plane between the autonomous flying robot 1 and the target object M when following the target object M. The separation distance is set in advance by an administrator of the autonomous flying robot 1 or the like. When monitoring a predetermined target object M using the autonomous flying robot 1, it is necessary to approach the target object M and obtain a more detailed captured image. However, in order to prevent an attack from an enemy target object M such as an intruder, it is necessary to separate the target object M by a certain distance or more. For this reason, the autonomous flying robot 1 of the present embodiment can acquire a detailed captured image of the target object M, and maintain the separation distance in advance so that the target object M is hardly attacked by the target object M. It is controlled to follow. The separation distance is set as, for example, 3 m.
制御部７は、ＣＰＵ等を備えたコンピュータで構成され、位置推定処理、速度推定処理、ボクセル情報更新処理、移動経路生成処理、移動制御処理（経路追従制御）を行う一連の処理として、位置推定手段７１、速度推定手段７２、経路探索手段７３、移動制御手段７４を含んでいる。 The control unit 7 is configured by a computer having a CPU and the like, and performs position estimation processing, speed estimation processing, voxel information updating processing, movement path generation processing, and movement control processing (path following control) as a series of processing. Means 71, speed estimation means 72, route search means 73, and movement control means 74.
位置推定手段７１は、位置検出センサ６の出力に基づいて、移動空間における自律飛行ロボット１の現在位置（自己位置）を推定する位置推定処理を行う。 The position estimating means 71 performs a position estimating process for estimating the current position (self position) of the autonomous flying robot 1 in the moving space based on the output of the position detecting sensor 6.
具体的には、位置検出センサ６から得られた複数の航法衛星からの航法信号に基づいて既知の周知技術に基づいて推定した緯度・経度と、距離検出センサ５から得られた高度とから自己位置の座標（Ｘs，Ｙs，Ｚs）を計算する。さらに、電子コンパスやジャイロセンサなどの位置検出センサ６からの出力を受けて自己位置として姿勢ＹＡＷを求める。なお、自己位置の推定方法はこれに限定されるものではなく、他の方法を用いて自律飛行ロボット１の現在位置を推定してもよい。 Specifically, the self-establishment is performed based on the latitude / longitude estimated based on a known technique based on navigation signals from a plurality of navigation satellites obtained from the position detection sensor 6 and the altitude obtained from the distance detection sensor 5. position coordinate (X s, Y s, Z s) is calculated. Further, an attitude YAW is obtained as a self-position by receiving an output from a position detection sensor 6 such as an electronic compass or a gyro sensor. The method for estimating the self-position is not limited to this, and the current position of the autonomous flying robot 1 may be estimated using another method.
位置推定手段７１は、推定された自己位置（座標：Ｘs，Ｙs，Ｚs及び姿勢ＹＡＷ）と管理装置１０２から受信した目標対象物Ｍの位置（座標：Ｘt，Ｙt，Ｚt）を経路探索手段７３へ出力する。 Position estimating means 71, the estimated self-position (coordinates: X s, Y s, Z s and the posture YAW) position of the target object M received from the management apparatus 102 (coordinates: X t, Y t, Z t ) Is output to the route searching means 73.
なお、位置推定手段７１は、目標対象物Ｍの位置に基づいてボクセル情報８３を更新する処理を行う。具体的には、記憶部８のボクセル情報８３に基づいたボクセル空間に目標対象物Ｍの位置を中心として予め定めた目標対象物Ｍの大きさと略同じ大きさの円柱モデル（例えば、監視対象の目標対象物Ｍを侵入者であるとしたとき、底面の半径０．３ｍ、高さ１．７ｍの円柱モデル）を配置し、当該円柱モデルと干渉するボクセルを占有ボクセルとして設定することによりボクセル情報８３を更新する。後述するように、自律飛行ロボット１は、占有ボクセルには移動しないように飛行制御されるが、上記のように目標対象物Ｍの位置に基づいてボクセル情報８３を更新することにより、自律飛行ロボット１と目標対象物Ｍとの接触を回避することができる。 The position estimating unit 71 performs a process of updating the voxel information 83 based on the position of the target object M. Specifically, a cylinder model (for example, the size of the monitored object) having a size substantially the same as the size of the target object M determined in advance in the voxel space based on the voxel information 83 in the storage unit 8 with the position of the target object M as a center. Assuming that the target object M is an intruder, a cylinder model having a bottom radius of 0.3 m and a height of 1.7 m) is arranged, and voxels that interfere with the cylinder model are set as occupied voxels, thereby obtaining voxel information. 83 is updated. As will be described later, the autonomous flying robot 1 is controlled to fly so as not to move to the occupied voxel, but by updating the voxel information 83 based on the position of the target object M as described above, the autonomous flying robot 1 The contact between 1 and the target object M can be avoided.
速度推定手段７２は、後述する移動制御手段７４における移動制御で利用するため、自律飛行ロボット１の現在の飛行速度（ｖx，ｖy，ｖz，ｖyaw）を推定する処理を行う。本実施形態では、位置推定手段７１にて推定した自己位置（座標：Ｘs，Ｙs，Ｚs及び姿勢ＹＡＷ）の時間変化から飛行速度を求める。この際、測定誤差等の影響を考慮して拡張カルマンフィルタを利用して飛行速度を推定することが好適である。この他にも、ＧＮＳＳにおけるドップラー効果を利用した速度推定方法を用いてもよい。 The speed estimating means 72 performs processing for estimating the current flight speed (v x , v y , v z , v yaw ) of the autonomous flying robot 1 for use in movement control in the movement control means 74 described later. In this embodiment, the self-position estimation by the position estimation means 71 (coordinates: X s, Y s, Z s and the posture YAW) determining the flight speed from the time variation of. At this time, it is preferable to estimate the flight speed using an extended Kalman filter in consideration of the influence of a measurement error or the like. Alternatively, a speed estimation method using the Doppler effect in the GNSS may be used.
経路探索手段７３は、自律飛行ロボットの現在位置（自己位置）から所定の移動目標位置に至る移動経路を算出する処理を行う。特に、本実施形態における経路探索手段７３は、移動経路を算出する際、風速情報８２に基づいて風速が大きいほど占有ボクセル（飛行禁止エリア）からより遠くに離れた位置に移動経路を生成する。 The route search means 73 performs a process of calculating a movement route from the current position (self position) of the autonomous flying robot to a predetermined movement target position. In particular, when calculating the moving route, the route searching means 73 according to the present embodiment generates a moving route at a position farther from the occupied voxel (flight prohibited area) as the wind speed increases based on the wind speed information 82.
まず、経路探索手段７３は、記憶部８に記憶された風速情報８２に基づいてボクセル情報８３を更新するボクセル情報更新処理を行う。以下、ボクセル情報更新処理の詳細について説明する。ボクセル情報更新処理では、移動空間における風速情報８２の風速値が大きいほど、近接ボクセルの範囲が大きく（広く）設定されるように、ボクセルコスト値を算出する。なお、本実施形態におけるボクセル情報更新処理では、自己位置に最も近い位置に設置された風速計１０５の風速値を利用することとして以下で説明する。自律飛行ロボット１自体が風速計１０５を搭載している他の実施形態においては、当該搭載している風速計１０５の風速値をボクセル情報更新処理にて利用することとし、搭載している風速計１０５が故障等によって利用できないときは、自己位置に近い位置に設置された風速計１０５の風速値を利用することとする。 First, the route search unit 73 performs a voxel information update process of updating the voxel information 83 based on the wind speed information 82 stored in the storage unit 8. Hereinafter, the details of the voxel information update processing will be described. In the voxel information update processing, the voxel cost value is calculated such that the larger the wind speed value of the wind speed information 82 in the moving space, the larger (wider) the range of the adjacent voxel is set. Note that the voxel information updating process in the present embodiment will be described below by using the wind speed value of the anemometer 105 installed at the position closest to the own position. In another embodiment in which the autonomous flying robot 1 itself is equipped with the anemometer 105, the wind speed value of the installed anemometer 105 is used in the voxel information update processing, and the installed anemometer 105 is used. When the 105 cannot be used due to a failure or the like, the wind speed value of the anemometer 105 installed at a position close to the self position is used.
本実施形態では、経路探索手段７３は、風速情報８２から風速値を読み出し、式（１）におけるλの値を、風速が大きいほど小さな値となるように更新し、更新した当該λの値を用いて、式（１）を用いて同様に各ボクセルのボクセルコスト値を算出するものとする。例えば、風速１０ｍの時はλ＝０．１、風速８ｍの時はλ＝０．１５、風速６ｍの時はλ＝０．２などといったように、予め風速の大きさが大きくなるほどλの値が小さくなるよう風速に対応付けてλの値を記憶しておき、ボクセル情報更新処理の実行時における風速値に基づいて計算に用いるλの値を読み出して、式（１）によりボクセルコストを計算する。このようにして各ボクセルについて求めたボクセルコスト値によってボクセル情報８３を更新する。そして、予め定めた閾値以上のボクセルコスト値を有するボクセルのボクセル属性を「近接ボクセル」、当該閾値未満のボクセルコスト値を有するボクセルのボクセル属性を「自由ボクセル」としてボクセル情報８３を更新する。 In the present embodiment, the route searching means 73 reads the wind speed value from the wind speed information 82, updates the value of λ in Expression (1) so that the value becomes smaller as the wind speed increases, and updates the updated value of λ. The voxel cost value of each voxel is calculated in the same manner using Expression (1). For example, λ = 0.1 when the wind speed is 10 m, λ = 0.15 when the wind speed is 8 m, λ = 0.2 when the wind speed is 6 m, and so on. The value of λ is stored in association with the wind speed so that is smaller, the value of λ used for the calculation is read out based on the wind speed value at the time of executing the voxel information update processing, and the voxel cost is calculated by equation (1). I do. The voxel information 83 is updated with the voxel cost value obtained for each voxel in this manner. Then, the voxel information 83 is updated by setting the voxel attribute of a voxel having a voxel cost value equal to or greater than a predetermined threshold value to “near voxel” and setting the voxel attribute of a voxel having a voxel cost value lower than the threshold value to “free voxel”.
図５は、ボクセル情報更新処理によって再設定されたボクセル情報８３を説明する図であり、時刻ｔのときにボクセルで表示した移動空間の一部を真上から見下ろしたときの図について表している。図５において、符号Ｂ１で表わした黒色で塗られた領域は「占有ボクセル」を示し、例えば、建造物などを構成する物体が占有する空間である。また、符号Ｂ２で表わしたハンチングで塗られた領域は「近接ボクセル」であり、符号Ｂ３で表わした白色の領域（何も塗られていない領域）は「自由ボクセル」である。図５（ａ）は、風速が小さい（弱風）のときに設定されたボクセル情報８３を表しており、図５（ｂ）は、風速が大きい（強風）のときに設定されたボクセル情報８３を表している。図５に表すように、移動空間における風速が大きくなるほど、占有ボクセルから離れた位置にあるボクセルが、（自由ボクセルではなく）近接ボクセルとして設定されることになる。 FIG. 5 is a diagram for explaining the voxel information 83 reset by the voxel information update process, and shows a diagram when a part of the moving space displayed by the voxel at time t is looked down from directly above. . In FIG. 5, an area painted in black represented by reference numeral B1 indicates an “occupied voxel” and is, for example, a space occupied by an object constituting a building or the like. Further, the area painted by hunting represented by reference numeral B2 is a “close voxel”, and the white area represented by reference numeral B3 (the area where nothing is painted) is “free voxel”. FIG. 5A shows the voxel information 83 set when the wind speed is low (weak wind), and FIG. 5B shows the voxel information 83 set when the wind speed is high (strong wind). Is represented. As shown in FIG. 5, as the wind speed in the moving space increases, the voxel located farther from the occupied voxel is set as a close voxel (not a free voxel).
なお、ボクセル情報更新処理は、経路探索手段７３によって移動経路を生成する際（後述する移動経路生成処理を実行する際）に、その都度実行してもよいし、所定期間毎に実行してもよいし、風速が所定範囲以上変化する度に実行してもよい。或いは、複数通りの風速値における全ボクセルのボクセルコスト値を式（１）によって予め算出した上で、風速値に対応付けて記憶しておき、ボクセル情報更新処理の実行時における風速値に対応する全ボクセルのボクセルコスト値を読み出して更新してもよい。 The voxel information updating process may be executed each time the route search unit 73 generates a moving route (when executing a moving route generating process described later), or may be executed at predetermined intervals. It may be executed every time the wind speed changes by a predetermined range or more. Alternatively, the voxel cost values of all the voxels at a plurality of wind speed values are calculated in advance by equation (1), stored in association with the wind speed values, and correspond to the wind speed values at the time of executing the voxel information update process. The voxel cost values of all voxels may be read and updated.
続いて、経路探索手段７３は、位置推定手段７１で推定された自己位置及び目標対象物位置８１と、記憶部８に記憶された各種情報とを用いて自律飛行ロボット１の移動経路を算出する移動経路生成処理を行う。以下に、移動経路生成処理の詳細について説明する。 Subsequently, the route searching unit 73 calculates the movement route of the autonomous flying robot 1 using the self-position and the target object position 81 estimated by the position estimating unit 71 and various information stored in the storage unit 8. A moving route generation process is performed. Hereinafter, the details of the movement route generation processing will be described.
移動経路生成処理では、まず、目標対象物位置８１から予め設定した離間距離（３ｍ）だけ水平方向に離れた位置であって予め設定された高度（３ｍ）の位置に移動目標位置を設定する処理を行う。この際、経路探索手段７３は、ボクセル情報８３を参照し、占有ボクセル又は近接ボクセルに含まれる位置には移動目標位置を設定しないようにする。なお、（目標対象物位置８１の時間変化により求めた）目標対象物Ｍの移動方向を目標対象物Ｍの正面方向として定め、当該正面方向に近い位置に移動目標位置を設定することが好適である。 In the movement route generation processing, first, the movement target position is set to a position horizontally separated from the target object position 81 by a predetermined separation distance (3 m) at a predetermined altitude (3 m). I do. At this time, the route search unit 73 refers to the voxel information 83 and does not set the movement target position at a position included in the occupied voxel or the close voxel. In addition, it is preferable that the moving direction of the target object M (determined by the time change of the target object position 81) is defined as the front direction of the target object M, and the moving target position is set at a position close to the front direction. is there.
次に移動経路生成処理では、設定された移動目標位置Ｑと、記憶部８に記憶されたボクセル情報８３及び移動空間グラフ情報８４と、位置推定手段７１にて推定された自己位置と、を用いて自律飛行ロボット１の移動経路を算出する。この際、空間情報８２及び移動空間グラフ情報８４を参照し、移動目標位置に対応するボクセルのノード（以下「スタートノード」という）から自己位置に対応するボクセルのノード（以下「ゴールノード」という）に至る総コスト値Ｃが最も小さくなる経路をＡ＊経路探索法により探索する。 Next, in the movement route generation processing, the set movement target position Q, the voxel information 83 and the movement space graph information 84 stored in the storage unit 8, and the own position estimated by the position estimation unit 71 are used. To calculate the movement route of the autonomous flying robot 1. At this time, referring to the space information 82 and the moving space graph information 84, the node of the voxel corresponding to the movement target position (hereinafter referred to as “start node”) to the node of the voxel corresponding to the own position (hereinafter referred to as “goal node”). Is searched for by the A * route search method.
Ａ＊経路探索法では、ある評価ノードｎ（ノードＩＤがｎのノード）における総コスト値Ｃ（ｎ）＝ｇ（ｎ）＋ｈ（ｎ）…式（２）で表わす。 In the A * route search method, a total cost value C (n) = g (n) + h (n) at a certain evaluation node n (a node having a node ID of n) is represented by Expression (2).
ここで、ｇ（ｎ）は、評価ノードｎについてのＡ＊経路探索法におけるｇコスト値であり、本実施形態では、スタートノードから評価ノードｎに至るまでの移動距離や障害物への接触危険性を考慮したコスト値として求める。すなわち、評価ノードｎのｇコスト値であるｇ（ｎ）は、隣接ノードにおけるｇコスト値であるｇ（ｎ−１）と、当該隣接ノードから評価ノードに至る距離に基づいて設定された距離コストＣＭ、評価ノードにおけるボクセルコストＣｖ（ｎ）との和とし、ｇ（ｎ）＝ｇ（ｎ−１）＋ＣＭ＋Ｃｖ（ｎ）により求める。また、ｈ（ｎ）は、評価ノードｎについてのＡ＊経路探索法におけるｈコスト値であり、評価ノードｎからゴールノードに至る距離の推定コスト値である。本実施形態では、評価ノードｎからゴールノードに至る直線距離によりｈ（ｎ）を求める。Ａ＊経路探索法では、スタートノードから順に隣接ノードの総コスト値Ｃ（ｎ）を算出していくことを繰り返し、最終的にゴールノードに至る総コスト値Ｃ（ｎ）が最も小さくなる経路を探索していく。 Here, g (n) is a g cost value in the A * route search method for the evaluation node n, and in the present embodiment, the moving distance from the start node to the evaluation node n and the risk of contact with an obstacle. It is calculated as a cost value taking into account the nature. That is, g (n) which is the g cost value of the evaluation node n is g (n-1) which is the g cost value of the adjacent node and the distance cost set based on the distance from the adjacent node to the evaluation node. The sum of the CM and the voxel cost Cv (n) at the evaluation node is obtained as g (n) = g (n-1) + CM + Cv (n). H (n) is an h cost value in the A * route search method for the evaluation node n, and is an estimated cost value of a distance from the evaluation node n to the goal node. In the present embodiment, h (n) is obtained from the linear distance from the evaluation node n to the goal node. In the A * route search method, the calculation of the total cost value C (n) of the adjacent node in order from the start node is repeated, and the route that finally has the smallest total cost value C (n) to the goal node is determined. Searching.
以下に、Ａ＊経路探索法による移動経路の生成について簡単に説明する。Ａ＊経路探索法では、まず、スタートノードに隣接する一又は複数のノード（隣接ノード）を評価ノードとして設定するところから開始する。この際、ボクセル情報８３のボクセル属性を参照し、占有ボクセル又は近接ボクセルに相当する位置にある隣接ノードは評価ノードから除外し、占有ボクセル又は近接ボクセルに移動経路が生成されないようにする。そして、それぞれの評価ノードについて式（２）により総コスト値を求める。次に、評価ノードの中で最小の総コスト値となるノードを注目ノードと称したとき、当該注目ノードの（これまで評価ノードと設定されていない新たな）隣接ノードを新たに評価ノードとして追加し、新たに追加された評価ノードに対しても同様に総コスト値を求める。続いて、同様に全ての評価ノードの中で最小の総コスト値となる評価ノードを注目ノードに再設定する。このように、評価ノードの総コスト値に基づいた注目ノードの再設定と、当該再設定に伴う新たな評価ノードの追加及び総コスト値の計算を繰り返し、最終的にゴールノードが注目ノードとして設定されたとき、経路の探索は終了する。 Hereinafter, generation of a moving route by the A * route searching method will be briefly described. In the A * route search method, first, one or more nodes (adjacent nodes) adjacent to the start node are set as evaluation nodes. At this time, by referring to the voxel attribute of the voxel information 83, an adjacent node located at a position corresponding to the occupied voxel or the neighboring voxel is excluded from the evaluation nodes, so that a movement route is not generated in the occupied voxel or the neighboring voxel. Then, a total cost value is obtained for each evaluation node by the equation (2). Next, when the node having the minimum total cost value among the evaluation nodes is referred to as a node of interest, a new node adjacent to the node of interest (newly not set as an evaluation node) is newly added as an evaluation node. Then, the total cost value is similarly obtained for the newly added evaluation node. Subsequently, similarly, the evaluation node having the minimum total cost value among all the evaluation nodes is reset to the target node. As described above, the resetting of the attention node based on the total cost value of the evaluation node, the addition of a new evaluation node and the calculation of the total cost value accompanying the resetting are repeated, and finally, the goal node is set as the attention node. When the search is completed, the route search ends.
このようにして、起点となるスタートノードから終点となるゴールノードに至る最良の移動経路が生成される。経路探索手段７３で生成された移動経路のデータは、経由点となるノードの位置（ｘ，ｙ，ｚ）の集合データであり、この情報は記憶部８に一時的に記憶される。図６は、風速に応じて生成された移動経路の違いについて説明する図であり、図６（ａ）は、風速が小さい（弱風）のときに設定された移動経路を表しており、図６（ｂ）は、風速が大きい（強風）のときに設定された移動経路を表している。このように、経路探索手段７３は、風速が大きいほど占有ボクセル（飛行禁止エリア）からより遠くに離れた位置に移動経路を生成する。 In this way, the best moving route from the start node serving as the starting point to the goal node serving as the ending point is generated. The data of the moving route generated by the route searching means 73 is a set of the positions (x, y, z) of the nodes serving as the waypoints, and this information is temporarily stored in the storage unit 8. FIG. 6 is a diagram for explaining the difference in the moving route generated according to the wind speed. FIG. 6A illustrates the moving route set when the wind speed is low (weak wind). 6 (b) shows the moving route set when the wind speed is high (strong wind). As described above, the route search unit 73 generates a movement route at a position farther away from the occupied voxel (flight prohibited area) as the wind speed increases.
なお、経路の探索方法は、Ａ＊経路探索法に限定されるものでなく、ダイクストラ法等の他の経路探索方法を適用してもよい。 Note that the route search method is not limited to the A * route search method, and another route search method such as the Dijkstra method may be applied.
移動制御手段７４は、経路探索手段７３にて算出された移動経路と位置推定手段７１にて推定された自己位置と速度推定手段７２で推定された飛行速度とを用いて、自律飛行ロボット１が経路探索手段７３で算出された移動経路に沿って飛行するように経路追従制御を行う。具体的には、移動経路、自己位置及び飛行速度を用いて各時刻での飛行制御値である制御指令値を求め、当該制御指令値に基づいてモータ４を制御し、ロータ２の回転数を制御する。 The movement control unit 74 uses the movement route calculated by the route search unit 73, the self-position estimated by the position estimation unit 71, and the flight speed estimated by the speed estimation unit 72, to control the autonomous flying robot 1. Route following control is performed so as to fly along the movement route calculated by the route searching means 73. Specifically, a control command value, which is a flight control value at each time, is obtained using the movement route, the self-position, and the flight speed, and the motor 4 is controlled based on the control command value. Control.
経路追従制御では、まず、各時刻での自律飛行ロボット１が目標とすべき直近の位置（以下、「ローカル目標」と呼ぶ）を算出する処理を行う。図７はローカル目標の算出を説明する図である。ローカル目標の算出にあたり、移動制御手段７４は、経路探索手段７３で生成された移動経路を記憶部８から読出し、自律飛行ロボット１が現在時刻で目指している経由点Ｗｐ１と前回通過済みである経由点Ｗｐ０との２点間を繋げた直線Ｗを求める。そして、移動制御手段７４は、求めた直線Ｗと自律飛行ロボット１の自己位置を中心とした球Ｓとの交点Ｌｐ’、Ｌｐを算出し、目指している経由点Ｗｐ１に近い交点Ｌｐをローカル目標として求める。このように、各時刻においてローカル目標を目指して自律飛行ロボット１が移動するよう飛行制御することで、常にローカル目標も移動経路上を移動目標位置Ｐｏに向かって移動していき、自律飛行ロボット１は移動経路に沿って飛行していくことになる。 In the route following control, first, a process of calculating the nearest position (hereinafter, referred to as “local target”) to be targeted by the autonomous flying robot 1 at each time is performed. FIG. 7 is a diagram illustrating the calculation of the local target. In calculating the local target, the movement control unit 74 reads the movement route generated by the route search unit 73 from the storage unit 8, and determines the route point Wp1 that the autonomous flying robot 1 is aiming at at the current time and the route that has been passed previously. A straight line W connecting the two points with the point Wp0 is obtained. Then, the movement control means 74 calculates the intersections Lp ′ and Lp of the obtained straight line W and the sphere S centered on the self-position of the autonomous flying robot 1, and sets the intersection Lp close to the target via point Wp 1 to the local target. Asking. In this way, by controlling the flight so that the autonomous flying robot 1 moves toward the local target at each time, the local target always moves on the moving path toward the moving target position Po, and the autonomous flying robot 1 moves. Will fly along the path of travel.
次に、経路追従制御では、算出したローカル目標に向かって飛行するようＸ、Ｙ、Ｚ、ヨー角の各方向毎に制御指令値ｕx，ｕy，ｕz，ｕψを算出する処理を行う。この際、現在の自己位置とローカル目標の位置との差異が小さくなるような制御指令値を求める。具体的には、ＸＹＺ軸方向の制御指令値ｕ＝（ｕx，ｕy，ｕz）は、位置推定手段７１で求められた自己位置ｒ＝（Ｘs，Ｙs，Ｚs）と速度推定手段７２で推定した速度ｖ＝（ｖx，ｖy，ｖz）とを利用し、ＰＩＤ制御により求める。ＸＹＺ軸方向の各制御指令値をｕ＝（ｕx，ｕy，ｕz）、ローカル目標をｒ’＝（ｘ，ｙ，ｚ）としたとき、制御指令値は、ｕ＝Ｋp（ｒ’−ｒ）＋Ｋd・ｖ＋Ｋi・ｅの式で算出される。ここで、Ｋp、Ｋd、ＫiはそれぞれＰＩＤ制御のゲインのことであり、ｅ＝（ｅx，ｅy，ｅz）は誤差の積分値である。一方、ヨー角方向の制御指令値ｕψは、ψ'を目標角度、ψを位置推定手段７１にて推定した自律飛行ロボット１の姿勢（角度）、ｖyawを速度推定手段７２で推定した角速度とすると、ｕψ＝Ｋp（ψ’−ψ）＋Ｋd・ｖyawの式のようなＰＤ制御により求める。なお、本実施形態では、目標角度ψ'を目標対象物Ｍの方向、すなわち、目標対象物Ｍの位置の方向を向く角度とした。 Next, in the path following control, a process of calculating control command values u x , u y , u z , u ψ for each of the X, Y, Z, and yaw angles so as to fly toward the calculated local target is performed. Do. At this time, a control command value that reduces the difference between the current self-position and the position of the local target is obtained. Speed Specifically, the control command value XYZ axis u = (u x, u y , u z) , the self-position r = obtained by the position estimation means 71 (X s, Y s, Z s) and Using the velocity v = (v x , v y , v z ) estimated by the estimating means 72, it is obtained by PID control. Each control command value of the XYZ-axis direction u = (u x, u y , u z), the local target r '= (x, y, z) when the control command value is, u = Kp (r' −r) + Kd · v + Kie · e. Here, Kp, Kd, Ki is that the gain of the PID control, respectively, e = (e x, e y, e z) is the integral value of the error. On the other hand, the control command value u の in the yaw angle direction is ψ ′ as the target angle, ψ as the attitude (angle) of the autonomous flying robot 1 estimated by the position estimating means 71, and v yaw as the angular velocity estimated by the speed estimating means 72. When obtained by the PD control as equation u ψ = Kp (ψ'-ψ ) + Kd · v yaw. Note that, in the present embodiment, the target angle 角度 ′ is an angle facing the direction of the target object M, that is, the direction of the position of the target object M.
このように、制御部７は、上述した位置推定手段７１、速度推定手段７２、経路探索手段７３、移動制御手段７４における各処理を逐次繰り返す。これにより、本実施形態の自律飛行ロボット１は、目標対象物Ｍから離間距離だけ離れた位置に対して移動目標位置を逐次更新し、その都度移動経路についても逐次更新していくことによって、目標対象物Ｍを撮像しながら追従移動することができる。特に本実施形態における自律飛行ロボット１は、風速が大きいほど占有ボクセル（飛行禁止エリア）から大きく離れるように移動経路を生成することにより、風による障害物への衝突する危険性を少なくすることができる。 As described above, the control unit 7 sequentially repeats the above-described processing in the position estimating unit 71, the speed estimating unit 72, the route searching unit 73, and the movement controlling unit 74. As a result, the autonomous flying robot 1 according to the present embodiment sequentially updates the movement target position with respect to a position that is separated from the target object M by the separation distance, and also sequentially updates the movement path each time. The user can follow the object M while capturing the image. In particular, the autonomous flying robot 1 according to the present embodiment reduces the risk of colliding with an obstacle due to wind by generating a movement path so as to be farther away from the occupied voxel (no-fly area) as the wind speed is higher. it can.
ところで、本発明は、上記実施形態に限定されるものではなく、特許請求の範囲に記載した技術的思想の範囲内で、更に種々の異なる実施形態で実施されてもよいものである。また、実施形態に記載した効果は、これに限定されるものではない。 By the way, the present invention is not limited to the above embodiments, and may be embodied in various different embodiments within the scope of the technical idea described in the claims. Further, the effects described in the embodiments are not limited thereto.
上記実施形態におけるボクセル情報更新処理では、風速情報８２に基づいて式（１）におけるλの値を変更して、各ボクセルのボクセルコストを算出した上で、閾値判定によりボクセル属性（近接ボクセル／自由ボクセル）を判定している。しかし、これに限定されるものではなく、風速が大きくなるほど占有ボクセル（飛行禁止エリア）からより遠くに離れた位置に移動経路が生成されるよう、他の方法を用いてボクセル情報８３を更新してもよい。例えば、全てのボクセルについての無風時におけるボクセルコストを式（１）によって予め算出してボクセル情報８３として記憶し、風速の大きさが大きくなるほど大きい値となる補正コスト値を別途算出して、予め記憶している無風時のボクセルコスト値に当該補正コスト値を加算することによりボクセルコストを算出し、ボクセル情報８３を更新してもよい。そして、当該ボクセルコスト値を閾値判定することによりボクセル属性を判定してもよい。 In the voxel information updating process in the above embodiment, the value of λ in Expression (1) is changed based on the wind speed information 82, the voxel cost of each voxel is calculated, and then the voxel attribute (proximity voxel / free Voxel). However, the present invention is not limited to this. The voxel information 83 is updated using another method so that a movement route is generated at a position farther away from the occupied voxel (no-fly area) as the wind speed increases. You may. For example, voxel costs for all voxels in the absence of wind are calculated in advance by equation (1) and stored as voxel information 83, and a correction cost value that increases as the wind speed increases is separately calculated. The voxel cost may be calculated by adding the correction cost value to the stored no-wind voxel cost value, and the voxel information 83 may be updated. Then, the voxel attribute may be determined by performing a threshold determination on the voxel cost value.
また、ボクセル情報更新処理では、ボクセルコストを更新せずに（一定のまま）、ボクセル属性のみを更新してもよい。例えば、風速の大きさに応じて近接ボクセルと判定するための閾値を変更し、当該変更した閾値を用いて各ボクセルが近接ボクセルであるか否かを判定し、ボクセル情報８３のボクセル属性を更新してもよい。すなわち、風速の大きさが大きいほど、近接ボクセルと判定するための閾値を小さくなるように設定することにより、あるボクセルのボクセルコスト値が風速に応じて変わらなくても、風速が大きくなるほど近接ボクセルとして設定され易くなる。他にも、風速が大きくなるほど大きな値となる離間距離を求め、あるボクセル（評価ボクセル）から当該離間距離の範囲内に占有ボクセルが存在していたときは、当該評価ボクセルのボクセル属性を近接ボクセルとして設定してもよい。この際、例えば、離間距離＝α×風速値により求めてもよい（ここでαは実験により求めたパラメータとする）。 In the voxel information updating process, only the voxel attribute may be updated without updating the voxel cost (keeping it constant). For example, a threshold value for determining a proximity voxel is changed according to the magnitude of the wind speed, and whether or not each voxel is a proximity voxel is determined using the changed threshold value, and the voxel attribute of the voxel information 83 is updated. May be. In other words, by setting the threshold value for determining the proximity voxel to be smaller as the magnitude of the wind speed is larger, even if the voxel cost value of a certain voxel does not change according to the wind speed, the closer the wind speed is, the closer the voxel cost becomes. It becomes easy to be set as. In addition, a separation distance having a larger value as the wind speed increases is determined, and when an occupied voxel exists within a range of a certain voxel (evaluation voxel) from the certain voxel, the voxel attribute of the evaluation voxel is set to a close voxel. You may set as. At this time, for example, the separation distance may be obtained by α = wind speed value (here, α is a parameter obtained by experiment).
また、上記実施形態におけるボクセル情報更新処理では、風速の大きさからなる風速情報８２に基づいてボクセル情報８３を更新している。しかし、これに限定されるものではなく、風速だけでなく風向も測定可能な風速計１０５を用い、当該風速計１０５によって出力された風速・風向に基づいてボクセル情報８３を更新してもよい。すなわち、風速計１０５にて測定された風向を参照して、風速が大きいほど風向の風下に位置する飛行禁止エリアからより遠くに離れた位置に移動経路を生成する。具体的には、生成される移動経路から見て風上側に位置する飛行禁止エリアからよりも、生成される移動経路から見て風下側に位置する飛行禁止エリアからの方が、より遠くに離れるような位置となるように移動経路を生成するものとする。例えば、あるボクセル（評価ボクセル）のボクセルコストを求めるのに際し、まず、評価ボクセルから占有ボクセルに向かう直線方向と風向とがなす角δが小さいほど、式（１）におけるλの値が小さくなるように設定する。例えば、式（１）におけるλ値を、λ＝λ0−β×ｃｏｓδ（０°≦δ＜９０°）…式（３）により求めることができる。ここで、λ0は無風時におけるλ値として予め設定された値であり、βは風速の大きさが大きくなるほど大きな値となる値であって風速に対応付けて予め設定された値である。なお、式（３）においてδ≧９０°の場合はλ＝λ0を用いるものとする。また、風向を考慮してボクセルコストを算出する場合、式（１）及び式（３）で対象とする占有ボクセルを、評価ボクセルに最も近い占有ボクセルではなく、評価ボクセルの所定範囲内の周囲に存在する占有ボクセルについて其々ボクセルコストを算出し、求めた各ボクセルコストの中で最も大きい値のボクセルコストを採用するのが好適である。図８は、式（１）及び式（３）により風速・風向に基づいてボクセル情報８３を更新した例を表す図である。図８（ａ）は、移動空間に東風が発生していた時のボクセル情報８３の例を表しており、図８（ｂ）は、西風が発生していた時のボクセル情報８３の例を表しており、図８（ａ）に表すように、東風が発生している場合、評価ボクセルから見て当該風向（東風）と同じ方向（東側）に存在する占有ボクセルの影響を強く受ける。したがって、占有ボクセルの西側に位置するボクセルの多くが近接ボクセルとして設定されることになる。一方、図８（ｂ）に表すように、西風が発生している場合は、占有ボクセルの東側に位置するボクセルの多くが近接ボクセルとして設定されることになる。このように風速だけでなく風向も考慮してボクセルコストを算出することにより、経路探索手段７３は、風速が大きいほど、風向と逆の方向（風上）にある占有ボクセルからよりも、風向と同じ方向（風下）にある占有ボクセルから大きく（より遠くに）離れるように移動経路を生成する。これにより、自律飛行ロボット１は、移動自由度を確保しつつ、より安全に飛行することが可能となる。なお式（３）におけるλ0は、風速に関係なく固定値としているが、当該λ0についても風速に応じて小さくなるように更新してもよい。 In the voxel information updating process in the above embodiment, the voxel information 83 is updated based on the wind speed information 82 including the magnitude of the wind speed. However, the present invention is not limited to this. The anemometer 105 that can measure not only the wind speed but also the wind direction may be used, and the voxel information 83 may be updated based on the wind speed and the wind direction output by the anemometer 105. That is, with reference to the wind direction measured by the anemometer 105, a movement route is generated at a position farther away from the no-fly area located downstream of the wind direction as the wind speed increases. Specifically, it is farther away from the no-fly area, which is located on the leeward side when viewed from the generated travel path, than from the no-fly area, which is located on the windward side when viewed from the generated travel path. It is assumed that the movement route is generated so as to be at such a position. For example, when calculating the voxel cost of a certain voxel (evaluation voxel), first, the smaller the angle δ between the straight line direction from the evaluation voxel to the occupied voxel and the wind direction, the smaller the value of λ in equation (1). Set to. For example, the value of λ in equation (1) can be determined by λ = λ 0 −β × cos δ (0 ° ≦ δ <90 °)... Here, λ 0 is a value preset as a λ value when there is no wind, and β is a value that increases as the wind speed increases, and is a value preset in association with the wind speed. When δ ≧ 90 ° in Expression (3), λ = λ 0 is used. Further, when calculating the voxel cost in consideration of the wind direction, the occupied voxel targeted by Expressions (1) and (3) is set not to the occupied voxel closest to the evaluation voxel but to the surroundings within a predetermined range of the evaluation voxel. It is preferable to calculate the voxel cost for each of the existing occupied voxels, and to use the largest voxel cost among the obtained voxel costs. FIG. 8 is a diagram illustrating an example in which the voxel information 83 is updated based on the wind speed and the wind direction according to Expressions (1) and (3). FIG. 8A shows an example of the voxel information 83 when the east wind is generated in the moving space, and FIG. 8B shows an example of the voxel information 83 when the west wind is generated. As shown in FIG. 8A, when the east wind is generated, the occupied voxel existing in the same direction (east side) as the wind direction (east wind) as viewed from the evaluation voxel is strongly affected. Therefore, most of the voxels located on the west side of the occupied voxels are set as the close voxels. On the other hand, as shown in FIG. 8B, when a west wind is generated, many voxels located on the east side of the occupied voxels are set as close voxels. By calculating the voxel cost in consideration of not only the wind speed but also the wind direction in this way, the route search unit 73 determines that the higher the wind speed is, the more the wind direction becomes from the occupied voxels in the opposite direction (windward) to the wind direction. The movement path is generated so as to largely (farther) away from the occupied voxel in the same direction (leeward). As a result, the autonomous flying robot 1 can fly more safely while securing the freedom of movement. Although λ 0 in Expression (3) is a fixed value irrespective of the wind speed, the λ 0 may be updated so as to become smaller according to the wind speed.
また、上記実施形態におけるボクセル情報更新処理では、ボクセル情報更新処理の実行時（現在）における風速の大きさに基づいて、式（１）の計算に用いるλの値を決定している。しかし、これに限定されるものではなく、現在から過去の所定期間（例えば、１０分間）における最大瞬間風速に基づいてλの値を決定してもよい。すなわち、現在から過去の所定期間における最大瞬間風速値が大きいほど、式（１）の計算に用いるλの値が小さくなるように決定する。したがって、経路探索手段７３は、現在から過去の所定期間における最大瞬間風速値が大きいほど、占有ボクセル（飛行禁止エリア）から大きく離れるような移動経路を生成するようになる。これにより、一時的に風が弱くなったタイミングで設定されたボクセルコスト（及びボクセル属性）に基づいて移動経路が生成され、再び風が強くなることによって障害物に衝突する危険性を低下させることができる。 In the voxel information updating process in the above embodiment, the value of λ used in the calculation of Expression (1) is determined based on the magnitude of the wind speed at the time (current) of the execution of the voxel information updating process. However, the present invention is not limited to this, and the value of λ may be determined based on the maximum instantaneous wind speed in a predetermined period from the present to the past (for example, 10 minutes). That is, it is determined that the larger the maximum instantaneous wind value in the predetermined period from the present to the past is, the smaller the value of λ used in the calculation of Expression (1) becomes. Therefore, the route searching means 73 generates a moving route that is farther away from the occupied voxel (flight prohibited area) as the maximum instantaneous wind speed value in the predetermined period from the present to the past is larger. Thereby, a moving route is generated based on the voxel cost (and voxel attribute) set at the timing when the wind temporarily becomes weak, and the risk of colliding with an obstacle due to the wind becoming strong again is reduced. Can be.
また、上記各実施形態におけるボクセル情報更新処理では、風速（及び風向）に基づいてボクセル情報８３を更新している。しかし、これに限定されるものではなく、現在から過去の所定期間内における最大瞬間風速と平均風速との比から突風率（突風率＝最大瞬間風速÷平均風速）を更に求め、突風率も考慮してボクセル情報８３を更新してもよい。すなわち、突風率が大きいほど式（１）の計算に用いるλの値が小さくなるように決定する。或いは、突風率が大きいほど各ボクセルのボクセルコストが大きくなるように補正したり、近接ボクセルを判定する閾値が小さくなるように補正した上で、ボクセル属性を更新してもよい。このようにしてボクセル情報８３を更新することにより、経路探索手段７３は、突風率が大きいほど、占有ボクセル（飛行禁止エリア）から大きく離れるような移動経路を生成するようになる。したがって、突風の影響を考慮して移動経路を生成することができ、突風により障害物に衝突する危険性を低下させることができる。 In the voxel information updating process in each of the above embodiments, the voxel information 83 is updated based on the wind speed (and the wind direction). However, the present invention is not limited to this. The gust rate is further obtained from the ratio of the maximum instantaneous wind speed to the average wind speed in the predetermined period from the present to the past (gust rate = maximum instantaneous wind speed / average wind speed), and the gust rate is also considered. Then, the voxel information 83 may be updated. That is, the value of λ used in the calculation of the equation (1) is determined to be smaller as the gust factor is larger. Alternatively, the voxel attribute may be updated after correcting so that the voxel cost of each voxel increases as the gust factor increases, or so that the threshold value for determining a nearby voxel decreases. By updating the voxel information 83 in this way, the route searching means 73 generates a moving route that is farther away from the occupied voxel (no-fly area) as the gust factor increases. Therefore, it is possible to generate the movement route in consideration of the influence of the gust, and it is possible to reduce the risk of colliding with an obstacle due to the gust.
また、上記各実施形態における移動経路生成処理では、Ａ＊経路探索法によって移動経路を生成する際、ボクセル情報８３のボクセル属性を参照することで近接ボクセルに移動経路が生成されないよう処理している。しかし、これに限定されるものではなく、ボクセル属性を参照せずにボクセルコストのみを用いて移動経路を生成してもよい。すなわち、この場合においても、風速に基づいてボクセルコストを更新することにより、風速が大きいほど占有ボクセル（飛行禁止エリア）から大きく離れるよう移動経路を生成されることに変わりない。 In the movement route generation processing in each of the above embodiments, when the movement route is generated by the A * route search method, the voxel information 83 is referred to the voxel attribute so that the movement route is not generated in the adjacent voxel. . However, the present invention is not limited to this, and the moving route may be generated using only the voxel cost without referring to the voxel attribute. That is, also in this case, by updating the voxel cost based on the wind speed, the moving route is generated such that the greater the wind speed is, the farther away from the occupied voxel (no-fly area).
また、上記実施の形態では、管理装置１０２に接続された目標対象物検出センサ１０４を用いて目標対象物Ｍを検出している。しかしながら、これに限定されるものではなく、撮像部３で取得した撮像画像を画像解析することにより、目標対象物Ｍの位置を推定してもよい。例えば、撮像画像の各フレームを画像処理して目標対象物Ｍの画像領域を抽出する処理を行う。この際、既知の従来技術（特開２００６−１４６５５１号公報を参照）であるオプティカルフロー法、ブースティング学習（例えば、Ｈａａｒ−ｌｉｋｅ特徴を用いているＡｄａＢｏｏｓｔベース識別器による顔検出手法）による識別器等を用いて目標対象物Ｍの画像領域（人物領域）を抽出する。次に、当該抽出された画像領域の位置に基づいて目標対象物Ｍと自律飛行ロボット１との距離を推定する。具体的には、抽出した目標対象物Ｍの画像領域の頭頂部の（撮像画像における）ｙ座標位置と距離との対応表を予め飛行高度毎に作成しておき、現在の飛行高度及び目標対象物Ｍの頭頂部のｙ座標位置を当該対応表に照らし合わせて自律飛行ロボット１との距離を推定する。しかし、これに限らず、抽出した目標対象物Ｍの頭部の大きさから距離を算出してもよい。すなわち、頭部の大きさと距離との対応表を予め作成しておき、抽出された目標対象物Ｍの頭部の大きさを当該対応表に照らし合わせて自律飛行ロボット１との距離を推定してもよい。 In the above embodiment, the target object M is detected using the target object detection sensor 104 connected to the management device 102. However, the present invention is not limited to this, and the position of the target object M may be estimated by performing image analysis on a captured image acquired by the imaging unit 3. For example, a process of extracting an image area of the target object M by performing image processing on each frame of the captured image is performed. At this time, a discriminator based on an optical flow method and boosting learning (for example, a face detection method using an AdaBoost-based discriminator using a Haar-like feature) which is a known conventional technique (refer to JP-A-2006-146551). The image area (person area) of the target object M is extracted using the method described above. Next, the distance between the target object M and the autonomous flying robot 1 is estimated based on the position of the extracted image area. Specifically, a correspondence table between the y coordinate position (in the captured image) and the distance at the top of the image area of the extracted target object M is created in advance for each flight altitude, and the current flight altitude and the target object The distance from the autonomous flying robot 1 is estimated by comparing the y coordinate position of the top of the object M with the correspondence table. However, the present invention is not limited to this, and the distance may be calculated from the size of the extracted head of the target object M. That is, a correspondence table between the head size and the distance is created in advance, and the distance to the autonomous flying robot 1 is estimated by comparing the extracted head size of the target object M with the correspondence table. You may.
また、自律飛行ロボット１に撮像部３としてカラーカメラの代わりに距離画像センサを搭載して、当該距離画像センサから取得した距離画像を用いて、既知の移動物体抽出技術により目標対象物Ｍを抽出して、抽出した目標対象物Ｍと自律飛行ロボット１との距離値と自己位置とから目標対象物Ｍの位置を推定してもよい。また、自律飛行ロボット１にレーザスキャナを搭載し、当該レーザスキャナの出力値と自己位置とを用いて目標対象物Ｍの位置を推定してもよい。 Further, a distance image sensor is mounted on the autonomous flying robot 1 instead of the color camera as the imaging unit 3, and a target object M is extracted by a known moving object extraction technique using a distance image acquired from the distance image sensor. Then, the position of the target object M may be estimated from the distance value between the extracted target object M and the autonomous flying robot 1 and the self-position. Alternatively, a laser scanner may be mounted on the autonomous flying robot 1, and the position of the target object M may be estimated using the output value of the laser scanner and its own position.
また、上記実施の形態では、制御部７において位置推定処理、速度推定処理、ボクセル情報更新処理、移動経路生成処理、移動制御処理（経路追従制御）の一連の処理を行っている。しかし、これに限らず、図示しない制御用のＰＣを用意し、当該ＰＣにこれらの一連の処理を実施させてもよい。すなわち、自律飛行ロボット１は、ＰＣによって行われた位置推定処理、速度推定処理、ボクセル情報更新処理、移動経路生成処理、移動制御処理によって得られた制御指令値を無線通信又は有線通信によりＰＣから受信し、当該制御指令値に基づいてモータ４の回転数を制御することにより、目的の位置に飛行するようにしてもよい。このように、外部ＰＣを用いて上記の一連の処理を分担することにより、自律飛行ロボット１のＣＰＵ処理負荷を低減することができ、ひいてはバッテリの消耗も抑えることができる。 Further, in the above embodiment, the control unit 7 performs a series of processing of the position estimation processing, the velocity estimation processing, the voxel information update processing, the movement path generation processing, and the movement control processing (path following control). However, the present invention is not limited to this, and a control PC (not shown) may be prepared, and the PC may execute a series of these processes. That is, the autonomous flying robot 1 transmits the control command values obtained by the position estimation processing, the velocity estimation processing, the voxel information update processing, the movement route generation processing, and the movement control processing performed by the PC from the PC by wireless communication or wired communication. By receiving and controlling the number of rotations of the motor 4 based on the control command value, the vehicle may fly to a target position. As described above, by sharing the above-described series of processing using the external PC, the CPU processing load of the autonomous flying robot 1 can be reduced, and the battery consumption can be suppressed.
１ 自律飛行ロボット、２（２ａ〜２ｄ） ロータ、３ 撮像部、４（４ａ〜４ｄ） モータ、５ 距離検出センサ、６ 位置検出センサ、７ 制御部、８ 記憶部、９ 通信部、７１ 位置推定手段、７２ 速度推定手段、７３ 経路探索手段、７４ 移動制御手段、８１ 目標対象物位置、８２ 風速情報、８３ ボクセル情報、８４ 移動空間グラフ情報、８５ 各種パラメータ、１００ 警備センタ、１０２ 管理装置、１０４ 目標対象物検出センサ、１０５ 風速計、１１０ 情報通信網。
Reference Signs List 1 autonomous flying robot, 2 (2a to 2d) rotor, 3 imaging unit, 4 (4a to 4d) motor, 5 distance detection sensor, 6 position detection sensor, 7 control unit, 8 storage unit, 9 communication unit, 71 position estimation Means, 72 speed estimation means, 73 route search means, 74 movement control means, 81 target object position, 82 wind speed information, 83 voxel information, 84 moving space graph information, 85 various parameters, 100 security center, 102 management device, 104 Target object detection sensor, 105 anemometer, 110 information communication network.
Claims (4)
前記移動空間内を複数のボクセルに分割し、前記ボクセルの各々について飛行禁止エリアか否かを示す属性及び自律移動ロボットの移動の障害となる度合いを示すボクセルコスト値を関連付けて記憶した記憶部と、
前記移動空間における前記自律飛行ロボットの自己位置を推定する位置推定手段と、
前記移動空間における風速を計測する風測定手段と、
前記属性および前記ボクセルコスト値に基づいて、前記飛行禁止エリアを通過せずに移動目標位置に到達するまでに通過する前記ボクセルの前記ボクセルコスト値を加算した総コスト値が最も小さくなる移動経路を探索する経路探索手段と、
前記移動経路に沿って移動するよう制御する移動制御手段と、を有し、
前記経路探索手段は、前記飛行禁止エリアの近傍に前記風速が大きいほど広い範囲に存在する前記ボクセルの前記ボクセルコスト値を大きな値に更新して前記移動経路を探索することを特徴とする自律飛行ロボット。 An autonomous flying robot that autonomously flies in a moving space,
A storage unit that divides the moving space into a plurality of voxels, and associates and stores an attribute indicating whether each of the voxels is a flight-forbidden area and a voxel cost value indicating a degree of obstruction of movement of the autonomous mobile robot. ,
Position estimating means for estimating the self-position of the autonomous flying robot in the moving space,
Wind measuring means for measuring a wind speed in the moving space,
Based on the attributes and the voxel cost value, the no-fly area wherein the total cost value is the smallest of that move obtained by adding the voxel cost value of voxels without passing through before reaching the movement target position Route search means for searching a route;
Movement control means for controlling to move along the movement path,
The autonomous flight characterized in that the route search means searches the movement route by updating the voxel cost value of the voxel present in a wider range as the wind speed increases near the no-fly area to a larger value. robot.
前記経路探索手段は、前記風向を参照し、前記範囲に存在するボクセルのうちの風上側よりも風下側のボクセルの前記ボクセルコスト値を大きな値に更新する請求項１に記載の自律飛行ロボット。 The wind measuring means further measures a wind direction in the moving space,
2. The autonomous flying robot according to claim 1, wherein the route search unit refers to the wind direction and updates the voxel cost value of voxels on the leeward side of the voxels existing in the range to a value larger than the leeward side. 3.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2015151739A JP6664162B2 (en) | 2015-07-31 | 2015-07-31 | Autonomous flying robot |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2015151739A JP6664162B2 (en) | 2015-07-31 | 2015-07-31 | Autonomous flying robot |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2017033232A JP2017033232A (en) | 2017-02-09 |
| JP6664162B2 true JP6664162B2 (en) | 2020-03-13 |
Family
ID=57986388
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2015151739A Active JP6664162B2 (en) | 2015-07-31 | 2015-07-31 | Autonomous flying robot |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP6664162B2 (en) |
Families Citing this family (19)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP6746516B2 (en) * | 2017-03-03 | 2020-08-26 | アルパイン株式会社 | Flight control system and scheduled route setting method for unmanned aerial vehicle |
| JP7044293B2 (en) * | 2017-03-17 | 2022-03-30 | 東京電力ホールディングス株式会社 | Equipment inspection system |
| JP6866203B2 (en) * | 2017-03-28 | 2021-04-28 | 株式会社ゼンリンデータコム | Drone navigation device, drone navigation method, drone navigation program, search data formation device and search data formation program |
| JP2018165931A (en) * | 2017-03-28 | 2018-10-25 | 株式会社ゼンリンデータコム | Control device for drone, control method for drone and control program for drone |
| JP6846742B2 (en) * | 2017-04-21 | 2021-03-24 | 国立研究開発法人産業技術総合研究所 | Tapping sound inspection device |
| JP6962720B2 (en) | 2017-06-21 | 2021-11-05 | エスゼット ディージェイアイ テクノロジー カンパニー リミテッドSz Dji Technology Co., Ltd | Flight control methods, information processing equipment, programs and recording media |
| JP7109174B2 (en) * | 2017-10-03 | 2022-07-29 | 株式会社トプコン | Route selection device, unmanned aircraft, data processing device, route selection processing method, and route selection processing program |
| JP6733948B2 (en) * | 2018-02-28 | 2020-08-05 | 株式会社ナイルワークス | Drone, its control method, and control program |
| JP2019164415A (en) * | 2018-03-19 | 2019-09-26 | セコム株式会社 | Flight control system |
| JP2019163992A (en) * | 2018-03-19 | 2019-09-26 | 東京電力ホールディングス株式会社 | Data processing device, data processing method and program |
| JP7109963B2 (en) * | 2018-03-30 | 2022-08-01 | セコム株式会社 | Route evaluation device |
| JP7130409B2 (en) * | 2018-03-30 | 2022-09-05 | セコム株式会社 | Control device |
| JP7107757B2 (en) * | 2018-06-06 | 2022-07-27 | 株式会社日立製作所 | Autonomous mobile device |
| JP7215706B6 (en) * | 2018-07-11 | 2023-02-14 | メトロウェザー株式会社 | Flight path calculation system, flight path calculation program, and unmanned aircraft path control method |
| JP7106417B2 (en) * | 2018-10-04 | 2022-07-26 | セコム株式会社 | Flight plan calculation device and program |
| WO2020179460A1 (en) * | 2019-03-06 | 2020-09-10 | ソニー株式会社 | Control device, control method, and program |
| JP2022540525A (en) * | 2019-05-24 | 2022-09-16 | エイ・キューブド・バイ・エアバス・エル・エル・シー | Unmanned Aerial Vehicle Risk Assessment |
| CN114521248A (en) * | 2019-09-30 | 2022-05-20 | 索尼集团公司 | Information processing apparatus, information processing method, and program |
| JP2022038675A (en) * | 2020-08-27 | 2022-03-10 | ソニーグループ株式会社 | Information processor and information processing method, computer program and mobile device |
Family Cites Families (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP4233703B2 (en) * | 1999-09-14 | 2009-03-04 | 三菱重工業株式会社 | Helicopter bank angle controller |
| JP5391164B2 (en) * | 2010-08-23 | 2014-01-15 | 日本電信電話株式会社 | Autonomous mobile robot motion planning method, autonomous mobile robot control method using autonomous mobile robot motion planning method, autonomous mobile robot motion planning device, autonomous mobile robot motion control device, autonomous mobile robot motion planning program, autonomous mobile robot Mobile robot control program |
| JP6029446B2 (en) * | 2012-12-13 | 2016-11-24 | セコム株式会社 | Autonomous flying robot |
-
2015
- 2015-07-31 JP JP2015151739A patent/JP6664162B2/en active Active
Also Published As
| Publication number | Publication date |
|---|---|
| JP2017033232A (en) | 2017-02-09 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP6664162B2 (en) | Autonomous flying robot | |
| JP6029446B2 (en) | Autonomous flying robot | |
| JP6599143B2 (en) | Autonomous mobile robot | |
| JP6195450B2 (en) | Autonomous flying robot | |
| US20220234733A1 (en) | Aerial Vehicle Smart Landing | |
| JP6527726B2 (en) | Autonomous mobile robot | |
| JP6235213B2 (en) | Autonomous flying robot | |
| JP5990453B2 (en) | Autonomous mobile robot | |
| JP6140458B2 (en) | Autonomous mobile robot | |
| JP7020421B2 (en) | Flight controls, unmanned aerial vehicles, flight control methods, and programs | |
| WO2018086133A1 (en) | Methods and systems for selective sensor fusion | |
| JP6014485B2 (en) | Autonomous flying robot | |
| KR20200031165A (en) | Navigation chart configuration method, obstacle avoidance method and device, terminal, drone | |
| JP6758068B2 (en) | Autonomous mobile robot | |
| JP2017182692A (en) | Autonomous Mobile Robot | |
| JP6014484B2 (en) | Autonomous mobile robot | |
| JP6469492B2 (en) | Autonomous mobile robot | |
| JP6530212B2 (en) | Autonomous mobile robot | |
| WO2021087701A1 (en) | Terrain prediction method and apparatus for undulating ground, and radar, unmanned aerial vehicle and operating control method | |
| JP2019164415A (en) | Flight control system | |
| WO2021087702A1 (en) | Sloped terrain prediction method and device, radar, unmanned aerial vehicle, and operation control method | |
| WO2018094576A1 (en) | Unmanned aerial vehicle control method, flight controller, and unmanned aerial vehicle | |
| JP6758069B2 (en) | Autonomous flying robot | |
| JP6595284B2 (en) | Autonomous mobile robot | |
| WO2020088399A1 (en) | Information processing device, flight control method, and flight control system |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20180523 |
|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20190405 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20190409 |
|
| A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20190610 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20191112 |
|
| A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20200108 |
|
| 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: 20200204 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20200218 |
|
| R150 | Certificate of patent or registration of utility model |
Ref document number: 6664162 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
| R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
| R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |