A neural network and laser radar based mobile robot positioning method

Abstract

The application discloses a mobile robot positioning method based on a neural network and a laser radar, laser radar and bottom vision are used for positioning, in an indoor environment, the laser radar can realize map construction and positioning functions of the mobile robot, but low-precision laser radars considering cost need to be matched with odometers to realize the positioning functions, which inevitably causes cumulative errors, which will reduce the positioning accuracy of the laser radar. The positioning frequency of the visual algorithm is low, and the positioning accuracy is improved. The algorithm adopts double-layer neural networks to compensate the state prediction model and the measurement model process, so that the positioning accuracy is optimized. The algorithm realizes the state prediction process through infrequent but more accurate visual positioning information, realizes the state updating process of the mobile robot through frequent laser radar positioning information, and continuously updates the weights and biases of the simultaneous neural network with the running of the algorithm.

Description

Technical Field

[0001] This invention belongs to the field of multi-sensor data fusion, specifically relating to a mobile robot localization method based on neural networks and lidar. Background Technology

[0002] With the development of intelligent manufacturing technology in my country, mobile robots have been widely used in daily life. Positioning, as a key technology in mobile robots, has always been a research hotspot due to its accuracy and cost control requirements.

[0003] GNSS technology is widely used for outdoor positioning, but it cannot be used for indoor positioning due to building obstructions. Furthermore, the complexity of indoor environments often necessitates multiple positioning solutions for different needs. Currently, the development of indoor positioning technology is dominated by multi-sensor fusion, achieving better accuracy and stability by fusing multiple types of sensors. Common multi-sensor data fusion positioning algorithms include weighted averaging, Kalman filtering, multi-Bayes estimation, and neural networks. The Extended Kalman Filter (EKF) algorithm is widely used in multi-sensor fusion to improve the positioning accuracy of a single sensor; however, its accuracy depends on the accuracy of the described system state model and output model. Moreover, the EKF uses uncorrelated white noise as noise for both state and measurement values; when this assumption is violated, the EKF algorithm may exhibit inaccurate estimations or even divergence. To ensure the reliability and accuracy of the fusion results, an online compensation technique is needed to address model errors and violations of noise assumptions.

[0004] Indoor positioning technologies encompass a variety of methods, with common solutions including LiDAR, IMU, odometry, depth vision, Bluetooth, Wi-Fi, and UWB. LiDAR, as a crucial sensor for indoor positioning, enables map building of complex indoor environments and map sharing, facilitating the deployment of mobile robots. (R. Ravindran, MJSantora and MM Jamali, "Camera, LiDAR, and Radar Sensor Fusion Based on Bayesian Neural Network (CLR-BNN)," in IEEE Sensors Journal, vol.22, no.7, pp.6964-6974, 1 April 1, 2022, doi:10.1109 / JSEN.2022.3154980.). Summary of the Invention

[0005] To address the aforementioned technical problems, this invention proposes a mobile robot localization method based on neural networks and lidar.

[0006] The present invention is achieved by at least one of the following technical solutions.

[0007] A mobile robot localization method based on neural networks and lidar includes the following steps:

[0008] S1. Establish a 2D lidar measurement and positioning model, and realize the positioning of the mobile robot in a known map environment through lidar and odometry.

[0009] S2. Using a positioning algorithm based on AprilTag positioning codes, the bottom camera is used to identify and obtain the pose information of the mobile robot.

[0010] S3. Construct a multi-rate sensor information model. When there are differences in positioning accuracy and information feedback rate during the multi-sensor fusion process, optimize the information structure to improve information utilization efficiency.

[0011] S4. A multi-sensor fusion algorithm based on a two-layer neural network is used to achieve the fusion positioning of lidar information and visual information. The multi-sensor fusion algorithm is based on extended Kalman filtering and includes a state prediction process and a state update process, which are implemented using positioning information from two sensors respectively. The fusion process improves positioning accuracy by updating the covariance matrix.

[0012] S5. Introduce a state compensation model and a measurement compensation model based on neural networks to compensate for errors in the construction of the state model and measurement model, including external interference errors and errors caused by insufficient understanding in the modeling process.

[0013] S6. Real-time error compensation is achieved by forward-computing the compensation values ​​of the final state compensation and measurement compensation networks;

[0014] S7, the multi-sensor fusion algorithm of the dual-layer neural network distinguishes between the main time and the secondary time during operation, and uses different methods to obtain the optimal estimate of the current time for the two types of time.

[0015] Further, step S1 includes the following steps:

[0016] The measurement and positioning model of the 2D lidar uses particle filtering for positioning, with the initial pose as the mean of the initial particle distribution, and the initial particle distribution is controlled according to the prior position.

[0017] Prediction: Using odometry information, predict the position of the particle at the next moment;

[0018] Update: The particle weight information is updated by matching the point cloud information scanned by LiDAR with the particle position map;

[0019] Clustering: Based on the weight information, the particles are clustered and the mean is calculated to achieve the current location.

[0020] Optimization: Optimize the positions of particles with smaller weights and update the particle positions according to the motion model;

[0021] Repeated prediction, update, clustering, and optimization processes.

[0022] Further, in step S2, the bottom camera recognizes the AprilTag positioning code to obtain the pose information of the mobile robot. A camera is installed at the center of the bottom of the mobile robot to obtain the image directly below it. When the mobile robot moves to the top of the AprilTag positioning code laid on the ground, the camera recognizes the AprilTag positioning code. The AprilTag positioning code information is recognized to obtain the number information and the homography matrix.

[0023] The number information is used to obtain the absolute position information of the AprilTag location code in the map environment. The deviation between the camera and the AprilTag location code can be obtained by using the homography matrix, the extrinsic parameter estimation of the camera, and the coordinates of the center point of the AprilTag location code in the image.

[0024] The pose of the mobile robot in the map environment is obtained by the relative deviation between the camera and the AprilTag positioning code, as well as the absolute position information of the AprilTag positioning code in the map environment.

[0025] Furthermore, in step S3, the multi-rate sensor information model classifies sensors into high-frequency low-precision sensors and low-frequency high-precision sensors based on information frequency and accuracy, and classifies positioning time nodes into primary time and secondary time.

[0026] Furthermore, step S4 specifically includes:

[0027] For a nonlinear process:

[0028] X k =f(X) k-1 )+u k-1

[0029] Z k =h(X) k )+v k

[0030] Where X k Z kLet f(·) and h(·) represent the state vector and measurement vector at time k, respectively, and let u represent the state model and measurement model under nonlinear conditions. k-1 and v k Let Q and R represent the noise of the state model at time k-1 and the noise of the measurement model at time k, respectively. In the state model and the measurement model, their covariance matrices are represented as Q and R, respectively.

[0031] Prediction process:

[0032] calculate The state equation is expressed as:

[0033]

[0034] The predicted representation of the covariance matrix is ​​as follows:

[0035]

[0036] in and These are the prior estimate of the state at time k and the posterior estimate of the state at time k-1, respectively. and P k-1 F represents the prior estimate of the state error covariance matrix at time k and the updated state error covariance matrix at time k-1, respectively. k-1 It is the Jacobian matrix of the linearized state model, and its value is calculated by the following formula:

[0037]

[0038] Where f is the state equation and X is the state vector;

[0039] Update process:

[0040] The gain of the update process is expressed as:

[0041]

[0042] Where K k For gain, H k The Jacobian matrix of the linearized measurement model is calculated using the following formula:

[0043]

[0044] Where h is the measurement equation. Posterior estimate.

[0045] The posterior state estimate at time k is expressed as:

[0046]

[0047] Zk For the actual measurement value, To measure the output value predicted by the measurement model, the measurement equation is expressed as:

[0048]

[0049] The updated state estimation error covariance matrix is:

[0050]

[0051] Where P k Let I be the covariance matrix of the updated state estimation error, and let I be the identity matrix.

[0052] Furthermore, in step S5, a state compensation model and a measurement compensation model based on a neural network are introduced, and the nonlinear process is modified. The specific process is as follows:

[0053] X k =f true (X k-1 )+u k-1

[0054] Z k =h true (X k )+v k

[0055] Where f true (·) and h true (·) represent the actual state model and the actual measurement model, respectively. During the construction of the state and measurement models, factors such as changes in the external environment cause deviations between the models and the actual situation. To compensate for potential errors, the state equations and measurement equations are modified.

[0056]

[0057]

[0058] Where N X and N Z The outputs of the state-compensated neural network and the measurement-compensated neural network are respectively. To compensate for the weights and biases of the neural network at time k-1, The weights and biases of the compensation neural network are measured at time k.

[0059] Furthermore, with the introduction of the compensation model, the prediction and update process changes, as follows:

[0060] Using high-frequency, low-precision sensor information to update the measurement compensation neural network φ ZUsing low-frequency, high-precision sensor information to update the state compensation neural network φ X Meanwhile, when only high-frequency sensor information is received, i.e., at the secondary moment, only φ is updated. Z The weights and biases of the two neural networks are added to the state, i.e.:

[0061]

[0062] Where X is the state vector of the nonlinear system. This is the augmented state vector;

[0063] Modify the state equations and measurement equations in the prediction process:

[0064]

[0065]

[0066] in For the augmented prior state estimation, we first consider the secondary time step, where only information from the high-frequency sensor is acquired. During the prediction phase, the parameters of the neural network remain unchanged, i.e., φ. k =φ k-1 , φ k and φ k-1 These are the parameters of the two neural networks at time k and time k-1, respectively.

[0067] The prediction of the covariance matrix is ​​transformed into:

[0068]

[0069] in The Jacobian matrix of the augmented state model can be calculated at second-order times as follows:

[0070] and Represented as φ X and φ Z Vector-aligned identity matrix;

[0071] During the prediction process, the weights and biases remain unchanged, and the state-compensated neural network does not include φ. Z Furthermore, the Kalman gain matrix can be expressed as:

[0072]

[0073] in To augment the Jacobian matrix of the measurement model, for sensor systems with multiple high-frequency but low-precision sensors, It can be represented as:

[0074]

[0075] Where N Z Related to the parameters of the state-compensated neural network, we obtain:

[0076]

[0077] Update phase:

[0078]

[0079]

[0080] During the primary timeframe, the low-frequency, high-precision sensor acquires positioning information, and its prediction and update process is similar to that of the secondary timeframes, but the measurement equations change:

[0081]

[0082] in h is the measurement value of the nth low-frequency, high-precision sensor. m For the measurement equation of the m-th high-frequency, low-precision sensor, N Z,m The output of the measurement compensation neural network for the m-th high-frequency, low-precision sensor is determined. Let be the prior state estimate of the nth low-frequency, high-precision sensor, and simultaneously the Jacobian matrix. In the main time interval, it is represented as:

[0083]

[0084]

[0085] Among them I n Let n be the identity matrix.

[0086] Furthermore, based on the state compensation model and measurement compensation model of neural networks, a single hidden layer feedforward neural network is used as the basic structure of the state compensation neural network and the measurement compensation neural network, with n allocated to each network respectively. X and n Z The number of neurons, where, for a state-compensated neural network, the input is the optimal state estimate from the previous time step. The output is state compensation. For a measurement compensation neural network, the input is the corrected prior state estimate at the current time step. The output is measurement compensation.

[0087] Tanh is used as the activation function for each neuron in both the state-compensated neural network and the measurement-compensated neural network.

[0088] Furthermore, in step S6, the bias and weights in the neural network are updated using the positioning information obtained at the main time step and the secondary time step. The specific process is as follows:

[0089] For a state-compensated neural network, the corresponding output is represented as:

[0090]

[0091] in and These represent the weights and biases from the input layer to the hidden layer, respectively. and These represent the weights and biases from the hidden layer to the output layer, respectively.

[0092]

[0093]

[0094] Where n is the number of low-frequency high-precision sensors, and the subscripts of the weights w and biases b indicate the connections of neurons, while the superscripts indicate the number of layers. Represented as the nth neuron in the first layer to the nth neuron in the second layer X The weights of each neuron, For the second layer nth X The weights from the nth neuron in the third layer to the nth neuron in the third layer. This is the optimal state estimate of the nth low-frequency high-precision sensor at the previous time step. For the second layer nth X The deviation of a single neuron This represents the deviation of the nth neuron in the third layer;

[0095] For the measurement compensation neural network, its output is represented as:

[0096]

[0097] in and The weights and biases from the input layer to the hidden layer. and These represent the weights and biases from the hidden layer to the output layer:

[0098]

[0099]

[0100] Where m represents the number of high-frequency and high-precision sensors. Represented as the nth neuron in the first layer to the nth neuron in the second layer Z The weights of each neuron, For the second layer nth Z The weights from the m-th neuron in the third layer to the m-th neuron in the third layer. This represents the prior state estimate of the m-th high-frequency, low-precision sensor at time k. For the second layer nth Z The deviation of a single neuron This represents the deviation of the m-th neuron in the third layer.

[0101] Furthermore, in step S7, the optimal estimate of the current time is obtained by using different methods for the main time and the secondary time, as follows:

[0102] When only information from high-frequency, low-precision sensors is acquired at the current moment, it is considered a secondary moment. At this time, the multi-sensor fusion algorithm of the dual-layer neural network updates the positioning of the mobile robot using the high-frequency, low-precision sensor information, ensuring the real-time positioning of the mobile robot during movement. In addition to eliminating abnormal information, the high-frequency, low-precision sensors are relatively stable in terms of information acquisition frequency, enabling stable positioning information updates. At the same time, the weights and parameters of the measurement compensation neural network are updated, optimizing the measurement compensation neural network model and further reducing positioning errors.

[0103] When a low-frequency, high-precision sensor acquires information, the matching time is fused with the secondary time closest to the main time. The extended Kalman filter algorithm is improved by using a state compensation neural network and a measurement compensation neural network to achieve state prediction and state update, including updating the parameters of the state compensation neural network and the measurement compensation neural network.

[0104] Compared with existing technologies, the beneficial effects of the present invention are as follows:

[0105] This invention improves the positioning accuracy of mobile robots by combining neural network technology to achieve data fusion from multiple rate sensors. It employs fusion of LiDAR and a bottom-mounted camera. The mobile robot acquires its own positioning at a certain frequency via LiDAR, and only acquires visual positioning when it moves above a positioning code. LiDAR positioning serves as high-frequency, low-precision positioning, while visual positioning serves as low-frequency, high-precision positioning. The low-frequency, high-precision sensor ensures the positioning accuracy of the mobile robot, while the high-frequency, low-precision sensor ensures the real-time positioning characteristics of the mobile robot.

[0106] This invention employs a two-layer neural network to compensate for state and measurement models. The state-compensation neural network reduces model errors caused by insufficient experience and model changes due to external environmental variations. The measurement model reduces the impact of noise during sensor positioning. It significantly reduces positioning errors caused by model inaccuracies and continuously updates its network weights and biases during algorithm operation, adapting to model deviations caused by external environmental changes, exhibiting dynamic adaptability. Secondly, the use of a LiDAR positioning sensor facilitates the expansion of other functions of the mobile robot, such as scene map construction, enabling adaptive path planning and dynamic obstacle avoidance algorithms. The positioning algorithm using a bottom-mounted camera allows for the placement of positioning codes in areas requiring precise positioning, facilitating high-precision docking, such as goods handover and workpiece positioning in industrial scenarios, and automatic charging in common scenarios. Attached Figure Description

[0107] Figure 1 This is a flowchart illustrating a mobile robot localization method based on neural networks and lidar in this embodiment.

[0108] Figure 2 This is a basic flowchart of the lidar positioning algorithm in this embodiment;

[0109] Figure 3 This is a schematic diagram illustrating the information acquisition of the high-frequency low-precision and low-frequency high-precision sensors in this embodiment;

[0110] Figure 4 This is a flowchart of the extended Kalman filter after introducing a neural network in this embodiment;

[0111] Figure 5 This is a schematic diagram of the neural network structure in this embodiment. Detailed Implementation

[0112] The application of the present invention will be described in detail below with reference to the embodiments and corresponding drawings, but the present invention is not limited to the described embodiments.

[0113] Example 1

[0114] like Figure 1 As shown, a mobile robot localization method based on neural networks and lidar includes the following steps:

[0115] 1) Establish a 2D LiDAR measurement and positioning model. This invention focuses on the implementation of LiDAR positioning, but it's important to note that LiDAR positioning relies on a known map, which can be acquired through a single mobile scan. A grid-based positioning method is used, dividing the map into three states: occupied, idle, and unknown. For example... Figure 2 As shown, the specific steps include:

[0116] Initialization: The particle filter method is used for localization, with the initial pose (default is 0) as the mean of the initial particle distribution, and the initial particle distribution is controlled according to the prior position.

[0117] Prediction: By obtaining the displacement changes of the mobile robot in adjacent moments through odometry information, the position information of the particles in the next moment is predicted. Since there are errors in the odometry prediction process, the particles will diverge in adjacent moments.

[0118] Update: The particle weight information is calculated by matching the point cloud information scanned by LiDAR with the particle position map. The measurement data is placed under the pose of each particle, and the probability of the measurement data occurring is determined. The particle weight is updated based on this probability.

[0119] Clustering: Based on weight information, particles are clustered and their mean values ​​are calculated to achieve current location. Location information can be obtained through the following methods:

[0120]

[0121] Where P k Let L be the pose obtained by the lidar at time k, L be the number of particles in the largest cluster after clustering weights, and ω be the number of particles in the largest cluster. i Let P be the weight of the i-th particle. k,i Let be the pose of the i-th particle at time k;

[0122] Optimization: The number and distribution of particles are optimized considering resampling and KLD sampling conditions;

[0123] Repeated prediction, update, clustering, and optimization processes.

[0124] The specific implementation method is as follows:

[0125] (1) Particle generation:

[0126] Based on the predetermined initial pose P0(x0,y0,θ0) of the unmanned vehicle, where x0, y0, and θ0 are the abscissa, ordinate, and heading angle, respectively, N uniformly distributed particles are randomly generated within the vehicle's initial motion range, and each particle has a corresponding pose (x0, y0, θ0). i ,y0 i ,θ0 i ), i = 1, 2, 3, ..., N.

[0127] (2) Update particle states using the autonomous vehicle motion model:

[0128] The state of the particles is updated based on the motion pose prediction equation of the mobile robot.

[0129]

[0130] Among them, P k P k-1 Let V represent the poses of the robot at time k and time k-1, respectively, corresponding to the two consecutive frames of laser data received. Let Δt be the time interval between the two times. k-1 Let ω be the velocity value of the mobile robot at time k-1. k-1 Let be the system noise at time k-1. To obtain the x-axis moving speed, y-axis moving speed, and rotational angular velocity at time k-1 using odometer information;

[0131] (3) Update particle weights:

[0132] The particle weights are updated using observations obtained from laser sensor-based and feature-code-based vision localization. As observations arrive sequentially, a corresponding weight is calculated for each particle; this weight represents the probability of obtaining the observation at the predicted pose for each particle. This evaluation is performed on all particles, with particles closer to the observed value receiving higher weights. The weights are calculated as follows:

[0133] ω i =1 / d i

[0134] Where ω i Let d be the weight of the i-th particle. i The distance is the Euclidean distance taken by the observation model for the i-th particle.

[0135] (4) Calculate the estimated values ​​of the state variables:

[0136] The estimated value of the state variable is calculated by taking the weighted average of all particles. That is, the estimated position of the robot when the unmanned vehicle receives the k-th frame of laser data.

[0137] (5) Resampling:

[0138] To address the weight degradation problem during the computation process, an effective particle number N is used. eff Measuring the degree of particle degradation:

[0139]

[0140] Where ω i The weight of the i-th particle, and the number of effective particles N. eff The smaller the value, the more severe the weight degradation. When N... eff The value is less than the threshold N thIf the condition is met, resampling is performed; otherwise, return to step 2. The specific resampling method is as follows: particles are filtered according to their weights. During the filtering process, the weights ω are adjusted. i The smallest particle is replaced with the particle with the largest weight, and so on, until N. eff Greater than threshold N th Repeat steps (2) to (5) until the mobile robot stops moving.

[0141] 2) Implement a positioning algorithm based on AprilTag positioning code to enable the bottom camera to recognize and obtain the robot's pose information.

[0142] The bottom camera is mounted at the center of the bottom of the mobile robot. When the robot moves over the location code, the bottom camera scans the code to achieve positioning. The specific implementation process is as follows:

[0143] The AprilTag recognition process begins with object detection, specifically using a clustering algorithm to detect line segments. Further, a quadrilateral detection algorithm selects the line segments. A depth-first search is employed to record the annular region formed by four line segments, obtaining the required quadrilateral. This quadrilateral is then transformed into a two-dimensional plane, and its midpoint is identified. Finally, it is inversely mapped back onto the original image to determine the center of the location code and obtain the deviation (Δx, Δy) in the image coordinate system.

[0144] As one example, the image is decoded using the AprilTag 36h11 type location code, with 36 valid squares. During decoding, black squares are defined as 1, and white squares as 0. Recognition proceeds sequentially from the top left to the bottom right. The decoded values ​​obtained in the four directions are different, but the AprilTag 36h11 type location code is designed so that only one direction can yield the correct decoded value. By calculating the rotation angle at which the correct decoded value is obtained, the relative angle Δθ in the image coordinate system can be calculated.

[0145] The positional deviation of the mobile robot relative to the positioning code can be obtained by the deviation (Δx, Δy) in the image coordinate system:

[0146]

[0147] Where Δx w Δy w The deviation value between the bottom camera and the AprilTag location code, z h The distance between the camera and the ground was determined when the camera was fixed. x f y u0 and v0 are internal parameters of the camera.

[0148] At the same time, the decoded value contains the location information of the AprilTag positioning code on the map, which can obtain the absolute pose on the map, and thus obtain the absolute pose of the mobile robot on the map.

[0149] 3) Construct a multi-rate sensor information model, such as Figure 3 As shown, when differences in positioning accuracy and information feedback rate exist during multi-sensor fusion, the efficiency of information utilization can be improved by optimizing the information receiving and processing method and distinguishing between primary and secondary moments. The specific process is as follows:

[0150] The sensors are differentiated based on information frequency and accuracy. During movement, the mobile robot can only achieve visual positioning by recognizing the AprilTag positioning code, and there is no cumulative error. Relatively speaking, this method has high positioning accuracy but low and inconsistent positioning information frequency. The lidar used in this invention obtains distance information from various angles through rotation. When the lidar acquires a complete circle of distance information, the positioning is updated accordingly. It is affected by its own sensor frequency but not by the movement of the mobile robot. Compared to visual positioning, it has lower positioning accuracy but higher positioning information frequency.

[0151] Based on frequent and infrequent positioning information, positioning time points are categorized into primary and secondary moments. In a multi-sensor fusion positioning system, positioning accuracy and frequency often differ, generally categorized as high-frequency, low-precision sensors and low-frequency, high-precision sensors. For example, in the fusion of inertial navigation and global navigation satellite systems (GNSS), inertial navigation typically operates at millisecond-level positioning frequencies and suffers from cumulative errors, resulting in decreasing accuracy over time. In contrast, GNSS positioning frequencies are at the second level and do not exhibit cumulative errors. Therefore, in the fusion of inertial navigation and GNSS, inertial navigation sensors function as high-frequency, low-precision sensors, while GNSS sensors function as low-frequency, high-precision sensors. This invention fuses visual and lidar information for positioning. Visual information, influenced by the camera's field of view, has a low positioning frequency and inherent uncertainty, yet its positioning accuracy reaches millimeter-level and is unaffected by cumulative errors. Lidar information, with a positioning frequency generally above 6Hz correlated with the radar sensor frequency, suffers from odometry-related accuracy issues and may exhibit local divergence. The primary moment refers to the moment when visual positioning with high accuracy but low frequency is obtained, while the secondary moment refers to the moment when lidar positioning with low accuracy but high frequency is obtained.

[0152] like Figure 3 As shown, the main time point refers to the moment when high-precision but low-frequency visual positioning is achieved, and the acquired positioning information is y. sThe secondary moment is the moment when the positioning accuracy is lower but the positioning frequency is higher, and the acquired positioning information is y. m .

[0153] 4) Based on the extended Kalman filter, a state compensation model and a measurement compensation model based on neural networks are introduced to achieve optimal state estimation through state prediction and state update processes.

[0154] For a nonlinear process:

[0155] X k =f(X) k-1 )+u k-1

[0156] Z k =h(X) k )+v k

[0157] Where X k Z k Let f(·) and h(·) represent the state vector and measurement vector at time k, respectively, and let u represent the state model and measurement model under nonlinear conditions. k-1 and v k Let Q and R represent the noise of the state model at time k-1 and the noise of the measurement model at time k, respectively. These noises are defined as white noise in the extended Kalman filter, and are characterized by being uncorrelated and having a mean of zero. In the state model and the measurement model, their covariance matrices are represented as Q and R, respectively.

[0158] Prediction process:

[0159] calculate The state equation is expressed as:

[0160]

[0161] The predicted representation of the covariance matrix is ​​as follows:

[0162]

[0163] in and These are the prior estimate of the state at time k and the posterior estimate of the state at time k-1, respectively. and P k-1 F represents the prior estimate of the state error covariance matrix at time k and the updated state error covariance matrix at time k-1, respectively. k-1 It is the Jacobian matrix of the linearized state model, and its value is calculated by the following formula:

[0164]

[0165] Where f is the state equation and X is the state vector.

[0166] Update process:

[0167] The gain of the update process is expressed as:

[0168]

[0169] Where K k H is the system gain. k The Jacobian matrix of the linearized measurement model is calculated using the following formula:

[0170]

[0171] Where h is the measurement equation. Posterior estimate.

[0172] The posterior state estimate at time k is expressed as:

[0173]

[0174] Z k It is a true measurement value. To measure the output value predicted by the measurement model, the measurement equation can be expressed as:

[0175]

[0176] The updated state estimation error covariance matrix is:

[0177]

[0178] Where P k Let I be the covariance matrix of the updated state estimation error, and let I be the identity matrix.

[0179] 5) Introduce a state compensation model and a measurement compensation model based on neural networks to compensate for errors in the model building process, including external interference errors and errors caused by insufficient understanding during the modeling process. The specific process is as follows:

[0180] Modify the nonlinear equation described in step 4):

[0181] X k =f true (X k-1 )+u k-1

[0182] Z k =h true (X k )+v k

[0183] Where f true (·) and h true (·) represent the actual state model and the actual measurement model, respectively. For extended Kalman filtering, the constructed model will deviate somewhat from the actual model. The reasons for this deviation may be as follows:

[0184] Inaccuracies in the state and measurement models may be due to a lack of experience during the construction process.

[0185] During system operation, changes in the external environment can cause deviations in the original model.

[0186] External noise interference is not simply white noise and may be correlated.

[0187] To compensate for potential errors, a two-layer compensation neural network is introduced based on the extended Kalman filter. The specific process is as follows: Figure 4 As shown.

[0188] Modify the state equation and measurement equation:

[0189]

[0190]

[0191] Where N X and N Z The outputs of the state-compensated neural network and the measurement-compensated neural network are respectively. To compensate for the weights and biases of the neural network at time k-1, The weights and biases of the compensation neural network are measured at time k.

[0192] To obtain accurate positioning information, the weights and biases of the state and measurement neural networks need to be trained. This invention uses the measurement residuals of each of the multi-rate sensors to update the corresponding neural network parameters. High-frequency, low-precision sensor information is used to update the weights and biases of the measurement compensation neural network, while low-frequency, high-precision sensor information is used to update the weights and biases of the state compensation neural network. Simultaneously, when only high-frequency sensor information is received, i.e., at the secondary time point, only φ is updated. Z The weights and biases of the two neural networks are added to the state, i.e.:

[0193]

[0194] Where X is the state vector of the nonlinear system. This is the augmented state vector.

[0195] Therefore, the state equation and measurement equation in the prediction process are modified as follows:

[0196]

[0197]

[0198] in This is for the augmented prior state estimation. We first consider the secondary time step, where only information from the high-frequency sensor is acquired. During the prediction phase, the parameters of the neural network remain unchanged, i.e., φ. k =φ k-1 , φ k and φ k-1 These are the parameters of the two neural networks at time k and time k-1, respectively.

[0199] The prediction of the covariance matrix can be transformed into:

[0200]

[0201] in The Jacobian matrix of the augmented state model can be calculated at second-order times as follows:

[0202] in and Represented as φ X and φ Z Vector-aligned identity matrix;

[0203] During the prediction process, the weights and biases remain unchanged, and the state-compensated neural network does not include φ. Z Furthermore, the Kalman gain matrix can be expressed as:

[0204]

[0205] in To augment the Jacobian matrix of the measurement model. For sensor systems with multiple high-frequency, low-precision sensors, It can be represented as:

[0206]

[0207] Where N Z Related to the parameters of the state-compensated neural network, we can obtain:

[0208]

[0209] Update phase:

[0210]

[0211]

[0212] During the primary timeframe, the low-frequency, high-precision sensor acquires positioning information, and its prediction and update process is similar to that of the secondary timeframes, but the measurement equations change:

[0213]

[0214] in h is the measurement value of the nth low-frequency, high-precision sensor. m For the measurement equation of the m-th high-frequency, low-precision sensor, N Z,m The output of the measurement compensation neural network for the m-th high-frequency, low-precision sensor is determined. Let be the prior state estimate of the nth low-frequency, high-precision sensor, and simultaneously the Jacobian matrix. In the main time, it can be represented as:

[0215]

[0216]

[0217] Among them I n Let n be the identity matrix.

[0218] 6) Real-time error compensation is achieved by forward-computing the compensation values ​​of the final state compensation and measurement compensation networks;

[0219] The neural network structure and calculation of the output compensation value, both the state compensation and measurement compensation neural networks use single-hidden-layer networks. This invention uses a single-hidden-layer feedforward neural network as the basic structure of the state and measurement compensation neural networks. Figure 5 Here is a schematic diagram of its neural network structure. Figure 5 'a' represents a state-compensated neural network. Figure 5 b is a measurement compensation neural network, and n is allocated to each of the two networks. X and n Z The number of neurons. For the state compensation network, its input is the optimal state estimate from the previous time step. The output is state compensation. For a measurement compensation network, the input is the corrected prior state estimate at the current time. The output is measurement compensation. Tanh is used as the activation function for each neuron in both the state-compensated neural network and the measurement-compensated neural network.

[0220] The bias and weights in the neural network are updated using the positioning information obtained from the main time step and the secondary time step. For the state compensation neural network, the corresponding output is represented as follows:

[0221]

[0222] in and These represent the weights and biases from the input layer to the hidden layer, respectively. and These represent the weights and biases from the hidden layer to the output layer, respectively.

[0223]

[0224]

[0225] Where n is the number of low-frequency high-precision sensors, and the subscripts of the weights w and biases b indicate the connections of neurons, while the superscripts indicate the number of layers. Represented as the nth neuron in the first layer to the nth neuron in the second layer X The weights of each neuron, For the second layer nth X The weights from the nth neuron in the third layer to the nth neuron in the third layer. This is the optimal state estimate of the nth low-frequency high-precision sensor at the previous time step. For the second layer nth X The deviation of a single neuron This represents the deviation of the nth neuron in the third layer;

[0226] For a measurement compensation neural network, its output can be expressed as:

[0227]

[0228] in and The weights and biases from the input layer to the hidden layer. and These represent the weights and biases from the hidden layer to the output layer:

[0229]

[0230]

[0231] Where m represents the number of high-frequency and high-precision sensors. Represented as the nth neuron in the first layer to the nth neuron in the second layer Z The weights of each neuron, For the second layer nth Z The weights from the m-th neuron in the third layer to the m-th neuron in the third layer. This represents the prior state estimate of the m-th high-frequency, low-precision sensor at time k. For the second layer nth Z The deviation of a single neuron This represents the deviation of the m-th neuron in the third layer.

[0232] 7) Obtain the optimal estimate of the current time using different methods for the major and minor time points. The specific process is as follows:

[0233] This invention distinguishes between primary and secondary moments. A secondary moment occurs when only information from high-frequency, low-precision sensors is received. In this case, the algorithm updates the mobile robot's localization using low-precision sensor information, ensuring real-time localization during movement. Besides eliminating abnormal information, the high-frequency sensors maintain a relatively stable information acquisition frequency, enabling consistent localization updates. Simultaneously, the weights and parameters of the measurement compensation neural network are updated, optimizing the network model and further reducing localization errors.

[0234] When a low-frequency, high-precision sensor acquires information, the matching time is fused with the secondary time closest to the main time. An extended Kalman filter algorithm is implemented through a two-layer neural network to achieve state prediction and state update, including parameter updates for the state compensation neural network and the measurement compensation neural network.

[0235] In practical mobile robot localization methods, achieving high-precision and stable positioning using a single sensor typically incurs high costs. Therefore, multi-sensor fusion technology is commonly used to improve positioning accuracy while reducing the cost of high-precision positioning, or to further enhance positioning accuracy and reliability.

[0236] The positioning method based on neural networks and lidar described in this invention can also be applied to sensor fusion with the following characteristics: The minimum locatable information acquired by sensors can be either high-frequency or low-frequency, resulting in significant differences in positioning accuracy achieved through a single sensor. Furthermore, high-frequency sensors typically correspond to low accuracy or high cumulative error, such as IMUs, lidar, and encoders, while low-frequency sensors typically correspond to high accuracy, such as vision sensors. Sensors possessing both of these properties require higher costs; therefore, this invention aims to reduce the cost of high-precision positioning. Figure 3 As shown, high-frequency information is represented by y during the sensor information acquisition process. m This indicates that low-frequency, high-precision information is represented by y. s This indicates that the time interval for acquiring high-frequency information is t. m The time interval for acquiring high-frequency information is t. s During the acquisition of location information, high-frequency information y m Typically, these are fixed-frequency sensors, providing stable frequency positioning information to ensure the real-time positioning of mobile robots. Low-frequency information y s It possesses high positioning accuracy, significantly improving positioning precision within positioning algorithms. Its positioning frequency is low, yet it allows for some variation in its positioning frequency, such as... Figure 3 As shown, y is obtaineds time interval t s It is not constant. In positioning algorithms, it can significantly improve positioning accuracy.

[0237] The positioning method based on neural networks and lidar described in this invention can be applied to industrial environments, including but not limited to warehousing environments and assembly workshops. In warehousing environments, the total input and output of goods is large, placing high demands on the movement speed of mobile robots. In factory environments, the docking process of workpieces places even higher demands on the positioning accuracy and operational stability of mobile robots.

[0238] The positioning method based on neural networks and lidar described in this invention can achieve stable and high-precision positioning, meeting the requirements of factory and warehouse environments. In known environments, lidar enables the positioning and navigation of mobile robots, facilitating convenient path planning. Lidar maintains good positioning performance even during movement, meeting the needs of rapid movement in industrial scenarios. In industrial scenarios where positioning accuracy requirements are lower during movement, the following methods can be used to improve positioning accuracy:

[0239] During the lidar positioning process, the positioning variance will gradually increase over time. An upper limit for the variance is set, and the state is predicted and updated through visual positioning after the upper limit is reached.

[0240] For scenarios requiring high positioning accuracy throughout the entire movement process, the AprilTag method can be used. One type of laying scheme is a grid-type laying, where positioning codes are laid out at fixed intervals. During path planning, the mobile robot only moves between positioning codes, allowing for visual positioning updates during operation and ensuring the robot's positioning accuracy.

[0241] This invention describes a positioning method based on neural networks and LiDAR, suitable for indoor document delivery and item transport using composite robots in office environments. Employing LiDAR sensors facilitates the deployment of multiple robots and map sharing. A shared map enables interaction between multiple robots and map updates. In office settings, numerous human factors make it impractical to lay positioning codes on the ground. Furthermore, office environments often have many load-bearing columns in corridors and buildings. Therefore, the positioning codes of this invention can be affixed to walls, and a visual camera can be used to identify the positioning codes in front of vehicles, enabling convenient sensor fusion positioning during movement.

[0242] In real-world scenarios, LiDAR can also be used for dynamic obstacle avoidance to prevent collisions and ensure the safety of mobile robots in complex environments. Secondly, camera-based pose recognition enables the application of more visual recognition algorithms, such as facial recognition and object detection, thereby enhancing the intelligence of mobile robots.

[0243] The preferred embodiments of the present invention disclosed above are merely illustrative of the invention. These preferred embodiments do not exhaustively describe all details, nor do they limit the invention to the specific implementations described. Clearly, many modifications and variations can be made based on the content of this specification. This specification selects and specifically describes these embodiments to better explain the principles and practical applications of the invention, thereby enabling those skilled in the art to better understand and utilize the invention. The invention is limited only by the claims and their full scope and equivalents.

Claims

1. A mobile robot localization method based on neural networks and lidar, characterized in that, Includes the following steps: S1. Establish a 2D lidar measurement and positioning model, and realize the positioning of the mobile robot in a known map environment through lidar and odometry. S2. Using a positioning algorithm based on AprilTag positioning codes, the bottom camera is used to identify and obtain the pose information of the mobile robot. S3. Construct a multi-rate sensor information model. When there are differences in positioning accuracy and information feedback rate during the multi-sensor fusion process, optimize the information structure to improve information utilization efficiency. S4. A multi-sensor fusion algorithm based on a two-layer neural network is used to achieve the fusion positioning of lidar information and visual information. The multi-sensor fusion algorithm is based on extended Kalman filtering and includes a state prediction process and a state update process, which are implemented using positioning information from two sensors respectively. The fusion process improves positioning accuracy by updating the covariance matrix. Step S4 specifically includes: For a nonlinear process: in , Represented as k The state vector and measurement vector at each moment, and This is represented as a state model and measurement model under nonlinear conditions. and They represent k Noise in the state model at time -1 k The noise of the time-mapping model is represented by the covariance matrices of the state model and the measurement model, respectively, as Q and R. Prediction process: calculate The state equation is expressed as: The predicted representation of the covariance matrix is ​​as follows: in and These are the prior estimate of the state at time k and the posterior estimate of the state at time k-1, respectively. and These represent the prior estimate of the state error covariance matrix at time k and the updated state error covariance matrix at time k-1, respectively. It is the Jacobian matrix of the linearized state model, and its value is calculated by the following formula: in The state equation is... It is a state vector; Update process: The gain of the update process is expressed as: in For gain, The Jacobian matrix of the linearized measurement model is calculated using the following formula: in For the measurement equation, Posterior estimate; The posterior state estimate at time k is expressed as: in For the actual measurement value, To measure the output value predicted by the measurement model, the measurement equation is expressed as: The updated state estimation error covariance matrix is: in The covariance matrix of the updated state estimation error. It is the identity matrix; S5. Introduce a state compensation model and a measurement compensation model based on neural networks to compensate for errors in the construction of the state model and measurement model, including external interference errors and errors caused by insufficient understanding in the modeling process. S6. Real-time error compensation is achieved by forward-computing the compensation values ​​of the final state compensation and measurement compensation networks; S7, the multi-sensor fusion algorithm of the dual-layer neural network distinguishes between the main time and the secondary time during operation, and uses different methods to obtain the optimal estimate of the current time for the two types of time.

2. The mobile robot localization method based on neural networks and lidar according to claim 1, characterized in that, Step S1 includes the following steps: The measurement and positioning model of the 2D lidar uses a particle filtering method for positioning, with the initial pose as the mean of the initial particle distribution, and the initial particle distribution is controlled according to the prior position. Prediction: Using odometry information, predict the position of the particle at the next moment; Update: The particle weight information is updated by matching the point cloud information scanned by LiDAR with the particle position map; Clustering: Based on the weight information, the particles are clustered and the mean is calculated to achieve the current location. Optimization: Optimize the positions of particles with smaller weights and update the particle positions according to the motion model; Repeated prediction, update, clustering, and optimization processes.

3. The mobile robot localization method based on neural networks and lidar according to claim 1, characterized in that, In step S2, the bottom camera recognizes the AprilTag positioning code to obtain the pose information of the mobile robot. A camera is installed at the center of the bottom of the mobile robot to obtain the image directly below it. When the mobile robot moves to the top of the AprilTag positioning code laid on the ground, the camera recognizes the AprilTag positioning code. Identify AprilTag location code information, obtain number information and homography matrix; The number information is used to obtain the absolute position information of the AprilTag location code in the map environment. The deviation between the camera and the AprilTag location code can be obtained by using the homography matrix, the extrinsic parameter estimation of the camera, and the coordinates of the center point of the AprilTag location code in the image. The pose of the mobile robot in the map environment is obtained by the relative deviation between the camera and the AprilTag positioning code, as well as the absolute position information of the AprilTag positioning code in the map environment.

4. The mobile robot localization method based on neural networks and lidar according to claim 1, characterized in that, In step S3, the multi-rate sensor information model classifies sensors into high-frequency low-precision sensors and low-frequency high-precision sensors based on information frequency and accuracy, and classifies positioning time nodes into primary time and secondary time.

5. The mobile robot localization method based on neural networks and lidar according to claim 1, characterized in that, In step S5, a state compensation model and a measurement compensation model based on a neural network are introduced, and the nonlinear process is modified. The specific process is as follows: in and These represent the actual state model and the actual measurement model, respectively. During the construction of the state model and the measurement model, deviations between the model and the actual situation may occur due to changes in the external environment. To compensate for the existing errors, the state equation and measurement equation are modified: in and The outputs of the state-compensated neural network and the measurement-compensated neural network are respectively. To compensate for the weights and biases of the neural network at time k-1, The weights and biases of the compensation neural network are measured at time k.

6. The mobile robot localization method based on neural networks and lidar according to claim 1, characterized in that, After introducing the compensation model, the prediction and update process changes, as follows: Using high-frequency, low-precision sensor information to update the measurement compensation neural network Using low-frequency, high-precision sensor information to update the state compensation neural network Meanwhile, when only high-frequency sensor information is received, i.e., at the secondary moment, only updates are performed. The weights and biases of the two neural networks are added to the state, i.e.: in For the state vector of a nonlinear system, This is the augmented state vector; Modify the state equations and measurement equations in the prediction process: in For the augmented prior state estimation, we first consider the secondary time step, where only information from the high-frequency sensor is acquired. During the prediction phase, the parameters of the neural network remain unchanged, i.e.: , and These are the parameters of the two neural networks at time k and time k-1, respectively. The prediction of the covariance matrix is ​​transformed into: in The Jacobian matrix of the augmented state model can be calculated at second-order times as follows: and Represented as and and Vector-aligned identity matrix; During the prediction process, the weights and biases remain unchanged, and the state-compensated neural network does not include... The Kalman gain matrix can be expressed as: in To augment the Jacobian matrix of the measurement model, for sensor systems with multiple high-frequency but low-precision sensors, It can be represented as: in Related to the parameters of the state-compensated neural network, we obtain: Update phase: During the primary timeframe, the low-frequency, high-precision sensor acquires positioning information, and its prediction and update process is similar to that of the secondary timeframes, but the measurement equations change: in This is the measurement value of the nth low-frequency, high-precision sensor. Let be the measurement equation for the m-th high-frequency, low-precision sensor. The output of the measurement compensation neural network for the m-th high-frequency, low-precision sensor is determined. Let be the prior state estimate of the nth low-frequency, high-precision sensor, and simultaneously the Jacobian matrix. , In the main time interval, it is represented as: in Let n be the identity matrix.

7. The mobile robot localization method based on neural networks and lidar according to claim 1, characterized in that, Based on neural network-based state compensation and measurement compensation models, a single-hidden-layer feedforward neural network is used as the structure for both the state compensation and measurement compensation neural networks. The two networks are respectively allocated... and The number of neurons, where, for a state-compensated neural network, the input is the optimal state estimate from the previous time step. The output is state compensation. For the measurement compensation neural network, its input is the corrected prior state estimate at the current time. The output is measurement compensation. ; use It serves as the activation function for each neuron in both state-compensated neural networks and measurement-compensated neural networks.

8. A mobile robot localization method based on neural networks and lidar according to claim 1, characterized in that, In step S6, the bias and weights in the neural network are updated using the positioning information obtained at the main time step and the secondary time step. The specific process is as follows: For a state-compensated neural network, the corresponding output is represented as: in and These represent the weights and biases from the input layer to the hidden layer, respectively. and These represent the weights and biases from the hidden layer to the output layer, respectively. , , , Where n is the number of low-frequency high-precision sensors, and the weights are... and deviation The subscript indicates the connection of neurons, and the superscript indicates the layer number. Represented as the nth neuron in the first layer to the nth neuron in the second layer The weights of each neuron, For the second layer The weights from the nth neuron in the third layer to the nth neuron in the third layer. This is the optimal state estimate of the nth low-frequency high-precision sensor at the previous time step. For the second layer The deviation of a single neuron This represents the deviation of the nth neuron in the third layer; For the measurement compensation neural network, its output is represented as: in and The weights and biases from the input layer to the hidden layer. and These represent the weights and biases from the hidden layer to the output layer: , , , Where m represents the number of high-frequency and high-precision sensors. Represented as the nth neuron in the first layer to the nth neuron in the second layer The weights of each neuron, For the second layer The weights from the m-th neuron in the third layer to the m-th neuron in the third layer. This represents the prior state estimate of the m-th high-frequency, low-precision sensor at time k. For the second layer The deviation of a single neuron This represents the deviation of the m-th neuron in the third layer.

9. A mobile robot localization method based on neural networks and lidar according to any one of claims 1 to 8, characterized in that, In step S7, the optimal estimate of the current time is obtained using different methods for the main time and the secondary time, as detailed below: When only information from high-frequency, low-precision sensors is acquired at the current moment, it is considered a secondary moment. At this time, the multi-sensor fusion algorithm of the dual-layer neural network updates the positioning of the mobile robot using the high-frequency, low-precision sensor information, ensuring the real-time positioning of the mobile robot during movement. In addition to eliminating abnormal information, the high-frequency, low-precision sensors are stable in terms of information acquisition frequency, enabling stable positioning information updates. At the same time, the weights and parameters of the measurement compensation neural network are updated to optimize the measurement compensation neural network model and reduce positioning errors. When a low-frequency, high-precision sensor acquires information, the matching time is fused with the secondary time closest to the main time. The extended Kalman filter algorithm is improved by using a state compensation neural network and a measurement compensation neural network to achieve state prediction and state update, including updating the parameters of the state compensation neural network and the measurement compensation neural network.

Images