Mobile robot positioning method based on multi-sensor combination

Abstract

The invention discloses a mobile robot positioning method based on multi-sensor combination, and belongs to the technical field of outdoor mobile robot positioning, and the method comprises the following steps: S1, sensor parameter calibration; s2, data acquisition and preprocessing; and S3, data fusion positioning. The method can adapt to various complex environments such as indoor and outdoor coexistence and multiple obstacles, continuously provides reliable and high-precision positioning information for the autonomous service robot, has the advantages of low cost, simple layout, good real-time performance, high reliability and the like, and can be widely applied to various autonomous robots.

Description

Technical Field

[0001] The present invention relates to the technical field of outdoor mobile robot positioning, and particularly relates to a mobile robot positioning method based on multi-sensor combination. Background Art

[0002] In an outdoor working environment, an autonomous mobile robot not only has a wide range of carrying capabilities but also has high flexibility, and can be used as an effective tool to help people complete specific tasks. In fields such as industry and service, their presence can be found, and mobile robots have been widely integrated into real life. In mobile robot technology, the positioning system is extremely important, and it is an indispensable part of the mobile robot in the process of perceiving its own position and performing path planning. Accurate and stable positioning is one of the key factors to ensure that the robot can accurately complete navigation and control tasks. Due to the uncertainty of the environment and the target itself, the mobile robot cannot fully master the surrounding environmental information, resulting in many problems in aspects such as positioning and navigation. Therefore, the research on mobile robot positioning and navigation related technologies has received extensive attention from many researchers.

[0003] Currently, in the process of navigation and control of outdoor mobile robots, various sensors and related positioning technologies are relied on to achieve their own precise positioning functions. Previous positioning and navigation technologies mainly included absolute positioning mainly based on GPS and relative positioning mainly based on IMU and odometer. To achieve the tasks of robot autonomous positioning and navigation, researchers fuse the data obtained by multiple sensors to achieve the robot's own positioning. GPS can provide positioning and navigation information 24 hours a day, but when there are large buildings blocking, the signal transmission will have errors, resulting in information transmission distortion. The INS (Inertial Navigation System) selects sensors that are not affected by external interference, namely IMU, so the positioning error it has in a short time is not large. However, when the time is too long, the system needs to process the error to obtain accurate positioning information. A pure visual navigation system uses a camera as a sensor to collect environmental information and processes the acquired pictures when the robot is moving. This method does not have cumulative errors, can obtain higher accuracy, and the visual sensor has a small volume, low price, high accuracy, and can also obtain the characteristic information of the surrounding environment, far better than INS. However, if it is interfered by rain, snow or other bad weather, it is very difficult for the visual navigation system to handle such situations. Considering the uncertainty of the outdoor environment, it is also difficult to ensure the accuracy of positioning with a single visual sensor for navigation.

[0004] Therefore, comprehensively using multiple sensors for comprehensive analysis and judgment to achieve optimal positioning has become a key research direction for mobile robots. For this purpose, a mobile robot positioning method based on multi-sensor combination is proposed. Summary of the Invention

[0005] The technical problem to be solved by the present invention is: how to fuse based on multi-sensor data for accurate positioning of outdoor mobile robots, and then meet the accuracy requirements of outdoor mobile robots in terms of positioning and navigation. A mobile robot positioning method based on multi-sensor combination is provided, which adopts a positioning method combining GPS, wheel odometer, binocular vision camera and lidar, can adapt to various complex environments such as coexistence of indoor and outdoor and multiple obstacles, continuously provides reliable and high-precision position information for autonomous service robots, and has the advantages of low cost, simple layout, good real-time performance, high reliability, etc., and can be widely applied to various autonomous robots.

[0006] The present invention solves the above technical problems through the following technical solutions. The present invention includes the following steps:

[0007] S1: Sensor parameter calibration

[0008] Calibrate the parameters of each sensor. The sensors include GPS, inertial measurement unit, binocular depth camera, lidar and wheel odometer;

[0009] S2: Data acquisition and preprocessing

[0010] Use the above calibrated sensors to perform data acquisition work, and classify and preprocess the data collected by each sensor;

[0011] S3: Data fusion positioning

[0012] Fuse the preprocessed data of GPS, inertial measurement unit, lidar and wheel odometer through the extended Kalman filter algorithm to obtain the pose information after the fusion of the extended Kalman filter algorithm, and then perform weighted fusion with the preprocessed data of the binocular depth camera again, and finally output the final positioning information of the mobile robot.

[0013] Further, in the step S1, the parameter calibration process of GPS is completed by using time synchronization correction and frequency error correction methods; the parameter calibration of the accelerometer, gyroscope, and magnetometer in the inertial measurement unit is respectively carried out to complete the parameter calibration process of the inertial measurement unit; the external parameters of the lidar are calibrated to complete the parameter calibration process of the lidar; the stereo parameters of the binocular depth camera are calibrated by using the Bouguet algorithm to complete the parameter calibration process of the binocular depth camera; by recording the specific values of the left and right wheel encoder counts when the mobile robot moves along three circular paths of different lengths, the parameter calibration process of the wheel odometer is realized.

[0014] Further, in the step S2, the data preprocessing process is as follows:

[0015] S21: The data collected by GPS is calculated and processed by using the pseudorange differential method, and the obtained data is interpolated by using the Neville interpolation method to obtain the first position information;

[0016] S22: The data collected by the inertial measurement unit is denoised by using the sliding window averaging method to obtain the first pose information;

[0017] S23: The ranging information of the lidar is obtained by using the principle of triangular ranging and the ToF ranging method; by fusing the methods of median filtering, limit filtering, and arithmetic mean filtering, the noise reduction processing of the ranging information is realized to obtain the second position information;

[0018] S24: The data collected by the binocular depth camera is preprocessed to obtain the second pose information;

[0019] S25: The data collected by the wheel odometer is processed by using the differential kinematic model to obtain the third pose information.

[0020] Further, in the step S21, the specific processing process is as follows:

[0021] S211: The data collected by GPS is calculated and processed by using the pseudorange differential method, and the specific process is as follows:

[0022] First, calculate the actual distance, and then compare the observed distance with the actual distance to determine the difference to be corrected, that is, obtain the correction value;

[0023] Then, the correction value is sent to GPS through wireless transmission to correct the data and obtain a distance value closer to the actual value, that is, obtain the true distance from the robot to the observed satellite;

[0024] S212: Use Neville interpolation method to interpolate the continuous data points collected by GPS in step S211. The continuous data points collected by GPS are the true distance data points from the robot to the observed satellite. The specific process is as follows:

[0025] S2121: Calculate the number of data points n to be inserted between the continuous data points collected by GPS:

[0026]

[0027] Among them, (x0, y0), (x1, y1) are the coordinates of adjacent data points collected by GPS;

[0028] S2122: Use Neville's algorithm to obtain a point between (x0, y0) and (x1, y1):

[0029]

[0030] S2122: At the obtained interpolation point (x 0,1 , y 0,1 ), perform interpolation with the original data points (x0, y0) and (x1, y1) to obtain:

[0031]

[0032] Through iteration, the iterative formula can be obtained:

[0033]

[0034] Through the above formula, n interpolation points can be obtained. Combine the interpolation points with the original data points to obtain the first position information.

[0035] Furthermore, in the step S211, the calculation process of the correction value is as follows:

[0036] S2111: Set the actual distance from the GPS reference station to the observed satellite as

[0037]

[0038] Among them, (x s , y s , z s ) is the spatial coordinate information of the observed satellite, and (x g , y g , z g ) is the spatial coordinate information of the GPS reference station;

[0039] S2112: Let the pseudorange of the observed satellite be Then:

[0040]

[0041] Among them, Δd is the pseudorange of the i-th satellite received by the GPS reference station, that is, the correction value;

[0042] The calculation process of the true distance from the robot to the observed satellite is as follows:

[0043] S2113: Let the true distance from the robot to the observed satellite be d mi :

[0044]

[0045] Among them, is the satellite observation value of GPS;

[0046] S2114: Calculate the change rate of the correction value:

[0047]

[0048] Among them, t″ is the next adjacent moment of t′;

[0049] S2113: Calculate the true distance from the robot to the observed satellite:

[0050]

[0051] Among them, is the observed distance, t a is the moment when the robot receives the correction value, t l is the moment when the GPS reference station sends the correction value.

[0052] Furthermore, in the step S22, the sliding window averaging method balances the data fluctuation and reduces the noise interference by applying a dynamic average window to the data collected by the inertial measurement unit. The calculation formula is as follows:

[0053]

[0054] Among them, X M represents the data value after noise reduction, represents the sum and average of all data values.

[0055] Furthermore, in the step S23, the specific processing process of noise reduction processing for the ranging information is as follows:

[0056] S231: Use limit filtering to eliminate the outliers in the data set;

[0057] S232: Use arithmetic mean filtering to calculate the average value of the filtered data;

[0058] S233: Combine the results of the arithmetic mean and median filtering to obtain the final processed data.

[0059] Furthermore, in the step S24, by comparing the key points of the left and right viewpoints in the binocular depth camera, the two-dimensional image features are remapped into the three-dimensional space to recover the three-dimensional information, that is, the second pose information is obtained.

[0060] Furthermore, in the step S25, the specific processing process is as follows:

[0061] S251: Continuously track and record the speeds of the left and right wheels of the robot through the wheel encoder. According to the differential kinematics model, use the following formula to calculate and obtain the dynamic state quantity of the robot:

[0062]

[0063] S252: Perform integral calculation on the dynamic state quantity, that is, the third pose information is obtained, expressed as

[0064]

[0065] Furthermore, in the step S3, the specific processing process is as follows:

[0066] S31: Initialize the system and determine the system state and the observed state quantity. The specific processing process is as follows:

[0067] The GPS state space is initialized as:

[0068]

[0069] where G x is the distance in the x direction measured by GPS; G y is the distance in the y direction measured by GPS; P x represents the position coordinate of the robot on the x-axis, P y represents the position coordinate of the robot on the y-axis. The speeds of the robot on the x-axis and y-axis are respectively represented by v x and v y ;

[0070] Then the mapping from the GPS state space to the measurement space is:

[0071]

[0072] The lidar state space is initialized as:

[0073]

[0074] Among them, L x and L y respectively represent the distances on the x-axis and y-axis measured by the lidar;

[0075] Then the mapping from the lidar state space to the measurement space is:

[0076]

[0077] The wheel odometer state space is initialized as:

[0078]

[0079] Among them, W x is the distance on the x-axis measured by the wheel odometer; W y is the distance on the y-axis measured by the wheel odometer;

[0080] Then the mapping from the wheel odometer state space to the measurement space is:

[0081]

[0082] The inertial measurement unit state space is initialized as:

[0083]

[0084] Among them, N ax and N ay respectively represent the accelerations of the robot in the x-axis and y-axis directions measured by the inertial measurement unit, respectively represent the rotational angular velocities of the robot around the x, y, and z axes;

[0085] Then the mapping from the inertial measurement unit state space to the measurement space is:

[0086]

[0087] Determine the system state X and the observed state quantity Y as follows:

[0088]

[0089] Among them, x, y, z are the displacement information of the robot's wheel odometer; is the speed information of the robot's odometer; is the acceleration information of the robot's odometer; q1, q2, q3, q4 represent quaternions; is the angular velocity; x L , y L , z L is the second position information measured by the lidar after preprocessing, x G , yG , z G is the first position information collected by GPS after preprocessing;

[0090] S32: Determine the system state equation and the observation equation, and obtain the fusion result of the extended Kalman filter algorithm. The specific process is as follows:

[0091] Determine the system state equation as follows:

[0092] X(k) = A(k)X(k - 1)+Γ(k)W(k)

[0093] where A(k) represents the control state matrix of the system; Γ(k) represents the relationship matrix between the noise and the system state; W(k) represents the noise;

[0094] Determine the observation equation as follows:

[0095] Y(k) = h(k)X(k)+V(k)

[0096] where h(k) represents the output matrix; V(k) represents the measurement noise;

[0097] According to the prediction state equation, we have:

[0098]

[0099] Y - (k) = h(k)X - (k)

[0100] According to the update state equation, we have:

[0101]

[0102] X(k|k) = X(k|k - 1)+Kg(k)(Z(k)-H(k)X(k|k - 1))

[0103] P(k|k) = (I - Kg(k)H(k))P(k|k - 1)

[0104] where Kg(k) represents the Kalman gain, X(k|k) represents the estimated value of the system state X, that is, the fusion result of the extended Kalman filter algorithm, which is also the pose information after fusion by the extended Kalman filter algorithm; P(k∣k) represents the measurement error covariance matrix; R(k) represents the covariance matrix of the measurement noise v(k) at the (k - 1)-th sampling;

[0105] S33: Perform weighted fusion on the pose information after fusion by the extended Kalman filter algorithm and the second pose information measured by the preprocessed binocular depth camera to obtain the final positioning information. The specific process is as follows:

[0106] Let m r be the pose information of the robot measured by the binocular depth camera:

[0107] m r = [x r , y r , z r , ψ r , θ r , υ r r

[0108] Among them, x r , y r , z r represent the coordinates of the robot on the x, y, and z axes measured by the binocular depth camera, and ψ r , θ r , υ r represent the rotation angles of the robot in the x, y, and z axis directions measured by the binocular depth camera;

[0109] m c is the pose information after being fused by the extended Kalman filter algorithm, expressed as follows:

[0110] m c = [x c , y c , z c , ψ c , θ c , υ c r

[0111] Among them, x c , y c , z c represent the coordinates of the robot on the x, y, and z axes after being fused by the extended Kalman filter algorithm, and ψ c , θ c , υ c represent the rotation angles of the robot in the x, y, and z axis directions after being fused by the extended Kalman filter algorithm;

[0112] Perform weight assignment on m r , m c to obtain:

[0113]

[0114] Among them, represents obtaining the pose information of the robot, that is, the final positioning information; k1 is the weighting coefficient.

[0115] The present invention has the following advantages compared with the prior art:

[0116] ​​1. The positioning method based on the combination of GPS, wheel odometer, lidar, binocular vision camera and lidar combination can adapt to various complex scenarios such as coexistence of indoor and outdoor environments and multiple obstacles. To meet the accuracy requirements of mobile robots in positioning and navigation, a method for multi-sensor combined positioning of mobile robots is proposed. By transmitting data back from multiple sensors for data complementation and information fusion, the positioning and navigation can be made more accurate and stable.

[0117] 2. The fusion of lidar and visible light camera has the advantages of low cost, small volume, simple layout and easy maintenance.

[0118] 3. The data of GPS, inertial measurement unit, lidar and wheel odometer after preprocessing are fused through the extended Kalman filter algorithm to obtain the pose information after fusion by the extended Kalman filter algorithm. Then, it is weighted and fused again with the data of the binocular depth camera after preprocessing, and finally the final positioning information of the mobile robot is output, which can realize reliable and high-precision robot positioning work. Brief Description of the Drawings

[0119] Figure 1 is a schematic flow chart of the mobile robot positioning method based on multi-sensor combination in the embodiment of the present invention;

[0120] Figure 2 is a schematic diagram of geomagnetometer calibration data acquisition in the embodiment of the present invention;

[0121] Figure 3 is a schematic diagram of the principle of triangulation ranging in the embodiment of the present invention;

[0122] Figure 4 is a schematic diagram of the principle of ToF ranging method in the embodiment of the present invention;

[0123] Figure 5 is a schematic diagram of the pinhole camera imaging model in the embodiment of the present invention;

[0124] Figure 6 is a schematic diagram of the imaging model of the binocular camera in the embodiment of the present invention;

[0125] Figure 7 is a schematic diagram of the differential kinematics model in the embodiment of the present invention. Detailed Embodiment

[0126] The embodiments of the present invention will be described in detail below. These embodiments are implemented on the premise of the technical solution of the present invention, and detailed implementation manners and specific operation processes are given. However, the protection scope of the present invention is not limited to the following embodiments.

[0127] As Figure 1As shown in the figure, this embodiment provides a technical solution: a mobile robot positioning method based on multi-sensor combination, including the following steps:

[0128] Step 1: Calibrate the parameters of GPS, inertial measurement unit, binocular depth camera (i.e., visual sensor), lidar, and wheel odometer;

[0129] In this embodiment, calibrating the parameters of the above sensors includes the following aspects:

[0130] 1) Use time synchronization correction and frequency error correction to complete the GPS parameter calibration process to optimize its positioning performance. Optimize the positioning performance through GPS parameter calibration. The calibration process includes time synchronization correction and frequency error correction optimization.

[0131] (1) Time synchronization correction

[0132] The present invention develops a simple software script for GPS time correction. First, connect the GPS to the Network Time Protocol (NTP) service. At 12:00 noon every day, the script accurately compares the difference between the GPS time and the UTC time synchronized through the NTP service and records the data. Secondly, according to the time difference data collected within a certain period, calculate the average time error. Finally, the software automatically adjusts the internal clock of the GPS to compensate for the detected average time error.

[0133] (2) Frequency error correction

[0134] During the error correction process, set the precision frequency meter and GPS in parallel, start them synchronously, record their respective timestamp data, regularly compare the data differences between the two, and calculate the average frequency error. After obtaining the average frequency error, apply the least squares method to minimize the sum of squared errors and obtain the frequency correction coefficient C.

[0135] 2) Calibrate the accelerometer, gyroscope, and magnetometer in the WHEELTEC100 inertial measurement module (i.e., the above inertial measurement unit) respectively, and then realize the parameter calibration process of the inertial measurement unit.

[0136] (1) Accelerometer parameter calibration

[0137] First, fix the accelerometer on a stable platform to avoid any unexpected movement or vibration; secondly, place the x, y, and z axes of the accelerometer upward, and use the acceleration of gravity of the earth as a reference value (about 9.81m / s 2) Reverse the placement of each axis of the accelerometer and record the reversed gravitational acceleration values. Remain stationary in each direction and record the output values of the accelerometer over a period of time. Collect the static output data of the accelerometer in all test directions, ensuring that there are a sufficient number of data points in each direction for reliable statistical analysis.

[0138] (2) Magnetometer calibration

[0139] The present invention uses an ellipsoid model to accurately calibrate the magnetometer. During the calibration process of the magnetometer, the sensor is statically placed in various postures to facilitate reading its information. If there is no zero drift in the output of the magnetometer, the acquired data should be located on a spherical sphere, the center of which is exactly zero. However, due to the interference of errors, the data may be scattered on an ellipsoid with a non-zero value region as the focus. The core step of the calibration operation is to accurately fit this ellipsoid, and the ellipsoid model is constructed as follows:

[0140] ((x - x0) / A) 2 + ((y - y0) / B) 2 + ((z - z0) / C) 2 = 1 (1)

[0141] The general form of the ellipsoid fitting model is as follows, where there are a total of 6 unknowns: (x0, y0, z0) is the center of the ellipsoid, which is the zero offset, and the scale factors are A, B, and C.

[0142] To ensure the accurate completion of the calibration process, the data acquisition stage must guarantee data including various different angles and postures. As Figure 2 shown, by making multiple rotations on the x, y, and z axes, multiple sets of data are collected to ensure that the spatial attitude angle data of the IMU is evenly distributed.

[0143] (3) Gyroscope calibration

[0144] To enhance the measurement accuracy, the gyroscope needs to be accurately calibrated because when measuring on the three axes of the gyroscope, there will be deviations due to variables such as the supply voltage and temperature, and the existence of zero offset will cause continuous accumulation of errors, which may further affect the measurement accuracy. To find a solution, the gyroscope is statically placed, and at the same time, the angular velocities of the three axes are sampled and averaged, which helps to obtain the minimum offset of each axis, as follows:

[0145]

[0146] where, Δω x , Δω y , Δω z respectively represent the minimum displacements in the x, y, and z directions, Sum the values in the x, y, and z directions and take the average.

[0147] (3) Calibrate the external parameters of the lidar.

[0148] The present invention conducts experiments on the external parameter calibration of the YDLIDAR4ROS lidar. The scanning angle range of this lidar is 360°, and the distance range is from 0.15 meters to 12 meters.

[0149] 4) Use the Bouguet algorithm to calibrate the stereo parameters of the binocular depth camera.

[0150] The present invention uses the Bouguet algorithm for stereo rectification and alignment, which can achieve the matching of the y - coordinates of the same spatial point in the left and right images.

[0151] The steps of binocular image stereo rectification based on the Bouguet algorithm are as follows:

[0152] (1) After calibrating the external parameters of the left and right cameras in the binocular depth camera, the rotation matrix R of the right camera relative to the left camera can be determined. To simplify the processing in the image rectification steps and perform necessary cropping to reduce information loss, the rotation matrix R is split into R l and R r .

[0153] R l =R 1 / 2 ,R r =R -1 / 2 (3)

[0154] (2) To keep the optical axes of the two cameras parallel, the initial step includes using the rotation matrix to rotate and adjust the left and right cameras. To ensure that the baseline of the left and right cameras is parallel to the imaging plane, an additional transformation process is required, which depends on using the translation vector t in the external parameters of the left and right cameras to construct a new transformation matrix R rect :

[0155]

[0156] where e1, e2, e3 are calculated based on the translation amount t, as shown below:

[0157]

[0158] (3) Combining step (1) and step (2), the corresponding comprehensive transformation matrix is obtained:

[0159]

[0160] Using this transformation matrix, the optical axes of the left and right cameras can be aligned, and the parallel relationship between the baseline and the imaging plane can be ensured.

[0161] (4) Finally, adopt the stereo rectification method to crop the rectified images, so as to obtain new binocular images and complete the parameter calibration process of the binocular depth camera.

[0162] (5) By recording the specific values of the left and right wheel encoder counts when the mobile robot moves along three circular arcs with different lengths, the parameter calibration process of the wheel odometer is realized.

[0163] The present invention records the specific values of the left and right wheel encoder counts of the mobile robot when the mobile robot moves along three circular arcs with different lengths. The lengths of the circular arcs are 2 meters, 3 meters, and 4 meters respectively, and the radii of the circular arcs are 1 meter, 1.5 meters, and 2 meters respectively. The wheel diameter of the mobile robot is 20 cm, the wheelbase is 20 cm, and the resolution of the Maxon EC45 motor encoder is 500 lines.

[0164] Step 2: Use the sensors calibrated above to perform data acquisition work, and preprocess the data collected by each sensor by category;

[0165] (1) Calculate and process the data collected by GPS using the pseudorange differential method, and perform interpolation processing on the obtained data using the Neville interpolation method to obtain position information;

[0166] To improve the positioning accuracy, the pseudorange differential method is adopted. This method simplifies the calculation process by comparing the known position coordinates of the GPS reference station and the position information of the observed satellites. First, calculate the actual distance, and then compare the observed distance with the actual distance to determine the difference that needs to be corrected. Set the actual distance from the GPS reference station to the satellite as Furthermore, further calculations can be carried out.

[0167]

[0168] Among them, (x s , y s , z s ) is the spatial coordinate information of the observed satellite, and (x g , y g , z g ) is the spatial coordinate information of the GPS reference station.

[0169] Let the pseudorange of the observed satellite be where i = 1, 2, 3, 4, then:

[0170]

[0171] Among them, Δd is the pseudorange of the i-th observed satellite received by the GPS reference station.

[0172] The correction value is sent to the GPS receiver of the robot itself through wireless transmission to correct the data and obtain a distance value closer to the actual one.

[0173] Let the true distance between the robot and the satellite be d mi :

[0174]

[0175] Among them, is the satellite observation value received by the robot.

[0176] Since the time when the robot receives the correction value is later than the time when the reference station sends it, and the correction value may change over time, it is necessary to calculate the change rate of the correction value to ensure the accuracy and real-time nature of the correction.

[0177]

[0178] Among them, t″ is the next adjacent moment of t′.

[0179] In summary, the true distance from the robot to the satellite can be expressed as:

[0180]

[0181] Among them, is the observed distance, t a is the moment when the robot receives the error, t l is the moment when the GPS reference station sends the error.

[0182] According to Neville interpolation method, for a set of specified data points a, there exists a unique polynomial of no more than a - 1 order that exactly passes through these points. Calculate the number of data points n to be inserted between the continuous data points collected by GPS:

[0183]

[0184] Among them, (x0, y0), (x1, y1) are the coordinates of adjacent data points collected by GPS.

[0185] Using Neville's algorithm, a point between (x0, y0) and (x1, y1) can be obtained:

[0186]

[0187] At the interpolated point (x 0,1 , y 0,1 ), interpolation is performed again with the original data points (x0, y0) and (x1, y1) to obtain:

[0188]

[0189] The iterative formula can be obtained through iteration:

[0190]

[0191] n interpolation points can be obtained from the above formula. By combining the interpolation points with the original data points, the position information can be obtained.

[0192] 2) Use the sliding window averaging method to denoise the data collected by the inertial measurement unit to obtain the pose information.

[0193] The present invention uses the sliding window averaging method to denoise the geomagnetic data and reduce the influence of noise. This method effectively balances the data fluctuations by applying a dynamic averaging window to the data set, thereby reducing the interference of noise in data interpretation and statistical analysis. The calculation formula is as follows:

[0194]

[0195] Where, X M represents the denoised data value, represents the sum of all data values averaged.

[0196] 3) Use the principle of triangular ranging and the ToF ranging method to obtain the ranging information of the lidar; by integrating the methods of median filtering, clipping filtering, and arithmetic mean filtering, the noise reduction processing of the data is realized to obtain the position information.

[0197] The triangulation method calculates the distance based on the geometric relationship between the laser emitter and the radar camera. The working principle of this method is as Figure 3 shown.

[0198] When the laser emitter emits a laser beam towards the target, the laser beam will hit the target and reflect back, and part of it will be captured by the radar camera. During this process, the angle between the laser beam and the horizontal plane is marked as β, the distance from the laser emitter to the target is denoted as d, and the vertical height from the emitter to the target is denoted as q. If the horizontal distance between the emitter and the camera is s, the focal length of the camera is f, and the projection length of the target on the camera imaging plane is x, then by applying the principle of similar triangles, a mathematical model describing the relationship between geometric parameters can be constructed:

[0199]

[0200] According to equations (21) and (22), the distance d to the target object is:

[0201]

[0202] The ToF ranging method is achieved through the transmitter and receiver of the radar, and the principle is as follows Figure 4 shown.

[0203] When the lidar system emits laser light to a target and receives its reflected light, the distance from the lidar to the target is determined by measuring the time difference between the emission and reception of the laser light. Set the initial time of laser emission as T0, and the time of receiving the reflected laser as T i . The propagation time T of the laser in the air f can be calculated as follows:

[0204]

[0205] Then the distance d from the lidar to the target object is

[0206] d = cT f (22)

[0207] where c is the propagation speed of the laser beam in the air, with the unit of m / s.

[0208] In the present invention, by integrating the methods of median filtering, clipping filtering, and arithmetic mean filtering, noise reduction processing of the data is implemented. First, outlier values in the data set are removed using clipping filtering. Second, the arithmetic mean filtering is used to calculate the average value of the filtered data. Finally, the results of the arithmetic mean and median filtering are combined to obtain the final processed data.

[0209] 4) The preprocessing process of the data collected by the binocular depth camera is completed through the camera projection model and the camera ranging model to obtain the pose information.

[0210] In this system, for the convenience of analysis and operation, three different coordinate systems are defined:

[0211] The camera coordinate system, denoted as O - x - y - z, where O is the origin of coordinates, located at the optical center of the camera, i.e., the position of the small hole in the pinhole model. The measurement unit of this coordinate system is millimeters (mm).

[0212] The physical coordinate system of the imaging plane, labeled as O - x' - y' - z', and its origin O' is the intersection point of the camera optical axis and the imaging plane. This coordinate system also uses millimeters (mm) as the unit.

[0213] The pixel coordinate system, denoted as o - u - v. In this coordinate system, the u - axis is parallel and in the same direction as the x - axis, the v - axis is parallel and in the same direction as the y - axis, and the origin of coordinates o is located at the upper left corner of the image, with the unit being one pixel point.

[0214] In the camera coordinate system, a point P in the real world is projected through the optical center O of the camera onto a point P' on the imaging plane O-x'-y'. Assuming the position of point P in the camera coordinate system is [X, Y, Z], then the position of point P' on the imaging plane is [X', Y', Z'], and assuming the distance from the imaging plane to the optical center is the focal length f. Using the principle of similar triangles, the corresponding mathematical formula (23) is obtained.

[0215]

[0216] The point P in space is first mapped to the corresponding point P'. Next, the coordinates [x, y] of point P' on the imaging plane are further transformed into [u, v] in the pixel coordinate system. T Coordinates. This step involves projecting a three-dimensional point in space through the camera center onto a point on the two-dimensional image plane and using the specific parameters of the camera system to project it into the pixel coordinate system, realizing the conversion process from three dimensions to a two-dimensional image.

[0217] Under the concept of the pinhole camera model, the process of mapping a three-dimensional object in the real world to two dimensions is irreversible, and this process inevitably leads to the loss of some spatial information. To reconstruct the three-dimensional shape of an object in an image, the strategy adopted in the present invention is to achieve this through a binocular vision system. By comparing the key points of the left and right viewpoints in the binocular camera system, the two-dimensional image features can be remapped into three-dimensional space, thereby effectively restoring the three-dimensional information.

[0218] As Figure 6 shown, O L and O R represent the optical centers of the left and right cameras, and the imaging planes are represented by two yellow-green rectangles and have the same focal length f. U L and U R respectively indicate the position coordinates of a certain spatial feature point in the left and right camera views. Suppose there is a three-dimensional point P in space. Through the principle of pinhole imaging, the left and right cameras each capture the image of this point, marked as P L and P R . Given the working principle of the dual-camera system, the translation of the imaging only occurs on the x-axis. Therefore, the y-axis coordinates of P L and P R are the same, and only the x-axis coordinates are different. The difference coordinate is U L and U R . Based on the above parameters, a set of constraint relations describing this process is constructed:

[0219]

[0220] Furthermore, it is obtained that:

[0221]

[0222] d = u L -u R (26)

[0223] The parallax d represents the displacement difference of the feature point positions in the x-axis direction in the two images. According to formula (26), this parallax value can help accurately locate the three-dimensional point P in space.

[0224] 5) Use the differential kinematic model to process the data collected by the wheel odometer to obtain the pose information.

[0225] The mobile robot developed in the present invention is equipped with a wheel encoder, which includes three core components: a light source, a rotating code disk with gratings and a grating plate, and several photosensitive elements. As the rotating shaft rotates, the equally spaced gratings on the code disk cause the light signal emitted by the light source to pass through periodically and be captured by the photosensitive elements. The photosensitive elements then generate periodic electrical signals, based on which the angular velocity of the rotating shaft can be calculated. Given the radius r of the robot wheel, the speed v of the robot wheel can be obtained as v = ω·r.

[0226] This model is equipped with left and right wheels that can achieve differential drive, and its motion depends on the differential kinematic principle. The differential kinematic model is as Figure 7 shown. By continuously tracking and recording the speeds of the left and right wheels of the robot through the wheel encoder, according to the differential kinematic model, equation (27) is applied to calculate and obtain the dynamic state information of the robot.

[0227]

[0228] Performing integral calculation on the above state variables can obtain the pose information, that is, the wheel odometer data is obtained

[0229]

[0230] Step 3: Use the extended Kalman filter algorithm to fuse the preprocessed data of GPS, inertial measurement unit, lidar and wheel odometer to obtain the pose information after fusion by the extended Kalman filter algorithm, and then perform weighted fusion again with the preprocessed data of the binocular depth camera, and finally output the final positioning information of the mobile robot.

[0231] In the positioning algorithm framework that fuses multi-source data, the system state consists of the robot's position, orientation, and data from lidar, binocular depth camera, GPS, wheel odometer, and inertial measurement unit. By incorporating the robot's current position and control input into the control equation, the algorithm predicts the robot's future position. Additionally, by combining the data collected from multiple sensors with the observation equation, the robot's next state is estimated more precisely.

[0232] First, initialize the system. The GPS state space is initialized as:

[0233]

[0234] where G x is the distance in the x-direction measured by GPS; G y is the distance in the y-direction measured by GPS.

[0235] Then the mapping from the state space to the measurement space is:

[0236]

[0237] The lidar state space is initialized as:

[0238]

[0239] where P x represents the position coordinate of the robot on the x-axis, and P y represents the position coordinate of the robot on the y-axis. The velocities of the robot on the x-axis and y-axis are represented by v x and v y respectively. The distances measured by the lidar on the x-axis and y-axis are represented by L x and L y respectively.

[0240] Then the mapping from the state space to the measurement space is:

[0241]

[0242] The wheel odometer state space is initialized as:

[0243]

[0244] where W x is the distance on the x-axis measured by the wheel odometer; W y is the distance on the y-axis measured by the wheel odometer.

[0245] Then the mapping from the state space to the measurement space is:

[0246]

[0247] The inertial measurement unit state space is initialized as:

[0248]

[0249] where N ax and N ay respectively represent the accelerations of the robot in the x-axis and y-axis directions measured by the inertial measurement unit. And respectively represent the rotational angular velocities of the robot around the x, y, and z axes. The above indicators act together to provide comprehensive dynamic information for the motion state of the robot.

[0250] Then the mapping from the state space to the measurement space is:

[0251]

[0252] The system state uses state variables such as displacement and velocity to represent the system state X, and the observed state variable Y is the processed lidar feature point data and the processed GPS feature point data. Then:

[0253]

[0254] where x, y, z are the displacement information of the robot's wheel odometer; is the velocity information of the robot's wheel odometer; is the acceleration information of the robot's wheel odometer; q1, q2, q3, q4 represent quaternions; is the angular velocity; x L , y L , z L is the position information measured by the lidar after preprocessing, x G , y G , z G is the position information collected by the GPS after preprocessing.

[0255] According to the state variables, the state equation can be obtained as:

[0256] (1) System state equation:

[0257] X(k) = A(k)X(k - 1) + Γ(k)W(k) (39)

[0258] where A(k) represents the control state matrix of the system; Γ(k) represents the relationship matrix between the noise and the system state; W(k) represents the noise.

[0259] (2) Observation equation:

[0260] Y(k) = h(k)X(k) + V(k) (40)

[0261] Among them, h(k) represents the output matrix; V(k) represents the measurement noise.

[0262] This system is a non - linear system. The decomposition matrix A(k) is obtained through Taylor expansion as follows:

[0263]

[0264] Among them, A p represents the linear state matrix of the system displacement, which is used to represent x, y, z, A0 is the non - linear state matrix of the system rotation, which is used to represent q1, q2, q3, q4,

[0265] Among them The rotation matrix A0 needs to be locally linearized according to the predicted state X(k|k - 1).

[0266] h(k) can be expressed as:

[0267]

[0268] According to the predicted state equation, we have:

[0269]

[0270] Y - (k) = h(k)X - (k) (44)

[0271]

[0272] Among them, represents the predicted value of the measurement state; Y - (k) represents the predicted value of the output state; P - (k|k - 1) represents the predicted matrix of the error covariance; A(k) represents the state linearization matrix; represents the estimated error covariance matrix; Q(k) represents the covariance matrix during the sampling of the system noise W(k).

[0273] According to the updated state equation, we have:

[0274]

[0275] X(k|k) = X(k|k - 1)+Kg(k)(Z(k)-H(k)X(k|k - 1)) (47)

[0276] P(k|k) = (I - Kg(k)H(k))P(k|k - 1) (48)

[0277] Where: Kg(k) represents the Kalman gain, X(k|k) represents the estimated value of the system state X; P(k∣k) represents the measurement error covariance matrix; R(k) represents the covariance matrix of the measurement noise v(k) at the k-1th sampling.

[0278] Finally, the data obtained by the binocular depth camera is weighted and fused with the EKF result (X(k|k) above), and the positioning information is finally obtained.

[0279] Let m r be the pose information of the robot measured by the binocular depth camera:

[0280] m r = [x r , y r , z r , ψ r , θ r , υ r r (39)

[0281] where, x r , y r , z r represent the coordinates of the robot measured by the binocular depth camera on the x, y, and z axes, ψ r , θ r , υ r represent the rotation angles of the robot measured by the binocular depth camera in the x, y, and z axis directions;

[0282] m c is the pose information after fusion by the extended Kalman filter algorithm, and can be expressed as

[0283] m c = [x c , y c , z c , ψ c , θ c , υ c r (40)

[0284] where, x c , y c , z c represent the coordinates of the robot on the x, y, and z axes after EKF fusion, ψ c , θ c , υ c represent the rotation angles of the robot in the x, y, and z axis directions after EKF fusion.

[0285] Weight distribution is performed on the data of the two to obtain:

[0286]

[0287] wherein represents the pose information of the robot for weighted positioning estimation; k1 is the weighting coefficient.

[0288] In summary, the mobile robot positioning method based on multi-sensor combination in the above embodiments can adapt to various complex environments such as coexistence of indoor and outdoor and multiple obstacles, continuously provide reliable and high-precision positioning information for autonomous service robots, and has the advantages of low cost, simple layout, good real-time performance, high reliability, etc., and can be widely applied to various autonomous robots.

[0289] Although the embodiments of the present invention have been shown and described above, it can be understood that the above embodiments are exemplary and should not be construed as limiting the present invention. Those of ordinary skill in the art can make changes, modifications, substitutions, and variations to the above embodiments within the scope of the present invention.

Claims

1. A mobile robot positioning method based on multi-sensor combination, characterized in that, It includes the following steps: S1: Sensor parameter calibration Calibrate the parameters of each sensor, where the sensors include GPS, inertial measurement unit, binocular depth camera, lidar, and wheel odometer; S2: Data acquisition and preprocessing Use the sensors calibrated above to perform data acquisition work, and classify and preprocess the data collected by each sensor; S3: Data fusion positioning Fuse the preprocessed data of GPS, inertial measurement unit, lidar, and wheel odometer through the extended Kalman filter algorithm to obtain the pose information after the extended Kalman filter algorithm fusion, and then perform weighted fusion with the preprocessed data of the binocular depth camera again to finally output the final positioning information of the mobile robot.

2. The mobile robot positioning method based on multi-sensor combination according to claim 1, wherein, In the step S1, complete the parameter calibration process of GPS using time synchronization correction and frequency error correction methods; calibrate the parameters of the accelerometer, gyroscope, and magnetometer in the inertial measurement unit respectively to complete the parameter calibration process of the inertial measurement unit; calibrate the external parameters of the lidar to complete the parameter calibration process of the lidar; use the Bouguet algorithm to perform stereo parameter calibration on the binocular depth camera to complete the parameter calibration process of the binocular depth camera; realize the parameter calibration process of the wheel odometer by recording the specific values of the left and right wheel encoder counts when the mobile robot moves along three circular arcs with different lengths.

3. A mobile robot positioning method based on multi-sensor combination according to claim 1, characterized in that, In the step S2, the data preprocessing process is as follows: S21: Calculate and process the data collected by GPS using the pseudorange differential method, and perform interpolation processing on the obtained data using the Neville interpolation method to obtain the first position information; S22: Denoise the data collected by the inertial measurement unit using the sliding window averaging method to obtain the first pose information; S23: Obtain the ranging information of the lidar using the triangulation ranging principle and the ToF ranging method; realize the noise reduction processing of the ranging information by fusing median filtering, limit filtering, and arithmetic mean filtering to obtain the second position information; S24: Preprocess the data collected by the binocular depth camera to obtain the second pose information; S25: Process the data collected by the wheel odometer using the differential kinematic model to obtain the third pose information.

4. A mobile robot positioning method based on multi-sensor combination according to claim 3, characterized in that, In the step S21, the specific processing process is as follows: S211: Calculate and process the data collected by GPS using the pseudorange differential method, and the specific process is as follows: First calculate the actual distance, and then compare the observed distance with the actual distance to determine the difference to be corrected, that is, obtain the correction value; Then send the correction value to GPS through wireless transmission to correct the data and obtain a distance value closer to the actual value, that is, obtain the true distance from the robot to the observed satellite; S212: Perform interpolation processing on the continuous data points collected by GPS processed in step S211 using the Neville interpolation method. The continuous data points collected by GPS are the true distance data points from the robot to the observed satellite, and the specific process is as follows: S2121: Calculate the number of data points n to be inserted between consecutive data points collected by GPS: Among them, (x0, y0) and (x1, y1) are the coordinates of adjacent data points collected by GPS; S2122: Use the Neville algorithm to obtain a point between (x0, y0) and (x1, y1): S2122: At the obtained interpolation point (x 0,1 , y 0,1 ), interpolation is performed again with the original data points (x0, y0) and (x1, y1) to obtain: Through iteration, the iterative formula can be obtained: Through the above formula, n interpolation points can be obtained. By merging the interpolation points with the original data points, the first position information is obtained.

5. A mobile robot positioning method based on multi-sensor combination according to claim 4, characterized in that, In the step S211, the calculation process of the correction value is as follows: S2111: Set the actual distance from the GPS reference station to the observed satellite as Among them, (x s , y s , z s ) is the spatial coordinate information of the observation satellite, and (x g , y g , z g ) is the spatial coordinate information of the GPS reference station; S2112: Let the pseudorange of the observation satellite be Then: Among them, Δd is the pseudorange of the i-th satellite received by the GPS reference station, that is, the correction value; The calculation process of the true distance from the robot to the observed satellite is as follows: S2113: Let the actual distance between the robot and the observation satellite be d mi : Among them, is the satellite observation value of GPS; S2114: Calculate the change rate of the correction value: Among them, t″ is the next adjacent moment of t′; S2113: Calculate the true distance from the robot to the observed satellite: Among them, is the observation distance, t a is the moment when the robot receives the correction value, t l is the moment when the GPS reference station sends the correction value.

6. A mobile robot positioning method based on multi-sensor combination according to claim 3, characterized in that, In the step S22, the moving window averaging method balances the data fluctuations and reduces the noise interference by applying a dynamic averaging window to the data collected by the inertial measurement unit. The calculation formula is as follows: Among them, X M represents the data value after noise reduction, represents the sum of all data values averaged.

7. A mobile robot positioning method based on multi-sensor combination according to claim 3, characterized in that In the step S23, the specific processing process of reducing the noise of the ranging information is as follows: S231: Use limit filtering to eliminate the outliers in the data set; S232: Use arithmetic mean filtering to calculate the average value of the filtered data; S233: Combine the results of the arithmetic mean and median filtering to obtain the final processed data.

8. A mobile robot positioning method based on multi-sensor combination according to claim 3, characterized in that, In the step S24, by comparing the key points of the left and right viewpoints in the binocular depth camera, the two-dimensional image features are remapped into the three-dimensional space to restore the three-dimensional information, that is, the second pose information is obtained.

9. A mobile robot positioning method based on multi-sensor combination according to claim 3, characterized in that, In the step S25, the specific processing process is as follows: S251: Continuously track and record the speeds of the left and right wheels of the robot through the wheel encoder. According to the differential kinematics model, use the following formula to calculate and obtain the dynamic state quantity of the robot: S252: Integrate the dynamic state quantity, and the third pose information can be obtained, which is expressed as 10. A mobile robot positioning method based on multi-sensor combination according to claim 3, characterized in that, In the step S3, the specific processing process is as follows: S31: Initialize the system and determine the system state and the observed state quantity. The specific processing process is as follows: The GPS state space is initialized as: Among them, G x is the distance in the x direction obtained by GPS measurement; G y is the distance in the y direction obtained by GPS measurement; P x represents the position coordinate of the robot on the x-axis, and P y represents the position coordinate of the robot on the y-axis. The speeds of the robot on the x-axis and y-axis are respectively represented by v x and v y ; Then the mapping from the GPS state space to the measurement space is: The lidar state space is initialized as: Among them, L x and L y respectively represent the distances on the x-axis and y-axis measured by the lidar; Then the mapping from the lidar state space to the measurement space is: The wheeled odometer state space is initialized as: Among them, W x is the distance on the x-axis measured by the wheel odometer; W y is the distance on the y-axis measured by the wheel odometer; Then the mapping from the wheeled odometer state space to the measurement space is: The inertial measurement unit state space is initialized as: Wherein, N ax and N ay respectively represent the accelerations of the robot measured by the inertial measurement unit in the x-axis and y-axis directions, respectively represent the rotational angular velocities of the robot about the x, y, and z axes; Then the mapping from the inertial measurement unit state space to the measurement space is: Determine the system state X and the observed state quantity Y as follows: Among them, x, y, and z are the displacement information of the robot's wheel odometer; is the speed information of the robot odometer; is the acceleration information of the robot odometer; q1, q2, q3, q4 represent quaternions; is the angular velocity; x L , y L , z L is the second position information measured by the lidar after preprocessing, x G , y G , z G is the first position information collected by the GPS after preprocessing; S32: Determine the system state equation and the observation equation, and obtain the fusion result of the extended Kalman filter algorithm. The specific process is as follows: Determine the system state equation as follows: X(k) = A(k)X(k - 1) + Γ(k)W(k) Among them, A(k) represents the control state matrix of the system; Γ(k) represents the relationship matrix between the noise and the system state; W(k) represents the noise; Determine the observation equation as follows: Y(k) = h(k)X(k) + V(k) Among them, h(k) represents the output matrix; V(k) represents the measurement noise; According to the prediction state equation, we have: Y - (k) = h(k)X - (k) According to the update state equation, we have: X(k|k) = X(k|k-1) + Kg(k)(Z(k) - H(k)X(k|k-1)) P(k|k) = (I - Kg(k)H(k))P(k|k-1) Where, Kg(k) represents the Kalman gain, X(k|k) represents the estimated value of the system state X, that is, the fusion result of the extended Kalman filter algorithm, which is also the pose information after the fusion of the extended Kalman filter algorithm; P(k∣k) represents the measurement error covariance matrix; R(k) represents the covariance matrix of the measurement noise v(k) at the (k - 1)-th sampling; S33: Weightedly fuse the pose information after the fusion of the extended Kalman filter algorithm and the second pose information measured by the preprocessed binocular depth camera to obtain the final positioning information. The specific process is as follows: Let m r be the pose information of the robot measured by the binocular depth camera: m r = [x r , y r , z r , ψ r , θ r , υ r r ​ Among them, x r , y r , z r represent the coordinates of the robot on the x, y, and z axes measured by the binocular depth camera, and ψ r , θ r , υ r represent the rotation angles of the robot in the x, y, and z axis directions measured by the binocular depth camera; m c The pose information fused by the extended Kalman filter algorithm is expressed as follows: m c = [x c , y c , z c , ψ c , θ c , υ c r ​ Among them, x c , y c , z c represent the coordinates of the robot on the x, y, and z axes after the fusion of the extended Kalman filter algorithm, and ψ c , θ c , υ c represent the rotation angles of the robot in the x, y, and z axis directions after the fusion of the extended Kalman filter algorithm; Assign weights to m r and m c to obtain: Among them, represents obtaining the pose information of the robot, that is, the final positioning information; k1 is a weighting coefficient.

Images