CN115127547B - A tunnel inspection vehicle positioning method based on strapdown inertial navigation system and image positioning - Google Patents

Abstract

The invention discloses a positioning method for a tunnel inspection vehicle based on a strapdown inertial navigation system and image positioning. The method comprises the following steps: firstly, the latitude and longitude of the tunnel inspection vehicle on the earth surface are obtained; the monocular array cameras at the front end and the rear end of the tunnel inspection vehicle successively shoot the discontinuous dotted lines between lanes in the tunnel, the two discontinuous dotted lines are the same, the yaw angle of the tunnel inspection vehicle is solved by taking the end point of the identification line and the center point of the bottom of the camera as feature points, the yaw angle and the position quantity of the encoder are fused and processed, and then the motion track of the tunnel inspection vehicle in the tunnel under the earth coordinate system is obtained by combining the initial moment position of the tunnel inspection vehicle entering the tunnel; the latitude is converted into a numerical value under the earth coordinate system and recorded as M, M is compared with X, and the absolute value is recorded as O, the longitude is converted into a numerical value under the earth coordinate system and recorded as N, N is compared with Y, and the absolute value is recorded as P, when O and P are both within the set error value, M and N are used to locate the tunnel inspection vehicle, otherwise M and N, X and Y are fused by a Kalman filter, and the fused numerical value locates the tunnel inspection vehicle.

Description

A tunnel inspection vehicle positioning method based on strapdown inertial navigation system and image positioning

Technical Field

The invention belongs to the field of tunnel detection and positioning, and in particular relates to a tunnel detection vehicle positioning method based on a strapdown inertial navigation system and image positioning.

Background Art

With the continuous development of the transportation industry, the number and length of tunnels are increasing. Regular inspection of tunnels is an important guarantee for the normal operation of highways and personal safety. Traditional tunnel inspection is usually done manually, which takes a long time and often requires blocking the tunnel. It has the disadvantages of low efficiency, high inspection intensity, and increased traffic pressure.

The existing technology uses tunnel inspection vehicles equipped with monocular array cameras, lasers, visible light and other devices to inspect the inner wall of the tunnel. However, in longer tunnels, the satellite signal is weak or even almost non-existent, and it is difficult to rely on satellites to determine the position of the tunnel inspection vehicle in actual tunnel inspection.

Summary of the invention

In view of the problems existing in the prior art, the present invention provides a tunnel inspection vehicle positioning method based on a strapdown inertial navigation system and image positioning, which can accurately reflect the transport orientation of the tunnel inspection vehicle and realize rapid detection of the tunnel inspection vehicle in the tunnel.

The present invention is achieved through the following technical solutions:

A tunnel inspection vehicle positioning method based on a strapdown inertial navigation system and image positioning comprises the following steps:

S1, first using an accelerometer to obtain a position update differential equation of the tunnel inspection vehicle, and then solving the position update differential equation to obtain the latitude and longitude of the tunnel inspection vehicle on the earth's surface;

The front monocular array camera of the tunnel inspection vehicle first shoots the discontinuous dotted lines between the lanes in the tunnel, and the rear monocular array camera of the tunnel inspection vehicle then shoots the discontinuous dotted lines between the lanes in the tunnel. The discontinuous dotted lines shot twice are the same. Then, the yaw angle of the tunnel inspection vehicle is solved with the endpoint of the identification line and the center point of the bottom of the monocular array camera as feature points. The obtained yaw angle and the position of the encoder are fused and processed. Then, combined with the initial position of the tunnel inspection vehicle entering the tunnel, the motion trajectory of the tunnel inspection vehicle in the tunnel in the earth coordinate system is obtained. The motion trajectory is a set of X and Y points, where X corresponds to the value of the tunnel inspection vehicle in the x direction in the earth coordinate system, and Y corresponds to the value of the tunnel inspection vehicle in the y direction in the earth coordinate system.

S2, convert the latitude of the tunnel inspection vehicle on the earth's surface into a numerical value in the earth's coordinate system and record it as M, compare M with X in the motion trajectory described in S1, and record the absolute value obtained as O, convert the longitude of the tunnel inspection vehicle on the earth's surface into a numerical value in the earth's coordinate system and record it as N, compare N with Y in the motion trajectory described in S1, and record the absolute value obtained as P, when O and P are both within the set error value, use M and N to locate the tunnel inspection vehicle, when at least one of O and P is greater than the set error value, M and N, as well as X and Y in the motion trajectory described in S1 are fused in the Kalman filter, and the fused value is used to locate the tunnel inspection vehicle.

Preferably, the position update differential equation described in S1 is as follows:

Wherein, L is the latitude of the tunnel inspection vehicle on the earth's surface, v _y is the velocity component of the tunnel inspection vehicle in the y ₁ direction in the coordinate system b, _RM is the radius of the earth's meridian corresponding to the point where the tunnel inspection vehicle is located, and h is the altitude; λ is the longitude of the tunnel inspection vehicle on the earth's surface, _vx is the velocity component of the tunnel inspection vehicle in the x ₁ direction in the coordinate system b, and _RN is the radius of curvature of the meridian corresponding to the point where the tunnel inspection vehicle is located;

In coordinate system b, with the center of mass of the tunnel inspection vehicle as the origin, the rightward direction along the axis of the tunnel inspection vehicle is the positive direction of the _x1 axis, and the forward direction along the perpendicular _x1 axis is the positive direction of the _y1 axis.

Preferably, the front-end monocular area array camera and the rear-end monocular area array camera in S1 extract feature point sets of two identical images, which are respectively recorded as N ₁ and N ₂ , and then find the feature point b and feature point c with the smallest and second smallest Euclidean distances between any feature point a in N ₁ and N ₂ , and the Euclidean distances between feature point a and feature point b and feature point c are d ₁ and d ₂ respectively. If the ratio of d ₁ and d ₂ is less than a threshold, feature point a and feature point b are determined to be a matching point pair, which is recorded as (a, b). The matching point pair sets of the two images are obtained according to the same process, and then the mismatches in the matching point pair set are eliminated, and then the endpoints of the identification line are selected as feature points.

Further, find out the matching point pairs in which the number of intersection points between any one feature point and other matching feature points is greater than 3 from the matching point pair set, determine these matching point pairs as wrong matching pairs, and eliminate the wrong matching pairs. Then calculate the Euclidean distance ratios _ri of the remaining matching point pairs, sort _ri in ascending order to form a new matching point pair set, and then divide the new matching point pair set into four groups on average. Select four matching point pairs at random from the first group. If none of the four matching point pairs are inliers, discard the first group and do not make a judgment on the remaining matching point pairs. If two matching point pairs in the first group are inliers, make an inlier judgment on the remaining matching point pairs in the first group. Perform the same operation on the remaining groups, select the group with the largest number of inliers as the optimal model, and complete the elimination of wrong matches in the matching point pair set.

Preferably, S1 transforms the endpoint of the identification line extracted by the front monocular area array camera and the center point of the bottom of the front monocular area array camera into a world coordinate system, which are recorded as (X ₁ , Y ₁ ) and (X ₂ , Y ₂ ) respectively; transforms the endpoint of the identification line extracted by the rear monocular area array camera and the center point of the bottom of the rear monocular area array camera into a world coordinate system, which are recorded as (X ₁ ′, Y ₁ ′) and (X ₂ ′, Y ₂ ′) respectively; the distance L ₁ from the front monocular area array camera to the endpoint of the identification line and the distance L ₂ from the rear monocular area array camera to the endpoint of the identification line are obtained by the following formula:

The yaw angle of the front monocular array camera and the yaw angle of the rear monocular array camera are expressed by the following formulas respectively:

Where _x1 and _x2 are the vertical distances from the front monocular array camera and the rear monocular array camera to the marking line, respectively. The yaw angle of the tunnel inspection vehicle is expressed as follows:

Preferably, S1 uses a recursive algorithm to fuse the obtained yaw angle and the encoder position. The specific formula is as follows:

Where τ∈(t _k ,t _k+1 ), v(τ) is the speed of the tunnel detection vehicle in the driving direction output by the encoder at different times τ, v _x represents the speed component of the tunnel detection vehicle along the x direction of the earth coordinate system, v _y represents the speed component of the tunnel detection vehicle along the y direction of the earth coordinate system, x(t _k+1 ) and y(t _k+1 ) represent the positions of the tunnel detection vehicle at time k+1, respectively. is the yaw angle of the tunnel inspection vehicle at different times τ, and the coordinate value point set of the tunnel inspection vehicle at different times is the motion trajectory of the tunnel inspection vehicle in the tunnel, which is specifically expressed as the following formula: s = {(x(t ₀ ), y(t ₀ )), (x(t ₁ ), y(t ₁ )), … , (x(t _k ), y(t _k )), (x(t _k+1 ), y(t _k+1 ))}.

Furthermore, when k=0, x(t ₀ ) and y(t ₀ ) are the initial positions of the tunnel inspection vehicle when it enters the tunnel, respectively, and the initial positions are provided by GPS.

Preferably, S2 uses the following formula when converting the latitude and longitude of the tunnel inspection vehicle on the earth's surface into values in the earth's coordinate system:

Among them, the earth coordinate system is denoted as coordinate system e;

Where: θ, γ, ψ are the pitch angle, roll angle and yaw angle of the tunnel inspection vehicle, respectively; λ and L are the longitude and latitude of the tunnel inspection vehicle, respectively.

Furthermore, the pitch angle, roll angle and yaw angle of the tunnel inspection vehicle are obtained according to the following process:

First, define the coordinate system n, with the center of mass of the tunnel inspection vehicle as the origin, the east as the positive direction of the _x2 axis, the north as the positive direction of the _y2 axis, and the vertical upward direction as the positive direction of the _z2 axis. Then define the vector r ^b in the coordinate system b, r ^b = xi + yi + zk, and convert it to the vector in the coordinate system n as r ⁿ , and then define the quaternion Using quaternion operation rules, r ⁿ can be expressed as follows:

Further we can get the following formula:

The attitude position matrix from coordinate system b to coordinate system n It is expressed as the following formula:

Expressed as q ₀ , q ₁ , q ₂ , q ₃ are all real numbers, i, j and k are all direction vectors, yes The conjugate matrix of .

and The relationship is expressed in matrix form as follows:

In the formula, is the angular velocity of the tunnel inspection vehicle in coordinate system b measured by the MEMS gyroscope, They are The angular velocity around the x1 _axis , _y1 axis, and _z1 axis, with the positive direction of the _{z1 axis perpendicular to the x1 axis} ; in Output the angular momentum of the tunnel inspection vehicle for the MEMS gyro, is the Earth's rotational velocity, is the angular velocity of the geographic coordinate system relative to the earth, yes The transposed matrix of

right and The relationship is calculated using the quaternion Picard algorithm, and the following result is obtained:

in: Further Taylor series expansion is performed to obtain the attitude position matrix shown in the following formula:

Further use T with different double subscripts to represent Posture position matrix Denoted as:

Finally, we get:

Preferably, when S2 uses a Kalman filter to fuse M and N, and X and Y in the motion trajectory described in S1, the following process is adopted:

First solve the noise variance of M and N And the noise variance of X and Y in the motion trajectory described in S1 The relationship between the current position Pos(k) of the tunnel inspection vehicle, the position Pos ₁ (k) corresponding to M and N, and the position Pos ₂ (k) corresponding to X and Y in the motion trajectory of S1 is as follows:

The current position Pos(k) of the tunnel detection vehicle is used to obtain the fused value.

Compared with the prior art, the present invention has the following beneficial technical effects:

The invention discloses a positioning method for a tunnel inspection vehicle based on a strapdown inertial navigation system and image positioning. Since GPS can hardly receive signals in a tunnel, the positioning of the tunnel inspection vehicle cannot be realized by relying on GPS. Therefore, an accelerometer can be used to obtain a differential equation for updating the position of the tunnel inspection vehicle, and the latitude and longitude of the tunnel inspection vehicle on the earth surface can be further solved to realize tracking of the position of the tunnel inspection vehicle. A front monocular array camera and a rear monocular array camera can successively shoot the same discontinuous dotted lines between lanes in the tunnel, and then the yaw angle of the tunnel inspection vehicle is solved with the end point of the marking line and the center point of the bottom of the monocular array camera as feature points. The obtained yaw angle and the position of the encoder can be fused and processed, and then combined with the initial moment position of the tunnel inspection vehicle entering the tunnel, the motion trajectory of the tunnel inspection vehicle in the tunnel in the earth coordinate system can be obtained. The motion trajectory is a point set of X and Y, wherein X corresponds to the value of the tunnel inspection vehicle in the x direction in the earth coordinate system, and Y corresponds to the value of the tunnel inspection vehicle in the y direction in the earth coordinate system, so as to realize real-time positioning of the tunnel inspection vehicle. Theoretically, these two methods should be able to maintain complete trajectory overlap, but the former has more serious error accumulation, and the latter has reduced positioning accuracy due to the monocular array camera being easily affected by light and the jitter of the tunnel inspection vehicle itself. Therefore, the latitude and longitude of the tunnel inspection vehicle on the earth's surface are converted into values in the earth's coordinate system, and then compared with the X and Y in the motion trajectory obtained by the monocular array camera. When the difference is within the set error value, the positioning error is small, and the converted latitude and longitude are used to locate the tunnel inspection vehicle. Otherwise, the converted latitude value, longitude value, and X and Y in the motion trajectory obtained by the monocular array camera are fused with a Kalman filter, and the fused values are used to locate the tunnel inspection vehicle, ultimately achieving high-precision comprehensive positioning and completing the positioning of the tunnel inspection vehicle during defect detection in the tunnel.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG1 is a schematic diagram of the installation positions of the monocular area array camera and the LED lamp on a tunnel inspection vehicle according to the present invention.

FIG. 2 is a schematic diagram of the installation position of the strapdown inertial navigation system of the present invention on the chassis of the tunnel inspection vehicle.

FIG. 3 is a schematic diagram of two monocular area array cameras of the present invention photographing the same road section.

FIG. 4 is a schematic diagram of yaw angle calculation of a tunnel inspection vehicle according to the present invention.

FIG5 is a general flow chart of the present invention.

FIG. 6 is a flow chart of the inertial navigation positioning of the present invention.

FIG. 7 is a flow chart of image positioning according to the present invention.

FIG8 is a flow chart of the fusion positioning of the present invention.

In the figure: tunnel inspection vehicle 1, front monocular area array camera 2, front LED light 3, rear monocular area array camera 4, rear LED light 5 and strapdown inertial navigation system 6.

DETAILED DESCRIPTION

The present invention is further described in detail below in conjunction with specific embodiments, which are intended to explain the present invention rather than to limit it.

The present invention discloses a method for linear positioning of a tunnel inspection vehicle based on a strapdown inertial navigation system (abbreviated as SINS) and image positioning, as shown in FIG1, comprising a front monocular array camera 2, a front LED lamp 3, a rear monocular array camera 4 and a rear LED lamp 5 installed at the top of the front end of the tunnel inspection vehicle 1, and a strapdown inertial navigation system 6 installed on the crossbeam in the middle of the chassis of the tunnel inspection vehicle 1 as shown in FIG2. The tunnel inspection vehicle 1 is modified from a passenger car chassis, and the tunnel inspection vehicle 1 maintains a stable speed when inspecting defects in the tunnel. The front monocular array camera 2 and the rear monocular array camera 4 are used for image acquisition of road marking lines. The front LED lamp 3 and the rear LED lamp 5 are used to realize lighting, have a high brightness effect, and can ensure the illumination of image acquisition in the tunnel. The strapdown inertial navigation system 7 includes a MEMS gyroscope, an accelerometer and a microcomputer, and the position of the tunnel inspection vehicle 1 is solved by updating the differential equation in combination with the position obtained by the accelerometer, so as to realize the positioning of the tunnel inspection vehicle 1 in the tunnel. The MEMS gyroscope can update the attitude of the tunnel inspection vehicle 1 in real time and obtain the corresponding attitude angle, namely the pitch angle, roll angle and yaw angle mentioned later. As shown in Figure 5, before the tunnel inspection vehicle 1 enters the tunnel, the on-board GPS is used to locate the tunnel inspection vehicle 1. When collecting images in the tunnel, the tunnel inspection vehicle 1 maintains a stable speed. After entering the tunnel, the monocular array camera is used to collect the marking lines in the tunnel to achieve positioning. By comparing the image positioning trajectory based on the marking lines in the tunnel in the trajectory diagram with the positioning trajectory based on the strapdown inertial navigation system 7, it is determined whether the two need to be fused through the Kalman filter according to the situation, and the positioning position of the tunnel inspection vehicle 1 is output. The entire positioning system uses the fusion technology of image and inertial navigation to achieve high-precision positioning.

The algorithm of the present invention is described in detail below. The algorithm of the present invention includes three parts: strapdown inertial navigation system positioning, image positioning and fusion positioning.

First, as shown in Figure 6, the strapdown inertial navigation system positioning is realized by using attitude position solution;

First, establish two coordinate systems: coordinate system b and coordinate system n. Take the center of mass of the tunnel inspection vehicle 1 as the origin o, take the right direction along the axis of the tunnel inspection vehicle 1 as the positive direction of the _x1 axis, take the vertical _x1 axis forward as the positive direction of the _y1 axis, take the vertical _x1 axis upward as the positive direction of the _z1 axis, and define the above-determined coordinate system as coordinate system b. Take the center of mass of the tunnel inspection vehicle 1 as the origin o, take the east direction as the positive direction of the _x2 axis, take the north direction as the positive direction of the _y2 axis, take the direction vertical to the sky as the positive direction of the _z2 axis, and define this coordinate system as coordinate system n.

The attitude and position solution is to find out the change of the current coordinate system b of the tunnel detection vehicle 1 relative to the coordinate system n. There is such an attitude and position matrix from the coordinate system b to the coordinate system n. For a vector in the coordinate system b, when it is multiplied on the left by the attitude and position matrix, it can be obtained that the size of this vector corresponds to the vector in the coordinate system n. The goal of the attitude and position solution is to calculate this attitude and position matrix in real time based on the data output by the MEMS gyroscope on the tunnel detection vehicle 1. The present invention adopts the quaternion method update algorithm to perform the attitude and position solution of the tunnel detection vehicle 1. The quaternion method update algorithm is a commonly used attitude solution algorithm for the current strapdown inertial navigation system. In addition, there are also the Euler angle method update algorithm, the direction cosine method update algorithm and the equivalent rotation vector method update algorithm.

When the posture of the tunnel inspection vehicle 1 changes, the positions of the origins of the coordinate system b and the coordinate system n and their corresponding coordinates will change accordingly. The MEMS gyroscope installed on the tunnel inspection vehicle 1 can sense the angular momentum of the tunnel inspection vehicle 1 in the coordinate system b at intervals, and then the angular velocity of the tunnel inspection vehicle 1 in the coordinate system b can be obtained. The accelerometer can sense the linear velocity of the tunnel inspection vehicle 1 in the coordinate system b. The sensing data of the two will be fed back to the microcomputer in real time, and the microcomputer can solve the attitude position matrix from the coordinate system b to the coordinate system n using the quaternion Picard algorithm Then, three angles about the attitude of the tunnel inspection vehicle 1 are obtained, that is, in the coordinate system b, the pitch angle θ of the tunnel inspection vehicle 1 rotating around the _x1 axis, the roll angle γ of the tunnel inspection vehicle 1 rotating around the _y1 axis, and the yaw angle ψ of the tunnel inspection vehicle 1 rotating around the _z1 axis. The output of the MEMS gyroscope is generally the angle increment within the sampling time interval. In order to avoid differential amplification of noise, the quaternion should be determined directly using the angle increment. The quaternion Pica algorithm is a common algorithm for calculating the quaternion update algorithm by the angle increment.

Define the vector r ^b in coordinate system b as xi+yi+zk, and the vector converted to coordinate system n is denoted as r ⁿ . Define quaternion Using quaternion operation rules, r ⁿ can be expressed as: Expand

The posture position matrix can be obtained as

q ₀ ,q ₁ ,q ₂ ,q ₃ are all real numbers, i, j and k are all direction vectors, yes The conjugate matrix of .

To obtain the attitude position matrix We must first find the quaternion Quaternion With angular velocity The relationship is expressed in matrix form as follows (1):

In the formula, They are the angular velocities of the tunnel inspection vehicle 1 around the x1 _- axis, y1 _- axis, and z1 _- axis measured by the MEMS gyroscope. in The angular momentum of the tunnel detection vehicle 1 is output by the MEMS gyro, is the Earth's rotational velocity, is the angular velocity of the geographic coordinate system relative to the earth, yes The transposed matrix of can be obtained above, and then substituted into formula (1).

For the solution of formula (1), the quaternion Picard algorithm is used to calculate and the result can be obtained:

in:

Then the Taylor series expansion of the result can be obtained as quaternion Then determine the real-time posture position matrix

The matrices in the second row of formula (2) are represented by T with different double subscripts, then formula (2) can be written as:

Since the 9 matrix values in formula (3) can be obtained, we can further obtain:

In formula (4), θ, γ, and ψ are the pitch angle, roll angle, and yaw angle of the tunnel inspection vehicle 1 mentioned above, respectively, which are called the three attitude angles of the tunnel inspection vehicle 1 and can be used for the conversion between the earth coordinate system and the coordinate system b in the subsequent fusion part.

The position update differential equation of tunnel inspection vehicle 1 is:

In formula (5), _vx , _vy , and _vz are the velocities of the three coordinate axes in the coordinate system b, respectively. The acceleration of the three coordinate axes measured by the accelerometer is a = [ _ax , _ay , _az ] ^T. By integrating the acceleration once, _vx , _vy , and _vz can be obtained. _{R M} and _RN are the radius of the earth's meridian circle and the curvature radius of the corresponding meridian circle at the location of the tunnel inspection vehicle 1, respectively. L is the latitude of the tunnel inspection vehicle 1 on the earth's surface, λ is the longitude of the tunnel inspection vehicle 1 on the earth's surface, and h is the altitude. By integrating formula (5) once, the latitude, longitude, and altitude of the location of the tunnel inspection vehicle 1 can be obtained.

Second, the image positioning as shown in Figure 7

Image positioning is based on the tunnel road marking lines. The images of the road marking lines are collected by the front monocular array camera 2 and the rear monocular array camera 4. As shown in Figure 3, the two curved solid lines represent the road boundaries, and the dotted dash line in the middle is the road marking line. Figure 3 schematically draws the curved line. In the actual tunnel, the curvature is small. v refers to the forward direction of the tunnel detection vehicle 1. _Tk represents the time _tk . The subsequent algorithm locates the position of the tunnel detection vehicle 1 by integrating the time period. The collected image is processed, and the coordinates of the extracted feature points are solved using the pinhole imaging model. Specifically, the front monocular array camera 2 and the rear monocular array camera 4 project the spatial points onto the two-dimensional photo. The projection process can be represented by the pinhole imaging model of the corresponding monocular array camera. The relationship between the image coordinates and the spatial coordinates can also be obtained using the pinhole imaging model. The pinhole imaging model is a camera imaging model commonly used now. Then, the attitude angles of the front monocular array camera 2 and the rear monocular array camera 4 are used to calculate the yaw angle of the tunnel inspection vehicle 1. The yaw angle here is different from the yaw angle obtained previously. The strapdown inertial navigation system positioning and image positioning need to calculate their respective yaw angles. Finally, the yaw angle of the tunnel inspection vehicle 1 and the position of the encoder are integrated to calculate the linear positioning of the tunnel inspection vehicle 1 based on the tunnel road marking line.

(1) Preprocess the image

The shooting environment of the monocular area array camera is on an outdoor road. The outdoor environment will interfere with the image, so the image needs to be filtered. The median filter method is used here. First, an odd number of data is taken from the sampling window, and then the median of these data is used as the gray value of the current pixel:

The filtered image is then binarized and a threshold T is set. When the image pixel value is greater than the threshold T, it is considered white; conversely, when the image pixel value is less than the threshold T, it is considered black.

The Roberts operator is used to extract the edge of the image. The difference between two adjacent pixels in the diagonal direction is used to approximate the gradient amplitude to detect the edge. The calculation formula is as follows:

Where f(x,y) is the input image, g(x,y) is the output image, and x and y are the pixel coordinates of the image.

(2) Extracting and matching image feature points

The lines between the lanes in the tunnel are white discontinuous dashed lines, so feature points can be selected based on the discontinuous dashed lines between the lanes in the tunnel. The image positioning of the present invention is based on the front monocular area array camera 2 and the rear monocular area array camera 4 on the top of the tunnel inspection vehicle 1 respectively shooting the same identification line image segment, and then calculating the position of the tunnel inspection vehicle 1, so the same image segment needs to be stereo matched.

First, the Hessian matrix is calculated for a point in the image on the σ scale, and its expression is:

Where L _xx (x,σ) represents the image point and the Gaussian second-order differential Convolution, L _xy (x,σ) represents the convolution of the image point and the Gaussian second-order differential convolution, _Lyx (x,σ) represents the convolution of the image point and the Gaussian second-order differential convolution, L _yy (x,σ) represents the convolution of the image point and the Gaussian second-order differential convolution.

The box filter structure is used to approximate the Gaussian second-order differential, and the Hessian value of the selected feature point and its surrounding points is calculated. Combined with the integral graph, fast calculation can be performed. The discriminant is:

Det(Hessian(x,σ))＝D _xx ×D _yy -(0.9D _xy ) ² (9)

In the formula, Det(Hessian(x,σ)) represents the determinant of the Hessian matrix, and _Dxx , _Dyy , and _Dxy are _Lxx (x,σ), _Lyy (x,σ), and _Lxy (x,σ) obtained by the box filter, respectively. If the determinant of the Hessian matrix is positive and the pixel has the largest eigenvalue among the nearby pixels, it is identified as a feature point. The weighting coefficient of 0.9 is to balance the approximation error of the box filter.

The feature point sets of the two images extracted by the front monocular array camera 2 and the rear monocular array camera 4 are recorded as N ₁ and N _2. Find the feature point a in N ₁ and the feature point b and feature point c with the smallest and second smallest Euclidean distances in N _2. The Euclidean distances of feature point a and feature point b, and the Euclidean distances of feature point a and feature point c are d ₁ and d ₂ respectively. If their ratio is less than the threshold, feature point a and feature point b are determined to be a matching point pair, recorded as (a, b), and the determination formula is as follows:

The same process is followed to obtain the set of matching point pairs of the two images.

(3) Improve the RANSAC algorithm to eliminate false matches

First, the currently available improved RANSAC algorithm is used to find matching point pairs in which the number of intersections between any feature point and other matching feature points is greater than 3 from the above matching point pair set, and these matching point pairs are determined to be wrong matching pairs, and the wrong matching pairs are eliminated. The Euclidean distance ratios _ri of the remaining matching point pairs are calculated, and _ri is sorted in ascending order to form a new matching point pair set. The set is then evenly divided into four groups, which can ensure that there are no less than five matching point pairs in the first group. Four matching point pairs are randomly selected from the first group as the determination of an optimal model. If none of the four selected matching point pairs are inliers, the probability of this group of models being the optimal model is very small. At this time, this group is discarded and other matching point pairs are not determined; if two matching point pairs in the first group are found to be inliers, the remaining matching point pairs in this group are determined as inliers. The same operation is performed on the remaining groups, and the group with the largest number of inliers is selected as the optimal model.

(4) Image positioning

The monocular area array camera is calibrated using Zhang Zhengyou's calibration method to obtain the internal and external parameters of the area array camera, and then the conversion relationship between the pixel coordinate system and the world coordinate system of the feature point is obtained as follows:

In the formula, _fx = f/dx, _fy = f/dy, _fx and _fy represent the scale factors of the horizontal and vertical axes of the image, _cx and _cy represent the coordinates of the origin of the image coordinate system in the pixel coordinate system, _Zc is a constant, u and v represent the horizontal and vertical coordinate values of the feature point in the pixel coordinate system, R and t are external parameters, _Xw , _Yw and _Zw represent the three-dimensional coordinates in the world coordinate system, M is the projection matrix, _M1 and _M2 are determined by the internal and external parameters obtained by the calibration of the area array camera, is the coordinate value of the feature point in the world coordinate system. In _M1 , _fx , _fy , _cx and _cy are only related to the internal structure of the monocular array camera, and these parameters are called the internal parameters of the monocular array camera; _M2 is only determined by the orientation of the monocular array camera with respect to the world coordinate system, and is called the external parameters of the monocular array camera. _M1 and _M2 are determined by the internal parameters and external parameters obtained by the calibration of the monocular array camera. M is the projection matrix. is the coordinate value of the feature point in the world coordinate system.

The endpoint of the marking line and the center point of the bottom of the monocular camera are selected as feature points. After coordinate transformation, the world coordinates of the endpoint of the marking line and the center point of the bottom of the monocular camera extracted by the front monocular array camera 2 are (X ₁ ,Y ₁ ) and (X ₂ ,Y ₂ ), respectively. The world coordinates of the endpoint of the marking line and the center point of the bottom of the monocular camera extracted by the rear monocular array camera 4 are (X ₁ ′,Y ₁ ′) and (X ₂ ′,Y ₂ ′), respectively. Then, the distance between the tunnel inspection vehicle 1 and the marking line captured by the front monocular array camera 2 can be obtained, which is recorded as L ₁ . Similarly, the distance between the tunnel inspection vehicle 1 and the marking line captured by the rear monocular array camera 4 can be obtained, which is recorded as L ₂ .

When the tunnel inspection vehicle 1 is two-dimensionally positioned, the front monocular array camera 2 installed on the top of the vehicle is used to shoot the marking line, and the yaw angle of the front monocular array camera 2 is calculated. The tunnel inspection vehicle 1 moves forward, and the rear monocular array camera 4 installed on the roof is used to shoot the original road section again, and the yaw angle of the rear monocular array camera 2 is calculated, as shown in FIG4 .

Where _x1 and _x2 are the vertical distances from the front monocular array camera 2 and the rear monocular array camera 4 to the marking line respectively. If the tunnel inspection vehicle 1 does not deflect on the road section, the yaw angles of the front and rear ends of the tunnel inspection vehicle 1 are equal; if the tunnel inspection vehicle 1 deflects on the road section, the difference between the yaw angles of the front and rear ends of the tunnel inspection vehicle is the yaw angle of the vehicle body on the road section.

In the formula, are the yaw angles of the front and rear ends of the tunnel inspection vehicle 1 respectively.

The encoder installed on the wheel output shaft of the tunnel inspection vehicle 1 can obtain the position of the tunnel inspection vehicle 1. By fusing the encoder position with the yaw angle, the position of the tunnel inspection vehicle 1 at the corresponding angle can be obtained, thus realizing the linear positioning of the tunnel inspection vehicle based on the tunnel marking line. The recursive algorithm is as follows:

Wherein, τ∈(t _k ,t _k+1 ), v(τ) is the speed of the tunnel detection vehicle 1 in the driving direction output by the encoder at different times τ, v _x represents the component of the speed of the tunnel detection vehicle 1 along the _x2 direction of the earth coordinate system, v _y represents the component of the speed of the tunnel detection vehicle 1 along the _y2 direction of the earth coordinate system, x(t _k+1 ) and y(t _k+1 ) represent the positions of the tunnel detection vehicle 1 at time k+1, respectively. is the yaw angle of the tunnel inspection vehicle 1 at different times τ calculated based on the tunnel marking line. When k=0, x(t ₀ ) and y(t ₀ ) are the initial positions of the tunnel inspection vehicle 1 at the initial moment of entering the tunnel. This position information is provided by GPS.

The motion trajectory of the tunnel inspection vehicle 1 in the tunnel is the point set of the coordinate values of the tunnel inspection vehicle 1 at different times, namely:

s={(x(t ₀ ),y(t ₀ )),(x(t ₁ ),y(t ₁ )),…,(x(t _k ),y(t _k )),(x( t _k+1 ),y(t _k+1 ))} (16)

In addition, due to wear and tear or road design factors, some sections of the tunnel may have no marking lines. Therefore, the present invention cannot locate the lateral position of the tunnel detection vehicle through images under the condition of short-distance unmarked roads. The white dashed and solid lines on the highway are called lane dividing lines, and their standard length is 6 meters and 9 meters apart. When two consecutive dashed and solid lines in the center of the road are missing, the short distance is 15 meters, so the default is the lateral position at the previous moment. If this distance is exceeded, the technical solution of the present invention is not applicable.

Take the moment t ₀ before the tunnel inspection vehicle 1 enters the tunnel as an example. The coordinate position of the tunnel inspection vehicle 1 at this time is (x(t ₀ ), y(t ₀ )). If the tunnel inspection vehicle does not deviate in this section, substituting it into equation (15), we have

If the tunnel inspection vehicle 1 deviates between time t ₁ and _{t 2} , then

Third, integrated positioning

Image positioning and strapdown inertial navigation system positioning are two independent systems. Before realizing fusion, the coordinate system should be unified first, that is, coordinate conversion between coordinate system b and earth coordinate system. The conversion relationship is as follows:

Wherein, θ, γ, ψ are the pitch angle, roll angle and yaw angle of the tunnel inspection vehicle 1 respectively; (λ, L) are the longitude and latitude of the tunnel inspection vehicle 1 respectively.

The image positioning trajectory based on the tunnel marking line and the positioning trajectory based on the strapdown inertial navigation system (i.e. the converted latitude and longitude values) are displayed in a trajectory diagram. The trajectory diagram shows the path of the tunnel inspection vehicle 1 at each moment and the real-time coordinate information of the corresponding points. In theory, the two should be able to maintain complete trajectory overlap, but the strapdown inertial navigation error accumulation is more serious. While relying solely on image positioning can have higher accuracy, the positioning accuracy is reduced because the monocular array camera is easily affected by light and the tunnel inspection vehicle 1 itself shakes. Based on this point, the fusion of image and strapdown inertial navigation can be used to achieve comprehensive positioning.

As shown in Figure 8, by comparing the real-time position data of the tunnel inspection vehicle 1 in the trajectory diagram, the set error value x is used to determine whether fusion positioning is required. When the absolute value of the difference between any coordinate value (x, y) of the tunnel inspection vehicle 1 in the two diagrams exceeds the set error value x = 0.2m, it is determined that the strapdown inertial navigation positioning error is large at this moment. At this time, the positioning value of the image is used as compensation for fusion positioning. The Kalman filter is used to fuse the two. First, the noise variance of the inertial navigation positioning (that is, the converted latitude and longitude values) is solved. Secondly, solve the noise variance of x and y in the motion trajectory of image positioning Solve the relationship between the current position Pos(k) of the tunnel inspection vehicle 1 and the positioning position Pos ₁ (k) corresponding to the converted latitude and longitude values and the positioning position Pos ₂ (k) corresponding to x and y in the motion trajectory of image positioning Then, the fused position parameters can be obtained to further locate the tunnel inspection vehicle. If the difference between any coordinate values of the tunnel inspection vehicle 1 in the two images does not exceed the error value of 0.2m, it is determined that the inertial navigation positioning is accurate at this moment, and the inertial navigation positioning data is output to locate the tunnel inspection vehicle.

Claims (10)

1. A method for positioning a tunnel inspection vehicle based on a strapdown inertial navigation system and image positioning, characterized in that it comprises the following steps: S1, first using an accelerometer to obtain a position update differential equation of the tunnel inspection vehicle, and then solving the position update differential equation to obtain the latitude and longitude of the tunnel inspection vehicle on the earth's surface; The front monocular array camera of the tunnel inspection vehicle first shoots the discontinuous dotted lines between the lanes in the tunnel, and the rear monocular array camera of the tunnel inspection vehicle then shoots the discontinuous dotted lines between the lanes in the tunnel. The discontinuous dotted lines shot twice are the same. Then, the yaw angle of the tunnel inspection vehicle is solved with the endpoint of the identification line and the center point of the bottom of the monocular array camera as feature points. The obtained yaw angle and the position of the encoder are fused and processed. Then, combined with the initial position of the tunnel inspection vehicle entering the tunnel, the motion trajectory of the tunnel inspection vehicle in the tunnel in the earth coordinate system is obtained. The motion trajectory is a set of X and Y points, where X corresponds to the value of the tunnel inspection vehicle in the x direction in the earth coordinate system, and Y corresponds to the value of the tunnel inspection vehicle in the y direction in the earth coordinate system. S2, convert the latitude of the tunnel inspection vehicle on the earth's surface into a numerical value in the earth's coordinate system and record it as M, compare M with X in the motion trajectory described in S1, and record the absolute value obtained as O, convert the longitude of the tunnel inspection vehicle on the earth's surface into a numerical value in the earth's coordinate system and record it as N, compare N with Y in the motion trajectory described in S1, and record the absolute value obtained as P, when O and P are both within the set error value, use M and N to locate the tunnel inspection vehicle, when at least one of O and P is greater than the set error value, M and N, as well as X and Y in the motion trajectory described in S1 are fused in the Kalman filter, and the fused value is used to locate the tunnel inspection vehicle. 2. The tunnel inspection vehicle positioning method based on strapdown inertial navigation system and image positioning according to claim 1 is characterized in that the position update differential equation described in S1 is as follows: Wherein, L is the latitude of the tunnel inspection vehicle on the earth's surface, v _y is the velocity component of the tunnel inspection vehicle in the y ₁ direction in the coordinate system b, _RM is the radius of the earth's meridian corresponding to the point where the tunnel inspection vehicle is located, and h is the altitude; λ is the longitude of the tunnel inspection vehicle on the earth's surface, _vx is the velocity component of the tunnel inspection vehicle in the x ₁ direction in the coordinate system b, and _RN is the radius of curvature of the meridian corresponding to the point where the tunnel inspection vehicle is located; In coordinate system b, with the center of mass of the tunnel inspection vehicle as the origin, the rightward direction along the axis of the tunnel inspection vehicle is the positive direction of the _x1 axis, and the forward direction along the perpendicular _x1 axis is the positive direction of the _y1 axis. 3. The method for positioning a tunnel inspection vehicle based on a strapdown inertial navigation system and image positioning according to claim 1 is characterized in that the front-end monocular array camera and the rear-end monocular array camera in S1 extract feature point sets of two identical images, which are respectively recorded as N ₁ and N ₂ , and then find out the feature points b and c with the smallest and second smallest Euclidean distances between any feature point a in N ₁ and N ₂ , and the Euclidean distances between feature point a and feature point b and feature point c are d ₁ and d ₂ respectively; if the ratio of d ₁ and d ₂ is less than a threshold value, the feature point a and feature point b are determined to be a matching point pair, which is recorded as (a, b), and the matching point pair sets of the two images are obtained according to the same process, and then the mismatches in the matching point pair set are eliminated, and then the endpoints of the identification line are selected as feature points. 4. The method for positioning a tunnel inspection vehicle based on a strapdown inertial navigation system and image positioning according to claim 3 is characterized in that, from the set of matching point pairs, matching point pairs in which the number of intersection points between any one feature point and other matching feature points is greater than 3 are found, these matching point pairs are determined to be wrong matching pairs, and the wrong matching pairs are eliminated, and then the Euclidean distance ratios _ri of the remaining matching point pairs are calculated, and _ri is sorted in ascending order to form a new set of matching point pairs, and then the new set of matching point pairs is evenly divided into four groups, and four pairs of matching point pairs are arbitrarily selected from the first group. If the four pairs of matching point pairs are not all inliers, the first group is discarded and the remaining matching point pairs are not determined. If two pairs of matching point pairs in the first group are inliers, the remaining matching point pairs in the first group are determined as inliers, and the same operation is performed on the remaining groups, and the group with the largest number of inliers is selected as the optimal model to complete the elimination of wrong matches in the set of matching point pairs. 5. The method for positioning a tunnel inspection vehicle based on a strapdown inertial navigation system and image positioning according to claim 1 is characterized in that, S1 transforms the endpoint of the marking line extracted by the front monocular array camera and the center point of the bottom of the front monocular array camera into a world coordinate system, which are recorded as (X ₁ , Y ₁ ) and (X ₂ , Y ₂ ) respectively; transforms the endpoint of the marking line extracted by the rear monocular array camera and the center point of the bottom of the rear monocular array camera into a world coordinate system, which are recorded as (X ₁ ′, Y ₁ ′) and (X ₂ ′, Y ₂ ′) respectively; the distance L ₁ from the front monocular array camera to the endpoint of the marking line and the distance L ₂ from the rear monocular array camera to the endpoint of the marking line are obtained by the following formula: The yaw angle of the front monocular array camera and the yaw angle of the rear monocular array camera are expressed by the following formulas respectively: Where _x1 and _x2 are the vertical distances from the front monocular array camera and the rear monocular array camera to the marking line, respectively. The yaw angle of the tunnel inspection vehicle is expressed as follows: 6. The method for positioning a tunnel inspection vehicle based on a strapdown inertial navigation system and image positioning according to claim 1 is characterized in that S1 uses a recursive algorithm to fuse the obtained yaw angle and the position of the encoder, and the specific formula is as follows; Where τ∈(t _k ,t _k+1 ), v(τ) is the speed of the tunnel detection vehicle in the driving direction output by the encoder at different times τ, v _x represents the speed component of the tunnel detection vehicle along the x direction of the earth coordinate system, v _y represents the speed component of the tunnel detection vehicle along the y direction of the earth coordinate system, x(t _k+1 ) and y(t _k+1 ) represent the positions of the tunnel detection vehicle at time k+1, respectively. is the yaw angle of the tunnel inspection vehicle at different times τ, and the coordinate value point set of the tunnel inspection vehicle at different times is the motion trajectory of the tunnel inspection vehicle in the tunnel, which is specifically expressed as the following formula: s = {(x(t ₀ ), y(t ₀ )), (x(t ₁ ), y(t ₁ )), … , (x(t _k ), y(t _k )), (x(t _k+1 ), y(t _k+1 ))}. 7. The method for positioning a tunnel inspection vehicle based on a strapdown inertial navigation system and image positioning according to claim 6, characterized in that, when k=0, x( _t0 ) and y( _t0 ) are respectively the initial positions of the tunnel inspection vehicle when it enters the tunnel, and the initial positions are provided by GPS. 8. The method for positioning a tunnel inspection vehicle based on a strapdown inertial navigation system and image positioning according to claim 2 is characterized in that S2 uses the following formula when converting the latitude and longitude of the tunnel inspection vehicle on the earth's surface into values in the earth's coordinate system: Among them, the earth coordinate system is denoted as coordinate system e; Where: θ, γ, ψ are the pitch angle, roll angle and yaw angle of the tunnel inspection vehicle, respectively; λ and L are the longitude and latitude of the tunnel inspection vehicle, respectively. 9. The method for positioning a tunnel inspection vehicle based on a strapdown inertial navigation system and image positioning according to claim 8, characterized in that the pitch angle, roll angle and yaw angle of the tunnel inspection vehicle are obtained according to the following process: First, define the coordinate system n, with the center of mass of the tunnel inspection vehicle as the origin, the east as the positive direction of the _x2 axis, the north as the positive direction of the _y2 axis, and the vertical upward direction as the positive direction of the _z2 axis. Then define the vector r ^b in the coordinate system b, r ^b = xi + yi + zk, and convert it to the vector in the coordinate system n as r ⁿ , and then define the quaternion Using quaternion operation rules, r ⁿ can be expressed as follows: Further we can get the following formula: The attitude position matrix from coordinate system b to coordinate system n It is expressed as the following formula: Expressed as q ₀ , q ₁ , q ₂ , q ₃ are all real numbers, i, j and k are all direction vectors, yes The conjugate matrix of ; and The relationship is expressed in matrix form as follows: In the formula, is the angular velocity of the tunnel inspection vehicle in coordinate system b measured by the MEMS gyroscope, They are The angular velocity around the x1 _axis , _y1 axis, and _z1 axis, with the positive direction of the _{z1 axis perpendicular to the x1 axis} ; in Output the angular momentum of the tunnel inspection vehicle for the MEMS gyro, is the Earth's rotational velocity, is the angular velocity of the geographic coordinate system relative to the earth, yes The transposed matrix of right and The relationship is calculated using the quaternion Picard algorithm, and the following result is obtained: in: Further Taylor series expansion is performed to obtain the attitude position matrix shown in the following formula: Further use T with different double subscripts to represent Posture position matrix Denoted as: Finally, we get: 10. The method for positioning a tunnel inspection vehicle based on a strapdown inertial navigation system and image positioning according to claim 1, characterized in that when S2 uses a Kalman filter to fuse M and N, and X and Y in the motion trajectory described in S1, the following process is adopted: First solve the noise variance of M and N And the noise variance of X and Y in the motion trajectory described in S1 The relationship between the current position Pos(k) of the tunnel inspection vehicle, the position Pos ₁ (k) corresponding to M and N, and the position Pos ₂ (k) corresponding to X and Y in the motion trajectory of S1 is as follows: The current position Pos(k) of the tunnel detection vehicle is used to obtain the fused value.