JP6140458B2 - Autonomous mobile robot - Google Patents

Description

  The present invention relates to an autonomous mobile robot that moves following a moving object such as an intruder, and more particularly, to an autonomous mobile robot that moves following such that it can be appropriately avoided from attacks, captures, and the like from intruders.

  Conventionally, an autonomous mobile robot that moves while following autonomously while maintaining a predetermined distance with respect to the tracking target has been proposed. For example, Patent Document 1 discloses an autonomous mobile robot that recognizes a tracking target based on a camera image, recognizes a distance to the tracking target, and performs movement control so that the tracking movement is maintained at a predetermined distance. ing. In the related art, by holding the obstacle area as map data in advance, the movement is stopped when there is an obstacle in its own moving direction, or the speed is increased when approaching the tracking target by a predetermined distance or more. By lowering or stopping, contact with an obstacle or following target is avoided.

JP 2004-299025 A

  However, the above-mentioned conventional technology assumes a tracking target that performs cooperative operation on an autonomous mobile robot, and does not cooperate so as to intentionally approach an autonomous mobile robot for an attack or the like. Does not assume the target to follow. Therefore, in the above prior art, when the distance to the tracking target becomes close, it is possible to prevent the collision with the tracking target by reducing the moving speed or stopping, but the attack from the tracking target is prevented. It cannot be avoided properly. When an autonomous mobile robot is used for security purposes such as monitoring of an intruder, it is necessary to be able to follow the intruder while appropriately avoiding attacks and captures from the intruder. Accordingly, an object of the present invention is to perform movement control so that a safe and stable follow-up movement can be performed by a lean movement while maintaining a certain relative distance so that an attack is not made by a follow-up target such as an intruder.

In order to solve such a problem, in an autonomous mobile robot that moves following a moving object with a predetermined separation distance in a moving space,
Position estimation means for performing processing for estimating the moving object position of the moving object and the self position of the autonomous mobile robot, and movement for setting a plurality of movement candidate positions at surrounding positions separated from the moving object position by the separation distance Candidate position setting means, movement target position setting means for evaluating each of the movement candidate positions and setting one of the movement candidate positions as a movement target position, and moving from the self position to the movement target position Movement control means for performing a process to be controlled, and to follow and move by sequentially repeating each process,
The movement target position setting means provides an autonomous mobile robot characterized in that the movement target position is evaluated so that the movement target position is closer to the movement candidate position.

  With this configuration, the autonomous mobile robot determines a movement candidate position closer to the self position as the movement target position and easily moves to that position. That is, since it is controlled to move from a moving object such as an intruder to a position that can maintain a predetermined separation distance at a short distance from its own position, it is possible to suppress useless movement around the moving object, and such useless movement. It is possible to reduce opportunities such as attacks from moving objects caused by. Therefore, it is possible to perform a stable follow-up movement while reducing the risk of an attack from a moving object.

  As a preferred aspect of the present invention, the movement target position setting means evaluates the movement target position closer to the movement target position set immediately before, so that the movement target position is more easily set.

  With such a configuration, the autonomous mobile robot is closer to the previously set movement target position because the movement candidate position with a smaller distance from the movement target position set immediately before (just before) is easier to be set as the next movement target position. It becomes easy to move to the movement candidate position. That is, it is difficult to move to a movement candidate position that is completely different from the previously set movement target position (for example, a movement candidate position in the opposite direction from the previously set movement target position). Therefore, it is possible to avoid a phenomenon (vibration phenomenon, which will be described later) that moves right and left at the current position. Therefore, useless movement around the moving object can be suppressed, and the risk of an attack from the moving object caused by such useless movement can be reduced.

  Further, as a preferred aspect of the present invention, the movement target position setting means performs the evaluation only for the movement candidate positions that are not located in the vicinity of the obstacle using a previously stored obstacle map.

  With such a configuration, the movement target position setting means of the present invention allows the vicinity of an obstacle (the area of the obstacle and the obstacle) to be selected from a plurality of movement candidate positions set at peripheral positions separated from the moving object position. The movement candidate position located in the area close to the object is excluded from the evaluation target, the remaining movement candidate positions are evaluated, and the movement target position is set from among them. Thereby, since the collision between the autonomous mobile robot and the obstacle can be avoided and the evaluation of all the movement candidate positions can be omitted, the amount of calculation for the evaluation can be reduced.

  As a preferred aspect of the present invention, the movement target position setting means uses the obstacle map to obtain a set of continuous movement candidate positions excluding the movement candidate positions located in the vicinity of the obstacle. The number of movement candidate positions constituting the cluster is identified as a cluster size, and the evaluation is performed such that the movement candidate position constituting a cluster having a larger cluster size is more easily set as the movement target position. .

  With this configuration, by making it easier to set a movement target position for a movement candidate position that constitutes a cluster having a large cluster size, an autonomous mobile robot can be used when a moving object such as an intruder approaches to attack. Even if it exists, it can avoid moving to a position with a higher freedom degree of movement, and it can be made difficult to be caught up near the wall.

  As a preferred aspect of the present invention, the autonomous mobile robot is an autonomous flying robot that freely flies within a predetermined space.

  As described above, the autonomous mobile robot according to the present invention does not perform useless movement around the intruder even when the intruder who is the subject of tracking approaches to perform an attack or the like. It is possible to perform a stable follow-up movement while avoiding the above.

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 Explanatory drawing about the setting of the movement candidate position to be evaluated Explanatory diagram for setting the movement target position Illustration of vibration phenomenon Illustration of clustering 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 mobile robot in the present invention is not limited to the autonomous flying robot as described above, but also includes an autonomous traveling robot that travels following a moving object. Further, the autonomous mobile robot of the present invention can be similarly applied to various moving objects other than a person such as an automobile.

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

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

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

  The storage unit 8 is an information storage device such as a ROM (Read Only Memory), a RAM (Random Access Memory), and an HDD (Hard Disk Drive). The storage unit 8 stores various programs and various data, and inputs / outputs such information to / from the control unit. In the various data, in addition to the 2D point information 81 and voxel information 82 corresponding to the “obstacle map” of the present invention, the 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 the data is read from 8 and used.

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

  The 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 get closer to the moving object and acquire a more detailed captured image. However, in order not to be attacked by a hostile moving object such as an intruder, it is necessary to be separated by a certain distance or more. Therefore, the autonomous flying robot 1 of the present embodiment can acquire a detailed captured image of the moving object and set 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 flight position (self-position) of the autonomous flying robot 1 in the flight space based on the outputs of the distance detection sensor 4 and the imaging unit 3, and the current position of the moving object. (Hereinafter referred to as “moving object position”) is performed.

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

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

  Next, in the self-position estimation process, the x, y and yaw angles ψ of the self-position are estimated. 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 in a surrounding area 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 at the time t 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 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. 5 to 8 are explanatory diagrams for setting the movement target position, and at time t, t + 1, t + 2, and t + n, a part of the flight space displayed by the voxel is looked down from right above as in FIG. It is a figure of time. 5 to 8, the moving objects M at the times t, t + 1, t + 2, and t + n are represented by symbols M0, M1, M2, and Mn, respectively.

  In the movement target position setting process, first, a process of excluding the movement candidate position Pc located in the vicinity of the obstacle from the evaluation target is performed. 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. For example, as illustrated in FIG. 5, the evaluation target described below is set 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. The movement candidate position Pc shown in FIG. 5 is obtained after the movement candidate position Pc that interferes with the adjacent voxel Bn is excluded 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. On the other hand, the occupied voxel Bo cannot be set as a movement target position and cannot be a movement path. In this way, by performing the process of excluding the movement candidate position Pc located in the vicinity of the obstacle from the evaluation target, the collision between the autonomous mobile robot and the obstacle can be avoided, and all the movement candidate positions are evaluated. Since this can be omitted, the amount of calculation for evaluation can be reduced.

  Next, in the movement target position setting process, at each movement candidate position Pc that is not excluded from the evaluation target, a cluster of continuous movement candidate positions Pc is clustered to identify one or a plurality of clusters. Here, clustering will be described with reference to FIG. In FIG. 8, there are three continuous movement candidate positions Pc in each movement candidate position Pc that are not excluded from the evaluation target, and are identified as three clusters indicated by reference numerals CL1 to CL3, respectively. Thus, after the cluster is specified from the movement candidate position Pc to be evaluated, the movement target position setting process performs a process of calculating the cluster size for each cluster. In the present embodiment, calculation is performed based on the number of movement candidate positions Pc constituting each cluster. For example, in the example of FIG. 8, the cluster size of the cluster CL1 is 7, the cluster size of the cluster CL2 is 34, and the cluster size of the cluster CL3 is 7. In the example of FIGS. 5 to 7, since there is one continuous movement candidate position Pc, the number of clusters is one.

ここで、ｒ＝（ｘ，ｙ，ｚ）は自律飛行ロボット１の現在の自己位置、Ｐ_prev＝（ｘ_prev，ｙ_prev，ｚ_prev）は前回設定した移動目標位置を示し、α、β、σは距離に応じて減衰する値を調整するためのパラメータである。すなわち、数１において、（ｒ−Ｐ_c）が自己位置から各移動候補位置までの距離を表し、（Ｐ_prev−Ｐ_c）が前回設定した移動目標位置からの移動候補位置までの距離を表す。また、Ｃ_sizeは移動候補位置Ｐｃの所属するクラスタのクラスタサイズであり、δはクラスタサイズに応じて増加する値を調整するためのパラメータである。 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 increased as the distance from the current position of the autonomous flying robot 1 to each movement candidate position is smaller. Further, the evaluation value is increased as the distance from the previously set movement target position to the movement candidate position is smaller. Further, evaluation is performed such that the evaluation value increases as the cluster size of the cluster to which each movement candidate position Pc belongs is larger. A specific evaluation formula for obtaining the evaluation value V in the present embodiment is expressed by Equation 1.

Here, r = (x, y, z) represents the current self-position of the autonomous flying robot 1, P _prev = (x _prev , y _prev , z _prev ) represents the previously set movement target position, and α, β, σ is a parameter for adjusting a value that attenuates according to the distance. That is, in Equation 1, (r−P _c ) represents the distance from the own position to each movement candidate position, and (P _prev −P _c ) represents the distance from the previously set movement target position to the movement candidate position. . C _size is the cluster size of the cluster to which the movement candidate position Pc belongs, and δ is a parameter for adjusting a value that increases in accordance with the cluster size.

  As described above, in the movement target position setting process, the evaluation value V is obtained, and the movement candidate position Pc that maximizes the evaluation value V is set as the movement target position Po. 6 to 8 indicate 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.

  As described above, by evaluating that the evaluation value increases as the distance from the self-position of the autonomous flying robot 1 to each movement candidate position Pc decreases, the autonomous flying robot 1 moves to the movement candidate position Pc closer to the self-position. It becomes easy to move. For example, as shown in FIG. 6, when the moving object moves from M0 to M1, the autonomous flying robot 1 does not use the previous movement candidate position Pc indicated by the arrow A12 but the arrow A11 that is closer to the own position. It becomes easy to set the indicated movement candidate position Pc as the movement target position Po. Therefore, by making it easy to move to the movement candidate position Pc close to the self position, it is possible to maintain the separation distance 83 from the moving object with a small movement distance, and thus it is possible to quickly move to a safe position. On the other hand, if it moves to the previous movement candidate position Pc indicated by the arrow A12, the movement distance becomes larger than the case where it moves to the previous movement candidate position Pc indicated by the arrow A11. The risk of being attacked from increases. In this way, by evaluating that the evaluation value increases as the distance from the current position of the autonomous flying robot 1 to each movement candidate position Pc becomes smaller, the intruder who is the tracking target has approached to attack or the like. Even in this case, since unnecessary movement is not performed around the intruder, stable follow-up movement can be performed while avoiding attacks and the like.

  Further, as described above, the vibration phenomenon can be suppressed by evaluating the evaluation value to be larger as the distance from the previously set movement target position to the movement candidate position is smaller. The effect will be specifically described with reference to FIG. As shown in FIG. 7, it is assumed that the moving object moves from the position of M1 to the position of M2 at time t + 2. At this time, it is assumed that the previous movement candidate position Pc1 indicated by the arrow A21 and the previous movement candidate position Pc2 indicated by the arrow A22 are the movement candidate positions closest to the self position. That is, it is assumed that the distance from the self position to the movement candidate position Pc1 is substantially equal to the distance from the self position to the movement candidate position Pc2. Here, if the movement candidate position Pc is evaluated only by the distance from the self position and the movement target position Po is set as described above, Pc1 is set as the movement target position Po depending on the evaluation time, or the movement candidate The position Pc2 is set as the movement target position Po. In particular, even if the moving object remains stopped at the position M2 due to the influence of the self-position estimation error or the moving object position estimation error, Pc1 is set as the movement target position Po depending on the evaluation time. , Pc2 may be repeatedly set. As described above, the autonomous flying robot 1 repeats, in a short time, that the movement candidate position Pc at a position completely different from the previously set movement target position Po is selected as the movement target position Po. There is a risk of causing a vibration phenomenon such as going back and forth at the self-position as shown. When a vibration phenomenon occurs, it becomes difficult for the autonomous flying robot 1 to quickly avoid an attack from a moving object or the like (that is, to keep a predetermined distance 83 from the moving object quickly). Therefore, by evaluating that the evaluation value becomes larger as the distance from the previously set movement target position Po to the movement candidate position Pc is smaller, the movement candidate position Pc at a position completely different from the movement target position Po set last time. Is less likely to be selected as the movement target position Po, and the above-described vibration phenomenon can be suppressed. In the example of FIG. 7, the movement candidate position Pc1 that is closer to the movement target position (Po in FIG. 6) set at time t + 1 is set as the movement target position Po.

  As described above, in the present embodiment, the evaluation value V of each movement candidate position Pc is evaluated so as to increase as the cluster size of the cluster to which the movement candidate position Pc belongs is increased. Therefore, the autonomous flying robot 1 can easily set the movement candidate position Pc belonging to the cluster having a large cluster size as the movement target position Po. In other words, the autonomous flying robot 1 can easily move toward an area where the movable area is larger (an area where the degree of freedom of movement is high). Therefore, even when the moving object M, which is an intruder, approaches the autonomous flying robot 1 to attack or the like, it is difficult to catch up near the wall by avoiding moving to a position with a high degree of freedom of movement. can do. For example, in the example of FIG. 8, it becomes difficult for the autonomous flying robot 1 to move to the cluster CL1 (and CL3) having a small cluster size, but to move to the cluster CL2 having a large cluster size. If the autonomous flying robot 1 moves to CL1 having a small cluster size, it gradually gets caught up in the corner of the wall, and eventually falls into a situation where it is easily attacked or captured. However, as the present embodiment moves to CL2 having a large cluster size, the degree of freedom of movement thereafter is large, so that there is a large space (space) for avoiding attacks by moving objects M and the like. Become. Thus, by making it easy to set the movement target position at a position where the cluster size is large, it is possible to appropriately avoid an attack from the moving object M or the like.

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

  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. 10 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.

  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 away from the moving object M by the separation distance, and sequentially updates the movement route each time, thereby moving the movement. It is possible to follow and fly while maintaining a predetermined separation distance from the object. At this time, the autonomous mobile robot 1 does not perform useless movement around the intruder even when the moving object M approaches to perform an attack or the like, and thus is stable while avoiding the attack or the like. Follow-up movement can be performed. Moreover, since it avoids moving to a position with a higher degree of freedom of movement, it can be made difficult to be caught up with a wall or the like.

  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 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 with a simple process although the tracking performance to the moving object M is inferior. 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 (4)

In an autonomous mobile robot that moves following a moving object with a predetermined separation distance in a moving space,
Position estimating means for performing a process of estimating a moving object position of the moving object and a self position of the autonomous mobile 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 evaluating each of the movement candidate positions and setting one of the movement candidate positions as a movement target position;
Movement control means for performing a process of controlling to move from the self position to the movement target position;
And following movement by sequentially repeating each of the above processes,
The movement target position setting means performs the evaluation only on the movement candidate position that is not located near the obstacle using a previously stored obstacle map, and the movement target position is closer to the movement candidate position. An autonomous mobile robot characterized by being evaluated so as to be easily set at a position. In an autonomous mobile robot that moves following a moving object with a predetermined separation distance in a moving space,
Position estimating means for performing a process of estimating a moving object position of the moving object and a self position of the autonomous mobile 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 evaluating each of the movement candidate positions and setting one of the movement candidate positions as a movement target position;
Movement control means for performing a process of controlling to move from the self position to the movement target position;
And following movement by sequentially repeating each of the above processes,
The movement target position setting means, said evaluated as likely to be set to a certain extent the movement target position candidate travel position close to the self-position, and, by using a pre-stored obstacle map, the position in the vicinity of obstacles A set of continuous movement candidate positions excluding the movement candidate positions to be identified as a cluster, the number of movement candidate positions constituting the cluster is calculated as a cluster size, and a movement candidate constituting a cluster with a large cluster size An autonomous mobile robot characterized in that an evaluation is performed such that the position is easier to be set as the movement target position. The movement target position setting means, autonomous mobile robot according to claim 1 or claim 2 for evaluating ease set as is the migration candidate position near the movement target position set immediately before the movement target position. The autonomous mobile robot according to any one of claims 1 to 3 , wherein the autonomous mobile robot is an autonomous flight robot that freely flies within a predetermined space.

Images