A trajectory optimization method and device for a differential robot and a related medium

Abstract

The application discloses a kind of trajectory optimization method, device and related medium of differential robot, the method includes obtaining the maximum speed threshold and acceleration upper limit value of differential kinematic model and respectively configuring car body level and wheel end level to obtain multi-dimensional motion constraint parameter set, then the speed limiting and acceleration constraint of car body level and wheel end level are executed, wheel speed is scaled according to difference keeping rule for wheel end acceleration overrun, and redistribution speed data is obtained by inverse kinematic model, the deviation of turning radius is calculated and the linear velocity is corrected when the deviation is overrun, and the multi-dimensional motion constraint parameter set is adjusted to optimize by collecting running feedback data.The application solves the technical problems that existing differential robot speed control technology cannot consider hardware safety, motion stability and trajectory tracking accuracy by multi-level progressive speed and acceleration constraint, difference keeping type wheel speed redistribution, and dynamic verification of turning radius and closed-loop parameter adaptive feedback.

Description

Technical Field

[0001] This invention relates to the field of intelligent robot technology, and in particular to a trajectory optimization method, device, and related medium for a differential robot. Background Technology

[0002] Differential robots, with their advantages of simple structure, convenient control, and low cost, have been widely used in industrial logistics, warehouse management, and commercial services. Their speed control performance directly determines the robot's motion stability, trajectory tracking accuracy, and operational safety. Trajectory optimization is a core requirement for differential robot operations, and its prerequisite is to achieve stable and precise speed and acceleration control. However, current technologies generally cannot simultaneously address acceleration constraint safety, hardware speed boundary compliance, and trajectory tracking accuracy.

[0003] Currently, existing speed control technologies for differential robots mainly fall into three categories. The first category is a single-level constraint scheme, which only applies hard constraints on acceleration for linear angular velocity or wheel speed in a single dimension. It lacks both hardware maximum speed constraints and trajectory optimization mechanisms. When wheel acceleration exceeds the limit, it directly reduces speed or stops, resulting in the original motion trend being interrupted and severe trajectory drift. The second category is a simple layered constraint scheme. Although it adds layered constraints on linear angular velocity and wheel speed, the layers are independent and lack coordination. Furthermore, when wheel acceleration exceeds the limit, it uses proportional speed reduction instead of maintaining the difference between the left and right wheel speeds, leading to changes in angular velocity trends and decreased trajectory tracking accuracy. The third category is a constraint scheme with simple calibration. It adds a turning radius calibration step to the constraints, but uses a fixed deviation threshold for indiscriminate calibration. The constraint parameters are fixed and cannot be dynamically adjusted according to load changes. The calibrated speed commands are not subject to secondary constraint verification and closed-loop optimization.

[0004] In summary, existing differential robot speed control technology has not constructed a complete trajectory optimization control scheme that covers multi-level speed and acceleration constraints, wheel speed redistribution, turning radius verification, and speed optimization, resulting in an inability to balance motion stability and trajectory tracking accuracy. Summary of the Invention

[0005] This invention provides a trajectory optimization method, device, and related medium for differential robots, aiming to solve the technical problem that existing differential robot speed control technology cannot simultaneously achieve motion stability and trajectory tracking accuracy.

[0006] In a first aspect, embodiments of the present invention provide a trajectory optimization method for a differential robot, comprising:

[0007] Obtain the wheelbase parameters and left and right wheel speed parameters of the differential robot, and establish the forward and inverse kinematic mapping relationship between the linear velocity and angular velocity of the vehicle centerline based on the wheelbase parameters and the left and right wheel speed parameters to obtain the differential kinematic model;

[0008] Based on the differential kinematics model, the maximum speed threshold and acceleration upper limit values ​​for the vehicle body level and wheel end level are configured respectively to obtain a multi-dimensional motion constraint parameter set;

[0009] Using the multidimensional motion constraint parameter set, the received original speed command is subjected to speed limiting and acceleration constraints at the vehicle body level and the wheel end level, respectively, to obtain hierarchical constraint speed data;

[0010] The wheel speeds with excessive wheel-end acceleration in the hierarchical constraint speed data are synchronously scaled according to a preset difference maintenance rule, and then back-calculated using a preset forward kinematics model to obtain the redistributed speed data;

[0011] Based on the redistributed speed data and the original speed command, the deviation between the target turning radius and the actual turning radius is calculated, and when the deviation exceeds the limit, the angular velocity is kept constant to correct the linear velocity, thereby obtaining an optimized speed command;

[0012] Based on the optimized speed command, operational feedback data is collected, and the multidimensional motion constraint parameter set is adjusted iteratively according to the operational feedback data to obtain the trajectory optimization control command.

[0013] Secondly, embodiments of the present invention provide a trajectory optimization device for a differential robot, comprising:

[0014] The model building unit is used to obtain the wheelbase parameters and left and right wheel speed parameters of the differential robot, and establish the forward and inverse kinematic mapping relationship between the linear velocity and angular velocity of the vehicle centerline based on the wheelbase parameters and the left and right wheel speed parameters to obtain the differential kinematic model.

[0015] The parameter configuration unit is used to configure the maximum speed threshold and acceleration upper limit values ​​at the vehicle body level and wheel end level respectively according to the differential kinematics model, so as to obtain a multi-dimensional motion constraint parameter set.

[0016] The speed constraint unit is used to execute the speed limiting and acceleration constraints at the vehicle body level and the wheel end level respectively on the received original speed command using the multi-dimensional motion constraint parameter set, so as to obtain hierarchical constraint speed data;

[0017] The speed adjustment unit is used to synchronously scale the wheel speeds with excessive wheel-end acceleration in the hierarchical constraint speed data according to a preset difference maintenance rule, and then back-calculate through a preset forward kinematics model to obtain the redistributed speed data.

[0018] The speed optimization unit is used to calculate the deviation between the target turning radius and the actual turning radius based on the redistributed speed data and the original speed command, and to keep the angular velocity unchanged when the deviation exceeds the limit in order to correct the linear velocity and obtain an optimized speed command.

[0019] The instruction output unit is used to collect operation feedback data based on the optimized speed instruction, and to adjust the multidimensional motion constraint parameter set according to the operation feedback data for iterative optimization to obtain the trajectory optimization control instruction.

[0020] Thirdly, embodiments of the present invention provide a computer device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the trajectory optimization method for the differential robot of the first aspect.

[0021] Fourthly, embodiments of the present invention provide a computer-readable storage medium, wherein a computer program is stored on the computer-readable storage medium, and when the computer program is executed by a processor, it implements the trajectory optimization method for the differential robot of the first aspect.

[0022] This invention provides a trajectory optimization method for a differential robot, including obtaining the wheelbase parameters and left and right wheel speed parameters of the differential robot, and establishing a forward and inverse kinematic mapping relationship between the linear velocity and angular velocity of the vehicle centerline based on the wheelbase parameters and the left and right wheel speed parameters to obtain a differential kinematic model; configuring maximum velocity thresholds and acceleration upper limits at the vehicle body level and wheel end level respectively according to the differential kinematic model to obtain a multi-dimensional motion constraint parameter set; and using the multi-dimensional motion constraint parameter set to execute the velocity limiting and acceleration constraints at the vehicle body level and wheel end level respectively on the received original velocity command to obtain hierarchical constraints. Speed ​​data; for wheel speeds with excessive wheel-end acceleration in the hierarchical constraint speed data, synchronous scaling is performed according to a preset difference-preserving rule, and then reverse-engineered using a preset forward kinematics model to obtain redistributed speed data; based on the redistributed speed data and the original speed command, the deviation between the target turning radius and the actual turning radius is calculated, and when the deviation exceeds the limit, the angular velocity is kept constant to correct the linear velocity, resulting in an optimized speed command; based on the optimized speed command, operational feedback data is collected, and the multi-dimensional motion constraint parameter set is adjusted according to the operational feedback data for iterative optimization to obtain a trajectory optimization control command. This invention solves the technical problem that existing differential robot speed control technology cannot simultaneously achieve motion stability and trajectory tracking accuracy through multi-level progressive speed and acceleration constraints, difference-preserving wheel speed redistribution, dynamic verification of turning radius, and adaptive feedback of closed-loop parameters.

[0023] This invention also provides a trajectory optimization device, computer equipment, and storage medium for a differential robot, which have the same beneficial effects as described above. Attached Figure Description

[0024] To more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings used in the following description of the embodiments will be briefly introduced. Obviously, the drawings described below are some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0025] Figure 1 A flowchart illustrating a trajectory optimization method for a differential robot provided in an embodiment of the present invention;

[0026] Figure 2 A comparison chart of optimized turning radii provided for embodiments of the present invention;

[0027] Figure 3 This is a schematic block diagram of a trajectory optimization device for a differential robot provided in an embodiment of the present invention.

[0028] Explanation of reference numerals in the attached figures:

[0029] 300. Trajectory optimization device for differential robot; 301. Model building unit; 302. Parameter configuration unit; 303. Speed ​​constraint unit; 304. Speed ​​adjustment unit; 305. Speed ​​optimization unit; 306. Command output unit. Detailed Implementation

[0030] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0031] It should be understood that, when used in this specification and the appended claims, the terms "comprising" and "including" indicate the presence of the described features, integrals, steps, operations, elements and / or components, but do not exclude the presence or addition of one or more other features, integrals, steps, operations, elements, components and / or collections thereof.

[0032] It should also be understood that the terminology used in this specification is for the purpose of describing particular embodiments only and is not intended to limit the invention. As used in this specification and the appended claims, the singular forms “a,” “an,” and “the” are intended to include the plural forms unless the context clearly indicates otherwise.

[0033] It should also be further understood that the term "and / or" as used in this specification and the appended claims refers to any combination of one or more of the associated listed items and all possible combinations, and includes such combinations.

[0034] Please see below. Figure 1 , Figure 1 The flowchart of a trajectory optimization method for a differential robot provided in an embodiment of the present invention specifically includes steps S101 to S106.

[0035] S101. Obtain the wheelbase parameters and left and right wheel speed parameters of the differential robot, and establish the forward and inverse kinematic mapping relationship between the linear velocity and angular velocity of the vehicle centerline based on the wheelbase parameters and the left and right wheel speed parameters to obtain the differential kinematic model.

[0036] S102. Based on the differential kinematics model, configure the maximum speed threshold and acceleration upper limit values ​​for the vehicle body level and wheel end level respectively to obtain a multi-dimensional motion constraint parameter set;

[0037] S103. Using the multidimensional motion constraint parameter set, the received original speed command is subjected to speed limiting and acceleration constraints at the vehicle body level and the wheel end level respectively to obtain hierarchical constraint speed data.

[0038] S104. The wheel speeds with excessive wheel-end acceleration in the hierarchical constraint speed data are synchronously scaled according to a preset difference maintenance rule, and then back-calculated using a preset forward kinematics model to obtain the redistributed speed data.

[0039] S105. Calculate the deviation between the target turning radius and the actual turning radius based on the redistributed speed data and the original speed command, and keep the angular velocity unchanged when the deviation exceeds the limit to correct the linear velocity, thereby obtaining an optimized speed command;

[0040] S106. Based on the optimized speed command, collect operation feedback data, and adjust the multidimensional motion constraint parameter set according to the operation feedback data for iterative optimization to obtain trajectory optimization control command.

[0041] In step S101, the differential robot consists of two independently driven coaxial wheels, with the center distance between the two wheels being the wheelbase L. Let the linear velocity of the left wheel be... The linear velocity of the right wheel is The linear velocity of the vehicle's centerline is v, and the angular velocity of the vehicle's centerline is w. Based on the wheelbase parameter L and the left and right wheel speed parameters... , Establish the positive kinematic mapping relationship between the linear velocity v and angular velocity w along the centerline of the vehicle body, and its expression is:

[0042] ;

[0043] Wherein, the linear velocity *v* represents the translational velocity of the vehicle's center along the direction of travel, determined by the arithmetic mean of the linear velocities of the left and right wheels; the angular velocity *w* represents the rotational velocity of the vehicle about its center point, determined by the ratio of the difference between the linear velocities of the left and right wheels to the wheelbase *L*. Correspondingly, an inverse kinematic mapping relationship is established, inversely derived from the linear velocity *v* and angular velocity *w* along the vehicle's centerline to obtain the linear velocities of the left and right wheels, expressed as:

[0044] ;

[0045] The above forward and inverse kinematic mapping relationships together constitute the differential kinematic model, which is the core mathematical foundation for subsequent speed constraints, wheel speed allocation, and trajectory optimization.

[0046] In step S102, based on the mapping relationship between the vehicle body centerline velocity, angular velocity, and left and right wheel linear velocities described by the differential kinematic model, motion constraint parameters are configured at both the vehicle body level and the wheel end level. At the vehicle body level, the maximum velocity threshold of the vehicle body centerline is configured, including the maximum linear velocity and maximum angular velocity of the vehicle body. At the wheel end level, the maximum allowable linear velocities of the left and right wheels are configured according to the rated parameters of the drive motor. Simultaneously, upper limits for acceleration at both the vehicle body level and the wheel end level are configured to constrain the rate of change of linear velocity, angular velocity, and left and right wheel linear velocities between adjacent control cycles. The maximum velocity thresholds and upper limits for acceleration at each level are integrated to obtain a multi-dimensional motion constraint parameter set, which provides constraint boundaries for subsequent execution of hierarchical constraints on the original speed command.

[0047] In one embodiment, step S102 includes:

[0048] The maximum linear velocity and maximum angular velocity of the vehicle body are configured separately.

[0049] By constraining the absolute value of the linear velocity to not exceed the maximum linear velocity of the vehicle body and the absolute value of the angular velocity to not exceed the maximum angular velocity of the vehicle body, the maximum speed constraint condition at the vehicle body level is obtained.

[0050] The linear velocities of the left and right wheels are calculated using the inverse kinematic mapping relationship of the differential kinematic model, and the maximum permissible linear velocities of the left and right wheels are configured according to the rated parameters of the drive motor.

[0051] By constraining the absolute value of the linear velocity of the left wheel to not exceed the maximum permissible linear velocity of the left wheel and the absolute value of the linear velocity of the right wheel to not exceed the maximum permissible linear velocity of the right wheel, the maximum speed constraint condition at the wheel end level is obtained.

[0052] The maximum speed constraint condition at the wheel end level is set to have a higher priority than the maximum speed constraint condition at the vehicle body level to obtain the maximum speed constraint parameters.

[0053] In this embodiment, the maximum speed constraint is a hard constraint that ensures the safe operation of the differential robot hardware. It is divided into two levels: vehicle body level and wheel end level, forming a two-dimensional protection together with the subsequent acceleration constraint. At the vehicle body level, the linear velocity v and angular velocity w of the vehicle body centerline are subject to comprehensive limitations imposed by the robot's mechanical structure, navigation safety, and the operating scenario, and thus have maximum allowable thresholds. Among them, the maximum linear velocity of the vehicle body... This represents the maximum allowable translational velocity of the vehicle's center along the direction of travel, and the maximum angular velocity of the vehicle. This indicates the maximum permissible rotational speed of the vehicle body around its center point. Both can be dynamically configured based on the robot model, load weight, and safety and efficiency requirements of the operational scenario. For example, in a warehouse AGV application scenario with a rated load of 500kg, the maximum linear speed of the vehicle body can be configured as follows: The maximum angular velocity of the vehicle body is configured as follows: To balance the efficiency and safety of warehousing operations.

[0054] Furthermore, by constraining the absolute value of the linear velocity to not exceed the maximum linear velocity of the vehicle body and the absolute value of the angular velocity to not exceed the maximum angular velocity of the vehicle body, the maximum speed constraint condition at the vehicle body level is obtained. Specifically, based on the above-configured maximum linear velocity of the vehicle body... and the maximum angular velocity of the vehicle body Constraints are established for the linear velocity v and angular velocity w of the vehicle centerline in the differential kinematic model, and their expressions are as follows:

[0055] ;

[0056] in, Represents the absolute value of linear velocity v. This represents the absolute value of the angular velocity w. The above constraint requires that the absolute value of the linear velocity along the vehicle's centerline does not exceed the maximum linear velocity of the vehicle at any given time. The absolute value of the angular velocity does not exceed the maximum angular velocity of the vehicle body at any given time. This ensures that the robot does not exceed the safe operating range at the overall motion level; this constraint is the vehicle-level maximum speed constraint.

[0057] Next, the left and right wheels of the differential robot are driven by independent drive motors. Based on the inverse kinematic mapping relationship established in step S101, the linear velocity of the left wheel can be calculated from the linear velocity v and angular velocity w of the vehicle's centerline. and the linear velocity of the right wheel Since each drive motor has a rated maximum speed, which corresponds to the maximum permissible linear velocity at the wheel end, the maximum permissible linear velocity of the left wheel is configured according to the rated parameters of the left and right wheel drive motors respectively. and the maximum permissible linear velocity of the right wheel .in, This indicates the maximum permissible linear speed of the left-wheel drive motor under rated operating conditions. This represents the maximum permissible linear speed of the right-wheel drive motor under rated operating conditions. Both are determined by the rated parameters of the drive motor and are insurmountable hardware constraints. For example, in the aforementioned warehouse AGV application scenario, based on the rated maximum speeds of the left and right wheel drive motors, the maximum permissible linear speed of the left wheel can be configured as follows: The maximum permissible linear speed of the right wheel is configured as follows: .

[0058] Furthermore, by constraining the absolute value of the left wheel's linear velocity to not exceed the maximum permissible linear velocity of the left wheel and the absolute value of the right wheel's linear velocity to not exceed the maximum permissible linear velocity of the right wheel, the maximum speed constraint condition at the wheel end level is obtained. Based on the above configuration of the maximum permissible linear velocity of the left wheel... and the maximum permissible linear velocity of the right wheel Regarding the linear velocity of the revolver and the linear velocity of the right wheel The constraint condition is defined as follows:

[0059] ;

[0060] in, Represents the linear velocity of the revolver The absolute value, Indicates the linear velocity of the right wheel The absolute value of the linear velocity of the left wheel. The above constraint requires that the absolute value of the linear velocity of the left wheel does not exceed the maximum permissible linear velocity of the left wheel at any time. The absolute value of the linear velocity of the right wheel does not exceed the maximum permissible linear velocity of the right wheel at any given time. This ensures that each drive motor does not exceed its speed, thus avoiding hardware damage caused by wheel-end overspeed. This constraint is the wheel-end level maximum speed constraint.

[0061] Finally, the priority of the wheel-end level maximum speed constraint is set higher than that of the vehicle-body level maximum speed constraint, resulting in the maximum speed constraint parameters. Since the wheel-end level maximum speed constraint corresponds to the hardware's insurmountable boundary determined by the drive motor's rated parameters, while the vehicle-body level maximum speed constraint corresponds to a dynamically configurable operating boundary based on the robot model, load weight, and operational scenario, the priority of the wheel-end level maximum speed constraint is set higher than that of the vehicle-body level maximum speed constraint during constraint execution. That is, when the speed constrained at the vehicle-body level is converted to left and right wheel speeds via inverse kinematic mapping, if the corresponding wheel speed exceeds the allowable range of the wheel-end level maximum speed constraint, the wheel-end level maximum speed constraint is used for correction to ensure the motor does not overspeed, fundamentally preventing hardware damage. Integrating the above vehicle-body level maximum speed constraint, wheel-end level maximum speed constraint, and their priority relationships yields the maximum speed constraint parameters. These parameters, as part of the multi-dimensional motion constraint parameter set, provide a complete constraint boundary for subsequent speed limiting processing.

[0062] In one embodiment, step S102 further includes:

[0063] Based on the differential kinematic model, the rate of change of the linear velocity in adjacent control cycles is set to linear acceleration, and the absolute value of the linear acceleration is constrained not to exceed the preset maximum linear acceleration upper limit, thus obtaining the linear acceleration constraint condition.

[0064] Based on the differential kinematic model, the rate of change of the angular velocity in adjacent control cycles is set to angular acceleration, and the absolute value of the angular acceleration is constrained not to exceed the preset maximum angular acceleration upper limit, thus obtaining the angular acceleration constraint condition;

[0065] The change rates of the linear velocity of the left wheel and the linear velocity of the right wheel in adjacent control cycles are respectively set to the acceleration of the left wheel and the acceleration of the right wheel, and the corresponding absolute values ​​are constrained not to exceed the corresponding maximum acceleration limits of the left wheel and the right wheel, respectively, and the wheel acceleration constraint conditions are obtained by integrating them.

[0066] By integrating the linear acceleration constraint, the angular acceleration constraint, and the wheel acceleration constraint, acceleration constraint parameters are obtained.

[0067] In this embodiment, based on the differential kinematics model, the rate of change of the linear velocity in adjacent control cycles is set to linear acceleration, and the absolute value of the linear acceleration is constrained not to exceed a preset maximum linear acceleration upper limit, thus obtaining the linear acceleration constraint condition. Specifically, the acceleration constraint is a core constraint that ensures the smoothness of the differential robot's motion, reduces mechanical wear, and avoids overload of the drive motor, forming a progressive protection together with the aforementioned maximum speed constraint. At the vehicle-level acceleration constraint level, linear acceleration... Defined as the rate of change of the linear velocity v along the vehicle centerline between adjacent control cycles, its discretized expression is:

[0068] ;

[0069] in, This indicates the linear velocity of the vehicle body during the current control cycle. This indicates the linear velocity of the vehicle body in the previous control cycle. Indicates the time interval between adjacent control cycles. A preset maximum linear acceleration upper limit is given. The constraint condition for the linear acceleration is established, and its expression is: This constraint requires that the absolute value of the rate of change of the vehicle's linear velocity between any two adjacent control cycles does not exceed the maximum linear acceleration limit. This effectively suppresses sudden changes in the vehicle's linear velocity, ensuring the robot's smooth movement along the direction of travel. For example, in a warehouse AGV application scenario with a rated load of 500kg, the maximum linear acceleration can be configured to... Control cycle time interval The change in the linear velocity of the vehicle body between adjacent control cycles shall not exceed This constraint is the linear acceleration constraint.

[0070] Furthermore, based on the differential kinematic model, the rate of change of the angular velocity in adjacent control cycles is set to angular acceleration, and the absolute value of the angular acceleration is constrained not to exceed a preset maximum angular acceleration upper limit, thus obtaining the angular acceleration constraint condition. Defined as the rate of change of the angular velocity w along the vehicle centerline between adjacent control cycles, its discretized expression is:

[0071] ;

[0072] in, This indicates the vehicle's angular velocity during the current control cycle. This indicates the vehicle's angular velocity in the previous control cycle. This indicates the time interval between adjacent control cycles. A preset maximum angular acceleration upper limit is given. The constraint condition for the angular acceleration is established, and its expression is: This constraint requires that the absolute value of the rate of change of the vehicle's angular velocity between any two adjacent control cycles does not exceed the maximum angular acceleration limit. This effectively suppresses sudden changes in the vehicle's angular velocity, preventing the robot from experiencing jitter or skidding during turns due to drastic changes in angular velocity. For example, in the aforementioned warehouse AGV application scenario, the maximum angular acceleration limit can be configured as follows: The change in the vehicle's angular velocity between adjacent control cycles does not exceed This constraint is the angular acceleration constraint.

[0073] Furthermore, the rates of change of the linear velocity of the left wheel and the linear velocity of the right wheel in adjacent control cycles are set to the acceleration of the left wheel and the acceleration of the right wheel, respectively, and the corresponding absolute values ​​are constrained not to exceed the corresponding maximum acceleration limits of the left wheel and the right wheel, respectively, thus obtaining the wheel acceleration constraint conditions. At the wheel-end acceleration constraint level, the left wheel acceleration... Defined as the linear velocity of the left wheel The rate of change between adjacent control cycles, right wheel acceleration Defined as the linear velocity of the right wheel The discretized expressions for the rate of change between adjacent control cycles are as follows:

[0074] ;

[0075] in, and These represent the linear velocities of the left and right wheels respectively during the current control cycle. and These represent the linear velocities of the left and right wheels, respectively, in the previous control cycle. This represents the time interval between adjacent control cycles. Given the maximum maximum acceleration limit of the left wheel. and the maximum acceleration limit of the right wheel Constraints are established for the acceleration of the left wheel and the acceleration of the right wheel, and their expressions are as follows: .in, This indicates the maximum allowable acceleration limit for the left-wheel drive motor. This indicates the maximum allowable acceleration limit for the right-wheel drive motor. Both can be configured independently based on the performance parameters of the left and right-wheel drive motors and the actual load conditions. The above constraint requires that the absolute value of the rate of change of the left wheel's linear velocity does not exceed the maximum acceleration limit for the left wheel between any two adjacent control cycles. The absolute value of the rate of change of the linear velocity of the right wheel does not exceed the upper limit of the maximum acceleration of the right wheel between any two adjacent control cycles. This ensures that each drive motor does not overload due to excessive acceleration. For example, in the aforementioned warehouse AGV application scenario, based on the performance difference between the left and right wheel drive motors, the maximum acceleration limit of the left wheel can be configured as follows: The maximum acceleration limit of the right wheel is configured as follows: Then the change in the linear velocity of the left wheel between adjacent control cycles does not exceed The change in the linear velocity of the right wheel between adjacent control cycles does not exceed By integrating the above left wheel acceleration constraint conditions with the right wheel acceleration constraint conditions, we obtain the wheel acceleration constraint conditions.

[0076] Finally, the linear acceleration constraint, angular acceleration constraint, and wheel acceleration constraint are integrated to obtain acceleration constraint parameters. The linear and angular acceleration constraints are vehicle-level acceleration constraints, used to constrain the rates of change of linear and angular velocities from the perspective of the overall vehicle motion. The wheel acceleration constraint is a wheel-end acceleration constraint, used to constrain the rates of change of linear velocities of the left and right wheels from the perspective of wheel-end drive. Integrating these three constraints yields acceleration constraint parameters, which, together with the aforementioned maximum speed constraint parameters, constitute the multidimensional motion constraint parameter set. This multidimensional motion constraint parameter set covers complete constraint boundaries at both the vehicle-level and wheel-end levels, and in both the maximum speed and acceleration dimensions, providing comprehensive parameter support for subsequent hierarchical constraints on the original speed command, wheel speed redistribution, and trajectory optimization.

[0077] In step S103, after receiving the original speed command, the linear velocity and angular velocity in the original speed command are first limited using the maximum speed threshold at the vehicle level in the multi-dimensional motion constraint parameter set. Then, based on the upper limit of acceleration at the vehicle level and the optimized vehicle speed from the previous frame, acceleration constraints are applied to the limited speed to obtain candidate speeds that meet the vehicle level constraints. Next, the candidate speeds are converted into left and right wheel speed candidate values ​​through the inverse kinematic mapping relationship of the differential kinematic model. The maximum allowable linear velocity at the wheel end level in the multi-dimensional motion constraint parameter set is used to limit the speeds of the left and right wheel speed candidate values ​​respectively. At the same time, wheel end acceleration is verified based on the upper limit of acceleration at the wheel end level and the optimized left and right wheel speeds from the previous frame. The results are then integrated to obtain hierarchical constraint speed data.

[0078] In one embodiment, step S103 includes:

[0079] Based on the original speed command, the linear velocity and the angular velocity are limited according to the maximum linear velocity and the maximum angular velocity of the vehicle body to obtain the initial value of the vehicle body speed;

[0080] Based on the optimized vehicle speed of the previous frame and the time interval between adjacent frames, the initial value of the vehicle speed is subjected to acceleration constraints to obtain candidate vehicle speeds.

[0081] Based on the inverse kinematic mapping relationship of the differential kinematic model, the candidate initial values ​​of the left wheel speed and the right wheel speed are calculated respectively according to the candidate vehicle speed, and the wheel speed candidate initial value data are integrated to obtain the wheel speed candidate initial value data.

[0082] Based on the maximum permissible linear velocity of the left wheel and the maximum permissible linear velocity of the right wheel, the candidate initial values ​​of wheel speed are respectively limited to obtain candidate values ​​of left wheel speed and right wheel speed.

[0083] Based on the optimized left and right wheel speed values ​​from the previous frame, calculate the wheel-end accelerations corresponding to the candidate left and right wheel speed values, and verify the hierarchical constraint speed data.

[0084] In this embodiment, the linear velocity and angular velocity are limited according to the vehicle's maximum linear velocity and maximum angular velocity based on the original velocity command, resulting in an initial vehicle velocity value. The first level is vehicle-level velocity and acceleration constraint. This level prioritizes applying dual constraints at the vehicle level to the original velocity command to ensure the compliance of the robot's overall motion. The linear velocity from the received original velocity command is... and angular velocity Then, based on the maximum linear velocity of the vehicle body in the multidimensional motion constraint parameter set... and the maximum angular velocity of the vehicle body Regarding the linear velocity and the angular velocity The initial values ​​of the vehicle speed are obtained by performing maximum speed limiting processing separately. and Its expression is:

[0085] ;

[0086] in, This is a limiting function used to restrict the input value x within the interval [min, max]. When x is less than min, it takes the minimum value; when x is greater than max, it takes the maximum value; otherwise, x remains unchanged. For example, in a warehouse AGV application scenario with a rated load of 500kg, the original speed command... Maximum linear velocity of the vehicle body Maximum angular velocity of the vehicle body ,because and After limiting the amplitude, the initial value of the vehicle speed is obtained. None of them were cut off.

[0087] Furthermore, based on the optimized vehicle velocity of the previous frame and the time interval between adjacent frames, an acceleration constraint is applied to the initial value of the vehicle velocity to obtain candidate vehicle velocities. The optimized linear velocity of the vehicle is then used as the basis for this process. and vehicle body angular velocity and the time interval between adjacent frames Combined with the upper limit of the maximum linear acceleration in the multidimensional motion constraint parameter set and the upper limit of maximum angular acceleration The initial value of the vehicle body speed and Acceleration constraints are applied separately to obtain candidate linear velocity values ​​from the candidate vehicle velocities. and angular velocity candidate values Their expressions are as follows:

[0088] ;

[0089] ;

[0090] in, The optimized linear velocity of the vehicle body from the previous frame. The optimized angular velocity of the vehicle body from the previous frame. The time interval between adjacent frames. This represents the maximum allowable change in linear velocity within one control cycle. This represents the maximum allowable change in angular velocity within one control cycle. The above constraints are strictly satisfied. as well as This effectively suppresses sudden changes in vehicle speed and ensures smooth movement. For example, in the aforementioned warehouse AGV application scenario, the optimized linear velocity of the vehicle in the previous frame... angular velocity Interval between adjacent frames Maximum linear acceleration limit Maximum angular acceleration limit When applying acceleration constraints to linear velocity, the allowable range of variation is... ,because After being limited, the following was obtained When applying acceleration constraints to angular velocity, the allowable range of variation is... ,because It is exactly within the constraint range, and after limiting, it is obtained Both satisfy the acceleration constraint conditions.

[0091] Furthermore, based on the inverse kinematic mapping relationship of the differential kinematic model, the initial candidate values ​​of the left wheel speed and right wheel speed are calculated separately according to the candidate vehicle speeds, and integrated to obtain the initial candidate wheel speed data. The second level is wheel-end level speed and acceleration constraints. This level performs dual constraints at the wheel-end level based on the candidate vehicle speeds to ensure the safe operation of the drive motor. Based on the inverse kinematic mapping relationship of the differential kinematic model established in step S101, the initial candidate linear velocity values ​​are calculated separately. and the candidate angular velocity values Derive candidate initial values ​​for left wheel speed separately Candidate initial values ​​for right wheel speed Its expression is:

[0092] ;

[0093] in, These are candidate values ​​for linear velocity after vehicle-level constraints. Here, L represents the candidate angular velocity value after vehicle-level constraints, and L is the wheelbase parameter. The above initial candidate left wheel velocity value... Candidate initial values ​​for right wheel speed The data is then integrated to obtain initial candidate values ​​for wheel speed. For example, in the aforementioned warehouse AGV application scenario, With a wheelbase L = 0.5m, the candidate initial value of the left wheel speed was calculated. Candidate initial value for right wheel speed .

[0094] Furthermore, based on the maximum permissible linear velocity of the left wheel and the maximum permissible linear velocity of the right wheel, the candidate initial wheel speed data are respectively limited to obtain candidate values ​​for the left wheel speed and the right wheel speed. The maximum permissible linear velocity of the left wheel is based on the multidimensional motion constraint parameter set. and the maximum permissible linear velocity of the right wheel For the candidate initial values ​​of the left wheel speed in the candidate initial value data of the wheel speed, Candidate initial values ​​for right wheel speed Perform maximum speed limiting processing on the wheel ends separately to obtain candidate values ​​for the left wheel speed. and right wheel speed candidate value Their expressions are as follows:

[0095] ;

[0096] ;

[0097] in, The maximum permissible linear velocity of the revolver. The maximum permissible linear speed of the right wheel is determined by the rated parameters of the drive motor. This limiting process ensures that the candidate speed values ​​for both wheels do not exceed the permissible speed range of their respective drive motors, preventing overspeeding at the wheel ends. For example, in the aforementioned warehouse AGV application scenario, the maximum permissible linear speed of the left wheel... Maximum permissible linear velocity of the right wheel ,because and After limiting the amplitude, the candidate values ​​for the left wheel speed are obtained. Candidate values ​​for right wheel speed None of them were cut off.

[0098] Finally, based on the optimized left and right wheel speed values ​​from the previous frame, the wheel-end accelerations corresponding to the candidate left and right wheel speed values ​​are calculated respectively, and the hierarchical constraint speed data is verified. (Based on the optimized left wheel speed value from the previous frame...) and right wheel speed value Calculate the candidate values ​​of the left wheel speed respectively. and right wheel speed candidate value The corresponding wheel-end acceleration is expressed as follows:

[0099] ;

[0100] in, For the acceleration of the left wheel end, The acceleration at the right wheel end, The left wheel speed value optimized from the previous frame. The right wheel speed value optimized from the previous frame. The time interval between adjacent frames. The calculated left wheel end acceleration... With the maximum acceleration limit of the revolver Compare, and compare the acceleration at the right wheel end. With the maximum acceleration limit of the right wheel If a comparison is made, and If the verification passes, the candidate value for the left wheel speed will be used directly. and right wheel speed candidate value The wheel speed is used as a constraint; if the verification fails, it is marked as wheel-end acceleration exceeding the limit, and the difference-preserving wheel speed reallocation mechanism needs to be triggered in the subsequent step S104. The above candidate vehicle speed, candidate left and right wheel speed values, and wheel-end acceleration verification results are integrated to obtain hierarchical constraint speed data. For example, in the aforementioned warehouse AGV application scenario, the optimized left wheel speed value of the previous frame Right wheel speed value Calculate the acceleration at the left wheel end. Right wheel end acceleration .because and If the verification fails, the difference-preserving wheel speed reallocation mechanism in step S104 needs to be triggered to perform compliant wheel speed reallocation.

[0101] In step S104, when the acceleration verification of the left and right wheel ends in the hierarchical constraint speed data fails, the candidate values ​​of the left and right wheel speeds are synchronously scaled proportionally according to a preset difference preservation rule. The core of this difference preservation rule is to always keep the sign and ratio of the difference between the candidate values ​​of the left and right wheel speeds unchanged, thereby ensuring that the scaled wheel speeds still retain the steering motion trend of the original speed command. During the scaling process, the scaling coefficient is calculated by comprehensively considering the constraints of the upper limit of wheel end acceleration and the maximum allowable linear velocity of the wheel end, so that the scaled left and right wheel speeds simultaneously satisfy the speed constraints and acceleration constraints at the wheel end level. After the synchronous scaling is completed, the new vehicle body linear velocity and the new vehicle body angular velocity are calculated by back-calculating the scaled left and right wheel speeds using a preset forward kinematics model, and then integrated to obtain the redistributed speed data.

[0102] In one embodiment, step S104 includes:

[0103] The hierarchical constraint speed data are respectively set with difference ratio constraints, wheel end dual constraints, and maximum feasible scaling factor constraints, and integrated to obtain the core constraint rules;

[0104] Based on the core constraint rules, the maximum scaling factor of the left wheel acceleration and the maximum scaling factor of the right wheel acceleration are calculated by taking the minimum value of the two, according to the ratio of the product of the upper limit of the left and right wheel acceleration and the time interval to the corresponding wheel speed change.

[0105] Based on the ratio of the maximum permissible linear velocity of the left wheel and the maximum permissible linear velocity of the right wheel to the corresponding candidate wheel speed values, calculate the maximum scaling factor of the maximum speed of the left wheel and the maximum scaling factor of the maximum speed of the right wheel respectively, and take the minimum value of the two to obtain the maximum speed constraint scaling factor;

[0106] The acceleration constraint scaling factor is compared with the maximum velocity constraint scaling factor, and the minimum value is taken to obtain the comprehensive scaling factor.

[0107] The candidate values ​​of left wheel speed and right wheel speed are synchronously and proportionally scaled using the comprehensive scaling factor to obtain the redistributed left wheel speed value and the redistributed right wheel speed value.

[0108] Based on the positive kinematic mapping relationship of the differential kinematic model, the new vehicle linear velocity and new vehicle angular velocity are calculated according to the left wheel speed value and the redistributed right wheel speed value, and the redistributed speed data is obtained by integrating them.

[0109] In this embodiment, the hierarchical constraint speed data is subject to difference ratio constraints, wheel-end dual constraints, and maximum feasible scaling factor constraints, which are then integrated to obtain core constraint rules. These core constraint rules include three basic principles. The first is the difference ratio constraint, which is the principle of unchanged steering trend, ensuring that the right wheel speed candidate value is maintained throughout the wheel speed redistribution process. With left wheel speed candidate value The first constraint is that the sign and ratio of the difference remain constant, ensuring that the steering trend of the vehicle's angular velocity is completely consistent with the original speed command, without altering the turning intent of the path planning. The second constraint is the wheel-end dual constraint, i.e., the dual constraint compliance principle. The redistributed left and right wheel speeds must simultaneously satisfy both the maximum wheel-end speed constraint and the wheel-end acceleration constraint, and cannot exceed the hardware safety boundary of the drive motor. The third constraint is the maximum feasible scaling factor constraint, i.e., the intent closeness principle. Under the premise of satisfying the above difference ratio constraint and wheel-end dual constraint, the maximum feasible scaling factor is taken to make the redistributed wheel speed as close as possible to the wheel speed candidate value, thereby reducing the amount of modification to the original speed command. Integrating the above three basic principles yields the core constraint rule, which runs through the entire process of subsequent scaling factor calculation and wheel speed redistribution.

[0110] Furthermore, based on the core constraint rules, the maximum scaling factor for the left wheel acceleration and the maximum scaling factor for the right wheel acceleration are calculated as the ratio of the product of the upper limit of the left and right wheel-end acceleration and the time interval to the corresponding wheel speed change. The minimum of the two is then taken to obtain the acceleration constraint scaling factor. To satisfy the wheel-end acceleration constraint, the maximum scaling factor that makes the scaled left and right wheel speeds satisfy their respective upper limits of wheel-end acceleration needs to be calculated. The maximum scaling factor for the left wheel acceleration is calculated separately. And the maximum scaling factor of the right wheel acceleration Its expression is:

[0111] ;

[0112] in, This is the upper limit of the revolver's maximum acceleration. This is the upper limit of the maximum acceleration of the right wheel. The time interval between adjacent frames. Candidate values ​​for left wheel speed. The left wheel speed value optimized from the previous frame. Candidate values ​​for right wheel speed. The right wheel speed value optimized from the previous frame. This represents the absolute value of the change in left wheel speed between adjacent frames. This represents the absolute value of the change in right wheel speed between adjacent frames. The meaning of the above formula is: numerator... The denominator represents the maximum permissible speed change of the revolver within one control cycle. This represents the change in the left wheel's speed candidate value relative to the actual speed change in the previous frame. The ratio of the two values ​​is the scaling factor that makes the left wheel's acceleration exactly meet the upper limit constraint. The calculation method for the right wheel is similar. The minimum value of the two is taken as the acceleration constraint scaling factor. Its expression is: The purpose of taking the minimum value is to ensure that the scaled left and right wheel speeds simultaneously meet their respective acceleration constraints, complying with the dual constraint compliance principle in the core constraint rules. For example, in a warehouse AGV application scenario with a rated load of 500kg, the maximum acceleration limit of the left wheel is... Maximum acceleration limit of the right wheel Interval between adjacent frames Left wheel speed candidate value The speed of the left wheel in the previous frame Candidate values ​​for right wheel speed The speed of the right wheel in the previous frame Calculations yielded , Acceleration constraint scaling factor .

[0113] Furthermore, based on the ratios of the maximum permissible linear velocity of the left wheel and the maximum permissible linear velocity of the right wheel to their corresponding candidate wheel speed values, the maximum scaling factor for the maximum speed of the left wheel and the maximum scaling factor for the maximum speed of the right wheel are calculated respectively, and the minimum of the two is taken to obtain the maximum speed constraint scaling factor. To satisfy the maximum speed constraint at the wheel end, it is necessary to calculate the maximum scaling factor that ensures the scaled left and right wheel speeds do not exceed their respective maximum permissible linear velocities. Calculate the maximum scaling factor for the maximum speed of the left wheel separately. And the maximum scaling factor of the right wheel's maximum speed Their expressions are as follows:

[0114] ;

[0115] ;

[0116] in, The maximum permissible linear velocity of the revolver. This represents the maximum permissible linear velocity of the right wheel. Candidate values ​​for left wheel speed. This represents the candidate value for the right wheel speed. The formula above means that when the candidate wheel speed value is not zero, the ratio of the maximum allowable linear speed to the absolute value of the candidate wheel speed value represents the scaling factor that makes the scaled wheel speed exactly reach the maximum allowable linear speed. The purpose of minimizing this ratio (within 1) is to ensure that the scaling factor does not exceed 1, meaning the scaling operation only performs a proportional reduction and does not amplify the wheel speed. When the candidate wheel speed value is zero, the scaling factor is directly taken as 1, indicating that no speed constraint correction is needed. The minimum of the two values ​​is taken as the maximum speed constraint scaling factor. Its expression is: For example, in the aforementioned warehouse AGV application scenario, the maximum permissible linear speed of the left wheel... Maximum permissible linear velocity of the right wheel Left wheel speed candidate value Candidate values ​​for right wheel speed Calculations yielded , Maximum speed constraint scaling factor This indicates that in this embodiment, none of the candidate wheel speed values ​​exceed the maximum speed constraint at the wheel end, and no scaling correction at the maximum speed level is required.

[0117] Furthermore, the acceleration constraint scaling factor is compared with the maximum speed constraint scaling factor, and the minimum value is taken to obtain the comprehensive scaling factor. To simultaneously satisfy both the wheel-end acceleration constraint and the wheel-end maximum speed constraint, the acceleration constraint scaling factor is... With the maximum speed constraint scaling factor The two values ​​are compared, and the minimum value is taken as the overall scaling factor. Its expression is: The purpose of taking the minimum value is to ensure that the scaled left and right wheel speeds are strictly compliant under the dual constraint conditions, conforming to the dual constraint compliance principle in the core constraint rules. Meanwhile, because... and These are the maximum feasible scaling factors under their respective constraints, and the combined scaling factor obtained by taking the minimum of the two. This refers to the maximum feasible scaling factor that simultaneously satisfies both constraints, conforming to the intent-closeness principle in the core constraint rules, ensuring that the redistributed wheel speed is as close as possible to the original candidate wheel speed value. For example, in the aforementioned warehouse AGV application scenario, the acceleration constraint scaling factor... Maximum speed constraint scaling factor Overall scaling factor This indicates that in this embodiment, the acceleration constraint is the main constraint bottleneck, and the scaling factor is determined by the acceleration constraint.

[0118] Furthermore, the candidate values ​​for left wheel speed and right wheel speed are simultaneously and proportionally scaled using the comprehensive scaling factor to obtain the redistributed left wheel speed value and the redistributed right wheel speed value. Based on the comprehensive scaling factor... For the candidate values ​​of the left wheel speed and the right wheel speed candidate value The left wheel speed value was obtained by performing synchronous proportional scaling separately. and redistribute right wheel speed Its expression is:

[0119] ;

[0120] in, The overall scaling factor is... Candidate values ​​for left wheel speed. This is a candidate value for the right wheel speed. Since both left and right wheel speed candidate values ​​are multiplied by the same overall scaling factor... The wheel speed difference is calculated by scaling proportionally. The sign of the difference is exactly the same as the sign of the difference in the original candidate value, thus ensuring that the turning trend remains unchanged before and after scaling, which conforms to the difference ratio constraint in the core constraint rule. For example, in the aforementioned warehouse AGV application scenario, the comprehensive scaling factor... Left wheel speed candidate value Candidate values ​​for right wheel speed The redistributed left wheel speed value was calculated. Redistribute right wheel speed Verify the left wheel acceleration after redistribution. and right wheel acceleration This can further verify whether it meets the wheel-end acceleration constraint requirements, and at the same time, the wheel speed difference before scaling. Scaled wheel speed difference The difference values ​​are all positive, indicating that the reversal trend remains unchanged.

[0121] Finally, based on the positive kinematic mapping relationship of the differential kinematic model, the new vehicle linear velocity and new vehicle angular velocity are calculated according to the redistributed left wheel speed value and the redistributed right wheel speed value, and integrated to obtain the redistributed speed data. Based on the positive kinematic mapping relationship of the differential kinematic model established in step S101, the redistributed speed data is obtained from the redistributed left wheel speed value. and the redistributed right wheel speed value Back-calculation of the new vehicle linear velocity and the new angular velocity of the vehicle body Its expression is:

[0122] ;

[0123] in, To redistribute the left wheel speed, To redistribute the right wheel speed, The wheelbase parameter, The new linear velocity of the vehicle body is obtained by reverse calculation after wheel speed redistribution. This is the new angular velocity of the vehicle body obtained by reverse calculation after wheel speed redistribution. The aforementioned new linear velocity of the vehicle body... New vehicle body angular velocity and redistribution of left wheel speed Redistribute right wheel speed The data is then integrated to obtain the redistributed speed data, which will serve as input for the dynamic verification of the turning radius and speed optimization in the subsequent step S105. For example, in the aforementioned warehouse AGV application scenario, the speed value of the redistributed left wheel is... Redistribute right wheel speed Wheelbase The new linear velocity of the vehicle body is obtained by reverse calculation. New vehicle angular velocity The data is then integrated to obtain the redistribution speed data.

[0124] In step S105, the target turning radius expected by path planning is calculated based on the ratio of linear velocity to angular velocity in the original speed command. The actual turning radius after constraint and redistribution is calculated based on the ratio of the new vehicle linear velocity to the new vehicle angular velocity in the redistributed speed data. The target turning radius and the actual turning radius are compared, and the deviation between them is calculated. When the deviation exceeds a preset allowable error threshold, the angular velocity is kept constant, and the linear velocity is corrected by multiplying the absolute value of the target turning radius and the new vehicle angular velocity while retaining the directional sign of the new vehicle linear velocity. This ensures that the corrected actual turning radius matches the target turning radius, eliminating the turning trajectory deviation introduced by multi-level constraints and wheel speed redistribution. After correction, the constraints of the corrected vehicle speed are checked to ensure that it still meets the maximum speed threshold and acceleration upper limit at the vehicle level. The speed is then converted into the final left and right wheel speeds through the inverse kinematic mapping relationship of the differential kinematic model, and the wheel-end constraints are checked to obtain the optimized speed command.

[0125] Combination Figure 2 As shown, in one embodiment, step S105 includes:

[0126] Based on whether the absolute value of the angular velocity in the original speed command is greater than a preset threshold, the absolute value of the ratio of the linear velocity to the angular velocity or infinity is calculated to obtain the target turning radius;

[0127] Based on whether the absolute value of the new vehicle body angular velocity in the redistributed speed data is greater than a preset threshold, the absolute value of the ratio of the new vehicle body linear velocity to the new vehicle body angular velocity or infinity is calculated to obtain the actual turning radius.

[0128] When the target turning radius is a finite value and the absolute value of the new vehicle body angular velocity is greater than a preset threshold, the absolute value of the difference between the actual turning radius and the target turning radius is calculated to obtain the deviation verification result;

[0129] When the deviation verification result is out of limit, the angular velocity is kept constant. The linear velocity is corrected by multiplying the target turning radius by the absolute value of the new vehicle body angular velocity and retaining the directional sign of the new vehicle body linear velocity, so as to obtain the optimized vehicle body velocity.

[0130] The optimized vehicle speed is subjected to a maximum speed limit and acceleration verification correction process to obtain the verified vehicle speed.

[0131] The vehicle speed after verification is converted into the final left wheel speed value and the final right wheel speed value by using the inverse kinematic mapping relationship of the differential kinematic model, and the maximum speed constraint condition at the wheel end level is verified to obtain the optimized speed command.

[0132] In this embodiment, based on whether the absolute value of the angular velocity in the original speed command is greater than a preset threshold, the absolute value of the ratio of the linear velocity to the angular velocity, or infinity, is calculated to obtain the target turning radius. It is the trajectory curvature expected by the path planning module, a core indicator for quantifying motion intent, based on the linear velocity in the original velocity command. and angular velocity Calculations are performed. The preset threshold is used to determine whether the robot is currently in a turning motion or a straight-line motion; in this embodiment, it is set to a value of [value missing]. When the angular velocity in the original velocity command When the absolute value is greater than the preset threshold, that is The robot is determined to be in a turning motion, and the target turning radius is calculated as the linear velocity. With the angular velocity The absolute value of the ratio is expressed as: .in, The linear velocity in the original velocity command. The angular velocity in the original velocity command. This represents the absolute value of the ratio of linear velocity to angular velocity, corresponding to the radius of the circular trajectory formed by the robot moving at its current linear and angular velocities. When the angular velocity in the original velocity command... When the absolute value is not greater than the preset threshold, that is The robot is determined to be in a straight-line motion state. At this time, the target turning radius is set to infinity, i.e. This indicates that the robot is moving in a straight line without triggering subsequent turning radius calibration operations, thus avoiding over-calibration that could affect the response efficiency and stability of the straight-line motion. Combining the above two situations, the target turning radius... The complete expression is:

[0133] ;

[0134] For example, in a warehouse AGV application scenario with a rated load of 500kg, the original speed command... , ,because Determine that the robot is in a turning motion and calculate the target turning radius. .

[0135] Furthermore, based on whether the absolute value of the new vehicle body angular velocity in the redistributed speed data is greater than a preset threshold, the absolute value of the ratio of the new vehicle body linear velocity to the new vehicle body angular velocity, or infinity, is calculated to obtain the actual turning radius. Actual turning radius It is the trajectory curvature actually executed by the robot after multi-level constraints and wheel speed redistribution, based on the new vehicle body linear velocity in the redistributed speed data. and the new angular velocity of the vehicle body Calculations are performed. When the new vehicle body angular velocity... When the absolute value is greater than the preset threshold, that is The actual turning radius is calculated as the new vehicle body linear velocity. With the new vehicle body angular velocity The absolute value of the ratio is expressed as: .in, The new vehicle linear velocity obtained by reverse calculation after wheel speed redistribution in step S104. This refers to the new vehicle body angular velocity obtained by reverse calculation after wheel speed redistribution in step S104. When the new vehicle body angular velocity... When the absolute value is not greater than the preset threshold, that is The actual turning radius is set to infinity, that is... Combining the above two scenarios, the actual turning radius... The complete expression is:

[0136] ;

[0137] For example, in the aforementioned warehouse AGV application scenario, the new vehicle linear velocity obtained in step S104 New vehicle body angular velocity ,because Calculate the actual turning radius .

[0138] Furthermore, when the target turning radius is a finite value and the absolute value of the new vehicle body angular velocity is greater than a preset threshold, the absolute value of the difference between the actual turning radius and the target turning radius is calculated to obtain the deviation verification result. Only when the target turning radius... The new angular velocity of the vehicle body is a finite value, meaning the robot is in a turning motion. The absolute value is greater than the preset threshold, i.e. During this process, a turning radius deviation check is performed. This design ensures that radius verification and speed optimization are only performed on the robot during turning; calibration is not triggered during straight-line motion, avoiding over-calibration that could affect motion response efficiency and straight-line trajectory stability. The deviation check is determined by the actual turning radius. With the target turning radius Does the absolute value of the difference exceed the preset allowable error threshold for the turning radius? Its expression is: .in, The permissible error threshold for the turning radius, with a value range of [value range missing]. It can be dynamically adjusted according to the robot model, operating conditions, and load. When the absolute value of the difference does not exceed... When the deviation verification result is within limits, it means that the actual turning radius after multi-level constraints and wheel speed redistribution is still within the allowable error range, and speed optimization is not required; when the absolute value of the difference exceeds... If the deviation check result is "out of limit," it means that the deviation between the actual turning radius and the target turning radius has exceeded the allowable range, and speed optimization needs to be triggered in subsequent steps. For example, in the aforementioned warehouse AGV application scenario, the allowable error threshold for the turning radius is... Target turning radius Actual turning radius Calculate the deviation ,because The deviation check result is out of limit, and speed optimization needs to be triggered.

[0139] Furthermore, when the deviation verification result exceeds the limit, the angular velocity remains unchanged. The linear velocity is corrected by multiplying the target turning radius by the absolute value of the new vehicle body angular velocity and retaining the directional sign of the new vehicle body linear velocity, resulting in the optimized vehicle body speed. Speed ​​optimization is triggered when the deviation exceeds the allowable threshold. The core optimization principle is to keep the steering angular velocity constant and only correct the linear velocity. This principle is designed to, on the one hand, maximize the preservation of the acceleration constraint results completed in the aforementioned steps, avoiding secondary exceedances of wheel-end acceleration due to angular velocity adjustments; on the other hand, by accurately restoring the desired trajectory curvature through linear velocity correction, turning trajectory drift is eliminated at its source. The optimized linear velocity... The calculation formula is: .in, The target turning radius is... Let be the absolute value of the new vehicle body angular velocity. The sign function is used to preserve the new linear velocity of the vehicle body. The forward or backward direction symbol ensures that the optimized motion direction is consistent with the original velocity command. The optimized angular velocity remains unchanged, i.e. The above optimizations demonstrate that the actual turning radius... The turning radius is perfectly consistent with the target turning radius, eliminating turning trajectory drift caused by multi-level constraints and wheel speed redistribution at its source, thus achieving precise maintenance of the path tracking intention. The optimized linear velocity described above... and angular velocity The integration yields an optimized vehicle speed. For example, in the aforementioned warehouse AGV application scenario, the target turning radius... New vehicle angular velocity The new vehicle body linear velocity (Positive direction indicates forward movement), optimized linear velocity angular velocity maintained .

[0140] Furthermore, the optimized vehicle speed is subjected to a maximum speed limit and acceleration verification correction process to obtain the verified vehicle speed. After speed optimization, a secondary constraint verification is required on the optimized vehicle speed to ensure that it still meets the maximum speed threshold and acceleration upper limit values ​​at the vehicle level in the multi-dimensional motion constraint parameter set. Regarding the maximum speed limit verification, the optimized linear velocity... Based on the maximum linear velocity of the vehicle body The expression for amplitude limiting is: .in, For the amplitude limiting function, This represents the maximum linear velocity of the vehicle body. For acceleration verification and correction, the optimized linear velocity is calculated. The optimized linear velocity of the vehicle body relative to the previous frame acceleration If this value exceeds the maximum linear acceleration limit Then, according to the upper limit of acceleration... Make corrections to ensure acceleration compliance. The vehicle speed, after corrections based on the maximum vehicle speed limit and acceleration, will be used as the verified vehicle speed. For example, in the aforementioned warehouse AGV application scenario, the optimized linear velocity... Maximum linear velocity of the vehicle body ,because Maximum speed limiting verification passed. Further acceleration verification was performed, checking the vehicle's linear velocity from the previous frame. acceleration ,because Acceleration exceeds the limit and needs to be corrected according to the upper limit of acceleration. After correction To ensure acceleration compliance.

[0141] Finally, the verified vehicle body speed is converted into the final left wheel speed value and the final right wheel speed value through the inverse kinematic mapping relationship of the differential kinematic model, and the maximum speed constraint condition at the wheel end level is verified to obtain the optimized speed command. Based on the inverse kinematic mapping relationship of the differential kinematic model established in step S101, the verified vehicle body linear velocity is... and vehicle body angular velocity Convert to final revolver speed value and final right wheel speed value Its expression is:

[0142] ;

[0143] in, The verified linear velocity of the vehicle body. The verified angular velocity of the vehicle body. This refers to the wheelbase parameter. After the conversion, the final left wheel speed value also needs to be... and final right wheel speed value The maximum speed constraint at the wheel end stage is verified again, i.e., the check is performed. and This checks whether the condition is met, ensuring that the final output wheel speed does not exceed the hardware safety boundary of the drive motor. The fully verified vehicle speed and the final left and right wheel speeds are then integrated to obtain the optimized speed command. For example, in the aforementioned warehouse AGV application scenario, the verified vehicle linear speed... angular velocity Wheelbase Calculate the final left wheel speed value Final right wheel speed value Verify the maximum speed constraint at the wheel end. and All constraints are met, resulting in an optimized speed command. The optimization effect can be referenced... Figure 2 As shown.

[0144] In step S106, the optimized speed command is output to the underlying controller of the differential robot for execution, and the robot's operation feedback data is collected in real time and updated to the multidimensional motion constraint parameter set. The optimization is continuously iterated in subsequent control cycles to obtain the trajectory optimization control command.

[0145] In one embodiment, step S106 includes:

[0146] Based on the optimized speed command, motor status data and trajectory deviation data are collected in real time.

[0147] Based on the motor status data, load changes are identified and the upper limit of acceleration is adjusted proportionally to obtain adaptive acceleration parameters;

[0148] Based on the adaptive acceleration parameters, the allowable error threshold for the turning radius is adjusted according to the deviation trend based on the trajectory deviation data to obtain the adaptive turning radius threshold.

[0149] Based on the adaptive turning radius threshold, the current operating condition is determined and the maximum linear speed of the vehicle body is adjusted according to the operating condition to obtain the adaptive maximum speed parameter;

[0150] The adaptive acceleration parameters, the adaptive turning radius threshold, and the adaptive maximum speed parameters are updated to the multidimensional motion constraint parameter set and continuously iterated and optimized to obtain trajectory optimization control commands.

[0151] In this embodiment, motor status data and trajectory deviation data are collected in real time based on the optimized speed command. After the optimized speed command obtained in step S105 is output to the underlying controller of the differential robot for execution, the robot's operation feedback data is collected in real time within each control cycle. The operation feedback data includes two types of core data: the first type is motor status data, including the motor current and motor torque data of the left and right wheel drive motors. This type of data is used to identify the load changes of the differential robot during operation. When the load increases, the motor current and torque increase accordingly, and when the load decreases, the motor current and torque decrease accordingly, thereby providing a quantitative basis for the subsequent adaptive adjustment of acceleration parameters; the second type is trajectory deviation data, which comes from the positioning module of the differential robot, including the lateral deviation and longitudinal deviation between the actual running path and the planned path. This type of data is used to quantitatively evaluate the trajectory optimization effect and provide real-time feedback for the subsequent adaptive adjustment of the turning radius allowable error threshold and the maximum linear speed of the vehicle. For example, in a warehouse AGV application scenario with a rated load of 500kg, the optimized linear speed in step S105 is used to... and angular velocity After being converted into the final left and right wheel speeds, the data is encapsulated into control messages and sent to the AGV's underlying controller. At the same time, in each control cycle, the current and torque data of the left and right wheel drive motors, as well as the lateral and longitudinal deviation data output by the positioning module, are collected.

[0152] Furthermore, based on the motor status data, load changes are identified and the upper limit of acceleration is adjusted proportionally to obtain adaptive acceleration parameters. Based on the changing trends of motor current and torque in the motor status data, the current load change of the differential robot is identified, and the upper limit of acceleration in the multi-dimensional motion constraint parameter set is dynamically adjusted proportionally accordingly. When the motor status data shows an increase in load, i.e., a continuous increase in motor current and torque, to prevent the drive motor from overloading due to the increased load, the upper limit of maximum linear acceleration in the multi-dimensional motion constraint parameter set is reduced proportionally. Maximum angular acceleration limit and the maximum acceleration limit of the revolver and the maximum acceleration limit of the right wheel This makes the acceleration constraints more stringent, thereby reducing the instantaneous output power demand of the motor under high load conditions and effectively preventing motor overload. When the motor status data shows that the load decreases, i.e., the motor current and torque gradually decrease, to ensure the robot's motion response efficiency, the above-mentioned acceleration upper limits are gradually restored to the initial configuration values ​​to avoid speed response lag caused by overly strict constraints. The above-mentioned acceleration upper limit values ​​after load adaptive adjustment are integrated to obtain adaptive acceleration parameters. For example, in the aforementioned warehouse AGV application scenario, when the AGV performs heavy-load handling tasks in the warehouse racking area, the motor current continuously increases. The system identifies this as an increased load condition and proportionally adjusts the maximum linear acceleration upper limit from the initial configuration value. Reduce to The maximum acceleration limit of the revolver is increased from... Reduce to The maximum acceleration limit of the right wheel is increased from... Reduce to This provides a greater safety margin for the drive motor under heavy load conditions.

[0153] Furthermore, based on the adaptive acceleration parameters, the allowable error threshold for the turning radius is adjusted according to the trajectory deviation data and the deviation trend to obtain the adaptive turning radius threshold. After the adaptive acceleration parameters have been adjusted, the allowable error threshold for the turning radius used in step S105 is further adjusted based on the changing trend of the lateral deviation in the trajectory deviation data and the changes in road surface wheel speed slip. Dynamic adaptive adjustments are performed. When the trajectory deviation data shows a continuous increase in lateral deviation, it indicates that the current turning radius calibration accuracy is insufficient, and the allowable error threshold for the turning radius needs to be reduced. This makes the deviation check in step S105 more sensitive, thereby improving the accuracy of the turning radius calibration and reducing lateral drift of the trajectory. When the road surface wheel speed slip increases, it indicates that the ground adhesion coefficient decreases. If the error threshold is kept too small at this time, it will be detrimental. This can lead to excessively frequent calibrations and cause motion jitter, therefore the allowable error threshold for the turning radius needs to be appropriately increased. A balance is struck between trajectory accuracy and motion smoothness, avoiding motion jitter caused by over-calibration. The adaptive turning radius threshold is obtained by integrating the aforementioned allowable error thresholds for adaptive deviation trend adjustment. For example, in the aforementioned warehouse AGV application scenario, when the AGV is running on a smooth epoxy floor, the wheel speed slippage increases, and the system adjusts the allowable error threshold for the turning radius from the initial configuration value. Increase to To avoid vehicle vibration caused by frequent calibration on low-friction surfaces; and when the AGV runs on rough concrete and the lateral deviation of the trajectory continues to increase, the system will adjust the turning radius allowable error threshold from the initial configuration value. Shrink to Improve the accuracy of turning radius calibration to enhance trajectory tracking performance.

[0154] Furthermore, based on the adaptive turning radius threshold, the current operating condition is determined and the maximum linear velocity of the vehicle body is adjusted accordingly to obtain the adaptive maximum speed parameter. After the adaptive turning radius threshold has been adjusted, the operating condition of the differential robot is determined by combining the current turning radius and the characteristics of the operating section, and the maximum linear velocity of the vehicle body in the multi-dimensional motion constraint parameter set is adjusted accordingly. Dynamic adaptive adjustments are made. When it is determined that the robot is currently in a small-radius turning situation, i.e., the target turning radius is small, the maximum linear velocity of the vehicle body is reduced proportionally. This reduces the robot's speed in curves, thereby improving steering stability and preventing sideslip or trajectory deviation caused by excessive turning speed. When the robot is determined to be traveling a long straight distance, i.e., the target turning radius is infinite or at its maximum, the maximum linear velocity of the vehicle is restored. The maximum linear velocity of the vehicle is adjusted to the initial configuration value or appropriately increased to ensure the robot's operating efficiency on straight sections. The adaptive maximum speed parameter is obtained by integrating the maximum linear velocity of the vehicle body after adaptive adjustment based on the operating conditions. For example, in the aforementioned warehouse AGV application scenario, when the AGV is running on a small-radius curve between shelves, the target turning radius is small, and the system adjusts the maximum linear velocity of the vehicle body from the initial configuration value. Reduce to This ensures the AGV passes smoothly and at low speed through curves; when the AGV enters the long straight passage between two rows of shelves, the system restores the vehicle's maximum linear speed to [the required speed]. This ensures the overall operational efficiency of warehousing and handling operations.

[0155] Finally, the adaptive acceleration parameters, the adaptive turning radius threshold, and the adaptive maximum speed parameters are updated to the multidimensional motion constraint parameter set and continuously iterated and optimized to obtain the trajectory optimization control command. The above three types of adaptive parameters are uniformly updated to the multidimensional motion constraint parameter set, replacing the corresponding original constraint parameter values. The updated multidimensional motion constraint parameter set will directly apply to the complete constraint and optimization process from steps S102 to S105 in the next control cycle, ensuring that subsequent speed limiting, acceleration constraints, wheel speed redistribution, and turning radius verification are all performed based on the latest adaptively adjusted constraint parameters. Within each control cycle, the system feeds back the trajectory deviation data and speed fluctuation data of the current cycle to the adaptive adjustment module of the multidimensional motion constraint parameter set, continuously iterating and optimizing the constraint parameters and scaling strategies, forming an automatic cycle of condition changes, parameter adjustments, and performance optimization. This fully closed-loop iterative optimization mechanism ensures that the differential robot maintains optimal trajectory tracking performance under all working conditions, and the final output speed command is the trajectory optimization control command after complete closed-loop optimization.

[0156] In summary, this invention constructs a differential kinematic model to establish the forward and inverse kinematic mapping relationship between the linear velocity and angular velocity of the vehicle centerline. Based on this, it configures the maximum velocity threshold and acceleration upper limit at both the vehicle body level and the wheel end level to form a multi-dimensional motion constraint parameter set. It applies a two-level progressive constraint to the original speed command, first the maximum velocity constraint and then the acceleration constraint. For scenarios where the wheel end acceleration exceeds the limit, it designs a difference-preserving synchronous scaling mechanism to redistribute the wheel speed. Then, through dynamic verification of the turning radius, it maintains the angular velocity unchanged when the deviation exceeds the limit to correct the linear velocity and achieve trajectory optimization. Finally, it dynamically adjusts the constraint parameters based on the running feedback data to complete the full closed-loop iterative optimization and form a complete trajectory optimization control scheme.

[0157] Based on the above technical solution, the present invention has the following beneficial effects:

[0158] This invention achieves dual protection at both the vehicle body and wheel end drive levels through a two-tiered, progressive maximum speed hard constraint, ensuring that the drive motor does not exceed its speed under any operating condition. This hardware-level safety feature avoids the risk of motor overload damage and effectively extends the robot's lifespan. Simultaneously, the coordinated design of the two-tiered acceleration constraints effectively suppresses abrupt changes in vehicle linear velocity, angular velocity, and left and right wheel speeds between adjacent control cycles, significantly improving the motion stability of the differential robot and reducing mechanical wear.

[0159] The difference-preserving wheel speed redistribution mechanism designed in this invention maintains the sign and ratio of the difference between the candidate wheel speeds by synchronous proportional scaling when the wheel-end acceleration exceeds the limit. This ensures that the steering trend of the vehicle's angular velocity before and after scaling is completely consistent with the original speed command, thus fundamentally solving the technical defects of existing technologies that lead to loss of motion intent and decreased trajectory tracking accuracy due to proportional speed reduction.

[0160] The dynamic turning radius verification and speed optimization mechanism designed in this invention calculates the deviation between the target turning radius and the actual turning radius, and when the deviation exceeds the limit, it keeps the angular velocity constant and only corrects the linear velocity, making the optimized actual turning radius completely consistent with the target turning radius. This accurately eliminates the turning trajectory drift caused by multi-level constraints and wheel speed redistribution, achieving precise maintenance of the path tracking intention. Simultaneously, this mechanism only triggers calibration in the turning state without interfering with linear motion, balancing trajectory accuracy and motion response efficiency.

[0161] The fully closed-loop parameter adaptive feedback mechanism constructed in this invention dynamically adjusts the upper limit of acceleration, the allowable error threshold of turning radius, and the maximum linear speed of the vehicle body by collecting motor status data and trajectory deviation data in real time. This enables the system to adapt to different loads and different road conditions, and has good multi-condition adaptability.

[0162] Combination Figure 3 As shown, Figure 3 This is a schematic block diagram of a trajectory optimization device for a differential robot provided in an embodiment of the present invention. The trajectory optimization device 300 for the differential robot includes:

[0163] The model building unit 301 is used to obtain the wheelbase parameters and left and right wheel speed parameters of the differential robot, and establish the forward and inverse kinematic mapping relationship between the linear velocity and angular velocity of the vehicle centerline based on the wheelbase parameters and the left and right wheel speed parameters to obtain the differential kinematic model.

[0164] The parameter configuration unit 302 is used to configure the maximum speed threshold and acceleration upper limit values ​​at the vehicle body level and wheel end level respectively according to the differential kinematics model, so as to obtain a multi-dimensional motion constraint parameter set.

[0165] The speed constraint unit 303 is used to execute the speed limiting and acceleration constraints at the vehicle body level and the wheel end level respectively on the received original speed command using the multi-dimensional motion constraint parameter set, so as to obtain hierarchical constraint speed data;

[0166] The speed adjustment unit 304 is used to synchronously scale the wheel speeds with excessive wheel-end acceleration in the hierarchical constraint speed data according to a preset difference maintenance rule, and then back-calculate through a preset forward kinematics model to obtain the redistributed speed data.

[0167] The speed optimization unit 305 is used to calculate the deviation between the target turning radius and the actual turning radius based on the redistributed speed data and the original speed command, and to keep the angular velocity unchanged when the deviation exceeds the limit in order to correct the linear velocity and obtain an optimized speed command.

[0168] The instruction output unit 306 is used to collect operation feedback data based on the optimized speed instruction, and to adjust the multidimensional motion constraint parameter set according to the operation feedback data for iterative optimization to obtain the trajectory optimization control instruction.

[0169] In this embodiment, the model building unit 301 acquires the wheelbase parameters and left and right wheel speed parameters of the differential robot, and establishes the forward and inverse kinematic mapping relationship between the linear velocity and angular velocity of the vehicle centerline based on the wheelbase parameters and the left and right wheel speed parameters, thus obtaining a differential kinematic model; the parameter configuration unit 302 configures the maximum speed threshold and acceleration upper limit values ​​at the vehicle body level and wheel end level respectively according to the differential kinematic model, thus obtaining a multi-dimensional motion constraint parameter set; the speed constraint unit 303 uses the multi-dimensional motion constraint parameter set to execute the speed limiting and acceleration constraints at the vehicle body level and wheel end level respectively on the received original speed command, thus obtaining hierarchical constraint speed data; speed adjustment. Unit 304 synchronously scales the wheel speeds with excessive wheel-end acceleration in the hierarchical constraint speed data according to a preset difference maintenance rule, and then performs back-calculation through a preset forward kinematic model to obtain redistributed speed data; Speed ​​optimization unit 305 calculates the deviation between the target turning radius and the actual turning radius based on the redistributed speed data and the original speed command, and keeps the angular velocity unchanged when the deviation exceeds the limit to correct the linear velocity, thereby obtaining an optimized speed command; Command output unit 306 collects operation feedback data based on the optimized speed command, and adjusts the multidimensional motion constraint parameter set according to the operation feedback data for iterative optimization to obtain a trajectory optimization control command.

[0170] In one embodiment, the parameter configuration unit 302 is specifically used for:

[0171] The maximum linear velocity and maximum angular velocity of the vehicle body are configured separately.

[0172] By constraining the absolute value of the linear velocity to not exceed the maximum linear velocity of the vehicle body and the absolute value of the angular velocity to not exceed the maximum angular velocity of the vehicle body, the maximum speed constraint condition at the vehicle body level is obtained.

[0173] The linear velocities of the left and right wheels are calculated using the inverse kinematic mapping relationship of the differential kinematic model, and the maximum permissible linear velocities of the left and right wheels are configured according to the rated parameters of the drive motor.

[0174] By constraining the absolute value of the linear velocity of the left wheel to not exceed the maximum permissible linear velocity of the left wheel and the absolute value of the linear velocity of the right wheel to not exceed the maximum permissible linear velocity of the right wheel, the maximum speed constraint condition at the wheel end level is obtained.

[0175] The maximum speed constraint condition at the wheel end level is set to have a higher priority than the maximum speed constraint condition at the vehicle body level to obtain the maximum speed constraint parameters.

[0176] In one embodiment, the parameter configuration unit 302 is further specifically used for:

[0177] Based on the differential kinematic model, the rate of change of the linear velocity in adjacent control cycles is set to linear acceleration, and the absolute value of the linear acceleration is constrained not to exceed the preset maximum linear acceleration upper limit, thus obtaining the linear acceleration constraint condition.

[0178] Based on the differential kinematic model, the rate of change of the angular velocity in adjacent control cycles is set to angular acceleration, and the absolute value of the angular acceleration is constrained not to exceed the preset maximum angular acceleration upper limit, thus obtaining the angular acceleration constraint condition;

[0179] The change rates of the linear velocity of the left wheel and the linear velocity of the right wheel in adjacent control cycles are respectively set to the acceleration of the left wheel and the acceleration of the right wheel, and the corresponding absolute values ​​are constrained not to exceed the corresponding maximum acceleration limits of the left wheel and the right wheel, respectively, and the wheel acceleration constraint conditions are obtained by integrating them.

[0180] By integrating the linear acceleration constraint, the angular acceleration constraint, and the wheel acceleration constraint, acceleration constraint parameters are obtained.

[0181] In one embodiment, the speed constraint unit 303 is specifically used for:

[0182] Based on the original speed command, the linear velocity and the angular velocity are limited according to the maximum linear velocity and the maximum angular velocity of the vehicle body to obtain the initial value of the vehicle body speed;

[0183] Based on the optimized vehicle speed of the previous frame and the time interval between adjacent frames, the initial value of the vehicle speed is subjected to acceleration constraints to obtain candidate vehicle speeds.

[0184] Based on the inverse kinematic mapping relationship of the differential kinematic model, the candidate initial values ​​of the left wheel speed and the right wheel speed are calculated respectively according to the candidate vehicle speed, and the wheel speed candidate initial value data are integrated to obtain the wheel speed candidate initial value data.

[0185] Based on the maximum permissible linear velocity of the left wheel and the maximum permissible linear velocity of the right wheel, the candidate initial values ​​of wheel speed are respectively limited to obtain candidate values ​​of left wheel speed and right wheel speed.

[0186] Based on the optimized left and right wheel speed values ​​from the previous frame, calculate the wheel-end accelerations corresponding to the candidate left and right wheel speed values, and verify the hierarchical constraint speed data.

[0187] In one embodiment, the speed adjustment unit 304 is specifically used for:

[0188] The hierarchical constraint speed data are respectively set with difference ratio constraints, wheel end dual constraints, and maximum feasible scaling factor constraints, and integrated to obtain the core constraint rules;

[0189] Based on the core constraint rules, the maximum scaling factor of the left wheel acceleration and the maximum scaling factor of the right wheel acceleration are calculated by taking the minimum value of the two, according to the ratio of the product of the upper limit of the left and right wheel acceleration and the time interval to the corresponding wheel speed change.

[0190] Based on the ratio of the maximum permissible linear velocity of the left wheel and the maximum permissible linear velocity of the right wheel to the corresponding candidate wheel speed values, calculate the maximum scaling factor of the maximum speed of the left wheel and the maximum scaling factor of the maximum speed of the right wheel respectively, and take the minimum value of the two to obtain the maximum speed constraint scaling factor;

[0191] The acceleration constraint scaling factor is compared with the maximum velocity constraint scaling factor, and the minimum value is taken to obtain the comprehensive scaling factor.

[0192] The candidate values ​​of left wheel speed and right wheel speed are synchronously and proportionally scaled using the comprehensive scaling factor to obtain the redistributed left wheel speed value and the redistributed right wheel speed value.

[0193] Based on the positive kinematic mapping relationship of the differential kinematic model, the new vehicle linear velocity and new vehicle angular velocity are calculated according to the left wheel speed value and the redistributed right wheel speed value, and the redistributed speed data is obtained by integrating them.

[0194] In one embodiment, the speed optimization unit 305 is specifically used for:

[0195] Based on whether the absolute value of the angular velocity in the original speed command is greater than a preset threshold, the absolute value of the ratio of the linear velocity to the angular velocity or infinity is calculated to obtain the target turning radius;

[0196] Based on whether the absolute value of the new vehicle body angular velocity in the redistributed speed data is greater than a preset threshold, the absolute value of the ratio of the new vehicle body linear velocity to the new vehicle body angular velocity or infinity is calculated to obtain the actual turning radius.

[0197] When the target turning radius is a finite value and the absolute value of the new vehicle body angular velocity is greater than a preset threshold, the absolute value of the difference between the actual turning radius and the target turning radius is calculated to obtain the deviation verification result;

[0198] When the deviation verification result is out of limit, the angular velocity is kept constant. The linear velocity is corrected by multiplying the target turning radius by the absolute value of the new vehicle body angular velocity and retaining the directional sign of the new vehicle body linear velocity, so as to obtain the optimized vehicle body velocity.

[0199] The optimized vehicle speed is subjected to a maximum speed limit and acceleration verification correction process to obtain the verified vehicle speed.

[0200] The vehicle speed after verification is converted into the final left wheel speed value and the final right wheel speed value by using the inverse kinematic mapping relationship of the differential kinematic model, and the maximum speed constraint condition at the wheel end level is verified to obtain the optimized speed command.

[0201] In one embodiment, the instruction output unit 306 is specifically used for:

[0202] Based on the optimized speed command, motor status data and trajectory deviation data are collected in real time.

[0203] Based on the motor status data, load changes are identified and the upper limit of acceleration is adjusted proportionally to obtain adaptive acceleration parameters;

[0204] Based on the adaptive acceleration parameters, the allowable error threshold for the turning radius is adjusted according to the deviation trend based on the trajectory deviation data to obtain the adaptive turning radius threshold.

[0205] Based on the adaptive turning radius threshold, the current operating condition is determined and the maximum linear speed of the vehicle body is adjusted according to the operating condition to obtain the adaptive maximum speed parameter;

[0206] The adaptive acceleration parameters, the adaptive turning radius threshold, and the adaptive maximum speed parameters are updated to the multidimensional motion constraint parameter set and continuously iterated and optimized to obtain trajectory optimization control commands.

[0207] Since the embodiments of the apparatus and the embodiments of the method correspond to each other, please refer to the description of the embodiments of the method for the embodiments of the apparatus, which will not be repeated here.

[0208] This invention also provides a computer-readable storage medium storing a computer program thereon, which, when executed, can perform the steps provided in the above embodiments. The storage medium may include various media capable of storing program code, such as a USB flash drive, a portable hard drive, a read-only memory (ROM), a random access memory (RAM), a magnetic disk, or an optical disk.

[0209] This invention also provides a computer device, which may include a memory and a processor. The memory stores a computer program, and when the processor calls the computer program in the memory, it can implement the steps provided in the above embodiments. Of course, the computer device may also include various network interfaces, a power supply, a graphics card, etc., to utilize the graphics card's performance to operate the model, such as for inference and training.

[0210] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on its differences from other embodiments. Similar or identical parts between embodiments can be referred to interchangeably. For the systems disclosed in the embodiments, since they correspond to the methods disclosed in the embodiments, the descriptions are relatively simple; relevant parts can be referred to in the method section. It should be noted that those skilled in the art can make various improvements and modifications to this application without departing from the principles of this application, and these improvements and modifications also fall within the protection scope of the claims of this application.

[0211] It should also be noted that, in this specification, relational terms such as "first" and "second" are used only to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitations, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element.

Claims

1. A trajectory optimization method for a differential robot, characterized in that, include: Obtain the wheelbase parameters and left and right wheel speed parameters of the differential robot, and establish the forward and inverse kinematic mapping relationship between the linear velocity and angular velocity of the vehicle centerline based on the wheelbase parameters and the left and right wheel speed parameters to obtain the differential kinematic model; Based on the differential kinematics model, the maximum speed threshold and acceleration upper limit values ​​for the vehicle body level and wheel end level are configured respectively to obtain a multi-dimensional motion constraint parameter set; Using the multidimensional motion constraint parameter set, the received original speed command is subjected to speed limiting and acceleration constraints at the vehicle body level and the wheel end level, respectively, to obtain hierarchical constraint speed data; The wheel speeds with excessive wheel-end acceleration in the hierarchical constraint speed data are synchronously scaled according to a preset difference maintenance rule, and then back-calculated using a preset forward kinematics model to obtain the redistributed speed data; Based on the redistributed speed data and the original speed command, the deviation between the target turning radius and the actual turning radius is calculated, and when the deviation exceeds the limit, the angular velocity is kept constant to correct the linear velocity, thereby obtaining an optimized speed command; Based on the optimized speed command, operational feedback data is collected, and the multidimensional motion constraint parameter set is adjusted according to the operational feedback data for iterative optimization to obtain the trajectory optimization control command; The process involves configuring maximum speed thresholds and acceleration limits at both the vehicle body and wheel end levels based on the differential kinematics model, resulting in a multi-dimensional motion constraint parameter set. This includes: configuring the maximum linear velocity and maximum angular velocity of the vehicle body; constraining the absolute values ​​of the linear velocity and angular velocity to not exceed the maximum linear velocity and angular velocity respectively, thus obtaining vehicle body-level maximum speed constraints; calculating the linear velocities of the left and right wheels using the inverse kinematic mapping relationship of the differential kinematics model, and configuring the maximum permissible linear velocities of the left and right wheels based on the rated parameters of the drive motor; constraining the absolute values ​​of the left and right wheel linear velocities to not exceed the maximum permissible linear velocities and angular velocity respectively, thus obtaining wheel end-level maximum speed constraints; and setting the priority of the wheel end-level maximum speed constraints to be higher than that of the vehicle body-level maximum speed constraints, thus obtaining maximum speed constraint parameters. The step of using the multi-dimensional motion constraint parameter set to execute the vehicle-level and wheel-end-level speed limiting and acceleration constraints on the received original speed command to obtain hierarchical constraint speed data includes: limiting the linear velocity and angular velocity according to the original speed command based on the maximum linear velocity and maximum angular velocity of the vehicle body to obtain an initial value of the vehicle body speed; applying acceleration constraints to the initial value of the vehicle body speed based on the optimized vehicle body speed of the previous frame and the time interval between adjacent frames to obtain candidate vehicle body speeds; calculating the initial values ​​of the left wheel speed and the right wheel speed based on the inverse kinematic mapping relationship of the differential kinematic model, and integrating them to obtain initial wheel speed data; limiting the initial value of the wheel speed data based on the maximum permissible linear velocity of the left wheel and the maximum permissible linear velocity of the right wheel to obtain candidate values ​​of the left wheel speed and right wheel speed; and calculating the wheel-end accelerations corresponding to the candidate values ​​of the left wheel speed and the right wheel speed based on the optimized left and right wheel speeds of the previous frame, and verifying the obtained hierarchical constraint speed data. The process of synchronously scaling wheel speeds with excessive wheel-end acceleration in the hierarchical constraint speed data according to a preset difference maintenance rule, and then back-calculating using a preset forward kinematics model to obtain redistributed speed data, includes: setting difference ratio constraints, wheel-end dual constraints, and maximum feasible scaling coefficient constraints for the hierarchical constraint speed data, and integrating them to obtain core constraint rules; based on the core constraint rules, calculating the maximum scaling coefficient of the left wheel acceleration and the maximum scaling coefficient of the right wheel acceleration respectively according to the ratio of the product of the upper limit of the left and right wheel-end acceleration and the time interval to the corresponding wheel speed change, and taking the minimum of the two to obtain the acceleration constraint scaling coefficient; and based on the maximum allowable linear velocity of the left wheel and the maximum allowable linear velocity of the right wheel... The ratio of the corresponding candidate wheel speed values ​​is used to calculate the maximum scaling factor for the left wheel maximum speed and the maximum scaling factor for the right wheel maximum speed, and the minimum value of the two is taken to obtain the maximum speed constraint scaling factor. The acceleration constraint scaling factor is compared with the maximum speed constraint scaling factor and the minimum value is taken to obtain the comprehensive scaling factor. The candidate left wheel speed value and the candidate right wheel speed value are synchronously and proportionally scaled using the comprehensive scaling factor to obtain the redistributed left wheel speed value and the redistributed right wheel speed value. Based on the positive kinematic mapping relationship of the differential kinematic model, the new vehicle linear velocity and the new vehicle angular velocity are calculated according to the left wheel speed value and the redistributed right wheel speed value, and the redistributed speed data is integrated to obtain the redistributed speed data. The process of calculating the deviation between the target turning radius and the actual turning radius based on the redistributed speed data and the original speed command, and maintaining the angular velocity unchanged to correct the linear velocity when the deviation exceeds a limit, to obtain an optimized speed command, includes: calculating the absolute value of the ratio of the linear velocity to the angular velocity or infinity based on whether the absolute value of the angular velocity in the original speed command is greater than a preset threshold, to obtain the target turning radius; calculating the absolute value of the ratio of the new vehicle body angular velocity to the new vehicle body linear velocity or infinity based on whether the absolute value of the new vehicle body angular velocity in the redistributed speed data is greater than a preset threshold, to obtain the actual turning radius; and when the target turning radius is a finite value and the absolute value of the new vehicle body angular velocity is greater than a preset threshold, to obtain the target turning radius; and when the absolute value of the new vehicle body angular velocity is greater than a preset threshold, calculating the absolute value of the ratio of the new vehicle body linear velocity to the new vehicle body angular velocity or infinity, to obtain the actual turning radius; and when the target turning radius is a finite value and ... calculating the actual turning radius; and when the target turning radius is a finite value and the absolute value of the new vehicle body angular velocity is greater than a preset threshold, calculating the absolute value of the ratio of the new vehicle body linear velocity to the new vehicle body angular velocity. When the value is greater than a preset threshold, the absolute value of the difference between the actual turning radius and the target turning radius is calculated to obtain the deviation verification result. When the deviation verification result is out of limit, the angular velocity is kept constant, and the linear velocity is corrected by multiplying the target turning radius by the absolute value of the new vehicle body angular velocity and retaining the directional sign of the new vehicle body linear velocity to obtain the optimized vehicle body speed. The optimized vehicle body speed is then subjected to vehicle body maximum speed limit and acceleration verification correction processing to obtain the verified vehicle body speed. The verified vehicle body speed is converted into the final left wheel speed value and the final right wheel speed value through the inverse kinematic mapping relationship of the differential kinematic model, and the wheel end level maximum speed constraint condition is verified to obtain the optimized speed command.

2. The trajectory optimization method for a differential robot according to claim 1, characterized in that, The step of configuring the maximum velocity threshold and acceleration upper limit values ​​at the vehicle body level and wheel end level respectively according to the differential kinematic model to obtain a multidimensional motion constraint parameter set also includes: Based on the differential kinematic model, the rate of change of the linear velocity in adjacent control cycles is set to linear acceleration, and the absolute value of the linear acceleration is constrained not to exceed the preset maximum linear acceleration upper limit, thus obtaining the linear acceleration constraint condition. Based on the differential kinematic model, the rate of change of the angular velocity in adjacent control cycles is set to angular acceleration, and the absolute value of the angular acceleration is constrained not to exceed the preset maximum angular acceleration upper limit, thus obtaining the angular acceleration constraint condition; The change rates of the linear velocity of the left wheel and the linear velocity of the right wheel in adjacent control cycles are respectively set to the acceleration of the left wheel and the acceleration of the right wheel, and the corresponding absolute values ​​are constrained not to exceed the corresponding maximum acceleration limits of the left wheel and the right wheel, respectively, and the wheel acceleration constraint conditions are obtained by integrating them. By integrating the linear acceleration constraint, the angular acceleration constraint, and the wheel acceleration constraint, acceleration constraint parameters are obtained.

3. The trajectory optimization method for a differential robot according to claim 1, characterized in that, The process of collecting operational feedback data based on the optimized speed command and iteratively optimizing the multidimensional motion constraint parameter set according to the operational feedback data to obtain trajectory optimization control commands includes: Based on the optimized speed command, motor status data and trajectory deviation data are collected in real time. Based on the motor status data, load changes are identified and the upper limit of acceleration is adjusted proportionally to obtain adaptive acceleration parameters; Based on the adaptive acceleration parameters, the allowable error threshold for the turning radius is adjusted according to the deviation trend based on the trajectory deviation data to obtain the adaptive turning radius threshold. Based on the adaptive turning radius threshold, the current operating condition is determined and the maximum linear speed of the vehicle body is adjusted according to the operating condition to obtain the adaptive maximum speed parameter; The adaptive acceleration parameters, the adaptive turning radius threshold, and the adaptive maximum speed parameters are updated to the multidimensional motion constraint parameter set and continuously iterated and optimized to obtain trajectory optimization control commands.

4. A trajectory optimization device for a differential robot, characterized in that, include: The model building unit is used to obtain the wheelbase parameters and left and right wheel speed parameters of the differential robot, and establish the forward and inverse kinematic mapping relationship between the linear velocity and angular velocity of the vehicle centerline based on the wheelbase parameters and the left and right wheel speed parameters to obtain the differential kinematic model. The parameter configuration unit is used to configure the maximum speed threshold and acceleration upper limit values ​​at the vehicle body level and wheel end level respectively according to the differential kinematics model, so as to obtain a multi-dimensional motion constraint parameter set. The speed constraint unit is used to execute the speed limiting and acceleration constraints at the vehicle body level and the wheel end level respectively on the received original speed command using the multi-dimensional motion constraint parameter set, so as to obtain hierarchical constraint speed data; The speed adjustment unit is used to synchronously scale the wheel speeds with excessive wheel-end acceleration in the hierarchical constraint speed data according to a preset difference maintenance rule, and then back-calculate through a preset forward kinematics model to obtain the redistributed speed data. The speed optimization unit is used to calculate the deviation between the target turning radius and the actual turning radius based on the redistributed speed data and the original speed command, and to keep the angular velocity unchanged when the deviation exceeds the limit in order to correct the linear velocity and obtain an optimized speed command. The instruction output unit is used to collect operation feedback data based on the optimized speed instruction, and to adjust the multi-dimensional motion constraint parameter set according to the operation feedback data for iterative optimization to obtain the trajectory optimization control instruction; The parameter configuration unit is specifically used to configure the maximum linear velocity and maximum angular velocity of the vehicle body respectively; to constrain the absolute values ​​of the linear velocities and angular velocities to not exceed the maximum linear velocity and maximum angular velocity of the vehicle body respectively, thus obtaining vehicle-level maximum speed constraints; to calculate the linear velocities of the left and right wheels through the inverse kinematic mapping relationship of the differential kinematic model, and to configure the maximum permissible linear velocities of the left and right wheels respectively according to the rated parameters of the drive motor; to constrain the absolute values ​​of the linear velocities of the left and right wheels to not exceed the maximum permissible linear velocities of the left and right wheels respectively, thus obtaining wheel-end level maximum speed constraints; and to set the priority of the wheel-end level maximum speed constraints to be higher than that of the vehicle-level maximum speed constraints, thus obtaining maximum speed constraint parameters. The speed constraint unit is specifically used to limit the linear velocity and the angular velocity according to the original speed command based on the maximum linear velocity and maximum angular velocity of the vehicle body to obtain an initial value of the vehicle body speed; and to apply acceleration constraints to the initial value of the vehicle body speed based on the vehicle body speed optimized in the previous frame and the time interval between adjacent frames to obtain a candidate vehicle body speed. Based on the inverse kinematic mapping relationship of the differential kinematic model, the initial candidate values ​​of the left wheel speed and the right wheel speed are calculated according to the candidate vehicle speeds, and integrated to obtain the initial candidate wheel speed data; the initial candidate wheel speed data are limited based on the maximum permissible linear velocity of the left wheel and the maximum permissible linear velocity of the right wheel to obtain the candidate values ​​of the left wheel speed and the right wheel speed. Based on the optimized left and right wheel speed values ​​of the previous frame, calculate the wheel end accelerations corresponding to the candidate left wheel speed values ​​and the candidate right wheel speed values, and verify the hierarchical constraint speed data. The speed adjustment unit is specifically used to set difference ratio constraints, wheel-end dual constraints, and maximum feasible scaling factor constraints for the hierarchical constraint speed data, and integrate them to obtain core constraint rules; based on the core constraint rules, the maximum scaling factor for the left wheel acceleration and the maximum scaling factor for the right wheel acceleration are calculated by dividing the product of the upper limit of the left and right wheel acceleration by the time interval by the corresponding wheel speed change, and the minimum value of the two is taken to obtain the acceleration constraint scaling factor; based on the ratio of the maximum allowable linear velocity of the left wheel and the maximum allowable linear velocity of the right wheel to the corresponding wheel speed candidate value, the maximum scaling factor for the maximum speed of the left wheel and the maximum speed of the right wheel are calculated, and the minimum value of the two is taken to obtain the maximum speed constraint scaling factor; the acceleration constraint scaling factor is compared with the maximum speed constraint scaling factor and the minimum value is taken to obtain the comprehensive scaling factor; The candidate values ​​of left wheel speed and right wheel speed are synchronously and proportionally scaled using the comprehensive scaling factor to obtain the redistributed left wheel speed value and the redistributed right wheel speed value. Based on the positive kinematic mapping relationship of the differential kinematic model, the new vehicle linear velocity and the new vehicle angular velocity are calculated according to the left wheel speed value and the redistributed right wheel speed value, and the redistributed speed data is integrated to obtain the redistributed speed data. The speed optimization unit is specifically used to calculate the absolute value or infinity of the ratio of linear velocity to angular velocity based on whether the absolute value of the angular velocity in the original speed command is greater than a preset threshold, to obtain the target turning radius; to calculate the absolute value or infinity of the ratio of the new vehicle body linear velocity to the new vehicle body angular velocity based on whether the absolute value of the new vehicle body angular velocity in the redistributed speed data is greater than a preset threshold, to obtain the actual turning radius; when the target turning radius is a finite value and the absolute value of the new vehicle body angular velocity is greater than a preset threshold, to calculate the absolute value of the difference between the actual turning radius and the target turning radius, to obtain a deviation verification result; when the deviation verification result is out of limit, to keep the angular velocity unchanged, to correct the linear velocity by using the product of the target turning radius and the absolute value of the new vehicle body angular velocity and retaining the direction sign of the new vehicle body linear velocity, to obtain the optimized vehicle body speed; and to perform maximum vehicle body speed limit and acceleration verification correction processing on the optimized vehicle body speed to obtain the verified vehicle body speed. The vehicle speed after verification is converted into the final left wheel speed value and the final right wheel speed value by using the inverse kinematic mapping relationship of the differential kinematic model, and the maximum speed constraint condition at the wheel end level is verified to obtain the optimized speed command.

5. A computer device, characterized in that, It includes a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the trajectory optimization method for a differential robot as described in any one of claims 1 to 3.

6. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a computer program that, when executed by a processor, implements the trajectory optimization method for the differential robot as described in any one of claims 1 to 3.

Images