TWI805141B - Positioning method and device for unmanned aerial vehicles - Google Patents

Abstract

Disclosed is a positioning method and device for unmanned aerial vehicles, wherein the unmanned aerial vehicle is loaded with an acceleration sensor, a gyroscope sensor, a height sensor, and an optical flow sensor. The positioning method includes: based on acceleration sensing The acceleration value measured by the gyro sensor and the angular velocity value measured by the gyroscope sensor are used to obtain the original value of the motion acceleration of the UAV in the horizontal heading coordinate system; based on the relative displacement value measured by the optical flow sensor and the relative height measured by the height sensor value, the acceleration value measured by the acceleration sensor, the angular velocity value measured by the gyroscope sensor, and the original value of the motion acceleration of the UAV in the horizontal heading coordinate system to obtain the motion acceleration correction value of the UAV in the horizontal heading coordinate system ; and based on the original motion acceleration value and the motion acceleration correction value of the UAV in the horizontal heading coordinate system, the position of the UAV in the horizontal heading coordinate system is obtained.

Description

Positioning method and device for unmanned aerial vehicles

The invention relates to the field of unmanned aerial vehicles, in particular to a positioning method and equipment for unmanned aerial vehicles.

With the development of drone technology, the application range of drones is getting wider and wider, and the consumer groups are getting bigger and bigger. In the mid-to-low-end market, which is sensitive to manufacturing costs, there is an increasing demand for drones with low cost and high performance. For scenarios and limitations such as no GPS (Global Positioning System, GPS) signal or weak GPS signal, small size of the UAV, and weak load capacity, the optical flow sensor can be used to detect the starting position of the UAV relative to the detected plane. Position (i.e., where the drone was at the moment of takeoff) displacement and movement velocity. However, in various maneuvering situations such as UAV take-off and landing, flight navigation, and target tracking, if the optical flow sensor is only used for UAV positioning, the cumulative offset of the optical flow sensor itself, the attitude and height position of the UAV itself Changes, unsatisfactory ground features or complex changes can easily become factors that cause UAV instability.

According to the positioning method for unmanned aerial vehicle according to the embodiment of the present invention, wherein the unmanned aerial vehicle is loaded with an acceleration sensor, a gyroscope sensor, a height sensor, and an optical flow sensor, the positioning method includes: The acceleration value measured by the sensor and the angular velocity value measured by the gyroscope sensor are used to obtain the original value of the motion acceleration of the UAV in the horizontal heading coordinate system; based on the relative displacement value measured by the optical flow sensor and the relative displacement value measured by the height sensor The altitude value, the acceleration value measured by the acceleration sensor, the angular velocity value measured by the gyroscope sensor, and the original value of the motion acceleration of the UAV in the horizontal heading coordinate system are used to obtain the motion acceleration correction of the UAV in the horizontal heading coordinate system value; and based on the original motion acceleration value and the motion acceleration correction value of the UAV in the horizontal heading coordinate system, the position of the UAV in the horizontal heading coordinate system is obtained.

According to the positioning device for unmanned aerial vehicle according to the embodiment of the present invention, wherein the unmanned aerial vehicle is loaded with an acceleration sensor, a gyroscope sensor, a height sensor, and an optical flow sensor, the positioning device includes: a memory, having computer-executable instructions stored thereon; and one or more processors, configured To execute computer-executable instructions to realize the above positioning method for drones.

According to the positioning method and positioning device for drones in the embodiments of the present invention, by combining the measurement results of the acceleration sensor, gyroscope sensor, height sensor, and optical flow sensor, it can be closer to the real, Smoother, real-time, and more precise positioning of drones.

100, S102, S104, S106: positioning method

200: Computer system

201: processing device

202: Read Only Memory (ROM)

203: Random Access Memory (RAM)

204: busbar

205: input/output (I/O) interface

206: input device

207: output device

208: storage device

209: Communication device

The present invention can be better understood from the following description of specific embodiments of the present invention in conjunction with the drawings, wherein: FIG. 1 shows a flow chart of a positioning method for a drone according to an embodiment of the present invention.

Fig. 2 shows a schematic diagram of a computer system that can implement a positioning method for a drone according to an embodiment of the present invention.

Features and exemplary embodiments of various aspects of the invention will be described in detail below. In the following detailed description, numerous specific details are set forth in order to provide a thorough understanding of the present invention. It will be apparent, however, to one skilled in the art that the present invention may be practiced without some of these specific details. The following description of the embodiments is only to provide a better understanding of the present invention by showing examples of the present invention. The present invention is by no means limited to any specific configurations and algorithms set forth below, but covers any modification, substitution and improvement of elements, components and algorithms without departing from the spirit of the invention. In the drawings and the following description, well-known structures and techniques have not been shown in order to avoid unnecessarily obscuring the present invention.

In view of the above-mentioned problems in the positioning of UAVs using optical flow sensors alone, a multi-sensor fusion complementary, high-precision, and high-robust positioning method for UAVs is proposed.

Fig. 1 shows a flowchart of a positioning method 100 for a drone according to an embodiment of the present invention. It should be noted that the positioning method 100 will use the measurement results from the acceleration sensor, gyroscope sensor, altitude sensor, and optical flow sensor of the UAV control system, and will use the UAV as the center 1. Always use the horizontal heading coordinate system with the nose direction of the drone as the 0 azimuth angle relative to the ground motion as the reference coordinate system to position the drone.

As shown in FIG. 1 , the positioning method 100 includes: S102, based on the acceleration value measured by the acceleration sensor and the angular velocity value measured by the gyroscope sensor, obtain the UAV in the horizontal heading coordinate system The original value of motion acceleration; S104, based on the relative displacement value measured by the optical flow sensor, the relative height value measured by the height sensor, the acceleration value measured by the acceleration sensor, the angular velocity value measured by the gyroscope sensor, and the value of the unmanned The original value of the motion acceleration of the UAV in the horizontal course coordinate system is used to obtain the correction value of the motion acceleration of the UAV in the horizontal course coordinate system; , to obtain the position of the UAV in the horizontal heading coordinate system.

In some embodiments, since the acceleration sensor measures the acceleration value of the UAV in the UAV carrier coordinate system, it is necessary to convert the acceleration value measured by the acceleration sensor into the coordinate system to obtain the UAV in the horizontal heading. The acceleration value in the coordinate system. That is, obtaining the original value of the motion acceleration of the UAV under the horizontal heading coordinate system includes: based on the acceleration value measured by the acceleration sensor and the angular velocity value measured by the gyroscope sensor, obtaining the acceleration value measured by the acceleration sensor Convert to the direction cosine matrix of the original value of the motion acceleration of the UAV in the horizontal heading coordinate system; and based on the acceleration value measured by the acceleration sensor and the direction cosine matrix, obtain the original value of the motion acceleration of the UAV in the horizontal heading coordinate system .

For example, according to equation (1), the acceleration value measured by the acceleration sensor can be converted into the original value of the motion acceleration of the UAV in the horizontal heading coordinate system.

a _n = a _b . R (1)

In equation (1), R is the direction cosine matrix used to convert the acceleration value measured by the acceleration sensor into the original value of the motion acceleration of the UAV in the horizontal heading coordinate system (the direction cosine when the yaw angle is 0 matrix), a _n is the original value of the motion acceleration of the UAV in the horizontal heading coordinate system, and a _b is the acceleration value measured by the acceleration sensor (in cm/s/s). Here, due to the use of the direction cosine matrix, the use of trigonometric functions is reduced, the computing power requirement for the computing device carried by the UAV is reduced, and the computing efficiency of the computing device is improved.

For the convenience of description, the unit of all motion accelerations below is cm/s/s, the unit of motion speed is cm/s, and the unit of relative displacement is cm.

In some embodiments, obtaining the motion acceleration correction value of the UAV under the horizontal heading coordinate system includes: based on the relative displacement value measured by the optical flow sensor, the relative height value measured by the height sensor, and the acceleration measured by the acceleration sensor value, and the angular velocity value measured by the gyroscope sensor, to obtain the relative displacement value of the UAV after altitude and angle compensation in the horizontal heading coordinate system; The relative displacement value after the altitude and angle compensation in the horizontal heading coordinate system and the relative displacement value after complementary filtering are obtained to obtain the relative displacement difference after the altitude and angle compensation of the UAV in the horizontal heading coordinate system; and based on the UAV in the The relative displacement difference and the first self-adjusting integral coefficient after the altitude and angle compensation in the horizontal heading coordinate system are used to obtain the motion acceleration correction value of the UAV in the horizontal heading coordinate system.

For example, according to equations (2) and (2-1), based on the relative displacement value and the first self-adjusting integral coefficient after the altitude and angle compensation of the UAV in the horizontal heading coordinate system, the horizontal heading coordinate of the UAV can be obtained The motion acceleration correction value under the system:

In equations (2) and (2-1), a _{n _ x _ correction} , a _{n _ y _ correction} are respectively the horizontal direction (x direction) and vertical direction (y direction) of the UAV in the horizontal heading coordinate system ) motion acceleration correction value, S _{opt _ x} , S _{opt y} are the relative displacement values after the height and angle compensation of the UAV in the x direction and y direction under the horizontal heading coordinate system respectively, S _{opt _ x _ dealt} , S _{opt _ y _ dealt} is the relative displacement difference of the UAV in the x direction and y direction under the horizontal heading coordinate system and the angle after compensation, and S _{n _ x _filter} and S _{n _ y _filter} are respectively Relative displacement values after complementary filtering in the x-direction and y-direction in the horizontal heading coordinate system, k1 is the first self-adjusting integral coefficient. Here, the initial values of a _{n _ x _ correction} and a _{n _ y _ correction} are both zero.

In some embodiments, obtaining the position of the UAV under the horizontal course coordinate system includes: based on the original motion acceleration value and the motion acceleration correction value of the UAV under the horizontal course coordinate system, obtaining the complementary position of the UAV under the horizontal course coordinate system. The filtered motion acceleration value; and based on the complementary filtered motion acceleration value of the drone in the horizontal heading coordinate system, the original value of the motion speed of the drone in the horizontal heading coordinate system is obtained.

For example, according to equation (3), based on the original motion acceleration value and the motion acceleration correction value of the UAV in the horizontal heading coordinate system, the complementary filtered motion acceleration value of the UAV in the horizontal heading coordinate system can be obtained.

In equation (3), a _{n _ x _filter} , a _{n _ y _filter} are the motion acceleration values after complementary filtering in the x direction and y direction of the UAV in the horizontal heading coordinate system, respectively, and a _{n _x_ origin} , a _{n _y_ origin} are the original values of motion acceleration in the x direction and y direction of the UAV in the horizontal heading coordinate system obtained according to equation (1), and a _{n _ x _ correction} , a _{n _ y _ correction} are the unmanned The correction value of the motion acceleration in the x direction and y direction of the aircraft in the horizontal heading coordinate system.

For example, according to equations (4) and (5), the original value of the motion velocity of the UAV in the horizontal heading coordinate system can be obtained based on the complementary filtered motion acceleration value of the UAV in the horizontal heading coordinate system.

In equation (4), V _{n _ x _origion} , V _{n _ x _origion} are the original values of the movement speed of the UAV in the x direction and y direction in the horizontal heading coordinate system, respectively, V _{n _ x _dealt} , V _{n _ y_dealt} is the speed increment of the drone in the x direction and y direction in the horizontal heading coordinate system, respectively, within a unit time (for example, 1s), and T represents time.

In some embodiments, obtaining the position of the UAV in the horizontal heading coordinate system may also include: based on the relative displacement difference and the second self-adjusting integral coefficient after the height and angle compensation of the UAV in the horizontal heading coordinate system, obtaining The movement speed correction value of the UAV in the horizontal course coordinate system; based on the original movement speed value and the movement speed correction value of the UAV in the horizontal course coordinate system, the complementary filtered movement speed of the UAV in the horizontal course coordinate system is obtained value.

For example, according to equation (6), based on the relative displacement difference and the second self-adjusting integral coefficient after the altitude and angle compensation of the drone in the horizontal heading coordinate system, the movement speed of the UAV in the horizontal heading coordinate system can be obtained correction value.

In equation (6), V _{n _ x _ correction} and V _{n _ y _ correction} are the movement speed correction values of the drone in the x direction and y direction under the horizontal heading coordinate system respectively, S _{opt _ x _ dealt} , S _{opt _ y _ dealt} is the relative displacement difference after the height and angle compensation of the drone in the x direction and y direction under the horizontal heading coordinate system, and k2 is the second self-adjusting integral coefficient. Here, the initial values of V _{n _ x _ correction} and V _{n _ y _ correction} are both zero.

For example, according to equation (7), based on the original value of the motion speed and the correction value of the motion speed of the UAV in the horizontal heading coordinate system, the complementary filtered motion speed value of the UAV in the horizontal heading coordinate system can be obtained.

In equation (7), V _{n _ x _ filter} , V _{n _ y _ filter} are the motion velocity values after complementary filtering in the x direction and y direction of the UAV in the horizontal heading coordinate system, V _{n _ x _origion} , V _{n _ x _origion} are the original values of the motion speed of the drone in the x direction and y direction in the horizontal heading coordinate system, respectively, V _{n _ x _ correction} , V _{n _ y _ correction} are the horizontal heading coordinates of the drone respectively The correction value of the movement speed in the x direction and y direction under the system.

In some embodiments, obtaining the position of the UAV in the horizontal heading coordinate system may also include: based on the motion velocity value after complementary filtering of the UAV in the horizontal heading coordinate system, obtaining the relative position of the UAV in the horizontal heading coordinate system. Shift the original value.

For example, the original value of the relative displacement of the UAV in the horizontal heading coordinate system can be obtained based on the complementary filtered motion velocity value of the UAV in the horizontal heading coordinate system according to equation (8).

In equation (8), S _{n _ x _origion} , S _{n _ y _origion} are the relative displacement original values of the UAV in the x direction and y direction under the horizontal heading coordinate system, V _{n _ x _ filter} , V _{n _ y _ filter} is the motion velocity value after complementary filtering in the x direction and y direction of the UAV in the horizontal course coordinate system, V _{n _ x _dealt} , V _{n _ y _dealt} are the UAV in the horizontal course coordinate system The speed increment in unit time (for example, 1s) in the x direction and y direction of , T represents time.

In some embodiments, obtaining the position of the UAV in the horizontal heading coordinate system may also include: based on the relative displacement difference and the third self-adjusting integral coefficient after the height and angle compensation of the UAV in the horizontal heading coordinate system, obtaining The relative displacement correction value of the UAV in the horizontal course coordinate system; and based on the relative displacement initial value and the relative displacement correction value of the UAV in the horizontal course coordinate system, obtain the relative displacement after complementary filtering of the UAV in the horizontal course coordinate system displacement value.

For example, according to equation (9), the relative displacement of the UAV in the horizontal course coordinate system can be obtained based on the relative displacement difference after the altitude and angle compensation of the UAV in the horizontal course coordinate system and the third self-adjusting integral coefficient correction value.

In equation (9), S _{n _ x _ correction} , S _{n _ y _ correction} are the relative displacement correction values of the drone in the x direction and y direction under the horizontal heading coordinate system respectively, S _{opt _ x _ dealt} , S _{opt _ y _ dealt} is the relative displacement difference after the height and angle compensation of the UAV in the x direction and y direction under the horizontal heading coordinate system, and k3 is the third self-adjusting integral coefficient. Here, the initial values of S _{n _ x _ correction} and S _{n _ y _ correction} are both zero.

For example, according to equation (10), based on the relative displacement initial value and the relative displacement correction value of the UAV in the horizontal course coordinate system, the complementary filtered relative displacement value of the UAV in the horizontal course coordinate system can be obtained.

In equation (10), S _{n _ x _filter} , S _{n _ y _filter} are the relative displacement values of the UAV after complementary filtering in the x direction and y direction in the horizontal heading coordinate system, respectively, S _{n _ x _origion} , S _{n _ y _origion} are the original relative displacement values of the UAV in the x-direction and y-direction under the horizontal course coordinate system, respectively, and S _{n _ x _ correction} , S _{n _ y _ correction} are the relative displacement values of the UAV under the horizontal course coordinate system. Relative displacement correction values in the x-direction and y-direction.

In some embodiments, the first, second, and third self-tuning integral coefficients are determined based on the self-tuning filter coefficients and the first, second, and third constants, respectively. For example, the first, second, and third self-adjusting integral coefficients may be determined based on the self-adjusting filter coefficients and the first, second, and third constants according to equation (11).

In equation (11), k1, k2, and k3 are the first, second, and third self-adjusting integral coefficients respectively, and k _filter is the self-adjusting filter coefficient, which needs to be adjusted dynamically to obtain the smooth and real Relative displacement, e 1, e 2, e 3 are the first, second, and third constants respectively.

In some embodiments, by monitoring the movement speed of the UAV relative to the detected plane, it can be judged whether the UAV is in the state of take-off, landing, ascending, descending, or maneuvering flight, and adjust the self-adjusting filter according to the state of the UAV. coefficient.

For example, according to equation (12), the fusion motion speed of the drone in the x direction, y direction, and z direction (vertical direction) in the horizontal heading coordinate system can be obtained, and then according to equation (13), The self-adjusting filter coefficients are self-adjustingly adjusted based on the fused motion speed of the UAV in the horizontal heading coordinate system.

In equation (12), V _{n _ mix} is the fusion motion speed of the drone in the x direction, y direction, and z direction in the horizontal heading coordinate system, V _{n _ x _ filter} , V _{n _ y _ filter} , V _{n _ z _ filter} are the motion velocity values after complementary filtering in the x direction, y direction, and z direction of the UAV in the horizontal heading coordinate system, respectively.

不變，當 V _{n_mix} 處在(B，C]區間時，使用函數g(V _{n_mix} )計算

的調節係數，其中，函 數g(V _{n_mix} )的輸出值在〔1，D]區間中，D為在速度C下測得的最終融合速度，位移良好的最大調節係數。 In equation (13), V _{n _ mix} is limited in the interval [0, C], the value of C depends on the upper limit of the measurable speed range of the optical flow sensor and the upper limit of the speed of the drone, and the value of B is The value depends on the speed tolerance of the drone hovering. When V _{n _ mix} is in the [0,B] interval, keep the coefficient

unchanged, when V _{n _ mix} is in the (B, C] interval, use the function g ( V _{n _ mix} ) to calculate

The adjustment coefficient of , wherein, the output value of the function g ( V _{n _ mix} ) is in the [1, D] interval, D is the final fusion speed measured at the speed C, and the maximum adjustment coefficient with good displacement.

進行動態調節。 Here, according to equation (14), based on the data quality parameter Q _opt of the optical flow sensor, the original filter coefficient

Make dynamic adjustments.

為在第一品質閾值A的條件下整定得到的 原始互補濾波係數。如等式(14)所示，當光流感測器的資料品質參數Q _opt 小於 第一品質閾值A時，

為原始互補濾波係數

；當光流感測器的資料品質 參數Q _opt 在[A,B)區間時(B為衡量光流感測器的資料品質的第二品質閾值)， 使用函數f(Q _opt )計算

的調節係數，其中，函數f(Q _opt )的輸出範圍在(0,1]區間中。 In equation (14),

is the original complementary filter coefficient obtained by setting under the condition of the first quality threshold A. As shown in equation (14), when the data quality parameter Qopt of the optical flow sensor _is less than the first quality threshold A,

is the original complementary filter coefficient

; When the data quality parameter Q _opt of the optical flow sensor is in the [A, B) interval (B is the second quality threshold for measuring the data quality of the optical flow sensor), use the function f ( Q _opt ) to calculate

The adjustment coefficient of , where the output range of the function f ( Q _opt ) is in the (0,1] interval.

To sum up, the positioning method for UAVs according to the embodiment of the present invention can be closer to Realistic, smoother real-time, and more precise positioning of drones. In addition, by self-adjusting the self-adjusting filter coefficient according to the motion speed after the complementary filtering of the UAV in the horizontal heading coordinate system, the real displacement of the UAV can be restored with high precision when the UAV is in a hovering and flying state. and speed conditions. In addition, when the UAV is in a maneuvering flight state such as hovering or navigation control, target following, and the environment changes, the real displacement and speed of the UAV can be restored with high accuracy. At the same time, the UAV is positioned based on the basic sensors of the UAV control system (ie, acceleration sensor, gyroscope sensor, altitude sensor, and optical flow sensor), without adding additional hardware Cost and load, small amount of calculation, suitable for various occasions that are sensitive to cost and limited in the volume and load capacity of drones.

Fig. 2 shows a schematic diagram of a computer system that can implement a displacement compensation method and device for a multi-rotor UAV according to an embodiment of the present invention. A computer system 200 suitable for implementing the embodiment of the present invention will be described below with reference to FIG. 2 . It should be understood that the computer system 200 shown in FIG. 2 is only an example, and should not limit the functions and scope of use of the embodiments of the present invention.

As shown in FIG. 2 , the computer system 200 may include a processing device (for example, a central processing unit, a graphics processing unit, etc.) 201, which may be stored in a read-only memory (Read Only Memory, ROM) 202 according to a program or from a memory The device 208 is loaded into random access memory (Random Access Memory. RAM) 203 to execute various appropriate actions and processes. In the RAM 203, various programs and data necessary for the operation of the computer system 200 are also stored. The processing device 201 , ROM 202 , and RAM 203 are connected to each other through a bus bar 204 . An input/output (I/O) interface 205 is also connected to the bus bar 204 .

Generally, the following devices can be connected to the I/O interface 205: an input device 206 including, for example, a touch screen, a touchpad, a camera, an accelerometer, a gyroscope, a sensor, etc.; including, for example, a liquid crystal display (Liquid Crystal Display, LCD), a speaker, a vibrator, a motor, an electronic speed controller, etc.; a storage device 208 including, for example, a flash card (Flash Card); and a communication device 209. The communication device 209 may allow the computer system 200 to communicate with other devices wirelessly or by wire to exchange data. While FIG. 2 shows computer system 200 with various devices, it should be understood that implementing or possessing all of the devices shown is not a requirement. More or fewer means may alternatively be implemented or provided. Each block shown in FIG. 2 may represent one device, or may represent multiple devices as required.

In particular, according to an embodiment of the present invention, the processes described above with reference to the flowcharts may be implemented as computer software programs. For example, an embodiment of the present disclosure provides a computer-readable storage medium storing a computer program including program codes for executing the method 100 shown in FIG. 1 . In such an embodiment, the computer program may be downloaded and installed from the network via the communication device 209 , or installed from the storage device 208 , or installed from the ROM 202 . When the computer program is executed by the processing device 201, the above-mentioned functions defined in the device according to the embodiment of the present invention are realized.

It should be noted that the computer-readable medium according to the embodiment of the present invention may be a computer-readable signal medium or a computer-readable storage medium or any combination of the above two. A computer-readable storage medium may be, for example, but not limited to, an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, device, or device, or any combination thereof. More specific examples of computer-readable storage media may include, but are not limited to, electrical connections with one or more wires, portable computer diskettes, hard disks, random access memory (RAM), read-only memory ( ROM), erasable programmable read only memory (Erasable Programmable Read Only Memory, EPROM or flash memory), optical fiber, compact disc read only memory (Compact Disc Read Only Memory, CD-ROM), optical memory components, magnetic memory components, or any suitable combination of the above. A computer-readable storage medium according to an embodiment of the present invention may be any tangible medium that contains or stores a program that can be executed by an instruction execution system, installed configuration or device use or in conjunction with it. In addition, a computer readable signal medium according to an embodiment of the present invention may include a data signal carrying computer readable program code in a baseband or as part of a carrier wave. Such propagated data signals may take a variety of forms, including but not limited to electromagnetic signals, optical signals, or any suitable combination of the foregoing. A computer-readable signal medium may also be any computer-readable medium other than a computer-readable storage medium, which can send, propagate, or transmit a program for use by or in conjunction with an instruction execution system, apparatus, or device. . The program code contained on the computer readable medium may be transmitted by any suitable medium, including but not limited to: wires, optical cables, radio frequency (Radio Frequency, RF), etc., or any suitable combination of the above.

Computer program code for performing operations in accordance with embodiments of the present invention may be written in one or more programming languages, or combinations thereof, including object-oriented programming languages—such as Java, Smalltalk, C++, and Includes conventional procedural programming languages -- such as "C" or similar programming languages. The code may execute entirely on the user's computer, partly on the user's computer, as a stand-alone package, partly on the user's computer and partly on a remote computer, or entirely on the remote computer or executed on the server. In cases involving a remote computer, the remote computer can be connected to the user computer through any kind of network, including a Local Area Network (LAN) or a Wide Area Network (WAN), Alternatively, it is possible to connect to an external computer (eg, via an Internet connection using an Internet service provider).

The flowchart and block diagrams in the figures illustrate the architecture, functionality, and operation of possible implementations of systems, methods and computer program products according to various embodiments of the present invention. In this regard, each block in a flowchart or block diagram may represent a module, program segment, or portion of code that includes one or more Executable instructions for logical functions. It should also be noted that, in some alternative implementations, the functions noted in the block may occur out of the order noted in the figures. For example, two blocks shown in succession may, in fact, be executed substantially concurrently, or they may sometimes be executed in the reverse order, depending upon the functionality involved. It should also be noted that each block in the block diagrams and/or flowcharts, and combinations of blocks in the block diagrams and/or flowcharts, can be implemented with a dedicated hardware-based computer that performs the specified function or operation. system, or may be implemented using a combination of dedicated hardware and computer instructions.

The present invention may be embodied in other specific forms without departing from its spirit and essential characteristics. For example, the algorithms described in certain embodiments may be modified without departing from the basic spirit of the invention in terms of system architecture. Therefore, the current embodiments are to be considered in all respects as illustrative rather than restrictive, the scope of the present invention is defined by the appended claims rather than the above description, and what falls within the meanings and equivalents of the claims All changes in scope are thereby embraced within the scope of the invention.

100, S102, S104, S106: positioning method

Claims (13)

A positioning method for a drone, wherein the drone is equipped with an acceleration sensor, a gyroscope sensor, a height sensor, and an optical flow sensor, and the positioning method includes: Based on the acceleration value measured by the acceleration sensor and the angular velocity value measured by the gyroscope sensor, the original value of the motion acceleration of the drone in the horizontal heading coordinate system is obtained; Based on the relative displacement value measured by the optical flow sensor, the relative height value measured by the height sensor, the acceleration value measured by the acceleration sensor, the angular velocity value measured by the gyro sensor, and the The original value of the motion acceleration of the UAV in the horizontal heading coordinate system, and obtain the correction value of the motion acceleration of the UAV in the horizontal heading coordinate system; and Based on the original motion acceleration value and the motion acceleration correction value of the UAV in the horizontal heading coordinate system, the position of the UAV in the horizontal heading coordinate system is acquired. The positioning method as described in claim 1, wherein obtaining the original value of the motion acceleration of the UAV in the horizontal heading coordinate system includes: Based on the acceleration value measured by the acceleration sensor and the angular velocity value measured by the gyroscope sensor, the acquisition is used to convert the acceleration value measured by the acceleration sensor into the UAV in the horizontal heading coordinate system The direction cosine matrix of the original value of the motion acceleration of ; and Based on the acceleration value measured by the acceleration sensor and the direction cosine matrix, the original value of the motion acceleration of the drone in the horizontal heading coordinate system is obtained, Wherein, the acceleration value measured by the acceleration sensor is the motion acceleration value of the UAV in the UAV carrier coordinate system. The positioning method as described in claim 1, wherein obtaining the motion acceleration correction value of the UAV in the horizontal heading coordinate system includes: Based on the relative displacement value measured by the optical flow sensor, the relative height value measured by the height sensor, the acceleration value measured by the acceleration sensor, and the angular velocity value measured by the gyroscope sensor, obtain The relative displacement value after the height and angle compensation of the UAV under the horizontal heading coordinate system; Based on the relative displacement value after the altitude and angle compensation of the UAV in the horizontal course coordinate system and the relative displacement value after complementary filtering, obtain the relative displacement of the UAV in the horizontal course coordinate system after altitude and angle compensation difference; and Based on the relative displacement difference after the altitude and angle compensation of the UAV in the horizontal heading coordinate system and the first self-adjusting integral coefficient to obtain the motion acceleration correction value of the UAV in the horizontal heading coordinate system, Wherein, the first self-adjusting integral coefficient is determined based on the self-adjusting filter coefficient and the first constant, and the self-adjusting filter coefficient is based on the original value of the motion acceleration and the motion acceleration correction of the UAV in the horizontal heading coordinate system The value is determined. The positioning method as described in claim 3, wherein obtaining the position of the UAV under the horizontal heading coordinate system includes: Based on the original motion acceleration value and the motion acceleration correction value of the UAV in the horizontal heading coordinate system, obtain the complementary filtered motion acceleration value of the UAV in the horizontal heading coordinate system; and Based on the motion acceleration value after complementary filtering of the UAV in the horizontal heading coordinate system, the original value of the motion speed of the UAV in the horizontal heading coordinate system is obtained. The positioning method as described in claim 4, wherein obtaining the position of the UAV in the horizontal heading coordinate system also includes: Based on the altitude and angle-compensated relative displacement difference and the second self-adjusting integral coefficient of the UAV in the horizontal course coordinate system, obtain the motion speed correction value of the UAV in the horizontal course coordinate system; and Based on the original value of the motion speed and the correction value of the motion speed of the UAV in the horizontal heading coordinate system, the complementary filtered motion speed value of the UAV in the horizontal heading coordinate system is obtained, Wherein, the second self-adjusting integral coefficient is determined based on the self-adjusting filter coefficient and the second constant, and the self-adjusting filter coefficient is determined based on the complementary filtered motion velocity value of the UAV in the horizontal heading coordinate system of. The positioning method as described in claim 5, wherein obtaining the position of the UAV in the horizontal heading coordinate system also includes: Based on the motion velocity value after complementary filtering of the UAV in the horizontal course coordinate system, the relative displacement original value of the UAV in the horizontal course coordinate system is acquired. The positioning method as described in claim 6, wherein obtaining the position of the UAV in the horizontal heading coordinate system also includes: Based on the relative displacement difference after the altitude and angle compensation of the UAV in the horizontal heading coordinate system and the third self-adjusting integral coefficient, to obtain the relative displacement correction value of the UAV in the horizontal heading coordinate system; and Based on the relative displacement initial value and the relative displacement correction value of the UAV in the horizontal course coordinate system, the complementary filtered relative displacement value of the UAV in the horizontal course coordinate system is obtained, Wherein, the third self-adjusting integral coefficient is determined based on the self-adjusting filter coefficient and a third constant. The positioning method according to claim 3, wherein the process of determining the self-adjusting filter coefficient includes: Based on the original motion acceleration value and the motion acceleration correction value of the UAV in the horizontal heading coordinate system, the complementary filtered motion acceleration value of the UAV in the horizontal heading coordinate system is obtained; Based on the complementary filtered motion acceleration value of the UAV in the horizontal heading coordinate system, the original value of the motion speed of the UAV in the horizontal heading coordinate system is obtained; Based on the relative displacement difference and the second self-adjusting integral coefficient after the altitude and angle compensation of the UAV in the horizontal course coordinate system, the movement speed correction value of the UAV in the horizontal course coordinate system is obtained; Based on the original motion speed value and the correction value of the motion speed of the UAV in the horizontal heading coordinate system, obtain the complementary filtered motion speed value of the UAV in the horizontal heading coordinate system; and determining the self-adjusting filter coefficient based on the complementary filtered motion velocity value of the UAV in the horizontal heading coordinate system, Wherein, the second self-adjusting integral coefficient is determined based on the self-adjusting filter coefficient and the second constant, and the self-adjusting filter coefficient is based on the complementary filtered motion speed of the UAV in the horizontal heading coordinate system The value is determined, and the initial value of the self-adjusting filter coefficient depends on the data quality parameter of the optical flow sensor, the data quality threshold used to measure the data quality of the optical flow sensor, and the use of the data The predetermined value of the original complementary filter coefficient obtained by quality threshold adjustment. The positioning method according to claim 8, wherein the initial values of the original value of the motion speed, the correction value of the motion speed, and the correction value of the motion acceleration of the UAV in the horizontal heading coordinate system are all zero. The positioning method according to claim 8, wherein, based on the data quality parameters of the optical flow sensor, the data quality threshold used to measure the data quality of the optical flow sensor, using the The self-adjusting filter coefficient is determined based on the original complementary filter coefficient obtained by setting the data quality threshold and the motion speed value of the UAV after complementary filtering in the horizontal heading coordinate system. The positioning method according to claim 7, wherein the initial value of the relative displacement original value and the relative displacement correction value of the UAV in the horizontal heading coordinate system are both zero. A positioning device for a drone, wherein the drone is equipped with an acceleration sensor, a gyroscope sensor, a height sensor, and an optical flow sensor, and the positioning device includes: memory on which computer-executable instructions are stored; and One or more processors configured to execute the computer-executable instructions to implement the positioning method described in any one of claims 1 to 11. A drone comprising: acceleration sensor; Gyro sensor; height sensor; optical flow sensor; and The positioning device for unmanned aerial vehicles described in claim 12.

Images