JP6014484B2 - 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 an intruder, it is necessary to be able to follow the intruder while avoiding attacks and captures from the intruder. In particular, it is necessary to be able to avoid the intruder and the autonomous mobile robot while maintaining an appropriate distance even when the intruder is caught up near the wall. Therefore, an object of the present invention is to perform movement control so that an intruder can follow the intruder while appropriately avoiding an attack from 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 while maintaining a predetermined separation distance in a moving space , a storage unit that stores an obstacle map representing an obstacle in the moving space; A position estimation unit that performs a process of estimating a moving object position of the moving object and a self position of the autonomous mobile robot, and a result of evaluating a peripheral area that is a surrounding area that is separated from the moving object position by the separation distance. has a movement target position setting means for processing for setting a moving target position in the position of the peripheral region, and a movement control unit that performs processing for controlling so as to move to the movement target position from the self-position, The movement target position setting means uses the obstacle map to move around the obstacle in the vicinity of the obstacle. Position identify one or more contiguous areas except the areas, for each position of the continuous area, a small distance between the self-position a large evaluation value as the position the continuous region belonging large An autonomous mobile robot is characterized in that an evaluation value that is larger as a position is calculated, and the movement target position is set at a position where the evaluation value is the largest .

  With this configuration, the movement target position setting means of the present invention allows the area near the obstacle (the area close to the obstacle and the obstacle) from the surrounding area (peripheral area) separated from the moving object position by a predetermined separation distance. ), And the remaining area (continuous area) is specified. Then, evaluation is performed so that the movement target position is more easily set at a position in the continuous area as the continuous area is larger in the identified continuous areas. In this way, by making it easier to set the movement target position in a larger area, the autonomous mobile robot can move more freely even when a moving object such as an intruder approaches to attack. It can be avoided to move to a higher position, making it difficult to catch up on the wall.

  Further, as a preferred aspect of the present invention, the movement target position setting means evaluates not to set the movement target position in the continuous area whose size is a predetermined value or less.

  With this configuration, in order to set a movement target position to another avoidance destination with a high degree of freedom of movement without setting a movement target position as a movement destination for avoiding a continuous area where the size of the area is extremely small Thus, it is possible to avoid the moving object more preferably. Moreover, the calculation amount required for the evaluation can be reduced by not performing the evaluation for setting the movement target position in such an extremely small continuous region.

  Further, as a preferred aspect of the present invention, the movement target position setting means evaluates so that the movement target position is more easily set at a position in the continuous area as the distance to the other continuous area is smaller. And

  With such a configuration, in the evaluation when setting the movement target position at a position in a certain continuous area, the distance between the continuous area and another continuous area is taken into consideration, and the smaller the distance, the more within the continuous area to be evaluated. It is evaluated so that it is easy to set the movement candidate position. That is, the smaller the distance is, the easier it is to move from the continuous area to be evaluated to another continuous area, and therefore, the evaluation is performed assuming that the degree of freedom of movement is relatively high. Thereby, the autonomous mobile robot can move to an area with a higher degree of freedom of movement, and can be more preferably avoided from attacks from moving objects.

  Moreover, as a preferable aspect of the present invention, the image processing apparatus further includes a movement candidate position setting unit that sets a plurality of movement candidate positions in the peripheral area, and the continuous area excludes the movement candidate positions located in the vicinity of the obstacle. It is an area composed of a set of continuous movement candidate positions, and the size of the continuous area is calculated by the number of movement candidate positions.

  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 of the present invention seems to move to a position with a higher degree of freedom of movement even when a moving object such as an intruder to be followed approaches to attack. It can be avoided and it can be made difficult to be caught up near the wall.

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 Illustration of clustering Explanatory diagram of the graph structure in the generation of travel paths Illustration of local target calculation Explanatory drawing of other embodiment Explanatory drawing of other embodiment Explanatory drawing of other embodiment

  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. 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. 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 it is read from 8 and used.

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

  The separation distance 83 is a relative distance to be maintained on the horizontal plane between the autonomous flying robot 1 and the moving object when following the moving object, and is a value set in advance by an administrator of the autonomous flying robot 1 or the like. When a predetermined moving object is monitored using the autonomous flying robot 1, it is necessary to approach the moving object and acquire a more detailed captured image. However, in order not to be attacked by a hostile moving object such as an intruder, it is necessary to be separated by a certain distance or more. Therefore, the autonomous flying robot 1 of the present embodiment can acquire a detailed captured image of the moving object and set a separation distance 83 in advance at a distance that is difficult to receive an attack from the moving object, while maintaining the separation distance. Make a follow-up flight. In this embodiment, it is assumed that the separation distance 83 is set to 3 m.

  The 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 means 731 performs a process of setting a plurality of movement candidate positions in a surrounding area (corresponding to “peripheral area” in the present invention) that is separated from the moving object position by a separation distance 83. 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. FIG. 5 is an explanatory view for setting the movement target position, and is a view when a part of the flight space displayed by the voxel is looked down from directly above at time t as in FIG.

  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. Specifically, as illustrated in FIG. 5, the following description is made so that the movement candidate position Pc at a position that interferes with a voxel other than the free voxel Bf (occupied voxel Bo, adjacent voxel Bn) is not set as the movement target position. Exclude from evaluation. The movement candidate position Pc shown in FIG. 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.

  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. FIG. 6 is a diagram for explaining clustering and is a diagram showing a state at time t + 1 after FIG. In the example illustrated in FIG. 6, the moving object M represents that the moving object M has moved from the moving object M0 at time t to the moving object M1 at time t + 1. At this time, in each movement candidate position Pc that is not excluded from the evaluation target, there are three continuous movement candidate positions Pc, which are identified as three clusters indicated by reference numerals CL1 to CL3, respectively. The “continuous region” of the present invention corresponds to the “cluster” of the present embodiment. 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. 6, 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. The “size of the continuous area” of the present invention corresponds to the “cluster size” of the present embodiment.

ここで、ｒ＝（ｘ，ｙ，ｚ）は自律飛行ロボット１の現在の自己位置、Ｐ_prev＝（ｘ_prev，ｙ_prev，ｚ_prev）は前回設定した移動目標位置を示し、α、β、σは距離に応じて減衰する値を調整するためのパラメータである。また、Ｃ_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. 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. 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. 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. 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.

  As described above, in this 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 increases. Therefore, the autonomous flying robot 1 can easily set the movement candidate position Pc belonging to a cluster (continuous region) 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. 6, 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 in this embodiment, moving to CL2 having a large cluster size increases the degree of freedom of movement thereafter, 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 (the size of the continuous area) is large, it is possible to appropriately avoid an attack from the moving object M or the like.

  The movement path generation means uses the voxel information stored in the storage unit, the movement target position calculated by the movement target position setting means, and the self position calculated by the position estimation means to determine the movement path of the autonomous flying robot. The movement route generation process to calculate is performed. Specifically, in the route generation process, first, the voxel information 82 is read from the storage unit, and the center of each voxel of the free voxel Bf and the neighboring voxel Bn, which is the divided movable space, is set as a node and adjacent to the node. A graph structure is generated with a line segment connecting nodes as an edge. FIG. 7 is a diagram for explaining the graph structure, in which a part (27) of movable voxels (free voxels Bf or adjacent voxels Bn) in the flight space are cut out. In FIG. 7, each cube represented by the symbol B represents a free voxel Bf or a neighboring voxel Bn. Further, a sphere filled with black or hatching at the center of these voxels B is a node, and a line segment displayed with a dotted line connecting the nodes is an edge. After generating the graph structure for all movable voxels based on the voxel information 82 in this way, from the voxel node where the autonomous flying robot 1 exists to the voxel node where the movement target position Po exists. Generate a travel route. Although various route generation methods can be applied to the generation method of the movement route, in the present embodiment, the movement route is searched using the A * (Aster) route search method. At this time, the voxel cost stored as the voxel information 82 is used as a movement cost in the A * route search method. 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. 8 is a diagram for explaining the calculation of the local target. In calculating the local target, the flight control means 74 reads the movement route generated by the route search means 73 from the storage unit 8, and the waypoint Wp1 that the autonomous flying robot is aiming at at the current time and the waypoint that has passed the previous time. A straight line W connecting two points with Wp0 is obtained. Then, the flight control means calculates intersections Lp ′ and Lp between the obtained straight line W and the sphere S centered on the self-position of the autonomous flying robot 1, and uses the intersection Lp close to the target via point Wp1 as a local target. Ask. In this way, by performing flight control so that the autonomous flying robot 1 moves toward the local target at each time, the local target always moves on the moving path toward the moving target position Po. Will fly along the path of travel.

Next, in the path following control process, 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. Further, even when the moving object M approaches the autonomous flying robot 1 in order to attack or the like, the moving object M is avoided to move to a position with a higher degree of freedom of movement, so that it is difficult to be caught up by a wall or the like. it can.

  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 movement target position setting means 732 clusters all successive movement candidate positions Pc to identify all clusters, obtains a cluster size for each cluster, and moves candidate positions Pc belonging to each cluster. An evaluation value V is calculated. However, the present invention is not limited to this, and the movement candidate position Pc belonging to a cluster whose cluster size is equal to or smaller than a predetermined threshold (that is, a region where the size of the continuous region is equal to or smaller than a predetermined value) is excluded from the evaluation target. It may not be set as the movement target position Po. When the moving object M and the autonomous flying robot 1 are in a positional relationship as shown in FIG. 9, the distance between the autonomous position of the autonomous flying robot 1 and the movement candidate position Pc belonging to the cluster CL1 is small. It is possible that the evaluation value V of the candidate position Pc is greatly calculated and set as the movement target position Po. However, when moving to a continuous area (cluster) having extremely small movement freedom as shown in FIG. 9, it is obvious that the risk of being attacked by the moving object M increases. Therefore, a continuous region with extremely small freedom of movement can be obtained by determining the minimum cluster size in advance and storing it in the storage unit 8 and excluding the movement candidate position Pc of the cluster smaller than the minimum cluster size from the evaluation target. It is possible not to move to (cluster).

  In the above embodiment, the evaluation value V of each movement candidate position Pc is calculated based on the cluster size of each cluster. However, the present invention is not limited to this, and the evaluation value V of each movement candidate position Pc may be calculated according to the distance between clusters (distance between continuous regions). That is, in evaluating whether or not to set a movement target position in a certain continuous area, the distance between the continuous area and another continuous area is taken into consideration, and the smaller the distance, the movement candidate in the continuous area. Evaluate so that the position can be easily set. Specifically, the evaluation value V of the movement candidate position Pc belonging to a certain cluster is evaluated so that the evaluation value V becomes higher as the cluster to which the movement candidate position Pc belongs is closer to the other cluster. When the moving object M and the autonomous flying robot 1 are in a positional relationship as shown in FIG. 10, the clusters CL1 and CL2 have the same cluster size, and the distance from the self position to the movement candidate position Pc is also substantially equal. Therefore, in the evaluation in the above embodiment, the evaluation value V of the movement candidate position Pc belonging to CL1 and the movement candidate position Pc belonging to CL2 are substantially equal. However, when the movement amount when moving from the CL1 region to the CL3 region is compared with the movement amount when moving from the CL2 region to the CL3 region, the distance R31 between CL1 and CL3 and CL2 to CL3. Comparison with the distance R23 between the two shows that the former has a relatively small amount of movement. Therefore, it can be said that the autonomous flying robot 1 has a relatively high degree of freedom of movement because it is easier to move to the CL3 area than the CL2 area. The process will be specifically described. First, a distance between a cluster to which a certain movement candidate position Pc belongs and another cluster is calculated. For example, in the example of FIG. 10, when the movement candidate position Pc whose evaluation value is to be calculated belongs to CL1, the distance to another cluster is calculated by R31 + R12. Similarly, when the movement candidate position Pc to be evaluated belongs to CL2, the distance to another cluster is calculated by R12 + R23. And it calculates so that evaluation value V may become large according to the distance with the calculated other cluster. As a result, the autonomous flying robot 1 can easily set the movement candidate position Pc of CL1 as the movement target position Po instead of the movement candidate position Pc of CL2, and moves toward the CL1 area as shown by the arrow in FIG. It becomes easy to do. Thus, by evaluating so that the evaluation value V becomes higher as the distance between a cluster to which a certain movement candidate position Pc belongs and another cluster is closer, it is possible to move to a region with a higher degree of freedom of movement. It is possible to more suitably avoid an attack from an object. The distance between the clusters may be the circumferential distance from the end of each cluster as shown in FIG. 10A, or the center positions P1 to P1 of each cluster as shown in FIG. A linear distance (R′12, R′23, R′31) from P3 may be used.

  In the above embodiment, the movement candidate position setting unit 731 sets a plurality of movement candidate positions Pc in a region (peripheral region) separated from the moving object position by the separation distance 83, and the movement target position setting unit 732 sets the obstacle. A cluster of movement candidate positions Pc excluding those located in the vicinity (occupied voxels Bo and adjacent voxels Bn) is clustered, and the “continuous area size” is obtained from the cluster size. However, the present invention is not limited to this, and the size of the continuous area may be obtained without setting the movement candidate position Pc. As shown in FIG. 11 (a), the movement target position setting means 732 is shown in FIG. 11 (b) from an area on the circumference (peripheral area BA) that is separated from the moving object position by a separation distance 83 (= L). As described above, the regions excluding the region located in the vicinity of the obstacle (occupied voxel Bo and adjacent voxel Bn) are specified as the continuous regions A1 to A3. And the magnitude | size (for example, length r1-r3 of a continuous area) of the specified continuous area A1-A3 is calculated | required as a magnitude | size of a continuous area. Then, an evaluation value is calculated for each continuous region. At this time, the evaluation value is calculated so that a higher evaluation value is calculated for a continuous region closer to the self-position, and a higher evaluation value is calculated for a larger continuous region size (r1 to r3). Then, the movement target position Po is set to a continuous area having the largest evaluation value and closest to the self position. In the example of FIG. 11, the continuous area having the largest evaluation value is calculated as A2, and the movement target position is set to Po, which is the position closest to the self position in A2. However, the movement target position Po may be set at the center position of the continuous area having the largest evaluation value.

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 BA ... Peripheral area

Claims (5)

In an autonomous mobile robot that moves following a moving object with a predetermined separation distance in a moving space,
A storage unit storing an obstacle map representing obstacles in the 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;
A movement target position setting means for performing processing for setting the position to move the target position of the peripheral region from the evaluation results of the peripheral region is a region around spaced by the distance from the moving object position,
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
Using said obstacle map to identify one or more contiguous areas excluding the area located in the vicinity of the obstacle in the peripheral region, for each position of the continuous area, the continuous area belongs An autonomous mobile robot characterized in that a larger evaluation value is calculated for a position having a larger evaluation value for a larger position and a distance from the self-position is smaller, and the movement target position is set at a position having the largest evaluation value .   The autonomous mobile robot according to claim 1, wherein the movement target position setting unit evaluates not to set the movement target position in the continuous area having a size equal to or less than a predetermined value.   The said movement target position setting means evaluates so that the said movement target position may be easily set to the position in this continuous area, so that the distance with another continuous area is small. Autonomous mobile robot. It further has a movement candidate position setting means for setting a plurality of movement candidate positions in the peripheral area,
The continuous area is an area composed of a set of continuous movement candidate positions excluding the movement candidate positions located in the vicinity of the obstacle, and the size of the continuous area is calculated by the number of movement candidate positions. The autonomous mobile robot according to any one of claims 1 to 3, wherein the autonomous mobile robot is provided. The autonomous mobile robot according to any one of claims 1 to 4, wherein the autonomous mobile robot is an autonomous flight robot that freely flies within a predetermined space.

Images