CN116989772A - Air-ground multi-mode multi-agent cooperative positioning and mapping method - Google Patents
Air-ground multi-mode multi-agent cooperative positioning and mapping method Download PDFInfo
- Publication number
- CN116989772A CN116989772A CN202311248105.4A CN202311248105A CN116989772A CN 116989772 A CN116989772 A CN 116989772A CN 202311248105 A CN202311248105 A CN 202311248105A CN 116989772 A CN116989772 A CN 116989772A
- Authority
- CN
- China
- Prior art keywords
- aerial vehicle
- unmanned aerial
- map
- optimization
- fusion
- 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.)
- Granted
Links
- 238000013507 mapping Methods 0.000 title claims abstract description 36
- 238000000034 method Methods 0.000 title claims abstract description 34
- 230000000007 visual effect Effects 0.000 claims abstract description 58
- 239000003795 chemical substances by application Substances 0.000 claims abstract description 54
- 230000004927 fusion Effects 0.000 claims abstract description 43
- 238000005259 measurement Methods 0.000 claims abstract description 38
- 238000005457 optimization Methods 0.000 claims abstract description 37
- 238000001514 detection method Methods 0.000 claims abstract description 22
- 239000003550 marker Substances 0.000 claims abstract description 17
- 238000006243 chemical reaction Methods 0.000 claims abstract description 15
- 239000013598 vector Substances 0.000 claims description 37
- 230000003287 optical effect Effects 0.000 claims description 17
- 238000004590 computer program Methods 0.000 claims description 12
- 230000010354 integration Effects 0.000 claims description 11
- 230000001133 acceleration Effects 0.000 claims description 6
- 238000004422 calculation algorithm Methods 0.000 claims description 5
- 230000006870 function Effects 0.000 claims description 4
- 230000005484 gravity Effects 0.000 claims description 4
- 238000013459 approach Methods 0.000 claims description 3
- 238000007781 pre-processing Methods 0.000 claims description 3
- 238000013519 translation Methods 0.000 claims description 3
- 230000008045 co-localization Effects 0.000 claims 2
- 230000008447 perception Effects 0.000 description 5
- 238000010586 diagram Methods 0.000 description 4
- 230000008569 process Effects 0.000 description 3
- 238000004364 calculation method Methods 0.000 description 2
- 230000007613 environmental effect Effects 0.000 description 2
- 238000003384 imaging method Methods 0.000 description 2
- 239000000463 material Substances 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 230000004044 response Effects 0.000 description 2
- 238000012876 topography Methods 0.000 description 2
- 230000009471 action Effects 0.000 description 1
- 230000002411 adverse Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 230000008878 coupling Effects 0.000 description 1
- 238000010168 coupling process Methods 0.000 description 1
- 238000005859 coupling reaction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 239000000284 extract Substances 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 239000011159 matrix material Substances 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 239000013589 supplement Substances 0.000 description 1
- 238000009827 uniform distribution Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/38—Electronic maps specially adapted for navigation; Updating thereof
- G01C21/3804—Creation or updating of map data
- G01C21/3833—Creation or updating of map data characterised by the source of data
- G01C21/3841—Data obtained from two or more sources, e.g. probe vehicles
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
- G01C21/1656—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with passive imaging devices, e.g. cameras
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/38—Electronic maps specially adapted for navigation; Updating thereof
- G01C21/3804—Creation or updating of map data
- G01C21/3807—Creation or updating of map data characterised by the type of data
- G01C21/3826—Terrain data
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The application provides a method for collaborative positioning and mapping of air-ground multi-mode multi-agent, which comprises the steps of obtaining measurement data of an agent, wherein the agent comprises an unmanned aerial vehicle and an unmanned vehicle, and the unmanned vehicle is provided with a visual marker; carrying out local view angle local map building through measurement data of the unmanned aerial vehicle, and carrying out global view angle local map building through measurement data of the unmanned aerial vehicle; detecting a visual marker through the unmanned aerial vehicle, acquiring the relative pose between the unmanned aerial vehicle and the unmanned aerial vehicle when the detection is successful, and carrying out space-ground view map fusion and optimization by utilizing the conversion relation of the relative pose; continuously detecting whether the intelligent agent passes through the overlapping area based on the loop, and when the overlapping area is detected, establishing association of two maps of the intelligent agent through the matched key frames to perform track calibration and similar view map fusion and optimization; and obtaining the pose track and the map which are globally consistent according to the map obtained by the fusion and optimization of the air-ground view angle and the map obtained by the fusion and optimization of the similar view angles.
Description
Technical Field
The application belongs to the field of positioning and mapping of unmanned systems.
Background
With the continuous breakthrough of the technology in the field of intelligent robots, intelligent robots are increasingly applied to solve complex problems in modern society, and autonomous navigation capability is considered as a basis for the intelligent robots to realize autonomous tasks. In order to realize autonomous planning and navigation of robots, how to cooperatively position and build maps (simultaneous localizationand mapping, SLAM) of intelligent robots becomes a current research hot subject, and has great potential in the fields of military, disaster environment search and rescue and the like.
At present, mutual pose correction and information perception among multiple robots become a difficulty. In the SLAM process, the robot cannot acquire the information of the whole environment in advance, and the map is constructed mainly by the robot. The traditional single robot has many defects such as small sensor action range, limited observation angle, high calculation complexity, weak storage capacity and the like when performing SLAM in a large-scale scene. The robot positioning error is larger and larger only by means of local sensor information, and finally, the map building and positioning offset are caused. For some special tasks, such as military, disaster search and rescue, etc., it is necessary to complete the task in as short a time as possible. For the above problems, by a plurality of robots dispersed in the environment to communicate and work in coordination with each other, the entire system can obtain a stronger environment detection capability and an accurate positioning capability.
Disclosure of Invention
The present application aims to solve at least one of the technical problems in the related art to some extent.
Therefore, the application aims to provide a space-earth multi-mode multi-agent collaborative positioning and mapping method which is used for constructing an environment map model with the global consistency.
In order to achieve the above objective, an embodiment of a first aspect of the present application provides a method for collaborative positioning and mapping of air-ground multi-mode multi-agents, including:
acquiring measurement data of an intelligent agent, wherein the intelligent agent comprises an unmanned aerial vehicle and an unmanned aerial vehicle, and the unmanned aerial vehicle is provided with a visual marker;
local view angle local map building is carried out according to the measurement data of the unmanned aerial vehicle, and global view angle local map building is carried out according to the measurement data of the unmanned aerial vehicle;
detecting the visual marker through the unmanned aerial vehicle, acquiring the relative pose between the unmanned aerial vehicle and the unmanned aerial vehicle when the detection is successful, and performing space-ground view map fusion and optimization by utilizing the conversion relation of the relative pose;
continuously detecting whether the intelligent body passes through an overlapping area based on loop, and when the overlapping area is detected, establishing association of two maps of the intelligent body through matched key frames to perform track calibration and similar view map fusion and optimization;
and obtaining a map with the pose track and the global consistency according to the map subjected to the air-ground view angle fusion and optimization and the map subjected to the similar view angle fusion and optimization.
In addition, the air-ground multi-mode multi-agent cooperative positioning and mapping method according to the embodiment of the application can also have the following additional technical characteristics:
further, in an embodiment of the present application, the acquiring measurement data of the agent includes:
preprocessing the measurement data, including visual detection, optical flow tracking and inertial measurement unit IMU pre-integration; wherein,
the visual detection and optical flow tracking includes: the velocity vector of the optical flow is obtained by using the least square method, and the velocity vector of the optical flow is obtained by using the following formula,
，
wherein ,、/>representing that the brightness of pixel points in the image is +.>、/>Image gradient in direction, +.>Is indicated at->Time gradient in direction, +_>、/>For the optical flow edge->、/>A speed vector of the shaft;
the inertial measurement unit IMU pre-integration includes:
，
wherein ,for IMU coordinate system, +.>For the coordinate system of IMU at origin initialization, i.e. world coordinate system, < >> and />Is the acceleration and angular velocity measured by the IMU, < >>Is->Rotation of time of day from IMU coordinate system to world coordinate system,/->Right multiplication is performed on the quaternion;
will be the firstFrame to->Integrating all IMU data between frames to obtain the +.>Frame position->Speed->And rotation->，/>As an initial value of the visual estimation, the rotation is in the form of a quaternion.
Further, in an embodiment of the present application, the local view local mapping is performed by the measurement data of the unmanned aerial vehicle, and the global view local mapping is performed by the measurement data of the unmanned aerial vehicle, including:
solving three-dimensional positions of pose and all road mark points of all frames in a sliding window by using SFM, and aligning with an IMU pre-integration value to obtain angular velocity bias, gravity direction, scale factors and corresponding speeds of each frame;
optimizing state variables within a window using a sliding window approach, inOptimized state vector in time window +.>As described below,
，
wherein , and />For the rotation and translation part of the camera pose, < +.>For the speed of the camera in the world coordinate system, and />Acceleration bias and angular velocity bias of the IMU, respectively; the optimization objective function of the state quantity of the system, as follows,
，
wherein ,for maximum estimated posterior value, ++>For sliding window initial residual,/->Observing the residual error for the IMU,>residual errors are observed for the camera.
Further, in an embodiment of the present application, the detecting whether the agent passes through the overlapping area based on the loop includes:
clustering all visual features, wherein one type of feature is a word, and all the features are a dictionary;
describing an image with a single vector:
calculating similarity between two images A and B：
，
wherein ,is a vector describing image a, +.>Is a vector describing image B, +.>Is the similarity between the two images A and B, < >>Vector representing descriptive image A>Is>Component(s)>Representing vectors describing image BIs>A component;
and if the similarity exceeds a threshold value, considering that loop-back occurs.
Further, in an embodiment of the present application, the clustering all visual features includes:
at the root node, all samples are clustered using the K-means algorithmClass, obtain the first layer;
for each node of the first layer, re-aggregating samples belonging to that node into a plurality of nodesClass, obtaining the next layer; and so on, finally obtaining a leaf layer, wherein the leaf layer is a word.
Further, in one embodiment of the present application, the describing an image with a single vector includes:
definition of the definitionFor words->Number of features involved->The number of features contained for all words, +.>For wordsThe number of occurrences in image A, +.>For words->Number of times co-occurring in all images, then
Words and phrasesWeight in image A +.>The method comprises the following steps: />，
By word bag, using single vectorsDescribing one image a:
，
wherein ,is the word that image A has in the dictionary, < >>Is thatWeight corresponding to->Is a vector describing image a.
In order to achieve the above object, a second aspect of the present application provides a space-to-ground multi-mode multi-agent cooperative positioning and mapping device, comprising the following modules:
the acquisition module is used for acquiring measurement data of an intelligent agent, wherein the intelligent agent comprises an unmanned aerial vehicle and an unmanned vehicle, and the unmanned vehicle is provided with a visual marker;
the map building module is used for carrying out local view angle local map building through the measurement data of the unmanned aerial vehicle and carrying out global view angle local map building through the measurement data of the unmanned aerial vehicle;
the space-ground visual angle fusion module is used for detecting the visual marker through the unmanned aerial vehicle, acquiring the relative pose between the unmanned aerial vehicle and the unmanned aerial vehicle when the detection is successful, and carrying out space-ground visual angle map fusion and optimization by utilizing the conversion relation of the relative pose;
the similar visual angle fusion module is used for continuously detecting whether the intelligent body passes through an overlapping area based on loop, and when the overlapping area is detected, the association of two maps of the intelligent body is established through matched key frames so as to perform track calibration and similar visual angle map fusion and optimization;
and the output module is used for obtaining a pose track and a map which is globally consistent according to the map obtained through the fusion and optimization of the air-ground view angles and the map obtained through the fusion and optimization of the similar view angles.
To achieve the above object, an embodiment of the present application provides a computer device, which is characterized by comprising a memory, a processor, and a computer program stored in the memory and capable of running on the processor, wherein when the processor executes the computer program, an air-to-ground multi-mode multi-agent collaborative positioning and mapping method is implemented as described above.
To achieve the above object, a fourth aspect of the present application provides a computer readable storage medium having a computer program stored thereon, wherein the computer program, when executed by a processor, implements a space-to-ground multi-modal multi-agent cooperative localization and mapping method as described above.
The air-ground multi-mode multi-agent cooperative positioning and mapping method provided by the embodiment of the application is suitable for various application scenes and can be used for remarkably improving the perception capability and the working efficiency of a single robot system. Particularly in the field of searching and rescuing, an aerial unmanned aerial vehicle (UnmannedAerial Vehicle, UAV) detects unknown environmental topography by virtue of the aerial viewing angle of the aerial unmanned aerial vehicle, and guides a ground robot (Unmanned ground vehicle, UGV) to drive into a target area to implement accurate rescuing task, so that searching and rescuing are realized in a manner which is efficient and low in cost compared with manpower. The aerial unmanned aerial vehicle has a ground global visual field, and can construct a globally consistent environment map model by combining the local environment perception capability of the ground robot, and the map model can provide global navigation necessary information for the ground robot.
Drawings
The foregoing and/or additional aspects and advantages of the application will become apparent and readily appreciated from the following description of the embodiments, taken in conjunction with the accompanying drawings, in which:
FIG. 1 is a schematic flow chart of a method for collaborative positioning and mapping of air-ground multi-mode multi-agents according to an embodiment of the present application.
FIG. 2 is a schematic diagram of a system for collaborative positioning and mapping of air-to-ground multi-modal multi-agents according to an embodiment of the present application.
FIG. 3 is a schematic diagram of a device for collaborative positioning and mapping of air-to-ground multi-mode multi-agents according to an embodiment of the present application.
Detailed Description
Embodiments of the present application are described in detail below, examples of which are illustrated in the accompanying drawings, wherein like or similar reference numerals refer to like or similar elements or elements having like or similar functions throughout. The embodiments described below by referring to the drawings are illustrative and intended to explain the present application and should not be construed as limiting the application.
The air-ground multi-mode multi-agent cooperative positioning and mapping method of the embodiment of the application is described below with reference to the accompanying drawings.
FIG. 1 is a schematic flow chart of a method for collaborative positioning and mapping of air-ground multi-mode multi-agents according to an embodiment of the present application.
As shown in FIG. 1, the air-ground multi-mode multi-agent cooperative positioning and mapping method comprises the following steps:
s101: acquiring measurement data of an intelligent agent, wherein the intelligent agent comprises an unmanned aerial vehicle and an unmanned aerial vehicle, and the unmanned aerial vehicle is provided with a visual marker;
further, in one embodiment of the present application, obtaining measurement data of an agent includes:
preprocessing measurement data, including visual detection, optical flow tracking and inertial measurement unit IMU pre-integration; wherein,
visual detection and optical flow tracking include: the velocity vector of the optical flow is obtained by using the least square method, and the velocity vector of the optical flow is obtained by using the following formula,
，
wherein ,、/>representing that the brightness of pixel points in the image is +.>、/>Image gradient in direction, +.>Is indicated at->Time gradient in direction, +_>、/>For the optical flow edge->、/>A speed vector of the shaft;
the inertial measurement unit IMU pre-integration includes:
，
wherein ,for IMU coordinate system, +.>For the coordinate system of IMU at origin initialization, i.e. world coordinate system, < >> and />Is the acceleration and angular velocity measured by the IMU, < >>Is->Rotation of time of day from IMU coordinate system to world coordinate system,/->Right multiplication is performed on the quaternion;
will be the firstFrame to->Integrating all IMU data between frames to obtain the +.>Frame position->Speed->And rotation->，/>As an initial value of the visual estimation, the rotation is in the form of a quaternion.
Specifically, FAST features are selected for feature extraction and optical flow tracking. In each new image, the existing feature points are tracked and new feature points are detected by the KLT algorithm. In order to ensure the uniform distribution of the feature points, the image is divided into a plurality of subareas with identical sizes, and each subarea extracts at most 10 FAST corner points, so that the number of corner points of each image is kept within a certain range. The displacement between two adjacent frames of images of the outdoor scene is larger, the brightness value of each pixel may be suddenly changed, and adverse effect is caused on the tracking of the characteristic points, so that the characteristic points need to be projected onto the unit sphere after outlier rejection. Outlier rejection uses the RANSAC algorithm to achieve more robust optical flow tracking in outdoor dynamic scenarios.
At the same time of visual detection and tracking, IMU pre-integration is performed. The IMU has fast response, is not affected by imaging quality, can estimate the characteristic of absolute scale, and supplements the visual positioning of the object with unstructured outdoor surface. If all the poses corresponding to all the sampling moments of the IMU are inserted between frames for optimization during camera pose estimation, the program operation efficiency is reduced, so that IMU pre-integration processing is needed, the acceleration and angular velocity measured values output at high frequency are converted into single observed values, and the measured values are linearized again in nonlinear iteration to form constraint factors of inter-frame state quantity.
S102: carrying out local view angle local map building through measurement data of the unmanned aerial vehicle, and carrying out global view angle local map building through measurement data of the unmanned aerial vehicle;
the initialization module is required to restore the scale of the monocular camera by loosely coupling the visual information and the IMU information. Firstly, solving the three-dimensional positions of the pose of all frames and all road mark points in a sliding window by using SFM, and then aligning the pose of all frames and the three-dimensional positions with the IMU pre-integral value obtained before, so as to solve the angular velocity bias, the gravity direction, the scale factor and the velocity corresponding to each frame. As systems run, the number of state variables increases, and sliding window methods are used to optimize the state variables within the window.
Further, in an embodiment of the present application, local view local mapping is performed by measurement data of an unmanned aerial vehicle, global view local mapping is performed by measurement data of an unmanned aerial vehicle, including:
solving three-dimensional positions of pose and all road mark points of all frames in a sliding window by using SFM, and aligning with an IMU pre-integration value to obtain angular velocity bias, gravity direction, scale factors and corresponding speeds of each frame;
optimizing state variables within a window using a sliding window approach, inOptimized state vector in time window +.>As described below,
，
wherein , and />For the rotation and translation part of the camera pose, < +.>For the speed of the camera in the world coordinate system, and />Acceleration bias and angular velocity bias of the IMU, respectively; the optimization objective function of the state quantity of the system, as follows,
，
wherein ,for maximum estimated posterior value, ++>For sliding window initial residual,/->Observing the residual error for the IMU,>residual errors are observed for the camera.
S103: detecting a visual marker through the unmanned aerial vehicle, acquiring the relative pose between the unmanned aerial vehicle and the unmanned aerial vehicle when the detection is successful, and carrying out space-ground view map fusion and optimization by utilizing the conversion relation of the relative pose;
a visual marker is arranged above the ground robot, the air robot can detect the marker once observing the marker, and once the detection is successful, the relative pose between the intelligent bodies at the air end and a corresponding group of key frames are sent to the rear end.
When the visual identification carried by the ground robot is detected at the air end, the conversion relation of the visual identification under the coordinate system of the airborne camera is obtained through the visual identificationLet the conversion relation from visual identification to ground robot camera be +>Obtaining the conversion relation between the unmanned aerial vehicle camera coordinate system and the unmanned aerial vehicle camera coordinate system at the current moment as +.>And the pose conversion relation between the space key frames at the current time is obtained. At the same time, the key frame generated by the space terminal at the current moment is known +.> and />They are +_with respective reference key frames-> and />The pose conversion relation between the maps can be obtained to obtain a pose conversion matrix +.>。
S104: continuously detecting whether the intelligent agent passes through the overlapping area based on the loop, and when the overlapping area is detected, establishing association of two maps of the intelligent agent through the matched key frames to perform track calibration and similar view map fusion and optimization;
the loop detection method is based on a Bag of Words model (Bag-of-Words), and the DBoW2 library is applied to index the image database in a positive reverse order. The bag-of-words model is to calculate the similarity between the counted bag-of-words vector and the current frame to judge whether to generate loop.
First, all visual features are clustered, and one type of feature, i.e., a set of locally adjacent feature points, is a "word", so that all features are a "dictionary". If one wants to share one of many imagesThe feature points are classified as->Class, make "dictionary" into->And (5) a tree, storing and inquiring. By determining which "words" in a "dictionary" are in an image, an image can be described with a vector.
Further, in one embodiment of the present application, detecting whether an agent passes through an overlap region based on a loop comprises:
clustering all visual features, wherein one type of feature is a word, and all the features are a dictionary;
describing an image with a single vector:
calculating similarity between two images A and B：
，
wherein ,is a vector describing image a, +.>Is a vector describing image B, +.>Is the similarity between the two images A and B, < >>Vector representing descriptive image A>Is>Component(s)>Representing vectors describing image BIs>A component;
if the similarity exceeds the threshold, then loop-back is considered to occur.
Further, in one embodiment of the application, clustering all visual features includes:
at the root node, all samples are clustered using the K-means algorithmClass, obtain the first layer;
for each node of the first layer, the samples belonging to the node are re-aggregated intoClass, obtaining the next layer; and so on, finally obtaining a leaf layer, wherein the leaf layer is a word.
Further, in one embodiment of the present application, describing an image with a single vector includes:
definition of the definitionFor words->Number of features involved->The number of features contained for all words, +.>For wordsThe number of occurrences in image A, +.>For words->Number of times co-occurring in all images, then
Words and phrasesWeight in image A +.>The method comprises the following steps: />，
By word bag, using single vectorsDescribing one image a:
，
wherein ,is the word that image A has in the dictionary, < >>Is thatWeight corresponding to->Is a vector describing image a.
S105: and obtaining the pose track and the map which are globally consistent according to the map obtained by the fusion and optimization of the air-ground view angle and the map obtained by the fusion and optimization of the similar view angles.
The map fusion and optimization module is divided into a similar view angle and an open-land view angle, the open-land view angle is executed only when the rear end receives information of successful detection of the visual sign, and the map fusion and optimization module can directly convert and match an open-land end map by utilizing a relative pose relationship to perform conversion, fusion and optimization;
the similar visual angle is always performed, and the newly-put key frame continuously detects whether the child terminal passes through the overlapping area based on the loop, wherein the overlapping area comprises the loop area of the child terminal and the overlapping area between the child terminals. Once the overlapping of the areas passed by the two sub-ends is detected, the association of the two maps is established through the matched key frames, the coordinate conversion relation is obtained, the map fusion is further carried out, the two maps are combined after the map fusion, the maps are deleted from the map stack, and a new global map generated by the combination of the maps is added into the map stack and directly associated with the two sub-ends.
After the back end is optimized, the optimized pose is sent to the corresponding sub-end, and the sub-end updates the pose and uses the pose as a constraint in the pose graph to perform local optimization, so that a local map of the sub-end is optimized to more accurately perform a subsequent positioning mapping process.
And finally outputting the pose track and the map which is globally consistent.
Fig. 2 is a schematic diagram of a space-to-ground multi-mode multi-agent collaborative positioning and mapping system according to an embodiment of the present application, which is divided into two parts: a child end and a back end. The unmanned aerial vehicle and the unmanned aerial vehicle are both called as intelligent agents, each intelligent agent independently operates a sub-end, the sub-end carries a camera, an IMU, a cradle head and a control system thereof, a communication unit and an onboard processing unit which can exchange data with a central server, an independent front-end visual inertial odometer operates, and key frames and map points are sent to the rear end and a local map with smaller scale is maintained; the rear end receives information of each sub-end, performs operations with large calculation amount such as visual marker detection, loop detection, fusion optimization and the like through the central server, outputs pose tracks, and creates a map with global consistency. The system uses ROS to communicate between the sub-end and the back-end, the sub-end transmits the captured key frames and map points to the back-end, and the back-end transmits the updated pose to the sub-end.
The space-earth multi-mode multi-agent cooperative positioning and mapping method provided by the application is suitable for various application scenes, and obviously improves the perception capability and the working efficiency of a single robot system. Particularly in the field of searching and rescuing, an aerial unmanned aerial vehicle (UnmannedAerial Vehicle, UAV) detects unknown environmental topography by virtue of the aerial viewing angle of the aerial unmanned aerial vehicle, and guides a ground robot (Unmanned ground vehicle, UGV) to drive into a target area to implement accurate rescuing task, so that searching and rescuing are realized in a manner which is efficient and low in cost compared with manpower. The aerial unmanned aerial vehicle has a ground global visual field, and can construct a globally consistent environment map model by combining the local environment perception capability of the ground robot, and the map model can provide global navigation necessary information for the ground robot.
Compared with the prior art, the application has the advantages that:
1) Pure vision SLAM has some drawbacks, vision is better for the capture of texture features in a scene, but is not sufficient for the capture of structural features in the environment, and is sensitive to initialization, illumination, and monocular cameras cannot get absolute dimensions of pose and map. According to the application, based on visual detection and tracking, the IMU sensor is added to perform multi-mode information fusion, the IMU response is fast, the IMU is not influenced by imaging quality, the characteristic of absolute scale can be estimated, and the visual positioning of an object with no structure on the outdoor surface is supplemented.
2) Vibration generated in the movement process of the intelligent body can be transmitted to the camera along with the fixed structure, so that the acquisition of characteristic points by the front-end system can be influenced.
In order to realize the embodiment, the application also provides an air-ground multi-mode multi-agent cooperative positioning and mapping device.
FIG. 3 is a schematic diagram of a device for collaborative positioning and mapping of air-to-ground multi-mode multi-agents according to an embodiment of the present application.
As shown in fig. 3, the air-ground multi-mode multi-agent cooperative positioning and mapping device comprises: the system comprises an acquisition module 100, a mapping module 200, an air-ground view angle fusion module 300, a similar view angle fusion module 400, an output module 500, wherein,
the acquisition module is used for acquiring measurement data of an intelligent agent, wherein the intelligent agent comprises an unmanned aerial vehicle and an unmanned vehicle, and the unmanned vehicle is provided with a visual marker;
the image building module is used for carrying out local view angle local image building through the measurement data of the unmanned aerial vehicle and carrying out global view angle local image building through the measurement data of the unmanned aerial vehicle;
the space-ground visual angle fusion module is used for detecting the visual markers through the unmanned aerial vehicle, acquiring the relative pose between the unmanned aerial vehicle and the unmanned aerial vehicle when the detection is successful, and carrying out space-ground visual angle map fusion and optimization by utilizing the conversion relation of the relative pose;
the similar visual angle fusion module is used for continuously detecting whether the intelligent agent passes through the overlapping area based on the loop, and when the overlapping area is detected, the association of two maps of the intelligent agent is established through the matched key frames, and track calibration, similar visual angle map fusion and optimization are carried out;
and the output module is used for obtaining the pose track and the map which is globally consistent according to the map obtained by the fusion and optimization of the air-ground view angle and the map obtained by the fusion and optimization of the similar view angle.
To achieve the above object, an embodiment of the present application provides a computer device, which is characterized by comprising a memory, a processor, and a computer program stored in the memory and capable of running on the processor, wherein the processor implements the air-to-ground multi-mode multi-agent cooperative positioning and mapping method as described above when executing the computer program.
To achieve the above object, a fourth aspect of the present application provides a computer-readable storage medium having a computer program stored thereon, wherein the computer program, when executed by a processor, implements the air-to-ground multi-modal multi-agent collaborative positioning and mapping method as described above.
In the description of the present specification, a description referring to terms "one embodiment," "some embodiments," "examples," "specific examples," or "some examples," etc., means that a particular feature, structure, material, or characteristic described in connection with the embodiment or example is included in at least one embodiment or example of the present application. In this specification, schematic representations of the above terms are not necessarily directed to the same embodiment or example. Furthermore, the particular features, structures, materials, or characteristics described may be combined in any suitable manner in any one or more embodiments or examples. Furthermore, the different embodiments or examples described in this specification and the features of the different embodiments or examples may be combined and combined by those skilled in the art without contradiction.
Furthermore, the terms "first," "second," and the like, are used for descriptive purposes only and are not to be construed as indicating or implying a relative importance or implicitly indicating the number of technical features indicated. Thus, a feature defining "a first" or "a second" may explicitly or implicitly include at least one such feature. In the description of the present application, the meaning of "plurality" means at least two, for example, two, three, etc., unless specifically defined otherwise.
While embodiments of the present application have been shown and described above, it will be understood that the above embodiments are illustrative and not to be construed as limiting the application, and that variations, modifications, alternatives and variations may be made to the above embodiments by one of ordinary skill in the art within the scope of the application.
Claims (9)
1. The air-ground multi-mode multi-agent cooperative positioning and mapping method is characterized by comprising the following steps of:
acquiring measurement data of an intelligent agent, wherein the intelligent agent comprises an unmanned aerial vehicle and an unmanned aerial vehicle, and the unmanned aerial vehicle is provided with a visual marker;
local view angle local map building is carried out according to the measurement data of the unmanned aerial vehicle, and global view angle local map building is carried out according to the measurement data of the unmanned aerial vehicle;
detecting the visual marker through the unmanned aerial vehicle, acquiring the relative pose between the unmanned aerial vehicle and the unmanned aerial vehicle when the detection is successful, and performing space-ground view map fusion and optimization by utilizing the conversion relation of the relative pose;
continuously detecting whether the intelligent body passes through an overlapping area based on loop, and when the overlapping area is detected, establishing association of two maps of the intelligent body through matched key frames to perform track calibration and similar view map fusion and optimization;
and obtaining a map with the pose track and the global consistency according to the map subjected to the air-ground view angle fusion and optimization and the map subjected to the similar view angle fusion and optimization.
2. The method of claim 1, wherein the obtaining measurement data of the agent comprises:
preprocessing the measurement data, including visual detection, optical flow tracking and inertial measurement unit IMU pre-integration; wherein,
the visual detection and optical flow tracking includes: the velocity vector of the optical flow is obtained by using the least square method, and the velocity vector of the optical flow is obtained by using the following formula,
，
wherein ,、/>representing that the brightness of pixel points in the image is +.>、/>Image gradient in direction, +.>Is indicated at->Time gradient in direction, +_>、/>For the optical flow edge->、/>A speed vector of the shaft;
the inertial measurement unit IMU pre-integration includes:
，
wherein ,for IMU coordinate system, +.>For the coordinate system of IMU at origin initialization, i.e. world coordinate system, < >> and />Is the acceleration and angular velocity measured by the IMU, < >>Is->Rotation of time of day from IMU coordinate system to world coordinate system,/->Right multiplication is performed on the quaternion;
will be the firstFrame to->Integrating all IMU data between frames to obtain the +.>Frame position->Speed->And rotation->，/>As a means ofThe initial value of the visual estimate is rotated into a quaternion form.
3. The method of claim 1, wherein the locally mapping from the perspective of the unmanned vehicle from the measurement data comprises:
solving three-dimensional positions of pose and all road mark points of all frames in a sliding window by using SFM, and aligning with an IMU pre-integration value to obtain angular velocity bias, gravity direction, scale factors and corresponding speeds of each frame;
optimizing state variables within a window using a sliding window approach, inOptimized state vector in time window +.>As described below,
，
wherein , and />For the rotation and translation part of the camera pose, < +.>For the speed of the camera in world coordinate system, +.>Andacceleration bias and angular velocity bias of the IMU, respectively; state quantity optimization of systemThe function of the target is then normalized, as follows,
，
wherein ,for maximum estimated posterior value, ++>For sliding window initial residual,/->Observing the residual error for the IMU,>residual errors are observed for the camera.
4. The method of claim 1, wherein the loop-based continuous detection of whether the agent passes through an overlap region comprises:
clustering all visual features, wherein one type of feature is a word, and all the features are a dictionary;
describing an image with a single vector:
calculating similarity between two images A and B：
，
wherein ,is a vector describing image a, +.>Is a descriptive diagramVector like B>Is the similarity between the two images A and B, < >>Vector representing descriptive image A>Is>Component(s)>Vector representing descriptive picture B>Is the first of (2)A component;
and if the similarity exceeds a threshold value, considering that loop-back occurs.
5. The method of claim 4, wherein the clustering all visual features comprises:
at the root node, all samples are clustered using the K-means algorithmClass, obtain the first layer;
for each node of the first layer, re-aggregating samples belonging to that node into a plurality of nodesClass, obtaining the next layer; and so on, finally obtaining a leaf layer, wherein the leaf layer is a word.
6. The method of claim 4, wherein said describing an image with a single vector comprises:
definition of the definitionFor words->Number of features involved->The number of features contained for all words, +.>For words->The number of occurrences in image A, +.>For words->Number of times co-occurring in all images, then
Words and phrasesWeight in image A +.>The method comprises the following steps: />，
By word bag, using single vectorsDescribing one image a:
，
wherein ,is the word that image A has in the dictionary, < >>Is thatWeight corresponding to->Is a vector describing image a.
7. The air-ground multi-mode multi-agent cooperative positioning and mapping device is characterized by comprising the following modules:
the acquisition module is used for acquiring measurement data of an intelligent agent, wherein the intelligent agent comprises an unmanned aerial vehicle and an unmanned vehicle, and the unmanned vehicle is provided with a visual marker;
the map building module is used for carrying out local view angle local map building through the measurement data of the unmanned aerial vehicle and carrying out global view angle local map building through the measurement data of the unmanned aerial vehicle;
the space-ground visual angle fusion module is used for detecting the visual marker through the unmanned aerial vehicle, acquiring the relative pose between the unmanned aerial vehicle and the unmanned aerial vehicle when the detection is successful, and carrying out space-ground visual angle map fusion and optimization by utilizing the conversion relation of the relative pose;
the similar visual angle fusion module is used for continuously detecting whether the intelligent body passes through an overlapping area based on loop, and when the overlapping area is detected, the association of two maps of the intelligent body is established through matched key frames so as to perform track calibration and similar visual angle map fusion and optimization;
and the output module is used for obtaining a pose track and a map which is globally consistent according to the map obtained through the fusion and optimization of the air-ground view angles and the map obtained through the fusion and optimization of the similar view angles.
8. A computer device comprising a memory, a processor and a computer program stored on the memory and executable on the processor, the processor implementing the air-to-ground multi-modal multi-agent co-localization and mapping method of any one of claims 1-6 when the computer program is executed by the processor.
9. A computer readable storage medium having stored thereon a computer program, wherein the computer program when executed by a processor implements the air-to-ground multi-modal multi-agent co-localization and mapping method of any one of claims 1-6.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311248105.4A CN116989772B (en) | 2023-09-26 | 2023-09-26 | Air-ground multi-mode multi-agent cooperative positioning and mapping method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311248105.4A CN116989772B (en) | 2023-09-26 | 2023-09-26 | Air-ground multi-mode multi-agent cooperative positioning and mapping method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116989772A true CN116989772A (en) | 2023-11-03 |
| CN116989772B CN116989772B (en) | 2024-01-02 |
Family
ID=88526957
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311248105.4A Active CN116989772B (en) | 2023-09-26 | 2023-09-26 | Air-ground multi-mode multi-agent cooperative positioning and mapping method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116989772B (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117191005A (en) * | 2023-11-08 | 2023-12-08 | 中国人民解放军国防科技大学 | Air-ground heterogeneous collaborative mapping method, device, equipment and storage medium |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110044354A (en) * | 2019-03-28 | 2019-07-23 | 东南大学 | A kind of binocular vision indoor positioning and build drawing method and device |
| CN210377164U (en) * | 2019-06-17 | 2020-04-21 | 酷黑科技（北京）有限公司 | Air-ground cooperative operation system |
| CN112802104A (en) * | 2021-02-04 | 2021-05-14 | 华南理工大学 | Loop detection method based on RGB-D camera |
| CN114485619A (en) * | 2022-01-26 | 2022-05-13 | 清华大学 | Multi-robot positioning and navigation method and device based on air-ground cooperation |
| CN114966789A (en) * | 2022-05-12 | 2022-08-30 | 华中科技大学 | Mapping method and system fusing GNSS and multi-view vision |
| CN115993825A (en) * | 2022-12-27 | 2023-04-21 | 中国人民解放军陆军工程大学 | Unmanned vehicle cluster control system based on air-ground cooperation |
| WO2023065342A1 (en) * | 2021-10-22 | 2023-04-27 | 华为技术有限公司 | Vehicle, vehicle positioning method and apparatus, device, and computer-readable storage medium |
| CN116358520A (en) * | 2023-03-16 | 2023-06-30 | 湖南大学 | Man-machine multi-node collaborative semantic laser SLAM system and method |
-
2023
- 2023-09-26 CN CN202311248105.4A patent/CN116989772B/en active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110044354A (en) * | 2019-03-28 | 2019-07-23 | 东南大学 | A kind of binocular vision indoor positioning and build drawing method and device |
| CN210377164U (en) * | 2019-06-17 | 2020-04-21 | 酷黑科技（北京）有限公司 | Air-ground cooperative operation system |
| CN112802104A (en) * | 2021-02-04 | 2021-05-14 | 华南理工大学 | Loop detection method based on RGB-D camera |
| WO2023065342A1 (en) * | 2021-10-22 | 2023-04-27 | 华为技术有限公司 | Vehicle, vehicle positioning method and apparatus, device, and computer-readable storage medium |
| CN114485619A (en) * | 2022-01-26 | 2022-05-13 | 清华大学 | Multi-robot positioning and navigation method and device based on air-ground cooperation |
| CN114966789A (en) * | 2022-05-12 | 2022-08-30 | 华中科技大学 | Mapping method and system fusing GNSS and multi-view vision |
| CN115993825A (en) * | 2022-12-27 | 2023-04-21 | 中国人民解放军陆军工程大学 | Unmanned vehicle cluster control system based on air-ground cooperation |
| CN116358520A (en) * | 2023-03-16 | 2023-06-30 | 湖南大学 | Man-machine multi-node collaborative semantic laser SLAM system and method |
Non-Patent Citations (3)
| Title |
|---|
| 徐文菁: "非确定环境下无人机与无人车动态协同设计", 《洛阳理工学院学报( 自然科学版)》, vol. 29, no. 4 * |
| 胡秋霞: "基于无人机与无人车协同的车辆路径规划", 《中国优秀硕士学位论文全文数据库 工程科技Ⅱ辑》 第5期 * |
| 邹鹏程: "微小型空地无人***协同导航技术研究", 《中国优秀硕士学位论文全文数据库 信息科技辑》 第3期 * |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117191005A (en) * | 2023-11-08 | 2023-12-08 | 中国人民解放军国防科技大学 | Air-ground heterogeneous collaborative mapping method, device, equipment and storage medium |
| CN117191005B (en) * | 2023-11-08 | 2024-01-30 | 中国人民解放军国防科技大学 | Air-ground heterogeneous collaborative mapping method, device, equipment and storage medium |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116989772B (en) | 2024-01-02 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110068335B (en) | Unmanned aerial vehicle cluster real-time positioning method and system under GPS rejection environment | |
| CN108711166B (en) | Monocular camera scale estimation method based on quad-rotor unmanned aerial vehicle | |
| CN107967457A (en) | A kind of place identification for adapting to visual signature change and relative positioning method and system | |
| CN110726406A (en) | Improved nonlinear optimization monocular inertial navigation SLAM method | |
| CN112734841B (en) | Method for realizing positioning by using wheel type odometer-IMU and monocular camera | |
| CN114088087B (en) | High-reliability high-precision navigation positioning method and system under unmanned aerial vehicle GPS-DENIED | |
| CN112556719B (en) | Visual inertial odometer implementation method based on CNN-EKF | |
| CN112734765A (en) | Mobile robot positioning method, system and medium based on example segmentation and multi-sensor fusion | |
| CN116989772B (en) | Air-ground multi-mode multi-agent cooperative positioning and mapping method | |
| Rady et al. | A hybrid localization approach for UAV in GPS denied areas | |
| CN108759826A (en) | A kind of unmanned plane motion tracking method based on mobile phone and the more parameter sensing fusions of unmanned plane | |
| Zhou et al. | On-board inertial-assisted visual odometer on an embedded system | |
| Lin et al. | R $^ 3$ LIVE++: A Robust, Real-time, Radiance reconstruction package with a tightly-coupled LiDAR-Inertial-Visual state Estimator | |
| CN108613675B (en) | Low-cost unmanned aerial vehicle movement measurement method and system | |
| CN114485640A (en) | Monocular vision inertia synchronous positioning and mapping method and system based on point-line characteristics | |
| CN114459467B (en) | VI-SLAM-based target positioning method in unknown rescue environment | |
| CN116007609A (en) | Positioning method and computing system for fusion of multispectral image and inertial navigation | |
| Xian et al. | Fusing stereo camera and low-cost inertial measurement unit for autonomous navigation in a tightly-coupled approach | |
| Wang et al. | Multi-UAV collaborative system with a feature fast matching algorithm | |
| Fusini et al. | Experimental validation of a uniformly semi-globally exponentially stable non-linear observer for gnss-and camera-aided inertial navigation for fixed-wing uavs | |
| Ling et al. | RGB-D inertial odometry for indoor robot via keyframe-based nonlinear optimization | |
| CN116907469A (en) | Synchronous positioning and mapping method and system for multi-mode data combined optimization | |
| CN114234967B (en) | Six-foot robot positioning method based on multi-sensor fusion | |
| Liu et al. | A multi-sensor fusion with automatic vision-LiDAR calibration based on Factor graph joint optimization for SLAM | |
| CN114199250A (en) | Scene matching navigation method and device based on convolutional neural network |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |