A method for constructing a visually immersive environment by fusing information from multiple sensors

Abstract

This invention proposes a method for constructing a visually immersive environment by fusing information from multiple sensors. This method enables accurate real-time mapping of the environment, enhancing the visual experience of images and providing robots operating outdoors with a rich 3D color point cloud map containing abundant visual information. This helps remote operators assess the environment. The invention optimizes inter-frame pose using a factor map approach, considering the influence of IMU pre-integration factors, radar odometry factors, loop closure optimization factors, and GPS factors to achieve more accurate point cloud registration. Furthermore, this invention can overlay a color 3D point cloud map based on frame-by-frame registration of 2D images and 3D point clouds, containing richer visual information and providing a stronger sense of visual immersion.

Description

Technical Field

[0001] This invention relates to the field of mapping technology, and more specifically to a method for constructing a visually immersive environment by fusing information from multiple sensors. Background Technology

[0002] Currently, robots are widely used in exploration and disaster relief scenarios. The terrain of the environment to be explored or the post-disaster site is complex and contains many obstacles, requiring precise environmental modeling capabilities and accurate self-localization capabilities to accurately construct a visually immersive environment. After perceiving the environment, humans can remotely control the robot to complete related tasks.

[0003] Current technologies typically employ LiDAR (Light Detection and Ranging) for point cloud mapping, using the outlines of the point cloud distribution to determine the actual environment. While LiDAR provides accurate depth information, it is prone to misidentification in challenging environments such as mountains and caves. This is because these environments are densely populated with objects of unclear and irregular shapes, such as trees and rocks, making accurate outline extraction impossible using only point cloud mapping. Alternatively, cameras or GPS can be used for point cloud mapping. Cameras provide rich color and texture information and can also provide localization, but their accuracy is low in outdoor environments. GPS offers high accuracy and real-time performance in outdoor environments, but its accuracy drops significantly when there are obstructed areas on the ground. Given the complex usage scenarios and the limitations of existing technologies, a more accurate visual presence reconstruction method is needed to overcome these shortcomings and better assist robots in their operations. Summary of the Invention

[0004] In view of this, the present invention provides a method for constructing a visually immersive environment by fusing information from multiple sensors, which can accurately map the on-site environment in real time and enhance the visual experience of the image.

[0005] To achieve the above-mentioned objectives, the technical solution of this invention is as follows:

[0006] The method for constructing a visually immersive environment by fusing information from multiple sensors includes the following steps:

[0007] S1. Install a sensor group and GPS on the robot to acquire 3D point cloud and 2D images of the environment frame by frame; the sensor group includes radar, camera and IMU.

[0008] S2. Based on the IMU measurement data, the robot's state information is pre-estimated, including velocity and pose; frames with pose changes greater than a threshold are selected as keyframes, and the motion constraints of the robot between keyframes are obtained using a pre-integration method. These motion constraints are IMU pre-integration factors.

[0009] S3. Extract feature points of the 3D point cloud under each key frame and match them to obtain the inter-frame pose constraints and local feature maps of the 3D point cloud of adjacent key frames. The inter-frame pose constraints are radar odometry factors.

[0010] S4. GPS obtains the robot's position constraints, which are GPS factors.

[0011] S5. Using the radar odometer factor as a constraint, perform loop closure optimization of the inter-frame pose, where the inter-frame pose is the loop closure optimization factor.

[0012] S6. Using the factors and local feature maps obtained from S2 to S6 as observation information constraints, and using the state information as state nodes, a factor map is established to obtain the optimal pose.

[0013] S7. Superimpose the colored 3D point cloud according to the optimal pose to form a colored 3D point cloud map.

[0014] Furthermore, the specific method of S1 is as follows: the camera and IMU are fixed inside the sensor assembly mounting box, the radar is fixed on the mounting box, and the mounting box is fixed on the robot; the GPS is fixed inside the robot, and the two antennas of the GPS are pointed to the front and rear sides inside the robot respectively; the camera and radar follow the robot's movement in the real environment and take pictures and scan frame by frame to obtain two-dimensional images and three-dimensional point clouds of the real environment; at the same time, the IMU measures the robot's acceleration, angular velocity and position in real time to form measurement data; the GPS performs real-time positioning of the robot.

[0015] Furthermore, the specific method of S3 is as follows:

[0016] S3-1. Using the sliding window method, select the n keyframes preceding the current keyframe k+1, extract the edge feature points and planar feature points of the 3D point cloud acquired at the current keyframe k+1, and calculate the local edge feature map composed of the edge feature points and the edge feature points of the preceding n keyframes. The distance from the edge line to d es And a local planar feature map composed of planar feature points and planar feature points from the first n keyframes. Planar distance d ps The inter-frame pose of the 3D point cloud between keyframe k and the current keyframe k+1 is obtained; inter-frame pose constraints are constructed as radar odometry factors.

[0017] S3-2. Based on the robot's state constraints between two keyframes obtained in S2, calculate the robot's inter-frame pose and combine the feature points obtained in S3-1 with... and Matching is performed to obtain the local map corresponding to the current keyframe k+1; based on the inter-frame pose of the 3D point cloud obtained in S3-1, the local map is transformed to the initial point cloud coordinate system, the local map is updated, and the point cloud registration at the current keyframe k+1 is completed; the next keyframe is updated to the current keyframe, and S3-1 and S3-2 are repeated until all keyframes have completed point cloud registration.

[0018] Local edge feature map in S3-1 and local planar feature maps The construction method is as follows: extract feature points from the 3D point cloud collected in each of the n keyframes to form the feature set of the keyframe. According to the corresponding inter-frame pose {T k-n ,...,T k} Construct a local map M k .

[0019] Furthermore, the method for extracting feature points in a 3D point cloud is as follows: calculate the curvature of all points in the 3D point cloud acquired during a certain key frame, select points with curvature greater than the upper threshold as edge feature points, and select points with curvature less than the lower threshold as planar feature points; the edge feature points and planar feature points constitute the features of the key frame.

[0020] Furthermore, the specific method of S5 is as follows:

[0021] S5-1. Calculate the absolute pose of the 3D point cloud based on the radar odometry factor obtained in S3, and construct the constraints for loop closure optimization, the expression of which is:

[0022] T ij =T i -1 T j

[0023] Among them, T i Let T be the absolute pose of the 3D point cloud at keyframe i. j T represents the absolute pose of the 3D point cloud at keyframe j. ij The inter-frame pose between keyframes i and j after loop closure optimization.

[0024] S5-2, T i -1 T j and T ij Representing the inter-frame pose residuals in Lie algebra space, we construct the Jacobian matrix of the inter-frame pose residuals. We sum all the residual terms of the inter-frame pose residuals, calculate the first derivative of the sum of the residual terms, and obtain the inter-frame pose that minimizes the sum of the residual terms, which serves as the loop closure optimization factor.

[0025] Furthermore, the specific method for creating factor graphs in S6 is as follows:

[0026] Using the state information of each keyframe as a state node, and the radar odometry factor, loop closure optimization factor, GPS factor, IMU pre-integration factor, and local feature map as observation information constraints, a factor graph is constructed: state nodes of adjacent keyframes are connected by radar odometry factor and IMU pre-integration factor as edges, state nodes of non-adjacent keyframes are connected by loop closure optimization factor as edges, the state node of the current keyframe is connected to the GPS factor, and the state node of the next keyframe is connected to the local feature map composed of the previous n keyframes.

[0027] Furthermore, the specific method for obtaining the color 3D point cloud in S7 is as follows: the 3D point cloud is registered frame by frame with the 2D image through joint calibration of the camera and radar to obtain the color 3D point cloud.

[0028] Furthermore, S2 also includes: point cloud distortion correction for 3D point clouds.

[0029] Beneficial effects:

[0030] 1. This invention proposes a method for constructing a visually immersive environment by fusing information from multiple sensors. This method can accurately map the on-site environment in real time, enhancing the visual experience of the images. It provides robots operating in outdoor environments with a three-dimensional color point cloud map rich in visual information, helping remote operators assess the environment. This invention optimizes inter-frame pose based on factor maps, considering the influence of IMU pre-integration factors, radar odometry factors, loop closure optimization factors, and GPS factors on inter-frame pose to achieve more accurate point cloud registration. This invention can overlay a color three-dimensional point cloud map based on frame-by-frame registration of two-dimensional images and three-dimensional point clouds, containing richer visual information and providing a stronger sense of visual immersion.

[0031] 2. This invention utilizes four sensors—a 3D LiDAR, a camera, an inertial navigation unit, and GPS—to construct a color point cloud map of the outdoor environment. This method retains the advantages of LiDAR point cloud mapping while adding color information to the environment, enhancing the remote operator's perception of the surroundings. The mapping device has a simple structure, operates fully automatically, and can quickly and accurately create a real-time map of the environment. This provides prior environmental information for the rescue robot's navigation module, enabling on-site positioning and navigation, improving control accuracy, and offering convenience, practicality, and manpower savings.

[0032] 3. The sensor assembly method of this invention can ensure that the robot's mechanical position is parallel to the ground at the physical level, which is convenient for the robot to operate in complex environments.

[0033] 4. When constructing radar odometry factors, this invention selects the 3D point cloud under the key frame with large pose changes from all frames as the calculation object, which greatly reduces the amount of calculation and improves the calculation speed.

[0034] 5. When constructing the loop closure optimization factor, this invention considers the influence of the radar odometry factor and optimizes the inter-frame pose using the loop closure detection method to achieve more accurate matching of 3D point cloud and 2D image.

[0035] 6. This invention performs frame-by-frame registration of 3D point clouds and 2D images, giving the 3D point cloud richer visual information, thereby helping remote operators of robots to more clearly perceive the types of obstacles they are facing in complex environments, and thus perform corresponding tasks. Attached Figure Description

[0036] Figure 1 This is a flowchart of the method of the present invention.

[0037] Figure 2 This is a schematic diagram of the sensor array.

[0038] Figure 3 This is a schematic diagram of a factor plot.

[0039] Among them, 1-radar, 2-camera, 3-IMU, 4-mounting box. Detailed Implementation

[0040] The present invention will now be described in detail with reference to the accompanying drawings and embodiments.

[0041] like Figure 1 As shown, this invention provides a method for constructing a visually immersive environment by fusing information from multiple sensors, the steps of which include:

[0042] Step 1: Install a sensor array on the robot to acquire point cloud information, image information, and the robot's own status information, and preprocess the point cloud information:

[0043] Step 1-1: Install a sensor array on the robot to acquire point cloud information, image information, and the robot's observation information from the environment, such as... Figure 2 As shown, the sensor array includes radar, a camera, an IMU (Integrated Measurement Unit), and GPS. The camera and IMU are fixed inside the sensor array's mounting box, while the radar is mounted on the box, which is positioned directly in front of the robot's perception platform. The GPS is fixed inside the robot, with its two antennas pointing towards the front and rear sides of the robot. In actual use, the camera and radar follow the robot's movement in the real environment, capturing images and scanning to obtain image and point cloud information. Simultaneously, the IMU collects real-time measurements of the robot's speed, acceleration, angular velocity, and other data, while the GPS provides real-time positioning for the robot.

[0044] Steps 1-2: Pre-estimate the robot's state information based on IMU measurement data and transform it from the IMU coordinate system to the robot coordinate system. State information includes the robot's velocity and pose, thus obtaining inter-frame pose changes (and inter-frame pose) and relative velocity. This invention defines one full rotation of the radar as one frame, the time interval between two frames as Δt, and pose as both position and orientation, expressed in the form of rotation angles.

[0045] Steps 1-3: Preprocess the point cloud information acquired by the radar to obtain the distortion-corrected 3D point cloud: Let P be the 3D point cloud obtained from the k-th frame scan. k ={p1,p2,...,p n} includes n points, where one point p i The coordinates in the radar coordinate system are (x i ,y i ,z i ).like Figure 2 As shown, in the radar coordinate system, Y l X is the direction the robot is moving. l Z represents the right side of the direction of travel. l Pointing upwards towards the robot, with the origin at the center of the radar. Calculate the start time t of the k-th frame. s and the end time t e Time scan line relative to X l The angle α between the positive and negative semi-axis start and α end Its expression is:

[0046]

[0047]

[0048] Where x1 is the starting time t s Time point p i In the radar coordinate system, the x-coordinate n Let t be the end time. e Time point p i In the radar coordinate system, the horizontal coordinate y1 represents the initial time t. s Time point p i In the radar coordinate system, the ordinate, y n Let t be the end time. e Time point p i The vertical coordinate in the radar coordinate system. Scan lines are classified according to the elevation angle of the scanned point cloud.

[0049] junction point p i The coordinates of point p are obtained. i Time t i Its expression is:

[0050]

[0051] Where, α i For point p i With X l The angle between the positive and negative axes.

[0052] Get time t i Then, IMU data interpolation can be used to quickly calculate time t. i The velocity of the point cloud at time (V) ix V iy V iz ), displacement (X) i ,Y i Z i ) and rotation angle (Roll) i Pitch i ,Yaw i ), combined with the initial time t s velocity (V) sx V sy V sz ), displacement (X) s ,Y s Z s ) and rotation angle (Roll) s Pitch s ,Yaw s The displacement distortion (Δ) caused by acceleration and deceleration is obtained. X ,Δ Y ,Δ Z Its expression is:

[0053] Δ X =X i -X s -V sx ×t i .

[0054] Δ Y =Y i -Y s -V sy ×t i

[0055] Δ Z =Z i -Z s -V sz ×t i

[0056] The motion distortion values ​​are rotated around the Y, X, and Z axes to obtain the motion distortion in the starting point coordinate system. Then, the point p... i The coordinates are first transformed to the world coordinate system, and then transformed from the world coordinate system to the 3D point cloud at the initial time t.s In the coordinate system, subtract the displacement distortion (Δ) from the coordinates. X ,Δ Y ,Δ Z ), to complete distortion correction.

[0057] Step 2: Construct factors for factor graph optimization, including IMU pre-integration factors, radar odometry factors, GPS factors, and loop closure optimization factors:

[0058] Step 2-1: Construct the IMU pre-integration factor:

[0059] A threshold is set to measure the inter-frame poses obtained in steps 1-2, and frames with inter-frame poses exceeding the threshold are defined as keyframes. The IMU pre-integration method is used to obtain the robot's motion constraints between keyframes, which are functions describing the changes in the robot's motion information. These motion constraints are used as IMU pre-integration factors. The IMU pre-integration factors include the relative velocity v. t,t+Δt Position p t,t+Δt and relative pose ΔR t,t+Δt Their expressions are as follows:

[0060]

[0061]

[0062]

[0063] Where g is the acceleration due to gravity, Δt is the time interval between two frames; R t Let R be the robot's pose at time t. t+Δt Let p be the robot's pose at time t+Δt; t Let p be the robot's pose at time t. t+Δt v represents the robot's position at time t+Δt; t Let v be the robot's velocity at time t. t+Δt Let p be the robot's velocity at time t+Δt. Since this invention uses a rotation angle to represent pose, therefore p... t p t+Δt It also corresponds to the robot's rotation angle at that moment, and its relative pose ΔR. t,t+Δt It can also refer to the change in the robot's rotation angle between two adjacent keyframes.

[0064] Step 2-2: Constructing the radar odometry factor:

[0065] Step 2-2 (1) Select the n keyframes before the current keyframe k+1 using the sliding window method, and extract the feature points of the 3D point cloud collected at the current keyframe k+1. Taking a point τ in the 3D point cloud as an example, select the five adjacent points before and after it in the same lidar beam where point τ is located, and calculate the coordinates X of point τ in the radar coordinate system at keyframe β. (β,τ) According to X (β,τ) The curvature c is calculated using the following expression:

[0066]

[0067] Where point τ′ is a point in the set S of points near itself, and X (β,τ′) The coordinates of τ′ in the radar coordinate system at keyframe β are calculated using the same method as the calculation of X. (β,τ) The method is the same.

[0068] The curvature values ​​of all points in a 3D point cloud are calculated. By comparing the curvature of each point, the point cloud can be divided into four types of feature points and invalid points. The four types of feature points are: primary edge points and secondary edge points with extremely large curvature values; primary plane points with extremely small curvature values; and secondary plane points with small curvature values. Invalid points are points without significant curvature values. In this invention, primary edge points and secondary edge points are collectively referred to as edge points above the upper threshold, and all edge feature points constitute edge features; primary plane points and secondary plane points are collectively referred to as plane points below the lower threshold, and all plane feature points constitute plane features. Planar features and edge features together constitute the features of the 3D point cloud.

[0069] Based on the robot's motion constraints between two keyframes, calculate the robot's inter-frame pose and combine the feature points obtained in S3-1 with... and Matching yields the local map corresponding to the current keyframe k+1. Local edge feature map. and local planar feature maps The construction method is as follows: extract feature points from n keyframes to form the feature set {F} of the keyframes. k-n ,...,F k}, based on the corresponding inter-frame pose {T k-n ,...,T k} Construct a local map M k .

[0070] Calculate the local edge feature map composed of the edge feature points and the edge feature points of the first n keyframes respectively. The distance from the edge line to d es And a local planar feature map composed of planar feature points and planar feature points from the first n keyframes. Planar distance d ps Its expression is:

[0071]

[0072]

[0073] in, The edge features of the 3D point cloud at keyframe k+1. The planar features of the 3D point cloud at keyframe k+1; Local edge feature map at keyframe k Edge feature points on the edge line l, Local edge feature map at keyframe k Edge feature points on the edge line m; Local planar feature maps at keyframe k Planes l, m, and n in the diagram.

[0074] Solving the optimal transformation problem with respect to distance yields the following results: Zhongyu Feature points matched by edge lines and The feature points for planar matching. The expression for the optimal transformation problem is:

[0075]

[0076] Among them, T k+1 This represents the inter-frame pose of the 3D point cloud at the current keyframe k+1. The edge features of the 3D point cloud at the current keyframe k+1. This represents the planar features of the 3D point cloud at the current keyframe k+1.

[0077] After obtaining the matching relationship between the feature points of the current keyframe k+1 and keyframe k, the inter-frame pose ΔT of the 3D point cloud between the two frames is derived. k,k+1 Inter-frame pose constraints were constructed and used as radar odometry factors, the expression of which is:

[0078] ΔT k,k+1 =T k T T k+1

[0079] Among them, T k The inter-frame pose of the 3D point cloud at keyframe k.

[0080] Step 2-2(2): Based on the motion constraints of the robot between the two keyframes obtained in Step 2-1, calculate the inter-frame pose of the robot, and combine the feature points obtained in S3-1 with... and Matching is performed to obtain the local map corresponding to the current keyframe k+1; based on the inter-frame pose of the 3D point cloud obtained in S3-1, the local map is transformed to the initial point cloud coordinate system, the local map is updated, and the point cloud registration at the current keyframe k+1 is completed; the next keyframe k+2 is updated to the current keyframe, and the process returns to step 2-2 to continue until all keyframes have completed point cloud registration.

[0081] Steps 2-3: Constructing GPS factors:

[0082] When the robot receives a GPS signal, it needs to be transformed into its own coordinate system. First, a transformation T is defined to convert the robot's own coordinates to GPS UTM coordinates:

[0083]

[0084] Where φ,θ, These are the robot's roll angle, pitch angle, and yaw angle in the initial UTM coordinate system. The UTM coordinates are for the initial GPS position report. The transformation formula for converting GPS signals to the robot's own coordinate system at any consecutive time is as follows:

[0085]

[0086] When a new state node is inserted into the factor graph, the GPS position of the latest state node is obtained by linear interpolation between GPS signals based on the key frame, thereby associating the GPS factor with the state node.

[0087] Steps 2-4: Constructing the loop closure optimization factor: Based on the radar odometry factor, establish the constraints for loop closure optimization, perform loop closure optimization on the inter-frame pose of the 3D point cloud, and obtain the loop closure optimization factor:

[0088] Step 2-4 (1) Calculate the absolute pose of the 3D point cloud based on the radar odometry factor obtained in Step 2, and construct the constraint conditions for loop closure optimization, the expression of which is:

[0089] T ij =T i -1 T j

[0090] Among them, T i Let T be the absolute pose of the 3D point cloud at keyframe i. j T represents the absolute pose of the 3D point cloud at keyframe j. ij The inter-frame pose between keyframes i and j after loop closure optimization.

[0091] Step 2-4(2), T i-1 T j and T ij Representing the inter-frame pose residuals in Lie algebra space, we construct the Jacobian matrix of the inter-frame pose residuals. We sum all the residual terms of the inter-frame pose residuals, calculate the first derivative of the sum of the residual terms, and obtain the inter-frame pose that minimizes the sum of the residual terms, which serves as the loop closure optimization factor.

[0092] T ij =T i -1 T j In Lie algebra space, it is represented as:

[0093] ξ ij =ln(T) i -1 T j ) ∨

[0094] =ln(exp(-ξ) i ) ∧ )exp(-ξ j ∧ ) ∨

[0095] Where, ξ ij For T ij The Lie algebra representation, ξ i For T i The Lie algebra representation, ξ j For T j The Lie algebra representation. When there is no error in the absolute pose, the two formulas above are equal. When there is an error in the absolute pose, the residual term e is calculated using both sides of the equation. ij :

[0096]

[0097] Add perturbations δξ to the absolute poses of the i-th and j-th sub-keyframes respectively. i ,δξ j Then solve for the Jacobian matrix, simplify using the adjoint property and the BCH formula, and at this point, the residual e ij Represented as:

[0098]

[0099] Where A is the Jacobian matrix. From the above equation, we can see that the residuals with respect to T... i Jacobian matrix A ij Residual about T j Jacobian matrix B ij Their expressions are as follows:

[0100]

[0101] Among them, residual e ij Jacobian matrix for:

[0102]

[0103] The Gauss-Newton method is used for optimization, and a first-order Taylor expansion is performed on the residuals:

[0104] e ij (x+Δx,x+Δx)=e ij (x+Δx)

[0105] ≈e ij +J ij Δx

[0106] Where x is the robot pose, x i Let x be the robot pose in the i-th frame. j Let J be the robot pose in frame j, and Δx be the correction value. ij For residual e ij Regarding the Jacobian matrix of inter-frame pose:

[0107]

[0108] For each residual term:

[0109]

[0110] Among them, Ω ij For residual e ij Information matrix, b ij For residual e ij The coefficient of the linear term in the rearranged expression, c ij For residual e ij The constant term in the rearranged expression, H ij For residual e ij The coefficients of the quadratic terms in the rearranged expression (i.e., the Hessian matrix).

[0111] Sum of all residual terms:

[0112]

[0113] Where c is the constant term in the summation, b is the coefficient of the linear term in the summation, and H is the coefficient of the quadratic term in the summation.

[0114] The expression for the increment ΔF(x) of the objective function is:

[0115] ΔF(x)=F(x+Δx)-F(x)

[0116] =2b TΔx+Δx T H i Δx

[0117] The optimization task described above is then transformed into finding Δx that minimizes ΔF(x). Let the first derivative of ΔF(x) be zero:

[0118]

[0119] Right now:

[0120] H i Δx=-b

[0121] Among them, H i Let J be the Hessian matrix of the i-th frame. Therefore, we only need to obtain J. ij Δx can then be obtained. The value of x is continuously adjusted based on the correction amount Δx, and the iteration is repeated multiple times until the residual meets the convergence condition, at which point the iteration terminates, and the optimization is completed.

[0122] Step 3: Construct a system with radar odometry factor, loop closure optimization factor, and GPS factor as edges, and robot state as nodes, as shown below. Figure 3 The factor graph shown is used to optimize the robot's inter-frame pose and obtain the optimal pose.

[0123] Figure 3 A factor graph is constructed to solve for the optimal pose at keyframe k. This invention considers the influence of five observation constraints on the robot's own state: radar odometry factor, loop closure optimization factor, GPS factor, IMU pre-integration factor, and local feature map. The constraints represented by the radar odometry factor and IMU pre-integration factor are located at the state nodes between every two adjacent keyframes. The constraints represented by the GPS factor independently affect the state nodes of each keyframe. The constraints represented by the loop closure optimization factor are located between the state nodes of two non-adjacent keyframes. The local feature map can be considered as a feature set of the first n keyframes, influencing the state of the next keyframe.

[0124] After establishing the factor graph, the inter-frame pose of each state node is optimized to obtain the optimal pose.

[0125] Step 4: Register the 3D point cloud with the 2D image frame by frame through joint calibration of the camera and radar to obtain a color 3D point cloud; according to the optimal pose, superimpose the color 3D point cloud frame by frame to form a color 3D point cloud map.

[0126] The mapping relationship between the camera and the LiDAR requires a series of coordinate system transformations, including world coordinate system, camera coordinate system, image coordinate system, and pixel coordinate system. The transformation from the world coordinate system to the camera coordinate system is a rigid body transformation, meaning the object does not deform; only rotation and translation are required. Let R be the rotation matrix, T be the translation matrix, and the world coordinate system be (X...).W ,Y W Z W The camera coordinate system is (X). C ,Y C Z C If the coordinate system is such that the transformation relationship between the two coordinate systems is:

[0127]

[0128] The transformation from the camera coordinate system to the image coordinate system reflects the projection mapping relationship during the imaging process. Let the image coordinate system be (x, y) and the camera focal length be f, then the transformation relationship between the two coordinate systems is:

[0129]

[0130] In this system, the Z-axis of the camera coordinate system is in the same direction as the Z-axis of the image coordinate system.

[0131] The pixel coordinate system is a direct coordinate system established with the top-left corner of the image as the origin, using pixels as the unit. It represents the coordinates of the image points of an object on the digital image. Let f u f v Let u0 and v0 be the effective focal lengths of the camera in the horizontal and vertical directions, respectively, and u0 and v0 be the coordinates of the center point of the imaging plane. Then the transformation relationship between the two coordinate systems is as follows:

[0132]

[0133] Among them, M in That is, the camera intrinsic parameter matrix, M ex That is, the camera extrinsic parameter matrix.

[0134] The point cloud of a lidar can be regarded as a three-dimensional coordinate point in the world coordinate system. The above parameter matrix can be obtained through the joint calibration method of camera and radar. Based on the intrinsic parameter matrix of the camera and the extrinsic parameter matrix between the camera and radar obtained through joint calibration, the point cloud of the 3D camera is transformed into the coordinate system of the GRB camera.

[0135] For a frame of point cloud data and a corresponding frame of 2D image data acquired by a 3D LiDAR, let ... For a laser point in a frame of point cloud data, its homogeneous coordinates can be represented as: laser point Points projected into pixel coordinate system It can be given by the following formula:

[0136]

[0137] Among them, h i For M in *M ex The row vector of the i-th row.

[0138] The coordinates of the three-dimensional colored laser points can then be expressed as:

[0139]

[0140] Here, R(u,v), G(u,v), and B(u,v) represent the three-dimensional color information of the projected pixels. This allows for the conversion of the original point cloud into a colored point cloud.

[0141] The current color point cloud is transformed to the target point cloud coordinate system at the initial time. The transformation formula is as follows:

[0142] y′ i =T(py) i ) = Ry i +t

[0143] Among them, y i (i = 1, ..., N) y N represents the coordinates of point i in the 3D point cloud at the current moment. y Let T(py) be the number of points in the point cloud at the current time. i Let y' be the coordinate transformation function from the current point cloud to the target point cloud. i Let p be the coordinates of the point cloud at the current moment to the point i in the target point cloud coordinate system, where p is the rotation and translation matrix, R is the rotation matrix, and t is the translation matrix.

[0144] Where p = [t x ,t y ,t z ,φ x ,φ y ,φ z ] T . t x ,t y ,t z Let φ represent the translation along the x, y, and z directions, respectively. x ,φ y ,φ z These represent the rotation amounts along the x, y, and z directions, respectively.

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

Claims

1. A method for constructing a visually immersive environment by fusing information from multiple sensors, characterized by the following steps: include: S1. Install a sensor group and GPS on the robot to acquire 3D point cloud and 2D image of the environment frame by frame; the sensor group includes radar, camera and IMU; S2. Based on the IMU measurement data, the robot's state information is pre-estimated, including velocity and pose; frames with pose changes greater than a threshold are selected as keyframes, and the motion constraints of the robot between keyframes are obtained using a pre-integration method. These motion constraints are IMU pre-integration factors. S3. Extract the feature points of the 3D point cloud under each key frame and match them to obtain the inter-frame pose constraints and local feature maps of the 3D point cloud of adjacent key frames. The inter-frame pose constraints are radar odometry factors. S4. GPS acquires the robot's position constraints, which are GPS factors. S5. Using the radar odometry factor as a constraint, perform loop closure optimization of the inter-frame pose, where the inter-frame pose is the loop closure optimization factor. S6. Using the factors and local feature maps obtained from S2 to S6 as observation information constraints, and using state information as state nodes, a factor map is established to obtain the optimal pose. S7. Superimpose the colored 3D point cloud according to the optimal pose to form a colored 3D point cloud map.

2. The method as described in claim 1, characterized in that, The specific method of S1 is as follows: the camera and IMU are fixed inside the sensor assembly mounting box, the radar is fixed on the mounting box, and the mounting box is fixed on the robot; the GPS is fixed inside the robot, and the two antennas of the GPS are pointed to the front and rear sides inside the robot respectively; the camera and radar follow the robot's movement in the real environment and take pictures and scan frame by frame to obtain two-dimensional images and three-dimensional point clouds of the real environment; at the same time, the IMU measures the robot's acceleration, angular velocity and position in real time to form measurement data; the GPS performs real-time positioning of the robot.

3. The method as described in claim 1, characterized in that, The specific method for S3 is as follows: S3-1. Using the sliding window method, select the n keyframes preceding the current keyframe k+1, extract the edge feature points and planar feature points of the 3D point cloud acquired at the current keyframe k+1, and calculate the local edge feature map composed of the edge feature points and the edge feature points of the preceding n keyframes. The distance from the edge line to d es And a local planar feature map composed of planar feature points and planar feature points from the first n keyframes. Planar distance d ps The inter-frame pose of the 3D point cloud between keyframe k and the current keyframe k+1 is obtained; inter-frame pose constraints are constructed as radar odometry factors. S3-2. Based on the robot's state constraints between two keyframes obtained in S2, calculate the robot's inter-frame pose and combine the feature points obtained in S3-1 with... and Matching is performed to obtain the local map corresponding to the current keyframe k+1; based on the inter-frame pose of the 3D point cloud obtained in S3-1, the local map is transformed to the initial point cloud coordinate system, the local map is updated, and the point cloud registration at the current keyframe k+1 is completed; the next keyframe is updated to the current keyframe, and S3-1 and S3-2 are repeated until all keyframes have completed point cloud registration. Local edge feature map in S3-1 and local planar feature maps The construction method is as follows: extract feature points from the 3D point cloud collected in each of the n keyframes to form the feature set of the keyframe. According to the corresponding inter-frame pose {T k-n ,...,T k } Construct a local map M k .

4. The method as described in claim 1 or 3, characterized in that, The method for extracting feature points from a 3D point cloud is as follows: calculate the curvature of all points in the 3D point cloud acquired during a certain key frame, select points with curvature greater than the upper threshold as edge feature points, and select points with curvature less than the lower threshold as planar feature points; edge feature points and planar feature points constitute the features of the key frame.

5. The method as described in claim 1, characterized in that, The specific method for S5 is as follows: S5-1. Calculate the absolute pose of the 3D point cloud based on the radar odometry factor obtained in S3, and construct the constraints for loop closure optimization, the expression of which is: T ij =T i -1 T j Among them, T i Let T be the absolute pose of the 3D point cloud at keyframe i. j Let T be the absolute pose of the 3D point cloud at keyframe j. ij The inter-frame pose between keyframe i and keyframe j after loop closure optimization; S5-2, T i -1 T j and T ij Representing the inter-frame pose residuals in Lie algebra space, we construct the Jacobian matrix of the inter-frame pose residuals. We sum all the residual terms of the inter-frame pose residuals, calculate the first derivative of the sum of the residual terms, and obtain the inter-frame pose that minimizes the sum of the residual terms, which serves as the loop closure optimization factor.

6. The method as described in claim 1, characterized in that, The specific method for creating a factor graph in S6 is as follows: Using the state information of each keyframe as a state node, and the radar odometry factor, loop closure optimization factor, GPS factor, IMU pre-integration factor, and local feature map as observation information constraints, a factor graph is constructed: state nodes of adjacent keyframes are connected by radar odometry factor and IMU pre-integration factor as edges, state nodes of non-adjacent keyframes are connected by loop closure optimization factor as edges, the state node of the current keyframe is connected to the GPS factor, and the state node of the next keyframe is connected to the local feature map composed of the previous n keyframes.

7. The method as described in claim 1, characterized in that, The specific method for obtaining a color 3D point cloud in S7 is as follows: the 3D point cloud is registered frame by frame with the 2D image through joint calibration of the camera and radar to obtain the color 3D point cloud.

8. The method as described in claim 1, characterized in that, S2 also includes: point cloud distortion correction for 3D point clouds.

Images