CN116360420A - CLF-CBF optimized unmanned vehicle motion planning control method - Google Patents

Abstract

The invention discloses a CLF-CBF optimized unmanned vehicle motion planning control method, which comprises the specific steps of establishing a two-wheel differential chassis model of an unmanned vehicle; establishing a differential motion model of two wheels of the unmanned vehicle; designing a Liapunov function to improve the tracking stability of a vehicle control path; providing input constraint and safety key constraint for a control system by controlling an obstacle function, so that the safety of the system is improved; establishing a CBF-NMPC model predictive control system; adding CLF constraint, and establishing a CBF-CLF-NMPC model predictive control system. The method can solve the problem that the path planning result effect of various planning algorithms is poor due to the characteristics of the various planning algorithms, fully improves the control performance of the vehicle on the premise that the safety of the unmanned vehicle is fully ensured, and realizes the optimal path planning and efficient control of the unmanned vehicle.

Description

CLF-CBF optimized unmanned vehicle motion planning control method

Technical Field

The invention relates to the technical field of unmanned vehicle motion control, in particular to a CLF-CBF optimized unmanned vehicle motion planning control method.

Background

Motion planning control is one of the key points in the field of unmanned vehicle research. The efficient motion planning control algorithm can enable the unmanned vehicle to reach the target point with the optimal performance at the minimum cost on the premise of no collision.

At present, a motion planning control algorithm of an unmanned vehicle cannot be well matched with a nonlinear movement state of the unmanned vehicle, the algorithm is easy to generate larger deviation on the prediction of a movement path, the prediction process is demanding on path points, and all the kinematic information of the unmanned vehicle on each point in the path is always considered, so that the robustness of the algorithm is poor. In addition, most path planning algorithms based on grid maps fail to consider the kinematic model of the unmanned vehicle, and the resulting planned path is not smooth.

In the aspect of optimal control, although most control methods take many consideration of the safety and comfort of the unmanned vehicle in various dimensions, there are few control methods which take potential conflicts in the running process of the vehicle into consideration, so that the unmanned vehicle cannot obtain better performance on the premise of system safety.

Therefore, a motion planning control method with good path planning effect, strong robustness and high safety is urgently needed. And controlling the unmanned vehicle to plan an optimal path to avoid potential risks, and realizing optimal path planning and efficient stable control of the unmanned vehicle.

Disclosure of Invention

Aiming at the technical defects, the invention aims to provide the CLF-CBF optimized unmanned vehicle motion planning control method, which can be used for introducing CLF (Control Lyapunov Function, control Lithospermon function) and CBF (Control Barrier Function, control obstacle function) to optimize NMPC (Nonlinear Model Predictive Control ) according to the problem that the conventional common path planning algorithm has poor path planning effect due to self characteristic limitation, and the obtained path planning algorithm predicts smooth path, has strong robustness, can fully promote the unmanned vehicle control performance, strengthens the avoidance capability of the unmanned vehicle to static obstacles and moving obstacles in a complex environment, and realizes optimal path planning and efficient stable control of the unmanned vehicle. .

In order to solve the technical problems, the invention adopts the following technical scheme:

the invention provides a CLF-CBF optimized unmanned vehicle motion planning control method, which comprises the following steps:

step 1, establishing a two-wheel differential chassis model of an unmanned vehicle;

step 2, establishing a differential motion model of two wheels of the unmanned vehicle;

step 3, designing a Liapunov function to improve the tracking stability of a vehicle control path;

step 4, providing input constraint and safety key constraint for a control system through controlling an obstacle function, and improving the safety of the system;

step 5, establishing a CBF-NMPC model predictive control system;

and 6, adding CLF constraint, and establishing a CBF-CLF-NMPC model predictive control system.

Preferably, in step 1, the two-wheel differential chassis model comprises a chassis, two driving wheels and four driven wheels, wherein the two driving wheels are positioned at the left side and the right side of the middle part of the chassis and are respectively driven by two independent motors; if the rotation speeds of the two motors are the same, the unmanned vehicle moves along a straight line, and if the rotation speeds of the two motors are different, the unmanned vehicle moves along a circle, and four driven wheels are respectively distributed at four corner points of the chassis and used for maintaining balance of the unmanned vehicle.

Preferably, in step 2, let l be the track width of the two driving wheels, v _l And v _r The speed of the two driving wheels is that r is the turning radius;

the linear velocity formula of the unmanned vehicle is:

because the radian of the rotation of the unmanned vehicle in unit time is very small, the approximate formula is as follows:

wherein is theta ₃ The rotating angle, i is the wheelbase of the vehicle;

calculating the angular velocity w as the rotation angle in unit time:

the turning radius is the ratio of the linear velocity v to the angular velocity w:

the speeds of the left driving wheel and the right driving wheel are calculated by the formula (1) and the formula (3):

preferably, in step 3, forIn a continuous autonomous system

Planning a starting point x ₀ And end point x _e Describing the lispro function as a continuously-derivable function V (x), satisfying the following two conditions:

V(x _e )＝0，V(x)>0，x≠x _e (7)

for a control affine system f (x) +g (x) u, the control lispro function is expressed as a continuously-derivable function V (x), with a constant C such that:

Ω _c ＝{x∈R ⁿ :v(x)≤C} (9)

V(x)>0,x∈R ⁿ \{x _e } (10)

wherein:

at this time, a control system with stable state can be obtained.

Preferably, in step 4, the system is considered

Wherein->

To obtain f (x) R ⁿ →R ⁿ Let it be assumed that set C is the upper level set of one smooth function h (x), i.e. the following condition is satisfied:

C＝{x∈Rn|h(x)≥0} (13)

Int(C)＝{x∈Rn|h(x)>0} (15)

and for all points on the boundary,

then if and only if->

When set C is a forward invariant set; the control barrier function is described as a continuously-derivable function B (x), with a constant B; such that:

C＝{x∈R ⁿ |B(x)≥0} (16)

sup[L _f B(x)+L _g B(x)u]+bB(x)≥0 (18)

wherein the method comprises the steps of

Is a shorthand; at this point the CBF-CLF constraint of the system is available.

Preferably, in step 5, the CBF constrained by the relaxation decay technique is added to the nonlinear model predictive control; wherein the decision variables are:

U＝[u ^T _t|t ,...,u ^T _t|t+N-1 ] ^T

CBF-NMPC can be expressed as the following formula:

x _t+k+1|t ＝f(x _t+k|t ,u _t+k|t ),k＝0,...,N-1 (20)

u _t+k|t ∈U,x _t+k|t ∈X,k＝0,...,N-1 (21)

x _t|t ＝x _t (22)

h(x _t+k+1|t )≥w _k (1-γ _k )h(x _t+k|t )，w _k ≥0，k＝0,...,M _CBF -1 (23)

equation (20) is the system dynamics, and input constraints (21) and initial conditions (22) are used for optimization; controlling the use of the Liapunov function v as a terminal cost and edge by beta amplification

Accumulating cost, if beta approaches infinity, no terminal cost constraint is required to be specified; the attenuation rate in CBF is controlled from a fixed value of 1-gamma _k Relaxation optimization to w _k (1-γ _k ) And the relaxation rate variable ψ (w _k ) Is added to the cost of (a); the relaxation variable is constrained by equation (23); at this time, a CBF-NMPC model predictive control system is obtained.

Preferably, in step 6, the stability criterion of the CLF is taken as constraint instead of the terminal cost, resulting in the following formula:

x _t+k+1|t ＝f(x _t+k|t ,u _t+k|t ),k＝0,...,N-1 (25)

u _t+k|t ∈U,x _t+k|t ∈X,k＝0,...,N-1 (26)

x _t|t ＝x _t (27)

h(x _t+k+1|t )≥w _k (1-γ _k )h(x _t+k|t )，w _k ≥0，k＝0,...,M _CBF -1 (28)

V(x _t+k+1|t )≤(1-α _k )V(x _t+k|t )+S _k ，k＝0,...,M _CLF -1 (29)

wherein M is _CLF And M _CBF Is the range of CLF and CBF constraint, is less than the predicted visual field length N of NMPC, and introduces a relaxation variable

To avoid infeasibility between CLF and CBF constraints, and to add an additional term Φ (s _k ) To minimize these relaxation variables; due to the introduction of the relaxation optimization variable w _k The computational complexity is increased to some extent, but reducing the constraint range can reduce the computational complexity, so that the CBF-CLF-NMPC motion planning control method is obtained.

The invention has the beneficial effects that: according to the invention, aiming at the problem of poor path planning effect caused by the limitation of the characteristics of the conventional common path planning algorithm, CLF (Control Lyapunov Function, controlled Lithoff function) and CBF (Control Barrier Function, controlled obstacle function) are introduced to optimize NMPC (Nonlinear Model Predictive Control ), the obtained path planning algorithm predicts a smooth path, has strong robustness, can fully improve the control performance of the unmanned vehicle, enhances the avoidance capability of the unmanned vehicle to static obstacles and moving obstacles in a complex environment, and realizes optimal path planning and efficient stable control of the unmanned vehicle.

Drawings

In order to more clearly illustrate the embodiments of the invention or the technical solutions in the prior art, the drawings that are required in the embodiments or the description of the prior art will be briefly described, it being obvious that the drawings in the following description are only some embodiments of the invention, and that other drawings may be obtained according to these drawings without inventive effort for a person skilled in the art.

FIG. 1 is a schematic view of a two-wheeled differential chassis model of an unmanned vehicle provided by an embodiment of the present invention;

FIG. 2 is a schematic diagram of a two-wheel differential motion model of an unmanned vehicle provided by an embodiment of the invention;

FIG. 3 is a schematic diagram of a path of a start point x0 and an end point xe provided by an embodiment of the present invention;

FIG. 4 is a schematic diagram of a smoothing function h (x) provided by an embodiment of the present invention;

fig. 5 is a NMPC algorithm simulation test chart provided by an embodiment of the present invention;

FIG. 6 is a graph of unmanned vehicle speed indicators provided by an embodiment of the present invention;

FIG. 7 is a simulated deployment diagram of a CBF-CLF-NMPC algorithm provided by an embodiment of the invention;

FIG. 8 is a chart of performance tests of a CBF-CLF-NMPC algorithm provided by an embodiment of the invention;

fig. 9 is a graph of linear velocity versus angular velocity of an unmanned vehicle provided by an embodiment of the present invention.

Detailed Description

The following description of the embodiments of the present invention will be made clearly and completely with reference to the accompanying drawings, in which it is apparent that the embodiments described are only some embodiments of the present invention, but not all embodiments. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.

As shown in fig. 1 to 4, a CLF-CBF optimized unmanned vehicle motion planning control method includes the steps of:

step 1, establishing a two-wheel differential chassis model of an unmanned vehicle;

the two-wheel differential chassis model comprises a chassis, two driving wheels and four driven wheels, wherein the two driving wheels are positioned at the left side and the right side of the middle part of the chassis and are respectively driven by two independent motors; if the rotation speeds of the two motors are the same, the unmanned vehicle moves along a straight line, and if the rotation speeds of the two motors are different, the unmanned vehicle moves along a circle, and four driven wheels are respectively distributed at four corner points of the chassis and used for maintaining balance of the unmanned vehicle.

Step 2, establishing a differential motion model of two wheels of the unmanned vehicle;

let l be the track width of the two driving wheels, v _l And v _r For the speed of two driving wheelsR is the turning radius;

the linear velocity formula of the unmanned vehicle is:

because the radian of the rotation of the unmanned vehicle in unit time is very small, the approximate formula is as follows:

wherein is theta ₃ The rotating angle, i is the wheelbase of the vehicle;

calculating the angular velocity as the rotation angle in unit time:

the turning radius is the ratio of the linear velocity to the angular velocity:

the speeds of the left driving wheel and the right driving wheel are calculated by the formula (1) and the formula (3):

step 3, designing a Liapunov function to improve the tracking stability of a vehicle control path;

for continuous autonomous systems

Planning a starting point x ₀ And end point x _e Describing the lispro function as a continuously-derivable function V (x), satisfying the following two conditions:

for a control affine system f (x) +g (x) u, the control lispro function is expressed as a continuously-derivable function V (x), with a constant C such that:

Ω _c ＝{x∈R ⁿ :v(x)≤C} (9)

V(x)>0,x∈R ⁿ \{x _e } (10)

wherein:

at this time, a control system with stable state can be obtained.

Step 4, providing input constraint and safety key constraint for a control system through controlling an obstacle function, and improving the safety of the system;

consider a system

Wherein->

To obtain f (x) R ⁿ →R ⁿ Let it be assumed that set C is the upper level set of one smooth function h (x), i.e. the following condition is satisfied:

C＝{x∈Rn|h(x)≥0} (13)

Int(C)＝{x∈Rn|h(x)>0} (15)

and for all points on the boundary,

then if and only if->

When set C is a forward invariant set; the control barrier function is described as a continuously-derivable function B (x), with a constant B; such that:

C＝{x∈R ⁿ |B(x)≥0} (16)

sup[L _f B(x)+L _g B(x)u]+bB(x)≥0 (18)

wherein the method comprises the steps of

Is a shorthand; at this point the CBF-CLF constraint of the system is available.

Step 5, establishing a CBF-NMPC model predictive control system;

adding the CBF constrained by the relaxation attenuation technology into nonlinear model predictive control; wherein the decision variables are:

U＝[u ^T _t|t ,...,u ^T _t|t+N-1 ] ^T

CBF-NMPC can be expressed as the following formula:

x _t+k+1|t ＝f(x _t+k|t ,u _t+k|t ),k＝0,...,N-1 (20)

u _t+k|t ∈U,x _t+k|t ∈X,k＝0,...,N-1 (21)

x _t|t ＝x _t (22)

h(x _t+k+1|t )≥w _k (1-γ _k )h(x _t+k|t )，w _k ≥0，k＝0,...,M _CBF -1 (23)

equation (20) is the system dynamics, and input constraints (21) and initial conditions (22) are used for optimization; controlling the use of the Liapunov function v as a terminal cost and edge by beta amplification

Accumulating cost, if beta approaches infinity, no terminal cost constraint is required to be specified; the attenuation rate in CBF is controlled from a fixed value of 1-gamma _k Relaxation optimization to w _k (1-γ _k ) And the relaxation rate variable ψ (w _k ) Is added to the cost of (a); the relaxation variable is constrained by equation (23); at this time, a CBF-NMPC model predictive control system is obtained.

Step 6, adding CLF constraint, and establishing a CBF-CLF-NMPC model predictive control system;

taking the stability criteria of the CLF as constraints rather than the terminal cost, the following formula is derived:

x _t+k+1|t ＝f(x _t+k|t ,u _t+k|t ),k＝0,...,N-1 (25)

u _t+k|t ∈U,x _t+k|t ∈X,k＝0,...,N-1 (26)

x _t|t ＝x _t (27)

h(x _t+k+1|t )≥w _k (1-γ _k )h(x _t+k|t )，w _k ≥0，k＝0,...,M _CBF -1 (28)

V(x _t+k+1|t )≤(1-α _k )V(x _t+k|t )+S _k ，k＝0,...,M _CLF -1 (29)

wherein M is _CLF And M _CBF Is the range of CLF and CBF constraint, is less than the predicted visual field length N of NMPC, and introduces a relaxation variable

To avoid infeasibility between CLF and CBF constraints, and to add an additional term Φ (s _k ) To minimize these relaxation variables; due to the introduction of the relaxation optimization variable w _k The computational complexity is increased to some extent, but the computational complexity can be reduced by reducing the constraint range, so that the CBF-CLF-NMPC model predictive control method is obtained.

As shown in fig. 5, the right-angle line segment is a prescribed route, the curve line segment is an actual route tracked by the unmanned vehicle through the NMPC algorithm, wherein two annular circles are static obstacles, and as can be seen from the figure, the unmanned vehicle bypasses the static obstacles during traveling and quickly returns to a preset route, and the prescribed route can be well fitted at the turning position.

As shown in fig. 6, the linear speed, angular speed, and rotational speed of the two driving wheels of the differential model of the unmanned vehicle during this process are given. It can be seen from the figure that the unmanned vehicle decelerates at the turning point and has different angular speeds during obstacle avoidance and turning, and from the two-drive wheel rotation speed diagram, the differential wheel trolley has identical rotation speeds of the two drive wheels during straight running, and turns through the speed difference between the two drive wheels when turning is needed.

As shown in fig. 7, the performance after DCLF and DCBF constraints were added in a multi-unmanned vehicle environment was tested. As can be seen from fig. 7, the unmanned vehicle car1 is an unmanned vehicle deployed with the NMPC-DCBF-DCLF algorithm, the remaining unmanned vehicles car2 are unmanned vehicles that irregularly move in the environment, and the unmanned vehicles car1 need to avoid other mobile agents to reach the target point in the upper right corner.

As shown in fig. 8 a-d, if there is no remaining moving unmanned vehicle car2, unmanned vehicle car1 can reach the target point directly along a straight line, but in its running diagram, it is detected that collision with moving unmanned vehicle car2 occurs, so it changes the traveling direction, after continuing to travel, it is found that front moving unmanned vehicle car2 may collide with the present position, and then unmanned vehicle car1 moves backward, and after not colliding, it continues to advance forward, finally reaching the target point.

As shown in fig. 9, the changes in linear and angular speeds during operation of the unmanned vehicle are illustrated. As can be seen from the figure, when the turning angle is small, the linear speed of the unmanned vehicle remains unchanged, if the unmanned vehicle needs to turn through a large angle, the unmanned vehicle can slow down, and if the current position collides with the predicted state of other mobile intelligent agents, the unmanned vehicle can also reverse to avoid collision.

The method can solve the problem that the path planning result effect of various planning algorithms is poor due to the characteristics of the various planning algorithms, fully improves the control performance of the vehicle on the premise that the safety of the unmanned vehicle is fully ensured, and realizes the optimal path planning and efficient control of the unmanned vehicle.

It will be apparent to those skilled in the art that various modifications and variations can be made to the present invention without departing from the spirit or scope of the invention. Thus, it is intended that the present invention also include such modifications and alterations insofar as they come within the scope of the appended claims or the equivalents thereof.

Claims (7)

1. The CLF-CBF optimized unmanned vehicle motion planning control method is characterized in that: the method comprises the following steps:

step 1, establishing a two-wheel differential chassis model of an unmanned vehicle;

step 2, establishing a differential motion model of two wheels of the unmanned vehicle;

step 3, designing a Liapunov function to improve the tracking stability of a vehicle control path;

step 4, providing input constraint and safety key constraint for a control system through controlling an obstacle function, and improving the safety of the system;

step 5, establishing a CBF-NMPC model predictive control system;

and 6, adding CLF constraint, and establishing a CBF-CLF-NMPC model predictive control system.

2. A CLF-CBF optimized unmanned vehicle motion planning control method as claimed in claim 1, wherein: in the step 1, the two-wheel differential chassis model comprises a chassis, two driving wheels and four driven wheels, wherein the two driving wheels are positioned at the left side and the right side of the middle part of the chassis and are respectively driven by two independent motors; if the rotation speeds of the two motors are the same, the unmanned vehicle moves along a straight line, and if the rotation speeds of the two motors are different, the unmanned vehicle moves along a circle, and four driven wheels are respectively distributed at four corner points of the chassis and used for maintaining balance of the unmanned vehicle.

3. A CLF-CBF optimized unmanned vehicle motion planning control method as claimed in claim 2, wherein: in step 2, let l be the track width of the two driving wheels, v _l And v _r The speed of the two driving wheels is that r is the turning radius;

the linear velocity formula of the unmanned vehicle is:

because the radian of the rotation of the unmanned vehicle in unit time is very small, the approximate formula is as follows:

wherein is theta ₃ The rotating angle, i is the wheelbase of the vehicle;

calculating the angular velocity w as the rotation angle in unit time:

the turning radius is the ratio of the linear velocity v to the angular velocity w:

the speeds of the left driving wheel and the right driving wheel are calculated by the formula (1) and the formula (3):

4. a CLF-CBF optimized unmanned vehicle motion planning control method as claimed in claim 3, wherein: in step 3, for a continuous autonomous system

Planning a starting point x ₀ And end point x _e Describing the lispro function as a continuously-derivable function V (x), satisfying the following two conditions:

V(x _e )＝0，V(x)>0，x≠x _e (7)

for a control affine system f (x) +g (x) u, the control lispro function is expressed as a continuously-derivable function V (x), with a constant C such that:

Ω _c ＝{x∈R ⁿ :v(x)≤C} (9)

V(x)>0,x∈R ⁿ \{x _e } (10)

wherein:

at this time, a control system with stable state can be obtained.

5. A CLF-CBF optimized unmanned vehicle motion planning control method as recited in claim 4, wherein: in step 4, consider the system

Wherein->

To obtain f (x) R ⁿ →R ⁿ Let it be assumed that set C is the upper level set of one smooth function h (x), i.e. the following condition is satisfied:

C＝{x∈Rn|h(x)≥0} (13)

Int(C)＝{x∈Rn|h(x)>0} (15)

and for all points on the boundary,

then if and only if->

When set C is a forward invariant set; the control barrier function is described as a continuously-derivable function B (x), with a constant B; such that:

C＝{x∈R ⁿ |B(x)≥0} (16)

sup[L _f B(x)+L _g B(x)u]+bB(x)≥0 (18)

wherein the method comprises the steps of

Is a shorthand; at this point the CBF-CLF constraint of the system is available.

6. A CLF-CBF optimized unmanned vehicle motion planning control method as recited in claim 5, wherein: in step 5, adding CBF constrained by relaxation attenuation technology into nonlinear model predictive control; wherein the decision variables are:

U＝[u ^T _t|t ,...,u ^T _t|t+N-1 ] ^T

CBF-NMPC can be expressed as the following formula:

x _t+k+1|t ＝f(x _t+k|t ,u _t+k|t ),k＝0,...,N-1 (20)

u _t+k|t ∈U,x _t+k|t ∈X,k＝0,...,N-1 (21)

x _t|t ＝x _t (22)

h(x _t+k+1|t )≥w _k (1-γ _k )h(x _t+k|t )，w _k ≥0，k＝0,...,M _CBF -1 (23)

equation (20) is the system dynamics, and input constraints (21) and initial conditions (22) are used for optimization; controlling the use of the Liapunov function v as a terminal cost and edge by beta amplification

Accumulating cost, if beta approaches infinity, no terminal cost constraint is required to be specified; the attenuation rate in CBF is controlled from a fixed value of 1-gamma _k Relaxation optimization to w _k (1-γ _k ) And the relaxation rate variable ψ (w _k ) Is added to the cost of (a); the relaxation variable is constrained by equation (23); at this time, a CBF-NMPC model predictive control system is obtained.

7. A CLF-CBF optimized unmanned vehicle motion planning control method as claimed in claim 6, wherein: in step 6, the stability criteria of CLF are taken as constraints instead of terminal cost, resulting in the following formula:

x _t+k+1|t ＝f(x _t+k|t ,u _t+k|t ),k＝0,...,N-1 (25)

u _t+k|t ∈U,x _t+k|t ∈X,k＝0,...,N-1 (26)

x _t|t ＝x _t (27)

h(x _t+k+1|t )≥w _k (1-γ _k )h(x _t+k|t )，w _k ≥0，k＝0,...,M _CBF -1 (28)

V(x _t+k+1|t )≤(1-α _k )V(x _t+k|t )+S _k ，k＝0,...,M _CLF -1 (29)

wherein M is _CLF And M _CBF Is the range of CLF and CBF constraint, is less than the predicted visual field length N of NMPC, and introduces a relaxation variable

To avoid infeasibility between CLF and CBF constraints, and to add an additional term Φ (s _k ) To minimize these relaxation variables; due to the introduction of the relaxation optimization variable w _k The computational complexity is increased to some extent, but the computational complexity can be reduced by reducing the constraint range, so that the CBF-CLF-NMPC model predictive control method is obtained.

Images