JP6029446B2 - Autonomous flying robot - Google Patents

Description

  The present invention relates to an autonomous flight robot that flies following a moving object such as an intruder and acquires an evidence image of the moving object such as an intruder.

  Conventionally, an autonomous mobile robot that autonomously moves and acquires an image of an intruder who has entered a monitoring area has been proposed. For example, in Patent Document 1, the intruder is imaged by rotating in the direction of the detected sensor, or the intruder is recognized based on the position of the face area detected from the captured image. An autonomous mobile robot that tracks and images an intruder by rotating itself in a direction is disclosed.

Japanese Patent Laid-Open No. 2006-024128

  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. When such an autonomous flying robot is used to monitor an intruder, even if the autonomous flying robot approaches a position at a predetermined distance from the intruder, the evidence image of the intruder cannot be acquired properly depending on the flight altitude. There was a problem. Therefore, the present invention appropriately determines evidence images such as an intruder's face image by determining the flight altitude of the autonomous flight robot according to the imaging direction of the camera installed in the autonomous flight robot and the distance from the intruder. The purpose is to be able to shoot.

In order to solve such a problem, an autonomous flying robot that follows a moving object while keeping a predetermined separation distance and images the moving object with an imaging unit, imaging condition information indicating a field of view of the imaging unit; A storage unit that stores a distance in a horizontal plane from the moving object and a feature position indicating a height position of a feature part of the moving object; a moving object position of the moving object; and a self-position and posture of the autonomous flying robot A position estimation unit that performs a process of estimating the movement path, a route search unit that performs a process of calculating a movement path of the autonomous flying robot, a control to move along the movement path, and the imaging unit moves the movement Flight control means for performing a process for controlling the posture so as to face the direction of the object position, and performing the follow-up flight by sequentially repeating each of the processes, the route search means A target flight altitude calculating means for calculating a target flight altitude that is an altitude capable of imaging a characteristic part of the moving object according to the separation distance, the imaging condition information, and the characteristic position; and a position of the target flight altitude. a movement target position setting means for setting a moving target position in the separation distance away from the moving object position, a movement route calculating means for calculating the movement path to move to the movement target position from the self-position An autonomous flight robot is provided.

  With this configuration, the autonomous flight robot of the present invention can follow and fly a moving object such as an intruder while maintaining a predetermined separation distance. Furthermore, the movement is performed by obtaining a flight altitude capable of imaging a characteristic part such as the face of the intruder from the imaging distance information such as the imaging distance (such as a depression angle) of the imaging unit and following the flight altitude. It is possible to follow and fly while appropriately imaging the characteristic part of the object.

Further, as a preferred aspect of the present invention, the target flight altitude calculating means extracts an image area of the moving object from a captured image captured by the imaging unit , and extracts the image area, the imaging condition information, and self-position information. It is assumed that the height of the moving object is estimated, the feature position is estimated from the height, and is 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.

  Moreover, as a preferred aspect of the present invention, the route search means further includes a movement candidate position setting means for setting a plurality of movement candidate positions at peripheral positions separated from the moving object position by the separation distance, and the movement The target position setting means evaluates each of the movement candidate positions and sets one of the movement candidate positions as a movement target position, estimates a front direction of the moving object, and Among them, it is evaluated that the movement candidate position located in the front direction is easily set as the movement target position.

  With this configuration, the autonomous flying robot of the present invention can easily follow and fly at a position in the front direction of the moving object. Thereby, it is possible to acquire a captured image with high evidence ability such as a front image of a characteristic part of a moving object, for example, a front face image of an intruder.

  As described above, the autonomous flying robot of the present invention appropriately captures an evidence image such as an intruder's face image by determining its own flight altitude according to the imaging direction (for example, depression angle) of the installed camera. However, it is possible to follow and fly with respect to the intruder.

Overview of autonomous flying robot Functional block diagram of autonomous flying robot Functional block diagram of route search means Illustration of target altitude calculation Explanatory drawing about the setting of the movement candidate position Explanatory diagram for setting the movement target position Explanatory diagram of the graph structure in the generation of travel paths Illustration of local target calculation Explanatory drawing of movement of moving object in front direction

  Hereinafter, an embodiment of an autonomous flight robot that follows a flying object while keeping a predetermined relative distance and captures an image of the moving object will be described in detail with reference to the accompanying drawings. The autonomous flying robot of the present invention can be similarly applied to a moving object 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. In addition, it is also used to estimate the flight altitude using distance information measured by reflecting a part of the laser emitted from the laser scanner by the mirror 5 toward the ground.

  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 and voxel information 82 corresponding to the “obstacle map” of the present invention, a separation distance 83, imaging condition information 84, a characteristic position 85, and settings used for each process of the control unit 7. Various parameters 86 such as values and thresholds, output values from each sensor and the like, captured images, 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 it 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 separation distance 83 is a relative distance to be maintained on the horizontal plane between the autonomous flying robot 1 and the moving object when following the moving object, and is a value set in advance by an administrator of the autonomous flying robot 1 or the like. When a predetermined moving object is monitored using the autonomous flying robot 1, it is necessary to approach 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 a separation distance 83 in advance at a distance that is difficult to receive an attack from the moving object, while maintaining the separation distance. Make a follow-up flight. In this embodiment, it is assumed that the 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 control unit 7 is configured by a computer including a CPU and the like, and as a series of processes for performing position estimation processing (self-position estimation processing, moving object position estimation processing), speed estimation processing, route search processing, and route tracking control processing, Position estimation means 71, speed estimation means 72, route search means 73, and flight control means 74 are included.

  The position estimation means 71 includes a self-position estimation process for estimating the current 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 , Referred to as “moving object position”).

  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, A process of 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. In estimating the x, y, and yaw angles ψ of the self-position, first, a point set as a two-dimensional map of the flight space corresponding to the flight altitude z is read 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 region. 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.

  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 74 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 route search unit 73 performs a process of calculating the movement route 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. FIG. 3 shows a functional block diagram of the route search means 73. As shown in FIG. 3, the route search means 73 evaluates the target flight altitude calculation means 734 for calculating the target flight altitude, the movement candidate position setting means 731 for setting the movement candidate position, and the plurality of movement candidate positions. A moving target position setting unit 732 that sets one moving target position from among them, and a moving path generation unit 733 that generates a moving path from the self position to the moving target position are configured. The details of the processing in each means constituting the route search means 73 will be described below.

  The target flight altitude calculation means 734 uses the separation distance 83, the imaging condition information 84, and the characteristic position 85 stored in the storage unit 8 to obtain a target flight altitude that is a flight altitude that the autonomous flying robot 1 should move. Perform altitude calculation processing.

In the target flight altitude calculation process, first, the height of the moving object is estimated, and the process of obtaining the height of the characteristic part using the estimated height information of the moving object is performed. 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. Can be requested.

  Next, in the target flight altitude calculation process, a process for calculating the target flight altitude is performed using the calculated height of the characteristic part or the like. FIG. 4 is a diagram illustrating the calculation of the target flight altitude, 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 portion corresponds to H, and the separation distance 83 corresponds to L. Here, when the depression angle of the imaging unit 3 is θ, a target flight altitude that is an altitude at which the face of the person as the moving object M can be imaged can be calculated by H + D = H + L · tan θ.

  As described above, by calculating the target flight altitude according to the feature position 85, the separation distance 83, and the imaging condition information 84 (the depression angle θ), the feature part (for example, the head such as the face) of the moving object M is imaged. It is possible to follow the moving object M while maintaining a predetermined separation distance 83 at a flight altitude capable of the following. In addition, by updating the characteristic position 85 according to the estimated height of the moving object M from the ground G, the target flight altitude can be set at a position where the characteristic part can be imaged according to the height of the moving object M. it can. For example, in consideration of the difference in height between a child and an adult when the moving object is a person, the target flight altitude can be set to a height at which the head can be imaged.

  When the process of calculating the target flight altitude is completed by the target flight altitude calculating means 734, the movement candidate position setting means 731 performs a process of setting a plurality of movement candidate positions at positions around the moving object. In the present embodiment, the movement candidate position is a circular position centered on the moving object M and having a radius of the separation distance 83 (= L) stored in the storage unit 8, and the target flight altitude calculation means 734 The calculated target flight altitude (= H + D) is set at equal intervals. FIG. 5 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 is looked down from directly above. In the figure, a black voxel represented by reference character Bo is an occupied voxel, a voxel painted by parallel oblique lines (hatching) represented by reference symbol Bn is a neighboring voxel, and a white voxel represented by reference symbol Bf is It is a free voxel. As shown in FIG. 5, a plurality of movement candidate positions Pc are arranged at equal intervals on the circumference of the radius L around the center of gravity position Mg of the moving object M. In the present embodiment, 72 movement candidate positions Pc are set at positions around the moving object M every 5 °.

  When the movement candidate position setting unit 731 finishes setting the movement candidate position Pc, the movement target position setting unit 732 evaluates the plurality of set movement candidate positions Pc and sets one of them as the movement target position. A movement target position setting process is performed. FIG. 6 is an explanatory diagram for setting the movement target position, and is a view when a part of the flight space displayed by the voxel is looked down from directly above as in FIG. In the movement target position setting process, first, 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 it interferes with the obstacle is excluded, and it is also in contact with the obstacle. Therefore, the process of excluding the movement candidate position Pc existing near the obstacle is performed. Specifically, as illustrated in FIG. 6, the following description is made so that the movement candidate position Pc at a position that interferes with a voxel other than the free voxel Bf (occupied voxel Bo, adjacent voxel Bn) is not set as the movement target position. Exclude from evaluation. The movement candidate position Pc shown in FIG. 6 is obtained by excluding the movement candidate position Pc that interferes with the adjacent voxel Bn from the movement candidate position Pc shown in FIG. 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.

ここで、ｒ＝（ｘ，ｙ，ｚ）は自律飛行ロボット１の現在の自己位置、Ｐ_prev＝（ｘ_prev，ｙ_prev，ｚ_prev）は前回設定した移動目標位置を示し、α、β、σは距離に応じて減衰する値を調整するためのパラメータである。図６では、点線で囲んだ移動候補位置が移動目標位置Ｐｏとして設定されたことを表している。このように、移動目標位置Ｐｏが設定されると、後述するように設定された移動目標位置Ｐｏに向かうように移動経路が生成され、自律飛行ロボット１は当該移動目標位置Ｐｏに向かって移動することとなる。 Next, in the movement target position setting process, a process for setting each movement candidate position by evaluating each movement candidate position Pc not excluded from the evaluation target is performed. In the present embodiment, an evaluation value of each movement candidate position Pc that is an evaluation target is obtained, and evaluation is performed such that the movement candidate position having the largest evaluation value is set as the movement target position. At this time, first, the evaluation value is set higher 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 set higher 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. FIG. 6 shows that the movement candidate position 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.

The movement path generation means uses the voxel information stored in the storage unit, the movement target position calculated by the movement target position setting means, and the self position calculated by the position estimation means to determine the movement path of the autonomous flying robot. The movement route generation process to calculate is performed. Specifically, in the route generation process, first, the voxel information 82 is read from the storage unit, and the center of each voxel of the free voxel Bf and the neighboring voxel Bn, which is the divided movable space, is set as a node and adjacent to the node. A graph structure is generated with a line segment connecting nodes as an edge. FIG. 7 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. 7, 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. Data of the generated moving route by the route searching means 73 is a set data of the transit point (x, y, z), the information is temporarily stored in the storage unit 8.

  The flight control means 74 uses the travel route information calculated by the route search means 73, the self-position estimated by the position estimation means 71, and the flight speed estimated by the speed estimation means 72, and the flight control value at each time. The autonomous flying robot 1 flies along the movement route calculated by the route searching means 73 by obtaining the velocity command value, controlling the motor 6 based on the velocity command value, and controlling the rotation speed of the rotor 2. Route tracking control processing is performed.

  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. 8 is a diagram for explaining the calculation of the local target. In calculating the local target, the flight control means 74 reads the movement route generated by the route search means 73 from the storage unit 8, and the waypoint Wp1 that the autonomous flying robot is aiming at at the current time and the waypoint that has passed the previous time. A straight line W connecting two points with 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, the speed command values u _x , u _y , u _z , u _ψ are calculated for each of the X, Y, Z, and yaw angles so as to fly toward the calculated local target. I do. 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, the target angle ψ ′ is an angle that faces the direction of the moving object M, that is, the direction of the moving object position.

  In this manner, the control unit 7 sequentially repeats the processes in the position estimation unit 71, speed estimation unit 72, route search unit 73, and flight control unit 74 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 of the moving object M and the separation distance, and sequentially updates the movement route each time. The follow-up flight can be performed at a flight altitude at which the moving object M can be appropriately imaged while maintaining a predetermined separation distance.

  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 embodiment, the position ratio of the moving object at the total height is stored in the storage unit 8 as the feature position 85, and the height H of the feature part is estimated using the position ratio and the estimated height of the moving object M. . However, the present invention is not limited to this, and the feature position 85 is stored as a predetermined fixed value (for example, 1.6 m), and the fixed value is used as the height H of the feature part without estimating the height of the moving object M. May be. As a result, for example, the target flight altitude is calculated without considering the height of an intruder or the like, so that the target flight altitude can be calculated with simple processing, although the accuracy of capturing the characteristic part of the moving object is reduced. .

  In the above embodiment, when each movement candidate position is evaluated by the movement target position setting means 732, the distance from the current position of the autonomous flying robot 1 to each movement candidate position Pc and the movement candidate from the previously set movement target position. Evaluation was made using the distance to the position Pc. However, the present invention is not limited to this, the front direction of the moving object M is estimated from the moving direction of the moving object M, and the evaluation value V increases as the movement candidate position Pc is located in the front direction. You may evaluate so that the position Pc may be easily set as a movement target position. FIG. 9 is a diagram illustrating setting of the movement target position in the embodiment. In FIG. 9, the symbol M0 is the position of the moving object at time t, and the symbol M1 is the position of the moving object at time t + 1. In this case, the moving direction of the moving object M is the direction represented by the symbol mv. Therefore, the direction is estimated as the front direction of the moving object. Then, when the front direction is 0 °, it is expressed by Equation 1 so that the evaluation value for the movement candidate position Pc (movement candidate position represented by a white circle in FIG. 9) within the range of ± 30 ° is high. A new evaluation value V is calculated by multiplying the evaluation value V obtained by the above equation by a weighting coefficient. At this time, it is preferable to multiply the weighting coefficient having a larger value as the movement candidate position is closer to 0 ° which is the front direction. By setting the movement target position using the new evaluation value V thus obtained, the movement candidate position Pc located in the front direction of the moving object M is set as the movement target position from among the movement candidate positions Pc. It becomes easy to be done. Therefore, since the autonomous flying robot 1 is easily positioned in the front direction of the moving object M, it is possible to easily acquire a captured image (for example, a face image) of the front of the moving object M, and acquire a captured image with high evidence ability. While following the moving object M, it is possible to fly. Note that the front direction of the moving object M is not limited to the method of estimating from the moving direction of the moving object M as described above, but the direction of the moving object M is estimated from the captured image acquired by the imaging unit 3. The front direction may be estimated. For example, when the moving object M is a person, the direction of the face is estimated using conventional techniques disclosed in Japanese Patent Application Laid-Open No. 2006-146551 and Japanese Patent Application Laid-Open No. 2007-229814, and the direction is defined as the front direction. It may be estimated. Or you may estimate the direction of the moving object M based on the recognized joint position using the prior art currently disclosed by Unexamined-Japanese-Patent No. 2008-234404.

In the above embodiment, the speed estimation means 72 calculates the self-speed so that the flight control means 74 calculates the speed command value. 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-described embodiment, the moving object position is estimated by analyzing the captured image acquired by the imaging unit 3. However, the present invention is not limited to this, and a distance image sensor is separately mounted on the imaging unit 3, a moving object is extracted by a known moving object extraction technique using the distance image acquired from the distance image sensor, and extracted movement The moving object position may be estimated from the distance value between the object and the autonomous flying robot 1. Alternatively, the moving object position may be estimated using a laser scanner mounted as the distance detection sensor 4. Alternatively, a moving object position may be detected using a fixed laser scanner separately installed on the ground or a wall surface, and the moving object position may be notified to the autonomous flying robot 1 via the communication unit 9.

  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, route search processing, and route tracking control processing. 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. The PC performs position estimation processing, speed estimation processing, route search processing, and route tracking control processing, and transmits the speed command value obtained in the route tracking control processing to the autonomous flying robot 1 by wireless communication. 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.

  In the above embodiment, the imaging condition information 84 is also a constant value, assuming that the installation angle of the imaging unit 3 (the depression angle θ that is the imaging direction) is fixed at the installation position. However, the present invention is not limited to this, and a camera control device that can freely change the imaging direction of the imaging unit 3 may be installed to control the imaging direction of the imaging unit 3. In this case, information on the imaging direction is acquired from the camera control device, and the imaging condition information 84 in the storage unit 8 is updated.

DESCRIPTION OF SYMBOLS 1 ... Autonomous flight robot 2 ... Rotor 3 ... Imaging part 4 ... Distance detection sensor 5 ... Mirror 6 ... Motor 7 ... Control part 8 ... Memory | storage part 81 ... 2D point information 82 ... voxel information 83 ... separation distance 84 ... imaging condition information 85 ... feature position 86 ... various parameters 71 ... position estimation means 72 ... speed estimation means 73 ... Route search means 731 ... Movement candidate position setting means 732 ... Movement target position setting means 733 ... Movement route generation means 734 ... Target flight altitude calculation means 74 ... Flight control means 9.・ ・ Communication unit M ・ ・ ・ moving object Mg ・ ・ ・ center of gravity of moving object G ・ ・ ・ ground Bo ・ ・ ・ occupied voxel Bn ・ ・ ・ proximity voxel Bf ・ ・ ・ free voxel Pc ・ ・ ・ moving candidate position Po ・..Moving target position

Claims (3)

An autonomous flying robot that follows a moving object while keeping a predetermined separation distance and images the moving object with an imaging unit,
A storage unit that stores imaging condition information indicating a field of view of the imaging unit, a separation distance on a horizontal plane from the moving object, and a feature position indicating a height position of a feature part of the moving object;
Position estimating means for performing processing for estimating the moving object position of the moving object and the self-position and posture of the autonomous flying robot;
Route search means for performing a process of calculating a movement route of the autonomous flying robot;
Flight control means for controlling to move along the moving path and controlling the posture so that the imaging unit faces the direction of the moving object position, and sequentially repeats the respective processes. It is a follow-up flight by
The route search means includes
A target flight altitude calculating means for calculating a target flight altitude that is an altitude at which a characteristic part of the moving object can be imaged according to the separation distance, the imaging condition information, and the characteristic position;
A movement target position setting means for setting a moving target position in a position spaced the distance from the moving object position an the target flying height of the position,
A movement path calculation means for calculating the movement path so as to move from the self position to the movement target position;
An autonomous flying robot characterized by comprising: The target flight altitude calculating means extracts an image area of the moving object from a captured image captured by the imaging unit , and calculates the height of the moving object using the image area, the imaging condition information, and the self-position information. The autonomous flying robot according to claim 1, wherein the autonomous flying robot estimates and stores the characteristic position from the height in the storage unit. The route search means further includes movement candidate position setting means for setting a plurality of movement candidate positions at peripheral positions separated from the moving object position by the separation distance,
The movement target position setting means evaluates each movement candidate position and sets one of the movement candidate positions as a movement target position, estimates the front direction of the moving object, and moves the movement The autonomous flying robot according to claim 1, wherein the candidate flight position is evaluated such that a movement candidate position located in the front direction is easily set as a movement target position.

Images