EKF-Based Template Matching VO and Wheel Odometry Fusion Localization Method

Abstract

The present invention discloses a method for fusing localization of template matching VO based on EKF and wheel odometer, including: creating a ground image dataset, adding terrain complexity as a label to the ground image dataset; training a convolutional neural network model using the ground image dataset; collecting a ground image and inputting it into the trained convolutional neural network model to obtain the terrain complexity of the current ground; calculating the image entropy of the current ground; calculating the template selection strategy of the current ground according to the terrain complexity and image entropy, and performing template matching VO estimation according to the template selection strategy to obtain a pose result; fusing the pose estimated by the wheel odometer and the pose calculated through the inertial measurement unit by means of an extended Kalman filter method, taking the fused pose as the predicted value of the secondary filter, and combining it with the pose result estimated by the template matching VO to obtain the final robot localization result, improving the stability of the localization result and achieving more accurate and robust localization.

Description

Technical Field

[0001] The present invention relates to a method for fusing localization of template matching VO based on EKF and wheel odometer, belonging to the technical field of robot localization. Background Art

[0002] In recent years, wheeled robots have been widely used in many fields such as daily life and industrial production, such as inspection, logistics, construction operations, etc. However, with the continuous development of the application fields of wheeled robots and the gradual complexity of their application scenarios, it is inevitable to put forward higher requirements for the environmental adaptability of the robots. Therefore, how to achieve efficient and accurate localization of wheeled robots in different scenarios has become a research hotspot and difficulty.

[0003] SLAM (Simultaneous Localization And Mapping) is mainly used to solve the problems of positioning and navigation and map construction of mobile robots during movement. Gmapping is a SLAM algorithm based on 2D lidar to complete the construction of a two-dimensional grid map. This algorithm can construct an indoor map in real time, with relatively small computational requirements and high accuracy for constructing a small-scale scene map.

[0004] Currently, widely used self-localization technologies include wheel odometer, inertial odometer, visual odometer, etc. They all have their own advantages and disadvantages. For example, the wheel odometer directly calculates the trajectory of the robot's pose through the output of the wheel encoder, and there are certain errors in the case of bumpy terrain or wheel slip; the inertial odometer is not affected by the external environment, but its error will accumulate over time; the visual odometer is greatly affected by ambient light and is mainly divided into feature-based and appearance-based. The feature-based method is suitable for scenes with rich textures, such as urban environments with a large number of feature points. This method cannot handle single-mode textureless or low-texture environments (such as sand, asphalt, concrete, etc.) because insufficient significant features can be detected in these environments. In contrast, the appearance-based template matching method is very suitable for use in low-texture scenes.

[0005] The main advantage of the visual odometry (VO) system based on template matching in the vehicle's positioning system is that it only needs to be implemented through a camera, does not require prior knowledge of the scene, motion pose, etc., and can adapt to problems such as vehicle slip, side shift, and non-flat ground movement that are likely to cause errors in the wheel odometer, and can still have excellent scene recognition capabilities in low-texture scenes. However, its limitation is mainly reflected in that the selection of the template in the process of template matching seriously affects the effect of the algorithm, and how to optimally select the template has become the key to determining the quality of the template matching VO algorithm. Summary of the Invention

[0006] The technical problem to be solved by the present invention is to provide a method for fusing localization of template matching VO based on EKF and wheel odometer, which fuses the visual localization result of template matching improved based on CNN and image entropy with the wheel odometer in the odometer coordinate system to obtain a more accurate and robust localization result.

[0007] The present invention adopts the following technical solutions to solve the above technical problems:

[0008] The method for fusing localization of template matching VO based on EKF and wheel odometer includes the following steps:

[0009] Step 1: Collect image data of different types of ground, create a ground image data set, and add terrain complexity as a label to each image in the ground image data set.

[0010] Step 2: Use the ground image data set with added labels to train a convolutional neural network model to obtain a trained convolutional neural network model.

[0011] Step 3: Collect ground images using the camera installed on the robot and input them into the trained convolutional neural network model to obtain the terrain complexity of the current ground.

[0012] Step 4: Calculate the image entropy of the current ground.

[0013] Step 5: According to the terrain complexity and image entropy, calculate the template selection strategy of the current ground in real time, and perform template matching VO estimation according to the template selection strategy to obtain a pose result.

[0014] Step 6: Fuse the pose estimated by the wheel odometer and the pose calculated by the inertial measurement unit through the extended Kalman filter method, i.e., the primary filter. Use the fused pose as the predicted value of the secondary filter, and combine the pose result estimated by the template matching VO as the observed value of the secondary filter to obtain the final robot localization result.

[0015] As a preferred solution of the present invention, the determining factors of the terrain complexity in Step 1 include ground material and bumpiness.

[0016] As a preferred solution of the present invention, the calculation formula of the image entropy in Step 4 is as follows:

[0017]

[0018]

[0019] Among them, S represents the image entropy, p j represents the probability that a pixel point with a gray value of j appears in the image, and n jrepresents the number of pixels with a gray value of j, T w and T h are the width and height of the image respectively.

[0020] As a preferred embodiment of the present invention, the template selection strategy for the current ground is calculated in real time according to the terrain complexity and the image entropy in step 5, specifically as follows:

[0021] The terrain complexity threshold E and the image entropy threshold F are preset. For the i-th frame of the ground image with a pixel size of 640*480, the terrain complexity ρ i and the image entropy S i of the i-th frame of the ground image are obtained according to steps 3 and 4. If ρ i is less than E and S i is less than F, a template with a size of 240×240 pixels is initialized in the ground image; if ρ i is greater than E and S i is less than F, a template with a size of 180×180 pixels is initialized in the ground image; if ρ i is less than E and S i is greater than F, a template with a size of 200×200 pixels is initialized in the ground image; if ρ i is greater than E and S i is greater than F, a template with a size of 160×160 pixels is initialized in the ground image; the center of the template coincides with the center of the ground image.

[0022] As a preferred embodiment of the present invention, the template matching VO estimation is performed according to the template selection strategy in step 5 to obtain the pose result, specifically as follows:

[0023] 1) Initialize template a in the i-th frame of the ground image according to the template selection strategy;

[0024] 2) Use the normalized cross-correlation matching method to search for the matching region b with the highest similarity to template a from left to right and top to bottom in the (i + 1)-th frame of the ground image. The size of the matching region b is the same as that of template a;

[0025] 3) Calculate the pixel displacement increments Δu and Δv according to the upper left pixel positions of the matching region b and template a;

[0026] 4) Convert the pixel displacement increments Δu and Δv to the true displacement increments of the camera in the world physical coordinate system;

[0027] 5) Solve according to the true displacement increments to obtain the robot pose result of the template matching VO estimation at the (i + 1) moment;

[0028] 6) Initialize the template in the ground image of the (i + 1)-th frame according to the template selection strategy and repeat the above process to obtain the robot pose result estimated by template matching VO at the (i + 2)-th moment, and so on.

[0029] As a preferred embodiment of the present invention, the observation model of the inertial measurement unit is as follows:

[0030] The attitude update equation of the inertial measurement unit is:

[0031]

[0032] where and respectively represent the attitude transformation matrices from the robot coordinate system to the navigation coordinate system at the current moment t and the previous moment; represents the skew-symmetric matrix formed by the relative rotation between the current moment and the previous moment;

[0033] The velocity and position of the inertial measurement unit in the navigation coordinate system are respectively:

[0034]

[0035]

[0036] where v t represents the velocity of the robot at the current moment; v t-1 represents the velocity of the robot at the previous moment; a t represents the acceleration measured by the IMU at the current moment; a t-1 represents the acceleration measured by the IMU at the previous moment; P t represents the position information of the robot at the current moment; P t-1 represents the position information of the robot at the previous moment; Δt is the time difference between the current moment and the previous moment.

[0037] As a preferred embodiment of the present invention, the extended Kalman filter method realizes two steps of prediction and update according to the motion model and the observation model of the system, and the motion model and the observation model are respectively:

[0038] x t = G t x t-1 + w t

[0039] z t = H t x t + V t

[0040] where x t is the system state matrix at the current moment t; x t-1is the system state matrix at the previous moment; G t is the system state transfer matrix at the current moment; z t is the observation value matrix of the system at the current moment; H t is the transfer matrix between the state quantity and the observation value of the system at the current moment; w t and V t are the process noise matrix and the observation noise matrix of the system at the current moment respectively;

[0041] Prediction process:

[0042]

[0043]

[0044] Among them, is the estimated value of the system state matrix at the current moment; is the state estimation covariance of the system at the current moment; R t is the process noise covariance of the system at the current moment; Σ t-1 is the system state covariance at the previous moment;

[0045] Update process:

[0046] Calculate the Kalman gain K t :

[0047]

[0048] Among them, Q t is the observation value error covariance matrix;

[0049] Use K t and the observation value to update the state matrix x of the mobile robot t and the system covariance matrix:

[0050]

[0051]

[0052] Among them, I is the identity matrix.

[0053] Compared with the prior art, the present invention adopts the above technical solutions and has the following technical effects:

[0054] 1. Based on the information entropy of the image as one of the main influencing factors, the present invention dynamically adjusts the template size used in the matching process in combination with the terrain complexity recognized by the Convolutional Neural Networks (CNN), greatly reducing the algorithm error caused by the ground environment factors, so as to achieve the purpose of improving the matching accuracy while ensuring the calculation efficiency.

[0055] 2. Aiming at the problem that a single sensor cannot accurately estimate the pose of a robot in a complex environment, the present invention proposes to fuse the improved template matching VO and wheel odometer based on the Extended Kalman Filter (EKF), so as to improve the stability of the positioning result and achieve more accurate and robust positioning. Brief Description of the Drawings

[0056] Figure 1 It is a flow chart of the method for fusing template matching VO and wheel odometer based on EKF of the present invention;

[0057] Figure 2 It is a schematic diagram of the convolution operation;

[0058] Figure 3 It is a schematic diagram of pooling;

[0059] Figure 4 It is a speed measurement model diagram of the wheel odometer;

[0060] Figure 5 It is a schematic diagram of target matching. Detailed Embodiment

[0061] The following details the embodiments of the present invention, and the examples of the embodiments are shown in the drawings. The embodiments described below with reference to the drawings are exemplary and are only used to explain the present invention and should not be construed as a limitation to the present invention.

[0062] Aiming at the problem that a single sensor cannot accurately estimate the pose of a robot in a complex environment, the present invention proposes a positioning method based on the fusion of improved template matching and wheel odometer EKF. This method fuses the visual positioning result of the improved template matching based on CNN and image entropy with the wheel odometer in the odometer coordinate system, and inputs the fused positioning information into the gmapping algorithm, so as to achieve more accurate and robust positioning and mapping.

[0063] First, a template matching VO algorithm that combines image entropy and deep learning is proposed. It makes full use of the image data from the downward-facing camera. Taking the information entropy of the image as one of the main influencing factors, it combines a convolutional neural network (CNN) to calculate the type of the scene environment, and calculates the optimal target matching strategy for the current image, greatly reducing the algorithm error caused by ground environmental factors and enhancing the robustness and accuracy of the template matching VO algorithm. Then, after obtaining the improved template matching VO, the pose estimated by the robot's wheel encoder and the pose after the attitude solution of the Inertial Measurement Unit (IMU) are fused through the Extended Kalman Filter (EKF) method. The fused robot pose is used as the predicted value of the primary filter, and combined with the observed value of the visual odometer to obtain the final attitude data. As Figure 1 shown, it is the flow chart of the fusion positioning method of the template matching VO based on EKF and the wheel odometer of the present invention. The specific steps are as follows:

[0064] Step 1: Collect image data of different types of ground, create a ground image dataset, and add a variable (terrain complexity ρ) representing the terrain complexity as a label to the dataset. The terrain complexity is determined by factors such as ground material and bumpiness, such as grassland, tiled floor, cement floor, etc.

[0065] Step 2: Use this dataset to train a convolutional neural network model. After training, the prediction model can fit different types of ground information based on the input images collected by the downward-facing camera installed on the mobile robot and predict the terrain complexity of the ground image.

[0066] Step 3: Input the image collected by the camera into the trained convolutional neural network model to obtain the terrain complexity ρ of the current ground. The prediction model includes main neural network layers such as an input layer, a convolutional layer, a pooling layer, and a fully connected layer. Among them, the convolutional layer is responsible for feature extraction and consists of multiple convolutional kernels, which can break through the limitations of traditional filters and automatically adjust weights according to the objective function. The number of parameters in the convolutional layer should be calculated according to the following formula:

[0067] Weight=C_h * C_w * in_channel * out_channel+out_channel

[0068] In the formula, Weight is the number of parameters in the convolutional layer, C_h and C_w are the height and width of the convolutional kernel respectively, in_channel and out_channel are the number of input and output channels respectively, and the relationship between the input and output sizes is as follows:

[0069]

[0070] In the formula, oM is the size of the output feature map, iM is the size of the input feature map, C is the size of the convolution kernel, P is the padding, and S is the stride. In a single channel, the convolution implementation process is basically the same as the principle of the convolution method used in general digital image processing, that is, by multiplying and adding the convolution kernel with the local part of the image to obtain a feature point, and then performing a sliding window operation to perform calculations on the entire image to obtain the output feature map. Figure 2 It shows the process of using a 3*3 convolution kernel to perform convolution operation on a 9*9 feature map.

[0071] The pooling layer is usually placed after the convolution layer to downsample the feature map, refine the effective information of the feature map, and reduce the network calculation amount. The commonly used pooling methods are max pooling (Maxpooling) and average pooling (Averagepooling). Max pooling is usually used for downsampling operations after the convolution layer, while average pooling is usually used to replace the fully connected layer to form a fully convolutional neural network. The pooling layer usually uses a pooling kernel with a size of 2*2 and a stride of 2 for pooling operations. Since the pooling kernel does not contain parameters that need to be learned, a pooling kernel can be used to process the feature maps of all input channels, so the number of output channels is equal to the number of input channels. For easy understanding, Figure 3 It shows the processes of performing 2*2 max pooling and average pooling on a 4*4 feature map respectively.

[0072] After calibrating the complexity of each picture according to experience (tagging), through the reasonable combination of multiple convolutional layers, pooling layers, and fully connected layers, after the model training is completed, the terrain complexity can be predicted through the texture, shadow, shape, and classification in the image input, and the terrain complexity ρ is obtained as the prediction result.

[0073] Step 4: Image entropy is a statistical form of features and an important indicator to measure the richness of information in an image. The larger the entropy value, the more abundant the information contained in the image, or the more chaotic the information distribution in the image. By calculating the image entropy S of the ground, the error existing in the neural network model can be overcome, and the accuracy of terrain description can be improved. The calculation formula for the one-dimensional entropy of an image is:

[0074]

[0075]

[0076] where p j represents the probability that a point with a gray value of j appears in the image, n j represents the number of pixel points with a gray value of j, T w and T h are the width and height of the image respectively.

[0077] Step 5: By obtaining the terrain complexity ρ and the ground image entropy S, the template selection strategy for the current area is calculated in real time to achieve a highly robust and accurate template matching VO algorithm. We can dynamically switch the template size according to the ground complexity and entropy value of the template. Set a threshold E and F for the complexity ρ and the image entropy S respectively. When both the terrain complexity ρ and the image entropy S are less than their thresholds, it indicates that there are few ground feature points, and a large template needs to be selected for matching, that is, a template window with a size of 240×240 pixels. If the terrain complexity ρ is greater than E and S is less than F, a template with a size of 180×180 pixels is selected; when ρ is less than E and S is greater than F, the ground type at this time may be a transitional type of two kinds of ground, and a template with a size of 200×200 pixels is used for matching to ensure accuracy; when both ρ and S are greater than the threshold, a template with a size of 160×160 pixels is used for matching to ensure the calculation efficiency. There is no specific formula for determining the size of the threshold E, which can be determined through statistical analysis of a large number of images and experimental results.

[0078] Step 6: After obtaining the improved template matching VO, the pose estimated by the wheel encoder of the robot and the pose after IMU attitude solution are fused by the Extended Kalman Filter (EKF) method. The robot pose obtained after fusion is used as the predicted value of the primary filter, combined with the observed value of the visual odometer, to obtain the final attitude data.

[0079] (1) Speed measurement model of the encoder

[0080] As Figure 4 shown, the number of pulses measured by the encoders installed on the left and right wheels can be converted into the linear velocities of the left and right wheels through the following formula, where n is the number of pulses received per second, r is the wheel radius, and f is the number of pulses emitted by the encoder when it rotates one circle, that is:

[0081]

[0082] Odometer speed model:

[0083]

[0084]

[0085] Among them, v and w are the linear velocity and angular velocity of the robot around the center of the robot, V r 、V l are the speeds of the left and right wheels of the robot, and d is the wheelbase.

[0086] Ideally, the robot motion model is as shown in the following formula:

[0087]

[0088] Among them, (x ′ , y ′ , θ ′ ) is the pose at the current moment in the world coordinate system, (x, y, θ) is the pose at the previous moment in the world coordinate system, and (dx, dy, dθ) is the motion increment in the robot coordinate system.

[0089] (2) IMU Observation Model

[0090] The IMU includes an internal gyroscope and accelerometer, and can calculate the robot pose by inertial navigation. Considering the imu attitude update equation in a two-dimensional environment is:

[0091]

[0092] In the formula, and respectively represent the attitude transformation matrices from the robot coordinate system to the navigation coordinate system at the current moment and the previous moment; represents the skew-symmetric matrix formed by the relative rotation between the current moment and the previous moment.

[0093] The velocity and position of the IMU in the navigation coordinate system are respectively:

[0094]

[0095]

[0096] In the formula, v t represents the robot velocity at the current moment; v t-1 represents the robot velocity at the previous moment; a t represents the acceleration measured by the IMU at the current moment; a t-1 represents the acceleration measured by the IMU at the previous moment; P t represents the robot position information at the current moment; P t-1 represents the robot position information at the previous moment; Δt is the time difference between the current moment and the previous moment.

[0097] (3) As Figure 5 shown, the steps of the visual odometer obtained by monocular camera template matching are as follows:

[0098] a. Obtain continuous image frames from the camera;

[0099] b. Initialize the template in the i-th image frame and determine the search area in the (i + 1)-th image frame;

[0100] c. Use the normalized cross-correlation matching method to match the template in the i-th image frame in the (i + 1)-th image frame to obtain the matching result with the maximum similarity;

[0101] d. Calculate the pixel displacement increments Δu and Δv based on the upper-left pixel positions of the matching region and the original template;

[0102] e. Convert the pixel displacement increments to the true displacement increments of the camera in the world physical coordinate system;

[0103] f. Obtain the yaw angle output by the IMU at the (i + 1)-th moment;

[0104] g. Use the position recurrence formula to calculate and output the position of the robot at the (i + 1)-th moment;

[0105] h. Return to step 1 and repeat the above steps.

[0106] (4) Extended Kalman filter fusion process

[0107] The main idea of the Extended Kalman Filter (EKF) is to expand the nonlinear equation into a linear equation by first-order Taylor expansion, and then use the Kalman filter algorithm to estimate the system state. The EKF algorithm realizes the prediction and update steps according to the system's motion model and observation model, and the motion model and observation model are respectively:

[0108] x t = G t x t-1 + w t

[0109] z t = H t x t + V t

[0110] In the formula, x t is the system state matrix at the current time t; x t-1 is the system state matrix at the previous time; G t is the system state transfer matrix at the current time; z t is the observation value matrix of the system at the current time; H t is the transfer matrix between the system state quantity and the observation value at the current time; w t and V t are the process noise matrix and the observation noise matrix of the system at the current time respectively;

[0111] Prediction process:

[0112]

[0113]

[0114] In the formula, is the estimated value of the system state matrix at the current time; is the state estimation covariance of the system at the current moment; R t is the process noise covariance of the system at the current moment; Σ t-1 is the system state covariance at the previous moment;

[0115] Update process:

[0116] First, calculate the Kalman gain K t :

[0117]

[0118] where Q t is the observation error covariance matrix;

[0119] Use K t and the observation value to update the state matrix x t of the mobile robot and the system covariance matrix:

[0120]

[0121]

[0122] where I is the identity matrix.

[0123] The process of extended Kalman filter fusing wheel odometer and IMU is as follows: the linear velocities of the left and right wheels are obtained through the encoder speed measurement model, which are input into the motion model for state estimation, and the pre-integrated IMU is input into the observation equation for state update. Through continuous iteration of the Kalman gain and formula, the optimal estimate of the system state is finally obtained. The result after fusing the wheel odometer and IMU is used as the predicted value of the secondary filter, and the pose result calculated by template matching VO is used as the observed value of the secondary filter. Substitute them into the prediction process and update process in (4), and use the Kalman gain and the observed value to update the state matrix and system covariance matrix of the mobile robot to obtain the final estimated value of the system state.

[0124] As time goes by, the error accumulated by the odometer will become larger and larger. Therefore, using the observation of template matching VO in the filter to make up for the accumulated error of the odometer can greatly reduce the situation where the algorithm converges to the local optimum.

[0125] The above embodiments are only used to illustrate the technical idea of the present invention, and the protection scope of the present invention cannot be limited thereby. Any modification made on the basis of the technical solution according to the technical idea proposed by the present invention falls within the protection scope of the present invention.

Claims

1. An EKF-based template matching VO and wheel odometer fusion positioning method, characterized in that, it includes the following steps: Step 1, collect image data of different types of ground, create a ground image dataset, and add terrain complexity as a label to each image in the ground image dataset; Step 2, use the ground image dataset with added labels to train a convolutional neural network model to obtain a trained convolutional neural network model; Step 3, use the camera installed on the robot to collect ground images and input them into the trained convolutional neural network model to obtain the terrain complexity of the current ground; Step 4, calculate the image entropy of the current ground; Step 5, according to the terrain complexity and image entropy, calculate the template selection strategy of the current ground in real time, and perform template matching VO estimation according to the template selection strategy to obtain a pose result; The specific method of calculating the template selection strategy of the current ground according to the terrain complexity and image entropy is as follows: Preset the terrain complexity threshold E and the image entropy threshold F. For the i-th frame of the ground image with a size of 640*480 pixels, obtain the terrain complexity ρ of the i-th frame of the ground image according to steps 3 and 4 i and the image entropy S i , if ρ i is less than E and S i is less than F, then initialize a template with a size of 240×240 pixels in the ground image; If ρ i is greater than E and S i is less than F, then initialize a template with a size of 180×180 pixels in the ground image; If ρ i is less than E and S i is greater than F, then initialize a template with a size of 200×200 pixels in the ground image; If ρ i is greater than E and S i is greater than F, then initialize a template with a size of 160×160 pixels in the ground image; the center of the template coincides with the center of the ground image; Step 6, fuse the pose estimated by the wheel odometer and the pose calculated by the inertial measurement unit through the extended Kalman filter method, that is, the primary filter, and use the fused pose as the predicted value of the secondary filter. Combine the pose result estimated by the template matching VO as the observed value of the secondary filter to obtain the final robot positioning result; The extended Kalman filter method realizes the prediction and update steps according to the motion model and observation model of the system. The motion model and observation model are respectively: x t = G t x t-1 + w t z t = H t x t + V t where, x t is the system state matrix at the current time t; x t-1 is the system state matrix at the previous time; G t is the system state transfer matrix at the current time; z t is the observation value matrix of the system at the current time; H t is the transfer matrix between the state quantity and the observation value of the system at the current time; w t and V t are the process noise matrix and the observation noise matrix of the system at the current time respectively; Prediction process: Among them, is the estimated value of the system state matrix at the current moment; is the state estimation covariance of the system at the current moment; R t is the process noise covariance of the system at the current moment; Σ t-1 is the system state covariance at the previous moment; Update process: Calculate the Kalman gain K t : where Q t is the covariance matrix of the observation errors; Use K t and the observed values to update the state matrix x t of the mobile robot and the system covariance matrix: where I is the identity matrix.

2. The EKF-based template matching VO and wheel odometer fusion positioning method according to claim 1, characterized in that, The determining factors of the terrain complexity in Step 1 include ground material and bumpiness.

3. The EKF-based template matching VO and wheel odometer fusion positioning method according to claim 1, characterized in that, The calculation formula of the image entropy in Step 4 is as follows: Among them, S represents the image entropy, and p j represents the probability that a pixel point with a gray value of j appears in the image, and n j represents the number of pixel points with a gray value of j, and T w and T h are the width and height of the image respectively.

4. The EKF-based template matching VO and wheel odometer fusion positioning method according to claim 1, characterized in that, The specific method of performing template matching VO estimation according to the template selection strategy in Step 5 to obtain a pose result is as follows: 1) Initialize the template a in the i-th frame of ground image according to the template selection strategy; 2) Use the normalized cross-correlation matching method to search for the matching region b with the highest similarity to the template a from left to right and top to bottom in the (i + 1)-th frame of ground image. The size of the matching region b is the same as that of the template a; 3) Calculate the pixel displacement increments Δu and Δv according to the upper left pixel positions of the matching region b and the template a; 4) Convert the pixel displacement increments Δu and Δv to the real displacement increments of the camera in the world physical coordinate system; 5) Calculate the robot pose result estimated by the template matching VO at the (i + 1) moment according to the real displacement increments; 6) Initialize the template in the (i + 1)-th frame of ground image according to the template selection strategy and repeat the above process to obtain the robot pose result estimated by the template matching VO at the (i + 2) moment, and so on.

5. The EKF-based template matching VO and wheel odometer fusion positioning method according to claim 1, characterized in that, The observation model of the inertial measurement unit is as follows: The attitude update equation of the inertial measurement unit is: Among them, and respectively represent the attitude transformation matrices from the robot coordinate system to the navigation coordinate system at the current time t and the previous time; represents the skew-symmetric matrix formed by the relative rotation between the current time and the previous time; The velocity and position of the inertial measurement unit in the navigation coordinate system are respectively: Among them, v t represents the speed of the robot at the current moment; v t-1 represents the speed of the robot at the previous moment; a t represents the acceleration measured by the IMU at the current moment; a t-1 represents the acceleration measured by the IMU at the previous moment; P t represents the position information of the robot at the current moment; P t-1 represents the position information of the robot at the previous moment; Δt is the time difference between the current moment and the previous moment.

Images