WO2024007485A1 - 基于视觉特征的空地多机器人地图融合方法 - Google Patents

基于视觉特征的空地多机器人地图融合方法 Download PDF

Info

Publication number
WO2024007485A1
WO2024007485A1 PCT/CN2022/127636 CN2022127636W WO2024007485A1 WO 2024007485 A1 WO2024007485 A1 WO 2024007485A1 CN 2022127636 W CN2022127636 W CN 2022127636W WO 2024007485 A1 WO2024007485 A1 WO 2024007485A1
Authority
WO
WIPO (PCT)
Prior art keywords
map
sub
ugv
uav
data
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Ceased
Application number
PCT/CN2022/127636
Other languages
English (en)
French (fr)
Inventor
孙荣川
陆锦宇
郁树梅
孙立宁
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Suzhou University
Original Assignee
Suzhou University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Suzhou University filed Critical Suzhou University
Publication of WO2024007485A1 publication Critical patent/WO2024007485A1/zh
Anticipated expiration legal-status Critical
Ceased legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING OR CALCULATING; COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T17/00—Three-dimensional [3D] modelling for computer graphics
    • G06T17/05—Geographic models
    • G—PHYSICS
    • G06—COMPUTING OR CALCULATING; COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T17/00—Three-dimensional [3D] modelling for computer graphics
    • G06T17/005—Tree description, e.g. octree, quadtree
    • G—PHYSICS
    • G06—COMPUTING OR CALCULATING; COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G06T7/74—Determining position or orientation of objects or cameras using feature-based methods involving reference images or patches
    • G—PHYSICS
    • G06—COMPUTING OR CALCULATING; COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/40—Extraction of image or video features
    • G06V10/46—Descriptors for shape, contour or point-related descriptors, e.g. scale invariant feature transform [SIFT] or bags of words [BoW]; Salient regional features
    • G—PHYSICS
    • G06—COMPUTING OR CALCULATING; COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
    • G06V10/77—Processing image or video features in feature spaces; using data integration or data reduction, e.g. principal component analysis [PCA] or independent component analysis [ICA] or self-organising maps [SOM]; Blind source separation
    • G06V10/80—Fusion, i.e. combining data from various sources at the sensor level, preprocessing level, feature extraction level or classification level
    • G—PHYSICS
    • G06—COMPUTING OR CALCULATING; COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V20/00—Scenes; Scene-specific elements
    • G06V20/40—Scenes; Scene-specific elements in video content
    • G06V20/41—Higher-level, semantic clustering, classification or understanding of video scenes, e.g. detection, labelling or Markovian modelling of sport events or news items
    • G—PHYSICS
    • G06—COMPUTING OR CALCULATING; COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/20—Special algorithmic details
    • G06T2207/20212—Image combination
    • G06T2207/20221—Image fusion; Image merging

Definitions

  • the present invention relates to the technical field of image positioning, and in particular to an air-ground multi-robot map fusion method based on visual features.
  • UGV Unmanned Ground Vehicle
  • UAV Unmanned Aerial Vehicle
  • SLAM Simultaneous Localization and Mapping
  • ground-to-air multi-robots can effectively solve many problems in SLAM, there are differences in the maps created by UGVs and UAVs using different sensors, and there is a problem that the same features cannot be extracted to correlate sub-map data, which cannot be effectively combined to form a more accurate form. High global consistency map.
  • the present invention provides an air-ground multi-robot map fusion method based on visual features.
  • the present invention provides an air-ground multi-robot map fusion method based on visual features, which includes the following steps:
  • Step S1 collect data independently through the ground mobile robot UGV and the unmanned aerial vehicle UAV, thereby obtaining UGV data and UAV data;
  • Step S2 Analyze and process the UGV data to construct a UGV sub-map; analyze and process the UAV data to construct a UAV sub-map;
  • Step S3 Construct a mathematical model of the UGV sub-map according to the UGV sub-map; construct a mathematical model of the UAV sub-map according to the UAV sub-map; perform image video on the mathematical model of the UGV sub-map and the mathematical model of the UAV sub-map Detect closed-loop correlation processing to obtain the correlation matrix between the UGV submap and the UAV submap;
  • Step S4 Fusion of the UGV sub-map and the UAV sub-map according to the correlation matrix to obtain a global three-dimensional grid map.
  • this visual feature-based air-ground multi-robot map fusion method uses ground mobile robots and drones to independently collect data and create their own sub-maps; based on the corresponding sub-maps, corresponding sub-map mathematical models are constructed respectively. , perform image video detection closed-loop correlation processing on different sub-map mathematical models, and obtain the correlation matrix between the UGV sub-map and the UAV sub-map; then according to the correlation matrix, fuse the UGV sub-map and the UAV sub-map to obtain the global three-dimensional grid Grid map; the above methods use algorithms to construct local sub-maps through UGV and UAV respectively.
  • the local map has the characteristics of small error and high accuracy.
  • the respective sub-maps are fused through data association, and finally can be merged into a global consistency with higher accuracy.
  • the map makes the map obtained by this method more complete and has smaller errors.
  • Figure 1 is a schematic flow chart of the air-ground multi-robot map fusion method based on visual features provided by the present invention.
  • Figure 2 is a flow chart corresponding to the visual feature-based air-ground multi-robot map fusion method shown in Figure 1.
  • Figure 3 is a data association structure diagram of air-ground multi-robots in the air-ground multi-robot map fusion method based on visual features shown in Figure 1.
  • Figure 4 is a schematic diagram of the optimization situation corresponding to the sub-map pose transformation optimization function in the air-ground multi-robot map fusion method based on visual features shown in Figure 1.
  • Figures 5, 7, and 9 are respectively the submaps created by UGV corresponding to the actual experimental scenarios 1, 2, and 3 of the present invention.
  • Figures 6, 8, and 10 are respectively the submaps created by the UAV corresponding to the actual experimental scenarios 1, 2, and 3 of the present invention.
  • Figure 11 is a schematic diagram comparing the mapping results of the actual experimental scenario 1 of the present invention.
  • Figure 12 is a schematic diagram comparing the mapping results of the actual experimental scenario 2 of the present invention.
  • Figure 13 is a schematic diagram comparing the mapping results of the actual experimental scenario 3 of the present invention.
  • Figure 14 is a comparison chart of mapping accuracy errors for actual experimental scenarios 1, 2, and 3 of the present invention.
  • FIG. 1 is a schematic flow chart of an air-ground multi-robot map fusion method based on visual features provided by an embodiment of the present invention.
  • the visual feature-based air-ground multi-robot map fusion method includes the following steps:
  • Step S1 collect data independently through the ground mobile robot UGV and the unmanned aerial vehicle UAV, thereby obtaining UGV data and UAV data;
  • Step S2 Analyze and process the UGV data to construct a UGV sub-map; analyze and process the UAV data to construct a UAV sub-map;
  • Step S3 Construct a UGV sub-map mathematical model based on the UGV sub-map; construct a UAV sub-map mathematical model based on the UAV sub-map; perform image video detection closed-loop correlation processing on the UGV sub-map mathematical model and the UAV sub-map mathematical model to obtain the UGV sub-map The correlation matrix between the map and the UAV submap;
  • Step S4 Fusion of the UGV sub-map and the UAV sub-map according to the correlation matrix to obtain a global three-dimensional grid map.
  • the visual feature-based air-ground multi-robot map fusion method uses ground mobile robots and drones to independently collect data and create their own sub-maps; based on the corresponding sub-maps, the corresponding sub-map mathematics are constructed respectively. model, and perform image and video detection closed-loop correlation processing on different sub-map mathematical models to obtain the correlation matrix between the UGV sub-map and the UAV sub-map; then according to the correlation matrix, the UGV sub-map and the UAV sub-map are fused to obtain the global stereo Raster map; the above methods use algorithms to construct local sub-maps through UGV and UAV respectively.
  • the local map has the characteristics of small error and high accuracy.
  • the respective sub-maps are fused through data association, and finally can be merged into a globally consistent one with higher accuracy.
  • the map is more complete and has smaller errors.
  • Figure 2 is a flow chart corresponding to the visual feature-based air-ground multi-robot map fusion method shown in Figure 1.
  • Figure 2 simplifies the process of the visual feature-based air-ground multi-robot map fusion method of the present invention in the form of a flow block diagram. It is essentially the same as the above-mentioned Figure 1, so Figure 2 will not be described in detail here.
  • step S1 data collection is performed independently by the ground mobile robot UGV and the unmanned aerial vehicle UAV, so that the UGV data and UAV data specifically include:
  • the camera equipment of the UAV is instructed to capture the indoor scene and obtain UAV image data.
  • the ground mobile robot will be equipped with lidar equipment.
  • the lidar equipment When the ground robot moves on the ground in the indoor space, the lidar equipment will conduct laser scanning on the ground area of the indoor space. Radar scanning is performed to obtain corresponding lidar data; the lidar data includes laser point cloud data of the indoor space ground area.
  • the drone will be equipped with camera equipment such as a depth camera. When the drone is flying over the indoor space, the camera equipment will photograph the area above the indoor space to obtain corresponding image data; where, The image data includes images of the area above the indoor space.
  • step S2 analyzing and processing the UGV data, and constructing the UGV sub-map specifically includes:
  • the Cartographer algorithm is a commonly used SLAM calculation method.
  • a lidar data frame will be formed accordingly.
  • the radar data frame is processed by the Cartographer algorithm to generate the corresponding local submap.
  • the lidar device performs multiple lidar scans in succession, multiple local submaps will be formed.
  • the Cartographer algorithm is used to process UGV lidar data.
  • the corresponding local submap generated can include:
  • the lidar data frame formed after the latest lidar scan is obtained, it is matched with the current recently established local submap, so that the latest lidar data frame is inserted into the optimal position of the current recently established local submap.
  • h k represents the lidar data frame formed after the latest lidar scan
  • T ⁇ represents inserting the lidar data frame formed after the latest lidar scan into the current recently established local submap.
  • Rotation matrix K represents the total number of lidar data frames obtained by lidar scanning
  • M smooth represents the smoothing function
  • argmin represents the use of Gauss-Newton method to solve the least squares problem
  • the local submap is updated while continuously inserting the latest lidar data frame.
  • the current local submap is considered to have been created.
  • the local submap can also use Octomap, a three-dimensional grid map based on the octree data structure.
  • the above map consists of three three-dimensional grids: occupied, free and unknown; among them, each three-dimensional grid can be determined by the following formula (2) for real-time updates,
  • the L(.) function represents the logarithmic ratio (Log Odds Ratio);
  • z 1:t+1 ) represents the measured data z 1 between the known time 1 and the time t+1 :The logarithmic ratio of leaf node n under the condition of t+1 ;
  • z 1:t-1 ) represents the measured data z 1:t-1 of the leaf node n under the condition of known time 1 to time t-1
  • z t ) represents the logarithmic ratio of leaf node n under the condition of known measurement data z t at time t.
  • calculating the transformation matrix between two consecutive local sub-maps specifically includes: calculating two local sub-maps corresponding to two lidar data frames detected by two consecutive scans of the lidar device in time.
  • two different local sub-maps are formed by different laser radar scans, so there are inevitably differences in three-dimensional space coordinates between the two local sub-maps.
  • the three-dimensional space coordinate transformation matrix between them can ensure accurate combination between the two local sub-maps and effectively reduce the error caused by the combination.
  • step S2 according to the transformation matrix, all local sub-maps are jointly constructed to form an UGV sub-map, which specifically includes:
  • analyzing and processing the UAV data and constructing the UAV submap specifically includes:
  • each frame of UAV image data is associated with its corresponding shooting depth of field, and the depth information corresponding to the 2D feature points of each frame of image is extracted, thereby converting the 2D feature points of each frame of image into 3D Feature points;
  • the beneficial effects of the above technical solution are:
  • the RGBD SLAM method is a commonly used SLAM calculation method.
  • UAV image data in the form of a frame sequence will be obtained.
  • Each frame of the UAV image data corresponds to a different shooting depth of field one by one.
  • Each frame of the UAV image data is associated with its corresponding shooting depth of field, and the 2D feature points of each frame of image are converted into 3D feature points. It can realize the three-dimensional conversion of UAV image data, thereby improving the three-dimensional visual reliability of the map after subsequent map fusion.
  • the RGBD SLAM method specifically includes:
  • the SLAM front-end associates a frame of image with the corresponding depth data, uses the pinhole camera model to construct similar triangles, and obtains point cloud information after converting the internal parameters of the camera and the external parameters of the camera.
  • the local inter-frame matching of the front-end visual odometry is implemented using the PnP Ransac (Perspective-n-Point Random sample consensus) method, in which the feature description factors are initially screened through the Hamming distance and then combined with the random sampling consistency algorithm for matching.
  • the depth information of the filtered 2D feature points is restored, thereby converting them into 3D feature points, and then the pose transformation of the camera is calculated through 3D-3D point pairs.
  • the above local inter-frame matching uses beam adjustment method reprojection to optimize pose ⁇ , which can be characterized by the following formula (3),
  • ⁇ * represents the vector corresponding to the optimized pose
  • ⁇ represents the vector corresponding to the pre-optimized pose
  • ⁇ represents converting the vector into an antisymmetric matrix
  • K represents the camera internal parameter matrix
  • P i represents The i-th spatial coordinate point
  • u i represents the i-th pixel coordinate
  • exp represents the exponential mapping
  • n represents the total number of pixels contained in each frame of the image
  • argmin represents the use of Gauss-Newton method to solve the least squares problem
  • s i represents the th pixel depth of i pixels
  • the SLAM backend uses the pose graph optimization method to estimate the pose E of the robot, which is shown in the following formula (4),
  • step S3 based on the UGV sub-map, constructing the UGV sub-map mathematical model specifically includes:
  • point cloud data, image pixel data, image depth data and pose data are extracted from the UGV submap, thereby constructing a mathematical model of the UGV submap.
  • the corresponding point cloud data, image pixel data, image depth data and pose data can be expressed as
  • the beneficial effect of the above technical solution is: through the above method, in the spatial coordinate system corresponding to the UGV submap, point cloud data, image pixel data, image depth data and pose data are used as the four model elements of the UGV submap mathematical model. , can fully and effectively characterize the UGV sub-map mathematical model to the maximum extent, thereby improving the reliability of the construction of the UGV sub-map mathematical model; among them, point cloud data, image pixel data, image depth data and pose data are used to construct the corresponding sub-map data
  • the model belongs to the conventional technology in this field and will not be described in detail here.
  • step S3 based on the UAV submap, constructing the UAV submap mathematical model specifically includes:
  • point cloud data, image pixel data, image depth data and pose data are extracted from the UAV submap, thereby constructing a mathematical model of the UAV submap.
  • the corresponding point cloud data, image pixel data, image depth data and pose data can be expressed as
  • the beneficial effect of the above technical solution is: through the above method, in the spatial coordinate system corresponding to the UAV submap, point cloud data, image pixel data, image depth data and pose data are used as the four model elements of the UAV submap mathematical model. , can fully and effectively characterize the UAV sub-map mathematical model to the maximum extent, thereby improving the reliability of the construction of the UAV sub-map mathematical model; among them, point cloud data, image pixel data, image depth data and pose data are used to construct the corresponding sub-map data
  • the model belongs to the conventional technology in this field and will not be described in detail here.
  • both the pose data of UGV and the pose data of UAV can be expressed in the form of 4-ary numbers, which can be shown in the following formula (5),
  • t represents the timestamp
  • x, y, z represent the translation on the X, Y, and Z axes
  • q x , q y , q z , q w represent the rotation expressed in the form of quaternion .
  • the attitude information represented by the quaternion can be converted into a pose transformation matrix T, which includes a 3 ⁇ 3 rotation matrix R, a 3 ⁇ 1 translation vector t, and a dimensionally compatible vector I whose values are all 1, that is,
  • the above rotation matrix R can be expressed by the following formula (6),
  • Figure 3 is a data association structure diagram of air-ground multi-robots in the air-ground multi-robot map fusion method based on visual features shown in Figure 1.
  • Figure 3 completely reflects the correlation scheme between point cloud data, image pixel data, image depth data and pose data in multi-robots in the air.
  • SubmapC1 is the submap established by it. Represents the set of UGV poses and corresponding transformation matrices, Represents the image frame data collection, Represents other depth data, point cloud data, etc.
  • SubmapC2 represents the submap established by it, Represents the UAV pose and corresponding transformation matrix set, Represents the image frame data collection, Represents the depth data stream collection, and the double solid line box in the middle represents the image similarity detection part.
  • the single solid line represents the data correspondence
  • the dotted line represents the correlation relationship in constructing the initial solution
  • the thick line represents the image similarity matching part.
  • step S3 image video detection closed-loop correlation processing is performed on the UGV submap mathematical model and the UAV submap mathematical model, and the correlation matrix obtained between the UGV submap and the UAV submap specifically includes:
  • the corresponding ORB features are extracted from the corresponding image sequences of the UGV submap mathematical model and the UAV submap mathematical model;
  • the optimal solution of the transformation matrix for converting the coordinates of the UAV submap to the UGV submap is obtained, and the optimal solution of the transformation matrix is used as the correlation matrix between the UGV submap and the UAV submap.
  • Judgment basis that is, if the Hamming distance between the ORB features of the two corresponding images in the two image sequences is less than the preset threshold, it is determined that the corresponding two images belong to similar images; otherwise, it is determined that the corresponding two images are not belonging to similar images); then use all similar images as the benchmark to construct the global constraint relationship between the UGV sub-map and the UAV sub-map, and perform the corresponding least squares problem optimization solution on the global constraint relationship to obtain the
  • the coordinates of the UAV submap are converted to the optimal solution of the transformation matrix on the UGV submap, thereby achieving the same spatial coordinate fusion of the UAV submap and the UGV submap.
  • the construction of the above-mentioned global constraint relationship and the corresponding least squares problem optimization solution for the above-mentioned global constraint relationship are conventional technologies in this field and will not be described in detail here.
  • ORB features are extracted from image sequences in submaps created by UGV and UAV respectively, and descriptors are used to perform data association on them.
  • Hamming distance is used as the basis for judging image similarity. When the Hamming distance between ORB features is less than the preset, it is determined that the corresponding two images belong to similar images. At this time, the ORB feature association is successful and the statistics is successful. The number of matching ORB features; otherwise, it is determined that the corresponding two images do not belong to similar images.
  • the depth information in the UAV image data is used to calculate the 3D position information of the corresponding visual features, and then the EPNP (Estimation Perspective N Point) method is used to calculate the distance between it and the UGV image. Relative pose transformation.
  • EPNP uses multiple pairs of successfully matched feature information to calculate the robot's pose transformation. It uses the weighted representation of the feature three-dimensional coordinates of virtual control points in the world coordinate system, and calculates the robot's pose transformation by solving the coordinates of the control points in the camera reference coordinate system. , as shown in the following formula (7):
  • U and V represent the unitary matrix obtained after singular decomposition, and Indicates the centroid coordinates of the ORB feature point set under the two cameras.
  • the pose is obtained and ⁇ * is converted into a pose transformation matrix
  • ⁇ * is converted into a pose transformation matrix
  • FIG. 4 shows the optimization direction.
  • the initial solution and pose are put into the optimizer.
  • the optimization method is calculated based on formula (14)
  • L sets of transformation matrices are used to calculate the distance error and e l (x) of the feature point pairs in the UAV to UGV subgraph. After optimization, the matrix with the smallest error is obtained.
  • the cost function adopts the global error function under the matching image of the L group as follows:
  • This invention optimizes through the LM algorithm to obtain the globally optimal pose transformation matrix.
  • the LM algorithm needs to calculate the Jacobian matrix J(x), the derivative of the error term with respect to the optimized variable camera pose, and linearize it as follows:
  • g represents the rest of the expansion
  • ⁇ represents the adjustment factor
  • step S4 the UGV submap and the UAV submap are fused according to the correlation matrix to obtain the global three-dimensional grid map, which specifically includes:
  • the UAV sub-map is converted to the coordinate system of the UGV sub-map; then according to the three-dimensional grid map fusion method, the UGV sub-map and the UAV sub-map are globally map fused to obtain a global consistency map for the ground and air multi-robots. .
  • the correlation matrix is used as the coordinate transformation matrix corresponding to the coordinate system of the UAV submap converted to the UGV submap, so that the UAV submap can be integrated into the UGV submap and the integration can be ensured
  • the obtained global consistency map of ground-air multi-robots has integrity and coordination in both the ground area and the upper air area of the indoor scene, avoiding distortion and distortion of the fused map.
  • a global coordinate system is established based on the first sub-map of UGV, and all sub-maps established by UGV and UAV are converted to this coordinate system to construct a global map.
  • the corresponding pose transformation matrix T is:
  • the optimal transformation matrix from the subimage created by UAV to the jth subimage of UGV is Matrix converting UAV subgraph to global coordinate system As shown in the following formula (17):
  • the optimal transformation matrix from the submap created by UAV to the jth submap created by UGV is The submap created by UAV is converted to the matrix of the global coordinate system. As shown in the following formula,
  • the optimal transformation matrix Convert the UAV submap to the coordinate system of the UGV submap, and then The local sub-map fusion results of the air and ground are converted into the world coordinate system, and the global map fusion is performed according to the three-dimensional grid map fusion method, and finally the global consistency map of the air and ground multi-robots is obtained.
  • the visual feature-based air-ground multi-robot map fusion method uses ground mobile robots and drones to independently collect data and create respective sub-maps; based on the corresponding sub-maps, the corresponding sub-map mathematics are constructed respectively. model, and perform image and video detection closed-loop correlation processing on different sub-map mathematical models to obtain the correlation matrix between the UGV sub-map and the UAV sub-map; then according to the correlation matrix, the UGV sub-map and the UAV sub-map are fused to obtain the global stereo Raster map; the above methods use algorithms to construct local sub-maps through UGV and UAV respectively.
  • the local map has the characteristics of small error and high accuracy.
  • the respective sub-maps are fused through data association, and finally can be merged into a globally consistent one with higher accuracy.
  • the map is more complete and has smaller errors.
  • UGV Blue Whale Robot Xiaoqiang XQ4-PRO
  • the drone is Amu Laboratory P200.
  • the UGV is equipped with RFANS-32 lidar and D435i depth camera, and the UAV is equipped with D435i depth camera. Both systems use Ubuntu16.04 system, the ROS version is Kinect, and the data processing computer configuration is I7-4790K CPU, NVIDIA GeForce GTX 970 and Ubuntu18.04.
  • the sensor used by the UGV to sense the environment is Beike Tianhui's RFANS-32 line mechanical lidar.
  • the performance parameters are as shown in Table 1 below:
  • the main sensor for the UAV to perceive the environment is the D435i depth camera, which carries an inertial measurement unit (IMU).
  • IMU inertial measurement unit
  • submaps created by UGV are shown in Figures 6, 9, and 12.
  • the submaps created by UAV are shown in Figures 7, 10, and 13.
  • Each three-dimensional grid in the submap corresponds to 0.1 meters in the real environment.
  • FIGS 11, 12, and 13 are local enlarged close-ups of the results obtained by fusing the UGV and UAV submaps using the pre-optimization matrix in Section 3.3.3.
  • Figures 11, 12, and 13 (b1) and (b2) construct multiple constraints based on poses and features, and fuse the UGV and UAV submaps through the optimized transformation matrix to obtain a partial enlargement of the environment model. Comparing the renderings before and after optimization of the multi-robot submap fusion in the open space, it can be seen that the pose matrix achieves better submap fusion effect after optimization.
  • Figure 11, Figure 12, and Figure 13 (c1) are global maps created by a single UGV using the Cartographer method.
  • the robot scans repeatedly in a small local area.
  • the narrow indoor area limits the emission angle of the lidar, causing the sensor to be unable to sense all indoor areas, and has the disadvantage of being unable to fully model the environment.
  • This invention uses UAV to obtain a higher quality point cloud map, and then converts it into a three-dimensional raster map. Then the method proposed by the present invention is used to fuse the UAV sub-map and the map built by the UGV to obtain a global map with better effects.
  • the global map obtained by the air-ground multi-robot submap fusion method based on visual features is shown in (c2) in Figure 11, Figure 12, and Figure 13.
  • the frame line indicates the area where the UGV and UAV submaps are fused using the algorithm proposed in this article.
  • Several corner points were selected in the actual environment, the distance between the grid corner points was calculated, and the distance in the real environment was measured and compared. The results are shown in Table 3 below.
  • the mapping using lidar by a single UGV robot is incomplete and prone to errors, resulting in low accuracy.
  • a global three-dimensional grid map can be constructed more accurately and comprehensively.

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • Software Systems (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Geometry (AREA)
  • Multimedia (AREA)
  • Computer Graphics (AREA)
  • General Health & Medical Sciences (AREA)
  • Computing Systems (AREA)
  • Databases & Information Systems (AREA)
  • Evolutionary Computation (AREA)
  • Computational Linguistics (AREA)
  • Medical Informatics (AREA)
  • Artificial Intelligence (AREA)
  • Remote Sensing (AREA)
  • Health & Medical Sciences (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Image Processing (AREA)

Abstract

本发明提供了基于视觉特征的空地多机器人地图融合方法，其利用地面移动机器人和无人机独立采集数据并创建各自的子地图；根据相应的子地图，分别构建相应的子地图数学模型，并对不同子地图数学模型进行图像视频检测闭环关联处理，得到UGV子地图与UAV子地图之间的关联矩阵；再根据关联矩阵，对UGV子地图和UAV子地图进行融合，得到全局立体栅格地图；上述方法分别通过UGV和UAV使用算法构建局部子地图，局部地图拥有误差小、精度高的特性，通过数据关联将各自的子地图进行融合，最终能够合并成精度较高的全局一致性地图，使得利用本方法得到的地图更加完整，并且具有更小的误差。

Description

基于视觉特征的空地多机器人地图融合方法 技术领域
本发明涉及图像定位的技术领域，特别涉及基于视觉特征的空地多机器人地图融合方法。
背景技术
目前，单独使用地面移动机器人(Unmanned Ground Vehicle，UGV)或无人机(Unmanned Aerial Vehicle，UAV)解决室内大场景中定位与建图(Simultaneous Localization and Mapping，SLAM)问题的效果各有优劣。UGV具有负载大的优势，可以携带多个高性能传感器实现精准的环境建模，但是受限于对环境的通过性，UGV无法遍历全部环境以构建完整地图。相对应的，UAV具有良好的环境通过能力，通过搭载小型深度相机可以快速构建稠密的点云地图，但是续航弱，且搭载的传感器性能有限导致大场景下效果较差。
虽然使用地空多机器人能够有效解决SLAM存在的众多问题，但是UGV和UAV使用不同传感器所创建地图存在差异性，存在无法提取出相同特征实现子地图数据关联的问题，这无法有效合并形成精度较高的全局一致性地图。
发明内容
针对现有技术存在的缺陷，本发明提供基于视觉特征的空地多机器人地图融合方法。
本发明提供基于视觉特征的空地多机器人地图融合方法，其包括如下步骤：
步骤S1，通过地面移动机器人UGV和无人机UAV分别独立进行数据采集，从而得到UGV数据和UAV数据；
步骤S2，对所述UGV数据进行分析处理，构建形成UGV子地图；对所述UAV数据进行分析处理，构建形成UAV子地图；
步骤S3，根据所述UGV子地图，构建UGV子地图数学模型；根据所述UAV子地图，构建UAV子地图数学模型；对所述UGV子地图数学模型和所述UAV子地图数学模型进行图像视频检测闭环关联处理，得到所述UGV子地图与所述UAV子地图之间的关联矩阵；
步骤S4，根据所述关联矩阵，对所述UGV子地图和所述UAV子地图进行融合，得到全局立体栅格地图。
相比于现有技术，该基于视觉特征的空地多机器人地图融合方法利用地面移动机器人和无人机独立采集数据并创建各自的子地图；根据相应的子地图，分别构建相应的子地图数学模型，并对不同子地图数学模型进行图像视频检测闭环关联处理，得到UGV子地图与UAV子地图之间的关联矩阵；再根据关联矩阵，对UGV子地图和UAV子地图进行融合，得到全局立体栅格地图；上述方法分别通过UGV和UAV使用算法构建局部子地图，局部地图拥有误差小、精度高的特性，通过数据关联将各自的子地图进行融合，最终能够合并成精度较高的全局一致性地图，使得利用本方法得到的地图更加完整，并且具有更小的误差。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍。
图1为本发明提供的基于视觉特征的空地多机器人地图融合方法的流程示意图。
图2为图1所示的基于视觉特征的空地多机器人地图融合方法对应的流程框图。
图3为图1所示的基于视觉特征的空地多机器人地图融合方法中空地多机器人数据关 联结构图。
图4为图1所示的基于视觉特征的空地多机器人地图融合方法中子地图位姿变换优化函数对应的优化情况示意图。
图5，7，9分别为本发明实际的实验场景1，2，3对应的UGV创建的子地图。
图6，8，10分别为本发明实际的实验场景1，2，3对应的UAV创建的子地图。
图11为本发明实际的实验场景1的建图结果对比示意图。
图12为本发明实际的实验场景2的建图结果对比示意图。
图13为本发明实际的实验场景3的建图结果对比示意图。
图14为本发明实际的实验场景1，2，3的建图精度误差对比图。
具体实施方式
参阅图1，为本发明实施例提供的基于视觉特征的空地多机器人地图融合方法的流程示意图。该基于视觉特征的空地多机器人地图融合方法包括如下步骤：
步骤S1，通过地面移动机器人UGV和无人机UAV分别独立进行数据采集，从而得到UGV数据和UAV数据；
步骤S2，对UGV数据进行分析处理，构建形成UGV子地图；对UAV数据进行分析处理，构建形成UAV子地图；
步骤S3，根据UGV子地图，构建UGV子地图数学模型；根据UAV子地图，构建UAV子地图数学模型；对UGV子地图数学模型和UAV子地图数学模型进行图像视频检测闭环关联处理，得到UGV子地图与UAV子地图之间的关联矩阵；
步骤S4，根据关联矩阵，对UGV子地图和UAV子地图进行融合，得到全局立体栅格地图。
上述技术方案的有益效果为：该基于视觉特征的空地多机器人地图融合方法利用地面移动机器人和无人机独立采集数据并创建各自的子地图；根据相应的子地图，分别构建相应的子地图数学模型，并对不同子地图数学模型进行图像视频检测闭环关联处理，得到UGV子地图与UAV子地图之间的关联矩阵；再根据关联矩阵，对UGV子地图和UAV子地图进行融合，得到全局立体栅格地图；上述方法分别通过UGV和UAV使用算法构建局部子地图，局部地图拥有误差小、精度高的特性，通过数据关联将各自的子地图进行融合，最终能够合并成精度较高的全局一致性地图，使得利用本方法得到的地图更加完整，并且具有更小的误差。
请参阅图2，为图1所示的基于视觉特征的空地多机器人地图融合方法对应的流程框图。该图2以流程框图的形式简化地呈现出本发明的基于视觉特征的空地多机器人地图融合方法的过程，其与上述图1本质上是相同的，故这里不对图2进行详细的叙述。
优选地，在步骤S1中，通过地面移动机器人UGV和无人机UAV分别独立进行数据采集，从而得到UGV数据和UAV数据具体包括：
当地面移动机器人UGV在室内场景的地面进行移动过程中，指示地面移动机器人UGV的激光雷达设备对室内场景进行扫描检测，得到UGV激光雷达数据；
当无人机UAV在室内场景进行飞行过程中，指示无人机UAV的摄像设备对室内场景进行拍摄，得到UAV图像数据。
上述技术方案的有益效果为：在实际测量工作中，地面移动机器人上会安装有激光雷达设备，当地面机器人在室内空间的地面进行移动过程中，激光雷达设备会对室内空间的地面区域进行激光雷达扫描，从而得到相应的激光雷达数据；其中，该激光雷法数据包括室内空间地面区域的激光点云数据。相应地，无人机上会安装有深度相机等摄像设备，当无人机在室内空间的上空进行飞行过程中，摄像设备会对室内空间的上空区域进行拍摄，从而得到相应的图像数据；其中，该图像数据包括室内空间上空区域的影像。
优选地，在步骤S2中，对UGV数据进行分析处理，构建形成UGV子地图具体包括：
利用Cartographer算法对UGV激光雷达数据进行处理，根据UGV激光雷达数据包含的每个激光雷达数据帧，生成对应的局部子地图；
计算连续两个局部子地图之间的变换矩阵；再根据变换矩阵，将所有局部子地图，共同构建形成UGV子地图。
上述技术方案的有益效果为：Cartographer算法为常用的SLAM计算方法，在实际操作中，每当激光雷达设备完成一次激光雷达扫描后，会相应形成一个激光雷达数据帧，此时对当前形成的激光雷达数据帧进行Cartographer算法处理，生成对应的局部子地图，这样当激光雷达设备在连续进行多次激光雷达扫描后，会形成多个局部子地图。再计算连续连个局部子地图之间在三维空间上的变换矩阵，以此对所有局部子地图进行三维空间上的组合。
在实际应用中，利用Cartographer算法对UGV激光雷达数据进行处理，根据UGV激光雷达数据包含的每个激光雷达数据帧，生成对应的局部子地图可包括：
每当获取得到最新一次激光雷达扫描后形成的激光雷达数据帧后，便与当前最近建立的局部子地图进行匹配，使得上述最新的激光雷达数据帧插入到当前最近建立的局部子地图最优位置，其具体利用下面公式(1)来实现上述过程：
Figure PCTCN2022127636-appb-000001
在上述公式(1)中，h k表示最新一次激光雷达扫描后形成的激光雷达数据帧；T ξ表示将最新一次激光雷达扫描后形成的激光雷达数据帧插入到当前最近建立的局部子地图对应旋转矩阵；K表示激光雷达扫描得到的激光雷达数据帧的总数量；M smooth表示平滑处理函数；argmin表示使用高斯牛顿方法求解最小二乘问题；
通过上述公式(1)，在不断插入最新的激光雷达数据帧的同时更新局部子地图，当不再有新的激光雷达数据帧插入到局部子地图时，即认为当前局部子地图已经创建完成。
此外，局部子地图还可使用基于八叉树数据结构的立体栅格地图Octomap，上述地图由占据、空闲和未知三种立体栅格组成；其中，每一个立体栅格可通过下面公式(2)进行实时更新，
L(n|z 1：t+1)＝L(n|z 1：t-1)+L(n|z t)    (2)
在上述公式(2)中，L(.)函数表示对数比(Log Odds Ratio)；L(n|z 1：t+1)表示已知时刻1到时刻t+1之间测量数据z 1:t+1条件下叶子节点n的对数比；L(n|z 1：t-1)表示已知时刻1到时刻t-1之间测量数据z 1:t-1条件下叶子节点n的对数比；L(n|z t)表示已知时刻t时刻测量数据z t条件下叶子节点n的对数比。
优选地，在步骤S2中，计算连续两个局部子地图之间的变换矩阵具体包括：计算激光雷达设备在时间上连续的两次扫描检测得到的两个激光雷达数据帧分别对应的两个局部子地图之间的变换矩阵；其中，变换矩阵是指两个局部子地图在室内场景对应的三维空间上的三维空间坐标变换矩阵。
上述技术方案的有益效果为：两个不同局部子地图是分属不同激光雷达扫描形成的，这样两个局部子地图相互之间不可避免存在三维空间坐标的差异，通过计算连续两个局部子地图之间在三维空间坐标变换矩阵，能够保证两个局部子地图之间精确进行组合，有效减小组合产生的误差。
优选地，在步骤S2中，根据变换矩阵，将所有局部子地图，共同构建形成UGV子地图具体包括：
根据变换矩阵，对所有局部子地图中所有连续的两个局部子地图分别进行拼接，从而构建形成UGV子地图。
上述技术方案的有益效果为：通过上述方式，利用在三维空间的坐标变换矩阵，能够将所有局部子地图中所有相邻连续的两个局部子地图分别进行拼接，从而实现所有局部子地图的相互逐一拼接，保证最终形成的UGV子地图能够全面反映室内场景地面区域的真实情况。
优选地，在步骤S2中，对UAV数据进行分析处理，构建形成UAV子地图具体包括：
利用RGBD SLAM方法，将UAV图像数据的每一帧图像与其对应的拍摄景深进行关联，提取得到每一帧图像的2D特征点对应的深度信息，从而将每一帧图像的2D特征点转换为3D特征点；
再根据每一帧图像的所有3D特征点信息，将UAV图像数据的所有帧图像组合转换为3D形式的UAV子地图。
上述技术方案的有益效果为：RGBD SLAM方法为常用的SLAM计算方法，在实际操作中，当深度相机等摄像设备对室内场景的上空区域进行深度拍摄后，会得到帧序列形式的UAV图像数据，该UAV图像数据中的每一帧图像一一对应不同的拍摄景深，将UAV图像数据的每一帧图像与其对应的拍摄景深进行关联，并将每一帧图像的2D特征点转换3D特征点，能够实现UAV图像数据的三维转换，从而提高后续进行地图融合后的地图三维视觉可靠性。
在实际应用中，RGBD SLAM方法具体包括：
SLAM前端将一帧图像和对应的深度数据关联，利用针孔相机模型构建相似三角形，结合相机内参数、相机外参数转换后得到点云信息。前端视觉里程计的局部帧间匹配采用PnP Ransac(Perspective-n-Point Random sample consensus)的方式来实现，其中的特征描述因子通过汉明距离进行初步筛选后结合随机采样一致性算法进行匹配。还有，将筛选后的2D特征点恢复处深度信息，从而转变为3D特征点，再通过3D-3D点对来计算相机的位姿变换。上述局部帧间匹配采用光束平差法重投影进行优化位姿μ，其可用下面公式(3)表征，
Figure PCTCN2022127636-appb-000002
在上述公式(3)中，μ *表示优化后的位姿对应的向量；μ表示优化前的位姿对应的向量；^表示将向量转换为反对称矩阵；K表示相机内参矩阵；P i表示第i个空间坐标点；u i表示第i个像素坐标；exp表示指数映射；n表示每一帧图像包含的像素点总数量；argmin表示使用高斯牛顿方法求解最小二乘问题；s i表示第i个像素的像素深度；
Figure PCTCN2022127636-appb-000003
Figure PCTCN2022127636-appb-000004
表示计算每一个投影点在图像上的实际像素坐标u i与预测像素坐标
Figure PCTCN2022127636-appb-000005
之间的误差，利用所有像素坐标的二阶范数||.|| 2 2，使其取最小值的位姿，即为相机的最优姿态估计μ *；
SLAM后端则使用位姿图优化方法估计机器人的位姿E，其通过下面公式(4)所示，
Figure PCTCN2022127636-appb-000006
在上述公式(4)中，
Figure PCTCN2022127636-appb-000007
表示第i个待优化关键图像帧的位姿；T i,j表示第i个待优化关键图像帧与第j个待优化关键图像帧之间的位姿变换；
Figure PCTCN2022127636-appb-000008
表示利用位姿约束T i,j将第j个关键帧位姿x * j投影到第i个关键帧位姿x * i所在的坐标系中，计算它们之间的位姿误差的二阶范数||.|| 2 2，使其取最小值，即得到所有关键帧的最优状态估计E；
通过构建一个最小二乘问题优化所有位姿，使得E最小
Figure PCTCN2022127636-appb-000009
的就是机器人的最优状态估 计。
优选地，在步骤S3中，根据UGV子地图，构建UGV子地图数学模型具体包括：
以UGV子地图对应的坐标系C 1为基准，从UGV子地图中提取得到点云数据、图像像素数据、图像深度数据和位姿数据，从而构建形成UGV子地图数学模型。其中，对应的点云数据、图像像素数据、图像深度数据和位姿数据可分别表示为
Figure PCTCN2022127636-appb-000010
Figure PCTCN2022127636-appb-000011
上述技术方案的有益效果为：通过上述方式，在UGV子地图对应的空间坐标系中，以点云数据、图像像素数据、图像深度数据和位姿数据作为UGV子地图数学模型的四个模型元素，能够最大限度地全面有效地表征UGV子地图数学模型，从而提高UGV子地图数学模型的构建可靠性；其中，利用点云数据、图像像素数据、图像深度数据和位姿数据构建相应子地图数据模型属于本领域的常规技术，这里不做详细的叙述。
优选地，在步骤S3中，根据UAV子地图，构建UAV子地图数学模型具体包括：
以UAV子地图对应的坐标系C 2为基准，从UAV子地图中提取得到点云数据、图像像素数据、图像深度数据和位姿数据，从而构建形成UAV子地图数学模型。其中，对应的点云数据、图像像素数据、图像深度数据和位姿数据可分别表示为
Figure PCTCN2022127636-appb-000012
Figure PCTCN2022127636-appb-000013
上述技术方案的有益效果为：通过上述方式，在UAV子地图对应的空间坐标系中，以点云数据、图像像素数据、图像深度数据和位姿数据作为UAV子地图数学模型的四个模型元素，能够最大限度地全面有效地表征UAV子地图数学模型，从而提高UAV子地图数学模型的构建可靠性；其中，利用点云数据、图像像素数据、图像深度数据和位姿数据构建相应子地图数据模型属于本领域的常规技术，这里不做详细的叙述。
综上，UGV的位姿数据和UAV的位姿数据均可采用4元数的形式表示，其可如下面公式(5)所示，
{t,x,y,z,q x,q y,q z,q w}   (5)
在上述公式(5)中，t表示时间戳；x、y、z表示在X、Y、Z轴上的平移；q x、q y、q z、q w表示以四元数形式表示的旋转。四元数表示的姿态信息可以转换为位姿变换矩阵T，包含3×3的旋转矩阵R、3×1的平移向量t和维数兼容的值全为1的向量I，即
Figure PCTCN2022127636-appb-000014
其中，上述旋转矩阵R可通过下面公式(6)表示，
Figure PCTCN2022127636-appb-000015
请参阅图3所示，为图1所示的基于视觉特征的空地多机器人地图融合方法中空地多机器人数据关联结构图。该图3完整反映了空地多机器人中点云数据、图像像素数据、图像深度数据和位姿数据之间的关联方案。在该图3中，对于UGV采集的数据，SubmapC1是其建立的子地图，
Figure PCTCN2022127636-appb-000016
代表UGV位姿及对应变换矩阵集合，
Figure PCTCN2022127636-appb-000017
代表图像帧数据集合，
Figure PCTCN2022127636-appb-000018
代表其它深度数据、点云数据等。对于UAV采集的数据，SubmapC2代表其建立的子地图，
Figure PCTCN2022127636-appb-000019
代表UAV位姿及对应变换矩阵集合，
Figure PCTCN2022127636-appb-000020
代表图像帧数据集合，
Figure PCTCN2022127636-appb-000021
代表深度数据流集合，中间的双实线框代表图像相似性检测部分。单实线代表数据对应关系，点划线代表构建初始解的关联关系，粗线代表图像相似度匹配部分。
优选地，在步骤S3中，对UGV子地图数学模型和UAV子地图数学模型进行图像视频检测闭环关联处理，得到UGV子地图与UAV子地图之间的关联矩阵具体包括：
从UGV子地图数学模型和UAV子地图数学模型各自对应的图像序列中分别提取得到相应的ORB特征；
基于ORB特征，确定UGV子地图与UAV子地图之间所有相似的图像；
根据UGV子地图与UAV子地图之间所有相似的图像，构建UGV子地图与UAV子地图之间的全局约束；
根据全局约束，得到将UAV子地图进行坐标转换到UGV子地图上的变换矩阵最优解，并将变换矩阵最优解作为UGV子地图与UAV子地图之间的关联矩阵。
上述技术方案的有益效果为：通过上述方式，以UGV子地图数学模型和UAV子地图数学模型各自对应的图像序列的ORB特征为基准，并利用汉明距离作为两个图像序列中图像相似性的判断依据(即若两个图像序列中对应的两个图像的ORB特征之间的汉明距离小于预设阈值，则确定对应的两个图像属于相似的图像；否则，确定对应的两个图像不属于相似的图像)；再以所有相似的图像为基准，构建UGV子地图与UAV子地图之间的全局约束关系，并且对该全局约束关系进行相应的最小二乘问题优化解算，能够得到将UAV子地图进行坐标转换到UGV子地图上的变换矩阵最优解，从而实现对UAV子地图与UGV子地图的同一空间坐标融合。其中，构建上述全局约束关系和对上述全局约束关系进行相应的最小二乘问题优化解算均属于本领域的常规技术，这里不做详细的叙述。
在实际应用中，分别从UGV和UAV创建的子地图中的图像序列提取ORB特征，使用描述子对它们进行数据关联。使用汉明距离作为图像相似性的判断依据，当ORB特征之间的汉明距离小于预设预设，则确定对应的两个图像属于相似的图像，此时判读ORB特征关联成功，以及统计成功匹配的ORB特征数量；否则，确定对应的两个图像不属于相似的图像。
比如，当UGV和UAV里对应的图像分别是
Figure PCTCN2022127636-appb-000022
和
Figure PCTCN2022127636-appb-000023
时，它们之间成功关联的ORB特征的数量记为
Figure PCTCN2022127636-appb-000024
计算子地图中所有图像之间的特征关联数
Figure PCTCN2022127636-appb-000025
之后，其中n，m分别表示UGV子地图和UAV子地图各自包含的图像数量，最终得到满足一定特征关联数的L个成功关联的图像对，形成如下集合：
Figure PCTCN2022127636-appb-000026
在得到UGV和UAV子地图中的关联图像对之后，利用UAV图像数据中的深度信息计算对应视觉特征的3D位置信息，之后，采用EPNP(Estimation Perspective N Point)方法计算它与UGV图像之间的相对位姿变换。
EPNP利用多对匹配成功的特征信息计算机器人位姿变换，在世界坐标系中使用虚拟控制点的加权表示特征三维坐标，通过求解控制点在相机参考坐标系下的坐标来计算机器人的位姿变换，如下面公式(7)所示：
Figure PCTCN2022127636-appb-000027
在上述公式(7)中，U和V表示奇异分解后得到的酉矩阵，
Figure PCTCN2022127636-appb-000028
和
Figure PCTCN2022127636-appb-000029
表示两个相机下ORB特征点集的质心坐标。
再输入两幅匹配好的图像
Figure PCTCN2022127636-appb-000030
和
Figure PCTCN2022127636-appb-000031
以及对应的深度数据流
Figure PCTCN2022127636-appb-000032
到EPNP中，计算得到UAV相机图像
Figure PCTCN2022127636-appb-000033
变换到UGV相机图像
Figure PCTCN2022127636-appb-000034
的位姿变换，再通过各自的相机坐标系转换为UAV和UGV基础坐标系之间的变换关系T k。
将位姿变换矩阵T k转换为旋转矩阵，并通过反对称以李代数形式的位姿ξ表示，使用BA构建最小二乘问题对其优化，其如下面公式(8)所示，
Figure PCTCN2022127636-appb-000035
在上述公式(8)中，ξ *表示优化后的位姿对应的向量；^表示将向量转换为反对称矩阵；K表示相机内参矩阵；P j表示第j个空间坐标点；u j表示第j个像素坐标；exp表示指数映射；n表示每一帧图像包含的像素点总数量；argmin表示使用高斯牛顿方法求解最小二乘问题； s j表示第j个像素的像素深度；
Figure PCTCN2022127636-appb-000036
表示计算每一个投影点在图像上的实际像素坐标u j与预测像素坐标
Figure PCTCN2022127636-appb-000037
之间的误差，利用所有像素坐标的二阶范数||.|| 2 2，使其取最小值的位姿，即为相机的最优姿态估计ξ *。
在优化结束后得到位姿，将ξ *转换为位姿变换矩阵
Figure PCTCN2022127636-appb-000038
选取具备最多特征关联束的图像对，利用里程计算信息计算它们之间的初始位姿变换矩阵，基于关联特征形成的约束进行优化，最后得到最优的位姿变换矩阵
Figure PCTCN2022127636-appb-000039
具体地，根据上述公式(5)和(6)将将UGV在i k的位姿矩阵化得到
Figure PCTCN2022127636-appb-000040
将UAV在j k的位姿矩阵化得到
Figure PCTCN2022127636-appb-000041
根据上述公式
Figure PCTCN2022127636-appb-000042
中匹配的图像
Figure PCTCN2022127636-appb-000043
计算UAV子图坐标系到UGV子图的初始变换矩阵为
Figure PCTCN2022127636-appb-000044
其如下所示：
Figure PCTCN2022127636-appb-000045
在上述公式(9)中，
Figure PCTCN2022127636-appb-000046
表示匹配图像之间的相机位姿变换。
再根据下面公式(10)中的所有L对相似图像集合计算每个
Figure PCTCN2022127636-appb-000047
得到矩阵集合如下：
Figure PCTCN2022127636-appb-000048
基于上式建立关于子地图位姿变换优化函数，选定具备最多特征关联数的初始解，基优化后得到
Figure PCTCN2022127636-appb-000049
Figure PCTCN2022127636-appb-000050
从图4可以得到上述公式(11)对应的优化情况，在该图4所示为优化方向，将初始解和位姿放入到优化器中，优化的方法是依据式(14)计算得到的L组变换矩阵，计算从UAV转换到UGV子图中特征点对的距离误差和e l(x)，优化后得到这个误差最小的矩阵。
代价函数采用如下的L组的匹配图像下的全局误差函数：
Figure PCTCN2022127636-appb-000051
本发明通过LM算法进行优化，得到全局最优的位姿变换矩阵。
其中，LM算法需要计算误差项关于优化变量相机位姿的导数雅克比矩阵J(x)，将其线性化后如下式(13)：
E(x+Δx)≈E(x)+J(x)Δx   (13)
将最小二乘问题展开对关于Δx的求导，令其为0，问题转化为下式(14)：
(H+λI)Δx＝g   (14)
在上述公式(14)中，H＝J(x) TJ(x)表示海森矩阵，g表示展开的其余部分，λ表示调节因子。
最后当优化误差与上一次优化的结果在一个很小的范围δ内，或者达到优化次数上限，算法退出，如下式(15)：
|E(x+Δx)-E(x)|<δ   (15)
从而在优化完毕后，得到最优的坐标系变换矩阵
Figure PCTCN2022127636-appb-000052
优选地，在步骤S4中，根据关联矩阵，对UGV子地图和UAV子地图进行融合，得到全局立体栅格地图具体包括：
根据关联矩阵，将UAV子地图转换到UGV子地图的坐标系后；再按照立体栅格地图融合方法，将UGV子地图和UAV子地图进行全局地图融合，从而得到地空多机器人全局一致性地图。
上述技术方案的有益效果为：通过上述方式，以关联矩阵作为UAV子地图转换到UGV子地图的坐标系对应的坐标变换矩阵，这样可将UAV子地图融合统一到UGV子地图中，并且确保融合得到的地空多机器人全局一致性地图在室内场景的地面区域和上空区域均具有完整性和协调性，避免融合得到地图发生扭曲和失真。
在实际应用中，以UGV的第一个子地图为基准建立全局坐标系，将UGV和UAV建立 的所有子地图转换到该坐标系下构建全局地图。对于UGV所创建的连续子地图，相应的位姿变换矩阵T为：
Figure PCTCN2022127636-appb-000053
在上述公式(16)中，
Figure PCTCN2022127636-appb-000054
表示第i个子地图相对于第1个子地图的位姿变换；
Figure PCTCN2022127636-appb-000055
表示单位矩阵。
UAV创建的子图到UGV第j张子图的最优变换矩阵为
Figure PCTCN2022127636-appb-000056
UAV子图转换到全局坐标系的矩阵
Figure PCTCN2022127636-appb-000057
如下式(17)所示：
Figure PCTCN2022127636-appb-000058
综上，首先根据最优变换矩阵
Figure PCTCN2022127636-appb-000059
将UAV子地图转换到UGV子地图的坐标系下，然后根据
Figure PCTCN2022127636-appb-000060
将空地局部子地图融合结果转换到世界坐标系下，按照立体栅格地图融合方法进行全局地图融合，最后得到空地多机器人全局一致性地图。
UAV创建的子地图到UGV创建的第j张子地图的最优变换矩阵为
Figure PCTCN2022127636-appb-000061
UAV创建的子地图转换到全局坐标系的矩阵
Figure PCTCN2022127636-appb-000062
如下式所示，
Figure PCTCN2022127636-appb-000063
首先根据最优变换矩阵
Figure PCTCN2022127636-appb-000064
将UAV子地图转换到UGV子地图的坐标系下，然后根据
Figure PCTCN2022127636-appb-000065
将空地局部子地图融合结果转换到世界坐标系下，按照立体栅格地图融合方法进行全局地图融合，最后得到空地多机器人全局一致性地图。
从上述实施例的内容可知，该基于视觉特征的空地多机器人地图融合方法利用地面移动机器人和无人机独立采集数据并创建各自的子地图；根据相应的子地图，分别构建相应的子地图数学模型，并对不同子地图数学模型进行图像视频检测闭环关联处理，得到UGV子地图与UAV子地图之间的关联矩阵；再根据关联矩阵，对UGV子地图和UAV子地图进行融合，得到全局立体栅格地图；上述方法分别通过UGV和UAV使用算法构建局部子地图，局部地图拥有误差小、精度高的特性，通过数据关联将各自的子地图进行融合，最终能够合并成精度较高的全局一致性地图，使得利用本方法得到的地图更加完整，并且具有更小的误差。
还有，为了验证本发明中的算法有效性，利用UGV和UAV在三个场景中进行了实验验证。UGV为蓝鲸机器人小强XQ4-PRO，无人机为阿木实验室P200。其中UGV搭载了RFANS-32激光雷达与D435i深度相机，UAV搭载了D435i深度相机。两者系统均采用Ubuntu16.04系统，ROS版本为Kinect，数据处理的计算机配置为I7-4790K CPU，NVIDIA GeForce GTX 970和Ubuntu18.04。
UGV感知环境采用的传感器是北科天绘RFANS-32线机械式激光雷达，性能参数如下面表1：
表1 RFANS-32性能参数
Figure PCTCN2022127636-appb-000066
UAV感知环境的主要传感器是D435i深度相机，该相机携带了惯性测量单元(Inertial Measurement Unit,IMU)，性能参数如下面表2：
表2 D435i性能参数
Figure PCTCN2022127636-appb-000067
进一步，UGV创建的子地图如图6、9、12所示。UAV创建的子地图如图7、10、13所示，子地图中每一个立体栅格对应真实环境中的0.1米。
对UGV和UAV子地图的建图结果分析如下：
图11、图12、图13中的(a1)和(a2)是使用3.3.3节中优化前矩阵对UGV和UAV子地图融合得到的结果局部放大特写。图11、图12、图13的(b1)和(b2)是根据位姿与特征构建多重约束，通过优化后的变换矩阵对UGV和UAV子地图融合得到环境模型的局部放大图。对比空地多机器人子地图融合优化前后的效果图可知，位姿矩阵在优化后实现了更好的子地图融合效果。
图11、图12、图13的(c1)是单UGV使用Cartographer方法建立的全局地图。UGV在室内环境采集数据时，机器人在局部小范围内反复扫描，室内狭窄区域限制了激光雷达的发射角，导致传感器无法感知室内所有区域，存在无法对环境进行完整建模的缺点。本发明使用UAV获得质量较高的点云地图，然后转化为立体栅格图。然后使用本发明所提方法将UAV子地图与UGV所建地图进行融合，得到效果更好的全局地图。
基于视觉特征的空地多机器人子地图融合方法得到的全局地图如图11、图12、图13中的(c2)所示，框线表示使用本文所提算法融合UGV和UAV子地图的区域。在实际环境中选取若干角点，计算了栅格角点之间的距离，并对真实环境距离进行测量与之对比，结果如下面表3所示。
表3 建图精度对比
Figure PCTCN2022127636-appb-000068
计算表3中场景1、2、3的建图精度算法与真实距离的最大误差和平均误差，结果如表4、图14所示。
表4 建图精度误差对比
Figure PCTCN2022127636-appb-000069
由表4、图14可知，在场景1、2、3中，本文所提的空地多机器人子地图融合方法的最大误差与平均误差均小于单机器人Cartographer方法。
经实验结果分析，单UGV机器人使用激光雷达建图不完整，且易产生误差导致精度较低。使用本发明所提的空地多机器人子地图融合方法可以更加精准、全面地构建全局立体栅格地图。

Claims (10)

  1. 基于视觉特征的空地多机器人地图融合方法，其特征在于包括如下步骤：
    步骤S1，通过地面移动机器人UGV和无人机UAV分别独立进行数据采集，从而得到UGV数据和UAV数据；
    步骤S2，对所述UGV数据进行分析处理，构建形成UGV子地图；对所述UAV数据进行分析处理，构建形成UAV子地图；
    步骤S3，根据所述UGV子地图，构建UGV子地图数学模型；根据所述UAV子地图，构建UAV子地图数学模型；对所述UGV子地图数学模型和所述UAV子地图数学模型进行图像视频检测闭环关联处理，得到所述UGV子地图与所述UAV子地图之间的关联矩阵；
    步骤S4，根据所述关联矩阵，对所述UGV子地图和所述UAV子地图进行融合，得到全局立体栅格地图。
  2. 如权利要求1所述的方法，其特征在于：
    在所述步骤S1中，通过地面移动机器人UGV和无人机UAV分别独立进行数据采集，从而得到UGV数据和UAV数据具体包括：
    当地面移动机器人UGV在室内场景的地面进行移动过程中，指示所述地面移动机器人UGV的激光雷达设备对室内场景进行扫描检测，得到UGV激光雷达数据；
    当无人机UAV在所述室内场景进行飞行过程中，指示所述无人机UAV的摄像设备对所述室内场景进行拍摄，得到UAV图像数据。
  3. 如权利要求2所述的方法，其特征在于：
    在所述步骤S2中，对所述UGV数据进行分析处理，构建形成UGV子地图具体包括：
    利用Cartographer算法对所述UGV激光雷达数据进行处理，根据UGV激光雷达数据包含的每个激光雷达数据帧，生成对应的局部子地图；
    计算连续两个局部子地图之间的变换矩阵；再根据所述变换矩阵，将所有局部子地图，共同构建形成UGV子地图。
  4. 如权利要求3所述的方法，其特征在于：
    在所述步骤S2中，计算连续两个局部子地图之间的变换矩阵具体包括：
    计算所述激光雷达设备在时间上连续的两次扫描检测得到的两个激光雷达数据帧分别对应的两个局部子地图之间的变换矩阵；其中，所述变换矩阵是指两个局部子地图在室内场景对应的三维空间上的三维空间坐标变换矩阵。
  5. 如权利要求4所述的方法，其特征在于：
    在所述步骤S2中，根据所述变换矩阵，将所有局部子地图，共同构建形成UGV子地图具体包括：
    根据所述变换矩阵，对所有局部子地图中所有连续的两个局部子地图分别进行拼接，从而构建形成UGV子地图。
  6. 如权利要求5所述的方法，其特征在于：
    在所述步骤S2中，对所述UAV数据进行分析处理，构建形成UAV子地图具体包括：
    利用RGBD SLAM方法，将所述UAV图像数据的每一帧图像与其对应的拍摄景深进行关联，提取得到每一帧图像的2D特征点对应的深度信息，从而将每一帧图像的2D特征点转换为3D特征点；
    再根据每一帧图像的所有3D特征点信息，将所述UAV图像数据的所有帧图像组合转换为3D形式的UAV子地图。
  7. 如权利要求6所述的方法，其特征在于：
    在所述步骤S3中，根据所述UGV子地图，构建UGV子地图数学模型具体包括：
    以所述UGV子地图对应的坐标系C 1为基准，从所述UGV子地图中提取得到点云数据、图像像素数据、图像深度数据和位姿数据，从而构建形成UGV子地图数学模型。
  8. 如权利要求7所述的方法，其特征在于：
    在所述步骤S3中，根据所述UAV子地图，构建UAV子地图数学模型具体包括：
    以所述UAV子地图对应的坐标系C 2为基准，从所述UAV子地图中提取得到点云数据、图像像素数据、图像深度数据和位姿数据，从而构建形成UAV子地图数学模型。
  9. 如权利要求8所述的方法，其特征在于：
    在所述步骤S3中，对所述UGV子地图数学模型和所述UAV子地图数学模型进行图像视频检测闭环关联处理，得到所述UGV子地图与所述UAV子地图之间的关联矩阵具体包括：
    从所述UGV子地图数学模型和所述UAV子地图数学模型各自对应的图像序列中分别提取得到相应的ORB特征；
    基于所述ORB特征，确定所述UGV子地图与所述UAV子地图之间所有相似的图像；
    根据所述UGV子地图与所述UAV子地图之间所有相似的图像，构建所述UGV子地图与所述UAV子地图之间的全局约束；
    根据所述全局约束，得到将所述UAV子地图进行坐标转换到所述UGV子地图上的变换矩阵最优解，并将所述变换矩阵最优解作为所述UGV子地图与所述UAV子地图之间的关联矩阵。
  10. 如权利要求9所述的方法，其特征在于：
    在所述步骤S4中，根据所述关联矩阵，对所述UGV子地图和所述UAV子地图进行融合，得到全局立体栅格地图具体包括：
    根据所述关联矩阵，将所述UAV子地图转换到所述UGV子地图的坐标系后；再按照立体栅格地图融合方法，将所述UGV子地图和所述UAV子地图进行全局地图融合，从而得到地空多机器人全局一致性地图。
PCT/CN2022/127636 2022-07-05 2022-10-26 基于视觉特征的空地多机器人地图融合方法 Ceased WO2024007485A1 (zh)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN202210791822.0 2022-07-05
CN202210791822.0A CN115222905B (zh) 2022-07-05 2022-07-05 基于视觉特征的空地多机器人地图融合方法

Publications (1)

Publication Number Publication Date
WO2024007485A1 true WO2024007485A1 (zh) 2024-01-11

Family

ID=83610553

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2022/127636 Ceased WO2024007485A1 (zh) 2022-07-05 2022-10-26 基于视觉特征的空地多机器人地图融合方法

Country Status (2)

Country Link
CN (1) CN115222905B (zh)
WO (1) WO2024007485A1 (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN119085646A (zh) * 2024-08-15 2024-12-06 同济大学 基于相对坐标系不确定性估计的多机器人定位与建图方法
CN120852515A (zh) * 2025-07-01 2025-10-28 安徽工程大学 一种动态场景下基于特征强化与运动判断的slam算法、存储介质及设备
CN121353463A (zh) * 2025-12-17 2026-01-16 东华大学 基于语义特征和地平面多约束融合的地空协同建图方法

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115222905B (zh) * 2022-07-05 2024-03-22 苏州大学 基于视觉特征的空地多机器人地图融合方法
CN115965673B (zh) * 2022-11-23 2023-09-12 中国建筑一局(集团)有限公司 基于双目视觉的集中式多机器人定位方法
CN116045962A (zh) * 2022-12-30 2023-05-02 南开大学 基于雾网络的建图定位方法及装置

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103632606A (zh) * 2012-08-27 2014-03-12 联想(北京)有限公司 信息处理方法和装置
CN103941750A (zh) * 2014-04-30 2014-07-23 东北大学 基于小型四旋翼无人机的构图装置及方法
CN110119144A (zh) * 2019-04-19 2019-08-13 苏州大学 基于子地图特征匹配的多机器人slam算法
CN111413965A (zh) * 2020-03-11 2020-07-14 西安工程大学 一种基于uav协同感知的ugv行驶路径规划方法
CN115222905A (zh) * 2022-07-05 2022-10-21 苏州大学 基于视觉特征的空地多机器人地图融合方法

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106595659A (zh) * 2016-11-03 2017-04-26 南京航空航天大学 城市复杂环境下多无人机视觉slam的地图融合方法
CN110110763B (zh) * 2019-04-19 2023-06-23 苏州大学 一种基于最大公共子图的栅格地图融合方法
CN113108773A (zh) * 2021-04-22 2021-07-13 哈尔滨理工大学 一种融合激光与视觉传感器的栅格地图构建方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103632606A (zh) * 2012-08-27 2014-03-12 联想(北京)有限公司 信息处理方法和装置
CN103941750A (zh) * 2014-04-30 2014-07-23 东北大学 基于小型四旋翼无人机的构图装置及方法
CN110119144A (zh) * 2019-04-19 2019-08-13 苏州大学 基于子地图特征匹配的多机器人slam算法
CN111413965A (zh) * 2020-03-11 2020-07-14 西安工程大学 一种基于uav协同感知的ugv行驶路径规划方法
CN115222905A (zh) * 2022-07-05 2022-10-21 苏州大学 基于视觉特征的空地多机器人地图融合方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
CHEN, YIBIN: "Air-ground Fusion Collaborative SLAM method based on Multi-modal Information", 31 December 2019 (2019-12-31), CN, pages 1 - 78, XP009551891 *
HE, YANBING.: "Research on Collaborative Mapping Technology of Air-ground Robots based on Multi-sensor Fusion", MASTER'S THESIS OF CHINA UNIVERSITY OF MINING AND TECHNOLOGY, 30 June 2022 (2022-06-30), China, pages 1 - 96, XP009551847, DOI: 10.27623/d.cnki.gzkyu.2022.000232 *

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN119085646A (zh) * 2024-08-15 2024-12-06 同济大学 基于相对坐标系不确定性估计的多机器人定位与建图方法
CN120852515A (zh) * 2025-07-01 2025-10-28 安徽工程大学 一种动态场景下基于特征强化与运动判断的slam算法、存储介质及设备
CN121353463A (zh) * 2025-12-17 2026-01-16 东华大学 基于语义特征和地平面多约束融合的地空协同建图方法

Also Published As

Publication number Publication date
CN115222905B (zh) 2024-03-22
CN115222905A (zh) 2022-10-21

Similar Documents

Publication Publication Date Title
WO2024007485A1 (zh) 基于视觉特征的空地多机器人地图融合方法
CN110189399B (zh) 一种室内三维布局重建的方法及系统
CN108537848B (zh) 一种面向室内场景重建的两级位姿优化估计方法
CN112435325A (zh) 基于vi-slam和深度估计网络的无人机场景稠密重建方法
CN110992487B (zh) 手持式飞机油箱快速三维地图重建装置及重建方法
Ding et al. Research on computer vision enhancement in intelligent robot based on machine learning and deep learning
CN109579843A (zh) 一种空地多视角下的多机器人协同定位及融合建图方法
CN107291093A (zh) 基于视觉slam的复杂环境下无人机自主降落区域选择方法
CN119478297B (zh) 一种基于5g飞控的实时三维建模方法、系统及介质
CN110334701A (zh) 数字孪生环境下基于深度学习和多目视觉的数据采集方法
CN108648194A (zh) 基于cad模型三维目标识别分割和位姿测量方法及装置
CN118736128A (zh) 一种基于无人机图像的目标快速三维建模系统
CN119314068A (zh) 无人机单目视觉森林火灾定位方法
CN111340942A (zh) 一种基于无人机的三维重建系统及其方法
Kostavelis et al. Visual odometry for autonomous robot navigation through efficient outlier rejection
CN113790711B (zh) 一种无人机低空飞行位姿无控多视测量方法及存储介质
CN120912759A (zh) 一种基于无人机图像和监控视频的工地三维场景重建方法
CN114529585A (zh) 基于深度视觉和惯性测量的移动设备自主定位方法
CN117470259A (zh) 一种子母式空地协同多传感器融合三维建图系统
CN115690623A (zh) 一种基于三维重建的远距离目标毁伤评估方法
CN120467335A (zh) 多视点自主协同定位方法及系统
Parvaz et al. Airborne cross-source point clouds fusion by slice-to-slice adjustment
CN107103582B (zh) 机器人视觉导航定位图像特征点的匹配方法
JP2012242315A (ja) 位置検知システム、それを用いた移動体、位置検知方法およびそのためのプログラム
CN115326073A (zh) 基于语义和多物体误差最小动态slam方法及机器人

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 22950044

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 22950044

Country of ref document: EP

Kind code of ref document: A1