SLAM-IMU coupling-based real-time positioning method under GPS signal blind area

Through SLAM-IMU coupling technology, the use of binocular cameras and IMU components to realize real-time positioning of workers in the blind spot of GPS signal, solving the problem of positioning accuracy and stability, and providing higher operability and safety guarantees.

CN118913265AActive Publication Date: 2024-11-08HUAQIAO UNIVERSITY
View PDF 6 Cites 0 Cited by

Patent Information

Application Number
CN202411119363.7
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-08-15
Publication Date
2024-11-08
Estimated Expiration
2044-08-15

AI Technical Summary

Technical Problem

The existing positioning technology has low positioning accuracy and stability in environments with severe occlusion and strong signal interference, especially in the blind spot of GPS signal, which is difficult to achieve accurate positioning of workers.

Method used

Using the positioning method based on SLAM-IMU coupling, a binocular camera and IMU element is used to establish a nonlinear objective function through image sequence, angular velocity and acceleration, and the real-time position and movement trajectory of workers are obtained through iterative optimization, combined with Shi-Tomasi corner point detection, Interpolation and optical flow method optimize visual information, pre-integrated IMU motion model, and reduce calculation amount and time.

Benefits of technology

Improve the integrity, accuracy and stability of positioning, reduce cumulative errors, provide low-cost and easy deployment operability, effectively solve the problem of inaccurate positioning under signal blind spots, and improve the safety management efficiency of construction sites. .

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN118913265A_ABST
    Figure CN118913265A_ABST
Patent Text Reader

Abstract

The invention discloses a GPS signal blind area real-time positioning method based on SLAM-IMU coupling, based on a positioning device comprising a binocular camera provided with an SLAM sensor and an IMU element, the positioning method comprises the following steps: the binocular camera provided with the SLAM sensor transmits captured image sequences at different discrete time points to a computer; the IMU element transmits the collected real-time angular velocity and acceleration to the computer; and the computer combines the image sequence, the real-time angular velocity and the acceleration to establish a nonlinear objective function containing a plurality of optimization variables, and the optimization variables are enabled to reach a globally optimal solution through multiple iterations, so that a real-time position and an action track are obtained. According to the method, the IMU data subjected to pre-integration processing is coupled through the visual SLAM sensor, iterative optimization is carried out, a real-time positioning algorithm based on multi-source information coupling is constructed, and the problem of self-positioning of workers in a GPS signal blind area is solved.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention relates to the field of SLAM positioning technology, and in particular to a real-time positioning method in a GPS signal blind area based on SLAM-IMU coupling. Background Art

[0002] The existing positioning technology mainly relies on the signals emitted by the positioning tags worn by workers. These signals are received and processed by intelligently deployed positioning base stations to obtain the location information of the workers, so as to accurately locate and manage the workers. The implementation basis of this technology is the signal transmission between the external fixed base station and the mobile target on site. However, it is worth noting that this positioning technology is highly sensitive to environments with severe occlusion and strong signal interference, which affects its positioning accuracy and stability to a certain extent.

[0003] SLAM refers to a method of achieving self-positioning in an unknown environment and building an environmental map at the same time. The term SLAM first appeared in the field of robotics. Its main function is to simultaneously estimate the posture of the device and the three-dimensional structure of the environment in an unknown environment. SLAM is a technology that uses visual information or laser and sensor data to achieve simultaneous positioning and map construction. Visual SLAM uses cameras to complete the perception of the environment. In an unknown environment, autonomously moving people or robots use the information data captured by the camera to make a preliminary estimate of their own position and their surrounding environment. As the robot moves, it accurately locates itself based on the real-time position estimate and the data perceived by the camera, and continuously builds and updates the map in the process, while reasonably planning the path of travel. It is widely used in 3D modeling, virtual reality (VR), robotics, autonomous driving and other fields. The successful application of these fields has made it possible for workers to accurately locate in scenarios where satellite triangulation positioning is invalid. It only needs to wear a visual camera on the personnel to determine the current position through changes in site information, which can be used in scenarios such as poor satellite positioning. The Chinese invention application with publication number CN114136311A discloses a laser SLAM positioning method based on IMU pre-integration, which combines IMU and laser SLAM to achieve pose solution. However, laser SLAM lacks the ability of loop detection, and it is difficult to eliminate the accumulated error. Summary of the invention

[0004] The purpose of the present invention is to solve the problems in the prior art.

[0005] The technical solution adopted by the present invention to solve the technical problem is: to provide a real-time positioning method in a GPS signal blind area based on SLAM-IMU coupling, based on a positioning device including a binocular camera with a SLAM sensor and an IMU element, the positioning method includes the following steps:

[0006] The binocular camera transmits the image sequences captured at different discrete time points to the computer;

[0007] The IMU element transmits the collected real-time angular velocity and acceleration to the computer;

[0008] The computer combines the image sequence, real-time angular velocity and acceleration to establish a nonlinear objective function containing multiple optimization variables. Through multiple iterations, the optimization variables are made to reach the global optimal solution, thereby obtaining the real-time position and movement trajectory.

[0009] Preferably, the process of the computer obtaining the real-time position and movement trajectory of the staff member comprises the following steps:

[0010] Performing corner point extraction, corner point optimization and corner point matching on the collected image sequence to establish visual information; the corner points refer to significant feature points;

[0011] According to the angular velocity and acceleration Construct an IMU motion model and pre-integrate the IMU motion model to obtain a simplified IMU motion model;

[0012] Combining visual information with a simplified IMU motion model to establish a nonlinear objective function containing multiple optimization variables;

[0013] The initial iteration values ​​of the optimization variables are determined based on the approximate values ​​measured by the sensors, and the global optimal solution is obtained through multiple iterations to obtain the real-time position and movement trajectory of the staff.

[0014] Preferably, the process of establishing visual information by the computer comprises the following steps:

[0015] Corner point extraction: Shi-Tomasi algorithm is used to extract corner points, and the quality of feature points is improved by selecting the best corner points;

[0016] Corner optimization uses interpolation method to detect sub-pixel corners. It performs linear interpolation along the edge of the object to avoid sudden changes in pixel values.

[0017] Corner matching uses the optical flow method to track and match corner points, uses the pixel brightness changes between adjacent frames to calculate the camera's displacement and rotation, and estimates the camera's motion in real time, thereby providing positioning information during the map construction process.

[0018] Preferably, the interpolation method uses linear interpolation to solve the pixel grayscale value, combining the known pixel grayscale values ​​around the desired point with two straight lines of known quantities, thereby finding the pixel grayscale value of the desired point.

[0019] Preferably, for the desired point P(x, y), the pixel grayscale values ​​of four points around P(x, y) are known, including the pixel grayscale values ​​of A(x1, y2), B(x2, y2), C(x1, y1) and D(x2, y1), y2>y>y1, x2>x>x1; the process of calculating the pixel grayscale value of P(x, y) includes:

[0020] Insert pixel points F = (x, y1) and E = (x, y2) between AB and CD respectively;

[0021] The pixel value of point P is calculated based on the pixel values ​​of E and F, expressed as:

[0022]

[0023] Where f(·) represents the pixel value.

[0024] Preferably, the corner point matching uses an optical flow method to track and match the corner points. When the m spatial points in the image move consistently in a continuous time in an m×m window, it is expressed as:

[0025]

[0026] Wherein, μ represents the velocity vector of the pixel along the x-axis, v represents the velocity vector of the pixel along the y-axis; F x express F y express F t express F is expressed as F(x,y,t), which refers to the pixel grayscale value of the point at coordinates (x,y) at time t.

[0027] Preferably, the IMU motion model is expressed as:

[0028]

[0029] in, represents the position at time j, represents the speed at time j, represents the posture at time j; represents the position at time i, represents the speed at time i, Δt represents the time interval from i to j, represents the posture at time t; Represents the actual value of acceleration at time t, g w represents the gravity vector in the global coordinate system, δ t Indicates a short time interval; Represents the actual value of the angular velocity at time t; Represents four-element multiplication.

[0030] Preferably, the pre-integration of the IMU motion model is specifically performed by aligning the IMU data of the first frame with the image of the a-th frame, and then aligning the IMU data of the last frame with the image of the b-th frame, and finally integrating the data to obtain a simplified IMU motion model represented as:

[0031]

[0032] in, Indicates the position of the IMU coordinate system relative to the world coordinate system at the k+1th moment, Indicates the velocity of the IMU coordinate system relative to the world coordinate system at the k+1th moment, It represents the posture from the IMU coordinate system to the world coordinate system at the k+1th moment; They represent the position, velocity, and attitude of the IMU coordinate system relative to the world coordinate system at the kth moment, Δt k represents the time interval from k to k+1, represents the accelerometer bias at time t, Indicates that it is biased by the gyroscope at time t; Indicates the accelerometer measurement value under the Body system, Indicates the gyroscope measurement value in the Body frame, b g and b a It refers to the zero bias of the gyroscope and the zero bias of the accelerometer. It is the rotation matrix of the IMU's Body coordinate system relative to the world coordinate system.

[0033] Preferably, the method of establishing a nonlinear objective function by combining visual information and a simplified IMU motion model comprises the following steps:

[0034] Construct a state vector, including sensor state, external parameters and inverse depth of spatial landmarks. The state vector X is expressed as:

[0035]

[0036] Among them, x0,x1,…,x n Represents the sensor state, including the sensor's monitoring parameters (coordinates, rate, rotation angle), accelerometer bias, and gyroscope readings, where the state quantity x of the kth frame is k Include translation Speed Rotation b a represents the bias of the accelerometer in the IMU, b g Indicates the deviation of the gyroscope in the IMU; λ0,λ1,…,λ mIndicates the inverse depth of the spatial landmark, which is the reciprocal of the landmark point depth; Represents the external parameters of the camera and IMU, including the translation and rotation relationship between the camera and IMU;

[0037] Constraints are imposed on the IMU model, mainly the changes in displacement, velocity, rotation, acceleration deviation and gyroscope deviation between two frames, and the IMU measurement residual is expressed as:

[0038]

[0039] Among them, δa is the position pre-integration error, δθ is the angle pre-integration error, δβ is the velocity pre-integration error, and δb a is the acceleration bias error, δb g is the gyroscope bias error; where, represents four-element multiplication, [·] xyz is a three-dimensional vector that takes only the imaginary part (x, y, z) of the quaternion; Represents the rotation matrix of the IMU body coordinate system from the kth to the k+1th frame, The position of the IMU coordinate system relative to the world coordinate system at the k+1th moment, The position of the IMU coordinate system relative to the world coordinate system at the kth moment, The velocity of the IMU coordinate system relative to the world coordinate system at the kth moment, The acceleration in the IMU body coordinate system from the kth to k+1th frame, Indicates that from frame b k To frame b k+1 The rotation in quaternion form in the IMU body coordinate system, The inverse of the rotation quaternion from the world coordinate system to the IMU body coordinate system at the kth moment, The rotation quaternion (attitude) converted from the body coordinate system to the world coordinate system at time k+1, Represents the rotation matrix from the IMU body coordinate system to the world coordinate system at the kth moment, Indicates the velocity of the IMU coordinate system relative to the world coordinate system at the k+1th moment, The deviation from the IMU body coordinate system at the kth moment to the IMU body coordinate system at the k+1th moment, Indicates the acceleration deviation of the IMU body coordinate system at the k+1th moment, represents the acceleration error of the IMU body coordinate system at the kth moment, Indicates the angular velocity deviation of the IMU body coordinate system at the k+1th moment, Angular velocity deviation of the IMU body coordinate system at the kth moment;

[0040] Constraints are imposed on the visual information to obtain the ideal sensor posture by minimizing the visual residual caused by pixel reprojection. The pixel coordinate value obtained by the first projection of the first landmark point in the i-th camera coordinate system is transformed and mapped to the j-th coordinate system. The difference between the transformed pixel coordinate value and the directly observed corresponding pixel coordinate value constitutes the visual residual, which is expressed as:

[0041]

[0042] in, Projection to C j The x value in the frame camera coordinate system, Projection to C j The y value in the frame camera coordinate system, Projection to C j The z value in the frame camera coordinate system, Represents the inverse homogeneous transformation matrix from the IMU coordinate system back to the camera coordinate system,

[0043] represents the inverse matrix of the homogeneous transformation matrix from the world coordinate system to the jth IMU coordinate system, represents the inverse matrix of the homogeneous transformation matrix from the world coordinate system to the jth IMU coordinate system, T bc Represents the homogeneous transformation matrix from the camera coordinate system to the IMU coordinate system; the feature point is in the Cth i The pixel coordinates on the frame are λ represents the inverse depth;

[0044] Taking into account the prior information, visual data and IMU constraint information, the optimal state vector of the integrated multi-sensor information is obtained through nonlinear optimization method, which is expressed as:

[0045]

[0046] Among them, ||r p -J p X|| 2 is the marginalized prior information, is the IMU measurement residual, is the visual residual, represents the noise covariance of IMU pre-integration, represents the noise covariance generated by vision.

[0047] Preferably, the optimization variables are optimized to reach a global optimal solution through multiple iterations, and the optimization variables include the IMU measurement residual Visual residual Noise covariance of IMU pre-integration and the visual noise covariance Initial iteration values ​​of the optimization variables are determined based on approximate values ​​of sensor measurements.

[0048] The present invention has the following beneficial effects:

[0049] (1) The three-dimensional graphics of the site constructed by binocular vision SLAM are optimized through the Shi-Tomasi corner detection algorithm, interpolation method and optical flow tracking algorithm, and the SLAM and IMU coupling positioning algorithm are used to further improve the integrity, accuracy and stability of the SLAM vision method and provide scale information, thereby realizing the real-time positioning of on-site workers; among them, the corner optimization uses the interpolation method to perform sub-pixel corner detection. By performing linear interpolation along the edge of the object, the value mutation caused by the pixel value mutation can be avoided, so that the pixel grayscale value of the point finally obtained is more accurate;

[0050] (2) Compared with laser SLAM positioning technology, the visual SLAM used in the present invention uses a large amount of redundant texture information, and loop detection is relatively easy. Even if a certain error is accumulated at the front end, the error can still be eliminated through loop correction. In addition, the price of laser radar is much higher than that of visual sensor, and the deployment cost is high, so it is not very operable in practical applications. In comparison, the visual SLAM used in the present invention has the advantages of low cost, easy deployment, high degree of visualization, etc., and is more operable in practical applications;

[0051] (3) Compared with adding IMU residual terms, point features and line feature residual terms to BA joint optimization pose calculation, the present invention adopts a sliding window method to optimize the calculation process. At the same time, through the Schur complement method, the constraints of the frame sliding out of the window and the frame in the window are converted into prior error information, and coupled to the objective function of coupled positioning, so as to reduce the calculation amount and calculation time of the coupling algorithm;

[0052] (4) The present invention combines IMU and laser SLAM to achieve real-time positioning of personnel, effectively solving the problem of inaccurate positioning caused by signal blind spots in existing positioning management systems, helping to protect the lives of workers on the construction site, improving the management efficiency of construction site supervisors, and providing favorable guarantees for the safety and stability of the entire construction process.

[0053] The present invention is further described in detail below in conjunction with the accompanying drawings and embodiments, but the present invention is not limited to the embodiments. BRIEF DESCRIPTION OF THE DRAWINGS

[0054] Figure 1 A method step diagram of an embodiment of the present invention;

[0055] Figure 2 It is a schematic diagram of a flow chart of an embodiment of the present invention;

[0056] Figure 3A schematic diagram of optimizing corner points using an interpolation method according to an embodiment of the present invention;

[0057] Figure 4 A schematic diagram of real-time positioning of a person's position and movement trajectory according to an embodiment of the present invention. DETAILED DESCRIPTION

[0058] See also Figure 1 and Figure 2 The method step diagram and flow chart of an embodiment of the present invention are shown, which include the following steps:

[0059] S101, installing a positioning device including a binocular camera with a SLAM sensor and an IMU element on a construction worker;

[0060] S102, the binocular camera equipped with the SLAM sensor transmits the captured image sequences at different discrete time points to the computer, and the IMU element transmits the collected real-time angular velocity and acceleration to the computer;

[0061] S103, the computer combines the collected image sequence, real-time angular velocity and acceleration to establish a nonlinear objective function containing multiple optimization variables, and through multiple iterations, the optimization variables are made to reach a global optimal solution, thereby obtaining the real-time position and movement trajectory of the construction personnel.

[0062] Specifically, S103 includes the following steps:

[0063] S1031, performing corner point extraction, corner point optimization and corner point matching on the collected image sequence to establish visual information; the corner points refer to significant feature points;

[0064] S1032, according to the angular velocity and acceleration Construct an IMU motion model and pre-integrate the IMU motion model to obtain a simplified IMU motion model;

[0065] S1033, combining visual information and a simplified IMU motion model to establish a nonlinear objective function including multiple optimization variables;

[0066] S1034, determining the initial iteration value of the optimization variable based on the approximate value measured by the sensor, obtaining the global optimal solution through multiple iterations, and obtaining the real-time position and movement trajectory of the staff.

[0067] Specifically, S1031 extracts corner points (significant feature points), optimizes corner points, and matches corner points on the captured image, and reversely infers the motion trajectory and position change of the camera based on the motion information of these feature points.

[0068] Specifically, the Shi-Tomasi algorithm is used to extract corner points, and the quality of feature points is improved by selecting the best corner points.

[0069] Specifically, the corner optimization uses the interpolation method to perform sub-pixel corner detection. By performing linear interpolation along the edge of the object, the sudden change of the value caused by the sudden change of the pixel value can be avoided, so that the pixel gray value of the point finally obtained is more accurate. The present invention uses linear interpolation to solve the pixel gray value. Linear interpolation is a method of combining the known pixel gray values ​​around the required point with two known straight lines to find the unknown value. See Figure 3 As shown in the figure, to find the pixel value of point P when the grayscale values ​​of four pixels ABCD are known, first insert two pixel points F=(x,y1) and E=(x,y2) in AB and CD on the x-axis respectively. Then, in the y-axis direction, the pixel value of point P is obtained according to the pixel values ​​of points E and F. The pixel value of point P is calculated as follows:

[0070]

[0071] Among them, f() represents the pixel value of a certain point.

[0072] Specifically, the corner point matching uses the optical flow method to track and match the corner points, and uses the pixel brightness changes between adjacent frames to calculate the camera's displacement and rotation, and estimates the camera's motion in real time, thereby providing positioning information during the map construction process. The image information during the optical flow tracking can be regarded as a function F(t) related to time, and the pixel grayscale value is F(x,y,t). The velocity vector of the pixel along the x-axis is Note that μ is the velocity vector along the y-axis. Let it be ν. F x , F y , F t , when in an m×m window, m quantities move consistently in a continuous time, we can get m 2 Equations:

[0073]

[0074] The above formula can Its speed of movement is

[0075] Specifically, S1032 constructs an IMU motion model according to the collected personnel acceleration, angular velocity, etc., and pre-integrates the IMU motion model to obtain a simplified IMU motion model; including the following steps:

[0076] S21, the measurement model of angular velocity and acceleration in IMU is as follows:

[0077]

[0078] Here, the symbol w refers to the global positioning coordinate system. and It represents the measured value of angular velocity and acceleration; ω b and a w is the actual value of angular velocity and acceleration. g and b a Refers to the zero bias of the gyroscope and accelerometer; n g and n a is the noise of the gyroscope and accelerometer. w =[0,0,g] T Represents the gravity vector in the global coordinate system.

[0079] S22, then take the derivatives of position, velocity, and attitude with respect to time, and obtain the following formula:

[0080]

[0081] Among them, P, v, and q represent the position, velocity, and attitude of the IMU respectively. is four-element multiplication.

[0082] S23, integrating the position, speed, and attitude measurement values ​​collected at time i, thereby obtaining the position, speed, and attitude at time j, as shown in the following formula:

[0083]

[0084] Here, δt represents a very short time interval, and it is assumed that the bias remains unchanged within δt. The value range of δt in the embodiment of the present invention is 0.001-0.002.

[0085] S24, pre-integrate the IMU data. Pre-integration is a technique used to process two consecutive frames of IMU data. It integrates the acceleration and angular velocity in order to more accurately derive the relative position and attitude of the camera. However, when processing image sequences, if there are multiple IMU data between consecutive frames (i.e., image a and image b), the entire calculation process will become quite complicated. Therefore, in order to make the model more concise, this study introduces a simplified technique. First, the IMU data of the first frame is aligned with the image of the ath frame, and then the IMU data of the last frame is aligned with the image of the bth frame. Finally, the data is integrated to accurately determine the position and attitude of the camera. The pre-integration formulas for position, velocity, and attitude in continuous time in the IMU method are as follows:

[0086]

[0087] in, Indicates the accelerometer measurement value under the Body system, Indicates the gyroscope measurement value in the Body frame, b g and b a It refers to the zero bias of the gyroscope and accelerometer. is the rotation matrix of the IMU's body coordinate system relative to the world coordinate system. In all the above formulas, the ^ indicates the measured value, which is affected by zero bias and noise, such as Without a real value, such as a w ,ω b .

[0088] Specifically, the S1033 establishes a nonlinear objective function by constructing residual variances of visual information and IMU information, including multiple optimization variables of images and IMU. Specifically, the following steps are included:

[0089] S31, construct a state vector; the state vector includes n+1 sensor states, an external parameter, and the inverse depth of m+1 spatial landmarks. The monitoring parameters of the sensor include basic information such as coordinates, speed, and rotation angle, while also considering accelerometer bias and gyroscope readings. The external parameters include the translation and rotation relationship between the camera and the IMU. The inverse depth of the spatial landmark is the inverse of the landmark point depth. X represents the state vector, x k Represents the state quantity of the kth frame, including five variables: the translation quantity, velocity quantity, rotation quantity, accelerometer deviation and gyroscope deviation of the kth frame. It represents the external parameters of the camera and IMU. The state vector is as follows:

[0090]

[0091] S32, for IMU constraints, mainly the changes in displacement, velocity, rotation, acceleration deviation and gyroscope deviation between two frames. The specific formula is as follows:

[0092]

[0093] The above are the P position, v velocity, q rotation, and two biases ba and bw at the kth and k+1th moments in the IMU. δa is the position pre-integration error, δθ is the angle pre-integration error. δβ is the velocity pre-integration error. δb a is the acceleration bias error, δb g is the gyroscope bias error. Represents four-element multiplication, that is, the state quantity from the kth frame to the k+1th frame, including a total of 10 variables. The displacement, velocity, and offset in the above error are obtained by direct subtraction. [·] xyz A three-dimensional vector that takes only the imaginary part (x, y, z) of the quaternion.

[0094] S33, for visual constraints, the ideal sensor posture can be obtained by minimizing the visual residual caused by pixel reprojection. The pixel coordinate value obtained by the first projection of the first landmark point in the i-th camera coordinate system is transformed and mapped to the j-th coordinate system. The difference between the pixel coordinate value thus transformed and the corresponding pixel coordinate value directly observed constitutes the visual residual. The calculation formula is as follows:

[0095]

[0096] Among them, the state quantity to be estimated is the three-dimensional space coordinates (x, y, z) of the feature point T , observation value (u,v) T are the coordinates of the feature in the normalized camera plane.

[0097] There are three variables to be optimized: external parameters, state variables, and inverse depth. The value projected to the camera coordinate system of the jth frame is:

[0098]

[0099] Among them, T bc is the rotation external parameter from camera to IMU. i Transformation matrix from IMU frame to world frame b j Transformation matrix from IMU frame to world frame The feature point is in the C i The pixel coordinates on the frame are is the inverse depth.

[0100] S34, when establishing the overall objective function, the prior information, visual data and IMU constraint information are comprehensively considered. The optimal state vector integrating multi-sensor information is obtained through nonlinear optimization method. The specific formula is as follows:

[0101]

[0102] in, represents the noise covariance of IMU pre-integration, represents the noise covariance generated by vision, r p -J p X is the marginalized prior information, is the IMU measurement residual, is the visual residual. Covariance is used to adjust the confidence of the IMU and visual sensors during coupling during data coupling. When the noise level of a sensor is significantly higher than that of another sensor, it means that the data reliability of the sensor is low. Therefore, when coupling data, it is necessary to appropriately reduce the weight ratio of the sensor and increase the weight ratio of the other sensor to enhance the relative reliability of the data of each sensor.

[0103] Specifically, S1034 determines the initial iteration value of the optimization variable based on the approximate value measured by the sensor, and performs in-depth optimization processing on this basis to ensure that the optimization variable can reach the global optimal solution as much as possible and avoid falling into the dilemma of local optimal solution. After coupling IMU with SLAM, the position and movement trajectory of the person can be located in real time on the computer, such as Figure 4 shown.

[0104] It can be seen that the present invention provides a real-time positioning method in a GPS signal blind spot based on SLAM-IMU coupling, which perceives the environment through a visual SLAM sensor and performs corner point matching to construct a three-dimensional image, and then couples and optimizes the pre-integrated IMU data, thereby constructing a real-time positioning algorithm based on multi-source information coupling, which solves the problem of workers' autonomous positioning in signal blind spots, helps supervisors understand the workers' positions in real time, and can also provide safety guidance for workers to reduce the occurrence of accidents.

[0105] The above are only preferred embodiments of the present invention and are not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the protection scope of the present invention.

Claims

1. A real-time positioning method in GPS signal blind area based on SLAM-IMU coupling, characterized in that: Based on a positioning device including a binocular camera with a SLAM sensor and an IMU element, the positioning method comprises the following steps: The binocular camera transmits the image sequences captured at different discrete time points to the computer; The IMU element transmits the collected real-time angular velocity and acceleration to the computer; The computer combines the image sequence, real-time angular velocity and acceleration to establish a nonlinear objective function containing multiple optimization variables. Through multiple iterations, the optimization variables are made to reach the global optimal solution, thereby obtaining the real-time position and movement trajectory.

2. The real-time positioning method in a GPS signal blind area based on SLAM-IMU coupling according to claim 1, characterized in that: The process of the computer obtaining the real-time position and movement trajectory of the staff member includes the following steps: Performing corner point extraction, corner point optimization and corner point matching on the collected image sequence to establish visual information; the corner points refer to significant feature points; According to the angular velocity and acceleration Construct an IMU motion model and pre-integrate the IMU motion model to obtain a simplified IMU motion model; Combining visual information with a simplified IMU motion model to establish a nonlinear objective function containing multiple optimization variables; The initial iteration values ​​of the optimization variables are determined based on the approximate values ​​measured by the sensors, and the global optimal solution is obtained through multiple iterations to obtain the real-time position and movement trajectory of the staff.

3. The real-time positioning method in a GPS signal blind area based on SLAM-IMU coupling according to claim 1, characterized in that: The process of the computer establishing visual information comprises the following steps: Corner point extraction: Shi-Tomasi algorithm is used to extract corner points, and the quality of feature points is improved by selecting the best corner points; Corner optimization uses interpolation method to detect sub-pixel corners. It performs linear interpolation along the edge of the object to avoid sudden changes in pixel values. Corner matching uses the optical flow method to track and match corner points, uses the pixel brightness changes between adjacent frames to calculate the camera's displacement and rotation, and estimates the camera's motion in real time, thereby providing positioning information during the map construction process.

4. The real-time positioning method in a GPS signal blind area based on SLAM-IMU coupling according to claim 3 is characterized in that: The interpolation method uses linear interpolation to solve the pixel grayscale value, combining the known pixel grayscale values ​​around the desired point with two straight lines of known quantities, thereby finding the pixel grayscale value of the desired point.

5. The real-time positioning method in a GPS signal blind area based on SLAM-IMU coupling according to claim 4 is characterized in that: For the required point P(x,y), the pixel grayscale values ​​of the four points around P(x,y) are known, including the pixel grayscale values ​​of A(x1,y2), B(x2,y2), C(x1,y1) and D(x2,y1), y2>y>y1, x2>x>x1; the process of calculating the pixel grayscale value of P(x,y) includes: Insert pixel points F = (x, y1) and E = (x, y2) between AB and CD respectively; The pixel value of point P is calculated based on the pixel values ​​of E and F, expressed as: Where f(·) represents the pixel value.

6. The real-time positioning method in a GPS signal blind area based on SLAM-IMU coupling according to claim 4 is characterized in that: The corner point matching uses the optical flow method to track and match the corner points. When the m spatial points in the image move consistently in a continuous time in an m×m window, it can be expressed as: Wherein, μ represents the velocity vector of the pixel along the x-axis, v represents the velocity vector of the pixel along the y-axis; F x express F y express F t express F is expressed as F(x,y,t), which refers to the pixel grayscale value of the point at coordinates (x,y) at time t.

7. The real-time positioning method in a GPS signal blind area based on SLAM-IMU coupling according to claim 6 is characterized in that: The IMU motion model is expressed as: in, represents the position at time j, represents the speed at time j, represents the posture at time j; represents the position at time i, represents the speed at time i, Δt represents the time interval from i to j, represents the posture at time t; Represents the actual value of acceleration at time t, g w represents the gravity vector in the global coordinate system, δ t Indicates a short time interval; Represents the actual value of the angular velocity at time t; Represents four-element multiplication.

8. The real-time positioning method in a GPS signal blind area based on SLAM-IMU coupling according to claim 7 is characterized in that: The IMU motion model is pre-integrated, specifically, by aligning the IMU data of the first frame with the image of the ath frame, and then aligning the IMU data of the last frame with the image of the bth frame, and finally integrating the data to obtain a simplified IMU motion model represented as: in, Indicates the position of the IMU coordinate system relative to the world coordinate system at the k+1th moment, Indicates the velocity of the IMU coordinate system relative to the world coordinate system at the k+1th moment, Indicates the posture from the IMU coordinate system to the world coordinate system at the k+1th moment; They represent the position, velocity, and attitude of the IMU coordinate system relative to the world coordinate system at the kth moment, Δt k represents the time interval from k to k+1, represents the accelerometer bias at time t, It indicates that it is biased by the gyroscope at time t; Indicates the accelerometer measurement value under the Body system, Indicates the gyroscope measurement value in the Body frame, b g and b a It refers to the zero bias of the gyroscope and the zero bias of the accelerometer. It is the rotation matrix of the IMU's Body coordinate system relative to the world coordinate system.

9. The real-time positioning method in a GPS signal blind area based on SLAM-IMU coupling according to claim 8, characterized in that: The method of combining visual information with a simplified IMU motion model to establish a nonlinear objective function includes the following steps: Construct a state vector, including sensor state, external parameters and inverse depth of spatial landmarks. The state vector X is expressed as: Among them, x0,x1,…,x n Represents the sensor state, including the sensor's monitoring parameters (coordinates, rate, rotation angle), accelerometer bias, and gyroscope readings, where the state quantity x of the kth frame is k Include translation Speed Rotation b a represents the bias of the accelerometer in the IMU, b g Indicates the deviation of the gyroscope in the IMU; λ0,λ1,…,λ m Indicates the inverse depth of the spatial landmark, which is the reciprocal of the landmark point depth; Represents the external parameters of the camera and IMU, including the translation and rotation relationship between the camera and IMU; Constraints are imposed on the IMU model, mainly the changes in displacement, velocity, rotation, acceleration deviation and gyroscope deviation between two frames, and the IMU measurement residual is expressed as: Among them, δa is the position pre-integration error, δθ is the angle pre-integration error, δβ is the velocity pre-integration error, and δb a is the acceleration bias error, δb g is the gyroscope bias error; where, represents four-element multiplication, [·] xyz is a three-dimensional vector that takes only the imaginary part (x, y, z) of the quaternion; Represents the rotation matrix of the IMU body coordinate system from the kth to the k+1th frame, The position of the IMU coordinate system relative to the world coordinate system at the k+1th moment, The position of the IMU coordinate system relative to the world coordinate system at the kth moment, The velocity of the IMU coordinate system relative to the world coordinate system at the kth moment, The acceleration in the IMU body coordinate system from the kth to k+1th frame, Indicates that from frame b k To frame b k+1 The rotation in quaternion form in the IMU body coordinate system, The inverse of the rotation quaternion from the world coordinate system to the IMU body coordinate system at the kth moment, The rotation quaternion converted from the body coordinate system to the world coordinate system at the k+1th moment, Represents the rotation matrix from the IMU body coordinate system to the world coordinate system at the kth moment, Indicates the velocity of the IMU coordinate system relative to the world coordinate system at the k+1th moment, The deviation from the IMU body coordinate system at the kth moment to the IMU body coordinate system at the k+1th moment, Indicates the acceleration deviation of the IMU body coordinate system at the k+1th moment, represents the acceleration error of the IMU body coordinate system at the kth moment, Indicates the angular velocity deviation of the IMU body coordinate system at the k+1th moment, Angular velocity deviation of the IMU body coordinate system at the kth moment; Constraints are imposed on the visual information to obtain the ideal sensor posture by minimizing the visual residual caused by pixel reprojection. The pixel coordinate value obtained by the first projection of the first landmark point in the i-th camera coordinate system is transformed and mapped to the j-th coordinate system. The difference between the pixel coordinate value obtained by the transformation and the corresponding pixel coordinate value observed directly constitutes the visual residual, which is expressed as: in, Projection to C j The x value in the frame camera coordinate system, Projection to C j The y value of the frame camera coordinate system, Projection to C j The z value in the frame camera coordinate system, Represents the inverse homogeneous transformation matrix from the IMU coordinate system back to the camera coordinate system, represents the inverse matrix of the homogeneous transformation matrix from the world coordinate system to the jth IMU coordinate system, represents the inverse matrix of the homogeneous transformation matrix from the world coordinate system to the jth IMU coordinate system, T bc Represents the homogeneous transformation matrix from the camera coordinate system to the IMU coordinate system; the feature point is in the Cth i The pixel coordinates on the frame are λ represents the inverse depth; Taking into account the prior information, visual data and IMU constraint information, the optimal state vector of the integrated multi-sensor information is obtained through nonlinear optimization method, which is expressed as: Among them, ||r p -J p X|| 2 is the marginalized prior information, is the IMU measurement residual, is the visual residual, represents the noise covariance of IMU pre-integration, represents the noise covariance generated by vision.

10. The real-time positioning method in a GPS signal blind area based on SLAM-IMU coupling according to claim 9, characterized in that: The optimization variables are optimized to reach the global optimal solution through multiple iterations, and the optimization variables include IMU measurement residuals Visual residual Noise covariance of IMU pre-integration and the visual noise covariance Initial iteration values ​​of the optimization variables are determined based on approximate values ​​of sensor measurements.

Citation Information

Patent Citations

  • Robot positioning method with fusion of visual features and IMU information

    CN110345944A

  • Universal visual SLAM (Simultaneous Localization and Mapping) method

    CN114111818A

  • Positioning mapping method based on visual laser radar inertia tight coupling

    CN116182837A

  • Binocular vision inertial odometer method using road sign point offset pose correction

    CN116342661A

  • Data processing method and apparatus

    US20210183100A1