Visual positioning multi-modal data fusion method

Abstract

The invention discloses a visual positioning multi-modal data fusion method. The method comprises the following steps: S1, acquiring image data; image information is obtained through a visual sensor and a laser radar; the image information comprises center point, size, category information and depth information of the target frame; the image information is converted into three-dimensional coordinate information; s2, acquiring expected speed information in a preset time through an IMU sensor, then determining expected pose information by scanning an Aruco code, and then adjusting an error value between the expected speed information and the predicted speed information by adopting a least square method to determine the predicted speed information; s3, predicting attitude data through the Markov chain data transition probability matrix, the predicted speed information, the predicted three-dimensional coordinate information and the target frame information of the current position in the image information, and then performing positioning movement according to the predicted attitude data. And high-accuracy attitude output of the mobile robot is realized for various fusion data of different mobile robots.

Description

Technical Field

[0001] The present invention belongs to the technical field of data fusion, and in particular relates to a visual positioning multimodal data fusion method. Background Art

[0002] AGV is the abbreviation of Automated Guided Vehicle, which means "automatic guided transport vehicle". In fact, it is a common mobile robot currently. It refers to a transport vehicle equipped with an electromagnetic or optical automatic guidance device, which can travel along a specified guidance path and has safety protection and various transfer functions. It is widely used in logistics, warehousing and transportation.

[0003] It is common for traditional industrial AGVs to use a single sensor as the AGV guidance decision-making technology solution, which greatly reduces the credibility and effectiveness of the navigation device information data source, thereby affecting the overall guidance decision-making ability of the system.

[0004] In order to solve the above problems, a patent document with Chinese patent application number 201810481828.1 and publication date 2018.11.02 discloses a multi-sensor data fusion method and an AGV navigation and positioning device. The multi-sensor data fusion AGV navigation and positioning device includes an AGV body and sensors arranged on the AGV body. The sensors include a first infrared sensor and a second infrared sensor arranged on the left and right sides of the AGV body, and an ultrasonic sensor arranged at the front of the AGV body. There are 3 to 5 ultrasonic sensors. The extended Kalman filter algorithm is used to fuse the multi-sensor data information. This method can improve the system's fault tolerance and data credibility and reduce the cost of data acquisition. The extended Kalman filter algorithm is used to track and predict the system state measurement data in real time, thereby improving the positioning accuracy.

[0005] This method uses the extended Kalman filter algorithm to fuse multi-sensor data information, but in the multi-modal fusion data, the high-precision positioning of the lidar will be very dependent on the data of the IMU inertial navigation, odometer and other sensors, and it will not consider the detection box size and image depth in the image information, so it is impossible to fuse the image information with the speed information. For example, when the obstacle at the current speed and posture is similar to the preset obstacle because the detection box is too small, the obstacle recognition error will be caused, resulting in posture prediction error, which will have a certain impact on the subsequent posture of the mobile robot. Summary of the invention

[0006] The purpose of the present invention is to provide a visual positioning multimodal data fusion method, which can achieve high-accuracy posture prediction of the robot through the fusion data of vision and IMU inertial sensors, thereby ensuring reliable obstacle avoidance and other processing.

[0007] To achieve the above purpose, a visual positioning multimodal data fusion method includes the following steps: S1 obtains image data; obtains image information through visual sensors and laser radar; the image information includes the center point, size, category information and depth information of the target frame; and converts the image information into three-dimensional coordinate information; S2 obtains the expected speed information within the preset time through the IMU sensor, and then determines the expected posture information by scanning the Aruco code, and then uses the least squares method to adjust the error value between the expected speed information and the predicted speed information to determine the predicted speed information, and uses the least squares method to adjust the error between the expected three-dimensional coordinate information and the predicted three-dimensional coordinate information to determine the predicted three-dimensional coordinate information; S3 predicts the posture data through the Markov chain data transfer probability matrix, predicted speed information, predicted three-dimensional coordinate information, and the target frame information of the current position in the image information, and then performs positioning and movement based on the predicted posture data.

[0008] The above method determines the expected speed information and the expected three-dimensional coordinate information of the current position in the three-dimensional image through the IMU sensor, and then adjusts the deviation between the predicted value and the expected value through the least squares method to adjust the predicted value, and then uses the adjusted predicted value, the posture information obtained by scanning the Aruco code, the information of the target frame in the image information, and the Markov chain data transfer probability matrix to predict the posture data, so that multiple data such as speed, position, posture, image target frame information are fused, so that the posture is associated with the speed, position, posture and target frame information in the image, thereby ensuring the reliability of the posture data.

[0009] Furthermore, S2 includes S21 and uses the least square method to adjust the error, such as the formula: ; Among them, N records the IMU sensor data within a period of time, F is the expected data fed back by scanning the Aruco code, and P is the predicted value, which contains the x, y, z position coordinates and the speed information in the x, y, z directions respectively. The speed information can be obtained through the IMU sensor, and E is the error value.

[0010] S22 adjusts the predicted value of the mobile robot according to the error value.

[0011] The above settings may have complex motion and environmental conditions in actual applications. By establishing a nonlinear mapping relationship, these complex scenarios can be better handled, and the adaptability and robustness of the model can be improved. By establishing a nonlinear mapping relationship, the relationship between the inertial sensor measurement value and the actual physical quantity can be better handled, the measurement accuracy can be improved, the error can be reduced, and different sensor models and complex scenarios can be adapted. The specific benefits will vary depending on the application scenario and requirements. In actual applications, it is necessary to select appropriate mapping functions and models according to the specific situation to achieve the best effect.

[0012] Furthermore, the step S3 includes: S31 If the Markov chain is in state j at time (t-1) and changes to state i at time t, then the conditional probability distribution matrix is: ; is the state at time t; is the state at time t-1; The Markov chain transition probability matrix finally formed by S32 is ; S33 estimates the output posture information of the mobile robot in the next state through the Markov chain transition probability matrix in the current state.

[0013] The above settings can predict future states by analyzing the current state and transition probability, and can be trained and adjusted based on actual data to adapt to different situations.

[0014] Furthermore, after step S33, it also includes: S34 determines the action in the current situation through a reward and penalty mechanism. If there is an obstacle on the right side, if it is estimated that the output posture information of the next state is to move to the right for collision, then a penalty process will be performed, so that the mobile robot will not consider the posture information that has been punished in the next posture information estimation process; if it is estimated that the output posture information of the next state is to move to the left for obstacle avoidance, then a reward process will be performed, so that the mobile robot will consider the posture information that has been rewarded in the next posture information estimation process.

[0015] The above settings can facilitate the mobile robot to adjust the estimated posture information in turn according to the subsequent actual posture information operation process, thereby ensuring the reliability of the estimated posture information.

[0016] Furthermore, each state includes: the center point, size and category information of the target frame; the depth information of the image information; the current coordinate information, the current speed information and the current posture information.

[0017] The above settings facilitate data fusion for data in each state. BRIEF DESCRIPTION OF THE DRAWINGS

[0018] Figure 1 This is the workflow of the present invention. DETAILED DESCRIPTION

[0019] The present invention is further described in detail below with reference to the accompanying drawings and specific embodiments.

[0020] like Figure 1 As shown, a visual positioning multimodal data fusion method includes the following steps: S1 obtains image data; image information obtained through visual sensors and laser radars; the image information includes the center point, size, category information, and depth information of the target frame; and Convert image information into three-dimensional coordinate information; in this embodiment, after the visual sensor and the laser radar obtain image data, the image information is obtained through the YOLOv5 algorithm, and then the three-dimensional coordinate information is obtained through the existing mapping matrix; S2 obtains the expected speed information within the preset time through the IMU sensor, and then determines the expected posture information by scanning the Aruco code, and then uses the least squares method to adjust the error value between the expected speed information and the predicted speed information to determine the predicted speed information, and uses the least squares method to adjust the error between the expected three-dimensional coordinate information and the predicted three-dimensional coordinate information to determine the predicted three-dimensional coordinate information; S2 includes S21 and S22, S21 uses the least squares method to adjust the error, as shown in the formula: ; Among them, N records the IMU safety data within a period of time, F is the expected data fed back by scanning the Aruco code, P is the predicted value, which contains the x, y, z position coordinates and speed information in the x, y, z directions respectively, and E is the error value.

[0021] S22 adjusts the predicted value of the mobile robot according to the error value.

[0022] S3 predicts the posture data through the Markov chain data transfer probability matrix, predicted speed information, predicted three-dimensional coordinate information, and the target frame information of the current position in the image information, and then performs positioning and movement based on the predicted posture data.

[0023] The step S3 includes S31-S34, S31 If the Markov chain is in state j at time (t-1) and changes to state i at time t, then the conditional probability distribution matrix is: ; is the state at time t; The state at time t-1; each state includes: the center point, size and category information of the target frame; the depth information of the image information; the current coordinate information, current speed information and current posture information; The Markov chain transition probability matrix finally formed by S32 is ; S33 estimates the output posture information of the mobile robot in the next state through the Markov chain transition probability matrix in the current state.

[0024] S34 determines the action in the current situation through a reward and penalty mechanism. If there is an obstacle on the right side, if it is estimated that the output posture information of the next state is to move to the right for collision, a penalty will be performed, so that the mobile robot will not consider the posture information that has been punished during the next posture information estimation process. If it is estimated that the output posture information of the next state is to move to the left for obstacle avoidance, a reward will be performed, so that the mobile robot will consider the posture information that has been rewarded during the next posture information estimation process.

[0025] The above settings can facilitate the mobile robot to adjust the estimated posture information in turn according to the subsequent actual posture information operation process, thereby ensuring the reliability of the estimated posture information.

[0026] The working principle of the present invention is as follows: the expected speed information and the expected three-dimensional coordinate information of the current position in the three-dimensional image are determined by an IMU sensor, and then the deviation between the predicted value and the expected value is adjusted by the least square method, and then the adjusted predicted value, the posture information obtained by scanning the Aruco code, the information of the target frame in the image information, and the Markov chain data transfer probability matrix are used together to predict the posture data, so that multiple data such as speed, position, posture, image target frame information, etc. are fused, so that the posture is associated with the speed, position, posture and the target frame information in the image, thereby ensuring the reliability of the posture data.

Claims

1. A visual positioning multimodal data fusion method, characterized by: The following steps are involved: S1 obtains image data; obtains image information through visual sensors and laser radar; the image information includes the center point, size, category information and depth information of the target frame; and converts the image information into three-dimensional coordinate information; S2 obtains the expected speed information within the preset time through the IMU sensor, and then determines the expected posture information by scanning the Aruco code, and then uses the least squares method to adjust the error value between the expected speed information and the predicted speed information to determine the predicted speed information, and uses the least squares method to adjust the error between the expected three-dimensional coordinate information and the predicted three-dimensional coordinate information to determine the predicted three-dimensional coordinate information; S3 predicts the posture data through the Markov chain data transfer probability matrix, predicted speed information, predicted three-dimensional coordinate information, and the target frame information of the current position in the image information, and then performs positioning and movement based on the predicted posture data.

2. The method for multimodal data fusion of visual positioning according to claim 1, characterized in that: S2 and S21 use the least square method to adjust the error, as shown in the formula: ； Among them, N records the IMU sensor data within a period of time, F is the expected data fed back by scanning the Aruco code, P is the predicted value, which contains the x, y, z position coordinates and the speed information in the x, y, z directions respectively, and E is the error value.

3. The method for multimodal data fusion of visual positioning according to claim 1, characterized in that: The step S3 comprises: S31 If the Markov chain is in state j at time (t-1) and changes to state i at time t, then the conditional probability distribution matrix is: ; is the state at time t; is the state at time t-1; The Markov chain transition probability matrix finally formed by S32 is ； S33 estimates the output posture information of the mobile robot in the next state through the Markov chain transition probability matrix in the current state.

4. The method for multimodal data fusion of visual positioning according to claim 3, characterized in that: Step S33 also includes: S34 determines the action in the current situation through a reward and penalty mechanism. If there is an obstacle on the right side, if it is estimated that the output posture information of the next state is to move to the right for collision, a penalty process will be performed, so that the mobile robot will not consider the posture information that has been punished in the next posture information estimation process; if it is estimated that the output posture information of the next state is to move to the left for obstacle avoidance, a reward process will be performed, so that the mobile robot will consider the posture information that has been rewarded in the next posture information estimation process.

5. The method for multimodal data fusion of visual positioning according to claim 3, characterized in that: Each state includes: the center point, size and category information of the target box; the depth information of the image information; the current coordinate information, current speed information and current posture information.

Images