Robot multi-sensor fusion positioning method and device, computer equipment and medium

Abstract

The invention relates to a robot multi-sensor fusion positioning method and device, computer equipment and a medium, and the method comprises the steps: obtaining a priori map of a preset driving route of a robot; multi-sensor data of multiple types of sensors in the running process of the robot in the preset route are collected in real time, and current position information is obtained in real time based on the multi-sensor data; performing preprocessing and fusion according to local sensor data in the multi-sensor data to obtain a local pose estimation result, and obtaining an initial sensor weight at a current position based on current position information and a priori map; according to the intensity of signals corresponding to different types of sensors in the multi-sensor data, the initial sensor weight is dynamically adjusted, and the adjusted sensor weight is obtained; and performing fusion based on global data in the multi-sensor data, a local pose estimation result and the adjusted sensor weight to obtain global positioning information of the robot.

Description

Technical Field

[0001] The present application relates to the field of fusion positioning technology, and in particular to a robot multi-sensor fusion positioning method, device, computer equipment, storage medium and computer program product. Background Art

[0002] The traditional multi-sensor fusion positioning system uses a combination of multiple sensors, such as GNSS, IMU (inertial measurement unit), wheel encoder and lidar, etc., and improves positioning accuracy by fusing the data information of each sensor.

[0003] In the traditional multi-sensor fusion positioning process, its workflow mainly includes the following core steps: a. Sensor data acquisition: GPS is used to obtain position information, IMU is used to obtain attitude information, wheel encoder is used to estimate displacement, and laser radar is used to build environmental maps; b. Sensor data preprocessing: filtering, denoising and other preprocessing operations are performed on the data of each sensor to ensure the accuracy of subsequent data fusion; c. Data fusion algorithm: Traditional systems often use data fusion algorithms such as Kalman filter (KalmanFilter), extended Kalman filter (EKF), particle filter (PF) or graph optimization. When processing multi-source sensor data, this algorithm usually sets fixed weights or assumes that the measurement errors of each sensor obey a specific statistical model, thereby fusing the output information of different sensors and calculating the final pose estimate; d. Position update and attitude estimation: The fused data is used to continuously update the position and attitude of the robot to ensure that the robot can estimate the current position information more accurately in different environments; e. Feedback control: According to the positioning results, the system will send motion control instructions to the robot to ensure that the robot moves according to the predetermined path or planned target.

[0004] Although the traditional multi-sensor fusion positioning solution can achieve basic positioning, its positioning process is easily affected by the environment, resulting in poor positioning accuracy. Summary of the invention

[0005] Based on this, it is necessary to provide an accurate robot multi-sensor fusion positioning method, device, computer equipment, storage medium and computer program product to address the above technical problems.

[0006] In a first aspect, the present application provides a robot multi-sensor fusion positioning method. The method comprises:

[0007] Obtain a priori map of the robot's preset driving route, wherein the priori map represents the association between position information and factor information, wherein the factor information represents the weights of different types of sensors at different positions, and wherein the weights represent the contribution ratio to the positioning accuracy;

[0008] Collecting multi-sensor data of various types of sensors of the robot in real time during the driving process of the preset route, and acquiring current position information in real time based on the multi-sensor data;

[0009] Preprocessing and fusing local sensor data in the multi-sensor data to obtain a local pose estimation result, and acquiring an initial sensor weight at the current position based on the current position information and the prior map;

[0010] Dynamically adjusting the initial sensor weight according to the strength of the signals corresponding to the different types of sensors in the multi-sensor data to obtain an adjusted sensor weight;

[0011] The global positioning information of the robot is obtained based on the fusion of the global data in the multi-sensor data, the local pose estimation result and the adjusted sensor weight.

[0012] In one embodiment, obtaining a priori map of a preset route for the robot to travel comprises:

[0013] Respectively obtain sensor data collected by different types of sensors to obtain a sensor data set;

[0014] Using an extended Kalman filter to perform data fusion on the sensor data set to determine the position corresponding to each collection point;

[0015] Extract the contribution ratio of different types of sensors to positioning accuracy during data fusion and obtain the sensor weight corresponding to each acquisition point;

[0016] The positions corresponding to the collection points are associated with the sensor weights to generate a priori maps.

[0017] In one embodiment, preprocessing and fusing local sensor data in the multi-sensor data to obtain a local pose estimation result includes:

[0018] Extracting local sensor data from the multi-sensor data;

[0019] The state of the robot is estimated by using a Kalman filter according to the local sensor data to obtain a local posture estimation result.

[0020] In one embodiment, estimating the state of the robot using a Kalman filter according to the local sensor data to obtain a local pose estimation result includes:

[0021] The linear velocity output by the wheel encoder and the angular velocity output by the IMU are used as initial conditions;

[0022] Approximate the robot's motion trajectory as an arc;

[0023] In the arc, based on the mileage calculation motion model of the probabilistic robot, the current robot posture is updated by considering the influence of motion noise to obtain the current posture after noise reduction;

[0024] Constructing an IMU observation model, and obtaining relative changes in position, velocity, and rotation between two odometer frames based on the IMU observation model;

[0025] Obtaining an IMU relative constraint based on a relative change in position, velocity, and rotation between the two odometer frames;

[0026] The de-noised current moment pose and the IMU relative constraint are fused to obtain a local pose estimate WIO.

[0027] In one embodiment, approximating the robot's motion trajectory to an arc includes:

[0028] Determine whether the angle change is less than a preset threshold;

[0029] If not, the robot's motion trajectory is approximated as an arc based on the velocity motion model of the probabilistic robot;

[0030] If so, the Runge-Kutta second-order integral is used for approximation, and the robot's motion trajectory is approximated as a circular arc.

[0031] In one embodiment, the global data includes GNSS data; the fusing of the global data in the multi-sensor data, the local pose estimation result and the adjusted sensor weight to obtain the global positioning information of the robot includes:

[0032] Loosely couple the GNSS data with the local pose estimation WIO to obtain a loosely coupled result;

[0033] Extracting a GNSS factor from the loosely coupled result and extracting a WIO factor from the local pose estimation WIO;

[0034] Performing factor modeling based on the GNSS factor and the WIO factor;

[0035] The factor modeling results are optimized based on the sliding window of segment elimination to obtain the global positioning information of the robot.

[0036] In a second aspect, the present application also provides a robot multi-sensor fusion positioning device. The device comprises:

[0037] A priori map acquisition module is used to obtain a priori map of the robot's preset route, wherein the priori map represents the association between position information and factor information, wherein the factor information represents the weights of different types of sensors at different positions, and wherein the weights represent the contribution ratio to the positioning accuracy;

[0038] A parameter acquisition module, used to collect multi-sensor data of various types of sensors of the robot in real time during the driving process of the preset route, and obtain current position information in real time based on the multi-sensor data;

[0039] An initial fusion processing module, used to preprocess and fuse local sensor data in the multi-sensor data to obtain a local pose estimation result, and obtain an initial sensor weight at the current position based on the current position information and the prior map;

[0040] A weight adjustment module, used to dynamically adjust the initial sensor weight according to the strength of the corresponding signals of different types of sensors in the multi-sensor data to obtain an adjusted sensor weight;

[0041] The fusion positioning module is used to obtain the global positioning information of the robot based on the global data in the multi-sensor data, the local pose estimation result and the adjusted sensor weight.

[0042] In a third aspect, the present application further provides a computer device. The computer device includes a memory and a processor, the memory stores a computer program, and the processor implements the following steps when executing the computer program:

[0043] Obtain a priori map of the robot's preset driving route, wherein the priori map represents the association between position information and factor information, wherein the factor information represents the weights of different types of sensors at different positions, and wherein the weights represent the contribution ratio to the positioning accuracy;

[0044] Collecting multi-sensor data of various types of sensors of the robot in real time during the driving process of the preset route, and acquiring current position information in real time based on the multi-sensor data;

[0045] Preprocessing and fusing local sensor data in the multi-sensor data to obtain a local pose estimation result, and acquiring an initial sensor weight at the current position based on the current position information and the prior map;

[0046] Dynamically adjusting the initial sensor weight according to the strength of the signals corresponding to the different types of sensors in the multi-sensor data to obtain an adjusted sensor weight;

[0047] The global positioning information of the robot is obtained based on the fusion of the global data in the multi-sensor data, the local pose estimation result and the adjusted sensor weight.

[0048] In a fourth aspect, the present application further provides a computer-readable storage medium. The computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the following steps are implemented:

[0049] Obtain a priori map of the robot's preset driving route, wherein the priori map represents the association between position information and factor information, wherein the factor information represents the weights of different types of sensors at different positions, and wherein the weights represent the contribution ratio to the positioning accuracy;

[0050] Collecting multi-sensor data of various types of sensors of the robot in real time during the driving process of the preset route, and acquiring current position information in real time based on the multi-sensor data;

[0051] Preprocessing and fusing local sensor data in the multi-sensor data to obtain a local pose estimation result, and acquiring an initial sensor weight at the current position based on the current position information and the prior map;

[0052] Dynamically adjusting the initial sensor weight according to the strength of the signals corresponding to the different types of sensors in the multi-sensor data to obtain an adjusted sensor weight;

[0053] The global positioning information of the robot is obtained based on the fusion of the global data in the multi-sensor data, the local pose estimation result and the adjusted sensor weight.

[0054] In a fifth aspect, the present application further provides a computer program product. The computer program product includes a computer program, and when the computer program is executed by a processor, the following steps are implemented:

[0055] Obtain a priori map of the robot's preset driving route, wherein the priori map represents the association between position information and factor information, wherein the factor information represents the weights of different types of sensors at different positions, and wherein the weights represent the contribution ratio to the positioning accuracy;

[0056] Collecting multi-sensor data of various types of sensors of the robot in real time during the driving process of the preset route, and acquiring current position information in real time based on the multi-sensor data;

[0057] Preprocessing and fusing local sensor data in the multi-sensor data to obtain a local pose estimation result, and acquiring an initial sensor weight at the current position based on the current position information and the prior map;

[0058] Dynamically adjusting the initial sensor weight according to the strength of the signals corresponding to the different types of sensors in the multi-sensor data to obtain an adjusted sensor weight;

[0059] The global positioning information of the robot is obtained based on the fusion of the global data in the multi-sensor data, the local pose estimation result and the adjusted sensor weight.

[0060] The above-mentioned robot multi-sensor fusion positioning method, device, computer equipment, storage medium and computer program product obtain a priori map of the robot's preset route, the priori map represents the correlation between position information and factor information, the factor information represents the weights of different types of sensors at different positions, and the weight represents the contribution ratio to positioning accuracy; real-time collection of multi-sensor data of multiple types of sensors in the robot's preset route, and based on the multi-sensor data, real-time acquisition of current position information; pre-processing and fusion of local sensor data in the multi-sensor data to obtain local pose estimation results, and based on the current position information and the priori map, obtain the initial sensor weight at the current position; dynamically adjust the initial sensor weight according to the strength of the corresponding signal of different types of sensors in the multi-sensor data to obtain the adjusted sensor weight; based on the global data in the multi-sensor data, the local pose estimation results and the adjusted sensor weight, fusion is performed to obtain the global positioning information of the robot. In the whole process, the priori map is introduced to realize the dynamic adjustment of the weights of different types of sensors in the fusion process, and the weight represents the contribution ratio to positioning accuracy, so the fusion positioning is more accurate, and accurate fusion positioning can be achieved. BRIEF DESCRIPTION OF THE DRAWINGS

[0061] Figure 1 A diagram showing an application environment of a robot multi-sensor fusion positioning method in one embodiment;

[0062] Figure 2 A schematic diagram of a process of a robot multi-sensor fusion positioning method in one embodiment;

[0063] Figure 3 A schematic diagram of a flow chart of a robot multi-sensor fusion positioning method in another embodiment;

[0064] Figure 4 The robot's motion trajectory is approximated as a circular arc schematic diagram based on the velocity motion model of the probabilistic robot;

[0065] Figure 5 is a schematic diagram of a sliding window based on a segment elimination algorithm;

[0066] Figure 6 It is a structural block diagram of a robot multi-sensor fusion positioning device in one embodiment;

[0067] Figure 7 FIG. 4 is a diagram showing the internal structure of a computer device in one embodiment. DETAILED DESCRIPTION

[0068] In order to make the purpose, technical solution and advantages of the present application more clearly understood, the present application is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and are not used to limit the present application.

[0069] The robot multi-sensor fusion positioning method provided in the embodiment of the present application can be applied to Figure 1 In the application environment shown. Among them, the terminal 102 communicates with the server 104 through the network. The data storage system can store the data that the server 104 needs to process. The data storage system can be integrated on the server 104, or it can be placed on the cloud or other network servers. The terminal 102 sends a fusion positioning request to the server 104, and the server 104 responds to the fusion positioning request to obtain the prior map of the robot's preset route; real-time collection of multi-sensor data of multiple types of sensors of the robot during the preset route, and based on the multi-sensor data, real-time acquisition of current position information; pre-processing and fusion of local sensor data in the multi-sensor data to obtain local pose estimation results, and based on the current position information and the prior map, obtain the initial sensor weight at the current position; according to the strength of the corresponding signals of different types of sensors in the multi-sensor data, dynamically adjust the initial sensor weight to obtain the adjusted sensor weight; based on the global data, local pose estimation results and adjusted sensor weights in the multi-sensor data, fusion is performed to obtain the global positioning information of the robot. Further, the server 104 can send the global positioning information to the terminal 102. The terminal 102 may be, but is not limited to, various personal computers, laptops, smart phones, tablet computers, IoT devices, and portable wearable devices. The IoT devices may be smart speakers, smart TVs, smart air conditioners, smart car-mounted devices, etc. The portable wearable devices may be smart watches, smart bracelets, head-mounted devices, etc. The server 104 may be implemented as an independent server or a server cluster consisting of multiple servers.

[0070] In one embodiment, Figure 2 As shown, a robot multi-sensor fusion positioning method is provided, and the method is applied to Figure 1 Taking the server 104 in the example as an example, the following steps are included:

[0071] S100: Obtain a priori map of a preset route for the robot to travel, where the priori map represents the correlation between position information and factor information, where the factor information represents the weights of different types of sensors at different positions, and where the weights represent the contribution ratio to the positioning accuracy.

[0072] The preset route refers to the route that the robot pre-sets during driving, which can be set according to the actual needs. The prior map can be pre-built, or it can be temporarily built based on data collected by different types of sensors. Specifically, the data collected by different types of sensors during the robot's driving on the preset route can be collected, and then the prior map can be built based on the position data during the entire driving process and the data on the contribution of different types of sensors to the final positioning accuracy.

[0073] S200: Collect multi-sensor data of various types of sensors of the robot in real time while the robot is traveling along a preset route, and obtain current position information in real time based on the multi-sensor data.

[0074] The sensor data of these different types of sensors are collected in real time during the actual driving process of the robot, and the current position information is obtained based on these sensor data for rough positioning. The current position information is a relatively rough position information, and its role is to obtain the initial sensor weight based on the prior map in subsequent processing. The process of obtaining the current position information based on multi-sensor data here can adopt the conventional multi-sensor fusion positioning method.

[0075] S300: Preprocess and fuse local sensor data in the multi-sensor data to obtain a local pose estimation result, and obtain an initial sensor weight at the current position based on the current position information and the prior map.

[0076] The local sensor data mainly includes the data collected by the wheel encoder and the data collected by the IMU, specifically the linear velocity v output by the wheel encoder and the angular velocity ω output by the IMU. This part of the data is preprocessed and fused to obtain the local pose estimation result of the robot. And based on the current position information and the prior map, the initial sensor weight of the current position is obtained. The initial sensor weight is used to characterize the weights considered based on different types of sensor data in the conventional multi-sensor fusion positioning process.

[0077] S400: dynamically adjusting initial sensor weights according to the strengths of signals corresponding to different types of sensors in the multi-sensor data to obtain adjusted sensor weights.

[0078] The initial sensor weight is dynamically adjusted according to the strength of the corresponding signals of different types of sensors actually collected to obtain the adjusted sensor weight. For example, when the GPS signal is weak, the system automatically reduces the GPS factor weight and increases the weight of the IMU and encoder to ensure positioning accuracy.

[0079] S500: The global positioning information of the robot is obtained by fusing the global data, the local pose estimation results and the adjusted sensor weights in the multi-sensor data.

[0080] Data fusion is performed based on the global data, local pose estimation results and adjusted sensor weights in multi-sensor data. Specifically, global factors can be extracted from the global data, local factors from the local pose estimation results, and the influence of sensor weights can be considered for data fusion, factor graph optimization can be performed, and finally global positioning information can be obtained.

[0081] The above robot multi-sensor fusion positioning method obtains a priori map of the preset route of the robot, the priori map represents the correlation between position information and factor information, the factor information represents the weights of different types of sensors at different positions, and the weight represents the contribution ratio to positioning accuracy; real-time multi-sensor data of multiple types of sensors of the robot during the preset route are collected, and the current position information is obtained in real time based on the multi-sensor data; pre-processing and fusion are performed according to the local sensor data in the multi-sensor data to obtain the local pose estimation result, and based on the current position information and the priori map, the initial sensor weight at the current position is obtained; according to the strength of the corresponding signal of different types of sensors in the multi-sensor data, the initial sensor weight is dynamically adjusted to obtain the adjusted sensor weight; based on the global data, the local pose estimation result and the adjusted sensor weight in the multi-sensor data, the fusion is performed to obtain the global positioning information of the robot. In the whole process, the priori map is introduced to realize the dynamic adjustment of the weights of different types of sensors in the fusion process, and the weight represents the contribution ratio to the positioning accuracy, so the fusion positioning is more accurate, and accurate fusion positioning can be achieved.

[0082] like Figure 3 As shown, in one embodiment, S100 includes:

[0083] S120: Acquire sensor data collected by sensors of different types respectively to obtain a sensor data set.

[0084] Different types of sensors mainly include GPS sensors, IMU, wheel encoders, etc. Among them, GPS sensors: provide global position p gps (t); IMU (Inertial Measurement Unit): provides angular velocity ω imu (t) and acceleration a imu(t); Wheel encoder: provides wheel rotation data v encoder (t) is used to estimate the driving speed.

[0085] S140: Using an extended Kalman filter to perform data fusion on the sensor data set to determine the position corresponding to each acquisition point.

[0086] Data fusion can use the extended Kalman filter (EKF) to estimate the state of the robot. The state vector of the robot is defined as:

[0087] X=[xyz Φ θ ψ] T

[0088] Among them, x, y, z are positions, Φ, θ, ψ are yaw angle, pitch angle, and roll angle respectively. The state evolution equation can be expressed as:

[0089] X(t+1)=f(X(t),u(t))+w(t)

[0090] Where f is the state transfer function, u(t) is the control input (including the acceleration and angular velocity of the IMU), and w(t) is Gaussian noise. Every time new sensor data is collected, the prediction and update steps of the extended Kalman filter are used to fuse the measurement results of multiple sensors:

[0091] Prediction Steps: Predict the new state. Update step: Update the state estimate through the Kalman gain K(t):

[0092] K(t)=P(t)H T (t)(H(t)P(t)H T (t)+R(t)) -1

[0093]

[0094] Where z(t) is the sensor measurement, H(t) is the observation matrix, and R(t) is the measurement noise covariance matrix.

[0095] S160: extracting the contribution ratio of different types of sensors to the positioning accuracy during the data fusion process, and obtaining the sensor weight corresponding to each acquisition point.

[0096] Different methods are used to extract sensor weights for different types of sensors. Specifically, during the data fusion process, the system will evaluate the contribution of each sensor at different locations. The weight factor of the sensor is defined as w i , where i represents different sensors, such as GPS, IMU, wheel encoder, etc. The weights are assigned based on the sensor signal quality and environmental conditions. Assume qi represents the standardized performance index of the sensor, then The specific weight extraction scheme is:

[0097] GPS weight extraction: GPS signal quality can be evaluated by signal-to-noise ratio (SNR) and HDOP. The lower the HDOP, the higher the positioning accuracy. Use the HDOP value to determine the GPS weight, the formula is:

[0098]

[0099] Then q GPS Normalize to weights:

[0100]

[0101] IMU weight extraction: IMU mainly provides attitude information by measuring angular velocity and acceleration. The main problem is drift. We evaluate the drift rate σ of the gyroscope gyro and accelerometer noise σ accel To calculate the factor weight of the IMU:

[0102]

[0103] Similarly, the normalized weights are:

[0104] Wheel encoder weight extraction: Wheel encoders rely on friction with the ground. If the road is slippery or muddy, the encoder accuracy will decrease. Therefore, by measuring the wheel rotation error ε encoder To determine the weight, defined as:

[0105]

[0106] Similarly, the normalized weights are:

[0107] Other sensor weight extraction: Other sensors (such as lidar or visual sensors) can also determine weights based on their characteristics. For example, the ranging accuracy of lidar may be affected by ambient lighting or obstacle density, so weights can be assigned based on its ranging accuracy or signal strength. The final prior "map" contains the location information x of each point and the weight w of each sensor i .

[0108] S180: Associating the positions corresponding to the collection points with the sensor weights to generate a priori maps.

[0109] For each collection point, its corresponding position is associated with the sensor weight to generate a priori map.

[0110] In one of the embodiments, preprocessing and fusing local sensor data in multi-sensor data to obtain a local pose estimation result includes: extracting local sensor data in the multi-sensor data; estimating the state of the robot using a Kalman filter based on the local sensor data to obtain a local pose estimation result.

[0111] Filter out the local sensor data related to the current task or scene from the multi-sensor dataset, and select the sensor data that is most helpful for local pose estimation according to the task requirements. For example, in obstacle detection tasks, lidar data may be more critical; while in image recognition tasks, camera data is more important. After extracting the local sensor data, a Kalman filter can be used to estimate the state of the robot to obtain the local pose estimation result. The Kalman filter is an efficient recursive filter that can estimate the state of the system through prediction and update steps in the presence of noise and uncertainty.

[0112] In one embodiment, the state of the robot is estimated using a Kalman filter according to local sensor data to obtain a local pose estimation result including:

[0113] Step 1: Use the linear velocity output by the wheel encoder and the angular velocity output by the IMU as initial conditions.

[0114] The linear velocity v output by the wheel encoder and the angular velocity ω output by the IMU are used as initial conditions.

[0115] Step 2: Approximate the robot's motion trajectory as an arc.

[0116] Here, two different methods are used to approximate the robot's motion trajectory as a circular arc according to the size of the angular velocity change. Specifically, it is determined whether the angular velocity change ωΔt is less than the threshold. If it is not less than the threshold, the robot's motion trajectory is approximated as a circular arc based on the probability robot's velocity motion model. The schematic diagram is shown in Figure 4 As shown. The pose expression is:

[0117]

[0118] If the angle change is less than the threshold, the Runge-Kutta second-order integral with smaller error is used for approximation. The pose expression at this time is:

[0119]

[0120] Step 3: In the arc, based on the mileage calculation motion model of the probabilistic robot, the current robot posture is updated considering the influence of motion noise to obtain the current posture after noise reduction.

[0121] Then, based on the probabilistic robot's odometer motion model, the robot's current position and posture are updated considering the influence of motion noise. The expression is:

[0122]

[0123] This model regards the robot movement as a rotation-translation-rotation process. and Represents the true value of the relative motion parameter of the process. The true value is equal to the measured value minus the motion noise. The expression is:

[0124]

[0125] The denoised pose at the current moment will be passed back to the integration process of the previous step as the initial condition.

[0126] Step 4: Construct an IMU observation model and obtain the relative changes in position, velocity, and rotation between two odometer frames based on the IMU observation model.

[0127] Where R w Represents the transformation from the world coordinate system to the IMU body coordinate system. k and b k+1 The time interval [t k , t k+1 ], there are several IMU measurements, which are integrated using the following formula:

[0128] in It can be regarded as the relative change of position, speed, and rotation between two odometer frames.

[0129] Step 5: Obtain the IMU relative constraints based on the relative changes in position, velocity, and rotation between the two odometer frames.

[0130] Based on the above formula, it can be found that the integral result is only related to the measured value and the bias, and the bias estimate is also dynamically changing. If the integration is performed again every time the bias changes, it is very complicated and wastes computing resources. Therefore, when the bias changes slightly, the integral result is directly approximated as the first-order differential of the bias. The linear relationship is:

[0131]

[0132] Step 6: Fuse the denoised current pose and IMU relative constraints to obtain the local pose estimate WIO.

[0133] Then, the denoised local pose estimate obtained in step 3 is fused with the IMU relative constraint obtained in step 5 through a Kalman filter to obtain a more accurate local pose estimate WIO.

[0134] In one embodiment, the global data includes GNSS data; based on the fusion of the global data, the local pose estimation results and the adjusted sensor weights in the multi-sensor data, the global positioning information of the robot includes:

[0135] The GNSS data is loosely coupled with the local pose estimation WIO to obtain a loosely coupled result; the GNSS factor is extracted from the loosely coupled result, and the WIO factor is extracted from the local pose estimation WIO; factor modeling is performed based on the GNSS factor and the WIO factor; the factor modeling result is optimized based on the sliding window of segment elimination to obtain the global positioning information of the robot.

[0136] The state quantities that need to be estimated in the graph optimization process are mainly the three-dimensional coordinates and posture of the robot, which can be expressed as follows:

[0137] χ=[p0, R0, p1, R1,…,p n , R n ]

[0138] p and R represent the three-dimensional coordinates and posture information of the robot in the world coordinate system respectively. The essence of state estimation is the maximum likelihood estimation (MLE) problem. MLE consists of the joint probability distribution of the robot posture within a certain period of time, assuming that all measurements are independent and the measurement uncertainty follows a Gaussian distribution. The problem is transformed into a nonlinear least squares problem, expressed as follows:

[0139]

[0140] Where S represents the set of measurements from WIO and GNSS, z and h represent the measured value and estimated value respectively. Then, the weight values ​​of each sensor factor obtained in the prior "map" are called to obtain:

[0141]

[0142] Process the GNSS raw data, filter out the bad satellite signals, and perform simple loose coupling with WIO, and then perform factor modeling. The GNSS factor is a global constraint. When using ceres for graph optimization, Terror needs to be used to generate a global residual, which is defined as:

[0143]

[0144] in represents the measurement value of GNSS at time t, represents the predicted value at time t, and They represent the position information received by GNSS at time t and the position information after loose coupling with WIO, which means that our final optimization result is closer to the GNSS measurement value.

[0145] Since the WIO factor depends on two adjacent states, it is a local constraint. When using ceres optimization, it is necessary to use RelativeRTerror to generate a local residual, which is defined as:

[0146]

[0147] in represents the WIO posture information at time t, It represents the optimized pose information at time t. The residual means the difference between the pose increment of two adjacent frames WIO and the pose increment after optimization, which means that the relative increments of the two should be as equal as possible.

[0148] In the final factor graph optimization process, a sliding window based on the segment elimination algorithm is used. Figure 5 is a schematic diagram of the sliding window. Figure 5 As shown in the figure, assuming that the size of the sliding window is 3 and the unit size of each segment is 4, in the change from time t0 to time t1, the first state x0 of the first segment moves out of the sliding window. At this time, the relative constraints related to x0 will be retained in the sliding window and continue to participate in the optimization; similarly, in the change from time t4 to time t5, the first state x4 of the second segment is removed from the sliding window. At this time, the relative constraints related to x4 need to be retained. Based on the above description, the state quantity and constraints can be obtained:

[0149]

[0150] e1=g(ξ0,ξ1)

[0151]

[0152] Among them, ξ i represents the i-th posture state, represents the GNSS state corresponding to the i-th state, function g represents the WIO relative constraint, and function f represents the GNSS global constraint.

[0153] Taking the Gauss-Newton method as an example, the final optimization equation can be obtained:

[0154]

[0155] J i Indicates e iThe derivative of the residual, based on a strategic rearrangement and Schur complement of the variables to be eliminated and retained, yields a new equation:

[0156]

[0157] H p and b p Represents the prior information of the state retained after elimination. r Represents the state quantity retained after elimination. Finally, according to Bayes' rule, the posterior probability can be expressed as the product of the prior probability and the likelihood. The state estimation problem becomes a MAP problem. We assume that the state from "frame m" to "frame n" is maintained in the sliding window, then the MAP problem is written as:

[0158]

[0159] Based on the strategy of retaining the relevant constraints of the first state of each segment, the number of Gauss-Newton iterations in the optimization can be reduced while avoiding discarding the constraint information, thereby improving the accuracy and robustness of the fusion results.

[0160] In general, in practical applications, the robot multi-sensor fusion positioning method of the present application has the following significant advantages.

[0161] Advantage 1: The introduction of prior maps enables dynamic weight adjustment, improves positioning accuracy, and enhances environmental adaptability.

[0162] In traditional technologies, in complex environments (such as cities, mountains, forests, etc.), sensors are greatly disturbed by the external environment, and positioning accuracy is difficult to guarantee. In addition, traditional multi-sensor fusion systems are usually unable to effectively utilize prior information about the environment and can only rely on real-time sensor data for positioning. This reliance on real-time data can easily lead to accumulation of positioning errors in scenarios with complex environments and large fluctuations in sensor performance. In addition, traditional multi-sensor fusion positioning systems usually use fixed weights to fuse sensor data. However, different sensors perform differently in different environments, and fixed weights can easily lead to excessive weights for some sensors in certain environments, affecting the overall positioning accuracy. For example, GPS will show large errors in urban environments with severe occlusion, and inertial measurement units (IMUs) will have drift problems after long-term operation.

[0163] This application introduces the concept of a priori map and proposes a dynamic sensor weight adjustment mechanism. By collecting data from each sensor on a specific route in advance, a priori map with location information and sensor factor information is constructed, and the weight of the sensor is dynamically adjusted according to the actual environmental conditions. This mechanism ensures that the data of each sensor can be weighted according to its real-time reliability in different environments, avoiding the limitations of fixed weights. This dynamic adjustment of weights ensures that the system can adaptively adjust the contribution ratio of the sensor according to different environmental conditions, thereby significantly improving the overall positioning accuracy.

[0164] Advantage 2: Lightweight design, suitable for resource-constrained scenarios

[0165] In traditional technologies, multi-sensor fusion algorithms (such as extended Kalman filter EKF, particle filter PF, graph optimization, etc.) usually have high computational complexity, especially when processing large amounts of data from multiple sensors, which requires high computing resources. This makes it difficult for these systems to run efficiently on embedded devices or resource-constrained hardware platforms.

[0166] This application scheme significantly reduces the system's demand for computing resources through lightweight algorithm design. Its main technical measure is to introduce a segment elimination algorithm in the graph optimization stage. On the basis of segmentation, based on the strategy of retaining the relevant constraints of the first state of each segment, it can reduce the number of Gauss-Newton iterations in the optimization while avoiding discarding constraint information, greatly reducing the computational burden.

[0167] Advantage 3: Modular design that is easy to expand

[0168] Traditional multi-sensor fusion systems are usually complex and difficult to expand or adapt to new sensor requirements. For example, when adding new sensors (such as lidar and visual sensors), the system needs to be modified a lot, making it difficult to flexibly respond to different application scenarios.

[0169] This application scheme adopts a modular design and supports flexible integration of multiple sensors. The system can easily add or reduce sensors according to actual needs, and the sensor weight adjustment and data fusion algorithm can be flexibly configured. Its advantages include:

[0170] Scalability of sensor types: In addition to common sensors such as GPS, IMU, encoders, etc., this application can be expanded to new sensors such as lidar, visual sensors, etc. to adapt to a variety of application scenarios. Flexibility of algorithms: The sensor fusion algorithm of this application supports a variety of data fusion methods such as extended Kalman filtering and particle filtering, and the fusion strategy can be switched or adjusted according to actual application requirements. This modular design enhances the scalability and flexibility of the system and solves the problem of complex and difficult to expand systems in the background technology.

[0171] It should be understood that, although the steps in the flowcharts involved in the above embodiments are displayed in sequence according to the indication of the arrows, these steps are not necessarily executed in sequence according to the order indicated by the arrows. Unless there is a clear explanation in this article, the execution of these steps is not strictly limited in order, and these steps can be executed in other orders. Moreover, at least a part of the steps in the flowcharts involved in the above embodiments may include multiple steps or multiple stages, and these steps or stages are not necessarily executed at the same time, but can be executed at different times, and the execution order of these steps or stages is not necessarily carried out in sequence, but can be executed in turn or alternately with other steps or at least a part of the steps or stages in other steps.

[0172] Based on the same inventive concept, the embodiment of the present application also provides a robot multi-sensor fusion positioning device for implementing the robot multi-sensor fusion positioning method involved above. The implementation scheme for solving the problem provided by the device is similar to the implementation scheme recorded in the above method, so the specific limitations in one or more robot multi-sensor fusion positioning device embodiments provided below can refer to the above limitations on the robot multi-sensor fusion positioning method, which will not be repeated here.

[0173] In one embodiment, Figure 6 As shown, a robot multi-sensor fusion positioning device is provided, comprising:

[0174] A priori map acquisition module 100 is used to obtain a priori map of a preset route for the robot to travel. The priori map represents the association between position information and factor information. The factor information represents the weights of different types of sensors at different positions. The weights represent the contribution ratio to the positioning accuracy.

[0175] The parameter acquisition module 200 is used to collect multi-sensor data of various types of sensors of the robot in real time during the driving process of the preset route, and obtain the current position information in real time based on the multi-sensor data;

[0176] The initial fusion processing module 300 is used to preprocess and fuse the local sensor data in the multi-sensor data to obtain the local pose estimation result, and obtain the initial sensor weight at the current position based on the current position information and the prior map;

[0177] The weight adjustment module 400 is used to dynamically adjust the initial sensor weight according to the strength of the corresponding signal of different types of sensors in the multi-sensor data to obtain the adjusted sensor weight;

[0178] The fusion positioning module 500 is used to obtain the global positioning information of the robot based on the global data, the local pose estimation results and the adjusted sensor weights in the multi-sensor data.

[0179] In one of the embodiments, the prior map acquisition module 100 is also used to separately acquire sensor data collected by different types of sensors to obtain a sensor data set; use an extended Kalman filter to fuse the sensor data set to determine the position corresponding to each collection point; extract the contribution ratio of different types of sensors to the positioning accuracy during the data fusion process to obtain the sensor weight corresponding to each collection point; associate the position corresponding to the collection point with the sensor weight to generate a prior map.

[0180] In one embodiment, the initial fusion processing module 300 is further used to extract local sensor data from the multi-sensor data; a Kalman filter is used to estimate the state of the robot based on the local sensor data to obtain a local pose estimation result.

[0181] In one of the embodiments, the initial fusion processing module 300 is also used to use the linear velocity output by the wheel encoder and the angular velocity output by the IMU as initial conditions; approximate the motion trajectory of the robot as a circular arc; update the current robot's posture in the circular arc based on the mileage calculation motion model of the probabilistic robot taking into account the influence of motion noise to obtain the current posture after noise reduction; construct an IMU observation model, and obtain the relative changes in position, speed and rotation between two odometer frames based on the IMU observation model; obtain the IMU relative constraint based on the relative changes in position, speed and rotation between the two odometer frames; fuse the current posture after noise reduction and the IMU relative constraint to obtain a local posture estimate WIO.

[0182] In one embodiment, the initial fusion processing module 300 is also used to determine whether the angle change is less than a preset threshold; if not, the robot's motion trajectory is approximated as a circular arc based on the probabilistic robot's velocity motion model; if so, the Runge-Kutta second-order integral is used for approximation processing to approximate the robot's motion trajectory as a circular arc.

[0183] In one of the embodiments, the global data includes GNSS data; the fusion positioning module 500 is also used to loosely couple the GNSS data with the local pose estimation WIO to obtain a loosely coupled result; extract the GNSS factor from the loosely coupled result, and extract the WIO factor from the local pose estimation WIO; perform factor modeling based on the GNSS factor and the WIO factor; optimize the factor modeling result based on the sliding window of segment elimination to obtain the global positioning information of the robot.

[0184] Each module in the above-mentioned robot multi-sensor fusion positioning device can be implemented in whole or in part by software, hardware and their combination. Each of the above-mentioned modules can be embedded in or independent of the processor in the computer device in the form of hardware, or can be stored in the memory of the computer device in the form of software, so that the processor can call and execute the corresponding operations of each of the above modules.

[0185] In one embodiment, a computer device is provided. The computer device may be a server, and its internal structure diagram may be as follows: Figure 7 As shown. The computer device includes a processor, a memory and a network interface connected through a system bus. Among them, the processor of the computer device is used to provide computing and control capabilities. The memory of the computer device includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system, a computer program and a database. The internal memory provides an environment for the operation of the operating system and the computer program in the non-volatile storage medium. The database of the computer device is used to store preset data. The network interface of the computer device is used to communicate with an external terminal through a network connection. When the computer program is executed by the processor, a robot multi-sensor fusion positioning method is implemented.

[0186] Those skilled in the art will understand that Figure 7 The structure shown in the figure is only a block diagram of a part of the structure related to the solution of the present application, and does not constitute a limitation on the computer device to which the solution of the present application is applied. The specific computer device may include more or fewer components than those shown in the figure, or combine certain components, or have a different arrangement of components.

[0187] In one embodiment, a computer device is provided, including a memory and a processor, wherein a computer program is stored in the memory, and the processor implements the above-mentioned robot multi-sensor fusion positioning method when executing the computer program.

[0188] In one embodiment, a computer-readable storage medium is provided, on which a computer program is stored. When the computer program is executed by a processor, the above-mentioned robot multi-sensor fusion positioning method is implemented.

[0189] In one embodiment, a computer program product is provided, including a computer program, which implements the above-mentioned robot multi-sensor fusion positioning method when executed by a processor.

[0190] Those of ordinary skill in the art can understand that all or part of the processes in the above-mentioned embodiment methods can be completed by instructing the relevant hardware through a computer program, and the computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the embodiments of the above-mentioned methods. Among them, any reference to the memory, database or other medium used in the embodiments provided in the present application can include at least one of non-volatile and volatile memory. Non-volatile memory can include read-only memory (ROM), magnetic tape, floppy disk, flash memory, optical memory, high-density embedded non-volatile memory, resistive random access memory (ReRAM), magnetoresistive random access memory (MRAM), ferroelectric random access memory (FRAM), phase change memory (PCM), graphene memory, etc. Volatile memory can include random access memory (RAM) or external cache memory, etc. As an illustration and not limitation, RAM can be in various forms, such as static random access memory (SRAM) or dynamic random access memory (DRAM). The database involved in each embodiment provided in this application may include at least one of a relational database and a non-relational database. Non-relational databases may include distributed databases based on blockchains, etc., but are not limited thereto. The processor involved in each embodiment provided in this application may be a general-purpose processor, a central processing unit, a graphics processor, a digital signal processor, a programmable logic device, a data processing logic device based on quantum computing, etc., but are not limited thereto.

[0191] The technical features of the above embodiments may be combined arbitrarily. To make the description concise, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0192] The above-described embodiments only express several implementation methods of the present application, and the descriptions thereof are relatively specific and detailed, but they cannot be understood as limiting the scope of the present application. It should be pointed out that, for a person of ordinary skill in the art, several variations and improvements can be made without departing from the concept of the present application, and these all belong to the protection scope of the present application. Therefore, the protection scope of the present application shall be subject to the attached claims.

Claims

1. A robot multi-sensor fusion positioning method, characterized in that: The method comprises: Obtain a priori map of the robot's preset driving route, wherein the priori map represents the association between position information and factor information, wherein the factor information represents the weights of different types of sensors at different positions, and wherein the weights represent the contribution ratio to the positioning accuracy; Collecting multi-sensor data of various types of sensors of the robot in real time during the driving process of the preset route, and acquiring current position information in real time based on the multi-sensor data; Preprocessing and fusing local sensor data in the multi-sensor data to obtain a local pose estimation result, and acquiring an initial sensor weight at the current position based on the current position information and the prior map; Dynamically adjusting the initial sensor weight according to the strength of the signals corresponding to the different types of sensors in the multi-sensor data to obtain an adjusted sensor weight; The global positioning information of the robot is obtained based on the fusion of the global data in the multi-sensor data, the local pose estimation result and the adjusted sensor weight.

2. The method according to claim 1, characterized in that The obtaining of a priori map of a preset route for the robot to travel comprises: Respectively obtain sensor data collected by different types of sensors to obtain a sensor data set; Using an extended Kalman filter to perform data fusion on the sensor data set to determine the position corresponding to each collection point; Extract the contribution ratio of different types of sensors to positioning accuracy during data fusion and obtain the sensor weight corresponding to each acquisition point; The positions corresponding to the collection points are associated with the sensor weights to generate a priori maps.

3. The method according to claim 1, characterized in that Preprocessing and fusing the local sensor data in the multi-sensor data to obtain the local pose estimation result includes: Extracting local sensor data from the multi-sensor data; The state of the robot is estimated by using a Kalman filter according to the local sensor data to obtain a local posture estimation result.

4. The method according to claim 3, characterized in that The method of estimating the state of the robot using a Kalman filter according to the local sensor data to obtain a local posture estimation result includes: The linear velocity output by the wheel encoder and the angular velocity output by the IMU are used as initial conditions; Approximate the robot's motion trajectory as an arc; In the arc, based on the mileage calculation motion model of the probabilistic robot, the current robot posture is updated by considering the influence of motion noise to obtain the current posture after noise reduction; Constructing an IMU observation model, and obtaining relative changes in position, velocity, and rotation between two odometer frames based on the IMU observation model; Obtaining an IMU relative constraint based on a relative change in position, velocity, and rotation between the two odometer frames; The de-noised current moment pose and the IMU relative constraint are fused to obtain a local pose estimate WIO.

5. The method according to claim 4, characterized in that Approximating the robot's motion trajectory to an arc includes: Determine whether the angle change is less than a preset threshold; If not, the robot's motion trajectory is approximated as an arc based on the velocity motion model of the probabilistic robot; If so, the Runge-Kutta second-order integral is used for approximation, and the robot's motion trajectory is approximated as a circular arc.

6. The method according to claim 4, characterized in that The global data includes GNSS data; the global positioning information of the robot obtained by fusing the global data in the multi-sensor data, the local pose estimation result and the adjusted sensor weight includes: Loosely couple the GNSS data with the local pose estimation WIO to obtain a loosely coupled result; Extracting a GNSS factor from the loosely coupled result and extracting a WIO factor from the local pose estimation WIO; Performing factor modeling based on the GNSS factor and the WIO factor; The factor modeling results are optimized based on the sliding window of segment elimination to obtain the global positioning information of the robot.

7. A robot multi-sensor fusion positioning device, characterized in that: The device comprises: A priori map acquisition module is used to obtain a priori map of the robot's preset route, wherein the priori map represents the association between position information and factor information, wherein the factor information represents the weights of different types of sensors at different positions, and wherein the weights represent the contribution ratio to the positioning accuracy; A parameter acquisition module, used to collect multi-sensor data of various types of sensors of the robot in real time during the driving process of the preset route, and obtain current position information in real time based on the multi-sensor data; An initial fusion processing module, used to preprocess and fuse local sensor data in the multi-sensor data to obtain a local pose estimation result, and obtain an initial sensor weight at the current position based on the current position information and the prior map; A weight adjustment module, used to dynamically adjust the initial sensor weight according to the strength of the corresponding signals of different types of sensors in the multi-sensor data to obtain an adjusted sensor weight; The fusion positioning module is used to obtain the global positioning information of the robot based on the global data in the multi-sensor data, the local pose estimation result and the adjusted sensor weight.

8. A computer device comprising a memory and a processor, wherein the memory stores a computer program, wherein: When the processor executes the computer program, the steps of the method according to any one of claims 1 to 6 are implemented.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the steps of the method according to any one of claims 1 to 6 are implemented.

10. A computer program product, comprising a computer program, characterized in that When the computer program is executed by a processor, the steps of the method according to any one of claims 1 to 6 are implemented.

Images