Dynamic obstacle avoidance trajectory tracking control method for Mecanum wheel mobile robot

Abstract

The invention relates to the technical field of mobile robot driving control, in particular to a Mecanum wheel mobile robot dynamic obstacle avoidance trajectory tracking control method, which comprises the following specific steps of: establishing an inverse / forward kinematics model of an omnidirectional mobile platform, and constructing a discretization state-space equation by taking a linear speed and an angular speed as control input; deducing a conversion relation between an airframe coordinate system and a world coordinate system; the state of an obstacle is estimated through extended Kalman filtering, data are fused through discrete motion and an observation model, and an obstacle avoidance safety area is constructed through a confidence ellipse with a certain confidence level. The method has the advantages that high-precision trajectory tracking and dynamic obstacle avoidance are realized through nonlinear model prediction, Kalman filtering fusion control and Mecanum wheel kinematics model adaptation, an error accumulation expansion confidence ellipse is combined when data packet loss is observed, so that the ellipse can still contain a real obstacle trajectory, and the adaptability and practicability are higher.

Description

Technical Field

[0001] This invention relates to the field of mobile robot drive control technology, specifically a dynamic obstacle avoidance trajectory tracking control method for a Mecanum wheel mobile robot. Background Technology

[0002] In the rapid development of industrial automation, intelligent warehousing, and flexible manufacturing, mobile robots, as core equipment for material transportation, environmental monitoring, and auxiliary operations, have made their motion flexibility and environmental adaptability key performance indicators. Mecanum wheel mobile robots, thanks to their unique wheel system structure, can achieve omnidirectional movement in a plane—lateral, longitudinal, and rotational—and can adjust their direction of movement without steering, significantly improving their efficiency in narrow spaces or complex paths. Therefore, they have broad application prospects in scenarios requiring high-precision motion control. However, the kinematic model of this type of robot exhibits strong nonlinear coupling characteristics, and real-world operating environments often contain various types of dynamic obstacles (such as other mobile devices, temporary stacks of objects, etc.). Furthermore, sensor data may be lost due to interference. These factors collectively pose serious challenges to their trajectory tracking accuracy and obstacle avoidance capabilities, becoming the core issues restricting their further widespread application.

[0003] Existing trajectory tracking and obstacle avoidance control for Mecanum wheel mobile robots still have significant limitations: traditional control methods (such as PID control) struggle to balance trajectory tracking accuracy with real-time dynamic obstacle avoidance, easily leading to tracking deviations or collisions due to untimely obstacle avoidance when facing dynamic obstacles; obstacle state estimation relies on continuous sensor data input, and when sensors lose data or experience data interruptions, errors accumulate rapidly, resulting in inaccurate obstacle avoidance safety zone delineation and increased collision risk; simultaneously, some control schemes do not fully adapt to the kinematic characteristics of Mecanum wheels, or their control constraint designs are too simplistic, making it difficult to balance motion flexibility and operational safety, thus limiting the practical value of mobile robots in complex dynamic environments. This invention constructs a fusion control framework combining nonlinear model predictive control and extended Kalman filtering to address the control challenges posed by nonlinear kinematic coupling and dynamic obstacle uncertainty in Mecanum wheel mobile robots, improving their trajectory tracking accuracy and obstacle avoidance robustness in complex dynamic environments, especially providing a reliable control scheme for scenarios with sensor data loss. Summary of the Invention

[0004] (a) Technical problems to be solved

[0005] To address the shortcomings of existing technologies, this invention provides a dynamic obstacle avoidance trajectory tracking control method for a Mecanum wheel mobile robot, which solves the problems mentioned in the background section.

[0006] (II) Technical Solution

[0007] To achieve the above objectives, the present invention specifically adopts the following technical solution:

[0008] A dynamic obstacle avoidance trajectory tracking control method for a Mecanum wheeled mobile robot, the specific steps of which include:

[0009] Step 1: Use the Mecanum wheel mobile robot model to obtain the transformation relationship between the body coordinate system and the world coordinate system, that is, the relationship between the state variables in the world coordinate system and the control input in the body coordinate system, so as to provide a motion model for the nonlinear model prediction trajectory tracking controller;

[0010] Step 2: Acquire observation data of dynamic obstacles using sensors mounted on the Mecanum wheeled mobile robot;

[0011] Step 3: Use the Extended Kalman Filter (EKF) to obtain the estimated state of the dynamic obstacle and its corresponding error covariance matrix;

[0012] Step 4: Based on the current obstacle position and error, use the unobserved prediction of the Extended Kalman Filter (EKF) to predict the future obstacle state, and with the error propagation of the unobserved prediction, finally obtain the future obstacle position estimate and the corresponding error covariance matrix.

[0013] Step 5: Construct a confidence ellipse region with the estimated position of the dynamic obstacle as the center, the corresponding error covariance matrix and a certain confidence level, so that it covers the real trajectory;

[0014] Step 6: Add the estimated position of the dynamic obstacle and the range of the confidence ellipse to the constraints of the nonlinear model predictive controller of the Mecanum wheel mobile robot, and dynamically adjust the size of the confidence ellipse range according to different error covariance matrices;

[0015] Step 7: Using the tracked trajectory as the target and dynamic obstacles as constraints, obtain the optimal control sequence for the future control range of the Mecanum wheel mobile robot. Apply the first term to the Mecanum wheel mobile robot, and then use the inverse kinematics equations of the Mecanum wheel mobile robot to obtain the angular velocities of the four Mecanum wheels as the final input for control, so as to achieve the purpose of dynamically adjusting the obstacle avoidance area while tracking the trajectory.

[0016] Repeating steps one through seven will enable the Mecanum wheel mobile robot to track and control its trajectory under any dynamic obstacle.

[0017] Furthermore, the relationship between the world coordinate system and the body coordinate system in step one is as follows: In the formula, X = [x w y w θ w ] T X is the state variable vector of the controlled robot in the world coordinate system. It is the derivative of the state variable vector of the controlled robot in the world coordinate system, x w It is the x-coordinate of the controlled robot in the world coordinate system, y w θ is the vertical coordinate of the controlled robot in the world coordinate system. w It is the heading angle of the controlled robot in the world coordinate system, and the symbol T is the transpose of the vector; in the formula It is the state transition matrix; where u b =[v bx v by ω b ] T u b It is the control input vector in the body coordinate system, v bx Let v be the linear velocity in the x-direction of the body coordinate system. by Let w be the linear velocity in the y-direction of the body coordinate system. by ω is the angular velocity in the body coordinate system.

[0018] Furthermore, in step two, the state variable X in the obstacle world coordinate system is selected. o =[x o y o v o θ o ] T In the formula X O It is the state variable vector of the dynamic obstacle in the world coordinate system, x o Let x be the x-axis coordinate of the obstacle in the world coordinate system, and y be the y-axis coordinate. o Let v be the y-coordinate of the obstacle in the world coordinate system. o Let θ be the velocity of the obstacle in the world coordinate system. o Let X be the heading angle of the obstacle in the world coordinate system; after discretizing the continuous system using the forward Euler method, a dynamic obstacle motion model is constructed as X. o,k+1 =f o (X o,k ), where X o,k+1 Let f be the state variable vector of the obstacle at time k+1. o (·) is a motion model function describing the relationship between the state of the obstacle and time, X o,k Let Z be the state variable vector of the obstacle at time k; construct the sensor observation model as Z o,k =h o (X o,k ), where Z o,k Let h be the sensor's observation of the obstacle's state information at time k. o (·) is a sensor observation model function that describes the mapping relationship between obstacle state and observation value.

[0019] Furthermore, step three considers process noise during state prediction. and observation noise Among them, w k The time k follows a normal distribution The zero-mean Gaussian process noise has a covariance of Q, and v k The distribution at time k follows a normal distribution. The zero-mean Gaussian observation noise has a covariance of R, and Given the state at time k-1, the prediction steps of EKF are used. Array, in the formula It is the prior state estimate at time k, obtained based on the posterior estimate at time k-1. Let be the posterior state estimate at time k-1. P is the prior error covariance matrix describing the uncertainty of the prior estimate at time k. o,k-1|k-1 Let F be the posterior error covariance matrix at time k-1. o,k The motion model f at time k o (·) The state of the obstacle at time k-1 The Jacobian matrix; then updated using the current sensor observations through the EKF update steps. To obtain a relatively accurate state estimate and the corresponding error covariance matrix at the current moment, where Y... o,k Let K be the observation residual at time k. o,k H is the Kalman gain at time k. o,k For the observation model h at time k o (·) The state of the obstacle at time k-1 Jacobian matrix, Let P be the posterior state estimate at time k. o,k|k Let I be the posterior error covariance matrix at time k. 4×4 It is a 4th order identity matrix.

[0020] Furthermore, step four involves obtaining the EKF-filtered estimate at the current time. P o,k|k Predict the trajectory for the next N steps, where N is the time domain for predicting the future trajectory. For the next N steps i = 0, 1, ..., N-1, we have: In the formula, i is the index of the prediction step. Let k be the time to predict the obstacle state estimate for the i-th step in the future. Let k be the obstacle error covariance matrix for predicting the i-th step in the future. Let the Jacobian matrix at time k be the one used to predict the i-th future step; initialized to... Finally, obtain the predicted states of all obstacles for the next N steps from the current moment. and the corresponding obstacle error covariance matrix Location prediction and confidence region are about [x o y o ] T Since the distribution is such that only the elements of the state prediction and error covariance with respect to the horizontal and vertical axes are taken as the inputs to the controller, then we have: and In the formula To obtain the predicted state The first two elements yield the horizontal and vertical coordinate vectors of the obstacle in the i-th step. To obtain The error covariance matrix of the obstacle position at the i-th step is obtained from the first 2×2 submatrix.

[0021] Furthermore, the ellipse in step five is the probability contour line of a bivariate Gaussian distribution; therefore, the confidence ellipse is defined by the Mahalanobis distance. In the formula It is the critical value of the chi-square distribution with 2 degrees of freedom and a confidence level of 1-α; the confidence ellipse with a fixed confidence level is mapped to an ellipse in Euclidean space, and the prediction error and confidence level are combined to form a dynamically adjustable obstacle uncertainty boundary, thereby improving the robustness and safety of obstacle avoidance.

[0022] Furthermore, the state variables of the nonlinear model predictive controller in step six are: X = [x w y w θ w ] T The control variable is: u b =[v bx v by ω b ] T The prediction time domain is N, and the sampling period is T. s The reference trajectory is set as a function of time, and its discretization is X. ref,k =[x ref,k y ref,k θ ref,k ] T In the formula X ref,k Let x be the reference trajectory vector that the robot expects to track at time k after discretization. ref,k Let x be the x-axis coordinate of the reference trajectory at time k, and y be the y-axis coordinate. ref,k Let θ be the y-axis coordinate of the reference trajectory at time k. ref,kLet be the heading angle of the reference trajectory at time k; the control objective is to minimize the error between the mobile robot platform state and the reference state while satisfying physical constraints; in particular, it is able to avoid obstacles and perform local path replanning. In addition to considering the boundary radius of the obstacle, the boundary radius of the Mecanum wheel mobile robot platform and the safety margin, a confidence ellipse with a certain confidence level is used as the obstacle avoidance area to eliminate the uncertainty of the extended Kalman filter estimation of the obstacle position caused by sensor error and process error.

[0023] Furthermore, in step seven, when the observation correction data of the dynamic obstacle is lost, the system performs observation-free prediction. The accumulation of errors causes the confidence ellipse to expand accordingly, and the obstacle avoidance area increases to cover the true position of the obstacle, enabling the controlled object to effectively avoid obstacles.

[0024] (III) Beneficial Effects

[0025] Compared with the prior art, the present invention provides a dynamic obstacle avoidance trajectory tracking control method for a Mecanum wheel mobile robot, which has the following beneficial effects:

[0026] (1) The accuracy of trajectory tracking and the reliability of dynamic obstacle avoidance are both improved.

[0027] This invention precisely adapts the omnidirectional motion characteristics of the Mecanum wheel through a kinematic modeling module, and combines it with the extended Kalman filter (EKF) for real-time and accurate estimation of obstacle states, providing reliable input for the nonlinear model predictive controller. The multi-objective optimization objective function and multi-constraint design of the nonlinear model predictive controller minimizes trajectory tracking errors while strictly adhering to safe distances and motion limits, enabling the robot to accurately follow the preset trajectory and quickly respond to obstacle avoidance requirements in dynamic obstacle environments. This effectively solves the problem that existing technologies struggle to balance tracking accuracy and real-time obstacle avoidance.

[0028] (2) It is more robust in the scenario of sensor packet loss.

[0029] For practical engineering scenarios involving sensor data interruption or packet loss, this invention uses an extended Kalman filter prediction mechanism combined with an error-accumulated dynamic expansion confidence ellipse to ensure that the safe zone still covers the true trajectory of obstacles even without any observation data input. This completely solves the problem of error accumulation and obstacle avoidance safety zone failure caused by data packet loss in existing technologies, significantly reduces the collision risk in complex environments, and improves the stability of system operation. Attached Figure Description

[0030] Figure 1 This is a schematic diagram of the Mecanum wheel mobile robot structure involved in the method proposed in this invention;

[0031] Figure 2 This is a system block diagram of the method proposed in this invention;

[0032] Figure 3 This is a flowchart illustrating the control process of the method proposed in this invention.

[0033] Figure 4 A schematic diagram illustrating trajectory tracking and dynamic obstacle avoidance of a Mecanum wheeled mobile robot;

[0034] Figure 5 Map for tracking the trajectory of the Mecanum wheel mobile robot;

[0035] Figure 6 This is a schematic diagram illustrating the trajectory tracking error of a Mecanum wheel mobile robot.

[0036] Figure 7 The diagram shows the obstacle avoidance experiment results of the method proposed in this invention under the condition of data loss from dynamic obstacle sensors in condition one.

[0037] Figure 8 This is an experimental diagram showing the obstacle avoidance effect of the method proposed in this invention under condition two, in the case of data loss from dynamic obstacle sensors. Detailed Implementation

[0038] 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 embodiments of the present invention, and not all embodiments. 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.

[0039] Example

[0040] like Figure 1-2 As shown, an embodiment of the present invention proposes a dynamic obstacle avoidance trajectory tracking control method for a Mecanum wheel mobile robot, including a dynamic obstacle avoidance trajectory tracking control method for a Mecanum wheel mobile robot based on nonlinear model prediction and extended Kalman filter NMPC-EKF fusion. The method includes four Mecanum wheels 1, 2, 3, and 4, four DC motors 5, 6, 7, and 8, an external sensor 9, and a controller 10. The DC motors 5, 6, 7, and 8 are respectively responsible for driving the Mecanum wheels 1, 2, 3, and 4. The external sensor 9 is connected to the controller 10, and the DC motors 5, 6, 7, and 8 are connected to the drive circuit 11. The drive circuit 11 is connected to the controller 10.

[0041] This invention discloses a dynamic obstacle avoidance trajectory tracking control method for a Mecanum wheel mobile robot, referring to... Figure 3 The specific steps of the method include:

[0042] Step 1: Utilize the Mecanum wheel mobile robot model to obtain the transformation relationship between the body coordinate system and the world coordinate system, i.e., the relationship between state variables in the world coordinate system and control inputs in the body coordinate system. This provides a motion model for the nonlinear model prediction trajectory tracking controller. The relationship between the world coordinate system and the body coordinate system is as follows: In the formula, X = [x w y w θ w ] T X is the state variable vector of the controlled robot in the world coordinate system. It is the derivative of the state variable vector of the controlled robot in the world coordinate system, x w It is the x-coordinate of the controlled robot in the world coordinate system, y w θ is the vertical coordinate of the controlled robot in the world coordinate system. w It is the heading angle of the controlled robot in the world coordinate system, and the symbol T is the transpose of the vector. In the formula... It is the state transition matrix. In the formula, u b =[v bx v by ω b ] T u b It is the control input vector in the body coordinate system, v bx Let v be the linear velocity in the x-direction of the body coordinate system. by Let w be the linear velocity in the y-direction of the body coordinate system. by ω is the angular velocity in the body coordinate system.

[0043] Step 2: Acquire observation data of dynamic obstacles using sensors mounted on the Mecanum wheel mobile robot. The state variable in the obstacle's world coordinate system is selected as X. o =[x o y o v o θ o ] T In the formula X O It is the state variable vector of the dynamic obstacle in the world coordinate system, x o Let x be the x-axis coordinate of the obstacle in the world coordinate system, and y be the y-axis coordinate. o Let v be the y-coordinate of the obstacle in the world coordinate system. o Let θ be the velocity of the obstacle in the world coordinate system. o Let X be the heading angle of the obstacle in the world coordinate system. After discretizing the continuous system using the forward Euler method, a dynamic obstacle motion model is constructed as X. o,k+1 =f o (X o,k ), where X o,k+1 Let f be the state variable vector of the obstacle at time k+1. o(·) is a motion model function describing the relationship between the state of the obstacle and time, X o,k Let Z be the state variable vector of the obstacle at time k. Construct the sensor observation model as Z. o,k =h o (X o,k ), where Z o,k Let h be the sensor's observation of the obstacle's state information at time k. o (·) is a sensor observation model function that describes the mapping relationship between obstacle state and observation value.

[0044] Step 3: Use the Extended Kalman Filter (EKF) to obtain the estimated state of the dynamic obstacle and its corresponding error covariance matrix. Consider process noise during state prediction. and observation noise Among them, w k The distribution at time k follows a normal distribution. The zero-mean Gaussian process noise has a covariance of Q, and v k The distribution at time k follows a normal distribution. The zero-mean Gaussian observation noise has a covariance of R, and Given the state at time k-1, the prediction steps of EKF are used. Obtain the prior estimate and prior error covariance matrix of the current time k, where It is the prior state estimate at time k, obtained based on the posterior estimate at time k-1. Let be the posterior state estimate at time k-1. P is the prior error covariance matrix describing the uncertainty of the prior estimate at time k. o,k-1|k-1 Let F be the posterior error covariance matrix at time k-1. o,k The motion model f at time k o (·) The state of the obstacle at time k-1 The Jacobian matrix. Then, the sensor observations at the current time are updated via the EKF update steps. To obtain a relatively accurate state estimate and the corresponding error covariance matrix at the current moment, where Y... o,k Let K be the observation residual at time k. o,k H is the Kalman gain at time k. o,k For the observation model h at time k o (·) The state of the obstacle at time k-1 Jacobian matrix, Let P be the posterior state estimate at time k. o,k|k Let I be the posterior error covariance matrix at time k. 4×4 It is a 4th order identity matrix.

[0045] Step 4: Based on the current obstacle position and error, use the unobserved prediction of the Extended Kalman Filter (EKF) to predict the future obstacle state, along with error propagation during the unobserved prediction, to finally obtain the estimated future obstacle position and the corresponding error covariance matrix. The obtained EKF-filtered estimate at the current time... P o,k|k Predict the trajectory for the next N steps, where N is the time domain for predicting the future trajectory. For the next N steps i = 0, 1, ..., N-1, we have: In the formula, i is the index of the prediction step. Let k be the time to predict the obstacle state estimate for the i-th step in the future. Let k be the obstacle error covariance matrix for predicting the i-th step in the future. Let the Jacobian matrix at time k be the matrix for predicting the i-th future step. Initialize to... Finally, obtain the predicted states of all obstacles for the next N steps from the current moment. and the corresponding obstacle error covariance matrix Location prediction and confidence region are about [x o y o ] T Since the distribution is such that only the elements of the state prediction and error covariance with respect to the horizontal and vertical axes are taken as the inputs to the controller, then we have: In the formula To obtain the predicted state The first two elements yield the horizontal and vertical coordinate vectors of the obstacle in the i-th step. To obtain The error covariance matrix of the obstacle position at the i-th step is obtained from the first 2×2 submatrix.

[0046] Step 5: Construct a confidence ellipse region centered on the estimated position of the dynamic obstacle, using the corresponding error covariance matrix and a certain confidence level, to cover the true trajectory. This ellipse represents the probability contour lines of a bivariate Gaussian distribution. The confidence ellipse is then defined by the Mahalanobis distance. In the formula It is the critical value of a chi-square distribution with 2 degrees of freedom and a confidence level of 1-α. By mapping the confidence ellipse with a fixed confidence level to an ellipse in Euclidean space, and combining the prediction error with the confidence level, a dynamically adjustable obstacle uncertainty boundary is formed, thereby improving the robustness and safety of obstacle avoidance.

[0047] Step Six: Add the estimated positions of the dynamic obstacles and the range of the confidence ellipse to the constraints of the nonlinear model predictive controller of the Mecanum wheel robot, and dynamically adjust the size of the confidence ellipse range according to different error covariance matrices. The state variables of the nonlinear model predictive controller are: X = [x w y w θw ] T The control variable is: u b =[v bx v by ω b ] T The prediction time domain is N, and the sampling period is T. s The reference trajectory is set as a function of time, and its discretization is X. ref,k =[x ref,k y ref,k θ ref,k ] T In the formula X ref,k Let x be the reference trajectory vector that the robot expects to track at time k after discretization. ref,k Let x be the x-axis coordinate of the reference trajectory at time k, and y be the y-axis coordinate. ref,k Let θ be the y-axis coordinate of the reference trajectory at time k. ref,k Let be the heading angle of the reference trajectory at time k. The control objective is to minimize the error between the mobile robot platform state and the reference state while satisfying physical constraints. In particular, it is able to avoid obstacles and perform local path replanning. In addition to considering the boundary radius of the obstacle, the boundary radius of the Mecanum wheel mobile robot platform, and the safety margin, a confidence ellipse with a certain confidence level is used as the obstacle avoidance area to eliminate the uncertainty in the extended Kalman filter estimation of the obstacle position caused by sensor errors and process errors.

[0048] Step 7: Using the tracked trajectory as the target and dynamic obstacles as constraints, obtain the optimal control sequence for the future control range of the Mecanum wheel robot. Apply the first term to the Mecanum wheel robot, and then use the inverse kinematics equations of the Mecanum wheel robot to obtain the angular velocities of the four Mecanum wheels, which serve as the final control input. This achieves the goal of dynamically adjusting the obstacle avoidance area while tracking the trajectory. When the observation correction data of the dynamic obstacle is lost, the system performs unobserved prediction. The accumulated error causes the confidence ellipse to expand, and the obstacle avoidance area increases to cover the true position of the obstacle, enabling the controlled object to effectively avoid obstacles.

[0049] Repeating steps one through seven will enable the Mecanum wheel mobile robot to track and control its trajectory under any dynamic obstacle.

[0050] The effects of the present invention will be further illustrated by the following experiments.

[0051] Experiment 1: The dynamic obstacle avoidance trajectory tracking control method for a Mecanum wheeled mobile robot based on nonlinear model prediction and extended Kalman filtering fusion, as proposed in this invention, was introduced into a terrain containing 5 dynamic obstacles for experimentation. The desired trajectory and the actual driving trajectory were set as follows: Figure 5 The trajectory tracking error of the polyline path shown is as follows: Figure 6As shown. During the time the mobile robot encounters the dynamic obstacle 2, it shuts down its external sensors to simulate a loss of sensor observation data. From... Figure 5 As can be seen from this, the method of the present invention can effectively avoid various dynamic obstacles and achieve trajectory tracking control throughout the entire path. From Figure 6 As can be seen from the present invention, the method maintains a small trajectory tracking error when avoiding various dynamic obstacles.

[0052] Experiment 2: Building upon Experiment 1, this experiment compares the method of this invention with two other methods: the fixed threshold method and the instantaneous state constraint method. The advantage of this method is illustrated using the obstacle avoidance success rate as an indicator. To verify the robustness of the control method under different obstacle state estimation error scenarios, two sets of comparative experiments are designed: The first set of experiments targets scenarios with large errors in obstacle velocity and heading angle estimation (corresponding to an error covariance matrix Q of diag(0.01)). 2 ; 0.01 2 ; 0.02 2 ; 0.0873 2 The second set of experiments targets scenarios where the estimation errors of the horizontal and vertical coordinates of obstacles are large (corresponding to an error covariance matrix Q of diag(0.03)). 2 ; 0.03 2 ; 0.01 2 ; 0.0524 2 In each of the two scenarios described above, 100 experiments were conducted independently. The results of the two sets of experiments are as follows: Figure 7 and Figure 8 As shown. From Figure 7 and Figure 8 As can be seen, the minimum obstacle avoidance distance of the method of the present invention is always greater than the boundary distance limit, while the other two methods have results that are less than the boundary distance limit. This indicates that the mobile robot using the other two methods collides during the driving process, that is, it cannot completely avoid dynamic obstacles.

[0053] The results of 100 repeated experiments in the two scenarios described above were statistically analyzed, and the success rates of the method of this invention and the other two methods in completely avoiding dynamic obstacles are shown in Table 1. Table 1 shows that the method of this invention can effectively avoid obstacles regardless of whether the obstacle's position error or its speed and heading angle error is large, achieving a 100% success rate in avoiding dynamic obstacles, which is a significant advantage compared to the other two methods.

[0054]

[0055] Table 1

[0056] Finally, it should be noted that the above descriptions are merely preferred embodiments of the present invention and are not intended to limit the present invention. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art can still modify the technical solutions described in the foregoing embodiments or make equivalent substitutions for some of the technical features. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A dynamic obstacle avoidance trajectory tracking control method for a Mecanum wheeled mobile robot, characterized in that: The specific steps of the method include: Step 1: Use the Mecanum wheel mobile robot model to obtain the transformation relationship between the body coordinate system and the world coordinate system, that is, the relationship between the state variables in the world coordinate system and the control input in the body coordinate system, so as to provide a motion model for the nonlinear model prediction trajectory tracking controller; Step 2: Acquire observation data of dynamic obstacles using sensors mounted on the Mecanum wheeled mobile robot; Step 3: Use the Extended Kalman Filter (EKF) to obtain the estimated state of the dynamic obstacle and its corresponding error covariance matrix; Step 4: Based on the current obstacle position and error, use the unobserved prediction of the Extended Kalman Filter (EKF) to predict the future obstacle state, and with the error propagation of the unobserved prediction, finally obtain the future obstacle position estimate and the corresponding error covariance matrix. Step 5: Construct a confidence ellipse region with the estimated position of the dynamic obstacle as the center, the corresponding error covariance matrix and a certain confidence level, so that it covers the real trajectory; Step 6: Add the estimated position of the dynamic obstacle and the range of the confidence ellipse to the constraints of the nonlinear model predictive controller of the Mecanum wheel mobile robot, and dynamically adjust the size of the confidence ellipse range according to different error covariance matrices; Step 7: Using the tracked trajectory as the target and dynamic obstacles as constraints, obtain the optimal control sequence for the future control range of the Mecanum wheel mobile robot. Apply the first term to the Mecanum wheel mobile robot, and then use the inverse kinematics equations of the Mecanum wheel mobile robot to obtain the angular velocities of the four Mecanum wheels as the final input for control, so as to achieve the purpose of dynamically adjusting the obstacle avoidance area while tracking the trajectory. Repeating steps one through seven will enable trajectory tracking control of the Mecanum wheel mobile robot under any dynamic obstacle.

2. The dynamic obstacle avoidance trajectory tracking control method for a Mecanum wheel mobile robot according to claim 1, characterized in that: The relationship between the world coordinate system and the body coordinate system in step one is as follows: In the formula, X = [x w y w θ w ] T X is the state variable vector of the controlled robot in the world coordinate system. It is the derivative of the state variable vector of the controlled robot in the world coordinate system, x w It is the x-coordinate of the controlled robot in the world coordinate system, y w θ is the vertical coordinate of the controlled robot in the world coordinate system. w It is the heading angle of the controlled robot in the world coordinate system, and the symbol T is the transpose of the vector; in the formula It is the state transition matrix; where u b =[v bx v by ω b ] T u b It is the control input vector in the body coordinate system, v bx Let v be the linear velocity in the x-direction of the body coordinate system. by Let w be the linear velocity in the y-direction of the body coordinate system. by ω is the angular velocity in the body coordinate system.

3. The dynamic obstacle avoidance trajectory tracking control method for a Mecanum wheel mobile robot according to claim 1, characterized in that: In step two, the state variable X in the obstacle world coordinate system is selected. o =[x o y o v o θ o ] T In the formula X O It is the state variable vector of the dynamic obstacle in the world coordinate system, x o Let x be the x-axis coordinate of the obstacle in the world coordinate system, and y be the y-axis coordinate. o Let v be the y-coordinate of the obstacle in the world coordinate system. o Let θ be the velocity of the obstacle in the world coordinate system. o Let X be the heading angle of the obstacle in the world coordinate system; after discretizing the continuous system using the forward Euler method, a dynamic obstacle motion model is constructed as X. o,k+1 =f o (X o,k ), where X o,k+1 Let f be the state variable vector of the obstacle at time k+1. o (·) is a motion model function describing the relationship between the state of the obstacle and time, X o,k Let Z be the state variable vector of the obstacle at time k; construct the sensor observation model as Z o,k =h o (X o,k ), where Z o,k Let h be the sensor's observation of the obstacle's state information at time k. o (·) is a sensor observation model function that describes the mapping relationship between obstacle state and observation value.

4. The dynamic obstacle avoidance trajectory tracking control method for a Mecanum wheel mobile robot according to claim 1, characterized in that: Step three considers process noise during state prediction. and observation noise Among them, w k The time k follows a normal distribution The zero-mean Gaussian process noise has a covariance of Q, and v k The time k follows a normal distribution The zero-mean Gaussian observation noise has a covariance of R, and Given the state at time k-1, the prediction steps of EKF are used. Obtain the prior estimate and prior error covariance matrix of the current time k, where It is the prior state estimate at time k, obtained based on the posterior estimate at time k-1. Let be the posterior state estimate at time k-1. P is the prior error covariance matrix describing the uncertainty of the prior estimate at time k. o,k-1|k-1 Let F be the posterior error covariance matrix at time k-1. o,k The motion model f at time k o (·) The state of the obstacle at time k-1 The Jacobian matrix; then updated using the current sensor observations through the EKF update steps. To obtain a relatively accurate state estimate and the corresponding error covariance matrix at the current moment, where Y... o,k Let K be the observation residual at time k. o,k H is the Kalman gain at time k. o,k For the observation model h at time k o (·) The state of the obstacle at time k-1 Jacobian matrix, Let P be the posterior state estimate at time k. o,k|k Let I be the posterior error covariance matrix at time k. 4×4 It is a 4th order identity matrix.

5. The dynamic obstacle avoidance trajectory tracking control method for a Mecanum wheeled mobile robot according to claim 1, characterized in that: Step four involves obtaining the EKF-filtered estimate at the current time. P o,k|k Predict the trajectory for the next N steps, where N is the time domain for predicting the future trajectory. For the next N steps i = 0, 1, ..., N-1, we have: In the formula, i is the index of the prediction step. Let k be the time to predict the obstacle state estimate for the i-th step in the future. Let k be the obstacle error covariance matrix for predicting the i-th step in the future. Let the Jacobian matrix at time k be the one used to predict the i-th future step; initialized to... Finally, obtain the predicted states of all obstacles for the next N steps from the current moment. and the corresponding obstacle error covariance matrix Location prediction and confidence region are about [x o y o ] T Given the distribution of the variables, we only take the elements of the state prediction and error covariance with respect to the horizontal and vertical axes as the input to the controller, then we have: and In the formula To obtain the predicted state The first two elements yield the horizontal and vertical coordinate vectors of the obstacle in the i-th step. To obtain The error covariance matrix of the obstacle position at the i-th step is obtained from the first 2×2 submatrix.

6. The dynamic obstacle avoidance trajectory tracking control method for a Mecanum wheeled mobile robot according to claim 1, characterized in that: The ellipse in step five is the probability contour line of a bivariate Gaussian distribution; therefore, the confidence ellipse is defined by the Mahalanobis distance. In the formula It is the critical value of the chi-square distribution with 2 degrees of freedom and a confidence level of 1-α; the confidence ellipse with a fixed confidence level is mapped to an ellipse in Euclidean space, and the prediction error and confidence level are combined to form a dynamically adjustable obstacle uncertainty boundary, thereby improving the robustness and safety of obstacle avoidance.

7. The dynamic obstacle avoidance trajectory tracking control method for a Mecanum wheel mobile robot according to claim 1, characterized in that: The state variable of the nonlinear model predictor controller in step six is: X = [x w y w θ w ] T The control variable is: u b =[v bx v by ω b ] T The prediction time domain is N, and the sampling period is T. s ; Let the reference trajectory be a function of time, and its discretization result be X. ref,k =[x ref,k y ref,k θ ref,k ] T In the formula X ref,k Let x be the reference trajectory vector that the robot expects to track at time k after discretization. ref,k Let x be the x-axis coordinate of the reference trajectory at time k, and y be the y-axis coordinate. ref,k Let θ be the y-axis coordinate of the reference trajectory at time k. ref,k Let be the heading angle of the reference trajectory at time k; the control objective is to minimize the error between the mobile robot platform state and the reference state while satisfying physical constraints; in particular, it is able to avoid obstacles and perform local path replanning. In addition to considering the boundary radius of the obstacle, the boundary radius of the Mecanum wheel mobile robot platform and the safety margin, a confidence ellipse with a certain confidence level is used as the obstacle avoidance area to eliminate the uncertainty of the extended Kalman filter estimation of the obstacle position caused by sensor error and process error.

8. The dynamic obstacle avoidance trajectory tracking control method for a Mecanum wheel mobile robot according to claim 1, characterized in that: In step seven, when the observation and correction data of the dynamic obstacle is lost, the system performs observation-free prediction. The accumulation of errors causes the confidence ellipse to expand, and the obstacle avoidance area increases to cover the true position of the obstacle, enabling the controlled object to effectively avoid obstacles.

Images