Visual feature constrained inertia / geomagnetic positioning method

Abstract

The invention relates to a visual feature constrained inertial / geomagnetic positioning method, and belongs to the technical field of navigation positioning. The method has the advantage that the method does not depend on collection, updating and maintenance of a high-precision map. A traditional image matching method usually needs to consume a large amount of time and resources to construct and update an accurate indoor map, but according to the method, by deploying a small number of feature markers with known coordinates in an indoor environment, the map construction and maintenance process can be greatly simplified, and the deployment cost of a system is reduced. Meanwhile, real-time updating of the multi-source sensor is combined, and pose estimation can be dynamically adjusted according to actual movement and environmental changes, so that higher precision and reliability are provided in actual use.

Description

Technical Field

[0001] The present invention relates to an inertial / geomagnetic positioning method constrained by visual features, belonging to the technical field of navigation and positioning. Background Art

[0002] Currently in the field of indoor positioning, although wireless signal technologies such as WiFi, Bluetooth, ZigBee, and ultra-wideband are widely used, each has its own deficiencies. Visual technology, especially vision-based positioning using cameras, is gradually becoming an important supplement to indoor positioning, with great potential. The indoor positioning technology based on the fusion of multi-source inertial sensors is the main direction of indoor positioning research. The inertial / magnetic field indoor positioning and navigation technology is a technology that uses the magnetic field information sensed by a magnetometer and the inertial information sensed by a MIMU for autonomous navigation, including two methods: magnetic heading correction and magnetic field matching positioning. The inertial / visual indoor positioning and navigation technology is a technology that uses the environmental information obtained by a camera and the inertial information sensed by a MIMU for autonomous navigation. From the perspective of the use method of visual information, it can be divided into three categories: indoor positioning and navigation technology based on image matching, vanishing point pose / inertial fusion technology, and image feature tracking / inertial fusion technology.

[0003] The inertial / magnetic field indoor positioning and navigation technology mainly includes three parts: magnetic map construction, magnetic field matching positioning, and magnetic field matching / inertial fusion. Among them, magnetic field matching is to compare and match the currently obtained magnetic field information with the previously collected magnetic field characteristics (magnetic map) in the given indication area to determine the position of the pedestrian. The indoor positioning and navigation technology of magnetic field matching / inertial fusion is to fuse the position calculated by the integrated navigation system with the position of magnetic field matching on the basis of magnetic field matching to obtain a more accurate position. The indoor positioning and navigation technology based on image matching uses the images captured by a camera to match the images in the pre-constructed map database to determine the position. The vanishing point pose / inertial fusion technology and the image feature tracking / inertial fusion technology do not require prior map information, directly estimate the human body pose from continuous image sequences, and combine with the inertial navigation system to obtain more accurate pose information.

[0004] Although some progress has been made in existing indoor positioning and navigation technologies, there are still several significant drawbacks. First, many existing technologies rely on high-precision sensors, which results in high costs and limits their popularity in large-scale applications, especially in low-cost consumer goods. Second, when facing environmental interference (such as electrical equipment, metal structures, etc.), the positioning accuracy of magnetic field matching and magnetic field matching / inertial fusion technologies will be affected. Especially in the case of insufficient or unstable magnetic field information, the positioning results are often inaccurate. In addition, indoor positioning technologies based on image matching require a high-precision map database, and the acquisition, update, and maintenance of maps are complex and resource-intensive tasks, which are particularly difficult in dynamic environments. Therefore, existing technologies have obvious limitations in low-cost applications, providing long-term stable accuracy, etc. Summary of the Invention

[0005] The technical problem to be solved by this invention is: to overcome the deficiencies of the prior art and propose a visual feature-constrained inertial / geomagnetic positioning method.

[0006] The technical solution of this invention is:

[0007] A visual feature-constrained inertial / geomagnetic positioning method, the steps of which include:

[0008] In the first step, the acceleration information and angular acceleration information of the target to be measured are obtained through an IMU sensor;

[0009] Integrate the velocity of the target to be measured according to the obtained acceleration information and angular acceleration information of the target to be measured to obtain a velocity integration result;

[0010] Integrate the rotation angle of the target to be measured according to the obtained acceleration information and angular acceleration information of the target to be measured to obtain a rotation angle integration result;

[0011] Then, perform position integration on the target to be measured according to the initial pose, velocity integration result, and rotation angle integration result of the target to be measured to obtain a position integration result, and finally obtain the pose 1 of the target to be measured including displacement and heading angle according to the position integration result;

[0012] In the second step, the environmental image is collected in real time through a binocular vision camera, the feature points in the collected environmental image are identified, and then the depth of the identified feature points and the coordinates of the feature points on the pixel plane are calculated;

[0013] In the third step, according to the depth of the feature points at the current moment, the depth of the feature points at the previous moment, and the coordinates of the feature points at the previous moment on the pixel plane calculated in the second step, solve the displacement and rotation angle of the feature points at the current moment, and then estimate the displacement of the target to be measured according to the displacement and rotation angle of the feature points at the current moment obtained by the solution;

[0014] In the fourth step, the triaxial magnetic field information of the geomagnetic sensor is resolved to obtain the heading information of the target to be measured;

[0015] In the fifth step, based on the pose 1 of the target to be measured obtained in the first step, the pose 2 of the target to be measured obtained in the third step, and the heading information of the target to be measured obtained in the fourth step, the state equation and the observation equation of the target to be measured are constructed, and the fused pose of the target to be measured is obtained according to the constructed state equation and observation equation of the target to be measured;

[0016] In the sixth step, after identifying the feature object with known coordinates using the binocular vision camera, the pixel coordinates of the feature object with known coordinates are read, and based on the internal parameter matrix of the binocular camera, the pixel coordinates of the feature object with known coordinates are normalized to obtain the normalized coordinates of the feature object with known coordinates;

[0017] In the seventh step, the position of the target to be measured is resolved according to the fused pose of the target to be measured obtained in the fifth step and the normalized coordinates of the feature object with known coordinates obtained in the sixth step.

[0018] The position of the target to be measured obtained in the seventh step and the rotation angle in the fused pose of the target to be measured are used as the initial pose of the target to be measured at the next moment, and the position of the target to be measured at the next moment is further obtained.

[0019] In the first step, the initial pose of the target to be measured includes the initial position coordinates, the initial velocity, the initial attitude, and the initial heading angle;

[0020] In the second step, the method for calculating the depth of the feature point is as follows: the depth of the feature point is calculated according to the parallax d of the same feature point (the difference in the positions of the corresponding feature points in the left and right images) and the parameters of the camera (the focal length f and the baseline B):

[0021]

[0022] The parallax d of the same feature point refers to the difference in the positions of the corresponding feature points in the left and right images;

[0023] The parameters of the camera include the focal length f and the baseline B;

[0024] In the fifth step, the state equation of the target to be measured is:

[0025] x pred (t + 1) = F·x(t) + B·u(t)

[0026] where F is the state transition matrix, which describes how the state is updated over time, B is the control input matrix, the control input u(t) is the acceleration and angular velocity of the IMU at time t; t is the time, and x(t) is the initial pose at time t;

[0027] The observation equation of the target to be measured is:

[0028] Z(t + 1) = H·x(t) + v

[0029]

[0030] Among them, z cam is the observation value from the vision sensor, including displacement and rotation, H cam is the observation matrix of the vision sensor, v cam is the binocular observation noise. Among them, z mag is the heading observation value from the geomagnetic sensor, H mag is the observation matrix of the geomagnetic sensor, v mag is the observation noise of the geomagnetic sensor;

[0031] In the sixth step, the normalized coordinates of the known coordinate feature objects obtained are:

[0032] x camera = K -1 ·p image

[0033] Among them, the image coordinate p image = [u, v] T is the pixel coordinate of the feature object in the image plane;

[0034] In the seventh step, the specific method for solving the position P of the target to be measured is: camera The solution is as follows:

[0035] p camera = R fusion -1 (X feature - T)

[0036] Among them, R fusion is the matrix describing the rotation between the camera coordinate system and the world coordinate system, X feature = [X, Y, Z] T is the position of the feature object in the world coordinate system, and T is the translation matrix of the origin of the camera coordinate system relative to the origin of the world coordinate system.

[0037] Beneficial effects

[0038] By introducing visual feature constraints, combining a small number of feature markers with known coordinates in the indoor environment, and using the fusion of low-cost inertial navigation, vision, and geomagnetic sensors, this method provides an efficient and economical positioning solution. Specifically, the system uses the joint information of a binocular camera, an inertial sensor, and a geomagnetic sensor to achieve attitude estimation. With the assistance of the coordinate information of the known feature markers and the calibration data, the binocular camera can provide key visual information to help the system perform real-time positioning and attitude estimation in a specific area.

[0039] During the positioning process, by analyzing the images and pose information near the calibrated visual feature points, the system can further calculate and calibrate the current position. This method makes full use of the image data captured by the binocular camera, as well as the motion trajectory provided by the inertial sensor and the direction information provided by the geomagnetic sensor, making the pose estimation more accurate. In practical applications, with the continuous progress of iterative calculations, the positioning accuracy is continuously improved. Especially around the calibration points or in dynamic environments, it can effectively reduce the positioning deviation caused by the errors of single sensors.

[0040] Compared with traditional image matching methods, the advantage of this method is that it does not need to rely on the acquisition, update and maintenance of high-precision maps. Traditional image matching methods usually require a lot of time and resources to construct and update accurate indoor maps, while this method can greatly simplify the map construction and maintenance process by deploying a small number of feature markers with known coordinates in the indoor environment, reducing the deployment cost of the system. At the same time, combined with the real-time update of multi-source sensors, it can dynamically adjust the pose estimation according to the actual movement and environmental changes, thus providing higher accuracy and reliability in practical use. By fusing binocular vision and geomagnetic information, the accuracy and anti-interference ability of the inertial navigation position and direction estimation in passive positioning are improved. Using a small number of visual features to constrain the positioning results avoids the complex technologies and high costs required for the construction and maintenance of high-precision visual maps and geomagnetic maps. The finally calculated camera position can not be directly brought into the state vector, and can be fused with the results of inertial navigation and when there are no feature objects by using Kalman filter. Specific implementation mode

[0041] The present invention will be further described below in conjunction with embodiments.

[0042] This method mainly includes two key links: fused pose calculation and visual feature constraint correction. Among them, the fused pose calculation link is based on inertial navigation, binocular vision, and geomagnetic sensors, and is mainly used to provide heading information and position estimation values for the visual feature constraint link. The visual feature constraint correction link is based on the heading information and the coordinates of known feature points, and uses the image data collected by the binocular vision camera for calculation, so as to obtain a high-precision self-positioning result.

[0043] Assume that the initial position of the target to be measured at 0s is:

[0044] (-2169274.44025033, 4388528.47618876, 4074892.00495392) m;

[0045] The initial velocity is 0 m / s, and the initial heading angle is 164.257197925°.

[0046] The accelerations in the x and y directions of the IMU sensor with a sampling frequency of 100HZ for the target to be measured are both 4m / s within a sampling period (0.01s). 2 The acceleration in the z direction is 1m / s 2 At 0.01s, the single-step integration results of the accelerations in the x, y, and z directions of the target to be measured are (0.04, 0.04, 0.01)m / s, which are the velocities of the target to be measured at this moment. The heading angular acceleration of the target to be measured is measured as 1rad / s 2 The single-step integration result of the heading angular acceleration of the target to be measured is 0.01° / s, which is the angular velocity of the target to be measured at this moment. Furthermore, the single-step displacement integration result of the target to be measured is (0.0002, 0.0002, 0.0005)m, and the single-step integration result of the rotation angle is 0.00005°. Then, the pose 1 includes the position (-2169274.44005033, 4388528.47638876, 4074892.00545392) and the heading angle 164.257248000°.

[0047] In the second step, on a pair of images collected by the binocular camera, the coordinates of a feature point are identified. The left eye is (796, 356), and the right eye is (736, 364).

[0048] In the third step, combining the images collected by the binocular camera at the previous moment, the displacement of the target to be measured is estimated to be 0.00057456m.

[0049] In the fourth step, the triaxial magnetic field information of the geomagnetic sensor is resolved to obtain the heading angle of the target to be measured as 164.257247925.

[0050] In the fifth step, an observation equation is constructed, and the Kalman filter is used to obtain the fused pose of the target to be measured, including the position coordinates (-2169274.44001033, 4388528.47628376, 4074892.00531392).

[0051] In the sixth step, the pixel coordinates of the feature object are extracted from the images captured by the binocular camera, and combined with the external parameter matrix and rotation matrix of the camera, the pixel coordinates are normalized according to the following formula.

[0052]

[0053] The normalized coordinates of the feature object are obtained as: (184.39941234, 7.23689476, 1263.0155449). In this case, the calibration parameters of the binocular camera are as follows:

[0054]

[0055] In the seventh step, combining the normalized coordinates of the feature object, the position of the target to be measured after visual correction is calculated as (-2169274.44003033, 4388528.47628876, 4074892.00535392), and the heading angle is: 164.257247925.

[0056] An inertial / geomagnetic positioning method constrained by visual features, the steps of which include:

[0057] In the first step, the acceleration information and angular acceleration information of the target to be measured are obtained through the IMU sensor;

[0058] Integrate the velocity of the target to be measured according to the obtained acceleration information and angular acceleration information of the target to be measured to obtain the velocity integration result;

[0059] Integrate the rotation angle of the target to be measured according to the obtained acceleration information and angular acceleration information of the target to be measured to obtain the rotation angle integration result;

[0060] Then, perform position integration on the target to be measured according to the initial pose, velocity integration result, and rotation angle integration result of the target to be measured to obtain the position integration result, and finally obtain the pose 1 of the target to be measured including displacement and rotation angle according to the position integration result;

[0061] In the second step, the environmental images are collected in real time through the binocular vision camera, the feature points in the collected environmental images are identified, and then the depth of the identified feature points and the coordinates of the feature points on the pixel plane are calculated;

[0062] In the third step, according to the depth of the feature points at the current moment, the depth of the feature points at the previous moment, and the coordinates of the feature points at the previous moment on the pixel plane calculated in the second step, solve the displacement and rotation angle of the feature points at the current moment, and then estimate the pose 2 of the target to be measured including displacement and rotation angle according to the displacement and rotation angle of the feature points at the current moment obtained by the solution;

[0063] In the fourth step, solve the three-axis magnetic field information of the geomagnetic sensor to obtain the heading information of the target to be measured;

[0064] In the fifth step, construct the state equation and observation equation of the target to be measured according to the pose 1 of the target to be measured obtained in the first step, the pose 2 of the target to be measured obtained in the third step, and the heading information of the target to be measured obtained in the fourth step, and obtain the fusion pose of the target to be measured according to the constructed state equation and observation equation of the target to be measured;

[0065] In the sixth step, after using the binocular vision camera to identify the feature object with known coordinates, read the pixel coordinates of the feature object with known coordinates, and normalize the pixel coordinates of the feature object with known coordinates according to the internal parameter matrix of the binocular camera to obtain the normalized coordinates of the feature object with known coordinates;

[0066] In the seventh step, calculate the position of the target to be measured according to the fused pose of the target to be measured obtained in the fifth step and the normalized coordinates of the known coordinate feature obtained in the sixth step.

[0067] Take the position of the target to be measured obtained in the seventh step and the rotation angle in the fused pose of the target to be measured as the initial pose of the target to be measured at the next moment, and further obtain the position of the target to be measured at the next moment.

[0068] In the first step, the initial pose of the target to be measured includes the initial position coordinates, initial velocity, initial attitude, and initial heading angle;

[0069] In the second step, the method for calculating the depth of the feature point is as follows: Calculate the depth of the feature point according to the parallax d of the same feature point (the difference in the positions of the corresponding feature points in the left and right images) and the parameters of the camera (focal length f and baseline B):

[0070]

[0071] The parallax d of the same feature point refers to the difference in the positions of the corresponding feature points in the left and right images;

[0072] The parameters of the camera include the focal length f and the baseline B;

[0073] In the fifth step, the state equation of the target to be measured is:

[0074] x pred (t + 1) = F·x(t) + B·u(t)

[0075] Where F is the state transition matrix, which describes how the state is updated over time, B is the control input matrix, the control input u(t) is the acceleration and angular velocity of the IMU at time t; t is the time, and x(t) is the initial pose at time t;

[0076] The observation equation of the target to be measured is:

[0077] Z(t + 1) = H·x(t) + v

[0078]

[0079] Where z cam is the observation value from the vision sensor, including displacement and rotation, H cam is the observation matrix of the vision sensor, v cam is the binocular observation noise, where z mag is the heading observation value from the geomagnetic sensor, H mag is the observation matrix of the geomagnetic sensor, v mag is the observation noise of the geomagnetic sensor;

[0080] In the sixth step, the normalized coordinates of the known coordinate feature objects obtained are as follows:

[0081] x camera = K -1 ·p image

[0082] where the image coordinate p image = [u, v] T is the pixel coordinate of the feature object in the image plane;

[0083] In the seventh step, the specific method for solving the position p of the target to be measured is as follows: camera The specific calculation method is:

[0084] p camera = R fusion -1 (X feature - T)

[0085] where R fusion is the matrix describing the rotation between the camera coordinate system and the world coordinate system, X feature = [X, Y, Z] T is the position of the feature object in the world coordinate system, and T is the translation matrix of the origin of the camera coordinate system relative to the origin of the world coordinate system.

[0086] In summary, the above is only the preferred embodiment of the present invention, and is not used to limit the protection scope of the present invention. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A visual feature-constrained inertial / geomagnetic positioning method, characterized in that The steps of the method include: In the first step, acceleration information and angular acceleration information of the target to be measured are obtained through an IMU sensor; Integrate the velocity of the target to be measured based on the obtained acceleration information and angular acceleration information of the target to be measured to obtain a velocity integration result; Integrate the rotation angle of the target to be measured based on the obtained acceleration information and angular acceleration information of the target to be measured to obtain a rotation angle integration result; Then, perform position integration on the target to be measured based on the initial pose, velocity integration result, and rotation angle integration result of the target to be measured to obtain a position integration result. Finally, obtain the pose 1 of the target to be measured including displacement and rotation angle based on the position integration result; In the second step, the binocular vision camera is used to collect environmental images in real time, identify the feature points in the collected environmental images, and then calculate the depth of the identified feature points and the coordinates of the feature points on the pixel plane; In the third step, based on the depth of the feature points at the current moment, the depth of the feature points at the previous moment, and the coordinates of the feature points on the pixel plane at the previous moment calculated and identified in the second step, solve the displacement and rotation angle of the feature points at the current moment, and then estimate the pose 2 of the target to be measured including displacement and rotation angle based on the displacement and rotation angle of the feature points at the current moment obtained by the solution; In the fourth step, solve the triaxial magnetic field information of the geomagnetic sensor to obtain the heading information of the target to be measured; In the fifth step, construct the state equation and observation equation of the target to be measured based on the pose 1 of the target to be measured obtained in the first step, the pose 2 of the target to be measured obtained in the third step, and the heading information of the target to be measured obtained in the fourth step, and obtain the fused pose of the target to be measured based on the constructed state equation and observation equation of the target to be measured; In the sixth step, after using the binocular vision camera to identify the feature object with known coordinates, read the pixel coordinates of the feature object with known coordinates, and normalize the pixel coordinates of the feature object with known coordinates based on the internal parameter matrix of the binocular camera to obtain the normalized coordinates of the feature object with known coordinates; In the seventh step, solve the position of the target to be measured based on the fused pose of the target to be measured obtained in the fifth step and the normalized coordinates of the feature object with known coordinates obtained in the sixth step.

2. A vision feature-constrained inertial / geomagnetic positioning method according to claim 1, characterized in that: The position of the target to be measured obtained in the seventh step and the rotation angle in the fused pose of the target to be measured are used as the initial pose of the target to be measured at the next moment, and the position of the target to be measured at the next moment is further obtained.

3. A vision feature-constrained inertial / geomagnetic positioning method according to claim 1, characterized in that: In the first step, the initial pose of the target to be measured includes initial position coordinates, initial velocity, initial attitude, and initial heading angle.

4. A vision feature-constrained inertial / geomagnetic positioning method according to claim 1, characterized in that: In the second step, the method for calculating the depth of the feature points is: calculate the depth of the feature points according to the parallax d of the same feature point and the parameters of the camera: The parallax d of the same feature point refers to the difference in the positions of the corresponding feature points in the left and right images; The parameters of the camera include the focal length f and the baseline B.

5. A visual feature-constrained inertial / geomagnetic positioning method according to claim 1, wherein: In the fifth step, the state equation of the target to be measured is: x pred (t + 1)=F·x(t)+B·u(t) Where F is the state transition matrix, which describes how the state is updated over time, B is the control input matrix, and the control input u(t) is the acceleration and angular velocity of the IMU at time t; t is the time, and x(t) is the initial pose at time t.

6. A visual feature-constrained inertial / geomagnetic positioning method according to claim 1, wherein: The observation equation of the target to be measured is: Z(t + 1) = H·x(t) + v where z cam is the observation value from the vision sensor, including displacement and rotation, H cam is the observation matrix of the vision sensor, v cam is the binocular observation noise, where z mag is the heading observation value from the geomagnetic sensor, H mag is the observation matrix of the geomagnetic sensor, v mag is the observation noise of the geomagnetic sensor.

7. A visual feature-constrained inertial / geomagnetic positioning method according to claim 1, wherein: In the sixth step, the normalized coordinates of the known coordinate feature are: x camera = K -1 ·p image Among them, the image coordinate p image = [u, v] T is the pixel coordinate of the feature object in the image plane.

8. A visual feature-constrained inertial / geomagnetic positioning method according to claim 1, wherein: In the seventh step, the position p of the target to be measured camera The specific solution method is as follows: p camera = R fusion -1 (X feature - T) where, R fusion is the matrix describing the rotation between the camera coordinate system and the world coordinate system, X feature = [X, Y, Z] T is the position of the feature object in the world coordinate system, and T is the translation matrix of the origin of the camera coordinate system relative to the origin of the world coordinate system.

Images