CN105759829A - Laser radar-based mini-sized unmanned plane control method and system - Google Patents

Abstract

The invention discloses a laser radar-based mini-sized unmanned plane control method comprising the following steps: real time laser range finding information is collected; according to the laser range finding information, an unmanned plane position and pose solution is sought, synchronous positioning and map building are conducted, and dynamic air route planning is conducted; after electric motor driving signals are generated, the flight of an unmanned plane can be controlled. A laser radar-based mini-sized unmanned plane control system is disclosed and comprises an unmanned plane flight control module, a laser range finding radar module and an electric motor. Low altitude complex environment unmanned plane autonomous navigation key technologies are applied to the field of equipment for civil use, and the reliability of low altitude complex environment navigation is improved while hardware cost is lowered. The laser radar-based mini-sized unmanned plane control method and system are widely applied to the technical field of unmanned planes.

Description

Laser radar-based micro unmanned aerial vehicle control method and system

Technical Field

The invention relates to the technical field of unmanned aerial vehicles, in particular to a method and a system for controlling a micro unmanned aerial vehicle based on a laser radar.

Background

IMU, inertial measurement unit.

In recent years, with the wide popularization of unmanned aerial vehicle products, the navigation technology of the unmanned aerial vehicle has great development, and with the progress of electronics and remote communication, a remote wireless navigation system is realized, such as technologies of phased array radar, GPS and the like, but most of the navigation technologies are only suitable for outdoor open fields and cannot be suitable for low-altitude complex environments, particularly for navigation of indoor unknown environments. Although similar navigation technologies in indoor or low-altitude complex environments exist in the industry at present, such as landmark identification and navigation technologies based on visual information, the navigation technologies have limited applicable environments, have high requirements on light of the environments, have complex visual algorithms and large hardware consumption, and have high requirements on software and hardware in real-time and accuracy; in the navigation technology based on the sonar sensor, because the measurement error is large, the acquired environmental information amount is small, and the navigation technology cannot be applied to a complicated indoor environment, and if a plurality of sonar sensor arrays are adopted to acquire more environmental information amounts, crosstalk may be generated when the indoor environment is crowded due to the sector area of the sonar sensor which emits at a scattering angle, so that distance measurement information is inaccurate; although the attitude estimation and motion recovery algorithm based on the optical flow sensor can be well applied to autonomous navigation in an unfamiliar environment, the visual information resolving speed and the optical flow reliability degree are still to be improved. Therefore, autonomous navigation in low-altitude complex environments has been a bottleneck in the wide-range application of micro unmanned aerial vehicles.

The existing low-altitude complex environment navigation technology in the industry at present has high requirements on the environment, low interference resistance and reliability, complex algorithm, huge hardware consumption and high requirements on software and hardware on instantaneity and accuracy. Therefore, improvements are needed.

Disclosure of Invention

In order to solve the technical problems, the invention aims to provide a laser radar-based micro unmanned aerial vehicle control method and a laser radar-based micro unmanned aerial vehicle control system.

The technical scheme adopted by the invention is as follows:

the invention provides a laser radar-based micro unmanned aerial vehicle control method, which comprises the following steps of:

collecting real-time laser ranging information;

and carrying out pose solution, synchronous positioning and map construction and dynamic route planning on the unmanned aerial vehicle according to the laser ranging information so as to generate a motor driving signal and further control the unmanned aerial vehicle to fly. As an improvement of the technical scheme, the unmanned aerial vehicle pose solution utilizes an artificial icon to perform pose solution according to the inertial measurement unit and the acquired information; calculating the added value of the posture by using an ICP (inductively coupled plasma) algorithm; respectively carrying out error analysis on the obtained pose information and the pose information obtained by the inertial measurement unit; and carrying out information fusion according to the error analysis result to obtain accurate pose information.

Further, the pose solution is solved by a singular value decomposition method.

Further, the synchronized positioning and mapping comprises: constructing a random target into a map, and predicting the track of a random target object; planning a path in a dynamic environment, and avoiding collision between the unmanned aerial vehicle and a random target object and arrival at a destination with the shortest time consumption; and constructing a map containing static feature points and dynamic random target tracks.

Further, by iteratively executing a kinematic prediction algorithm and a measurement update algorithm, the drone is synchronously positioned and an environmental feature map is created.

Further, the synchronized positioning and mapping comprises:

collecting data information, and filtering to remove noise;

the obtained information is subjected to region segmentation and feature extraction, and a map is represented by adopting straight line features.

Further, the dynamic route planning calculates the current position coordinate and the target position coordinate of the unmanned aerial vehicle, the distance to be flown delta x and the lateral deviation delta y of the unmanned aerial vehicle by using a pose solving and synchronous positioning and map building algorithm, and converts the current position coordinate and the target position coordinate into a pitch angle theta _ cmd and a roll angle theta gamma _ cmd of the attitude control loop.

Further, the solution formula of the distance to fly Δ x is as follows:

$\mathrm{\θ}\_cmd\left(t\right)=Kxp\×\mathrm{\Δ}x\left(t\right)+Kxt\×\∫\mathrm{\Δ}x\left(t\right)dt+KxD\×\frac{d\mathrm{\Δ}x\left(t\right)}{dt}$

in the formula, Kxp is a proportional coefficient of the control of the distance to be flown, Kxt is an integral coefficient of the control of the distance to be flown, and KxD is a differential coefficient of the control of the distance to be flown.

On the other hand, the invention also provides a laser radar-based micro unmanned aerial vehicle control system, which comprises:

unmanned aerial vehicle flies control module, laser rangefinder radar module and motor, wherein:

the laser ranging radar module is positioned at the top of the unmanned aerial vehicle and used for acquiring real-time laser ranging information;

laser rangefinder radar module and motor all fly with unmanned aerial vehicle and control the module and be connected, this unmanned aerial vehicle flies to control the module and is used for carrying out unmanned aerial vehicle position and appearance solution, synchronous positioning and map construction and dynamic course planning according to laser rangefinder information to generate motor drive signal, and then control the flight of unmanned aerial vehicle.

In another aspect, the present invention further provides a laser radar-based micro drone control system, including:

the first module is used for acquiring real-time laser ranging information;

and the second module is used for executing pose solving, synchronous positioning and mapping of the unmanned aerial vehicle and dynamic route planning according to the laser ranging information so as to generate a motor driving signal and further control the unmanned aerial vehicle to fly.

The invention has the beneficial effects that: the invention provides a laser radar-based micro unmanned aerial vehicle control method and a laser radar-based micro unmanned aerial vehicle control system, wherein a set of unmanned aerial vehicle flight control navigation system with obstacle avoidance and an autonomous planning path is formed by optimizing and flight control of a laser ranging radar through an algorithm, and the problem that an existing aircraft is unsafe to fly in a complex environment is solved. It can realize that miniature unmanned aerial vehicle keeps away the barrier and independently optimizes the flight path function, makes miniature unmanned aerial vehicle carry out safe flight under indoor or low latitude complex environment. Compared with the traditional detection navigation system, the micro unmanned aerial vehicle obstacle avoidance and autonomous navigation system based on the laser ranging radar has the following advantages: the hardware cost is low; the device is suitable for indoor or low-altitude complex environments and has all-weather capability; the electromagnetic interference capability is strong, and the influence of the ambient temperature and sunlight is not easy to occur; the invisible property is strong, and the invisible mask can penetrate a certain shelter, camouflage and shelter; has high distance, angle and speed resolution, and can simultaneously obtain multiple data of the target. The autonomous navigation key technology of the low-altitude complex environment unmanned aerial vehicle is pushed to the civil field, the hardware cost is reduced, and the reliability of low-altitude complex environment navigation is improved.

Drawings

The following further describes embodiments of the present invention with reference to the accompanying drawings:

FIG. 1 is a schematic diagram of a laser range radar-based drone control system according to an embodiment of the present invention;

FIG. 2 is a schematic diagram of the operation of a laser ranging radar according to an embodiment of the present invention;

FIG. 3 is a schematic diagram of a process flow in a synchronized positioning and mapping algorithm in accordance with an embodiment of the present invention;

FIG. 4 is a schematic diagram of a dynamic route planning algorithm in accordance with an embodiment of the present invention;

fig. 5 is a schematic diagram of pose loop control according to an embodiment of the present invention.

Detailed Description

It should be noted that the embodiments and features of the embodiments in the present application may be combined with each other without conflict.

Fig. 1 is a schematic diagram of a laser range radar-based drone control system according to an embodiment of the present invention. Unmanned aerial vehicle keeps away barrier and autonomous navigation comprises laser range radar module and flight control system, and laser range radar sends the distance data and the visual data of gathering to unmanned aerial vehicle flight control system in real time, and flight control system analyzes out range radar's data message through internal central processing unit, and multiple algorithm such as through position appearance solution, map construction and developments are kept away the barrier and are handled to fuse through special algorithm with the aircraft gesture, convert motor drive signal control unmanned aerial vehicle flight into, thereby realize keeping away barrier and autonomous path planning. The laser ranging radar module carries out 360-degree all-dimensional scanning ranging detection; the laser ranging radar module is positioned at the top of the unmanned aerial vehicle; laser rangefinder radar module and motor all are connected with unmanned aerial vehicle flight control module.

The invention provides a laser radar-based micro unmanned aerial vehicle control system, which comprises:

unmanned aerial vehicle flies control module, laser rangefinder radar module and motor, wherein:

the laser ranging radar module is positioned at the top of the unmanned aerial vehicle and used for acquiring real-time laser ranging information;

laser rangefinder radar module and motor all fly with unmanned aerial vehicle and control the module and be connected, this unmanned aerial vehicle flies to control the module and is used for carrying out unmanned aerial vehicle position and appearance solution, synchronous positioning and map construction and dynamic course planning according to laser rangefinder information to generate motor drive signal, and then control the flight of unmanned aerial vehicle.

Further, the laser ranging radar module comprises a vision acquisition system. Specifically, the laser ranging radar module performs 360-degree omni-directional scanning ranging detection.

On the other hand, the invention also provides a laser radar-based micro unmanned aerial vehicle control system, which comprises:

the first module is used for acquiring real-time laser ranging information;

and the second module is used for executing pose solving, synchronous positioning and mapping of the unmanned aerial vehicle and dynamic route planning according to the laser ranging information so as to generate a motor driving signal and further control the unmanned aerial vehicle to fly.

Fig. 2 is a schematic diagram of a laser ranging radar according to an embodiment of the present invention. The laser ranging radar adopts a laser triangular ranging technology and is matched with an internal integrated high-speed vision acquisition processing structure, ranging actions can be carried out for more than 2000 times per second, a modulated infrared laser signal is transmitted in the ranging process every time, and reflected light generated after the laser signal irradiates a target object is received by a vision acquisition system of the ranging radar. The distance value between the irradiated target object and the ranging radar and the current included angle information are output from the communication interface through real-time calculation by the embedded DSP. Simultaneously, the range finding core carries out clockwise rotation under motor structure's drive to the realization is to 360 degrees all-round scanning range finding detection of environment.

The bottleneck faced by the unmanned aerial vehicle obstacle avoidance and autonomous navigation technology is mainly the navigation technology of indoor unknown environment. The indoor environment has unknown and complex properties, namely, the unknown property is represented by the fact that the unmanned aerial vehicle has no knowledge of the indoor environment, the size of the indoor environment, the shape and the distribution of obstacles and no reference object which is manually set. The complexity is expressed in the environment that the unmanned aerial vehicle is in a lot of uncertainties and randomness, such as random arrangement or mutual shielding of obstacles, change of indoor illumination due to shooting at different angles of the unmanned aerial vehicle, and the like. At present, a series of researches are also carried out on the navigation control of the indoor unknown environment, but a unified and complete system is not formed, and a plurality of key theories and technologies are not solved and completed. The problems mainly relate to the technical problems of modeling of an indoor environment, positioning of the unmanned aerial vehicle, adjustment of a navigation controller of the unmanned aerial vehicle, real-time motion control, a navigation control method and the like. The laser ranging radar module adopted by the invention has the characteristics of high measuring speed, high measuring precision and insensitivity to noise and illumination intensity in the environment, meets the real-time performance and precision of the navigation requirement of the unmanned aerial vehicle, is matched and fused with a flight control system, and breaks through the key technology for realizing onboard obstacle avoidance of the micro unmanned aerial vehicle.

Unmanned aerial vehicle developments keep away barrier control technique: in the process of executing tasks, the unmanned aerial vehicle senses the environment and detects and avoids dangers, and the necessary conditions for ensuring the safety of the unmanned aerial vehicle are met, so that the unmanned aerial vehicle needs to detect encountered dynamic and static target objects in the whole flight process and plan a reasonable air route to avoid obstacles.

The real-time dynamic route planning technology of the unmanned aerial vehicle comprises the following steps: in order to avoid collision of the unmanned aerial vehicle with a random target and shortest time for reaching a direction point, path planning needs to be carried out in a dynamic environment.

A micro unmanned aerial vehicle control method based on laser radar comprises the following steps:

collecting real-time laser ranging information;

and carrying out pose solution, synchronous positioning and map construction and dynamic route planning on the unmanned aerial vehicle according to the laser ranging information so as to generate a motor driving signal and further control the unmanned aerial vehicle to fly.

Specifically, the collecting real-time laser ranging information includes:

emitting a laser signal to a target object;

receiving light information data reflected by the target object;

and resolving the data and outputting.

As an improvement of the technical scheme, the unmanned aerial vehicle pose solution utilizes an artificial icon to perform pose solution according to the inertial measurement unit and the acquired information;

calculating the added value of the posture by using an ICP (inductively coupled plasma) algorithm;

respectively carrying out error analysis on the obtained pose information and the pose information obtained by the inertial measurement unit;

and carrying out information fusion according to the error analysis result to obtain accurate pose information.

As an improvement of the technical scheme, the pose resolving adopts a singular value decomposition method to solve.

As an improvement of the technical solution, the synchronized positioning and mapping includes:

constructing a random target into a map, and predicting the track of a random target object;

planning a path in a dynamic environment, and avoiding collision between the unmanned aerial vehicle and a random target object and arrival at a destination with the shortest time consumption;

and constructing a map containing static feature points and dynamic random target tracks.

Further, by iteratively executing a kinematic prediction algorithm and a measurement update algorithm, the drone is synchronously positioned and an environmental feature map is created.

Further, the synchronized positioning and mapping comprises:

collecting data information, and filtering to remove noise;

the obtained information is subjected to region segmentation and feature extraction, and a map is represented by adopting straight line features.

Further, the dynamic route planning calculates the current position coordinate and the target position coordinate of the unmanned aerial vehicle, the distance to be flown delta x and the lateral deviation delta y of the unmanned aerial vehicle by utilizing the pose solving and synchronous positioning and map building algorithm, and converts the current position coordinate and the target position coordinate into the pitch angle theta _ cmd and the roll angle theta gamma _ cmd of the attitude control loop.

Further, the solution formula of the distance to fly Δ x is as follows:

$\mathrm{\θ}\_cmd\left(t\right)=Kxp\×\mathrm{\Δ}x\left(t\right)+Kxt\×\∫\mathrm{\Δ}x\left(t\right)dt+KxD\×\frac{d\mathrm{\Δ}x\left(t\right)}{dt}$

in the formula, Kxp is a proportional coefficient of the control of the distance to be flown, Kxt is an integral coefficient of the control of the distance to be flown, and KxD is a differential coefficient of the control of the distance to be flown.

The unmanned aerial vehicle flight control system is combined with the laser ranging radar module with small size, light weight and good performance, and meanwhile, a pose solving algorithm, a map building algorithm and a dynamic obstacle avoidance scheme are optimized on software, so that reliable obstacle avoidance and autonomous navigation functions are realized.

1. Pose solving algorithm of micro unmanned aerial vehicle

Pose calculation of the unmanned aerial vehicle is very important and necessary for navigation, positioning and other applications of the unmanned aerial vehicle. The relatively accurate attitude information of the unmanned aerial vehicle is a premise that each application which utilizes the pose of the unmanned aerial vehicle to calculate can obtain a better result. Because the micro unmanned aerial vehicle cannot carry an inertia measurement unit for measuring a more accurate attitude angle due to the problems of effective load, endurance and the like, the invention utilizes IMU with lower accuracy and information obtained by other ways, such as image matching, artificial icons and the like, to carry out information fusion so as to obtain a more accurate pose estimation result. According to the IMU and the visual information, the process of estimating the pose of the micro unmanned aerial vehicle comprises the following aspects:

(1) resolving the pose by using the artificial icon;

(2) calculating the added value of the attitude by using an ICP (iterative closestPoint) algorithm;

(3) respectively carrying out error analysis on the obtained pose information and the pose information obtained by the IMU by utilizing a rear intersection principle;

(4) and according to the error analysis result, carrying out information fusion to obtain more accurate pose information.

The pose solving algorithm is concretely implemented as follows:

the invention adopts a Singular Value Decomposition (SVD) method, the SVD method can accurately calculate the pose, and the essence is the coordinates of the centroid of the calculation point set in two coordinate systems. Assuming that they have a coordinate transformation relationship as represented by the following formula (1), minimum deviation fitting of all points in two coordinate systems is achieved.

The calculation idea of the SVD method is as follows:

(1) respectively calculating the coordinate P of the centroid in the global coordinate system and the local coordinate system^’And J^’：

(2) Computing a covariance matrixWherein Q_i＝P_i-P’,Q_i’＝J_i-J’；

(3) Performing singular value decomposition (H-UDV) on the covariance matrix H^TWhere D is a diagonal matrix, V and U are orthogonal matrices, V^TIs the transpose of matrix V;

(4) calculating matrix R ═ VU^TAnd solving the value | R | of the determinant;

if R is l, the matrix is a pose matrix to be solved;

if R | ═ l, let V' ═ V1V2-V3]The pose matrix is R ═ V' U^TWherein

V1, V2, -V3 are respectively the l, 2 and 3 columns of the matrix V;

(5) after the attitude matrix foot is solved, the value of the attitude of the unmanned aerial vehicle can be obtained,

wherein R is_ijThe position vector T, which represents the ith row and jth column elements of the matrix R, can be obtained from the formula P RJ + T, thereby obtaining the values x, y, z.

2. Synchronous positioning and mapping

The SLAM (Simultaneous localization and mapping) algorithm, namely a synchronous positioning and map construction algorithm, is successfully applied to navigation of ground and underwater robots, is gradually expanded to the application of unmanned aerial vehicles, and has an extremely important significance for guiding unmanned aerial vehicles to autonomously explore unknown environments.

The SLAM algorithm firstly extracts scene features as landmarks, calculates the positions of the landmarks relative to a carrier, and finally obtains and records the positions of the landmarks in a map by combining the position of the carrier. After the indoor unmanned aerial vehicle enters the area again, the global position information of the unmanned aerial vehicle can be judged by matching the recorded landmark templates. The SLAM algorithm focuses on landmark representation, data association, and accumulated errors caused by observation errors, pose solution errors, and erroneous data association. The accuracy and robustness of map establishment can be effectively improved by adopting modes such as extended Kalman filtering and the like. Related research has already begun and achieved great success in the field of ground robots. However, the indoor unmanned aerial vehicle has more degrees of freedom and smaller load, and brings new challenges to the description method of the landmarks in the SLAM algorithm. The invention mainly comprises the following parts:

1. constructing a random target into a map, and predicting the track of the random target;

2. in order to avoid the collision between the robot and a random target and to reach a direction point in a shortest time-consuming manner, path planning needs to be carried out in a dynamic environment;

3. and constructing a map containing static feature points and dynamic random target tracks.

The geometric feature map is a compact map representation method which is firstly drawn by acquiring data by Lu and Milios through a laser range finder and extracting linear features. Most environments, especially indoor environments, can accurately reflect environment information by using a map described by geometric characteristics such as line segments, circles, arcs and the like. The geometric feature map has small information storage amount, is convenient for pose estimation and target identification, and is widely applied to navigation and path planning of robots.

Fig. 3 is a schematic diagram of a process for creating a synchronous positioning and mapping algorithm according to an embodiment of the present invention. The creation of a local map based on a laser range finder is described by using a geometric feature map, and the creation flow is shown in fig. 3. The data collected by the laser range finder are point sets without direct maps, and the data are subjected to filtering processing to remove noise interference, then are subjected to region segmentation and feature extraction, and finally are expressed by adopting straight line features.

The implementation method of the synchronous positioning and map building algorithm comprises the following steps:

the SLAM algorithm adopts a feature map to express an environment, and under a global reference coordinate system, the pose of the unmanned aerial vehicle at the k moment is expressed as the following vector:

X_r，k＝[x_r，ky_r，kθ_r，k]^T∈R³

each feature f_iIs expressed as a vector

$X_{f_i}={\left[\begin{array}{cccc}{p}_{f_i}^1& p_{f_i}^2& \mathrm{...}& p_{f_i}^m\end{array}\right]}^T\∈R^m$

The unmanned aerial vehicle position and orientation vector and the parameter vector of n characteristics constitute a joint state vector, as follows:

$X_k=\[x_{r,k}^T{x}_{f1}^T{x}_{f2}^T\mathrm{...}{x}_{fn}^T\]\∈R^{mn+3}$

X_kand representing the pose information and the environment map information of the unmanned aerial vehicle in the global reference coordinate system. In SLAM, State X_kBy a mean value ofVariance is P_kkIs described by the gaussian distribution of (a). Using the horizontal and vertical coordinates to describe its position, X_fi＝[x_fiy_fi]^T∈R². k +1 time unmanned aerial vehicle slave X_r,kMove to X_r,k+1. Sensor data obtained by the drone between the time k and the time k +1, state X_k+1The estimate of (d) can be obtained by the following two steps:

a kinematic prediction

The kinematic equation of the system between discrete time k and k +1 is:

X_k+1＝f(X_k，u_k，ω_k)

wherein u is_kCorresponding control input for the system; omega_kTo describe u_kAssuming that it obeys a mean of zero and a variance of Q_kGaussian distribution ofI.e. by

ω_k～N(0，Q_k)

Passing the system state X at time k according to the following two equations_kFor state X at time k +1_k+1Performing kinematic prediction:

${\hat{X}}_{k+1k}=f({\hat{X}}_k,u_k,0)$

P_k+1k＝FP_kkF^T+GQ_kG^T

wherein,

respectively, a kinematic model versus a state variable X_kAnd control input u_kA jacobian matrix.

b measurement update

The observation of the unmanned aerial vehicle on the environment is the relative measurement between the position of the unmanned aerial vehicle and the characteristic parameters, namely the observation is the state X_kFunction of (c):

z_k＝g(X_k，v_k)

wherein v is_kFor the sensor to observe noise, assume that it obeys a mean of zero variance of R_kI.e.:

v_k～N(0，R_k)

k +1 moment, the unmanned plane is at position X_r,k+1Obtaining an observation z of an environment_k+1. Simultaneous utilization of state estimationObtaining a predicted value about the feature:

${\hat{z}}_{k+1}=g({\hat{X}}_{k+1k},0)$

to state X according to deviation of observed value and predicted value_k+1And (3) updating measurement:

$v_{k+1}=z_{k+1}-{\hat{z}}_{k+1}$

${\hat{X}}_{k+1}={\hat{X}}_{k+1k}+W_{k+1}{v}_{k+1}$

P_k+1K+1＝P_k+1k-W_k+1S_k+1W_k+1 ^T

W_k+1＝P_k+1kH^TS^-1 _k+1

S_k+1＝HP_k+1kH^T+KR_k+1K^T

wherein,

which are respectively the observation model to the state variable X_k+1And noise v_k+1A jacobian matrix.

By iteratively performing the above kinematic predictions and measurement updates, the algorithm synchronously locates the drones and creates an environmental feature map.

3. Dynamic obstacle avoidance algorithm of unmanned aerial vehicle

The unmanned aerial vehicle is a necessary condition for ensuring the safety of the unmanned aerial vehicle by sensing the environment and detecting and avoiding dangers in the process of executing tasks. The unmanned aerial vehicle needs to detect encountered dynamic and static target objects and plan a reasonable air route to avoid obstacles in the whole flight process. Meanwhile, the unmanned aerial vehicle is likely to encounter emergency at any time in the flying process, so that the unmanned aerial vehicle emergency landing technology is necessary to be explored. According to multiple information such as laser rangefinder, vision, to indoor unmanned aerial vehicle perception and avoid and develop the research to moving target, emergent landing area detect and plan the airline and be the key point, mainly include 3 parts:

1. an aerial threat moving target detection and three-dimensional moving trend analysis technology;

2. unmanned aerial vehicle emergency landing detection technology;

3. an unmanned aerial vehicle real-time dynamic route planning technology.

The specific implementation method comprises the following steps:

and the dynamic route planning control loop adopts PID control to convert the deviation of the position into a corresponding attitude angle. And if the forward flight is needed, converting the forward distance into a corresponding pitch angle.

Referring to FIG. 4, a schematic diagram of a dynamic route planning algorithm according to an embodiment of the invention is shown. The coordinate system of the unmanned aerial vehicle body is a right-hand system, the positive direction of an OX shaft of the body coordinate system is the direction pointed by the head, namely the advancing direction of the unmanned aerial vehicle, and the corresponding coordinate is called as the distance to be flown; the OZ axis of the machine body coordinate system faces downwards in the forward direction; according to a Cartesian coordinate system, the positive direction of an OY axis of a body coordinate system is perpendicular to the advancing direction of the unmanned aerial vehicle and points to the right, and the corresponding coordinate is called lateral offset.

Fig. 5 is a schematic diagram of pose loop control according to an embodiment of the present invention. The dynamic route planning needs to calculate the current position coordinate of the unmanned aerial vehicle through a pose solving and map building algorithm, the difference value of the target position and the current position is used as the distance to be flown delta x and the lateral offset delta y of the unmanned aerial vehicle, and the difference value is converted into the pitch angle theta _ cmd and the roll angle theta gamma _ cmd of an attitude control loop through a special algorithm. The control method of the to-be-flown distance control loop is the same as that of the sideslip control loop, and the to-be-flown distance control loop is taken as an example for description, and the control rule is as follows:

$\mathrm{\θ}\_cmd\left(t\right)=Kxp\×\mathrm{\Δ}x\left(t\right)+Kxt\×\∫\mathrm{\Δ}x\left(t\right)dt+KxD\×\frac{d\mathrm{\Δ}x\left(t\right)}{dt}$

in the formula, Kxp is a proportional coefficient of the control of the distance to be flown, Kxt is an integral coefficient of the control of the distance to be flown, and KxD is a differential coefficient of the control of the distance to be flown. The differential term adopts a differential method to calculate the moving speed of the unmanned aerial vehicle.

The invention is applied to a micro unmanned aerial vehicle product, and the composition of the product comprises the following components: laser rangefinder radar module and unmanned aerial vehicle flight control system. Through the system, the unmanned aerial vehicle can timely avoid obstacles in an indoor or low-altitude complex environment so as to prevent mistaken collision, and simultaneously, the optimal route is autonomously selected for safe flight, so that the aims of avoiding obstacles and autonomously navigating are fulfilled.

The realization method comprises the following steps: a laser ranging radar module is installed at the top of an unmanned aerial vehicle body, collected environment data are transmitted to a central processing unit inside the unmanned aerial vehicle in real time, the environment data are subjected to algorithm processing such as special pose solving, map building and dynamic obstacle avoidance, the unmanned aerial vehicle is controlled together after being fused with attitude data of a flight control system, and therefore the functions of obstacle avoidance and autonomous optimization of flight paths are achieved.

According to the invention, the laser ranging radar module is arranged at the top of the aircraft, the acquired information is transmitted to the processor in the aircraft, and the aircraft is controlled together with the flight control system after fusion operation of a plurality of algorithms such as pose control, map construction and dynamic obstacle avoidance, so that obstacle avoidance and autonomous optimization of flight paths are realized. The low-altitude complex environment micro unmanned aerial vehicle airborne autonomous navigation system based on the laser range radar can well reflect the barrier information in the indoor and low-altitude complex environment by means of more data points with reliable measurement precision and measurement distance information, and well makes up the bottleneck of the prior art.

While the preferred embodiments of the present invention have been illustrated and described, it will be understood by those skilled in the art that various changes in form and details may be made therein without departing from the spirit and scope of the invention as defined by the appended claims.

Claims (10)

1. A micro unmanned aerial vehicle control method based on laser radar is characterized by comprising the following steps:

collecting real-time laser ranging information;

and carrying out pose solution, synchronous positioning and map construction and dynamic route planning on the unmanned aerial vehicle according to the laser ranging information so as to generate a motor driving signal and further control the unmanned aerial vehicle to fly.

2. The method of claim 1, wherein the method comprises:

the unmanned aerial vehicle pose solution utilizes the artificial icons to carry out pose solution according to the inertial measurement unit and the acquired information;

calculating the added value of the posture by using an ICP (inductively coupled plasma) algorithm;

respectively carrying out error analysis on the obtained pose information and the pose information obtained by the inertial measurement unit;

and carrying out information fusion according to the error analysis result to obtain accurate pose information.

3. The method of claim 2, wherein the pose solution is solved using singular value decomposition.

4. The method of any of claims 1 to 3, wherein the synchronized positioning and mapping comprises:

constructing a random target into a map, and predicting the track of a random target object;

planning a path in a dynamic environment, and avoiding collision between the unmanned aerial vehicle and a random target object and arrival at a destination with the shortest time consumption;

and constructing a map containing static feature points and dynamic random target tracks.

5. The method of claim 4, wherein the drone is positioned and the map of the environmental characteristics is created synchronously by iteratively executing a kinematic prediction algorithm and a measurement update algorithm.

6. The lidar based drone maneuver method of claim 5, wherein the synchronized positioning and mapping comprises:

collecting data information, and filtering to remove noise;

the obtained information is subjected to region segmentation and feature extraction, and a map is represented by adopting straight line features.

7. The method as claimed in claim 6, wherein the dynamic route planning uses pose solving and synchronous positioning and mapping algorithms to calculate the current position coordinates and target position coordinates of the unmanned aerial vehicle, the distance to fly Δ x and the lateral offset Δ y of the unmanned aerial vehicle, and converts the current position coordinates and target position coordinates and the distance to fly Δ x and lateral offset Δ y into a pitch angle θ _ cmd and a roll angle θ γ _ cmd of the attitude control loop.

8. The method of claim 7, wherein the solution formula of the distance to fly Δ x is as follows:

in the formula, Kxp is a proportional coefficient of the control of the distance to be flown, Kxt is an integral coefficient of the control of the distance to be flown, and KxD is a differential coefficient of the control of the distance to be flown.

9. The utility model provides a miniature unmanned aerial vehicle control system based on laser radar which characterized in that includes:

unmanned aerial vehicle flies control module, laser rangefinder radar module and motor, wherein:

the laser ranging radar module is positioned at the top of the unmanned aerial vehicle and used for acquiring real-time laser ranging information;

laser rangefinder radar module and motor all fly with unmanned aerial vehicle and control the module and be connected, this unmanned aerial vehicle flies to control the module and is used for carrying out unmanned aerial vehicle position and appearance solution, synchronous positioning and map construction and dynamic course planning according to laser rangefinder information to generate motor drive signal, and then control the flight of unmanned aerial vehicle.

10. The utility model provides a miniature unmanned aerial vehicle control system based on laser radar which characterized in that includes:

the first module is used for acquiring real-time laser ranging information;

and the second module is used for executing pose solving, synchronous positioning and mapping of the unmanned aerial vehicle and dynamic route planning according to the laser ranging information so as to generate a motor driving signal and further control the unmanned aerial vehicle to fly.