CN110988393A - Unmanned aerial vehicle wind speed and direction measurement and correction algorithm based on ultrasonic anemoscope - Google Patents

Unmanned aerial vehicle wind speed and direction measurement and correction algorithm based on ultrasonic anemoscope Download PDF

Info

Publication number
CN110988393A
CN110988393A CN201911278177.7A CN201911278177A CN110988393A CN 110988393 A CN110988393 A CN 110988393A CN 201911278177 A CN201911278177 A CN 201911278177A CN 110988393 A CN110988393 A CN 110988393A
Authority
CN
China
Prior art keywords
unmanned aerial
aerial vehicle
wind
wind speed
angle
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN201911278177.7A
Other languages
Chinese (zh)
Inventor
陈冲
石晓雨
张彪
丘仲锋
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Nanjing Kaitianyan Uav Technology Co ltd
Original Assignee
Nanjing Kaitianyan Uav Technology Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Nanjing Kaitianyan Uav Technology Co ltd filed Critical Nanjing Kaitianyan Uav Technology Co ltd
Priority to CN201911278177.7A priority Critical patent/CN110988393A/en
Publication of CN110988393A publication Critical patent/CN110988393A/en
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01P—MEASURING LINEAR OR ANGULAR SPEED, ACCELERATION, DECELERATION, OR SHOCK; INDICATING PRESENCE, ABSENCE, OR DIRECTION, OF MOVEMENT
    • G01P5/00—Measuring speed of fluids, e.g. of air stream; Measuring speed of bodies relative to fluids, e.g. of ship, of aircraft
    • G01P5/24—Measuring speed of fluids, e.g. of air stream; Measuring speed of bodies relative to fluids, e.g. of ship, of aircraft by measuring the direct influence of the streaming fluid on the properties of a detecting acoustical wave
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01P—MEASURING LINEAR OR ANGULAR SPEED, ACCELERATION, DECELERATION, OR SHOCK; INDICATING PRESENCE, ABSENCE, OR DIRECTION, OF MOVEMENT
    • G01P13/00—Indicating or recording presence, absence, or direction, of movement
    • G01P13/02—Indicating direction only, e.g. by weather vane
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01P—MEASURING LINEAR OR ANGULAR SPEED, ACCELERATION, DECELERATION, OR SHOCK; INDICATING PRESENCE, ABSENCE, OR DIRECTION, OF MOVEMENT
    • G01P21/00—Testing or calibrating of apparatus or devices covered by the preceding groups
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01P—MEASURING LINEAR OR ANGULAR SPEED, ACCELERATION, DECELERATION, OR SHOCK; INDICATING PRESENCE, ABSENCE, OR DIRECTION, OF MOVEMENT
    • G01P21/00—Testing or calibrating of apparatus or devices covered by the preceding groups
    • G01P21/02—Testing or calibrating of apparatus or devices covered by the preceding groups of speedometers
    • G01P21/025—Testing or calibrating of apparatus or devices covered by the preceding groups of speedometers for measuring speed of fluids; for measuring speed of bodies relative to fluids

Abstract

The invention discloses an unmanned aerial vehicle wind speed and direction measurement and correction algorithm based on an ultrasonic anemoscope, which comprises wind speed and direction measurement and correction algorithms in two states, one is the wind speed and direction measurement and correction algorithm in the hovering state of the unmanned aerial vehicle, the other is the wind speed and direction measurement and correction algorithm in the advancing state of the unmanned aerial vehicle, the three Euler angles of the unmanned aerial vehicle are obtained by utilizing the relation between quaternion and an attitude matrix, the inclination angle of the unmanned aerial vehicle is further obtained, and the wind speed and direction result measured by the ultrasonic anemoscope is corrected according to the size of the inclination angle. Compared with the prior art, the invention has the beneficial effects that: the measurement and correction algorithm overcomes the measurement result error caused by the inclination of the body plane when the unmanned aerial vehicle moves forward or blows, and has the advantages of high measurement precision and long service life.

Description

Unmanned aerial vehicle wind speed and direction measurement and correction algorithm based on ultrasonic anemoscope
Technical Field
The invention relates to a wind measurement and correction algorithm, in particular to an unmanned aerial vehicle wind speed and direction measurement and correction algorithm based on an ultrasonic anemoscope.
Background
At present, unmanned aerial vehicles are widely applied to the fields of field construction, exploration, transportation, travel, rescue, particularly work on rivers and lakes and the like. With the application of unmanned aerial vehicles, it is becoming a research hotspot to adopt multi-rotor unmanned aerial vehicles for meteorological monitoring.
However, in the prior art, the wind measurement of the multi-rotor unmanned aerial vehicle is mainly realized by directly additionally arranging a tower-shaped ultrasonic anemoscope on the upper part of a machine body. The prior art not only can lead to the reduction of unmanned aerial vehicle flight stability because of changing focus, increase windage, but also can receive the influence that rotor self produced the wind field and greatly reduce measurement accuracy because of ultrasonic wave anemorumbometer at the during operation. So prior art can't realize on many rotor unmanned aerial vehicle to the accurate monitoring of wind speed, wind direction.
Considering that the wind speed and direction correction algorithm can be installed on an unmanned aerial vehicle, an ultrasonic anemoscope with a simple principle is selected, and the correction algorithm for measuring the wind speed and the direction of the unmanned aerial vehicle based on the ultrasonic anemoscope is invented. The wind speed and the wind direction are measured by using the propagation characteristics of the ultrasonic waves, and the method has the advantages of high measurement precision and long service life. It is suitable for various environments and has high reliability. Therefore, the wind measuring method is combined with the unmanned aerial vehicle, and the advantages of the wind measuring method can be fully exerted. However, because the sensor is placed on the unmanned aerial vehicle, the body plane can be inclined when the unmanned aerial vehicle moves forward or blows, the ultrasonic anemorumbometer also inclines accordingly, and because the ultrasonic anemorumbometer only measures the wind parallel to the plane of the ultrasonic anemorumbometer, the error of the measurement result is caused, and therefore the result is required to be corrected.
Disclosure of Invention
Aiming at the defects of the prior art, the invention provides an unmanned aerial vehicle wind speed and direction measurement and correction algorithm based on an ultrasonic anemoscope.
In order to solve the technical problems, the invention adopts the technical scheme that:
an unmanned aerial vehicle wind speed and direction measurement and correction algorithm based on an ultrasonic anemoscope comprises wind speed and direction measurement and correction algorithms in two states, one is the wind speed and direction measurement and correction algorithm in the hovering state of the unmanned aerial vehicle, the other is the wind speed and direction measurement and correction algorithm in the advancing state of the unmanned aerial vehicle, the two wind speed and direction measurement and correction algorithms are used for obtaining three Euler angles of the unmanned aerial vehicle by utilizing the relation between quaternions and an attitude matrix so as to obtain the inclination angle of the unmanned aerial vehicle, and the wind speed and direction result measured by the ultrasonic anemoscope is corrected according to the size of the inclination angle;
a reference coordinate system R, referred to as R system for short, a rigid body rotating at a fixed point relative to the R system, the fixed point being O; selecting a coordinate system b and a rigid body to be fixedly connected, wherein the coordinate system b is called b system for short, supposing that the initial time b system is superposed with R system, and setting OA ═ R as an initial position vector and OA ═ R' as a rotated vector; according to the Euler's theorem, the rotation of the rigid body from the A position to the A' position is equivalent to a unit instantaneous axis considering only the positions at the initial time and the final time
Figure BDA0002315898930000021
Through theta1Completing the angle at one time; decomposing, rotating and synthesizing the vector to obtain:
r'＝rcosθ1+(1-cosθ1)(u·r)u+u×rsinθ1； (1)
transformed by the triple vector product formula:
r'＝r+u×rsinθ1+(1-cosθ1)u×(u×r)； (2)
note the book
Figure BDA0002315898930000022
So that there are
Figure BDA0002315898930000023
Order:
Figure BDA0002315898930000024
then:
u×r＝Ur；
u×(u×r)＝U·Ur；
therefore, the method comprises the following steps:
Figure BDA0002315898930000025
order:
Figure BDA0002315898930000026
equation (3) can be written as:
r'＝Dr； (5)
rigid body fixed connection coordinate system b for recording initial time0Since the rigid body fixed coordinate system b at the initial time coincides with the reference coordinate system R, there are:
Figure BDA0002315898930000031
in the rotating process, the position vector and the b system are fixedly connected with the rigid body, so that the relative angular position of the position vector and the b system is always unchanged, namely:
Figure BDA0002315898930000032
so as to obtain:
r＝r'b； (8)
bringing formula (8) into formula (5):
r'＝Dr'b； (9)
the formula indicates that D is a coordinate transformation matrix from a b system to an R system;
Figure BDA0002315898930000033
namely:
Figure BDA0002315898930000034
order:
Figure BDA0002315898930000041
with q0,q1,q2,q3Constructing a quaternion:
Figure BDA0002315898930000042
wherein i, j, k are not only unit vectors which are mutually orthogonal, but also imaginary unit
Figure BDA0002315898930000043
Q is to be0,q1,q2,q3Further simplified in formula (10):
Figure BDA0002315898930000044
if the reference coordinate system is a navigation coordinate system n and the coordinate system fixedly connected with the rigid body is a body coordinate system, the coordinate transformation matrix
Figure BDA0002315898930000045
Is the attitude matrix
Figure BDA0002315898930000046
Under an engine body axis coordinate system, the unmanned aerial vehicle firstly rotates around an x axis for roll angle α, then rotates around a y axis for pitch angle β, and finally rotates around a z axis for course angle gamma, and then a transformation matrix under the coordinate system is as follows:
Figure BDA0002315898930000047
comparing equations (13) and (14) results in three attitude angles:
Figure BDA0002315898930000048
β＝-arcsinT31＝-arcsin2(q1q3-q0q2)；
Figure BDA0002315898930000051
wherein, T11Is cos β cos gamma, T21Is cos β sin gamma, T31Is-sin β, T32Sin α cos β, T33Cos α cos β;
let the normal vector of the original plane of the unmanned aerial vehicle be
Figure BDA0002315898930000052
The normal vector of the resulting plane after rotation is:
Figure BDA0002315898930000053
so the tilt angle θ of the drone is:
Figure BDA0002315898930000054
θ＝arccos(cosαcosβ)； (17)
the wind speed v obtained by the ultrasonic wind measurement principle is as follows:
Figure BDA0002315898930000055
wherein, t1、t2Propagation time in the x-axis direction, t3、t4Is the propagation time in the y-axis direction,
wind direction
Figure BDA0002315898930000056
Comprises the following steps:
Figure BDA0002315898930000057
where k is an integer, and the formula is followed by a notation of what value k takes under various circumstances.
When the unmanned aerial vehicle is in a hovering state, the speed of the unmanned aerial vehicle measured by a GPS (global positioning system) arranged in the unmanned aerial vehicle is 0, and the wind direction measured by the ultrasonic anemoscope is consistent with the actual wind direction; decompose into with wind the component parallel with the unmanned aerial vehicle plane and vertically component, the vertical component does not have the influence to ultrasonic wave anemoscope, so the wind that ultrasonic wave anemoscope surveyed is the parallel component, and the wind direction angle that surveys is unanimous with former wind direction angle, so the result of revising is:
Figure BDA0002315898930000058
Figure BDA0002315898930000061
wherein v isPractice ofIn order to be the actual wind speed,
Figure BDA0002315898930000062
is the actual wind direction and gamma is the heading angle.
When the unmanned aerial vehicle is in a forward state, the GPS (global positioning system) arranged in the unmanned aerial vehicle measures the forward speed of the unmanned aerial vehicle as v1The unmanned aerial vehicle advances at a certain speed, which is equivalent to that when the unmanned aerial vehicle suspends, the wind with the same size blows along the direction opposite to the advancing direction of the unmanned aerial vehicle; the speed measured by the ultrasonic anemorumbometer is v, and v is the measured speedCombination of Chinese herbsIs equivalent wind speed v under the advancing state of the unmanned aerial vehicle1With the actual wind speed v to be measured2The synthetic wind of (2) is:
Figure BDA0002315898930000063
Figure BDA0002315898930000064
wherein v is the speed measured by the ultrasonic anemorumbometer,
Figure BDA0002315898930000065
is the equivalent wind speed of the unmanned plane in the advancing state,
Figure BDA0002315898930000066
the actual wind speed to be measured;
under the organism coordinate system, no matter how the plane of the unmanned aerial vehicle rotates, the original point can be passed, so the equation of the plane of the unmanned aerial vehicle is as follows:
Ax+By+Cz＝0
and because its normal vector is:
Figure BDA0002315898930000067
obtaining:
Figure BDA0002315898930000068
let z be 0 the equation of intersection available is:
Ax+By＝0； (25)
when B is greater than 0, the resultant wind vCombination of Chinese herbsIs decomposed into
Figure BDA0002315898930000069
And
Figure BDA00023158989300000610
Figure BDA00023158989300000611
is the equivalent wind speed of the unmanned plane in the advancing state,
Figure BDA00023158989300000612
for the actual wind speed to be measured,
Figure BDA00023158989300000613
angle of wind direction, theta, of the resultant wind2Is the wind direction angle of the actual wind; the coordinate of the resultant wind is
Figure BDA00023158989300000614
The coordinate of the equivalent wind speed is
Figure BDA00023158989300000615
From formula (23):
Figure BDA0002315898930000071
the actual wind speed is as follows:
Figure BDA0002315898930000072
the actual wind direction angle θ obtained in this case2Comprises the following steps:
Figure BDA0002315898930000073
when B is less than 0, the plane is in a wind direction decomposition schematic diagram, the required wind direction angle is negative, and the following can be obtained in the same way:
Figure BDA0002315898930000074
Figure BDA0002315898930000075
in particular, when a is 0 and B > 0,
Figure BDA0002315898930000076
at this time
Figure BDA0002315898930000077
When A is 0 and B is less than 0,
Figure BDA0002315898930000078
at this time
Figure BDA0002315898930000079
Therefore, in summary, the following can be obtained:
Figure BDA00023158989300000710
will theta2To angles within [0,2 π), i.e.:
Figure BDA00023158989300000711
wherein, theta'2Converting the solved wind direction angle into an angle in [0,2 pi ];
the wind direction at a certain time is as follows along with the change of time:
Figure BDA0002315898930000081
where γ is the heading angle.
Compared with the prior art, the invention has the beneficial effects that: the measurement and correction algorithm overcomes the measurement result error caused by the inclination of the body plane when the unmanned aerial vehicle moves forward or blows, and has the advantages of high measurement precision and long service life.
Drawings
FIG. 1 is a schematic diagram of rigid body rotation;
FIG. 2 is an exploded view of the wind direction;
FIG. 3 is a schematic view of a plane wind direction combination decomposition when B > 0;
FIG. 4 is a schematic exploded view of a plane wind-converging direction when B < 0.
Wherein, 1-plane of the unmanned aerial vehicle, 2-horizontal plane, 3-wind direction, 4-component of the wind direction along the plane of the unmanned aerial vehicle, 5-component of the wind direction vertical to the plane of the unmanned aerial vehicle, and 6-intersection line of the plane of the unmanned aerial vehicle and the horizontal plane.
Detailed Description
The present invention will now be described in further detail with reference to the accompanying drawings. These drawings are simplified schematic views illustrating only the basic structure of the present invention in a schematic manner, and thus show only the constitution related to the present invention.
An unmanned aerial vehicle wind speed and direction measurement and correction algorithm based on an ultrasonic anemoscope comprises wind speed and direction measurement and correction algorithms in two states, one is the wind speed and direction measurement and correction algorithm in the hovering state of the unmanned aerial vehicle, the other is the wind speed and direction measurement and correction algorithm in the advancing state of the unmanned aerial vehicle, the two wind speed and direction measurement and correction algorithms are used for obtaining three Euler angles of the unmanned aerial vehicle by utilizing the relation between quaternions and an attitude matrix so as to obtain the inclination angle of the unmanned aerial vehicle, and the wind speed and direction result measured by the ultrasonic anemoscope is corrected according to the size of the inclination angle;
FIG. 1 is a schematic diagram of the rotation of a rigid body having a reference coordinate system R, referred to as the R system, with a rigid body rotating at a fixed point relative to the R system, the fixed point being O; selecting a coordinate system b and a rigid body to be fixedly connected, wherein the coordinate system b is called b system for short, supposing that the initial time b system is superposed with R system, and setting OA ═ R as an initial position vector and OA ═ R' as a rotated vector; according to the Euler's theorem, the rotation of the rigid body from the A position to the A' position is equivalent to a unit instantaneous axis considering only the positions at the initial time and the final time
Figure BDA0002315898930000091
Through theta1Completing the angle at one time; decomposing, rotating and synthesizing the vector to obtain:
r'＝rcosθ1+(1-cosθ1)(u·r)u+u×rsinθ1； (1)
transformed by the triple vector product formula:
r'＝r+u×rsinθ1+(1-cosθ1)u×(u×r)； (2)
note the book
Figure BDA0002315898930000092
So that there are
Figure BDA0002315898930000093
Order:
Figure BDA0002315898930000094
then:
u×r＝Ur；
u×(u×r)＝U·Ur；
therefore, the method comprises the following steps:
Figure BDA0002315898930000095
order:
Figure BDA0002315898930000096
equation (3) can be written as:
r'＝Dr； (5)
rigid body fixed connection coordinate system b for recording initial time0Since the rigid body fixed coordinate system b at the initial time coincides with the reference coordinate system R, there are:
Figure BDA0002315898930000097
in the rotating process, the position vector and the b system are fixedly connected with the rigid body, so that the relative angular position of the position vector and the b system is always unchanged, namely:
Figure BDA0002315898930000101
so as to obtain:
r＝r'b； (8)
bringing formula (8) into formula (5):
r'＝Dr'b； (9)
the formula indicates that D is a coordinate transformation matrix from a b system to an R system;
Figure BDA0002315898930000102
namely:
Figure BDA0002315898930000103
order:
Figure BDA0002315898930000104
with q0,q1,q2,q3Construction quaternion：
Figure BDA0002315898930000111
Wherein i, j, k are not only unit vectors which are mutually orthogonal, but also imaginary unit
Figure BDA0002315898930000112
Q is to be0,q1,q2,q3Further simplified in formula (10):
Figure BDA0002315898930000113
if the reference coordinate system is a navigation coordinate system n and the coordinate system fixedly connected with the rigid body is a body coordinate system, the coordinate transformation matrix
Figure BDA0002315898930000114
Is the attitude matrix
Figure BDA0002315898930000115
Under an engine body axis coordinate system, the unmanned aerial vehicle firstly rotates around an x axis for roll angle α, then rotates around a y axis for pitch angle β, and finally rotates around a z axis for course angle gamma, and then a transformation matrix under the coordinate system is as follows:
Figure BDA0002315898930000116
comparing equations (13) and (14) results in three attitude angles:
Figure BDA0002315898930000117
β＝-arcsinT31＝-arcsin2(q1q3-q0q2)；
Figure BDA0002315898930000118
wherein, T11Is cos β cos gamma, T21Is cos β sin gamma, T31Is-sin β, T32Sin α cos β, T33Cos α cos β;
let the normal vector of the original plane of the unmanned aerial vehicle be
Figure BDA0002315898930000119
The normal vector of the resulting plane after rotation is:
Figure BDA0002315898930000121
so the tilt angle θ of the drone is:
Figure BDA0002315898930000122
θ＝arccos(cosαcosβ)； (17)
the wind speed v obtained by the ultrasonic wind measurement principle is as follows:
Figure BDA0002315898930000123
wherein, t1、t2Propagation time in the x-axis direction, t3、t4Is the propagation time in the y-axis direction,
wind direction
Figure BDA0002315898930000124
Comprises the following steps:
Figure BDA0002315898930000125
where k is an integer, and the formula is followed by a notation of what value k takes under various circumstances.
Fig. 2 is the wind direction and decomposes the sketch map, and 1 is the unmanned aerial vehicle plane, and 2 are the horizontal plane, and 3 are the wind direction, and 4 are wind direction along the planar component of unmanned aerial vehicle, and 5 are the planar component of wind direction perpendicular unmanned aerial vehicle, and 6 are the intersecting line of unmanned aerial vehicle plane and horizontal plane.
When the unmanned aerial vehicle is in the state of hovering, the unmanned aerial vehicle speed that GPS surveyed is 0, and wind direction 3 is actual wind direction this moment. Decompose into with wind the component parallel with the unmanned aerial vehicle plane and vertically component, the vertical component does not have the influence to ultrasonic wave anemoscope, so the wind that ultrasonic wave anemoscope surveyed is the parallel component, and the wind direction angle that surveys is unanimous with former wind direction angle, so the result of revising is:
Figure BDA0002315898930000126
Figure BDA0002315898930000127
wherein v isPractice ofIn order to be the actual wind speed,
Figure BDA0002315898930000128
is the actual wind direction and gamma is the heading angle.
When the unmanned aerial vehicle is in a forward state, the GPS measures the forward speed of the unmanned aerial vehicle as v1And the unmanned aerial vehicle advances at a certain speed, and when the unmanned aerial vehicle suspends equivalently, the wind with the same size blows along the opposite direction of the advancing direction of the unmanned aerial vehicle. The speed measured by the ultrasonic anemorumbometer is v, and v is the measured speedCombination of Chinese herbsIs equivalent wind speed v under the advancing state of the unmanned aerial vehicle1With the actual wind speed v to be measured2The synthetic wind of (2) is:
Figure BDA0002315898930000131
Figure BDA0002315898930000132
wherein v is the speed measured by the ultrasonic anemorumbometer,
Figure BDA0002315898930000133
is the equivalent wind speed of the unmanned plane in the advancing state,
Figure BDA0002315898930000134
is the actual wind speed to be measured.
Under the organism coordinate system, no matter how the plane of the unmanned aerial vehicle rotates, the original point can be passed, so the equation of the plane of the unmanned aerial vehicle is as follows:
Ax+By+Cz＝0
and because its normal vector is:
Figure BDA0002315898930000135
obtaining:
Figure BDA0002315898930000136
let z be 0 the equation of intersection available is:
Ax+By＝0； (25)
FIG. 3 is a schematic view of the plane synthesized wind direction decomposition when B > 0, the synthesized wind vCombination of Chinese herbsIs decomposed into
Figure BDA0002315898930000137
And
Figure BDA0002315898930000138
Figure BDA0002315898930000139
is the equivalent wind speed of the unmanned plane in the advancing state,
Figure BDA00023158989300001310
for the actual wind speed to be measured,
Figure BDA00023158989300001311
angle of wind direction, theta, of the resultant wind2Is the wind direction angle of the actual wind. The coordinate of the resultant wind is
Figure BDA00023158989300001312
The coordinate of the equivalent wind speed is
Figure BDA00023158989300001313
Is composed of(23) Can obtain the product
Figure BDA00023158989300001314
The actual wind speed is as follows:
Figure BDA0002315898930000141
the wind direction angle obtained in this case is:
Figure BDA0002315898930000142
FIG. 4 is a schematic diagram of a planar anemometry decomposition when B < 0, where the required wind direction angle is negative, and the same can be obtained:
Figure BDA0002315898930000143
Figure BDA0002315898930000144
in particular, when a is 0 and B > 0,
Figure BDA0002315898930000145
at this time
Figure BDA0002315898930000146
When A is 0 and B is less than 0,
Figure BDA0002315898930000147
at this time
Figure BDA0002315898930000148
Therefore, in summary, the following can be obtained:
Figure BDA0002315898930000149
will theta2Into angles within [0,2 π) ], i.e.
Figure BDA00023158989300001410
Wherein, theta'2Converting the solved wind direction angle into an angle in [0,2 pi ];
the wind direction at a certain time is as follows along with the change of time:
Figure BDA0002315898930000151
where γ is the heading angle.
In light of the foregoing description of the preferred embodiment of the present invention, many modifications and variations will be apparent to those skilled in the art without departing from the spirit and scope of the invention. The technical scope of the present invention is not limited to the content of the specification, and must be determined according to the scope of the claims.

Claims (3)

1. The utility model provides an unmanned aerial vehicle wind speed and direction measures and correction algorithm based on ultrasonic wave anemoscope, includes the wind speed and direction measurement and correction algorithm of two kinds of states, one kind is that unmanned aerial vehicle hovers the state wind speed and direction measurement and correction algorithm, and another kind is that unmanned aerial vehicle advances the state wind speed and direction measurement and correction algorithm, its characterized in that: the two wind speed and direction measurement and correction algorithms use the relation between quaternion and attitude matrix to obtain three Euler angles of the unmanned aerial vehicle, further obtain the inclination angle of the unmanned aerial vehicle, and correct the wind speed and direction result measured by the ultrasonic anemoscope according to the size of the inclination angle;
a reference coordinate system R, referred to as R system for short, a rigid body rotating at a fixed point relative to the R system, the fixed point being O; selecting a coordinate system b and a rigid body to be fixedly connected, wherein the coordinate system b is called b system for short, supposing that the initial time b system is superposed with R system, and setting OA ═ R as an initial position vector and OA ═ R' as a rotated vector; according to the Euler's theorem, the rotation of the rigid body from the A position to the A' position is equivalent to a unit instantaneous axis considering only the positions at the initial time and the final time
Figure FDA0002315898920000014
Through theta1Completing the angle at one time; decomposing, rotating and synthesizing the vector to obtain:
r'＝rcosθ1+(1-cosθ1)(u·r)u+u×rsinθ1；(1)
transformed by the triple vector product formula:
r'＝r+u×rsinθ1+(1-cosθ1)u×(u×r)；(2)
note the book
Figure FDA0002315898920000011
So that there are
Figure FDA0002315898920000012
Order:
Figure FDA0002315898920000013
then:
u×r＝Ur；
u×(u×r)＝U·Ur；
therefore, the method comprises the following steps:
Figure FDA0002315898920000021
order:
Figure FDA0002315898920000022
equation (3) can be written as:
r'＝Dr； (5)
rigid body fixed connection coordinate system b for recording initial time0Since the rigid body fixed coordinate system b at the initial time coincides with the reference coordinate system R, there are:
Figure FDA0002315898920000023
in the rotating process, the position vector and the b system are fixedly connected with the rigid body, so that the relative angular position of the position vector and the b system is always unchanged, namely:
Figure FDA0002315898920000024
so as to obtain:
r＝r'b； (8)
bringing formula (8) into formula (5):
r'＝Dr'b； (9)
the formula indicates that D is a coordinate transformation matrix from a b system to an R system;
Figure FDA0002315898920000025
namely:
Figure FDA0002315898920000026
order:
Figure FDA0002315898920000031
with q0,q1,q2,q3Constructing a quaternion:
Figure FDA0002315898920000032
wherein i, j, k are not only unit vectors which are mutually orthogonal, but also imaginary unit
Figure FDA0002315898920000033
Q is to be0,q1,q2,q3Further simplified in formula (10):
Figure FDA0002315898920000034
if the reference coordinate system is a navigation coordinate system n and the coordinate system fixedly connected with the rigid body is a body coordinate system, the coordinate transformation matrix
Figure FDA0002315898920000035
Is the attitude matrix
Figure FDA0002315898920000036
Under an engine body axis coordinate system, the unmanned aerial vehicle firstly rotates around an x axis for roll angle α, then rotates around a y axis for pitch angle β, and finally rotates around a z axis for course angle gamma, and then a transformation matrix under the coordinate system is as follows:
Figure FDA0002315898920000037
comparing equations (13) and (14) results in three attitude angles:
Figure FDA0002315898920000038
β＝-arcsinT31＝-arcsin2(q1q3-q0q2)；
Figure FDA0002315898920000041
wherein, T11Is cos β cos gamma, T21Is cos β sin gamma, T31Is-sin β, T32Sin α cos β, T33Cos α cos β;
let the normal vector of the original plane of the unmanned aerial vehicle be
Figure FDA0002315898920000042
The normal vector of the resulting plane after rotation is:
Figure FDA0002315898920000043
so the tilt angle θ of the drone is:
Figure FDA0002315898920000044
θ＝arccos(cosαcosβ)；(17)
the wind speed v obtained by the ultrasonic wind measuring principle is as follows:
Figure FDA0002315898920000045
wherein, t1、t2Propagation time in the x-axis direction, t3、t4Is the propagation time in the y-axis direction,
wind direction
Figure FDA0002315898920000046
Comprises the following steps:
Figure FDA0002315898920000047
where k is an integer, and the formula is followed by a notation of what value k takes under various circumstances.
2. The ultrasonic anemoscope-based unmanned aerial vehicle anemometry and correction algorithm of claim 1, wherein: when the unmanned aerial vehicle is in a hovering state, the speed of the unmanned aerial vehicle measured by a GPS (global positioning system) arranged in the unmanned aerial vehicle is 0, and the wind direction measured by the ultrasonic anemoscope is consistent with the actual wind direction; decompose into with wind the component parallel with the unmanned aerial vehicle plane and vertically component, the vertical component does not have the influence to ultrasonic wave anemoscope, so the wind that ultrasonic wave anemoscope surveyed is the parallel component, and the wind direction angle that surveys is unanimous with former wind direction angle, so the result of revising is:
Figure FDA0002315898920000051
Figure FDA0002315898920000052
wherein the content of the first and second substances,
Figure FDA0002315898920000053
in order to be the actual wind speed,
Figure FDA0002315898920000054
is the actual wind direction and gamma is the heading angle.
3. The ultrasonic anemoscope-based unmanned aerial vehicle anemometry and correction algorithm of claim 1, wherein: when the unmanned aerial vehicle is in a forward state, the GPS (global positioning system) arranged in the unmanned aerial vehicle measures the forward speed of the unmanned aerial vehicle as v1The unmanned aerial vehicle advances at a certain speed, which is equivalent to that when the unmanned aerial vehicle suspends, the wind with the same size blows along the direction opposite to the advancing direction of the unmanned aerial vehicle; the speed measured by the ultrasonic anemorumbometer is v, and v is the measured speedCombination of Chinese herbsIs equivalent wind speed v under the advancing state of the unmanned aerial vehicle1With the actual wind speed v to be measured2The synthetic wind of (2) is:
Figure FDA0002315898920000055
Figure FDA0002315898920000056
wherein v is the speed measured by the ultrasonic anemorumbometer,
Figure FDA0002315898920000057
is the equivalent wind speed of the unmanned plane in the advancing state,
Figure FDA0002315898920000058
the actual wind speed to be measured;
under the organism coordinate system, no matter how the plane of the unmanned aerial vehicle rotates, the original point can be passed, so the equation of the plane of the unmanned aerial vehicle is as follows:
Ax+By+Cz＝0
and because its normal vector is:
Figure FDA0002315898920000059
obtaining:
Figure FDA00023158989200000510
let z be 0 the equation of intersection available is:
Ax+By＝0； (25)
when B is greater than 0, the resultant wind vCombination of Chinese herbsIs decomposed into
Figure FDA0002315898920000061
And
Figure FDA0002315898920000062
Figure FDA0002315898920000063
is the equivalent wind speed of the unmanned plane in the advancing state,
Figure FDA0002315898920000064
for the actual wind speed to be measured,
Figure FDA0002315898920000065
angle of wind direction, theta, of the resultant wind2Is the wind direction angle of the actual wind; the coordinate of the resultant wind is
Figure FDA0002315898920000066
The coordinate of the equivalent wind speed is
Figure FDA0002315898920000067
By the formula (23)Obtaining:
Figure FDA0002315898920000068
the actual wind speed is as follows:
Figure FDA0002315898920000069
the actual wind direction angle θ obtained in this case2Comprises the following steps:
Figure FDA00023158989200000610
when B is less than 0, the plane is in a wind direction decomposition schematic diagram, the required wind direction angle is negative, and the following can be obtained in the same way:
Figure FDA00023158989200000611
Figure FDA00023158989200000612
in particular, when a is 0 and B > 0,
Figure FDA00023158989200000613
at this time
Figure FDA00023158989200000614
θ2＝0；
When A is 0 and B is less than 0,
Figure FDA00023158989200000615
at this time
Figure FDA00023158989200000616
θ2＝π；
Therefore, in summary, the following can be obtained:
Figure FDA0002315898920000071
will theta2To angles within [0,2 π), i.e.:
Figure FDA0002315898920000072
wherein, theta'2Converting the solved wind direction angle into an angle in [0,2 pi ];
the wind direction at a certain time is as follows along with the change of time:
Figure FDA0002315898920000073
where γ is the heading angle.
CN201911278177.7A 2019-12-12 2019-12-12 Unmanned aerial vehicle wind speed and direction measurement and correction algorithm based on ultrasonic anemoscope Pending CN110988393A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911278177.7A CN110988393A (en) 2019-12-12 2019-12-12 Unmanned aerial vehicle wind speed and direction measurement and correction algorithm based on ultrasonic anemoscope

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911278177.7A CN110988393A (en) 2019-12-12 2019-12-12 Unmanned aerial vehicle wind speed and direction measurement and correction algorithm based on ultrasonic anemoscope

Publications (1)

Publication Number Publication Date
CN110988393A true CN110988393A (en) 2020-04-10

Family

ID=70092984

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911278177.7A Pending CN110988393A (en) 2019-12-12 2019-12-12 Unmanned aerial vehicle wind speed and direction measurement and correction algorithm based on ultrasonic anemoscope

Country Status (1)

Country Link
CN (1) CN110988393A (en)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113092813A (en) * 2021-03-15 2021-07-09 南京航空航天大学 System and method for detecting self-adaptive wind speed and wind direction under parachute landing situation
CN113252294A (en) * 2021-06-16 2021-08-13 西南交通大学 Cross-sea bridge space wind speed and direction testing system and monitoring method

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20160109475A1 (en) * 2014-10-16 2016-04-21 The Boeing Company Methods and systems for airborne vehicle wind depiction
CN105954819A (en) * 2016-05-24 2016-09-21 南京信息工程大学 Wind speed measuring device based on UAV (Unmanned Aerial Vehicle) inclination angle detection and operation method of wind speed measuring device
CN108152529A (en) * 2017-11-02 2018-06-12 成都飞机工业（集团）有限责任公司 A kind of method based on flight parameter calculation of wind speed and wind direction
CN109374924A (en) * 2018-10-25 2019-02-22 西北工业大学 A kind of lateral wind field estimation method of cross based on quadrotor drone inclination angle
CN110134134A (en) * 2019-05-24 2019-08-16 南京信息工程大学 A kind of wind detection method under unmanned plane floating state

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20160109475A1 (en) * 2014-10-16 2016-04-21 The Boeing Company Methods and systems for airborne vehicle wind depiction
CN105954819A (en) * 2016-05-24 2016-09-21 南京信息工程大学 Wind speed measuring device based on UAV (Unmanned Aerial Vehicle) inclination angle detection and operation method of wind speed measuring device
CN108152529A (en) * 2017-11-02 2018-06-12 成都飞机工业（集团）有限责任公司 A kind of method based on flight parameter calculation of wind speed and wind direction
CN109374924A (en) * 2018-10-25 2019-02-22 西北工业大学 A kind of lateral wind field estimation method of cross based on quadrotor drone inclination angle
CN110134134A (en) * 2019-05-24 2019-08-16 南京信息工程大学 A kind of wind detection method under unmanned plane floating state

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113092813A (en) * 2021-03-15 2021-07-09 南京航空航天大学 System and method for detecting self-adaptive wind speed and wind direction under parachute landing situation
CN113092813B (en) * 2021-03-15 2022-01-04 南京航空航天大学 System and method for detecting self-adaptive wind speed and wind direction under parachute landing situation
CN113252294A (en) * 2021-06-16 2021-08-13 西南交通大学 Cross-sea bridge space wind speed and direction testing system and monitoring method

Similar Documents

Publication Publication Date Title
CN110134134B (en) Wind measuring method for unmanned aerial vehicle in hovering state
CN103363992B (en) Based on four rotor wing unmanned aerial vehicle attitude heading reference system calculation methods of Gradient Descent
CN105841698B (en) A kind of AUV rudder angle precision real time measuring systems without zeroing
CN105004354B (en) Unmanned plane visible ray and infrared image object localization method under large slanting view angle machine
CN106871927A (en) A kind of UAV electro-optical pod&#39;s alignment error Calibration Method
CN101813465B (en) Monocular vision measuring method of non-contact precision measuring corner
CN104459728B (en) A kind of magnetic declination calibration steps positioned based on GNSS
CN110988393A (en) Unmanned aerial vehicle wind speed and direction measurement and correction algorithm based on ultrasonic anemoscope
CN107831515A (en) Underwater Navigation method and system
CN1693851A (en) Aviation linear array CCD image geometric rough correct algorithm
CN107063244A (en) A kind of aircraft flight process analogy method
CN105892498A (en) Target staring and scanning control system based on triaxial holder
CN110779514B (en) Hierarchical Kalman fusion method and device for auxiliary attitude determination of bionic polarization navigation
CN106123917A (en) Consider the SINS compass alignment methods of outer lever arm effect
CN1069406C (en) Component magnetometer correction method and correctable device
CN101793517B (en) Online quick method for improving accuracy of attitude determination of airborne platform
CN105321186B (en) A kind of enclosure space map acquisition method and device based on orthogonal projection
CN105953797A (en) Combined navigation device based on single-axis gyroscope, inclinometer and milemeter and method
CN111412900B (en) Digital attitude compensation and wave measurement system and control method
CN104614746B (en) Device and method for calibrating attitude and heading reference system based on satellite receiver
CN108827302A (en) Multi-rotor aerocraft air navigation aid based on rotor tachometric survey
CN105928513B (en) A kind of airborne synthetic aperture radar movement parameter measurement method based on position and attitude measuring system
CN106968665A (en) A kind of method that oil well deviational survey is carried out using inertial navigation system
CN109708661B (en) Visual axis inertia stabilization method of inertia stabilization equipment with two-axis frame
CN106595657B (en) Axial symmetry aircraft attitude measurement device and its measurement method

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
WD01 Invention patent application deemed withdrawn after publication
WD01 Invention patent application deemed withdrawn after publication

Application publication date: 20200410