A method for multi-sensor fusion positioning and correction of a trolley

Abstract

This application discloses a multi-sensor fusion positioning and correction method for a trolley. The method includes: acquiring multi-source sensor data; extracting the absolute value of the difference between longitudinal velocity and wheel-end linear velocity as a slip observation feature value; in the error state Kalman filter model, exponentially expanding the measurement noise covariance matrix of the wheel encoder when the feature value is not less than a threshold, and outputting a corrected pose; inputting the corrected pose and feature value into a model predictive controller, dynamically increasing the control increment penalty weight, solving for the optimal correction torque command sequence, and executing it. This application can actively isolate false mileage data generated by slippage, ensuring absolute positioning accuracy, and actively softening the correction action to avoid dynamic oscillations in the system.

Description

Technical Field

[0001] This application relates to the fields of automatic control and mobile robot navigation technology, specifically a multi-sensor fusion positioning and correction method for a trolley, which is particularly suitable for sliding adaptive multi-sensor fusion control in heavy-load transportation scenarios with high inertia, and meets the classification requirements of the International Patent Classification. Background Technology

[0002] In heavy manufacturing scenarios such as aerospace component assembly and large mold transfer, omnidirectional or differential speed transfer trolleys are widely used for automated material handling. These high-load operation scenarios place stringent requirements on the trajectory tracking accuracy and smooth operation of the trolleys.

[0003] Currently, traditional trolley navigation control systems mostly employ a separate architecture. The upper-level computing system typically uses LiDAR or visual odometers combined with wheel encoders for basic position calculation, while the lower-level control system uses proportional-integral-derivative (PID) algorithms for path tracking and feedback correction. In heavy-duty workshop environments, this conventional technology has significant operational flaws. Firstly, workshops are filled with complex dynamic obstacles and often have oil stains on the ground. Sudden changes in adhesion during trolley start-up or correction maneuvers can easily lead to localized wheel slippage. This slippage causes the wheel encoders to output incorrect mileage data, contaminating the entire multi-sensor fusion positioning model and causing the system's positioning coordinates to diverge and drift. Secondly, heavy-duty trolleys have high loads and large moments of inertia. When the positioning system detects a trajectory deviation and issues a correction command, traditional feedback control algorithms typically output a large compensation torque instantaneously. This abrupt torque change, when ground adhesion is insufficient, further exacerbates wheel slippage, causing the vehicle to oscillate low-frequency on both sides of the target path. In severe cases, this can lead to sideslip and loss of control, posing a significant safety hazard. Summary of the Invention

[0004] This application provides a multi-sensor fusion positioning and correction method for trolleys, which solves the positioning distortion and correction oscillation defects caused by wheel slippage in existing heavy-duty trolleys in complex ground environments.

[0005] This application provides a multi-sensor fusion positioning and correction method for a trolley, including: real-time acquisition of data from an inertial measurement unit (IMU), a lidar sensor, and a wheel encoder; extraction of the longitudinal linear acceleration output by the IMU within the current period; and integration of the longitudinal linear acceleration to obtain the longitudinal velocity V. imu The wheel end linear velocity V is calculated by combining the angular velocity output by the wheel encoder. wheel With the longitudinal velocity V imu With the linear velocity V at the wheel end wheel The absolute value of the difference is used as the slip observation characteristic value S slipIn the error state Kalman filter model of the fusion positioning thread, when the slip observation feature value S slip Greater than or equal to the preset slip dead zone threshold S th At that time, the measurement noise covariance matrix R of the wheel encoder odom Perform exponential expansion and output the corrected trolley pose information; combine the corrected trolley pose information with the sliding observation feature value S. slip The input is given to the model prediction controller, and in the cost function of the model prediction controller, the slip observation feature value S is calculated. slip The control increment penalty weight W is dynamically increased to obtain the optimal correction torque command sequence, and the optimal correction torque command sequence is sent to the drive motor for execution.

[0006] In a preferred embodiment, the inertial measurement unit is rigidly mounted at the geometric center of the trolley chassis, and the wheel encoder is coaxially mounted at the drive wheel axle end of the trolley. The real-time acquisition of data from the inertial measurement unit, the lidar, and the wheel encoder includes: acquiring the triaxial acceleration and triaxial angular velocity of the inertial measurement unit; acquiring the pulse signal from the wheel encoder and converting the pulse signal into the current angular velocity of the drive wheel; receiving the point cloud data output by the lidar and performing distortion correction processing on the point cloud data.

[0007] In a preferred embodiment, the longitudinal linear acceleration output by the inertial measurement unit within the current period is extracted, and the longitudinal linear acceleration is integrated to obtain the longitudinal velocity V. imu The process includes: calculating the attitude update matrix for the current moment using the Runge-Kutta method based on the three-axis angular velocities output by the inertial measurement unit and the attitude matrix from the previous moment; converting the three-axis accelerations to the global navigation coordinate system using the attitude update matrix and subtracting the calibrated gravity acceleration vector; converting the true acceleration vector after removing the gravity acceleration vector back to the vehicle coordinate system and extracting the corresponding longitudinal component as the longitudinal linear acceleration; and performing discrete-time integration on the longitudinal linear acceleration based on a preset sampling period span to obtain the longitudinal velocity V at the current moment. imu .

[0008] In a preferred embodiment, the measurement noise covariance matrix R of the wheel encoder odom Performing exponential expansion includes: calculating the slip observation eigenvalue S slip With the slip dead zone threshold S th The difference is calculated; the difference is multiplied by a preset sensitivity coefficient k to obtain the expansion exponent; the exponential amplification factor corresponding to the expansion exponent is calculated using the natural constant as the base; the measurement noise covariance matrix R is then calculated.odom Multiplying by the exponential magnification factor yields the expanded target covariance matrix R'. odom and the target covariance matrix R' odom Substitute the values ​​into the error state Kalman filter model for measurement update calculation.

[0009] In a preferred embodiment, the error state Kalman filter model performs system propagation calculations using a continuous-time state equation, wherein the state variables of the continuous-time state equation include pose error, velocity error, and sensor bias error; during the measurement update phase, the scanning matching pose output by the lidar is combined with the target covariance matrix R'. odom Calculate the Kalman gain; use the Kalman gain to correct the error of the prior state obtained by the propagation operation of the system, and inject the corrected state variables into the nominal state, and output the corrected trolley pose information; reset the pose error and the velocity error in the state variables to zero.

[0010] In a preferred embodiment, the cost function of the model prediction controller is based on the slip observation feature value S. slip Dynamically increasing the control increment penalty weight W includes: determining the slip observation feature value S. slip Is it greater than the slip dead zone threshold S? th ; in the slip observation characteristic value S slip Greater than the slip dead zone threshold S th In the case of the slip observation feature value S slip With the slip dead zone threshold S th The difference multiplied by the preset penalty ratio coefficient And add it to the basic weight parameter W0 to obtain the reconstructed control increment penalty weight W; in the slip observation feature value S slip Less than or equal to the slip dead zone threshold S th In the case of [condition], the basic weight parameter W0 is maintained as the control increment penalty weight W.

[0011] In a preferred embodiment, the cost function includes a path deviation penalty term and a control increment penalty term; the path deviation penalty term is configured as a weighted sum of the squared errors between the desired pose and the predicted pose in each prediction time domain; the control increment penalty term is configured as a weighted sum of the squared changes in output torque in adjacent control cycles in each prediction time domain, and the corresponding weight coefficient adopts the control increment penalty weight W; the cost function is minimized by a quadratic programming solver, and the first control increment in the obtained control sequence is extracted and added to the output torque of the previous cycle to obtain the optimal correction torque command sequence for the current cycle.

[0012] In a preferred embodiment, after the optimal correction torque command sequence is issued to the drive motor for execution, the method further includes: continuously monitoring the slip observation characteristic value S. slip Within a preset time window, when the sliding observation feature value S... slip The slip dead zone threshold S remains continuously below the threshold value. th At that time, the measurement noise covariance matrix R of the wheel encoder is gradually recovered according to the preset attenuation rate. odom Return to the initial matrix state, and gradually restore the control increment penalty weight W to the basic weight parameter state.

[0013] This application also provides a control device, including a memory and a processor, wherein the memory stores a computer program, and the computer program, when executed by the processor, implements the above-described trolley multi-sensor fusion positioning and correction method.

[0014] This application also provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the above-described trolley multi-sensor fusion positioning and correction method.

[0015] This application extracts slip observation features of vehicle motion from the underlying filtered positioning model and dynamically adjusts the observation weight matrix of the encoder data in real time based on these features. This actively isolates false mileage data caused by slippage, ensuring absolute positioning accuracy on complex terrain. The slip features are then incorporated into the control layer for dynamic reconstruction of the cost function of the model predictive controller. This allows the trolley to adaptively increase torque change penalties when slippage is detected, actively softening correction actions. From a physics perspective, this avoids overshoot and low-frequency oscillations at the dynamic level, achieving deep coupling and interlocking between navigation status and chassis dynamics control, thus improving the driving safety of heavy-duty trolleys. Attached Figure Description

[0016] Figure 1 This is a schematic diagram of the overall physical and logical architecture of the trolley provided in an embodiment of the present invention.

[0017] Figure 2 This is a flowchart illustrating the trolley multi-sensor fusion positioning and correction method provided in an embodiment of the present invention.

[0018] Figure 3 This is a structural block diagram of the control device provided in an embodiment of the present invention. Detailed Implementation

[0019] To make the objectives, technical solutions, and advantages of this application clearer, the following detailed description is provided in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the scope of this application.

[0020] Please see Figure 1 This demonstrates the overall hardware configuration and software resource allocation architecture of the trolley's multi-sensor fusion positioning and correction system. In this physical architecture, the trolley's electronic control system revolves around a main control computer 101. The main control computer 101 employs a high-performance industrial-grade control host equipped with a real-time operating system, and its internal high-frequency logic processing unit meets the hard real-time requirements of the control loop. To ensure the stability of the entire onboard electronic system during high-power correction operations, a power management module 102 is provided. This power management module 102 includes a high-rate power battery unit and a DC-DC buck converter, used to supply a stable low-voltage operating voltage to the main control computer 101 and various sensors, while simultaneously supplying high-voltage power to the high-power drive components. By introducing the power management module 102, the back electromotive force during motor start-up and shutdown can be effectively prevented from interfering with the sensitive computing unit, ensuring the purity of the sensed data.

[0021] The main control computer 101 is peripherally equipped with various environmental and vehicle status perception hardware. The environmental perception module mainly includes two two-dimensional LiDARs 103 installed diagonally on the trolley. The diagonal arrangement reduces the obstruction of the radar field of view by the trolley structure, thereby providing a complete environmental point cloud profile. The chassis motion state measurement module includes a six-axis inertial measurement unit 104 and incremental photoelectric encoders 105. The six-axis inertial measurement unit 104 is rigidly fixed at the geometric center of the trolley chassis. This installation position can eliminate the centripetal acceleration lever effect error caused by different turning radii. Multiple incremental photoelectric encoders 105 are coaxially mounted on the axle ends of each drive wheel of the trolley to collect direct mechanical rotation signals from the wheel ends. The trolley's actuators include multiple servo motors distributed on each wheel and their matching servo drivers 106.

[0022] In terms of communication interfaces and data flow, the system employs frequency-division heterogeneous transmission logic. Angular velocity and linear acceleration data output by the six-axis inertial measurement unit 104, and pulse count data output by the incremental photoelectric encoder 105, are fed into the main control computer 101 via the controller local area network bus at a high frequency of 100Hz to 200Hz. Two-dimensional point cloud data containing a large number of spatial coordinate points generated by the two-dimensional lidar 103 is transmitted to the main control computer 101 via a high-bandwidth, high-throughput Ethernet port at a relatively low frequency of 10Hz to 20Hz. The memory space of the main control computer 101 is divided into a fusion positioning thread and a motion control thread at the operational level. The fusion positioning thread is responsible for consuming multi-source sensor data and generating high-precision pose data packets with slip indicators. This data packet is written into a shared memory mechanism and then read with low latency by the high-priority motion control thread. The motion control thread generates torque commands based on the acquired pose and features and sends them to the servo driver 106 via a highly real-time industrial bus to execute the final action. It should be noted that this architecture is not limited to omnidirectional or differential trolleys, and those skilled in the art have also applied it to heavy-duty vehicles with Ackerman steering mechanisms.

[0023] Please see Figure 2 , Figure 2 The process flow of the multi-sensor fusion positioning and correction method for the trolley, specifically executed within the main control computer 101, is demonstrated. This method utilizes the aforementioned hardware to acquire environmental conditions and completes closed-loop control at the software algorithm level, including slip quantization, chassis state isolation, and flexible anti-vibration correction.

[0024] After system startup, step S201 is entered, where the main control computer 101 acquires and preprocesses multi-sensor data in real time. In this step, the main control computer 101 acquires the three-axis acceleration and three-axis angular velocity of the six-axis inertial measurement unit 104 through a hardware interface, and obtains the pulse signal from the incremental photoelectric encoder 105. Since the underlying hardware output is only the accumulated value of electrical pulses, the main control computer 101, according to the system clock, converts the pulse signal with respect to time into the physical angular velocity data of the current drive wheel. The main control computer 101 receives point cloud data output by the two-dimensional lidar 103. Due to the mechanical scanning cycle between lidar laser emission and reception, motion distortion occurs in the point cloud when the trolley moves at high speed. Based on the inertial navigation data acquired simultaneously, the main control computer 101 uses timestamp interpolation to correct the motion distortion of the point cloud data, and executes basic initial point cloud denoising and feature extraction algorithm modules to extract effective spatial geometric features for matching.

[0025] After completing the basic data acquisition, the process proceeds to step S202, where the main control computer 101 performs quantization extraction of the slip state observation features. Specifically, the system extracts the longitudinal linear acceleration output by the inertial measurement unit (IMU) within the current cycle and performs integration. Since the six-axis IMU 104's measurement coordinate axes do not always coincide with the geographic horizontal plane as the vehicle pitches or tilts, the main control computer 101 uses the three-axis angular velocities output by the six-axis IMU 104 and the attitude matrix from the previous moment to solve the differential equations using the fourth-order Runge-Kutta method, resulting in a high-precision attitude update matrix for the current moment. The measured three-axis acceleration is then transformed from the body coordinate system to the global navigation coordinate system using this attitude update matrix. In the global navigation coordinate system, the pre-calibrated local gravity acceleration vector is subtracted from this acceleration vector. The true pure motion acceleration vector, after removing the gravity acceleration vector, is then inversely transformed back to the vehicle coordinate system, from which the corresponding longitudinal component is accurately extracted as the true longitudinal linear acceleration of the vehicle. Based on a preset high-frequency sampling period span, the main control computer 101 performs discrete-time integral calculations on the longitudinal linear acceleration to obtain the vehicle's true longitudinal velocity V, which is independent of the ground friction environment. imu .

[0026] The main control computer 101 acquires the wheel-end speeds of the chassis. Specifically, based on wheel system kinematics, the main control computer 101 calculates the wheel-end linear speeds according to the drive wheel angular velocities obtained from the encoder. In a preferred embodiment, this calculation logic is implemented using the following formula:

[0027]

[0028] Among them, variable V wheel The variable represents the linear velocity of the driving wheel on its outer circumference (unit: meters per second). The parameter R represents the wheel rotational angular velocity (in radians per second) measured and converted by the incremental photoelectric encoder 105, and the parameter R represents the factory-calibrated physical rolling radius of the wheel (in meters).

[0029] The longitudinal velocity V, representing the actual spatial displacement velocity of the vehicle body, was obtained separately. imu The wheel-end linear velocity V represents the state of contact between the vehicle body and the ground. wheel Then, the main control computer 101 performs synchronous comparison and calculation on the two sets of values. The system uses the following formula to extract the slip characteristics:

[0030]

[0031] Wherein, variable S slip This represents the characteristic value observed during slippage. Due to the actual longitudinal velocity V of the vehicle body... imuThe high-frequency integral derived from the internal inertial elements is unaffected by external slippage in the short term; while the wheel-end linear velocity V wheel It relies on the static friction contact between the wheel and the ground. When the wheel spins or locks up and slips, a significant deviation occurs between the two. Therefore, the slip observation characteristic value S is constructed by calculating the absolute value of the difference between the two. slip It can physically map with high sensitivity the severity of the current trolley chassis losing ground adhesion.

[0032] The method proceeds to step S203, where the fusion positioning thread, based on the error state Kalman filter model, adjusts the covariance weights and corrects the pose according to the slip observation feature values. In traditional filtering models, the sensor's confidence level is often statically fixed. This embodiment overcomes this limitation. The main control computer 101 has a preset physical critical parameter, namely the slip dead zone threshold S. th This parameter characterizes the allowable elastic slip range when the trolley is rolling normally on the ground. In this embodiment, S is calibrated based on the physical friction characteristics of the trolley's rubber tires and a typical epoxy resin floor. th The specific value is 0.15 m / s.

[0033] The main control computer 101 compares and calculates the slip observation characteristic value S in real time. slip With the slip dead zone threshold S th When detected At this point, the trolley is determined to be in a normal, steady-state rolling state. At this time, in the error state Kalman filter observation equation, the measurement noise covariance matrix R of the incremental photoelectric encoder 105... odom The factory-calibrated diagonal matrix constants remain unchanged.

[0034] When detected At this point, the main control computer 101 determines that the drive wheel has slipped significantly beyond the static friction circle boundary. The odometer output will then show a high level of false increments. Therefore, the main control computer 101 performs real-time exponential expansion calculations on the measurement noise covariance matrix of the wheel encoder. In a preferred embodiment, this expansion adjustment logic is implemented using the following exponential function formula:

[0035]

[0036] Wherein, symbol R' odom R represents the expanded target covariance matrix. odom The variable S represents the original measurement noise covariance matrix of the wheel encoder. slip Represents the currently calculated slip observation characteristic value; variable S thThis represents the calibrated slip dead zone threshold (0.15 m / s); parameter k is a preset weight attenuation gain parameter used to adjust the steepness of the dilation function; in this embodiment, k is set to 10; function This represents an exponential operation with the natural constant e as the base. Through this exponential expansion mathematical operation, the measurement noise variance of the encoder dimension can be increased non-linearly once the slippage severity exceeds the dead zone threshold. This means that in the underlying multi-data fusion mathematical calculations, the main control computer 101 significantly reduces the confidence weight of the wheel odometer within the system at the moment of slippage.

[0037] The expanded matrix directly participates in the filter update. The error-state Kalman filter model performs system propagation prior calculations through a continuous-time state equation. The state variables of this continuous-time state equation mainly include the system's pose error, velocity error, and the zero-bias errors of each sensor. During the measurement update phase, the system receives the absolute matching pose calculated by the two-dimensional lidar 103 using a scanning matching algorithm and modifies it with the expanded target covariance matrix R'. odom The Kalman gain matrix is ​​calculated by combining the matrix parameters. The calculated Kalman gain is then used to correct the prior error state obtained from the system propagation operation. The corrected error state is injected into the accumulated nominal state using multiplicative or additive rules, thereby outputting the accurately corrected vehicle pose information. At the end of each iteration cycle, the pose error and velocity error in the state variables are forcibly reset to zero to prevent error accumulation and divergence. By employing dynamic expansion features, false wheel-end displacement data can be effectively avoided from distorting the true radar observation position, ensuring that the positioning coordinate system does not drift in complex ground environments.

[0038] To enable the correction action to flexibly adapt to ground conditions, the method proceeds to step S204, where the system collaboratively reconstructs and solves the control logic and slip characteristics in the model predictive controller. The motion control thread reads the corrected trolley pose information and the slip observation feature value S through shared memory. slip This model is then incorporated into the model predictive control algorithm framework. The main control computer 101 contains a pre-set kinematic and dynamic nominal matrix model of the trolley. At the start of each control cycle, the algorithm recursively predicts the trajectory sequence of the trolley over the next N time steps using state-space equations, based on the pre-set desired reference path and the currently acquired corrected pose. In this step, the prediction time domain N is preferably set to 20 in this embodiment.

[0039] The model predictive controller needs to inversely deduce the optimal output for the current period by solving for the minimum value of a specific cost function. This scheme performs an anti-oscillation reconstruction of the traditional cost function based on slip characteristics. The main control computer 101 calculates the slip observation feature value S based on this. slipThe control increment penalty weight W is dynamically increased. The specific reconstruction mechanism is as follows: the main control computer 101 determines S. slip Is it greater than the slip dead zone threshold S? th If the threshold is not exceeded, the control increment penalty weight W remains equal to the pre-calibrated base weight parameter W0. If the threshold is exceeded, the cost function is reconstructed based on the linear piecewise mapping relationship. In a preferred embodiment, this dynamic mapping calculation logic is implemented through the following formula:

[0040]

[0041] Wherein, variable W represents the control increment penalty weight applied to the current control cycle after reconstruction; parameter W0 represents the basic control increment penalty coefficient, used to ensure the system responsiveness in the non-slip state; parameter The sensitivity factor is set; function For the operation of finding the maximum value; variable S slip With S th The definition is as described above.

[0042] With the dynamic adjustment of weights, the motion control thread constructs a comprehensive cost function that includes path deviation penalties and control increment penalties. In a preferred embodiment, this reconstructed cost function used for optimization is implemented using the following formula:

[0043]

[0044] Wherein, variable J represents the total cost function value that needs to be minimized within the current prediction period; parameter N represents the prediction time domain (with a value of 20); variable i is the time step number; variable e y,i This represents the lateral distance deviation between the predicted trajectory of the trolley and the set reference path at the i-th prediction time step; parameter Q is the lateral deviation penalty weight constant based on the parameter, used to urge the system to closely follow the set trajectory; variables This represents the servo motor torque increment sent by the system to the servo driver between adjacent control time steps; variable W is the control increment penalty weight calculated by the aforementioned formula.

[0045] After establishing the cost function J, the quadratic programming solver configured within the main control computer 101 iteratively searches for a sequence of control variables that minimizes J within the constraint space. The system extracts the first control increment element from this sequence, adds it to the actual output torque value of the previous control cycle, and synthesizes the optimal correction torque command sequence for the current cycle, which is then sent to the servo driver 106 for execution via the bus. This is achieved by combining the control increment penalty weight W with the slip characteristic S... slipBy employing a physically cascaded reconstruction method, when the underlying positioning system detects signs of slippage due to oil contamination or rapid acceleration, the model predictive controller will generate high-cost high-torque actions due to the sharp amplification of W. Under this mechanism, the quadratic programming solver will be forced to output weak and gradual torque increment commands. This allows the trolley chassis to actively soften the correction torque output while sacrificing brief tracking adhesion, prioritizing the restoration of effective friction and grip between the tires and the ground. This eliminates the low-frequency oscillations and loss of control risks caused by the loss of adhesion in the large inertial system.

[0046] To ensure the system can quickly regain responsiveness after leaving the slippage area, the method includes a smooth recovery process in step S205. After the optimal correction torque command sequence is issued to the drive motor for execution, the background resident program of the main control computer 101 continuously monitors the slippage observation characteristic value S. slip The system defines a preset time window in memory. Within this preset time window, when S is detected... slip The value is consistently less than the slip dead zone threshold S th At this point, it is determined that the chassis has completely detached from the low-adhesion road surface. To prevent sudden system vibrations, the main control computer 101 gradually restores the measurement noise covariance matrix R of the wheel encoder according to a preset smooth decay rate. odom Returning to the initial diagonal matrix state, the control increment penalty weight W of the model predictive controller is gradually restored to the basic weight parameter W0 state to ensure a smooth transition when crossing different ground media.

[0047] Before closed-loop control operation, the selection of key thresholds depends on mechanical characteristics. The method performs an offline calibration process in step S206. Specifically, the slip dead zone threshold S... th The calibration was determined through offline testing on a real vehicle. During the calibration phase, engineers controlled the vehicle to perform multiple sets of linear acceleration operations with different acceleration rates and circular driving operations with a fixed turning radius on surfaces with different friction coefficients within the workshop. During the driving, the main control computer 101 synchronously collected V-band data at high frequency under an absolutely slip-free rolling state. imu With V wheel The system records ideal difference data samples. After collection, the system performs offline Gaussian distribution fitting on the sample set and calculates the mathematical standard deviation of the ideal difference data within the effective statistical interval. The system uses a specific preset multiple of this standard deviation as the boundary line separating normal elastic slip from harmful uncontrolled slip, thereby establishing the slip dead zone threshold S. th The calibration value is a fixed value of 0.15 m / s. This calibration mechanism provides a robust physical reference for the adaptive model.

[0048] The multi-sensor fusion positioning and correction method for the trolley in this embodiment extracts slip observation feature values ​​precisely by quantizing them from the high-frequency data stream at the lower level, and innovatively applies them across layers. At the positioning filtering level, the erroneous mileage data is eliminated by exponentially expanding the encoder observation covariance based on these features. At the top-level control level, the control increment penalty weights of the cost function are reconstructed using these features to limit torque mutations. These two technical measures solve the problem of absolute pose loss through a highly coupled hardware and software mechanism, avoiding harmful oscillations in the power system caused by blind correction, and enhancing the overall navigation stability of heavy-duty equipment. It should be noted that the above selection of communication frequency, penalty coefficient, and filtering architecture is only an example. Those skilled in the art can also achieve the purpose of this application by using processing devices with comparable performance and isomorphic algorithm variations.

[0049] Please see Figure 3 Based on the implementation of the control method, embodiments of this application provide a control device. For example... Figure 3 As shown, the control device has a high degree of integration at the hardware level. Its core components include a processor 301 and a memory 302 connected to the processor 301 via a high-speed bus. The memory 302 serves as a non-volatile storage medium, internally partitioned to store low-level firmware and high-level computer programs. The computer program is compiled from the multi-threaded task module described in the aforementioned method embodiment. When the device is powered on, the computer program is retrieved and executed by the processor 301, thereby seamlessly implementing all the logical control steps of the trolley multi-sensor fusion positioning and correction method described above at the physical hardware level.

[0050] This application provides a computer-readable storage medium. The computer-readable storage medium may be in the form of an optical storage disk, a magnetic hard disk array, or a flash memory chip. The computer-readable storage medium contains executable computer program instructions. When these computer program instructions are deployed in the processor of the main control environment of an industrial robot or trolley and executed, they can drive the corresponding physical actuator to implement the processing flow and action logic of the trolley multi-sensor fusion positioning and correction method disclosed in the embodiments of this application.

[0051] The above description is merely a preferred embodiment of this application and is not intended to limit this application. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of this application should be included within the protection scope of this application.

Claims

1. A method for multi-sensor fusion positioning and correction of a trolley, characterized in that, include: Real-time acquisition of data from the inertial measurement unit, lidar, and wheel encoder; Extract the longitudinal linear acceleration output by the inertial measurement unit during the current period, and perform an integral operation on the longitudinal linear acceleration to obtain the longitudinal velocity V. imu The wheel end linear velocity V is calculated by combining the angular velocity output by the wheel encoder. wheel With the longitudinal velocity V imu With the linear velocity V at the wheel end wheel The absolute value of the difference is used as the slip observation characteristic value S slip ; In the error state Kalman filter model of the fusion positioning thread, when the slip observation feature value S slip Greater than or equal to the preset slip dead zone threshold S th At that time, the measurement noise covariance matrix R of the wheel encoder odom Perform exponential expansion and output the corrected trolley pose information; The corrected trolley pose information and the slip observation feature value S are used to... slip The input is fed into the model prediction controller, and in the cost function of the model prediction controller, the slip observation feature value S is calculated. slip The control increment penalty weight W is dynamically increased to obtain the optimal correction torque command sequence, and the optimal correction torque command sequence is sent to the drive motor for execution.

2. The method as described in claim 1, characterized in that, The inertial measurement unit is rigidly mounted at the geometric center of the trolley chassis, and the wheel encoder is coaxially mounted on the drive wheel axle end of the trolley; the real-time acquisition of data from the inertial measurement unit, lidar, and wheel encoder includes: The triaxial acceleration and triaxial angular velocity of the inertial measurement unit are collected; Acquire the pulse signal from the wheel encoder and convert the pulse signal into the angular velocity of the current drive wheel; The point cloud data output by the lidar is received, and distortion correction processing is performed on the point cloud data.

3. The method as described in claim 2, characterized in that, The longitudinal linear acceleration output by the inertial measurement unit within the current period is extracted, and the longitudinal linear acceleration is integrated to obtain the longitudinal velocity V. imu ,include: Based on the three-axis angular velocities output by the inertial measurement unit and the attitude matrix of the previous moment, the attitude update matrix of the current moment is calculated using the Runge-Kutta method. The attitude update matrix is ​​used to transform the three-axis acceleration to the global navigation coordinate system, and the calibrated gravitational acceleration vector is subtracted. The true acceleration vector after removing the gravitational acceleration vector is transformed back to the vehicle coordinate system, and the corresponding longitudinal component is extracted as the longitudinal linear acceleration. Based on a preset sampling period span, the longitudinal linear acceleration is calculated by discrete-time integration to obtain the longitudinal velocity V at the current moment. imu .

4. The method as described in claim 1, characterized in that, The measurement noise covariance matrix R of the wheel encoder odom Exponential expansion includes: Calculate the slip observation characteristic value S slip With the slip dead zone threshold S th The difference; The difference is multiplied by a preset sensitivity coefficient k to obtain the expansion index term; Using the natural constant as the base, calculate the exponential amplification factor corresponding to the expansion exponential term; The measurement noise covariance matrix R odom Multiplying by the exponential magnification factor yields the expanded target covariance matrix R'. odom and the target covariance matrix R' odom Substitute the values ​​into the error state Kalman filter model for measurement update calculation.

5. The method as described in claim 4, characterized in that, The error state Kalman filter model performs system propagation calculations through continuous-time state equations, and the state variables of the continuous-time state equations include pose error, velocity error, and sensor zero bias error. During the measurement update phase, the scanning matching pose output by the lidar is combined with the target covariance matrix R' odom Calculate the Kalman gain; The prior state obtained from the propagation operation of the system is corrected by using the Kalman gain, and the corrected state is injected into the nominal state, and the corrected trolley pose information is output. The pose error and velocity error in the state variables are reset to zero.

6. The method as described in claim 1, characterized in that, In the cost function of the model prediction controller, based on the slip observation feature value S slip Dynamically increasing the control increment penalty weight W includes: Determine the slip observation feature value S slip Is it greater than the slip dead zone threshold S? th ; The slip observation characteristic value S slip Greater than the slip dead zone threshold S th In the case of the slip observation feature value S slip With the slip dead zone threshold S th The difference multiplied by the preset penalty ratio coefficient And add it to the basic weight parameter W0 to obtain the reconstructed control increment penalty weight W; The slip observation characteristic value S slip Less than or equal to the slip dead zone threshold S th In the case of [condition], the basic weight parameter W0 is maintained as the control increment penalty weight W.

7. The method as described in claim 6, characterized in that, The cost function includes a path deviation penalty term and a control increment penalty term; The path deviation penalty term is configured as a weighted sum of the squared errors between the expected pose and the predicted pose in each prediction time domain. The control increment penalty term is configured as a weighted sum of the squares of the output torque changes in adjacent control cycles within each prediction time domain, and the corresponding weight coefficient adopts the control increment penalty weight W. The cost function is minimized by a quadratic programming solver, and the first control increment in the obtained control sequence is extracted and added to the output torque of the previous cycle to obtain the optimal correction torque command sequence for the current cycle.

8. The method as described in claim 1, characterized in that, After the optimal correction torque command sequence is issued to the drive motor for execution, the method further includes: Continuously monitor the slip observation feature value S slip ; Within a preset time window, when the slip observation feature value S slip The slip dead zone threshold S remains continuously below the threshold value. th At that time, the measurement noise covariance matrix R of the wheel encoder is gradually recovered according to the preset attenuation rate. odom Return to the initial matrix state, and gradually restore the control increment penalty weight W to the basic weight parameter state.

9. A control device, characterized in that, It includes a memory and a processor, wherein the memory stores a computer program, which, when executed by the processor, implements the trolley multi-sensor fusion positioning and correction method as described in claim 1.

10. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a computer program that, when executed by a processor, implements the trolley multi-sensor fusion positioning and correction method as described in claim 1.

Images