CN111862200B - Unmanned aerial vehicle positioning method in coal shed - Google Patents

Unmanned aerial vehicle positioning method in coal shed Download PDF

Info

Publication number
CN111862200B
CN111862200B CN202010607883.8A CN202010607883A CN111862200B CN 111862200 B CN111862200 B CN 111862200B CN 202010607883 A CN202010607883 A CN 202010607883A CN 111862200 B CN111862200 B CN 111862200B
Authority
CN
China
Prior art keywords
map
frame
points
pose
steps
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
Application number
CN202010607883.8A
Other languages
Chinese (zh)
Other versions
CN111862200A (en
Inventor
董延超
王浩天
宁少淳
冀玲玲
岳继光
何斌
沈润杰
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Tongji University
Original Assignee
Tongji University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Tongji University filed Critical Tongji University
Priority to CN202010607883.8A priority Critical patent/CN111862200B/en
Publication of CN111862200A publication Critical patent/CN111862200A/en
Application granted granted Critical
Publication of CN111862200B publication Critical patent/CN111862200B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C11/00—Photogrammetry or videogrammetry, e.g. stereogrammetry; Photographic surveying
    • G01C11/04—Interpretation of pictures
    • 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
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F16/00—Information retrieval; Database structures therefor; File system structures therefor
    • G06F16/20—Information retrieval; Database structures therefor; File system structures therefor of structured data, e.g. relational data
    • G06F16/29—Geographical information databases
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T11/00—2D [Two Dimensional] image generation
    • G06T11/003—Reconstruction from projections, e.g. tomography
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T3/00—Geometric image transformations in the plane of the image
    • G06T3/60—Rotation of whole images or parts thereof
    • G06T3/604—Rotation of whole images or parts thereof using coordinate rotation digital computer [CORDIC] devices
    • 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)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Databases & Information Systems (AREA)
  • Multimedia (AREA)
  • Automation & Control Theory (AREA)
  • Data Mining & Analysis (AREA)
  • General Engineering & Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Image Analysis (AREA)

Abstract

The invention relates to a method for positioning an unmanned aerial vehicle in a coal shed, which comprises the following steps: obtaining a priori visual tag map; step 2: initializing the SLAM system; step 3: solving the pose of the camera, updating the map at the same time, and then removing redundant key frames and redundant map points in the map; step 4: and (3) optimizing the camera pose solved in the step (3) to obtain a final camera pose system, wherein the camera pose system comprises the camera pose, the visual tag pose and the map point space position. Compared with the prior art, the invention has the advantages of high positioning precision and the like.

Description

Unmanned aerial vehicle positioning method in coal shed
Technical Field
The invention relates to the technical field of unmanned aerial vehicle positioning in a coal shed, in particular to a method for positioning an unmanned aerial vehicle in a coal shed based on priori visual labels and natural characteristic point maps.
Background
In the energy structure of China, coal takes the dominant role, and the thermal power generation is mainly performed by coal. In recent years, different types of stacking volume measurement systems are sequentially introduced by higher institutions and scientific research institutions at home and abroad, the system is widely applied to stacking inventory of resource enterprises such as electric power, steel, metallurgy and the like, the laser disk coal system based on the traveling crane or bucket wheel machine at home and abroad has the defects of complex equipment wiring, long measurement period and difficult maintenance, the portable laser disk coal system is relatively simple and convenient to operate, the performance is greatly improved compared with a fixed disk coal instrument, but the whole measurement process needs the participation of operators, and the selection of measurement points directly influences the accuracy of measurement results. Along with the development of unmanned aerial vehicle technology, the coal-coiling system of the thermal power plant based on an unmanned aerial vehicle platform also has rapid development. The rotary wing aircraft has the characteristics of low flying speed, capability of taking off and landing vertically, accurate flying action, flexible operation, simple structure and the like, and is applied to coal coiling work of part of thermal power plants.
For example, chinese patent CN209274918U discloses an unmanned aerial vehicle coal-coiling system, including unmanned aerial vehicle, remote control station and ground station, still including carrying on unmanned aerial vehicle's RTK positioning module and water sample collection system, wherein RTK positioning module is used for obtaining unmanned aerial vehicle's positional information, and this system has the advantage that can independently gather the water sample to obtain unmanned aerial vehicle positional information. However, the positioning accuracy of the unmanned aerial vehicle positioning module in the system is low, and stable and safe navigation information cannot be provided for the coal-based unmanned aerial vehicle system under the condition that GNSS signals are weak or the GNSS signals do not exist.
Disclosure of Invention
The invention aims to overcome the defects of the prior art and provide a positioning method of an unmanned aerial vehicle in a coal shed with high positioning accuracy.
The aim of the invention can be achieved by the following technical scheme:
a method for positioning an unmanned aerial vehicle in a coal shed comprises the following steps:
step 1: obtaining a priori visual tag map;
step 2: initializing the SLAM system;
step 3: solving the pose of the camera, updating the map at the same time, and then removing redundant key frames and redundant map points in the map;
step 4: and (3) optimizing the camera pose solved in the step (3) to obtain a final camera pose system, wherein the camera pose system comprises the camera pose, the visual tag pose and the map point space position.
Preferably, the prior visual tag map in the step 1 is specifically:
the prior visual tag map arranged in the coal shed comprises a plurality of two-dimensional codes, and each two-dimensional code has independent ID and pose information.
Preferably, the step 2 specifically includes:
step 2-1: solving the relative pose between the first frame and the second frame of the image by using a relative pose solving sub-method based on a priori visual label, judging whether the SLAM system successfully solves the relative pose, if so, executing the step 2-3, otherwise, executing the step 2-2;
step 2-2: and (2) solving the relative pose between the first frame and the second frame of the image by using a relative pose solving sub-method based on the characteristic points, and then executing the steps (2-3):
step 2-3: triangularizing the feature points;
step 2-4: and constructing the feature points obtained by triangulation as map points, adding the key frames and the points and the observation attributes between the points and the key frames, calculating the optimal descriptors, updating the observation direction and the distance range, inserting new map points into the map, and finishing the updating of the initial map.
More preferably, the prior visual tag-based relative pose solving sub-method specifically comprises the following steps:
calculate the first frame f 0 Middle visual tag and first frame f 0 Is gamma of the relative pose of (2) 0 Second frame f 1 Middle visual tag and second frame f 1 Is gamma of the relative pose of (2) 1 The relative pose T between two frames is specifically: t=γ 0 -1 *γ 1 。
More preferably, the feature point-based relative pose solving sub-method specifically comprises:
firstly, calculating an essential matrix model and a homography matrix model;
the essential matrix model calculation method comprises the following steps: first frame f of image 0 And a second frame f 1 A pair of characteristic points p 1 and p2 Is the projection of a point P in space in a picture, a pair of characteristic points P 1 and p2 A point P in space can be determined, in the first frame f 0 The spatial position of the point P under the set coordinate system is as follows:
P＝[X,Y,Z] T
in the camera projection model, point p 1 and p2 The pixel coordinates of (a) are respectively:
x 0 ＝KP
x 1 ＝K(RP+t)
wherein K is an internal reference matrix of the camera, and is obtained according to epipolar constraint:
x 1 T K -T t^RK -1 x 0 ＝0
let the essential matrix e=t≡r, then based on the point p 1 and p2 Solving an essential matrix E by pixel positions;
the calculation method of the homography matrix model comprises the following steps: solving a homography matrix H by using a DLT algorithm;
then, the scores of the essential matrix model and the homography matrix model are calculated, and the score calculation method comprises the following steps:
Figure BDA0002561449950000031
Figure BDA0002561449950000032
wherein ,
Figure BDA0002561449950000033
for matching point pairs->
Figure BDA0002561449950000034
Reprojection errors of the current frame are obtained through an essential matrix model or a homography matrix model; />
Figure BDA0002561449950000035
For matching point pairs->
Figure BDA0002561449950000036
Re-projection errors in a reference frame through an essential matrix model or a homography matrix model;
calculating a judgment index R H The calculation method comprises the following steps:
Figure BDA0002561449950000037
if R is H And (3) selecting a homography matrix H to restore the pose (R, t) if the homography matrix H is more than 0.45, otherwise selecting an essential matrix E to restore (R, t).
More preferably, the step 2-3 specifically comprises:
the spatial point P is at the first frame f of the initial frame 0 The positions in the normalized plane of (a) are:
Figure BDA0002561449950000038
the spatial point P is in the second frame f of the current frame 1 The positions in the normalized plane of (a) are:
Figure BDA0002561449950000039
the relation according to the position change is as follows:
z r p r ＝T rw P w
z c p c ＝T cw P w
wherein ,Trw For the identity matrix of the initial frame, T cw For the second frame f 1 And a first frame f 0 Is changed in relative pose;
three-dimensional vector cross multiplication is zero, and the following steps are obtained:
p r ×z r p r ＝p r ×T rw P w ＝0
p c ×z c p c ＝p c ×T cw P w ＝0
after finishing, the following steps are obtained:
Figure BDA0002561449950000041
finally, solving to obtain the space coordinates P of the feature points w 。
More preferably, the camera pose solving method in the step 3 is a first pose solving sub-method or a second pose solving sub-method;
the first pose solving sub-method specifically comprises the following steps:
adopting a constant speed model, obtaining the initial pose of the camera in the current frame according to the pose and the speed of the previous frame image, wherein the pose change of the camera from the moment k to the moment k-1 is the same as the pose change of the camera from the moment k-1 to the moment k-2, namely:
Figure BDA0002561449950000042
thus, there is obtained:
Figure BDA0002561449950000043
the above steps are recursively and inversely transformed, and the concrete steps are as follows:
Figure BDA0002561449950000044
Figure BDA0002561449950000045
Figure BDA0002561449950000046
the movement speed of the camera is as follows:
Figure BDA0002561449950000047
calculating a translation vector t from the current frame to the previous frame, re-projecting the three-dimensional point corresponding to the characteristic point of the last frame to the current frame, searching the characteristic point at the re-projection position, and optimizing the pose of the current frame according to a method for minimizing the re-projection error;
the second pose solving sub-method specifically comprises the following steps:
obtaining a matching map point and visual labels obtained by matching a reference key frame with a current frame, and if the number of the matched visual labels is more than two, according to the world pose of the visual labels
Figure BDA0002561449950000048
And the relative pose relationship of the local coordinate system of the visual tag and the current frame coordinate system +.>
Figure BDA0002561449950000049
Obtaining the initial pose of the current frame:
Figure BDA00025614499500000410
then optimizing the pose of the current frame according to a method for minimizing the reprojection error;
the calculation method of the re-projection error comprises the following steps:
Figure BDA0002561449950000051
Figure BDA0002561449950000052
wherein, (R, t) is the transformation relation of the visual label corner point converted from the visual label local coordinate system to the camera coordinate system; x is x i Pixel coordinates of corner points of the visual tag in the image; p is p i The coordinates of the corner points of the visual tag in a local coordinate system of the visual tag; pi m Is a camera projection model.
Preferably, the specific method for updating the map in the step 3 is as follows:
for the current frame, if: at least one new visual label which does not exist in the original map or the number of the feature points tracked in the current frame is more than 50 or the repetition rate of the feature points tracked by the current frame and the reference key frame is less than 90 percent, the current frame is used as the key frame to be added into the map, and meanwhile, the map points obtained by triangulating the current frame are also added into the map.
Preferably, the method for removing redundant key frames in the map specifically includes:
if three key frames exist in the map, more than 90% of map points and all visual labels in the key frames can be observed, judging the key frames as redundant key frames, and removing the redundant key frames from the map;
the method for removing the redundant map points comprises the following steps:
and projecting map points to the common-view key frame, extracting characteristic points in a grid where a projection area of the common-view key frame is located, and if the Hamming distance of the characteristic point descriptors is smaller than 30, replacing the current map points with corresponding map points in the common-view key frame.
Preferably, the step 4 specifically includes:
firstly, a key frame with more than 50 common-view map points or more than 2 common-view visual labels and corresponding map points and visual labels are searched in a map, and then the pose, the pose of the visual labels and the spatial position of the map points of a camera with common-view relation in the tracking process are optimized by using an L-M method, wherein the optimization method comprises the following steps:
Figure BDA0002561449950000053
Figure BDA0002561449950000054
wherein ,w1 and w2 Respectively representing the proportion of map points and visual labels in the optimization; p is p i p Representing map points in a space; x is x i p Representing a pixel point corresponding to the map point; p is p i m Representing coordinates of the visual tag corner points in a visual tag local coordinate system; x is x i m Representing pixel points corresponding to the corner points of the visual labels; pi m Representing a projection equation of the camera;
the w is 1 and w2 The calculation method of (1) is as follows:
w 1 ＝1
Figure BDA0002561449950000061
wherein m is the number of map points observed in the current frame; n is the number of a priori data tags observed in the current frame.
Compared with the prior art, the invention has the following advantages:
the positioning accuracy is higher: according to the method for positioning the unmanned aerial vehicle in the coal shed, the real pose of the unmanned aerial vehicle is determined through the priori visual tag map, and the dimension and the pose of the priori visual tag are known, so that the accuracy of the pose of the unmanned aerial vehicle determined through the priori visual tag is higher, and the positioning accuracy of the unmanned aerial vehicle is also higher.
Drawings
FIG. 1 is a schematic flow chart of a method for positioning an unmanned aerial vehicle in a coal shed;
FIG. 2 is a layout of a priori visual tags in an embodiment of the present invention;
FIG. 3 is a schematic diagram of a SLAM map according to an embodiment of the present invention;
fig. 4 is a schematic diagram of path planning of the unmanned aerial vehicle according to an embodiment of the present invention;
FIG. 5 is a graph comparing the trajectory on the X-axis of a method for positioning a drone using the present invention and a method for positioning a drone using GPS in an embodiment of the present invention;
FIG. 6 is a graph comparing the trajectory on the Y-axis of a method for positioning a drone using the present invention and a method for positioning a drone using GPS in an embodiment of the present invention;
fig. 7 is a graph comparing the trajectory on the Z axis of the present invention using the present invention and the unmanned positioning method using GPS.
Detailed Description
The following description of the embodiments of the present invention will be made clearly and fully with reference to the accompanying drawings, in which it is evident that the embodiments described are some, but not all embodiments of the invention. All other embodiments, which can be made by those skilled in the art based on the embodiments of the present invention without making any inventive effort, shall fall within the scope of the present invention.
The method for positioning the unmanned aerial vehicle in the coal shed comprises the following steps:
step 1: obtaining a priori visual tag map;
as with human mapping, the process of describing and recognizing the environment by the robot mainly depends on the map, and the current environment information is described through the environment map. As shown in fig. 2, a map comprising a plurality of two-dimensional codes is arranged in a coal shed, each two-dimensional code has independent ID and pose information, and the visual tag map is used as priori knowledge of unmanned aerial vehicle navigation;
step 2: initializing the SLAM system, specifically:
step 2-1: solving the relative pose between the first frame and the second frame of the image by using a relative pose solving sub-method based on a priori visual label, judging whether the SLAM system successfully solves the relative pose, if so, executing the step 2-3, otherwise, executing the step 2-2;
step 2-2: and (2) solving the relative pose between the first frame and the second frame of the image by using a relative pose solving sub-method based on the characteristic points, and then executing the steps (2-3):
the prior visual tag-based relative pose solving sub-method specifically comprises the following steps:
calculate the first frame f 0 Middle visual tag and first frame f 0 Is gamma of the relative pose of (2) 0 Second frame f 1 Middle visual tag and second frame f 1 Is gamma of the relative pose of (2) 1 The relative pose T between two frames is specifically: t=γ 0 -1 *γ 1 ；
The relative pose solving sub-method based on the characteristic points specifically comprises the following steps:
firstly, calculating an essential matrix model and a homography matrix model;
the essential matrix model calculation method comprises the following steps: first frame f of image 0 And a second frame f 1 A pair of characteristic points p 1 and p2 Is the projection of a point P in space in a picture, a pair of characteristic points P 1 and p2 A point P in space can be determined, in the first frame f 0 The spatial position of the point P under the set coordinate system is as follows:
P＝[X,Y,Z] T
in the camera projection model, point p 1 and p2 The pixel coordinates of (a) are respectively:
x 0 ＝KP
x 1 ＝K(RP+t)
wherein K is an internal reference matrix of the camera, and is obtained according to epipolar constraint:
x 1 T K -T t^RK -1 x 0 ＝0
let the essential matrix e=t≡r, then based on the point p 1 and p2 Solving an essential matrix E by pixel positions;
the calculation method of the homography matrix model comprises the following steps: solving a homography matrix H by using a DLT algorithm;
then, the scores of the essential matrix model and the homography matrix model are calculated, and the score calculation method comprises the following steps:
Figure BDA0002561449950000081
Figure BDA0002561449950000082
wherein ,
Figure BDA0002561449950000083
for matching point pairs->
Figure BDA0002561449950000084
Reprojection errors of the current frame are obtained through an essential matrix model or a homography matrix model; />
Figure BDA0002561449950000085
For matching point pairs->
Figure BDA0002561449950000086
Re-projection errors in a reference frame through an essential matrix model or a homography matrix model;
calculating a judgment index R H The calculation method comprises the following steps:
Figure BDA0002561449950000087
if R is H More than 0.45, the homography matrix H is selected to recover the pose (R, t) Otherwise, selecting an essential matrix Eback (R, t);
step 2-3: triangularizing the feature points, wherein the method comprises the following specific steps of:
the spatial point P is at the first frame f of the initial frame 0 The positions in the normalized plane of (a) are:
Figure BDA0002561449950000088
the spatial point P is in the second frame f of the current frame 1 The positions in the normalized plane of (a) are:
Figure BDA0002561449950000089
the relation according to the position change is as follows:
z r p r ＝T rw P w
z c p c ＝T cw P w
wherein ,Trw For the identity matrix of the initial frame, T cw For the second frame f 1 And a first frame f 0 Is changed in relative pose;
three-dimensional vector cross multiplication is zero, and the following steps are obtained:
p r ×z r p r ＝p r ×T rw P w ＝0
p c ×z c p c ＝p c ×T cw P w ＝0
after finishing, the following steps are obtained:
Figure BDA00025614499500000810
finally, solving to obtain the space coordinates P of the feature points w 。
Step 2-4: and constructing the feature points obtained by triangulation as map points, adding the key frames and the points and the observation attributes between the points and the key frames, calculating the optimal descriptors, updating the observation direction and the distance range, inserting new map points into the map, and finishing the updating of the initial map.
Step 3: solving the pose of the camera, updating the map at the same time, and then removing redundant key frames and redundant map points in the map;
the pose is solved in two ways, namely, the pose is solved by using a first pose solving sub-method or a second pose solving sub-method.
The first pose solving sub-method specifically comprises the following steps:
adopting a constant speed model, obtaining the initial pose of the camera in the current frame according to the pose and the speed of the previous frame image, wherein the pose change of the camera from the moment k to the moment k-1 is the same as the pose change of the camera from the moment k-1 to the moment k-2, namely:
Figure BDA0002561449950000091
thus, there is obtained:
Figure BDA0002561449950000092
the above steps are recursively and inversely transformed, and the concrete steps are as follows:
Figure BDA0002561449950000093
Figure BDA0002561449950000094
Figure BDA0002561449950000095
the movement speed of the camera is as follows:
Figure BDA0002561449950000096
calculating a translation vector t from the current frame to the previous frame, re-projecting the three-dimensional point corresponding to the characteristic point of the last frame to the current frame, searching the characteristic point at the re-projection position, and optimizing the pose of the current frame according to a method for minimizing the re-projection error;
the second pose solving sub-method specifically comprises the following steps:
obtaining a matching map point and visual labels obtained by matching a reference key frame with a current frame, and if the number of the matched visual labels is more than two, according to the world pose of the visual labels
Figure BDA0002561449950000097
And the relative pose relationship of the local coordinate system of the visual tag and the current frame coordinate system +.>
Figure BDA0002561449950000098
Obtaining the initial pose of the current frame:
Figure BDA0002561449950000099
then optimizing the pose of the current frame according to a method for minimizing the reprojection error;
the calculation method of the re-projection error comprises the following steps:
Figure BDA0002561449950000101
Figure BDA0002561449950000102
wherein, (R, t) is the transformation relation of the visual label corner point converted from the visual label local coordinate system to the camera coordinate system; x is x i Pixel coordinates of corner points of the visual tag in the image; p is p i The coordinates of the corner points of the visual tag in a local coordinate system of the visual tag; pi m A projection model for a camera;
the specific method for updating the map comprises the following steps:
for the current frame, if:
(1) At least one new visual label which does not exist in the original map or the number of the feature points tracked in the current frame is more than 50;
(2) The repetition rate of the feature points tracked by the current frame and the reference key frames is less than 90%;
if either of the two conditions is satisfied, the current frame is added to the map as a key frame, and the map point obtained by triangulating the current frame is also added to the map.
The method for removing the redundant key frames in the map comprises the following steps:
if three key frames exist in the map, more than 90% of map points and all visual labels in the key frames can be observed, judging the key frames as redundant key frames, and removing the redundant key frames from the map;
the method for removing the redundant map points comprises the following steps:
and projecting map points to the common-view key frame, extracting characteristic points in a grid where a projection area of the common-view key frame is located, and if the Hamming distance of the characteristic point descriptors is smaller than 30, replacing the current map points with corresponding map points in the common-view key frame.
Step 4: optimizing the camera pose solved in the step 3 to obtain a final camera pose system, wherein the final camera pose system comprises the spatial positions of the camera pose, the visual tag pose and map points, and the method specifically comprises the following steps:
firstly, a key frame with more than 50 common-view map points or more than 2 common-view visual labels and corresponding map points and visual labels are searched in a map, and then a camera gesture system with a common-view relation in a tracking process, namely a camera pose, a visual label pose and a map point space position are optimized by using an L-M method, wherein the optimization method comprises the following steps:
Figure BDA0002561449950000111
Figure BDA0002561449950000112
/>
wherein ,w1 and w2 Respectively representing the proportion of map points and visual labels in the optimization; p is p i p Representing map points in a space; x is x i p Representing a pixel point corresponding to the map point; p is p i m Representing coordinates of the visual tag corner points in a visual tag local coordinate system; x is x i m Representing pixel points corresponding to the corner points of the visual labels; pi m Representing a projection equation of the camera;
the w is 1 and w2 The calculation method of (1) is as follows:
w 1 ＝1
Figure BDA0002561449950000113
wherein m is the number of map points observed in the current frame; n is the number of a priori data tags observed in the current frame.
Finally, the optimized camera attitude system, namely the camera pose, the visual tag pose and the map point space position, is obtained, so that the accurate positioning of the unmanned aerial vehicle in the coal shed is completed.
Meanwhile, when the steps are carried out, maintenance and update are carried out on the SLAM Map in real time, the Map storage is to store each element in Map and the relation between the elements, and the data needed in the tracking process are natural, namely objects to be stored. The map mainly comprises a key frame, natural characteristic map points, two-dimensional codes, a BoW vector, a common view, a growing tree and the like, wherein the three tracking models and the local map tracking and the like are adopted in the tracking process, the local map tracking needs to use elements such as a 3D map point, a common view relation and the like, the reference frame model needs to use the BoW vector of the key frame, the repositioning needs to use the BoW vector, the 3D point, the two-dimensional codes and the like, and the basic elements need to be stored.
In this embodiment, as shown in fig. 3, a SLAM map constructed by an unmanned aerial vehicle includes a visual tag, three-dimensional map points and a key frame, and new elements are continuously added into the map along with the movement of the unmanned aerial vehicle; meanwhile, the map has a storage function, and the map stored before the same scene is used for multiple movements can be used as prior information; the visual tag elements in the map can bring a strong constraint to the positioning of the unmanned aerial vehicle, and when the corresponding visual tag is detected in the image, the pose of the unmanned aerial vehicle and the pose of the visual tag are optimized simultaneously. The path planned by the unmanned aerial vehicle according to the prior visual tag is shown in fig. 4, and the world coordinate system can be constructed through the visual tag and the path planning can be performed. Meanwhile, the unmanned aerial vehicle positioning method and the unmanned aerial vehicle running track determined by the unmanned aerial vehicle positioning method based on the GPS are compared in an outdoor meadow lean texture scene, and schematic diagrams are shown in fig. 5, 6 and 7, and can be seen from the figures. The unmanned aerial vehicle track output by the unmanned aerial vehicle positioning method is basically consistent with the unmanned aerial vehicle track output by the GPS, the unmanned aerial vehicle can perform high-precision positioning in real time in the autonomous flight process, the average distance errors of the x axis, the y axis and the z axis are 0.072m, 0.107m and 0.206m, the scale information is consistent with the real world, and the unmanned aerial vehicle positioning method can still realize high-precision positioning for poor texture scenes which cannot be operated by the traditional vision SLAM systems of coal sheds, grasslands, factories and the like.
While the invention has been described with reference to certain preferred embodiments, it will be understood by those skilled in the art that various changes and substitutions of equivalents may be made and equivalents will be apparent to those skilled in the art without departing from the scope of the invention. Therefore, the protection scope of the invention is subject to the protection scope of the claims.

Claims (7)

1. The unmanned aerial vehicle positioning method in the coal shed is characterized by comprising the following steps of:
step 1: obtaining a priori visual tag map;
step 2: initializing the SLAM system;
step 3: solving the pose of the camera, updating the map at the same time, and then removing redundant key frames and redundant map points in the map;
step 4: optimizing the camera pose solved in the step 3 to obtain a final camera pose system, wherein the camera pose system comprises the camera pose, the visual tag pose and the map point space position;
the step 4 specifically comprises the following steps:
firstly, a key frame with more than 50 common-view map points or more than 2 common-view visual labels and corresponding map points and visual labels are searched in a map, and then the pose, the pose of the visual labels and the spatial position of the map points of a camera with common-view relation in the tracking process are optimized by using an L-M method, wherein the optimization method comprises the following steps:
Figure FDA0004014209040000011
Figure FDA0004014209040000012
wherein ,w1 and w2 Respectively representing the proportion of map points and visual labels in the optimization; p is p i p Representing map points in a space; x is x i p Representing a pixel point corresponding to the map point; p is p i m Representing coordinates of the visual tag corner points in a visual tag local coordinate system; x is x i m Representing pixel points corresponding to the corner points of the visual labels; pi m Representing a projection equation of the camera;
Figure FDA0004014209040000013
world pose representing visual tag +.>
Figure FDA0004014209040000014
Representing an initial pose of the current frame;
the w is 1 and w2 Is calculated by the method of (a)The method comprises the following steps:
w 1 ＝1
Figure FDA0004014209040000015
wherein m is the number of map points observed in the current frame; n is the number of a priori data tags observed in the current frame;
the step 2 specifically comprises the following steps:
step 2-1: solving the relative pose between the first frame and the second frame of the image by using a relative pose solving sub-method based on a priori visual label, judging whether the SLAM system successfully solves the relative pose, if so, executing the step 2-3, otherwise, executing the step 2-2;
step 2-2: and (2) solving the relative pose between the first frame and the second frame of the image by using a relative pose solving sub-method based on the characteristic points, and then executing the steps (2-3):
step 2-3: triangularizing the feature points;
step 2-4: and constructing the feature points obtained by triangulation as map points, adding the key frames and the points and the observation attributes between the points and the key frames, calculating the optimal descriptors, updating the observation direction and the distance range, inserting new map points into the map, and finishing the updating of the initial map.
2. The method for positioning the unmanned aerial vehicle in the coal shed according to claim 1, wherein the priori visual tag map in the step 1 is specifically:
the prior visual tag map arranged in the coal shed comprises a plurality of two-dimensional codes, and each two-dimensional code has independent ID and pose information.
3. The method for positioning the unmanned aerial vehicle in the coal shed according to claim 1, wherein the prior visual tag-based relative pose solving sub-method is specifically as follows:
calculate the first frame f 0 Middle visual tag and first frame f 0 Is gamma of the relative pose of (2) 0 Second frame f 1 Middle visual tag and second frame f 1 Is gamma of the relative pose of (2) 1 The relative pose T between two frames is specifically: t=γ 0 -1 *γ 1 。
4. The method for positioning the unmanned aerial vehicle in the coal shed according to claim 1, wherein the characteristic point-based relative pose solving sub-method is specifically as follows:
firstly, calculating an essential matrix model and a homography matrix model;
the essential matrix model calculation method comprises the following steps: first frame f of image 0 And a second frame f 1 A pair of characteristic points p 1 and p2 Is the projection of a point P in space in a picture, a pair of characteristic points P 1 and p2 A point P in space can be determined, in the first frame f 0 The spatial position of the point P under the set coordinate system is as follows:
P＝[X,Y,Z] T
in the camera projection model, point p 1 and p2 The pixel coordinates of (a) are respectively:
x 0 ＝KP
x 1 ＝K(RP+t)
wherein K is an internal reference matrix of the camera, and is obtained according to epipolar constraint:
x 1 T K -T t^RK -1 x 0 ＝0
let the essential matrix e=t≡r, then based on the point p 1 and p2 Solving an essential matrix E by pixel positions;
the calculation method of the homography matrix model comprises the following steps: solving a homography matrix H by using a DLT algorithm;
then, the scores of the essential matrix model and the homography matrix model are calculated, and the score calculation method comprises the following steps:
Figure FDA0004014209040000031
Figure FDA0004014209040000032
wherein ,
Figure FDA0004014209040000033
for matching point pairs->
Figure FDA0004014209040000034
Reprojection errors of the current frame are obtained through an essential matrix model or a homography matrix model; />
Figure FDA0004014209040000035
For matching point pairs->
Figure FDA0004014209040000036
Re-projection errors in a reference frame through an essential matrix model or a homography matrix model;
calculating a judgment index R H The calculation method comprises the following steps:
Figure FDA0004014209040000037
if R is H And (3) selecting a homography matrix H to restore the pose (R, t) if the homography matrix H is more than 0.45, otherwise selecting an essential matrix E to restore (R, t).
5. The method for positioning the unmanned aerial vehicle in the coal shed according to claim 1, wherein the steps 2-3 are specifically as follows:
the spatial point P is at the first frame f of the initial frame 0 The positions in the normalized plane of (a) are:
Figure FDA0004014209040000038
the spatial point P is in the second frame f of the current frame 1 The positions in the normalized plane of (a) are：
Figure FDA0004014209040000039
The relation according to the position change is as follows:
z r p r ＝T rw P w
z c p c ＝T cw P w
wherein ,Trw For the identity matrix of the initial frame, T cw For the second frame f 1 And a first frame f 0 Is changed in relative pose;
three-dimensional vector cross multiplication is zero, and the following steps are obtained:
p r ×z r p r ＝p r ×T rw P w ＝0
p c ×z c p c ＝p c ×T cw P w ＝0
after finishing, the following steps are obtained:
Figure FDA00040142090400000310
finally, solving to obtain the space coordinates P of the feature points w 。
6. The method for positioning the unmanned aerial vehicle in the coal shed according to claim 1, wherein the specific method for updating the map in the step 3 is as follows:
for the current frame, if: at least one new visual label which does not exist in the original map or the number of the feature points tracked in the current frame is more than 50 or the repetition rate of the feature points tracked by the current frame and the reference key frame is less than 90 percent, the current frame is used as the key frame to be added into the map, and meanwhile, the map points obtained by triangulating the current frame are also added into the map.
7. The method for positioning the unmanned aerial vehicle in the coal shed according to claim 1, wherein the method for removing the redundant key frames in the map is specifically as follows:
if three key frames exist in the map, more than 90% of map points and all visual labels in the key frames can be observed, judging the key frames as redundant key frames, and removing the redundant key frames from the map;
the method for removing the redundant map points comprises the following steps:
and projecting map points to the common-view key frame, extracting characteristic points in a grid where a projection area of the common-view key frame is located, and if the Hamming distance of the characteristic point descriptors is smaller than 30, replacing the current map points with corresponding map points in the common-view key frame.
CN202010607883.8A 2020-06-30 2020-06-30 Unmanned aerial vehicle positioning method in coal shed Active CN111862200B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010607883.8A CN111862200B (en) 2020-06-30 2020-06-30 Unmanned aerial vehicle positioning method in coal shed

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010607883.8A CN111862200B (en) 2020-06-30 2020-06-30 Unmanned aerial vehicle positioning method in coal shed

Publications (2)

Publication Number Publication Date
CN111862200A CN111862200A (en) 2020-10-30
CN111862200B true CN111862200B (en) 2023-04-28

Family

ID=72989892

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010607883.8A Active CN111862200B (en) 2020-06-30 2020-06-30 Unmanned aerial vehicle positioning method in coal shed

Country Status (1)

Country Link
CN (1) CN111862200B (en)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112945240B (en) * 2021-03-16 2022-06-07 北京三快在线科技有限公司 Method, device and equipment for determining positions of feature points and readable storage medium
CN113345017B (en) * 2021-05-11 2022-09-20 香港理工大学深圳研究院 Method for assisting visual SLAM by using mark
CN115170847B (en) * 2022-09-05 2022-12-13 煤炭科学研究总院有限公司 Coal mine underground monocular vision positioning method and device based on prior map constraint

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102359788A (en) * 2011-09-09 2012-02-22 华中科技大学 Series image target recursive identification method based on platform inertia attitude parameter
CN107830854A (en) * 2017-11-06 2018-03-23 深圳精智机器有限公司 Vision positioning method based on sparse cloud of ORB and Quick Response Code
CN109345588A (en) * 2018-09-20 2019-02-15 浙江工业大学 A kind of six-degree-of-freedom posture estimation method based on Tag
CN109509230A (en) * 2018-11-13 2019-03-22 武汉大学 A kind of SLAM method applied to more camera lens combined type panorama cameras
CN109544636A (en) * 2018-10-10 2019-03-29 广州大学 A kind of quick monocular vision odometer navigation locating method of fusion feature point method and direct method
WO2019169540A1 (en) * 2018-03-06 2019-09-12 斯坦德机器人（深圳）有限公司 Method for tightly-coupling visual slam, terminal and computer readable storage medium
CN110533587A (en) * 2019-07-03 2019-12-03 浙江工业大学 A kind of SLAM method of view-based access control model prior information and map recovery
CN111060115A (en) * 2019-11-29 2020-04-24 中国科学院计算技术研究所 Visual SLAM method and system based on image edge features

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102359788A (en) * 2011-09-09 2012-02-22 华中科技大学 Series image target recursive identification method based on platform inertia attitude parameter
CN107830854A (en) * 2017-11-06 2018-03-23 深圳精智机器有限公司 Vision positioning method based on sparse cloud of ORB and Quick Response Code
WO2019169540A1 (en) * 2018-03-06 2019-09-12 斯坦德机器人（深圳）有限公司 Method for tightly-coupling visual slam, terminal and computer readable storage medium
CN109345588A (en) * 2018-09-20 2019-02-15 浙江工业大学 A kind of six-degree-of-freedom posture estimation method based on Tag
CN109544636A (en) * 2018-10-10 2019-03-29 广州大学 A kind of quick monocular vision odometer navigation locating method of fusion feature point method and direct method
CN109509230A (en) * 2018-11-13 2019-03-22 武汉大学 A kind of SLAM method applied to more camera lens combined type panorama cameras
CN110533587A (en) * 2019-07-03 2019-12-03 浙江工业大学 A kind of SLAM method of view-based access control model prior information and map recovery
CN111060115A (en) * 2019-11-29 2020-04-24 中国科学院计算技术研究所 Visual SLAM method and system based on image edge features

Non-Patent Citations (6)

* Cited by examiner, † Cited by third party
Title
A Novel Texture-Less Object Oriented Visual SLAM System;Yanchao Dong, Senbo Wang , Jiguang Yue, Ce Chen, Shibo He, Haoti;《IEEE Transactions on Intelligent Transportation Systems》;20191114;全文 *
Rau＇l Mur-Artal and Juan D. Tardo＇s.ORB-SLAM2: An Open-Source SLAM System for Monocular, Stereo,and RGB-D Cameras.《IEEE TRANSACTIONS ON ROBOTICS》.2017, *
Rau＇l Mur-Artal, J. M. M. Montiel, Juan D. Tardo＇s.ORB-SLAM: A Versatile and Accurate Monocular SLAM System.《IEEE TRANSACTIONS ON ROBOTICS》.2015, *
Real-time Omnidirectional Visual SLAM with Semi-Dense Mapping;Senbo Wang，iguang Yue，Yanchao Dong，Runjie Shen，Xinyu Zhang;《2018 IEEE Intelligent Vehicles Symposium (IV)》;20181022;全文 *
基于图优化 SLAM 的移动机器人导航方法研究;邹 谦;《中国优秀博硕士学位论文全文数据库(硕士)信息科技辑》;20180215;全文 *
高翔，张涛.《视觉SLAM十四讲》.《视觉SLAM十四讲》.2019, *

Also Published As

Publication number Publication date
CN111862200A (en) 2020-10-30

Similar Documents

Publication Publication Date Title
CN112347840B (en) Vision sensor laser radar integrated unmanned aerial vehicle positioning and image building device and method
CN111862200B (en) Unmanned aerial vehicle positioning method in coal shed
CN105843223B (en) A kind of mobile robot three-dimensional based on space bag of words builds figure and barrier-avoiding method
CN109186606B (en) Robot composition and navigation method based on SLAM and image information
CN107741234A (en) The offline map structuring and localization method of a kind of view-based access control model
CN102426019B (en) Unmanned aerial vehicle scene matching auxiliary navigation method and system
Li et al. Efficient laser-based 3D SLAM for coal mine rescue robots
CN102967305B (en) Multi-rotor unmanned aerial vehicle pose acquisition method based on markers in shape of large and small square
CN109579843A (en) Multirobot co-located and fusion under a kind of vacant lot multi-angle of view build drawing method
CN110361027A (en) Robot path planning method based on single line laser radar Yu binocular camera data fusion
CN111178148B (en) Ground target geographic coordinate positioning method based on unmanned aerial vehicle vision system
CN110930495A (en) Multi-unmanned aerial vehicle cooperation-based ICP point cloud map fusion method, system, device and storage medium
CN105865449A (en) Laser and vision-based hybrid location method for mobile robot
CN111664843A (en) SLAM-based intelligent storage checking method
CN108267121A (en) The vision navigation method and system of more equipment under a kind of variable scene
CN110827353B (en) Robot positioning method based on monocular camera assistance
CN112833892B (en) Semantic mapping method based on track alignment
CN114332360A (en) Collaborative three-dimensional mapping method and system
CN111812978B (en) Cooperative SLAM method and system for multiple unmanned aerial vehicles
Liu A robust and efficient lidar-inertial-visual fused simultaneous localization and mapping system with loop closure
Zhang et al. ST-VIO: Visual-inertial odometry combined with image segmentation and tracking
CN116295412A (en) Depth camera-based indoor mobile robot dense map building and autonomous navigation integrated method
CN113674355A (en) Target identification and positioning method based on camera and laser radar
CN107741233A (en) A kind of construction method of the outdoor map of three-dimensional
CN117406771B (en) Efficient autonomous exploration method, system and equipment based on four-rotor unmanned aerial vehicle

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