CN103868521A - Autonomous quadrotor unmanned aerial vehicle positioning and controlling method based on laser radar - Google Patents

Abstract

The invention belongs to the technical field of autonomous flight control research on a quadrotor unmanned aerial vehicle, and discloses an autonomous positioning method based on a two-dimensional laser radar as well as a design method for a quadrotor unmanned aerial vehicle control system based on the positioning method and other onboard MEMS (micro electro mechanical systems) chips. According to the technical scheme adopted by the invention, the autonomous quadrotor unmanned aerial vehicle positioning and controlling method based on the laser radar comprises the following steps: preliminarily positioning the unmanned aerial vehicle in the horizontal direction by utilizing the two-dimensional laser radar, and acquiring a preliminary position value of the unmanned aerial vehicle in the height direction by utilizing an onboard barometer; then acquiring the position information of the unmanned aerial vehicle with high frequency by combining with onboard accelerometer chips by utilizing a complementary filtering algorithm; and finally, applying to unmanned aerial vehicle control systems without GPS signals based on the position information. The autonomous quadrotor unmanned aerial vehicle positioning and controlling method based on the laser radar, which is provided by the invention, is mainly applied to design and manufacturing of autonomous flight control devices of the unmanned aerial vehicle.

Description

The autonomous location of four rotor wing unmanned aerial vehicles and control method based on laser radar

Technical field

The invention belongs to the technical field of four rotor wing unmanned aerial vehicle autonomous flight control researchs, specifically, relate to the autonomous location of four rotor wing unmanned aerial vehicles and control method;

Background technology

Unmanned spacecraft is called for short unmanned plane, is that a kind of to utilize wireless remote control or self programmed control be main not manned aircraft; Compared with manned aircraft, unmanned plane has the advantages such as volume is little, cost is low, easy to use; According to the difference of its structural design, unmanned plane can be divided into two types of fixed-wing and rotors; Four rotor wing unmanned aerial vehicles belong to the one in rotor wing unmanned aerial vehicle, it is small and exquisite, simple in structure, safe and to the advantage such as flight space requirement is low that it has volume, have broad application prospects at military and civil area, comprise that topographic mapping, power grid maintenance, air transportation, disaster the practical application such as search and rescue and trace and monitor; Four rotor wing unmanned aerial vehicles are that one is typically owed driving, strong coupling, quiet unsettled nonlinear system, and it is flown to control to study important theory value; Abroad for four rotor wing unmanned aerial vehicle research startings early, technology is also more ripe; For example Univ Pennsylvania USA, Massachusetts Institute of Technology (MIT), Georgia Tech, Freiburg, Germany university, the Research Team of the countries such as the federal science and engineering of Zurich, SUI and France, Australia, Japan, Korea S, has obtained comparatively deep scientific payoffs; To more complicated maneuver, more complete tasks carrying aspect is studied now; In recent years, domestic also in the research of progressively carrying out four rotor wing unmanned aerial vehicles, as the well-known colleges and universities such as the Chinese Academy of Sciences, Tsing-Hua University, the National University of Defense technology, BJ University of Aeronautics & Astronautics, Northeastern University have all carried out certain research;

Four rotor wing unmanned aerial vehicle electronic systems can be controlled two subsystems by sensor and flight and form; Sensing system need to accurately be measured or estimate unmanned plane state, and state of flights such as unmanned plane position, speed, attitude angle, angular velocity is passed to flight subsystem by it, and realize good flight by it and control; Sensing system can be subdivided into attitude sensor and position transducer two classes; Conventional attitude sensor is made up of MEMS elements such as three axis accelerometer, three-axis gyroscope, magnetometers, and it all can use in indoor and outdoor; Position transducer more adopts the NAVSTAR such as GPS, the Big Dipper, but this kind of sensor is difficult between the faint housing-group of gps signal and in indoor environment, realizes accurately location; For solving without accurate orientation problem under GPS environment, common method can be divided into based on motion capture system location such as VICON in the world, based on airborne camera location and based on three kinds of airborne laser radar location; Utilize laser radar as airborne position transducer, compared to additive method, have positioning precision high, weak to environment degree of dependence, be beneficial to advantages such as outdoor expansions, be the hot issue of international control field research;

But in China for without gps signal environment, design the autonomous location of a set of four rotor wing unmanned aerial vehicles based on laser radar and control system still in the research starting stage, unmanned plane location and the control system of therefore design a set of registration, perfect in shape and function, easily expand, control performance is good have great theory value and meaning, and can widen the application of four rotor wing unmanned aerial vehicles;

Summary of the invention

The present invention is intended to overcome the deficiencies in the prior art, a set of new autonomic positioning method based on two-dimensional laser radar is proposed, and four rotor wing unmanned aerial vehicle Control System Design methods based on this localization method and other airborne MEMS chip, the technical solution used in the present invention is for this reason, the autonomous location of four rotor wing unmanned aerial vehicles and control method based on laser radar, first utilize two-dimensional laser radar to carry out the Primary Location of unmanned plane horizontal direction, utilize airborne barometer to obtain the preliminary positional value of the short transverse of unmanned plane; Utilize afterwards complementary filter algorithm, in conjunction with airborne accelerometer chip, obtain the unmanned plane positional information of higher frequency; Finally, based on this positional information, be applied to without the unmanned aerial vehicle control system under gps signal.

In fact localization method of the present invention was completed by following two stages:

First stage is that the three-dimensional position uncorrected data of lower frequency obtains the stage.Wherein planar position is mainly used the location algorithm of laser radar, and it comprises tripleplane, changes three parts of figure strategy and coupling location, produces two dimensional surface location uncorrected data; And short transverse uncorrected data is obtained by airborne barometer chip measurement data.

Second stage is that higher frequency locator data is obtained the stage.It is specifically realized based on complementary filter, uses airborne accelerometer to merge the uncorrected data that the first stage produces, and obtains the position data that renewal rate is higher.

Horizontal direction Primary Location concrete steps based on laser radar are:

1) three-dimensional environment model is to equatorial projection

Utilize robot kinematics's coordinate transformation method, and the Eulerian angle that obtain in conjunction with Airborne Inertial navigation elements, the reference picture that Laser Radar Scanning is obtained and present image project on surface level, in this plane, recycling two dimensional surface matching process carries out displacement and measurement of angle, thereby obtains unmanned plane accurate positional information in the horizontal direction, and concrete steps are as follows, if laser radar, at moment t, scans and measures a little in three dimensions by i article of laser beam of counterclockwise order its laser radar coordinate system give a definition for

wherein θ _irepresent this laser beam place angle, r _irepresent the distance that this laser beam records, meanwhile according to Airborne Inertial navigation elements, recording laser radar roll angle of living in is that γ, the angle of pitch are β, by (1) formula, and will

be projected in world coordinate system:

$P_{i3d}=R_{\mathrm{pitch}}\×R_{\mathrm{roll}}\×P_i^l---\left(1\right)$

Wherein $R_{\mathrm{roll}}=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \left(\mathrm{\γ}\right)& -\sin \left(\mathrm{\γ}\right)\\ 0& \sin \left(\mathrm{\γ}\right)& \cos \left(\mathrm{\γ}\right)\end{array}\right],$ $R_{\mathrm{pitch}}=\left[\begin{array}{ccc}\cos \left(\mathrm{\β}\right)& 0& \sin \left(\mathrm{\β}\right)\\ 0& 1& 0\\ -\sin \left(\mathrm{\β}\right)& 0& \cos \left(\mathrm{\β}\right)\end{array}\right]$

Finally by P _i3dz axis component be set to 0, thereby obtain gathering a three-dimensional point in image

subpoint P on two dimensional surface _i2d, the image being made up of all subpoints is here the projected image of three-dimensional environment model on two dimensional surface;

2) iterative unmanned plane in-plane displancement and corner method according to a preliminary estimate

Utilize 1) in projecting method by unmanned plane not in the same time, the image projection collecting under different angles is to same plane; Then utilize iterative method to estimate unmanned plane in-plane displancement and corner, therefore needing to choose two width compares in conplane image, piece image is wherein chosen to be to the projected image that current time collects, and another width reference picture is chosen according to changing figure strategy;

Obtain after two width movement images according to changing figure strategy, need to utilize iterative estimate method to estimate unmanned plane in-plane displancement and corner; Here estimated reference picture is defined as to q to the transformation relation of present image, it comprises the rotational transform R (θ) and the translation transformation t that are determined by plane crab angle θ, definition p _ifor laser radar collect when a point in front projection image, definition

to put p for utilizing estimated displacement and corner _iproject to the point in reference picture, definition

and for reference picture middle distance

nearest consecutive point;

If current iteration number of times is k and k > 0, iteration until convergence or be absorbed in iteration loop, its each iteration step is described below:

The first step, arranges iterative initial value: if enter for the first time iterative program, and its conversion conjecture initial value q ₀be defined as q ₀=(t ₀, θ ₀), utilize Airborne Inertial navigation element, by recording difference with reference to crab angle between moment and current time as θ ₀, and establish initial time t ₀be 0, afterwards in each iteration, iterative initial value q _k=(t _k, θ _k) be set as the result that previous step calculates, θ _k, t _kthe difference of the crab angle while being respectively the k time iteration and moment;

Second step, determines matching relationship: by the iterative initial value q choosing _kbe applied to present image point p _iupper, obtain this coordinate on reference picture for each

reference picture institute a little in searching distance two nearest consecutive point

and wherein subscript j ₁, j ₂representative point

and point

respectively by laser radar j in the counterclockwise direction ₁and j ₂bar laser beam collects, and upper footnote i representative point

and point

with the some p in present image _imatch, then this matching relationship can be reduced to <i, j ₁ ⁱ, j ₂ ⁱ>, the some p that in present image, order label is i _i, being matched with in reference picture is j by order label ₁ ⁱand j ₂ ⁱtwo determined line segments of consecutive point

first the method need to determine the point that meets (2) formula in reference picture

$j_1^i=\arg \min {\left|\right|p_j-p_i^w\left|\right|}^2---\left(2\right)$

Here first need, according to laser beam resolution, initial search point is set, as shown in Equation 3;

$({\&angle;P}_i^w-{\mathrm{\&psi;}}_0)\&times;[\mathrm{nrays}/(\mathrm{\&theta;}\max -\mathrm{\&theta;}\min )]---\left(3\right)$

Wherein ψ ₀for preliminary sweep laser beam angular, nrays is laser beam sum, and θ max-θ min is initial and stops laser beam angular difference, afterwards relatively before and after starting point 2 with

range difference, get distance less some place direction start search; If current point with the bee-line that obtains before having exceeded of distance, stop the search of this direction and turn to starting point other direction to continue search; After this,, in the time that the search of both direction all reaches end condition, obtain the shortest coupling

finally choose

in 2 of front and back, with

that conduct that distance is nearer

so just, can obtain effectively mating <i j ₁ ⁱ, j ₂ ⁱ> and matching distance thereof;

The 3rd step, cutting part coupling is right: first crop a bit in repeated matching reference diagram

part coupling right, set up storage of array all with

relevant coupling is right, mates corresponding matching distance by traveling through these, takes out wherein minimum distance value, crops afterwards and is greater than this coupling apart from certain multiple to <i, j ₁ ⁱ, j ₂ ⁱ>, thereafter by remaining all couplings to by the sequence of matching distance length, then it is right to crop by preset ratio the coupling that its middle distance grows;

The 4th step, definition (4) is error function J,

$J(q_{k+1},C_k)=\underset{i}{\mathrm{\&Sigma;}}\left({n_i}^T\right[R\left({\mathrm{\&theta;}}_{k+1}\right)P_i+t_{k+1}-{P_{j_1}}^i\left]\right)---\left(4\right)$

Wherein n _ifor perpendicular to line segment

normal vector, C _kfor all effective matching relationships of current k step, this formula can be described as asks for transformation relation q _k+1=(t _k+1, θ _k+1), make not cropped all coupling centerings, point

to the line segment mating with it

distance summation the shortest, this nonlinear equation of (4) formula is asked for to extreme value, calculate unmanned plane horizontal direction position and corner variation estimate q _k+1=(t _k+1, θ _k+1), if do not meet stopping criterion for iteration, this value is set to iterative initial value, to first step continuation interative computation, until meet default termination of iterations condition.

Changing figure strategy is specially: first equal angles is chosen 1/4 laser beam and sampled, near secondly selected hover point, 30 centimetres of positive and negative 30 cm x of horizontal direction and corner are no more than the region of 30 ° for not changing graph region, in this region, fixing the first secondary projected image that hover point collects is reference picture, and what all the other moment samplings obtained all compares with the first secondary projected image when front projection image; When aircraft flies out behind this region, need to replace the existing displacement of reference picture record-setting flight device, again centered by new reference picture collection point, generate and do not change graph region afterwards, proceed coupling location.

Utilize 720MHz ARM process chip executive level direction localization method, its output Data Update frequency is between 10-20Hz.

Step 2: based on airborne barometer chip short transverse Primary Location

The present invention adopts unmanned aerial vehicle onboard barometer chip to measure unmanned plane height, because the running of unmanned plane rotor exerts an influence near air-flow barometer chip, causes in the situation that there is no safeguard measure, and barometer chip measurement data is difficult to use; Therefore the present invention is placed in the flight control system of loading barometer chip in the good plastic casing of closed performance, and utilize the materials such as sponge to block the gap of plastic casing and barometer chip chamber, thereby effectively get rid of the impact that unmanned plane rotor turns round near air-flow barometer chip; Gather afterwards airborne MEMS barometer data measured, its renewal frequency is 10Hz;

Step 3: the no-manned plane three-dimensional location estimation based on complementary filter

Adopt a kind of three-dimensional complementary filtering method, the no-manned plane three-dimensional acceleration information that utilizes airborne accelerometer to obtain, position data in step 1,2 is carried out to interpolation, thereby improve position data renewal frequency, be specially: unmanned plane acceleration transducer is upgraded to the measured value under the airborne coordinate system obtaining, be projected to world coordinate system by coordinate transform, and by twice integral and calculating, the 100Hz position quantity that obtains being estimated by accelerometer passes to on-board controller and carries out position control; In the horizontal direction, when laser radar matching process calculates when complete, calculate the difference of laser radar locator data and accelerometer estimated value, adjust integrated acceleration amount of bias by feedback system again, thereby realize the data fusion between these two kinds of foreign peoples's sensors of accelerometer and laser radar, complementary filter gain k ₁, k ₂, k ₃can be defined as follows by timeconstantτ:

$\begin{array}{c}{k}_1=\frac{3}{\mathrm{\&tau;}}\\ k_2=\frac{3}{{\mathrm{\&tau;}}^2}\\ k_3=\frac{1}{{\mathrm{\&tau;}}^3}\end{array}---\left(5\right)$

In like manner, in short transverse, also adopt complementary filter, realize accelerometer and barometrical data fusion.

Technical characterstic of the present invention and effect:

1. system stability is reliable;

The system building method that the present invention proposes, is distributed in laser radar alignment sensor and bottom flight control in different onboard units, has guaranteed that whole system is stable, efficient, reliable;

2. matching locating method real-time is high; Traditional ICP matching process often needs to carry out repeatedly iteration, searches and just can find coupling right one by one, the therefore low shortcoming of these method ubiquity searching efficiencies; And the modified ICP method that the present invention uses has adopted and has set up in advance jumping table, cutting coupling equity strategy, its searching efficiency, apparently higher than traditional method for searching, therefore can be realized on on-board controller;

3. data fusion method is succinctly efficient; Due to adopted complementary filter method programming realization uncomplicated, calculated amount is less than common Kalman filtering algorithm, is therefore convenient to use in the airborne singlechip chip of low speed;

4. high without gps signal environment unmanned plane using value; The location based on laser radar and control system building plan that the present invention proposes, expanded the application of UAS, especially in without gps signal situation, and the demand of accurately locating and controlling;

Accompanying drawing explanation

Fig. 1 modified localization method process flow diagram;

Fig. 2 one dimension complementary filter block diagram;

The four rotor wing unmanned aerial vehicle hardware structure diagrams of Fig. 3 based on laser radar;

Fig. 4 is based on laser radar four rotor wing unmanned aerial vehicle software architecture diagrams;

The indoor hovering experimental result picture of Fig. 5 tetra-rotor wing unmanned aerial vehicle;

Embodiment

The technical solution used in the present invention is the autonomous location of four rotor wing unmanned aerial vehicles and control method, in-plane displancement and outer corner measurement: adopt laser radar to carry out iterative relative positioning, therefore need to choose two width images and compare, define p here _ifor a point in present image, definition

for the point in reference picture, reference picture is defined as rotation R (θ) and translation t to the transformation relation of present image;

If current iteration number of times is k and k > 0, iteration until convergence or be absorbed in iteration loop, its each iteration step is described below:

The first step, arranges iterative initial value: if enter for the first time iterative program, and its conversion conjecture initial value q ₀=(t ₀, θ ₀), utilize Airborne Inertial navigation element, by recording between two moment crab angle difference as θ ₀, t ₀can be set to 0, afterwards in each iteration, iterative initial value q _k=(t _k, θ _k) be set as the result that calculates for previous step;

Second step, determines matching relationship: by the iterative initial value q choosing _kbe applied to present image point p _iupper, obtain this coordinate on reference picture for each

reference picture institute a little in searching distance

two nearest points

and

definition matching relationship is <i, j ₁ ⁱ, j ₂ ⁱ>, present image mid point i is matched with reference picture middle conductor j ₁ ⁱ_ j ₂ ⁱ; First the method need to determine the point that meets (1) formula

$j_1^i=\arg \min {\left|\right|p_j-p_i^w\left|\right|}^2---\left(1\right)$

First according to laser beam resolution, initial search point is set, as shown in Equation 2.

$({\&angle;P}_i^w-{\mathrm{\&psi;}}_0)\&times;[\mathrm{nrays}/(\mathrm{\&theta;}\max -\mathrm{\&theta;}\min )]---\left(2\right)$

Wherein ψ ₀for preliminary sweep laser beam angular, nrays is laser beam sum, and θ max-θ min is minimax laser beam difference, afterwards relatively starting point upper and lower 2 with

range difference, get distance less some place direction start search; If current point with

distance obtain bee-line before having exceeded, stop search for and turn to starting point other direction to continue to search for; After this,, in the time that the search of both direction all reaches end condition, obtain the shortest coupling

finally choose

in 2 of front and back, with

that conduct that distance is nearer

so just, can obtain effectively mating <i j ₁ ⁱ, j ₂ ⁱ> and matching distance thereof;

The 3rd step, cutting part coupling is right: first crop a bit in repeated matching reference diagram

part coupling right, set up storage of array all with

relevant coupling, mates corresponding matching distance by traveling through these, takes out wherein minimum distance value, crops afterwards and is greater than this coupling apart from certain multiple to <i, j ₁ ⁱ, j ₂ ⁱ>, thereafter remaining all couplings are sorted to pressing matching distance length, then it is right to crop the shorter coupling of its middle distance by preset ratio;

The 4th step, definition (3) is error function, wherein n _ifor perpendicular to line segment j ₁ ⁱ_ j ₂ ⁱnormal vector, this formula can be described as asks for transformation relation q _k+1=(t _k+1, θ _k+1), make not cropped all coupling centerings, point to the line segment mating with it

distance summation;

$J(q_{k+1},C_k)=\underset{i}{\mathrm{\&Sigma;}}\left({n_i}^T\right[R\left({\mathrm{\&theta;}}_{k+1}\right)P_i+t_{k+1}-{P_{j_1}}^i\left]\right)---\left(3\right)$

This nonlinear equation of (3) formula is asked for to extreme value, and q is estimated in the variation that calculates position and attitude _k+1=(t _k+1, θ _k+1), if do not meet stopping criterion for iteration, this is worth to iterative initial value the most, to first step continuation interative computation, until meet default termination of iterations condition.

Employing is changed figure strategy and is improved adopting laser radar to carry out iterative relative positioning:

First equal angles is chosen 1/4 laser beam and is sampled, near secondly selected hover point, the positive and negative 60cm*60cm of horizontal direction, corner is no more than the region of 30 ° for not changing graph region, in this region, the first sub-picture that fixing hover point collects is reference picture, and all the other moment present image obtaining of sampling all compares with the first sub-picture; When aircraft flies out behind this region, need to replace the existing displacement of reference picture record-setting flight device, again centered by new reference picture collection point, generate and do not change graph region afterwards, proceed coupling location.

Under three-dimensional environment, locate: utilize robot kinematics's coordinate transformation method, the Eulerian angle that obtain in conjunction with inertial navigation unit, the reference picture that Laser Radar Scanning is obtained and present image project on surface level, in this plane, recycling two dimensional surface matching process carries out displacement and measurement of angle, thereby obtain unmanned plane accurate positional information in the horizontal direction, concrete steps are as follows, establish moment t laser beam i and scan and measure a little in three dimensions

its laser radar coordinate system give a definition for

wherein θ _irepresent this laser beam place angle, r _irepresent the distance that this laser beam records, meanwhile according to Airborne Inertial navigation elements, recording laser radar roll angle of living in is that γ, the angle of pitch are β, by (4) formula, and will

be projected in world coordinate system:

$P_i=R_{\mathrm{pitch}}\&times;R_{\mathrm{roll}}\&times;P_i^l---\left(4\right)$

Wherein $R_{\mathrm{roll}}=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \left(\mathrm{\&gamma;}\right)& -\sin \left(\mathrm{\&gamma;}\right)\\ 0& \sin \left(\mathrm{\&gamma;}\right)& \cos \left(\mathrm{\&gamma;}\right)\end{array}\right],$ $R_{\mathrm{pitch}}=\left[\begin{array}{ccc}\cos \left(\mathrm{\&beta;}\right)& 0& \sin \left(\mathrm{\&beta;}\right)\\ 0& 1& 0\\ -\sin \left(\mathrm{\&beta;}\right)& 0& \cos \left(\mathrm{\&beta;}\right)\end{array}\right]$

Finally by P _iz axis component be set to 0, thereby obtain three-dimensional point projection on two dimensional surface, then applies 1 midplane measuring method and calculates unmanned plane displacement in the horizontal direction.

Adopt three-dimensional complementary filtering method, by the acceleration information collecting, the position data that localization method and barometer are obtained is carried out interpolation, improve position data renewal frequency, be specially: unmanned plane acceleration transducer is upgraded to the data that obtain, project to world coordinate system by coordinate transform, by twice integral and calculating, the 100Hz position quantity that obtains being estimated by accelerometer passes to on-board controller and carries out position control, when laser radar matching process calculates when complete, calculate the difference of laser radar locator data and accelerometer estimated value, adjust integrated acceleration amount of bias by feedback system again, thereby realize the data fusion between these two kinds of foreign peoples's sensors of accelerometer and laser radar, complementary filter gain k ₁, k ₂, k ₃be defined as follows by timeconstantτ:

$\begin{array}{c}{k}_1=\frac{3}{\mathrm{\&tau;}}\\ k_2=\frac{3}{{\mathrm{\&tau;}}^2}\\ k_3=\frac{1}{{\mathrm{\&tau;}}^3}\end{array}---\left(5\right)$

In like manner, short transverse adopts complementary filter, realizes accelerometer and barometrical data fusion.

" three-dimensional " in three-dimensional complementary wave filter refers to two translational degree of freedom and adds a short transverse degree of freedom.An acceleration transducer can be installed on unmanned body, and in order to record the current acceleration of unmanned plane, this sensor renewal frequency is higher, is 100Hz.And the unmanned plane two dimensional surface displacement renewal frequency that only relies on laser radar matching algorithm to estimate only has 10-20Hz, and the altitude information renewal frequency that barometer chip records only has 10Hz.And only use uncorrected data location, be difficult to obtain good unmanned aerial vehicle (UAV) control effect.Therefore the present invention has adopted complementary filter to merge acceleration and two sensors of laser radar, makes sensor output locator data frequency upgrading to 100Hz.

" complementary filter gain " is the parameter on wave filter backfeed loop, estimate or the affect power of barometric surveying data on integrated acceleration in forward path with deciding laser radar, be to determine that wave filter Output rusults more approaches laser radar or barometer uncorrected data, or the position data of estimating closer to integrated acceleration.

Between " complementary filter gain ", have contact, choosing of its value can rely on " time constant " to determine.

Below in conjunction with the drawings and specific embodiments, further describe the present invention.

Four rotor wing unmanned aerial vehicles are independently located and control in without gps signal situation, require unmanned plane can process at short notice the metrical information of foreign peoples's multisensor, as sensing equipments such as laser radar, acceleration, gyroscope, magnetometers; Because computing power and the storage resources of conventional one-piece machine controller are difficult to meet complicated approach real-time processing requirement, especially be difficult to solve the problem of laser radar positioning method real-time, the present invention has designed a kind of based on ARM Cortex-A8 microprocessor and in conjunction with four rotor wing unmanned aerial vehicle system building schemes of Atmega2560 on-board controller, by discrete on different onboard units to laser radar location and bottom control; In addition the present invention utilizes complementary filter method to merge laser radar data and acceleration information, has guaranteed high stability, high precision and the high reliability of practical flight system;

1. four rotor wing unmanned aerial vehicle system hardware compositions

This forms as shown in Figure of description 3 without four rotor wing unmanned aerial vehicle system hardwares under gps signal, and this figure has shown four rotor wing unmanned aerial vehicle system hardware composition and the annexations based on laser radar, introduces in detail each several part characteristic and function thereof below;

1) four rotor unmanned aircrafts

Fuselage body of the present invention adopts small-sized electric model plane quadrotor body, and its diameter is 450mm, uses nylon to add fiber material and makes, and carries 4 groups of brushless electric machines, electricity tune and rotor; Can use Futaba telepilot and 2.4GHz receiver to realize the manual flight of quadrotor;

2) Hokuyo UTM-30LX laser radar

The present invention uses the Japanese Hokuyo UTM-30LX of company laser radar as airborne alignment sensor, and its supply voltage is DC12V ± 10%, and laser beam flying angle is 270 °, and resolution is about 0.25 °, and finding range is 0.1-30m, and be 25ms sweep time;

3) airborne microprocessor

The present invention selects 720MHz flush bonding processor based on Cortex-A8 chip as airborne computing unit; The embedded terminal (SuSE) Linux OS of this microprocessor, operation laser radar positioning method, and the position data calculating is sent to the control of flying of airborne microcontroller;

4) airborne microcontroller

The present invention selects Atmega2560 chip design on-board controller; This flight control system is integrated, and three axis accelerometer, three-axis gyroscope, three axle magnetic force are taken into account height barometer, and it can realize plurality of flight such as increasing steady flight, spot hover, Track In Track, path planning; The present invention writes APM Open Source Code again, has improved its position data transmission interface, realizes the object of compatible laser radar locator data;

5) system power

It is quadrotor power supply that system adopts 2300mAh3S lithium polymer battery; This battery needs well to charge to 12.6V before using, and can adopt high power switching power supply and charger to complete, and charging current should not be greater than 4A; In flight course, need to connect voltage alarms based on pattern recognition, prevent that lithium polymer battery voltage is lower than 10.8V; Input voltage is that UTM-30LX laser radar and the electron speed regulator of 12V can directly be powered by lithium polymer battery, input voltage is that the electronic equipments such as the airborne processor, on-board controller, RC receiver of 5V can adjust output terminal to power by electricity, and the chips such as the barometer that input voltage is 3.3V can be powered by 5V to 3.3V voltage transitions chip on microcontroller circuit plate;

Four rotor wing unmanned aerial vehicle location and the control system based on laser radar that the present invention builds, gross weight 1600g, the full load flight time can reach 7 minutes;

2. four rotor wing unmanned aerial vehicle system software composition and control signal flow processs

Figure of description 4 has been shown four rotor wing unmanned aerial vehicle system software composition and the control signal flow processs based on laser radar, specific as follows:

1) airborne microprocessor portion

The embedded (SuSE) Linux OS of airborne microprocessor, adopts C language compilation positioning software; This software is organized by multiple threads, comprises laser radar data and reads thread, projection and matching algorithm thread, serial communication thread, daily record thread etc.; Wherein gather laser radar uncorrected data in serial ports obstruction mode, its renewal frequency reaches 40Hz; The modified scan matching method that comprises tripleplane that the present invention proposes, manages herein renewal frequency in device and can reach 20Hz; By modified GPS communication interface, the virtual GPS position data calculating can be passed to airborne microcontroller, consider that microcontroller processing power is limited here, therefore renewal frequency is made as to 10Hz;

2) airborne microcontroller part

Airborne microcontroller gathers the chip datas such as peripheral accelerometer, gyroscope, magnetometer by I2C interface, utilize nonlinear complementarity filtering method to resolve UAV Attitude data, and its renewal frequency is 100Hz; In addition airborne microcontroller also can gather barometer information in peripheral chip, and its renewal frequency is 10Hz; Owing to having adopted complementary filter method in microcontroller, by acceleration location of interpolation sensing data, be promoted to 100Hz by comprising laser radar data in interior three-dimensional location data renewal frequency, thereby reached comparatively accurate position control effect;

3) configuration of system each several part software and control signal flow process

System uses Futaba telepilot and eight passage RC receivers to carry out remote control and pattern is switched; Before aircraft takeoff, need to carry out telepilot and receiver signal effectively to frequently, guarantee that signal of communication stablizes smooth and easy; RC remote-control receiver signal wire and 4 electron speed regulator signal wires corresponding to motor all need to receive on airborne microcontroller; For reaching flight optimization effect, system can adopt external magnetometer method, thereby alleviates the impact of electromagnetic interference (EMI) on crab angle;

Airborne microprocessor, by serial ports 1 junctor bone laser radar, measures current environment image, and carries out Image Iterative coupling, estimation unmanned plane horizontal level; Be connected with gps signal interface on airborne microcontroller by serial ports 2, thereby locator data is transferred to microcontroller, carry out UAV Flight Control;

For reaching optimizer system state observation and manipulation demand, can use land station's remote control software; One group of data radio station of this software application is connected with airborne microcontroller, Real-time Obtaining unmanned plane current flight attitude, position, speed, acceleration and all kinds of method parameter; Can realize the function such as unmanned plane trajectory planning, aerial mission switching by earth station system in addition;

3. without unmanned plane hovering experimental example under gps signal environment

1. optimum efficiency control method is chosen

In the present invention, modified Iterative matching method can be carried out horizontal direction location, is merged and is obtained high precision, Gao Gengxin rate position data by complementary filter method and acceleration transducer; Can be used in conjunction with multiple flight control method as alignment sensor, for reaching flight optimization effect, the following inner and outer ring control method of this experimental applications, outer shroud is position ring, and interior ring is attitude ring, and it is given that position ring is set attitude;

In the method, each position and attitude degree of freedom are all controlled separately; First consider position ring, this ring is designed to position-speed double ring structure, establishes under unmanned plane body axis system, and given reference position is p ^ref, calculating current location by complementary filter is p ^actual, the design of position ring PI control rate is as follows:

$v^{\mathrm{ref}}=K_{\mathrm{pp}}\&CenterDot;(p^{\mathrm{ref}}-p^{\mathrm{actual}})+K_{\mathrm{ip}}\&CenterDot;\underset{0}{\overset{t}{\&Integral;}}(p^{\mathrm{ref}}-p^{\mathrm{actual}})\mathrm{dt}---\left(6\right)$

Wherein K _pp, K _ipbe respectively ratio, storage gain, v ^reffor the given reference value of speed ring; The present speed data v calculating according to complementary filter ^actual, design PID control rate is as follows:

$a^{\mathrm{ref}}=K_{\mathrm{pv}}\&CenterDot;(v^{\mathrm{ref}}-v^{\mathrm{actual}})+K_{\mathrm{dv}}\&CenterDot;({\stackrel{.}{v}}^{\mathrm{ref}}-{\stackrel{.}{v}}^{\mathrm{actual}})+k_{\mathrm{iv}}\&CenterDot;\underset{0}{\overset{t}{\&Integral;}}(v^{\mathrm{ref}}-v^{\mathrm{actual}})\mathrm{dt}---\left(7\right)$

Wherein K _pv, K _dv, K _ivbe respectively ratio, differential, storage gain, a ^refin attitude ring, the given reference value of the corresponding attitude angle of this position freedom;

Consider afterwards the design of attitude ring controller, this ring is designed to angle-angular velocity dicyclo control structure, calculates attitude of flight vehicle angle a by nonlinear complementarity filtering method in microcontroller ^actual, design PI controller here as follows:

${\mathrm{\&Omega;}}^{\mathrm{ref}}=K_{\mathrm{pa}}\&CenterDot;(a^{\mathrm{ref}}-a^{\mathrm{actual}})+K_{\mathrm{ia}}\&CenterDot;\underset{0}{\overset{t}{\&Integral;}}(a^{\mathrm{ref}}-a^{\mathrm{actual}})\mathrm{dt}---\left(8\right)$

Wherein K _pa, K _iabe respectively ratio, storage gain, Ω ^reffor the given reference value of angular velocity ring; Obtain current angular velocity data Ω according to gyroscope ^actual, design PID control rate is as follows:

$u=K_{\mathrm{p\&Omega;}}\&CenterDot;({\mathrm{\&Omega;}}^{\mathrm{ref}}-{\mathrm{\&Omega;}}^{\mathrm{actual}})+K_{\mathrm{d\&Omega;}}\&CenterDot;({\stackrel{.}{\mathrm{\&Omega;}}}^{\mathrm{ref}}-{\stackrel{.}{\mathrm{\&Omega;}}}^{\mathrm{actual}})+k_{\mathrm{i\&Omega;}}\&CenterDot;\underset{0}{\overset{t}{\&Integral;}}({\mathrm{\&Omega;}}^{\mathrm{ref}}-{\mathrm{\&Omega;}}^{\mathrm{actual}})\mathrm{dt}---\left(9\right)$

Wherein K _{p Ω}, K _{d Ω}, K _{i Ω}be respectively ratio, differential, storage gain, u is controller output;

For guaranteeing practical flight safety, in position ring and attitude ring, all set upper limit of integral I _{max_p}and I _{max_a}; Finally according to each rotor affixed position and rotation direction on aircraft, calculate by u rotating speed that each motor is corresponding and export to electron speed regulator and carry out Electric Machine Control;

2. hovering experimental example

This experiment is carried out under following parameter:

1) in localization method, minimum iteration distance is set as 0.00001 centimetre, minimum iteration corner and is set as 0.000001 radian, minimum is not changed graph region and is set as 60 centimetres of 60 cm x, it is 30 ° that minimum is not changed figure angle initialization, and complementary filter filter gain τ is set as 1;

2) in control method, parameter arranges as follows:

k _pp＝0.432 k _iv＝0.052 k _pa＝4

k _ip＝0 k _pΩ＝0.2530 k _ia＝0.1

k _pv＝20 k _dΩ＝0.0060 I _{max_a}＝5

k _dv＝0.09 k _iΩ＝0.1 I _{max_p}＝30

According to this parameter, utilize the aircraft platform proposing in the present invention to carry out indoor hovering flight, experimental result is by shown in instructions Fig. 5; This figure recorded and once taken off-hover-and complete flight course lands; Article 1, vertical dotted line has recorded manually-has independently hovered switching instant, and the vertical dotted line of Article 2 has recorded autonomous hovering-manual switchover moment; Wherein component (a) represents attitude of flight vehicle, and dotted line represents under floating state, and the attitude angle that position ring calculates is given, and wherein solid line is for recording actual attitude angle; As can be seen from the figure attitude angle is followed the tracks of quick and precisely, and autonomous hovering flight attitude is steady, and amplitude is no more than positive and negative 1 °; Component (b) represents position of aircraft state, and this aircraft is in hovering process as can be seen from Figure, and station keeping precision is no more than positive and negative 10 centimetres, has obtained good flight control effect.

Claims (5)

1. the autonomous location of four rotor wing unmanned aerial vehicles based on laser radar and a control method, is characterized in that, first utilizes two-dimensional laser radar to carry out the Primary Location of unmanned plane horizontal direction, utilizes airborne barometer to obtain the preliminary positional value of the short transverse of unmanned plane; Utilize afterwards complementary filter algorithm, in conjunction with airborne accelerometer chip, obtain the unmanned plane positional information of higher frequency; Finally, based on this positional information, be applied to without the unmanned aerial vehicle control system under gps signal.

2. the autonomous location of four rotor wing unmanned aerial vehicles based on laser radar as claimed in claim 1 and control method, is characterized in that, the horizontal direction Primary Location concrete steps based on laser radar are:

1) three-dimensional environment model is to equatorial projection

Utilize robot kinematics's coordinate transformation method, and the Eulerian angle that obtain in conjunction with Airborne Inertial navigation elements, the reference picture that Laser Radar Scanning is obtained and present image project on surface level, in this plane, recycling two dimensional surface matching process carries out displacement and measurement of angle, thereby obtains unmanned plane accurate positional information in the horizontal direction, and concrete steps are as follows, if laser radar, at moment t, scans and measures a little in three dimensions by i article of laser beam of counterclockwise order

its laser radar coordinate system give a definition for

wherein θ _irepresent this laser beam place angle, r _irepresent the distance that this laser beam records, meanwhile according to Airborne Inertial navigation elements, recording laser radar roll angle of living in is that γ, the angle of pitch are β, by (1) formula, and will

be projected in world coordinate system:

$P_{i3d}=R_{\mathrm{pitch}}\&times;R_{\mathrm{roll}}\&times;P_i^l---\left(1\right)$

Wherein $R_{\mathrm{roll}}=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \left(\mathrm{\&gamma;}\right)& -\sin \left(\mathrm{\&gamma;}\right)\\ 0& \sin \left(\mathrm{\&gamma;}\right)& \cos \left(\mathrm{\&gamma;}\right)\end{array}\right],$ $R_{\mathrm{pitch}}=\left[\begin{array}{ccc}\cos \left(\mathrm{\&beta;}\right)& 0& \sin \left(\mathrm{\&beta;}\right)\\ 0& 1& 0\\ -\sin \left(\mathrm{\&beta;}\right)& 0& \cos \left(\mathrm{\&beta;}\right)\end{array}\right]$

Finally by P _i3dz axis component be set to 0, thereby obtain gathering a three-dimensional point in image subpoint P on two dimensional surface _i2d, the image being made up of all subpoints is here the projected image of three-dimensional environment model on two dimensional surface;

2) iterative unmanned plane in-plane displancement and corner method according to a preliminary estimate

Utilize 1) in projecting method by unmanned plane not in the same time, the image projection collecting under different angles is to same plane; Then utilize iterative method to estimate unmanned plane in-plane displancement and corner, therefore needing to choose two width compares in conplane image, piece image is wherein chosen to be to the projected image that current time collects, and another width reference picture is chosen according to changing figure strategy;

Obtain after two width movement images according to changing figure strategy, need to utilize iterative estimate method to estimate unmanned plane in-plane displancement and corner; Here estimated reference picture is defined as to q to the transformation relation of present image, it comprises the rotational transform R (θ) and the translation transformation t that are determined by plane crab angle θ, definition p _ifor laser radar collect when a point in front projection image, definition to put p for utilizing estimated displacement and corner _iproject to the point in reference picture, definition

and

for reference picture middle distance

nearest consecutive point;

If current iteration number of times is k and k > 0, iteration until convergence or be absorbed in iteration loop, its each iteration step is described below:

The first step, arranges iterative initial value: if enter for the first time iterative program, and its conversion conjecture initial value q ₀be defined as q ₀=(t ₀, θ ₀), utilize Airborne Inertial navigation element, by recording difference with reference to crab angle between moment and current time as θ ₀, and establish initial time t ₀be 0, afterwards in each iteration, iterative initial value q _k=(t _k, θ _k) be set as the result that previous step calculates, θ _k, t _kthe difference of the crab angle while being respectively the k time iteration and moment;

Second step, determines matching relationship: by the iterative initial value q choosing _kbe applied to present image point p _iupper, obtain this coordinate on reference picture for each

reference picture institute a little in searching distance

two nearest consecutive point

and

wherein subscript j ₁, j ₂representative point

and point

respectively by laser radar j in the counterclockwise direction ₁and j ₂bar laser beam collects, and upper footnote i representative point

and point

with the some p in present image _imatch, then this matching relationship can be reduced to <i, j ₁ ⁱ, j ₂ ⁱ>, defining this matching relationship is <i, j ₁ ⁱ, j ₂ ⁱ>, present image mid point i is matched with reference picture middle conductor

first the method need to determine the point that meets (2) formula in reference picture

$j_1^i=\arg \min {\left|\right|p_j-p_i^w\left|\right|}^2---\left(2\right)$

Here first need, according to laser beam resolution, initial search point is set, as shown in Equation 3;

$({\&angle;P}_i^w-{\mathrm{\&psi;}}_0)\&times;[\mathrm{nrays}/(\mathrm{\&theta;}\max -\mathrm{\&theta;}\min )]---\left(3\right)$

Wherein ψ ₀for preliminary sweep laser beam angular, nrays is laser beam sum, and θ max-θ min is initial and stops laser beam angular difference, afterwards relatively before and after starting point 2 with

range difference, get distance less some place direction start search; If current point with the bee-line that obtains before having exceeded of distance, stop the search of this direction and turn to starting point other direction to continue search; After this,, in the time that the search of both direction all reaches end condition, obtain the shortest coupling

finally choose

in 2 of front and back, with

that conduct that distance is nearer

so just, can obtain effectively mating <i j ₁ ⁱ, j ₂ ⁱ> and matching distance thereof;

The 3rd step, cutting part coupling is right: first crop a bit in repeated matching reference diagram part coupling right, set up storage of array all with

relevant coupling is right, mates corresponding matching distance by traveling through these, takes out wherein minimum distance value, crops afterwards and is greater than this coupling apart from certain multiple to <i, j ₁ ⁱ, j ₂ ⁱ>, thereafter by remaining all couplings to by the sequence of matching distance length, then it is right to crop by preset ratio the coupling that its middle distance grows;

The 4th step, definition (4) is error function J,

$J(q_{k+1},C_k)=\underset{i}{\mathrm{\&Sigma;}}\left({n_i}^T\right[R\left({\mathrm{\&theta;}}_{k+1}\right)P_i+t_{k+1}-{P_{j_1}}^i\left]\right)---\left(4\right)$

Wherein n _ifor perpendicular to line segment

normal vector, C _kfor all effective matching relationships of current k step, this formula can be described as asks for transformation relation q _k+1=(t _k+1, θ _k+1), make not cropped all coupling centerings, point to the line segment mating with it

distance summation the shortest, this nonlinear equation of (4) formula is asked for to extreme value, calculate unmanned plane horizontal direction position and corner variation estimate q _k+1=(t _k+1, θ _k+1), if do not meet stopping criterion for iteration, this value is set to iterative initial value, to first step continuation interative computation, until meet default termination of iterations condition.

Four rotor wing unmanned aerial vehicle autonomic positioning method and the control methods based on laser radar as claimed in claim 1, it is characterized in that, changing figure strategy is specially: first equal angles is chosen 1/4 laser beam and sampled, near secondly selected hover point, 30 centimetres of positive and negative 30 cm x of horizontal direction and corner are no more than the region of 30 ° for not changing graph region, in this region, fixing the first secondary projected image that hover point collects is reference picture, and what all the other moment samplings obtained all compares with the first secondary projected image when front projection image; When aircraft flies out behind this region, need to replace the existing displacement of reference picture record-setting flight device, again centered by new reference picture collection point, generate and do not change graph region afterwards, proceed coupling location.

3. the autonomous location of four rotor wing unmanned aerial vehicles based on laser radar as claimed in claim 1 and control method, is characterized in that, utilizes 720MHz ARM process chip executive level direction localization method, and its output Data Update frequency is between 10-20Hz.

4. the autonomous location of four rotor wing unmanned aerial vehicles based on laser radar as claimed in claim 1 and control method, it is characterized in that, based on airborne barometer chip short transverse Primary Location be specially: adopt unmanned aerial vehicle onboard barometer chip to measure unmanned plane height, be specially: first effectively get rid of the impact that unmanned plane rotor turns round near air-flow barometer chip, the flight control system that is about to loading barometer chip is placed in the good plastic casing of closed performance, and utilize the materials such as sponge to block the gap of plastic casing and barometer chip chamber, thereby effectively get rid of the impact that unmanned plane rotor turns round near air-flow barometer chip, gather afterwards airborne MEMS barometer data measured, its renewal frequency is 10Hz.

5. the autonomous location of four rotor wing unmanned aerial vehicles based on laser radar as claimed in claim 1 and control method, it is characterized in that, no-manned plane three-dimensional location estimation based on complementary filter is specially: adopt a kind of three-dimensional complementary filtering method, the no-manned plane three-dimensional acceleration information that utilizes airborne accelerometer to obtain, to step 1, position data in 2 is carried out interpolation, thereby improve position data renewal frequency, be specially: unmanned plane acceleration transducer is upgraded to the measured value under the airborne coordinate system obtaining, be projected to world coordinate system by coordinate transform, and by twice integral and calculating, the 100Hz position quantity that obtains being estimated by accelerometer passes to on-board controller and carries out position control, in the horizontal direction, when laser radar matching process calculates when complete, calculate the difference of laser radar locator data and accelerometer estimated value, adjust integrated acceleration amount of bias by feedback system again, thereby realize the data fusion between these two kinds of foreign peoples's sensors of accelerometer and laser radar, complementary filter gain k ₁, k ₂, k ₃can be defined as follows by timeconstantτ:

$\begin{array}{c}{k}_1=\frac{3}{\mathrm{\&tau;}}\\ k_2=\frac{3}{{\mathrm{\&tau;}}^2}\\ k_3=\frac{1}{{\mathrm{\&tau;}}^3}\end{array}---\left(5\right)$

In like manner, in short transverse, also adopt complementary filter, realize accelerometer and barometrical data fusion.

Images