Robot adaptive motion control method and system
By combining step testing and mapping functions with delay compensation and multi-objective optimization, the problems of model mismatch and delay amplification in robot motion control are solved, achieving high-precision trajectory tracking and adaptive enhancement while reducing computational burden.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- 四川中科友成科技有限公司
- Filing Date
- 2025-11-27
- Publication Date
- 2026-04-28
AI Technical Summary
Existing robot motion control algorithms suffer from model mismatch, parameter drift, and delay amplification issues in complex environments, leading to decreased trajectory tracking accuracy or even failure.
A stepped testing module is used to obtain robot motion parameters. A parameter fitting module is used to establish a mapping function between control quantity and motion speed. Combined with a delay compensation module and a multi-objective optimization module, the control delay can be predicted and optimized. The key parameters are dynamically updated using an online update module.
It improves the trajectory tracking accuracy and adaptability of robots in complex environments, reduces computational burden, reduces motion jitter, and enhances robustness to changes in motor parameters and load.
Abstract
Description
Technical Field
[0001] This invention relates to the field of mobile robot navigation technology, and in particular to a robot adaptive motion control method and system. Background Technology
[0002] Autonomous mobile robots, such as unmanned vehicles, drones, robotic dogs, and humanoid robots, are being rapidly applied in scenarios such as warehousing and logistics, security patrols, and disaster relief. Their core lies in real-time and accurate tracking of planned trajectories. Existing motion control algorithms mainly include PID, Model Predictive Control (MPC), and Pure Pursuit. Among them: PID control: The control quantity is calculated based on the proportional-integral-derivative of the pose error, resulting in a simple structure. However, it has poor adaptability to nonlinear terrain, parameter tuning relies on experience, and it cannot actively compensate for system delays.
[0003] MPC control: Theoretically, it can handle constraints by performing rolling optimization of the predictive model over multiple time steps. However, it requires a large amount of real-time computation and high hardware computing power; when the actual parameters mismatch with the predictive model, the control accuracy drops sharply.
[0004] Pure Pursuit control: It calculates curvature by finding the aiming point based on geometric relationships, which is simple in form. However, it is poorly adaptable to complex nonlinear trajectories, the aiming distance is difficult to adaptively adjust with changes in terrain, the control delay amplifies the steering error, and it is prone to collision risks at sharp bends.
[0005] In summary, existing technologies generally suffer from the coupling problem of "model mismatch - parameter drift - delay amplification" in complex environments, which leads to a decrease in trajectory tracking accuracy or even failure. Summary of the Invention
[0006] To solve the above problems, the technical solution adopted by the present invention is as follows: A robot adaptive motion control system includes a step test module for sending a sequence of control quantities that increase or decrease stepwise to the robot, and recording the robot's odometry data and timestamped control quantity data under each control quantity. The parameter fitting module is used to parse the odometry data and control data, extract key parameters of robot motion, and establish a mapping function between control quantities and motion speed. The delay compensation module is used to maintain a queue of historical control variables and predict the robot's expected pose when the control command issued at time t is executed based on the control delay τ. The multi-objective optimization module is used to solve for the optimal control quantity that minimizes the weighted cost function of position error, attitude error, velocity error, and control quantity change rate, with the desired pose as the initial state and the desired trajectory target point as the terminal constraint. The online update module is used to continuously collect motion data during robot movement and dynamically update the mapping function and key parameters.
[0007] Furthermore, the key parameters include: control frequency, control delay τ, maximum speed of the three axes, maximum acceleration of the three axes, minimum effective control quantity boundary, and maximum effective control quantity boundary.
[0008] Furthermore, the mapping function is one of a quadratic polynomial, an exponential function, or a piecewise linear function, and the coefficients are determined by least squares fitting.
[0009] Furthermore, the delay compensation module predicts the desired pose through the following steps: A: Read the historical control quantity queue within the time period [t-τ, t]; B: Input the historical control values into the mapping function sequentially to obtain the velocity sequence; C: Integrate the velocity sequence to obtain the robot's desired pose at time t+τ.
[0010] Furthermore, the cost function J of the multi-objective optimization module is: J = w1·||p target - p pred ||² + w²·||θ target - θ pred ||² + w3·||v target - v pred ||²+ w4·||u - u pred ||² Where, p target θ target v target p represents the terminal position, terminal attitude, and terminal velocity, respectively. pred θ pred v pred Let U represent the initial position, initial attitude, and initial velocity, respectively, and u be the control variable to be determined. pred w1 to w4 are the initial control variables and weighting coefficients.
[0011] Furthermore, the online update module only triggers the step test module to recalibrate when it detects a sudden change in the robot's motion performance or when the cumulative error exceeds a threshold.
[0012] A robot adaptive motion control method includes the following steps: Step S1: Perform a dynamic step test on the robot and collect odometry data and timestamped control data under different step control quantities; Step S2: Analyze the data, extract key parameters of robot motion, and fit a mapping function between control quantity and motion speed; Step S3: In the motion control phase, maintain the historical control quantity queue and predict the robot's expected pose when the current control command is executed based on the control delay τ. Step S4: Using the desired pose as the initial state and the desired trajectory target point as the terminal constraint, establish a multi-objective optimization problem and solve for the optimal control quantity. Step S5: Send the optimal control quantity to the robot and continuously collect data during the movement to dynamically update the mapping function and key parameters.
[0013] Furthermore, the extraction of key parameters in step S2 includes: performing piecewise linear fitting on the control quantity and motion velocity curves to identify the boundaries of the minimum and maximum effective control quantities; taking the derivative of the velocity response curve to obtain the maximum acceleration of the three axes; and taking the average value of the steady-state velocity range to obtain the maximum velocity of the three axes.
[0014] Furthermore, in step S3, the control delay τ is determined by the cross-correlation function method. The cross-correlation function between the control quantity sequence and the velocity sequence is calculated, and the time difference corresponding to the peak value of the cross-correlation is taken as τ.
[0015] Furthermore, the multi-objective optimization problem in step S4 is solved using quadratic programming, and the constraints include: u min ≤ u ≤ u max ;|u - u pred | ≤ Δu max Among them, u min For the minimum effective control quantity boundary, u max Δu is the boundary of the maximum effective control quantity. max This is a single-time control increment limit.
[0016] The beneficial effects of this invention are: 1. Strong adaptability: The step test automatically acquires actual motion parameters, adapts to different individual robots and performance degradation scenarios, eliminates dependence on preset models, and solves the model mismatch problem.
[0017] 2. High-precision trajectory tracking: Control delay compensation mechanism reduces positioning deviation; multi-target optimization balances trajectory accuracy and control smoothness, reducing motion jitter.
[0018] 3. High computational efficiency: The parameter fitting function simplifies the solution process for control quantities, greatly improving the solution efficiency; the step test is only triggered when a new device is initialized or when there is a sudden change in performance, without increasing the real-time burden.
[0019] 4. Weak parameter dependence: No precise dynamic model is required, and it is highly robust to changes in motor parameters and load. Detailed Implementation
[0020] To make the objectives, technical solutions, and advantages of the embodiments of this disclosure clearer, the technical solutions of the embodiments of this disclosure are described clearly and completely. Obviously, the described embodiments are only some, not all, of the embodiments of this disclosure. All other embodiments obtained by those skilled in the art based on the described embodiments of this disclosure without creative effort are within the scope of protection of this disclosure.
[0021] Unless otherwise defined, the technical or scientific terms used in this disclosure shall have the ordinary meaning understood by one of ordinary skill in the art to which this disclosure pertains. The terms "first," "second," and similar terms used in this disclosure do not indicate any order, quantity, or importance, but are merely used to distinguish different components. Terms such as "comprising" or "including" mean that the element or object preceding the word encompasses the elements or objects listed following the word and their equivalents, without excluding other elements or objects. Terms such as "upper," "lower," "left," and "right" are used only to indicate relative positional relationships, and these relative positional relationships may change accordingly when the absolute position of the described object changes.
[0022] An adaptive motion control system for robots, It includes a step test module, which sends a sequence of control quantities that increase or decrease stepwise to the robot and records the robot's odometry data and timestamped control quantity data under each control quantity.
[0023] The parameter fitting module is used to analyze the odometry data and control data, extract key parameters of robot motion, and establish a mapping function between control quantities and motion speed. The key parameters include: control frequency, control delay τ, maximum speed of the three axes, maximum acceleration of the three axes, minimum effective control quantity boundary, and maximum effective control quantity boundary. The mapping function is one of a quadratic polynomial, an exponential function, or a piecewise linear function, and its coefficients are determined using a least-squares fitting method.
[0024] The delay compensation module maintains a historical control quantity queue and predicts the robot's desired pose when the control command issued at time t is executed, based on the control delay τ. The delay compensation module predicts the desired pose through the following steps: A: Read the historical control quantity queue within the time period [t-τ, t]; B: Input the historical control values into the mapping function sequentially to obtain the velocity sequence; C: Integrate the velocity sequence to obtain the robot's desired pose at time t+τ.
[0025] A multi-objective optimization module is used to find the optimal control quantity that minimizes the weighted cost function of position error, attitude error, velocity error, and control quantity change rate, taking the desired pose as the initial state and the desired trajectory target point as the terminal constraint. The cost function J of the multi-objective optimization module is: J = w1·||p target - p pred ||² + w²·||θ target - θ pred ||² + w3·||v target - v pred ||²+ w4·||u - u pred ||² Where, p target θ target v target p represents the terminal position, terminal attitude, and terminal velocity, respectively. pred θ pred v pred Let U represent the initial position, initial attitude, and initial velocity, respectively, and u be the control variable to be determined. pred w1 to w4 are the initial control variables and weighting coefficients.
[0026] The online update module continuously collects motion data during robot movement and dynamically updates the mapping function and key parameters. The online update module only triggers recalibration of the stepped test module when a sudden change in robot motion performance or a cumulative error exceeding a threshold is detected, ensuring data accuracy.
[0027] A robot adaptive motion control method includes the following steps: Step S1: Perform a dynamic step test on the robot and collect odometer data and timestamped control data under different step control quantities.
[0028] Step S2: Analyze the data, extract key parameters of robot motion, and fit the mapping function between control quantity and motion speed; the extraction of key parameters includes: performing piecewise linear fitting on the curves of control quantity and motion speed to identify the boundaries of minimum and maximum effective control quantity; differentiating the velocity response curve to obtain the maximum acceleration of the three axes; and taking the average value of the steady-state velocity range to obtain the maximum velocity of the three axes.
[0029] Step S3: In the motion control phase, maintain the historical control quantity queue and predict the robot's expected pose when the current control command is executed based on the control delay τ. The control delay τ is determined by the cross-correlation function method. Calculate the cross-correlation function between the control quantity sequence and the velocity sequence, and take the time difference corresponding to the peak value of the cross-correlation as τ.
[0030] Step S4: Using the desired pose as the initial state and the desired trajectory target point as the terminal constraint, establish a multi-objective optimization problem and solve for the optimal control quantity; the multi-objective optimization problem is solved using quadratic programming, and the constraints include: u min ≤ u≤ u max ;|u - u pred | ≤ Δu max Among them, u min For the minimum effective control quantity boundary, u max Δu is the boundary of the maximum effective control quantity. max This is a single-time control increment limit.
[0031] Step S5: Send the optimal control quantity to the robot and continuously collect data during the movement to dynamically update the mapping function and key parameters; the dynamic update adopts a sliding window mechanism, retaining only the most recent N sets of control quantities and movement speed samples, and re-performing the fitting to adapt to the slow changes in robot performance.
[0032] In one embodiment of the invention, an outdoor asphalt road test was conducted on the unmanned vehicle. The unmanned vehicle adopted a four-wheel independent drive skateboard chassis, and the onboard industrial control computer (Intel i7-1260U) was connected to four motor drivers via a CAN bus. The IMU and GNSS integrated navigation module broadcast odometer information at a frequency of 100 Hz. The industrial control computer ran ROS2, and the step test module, parameter fitting module, delay compensation module, multi-objective optimization module, and online update module all ran in the same process as C++ nodes.
[0033] On the smooth road section, the longitudinal speed control quantity is first sent in a stepped increment, with a step amplitude of 0.1 m / s and each step lasting 2s, up to a maximum of 2.5 m / s. At the same time, the left and right wheel speeds, IMU speed and timestamps are recorded. After the test is completed, the parameter fitting module uses the least squares quadratic polynomial to fit the result: v = 0.92u² + 0.11u + 0.02, where u∈[0,1] is the normalized control quantity and v is the longitudinal speed (m / s). The maximum acceleration is obtained by differentiation as 2.3 m / s², and the control delay τ is measured as 120 ms by the cross-correlation method.
[0034] During the motion phase, the delay compensation module maintains a historical control quantity queue of length L = ⌈τ / Ts⌉ = 12 (Ts = 10 ms). When the control quantity needs to be calculated at time t, the 12 historical control quantities in the interval [t−τ,t] are retrieved, substituted sequentially into the mapping function to obtain the velocity sequence, and then integrated forward to obtain the desired pose P at time t+τ. pred , with P pred In the initial state, the trajectory tracker samples the desired target point G at the preview time Tp = 0.5 s. target =(p target θtarget v target ).
[0035] Construct the cost function: J = 5||p−p target ||² + 3||θ− θtarget ||² + 2||v−v target ||² + 1||u−u pred ||² Constraints: 0≤u≤1, |u−u pred |≤0.15. The optimal control quantity u is obtained within 2 ms using the OSQP solver. opt The system then issues a warning and monitors the mean square tracking error in real time. When the error exceeds 0.3 m for 5 consecutive seconds, a "performance mutation" flag is triggered, the step test is re-executed, and the mapping function is updated. In the scenario of a 20% drop in tire pressure, the system automatically triggers an update at the 8th second, and the tracking error decreases from 0.32 m to 0.09 m.
[0036] The above description is merely a specific embodiment of this disclosure, but the scope of protection of this disclosure is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in this disclosure should be included within the scope of protection of this disclosure. Therefore, the scope of protection of this disclosure should be determined by the scope of the claims.
Claims
1. A robot adaptive motion control system, characterized in that: This includes a step test module, which sends a sequence of control quantities that increase or decrease stepwise to the robot and records the robot's odometry data and timestamped control quantity data under each control quantity. The parameter fitting module is used to parse the odometry data and control data, extract key parameters of robot motion, and establish a mapping function between control quantities and motion speed. The delay compensation module is used to maintain a queue of historical control variables and predict the robot's expected pose when the control command issued at time t is executed based on the control delay τ. The multi-objective optimization module is used to solve for the optimal control quantity that minimizes the weighted cost function of position error, attitude error, velocity error, and control quantity change rate, with the desired pose as the initial state and the desired trajectory target point as the terminal constraint. The online update module is used to continuously collect motion data during robot movement and dynamically update the mapping function and key parameters.
2. The robot adaptive motion control system according to claim 1, characterized in that: The key parameters include: control frequency, control delay τ, maximum speed of the three axes, maximum acceleration of the three axes, minimum effective control quantity boundary, and maximum effective control quantity boundary.
3. The robot adaptive motion control system according to claim 2, characterized in that: The mapping function is one of a quadratic polynomial, an exponential function, or a piecewise linear function, and its coefficients are determined by least squares fitting.
4. The robot adaptive motion control system according to claim 3, characterized in that: The delay compensation module predicts the desired pose through the following steps: A: Read the historical control quantity queue within the time period [t-τ, t]; B: Input the historical control values into the mapping function sequentially to obtain the velocity sequence; C: Integrate the velocity sequence to obtain the robot's desired pose at time t+τ.
5. A robot adaptive motion control system according to claim 4, characterized in that: The cost function J of the multi-objective optimization module is: J = w1·||p target -p pred ||² + w2·||θ target - θ pred ||² + w3·||v target - v pred ||²+ w4·||u - u pred ||² Where, p target θ target v target p represents the terminal position, terminal attitude, and terminal velocity, respectively. pred θ pred v pred Let U represent the initial position, initial attitude, and initial velocity, respectively, and u be the control variable to be determined. pred w1 to w4 are the initial control variables and weighting coefficients.
6. The robot adaptive motion control system according to claim 5, characterized in that: The online update module only triggers the step test module to recalibrate when it detects a sudden change in robot motion performance or when the cumulative error exceeds the threshold.
7. A robot adaptive motion control method, used in the robot adaptive motion control system according to any one of claims 1-6, characterized in that: Includes the following steps: Step S1: Perform a dynamic step test on the robot and collect odometry data and timestamped control data under different step control quantities; Step S2: Analyze the data, extract key parameters of robot motion, and fit a mapping function between control quantity and motion speed; Step S3: In the motion control phase, maintain the historical control quantity queue and predict the robot's expected pose when the current control command is executed based on the control delay τ. Step S4: Using the desired pose as the initial state and the desired trajectory target point as the terminal constraint, establish a multi-objective optimization problem and solve for the optimal control quantity. Step S5: Send the optimal control quantity to the robot and continuously collect data during the movement to dynamically update the mapping function and key parameters.
8. The robot adaptive motion control method according to claim 7, characterized in that: The extraction of key parameters in step S2 includes: performing piecewise linear fitting on the control quantity and motion velocity curves to identify the boundaries of the minimum and maximum effective control quantities; taking the derivative of the velocity response curve to obtain the maximum acceleration of the three axes; and taking the average value of the steady-state velocity range to obtain the maximum velocity of the three axes.
9. The robot adaptive motion control method according to claim 8, characterized in that: In step S3, the control delay τ is determined by the cross-correlation function method. The cross-correlation function between the control quantity sequence and the velocity sequence is calculated, and the time difference corresponding to the peak value of the cross-correlation is taken as τ.
10. The robot adaptive motion control method according to claim 10, characterized in that: The multi-objective optimization problem in step S4 is solved using quadratic programming, and the constraints include: u min ≤ u ≤ u max ;|u - u pred | ≤Δu max Among them, u min For the minimum effective control quantity boundary, u max Δu is the boundary of the maximum effective control quantity. max This is a single-time control increment limit.