CN107728182B - Flexible multi-baseline measurement method and device based on camera assistance - Google Patents

Flexible multi-baseline measurement method and device based on camera assistance Download PDF

Info

Publication number
CN107728182B
CN107728182B CN201710837812.5A CN201710837812A CN107728182B CN 107728182 B CN107728182 B CN 107728182B CN 201710837812 A CN201710837812 A CN 201710837812A CN 107728182 B CN107728182 B CN 107728182B
Authority
CN
China
Prior art keywords
camera
measurement
imu
sub
pose
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
CN201710837812.5A
Other languages
Chinese (zh)
Other versions
CN107728182A (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.)
Beihang University
Original Assignee
Beihang 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 Beihang University filed Critical Beihang University
Priority to CN201710837812.5A priority Critical patent/CN107728182B/en
Publication of CN107728182A publication Critical patent/CN107728182A/en
Application granted granted Critical
Publication of CN107728182B publication Critical patent/CN107728182B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/53—Determining attitude

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Automation & Control Theory (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

The invention discloses a flexible multi-baseline measurement method based on camera assistance, which is characterized in that a state equation and a measurement equation based on visual assistance are established according to a feature point pose calculation result and a strapdown calculation result; fusing visual data and POS data information; and (3) high-precision measurement of distributed flexible multiple baselines. The method overcomes the defect of low alignment precision under the dynamic condition of the traditional initial alignment method, has the characteristics of high precision and strong anti-interference capability, can be used for measuring the length of a flexible base line among multiple loads when a carrier is subjected to flexural deformation, and improves the relative position and posture precision among the multiple loads. The invention also discloses a camera-assisted flexible multi-baseline measuring device.

Description

Flexible multi-baseline measurement method and device based on camera assistance
Technical Field
The invention relates to the technical field of aerospace, in particular to a flexible multi-baseline measurement method and device based on camera assistance.
Background
The high-precision POS is composed of an Inertial Measurement Unit (IMU), a navigation Computer System (PCS), and a GPS. The high-precision POS can provide high-frequency and high-precision time, space and precision information for the high-resolution aerial remote sensing system, improves imaging precision and efficiency through motion error compensation, and is the key for realizing high-resolution imaging. China makes certain progress in the aspect of single POS imaging, but due to the requirement traction of earth observation loads, such as integrating a high-resolution mapping camera, a full-spectrum imaging spectrometer and multitask loads of an SAR on the same carrier, an airborne distributed array antenna SAR, a flexible multi-baseline interference SAR, a carrier-borne sparse array imaging radar and the like, a plurality of or a plurality of loads are installed at different positions of an airplane, and the traditional single POS system cannot realize multi-point high-precision position attitude measurement and time unification of each load data.
Meanwhile, for an aerial remote sensing system integrating multiple loads and an array load, due to the factors of flexural deformation, vibration and the like of an airplane body and a flexible lever arm, position, speed and attitude information of the multiple loads distributed at different positions of the airplane cannot be measured by a single POS. If each load is provided with one POS, the weight and the cost are increased, and different system errors exist among different POSs, so that data among a plurality of loads are difficult to fuse, and therefore a high-precision distributed space-time reference system is urgently needed to be established, and high-precision time and space information is provided for all loads in a high-performance aerial remote sensing system.
The existing flexible lever arm measuring method (publication number: CN 102322873) builds a flexible lever arm testing environment and provides a flexible lever arm measuring accuracy verification method, a detailed flexible lever arm measuring algorithm is not provided, and the position and attitude measuring accuracy of a subsystem can be directly limited. Aiming at the problem that the measurement precision requirement of the flexible baseline measurement characteristic is high, the relative position posture relation between the main subsystem and the subsystem is measured by using the camera while the high-precision main IMU is used for transferring and aligning the subsystem, and the measured information is used for assisting in transferring and aligning, so that the real-time navigation precision of the whole system is improved, and the accurate measurement of the flexible multiple baselines is realized.
Disclosure of Invention
Therefore, it is necessary to provide a camera-assisted flexible multi-baseline measurement method and device for solving the problems in the conventional technology, which can overcome the disadvantage of low alignment accuracy in the dynamic condition of the conventional initial alignment method, have the characteristics of high accuracy and strong anti-interference capability, and can be used for measuring the length of a flexible baseline between multiple loads when a carrier has flexural deformation, and improve the accuracy of relative position and attitude between the multiple loads.
In a first aspect, an embodiment of the present invention provides a camera-assisted flexible multi-baseline measurement method, where the method includes: establishing a state equation and a measurement equation based on visual assistance according to the feature point pose calculation result and the strapdown calculation result; fusing visual data and POS data information; and (3) high-precision measurement of distributed flexible multiple baselines.
In one embodiment, the feature point pose calculation result is completed through camera modeling and camera calibration; the strapdown solution result is operated and completed through the distributed POS system.
In one embodiment, the camera modeling is a process of combining a camera coordinate system and an image coordinate system, calibrating the camera by adopting a universal checkerboard grid calibration method, obtaining more than two template images in different directions through the camera, and obtaining internal parameters of the camera by utilizing an identity matrix between a feature point on a plane template and an image point corresponding to the feature point.
In one embodiment, the method further comprises the following steps: the camera extracts the features of any sub IMU surface target, the pose relation of each target relative to the camera is obtained through a P4P method, and the pose relation from the sub IMU measurement center to the camera is obtained.
In one embodiment, the acquiring the pose relationship of each target relative to the camera by the P4P method includes: extracting the target edge; judging through a quadrilateral shape, and straightening and binarizing the acquired image; performing rapid line-row scanning in the determined quadrangle, and judging the number and the central point coordinate of the target; the pose relationship between the two coordinate systems can be uniquely determined by using the 4 characteristic points.
In one embodiment, the method further comprises the following steps: and obtaining the pose relation between any sub IMU and the main IMU by utilizing the position installation relation between the camera and the main IMU.
In one embodiment, the distributed flexible multi-baseline high-precision measurement includes: the pose information of the sub IMU acquired by the camera is used as measurement information; carrying out transfer alignment by adopting a matching method based on position, speed and posture to obtain accurate subsystem combined navigation information; and solving the accurate base line length between the main/sub IMUs to finish the flexible multi-base line measurement.
In one embodiment, the method further comprises the following steps: the accurate integrated navigation information of the main IMU and the pose information of the main IMU and the sub IMU acquired by the camera are used as the reference of the transfer alignment of the sub IMU; the error of the sub IMU is reflected by calculating the direct measurement difference between the main IMU and the sub IMU; and the lever arm correction is realized through a matching method of position, speed and attitude, and the integral smoothing is carried out on the flexural deformation noise.
In one embodiment, the position + velocity + attitude matching method includes:
when the equivalent weight measurement selects the position error, the speed error and the attitude error of the main inertial measurement unit and the sub inertial measurement unit for transfer alignment, the measurement equation is Z (t) ═ H (t) X (t) + v (t), wherein Z is a measurement variable, H is a measurement matrix, v is measurement noise,
Z＝[δL δR δh δψ δθ δγ δVEδVNδVU]T
wherein, δ L δ R δ h is the position error of the system, δ ψ, δ θ, δ γ are the course angle error, pitch angle error, roll angle error of the system, i.e. three attitude errors, δ VE,δVN,δVUThe speed errors of the system in east direction, north direction and sky direction are three speed errors.
In a second aspect, the present invention provides a computer-readable storage medium, on which a computer program is stored, where the computer program, when executed by a processor, implements the camera-assisted flexible multi-baseline measurement method according to the first aspect.
In a third aspect, an embodiment of the present invention provides a computer program product containing instructions, which when run on a computer, causes the computer to perform the method according to the first aspect.
In a fourth aspect, an embodiment of the present invention further provides a camera-assisted flexible multi-baseline measurement apparatus, where the apparatus includes: the equation establishing module is used for establishing a state equation and a measurement equation based on visual assistance according to the feature point posture calculation result and the strapdown calculation result; the fusion module is used for fusing the visual data and the POS data information; and the measuring module is used for high-precision measurement of the distributed flexible multiple baselines.
According to the flexible multi-baseline measurement method and device based on camera assistance, a state equation and a measurement equation based on visual assistance are established through a feature point pose calculation result and a strapdown calculation result; fusing visual data and POS data information; and (3) high-precision measurement of distributed flexible multiple baselines. The method overcomes the defect of low alignment precision under the dynamic condition of the traditional initial alignment method, has the characteristics of high precision and strong anti-interference capability, can be used for measuring the length of a flexible base line among multiple loads when a carrier is subjected to flexural deformation, and improves the relative position and posture precision among the multiple loads.
Drawings
FIG. 1 is a schematic flow chart of a camera-based flexible multi-baseline measurement method according to an embodiment of the present invention;
FIG. 2 is a schematic flow chart of a centralized camera-assisted flexible multi-baseline measurement method according to another embodiment of the present invention;
FIG. 3 is a block diagram of an exemplary camera-based flexible multi-baseline measurement device in an embodiment of the invention;
FIG. 4 is a block diagram of a flexible multi-baseline measurement device based on camera assistance in an embodiment of the invention; and
fig. 5 is a schematic structural diagram of a flexible multi-baseline measurement device based on camera assistance in an embodiment of the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more clearly understood, the flexible multi-baseline measurement method and apparatus based on camera assistance according to the present invention are further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention.
Fig. 1 is a schematic flow chart of a flexible multi-baseline measurement method based on camera assistance in an embodiment. The method specifically comprises the following steps:
and 102, establishing a state equation and a measurement equation based on visual assistance according to the feature point pose calculation result and the strapdown calculation result. In this embodiment, the feature point pose calculation result is completed by camera modeling and camera calibration; the strapdown resolution results are operationally completed through the distributed POS system.
It should be noted that the camera modeling is a process of combining a camera coordinate system and an image coordinate system, calibrating the camera by using a general checkerboard grid calibration method, obtaining more than two template images in different directions by the camera, and obtaining internal parameters of the camera by using an identity matrix between a feature point on a planar template and an image point corresponding to the feature point.
And step 104, fusing the visual data with the POS data information.
And 106, measuring the distributed flexible multiple baselines at high precision.
In this embodiment, the distributed flexible multi-baseline high-precision measurement includes: the pose information of the sub IMU acquired by the camera is used as measurement information; carrying out transfer alignment by adopting a matching method based on position, speed and posture to obtain accurate subsystem combined navigation information; and solving the accurate base line length between the main/sub IMUs to finish the flexible multi-base line measurement.
Further, the present disclosure provides a flexible multi-baseline measurement method based on camera assistance, in an embodiment, the method further includes: the camera extracts the features of any sub IMU surface target, the pose relation of each target relative to the camera is obtained through a P4P method, and the pose relation from the sub IMU measurement center to the camera is obtained. Specifically, the acquiring the pose relationship of each target relative to the camera by the P4P method includes: extracting the target edge; judging through a quadrilateral shape, and straightening and binarizing the acquired image; performing rapid line-row scanning in the determined quadrangle, and judging the number and the central point coordinate of the target; the pose relationship between the two coordinate systems can be uniquely determined by using the 4 characteristic points.
Still further, in one embodiment, the method further comprises: and obtaining the pose relation between any sub IMU and the main IMU by utilizing the position installation relation between the camera and the main IMU.
Still further, in one embodiment, the method further comprises: the accurate integrated navigation information of the main IMU and the pose information of the main IMU and the sub IMU acquired by the camera are used as the reference of the transfer alignment of the sub IMU; the error of the sub IMU is reflected by calculating the direct measurement difference between the main IMU and the sub IMU; and the lever arm correction is realized through a matching method of position, speed and attitude, and the integral smoothing is carried out on the flexural deformation noise.
It should be noted that the matching method of position + velocity + attitude is as follows: when the equivalent weight measurement selects the position error, the speed error and the attitude error of the main inertial measurement unit and the sub inertial measurement unit for transfer alignment, the measurement equation is Z (t) ═ H (t) X (t) + v (t), wherein Z is a measurement variable, H is a measurement matrix, v is measurement noise,
Z＝[δL δR δh δψ δθ δγ δVEδVNδVU]T
wherein, δ L δ R δ h is the position error of the system, δ ψ, δ θ, δ γ are the course angle error, pitch angle error, roll angle error of the system, i.e. three attitude errors, δ VE,δVN,δVUThe speed errors of the system in east direction, north direction and sky direction are three speed errors.
According to the camera-aided flexible multi-baseline measurement method, a state equation and a measurement equation based on visual assistance are established according to the feature point pose calculation result and the strapdown calculation result; fusing visual data and POS data information; and (3) high-precision measurement of distributed flexible multiple baselines. The method overcomes the defect of low alignment precision under the dynamic condition of the traditional initial alignment method, has the characteristics of high precision and strong anti-interference capability, can be used for measuring the length of a flexible base line among multiple loads when a carrier is subjected to flexural deformation, and improves the relative position and posture precision among the multiple loads.
For a clearer understanding and application of the camera-assisted flexible multi-baseline measurement method proposed by the present invention, the following example is made. It should be noted that the scope of the present disclosure is not limited to the following examples.
In particular, as shown in fig. 2-4, the inertial measurement units of the position and attitude measurement system (POS) are mounted to corresponding nodes of the aerial carrier, wherein the main IMU is in the nacelle under the belly, the sub IMUs are at each node of the wing, the targets are attached to one side surface of the sub IMUs, and the cameras are mounted in the nacelle and rigidly connected with the main IMU, and the distributed POS measurement system is started to perform measurement as shown in fig. 2.
Further, the primary IMU initially aligns and integrates navigation. On one hand: the primary IMU initial alignment is accomplished using conventional analytical methods. Specifically, in a carrier coordinate system: gravity acceleration g and earth rotation angular velocity omegaieMay be obtained from the outputs of an accelerometer and a gyroscope; under the navigation coordinate system: the local longitude λ, latitude L can be obtained from GPS data, the gravitational acceleration g and the rotational angular velocity ω of the earthieThe components in the geographic coordinate system are all determinable, as follows:
Figure GDA0001490105560000071
c. strapdown matrix
Figure GDA0001490105560000072
The following equation is used:
Figure GDA0001490105560000073
on the other hand: and (4) real-time navigation of a main system, including strapdown solution and Kalman filtering. It should be noted that, the position, the speed, and the attitude at the above moment of the strapdown solution are used as initial values of the current strapdown solution, and the inertial navigation result at the current moment is obtained by combining the main IMU data at the current moment. The method mainly comprises attitude matrix updating, attitude calculation, speed calculation, position matrix updating and position calculation, and is described as follows:
attitude matrix update and attitude calculation
Updating attitude matrix by quaternion method
Figure GDA0001490105560000081
Figure GDA0001490105560000082
The initial quaternion calculation formula is:
Figure GDA0001490105560000083
the attitude update calculation can be performed by the following formula:
Figure GDA0001490105560000084
the course angle psi is an included angle between the projection of the IMU coordinate system y axis on the navigation coordinate system horizontal plane (XY plane) and the navigation coordinate system y axis, and calculated from the navigation coordinate system y axis, the 'anticlockwise' is positive, and the effective range is [0 degrees, 360 degrees ]; the pitch angle theta is an included angle between the y axis of the IMU coordinate system and the horizontal plane (XY plane) of the navigation coordinate system, the load head-up is taken as positive, namely the vector direction of the y axis of the IMU coordinate system is higher than the horizontal plane and is positive, otherwise, the vector direction is negative, and the effective range is [ -90 degrees, 90 degrees ]; the roll angle γ is defined as the IMU right dip being positive (with the IMU coordinate system y axis vector pointing forward, the IMU coordinate system x axis pointing right), the left dip being negative, the effective range being [ -180 °, 180 ° ]. After the attitude update, the result is calculated by the following formula:
Figure GDA0001490105560000085
velocity calculation
The velocity update is calculated by:
Figure GDA0001490105560000091
in the formula
Figure GDA0001490105560000092
For the velocity increment along the three axes of x, y and z in the navigation coordinate system,
Figure GDA0001490105560000093
the projection of the acceleration of a carrier coordinate system relative to an inertia space on three axes of x, y and z is realized,
Figure GDA0001490105560000094
the acceleration is obtained by the above formula for the projection of the self-transmission angular velocity of the earth in the directions of three axes of x, y and z under the navigation coordinate system
Figure GDA0001490105560000095
Then
Figure GDA0001490105560000096
Location matrix update and location calculation
The position matrix update is performed by the following differential equation:
Figure GDA0001490105560000097
in the formula
Figure GDA0001490105560000098
Respectively, the projection of the rotation angle rate of the navigation coordinate system relative to the earth coordinate system in the directions of three axes of x, y and z under the navigation coordinate system, and the position matrix is updated by adopting a first-order Euler method, wherein the speed expression is as follows:
Figure GDA0001490105560000099
wherein T is the sampling period of the inertial navigation system. After the position matrix is updated, the navigation position parameters can be calculated and recorded
Figure GDA00014901055600000910
Comprises the following steps:
Figure GDA00014901055600000911
the height H is diverged due to the fact that a height calculation channel of the pure inertial navigation system is divergent, and external height information is used for damping the height channel of the strapdown calculation algorithm.
It should be further explained that, regarding the pose solution, the calibration of the camera is first required. Specifically, monocular vision calibration adopts a calibration method of Zhangyingyou, and the method adopts a plane lattice template (usually a checkerboard template) with accurate positioning information, obtains more than two template images in different directions through a camera, and obtains internal parameters of the camera by utilizing an identity matrix between a characteristic point on the plane template and a corresponding image point.
Assuming the plane of the plane template as Z in the world coordinate systemwPlane of 0, homogeneous coordinate of object point P is P ═ (X)w,Yw,0,1)TThe homogeneous coordinate of the undistorted image point corresponding to the image plane is p ═ u, v,1)TThe rotation matrix R is represented as R ═ R1,r2,r3]From the linear model of the camera imaging, the following relationship can be obtained:
Figure GDA0001490105560000101
wherein s is an arbitrary non-zero scale factor, and K is a camera intrinsic parameter matrix. If used, the
Figure GDA0001490105560000102
Representing the homogeneous coordinates of the point P in the template coordinate system, the above formula can be rewritten as follows
Figure GDA0001490105560000103
Wherein, H ═ λ K [ r ]1,r2,r3]Is the homography matrix from the template plane to the image plane, λ is a constant factor. H is given as1,h2,h3]Then there is [ h1h2h3]＝λK[r1r2t]。
Given a planar template and its corresponding image, the homography matrix H between them can be estimated using a direct linear transformation method and then optimized with maximum likelihood estimation to reduce the effects of image noise.
From the above formula r1＝(1/λ)K-1h1And r2＝(1/λ)K-1h2. Orthogonality according to the rotation matrix R has R1 Tr20 and r1||＝||r21. Therefore, two constraint equations of the homography matrix H to the camera intrinsic parameter matrix K can be obtained:
Figure GDA0001490105560000104
let B equal to K-TK-1＝(Bij)3×3B is then a symmetric matrix describing the projection of an absolute quadratic curve (absoluteconic) on the image plane. B has 6 different elements in total according to symmetry, so that a six-dimensional vector B ═ can be defined (B)11,B12,B22,B13,B23,B33)TIt is described. Let the ith column vector in H be H ═ H (H)i1,hi2,hi3)TThe above equation can be organized into two homogeneous equations with b as the unknown quantity:
Figure GDA0001490105560000111
wherein v isij＝(hi1hj1,hi1hj2+hi2hj1,hi2hj2,hi1hj3+hi3hj1,hi2hj3+hi3hj2,hi3hj3)TFor n images, the equations shown in the resulting n sets of equations are stacked and written in matrix form:
Vb＝0
where V is a 2n × 6 matrix. In general, for n ≧ 3, b can be uniquely determined in the sense of differing by a scale factor. Since the 4-parameter model is used herein, there areB120, so [010000 ] can be used]b is 0 as an additional equation to the above equation, and b is solved using only two images. The unit of b is solved as matrix VTAnd V is the characteristic vector corresponding to the minimum characteristic value. After B is obtained, K can be solved by performing Cholesky matrix decomposition on the matrix B-1And further inverting the obtained K to obtain K, and also directly obtaining an analytic solution of each element of the K according to the relation between the K and the B. After the internal parameter matrix K is calculated, the external parameters corresponding to each image can be solved:
Figure GDA0001490105560000112
since the influence of image distortion and noise is not considered in the above solving process, the obtained result is only a rough estimation of the camera model parameters, and needs to be further optimized under the condition of considering the image distortion and the noise. For a calibration process using n images, if the number of feature points on each image is m, an optimized objective function can be established as follows:
Figure GDA0001490105560000113
wherein, PijIs a characteristic point PjActual image point on the ith image, and
Figure GDA0001490105560000121
then is PjAnd virtual projection image points under a camera model formed by the current internal parameters and the current external parameters of the ith calibration image. And (3) performing iterative optimization on the formula by using a Levenberg-Marquardt algorithm, and finally obtaining a camera internal and external parameter calibration result with high precision.
And further, target feature extraction and pose calculation are carried out. Specifically, a four-point measurement model (perspective-4-point-projection) is also called a 4-point perspective projection problem, which is called P4P for short, and the corresponding N-point perspective projection model is called a PNP model for short. The 4 characteristic points can uniquely determine the pose relationship between the two coordinate systems, and a plurality of scholars analyze the four-point pose calculation method.
Resolving the relation according to the known three-point pose:
Figure GDA0001490105560000122
a solution model based on 4 feature points can be obtained, that is, 3 univariate quartic equations for X can be obtained when n is 4, as shown below:
Figure GDA0001490105560000123
writing in matrix form:
Figure GDA0001490105560000124
wherein, T5＝[T1T2T3T4T5]T，A3×5The rank of (2) is 3 at maximum, and its SVD (singular value decomposition) is U3×5diag(σ1,σ2,σ3,0,0)(v1,v2,v3,v4,v5)TThen T is5Can be represented as T5＝λv4+ρv5。
Analysis according to L.Quan gives the formula TiTj＝TkTlWhere i + j + l, 0 ≦ i, j, k, l ≦ 4, the sum may be:
b1λ2+b2λρ+b3ρ2＝0
Figure GDA0001490105560000135
Figure GDA0001490105560000136
wherein
Figure GDA0001490105560000137
For { (i, j, k, l), i + j ═ k + l&There are 7 cases where 0. ltoreq. i, j, k, l. ltoreq.4, and therefore there are 7 different groups (b)1,b2,b3) Bringing them into the above formula and expressing them in a matrix form yields the formula:
Figure GDA0001490105560000131
wherein Y is3＝[Y0,Y1,Y2]TThus can be obtained
Figure GDA0001490105560000132
The combined upper formula can be solved to obtain lambda, rho and T5The value of X can also be solved, and the final distance value
Figure GDA0001490105560000133
It will be appreciated that the subsystem builds a model of the transfer alignment containing the flexure lever arm errors, with the transfer alignment using a non-linear filter matching method based on "position + velocity + attitude". The principle is that the difference between the high-precision speed and attitude information of the main POS and the speed and attitude information of the sub POS is used to estimate and correct the attitude error angle between the main POS and the sub POS. The model of the filter includes a state equation and a measurement equation. The method comprises the following specific steps:
attitude measurement δ a' ═ [ δ ψ δ θ δ γ ] is the difference between the attitude angle of the main POS and the attitude angle of the sub IMU after the flexure angle compensation measured by the grating, and its expression is as follows:
δa′＝as-a′m
in the formula: a issIs the attitude of the subsystem in the navigation coordinates, a'mThe measured attitude angle of the sub-IMU for the camera can be transferred by aligning the attitude transfer matrix
Figure GDA0001490105560000134
Inverse solution of the attitude angle to obtain, wherein CθCan be composed ofAnd (5) calculating and obtaining the pose of the computer.
The position quantity measurement δ P '═ δ L' δ λ 'δ h' ] is the difference between the latitude, longitude and altitude between the position of the sub IMU and the sub IMU measured by the master POS, and is expressed as follows:
Figure GDA0001490105560000141
in the formula: psAnd PmThe positions of the subsystem and the main system under the navigation coordinates are respectively.
The airborne distributed measurement system is a nonlinear system in practical application, so that a transfer alignment model of the sub-nodes is a nonlinear model which comprises an attitude angle error equation, a speed error equation, a position error equation, an inertial device error equation, a scale factor error equation, an inertial device installation error equation, a flexible deformation angle error equation and a flexible deformation displacement error equation, and the specific steps are as follows:
attitude angle error differential equation:
Figure GDA0001490105560000142
velocity error differential equation:
Figure GDA0001490105560000143
differential equation of position error:
Figure GDA0001490105560000144
differential equation of error of inertial instrument:
Figure GDA0001490105560000145
differential equation of the fixed installation error angle ρ:
Figure GDA0001490105560000146
scale factor error δ KgAnd δ KaDifferential equation of (a):
Figure GDA0001490105560000147
differential equations for the mounting errors δ G and δ a:
Figure GDA0001490105560000151
flexible deformation displacement differential equation:
Figure GDA0001490105560000152
Figure GDA0001490105560000153
in the formula:
Figure GDA0001490105560000154
is the subsystem attitude misalignment angle phiE、φNAnd phiUEast, north, and sky misalignment angles, respectively, subscripts E, N and U denoting east, north, and sky, respectively;
Figure GDA0001490105560000155
navigating the angular velocity of the subsystem relative to the inertial system;
Figure GDA0001490105560000156
is composed of
Figure GDA0001490105560000157
The error angular velocity of (1);
Figure GDA0001490105560000158
attitude matrix for child IMU carrier to its navigation system
Figure GDA0001490105560000159
An estimated value of (d);
Figure GDA00014901055600001525
and
Figure GDA00014901055600001510
respectively, subsystem speed and speed error, where VE、VNAnd VUEast, north and sky velocity, respectively, delta VE、δVNAnd δ VUEast, north and sky speed errors, respectively;
Figure GDA00014901055600001511
is the specific force of the subsystem, where fE、fNAnd fUEast, north and sky forces, respectively;
Figure GDA00014901055600001512
and
Figure GDA00014901055600001513
the angular speed and the error of the subsystem navigation system relative to the earth coordinate system are respectively;
Figure GDA00014901055600001514
and
Figure GDA00014901055600001515
the angular speed and the error of the subsystem navigation system relative to the earth coordinate system are respectively; l, lambda, H, delta L, delta lambda and delta H are respectively subsystem latitude, longitude, altitude, latitude error, longitude error and altitude error;
Figure GDA00014901055600001516
is the first derivative of the latitude and,
Figure GDA00014901055600001517
is the first derivative of longitude; rMAnd RNRespectively the main curvature radius along the meridian circle and the prime circle; epsilonb＝[εxεyεz]TAnd
Figure GDA00014901055600001518
respectively carrying out constant drift and constant adding bias on the gyroscope of the subsystem;
Figure GDA00014901055600001519
and
Figure GDA00014901055600001520
respectively adding the normal value offset of the x axis, the y axis and the z axis of the subsystem carrier system; delta Kg＝diag[δKgx,δKgy,δKgz]And δ Ka＝diag[δKax,δKay,δKaz]Scale factor error matrices for the gyroscope and accelerometer, respectively;
Figure GDA00014901055600001521
and
Figure GDA00014901055600001522
respectively are installation error matrixes of a gyroscope and an accelerometer; r isx、
Figure GDA00014901055600001523
Respectively torsional displacement, torsional rate and torsional acceleration, r, around the x-axis of the coordinate system of the bodyy、
Figure GDA00014901055600001524
Respectively bending displacement, bending speed and bending acceleration around the y-axis of the body coordinate system.
Since the wing is twisted around the x-axis and bent around the y-axis to cause displacement in the z-axis direction, the total deformation displacement of the flexible base line in the z-axis is rz＝rx+ryThe same principle can be used to obtain the total deformation rate
Figure GDA0001490105560000161
And acceleration
Figure GDA0001490105560000162
Figure GDA0001490105560000165
As a first-order modal damping coefficient,
Figure GDA0001490105560000166
for frequency of the first order mode, #1(x)、η1(x) Values for bending and torsional mode functions. The state equation and the measurement equation for establishing the transfer alignment system model are as follows:
Figure GDA0001490105560000163
in the formula: the state variable X expression is:
Figure GDA0001490105560000164
wherein G (t) is a system noise driving array; w (t) is system noise; h (t) is a measurement matrix; v (t) is measurement noise.
In summary, the principle of the flexible multi-baseline measurement device based on camera assistance proposed by the present disclosure is: firstly, a test environment of the flexible lever arm is built, a high-precision main inertia measurement unit (main IMU) and a plurality of low-precision sub inertia measurement units (sub IMUs) are installed on corresponding installation nodes of a flexible lever arm structure frame, a main target and a plurality of sub targets are respectively attached to one side of the main IMU and one side of the sub IMU, and a system is powered on. IMU carries on the initial alignment, realize the output of position, speed, attitude information; the method comprises the following steps that 1, a camera captures an image with a main target and a sub-target, and a pose relation between the main target and the sub-target is obtained through pose resolving; converting the sub-target information captured by other cameras, and uniformly solving the pose relation relative to the main target; the sub IMU establishes a transfer alignment model containing a deflection lever arm error by adopting a nonlinear filtering matching method based on 'position + speed + attitude', performs transfer alignment by means of position information attitude information of the main IMU and the pose relationship of the main IMU and the sub IMU obtained by vision, obtains accurate position, speed and attitude information of a subsystem and the relative relationship between the main IMU and the sub IMU, and realizes flexible multi-baseline measurement.
It needs to be further explained that compared with the prior art, the method has the advantages that aiming at the problem of low accuracy of the sub-IMU, the position and pose measurement accuracy is improved by adopting a visual auxiliary means, the defect of low transfer alignment accuracy of the low-accuracy sub-IMU is overcome, and the sub-IMU strapdown resolving accuracy is improved; the method of using the camera chain is used for realizing multi-node information measurement, the problem of insufficient field of view of a single camera is solved, and the length of a measurable baseline is increased; the deflection deformation error between the main IMU and the sub IMU is used as a state variable to establish a deflection deformation model, a high-precision integrated navigation result is obtained by adopting a transfer alignment method based on 'position + attitude', the defect that the traditional inertia/satellite integrated navigation method is greatly influenced by lever arm change is overcome, and the measurement precision of the system on the flexible lever arm is improved.
Based on the same inventive concept, a flexible multi-baseline measuring device based on camera assistance is also provided. Because the principle of the device for solving the problems is similar to that of the flexible multi-baseline measurement method based on camera assistance, the device can be implemented according to the specific steps and time limits of the method, and repeated parts are not repeated.
Fig. 5 is a schematic structural diagram of a flexible multi-baseline measurement apparatus based on camera assistance in an embodiment. The camera-based assisted flexible multi-baseline measurement device 10 comprises: an equation building module 200, a fusion module 400, and a measurement module 600.
The equation establishing module 200 is configured to establish a state equation and a measurement equation based on visual assistance according to the feature point posture solution result and the strapdown solution result; the fusion module 400 is used for fusing the visual data and the POS data information; the measurement module 600 is used for distributed flexible multi-baseline high-precision measurement.
According to the camera-assistance-based flexible multi-baseline measuring device, the state equation and the measurement equation based on the vision assistance are established through the equation establishing module 200 through the feature point pose resolving result and the strapdown resolving result; then, the visual data and the POS data information are fused through a fusion module 400; finally, the distributed flexible multi-baseline high-precision measurement is performed through the measurement module 600. The device overcomes the defect of low alignment precision under the dynamic condition of the traditional initial alignment method, has the characteristics of high precision and strong anti-interference capability, can be used for measuring the length of a flexible base line among multiple loads when a carrier is subjected to flexural deformation, and improves the relative position and posture precision among the multiple loads.
The embodiment of the invention also provides a computer readable storage medium. The computer-readable storage medium has stored thereon a computer program, which is executed by the processor of fig. 1.
The embodiment of the invention also provides a computer program product containing the instruction. Which when run on a computer causes the computer to perform the method of fig. 1 described above.
It will be understood by those skilled in the art that all or part of the processes of the methods of the embodiments described above can be implemented by a computer program, which can be stored in a computer-readable storage medium, and when executed, can include the processes of the embodiments of the methods described above. The storage medium may be a magnetic disk, an optical disk, a Read-Only Memory (ROM), a Random Access Memory (RAM), or the like.
The technical features of the embodiments described above may be arbitrarily combined, and for the sake of brevity, all possible combinations of the technical features in the embodiments described above are not described, but should be considered as being within the scope of the present specification as long as there is no contradiction between the combinations of the technical features.
The above-mentioned embodiments only express several embodiments of the present invention, and the description thereof is more specific and detailed, but not construed as limiting the scope of the present invention. It should be noted that, for a person skilled in the art, several variations and modifications can be made without departing from the inventive concept, which falls within the scope of the present invention. Therefore, the protection scope of the present patent shall be subject to the appended claims.

Claims (5)

1. A camera-assisted flexible multi-baseline measurement method, the method comprising:
establishing a state equation and a measurement equation based on visual assistance according to the feature point pose calculation result and the strapdown calculation result;
fusing visual data and POS data information;
distributed flexible multi-baseline high-precision measurement;
the pose calculation result of the feature points is completed through camera modeling and camera calibration; the strapdown resolving result is operated and completed through a distributed POS system;
the camera modeling is a process of combining a camera coordinate system and an image coordinate system, calibrating the camera by adopting a universal chessboard grid calibration method, obtaining more than two template images in different directions through the camera, and solving internal parameters of the camera by utilizing an unity matrix between a characteristic point on a plane template and a corresponding image point of the characteristic point;
wherein, the flexible many baselines of distributed high accuracy measurement includes: the pose information of the sub IMU acquired by the camera is used as measurement information; carrying out transfer alignment by adopting a matching method based on position, speed and posture to obtain accurate subsystem combined navigation information; solving the accurate base line length between the main/sub IMUs to complete the flexible multi-base line measurement;
further comprising: the accurate integrated navigation information of the main IMU and the pose information of the main IMU and the sub IMU acquired by the camera are used as the reference of the transfer alignment of the sub IMU; the error of the sub IMU is reflected by calculating the direct measurement difference between the main IMU and the sub IMU; the lever arm correction is realized through a position + speed + posture matching method, and the integral smoothing is carried out on the flexural deformation noise;
the matching method of the position, the speed and the posture comprises the following steps: when the equivalent weight measurement selects the position error, the speed error and the attitude error of the main inertial measurement unit and the sub inertial measurement unit for transfer alignment, the measurement equation is Z (t) ═ H (t) X (t) + v (t), wherein Z is a measurement variable, H is a measurement matrix, v is measurement noise,
Z＝[δL δR δh δψ δθ δγ δVEδVNδVU]T
wherein, δ L δ R δ h is the position error of the system, δ ψ, δ θ, δ γ are the course angle error, pitch angle error, roll angle error of the system, i.e. three attitude errors, δ VE,δVN,δVUThe speed errors of the system in east direction, north direction and sky direction are three speed errors.
2. The method of claim 1, further comprising: the camera extracts the features of any sub IMU surface target, the pose relation of each target relative to the camera is obtained through a P4P method, and the pose relation from the sub IMU measurement center to the camera is obtained.
3. The method according to claim 2, wherein acquiring the pose relationship of each target with respect to the camera by the P4P method includes: extracting the target edge;
judging through a quadrilateral shape, and straightening and binarizing the acquired image;
performing rapid line-row scanning in the determined quadrangle, and judging the number and the central point coordinate of the target;
the pose relationship between the two coordinate systems can be uniquely determined by using the 4 characteristic points.
4. The method of claim 1, further comprising: and obtaining the pose relation between any sub IMU and the main IMU by utilizing the position installation relation between the camera and the main IMU.
5. A camera-assisted based flexible multi-baseline measurement apparatus, the apparatus comprising:
the equation establishing module is used for establishing a state equation and a measurement equation based on visual assistance according to the feature point posture calculation result and the strapdown calculation result;
the fusion module is used for fusing the visual data and the POS data information;
and the measuring module is used for high-precision measurement of the distributed flexible multiple baselines.
CN201710837812.5A 2017-09-18 2017-09-18 Flexible multi-baseline measurement method and device based on camera assistance Active CN107728182B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710837812.5A CN107728182B (en) 2017-09-18 2017-09-18 Flexible multi-baseline measurement method and device based on camera assistance

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710837812.5A CN107728182B (en) 2017-09-18 2017-09-18 Flexible multi-baseline measurement method and device based on camera assistance

Publications (2)

Publication Number Publication Date
CN107728182A CN107728182A (en) 2018-02-23
CN107728182B true CN107728182B (en) 2020-03-24

Family

ID=61206604

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710837812.5A Active CN107728182B (en) 2017-09-18 2017-09-18 Flexible multi-baseline measurement method and device based on camera assistance

Country Status (1)

Country Link
CN (1) CN107728182B (en)

Families Citing this family (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108375383B (en) * 2018-02-22 2019-12-24 北京航空航天大学 Multi-camera-assisted airborne distributed POS flexible baseline measurement method and device
CN108663043B (en) * 2018-05-16 2020-01-10 北京航空航天大学 Single-camera-assisted distributed POS main node and sub node relative pose measurement method
CN110443854A (en) * 2019-08-05 2019-11-12 兰州交通大学 Based on fixed target without relative pose scaling method between public view field camera
CN110376614B (en) * 2019-08-08 2020-06-12 中国人民解放军军事科学院国防科技创新研究院 Navigation anti-interference enhanced load system and method based on low-earth-orbit satellite
CN110745702B (en) * 2019-10-16 2020-11-17 武汉理工大学 Monocular camera-based crane working space multi-information measuring method and system
CN110646016B (en) * 2019-11-11 2021-04-13 北京航空航天大学 Distributed POS calibration method and device based on theodolite and vision-assisted flexible base line
CN111024117B (en) * 2019-11-21 2023-03-14 中国航空工业集团公司西安飞行自动控制研究所 Vision-based inertial navigation system rapid alignment system and alignment method
CN111009012B (en) * 2019-11-29 2023-07-28 四川沃洛佳科技有限公司 Unmanned aerial vehicle speed measuring method based on computer vision, storage medium and terminal
CN111563294B (en) * 2020-04-23 2023-06-23 西北工业大学 Band gap-based optimization design method for curved bar periodic structure
CN113188566B (en) * 2021-03-23 2023-09-29 北京航空航天大学 Airborne distributed POS data fusion method

Family Cites Families (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102322873B (en) * 2011-08-23 2014-02-26 北京航空航天大学 Distributed POS ground demonstration verification system
CN103852760B (en) * 2012-12-04 2016-02-03 中国科学院电子学研究所 A kind of many base measurements method based on rigidity and flexible baseline combination
CN103913181B (en) * 2014-04-24 2017-03-29 北京航空航天大学 A kind of airborne distributed POS Transfer Alignments based on parameter identification
US9900511B2 (en) * 2015-04-10 2018-02-20 Freefly Systems, Inc. Method, system, and device for controlling a stabilized camera remotely
CN105652305B (en) * 2016-01-08 2018-02-06 深圳大学 The three-dimensional localization method for determining posture and system of a kind of dynamic environment lower railway detection platform
CN106352876B (en) * 2016-07-25 2018-05-25 北京航空航天大学 A kind of airborne distribution POS Transfer Alignments based on H ∞ and CKF mixed filterings
CN106289246B (en) * 2016-07-25 2018-06-12 北京航空航天大学 A kind of flexible link arm measure method based on position and orientation measurement system
CN106251305B (en) * 2016-07-29 2019-04-30 长春理工大学 A kind of realtime electronic image stabilizing method based on Inertial Measurement Unit IMU

Also Published As

Publication number Publication date
CN107728182A (en) 2018-02-23

Similar Documents

Publication Publication Date Title
CN107728182B (en) Flexible multi-baseline measurement method and device based on camera assistance
CN106289246B (en) A kind of flexible link arm measure method based on position and orientation measurement system
CN110501024B (en) Measurement error compensation method for vehicle-mounted INS/laser radar integrated navigation system
CN108387227B (en) Multi-node information fusion method and system of airborne distributed POS
CN108375383B (en) Multi-camera-assisted airborne distributed POS flexible baseline measurement method and device
CN103913181B (en) A kind of airborne distributed POS Transfer Alignments based on parameter identification
CN103994763B (en) The SINS/CNS deep integrated navigation system of a kind of Marsokhod and its implementation
Ahn et al. Fast alignment using rotation vector and adaptive Kalman filter
CN104567819B (en) A kind of star loaded camera full filed drift angle determines and compensation method
CN108845335A (en) Unmanned aerial vehicle ground target positioning method based on image and navigation information
CN108458709B (en) Airborne distributed POS data fusion method and device based on vision-aided measurement
CN106767673B (en) A kind of direction measurement method of satellite high-precision optical sensitive load
CN108398130B (en) Distributed POS transfer alignment modeling method and device of flexural deformation measurement network
CN104236586B (en) Moving base transfer alignment method based on measurement of misalignment angle
CN107764261B (en) Simulation data generation method and system for distributed POS (point of sale) transfer alignment
Lu et al. Applied quaternion optimization method in transfer alignment for airborne AHRS under large misalignment angle
CN104698486A (en) Real-time navigation method of data processing computer system for distributed POS
CN107765244B (en) InSAR baseline measurement method and device based on airborne double antennas
CN104215244B (en) Re-entry space vehicle integrated navigation robust filtering method based on launch inertial coordinate system
CN114608554B (en) Handheld SLAM equipment and robot instant positioning and mapping method
CN112325886A (en) Spacecraft autonomous attitude determination system based on combination of gravity gradiometer and gyroscope
CN106352897B (en) It is a kind of based on the silicon MEMS gyro estimation error of monocular vision sensor and bearing calibration
CN108917772A (en) Noncooperative target Relative Navigation method for estimating based on sequence image
CN111189442A (en) Multi-source navigation information state prediction method of unmanned aerial vehicle based on CEPF
CN108303120B (en) Real-time transfer alignment method and device for airborne distributed POS

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