CN113804183B - Real-time topographic mapping method and system - Google Patents
Real-time topographic mapping method and system Download PDFInfo
- Publication number
- CN113804183B CN113804183B CN202111095595.XA CN202111095595A CN113804183B CN 113804183 B CN113804183 B CN 113804183B CN 202111095595 A CN202111095595 A CN 202111095595A CN 113804183 B CN113804183 B CN 113804183B
- Authority
- CN
- China
- Prior art keywords
- map
- point cloud
- tsdf
- real
- node
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 51
- 238000013507 mapping Methods 0.000 title claims abstract description 47
- 239000010410 layer Substances 0.000 claims description 62
- 238000012876 topography Methods 0.000 claims description 19
- 230000010354 integration Effects 0.000 claims description 11
- 239000011159 matrix material Substances 0.000 claims description 8
- 239000002344 surface layer Substances 0.000 claims description 8
- 238000012545 processing Methods 0.000 claims description 7
- 238000009499 grossing Methods 0.000 claims description 6
- 230000000007 visual effect Effects 0.000 claims description 6
- RZVHIXYEVGDQDX-UHFFFAOYSA-N 9,10-anthraquinone Chemical group C1=CC=C2C(=O)C3=CC=CC=C3C(=O)C2=C1 RZVHIXYEVGDQDX-UHFFFAOYSA-N 0.000 claims description 5
- 238000012937 correction Methods 0.000 claims description 3
- 238000001514 detection method Methods 0.000 description 10
- 238000005259 measurement Methods 0.000 description 7
- 238000010586 diagram Methods 0.000 description 5
- 230000005540 biological transmission Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000000605 extraction Methods 0.000 description 2
- 238000001914 filtration Methods 0.000 description 2
- 238000012805 post-processing Methods 0.000 description 2
- 238000007781 pre-processing Methods 0.000 description 2
- 238000004422 calculation algorithm Methods 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000004590 computer program Methods 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000012423 maintenance Methods 0.000 description 1
- 238000006467 substitution reaction 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/3807—Creation or updating of map data characterised by the type of data
- G01C21/3826—Terrain data
-
- 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/38—Electronic maps specially adapted for navigation; Updating thereof
- G01C21/3863—Structures of map data
- G01C21/387—Organisation of map data, e.g. version management or database structures
- G01C21/3878—Hierarchical structures, e.g. layering
Landscapes
- Engineering & Computer Science (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Databases & Information Systems (AREA)
- Processing Or Creating Images (AREA)
- Instructional Devices (AREA)
Abstract
The invention provides a real-time topographic mapping method, which comprises the following steps: acquiring a point cloud key frame and position and posture information corresponding to the point cloud key frame in real time; updating a map layer of the TSDF map in an incremental manner by using the point cloud keyframes and the corresponding position and posture information; and expanding the map layer of the updated TSDF map by a preset thickness to generate an elevation map containing the safety boundary. The invention also provides a real-time topographic mapping system. The method and the system provided by the invention can guide obstacle avoidance in real time and have small topographic data volume.
Description
Technical Field
The invention relates to the field of automatic driving, in particular to a real-time topographic mapping method and system.
Background
Mapping refers to the activities of determining, collecting, expressing, and processing and providing acquired data, information, achievements of natural geographic factors or the shape, size, spatial location, attributes thereof, and the like of an earth's surface artificial facility. Real-time mapping refers to the process of dynamically constructing a geographic information map by receiving mapping information in real time by means of a real-time image transmission system.
The aerocar is one of the development directions of future vehicles, and the autopilot function of the intelligent aerocar plays an important role. When the automatic driving function works, the terrain information in a certain distance range of the current position needs to be obtained in advance so as to avoid risks in advance, and meanwhile, the terrain data is also the front information of the flight path planning. However, the requirement of the air flight on the terrain map is that the terrain area with the same size is expressed by a smaller data volume under the condition of ensuring safety so that the aircraft can acquire the terrain information at a high speed.
In some technologies, sensors and processor modules such as a laser radar, an IMU, a GPS, an electronic compass and the like are included in aircraft onboard equipment, but the function of the processor module is only to wirelessly transmit collected data to a ground station system for post-processing and display of the data. The technologies mainly perform real-time data acquisition, real-time transmission or storage and then perform off-line post-processing, so that the aim of real-time mapping and guiding the aircraft to avoid the obstacle cannot be achieved.
Disclosure of Invention
The invention aims to overcome the defects of the prior art and provides a real-time surveying and mapping method capable of guiding an aircraft to avoid an obstacle in real time.
The invention firstly provides a real-time topographic mapping method, which comprises the following steps: acquiring a point cloud key frame and position and posture information corresponding to the point cloud key frame in real time; updating a map layer of the TSDF map in an incremental manner by using the point cloud keyframes and the corresponding position and posture information; and expanding the map layer of the updated TSDF map by a preset thickness to generate an elevation map containing the safety boundary.
Further, the updating the map layer of the TSDF map in an incremental manner using the point cloud keyframes and the corresponding position and pose information includes: and fusing the point cloud keyframes representing multiple layers of point cloud keyframes scanned at different moments in the same field of view and corresponding position and posture information into a single surface layer in an incremental mode, and generating an updated TSDF map layer.
Further, the expanding the map layer of the updated TSDF map by a preset thickness generates an elevation map including a safety boundary, including: and expanding a safety area with a preset distance along the normal direction of the surface to increase the preset thickness of the map layer of the updated TSDF map, and generating an elevation map containing the safety boundary.
Further, the obtaining the motion compensated point cloud keyframe and the position and posture information corresponding to the point cloud keyframe specifically includes: acquiring point cloud information of a current working scene in a visual field range and corresponding high-frequency space pose information and low-frequency global space position information through a sensor; pre-integrating the high-frequency space pose information; calculating the pose matrix of each point relative to the first point in the point cloud frame by interpolation of the information pre-integration result; obtaining a corrected point cloud frame according to the pose matrix and the low-frequency global space position information correction point Yun Zhen; and when the variation of the carrier position or angle is larger than a preset threshold value, selecting the corrected point cloud frame corresponding to the moment as a point cloud key frame.
Further, fusing the point cloud keyframes representing multiple layers of point cloud keyframes scanned at different times in the same field of view and corresponding position and posture information into a single surface layer in an incremental manner, and generating an updated TSDF map layer comprises: converting the original point cloud data expressed under the obtained point cloud coordinate system into point cloud data under a TSDF topography global coordinate system; under a TSDF topography global coordinate system, calculating a node center coordinate and a cut-off distance corresponding to each point in the point cloud key frame data; and updating the TSDF map layer data by using the calculated node center coordinates and the cut-off distance corresponding to each point. Further, the expanding the safety area with a preset distance along the surface normal direction for adding a preset thickness to the map layer of the updated TSDF map, and generating the elevation map including the safety boundary includes: in the vertical direction, expanding the map layer upwards along the height of the terrain to enable the map layer to be expanded outwards by a preset distance along the normal direction of the surface; in the horizontal direction, the map layer is expanded outwards along the horizontal direction, so that the map layer is expanded outwards along the surface normal direction by a preset distance. .
Further, the method further comprises: establishing elevation nodes corresponding to the elevation map according to updated nodes in the TSDF map; wherein, in vertical direction, upwards expanding the map layer along the topography height makes the map layer outwards expand the preset distance along surface normal direction includes: updating the interception distance of the corresponding elevation node according to the interception distance of the updated node in the TSDF map and the preset distance; in the horizontal direction, outwards expanding the map layer along the horizontal direction to enable the map layer to outwards expand a preset distance along the surface normal direction, and the method comprises the following steps: searching an elevation topographic map node of the elevation node within a preset neighborhood radius range as a neighborhood node; updating the cutoff distance of the neighborhood nodes according to the cutoff distance of the neighborhood nodes and the cutoff distance of the current elevation nodes.
Further, any one of the real-time topographic mapping methods above further comprises: the boundary of the elevation map of the safety boundary is subjected to simplified smoothing processing, and the data volume of the elevation map of the safety boundary after updating is compressed.
Further, the boundary of the elevation map of the safety boundary is subjected to simplified smoothing processing, and the data volume of the elevation map of the safety boundary after being compressed and updated comprises: calculating the absolute height value of the current node by using the absolute height value before updating the current node, the preset smooth height and the absolute height value of the anchor point at the bottom of the current node; the absolute altitude value of the current node is updated in the elevation map data.
The invention also provides a real-time topographic mapping system which is applied to an aircraft and comprises a flight control unit and a topographic real-time mapping module, wherein the topographic real-time mapping module generates an updated safety boundary topographic map, and the topographic real-time mapping module executes any one of the real-time topographic mapping methods.
According to the invention, the data acquired in real time is processed, and the safety area with a certain thickness is added on the basis of the updated map, so that the topographic data generated in real time can be used for generating a topographic map for flight control obstacle avoidance.
Drawings
FIG. 1 is a schematic illustration of a real-time topography mapping method according to an embodiment of the present invention;
FIG. 2 is a schematic illustration of a real-time topography mapping method according to another embodiment of the present invention;
FIG. 3 is a schematic illustration of a real-time topography mapping method according to yet another embodiment of the present invention;
FIG. 4 is a schematic diagram of a real-time topography mapping method according to yet another embodiment of the present invention
FIG. 5 is a schematic illustration of a real-time topography mapping method according to yet another embodiment of the present invention;
FIG. 6 is a schematic illustration of a real-time topography mapping method according to another embodiment of the present invention;
FIG. 7 illustrates in two dimensions the principle of generation of a map of the topography of a security boundary in accordance with one embodiment of the present invention;
FIG. 8 is a flow diagram of a process for generating a map of a security boundary topography in accordance with one embodiment of the invention;
FIG. 9 is a block schematic diagram of a real-time topography system according to one embodiment of the present invention;
FIG. 10 is a detailed schematic diagram of a real-time topography system according to another embodiment of the present invention.
Detailed Description
The following describes in further detail the embodiments of the present invention with reference to the drawings and examples. The following examples are illustrative of the invention and are not intended to limit the scope of the invention.
Referring to fig. 1, a real-time topographic mapping method according to a preferred embodiment of the present invention includes the steps of: sc, acquiring a point cloud key frame and position and posture information corresponding to the point cloud key frame in real time; se, updating a map layer of a TSDF (truncated signed distance function; based on truncated signed distance function) map in an incremental manner using the point cloud keyframes and the corresponding position and pose information; and Sg, expanding the map layer of the updated TSDF map with preset thickness to generate an elevation map containing the safety boundary.
According to the invention, the data acquired in real time is processed, the safety area with a certain thickness is added on the basis of the updated map, the map layer of the updated TSDF map is expanded with a preset thickness to generate the elevation map containing the safety boundary, and the topographic data generated in real time can be used for generating a topographic map for flight control obstacle avoidance.
Please refer to fig. 2. The method comprises the steps of acquiring a point cloud keyframe and position and posture information corresponding to the point cloud keyframe in real time, namely, step Sc comprises the following steps:
sc1: the method comprises the steps of acquiring point cloud information of a current working scene in a visual field range and corresponding high-frequency space pose information and low-frequency global space position information through a sensor, specifically, providing the high-frequency space pose information through an IMU (inertial measurement unit; namely an inertial measurement unit), providing the point cloud information of the working scene in the visual field range through a LIDAR (Light Detection And Ranging; namely laser detection and measurement), and providing the low-frequency global space position information through a GNSS (Global Navigation Satellite System; global navigation satellite system) to carry out accumulated error compensation on the high-frequency space pose information provided by the IMU.
Sc3: and carrying out information pre-integration on the high-frequency space pose information. In the information pre-integration, IMU information pre-integration is performed by calculating relative motion between point cloud frames of the laser radar.
Sc5: assuming that the points Yun Zhen are acquired by the IMU at the uniform linear velocity and the uniform angular velocity, performing interpolation calculation through the information pre-integration result in the step Sc3, and calculating a pose matrix of each point relative to the first point in the point cloud frame for correcting the point cloud frame.
Sc7: and obtaining a corrected point cloud frame according to the pose matrix and the low-frequency global space position information correction point Yun Zhen. The points in a frame of point cloud are not collected at the same moment, and in the collection process, the laser radar moves along with the carrier, so that the coordinate systems of different laser points are always changed, and finally the formed point cloud frame data cannot accurately reflect the geometric shape of an object in a visual field, namely distortion is generated; in order to simplify the operation, it is assumed that the carrier collects a frame of point cloud at the uniform linear velocity and the uniform angular velocity, and the pose matrix of each point relative to the first point in the frame is calculated by interpolating the pre-integration result of the IMU information and is used for correcting the point cloud frame.
Sc9: when the variation of the carrier position or angle is larger than a preset threshold value, selecting the corrected point cloud frame corresponding to the moment as a point cloud key frame, namely selecting the point cloud frame formed by the point cloud data corresponding to the moment as a point cloud key frame, and on the premise of meeting the topographic mapping requirement, the overlarge data volume is unfavorable for the real-time processing, so that key frame detection and extraction are needed; in the invention, the key frame detection only depends on the variation of the carrier position and angle, and when any variation is larger than a preset threshold value, the corrected point cloud frame corresponding to the moment is selected as the key frame.
In a preferred embodiment, if another sensor capable of indirectly acquiring point cloud data, such as a binocular camera, is used instead of the LIDAR, the point cloud frame motion compensation step may be omitted, referring to fig. 3, and in this case, the step of acquiring the point cloud keyframe and the position and posture information corresponding to the point cloud keyframe in real time, that is, step Sc includes the steps of: sc2: providing high-frequency space pose information through the IMU, and acquiring point cloud data through a binocular camera; sc4: pre-integrating the high-frequency space pose information; sc6: and when the variation of the carrier position or angle is larger than a preset threshold value, selecting a point cloud frame formed by the point cloud data corresponding to the moment as a point cloud key frame.
Referring to fig. 4, the step of updating the map layer of the TSDF map in an incremental manner using the point cloud keyframe and the corresponding position and posture information, that is, step Se includes the steps of: se2, fusing the point cloud keyframes representing multiple layers of point cloud keyframes scanned at different moments in the same field of view and position and posture information corresponding to the point cloud keyframes into a single surface layer in an incremental mode, and generating an updated map layer. More specifically, step Se2 includes the steps of: se22 converts the original point cloud data expressed under the obtained point cloud coordinate system into point cloud data under a TSDF topographic global coordinate system, specifically converts the point cloud data expressed under the point cloud coordinate system into the TSDF topographic global coordinate system, so that the data obtained from the laser radar sensor at different positions at different moments are unified into the same coordinate system; calculating the node center coordinates and the cut-off distance corresponding to each point in the point cloud key frame data under the TSDF topography global coordinate system; specifically, calculating a node index corresponding to each point in the point cloud frame data, namely a node center coordinate, so as to facilitate retrieval and creation; calculating a node distance average value belonging to the same node index midpoint, and creating or updating a corresponding node cut-off distance value according to the node distance average value; se26, using the calculated node center coordinates and cut-off distances corresponding to each point to update the TSDF map layer data, and determining the updating iteration mode of the range and cut-off distances of the nodes needing to be updated, wherein the main goal of the step is to fuse the multi-layer point cloud data scanned at different moments in the same area into a single surface layer in an incremental mode; when the TSDF method is adopted, the TSDF map layer consists of a series of TSDF nodes with cubes with determined side lengths, namely the resolution of the TSDF map, so that the data volume of the TSDF map is only in direct proportion to the map area to be represented, and is irrelevant to decoupling of the coverage times of the scanning surface.
More specifically, referring to fig. 5, when the TSDF method is adopted, the step of fusing the point cloud keyframes representing multiple layers of point cloud keyframes scanned at different times in the same field of view and corresponding position and posture information into a single surface layer in an incremental manner, and generating an updated TSDF map layer includes: se221, according to the input kth key frame point cloudAnd its corresponding global pose->Converting a point cloud from a sensor coordinate system to a TSDF global coordinate system +.>The origin of the point cloud of the frame is converted into the TSDF global coordinate system to be +.>Converting point cloud data expressed under a point cloud coordinate system into a TSDF global coordinate system, so that data acquired from a laser radar sensor at different positions at different moments are unified into the same TSDF coordinate system; se241, respectively calculating the node center coordinates corresponding to each point p in the key frame i The (x, y, z) corresponding TSDF node index is +.>The central coordinates of the nodes areWherein->The representation is rounded down, and TSDF node indexes corresponding to each point in the point cloud frame data are calculated so as to facilitate retrieval and creation; and, calculating the average value p of all points belonging to the same TSDF node in the key frame point cloud m Node index->And center coordinates->Check index +.>If not, then a new TSDF node is created and the distance of the node is calculated as +.> Se261, updated with->The size range is [ -D T ,D T ]Distance values within all TSDF nodes within; the update is calculated by the following (2) (3), wherein d i+1 Calculated by formula (1), w i+1 ＝1，/>W i+1 ＝W i +w i+1 ......(3)。
Referring to fig. 6, 7 and 8, the step of expanding the map layer of the updated TSDF by a predetermined distance along the surface normal direction to increase the safety area with a predetermined thickness and generating an elevation map including a safety boundary, that is, step Sg includes the steps of:
and Sg4, updating the corresponding topographic map node for each TSDF node in the polling process, wherein the height value of the topographic map node is processed according to the following formula (4). This stepApplying the security boundary parameter d s D expanding the height of the terrain upwards in the vertical direction s A safe distance of length;
for each node, expanding the map layer upwards along the terrain height in the vertical direction to enable the map layer to be expanded outwards by a preset distance along the surface normal direction;
specifically, step Sg4 includes step Sg45, calculating a node surface safety distance, that is, updating the truncated distance of the corresponding elevation node according to the updated truncated distance of the node in the TSDF map and a preset distance: determining whether there is an index in the safety boundary topography mapIs defined by the topographic map node: if not, create node ++>And distance +.>Initialized to->Wherein->Is node->A cut-off distance inside; if so, updating the cutoff distance inside the topographic map node as follows:
wherein,is the topographic map node +>Distance value d before update of (d) s Is the outward expansion distance;
sg6 expanding the terrain safety boundary in the horizontal direction by d s A distance; for each node, the map layer is expanded outwards in the horizontal direction, so that the map layer is expanded outwards by a preset distance in the surface normal direction.
Specifically, unlike Sg45, which updates the truncated distance of the corresponding elevation node according to the updated truncated distance of the node in the TSDF map and the preset distance, step Sg6 includes step Sg67 and step Sg69. Step Sg67: and searching the elevation topographic map node of the elevation node within a preset neighborhood radius range as a neighborhood node. Step Sg69: updating the cutoff distance of the neighborhood node according to the cutoff distance of the neighborhood node and the cutoff distance of the current elevation node, namely updating the elevation nodeNeighborhood radius d s Cut-off distances of other nodes within range.
In the step Sg69, the elevation node is updatedNeighborhood radius d s When the cutoff distances of other nodes in the range are equal to the cutoff distances of other nodes in the neighborhood of the current elevation node, new distance values are generated according to preset rules and assigned. For example, the truncated distance of the current elevation node may be compared to the truncated distances of other nodes in the neighborhood of the node, and a larger or maximum value may be selected as the new distance value for the current node.
Through the steps Sg45, sg67 and Sg69, the elevation nodes corresponding to the elevation map can be created according to the updated nodes in the TSDF map. In other words, an elevation node corresponding to an elevation map is created according to the updated node in the TSDF map, the map layer is expanded upwards along the terrain height in the vertical direction to expand the map layer by a preset distance along the surface normal direction, and the interception distance of the corresponding elevation node is updated according to the interception distance and the preset distance of the updated node in the TSDF map; and in the horizontal direction, expanding the map layer outwards along the horizontal direction to enable the map layer to be expanded outwards along the surface normal direction by a preset distance, firstly searching an elevation topographic map node of the elevation node in a preset neighborhood radius range as a neighborhood node, and then updating the cutoff distance of the neighborhood node according to the cutoff distance of the neighborhood node and the cutoff distance of the current elevation node.
In the step of expanding the map layer upward along the height of the terrain in the vertical direction to enable the map layer to be expanded outward along the normal direction of the surface by a preset distance for each node, that is, before the step Sg4, the method can further comprise the step Sg2 of distributing a storage space for each node according to the updating sequence of the nodes for the updated map layer. Specifically, step Sg2 includes the steps of: step Sg21, determining that the TSDF integrator adds the TSDF node with the changed internal value to the hash value queue according to the time sequence and independently executing the TSDF integrator as a thread or a process, constructing the hash value queue of the node with the updated TSDF value when the map increment is updated, preferably constructing the hash value queue of the node with the updated TSDF value according to the time sequence of the node update, firstly judging whether the queue is empty when the update process begins, and continuing to wait until a new TSDF node is added; step Sg23, polling to process TSDF node data in a hash value queue, sequentially taking out hash values from the hash value queue, and acquiring indexes of corresponding TSDF nodes according to the hash valuesAnd node distance->Preferably, the hash values are sequentially taken out from the hash value queue according to the first-in first-out principle, the hash value of the first elevation node is firstly obtained, then the hash value of the neighborhood node is obtained, and for each hash value, the step Sg45 is executed once.
Further, the step of adding a safety area with a predetermined thickness to the updated TSDF map layer, that is, the real-time topographic mapping method may further include the step of Si: and (3) simplifying and smoothing the boundary of the elevation map of the safety boundary, and compressing the data volume of the updated safety boundary topographic map.
Specifically, referring to fig. 7 and 8, step Si includes step Si1 of calculating an absolute height value of the current node using an absolute height value before updating the current node, a preset smooth height and an absolute height value of a bottom anchor point of the current node; and Si3, updating the absolute altitude value of the current node in the elevation map data. The step of calculating the absolute height value of the current node by using the absolute height value before updating the current node, the preset smooth height and the absolute height value of the anchor point at the bottom of the current node, namely, the step Si1 specifically comprises the following steps: setting a preset smooth height as H tol I.e. with a smooth height in the range H tol ，H tol Corresponding to the height of the cylinder in fig. 7, the cylinder anchor point is the center of the lower end face of the cylinder, namely the node. And updating the distance value of the topographic map node by adopting a breadth-first search method. The absolute height value before the update of the last node is h' n-1 The absolute height value before the update of the current node is h' n The absolute height value of the anchor point at the bottom of the current node is h' n +H tol The updated absolute height value of the current node is
The main objective of the steps shown in fig. 6 and 7 is to add a certain thickness of safety area on the basis of TSDF map establishment, and also to reduce the data volume of the topographic map per unit area. FIG. 7 is a schematic diagram in two dimensions, in which a safety region with a certain thickness is required to be added in consideration of factors affecting the accuracy of image construction such as laser radar accuracy and pose data error, and the thickness of the safety region is denoted as d s The safety zone is defined as extending outwardly a distance d in the direction of the surface normal s The resulting boundary surface, i.e. moving outwardly a distance d in the direction normal to the outer surface s And the boundary surface obtained. Because the surface often has a large amount of forests, houses and the likeObjects with low degree, and the aerial route of the aircraft is generally located above the objects, so that similar detail information has little significance on aerial flight, and in order to reduce the data volume of the topographic map, the detail needs to be filtered out. The columns with cross marks and dot marks in fig. 7 represent the topographic data before and after filtering, respectively, and after filtering, the detailed information of fluctuation in the height range of the column is smoothed, i.e. the column with dot marks is smoothed, and finally the topographic map is expressed by a small number of points represented by the columns with cross marks.
The above simplified smoothing boundary step may also be evolved into a local area map-based simplification method, such as the concave-convex simplified algorithm commonly found in three-dimensional grids.
The real-time topographic mapping method may further include the steps of: sb: and constructing a hash table according to the characteristics of the TSDF map layer, wherein each node in the TSDF map layer comprises three fields, namely a hash value hn, a node center coordinate (x, y, z) n and a cut-off distance dn.
The invention also provides a real-time topographic mapping system, please refer to fig. 9, which is applied to an aircraft and comprises a flight control unit FCU and a topographic real-time mapping module, wherein the topographic real-time mapping module generates an updated safe boundary topographic map, and the topographic real-time mapping module executes any one of the real-time topographic mapping methods. The terrain real-time mapping module maps the terrain in real time and has maintenance, updating and loading functions, can acquire a safe boundary topographic map in a certain range, and transmits the safe boundary topographic map to the FCU for controlling the flight so as to achieve the purpose of guiding obstacle avoidance.
With further reference to fig. 10, the present application proposes the method and system for mapping the terrain in real time, which mainly includes three parts, namely data preprocessing, TSDF map incremental update and safety boundary terrain map incremental update, aiming at the characteristics of real-time mapping of the terrain map by air flight and low data amount used for representing a unit area.
The data preprocessing part aims at providing the point cloud keyframe subjected to motion compensation and the position and posture information corresponding to the keyframe for the TSDF map increment updating part. Information acquisition is performed by an IMU (inertial measurement unit, i.e., an inertial measurement unit), a LIDAR (Light Detection And Ranging, i.e., a laser detection and measurement unit), and a GNSS (Global Navigation Satellite System global navigation satellite system) as input data, the IMU information provides high-frequency spatial pose information, the LIDAR provides point cloud information of a working scene within a field of view, the GNSS provides low-frequency global spatial position information, and accumulated error compensation is performed on pose information provided by the IMU. The part can be divided into IMU information pre-integration, point cloud frame motion compensation and key frame detection. In the IMU information pre-integration, the IMU information pre-integration is performed by calculating the relative motion between the point cloud frames of the laser radar, and the process can be realized by using a more classical and universal method. In point cloud frame motion compensation, points in a frame of point cloud are not acquired at the same time. In the acquisition process, the laser radar moves along with the carrier, so that coordinate systems of different laser points are always changed, and finally formed point cloud frame data cannot accurately reflect the geometric shape of an object in a visual field, namely distortion is generated. In order to simplify the operation, it is assumed that a carrier, namely an aircraft, collects a frame of point cloud at a uniform linear velocity and a uniform angular velocity, and a pose matrix of each point relative to a first point in a frame is calculated by interpolating a pre-integration result of IMU information and is used for correcting the point cloud frame. In the key frame detection, the point cloud has large data volume, and on the premise of meeting the requirements of topographic mapping, the excessive data volume is unfavorable for the real-time processing, so that the key frame detection and extraction are needed. In the invention, the key frame detection only depends on the variation of the carrier position and angle, and when any variation is larger than a preset threshold, the point cloud frame corresponding to the moment is selected as the key frame.
The main objective of the incremental updating part of the TSDF map is to fuse the multi-layer point cloud data scanned at different moments in the same area, namely the point cloud key frames, and the position and posture information corresponding to the point cloud key frames into a single surface layer in an incremental mode so as to generate an updated TSDF map layer. Because the TSDF map layer is composed of a series of TSDF nodes with cubes defining a side length, i.e. the resolution of the TSDF map, the data volume of the TSDF map is only directly proportional to the map area to be represented, and thus independent of the decoupling of the number of coverage of the glance. The TSDF map layer is a hash table, and as shown in table 1, each node has three fields, namely a hash value, a node center coordinate and a cut-off distance. The Hash value is calculated by the node center coordinates, so that low repeatability of the Hash value of each node is guaranteed, and the node attributes can be quickly inquired through the Hash value.
Table 1 TSDF graph hash table
Let the cutoff distance D T After the resolution of the TSDF nodes is rho, incremental updating of the TSDF map is carried out.
The main objective of the incremental updating part of the security boundary topographic map is to increase a security area with a certain thickness on the basis of TSDF map establishment, and simultaneously reduce the data volume of the topographic map per unit area. See for example the above explanation of step Sg and fig. 6, 7 and 7.
The present application finally provides a computer readable storage medium having a computer program stored thereon, which when executed by a processor, implements the steps of any of the real-time mapping methods described above.
In summary, the invention can generate the topographic map for the flying control obstacle avoidance by processing the data acquired in real time, and the invention can simplify the surface geometry and reduce the data volume by improving the measurement characteristics and accuracy of the measuring sensor.
The foregoing is merely illustrative of the present invention, and the present invention is not limited thereto, and any person skilled in the art will readily recognize that variations or substitutions are within the scope of the present invention. Therefore, the protection scope of the present invention shall be subject to the protection scope of the claims.
Claims (9)
1. A method of real-time topographic mapping, comprising:
acquiring a point cloud key frame and position and posture information corresponding to the point cloud key frame in real time;
updating a map layer of the TSDF map in an incremental manner by using the point cloud keyframes and the corresponding position and posture information;
expanding the map layer of the updated TSDF map by a preset thickness to generate an elevation map containing a safety boundary;
the real-time acquisition of the point cloud keyframe and the position and posture information corresponding to the point cloud keyframe specifically comprises:
acquiring point cloud information of a current working scene in a visual field range and corresponding high-frequency space pose information and low-frequency global space position information through a sensor;
pre-integrating the high-frequency space pose information;
calculating the pose matrix of each point relative to the first point in the point cloud frame by interpolation of the information pre-integration result;
obtaining a corrected point cloud frame according to the pose matrix and the low-frequency global space position information correction point Yun Zhen;
and when the variation of the carrier position or angle is larger than a preset threshold value, selecting the corrected point cloud frame at the corresponding moment as a point cloud key frame.
2. The real-time terrain mapping method of claim 1, wherein the incrementally updating the map layer of the TSDF map using the point cloud keyframes and the corresponding position and pose information comprises:
and fusing the point cloud keyframes representing multiple layers of point cloud keyframes scanned at different moments in the same field of view and corresponding position and posture information into a single surface layer in an incremental mode, and generating an updated TSDF map layer.
3. The method of claim 2, wherein expanding the map layer of the updated TSDF map with a preset thickness generates an elevation map containing safety boundaries, comprising:
and expanding a safety area with a preset distance along the normal direction of the surface to increase the preset thickness of the map layer of the updated TSDF map, and generating an elevation map containing the safety boundary.
4. The method of claim 2, wherein incrementally fusing the point cloud keyframes representing multiple layers of point cloud keyframes scanned at different times within the same field of view and corresponding position and pose information into a single surface layer, generating an updated TSDF map layer comprises:
converting the original point cloud data expressed under the obtained point cloud coordinate system into point cloud data under a TSDF topography global coordinate system;
under a TSDF topography global coordinate system, calculating a node center coordinate and a cut-off distance corresponding to each point in the point cloud key frame data;
and updating the TSDF map layer data by using the calculated node center coordinates and the cut-off distance corresponding to each point.
5. A method of real-time topographic mapping according to claim 3, wherein the expanding outwardly a predetermined distance along the surface normal to add a predetermined thickness of a safety area to the map layer of the updated TSDF map, generating an elevation map including safety boundaries, comprises:
in the vertical direction, expanding the map layer upwards along the height of the terrain to enable the map layer to be expanded outwards by a preset distance along the normal direction of the surface;
in the horizontal direction, the map layer is expanded outwards along the horizontal direction, so that the map layer is expanded outwards along the surface normal direction by a preset distance.
6. The real-time topography mapping method of claim 5, further comprising:
establishing elevation nodes corresponding to the elevation map according to updated nodes in the TSDF map;
wherein, in vertical direction, upwards expanding the map layer along the topography height makes the map layer outwards expand the preset distance along surface normal direction includes:
updating the interception distance of the corresponding elevation node according to the interception distance of the updated node in the TSDF map and the preset distance;
in the horizontal direction, outwards expanding the map layer along the horizontal direction to enable the map layer to outwards expand a preset distance along the surface normal direction, and the method comprises the following steps:
searching an elevation topographic map node of the elevation node within a preset neighborhood radius range as a neighborhood node;
updating the cutoff distance of the neighborhood nodes according to the cutoff distance of the neighborhood nodes and the cutoff distance of the current elevation nodes.
7. The real-time topographic mapping method according to any one of claims 1-6, further comprising:
the boundary of the elevation map of the safety boundary is subjected to simplified smoothing processing, and the data volume of the elevation map of the safety boundary after updating is compressed.
8. The method of real-time topographic mapping according to claim 7, wherein the boundary of the elevation map of the safety boundary is subjected to a simplified smoothing process, and the compressing the data amount of the updated elevation map of the safety boundary includes:
calculating the absolute height value of the current node by using the absolute height value before updating the current node, the preset smooth height and the absolute height value of the anchor point at the bottom of the current node;
the absolute altitude value of the current node is updated in the elevation map data.
9. A real-time terrain mapping system, characterized in that it is applied to an aircraft, comprising a flight control unit and a terrain real-time mapping module, which generates an updated safety boundary terrain map, said terrain real-time mapping module performing the real-time terrain mapping method according to any of claims 1 to 8.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111095595.XA CN113804183B (en) | 2021-09-17 | 2021-09-17 | Real-time topographic mapping method and system |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111095595.XA CN113804183B (en) | 2021-09-17 | 2021-09-17 | Real-time topographic mapping method and system |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113804183A CN113804183A (en) | 2021-12-17 |
| CN113804183B true CN113804183B (en) | 2023-12-22 |
Family
ID=78939587
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111095595.XA Active CN113804183B (en) | 2021-09-17 | 2021-09-17 | Real-time topographic mapping method and system |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113804183B (en) |
Citations (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN1567385A (en) * | 2003-06-19 | 2005-01-19 | 邓兴峰 | Panoramic reconstruction method of three dimensional image from two dimensional image |
| CN103116360A (en) * | 2013-01-31 | 2013-05-22 | 南京航空航天大学 | Unmanned aerial vehicle obstacle avoidance controlling method |
| CN106446815A (en) * | 2016-09-14 | 2017-02-22 | 浙江大学 | Simultaneous positioning and map building method |
| CN106663411A (en) * | 2014-11-16 | 2017-05-10 | 易欧耐特感知公司 | Systems and methods for augmented reality preparation, processing, and application |
| CN109923589A (en) * | 2016-11-14 | 2019-06-21 | 深圳市大疆创新科技有限公司 | Building and update hypsographic map |
| CN110335316A (en) * | 2019-06-28 | 2019-10-15 | Oppo广东移动通信有限公司 | Method, apparatus, medium and electronic equipment are determined based on the pose of depth information |
| CN110631581A (en) * | 2018-06-22 | 2019-12-31 | 华为技术有限公司 | Method for establishing indoor 3D map and unmanned aerial vehicle |
| CN112083725A (en) * | 2020-09-04 | 2020-12-15 | 湖南大学 | Structure-shared multi-sensor fusion positioning system for automatic driving vehicle |
| CN112947531A (en) * | 2021-03-26 | 2021-06-11 | 上海工程技术大学 | Control method, device and equipment of unmanned aerial vehicle and storage medium |
| CN113052906A (en) * | 2021-04-01 | 2021-06-29 | 福州大学 | Indoor robot positioning method based on monocular camera and odometer |
| CN113063427A (en) * | 2020-01-02 | 2021-07-02 | 广东博智林机器人有限公司 | Indoor high-precision map production method, device, equipment and storage medium |
| CN113342038A (en) * | 2016-02-29 | 2021-09-03 | 星克跃尔株式会社 | Method and system for generating a map for flight of an unmanned aerial vehicle |
| CN113379910A (en) * | 2021-06-09 | 2021-09-10 | 山东大学 | Mobile robot mine scene reconstruction method and system based on SLAM |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US10937182B2 (en) * | 2017-05-31 | 2021-03-02 | Google Llc | Non-rigid alignment for volumetric performance capture |
-
2021
- 2021-09-17 CN CN202111095595.XA patent/CN113804183B/en active Active
Patent Citations (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN1567385A (en) * | 2003-06-19 | 2005-01-19 | 邓兴峰 | Panoramic reconstruction method of three dimensional image from two dimensional image |
| CN103116360A (en) * | 2013-01-31 | 2013-05-22 | 南京航空航天大学 | Unmanned aerial vehicle obstacle avoidance controlling method |
| CN106663411A (en) * | 2014-11-16 | 2017-05-10 | 易欧耐特感知公司 | Systems and methods for augmented reality preparation, processing, and application |
| CN113342038A (en) * | 2016-02-29 | 2021-09-03 | 星克跃尔株式会社 | Method and system for generating a map for flight of an unmanned aerial vehicle |
| CN106446815A (en) * | 2016-09-14 | 2017-02-22 | 浙江大学 | Simultaneous positioning and map building method |
| CN109923589A (en) * | 2016-11-14 | 2019-06-21 | 深圳市大疆创新科技有限公司 | Building and update hypsographic map |
| CN110631581A (en) * | 2018-06-22 | 2019-12-31 | 华为技术有限公司 | Method for establishing indoor 3D map and unmanned aerial vehicle |
| CN110335316A (en) * | 2019-06-28 | 2019-10-15 | Oppo广东移动通信有限公司 | Method, apparatus, medium and electronic equipment are determined based on the pose of depth information |
| CN113063427A (en) * | 2020-01-02 | 2021-07-02 | 广东博智林机器人有限公司 | Indoor high-precision map production method, device, equipment and storage medium |
| CN112083725A (en) * | 2020-09-04 | 2020-12-15 | 湖南大学 | Structure-shared multi-sensor fusion positioning system for automatic driving vehicle |
| CN112947531A (en) * | 2021-03-26 | 2021-06-11 | 上海工程技术大学 | Control method, device and equipment of unmanned aerial vehicle and storage medium |
| CN113052906A (en) * | 2021-04-01 | 2021-06-29 | 福州大学 | Indoor robot positioning method based on monocular camera and odometer |
| CN113379910A (en) * | 2021-06-09 | 2021-09-10 | 山东大学 | Mobile robot mine scene reconstruction method and system based on SLAM |
Non-Patent Citations (1)
| Title |
|---|
| 面向室内动态环境的半直接法RGB-D SLAM算法;高成强 等;《机器人》;第41卷(第3期);372-383 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113804183A (en) | 2021-12-17 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112347840B (en) | Vision sensor laser radar integrated unmanned aerial vehicle positioning and image building device and method | |
| AU2019217205B2 (en) | Method of and apparatus for analyzing images | |
| KR102007567B1 (en) | Stereo drone and method and system for calculating earth volume in non-control points using the same | |
| KR20190051704A (en) | Method and system for acquiring three dimentional position coordinates in non-control points using stereo camera drone | |
| KR20180079428A (en) | Apparatus and method for automatic localization | |
| CN111338383B (en) | GAAS-based autonomous flight method and system, and storage medium | |
| RU2487419C1 (en) | System for complex processing of information of radio navigation and self-contained navigation equipment for determining real values of aircraft navigation parameters | |
| CN104360362B (en) | Method and system for positioning observed object via aircraft | |
| KR20180127568A (en) | Method and apparatus of generating 3-dimension route applied geographic information | |
| KR102289752B1 (en) | A drone for performring route flight in gps blocked area and methed therefor | |
| KR101877900B1 (en) | 3d flight route creating system and method by predicting battery consumption | |
| CN107063187A (en) | A kind of height of tree rapid extracting method of total powerstation and unmanned plane image association | |
| CN110220517A (en) | A kind of Indoor Robot robust slam method of the combining environmental meaning of one's words | |
| CN113920186B (en) | Low-altitude unmanned-machine multi-source fusion positioning method | |
| US10621742B2 (en) | Method for producing a depth map | |
| CN113155126B (en) | Visual navigation-based multi-machine cooperative target high-precision positioning system and method | |
| KR102325119B1 (en) | Vehicles updating point cloud maps using lidar coordinates | |
| CN113804183B (en) | Real-time topographic mapping method and system | |
| KR102130687B1 (en) | System for information fusion among multiple sensor platforms | |
| CN115950435A (en) | Real-time positioning method for unmanned aerial vehicle inspection image | |
| CN117554989A (en) | Visual fusion laser radar SLAM positioning navigation method and unmanned aerial vehicle system thereof | |
| Ma et al. | A review: The survey of attitude estimation in autonomous uav navigation | |
| KR101181742B1 (en) | Apparatus and method for land-use map renewel | |
| Ishii et al. | Autonomous UAV flight using the Total Station Navigation System in Non-GNSS Environments | |
| CN116793340B (en) | Unmanned aerial vehicle automatic landing navigation method and device and electronic equipment |
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 |