CN103453875A - Real-time calculating method for pitch angle and roll angle of unmanned aerial vehicle - Google Patents

Abstract

The invention discloses a real-time calculating method for the pitch angle and the roll angle of an unmanned aerial vehicle. The method comprises the following steps: firstly, establishing a geodetic coordinate system, a body coordinate system and an image coordinate system; calculating an optical flow field of an image collected by a camera through an airborne calculator; extracting the position information of a heaven and earth segment line in the image coordinate system according to the characteristic of low speed of the optical flow field in the sky and high speed of the optical flow field on the ground; then, calculating slope and intercept of the heaven and earth segment line in the image coordinate system; establishing a corresponding relation between the geodetic coordinate system and the image coordinate system according to a camera projection model, and deducing a corresponding relation between the body coordinate system and the image coordinate system, namely the corresponding relation between the roll angle and the pitch angle of the unmanned aerial vehicle and the slope and the intercept of the heaven and earth segment line so as to further calculate information of the roll angle and the pitch angle of the unmanned aerial vehicle in real time. According to the method disclosed by the invention, few equipment is adopted and is simple, and the calculation result is easy to implement.

Description

A kind of for the unmanned plane angle of pitch and roll angle real-time computing technique

Technical field

Invention relates to a kind of real-time computing technique for the unmanned plane angle of pitch and roll angle, belongs to the unmanned aerial vehicle (UAV) control technical field.

Background technology

At present, the real-time computing technique of the known unmanned plane angle of pitch and roll angle all completes by systems such as inertial measurement cluster, GPS, there are the part computing method to come detection feature object or unique point with two video cameras, thereby calculate the angle of pitch and the roll angle of unmanned plane.But the instrument and equipment weight that said method is used is large and system architecture is complicated, is not easy to unmanned plane and uses.

Summary of the invention

In view of this, the invention provides a kind ofly for the unmanned plane angle of pitch and roll angle real-time computing technique, only need a video camera, just can calculate in real time the unmanned plane angle of pitch and roll angle information.

A kind of for the unmanned plane angle of pitch and roll angle real-time computing technique, coordinate predefine: set up body axis system OXYZ, wherein initial point O is positioned at video camera photocentre place, and OZ is parallel with optical axis and point to head, and OY vertically points to the earth's core; Set up image coordinate system O ₀uv, the video camera focal plane left upper apex of installing on the definition unmanned plane is O _o, u and v are respectively the both directions of focal plane horizontal and vertical;

The first step: set up the index J of search world cut-off rule,

J＝D(u _sky)+D(v _sky)+D(u _grd)+D(v _grd)

D (u wherein _sky) mean the variance on the u direction of sky area light flow field, D (v _sky) mean the variance on the v direction of sky area light flow field, D (u _grd) mean the variance on the optical flow field u direction of the earth zone, D (v _grd) mean the variance on the optical flow field v direction of the earth zone;

Second step: utilize and be arranged on the camera acquisition image on unmanned plane, and obtain the optical flow field of image, based on index J, extract world cut-off rule at image coordinate system O ₀straight-line equation in uv;

The detailed process of this step is:

At first, at image coordinate system O ₀in uv, use the image of a straight cuts camera acquisition, and the straight line two side areas is defined as respectively to day dummy section and the earth zone; Secondly, the variance of calculated line two side areas optical flow field on u and v direction, obtain index J by these variance summations respectively; Again, this straight line is moved on image until travel through whole image-region, by index J value a hour corresponding straight line be defined as world cut-off rule, extract world cut-off rule at image coordinate system O ₀straight-line equation v=ku+b in uv, wherein k is that straight slope and b are the straight line intercepts;

The 3rd step: the k in the straight-line equation v=ku+b of world cut-off rule and the corresponding relation between b substitution body axis system and image coordinate system are expressed to formula, calculate roll angle φ and pitching angle theta,

$\left\{\begin{array}{c}\mathrm{\φ}=\arctan \left(k\frac{{\mathrm{\α}}_x}{{\mathrm{\α}}_y}\right)\\ \mathrm{\θ}=\arctan \left[\frac{(v_0-{\mathrm{ku}}_0-b)\cos \mathrm{\φ}}{{\mathrm{\α}}_y}\right]\end{array}\right.$

α wherein _xand α _ybe respectively from the imaging plane to the plane of delineation at the amplification coefficient of x direction and y direction, u ₀and v ₀be respectively the length of image and wide.

Beneficial effect:

1, the present invention only needs to use a video camera, does not need other redundance units, makes computing equipment simple in structure, easily realizes.

2, the present invention adopts to the extraction of the world cut-off rule method of calculating light stream, calculate the optical flow field of the image information that comprises world cut-off rule of camera acquisition, and by sparse and these the dense characteristics of earth light flow field of sky background optical flow field, divide world cut-off rule, with traditional roll angle and angle of pitch computing method, the method of particularly using world Area Ratio to obtain the angle of pitch is compared, and the precision as a result that this method obtains is higher.

3, the present invention in the situation that obtain the priori such as camera intrinsic parameter, carrys out the angle of pitch and the roll angle of calculating aircraft with the camera intrinsic parameter model, takes full advantage of existence conditions, makes simplify of arithmetic, has saved equipment cost.

The accompanying drawing explanation

The installation site schematic diagram that Fig. 1 is video camera of the present invention and unmanned plane;

The schematic diagram that Fig. 2 is three coordinate systems setting up of the present invention.

Fig. 3 extracts the algorithm flow chart of world cut-off rule in the present invention.

Wherein, 1. unmanned plane, 2. video camera, 3. the earth, 4. world cut-off rule, 5. earth coordinates, 6. body axis system, 7. image coordinate system.

Embodiment

Below in conjunction with the accompanying drawing embodiment that develops simultaneously, describe the present invention.

The invention provides a kind ofly for the unmanned plane angle of pitch and roll angle real-time computing technique, as shown in Figure 1, video camera 2 is connected with unmanned plane 1, is arranged on its head; Set up earth coordinates 5, body axis system 6 and 7, three coordinate systems of image coordinate system and be right-handed coordinate system; As shown in Figure 2, O _wx _wy _wz _wearth coordinates 5, O _wbe positioned at infinite point, O _wy _wstraight up, O _wz _wparallel with the body longitudinal axis; OXYZ is body axis system 6, and wherein O is positioned at video camera photocentre place, and OZ is parallel with optical axis and point to head, and OY vertically points to the earth's core; O ₀uv is the image coordinate system 7 of imaging plane, and definition focal plane left upper apex is O _o, u and v are respectively the both directions of focal plane horizontal and vertical.

Utilize video camera 2 to collect image and obtain the optical flow field of image, extracting the positional information of world cut-off rule 4 in image coordinate system 7, calculating afterwards slope and the intercept of world cut-off rule 4 in image coordinate system 7, its extract and computation process as follows:

At first obtain the optical flow field of image, consider that the partial velocity variation of corresponding sky in optical flow field is comparatively mild, the part of corresponding ground changes comparatively violent; Therefore, if using the pixel of sky part correspondence as a set, its corresponding light stream velocity amplitude variance should be less; In like manner, above ground portion also has same alike result, and when the world is cut apart by ideal, sky part and above ground portion should have all features of minimum of variance, therefore adopts the index of following formula as search world cut-off rule 4:

J＝D(u _sky)+D(v _sky)+D(u _grd)+D(v _grd)

D (u wherein _sky) mean the variance on the u direction of sky area light flow field, D (v _sky) mean the variance on the v direction of sky area light flow field, D (u _grd) mean the variance on the optical flow field u direction of the earth zone, D (v _grd) mean the variance on the optical flow field v direction of the earth zone;

In image coordinate system 7, the image that uses a straight cuts to obtain, suppose that the image length and width are for (u ₀, v ₀), parameter γ ∈ [0,180 °), the angle that wherein γ is straight line and horizontal direction, then calculate respectively the calculated line two side areas at optical flow field the variance on u and v direction, these variances are sued for peace and are designated as J; This straight line is moved on image until travel through whole image-region, the linear position that makes the value minimum of J is the position of world cut-off rule; Obtain thus the straight-line equation v=ku+b of world cut-off rule 4, wherein k is that straight slope and b are the straight line intercepts; Idiographic flow is as follows:

1: input initial value J=∞, γ=0 °, input picture length and width u ₀, v ₀, input intermediate variable B=0, K=0;

2: judge whether γ equals 90 °, if not, continues next step, if directly jump to 8;

3: make variable k=tan γ, b=0;

4: judge whether b is less than v ₀-ku ₀if,, continue next step, if not, directly jump to 7;

5: with straight line, v=ku+b is cut apart image, calculates D (u _sky), D (v _sky), D (u _grd) and D (v _grd), calculate J ₁=D (u _sky)+D (v _sky)+D (u _grd)+D (v _grd), if J ₁<J, make J=J ₁, B=b, K=k, continue next step, otherwise,, to J, B and K assignment again, directly do not jump to 6;

6: make b from adding 1, jump to 4;

7: make γ from adding 1, jump to 2;

8: make b=B, k=K, output world cut-off rule equation v=ku+b, finish; As shown in Figure 3;

Can obtain the straight-line equation v=ku+b of world cut-off rule 4, wherein k is straight slope, and b is the straight line intercept;

Set up the corresponding relation between earth coordinates 5 and image coordinate system 7 according to the video camera projection model, and derive thus the corresponding relation between body axis system 6 and image coordinate system 7, its derivation step is as follows:

If on world cut-off rule 4, any point p is at O _wx _wy _wz _wearth coordinates 5 coordinates are (x _h, 0,0), if using the initial point O of camera coordinate system as center, because ground level is that circumference piles, can be by the initial point O of world coordinate system _wbe located on camera coordinate system yOz plane, like this, the world coordinates (X of video camera _w, Y _w, Z _w) can be set as (0, t _y, t _z), t wherein _ythe height of video camera apart from ground, t _zthe horizontal range of camera coordinate system initial point apart from world cut-off rule 4;

According to camera model, set up earth coordinates p (x _h, 0,0) 5 with image coordinate system 7 on to put the relation of p ' (u, v) as follows:

$\mathrm{sm}={\mathrm{MR}}_z^{\mathrm{\&phi;}}{R}_x^{\mathrm{\&theta;}}{R}_y^{\mathrm{\&psi;}}({\mathrm{Rp}}_w+t)---\left(1\right)$

Wherein s is scale factor, m=[u, v, 1] be that a p ' is at image coordinate system O ₀homogeneous coordinates on uv7, M is the Intrinsic Matrix of camera,

$M=\left[\begin{array}{ccc}{\mathrm{\&alpha;}}_x& 0& {\mathrm{<figure-callout\; id="0"\; label="u"\; filenames="HDA00003631736600021.png"\; state="\{\{state\}\}">u</figure-callout>}}_0\\ 0& {\mathrm{\&alpha;}}_{\mathrm{<figure-callout\; id="0"\; label="y\; v"\; filenames="HDA00003631736600021.png"\; state="\{\{state\}\}">y</figure-callout>}}& v_{}{}_0\\ 0& 0& 1\end{array}\right]---\left(2\right)$

α in Intrinsic Matrix _xand α _yrespectively at x direction of principal axis and the axial amplification coefficient of y, α from the imaging plane to the plane of delineation _x, α _y, u ₀and v ₀by in advance the demarcation of video camera 2 being obtained;

be that video camera 2 rotates θ around the x axle respectively, around y axle rotation ψ, around the rotation matrix of z axle rotation φ, they are respectively:

$R_x^{\mathrm{\&theta;}}=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\&theta;}& -\sin \mathrm{\&theta;}\\ 0& \sin \mathrm{\&theta;}& \cos \mathrm{\&theta;}\end{array}\right]---\left(3\right)$

$R_y^{\mathrm{\&psi;}}=\left[\begin{array}{ccc}\cos \mathrm{\&psi;}& 0& \sin \mathrm{\&psi;}\\ 0& 1& 0\\ -\sin \mathrm{\&psi;}& \sin \mathrm{\&theta;}& \cos \mathrm{\&psi;}\end{array}\right]---\left(4\right)$

$R_z^{\mathrm{\&phi;}}=\left[\begin{array}{ccc}\cos \mathrm{\&phi;}& -\sin \mathrm{\&phi;}& 0\\ \sin \mathrm{\&phi;}& \cos \mathrm{\&phi;}& 0\\ 1& 1& 1\end{array}\right]---\left(5\right)$

The transition matrix that R is body axis system 6 and earth coordinates 5,

$R=\left[\begin{array}{ccc}1& 0& 0\\ 0& -1& 0\\ 0& 0& -1\end{array}\right]---\left(6\right)$

P _wfor the world coordinates of any point on the cut-off rule of the world,

$p_W=\left[\begin{array}{c}{x}_{\mathrm{<figure-callout\; id="0"\; label="h"\; filenames="HDA00003631736600021.png"\; state="\{\{state\}\}">h</figure-callout>}}\\ 0\\ 0\end{array}\right]---\left(7\right)$

T is world coordinate system initial point O _wcoordinate in camera coordinate system, be a translation vector, supposes O _won the xOy plane, have

$t=\left[\begin{array}{c}0\\ t_y\\ t_z\end{array}\right]---\left(8\right)$

By formula (2-8) substitution (1), the camera model deformable is:

$s\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{c}{\mathrm{\&alpha;}}_x[x_h\cos \mathrm{\&phi;}-\sin \mathrm{\&phi;}(t_y\cos \mathrm{\&theta;}-t_z\sin \mathrm{\&theta;})]+u_0(t_y\sin \mathrm{\&theta;}+t_z\cos \mathrm{\&theta;})\\ {\mathrm{\&alpha;}}_y[x_h\sin \mathrm{\&phi;}+\cos \mathrm{\&phi;}(t_y\cos \mathrm{\&theta;}-t_z\sin \mathrm{\&theta;})]+u_0(t_y\sin \mathrm{\&theta;}+t_z\cos \mathrm{\&theta;})\\ t_y\sin \mathrm{\&theta;}+t_z\cos \mathrm{\&theta;}\end{array}\right]---\left(9\right)$

Make s=1, (9) formula can be reduced to:

$\left\{\begin{array}{c}u-{\mathrm{<figure-callout\; id="0"\; label="u"\; filenames="HDA00003631736600021.png"\; state="\{\{state\}\}">u</figure-callout>}}_0=\frac{{\mathrm{\&alpha;}}_x[x_h\cos \mathrm{\&phi;}-\sin \mathrm{\&phi;}(t_y\cos \mathrm{\&theta;}-t_z\sin \mathrm{\&theta;})]}{t_y\sin \mathrm{\&theta;}+t_z\cos \mathrm{\&theta;}}\\ v-{\mathrm{<figure-callout\; id="0"\; label="v"\; filenames="HDA00003631736600021.png"\; state="\{\{state\}\}">v</figure-callout>}}_0=\frac{{\mathrm{\&alpha;}}_y[x_h\sin \mathrm{\&phi;}+\cos \mathrm{\&phi;}(t_y\cos \mathrm{\&theta;}-t_z\sin \mathrm{\&theta;})]}{t_y\sin \mathrm{\&theta;}+t_z\cos \mathrm{\&theta;}}\end{array}\right.---\left(10\right)$

The displacement variable, order:

$\left\{\begin{array}{c}x=u-u_0\\ y=v-v_0\\ A=t_y\cos \mathrm{\&theta;}-t_z\sin \mathrm{\&theta;}\\ B=t_y\sin \mathrm{\&theta;}+t_z\cos \mathrm{\&theta;}\end{array}\right.---\left(11\right)$

Can obtain:

$\left\{\begin{array}{c}x=\frac{{\mathrm{\&alpha;}}_x(x_h\cos \mathrm{\&phi;}-A\sin \mathrm{\&phi;})}{B}\\ y=\frac{{\mathrm{\&alpha;}}_y(x_h\sin \mathrm{\&phi;}-A\cos \mathrm{\&phi;})}{B}\end{array}\right.---\left(12\right)$

When the time, cancellation x _h, obtain:

$y=\left(\frac{{\mathrm{\&alpha;}}_y}{{\mathrm{\&alpha;}}_x}\tan \mathrm{\&phi;}\right)x+\frac{{\mathrm{\&alpha;}}_y}{\cos \mathrm{\&phi;}}\frac{A}{B}---\left(13\right)$

By formula (11), can obtain:

$\frac{A}{B}=\frac{t_y\cos \mathrm{\&theta;}-t_z\sin \mathrm{\&theta;}}{t_y\sin \mathrm{\&theta;}+t_z\cos \mathrm{\&theta;}}=\frac{(t_y/t_z)\cos \mathrm{\&theta;}-\sin \mathrm{\&theta;}}{(t_y/t_z)\sin \mathrm{\&theta;}+t_z\cos \mathrm{\&theta;}}---\left(14\right)$

For video camera,, in infinite point, therefore there is t in world cut-off rule 4 _z>>t _yso:

$\underset{t_z\&RightArrow;\&infin;}{\mathrm{lin}}(t_y/t_z)=0$

That is:

$\underset{t_z\&RightArrow;\&infin;}{\mathrm{lin}}\frac{A}{B}=\frac{-\sin \mathrm{\&theta;}}{\cos \mathrm{\&theta;}}=-\tan \mathrm{\&theta;}---\left(15\right)$

Formula (13) can be write as:

$y=\left(\frac{{\mathrm{\&alpha;}}_y}{{\mathrm{\&alpha;}}_x}\tan \mathrm{\&phi;}\right)x-\frac{{\mathrm{\&alpha;}}_y}{\cos \mathrm{\&phi;}}\tan \mathrm{\&theta;}---\left(16\right)$

By formula (11) substitution (16):

$v-{\mathrm{<figure-callout\; id="0"\; label="v"\; filenames="HDA00003631736600021.png"\; state="\{\{state\}\}">v</figure-callout>}}_0=\left(\frac{{\mathrm{\&alpha;}}_y}{{\mathrm{\&alpha;}}_x}\tan \mathrm{\&phi;}\right)(u-u_0)-\frac{{\mathrm{\&alpha;}}_y}{\cos \mathrm{\&phi;}}\tan \mathrm{\&theta;}---\left(17\right)$

The above formula deformable is straight-line equation:

$v=\left(\frac{{\mathrm{\&alpha;}}_y}{{\mathrm{\&alpha;}}_x}\tan \mathrm{\&phi;}\right)u+v_0-\left(\frac{{\mathrm{\&alpha;}}_y}{{\mathrm{\&alpha;}}_x}\tan \mathrm{\&phi;}\right)u_0-\frac{{\mathrm{\&alpha;}}_y}{\cos \mathrm{\&phi;}}\tan \mathrm{\&theta;}---\left(18\right)$

Above formula is the straight-line equation of world cut-off rule 4 in the plane of delineation, and its slope k and intercept b are respectively:

$\left\{\begin{array}{c}k=\frac{a_y\tan \mathrm{\&phi;}}{a_x}\\ b=v_0-{\mathrm{ku}}_0-\frac{a_y\tan \mathrm{\&theta;}}{\cos \mathrm{\&phi;}}\end{array}\right.---\left(19\right)$

Roll angle φ and pitching angle theta can slope k and intercept b in above formula be tried to achieve respectively:

$\left\{\begin{array}{c}\mathrm{\&phi;}=\arctan \left(k\frac{{\mathrm{\&alpha;}}_x}{{\mathrm{\&alpha;}}_y}\right)\\ \mathrm{\&theta;}=\arctan \left[\frac{(v_0-{\mathrm{ku}}_0-b)\cos \mathrm{\&phi;}}{{\mathrm{\&alpha;}}_y}\right]\end{array}\right.---\left(20\right)$

α wherein _xand α _ybe respectively from the imaging plane to the plane of delineation at the amplification coefficient of x direction and y direction, u ₀and v ₀be respectively the length of image and wide.

In sum, these are only preferred embodiment of the present invention, be not intended to limit protection scope of the present invention.Within the spirit and principles in the present invention all, any modification of doing, be equal to replacement, improvement etc., within all should being included in protection scope of the present invention.

Claims (1)

1. one kind for the unmanned plane angle of pitch and roll angle real-time computing technique, it is characterized in that, calculation procedure is as follows:

Coordinate predefine: set up body axis system OXYZ, wherein initial point O is positioned at video camera photocentre place, and OZ is parallel with optical axis and point to head, and OY vertically points to the earth's core; Set up image coordinate system O ₀uv, the video camera focal plane left upper apex of installing on the definition unmanned plane is O _o, u and v are respectively the both directions of focal plane horizontal and vertical;

The first step: set up the index J of search world cut-off rule,

J＝D(u _sky)+D(v _sky)+D(u _grd)+D(v _grd)

D (u wherein _sky) mean the variance on the u direction of sky area light flow field, D (v _sky) mean the variance on the v direction of sky area light flow field, D (u _grd) mean the variance on the optical flow field u direction of the earth zone, D (v _grd) mean the variance on the optical flow field v direction of the earth zone;

Second step: utilize and be arranged on the camera acquisition image on unmanned plane, and obtain the optical flow field of image, based on index J, extract world cut-off rule at image coordinate system O ₀straight-line equation in uv;

The detailed process of this step is:

At first, at image coordinate system O ₀in uv, use the image of a straight cuts camera acquisition, and the straight line two side areas is defined as respectively to day dummy section and the earth zone; Secondly, the variance of calculated line two side areas optical flow field on u and v direction, obtain index J by these variance summations respectively; Again, this straight line is moved on image until travel through whole image-region, by index J value a hour corresponding straight line be defined as world cut-off rule, extract world cut-off rule at image coordinate system O ₀straight-line equation v=ku+b in uv, wherein k is that straight slope and b are the straight line intercepts;

The 3rd step: the k in the straight-line equation v=ku+b of world cut-off rule and the corresponding relation between b substitution body axis system and image coordinate system are expressed to formula, calculate roll angle φ and pitching angle theta,

$\left\{\begin{array}{c}\mathrm{\&phi;}=\arctan \left(k\frac{{\mathrm{\&alpha;}}_x}{{\mathrm{\&alpha;}}_y}\right)\\ \mathrm{\&theta;}=\arctan \left[\frac{(v_0-{\mathrm{ku}}_0-b)\cos \mathrm{\&phi;}}{{\mathrm{\&alpha;}}_y}\right]\end{array}\right.$

α wherein _xand α _ybe respectively from the imaging plane to the plane of delineation at the amplification coefficient of x direction and y direction, u ₀and v ₀be respectively the length of image and wide.

Images