CN112179357B - Method and system for visual navigation of plane moving target based on monocular camera - Google Patents

Method and system for visual navigation of plane moving target based on monocular camera Download PDF

Info

Publication number
CN112179357B
CN112179357B CN202011022982.6A CN202011022982A CN112179357B CN 112179357 B CN112179357 B CN 112179357B CN 202011022982 A CN202011022982 A CN 202011022982A CN 112179357 B CN112179357 B CN 112179357B
Authority
CN
China
Prior art keywords
coordinate system
moving target
point
pose
camera
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
CN202011022982.6A
Other languages
Chinese (zh)
Other versions
CN112179357A (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.)
National University of Defense Technology
Original Assignee
National University of Defense Technology
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 National University of Defense Technology filed Critical National University of Defense Technology
Priority to CN202011022982.6A priority Critical patent/CN112179357B/en
Publication of CN112179357A publication Critical patent/CN112179357A/en
Application granted granted Critical
Publication of CN112179357B publication Critical patent/CN112179357B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G06—COMPUTING OR CALCULATING; COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Theoretical Computer Science (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

The invention relates to a monocular camera-based visual navigation method and a monocular camera-based visual navigation system for a plane moving target, which comprises the steps of establishing a world coordinate system, a camera coordinate system, a moving target coordinate system and a photo plane coordinate system; the monocular camera forms a 2D-3D point pair by acquiring a control point in real time and a point pair of which the control point falls on a physical coordinate system of an image plane in the moving process of the moving target; carrying out coordinate conversion on the 2D-3D point pair according to a coordinate system, and solving the pose of the moving target; and assisting the moving target to move forwards according to the pose of the moving target. The invention adopts a thought method of space geometric modeling and an RANSAC algorithm, models a set of all possible positions of the optical center of a camera into a circle which takes a control point as a circle center and the distance between the control point and the optical center as a radius, decouples unknown position parameters and attitude parameters in a collinear equation and then respectively solves the unknown position parameters and attitude parameters, and simultaneously eliminates outliers by using the RANSAC algorithm, thereby being a real-time visual navigation method with high speed, high robustness and high precision.

Description

Monocular camera-based visual navigation method and system for plane moving target
Technical Field
The invention belongs to the field of visual navigation, and particularly relates to a monocular camera-based visual navigation method and system for a plane moving target.
Background
The visual navigation method is more and more concerned about due to the characteristics of non-contact, high precision, low cost and the like, and compared with a laser and radar navigation method, the visual navigation information acquisition stage only uses an optical camera to acquire an image without actively transmitting laser and electromagnetic waves to a target and receiving returned information; compared with the global satellite navigation system, the visual navigation system is not limited by the coverage of satellite signals and has very strong anti-interference performance, radio and GPS signals are easy to interfere and block, the visual navigation system can be carried out indoors or even underground, a satellite navigation system does not need to be established, a signal receiving device does not need to be developed, and the cost is very low.
The plane motion is an important and frequently-appearing scene in the application of the unmanned device, the visual navigation of a plane motion target is an important branch of the visual navigation, the camera pose is estimated in real time by using a few control points or feature points and point pairs consisting of corresponding image points, and the position and the posture of the motion target are further estimated in real time according to the installation relationship of the camera and the motion target. The method has very important application value in visual odometers, robots and unmanned dispatching.
The existing absolute pose estimation method based on two pairs of 2D-3D point pairs is difficult to meet the real-time requirement due to the complex calculation and long pose estimation time; in the other method, because the relative pose between the optical center of the camera and the moving target is not considered, and the center of the moving target is equal to the optical center and the optical axis of the camera is parallel to the plane where the moving target is located, the method causes larger error and inaccurate positioning in practical application.
Disclosure of Invention
The invention aims to solve the technical problem of how to quickly and real-timely perform visual navigation of a moving target under the condition of considering the relative pose between an optical center of a camera and the moving target, and provides a monocular camera-based visual navigation method and a monocular camera-based visual navigation system for a plane moving target.
In order to solve the problem, the technical scheme adopted by the invention is as follows:
a monocular camera-based visual navigation method for a plane moving target comprises the following steps:
step 1: constructing a world coordinate system W-XYZ and a moving object coordinate system B-XB YB ZBCamera coordinate system C-XC YCZCImage plane physical coordinate system
Figure BDA0002701262800000011
The monocular camera is arranged on a moving target, the moving plane where the moving target is located is an X-Y plane, and the origin of the moving target coordinate system isPoint B is the center of the moving target, and the coordinate of the origin B in the world coordinate system is tb＝[tx，ty，tz]Said image plane physical coordinate system
Figure BDA0002701262800000021
The origin O is the monocular camera optical axis CZCThe point of intersection with the image plane is,
Figure BDA0002701262800000022
shaft and
Figure BDA0002701262800000023
the axial direction is consistent with the coordinate system of the camera, the coordinate system of the image pixel is I-xy, and the visual angle direction of the image shot by the monocular camera is taken as
Figure BDA0002701262800000024
The upper left corner I of the image plane is an original point, and the directions of the x axis and the y axis and the image plane are in a physical coordinate system
Figure BDA0002701262800000025
The consistency is achieved;
step 2: the monocular camera forms a 2D-3D point pair by acquiring a control point in real time and a point pair of which the control point falls on an image plane physical coordinate system in the moving target forward process;
and step 3: carrying out coordinate conversion on the 2D-3D point pair according to the coordinate system in the step 1, and solving the pose of the moving target;
and 4, step 4: and assisting the moving target to move forwards according to the pose of the moving target.
Further, the method for acquiring the 2D-3D point pair is as follows:
(1) coding cooperation marks are uniformly distributed on control points in a moving target activity scene;
(2) extracting image points corresponding to the control points by using a template matching method;
(3) and eliminating the interference points to obtain 2D-3D point pairs.
Further, the relationship between the coordinate systems in step 1 is:
1) controlSystem point in camera coordinate system C-XCYCZCAnd a moving object coordinate system B-XBYBZBThe conversion relation on the coordinates in (1) is:
Figure BDA0002701262800000026
wherein
Figure BDA0002701262800000027
To control the coordinates of the point in the camera coordinate system,
Figure BDA0002701262800000028
as coordinates of the control point in the coordinate system of the moving object, tc＝(txc，tyc，tzc) The coordinates of the optical center C of the camera under a moving target coordinate system; rCAs a camera coordinate system C-XCYCZCRelative to a moving object coordinate system B-XBYBZBOf the rotation matrix
Figure BDA0002701262800000029
The angle alpha is a pitch angle of the camera coordinate system relative to the moving target coordinate system;
2) the control points are in a world coordinate system W-XYZ and a moving target coordinate system B-XBYBZBThe middle coordinate transformation relation is
Figure BDA0002701262800000031
Wherein
Figure BDA0002701262800000032
Respectively the coordinates of the control point in the world coordinate system W-XYZ, tb＝[tx，ty，tz]For the coordinates of the moving object in the world coordinate system,
Figure BDA0002701262800000033
the method comprises the following steps that a rotation matrix of a moving target coordinate system relative to a world coordinate system is obtained, and theta is a yaw angle of the moving target;
3) let the focal length of the camera be denoted as f and the pixel size be denoted as (d)x，dy) Intrinsic parameter matrix of camera
Figure BDA0002701262800000034
Wherein
Figure BDA0002701262800000035
Is an equivalent focal length, (C)x，Cy) Is the coordinate of the image principal point;
4) establishing collinearity equations
Figure BDA0002701262800000036
Wherein the lambda is a proportionality coefficient,
Figure BDA0002701262800000037
the coordinates of the image points are in homogeneous order,
Figure BDA0002701262800000038
and u, v and w are intermediate variables of the world coordinates of the corresponding points.
Further, the method for solving the pose of the moving object in the step 3 is as follows:
when the 2D-3D point pairs are in 2 groups, the method for solving the pose of the moving object comprises the following steps:
step 3.1: solving the horizontal distance D from the optical center C of the camera to the control point;
c is an optical center, C' is the projection of C on the X-Y plane of a world coordinate system, an image point P and a control point P are a group of 2D-3D point pairs, the included angle between the optical axis and the horizontal plane is alpha, and the image plane physical coordinate system of the image point P (X, Y)
Figure BDA0002701262800000039
Coordinates of (2)
Figure BDA00027012628000000310
Satisfy the requirement of
Figure BDA00027012628000000311
Figure BDA00027012628000000312
Wherein (d)x，dy) Is the pixel size (C)x，Cy) As principal point-like coordinates. p is a radical ofyIs like a point at
Figure BDA00027012628000000313
Projected point on axis, PyIs a reaction of with pyCorresponding object space points. Optical axis and CpyIs gamma, CPyAnd C' PyIs beta, C' Py⊥PPy。
Based on the principle of pinhole imaging and the theory of similar triangles, there are
Figure BDA0002701262800000041
β＝α-γ， (8)
Figure BDA0002701262800000042
Figure BDA0002701262800000043
Figure BDA0002701262800000044
Figure BDA0002701262800000045
Figure BDA0002701262800000046
f is the focal length, | C' C | is equal to the height H of the optical center of the cameraC≡tz+tzc. Simultaneous equations (7) - (13) are solved to obtain the horizontal distance D from the optical center C to the control point as | C' P |;
step 3.2: calculating the coordinate of the optical center C according to the horizontal distance D from the optical center C to the control point | C' P |; take two control points P1(X1，Y1，Z1)，P2(X2，Y2，Z2) The equation is shown
Figure BDA0002701262800000047
Figure BDA0002701262800000048
Z＝HC.
Simplifying the equation to obtain
Figure BDA0002701262800000049
Figure BDA00027012628000000410
Z＝HC， (16)
Wherein (X, Y) is the horizontal coordinate of the optical center in the world coordinate system W-XYZ, Di(i is 1,2) is the horizontal distance from the optical center to the control point, equations (14) - (16) are combined, and the coordinate (X) of the optical center C in the world coordinate system W-XYZ is solvedC0，YC0，ZC0)。
Step 3.3: solving the yaw angle theta of the moving target and eliminating a false root of an optical center coordinate solution;
two sets of optical center coordinates (X) are respectively combinedC01，YC01，ZC01)，(XC02，YC02，ZC02) Substitution equation (17)
Figure BDA0002701262800000051
A yaw angle theta can be calculated for each 2D-3D point pair. When the optical center coordinate value is a true root, the difference distance between the two yaw angles is smaller than that when the optical center coordinate value is a false root, and therefore the false root can be eliminated. Solving the yaw angle theta by 2 point pairs when the final estimation result of the yaw angle theta takes the optical center coordinate value as the true rootiAverage value of (i ═ 1,2), i.e., θ ═ θ1+θ2)/2。
Step 3.4: solving world coordinates (t) of moving objectsx，ty)
The coordinates of the moving object in the world coordinate system can be calculated according to equation (2)
Figure BDA0002701262800000052
Wherein
Figure BDA0002701262800000053
Is the coordinate of the optical center in the world coordinate system,
Figure BDA0002701262800000054
is a rotation matrix between the coordinate system of the moving object and the coordinate system of the world,
Figure BDA0002701262800000055
the coordinates of the optical center of the camera in the coordinate system of the moving object.
Further, when the number of the 2D-3D point pairs is greater than 2, the method for solving the pose of the moving object is as follows:
step 3.1': eliminating outliers by using RANSAC algorithm to obtain an interior point set
In RANSAC algorithm, 2 groups of point pairs are randomly sampled and (t) is calculatedx，tyTheta), synthesizing each solution into a corresponding collinear equation through formula (4), calculating a pixel reprojection error, and determining that (t) can be effectively solved through the magnitude relation between the reprojection error and a given threshold valuex，tyAnd theta) and iterating for multiple times to obtain a maximum set of interior points.
Step 3.2': traversing the inner point set to obtain any 2 pairs of point pairs to combine and solve the target pose;
traversing all 2-point combinations in the inner point set, and respectively solving the target pose parameter (t)xi,tyi,θi) Sn, sn being the total number of combinations. When the logarithm of the concentration points of the inner points is m,
Figure BDA0002701262800000056
Figure BDA0002701262800000057
the number of combinations of 2 objects is arbitrarily selected from the m objects.
Step 3.3': taking the mean value as a pose estimation value
Taking the average value of the target pose parameters obtained in the step 3.2 as a final estimation result, namely
Figure BDA0002701262800000061
Further, when the logarithm of the 2D-3D points is 1, the method for solving the pose of the moving object is as follows: if the yaw angle theta at the previous moment is taken as the yaw angle at the current moment, the unknown parameters of the equation of the collinearity equation (4) are only (t)x，ty) And the times are one, and the unknown parameter (t) is directly solved linearlyx，ty) Outputting the pose parameter (t) of the moving objectx，ty，θ)。
When no effective control point exists in the field of view of the moving target in a short time, the pose parameter of the moving target is not updated;
when the duration that the monocular camera cannot see the control point exceeds a threshold, an alarm message is issued.
The invention also provides a monocular camera-based visual navigation system for the plane moving target, which comprises
An input module: acquiring a front image of the moving target in real time through a monocular camera and transmitting the front image into a pose calculation module;
a pose calculation module: the method comprises the steps of processing a moving object front image collected by an input module in real time, extracting image points corresponding to control points in a photo of the front image to obtain 2D-3D point pairs, solving the position and pose to obtain position and pose information of a moving object and outputting the position and pose information to the control module;
an input module: acquiring a front image of the moving target in real time through a monocular camera and transmitting the front image into a pose calculation module;
a pose calculation module: the method comprises the steps that an image in front of a moving target collected by an input module is processed in real time, image points corresponding to control points in a photo of the image in front are extracted to obtain 2D-3D point pairs, and pose information of the moving target is obtained through pose solving;
an output and alarm module: the system is used for outputting or giving an alarm to the pose information obtained by the pose calculation module and transmitting the pose information to the control module;
and the control module is used for assisting the moving target to move forwards according to the pose information of the moving target output by the output and alarm module.
Further, the pose calculation module includes
The control point extraction unit is used for extracting control point information in the image in front of the moving target, which is acquired by the input module;
the control point-image point matching unit is used for matching control point information, namely image points, in the image in front of the moving target, acquired by the input module with the control points to obtain 2D-3D point pairs;
the 2D-3D point pair storage unit is used for storing the 2D-3D point pair obtained by the control point-image point matching unit;
and the pose solving unit is used for solving the pose of the 2D-3D point pairs in the storage unit to obtain the pose information of the moving target.
The present invention also provides a computer readable medium storing a computer program of a monocular camera based visual navigation method for a planar moving object, the computer program being executed by a processor to implement the aforementioned steps of the monocular camera based visual navigation method for a planar moving object.
The invention also provides computer equipment which comprises a memory and a processor, wherein the memory stores a computer program of the monocular camera based plane moving object visual navigation method, and the processor realizes the steps of the monocular camera based plane moving object visual navigation method when executing the computer program.
Compared with the prior art, the invention has the following beneficial effects:
the invention relates to a monocular camera-based visual navigation method and a monocular camera-based visual navigation system for a plane moving target, which comprises the steps that 1) a thought method of space geometric modeling is adopted, a set of all possible positions of an optical center of a camera is modeled into a circle which takes a control point as a circle center and the distance from the control point to the optical center as a radius, unknown position parameters and attitude parameters in a collinear equation are decoupled and then are respectively solved, and the monocular camera-based visual navigation system has better robustness; 2) from the geometric perspective, the accurate pose result can be obtained by solving without iteration, the operation speed is high, the pose information precision is high, and the real-time navigation requirement is met. Experimental simulation data show that under the ideal condition of no error, the calculation result of the method is completely correct, and when Gaussian noise of a certain order of magnitude exists, the method still has higher robustness and precision.
Drawings
FIG. 1 is a monocular visual navigation geometry model of a planar moving object;
FIG. 2 is a flow chart of a monocular visual navigation method for a planar moving object;
FIG. 3 is a schematic diagram of monocular vision solving for the horizontal distance of the optical center to the target;
FIG. 4 is a block diagram of a monocular visual navigation system for a planar moving object.
Detailed Description
Fig. 1 to 4 show a specific embodiment of a monocular camera-based visual navigation method for a plane moving object according to the present invention, which takes a dock truck automatic dispatching scene as an example, and specifically includes the following steps:
step 1: constructing a world coordinate system W-XYZ and a moving object coordinate system B-XBYBZBCamera coordinate system C-XCYCZCImage plane physical coordinate system
Figure BDA0002701262800000071
The monocular camera is arranged on a moving target, the moving plane where the moving target is located is an X-Y plane, an origin B of the moving target coordinate system is the center of the moving target, and the coordinate of the origin B in a world coordinate system is tb＝[tx，ty，tz]The origin O of the physical coordinate system of the image plane is the optical axis CZ of the monocular cameraCThe point of intersection with the image plane is,
Figure BDA0002701262800000081
shaft and
Figure BDA0002701262800000082
the axial direction is consistent with the coordinate system of the camera, the coordinate system of the image pixel is I-xy, and the visual angle direction of the image shot by the monocular camera is taken as
Figure BDA0002701262800000083
The upper left corner I of the image plane is an original point, and the directions of the x axis and the y axis and the image plane are in a physical coordinate system
Figure BDA0002701262800000084
The consistency is achieved;
in this embodiment, as shown in fig. 1, a world coordinate system is W-XYZ, and a motion plane where a moving target is located is an X-Y plane; coordinate system of moving object is B-XBYBZBWherein the origin B isCenter of moving object, i.e. truck, its coordinate t in world coordinate systemb＝[tx，ty，tz]So that it is a rotation matrix of the moving object coordinate system relative to the world coordinate system
Figure BDA0002701262800000085
Where θ is the yaw angle of the moving object.
With the camera optical axis CZCThe intersection O with the photo plane is used as the origin, a physical coordinate system of the photo plane is established,
Figure BDA0002701262800000086
shaft and
Figure BDA0002701262800000087
the axial direction is consistent with the camera coordinate system, and the distance from the origin O of the image plane physical coordinate system to the origin C of the camera coordinate system is the focal length f of the monocular camera; the image pixel coordinate system I-xy is the upper left corner of the image (the visual angle direction is
Figure BDA0002701262800000088
Upper left corner) I as origin, x-axis and y-axis directions and image plane physical coordinate system
Figure BDA0002701262800000089
And (5) the consistency is achieved.
The moving object moves on the plane X-Y, so that the Z coordinate of the moving object in the world coordinate system W-XYZ is a fixed value, namely Z ≡ tzHorizontal coordinate (t)x，ty) And the yaw angle theta changes along with the movement of the target, and is a parameter to be solved for visual navigation. The installation relation between the camera and the moving object is known, and the optical center C of the camera is positioned in a moving object coordinate system B-XBYBZBCoordinate of (3) is tc＝(txc，tyc，tzc) Camera coordinate system C-XCYCZCRelative to a moving object coordinate system B-XBYBZBIs Rc，
Figure BDA00027012628000000810
The angle alpha is a pitch angle of the camera coordinate system relative to the moving object coordinate system.
Then there is
Figure BDA00027012628000000811
Wherein
Figure BDA0002701262800000091
To control the coordinates of the point in the camera coordinate system,
Figure BDA0002701262800000092
for the coordinates of the control point in the coordinate system of the moving object,
the control points are in a world coordinate system W-XYZ and a moving target coordinate system B-XBYBZBThe middle coordinate transformation relation is
Figure BDA0002701262800000093
Wherein
Figure BDA0002701262800000094
Respectively the coordinates of the control point in the world coordinate system W-XYZ, tb＝[tx，ty，tz]For the coordinates of the moving object in the world coordinate system,
in the embodiment, the camera continues to use the habit in photogrammetry, focuses on dynamic and real-time image acquisition and measurement, and the specific type and model can be selected according to actual needs and sampling frequency. The intrinsic parameters of the camera are known, the focal length of the camera is f, and the pixel size is expressed as (d)x，dy) Intrinsic parameter matrix of camera
Figure BDA0002701262800000095
Wherein
Figure BDA0002701262800000096
Is an equivalent focal length, (C)x，Cy) As principal point-like coordinates. The monocular camera is arranged on the moving target truck, the installation relation of the monocular camera and the moving target truck is known, and a camera coordinate system C-X is established by taking the camera optical center C as an originCYCZCThe camera coordinate system has a roll angle of 0, a yaw angle of 0 and a pitch angle of a downward viewing angle α with respect to the moving object coordinate system, as shown in fig. 3, and this is arranged to facilitate the solution of the horizontal distance from the optical center of the camera to the control point.
Establishing collinearity equations
Figure BDA0002701262800000097
Wherein the lambda is a proportionality coefficient,
Figure BDA0002701262800000098
the coordinates of the image points are in homogeneous order,
Figure BDA0002701262800000099
is the world coordinate of the corresponding point.
Step 2: arranging a plurality of control points P in the moving scene of the moving target, acquiring images in real time by the monocular camera in the moving process of the moving target, analyzing the images, and extracting image points P of the control points P on an image plane physical coordinate system to form 2D-3D point pairs;
in this embodiment, a plurality of control points P are arranged in the moving object activity sceneiI 1,2 … m, coordinate P of control pointi＝(Xi，Yi，Zi) I is 1,2 … m. The monocular camera collects images in real time in the moving target advancing process and analyzes the physical coordinate system of the photo extraction and control point falling on the image planeForming a 2D-3D point pair by the image points;
in this embodiment, the method for extracting the 2D-3D point pair from the image collected by the monocular camera includes:
(1) coding cooperation marks are distributed on control points in the moving target activity scene in advance;
(2) and extracting image points corresponding to the coding cooperation marks on the control points by using a template matching method to obtain 2D-3D point pairs. Through template matching, the image point formed by which control point falls in the acquired image can be found from the image acquired by the monocular camera, so that a 2D-3D point pair formed by the image point and the control point is obtained.
In this embodiment, the monocular camera collects images in real time, and extracts the corresponding image point p of the control point through template matchingjJ is 1,2, …, n, resulting in a 2D-3D point pair (p)j，Pj)，j＝1，2，…，n。
And step 3: carrying out coordinate conversion on the 2D-3D point pair according to the coordinate system in the step 1, and solving the pose of the moving target;
in this embodiment, the method for solving the pose of the moving object is shown in fig. 2,
according to the difference of the number of the 2D-3D point pairs extracted by the monocular camera, the method for solving the pose of the moving object respectively comprises the following steps:
1) when the 2D-3D point pairs extracted by the monocular camera are 2 groups, the method for solving the pose of the moving object is as follows:
step 3.1: solving the horizontal distance D from the optical center C of the camera to the control point;
as shown in FIG. 3, C is the optical center, C' is the projection of the optical center C on the X-Y plane of the world coordinate system, the image point P and the control point P are a set of 2D-3D point pairs, the included angle between the optical axis and the horizontal plane is alpha, and the image plane physical coordinate system of the image point P (X, Y)
Figure BDA0002701262800000101
Coordinates of (2)
Figure BDA0002701262800000102
Satisfy the requirement of
Figure BDA0002701262800000103
Figure BDA0002701262800000104
Wherein (d)x，dy) Is the pixel size (C)x，Cy) As principal point-like coordinates, pyIs like a point at
Figure BDA0002701262800000105
Projected point on axis, PyIs a reaction of with pyCorresponding object point, optical axis and CpyIs gamma, CPyAnd C' PyIs beta, C' Py⊥PPyThe image point refers to a corresponding point of an image formed by an actual point (i.e., an object point, such as a control point) in the world coordinate system.
Based on the principle of pinhole imaging and the theory of similar triangles, there are
Figure BDA0002701262800000106
β＝α-γ， (8)
Figure BDA0002701262800000111
Figure BDA0002701262800000112
Figure BDA0002701262800000113
Figure BDA0002701262800000114
Figure BDA0002701262800000115
f is the focal length, | C' C | is equal to the height H of the optical center of the cameraC≡tz+tzc. And (4) solving simultaneous equations (7) - (13) to obtain the horizontal distance D between the optical center C and the control point P as | C' P |.
Step 3.2: solving the coordinate of the optical center C according to the horizontal distance from the optical center C to the control point P;
in this embodiment, no constraint condition is added, and the position range where the optical center of the camera of any group of 2D-3D point pairs is located is the upper hemispherical surface on an X-Y plane, the spherical center is a control point, and the radius is the distance from the optical center to the control point. But since the camera height is a fixed value HC≡tz+tzcThe position range is changed into the upper hemisphere and Z ═ HCThe circumference where the planes meet.
Take two control points P1(X1，Y1，Z1)，P2(X2，Y2，Z2) The equation is shown
Figure BDA0002701262800000116
Figure BDA0002701262800000117
Z＝HC.
Simplifying the equation to obtain
Figure BDA0002701262800000118
Figure BDA0002701262800000119
Z＝HC， (16)
Wherein (X, Y) is horizontal coordinate of optical center C in world coordinate system W-XYZ, Z is height value of optical center C in world coordinate system from horizontal plane X-Y, and D is height value of optical center C in world coordinate system from horizontal plane X-Yi(i is 1,2) is the horizontal distance from the optical center to the control point, equations (14) - (16) are combined, and the coordinate (X) of the optical center C in the world coordinate system W-XYZ is solvedC0，YC0，ZC0)。
By adopting a spatial geometric modeling idea method, the method models a set of all possible positions of the optical center of the camera into a circle which takes a control point as a circle center and takes the distance from the control point to the optical center as a radius, decouples unknown position parameters and attitude parameters in a collinear equation and then respectively solves the unknown position parameters and the attitude parameters, and has better robustness; from the geometric perspective, the accurate pose result can be obtained by solving without iteration, the operation speed is high, the pose information precision is high, and the real-time navigation requirement is met.
Step 3.3: solving the yaw angle theta of the moving target and eliminating a false root of an optical center coordinate solution;
two sets of optical center coordinates (X) are respectively combinedC01，YC01，ZC01)，(XC02，YC02，ZC02) Substitution equation (17)
Figure BDA0002701262800000121
Each 2D-3D point pair can respectively calculate a yaw angle theta, when the optical center coordinate value is a true root, the difference distance between the two yaw angles is smaller than that when the optical center coordinate value is a false root, the false root can be eliminated, and when the optical center coordinate value is a true root, the final estimation result of the yaw angle theta is obtained, 2 point pairs are used for solving the yaw angle thetaiAverage value of (i ═ 1,2), i.e., θ ═ θ1+θ2)/2；
Step 3.4: solving world coordinates (t) of moving objectsx，ty)
The coordinates of the moving object in the world coordinate system can be calculated according to equation (2)
Figure BDA0002701262800000122
Wherein
Figure BDA0002701262800000123
Is the coordinates of the optical center C in the world coordinate system,
Figure BDA0002701262800000124
is a rotation matrix between the coordinate system of the moving object and the coordinate system of the world,
Figure BDA0002701262800000125
coordinates of the optical center of the camera in a coordinate system of the moving target are obtained;
2) when the number of the 2D-3D point pairs extracted by the monocular camera is larger than 2, the method for solving the pose of the moving object comprises the following steps:
step 3.1': eliminating outliers in all the extracted 2D-3D point pairs to obtain an inner point set; the inner point set is the set of 2D-3D point pairs left after the outliers of the interference are removed.
The method for eliminating outliers comprises the following steps:
randomly sampling 2 groups of point pairs by using RANSAC algorithm to calculate the pose (t) of the moving target in a world coordinate systemx，tyTheta), synthesizing each solution into a corresponding collinear equation through formula (4), calculating a pixel reprojection error, and determining that (t) can be effectively solved through the magnitude relation between the reprojection error and a given threshold valuex，tyTheta), repeating the step until all the point pairs are combined pairwise to solve the image point reprojection error to obtain a maximum interior point set;
step 3.2': traversing any two 2D-3D point pair combinations in the inner point set and solving the target pose of each two point pair combinations;
traversing all 2-point combinations in the inner point set, and respectively solving the target pose parameter (t)xi,tyi,θi) Sn, sn is the total number of combinations, when the logarithm of the concentration points of the interior points is m,
Figure BDA0002701262800000131
Figure BDA0002701262800000132
the number of combinations of 2 objects is arbitrarily selected from the m objects.
Step 3.3': taking the average value of the target pose parameters obtained in the step 3.2 as a pose estimation value, namely
Figure BDA0002701262800000133
3) When the logarithm of 2D-3D points extracted by the monocular camera is 1, the method for solving the pose of the moving object comprises the following steps:
taking the yaw angle theta of the moving target at the previous moment as the yaw angle of the current moment, and linearly solving the unknown parameter (t) according to the collinear equation (4)x，ty) Outputting the pose parameter (t) of the moving objectx，ty，θ)；
4) When no effective control point is extracted from the visual field of the moving target in a short time by the monocular camera, the pose parameter of the moving target is not updated;
5) and when the monocular camera does not extract the effective control point within the time that the duration time exceeds a threshold value, sending out alarm information.
The invention simultaneously considers a plurality of conditions such as a group of point pairs, two groups of point pairs, a plurality of groups of point pairs, extreme conditions and the like, considers the installation relation of the camera and the moving target, and has certain expandability and universality.
And 4, step 4: and assisting the moving target to move forwards according to the pose of the moving target.
According to the position of the truck on the scene plane and the yaw angle (t) of the truck calculated in the step 3x，tyAnd theta) is the position and posture information of the truck, the position and posture information of the truck is sent to a control system of the truck, and the control system determines how the truck can move forwards according to the position and posture information of the current truck. And the truck is assisted to finish normal work. When an extreme condition is met, namely the condition that no control point exists in the visual field of the monocular camera when the truck is in the working state, the duration of the condition exceeds a threshold valueAnd sending an alarm signal to control the truck to stop working and trigger the alarm module to give an alarm.
The invention also provides a monocular camera-based visual navigation system for a plane moving target, which is shown in figure 4 and comprises
An input module: acquiring a front image of the moving target in real time through a monocular camera and transmitting the front image into a calculation and storage module;
a pose calculation module: the method comprises the steps of processing a moving object front image collected by an input module in real time, extracting image points corresponding to control points in a photo of the front image to obtain 2D-3D point pairs, solving the position and pose to obtain position and pose information of a moving object and outputting the position and pose information to the control module;
an input module: acquiring a front image of the moving target in real time through a monocular camera and transmitting the front image into a pose calculation module;
a pose calculation module: the method comprises the steps that an image in front of a moving target collected by an input module is processed in real time, image points corresponding to control points in a photo of the image in front are extracted to obtain 2D-3D point pairs, and pose information of the moving target is obtained through pose solving;
an output and alarm module: the system is used for outputting or giving an alarm to the pose information obtained by the pose calculation module and transmitting the pose information to the control module;
and the control module is used for assisting the moving target to move forwards according to the pose information of the moving target output by the output and alarm module.
In this embodiment, the pose calculation module includes
The control point extraction unit is used for extracting control point information in the image in front of the moving target, which is acquired by the input module;
the control point-image point matching unit is used for matching control point information, namely image points, in the image in front of the moving target, acquired by the input module with the control points to obtain 2D-3D point pairs;
the 2D-3D point pair storage unit is used for storing the 2D-3D point pair obtained by the control point-image point matching unit;
and the pose solving unit is used for solving the pose of the 2D-3D point pairs in the storage unit to obtain the pose information of the moving target. In this embodiment, the method for solving the pose information in the pose solving unit uses the above-described method for solving the pose of the moving object, and different solving methods are used according to the number of the point pairs in the 2D-3D point pair storage unit.
The present invention also provides a computer readable medium storing a computer program of a monocular camera based visual navigation method for a planar moving object, the computer program being executed by a processor to implement the aforementioned steps of the monocular camera based visual navigation method for a planar moving object.
The invention also provides computer equipment which comprises a memory and a processor, wherein the memory stores a computer program of the monocular camera based plane moving object visual navigation method, and the processor realizes the steps of the monocular camera based plane moving object visual navigation method when executing the computer program.
The above is only a preferred embodiment of the present invention, and the protection scope of the present invention is not limited to the above-mentioned embodiments, and all technical solutions belonging to the idea of the present invention belong to the protection scope of the present invention. It should be noted that modifications and embellishments within the scope of the invention may be made by those skilled in the art without departing from the principle of the invention.

Claims (6)

1.一种基于单目相机的平面运动目标视觉导航方法，其特征在于，包括以下步骤：1. a plane moving target visual navigation method based on monocular camera, is characterized in that, comprises the following steps: 步骤1：构建世界坐标系W-XYZ、运动目标坐标系为B-XBYBZB、相机坐标系C-XCYCZC、像平面物理坐标系
Figure FDA0003440336960000011
图像像素坐标系I-xy，所述单目相机安装在运动目标上，运动目标所在的运动平面为X-Y平面，运动目标坐标系的原点B为运动目标的中心，原点B在世界坐标系中的坐标为tb＝[tx,ty,tz]，所述像平面物理坐标系的原点O为单目相机光轴CZC与像平面的交点，
Figure FDA0003440336960000012
轴和
Figure FDA0003440336960000013
轴方向与相机坐标系一致，图像像素坐标系I-xy，以单目相机所拍摄像片视角方向为
Figure FDA0003440336960000014
的左上角I为原点，x轴和y轴方向与像平面物理坐标系
Figure FDA0003440336960000015
一致；Step 1: Build the world coordinate system W-XYZ, the moving target coordinate system as BX B Y B Z B , the camera coordinate system CX C Y C Z C , and the image plane physical coordinate system
Figure FDA0003440336960000011
The image pixel coordinate system I-xy, the monocular camera is installed on the moving target, the moving plane where the moving target is located is the XY plane, the origin B of the moving target coordinate system is the center of the moving target, and the origin B is in the world coordinate system. The coordinates are t b =[t x , ty , t z ], the origin O of the physical coordinate system of the image plane is the intersection of the optical axis CZ C of the monocular camera and the image plane,
Figure FDA0003440336960000012
axis and
Figure FDA0003440336960000013
The axis direction is consistent with the camera coordinate system, the image pixel coordinate system is I-xy, and the viewing angle direction of the photo taken by the monocular camera is
Figure FDA0003440336960000014
The upper left corner I is the origin, and the x-axis and y-axis directions are related to the physical coordinate system of the image plane
Figure FDA0003440336960000015
consistent; 步骤2：所述单目相机在运动目标前行过程中通过实时获取控制点以及控制点落在像平面物理坐标系上的像点对形成2D-3D点对；Step 2: The monocular camera forms a 2D-3D point pair by acquiring the control point in real time and the image point pair where the control point falls on the physical coordinate system of the image plane during the moving target forward process; 步骤3：将所述2D-3D点对根据步骤1中的坐标系进行坐标转换,求解运动目标位姿；Step 3: perform coordinate transformation on the 2D-3D point pair according to the coordinate system in step 1, and solve the pose of the moving target; 步骤4：根据所述运动目标位姿辅助运动目标前行；Step 4: assist the moving target to move forward according to the moving target pose; 步骤1中各坐标系之间的关系为：The relationship between the coordinate systems in step 1 is: 1)控制点在相机坐标系C-XCYCZC和运动目标坐标系B-XBYBZB中的坐标上的转换关系为：1) The transformation relationship of the control point on the coordinates of the camera coordinate system CX C Y C Z C and the moving target coordinate system BX B Y B Z B is:
Figure FDA0003440336960000016
Figure FDA0003440336960000016
 其中
Figure FDA0003440336960000017
为控制点在相机坐标系中的坐标，
Figure FDA0003440336960000018
为控制点在运动目标坐标系中的坐标，in
Figure FDA0003440336960000017
is the coordinate of the control point in the camera coordinate system,
Figure FDA0003440336960000018
is the coordinate of the control point in the moving target coordinate system, tc＝(txc,tyc,tzc)为相机光心C在运动目标坐标系下的坐标；RC为相机坐标系C-XCYCZC相对于运动目标坐标系B-XBYBZB的旋转矩阵t c =(t xc , t yc , t zc ) is the coordinate of the camera optical center C in the moving target coordinate system; RC is the camera coordinate system CX C Y C Z C relative to the moving target coordinate system BX B Y B Z Rotation matrix of B
Figure FDA0003440336960000019
Figure FDA0003440336960000019
 角度α为相机坐标系相对于运动目标坐标系的俯仰角；The angle α is the pitch angle of the camera coordinate system relative to the moving target coordinate system; 2)控制点在世界坐标系W-XYZ和运动目标坐标系B-XBYBZB中坐标变换关系为2) The coordinate transformation relationship of the control point in the world coordinate system W-XYZ and the moving target coordinate system BX B Y B Z B is:
Figure FDA0003440336960000021
Figure FDA0003440336960000021
 其中
Figure FDA0003440336960000022
分别为控制点在世界坐标系W-XYZ中的坐标，tb＝[tx,ty,tz]为运动目标在世界坐标系中的坐标，in
Figure FDA0003440336960000022
are the coordinates of the control point in the world coordinate system W-XYZ, respectively, t b = [t x , t y , t z ] are the coordinates of the moving target in the world coordinate system,
Figure FDA0003440336960000023
为运动目标坐标系相对于世界坐标系的旋转矩阵，θ为运动目标的偏航角；
Figure FDA0003440336960000023
is the rotation matrix of the moving target coordinate system relative to the world coordinate system, and θ is the yaw angle of the moving target; 3)设相机的焦距表示为f，像元尺寸表示为(dx,dy)，相机内参数矩阵3) Let the focal length of the camera be expressed as f, the pixel size as (d x , d y ), and the camera internal parameter matrix
Figure FDA0003440336960000024
Figure FDA0003440336960000024
 其中
Figure FDA0003440336960000025
为等效焦距，(Cx,Cy)为像主点坐标；in
Figure FDA0003440336960000025
is the equivalent focal length, (C x ,C y ) is the image principal point coordinate; 4)建立共线方程4) Establish a collinear equation
Figure FDA0003440336960000026
Figure FDA0003440336960000026
 其中λ为比例系数，
Figure FDA0003440336960000027
为像点齐次坐标，
Figure FDA0003440336960000028
为对应点的世界坐标，u、v、w为中间变量；where λ is the scale factor,
Figure FDA0003440336960000027
is the homogeneous coordinate of the image point,
Figure FDA0003440336960000028
is the world coordinate of the corresponding point, u, v, w are intermediate variables; 步骤3中求解运动目标位姿的方法是：The method for solving the pose of the moving target in step 3 is: 根据单目相机所提取的2D-3D点对数不同，运动目标位姿的求解方法分别为：According to the difference in the number of 2D-3D point pairs extracted by the monocular camera, the methods for solving the pose of the moving target are as follows: 1)当单目相机所提取的2D-3D点对为2组时，则运动目标位姿的求解方法是：1) When the 2D-3D point pairs extracted by the monocular camera are 2 groups, the solution method of the moving target pose is: 步骤3.1：求相机光心C到控制点水平距离D；Step 3.1: Find the horizontal distance D from the camera optical center C to the control point; C为光心，C′为光心C在世界坐标系X-Y平面的投影，像点p和控制点P是一组2D-3D点对，光轴与水平面的夹角为α，像点p(x,y)的像平面物理坐标系
Figure FDA0003440336960000029
上的坐标
Figure FDA0003440336960000031
满足C is the optical center, C' is the projection of the optical center C on the XY plane of the world coordinate system, the image point p and the control point P are a set of 2D-3D point pairs, the angle between the optical axis and the horizontal plane is α, and the image point p ( x,y) image plane physical coordinate system
Figure FDA0003440336960000029
coordinates on
Figure FDA0003440336960000031
Satisfy
Figure FDA0003440336960000032
Figure FDA0003440336960000032
Figure FDA0003440336960000033
Figure FDA0003440336960000033
 其中(dx,dy)为像元尺寸，(Cx,Cy)为像主点坐标，py为像点在
Figure FDA0003440336960000034
轴上的投影点，Py为与py对应的物方点，光轴与Cpy的夹角为γ，CPy与C′Py的夹角为β，C′Py⊥PPy；Where (d x , dy ) is the pixel size, (C x , Cy ) is the image principal point coordinate, and p y is the image point at
Figure FDA0003440336960000034
The projection point on the axis, P y is the object point corresponding to py, the angle between the optical axis and Cpy is γ, the angle between CP y and C'P y is β , C'P y ⊥ PP y ; 根据小孔成像原理和相似三角形理论，有According to the principle of pinhole imaging and the similar triangle theory, we have
Figure FDA0003440336960000035
Figure FDA0003440336960000035
 β＝α+γ, (8)β=α+γ, (8)
Figure FDA0003440336960000036
Figure FDA0003440336960000036
Figure FDA0003440336960000037
Figure FDA0003440336960000037
Figure FDA0003440336960000038
Figure FDA0003440336960000038
Figure FDA0003440336960000039
Figure FDA0003440336960000039
Figure FDA00034403369600000310
Figure FDA00034403369600000310
 f为焦距，|C′C|等于相机光心高度HC≡tz+tzc，联立方程(7)-(13)，解得光心C到控制点P的水平距离D＝|C′P|f is the focal length, |C′C| is equal to the height of the optical center of the camera H C ≡t z +t zc , equations (7)-(13) are simultaneously solved, and the horizontal distance D=|C′P from the optical center C to the control point P is obtained | 步骤3.2：根据所述光心C到控制点P的水平距离D＝|C′P|求光心C的坐标；取两个控制点P1(X1,Y1,Z1),P2(X2,Y2,Z2)，列出方程Step 3.2: Find the coordinates of the optical center C according to the horizontal distance D=|C′P| from the optical center C to the control point P; take two control points P 1 (X 1 , Y 1 , Z 1 ), P 2 (X 2 , Y 2 , Z 2 ), list the equations
Figure FDA00034403369600000311
Figure FDA00034403369600000311
Figure FDA00034403369600000312
Figure FDA00034403369600000312
 Z＝HC.Z = H C . 将方程化简，得Simplify the equation to get
Figure FDA00034403369600000313
Figure FDA00034403369600000313
Figure FDA00034403369600000314
Figure FDA00034403369600000314
 Z＝HC, (16)Z=H C , (16) 其中(X,Y)为光心C在世界坐标系W-XYZ中的水平坐标，Z为光心C在世界坐标系中距离水平面X-Y的高度值，Di(i＝1,2)为光心到控制点的水平距离，联立方程(14)-(16)，求解出光心C在世界坐标系W-XYZ中的坐标(XC0,YC0,ZC0)；Where (X, Y) is the horizontal coordinate of the optical center C in the world coordinate system W-XYZ, Z is the height value of the optical center C from the horizontal plane XY in the world coordinate system, and Di ( i =1,2) is the light The horizontal distance from the center to the control point, equations (14)-(16) are simultaneously solved, and the coordinates (X C0 , Y C0 , Z C0 ) of the optical center C in the world coordinate system W-XYZ are solved; 步骤3.3：求解运动目标的偏航角θ并剔除光心坐标解的假根；Step 3.3: Solve the yaw angle θ of the moving target and remove the false root of the optical center coordinate solution; 分别将两组光心坐标(XC01,YC01,ZC01),(XC02,YC02,ZC02)代入方程(17)Substitute the two sets of optical center coordinates (X C01 , Y C01 , Z C01 ), (X C02 , Y C02 , Z C02 ) into equation (17)
Figure FDA0003440336960000041
Figure FDA0003440336960000041
 每一个2D-3D点对分别可以计算出一个偏航角θ，当光心坐标取值为真根时，这两个偏航角相差距离要小于光心坐标取值为假根时，故据此剔除掉假根，偏航角θ的最终估计结果取光心坐标取值为真根时2个点对求解偏航角θi(i＝1,2)的平均值，即θ＝(θ1+θ2)/2；A yaw angle θ can be calculated for each 2D-3D point pair. When the optical center coordinate is the true root, the difference between the two yaw angles is smaller than when the optical center coordinate is the false root. This eliminates the false root, and the final estimation result of the yaw angle θ takes the average value of the 2 point pairs to solve the yaw angle θ i (i=1, 2) when the optical center coordinate is the true root, that is, θ=(θ 1 +θ 2 )/2; 步骤3.4：求解运动目标的世界坐标(tx,ty)Step 3.4: Solve the world coordinates of the moving target (t x , t y ) 根据等式(2)可以计算得到运动目标在世界坐标系中坐标According to equation (2), the coordinates of the moving target in the world coordinate system can be calculated
Figure FDA0003440336960000042
Figure FDA0003440336960000042
 其中
Figure FDA0003440336960000043
为光心C在世界坐标系中的坐标，
Figure FDA0003440336960000044
为运动目标坐标系与世界坐标系之间的旋转矩阵，
Figure FDA0003440336960000045
为相机光心在运动目标坐标系中的坐标；in
Figure FDA0003440336960000043
is the coordinate of the optical center C in the world coordinate system,
Figure FDA0003440336960000044
is the rotation matrix between the moving target coordinate system and the world coordinate system,
Figure FDA0003440336960000045
is the coordinate of the camera optical center in the moving target coordinate system; 2)当单目相机所提取的2D-3D点对数大于2时，求解运动目标位姿的方法是：2) When the number of 2D-3D point pairs extracted by the monocular camera is greater than 2, the method for solving the pose of the moving target is: 步骤3.1’：剔除所提取的所有2D-3D点对中的野值，得到内点集；Step 3.1': remove the outliers in all the extracted 2D-3D point pairs to obtain the inner point set; 步骤3.2’：遍历内点集内任意两个2D-3D点对组合并求解每两个点对组合的目标位姿；Step 3.2': Traverse any two 2D-3D point pair combinations in the inner point set and solve the target pose of each two point pair combination; 遍历内点集中的所有2点组合，分别求解目标位姿参数(txi,tyi,θi)(i＝1,2...sn)，sn为组合总数目，当内点集中点对数为m时，
Figure FDA0003440336960000046
Figure FDA0003440336960000047
为m个物体中任取2个物体的组合数；Traverse all 2-point combinations in the interior point set, and solve the target pose parameters (t xi , t yi , θ i ) (i=1, 2...sn) respectively, sn is the total number of combinations, when the interior point set points to When the number is m,
Figure FDA0003440336960000046
Figure FDA0003440336960000047
is the number of combinations of 2 objects randomly selected from m objects; 步骤3.3’：取步骤3.2中所求目标位姿参数的平均值作为位姿估计值，即Step 3.3': Take the average value of the target pose parameters obtained in step 3.2 as the pose estimation value, namely
Figure FDA0003440336960000051
Figure FDA0003440336960000051
 3)当单目相机所提取的2D-3D点对数为1时，求解运动目标位姿的方法是：3) When the logarithm of 2D-3D points extracted by the monocular camera is 1, the method for solving the pose of the moving target is: 取上一时刻运动目标的偏航角θ作为当前时刻的偏航角，则根据共线方程(4)线性求解出未知参数(tx,ty)，输出运动目标的位姿参数(tx,ty,θ)；Take the yaw angle θ of the moving target at the previous moment as the yaw angle at the current moment, then linearly solve the unknown parameters (t x , t y ) according to the collinear equation (4), and output the pose parameters (t x ) of the moving target , ty ,θ); 4)当单目相机在短时间内运动目标的视野中没有提取到有效控制点时，运动目标的位姿参数不更新；4) When the monocular camera does not extract effective control points in the field of view of the moving target in a short time, the pose parameters of the moving target are not updated; 5)当单目相机在持续时间超过一个阈值的时间内都没有提取到有效控制点时，发出警报信息。5) When the monocular camera fails to extract an effective control point for a duration exceeding a threshold, an alarm message is issued. 2.根据权利要求1所述的方法，其特征在于，获取2D-3D点对的方法是：2. method according to claim 1, is characterized in that, the method that obtains 2D-3D point pair is: (1)在运动目标活动场景中的控制点上均布设有编码合作标志；(1) Coding cooperation signs are evenly distributed on the control points in the moving target activity scene; (2)运用模板匹配的方法，提取与控制点上编码合作标志相对应的像点得到2D-3D点对；(2) Using the method of template matching, extract the image points corresponding to the coded cooperation signs on the control points to obtain 2D-3D point pairs; (3)剔除干扰点，得到2D-3D点对。(3) Eliminate interference points to obtain 2D-3D point pairs. 3.根据权利要求1所述的方法，其特征在于，步骤3.1’中野值的剔除方法是：使用RANSAC算法，随机采样2组点对，计算运动目标在世界坐标系中的位姿(tx,ty,θ)，通过公式(4)将每个解合成相应的共线方程，再计算像点重投影误差，通过重投影误差与给定阈值的大小关系来确定能够有效求解(tx,ty,θ)的内点，迭代多次求解后获得最大内点集合。3. method according to claim 1, is characterized in that, the elimination method of outlier in step 3.1' is: use RANSAC algorithm, randomly sample 2 groups of point pairs, calculate the pose (t x , t y , θ), synthesize each solution into a corresponding collinear equation by formula (4), and then calculate the reprojection error of the image point, and determine the relationship between the reprojection error and the given threshold that can effectively solve (t x , t y , θ), the largest set of interior points is obtained after iteratively solve for many times. 4.一种基于单目相机的平面运动目标视觉导航系统，其特征在于，包括4. a plane moving target vision navigation system based on monocular camera, is characterized in that, comprises 输入模块：通过单目相机实时采集运动目标前方图像并传入到位姿计算模块中；Input module: real-time capture of the front image of the moving target through the monocular camera and transfer it to the pose calculation module; 位姿计算模块：通过对输入模块所采集的运动目标前方图像进行实时处理，提取所述前方图像的相片中与控制点相对应的像点得到2D-3D点对，通过位姿求解得到运动目标的位姿信息；Pose calculation module: through real-time processing of the front image of the moving target collected by the input module, extract the image points corresponding to the control points in the photo of the front image to obtain 2D-3D point pairs, and obtain the moving target through the pose solution pose information; 输出与警报模块：用于将位姿计算模块求解得到的位姿信息输出或发出警报，同时将位姿信息传递给控制模块；Output and alarm module: used to output the pose information obtained by the pose calculation module or issue an alarm, and transmit the pose information to the control module at the same time; 控制模块，根据输出与警报模块所输出的运动目标位姿信息辅助运动目标前行；The control module assists the moving target to move forward according to the moving target pose information output by the output and alarm module; 所述位姿计算模块包括The pose calculation module includes 控制点提取单元，用于提取输入模块所采集的运动目标前方图像中的控制点信息；a control point extraction unit, used for extracting the control point information in the image in front of the moving target collected by the input module; 控制点-像点匹配单元，用于将输入模块所采集的运动目标前方图像中的控制点信息即像点与控制点进行匹配，得到2D-3D点对；The control point-image point matching unit is used to match the control point information in the image in front of the moving target collected by the input module, that is, the image point and the control point to obtain a 2D-3D point pair; 2D-3D点对存储单元，用于存储控制点-像点匹配单元得到的2D-3D点对；The 2D-3D point pair storage unit is used to store the 2D-3D point pairs obtained by the control point-image point matching unit; 位姿求解单元，用于对2D-3D点对存储单元中的2D-3D点对进行位姿求解得到运动目标的位姿信息；The pose solving unit is used to solve the pose of the 2D-3D point pair in the 2D-3D point pair storage unit to obtain the pose information of the moving target; 所述求解得到运动目标的位姿信息的方法包括：The method for obtaining the pose information of the moving target by the solution includes: 构建世界坐标系W-XYZ、运动目标坐标系为B-XBYBZB、相机坐标系C-XCYCZC、像平面物理坐标系
Figure FDA0003440336960000061
图像像素坐标系I-xy，所述单目相机安装在运动目标上，运动目标所在的运动平面为X-Y平面，运动目标坐标系的原点B为运动目标的中心，原点B在世界坐标系中的坐标为tb＝[tx,ty,tz]，所述像平面物理坐标系的原点O为单目相机光轴CZC与像平面的交点，
Figure FDA0003440336960000062
轴和
Figure FDA0003440336960000063
轴方向与相机坐标系一致，图像像素坐标系I-xy，以单目相机所拍摄像片视角方向为
Figure FDA0003440336960000064
的左上角I为原点，x轴和y轴方向与像平面物理坐标系
Figure FDA0003440336960000065
一致；Build the world coordinate system W-XYZ, the moving target coordinate system as BX B Y B Z B , the camera coordinate system CX C Y C Z C , and the image plane physical coordinate system
Figure FDA0003440336960000061
The image pixel coordinate system I-xy, the monocular camera is installed on the moving target, the moving plane where the moving target is located is the XY plane, the origin B of the moving target coordinate system is the center of the moving target, and the origin B is in the world coordinate system. The coordinates are t b =[t x , ty , t z ], the origin O of the physical coordinate system of the image plane is the intersection of the optical axis CZ C of the monocular camera and the image plane,
Figure FDA0003440336960000062
axis and
Figure FDA0003440336960000063
The axis direction is consistent with the camera coordinate system, the image pixel coordinate system is I-xy, and the viewing angle direction of the photo taken by the monocular camera is
Figure FDA0003440336960000064
The upper left corner I is the origin, and the x-axis and y-axis directions are related to the image plane physical coordinate system
Figure FDA0003440336960000065
consistent; 其中，各坐标系之间的关系为：Among them, the relationship between each coordinate system is: 1)控制点在相机坐标系C-XCYCZC和运动目标坐标系B-XBYBZB中的坐标上的转换关系为：1) The transformation relationship of the control point on the coordinates of the camera coordinate system CX C Y C Z C and the moving target coordinate system BX B Y B Z B is:
Figure FDA0003440336960000066
Figure FDA0003440336960000066
 其中
Figure FDA0003440336960000067
为控制点在相机坐标系中的坐标，
Figure FDA0003440336960000068
为控制点在运动目标坐标系中的坐标，in
Figure FDA0003440336960000067
is the coordinate of the control point in the camera coordinate system,
Figure FDA0003440336960000068
is the coordinate of the control point in the moving target coordinate system, tc＝(txc,tyc,tzc)为相机光心C在运动目标坐标系下的坐标；RC为相机坐标系C-XCYCZC相对于运动目标坐标系B-XBYBZB的旋转矩阵t c =(t xc , t yc , t zc ) is the coordinate of the camera optical center C in the moving target coordinate system; RC is the camera coordinate system CX C Y C Z C relative to the moving target coordinate system BX B Y B Z Rotation matrix of B
Figure FDA0003440336960000069
Figure FDA0003440336960000069
 角度α为相机坐标系相对于运动目标坐标系的俯仰角；The angle α is the pitch angle of the camera coordinate system relative to the moving target coordinate system; 2)控制点在世界坐标系W-XYZ和运动目标坐标系B-XBYBZB中坐标变换关系为2) The coordinate transformation relationship of the control point in the world coordinate system W-XYZ and the moving target coordinate system BX B Y B Z B is:
Figure FDA0003440336960000071
Figure FDA0003440336960000071
 其中
Figure FDA0003440336960000072
分别为控制点在世界坐标系W-XYZ中的坐标，tb＝[tx,ty,tz]为运动目标在世界坐标系中的坐标，in
Figure FDA0003440336960000072
are the coordinates of the control point in the world coordinate system W-XYZ, respectively, t b = [t x , t y , t z ] are the coordinates of the moving target in the world coordinate system,
Figure FDA0003440336960000073
为运动目标坐标系相对于世界坐标系的旋转矩阵，θ为运动目标的偏航角；
Figure FDA0003440336960000073
is the rotation matrix of the moving target coordinate system relative to the world coordinate system, and θ is the yaw angle of the moving target; 3)设相机的焦距表示为f，像元尺寸表示为(dx,dy)，相机内参数矩阵3) Let the focal length of the camera be expressed as f, the pixel size as (d x , d y ), and the camera internal parameter matrix
Figure FDA0003440336960000074
Figure FDA0003440336960000074
 其中
Figure FDA0003440336960000075
为等效焦距，(Cx,Cy)为像主点坐标；in
Figure FDA0003440336960000075
is the equivalent focal length, (C x ,C y ) is the image principal point coordinate; 4)建立共线方程4) Establish a collinear equation
Figure FDA0003440336960000076
Figure FDA0003440336960000076
 其中λ为比例系数，
Figure FDA0003440336960000077
为像点齐次坐标，
Figure FDA0003440336960000078
为对应点的世界坐标，u、v、w为中间变量；where λ is the scale factor,
Figure FDA0003440336960000077
is the homogeneous coordinate of the image point,
Figure FDA0003440336960000078
is the world coordinate of the corresponding point, u, v, w are intermediate variables; 求解运动目标位姿的方法是：The method to solve the pose of the moving target is: 根据单目相机所提取的2D-3D点对数不同，运动目标位姿的求解方法分别为：According to the difference in the number of 2D-3D point pairs extracted by the monocular camera, the methods for solving the pose of the moving target are as follows: 1)当单目相机所提取的2D-3D点对为2组时，则运动目标位姿的求解方法是：1) When the 2D-3D point pairs extracted by the monocular camera are 2 groups, the solution method of the moving target pose is: 步骤1：求相机光心C到控制点水平距离D；Step 1: Find the horizontal distance D from the camera optical center C to the control point; C为光心，C′为光心C在世界坐标系X-Y平面的投影，像点p和控制点P是一组2D-3D点对，光轴与水平面的夹角为α，像点p(x,y)的像平面物理坐标系
Figure FDA0003440336960000081
上的坐标
Figure FDA0003440336960000082
满足C is the optical center, C' is the projection of the optical center C on the XY plane of the world coordinate system, the image point p and the control point P are a set of 2D-3D point pairs, the angle between the optical axis and the horizontal plane is α, and the image point p ( x,y) image plane physical coordinate system
Figure FDA0003440336960000081
coordinates on
Figure FDA0003440336960000082
Satisfy
Figure FDA0003440336960000083
Figure FDA0003440336960000083
Figure FDA0003440336960000084
Figure FDA0003440336960000084
 其中(dx,dy)为像元尺寸，(Cx,Cy)为像主点坐标，py为像点在
Figure FDA0003440336960000085
轴上的投影点，Py为与py对应的物方点，光轴与Cpy的夹角为γ，CPy与C′Py的夹角为β，C′Py⊥PPy；Where (d x , dy ) is the pixel size, (C x , Cy ) is the image principal point coordinate, and p y is the image point at
Figure FDA0003440336960000085
The projection point on the axis, P y is the object point corresponding to py, the angle between the optical axis and Cpy is γ, the angle between CP y and C'P y is β , C'P y ⊥ PP y ; 根据小孔成像原理和相似三角形理论，有According to the principle of pinhole imaging and the similar triangle theory, we have
Figure FDA0003440336960000086
Figure FDA0003440336960000086
 β＝α+γ, (8)β=α+γ, (8)
Figure FDA0003440336960000087
Figure FDA0003440336960000087
Figure FDA0003440336960000088
Figure FDA0003440336960000088
Figure FDA0003440336960000089
Figure FDA0003440336960000089
Figure FDA00034403369600000810
Figure FDA00034403369600000810
Figure FDA00034403369600000811
Figure FDA00034403369600000811
 f为焦距，|C′C|等于相机光心高度HC≡tz+tzc，联立方程(7)-(13)，解得光心C到控制点P的水平距离D＝|C′P|f is the focal length, |C′C| is equal to the height of the optical center of the camera H C ≡t z +t zc , equations (7)-(13) are simultaneously solved, and the horizontal distance D=|C′P from the optical center C to the control point P is obtained | 步骤2：根据所述光心C到控制点P的水平距离D＝|C′P|求光心C的坐标；取两个控制点P1(X1,Y1,Z1),P2(X2,Y2,Z2)，列出方程Step 2: Find the coordinates of the optical center C according to the horizontal distance D=|C′P| from the optical center C to the control point P; take two control points P 1 (X 1 , Y 1 , Z 1 ), P 2 (X 2 , Y 2 , Z 2 ), list the equations
Figure FDA00034403369600000812
Figure FDA00034403369600000812
Figure FDA00034403369600000813
Figure FDA00034403369600000813
 Z＝HC.Z = H C . 将方程化简，得Simplify the equation to get
Figure FDA00034403369600000814
Figure FDA00034403369600000814
Figure FDA00034403369600000815
Figure FDA00034403369600000815
 Z＝HC, (16)Z=H C , (16) 其中(X,Y)为光心C在世界坐标系W-XYZ中的水平坐标，Z为光心C在世界坐标系中距离水平面X-Y的高度值，Di(i＝1,2)为光心到控制点的水平距离，联立方程(14)-(16)，求解出光心C在世界坐标系W-XYZ中的坐标(XC0,YC0,ZC0)；Where (X, Y) is the horizontal coordinate of the optical center C in the world coordinate system W-XYZ, Z is the height value of the optical center C from the horizontal plane XY in the world coordinate system, and Di ( i =1,2) is the light The horizontal distance from the center to the control point, equations (14)-(16) are simultaneously solved, and the coordinates (X C0 , Y C0 , Z C0 ) of the optical center C in the world coordinate system W-XYZ are solved; 步骤3：求解运动目标的偏航角θ并剔除光心坐标解的假根；Step 3: Solve the yaw angle θ of the moving target and remove the false root of the optical center coordinate solution; 分别将两组光心坐标(XC01,YC01,ZC01),(XC02,YC02,ZC02)代入方程(17)Substitute the two sets of optical center coordinates (X C01 , Y C01 , Z C01 ), (X C02 , Y C02 , Z C02 ) into equation (17)
Figure FDA0003440336960000091
Figure FDA0003440336960000091
 每一个2D-3D点对分别可以计算出一个偏航角θ，当光心坐标取值为真根时，这两个偏航角相差距离要小于光心坐标取值为假根时，故据此剔除掉假根，偏航角θ的最终估计结果取光心坐标取值为真根时2个点对求解偏航角θi(i＝1,2)的平均值，即θ＝(θ1+θ2)/2；A yaw angle θ can be calculated for each 2D-3D point pair. When the optical center coordinate is the true root, the difference between the two yaw angles is smaller than when the optical center coordinate is the false root. This eliminates the false root, and the final estimation result of the yaw angle θ takes the average value of the 2 point pairs to solve the yaw angle θ i (i=1, 2) when the optical center coordinate is the true root, that is, θ=(θ 1 +θ 2 )/2; 步骤4：求解运动目标的世界坐标(tx,ty)Step 4: Solve the world coordinates of the moving target (t x , t y ) 根据等式(2)可以计算得到运动目标在世界坐标系中坐标According to equation (2), the coordinates of the moving target in the world coordinate system can be calculated
Figure FDA0003440336960000092
Figure FDA0003440336960000092
 其中
Figure FDA0003440336960000093
为光心C在世界坐标系中的坐标，
Figure FDA0003440336960000094
为运动目标坐标系与世界坐标系之间的旋转矩阵，
Figure FDA0003440336960000095
为相机光心在运动目标坐标系中的坐标；in
Figure FDA0003440336960000093
is the coordinate of the optical center C in the world coordinate system,
Figure FDA0003440336960000094
is the rotation matrix between the moving target coordinate system and the world coordinate system,
Figure FDA0003440336960000095
is the coordinate of the camera optical center in the moving target coordinate system; 2)当单目相机所提取的2D-3D点对数大于2时，求解运动目标位姿的方法是：2) When the number of 2D-3D point pairs extracted by the monocular camera is greater than 2, the method for solving the pose of the moving target is: 步骤1’：剔除所提取的所有2D-3D点对中的野值，得到内点集；Step 1': Eliminate outliers in all the extracted 2D-3D point pairs to obtain an interior point set; 步骤2’：遍历内点集内任意两个2D-3D点对组合并求解每两个点对组合的目标位姿；Step 2': Traverse any two 2D-3D point pair combinations in the inner point set and solve the target pose of each two point pair combination; 遍历内点集中的所有2点组合，分别求解目标位姿参数(txi,tyi,θi)(i＝1,2...sn)，sn为组合总数目，当内点集中点对数为m时，
Figure FDA0003440336960000096
Figure FDA0003440336960000097
为m个物体中任取2个物体的组合数；Traverse all 2-point combinations in the interior point set, and solve the target pose parameters (t xi , t yi , θ i ) (i=1, 2...sn) respectively, sn is the total number of combinations, when the interior point set points to When the number is m,
Figure FDA0003440336960000096
Figure FDA0003440336960000097
is the number of combinations of 2 objects randomly selected from m objects; 步骤3’：取步骤3.2中所求目标位姿参数的平均值作为位姿估计值，即Step 3': take the average value of the target pose parameters obtained in step 3.2 as the pose estimation value, that is
Figure FDA0003440336960000101
Figure FDA0003440336960000101
 3)当单目相机所提取的2D-3D点对数为1时，求解运动目标位姿的方法是：3) When the logarithm of 2D-3D points extracted by the monocular camera is 1, the method for solving the pose of the moving target is: 取上一时刻运动目标的偏航角θ作为当前时刻的偏航角，则根据共线方程(4)线性求解出未知参数(tx,ty)，输出运动目标的位姿参数(tx,ty,θ)；Take the yaw angle θ of the moving target at the previous moment as the yaw angle at the current moment, then linearly solve the unknown parameters (t x , t y ) according to the collinear equation (4), and output the pose parameters (t x ) of the moving target , ty ,θ); 4)当单目相机在短时间内运动目标的视野中没有提取到有效控制点时，运动目标的位姿参数不更新；4) When the monocular camera does not extract effective control points in the field of view of the moving target in a short time, the pose parameters of the moving target are not updated; 5)当单目相机在持续时间超过一个阈值的时间内都没有提取到有效控制点时，发出警报信息。5) When the monocular camera fails to extract an effective control point for a duration exceeding a threshold, an alarm message is issued. 5.一种计算机可读介质，存储有基于单目相机的平面运动目标视觉导航方法的计算机程序，其特征在于，所述计算机程序被处理器执行以实现权利要求1至3中任一项所述的基于单目相机的平面运动目标视觉导航方法的步骤。5. A computer-readable medium storing a computer program based on a monocular camera-based visual navigation method for a plane moving target, wherein the computer program is executed by a processor to realize the method described in any one of claims 1 to 3. The steps of the described method for visual navigation of a plane moving target based on a monocular camera. 6.一种计算机设备，包括存储器和处理器，所述存储器存储有基于单目相机的平面运动目标视觉导航方法的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1至3中任一项所述的基于单目相机的平面运动目标视觉导航方法的步骤。6. A computer device, comprising a memory and a processor, wherein the memory stores a computer program for a method for visual navigation of a plane moving target based on a monocular camera, wherein the processor implements the claims when executing the computer program Steps of the monocular camera-based visual navigation method for a plane moving target described in any one of 1 to 3.
CN202011022982.6A 2020-09-25 2020-09-25 Method and system for visual navigation of plane moving target based on monocular camera Active CN112179357B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011022982.6A CN112179357B (en) 2020-09-25 2020-09-25 Method and system for visual navigation of plane moving target based on monocular camera

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011022982.6A CN112179357B (en) 2020-09-25 2020-09-25 Method and system for visual navigation of plane moving target based on monocular camera

Publications (2)

Publication Number Publication Date
CN112179357A CN112179357A (en) 2021-01-05
CN112179357B true CN112179357B (en) 2022-04-01

Family

ID=73944512

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011022982.6A Active CN112179357B (en) 2020-09-25 2020-09-25 Method and system for visual navigation of plane moving target based on monocular camera

Country Status (1)

Country Link
CN (1) CN112179357B (en)

Families Citing this family (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113128434B (en) * 2021-04-27 2023-11-21 南京大学 A method for 3D target detection on monocular RGB images
CN113379840B (en) * 2021-06-10 2023-02-28 北京航空航天大学 A Monocular Vision Pose Estimation Method Based on Coplanar Targets
CN113674353B (en) * 2021-08-18 2023-05-16 中国人民解放军国防科技大学 Accurate pose measurement method for space non-cooperative target
CN114777786B (en) * 2022-04-18 2024-11-29 中国人民解放军陆军军事交通学院军事交通运输研究所 Multi-agent cooperative target positioning method based on variable-focus and variable-pose optical module
CN115167395A (en) * 2022-06-16 2022-10-11 北京博雅睿视科技有限公司 In-plane robot movement control method, device, robot and storage medium
CN115493598B (en) * 2022-11-15 2023-03-10 西安羚控电子科技有限公司 Target positioning method and device in motion process and storage medium
CN120107475B (en) * 2025-02-18 2025-11-25 西安电子科技大学 Monocular vision-based flight target track reconstruction method and system

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106651953A (en) * 2016-12-30 2017-05-10 山东大学 Vehicle position and gesture estimation method based on traffic sign
CN108663052A (en) * 2018-04-28 2018-10-16 北京空间飞行器总体设计部 Autonomous space non-cooperative target Relative Navigation camera is directed toward control method on a kind of star
CN109029417A (en) * 2018-05-21 2018-12-18 南京航空航天大学 Unmanned plane SLAM method based on mixing visual odometry and multiple dimensioned map
CN109983507A (en) * 2016-12-21 2019-07-05 英特尔公司 The positioning returned based on extensive CNN is carried out via two-dimensional map
CN111191625A (en) * 2020-01-03 2020-05-22 浙江大学 Object identification and positioning method based on laser-monocular vision fusion

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR101085390B1 (en) * 2008-04-30 2011-11-21 주식회사 코아로직 Image representation method, apparatus for 3D navigation and mobile device including the apparatus
EP2622576A4 (en) * 2010-10-01 2017-11-08 Saab AB Method and apparatus for solving position and orientation from correlated point features in images
CN110956660B (en) * 2018-09-26 2023-10-10 深圳市优必选科技有限公司 Positioning methods, robots and computer storage media

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109983507A (en) * 2016-12-21 2019-07-05 英特尔公司 The positioning returned based on extensive CNN is carried out via two-dimensional map
CN106651953A (en) * 2016-12-30 2017-05-10 山东大学 Vehicle position and gesture estimation method based on traffic sign
CN108663052A (en) * 2018-04-28 2018-10-16 北京空间飞行器总体设计部 Autonomous space non-cooperative target Relative Navigation camera is directed toward control method on a kind of star
CN109029417A (en) * 2018-05-21 2018-12-18 南京航空航天大学 Unmanned plane SLAM method based on mixing visual odometry and multiple dimensioned map
CN111191625A (en) * 2020-01-03 2020-05-22 浙江大学 Object identification and positioning method based on laser-monocular vision fusion

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
A fast and robust vision-based horizon tracking method;Hao Guo 等;《2015 12th International Computer Conference on Wavelet Active Media Technology and Information Processing (ICCWAMTIP)》;20160620;第1-4页 *
ＩＥＰｎＰ：一种基于ＥＰｎＰ的相机位姿迭代估计算法;陈鹏 等;《光学学报》;20180430;第38卷(第4期);第1-7页 *

Also Published As

Publication number Publication date
CN112179357A (en) 2021-01-05

Similar Documents

Publication Publication Date Title
CN112179357B (en) Method and system for visual navigation of plane moving target based on monocular camera
CN111258313B (en) Multi-sensor fusion SLAM system and robot
CN110070615B (en) Multi-camera cooperation-based panoramic vision SLAM method
CN110176032B (en) Three-dimensional reconstruction method and device
CN112184824B (en) Camera external parameter calibration method and device
CN107481270B (en) Table tennis target tracking and trajectory prediction method, device, storage medium and computer equipment
JP5992184B2 (en) Image data processing apparatus, image data processing method, and image data processing program
CN100504299C (en) Method for obtaining three-dimensional information of space non-cooperative object
CN113850126A (en) Target detection and three-dimensional positioning method and system based on unmanned aerial vehicle
CN112304307A (en) Positioning method and device based on multi-sensor fusion and storage medium
CN111326023A (en) Method, device, equipment and storage medium for UAV route early warning
CN108520559B (en) Unmanned aerial vehicle positioning and navigation method based on binocular vision
CN108594851A (en) A kind of autonomous obstacle detection system of unmanned plane based on binocular vision, method and unmanned plane
CN111623773B (en) Target positioning method and device based on fisheye vision and inertial measurement
Schneider et al. Incremental real-time bundle adjustment for multi-camera systems with points at infinity
CN113447948B (en) Camera and multi-laser-radar fusion method based on ROS robot
CN114119652A (en) Method and device for three-dimensional reconstruction and electronic equipment
CN111862214A (en) Computer equipment positioning method and device, computer equipment and storage medium
Jian et al. Lvcp: Lidar-vision tightly coupled collaborative real-time relative positioning
WO2025055725A1 (en) Visual positioning method and system for unmanned aerial vehicle
CN114529585A (en) Mobile equipment autonomous positioning method based on depth vision and inertial measurement
CN113570663B (en) Indoor positioning method based on fusion of single-line laser radar and top-view camera
CN115619851A (en) Anchor-based VSLAM back-end optimization method, device, medium, equipment and vehicle
US20210156710A1 (en) Map processing method, device, and computer-readable storage medium
CN119169073A (en) Anchor hole positioning method and system

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