CN109764880A - The vision inertia ranging method and system of close coupling vehicle wheel encoder data - Google Patents
The vision inertia ranging method and system of close coupling vehicle wheel encoder data Download PDFInfo
- Publication number
- CN109764880A CN109764880A CN201910127778.1A CN201910127778A CN109764880A CN 109764880 A CN109764880 A CN 109764880A CN 201910127778 A CN201910127778 A CN 201910127778A CN 109764880 A CN109764880 A CN 109764880A
- Authority
- CN
- China
- Prior art keywords
- sliding window
- imu
- image
- frame
- frame image
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 78
- 230000008878 coupling Effects 0.000 title claims abstract description 41
- 238000010168 coupling process Methods 0.000 title claims abstract description 41
- 238000005859 coupling reaction Methods 0.000 title claims abstract description 41
- 239000011159 matrix material Substances 0.000 claims abstract description 135
- 230000010354 integration Effects 0.000 claims abstract description 95
- 238000005457 optimization Methods 0.000 claims description 32
- 230000006870 function Effects 0.000 claims description 28
- 230000001133 acceleration Effects 0.000 claims description 15
- 238000005259 measurement Methods 0.000 claims description 14
- 230000005484 gravity Effects 0.000 claims description 9
- 238000004364 calculation method Methods 0.000 claims description 8
- 230000008569 process Effects 0.000 claims description 7
- 230000008859 change Effects 0.000 claims description 6
- 238000005070 sampling Methods 0.000 claims description 6
- 238000010276 construction Methods 0.000 claims description 3
- 239000000284 extract Substances 0.000 claims description 3
- 239000000203 mixture Substances 0.000 claims description 3
- 230000003287 optical effect Effects 0.000 claims description 3
- 230000002123 temporal effect Effects 0.000 claims description 3
- HOWHQWFXSLOJEF-MGZLOUMQSA-N systemin Chemical compound NCCCC[C@H](N)C(=O)N[C@@H](CCSC)C(=O)N[C@@H](CCC(N)=O)C(=O)N[C@@H]([C@@H](C)O)C(=O)N[C@@H](CC(O)=O)C(=O)OC(=O)[C@@H]1CCCN1C(=O)[C@H]1N(C(=O)[C@H](CC(O)=O)NC(=O)[C@H](CCCN=C(N)N)NC(=O)[C@H](CCCCN)NC(=O)[C@H](CO)NC(=O)[C@H]2N(CCC2)C(=O)[C@H]2N(CCC2)C(=O)[C@H](CCCCN)NC(=O)[C@H](CO)NC(=O)[C@H](CCC(N)=O)NC(=O)[C@@H](NC(=O)[C@H](C)N)C(C)C)CCC1 HOWHQWFXSLOJEF-MGZLOUMQSA-N 0.000 claims description 2
- 108010050014 systemin Proteins 0.000 claims description 2
- XCWPUUGSGHNIDZ-UHFFFAOYSA-N Oxypertine Chemical compound C1=2C=C(OC)C(OC)=CC=2NC(C)=C1CCN(CC1)CCN1C1=CC=CC=C1 XCWPUUGSGHNIDZ-UHFFFAOYSA-N 0.000 claims 1
- 238000011156 evaluation Methods 0.000 claims 1
- 230000004927 fusion Effects 0.000 description 4
- 230000000694 effects Effects 0.000 description 3
- 238000010586 diagram Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 230000006399 behavior Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
Landscapes
- Gyroscopes (AREA)
- Navigation (AREA)
- Image Analysis (AREA)
- Image Processing (AREA)
Abstract
The present invention relates to a kind of vision inertia ranging method and system of close coupling vehicle wheel encoder data, vision inertia ranging method includes: step 1: the init state that the vision inertia ranging method is arranged is " unfinished ", and during initialization；Step 2: the essential matrix between present image and previous frame image is calculated, the first sliding window and the second sliding window are added simultaneously by the essential matrix and with corresponding IMU measuring value and wheel encoder count；Step 3: IMU measuring value corresponding to last frame image and wheel encoder count carry out IMU- and take turns sub-encoders pre-integration；Whether the state for judging initialization is otherwise " being completed " if so then execute step five executes step 4；Step 4: parameter is initialized using pre-integration result；Step 5: the corresponding parameter of each image in the first sliding window is obtained；Step 6: the vision inertia ranging of close coupling vehicle wheel encoder data is carried out according to the parameter.
Description
Technical field
The present invention relates to the positioning and navigation fields of fusion multisensor, and in particular to a kind of close coupling vehicle wheel encoder
The vision inertia ranging method and system of data.
Background technique
Use the vision inertia ranging side of camera and IMU (Inertial Measurement Unit, Inertial Measurement Unit)
Method achieves good effect in recent years, because vision and the information of IMU have the characteristics that complementation.However, because the movement of vehicle
Form is more special, such as is easy to do approximate linear uniform motion, and the size estimation in vision inertia ranging method is not allowed sometimes
Really.On the other hand, wheel sub-encoders can provide the accurate information for determining scale, and its data are easily obtained.Cause
This, fusion camera, IMU and wheel sub-encoders are the wise selections of indoor vehicle positioning.
Several existing methods use respectively Extended Kalman filter and nonlinear optimization etc. it is different strategy fusion camera,
The information of IMU and wheel sub-encoders.These three methods require wheel encoder system and provide angular speed, although because having IMU
Middle gyroscope using above-mentioned angular speed not necessarily.On the one hand, in order to obtain above-mentioned angular speed, at least need to install two wheels
Sub-encoders.On the other hand, because the angular speed derived by wheel encoder is always in ground level, so in fact making
Vehicle is almost the hypothesis of plane motion between time upper two adjacent states.But this assume in vehicle climb and fall or
Large error is likely to result in when jolting.
Summary of the invention
In order to solve the above problem in the prior art, in order to reduce the error generated in vehicle travel process, this hair
It is bright that a kind of vision inertia ranging method and system of close coupling vehicle wheel encoder data are provided.
In order to solve the above technical problems, the present invention provides following schemes:
A kind of vision inertia ranging method of close coupling vehicle wheel encoder data, the vision inertia ranging method packet
It includes:
Step 1: the init state that the vision inertia ranging method is arranged is " unfinished ", and is initializing and surveying
During journey, camera image, Inertial Measurement Unit IMU measuring value and wheel encoder count are obtained in real time；
Step 2: calculating the essential matrix between present image and previous frame image, by the essential matrix and with it is right
The IMU measuring value and wheel encoder count while the first sliding window of addition answered and the second sliding window, while according to current
Whether image is that key frame adjusts accordingly first sliding window and the second sliding window, and the current image is the
Arbitrary image after one frame image；
Step 3: IMU corresponding to last frame image respective in first sliding window and the second sliding window
Measuring value and wheel encoder count carry out IMU- and take turns sub-encoders pre-integration；Whether the state for judging initialization is " complete
At ", if then successively executing step 5 and step 6, otherwise execute step 4；
Step 4: judge the picture number in the first sliding window whether less than the maximum figure accommodated in the first sliding window
As otherwise number continues to judge whether the parallax between the image in the second sliding window is less than setting if then step 4 terminates
Parallax threshold value otherwise continue to judge in the second sliding window whether is effective essential matrix number if then step 4 terminates
Less than the number threshold value of setting, if it is step 4 terminates, otherwise according in the second sliding window from the second frame image
IMU- wheel encodes in effective essential matrix and the second sliding window between each frame image and its previous frame image
Device pre-integration as a result, calibration IMU in gyroscope drift initial value；Using gyroscope drift to every in the second sliding window
It opens the corresponding IMU measuring value of image and wheel encoder count re-starts IMU- wheel sub-encoders pre-integration；Utilize pre-integration
As a result parameter is initialized；
Step 5: calculating in the first sliding window includes re-projection error item, IMU- wheel encoder errors item and side
Sensor pose, speed and offset and match point depth are iterated optimization together, obtained by the cost function of edge error term
The corresponding parameter of each image in the first sliding window；
Step 6: the vision inertia ranging of close coupling vehicle wheel encoder data is carried out according to the parameter.
Optionally, the parameter includes sensor pose, speed, offset and three-dimensional point depth.
Optionally, the real-time acquisition camera image, IMU measuring value and wheel encoder count, specifically include:
While obtaining present image, the IMU measuring value and wheel between the image and previous frame image acquisition moment are obtained
Sub-encoders reading, as the corresponding IMU measuring value of described image and wheel encoder count；
Wherein, described image, IMU measuring value and wheel encoder count have respective timestamp；So that wheel
During sub-encoders reading is sampled, the timestamp of sampling is consistent with the timestamp of IMU measuring value.
Optionally, step 2 specifically includes:
Obtain present image angle point: be directed to first frame image, using apply-Damiano Tommasi (Shi-Tomasi) method extract institute
State the angle point of first frame image；For other images in addition to first frame, previous frame image is appeared in using optical flow method tracking
On angle point；
Wherein, if some angle point of previous frame image can successfully track to obtain an angle on the present image
Point, then the two angle points form a pair of of match point between the present image and previous frame image；
Using all matching double points between the present image and previous frame image, random sampling unification algorism is used
RANSAC frame calculates the essential matrix between the present image and previous frame image using 5 methods；If the essence square
Battle array interior number be more than threshold value, then record the essential matrix, and decompose the essential matrix, obtain described image with it is previous
Spin matrix between frame image, while it is effective for recording the essential matrix validity；Otherwise it is invalid for recording its validity；
The present image is added the first sliding window and the second sliding window simultaneously, frame number is respectively k+1, m+1,
Terminate if k < K, otherwise judges that present image is added before window whether last frame image in the first sliding window is crucial
Frame；The present image in the first sliding window is abandoned if it is not key frame, while will be deserved in the first sliding window
The corresponding all IMU measuring values of preceding image and wheel encoder count are added to the present image being newly added in the first sliding window
In corresponding IMU measuring value and wheel encoder count, last frame picture numbers subtract 1 in the first sliding window；If crucial
Frame then carries out marginalisation to the corresponding parameter of one frame image of foremost in the first sliding window and operates to obtain marginalisation error term；
And described image and corresponding all IMU measuring values and wheel encoder count are abandoned, each frame number in the first sliding window
Subtract 1, all timestamps in the second sliding window are then less than to n of the 1st frame image temporal stamp in current first sliding window
Image and its corresponding IMU measuring value and wheel encoder count abandon, while each frame number subtracts n in the second sliding window；
Wherein, k and m is respectively the picture number in current first sliding window and the second sliding window, and K is the first sliding
The most picture numbers that can be accommodated in window；
To the corresponding IMU measuring value of last frame image and wheel encoder count in the first sliding window and right
The corresponding IMU measuring value of last frame image and wheel encoder count in the second sliding window carry out pre-integration operation.
Optionally, the progress pre-integration operation, specifically includes:
I successively takes 0,2 ..., and s-2 is successively calculated
Ji+1=AiJi
Value is Ji+1The 7th row to the 9th row, the 16th column to the 18th column constitute matrix；
Until final result∑s-1WithIt is computed；
Wherein, s indicates to participate in the IMU measuring value of pre-integration and the number of wheel encoder count,AndIt is the rotation indicated by quaternary number,Indicate that the multiplication of quaternary number, R () are indicated a quaternary
Number is converted into the function of its corresponding spin matrix,Indicate the 3-axis acceleration in i-th of IMU measuring value,Indicate i-th
Three axis angular rates in a IMU measuring value,Indicate the reading of i-th of wheel sub-encoders, baFor accelerometer in current IMU
Offset, bωFor the offset of gyroscope in current IMU, δtiIndicate the timestamp and i-th of IMU measuring value of i+1 IMU measuring value
Timestamp difference,It indicates from bodywork reference frame to the spin matrix of IMU coordinate system, with the vector in bodywork reference frame
The vector in IMU coordinate system is obtained, is obtained by off-line calibration；
Wherein, bodywork reference frame is defined as: using left rear wheel as origin, x-axis is directed toward off hind wheel, and y-axis is directed toward the near front wheel, z
The vertical chassis of axis is upward.
Optionally, step 4 specifically includes:
Judge the picture number in the first sliding window whether less than the maximum image number accommodated in the first sliding window
K, if then step 4 terminates；Otherwise it performs the next step；
For all images in the first sliding window, have if there is a frame image with last frame image in window super
M is crossed to match point, and the mean parallax of these matching double points is more than p pixel, and utilizes RANSAC using these matching double points
Frame calculates essential matrix using 5 methods, and having more than n is interior point to match point, is thened follow the steps in next step；Otherwise step 4
Terminate；
The validity of the corresponding essential matrix of all images in the second sliding window is judged for effective number, if it is less than
The threshold value r of effective essential matrix number, then step 4 terminates；Otherwise using in the second sliding window from the second frame image it is every
IMU- takes turns sub-encoders in effective essential matrix and the second sliding window between one frame image and its previous frame image
Pre-integration as a result, using least square method optimization computing gyroscope offset:
Wherein, bωFor gyroscope drift to be optimized, initial value is null vector；F is indicated in the second sliding window from the second frame
Image plays the effective frame number set of essential matrix between each frame image and its previous frame image, and m indicates the second sliding
Picture numbers in window,To decompose the corresponding quaternary number of spin matrix that essential matrix obtains, indicate from the second sliding window
Mouthful in m+1 frame image camera coordinates system to m frame image camera coordinates system rotation,It is to indicate from camera coordinates
It is the quaternary number to the rotation of IMU coordinate system, It can be obtained in advance by off-line calibration,It indicates
The multiplication of quaternary number；It is a rotation quaternary number, is gyroscope drift bωFunction, r indicates effective essence
The threshold value of matrix number, ‖ ‖2Indicate 2 norms of vector:
δbωIt is bωRenewal amount,WithIt is the m frame and m+1 frame figure in the second sliding window respectively
A part of the rotation quaternary number and Jacobian matrix of IMU- wheel sub-encoders pre-integration as between, with the second sliding window
Sliding, pre-integration obtains between arbitrary neighborhood two field pictures in windowWithIt is known；
For the corresponding IMU measuring value of each frame image and wheel encoder count in the second sliding window, successively weigh
Pre-integration operation is newly carried out, wherein gyroscope drift bωTo optimize obtained value；
Calculated pre-integration is as a result, successively calculate out of second sliding window first frame to the rotation of m frame
Matrix
Wherein m successively takes 1,2 ..., M, and M is the number of image in the second sliding window；For unit battle array；
For the continuous image of two frame every in the second sliding window, equationof structure
Wherein,WithFor m frame and m+1 frame figure in the second sliding window
The result of IMU- wheel sub-encoders pre-integration as between；For quaternary numberCorresponding spin matrix form, Δ
tmFor in the second sliding window between m+1 frame and m frame image timestamp difference；For spin matrix；For
It is the acceleration of gravity in window under the corresponding IMU coordinate system of first frame image, its initial value can be byIt calculates, g is the size of local gravitational acceleration；B isTangent plane on one group of orthogonal basis；For movement velocity of the corresponding position IMU of m frame image under the corresponding IMU coordinate system of m frame image,For m
Movement velocity of the corresponding position IMU of+1 frame image under the corresponding IMU coordinate system of m+1 frame image；Δ g is acceleration of gravity
Renewal amount；
The equation can be denoted as:
M successively takes 1,2 ... M-1, M is the number of image in the second sliding window, available M-1 equation, them
Simultaneous obtains error equation:
It is denoted as L=AX
The all elements that X includes are variable to be optimized, and all elements of L and A are known；
Solve minX(AX-L)T(AX-L), it is updated using the Δ g in X
Using updatedIts orthogonal basis B for cutting space is recalculated, and is utilized newlyL is recalculated with B
And A, repeat this step, iterative calculation；When the number of iterations reaches preset the number of iterations threshold value, step 4 terminates；Note
It records at this timeWith
For the corresponding IMU measuring value of each frame image and wheel encoder count in the first sliding window, successively weigh
It is new to carry out pre-integration operation；
Calculate a matrixSo thatUtilize the pre-integration result
WithK is picture numbers in the first sliding window, and being calculated using the method for dead reckoning is indicated out of first sliding window
The spin matrix of the corresponding IMU coordinate system of first frame image in the corresponding IMU coordinate system to the first sliding window of kth frame imageAnd the first corresponding IMU coordinate origin of the kth frame image first frame image in the first sliding window in sliding window
Position in corresponding IMU coordinate systemIn view of the first frame image of the first sliding window and the second sliding window is identical,
And first each frame image in sliding window can find image identical with it in the second sliding window, calculate IMU
The initial value of parameter to be optimized in error term:
Wherein, Q () indicates the function that a spin matrix is converted to corresponding quaternary number,For step
In S4006 in calculated second sliding window the corresponding position IMU of m (k) frame image in the corresponding IMU of m (k) frame image
Movement velocity under coordinate system, m (k) indicate the corresponding figure in the second sliding window of the kth frame image in the first sliding window
As serial number, bωFor gyroscope drift；
Utilize the pose of the corresponding IMU coordinate system of image in first sliding windowWithIt can calculate
Image pose in first sliding window out；It is carried out using the image pose in the first sliding window and the match point on image
Trigonometric ratio calculates, and obtains the depth of match point, using the depth as in parameter to be optimized needed for construction re-projection error item
The initial value of match point depth parameter；
The state that initialization is arranged is " being completed ".
Optionally, step 5 specifically includes:
According to the common practice based on the vision inertia ranging method of nonlinear optimization in sliding window in the first sliding window
Cost function is constructed in mouthful, and carries out nonlinear optimization solution, obtains sensor pose, speed and offset and match point depth
Estimated value；
It wherein, include re-projection error item, imu error item and marginalisation error term in the cost function；
The re-projection error item and the marginalisation error term use the vision based on nonlinear optimization in sliding window
The common practice of inertia ranging method is constructed；
The imu error item is constructed using following methods:
Wherein, CIMU(x) imu error item is indicated；X is by x1, x2..., xkComposition；K indicates the frame sequence of image in sliding window
Number；xkIt is the state vector of kth frame, ∑K, k+1The covariance matrix that pre-integration obtains in step S2006 is indicated, with first
The sliding of sliding window, the covariance matrix that pre-integration obtains between arbitrary neighborhood two field pictures in window are known；It indicates residual vector, has with the state vector of+1 frame of kth frame and kth and the IMU pre-integration between them
It closes:
WithThe position and speed of kth frame image exposure moment camera respectively under world coordinate system；WithThe position and speed of+1 exposed frame moment of kth camera respectively under world coordinate system；WithRespectively
The offset of accelerometer and gyroscope under k frame IMU body coordinate system；WithRespectively in+1 frame IMU sheet of kth
The offset of accelerometer and gyroscope under body coordinate system；Be with quaternary number indicate in the kth frame time of exposure from IMU ontology
Rotation of the coordinate system to world coordinate system；ForSpin matrix form；For with quaternary number table
The rotation at+1 frame image exposure moment of kth from IMU body coordinate system to world coordinate system shown；For
Spin matrix form；gwIt is a constant for the acceleration of gravity in world coordinate system；ΔtkIt is exposed for+1 frame of kth and kth frame
Light time carves the difference of timestamp；It is coordinate of the origin of bodywork reference frame in IMU coordinate system, passes through
It is calculated,It indicates from bodywork reference frame to the spin matrix of IMU coordinate system,Indicate the center of IMU in car body coordinate
Coordinate in system measures in advance； WithThe IMU- wheel between+1 frame image of kth frame and kth
Encoder pre-integration as a result, with the first sliding window sliding, pre-integration obtains between arbitrary neighborhood two field pictures in window
It arrivesWithIt is known；
With
It is parameter to be optimized.
To solve the above problems, the present invention also provides another technical solutions:
A kind of vision inertia ranging system of close coupling vehicle wheel encoder data, described includes vision inertia ranging system
System includes:
Initialization unit, the init state for the vision inertia ranging method to be arranged are " unfinished "；
Acquiring unit, for obtaining camera image, Inertial Measurement Unit IMU in real time during initialization and ranging
Measuring value and wheel encoder count；It is configured that a camera, real-time image acquisition；One IMU obtains IMU in real time and measures
Value；The equipment that the data of sub-encoders are taken turns on a set of acquisition vehicle, obtains wheel encoder count in real time；A set of timing device,
The timestamp under same time reference is provided for three kinds of data；
Camera attitude calculation unit, for calculating the essential matrix between present image and previous frame image, by described
Stromal matrix and the first sliding window and the second sliding window are added simultaneously with corresponding IMU measuring value and wheel encoder count
Mouthful, while whether being that key frame adjusts accordingly first sliding window and the second sliding window according to present image；
Judging unit, for corresponding to last frame image respective in first sliding window and the second sliding window
IMU measuring value and wheel encoder count carry out IMU- take turns sub-encoders pre-integration；Whether the state for judging initialization is "
It completes "；
Execution unit is judged, for being " unfinished " in the state that the judging result of first judging unit is initialization
When, judge the picture number in the first sliding window whether less than the maximum image number accommodated in the first sliding window, if
Then terminate, otherwise continue to judge whether the parallax between the image in the second sliding window is less than threshold value, if then terminating, otherwise after
It is continuous；If it is determined that whether effective essential matrix number is less than the number threshold value of setting in the second sliding window, if then terminating,
Otherwise according in the second sliding window from the second frame image it is effective between each frame image and its previous frame image
In essential matrix and the second sliding window IMU- take turns sub-encoders pre-integration as a result, gyroscope drift in calibration IMU
Initial value；Using gyroscope drift to the corresponding IMU measuring value of every image in the second sliding window and wheel encoder count
Re-start IMU- wheel sub-encoders pre-integration；Parameter is initialized using pre-integration result；
Optimize unit, for the judging result of first judging unit be initialization state be " being completed " when or
After the completion of judging execution unit initialization described in person, calculating in the first sliding window includes re-projection error item, IMU- wheel
The cost function of encoder errors item and marginalisation error term, by sensor pose, speed and offset and match point depth one
It rises and is iterated optimization, obtain the corresponding parameter of each image in the first sliding window；
Ranging unit, for carrying out the vision inertia ranging of close coupling vehicle wheel encoder data according to the parameter.
To solve the above problems, the present invention also provides another technical solutions:
A kind of vision inertia ranging system of close coupling vehicle wheel encoder data, comprising:
Processor；And
It is arranged to the memory of storage computer executable instructions, the executable instruction makes the place when executed
It manages device and executes following operation:
Step 1: the init state that the vision inertia ranging method is arranged is " unfinished ", and is initializing and surveying
During journey, camera image, Inertial Measurement Unit IMU measuring value and wheel encoder count are obtained in real time；
Step 2: calculating the essential matrix between present image and previous frame image, by the essential matrix and with it is right
The IMU measuring value and wheel encoder count while the first sliding window of addition answered and the second sliding window, while according to current
Whether image is that key frame adjusts accordingly first sliding window and the second sliding window, and the current image is the
Arbitrary image after one frame image；
Step 3: IMU corresponding to last frame image respective in first sliding window and the second sliding window
Measuring value and wheel encoder count carry out IMU- and take turns sub-encoders pre-integration；Whether the state for judging initialization is " complete
At ", if so then execute step five, otherwise execute step 4；
Step 4: judge the picture number in the first sliding window whether less than the maximum figure accommodated in the first sliding window
As otherwise number continues to judge whether the parallax between the image in the second sliding window is less than setting if then step 4 terminates
Parallax threshold value otherwise continue to judge in the second sliding window whether is effective essential matrix number if then step 4 terminates
Less than the number threshold value of setting, if it is step 4 terminates, otherwise according in the second sliding window from the second frame image
IMU- wheel encodes in effective essential matrix and the second sliding window between each frame image and its previous frame image
Device pre-integration as a result, calibration IMU in gyroscope drift initial value；Using gyroscope drift to every in the second sliding window
It opens the corresponding IMU measuring value of image and wheel encoder count re-starts IMU- wheel sub-encoders pre-integration；Utilize pre-integration
As a result parameter is initialized；
Step 5: calculating in the first sliding window includes re-projection error item, IMU- wheel encoder errors item and side
Sensor pose, speed and offset and match point depth are iterated optimization together, obtained by the cost function of edge error term
The corresponding parameter of each image in the first sliding window；
Step 6: the vision inertia ranging of close coupling vehicle wheel encoder data is carried out according to the parameter.
To solve the above problems, the present invention also provides another technical solutions:
A kind of computer readable storage medium, the computer-readable recording medium storage one or more program are described
One or more programs are when the electronic equipment for being included multiple application programs executes, so that the electronic equipment executes following behaviour
Make:
Step 1: the init state that the vision inertia ranging method is arranged is " unfinished ", and is initializing and surveying
During journey, camera image, Inertial Measurement Unit IMU measuring value and wheel encoder count are obtained in real time；
Step 2: calculating the essential matrix between present image and previous frame image, by the essential matrix and with it is right
The IMU measuring value and wheel encoder count while the first sliding window of addition answered and the second sliding window, while according to current
Whether image is that key frame adjusts accordingly first sliding window and the second sliding window, and the current image is the
Arbitrary image after one frame image；
Step 3: IMU corresponding to last frame image respective in first sliding window and the second sliding window
Measuring value and wheel encoder count carry out IMU- and take turns sub-encoders pre-integration；Whether the state for judging initialization is " complete
At ", if so then execute step five, otherwise execute step 4；
Step 4: judge the picture number in the first sliding window whether less than the maximum figure accommodated in the first sliding window
As otherwise number continues to judge whether the parallax between the image in the second sliding window is less than setting if then step 4 terminates
Parallax threshold value otherwise continue to judge in the second sliding window whether is effective essential matrix number if then step 4 terminates
Less than the number threshold value of setting, if it is step 4 terminates, otherwise according in the second sliding window from the second frame image
IMU- wheel encodes in effective essential matrix and the second sliding window between each frame image and its previous frame image
Device pre-integration as a result, calibration IMU in gyroscope drift initial value；Using gyroscope drift to every in the second sliding window
It opens the corresponding IMU measuring value of image and wheel encoder count re-starts IMU- wheel sub-encoders pre-integration；Utilize pre-integration
As a result parameter is initialized；
Step 5: calculating in the first sliding window includes re-projection error item, IMU- wheel encoder errors item and side
Sensor pose, speed and offset and match point depth are iterated optimization together, obtained by the cost function of edge error term
The corresponding parameter of each image in the first sliding window；
Step 6: the vision inertia ranging of close coupling vehicle wheel encoder data is carried out according to the parameter.
According to an embodiment of the invention, the invention discloses following technical effects:
The present invention is encoded using image, the accelerometer data in IMU and the gyro data and wheel of monocular camera
Device data have merged IMU and wheel encoder data in the pre-integration stage, to carry out accurate ruler in the nonlinear optimization stage
The freedom degree camera of degree tracks, so as to reduce the error generated in vehicle travel process.
Detailed description of the invention
Fig. 1 is the flow chart of the vision inertia ranging method of close coupling vehicle wheel encoder data of the present invention；
Fig. 2 is data-optimized comparison diagram；
Fig. 3 is the modular structure signal of the vision inertia ranging system of close coupling vehicle wheel encoder data of the present invention
Figure.
Symbol description:
Acquiring unit -1, camera Attitude Calculation module -2, judging unit -3 optimize unit -4, ranging unit -5.
Specific embodiment
The preferred embodiment of the present invention described with reference to the accompanying drawings.It will be apparent to a skilled person that this
A little embodiments are used only for explaining technical principle of the invention, it is not intended that limit the scope of the invention.
The present invention provides a kind of vision inertia ranging method of close coupling vehicle wheel encoder data, utilizes monocular camera
Image, the accelerometer data in IMU and gyro data and wheel encoder data, merged in the pre-integration stage
IMU and wheel encoder data, thus have the freedom degree camera of accurate scale to track in the nonlinear optimization stage, so as to
Reduce the error generated in vehicle travel process.
In order to make the foregoing objectives, features and advantages of the present invention clearer and more comprehensible, with reference to the accompanying drawing and specific real
Applying mode, the present invention is described in further detail.
The vision inertia ranging method of close coupling vehicle wheel encoder data of the present invention includes:
Step 1: the init state that the vision inertia ranging method is arranged is " unfinished ", and in the process of initialization
In, camera image, Inertial Measurement Unit IMU measuring value and wheel encoder count are obtained in real time；
Step 2: calculate the essential matrix between present image and previous frame image, by the essential matrix and with it is corresponding
IMU measuring value and wheel encoder count the first sliding window and the second sliding window are added simultaneously, while according to current figure
Seem it is no first sliding window and the second sliding window are adjusted accordingly for key frame, the current image is first
Arbitrary image after frame image；
Step 3: IMU amount corresponding to last frame image respective in first sliding window and the second sliding window
Measured value and wheel encoder count carry out IMU- and take turns sub-encoders pre-integration；Whether the state for judging initialization is " being completed ",
If so then execute step five, step 4 is otherwise executed；
Step 4: judging the picture number in the first sliding window whether less than the maximum figure accommodated in the first sliding window
As number, if then step 4 terminates, otherwise continue to judge whether the parallax between the image in the second sliding window is less than setting
Otherwise parallax threshold value continues to judge whether effective essential matrix number is less than in the second sliding window if then step 4 terminates
The number threshold value of setting, if it is step 4 terminates, otherwise according to each frame from the second frame image in the second sliding window
IMU- takes turns the pre- product of sub-encoders in effective essential matrix and the second sliding window between image and its previous frame image
Point as a result, calibration IMU in gyroscope drift initial value；Using gyroscope drift to every image in the second sliding window
Corresponding IMU measuring value and wheel encoder count re-start IMU- wheel sub-encoders pre-integration；Utilize pre-integration result pair
Parameter is initialized；
Step 5: calculating in the first sliding window includes re-projection error item, IMU- wheel encoder errors item and edge
The cost function for changing error term, is iterated optimization for sensor pose, speed and offset and match point depth together, obtains
The corresponding parameter of each image in first sliding window；
Step 6: the vision inertia ranging of close coupling vehicle wheel encoder data is carried out according to the parameter.
Preferably, before Optimal Parameters, generally each parameter is initialized.
Below with specific embodiment explanation:
As shown in Figure 1, the vision inertia ranging method of close coupling vehicle wheel encoder data of the present invention includes:
Step S1000, the state that the initialization of this odometry is arranged is " unfinished "；
Step S2000 obtains camera image, IMU measuring value and wheel encoder count in real time；One frame image of every acquisition,
Its essential matrix between previous frame image is calculated, by itself and corresponding IMU measuring value and wheel encoder count
Be added the first sliding window and the second sliding window simultaneously, at the same according to it whether be key frame to above-mentioned two sliding window into
The corresponding adjustment of row；To the corresponding IMU measuring value of last frame image respective in two sliding windows and wheel encoder count
It carries out IMU- and takes turns sub-encoders pre-integration；
Step S3000, whenever the corresponding IMU measuring value of last frame image respective in two windows and wheel sub-encoders
When the IMU- wheel sub-encoders pre-integration of reading is all completed, judge whether the state of the initialization of this odometry is " being completed ",
It is no to then follow the steps S4000 if so then execute step S5000；
Step S4000 judges the picture number in the first sliding window, if it in the first sliding window less than accommodating most
Then this step terminates big picture number, otherwise continues；The parallax between the image in the second sliding window is judged, if it is less than threshold value
Then this step terminates, and otherwise continues；This step knot if essential matrix number effective in the second sliding window is less than threshold value
Otherwise beam continues；According in the second sliding window from the second frame image between each frame image and its previous frame image
Effective essential matrix and the second sliding window in IMU- wheel sub-encoders pre-integration as a result, calibration IMU in gyro
The initial value of instrument offset；Using above-mentioned gyroscope drift to the corresponding IMU measuring value of every image in the second sliding window and wheel
Sub-encoders reading re-starts IMU- wheel sub-encoders pre-integration；It is sensed using above-mentioned pre-integration result to needed for this odometry
The parameter initializations such as device pose, speed, offset and three-dimensional point depth, and the state that the initialization of this odometry is arranged is "
It completes "；
Step S5000, in the first sliding window calculate comprising re-projection error item, IMU- wheel encoder errors item and
Sensor pose, speed and offset and match point depth are iterated optimization by the cost function of marginalisation error term together,
To obtain the corresponding sensor pose of each image, speed and offset and match point depth in the first sliding window.
Preferably, step S2000 is specifically included:
Step S2001, obtains camera image in real time；While obtaining a frame image, while obtaining the image and former frame
Image obtains the IMU measuring value and wheel encoder count between the moment, as the corresponding IMU measuring value of described image and wheel
Encoder count；
Wherein, described image, IMU measuring value and wheel encoder count have respective timestamp；
The acquisition frequency of IMU measuring value and wheel encoder count is all remarkably higher than the acquisition frequency of image.
Step S2002 samples wheel encoder count, the timestamp of the timestamp after sampling and IMU measuring value
Unanimously.
Step S2003, for the image that a frame is got, using apply-Damiano Tommasi (Shi-Tomasi) method extracts angle point；
If the image is not the first frame image obtained since the starting of this odometry, former frame is appeared in using optical flow method tracking
Angle point on image, and using the angle point in described two sources as the angle point appeared on this image；
Wherein, if some angle point of previous frame image can successfully track to obtain an angle point on this image,
A pair of of match point between the two angle point shape cost images and previous frame image.
Step S2004, if this image described in step S2003 is not the first frame figure obtained since the starting of this odometry
Picture, then being used random sampling unification algorism (RANSAC) using all matching double points between this image and previous frame image
Frame calculates the essential matrix between this image and previous frame image using 5 methods；If meeting the interior point of the essential matrix
Number is more than threshold value, then recording the essential matrix, and decomposes it and obtains the spin moment between this image and previous frame image
Battle array, while it is effective for recording the essential matrix validity；Otherwise it is invalid for recording its validity；If institute in step S2003
Stating this image is the first frame image obtained since the starting of this odometry, and it is invalid for recording the validity of the essential matrix.
Described image is added the first sliding window simultaneously and the second sliding window, frame number is respectively k+ by step S2005
1, m+1, this step terminates if k < K, otherwise judges that image is added before window that last frame image is in the first sliding window
No is key frame；The image in the first sliding window is abandoned if it is not key frame, while will be in the first sliding window
It is corresponding that the corresponding all IMU measuring values of the image and wheel encoder count are added to the image being newly added in the first sliding window
IMU measuring value and wheel encoder count in, last frame picture numbers subtract 1 in the first sliding window；If it is key frame,
It then carries out marginalisation to the corresponding parameter of one frame image of foremost in the first sliding window to operate to obtain marginalisation error term, and will
Its all IMU measuring value corresponding with it and wheel encoder count abandon, and each frame number subtracts 1 in the first sliding window, then
By all timestamps in the second sliding window be less than in current first sliding window the n images of the 1st frame image temporal stamp and its
Corresponding IMU measuring value and wheel encoder count abandon, while each frame number subtracts n in the second sliding window.
Wherein, k and m is respectively the picture number in current first sliding window and the second sliding window, and K is the first sliding
The most picture numbers that can be accommodated in window.In the present embodiment, K=10.
Last frame of the present invention and one frame of foremost respectively refer to newest and oldest one of timestamp in sliding window
Frame image.
Step S2006, for the corresponding IMU measuring value of last frame image and wheel sub-encoders in the first sliding window
Reading and the corresponding IMU measuring value of last frame image and wheel encoder count in the second sliding window carry out pre-integration
Operation.
Preferably, the progress pre-integration operation, specifically includes:
I successively takes 0,2 ..., and s-2 is successively calculated
Ji+1=AiJi；
Value is Ji+1The 7th row to the 9th row, the 16th column to the 18th column constitute matrix；
Until final result∑s-1WithIt is computed；
Wherein, s indicates to participate in the IMU measuring value of pre-integration and the number of wheel encoder count,AndIt is the rotation indicated by quaternary number,Indicate that the multiplication of quaternary number, R () are indicated a quaternary
Number is converted into the function of its corresponding spin matrix,Indicate the 3-axis acceleration in i-th of IMU measuring value,Indicate i-th
Three axis angular rates in a IMU measuring value,Indicate the reading of i-th of wheel sub-encoders, baFor accelerometer in current IMU
Offset, bωFor the offset of gyroscope in current IMU, δ tiThe timestamp and i-th of IMU for indicating i+1 IMU measuring value measure
The difference of the timestamp of value,It indicates from bodywork reference frame to the spin matrix of IMU coordinate system, with its premultiplication bodywork reference frame
In vector obtain the vector in IMU coordinate system, can be obtained by off-line calibration；
Bodywork reference frame is defined as: using left rear wheel as origin, x-axis is directed toward off hind wheel, and y-axis is directed toward the near front wheel, and z-axis is vertical
Chassis is upward.
Step S4000 is specifically included:
Step S4001 judges the picture number in the first sliding window, if it in the first sliding window less than accommodating most
Big picture number K then end step S4000；Otherwise step S4000 terminates；
Step S4002, for all images in the first sliding window, if there is in a frame image and window last
Frame image has more than m to match point, and the mean parallax of these matching double points is more than p pixel, and uses these matching double points
Using RANSAC frame, essential matrix is calculated using 5 methods, having more than n is interior point to match point, is thened follow the steps in next step；
Otherwise step S4000；
Step S4003 judges the validity of the corresponding essential matrix of all images in the second sliding window for effective
Number, if its threshold value r for being less than effective essential matrix number, terminates；Otherwise it utilizes in the second sliding window from the second frame figure
As playing IMU- wheel in the effective essential matrix and the second sliding window between each frame image and its previous frame image
Encoder pre-integration as a result, using least square method optimization computing gyroscope offset:
Wherein, bωFor gyroscope drift to be optimized, initial value is null vector；F is indicated in the second sliding window from the second frame
Image plays the effective frame number set of essential matrix between each frame image and its previous frame image, and m indicates the second sliding
Picture numbers in window,To decompose the corresponding quaternary number of spin matrix that essential matrix obtains, indicate from the second sliding window
Mouthful in m+1 frame image camera coordinates system to m frame image camera coordinates system rotation,It is to indicate from camera coordinates
It is the quaternary number to the rotation of IMU coordinate system, It can be obtained in advance by off-line calibration,Indicate four
The multiplication of first number；It is a rotation quaternary number, it is gyroscope drift bωFunction:
δbωIt is bωRenewal amount,WithIt is the m frame and m+1 frame figure in the second sliding window respectively
A part of the rotation quaternary number and Jacobian matrix of IMU- wheel sub-encoders pre-integration as between, with the second sliding window
Sliding, pre-integration obtains between arbitrary neighborhood two field pictures in windowWithIt is known；
Step S4004 for the corresponding IMU measuring value of each frame image in the second sliding window and takes turns sub-encoders reading
Number successively re-starts pre-integration operation, wherein gyroscope drift bωTo optimize obtained value.
Step S4005, calculated pre-integration is as a result, successively calculate out of second sliding window first frame to
The spin matrix of m frame
Wherein m successively takes 1,2 ..., M, and M is the number of image in the second sliding window；For unit battle array；
Step S4006, for the continuous image of two frame every in the second sliding window, equationof structure
Wherein,WithFor m frame and m+1 frame figure in the second sliding window
The result of IMU- wheel sub-encoders pre-integration as between；For quaternary numberCorresponding spin matrix form, Δ
tmFor in the second sliding window between m+1 frame and m frame image timestamp difference；For spin matrix；For
It is the acceleration of gravity in window under the corresponding IMU coordinate system of first frame image, its initial value can be byIt calculates, g is the size of local gravitational acceleration；B isTangent plane on one group of orthogonal basis；For movement velocity of the corresponding position IMU of m frame image under the corresponding IMU coordinate system of m frame image,For m
Movement velocity of the corresponding position IMU of+1 frame image under the corresponding IMU coordinate system of m+1 frame image；Δ g is acceleration of gravity
Renewal amount；
The equation can be denoted as
M successively takes 1,2 ... M-1, M is the number of image in the second sliding window, available M-1 equation, them
Simultaneous obtains error equation:
It is denoted as L=AX
The all elements that X includes are variable to be optimized, and all elements of L and A are known；
Solve minX(AX-L)T(AX-L), it is updated using the Δ g in X
Using updatedIts orthogonal basis B for cutting space is recalculated, and is utilized newlyL is recalculated with B
And A, repeat this step, iterative calculation；When the number of iterations reaches preset the number of iterations threshold value, step S4000 terminates；
It records at this timeWith
Step S4007 for the corresponding IMU measuring value of each frame image in the first sliding window and takes turns sub-encoders reading
Number successively re-starts pre-integration operation, wherein gyroscope drift bωUse the value optimized in step S4003；
Step S4008 calculates a matrixSo thatUtilize the pre-integration
As a resultWithK is picture numbers in the first sliding window, and being calculated using the method for dead reckoning indicates sliding from first
The corresponding IMU coordinate system of first frame image in the corresponding IMU coordinate system to the first sliding window of kth frame image in dynamic window
Spin matrixAnd first the corresponding IMU coordinate origin of kth frame image in sliding window in the first sliding window
Position in the corresponding IMU coordinate system of one frame imageIn view of the first frame of the first sliding window and the second sliding window
Image is identical, and each frame image in the first sliding window can find figure identical with it in the second sliding window
Picture calculates the initial value of parameter to be optimized in imu error item:
Wherein, Q () indicates the function that a spin matrix is converted to corresponding quaternary number,For step
In S4006 in calculated second sliding window the corresponding position IMU of m (k) frame image in the corresponding IMU of m (k) frame image
Movement velocity under coordinate system, m (k) indicate the corresponding figure in the second sliding window of the kth frame image in the first sliding window
As serial number, bωFor calculated gyroscope drift；
Utilize the pose of the corresponding IMU coordinate system of image in first sliding windowWithIt can calculate
Image pose in first sliding window out；It is carried out using the image pose in the first sliding window and the match point on image
Trigonometric ratio calculates, and obtains the depth of match point, using the depth as in parameter to be optimized needed for construction re-projection error item
The initial value of match point depth parameter.
Step S4009, the state that the initialization of this odometry is arranged is " being completed ".
In the present embodiment, m=20, p=14, n=10, r=3, c=4.
Further, step S5000 is specifically included:
According to the common practice based on the vision inertia ranging method of nonlinear optimization in sliding window in the first sliding window
Mouthful in construct cost function, using Dogleg algorithm carry out nonlinear optimization solution, obtain sensor pose, speed and offset with
And the estimated value of match point depth.
It wherein, include re-projection error item, imu error item and marginalisation error term in the cost function；
The re-projection error item and the marginalisation error term use the vision based on nonlinear optimization in sliding window
The common practice of inertia ranging method is constructed；
The imu error item is constructed using following methods:
Wherein, CIMU(x) imu error item is indicated；X is by x1, x2..., xkComposition；K indicates the frame sequence of image in sliding window
Number；xkIt is the state vector of kth frame, ∑K, k+1The covariance matrix that pre-integration obtains in step S2006 is indicated, with first
The sliding of sliding window, the covariance matrix that pre-integration obtains between arbitrary neighborhood two field pictures in window are known.It indicates residual vector, has with the state vector of+1 frame of kth frame and kth and the IMU pre-integration between them
It closes:
WithThe position and speed of kth frame image exposure moment camera respectively under world coordinate system；WithThe position and speed of+1 exposed frame moment of kth camera respectively under world coordinate system；WithRespectively
The offset of accelerometer and gyroscope under k frame IMU body coordinate system；WithRespectively in+1 frame IMU sheet of kth
The offset of accelerometer and gyroscope under body coordinate system；Be with quaternary number indicate in the kth frame time of exposure from IMU ontology
Rotation of the coordinate system to world coordinate system；ForSpin matrix form；For with quaternary number table
The rotation at+1 frame image exposure moment of kth from IMU body coordinate system to world coordinate system shown；For
Spin matrix form；gwIt is a constant for the acceleration of gravity in world coordinate system；ΔtkIt is exposed for+1 frame of kth and kth frame
Light time carves the difference of timestamp；It is coordinate of the origin of bodywork reference frame in IMU coordinate system, it can pass throughIt is calculated,It indicates coordinate of the center of IMU in bodywork reference frame, can measure in advance.
WithIMU- takes turns the pre- product of sub-encoders between+1 frame image of kth frame and kth
Point as a result, with the first sliding window sliding, pre-integration obtains between arbitrary neighborhood two field pictures in windowWithIt is known；WithIt is to excellent
The parameter of change.
Table 1 indicates initialization module and existing the initialization module vehicle driving before initializing successfully of the invention
Comparison of the distance in two sets of data.As can be seen that using the initialization module of the invention vehicle row before initializing successfully
The distance sailed is considerably shorter, illustrates the initialization speed of this method faster.
Comparison of the distance of vehicle driving in two sets of data before table 1 initializes successfully
| The present invention | [5] initialization module in | |
| Data 1 | 2.01m | 56.6m |
| Data 2 | 1.13m | 29.9m |
(a) is partially in Fig. 2 and (b) is partially that vision inertia ranging method of the present invention is used respectively described in step S4000
After method and existing initialization module are initialized, preceding 100 frame image successively carries out sensor position described in step S5000
When appearance optimizes, the target function value comparison diagram after optimization.
As can be seen that using the initialization module of this method in two sets of data, it is excellent that preceding 100 frame image carries out the pose
When change, the target function value after optimization is smaller, because of target function value Representative errors, this shows the initial of this method
It is more acurrate to change result.
The present invention relates to the positioning and navigation fields of fusion multisensor, and in particular to a kind of close coupling vehicle wheel encoder
The vision inertia odometry and equipment of data can be improved initialization speed and precision and improve the accuracy of track.
In addition, the present invention also provides a kind of vision inertia ranging systems of close coupling vehicle wheel encoder data.Such as Fig. 3
Shown, the vision inertia ranging system of close coupling vehicle wheel encoder data of the present invention includes initialization unit 1, acquiring unit
2, camera attitude calculation unit 3, judging unit 4, judge execution unit 5, optimization unit 6 and ranging unit 7.
Specifically, it is " not complete that the initialization unit 1, which is used to be arranged the init state of the vision inertia ranging method,
At "；
The acquiring unit 2 is used to obtain camera image, Inertial Measurement Unit in real time during initialization and ranging
IMU measuring value and wheel encoder count；It is configured that a camera, real-time image acquisition；One IMU obtains IMU amount in real time
Measured value；The equipment that the data of sub-encoders are taken turns on a set of acquisition vehicle, obtains wheel encoder count in real time；A set of timing is set
It is standby, the timestamp under same time reference is provided for three kinds of data；
The camera attitude calculation unit 3 is used to calculate the essential matrix between present image and previous frame image, by institute
It states essential matrix and the first sliding window and the second sliding is added simultaneously with corresponding IMU measuring value and wheel encoder count
Window, while whether being that key frame accordingly adjusts first sliding window and the second sliding window according to present image
It is whole；
The judging unit 4 is used for respective last frame image in first sliding window and the second sliding window
Corresponding IMU measuring value and wheel encoder count carry out IMU- and take turns sub-encoders pre-integration；Whether judge the state initialized
For " being completed "；
It is described to judge that execution unit 5 is used in the state that the judging result of first judging unit is initialization as " not
Complete " when, judge the picture number in the first sliding window whether less than the maximum image number accommodated in the first sliding window,
If then terminating, otherwise continue to judge whether the parallax between the image in the second sliding window is less than threshold value, it is no if then terminating
Then continue；If it is determined that whether effective essential matrix number is less than the number threshold value of setting in the second sliding window, if then
Terminate, otherwise according to the having between each frame image and its previous frame image from the second frame image in the second sliding window
In the essential matrix of effect and the second sliding window IMU- take turns sub-encoders pre-integration as a result, calibration IMU in gyroscope it is inclined
The initial value of shifting；Using gyroscope drift to the corresponding IMU measuring value of every image in the second sliding window and wheel sub-encoders
Reading re-starts IMU- wheel sub-encoders pre-integration；Parameter is initialized using pre-integration result；
The optimization unit 6 is used in the state that the judging result of first judging unit is initialization be " being completed "
When or the judgement execution unit initialization after the completion of, in the first sliding window calculate include re-projection error item, IMU-
The cost function of wheel encoder errors item and marginalisation error term, sensor pose, speed and offset and match point is deep
Degree is iterated optimization together, obtains the corresponding parameter of each image in the first sliding window；
The vision inertia that the ranging unit 7 is used to carry out close coupling vehicle wheel encoder data according to the parameter is surveyed
Journey.
Further, the present invention also provides a kind of vision inertia ranging system of close coupling vehicle wheel encoder data,
Include:
Processor；And
It is arranged to the memory of storage computer executable instructions, the executable instruction makes the place when executed
It manages device and executes following operation:
Step 1: the init state that the vision inertia ranging method is arranged is " unfinished ", and in initialization and ranging
During, camera image, Inertial Measurement Unit IMU measuring value and wheel encoder count are obtained in real time；
Step 2: calculate the essential matrix between present image and previous frame image, by the essential matrix and with it is corresponding
IMU measuring value and wheel encoder count the first sliding window and the second sliding window are added simultaneously, while according to current figure
Seem it is no first sliding window and the second sliding window are adjusted accordingly for key frame, the current image is first
Arbitrary image after frame image；
Step 3: IMU amount corresponding to last frame image respective in first sliding window and the second sliding window
Measured value and wheel encoder count carry out IMU- and take turns sub-encoders pre-integration；Whether the state for judging initialization is " being completed ",
It is no to then follow the steps 4 if so then execute step 5；
Step 4: judging the picture number in the first sliding window whether less than the maximum figure accommodated in the first sliding window
As number, if then step 4 terminates, otherwise continue to judge whether the parallax between the image in the second sliding window is less than setting
Otherwise parallax threshold value continues to judge whether effective essential matrix number is less than in the second sliding window if then step 4 terminates
The number threshold value of setting, if it is step 4 terminates, otherwise according to each frame from the second frame image in the second sliding window
IMU- takes turns the pre- product of sub-encoders in effective essential matrix and the second sliding window between image and its previous frame image
Point as a result, calibration IMU in gyroscope drift initial value；Using gyroscope drift to every image in the second sliding window
Corresponding IMU measuring value and wheel encoder count re-start IMU- wheel sub-encoders pre-integration；Utilize pre-integration result pair
Parameter is initialized；
Step 5: calculating in the first sliding window includes re-projection error item, IMU- wheel encoder errors item and edge
The cost function for changing error term, is iterated optimization for sensor pose, speed and offset and match point depth together, obtains
The corresponding parameter of each image in first sliding window；
Step 6: the vision inertia ranging of close coupling vehicle wheel encoder data is carried out according to the parameter.
Further, the present invention also provides a kind of computer readable storage medium, the computer readable storage medium is deposited
Store up one or more programs, one or more of programs when the electronic equipment for being included multiple application programs executes so that
The electronic equipment executes following operation:
Step 1: the init state that the vision inertia ranging method is arranged is " unfinished ", and in initialization and ranging
During, camera image, Inertial Measurement Unit IMU measuring value and wheel encoder count are obtained in real time；
Step 2: calculate the essential matrix between present image and previous frame image, by the essential matrix and with it is corresponding
IMU measuring value and wheel encoder count the first sliding window and the second sliding window are added simultaneously, while according to current figure
Seem it is no first sliding window and the second sliding window are adjusted accordingly for key frame, the current image is first
Arbitrary image after frame image；
Step 3: IMU amount corresponding to last frame image respective in first sliding window and the second sliding window
Measured value and wheel encoder count carry out IMU- and take turns sub-encoders pre-integration；Whether the state for judging initialization is " being completed ",
It is no to then follow the steps 4 if so then execute step 5；
Step 4: judging the picture number in the first sliding window whether less than the maximum figure accommodated in the first sliding window
As number, if then step 4 terminates, otherwise continue to judge whether the parallax between the image in the second sliding window is less than setting
Otherwise parallax threshold value continues to judge whether effective essential matrix number is less than in the second sliding window if then step 4 terminates
The number threshold value of setting, if it is step 4 terminates, otherwise according to each frame from the second frame image in the second sliding window
IMU- takes turns the pre- product of sub-encoders in effective essential matrix and the second sliding window between image and its previous frame image
Point as a result, calibration IMU in gyroscope drift initial value；Using gyroscope drift to every image in the second sliding window
Corresponding IMU measuring value and wheel encoder count re-start IMU- wheel sub-encoders pre-integration；Utilize pre-integration result pair
Parameter is initialized；
Step 5: calculating in the first sliding window includes re-projection error item, IMU- wheel encoder errors item and edge
The cost function for changing error term, is iterated optimization for sensor pose, speed and offset and match point depth together, obtains
The corresponding parameter of each image in first sliding window；
Step 6: the vision inertia ranging of close coupling vehicle wheel encoder data is carried out according to the parameter.
Compared with the existing technology, computer readable storage medium of the present invention, close coupling vehicle wheel encoder data view
Feel that inertia ranging system is identical as the beneficial effect of vision inertia ranging method of above-mentioned close coupling vehicle wheel encoder data,
Details are not described herein.
So far, it has been combined preferred embodiment shown in the drawings and describes technical solution of the present invention, still, this field
Technical staff is it is easily understood that protection scope of the present invention is expressly not limited to these specific embodiments.Without departing from this
Under the premise of the principle of invention, those skilled in the art can make equivalent change or replacement to the relevant technologies feature, these
Technical solution after change or replacement will fall within the scope of protection of the present invention.
Claims (10)
1. a kind of vision inertia ranging method of close coupling vehicle wheel encoder data, which is characterized in that the vision inertia
Ranging method includes:
Step 1: the init state that the vision inertia ranging method is arranged is " unfinished ", and in initialization and ranging
In the process, camera image, Inertial Measurement Unit IMU measuring value and wheel encoder count are obtained in real time；
Step 2: calculating the essential matrix between present image and previous frame image, by the essential matrix and with it is corresponding
The first sliding window and the second sliding window is added in IMU measuring value and wheel encoder count simultaneously, while according to present image
It whether is that key frame adjusts accordingly first sliding window and the second sliding window, the current image is first frame
Arbitrary image after image；
Step 3: IMU corresponding to last frame image respective in first sliding window and the second sliding window is measured
Value and wheel encoder count carry out IMU- and take turns sub-encoders pre-integration；Whether the state for judging initialization is " being completed ", if
It is to then follow the steps five, otherwise executes step 4；
Step 4: judge the picture number in the first sliding window whether less than the maximum image number accommodated in the first sliding window
Otherwise mesh continues to judge the view whether parallax between the image in the second sliding window is less than setting if then step 4 terminates
Otherwise poor threshold value continues to judge whether effective essential matrix number is less than in the second sliding window if then step 4 terminates
The number threshold value of setting, if it is step 4 terminates, otherwise according in the second sliding window from the second frame image it is each
It is pre- to take turns sub-encoders by IMU- in effective essential matrix and the second sliding window between frame image and its previous frame image
Integral as a result, calibration IMU in gyroscope drift initial value；Using gyroscope drift to every figure in the second sliding window
As corresponding IMU measuring value and wheel encoder count re-start IMU- wheel sub-encoders pre-integration；Utilize pre-integration result
Parameter is initialized；
Step 5: calculating in the first sliding window includes re-projection error item, IMU- wheel encoder errors item and marginalisation
Sensor pose, speed and offset and match point depth are iterated optimization by the cost function of error term together, obtain
The corresponding parameter of each image in one sliding window；
Step 6: the vision inertia ranging of close coupling vehicle wheel encoder data is carried out according to the parameter.
2. the vision inertia ranging method of close coupling vehicle wheel encoder data according to claim 1, feature exist
In the parameter includes sensor pose, speed, offset and three-dimensional point depth.
3. the vision inertia ranging method of close coupling vehicle wheel encoder data according to claim 1, feature exist
In the real-time acquisition camera image, IMU measuring value and wheel encoder count specifically include:
While obtaining present image, the IMU measuring value and wheel obtained between the image and previous frame image acquisition moment is compiled
Code device reading, as the corresponding IMU measuring value of described image and wheel encoder count；
Wherein, described image, IMU measuring value and wheel encoder count have respective timestamp；So that being compiled to wheel
During code device reading is sampled, the timestamp of sampling is consistent with the timestamp of IMU measuring value.
4. the vision inertia ranging method of close coupling vehicle wheel encoder data according to claim 3, feature exist
In step 2 specifically includes:
Obtain the angle point of present image: being directed to first frame image, using apply-Damiano Tommasi method extracts the angle of the first frame image
Point；For other images in addition to first frame, the angle point on previous frame image is appeared in using optical flow method tracking；
Wherein, if some angle point of previous frame image can successfully track to obtain an angle point on the present image,
The two angle points form a pair of of match point between the present image and previous frame image；
Using all matching double points between the present image and previous frame image, random sampling unification algorism RANSAC is used
Frame calculates the essential matrix between the present image and previous frame image using 5 methods；If the essential matrix is interior
Point number is more than threshold value, then records the essential matrix, and decompose the essential matrix, obtain described image and previous frame image
Between spin matrix, while it is effective for recording the essential matrix validity；Otherwise it is invalid for recording its validity；
The present image is added the first sliding window and the second sliding window simultaneously, frame number is respectively k+1, m+1, if
K < K then terminates, and otherwise judges that present image is added before window whether last frame image is key frame in the first sliding window；If
It is not that key frame then abandons the present image in the first sliding window, while by the present image in the first sliding window
It is corresponding that corresponding all IMU measuring values and wheel encoder count are added to the present image being newly added in the first sliding window
In IMU measuring value and wheel encoder count, last frame picture numbers subtract 1 in the first sliding window；It is if key frame, then right
The corresponding parameter progress marginalisation of one frame image of foremost operates to obtain marginalisation error term in first sliding window；And it will be described
Image and corresponding all IMU measuring values and wheel encoder count abandon, and each frame number subtracts 1 in the first sliding window, then
By all timestamps in the second sliding window be less than in current first sliding window the n images of the 1st frame image temporal stamp and its
Corresponding IMU measuring value and wheel encoder count abandon, while each frame number subtracts n in the second sliding window；
Wherein, k and m is respectively the picture number in current first sliding window and the second sliding window, and K is the first sliding window
The middle most picture numbers that can be accommodated；
To the corresponding IMU measuring value of last frame image and wheel encoder count in the first sliding window, and for
The corresponding IMU measuring value of last frame image and wheel encoder count in two sliding windows carry out pre-integration operation.
5. the vision inertia ranging method of close coupling vehicle wheel encoder data according to claim 4, feature exist
In the progress pre-integration operation specifically includes:
I successively takes 0,2 ..., and s-2 is successively calculated
Ji+1=AiJi
Value is Ji+1The 7th row to the 9th row, the 16th column to the 18th column constitute matrix；
Until final result∑s-1WithIt is computed；
Wherein, s indicates to participate in the IMU measuring value of pre-integration and the number of wheel encoder count,AndIt is the rotation indicated by quaternary number,Indicate that the multiplication of quaternary number, R () are indicated a quaternary
Number is converted into the function of its corresponding spin matrix,Indicate the 3-axis acceleration in i-th of IMU measuring value,Indicate i-th
Three axis angular rates in a IMU measuring value,Indicate the reading of i-th of wheel sub-encoders, baFor accelerometer in current IMU
Offset, bωFor the offset of gyroscope in current IMU, δ tiThe timestamp and i-th of IMU for indicating i+1 IMU measuring value measure
The difference of the timestamp of value,Indicate from bodywork reference frame to the spin matrix of IMU coordinate system, in bodywork reference frame to
The vector in IMU coordinate system is measured, is obtained by off-line calibration；
Wherein, bodywork reference frame is defined as: using left rear wheel as origin, x-axis is directed toward off hind wheel, and y-axis is directed toward the near front wheel, and z-axis is hung down
Straight chassis is upward.
6. the vision inertia ranging method of close coupling vehicle wheel encoder data according to claim 1, feature exist
In step 4 specifically includes:
Judge the picture number in the first sliding window whether less than the maximum image number K accommodated in the first sliding window, if
It is that step 4 terminates；Otherwise it performs the next step；
For all images in the first sliding window, m is had more than if there is last frame image in a frame image and window
To match point, and the mean parallax of these matching double points is more than p pixel, and utilizes RANSAC frame using these matching double points
Frame calculates essential matrix using 5 methods, and having more than n is interior point to match point, is thened follow the steps in next step；Otherwise step 4 knot
Beam；
Judge the validity of the corresponding essential matrix of all images in the second sliding window for effective number, if its be less than it is effective
Essential matrix number threshold value r, then step 4 terminates；Otherwise using in the second sliding window from the second frame image each frame
IMU- takes turns the pre- product of sub-encoders in effective essential matrix and the second sliding window between image and its previous frame image
Point as a result, using least square method optimization computing gyroscope offset:
Wherein, bωFor gyroscope drift to be optimized, initial value is null vector；F is indicated in the second sliding window from the second frame image
The effective frame number set of essential matrix between each frame image and its previous frame image is played, m indicates the second sliding window
Middle picture numbers,To decompose the corresponding quaternary number of spin matrix that essential matrix obtains, indicate from the second sliding window
The camera coordinates system of m+1 frame image to m frame image camera coordinates system rotation,Be indicate from camera coordinates system to
The quaternary number of the rotation of IMU coordinate system, It can be obtained in advance by off-line calibration,Indicate quaternary
Several multiplication；It is a rotation quaternary number, is gyroscope drift bωFunction, r indicates effective essential matrix
The threshold value of number, ‖ ‖2Indicate 2 norms of vector:
δbωIt is bωRenewal amount,WithBe respectively m frame in the second sliding window and m+1 frame image it
Between IMU- wheel sub-encoders pre-integration rotation quaternary number and Jacobian matrix a part, with the cunning of the second sliding window
Dynamic, pre-integration obtains between arbitrary neighborhood two field pictures in windowWithIt is known；
For the corresponding IMU measuring value of each frame image and wheel encoder count in the second sliding window, successively again into
Row pre-integration operates, wherein gyroscope drift bωTo optimize obtained value；
Calculated pre-integration is as a result, successively calculate out of second sliding window first frame to the spin matrix of m frame
Wherein m successively takes 1,2 ..., M, and M is the number of image in the second sliding window；For unit battle array；
For the continuous image of two frame every in the second sliding window, equationof structure
Wherein,WithFor in the second sliding window between m frame and m+1 frame image
The result of IMU- wheel sub-encoders pre-integration；For quaternary numberCorresponding spin matrix form, Δ tmIt is
In two sliding windows between m+1 frame and m frame image timestamp difference；For spin matrix；ForIt is in window
Acceleration of gravity in mouthful under the corresponding IMU coordinate system of first frame image, its initial value can be by
It calculates, g is the size of local gravitational acceleration；B isTangent plane on one group of orthogonal basis；For m frame image pair
Movement velocity of the position IMU answered under the corresponding IMU coordinate system of m frame image,For the corresponding IMU of m+1 frame image
Movement velocity of the position under the corresponding IMU coordinate system of m+1 frame image；Δ g is the renewal amount of acceleration of gravity；
The equation can be denoted as:
M successively takes 1,2 ... M-1, M is the number of image in the second sliding window, available M-1 equation, their simultaneous
Obtain error equation:
It is denoted as L=AX
The all elements that X includes are variable to be optimized, and all elements of L and A are known；
Solve minX(AX-L)T(AX-L), it is updated using the Δ g in X
Using updatedIts orthogonal basis B for cutting space is recalculated, and is utilized newlyL and A are recalculated with B,
Repeat this step, iterative calculation；When the number of iterations reaches preset the number of iterations threshold value, step 4 terminates；Record this
WhenWith
For the corresponding IMU measuring value of each frame image and wheel encoder count in the first sliding window, successively again into
The operation of row pre-integration；
Calculate a matrixSo thatUtilize the pre-integration resultWithK is picture numbers in the first sliding window, and being calculated using the method for dead reckoning indicates the kth out of first sliding window
The spin matrix of the corresponding IMU coordinate system of first frame image in the corresponding IMU coordinate system to the first sliding window of frame imageAnd the first corresponding IMU coordinate origin of the kth frame image first frame image in the first sliding window in sliding window
Position in corresponding IMU coordinate systemIn view of the first frame image of the first sliding window and the second sliding window is identical,
And first each frame image in sliding window can find image identical with it in the second sliding window, calculate IMU
The initial value of parameter to be optimized in error term:
Wherein, Q () indicates the function that a spin matrix is converted to corresponding quaternary number,For in step S4006
The corresponding position IMU of m (k) frame image is in the corresponding IMU coordinate system of m (k) frame image in calculated second sliding window
Under movement velocity, m (k) indicate the first sliding window in kth frame image in the second sliding window corresponding picture numbers,
bωFor gyroscope drift；
Utilize the pose of the corresponding IMU coordinate system of image in first sliding windowWithCan be extrapolated
Image pose in one sliding window；Triangle is carried out using the image pose in the first sliding window and the match point on image
Change and calculate, obtain the depth of match point, using the depth as the matching in parameter to be optimized needed for construction re-projection error item
The initial value of point depth parameter；
The state that initialization is arranged is " being completed ".
7. the vision inertia ranging method of close coupling vehicle wheel encoder data according to claim 2, feature exist
In step 5 specifically includes:
According to the common practice based on the vision inertia ranging method of nonlinear optimization in sliding window in the first sliding window
Cost function is constructed, and carries out nonlinear optimization solution, obtains estimating for sensor pose, speed and offset and match point depth
Evaluation；
It wherein, include re-projection error item, imu error item and marginalisation error term in the cost function；
The re-projection error item and the marginalisation error term use the vision inertia based on nonlinear optimization in sliding window
The common practice of ranging method is constructed；
The imu error item is constructed using following methods:
Wherein, CIMU(x) imu error item is indicated；X is by x1, x2..., xkComposition；K indicates the frame number of image in sliding window；xk
It is the state vector of kth frame, ∑K, k+1The covariance matrix that pre-integration obtains in step S2006 is indicated, with the first sliding window
Mouthful sliding, the covariance matrix that pre-integration obtains between arbitrary neighborhood two field pictures in window is known；It indicates residual vector, has with the state vector of+1 frame of kth frame and kth and the IMU pre-integration between them
It closes:
WithThe position and speed of kth frame image exposure moment camera respectively under world coordinate system；With
The position and speed of+1 exposed frame moment of kth camera respectively under world coordinate system；WithRespectively in kth frame
The offset of accelerometer and gyroscope under IMU body coordinate system；WithRespectively sat in+1 frame IMU ontology of kth
Mark is the offset of lower accelerometer and gyroscope；Be with quaternary number indicate in the kth frame time of exposure from IMU ontology coordinate
It is the rotation to world coordinate system；ForSpin matrix form；It is indicated with quaternary number
In rotation of+1 frame image exposure moment of kth from IMU body coordinate system to world coordinate system；ForRotation
Turn matrix form；gwIt is a constant for the acceleration of gravity in world coordinate system；ΔtkWhen being exposed for+1 frame of kth and kth frame
Carve the difference of timestamp；It is coordinate of the origin of bodywork reference frame in IMU coordinate system, passes throughIt calculates
It obtains,It indicates from bodywork reference frame to the spin matrix of IMU coordinate system,Indicate the center of IMU in bodywork reference frame
Coordinate, in advance measure；WithIMU- wheel is compiled between+1 frame image of kth frame and kth
Code device pre-integration as a result, with the first sliding window sliding, pre-integration obtains between arbitrary neighborhood two field pictures in window
'sWithIt is known；
WithIt is
Parameter to be optimized.
8. a kind of vision inertia ranging system of close coupling vehicle wheel encoder data, which is characterized in that described includes vision
Inertia ranging system includes:
Initialization unit, the init state for the vision inertia ranging method to be arranged are " unfinished "；
Acquiring unit, for obtaining camera image in real time during initialization and ranging, Inertial Measurement Unit IMU is measured
Value and wheel encoder count；It is configured that a camera, real-time image acquisition；One IMU obtains IMU measuring value in real time；One
Set obtains the equipment that the data of sub-encoders are taken turns on vehicle, obtains wheel encoder count in real time；A set of timing device is described
Three kinds of data provide the timestamp under same time reference；
Camera attitude calculation unit, for calculating the essential matrix between present image and previous frame image, by the essential square
Battle array and the first sliding window and the second sliding window are added simultaneously with corresponding IMU measuring value and wheel encoder count, together
When according to present image whether be that key frame adjusts accordingly first sliding window and the second sliding window；
Judging unit, for corresponding to respective last frame image in first sliding window and the second sliding window
IMU measuring value and wheel encoder count carry out IMU- and take turns sub-encoders pre-integration；Whether the state for judging initialization is " complete
At "；
Judge execution unit, for the judging result of first judging unit be initialization state be " unfinished " when,
The picture number in the first sliding window is judged whether less than the maximum image number accommodated in the first sliding window, if then tying
Beam, otherwise continues to judge whether the parallax between the image in the second sliding window is less than threshold value, if then terminating, otherwise continues；
If it is determined that whether effective essential matrix number is less than the number threshold value of setting in the second sliding window, it is no if then terminating
Then according to the effective sheet from the second frame image between each frame image and its previous frame image in the second sliding window
In stromal matrix and the second sliding window IMU- take turns sub-encoders pre-integration as a result, calibration IMU in gyroscope drift just
Initial value；Using gyroscope drift to the corresponding IMU measuring value of every image in the second sliding window and wheel encoder count weight
The new IMU- that carries out takes turns sub-encoders pre-integration；Parameter is initialized using pre-integration result；
Optimize unit, for when the state that the judging result of first judging unit is initialization is " being completed " or institute
It states after the completion of judging execution unit initialization, calculates in the first sliding window and encoded comprising re-projection error item, IMU- wheel
The cost function of device error term and marginalisation error term, by sensor pose, speed and offset and match point depth together into
Row iteration optimization, obtains the corresponding parameter of each image in the first sliding window；
Ranging unit, for carrying out the vision inertia ranging of close coupling vehicle wheel encoder data according to the parameter.
9. a kind of vision inertia ranging system of close coupling vehicle wheel encoder data characterized by comprising
Processor；And
It is arranged to the memory of storage computer executable instructions, the executable instruction makes the processor when executed
Execute following operation:
Step 1: the init state that the vision inertia ranging method is arranged is " unfinished ", and in initialization and ranging
In the process, camera image, Inertial Measurement Unit IMU measuring value and wheel encoder count are obtained in real time；
Step 2: calculating the essential matrix between present image and previous frame image, by the essential matrix and with it is corresponding
The first sliding window and the second sliding window is added in IMU measuring value and wheel encoder count simultaneously, while according to present image
It whether is that key frame adjusts accordingly first sliding window and the second sliding window, the current image is first frame
Arbitrary image after image；
Step 3: IMU corresponding to last frame image respective in first sliding window and the second sliding window is measured
Value and wheel encoder count carry out IMU- and take turns sub-encoders pre-integration；Whether the state for judging initialization is " being completed ", if
It is to then follow the steps five, otherwise executes step 4；
Step 4: judge the picture number in the first sliding window whether less than the maximum image number accommodated in the first sliding window
Otherwise mesh continues to judge the view whether parallax between the image in the second sliding window is less than setting if then step 4 terminates
Otherwise poor threshold value continues to judge whether effective essential matrix number is less than in the second sliding window if then step 4 terminates
The number threshold value of setting, if it is step 4 terminates, otherwise according in the second sliding window from the second frame image it is each
It is pre- to take turns sub-encoders by IMU- in effective essential matrix and the second sliding window between frame image and its previous frame image
Integral as a result, calibration IMU in gyroscope drift initial value；Using gyroscope drift to every figure in the second sliding window
As corresponding IMU measuring value and wheel encoder count re-start IMU- wheel sub-encoders pre-integration；Utilize pre-integration result
Parameter is initialized；
Step 5: calculating in the first sliding window includes re-projection error item, IMU- wheel encoder errors item and marginalisation
Sensor pose, speed and offset and match point depth are iterated optimization by the cost function of error term together, obtain
The corresponding parameter of each image in one sliding window；
Step 6: the vision inertia ranging of close coupling vehicle wheel encoder data is carried out according to the parameter.
10. a kind of computer readable storage medium, which is characterized in that the computer-readable recording medium storage is one or more
Program, one or more of programs are when the electronic equipment for being included multiple application programs executes, so that the electronic equipment
Execute following operation:
Step 1: the init state that the vision inertia ranging method is arranged is " unfinished ", and in initialization and ranging
In the process, camera image, Inertial Measurement Unit IMU measuring value and wheel encoder count are obtained in real time；
Step 2: calculating the essential matrix between present image and previous frame image, by the essential matrix and with it is corresponding
The first sliding window and the second sliding window is added in IMU measuring value and wheel encoder count simultaneously, while according to present image
It whether is that key frame adjusts accordingly first sliding window and the second sliding window, the current image is first frame
Arbitrary image after image；
Step 3: IMU corresponding to last frame image respective in first sliding window and the second sliding window is measured
Value and wheel encoder count carry out IMU- and take turns sub-encoders pre-integration；Whether the state for judging initialization is " being completed ", if
It is to then follow the steps five, otherwise executes step 4；
Step 4: judge the picture number in the first sliding window whether less than the maximum image number accommodated in the first sliding window
Otherwise mesh continues to judge the view whether parallax between the image in the second sliding window is less than setting if then step 4 terminates
Otherwise poor threshold value continues to judge whether effective essential matrix number is less than in the second sliding window if then step 4 terminates
The number threshold value of setting, if it is step 4 terminates, otherwise according in the second sliding window from the second frame image it is each
It is pre- to take turns sub-encoders by IMU- in effective essential matrix and the second sliding window between frame image and its previous frame image
Integral as a result, calibration IMU in gyroscope drift initial value；Using gyroscope drift to every figure in the second sliding window
As corresponding IMU measuring value and wheel encoder count re-start IMU- wheel sub-encoders pre-integration；Utilize pre-integration result
Parameter is initialized；
Step 5: calculating in the first sliding window includes re-projection error item, IMU- wheel encoder errors item and marginalisation
Sensor pose, speed and offset and match point depth are iterated optimization by the cost function of error term together, obtain
The corresponding parameter of each image in one sliding window；
Step 6: the vision inertia ranging of close coupling vehicle wheel encoder data is carried out according to the parameter.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910127778.1A CN109764880B (en) | 2019-02-19 | 2019-02-19 | Visual inertial ranging method and system for tightly-coupled vehicle wheel encoder data |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910127778.1A CN109764880B (en) | 2019-02-19 | 2019-02-19 | Visual inertial ranging method and system for tightly-coupled vehicle wheel encoder data |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109764880A true CN109764880A (en) | 2019-05-17 |
| CN109764880B CN109764880B (en) | 2020-12-25 |
Family
ID=66456923
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910127778.1A Active CN109764880B (en) | 2019-02-19 | 2019-02-19 | Visual inertial ranging method and system for tightly-coupled vehicle wheel encoder data |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109764880B (en) |
Cited By (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110260861A (en) * | 2019-06-13 | 2019-09-20 | 北京华捷艾米科技有限公司 | Pose determines method and device, odometer |
| CN110428452A (en) * | 2019-07-11 | 2019-11-08 | 北京达佳互联信息技术有限公司 | Detection method, device, electronic equipment and the storage medium of non-static scene point |
| CN110956665A (en) * | 2019-12-18 | 2020-04-03 | 中国科学院自动化研究所 | Vehicle turning track bidirectional calculation method, system and device |
| CN111145267A (en) * | 2019-12-25 | 2020-05-12 | 重庆自行者科技有限公司 | IMU (inertial measurement unit) assistance-based 360-degree panoramic view multi-camera calibration method |
| CN111753938A (en) * | 2020-06-23 | 2020-10-09 | 联想(北京)有限公司 | Position acquisition method and device and electronic equipment |
| CN111795686A (en) * | 2020-06-08 | 2020-10-20 | 南京大学 | Method for positioning and mapping mobile robot |
| CN112815939A (en) * | 2021-01-04 | 2021-05-18 | 清华大学深圳国际研究生院 | Pose estimation method for mobile robot and computer-readable storage medium |
| CN113074754A (en) * | 2021-03-27 | 2021-07-06 | 上海智能新能源汽车科创功能平台有限公司 | Visual inertia SLAM system initialization method based on vehicle kinematic constraint |
| CN113295089A (en) * | 2021-04-07 | 2021-08-24 | 深圳市异方科技有限公司 | Compartment volume rate measuring method based on visual inertia SLAM |
| CN113793381A (en) * | 2021-07-27 | 2021-12-14 | 武汉中海庭数据技术有限公司 | Monocular visual information and wheel speed information fusion positioning method and system |
| CN113962115A (en) * | 2021-12-23 | 2022-01-21 | 深圳佑驾创新科技有限公司 | Method, device, equipment and storage medium for optimizing and calibrating vehicle tire coefficient map |
| CN115855117A (en) * | 2023-02-16 | 2023-03-28 | 深圳佑驾创新科技有限公司 | Combined calibration method for installation postures of camera and inertia measurement unit relative to vehicle body |
| CN117168530A (en) * | 2023-11-03 | 2023-12-05 | 泉州昆泰芯微电子科技有限公司 | Self-calibration method of magnetic encoder, magnetic encoder and motor |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102155955A (en) * | 2011-03-11 | 2011-08-17 | 天津理工大学 | Stereoscopic vision mile meter and measuring method |
| US9709404B2 (en) * | 2015-04-17 | 2017-07-18 | Regents Of The University Of Minnesota | Iterative Kalman Smoother for robust 3D localization for vision-aided inertial navigation |
| CN107869989A (en) * | 2017-11-06 | 2018-04-03 | 东北大学 | A kind of localization method and system of the fusion of view-based access control model inertial navigation information |
| CN108629793A (en) * | 2018-03-22 | 2018-10-09 | 中国科学院自动化研究所 | The vision inertia odometry and equipment demarcated using line duration |
| CN108717712A (en) * | 2018-05-29 | 2018-10-30 | 东北大学 | A kind of vision inertial navigation SLAM methods assumed based on ground level |
| CN109029448A (en) * | 2018-06-28 | 2018-12-18 | 东南大学 | The IMU of monocular vision inertial positioning assists trace model |
| KR20190001086A (en) * | 2017-06-26 | 2019-01-04 | 서울대학교산학협력단 | Sliding windows based structure-less localization method using inertial and single optical sensor, recording medium and device for performing the method |
-
2019
- 2019-02-19 CN CN201910127778.1A patent/CN109764880B/en active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102155955A (en) * | 2011-03-11 | 2011-08-17 | 天津理工大学 | Stereoscopic vision mile meter and measuring method |
| US9709404B2 (en) * | 2015-04-17 | 2017-07-18 | Regents Of The University Of Minnesota | Iterative Kalman Smoother for robust 3D localization for vision-aided inertial navigation |
| KR20190001086A (en) * | 2017-06-26 | 2019-01-04 | 서울대학교산학협력단 | Sliding windows based structure-less localization method using inertial and single optical sensor, recording medium and device for performing the method |
| CN107869989A (en) * | 2017-11-06 | 2018-04-03 | 东北大学 | A kind of localization method and system of the fusion of view-based access control model inertial navigation information |
| CN108629793A (en) * | 2018-03-22 | 2018-10-09 | 中国科学院自动化研究所 | The vision inertia odometry and equipment demarcated using line duration |
| CN108717712A (en) * | 2018-05-29 | 2018-10-30 | 东北大学 | A kind of vision inertial navigation SLAM methods assumed based on ground level |
| CN109029448A (en) * | 2018-06-28 | 2018-12-18 | 东南大学 | The IMU of monocular vision inertial positioning assists trace model |
Non-Patent Citations (5)
| Title |
|---|
| JING WANG等: "Visual SLAM incorporating wheel odometer for indoor robots", 《PROCEEDINGS OF THE 36TH CHINESE CONTROL CONFERENCE》 * |
| KEJIAN J. WU等: "VINS on Wheels", 《2017 IEEE INTERNATIONAL CONFERENCE ON ROBOTICS AND AUTOMATION (ICRA)》 * |
| TASHFEEN B. KARAMAT等: "Novel EKF-Based Vision/Inertial System Integration for Improved Navigation", 《IEEE TRANSACTIONS ON INSTRUMENTATION AND MEASUREMENT》 * |
| 李建禹: "基于单目视觉与IMU结合的SLAM技术研究", 《中国优秀硕士学位论文全文数据库 信息科技辑》 * |
| 王京: "基于传感器数据融合的单目视觉SLAM方法研究", 《中国优秀硕士学位论文全文数据库 信息科技辑》 * |
Cited By (24)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110260861A (en) * | 2019-06-13 | 2019-09-20 | 北京华捷艾米科技有限公司 | Pose determines method and device, odometer |
| CN110260861B (en) * | 2019-06-13 | 2021-07-27 | 北京华捷艾米科技有限公司 | Pose determination method and device and odometer |
| CN110428452B (en) * | 2019-07-11 | 2022-03-25 | 北京达佳互联信息技术有限公司 | Method and device for detecting non-static scene points, electronic equipment and storage medium |
| CN110428452A (en) * | 2019-07-11 | 2019-11-08 | 北京达佳互联信息技术有限公司 | Detection method, device, electronic equipment and the storage medium of non-static scene point |
| CN110956665A (en) * | 2019-12-18 | 2020-04-03 | 中国科学院自动化研究所 | Vehicle turning track bidirectional calculation method, system and device |
| CN110956665B (en) * | 2019-12-18 | 2023-06-23 | 中国科学院自动化研究所 | Bidirectional calculation method, system and device for turning track of vehicle |
| CN111145267A (en) * | 2019-12-25 | 2020-05-12 | 重庆自行者科技有限公司 | IMU (inertial measurement unit) assistance-based 360-degree panoramic view multi-camera calibration method |
| CN111145267B (en) * | 2019-12-25 | 2023-09-05 | 重庆自行者科技有限公司 | 360-degree panoramic view multi-camera calibration method based on IMU assistance |
| CN111795686A (en) * | 2020-06-08 | 2020-10-20 | 南京大学 | Method for positioning and mapping mobile robot |
| CN111795686B (en) * | 2020-06-08 | 2024-02-02 | 南京大学 | Mobile robot positioning and mapping method |
| CN111753938A (en) * | 2020-06-23 | 2020-10-09 | 联想(北京)有限公司 | Position acquisition method and device and electronic equipment |
| CN111753938B (en) * | 2020-06-23 | 2021-12-24 | 联想(北京)有限公司 | Position acquisition method and device and electronic equipment |
| CN112815939A (en) * | 2021-01-04 | 2021-05-18 | 清华大学深圳国际研究生院 | Pose estimation method for mobile robot and computer-readable storage medium |
| CN112815939B (en) * | 2021-01-04 | 2024-02-23 | 清华大学深圳国际研究生院 | Pose estimation method of mobile robot and computer readable storage medium |
| CN113074754A (en) * | 2021-03-27 | 2021-07-06 | 上海智能新能源汽车科创功能平台有限公司 | Visual inertia SLAM system initialization method based on vehicle kinematic constraint |
| CN113295089A (en) * | 2021-04-07 | 2021-08-24 | 深圳市异方科技有限公司 | Compartment volume rate measuring method based on visual inertia SLAM |
| CN113295089B (en) * | 2021-04-07 | 2024-04-26 | 深圳市异方科技有限公司 | Carriage volume rate measuring method based on visual inertia SLAM |
| CN113793381A (en) * | 2021-07-27 | 2021-12-14 | 武汉中海庭数据技术有限公司 | Monocular visual information and wheel speed information fusion positioning method and system |
| CN113962115A (en) * | 2021-12-23 | 2022-01-21 | 深圳佑驾创新科技有限公司 | Method, device, equipment and storage medium for optimizing and calibrating vehicle tire coefficient map |
| CN113962115B (en) * | 2021-12-23 | 2022-04-05 | 深圳佑驾创新科技有限公司 | Method, device, equipment and storage medium for optimizing and calibrating vehicle tire coefficient map |
| CN115855117A (en) * | 2023-02-16 | 2023-03-28 | 深圳佑驾创新科技有限公司 | Combined calibration method for installation postures of camera and inertia measurement unit relative to vehicle body |
| CN115855117B (en) * | 2023-02-16 | 2023-06-02 | 深圳佑驾创新科技有限公司 | Combined calibration method for mounting posture of camera and inertial measurement unit relative to vehicle body |
| CN117168530A (en) * | 2023-11-03 | 2023-12-05 | 泉州昆泰芯微电子科技有限公司 | Self-calibration method of magnetic encoder, magnetic encoder and motor |
| CN117168530B (en) * | 2023-11-03 | 2024-01-26 | 泉州昆泰芯微电子科技有限公司 | Self-calibration method of magnetic encoder, magnetic encoder and motor |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109764880B (en) | 2020-12-25 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109764880A (en) | The vision inertia ranging method and system of close coupling vehicle wheel encoder data | |
| CN104180818B (en) | A kind of monocular vision mileage calculation device | |
| CN108253963B (en) | Robot active disturbance rejection positioning method and positioning system based on multi-sensor fusion | |
| CN108731670A (en) | Inertia/visual odometry combined navigation locating method based on measurement model optimization | |
| CN105953796A (en) | Stable motion tracking method and stable motion tracking device based on integration of simple camera and IMU (inertial measurement unit) of smart cellphone | |
| CN109991636A (en) | Map constructing method and system based on GPS, IMU and binocular vision | |
| CN111811506A (en) | Visual/inertial odometer combined navigation method, electronic equipment and storage medium | |
| CN107941217B (en) | Robot positioning method, electronic equipment, storage medium and device | |
| CN109540126A (en) | A kind of inertia visual combination air navigation aid based on optical flow method | |
| CN110956665B (en) | Bidirectional calculation method, system and device for turning track of vehicle | |
| CN109141433A (en) | A kind of robot indoor locating system and localization method | |
| CN106017463A (en) | Aircraft positioning method based on positioning and sensing device | |
| CN112083725A (en) | Structure-shared multi-sensor fusion positioning system for automatic driving vehicle | |
| CN110726406A (en) | Improved nonlinear optimization monocular inertial navigation SLAM method | |
| CN106052683A (en) | Robot motion attitude estimating method | |
| CN107796391A (en) | A kind of strapdown inertial navigation system/visual odometry Combinated navigation method | |
| CN112697138B (en) | Bionic polarization synchronous positioning and composition method based on factor graph optimization | |
| CN110595503B (en) | Self-alignment method of SINS strapdown inertial navigation system shaking base based on lie group optimal estimation | |
| CN208751577U (en) | A kind of robot indoor locating system | |
| CN113189613B (en) | Robot positioning method based on particle filtering | |
| CN115479602A (en) | Visual inertial odometer method fusing event and distance | |
| CN113503873B (en) | Visual positioning method for multi-sensor fusion | |
| CN110207692A (en) | A kind of inertia pre-integration pedestrian navigation method of map auxiliary | |
| CN110260861A (en) | Pose determines method and device, odometer | |
| CN113639722B (en) | Continuous laser scanning registration auxiliary inertial positioning and attitude determination method |
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 | ||
| CB03 | Change of inventor or designer information |
Inventor after: Gao Wei Inventor after: Liu Jinxu Inventor after: Hu Zhanyi Inventor before: Liu Jinxu Inventor before: Gao Wei Inventor before: Hu Zhanyi |
|
| CB03 | Change of inventor or designer information | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |