CN112506224A - Path planning method based on obstacle avoidance sensor - Google Patents

Abstract

The invention relates to a path planning method based on an obstacle avoidance sensor. The path planning algorithm of the space quintic PH curve based on the four-element control points can be used for re-planning a path through the position of an obstacle measured by the obstacle avoidance sensor. Has the advantages that: the safety of the rotor craft is improved; and re-planning in real time on line.

Description

Path planning method based on obstacle avoidance sensor

Technical Field

The invention belongs to the technical field of unmanned aerial vehicle application, relates to a path planning algorithm of an unmanned aerial vehicle, and particularly relates to a path planning method based on an obstacle avoidance sensor.

Background

With the development of science and technology, the unmanned aerial vehicle is well applied to many fields, and is applied to military fields such as battlefield situation investigation, target tracking, tiny target striking and the like. Unmanned aerial vehicles in the civil field have wide application in the fields of aerial photography, security, oil and gas pipeline inspection, bridge inspection, electric power option and the like. Because the unmanned aerial vehicle is widely applied, high requirements are placed on the safety reliability and the stability of the unmanned aerial vehicle, and a path planning algorithm based on an obstacle avoidance sensor and a strategy after obstacle avoidance are very important. Because unmanned aerial vehicle need carry out the operation flow under the operating mode condition of complicacy, consequently all can be equipped with various obstacle-avoiding sensors on the unmanned aerial vehicle to this makes unmanned aerial vehicle avoid the barrier, accomplishes the operation flow smoothly, keeps away the obstacle sensor at the wide application in unmanned aerial vehicle field at present and has:

(1) carrying out binocular vision obstacle avoidance;

(2) obstacle avoidance by the millimeter wave radar;

(3) ultrasonic obstacle avoidance;

(4) obstacle avoidance is carried out on the binocular vision and the TOF sensor;

(5) laser radar obstacle avoidance

Different obstacle avoidance strategies can be adopted according to different scenes, and different obstacle avoidance sensors have different advantages and disadvantages. And after the obstacle information is obtained by the obstacle avoidance sensor, an online real-time path can be planned according to a path planning algorithm, obstacles are avoided, and the smooth completion of the operation flow is ensured.

Disclosure of Invention

Technical problem to be solved

In order to avoid the defects of the prior art, the invention provides a path planning method based on an obstacle avoidance sensor, and provides a path planning algorithm of a space quintic PH curve based on four-element control points. The method can be used for replanning the path through the obstacle position measured by the obstacle avoidance sensor.

Technical scheme

A path planning method based on an obstacle avoidance sensor is characterized by comprising the following steps:

step 1: setting the maximum value of the unmanned aerial vehicle road track curvature, the maximum value of the track torque and the minimum curve bending energy

The maximum value of the unmanned aerial vehicle track curvature is set as:

the curvature of the flight track of the unmanned aerial vehicle meets the condition that | kappa | is less than or equal to kappa_max；

The maximum value of the track torque of the unmanned aerial vehicle is set as:

unmanned aerial vehicle flight track torque satisfies that | tau | < tau |_max；

Step 2: according to the current position and the final position, a spatial quintic PH curve, namely a flight path r (t) of the rotorcraft is calculated by adopting a four-element control point method:

wherein: p_iIs the initial rotorcraft position, P_fThe terminal position of the rotor craft, i, j, k is a space three-dimensional base coordinate;

wherein: (lambda_i,u_i,v_i) And (lambda)_f,u_f,v_f) Is the initial rotorcraft position P_iAnd P_fDifferential d of_iAnd d_fA direction cosine matrix of (a); phi is a₀,φ₂The angle of freedom is variable, an

In the above formula, c ═ c_x·i+c_y·j+c_z·k；

Is calculated to obtain wherein

And step 3: the obstacle avoidance sensor detects the position of an obstacle in front of the rotor wing aircraft, the position information of the obstacle is fed back to the rotor wing aircraft, and the curvature and the torque are adjusted according to the position information of the obstacle;

formula for calculating curvature:

wherein:

is the first order differential of the flight trajectory,

is the second differential of the trajectory; x represents cross multiplication operation, |, represents a modulus value;

according to the requirement of curvature:

|κ(t)|≤κ_max

calculation formula of torque:

wherein:

is the first order difference of the traces,

the second-order difference of the flight trajectory is represented by x which represents cross multiplication operation; the torque needs to meet the torque requirement:

|τ(t)|≤τ_max

track torque τ max τ_max；

And 4, step 4: and (3) according to the curvature and the torque adjusted in the step (3), recalculating the flight path by adopting a spatial quintic PH curve of the four-element control point:

advantageous effects

The invention provides a path planning method based on an obstacle avoidance sensor. The path planning algorithm of the space quintic PH curve based on the four-element control points can be used for re-planning a path through the position of an obstacle measured by the obstacle avoidance sensor.

The invention has the beneficial effects that:

(1) the safety of the rotor craft is improved;

(2) and re-planning in real time on line.

Drawings

FIG. 1: quintic PH curve path planning algorithm calculation flow

Detailed Description

The invention will now be further described with reference to the following examples and drawings:

step 1, setting the curvature, torque and minimum bending energy of the unmanned aerial vehicle path

Setting unmanned aerial vehicle track curvature kappa maximum value kappa_maxThe method comprises the following steps:

the curvature on the flight path of the drone must be such that

|κ|≤κ_max (2)

The unmanned aerial vehicle track can be calculated by a four-element expression of the track and the differentiation of the track.

Second, the maximum value tau of the track torque tau of the unmanned aerial vehicle is set_maxThe method comprises the following steps:

the flight track torque of the unmanned aerial vehicle should meet

|τ|≤τ_max (4)

Different rotorcraft may set different maximum values of curvature and torque according to the power standard. Adjustments can be made at retest times.

Step 2, calculating the flight path of the unmanned aerial vehicle according to the four-element control points

The unmanned aerial vehicle flight path expression based on four element control points is

Where t represents time, r (t) is the trajectory path of the rotorcraft, P₀～P₅Is 6 control points.

The calculation of the control point is shown in equation (6).

Wherein P is_iIs the initial rotorcraft position, P_fIs the rotor craft terminal position, i, j, k is the space three-dimensional base coordinate.

(λ_i,u_i,v_i) And (lambda)_f,u_f,v_f) Is the initial rotorcraft position P_iAnd P_fDifferential d of_iAnd d_fThe direction cosine matrix of (2). Phi is a₀,φ₂The variation of the free angle. And is

In the above formula, c ═ c_x·i+c_y·j+c_zK can pass through.

By A₀，A₁，A₂A control point P can be calculated₀，P₁，P₂，P₃，P₄，P₅。

In the actual flight trajectory of the unmanned aerial vehicle, expression c_x,c_y,c_zThe angle values in (1) are as follows:

step 3, adjusting the curvature and the torque of the path

After the obstacle avoidance sensor of the rotor aircraft detects the obstacle, the position information of the obstacle is received, and the curvature and the torque are readjusted by a flight control system on the rotor aircraft.

Calculating formula of real-time curvature of flight path:

is the first order difference of the traces,

the second order difference of the flight trajectory is represented by x, which represents a cross product operation.

According to the fact that the curvature requirement can be met:

|κ(t)|≤κ_max (12)

the formula for calculating the torque tau (t) of the flight path of the unmanned aerial vehicle is as follows:

the torque needs to meet the torque requirement

|τ(t)|≤τ_max (14)

Step 4, recalculating the spatial PH curve based on the four-element control points

And recalculating the flight path of the unmanned aerial vehicle according to the curvature and the torque of the adjusted path and the four-element control points. And (3) calculating the flight track of the unmanned aerial vehicle according to the four-element control points in step 2.

Claims (1)

1. A path planning method based on an obstacle avoidance sensor is characterized by comprising the following steps:

step 1: setting the maximum value of the unmanned aerial vehicle road track curvature, the maximum value of the track torque and the minimum curve bending energy

Maximum curvature of unmanned aerial vehicle trackThe values are set to:

the curvature of the flight track of the unmanned aerial vehicle meets the condition that | kappa | is less than or equal to kappa_max；

The maximum value of the track torque of the unmanned aerial vehicle is set as:

unmanned aerial vehicle flight track torque satisfies that | tau | < tau |_max；

Step 2: according to the current position and the final position, a spatial quintic PH curve, namely a flight path r (t) of the rotorcraft is calculated by adopting a four-element control point method:

wherein: p_iIs the initial rotorcraft position, P_fThe terminal position of the rotor craft, i, j, k is a space three-dimensional base coordinate;

wherein: (lambda_i,u_i,v_i) And (lambda)_f,u_f,v_f) Is the initial rotorcraft position P_iAnd P_fDifferential d of_iAnd d_fA direction cosine matrix of (a); phi is a₀,φ₂The angle of freedom is variable, an

In the above formula, c ═c_x·i+c_y·j+c_z·k；

Is calculated to obtain wherein

And step 3: the obstacle avoidance sensor detects the position of an obstacle in front of the rotor wing aircraft, the position information of the obstacle is fed back to the rotor wing aircraft, and the curvature and the torque are adjusted according to the position information of the obstacle;

formula for calculating curvature:

wherein:

is the first order differential of the flight trajectory,

is the second differential of the trajectory; x represents cross multiplication operation, |, represents a modulus value;

according to the requirement of curvature:

|κ(t)|≤κ_max

calculation formula of torque:

wherein:

is the first order difference of the traces,

the second-order difference of the flight trajectory is represented by x which represents cross multiplication operation;

the torque needs to meet the torque requirement:

|τ(t)|≤τ_max

track torque τ max τ_max；

And 4, step 4: and (3) according to the curvature and the torque adjusted in the step (3), recalculating the flight path by adopting a spatial quintic PH curve of the four-element control point:

Images