High-precision wheeled robot trajectory tracking cooperative control system

Abstract

The invention discloses a high-precision wheeled robot trajectory tracking cooperative control system, and the system comprises a control module which is used for generating a control signal according to a trajectory tracking error; the multi-robot cooperation module is used for realizing task distribution and real-time communication among a plurality of robots; the sensing and positioning module is used for fusing multi-sensor data to realize real-time positioning; the electrical optimization module is used for performing motor energy management and energy recovery; and the path planning and dynamic obstacle avoidance module is used for generating an obstacle avoidance path and adjusting a global path. According to the invention, through a sliding mode control algorithm, a distributed cooperation mechanism and an energy management technology, high-precision trajectory tracking, efficient task cooperation and energy utilization optimization are realized, and the system performance and robustness are improved.

Description

Technical Field

[0001] The present invention relates to the technical field of robot trajectory control, and in particular to a high-precision wheeled robot trajectory tracking collaborative control system. Background Art

[0002] Wheeled robot trajectory tracking collaborative control systems are widely used in industry, logistics, agriculture and other fields. Their core goal is to enable robot groups to complete efficient and accurate trajectory tracking tasks in complex environments through control algorithms and collaborative mechanisms.

[0003] Existing wheeled robot trajectory tracking collaborative control systems mostly use PID control and collaborative algorithms based on fuzzy logic, and achieve high trajectory tracking accuracy through simple trajectory error compensation. In addition, these systems can complete basic task collaboration with a small number of robots through fixed priority task allocation. At the same time, combined with low-complexity path planning algorithms, the computational burden of the system is reduced, and good reliability and execution efficiency are shown in low-dynamic environments.

[0004] However, the existing wheeled robot trajectory tracking collaborative control system still has the following shortcomings. First, the adaptability and robustness in high dynamic environments are poor, and the control algorithm is difficult to effectively deal with the interference of complex path errors and dynamic obstacles. Second, the task allocation mechanism is prone to resource conflicts and insufficient global optimization when the number of robots increases or the complexity of tasks increases. In addition, the energy management technology of the existing system has not yet achieved efficient energy recovery and battery balancing, resulting in short robot operating time and low energy utilization. Summary of the invention

[0005] The present invention provides a high-precision wheeled robot trajectory tracking collaborative control system with better adaptability to dynamic environments, higher multi-robot task collaboration efficiency, and stronger energy management and endurance capabilities, which can solve at least one of the above-mentioned technical problems.

[0006] In order to solve the above technical problems, the present invention adopts the following technical solution: a high-precision wheeled robot trajectory tracking collaborative control system, comprising:

[0007] A control module, used to generate a control signal according to a trajectory tracking error, the control module comprising a sliding mode control and gain adjustment unit, a trajectory error calculation unit and a control signal output unit;

[0008] A multi-robot collaboration module, used to realize task allocation and real-time communication among multiple robots, the multi-robot collaboration module includes a distributed task allocation unit, a time synchronization unit and a collaborative optimization and communication unit;

[0009] A sensing and positioning module is used to fuse multi-sensor data to achieve real-time positioning. The sensing and positioning module includes a multi-sensor data acquisition unit, an extended Kalman filter algorithm unit and a real-time positioning unit;

[0010] An electrical optimization module, used for motor energy management and energy recovery, the electrical optimization module comprising an energy recovery unit, a battery balancing unit and a battery management and energy distribution unit;

[0011] The path planning and dynamic obstacle avoidance module is used to generate an obstacle avoidance path and adjust the global path. The path planning and dynamic obstacle avoidance module includes a dynamic obstacle detection unit, a nonlinear optimization path planning unit and a path adjustment and obstacle avoidance execution unit.

[0012] Furthermore, the control module adopts the sliding mode control and gain adjustment unit to combine the sliding mode control with the adaptive gain adjustment to form a control algorithm, and the control algorithm includes the following steps:

[0013] S1.1, the trajectory error calculation unit calculates the trajectory error:

[0014] e x =x d -x,e y =y d -y,e θ =θ d -θ

[0015] Among them, x, y and θ are the current positions, x d ,y d and θ d is the desired position;

[0016] S1.2. Define the sliding surface:

[0017]

[0018] Among them, k v and k ω All are gain parameters;

[0019] S1.3, the control signal output unit control law output:

[0020] u=-(K c +K a |s|)tanh(s)

[0021] Among them, k c is a fixed gain, k a are all dynamic gain adjustment coefficients, and S is the absolute value of the sliding surface.

[0022] Furthermore, the multi-robot collaboration module optimizes the distributed task allocation unit and the time synchronization unit in a collaborative manner, and the distributed task allocation includes the following steps:

[0023] S2.a1. Establishing the task benefit function:

[0024]

[0025] in:

[0026] R i : represents the i-th robot in the robot set, i = 1, 2, ..., n;

[0027] T j : represents the jth task in the task set, j = 1, 2, ..., m;

[0028] d ij :Represents robot R i To Task T j The physical distance in meters;

[0029] ε: a small positive number used to avoid the problem of the denominator being zero;

[0030] α: Energy consumption weight factor, used to adjust the priority ratio between task distance and energy consumption;

[0031] E ij :Represents robot R i Execute Task T j The estimated energy required, in joules;

[0032] S2.a2, build optimization goals:

[0033]

[0034] in:

[0035] max: indicates that the optimization goal is to maximize the global task benefit, that is, to maximize the total benefit of the robot performing the task;

[0036] For all robots R i and Task T j The system comprehensively considers the matching relationship between all robots and tasks.

[0037] x ij : represents a binary decision variable used to determine the robot R i Whether to assign task T j ;

[0038] S2.a3, determine the constraints:

[0039]

[0040] Represents each robot R i Only one task can be executed at a time;

[0041] x ij ∈{0,1}: represents x ij It is a binary variable and must be 0 or 1, 0 means the robot does not perform the task, 1 means the robot performs the task;

[0042] S2.4, use distributed algorithm to solve task allocation in real time.

[0043] Furthermore, the multi-robot collaboration module realizes time synchronization through the IEEE1588PTP protocol, including the following steps:

[0044] S2.b1, realize the broadcast and reception of synchronization information through the master-slave clock structure;

[0045] S2.b2. Adjust the error of the slave clock according to the timestamp information of the master clock:

[0046]

[0047] Among them, Δ t is the time error, τ master Transmission delay from master clock to slave clock, τ slave is the transmission delay from the slave clock to the master clock;

[0048] S2.b3, Execution of time synchronization formula.

[0049] Furthermore, the sensing and positioning module adopts the extended Kalman filter algorithm unit to implement multi-sensor fusion positioning through the extended Kalman filter algorithm, including the following steps:

[0050] S3.1. Establish a state update model:

[0051] x k+1 =f(x k ,u k )+w k

[0052] Among them, x k is the system status, u k is the control input, w k is the process noise;

[0053] S3.2. Establish observation model:

[0054] z k =h(x k)+v k

[0055] Among them, z k is the observed value, v k is the observation noise;

[0056] S3.3. Update the system state through prediction and correction steps.

[0057] Furthermore, the electrical optimization module uses the energy recovery circuit in the energy recovery unit to achieve energy recovery and reuse through reverse power generation by a DC motor, including the following steps:

[0058] S4.a1, during braking or deceleration, the power is recovered by reverse power generation:

[0059] P rec =T brake ·ω brake

[0060] in:

[0061] P rec is the recovery power during braking, in watts, W;

[0062] T brake is the torque acting on the motor shaft during braking, in Newton-meters, N·m;

[0063] ω brake is the angular velocity of the motor when braking, in radians per second, rad / s;

[0064] S4.a2, store the recovered energy in the supercapacitor, and the output current is:

[0065]

[0066] in:

[0067] I rec is the recovery current, in ampere, A;

[0068] P rec is the recovered power, in watts, W;

[0069] V rec is the voltage of the energy storage device in volts, V.

[0070] Furthermore, the electrical optimization module uses the battery balancing circuit in the battery balancing unit to achieve voltage consistency, and the balancing condition is:

[0071] V i =V j , ∈Battery Pack

[0072] Among them, V i Indicates the real-time voltage of the ith single cell in the battery pack, in volts, V; V j Indicates the real-time voltage of the jth single cell in the battery pack, in volts, V; ∈ means that the formula is applicable to any two single cells in the battery pack, that is, the balancing management needs to be performed on all battery cells in the battery pack.

[0073] Furthermore, the motor dynamic control is achieved through closed-loop current feedback, including the following steps:

[0074] S4.b1, real-time acquisition of motor output current i;

[0075] S4.b2, adjust the input voltage V:

[0076]

[0077] Where V is the motor input voltage, K p is the proportional gain, i r is the reference current, i is the actual current, (i r -i) is the current error, K d is the differential gain, is the rate of change of current error;

[0078] S4.b3, Implementation of control formula:

[0079] S4.b31, real-time acquisition of reference current i r and the actual current i, calculate the current error: e i =i r -i;

[0080] S4.b32, according to the magnitude of the current error, proportional gain K p Generates the initial voltage input adjustment: V p =K p ·e i ;

[0081] S4.b33. Calculate the rate of change of current error over time:

[0082] S4.b34, according to the differential gain K d Generate the derivative compensation term:

[0083] S4.b35, superimpose the proportional and differential control quantities, and output the final motor drive voltage: V = V p +V d .

[0084] Furthermore, the path planning and dynamic obstacle avoidance module adopts the nonlinear optimization path planning unit and the path adjustment and obstacle avoidance execution unit to generate an obstacle avoidance path based on nonlinear optimization, including the following steps:

[0085] S5.1. Obtaining the position and speed of an obstacle based on the dynamic obstacle detection unit;

[0086] S5.2. Construct obstacle avoidance path planning objective function:

[0087] min∫0 T ∥x(t)-x obstacle (t)∥ 2 dt

[0088] in:

[0089] T: time range of path planning;

[0090] x(t): The current position vector of the robot at time t, expressed in two-dimensional coordinates (x, y);

[0091] x obstacle : The position vector (x o ,y o ), acquired by sensors in real time;

[0092] ∥x(t)-x obstacle (t)∥ 2 : The squared Euclidean distance between the robot and the obstacle, used to measure the safety of the obstacle avoidance path;

[0093] Integrate the distance over the entire time range to ensure the overall optimality of path planning;

[0094] S5.3. Determine the path planning constraints as follows:

[0095] v≤v max ,a≤a max

[0096] Where v is the current linear velocity of the robot, v max is the maximum limit of the robot's linear velocity, which is determined by the hardware and task requirements, a is the robot's current acceleration, and a max This is the maximum limit of the robot's acceleration, which is used to avoid power system overload or safety problems caused by excessive acceleration.

[0097] Furthermore, the high-precision wheeled robot trajectory tracking collaborative control system has a fault-tolerant mechanism, which automatically switches to a backup communication channel when communication fails, and enters a single-machine independent mode to complete the trajectory tracking task.

[0098] The beneficial effects of the present invention are embodied in:

[0099] 1. The present invention realizes precise control of trajectory tracking error by combining sliding mode control with adaptive gain adjustment algorithm, and improves the accuracy of multi-sensor data fusion positioning by combining extended Kalman filter technology, and has excellent path following ability and anti-disturbance performance in dynamic and complex environments.

[0100] 2. The system provided by the present invention realizes the optimal task allocation with global benefit maximization through distributed task allocation and time synchronization mechanism, and combines energy consumption weight adjustment and dynamic task adjustment to improve the efficiency of multi-robot collaboration, while reducing resource conflicts and energy waste.

[0101] 3. The present invention realizes effective recovery of braking energy and consistent regulation of voltage between single cells through energy recovery circuit and battery balancing management, thereby improving energy utilization and system endurance performance, and extending the service life of the battery pack. BRIEF DESCRIPTION OF THE DRAWINGS

[0102] The drawings described herein are used to provide further understanding of the present application and constitute a part of the present application. The illustrative embodiments of the present application and their descriptions are used to explain the present application and do not constitute improper limitations on the present application.

[0103] Figure 1 It is a schematic diagram of the overall structure of the system of an embodiment of the present invention.

[0104] Figure 2 It is a structural block diagram of a computer device according to an embodiment of the present invention.

[0105] The components in the accompanying drawings are marked as follows: 1. Control module; 2. Multi-robot collaboration module; 3. Sensing and positioning module; 4. Electrical optimization module; 5. Path planning and dynamic obstacle avoidance module; 101. Sliding mode control and gain adjustment unit; 102. Trajectory error calculation unit; 103. Control signal output unit; 201. Distributed task allocation unit; 202. Time synchronization unit; 203. Collaborative optimization and communication unit; 301. Multi-sensor data acquisition unit; 302. Extended Kalman filter algorithm unit; 303. Real-time positioning unit; 401. Energy recovery unit; 402. Battery balancing unit; 403. Battery management and energy distribution unit; 501. Dynamic obstacle detection unit; 502. Nonlinear optimization path planning unit; 503. Path adjustment and obstacle avoidance execution unit. DETAILED DESCRIPTION

[0106] The technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, rather than all of the embodiments. In the absence of conflict, the embodiments in this application and the features in the embodiments can be combined with each other. Based on the embodiments in the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention.

[0107] It should be noted that the meaning of "and / or" appearing in the full text includes three parallel solutions. Taking "A and / or B" as an example, it includes solution A, solution B, or solutions that satisfy both A and B. In addition, "multiple" means more than two. In addition, the technical solutions between the various embodiments can be combined with each other, but they must be based on the ability of ordinary technicians in this field to implement them. When the combination of technical solutions is contradictory or cannot be implemented, it should be deemed that such combination of technical solutions does not exist and is not within the scope of protection required by the present invention.

[0108] See also Figure 1 The embodiment of the present invention provides a high-precision wheeled robot trajectory tracking collaborative control system, comprising:

[0109] A control module 1 is used to generate a control signal according to a trajectory tracking error. The control module 1 includes a sliding mode control and gain adjustment unit 10, a trajectory error calculation unit 102 and a control signal output unit 103. The control module 1 improves the trajectory tracking accuracy by adjusting the control signal in real time, especially in a complex dynamic environment, effectively reducing path deviation and control chattering;

[0110] A multi-robot collaboration module 2 is used to realize task allocation and real-time communication among multiple robots. The multi-robot collaboration module 2 includes a distributed task allocation unit 201, a time synchronization unit 202, and a collaboration optimization and communication unit 203. The multi-robot collaboration module 2 improves collaboration efficiency in a multi-robot operation scenario, reduces task allocation conflicts, and enhances the robustness of the system through real-time communication and time synchronization;

[0111] The sensing and positioning module 3 is used to fuse multi-sensor data to achieve real-time positioning. The sensing and positioning module 3 includes a multi-sensor data acquisition unit 301, an extended Kalman filter algorithm unit 302 and a real-time positioning unit 303. The sensing and positioning module 3 greatly improves the positioning accuracy of the robot in dynamic and complex environments, while ensuring real-time and robustness;

[0112] An electrical optimization module 4, which is used for motor energy management and energy recovery. The electrical optimization module 4 includes an energy recovery unit 401, a battery balancing unit 402, and a battery management and energy distribution unit 403. The electrical optimization module 4 performs well in reducing energy waste, increasing battery life, and extending battery life. At the same time, the overall energy efficiency of the system is improved through energy recovery technology;

[0113] The path planning and dynamic obstacle avoidance module 5 is used to generate an obstacle avoidance path and adjust the global path. The path planning and dynamic obstacle avoidance module 5 includes a dynamic obstacle detection unit 501, a nonlinear optimization path planning unit 502 and a path adjustment and obstacle avoidance execution unit 503. The path planning and dynamic obstacle avoidance module 5 improves the robot's movement safety and path following ability in dynamic and complex scenes, and is suitable for operations in high-density obstacle environments.

[0114] See also Figure 1 In this embodiment, the control module 1 uses the sliding mode control and gain adjustment unit 101 to combine the sliding mode control with the adaptive gain adjustment to form a control algorithm. The control algorithm includes the following steps:

[0115] S1.1, the trajectory error calculation unit 102 calculates the trajectory error:

[0116] e x =x d -x,e y =y d -y,e θ =θ d -θ

[0117] Among them, x, y and θ are the current positions, x d ,y d and θ d is the desired position;

[0118] S1.2. Define the sliding surface:

[0119]

[0120] Among them, k v and k ω All are gain parameters;

[0121] S1.3, the control signal output unit 103 outputs the control law:

[0122] u=-(K c +K a |s|)tanh(s)

[0123] Among them, k c is a fixed gain, k aare all dynamic gain adjustment coefficients, and S is the absolute value of the sliding surface.

[0124] The control law achieves high adaptability to complex environments through the combination of dynamic gain adjustment and continuous functions, avoids the chattering problem of traditional sliding mode control, and improves the robustness of the system in path tracking.

[0125] See also Figure 1 In this embodiment, the multi-robot collaboration module 2 optimizes the distributed task allocation unit 201 and the time synchronization unit 202 in a collaborative manner, and the distributed task allocation includes the following steps:

[0126] S2.a1. Establishing the task benefit function:

[0127]

[0128] in:

[0129] R i : represents the i-th robot in the robot set, i = 1, 2, ..., n;

[0130] T j : represents the jth task in the task set, j = 1, 2, ..., m; a task can be a specific target point in the path, the transportation of items, or other execution actions;

[0131] d ij :Represents robot R i To Task T j The physical distance in meters (m);

[0132] ε: a small positive number used to avoid the problem of the denominator being zero;

[0133] α: Energy consumption weight factor, used to adjust the priority ratio between task distance and energy consumption;

[0134] E ij :Represents robot R i Execute Task T j The estimated energy required, in joules (J), can be calculated based on the complexity of the task (such as path length, object weight, etc.) and the energy consumption characteristics of the robot;

[0135] Through the design of the task benefit function, the distance and energy consumption weights in task allocation can be dynamically balanced to improve the global optimality and system efficiency of task allocation;

[0136] S2.a2, build optimization goals:

[0137]

[0138] in:

[0139] max: indicates that the optimization goal is to maximize the global task benefit, that is, to maximize the total benefit of the robot performing the task;

[0140] For all robots R i and Task T j The system comprehensively considers the matching relationship between all robots and tasks.

[0141] x ij : represents a binary decision variable used to determine the robot R i Whether to assign task T j ;

[0142] The optimization goal is to maximize the global task benefit and ensure the highest efficiency of the overall task allocation of the robot system. This optimization goal can balance the matching relationship between multiple robots and tasks, avoid the loss of global efficiency due to the maximization of local benefits, and improve the overall collaboration ability of the multi-robot system.

[0143] S2.a3, determine the constraints:

[0144]

[0145] Represents each robot R i Only one task can be executed at a time;

[0146] This constraint ensures the physical limitations of the robot in task allocation, i.e., a robot cannot complete multiple tasks at the same time;

[0147] x ij ∈{0,1}: represents x ij It is a binary variable and must be 0 or 1, 0 means the robot does not perform the task, 1 means the robot performs the task;

[0148] Task allocation needs to meet specific physical and logical constraints to ensure that there will be no resource conflicts or logical errors in the robot's collaboration process. Strict constraints ensure the rationality of task allocation and the feasibility of robot operation, avoiding logical conflicts and waste of resources.

[0149] S2.4, using distributed algorithms to solve task allocation in real time;

[0150] The distributed task allocation algorithm realizes global task allocation through local calculation and collaborative optimization, without the need for a central controller, which improves the robustness and real-time performance of the system. The distributed task allocation algorithm reduces the burden of central control, improves the scalability of the system, and has the ability to respond quickly to dynamic environments.

[0151] See also Figure 1 In this embodiment, the multi-robot collaboration module 2 implements time synchronization through the IEEE1588PTP protocol, including the following steps:

[0152] S2.b1, realize the broadcast and reception of synchronization information through the master-slave clock structure;

[0153] IEEE1588PTP is a high-precision time synchronization protocol that ensures time consistency in a distributed multi-robot system through a master-slave clock structure. The master clock is responsible for broadcasting time information, and the slave clock receives timestamps and adjusts its own clock to eliminate errors, thereby ensuring that the master clock provides a unified time reference for multiple robots and avoids task collaboration deviations caused by time differences.

[0154] S2.b2. Adjust the error of the slave clock according to the timestamp information of the master clock:

[0155]

[0156] Among them, Δ t is the time error, τ master Transmission delay from master clock to slave clock, τ slave is the transmission delay from the slave clock to the master clock;

[0157] Through timestamp exchange and deviation adjustment between the master and slave clocks, the slave clock can correct its own time error in real time, ensuring that the entire system achieves time synchronization at the microsecond level;

[0158] S2.b3, execution of time synchronization formula;

[0159] The time synchronization formula execution steps enable the multi-robot system to maintain high-precision time consistency during long-term operation, avoid coordination problems caused by time drift, and enable the multi-robot system to maintain high-precision time consistency in complex task collaboration, thereby improving collaboration efficiency and robustness.

[0160] See also Figure 1 In this embodiment, the sensing and positioning module 3 uses the extended Kalman filter algorithm unit 302 to implement multi-sensor fusion positioning through the extended Kalman filter algorithm, including the following steps:

[0161] S3.1. Establish a state update model:

[0162] x k+1 =f(x k ,u k )+w k

[0163] Among them, x k is the system status, uk is the control input, w k is the process noise;

[0164] The state update model provides initial values ​​for subsequent state estimation and improves prediction accuracy by combining the robot's motion characteristics. By introducing process noise, the model enhances robustness to uncertain disturbances.

[0165] S3.2. Establish observation model:

[0166] z k =h(x k )+v k

[0167] Among them, z k is the observed value, v k is the observation noise;

[0168] The observation model is used to describe the system state measured by the sensor, and the state correction is completed by combining the observed value and the predicted value. By establishing a unified observation model of multi-source sensors, the advantages of multiple data sources are integrated to improve the system's state perception ability and observation accuracy;

[0169] S3.3, updating the system state through prediction and correction steps;

[0170] By combining prediction with correction, EKF significantly reduces the impact of sensor noise and process errors, ensuring the stability and accuracy of system state estimation.

[0171] See also Figure 1 In this embodiment, the electrical optimization module 4 uses the energy recovery circuit in the energy recovery unit 401 to achieve energy recovery and reuse through reverse power generation by a DC motor, including the following steps:

[0172] S4.a1, during braking or deceleration, the power is recovered by reverse power generation:

[0173] P rec =T brake ·ω brake

[0174] in:

[0175] P rec is the recovery power during braking, in watts, W;

[0176] T brake is the torque acting on the motor shaft during braking, in Newton-meters, N·m;

[0177] ω brake is the angular velocity of the motor when braking, in radians per second, rad / s;

[0178] By achieving reverse power generation during braking or deceleration, the energy recovery circuit converts kinetic energy that would otherwise be wasted into electrical energy, thereby improving the energy efficiency of the system and extending the robot's endurance.

[0179] S4.a2, store the recovered energy in the supercapacitor, and the output current is:

[0180]

[0181] in:

[0182] I rec is the recovery current, in ampere, A;

[0183] P rec is the recovered power, in watts, W;

[0184] V rec is the voltage of the energy storage device, in volts, V;

[0185] The efficient storage of recovered energy by supercapacitors not only achieves energy reuse, but can also quickly release high power to support robot operation when needed, improving the performance and endurance of the overall system. The system integration optimizes the scheduling and distribution of energy recovery, avoids overload of energy storage equipment, and improves the overall energy utilization and operational reliability of the system.

[0186] See also Figure 1 In this embodiment, the electrical optimization module 4 uses the battery balancing circuit in the battery balancing unit 402 to achieve voltage consistency, and the balancing condition is:

[0187] V i =V j , ∈Battery Pack

[0188] Among them, V i Indicates the real-time voltage of the ith single cell in the battery pack, in volts, V; V j Indicates the real-time voltage of the jth single cell in the battery pack, in volts, V; ∈ means that the formula is applicable to any two cells in the battery pack, that is, the balancing management needs to be performed on all the battery cells in the battery pack;

[0189] Through the above implementation, the balancing system can improve the consistency of the battery pack, improve the performance during charging and discharging, and extend the life of the battery pack. In addition, it performs well in energy utilization efficiency and system safety, and is suitable for efficient battery management needs under complex load environments.

[0190] See also Figure 1In this embodiment, the motor dynamic control is implemented by closed-loop current feedback, including the following steps:

[0191] S4.b1, real-time acquisition of motor output current i;

[0192] By real-time monitoring of the output current of the motor during operation, the current motor load condition and actual operating status can be obtained, laying the foundation for closed-loop control and effectively improving the response speed and stability of the motor;

[0193] S4.b2, adjust the input voltage V:

[0194]

[0195] Where V is the motor input voltage, K p is the proportional gain, i r is the reference current, i is the actual current, (i r -i) is the current error, K d is the differential gain, is the rate of change of current error;

[0196] The rate of change of the current error, based on the current error of the motor, generates a dynamically adjusted motor input voltage through proportional control and differential control to ensure that the motor operation meets the reference current requirements;

[0197] S4.b3, Implementation of control formula:

[0198] S4.b31, real-time acquisition of reference current i r and the actual current i, calculate the current error: e i =i r -i;

[0199] By calculating the current error, the control system can obtain the deviation between the current state and the target state of the motor in real time, providing data support for subsequent adjustments;

[0200] S4.b32, according to the magnitude of the current error, proportional gain K p Generates the initial voltage input adjustment: V p =K p ·e i ;

[0201] Proportional control quickly responds to current errors, provides basic voltage adjustment, and effectively reduces motor operation deviations;

[0202] S4.b33. Calculate the rate of change of current error over time:

[0203] Calculating the change rate of the current error can capture the dynamic change trend of the current error, provide input for subsequent differential control, and enhance system stability;

[0204] S4.b34, according to the differential gain K d Generate the derivative compensation term:

[0205] The differential compensation term suppresses the oscillation and instability caused by rapid current changes by responding to the error changes, thus improving the dynamic performance of the system.

[0206] S4.b35, superimpose the proportional and differential control quantities, and output the final motor drive voltage: V = V p +V d ;

[0207] The final drive voltage is combined with proportional and differential control to achieve precise adjustment of the motor's operating state, effectively improving the robustness and response speed of the control system.

[0208] See also Figure 1 In this embodiment, the path planning and dynamic obstacle avoidance module 5 uses the nonlinear optimization path planning unit 502 and the path adjustment and obstacle avoidance execution unit 503 to generate an obstacle avoidance path based on nonlinear optimization, including the following steps:

[0209] S5.1. Obtaining the position and speed of an obstacle based on the dynamic obstacle detection unit 501;

[0210] S5.2. Construct obstacle avoidance path planning objective function:

[0211] min∫0 T ∥x(t)-x obstacle (t)∥ 2 dt

[0212] in:

[0213] T: The time range of path planning, usually the time period from the current moment of the robot to the target position;

[0214] x(t): The current position vector of the robot at time t, usually expressed as a two-dimensional coordinate (x, y);

[0215] x obstacle : The position vector (x o ,y o ), acquired by sensors in real time;

[0216] ∥x(t)-x obstacle (t)∥ 2: The squared Euclidean distance between the robot and the obstacle, used to measure the safety of the obstacle avoidance path;

[0217] Integrate the distance over the entire time range to ensure the overall optimality of path planning;

[0218] By constructing a globally optimal obstacle avoidance objective function, the robot can plan a smooth and safe path in a dynamic environment, effectively avoiding collisions with obstacles while optimizing the path length;

[0219] S5.3. Determine the path planning constraints as follows:

[0220] v≤v max ,a≤a max

[0221] Where v is the current linear velocity of the robot, v max is the maximum limit of the robot's linear velocity, which is determined by the hardware and task requirements, a is the robot's current acceleration, and a max The maximum limit of the robot's acceleration is used to avoid power system overload or safety problems caused by excessive acceleration;

[0222] Through reasonable speed and acceleration constraints, the path planning and dynamic obstacle avoidance module 5 avoids power system overload or safety problems caused by overspeeding or excessive acceleration, further improves the feasibility and stability of the planned path, uses embedded GPU or FPGA to accelerate the path planning algorithm, shortens the calculation time, and through real-time updating and efficient calculation of path points, the system has the ability to respond quickly in a dynamic environment, ensuring the safety and efficiency of the robot's movement.

[0223] See also Figure 1 In this embodiment, the high-precision wheeled robot trajectory tracking collaborative control system has a fault-tolerant mechanism. When the communication fails, it automatically switches to the backup communication channel and enters the stand-alone independent mode to complete the trajectory tracking task.

[0224] The system monitors the communication status in real time and uses periodic heartbeat signals or message confirmation mechanisms to detect whether the main communication channel is operating normally. When the communication fails, the system will automatically enable the backup communication channel. The backup channel may include wireless networks (such as Wi-Fi, Bluetooth), wired connections, or temporarily established self-organizing networks (such as Zigbee, LoRa). The switching process is automatically completed by the communication module, and the communication protocol and encryption mechanism are adaptively adjusted to ensure the stability and security of data transmission. At the same time, when the communication is completely interrupted and cannot be restored, the system enters the stand-alone mode, enables the locally stored mission plan and path data, and continues to complete the current trajectory tracking task through the built-in control module and positioning module, so that the system's fault tolerance performance in complex tasks and dynamic environments is greatly improved, providing a strong technical guarantee for practical applications.

[0225] An embodiment of the present invention further provides a computer-readable storage medium storing a computer program. When the computer program is executed by a processor, the processor executes the steps in the above-mentioned high-precision wheeled robot trajectory tracking collaborative control system.

[0226] See also Figure 2 An embodiment of the present invention further provides a computer device, including a memory and a processor, wherein the memory stores a computer program, and when the computer program is executed by the processor, the processor executes the various steps in the above-mentioned high-precision wheeled robot trajectory tracking collaborative control system.

[0227] An embodiment of the present invention also provides a computer program product containing instructions, which, when executed on a computer, enables the computer to execute the various steps in the above-mentioned high-precision wheeled robot trajectory tracking collaborative control system.

[0228] It should be noted that those skilled in the art can understand that all or part of the steps implemented in the embodiments of the present invention can be implemented in whole or in part by software, hardware, firmware or any combination thereof. When implemented using hardware, it can be implemented in whole or in part in the form of purchasing standard parts or modified parts. When implemented using software, it can be implemented in whole or in part in the form of a computer program product. The computer program product includes one or more computer instructions. When the computer program instructions are loaded and executed on a computer, the process or function described in the embodiment of the present application is generated in whole or in part. The computer can be a general-purpose computer, a special-purpose computer, a computer network, or other programmable device. The computer instructions can be stored in a computer-readable storage medium, or transmitted from one computer-readable storage medium to another computer-readable storage medium. For example, the computer instructions can be transmitted from a website site, computer, server or data center by wired (e.g., coaxial cable, optical fiber, digital subscriber line (DSL)) or wireless (e.g., infrared, wireless, microwave, etc.) mode to another website site, computer, server or data center. The computer-readable storage medium can be any available medium that a computer can access or a data storage device such as a server or data center that includes one or more available media integrated. The available medium may be a magnetic medium (eg, a floppy disk, a hard disk, a magnetic tape), an optical medium (eg, a DVD), or a semiconductor medium (eg, a solid state disk (SSD)).

[0229] In summary, the present invention provides a high-precision wheeled robot trajectory tracking collaborative control system to solve the problems of insufficient adaptability of wheeled robot trajectory tracking systems in dynamic environments, low efficiency of multi-robot task coordination, and weak energy management and endurance in the prior art. On the one hand, through sliding mode control combined with adaptive gain adjustment algorithm, accurate control of trajectory tracking error is achieved, and the accuracy of multi-sensor data fusion positioning is improved in combination with extended Kalman filter technology, and it has excellent path following ability and anti-disturbance performance in dynamic and complex environments; on the other hand, through distributed task allocation and time synchronization mechanism, task optimization allocation with global benefit maximization is achieved, and combined with energy consumption weight adjustment and dynamic task adjustment, the efficiency of multi-robot collaboration is improved, while reducing resource conflicts and energy waste; on the other hand, through energy recovery circuit and battery balancing management, the system realizes effective recovery of braking energy and consistent regulation of voltage between single cells, improves energy utilization and system endurance performance, and prolongs the service life of the battery pack.

[0230] It should be understood that the examples and implementation modes described herein are for illustrative purposes only and are not intended to limit the present invention. Those skilled in the art may make various modifications or changes based on the examples and implementation modes. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. A high-precision wheeled robot trajectory tracking collaborative control system, characterized in that: include: A control module (1) is used to generate a control signal according to a trajectory tracking error, wherein the control module (1) comprises a sliding mode control and gain adjustment unit (10), a trajectory error calculation unit (102) and a control signal output unit (103); A multi-robot collaboration module (2), used for implementing task allocation and real-time communication among multiple robots, the multi-robot collaboration module (2) comprising a distributed task allocation unit (201), a time synchronization unit (202) and a collaboration optimization and communication unit (203); A sensing and positioning module (3) is used to fuse multi-sensor data to achieve real-time positioning, wherein the sensing and positioning module (3) comprises a multi-sensor data acquisition unit (301), an extended Kalman filter algorithm unit (302) and a real-time positioning unit (303); An electrical optimization module (4) for performing motor energy management and energy recovery, wherein the electrical optimization module (4) comprises an energy recovery unit (401), a battery balancing unit (402) and a battery management and energy distribution unit (403); A path planning and dynamic obstacle avoidance module (5) is used to generate an obstacle avoidance path and adjust a global path, wherein the path planning and dynamic obstacle avoidance module (5) comprises a dynamic obstacle detection unit (501), a nonlinear optimization path planning unit (502) and a path adjustment and obstacle avoidance execution unit (503).

2. The high-precision wheeled robot trajectory tracking collaborative control system according to claim 1, characterized in that: The control module (1) uses the sliding mode control and gain adjustment unit (101) to combine sliding mode control with adaptive gain adjustment to form a control algorithm, and the control algorithm includes the following steps: S1.1, the trajectory error calculation unit (102) calculates the trajectory error: e x =x d -x,e y =y d -y,e θ =θ d -θ Among them, x, y and θ are the current positions, x d ,y d and θ d is the desired position; S1.

2. Define the sliding surface: Among them, k v and k ω All are gain parameters; S1.3, the control signal output unit (103) outputs the control law: u=-(K c +K a |s|)tanh(s) Among them, k c is a fixed gain, k a are all dynamic gain adjustment coefficients, and S is the absolute value of the sliding surface.

3. The high-precision wheeled robot trajectory tracking collaborative control system according to claim 1, characterized in that: The multi-robot collaborative module (2) optimizes the distributed task allocation unit (201) and the time synchronization unit (202) in a collaborative manner, and the distributed task allocation includes the following steps: S2.a1. Establishing the task benefit function: in: R i : represents the i-th robot in the robot set, i = 1, 2, ..., n; T j : represents the jth task in the task set, j = 1, 2, ..., m; d ij :Represents robot R i To Task T j The physical distance in meters; ε: a small positive number used to avoid the problem of the denominator being zero; α: Energy consumption weight factor, used to adjust the priority ratio between task distance and energy consumption; E ij :Represents robot R i Execute Task T j The estimated energy required, in joules; S2.a2, build optimization goals: in: max: indicates that the optimization goal is to maximize the global task benefit, that is, to maximize the total benefit of the robot performing the task; :For all robots R i and Task T j The system comprehensively considers the matching relationship between all robots and tasks. x ij : represents a binary decision variable used to determine the robot R i Whether to assign task T j ; S2.a3, determine the constraints: Represents each robot R i Only one task can be executed at a time; x ij ∈{0,1}: represents x ij It is a binary variable and must be 0 or 1, 0 means the robot does not perform the task, 1 means the robot performs the task; S2.4, use distributed algorithm to solve task allocation in real time.

4. The high-precision wheeled robot trajectory tracking collaborative control system as claimed in claim 3, characterized in that: The multi-robot collaboration module (2) realizes time synchronization through the IEEE1588PTP protocol, comprising the following steps: S2.b1, realize the broadcast and reception of synchronization information through the master-slave clock structure; S2.b2. Adjust the error of the slave clock according to the timestamp information of the master clock: Among them, Δ t is the time error, τ master Transmission delay from master clock to slave clock, τ slave is the transmission delay from the slave clock to the master clock; S2.b3, Execution of time synchronization formula.

5. The high-precision wheeled robot trajectory tracking collaborative control system according to claim 1, characterized in that: The sensing and positioning module (3) adopts the extended Kalman filter algorithm unit (302) to implement multi-sensor fusion positioning via an extended Kalman filter algorithm, comprising the following steps: S3.

1. Establish a state update model: x k+1 =f(x k ,u k )+w k Among them, x k is the system status, u k is the control input, w k is the process noise; S3.

2. Establish observation model: z k =h(x k )+v k Among them, z k is the observed value, v k is the observation noise; S3.

3. Update the system state through prediction and correction steps.

6. The high-precision wheeled robot trajectory tracking collaborative control system according to claim 1, characterized in that: The electrical optimization module (4) uses the energy recovery circuit in the energy recovery unit (401) to achieve energy recovery and reuse through reverse power generation by a DC motor, comprising the following steps: S4.a1, during braking or deceleration, the power is recovered by reverse power generation: P rec =T brake ·oh brake in: P rec is the recovery power during braking, in watts, W; T brake is the torque acting on the motor shaft during braking, in Newton-meters, N·m; ω brake is the angular velocity of the motor when braking, in radians per second, rad / s; S4.a2, store the recovered energy in the supercapacitor, and the output current is: in: I rec is the recovery current, in ampere, A; P rec is the recovered power, in watts, W; V rec is the voltage of the energy storage device in volts, V.

7. The high-precision wheeled robot trajectory tracking collaborative control system according to claim 6, characterized in that: The electrical optimization module (4) uses the battery balancing circuit in the battery balancing unit (402) to achieve voltage consistency, and the balancing condition is: V i =V j , Battery Pack Among them, V i Indicates the real-time voltage of the ith single cell in the battery pack, in volts, V; V j Indicates the real-time voltage of the jth single cell in the battery pack, in volts, V; The expression formula is applicable to any two single cells in the battery pack, that is, the balancing management needs to be performed on all battery cells in the battery pack.

8. The high-precision wheeled robot trajectory tracking collaborative control system according to claim 6, characterized in that: The motor dynamic control is achieved through closed-loop current feedback and includes the following steps: S4.b1, real-time acquisition of motor output current i; S4.b2, adjust the input voltage V: Where V is the motor input voltage, K p is the proportional gain, i r is the reference current, i is the actual current, (i r -i) is the current error, K d is the differential gain, is the rate of change of current error; S4.b3, Implementation of control formula: S4.b31, real-time acquisition of reference current i r and the actual current i, calculate the current error: e i =i r -i; S4.b32, according to the magnitude of the current error, proportional gain K p Generates the initial voltage input adjustment: V p =K p ·e i ; S4.b33. Calculate the rate of change of current error over time: S4.b34, according to the differential gain K d Generate the derivative compensation term: S4.b35, superimpose the proportional and differential control quantities, and output the final motor drive voltage: V = V p +V d .

9. The high-precision wheeled robot trajectory tracking collaborative control system according to claim 1, characterized in that: The path planning and dynamic obstacle avoidance module (5) uses the nonlinear optimization path planning unit (502) and the path adjustment and obstacle avoidance execution unit (503) to generate an obstacle avoidance path based on nonlinear optimization, comprising the following steps: S5.

1. Obtaining the position and speed of an obstacle based on the dynamic obstacle detection unit (501); S5.

2. Construct obstacle avoidance path planning objective function: in: T: time range of path planning; x(t): The current position vector of the robot at time t, expressed in two-dimensional coordinates (x, y); x obstacle : The position vector (x o ,y o ), acquired by sensors in real time; ∥x(t)-x obstacle (t)∥ 2 : The squared Euclidean distance between the robot and the obstacle, used to measure the safety of the obstacle avoidance path; : Integrate the distance within the entire time range to ensure the overall optimality of path planning; S5.

3. Determine the path planning constraints as follows: v≤v max ,a≤a max Where v is the current linear velocity of the robot, v max is the maximum limit of the robot's linear velocity, which is determined by the hardware and task requirements, a is the robot's current acceleration, and a max This is the maximum limit of the robot's acceleration, which is used to avoid power system overload or safety problems caused by excessive acceleration.

10. The high-precision wheeled robot trajectory tracking collaborative control system according to claim 1, characterized in that: The high-precision wheeled robot trajectory tracking collaborative control system has a fault-tolerant mechanism. When communication fails, it automatically switches to a backup communication channel and enters a single-machine independent mode to complete the trajectory tracking task.

Images