JP6900029B2 - Unmanned aerial vehicle, position estimation device, flight control device, position estimation method, control method and program - Google Patents

Description

The present invention relates to an unmanned aerial vehicle, particularly an unmanned aerial vehicle that performs self-position estimation, a position estimation device, a flight control device, a position estimation method, a control method, and a program of the unmanned aerial vehicle.

Conventionally, an unmanned aerial vehicle either flies by the operator transmitting a control signal from a control transmitter on the ground to an unmanned aerial vehicle in the sky and maneuvering, or autonomously flies according to a flight plan by installing an autonomous control device. Was there.

In recent years, various autonomous control devices for autonomously flying unmanned aerial vehicles, including fixed-wing aircraft and rotary-wing aircraft, have been developed. In Patent Documents 1 and 2, a sensor that detects the position, attitude, altitude, and orientation of the small unmanned helicopter, a main calculation unit that calculates a control command value to a servomotor that moves the steering wheel of the small unmanned helicopter, and a sensor are used. An autonomous control device has been proposed in which a sub-calculation unit that collects data and converts the calculation result of the main calculation unit into a pulse signal to a servomotor is assembled in one small frame box.

Japanese Unexamined Patent Publication No. 2004-256020 Japanese Unexamined Patent Publication No. 2004-256022

The unmanned aerial vehicle includes a GPS sensor that acquires position information and a barometric pressure sensor that acquires altitude information. However, when an unmanned aerial vehicle flies under the erection part of a structure having an erection part such as below the bridge, the position accuracy acquired by GPS is not sufficient, and when there is a river under the bridge, the barometric pressure sensor acquires it. The high degree of accuracy is not sufficient. In particular, when inspecting the lower surface of a bridge using an unmanned aerial vehicle, there is a problem that it is difficult to control the flight of the unmanned aerial vehicle while maintaining a certain distance from the lower surface of the bridge. Although it is possible to control an unmanned aerial vehicle by transmitting control information from the outside, it is desirable that the flight of the unmanned aerial vehicle is ultimately controlled autonomously. In addition, there is a problem that it is difficult to visually confirm and maneuver inside a structure such as below a bridge.

Therefore, the applicant has developed a technique for estimating the position (altitude) of an unmanned aerial vehicle by using SLAM (Simultaneous Localization And Mapping) technology using a laser when controlling the flight of the unmanned aerial vehicle below a bridge. Thought. However, when using reflections from the ground, it is difficult to make correct measurements using reflections from rivers, and when using reflections from the underside of a bridge, it suddenly rises or flies when it deviates from the underside of the bridge. There was a problem that it became unstable.

The present invention has been made to solve such a problem, and the position of the unmanned aerial vehicle is determined in order to control the flight of the unmanned aerial vehicle under the erection portion of the structure having the erection portion such as below the bridge. The main purpose is to provide a device or the like capable of estimating.

In order to achieve the above object, the position estimation device as one aspect of the present invention includes an environment acquisition unit that acquires the position of the unmanned aerial vehicle by using a global sensor for measuring the absolute position of the unmanned aerial vehicle, and the above. The relative position of the unmanned aerial vehicle with respect to the obstacles around the unmanned aerial vehicle is estimated using a local sensor for measuring the relative position of the unmanned aerial vehicle, and the estimated relative position is based on the estimated likelihood. The self-position estimation unit that outputs the relative position of the unmanned aerial vehicle when it is determined to be valid, and the self-position estimation unit that outputs the relative position of the unmanned aerial vehicle, if the relative position of the unmanned aerial vehicle is output, based on the relative position. Moreover, when the position is acquired by the environment acquisition unit, it is characterized by including an estimator for estimating the position and speed of the unmanned aerial vehicle based on the position.

Further, preferably in the present invention, the self-position estimation unit outputs the relative position of the unmanned aerial vehicle when the distance measured by the local sensor is within a predetermined distance.

Further, preferably in the present invention, the self-position estimation unit estimates the relative position of the unmanned aerial vehicle based on the point cloud data acquired by using a laser, and the estimation is based on the likelihood of the estimation. When it is determined that the relative position is valid, the relative position of the unmanned aerial vehicle is output.

Further, preferably in the present invention, the self-position estimation unit estimates the relative position of the unmanned aerial vehicle by using SLAM technology.

Further, preferably in the present invention, the self-position estimation unit creates a map using SLAM technology, and the relative position estimated based on the degree of agreement between the created map and the acquired point cloud data. Is valid or not.

Further, in the present invention, preferably, the environment acquisition unit acquires altitude by using a barometric pressure sensor.

Further, in the present invention, the position is preferably altitude, and the speed is a vertical speed.

Further, preferably, in the present invention, the estimator corrects the speed of the unmanned aircraft and the speed based on the relative position when the relative position of the unmanned aircraft is output by the self-position estimation unit. The first estimator that estimates and outputs the acceleration bias value of, and when the speed and the acceleration bias value are output, based on the output speed and the acceleration bias value, and by the environment acquisition unit, the above. When the position is acquired, the position and speed of the unmanned aircraft estimated by the estimator include a second estimator that estimates and outputs the position and speed of the unmanned aircraft based on the position. Is the position and speed output by the second estimator.

Further, preferably, in the present invention, when the relative position of the unmanned aerial vehicle is output by the self-position estimation unit, the first estimator uses the relative position as an observation value to obtain the speed of the unmanned aerial vehicle and the speed of the unmanned aerial vehicle. It is a Kalman filter that estimates and outputs the acceleration bias value.

Further, preferably in the present invention, the first estimator is a state equation expressing the relationship between the current state vector and the past state vector including the velocity, the position, and the acceleration bias value as components, and the observation. The velocity and acceleration bias values of the unmanned aircraft are estimated based on the observation equation representing the relationship between the values and the state vector.

Further, preferably in the present invention, when the speed and the acceleration bias value are output, the second estimator uses the output speed and the acceleration bias value as observation values, and the environment acquisition unit uses the output. When the position is acquired, the Kalman filter estimates and outputs the position and speed of the unmanned aircraft by using the position as an observation value.

Further, preferably in the present invention, the second estimator preferably has a state equation expressing the relationship between the current state vector and the past state vector including the velocity, the position, and the acceleration bias value as components, and the observation. The position and velocity of the unmanned aircraft are estimated based on the observation equation representing the relationship between the value and the state vector.

The second estimator sets the likelihood of the velocity and the acceleration bias value output from the first estimator to be larger than the likelihood of the position acquired by the environment acquisition unit. The position and speed of the unmanned aerial vehicle are estimated and output.

Further, preferably in the present invention, the environment acquisition unit further acquires the acceleration by using the acceleration sensor, and the first estimator and the second estimator control the acceleration acquired from the environment acquisition unit. Used as input.

Further, in order to achieve the above object, the flight control device as one aspect of the present invention is a flight control device for controlling the flight of an unmanned aerial vehicle provided with the above position estimation device, and is estimated by the above estimator. It is characterized in that the flight of the unmanned aerial vehicle is controlled by using the position and speed of the unmanned aerial vehicle.

Further, in the present invention, preferably, the distance to the obstacle is determined using the relative position estimated by the self-position estimation unit, and the position and speed of the unmanned aerial vehicle estimated by the estimator are used. The flight of the unmanned aerial vehicle is controlled so as to keep the determined distance above a certain level and avoid a collision.

Further, preferably, in the present invention, when the unmanned aerial vehicle flies below the erection portion of the structure having the erection portion, the position and speed of the unmanned aerial vehicle estimated by the estimator are used to obtain the erection portion. The flight of the unmanned aerial vehicle is controlled so as to keep the distance from the lower surface constant.

Further, in order to achieve the above object, the unmanned aerial vehicle as one aspect of the present invention is characterized by including the above flight control device.

Further, in order to achieve the above object, the position estimation method as one aspect of the present invention is a method of estimating the position of the unmanned aerial vehicle, and uses a sensor for measuring the absolute position of the unmanned aerial vehicle. A step of acquiring the position of the unmanned aerial vehicle and a step of estimating the relative position of the unmanned aerial vehicle using a sensor for measuring the relative position of the unmanned aerial vehicle. The step of estimating the relative position of the unmanned aerial vehicle when it is determined that the relative position is valid, and the position obtained based on the relative position when the relative position of the unmanned aerial vehicle is estimated. If so, it is characterized by including a step of estimating the position and speed of the unmanned aerial vehicle based on the position.

Further, preferably in the present invention, the step of estimating the position and speed of the unmanned aircraft corrects the speed and speed of the unmanned aircraft based on the relative position when the relative position of the unmanned aircraft is output. The first estimation step of estimating and outputting the acceleration bias value for the calculation, and when the speed and the acceleration bias value are output, based on the output speed and the acceleration bias value, and the environment acquisition unit. When the position is acquired by the above, the position and speed of the unmanned aircraft are estimated and output based on the position, and the second estimation step is included, and the unmanned aircraft estimated by the estimation step is included. The position and speed are the positions and speeds output by the second estimation step.

Further, preferably in the present invention, in the first estimation step, when the relative position of the unmanned aerial vehicle is output, the Kalman filter process is executed using the relative position as an observed value, thereby performing the unmanned aerial vehicle. Estimates and outputs the speed and acceleration bias values of.

Further, preferably in the present invention, in the second estimation step, when the speed and the acceleration bias value are output, the output speed and the acceleration bias value are used as observation values, and the environment acquisition unit uses the output. When the above position is acquired, the position and speed of the unmanned aircraft are estimated and output by executing the Kalman filter processing using the position as an observed value.

In the second estimation step, the likelihood of the velocity and the acceleration bias value output from the first estimation step is set to be larger than the likelihood of the position acquired by the environment acquisition unit. The position and speed of the unmanned aerial vehicle are estimated and output.

Further, in order to achieve the above object, the position estimation program as one aspect of the present invention is a program for estimating the position of an unmanned aerial vehicle, and causes a computer to execute each step of the above position estimation method. It is a feature.

Further, in order to achieve the above object, the control method as one aspect of the present invention is a method of controlling the flight of an unmanned aerial vehicle having a method of estimating the position of the unmanned aerial vehicle, and by the above-mentioned estimation step. It is characterized by including a step of controlling the flight of the unmanned aerial vehicle using the estimated position and speed of the unmanned aerial vehicle.

Further, in order to achieve the above object, the control program as one aspect of the present invention is a program for controlling the flight of an unmanned aerial vehicle, and is characterized in that each step of the above control method is executed by a computer. To do.

According to the present invention, the position of the unmanned aerial vehicle can be estimated in order to control the flight of the unmanned aerial vehicle under the erection portion of the structure having the erection portion such as below the bridge.

It is an external view of the unmanned aerial vehicle according to one Embodiment of this invention. It is a hardware block diagram of the unmanned aerial vehicle of FIG. It is a functional block diagram of the flight control device by one Embodiment of this invention. It is a block diagram of the 1st Kalman filter according to one Embodiment of this invention. It is a block diagram of the 2nd Kalman filter by one Embodiment of this invention. It is a figure which shows the state that the unmanned aerial vehicle by one Embodiment of this invention flies under the bridge.

FIG. 1 is an external view of an unmanned aerial vehicle (multicopter) 1 according to an embodiment of the present invention. The unmanned aerial vehicle 1 includes a main body 2, six motors 3, six rotors (rotor blades) 4, six arms 5 connecting the main body 2 and each motor 3, a landing gear 6, and a local sensor. 7 and.

The six rotors 4 rotate by driving each of the motors 3 to generate lift. The main body 2 controls the drive of the six motors 3 to control the rotation speed and rotation direction of each of the six rotors 4, so that the unmanned aerial vehicle 1 can fly ascending, descending, flying back and forth and left and right, turning, and the like. Be controlled. The landing gear 6 contributes to the prevention of the unmanned aerial vehicle 1 from tipping over during takeoff and landing, and protects the main body 2 motor 3 of the unmanned aerial vehicle 1, the rotor 4, and the like.

The local sensor 7 is a laser scanner (laser range finder) that measures the surrounding conditions of the unmanned aerial vehicle 1 using a laser. The laser scanner irradiates the laser in a fan shape forward and upward, and uses the information obtained by the reflection to measure the distance to an object (for example, an obstacle such as a structure) around the unmanned aerial vehicle 1. , Makes it possible to create the shape of surrounding objects. The direction of irradiating the laser is an example, but it is preferable to include at least the upper side. As described above, in the present embodiment, the laser scanner is used as the local sensor 7, and the local sensor 7 is a sensor used to measure the relative position of the unmanned aerial vehicle 1 with respect to an object around the unmanned aerial vehicle 1, and is unmanned. Anything that can measure the positional relationship between the aircraft 1 and surrounding objects is sufficient. Therefore, for example, the number of lasers used in the laser scanner may be one or may be plural. Further, for example, the local sensor 7 can be an ultrasonic sensor or an image sensor. The local sensor 7 is preferably used when utilizing SLAM technology.

For example, if the local sensor 7 is an image sensor, the unmanned aerial vehicle 1 includes an image pickup device. The image pickup apparatus includes a monocular camera or a stereo camera composed of an image sensor or the like, and acquires an image or an image of the surroundings of the unmanned aerial vehicle 1 by taking an image of the surroundings of the unmanned aerial vehicle 1. In this case, preferably, the unmanned aerial vehicle 1 includes a motor capable of changing the direction of the camera, and the flight control device 11 controls the operation of the camera and the motor. For example, the unmanned aerial vehicle 1 continuously acquires images using a monocular camera, or acquires images using a stereo camera, and analyzes the acquired images to obtain the distance to surrounding objects and the said. Acquires information on the shape of an object. The image pickup apparatus may be an infrared depth sensor capable of acquiring shape data by infrared projection.

Although the local sensor 7 will be described as being attached to the outside of the main body 2, it may be attached to the inside of the main body 2 as long as it can measure the positional relationship between the unmanned aerial vehicle 1 and the surrounding environment.

FIG. 2 is a hardware configuration diagram of the unmanned aerial vehicle 1 of FIG. The main body 2 of the unmanned aerial vehicle 1 includes a flight control device (flight controller) 11, a transmitter / receiver 12, a sensor 13, a speed controller (ESC: Electric Speed Controller) 14, and a battery power supply (not shown). Be prepared.

The transmitter / receiver 12 transmits / receives various data signals to / from the outside, and includes an antenna. For convenience of explanation, the transmitter / receiver 12 will be described as one device, but the transmitter and the receiver may be installed separately.

The sensor 13 includes a global sensor used to measure the absolute position of the unmanned aerial vehicle 1. Global sensors include various sensors such as a GPS sensor that acquires position information, an acceleration sensor that acquires acceleration, a gyro sensor that detects attitude, a pressure sensor that acquires altitude, and a geomagnetic sensor that acquires orientation. As described above, the global sensor is not a sensor used to measure the relative position of the unmanned aerial vehicle 1 based on the positional relationship with surrounding objects. The unmanned aerial vehicle 1 is configured to be able to acquire the position information of the unmanned aerial vehicle including the tilt and rotation of the airframe, the latitude and longitude during flight, the altitude, and the direction of the airframe based on the information acquired from these sensors 13 and the local sensor 7. Will be done. However, for convenience of explanation, the sensor 13 has been described as being inside the main body 2, but some or all of the sensors included in the sensor 13 may be attached to the outside of the main body 2.

The flight control device 11 performs arithmetic processing based on various information to control the unmanned aerial vehicle 1. The flight control device 11 includes a processor 21, a storage device 22, a communication IF 23, a sensor IF 24, and a signal conversion circuit 25. These are connected via the bus 26.

The processor 21 controls the operation of the entire flight control device 11, and is, for example, a CPU. As the processor, an electronic circuit such as an MPU may be used. The processor 21 executes various processes by reading and executing a program or data stored in the storage device 22.

The storage device 22 includes a main storage device 22a and an auxiliary storage device 22b. The main storage device 22a is a semiconductor memory such as a RAM. The RAM is a volatile storage medium capable of reading and writing information at high speed, and is used as a storage area and a work area when a processor processes information. The main storage device 22a may include a ROM, which is a read-only non-volatile storage medium. In this case, the ROM stores programs such as firmware. The auxiliary storage device 22b stores various programs and data used by the processor 21 when executing each program. The auxiliary storage device 22b is, for example, a hard disk device, but may be any non-volatile storage or non-volatile memory as long as it can store information, and may be removable. The auxiliary storage device 22b stores, for example, an operating system (OS), middleware, application programs, various data that can be referred to when these programs are executed, and the like.

The communication IF 23 is an interface for connecting to the transmitter / receiver 12. The sensor IF 24 is an interface for inputting data acquired by various sensors 13 or a local sensor 7. For convenience of description, each IF will be described as one, but it is understood that different IFs can be provided for each device or sensor.

The signal conversion circuit 25 generates a pulse signal such as a PWM signal and sends it to the ESC 14. The ESC 14 converts the pulse signal generated by the signal conversion circuit 25 into a drive current to the motor 3 and supplies the current to the motor 3.

The battery power source is a battery device such as a lithium polymer battery or a lithium ion battery, and supplies electric power to each component. Since a large power source is required to operate the motor 3, the ESC 14 is preferably directly connected to the battery power source, adjusts the voltage and current of the battery power source, and supplies the drive current to the motor 3.

Preferably, the storage device 22 stores a flight control program in which a flight control algorithm for controlling the attitude and basic flight movements of the unmanned aerial vehicle 1 during flight is implemented. When the processor 21 executes the flight control program, the flight control device 11 performs arithmetic processing so as to reach the set target altitude and target speed, calculates the rotation speed and rotation speed of each motor 3, and gives a control command. Calculate value data. At this time, the flight control device 11 acquires various information such as the attitude of the unmanned aerial vehicle 1 in flight from various sensors 13, and performs arithmetic processing based on the acquired data and the set target altitude and target speed.

The signal conversion circuit 25 of the flight control device 11 converts the control command value data calculated as described above into a PWM signal and sends it to the ESC 14. The ESC 14 rotates the motor 3 by converting the signal received from the signal conversion circuit 25 into a drive current to the motor 3 and supplying the signal to the motor 3. In this way, the main body 2 including the flight control device 11 controls the rotation speed of the rotor 4 and controls the flight of the unmanned aerial vehicle 1.

In controlling the flight of such an unmanned aerial vehicle 1, the flight control device 11 needs to determine the altitude (or position) and speed of the unmanned aerial vehicle 1. However, if the information acquired from the various sensors 13 and the like is used as it is, the error may increase. In the present embodiment, the flight control device 11 determines (estimates) the altitude (or position) and speed of the unmanned aerial vehicle 1 based on the information acquired from various sensors 13 and the like.

In one example, the flight control program includes parameters such as flight route and flight speed including latitude / longitude and altitude, and the flight control device 11 sequentially determines the target altitude and target speed and performs the above arithmetic processing. , The unmanned aircraft 1 is made to fly autonomously.

In one example, the flight control device 11 determines the target altitude and the target speed by receiving instructions such as ascending / descending / advancing / retreating from an external transmitter via the transmitter / receiver 12, and performs the above arithmetic processing. By doing so, the flight of the unmanned aerial vehicle 1 is controlled.

In one example, the unmanned aerial vehicle 1 stores the time-series data of the position information actually flew by the unmanned aerial vehicle 1 and the image data acquired at each position in the auxiliary storage device 22b.

In one example, the unmanned aerial vehicle 1 is a self-contained unmanned aerial vehicle. For example, the unmanned aerial vehicle 1 is a commercially available mini surveyor MS-06LA (Autonomous Control Systems Laboratory Co., Ltd.), Snap (Vantage Robotics), AR. It can be Drone 2.0 (Parrot) or Bebop Drone (Parrot).

Hereinafter, when the unmanned aerial vehicle 1 is flown below the erection portion of the structure having the erection portion, for example, below the bridge, the flight control device 11 maintains the unmanned aerial vehicle 1 at a certain distance or more from the lower surface of the bridge. An embodiment in which control for flying is performed will be described. In the present embodiment, the flight control device 11 controls the position and speed in the height direction (vertical direction), and the flight control device 11 uses the first estimator and the second estimator for the control. The estimated altitude and the estimated vertical velocity (hereinafter, simply referred to as "estimated velocity") are calculated. In the present embodiment, the vertical direction (height direction) will be described for convenience of explanation, but it is understood that the control of the flight control device 11 according to the present embodiment can also be applied to the horizontal direction. Further, in the present embodiment, the first Kalman filter (first KF) 33 is used as the first estimator, and the second Kalman filter (second KF) 34 is used as the second estimator. Not limited to these.

FIG. 3 is a functional block diagram of the flight control device 11 according to the embodiment of the present invention. The flight control device 11 includes an environment acquisition unit 31, a self-position estimation unit 32, a first 33, and a second KF 34. In the present embodiment, these functions are realized by executing the program by the processor 21. In this way, since various functions are realized by reading the program, a part of one part (function) may be possessed by another part. The arrows in the figure indicate the main exchange of information between the parts, but are not limited to these as long as the operation according to the embodiment of the present invention can be realized. Preferably, the flight control device 11 performs calculations according to a cycle determined by, for example, a program to be read, and exchanges data for each cycle. However, each of the above functions may be realized by hardware by configuring an electronic circuit or the like. Further, in the present embodiment, the above function will be described as a function of the flight control device 11, but the function is a function of estimating the position of the unmanned aerial vehicle 1 necessary for controlling the flight of the unmanned aerial vehicle 1. Therefore, in one example, the flight control device 11 includes a position estimation device, and the position estimation device includes an environment acquisition unit 31, a self-position estimation unit 32, a first 33, and a second KF 34. ..

The environment acquisition unit 31 acquires the position of the unmanned aerial vehicle 1 using the global sensor. In the present embodiment, the global sensor includes a barometric pressure sensor and an acceleration sensor, and the environment acquisition unit 31 acquires the absolute altitude using the barometric pressure sensor and acquires the acceleration using the acceleration sensor. The environment acquisition unit 31 outputs the acquired acceleration data to the first KF33, and outputs the acquired acceleration data and absolute altitude data to the second KF34. In the present specification, the output of data means the transfer of data between each functional unit (for example, each program). For example, the environment acquisition unit 31 stores and continuously updates the absolute altitude acquired by using the barometric pressure sensor in a predetermined memory area of the storage device 22. In this case, the first KF33 refers to the memory area whose absolute altitude is updated (stored) for each cycle of calculation, and uses the data when it is updated.

The self-position estimation unit 32 estimates the self-position of the unmanned aerial vehicle 1 based on the point cloud data of objects around the unmanned aerial vehicle 1 such as obstacles acquired by using a laser scanner (local sensor 7). The obstacle is, for example, a structure having an erection part. The self-position estimated by the self-position estimation unit 32 is the relative position of the unmanned aerial vehicle 1 with respect to the objects around the unmanned aerial vehicle 1. In the present embodiment, the self-position estimation unit 32 estimates the self-position of the unmanned aerial vehicle 1 by using SLAM technology. Since the SLAM technology is a known technology, its description is omitted, but it recognizes surrounding objects using a local sensor 7 such as a laser scanner, and simultaneously estimates its own position and creates a map based on the object recognition results. It is something to do. In the present embodiment, the flight control device 11 controls to fly the unmanned aerial vehicle 1 while maintaining a certain distance from the lower surface of the bridge. Therefore, as described above, the laser scanner used when using the SLAM technology is , At least attached to the main body 2 so that it can be scanned by irradiating the laser upward.

The self-position estimation unit 32 outputs the position of the unmanned aerial vehicle 1 when it is determined that the position estimated based on the likelihood of self-position estimation is valid. In the present embodiment, the self-position estimation unit 32 estimates and outputs the relative altitude of the unmanned aerial vehicle 1 by using SLAM technology.

In one example, the self-position estimation unit 32 uses a laser scanner to acquire point cloud data around the unmanned aerial vehicle 1. The self-position estimation unit 32 starts the estimation calculation when the point cloud data within the predetermined distance range (for example, 0.1 to 20 m) can be acquired by the distance measurement distance by the laser, and starts to acquire the point cloud data. The self-position at the right timing is set as the reference coordinate. Then, the self-position estimation unit 32 estimates the self-position while creating a map using the acquired point cloud data. The self-position estimation unit 32 calculates the degree of coincidence between the created map and the point cloud data acquired by using the laser as the likelihood of self-position estimation. The self-position estimation unit 32 determines whether or not the calculated likelihood of self-position estimation is equal to or higher than a preset reference value, and if it is greater than or equal to the reference value, it is determined to be valid, and if it is less than the reference value, it is determined not to be valid. To do. For example, when the obstacle does not exist within a predetermined distance, the self-position estimation unit 32 has a likelihood of self-position estimation less than the reference value, and cannot perform effective self-position estimation. In this way, the self-position estimation unit 32 determines the relative of the unmanned aerial vehicle 1 when the unmanned aerial vehicle 1 approaches an obstacle within a predetermined distance and when sufficient point cloud data is collected for self-position estimation. It is configured to estimate and output the altitude.

However, the configuration of the self-position estimation unit 32 is not limited to the above. For example, the self-position estimation unit 32 can use the odometry information acquired by using the acceleration sensor or the gyro sensor for the self-position estimation. Further, the self-position estimation unit 32 acquires an image using an imaging device, extracts the position of an object in the acquired image or a point on the surface as a feature point, and extracts the extracted pattern and the created map (or acquired). It is also possible to match the patterns of point clouds). Further, the likelihood of self-position estimation is not limited to the above example.

The first KF33 estimates the speed of the unmanned aerial vehicle 1 and the acceleration bias value for correcting the speed by using the relative altitude (relative position) of the unmanned aerial vehicle 1 output by the self-position estimation unit 32 as an observed value. To do. The first KF33 uses the relative altitude of the unmanned aerial vehicle 1 as an observed value in this embodiment. The first KF33 outputs the estimated velocity and acceleration bias values to the second KF34. Preferably, when the self-position estimation unit 32 outputs the relative altitude of the unmanned aerial vehicle 1, the first KF 33 estimates the speed and acceleration bias value of the unmanned aerial vehicle 1 by using the relative altitude as an observed value. , Output to the second KF34. Further, the first KF 33 uses the acceleration acquired from the environment acquisition unit 31 as a control input.

When the first KF33 outputs the velocity and acceleration bias values, the second KF34 uses the output velocity and acceleration bias values as observation values, and the environment acquisition unit 31 acquires the absolute altitude (absolute position). When (output), the absolute altitude and speed of the unmanned aircraft 1 are estimated using the absolute altitude as an observed value. The second KF34 stores and updates the estimated absolute altitude and velocity in, for example, a memory area referred to by the flight control program as the estimated altitude (estimated position) and estimated velocity. Further, the second KF 34 uses the acceleration acquired from the environment acquisition unit 31 as a control input.

As described above, the flight control device 11 utilizes the Kalman filter having a two-stage configuration and uses the position and speed estimated by the second KF34 as the estimated position and estimated speed of the unmanned aerial vehicle 1 to fly the unmanned aerial vehicle 1. To control. In one example, the flight control device 11 determines the distance between the unmanned aerial vehicle 1 and the obstacle using the relative position estimated by the self-position estimation unit 32. The flight control device 11 uses the estimated position and estimated speed of the unmanned aerial vehicle 1 to maintain the determined distance between the unmanned aerial vehicle 1 and the obstacle at a certain level or more to avoid a collision. To control. In this case, for example, the flight control device 11 keeps the distance above a certain level by attenuating the target speed when the distance becomes small according to the determined distance between the unmanned aerial vehicle 1 and the obstacle. .. Further, in one example, the flight control device 11 uses the estimated position and estimated speed of the unmanned aerial vehicle 1 to control the flight of the unmanned aerial vehicle 1 so as to maintain a constant distance from the lower surface of the bridge.

Hereinafter, the details of the operation of the first KF33 and the second KF34 will be described using the state space model assumed in the present embodiment. Since the operating principle of a general Kalman filter is known, detailed description thereof will be omitted, but the Kalman filter is a state estimation based on a time series model. By constructing such a state space model, it is possible to estimate the observed time series data and the state variables contained behind the time series data.

The state space model (process model) of the first KF33 of the present embodiment is described below. Equation (1) is an equation of state, and equation (2) is an observation equation. FIG. 4 is a block diagram of the first KF33.

Here, k represents time in time series data and is represented by an integer. x ₁ (k) is the state variable (state vector) at time k, A ₁ is the system matrix (state transition matrix), B ₁ is the drive matrix, u ₁ (k) is the control input, and Nv (k) is the system noise. (Process noise) (generally represented by v (k), but in order to distinguish it from the velocity v (k) described later, it is expressed as Nv (k) in the present specification). As can be understood from the equation (1), the equation of state shows the relationship between the state vector at time k and the state vector at time k-1 one step before. y ₁ (k) is the observed amount input at time k, c ₁ is the observation matrix, and w (k) is the observed noise. The system noise and the observed noise are based on the Gaussian distribution in which the average value is 0 and the variance value is the design value, which are prerequisites for applying the Kalman filter. y ₁ (k) is an observed value when the relative altitude data is output (updated) from the self-position estimation unit 32. Specifically, these values are as follows.

From equations (3) to (7), it is understood that equation (1) represents the following equations (9) to (11).

Here, v (k) is the speed of the unmanned aerial vehicle 1, Z _rel (k) is the relative altitude of the unmanned aerial vehicle 1, and a _bias (k) is the acceleration bias value for correcting v (k). The value in k is shown. a is an acceleration, and the acceleration acquired by the acceleration sensor is _{input to the control input u 1 (k).} Further, the flight control device 11 performs calculations according to a cycle determined by a program such as a flight control program, and dt indicates a minute time of one step from time k to time k + 1. For example, when the first KF33 is operated in a 50 Hz loop, dt = 0.02 (s). In this case, preferably, the second KF34 is also operated in a 50 Hz loop.

Next, the processing of the prediction step and the filtering step in the first KF33 will be described, and the likelihood of each value will be described.

The prediction step is at time k based on the data available by time k-1.

を用いて、事前推定値

を修正して事後推定値

を推定し、事後誤差共分散行列

を推定する処理であり、以下の式（１４）〜（１６）で表される。

The filtering step is the observed value

Pre-estimated value using

Correct the ex post facto estimate

Estimate the posterior error covariance matrix

Is a process for estimating, and is represented by the following equations (14) to (16).

はカルマンゲインである。Ｑはシステム雑音の分散行列であり、空間状態モデルの尤度を示す。Ｒは観測ノイズの分散行列であり、観測値の尤度を示すものである。

は推定値の分散行列であり、推定値の尤度を示すものである。

Is Kalman gain. Q is the variance matrix of the system noise and indicates the likelihood of the spatial state model. R is a variance matrix of the observed noise and indicates the likelihood of the observed value.

Is a variance matrix of the estimated values and indicates the likelihood of the estimated values.

の値が増加し続け、推定値の尤度が低下する。第１のＫＦ３３は、観測値が得られたタイミングで、

が小さい大きさとなる

演算され、推定誤差

が減少し、推定値の尤度が増加する。 From the above, when the first KF33 continues to execute only the prediction step according to the calculation cycle of the program,

The value of is continuously increasing and the likelihood of the estimated value is decreasing. The first KF33 is the timing when the observed value is obtained.

Becomes a small size

Calculated and estimated error

Decreases and the likelihood of the estimate increases.

When the relative altitude is output from the self-position estimation unit 32, the first KF 33 performs a filtering step process, estimates the speed and acceleration bias value of the unmanned aerial vehicle 1, and outputs the relative altitude to the KF 2. When the relative altitude continues to be output from the self-position estimation unit 32, the first KF33 continues the processing of the filtering step according to the calculation cycle of the program, estimates the speed and acceleration bias value of the unmanned aerial vehicle 1, and outputs it to the KF2. Continue to do.

On the other hand, when the relative altitude is not output from the self-position estimation unit 32, the first KF 33 only processes the prediction step and does not output the estimated speed and acceleration bias value of the unmanned aerial vehicle 1 to the KF2. However, when the relative altitude is no longer output from the self-position estimation unit 32, the state vector holds the velocity and acceleration bias values when the filtering step is processed, so that the velocity and acceleration of the unmanned aircraft 1 to be estimated are estimated. It is possible to prevent deterioration of the accuracy of the bias value.

が比較的小さい推定値に限定して、第２のＫＦ３４に出力することができ、第２のＫＦ３４は、比較的信頼度が高い推定値を観測値として用いることができる。 In this way, the first KF33 has an estimation error.

Can be output to the second KF34 by limiting the estimated value to a relatively small estimated value, and the second KF34 can use an estimated value having a relatively high reliability as an observed value.

Next, the state space model of the second KF34 of this embodiment is described below. Equation (17) is an equation of state, and equations (18) and (19) indicate observation equations. FIG. 5 is a block diagram of the second KF34.

x ₂ (k) is the state variable (state vector) at time k, A ₂ is the system matrix (state transition matrix), B ₂ is the drive matrix, u ₂ (k) is the control input, and Nv (k) is the system noise. (Process noise). y _2a (k) and y _2b (k) indicate the observable input at time k, c _2a and c _2b indicate the observation matrix, and w (k) indicates the observation noise. y _2a (k) is the observed value when the data of the estimated velocity and the estimated acceleration bias value are output (updated) from the first KF 33, and y _2b (k) is the absolute altitude from the environment acquisition unit 31. It is an observed value when it is output. Similarly, c _2a is an observation matrix when the data of the estimated velocity and the estimated acceleration bias value are output from the first KF 33, _{and c 2 b} is the observation matrix when the absolute altitude is output from the environment acquisition unit 31. Is. Specifically, these values are defined as follows.

From equations (20) to (25), it is understood that equation (17) represents the following equations (28) to (32).

Here, Z _pres (k) is the absolute altitude of the unmanned aerial vehicle 1, v _bias (k) is the speed bias value, and Z _bias (k) is the altitude bias value, which indicate the values at time k.

Next, the processing of the prediction step and the filtering step in the second KF34 will be described, and the likelihood of each value will be described.

The prediction step is represented by the following equations (33) and (34).

There are two filtering steps: (a) when the estimated speed and the estimated acceleration bias value are output from the first KF33, and (b) when the absolute altitude is output from the environment acquisition unit 31.

(A) When the estimated speed and the estimated acceleration bias value are output from the first KF33, they are represented by the following equations (35) to (37).

(B) When the absolute altitude is output from the environment acquisition unit 31, it is represented by the following equations (38) to (40).

Here, R _2a and R _2b are variance matrices of observed noise, and are represented by the following equations (41) and (42).

r _vel is the variance value (likelihood) of the velocity obtained from the first KF33, r _abias is the variance value (likelihood) of the acceleration bias value obtained from the first KF33, and r _pres is. It is the variance value (likelihood) of the absolute altitude obtained from the environment acquisition unit 31.

値が増加し続け、推定値の尤度が低下する。第２のＫＦ３４は、観測値が得られたタイミングで、

小さい大きさとなる

演算され、推定誤差

減少し、推定値の尤度が増加する。ただし第１のＫＦ３３からの推定値の出力及び環境取得部３１からの絶対高度の出力は非同期的に発生するため、第２のＫＦ３４は、それぞれが出力されるタイミングで、それぞれのフィルタリングステップを実行する。このようにして、第２のＫＦ３４は、無人航空機１の高度及び速度を推定し続ける。また前述のとおり、第１のＫＦ３３から出力される推定値の信頼度は比較的高いため、ｒ_velとｒ_abiasの大きさは、ｒ_presと比較して小さく設定する。これにより、第１のＫＦ３３から推定値が出力される場合は、環境取得部３１から得られる絶対高度より強力に更新処理を行うことが可能となる。 From the above, when the second KF34 continues to execute only the prediction step according to the calculation cycle of the program,

The value continues to increase and the likelihood of the estimate decreases. The second KF34 is at the timing when the observed value is obtained.

Become a small size

Calculated and estimated error

It decreases and the likelihood of the estimate increases. However, since the output of the estimated value from the first KF33 and the output of the absolute altitude from the environment acquisition unit 31 are generated asynchronously, the second KF34 executes each filtering step at the timing when each is output. To do. In this way, the second KF34 continues to estimate the altitude and speed of the unmanned aerial vehicle 1. Further, as described above, since the reliability of the estimated value output from the first KF33 is relatively high, the magnitudes of _{r vel} and r _abias are set smaller than those of _{r pres.} As a result, when the estimated value is output from the first KF 33, the update process can be performed more powerfully than the absolute altitude obtained from the environment acquisition unit 31.

When the estimated value is not output from the first KF33, the second KF34 only processes the prediction step and the filtering step of (b), but continues to estimate the altitude and speed of the unmanned aerial vehicle 1. When the estimated value is no longer output from the first KF33, the second KF34 holds the velocity and the acceleration bias value having a relatively high likelihood immediately before that, and therefore the accuracy of the estimated altitude and the estimated velocity to be calculated. Deterioration can be prevented. For example, in the equation (30), when the estimated value is no longer output from the first KF 33, the absolute altitude obtained from the environment acquisition unit 31 is fed back to each bias value to suppress the drift that may occur, and the bias value is suppressed with the passage of time. Indicates that

With such a configuration, when the relative altitude is not output from the self-position estimation unit 32, for example, when SLAM is invalidated, the second KF34 stabilizes the estimated altitude and the estimated speed, and the unmanned aerial vehicle It is possible to prevent a sudden rise in 1 and instability.

FIG. 6 is a diagram showing a state in which the unmanned aerial vehicle 1 flies below the bridge 41. The bridge 41 has a bridge girder portion 42 and a pier portion 43, and the pier portion 43 has a beam portion 43a and a column portion 43b. Here, the flight control device 11 when the unmanned aerial vehicle 1 provided with the image pickup device flies from the area A to the area F so as to maintain a certain distance (for example, 2 m) from the lower surface in order to image the lower surface of the bridge 41. The control of is described. At this time, the flight control device 11 calculates the estimated altitude and the estimated speed using the first KF33 and the second KF34 as described above, and controls the flight of the unmanned aerial vehicle 1 using the estimated altitude and the estimated speed. ..

A case where the unmanned aerial vehicle 1 flies in the area A will be described. The area A is assumed to be an area separated from the bridge 41 by a predetermined distance (for example, 20 m). In this case, the self-position estimation unit 32 determines that the position estimated based on the likelihood of self-position estimation is not valid, and the first KF 33 cannot use the relative altitude as the observed value. Therefore, the second KF34 estimates the altitude and speed of the unmanned aerial vehicle 1 depending on the absolute altitude acquired by the environment acquisition unit 31, and it is considered that the accuracy of the estimated value becomes relatively poor. However, in region A, the unmanned aerial vehicle 1 is unlikely to collide with the bridge 41. In one example, in region A, the flight control device 11 uses the estimated position and estimated speed of the unmanned aerial vehicle 1 to control the flight of the unmanned aerial vehicle 1 to a preset altitude.

The case where the unmanned aerial vehicle 1 flies from the area A to the area B will be described. Region B is assumed to be a region below the tip surface of the recess of the bridge girder portion 42. In this case, the self-position estimation unit 32 starts the estimation calculation when the point cloud data whose distance measurement distance by the laser is within a predetermined distance can be acquired, and the self-position at the timing when the point cloud data is started to be acquired. Is defined as the reference coordinate. In the area B, it is conceivable that the obstacle will continue to exist within a predetermined distance with respect to the unmanned aerial vehicle 1 flying so as to maintain a certain distance. Therefore, the self-position estimation unit 32 outputs the position of the unmanned aerial vehicle 1 to the first KF 33 as long as the position estimated based on the likelihood of self-position estimation continues to be determined to be valid. In this case, the first KF33 outputs the estimated speed and the estimated acceleration bias value to the second KF34, and the second KF34 outputs the estimated speed and the estimated acceleration bias value and the absolute altitude output by the environment acquisition unit 31. The altitude and speed of the unmanned aerial vehicle 1 are estimated using the observed values. In the region B, the flight control device 11 uses the estimated position and the estimated speed of the unmanned aerial vehicle 1 to fly the unmanned aerial vehicle 1 so as to maintain a constant distance from the tip surface of the recess of the bridge girder portion 42 in the region B. To control.

A case where the unmanned aerial vehicle 1 flies from the area B to the area D will be described. It is assumed that the region C is a region below the recessed region of the bridge girder portion 42, and the region D is a region below the tip surface of the recess of the bridge girder portion 42. The case where the unmanned aerial vehicle 1 flies from the area B to the area D is the same as the case where the unmanned aerial vehicle 1 flies in the area B. However, when the unmanned aerial vehicle 1 flies from the area B to the area C, the flight control device 11 raises the unmanned aerial vehicle 1 by the protruding length of the recess. When the unmanned aerial vehicle 1 flies from the area C to the area D, the flight control device 11 lowers the unmanned aerial vehicle 1 by the protruding length of the recess. When flying from the area B to the area D, the flight control device 11 keeps the height in the area B and flies, that is, the flight from the area B to the area D at a substantially constant altitude. It is also possible to control the unmanned aerial vehicle 1.

The case where the unmanned aerial vehicle 1 flies from the area D to the area E will be described. The area E is assumed to be an area separated from the bridge 41 by a predetermined distance (for example, 20 m). In this case, the self-position estimation unit 32 determines that the position estimated based on the likelihood of self-position estimation is not valid, and the first KF 33 cannot use the relative altitude as the observed value. Therefore, the second KF34 estimates the altitude and speed of the unmanned aerial vehicle 1 depending on the absolute altitude acquired by the environment acquisition unit 31. However, as described above, when the estimated value from the first KF33 is no longer output, the second KF34 holds the velocity and acceleration bias values with relatively high likelihood at that time, so the estimation is calculated. It is possible to prevent the accuracy deterioration of altitude and estimated speed. In one example, in region E, the flight control device 11 uses the estimated position and speed of unmanned aerial vehicle 1 to control the flight of unmanned aerial vehicle 1 so as to maintain altitude in region D.

For example, as an altitude control method similar to that of the present invention, the altitude information obtained from the self-position estimation unit 32 is used in the place where the self-position estimation unit 32 can be used, and the environment is acquired in the place where the self-position estimation unit 32 cannot be used. A method of using the altitude information obtained from the unit 31 can be considered. However, since this method directly uses the information of the self-position estimation unit 32, for example, the information acquired from the laser, when the self-position estimation unit 32 becomes unavailable, unstable operation such as a sudden rise occurs. There is a problem that it is easy.

With the configuration as in this embodiment, when the relative altitude is no longer output from the self-position estimation unit 32, the second KF 34 can stabilize the estimated altitude and the estimated speed. Since the flight control device 11 can control the flight of the unmanned aerial vehicle 1 using such an estimated altitude and the estimated speed, it is possible to prevent the unmanned aerial vehicle 1 from suddenly ascending and destabilizing its operation. ..

The case where the unmanned aerial vehicle 1 flies from the area E to the area F will be described. It is assumed that the region F is a region below the tip surface of the recess of the bridge girder portion 42 in the bridge 41 adjacent to the bridge 41 above the regions B to D. The case where the unmanned aerial vehicle 1 flies from the area E to the area F is the same as the case where the unmanned aerial vehicle 1 flies from the area A to the area B. However, when the unmanned aerial vehicle 1 flies from the area D to the area F in a relatively short time, it is considered that the accuracy deterioration of the estimated altitude and the estimated speed calculated by the second KF34 in the area E is relatively small, so that the flight control The device 11 can fly the unmanned aerial vehicle 1 more smoothly.

Next, the main functions and effects of the flight control device 11 according to the embodiment of the present invention will be described. In the present embodiment, the self-position estimation unit 32 estimates the relative altitude of the unmanned aerial vehicle 1 and outputs it to the first KF 33 when the likelihood of the self-position estimation is equal to or higher than the reference value. The first KF33 estimates the speed and acceleration bias value of the unmanned aerial vehicle 1 using the output relative altitude as an observed value, and outputs the output to the second KF34. The second KF34 uses the estimated value as an observed value when the estimated value is output from the first KF33, and uses the absolute altitude as an observed value when the absolute altitude is acquired from the environment acquisition unit 31. Estimate the altitude and speed of the unmanned aerial vehicle 1.

Here, when there is a river under the bridge, it is considered that the altitude accuracy acquired by the barometric pressure sensor is not sufficient. Therefore, the flight control device 11 of the present embodiment sets the reliability of the relative altitude estimated by the self-position estimation unit 32 to be higher than the reliability of the absolute altitude acquired by the environment acquisition unit 31. That is, in the second KF34, the variance value of the velocity and the variance value of the acceleration bias value obtained from the first KF33 are set smaller than the variance value of the absolute altitude obtained from the environment acquisition unit 31 (likelihood). Is set large).

With such a configuration, when the estimated value is output from the first KF33, the second KF34 can perform the update process more powerfully than the absolute altitude obtained from the environment acquisition unit 31. .. Further, when the estimated value is no longer output from the first KF33, the second KF34 holds the velocity and the acceleration bias value having a relatively high likelihood immediately before that, so that the estimated altitude and the estimated velocity to be calculated It is possible to prevent deterioration of accuracy. As a result, when the relative altitude is no longer output from the self-position estimation unit 32, the flight control device 11 can prevent the unmanned aerial vehicle 1 from suddenly ascending or destabilizing.

In another embodiment, the control method or program that realizes the information processing shown in the functional block diagram of the embodiment of the present invention described above can be used, and the computer-readable storage medium that stores the program can be used. You can also do it.

Each of the examples described above is an example for explaining the present invention, and the present invention is not limited to these examples. For example, in the above embodiment, the unmanned aerial vehicle 1 does not have to be an autonomous flight type unmanned aerial vehicle, and the functional configuration of the unmanned aerial vehicle 1 is not limited to that shown in FIG. The rotor blades for levitating the unmanned aerial vehicle 1 are not limited to the six rotors 4 as shown in FIG. Good. The size of the unmanned aerial vehicle 1 is also arbitrary. Each embodiment can be applied to any embodiment of the present invention in appropriate combinations as long as there is no contradiction. That is, the present invention can be carried out in various forms as long as it does not deviate from the gist thereof.

1 Unmanned aerial vehicle 2 Main body 3 Motor 4 Rotor (rotor)
5 Arm 6 Landing gear 7 Local sensor 11 Flight control device 12 Transmitter / receiver 13 Sensor 14 Speed controller (ESC)
21 Processor 22 Storage device 23 Communication IF
24 IF for sensor
25 Signal conversion circuit 31 Environment acquisition unit 32 Self-position estimation unit 33 First Kalman filter (first KF)
34 Second Kalman filter (second KF)
41 Bridge 42 Bridge girder 43 Pier 43a Beam 43b Pillar

Claims (24)

An environment acquisition unit that acquires the position of the unmanned aerial vehicle using a global sensor for measuring the absolute position of the unmanned aerial vehicle, and
The relative position of the unmanned aerial vehicle with respect to an object around the unmanned aerial vehicle is estimated using a local sensor for measuring the relative position of the unmanned aerial vehicle, and the estimated relative position is determined based on the estimated likelihood. A self-position estimation unit that outputs the relative position of the unmanned aerial vehicle when it is determined to be valid, and
The position of the unmanned aerial vehicle is based on the relative position when the relative position of the unmanned aerial vehicle is output by the self-position estimation unit, and based on the position when the position is acquired by the environment acquisition unit. And an estimator to estimate the speed ,
The estimator is
When the relative position of the unmanned aerial vehicle is output by the self-position estimation unit, the speed of the unmanned aerial vehicle and the acceleration bias value for correcting the speed are estimated and output based on the relative position. Estimator and
When the speed and the acceleration bias value are output, based on the output speed and the acceleration bias value, and when the position is acquired by the environment acquisition unit, based on the position, the unmanned aerial vehicle Includes a second estimator that estimates and outputs position and velocity,
The position and speed of the unmanned aerial vehicle estimated by the estimator are the positions and speeds output by the second estimator.
Position estimator. The position estimation device according to claim 1, wherein the self-position estimation unit outputs the relative position of the unmanned aerial vehicle when the distance measured by the local sensor is within a predetermined distance. The self-position estimation unit estimates the relative position of the unmanned aerial vehicle based on the point cloud data acquired by using the laser, and determines that the estimated relative position is valid based on the estimated likelihood. The position estimation device according to claim 1 or 2, which outputs the relative position of the unmanned aerial vehicle when the vehicle is used. The position estimation device according to any one of claims 1 to 3, wherein the self-position estimation unit estimates the relative position of the unmanned aerial vehicle by using SLAM technology. The self-position estimation unit creates a map using SLAM technology, and determines whether or not the estimated relative position is valid based on the degree of agreement between the created map and the acquired point cloud data. The position estimation device according to claim 4, wherein the position estimation device is used for determination. The position estimation device according to any one of claims 1 to 5, wherein the environment acquisition unit acquires altitude using a barometric pressure sensor. The position estimation device according to any one of claims 1 to 6, wherein the position is altitude and the speed is a vertical speed. When the relative position of the unmanned aerial vehicle is output by the self-position estimation unit, the first estimator estimates and outputs the speed and acceleration bias value of the unmanned aerial vehicle using the relative position as an observed value. The position estimation device according to any one of claims 1 to 7, which is a Kalman filter. The first estimator obtains a state equation representing the relationship between the current state vector and the past state vector including the velocity, the position, and the acceleration bias value as components, and the relationship between the observed value and the state vector. The position estimation device according to claim 8 , which estimates the speed and acceleration bias value of the unmanned aircraft based on the observation equation represented. When the speed and the acceleration bias value are output, the second estimator uses the output speed and the acceleration bias value as observation values, and when the position is acquired by the environment acquisition unit, the second estimator uses the output speed and the acceleration bias value as observation values. The position estimation device according to any one of claims 1 to 9 , which is a Kalman filter that estimates and outputs the position and speed of the unmanned aircraft using the position as an observed value. The second estimator obtains a state equation expressing the relationship between the current state vector and the past state vector including the velocity, the position, and the acceleration bias value as components, and the relationship between the observed value and the state vector. The position estimation device according to claim 10 , which estimates the position and speed of the unmanned aircraft based on the observation equation represented. The second estimator sets the likelihood of the velocity and the acceleration bias value output from the first estimator to be larger than the likelihood of the position acquired by the environment acquisition unit. The position estimation device according to any one of claims 1 to 11 , which estimates and outputs the position and speed of the unmanned aerial vehicle. The environment acquisition unit further acquires acceleration using an acceleration sensor, and then obtains acceleration.
The position estimation device according to any one of claims 1 to 12 , wherein the first estimator and the second estimator use the acceleration acquired from the environment acquisition unit as a control input. A flight control device for controlling the flight of an unmanned aerial vehicle, comprising the position estimation device according to any one of claims 1 to 13.
A flight control device that controls the flight of the unmanned aerial vehicle using the position and speed of the unmanned aerial vehicle estimated by the estimator. The flight control device is
The distance to the object is determined using the relative position estimated by the self-position estimation unit, and the determined distance is set to a certain value or more by using the position and speed of the unmanned aerial vehicle estimated by the estimator. The flight control device according to claim 14 , which controls the flight of the unmanned aerial vehicle so as to hold and avoid a collision. When the unmanned aerial vehicle flies below the erection portion of a structure having an erection portion, the position and speed of the unmanned aerial vehicle estimated by the estimator are used to keep the distance from the lower surface of the erection portion constant. The flight control device according to claim 15 , which controls the flight of the unmanned aerial vehicle so as to perform the operation. An unmanned aerial vehicle comprising the flight control device according to any one of claims 14 to 16. A method of estimating the position of an unmanned aerial vehicle,
A step of acquiring the position of the unmanned aerial vehicle using a sensor for measuring the absolute position of the unmanned aerial vehicle, and
It is a step of estimating the relative position of the unmanned aerial vehicle using the sensor for measuring the relative position of the unmanned aerial vehicle, and it is determined that the estimated relative position is valid based on the likelihood of the estimation. In this case, the step of estimating the relative position of the unmanned aerial vehicle and
A step of estimating the position and speed of the unmanned aerial vehicle based on the relative position of the unmanned aerial vehicle when it is estimated, and based on the position when the position is acquired.
Only including,
The step of estimating the position and speed of the unmanned aerial vehicle is
When the relative position of the unmanned aerial vehicle is output, the first estimation step of estimating and outputting the speed of the unmanned aerial vehicle and the acceleration bias value for correcting the speed based on the relative position, and
When the speed and the acceleration bias value are output, based on the output speed and the acceleration bias value, and when the position is acquired by the environment acquisition unit, based on the position, the unmanned aerial vehicle Includes a second estimation step that estimates and outputs position and velocity,
The position and speed of the unmanned aerial vehicle estimated by the estimation step are the positions and speeds output by the second estimation step.
Method. In the first estimation step, when the relative position of the unmanned aerial vehicle is output, the speed and acceleration bias value of the unmanned aerial vehicle are estimated by executing Kalman filtering using the relative position as an observed value. The method according to claim 18 , wherein the method is output. In the second estimation step, when the velocity and the acceleration bias value are output, the output velocity and the acceleration bias value are used as observation values, and when the position is acquired by the environment acquisition unit, the position is acquired. The method according to claim 18 or 19 , wherein the position and speed of the unmanned aircraft are estimated and output by performing Kalman filtering using the position as an observed value. In the second estimation step, the likelihood of the velocity and the acceleration bias value output from the first estimation step is set to be larger than the likelihood of the position acquired by the environment acquisition unit. The method according to any one of claims 18 to 20 , wherein the position and speed of the unmanned aerial vehicle are estimated and output. A program for estimating the position of an unmanned aerial vehicle, which causes a computer to execute each step of the method according to any one of claims 18 to 21. A method for controlling the flight of an unmanned aerial vehicle, which comprises the method of estimating the position of the unmanned aerial vehicle according to any one of claims 18 to 21.
A method comprising controlling the flight of the unmanned aerial vehicle using the position and speed of the unmanned aerial vehicle estimated by the estimated step. A program that controls the flight of an unmanned aerial vehicle and causes a computer to perform each step of the method according to claim 23.

Images