Mobile robot control method and system based on multi-source fusion dynamic trajectory replanning

Abstract

The invention discloses a mobile robot control method and system based on multi-source fusion dynamic trajectory re-planning. The method comprises the steps that vehicle kinematics data and environment characteristic observation data are acquired in real time; state prediction is carried out based on a kinematic model; performing multi-dimensional feature extraction and confidence evaluation on the environment features; carrying out measurement updating by utilizing extended Kalman filtering; performing time domain differential analysis on the posterior state; when the state jump is detected, generating a smooth dynamic transition track by taking the current posterior state as a starting point; acquiring predicted curvature information of the front path from the global map; inputting the confidence score and the predictive curvature into a fuzzy logic controller, and calculating the adjustment amount of PID control gain; and calculating a motor control instruction based on the dynamic transition trajectory by using an adaptive PID algorithm, and outputting a final control signal. According to the method, high-precision positioning and stable control can be realized only by depending on a low-cost vehicle-mounted sensor, the cost is low, the universality is high, and the method is suitable for various complex indoor scenes.

Description

Technical Field

[0001] This invention belongs to the field of autonomous navigation and motion control technology for mobile robots, specifically relating to a mobile robot control method and system based on multi-source fusion dynamic trajectory replanning. Background Technology

[0002] While various sectors widely prioritize the automation and safety of internal logistics, the stable operation and control of transportation equipment in complex environments are equally crucial. In modern warehousing and logistics, navigation instability in automated mobile warehouses (AMRs) not only leads to inefficient material transfers but can also cause accidents such as cargo tipping and vehicle collisions. In precision manufacturing, path tracking jitter in shop floor robots not only increases energy consumption but can also cause irreversible physical damage to expensive production equipment if they deviate from their preset trajectories. Therefore, timely, smooth, and high-precision control of vehicle movement can effectively reduce the probability of repeated logistics hazards.

[0003] Currently, to address the navigation problem of mobile robots in indoor environments where satellite signals are denied, some organizations utilize magnetic strips or QR codes to lay fixed guidance paths, achieving deterministic navigation in the area to be tested. Other researchers have proposed using high-precision lidar to build a SLAM (Simultaneous Localization and Mapping) system, matching environmental point clouds in real time to obtain global pose. Still others have implemented autonomous localization based on visual SLAM principles, combining feature point extraction and loop closure detection. Additionally, there are methods based on multi-sensor data fusion, using extended Kalman filtering and fusing odometry and sparse landmark information for localization correction. Regarding the control oscillation problem that occurs during localization correction, some researchers have proposed improvements through filtering algorithms, introducing moving averages or low-pass filters to smooth localization data and reduce the impact of state transitions on the controller. However, phase lag is unavoidable, making this approach ineffective in real-time response to sharp bends or obstacle avoidance situations. Therefore, some research institutions have proposed a model predictive control (MPC) method, using future states to constrain control variables, but its parameter tuning is complex and requires significant computational power.

[0004] However, in practical applications, both the SLAM scheme based on heavy sensors and the MPC control method based on complex optimization algorithms are relatively complex and difficult to implement. They lack economic efficiency and are easily limited by computing power. In contrast, the method of using multi-source information fusion technology to obtain the state and combining it with dynamic trajectory replanning strategy to eliminate the impact of positioning jumps not only retains the economic efficiency of low-cost sensors, but also solves the control oscillation problem caused by discrete correction. It is easy to implement and has strong environmental adaptability. Summary of the Invention

[0005] Purpose of the invention: This invention proposes a mobile robot control method and system based on multi-source fusion and dynamic trajectory replanning, which can achieve high-precision positioning and stable control by relying solely on low-cost on-board sensors. It is low in cost and highly versatile, and suitable for various complex indoor scenarios.

[0006] Technical solution: The mobile robot control method based on multi-source fusion and dynamic trajectory replanning described in this invention includes the following steps:

[0007] (1) Use multi-source sensors to acquire vehicle kinematic data and environmental feature observation data in real time, and perform time synchronization and preprocessing on the data;

[0008] (2) Based on the kinematic model, state prediction is performed to obtain the prior state estimate of the vehicle;

[0009] (3) Multidimensional feature extraction and confidence assessment are performed on the observed environmental features to obtain the observation confidence score;

[0010] (4) Construct an adaptive measurement noise matrix based on confidence scores, and use extended Kalman filtering for measurement updates to obtain the posterior state estimate;

[0011] (5) Perform time-domain differential analysis on the posterior state to detect whether there is a location state jump; if the state difference between the previous and next times exceeds the preset safety threshold, it is determined that a location jump has occurred.

[0012] (6) When a state transition is detected, a smooth dynamic transition trajectory is generated starting from the current posterior state;

[0013] (7) Obtain the predicted curvature information of the path ahead from the global map;

[0014] (8) Input the confidence score and the predicted curvature into the fuzzy logic controller and calculate the adjustment amount of the PID control gain;

[0015] (9) Calculate the motor control command based on the dynamic transition trajectory using the adaptive PID algorithm and output the final control signal.

[0016] Furthermore, the implementation process of step (2) is as follows:

[0017] The nonlinear state prediction model is constructed as follows:

[0018]

[0019] in, Indicates the first The prior state estimate of the vehicle at time 1; Indicates the first The posterior state estimate of the vehicle at time 1; Indicates the first The system control input vector at any given time is determined by the vehicle's longitudinal velocity. lateral velocity and rotational angular velocity composition; This represents the coordinate rotation matrix, whose physical meaning is to project and transform the velocity vector in the vehicle coordinate system to the global coordinate system;

[0020] Expanding the matrix yields the prior states. Scalar calculation formula:

[0021]

[0022] in, This represents the prior state estimate at time k (before visual observations are fused). This is an estimate of the posterior state from the previous time step; Indicates the first The heading angle of the vehicle in the global coordinate system at any given time is used to calculate the trigonometric function components of the rotation matrix; This represents the time interval between two adjacent moments;

[0023] State transition function Regarding the state vector Find the partial derivatives and calculate the Jacobian matrix. :

[0024]

[0025] Among them, the position is related to the heading angle The partial derivative terms are calculated as follows:

[0026]

[0027] Substituting the partial derivative terms, we obtain the final linearized Jacobian matrix used for covariance propagation. :

[0028]

[0029] Explicit introduction The related term, namely the speed of lateral movement of the vehicle body, describes the coupled effect of the lateral motion unique to the Mecanum wheel chassis on the uncertainty of position prediction.

[0030] Furthermore, the implementation process of step (3) is as follows:

[0031] Edge detection and Hough transform are performed on the preprocessed image within the region of interest to extract several line segment sets. The discrete line segments are then fitted to the equation of the environmental feature line using the least squares method.

[0032] Based on the fitted linear equation, the geometric relationship between the vehicle center and the recognition features is calculated, and an observation vector is constructed. ;

[0033] Define feature continuity index Geometric consistency index Two independent quality assessment dimensions are used, and the indicators of the two dimensions are weighted and fused to generate the final observation confidence score. :

[0034]

[0035]

[0036]

[0037]

[0038] in, This is a line width correction factor. It is applied when the road surface is covered with oil, worn, or excessive sunlight causes the lines to break. Significant reduction, leading to The value decreased; For the fit variance, The variance is the sensitivity attenuation coefficient, which is used when complex textures on the ground cause the extracted edge points to be disordered. Enlargement, leading to It rapidly approaches 0; and Let be the weighting coefficient, satisfying .

[0039] Furthermore, the implementation process of step (4) is as follows:

[0040] The measurement noise covariance matrix R is designed as a confidence score. function The adaptive measurement noise matrix is ​​defined as follows:

[0041]

[0042] in, Based on confidence score The adaptive measurement noise covariance matrix at time k is the variable. This represents the confidence score for environmental characteristics; Indicates taking and The larger value in the fraction is used to ensure that the denominator is always a valid positive number; Let be the reference noise covariance matrix, representing the noise under ideal observation conditions. The inherent physical noise of the lower sensor; Represents the lateral distance observations in the baseline matrix The noise variance; Represents the heading angle observations in the reference matrix. The noise variance; Use a very small positive number to prevent the denominator from being zero.

[0043] when When (observation is clear), The filter assumes minimal observation noise and tends to accept the observed values; when When (observation is blurred), The filter considers the observation noise to be extremely high and almost completely ignores the observation values, thereby avoiding erroneous features from causing the localization to be biased.

[0044] Calculate the observation Jacobian matrix and Kalman gain :

[0045]

[0046] in, Let represent the optimal Kalman gain matrix at time k. This weighting coefficient matrix is ​​used to determine the extent to which the system trusts the current observation information. Let represent the prior error covariance matrix at time k; Let represent the observation Jacobian matrix at time k; This represents the transpose of the observation Jacobian matrix; This indicates the confidence score at time k. Dynamically calculated adaptive measurement noise covariance matrix;

[0047] The prior state is corrected using Kalman gain to obtain the posterior state estimate. :

[0048]

[0049] Update the posterior error covariance matrix Used for iteration in the next time step:

[0050]

[0051] in, Let represent the posterior state estimate at time k; This represents the prior state estimate at time k; This represents the measurement residual at time k, i.e., the actual observation vector. Compared with theoretically predicted observation vectors The difference between them; Let represent the posterior error covariance matrix at time k; I represents the identity matrix with the same dimension as the state vector.

[0052] Furthermore, the implementation process of step (5) is as follows:

[0053] The posterior state estimation sequence is monitored in real time. By calculating the rate of change of vehicle position and heading within adjacent control cycles and combining this with the vehicle's own physical motion limit constraints, non-physical state abrupt changes caused by discrete landmark corrections are identified. Specifically, this includes the following state differential analysis and threshold decision process:

[0054]

[0055]

[0056] in, This represents the differential increment of the vehicle's position at time k; This represents the estimated posterior x-coordinate of the vehicle at time k. This represents the estimated posterior ordinate of the vehicle at time k. This represents the estimated posterior x-coordinate of the vehicle at time k-1. This represents the estimated posterior ordinate of the vehicle at time k-1. This represents the differential increment of the vehicle's heading at time k; Let represent the estimated posterior heading angle of the vehicle at time k; This represents the estimated posterior heading angle of the vehicle at time k-1.

[0057] Construct dynamic thresholds based on physical limits, including position change judgment thresholds. and heading jump determination threshold :

[0058]

[0059]

[0060] in, This indicates the location tolerance coefficient; This indicates the design maximum linear speed of the omnidirectional mobile delivery vehicle; Indicates the duration of the system's control cycle; This indicates the baseline noise level of the positioning system; Indicates the heading tolerance factor; This indicates the maximum design rotational angular velocity of the omnidirectional mobile delivery vehicle; Indicates the baseline noise level for heading measurements;

[0061] Define a state transition flag based on the calculated increment and threshold. The decision logic function is as follows:

[0062]

[0063] in, This indicates a state transition detection flag; 1 indicates a transition has been detected, and 0 indicates normal operation. This represents the positional differential increment at time k; This indicates the threshold for determining position transitions; This represents the differential increment of the heading at time k; This indicates the threshold for determining a course jump; Represents the logical OR operator.

[0064] Furthermore, the implementation process of step (6) is as follows:

[0065] Obtain the corrected true position and heading of the vehicle at the current moment, and use them as the starting point and tangent of the trajectory;

[0066] Search for a point at a preset distance ahead along the original global path, and use it as the trajectory endpoint and the endpoint tangent;

[0067] Using the third-order Bézier curve polynomial equation, a smooth parametric curve connecting the two points is calculated based on the start point, end point, and their tangential constraints. To satisfy positional continuity and tangential continuity, and to ensure that the transition trajectory is consistent with the vehicle's current heading at the start point and smoothly tangent to the original path at the end point, the control points are calculated as follows:

[0068]

[0069] in, This indicates the first control point, i.e., the starting point of the trajectory; The x and y coordinates represent the starting point of the trajectory; This indicates the fourth control point, which is the endpoint of the trajectory; The x and y coordinates represent the endpoint of the trajectory; This indicates the second control point, used to constrain the tangent direction of the starting point; Indicates the curvature control coefficient; Indicates the direction of the tangent at the starting point of the trajectory; This indicates the third control point, used to constrain the tangent direction at the endpoint; Indicates the direction of the tangent at the endpoint of the trajectory;

[0070] Based on four control points, a third-order Bézier curve equation with respect to the normalized time parameter t is constructed. :

[0071]

[0072] Where B(t) represents the point coordinate vector on the generated transition trajectory. ; t represents the normalized time parameter;

[0073] Perform equal-interval sampling on parameter t, and calculate the coordinates and target heading for each sampling point:

[0074]

[0075] in, Represents the x and y coordinates of the i-th discrete trajectory point; The equation of the Bézier curve is expressed in The x and y coordinate components at the location; The target tangential angle, i.e. the desired heading, is represented by the i-th discrete trajectory point. This represents the first derivative of the Bézier curve equation with respect to the parameter t, reflecting the tangent vector at that point.

[0076] Furthermore, the implementation process of step (7) is as follows:

[0077] Determine the aiming distance With search target point :

[0078]

[0079]

[0080] in, This represents the velocity gain coefficient, which predicts the path several seconds into the future. This indicates the current combined speed modulus of the vehicle (; This indicates the minimum basic aiming distance to prevent aiming points from coinciding when stationary or at low speed.

[0081] This represents a path indexing function used to return the coordinates of a path point at a specified arc length; This represents the projected arc length of the vehicle's current position on the path;

[0082] According to the pre-aiming point Calculate the geometric curvature at the point corresponding to the path type. :

[0083]

[0084] in, This indicates the predicted path curvature; the larger the value, the sharper the curve. Indicates the aiming point The corresponding normalized time parameter; This represents the first derivative of the x and y coordinates of the Bézier curve equation with respect to the parameter t; This represents the second derivative of the Bézier curve equation with respect to the parameter t; This indicates absolute value operations, where the magnitude of curvature is considered rather than the direction of bending.

[0085] For the calculated Perform first-order low-pass filtering:

[0086]

[0087] in, This represents the final predicted curvature after filtering; Indicates the curvature filter coefficients; This represents the original curvature calculated at the current moment; It represents the final predicted curvature at the previous moment.

[0088] Furthermore, the implementation process of step (8) is as follows:

[0089] Define the fuzzy subsets and membership functions of the input variables confidence score and predicted curvature: Confidence Score As input to perceived quality, its fuzzy subset is defined as ;when When the value is close to 0, it indicates severe distortion or loss of visual features, and the sensitivity to localization correction should be reduced; when... A value close to 1 indicates that visual observation is extremely reliable and can maintain standard control gain; predictable curvature. As input to path complexity, its fuzzy subset is defined as The greater the curvature, the more likely the vehicle is about to enter a sharp bend or is in a high-curvature section of the transition trajectory, requiring a faster response speed to reduce lag.

[0090] Establish a fuzzy inference rule base and set the control logic; output the proportional gain adjustment coefficient. and differential gain adjustment coefficient ;

[0091] The centroid method is used to defuzzify the inference results and output the real-time adjustment values ​​of the proportional, integral, and derivative gains; thus achieving adaptive adjustment of the control parameters.

[0092] Furthermore, the process of using the centroid method to defuzzify the reasoning results is as follows:

[0093] The unfuzzy subset is the geometric center of the area enclosed by the membership functions output by the fuzzy subset, and its mathematical continuous form is:

[0094]

[0095] In the discrete calculation of practical control systems, the numerator is weighted, the denominator is the total area, and the result is the coordinates of the equilibrium point of the geometric figure on the horizontal axis, i.e., the centroid. The formula simplifies to:

[0096]

[0097] in, The output variable is the discrete sampled value within its domain; For sampling points The total membership degree after merging according to the inference rules; For the final, precisely quantized value; Output membership function at sampling points The height of the sampling point is calculated by summing the heights of all sampling points, which is geometrically equivalent to calculating the area of ​​the total output region formed by the superposition of multiple activated rules.

[0098] The mobile robot control system based on multi-source fusion dynamic trajectory replanning described in this invention includes a data acquisition and preprocessing module, an environmental feature confidence assessment module, a weighted fusion localization module, a dynamic trajectory replanning module, and a map prediction and adaptive control module; wherein:

[0099] The data acquisition and preprocessing module uses multi-source sensors mounted on the vehicle to acquire vehicle kinematic data and environmental feature observation data in real time, and completes data time synchronization and noise reduction processing.

[0100] The environmental feature confidence assessment module extracts ground guide lines or intersection landmark features from preprocessed sensing data, calculates observation confidence scores based on feature clarity, geometric continuity, and fitting residuals, and quantifies the reliability of current environmental observations.

[0101] The weighted fusion positioning module is used to introduce the confidence score as a weight adjustment factor into the extended Kalman filter algorithm, dynamically adjust the measurement noise covariance matrix, fuse dead reckoning data and environmental observation data, and output a smooth and high-precision global state estimate of the vehicle.

[0102] The dynamic trajectory replanning module monitors the rate of change of the positioning status in real time. When it detects a non-physical state jump caused by the correction of discrete landmarks, it generates a smooth transition spline curve connecting the current position and the global path, starting from the corrected current real position.

[0103] The map prediction and adaptive control module uses the generated transition curve as a temporary tracking target. It combines the predicted curvature information extracted from the global map with the current confidence score, uses fuzzy logic to adjust the PID control gain in real time, calculates the motor control command, and outputs the result.

[0104] Beneficial Effects: Compared with existing technologies, the present invention offers the following advantages: It achieves high-precision positioning and smooth control using only low-cost vehicle-mounted sensors. While being cost-effective, it is highly versatile and applicable to complex indoor spaces where stable GPS or BeiDou signals cannot be received, and where there are extractable feature lines such as guide lines and corner lines, such as in indoor warehouse scenarios. Furthermore, the present invention utilizes state transitions to trigger dynamic trajectory replanning, transforming abrupt changes in positioning data into smooth transition curves in geometric space. Combined with the continuity characteristics of parametric spline curves, it eliminates input jitter in the control system from a physical perspective, significantly improving the smoothness of vehicle operation. Considering the impact of environmental interference on positioning accuracy, the present invention designs an adaptive weighted fusion algorithm based on feature confidence and, in conjunction with map prediction information, adjusts the control gain in real time using fuzzy logic. This integrates perception quality and path geometric attributes, improving the system's robustness and environmental adaptability in complex industrial environments. Attached Figure Description

[0105] Figure 1 This is a schematic diagram of a mobile robot control system based on multi-source fusion dynamic trajectory replanning;

[0106] Figure 2 This is a flowchart of a mobile robot control method based on multi-source fusion dynamic trajectory replanning;

[0107] Figure 3 A schematic diagram illustrating the application scenario of this invention. Detailed Implementation

[0108] The present invention will now be described in further detail with reference to the accompanying drawings.

[0109] like Figure 1 As shown, the present invention provides a mobile robot control system based on multi-source fusion dynamic trajectory replanning, including a data acquisition and preprocessing module, an environmental feature confidence assessment module, a weighted fusion positioning module, a dynamic trajectory replanning module, and a map prediction and adaptive control module.

[0110] The data acquisition and preprocessing module uses multi-source sensors mounted on the vehicle to acquire vehicle kinematic data (angular velocity, linear velocity) and environmental feature observation data (visual images or radar point clouds) in real time, and completes data time synchronization and noise reduction processing.

[0111] The environmental feature confidence assessment module extracts ground guide lines or intersection landmark features from preprocessed sensing data, calculates the observation confidence score based on the feature clarity, geometric continuity, and fitting residuals, and quantifies the reliability of the current environmental observation.

[0112] The weighted fusion positioning module incorporates confidence scores as weight adjustment factors into the extended Kalman filter algorithm, dynamically adjusts the measurement noise covariance matrix, and fuses dead reckoning data with environmental observation data to output a smooth and high-precision global state estimate of the vehicle.

[0113] The dynamic trajectory replanning module monitors the rate of change of positioning status in real time. When it detects a non-physical state jump caused by discrete landmark correction, it generates a smooth transition spline curve connecting the current position and the global path, starting from the corrected current real position.

[0114] The map prediction and adaptive control module uses the generated transition curve as a temporary tracking target. It combines the predicted curvature information extracted from the global map with the current confidence score, uses fuzzy logic to adjust the PID control gain in real time, calculates the motor control command, and outputs the result.

[0115] like Figure 2 As shown, this invention also provides a mobile robot control method based on multi-source fusion dynamic trajectory replanning, which includes state perception, quality assessment, fusion correction, jump detection, and flexible control processes. Essentially, it designs a robust navigation framework suitable for satellite signal rejection environments, rationally planning the positioning correction process into two stages: "truth-seeking" and "stability-seeking." A confidence assessment mechanism is used to filter high-quality observation data to obtain the true position, and a dynamic trajectory replanning strategy is used to mitigate the control shock caused by sudden positioning changes. The overall tracking performance of the system is improved by integrating environmental quality and path geometry attributes. The input data is a heterogeneous data stream collected in real time by multiple source sensors.

[0116] The control system and method designed in this invention can be applied to, for example... Figure 3 The scenario shown demonstrates how omnidirectional mobile delivery vehicles within a large warehousing and logistics center perform high-precision material transfer tasks under complex conditions, such as damaged ground guide lines or changing lighting. The specific implementation process is as follows:

[0117] Step 1: Use multi-source sensors to acquire vehicle kinematic data and environmental feature observation data in real time.

[0118] Taking a Mecanum wheel delivery vehicle in a large warehousing and logistics center as a typical application scenario, the vehicle's motion status and external environment images are acquired in real time using an inertial measurement unit (IMU) rigidly connected to the vehicle body, incremental encoders installed on the four drive wheels, and a forward-looking monocular camera.

[0119] (1.1) Odometry data acquisition based on Mecanum wheel kinematics.

[0120] The rotational speed of the left front wheel was collected using four wheel encoders. ), Right front wheel speed ( ), left rear wheel speed ( ) and right rear wheel speed ( The real-time rotational speed pulse of the vehicle is obtained. Based on the inverse kinematics model of the Mecanum wheel, the longitudinal velocity of the vehicle in the vehicle coordinate system is calculated. lateral velocity and rotational angular velocity .

[0121] The calculation formula is shown in the matrix below:

[0122]

[0123] in, Let the radius of the Mecanum wheel be 1. This is the longitudinal distance from the center of the vehicle body to the axle. This is the lateral distance from the center of the vehicle body to the axle. Longitudinal velocity, Lateral velocity, Rotational angular velocity, Left front wheel speed, Right front wheel speed, Left rear wheel speed, Right rear wheel speed.

[0124] At the same time, the vehicle's three-axis acceleration is collected using an IMU. and high-precision Z-axis angular velocity Considering that wheel slippage will affect the angular velocity calculated by the encoder. There is a large margin of error; this step uses the IMU. (Z-axis angular velocity) is used as the true value of the vehicle's steering observation.

[0125] (1.2) Acquisition of environmental characteristic observation data.

[0126] The system uses a vehicle-mounted forward-facing camera to capture environmental images at a fixed frame rate. A region of interest (ROI) is defined; typically, the lower half to two-thirds of the image is cropped as the detection area for ground guide lines, and the middle portion of the image is cropped as the detection area for QR codes or intersection landmarks.

[0127] (1.3) Data time synchronization and preprocessing.

[0128] Because of the frequency difference between the IMU (sampling rate > 100 Hz), the encoder (sampling rate > 50 Hz), and the effective frame rate after visual algorithm processing (usually about 30~60 Hz), time alignment is required.

[0129] First, using IMU timestamps Based on this, linear interpolation is used to align the encoder data to the IMU time.

[0130] Secondly, the raw data undergoes filtering preprocessing: a moving average filter is used to remove high-frequency vibration noise from the IMU data; a Gaussian filter is used to remove image noise from the image data, and distortion correction is performed based on the camera intrinsic parameter matrix K and distortion coefficient D to eliminate barrel distortion caused by the wide-angle lens, ensuring the accuracy of geometric features in subsequent calculations. All sensor data is then uniformly transformed to the vehicle center coordinates using an extrinsic parameter matrix.

[0131] Step 2: Perform state prediction based on the kinematic model to obtain the prior state estimate of the vehicle.

[0132] Using the vehicle longitudinal velocity obtained in step 1 lateral velocity (From Mecanum wheel odometer) and high-precision angular velocity (Z-axis angular velocity from the IMU), combined with the discrete-time kinematic model of the omnidirectional moving chassis, is used to calculate the vehicle's prior position and heading in the global coordinate system for the next control cycle. This prior state will serve as the benchmark value for subsequent multi-source fusion.

[0133] (2.1) Establish the system state vector and control input vector.

[0134] Define the vehicle's state vector in the global coordinate system. , including x-axis y-axis and heading angle :

[0135]

[0136] Define the system's control input vector It consists of the vehicle body velocity components and IMU angular velocity calculated in step 1:

[0137]

[0138] in, This represents the control input vector of the system at time k. This represents the longitudinal linear velocity of the vehicle (i.e., the velocity along the X-axis of the vehicle coordinate system) obtained by the four Mecanum wheel encoders through inverse kinematics at time k. This represents the lateral linear velocity of the vehicle (i.e., the velocity along the Y-axis of the vehicle coordinate system) obtained by the four Mecanum wheel encoders through inverse kinematics at time k. This component reflects the omnidirectional movement characteristic of the Mecanum wheels. This represents the high-precision vehicle rotation angular velocity (i.e., the angular velocity of rotation around the Z-axis) directly acquired by the inertial measurement unit (IMU) at time k. Here, IMU data is preferred over encoder-derived data to suppress the cumulative heading error caused by wheel slippage.

[0139] (2.2) Construct nonlinear state transition equations.

[0140] Based on the omnidirectional movement characteristics of the Mecanum wheel chassis, the vehicle... The pose changes within the time interval need to be projected from the vehicle coordinate system to the global coordinate system using a rotation matrix. The nonlinear state prediction model is constructed as follows:

[0141]

[0142] in, This represents the prior state estimate of the vehicle at time k, including the state vector heading in the global coordinate system. x-axis y-axis It is a result calculated solely based on the state of the previous moment and the current kinematics, and has not yet incorporated the visual observation data of the current moment; Indicates the first The posterior state estimate of the vehicle at time (previous control cycle), which is the optimal state after multi-source fusion correction at the previous time. The system control input vector at time k is represented by the vehicle's longitudinal velocity. lateral velocity and rotational angular velocity composition; This represents the coordinate rotation matrix, which physically transforms the velocity vector in the vehicle coordinate system to the global coordinate system.

[0143] Expanding the matrix yields the prior states. Scalar calculation formula:

[0144]

[0145] in, This represents the prior state estimate at time k (before visual observations are fused). This is an estimate of the posterior state from the previous time step; Indicates the first The heading angle of the vehicle in the global coordinate system at any given time is used to calculate the trigonometric function components of the rotation matrix; It represents the time interval between two adjacent moments.

[0146] (2.3) Calculate the state transition Jacobian matrix.

[0147] Since the above state equation is nonlinear, in order to update the covariance matrix in the extended Kalman filter, the state transition function needs to be modified. Regarding the state vector Find the partial derivatives and calculate the Jacobian matrix. :

[0148]

[0149] Among them, the position is related to the heading angle The partial derivative terms are calculated as follows:

[0150]

[0151] Substituting the partial derivative terms, we obtain the final linearized Jacobian matrix used for covariance propagation. :

[0152]

[0153] This step involves explicitly introducing... Related items ( The relevant term refers to the calculation component that directly reflects the "lateral translation speed" of the Mecanum wheel in the mathematical formula.

[0154] Specifically, this includes: ① the displacement term in the state prediction equation, i.e., the coordinate changes caused by the lateral velocity in the x and y directions at each moment. ② the Jacobian matrix. Coupling terms in the Jacobian matrix: Position versus heading angle in the Jacobian matrix The partial derivative terms accurately describe the coupling effect of the unique lateral motion of the Mecanum wheel chassis on the uncertainty of position prediction, which is different from the traditional differential model (without...). Key improvements (item).

[0155] (2.4) Prediction of prior error covariance matrix.

[0156] Based on the Kalman filter principle, using the Jacobian matrix The posterior error covariance of the previous time step The prior error covariance matrix is ​​obtained by passing it to the current time step and adding process noise. :

[0157]

[0158] in, This is the process noise covariance matrix, used to characterize the uncertainties introduced during dead reckoning due to wheel slippage, uneven ground, and mechanical clearance. In this embodiment, Designed as a diagonal matrix:

[0159]

[0160] In the formula, These represent the standard deviations of process noise related to velocity and time, respectively. This step quantifies the positional uncertainty relying solely on dead reckoning, providing a statistical benchmark for weighted corrections using visual observations in subsequent steps 3 and 4.

[0161] Step 3: Perform multidimensional feature extraction and confidence assessment on the observed environmental features to obtain the observation confidence score.

[0162] This step utilizes the preprocessed environmental image obtained in step 1 to extract ground guide lines or landmark features and calculates the geometric residuals of the observed values ​​relative to the prior estimates. This step does not assume that visual observations are absolutely reliable; instead, it dynamically generates a normalized confidence score by calculating the pixel density of the features and the geometric fit variance. It is used to quantify the reliability of current environmental observation data.

[0163] (3.1) Environmental feature extraction and geometric modeling.

[0164] Edge detection and Hough transform are performed on the preprocessed image within the region of interest (ROI) to extract several sets of line segments. These discrete line segments are then fitted to the equations of the environmental feature lines using the least squares method.

[0165]

[0166] in, These are the coefficients of the fitted straight line. Based on this equation, the perpendicular distance from the vehicle's center point (assumed to be the origin of the vehicle coordinate system) to the identified straight line is calculated. and angle deviation , as a visual measurement value.

[0167] (3.2) Observation vector The solution.

[0168] Based on the above fitted straight line equation, the geometric relationship between the vehicle center (origin O(0,0) of the vehicle coordinate system) and the recognition features is calculated, and the observation vector is constructed. :

[0169]

[0170] in, The perpendicular distance from the vehicle center to the recognition line (lateral deviation observation value) is calculated using the following formula:

[0171]

[0172] in, To identify the normal angle of a straight line (heading deviation observation), the calculation formula is as follows:

[0173]

[0174] This observation vector The information will be directly used as the input to the Kalman filter in step 4 and compared with the predicted observations of the prior state to calculate the innovation.

[0175] (3.3) Calculate multidimensional quality indicators.

[0176] To assess the reliability of the above measurements, this invention defines two independent quality evaluation dimensions:

[0177] Dimension ①: Feature Continuity Index ( This metric reflects the clarity and integrity of the ground guide line. It calculates the actual number of edge pixels on the fitted line. The theoretical total pixel length along the straight line of the ROI region The ratio:

[0178]

[0179] in, This is a line width correction factor. When the road surface is covered with oil, worn, or excessively bright light causes the lines to break,... Significant reduction, leading to The value decreased.

[0180] Dimension 2: Geometric Consistency Index This metric reflects the degree of fit between observed features and the geometric model. The mean square error of all edge feature points to the fitted line is calculated and mapped to the interval [0, 1].

[0181]

[0182]

[0183] in, For the fit variance, This represents the sensitivity attenuation coefficient. When complex textures on the ground (such as scratches or reflective tape) cause the extracted edge points to be disorganized, the variance... Enlargement, leading to It rapidly approaches 0.

[0184] (3.4) Generate a comprehensive confidence score .

[0185] The indicators from the two dimensions mentioned above are weighted and fused to generate the final observation confidence score. :

[0186]

[0187] in, and Let be the weighting coefficient, satisfying , usually take Emphasizing the continuity of features. (High confidence) indicates that the ground guide line is clear, continuous and straight, and visual observation is extremely reliable. (Low confidence): This indicates that the ground features are blurry, severely broken, or full of noise. If visual data is forcibly used for correction in this case, it is very easy to cause the positioning to diverge.

[0188] The calculations obtained through this step The value will be directly used as an "adjustment knob" and input into the extended Kalman filter in step 4 to dynamically control the degree of acceptance of visual information by the system.

[0189] Step 4: Construct an adaptive measurement noise matrix based on the confidence score, and use extended Kalman filtering to update the measurement and obtain the posterior state estimate.

[0190] The environmental characteristic observation values ​​calculated in step 3 and its confidence score The prior state calculated in step 2 Fusion is performed. By dynamically adjusting the measurement noise parameters of the filter, strong corrections are applied to eliminate accumulated drift when the observation quality is good, and automatic degradation to dead reckoning is performed to suppress noise interference when the observation quality is poor.

[0191] (4.1) Construct nonlinear observation equations.

[0192] Establish vehicle status based on global map information. The geometric mapping relationship between (at time T) and the observed environmental characteristic Z.

[0193] Assume that the environmental features extracted in step 3 (such as ground guide lines or intersection landmarks) have known straight line equations in the global map. .

[0194] Then the predicted observation value (That is, the theoretical distance and angle from the vehicle's prior position to the map feature) can be represented as a nonlinear function of the state vector. :

[0195]

[0196] in, This represents the predicted observation vector at time k; For the predicted vehicle's lateral and longitudinal coordinates, The predicted vehicle heading angle; This represents the coefficients of the guide line equation for the current road segment retrieved from the global map database, corresponding to the straight line equation. ; This indicates the vehicle's current location on the global map.

[0197] Calculate measurement residuals That is, the actual observation value of the sensor. Compared with theoretical predictions Differences between them:

[0198]

[0199] The residual reflects the degree of deviation between the dead reckoning results and the actual environment (i.e., the cumulative drift).

[0200] (4.2) Construct an adaptive measurement noise covariance matrix based on confidence level.

[0201] In standard extended Kalman filtering, the measurement noise covariance matrix R is typically a fixed constant matrix. However, in this invention, R is designed to represent the confidence score. function The adaptive measurement noise matrix is ​​defined as follows:

[0202]

[0203] in, Based on confidence score The adaptive measurement noise covariance matrix at time k is the variable. The environmental feature confidence score calculated in step 3 is a dimensionless value normalized to the interval [0, 1], reflecting the quality of the current visual observation. Indicates taking and The larger value in the fraction is used to ensure that the denominator is always a valid positive number; Let be the reference noise covariance matrix, representing the noise under ideal observation conditions ( The inherent physical noise of the sensor; Represents the lateral distance observations in the baseline matrix The noise variance is such that the smaller the value, the more accurate the distance measurement is under ideal conditions. Represents the heading angle observations in the reference matrix. The noise variance is such that the smaller the value, the more accurate the angle measurement is under ideal conditions. For a very small positive number (e.g.) This prevents the denominator from being zero. When (observation is clear), The filter assumes minimal observation noise and tends to accept the observed values; when When (observation is blurred), The filter assumes that the observation noise is extremely high and almost completely ignores the observation values, thereby avoiding erroneous features from causing the positioning to be off track.

[0204] In standard extended Kalman filtering applications, the measurement noise covariance matrix This is typically set as a pre-defined fixed constant. This approach assumes that the sensor's error characteristics are stable and consistent under all conditions. This invention, considering noise caused by changes in illumination and guide wire breakage, proposes dynamic adjustment and provides specific mathematical mapping relationships. This ensures that the inherent physical noise or error of the environment is reasonably quantified under non-ideal operating conditions; because the R measurement noise covariance matrix will be updated adaptively when the environment changes, the degree of doubt of the filter on the observation data can also be dynamically adjusted with the environment.

[0205] Through feature continuity ( ) and "geometric consistency ( The two independent dimensions are weighted and fused to generate the result. By directly deriving the localization confidence level from underlying image features (such as edge pixel density and fitting variance), a multi-dimensional confidence score is constructed from both computer vision and control theory perspectives. . The adaptive adjustment is combined with subsequent state transition detection and Bézier curve replanning. The chain-like design of perception quality assessment - filter adaptation - trajectory flexible digestion constructs a complete anti-interference closed loop.

[0206] (4.3) Calculate the observation Jacobian matrix.

[0207] For nonlinear observation functions Calculate the partial derivatives and then the linearized observation Jacobian matrix. :

[0208]

[0209] in, Let represent the observation Jacobian matrix at time k, used to represent the nonlinear observation function. A first-order Taylor expansion (linearization) is performed near the prior state point to establish a linear mapping relationship between small changes in the state space and small changes in the observation space. Represents the observation function The partial derivative operator with respect to the state vector X; This represents a symbolic function, specifically referring to... This indicates whether the vehicle is currently on the left or right side of the guide line, determining the direction of the lateral deviation gradient descent;

[0210] 0 (first row, third column) indicates the lateral distance observation relative to the heading angle. The partial derivative is 0, meaning that under the current geometric model, simple in-situ rotation does not change the perpendicular distance from the vehicle center to the straight line; -1 (second row, third column) indicates the difference between the heading deviation observation and the heading angle. The partial derivatives of .

[0211] (4.4) Perform Kalman gain calculation and state update.

[0212] Calculate Kalman gain This gain determines the magnitude of the correction:

[0213]

[0214] in, Let represent the optimal Kalman gain matrix at time k. This weighting coefficient matrix is ​​used to determine the extent to which the system trusts the current observation information. The prior error covariance matrix at time k is predicted in step 2, representing the level of state uncertainty when relying solely on dead reckoning. The Jacobian matrix representing the observations at time k is used to represent the uncertainty in the state space. ) is mapped to the observation space so as to be compatible with measurement noise ( Perform dimensional matching and comparison; This represents the transpose of the observation Jacobian matrix; This indicates the confidence score at time k. The dynamically calculated adaptive measurement noise covariance matrix enables perceived quality data to directly determine the Kalman gain's confidence weight for the observations through algebraic operations.

[0215] The prior state is corrected using Kalman gain to obtain the posterior state estimate. (i.e., the final output vehicle position):

[0216]

[0217] Update the posterior error covariance matrix Used for iteration in the next time step:

[0218]

[0219] in, Let represent the posterior state estimate at time k; This represents the prior state estimate at time k; This represents the measurement residual at time k, i.e., the actual observation vector. Compared with theoretically predicted observation vectors The difference between them; Let represent the posterior error covariance matrix at time k; I represents the identity matrix with the same dimension as the state vector.

[0220] Through this step, the system outputs a fused global coordinate system. If in step 3 The output posterior state estimate is high and the cumulative error is large. Posterior state estimate relative to the previous time step Significant displacement jumps may occur, which will serve as the basis for triggering step 5 (trajectory replanning).

[0221] Step 5: Perform time-domain differential analysis on the posterior state to detect whether there is a change in the positioning state.

[0222] This step aims to monitor the posterior state estimation sequence output from step 4 in real time. By calculating the rate of change of vehicle position and heading within adjacent control cycles and combining this with the vehicle's own physical motion limit constraints, it identifies non-physical state abrupt changes caused by discrete landmark corrections. This detection result will serve as the sole decision signal to trigger subsequent dynamic trajectory replanning strategies.

[0223] (5.1) Specifically, it includes the following state differential analysis and threshold decision process:

[0224]

[0225]

[0226] in, This represents the differential increment of the vehicle's position at time k (in meters). This represents the estimated posterior x-coordinate of the vehicle at time k. This represents the estimated posterior ordinate of the vehicle at time k. This represents the estimated posterior x-coordinate of the vehicle at time k-1. This represents the estimated posterior ordinate of the vehicle at time k-1. This represents the differential increment of the vehicle's heading at time k (in radians). Let represent the estimated posterior heading angle of the vehicle at time k; This represents the estimated posterior heading angle of the vehicle at time k-1.

[0227] (5.2) Construct a dynamic threshold based on physical limits.

[0228] To distinguish between normal rapid vehicle displacement and position jumps caused by algorithm correction, this invention constructs a dynamic decision threshold based on the vehicle's kinematic parameters.

[0229] Position change judgment threshold Defined as:

[0230]

[0231] Heading jump threshold Defined as:

[0232]

[0233] in, This represents the location tolerance factor (typically between 1.2 and 1.5, used to accommodate measurement noise). This indicates the maximum design linear speed of the omnidirectional mobile delivery vehicle (e.g., 1.5 m / s). Indicates the duration of the system's control cycle (e.g., 0.02s). This indicates the baseline noise level of the positioning system (e.g., 0.02m).

[0234] Indicates the heading tolerance factor; This indicates the maximum design rotational angular velocity of the omnidirectional mobile delivery vehicle; This indicates the baseline noise level for heading measurements.

[0235] (5.3) Execution state transition logic decision.

[0236] Define a state transition flag based on the calculated increment and threshold. The decision logic function is as follows:

[0237]

[0238] in, This indicates the state transition detection flag (1 indicates a transition was detected, 0 indicates normal). This represents the positional differential increment at time k; This indicates the threshold for determining position transitions; This represents the differential increment of the heading at time k; This indicates the threshold for determining a course jump; Represents the logical OR operator. When When, it indicates that the vehicle is A physically impossible displacement or rotation occurred within a given time (e.g., a 0.5-meter lateral movement in 0.02 seconds). This can only be due to the high-weighted landmark observation information introduced by the extended Kalman filter in step 4, which forcibly corrects the accumulated drift. At this point, the system uses this signal as a trigger source to activate the dynamic trajectory replanning module in step 6. When the output value of the state transition detection flag Flag_jump is logic 1, this Boolean signal flows to the control logic layer as a trigger instruction, and the function jumps to the dynamic trajectory replanning module for execution, i.e., step 6 is based on the current posterior state. A program for calculating the trajectory of a third-order Bézier curve.

[0239] Step 6: When a state transition is detected, generate a smooth dynamic transition trajectory starting from the current posterior state.

[0240] The execution of this step is affected by the state transition flag output in step 5. Control. When At this time, the system no longer forces the vehicle to immediately return to the original global path. Instead, it uses a parametric spline curve algorithm to construct a virtual channel that smoothly transitions from the current corrected real position to the forward global path, serving as a temporary tracking target for the controller.

[0241] (6.1) Determine the boundary constraints.

[0242] To ensure that the generated trajectory seamlessly connects with the vehicle's current motion state at the starting point and smoothly merges with the global path at the ending point, the starting state of the transition curve needs to be defined. and endpoint state .

[0243] The starting state is set to the posterior state estimate at the current time step:

[0244]

[0245] in, The starting state vector representing the transition trajectory; The x and y coordinates represent the starting point of the trajectory; Indicates the tangent direction at the starting point of the trajectory (i.e., the vehicle's current heading); This represents the posterior true position and heading of the vehicle at time k.

[0246] The endpoint state is set to the projection point at a preset merging distance along the original global path:

[0247]

[0248] in, The final state vector representing the transition trajectory; The x and y coordinates represent the endpoint of the trajectory; Indicates the tangent direction at the end of the trajectory (i.e., the tangent of the global path at that point); Indicates the distance from the current projection point on the original global path. The status of the path point at the location; This indicates the preset ingress distance (e.g., 3.0 meters), and this parameter determines the speed of the regression process.

[0249] (6.2) Calculate the control points of the Bézier curve.

[0250] This invention uses a third-order Bézier curve to describe the transition trajectory, which consists of four control points. Uniquely determined. To satisfy positional continuity ( ) and tangential continuity ( To ensure the transition trajectory aligns with the vehicle's current heading at the starting point and smoothly merges with the original path at the ending point, the control points are calculated as follows:

[0251]

[0252] in, This indicates the first control point (i.e., the starting point of the trajectory); The x and y coordinates represent the starting point of the trajectory; This indicates the fourth control point (i.e., the end point of the trajectory); The x and y coordinates represent the endpoint of the trajectory; This indicates the second control point, used to constrain the tangent direction of the starting point; This represents the curvature control coefficient, typically taken as 1 / 3 of the merging distance. ; Indicates the direction of the tangent at the starting point of the trajectory; This indicates the third control point, used to constrain the tangent direction at the endpoint; Indicates the direction of the tangent at the end of the trajectory.

[0253] (6.3) Generate parameterized trajectory equations.

[0254] Based on the four control points mentioned above, a third-order Bézier curve equation with respect to the normalized time parameter t is constructed. :

[0255]

[0256] Where B(t) represents the point coordinate vector on the generated transition trajectory. ; t represents the normalized time parameter, which ranges from 0 to 1; The coordinates of the four control points calculated in step (2) are represented.

[0257] (6.4) Trajectory discretization and reference point generation.

[0258] To enable tracking by the digital controller, the continuous curve equation needs to be discretized into a sequence of reference points. The parameter t is sampled at equal intervals, and the coordinates and target heading of each sampling point are calculated.

[0259]

[0260] in, Represents the x and y coordinates of the i-th discrete trajectory point; The equation of the Bézier curve is expressed in The x and y coordinate components at the location; represents the target tangential angle (i.e., the desired heading) of the i-th discrete trajectory point; This represents the first derivative of the Bézier curve equation with respect to the parameter t, reflecting the tangent vector at that point.

[0261] Through this step, the system successfully constructed a smooth S-shaped transition curve. The curve's position and heading at the starting point are perfectly aligned with the vehicle's current state (with zero error), and it smoothly intersects the global path at the ending point. Subsequent controllers only need to track this curve to achieve oscillation-free, flexible regression.

[0262] Step 7: Obtain the predicted curvature information of the path ahead from the global map.

[0263] This step aims to establish a map-based environmental perception mechanism. During the dynamic transition trajectory or regression to the original global path generated in vehicle tracking step 6, the system searches for a target point ahead on the target path based on the vehicle's current real-time speed and position, and calculates the path curvature at that point. This curvature value reflects the geometric complexity of the road segment ahead and will serve as an important input variable for subsequent adaptive control.

[0264] (7.1) Determine the aiming distance and search aiming point.

[0265] To ensure the control system has sufficient response time to handle changes in the path ahead, a dynamic pre-aiming distance needs to be set. This distance is positively correlated with the vehicle's current speed.

[0266]

[0267] in, This represents the velocity gain coefficient, which physically indicates the predicted path length in the next few seconds. This indicates the current combined speed modulus of the vehicle (derived from the odometer data calculated in step 1). This indicates the minimum basic aiming distance to prevent aiming points from coinciding when stationary or at low speeds. Based on the calculated... Search for a pre-aiming point on the currently active target path (i.e., the transition spline curve generated in step 6 or the original global path). :

[0268]

[0269] in, This indicates the coordinates of the path preview point found. ; This represents a path indexing function used to return the coordinates of a path point at a specified arc length; This represents the projected arc length of the vehicle's current position on the path; Indicates the dynamic aiming distance.

[0270] (7.2) Calculate the curvature of the known path.

[0271] According to the pre-aiming point Calculate the geometric curvature at the point on the path of the given location (Bézier curve segment or global path segment). If the pre-aimed point lies on the third-order Bézier transition curve generated in step 6, the curvature is calculated using the derivative of the parameterized equation:

[0272]

[0273] in, Indicates the curvature of the predicted path (unit: The larger the value, the sharper the curve. Indicates the aiming point The corresponding normalized time parameter; This represents the first derivative of the Bézier curve equation with respect to the parameter t (i.e., the tangent vector components); This represents the second derivative of the Bézier curve equation with respect to the parameter t (i.e., the acceleration vector components). This indicates absolute value operations, where the focus is on the magnitude of curvature rather than the direction of bending.

[0274] (7.3) Low-pass filtering of curvature information.

[0275] To prevent frequent fluctuations in the calculated curvature value caused by path discretization noise, which could lead to abrupt changes in the control gain, the calculated... Perform first-order low-pass filtering:

[0276]

[0277] in, This represents the final predicted curvature after filtering, and this value will be directly input to the fuzzy controller in step 8; Indicates the curvature filter coefficients; This represents the original curvature calculated at the current moment; It represents the final predicted curvature at the previous moment.

[0278] This step allows the system to anticipate the geometric characteristics of the road ahead. When a vehicle is about to transition from a straight section to a curved section of a Bézier curve, or is about to merge into the overall path from a transition curve, Changes will inform the controller in advance to adjust the gain, thus achieving a "knowing before it turns" feedforward control effect.

[0279] Step 8: Input the confidence score and the predicted curvature into the fuzzy logic controller to calculate the adjustment amount of the PID control gain.

[0280] This step aims to establish a mapping relationship between perceived environmental quality and control response characteristics. The environmental characteristic confidence scores calculated in step 3 are then used. and the path prediction curvature obtained in step 7 As input variables, the system, based on the reliability of the current visual observation and the geometric complexity of the path ahead, outputs the proportional gain adjustment coefficient of the PID controller in real time by consulting a pre-set fuzzy inference rule table. and differential gain adjustment coefficient This allows for adaptive adjustment of control parameters.

[0281] (8.1) Define input variables and fuzzification process.

[0282] Define two input linguistic variables for the fuzzy controller: environmental feature confidence and predicted path curvature.

[0283] Input ①: Confidence score Directly obtain the normalized score generated in step 3. (Values ​​ranging from [0,1]) are used as input for perceived quality. Its fuzzy subset is defined as... .

[0284] when When the value is close to 0, it indicates severe distortion or loss of visual features, and the system should reduce its sensitivity to localization corrections. When the value is close to 1, it indicates that the visual observation is extremely reliable and the system can maintain the standard control gain.

[0285] Input ②: Predict curvature : Directly obtain the result calculated by map preview in step 7 As input for path complexity, its fuzzy subset is defined as... A greater curvature indicates that the vehicle is about to enter a sharp curve or is in a high-curvature section of the transition trajectory, requiring an increased response speed to reduce lag.

[0286] (8.2) The fuzzy controller matches the preset control logic according to the state of the input variables and outputs the proportional gain adjustment coefficient. and differential gain adjustment coefficient The core logic connections are shown in Table 1:

[0287] Table 1 Fuzzy Inference Rules

[0288]

[0289] Meaning 1: When observations are unreliable, the response to noise is reduced by decreasing the proportional gain, and the damping is increased by increasing the differential gain to suppress oscillations.

[0290] Meaning 2 Explanation: The observation is reliable and the route is straight, so the baseline parameters are maintained to ensure stable cruise.

[0291] Meaning 3: The observation is reliable and the curve is sharp. By increasing the proportional gain, the dynamic response speed can be improved and the tracking error can be reduced.

[0292] (8.3) Quantization mapping of membership function.

[0293] The membership function converts precise physical input quantities into the degree of membership of fuzzy sets. For the confidence score range of values ​​is and predicted curvature Quantization is performed using a triangular membership function, which has lower computational cost. Confidence scoring. Fuzzy subset Its membership function The mathematical expression can be defined as:

[0294]

[0295] in, For real-time input value; This is a preset threshold parameter used to define the low, medium, and high coverage ranges. Through this mapping, a specific numerical value... You may get and The quantification result, that is, the state simultaneously has The "low confidence" attribute and The "medium confidence level" attribute.

[0296] (8.4) After the fuzzy inference rule table (Table 1) is matched, the system will obtain multiple activated fuzzy subsets of output. These subsets need to be converted into precise coefficients that can be used for PID gain correction. and The centroid method is used for defuzzification.

[0297] The essence of this process is to find the geometric center of the total output region formed by all activated rules. Its mathematical continuous form is:

[0298]

[0299] In the discrete calculation of practical control systems, the numerator is weighted, the denominator is the total area, and the result is the coordinates of the equilibrium point of the geometric figure on the horizontal axis, i.e., the centroid. The formula simplifies to:

[0300]

[0301] in, The output variable (adjustment coefficient) is a discrete sampled value within its domain; For sampling points The total membership degree after merging according to the inference rules; For the final, precisely quantized value; Output membership function at sampling points The height of each sampling point is summed. Geometrically, this is equivalent to calculating the area of ​​the total output region formed by the superposition of multiple activated rules.

[0302] (8.5) Real-time update of PID control gain

[0303] Based on the adjustment coefficients output by the fuzzy controller, the preset reference gain of the system is corrected using a linear weighting formula, and the final PID gain parameters used in the current control cycle are calculated:

[0304]

[0305] in, This indicates the proportional gain ultimately used in this control cycle; This represents the system's preset reference proportional gain, which is obtained based on the vehicle dynamics model calibration. The scaling factor represents the weight of the proportional gain adjustment and is used to limit the range of gain adjustment. This represents the proportional gain adjustment coefficient output by the fuzzy controller, and its value range is typically (-1, 1).

[0306]

[0307] in, This indicates the differential gain ultimately used in this control cycle; This represents the system's preset reference differential gain; The scaling factor represents the weight for adjusting the differential gain; This represents the differential gain adjustment coefficient output by the fuzzy controller.

[0308] Through this step, the control system can dynamically and softly adjust the sensitivity of the controller according to the clarity of environmental features and the curvature of the path, ensuring that the vehicle can maintain optimal operating conditions in straight lines, curves, and interference conditions.

[0309] Step 9: Calculate the motor control command based on the dynamic transition trajectory using the adaptive PID algorithm, and output the final control signal.

[0310] This step uses the dynamic transition trajectory generated in step 6 as the tracking target, and uses the adaptive PID gain parameters calculated in step 8 to calculate the target speed of the vehicle body required to eliminate the tracking error. The speed command is decomposed into the target rotational speed of the four drive wheels through the Mecanum wheel inverse kinematics model to complete the control closed loop.

[0311] (9.1) Calculate the tracking error in the vehicle coordinate system.

[0312] Since the Mecanum wheel chassis performs omnidirectional motion in its own coordinate system, the position deviation projection in the global coordinate system must first be transformed to the vehicle coordinate system.

[0313] Reference point acquisition: Extract the coordinates of the reference point corresponding to the current moment from the dynamic transition spline curve generated in step 6. and target heading angle .

[0314] Coordinate transformation: using the posterior state estimation of the current vehicle (From step 4), calculate the lateral error in the vehicle coordinate system. and heading error :

[0315]

[0316] in, This indicates the lateral position error when transformed to the vehicle coordinate system (the lateral displacement deviation that needs to be eliminated). This represents the global x-coordinate of the current trajectory reference point (from the dynamic transition trajectory); This represents the vehicle's current true global x-coordinate; This indicates the vehicle's current true global heading angle; This represents the global ordinate of the current trajectory reference point; This represents the vehicle's current true global ordinate. Indicates the heading angle error; This indicates the target heading angle of the current trajectory reference point.

[0317] (9.2) Generate the vehicle target speed command.

[0318] Using the adaptive gain output from step 8, the lateral speed and rotational angular velocity that the vehicle body needs to execute are calculated through PD (proportional-derivative) control to eliminate the above-mentioned errors.

[0319]

[0320] in, Indicates the lateral movement speed of the vehicle target; This represents the final horizontal scaling gain output from step 8; This represents the lateral error at the current moment; This represents the final transverse differential gain output from step 8; This represents the lateral error of the previous control cycle; Indicates the system control cycle; This indicates the vehicle's angular velocity of rotation. Indicates the proportional gain of the heading control channel; This indicates the heading error at the current moment; Represents the differential gain of the heading control channel; This represents the heading error of the previous control cycle. If a confidence score is detected in step 8... If it is lower, then the one used here Smaller, even Larger (due to positioning jump), output It also avoids drastic changes, thus achieving a smooth response at the physical level.

[0321] (9.3) Mecanum wheel inverse kinematic distribution.

[0322] The calculated vehicle speed (including longitudinal planning speed) Lateral speed control Rotation control speed It can be broken down into the rotational speeds of the four wheels.

[0323]

[0324] in, These represent the target rotational speeds (in rad / s) of the left front wheel, right front wheel, left rear wheel, and right rear wheel, respectively; R represents the radius of the Mecanum wheel; a is the longitudinal distance from the vehicle's geometric center to the axle; b is the lateral distance from the vehicle's geometric center to the axle. This represents the longitudinal cruising speed given by the global path planning layer; This indicates the lateral correction speed calculated in this step; This indicates the heading correction angular velocity calculated in this step.

[0325] Ultimately, the system sends these four speed commands to the underlying motor driver, driving the vehicle to perform smooth physical movements and completing a flexible response to positioning changes.

[0326] The technical solutions provided by the embodiments of the present invention have been described in detail above. Specific examples have been used in this patent to illustrate the principles and implementation methods of the embodiments of the present invention. Unless otherwise specified, the above embodiments and features described herein can be combined with each other. The above descriptions are merely preferred embodiments of the present invention and are not intended to limit the present invention. 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 mobile robot control method based on multi-source fusion dynamic trajectory replanning, characterized in that, Includes the following steps: (1) Use multi-source sensors to acquire vehicle kinematic data and environmental feature observation data in real time, and perform time synchronization and preprocessing on the data; (2) Based on the kinematic model, state prediction is performed to obtain the prior state estimate of the vehicle; (3) Multidimensional feature extraction and confidence assessment are performed on the observed environmental features to obtain the observation confidence score; (4) Construct an adaptive measurement noise matrix based on confidence scores, and use extended Kalman filtering for measurement updates to obtain the posterior state estimate; (5) Perform time-domain differential analysis on the posterior state to detect whether there is a location state jump; if the state difference between the previous and next times exceeds the preset safety threshold, it is determined that a location jump has occurred. (6) When a state transition is detected, a smooth dynamic transition trajectory is generated starting from the current posterior state; (7) Obtain the predicted curvature information of the path ahead from the global map; (8) Input the confidence score and the predicted curvature into the fuzzy logic controller and calculate the adjustment amount of the PID control gain; (9) Calculate the motor control command based on the dynamic transition trajectory using the adaptive PID algorithm and output the final control signal.

2. The mobile robot control method based on multi-source fusion dynamic trajectory replanning according to claim 1, characterized in that, The implementation process of step (2) is as follows: The nonlinear state prediction model is constructed as follows: ； in, Indicates the first The prior state estimate of the vehicle at time 1; Indicates the first The posterior state estimate of the vehicle at time 1; Indicates the first The system control input vector at any given time is determined by the vehicle's longitudinal velocity. lateral velocity and rotational angular velocity composition; This represents the coordinate rotation matrix, which physically transforms the velocity vector in the vehicle coordinate system to the global coordinate system. Expanding the matrix yields the prior states. Scalar calculation formula: ； in, This represents the prior state estimate at time k (before visual observations are fused). This is an estimate of the posterior state from the previous time step; Indicates the first The heading angle of the vehicle in the global coordinate system at any given time is used to calculate the trigonometric function components of the rotation matrix; This represents the time interval between two adjacent moments; State transition function Regarding the state vector Find the partial derivatives and calculate the Jacobian matrix. : ； Among them, the position is related to the heading angle The partial derivative terms are calculated as follows: ； Substituting the partial derivative terms, we obtain the final linearized Jacobian matrix used for covariance propagation. : ； Explicit introduction The related term, namely the speed of lateral movement of the vehicle body, describes the coupled effect of the lateral motion unique to the Mecanum wheel chassis on the uncertainty of position prediction.

3. The mobile robot control method based on multi-source fusion dynamic trajectory replanning according to claim 1, characterized in that, The implementation process of step (3) is as follows: Edge detection and Hough transform are performed on the preprocessed image within the region of interest to extract several line segment sets. The discrete line segments are then fitted to the equation of the environmental feature line using the least squares method. Based on the fitted linear equation, the geometric relationship between the vehicle center and the recognition features is calculated, and an observation vector is constructed. ; Define feature continuity index Geometric consistency index Two independent quality assessment dimensions are used, and the indicators of the two dimensions are weighted and fused to generate the final observation confidence score. : ； ； ； ； in, This is a line width correction factor. It is applied when the road surface is covered with oil, worn, or excessive sunlight causes the lines to break. Significant reduction, leading to The value decreased; For the fit variance, The variance is the sensitivity attenuation coefficient, which is used when complex textures on the ground cause the extracted edge points to be disordered. Enlargement, leading to It rapidly approaches 0; and Let be the weighting coefficient, satisfying .

4. The mobile robot control method based on multi-source fusion dynamic trajectory replanning according to claim 1, characterized in that, The implementation process of step (4) is as follows: The measurement noise covariance matrix R is designed as a confidence score. function The adaptive measurement noise matrix is ​​defined as follows: ； in, Based on confidence score The adaptive measurement noise covariance matrix at time k is the variable. This represents the confidence score for environmental characteristics; Indicates taking and The larger value in the fraction is used to ensure that the denominator is always a valid positive number; Let be the reference noise covariance matrix, representing the noise under ideal observation conditions. The inherent physical noise of the lower sensor; Represents the lateral distance observations in the baseline matrix The noise variance; Represents the heading angle observations in the reference matrix. The noise variance; Use a very small positive number to prevent the denominator from being zero. when When (observation is clear), The filter assumes minimal observation noise and tends to accept the observed values; when When (observation is blurred), The filter considers the observation noise to be extremely high and almost completely ignores the observation values, thereby avoiding erroneous features from causing the localization to be biased. Calculate the observation Jacobian matrix and Kalman gain : ； in, Let represent the optimal Kalman gain matrix at time k. This weighting coefficient matrix is ​​used to determine the extent to which the system trusts the current observation information. Let represent the prior error covariance matrix at time k; Let represent the observation Jacobian matrix at time k; This represents the transpose of the observation Jacobian matrix; This indicates the confidence score at time k. Dynamically calculated adaptive measurement noise covariance matrix; The prior state is corrected using Kalman gain to obtain the posterior state estimate. : ； Update the posterior error covariance matrix Used for iteration in the next time step: ； in, Let represent the posterior state estimate at time k; This represents the prior state estimate at time k; This represents the measurement residual at time k, i.e., the actual observation vector. Compared with theoretically predicted observation vectors The difference between them; Let represent the posterior error covariance matrix at time k; I represents the identity matrix with the same dimension as the state vector.

5. The mobile robot control method based on multi-source fusion dynamic trajectory replanning according to claim 1, characterized in that, The implementation process of step (5) is as follows: The posterior state estimation sequence is monitored in real time. By calculating the rate of change of vehicle position and heading within adjacent control cycles and combining this with the vehicle's own physical motion limit constraints, non-physical state abrupt changes caused by discrete landmark corrections are identified. Specifically, this includes the following state differential analysis and threshold decision process: ； ； in, This represents the differential increment of the vehicle's position at time k; This represents the estimated posterior x-coordinate of the vehicle at time k. This represents the estimated posterior ordinate of the vehicle at time k. This represents the estimated posterior x-coordinate of the vehicle at time k-1. This represents the estimated posterior ordinate of the vehicle at time k-1. This represents the differential increment of the vehicle's heading at time k; Let represent the estimated posterior heading angle of the vehicle at time k; This represents the estimated posterior heading angle of the vehicle at time k-1. Construct dynamic thresholds based on physical limits, including position change judgment thresholds. and heading jump determination threshold : ； ； in, This indicates the location tolerance coefficient; This indicates the design maximum linear speed of the omnidirectional mobile delivery vehicle; Indicates the duration of the system's control cycle; This indicates the baseline noise level of the positioning system; Indicates the heading tolerance factor; This indicates the maximum design rotational angular velocity of the omnidirectional mobile delivery vehicle; Indicates the baseline noise level for heading measurements; Define a state transition flag based on the calculated increment and threshold. The decision logic function is as follows: ； in, This indicates a state transition detection flag; 1 indicates a transition has been detected, and 0 indicates normal operation. This represents the differential increment of the position at time k; This indicates the threshold for determining position transitions; This represents the differential increment of the heading at time k; This indicates the threshold for determining a course jump; Represents the logical OR operator.

6. The mobile robot control method based on multi-source fusion dynamic trajectory replanning according to claim 1, characterized in that, The implementation process of step (6) is as follows: Obtain the corrected true position and heading of the vehicle at the current moment, and use them as the starting point and tangent of the trajectory; Search for a point at a preset distance ahead along the original global path, and use it as the trajectory endpoint and the endpoint tangent; Using the third-order Bézier curve polynomial equation, a smooth parametric curve connecting the two points is calculated based on the start point, end point, and their tangential constraints. To satisfy positional continuity and tangential continuity, and to ensure that the transition trajectory is consistent with the vehicle's current heading at the start point and smoothly tangent to the original path at the end point, the control points are calculated as follows: ； in, This indicates the first control point, i.e., the starting point of the trajectory; The x and y coordinates represent the starting point of the trajectory; This indicates the fourth control point, which is the endpoint of the trajectory; The x and y coordinates represent the endpoint of the trajectory; This indicates the second control point, used to constrain the tangent direction of the starting point; Indicates the curvature control coefficient; Indicates the direction of the tangent at the starting point of the trajectory; This indicates the third control point, used to constrain the tangent direction at the endpoint; Indicates the direction of the tangent at the endpoint of the trajectory; Based on four control points, a third-order Bézier curve equation with respect to the normalized time parameter t is constructed. : ； Where B(t) represents the point coordinate vector on the generated transition trajectory. ; t represents the normalized time parameter; Perform equal-interval sampling on parameter t, and calculate the coordinates and target heading for each sampling point: ； in, Represents the x and y coordinates of the i-th discrete trajectory point; The equation of the Bézier curve is shown in The x and y coordinate components at the location; The target tangential angle, i.e. the desired heading, is represented by the i-th discrete trajectory point. This represents the first derivative of the Bézier curve equation with respect to the parameter t, reflecting the tangent vector at that point.

7. The mobile robot control method based on multi-source fusion dynamic trajectory replanning according to claim 1, characterized in that, The implementation process of step (7) is as follows: Determine the aiming distance With search target point : ； ； in, This represents the velocity gain coefficient, predicting the path several seconds into the future. This indicates the current combined speed modulus of the vehicle (; This indicates the minimum basic aiming distance to prevent aiming points from coinciding when stationary or at low speed. This represents a path indexing function used to return the coordinates of a path point at a specified arc length; This represents the projected arc length of the vehicle's current position on the path; According to the pre-aiming point Calculate the geometric curvature at the point corresponding to the path type. : ； in, This indicates the predicted path curvature; the larger the value, the sharper the curve. Indicates the aiming point The corresponding normalized time parameter; This represents the first derivative of the x and y coordinates of the Bézier curve equation with respect to the parameter t; This represents the second derivative of the Bézier curve equation with respect to the parameter t; This indicates absolute value operations, where the magnitude of curvature is considered rather than the direction of bending. For the calculated Perform first-order low-pass filtering: ； in, This represents the final predicted curvature after filtering; Indicates the curvature filter coefficients; This represents the original curvature calculated at the current moment; It represents the final predicted curvature at the previous moment.

8. The mobile robot control method based on multi-source fusion dynamic trajectory replanning according to claim 1, characterized in that, The implementation process of step (8) is as follows: Define the fuzzy subsets and membership functions of the input variables confidence score and predicted curvature: Confidence Score As input to perceived quality, its fuzzy subset is defined as ;when When the value is close to 0, it indicates severe distortion or loss of visual features, and the sensitivity to localization correction should be reduced; when... A value close to 1 indicates that visual observation is extremely reliable and can maintain standard control gain; predictable curvature. As input to path complexity, its fuzzy subset is defined as ; The greater the curvature, the more likely the vehicle is to enter a sharp curve or be in a high-curvature section of the transition trajectory, requiring an increased response speed to reduce lag. Establish a fuzzy inference rule base and set the control logic; output the proportional gain adjustment coefficient. and differential gain adjustment coefficient ; The centroid method is used to defuzzify the inference results and output the real-time adjustment values ​​of the proportional, integral, and derivative gains; thus achieving adaptive adjustment of the control parameters.

9. The mobile robot control method based on multi-source fusion dynamic trajectory replanning according to claim 8, characterized in that, The process of using the centroid method to defuzzify the reasoning results is as follows: The unfuzzy subset is the geometric center of the area enclosed by the membership functions output by the fuzzy subset, and its mathematical continuous form is: ； In the discrete calculation of practical control systems, the numerator is weighted, the denominator is the total area, and the result is the coordinates of the equilibrium point of the geometric figure on the horizontal axis, i.e., the centroid. The formula simplifies to: ； in, The output variable is the discrete sampled value within its domain; For sampling points The total membership degree after merging according to the inference rules; For the final, precisely quantized value; Output membership function at sampling points The height of the sampling point is calculated by summing the heights of all sampling points, which is geometrically equivalent to calculating the area of ​​the total output region formed by the superposition of multiple activated rules.

10. A mobile robot control system based on multi-source fusion dynamic trajectory replanning using the method described in any one of claims 1 to 9, characterized in that, It includes a data acquisition and preprocessing module, an environmental feature confidence assessment module, a weighted fusion positioning module, a dynamic trajectory replanning module, and a map prediction and adaptive control module; among which: The data acquisition and preprocessing module uses multi-source sensors mounted on the vehicle to acquire vehicle kinematic data and environmental feature observation data in real time, and completes data time synchronization and noise reduction processing. The environmental feature confidence assessment module extracts ground guide lines or intersection landmark features from preprocessed sensing data, calculates observation confidence scores based on feature clarity, geometric continuity, and fitting residuals, and quantifies the reliability of current environmental observations. The weighted fusion positioning module is used to introduce the confidence score as a weight adjustment factor into the extended Kalman filter algorithm, dynamically adjust the measurement noise covariance matrix, fuse dead reckoning data and environmental observation data, and output a smooth and high-precision global state estimate of the vehicle. The dynamic trajectory replanning module monitors the rate of change of the positioning status in real time. When it detects a non-physical state jump caused by the correction of discrete landmarks, it generates a smooth transition spline curve connecting the current position and the global path, starting from the corrected current real position. The map prediction and adaptive control module uses the generated transition curve as a temporary tracking target. It combines the predicted curvature information extracted from the global map with the current confidence score, uses fuzzy logic to adjust the PID control gain in real time, calculates the motor control command, and outputs the result.

Images