A method, device, equipment and medium for estimating heading of unmanned aerial vehicle

Abstract

The present application provides a method, device, equipment and medium for estimating the heading of a drone, including: during the drone inspection process, determining the inspection stage of the drone based on the magnetic compass data; when the drone is in the normal stage of the magnetic compass data, calculating the standard direction vector of the cable based on the relative position of the cable and the drone; after the drone enters the strong magnetic interference stage from the normal stage of the magnetic compass data, calculating the error direction vector of the cable in the strong magnetic interference stage based on the relative position of the cable and the drone; determining the heading angle of the drone in the strong magnetic interference stage based on the standard direction vector and the error direction vector. The present application estimates the heading angle based on the standard direction vector and the error direction vector in the strong magnetic interference stage, and does not rely on the magnetic compass data, so that the correct heading angle can be obtained even in the case of strong magnetic interference, ensuring that the heading estimation of the drone is correct during the entire process of approaching the transmission line for inspection, and reducing the risk of the drone exploding.

Description

Technical Field

[0001] The present application relates to the field of power inspection, and in particular to a method, device, equipment and medium for estimating the heading of an unmanned aerial vehicle. Background Art

[0002] Small drones have been widely used in the field of power inspections, using drones to inspect power towers, cables, etc. at a certain distance. As inspection requirements increase, some mission scenarios require drones to be docked on cables and use the equipment on the drones to conduct close inspections of the cables, which poses a challenge to the drone's heading estimation.

[0003] There is electromagnetic interference near high-voltage transmission lines, and the closer the drone is to the cables, the stronger the electromagnetic interference. In the current drone navigation method, the magnetic compass is mainly used to correct the heading angle of the drone. However, when the drone is subject to strong magnetic interference, using the magnetic compass to correct the heading angle will lead to inaccurate heading estimation, affecting the flight stability of the drone, and in severe cases, causing the drone to explode. Summary of the invention

[0004] In view of this, the present application provides a method, device, equipment and medium for estimating the heading angle of a drone, which is used to estimate the heading angle of the drone without relying on a magnetic compass during a strong magnetic interference stage. The technical solution is as follows:

[0005] A method for estimating a heading of an unmanned aerial vehicle, comprising:

[0006] During the inspection process of the drone, the inspection stage of the drone is determined according to the magnetic compass data, wherein the inspection stage includes the strong magnetic interference stage and the normal magnetic compass data stage;

[0007] When the UAV is in the normal stage of magnetic compass data, the standard direction vector of the cable is calculated according to the relative position of the cable and the UAV;

[0008] After the UAV enters the strong magnetic interference stage from the normal stage of magnetic compass data, the error direction vector of the cable in the strong magnetic interference stage is calculated according to the relative position of the cable and the UAV;

[0009] According to the standard direction vector and the error direction vector, the heading angle of the UAV during the strong magnetic interference stage is determined.

[0010] Optionally, the inspection phase of the drone is determined based on the magnetic compass data, including:

[0011] If the modulus value of the magnetic compass data is less than a preset first threshold, or the modulus value of the magnetic compass data is greater than a preset second threshold, it is determined that the drone is in a strong magnetic interference stage; otherwise, it is determined that the drone is in a normal magnetic compass data stage, wherein the first threshold is less than the second threshold.

[0012] Optionally, when the drone is in the normal stage of magnetic compass data, the standard direction vector of the cable is calculated according to the relative position of the cable and the drone, including:

[0013] When the UAV is in a normal stage of magnetic compass data, coordinates of any two points on the cable relative to the UAV are obtained, and the two obtained coordinates are used as the first coordinate and the second coordinate respectively;

[0014] Calculate a standard direction vector of the cable according to the first coordinate and the second coordinate;

[0015] After the drone enters the strong magnetic interference stage from the normal stage of magnetic compass data, the error direction vector of the cable in the strong magnetic interference stage is calculated according to the relative position of the cable and the drone, including:

[0016] After the drone enters the strong magnetic interference stage from the normal stage of magnetic compass data, the coordinates of any two points on the cable relative to the drone are obtained, and the two obtained coordinates are used as the third coordinate and the fourth coordinate respectively;

[0017] According to the third coordinate and the fourth coordinate, the error direction vector of the cable in the strong magnetic interference stage is calculated.

[0018] Optionally, obtain the coordinates of any two points on the cable relative to the drone, including:

[0019] The coordinates of any two points on the cable relative to the drone are obtained through the laser radar carried on the drone.

[0020] Optionally, the heading angle of the UAV in the strong magnetic interference stage is determined according to the standard direction vector and the error direction vector, including:

[0021] Determine the direction error correction amount of the cable according to the standard direction vector and the error direction vector;

[0022] The gravity acceleration vector is corrected according to the coordinate conversion matrix from the inertial coordinate system to the drone's body coordinate system and the first direction vector of the inertial coordinate system to obtain the acceleration error correction value;

[0023] The heading angle of the UAV during the strong magnetic interference stage is determined based on the direction error correction, acceleration error correction, gyroscope angular velocity measurement value, dynamic error correction factor and static error correction factor.

[0024] Optionally, the heading angle of the UAV in the strong magnetic interference stage is determined according to the direction error correction, the acceleration error correction, the angular velocity measurement value of the gyroscope, the dynamic error correction factor and the static error correction factor, including:

[0025] Calculating a first integral correction amount of the gyroscope angular velocity according to the direction error correction amount, the acceleration error correction amount, the angular velocity measurement value of the gyroscope, the dynamic error correction factor and the static error correction factor;

[0026] The heading angle of the UAV during the strong magnetic interference stage is determined based on the first integral correction of the gyroscope angular velocity.

[0027] Optionally, also include:

[0028] When the UAV is in the normal stage of magnetic compass data, the magnetic compass data is corrected according to the coordinate conversion matrix from the inertial coordinate system to the UAV's body coordinate system and the second direction vector of the inertial coordinate system to obtain the error correction value of the magnetic compass;

[0029] Calculating a second integral correction amount of the gyroscope angular velocity according to the error correction amount of the magnetic compass, the error correction amount of the acceleration, the angular velocity measurement value of the gyroscope, the dynamic error correction factor and the static error correction factor;

[0030] The heading angle of the UAV during the normal phase of the magnetic compass data is determined based on the second integral correction of the gyroscope angular velocity.

[0031] A heading estimation device for an unmanned aerial vehicle, comprising:

[0032] The inspection stage determination module is used to determine the inspection stage of the drone according to the magnetic compass data during the inspection of the drone, wherein the inspection stage includes the strong magnetic interference stage and the normal magnetic compass data stage;

[0033] A standard direction vector determination module is used to calculate the standard direction vector of the cable according to the relative position of the cable and the UAV when the UAV is in the normal stage of magnetic compass data;

[0034] The error direction vector determination module is used to calculate the error direction vector of the cable in the strong magnetic interference stage according to the relative position of the cable and the drone after the drone enters the strong magnetic interference stage from the normal stage of magnetic compass data;

[0035] The heading angle determination module in the strong magnetic stage is used to determine the heading angle of the UAV in the strong magnetic interference stage according to the standard direction vector and the error direction vector.

[0036] A heading estimation device for a drone, comprising a memory and a processor;

[0037] Memory, used to store programs;

[0038] The processor is used to execute the program to implement each step of the heading estimation method for a UAV as described in any one of the above items.

[0039] A readable storage medium stores a computer program, and when the computer program is executed by a processor, each step of the method for estimating the heading of a UAV as described above is implemented.

[0040] Through the above technical solutions, it can be known that the heading estimation method of the UAV provided by the present application determines the inspection stage of the UAV according to the magnetic compass data during the inspection of the UAV. When the UAV is in the normal stage of the magnetic compass data, the standard direction vector of the cable is calculated according to the relative position of the cable and the UAV. After the UAV enters the strong magnetic interference stage from the normal stage of the magnetic compass data, the error direction vector of the cable in the strong magnetic interference stage is calculated according to the relative position of the cable and the UAV. The heading angle of the UAV in the strong magnetic interference stage is determined according to the standard direction vector and the error direction vector. The present application can estimate the heading angle based on the standard direction vector and the error direction vector in the strong magnetic interference stage, without relying on the magnetic compass data, so that the correct heading angle can be obtained even in the case of strong magnetic interference, ensuring the correct heading estimation of the UAV during the entire process of approaching the transmission line inspection, and reducing the risk of the UAV exploding. BRIEF DESCRIPTION OF THE DRAWINGS

[0041] In order to more clearly illustrate the embodiments of the present application or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on the provided drawings without paying any creative work.

[0042] Figure 1 A schematic diagram of a flow chart of a method for estimating the heading of a drone provided in an embodiment of the present application;

[0043] Figure 2 A schematic diagram of the route of the drone inspection provided in the embodiment of the present application;

[0044] Figure 3 A schematic diagram of the structure of a heading estimation device for a drone provided in an embodiment of the present application;

[0045] Figure 4 This is a hardware structure block diagram of the heading estimation device for the drone provided in an embodiment of the present application. DETAILED DESCRIPTION

[0046] The following will be combined with the drawings in the embodiments of the present application to clearly and completely describe the technical solutions in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.

[0047] The present application provides a method for estimating the heading of a drone. Heading estimation refers to obtaining the heading angle of the drone body coordinate system relative to the inertial coordinate system based on various sensor information.

[0048] Optionally, the heading estimation method provided in the present application is suitable for scenarios of close-range inspection of high-voltage transmission lines. The heading estimation method for the drone provided in the present application is described in detail through the following embodiments.

[0049] See also Figure 1 , shows a schematic flow chart of a method for estimating the heading of a drone provided in an embodiment of the present application, and the method for estimating the heading of a drone may include:

[0050] Step S101: during the inspection of the drone, the inspection stage of the drone is determined according to the magnetic compass data.

[0051] Among them, the inspection stage includes the strong magnetic interference stage and the normal magnetic compass data stage.

[0052] During the strong magnetic interference stage, the drone approaches the high-voltage transmission cable, and the distance between the two is relatively close. The magnetic compass integrated on the drone will be affected by the electromagnetic interference, resulting in inaccurate magnetic compass data. During the normal magnetic compass data stage, the drone is far away from the cable, the magnetic compass is not affected by electromagnetic interference, or is less affected by electromagnetic interference, and the magnetic compass data is relatively accurate.

[0053] In this step, each location inspected by the drone has corresponding magnetic compass data, and based on the magnetic compass data, it can be determined whether the drone is in a strong magnetic interference stage or a normal magnetic compass data stage when it is at the corresponding location.

[0054] Optionally, the route of the drone inspection can be pre-set, e.g. Figure 2 The schematic diagram of the drone inspection route is shown in the figure. The drone can follow Figure 2 Specifically, the UAV takes off from a take-off point far away from the bottom of the cable, and ascends and flies level to the top of the cable. During this process, since the UAV is far away from the cable, the magnetic compass data can be regarded as normal, and the UAV is in the normal magnetic compass data stage. When the UAV descends from the top of the cable, the UAV gradually approaches the cable, the magnetic compass data becomes less and less accurate, and the UAV enters the strong magnetic interference stage.

[0055] Step S102: when the UAV is in the normal stage of magnetic compass data, the standard direction vector of the cable is calculated according to the relative position of the cable and the UAV.

[0056] Here, the standard direction vector of the cable specifically refers to the standard direction vector of the cable in the inertial coordinate system.

[0057] Specifically, in this step, the first direction vector of the cable in the inertial coordinate system can be calculated according to the relative position of the cable and the drone when the magnetic compass data is normal. Since the direction of the cable hardly changes in the inertial space, the calculated first direction vector can be used as the standard direction vector of the cable.

[0058] Step S103: after the drone enters the strong magnetic interference stage from the normal stage of magnetic compass data, the error direction vector of the cable in the strong magnetic interference stage is calculated according to the relative position of the cable and the drone.

[0059] Here, the error direction vector of the cable during the strong magnetic interference stage specifically refers to the error direction vector of the cable in the inertial coordinate system during the strong magnetic interference stage.

[0060] Specifically, in this step, when the UAV is in the strong magnetic interference stage, according to the relative position of the cable and the UAV, the second direction vector of the cable in the inertial coordinate system is calculated as the error direction vector of the cable in the strong magnetic interference stage.

[0061] It is worth noting that in this step, at each moment (or each position) when the UAV is in the strong magnetic interference stage, the error direction vector of the cable corresponding to each moment (or each position) is calculated based on the relative position of the cable and the UAV. Then, the heading angle estimated in the following steps refers to a heading angle estimated at each moment (or each position).

[0062] Step S104: Determine the heading angle of the UAV in the strong magnetic interference stage according to the standard direction vector and the error direction vector.

[0063] During the strong magnetic interference stage, the magnetic compass data is no longer used for the heading estimation of the UAV. In order to accurately estimate the heading under this condition, the above steps S102 and S103 are used to calculate the standard direction vector of the cable and the error direction vector of the cable during the strong magnetic interference stage. Then, the heading angle of the UAV during the strong magnetic interference stage is determined based on the standard direction vector and the error direction vector. Compared with the prior art method of estimating the heading angle using magnetic compass data, this step can obtain a more accurate heading angle.

[0064] The method for estimating the heading of a drone provided by the present application determines the inspection stage of the drone based on the magnetic compass data during the drone inspection process. When the drone is in the normal stage of the magnetic compass data, the standard direction vector of the cable is calculated based on the relative position of the cable and the drone. After the drone enters the strong magnetic interference stage from the normal stage of the magnetic compass data, the error direction vector of the cable in the strong magnetic interference stage is calculated based on the relative position of the cable and the drone. The heading angle of the drone in the strong magnetic interference stage is determined based on the standard direction vector and the error direction vector. The present application can estimate the heading angle based on the standard direction vector and the error direction vector in the strong magnetic interference stage, without relying on the magnetic compass data, so that the correct heading angle can be obtained even in the case of strong magnetic interference, ensuring that the heading estimation of the drone is correct during the entire process of approaching the transmission line for inspection, and reducing the risk of the drone exploding.

[0065] In one embodiment of the present application, the magnetic compass data is defined as υ m , m is a 3×1 vector.

[0066] Considering that the magnetic compass data will be normalized before takeoff, when the magnetic compass data is normal, m | is around 1, so the above step S101 can be based on |υ m |Whether it is near 1, determine the inspection stage the drone is in.

[0067] Based on this, the process of the above step S101 "determining the inspection stage of the drone according to the magnetic compass data" may include: if the modulus value of the magnetic compass data |υ m | is less than the preset first threshold value υ min , or the modulus of the magnetic compass data is greater than the preset second threshold value v max , it is determined that the drone is in a strong magnetic interference stage, otherwise, it is determined that the drone is in a normal magnetic compass data stage, wherein the first threshold is less than the second threshold.

[0068] Specifically, the modulus value of the magnetic compass data |υ m |Withυ min and max For comparison, if |υ m |<υ min or |υ m |>υ max , it is considered that the magnetic compass is subject to strong magnetic interference, and the drone is determined to be in the strong magnetic interference stage; if υ min ≤|υ m |≤υ max , it is considered that the magnetic compass has not received strong magnetic interference, and it is determined that the drone is in the normal stage of magnetic compass data.

[0069] It is worth noting that the above ismin and max The value of the pre-set threshold can be determined according to actual conditions and is not specifically limited in this application.

[0070] In another embodiment of the present application, the aforementioned process of "step S102, when the UAV is in the normal stage of magnetic compass data, calculating the standard direction vector of the cable according to the relative position of the cable and the UAV" and "step S103, after the UAV enters the strong magnetic interference stage from the normal stage of magnetic compass data, calculating the error direction vector of the cable in the strong magnetic interference stage according to the relative position of the cable and the UAV" are introduced.

[0071] Specifically, the process of “step S102, when the UAV is in the normal stage of magnetic compass data, calculating the standard direction vector of the cable according to the relative position of the cable and the UAV” may include:

[0072] Step S1021: when the drone is in a normal stage of magnetic compass data, the coordinates of any two points on the cable relative to the drone are obtained, and the two obtained coordinates are used as the first coordinate and the second coordinate respectively.

[0073] In this step, the first coordinate is defined as (x1, y1, z1) and the second coordinate is defined as (x2, y2, z2).

[0074] Optionally, a laser radar can be mounted on a drone. In this case, the coordinates of any two points on the cable relative to the drone can be obtained through the laser radar mounted on the drone. Specifically, the drone can collect (i.e., collect in real time) the coordinates of multiple points on the cable relative to the drone at each moment. In this step, the coordinates of any two points relative to the drone can be selected from them as the first coordinate (x1, y1, z1) and the second coordinate (x2, y2, z2), respectively.

[0075] To facilitate drone inspections, the laser radar carried on the drone can be set at the bottom of the drone. Then, in this step, when the drone flies above the cable (preferably, the drone flies directly above the cable), the laser radar can be used to obtain the coordinates of the points on the cable relative to the drone.

[0076] Of course, using laser radar to identify cables is only an optional implementation of this step. In addition, this step can also be implemented in other ways. For example, a visual method can be used to implement cable identification to obtain the first coordinate and the second coordinate.

[0077] Step S1022: Calculate the standard direction vector of the cable according to the first coordinate and the second coordinate.

[0078] Optionally, this step may use the following formula (1) to construct the standard direction vector of the cable in the inertial coordinate system.

[0079] η0=T NB (x2-x1,y2-y1,0) T Formula (1)

[0080] Where, T NB is the coordinate transformation matrix from the drone body coordinate system to the inertial coordinate system, and η0 is the standard direction vector of the cable in the inertial coordinate system. Here, the drone body coordinate system (or drone body coordinate system) is based on the convention of drone design. Usually, the x-axis points to the front of the drone in the longitudinal symmetry plane, the z-axis points vertically downward, and the y-axis, x-axis, and z-axis form a right-handed coordinate system.

[0081] Corresponding to the aforementioned step S102, the process of "step S103, after the drone enters the strong magnetic interference stage from the normal stage of magnetic compass data, calculating the error direction vector of the cable in the strong magnetic interference stage according to the relative position of the cable and the drone" may include:

[0082] Step S1031, after the drone enters the strong magnetic interference stage from the normal stage of magnetic compass data, the coordinates of any two points on the cable relative to the drone are obtained, and the two obtained coordinates are used as the third coordinate and the fourth coordinate respectively.

[0083] In this step, the third coordinate is defined as (x3, y3, z3) and the second coordinate is defined as (x4, y4, z4).

[0084] Similar to the aforementioned step S1021, this step can also use laser radar to identify cables or visual methods to identify cables or other methods to obtain the third coordinate and the fourth coordinate. The specific process can refer to the introduction in step S1021 and will not be repeated here.

[0085] Step S1032: Calculate the error direction vector of the cable in the strong magnetic interference stage according to the third coordinate and the fourth coordinate.

[0086] Optionally, this step may use the following formula (2) to construct the error direction vector of the cable in the inertial coordinate system during the strong magnetic interference stage.

[0087] η t =T NB (x4-x3,y4-y3,0) T Formula (2)

[0088] Where η t is the error direction vector of the cable in the inertial coordinate system during the strong magnetic interference stage.

[0089] In summary, this embodiment collects cable data in the normal stage of magnetic compass data to construct a standard direction vector, and collects cable data in the strong magnetic interference stage to construct an error direction vector, so that the magnetic compass data is no longer used for heading angle estimation in the future, but the heading angle is estimated based on the standard direction vector and the error direction vector, ensuring accurate estimation of the heading.

[0090] The following embodiment introduces the process of "step S104, determining the heading angle of the UAV in the strong magnetic interference stage according to the standard direction vector and the error direction vector".

[0091] Specifically, the process of "step S104, determining the heading angle of the UAV in the strong magnetic interference stage according to the standard direction vector and the error direction vector" includes:

[0092] Step S1041, determining the direction error correction amount of the cable according to the standard direction vector and the error direction vector.

[0093] In this step, the following error function can be constructed, namely:

[0094] e l =η0×η t Formula (3)

[0095] In the formula, e l It refers to the direction error correction of the cable.

[0096] Step S1042: Correct the gravity acceleration vector according to the coordinate conversion matrix from the inertial coordinate system to the drone's body coordinate system and the first direction vector of the inertial coordinate system to obtain an acceleration error correction amount.

[0097] Here, the gravity acceleration vector is measured by the inertial device integrated on the drone.

[0098] In this step, the acceleration error correction can be obtained using the following formula (4):

[0099] e a =υ a ×(T BN ·e z ) Formula (4)

[0100] Among them, e a Refers to the error correction of acceleration, υ a Refers to the gravity acceleration vector measured by the inertial device, T BN It refers to the coordinate transformation matrix from the inertial coordinate system to the drone's body coordinate system, e z =[0,0,1] T It refers to the first direction vector of the inertial coordinate system. Here, the first direction specifically refers to the z direction.

[0101] Step S1043: Determine the heading angle of the UAV in the strong magnetic interference stage according to the direction error correction, the acceleration error correction, the angular velocity measurement value of the gyroscope, the dynamic error correction factor and the static error correction factor.

[0102] Specifically, this step can first calculate the first integral correction of the gyroscope angular velocity based on the direction error correction, the acceleration error correction, the gyroscope angular velocity measurement value, the dynamic error correction factor and the static error correction factor, and then determine the heading angle of the UAV in the strong magnetic interference stage based on the first integral correction of the gyroscope angular velocity.

[0103] Here, the first integral correction value of the gyroscope angular velocity can be obtained by using the following formula (5):

[0104] g′=g+k p (e a +e l )+k i ∫(e a +e l ) Formula (5)

[0105] Where g′ refers to the first integral correction of the gyroscope angular velocity, g refers to the angular velocity measurement value of the gyroscope, and k p refers to the dynamic error correction factor, k i Refers to the static error correction factor.

[0106] After obtaining the first integral correction of the gyroscope angular velocity, this step can use the differential equation of the attitude angle and angular velocity g′ to obtain the heading angle of the UAV during the strong magnetic interference stage, and can also obtain the pitch angle and roll angle of the UAV during the strong magnetic interference stage.

[0107] Here, the method of using the differential equation of the attitude angle and the angular velocity g′ to obtain the heading angle of the UAV in the strong magnetic interference stage is a prior art. For details, please refer to the description in the prior art and will not be described in detail here.

[0108] Through the above steps, the heading angle of the UAV can be accurately estimated when the magnetic compass is disturbed.

[0109] In an optional embodiment, this embodiment also provides a method for estimating a heading angle when the UAV is in a normal stage of magnetic compass data.

[0110] Specifically, the method for estimating the heading angle when the UAV is in the normal stage of magnetic compass data includes the following steps:

[0111] Step S201: When the drone is in a normal magnetic compass data stage, the magnetic compass data is corrected according to the coordinate conversion matrix from the inertial coordinate system to the drone's body coordinate system and the second direction vector of the inertial coordinate system to obtain an error correction value of the magnetic compass.

[0112] Here, the drone being in the normal magnetic compass data stage may be the drone being in the normal magnetic compass data stage after taking off, or entering the normal magnetic compass data stage from the strong magnetic interference stage.

[0113] In this step, the error correction of the magnetic compass can be obtained using the following formula (6):

[0114] e m =υ m ×(T BN ·e y ) Formula (6)

[0115] Among them, e m Refers to the error correction of the magnetic compass, υ m Refers to the 3×1 vector (magnetic compass data) measured by the magnetic compass, e y =[0,1,0] T It refers to the second direction vector of the inertial coordinate system. Here, the second direction specifically refers to the y direction.

[0116] Step S202: Determine the heading angle of the UAV in the normal stage of the magnetic compass data according to the error correction of the magnetic compass, the error correction of the acceleration, the angular velocity measurement value of the gyroscope, the dynamic error correction factor and the static error correction factor.

[0117] Specifically, this step can first calculate the second integral correction of the gyroscope angular velocity according to the error correction of the magnetic compass, the error correction of the acceleration, the angular velocity measurement value of the gyroscope, the dynamic error correction factor and the static error correction factor, and then determine the heading angle of the drone in the normal stage of the magnetic compass data according to the second integral correction of the gyroscope angular velocity.

[0118] Here, the second integral correction value of the gyroscope angular velocity can be obtained by using the following formula (7):

[0119] g”=g+k p (e a +e m )+k i ∫(e a +e m ) Formula (7)

[0120] Here, g" refers to the second integral correction of the gyroscope angular velocity.

[0121] After obtaining the second integral correction of the gyroscope angular velocity, this step can use the differential equation of the attitude angle and the angular velocity g" to obtain the heading angle of the UAV in the normal stage of the magnetic compass data, and can also obtain the pitch angle and roll angle of the UAV in the normal stage of the magnetic compass data.

[0122] Here, the method of using the differential equation of the attitude angle and the angular velocity g" to obtain the heading angle of the drone in the normal stage of the magnetic compass data is a prior art. For details, please refer to the description in the prior art and will not be described in detail here.

[0123] In summary, this embodiment divides the flight process of the drone into a normal stage of magnetic compass data and a strong magnetic interference stage. In the normal stage of magnetic compass data, a heading estimation method based on Mahony complementary filtering is adopted, and the data of the cable is collected to construct a standard direction vector, which is used as a reference vector for heading angle estimation in the strong magnetic interference stage. In the strong magnetic interference stage, the error direction vector of the cable is constructed. Based on the cable direction vectors of the two stages, the cable direction error correction replaces the error correction of the magnetic compass, thereby realizing the drone heading estimation that does not rely on the magnetic compass in the strong magnetic interference stage, ensuring the correct heading estimation of the drone during the entire process of approaching the transmission line for inspection, and reducing the risk of the drone exploding.

[0124] An embodiment of the present application also provides a heading estimation device for a drone. The heading estimation device for a drone provided in an embodiment of the present application is described below. The heading estimation device for a drone described below and the heading estimation method for a drone described above can be referenced to each other.

[0125] See also Figure 3 , shows a schematic diagram of the structure of a heading estimation device for a drone provided in an embodiment of the present application, such as Figure 3 As shown, the heading estimation device of the UAV may include: an inspection phase determination module 301, a standard direction vector determination module 302, an error direction vector determination module 303 and a strong magnetic phase heading angle determination module 304.

[0126] The inspection stage determination module 301 is used to determine the inspection stage of the drone according to the magnetic compass data during the inspection of the drone, wherein the inspection stage includes the strong magnetic interference stage and the normal magnetic compass data stage.

[0127] The standard direction vector determination module 302 is used to calculate the standard direction vector of the cable according to the relative position of the cable and the drone when the drone is in the normal stage of magnetic compass data.

[0128] The error direction vector determination module 303 is used to calculate the error direction vector of the cable in the strong magnetic interference stage according to the relative position of the cable and the drone after the drone enters the strong magnetic interference stage from the normal stage of magnetic compass data.

[0129] The strong magnetic stage heading angle determination module 304 is used to determine the heading angle of the UAV in the strong magnetic interference stage according to the standard direction vector and the error direction vector.

[0130] In one possible implementation, the inspection stage determination module 301 can be specifically used for: if the modulus value of the magnetic compass data is less than a preset first threshold, or the modulus value of the magnetic compass data is greater than a preset second threshold, then it is determined that the drone is in a strong magnetic interference stage; otherwise, it is determined that the drone is in a normal magnetic compass data stage, wherein the first threshold is less than the second threshold.

[0131] In a possible implementation, the standard direction vector determination module 302 may include: a data normal phase coordinate acquisition module and a standard direction vector calculation module.

[0132] The data normal phase coordinate acquisition module is used to obtain the coordinates of any two points on the cable relative to the drone when the drone is in the normal phase of magnetic compass data, and use the two acquired coordinates as the first coordinate and the second coordinate respectively.

[0133] The standard direction vector calculation module is used to calculate the standard direction vector of the cable according to the first coordinate and the second coordinate.

[0134] In a possible implementation, the error direction vector determination module 303 may include: a strong magnetic stage coordinate acquisition module and an error direction vector calculation module.

[0135] The strong magnetic stage coordinate acquisition module is used to obtain the coordinates of any two points on the cable relative to the drone after the drone enters the strong magnetic interference stage from the normal magnetic compass data stage, and use the two obtained coordinates as the third coordinate and the fourth coordinate respectively.

[0136] The error direction vector calculation module is used to calculate the error direction vector of the cable in the strong magnetic interference stage according to the third coordinate and the fourth coordinate.

[0137] In a possible implementation, the process of obtaining the coordinates of any two points on the cable relative to the drone in the first-stage coordinate acquisition module and the second-stage coordinate acquisition module includes: obtaining the coordinates of any two points on the cable relative to the drone through a laser radar carried on the drone.

[0138] In a possible implementation, the strong magnetic stage heading angle determination module 304 may include: a direction error correction amount determination module, an acceleration error correction amount determination module and a strong magnetic stage heading angle calculation module.

[0139] The direction error correction amount determination module is used to determine the direction error correction amount of the cable according to the standard direction vector and the error direction vector.

[0140] The acceleration error correction amount determination module is used to correct the gravity acceleration vector according to the coordinate conversion matrix from the inertial coordinate system to the body coordinate system of the drone and the first direction vector of the inertial coordinate system to obtain the acceleration error correction amount.

[0141] The heading angle estimation module in the strong magnetic stage is used to determine the heading angle of the UAV in the strong magnetic interference stage according to the direction error correction, the acceleration error correction, the angular velocity measurement value of the gyroscope, the dynamic error correction factor and the static error correction factor.

[0142] In a possible implementation, the strong magnetic stage heading angle estimation module may include: a first integral correction amount calculation module and a strong magnetic stage heading angle calculation module.

[0143] The first integral correction calculation module is used to calculate the first integral correction of the gyroscope angular velocity according to the direction error correction, the acceleration error correction, the angular velocity measurement value of the gyroscope, the dynamic error correction factor and the static error correction factor.

[0144] The strong magnetic stage heading angle calculation module is used to determine the heading angle of the UAV in the strong magnetic interference stage according to the first integral correction value of the gyroscope angular velocity.

[0145] In a possible implementation, the heading estimation device for a drone provided in an embodiment of the present application may further include: a magnetic compass error correction amount determination module, a second integral correction amount calculation module, and a data normal phase heading angle calculation module.

[0146] The magnetic compass error correction determination module is used to correct the magnetic compass data according to the coordinate conversion matrix from the inertial coordinate system to the drone's body coordinate system and the second direction vector of the inertial coordinate system when the drone is in the normal stage of magnetic compass data, so as to obtain the error correction of the magnetic compass.

[0147] The second integral correction calculation module is used to calculate the second integral correction of the gyroscope angular velocity according to the error correction of the magnetic compass, the error correction of the acceleration, the angular velocity measurement value of the gyroscope, the dynamic error correction factor and the static error correction factor.

[0148] The heading angle calculation module in the normal data stage is used to determine the heading angle of the drone in the normal magnetic compass data stage according to the second integral correction amount of the gyroscope angular velocity.

[0149] The embodiment of the present application also provides a heading estimation device for a drone. Optionally, Figure 4 The hardware structure diagram of the heading estimation device of the UAV is shown in FIG. Figure 4 , the hardware structure of the heading estimation device of the UAV may include: at least one processor 401, at least one communication interface 402, at least one memory 403 and at least one communication bus 404;

[0150] In the embodiment of the present application, the number of the processor 401, the communication interface 402, the memory 403, and the communication bus 404 is at least one, and the processor 401, the communication interface 402, and the memory 403 communicate with each other through the communication bus 404;

[0151] The processor 401 may be a central processing unit (CPU), or an application-specific integrated circuit (ASIC), or one or more integrated circuits configured to implement the embodiments of the present invention;

[0152] The memory 403 may include a high-speed RAM memory, and may also include a non-volatile memory, such as at least one disk memory;

[0153] The memory 403 stores a program, and the processor 401 can call the program stored in the memory 403, and the program is used to:

[0154] During the inspection process of the drone, the inspection stage of the drone is determined according to the magnetic compass data, wherein the inspection stage includes the strong magnetic interference stage and the normal magnetic compass data stage;

[0155] When the UAV is in the normal stage of magnetic compass data, the standard direction vector of the cable is calculated according to the relative position of the cable and the UAV;

[0156] After the UAV enters the strong magnetic interference stage from the normal stage of magnetic compass data, the error direction vector of the cable in the strong magnetic interference stage is calculated according to the relative position of the cable and the UAV;

[0157] According to the standard direction vector and the error direction vector, the heading angle of the UAV during the strong magnetic interference stage is determined.

[0158] Optionally, the detailed functions and extended functions of the program may refer to the above description.

[0159] An embodiment of the present application also provides a readable storage medium having a computer program stored thereon. When the computer program is executed by a processor, the heading estimation method of the drone as described above is implemented.

[0160] Optionally, the detailed functions and extended functions of the program may refer to the above description.

[0161] Finally, it should be noted that, in this article, relational terms such as and second are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include", "comprises" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of further restrictions, the elements defined by the statement "comprises a ..." do not exclude the presence of other identical elements in the process, method, article or device including the elements.

[0162] The various embodiments in this specification are described in a progressive manner, and each embodiment focuses on the differences from other embodiments. The same or similar parts between the various embodiments can be referenced to each other.

[0163] The above description of the disclosed embodiments enables those skilled in the art to implement or use the present application. Various modifications to these embodiments will be apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the present application. Therefore, the present application will not be limited to the embodiments shown herein, but will conform to the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A method for estimating the heading of an unmanned aerial vehicle, characterized in that: include: During the inspection of the drone, the inspection stage of the drone is determined according to the magnetic compass data, wherein the inspection stage includes a strong magnetic interference stage and a normal magnetic compass data stage; When the drone is in the normal stage of the magnetic compass data, the standard direction vector of the cable is calculated according to the relative position of the cable and the drone; wherein, when the drone is in the normal stage of the magnetic compass data, the standard direction vector of the cable is calculated according to the relative position of the cable and the drone, including: when the drone is in the normal stage of the magnetic compass data, the coordinates of any two points on the cable relative to the drone are obtained, and the two obtained coordinates are used as the first coordinate and the second coordinate respectively; the standard direction vector of the cable is calculated according to the first coordinate and the second coordinate; After the UAV enters the strong magnetic interference stage from the normal stage of the magnetic compass data, the error direction vector of the cable in the strong magnetic interference stage is calculated according to the relative position of the cable and the UAV; wherein, after the UAV enters the strong magnetic interference stage from the normal stage of the magnetic compass data, the error direction vector of the cable in the strong magnetic interference stage is calculated according to the relative position of the cable and the UAV, including: after the UAV enters the strong magnetic interference stage from the normal stage of the magnetic compass data, the coordinates of any two points on the cable relative to the UAV are obtained, and the two obtained coordinates are used as the third coordinate and the fourth coordinate respectively; according to the third coordinate and the fourth coordinate, the error direction vector of the cable in the strong magnetic interference stage is calculated; The heading angle of the UAV in the strong magnetic interference stage is determined according to the standard direction vector and the error direction vector.

2. The method for estimating the heading of an unmanned aerial vehicle according to claim 1, characterized in that: Determining the inspection stage of the drone according to the magnetic compass data includes: If the modulus value of the magnetic compass data is less than a preset first threshold, or the modulus value of the magnetic compass data is greater than a preset second threshold, it is determined that the drone is in the strong magnetic interference stage; otherwise, it is determined that the drone is in the normal stage of the magnetic compass data, wherein the first threshold is less than the second threshold.

3. The method for estimating the heading of an unmanned aerial vehicle according to claim 1, characterized in that: The obtaining of coordinates of any two points on the cable relative to the drone comprises: The coordinates of any two points on the cable relative to the drone are obtained by using a laser radar carried by the drone.

4. The method for estimating the heading of an unmanned aerial vehicle according to claim 1, characterized in that: The determining the heading angle of the UAV in the strong magnetic interference stage according to the standard direction vector and the error direction vector comprises: Determining a direction error correction amount of the cable according to the standard direction vector and the error direction vector; Correcting the gravity acceleration vector according to the coordinate conversion matrix from the inertial coordinate system to the body coordinate system of the drone and the first direction vector of the inertial coordinate system to obtain an acceleration error correction amount; The heading angle of the UAV in the strong magnetic interference stage is determined according to the direction error correction, the acceleration error correction, the angular velocity measurement value of the gyroscope, the dynamic error correction factor and the static error correction factor.

5. The method for estimating the heading of an unmanned aerial vehicle according to claim 4, characterized in that: The method of determining the heading angle of the UAV in the strong magnetic interference stage according to the direction error correction amount, the acceleration error correction amount, the angular velocity measurement value of the gyroscope, the dynamic error correction factor and the static error correction factor includes: Calculating a first integral correction amount of the gyroscope angular velocity according to the direction error correction amount, the acceleration error correction amount, the angular velocity measurement value of the gyroscope, the dynamic error correction factor and the static error correction factor; The heading angle of the UAV in the strong magnetic interference stage is determined according to the first integral correction amount of the gyroscope angular velocity.

6. The method for estimating the heading of an unmanned aerial vehicle according to claim 5, characterized in that: Also includes: When the drone is in the normal stage of the magnetic compass data, the magnetic compass data is corrected according to the coordinate conversion matrix from the inertial coordinate system to the drone's body coordinate system and the second direction vector of the inertial coordinate system to obtain an error correction value of the magnetic compass; Calculating a second integral correction amount of the gyroscope angular velocity according to the error correction amount of the magnetic compass, the error correction amount of the acceleration, the angular velocity measurement value of the gyroscope, the dynamic error correction factor and the static error correction factor; The heading angle of the UAV in the normal stage of the magnetic compass data is determined according to the second integral correction amount of the gyroscope angular velocity.

7. A heading estimation device for an unmanned aerial vehicle, characterized in that: include: An inspection stage determination module is used to determine the inspection stage of the drone according to the magnetic compass data during the inspection of the drone, wherein the inspection stage includes a strong magnetic interference stage and a normal magnetic compass data stage; A standard direction vector determination module, used to calculate the standard direction vector of the cable according to the relative position of the cable and the drone when the drone is in the normal stage of the magnetic compass data; Wherein, the standard direction vector determination module includes a data normal phase coordinate acquisition module and a standard direction vector calculation module; The data normal stage coordinate acquisition module is used to acquire the coordinates of any two points on the cable relative to the drone when the drone is in the magnetic compass data normal stage, and use the acquired two coordinates as the first coordinate and the second coordinate respectively; The standard direction vector calculation module is used to calculate the standard direction vector of the cable according to the first coordinate and the second coordinate; An error direction vector determination module is used to calculate the error direction vector of the cable in the strong magnetic interference stage according to the relative position of the cable and the drone after the drone enters the strong magnetic interference stage from the normal stage of the magnetic compass data; Wherein, the error direction vector determination module includes: a strong magnetic stage coordinate acquisition module and an error direction vector calculation module; The strong magnetic stage coordinate acquisition module is used to obtain the coordinates of any two points on the cable relative to the drone after the drone enters the strong magnetic interference stage from the normal stage of magnetic compass data, and use the two obtained coordinates as the third coordinate and the fourth coordinate respectively; The error direction vector calculation module is used to calculate the error direction vector of the cable in the strong magnetic interference stage according to the third coordinate and the fourth coordinate; The strong magnetic stage heading angle determination module is used to determine the heading angle of the UAV in the strong magnetic interference stage according to the standard direction vector and the error direction vector.

8. A heading estimation device for an unmanned aerial vehicle, characterized in that: including memory and processor; The memory is used to store programs; The processor is used to execute the program to implement each step of the heading estimation method for a UAV as described in any one of claims 1 to 6.

9. A readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, each step of the method for estimating the heading of a UAV as claimed in any one of claims 1 to 6 is implemented.

Images