JP6195450B2 - Autonomous flying robot - Google Patents

Description

  The present invention relates to an autonomous flying robot that flies following a moving object such as an intruder, and more particularly to an autonomous flying robot that quickly finds the moving object and performs a follow-up flight even when the moving object is lost.

  Conventionally, an autonomous mobile robot that moves while following autonomously while maintaining a predetermined distance with respect to the tracking target has been proposed. For example, Patent Document 1 discloses an autonomous mobile robot that recognizes a tracking target based on a camera image, recognizes a distance to the tracking target, and performs movement control so that the tracking movement is maintained at a predetermined distance. ing. In the related art, when the tracking target is lost (that is, when the person who is the tracking target fails to be recognized), the tracking is stopped and the operation is stopped.

JP 2004-299025 A

  However, the above-described conventional technology does not perform control so that the tracking target can be quickly found when the tracking target is lost. When using autonomous mobile robots for security purposes such as monitoring intruders, in order to monitor the actions of intruders and to avoid attacks from intruders, we quickly find missing intruders and follow them. Need to continue. By the way, in recent years, from the aspect of being able to move freely in space without being restricted by obstacles such as objects and steps on the moving path, it is possible to move to a position where it is difficult to be attacked by intruders, etc. The use of autonomous flying robots, such as autonomously moving a flying robot to monitor an intruder, has been studied. Such an autonomous flight robot has a feature that the surroundings can be monitored from a bird's-eye view by raising the flight altitude. Accordingly, the present invention focuses on the characteristics of such an autonomous flying robot and aims to perform movement control so that the tracking target can be quickly found by raising the flight altitude when the tracking target is lost.

  In order to solve such a problem, an autonomous flying robot that follows a moving object in a flight space and images the moving object with an imaging unit, the storage unit and a process for estimating the self-position of the autonomous flying robot A position estimation unit that performs a process of estimating a moving object position of the moving object based on a captured image acquired by the imaging unit and the self position, and the moving object position can be estimated by the position estimation unit, A normal setting process for setting a movement target position at a position of a predetermined standard altitude and a position away from the moving object position by a predetermined standard separation distance; and when the moving object position cannot be estimated by the position estimating means, Target setting means for performing search setting processing for setting a movement target position at a high altitude position higher than the standard altitude, and control to move from the self position to the movement target position A processing movement control means for performing that, the, to provide autonomous flying robots, characterized in that to follow the flight by repeating the respective processes successively.

  With such a configuration, the target setting unit of the present invention can detect a moving object by the imaging unit, loses sight of the moving object, and cannot estimate the position of the moving object (hereinafter referred to as “moving object remnant”). Performs a process of setting the movement target position at a position higher than the flight altitude when following the flight without losing sight of the moving object (normal time). As a result, the autonomous flying robot of the present invention has a wide range (field of view) that can be imaged by the imaging unit by performing flight control so as to increase the flight altitude even when a moving object is lost. It is possible to look down on the surroundings, and thus, it is possible to quickly find a moving object, and it is possible to quickly continue the follow-up flight.

  Moreover, as a preferred aspect of the present invention, the storage unit stores imaging condition information indicating a field of view of the imaging unit and a predetermined reference position in the flight space, and the position estimation unit is configured to determine an attitude of the autonomous flying robot. Further, the target setting means calculates a search separation distance at which the reference position can be imaged from the high altitude position using the imaging condition information as the search setting processing, and the search separation distance from the reference position. The movement target unit sets the movement target position at a position separated by a distance, and the movement control unit sets the posture so that the imaging unit faces the direction of the moving object position when the position estimation unit can estimate the moving object position. When the position of the moving object cannot be estimated by the position estimation means, the posture is controlled so that the imaging unit faces the direction of the reference position.

  With such a configuration, during normal flight following a moving object, posture control is performed so that the position of the moving object is directed from the position of the standard altitude at a position away from the moving object position by a predetermined standard separation distance. Accordingly, it is possible to follow and fly while always imaging a moving object. On the other hand, when a moving object is lost, a search separation distance that can image a predetermined reference position (for example, an entrance / exit position where an intruder can be easily found or a detected sensor installation position) is calculated from a height position at a high altitude. Then, a position separated by the search separation distance calculated from the reference position and a height position at a high altitude is set as a movement target position, and posture control is performed so as to face the direction of the reference position. As a result, when the moving object is lost, the moving position is moved from a high altitude position toward a position where the reference position can be imaged, and the imaging unit is always directed toward the reference position, so that it is easy to find a missing moving object. The reference position can be looked down from a high altitude, and a moving object can be found more quickly.

  Moreover, as a preferable aspect of the present invention, the movement control means performs a process of obtaining the reference position from the moving object position estimated in the most recent period and storing it in the storage unit as the search setting process. .

  With this configuration, the movement control unit of the present invention sets the reference position based on the moving object position that can be estimated in the most recent period when the moving object is lost. For example, the moving object position estimated immediately before is set as the moving object position. In general, when a moving object being tracked is lost, it is highly likely that the moving object is present in the vicinity of the position where it was lost. Therefore, a moving object can be found more efficiently by setting a reference position based on a moving object position in a period immediately before losing sight and looking down on the reference position from a high altitude.

  As a preferred aspect of the present invention, the storage unit further stores a feature position indicating a height position of a feature part of the moving object, and the target setting unit includes the imaging condition information as the normal setting process. Calculating the altitude at which the characteristic part of the moving object can be imaged from the position separated from the moving object position by the standard separation distance using the characteristic position, and storing the altitude as the standard altitude in the storage unit; To do.

  With such a configuration, the autonomous flying robot of the present invention images a characteristic part such as the face of the intruder from imaging condition information such as a separation distance (standard separation distance) from a moving object and an imaging direction (a depression angle, etc.) of the imaging unit. By obtaining a flying altitude (standard altitude) that can be performed and performing a follow-up flight at the flight altitude, it is possible to follow-up while normally imaging a characteristic part of a moving object.

  As a preferred aspect of the present invention, the target setting means extracts an image area of the moving object from the captured image, and uses the image area, the imaging condition information, and the self-position to determine the height of the moving object. It is assumed that the characteristic position is estimated from the height and stored in the storage unit.

  With this configuration, the autonomous flying robot of the present invention estimates the height of the moving object, for example, the height of the intruder from the captured image acquired by the imaging unit, and the feature position (for example, the face position) according to the estimated height. , The height from the ground to the center of the face). As a result, it is possible to calculate the flight altitude at which the characteristic part can be appropriately imaged according to the size of the moving object, for example, the height of the intruder, and to follow the flight while appropriately imaging the characteristic part.

  As described above, when the autonomous flying robot of the present invention loses sight of the intruder (moving object) to be tracked, it quickly finds the intruder by increasing the flight altitude and monitoring the surrounding area. can do.

Overview of autonomous flying robot Functional block diagram of autonomous flying robot Flow chart of moving target position setting process Illustration for calculating the standard altitude Explanatory drawing about calculation of search separation distance Explanatory drawing about setting of movement candidate position Explanatory diagram for setting the movement target position Explanatory drawing about setting of moving target position when moving object is lost Explanatory diagram of the graph structure in the generation of travel paths Illustration of local target calculation

  Hereinafter, an embodiment of an autonomous flying robot that follows a flying object while keeping a predetermined relative distance to an intruder as a moving object and captures an image of the moving object will be described in detail with reference to the accompanying drawings. Further, the flying mobile robot of the present invention can be similarly applied to various moving objects other than a person such as an automobile.

  FIG. 1 shows an overview of an autonomous flying robot 1 used in the present embodiment. FIG. 2 shows a functional block diagram of the autonomous flying robot 1 used in the present embodiment. As shown in FIG. 1, in the autonomous flying robot 1 used in the present embodiment, four rotors 2 (propellers) indicated by reference numerals 2a to 2d are present on one plane, and each rotor 2 is not shown in a battery (two This is a quad-rotor type small unmanned helicopter that flies by rotating by a motor 6 driven by a secondary battery. Generally, in a single rotor type helicopter, the azimuth angle is maintained by canceling the counter torque generated by the main rotor with the moment generated by the tail rotor. On the other hand, in a quadrotor type helicopter such as the autonomous flying robot 1 used in the present embodiment, counter torque is canceled by using a rotor 2 that rotates in different directions in front and rear and left and right. For example, when it is desired to rotate the machine body in the yaw direction, a difference is given to the rotational speeds of the front and rear rotors 2a, 2c and the left and right rotors 2d, 2b as indicated by arrows fa to fd. Thus, by controlling the rotation speed of each rotor 2, various movements and adjustments of the posture can be performed.

The imaging unit 3 includes a two-dimensional image sensor having an optical system such as a lens and a two-dimensional array element such as a CCD or CMOS with predetermined pixels (for example, 640 × 480 pixels), and captures captured images of the flight space for a predetermined time. This is a so-called color camera that is acquired at intervals. In the present embodiment, the imaging unit 3 is installed in the housing part so that the optical axis thereof captures the front direction of the autonomous flying robot 1 and is a space obliquely below by a predetermined depression angle θ from the horizontal plane (XY plane). It is installed to take pictures. The acquired captured image is output to the control unit 7 described later, and is stored in the storage unit 8 by the control unit 7, or transmitted to an external device (not shown) via the communication unit 9 described later.
The communication unit 9 is a communication module for performing wireless communication with an external device by, for example, a wireless LAN or a mobile phone line. In the present embodiment, by transmitting the captured image acquired by the imaging unit 3 to a PC installed in a security center (not shown), it is possible for a security guard or the like to remotely monitor an intruder.

  The distance detection sensor 4 is a sensor that detects a distance between an obstacle existing around the autonomous flying robot 1 and the distance detection sensor 4 and acquires a relative position of the obstacle existing in the sensor detection range. is there. In the present embodiment, a laser scanner is provided as the distance detection sensor 4. The laser scanner performs two-dimensional scanning for each angle of a certain angular sample interval, thereby polar information indicating the distance information from the ground (or floor surface) to an object (obstacle) existing in the horizontal plane at a certain height. Can be obtained as: Here, the two-dimensional scan in the laser scanner means that a search signal that is a laser beam is transmitted radially so as to scan a preset detection area, and is returned after being reflected by an object in the detection area. The distance to the object is calculated from the time difference between transmission and reception, and the direction in which the search signal is transmitted and the calculated distance are obtained. In this embodiment, a laser sensor capable of measuring a distance from an obstacle around the autonomous flying robot is used with an angle sample interval of 0.25 °, a detection angle range of 360 °, and a sensor detection range of 30 m. The distance detection sensor 4 is also used to estimate the flight altitude using distance information measured by reflecting a part of the laser emitted from the laser scanner toward the ground with a mirror 5.

The storage unit 8 is an information storage device such as a ROM (Read Only Memory), a RAM (Random Access Memory), and an HDD (Hard Disk Drive). The storage unit 8 stores various programs and various data, and inputs / outputs such information to / from the control unit. The various data includes 2D point information 81, voxel information 82, standard separation distance 83, imaging condition information 84, feature position 85, standard altitude 86, and various setting values and threshold values used for each process of the control unit 7. The parameter 84, the output value from each sensor, a captured image, and the like are included.

  The 2D point information 81 is information used to estimate the current flight position (hereinafter referred to as “self position”) of the autonomous flying robot by the position estimation unit 71 described later, and is two-dimensional in a horizontal plane called global coordinates. This is coordinate information of a point set that is expressed on the absolute coordinates and represents the outer shape of an obstacle such as a building in the flight space. In the present embodiment, a plurality of point sets set for each flight altitude are stored in advance in the storage unit 8 as 2D point information, and the point set of the flight altitude corresponding to the flight altitude of the autonomous flying robot is stored in the storage unit. It is assumed that the data is read from 8 and used.

  The voxel information 82 is information that divides the flight space into a plurality of voxels as a voxel space and represents the structure of obstacles in the flight space, and is information that is set in advance by an administrator or the like and stored in the storage unit 8. . In the present embodiment, the flying space is divided into a predetermined size (for example, 15 cm × 15 cm), and voxels located on obstacles such as buildings are defined as “occupied voxels”, and the space where the autonomous flying robot 1 cannot move is defined. It was. A voxel located in the space existing near the occupied voxel is defined as a “proximity voxel”, and a voxel located in an area where other flight is possible is defined as a “free voxel”. Each voxel is given a voxel cost value indicating an occupancy so that it can be used when a movement route is generated by the movement route generation means described later. The voxel cost value of the occupied voxel takes the maximum value, and the voxel cost is calculated from the formula of (voxel cost value) = exp {−λ · (distance from the occupied voxel)} so that the voxel cost value decreases as the distance increases. The value was calculated. Here, λ is a parameter obtained by experiment. A voxel having a voxel cost value equal to or greater than a predetermined threshold is set as a “proximity voxel”. Further, a voxel having a voxel cost value smaller than the threshold value is set as “free voxel”, and the voxel cost value of the voxel regarded as a free voxel is reset to 0. In order to prevent the autonomous flying robot 1 from going outside the predetermined movable space in the flight space, the voxel that is the boundary between the movable space and the outside is set as the occupied voxel.

  The standard separation distance 83 is a relative distance in the horizontal plane that should be maintained between the autonomous flying robot 1 and the moving object when following the flight without losing sight of the moving object (normal time). This value is preset by a person or the like. When a predetermined moving object is monitored using the autonomous flying robot 1, it is necessary to get closer to the moving object and acquire a more detailed captured image. However, in order not to be attacked by a hostile moving object such as an intruder, it is necessary to be separated by a certain distance or more. Therefore, the autonomous flying robot 1 of the present embodiment can acquire a detailed captured image of the moving object and set the standard separation distance 83 in advance as a distance that is difficult to receive an attack from the moving object. Keep following flight while keeping. In this embodiment, it is assumed that the standard separation distance 83 is set to 3 m.

  The imaging condition information 84 is information representing the field of view of the imaging unit 3, and the depression angle θ of the imaging unit 3 is stored in the storage unit 8 as the imaging condition information 84 in this embodiment. That is, the imaging condition information 84 is a value that is appropriately set by an administrator or the like depending on the installation angle of the imaging unit 3. In another embodiment in which a camera control device capable of changing the imaging direction of the imaging unit 3 is mounted on the autonomous flying robot 1, information about the imaging direction may be acquired from the camera control device. .

  The feature position 85 is information representing the position of the feature part of the moving object. Here, the characteristic part is a characteristic part for identifying the moving object, and is a part of the moving object that can obtain a captured image with high evidence ability when the moving object is imaged. For example, when the moving object is a person, the characteristic part is a head (face), and when the moving object is an automobile, the characteristic part is a license plate part. In the present embodiment, it is assumed that the position ratio (for example, the position of the characteristic part is set to 0.95 when the total length is 1) is set as the characteristic position 85 by the administrator or the like in the total height of the moving object.

  The standard altitude 86 is the altitude that the autonomous flying robot 1 should fly in normal times (when the moving object is not lost), and is set to the height of the moving object (the height of the characteristic part) by the target setting means 73 described later. Accordingly, the standard altitude 86 is obtained and updated. Note that the initial value is set in advance by an administrator of the autonomous flying robot 1 or the like, and is set as 2.5 m in the present embodiment.

  The control unit 7 is composed of a computer having a CPU and the like, and performs position estimation processing (self-position estimation processing, moving object position estimation processing), speed estimation processing, movement target position setting processing, route generation processing, and route follow-up control processing. As a series of processing to be performed, position estimation means 71, speed estimation means 72, target setting means 73, movement route generation means 74, and flight control means 75 are included.

  The position estimation means 71 includes a self-position estimation process for estimating the current flight position (self-position) of the autonomous flying robot 1 in the flight space based on the outputs of the distance detection sensor 4 and the imaging unit 3, and the current position of the moving object. (Hereinafter referred to as “moving object position”) is performed.

  In the self-position estimation process, using the output of the distance detection sensor 4 and the 2D point information 81 stored in the storage unit 8, as the self-position, the position x, y on the horizontal plane (XY plane), the flight altitude z, Processing for estimating the yaw angle ψ, which is the rotation angle with respect to the Z axis, is performed.

  In the self-position estimation process, first, the flight altitude z of the self-position is calculated using distance information measured by reflecting a part of the laser emitted from the laser scanner toward the ground with the mirror 5. The distance information measured by the laser scanner is not always accurately measured as the distance to the ground because an object other than a building (such as a luggage or a vehicle) exists on the ground. Therefore, in this embodiment, the flight altitude is estimated by applying the extended Kalman filter to the distance information measured by the laser scanner so that it is not easily affected by obstacles other than these buildings.

Next, in the self-position estimation process, the x, y and yaw angles ψ of the self-position are estimated. Per the estimated self-position x, y, the yaw angle [psi, first reads the set point as a two-dimensional map of the flight space corresponding to the altitude z from the 2D point information 81 stored in the storage unit. Then, using the obtained point set and the output of the laser scanner, the x, y, and yaw angles ψ of the self-position are estimated by known scan matching using an ICP (Iterative Closest Point) algorithm. The ICP algorithm performs a matching process by obtaining a combination that minimizes the Euclidean distance between the two point sets A and B. That is, by matching the point set of the 2D point information 81 with the input point set that is the scan data acquired from the laser scanner mounted on the autonomous flying robot 1 to correct the position error, x, y, and yaw angle ψ can be estimated.

  In the moving object position estimation process, a captured image that is an output of the imaging unit 3 is subjected to image processing, and a process of estimating the moving object position is performed. In the moving object position estimation process, first, each frame of the captured image is subjected to image processing to extract an image area of the moving object. In the present embodiment, an image region of a moving object is extracted using an optical flow method that is a known prior art (see, for example, Japanese Patent Application Laid-Open No. 2006-146551). However, the present invention is not limited to this, and using a classifier based on known prior art boosting learning (for example, a face detection method using an AdaBoost-based classifier using Haar-like features), a human region that is a moving object is detected. It may be determined whether or not the image area is included, and the image area identified as the person area may be extracted. Moreover, you may use combining said each technique. Next, in the moving object position estimation process, the distance between the moving object and the autonomous flying robot 1 is estimated based on the position of the extracted image area of the moving object. Specifically, a correspondence table between the y coordinate position (in the captured image) and the distance of the top of the extracted moving object image area is prepared in advance for each flight altitude, and the current flight altitude and the head of the moving object are created. The distance from the moving body is estimated by comparing the top y coordinate position with the correspondence table. However, the present invention is not limited to this, and the distance may be calculated from the size of the head of the extracted moving object. That is, a correspondence table between the size and distance of the head may be created in advance, and the distance to the moving object may be estimated by comparing the size of the head of the extracted moving object with the correspondence table. And the direction where a moving object is located is calculated | required using the position coordinate (top position) on the picked-up image of the uppermost part of the extracted person area. At this time, a correspondence table between the top position and the direction in which the moving object is located is stored in the storage unit 8 in advance, and the direction in which the moving object is located is obtained by referring to the correspondence table. Then, the position coordinates in the flight space in the direction in which the moving object exists from the self position and the distance from the calculated moving object is obtained, and the position of height 0 in the position coordinates is estimated as the moving object position. . Note that the image area of the moving object extracted from the captured image is temporally tracked.

  When the moving object position is calculated in the moving object position estimation process, the position estimating unit 71 performs a process of updating the voxel information 82 based on the moving object position. Specifically, in the voxel space based on the voxel information 82 of the storage unit 8, a cylindrical model (approximately the same size as a moving object that is predetermined as a moving object to be monitored centered on the calculated moving object position ( For example, when a moving object to be monitored is an intruder, a cylinder model having a bottom radius of 0.3 m and a height of 1.7 m is arranged, and a voxel that interferes with the cylinder model is set as an occupied voxel. To update the voxel information 82. As will be described later, the autonomous flying robot 1 is flight-controlled so as not to move to the occupied voxel. However, the autonomous flying robot 1 moves with the autonomous flying robot 1 by updating the voxel information 82 based on the moving object position as described above. Contact with an object can be avoided.

The speed estimation means 72 performs processing for estimating the current flight speed (v _x , v _y , v _z , v _yaw ) of the autonomous flying robot 1 in order to be used in path following control in the flight control means 75 described later. In the present embodiment, the flight speed is obtained from the time change of the self position (x, y, z, yaw) estimated by the position estimation means 71. At this time, the flight speed was estimated using an extended Kalman filter in consideration of the influence of measurement errors and the like.

  The target setting unit 73 sets a movement target position of the autonomous flying robot 1 using the self position and the moving object position calculated by the position estimation unit 71 and various information stored in the storage unit 8. I do. FIG. 3 is a flowchart showing the movement target position setting process, and the details of the movement target position setting process will be described below with reference to FIG.

In the movement target position setting process, first, it is determined whether or not the moving object position can be estimated by the moving object position estimation process in the position estimation means 71 (ST10). In this embodiment, when the moving object position estimation process cannot extract the moving object image area, or when the extracted moving object image area is not a person area, the moving object position estimation process cannot estimate the moving object position. Determined. ST10 when the moving object position is determined to come estimated at (ST10-Yes), usually set as processing for setting the movement target position in the target setting means 73 normal (when not lose sight of the moving object) Processing is performed (ST11 to ST15).

In the normal setting process, first, the height of the moving object is estimated, and the process of obtaining the height of the feature part using the estimated height information of the moving object is performed (ST11). In the present embodiment, the height of the moving object is determined based on the moving object position estimated by the position estimating means 71 (distance between the moving object and the autonomous flying robot 1) and the size of the extracted moving object image area. presume. Specifically, a correspondence table between the size of the moving object image area extracted by the moving object position estimation process and the height of the moving object is created in advance for each flight altitude, and the current flight altitude and moving object image are created. The height of the moving object is estimated by comparing the size of the area with the correspondence table. Next, the feature position 85 is read from the storage unit 8, and the height of the feature part is calculated by multiplying the estimated height of the moving object by the position ratio of the feature position 85. For example, when the position ratio stored as the feature position 85 is 0.95 and the height of the moving object is estimated to be 1.7 m, the height of the feature portion is 1.7 m × 0.95≈1.62 m. D can be determined.

Next, in the normal setting process, a standard altitude 86 that is an altitude at which a person's face can be imaged at a normal time is calculated using the calculated height of the characteristic part, and the standard altitude 86 is calculated based on the flight altitude at which the standard altitude 86 should be moved. A process of setting as a certain target flight altitude is performed (ST12). FIG. 4 is a diagram for explaining the calculation of the standard altitude 86, and is a schematic diagram when the autonomous flying robot 1 and the moving object M are viewed from the lateral direction. In FIG. 4, the height of the characteristic part of the moving object M corresponds to D, and the standard separation distance 83 corresponds to L. Here, when the depression angle of the imaging unit 3 is θ, a standard altitude 86 (corresponding to a symbol H in FIG. 4) that can capture the face of the person as the moving object M is calculated by H = D + L · tan θ. be able to. In this way, by calculating the standard height 86 according to the feature position 85, the standard separation distance 83, and the imaging condition information 84 (the depression angle θ), the feature part of the moving object M (for example, a head such as a face) at normal times. It is possible to follow the moving object M while maintaining a predetermined standard separation distance 83 at the flight altitude at which the image can be captured. Further, by obtaining the height of the feature position 85 according to the estimated height of the moving object M from the ground G, the position where the feature part can be imaged according to the height of the moving object M is set as the standard height 86. be able to. For example, the standard altitude 86 can be set to a height at which the head can be imaged in consideration of a difference in height between a child and an adult when the moving object is a person.

Next, in the normal setting process, a process of setting a plurality of movement candidate positions in a surrounding area separated from the movement candidate position by a standard separation distance 83 is performed (ST13). In the present embodiment, the movement candidate positions are circular positions with the moving object position as the center and the standard separation distance as the radius, and the target flight altitude areas set in ST12 are equally spaced. Set. FIG. 6 is an explanatory diagram for setting the movement candidate position, and is a view when a part of the flight space displayed by the voxel at time t is looked down from directly above. In the figure, a black voxel represented by reference numeral Bo is an occupied voxel, a voxel painted by parallel oblique lines (hatching) represented by reference numeral Bn is a neighboring voxel, and a white voxel represented by reference numeral Bf is It is a free voxel. In the example of FIG. 6, the position of the moving object M (center of gravity position Mg) is set as the reference position, and a plurality of movement candidate positions Pc are arranged on the circumference of the radius Lc that is a separation distance around the reference position Mg. Are installed at equal intervals. In the present embodiment, 72 movement candidate positions Pc are installed at intervals of 5 ° around the reference position Mg.

Next, in the normal setting process, a process of excluding the movement candidate position Pc located in the vicinity of the obstacle from the evaluation target when evaluating the movement target position described later is performed (ST14). That is, in order to prevent the autonomous flying robot 1 from coming into contact with an obstacle, the movement candidate position Pc at a position where the autonomous flying robot 1 interferes with the obstacle is excluded, and the movement candidate position Pc approaching the obstacle also has a risk of contact. Therefore, the process of excluding the movement candidate position Pc is performed. For example, as shown in FIG. 7 , from the evaluation targets described below, the movement candidate position Pc that is in a position that interferes with voxels other than the free voxel Bf (occupied voxel Bo, neighboring voxel Bn) is not set as the movement target position. exclude. Movement candidate position Pc represented in FIG. 7 is of after excluding the movement candidate position Pc that is interfering with the adjacent voxels Bn from the movement candidate position Pc, represented by FIG. 6, the migration candidate that is evaluated This is the position Pc. It is assumed that the proximity voxel Bn can be used as a movement path although it cannot be set as the movement target position. On the other hand, the occupied voxel Bo cannot be set as a movement target position and cannot be a movement path. Thus, by performing the process of excluding the movement candidate position Pc located in the vicinity of the obstacle from the evaluation target, the collision between the autonomous flying robot and the obstacle can be avoided, and all the movement candidate positions are evaluated. Since this can be omitted, the amount of calculation for evaluation can be reduced.

ここで、ｒ＝（ｘ，ｙ，ｚ）は自律飛行ロボット１の現在の自己位置、Ｐ_prev＝（ｘ_prev，ｙ_prev，ｚ_prev）は前回設定した移動目標位置を示し、α、β、σは距離に応じて減衰する値を調整するためのパラメータである。すなわち、数１において、（ｒ−Ｐ_c）が自己位置から各移動候補位置までの距離を表し、（Ｐ_prev−Ｐ_c）が前回設定した移動目標位置からの移動候補位置までの距離を表す。以上のようにして、移動目標位置設定処理では評価値Ｖを求め、評価値Ｖが最大となる移動候補位置Ｐｃを移動目標位置Ｐｏとして設定する。図７の例では、点線で囲んだ移動候補位置Ｐｃが移動目標位置Ｐｏとして設定されたことを表している。このように、移動目標位置Ｐｏが設定されると、後述するように設定された移動目標位置Ｐｏに向かうように移動経路が生成され、自律飛行ロボット１は当該移動目標位置Ｐｏに向かって移動していくこととなる。 Next, the target setting means 73 calculates the evaluation value of each movement candidate position Pc not excluded from the evaluation target, and sets the movement candidate position having the largest evaluation value as the movement target position (ST15). At this time, first, the evaluation value is increased as the distance from the current position of the autonomous flying robot 1 to each movement candidate position is smaller. Further, the evaluation value is increased as the distance from the previously set movement target position to the movement candidate position is smaller. A specific evaluation formula for obtaining the evaluation value V in the present embodiment is expressed by Equation 1.

Here, r = (x, y, z) represents the current self-position of the autonomous flying robot 1, P _prev = (x _prev , y _prev , z _prev ) represents the previously set movement target position, and α, β, σ is a parameter for adjusting a value that attenuates according to the distance. That is, in Equation 1, (r−P _c ) represents the distance from the own position to each movement candidate position, and (P _prev −P _c ) represents the distance from the previously set movement target position to the movement candidate position. . As described above, in the movement target position setting process, the evaluation value V is obtained, and the movement candidate position Pc that maximizes the evaluation value V is set as the movement target position Po. In the example of FIG. 7 , the movement candidate position Pc surrounded by the dotted line is set as the movement target position Po. Thus, when the movement target position Po is set, a movement path is generated so as to go to the movement target position Po set as described later, and the autonomous flying robot 1 moves toward the movement target position Po. It will be followed.

Subsequently, when it is determined in ST10 that the moving object position cannot be estimated (ST10-No), the target setting unit 73 performs search setting processing as processing for setting the moving target position when the moving object is lost (ST16). ~ ST21). In the search setting process, first, it is determined whether or not the target flight altitude previously set (immediately before) is 8 m or more (ST16).
When it is determined in ST16 that the previously set target flight altitude is 8 m or more (ST16-Yes), the process proceeds to ST18. On the other hand, when it is determined in ST16 that the previously set target flight altitude is not 8 m or more (ST16-No), the target flight altitude is 1 m higher than the current flight altitude (the z-axis coordinate of the self position). Processing to correct the value is performed (ST17). For example, when the current flight altitude is 3 m, in ST17, 3 + 1 = 4 m is set as the target flight altitude in the current process. In the above embodiment, the target flight altitude is set as “current flight altitude + 1” in ST17. However, the present invention is not limited to this, and the process of increasing the target flight altitude set last time (immediately before) by 1 m is performed. May be performed. For example, when the target flight altitude of 3 m calculated as the standard altitude 86 is set in the immediately preceding process, in ST17, 3 + 1 = 4 m is set as the target flight altitude in the current process. In any case, the target flight altitude set in ST17 is set as a high altitude position higher than the standard altitude 86.

Next, in the search setting process, a search separation that is a separation distance at which the reference position can be imaged from the target flight altitude (high altitude position) calculated in ST17 when the moving object is lost using the imaging condition information 84 stored in advance. Processing for calculating the distance is performed (ST18). In the present embodiment, it is assumed that the position coordinates (hereinafter referred to as “entrance position”) of the entrance / exit of the monitored building existing in the flight space are set as the reference position of the present invention. For example, it is assumed that a previously measured horizontal position having a height of 0 m is set in advance by an administrator or the like and stored in the storage unit 8 as the entrance / exit position. FIG. 5 is a diagram for explaining the calculation of the search separation distance, and is a schematic diagram when the autonomous flying robot 1 and the building A are viewed from the lateral direction in the flight space. In the figure, symbol E corresponds to the entrance / exit of the building A, symbol Eg corresponds to the entrance / exit position, symbol H ′ corresponds to the target flight altitude calculated in ST17, and symbol L ′ corresponds to the search separation distance. Here, when the depression angle of the imaging unit 3 is θ, the search separation distance L ′, which is the separation distance at which the entrance / exit position Eg of the building A can be imaged, can be calculated by L ′ = H ′ / tan θ. In the present embodiment, the entrance / exit position Eg is 0 m in height, but the height may be an arbitrary predetermined value D ′. In this case, 'a, L' search distance L = (H '- D' ) / can be calculated by tan .theta. In the present embodiment, the reference position is set as the entrance / exit position Eg. However, the present invention is not limited to this, and may be, for example, the position of a sensor that senses the presence of an intruder.

Next, in the search setting process, each process for setting the movement target position Po is performed using the calculated target flight altitude and the search separation distance L ′ (ST19 to ST21). FIG. 8 is a diagram for explaining the setting of the movement target position when the moving object is lost. As represented in the figure, as usual setting process even when the moving object Shisseki, from the set reference position at ST18 (entrance position Eg), separated by the search distance L 'which is set in ST 18 A plurality of movement candidate positions Pc are set in the position (ST19). Then, the movement candidate position Pc located in the vicinity of the obstacle is excluded from the evaluation target (ST20), and each movement candidate position Pc that is the evaluation target is evaluated by the equation 1, and the movement with the maximum evaluation value V is performed. Candidate position Pc is set as movement target position Po (ST21). As described above, since each process from ST19 to ST21 in the search setting process is performed in substantially the same manner as each process from ST13 to ST15 in the normal setting means, detailed description thereof is omitted here.

When the target setting unit 73 finishes the movement target position setting process, the movement path generation unit 74 uses the voxel information stored in the storage unit, the movement target position calculated by the target setting unit, and the position estimation unit 71. A movement path generation process for calculating the movement path of the autonomous flying robot is performed using the calculated self-position. Specifically, in the movement path generation process, first, the voxel information 82 is read from the storage unit, and the center of each of the free voxels Bf and the neighboring voxels Bn, which are divided movable spaces, is set as a node and adjacent to the node. A graph structure is generated with a line segment connecting nodes to be used as an edge. FIG. 9 is a diagram for explaining the graph structure, in which a part (27) of movable voxels (free voxels Bf or adjacent voxels Bn) in the flight space are cut out. In FIG. 9, each cube represented by the symbol B represents a free voxel Bf or a neighboring voxel Bn. Further, a sphere filled with black or hatching at the center of these voxels B is a node, and a line segment displayed with a dotted line connecting the nodes is an edge. After generating the graph structure for all movable voxels based on the voxel information 82 in this way, from the voxel node where the autonomous flying robot 1 exists to the voxel node where the movement target position Po exists. Generate a travel route. Although various route generation methods can be applied to the generation method of the movement route, in the present embodiment, the movement route is searched using the A * (Aster) route search method. At this time, the voxel cost stored as the voxel information 82 is used as a movement cost in the A * route search method. The data of the movement route generated by the movement route generation means 74 is set data of via points (x, y, z), and this information is temporarily stored in the storage unit 8.

  The flight control means 75 uses the travel route information calculated by the travel route generation means 74, the self-position estimated by the position estimation means 71, and the flight speed estimated by the speed estimation means 72 to control flight at each time. A speed command value that is a value is obtained, the motor 6 is controlled based on the speed command value, and the number of rotations of the rotor 2 is controlled, so that the autonomous flying robot 1 follows the movement path calculated by the movement path generation means 74. The route following control process is performed. The “movement control means” in the present invention corresponds to the movement path generation means 74 and the flight control means 75 in the present embodiment.

  In the route follow-up control process, first, a process of calculating the nearest position (hereinafter referred to as “local target”) to be targeted by the autonomous flying robot at each time is performed. FIG. 10 is a diagram for explaining the calculation of the local target. In calculating the local target, the flight control means 75 reads the movement route generated by the movement route generation means 74 from the storage unit 8, and passes through the waypoint Wp1 that the autonomous flying robot is aiming at at the current time and the previous passage. A straight line W connecting the two points with the point Wp0 is obtained. Then, the flight control means calculates intersections Lp ′ and Lp between the obtained straight line W and the sphere S centered on the self-position of the autonomous flying robot 1, and uses the intersection Lp close to the target via point Wp1 as a local target. Ask. In this way, by performing flight control 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. Will fly along the path of travel.

Next, in the path following control process, speed command values u _x , u _y , u _z , u _ψ are calculated for each direction of X, Y, Z, and yaw angle ψ so as to fly toward the calculated local target. Process. At this time, a speed command value is determined so that the difference between the current self position and the position of the local target is small. Specifically, the speed command value u = (u _x , u _y , u _z ) in the XYZ direction is estimated by the self position r = (x, y, z) obtained by the position estimation unit and the speed estimation unit. Using velocity v = (v _x , v _y , v _z ), it is obtained by PID control. When each speed command value in the XYZ-axis directions is u = (u _x , u _y , u _z ) and the local target is r ′ = (x, y, z), the speed command value is u = K _p (r '-R) + K _d · v + K _i · e is calculated. Here, K _p , K _d , and K _i are gains of PID control, respectively, and e = (e _x , e _y , e _z ) is an integrated value of errors. On the other hand, the speed command value u _ψ in the yaw angle direction includes the target angle of ψ ′, the attitude (angle) of the autonomous flying robot 1 estimated by the position estimating means 71, and the angular velocity estimated by the speed estimating means 72 of v _yaw. When obtained by the PD control as equation _{u ψ = K p (ψ'-} ψ) + K d · v ψ. In the present embodiment, in the normal state, the target angle ψ ′ is set to the direction of the moving object M, that is, the direction of the moving object position. When the moving object is lost, the target angle ψ ′ is set to the direction of the reference position, that is, the angle of the entrance / exit position set in advance.

  In this way, the control unit 7 sequentially repeats the processes in the position estimation unit 71, the speed estimation unit 72, the target setting unit 73, the movement route generation unit 74, and the flight control unit 75 described above. As a result, the autonomous flying robot 1 of the present embodiment sequentially updates the movement target position with respect to the position away from the moving object M by the separation distance, and sequentially updates the movement route each time, thereby moving the movement. It is possible to follow and fly while maintaining a predetermined separation distance from the object. Further, the autonomous flying robot 1 sets the target flight altitude at a high altitude position higher than the standard altitude 86 when the moving object is lost. As described above, even when the autonomous flying robot 1 loses sight of the moving object M, the range (field of view) that can be imaged by the imaging unit 3 is widened by performing flight control so as to increase the flight altitude, so It is possible to quickly find a moving object. When the moving object M is found from a high altitude position and the moving object position can be estimated, the follow-up flight can be resumed while maintaining a predetermined separation distance from the moving object M.

  Further, in normal times, the standard separation distance 83 is set to the separation distance Lc, and the posture control is performed so as to face the direction of the moving object M, thereby maintaining the separation distance Lc that is difficult to receive an attack from the moving object M. The moving object M can be imaged. Then, when the moving object is lost, a search separation distance at which the reference position can be imaged is calculated using the imaging condition information 84 (the depression angle θ), set to the separation distance Lc, and posture control is performed so as to face the direction of the reference position. By doing so, the horizontal position can be set so that the reference position (for example, the entrance / exit position) can be continuously imaged even when the moving object is moved up to a high altitude position.

  By the way, the present invention is not limited to the above-described embodiment, and may be implemented 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 to this.

  In the above-described embodiment, a predetermined position (entrance / exit position) set in advance is used as a reference position in the search setting process when the moving object is lost (ST19). However, the present invention is not limited to this, and a position obtained from the most recently estimated moving object position may be set as the reference position. For example, the moving object position that can be estimated most recently is temporarily stored in the storage unit 8, and the moving object position is set as the reference position. In general, when the moving object M being tracked is lost, the moving object M exists in the vicinity of the lost position. Therefore, the moving object M can be discovered more efficiently by setting the moving object position immediately before losing sight to the reference position and moving up to a high altitude position. In addition, the moving direction and moving speed of the moving object M are estimated from the time series information of the moving object position that can be estimated in the most recent period, and the moving object position at the current time is estimated using these pieces of information. The object position may be set as the reference position.

  In the above embodiment, in the search setting process when the moving object is lost, the target flight altitude is changed stepwise to a higher altitude position higher than the standard altitude 86 (ST17). However, the present invention is not limited to this, and a high altitude position higher than the preset standard altitude 86 (for example, a position having a height of 7 m) may be set as the target flight altitude when the moving object is lost.

  In the above embodiment, in the search setting process when the moving object is lost, the search separation distance is calculated in order to obtain a position where the reference position (entrance / entrance position) can be imaged (ST18), and the surroundings separated from the reference position by the search separation distance The movement candidate position is set at the position (ST19). However, the present invention is not limited to this, and the preset search separation distance (fixed value) is read without calculating the search separation distance in ST18, and the movement candidate position Pc is set in ST19 using the search separation distance. May be. Alternatively, when the moving object is lost, the search separation distance is not calculated in ST18, and the movement candidate position Pc is not set in ST19, and a high altitude higher than the standard altitude 86 set in ST17 is set. The movement target position Po may be set at a predetermined horizontal position (fixed value) in the flight space.

  In the above-described embodiment, in the normal setting process at the normal time, the position ratio at the total height of the moving object is stored in the storage unit 8 as the characteristic position 85, and the position of the characteristic part is calculated using the position ratio and the estimated height of the moving object M. The height D is estimated (ST11), and the standard altitude 86 (= H), which is the flight altitude capable of imaging the characteristic part, is calculated using the height D of the characteristic part, and the standard altitude 86 is set as the target flight altitude. (ST12). However, the present invention is not limited to this, and a predetermined fixed value is stored as the feature position 85 (eg, 1.6 m), and the fixed value is characterized in ST12 without estimating the height of the moving object M in ST11. You may utilize as the height D of a site | part. Alternatively, a predetermined fixed value may be stored as the standard altitude 86 (for example, 2.5 m), the process of ST11 may be omitted, and the fixed value may be set as the target flight altitude at ST12.

  In the above embodiment, the standard separation distance 83 is set as a distance in the horizontal plane that should be maintained between the autonomous flying robot 1 and the moving object. It may be set as a straight line distance to be maintained. For example, the linear distance from the optical center of the imaging unit 3 to the feature position of the moving object may be set in the storage unit 8. Similarly, the search separation distance is not limited to being calculated as a distance on a horizontal plane as in the above-described embodiment, but may be calculated as a linear distance at which a reference position can be imaged from a high altitude position.

In the above embodiment, in order to calculate the speed command value by the flight control means 75 , the speed estimation means 72 calculates the self speed. However, the present invention is not limited to this, and the speed command value may be calculated without the speed estimation means 72 and without calculating the self speed. In this case, it is possible to follow the flight by a simple process that follow-up performance of the mobile object M is poor. In the above embodiment, the speed estimation unit 72 calculates the self speed based on the self position. However, the present invention is not limited to this. A sensor that uses a sensor such as an IMU (Internal Measurement Unit) or a camera that captures an image directly below the autonomous flying robot 1 is installed, and is known based on an image captured from the camera. The self velocity may be calculated using an optical flow method.

  In the above embodiment, the position detection unit 71 estimates the self position from the output of the laser sensor using a laser scanner as the distance detection sensor 4. However, the present invention is not limited to this, and the self-position may be estimated by a known conventional technique using another sensor such as a GPS, a gyro sensor, an electronic compass, and an atmospheric pressure sensor instead of the laser scanner.

  In the above embodiment, the control unit 7 performs a series of processing of position estimation processing (self-position estimation processing, moving object position estimation processing), speed estimation processing, movement target position setting processing, movement route generation processing, and route tracking control processing. ing. However, the present invention is not limited to this, and a control PC (not shown) may be prepared and the PC may perform a series of these processes. That is, the autonomous flying robot 1 transmits the output of the laser scanner of the distance detection sensor 4 and the captured image acquired by the imaging unit 3 to the PC by wireless communication via the communication unit 9. Then, the PC performs position estimation processing, speed estimation processing, movement target position setting processing, movement route generation processing, and route follow-up control processing, and the speed command value obtained in the route follow-up control processing is obtained by autonomous communication by wireless communication. 1 to send. Then, the autonomous flying robot 1 flies to a target position by controlling the number of revolutions of the motor 6 based on the speed command value received via the communication unit 9. As described above, by sharing the above-described series of processes using the external PC, it is possible to reduce the CPU processing load of the autonomous flying robot 1 and to suppress the battery consumption.

DESCRIPTION OF SYMBOLS 1 ... Autonomous flight robot 2 ... Rotor 3 ... Imaging part 4 ... Distance detection sensor 5 ... Mirror 6 ... Motor 7 ... Control part 71 ... Position estimation means 72. ..Speed estimation means 73 ... Target setting means 74 ... Movement path generation means 75 ... Flight control means 8 ... Storage unit 81 ... 2D point information 82 ... Voxel information 83 ... Standard separation distance 84 ... Imaging condition information 85 ... Characteristic position 86 ... Standard altitude 87 ... Various parameters 9 ... Communication part L ... Standard separation distance L '... Search separation distance M ... Moving object Mg ... Center of gravity of moving object G ... Ground A ... Building E ... Entrance / exit Eg ... Entrance / exit position Bo ... Occupied voxel Bn ... Proximity voxel Bf ... Free voxel Pc ... Movement candidate position Po ...・ Target position

Claims (5)

An autonomous flying robot that follows a moving object in flight space and images the moving object with an imaging unit,
A storage unit;
Position estimation means for performing processing for estimating the self-position of the autonomous flying robot, processing for estimating the moving object position of the moving object based on the captured image acquired by the imaging unit and the self-position, and
A normal setting process in which when the moving object position can be estimated by the position estimating means, the moving target position is set to a position at a predetermined standard altitude and a predetermined standard separation distance from the moving object position; A target setting means for performing a search setting process for setting a movement target position at a high altitude higher than the standard altitude when the position estimating means cannot estimate the moving object position;
Movement control means for performing a process of controlling to move from the self position to the movement target position,
An autonomous flying robot characterized in that it follows and flies by sequentially repeating each of the above processes. The storage unit stores imaging condition information indicating a field of view of the imaging unit and a predetermined reference position in the flight space,
The position estimating means further estimates the attitude of the autonomous flying robot,
The target setting means calculates a search separation distance at which the reference position can be imaged from the high altitude position using the imaging condition information as the search setting processing, and a position separated from the reference position by the search separation distance Set the movement target position to
The movement control unit controls the posture so that the imaging unit faces the direction of the moving object position when the position estimating unit can estimate the moving object position, and the position estimating unit controls the moving object position. The autonomous flying robot according to claim 1, wherein the posture is controlled so that the imaging unit faces a direction of the reference position when it cannot be estimated.   The autonomous flight robot according to claim 2, wherein the movement control unit performs a process of obtaining the reference position from the moving object position estimated in the most recent period and storing the reference position in the storage unit as the search setting process. The storage unit further stores a feature position indicating a height position of a feature part of the moving object,
The target setting means calculates, as the normal setting process, an altitude at which a feature part of the moving object can be imaged from a position separated from the moving object position by the standard separation distance using the imaging condition information and the feature position. The autonomous flying robot according to claim 1, wherein the altitude is stored in the storage unit as the standard altitude. The target setting means extracts an image area of the moving object from the captured image, estimates a height of the moving object using the image area, the imaging condition information, and the self-position, and determines from the height The autonomous flying robot according to claim 4, wherein the characteristic position is estimated and stored in the storage unit.

Images