Repeated positioning precision self-correcting method and device for heavy-load automatic guided vehicle

Abstract

The invention discloses a heavy-load automatic guided vehicle repeated positioning precision self-correction method and device, and the method comprises the steps: obtaining AGV operation state data and environment data collected by different sensors, and forming a multi-source data set; preprocessing the multi-source data set, constructing an environment model, and calculating attitude information in real time; based on an environment model, determining a positioning error and carrying out error modeling, correcting the weight of each sensor, each filter parameter, AGV position information and AGV attitude information in real time, and optimizing an AGV advancing path; fusing the multi-source data set by using an improved Kalman filtering algorithm, optimizing a Kalman filter in real time, and correcting a positioning error so as to further correct AGV position information; correcting an AGV advancing path in real time according to the image data of the surrounding environment of the AGV; and a regular correction mechanism is set, and the AGV is controlled to regularly execute a specified correction action. According to the method, the repeated positioning precision is improved, and stable operation of the AGV in a dynamic environment is ensured.

Description

Technical Field

[0001] The present invention relates to the field of automatic navigation and control technology, and in particular to a method and device for self-correcting the repeatable positioning accuracy of a heavy-duty automatic guided vehicle (AGV) based on an inertial measurement unit (IMU) and visual positioning. Background Art

[0002] In the field of automatic navigation and positioning, improving the positioning accuracy of heavy-duty AGVs is of great significance. Traditional AGV positioning technology mainly relies on a single sensor, such as IMU or laser. However, these methods have many limitations and defects. For example, IMU sensors are prone to cumulative drift errors, while visual sensors are unstable under lighting changes and obstacle occlusion.

[0003] In recent years, multi-sensor fusion technology has been widely used in the field of AGV navigation. As a method to improve positioning accuracy, the fusion of IMU and visual sensors makes up for the shortcomings of single sensor solutions by combining acceleration, angular velocity and visual feature point information.

[0004] However, the existing multi-sensor fusion technology still has many problems. For example, (1) Multi-sensor data fusion is complex and difficult: IMU and laser data have different characteristics (IMU provides high-frequency, short-term accurate data, while visual sensors provide global position information), and the temporal and spatial alignment, data fusion and synchronization of the two are difficult. Especially in high-dynamic scenes, IMU and visual data fusion often appear inconsistent, resulting in increased positioning errors. (2) Path planning relies on a single algorithm: Existing path planning methods usually rely on fixed algorithms, such as A* and Dijkstra algorithms, but these algorithms are difficult to cope with dynamically changing environments and lack real-time and flexibility. When obstacles appear or the environment changes, the path planning efficiency decreases, which can easily cause AGVs to stagnate or detour excessively. (3) Insufficient real-time correction capabilities and inability to cope with long-term accumulated errors: Although multi-sensor fusion in existing technologies can improve the positioning accuracy of AGVs in the short term, over time, IMU drift and visual errors accumulate, and the system lacks an effective self-correction mechanism to dynamically adjust sensor weights and update positioning parameters, resulting in a decrease in the positioning accuracy of AGVs in long-term operation. (4) Poor robustness in complex environments and difficulty in stable operation in dynamic environments: When the AGV enters a complex or dynamically changing environment (such as an area with dense traffic or frequently changing obstacles in a factory), the existing AGV system has difficulty adjusting the path or sensor weight in real time, showing poor environmental adaptability and robustness, which may lead to misoperation or collision.

[0005] Therefore, in practical applications, how to effectively combine these improved algorithms to form a systematic positioning error self-correction method still needs further research and exploration. Summary of the invention

[0006] In order to solve the problems existing in the background technology, the present invention proposes a method and device for self-correcting the repeatable positioning accuracy of a heavy-loaded automatic guided vehicle.

[0007] On one hand, the present invention provides a method for self-correcting the repeatability positioning accuracy of a heavy-duty automatic guided vehicle, comprising:

[0008] Obtain the operating status data and environmental data of the automated guided vehicle (AGV) collected by different sensors to form a multi-source data set;

[0009] Preprocess the multi-source data set, build an environment model, and calculate the posture information in real time;

[0010] Based on the environmental model, the positioning error is determined and error modeling is performed, the weights of each sensor and each filter parameter, AGV position information and AGV posture information are corrected in real time, and the AGV travel path is optimized;

[0011] The improved Kalman filter algorithm is used to further fuse the multi-source data set, and the Kalman filter is optimized in real time to correct the positioning error and further correct the AGV position information.

[0012] The AGV's travel path is corrected in real time based on the image data of the AGV's surrounding environment; and a regular correction mechanism is set up to control the AGV to perform specified correction actions regularly.

[0013] In one embodiment of the present invention, the AGV operation status data includes: the acceleration and angular velocity information of the AGV collected by the inertial measurement unit (IMU) sensor, and the speed and displacement information of the AGV collected by the odometer sensor; the environment data includes image data of the AGV surrounding environment collected by the visual sensor, and obstacle information collected by the obstacle detection sensor.

[0014] In one embodiment of the present invention, the acceleration and angular velocity information of the AGV is collected by an IMU sensor, and the specific method includes: installing the IMU sensor at an appropriate position of the automatic guided vehicle; statically calibrating the IMU sensor, and recording the zero bias value of the sensor when the AGV is stationary; dynamically calibrating the IMU sensor, and calibrating the scale factor of the sensor during the movement of the AGV to eliminate the proportional error between the sensor output and the actual value; setting the sampling frequency of the IMU sensor, and obtaining the initial acceleration and initial angular velocity at each sampling moment; performing zero bias correction on the initial acceleration and the initial angular velocity, respectively, to obtain the corrected acceleration and angular velocity; filtering the corrected acceleration and angular velocity to obtain filtered acceleration and angular velocity.

[0015] In one embodiment of the present invention, image data of the surrounding environment of the AGV is collected by a visual sensor. The specific method includes: before each image collection, the ambient light intensity is calculated in real time, and the exposure time and gain of the camera are dynamically adjusted according to the set target brightness; the position change of the camera within the exposure time is predicted by using the filtered acceleration and angular velocity, and the motion blur kernel function is calculated, and the image is deblurred by using the motion blur kernel function; a Gaussian pyramid is constructed for the deblurred image to generate a multi-scale image, and feature points are extracted at each scale to obtain a number of feature points;

[0016] For each feature point, the main direction is calculated, and a feature descriptor with rotation invariance is generated, which specifically includes: for each feature point, the gradient amplitude and gradient direction of the pixels in its neighborhood are calculated; in the neighborhood of the feature point, a direction histogram is constructed, the gradient direction is divided into several buckets, the gradient amplitudes of the corresponding directions are accumulated, and the angle corresponding to the direction with the largest amplitude in the histogram is obtained as the main direction; with the feature point as the center, the coordinate axis is rotated according to the main direction, the neighborhood is divided into several sub-regions, and in each sub-region, the direction and amplitude of the gradient are calculated to form a descriptor vector; the descriptor vectors of all sub-regions are combined to form the feature descriptor;

[0017] According to the feature response value and the spatial distribution of the feature points, the feature points are screened, the screened feature points are used as target feature points, and the position, scale, main direction and feature descriptor of the target feature points are recorded.

[0018] In one embodiment of the present invention, preprocessing the multi-source data set includes:

[0019] The multi-source data set is subjected to denoising, including: filtering the AGV running status data through a low-pass filter; applying an image smoothing algorithm to filter the image data of the AGV surrounding environment collected by the visual sensor; and using a median filter to remove extreme erroneous measurement values ​​for the obstacle information collected by the obstacle detection sensor;

[0020] Perform data normalization on the denoised multi-source data sets;

[0021] The normalized multi-source datasets are then subjected to posture integration.

[0022] In one embodiment of the present invention, the first AGV position information obtained by the IMU sensor and the second AGV position information obtained by the visual sensor are compared in real time to detect the positioning error, including: calculating the first AGV position information based on the acceleration information and the angular velocity information; calculating the second AGV position information by matching the feature points between consecutive frames based on the position information of the feature points extracted from the image data of the surrounding environment of the AGV; calculating the difference between the first AGV position information and the second AGV position information, and if the difference exceeds a preset first threshold, determining that there is a positioning error; detecting the direction of the positioning error;

[0023] According to the detected positioning error, the error is modeled and a total error model is constructed, including: according to the detected positioning error, the source of the error is determined, the errors from different sources are modeled respectively, and various error models are integrated to obtain the total error model; according to the total error model, the future error accumulation trend is predicted; and the total error model is corrected in real time.

[0024] In one embodiment of the present invention, an improved Kalman filter algorithm is used to fuse the multi-source data set, including: initializing Kalman filter parameters using the total error model, the initial state vector of the Kalman filter including initial AGV position information and initial AGV posture information; predicting the AGV state at the next moment based on the acceleration information and angular velocity information collected by the IMU sensor using the Kalman filter to obtain a predicted state; obtaining a measured state based on image data of the AGV surrounding environment collected by a visual sensor, comparing the measured state with the predicted state using the Kalman filter, and updating the predicted state; using the updated predicted state, outputting updated AGV position information and AGV posture information;

[0025] By dynamically adjusting the weight coefficient of the Kalman filter, the IMU cumulative error is corrected using the second AGV position information, including: obtaining a visual measurement value from the difference between the second AGV position information and the first AGV position information to correct the IMU cumulative error; dynamically adjusting the weight coefficient of the Kalman filter according to the IMU cumulative error; using the adjusted weight coefficient of the Kalman filter, fusing the second AGV position information into the Kalman filter, correcting the IMU cumulative error, and further correcting the AGV position information;

[0026] According to environmental changes and sensor status, the process noise matrix and measurement noise matrix of the Kalman filter are adjusted in real time to optimize the Kalman filter, including: according to environmental changes and sensor status, the process noise matrix and measurement noise matrix of the Kalman filter are adjusted in real time to update the gain of the Kalman filter; by regularly feeding back changes in errors and noise parameters, the dynamic adjustment mechanism of the Kalman filter is gradually optimized.

[0027] In one embodiment of the present invention, a dynamic path planning algorithm is used to correct the AGV's path in real time based on the image data of the AGV's surrounding environment, including: when the visual sensor detects an obstacle in front or the lighting environment changes affect the accuracy of visual positioning, the system immediately starts the path adjustment mechanism; the AGV calculates the optimal path in real time through the dynamic path planning algorithm; when an obstacle is detected approaching or may affect the current path, the AGV calculates an obstacle avoidance path based on the movement trajectory of the obstacle; the path is corrected based on the second AGV position information provided by the visual sensor, the obstacle position information, the first AGV position information collected by the IMU sensor, and the obstacle information collected by the obstacle detection sensor, and the final path correction value is calculated; by detecting the error between the current path and the expected path, the path planning strategy is continuously optimized.

[0028] In one embodiment of the present invention, a periodic correction mechanism is set. During the operation of the AGV, the AGV is periodically allowed to enter a preset correction position, and the AGV performs specified correction actions, including: pre-setting a number of fixed correction positions, and the correction positions are within the coverage of a fixed camera; when the AGV is monitored to travel to the correction position, normal path planning and navigation operations are suspended, and a series of specified correction actions are performed; the current position of the AGV is compared with the third position information provided by the fixed camera to correct the positioning error; positioning parameters are corrected according to the third position information and the corrected positioning error; the weights of the IMU sensor and the visual sensor are adjusted according to the corrected positioning parameters; and the trigger frequency and correction position settings of the periodic correction mechanism are optimized.

[0029] On the other hand, the present invention also discloses a device for self-calibrating the repeatability of a heavy-loaded automated guided vehicle. The device adopts the above-mentioned method for self-calibrating the repeatability of a heavy-loaded automated guided vehicle. The device comprises:

[0030] The acquisition module is used to obtain the running status data and environment data of the automatic guided vehicle (AGV) collected by different sensors to form a multi-source data set;

[0031] The environment modeling module is used to pre-process the multi-source data set, build an environment model, and calculate the posture information in real time;

[0032] A self-correction module, which is used to determine the positioning error and perform error modeling based on the environmental model, to correct the weights of each sensor and each filter parameter, AGV position information and AGV posture information in real time, and to optimize the AGV travel path;

[0033] The Kalman filter module is used to further fuse the multi-source data set using the improved Kalman filter algorithm, and optimize the Kalman filter in real time to correct the positioning error, so as to further correct the AGV position information;

[0034] The dynamic adaptive module is used to correct the AGV's travel path in real time based on the image data of the AGV's surrounding environment; and by setting a regular correction mechanism, the AGV is controlled to perform specified correction actions regularly.

[0035] In addition, another aspect of the present invention provides a readable storage medium, on which a program or instruction is stored. When the program or instruction is executed by a processor, the steps of the above-mentioned heavy-loaded automatic guided vehicle repeatable positioning accuracy self-correction method are implemented, and the same technical effect can be achieved.

[0036] The present invention has outstanding substantive features and significant progress compared to the prior art, specifically:

[0037] The method for self-correcting the repeated positioning accuracy of a heavy-loaded automated guided vehicle disclosed in the present invention solves the positioning error problem when the AGV runs for a long time in a complex environment, improves the repeated positioning accuracy, and ensures the stable operation of the AGV in a dynamic environment. BRIEF DESCRIPTION OF THE DRAWINGS

[0038] Figure 1 The overall process diagram of the method for self-correcting the repeatable positioning accuracy of a heavy-loaded automatic guided vehicle disclosed in one embodiment of the present invention is shown;

[0039] FIG2(a) shows a schematic diagram of a specific process of the IMU sensor collecting acceleration and angular velocity data of the AGV in three-dimensional space in real time;

[0040] FIG2( b ) shows a schematic diagram of a specific process of the visual sensor collecting image data of the AGV surrounding environment in real time;

[0041] Figure 3 It shows that the obstacle detection sensor scans the surrounding environment in real time and generates obstacle information in three-dimensional space;

[0042] Figure 4 A schematic diagram of a constructed three-dimensional dynamic environment model is shown;

[0043] Figure 5 A schematic diagram of positioning errors between the IMU sensor and the visual sensor is shown;

[0044] Figure 5 A schematic diagram of positioning errors between the IMU sensor and the visual sensor is shown;

[0045] Figure 6 Shows Figure 1 Detailed flow chart of step S4 in FIG.

[0046] Figure 7 The color monitoring effect diagram of the visual sensor is shown;

[0047] Figure 8 A schematic diagram of the structure of a self-correcting device for repeated positioning accuracy of a heavy-loaded automatic guided vehicle disclosed in an embodiment of the present invention is shown.

[0048] Wherein, the accompanying drawings are marked as follows:

[0049] 300- Heavy-load automatic guided vehicle repeatable positioning accuracy self-correction device;

[0050] 301-acquisition module;

[0051] 302-environment modeling module;

[0052] 303-self-correction module;

[0053] 304- Kalman filter module;

[0054] 305-Dynamic adaptive module. DETAILED DESCRIPTION

[0055] In order to make the above features and effects of the present invention more clearly understood, embodiments are given below and described in detail with reference to the accompanying drawings.

[0056] See also Figure 1 As shown, Figure 1 The figure shows the overall process diagram of a method for self-correcting the repeatable positioning accuracy of a heavy-loaded automatic guided vehicle disclosed in an embodiment of the present invention.

[0057] A method for self-correcting the repeatability of a heavy-duty automatic guided vehicle comprises at least the following steps:

[0058] Step S1, obtaining AGV operation status data and environment data collected by different sensors to form a multi-source data set. The AGV operation status data includes: the acceleration and angular velocity information of the AGV collected by the IMU sensor, the speed and displacement information of the AGV collected by the odometer sensor; the environment data includes the image data of the AGV surrounding environment collected by the visual sensor, and the obstacle information collected by the obstacle detection sensor.

[0059] In practice, in order to ensure that AGVs can operate stably in complex environments and accurately respond to external changes, it is necessary to collect AGV operating status data and environmental data in real time through multiple sensors to form a multi-source data set. Then, through comprehensive monitoring of environmental parameters and AGV operating status, posture adjustments and motion control decisions can be made based on real-time data.

[0060] In this embodiment, the AGV running status data specifically includes the acceleration and angular velocity information of the AGV collected by the IMU sensor, the speed and displacement information of the AGV collected by the odometer sensor, etc. The IMU sensor, odometer and other sensors are used to record the motion status of the AGV in real time, and the motion and posture information of the AGV is obtained. Specifically:

[0061] In one embodiment, the acceleration and angular velocity data of the AGV in three-dimensional space are collected in real time by an IMU sensor, the motion state of the AGV is recorded, and basic data is provided for subsequent positioning and posture estimation. Referring to FIG2(a), FIG2(a) shows a schematic diagram of a specific process of the IMU sensor collecting the acceleration and angular velocity data of the AGV in three-dimensional space in real time. Specifically, the following steps are included:

[0062] First, in step S111, the IMU sensor is firmly installed at an appropriate position of the AGV, usually at the center of mass of the AGV, to reduce the measurement error caused by movement. Ensure that the installation direction of the sensor is consistent with the coordinate system of the AGV, that is, the X, Y, and Z axes of the sensor correspond to the forward direction, lateral direction, and vertical direction of the AGV, respectively.

[0063] Then, step S112, calibrate the IMU sensor, including static calibration and dynamic calibration. Static calibration refers to recording the bias value of the sensor when the AGV is stationary: Among them, Bias acc is the zero bias of the accelerometer, Bias gyro is the gyroscope bias, a i and ω i are the acceleration and angular velocity readings of the ith sampling, respectively. N is the number of sampling times. Dynamic calibration is to calibrate the scale factor of the sensor during the AGV movement to eliminate the proportional error between the sensor output and the actual value.

[0064] Next, step S113, set the sampling frequency f of the IMU sensor s According to the motion characteristics and real-time requirements of AGV, a sampling rate of 100Hz or higher is usually selected to ensure that the detailed changes of AGV motion are captured. When real-time data collection starts, at each sampling time t k , directly read the data of the accelerometer and gyroscope to obtain the initial acceleration and initial angular velocity, where: the initial acceleration is The initial angular velocity is Among them, a x (t k ), a y (t k ), a z (t k ) are respectively at time tk Measured acceleration of X, Y, and Z axes; ω x (t k ),ω y (t k ),ω z (t k ) are the angular velocities of the X, Y, and Z axes respectively.

[0065] Next, step S114, the initial acceleration and initial angular velocity are subjected to zero bias correction to obtain the corrected acceleration a(t k ) and angular velocity ω(t k ), where: a(t k )=a raw (t k )-Bias acc ;ω(t k )=ω raw (t k )-Bias gyro ; Among them: a(t k ) is the corrected acceleration vector, ω(t k ) is the corrected angular velocity vector.

[0066] In addition, in order to further reduce the impact of high-frequency noise and ensure the accuracy and stability of the data, the corrected acceleration and angular velocity are filtered (step S115). In one embodiment, a first-order low-pass filter is used, and its recursive formula is: f (t k )=α·a(t k )+(1-α)·a f (t k-1 ),ω f (t k )=α·ω(t k )+(1-α)·ω f (t k-1 ), where a f (t k ) and ω f (t k ) are the filtered acceleration and angular velocity, α is the filter coefficient, the value range is (0,1), which can be set according to actual needs; t k-1 is the previous sampling moment. Through filtering, the sensor noise can be effectively reduced and the credibility of the data can be improved.

[0067] In addition, in some embodiments, the acceleration and angular velocity information of the AGV collected by the IMU sensor is further integrated with the obstacle information collected by the obstacle detection sensor (such as a laser navigation sensor or an ultrasonic sensor) to improve the positioning accuracy and environmental monitoring capabilities. This process can effectively identify obstacles in the environment and their locations, ensuring the stable operation of the AGV in a complex dynamic environment. At the same time, the filtered acceleration and angular velocity information is stored or sent to the data processing module together with the corresponding timestamp to provide input for subsequent posture estimation and position calculation. The data fused with the timestamp includes: timestamp t k Indicates the specific time of data acquisition and the filtered acceleration vector a f (t k ), the filtered angular velocity vector ω f (t k ).

[0068] Through the above steps, it is ensured that the data collected by the IMU sensor can be collected accurately, stably and in real time, providing reliable data support for the high-precision positioning and posture estimation of heavy-load AGVs.

[0069] In one embodiment, the odometer is responsible for recording the speed and displacement information of the AGV in real time. Based on the rotation distance of the AGV wheels, the odometer calculates the actual driving speed v(t) of the AGV: Among them, v(t) is the speed at time t, and Δs is the displacement of the AGV within the time interval Δt. By real-time monitoring of the speed, the AGV can make adjustments according to the current motion state and optimize the operation path.

[0070] In this embodiment, the environmental data of the AGV mainly includes image data of the AGV surrounding environment collected by the visual sensor, obstacle information collected by the obstacle detection sensor, and the like.

[0071] In one embodiment, a visual sensor is used to monitor changes in the surrounding environment in real time, collect image data of the AGV's surrounding environment, including light intensity, color changes, and the spatial position and state of objects in the environment (including the position and speed of moving obstacles), obtain visual information of the AGV's surrounding environment, and provide necessary data support for environmental modeling and visual positioning.

[0072] Specifically: First, high dynamic range (HDR) cameras, lidars, etc. are selected as visual sensors to adapt to environmental changes under different lighting conditions. When installing the visual sensor, ensure that its optical axis is consistent with the forward direction of the AGV, and accurately calibrate the camera's internal parameters and distortion correction parameters. Then, during the image acquisition process, an adaptive exposure control and motion blur compensation method that integrates IMU data is implemented. Referring to Figure 2(b), Figure 2(b) shows a specific flow chart of the visual sensor collecting image data of the AGV's surrounding environment in real time. Specifically, it includes the following steps:

[0073] Step S121: Before each image acquisition, calculate the ambient light intensity L in real time current , and according to the set target brightness L target , dynamically adjust the camera exposure time T exp and gain G, where: in, represents the initial exposure time, G init represents the initial gain, L current Indicates the current ambient light intensity, calculated as the average or median value of the image brightness, L target Indicates the target brightness value, which is preset to ensure image quality. Monitoring of ambient light intensity can help the system adapt to different lighting conditions and prevent the visual sensor from failing when the light is too bright or too dark. Light intensity L light The change in (t) can be calculated by the average value of the image brightness: Where: I pixel (i, t) is the brightness value of the i-th pixel, and N is the total number of pixels in the image. By calculating the average brightness, the system can adjust the camera's exposure settings based on real-time lighting information.

[0074] Since the motion of the AGV may cause motion blur in the image, the angular velocity ω(t) and acceleration a(t) provided by the IMU sensor are used to predict the change in the camera's posture during the exposure time, calculate the motion blur kernel function k(x, y), and then deblur the image after image acquisition (step S122). The deblurred image is: Among them, I blur represents the original blurred image, I sharp represents the clear image after deblurring, k(x,y) represents the motion blur kernel function, and represents the point spread function caused by the movement of the camera during exposure. represents the convolution operator, k -1 (x, y) represents the inverse filter of the motion blur kernel.

[0075] Step S123: Deblurring the image I sharp Construct a Gaussian pyramid to generate multi-scale images Is , and extract feature points at each scale to obtain several feature points, where s represents the scale level. Specifically: through the Gaussian filter G(x, y, σ s ) for the deblurred image I sharp Smoothing to generate multi-scale images: I s (x,y)=I sharp (x,y)*G(x,y,σ s ), where σ s is a scale parameter, and * represents convolution. Then, in some embodiments, a modified speeded up robust feature (SURF) algorithm is used to extract feature points at each scale. Specifically, the determinant of the Hessian matrix is ​​calculated at each scale, and extreme value points are detected as feature points.

[0076] Step S124: For the detected feature points, calculate their main direction θ i , and generate a feature descriptor D with rotation invariance i The descriptor calculation is based on the gradient information around the feature point to ensure the reliability of feature matching under different viewing angles. Specifically, first for each feature point (x i ,y i ), calculate the gradient magnitude m(x,y) and gradient direction φ(x,y) of the pixels in its neighborhood: in, and They are the gradients of the image in the x and y directions, respectively, which can be calculated by the Sobel operator.

[0077] Then, in the neighborhood of the feature point, a direction histogram is constructed, and the gradient direction φ(x, y) is divided into several buckets, usually 36 buckets (10° per bucket), and the gradient amplitudes of the corresponding directions are accumulated to determine the main direction θ i is the angle corresponding to the direction with the largest amplitude in the histogram: Where: δ is the Dirac function, when φ(x,y)=θ, δ=1, otherwise δ=0.

[0078] Finally, with the feature point as the center, follow the main direction θ i Rotate the coordinate axis and divide the neighborhood into several sub-regions, usually 4×4 sub-regions. In each sub-region, calculate the direction and magnitude of the gradient to form a descriptor vector. Combine the descriptor vectors of all sub-regions into a feature descriptor D with a length of 64 or 128. i , and normalize it: Where: ||D i || is the L2 norm of the feature descriptor.

[0079] In addition, in some embodiments, it is necessary to further screen the characteristic points obtained in step S123 according to the characteristic response value R i And the spatial distribution of feature points, select high-quality and evenly distributed feature points as target feature points to avoid feature points being too dense or sparse. And record the position of the target feature point (x i ,y i ), scale s i , main direction θ i and feature descriptor D i (Step S125) Specifically, for each feature point, calculate its feature response value R i , usually using the determinant of the Hessian matrix: R i =det(H)=L xx L yy -(L xy ) 2 , where the Hessian matrix H is defined as: L xx , L xy , L yy is the second-order partial derivative of the image, which can be obtained by convolution. Then, set the response value threshold R threshold , retain the characteristic response value greater than the response value threshold (i.e. R i >R threshold ) feature points. To ensure uniform spatial distribution of feature points, the image is divided into several grids, and only the first N feature points with the highest feature response values ​​are retained in each grid. Finally, the selected feature points are used as target feature points, and the relevant information of the target feature points is recorded, including the location of the feature points (x i ,y i ), scale s i , main direction θ i and feature descriptor D i At the same time, the timestamp t k , deblurred image I sharp And the filtered feature point information {(x i ,y i ,s i ,θ i , D i )} are stored together or sent to the data processing module to provide high-quality data input for subsequent visual positioning and environmental modeling.

[0080] In addition, in some embodiments, the visual sensor can also monitor the color changes of the environment. Color information is a key parameter that helps the system identify different objects and environments. Through the color histogram analysis of the RGB image, the system can detect the color changes of objects in the environment, which is further used for object recognition or obstacle discrimination.color The calculation formula of (t) is: Where: C(i, t) is the color value of the i-th pixel, C ref is the reference color, δ is the indicator function, which is 1 when the colors match, otherwise it is 0. The color monitoring effect diagram is shown in Figure 7 The figure shows the distribution of pixels in each color channel (red, green, blue) in the image captured by the AGV visual sensor. The horizontal axis represents the range of pixel values ​​(from 0 to 255), that is, the intensity of the color, while the vertical axis represents the number of pixels corresponding to each color intensity. Through color histogram analysis, the AGV can monitor color changes in the environment in real time, such as changes in lighting conditions, color differences of obstacles, etc.

[0081] In addition, the visual sensor not only monitors the static environment, but also tracks the position and speed of moving obstacles in real time. By tracking feature points between consecutive frames, the system can detect the movement trajectory of obstacles. obs (x,y,z,t) and velocity V obs (t) is obtained by matching and calculating the feature points in the image sequence: Among them, P obs (t) is the position of the obstacle at time t, and Δt is the sampling interval. By monitoring the moving speed and trajectory of the obstacle, the system can predict its future position and provide support for obstacle avoidance strategies.

[0082] Through the above process, AGV can collect high-quality visual data in real time in complex lighting environments and motion states, improving the stability and matching accuracy of feature points.

[0083] In one embodiment, the obstacle detection sensor equipped by the AGV scans the surrounding environment in real time to generate obstacle information in three-dimensional space, such as Figure 3 Specifically, the obstacle detection sensor calculates the distance and position of the obstacle by emitting and receiving laser beams or ultrasonic waves, and the detection results are recorded in the form of three-dimensional coordinates: obs (x, y, z, t), where P obs Represents the three-dimensional coordinate position (x, y, z) of the obstacle at time t. In this way, the obstacle detection sensor can quickly identify surrounding obstacles and their positions in a dynamic environment, ensuring that the AGV can make obstacle avoidance decisions in a timely manner, thereby maintaining stable operation.

[0084] In summary, through the above process, the AGV operation status data (including the acceleration and angular velocity information of the AGV collected by the IMU sensor, the speed and displacement information of the AGV collected by the odometer sensor) and the environmental data (including the image data of the AGV surrounding environment collected by the visual sensor, and the obstacle information collected by the obstacle detection sensor) are obtained to form a multi-source data set. Finally, all the collected environmental data and operation status data will be summarized and processed through the central control of the AGV. These data are combined with the navigation algorithm of the AGV to ensure that the posture and driving path can be adjusted in real time in a complex environment, avoiding collisions or misoperations, and improving operation efficiency and safety. Through the above data collection method, the AGV can fully perceive the external environment and its own operation status, provide reliable data support for subsequent navigation, obstacle avoidance and path planning, and can maintain a high degree of stability and control accuracy in a changing environment.

[0085] Step S2: preprocess the multi-source data set, build an environment model, and calculate the AGV posture information in real time.

[0086] In this embodiment, the multi-source data set needs to be further pre-processed, including data denoising, data normalization and posture integration.

[0087] Specifically, during the collection process, AGV obtains a large amount of data from a variety of sensors, but this data is often subject to noise and interference, especially in dynamic and complex environments. In order to ensure the accuracy of the data, it is first necessary to denoise the data. Specifically, it includes:

[0088] For the acceleration and angular velocity data collected by the IMU sensor, a low-pass filter is used to filter the noise: a filtered (t) = a(t) + (1-α)a filtered (t-Δt), where a filtered (t) is the filtered acceleration data, a(t) is the original acceleration data, α is the filter coefficient, which controls the intensity of denoising, and Δt is the sampling time interval. Similarly, the angular velocity data ω(t) also needs to undergo the same low-pass filtering process.

[0089] The speed and displacement information of the AGV collected by the odometer sensor also needs to undergo the same low-pass filtering process.

[0090] For the image data of the AGV surrounding environment collected by the visual sensor, an image smoothing algorithm (such as Gaussian filtering) is applied to reduce the noise in the image and maintain the clarity of the image. The image data after filtering is: Among them, G s (i, j) represents the spatial weight, G r(I(x, y), I(x+i, y+j) represents the weight based on the grayscale difference of pixels. This formula not only smoothes the image during image denoising, but also retains important edge and detail information. It is particularly suitable for AGV's visual sensor to process images in dynamic environments.

[0091] For obstacle data collected by obstacle detection sensors such as lidar, median filtering is used to remove extreme erroneous measurement values ​​to ensure more accurate detection of each obstacle position. The obstacle data after filtering is: filtered (x,y)=median{w(i,j)·I(x+i,y+j)|-k≤i,j≤k}; where w(i,j) is a weight based on the grayscale difference of pixels. By assigning smaller weights to neighboring pixels that are more different from the central pixel, the details and features in the data are retained while removing noise.

[0092] In addition, after data denoising, the AGV operating status data and the environment data after denoising are further normalized to ensure that all data are processed at the same scale. Since different sensors measure different units and ranges, unifying the data scale is crucial for subsequent posture calculations. The normalization formula is as follows: Among them, x norm is the normalized data, x is the original data, and x min and x max are the minimum and maximum values ​​in the data set, respectively. By normalizing the data to the [0,1] interval, it is possible to ensure that the data output by different sensors can be processed within the same computing framework.

[0093] In addition, after data normalization, it is necessary to further integrate the data of different sensors to integrate the AGV operation status data and the environment data into a multidimensional data matrix: D(t) = {a(t), ω(t), P obs (x, y, z, t)}, where: D(t) represents all sensor data collected at time t, including acceleration a(t), angular velocity ω(t) and the three-dimensional coordinates P of the obstacle obs (x, y, z, t). The data integration in this step provides comprehensive basic data for subsequent modeling and error detection.

[0094] In step S2, after the preprocessing of the multi-source data set is completed through the above preprocessing process, a three-dimensional dynamic environment model is further constructed based on the preprocessed AGV operation status data and the environment data. Figure 4 As shown in the figure, the purpose of this environmental modeling is to ensure that the AGV can accurately perceive the layout and dynamic changes of surrounding obstacles in a complex environment, and at the same time estimate its posture (position, direction, angle) in real time. Specifically:

[0095] First, based on the obstacle position information P obs (x, y, z, t) and AGV real-time position P AGV (x, y, z, t), generate the environment model of the AGV at each time point: E(t) = f(P obs (x,y,z,t),P AGV (x, y, z, t)); where E(t) is the environment model of the AGV at time t, and f is the modeling function that combines the obstacle position and the AGV position. This model not only describes the static obstacles (such as walls and fixed objects) in the environment where the AGV is located, but also includes dynamically changing obstacles (such as pedestrians or moving objects).

[0096] In addition, in some embodiments, based on the environmental model, the AGV operation status data collected by IMU sensors, visual sensors, obstacle detection sensors, etc. and the surrounding environment data are integrated to calculate the AGV posture information in real time. The posture information includes the AGV's displacement, rotation angle, and reference point position relative to the environment.

[0097] With real-time updated posture information, the AGV can adjust navigation based on its actual position and the relationship between the environment. The posture information is fed back to the main control system, and the controller dynamically adjusts the navigation parameters based on the posture data to optimize the running trajectory.

[0098] After combining environmental modeling with posture calculation, the system not only updates the environmental information around the AGV in real time, but also adjusts the path according to the posture. This ensures that the AGV maintains a stable operating posture in a dynamic and complex environment, and makes timely obstacle avoidance decisions when encountering obstacles.

[0099] Step S3: Based on the environmental model, determine the positioning error and perform error modeling, correct the weights of each sensor and each filter parameter, AGV position information and AGV posture information in real time, and optimize the planned path.

[0100] In this embodiment, during the operation of the AGV, it is first necessary to compare the position information obtained by the IMU and visual positioning with the pre-set path or target position in real time, detect the positioning error, and identify the size and direction of the error. Specifically:

[0101] First, obtain the position information from the multi-source sensor. When the AGV is running, it will obtain the current position information from the IMU sensor and the visual sensor in real time. Among them: the position information provided by the IMU sensor is based on the acceleration information collected by the accelerometer and the angular velocity information collected by the gyroscope, and the first AGV position information P is obtained. IMU (t), Among them, PIMU (t) is the first AGV position information estimated by the IMU sensor at time t, v IMU (t-Δt) is the velocity at the previous time point, a IMU (t-Δt) is the acceleration at the previous time point, and Δt is the time interval. The visual sensor extracts the position information of the feature points based on the image data of the AGV's surrounding environment, and calculates the displacement of the AGV during operation by matching the feature points between consecutive frames. The second AGV position information P vision (t).

[0102] Then, after obtaining the first AGV position information and the second AGV position information, the two are compared to calculate the difference ΔP(t) between the first AGV position information and the second AGV position information = P IMU (t)-P vision (t), where ΔP(t) is the difference between the position information of the two sensors at time t. The positioning error between the IMU and the visual sensor can be determined by this difference. Then, if the difference ΔP(t) exceeds the set first threshold, it is determined that there is a positioning error, which is expressed as: Among them, E loc (t) is the positioning error at time t, x diff ,y diff and z diff Represents the position information difference on the X, Y, and Z axes respectively. This error value quantifies the difference between IMU and visual positioning.

[0103] Finally, the direction of the positioning error is detected. After quantifying the error size, the direction of the error needs to be determined. The calculation of the direction is based on the displacement difference of the X, Y, and Z axes in the three-dimensional coordinate system: Among them, θ error Indicates the direction of the error in the horizontal plane, φ error Indicates the error angle in the vertical direction. These two angles provide direction reference for subsequent positioning correction.

[0104] Finally, if Figure 5 The figure shows the first AGV position information and the second AGV position information detected by the IMU sensor and the visual sensor during the AGV operation, as well as the error vector between them. By comparing the first AGV position information provided by the IMU and the second AGV position information provided by the visual sensor, the size and direction of the positioning error can be detected in real time. At the same time, when setting the correction position, the AGV uses the high-precision feedback of the visual sensor for self-correction to improve the positioning accuracy.

[0105] In addition, in this embodiment, after the error is detected, the error is further modeled. Specifically:

[0106] First, analyze the current error source. According to the positioning error detected in the previous step, determine whether the source of the error is sensor drift, environmental interference, or mechanical wear. In order to accurately model these errors, according to the detected positioning error, it is necessary to divide it into different sources, and model the errors from different sources separately to further integrate various error models to obtain the total error model. Specifically:

[0107] Where: Sensor drift error usually accumulates over time and manifests as a linear deviation, which can be modeled using the following formula: E drift (t) = E drift (t-Δt)+α·Δt; where E drift (t) is the drift error at the current moment, and α is the drift rate coefficient, which indicates the accumulation speed of the error over time Δt.

[0108] Environmental interference errors are usually caused by external factors (such as light, obstacles, etc.). The error varies with environmental fluctuations and manifests as nonlinear interference. It can be modeled by the following formula: E env (t) = β·ΔP env (t); where β is the environmental interference coefficient, ΔP env (t) is the amount of environmental change, such as changes in lighting or the position of obstacles.

[0109] Mechanical wear error is usually caused by the wear of mechanical parts, resulting in displacement or velocity deviation. It can be modeled by the following formula: E mech (t) = γ·Δs(t); wherein γ is the wear rate coefficient, and Δs(t) represents the displacement change caused by the wear of the mechanical component at time t.

[0110] Then, the total error model is established by combining different error types. In the actual operation process, the error sources are often multi-source, so the total error model is established by integrating various error models: E total (t) = w IMU ·E IMU (t)+w vision ·E vision (t)+w env ·E env (t); where E total (t) is the total error after multi-source sensor fusion, w IMU 、w vision and w envThe weights of IMU, visual sensor and environmental errors are adjusted dynamically according to the real-time environment and sensor performance. This fusion process ensures that errors from different sources can be integrated to improve the accuracy of error modeling.

[0111] In addition, in some embodiments, the future error accumulation trend is further predicted based on the established total error model. The error not only affects the positioning accuracy at the current moment, but also may gradually increase over time. Therefore, the future error accumulation is predicted by the recursive formula: E pred (t+Δt)=E total (t)+γ·(E total (t)-E total (t-Δt)); where E pred (t+Δt) is the error predicted in the future Δt time, and γ is the error accumulation rate factor. This formula can help determine the future error accumulation, so as to make corrections in advance to avoid further expansion of the error.

[0112] In addition, in some embodiments, the total error model is adjusted in real time. After the total error model is established, it can be adjusted according to the real-time error to update the sensor's calibration parameters to prevent excessive error accumulation. For example, when the drift error of the IMU sensor is detected to increase significantly, the data weight of the visual sensor can be increased to reduce the reliance on the IMU data and ensure positioning accuracy.

[0113] In addition, in this embodiment, after the total error model is constructed, a self-correction module is further constructed based on the output of the total error model. The self-correction module is mainly used to adjust the weights and filter parameters of each sensor, optimize the planning path, and calibrate the AGV position information and AGV posture information. Real-time correction is achieved through the self-correction module to ensure that the impact of error accumulation on positioning accuracy is effectively reduced in complex environments. Specifically:

[0114] For dynamic adjustment of the weight of each sensor, according to the error information from different sources in the total error model, the weight coefficient of each sensor will be adjusted according to the current environment and sensor performance. The weight of the sensor can be updated by the following formula: sensor (t+Δt)=w sensor (t)+η·(E sensor (t)-E avg (t)); where w sensor (t+Δt) is the weight of the sensor in the next time step, η is the adjustment rate, E sensor (t) is the current error value of the sensor, E avg(t) is the average error value of all sensors. The core idea of ​​weight adjustment is to increase the weight of sensors with better performance and reduce the impact of sensors with larger errors on the overall positioning.

[0115] Then, in order to further optimize the output of the sensor, the filter parameters used in the S1-S5 process (such as the process noise and measurement noise matrix in the Kalman filter, the parameter adjustment of the Kalman filter will be described in detail in step S4) will be dynamically adjusted to better adapt to environmental changes. The recursive formula for adjusting the filter is: Q(t+Δt)=Q(t)+δ Q ·(E(t)-E threshold ); R(t+Δt)=R(t)+δ R ·(E(t)-E threshold ), where Q(t) and R(t) represent the process noise and measurement noise matrices respectively, E(t) is the current error, and E threshold is the set error threshold, δ Q and δ R is the adjustment factor. When the detected error gradually increases, the filter will dynamically adjust to adapt to the new noise level, thereby better suppressing the impact of noise on positioning accuracy.

[0116] In addition, in some embodiments, the current positioning accuracy is further determined based on the output of the total error model, and when the current positioning accuracy exceeds the preset second threshold, it indicates that the positioning accuracy is poor and the path needs to be further optimized. In particular, when the error accumulation is significant, the AGV may need to re-plan the path based on the current position to avoid further expansion of the error. The goal of path optimization is to reduce the interference caused by environmental changes and ensure the stability and accuracy of the path. The basic formula for path optimization is: P opt (t+Δt)=P(t)+λ·(P target -P(t)); where P opt (t+Δt) is the optimized path point, P(t) is the current path point, P target is the target path point, and λ is the path adjustment coefficient. Through this dynamic path optimization formula, the AGV can gradually adjust to the optimal path to reduce the path deviation caused by positioning error.

[0117] In addition, in some embodiments, after adjusting the weights of each sensor and the filter parameters and optimizing the path, further according to the first AGV position information P IMU (t), second AGV position information P vision (t calibrates the AGV position information to obtain the initially corrected AGV position information; and according to the first posture information θ provided by the IMU sensor IMU (t), the second posture information θ provided by the visual sensorvision (t) Calibrate the AGV posture information to obtain the preliminary corrected AGV posture information. That is: P 1_corrected (t) = w IMU ·P IMU (t)+w vision ·P vision t);θ 1_corrected (t) = w IMU ·θ IMU (t)+w vision ·θ vision (t); where P 1_corrected t) and θ 1_corrected (t) are the AGV position information and posture information after preliminary correction, w IMU and w vision The weights for dynamically adjusting the IMU sensor and the visual sensor are respectively. By fusing sensor data in real time, the AGV can maintain high positioning accuracy in complex environments and reduce the accumulation of errors.

[0118] Step S4: Using the improved Kalman filter algorithm, further fuse the multi-source data set, optimize the Kalman filter in real time, correct the IMU accumulated error, and further correct the AGV position information.

[0119] refer to Figure 6 As shown in Figure 6 A detailed flow chart of step S4 is shown.

[0120] In this embodiment, step S41 uses an improved Kalman filter algorithm to further fuse the AGV operating status data and the surrounding environment data collected by different sensors, comprehensively consider the advantages and disadvantages of different sensors, reduce the impact of single sensor errors, and improve positioning accuracy.

[0121] Specifically, in step S41, the Kalman filter parameters are first initialized based on the total error model in step S3. The self-correction module has identified and reduced the sensor error, on this basis, the initial state vector X0 and the error covariance matrix P0 of the Kalman filter are dynamically adjusted. The initial state vector X0 contains the initial position and posture information of the AGV: X0 = [P0, θ0] T ,By setting these parameters reasonably, it is ensured that the data fusion in the subsequent filtering steps can be stable and efficient.

[0122] Then, the prediction phase of the Kalman filter is entered. In the prediction phase of the Kalman filter, the AGV state at the next moment is predicted based on the real-time acceleration and angular velocity data collected by the IMU sensor to obtain the predicted state: X t|t-1 =AX t-1|t-1 +BUt-1 , P t|t-1 =AP t-1|t-1 A T +Q; where the state transfer matrix A and the control input matrix B are calculated based on the motion model and control input. The acceleration U provided by the IMU t-1 The angular velocity data is used to predict the position and attitude. The error covariance matrix P t|t-1 The process noise Q is taken into account in this process to handle the influence of IMU drift and other sensor noise in the prediction step. Then, the image data of the AGV surrounding environment collected by the visual sensor provides the location of the feature points and the visual information of the surrounding environment to obtain the measured state Z t , the measured state Z is converted into t With the predicted state X t|t-1 Compared with the previous step, the predicted state is updated. The key to the update step is the Kalman gain K t , which determines how to balance the predicted and observed values:

[0123] K t =P t|t-1 H T (HP t|t-1 H T +R) -1

[0124] X t|t =X t|t-1 +K t (Z t -HX t|t-1 )

[0125] P t|t =(IK t H)P t|t-1

[0126] Kalman gain K t Dynamically adjust the trust level of IMU sensor and visual sensor data. Based on the drift of IMU and the environmental adaptability of visual sensor, the weight of visual sensor can be adaptively increased, especially when the IMU error is large.

[0127] Finally, using the updated predicted state, the Kalman filter outputs the updated AGV position information AGV position information P fused (t) and AGV posture information θ fused (t): P fused (t) = P t|t ,θ fused (t) = θ t|t .

[0128] These data are processed by multi-sensor fusion and Kalman filtering, which significantly reduces the impact of single sensor errors. By combining the advantages of visual sensors and IMUs, high-precision positioning performance can be maintained in complex environments. At the same time, quantitative indicators can be introduced, such as setting error thresholds, to guide the implementation of feedback mechanisms and ensure that the operating parameters of AGVs are adjusted in a timely manner when the threshold is exceeded.

[0129] In addition, in some embodiments, to address the cumulative error problem of the IMU sensor, the weight coefficient of the Kalman filter is dynamically adjusted to give the visual sensor a higher weight, and the position information of the second AGV positioned by visual positioning is used to offset the cumulative error of the IMU, thereby ensuring the positioning accuracy of the AGV in long-term operation (step S42).

[0130] Specifically, in step S42, the source of the IMU cumulative error needs to be identified first. During operation, the drift problem of the IMU sensor will accumulate over time, and this cumulative error is particularly obvious during long-term operation. Through the previous multi-sensor data fusion, the IMU drift error can be identified and the size of the error can be quantified. The IMU cumulative error E drift (t) can be expressed as: E drift (t) = E drift (t-Δt)+α·Δt, where α is the IMU drift rate, indicating the linear growth of the cumulative error over time. The detection of the cumulative error provides a basis for subsequent corrections.

[0131] Then, the visual sensor is used for absolute positioning. Since the drift error of the IMU cannot be completely eliminated, the visual sensor can offset this cumulative error by providing absolute position information in the environment, that is, the second AGV position information. The visual sensor can calculate relatively accurate position information, that is, the second AGV position information, through feature point matching and visual mileage. The visual measurement value E is obtained by the difference between the second AGV position information and the first AGV position information. vision (t) is used to correct the IMU cumulative error. Visual measurement value E vision (t) = P vision (t)-P IMU (t), where P vision (t) is the second AGV position information measured by the visual sensor, P IMU (t) is the first AGV position information predicted by IMU.

[0132] Next, the weight coefficient of the Kalman filter is adjusted dynamically. In the case of obvious error accumulation, the weight of the visual sensor will be dynamically increased to reduce the impact of IMU data, thereby reducing the impact of IMU drift on the overall positioning accuracy. The weight adjustment formula is: vision (t+Δt)=wvision (t)+η·E drift (t), w IMU (t+Δt)=1-w vision (t+Δt); where η is the adjustment rate coefficient, ensuring that as the IMU cumulative error increases, the visual sensor gradually becomes dominant, while the weight of the IMU decreases accordingly. This dynamic adjustment process ensures that the laser navigation, IMU, and visual sensor can be balanced according to real-time error feedback, thereby optimizing sensor fusion.

[0133] Finally, the IMU cumulative error is corrected. The dynamically adjusted weight coefficient w vision 、w IMU , the second AGV position information provided by the visual sensor is integrated into the Kalman filter, the IMU cumulative error is corrected, and the AGV position information is further corrected to obtain the corrected AGV position information: P 2_corrected (t) = w IMU (t)·P IMU (t)+w vision (t)·P vision (t), corrected position: P 2_corrected (t) can further effectively reduce the impact of IMU drift error on positioning accuracy, ensuring that AGV can maintain high-precision positioning effect during long-term operation.

[0134] In addition, in some embodiments, the process noise matrix and the measurement noise matrix of the filter are further adjusted in real time according to environmental changes and sensor states to optimize the filter (step S43) and improve the adaptability and stability of the filter. Specifically:

[0135] First, analyze the current environment and sensor status. In complex environments, external factors such as lighting, obstacles, and vibrations can have a significant impact on sensor performance. For example, visual sensors may have increased measurement errors in low-light environments, while IMUs may have increased noise during intense motion. Therefore, the key to filter optimization is to dynamically adjust the noise model based on environmental changes. To this end, the system analyzes the environmental parameters P in real time. env (t) and sensor performance S sensor (t).

[0136] Then, the process noise matrix Q is adjusted in real time. When the uncertainty in the environment (such as intensified AGV movement and increased road vibration) increases, the system will increase the value of the process noise matrix Q accordingly to reflect the uncertainty of the system state. The specific adjustment formula is: Q(t+Δt)=Q(t)+α Q ·(S sensor (t)-S threshold ), where α Q is the adjustment coefficient, (Ssensor (t) represents the performance index of the sensor, such as the acceleration change rate of the IMU or the number of feature points of the visual sensor, S threshold is the set sensor performance threshold. When the sensor noise increases, the system will increase the process noise matrix Q, making the Kalman filter more dependent on the measurement data, and when the sensor state is stable, Q is reduced to enhance the accuracy of the prediction.

[0137] Next, dynamically adjust the measurement noise matrix R. The measurement noise mainly comes from the error of the sensor itself. Especially when the environment changes dramatically, such as light changes or obstacles, the measurement error of the visual sensor will increase significantly. Therefore, the adjustment formula of the measurement noise matrix R is: R(t+Δt)=R(t)+β R ·(P env (t)-P threshold ), where β R is the adjustment coefficient, P env (t) represents the current environmental parameters (such as light intensity, obstacle position), P threshold is the environmental noise threshold. By dynamically adjusting the measurement noise matrix, the system can flexibly handle the uncertainty of sensor data according to changes in the external environment, ensuring that the filter can maintain stability when the sensor measurement quality changes.

[0138] Then, the filter update strategy is optimized. Through the dynamic adjustment of the process noise and measurement noise matrix mentioned above, the filter gain K t Will also change accordingly: K t =P t|t-1 H T (HP t|t-1 H T +R(t)) -1 , filter gain K t Determines how the system balances the predicted value and the measured value. When the environmental noise and sensor error increase, the filter gain will be more dependent on the measured data, and vice versa. By flexibly adjusting the process noise and measurement noise, the filter can effectively cope with the uncertainty caused by environmental changes and improve positioning accuracy.

[0139] Finally, the feedback adjustment process. In the long-term operation, the system gradually optimizes the dynamic adjustment mechanism of the filter by regularly feeding back the changes in error and noise parameters to ensure its stable operation in complex environments. The feedback formula is: Among them, Q opt and R opt is the expected optimal process noise and optimal measurement noise, and the system further adjusts the current noise parameters based on the feedback data.

[0140] Step S5: According to the image data of the surrounding environment of the AGV, the AGV travel path is corrected in real time, and a periodic correction mechanism is set to control the AGV to perform specified correction actions regularly.

[0141] As described in the previous step S1, the visual sensor can monitor the changes in the environment around the AGV in real time and collect image data of the environment around the AGV, including light intensity, color changes, the position and speed of moving obstacles, etc. All collected environmental data (light, color, obstacle position and speed) will be transmitted to the central data framework of the system in real time, providing basic data support for the execution of dynamic path planning, obstacle avoidance strategy and control algorithm. Through this data feedback mechanism, the system can not only flexibly respond to changes in the external environment, but also adaptively adjust its motion strategy to ensure stable and accurate operation in complex environments.

[0142] In this regard, in this embodiment, a dynamic path planning algorithm is further used based on the image data of the AGV's surrounding environment to adjust the AGV's travel path in real time, avoid new obstacles, optimize the travel route, and ensure that the visual positioning maintains stable operation under various environmental conditions. Specifically:

[0143] First, the path is blocked based on the obstacles and environmental changes monitored by the visual sensor. When the visual sensor detects an obstacle ahead or changes in the lighting environment affect the accuracy of visual positioning, the system immediately activates the path adjustment mechanism. The position and moving speed of the obstacle P obs (x, y, z, t) and V obs (t) is the key input for path adjustment.

[0144] Next, the dynamic path planning algorithm is used to adjust the path. In path planning, the AGV will comprehensively consider the current obstacle position, target position and path feasibility. The AGV uses the A* algorithm to calculate the optimal path in real time to ensure that it can reach the target point quickly while avoiding obstacles. The calculation formula for dynamic path planning is as follows: Among them, P new (t) represents the new path after dynamic adjustment, P current (t) represents the current location of the AGV, P target represents the target position, P obs (t) represents the position of the obstacle, V obs (t) represents the moving speed of the obstacle, and λ and μ are control coefficients.

[0145] In order to ensure that AGVs can more flexibly adapt to different lighting conditions and obstacles in complex environments, the system will dynamically adjust parameters such as λ and μ according to environmental changes. This parameter adjustment will dynamically optimize path planning and obstacle avoidance strategies based on real-time monitoring results to ensure that AGVs can continue to operate stably under various complex conditions. By continuously adjusting these key parameters, AGVs can adapt to rapid changes in the environment and maintain efficient path planning and safe obstacle avoidance.

[0146] Then, real-time obstacle avoidance and path optimization. When the system detects that an obstacle is approaching or may affect the current path, the AGV will calculate an obstacle avoidance path based on the movement trajectory of the obstacle. The planning of the obstacle avoidance path combines the dynamic window method (DWA) to ensure that the AGV can avoid moving obstacles in a dynamic environment. The obstacle avoidance path optimization formula is: P safe (t) = min(P new (t),P alternate (t)), where P safe (t) is the final calculated safe path, P alternate (t) is the alternative path. The AGV selects the optimal driving route after avoiding obstacles to ensure path safety.

[0147] Next, the path is corrected in conjunction with the visual sensor. During the dynamic path adjustment process, the second AGV position information and obstacle position information provided by the visual sensor will be used for path correction to ensure the accuracy of path planning. The visual sensor obtains the precise position of the obstacle through feature point tracking, and performs data fusion with the first AGV position information collected by the IMU sensor and the obstacle information collected by the obstacle detection sensor to calculate the final path correction value. This data fusion ensures that the AGV can accurately adjust the path and avoid deviations caused by single sensor errors.

[0148] Finally, feedback and continuous adjustment. AGV uses the feedback mechanism to update environmental data and path planning parameters in real time. The system detects the current path P measured With the expected path P expected The error is calculated and the path planning strategy is continuously optimized. The feedback adjustment formula is: Through this feedback, the AGV can quickly respond to environmental changes and adjust its driving strategy to ensure that it always travels on a safe and efficient path.

[0149] In addition, in some embodiments, a periodic calibration mechanism is further provided. During the operation of the AGV, the AGV is periodically allowed to enter a pre-set calibration position, which is a fixed position within the coverage of a fixed camera. The AGV performs a specified calibration action and uses the positioning feedback information captured by the camera to calibrate its own positioning parameters to eliminate the accumulated positioning error. Specifically:

[0150] First, set the calibration position. In order to regularly calibrate the positioning parameters of the AGV, the system will pre-set several fixed calibration positions. These positions are usually key points in the environment and are within the coverage of fixed cameras or other high-precision positioning equipment. cal They will appear regularly on the AGV's path, and when the AGV reaches these locations, the correction mechanism will be triggered.

[0151] Then, the AGV enters the correction position and performs the correction action. When the AGV reaches the preset correction position, the system will suspend normal path planning and navigation operations and perform a series of specified correction actions, such as slow movement or turning, so that the fixed camera covering the correction position can more accurately capture the position and posture of the AGV. The current position of the AGV P AGV (t) and the third position information P provided by the camera cam (t) for comparison, the third position information P cam (t) accurate position, correct positioning error: ΔP error (t) = P cam (t)-P AGV (t), where ΔP error (t) is the positioning error detected when correcting the position. Through this comparison, the system can accurately calculate the difference between the current position of the AGV and the preset target position.

[0152] Next, the positioning parameters are corrected according to the third position information fed back by the camera. The high-precision third position information P provided by the fixed camera cam (t) is used as a reference point, and the positioning parameters of the AGV's sensors (such as IMU and visual sensors) are corrected based on this feedback. The correction formula is as follows: corrected (t) = P AGV (t)+β·ΔP error (t), where P corrected (t) is the corrected AGV position, and β is the correction coefficient, which is used to control the amplitude of the correction. This correction process ensures that the AGV can eliminate the accumulated positioning error and maintain high-precision positioning in long-term operation.

[0153] Then, the positioning parameters are adjusted dynamically. The corrected positioning parameters will be fed back to the sensor fusion model to adjust the weights of the IMU and the visual sensor to reduce the subsequent positioning errors. For example, if a large IMU drift is detected at a fixed position, the system will increase the weight of the visual sensor in future path planning to reduce the impact of the IMU on positioning. The parameter adjustment formula is: IMU (t+Δt)=w IMU (t)·(1-α), w vision (t+Δt)=w vision (t)·(1+α); where α is the adjustment coefficient, which represents the adjustment range of the IMU and visual sensor weights. Through this adaptive adjustment, the system can further reduce the accumulation of positioning errors in future operations.

[0154] Finally, regular feedback and recalibration. The system optimizes the trigger frequency and position settings of the correction mechanism by performing feedback analysis on the data of the correction position to ensure that the correction is performed at the appropriate time and place. The feedback formula is: Through the average feedback of the error, the system can judge the effectiveness of the correction mechanism and dynamically adjust the correction frequency to ensure that the positioning accuracy of the AGV is maintained within the preset range during long-term operation.

[0155] In summary, the self-correction method for repeated positioning accuracy of heavy-duty automated guided vehicles disclosed in the present invention solves the positioning error problem of AGVs during long-term operation in complex environments, improves repeated positioning accuracy, and ensures the stable operation of AGVs in dynamic environments. This is specifically reflected in the following aspects:

[0156] First, the present invention is based on multiple sensors. By integrating visual sensors, IMU, lidar or ultrasonic sensors, the present invention collects AGV operation status data and environmental data, builds a high-precision environmental model, and provides data support for path planning and error correction through real-time environmental monitoring technology. Compared with the traditional single sensor modeling method, the multi-source sensor fusion of the present invention further improves the comprehensiveness and accuracy of environmental perception.

[0157] Secondly, the present invention adopts an improved Kalman filter algorithm, dynamically fuses the data of IMU and visual sensor, combines the high-frequency short-term data of IMU and the absolute positioning information of visual sensor, adjusts the sensor weight in real time and corrects the cumulative error of IMU, and ensures the positioning accuracy of long-term operation. This overcomes the problem of accuracy reduction caused by IMU cumulative drift and visual sensor instability in the prior art, greatly improves the repeated positioning accuracy of AGV in complex dynamic environments, and especially makes the positioning more stable in long-term operation.

[0158] In addition, by introducing the adaptive filter parameter optimization mechanism, the system can dynamically adjust the process noise and measurement noise matrix of the Kalman filter according to the changes in the environment and sensor status, ensuring that the multi-sensor data has higher accuracy and stability when fused. Compared with the existing fixed parameter filtering method, the improved Kalman filter algorithm ensures that the data fusion between the IMU and the visual sensor is more accurate and efficient, effectively eliminates the spatiotemporal synchronization error between sensors, improves the positioning accuracy of the system, and enhances the adaptability and robustness of the system.

[0159] Third, the present invention introduces the absolute position information of the visual sensor to correct the accumulated drift of the IMU in real time. The multi-sensor fusion and adaptive weight adjustment mechanism are adopted to ensure the reliability of IMU data in complex dynamic environments. This technical means solves the problem of the gradual decline in the accuracy of existing IMU sensors during long-term use, effectively suppresses the accumulated drift error of the IMU sensor, and maintains high-precision attitude and position estimation, especially during long-term operation.

[0160] Fourthly, the present invention combines the real-time environmental monitoring of visual sensors, and the system adjusts the AGV's path in real time through a dynamic path planning algorithm to avoid obstacles and optimize the driving route. The path planning algorithm uses dynamic information in the environment, combined with the A* algorithm and feedback mechanism, to improve the dynamic response capability of the system. This technical means enables the AGV to adapt to frequent changes in a dynamic and complex environment, allowing the AGV system to operate stably in a complex and dynamically changing environment, avoiding the positioning failure problem of the prior art under frequent changes in obstacles or light fluctuations.

[0161] Fifth, the present invention sets up a regular calibration mechanism. During the operation of the system, the AGV is regularly put into the preset calibration position, and the sensor is calibrated and the positioning parameters are adjusted through the high-precision position information provided by the fixed camera. This technical means solves the problem of positioning error accumulation in long-term operation in the prior art through regular calibration and feedback adjustment, ensuring that the AGV can regularly eliminate the accumulated errors during long-term operation, so that the AGV can still maintain positioning accuracy even after long-term operation.

[0162] In addition, it should be noted that the high-precision self-correction method disclosed by the present invention is applicable to multiple fields such as intelligent warehousing, automated logistics centers, e-commerce distribution, ports and docks, etc., by improving the operating efficiency and path planning capabilities of AGVs, reducing manual intervention and reducing operating costs. With the rapid development of intelligent manufacturing and automated logistics, the market demand for high-precision AGV technology will continue to increase. The technical solution of this application can help enterprises achieve automation upgrades and refined management, and has strong market competitiveness and broad application potential.

[0163] In addition, corresponding to the above-mentioned heavy-duty automatic guided vehicle repeatability self-calibration method, another embodiment of the present invention also discloses a heavy-duty automatic guided vehicle repeatability self-calibration device, referring to Figure 8 , Figure 8 The structure diagram of the heavy-duty automatic guided vehicle repeatable positioning accuracy self-correction device 300 is shown, which can be realized by Figure 1 ,Figure 2(a),Figure 2(a), Figure 6 The various processes of the self-correction method for repeatable positioning accuracy of a heavy-loaded automated guided vehicle are shown in FIG.

[0164] A heavy-duty automatic guided vehicle repeatability positioning accuracy self-correction device 300, the device at least comprising:

[0165] The acquisition module 301 is used to obtain the running status data and the environment data of the automatic guided vehicle (AGV) collected by different sensors to form a multi-source data set.

[0166] The environment modeling module 302 is used to pre-process the multi-source data set, build an environment model, and calculate posture information in real time.

[0167] The self-correction module 303 is used to determine the positioning error and perform error modeling based on the environmental model, to correct the weights of each sensor and each filter parameter, AGV position information and AGV posture information in real time, and to optimize the AGV travel path.

[0168] The Kalman filter module 304 is used to utilize the improved Kalman filter algorithm to further fuse the multi-source data set, optimize the Kalman filter in real time, correct the positioning error, and further correct the AGV position information.

[0169] The dynamic adaptive module 305 is used to correct the AGV's travel path in real time according to the image data of the AGV's surrounding environment; and to control the AGV to perform designated correction actions periodically by setting a periodic correction mechanism.

[0170] It should be noted that for the detailed technical contents of each module in the device, please refer to the description of the corresponding method, and no specific explanation will be given here.

[0171] In addition, corresponding to the above-mentioned self-correction method for the repeatability of the positioning accuracy of the heavy-duty automatic guided vehicle, another embodiment of the present invention also discloses an electronic device, including a processor, a memory, a program or instruction stored in the memory and executable on the processor, which implements the steps of the above-mentioned self-correction method for the repeatability of the positioning accuracy of the heavy-duty automatic guided vehicle when executed by the processor, and can achieve the same technical effect. Among them, the processor is the processor in the electronic device in the above-mentioned embodiment. The readable storage medium includes a computer-readable storage medium, such as a computer read-only memory (ROM), a random access memory (RAM), a disk or an optical disk, etc.

[0172] Another embodiment of the present invention further provides a readable storage medium, on which a program or instruction is stored. When the program or instruction is executed by a processor, the steps of the above-mentioned self-correction method for repeatable positioning accuracy of the heavy-loaded automatic guided vehicle are implemented, and the same technical effect can be achieved.

[0173] It should be noted that, in this article, the terms "comprises", "includes" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also includes other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of further restrictions, an element defined by the sentence "comprises a ..." does not exclude the presence of other identical elements in the process, method, article or device including the element. In addition, it should be pointed out that the scope of the methods and devices in the embodiments of the present application is not limited to performing functions in the order shown or discussed, and may also include performing functions in a substantially simultaneous manner or in reverse order according to the functions involved. For example, the described method may be performed in an order different from that described, and various steps may also be applied, omitted, or combined.

[0174] The embodiments of the present application are described above in conjunction with the accompanying drawings, but the present application is not limited to the above-mentioned specific implementation methods. The above-mentioned specific implementation methods are merely illustrative and not restrictive. Under the guidance of the present application, ordinary technicians in this field can also make many forms without departing from the purpose of the present application and the scope of protection of the claims, all of which are within the protection of the present application.

Claims

1. A method for self-correcting the repeatability of a heavy-duty automated guided vehicle, characterized in that: Include: Obtain the operating status data and environmental data of the automated guided vehicle (AGV) collected by different sensors to form a multi-source data set; Preprocess the multi-source data set, build an environment model, and calculate the posture information in real time; Based on the environmental model, the positioning error is determined and error modeling is performed, the weights of each sensor and each filter parameter, AGV position information and AGV posture information are corrected in real time, and the AGV travel path is optimized; The improved Kalman filter algorithm is used to further fuse the multi-source data set, and the Kalman filter is optimized in real time to correct the positioning error and further correct the AGV position information. According to the image data of the AGV's surrounding environment, the AGV's travel path is corrected in real time; and By setting up a periodic correction mechanism, the AGV is controlled to perform specified correction actions regularly.

2. The method according to claim 1, characterized in that The AGV operation status data includes: the acceleration and angular velocity information of the AGV collected by the inertial measurement unit (IMU) sensor, and the speed and displacement information of the AGV collected by the odometer sensor; the environment data includes the image data of the AGV surrounding environment collected by the visual sensor and the obstacle information collected by the obstacle detection sensor.

3. The method according to claim 2, characterized in that in: The acceleration and angular velocity information of AGV is collected through IMU sensors. The specific methods include: Install the IMU sensor at the appropriate location on the AGV; Perform static calibration on the IMU sensor and record the zero bias value of the sensor when the AGV is stationary. Dynamically calibrate the IMU sensor. During the AGV movement, calibrate the sensor's scale factor to eliminate the proportional error between the sensor output and the actual value. Set the sampling frequency of the IMU sensor and obtain the initial acceleration and initial angular velocity at each sampling moment; Performing zero bias correction on the initial acceleration and the initial angular velocity respectively to obtain corrected acceleration and angular velocity; The corrected acceleration and angular velocity are filtered to obtain filtered acceleration and angular velocity.

4. The method according to claim 2, characterized in that: in: The image data of the AGV's surrounding environment is collected through visual sensors. The specific methods include: Before each image acquisition, the ambient light intensity is calculated in real time, and the camera exposure time and gain are dynamically adjusted according to the set target brightness; The filtered acceleration and angular velocity are used to predict the change of the camera's posture during the exposure time, and the motion blur kernel function is calculated. The image is deblurred using the motion blur kernel function. Construct a Gaussian pyramid for the deblurred image to generate a multi-scale image, and extract feature points at each scale to obtain several feature points; For each feature point, the main direction is calculated and a feature descriptor with rotation invariance is generated, including: For each feature point, calculate the gradient magnitude and gradient direction of the pixels in its neighborhood; In the neighborhood of the feature point, a direction histogram is constructed, the gradient direction is divided into several buckets, the gradient amplitudes of the corresponding directions are accumulated, and the angle corresponding to the direction with the largest amplitude in the histogram is obtained as the main direction; With the feature point as the center, the coordinate axis is rotated according to the main direction to divide the neighborhood into several sub-regions, and in each sub-region, the direction and amplitude of the gradient are calculated to form a descriptor vector; The descriptor vectors of all sub-regions are combined to form the feature descriptor; According to the feature response value and the spatial distribution of the feature points, the feature points are screened, the screened feature points are used as target feature points, and the position, scale, main direction and feature descriptor of the target feature points are recorded.

5. The method according to claim 2, characterized in that: The multi-source dataset is preprocessed, including: The multi-source data set is subjected to denoising, including: The AGV operation status data is subjected to noise filtering through a low-pass filter; Apply image smoothing algorithm to filter noise from image data of the AGV surrounding environment collected by the visual sensor; For obstacle information collected by obstacle detection sensors, median filtering is used to remove extreme erroneous measurement values; Perform data normalization on the denoised multi-source data sets; The normalized multi-source datasets are then subjected to posture integration.

6. The method according to claim 2, characterized in that in: Compare the first AGV position information obtained by the IMU sensor and the second AGV position information obtained by the visual sensor in real time to detect positioning errors, including: Calculate the first AGV position information based on the acceleration information and the angular velocity information; Based on the position information of the feature points extracted from the image data of the surrounding environment of the AGV, the second AGV position information is calculated by matching the feature points between consecutive frames; Calculating the difference between the first AGV position information and the second AGV position information, and if the difference exceeds a preset first threshold, determining that there is a positioning error; Detect the direction of positioning error; According to the detected positioning error, the error is modeled and a total error model is constructed, including: According to the detected positioning error, the source of the error is determined, the errors from different sources are modeled respectively, and various error models are integrated to obtain the total error model; According to the total error model, the future error accumulation trend is predicted; The total error model is corrected in real time.

7. The method according to claim 6, characterized in that in: The multi-source data set is fused using the improved Kalman filter algorithm, including: The total error model is used to initialize the Kalman filter parameters. The initial state vector of the Kalman filter includes the initial AGV position information and the initial AGV posture information. Based on the acceleration information and angular velocity information collected by the IMU sensor, the Kalman filter is used to predict the AGV state at the next moment to obtain the predicted state; Based on the image data of the surrounding environment of the AGV collected by the visual sensor, a measured state is obtained, and the measured state is compared with the predicted state by using a Kalman filter to update the predicted state; Using the updated predicted state, output updated AGV position information and AGV posture information; By dynamically adjusting the weight coefficient of the Kalman filter and using the second AGV position information, the IMU cumulative error is corrected, including: Obtaining a visual measurement value from the difference between the second AGV position information and the first AGV position information to correct the IMU cumulative error; Dynamically adjust the weight coefficient of the Kalman filter based on the IMU cumulative error; By using the adjusted weight coefficient of the Kalman filter, the second AGV position information is integrated into the Kalman filter to correct the IMU cumulative error and further correct the AGV position information; According to environmental changes and sensor status, the process noise matrix and measurement noise matrix of the Kalman filter are adjusted in real time to optimize the Kalman filter, including: According to environmental changes and sensor status, the process noise matrix and measurement noise matrix of the Kalman filter are adjusted in real time to update the gain of the Kalman filter; By regularly feeding back the changes in error and noise parameters, the dynamic adjustment mechanism of the Kalman filter is gradually optimized.

8. The method according to claim 6, characterized in that in: Based on the image data of the AGV's surrounding environment, a dynamic path planning algorithm is used to correct the AGV's travel path in real time, including: When the visual sensor detects an obstacle ahead or changes in the lighting environment affect the accuracy of visual positioning, the system immediately activates the path adjustment mechanism; AGV uses a dynamic path planning algorithm to calculate the optimal path in real time; When an obstacle is detected approaching or may affect the current path, the AGV will calculate an obstacle avoidance path based on the obstacle's motion trajectory; The path is corrected based on the second AGV position information and obstacle position information provided by the visual sensor, the first AGV position information collected by the IMU sensor, and the obstacle information collected by the obstacle detection sensor, and a final path correction value is calculated; By detecting the error between the current path and the expected path, the path planning strategy is continuously optimized.

9. The method according to claim 6, characterized in that in: Set up a regular correction mechanism. During the operation of the AGV, the AGV is regularly placed in the pre-set correction position. The AGV performs the specified correction actions, including: A number of fixed calibration positions are preset, and the calibration positions are within the coverage of the fixed camera; When the AGV is detected to be driving to the correction position, the normal path planning and navigation operations are suspended and a series of specified correction actions are performed; Compare the current position of the AGV with the third position information provided by the fixed camera to correct the positioning error; Correcting the positioning parameters according to the third position information and the corrected positioning error; Adjust the weights of the IMU sensor and the visual sensor based on the corrected positioning parameters; Optimize the trigger frequency and correction position settings of the periodic correction mechanism.

10. A self-correcting device for repeated positioning accuracy of a heavy-duty automatic guided vehicle, characterized in that: The method for self-correcting the repeatability accuracy of a heavy-loaded automated guided vehicle according to any one of claims 1 to 9 is adopted, and the self-correcting device for self-correcting the repeatability accuracy of a heavy-loaded automated guided vehicle comprises: The acquisition module is used to obtain the running status data and environment data of the automatic guided vehicle (AGV) collected by different sensors to form a multi-source data set; The environment modeling module is used to pre-process the multi-source data set, build an environment model, and calculate the posture information in real time; A self-correction module, which is used to determine the positioning error and perform error modeling based on the environmental model, to correct the weights of each sensor and each filter parameter, AGV position information and AGV posture information in real time, and to optimize the AGV travel path; The Kalman filter module is used to further fuse the multi-source data set using the improved Kalman filter algorithm, and optimize the Kalman filter in real time to correct the positioning error, so as to further correct the AGV position information; The dynamic adaptive module is used to correct the AGV's travel path in real time based on the image data of the AGV's surrounding environment; and by setting a regular correction mechanism, the AGV is controlled to perform specified correction actions regularly.

Images