Robot formation control method based on double-layer nonlinear model predictive control

Abstract

The invention discloses a robot formation control method based on double-layer nonlinear model predictive control, and relates to the technical field of model predictive control. In order to solve the defects that in the prior art, a pilot vehicle is excessively dependent, and a navigator is difficult to replace quickly during communication packet loss or fault, the technical scheme provided by the invention comprises the following steps: collecting a reference track and initializing control parameters and initial poses of all vehicles; controlling a pilot vehicle according to the reference trajectory and generating an expected formation matrix and an orientation compensation angle; converting the expected formation matrix and the orientation compensation angle into a global target pose of the following vehicle; controlling the following vehicle to perform nonlinear model prediction tracking based on the global target pose of the following vehicle; and when detecting that the pilot vehicle is abnormal, replacing a new pilot vehicle and updating the formation. The method is suitable for unmanned vehicle or multi-robot formation control tasks requiring high-reliability, low-delay switching and real-time formation keeping in a complex dynamic environment.

Description

Technical Field

[0001] The present invention relates to the technical field of model predictive control, and in particular to robot formation control based on double-layer nonlinear model predictive control. Background Art

[0002] With the rapid development of artificial intelligence and autonomous driving technologies, unmanned vehicle control technology has been widely applied in various fields, including intelligent transportation, logistics distribution, and military affairs. However, facing increasingly complex and changing tasks and environments, a single unmanned vehicle is no longer able to meet the requirements of the task. A platoon system of multiple unmanned vehicles is needed to coordinate and complete the task and improve the efficiency of mission execution.

[0003] Unmanned vehicle platooning control technology has demonstrated significant application value in intelligent transportation, logistics distribution, and military applications. In intelligent transportation, the coordinated platooning of multiple unmanned vehicles optimizes road resource utilization, improves traffic flow, and reduces congestion. In large warehouses, ports, and distribution centers, unmanned vehicle platoons leverage spatial layout and functional complementarity to coordinate cargo handling, achieving efficient cargo transfer, improving logistics scheduling, and enhancing overall operational effectiveness. In scenarios such as outdoor material transportation and agricultural automation, unmanned vehicle platoons can effectively implement large-scale operations and improve resource utilization. In the defense and military sectors, unmanned ground vehicles are used for reconnaissance, combat resupply, and casualty rescue missions. The coordinated operation of multiple unmanned vehicles significantly improves mission efficiency, enhances tactical flexibility, and effectively reduces the risk of personnel exposure in hazardous environments. While unmanned vehicle platooning systems address the perception and action limitations of individual unmanned vehicles, the system's algorithms still face numerous challenges. Ensuring the stability and accuracy of a formation maintained by multiple unmanned vehicles in a dynamic environment is a critical issue. Furthermore, communication delays and packet loss between vehicles can lead to inconsistent information, impacting the stability of the overall formation. Consequently, unmanned vehicle platooning control technology has garnered widespread attention both domestically and internationally.

[0004] Research on control algorithms for unmanned vehicle platooning not only helps drive technological innovation in intelligent transportation and logistics but also expands its application in fields such as military and security, laying a solid foundation for building a more intelligent and automated social system. However, uncertainties such as unstructured roads, dynamic obstacles, communication latency, and changes in platoon topology remain major challenges for unmanned vehicle platooning control systems.

[0005] In summary, the existing technology has the defects of over-reliance on the pilot vehicle, difficulty in quickly replacing the pilot vehicle when communication packets are lost or malfunctions, and lack of efficient, distributed nonlinear predictive control in large-scale formations or dynamic topology changes. Summary of the Invention

[0006] To address the shortcomings of existing technologies, such as over-reliance on the pilot vehicle, difficulty in quickly replacing the pilot vehicle in the event of communication packet loss or failure, and lack of efficient, distributed nonlinear predictive control in large-scale formations or dynamic topology changes, the present invention provides the following technical solutions: A robot formation control method based on double-layer nonlinear model predictive control, comprising: The steps of collecting reference trajectories and initializing the control parameters and initial poses of all vehicles; The step of controlling the lead vehicle according to the reference trajectory and generating a desired formation matrix and a heading compensation angle; The step of converting the desired formation matrix and the heading compensation angle into a global target pose of the following vehicle; The step of controlling the following vehicle to perform nonlinear model prediction tracking based on the global target posture of the following vehicle; When an abnormality is detected in the lead vehicle, a new lead vehicle is replaced and the formation is updated.

[0007] Furthermore, a preferred embodiment is provided, wherein the reference trajectory is generated by a path planning algorithm or imported from a preset path.

[0008] Furthermore, a preferred embodiment is provided, in which the process of generating the desired formation matrix includes rotating, scaling and translating the preset desired relative posture matrix according to the current posture of the pilot vehicle.

[0009] Furthermore, a preferred embodiment is provided in which the heading compensation angle is calculated by the relative position of each following vehicle in the pilot coordinate system and the turning radius of the pilot vehicle.

[0010] Furthermore, a preferred embodiment is provided, in which the global target posture is obtained by combining a preset desired relative posture with a heading compensation angle and performing a coordinate transformation based on the posture of the pilot vehicle.

[0011] Furthermore, a preferred embodiment is provided in which the following vehicle calculates the control input using a nonlinear model predictive control method of rolling optimization and selects the optimal control quantity for execution.

[0012] A robot formation control device based on a double-layer nonlinear model predictive control is also provided, comprising: A module that collects reference trajectories and initializes the control parameters and initial poses of all vehicles; A module for controlling the pilot vehicle according to the reference trajectory and generating a desired formation matrix and a heading compensation angle; A module for converting the desired formation matrix and the heading compensation angle into a global target pose of the following vehicle; A module for controlling the following vehicle to perform nonlinear model prediction tracking based on the global target posture of the following vehicle; A module that replaces a new lead vehicle and updates the formation when an abnormality is detected in the lead vehicle.

[0013] A computer storage medium is also provided for storing a computer program, and when the computer program is read by a computer, the computer executes the method.

[0014] A computer is also provided, comprising a processor and a storage medium, wherein when the processor reads a computer program stored in the storage medium, the computer executes the method.

[0015] A computer program product is also provided, which is a computer program that implements the method when the computer program is executed.

[0016] Compared with the prior art, the technical solution provided by the present invention is beneficial in that: The two-layer NMPC control framework achieves the ability to distribute optimization solutions in large-scale formations by having the lead vehicle and follower vehicles run predictive controllers independently. This effectively avoids the exponential growth of computational complexity caused by the increase in the number of vehicles in centralized methods. Compared with existing centralized NMPC formation control, the system responds faster and has higher real-time performance.

[0017] The leader replacement mechanism quickly selects the optimal follower to take over the leader role by combining distance evaluation and speed stability evaluation indicators when communication is lost or fails, ensuring that the formation can seamlessly switch and maintain the original formation structure after the core node fails. This is in stark contrast to the traditional leader-follower strategy, which directly leads to the dissolution of the formation in the event of a failure.

[0018] The formation matrix and its homogeneous and heterogeneous transformation strategies enable the formation to flexibly adapt to changes in the spatial environment by defining the expected posture of the formation in the leader coordinate system and supporting translation, rotation, scaling and formation reconstruction. The efficiency of formation adjustment in narrow channels or open areas is significantly better than the virtual structure method that only relies on local behavior rules.

[0019] The desired heading angle compensation module dynamically adjusts the desired heading of each vehicle based on a non-holonomic constraint model combined with the leader's turning radius and the follower's relative position. This effectively eliminates the trajectory unevenness and formation drift caused by the differential drive characteristics. The smoothness and stability of the formation motion trajectory are significantly higher than traditional methods that do not use heading compensation.

[0020] It is suitable for unmanned vehicle or multi-robot formation control tasks that require high reliability, low-latency switching and real-time formation maintenance in complex dynamic environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0021] Figure 1 This is a schematic diagram of the leader-follower strategy structure; Figure 2 Schematic diagram of three isomorphic formation transformations; Among them, the left side is translation transformation, the middle is rotation transformation, and the right side is scaling transformation; Figure 3 Schematic diagram of two heterogeneous formation transformations; Among them, the left side is horizontal transformation, and the right side is vertical transformation; Figure 4 This is a schematic diagram of the triangular formation when driving; Figure 5 Schematic diagram of the two formation trajectories; The left side shows the formation trajectory after compensation, and the right side shows the formation trajectory without compensation. Leader represents the actual motion trajectory of the leader vehicle, follower1 represents the motion trajectory of the first following vehicle, and follower2 represents the motion trajectory of the second following vehicle. Figure 6 Replacement mechanism flow chart for the navigator; Figure 7 Schematic diagram of the double-layer NMPC formation control framework; Figure 8 This is the flow chart of the formation control algorithm; Figure 9 This is the experimental schematic diagram of the linear motion trajectory of the triangle formation; Figure 10 Schematic diagram of the experiment of the circular motion trajectory of the triangular formation. DETAILED DESCRIPTION

[0022] In order to make the advantages and benefits of the technical solution provided by the present invention more clearly reflected, the technical solution provided by the present invention is now further described in detail with reference to the accompanying drawings, specifically: Implementation 1: This implementation provides a robot formation control method based on a two-layer nonlinear model predictive control, including: The steps of collecting reference trajectories and initializing the control parameters and initial poses of all vehicles; The step of controlling the lead vehicle according to the reference trajectory and generating a desired formation matrix and a heading compensation angle; The step of converting the desired formation matrix and the heading compensation angle into a global target pose of the following vehicle; The step of controlling the following vehicle to perform nonlinear model prediction tracking based on the global target posture of the following vehicle; When an abnormality is detected in the lead vehicle, a new lead vehicle is replaced and the formation is updated.

[0023] The reference trajectory is generated by a path planning algorithm or imported from a preset path.

[0024] The process of generating the desired formation matrix includes rotating, scaling, and translating the preset desired relative pose matrix according to the current pose of the pilot vehicle.

[0025] The heading compensation angle is calculated based on the relative position of each following vehicle in the pilot coordinate system and the turning radius of the pilot vehicle.

[0026] The global target pose is obtained by combining the preset desired relative pose with the heading compensation angle and performing coordinate transformation based on the pose of the pilot vehicle.

[0027] The following vehicle uses a nonlinear model predictive control method with rolling optimization to calculate the control input and select the optimal control quantity for execution.

[0028] Implementation Method 2: This implementation method further describes the technical solution provided in Implementation Method 1 in detail. Specifically: Step 1: Generate reference trajectory and initialize vehicle state and controller parameters; This system can generate a reference trajectory that meets the kinematic characteristics of the unmanned vehicle through a path planning algorithm, which can also be pre-set manually; upload the reference trajectory to the pilot vehicle control unit; and configure controller parameters such as the initial pose, sampling period, NMPC prediction step number, and cost function weight for the pilot vehicle and all following vehicles to ensure that subsequent prediction optimization has a basis to follow.

[0029] Step 2: The leader vehicle tracks the reference trajectory based on NMPC and updates the desired formation matrix; The lead vehicle obtains its current position and heading in real time, and uses the nonlinear model predictive control method to perform rolling optimization of the motion trajectory within multiple future sampling periods; on this basis, combined with the preset formation matrix and the required formation transformation method (translation, rotation, scaling or reconstruction), it calculates and updates the current expected formation matrix, and outputs the expected position matrix and compensation orientation information at this moment.

[0030] Step 3: Calculate and compensate the desired orientation of the following vehicle to eliminate turning deviation; Based on the linear and angular velocity of the lead vehicle, as well as the relative positions of the following vehicles in the leader's coordinate system, the turning radius is dynamically calculated and a compensation angle is calculated for each following vehicle. This compensation angle is then added to the relative orientation of each following vehicle to obtain the final desired orientation of each following vehicle, ensuring a smooth and consistent formation during the turn.

[0031] Step 4: Generate the expected position of the following vehicle in the global coordinate system and send it; The compensated relative pose is transformed with the updated desired formation matrix and the global pose of the leader vehicle to obtain the desired horizontal and vertical coordinates and orientation of each following vehicle in the global coordinate system; the corresponding desired pose is broadcast to the control units of each following vehicle in real time through the on-board communication network.

[0032] Step 5: The following vehicle independently runs the NMPC controller to achieve desired posture tracking; Each following vehicle uses the received desired posture as the target and the current actual posture as the starting point, and adopts the nonlinear model predictive control method to predict and optimize the state and control input within multiple future sampling periods. The posture error and control energy consumption are comprehensively considered in the cost function to solve the optimal control sequence, and the first control instruction in the sequence is sent to the actuator. After execution, the new actual posture is obtained in real time, and the next round of prediction optimization is entered.

[0033] Step 6: Monitor the pilot vehicle status and replace the pilot in case of failure; The system periodically monitors the communication signals and motion status of the leader vehicle. If no signal is received from the leader vehicle within multiple consecutive control cycles or its speed and acceleration fluctuations exceed the threshold, the leader replacement mechanism is triggered. All following vehicles broadcast their current position and speed stability indicators. The system calculates the total evaluation index based on the weighted distance evaluation and speed stability evaluation, and selects the following vehicle with the highest score as the new leader. After the role switch between the old and new leaders is completed, the formation matrix is ​​updated and the vehicle IDs are reallocated to ensure that the formation continues to operate smoothly in the event of a failure.

[0034] Implementation Method 3: Combination Figure 1-10 This embodiment further describes the above technical solution in detail through specific examples, specifically: The leader-follower strategy is a commonly used formation control method. The leader-follower strategy of the unmanned vehicle formation can achieve coordinated movement between unmanned vehicles. Under the leadership of the leader unmanned vehicle, the entire formation can complete tasks more efficiently and improve work efficiency. The leader is controlled by an external system, sets the movement strategy and target setting of the formation, and is responsible for executing the overall planning and setting the movement strategy and target of the formation. The following unmanned vehicle adjusts its own position and speed to maintain the formation structure according to the formation target position set by the leader and the predetermined formation maintenance rules. The leader unmanned vehicle and the following unmanned vehicle exchange information through the communication system to ensure the consistency of the movement of the entire formation. The leader-follower strategy structure is as follows: Figure 1 .

[0035] In order to enhance the adaptability of the formation, this implementation proposes a formation matrix method based on the leader-follower strategy, which enables the formation system to be flexibly expanded and adjusted in size. At the same time, a control algorithm is used to ensure that each unmanned vehicle is always in the desired position.

[0036] Assume that in the two-dimensional plane, in the global coordinate system, the position of the navigator is: (1) in, 、 The horizontal and vertical coordinates of the navigator respectively, θ L is the heading angle of the pilot.

[0037] In the leader's coordinate system, the follower's position, that is, the relative position, is: (2) in, 、 、 are the horizontal and vertical coordinates and heading angle of the follower relative to the leader.

[0038] Convert the relative coordinate system to the global coordinate system using the rotation matrix W : (3) Therefore, in the global coordinate system, the follower's posture It can be expressed as: (4) In order to reflect the relative position relationship between unmanned vehicles in the formation, the formation matrix is ​​introduced S , describes the expected position of the unmanned vehicle in the formation in the navigator's coordinate system. From formula (2), we can see that is the position in the navigator's coordinate system, and the formation contains n followers, then define the formation matrix S for: (5) in represent i The relative pose of each follower in the leader's coordinate system.

[0039] Combined rotation matrix W , the follower's global coordinates are obtained by matrix calculation: (6) After the formula is expanded: (7) Through this formation matrix, formation expansion and formation change can also be achieved. The following will introduce two formation transformation strategies: homogeneous transformation and heterogeneous transformation.

[0040] 1) Isomorphic transformation Assume the original formation matrix is S old , the target formation matrix is S new ,but: (8) in, T is the transformation matrix, which is used to realize the translation, rotation and scaling of the formation.

[0041] In actual application scenarios, when the unmanned vehicle formation needs to change its direction of travel while maintaining the relative position relationship between the unmanned vehicles, this can be achieved through the rotation transformation strategy. The rotation transformation is: (9) in, α is the rotation angle.

[0042] The unmanned vehicle formation adjusts its size based on the size of the surrounding space. When the formation enters a wider area, it expands its formation through scaling transformation to increase the coverage area; when approaching a narrow passage, it shrinks its formation to ensure safe passage. The scaling transformation is: (10) in, k 1 and k 2 are the scaling factors of the horizontal and vertical axes respectively.

[0043] When the unmanned vehicle formation moves from one area to another, the overall position of the formation is adjusted through translation transformation while keeping the formation unchanged. The translation transformation is: (11) in, is the translation amount.

[0044] 2) Heterogeneous transformation Set up in formation N Unmanned vehicles, the initial formation matrix is S old , the target formation matrix is S new ,but: (12) Among them, the transformation function f It can be expressed as the reconstruction of the unmanned vehicle fleet or changing the number of unmanned vehicles in the formation.

[0045] When the unmanned vehicle formation needs to switch from a dense formation to a dispersed search formation, some unmanned vehicles change their own movement paths and rebuild a new formation structure to meet the needs of the search mission.

[0046] Unlike omnidirectional vehicles, differential-drive autonomous vehicles (DRVs) are subject to nonholonomic constraints. Consider a platoon of autonomous vehicles as a rigid structure. While the angular velocity of each point on the structure remains constant during circular motion, the linear velocities vary in magnitude and direction. When a platoon of differential-drive autonomous vehicles must maintain a specific formation and make turns, the perpendicular lines of motion of each vehicle ultimately converge on the same center of rotation. Consequently, there is a certain deviation between the desired heading angle of each following vehicle and the final target heading of the platoon as a whole.

[0047] In order to ensure that the entire formation can complete the turning movement stably, it is necessary to compensate for the deviation by dynamically adjusting the desired heading angle of each following vehicle to eliminate the impact of the deviation on the stability of the formation. The formation maintains the formation movement, and the overall angular velocity is the same, that is, the angular velocity of each unmanned vehicle is the same. It is known that the linear velocity of the leader unmanned vehicle is , the angular velocity is , then the turning radius of the pilot is for: (13) In the leader coordinate system in the two-dimensional plane, the follower's position is ,in Taking the triangle formation as an example, Figure 4 shown.

[0048] Compensation angle for: (14) Therefore, the expected angle after follower compensation is for: (15) Then the new formation matrix is ​​obtained as: (16) Then, the nonlinear model predictive control method is used to control the unmanned vehicle from its current position to the desired position after dynamic heading angle compensation.

[0049] Figure 5The trajectories of the formation before and after compensation are shown. Because the unmanned vehicle is a non-holonomic structure, when there is lateral error and the difference between the current heading angle and the desired heading angle is small, the unmanned vehicle needs to oscillate back and forth to reach the desired position, resulting in an uneven trajectory and unstable formation. After compensating for the desired heading angle, the formation trajectory is significantly improved, and the formation can maintain stable operation and a smooth trajectory.

[0050] While the pilot-follower strategy is a widely used platooning strategy, it suffers from a serious flaw in that the stability and operation of the entire platooning system is overly dependent on the pilot. If the pilot vehicle loses communication or experiences an operational failure, the entire platooning system loses its core reference point and cannot maintain overall platoon operation, leading to serious consequences such as platoon dissolution and vehicle collisions.

[0051] To address this issue, this implementation proposes a leader replacement mechanism. When the leader loses communication or fails, the formation system can quickly select a new unmanned vehicle from the follower queue as the leader to lead the entire formation to continue operating. When selecting a new leader, it is necessary to select the unmanned vehicle closest to the original leader to meet the requirements of quickly replacing the leader, reduce the formation adjustment time, better maintain the original formation structure, and reduce the complexity of formation reorganization; at the same time, the new leader must also meet the requirements of speed and acceleration stability. Unstable speed and acceleration will cause large errors when the followers update the formation matrix, causing unstable follower movement, which may lead to serious consequences such as collisions. For example, if the new leader suddenly accelerates or brakes suddenly, the followers may be unable to react in time and cause a rear-end collision.

[0052] The navigator replacement mechanism needs to consider not only the distance factor, but also the motion status of the new navigator to ensure that the formation system can continue to operate safely and stably when the navigator fails.

[0053] Select the follower closest to the original leader as the new leader and calculate the distance evaluation index: (17) in, It is i The position of the follower, It is the original navigator position.

[0054] At the same time, we need to select the one with more stable speed and acceleration as the new leader and calculate the speed stability evaluation index: (18) in, v and are the linear velocity and angular velocity of the follower, respectively.

[0055] The total evaluation index is obtained by weighting the distance evaluation index and the speed stability evaluation index: (19) in, a 、 b are the coefficients of distance evaluation index and speed stability evaluation index respectively.

[0056] During the initialization phase of the platoon system, each unmanned vehicle is uniquely assigned an ID. The leader is assigned initial ID 0, and followers are numbered in order of operation. When the leader is operating normally, it periodically broadcasts status signals to other members of the platoon. The system uses these signals to monitor the leader's status in real time. If the system fails to receive a leader signal for multiple consecutive cycles, the leader is deemed disconnected, triggering an election mechanism. All followers immediately initiate the leader replacement process: 1) The follower broadcasts its current position and velocity stability; 2) Based on the preset evaluation indicators, the best followers are selected as the new leaders; 3) Update the formation matrix, complete the leader replacement process, and the formation operates normally.

[0057] After the new navigator takes over control, the formation system reallocates roles through IDs, while the positions of other unmanned vehicles remain unchanged to ensure the stability of the formation system structure. If the original navigator's fault is resolved, it will rejoin the formation network. After the system detects the signal, it will be set to the last ID and inherit the original mission parameters. Figure 6 The pilot failure mechanism flow is shown.

[0058] This two-layer nonlinear model predictive controller, based on a leader-follower strategy, consists of a two-layer structure. The upper-layer control is performed by the leader vehicle, which is responsible for tracking the entire formation trajectory. The upper-layer leader vehicle first determines a reference trajectory that satisfies the kinematic characteristics of the unmanned vehicle. This reference trajectory can be generated by a path planning algorithm or manually pre-set. The leader vehicle then uses nonlinear model predictive control (NMPC) to accurately follow this reference trajectory and updates its own position and posture in real time. Finally, using the set formation matrix and desired heading angle compensation information, the leader vehicle calculates the desired position and posture for each follower vehicle in the formation in real time.

[0059] In the lower-level control structure, the following vehicles serve as the primary control objects. Each vehicle independently receives the desired position and posture transmitted by the upper-level pilot vehicle and is responsible for accurately reaching the desired position and posture. The following vehicles combine the real-time position and posture information of the pilot vehicle and the formation matrix to calculate their respective current desired target states. They also use the NMPC method to accurately achieve dynamic tracking of the position and posture. Each following vehicle independently runs its own NMPC controller, using its current state as the starting point in real time to perform rolling optimization predictions of the motion trajectory within multiple time steps in the future, and selects the first step in the optimal control sequence obtained by the optimization calculation as the execution instruction. After executing this instruction, the following vehicle quickly obtains its actual position and posture information, feeds it back to the controller, and continues the next round of prediction optimization iterations to achieve continuous and accurate tracking of the motion trajectory, so that the trajectory tracking error gradually converges dynamically.

[0060] The dual-layer control architecture designed in this study integrates a distributed structure, enabling each unmanned vehicle to have an independent NMPC control unit. Each unmanned vehicle no longer relies on a central node and can independently complete the planning and control of the target posture regardless of the number of unmanned vehicles in the formation. This not only makes the increase or decrease of the formation size more flexible, but also reduces the demand for hardware computing power for a single unmanned vehicle. Figure 7 shown.

[0061] The follower's motion model is the same as the navigator's, and its steps are similar to those of the navigator, but their purposes are different. The navigator is used to follow the reference trajectory, while the follower is used to track the desired posture. This implementation uses the navigator as an example and adopts a four-wheel differential unmanned vehicle model, which is a non-holonomic constraint model. The unmanned vehicle kinematic equation is: (20) in, is the sampling period, is the control input, is the posture state of the unmanned vehicle, is a nonlinear function describing the kinematics of the system.

[0062] The core of the algorithm is to predict the system behavior in the future by building a system model and calculating the best control input through optimization algorithm. step, expressed as: (twenty one) in, and In discrete time k The pose state and control input, is the posture state at the next time.

[0063] At each control moment, the current system state and model are used to predict the system behavior in the future. In discrete time systems, the corresponding future In each control cycle, the discretized nonlinear model is used to predict the future state based on the current state of the unmanned vehicle. The state of the time step is: (twenty two) In order to make the predicted trajectory safe and as close as possible to the reference state while limiting the control input, it is necessary to set an appropriate cost function and then solve the function optimization problem to calculate the optimal control input so that the system can achieve the expected performance target in the future time period.

[0064] Set the cost function based on the state error between the current position of the unmanned vehicle and the reference trajectory, as well as the future state predicted by the current position of the unmanned vehicle and the current control input. for: (twenty three) in, is the posture state of the unmanned vehicle, , is the reference trajectory, For the stage cost, For the terminal price. Expressed as: (twenty four) in, represents the Euclidean norm, Is the weighted parameter of the control input. Converted into matrix quadratic form: (25) in, Q is a positive definite weight matrix, which adjusts the state error weight, R is a semi-positive definite weight matrix that adjusts the control input weights, To predict i The control input of the unmanned vehicle when walking, To predict i The tracking error of the unmanned vehicle is expressed as: (26) Terminal price for: (27) in, H It is a positive definite weight matrix that adjusts the error weight of the terminal state of the unmanned vehicle.

[0065] Determine the optimal control sequence by minimizing the cost function : (28) in, is a constant, representing the range of the unmanned vehicle's linear velocity and angular velocity. The hard constraints are the unmanned vehicle's kinematic model constraints and control input constraints.

[0066] For all possible control sequences Optimize. Found the optimal control sequence for: (29) Then the control input As the best feedback at the current point in time.

[0067] The optimized control input is applied to the system and the system state is updated. At the next control moment, the above process is repeated and the control input is recalculated using the new system state and prediction information. This cost function comprehensively considers the trajectory tracking accuracy and control energy consumption. The minimized cost function value obtained can not only ensure the high-precision tracking of the unmanned vehicle, but also effectively suppress the violent fluctuations of the control input. The optimal control law obtained by this control method can ensure the good overall control performance of the unmanned vehicle. Figure 8 The following is the flow chart of the formation algorithm.

[0068] In the experiment, straight line formation driving and circular trajectory driving were carried out respectively, such as Figure 9 and 10 As shown, the achieved effect is consistent with expectations.

[0069] The above further describes the technical solution provided by the present invention in detail through several specific embodiments in order to highlight the advantages and benefits of the technical solution provided by the present invention. However, the several specific embodiments described above are not intended to limit the present invention. Any reasonable modification and improvement of the present invention, combination of embodiments and equivalent replacement based on the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. A robot formation control method based on double-layer nonlinear model predictive control, characterized in that: include: The steps of collecting reference trajectories and initializing the control parameters and initial poses of all vehicles; The step of controlling the lead vehicle according to the reference trajectory and generating a desired formation matrix and a heading compensation angle; The step of converting the desired formation matrix and the heading compensation angle into a global target pose of the following vehicle; The step of controlling the following vehicle to perform nonlinear model prediction tracking based on the global target posture of the following vehicle; When an abnormality is detected in the lead vehicle, a new lead vehicle is replaced and the formation is updated.

2. The robot formation control method based on double-layer nonlinear model predictive control according to claim 1, characterized in that: The reference trajectory is generated by a path planning algorithm or imported from a preset path.

3. The robot formation control method based on double-layer nonlinear model predictive control according to claim 1, characterized in that: The process of generating the desired formation matrix includes rotating, scaling, and translating the preset desired relative pose matrix according to the current pose of the pilot vehicle.

4. The robot formation control method based on double-layer nonlinear model predictive control according to claim 1, characterized in that: The heading compensation angle is calculated based on the relative position of each following vehicle in the pilot coordinate system and the turning radius of the pilot vehicle.

5. The robot formation control method based on double-layer nonlinear model predictive control according to claim 1, characterized in that: The global target pose is obtained by combining the preset desired relative pose with the heading compensation angle and performing coordinate transformation based on the pose of the pilot vehicle.

6. The robot formation control method based on double-layer nonlinear model predictive control according to claim 1, characterized in that: The following vehicle uses a nonlinear model predictive control method with rolling optimization to calculate the control input and select the optimal control quantity for execution.

7. A robot formation control device based on double-layer nonlinear model predictive control, characterized in that: include: A module that collects reference trajectories and initializes the control parameters and initial poses of all vehicles; A module for controlling the pilot vehicle according to the reference trajectory and generating a desired formation matrix and a heading compensation angle; A module for converting the desired formation matrix and the heading compensation angle into a global target pose of the following vehicle; A module for controlling the following vehicle to perform nonlinear model prediction tracking based on the global target posture of the following vehicle; A module that replaces a new lead vehicle and updates the formation when an abnormality is detected in the lead vehicle.

8. A computer storage medium for storing a computer program, characterized in that When the computer program is read by a computer, the computer executes the method according to claim 1 .

9. A computer comprising a processor and a storage medium, characterized in that When the processor reads the computer program stored in the storage medium, the computer executes the method according to claim 1 .

10. A computer program product, being a computer program, characterized in that When the computer program is executed, the method according to claim 1 is implemented.

Images