A multi-sensor fusion positioning method for mobile robots based on dual-layer vision

Abstract

The present invention discloses a multi-sensor fusion positioning method for a mobile robot based on double-layer vision. A novel feature code is adopted to realize the collaborative recognition and positioning function of top and bottom cameras. At the same time, the obtained posture information is used for the data fusion algorithm of the mobile robot to realize a high-precision robot positioning function. During driving, the top camera is used to recognize the feature code laid on the ground. The mobile robot can correct its own posture in advance to improve the robustness of the system. The bottom camera works only when the robot runs above the feature code. It is used for the precise positioning of the mobile robot and the redundant control during the rotation process. Since the camera recognizes the feature code intermittently in time, a high frequency of information update is introduced. At the same time, there is no intermittent IMU sensor and wheel odometer. The information provided by the IMU and the odometer is used for state prediction. The state update of the data fusion filter is realized through the intermittent data of the camera.

Description

Technical Field

[0001] The invention belongs to the field of sensors and positioning, and in particular relates to a multi-sensor fusion method based on double-layer vision. Background Art

[0002] With the development of intelligent manufacturing technology in my country, various types of highly intelligent mobile robots are increasingly being used in people's production and life. As the basis of mobile robots, the positioning system plays a vital role in assisting the robot's movement.

[0003] At present, there are various types of sensors used for navigation and positioning in mobile robots in the application field, such as LiDAR, IMU, UWB, Bluetooth, odometer, GNSS, magnetometer, etc. In outdoor situations, mature GNSS positioning technology is usually used, which can obtain the absolute position coordinates of the robot on the earth. In addition, there are base station positioning technology and IP positioning (Girbés-Juan V, Armesto L, Hernández-Ferrándiz D, et al. Asynchronous Sensor Fusion of GPS, IMU and CAN-Based Odometry for Heavy-Duty Vehicles [J]. IEEE Transactions on Vehicular Technology, 2021, 70 (9): 8617-8626.). Indoor positioning technology includes many types, and common positioning solutions include LiDAR, IMU, odometer, depth vision, Bluetooth, WIFI and UWB. Due to the complexity and variability of the indoor environment, the positioning sensor often loses positioning due to occlusion and signal problems, or generates large positioning errors. Therefore, for indoor positioning, it is very necessary to seek a stable and high-precision positioning solution. At present, indoor positioning usually adopts a multi-sensor fusion solution to improve accuracy and stability. There are multiple choices in sensors, and the common ones are lidar, IMU, odometer and UWB as positioning sensors (Lee Y, Lim D. Vision / UWB / IMU sensor fusion based localization using an extended Kalman filter[C] / / 2019IEEE Eurasia Conference on IOT, Communication and Engineering(ECICE).IEEE, 2019:401-403.). Most of the fusion positioning methods use lidar and UWB and other high-precision sensors to improve the accuracy of the positioning system, but the use of these sensors will inevitably increase the positioning cost significantly. As the number of mobile robots increases, the cost increase is more obvious. The cost issue of high-precision positioning system has become a major obstacle to its expansion into the mid- and low-end fields. For common positioning sensors, the information they obtain usually has a continuous and fixed sampling period, and there is little research on the possible asynchronous information processing scheme. Summary of the invention

[0004] In order to solve the technical problem, the present invention proposes a multi-sensor fusion positioning method for a mobile robot based on double-layer vision, which includes: obtaining the speed of the mobile robot wheel group through a wheel odometer, and obtaining the heading angle and other information of the mobile robot through an IMU, and using the top camera and the bottom camera to identify the feature code at the same time. The top camera can identify the feature code in advance before the mobile robot reaches the feature code, and obtain the posture state of the mobile robot. The bottom camera starts to work when the mobile robot reaches the top of the feature code, and its recognition and positioning accuracy is high, and it is usually used for precise positioning and deviation compensation of various high-precision operations.

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

[0006] A multi-sensor fusion positioning method for a mobile robot based on double-layer vision comprises the following steps:

[0007] S1. Construct a dynamic model based on the corresponding system control quantity of the differential wheeled mobile robot and the information obtained by the sensor;

[0008] S2, identifying the external positioning code in the new feature code through the camera on the top of the mobile robot, and extracting and obtaining the position and posture of the mobile robot before the mobile robot reaches the new feature code;

[0009] S3, using the camera at the bottom of the mobile robot to identify the middle positioning code at the bottom of the mobile robot. When the mobile robot moves above the new feature code, the middle positioning code in the new feature code is identified by the bottom camera, and the positioning deviation can be obtained by the coordinate conversion algorithm, and the position and posture of the mobile robot can be obtained by the matching algorithm.

[0010] S4. Build a camera sensor measurement model based on the position and positioning error of the mobile robot, analyze the error in the positioning algorithm process, and obtain the state vector of the mobile robot and the covariance matrix related to the camera measurement error;

[0011] S5. The angular velocity of the mobile robot is obtained by using the IMU sensor as a state prediction sensor, and the state of the mobile robot at the next moment is predicted by combining the IMU angular velocity dynamics model and the differential wheel dynamics model. The state prediction result is fused with the camera state vector and the heading angle part in the covariance matrix;

[0012] S6. Obtain the angular velocity of the wheels of the mobile robot through the odometer, and perform state prediction on the mobile robot at the next moment in combination with the dynamic model. The state prediction result is data-fused with the coordinate part of the camera state vector and the covariance matrix;

[0013] S7. After the data fusion is completed, the obtained posture data of the mobile robot is further smoothed by using Rauch-Tung-Striebel reverse smoothing to obtain the optimal estimate of the state of the mobile robot.

[0014] Furthermore, the kinetic model is as follows:

[0015] Define the state vector of the mobile robot Where [x, y] represents the position coordinates of the mobile robot in the planar world coordinate system, θ is the angle between the mobile robot and the x-axis in the world coordinate system, δ, They are respectively expressed as the corresponding system deviation parameters affected by the linear velocity and angular velocity; the corresponding dynamic model is expressed as:

[0016]

[0017] in, is the derivative of the state vector with respect to time, reflecting the state change of the mobile robot. are the derivatives of x, y, θ, ω w The angular velocity vector of the wheel rotation is defined as [ω r ,ω l ] T ,ω r ,ω l They represent the angular velocity of the right and left wheels of the mobile robot respectively, and f(X) is the calculation function of the state change of the mobile robot;

[0018] According to the structure of the differential wheeled robot, f(X) is expressed as:

[0019]

[0020] Where r represents the radius of the wheel of the differential wheeled mobile robot, and d represents the distance between the two wheels of the mobile robot;

[0021] The state observation vector Z of the mobile robot by the sensor is expressed by the measurement equation:

[0022] Z=h(X)

[0023] Where X is the state vector of the mobile robot, h(·) is the nonlinear transfer matrix, which represents the relative relationship between the sensor observations and the state of the mobile robot;

[0024] By adding the time period T s Discretize the system and add the system noise error caused by IMU and odometer:

[0025] X k+1 =Xk +f(X k )T s Δω

[0026] Z k =h(X k )

[0027] Where X k+1 , X k They are represented as the state vectors of the mobile robot at time k+1 and time k, respectively, Z k Represented as the state observation vector at time k; Δω=ω wk +ε k ,ω wk is the wheel speed vector at the kth moment, ε k is the system noise at the kth moment, and the state vector at the k+1th moment is obtained through the state change at the kth moment, T s is the time interval between two adjacent moments, that is, the time interval between moments k and k+1.

[0028] Furthermore, the new feature code includes an external positioning code and a central positioning code;

[0029] The external positioning code adopts the self-encoding method, realizes the relative position positioning of the mobile robot by identifying the black and white features, and realizes the absolute position positioning of the mobile robot by identifying the numbers;

[0030] The central positioning code uses the Apriltag code, and its type is 36h11.

[0031] Furthermore, the top camera of the mobile robot recognizes the new feature code, including the following steps:

[0032] First, the image of the new feature code in front of the mobile robot is obtained through the top camera, and the features of the image are extracted through visual processing. The obtained data is identified by scanning line by line, and the external feature code has a recognizable pattern;

[0033] Secondly, an affine transformation is performed on the identified positioning to convert the feature code into a state facing the camera. At the same time, the digital part of the feature code is extracted for recognition. The recognition result is expressed as the absolute position of the positioning feature code in the world coordinate system.

[0034] Finally, the position and posture of the mobile robot are calculated by the deviation and the absolute position of the feature code in the world coordinate system. The specific calculation method is as follows:

[0035] After the camera recognizes the feature code, it obtains the coordinates (u, v) of the center point of the feature code in the image coordinate system and calculates the relative position and posture of the camera:

[0036]

[0037] Among them, x c ,y c is the coordinate of the feature code center relative to the camera center, z c It is expressed as the distance between the center of the feature code and the center of the camera in the direction of the optical axis, f x 、f y 、u 0 、v 0 are the parameters obtained through the internal calibration of the camera, and u and v represent the coordinates of a certain point on the image;

[0038] By obtaining the positioning of the camera and the installation position of the camera on the mobile robot, the position and posture of the mobile robot are obtained:

[0039]

[0040] Where R is a 3x3 rotation matrix, M represents the translation relationship of the coordinates, and the position of the mobile robot in the world coordinate system is obtained through the transformation matrix.

[0041] Furthermore, the positioning deviation is expressed as:

[0042]

[0043] where Δx b , Δy b is the deviation between the middle positioning code and the image center in the bottom camera image coordinate system, f x 、f y 、u 0 、v 0 is the internal parameter of the camera, z c Indicates the distance between the center of the camera and the ground in the direction of the camera optical axis.

[0044] Furthermore, step S4 specifically includes the following steps:

[0045] The relative distances Δx, Δy and relative angles Δθ between the camera position and the center of the feature code obtained by the camera, the state vector of the mobile robot is its coordinates and heading angle in the world coordinate system. In order to realize the conversion from relative distance to state vector, a camera sensor measurement model is constructed:

[0046]

[0047] in is the state measurement vector of the camera, h c (X k ) The state transition function of the camera, Δx c ,

[0048] Δyc , Δθ c are the offsets between the mobile robot and the center of the feature code, x q ,y q is the coordinate of the feature code center in the world coordinate system, x k ,y k is the coordinate of the mobile robot, θ k is the heading angle of the mobile robot, and is the fixed installation distance between the camera and the center of the mobile robot; the above process does not consider the error of the camera in the recognition and positioning process. In actual situations, in order to achieve accurate positioning, it is necessary to add corresponding error terms. Therefore, when the camera recognizes the feature code, its corresponding measurement equation is expressed as:

[0049]

[0050] in is the measurement value obtained by the camera, which includes the measurement error of the camera, They are Δx c , Δy c , Δθ k The measurements obtained by the camera, is the measurement uncertainty at the kth moment, They are the uncertainties of x, y, and θ about themselves, and the uncertainties are obtained by multiple positioning at fixed points.

[0051] Furthermore, step S5 specifically includes the following steps:

[0052] Define the angular state vector of the mobile robot where θ g is the angle measured by IMU, u g is the measurement error coefficient of the angle and angular velocity obtained by IMU measurement, and the time interval between each sensor measurement is T s , then the sensor angle measurement model is expressed as:

[0053]

[0054] in is the state vector about the heading angle, the subscript k is the variable value at time k, and the similar subscript k-1 is the k-1 time. represents the angle measured by IMU at time k, Represents the measurement error coefficient of the angle and angular velocity obtained by the IMU at time k, ω k is the angular velocity of the mobile robot at time k, Represented as the noise error generated by IMU, is the measured state vector, is the state transfer function. After the top camera and the bottom camera complete feature recognition and positioning, the observed value obtained is the angle offset Δθ relative to the new feature code. c ,Right now

[0055]

[0056] where θ tag is the installation angle of the feature code;

[0057] The corresponding measurement equation is expressed as:

[0058]

[0059] in is the true value of the observation, Represents the sensor measurement value, To obtain the angle deviation between the mobile robot and the feature code for visual recognition, Expressed as the measurement error of the camera, if with u g If the noises are white and do not affect each other, they are fused through extended Kalman filtering;

[0060] The prediction stage is expressed as:

[0061]

[0062] in, is the predicted value of the mobile robot’s state in the prediction stage, is the covariance matrix of the prediction stage, is the best state estimate at time k-1, is the transformation matrix from time k to time k+1, is the angular velocity measured by IMU at the kth moment, are the Jacobian matrices of the state vector X and angular velocity ω according to the measurement equation, Q k-1 is the covariance matrix of the previous moment, is the noise variance of IMU; when the camera does not recognize the feature code, the observation value cannot be obtained, and the state vector and covariance matrix in the prediction result are used as the optimal estimate at the current moment;

[0063] If the camera recognizes the feature code, that is, there is an observation value, then the robot's state is updated, and the updating process is also a fusion process:

[0064]

[0065] Where I is the identity matrix, F is the output matrix of the system, here F = [-1, 0], is the extended Kalman filter gain, which is used to determine the weight of the sensor measurement value during the update. The extended Kalman filter data fusion is realized through the above formula to obtain the optimized heading angle. The top camera and the bottom camera are the same in the update process. By changing the variance of the positioning information during the measurement process Finish.

[0066] Furthermore, step S6 specifically includes the following steps:

[0067] Define the state vector X = [x, y, δ l ,δ r ], where x, y represent the position coordinates of the mobile robot in the plane world coordinate system, δ l , δ r They are respectively expressed as the measurement error coefficients of the odometer related to the left and right wheel speeds, and the corresponding state prediction model is expressed as:

[0068]

[0069] where x k ,y k represents the position coordinates of the mobile robot corresponding to the plane world coordinate system at time k, are the measurement error coefficients of the odometer at time k, are the angular velocities of the left and right wheels measured by the odometer at time k, represents the noise error of the odometer, r represents the radius of the wheel of the differential wheeled mobile robot, θ k-1 is the heading angle at time k-1.

[0070] Furthermore, the Rauch-Tung-Striebel reverse smoothing is as follows:

[0071]

[0072]

[0073] in, is the predicted covariance matrix at time k+1, Q k is the covariance matrix after data fusion at time k, G k is the transformation matrix, is the optimized state vector of the mobile robot at the kth moment. The state of the mobile robot at the kth moment is optimized by using the data at the k+1th moment for reverse smoothing. k 、N kare the Jacobian matrices of the state vector X and angular velocity ω according to the measurement equation at the kth moment, is the noise variance of the IMU at the kth moment, is the best state estimate at time k.

[0074] Furthermore, the external positioning code on the feature code includes a digital feature, and the absolute position of the feature code is represented by two numbers, wherein the digital recognition results are defined as a, b, and c, respectively, and the corresponding coordinates of the feature code are represented as:

[0075] (x tag ,y tag )=(a*L,b*L)

[0076] Among them, L is the laying spacing of the feature code in the actual scene, (x tag ,y tag ) is the coordinate of the feature code in the world coordinate system. The recognition result c is not used for positioning, but is used for feature code node definition in actual scenarios.

[0077] Compared with the prior art, the present invention has the following beneficial effects:

[0078] The present invention is based on a double-layer visual servo positioning system, adopts a new type of feature code, integrates the external positioning code and the middle feature code, uses the top and bottom cameras to obtain the position and posture of the mobile robot respectively, and integrates with the IMU and the odometer to achieve a high-precision positioning solution. Through dual-vision camera positioning, the mobile robot can achieve predictive positioning through the top camera during movement, which improves the flexibility of the mobile robot. The bottom camera can improve the stopping accuracy of the mobile robot. For the needs of multi-point precise positioning, the cost of the mobile robot is greatly reduced. At the same time, the data redundancy of multiple sensors can improve the robustness and stability of the mobile robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0079] Figure 1 This is a flow chart of a multi-sensor fusion positioning method for a mobile robot based on double-layer vision in this embodiment;

[0080] Figure 2 This is a schematic diagram of the external positioning code and the central fusion feature code of this embodiment;

[0081] Figure 3 This is a structural diagram of a differential wheeled mobile robot vehicle configured with multiple sensors in this embodiment;

[0082] Figure 4 This is a schematic diagram of the double-layer visual recognition feature code of the mobile locomotive in this embodiment;

[0083] Figure 5This is a schematic diagram of the mobile robot's roof vision realizing environmental perception in an actual scenario in this embodiment. DETAILED DESCRIPTION

[0084] The application of the present invention is described in detail below by combining embodiments and corresponding drawings, but the present invention is not limited to the described implementation modes.

[0085] Example 1

[0086] like Figure 1 As shown, a multi-sensor fusion positioning method for a mobile robot based on double-layer vision is applied to double-layer visual servo navigation of motion control, including the following steps:

[0087] 1) Design new feature codes and use them for visual recognition to achieve accurate recognition and positioning of new feature codes at long and short distances;

[0088] like Figure 2 As shown, the fusion feature code of the present invention includes an external positioning code and a central positioning code. Except for the central positioning code, all features belong to the external positioning code. The new feature code fuses the external positioning code with the central positioning code and adopts the same positioning feature code to realize multiple monocular vision positioning schemes. The fusion of the two satisfies the recognition of the top camera and the bottom camera at the same time, and can realize various recognition and positioning in multiple situations, multiple angles and multiple schemes, and realize multi-angle recognition methods, providing a basis for the double-layer visual positioning system.

[0089] The external positioning code of the top visual recognition feature code is adopted. The top feature code recognition process includes the following steps: first, the front image of the mobile robot is acquired through the top camera, and the image is subjected to feature extraction through visual processing to reduce the image data and improve the recognition speed. Then, the acquired data is recognized by scanning line by line. The external feature code has an easily recognizable pattern, which includes a black square block and a black square ring surrounded by the black square block. The visual feature has a simple structure and is easy to recognize. In the direction perpendicular to the black edge, the length of the black area from the outside to the inside and the length of the white area have a proportional characteristic relationship, wherein black: white: black: white black = 1:1:4:1:1. By recognizing the proportional feature, the positioning of the feature code in the image can be obtained, and the deviation between the mobile robot and the feature code can be obtained by coordinate transformation.

[0090] Secondly, an affine transformation is performed on the identified positioning to transform the feature code into a state facing the camera. At the same time, the digital part of the feature code is extracted for recognition. The recognition result is expressed as the absolute position of the positioning feature code in the world coordinate system.

[0091] Finally, the position of the mobile robot can be calculated through the deviation and the position of the feature code. The specific calculation method is as follows:

[0092] After the camera recognizes the feature code, it obtains the coordinates (u, v) of the center point of the feature code in the image coordinate system and calculates the relative position and posture of the camera:

[0093]

[0094] Among them, x c ,y c is the coordinate of the feature code center relative to the camera center, z c It is expressed as the distance between the center of the feature code and the center of the camera in the direction of the optical axis, f x 、f y 、i 0 、v 0 The parameters are obtained through the internal calibration of the camera. u and v represent the coordinates of a certain point on the image. Since the camera has a certain height and posture when installed, the size characteristics of the laid feature code are known, so the above method can realize the depth estimation of monocular vision. By obtaining the positioning of the camera, according to the installation position of the camera on the mobile robot, the position and posture of the mobile robot are obtained. Specifically:

[0095]

[0096] Where R is a 3x3 rotation matrix, M represents the translation relationship of the coordinates, and the position of the mobile robot in the world coordinate system is obtained through the transformation matrix.

[0097] The external positioning code adopts a self-encoding method, realizes the relative position positioning of the mobile robot by identifying black and white features, and realizes the positioning of the absolute posture of the mobile robot by identifying numbers. The mobile robot scans the black and white squares. Since the camera shooting is blurred during the movement of the mobile robot, the column characteristics of the external positioning code are discarded, and the feature recognition process is completed by analyzing the image according to the row characteristics. Since each feature square has a fixed distance in reality, the image coordinate system and the world coordinate system are transformed through coordinate transformation to obtain the relative position of the mobile robot and the positioning feature code, that is, to obtain the posture deviation of the mobile robot from the feature code. Further, the feature code realizes the positioning of the absolute posture of the mobile robot according to the identification number, wherein the digital information contains the absolute position of the feature code in the world coordinate system, and the absolute posture of the mobile robot can be obtained by combining the relative position deviation obtained.

[0098] The middle positioning code of the new characteristic code using bottom vision recognition includes the following steps: the bottom camera is placed at the bottom center of the mobile robot. Since the optical axis of the bottom camera is perpendicular to the ground, the center of the camera is at a fixed distance from the ground and the size of the middle positioning code has been determined in the design. When the mobile robot moves above the positioning code, monocular vision positioning is achieved through recognition algorithm and matching algorithm. At the same time, when the mobile robot rotates or moves above the positioning code, it can feedback accurate positioning information through recognition results in real time, and can be used as a site or work point to meet various needs that require precise positioning.

[0099] The central positioning code uses the Apriltag code, and its recognition process is relatively simple. It uses the 36h11 type, in which there are 36 valid squares. The outer black squares are convenient for the positioning of the algorithm. The central 36 squares are encoded by alternating black and white colors, which meets the rotation invariance. By identifying the black and white codes of the internal 36 squares respectively, identifying the black code as 0 and the white code as 1, the code is converted into a binary code. The 36 squares can obtain a binary code with a length of 36 to achieve decoding. In order to determine the relative rotation direction of the mobile robot, the Apriltag code needs to be rotated 90° three times, decoded four times, and matched with the existing coding library. The binary code in the coding library is bound to the absolute coordinates, so the absolute coordinates of the central positioning code can be obtained. The relative deviation between the mobile robot and the central positioning code can be obtained through coordinate transformation. The relative deviation vector can be expressed as:

[0100]

[0101] where Δx b , Δy b is the deviation between the middle positioning code and the image center in the bottom camera image coordinate system, f x 、f y 、u 0 、v 0 It is the internal parameter of the camera.

[0102] In the process of identifying the external positioning code, since the central positioning code adopts the Apriltag code, its characteristics are different from the positioning pattern of the external positioning code. Through algorithm filtering, the external positioning code is not affected by the change of the central feature code during the identification process.

[0103] Specifically, the present invention uses a differential wheeled mobile robot as the basis for the overall configuration, such as Figure 3As shown in the figure, the top camera is installed on the top of the mobile robot, the optical axis and the forward direction of the mobile robot are in the same plane, and the projection of the optical axis and the ground forms an angle with the optical axis, and the angle is 35°. The camera installed on the top is used to identify the external positioning code. The characteristics of the external positioning code are relatively obvious, and can achieve identification and positioning at a long distance, which provides conditions for the identification of the top feature code and reduces the accuracy requirements for the camera. The bottom feature code is installed at the bottom center of the mobile robot. It is close to the ground and is used to identify the middle feature code. At the same time, it is configured with a corresponding light source, which has the characteristics of stable identification and high positioning accuracy. In terms of control, the processing of the information obtained by the camera is realized through the industrial computer, and the data of multiple sensors are fused through the extended Kalman filter algorithm and the RTSS reverse smoothing algorithm.

[0104] The process of mobile robot identifying feature code during movement is as follows: Figure 4 As shown in the figure, when the mobile robot moves to the top of the feature code, the bottom camera is used to recognize and locate the middle positioning code. When the mobile robot is moving, if another feature code appears in front of it, it can be pre-recognized before the mobile robot arrives, and the corresponding deviation information can be obtained in advance.

[0105] 2) Based on the differential wheeled mobile robot, a corresponding dynamic model is constructed to link the corresponding system control quantity with the information obtained by the sensor, providing a practical basis for the realization of multi-sensor fusion technology. Define the state vector of the mobile robot Where [x, y] represents the position coordinates of the mobile robot in the plane world coordinate system. θ is the angle of the mobile robot relative to the x-axis direction of the world coordinate system. δ, They are respectively expressed as the corresponding system deviation parameters affected by the linear velocity and angular velocity. The corresponding dynamic model is expressed as:

[0106]

[0107] in, is the derivative of the state vector with respect to time, reflecting the state change of the mobile robot. are the derivatives of x, y, θ, ω w The angular velocity of the wheel is defined as [ω r ,ω l ] T ,ω r ,ω l They represent the angular velocity of the right and left wheels of the mobile robot respectively, and f(X) is the calculation function of the state change of the mobile robot.

[0108] According to the structure of the differential wheeled robot, f(X) can be expressed as:

[0109]

[0110] Where r represents the radius of the wheel of the differential wheeled mobile robot, and d represents the distance between the two wheels of the mobile robot;

[0111] The state observation vector Z of the sensor to the mobile robot can be expressed by the measurement equation:

[0112] Z=h(X)

[0113] Where X is the state vector of the mobile robot, and h(·) is a nonlinear transfer matrix that represents the relative relationship between the sensor’s observations and the mobile robot. By adding the time period T s Discretize the system and add the system noise error caused by IMU and odometer into the system:

[0114] X k+1 =X k +f(X k )T s Δω

[0115] Z k+1 =h(X)

[0116] Where X k+1 , X k They are represented as the state vectors of the mobile robot at time k+1 and time k, respectively, Z k+1 Represented as the state observation vector at time k; Δω=ω wk +ε k ,ω wk is the wheel speed vector at the kth moment, ε k is the system noise at the kth moment, and the state vector at the k+1th moment can be obtained through the state change at the kth moment.

[0117] The relative distances Δx and Δy and the relative angle Δθ between the camera position and the center of the feature code can be obtained through the camera. However, the position of the camera does not represent the position of the mobile robot. In the actual process, there is usually a certain offset between the camera and the center of the mobile robot. However, the fixed position of the camera is known, and the relative distance between the mobile robot and the center of the positioning code can be obtained through the change of coordinates. Therefore, the measurement equation of the camera is expressed as:

[0118]

[0119] in is the state measurement vector of the camera, h c (X k ) The state transition function of the camera, Δx c , Δy c , Δθ care the offsets between the robot and the center of the feature code, x q ,y q is the coordinate of the feature code center in the world coordinate system, x k ,y k is the coordinate of the mobile robot, θ k is the heading angle of the mobile robot, and is the fixed installation distance between the camera and the center of the mobile robot. Taking the top camera as an example, the measurement process is as follows: Figure 5 As shown, the ground is paved with evenly spaced feature codes, the distance between adjacent feature codes is L, and the fixed coordinates of the feature codes are (x tag ,y tag ), the angle between the feature code and the x-axis of the world coordinate system is θ tag After the mobile robot recognizes the feature code through the top camera, it obtains the relative deviation through the corresponding decoding process, where Δx c , Δy c , Δθ c In the top camera recognition, it is Δx top , Δy top , Δθ top .

[0120] When the camera recognizes the feature code, the corresponding measurement equation is:

[0121]

[0122] in The measurement value obtained by the camera includes the measurement error of the camera. They are Δx c , Δy c , Δθ k The measurements obtained by the camera, For the kT s The measurement uncertainty at time, are the uncertainties of x, y, and θ about themselves.

[0123] 3) Since the recognition of feature codes is intermittent and there may be cases where no feature codes are recognized, it is necessary to use IMU sensors as state prediction sensors. The high-frequency information feedback characteristics of IMU sensors are less affected by external factors. As a guarantee of the positioning system, the state of the mobile robot is predicted;

[0124] The angular velocity of the mobile robot is obtained through the IMU, and the data is fused with the angle obtained by the mobile robot scanning the code to obtain accurate positioning effect. The specific implementation method is as follows:

[0125] Define the state vector where θg Defined as the angle measured by the IMU, u g Positioning is the measurement error coefficient related to angular velocity, where each sensor measurement time interval T s , then the sensor angle measurement model is expressed as:

[0126]

[0127] in It is represented as the state vector about the heading angle. The subscript k represents the variable value at time k. Similarly, the subscript k-1 represents the k-1 time. k It is expressed as the angular velocity of the mobile robot at time k, Represented as the noise error generated by IMU, is the measured state vector, is the state transfer function. After the top camera and the bottom camera complete feature recognition and positioning, the observed value obtained is the angle offset Δθ relative to the new feature code. c .Right now

[0128]

[0129] Therefore, the corresponding measurement equation can be expressed as

[0130]

[0131] in is the true value of the observation, Indicates the sensor measurement value, To obtain the angle deviation between the mobile robot and the feature code for visual recognition, Expressed as the measurement error of the camera, if with u g are white noises and do not affect each other, and are fused through extended Kalman filtering. According to the above formula, the prediction stage can be expressed as

[0132]

[0133] in, is the predicted value of the mobile robot’s state in the prediction stage, is the covariance matrix of the prediction stage, is the best state estimate at time k, is the transformation matrix from time k to time k+1, is the angular velocity measured by IMU at the kth moment, are the Jacobian matrices of the state vector X and angular velocity ω according to the measurement model equation, Q k-1 is the covariance matrix of the previous moment, is the noise variance of IMU;

[0134] Furthermore, the update step can be expressed as

[0135]

[0136] Where I is the identity matrix, F is the output matrix of the system, F = [-1, 0], is the extended Kalman filter gain, which is used to determine the weight of the sensor measurement value during the update. The extended Kalman filter data fusion is realized through the above formula to obtain the optimized heading angle. The top camera and the bottom camera are the same in the update process. By changing the variance of the positioning information during the measurement process Finish.

[0137] Similarly, define the state vector X = [x, y, δ l ,δ r ], where δ l , δ r They are respectively expressed as the measurement error coefficients of the odometer related to the left and right wheel speeds. The corresponding state prediction model can be expressed as:

[0138]

[0139] in are the measurement error coefficients of the odometer at time k, are the angular velocities of the left and right wheels measured by the odometer at time k, r represents the radius of the wheel of the differential wheeled mobile robot, θ k-1 is the heading angle at time k-1, Represents the noise error of the odometer. In actual application, the positioning noise of the camera and the noise of the odometer do not interfere with each other. If both are white noise, the extended Kalman filter method can be used to convert the robot coordinates obtained by the camera and the coordinates predicted by the odometer The specific fusion process is similar to the heading angle fusion, except that the corresponding Jacobian matrix and the subsequent covariance matrix are different.

[0140] The present invention adopts double-layer vision for fusion. In the implementation process, the corresponding posture information is obtained through the top and bottom camera recognition as the input of the measurement equation. Since the positions of the recognized positioning feature codes are different, the external parameters of the cameras are different, and the positioning covariance parameters are also different. At the same time, continuous state prediction and intermittent state update are adopted during the positioning time process to cope with the recognition of discontinuous positioning feature codes. The present invention adopts a fixed multi-point test method to determine the positioning error parameters of the camera.

[0141] Example 2

[0142] The present invention proposes a multi-sensor fusion positioning method for a mobile robot based on double-layer vision. The invention includes: (1) using innovative feature codes to achieve multi-angle recognition and positioning functions, placing cameras on the top and bottom of the mobile robot, and obtaining the position and posture of the mobile robot by identifying the fixed and ground feature codes before the mobile robot moves above the feature code. When the mobile robot moves above the feature code, the position and posture of the mobile robot is obtained by scanning and identifying the positioning code in the middle of the new feature code. (2) A double-layer servo navigation method based on a new feature code, which recognizes feature codes respectively through dual cameras, and obtains high-precision positioning information by fusing IMU, wheel odometer and intermittently acquired visual positioning codes.

[0143] After the Kalman filter is implemented, the direction of the filtered data is smoothed by saving it to achieve data optimization, so as to improve the matching degree between the positioning trajectory of the mobile robot and the actual situation. As a method of post-processing or quasi-real-time data processing, smoothing technology can improve the accuracy of data processing to a certain extent and has been widely used in the field of surveying and mapping. Smoothing technology is generally divided into three categories: fixed interval smoothing, fixed point smoothing and fixed lag smoothing. Among them, the most widely used method in data post-processing is fixed interval smoothing.

[0144] The RTS algorithm is an optimal smoothing algorithm for a fixed interval. It performs reverse smoothing on the acquired data through the Rauch-Tung-Striebel smoothing algorithm. It can effectively solve the problem of discontinuous visual positioning data caused by the intermittent feature code recognition, which in turn causes positioning offset. It makes full use of the measurement values ​​at all times in the interval to estimate the state at a certain moment, thereby further improving the positioning accuracy. The reverse smoothing technology is performed after the Kalman filter, and the data after the Kalman filter is used to complete the smoothing. The specific steps are as follows:

[0145]

[0146] in, is the predicted covariance matrix at time k+1, Q k is the covariance matrix after data fusion at time k, G k is the transformation matrix, is the optimized state vector of the mobile robot at the kth moment. The state of the mobile robot at the kth moment is optimized by using the data at the k+1th moment for reverse smoothing.

[0147] The mobile robot multi-sensor fusion positioning method based on double-layer vision described in the present invention can also be used for fixed-point landing of unmanned aerial vehicles, and is used to improve the stability and accuracy of the landing of unmanned aerial vehicles. When the unmanned aerial vehicle is preparing to land, it first identifies the external feature code through vision and locates the landing point. When it lands to a certain height, it can identify the central feature code through vision to obtain a more accurate positioning. Since the unmanned aerial vehicle flies at a higher position, during the landing process, due to interference during flight, the landing position will deviate, and the positioning will be inaccurate. In the case of a large space required to be reserved during the landing process, a more accurate positioning effect can be obtained by processing the positioning trajectory using a smoothing algorithm.

[0148] In the early stage of the landing process, the drone is far away from the ground, and the features required for visual recognition are large. The innovative feature code designed is used to identify the external feature code, and it also has the characteristics of anti-motion blur. The drone can achieve positioning by identifying the external positioning code, and can adjust its posture faster and more timely at a longer distance. When the drone further lowers its altitude and moves to a distance closer to the feature code, the external positioning code and the central positioning code can be recognized at the same time, and the positioning effect can be optimized through the fusion algorithm. When the drone's altitude is further reduced, the external positioning code is in the camera's field of view and cannot be fully displayed. At this time, the external positioning code can no longer be recognized, and the precise positioning of the drone is controlled through the central positioning code. It is worth mentioning that when the external feature code is recognized, the drone is controlled to move above the feature code through a deviation-based control algorithm to ensure the recognition of the central positioning code.

[0149] Example 3

[0150] The dual-layer visual positioning method can improve the movement efficiency of mobile robots by designing node types in industrial situations with multi-node requirements. It is suitable for logistics warehousing and high-beat industrial transportation scenarios. In the process of identifying external feature codes, the numbers in the external feature codes can not only include positioning information, but also feature code node information, by defining the node, such as fast-moving nodes, stop nodes, turning nodes, etc. During the operation of the mobile robot, the top camera can realize the function of prediction without reaching the node, and there is no need to identify and wait on the feature code when passing through the fast-moving node. At the same time, for high-precision positioning nodes, it can provide more accurate positioning effects at the expense of control time.

[0151] According to different positioning nodes, the present invention can adopt the following Figure 2 The characteristic code shown in the figure changes one of the digital recognition areas of the external positioning code on the basis of the original one, changing it into a square shape that is easier to identify.

[0152] The innovative feature code adopted can set the node type through self-encoding, which can be specifically divided into fast-moving nodes, stop nodes, etc. The specific implementation process is as follows: on the external positioning code on the feature code, the digital feature can not only contain the location information of the located feature code, but also can define the feature code accordingly, so that the mobile robot can realize its own positioning and position status update without stopping, and can pre-determine the control of the mobile robot through node definition. The absolute position of the feature code can be achieved by two digits, and letters can be used to expand when the number of digits is insufficient. The top camera has identified the digital features in the process of identifying the external positioning code, and the digital recognition results are defined as a, b, and c respectively. The corresponding coordinates of the feature code are expressed as:

[0153] (x tag ,y tag )=(a*L,b*L)

[0154] Among them, L is the laying spacing of the feature code in the actual scene, (x tag ,y tag ) is represented as the coordinates of the feature code in the world coordinate system. When the actual scene requires a more complex site, the positioning method that can be achieved by numbers can only achieve positioning within the range of 10L*10L. The positioning range is expanded by replacing the digital features with extended letter features. Since the symbols used are standard characters, after affine transformation, the recognition difficulty is relatively low and accurate recognition can be achieved. The positioning coordinate calculation method of the feature code is the same as the digital solution, but the result is expressed in decimal ASCII code.

[0155] The digital recognition result c is used for defining the feature code node. The recognition result is compared with the pre-defined node library. Combined with the feature of advance recognition of the top vision, the mobile robot can obtain node information in advance when reaching the feature code, which can be applied to the optimization system. The remaining digital part is used for node definition. When the system can adopt different positioning strategies for different nodes according to the needs, it can realize the optimization control of the system.

[0156] Example 4

[0157] The multi-sensor fusion positioning method based on double-layer vision in the present invention can be applied to composite robots. Since the motion performance of mobile robots is different, there will be certain errors in the control accuracy. The present invention achieves high-precision positioning by multi-sensor fusion. When the motion control system of the mobile robot cannot guarantee that it moves to the required positioning accuracy, compensation is provided by providing positioning deviation.

[0158] Composite robots usually add mechanical arms, gripping devices, etc. on the basis of mobile robots. In actual scenarios, mobile robots, as the basis of movement, usually only need to meet the role of transporting the upper structure. However, with the increasing demand for high-precision movement, the requirements for mobile chassis are also increasing, and the corresponding control time and chassis structure design requirements are also increasing accordingly, which inevitably leads to an increase in costs. The use of double-layer vision for fusion positioning can achieve high-precision positioning requirements. Even if the mobile robot cannot achieve the required accuracy at the required position, it can still feedback the positioning deviation and feedback the error to the upper mechanism through error compensation. The upper mechanism's precise operation is completed through coordinated control of the upper mechanism.

[0159] The preferred embodiments of the present invention disclosed above are only used to help illustrate the present invention. The preferred embodiments do not describe all the details in detail, nor do they limit the invention to the specific implementation methods described. Obviously, many modifications and changes can be made according to the content of this specification. This specification selects and specifically describes these embodiments in order to better explain the principles and practical applications of the present invention, so that those skilled in the art can understand and use the present invention well. The present invention is limited only by the claims and their full scope and equivalents.

Claims

1. A mobile robot multi-sensor fusion positioning method based on double-layer vision, characterized in that: The following steps are involved: S1. Construct a dynamic model based on the corresponding system control quantity of the differential wheeled mobile robot and the information obtained by the sensor; S2, identifying the external positioning code in the new feature code through the camera on the top of the mobile robot, and extracting and obtaining the position and posture of the mobile robot before the mobile robot reaches the new feature code; S3, using the camera at the bottom of the mobile robot to identify the middle positioning code at the bottom of the mobile robot. When the mobile robot moves above the new feature code, the middle positioning code in the new feature code is identified by the bottom camera, and the positioning deviation can be obtained by the coordinate conversion algorithm, and the position and posture of the mobile robot can be obtained by the matching algorithm. S4. Build a camera sensor measurement model based on the position and positioning error of the mobile robot, analyze the error in the positioning algorithm process, and obtain the state vector of the mobile robot and the covariance matrix related to the camera measurement error; S5. The angular velocity of the mobile robot is obtained by using the IMU sensor as a state prediction sensor. The state of the mobile robot at the next moment is predicted by combining the IMU angular velocity dynamics model and the differential wheel dynamics model. The state prediction result is fused with the camera state vector and the heading angle part in the covariance matrix. Specifically, the following steps are included: Define the mobile robot angle state vector X θ =[θ g ,u g ], where θ g is the angle measured by IMU, u g is the measurement error coefficient of the angle and angular velocity obtained by IMU measurement, and the time interval between each sensor measurement is T s , then the sensor angle measurement model is expressed as: Among them, T s is the time interval between two adjacent moments, that is, the time interval between moments k and k+1, is the state vector about the heading angle, the subscript k is the variable value at time k, and the similar subscript k-1 is the k-1 time. represents the angle measured by IMU at time k, Represents the measurement error coefficient of the angle and angular velocity obtained by the IMU at time k, ω k is the angular velocity of the mobile robot at time k, Represented as the noise error generated by IMU, is the measured state vector, is the state transfer function. After the top camera and the bottom camera complete feature recognition and positioning, the observed value obtained is the angle offset Δθ relative to the new feature code. c ,Right now where θ tag is the installation angle of the feature code; The corresponding measurement equation is expressed as: in is the true value of the observation, Represents the sensor measurement value, To obtain the angle deviation between the mobile robot and the feature code for visual recognition, Expressed as the measurement error of the camera, if with u g If the noises are white and do not affect each other, they are fused through extended Kalman filtering; The prediction stage is expressed as: in, is the predicted value of the mobile robot’s state in the prediction stage, is the covariance matrix of the prediction stage, is the best state estimate at time k-1, is the transformation matrix from time k to time k+1, is the angular velocity measured by IMU at the kth moment, are the Jacobian matrices of the state vector X and angular velocity ω according to the measurement equation, Q k-1 is the covariance matrix of the previous moment, is the noise variance of IMU; when the camera does not recognize the feature code, the observation value cannot be obtained, and the state vector and covariance matrix in the prediction result are used as the optimal estimate at the current moment; If the camera recognizes the feature code, that is, there is an observation value, then the robot's state is updated, and the updating process is also a fusion process: Where I is the identity matrix, F is the output matrix of the system, here F = [-1, 0], is the extended Kalman filter gain, which is used to determine the weight of the sensor measurement value during the update. The extended Kalman filter data fusion is realized through the above formula to obtain the optimized heading angle. The top camera and the bottom camera are the same in the update process. By changing the variance of the positioning information during the measurement process Finish; S6. Obtain the angular velocity of the wheels of the mobile robot through the odometer, and perform state prediction on the mobile robot at the next moment in combination with the dynamic model. The state prediction result is data-fused with the coordinate part of the camera state vector and the covariance matrix; S7. After the data fusion is completed, the obtained posture data of the mobile robot is further smoothed by using Rauch-Tung-Striebel reverse smoothing to obtain the optimal estimate of the state of the mobile robot.

2. The mobile robot multi-sensor fusion positioning method based on double-layer vision according to claim 1 is characterized in that: The kinetic model is as follows: Define the state vector of the mobile robot Where [x, y] represents the position coordinates of the mobile robot in the planar world coordinate system, θ is the angle between the mobile robot and the x-axis in the world coordinate system, δ, They are respectively expressed as the corresponding system deviation parameters affected by the linear velocity and angular velocity; the corresponding dynamic model is expressed as: in, is the derivative of the state vector with respect to time, reflecting the state change of the mobile robot. are the derivatives of x, y, θ, ω w The angular velocity vector of the wheel rotation is defined as [ω r ,ω l ] T ,ω r ,ω l They represent the angular velocities of the right and left wheels of the mobile robot, respectively, and f(X) is the calculation function of the state change of the mobile robot; According to the structure of the differential wheeled robot, f(X) is expressed as: Where r represents the radius of the wheel of the differential wheeled mobile robot, and d represents the distance between the two wheels of the mobile robot; The state observation vector Z of the mobile robot by the sensor is expressed by the measurement equation: Z=h(X) Where X is the state vector of the mobile robot, h(·) is the nonlinear transfer matrix, which represents the relative relationship between the sensor observations and the state of the mobile robot; By adding the time period T s Discretize the system and add the system noise error caused by IMU and odometer: X k+1 =X k +f(X k )T s Δω Z k =h(X k ) Where X k+1 , X k They are represented as the state vectors of the mobile robot at time k+1 and time k, respectively, Z k Represented as the state observation vector at time k; Δω=ω wk +ε k ,ω wk is the wheel speed vector at the kth moment, ε k is the system noise at the kth moment, and the state vector at the k+1th moment is obtained through the state change at the kth moment, T s is the time interval between two adjacent moments, that is, the time interval between moments k and k+1.

3. The mobile robot multi-sensor fusion positioning method based on double-layer vision according to claim 1 is characterized in that: The new feature codes include external positioning codes and central positioning codes; The external positioning code adopts the self-encoding method, realizes the relative position positioning of the mobile robot by identifying the black and white features, and realizes the absolute position positioning of the mobile robot by identifying the numbers; The central positioning code uses the Apriltag code, and its type is 36h11.

4. The mobile robot multi-sensor fusion positioning method based on double-layer vision according to claim 1 is characterized in that: The camera on the top of the mobile robot recognizes the new feature code, including the following steps: First, the image of the new feature code in front of the mobile robot is obtained through the top camera, and the features of the image are extracted through visual processing. The obtained data is identified by scanning line by line, and the external feature code has a recognizable pattern; Secondly, an affine transformation is performed on the identified positioning to convert the feature code into a state facing the camera. At the same time, the digital part of the feature code is extracted for recognition. The recognition result is expressed as the absolute position of the positioning feature code in the world coordinate system. Finally, the position and posture of the mobile robot are calculated by the deviation and the absolute position of the feature code in the world coordinate system. The specific calculation method is as follows: After the camera recognizes the feature code, it obtains the coordinates (u, v) of the center point of the feature code in the image coordinate system and calculates the relative position and posture of the camera: Among them, x c ,y c is the coordinate of the feature code center relative to the camera center, z c It is expressed as the distance between the center of the feature code and the center of the camera in the direction of the optical axis, f x 、f y , u0, v0 are the parameters obtained through the internal calibration of the camera, and u and v represent the coordinates of a certain point on the image; By obtaining the positioning of the camera and the installation position of the camera on the mobile robot, the position and posture of the mobile robot are obtained: Where R is a 3x3 rotation matrix, M represents the translation relationship of the coordinates, and the position of the mobile robot in the world coordinate system is obtained through the transformation matrix.

5. The mobile robot multi-sensor fusion positioning method based on double-layer vision according to claim 1 is characterized in that: The positioning deviation is expressed as: where Δx b , Δy b is the deviation between the middle positioning code and the image center in the bottom camera image coordinate system, f x 、f y , u0, v0 are the internal parameters of the camera, z c Indicates the distance between the center of the camera and the ground in the direction of the camera optical axis.

6. The mobile robot multi-sensor fusion positioning method based on double-layer vision according to claim 1 is characterized in that: Step S6 specifically includes the following steps: Define the state vector X = [x, y, δ l ,δ r ], where x, y represent the position coordinates of the mobile robot in the plane world coordinate system, δ l , δ r They are respectively expressed as the measurement error coefficients of the odometer related to the left and right wheel speeds, and the corresponding state prediction model is expressed as: where x k ,y k represents the position coordinates of the mobile robot corresponding to the plane world coordinate system at time k, are the measurement error coefficients of the odometer at time k, are the angular velocities of the left and right wheels measured by the odometer at time k, represents the noise error of the odometer, r represents the radius of the wheel of the differential wheeled mobile robot, θ k-1 is the heading angle at time k-1.

7. The mobile robot multi-sensor fusion positioning method based on double-layer vision according to claim 1 is characterized in that: Rauch-Tung-Striebel backsmoothing is as follows: in, is the k+1 time prediction covariance matrix, Q k is the covariance matrix after data fusion at time k, G k is the transformation matrix, is the optimized state vector of the mobile robot at the kth moment. The state of the mobile robot at the kth moment is optimized by using the data at the k+1th moment for reverse smoothing. k 、N k are the Jacobian matrices of the state vector X and angular velocity ω according to the measurement equation at the kth moment, is the noise variance of the IMU at the kth moment, is the best state estimate at time k.

8. The mobile robot multi-sensor fusion positioning method based on double-layer vision according to claim 2 is characterized in that: The external positioning code on the feature code includes a digital feature. The absolute position of the feature code is defined by two numbers, where the digital recognition results are defined as a, b, and c respectively. The corresponding coordinates of the feature code are expressed as: (x tag ,y tag )=(a*L,b*L) Among them, L is the laying spacing of the feature code in the actual scene, (x tag ,y tag ) is the coordinate of the feature code in the world coordinate system. The recognition result c is not used for positioning, but is used for feature code node definition in actual scenarios.

Images