JP6014485B2 - 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 while maintaining a predetermined distance, and more particularly to an autonomous flying robot that flies so as not to move to a position outside a predetermined movable space.

  Conventionally, an autonomous mobile robot that moves while following an object to be followed autonomously has been proposed. For example, in Patent Document 1, a tracking target is recognized based on distance data from a distance measuring sensor and a camera image, a relative distance between the tracking target and the tracking target is calculated, and the tracking target is changed according to a change in relative position. On the other hand, an autonomous mobile robot that controls movement so as to follow and move is disclosed.

JP 2004-126802 A

  However, the above prior art does not consider the range in which the autonomous mobile robot can move following. For this reason, as long as the autonomous mobile robot according to the related art recognizes the tracking target, the autonomous mobile robot continues to move following the tracking target. By the way, if an autonomous mobile robot is moved to a public space, there is a risk of danger to third parties such as passers-by and passing vehicles. It is necessary to consider limiting to a limited movable space such as inside. In particular, when an autonomous mobile robot is an autonomous flying robot that moves freely in space, there is a great need for safety measures because damage to third parties during a crash or contact is great. Accordingly, an object of the present invention is to make a follow-up movement with respect to a follow-up target so that the autonomous flying robot does not move out of a predetermined movable space.

  In order to solve such a problem, in an autonomous flying robot that follows and keeps a predetermined separation distance from a moving object in a flight space, the flight representing a movable space in the flight space and an obstacle in the flight space A storage unit storing a spatial map; position estimating means for performing a process of estimating a moving object position of the moving object and a self position of the autonomous flying robot; and a surrounding position separated from the moving object position by the separation distance. Movement candidate position setting means for setting a plurality of movement candidate positions; movement target position setting means for performing processing for setting one of the movement candidate positions as a movement target position from the result of evaluating each movement candidate position; Movement control means for performing processing for controlling movement from the self position to the movement target position, and by sequentially repeating each of the processing, The movement target position setting means is an area in the movable space when the self position is located in the movable space from the flight space map and the self position, and There is provided an autonomous flying robot characterized in that the evaluation is performed only for the movement candidate positions that are not in the vicinity of an obstacle.

  With this configuration, the movement target position setting means of the present invention has a plurality of movement candidates installed at surrounding positions separated from the moving object position by a predetermined separation distance when the self position is located in the movable space. In the position, a movement candidate position that is in the movable space and is not in the area near the obstacle (the area near the obstacle and the obstacle) is evaluated, and one of the movement candidate positions is determined from the evaluation result. Set to. That is, since the movement candidate position located outside the movable space and in the vicinity of the obstacle is not an evaluation target, the movement candidate position is not set as the movement target position. Therefore, the autonomous flying robot can follow the moving object while keeping a predetermined separation distance so as not to move toward the position outside the movable space or near the obstacle. Thereby, it is possible to avoid contact with third parties existing outside the movable space such as outside the site as much as possible, and it is possible to ensure safety for the third parties.

  Further, as a preferred aspect of the present invention, the movement target position setting means is configured such that, when the self position is located outside the movable space, the obstacle in the movable space regardless of the movement candidate position. It is assumed that the movement target position is set to a return position that is a position in a region where no is present and is closest to the self position.

  With such a configuration, the movement target position setting means of the present invention allows the movement from the self position without evaluating the movement candidate position set by the movement candidate position setting means when the self position is located outside the movable space. A position where no obstacle exists in the nearest movable space (return position) is set as a movement target position. As a result, the autonomous flying robot performs movement control to return to the movable space in the shortest distance even if it is swept out of the movable space due to sudden disturbance such as a gust of wind. , It is possible to ensure the safety of a third party existing outside the movable space such as outside the site.

  Further, as a preferred aspect of the present invention, the movement target position setting means sets the movement target position to an emergency landing position that is the return position and a position at a predetermined altitude, and the movement control means Landing control for gradually lowering the flight altitude is performed when the emergency landing position is set by the movement target position setting means.

  With this configuration, the movement target position setting means is, for example, a position where there is no obstacle in the movable space closest to the self position (return position), and a low position where emergency landing is possible (emergency landing position) Set the movement target position to. As a result, even if it is swept out of the movable space due to a sudden disturbance such as a gust of wind, and even if stable flight is difficult due to continuous strong wind, Movement control can be performed so as to land toward a safe position (movable space in the site or the like), and safety for a third party existing outside the movable space such as outside the site can be ensured.

  Further, as a preferred aspect of the present invention, the movement target position setting means performs skip control to skip the process of setting the movement target position when the position estimation means cannot estimate the self position on a horizontal plane, The movement control means performs landing control that gradually lowers the flight altitude when the skip control is performed by the movement target position setting means.

  With this configuration, when the self-position on the horizontal plane cannot be estimated by the position estimation unit of the present invention, the movement is controlled to land on the spot without setting the movement target position. That is, in a situation where the self-position cannot be estimated, the robot does not move until the self-position can be estimated, but landes promptly. As a result, it is possible to prevent the autonomous flying robot from leaving the movable space and flying freely beyond the non-movable space.

  Further, as a preferred aspect of the present invention, a communication unit that communicates with an external device is further included, and the movement control means gradually lowers the flight altitude when communication with the external device becomes impossible at the communication unit. Landing control shall be performed.

  With such a configuration, it is possible to maintain safety for a third party by performing movement control so as to land in an abnormal state where communication with an external device cannot be performed so that autonomous flight is not continued.

Moreover, as a preferable aspect of the present invention, it is further provided with a notification unit that issues a warning to the surroundings by sound or light when the landing control is performed by the movement control means.

  With such a configuration, a security guard or the like who has rushed to cope can find the landing place of the autonomous flying robot at an early stage, and can detect an abnormal state at an early stage.

  As described above, the autonomous flying robot of the present invention can ensure safety for a third party by following and moving so that the autonomous flying robot does not move out of a predetermined movable space.

Overview of autonomous flying robot Functional block diagram of autonomous flying robot Functional block diagram of route search means Explanatory drawing about the setting of the movement candidate position Flow chart of moving target position setting process Explanatory diagram for setting the movement target position Explanatory diagram for setting the emergency landing position Explanatory diagram of the graph structure in the generation of travel paths Illustration of local target calculation

  In the following, an image of an intruder as a moving object is captured and the following flight is performed while maintaining a predetermined relative distance. In particular, even when the intruder approaches to attack or capture, it is avoided appropriately. An embodiment of an autonomous mobile robot capable of flying like this (hereinafter referred to as “autonomous flying robot”) will be described in detail with reference to the accompanying drawings. The autonomous flying 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. 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 “flight space map” of the present invention, a separation distance 83, various parameters 84 such as setting values and threshold values used for each processing of the control unit 7, , Output values from each sensor, 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 a predetermined movable space in the flight space, voxels located in a space outside the movable space (non-movable space) are set as occupied voxels. Further, similarly to the case of an obstacle, a voxel located in a space existing near a non-movable space (occupied voxel) is also set as “neighboring voxel”, and the voxel cost value is calculated in the same manner.

  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 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 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. 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 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 is a movement candidate position setting means 731 for setting a movement candidate position, and a movement target position for evaluating a plurality of movement candidate positions and setting one movement target position among them. The setting unit 732 and the movement route generation unit 733 that generates a movement route from the self position to the movement target position are configured. The details of the processing in each means constituting the route search means 73 will be described below.

  The movement candidate position setting unit 731 performs a process of setting a plurality of movement candidate positions at surrounding positions separated by a separation distance 83 from the moving object position. In this 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 a target predetermined by the administrator or the like. In the region of the flight altitude (for example, 2 m), the distance is set so as to be equally spaced. FIG. 4 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. 4, 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 installed at positions around the moving object M every 5 °. The occupied voxel occupying the right side of FIG. 4 is a non-movable space of the autonomous flying robot 1, and in the real world space, it is a space corresponding to a public space (for example, a public road) outside the site (movable space). . In FIG. 4, it is assumed that the voxel space is expressed three-dimensionally although the voxel space is expressed two-dimensionally because the flight space is looked down from directly above.

  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. 5 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 self-position has been estimated by the self-position estimation process in the position estimation means 71 (ST10). As described above, in the self-position estimation process in the present embodiment, the self-position is estimated by matching the input point set that is the output of the distance detection sensor 4 (laser scanner) and the point set of the 2D point information 81. During this matching, a score corresponding to the Euclidean distance between the two point sets is obtained (the score becomes lower as the distance between the point sets becomes larger), and when the score is smaller than a predetermined threshold, self It is assumed that the position cannot be estimated. In the present embodiment, it is determined whether or not the self-position can be estimated from the score, but the present invention is not limited to this. For example, the points of the input point set that are significantly different in position from the point set of the 2D point information 81 are excluded from the matching process as “outliers”. A method may be used in which it is assumed that cannot be estimated.

  When it is determined in ST10 that the self-position has been estimated (ST10-Yes), it is determined whether or not the estimated self-position is located in the movable space (ST11). In this embodiment, when the estimated self position is at the position of the occupied voxel, it is determined that the self position is not located in the movable space. In other words, in the present embodiment, the occupied voxel represents either an obstacle or a non-movable space, so that the self-position is located at the position of the occupied voxel is a space other than the obstacle. It is considered as an immovable space.

  When it is determined in ST11 that the estimated self-position is located in the movable space (ST11-Yes), a process of setting a movement candidate position to be evaluated is performed (ST12). In the present embodiment, the movement candidate position Pc to be evaluated is set by excluding the movement candidate position Pc located in the vicinity of the obstacle from the evaluation target. That is, in order to prevent the autonomous flying robot 1 from coming into contact with the obstacle, the movement candidate position Pc at a position where the autonomous flying robot 1 interferes with the obstacle is excluded, and the risk of contact increases due to approaching the obstacle. A process of excluding the movement candidate position Pc existing near the obstacle 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 above just like FIG. As shown in FIG. 6, 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 excluded from the evaluation target described below so as not to be set as the movement target position. To do. The movement candidate position Pc shown in FIG. 6 is obtained after the movement candidate position Pc that interferes with the occupied voxel Bo or the neighboring voxel Bn is excluded from the movement candidate position Pc shown in FIG. This is the movement candidate position Pc. When the self position is located in the movable space, the adjacent voxel Bn cannot be set as the movement target position but can be set as the movement path, and the occupied voxel Bo cannot be set as the movement target position. It is assumed that the route cannot be a travel route. On the other hand, when the self position is located outside the movable space (in the non-movable space), the adjacent voxel Bn can be set as the movement target position and can be used as a movement path, and the occupied voxel Bo in the non-movable space is assumed. Can be a travel path.

ここで、ｒ＝（ｘ，ｙ，ｚ）は自律飛行ロボット１の現在の自己位置、Ｐ_prev＝（ｘ_prev，ｙ_prev，ｚ_prev）は前回設定した移動目標位置を示し、α、β、σは距離に応じて減衰する値を調整するためのパラメータである。 Subsequently, in the movement target position setting process, a process of setting each movement candidate position by evaluating each movement candidate position Pc not excluded from the evaluation target is performed (ST13). 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. In this case, 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 decreased. 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.

  Subsequently, in the movement target position setting process, the movement candidate position Pc having the maximum evaluation value V is set as the movement target position Po (ST14). The example of FIG. 6 represents that 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.

  When it is determined in ST10 that the self-position has not been estimated (ST10-No), skip control for skipping the process of setting the movement candidate position Pc is performed (ST15).

  When it is determined in ST11 that the estimated self-position is not located in the movable space (ST11-No), an emergency landing position is calculated (ST16), and the movement target position Po is calculated to the calculated emergency landing position. Is set (ST17). Specifically, first, a position where there is no obstacle in the movable space (an area other than the occupied voxel) and the position where the distance from the self position is the shortest (the position closest to the self position) is set as the return position. Set. Then, a position where the position in the horizontal plane is the return position and the altitude is the landing posture altitude previously set in the storage unit 8 is calculated as the emergency landing position. Then, the emergency landing position is set as the movement target position Po. For example, as shown in FIG. 7, since the self-position of the autonomous flying robot 1 is in the position of the occupied voxel Bo, the self-position is not located in the movable space in ST11 (that is, located in the non-movable space). Is determined). In ST16, the movement target position setting means 732 sets the emergency landing position at the position of the predetermined landing posture altitude (for example, 30 cm) that is the position in the movable space that is the closest in horizontal distance from the self position. To do. In ST17, the emergency landing position is set as the movement target position Po regardless of the movement candidate position Pc.

  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. 8 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. 8, 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 generated travel route data generated by the route search means 73 is set data of the waypoint (x, y, z), and this information is temporarily stored in the storage unit 8. When the emergency landing position is set as the movement target position setting means 732 in ST16 of the movement target position setting process described above, the movement route generation means 733 determines the movement target from its own position regardless of the voxel movement cost. A straight line reaching the position Po is temporarily stored in the storage unit 8 as a movement path.

  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. The “movement control means” in the present invention corresponds to the movement route generation means 733 and the flight control means 74 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. 9 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, 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, the target angle ψ ′ is an angle that faces the direction of the moving object M, that is, the direction of the moving object position.

  When skip control is performed in ST15 of the movement target position setting process, or when the emergency landing position is set as the movement target position Po and the self position is located at the emergency landing position in ST17, the flight The control means 74 performs landing control. In the landing control, the flight control means 74 performs control to gradually lower the flight altitude of the autonomous flight robot 1. After the landing control is started by the flight control means 74, the above-described position estimation processing (self-position estimation processing, moving object position estimation processing), speed estimation processing, route search processing, and route tracking control processing are performed. The rotational speed of the rotor 2 is controlled so that the flight altitude is lowered until the autonomous flying robot 1 is landed on the ground.

  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. Thereby, the autonomous flying robot 1 of the present embodiment sequentially updates the movement target position Po with respect to the position separated from the moving object M by the separation distance, and sequentially updates the movement route each time. It is possible to follow the flight while maintaining a predetermined distance 83 from the moving object. At this time, by not setting the movement target position Po as a non-movable space, it is possible to follow the flight only in the movable space, and for example, it is possible to maintain safety for a third party outside the site. In addition, even if the autonomous flying robot 1 moves unintentionally to the immovable space due to a sudden disturbance such as a gust of wind, by moving to land in the movable space with the shortest distance, Even in situations where flight is likely to be unstable, such as during strong winds, safety against third parties outside the site can be maintained. Furthermore, even when the self-position cannot be estimated, by controlling the flight altitude so as to land on the spot, the autonomous flying robot 1 moves away from the movable space and moves ahead of the non-movable space. It is possible to prevent flying.

  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, when the movement target position setting means 732 determines that the self position is not located in the movable space (ST11-No), the emergency landing position that is the return position and the landing posture altitude is calculated. (ST16), the movement target position Po is set to the emergency landing position (ST17), and the landing control is performed when the flight control means 74 is located at the movement target position Po that is the emergency landing position. ing. However, the present invention is not limited to this, and when it is determined that the self position is not located in the movable space (ST11-No), the return position (the position in the movable space where there is no obstacle, May be set as the movement target position Po. That is, when the self-position is located in the immovable space, the autonomous flying robot 1 moves toward the return position in the movable space, and after moving to the return position, does not land, The movement is controlled so as to follow and move at a position separated by a separation distance 83. As a result, even if the autonomous flying robot 1 unintentionally moves to a non-movable space due to a sudden disturbance such as a gust of wind, it continues to follow and fly within the movable space with the shortest distance. can do. Further, not limited to these embodiments, when the movement target position setting means 732 determines that the self position is not located in the movable space (ST11-No), it is desired to move to the emergency landing position or the return position. Instead, landing control may be performed from the self-position. As a result, it is possible to keep the safety to a third party as much as possible even in a strong wind where the movement cannot be controlled. Further, not limited to these embodiments, when the flight control means 74 detects that communication with the external device is no longer possible in the communication unit 9, the flight control unit 74 at that time or a predetermined predetermined landing position at that time Landing control may be performed at. In this way, in an abnormal state in which communication with an external device is not possible, it is possible to maintain safety for a third party by performing movement control so as to land and not continuing autonomous flight.

  Further, a notification unit such as a buzzer or a lamp may be provided to alert the surroundings by sound or light when landing control is performed in an emergency or the like. Thereby, the guards who rushed to the countermeasure can find the landing place at an early stage and can detect the abnormal state at an early stage.

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.

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 ... 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 path generation means 74 ... Flight control means 9 ... Communication unit M ... Moving object Mg ... Center of gravity of moving object Bo ... Occupied voxel Bn ... Proximity voxel Bf ... Free voxel Pc ... Movement candidate position Po ... Movement target position

Claims (6)

In an autonomous flight robot that follows and keeps a predetermined separation distance from a moving object in the flight space,
A storage unit storing a movable space in the flight space and a flight space map representing obstacles in the flight space;
Position estimating means for performing processing for estimating the moving object position of the moving object and the self-position of the autonomous flying robot;
Movement candidate position setting means for setting a plurality of movement candidate positions at surrounding positions separated from the moving object position by the separation distance;
A movement target position setting means for performing a process of setting one of the movement candidate positions as a movement target position from a result of evaluating each of the movement candidate positions;
Movement control means for performing a process of controlling to move from the self position to the movement target position, and following the movement by sequentially repeating the processes,
The movement target position setting means includes
When the self-position is located in the movable space from the flight space map and the self-position, the evaluation is performed only for the movement candidate position that is an area in the movable space and is not in the vicinity of the obstacle. An autonomous flying robot characterized by   When the self-position is located outside the movable space, the movement target position setting means is a position in the area where the obstacle does not exist in the movable space regardless of the movement candidate position. The autonomous flying robot according to claim 1, wherein the movement target position is set at a return position that is closest to the self position. The movement target position setting means sets the movement target position to the emergency landing position that is the return position and a position at a predetermined altitude,
The autonomous flight robot according to claim 2, wherein the movement control unit performs landing control for gradually decreasing a flight altitude when the self-position is at the emergency landing position set by the movement target position setting unit. The movement target position setting means performs skip control to skip the process of setting the movement target position when the position estimation means cannot estimate the self position on a horizontal plane.
The autonomous flight according to any one of claims 1 to 3, wherein when the skip control is performed by the movement target position setting unit, the movement control unit performs landing control that gradually decreases a flight altitude. robot. A communication unit that communicates with an external device;
The autonomous flight robot according to any one of claims 1 to 4, wherein the movement control unit performs landing control for gradually decreasing a flight altitude when the communication unit cannot communicate with the external device. The autonomous flying robot according to any one of claims 1 to 5, further comprising a notification unit that issues a warning to the surroundings by sound or light when the landing control is performed by the movement control unit.

Images