JP6758069B2 - Autonomous flying robot - Google Patents

Description

The present invention relates to an autonomous flying robot that autonomously flies in a moving space.

Conventionally, an autonomous flight robot capable of calculating a movement path from a current position to a predetermined movement target position and autonomously flying along the movement path has been proposed. For example, in Patent Document 1, a movement target position is set at a position separated by a predetermined distance from a moving object such as a thief, a movement path from the current position to the movement target position is generated, and the movement path is followed. An autonomous flight robot that is controlled to fly is disclosed.

Japanese Unexamined Patent Publication No. 2014-119901

However, in the above-mentioned conventional technique, flight control may not be possible in rare cases due to an abnormality of various sensors used for attitude control or the like or the influence of a disturbance such as a gust. For example, even if the autonomous flight robot controls the flight with the intention of hovering to the same position, it may perform vibration movement different from the intention of moving left and right around the hovering position. If such a vibration state in which autonomous flight control does not work occurs, it may develop into a dangerous situation such as flying far outside the permitted area or coming into contact with a building or the like. ..

Therefore, the present invention quickly eliminates the vibration state and secures safety by determining whether or not the vibration state has occurred and stopping the flight at the timing when the speed becomes the minimum when the vibration state occurs. The purpose is.

One aspect of the present invention is an autonomous flight robot that autonomously flies in a moving space, a speed estimation means that estimates a flight speed from a measured value obtained by measuring the flight state of the autonomous flight robot, and a predetermined time from the current time. When the vibration state detecting means for detecting the vibration state in which the flight speed repeats code reversal based on the change history of the flight speed in the past period up to the previous time and the vibration state being detected by the vibration state detecting means. The autonomous flight robot is characterized by having a flight control means for stopping the flight at the timing when the flight speed becomes the minimum.

Here, it is preferable that the vibration state detecting means determines the vibration state when the code of the flight speed in the past period has changed by a predetermined number of times or more.

Further, it is preferable that the vibration state detecting means set the past period, which is longer as the amplitude value of the flight speed in the immediate vicinity of the current time is larger.

Further, when the vibration state detection means changes the code of the flight speed more than the predetermined number of times in the past period and the amplitude value tends to decrease with the passage of time, after the lapse of a predetermined determination time from the current time. If the amplitude value in the above is larger than a predetermined amplitude threshold value, it is preferable to determine that the vibration state is present.

Further, an obstacle information indicating the position of an obstacle existing in the flight space is stored, and a self-position estimation unit for estimating the self-position of the autonomous flight robot is further provided, and the flight control unit is provided with the obstacle information. It is preferable to perform the flight stop when there are no obstacles below the self-position using.

Further, the flight control means further includes a route search means for setting a movement target position in the flight space and obtaining a flight path from the self position to the movement target position, and the flight control means flies along the flight path. It is preferable that the control and vibration state detecting means changes the number-of-times threshold value to a large value when the change in the movement target position is large, and changes the number-time threshold value to a small value when the change in the movement target position is small. is there.

According to the present invention, by stopping the flight at the timing when the speed becomes the minimum when the vibration state is reached, the vibration state can be quickly eliminated and safety can be ensured.

It is a figure which shows the structure of the autonomous flight robot in embodiment of this invention. It is a figure which shows the structure of the autonomous flight robot system in embodiment of this invention. It is a functional block diagram which shows the structure of the autonomous flight robot in embodiment of this invention. It is a flow figure of the vibration state detection processing in embodiment of this invention. It is a figure which shows the change (increasing tendency) of the flight speed in embodiment of this invention. It is a figure which shows the change (decrease tendency) of the flight speed in embodiment of this invention. It is a figure explaining the calculation process of the local target in embodiment of this invention.

The autonomous flight robot 1 according to the embodiment of the present invention is a quad rotor type small unmanned helicopter as shown in the overview diagram of FIG. The scope of application of the present invention is not limited to the quad rotor type small unmanned helicopter, and the same can be applied to the single rotor type small unmanned helicopter.

As shown in the system configuration diagram of FIG. 2, the autonomous flight robot 1 communicates with an external security center 100 and a management device 102, tracks a predetermined moving object existing in the moving space as a target object M, and measures the object. It is configured to exert a predetermined function with respect to the target object M. The moving object that becomes the target object M is, for example, a person (such as a thief) who has invaded the monitoring area. In the present embodiment, a function of imaging the target object M will be described as an example as a predetermined function, but the function is not particularly limited, and a sound is emitted to the target object M or a warning by light emission is given. It may be a function such as going.

The security center 100 and the management device 102 are connected so as to be able to transmit information via an information communication network 110 such as the Internet. Further, the autonomous flight robot 1 and the management device 102 are connected so as to be able to transmit information by wireless communication or the like. The security center 100 communicates with the autonomous flight robot 1 via the management device 102, and receives an image captured by the target object M captured by the autonomous flight robot 1. The security center 100 may have a function of performing image processing on the captured image and issuing a warning to an administrator or the like (not shown) who is monitoring an abnormality at the security center 100. Further, it has a function of receiving information on the position (coordinates) of the target object M from the management device 102 and managing the target object M in association with the captured image captured by the autonomous flight robot 1. May be good.

The management device 102 includes fixed moving object detection sensors 104 (104a, 104b ...) Installed on the ground, a wall surface, or the like, and detects the position of each target object M. The moving object detection sensor 104 can be, for example, a laser sensor. The laser sensor two-dimensionally scans the laser at every angle of a certain angle sample interval to detect an object (obstacle) existing within the detection range in a horizontal plane at a certain height from the ground (or floor surface). The distance information of is acquired as a polar coordinate value. The laser sensor radially scans the exploration signal, which is the laser beam, receives the exploration signal reflected by the object and returns, calculates the distance to the object from the time difference between transmission and reception, and installs the laser sensor. The polar coordinate value of the position of the object is obtained from the coordinates of the above and the direction in which the search signal is transmitted and the calculated distance, and the three-dimensional Cartesian coordinate values (X _t , Y _t , Z _t ) are obtained from the polar coordinate value. The management device 102 transmits the position of the target object M obtained by the moving object detection sensor 104 to the autonomous flight robot 1. When the autonomous flight robot 1 receives the position of the target object M, it calculates its own movement path based on the position and moves along the movement path. The management device 102 tracks the target object M over the detection ranges of the plurality of laser sensors by verifying the identity of the target objects M existing in the regions where the detection ranges of the laser sensors overlap. That is, even if the target object M existing in the detection range of the laser sensor 104a moves to the detection range of the laser sensor 104b, the management device 102 can determine that the target object M is the same object. ..

Hereinafter, the configuration and function of the autonomous flight robot 1 will be described with reference to the overview diagram of FIG. 1 and the functional block diagram of FIG.

As shown in FIG. 1, the autonomous flight robot 1 has four rotors (propellers) 2 (2a to 2d) on one plane. Each rotor 2 is rotated by using a motor 4 (4a to 4d) driven by a battery (secondary battery: not shown). Generally, in a single rotor type helicopter, the azimuth is maintained by canceling the anti-torque generated by the main rotor with the moment generated by the tail rotor. On the other hand, in a quad rotor type helicopter such as the autonomous flight robot 1, anti-torque is canceled by using a rotor 2 that rotates in different directions in the front-rear direction and the left-right direction. Then, by controlling the rotation speeds (fa to fd) of each rotor 2, various movements and postures of the aircraft can be adjusted. For example, when it is desired to rotate the machine body in the yaw direction, the rotation speeds of the front and rear rotors 2a and 2c and the left and right rotors 2d and 2b may be different.

The image pickup unit 3 is composed of, for example, an optical system such as a lens and a two-dimensional image sensor having a two-dimensional array element such as a CCD or CMOS having a predetermined number of pixels (for example, a resolution of 640 × 480 pixels), and is an image captured in a moving space. Is a so-called color camera that acquires images at predetermined time intervals. In the present embodiment, the imaging unit 3 is installed in the housing portion so that its optical axis images the front direction of the autonomous flying robot 1, and is obliquely downward from the horizontal plane (XY plane) by a predetermined depression angle θ. It is installed so that the space is imaged at a viewing angle φ (angle of view). The acquired captured image is output to the control unit 7 described later, stored in the storage unit 8 by the control unit 7, or transmitted to the management device 102 via the communication unit 9 described later.

The distance detection sensor 5 is a sensor that detects the distance between an obstacle existing around the autonomous flight robot 1 and the autonomous flight robot 1 and acquires the relative position of the obstacle existing within the sensor detection range. is there. In the present embodiment, a microwave sensor is provided as the distance detection sensor 5. The microwave sensor emits microwaves into space and detects the reflected waves to detect an object around the autonomous flight robot 1 and obtain the distance to the object. The distance detection sensor 5 can be provided, for example, toward the front of the autonomous flight robot 1 and used to measure the distance to the target object M. Further, the distance detection sensor 5 can be provided, for example, downward on the lower part of the autonomous flight robot 1 and used to measure the distance (altitude) from the ground. Further, the distance detection sensor 5 can be provided, for example, toward the optical axis direction of the imaging unit 3 and can be used to measure the distance to the target object M, which is the imaging object of the imaging unit 3.

The position detection sensor 6 is a sensor for acquiring the current position of the autonomous flight robot 1. The position detection sensor 6 receives, for example, a radio wave (navigation signal) transmitted from a navigation satellite such as a GNSS (Global Navigation Satellite System). The position detection sensor 6 receives navigation signals transmitted from a plurality of navigation satellites and inputs them to the control unit 7. The position detection sensor 6 may use other sensors such as a laser scanner, a gyro sensor, an electronic compass, and a barometric pressure sensor to acquire information for obtaining a self-position by a known technique.

The communication unit 9 is a communication module for wireless communication with the management device 102 by, for example, a wireless LAN or a mobile phone line. In the present embodiment, the captured image acquired by the imaging unit 3 is transmitted to the management device 102 by the communication unit 9, and the captured image is transmitted from the management device 102 to the security center 100, whereby a security guard or the like remotely invades. Allows you to monitor people. Further, the communication unit 9 receives the position (coordinates: X _t , Y _t , Z _t ) of the target object M from the management device 102, thereby enabling the setting of the movement route as described later.

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 and outputs these information to and from the control unit 7. The various data include information used for each process of the control unit 7 such as the target object position 81, change history information 82, voxel information 83, and various parameters 84, output values of each sensor, captured images, and the like.

The target object position 81 is position information (coordinates: X _t , Y _t , Z _t ) of the target object M received from the management device 102. In the present embodiment, the target object position 81 is set to the foot position of the target object M, that is, the position where the target object M is in contact with the ground (floor surface). When the control unit 7 receives the position of the target object M via the communication unit 9, the control unit 7 stores the position of the target object M in the storage unit 8 as the target object position 81. The management device 102 indicates the posture of the target object M (for example, the front direction of the target object M) based on the time change of the position (coordinates: X _t , Y _t , Z _t ) of the target object M. Information) may be estimated and stored as the target object position 81.

The change history information 82 is information representing a change history from the past to the present of the flight speed in which the flight speed of the autonomous flight robot 1 and the time at which the flight speed is estimated are associated with each other. In the present embodiment, each time the flight speed is estimated by the speed estimation means 72 described later, the flight speed and the estimated time are associated with each other and stored in the storage unit 8 as change history information 82.

The voxel information 83 is information representing the structure of obstacles in the moving space by dividing the moving space into a plurality of voxels as the voxel space, which is set in advance by an administrator or the like and stored in the storage unit 8, and will be described later. This is the information updated by the position estimation means 71 so as to be performed. In the present embodiment, the moving space is equally divided into voxels of a predetermined size (for example, 15 cm × 15 cm × 15 cm), the voxel ID which is the identifier of each voxel, and the position (three-dimensional coordinates) of the voxels in the moving space. , The voxel attribute and the voxel cost value are associated with each other and stored as voxel information 83. The voxel corresponding to the no-fly zone defines its voxel attribute as "occupied voxel" to be a space in which the autonomous flight robot 1 cannot move. The no-fly zone includes, for example, an area corresponding to an obstacle such as a building, an area outside the site where flight is permitted, and an area higher than the altitude at which flight is permitted. Then, the voxel attribute of the voxel located in the space existing near the occupied voxel is defined as "proximity voxel", and the voxel attribute of the voxel located in the other freely flyable area is defined as "free voxel".

A voxel cost value indicating the degree of occupancy is associatedly registered with each voxel so that it can be used when a movement route is generated by the route search means 73 described later. The voxel cost value takes the maximum value C _max in the occupied voxel, and is set so as to become smaller as the distance from the occupied voxel increases. For example, the voxel cost value of a certain voxel (evaluation voxel) is preferably calculated by the formula of voxel cost value = C _max × exp {−λ × R}. Here, λ is a parameter obtained by experiment, and R is the distance from the occupied voxel closest to the evaluation voxel. Then, a voxel having a voxel cost value equal to or higher than a predetermined threshold value is referred to as a "proximity voxel". Further, a voxel whose voxel cost value is smaller than the threshold value is set as a "free voxel", and a voxel cost value of a voxel regarded as a free voxel is set to 0.

Further, the storage unit 8 also stores the separation distance and the like as various parameters 84. The separation distance is a relative distance between the autonomous flight robot 1 and the target object M that should be maintained in the horizontal plane when flying following the target object M. The separation distance is preset by the administrator of the autonomous flight robot 1 or the like. When monitoring a predetermined target object M using the autonomous flight robot 1, it is necessary to be able to approach the target object M and acquire a more detailed captured image. However, in order to prevent being attacked by a hostile target object M such as an intruder, it is necessary to keep a certain distance or more. Therefore, the autonomous flight robot 1 of the present embodiment can acquire a detailed captured image of the target object M, and keeps the separation distance in advance at a distance that is less likely to be attacked by the target object M. It is controlled to follow the flight. The separation distance is set to, for example, 3 m. In addition to the separation distance, conditions and formulas for determining the vibration state such as the number-of-times threshold value and the amplitude threshold value, which will be described later, are also stored in the storage unit 8 as various parameters 84.

The control unit 7 is composed of a computer equipped with a CPU or the like, and is a position estimation means as a series of processes for performing position estimation processing, speed estimation processing, route search processing, vibration state detection processing, route tracking control, flight stop control, and the like. The speed estimation means 72, the route search means 73, the movement control means 74, and the vibration state detecting means 75 are included.

The position estimation means 71 performs a position estimation process for estimating the self-position, which is the current position of the autonomous flight robot 1 in the moving space, based on the output of the position detection sensor 6.

Specifically, the coordinates of the self-position (X) from the latitude / longitude estimated by a well-known technique based on the signals from a plurality of navigation satellites obtained from the position detection sensor 6 and the altitude obtained from the distance detection sensor 5. _s , Y _s , Z _s ) is calculated. Further, the attitude YAW is obtained as the self-position by receiving the output from the position detection sensor 6 such as the electronic compass and the gyro sensor. The self-position estimation method is not limited to this, and the current position of the autonomous flight robot 1 may be estimated by using another method.

The position estimating means 71 receives the estimated self-position (coordinates: X _s , Y _s , Z _s and attitude YAW) and the position of the target object M received from the management device 102 (coordinates: X _t , Y _t , Z _t). ) Is output to the route search means 73. Further, the position estimation means 71 outputs the estimated self-position to the speed estimation means 72 and the movement control means 74.

The position estimation means 71 performs a process of updating the voxel information 83 based on the position of the target object M. Specifically, a cylindrical model having a size substantially the same as the size of the target object M predetermined in the voxel space based on the voxel information 83 of the storage unit 8 with the position of the target object M as the center of gravity (for example, the monitoring target). Assuming that the target object M is an intruder, voxel information is provided by arranging a cylindrical model with a bottom radius of 0.3 m and a height of 1.7 m) and setting a voxel that interferes with the cylindrical model as an occupied voxel. Update 83. As will be described later, the autonomous flight robot 1 is flight-controlled so as not to move to the occupied voxel. However, by updating the voxel information 83 based on the position of the target object M as described above, the autonomous flight robot 1 It is possible to avoid contact between 1 and the target object M.

Since the speed estimation means 72 is used for movement control in the movement control means 74, which will be described later, the speed estimation means 72 performs a speed estimation process for estimating the current flight speed (v _x , v _y , v _z , v _yaw ) of the autonomous flight robot 1. .. In the present embodiment, the flight speed is obtained from the time change of the self-position (coordinates: X _s , Y _s , Z _s and attitude YAW) estimated by the position estimation means 71. At this time, it is preferable to estimate the flight speed by using an extended Kalman filter in consideration of the influence of measurement error and the like. Further, the estimated flight speed is a vector value considering not only the magnitude but also the direction of the speed, and has positive and negative signs according to the direction. For example, the flight speeds in the north and east directions are set as positive, and the flight speeds in the south and west directions are set as negative. In the present embodiment, the flight speed is obtained from the time change of the self-position, but the flight speed is not limited to this, and a speed estimation method using the Doppler effect in GNSS may be used. When the flight speed is estimated, the speed estimation means 72 sequentially stores it in the storage unit 8 as change history information 82 in association with the estimated time. Further, the speed estimation means 72 outputs the estimated flight speed to the movement control means 74.

The route search means 73 uses the self-position estimated by the position estimation means 71, the moving object position 81 stored in the storage unit 8, and various information to obtain a movement route from the self-position to a predetermined movement target position. Performs the route search process to be calculated.

In the route search process, first, a process of setting a movement candidate position at a position separated from the target object position 81 by a distance is performed. That is, a plurality of movement candidate positions are set in the surrounding area separated by the distance from the target object position 81. In the present embodiment, the movement candidate position is a circular position centered on the target object position 81 and the radius is a preset separation distance, and the flight altitude (for example,) predetermined by the administrator or the like. Set in the area of 3m) so that they are evenly spaced.

Next, in the route search process, each of the set movement candidate positions is evaluated, and the movement candidate position having the highest evaluation value is set as the movement target position. At this time, first, in order to prevent the autonomous flight robot 1 from coming into contact with an obstacle, the lowest evaluation is made so that the movement target position is not set at the position included in the occupied voxel or the proximity voxel with reference to the voxel information 83. Make it a value. Further, the smaller the distance from the self-position of the autonomous flight robot 1 to each movement candidate position, the higher the evaluation value. Further, the smaller the distance from the previously set movement target position to the movement candidate position, the higher the evaluation value.

Next, in the route search process, a movement route generation process for calculating the movement route of the autonomous flight robot 1 is performed using the self-position estimated by the position estimation means 71 and the set movement target position. Specifically, in the movement route generation process, first, the voxel information 82 is read from the storage unit 8, and the center of each voxel of the free voxel and the proximity voxel, which are divided movable spaces, is set as a node, and the node is adjacent to the node. A graph structure is generated using a line segment connecting nodes as an edge. At this time, it is assumed that the distance cost obtained based on the distance between adjacent nodes is set as the edge weight. After generating a graph structure for all movable voxels based on the voxel information 82 in this way, the movement from the voxel node in which the autonomous flight robot 1 exists to the voxel node in which the movement target position exists. Generate a route. Various route generation methods can be applied to the movement route generation method, but in the present embodiment, the movement route is searched by using the A * (Aster) route search method. At this time, the voxel cost and the distance cost stored as the voxel information 82 are used as the movement cost in the A * route search method. The data of the movement route generated by the route search means 73 is a set data of waypoints (x, y, z), and this information is temporarily stored in the storage unit 8.

The vibration state detecting means 75 uses the change history information 82 indicating the past change history of the flight speed estimated by the speed estimating means 72, and the current flight state is a vibration state in which autonomous flight control does not function. Performs vibration state detection processing to detect whether or not. In the vibration state detection process, the vibration state is detected based on whether or not the flight speed repeats code reversal in the past period from the current time to a predetermined time ago. Hereinafter, an example of the flow of the vibration state detection process executed by the vibration state detection means 75 according to the autonomous flight robot 1 of the present embodiment will be described in detail with reference to FIG. The vibration state detection process is performed at a preset timing (for example, every 5 seconds).

In the vibration state detection process, first, a past period, which is a period for determining whether or not the vibration state is present, is set (ST1). In the present embodiment, the larger the amplitude value of the current latest flight speed with reference to the change history information 82, the longer the past period is set. 5 and 6 are graphs showing changes in flight speed, FIG. 5 is a diagram showing changes in which the amplitude value of flight speed tends to increase, and FIG. 6 is a diagram showing changes in flight speed amplitude value. It is a figure which showed the change which is declining. In FIG. 5, when the current time is t', the vibration state detecting means 75 reads the change history of the flight speed in the past period in the past by a predetermined time from the current time with reference to the change history information 82, and the latest flight. The amplitude value a3 of the velocity is obtained. Then, for example, it is preferable to calculate the length T of the past period by the formula of T = α × a3 + T ₀ based on the amplitude value a3 of the latest flight speed. Here, α is a parameter obtained by the experiment, and T ₀ is the period length set by the experiment as the reference length of the past period. In FIG. 6, when the current time is t'', the amplitude value of the latest flight speed is b3, which is larger than the latest amplitude value a3 of the time t'(b3> a3). The period length T'indicates that the period length (T'> T) longer than the past period length T is set.

Next, in the vibration state detection process, the change history information 82 is referred to, the number of changes in the flight speed code in the past period set in ST1 from the current time is counted, and the number of changes in the code is a preset number threshold value. It is determined whether or not it is the above (ST2). In the present embodiment, the following will be described with the number-of-times threshold value being three times. In FIG. 5, when the current time is t', the vibration state detecting means 75 reads out the change history of the flight speed in the past period T from the change history information 82. Then, the number of changes in the flight speed record code in the past period T is counted. In FIG. 5, during the past period T, the flight speed code has changed three times, t1, t2, and t3, which is equal to or greater than the number threshold value (ST2-Yes), so the process proceeds to ST3. .. Similarly, in the case of FIG. 6, the code of the flight speed has changed three times during the past period T', which is t1', t2', and t3', which is equal to or greater than the number threshold value (ST2-. Yes), proceed to ST3. On the other hand, when the number of changes in the code of the flight speed is not equal to or greater than the number threshold value (ST2-No), it is determined that the flight speed is not currently in the vibration state, and the vibration state detection process is terminated.

Next, in the vibration state detection process, the average value of the amplitudes obtained in ST2 is obtained, and it is determined whether or not the average value is equal to or higher than the preset amplitude threshold value (ST3). When the amplitude threshold is Th1, in the example of FIG. 5, the average value A of the amplitude is obtained by A = (a1 + a2 + a3) / 3, and it is determined whether or not the obtained average value A is Th1 or more. When the obtained average value is equal to or higher than the preset amplitude threshold Th1 (ST3-Yes), the process proceeds to ST4. On the other hand, when the average value is not equal to or higher than the preset amplitude threshold value (ST3-No), it is determined that the vibration state is not present and the vibration state detection process is terminated. In this way, by determining the amplitude value in the past period as a threshold value, it is possible to prevent erroneous determination of vibrational movement, which can occur even in normal flight control, as an abnormal vibration state. In the present embodiment, the above determination is made by comparing the average value A of the amplitudes obtained in ST2 with the amplitude threshold Th1 in ST3, but the above determination is not limited to this, and the amplitude value obtained in ST2 is not limited to this. The above determination may be made based on whether or not all of the above are equal to or higher than the amplitude threshold Th1.

Next, in the vibration state detection process, it is determined whether or not the change in the amplitude value in the past period tends to decrease (ST4). In the example of FIG. 5, the change in the amplitude value in the past period T increases as time advances to a1, a2, and a3, and it is determined that there is no decreasing tendency (ST4-No), and the process proceeds to ST7. .. On the other hand, in the example of FIG. 6, the change in the amplitude value in the past period T'becomes small as time progresses to b1, b2, and b3, and it is determined that the amplitude is decreasing (ST4-Yes). Proceed to ST5.

When it is determined in ST4 that the change in the amplitude value tends to decrease (ST4-Yes), the vibration state detection process waits for a predetermined determination time (ST5). Then, it is determined whether or not the amplitude value immediately after the determination time has elapsed is equal to or greater than the amplitude threshold Th1 (ST6). In the example of FIG. 6, the amplitude value b6 immediately after the time t ″ ″ when the determination time T ″ has elapsed is compared with the amplitude threshold value Th1. When the amplitude value immediately after the determination time has elapsed is not equal to or higher than the amplitude threshold Th1 (ST6-No), it is determined that the vibration state is not present and the vibration state detection process is terminated. On the other hand, when the amplitude value immediately after the time when the determination time has elapsed is equal to or higher than the amplitude threshold Th1 (ST6-Yes), the process proceeds to ST7. In the present embodiment, in ST6, it is determined whether or not the amplitude value immediately after the determination time has elapsed is equal to or higher than the amplitude threshold Th1. However, the present invention is not limited to this, and the amplitude value within the determination time is determined. The above determination may be made based on whether or not the average value of is equal to or higher than the amplitude threshold Th1. Further, in the present embodiment, the determination time is set to a preset value, but the determination time is not limited to this, and the time until an amplitude value of a predetermined number of times (for example, 3 times) or more is obtained may be used as the determination time. For example, in the example of FIG. 6, the time from the current time t = t'' to the determination of the third amplitude value b6 is set as the determination time, and the b6 is compared with the amplitude threshold Th1 to perform the above determination. You may.

In the vibration state detection process, when it is determined that the change in the amplitude value in the past period does not tend to decrease (ST4-No), or when the amplitude value after the elapse of the determination time is equal to or greater than the amplitude threshold Th1 (). ST6-Yes), it is determined that the current flight state is a vibration state in which autonomous flight control does not work (ST7). When it is determined that the vehicle is in a vibration state, the vibration state detecting means 75 outputs information indicating that the vibration state is in the movement control means 74, and the flight speed (absolute value) is the minimum based on the change history information 82. (That is, the time length at which the flight speed is minimized with respect to the present) is obtained, and the timing value representing the timing is output to the movement control means 74. For example, when the current flight speed is exactly zero, "0" is output as a timing value to the movement control means 74. Further, with reference to the change history information 82, the future time at which the flight speed (absolute value) becomes the minimum (for example, the flight speed is zero) is estimated based on the amplitude value and the period (vibration frequency) of the change in the flight speed. Then, the time length from the current time to the future time is output as a timing value (for example, "0.5 seconds later"). When the timing value is output to the movement control means 74, the vibration state detection process ends.

The movement control means 74 (flight control means in the present invention) combines the movement path 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. By using this, the autonomous flight robot 1 performs path tracking control so as to fly along the movement path calculated by the route search means 73. Specifically, the control command value, which is the flight control value at each time, is obtained using the movement path, the self-position, and the flight speed, the motor 4 is controlled based on the control command value, and the rotation speed of the rotor 2 is determined. Control. Further, when the vibration state detecting means 75 determines that the movement control means 74 is in a vibration state, the movement control means 74 controls the motor 4 based on the timing value received from the vibration state detecting means 75, and the rotor 2 controls the motor 4 to stop the flight. Flight stop control is performed to control the number of revolutions of.

In the route tracking control, first, a process of calculating the nearest position (hereinafter, referred to as “local target”) to be targeted by the autonomous flight robot 1 at each time is performed. FIG. 7 is a diagram illustrating the calculation of the local target. In calculating the local target, the movement 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 flight robot 1 is aiming at at the current time and the way that has passed last time. The straight line W connecting the two points with the point Wp0 is obtained. Then, the movement control means 74 calculates the intersection points Lp'and Lp between the obtained straight line W and the sphere S centered on the self-position of the autonomous flight robot 1, and sets the intersection point Lp close to the target waypoint Wp1 as a local target. Ask as. In this way, by controlling the flight so that the autonomous flight robot 1 moves toward the local target at each time, the local target always moves on the movement path toward the movement target position Po, and the autonomous flight robot 1 always moves. Will fly along the movement path.

Next, in the route tracking control, the process of calculating the control command values u _x , u _y , u _z , u _ψ for each direction of the X, Y, Z, and yaw angles so as to fly toward the calculated local target is performed. Do. At this time, a control command value is obtained so that the difference between the current self position and the local target position becomes small. Speed Specifically, the control command value XYZ axis _{u = (u x, u y} , u z) , the self-position r = obtained by the position estimation means _{71 (X s, Y s,} Z s) and It is obtained by PID control using the velocity v = (v _x , v _y , v _z ) estimated by the estimation means 72. Each control command value of the XYZ-axis direction _{u = (u x, u y} , u z), the local target r '= (x, y, z) when the control command value is, u = Kp (r' -R) + Kd · v + Ki · e is calculated. Here, Kp, Kd, Ki is that the gain of the PID control, _{respectively, e = (e x, e} y, e z) is the integral value of the error. On the other hand, for the control command value u _ψ in the yaw angle direction, ψ'is the target angle, ψ is the attitude (angle) of the autonomous flying robot 1 estimated by the position estimation means 71, and v _yaw is the angular velocity estimated by the speed estimation means 72. Then, it is obtained by PD control such as the equation u _ψ = Kp (ψ'−ψ) + Kd · v _yaw . In the present embodiment, the target angle ψ'is set as the direction of the target object M, that is, the angle facing the direction of the position of the target object M.

In the flight stop control, the rotation of the motor 4 is stopped based on the timing value received from the vibration state detecting means 75, and the rotation of the rotor 2 is stopped. That is, the rotation of the motor 4 is stopped after a period corresponding to the timing value elapses with respect to the current time. In the present embodiment, the flight is stopped (emergency landing) by suddenly stopping the rotation of the motor 4 based on the timing value received from the vibration state detecting means 75, but the timing value is not limited to this. Based on this, the flight may be stopped by gradually reducing the rotation speed of the motor 4.

As described above, since the autonomous flight robot 1 in the present embodiment repeats the code reversal of the flight speed based on the change history of the flight speed in the predetermined past period by the vibration state detecting means 75, the vibration state. When the vibration state detecting means 75 detects that the flight is in a vibrating state, the movement control means 74 stops the flight (emergency landing) at the timing when the flight speed becomes the minimum. Therefore, the autonomous flight robot 1 in the present embodiment can be stopped at the timing when the flight speed becomes the minimum when the current flight state is a vibration state in which the autonomous flight control does not function, and it is quick and safe. It will be possible to make an emergency landing. That is, since the flight speed is the minimum, the inertial movement at the time of landing (fall) can be minimized, and the fall distance can be minimized, so that the possibility of collision with a surrounding object can be reduced. Further, since the kinetic energy at the time of landing (falling) can be minimized, the damage to the surrounding objects and the autonomous flight robot 1 itself at the time of collision can be minimized.

Further, the autonomous flight robot 1 in the present embodiment determines that the vibration state is determined by the vibration state detecting means 75 when the code of the flight speed has changed by the number of times threshold value or more in a predetermined past period. In this way, by detecting the vibration state by the change in the flight speed, the vibration state can be detected easily and surely.

Further, in the autonomous flight robot 1 of the present embodiment, the vibration state detecting means 75 sets a past period so that the larger the amplitude value of the nearest (immediately before) from the current time, the longer the period, and the flight in the past period It is determined whether or not the vehicle is in a vibrating state based on the change in speed. That is, when the autonomous flight robot 1 is in a vibrating state, the larger the amplitude value immediately before, the longer the period of vibration tends to be. Therefore, the larger the latest amplitude value is, the longer the past period is, so that an appropriate past period can be set according to the vibration situation, and the vibration detection accuracy can be improved.

Further, the autonomous flight robot 1 in the present embodiment is determined from the present when the sign of the velocity has changed by the number of times threshold value or more in the past period and the maximum value of the amplitude value in the past period tends to decrease with the passage of time. Judgment is suspended for the determination time, and if the amplitude value after the elapse of the determination time is larger than the amplitude threshold value, it is determined to be in the vibration state. As a result, even if the sign of the flight speed in the past period is equal to or higher than the number threshold value, if the vibration movement tends to converge (the amplitude value tends to decrease), it is not regarded as a vibration state. Therefore, even if the flight stop control does not land, it is possible to observe the situation until the flight control is restored, and it is possible to quickly return to the normal flight.

By the way, the present invention is not limited to the above-described embodiment, and may be further implemented in various different embodiments within the scope of the technical idea described in the claims. Moreover, the effect described in the embodiment is not limited to this.

In the above embodiment, the speed estimating means 72 obtains the flight speed obtained from the time change of the self-position which is the measured value of the flight state in the present invention, and the vibration state detecting means 75 obtains the past change history of the flight speed. Based on the change history information 82 shown, the vibration state is detected from the repeated sign reversal of the flight speed in the past period. However, not limited to this, as the measured value of the flight state, for example, the value obtained by integrating the measured values by the separately provided acceleration sensor or gyro sensor is obtained as the flight speed, and the vibration state in which the flight speed repeats code reversal. May be detected. As a result, the time change of the attitude of the autonomous flight robot 1 can also be obtained as the flight speed, and the vibration state of the attitude can be detected.

Further, the movement control means 74 in the above embodiment performs flight stop control for stopping the rotation of the motor 4 based on the timing value output by the vibration state detecting means 75. However, not limited to this, by referring to the voxel information 83 (obstacle information in the present invention), an obstacle other than the ground (occupied voxel other than the ground) is below the self-position at the timing when the rotation of the motor 4 is stopped. It may be determined whether or not there is an obstacle, and flight stop control may be performed to stop the rotation of the motor 4 when there is no obstacle. Alternatively, by referring to the target object position 81, flight stop control may be performed to stop the rotation of the motor 4 when the target object M does not exist below the self-position. As a result, when an obstacle such as a person or an object exists below the autonomous flight robot 1, the flight is not stopped (falls), so that the vibration state can be eliminated and landing can be performed at a safer timing.

Further, in the above embodiment, the vibration state detecting means 75 detects the vibration state using a preset number-of-times threshold value and an amplitude threshold value. However, the present invention is not limited to this, and at least one of these threshold values may be changed based on the position change of the movement target position. Specifically, when the movement target position changes significantly, the number-of-times threshold value and the amplitude threshold value are changed to a large value, and conversely, when the change in the movement target position is small, the number-of-times threshold value and the amplitude threshold value are changed to a small value. That is, when the moving target position is significantly changed, there is a high possibility that the change is likely to occur in the vibration state. Therefore, by tightening the conditions for determining the vibration state, it is possible to reduce the false detection of the vibration state. On the contrary, when the change in the moving target position is small, the loss detection of the vibration state can be reduced by relaxing the condition for determining the vibration state. Therefore, the accuracy of detecting the vibration state can be improved.

Further, in the above embodiment, when the movement control means 74 outputs the vibration state and the timing value for stopping the rotation of the motor 4 from the vibration state detecting means 75, the motor 4 is based on the timing value. Stop the rotation of. However, not limited to this, when the vibration state detecting means 75 outputs that the vibration state is in the movement control means 74, the self-position and the self-position at the time indicated by the timing value with respect to the route search means 73 The landing position immediately below may be output, and the route search means 73 may be made to obtain a movement route (landing route) from the self-position to the landing position.

Further, in the above embodiment, the target object M is detected by using the moving object detection sensor 104 connected to the management device 102. However, the present invention is not limited to this, and the position of the target object M may be estimated by performing image analysis on the captured image acquired by the imaging unit 3. For example, each frame of the captured image is image-processed to extract an image region of the target object M. At this time, an optical flow method which is a known technique (see Japanese Patent Application Laid-Open No. 2006-146551), a classifier by boosting learning (for example, a face detection method using an AdaBoost-based classifier using the Har-like feature), etc. Is used to extract the image area (person area) of the target object M. Next, the distance between the target object M and the autonomous flight robot 1 is estimated based on the position of the extracted image region. Specifically, a correspondence table between the y-coordinate position (in the captured image) of the crown of the image region of the extracted target object M and the distance is prepared in advance for each flight altitude, and the current flight altitude and the target object are created. The distance to the autonomous flight robot 1 is estimated by comparing the y-coordinate position of the top of the object M with the correspondence table. However, the distance is not limited to this, and the distance may be calculated from the size of the head of the extracted target object M. That is, a correspondence table between the size of the head and the distance is created in advance, and the distance to the autonomous flight robot 1 is estimated by comparing the size of the head of the extracted target object M with the correspondence table. You may.

Further, the autonomous flight robot 1 is equipped with a distance image sensor as an imaging unit 3 instead of a color camera, and the target object M is extracted by a known moving object extraction technique using the distance image acquired from the distance image sensor. Then, the position of the target object M may be estimated from the distance value between the extracted target object M and the autonomous flying robot 1 and the self-position. Alternatively, the autonomous flight robot 1 may be equipped with a laser scanner, and the position of the target object M may be estimated using the output value of the laser scanner and its own position.

Further, in the above embodiment, the control unit 7 performs a series of processes of position estimation processing, speed estimation processing, movement route generation processing, and route tracking control. However, the present invention is not limited to this, and a control PC (not shown) may be prepared and the PC may be allowed to perform a series of these processes. That is, the autonomous flight robot 1 receives the control command value obtained by the position estimation process, the speed estimation process, the movement route generation process, and the route tracking control performed by the PC from the PC by wireless communication or wired communication, and controls the control. By controlling the rotation speed of the motor 4 based on the command value, the robot may fly to a target position. In this way, by sharing the above series of processes using the external PC, the CPU processing load of the autonomous flight robot 1 can be reduced, and eventually the battery consumption can be suppressed.

1 ... Autonomous flight robot 2 ... Rotor 3 ... Imaging unit 4 ... Motor 5 ... Distance detection sensor 6 ... Position detection sensor 7 ... Control unit 8 ... Storage unit 9・ ・ ・ Communication unit 71 ・ ・ ・ Position estimation means 72 ・ ・ ・ Velocity estimation means 73 ・ ・ ・ Route search means 74 ・ ・ ・ Movement control means 75 ・ ・ ・ Vibration state detection means 81 ・ ・ ・ Target object position 82・ ・ ・ Change history information 83 ・ ・ ・ Boxel information 84 ・ ・ ・ Various parameters 100 ・ ・ ・ Security center 102 ・ ・ ・ Management device 104 ・ ・ ・ Moving object detection sensor 110 ・ ・ ・ Information communication network M ・ ・ ・ Target Object

Claims (6)

An autonomous flying robot that autonomously flies in a moving space.
A speed estimation means for estimating the flight speed from the measured values obtained by measuring the flight state of the autonomous flight robot, and
A vibration state detecting means for detecting a vibration state in which the flight speed repeats code reversal based on a history of changes in the flight speed in the past period from the current time to a predetermined time ago.
When the vibration state is detected by the vibration state detecting means, the flight control means for stopping the flight at the timing when the flight speed becomes the minimum, and the flight control means.
An autonomous flying robot characterized by having.
The autonomous flight robot according to claim 1, wherein the vibration state detecting means determines the vibration state when the code of the flight speed in the past period changes by a predetermined number of times or more.
The autonomous flight robot according to any one of claims 1 or 2, wherein the vibration state detecting means sets the past period longer as the amplitude value of the flight speed in the immediate vicinity of the current time is larger.
The vibration state detecting means has a predetermined determination time from the current time when the sign of the flight speed has changed by a predetermined number of times threshold value or more in the past period and the amplitude value of the flight speed tends to decrease with the passage of time. The autonomous flying robot according to any one of claims 1 to 3, wherein if the amplitude value after the lapse is larger than a predetermined amplitude threshold value, the vibration state is determined.
The obstacle information indicating the position of the obstacle existing in the moving space is stored, and the obstacle information is stored.
Further provided with a self-position estimation unit that estimates the self-position of the autonomous flying robot,
The autonomous flight robot according to any one of claims 1 to 4, wherein the flight control means uses the obstacle information to execute the flight stop when there is no obstacle below the self-position.
Further having a route search means for setting a movement target position in the flight space and finding a flight path from its own position to the movement target position.
The flight control means controls to fly along the flight path and controls the flight.
The autonomous state according to claim 2 , wherein the vibration state detecting means changes the number-of-times threshold value to a large value when the change in the movement target position is large, and changes the number-time threshold value to a small value when the change in the movement target position is small. Flying robot.

Images