An Unmanned Vehicle Motion Planning and Control Method Optimized by CLF-CBF

Abstract

The present invention discloses a method for optimizing the motion planning and control of an unmanned vehicle by using CLF-CBF, and the specific steps include establishing a two-wheel differential chassis model of the driverless vehicle; establishing a two-wheel differential kinematic model of the driverless vehicle; designing a Lyapunov function to improve the stability of the vehicle control path tracking; providing input constraints and safety-critical constraints for the control system by controlling the obstacle function to improve the system safety; establishing a CBF-NMPC model predictive control system; adding CLF constraints to establish a CBF-CLF-NMPC model predictive control system. This method can solve the problem of poor path planning results caused by the characteristics of various planning algorithms, fully improve the vehicle control performance on the premise that the safety of the driverless vehicle is sufficiently guaranteed, and achieve the optimal path planning and efficient control of the driverless vehicle.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned vehicle motion control, and particularly to a motion planning control method for an unmanned vehicle optimized by CLF-CBF. Background Art

[0002] Motion planning control is one of the key points in the research field of unmanned vehicles. An efficient motion planning control algorithm can enable an unmanned vehicle to reach the target point with the minimum cost and optimal performance without collision.

[0003] Currently, the motion planning control algorithms for unmanned vehicles cannot well match the non-linear moving state of unmanned vehicles. The algorithms are prone to large deviations in predicting the moving path, and the prediction process has strict requirements for path points, often needing to consider all the kinematic information of the unmanned vehicle at each point on the path, resulting in poor robustness of the algorithms. In addition, most of the path planning algorithms based on grid maps do not consider the kinematic model of the unmanned vehicle, and the obtained planned paths are not smooth.

[0004] In terms of optimal control, although most control methods have considered the safety and comfort of unmanned vehicles in many dimensions, few control methods take into account the potential conflicts during the vehicle operation process, resulting in that unmanned vehicles cannot obtain better performance on the premise of system safety.

[0005] Therefore, there is an urgent need for a motion planning control method with good path planning effect, strong robustness, and high safety. Control the unmanned vehicle to plan the optimal path to avoid potential risks, and achieve the optimal path planning and efficient and stable control of the unmanned vehicle. Summary of the Invention

[0006] Aiming at the above existing technical deficiencies, the purpose of the present invention is to provide a motion planning control method for an unmanned vehicle optimized by CLF-CBF, which can introduce CLF (Control Lyapunov Function) and CBF (Control Barrier Function) to optimize NMPC (Nonlinear Model Predictive Control) for the problem that the existing common path planning algorithms have poor path planning effects due to their own characteristic limitations. The obtained path planning algorithm has a smooth predicted path and strong robustness, can fully improve the control performance of the unmanned vehicle, enhance the avoidance ability of the unmanned vehicle for static obstacles and moving obstacles in a complex environment, and achieve the optimal path planning and efficient and stable control of the unmanned vehicle.

[0007] To solve the above technical problems, the present invention adopts the following technical solutions:

[0008] The present invention provides a motion planning and control method for an unmanned vehicle optimized by CLF-CBF, comprising the following steps:

[0009] Step 1: Establish a two-wheel differential chassis model for the driverless vehicle;

[0010] Step 2: Establish a two-wheel differential kinematic model for the driverless vehicle;

[0011] Step 3: Design a Lyapunov function to improve the stability of the vehicle control path tracking;

[0012] Step 4: Provide input constraints and safety-critical constraints for the control system by controlling the barrier function to improve the system safety;

[0013] Step 5: Establish a CBF-NMPC model predictive control system;

[0014] Step 6: Add CLF constraints to establish a CBF-CLF-NMPC model predictive control system.

[0015] Preferably, in Step 1, the two-wheel differential chassis model includes a chassis, two driving wheels and four passive wheels. The two driving wheels are located on the left and right sides in the middle of the chassis and are respectively driven by two independent motors; if the rotational speeds of the two motors are the same, the driverless vehicle moves in a straight line, and if the rotational speeds of the two motors are different, the driverless vehicle moves in a circle. The four passive wheels are respectively distributed at the four corner points of the chassis to maintain the balance of the driverless vehicle.

[0016] Preferably, in Step 2, let l be the wheelbase of the two driving wheels, v l and v r be the speeds of the two driving wheels, and r be the turning radius;

[0017] Then the linear velocity formula of the driverless vehicle is:

[0018]

[0019] Since the radian of the driverless vehicle rotating within a unit time is very small, its approximate formula is:

[0020]

[0021] where θ 3 is the rotation angle, and l is the wheelbase of the vehicle;

[0022] Calculate the angular velocity w as the angle rotated within a unit time:

[0023]

[0024] The turning radius is the ratio of the linear velocity v to the angular velocity w:

[0025]

[0026] The speeds of the left and right drive wheels are calculated by formulas (1) and (3) as follows:

[0027]

[0028]

[0029] Preferably, in step 3, for a continuous autonomous system Plan a path from the starting point x 0 to the ending point x e such that the Lyapunov function is described as a continuously differentiable function V(x) satisfying the following two conditions:

[0030] V(x e ) = 0, V(x) > 0, x ≠ x e (7)

[0031]

[0032] For a control-affine system f(x) + g(x)u, the control Lyapunov function is expressed as a continuously differentiable function V(x), and there exists a constant C such that:

[0033] Ω c = {x ∈ R n : v(x) ≤ C} (9)

[0034] V(x) > 0, x ∈ R n \{x e} (10)

[0035]

[0036] Where:

[0037]

[0038] A control system with stable states can be obtained at this time.

[0039] Preferably, in step 4, consider the system Where Get f(x): R n → R n and assume that the set C is the upper level set of a smooth function h(x), that is, it satisfies the following conditions:

[0040] C = {x ∈ Rn | h(x) ≥ 0} (13)

[0041]

[0042] Int(C) = {x ∈ Rn | h(x) > 0} (15)

[0043] And for all points on the boundary Then if and only if the set C is a forward invariant set; the control barrier function is described as a continuously differentiable function B(x), and there exists a constant b; such that:

[0044] C = {x ∈ R n | B(x) ≥ 0} (16)

[0045]

[0046] sup[L f B(x) + L g B(x)u] + bB(x) ≥ 0 (18)

[0047] Where is a shorthand; at this time, the CBF-CLF constraints of the system can be obtained.

[0048] Preferably, in step 5, the CBF with relaxation decay technology constraints is added to the nonlinear model predictive control; where the decision variables are:

[0049] U = [u T t|t ,..., u T t|t+N-1 T

[0050]

[0051] CBF-NMPC can be expressed as the following formula:

[0052]

[0053] x t+k+1|t = f(x t+k|t , u t+k|t ), k = 0,..., N - 1 (20)

[0054] u t+k|t ∈ U, x t+k|t ∈ X, k = 0,..., N - 1 (21)

[0055] x t|t = x t (22)

[0056] h(x t+k+1|t ) ≥ w​k (1 - γ k )h(x t+k|t ), w k ≥0, k = 0, ..., M CBF -1 (23)

[0057] Equation (20) is for system dynamics, and the input constraint (21) and the initial condition (22) are used for optimization; the control Lyapunov function v is amplified by β and used as the terminal cost and the cumulative cost along If β approaches infinity, there is no need to specify the terminal cost constraint; the decay rate in the CBF is relaxed from the fixed value 1 - γ k to w k (1 - γ k ), and an additional cost of the relaxation rate variable Ψ(w k ) is included in the optimization; the relaxation variable is constrained by Equation (23); at this time, the CBF - NMPC model predictive control system is obtained.

[0058] Preferably, in step 6, the stability criterion of the CLF is used as a constraint instead of the terminal cost, and the following equation is obtained:

[0059]

[0060] x t+k+1|t = f(x t+k|t , u t+k|t ), k = 0, ..., N - 1 (25)

[0061] u t+k|t ∈U, x t+k|t ∈X, k = 0, ..., N - 1 (26)

[0062] x t|t = x t (27)

[0063] h(x t+k+1|t ) ≥ w k (1 - γ k )h(x t+k|t ), w k ≥0, k = 0, ..., M CBF -1 (28)

[0064] V(x t+k+1|t ) ≤ (1 - α k )V(x t+k|t ) + S k , k = 0, ..., M CLF -1 (29)

[0065] where M CLF and MCBF It is the range of CLF and CBF constraints. Select a value less than the NMPC prediction horizon length N, and introduce slack variables to avoid infeasibility between CLF and CBF constraints, and add an additional term Φ(s k ) to minimize these slack variables; due to the introduction of the slack optimization variable w k , the computational complexity has increased, but reducing the constraint range can reduce the computational complexity. Thus, the CBF-CLF-NMPC motion planning control method is obtained.

[0066] The beneficial effects of the present invention are as follows: The present invention can address the problem of poor path planning effect caused by the limitations of existing common path planning algorithms due to their own characteristics. By introducing CLF (Control Lyapunov Function) and CBF (Control Barrier Function) to optimize NMPC (Nonlinear Model Predictive Control), the obtained path planning algorithm has a smooth predicted path and strong robustness, can fully improve the control performance of driverless vehicles, enhance the ability of driverless vehicles to avoid static and moving obstacles in complex environments, and achieve optimal path planning and efficient and stable control of driverless vehicles. BRIEF DESCRIPTION OF THE DRAWINGS

[0067] In order to more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the drawings required for use in the description of the embodiments or the prior art. Obviously, the drawings in the following description are only some embodiments of the present invention. For those of ordinary skill in the art, without creative efforts, other drawings can be obtained based on these drawings.

[0068] Figure 1 It is a schematic diagram of the two-wheel differential chassis model of a driverless vehicle provided by an embodiment of the present invention;

[0069] Figure 2 It is a schematic diagram of the two-wheel differential motion model of a driverless vehicle provided by an embodiment of the present invention;

[0070] Figure 3 It is a path schematic diagram of the starting point x0 and the ending point xe provided by an embodiment of the present invention;

[0071] Figure 4 It is a schematic diagram of the smooth function h(x) provided by an embodiment of the present invention;

[0072] Figure 5 It is a simulation test diagram of the NMPC algorithm provided by an embodiment of the present invention;

[0073] Figure 6 is the speed index diagram of the driverless vehicle provided by the embodiment of the present invention;

[0074] Figure 7 is the simulation deployment diagram of the CBF-CLF-NMPC algorithm provided by the embodiment of the present invention;

[0075] Figure 8 is the performance test diagram of the CBF-CLF-NMPC algorithm provided by the embodiment of the present invention;

[0076] Figure 9 is the linear speed and angular speed diagram of the driverless vehicle provided by the embodiment of the present invention. Detailed implementation manners

[0077] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the protection scope of the present invention.

[0078] As Figures 1 to 4 shown, a motion planning and control method for an unmanned vehicle optimized by CLF-CBF includes the following steps:

[0079] Step 1: Establish a two-wheel differential chassis model for the driverless vehicle;

[0080] The two-wheel differential chassis model includes a chassis, two driving wheels and four passive wheels. The two driving wheels are located on the left and right sides in the middle of the chassis and are respectively driven by two independent motors; if the rotational speeds of the two motors are the same, the driverless vehicle moves in a straight line, and if the rotational speeds of the two motors are different, the driverless vehicle moves in a circle. The four passive wheels are respectively distributed at the four corner points of the chassis to maintain the balance of the driverless vehicle.

[0081] Step 2: Establish a two-wheel differential kinematic model for the driverless vehicle;

[0082] Let l be the wheelbase of the two driving wheels, v l and v r be the speeds of the two driving wheels, and r be the turning radius;

[0083] Then the linear speed formula of the driverless vehicle is:

[0084]

[0085] Since the radian of the driverless vehicle rotating within a unit time is very small, its approximate formula is:

[0086]

[0087] where θ 3 is the rotation angle, and l is the wheelbase of the vehicle;

[0088] The angular velocity is calculated as the angle rotated per unit time:

[0089]

[0090] The turning radius is the ratio of the linear velocity to the angular velocity:

[0091]

[0092] The speeds of the left and right drive wheels are calculated through formulas (1) and (3) as:

[0093]

[0094]

[0095] Step 3: Design a Lyapunov function to improve the vehicle control path tracking stability;

[0096] For a continuous autonomous system Plan a path from the starting point x 0 to the ending point x e and describe the Lyapunov function as a continuously differentiable function V(x) that satisfies the following two conditions:

[0097]

[0098]

[0099] For a control affine system f(x)+g(x)u, the control Lyapunov function is expressed as a continuously differentiable function V(x), and there exists a constant C such that:

[0100] Ω c ={x∈R n :v(x)≤C} (9)

[0101] V(x)>0, x∈R n \{x e} (10)

[0102]

[0103] where:

[0104]

[0105] At this time, a control system with stable states can be obtained.

[0106] Step 4: Provide input constraints and safety-critical constraints for the control system by controlling the barrier function to enhance system safety;

[0107] Consider the system where Obtain \(f(x):\mathbb{R}\) n \(\to\mathbb{R}\) n , and assume that the set \(C\) is the upper level set of a smooth function \(h(x)\), that is, it satisfies the following conditions:

[0108] \(C = \{x\in\mathbb{R}^n|h(x)\geq0\}\ (13)\)

[0109]

[0110] \(\text{Int}(C)=\{x\in\mathbb{R}^n|h(x)>0\}\ (15)\)

[0111] And for all points on the boundary, Then if and only if when, the set \(C\) is a forward invariant set; the control barrier function is described as a continuously differentiable function \(B(x)\), and there exists a constant \(b\); such that:

[0112] \(C = \{x\in\mathbb{R}\) n \(|B(x)\geq0\}\ (16)\)

[0113]

[0114] \(\sup[L\) f \(B(x)+L\) g \(B(x)u]+bB(x)\geq0\ (18)\)

[0115] where is a shorthand; at this time, the CBF-CLF constraints of the system can be obtained.

[0116] Step 5: Establish a CBF-NMPC model predictive control system;

[0117] Add the CBF with relaxation decay technology constraints to the nonlinear model predictive control; where the decision variable is:

[0118] \(U = [u\) T t|t ,\(\cdots,u\) T t|t+N-1 \ T

[0119]

[0120] CBF-NMPC can be expressed as the following formula:

[0121]

[0122] x t+k+1|t = f(x t+k|t , u t+k|t ), k = 0, ..., N - 1 (20)

[0123] u t+k|t ∈U, x t+k|t ∈X, k = 0, ..., N - 1 (21)

[0124] x t|t = x t (22)

[0125] h(x t+k+1|t ) ≥ w k (1 - γ k )h(x t+k|t ), w k ≥ 0, k = 0, ..., M CBF - 1 (23)

[0126] Equation (20) represents the system dynamics, and the input constraint (21) and the initial condition (22) are used for optimization; the control Lyapunov function v is amplified by β and used as the terminal cost and the cumulative cost along If β approaches infinity, there is no need to specify the terminal cost constraint; the decay rate in the CBF is relaxed from the fixed value 1 - γ k to w k (1 - γ k ), and an additional cost of the relaxation rate variable Ψ(w k ) is included in the optimization; the slack variable is constrained by Equation (23); at this time, the CBF-NMPC model predictive control system is obtained.

[0127] Step 6: Add the CLF constraint to establish the CBF-CLF-NMPC model predictive control system;

[0128] Taking the stability criterion of the CLF as a constraint instead of the terminal cost, the following equation is obtained:

[0129]

[0130] x t+k+1|t = f(x t+k|t , u t+k|t ), k = 0, ..., N - 1 (25)

[0131] u t+k|t ∈U, x t+k|t ∈X, k = 0, ..., N - 1 (26)

[0132] xt|t = x t (27)

[0133] h(x t+k+1|t ) ≥ w k (1 - γ k )h(x t+k|t ),w k ≥ 0, k = 0, ..., M CBF -1 (28)

[0134] V(x t+k+1|t ) ≤ (1 - α k )V(x t+k|t ) + S k ,k = 0, ..., M CLF -1 (29)

[0135] where M CLF and M CBF are the ranges of the CLF and CBF constraints, chosen to be less than the NMPC prediction horizon length N. Slack variables are introduced to avoid infeasibility between the CLF and CBF constraints, and an additional term Φ(s k ) is added to minimize these slack variables; due to the introduction of the slack optimization variable w k , the computational complexity increases, but reducing the constraint range can reduce the computational complexity, and thus the CBF-CLF-NMPC model predictive control method is obtained.

[0136] As Figure 5 shown, the straight-line segments are the specified routes, and the curved segments are the actual routes followed by the driverless vehicle through the NMPC algorithm. Among them, the two circular rings are static obstacles. It can be seen from the figure that the driverless vehicle bypasses the static obstacles during driving and quickly returns to the predetermined route, and can also well fit the established route at the turning points.

[0137] As Figure 6 shown, the linear velocity, angular velocity of the driverless vehicle during this process, and the rotational speeds of the two drive wheels of the differential wheel model are given. It can be seen from the figure that the driverless vehicle decelerates at the turning points and has different angular velocities during obstacle avoidance and turning. It can be seen from the rotational speed diagram of the two drive wheels that the rotational speeds of the two drive wheels of the differential wheel car are the same during straight driving, and when turning is required, turning is achieved through the speed difference between the two drive wheels.

[0138] As Figure 7 shown, the performance after adding DCLF and DCBF constraints is tested in a multi-driverless vehicle environment. From Figure 7It can be seen that the unmanned vehicle car1 is an unmanned vehicle that deploys the NMPC-DCBF-DCLF algorithm, and the other unmanned vehicle car2 is an unmanned vehicle that moves irregularly in the environment. The unmanned vehicle car1 needs to avoid other mobile agents to reach the target point in the upper right corner.

[0139] like Figure 8 As shown in the figure ad, if there is no other moving unmanned vehicle car2, the unmanned vehicle car1 can reach the target point directly in a straight line, but in its operation diagram, it detects that it will collide with the moving unmanned vehicle car2, so it changes its walking direction. After continuing to drive, it finds that the moving unmanned vehicle car2 in front may collide with the current position, so the unmanned vehicle car1 moves backward, and continues to move forward after there is no collision, and finally reaches the target point.

[0140] like Figure 9 As shown in the figure, the changes in linear velocity and angular velocity of the unmanned vehicle during operation are shown. As can be seen from the figure, when the turning angle is small, the linear velocity of the unmanned vehicle remains unchanged. If a large angle is required, the unmanned vehicle will slow down. If the current position conflicts with the predicted state of other mobile agents, the unmanned vehicle will also back up to avoid collision.

[0141] This method can solve the problem of poor path planning results caused by the characteristics of various planning algorithms, fully improve the vehicle control performance under the premise that the safety of the unmanned vehicle is fully guaranteed, and realize the optimal path planning and efficient control of the unmanned vehicle.

[0142] Obviously, those skilled in the art can make various changes and modifications to the present invention without departing from the spirit and scope of the present invention. Thus, if these modifications and variations of the present invention fall within the scope of the claims of the present invention and their equivalents, the present invention is also intended to include these modifications and variations.

Claims

1. A motion planning and control method for an unmanned vehicle optimized by CLF-CBF, characterized in that: It includes the following steps: Step 1, establish a two-wheel differential chassis model for the driverless vehicle; Step 2, establish a two-wheel differential kinematic model for the driverless vehicle; Step 3, design a Lyapunov function to improve the vehicle control path tracking stability; Step 4, provide input constraints and safety-critical constraints for the control system through the control barrier function to improve system safety; Step 5, establish a CBF-NMPC model predictive control system; Step 6, add CLF constraints to establish a CBF-CLF-NMPC model predictive control system.

2. A motion planning and control method for an unmanned vehicle optimized by CLF-CBF as described in claim 1, characterized in that: In step 1, the two-wheel differential chassis model includes a chassis, two driving wheels and four passive wheels. The two driving wheels are located on the left and right sides in the middle of the chassis and are respectively driven by two independent motors; if the rotational speeds of the two motors are the same, the driverless vehicle moves in a straight line, if the rotational speeds of the two motors are different, the driverless vehicle moves in a circle, and the four passive wheels are respectively distributed at the four corner points of the chassis to maintain the balance of the driverless vehicle.

3. A motion planning and control method for an unmanned vehicle optimized by CLF-CBF as described in claim 2, characterized in that: In Step 2, let l be the wheelbase of the two driving wheels, v l and v r be the speeds of the two driving wheels, and r be the turning radius; Then the linear velocity formula of the driverless vehicle is: Since the radian of rotation of the driverless vehicle per unit time is very small, its approximate formula is: where θ 3 is the angle of rotation, and l is the wheelbase of the vehicle; Calculate the angular velocity w as the angle rotated per unit time: The turning radius is the ratio of the linear velocity v and the angular velocity w: Calculate the speeds of the left and right driving wheels through formulas (1) and (3):

4. A motion planning and control method for an unmanned vehicle optimized by CLF-CBF as described in claim 3, characterized in that: In step 3, for a continuous autonomous system Plan the starting point x 0 and the ending point x e For a path, describe the Lyapunov function as a continuously differentiable function V(x) that satisfies 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 Lyapunov function is expressed as a continuously differentiable function V(x), and there exists a constant C such that: Ω c ={x ∈ R n : v(x) ≤ C} (9) V(x)>0, x∈R n \{x e} (10) Where: At this time, a control system with stable states can be obtained.

5. A motion planning and control method for an unmanned vehicle optimized by CLF-CBF as described in claim 4, characterized in that: In step 4, consider the system where obtain f(x):R n →R n , assume that the set C is a upper level set of a smooth function h(x), that is, it satisfies the following conditions: 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 the set C is a forward invariant set; the control barrier function is described as a continuously differentiable function B(x), and there exists a constant b such that: C = {x ∈ R n | B(x) ≥ 0} (16) sup[L f B(x)+L g B(x)u]+bB(x)≥0 (18) wherein is a shorthand; at this time, the CBF-CLF constraint of the system can be obtained.

6. A motion planning and control method for an unmanned vehicle optimized by CLF-CBF as described in claim 5, characterized in that: In step 5, add the CBF constrained by the relaxation decay technique to the nonlinear model predictive control; Among them, the decision variable is: 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 for system dynamics, and the input constraint (21) and the initial condition (22) are used for optimization; the control Lyapunov function v is amplified by β and used as the terminal cost and along the cumulative cost. If β approaches infinity, there is no need to specify the terminal cost constraint; the decay rate in the CBF is relaxed from the fixed value 1 - γ k to w k (1 - γ k ), and an additional cost of the relaxation rate variable Ψ(w k ) is included in the optimization; the relaxation variable is constrained by Equation (23); at this time, the CBF-NMPC model predictive control system is obtained.

7. A motion planning and control method for an unmanned vehicle optimized by CLF-CBF as described in claim 6, characterized in that: In step 6, use the stability criterion of CLF as a constraint instead of the terminal cost to obtain 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) where M CLF and M CBF are the ranges of the CLF and CBF constraints, choosing less than the NMPC prediction horizon length N, introducing slack variables to avoid infeasibility between the CLF and CBF constraints, and adding an additional term Φ(s k ) to minimize these slack variables; due to the introduction of the slack optimization variable w k , the computational complexity has increased, but reducing the constraint range can reduce the computational complexity, and thus the CBF-CLF-NMPC model predictive control method is obtained.

Images