Unmanned vehicle formation planning and obstacle avoidance method

Abstract

The invention discloses an unmanned vehicle formation planning and obstacle avoidance method, and belongs to the field of engineering control, and the method comprises the steps: constructing a kinematics model and a dynamics model of an unmanned vehicle; based on the kinematics model and the dynamics model, a pilot following method and a virtual structure method are adopted to obtain a follower unmanned vehicle space attitude tracking error model; obtaining a multi-unmanned vehicle formation system model based on the follower unmanned vehicle attitude tracking error model; obtaining a target point distance factor and an adaptive adjustment factor, and establishing a potential field function model; based on the multi-unmanned vehicle formation system model and the potential field function model, obtaining a dynamic obstacle avoidance path planning model; and based on the dynamic obstacle avoidance path planning model, a formation tracking control model is obtained by adopting a backstepping sliding mode algorithm, and the formation tracking control model is used for controlling unmanned vehicle formation. The method solves the problems that an existing artificial potential field method is unreachable in target, prone to falling into local optimum, too large in gravitational force and the like.

Description

Technical Field

[0001] The invention discloses an unmanned vehicle formation planning and obstacle avoidance method, belonging to the field of engineering control. Background Art

[0002] In recent years, mobile unmanned vehicles (UAVs) have rapidly developed. Using their sensors and programming, they can perform a variety of tasks, from simple handling and cleaning to complex monitoring and decision support. They have been widely used in rescue, logistics, transportation, and other fields, improving work efficiency and productivity. Collaborative UAVs can significantly improve operational efficiency and reliability, offering advantages such as scalability, robustness, safety, and energy efficiency. This further enhances the intelligence level of the system and is gradually becoming a key driver of economic and social development. With the continuous development and in-depth application of UAV technology, the demand for UAVs in modern society has become increasingly diversified. In particular, the demand for collaborative tasks in complex environments is growing. Reasonable motion paths and stable formations are particularly important in the collaborative process of UAVs. Path planning and obstacle avoidance control in formations are particularly prominent issues. The traditional artificial potential field (APF) method is a simple and effective path planning method that can plan a path from a starting point to a destination in complex environments while avoiding obstacles. However, when UAVs follow UAVs in complex obstacle environments, traditional APFs suffer from drawbacks such as unreachable targets, easy local optima, and excessive gravitational forces. Summary of the Invention

[0003] The purpose of the present invention is to solve the problems of existing artificial potential field methods such as unreachable targets, easy to fall into local optimality and excessive gravity, and to propose a method for unmanned vehicle formation planning and obstacle avoidance.

[0004] The problem to be solved by the present invention is achieved by the following technical solutions:

[0005] A method for planning and avoiding obstacles in an unmanned vehicle formation, wherein the unmanned vehicle formation includes multiple unmanned vehicles, one of which is a leader and the other are followers. The method is used to control the followers so that the unmanned vehicle formation maintains formation and avoids obstacles during movement. The method comprises the following steps: constructing a kinematic model and a dynamic model of the unmanned vehicle; based on the kinematic model and the dynamic model, adopting a leader-following method and a virtual structure method to obtain a follower unmanned vehicle posture tracking error model; based on the follower unmanned vehicle posture tracking error model, obtaining a multi-unmanned vehicle formation system model; obtaining a target point distance factor and an adaptive adjustment factor to establish a potential field function model; based on the multi-unmanned vehicle formation system model and the potential field function model, obtaining a dynamic obstacle avoidance path planning model; based on the dynamic obstacle avoidance path planning model, adopting a backstepping sliding mode algorithm to obtain a formation tracking control model, and the formation tracking control model is used to control the unmanned vehicle formation.

[0006] Preferably, constructing the kinematic model and the dynamic model of the unmanned vehicle includes: establishing an inertial coordinate system with the ground as a reference system and a local coordinate system of the robot itself; based on the inertial coordinate system, obtaining the coordinates of the rear axle center of the unmanned vehicle in the inertial coordinate system and the coordinates of the front axle center of the unmanned vehicle in the inertial coordinate system; obtaining the basic data of the unmanned vehicle, and based on the coordinates of the rear axle center of the unmanned vehicle in the inertial coordinate system, the coordinates of the front axle center of the unmanned vehicle in the inertial coordinate system and the basic data of the unmanned vehicle, obtaining the kinematic model of the unmanned vehicle in the local coordinate system of the robot itself; based on the kinematic model of the unmanned vehicle, obtaining the dynamic model of the unmanned vehicle in the local coordinate system of the robot itself.

[0007] Preferably, based on the kinematic model and the dynamic model, the pilot following method and the virtual structure method are used to obtain the follower unmanned vehicle posture tracking error model, including: obtaining the corresponding coordinates of the pilot unmanned vehicle in the inertial coordinate system; based on the corresponding coordinates of the pilot unmanned vehicle in the inertial coordinate system, obtaining the pilot unmanned vehicle state vector, virtual follower state vector and follower unmanned vehicle state vector in the robot's own local coordinate system; based on the pilot unmanned vehicle state vector, the virtual follower state vector and the follower unmanned vehicle state vector, obtaining the follower unmanned vehicle posture tracking error model.

[0008] Preferably, the multi-unmanned vehicle formation system model is obtained based on the follower unmanned vehicle posture tracking error model, including: obtaining the actual angle between the follower unmanned vehicle and the forward direction of the lead unmanned vehicle; based on the actual angle between the follower unmanned vehicle and the forward direction of the lead unmanned vehicle and the follower unmanned vehicle posture tracking error model, the multi-unmanned vehicle formation system model is obtained.

[0009] Preferably, obtaining the target point distance factor and the adaptive adjustment factor and establishing the potential field function model includes: obtaining the target point distance factor and the adaptive adjustment factor; establishing a repulsive potential field function, a rotational potential field function, a gravitational potential field function and an internal potential field function between formation members based on the target point distance factor and the adaptive adjustment factor; obtaining a potential field function model based on the repulsive potential field function, the rotational potential field function, the gravitational potential field function and the internal potential field function between formation members.

[0010] Preferably, the dynamic obstacle avoidance path planning model is obtained based on the multi-unmanned vehicle formation system model and the potential field function model, including: obtaining the multi-unmanned vehicle formation system linear model based on the multi-unmanned vehicle formation system model; obtaining the dynamic obstacle avoidance path planning model based on the multi-unmanned vehicle formation system linear model.

[0011] Preferably, based on the dynamic obstacle avoidance path planning model, the backstepping sliding mode algorithm is used to obtain the formation tracking control model, including: obtaining virtual control parameters, and based on the first-layer sliding mode function and the second-layer sliding mode function, using the backstepping sliding mode algorithm to obtain the formation tracking control model.

[0012] The present invention has the following beneficial effects compared with the prior art:

[0013] The present invention discloses a method for unmanned vehicle formation planning and obstacle avoidance. The IAPF potential field function is introduced into model prediction, and formation control is converted into a tracking control problem of multiple unmanned vehicles. A formation tracking control model based on a backstepping sliding mode algorithm is constructed, which can generate a reasonable path and achieve stable operation of the formation and flexible obstacle avoidance, thereby effectively solving the problems of path planning, obstacle avoidance and formation stability. BRIEF DESCRIPTION OF THE DRAWINGS

[0014] Figure 1 It is a flow chart of the unmanned vehicle formation planning and obstacle avoidance method of the present invention.

[0015] Figure 2 This is a structural diagram of the wheeled unmanned vehicle model used in the unmanned vehicle platoon planning and obstacle avoidance method of the present invention;

[0016] Figure 3 A schematic diagram of a formation in the unmanned vehicle formation planning and obstacle avoidance method of the present invention;

[0017] Figure 4 A schematic diagram of the unmanned vehicle platoon planning and obstacle avoidance method of the present invention;

[0018] Figure 5 The unmanned vehicle formation planning and obstacle avoidance method of the present invention is based on the formation control composition of the backstepping sliding film algorithm;

[0019] Figure 6 A flow chart of the obstacle avoidance strategy for the unmanned vehicle formation planning and obstacle avoidance method of the present invention;

[0020] Figure 7 This is a physical simulation experiment scene of the Gazebo interface for the unmanned vehicle formation planning and obstacle avoidance method of the present invention;

[0021] Figure 8 This is the Rviz interface physical simulation experiment scene of the unmanned vehicle formation planning and obstacle avoidance method of the present invention;

[0022] Figure 9 The actual motion trajectory of multiple unmanned vehicle formations in the unmanned vehicle formation planning and obstacle avoidance method of the present invention;

[0023] Figure 10 This is the actual motion trajectory error diagram of the unmanned vehicle formation planning and obstacle avoidance method of the present invention. DETAILED DESCRIPTION

[0024] The following is based on the attached Figures 1-10 The present invention will be further described:

[0025] The technical solution of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the embodiments described are only some embodiments of the present invention, not all embodiments. All other embodiments obtained by ordinary technicians in this field based on the embodiments of the present invention without making any creative efforts shall fall within the scope of protection of the present invention.

[0026] In the description of the present invention, it should be noted that the terms "center", "up", "down", "left", "right", "vertical", "horizontal", "inside", "outside", etc., indicating directions or positional relationships, are based on the directions or positional relationships shown in the accompanying drawings. They are only for the convenience of describing the present invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific direction, be constructed and operated in a specific direction. Therefore, they cannot be understood as limiting the present invention.

[0027] In the description of the present invention, it should be noted that, unless otherwise expressly specified or limited, the terms "mounted," "connected," and "connected" should be understood in a broad sense. For example, they may refer to fixed, detachable, or integral connections; mechanical or electrical connections; direct or indirect connections through an intermediate medium; and internal communication between two components. Those skilled in the art will understand the specific meanings of the above terms in the present invention based on the specific circumstances.

[0028] like Figure 1As shown, the first embodiment of the present invention provides, based on the existing technology, an unmanned vehicle formation including multiple unmanned vehicles, one of which is a leader and the other unmanned vehicles are followers. The unmanned vehicle formation planning and obstacle avoidance method is used to control the followers so that the unmanned vehicle formation maintains formation and avoids obstacles during movement. The unmanned vehicle formation planning and obstacle avoidance method includes the following steps:

[0029] Step S10: construct the kinematic model and dynamic model of the unmanned vehicle. The specific steps are as follows:

[0030] Step S11: Establish an inertial coordinate system with the ground as the reference system and the robot's own local coordinate system. The specific contents are as follows:

[0031] like Figure 2 As shown in the figure, an inertial coordinate system XOY with the ground as the reference system and the robot's own local coordinate system xoy are established respectively. The inertial coordinate system is the coordinate system used by the inertial navigation system, while the robot's own local coordinate system is mainly used to describe the relative motion of the robot.

[0032] Step S12: Based on the inertial coordinate system, the coordinates of the center of the rear axle of the unmanned vehicle and the coordinates of the center of the front axle of the unmanned vehicle in the inertial coordinate system are obtained. That is, the coordinates of the center of the rear axle and the center of the front axle of the unmanned vehicle in the inertial coordinate system are (X r ,Y r ) and (X f ,Y f ).

[0033] Step S13: Obtain the basic data of the unmanned vehicle. Based on the coordinates of the center of the rear axle of the unmanned vehicle in the inertial coordinate system, the coordinates of the center of the front axle of the unmanned vehicle in the inertial coordinate system, and the basic data of the unmanned vehicle, obtain the kinematic model of the unmanned vehicle in the local coordinate system of the robot itself. The specific content is as follows:

[0034] Obtain the basic data of the unmanned vehicle, which includes the speed of the left and right drive wheels on the rear axle, the speed at the center of the unmanned vehicle, the front and rear wheelbase, the instantaneous turning radius of the rear axle center, and the attitude angle of the unmanned vehicle. Based on the coordinates of the unmanned vehicle's rear axle center in the inertial coordinate system, the coordinates of the unmanned vehicle's front axle center in the inertial coordinate system, and the basic data of the unmanned vehicle, obtain the unmanned vehicle's kinematic model in the robot's own local coordinate system:

[0035]

[0036] Among them, v is the velocity at the center of the unmanned vehicle, ω is the angular velocity of the unmanned vehicle, L is the front and rear wheelbase, is the attitude angle of the unmanned vehicle.

[0037] The speed of the center of the autonomous vehicle is

[0038]

[0039] The angular velocity of the unmanned vehicle is

[0040]

[0041] Among them, v l is the speed of the left drive wheel on the rear axle and v r are the speeds of the right driving wheel on the rear axle.

[0042] Step S14: Based on the unmanned vehicle kinematic model, the unmanned vehicle dynamics model in the robot's local coordinate system is obtained. The specific contents are as follows:

[0043] Consider the case where the center of mass and the center of shape coincide and the sideslip and friction are ignored. At this time, the front and rear wheelbase L = 0, and the Lagrangian function of the system is defined as

[0044] M=TU (4)

[0045] Where T represents the kinetic energy of the system and U represents the potential energy of the system.

[0046] Since the unmanned vehicle is traveling on a plane, the system potential energy U=0.

[0047]

[0048] Based on the nonholonomic constraints of the unmanned vehicle and the Lagrange principle, the following Routh equation is obtained:

[0049]

[0050] in is the Lagrange multiplier defined, is the nonholonomic motion constraint of the autonomous vehicle, F f is the force exerted on the centroid of the autonomous vehicle, T l ,T r is the torque of the left and right motors, then

[0051]

[0052] Combining the above equations, we can get the unmanned vehicle dynamics model as

[0053]

[0054] in u1=T l +T r ,u2=T l -T r .

[0055] Step S20: Based on the kinematic model and the dynamic model, the pilot-following method and the virtual structure method are used to obtain the tracking error model of the follower unmanned vehicle. The specific steps are as follows:

[0056] Step S21: Obtain the corresponding coordinates of the pilot unmanned vehicle in the inertial coordinate system. The specific contents are as follows:

[0057] In step S21, Figure 3 As shown in the figure, by setting up a virtual follower, the formation problem is transformed into a tracking control problem. First, the position information of the virtual follower is calculated based on the preset leader-follower formation parameters (including azimuth and relative position). Then, under the control of the designed controller, the follower unmanned vehicle is enabled to track the position of the virtual follower and maintain the tracking error to zero, thereby indirectly ensuring that each follower unmanned vehicle follows the leader unmanned vehicle at a predetermined distance and angle.

[0058] Step S22: Based on the corresponding coordinates of the pilot unmanned vehicle in the inertial coordinate system, the state vector of the pilot unmanned vehicle, the state vector of the virtual follower, and the state vector of the follower unmanned vehicle in the local coordinate system of the robot are obtained. The specific steps are as follows:

[0059] Based on the pilot unmanned vehicle q i The corresponding coordinates in the inertial coordinate system are (x i ,y i ), the state vector of the pilot unmanned vehicle in generalized coordinates is defined as The virtual follower state vector is The state vector of the follower autonomous vehicle is

[0060] Step S23: Based on the state vector of the leading unmanned vehicle, the state vector of the virtual follower, and the state vector of the follower unmanned vehicle, a tracking error model of the follower unmanned vehicle posture is obtained. The specific content is as follows:

[0061] The state vector of the leading unmanned vehicle, the state vector of the virtual follower, and the state vector of the follower unmanned vehicle:

[0062]

[0063]

[0064] in as well as Virtual Followers The expected global coordinates of as well as Follower unmanned vehicle The actual global coordinates of For virtual followers With the pilot unmanned vehicle q ithe distance between the follower unmanned vehicle and the virtual follower unmanned vehicle ij for the follower unmanned vehicle and the lead unmanned vehicle q i the distance between the follower unmanned vehicle and the lead unmanned vehicle; for the follower unmanned vehicle and the lead unmanned vehicle.

[0065] The pose tracking error model of the follower unmanned vehicle is obtained by simultaneously solving the equations The pose tracking error model of the follower unmanned vehicle is obtained by simultaneously solving the equations

[0066]

[0067] Then let This ensures that each follower unmanned vehicle can track the corresponding virtual follower, thereby maintaining the desired formation structure.

[0068] Step S30, based on the pose tracking error model of the follower unmanned vehicle, obtains a multi-unmanned vehicle formation system model, and the specific steps are as follows:

[0069] Step S31, the actual angle between the follower unmanned vehicle and the lead unmanned vehicle is obtained, and the specific content is as follows:

[0070] As shown in the multi-unmanned vehicle formation tracking control structure diagram, the motion trajectory of the formation is mainly determined by the lead unmanned vehicle. The actual angle between the follower unmanned vehicle and the lead unmanned vehicle is: Figure 4

[0071] Where: L ijx is the component of L ij in the x-axis, L ijy is the component of L ij in the y-axis,

[0072] for the follower unmanned vehicle and the lead unmanned vehicle.

[0073] The above L ij , L ijx and L ijy are:

[0074]

[0075]

[0076] Step S32, based on the actual angle between the follower unmanned vehicle and the lead unmanned vehicle and the pose tracking error model of the follower unmanned vehicle, obtains a multi-unmanned vehicle formation system model, and the specific content is as follows:

[0077] ​Based on the actual angle between the follower unmanned vehicle and the leading unmanned vehicle’s forward direction and the follower unmanned vehicle’s posture tracking error model, a multi-unmanned vehicle formation system model based on the leader-follower strategy can be obtained:

[0078]

[0079] in:

[0080] Step S40: Obtain the target point distance factor and the adaptive adjustment factor to establish a potential field function model. The specific steps are as follows:

[0081] Step S41, obtaining the target point distance factor and the adaptive adjustment factor, where: in the traditional artificial potential field, the repulsive force and the attractive force acting on the unmanned vehicle are independent of each other, resulting in a mismatch in the forces acting on the unmanned vehicle. By introducing the target point distance factor and the adaptive adjustment factor into the repulsive potential field function, the repulsive force can be made to depend not only on the obstacle, but also on the distance from the unmanned vehicle to the target point.

[0082] Step S42: Based on the target point distance factor and the adaptive adjustment factor, a repulsive potential field function, a rotational potential field function, an attractive potential field function, and an internal potential field function between formation members are established. The specific contents are as follows:

[0083] The improved repulsive potential field function can be defined as

[0084]

[0085] Among them, μ>0 is an adaptive adjustment factor used to balance attraction and repulsion.

[0086] By designing a rotating potential field U esc (q), set the judgment condition so that when the unmanned vehicle falls into the local optimal value state, the potential field can generate a rotation force (F esc (q)) to force the unmanned vehicle to escape from the local optimal point. The direction of the rotational force is perpendicular to the direction of gravity, and its magnitude is adjusted by the product of the rotation gain factor and the gravity. The designed rotational potential field function is:

[0087]

[0088] Where η represents the rotation gain factor, which is used to adjust the strength of the rotation force. δ is a set constant. n and x n-1 They represent the positions of the unmanned vehicle at the nth step and the n-1th step respectively.

[0089] By improving the gravitational potential field function, it is possible to prevent it from being "pulled" by the strong attraction of the target point in complex environments and colliding with obstacles. Specifically, by adding a gravitational adaptation adjustment factor and improving the boundary value of the gravitational potential field, the magnitude of gravity can be adaptively adjusted, thereby controlling the gravity at the starting point to a reasonable magnitude range. Improved gravitational potential field function:

[0090]

[0091] Where γ is the set gravity adaptation adjustment factor, and ξ is the distance boundary between the unmanned vehicle and the target point.

[0092] By designing an internal potential field function between formation members to generate the necessary repulsive force, this repulsive force between members can ensure that the unmanned vehicles maintain a certain distance during the formation movement, which can effectively reduce or avoid the occurrence of collision risks. The internal potential field function between formation members can be defined as

[0093]

[0094] where k ij is the internal repulsion adjustment factor, which controls the strength of the repulsive force. ij It is an unmanned vehicle R i and R j The distance between them. r is the safe distance set for unmanned vehicle collision avoidance.

[0095] Under the action of the internal potential field function, when the unmanned vehicle R i Obstacle avoidance approaching unmanned vehicle R j When the safety distance r gradually decreases, the mutual repulsive force will increase, thus generating a repulsive force to push the unmanned vehicle R i Stay away from driverless cars j Avoid collisions. The introduction of the internal potential field function enables the unmanned vehicle to not only avoid obstacles when performing tasks, but also maintain the formation structure and avoid collisions with neighboring robots.

[0096] Step S43: Based on the repulsive potential field function, the rotational potential field function, the gravitational potential field function, and the internal potential field function between formation members, a potential field function model is obtained:

[0097] The resultant potential field acting on the unmanned vehicle can be expressed as:

[0098] U all (q)=U att (q)+U rep (q)+U esc (q)+U ij (q) (20)

[0099] Step S50: Based on the multi-unmanned vehicle platoon system model and the potential field function model, a dynamic obstacle avoidance path planning model is obtained. The specific steps are as follows:

[0100] Step S51: Based on the multi-unmanned vehicle platooning system model, a linear model of the multi-unmanned vehicle platooning system is obtained. The specific content is as follows:

[0101] According to the kinematic model of the unmanned vehicle, define u=[v,ω] T , Assuming the wheelbase L = 0, the kinematic equation of the unmanned vehicle is expressed as a nonlinear model:

[0102]

[0103] Linearizing it, we can get

[0104]

[0105] in and is the partial derivative matrix of f(ξ,u) with respect to ξ and u. And the linearized kinematic equation of the unmanned vehicle is rewritten as At this time, solving the partial derivative matrix can be obtained

[0106]

[0107] Step S52: obtaining a dynamic obstacle avoidance path planning model based on the linear model of the multi-unmanned vehicle platooning system.

[0108] definition Then the linearized model of the kinematic equation can be expressed as When the period is T, it is discretized to obtain

[0109]

[0110] The state of the unmanned vehicle in the kth cycle is obtained by sorting out k+1 The actual state ξ k , actual control quantity u k , reference state x r and reference control quantity u r Calculated

[0111]

[0112] Assume that the controller prediction step number is N, and the reference state sequence X={ξ k,r ,ξ k+1,r ,…,ξ k+N-1,r}, the reference input sequence is U={u k,r ,u k+1,r ,…,u k+N-1,r}, with the control sequence U in the next N cycles = {u k ,u k+1 ,…,u k+N-1} as a decision variable, the state of the k+Nth cycle can be predicted and can be expressed as

[0113]

[0114] Then the objective function of designing IAPF-MPC can be expressed as

[0115]

[0116] Where Q, R, F, and ρ are the weight matrices and coefficients of the corresponding terms. The first term reflects the tracking trajectory requirement of the leading unmanned vehicle, ensuring a small tracking error and providing a tracking position for the follower. The second term is the obstacle avoidance potential field term, which restricts the unmanned vehicle from approaching obstacles and plans a reasonable obstacle avoidance path. The third term represents the constraint requirements on the control input, controlling the amplitude and frequency of obstacle avoidance to maintain the stability of the unmanned vehicle's obstacle avoidance motion.

[0117] In step S60 , based on the dynamic obstacle avoidance path planning model, a backstepping sliding mode algorithm is used to obtain a formation tracking control model, which is used to control the unmanned vehicle formation.

[0118] First, the position tracking control law based on the sliding mode algorithm is designed, and the position state space equation of the unmanned vehicle is obtained according to the unmanned vehicle dynamics model. definition Rewriting the follower posture tracking error model can be obtained

[0119]

[0120] in denote the derivative of the virtual follower position and the desired tracking position of the follower, respectively;

[0121] First consider controlling the error x in the x direction e , design u 11 =u eq11 +u sw11 As the virtual control input of the system, the unmanned vehicle dynamics model and the follower posture tracking error model can be used to obtain the tracking error equation of the unmanned vehicle in the x-direction coordinate system:

[0122]

[0123] Then design the first layer sliding mode function S1=c1e1+e2 and derive it as follows:

[0124]

[0125] Among them, c1>0, u eq11 and u sw11 They represent equivalent control and switch control respectively.

[0126] The virtual control input is further designed as

[0127]

[0128] Where l1>0,η1>0, sgn(S1) is the sign function of S1.

[0129] Substituting the virtual control input into the derivative of the first layer sliding mode function, we can get

[0130]

[0131] Design the second-layer sliding mode function S2=α1S1+β1(c2e3+e4) and derive it to obtain

[0132] Where c2>0,u eq12 and u sw12 They represent equivalent control and switch control respectively.

[0133] Design the virtual control input as

[0134]

[0135] Substituting the virtual control input into the derivative of the first-layer sliding mode function, the actual position tracking control law can be designed as

[0136]

[0137] Among them, the parameters l2>0, η2>0, u1=u 11 +u 12 .

[0138] Next, based on the backstepping strategy, the angle tracking control law is obtained by designing the Lyapunov function.

[0139] For steering angle tracking control system, is the steering angle tracking error, z1 is the virtual control input of the steering angle tracking control system, and the state variable is selected as The following Lyapunov function is selected as And taking the derivative we can get

[0140]

[0141] When z1=k1x1 is selected as the virtual control input, it can be converted into

[0142]

[0143] Where k1>0 is a positive proportional coefficient. The actual angle tracking control law is designed as follows: Where k2>0 is a positive proportional coefficient.

[0144] Combining the Lyapunov function and the actual angle tracking control law, we can obtain:

[0145]

[0146] By designing the angle tracking control law based on the backstepping method and combining it with the Lyapunov stability theorem, it can be proved that under the action of the control law u2, the steering angle error It will gradually converge to zero over time, realizing the angle tracking control of the formation system. l +T r , u2=T l -T r Get the overall actual control law According to the actual position tracking control law and the actual angle tracking control law, the actual control inputs u1 and u2 of each subsystem are obtained. The overall actual control input of the formation tracking controller is expressed as T = [T l ,T r ] T To indicate that the follower unmanned vehicle formation tracking system is asymptotically stable at this time.

[0147] Designed by the second layer sliding mode function Consider the Lyapunov function:

[0148]

[0149] Combining the second layer sliding mode function and V1, we can get the derivative of V3 with respect to time:

[0150]

[0151] According to the Lyapunov stability theorem, when x1,x2≠0, we have The follower unmanned vehicle formation tracking system is asymptotically stable, that is, the formation tracking controller designed under the backstepping sliding mode algorithm can make the system tracking error converge to zero over time, and can realize the formation tracking control, such as Figure 5 and 6 shown.

[0152] By installing Ubuntu 18.04 in the Linux operating system and using its built-in Gazebo and Rviz, a joint simulation platform was built to simulate the actual obstacle environment for joint simulation experiments based on MATLAB and ROS. In this platform, airborne laser radar scanning SLAM is used for mapping, obtaining a global map, and grayscale processing of the actual map. The joint simulation of MATLAB and ROS further improves the function of the experimental platform. Set the prediction time domain to N p =10, the control time domain is N c =5, the initial position of the unmanned vehicle is set to (0m, 0m, 0m), (-1.5m, 1.5m, 0m), (-1.5m, -1.5m, 0m), (-3m, 4.5m, 0m), (-3m, -4.5m, 0m). The sampling period is 0.1s, the reference speed of the unmanned vehicle is set to 0.2m / s, the upper and lower limits of the speed are set to ±0.35m / s, and the upper and lower limits of the acceleration are set to ±0.15m / s. 2 , the upper and lower limits of the steering angle are set to ±10rad / s.

[0153] Figure 7 and Figure 8 The demonstration showcases different scenarios of the starting positions and formation movement of five autonomous vehicles in the Gazebo and Rviz interfaces. Each vehicle starts from a different initial position and drives towards its target. All five vehicles quickly respond, tracking and forming a formation, maintaining a square formation. When encountering obstacles, they strategically avoid them and ultimately reach their target safely.

[0154] like Figure 9 Figure 2 shows the actual trajectory of a multi-route vehicle formation consisting of five unmanned vehicles. The figure shows that the lead vehicle is able to plan a relatively reasonable collision avoidance trajectory and lead the formation to the target location. The four follower vehicles, guided by the leader, are able to achieve stable tracking, obstacle avoidance, and formation recovery.

[0155] like Figure 10 As shown in the figure, a formation tracking error diagram of the actual operation of the formation is given. The following detailed analysis shows that the multi-unmanned vehicle system can form a relatively stable formation in about 10 seconds. At t = 21 seconds, the follower (No. 1) encountered an obstacle and avoided it. At t = 28 seconds, the follower (No. 2) and follower (No. 3) encountered obstacles and avoided them respectively. After the obstacle avoidance was completed, each follower unmanned vehicle restored the formation. At t = 48 seconds, the leading unmanned vehicle (No. 5) encountered an obstacle and the formation performed an overall obstacle avoidance movement, completing the formation's turning action. The experiment shows that the motion error during the overall movement of the formation is small, the obstacle avoidance method is relatively flexible, and the formation remains relatively stable overall.

[0156] Although the embodiments of the present invention have been disclosed above, they are not limited to the applications listed in the specification and exemplary embodiments. They can be applied to a variety of fields suitable for the present invention. Further modifications will be readily apparent to those skilled in the art. Therefore, the present invention is not limited to the specific details and illustrations shown and described herein without departing from the general concept defined by the claims and their equivalents.

Claims

1. A method for unmanned vehicle formation planning and obstacle avoidance, characterized in that: The unmanned vehicle formation includes multiple unmanned vehicles, one of which is a leader and the other are followers. The unmanned vehicle formation planning and obstacle avoidance method is used to control the followers so that the unmanned vehicle formation maintains formation and avoids obstacles during movement. The unmanned vehicle formation planning and obstacle avoidance method includes the following steps: Construct kinematic and dynamic models of unmanned vehicles; Based on the kinematic model and the dynamic model, a follower unmanned vehicle posture tracking error model is obtained by using a pilot-following method and a virtual structure method; Based on the follower unmanned vehicle posture tracking error model, a multi-unmanned vehicle formation system model is obtained; Obtain the target point distance factor and adaptive adjustment factor, and establish the potential field function model; Based on the multi-unmanned vehicle platooning system model and the potential field function model, a dynamic obstacle avoidance path planning model is obtained; Based on the dynamic obstacle avoidance path planning model and the backstepping sliding mode algorithm, a formation tracking control model is obtained, which is used to control the unmanned vehicle formation.

2. The unmanned vehicle formation planning and obstacle avoidance method according to claim 1, characterized in that: Constructing the kinematic model and the dynamic model of the unmanned vehicle includes: Establish an inertial coordinate system with the ground as the reference system and the robot's own local coordinate system; Based on the inertial coordinate system, the coordinates of the center of the rear axle of the unmanned vehicle in the inertial coordinate system and the coordinates of the center of the front axle of the unmanned vehicle in the inertial coordinate system are obtained; Obtaining basic data of the unmanned vehicle, and obtaining a kinematic model of the unmanned vehicle in the local coordinate system of the robot itself based on the coordinates of the center of the rear axle of the unmanned vehicle in the inertial coordinate system, the coordinates of the center of the front axle of the unmanned vehicle in the inertial coordinate system, and the basic data of the unmanned vehicle; Based on the unmanned vehicle kinematic model, the unmanned vehicle dynamics model in the robot's own local coordinate system is obtained.

3. The unmanned vehicle formation planning and obstacle avoidance method according to claim 2, characterized in that: Based on the kinematic model and the dynamic model, the pilot-following method and the virtual structure method are used to obtain the follower unmanned vehicle posture tracking error model, including: Get the corresponding coordinates of the pilot unmanned vehicle in the inertial coordinate system; Based on the corresponding coordinates of the pilot unmanned vehicle in the inertial coordinate system, the pilot unmanned vehicle state vector, the virtual follower state vector and the follower unmanned vehicle state vector in the robot's own local coordinate system are obtained; Based on the state vector of the leading unmanned vehicle, the state vector of the virtual follower and the state vector of the follower unmanned vehicle, a position tracking error model of the follower unmanned vehicle is obtained.

4. The unmanned vehicle formation planning and obstacle avoidance method according to claim 3, characterized in that: Based on the follower unmanned vehicle posture tracking error model, the multi-unmanned vehicle platooning system model is obtained, including: Obtain the actual angle between the follower unmanned vehicle and the leading unmanned vehicle; Based on the actual angle between the follower unmanned vehicle and the forward direction of the lead unmanned vehicle and the follower unmanned vehicle posture tracking error model, a multi-unmanned vehicle formation system model is obtained.

5. The unmanned vehicle formation planning and obstacle avoidance method according to claim 4, characterized in that: Obtaining the target point distance factor and the adaptive adjustment factor, and establishing the potential field function model, includes: Obtaining the target point distance factor and the adaptive adjustment factor; Based on the target point distance factor and the adaptive adjustment factor, a repulsive potential field function, a rotational potential field function, an attractive potential field function and an internal potential field function between formation members are respectively established; A potential field function model is obtained based on the repulsive potential field function, the rotational potential field function, the gravitational potential field function and the internal potential field function between the formation members.

6. The unmanned vehicle formation planning and obstacle avoidance method according to claim 5, characterized in that: Based on the multi-unmanned vehicle platooning system model and the potential field function model, the dynamic obstacle avoidance path planning model is obtained, including: Based on the multi-unmanned vehicle platooning system model, a linear model of the multi-unmanned vehicle platooning system is obtained; Based on the linear model of the multi-unmanned vehicle platooning system, the dynamic obstacle avoidance path planning model is obtained.

7. The unmanned vehicle formation planning and obstacle avoidance method according to claim 6, characterized in that: Based on the dynamic obstacle avoidance path planning model, the backstepping sliding mode algorithm is adopted to obtain the formation tracking control model, including: Acquire virtual control parameters, and based on the first-layer sliding mode function and the second-layer sliding mode function, adopt the backstepping sliding mode algorithm to obtain the actual position tracking control model and the formation tracking control model.

Images