Dynamic path planning control method for adaptable multi-motion mode robot chassis

Abstract

The application discloses a dynamic path planning control method of a robot chassis adaptable to multiple motion modes. The method comprises the following steps: firstly, acquiring current system state information of the robot chassis; then, selecting an optimal motion mode of a current control period in combination with an optimal motion mode of a previous control period and the current system state information of the robot chassis; under the optimal motion mode of the current control period, performing weighted update on a control input sequence through path integral mode and issuing the first control input in the updated control input sequence to a robot chassis actuator; constantly acquiring system state information of the robot chassis and rolling update of the control input sequence and issuing the control input sequence to the robot chassis actuator, so as to realize planning control of a robot chassis path. The application realizes smooth switching between multiple motion modes and dynamic control optimization, and improves adaptability, stability and control precision of a robot chassis with multiple motion modes in a complex scene.

Description

Technical Field

[0001] This invention relates to a dynamic path planning and control method for a robot chassis in the field of intelligent mobile robot control technology, specifically to a dynamic path planning and control method for a robot chassis adaptable to multiple motion modes. Background Technology

[0002] Achieving high-performance, robust online trajectory optimization has always been a significant challenge in robot and intelligent vehicle control. Especially in task scenarios characterized by rapid dynamic changes, strong nonlinearity, and uncertainty, traditional hierarchical control often lacks sufficient responsiveness and flexibility. To address this, the Model Predictive Path Integral Control (MMCI) algorithm, proposed in recent years, has become an effective method for solving nonlinear optimal control problems.

[0003] MPPI (Model Predictive Path Integral) was initially designed for scenarios such as path planning for autonomous vehicles under extreme control conditions, motion generation for quadruped / bipedal robots in complex terrain, and real-time trajectory optimization for highly maneuverable aircraft. These tasks generally have the following characteristics: strong nonlinearity, complex system models, high-speed decision-making, real-time control required, and trajectories that cannot be pre-planned and must be dynamically generated based on the state.

[0004] Unlike traditional MPC (Model Predictive Control), MPPI is based on the stochastic sampling theory of path integrals. It optimizes the control input by probabilistically modeling and sampling the importance of future trajectories. This mechanism offers several advantages: it eliminates the need for derivative solving, making it suitable for complex or non-differentiable dynamic systems; it is compatible with arbitrary cost functions, offering high design freedom; it can be implemented in parallel, adapting to GPU acceleration to meet real-time control requirements; and it demonstrates good robustness and maneuverability in practical tasks.

[0005] However, MPPI also reveals certain limitations when dealing with omnidirectional robot chassis with switchable motion modes, such as four-wheel chassis: switching motion modes introduces discontinuous or hybrid dynamics models, and the continuous sampling mechanism used by MPPI is difficult to capture such state transition behavior patterns; the path integral is based on a soft optimal distribution, which may ignore high-cost but necessary trajectory segments that have significance for policy switching, resulting in conservative policy convergence and difficulty in crossing mode boundaries. Summary of the Invention

[0006] To address the problems of unstable control switching, large path errors, and low control efficiency in multi-motion-mode robot chassis under complex task environments, this invention proposes a dynamic path planning and control method adaptable to multi-motion-mode robot chassis. The proposed method integrates motion mode discrimination, trajectory sampling optimization, path integral weighted control updates, and a rolling execution feedback mechanism to achieve efficient control of the robot chassis in multi-task, multi-state environments.

[0007] The technical solution of the present invention is as follows:

[0008] I. A dynamic path planning and control method for robot chassis adaptable to multiple motion modes

[0009] Step S1: Obtain the current system status information of the robot chassis;

[0010] Step S2: Combining the optimal motion mode of the previous control cycle with the current system state information of the robot chassis, select the optimal motion mode for the current control cycle from all motion modes of the robot chassis.

[0011] Step S3: In the optimal mode of the current control cycle, the control input sequence is updated by weighting using path integration, and the first control input in the updated control input sequence is sent to the robot chassis actuator;

[0012] Step S4: Repeat steps S1-S3 to continuously acquire the system status information of the robot chassis and continuously update the control input sequence and send it to the robot chassis actuator, thereby realizing the planning and control of the robot chassis path.

[0013] Step S2 specifically involves:

[0014] Step S21: Based on the current system state information of the robot chassis, select and construct a set of candidate motion modes from all motion modes of the robot chassis; then, based on the spatial dimension and complexity of the control target, assign the corresponding number of trajectory samples to different candidate motion modes in the set of candidate motion modes.

[0015] Step S22: Generate a set of control input disturbances; then, based on the current set of control input disturbances and the number of trajectory samples corresponding to each candidate motion mode, perform multi-trajectory forward simulation under each candidate motion mode to obtain several sampled trajectories corresponding to each candidate motion mode.

[0016] Step S23: Combining the optimal motion mode of the previous control cycle and the current system state information of the robot chassis, calculate the cost corresponding to each sampled trajectory in each candidate motion mode, and then calculate the average total cost corresponding to each candidate motion mode. The candidate motion mode with the smallest average total cost is taken as the optimal motion mode for the current control cycle.

[0017] The step of selecting and constructing a candidate motion mode set from all motion modes of the robot chassis based on the current system state information of the robot chassis includes:

[0018] First, based on the chassis position coordinates, heading angle θ, and target point coordinates from the current system status information of the robot chassis, calculate the orientation angle error |Δθ|, global straight-line distance d, and forward displacement |x|. rel |;

[0019] Based on the orientation angle error |Δθ|, the global straight-line distance d, and the forward displacement |x rel Candidate motion modes are selected from all motion modes of the robot chassis according to the following modal discrimination rules, thereby constructing a candidate motion mode set:

[0020] Ackermann-like patterns are used as candidate motion patterns by default;

[0021] If the direction angle error |Δθ| < the parallel judgment threshold δ align The omnidirectional movement mode was selected as the candidate motion mode.

[0022] If the direction angle error |Δθ| < the parallel judgment threshold δ align And the forward displacement |x rel |< Lateral slip judgment value ε x Lateral sliding mode is selected as a candidate motion mode, while omnidirectional movement mode is removed from the candidate motion mode set.

[0023] If the direction angle error |Δθ| > steering threshold δ rotate The stationary rotation mode is selected as the candidate motion mode; if the direction angle error |Δθ| > the angle adjustment threshold δ minor And the global straight-line distance d < the proximity judgment distance d close The stationary rotation mode was also selected as a candidate motion mode.

[0024] The process of allocating corresponding trajectory sampling numbers to different candidate motion patterns in the candidate motion pattern set based on the spatial dimension and complexity of the control target includes:

[0025] Based on the spatial dimension and complexity of the control target, corresponding sampling weight coefficients are set for all motion modes of the robot chassis, where the sampling weight coefficient α for the Ackermann-like mode is... ackermann : Sampling weight coefficient α for omnidirectional movement mode holonomic : Sampling weighting coefficient α for lateral sliding mode lateral : Sampling weighting coefficient α for in-place rotation mode rotate= [0.6,0.8]: [0.1,0.2]: [0.1,0.15]: [0.05,0.1]; If the candidate motion pattern set does not contain one or more of the omnidirectional movement mode, lateral sliding mode, and stationary rotation mode, its corresponding sampling weight coefficient is 0, and the sum of the sampling weight coefficients of the candidate motion patterns in the candidate motion pattern set is equal to 1; Finally, the number of trajectory samples is allocated according to the ratio of the sampling weight coefficients of the candidate motion patterns in the candidate motion pattern set.

[0026] The process of combining the optimal motion mode from the previous control cycle with the current system state information of the robot chassis to calculate the cost corresponding to each sampled trajectory under each candidate motion mode includes:

[0027]

[0028]

[0029] Among them, S (k) Let k be the composite cost of the sampling trajectory. This represents the cumulative running cost of the sampled trajectory k at each time step during the prediction period; Represents the terminal state cost of the sampling trajectory k; Represents the mode switching cost of sampling trajectory k; The cost of switching the basic mode for the sampling trajectory k; This is the cost of the velocity discontinuity of the sampling trajectory k; The cost is the discontinuity in the direction angle of the sampled trajectory k; and Let v represent the linear velocity vectors at step j and step (j+1), respectively; x,j v y,j Let represent the x and y components of the linear velocity vector at step j, respectively; N represents the total number of steps; |||| represents the magnitude of the vector; w v Penalty weights for speed changes; and These are the velocity direction angles at step j and step (j+1), respectively; w θ The penalty weight is applied to changes in the orientation angle.

[0030] The calculation of the average total cost corresponding to each candidate motion pattern includes:

[0031] For each candidate motion mode, the costs of all sampled trajectories are sorted in ascending order, and the top η values ​​in the ascending cost queue are retained. retain % of the sampled trajectories and obtain the effective trajectory set of the candidate motion mode; calculate the average total cost corresponding to the candidate motion mode based on the effective trajectory set of the candidate motion mode; traverse and process different candidate motion modes and calculate the corresponding average total cost.

[0032] II. A dynamic path planning control system adaptable to multi-motion mode robot chassis

[0033] The sensor fusion state estimation module is used to obtain the system state information of the robot chassis;

[0034] The optimal motion mode generation module is used to combine the optimal motion mode of the previous control cycle with the current system state information of the robot chassis to select the optimal motion mode for the current control cycle from all motion modes of the robot chassis.

[0035] The control input sequence update module is used to perform a weighted update of the control input sequence in the optimal mode of the current control cycle by means of path integration, and send the first control input in the updated control input sequence to the robot chassis actuator.

[0036] III. A computer device

[0037] The device includes a memory and a processor. The memory stores a computer program, and the processor executes the computer program to implement the steps of the dynamic path planning and control method for a robot chassis adaptable to multiple motion modes.

[0038] IV. A computer-readable storage medium

[0039] The medium stores a computer program, which, when executed by a processor, implements the steps of a dynamic path planning and control method for a robot chassis adaptable to multiple motion modes.

[0040] V. A computer program product

[0041] The product includes a computer program / instruction that, when executed by a processor, implements the steps of the dynamic path planning and control method for a multi-motion-mode robot chassis.

[0042] The beneficial effects of this invention are as follows:

[0043] This invention combines the state evolution patterns of a robot chassis under different time periods and task scenarios with the system response characteristics of various typical motion modes. It achieves real-time discrimination, cost modeling, and dynamic switching of multiple motion modes, such as omnidirectional movement, lateral sliding, Ackermann-like mode, and stationary rotation. This solves the problems of path deviation, increased energy consumption, and discontinuous actions caused by fixed strategies or unsuitable responses in traditional control systems, and improves the adaptability and decision-making intelligence of the control system in complex dynamic environments.

[0044] The method proposed in this invention improves the search efficiency of the controller in the high-dimensional action space through structured trajectory perturbation sampling, multimodal joint simulation and cost weight adjustment mechanism, and optimizes the quality of the final control command through trajectory screening and control update mechanism, thus achieving the goal of balancing control accuracy and real-time performance.

[0045] Furthermore, by introducing trajectory preservation, modal reset, and instruction rolling mechanisms, this invention enables the robot chassis system to possess stronger anomaly suppression capabilities and behavioral stability. It exhibits good versatility and practical value in scenarios such as multimodal task planning, multi-target path tracking, and dynamic obstacle avoidance. Without significantly increasing computational resource overhead, it achieves efficient, stable, and scalable control of the multimodal autonomous chassis platform. Attached Figure Description

[0046] Figure 1 This is a schematic diagram of the overall process of the present invention.

[0047] Figure 2 This is a detailed flowchart of step S2 of the present invention.

[0048] Figure 3 These are schematic diagrams of various preset modes; where (a) is a motion diagram of the Ackermann-like mode, (b) is a motion diagram of the stationary rotation mode, (c) is a motion diagram of the lateral sliding mode, and (d) is a motion diagram of the omnidirectional movement mode. Detailed Implementation

[0049] To enable those skilled in the art to better understand the present invention and to more clearly define the scope of protection of the present invention, the present invention will be described in detail below with reference to certain specific embodiments and accompanying drawings. It should be noted that the following are only some specific embodiments of the present invention, and are merely a part of the embodiments of the present invention. The specific and direct descriptions of related structures are only for the convenience of understanding the present invention, and the specific features do not necessarily or directly limit the scope of the present invention. Conventional selections and substitutions made by those skilled in the art under the guidance of the present invention, as well as reasonable arrangements and combinations of several technical features under the guidance of the present invention, should all be considered within the scope of protection of the present invention.

[0050] like Figure 1 As shown, the dynamic path planning and control method for a robot chassis adaptable to multiple motion modes proposed in this invention includes the following steps:

[0051] Step S1: Obtain the current system state information of the robot chassis through the sensor fusion state estimation module (which consists of multiple sensors such as IMU and wheel speedometers). The system state information includes the chassis position coordinates, heading angle, angular velocity, linear velocity, and target point coordinates. Specifically, the position coordinates (x, y) of the chassis in the global coordinate system are obtained through laser SLAM as the localization module; the heading angle θ and angular velocity ω of the current four-wheel chassis are obtained through the inertial measurement unit; and the linear velocity of each steering wheel is obtained through the wheel speedometers on the four-wheel chassis and converted into the linear velocity component v of the chassis in the vehicle coordinate system. x ,v y Obtain short-term predicted target points (x) from the path planning task. goal ,y goal ).

[0052] Step S2: Combining the optimal motion mode of the previous control cycle with the current system state information of the robot chassis, select the optimal motion mode for the current control cycle from all motion modes of the robot chassis.

[0053] like Figure 2 As shown, step S2 specifically involves:

[0054] Step S21: Based on the current system state information of the robot chassis, select and construct a set of candidate motion modes from all motion modes of the robot chassis; then, based on the spatial dimension and complexity of the control target, assign the corresponding number of trajectory samples to different candidate motion modes in the set of candidate motion modes.

[0055] Based on the current system state information of the robot chassis, a set of candidate motion modes is selected and constructed from all motion modes of the robot chassis, including:

[0056] First, based on the chassis position coordinates, heading angle θ, and target point coordinates from the current system status information of the robot chassis, calculate the orientation angle error |Δθ|, global straight-line distance d, and forward displacement |x|. rel The specific formula is as follows:

[0057] |Δθ|=|θ target -θ|

[0058] θ target =arctan2(x goal -y,x goal -x)

[0059]

[0060] |x rel |=|cos(θ)·(x goal -x) 2 +sin(θ)·(ygoal -y) 2 |

[0061] Where, θ target The target heading angle represents the direction from the current position to the target point; the heading angle error Δθ represents the angle between the current chassis orientation and the target direction. A larger value indicates a more significant deviation from the target heading, suggesting the need for attitude adjustments; the global straight-line distance d represents the straight-line distance between the chassis and the target point; the forward displacement |x rel | This is the projection of the target point's position relative to the forward direction of the current chassis coordinate system, indicating how far the target point is from the current position along the direction directly in front of the chassis; x goal and y goal These are the two coordinates of the target point, and x and y are the two position coordinates of the robot chassis. || is the absolute value operation.

[0062] The current velocity modulus ||v|| was also calculated, satisfying... Measuring the overall motion intensity of the chassis; velocity direction angle θ v , satisfying θ v =arctan2(v y ,v x This is used to determine whether the direction of movement is consistent with the orientation of the vehicle.

[0063] Based on the orientation angle error |Δθ|, the global straight-line distance d, and the forward displacement |x rel Candidate motion modes are selected from all motion modes of the robot chassis according to the following modal discrimination rules, thereby constructing a candidate motion mode set:

[0064] The Ackermann-like mode is the default candidate motion mode. As a general solution that takes into account both direction adjustment and curve propulsion, the Ackermann-like mode is suitable for most medium-speed navigation tasks.

[0065] If the direction angle error |Δθ| < the parallel judgment threshold δ align In this embodiment, δ align Taking 5° indicates that the chassis is facing the target direction, and the omnidirectional movement mode is selected as the candidate motion mode.

[0066] If the direction angle error |Δθ| < the parallel judgment threshold δ align And the forward displacement |x rel |< Lateral slip judgment value ε x In this embodiment, ε x Taking 0.05m indicates that the chassis is facing the target direction, the forward distance is small, and the lateral displacement is the main one. Therefore, the lateral sliding mode can be used as a candidate motion mode, while the omnidirectional movement mode is removed from the candidate motion mode set.

[0067] If the direction angle error |Δθ| > steering threshold δ rotate In this embodiment, δ rotate Taking 90° indicates a significant deviation from the target direction, requiring substantial attitude adjustment. The stationary rotation mode should be considered the candidate motion mode. If the orientation angle error |Δθ| > the angle adjustment threshold δ... minor In this embodiment, δ minor Take 5°; and the global straight-line distance d < the proximity judgment distance d close In this embodiment, d close If the angle deviation is 0.08m, it indicates that the angle deviation is moderate but the target point is extremely close. Therefore, it is believed that continuing to advance in other modes at this time may lead to error amplification or trajectory overshoot. In-situ attitude adjustment should be prioritized, so the in-situ rotation mode is also selected as a candidate motion mode.

[0068] Based on the spatial dimension and complexity of the control target, the corresponding number of trajectory samples is assigned to different candidate motion patterns in the candidate motion pattern set, including:

[0069] Based on the control target space dimension and complexity, corresponding sampling weight coefficients are set for all motion modes of the robot chassis. The control target space dimension and complexity reflect the differences in functional positioning and strategy requirements of each mode during task execution. Among them, the sampling weight coefficient α of the Ackermann-like mode... ackermann : Sampling weight coefficient α for omnidirectional movement mode holonomic : Sampling weighting coefficient α for lateral sliding mode lateral : Sampling weighting coefficient α for in-place rotation mode rotate = [0.6,0.8]: [0.1,0.2]: [0.1,0.15]: [0.05,0.1]; If the candidate motion pattern set does not contain one or more of the omnidirectional movement mode, lateral sliding mode, and stationary rotation mode, its corresponding sampling weight coefficient is 0, and the sum of the sampling weight coefficients of the candidate motion patterns in the candidate motion pattern set is equal to 1; Finally, the number of trajectory samples is allocated according to the ratio of the sampling weight coefficients of the candidate motion patterns in the candidate motion pattern set.

[0070] The motion diagrams for the Ackermann-like mode, stationary rotation mode, lateral sliding mode, and omnidirectional movement mode are as follows: Figure 3 of (a), Figure 3 (b) Figure 3 (c) and Figure 3 As shown in (d). The specific explanation of the sampling weighting coefficients for each motion mode is as follows:

[0071] Ackermann-like mode control inputs act simultaneously on the system's forward velocity and lateral velocity (v).x ,v y The system, along with its angular velocity ω, exhibits significant state coupling and strong trajectory influence, making it suitable for most task scenarios, including turning, forward adjustment, and path following. Its control strategy space is complex, requiring the largest amount of sampled trajectory data. A suitable preset sampling weight coefficient range is α. ackermann ∈[0.6,0.8]. In this embodiment, α ackermann Take 0.6;

[0072] Omnidirectional movement mode is mainly used to achieve planar movement in any direction while maintaining the same attitude, with control input covering the two-dimensional velocity space (v). x ,v y However, it does not involve attitude adjustment. Although the control variables are two-dimensional, the control target space is limited to a translation task without rotation, which is suitable for task scenarios with relatively open space and weak attitude requirements. Therefore, a moderate number of sampling trajectories are allocated, and the appropriate preset sampling weight coefficient range is α. holonomic ∈[0.1,0.2]. In this embodiment, α holonomic Take 0.2;

[0073] In the lateral slip mode, only the lateral velocity v is involved as the control variable. y However, its target is planar position, which is a direct control dimension of the actual path trajectory. It is mainly used for tasks such as lateral insertion, obstacle avoidance, and path fine-tuning. Although the control variable is one-dimensional, its behavior has a real effect on path changes, so a certain sampling coverage needs to be ensured. Therefore, its weight is slightly higher than that of the rotation mode, and the appropriate preset sampling weight coefficient range is α. lateral ∈[0.1,0.15]. In this embodiment, α lateral Take 0.1;

[0074] In-situ rotation mode: The control variable in this mode is the attitude angular velocity ω, and the control objective is limited to angular space adjustment. In most scenarios, it serves as an auxiliary behavior to prepare for the next action. Its control effect does not directly affect the position of the path trajectory; it only has a significant effect in end-point pose fixation or initial adjustment. Given its fast control strategy space convergence speed and low prediction requirements, a minimum number of sampled trajectories are allocated, and the preset sampling weight coefficient is α. rotate ∈[0.05,0.1], and in this embodiment, it is taken as 0.1.

[0075] Unlike traditional cost-based pattern selection methods, this invention does not directly select a single pattern for trajectory sampling. Instead, it allocates trajectory sampling resources in a differentiated manner, giving the versatile and dynamically changing model a larger exploration space, thereby naturally obtaining more strategy possibilities in the sampling integral.

[0076] It should be emphasized that although both lateral sliding and in-situ rotation are one-dimensional in terms of the number of control variables, the former acts on spatial position while the latter only acts on angular orientation, and their direct impact on the path execution results is different, thus there are reasonable differences in trajectory resource allocation.

[0077] Furthermore, to adapt to the dynamic requirements of different task scenarios, the aforementioned weight parameters can be adjusted online. In tasks requiring precise orientation alignment, the weight parameter α of the in-place rotation mode can be increased. rotate In parking or clamping tasks requiring lateral insertion, the weighting parameter α of the lateral slip mode... lateral This should be improved accordingly. Therefore, during trajectory sampling, the automatic guidance system explores more strategies within the most critical control modes of the current task, enhancing control accuracy and behavioral intelligence.

[0078] Step S22: Generate a set of control input disturbances; then, based on the current set of control input disturbances and the number of trajectory samples corresponding to each candidate motion mode, perform multi-trajectory forward simulation under each candidate motion mode to obtain several sampled trajectories corresponding to each candidate motion mode.

[0079] Step S22 is as follows:

[0080] Load the dynamic model triplet (f) corresponding to the candidate motion mode from the preset model library. m G m B m This serves as the modeling basis for trajectory prediction and control optimization within the current control cycle, where f m G represents the natural state evolution function of the system; m B represents the control input coupling function; m This represents the noise disturbance mapping matrix. The triplet is used to describe the continuous evolution formula of the system state:

[0081]

[0082] Where, x t and x t+1 Let u be the system state variables at time t and time t+1, respectively. t To control the input, ∈ t This represents an independent Gaussian perturbation. Δt represents the time step.

[0083] In omnidirectional movement mode, the motion characteristics allow linear movement in any direction, but turning or adjusting orientation is not possible. The state variables are x = [x, y, θ]. T Control input u = [v x ,v y ] T System model triple (f holo Gholo B holo )as follows:

[0084]

[0085] Where T represents the transpose operation, v x ,v y For the two linear velocity components in the local coordinate system, γ is a scaling factor used to represent the disturbance intensity. It acts on the covariance structure of the disturbance term and is used to control the magnitude of the random disturbance added during trajectory sampling.

[0086] For the lateral slip mode, the motion is characterized by linear movement only along the y-axis. The state variables are x = [x, y, θ]. T , control input u = [v y ], system model triple (f lat G lat B lat )as follows:

[0087]

[0088] For the in-place rotation mode, the motion characteristic is that only the attitude angle is adjusted. The state variables are x = [x, y, θ]. T Control input u = [ω], system model triple (f rot G rot B rot )as follows:

[0089]

[0090] Where ω is the angular velocity.

[0091] For Ackermann-like patterns, the motion characteristics involve adjustments only to the attitude angles. The state variables are x = [x, y, θ]. T Control input u = [v, ω] T System model triple (f ack G ack B ack )as follows:

[0092]

[0093] Among them, I 3×3 It is a 3×3 identity matrix.

[0094] The sum of the sampling weight coefficients of the candidate motion patterns in the candidate motion pattern set is less than or equal to 1, specifically including:

[0095] For the motion modes actually selected in the candidate set, the effective subset is first extracted according to their corresponding preset sampling weights: A = {α} m |m∈M c}, M c For the actual selected motion pattern, α m This represents the preset sampling weight coefficients corresponding to motion mode m; then the weight normalization factor is calculated. Then determine the actual number of trajectory samples obtained for each candidate motion mode m. This indicates a floor operation to ensure that the final sum of the allocations for all modes does not exceed the total number of trajectories K. total If there are remaining trajectories, they are assigned to the motion pattern with the highest weight.

[0096] For each motion mode and its corresponding dynamic model, a structured Gaussian perturbation method is used to generate a set of control inputs. Based on the current set of control input perturbations, multi-trajectory forward simulation is performed to obtain several sampled trajectories corresponding to each candidate mode. Specifically:

[0097] Under each candidate pattern m, each sampling trajectory is numbered k = 1 to K. m The system performs the following trajectory simulation process:

[0098] Initialize the simulation trajectory to its initial state:

[0099]

[0100] in, Let x be the initial state vector of the k-th sampling trajectory. t The system state at the start of the current control cycle; for the predicted time steps j = 0 to N-1, a zero-mean Gaussian perturbation is applied to the control input:

[0101]

[0102] in, Let N(0,σ) be the Gaussian perturbation vector of the k-th trajectory at the j-th step control input. 2 I) represents a multidimensional zero-mean, covariance σ. 2 I follows a normal distribution.

[0103] And construct the disturbance control sequence:

[0104]

[0105] in, u is the disturbance control input for the k-th sampled trajectory at step j. j This is the initial control input for step j. Let be the Gaussian perturbation vector of the k-th sampled trajectory at the j-th step control input;

[0106] Next, the dynamic model of the corresponding mode is used to predict the state evolution:

[0107]

[0108] in, Let be the state vector of the k-th trajectory at step j; Δt represents the simulation step size of the control cycle; Used to maintain unit consistency of noise terms in simulation; f m G represents the natural state evolution function of the system; m B represents the control input coupling function; m This represents the noise perturbation mapping matrix.

[0109] Step S23: Combining the optimal motion mode from the previous control cycle with the current system state information of the robot chassis, calculate the cost corresponding to each sampled trajectory under each candidate motion mode, and then calculate the average total cost corresponding to each candidate motion mode. Select the candidate motion mode with the minimum average total cost as the optimal motion mode for the current control cycle. The process of selecting the candidate motion mode with the minimum average total cost ensures that the system considers the overall trend of trajectory performance while eliminating the influence of a few abnormal trajectories on the mode evaluation, thereby improving the accuracy and stability of the optimal mode selection.

[0110] This invention embodies the core idea of ​​path integral control, namely, achieving control optimization in non-analytical form through a large number of random samples.

[0111] Combining the optimal motion mode from the previous control cycle with the current system state information of the robot chassis, the cost corresponding to each sampled trajectory under each candidate motion mode is calculated, including:

[0112]

[0113]

[0114] Among them, S (k) Let k be the composite cost of the sampling trajectory. This represents the cumulative running cost of the sampled trajectory k at each time step during the prediction period. The time cost term is used to evaluate the degree of deviation between the current position of the trajectory and the target point at each prediction time step, as well as the intensity of control input usage, reflecting the overall performance and energy consumption level of the trajectory during path execution. This represents the terminal state cost of the sampled trajectory k, i.e., the terminal penalty caused by the deviation between the system state and the target point at the end of the prediction. It is used to evaluate whether the trajectory's terminal state successfully achieves precise alignment with the target position and attitude. This part emphasizes the final mission completion accuracy, ensuring that the trajectory is not only feasible in process, but that the terminal state can also be used for subsequent execution. This represents the cost of switching the sampling trajectory k mode, which is the additional cost caused by the mode switching when the candidate motion mode corresponding to the current sampling trajectory is the same as the optimal motion mode in the previous control cycle. This is because in the actual operation of a multi-motion mode system, the switching of control modes usually introduces interruption of system response, energy loss or decrease in control accuracy. The basic mode switching cost of the sampling trajectory k is given by the mode switching cost matrix preset according to the actual situation of the robot chassis. It reflects the system structural costs such as the minimum execution time, steering wheel adjustment time, and mode stabilization waiting time required to switch from the optimal motion mode of the previous control cycle to the candidate motion mode m of the current sampling trajectory. The cost of velocity discontinuity in the sampled trajectory k is used to penalize the degree of jump in the velocity vector between adjacent time steps in the sampled trajectory, reflecting the risk of system oscillation that may be caused by excessive jerk and acceleration. The cost of the discontinuity in the k-direction angle of the sampled trajectory is used to penalize abrupt changes in the velocity direction angle, emphasizing the smooth transition of the control direction; and Let v represent the linear velocity vectors at step j and step (j+1), respectively; x,j v y,j Let represent the x and y components of the linear velocity vector at step j, respectively; N represents the total number of steps; || represents the magnitude of the vector; w v The speed change penalty weight is used to penalize discontinuous trajectory speeds. and These are the velocity direction angles at step j and step (j+1), respectively; w θ Penalty weights for changes in orientation angle. Cost of orientation angle discontinuity. This is used to ensure that the velocity direction generated by the controller maintains a certain continuity even during motion mode switching, which contributes to the physical executability and stability of the system trajectory. When the candidate motion mode corresponding to the current sampled trajectory is the stationary rotation mode, this item is not calculated because the change in velocity direction angle during switching is meaningless. θ It equals 0.

[0115] By jointly modeling the costs of the above three parts, the cost function not only accurately models the task completion accuracy and control energy consumption, but also fully considers the continuity and physical feasibility of system state evolution in multimodal system switching scenarios, thereby improving the system's control smoothness and mode switching robustness.

[0116] Calculate the average total cost for each candidate motion pattern, including:

[0117] For each candidate motion mode, the costs of all sampled trajectories are sorted in ascending order, and the top η values ​​in the ascending cost queue are retained. retain % of the sampling trajectory, η retain The process involves reserving trajectory retention coefficients and obtaining a set of effective trajectories for the candidate motion mode. This processing strategy enables trajectory evaluation to retain key strategy information while possessing adaptive adjustment capabilities, allowing for flexible setting of the trajectory retention ratio based on the specific task's requirements for trajectory stability and robustness. The average total cost corresponding to the candidate motion mode is calculated based on the set of effective trajectories, used to evaluate the overall performance of the motion mode in the current control cycle, i.e., to measure the average execution quality of the mode. Different candidate motion modes are processed iteratively, and their corresponding average total costs are calculated.

[0118] Step S3: In the optimal mode of the current control cycle, the control input sequence is updated by weighting through path integration and the first control input in the updated control input sequence is sent to the robot chassis actuator, that is, motion commands are sent to the motor driver, steering wheel unit and other related components. These control commands will be directly used to drive the chassis to produce actual movements and execute corresponding motion control behaviors.

[0119] In the optimal mode of the current control cycle, the control input sequence is updated using a weighted approach via path integration, specifically including:

[0120] The sampling weights for each trajectory are obtained by normalizing the average total cost using the soft optimal path integral formula:

[0121]

[0122] in, λ represents the importance weight of the k-th trajectory; λ is a temperature parameter, indicating the sensitivity of trajectory selection. S represents the number of valid trajectories. (k) This represents the cumulative cost of the k-th trajectory.

[0123] Determining the optimal motion mode m * Then, the system uses the complete set of valid trajectories retained in this mode. and their corresponding path integral weights For control input sequence Perform a weighted update. The update method is as follows:

[0124]

[0125] Among them, uj This is the control input for the j-th control step; The control disturbance of the k-th valid trajectory at the j-th step; Δt is the time step. For the optimal motion mode m * The formula calculates the number of valid trajectories. It corrects the original control sequence by weighting the perturbation control input, causing it to converge in the direction of minimum total cost, thus updating the path integral of the control strategy.

[0126] This invention ensures that the controller can make decisions based on the performance of a group of trajectories rather than a single optimal trajectory between different modes, thereby enhancing the stability and practical feasibility of the system in a multimodal control environment. It is particularly suitable for highly dynamic mobile platforms with continuous switching scenarios.

[0127] Step S4: Repeat steps S1-S3, continuously acquiring the system status information of the robot chassis and continuously updating the control input sequence (i.e., the control input sequence is rolled forward one position in chronological order, and a default control value is added at the end to maintain a consistent control sequence length, i.e., u1 is moved to u0, u2 is moved to u1, and so on, until u0 is moved to u0). N-1 Move to u N-2 Finally, to pad the sequence length, add u to the end of the sequence. N-1 A default control input is introduced as a placeholder value to maintain the integrity of the future prediction sequence and sent to the robot chassis actuator, thereby realizing the planning control of the robot chassis path, that is, realizing complete closed-loop control.

[0128] This invention ensures seamless integration of control output from planning to actual action, and maintains the temporal continuity of control input through a rolling sequence mechanism. Simultaneously, real-time state awareness and feedback updates construct a complete perception-decision-control closed-loop framework, providing data support and environmental adaptability for the stable operation and high-performance output of the path integral controller, making it suitable for long-term stable control of multimodal mobile platforms in dynamic environments.

[0129] This invention proposes a dynamic path planning and control system for a robot chassis adaptable to multiple motion modes. The system includes:

[0130] The sensor fusion state estimation module is used to obtain the system state information of the robot chassis;

[0131] The optimal motion mode generation module is used to combine the optimal motion mode of the previous control cycle with the current system state information of the robot chassis to select the optimal motion mode for the current control cycle from all motion modes of the robot chassis.

[0132] The control input sequence update module is used to perform a weighted update of the control input sequence in the optimal mode of the current control cycle by means of path integration, and send the first control input in the updated control input sequence to the robot chassis actuator.

[0133] This invention proposes a computer device, including a memory and a processor. The memory stores a computer program, and the processor executes the computer program to implement the steps of a dynamic path planning and control method for a robot chassis adaptable to multiple motion modes.

[0134] This invention proposes a computer-readable storage medium storing a computer program, which, when executed by a processor, implements the steps of a dynamic path planning and control method for a robot chassis adaptable to multiple motion modes.

[0135] This invention proposes a computer program product, including a computer program / instruction, which, when executed by a processor, implements the steps of a dynamic path planning and control method for a robot chassis adaptable to multiple motion modes.

Claims

1. A dynamic path planning and control method for a robot chassis adaptable to multiple motion modes, characterized in that, Includes the following steps: Step S1: Obtain the current system status information of the robot chassis; Step S2: Combining the optimal motion mode of the previous control cycle with the current system state information of the robot chassis, select the optimal motion mode for the current control cycle from all motion modes of the robot chassis. Step S2 specifically involves: Step S21: Based on the current system state information of the robot chassis, select and construct a set of candidate motion modes from all motion modes of the robot chassis; then, based on the spatial dimension and complexity of the control target, assign the corresponding number of trajectory samples to different candidate motion modes in the set of candidate motion modes. Step S22: Generate a set of control input disturbances; Then, based on the current set of control input disturbances and the number of trajectory samples corresponding to each candidate motion mode, multi-trajectory forward simulation is performed under each candidate motion mode to obtain several sampled trajectories corresponding to each candidate motion mode. Step S23: Combining the optimal motion mode of the previous control cycle and the current system state information of the robot chassis, calculate the cost corresponding to each sampled trajectory in each candidate motion mode, and then calculate the average total cost corresponding to each candidate motion mode. The candidate motion mode with the smallest average total cost is taken as the optimal motion mode for the current control cycle. The process of allocating corresponding trajectory sampling numbers to different candidate motion patterns in the candidate motion pattern set based on the spatial dimension and complexity of the control target includes: Based on the control target space dimension and complexity, corresponding sampling weight coefficients are set for all motion modes of the robot chassis, among which the sampling weight coefficients for the Ackermann-like mode are... Sampling weight coefficients for omnidirectional movement mode Sampling weight coefficients for lateral sliding mode Sampling weighting coefficients for in-place rotation mode = : : : If the candidate motion pattern set does not contain one or more of the omnidirectional movement mode, lateral sliding mode, and stationary rotation mode, its corresponding sampling weight coefficient is 0, and the sum of the sampling weight coefficients of the candidate motion patterns in the candidate motion pattern set is equal to 1; finally, the number of trajectory samples is allocated according to the ratio of the sampling weight coefficients of the candidate motion patterns in the candidate motion pattern set. The process of combining the optimal motion mode from the previous control cycle with the current system state information of the robot chassis to calculate the cost corresponding to each sampled trajectory under each candidate motion mode includes: in, Let k be the composite cost of the sampling trajectory. This represents the cumulative running cost of the sampled trajectory k at each time step during the prediction period; Represents the terminal state cost of the sampling trajectory k; Represents the mode switching cost of sampling trajectory k; The cost of switching the basic mode for the sampling trajectory k; This is the cost of the velocity discontinuity of the sampling trajectory k; The cost is the discontinuity in the direction angle of the sampled trajectory k; and They represent the first Step and the first The linear velocity vector of the step; They represent the first The linear velocity vector of a step in the x and y directions; N represents the total number of steps; Represents the magnitude of a vector; Penalty weights for speed changes; and The first Step and the first The direction angle of the step's velocity; Penalty weight for changes in orientation angle; Step S3: In the optimal mode of the current control cycle, the control input sequence is updated by weighting using path integration, and the first control input in the updated control input sequence is sent to the robot chassis actuator; Step S4: Repeat steps S1-S3 to continuously acquire the system status information of the robot chassis and continuously update the control input sequence and send it to the robot chassis actuator, thereby realizing the planning and control of the robot chassis path.

2. The dynamic path planning and control method for a robot chassis adaptable to multiple motion modes according to claim 1, characterized in that, The step of selecting and constructing a candidate motion mode set from all motion modes of the robot chassis based on the current system state information of the robot chassis includes: First, based on the chassis position coordinates and heading angle in the current system status information of the robot chassis... Calculate the direction angle error based on the target point coordinates. Global straight-line distance and forward displacement ; Based on direction angle error Global straight-line distance and forward displacement Candidate motion modes are selected from all motion modes of the robot chassis according to the following modal discrimination rules, thereby constructing a candidate motion mode set: Ackermann-like patterns are used as candidate motion patterns by default; If the direction angle error The omnidirectional movement mode was selected as the candidate motion mode. If the direction angle error And forward displacement Lateral sliding mode is selected as a candidate motion mode, while omnidirectional movement mode is removed from the candidate motion mode set. If the direction angle error The stationary rotation mode is selected as the candidate motion mode; if the direction angle error And global straight-line distance Proximity judgment distance value The stationary rotation mode was also selected as a candidate motion mode.

3. The dynamic path planning and control method for a robot chassis adaptable to multiple motion modes according to claim 1, characterized in that, The calculation of the average total cost corresponding to each candidate motion pattern includes: For each candidate motion mode, the costs of all sampled trajectories are sorted in ascending order, and the top values ​​in the ascending cost queue are retained. % of the sampled trajectories and obtain the effective trajectory set of the candidate motion mode; calculate the average total cost corresponding to the candidate motion mode based on the effective trajectory set of the candidate motion mode; traverse and process different candidate motion modes and calculate the corresponding average total cost.

4. A dynamic path planning and control system for implementing the method of claim 1, adaptable to a multi-motion-mode robot chassis, characterized in that, include: The sensor fusion state estimation module is used to obtain the system state information of the robot chassis; The optimal motion mode generation module is used to combine the optimal motion mode of the previous control cycle with the current system state information of the robot chassis to select the optimal motion mode for the current control cycle from all motion modes of the robot chassis. The control input sequence update module is used to perform a weighted update of the control input sequence in the optimal mode of the current control cycle by means of path integration, and send the first control input in the updated control input sequence to the robot chassis actuator.

5. A computer device comprising a memory and a processor, wherein the memory stores a computer program, characterized in that, When the processor executes the computer program, it implements the steps of the dynamic path planning and control method for a multi-motion mode robot chassis according to any one of claims 1 to 3.

6. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the steps of the dynamic path planning and control method for a multi-motion mode robot chassis according to any one of claims 1 to 3.

7. A computer program product comprising a computer program / instructions, characterized in that, When the computer program / instructions are executed by the processor, they implement the steps of the dynamic path planning and control method for a multi-motion mode adaptable robot chassis as described in any one of claims 1 to 3.

Images