Online dead reckoning error prediction method based on robot motion and earth surface perception

Abstract

The invention provides an online dead reckoning error prediction method based on robot motion and earth surface perception. The method comprises the following steps: constructing an online dead reckoning error neural network predictor model of a robot, wherein the online dead reckoning error neural network predictor model comprises a body convolutional neural network CNN, a visual CNN and a prediction MLP; parameters of the online dead reckoning error neural network predictor are trained, and a trained online dead reckoning error neural network predictor model is obtained; and performing online dead reckoning error prediction on the robot by using a machine body CNN, a visual CNN and a prediction MLP in the trained online dead reckoning error neural network predictor model. According to the method, the neural network parameters can be incrementally optimized on line by using the robot data, and the method has continuous learning ability, so that the adaptability of a robot positioning system to complex motion and varied surface conditions is improved, the method has very high application value, and the blank in the aspect of robot dead reckoning error prediction is filled up.

Description

Technical Field

[0001] The present invention relates to the technical field of dead reckoning, and particularly to an online dead reckoning error prediction method based on robot motion and ground perception. Background Art

[0002] Dead reckoning positioning is a method that uses sensor inputs such as wheel speed encoders and IMUs (Inertial Measurement Units) to estimate the relative pose change of a robot between frames (relative positioning) based on a kinematic model, and then calculates the global pose of the robot (global positioning). Positioning is the key for a mobile robot to achieve autonomous navigation. In the absence of satellite positioning, the current mainstream mobile robot positioning system estimates the relative pose change (relative positioning) by fusing dead reckoning and environmental perception, and then calculates the position and pose in the global coordinate system (global positioning) through methods such as calculation, loop detection, and optimization. Dead reckoning (DR), that is, a method that uses sensor inputs such as wheel speed encoders and inertial measurement units (IMUs) to estimate the relative pose change of a robot between frames based on a kinematic model, and then calculates the global pose of the robot.

[0003] Environmental perception positioning refers to achieving relative positioning (such as visual odometry VO, laser odometry LO) or global positioning (such as simultaneous localization and mapping SLAM) of a robot through data matching of environmental perception sensors such as cameras and lidar. Dead reckoning and environmental perception are different modalities of positioning methods, using different sensors and algorithms, and having their own advantages and disadvantages. Multi-modal fusion positioning aims to give full play to the advantages of each method and make them complementary. The key lies in accurately evaluating the positioning errors of each modality in different scenarios, so that the modality with small error and high accuracy can play a greater role.

[0004] Methods for evaluating environmental perception positioning errors have been widely studied. Currently, an environmental perception positioning error evaluation method in the prior art includes: a visual / laser odometry error prediction model based on environmental perception positioning, which maps environmental data collected by a camera or lidar into an information matrix of odometry error, so as to improve the adaptability of robot fusion positioning in different scenarios. On the other hand, although dead reckoning is widely used and regarded as a basic model for mobile robot positioning, the positioning error of dead reckoning has rarely been studied in depth. In practical applications, the dead reckoning positioning error is often estimated by constructing a simplified error model and using a pre-calibrated error coefficient.

[0005] The disadvantages of the above-mentioned online dead reckoning error prediction method in the prior art include: Since the pre-calibrated error coefficients are used, it is impossible to adapt to the robot's motion and environmental changes online, and it is difficult to accurately estimate the error. Different robot motions bring different dead reckoning errors. In particular, measurement errors such as wheel speed encoders and IMUs are not only related to the robot's motion but also greatly affected by the surface conditions. For example, on a slippery surface, although the measured value of the wheel speed encoder is large, due to wheel slip, the actual travel of the robot may be very short, resulting in a large odometry error. Similarly, the wheel speed encoder also has a large odometry error in surface environments such as muddy, sandy, carpeted, and grassy areas. Summary of the Invention

[0006] An embodiment of the present invention provides an online dead reckoning error prediction method based on robot motion and surface perception to effectively improve the positioning ability of the robot.

[0007] To achieve the above object, the present invention adopts the following technical solutions.

[0008] An online dead reckoning error prediction method based on robot motion and surface perception includes:

[0009] Construct an online dead reckoning error neural network predictor model for the robot, including a body convolutional neural network (CNN), a vision CNN, and a prediction multi-layer perceptron (MLP);

[0010] Train the parameters of the online dead reckoning error neural network predictor to obtain a trained online dead reckoning error neural network predictor model;

[0011] Use the body CNN, vision CNN, and prediction MLP in the trained online dead reckoning error neural network predictor model to predict the online dead reckoning error of the robot.

[0012] Preferably, the construction of the online dead reckoning error neural network predictor model for the robot, including a body CNN, a vision CNN, and a prediction MLP, includes:

[0013] Construct an online dead reckoning error neural network predictor model including a body CNN, a vision CNN, and a prediction MLP. The vision CNN uses the pre-trained MobileNet v2 model on ImageNet, inputs image patches of 224×224, and outputs a 1×512 vision feature vector. The body CNN uses a one-dimensional convolutional neural network, inputs a 1×7 vector, which includes the speed obtained from the wheel speed encoder, the linear accelerations in 3 dimensions and the angular velocities in 3 dimensions obtained from the IMU, and after upsampling, outputs a 1×256 body feature vector. The prediction MLP uses a multi-layer perceptron, takes as input the concatenated vector of the 1×768 vision feature vector and the body feature vector, and outputs a 1×4 error coefficient vector.

[0014] Preferably, training the parameters of the online dead reckoning error neural network predictor to obtain a trained online dead reckoning error neural network predictor model includes:

[0015] Let the relative pose estimates of the robot obtained by dead reckoning and vision / laser odometry be u t and z t , and let μ t be the fused relative positioning result. Assume that the errors of u t , z t , and μ t all follow Gaussian distributions, and their error covariance matrices are R t , Q t , and Σ t respectively;

[0016] Obtain z t using vision / laser odometry, calculate Q t using the matching error of corresponding point pairs, obtain u t using the dead reckoning formula 3, obtain R Θ using the online dead reckoning error neural network predictor π t , perform fusion positioning calculation using the information filter method to solve for μ t , and Σ t . Let the information matrix be The information vector is ξ t =Ω t μ t . Denote the fusion positioning algorithm as This processing process is abbreviated as:

[0017]

[0018] Specific calculation steps

[0019] Based on the dead reckoning prediction information matrix

[0020] Dead reckoning prediction information vector

[0021] Update the information matrix using environmental perception

[0022] Update the information vector using environmental perception

[0023] Calculate the covariance matrix of the integrated positioning

[0024] Calculate the integrated positioning result μ t = Σ t ξ t

[0025] Taking two-dimensional positioning as an example, the relative pose estimate μ at time t t = (μ x,t , μ y,t , μ θ,t ) T Contains the displacement and rotation angle of the robot in the x and y coordinate components relative to the previous time t-1, and is represented in the form of a coordinate transformation matrix:

[0026]

[0027] Given the initial position and pose of the robot in the global coordinate system And the relative positioning estimates μ 1 , μ 2 , … μ t The global pose estimation of the robot is as follows:

[0028]

[0029] Let Be the above coordinate transformation function, then it can be abbreviated as:

[0030]

[0031] Let I t Be the sensor data at the current time collected by the wheel speed encoder, IMU, and camera at time t, and π Θ Be the online dead reckoning error neural network predictor, where Θ represents the parameters of the neural network, and A t = (α 1 , …, α 4 ) t Is the error coefficient of the dead reckoning model, and R t Is the dead reckoning error covariance matrix estimated based on A t And formula 4, then the forward inference process of the robot's integrated positioning is as follows:

[0032] Covariance matrix \(R\) of dead reckoning error t =\(\pi\) Θ (I t )

[0033] Relative positioning by fusing dead reckoning and environmental perception

[0034] Global dead reckoning

[0035] Suppose the robot starts from the initial point with known true position After \(T\) frames, the true position in the global coordinate system is obtained The result of fused dead reckoning is Suppose \(T>\tau\), where \(\tau\) is the minimum number of iterative frames, and the loss is defined as:

[0036]

[0037] where \(\lambda\) is a hyperparameter for balancing position accuracy and orientation accuracy. Let \(\Theta\) be the parameters of the body CNN, visual CNN, and predictive MLP neural networks. Then the learning objective is to optimize \(\Theta\) to minimize the loss function \(J\):

[0038]

[0039] During the training process, for each training trajectory, the loss \(J\) is calculated according to Equation (10), and the neural network parameters are updated according to Equation (12). This process is repeated until the network parameters converge;

[0040] A trained online dead reckoning error neural network predictor model is obtained

[0041] Preferably, using the body CNN, visual CNN, and predictive MLP in the trained online dead reckoning error neural network predictor model to predict the online dead reckoning error of the robot includes:

[0042] Collect the body data of the robot to be recognized, including travel, acceleration, and angular velocity, through a wheel speed encoder and an IMU. Input the body data into the trained online dead reckoning error neural network predictor model. Cut a 224×224 image patch from the bottom of the original 1024×768 image in the body data and input it into the visual CNN. The visual CNN outputs a 1×512 visual feature vector. Input a 1×7 vector including the linear acceleration in 3 dimensions obtained from the speed sensor and the angular velocity in 3 dimensions obtained from the inertial measurement unit IMU into the body CNN. The body CNN outputs a 1×256 body feature vector. Connect the visual feature vector and the body feature vector to obtain a 1×768 vector. Input the 1×768 vector into the prediction MLP, and output a 1×4 error coefficient vector α 1 ,…,α 4 , and then use the following formulas (1)-(5) to obtain the dead reckoning result u of the robot to be recognized t of the covariance matrix R of the error t ;

[0043] Suppose that during the process from time t−1 to time t, the travel of the robot measured by the wheel speed encoder is Δs, and the change in the heading angle of the robot measured by the IMU is Δθ. The time consumed in this process is Δt. Then the measured values of the speed v and angular velocity ω of the robot are calculated as follows:

[0044]

[0045] where ∈ t and ∈ ω are the measurement errors of the speed v and angular velocity ω, which follow a Gaussian distribution with zero mean and variances of α 1 Δs 2 +α 2 Δθ 2 ,α 3 Δs 2 +α 4 Δθ 2 , and α 1 ,…,α 4 are the error coefficients output by the aforementioned online dead reckoning error prediction model;

[0046] Use the robot motion model for dead reckoning, and estimate the relative pose u at time t t =(u x,t ,u y,t ,u θ,t ) T as follows:

[0047]

[0048] where, is the orientation angle of the robot in the global coordinate system at time t-1; ∈ t is u t The error of, which follows a zero-mean Gaussian distribution, R t is the covariance matrix of the error, calculated as follows:

[0049]

[0050] where

[0051]

[0052] Substitute the error coefficients α 1 ,…,α 4 output by the aforementioned online dead reckoning error prediction model into formula (4) to obtain the covariance matrix R t of the error of the dead reckoning result u t of the robot to be recognized.

[0053] As can be seen from the technical solutions provided by the embodiments of the present invention above, the method of the present invention does not rely on manual annotation, can use robot data to incrementally optimize neural network parameters online, has the ability of continuous learning, and thus improves the adaptability of the robot positioning system to complex movements and diverse surface conditions. It has strong application value and fills the gap in the prediction of dead reckoning errors of robots.

[0054] Additional aspects and advantages of the present invention will be given in part in the following description, which will become apparent from the following description, or can be understood through the practice of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0055] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings required for the description of the embodiments will be briefly introduced below. Obviously, the drawings in the following description are only some embodiments of the present invention. For those of ordinary skill in the art, other drawings can be obtained based on these drawings without creative efforts.

[0056] Figure 1 is the processing flow chart of an online dead reckoning error prediction method for robot motion and surface perception provided by the embodiments of the present invention;

[0057] Figure 2 is the schematic diagram of the implementation principle of an online dead reckoning error neural network predictor provided by the embodiments of the present invention;

[0058] Figure 3 is the schematic diagram of the implementation principle of a parameter training method for a dead reckoning error neural network predictor provided by the embodiments of the present invention. DETAILED DESCRIPTION OF THE INVENTION

[0059] Embodiments of the present invention will be described in detail below. Examples of the embodiments are shown in the accompanying drawings, where the same or similar reference numerals denote the same or similar elements or elements having the same or similar functions throughout. The embodiments described below by referring to the drawings are exemplary and are only used to explain the present invention and should not be construed as a limitation of the present invention.

[0060] Those skilled in the art of the present technology can understand that unless specifically stated otherwise, the singular forms "a", "an", "the" and "said" used herein may also include the plural forms. It should be further understood that the term "comprising" used in the specification of the present invention means the presence of the described features, integers, steps, operations, elements and / or components, but does not exclude the presence or addition of one or more other features, integers, steps, operations, elements, components and / or their groups. It should be understood that when we say that an element is "connected" or "coupled" to another element, it can be directly connected or coupled to other elements, or there may also be intermediate elements. In addition, the "connection" or "coupling" used herein may include wireless connection or coupling. The phrase "and / or" used herein includes any and all combinations of any one of the one or more related listed items.

[0061] Those skilled in the art of the present technology can understand that unless otherwise defined, all terms (including technical terms and scientific terms) used herein have the same meaning as the general understanding of those of ordinary skill in the field to which the present invention belongs. It should also be understood that terms such as those defined in a general dictionary should be understood to have a meaning consistent with the meaning in the context of the prior art and will not be interpreted with an idealized or overly formal meaning unless defined as here.

[0062] For the convenience of understanding the embodiments of the present invention, the following will further explain with several specific embodiments as examples in conjunction with the accompanying drawings, and each embodiment does not constitute a limitation to the embodiments of the present invention.

[0063] An embodiment of the present invention provides an online dead reckoning error prediction method for robot motion and ground surface perception. First, using the robot motion model and the error propagation theory, the dead reckoning positioning and error model are derived. Secondly, a neural network predictor is designed, which takes the sensor data such as robot motion and ground surface as input, and online predicts the error coefficient and calculates the covariance matrix of the dead reckoning error. Furthermore, in the fusion positioning framework including dead reckoning and environmental perception, using the online obtained robot data, with the goal of minimizing the fusion positioning error, the parameters of the dead reckoning error prediction network are trained unsupervised.

[0064] First, the dead reckoning and error model are given below, and then the method of the online dead reckoning error neural network predictor of the present invention is described.

[0065] Using the robot motion model and error propagation theory, the dead reckoning positioning and error model are deduced. Suppose that during the process from time t-1 to time t, the travel distance of the robot measured by the wheel speed encoder is Δs, and the change in the heading angle of the robot measured by the IMU (Inertial Measurement Unit) is Δθ. The time consumed for this process is Δt. Then the measured values of the speed v and angular velocity ω of the robot are calculated as follows:

[0066]

[0067] where ∈ t and ∈ ω are the measurement errors of the speed v and angular velocity ω, which follow a Gaussian distribution with zero mean and variances of α 1 Δs 2 +α 2 Δθ 2 , α 3 Δs 2 +α 4 Δθ 2 . Intuitively, the longer the travel distance and the larger the turning angle of the robot, the greater the possible measurement error, the greater the uncertainty brought to the estimation of the speed and angular velocity of the robot, and their effects are correlated. α 1 , …, α 4 are error coefficients, which are obtained by online prediction using a trained online dead reckoning error neural network predictor.

[0068] Using the robot motion model for dead reckoning, the relative pose u t =(u x,t , u y,t , u θ,t ) T at time t is estimated as follows:

[0069]

[0070] where, is the heading angle of the robot in the global coordinate system at time t-1; ∈ t is the error of u t , which follows a Gaussian distribution with zero mean, and R t is the covariance matrix of the error, calculated as follows:

[0071]

[0072] where

[0073]

[0074] The measured values of speed v and angular velocity ω, and the error coefficients α online predicted using the trained online dead reckoning error neural network predictor 1 ,…,α 4 Substituting the above into the formula can estimate the covariance matrix R t .

[0075] The specific implementation steps of the dead reckoning method at time t are as follows:

[0076] (1) Input the measured values of the wheel speed encoder and IMU during the process from time t−1 to time t

[0077] (2) Use formula 3 to calculate the relative pose change u of the robot t

[0078] (3) Use the trained online dead reckoning error neural network predictor to predict the error coefficients α online 1 ,…,α 4

[0079] (4) Substitute the measured values of speed v and angular velocity ω, and the error coefficients α 1 ,…,α 4 into formula 4 to calculate the covariance matrix R of the error t .

[0080] The processing flow of an online dead reckoning error prediction method for robot motion and ground surface perception provided by an embodiment of the present invention is as Figure 1 shown, and includes the following processing steps:

[0081] Step S10: Construct an online dead reckoning error neural network predictor model for the robot

[0082] The online dead reckoning error neural network predictor model includes a body CNN (Convolutional Neural Networks), a visual CNN, and a prediction MLP. The specific structure and parameter information are as follows:

[0083] 1. A visual CNN: Use the pre-trained MobileNet v2 model on ImageNet, input an image block of 224×224, and output a visual feature vector of 1×512;

[0084] 2. A body CNN: Use a one-dimensional convolutional neural network, input a vector of 1×7 (the speed obtained from the wheel speed encoder, the linear accelerations in 3 dimensions and the angular velocities in 3 dimensions obtained from the IMU), and after upsampling, output a body feature vector of 1×256;

[0085] 3. A predictive MLP: Using a multi-layer perceptron, the input is a vector obtained by concatenating a 1×768 visual feature vector and a body feature vector, and the output is a 1×4 error coefficient vector.

[0086] Step S20: Train the parameters of the online dead reckoning error neural network predictor to obtain a trained online dead reckoning error neural network predictor model.

[0087] Step S30: Use the trained online dead reckoning error neural network predictor model to perform online dead reckoning error prediction on the robot.

[0088] Input data of the trained online dead reckoning error neural network predictor model: Cut a 224×224 image patch from the bottom of the original 1024×768 image of the robot to be recognized and input it into the visual CNN; the speed, the linear accelerations in 3 dimensions and the angular velocities in 3 dimensions obtained from the IMU, and a 1×7 vector are input into the body CNN. Among them, the speed is calculated by the wheel speed encoder as follows: Let the wheel speed encoder count of two adjacent frames be A, the pre-calibrated travel-encoder conversion coefficient be B (for example, 1Count≈0.003846154 meter), and the inter-frame time be C, then the speed = A×B / C.

[0089] Output data: The output is a 1×4 error coefficient vector, and then using formula 4, the covariance matrix R of the error is obtained. t .

[0090] Processing process: Input the 224×224 image patch into the visual CNN to obtain a 1×512 visual feature vector, input the 1×7 vector into the body CNN to obtain a 1×256 body feature vector, concatenate the two feature vectors to obtain a 1×768 vector, input it into the predictive MLP, and output a 1×4 error coefficient vector α 1 ,…,α 4 , and then using formula 4, the covariance matrix R of the error of the above-mentioned robot to be recognized is obtained. t .

[0091] The schematic diagram of the implementation principle of an online dead reckoning error neural network predictor provided by an embodiment of the present invention is as Figure 2 shown. The online dead reckoning error neural network predictor uses the robot motion model and the error propagation theory to derive a dead reckoning error model containing multiple coefficients. Using the motion of the robot and sensor data such as the surface image at the current moment as input, it online predicts the dead reckoning error coefficients, so as to calculate the covariance matrix of the dead reckoning error.

[0092] As Figure 2As shown in the figure, body data such as the travel, acceleration, and angular velocity of the robot are collected through a wheel speed encoder and an IMU. The body data is input into a CNN network (referred to as the body CNN), and the body CNN extracts body features from the body data. On the other hand, the front road surface image is collected through a camera. The front road surface image is used as visual data, and the visual data is input into another convolutional neural network (referred to as the visual CNN), and the visual CNN extracts visual features from the visual data. After fusing the body features and visual features, they are input into a multi-layer perceptron (MLP, Multilayer Perceptron) (referred to as the prediction MLP), and the prediction error coefficient α 1 ,…,α 4 .

[0093] Let I t be the current sensor data collected at time t through a wheel speed encoder, IMU, camera, etc., and π Θ be the online dead reckoning error neural network predictor, where Θ represents the parameters of the neural network, and A t =(α 1 ,…,α 4 ) t is the error coefficient of the dead reckoning model, and R t is the dead reckoning error covariance matrix calculated based on A t and formula 4. Then the calculation process of the online dead reckoning error neural network predictor can be expressed as R t =π Θ (I t ).

[0094] The implementation principle of the parameter training method for the online dead reckoning error neural network predictor model designed in the present invention is as Figure 3 shown. In the parameter training method, based on the fusion positioning framework of dead reckoning and environmental perception, using the online obtained robot data, after continuous multi-frame iteration, the fusion positioning result and the positioning ground truth are compared, and the positioning loss is calculated and the neural network predictor parameters are optimized by backpropagation.

[0095] First, the fusion positioning framework of dead reckoning and environmental perception is given below, and then the neural network parameter training method of the dead reckoning error predictor of the present invention is described.

[0096] Fusion positioning framework of dead reckoning and environmental perception: As in Figure 3 the forward inference process. Dead reckoning and visual / laser odometry (VO / LO) respectively obtain the relative pose estimates u t and z t of the robot. Let μ t be the fused relative positioning result. Assume u t ,z t ,μt The errors all follow a Gaussian distribution, and their covariance matrices of errors are R t , Q t , Σ t .

[0097] The present invention uses visual / laser odometry to obtain z t , calculates Q using the matching error of corresponding point pairs t , obtains u using the dead reckoning formula 3 t , predicts R using the online dead reckoning error neural network predictor π Θ obtains R t , and performs fusion positioning calculation using the information filter method to solve for μ t , Σ t . Let the information matrix be The information vector is ξ t = Ω t μ t , and denote the fusion positioning algorithm as This processing procedure can be abbreviated as:

[0098]

[0099] Specific calculation steps

[0100] Based on dead reckoning, predict the information matrix

[0101] Based on dead reckoning, predict the information vector

[0102] Use environmental perception to update the information matrix

[0103] Use environmental perception to update the information vector

[0104] Calculate the covariance matrix of fusion positioning

[0105] Calculate the fusion positioning result μ t = Σ t ξ t

[0106] Taking two-dimensional positioning as an example, the relative pose estimate μ at time t t = (μ x,t , μ y,t , μ θ,t ) T includes the displacement and rotation angle of the robot relative to the previous time t - 1 in the x and y coordinate components, and can be represented in the form of a coordinate transformation matrix:

[0107]

[0108] The initial pose of the given robot in the global coordinate system and the relative positioning estimates μ for consecutive multiple frames 1 , μ 2 , … μ t , the global pose of the robot can be deduced as follows:

[0109]

[0110] Let be the above coordinate transformation function, which can be abbreviated as:

[0111]

[0112] Let I t be the sensor data at the current moment collected by the wheel speed encoder, IMU, camera, etc. at time t, and π Θ be the neural network predictor for the online dead reckoning error, where Θ represents the parameters of the neural network, and A t =(α 1 , …, α 4 ) t be the error coefficient of the dead reckoning model, and R t be the dead reckoning error covariance matrix estimated based on A t and Equation 4. Then the forward inference process of the robot's integrated positioning is as follows.

[0113] Specific calculation steps

[0114] Predict the covariance matrix R of the dead reckoning error t = π Θ (I t )

[0115] Fuse dead reckoning and environmental perception for relative positioning

[0116] Global positioning deduction

[0117] The parameter training process of the online dead reckoning error neural network predictor provided by the embodiment of the present invention includes:

[0118] Let the robot start from the initial point with known position truth , and after T frames, obtain the positioning truth in the global coordinate system through methods such as GPS or closed-loop detection The integrated positioning deduction result is To improve the stability of the learning algorithm, usually set T > τ, where τ is the minimum number of iterative frames to ensure significant error accumulation in the positioning deduction. Define the loss:

[0119]

[0120] Among them, λ is a hyperparameter for the balance position accuracy and the orientation accuracy. Let Θ be the parameters of the body CNN, the vision CNN, and the prediction MLP neural network. Then the learning objective is to optimize Θ to minimize the loss function J:

[0121]

[0122] During the training process, for each training trajectory, the loss J is calculated according to Equation 10, and the neural network parameters are updated according to Equation 12. This cycle continues until the network parameters converge.

[0123]

[0124] The trained online dead reckoning error neural network predictor model is obtained.

[0125] In summary, the mobile robot and ground unmanned platform technologies in the embodiments of the present invention have broad application prospects in industries, national defense, etc. Among them, the positioning technology is the basis for performing various tasks. Under the condition of no satellite positioning, dead reckoning positioning input by sensors such as wheel speed encoders and inertial measurement units (IMUs), and environmental perception positioning (such as VO / LO, VSLAM / LSLAM) that matches the data of environmental perception sensors such as cameras and lidar are two main modalities. Multi-modal fusion positioning is the key to achieving accurate positioning of mobile robots, and a reliable error model is the basis for effective fusion. Most existing studies focus on environmental perception positioning, but lack in-depth research on the error model of dead reckoning. In practical applications, the dead reckoning positioning error is often estimated by constructing a simplified error model and using pre-calibrated error coefficients, and it cannot adapt to the robot's motion and environmental changes online.

[0126] The present invention proposes an online dead reckoning error prediction method considering the robot's motion and surface conditions. This method can use the robot data to incrementally optimize the model parameters of the prediction neural network online, and its goal is to improve the fusion positioning accuracy. This method does not rely on manual annotation, has the ability of continuous learning, and can improve the adaptability of the robot positioning system to complex motions and diverse surface conditions.

[0127] Those of ordinary skill in the art can understand that the drawings are only schematic diagrams of one embodiment, and the modules or processes in the drawings are not necessarily essential for implementing the present invention.

[0128] As can be seen from the description of the above embodiments, those skilled in the art can clearly understand that the present invention can be implemented by means of software plus a necessary general hardware platform. Based on such an understanding, the technical solution of the present invention, in essence, or the part that makes a contribution to the prior art, can be embodied in the form of a software product. This computer software product can be stored in a storage medium, such as ROM / RAM, magnetic disk, optical disk, etc., and includes several instructions for causing a computer device (which can be a personal computer, a server, or a network device, etc.) to execute the methods described in various embodiments or some parts of the embodiments of the present invention.

[0129] Each embodiment in this specification is described in a progressive manner. For the same or similar parts among the embodiments, reference can be made to each other. Each embodiment focuses on the differences from other embodiments. In particular, for the device or system embodiments, since they are basically similar to the method embodiments, the description is relatively simple. For the relevant parts, reference can be made to the partial description of the method embodiments. The device and system embodiments described above are only illustrative. The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they can be located in one place or distributed to multiple network units. Some or all of the modules can be selected according to actual needs to achieve the purpose of the solution of this embodiment. Those of ordinary skill in the art can understand and implement it without creative efforts.

[0130] The above is only a preferred specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any changes or substitutions that can be easily thought of by those skilled in the art within the technical scope disclosed by the present invention should be covered by the protection scope of the present invention. Therefore, the protection scope of the present invention should be subject to the protection scope of the claims.

Claims

1. An online dead reckoning error prediction method based on robot motion and surface perception, characterized in that: include: Build the robot's online dead reckoning error neural network predictor model, including the body convolutional neural network CNN, vision CNN and prediction MLP; Training parameters of the online dead reckoning error neural network predictor to obtain a trained online dead reckoning error neural network predictor model; The robot’s online dead reckoning error prediction is performed using the trained body CNN, vision CNN, and prediction MLP in the online dead reckoning error neural network predictor model.

2. The method according to claim 1, characterized in that The online dead reckoning error neural network predictor model for building a robot, including a body CNN, a visual CNN and a prediction MLP, includes: An online dead reckoning error neural network predictor model including a body CNN, a visual CNN and a prediction MLP is constructed. The visual CNN uses a MobileNet v2 model pre-trained on ImageNet, inputs a 224×224 image block, and outputs a 1×512 visual feature vector. The body CNN uses a one-dimensional convolutional neural network, inputs a 1×7 vector including the speed obtained by the wheel speed encoder, the three-dimensional linear acceleration obtained by the IMU, and the three-dimensional angular velocity, and outputs a 1×256 body feature vector after upsampling. The prediction MLP uses a multi-layer perceptron, inputs a 1×768 visual feature vector and a vector concatenated with a body feature vector, and outputs a 1×4 error coefficient vector.

3. The method according to claim 2, characterized in that The step of training the parameters of the online dead reckoning error neural network predictor to obtain a trained online dead reckoning error neural network predictor model includes: Assume that dead reckoning and visual / laser odometer obtain the robot's relative pose estimate u t and z t , let μ t To fuse the relative positioning results, assume that u t ,z t ,μ t The errors all obey Gaussian distribution, and the covariance matrices of the errors are R t ,Q t ,Σ t ; Use vision / laser odometry to get z t , calculate Q using the matching error of corresponding point pairs t , using the dead reckoning formula 3 to get u t , using the online dead reckoning error neural network predictor π Θ Get R t , using the information filter method to perform fusion positioning calculation and solve μ t ,Σ t , let the information matrix be The information vector is ξ t =Ω t μ t , the fusion positioning algorithm is The process can be summarized as follows: Specific calculation steps Information matrix based on dead reckoning prediction Prediction information vector based on dead reckoning Using environmental perception to update the information matrix Update information vector using environmental perception Calculate the covariance matrix of fusion positioning Calculate the fusion positioning result μ t =Σ t ξ t Taking two-dimensional positioning as an example, the relative pose estimation μ at time t is t =(μ t,t ,μ y,t ,μ θ,t ) T Contains the displacement and rotation angle of the robot in the x and y coordinate components relative to time t-1, expressed in the form of a coordinate transformation matrix: Given the initial position of the robot in the global coordinate system And the relative positioning estimation of multiple frames μ1,μ2,…μ t ,The global pose of the robot is calculated as follows: set up is the coordinate transformation function mentioned above, which can be abbreviated as: Let I t is the current sensor data collected by the wheel speed encoder, IMU, and camera at time t, π Θ is the online dead reckoning error neural network predictor, where Θ represents the parameters of the neural network, A t =(α1,…,α4) t is the error coefficient of the dead reckoning model, R t Based on A t And the dead reckoning error covariance matrix estimated by formula 4, the forward reasoning process of robot fusion positioning is as follows: Covariance matrix R of predicted dead reckoning error t =π Θ (I t ) Combining dead reckoning and environmental perception for relative positioning Global dead reckoning Assume that the robot starts from the initial point with known true position After T frames, the true positioning value in the global coordinate system is obtained. The fusion positioning result is Let Y>τ, τ is the minimum number of iteration frames, and define the loss: Among them, λ is a hyperparameter that balances position accuracy and orientation accuracy. Let Θ be the parameters of the body CNN, visual CNN and prediction MLP neural network. Then the learning goal is to optimize Θ to minimize the loss function J: During the training process, for each training trajectory, the loss J is calculated according to Formula 10, and the neural network parameters are updated according to Formula 12, and this cycle is repeated until the network parameters converge. Get the trained online dead reckoning error neural network predictor model.

4. The method according to claim 3, characterized in that The method of using the trained body CNN, visual CNN and prediction MLP in the online dead reckoning error neural network predictor model to perform online dead reckoning error prediction on the robot includes: The body data of the robot to be identified, including stroke, acceleration and angular velocity, are collected through the wheel speed encoder and IMU, and the body data is input into the trained online dead reckoning error neural network predictor model. A 224×224 image block is cut from the bottom of the original 1024×768 image in the body data and input into the visual CNN. The visual CNN outputs a 1×512 visual feature vector. A 1×7 vector including the three-dimensional linear acceleration obtained by the speed sensor and the three-dimensional angular velocity obtained by the inertial measurement unit IMU is input into the body CNN. The body CNN outputs a 1×256 body feature vector. The visual feature vector and the body feature vector are connected to obtain a 1×768 vector. The 1×768 vector is input into the prediction MLP, and a 1×4 error coefficient vector α1,…,α4 is output. Then, the following formulas (1)-(5) are used to obtain the dead reckoning result u of the robot to be identified: t The error covariance matrix R t ; Assume that from time t-1 to time t, the robot travel measured by the wheel speed encoder is Δs, the robot heading angle change measured by the IMU is Δθ, and the process takes Δt. Then the measured values ​​of the robot's velocity v and angular velocity ω are calculated as follows: where ∈ t With ∈ ω is the measurement error of velocity v and angular velocity ω, which has zero mean and variance of α1Δs 2 +α2Δθ 2 ,α3Δs 2 +α4Δθ 2 Gaussian distribution of , α1,…,α4 are the error coefficients output by the above-mentioned online dead reckoning error prediction model; Using the robot motion model to perform dead reckoning, the relative position u at time t t =(u x,t ,u y,t ,u θ,t ) T The estimates are as follows: in, is the orientation angle of the robot in the global coordinate system at time t-1; ∈ t for u t The error is subject to zero-mean Gaussian distribution, R t is the covariance matrix of the error, calculated as follows: in Substitute the error coefficients α1,…,α4 output by the online dead reckoning error prediction model into formula (4) to obtain the dead reckoning result u of the robot to be identified. t The error covariance matrix R t .

Images