CN109631887B - Inertial navigation high-precision positioning method based on binocular, acceleration and gyroscope - Google Patents

Abstract

The present invention claims to protect a high-precision positioning method of inertial navigation based on binocular, acceleration and gyroscope. The present invention is designed to determine the three-dimensional coordinates of the feature point of the inertial navigation reference position based on binocular positioning + high-precision GPS information, and the coordinate value of the feature point is used as the The reference point is used to update the three-dimensional coordinates of inertial navigation in real time to eliminate the cumulative error caused by the integration of inertial navigation, and realize high-precision positioning of vehicles based on inertial navigation and binocular positioning combined with auxiliary indoor parking lots. The structural block diagram of the present invention includes the following parts: 1. the composition of the high-precision inertial navigation system and the structure of the reference coordinate system; 2. the acquisition method of the initial coordinates of the inertial navigation and navigation; 3. the selection method of the binocular high-precision positioning reference point; 4. . The selection and design of the high-precision inertial navigation coordinate system; 5. The conversion method of the high-precision inertial navigation coordinate system; 6. The elimination method of the accumulated error of the inertial navigation.

Description

High-precision positioning method of inertial navigation based on binocular, acceleration and gyroscope

technical field

The invention belongs to the field of high-precision positioning of integrated navigation, and in particular relates to a high-precision positioning method for visual navigation aided inertial navigation based on indoor parking.

Background technique

As a popular positioning method, GPS has been widely used in terrestrial vehicle positioning, but the signal of GPS for indoor parking is often blocked. Indoor high-precision positioning is helpful for vehicles to quickly find suitable parking spaces through navigation routes. In the current situation where many indoor parking lots are distributed, this technology is very necessary.

The Inertial Navigation System (INS) uses Newton's laws of mechanics as its working principle, and uses the inertial measurement unit (IMU) installed on the carrier to measure the angular velocity and acceleration information of the carrier, and obtain the position of the carrier through integral operation. , speed and attitude and other navigation parameters. Inertial navigation has complete autonomy and anti-interference, and has high accuracy in a short time, but due to the principle of integration, its errors accumulate over time, so the accuracy of long-term work is poor. In order to reduce the accumulation of errors, external observations should be introduced to correct the inertial navigation system.

A common form of vision-aided inertial navigation system (INS) combines a monocular or binocular camera system with a low-cost IMU, which is widely used in robotics and autonomous vehicle navigation. This method can reduce the positioning error caused by the uncertainty of visual feature extraction in space to a large extent, reduce the drift phenomenon, and overcome the error accumulated in the inertial measurement positioning. The visual aided inertial navigation method mainly uses the forward-looking camera to capture prominent features, and uses various features to assist the navigation of man-made buildings. This paper utilizes the 3D features obtained by the binocular vision system; therefore, the method of combining the binocular vision system with the inertial navigation system can be applied to environments where angular features are common.

Nowadays, there are many data fusion algorithms for visual navigation and inertial navigation, but these fusion algorithms can not eliminate the accumulated error of inertial navigation very well, and the related algorithms are relatively complex.

SUMMARY OF THE INVENTION

The present invention aims to solve the above problems of the prior art. A method is proposed to eliminate the accumulated error caused by the integration of inertial navigation, which greatly improves the accuracy, speed and accuracy of the matching, and the stability and robustness of the matching are also greatly improved, and the real-time performance is good when updating the position information of the inertial navigation. The high-precision positioning method of inertial navigation based on binocular, acceleration and gyroscope. The technical scheme of the present invention is as follows:

A high-precision positioning method for inertial navigation based on binocular, acceleration and gyroscope, which comprises the following steps:

Step 1. Establish a high-precision inertial system based on an inertial navigation module, a binocular stereo vision system and a vehicle, and establish a reference coordinate system structure;

Step 2. Obtaining the starting coordinates of inertial navigation: The vehicle uses the starting coordinate point as the starting point for inertial navigation, and adopts the improved stereo matching method based on the SIFT algorithm, and selects according to the positional relationship of the points with significant corner features in the image. The area of interest, the vehicle completes the positioning and tracking of the area of interest through continuous position accumulation during the driving process. In the subsequent process, only the area of interest can be image-matched to complete the feature point extraction of binocular vision at the initial moment. , to achieve the purpose of high matching accuracy and fast speed;

Step 3. Selection of the reference point for binocular high-precision positioning: select the corner or corner with significant corner features in the indoor parking lot as the reference point for binocular high-precision positioning, and use the reference point as the origin to establish a reference coordinate system;

Step 4. Selection and design of high-precision inertial navigation coordinate system: select the coordinate system including the earth coordinate system, navigation coordinate system, carrier coordinate system, camera coordinate system, and world coordinate system in the navigation process, so as to satisfy the final pair of relevant coordinates. the solution;

Step 5. Conversion of high-precision inertial navigation coordinate system: obtain the position information of the feature point in the world coordinate system, the position information of the vehicle in the inertial navigation and navigation coordinate system, and the conversion relationship between the world coordinate system and the inertial navigation and navigation coordinate system ;

Step 6. Elimination of accumulated errors of inertial navigation: Determine the three-dimensional coordinates of the inertial navigation reference position feature points based on binocular positioning and high-precision GPS information, and use the feature point coordinate values as reference points to update the three-dimensional inertial navigation coordinates in real time to eliminate inertial navigation due to The accumulated error generated by the integration realizes the precise positioning of vehicles based on inertial navigation and binocular positioning combined with auxiliary indoor parking lot.

Further, the inertial navigation module of the step 1 is composed of a three-axis acceleration sensor, a three-axis gyroscope and a three-axis magnetometer, and the parameters of each sensor are obtained by measurement, and the coordinates in the inertial navigation system are obtained through quaternion algorithm calculation. The position parameters of the carrier equipped with the inertial navigation module are obtained through the speed integration and position integration, and the image information of binocular vision is used to determine the three-dimensional coordinates of the inertial navigation reference position feature point through the visual correlation algorithm. The difference between the coordinate values of the feature points is used as a reference point to update the three-dimensional coordinates of the inertial navigation in real time, so as to eliminate the accumulated error caused by the integration of the inertial navigation.

Further, the acquisition of the starting coordinate point in step 2 specifically includes: when entering an indoor garage with no GPS signal or a very weak GPS signal, the vehicle judges that the binocular on the vehicle at the starting point is based on the last valid GPS information obtained. Whether the stereo vision system can obtain the position information of the characteristic corner points, interpolate the position information with the delay time of this process and the speed and heading angle information of the vehicle, and obtain the starting coordinates of inertial navigation. The starting coordinates of the vehicle are Initial position for inertial navigation.

Further, the step 2 of the improved stereo matching method based on the SIFT algorithm specifically includes: first, by performing epipolar constraints on the collected left and right images, secondly, selecting the ROI (Region of Interest) region from the left image, and then reducing the dimension of the feature vector. To reduce the time-consuming, the BBF (Best Bin First) algorithm based on KD tree is used to speed up the matching speed, and finally the RANSAC (Random Sample Consensus) algorithm is used to remove the mismatch.

Further, the step 3 uses the characteristics of the corner points, that is, the corner points are the local maximum points of curvature on the target contour, which are the decisive features of the object contour, and the corner points also have rotation invariance, when the vehicle enters the room from the garage. After the parking lot, the binocular camera on the vehicle selects the nearest object with significant corner features in the driving direction of the vehicle as the reference object for binocular positioning to extract feature points, and uses the acquired corner as the binocular high-precision positioning reference point.

Further, the step 5 also includes conversion between the following coordinate systems:

First, the conversion of the carrier coordinate system and the navigation coordinate system: the attitude conversion of the carrier in space is analogous to the carrier coordinate system b to the navigation coordinate system n after three basic rotation transformations around the pitch axis, roll axis and heading axis:

Second, the conversion between the camera coordinate system and the world coordinate system: select the world coordinate system to coincide with the navigation coordinate system, and the coordinates (x _c , y _c , z _c ) of the spatial feature points in the camera coordinate system are converted to obtain spatial features The coordinates of the point in the world coordinate system (x _w , y _w , z _w ), that is, the coordinate points (x _n , y _n , z _n ) of the spatial feature point in the navigation coordinate system are obtained:

来表示。Third, the conversion between the earth coordinate system and the navigation coordinate system: firstly, rotate the earth coordinate system O _e x _e y _e z _e around the z _e axis by an angle of λ to obtain O _e1 -x _e1 y _e1 z _e1 . Then rotate 90°-L around the y _e1 axis to get O _e2 -x _e2 y _e2 z _e2 . Finally, rotate 90° around the z _e2 axis. After the above steps, the earth coordinate system O _e -x _e y _e z _e can be converted to the navigation coordinate system O _n -x _n y _n z _n , the mutual relationship between the two coordinate systems The transformation of can be done by the matrix

To represent.

Further, the coordinates (x _c , y _c , z _c ) of the spatial feature points in the camera coordinate system are converted to obtain the coordinates (x _w , y _w , z _w ) of the spatial feature points in the world coordinate system, specifically include:

其中R表示3×3的旋转矩阵，O^T＝(0，0，0)。The homogeneous coordinates of the spatial feature points in the world coordinate system and the camera coordinate system are (x _w , y _w , z _w , 1) and (x _c , y _c , z _c , 1) respectively, where the homogeneous coordinate system is A vector of dimension n is represented by a vector of n+1 dimension, and the coordinate value of the origin of the world coordinate system in the camera coordinate system is t=(t _x , _ty , t _z ) ^T , this 3-dimensional vector is the translation vector, and the direction vector R is the direction vector used by the camera coordinate system to describe the relationship between the two in the world coordinate system. Combining the sine and cosine relationships of the three attitude angles to form a 3×3 orthogonal matrix, the world coordinate system and The relationship between the camera coordinate systems is

where R represents a 3×3 rotation matrix, O ^T =(0, 0, 0).

Further, the elimination of the accumulated error of the inertial navigation in the step 6: the coordinates of the starting point of the inertial navigation are determined by the binocular stereo vision positioning and high-precision GPS information, and the inertial navigation is turned on when the vehicle enters an area with no GPS signal or a very weak GPS signal. In the mode, the binocular stereo vision system is used to obtain the position information of the corners or inflection points with significant corner features, and the position information of the feature points is used as the reference point to establish a three-dimensional reference coordinate system, and the position of the same reference point obtained by the vehicle at different positions at different times is used. information, and update the vehicle position through the difference between the coordinate transformation and the vehicle position at the adjacent time. During the driving process, the vehicle chooses whether to replace the reference object and the reference point by judging the distance of the reference point and the set minimum distance threshold of the reference point. Finally, the precise positioning of vehicles based on inertial navigation and binocular positioning combined with auxiliary indoor parking lot is realized.

The advantages and beneficial effects of the present invention are as follows:

The present invention is a high-precision positioning method of inertial navigation based on binocular, acceleration and gyroscope. The binocular vision navigation system is used to obtain the position information of reference corner points to update the navigation information of inertial navigation, and finally high-precision positioning of indoor vehicles is realized. . The error generated by the system in the present invention is related to the accuracy of binocular positioning, not inertial navigation. In the process of inertial navigation, the closer the vehicle is to the reference point, the higher the accuracy of binocular positioning, that is, the accuracy of the entire system is higher. The higher it is, the reference point is continuously updated by setting the distance threshold, so as to eliminate the accumulated error of inertial navigation due to integration.

Second, the method of the present invention is simple and reliable, with simple tools and procedures, and the improved stereo matching method of the binocular vision navigation system based on the SIFT algorithm greatly improves the accuracy, speed and accuracy of matching, as well as the stability and robustness of matching. The performance has also been greatly improved, and the real-time performance is good when updating the inertial navigation position information.

To sum up, the present invention uses the position interpolation algorithm to obtain the initial coordinates of the inertial navigation, obtains the position vector indoors through the inertial navigation of acceleration and gyroscope, and uses the binocular visual data to update the position information of the inertial navigation in the indoor navigation, and finally realizes the The reconstruction of the motion trajectory of the indoor vehicle, so that the indoor parking lot vehicle can accurately arrive at the parking position through the navigation information.

Description of drawings

FIG. 1 is a general structural block diagram of an inertial navigation high-precision positioning method according to a preferred embodiment of the present invention.

FIG. 2 is a schematic diagram of the composition of the high-precision inertial navigation system and the structure of the reference coordinate system of the present invention.

FIG. 3 is a structural block diagram of the inertial navigation coordinate update based on binocular vision of the present invention.

FIG. 4 is a schematic diagram of the dynamic coordinate system transformation of the reference point of the high-precision inertial navigation system of the present invention.

FIG. 5 is a schematic plan view of a vehicle parking in an indoor parking lot according to the present invention.

FIG. 6 is a schematic diagram of the steps of the improved stereo matching method based on the SIFT algorithm.

Description of reference numbers:

①—Measurement vehicle equipped with nine-axis inertial navigation measurement device, binocular camera and computer;

②—Navigation coordinate N; ③—Indoor garage entry point;

④—carrier coordinate system; ⑤—camera coordinate system;

⑥—The position of the vehicle at the next moment; ⑦—Objects with significant corner features of the indoor parking lot;

⑧—reference corner;

⑨—The position of the first feature point obtained by the binocular camera at time t ₀ ;

⑩—The position of the first feature point obtained by the binocular camera at time _t1 ;

a—measurement vehicle equipped with nine-axis inertial navigation measurement device, binocular camera and computer;

b—reference point corners of buildings with significant corner characteristics;

c—The navigation route displayed on the high-precision map of the indoor garage after the owner has selected the parking space;

d—The parking location selected by the owner.

Detailed ways

The technical solutions in the embodiments of the present invention will be described clearly and in detail below with reference to the accompanying drawings in the embodiments of the present invention. The described embodiments are only some of the embodiments of the invention.

The technical scheme that the present invention solves the above-mentioned technical problems is:

The object of the present invention can be realized through the following technical solutions:

(1) Composition and reference coordinate system structure of high-precision inertial navigation system

Figure 2 is a schematic diagram of the composition of the high-precision inertial navigation system and the structure of the reference coordinate system of the present invention, and ① in the figure is a vehicle equipped with a nine-axis inertial navigation measurement device, a binocular camera and a computer for measurement. The indoor parking lot scene of object ⑦, where the high-precision map of the indoor parking lot has been obtained. The coordinate system O _e -x _e y _{e ze} is the earth coordinate system, which is used in FIG. 2 to illustrate the idea of the present invention, and is not the actual position of the earth coordinate. The coordinates (λ ₀ , L ₀ , H ₀ ) in the figure are the coordinate information obtained by the high-precision GPS at the entry point ③ of the indoor garage. Establish a coordinate system ②, that is, the navigation coordinate system O _n -x _n y _n z _n and the world coordinate system O _w -x _w y _w z _w . After the vehicle enters the indoor parking lot, the binocular stereo vision system uses the binocular camera to extract and locate the reference point ⑧, that is, the corner point of the first corner point of the building with significant features, and obtain the reference corner point in the camera coordinate system. The position parameter ⑨(x _w0 y _w0 z _w0 ) at the initial moment, at the same time, the inertial navigation measurement device ④ starts to obtain the position information of the vehicle in the navigation coordinate system, and ⑤ is the carrier coordinate system O _b -x _b y _b z _b . At the position of the vehicle at the next moment ⑥, the binocular stereo vision system obtains the position information of the feature point at this moment ⑩(x _w1 y _w1 z _w1 ), and uses the difference between the coordinate values of the feature points at the two moments as a reference point to real-time The position information of inertial navigation in the navigation coordinate system is updated, and the position information in the high-precision indoor parking lot map is obtained through the transformation of the earth coordinate system and the navigation coordinate system. In order to achieve the purpose of using the binocular stereo vision positioning coordinates to update the navigation position parameters of the inertial navigation system to eliminate the position error of the inertial navigation system.

Figure 3 is a structural block diagram of the inertial navigation coordinate update based on binocular vision, wherein the inertial navigation module is composed of a three-axis acceleration sensor, a three-axis gyroscope and a three-axis magnetometer. The parameters of each sensor are measured and obtained through the quaternion The algorithm solves the coordinate system transformation in the inertial navigation system, and obtains the position parameters of the carrier carrying the inertial navigation module through the speed integral and the position integral. Using the image information of binocular vision, the three-dimensional coordinates of the inertial navigation reference position feature points are determined by the visual correlation algorithm, and the three-dimensional coordinates of the inertial navigation are updated in real time by using the coordinate value of the feature point at the current moment as the reference point, so as to eliminate the accumulation of inertial navigation generated by integration. error.

(2) Acquisition method of inertial navigation navigation starting coordinates

According to the object scene of the present invention, before the vehicle enters the garage, obtain valid GPS information that can still be received, including the three-dimensional coordinates of the vehicle in the earth coordinate system of the garage entrance position, that is, use the high-precision GPS module to obtain the current longitude , latitude, altitude location information. The method for judging the starting point, according to the requirements of the invention, the important condition that the starting point of inertial navigation and navigation in the present invention needs to meet is that the binocular stereo vision system on the vehicle can obtain the position information of the feature point, that is, the vehicle can enter from the garage entrance. Obtain the location information of the first building corner with significant corner features. When entering an indoor garage with no GPS signal or very weak GPS signal, the vehicle uses the last valid GPS information as the initial position for inertial navigation. But in fact, after obtaining the last valid GPS information, due to environmental factors and the binocular vision system may not be able to obtain the location information of the feature points in time, there is a delay time. During this process, the vehicle continues to drive, and it is necessary to use the last acquired valid GPS information (λ′ ₀ , L′ ₀ , H′ ₀ ) as the basis to calculate the vehicle inertial navigation start according to the delay time and the vehicle’s speed and heading angle. Position coordinates (λ ₀ , L ₀ , H ₀ ) are interpolated. Assuming the delay time T _X , the exact position of the vehicle is calculated according to the speed and heading angle of the vehicle, that is, the error position (Δλ, ΔL, ΔH) of the vehicle within the interpolation delay time T _X , and the finally obtained inertial navigation starting position coordinates are: (λ ₀ , L ₀ , H ₀ )=(λ′ ₀ +Δλ, L′ ₀ +ΔL, H′ ₀ +ΔH). Considering that in the actual indoor parking lot, the process time for the binocular stereo vision system to obtain feature points after the vehicle enters the parking lot is generally not very long, that is, the corresponding delay time is very short, and the vehicle speed is very low, then the interpolation method is used to obtain The inertial navigation starting position coordinate error is very small.

(3) Selection method of binocular high-precision positioning reference point

Corner points are very important features of images, which play a very important role in the understanding and analysis of image graphics. Corner points can effectively reduce the amount of information data while retaining the important features of image graphics, make the information content very high, effectively improve the speed of calculation, and facilitate the reliable matching of images, making real-time processing possible. Corners play a very important role in 3D scene reconstruction, motion estimation, object tracking, object recognition, image registration and matching and other computer vision fields.

There are many options for selecting reference objects for binocular positioning of vehicles in indoor parking lots. The present invention selects corners or corners with significant corner features that are ubiquitous in indoor parking lots and are easy to identify and feature extraction as reference points for binocular high-precision positioning. When the vehicle enters the indoor parking lot from the garage, the binocular camera on the vehicle selects the nearest object with significant corner features in the driving direction of the vehicle as the reference object for binocular positioning to extract feature points. The improved stereo matching method of the SIFT algorithm extracts feature points, selects the closest point in the feature points as the reference point for binocular positioning, and obtains the three-dimensional coordinates of this point in the camera coordinate system. Error correction of inertial navigation is performed later using the coordinate information of this point.

(4) Selection and design of high-precision inertial navigation coordinate system

In the present invention, a navigation coordinate system in a high-precision positioning method of inertial navigation based on binocular, acceleration and gyroscope, as shown in Figure 2, consists of the following parts:

First, the earth coordinate system O _e -x _e y _e z _e , the earth coordinate system rotates synchronously with the earth, the z _e axis points to the regional rotation axis, the x _e axis points to the intersection of the prime meridian and the equator, and the y _e axis is at the equator In the plane, it forms a right-handed coordinate system with the x _e axis and the z _e axis;

Second, the navigation coordinate system O _n -x _n y _n z _n , its origin O coincides with the center of gravity of the carrier, that is, the vehicle, and the coordinate axes point to the north, east and sky directions respectively;

Third, the carrier coordinate system O _b -x _b y _b z _b , a coordinate system created with the center of gravity of the vehicle as the origin, where the x _b axis points to the front of the carrier, the y _b axis is along the longitudinal direction of the carrier, and the z _b axis points to the vertical axis of the carrier direction;

Fourth, the camera coordinate system O _c -x _c y _c z _c takes the optical center of the camera as the coordinate origin, the z _c axis is perpendicular to the plane of the image coordinate system, and the y _c axis is vertically downward;

Fifth, the world coordinate system O _w -x _w y _w z _w , the world coordinate system describes the relative positional relationship of objects in the actual environment, which associates the image pixel coordinate system with the physical coordinate system, the camera coordinate system and space objects, The selection of the world coordinate system in the present invention coincides with the navigation coordinate system in the inertial navigation system;

Also includes image pixel coordinate system and image physical coordinate system.

(5) Conversion method of high-precision inertial navigation coordinate system

An inertial navigation coordinate transformation method in an inertial navigation high-precision positioning method based on binocular, acceleration and gyroscope. The movement of making a rigid body rotate once around a fixed axis is called basic rotation. If the space coordinate system is analogous to a rigid body, then The angular position relationship between the two coordinate systems can be represented by multiple basic rotations, and the transformation matrix between the coordinate systems can be obtained by multiplying the basic rotation matrices of the rigid body.

First, the conversion between the carrier coordinate system and the navigation coordinate system: the attitude transformation of the carrier in space is analogous to the carrier coordinate system b to the navigation coordinate system n after three basic rotation transformations around the pitch axis, roll axis and heading axis:

其中O^T＝(0，0，0)；Second, the conversion between the camera coordinate system and the world coordinate system: according to the requirements of the invention and convenient calculation, in the present invention, the world coordinate system and the navigation coordinate system are selected to overlap, as shown in FIG. 2 . The coordinates (x _c , y _c , z _c ) of the spatial feature points in the camera coordinate system are transformed to obtain the coordinates (x _w , y _w , z _w ) of the spatial feature points in the world coordinate system, that is, the spatial feature points are obtained. The coordinate point (x _n , y _n , z _n ) in the navigation coordinate system, the transformation process is: the homogeneous coordinates of the spatial feature point in the world coordinate system and the camera coordinate system are (x _w , y _w , z ) _w , 1) and (x _c , y _c , z _c , 1), in which the homogeneous coordinate system is to represent the vector of dimension n by the vector of (n+1) dimension, and the origin of the world coordinate system is at the camera. The coordinate value in the coordinate system is t=(t _x , _ty , t _z ) ^T , this 3-dimensional vector is the translation vector, and the direction vector R is the direction used by the camera coordinate system to describe the relationship between the two in the world coordinate system Vector, a 3×3 orthogonal matrix combined by the sine and cosine relationship of the three attitude angles, the relationship between the world coordinate system and the camera coordinate system is

where O ^T = (0, 0, 0);

来表示。Third, the conversion between the earth coordinate system and the navigation coordinate system: firstly, rotate the earth coordinate system O _e x _e y _e z _e around the z _e axis by an angle of λ to obtain O _e1 -x _e1 y _e1 z _e1 . Then rotate 90°-L around the y _e1 axis to get O _e2 -x _e2 y _e2 z _e2 . Finally, rotate 90° around the z _e2 axis. After the above steps, the earth coordinate system O _e -x _e y _e z _e can be converted to the navigation coordinate system O _n -x _n y _n z _n , and the mutual transformation between the two coordinate systems can be done through the matrix

To represent.

(6) Elimination method of inertial navigation accumulated error:

The premise of the navigation method based on the present invention is that there is a high-precision map of the indoor garage, and the relevant information of each parking space can be accurately displayed in the high-precision map of the indoor garage, including the position information of the parking space and whether it is an empty parking space. The system selects a suitable empty parking space to park its own vehicle. After the owner selects the parking space, the navigation route is displayed on the high-precision map of the indoor garage. The process of simulating parking is shown in Figure 5, and Figure 5 is a schematic diagram of a parking lot in an indoor parking lot. Among them, a is a vehicle equipped with inertial navigation sensors and binocular cameras, b is a building with significant corner characteristics distributed in the indoor parking lot, and c is the navigation route displayed on the high-precision map of the indoor garage after the car owner has selected a parking space , d is the parking location selected by the owner.

Figure 2 is a schematic diagram of the composition of the high-precision inertial navigation system and the structure of the reference coordinate system. The dynamic coordinate system is established with the reference corner ⑧ as the coordinate origin. Assume that at the moment t ₀ =0, the position information of the vehicle at the entrance of the garage obtained by high-precision GPS is (λ ₀ , L ₀ , H ₀ ), and at this moment, the binocular vision system detects the first corner with significant features The position of the characteristic corner point ⑧ of the building, and the coordinates of the corner point in the world coordinate system (x _w0 , y _w0 , z _w0 ) are calculated, then in the coordinate system established with the characteristic corner point as the coordinates, the vehicle is currently The position of is (-x _w0 , -y _w0 , -z _w0 ), and in the inertial navigation system at the current moment, the coordinates of the vehicle in the navigation coordinate system are (x _n0 , y _n0 , z _n0 ). After a period of time, at time t ₁ , the binocular vision system detects the position of the feature corner ⑧, and calculates the coordinates of the lower corner point in the world coordinate system at time t ₁ (x _w1 , y _w1 , z _w1 ) , in the coordinate system established with the characteristic corners as coordinates, the current position of the vehicle is (-x _w1 ,-y _w1 ,-z _w1 ), at this moment the inertial navigation system obtains the position of the vehicle (x _n1 ,y _n1 ,z _n1 ). The position information of the previous two times t ₀ and t ₁ is obtained, then the position information obtained by the binocular stereo vision system obtains the position information of the vehicle in the world coordinate system at time t _{1 at} time t 1, namely (x _w1 -x _w0 , y _w1 -y _w0 , z _w1 -z _w0 ). In the present invention, the selected world coordinate system and the navigation coordinate system are the same coordinate system, then the coordinates of the vehicle in the navigation coordinate system measured by the binocular vision system are (x _n1 ′, y _n1 ′, z _n1 ′)= (x _w1 -x _w0 , y _w1 -y _w0 , z _w1 -z _w0 ). The position information (x _n1 ', y _n1 ', z _n1 ') in the navigation coordinate system is used to replace the position information (x _n1 , y _n1 , z _n1 ) in the navigation coordinate system of the inertial navigation system vehicle at time t ₁ , that is, the updated The position information of the inertial navigation system vehicle in the navigation coordinate system, so as to eliminate the accumulated error of inertial navigation from time _t0 to time _t1 . Through the transformation of the earth coordinate system and the navigation coordinate system, the position information (λ ₁ , L ₁ , H ₁ ) of the vehicle in the high-precision map of the indoor parking lot is obtained. The vehicle continues to drive, and at time t ₂ , the position (λ ₁ , L ₁ , H ₁ ) in the high-precision map of the indoor parking lot is used as the starting point, and the relative earth is established with (λ ₁ , L ₁ , H ₁ ) as the origin Coordinate system, the same method is used to eliminate the accumulated error of the inertial navigation system at time t ₂ , and the position (λ ₂ , L ₂ , H ₂ ) of the vehicle in the high-precision map of the indoor parking lot at time t ₂ is also obtained. The method of eliminating the accumulated error of the inertial navigation system at the later time is the same as the method at the previous time, wherein the intervals of time t ₀ , t ₁ , and t ₂ are equal.

When the binocular stereo vision system detects the distance of the vehicle from the first building with significant corner features at time t _n , s _n < p, where p is a threshold set according to actual requirements, as shown in Figure 4 , the position information of the first corner detected by the binocular stereo vision system at time t _n is (x _wn , y _wn , z _wn ), and at this moment, the corner of the building with the next nearest corner point with significant features is obtained. Point location information, that is, the location information (x _wn ′, y _wn ′, z _wn ′) of the building with significant second corner features. The position information in the indoor high-precision map is obtained by updating the vehicle position information at the previous time t _n-1 , which is (λ _n , L _n , H _n ). The corner position information (x _wn+1 , y _wn+1 , z _wn+1 ) of the second building with significant corner features obtained by the binocular stereo vision system at time t _n+1 and the information obtained at the previous time Position information (x _wn ', y _wn ', z _wn '), update the position information (λ _n+1 , L _n+1 , H _n+1 ) of the vehicle at time t _n+1 . In the following moments, when the vehicle travels to a building with significant off-corner features less than a set threshold, the reference corner is replaced by the above algorithm. The error generated by the system is related to the accuracy of binocular positioning, and has nothing to do with inertial navigation. In the process of inertial navigation, the closer the vehicle is to the reference point, the higher the accuracy of binocular positioning, that is, the higher the accuracy of the entire system. In this way, the accumulated error of inertial navigation due to integration is eliminated.

The purpose of the present invention is to determine the three-dimensional coordinates of the inertial navigation reference position feature points based on binocular positioning + high-precision GPS information, and to update the three-dimensional coordinates of the inertial navigation in real time by using the feature point coordinate values as the reference points to eliminate the accumulated errors of the inertial navigation due to integration. , to achieve accurate positioning of vehicles based on inertial navigation and binocular positioning combined with auxiliary indoor parking lots. In order to achieve this purpose, the specific implementation manner is as follows:

According to the actual situation, when the vehicle enters the parking lot and completes the entire action process of parking, the present invention first provides a method for acquiring the starting coordinates of inertial navigation. In the present invention, the important condition that the starting point of inertial navigation and navigation needs to meet is that the binocular stereo vision system on the vehicle can obtain the position information of the feature point, that is, the position information of the first corner point can be obtained after the vehicle enters from the garage entrance. . When entering an indoor garage with no GPS signal or very weak GPS signal, the vehicle uses the last valid GPS information as the initial position for inertial navigation. But in fact, after obtaining the last valid GPS information, due to environmental factors and the binocular vision system may not be able to obtain the location information of the feature points in time, there is a delay time. During this process, the vehicle continues to drive, and it is necessary to use the last acquired valid GPS information (λ′ ₀ , L′ ₀ , H′ ₀ ) as the basis to calculate the start of the vehicle inertial navigation according to the delay time and the vehicle’s speed and heading angle. Position coordinates (λ ₀ , L ₀ , H ₀ ) are interpolated. Assuming the delay time T _X , the exact position of the vehicle is calculated according to the speed and heading angle of the vehicle, that is, the error position (Δλ, ΔL, ΔH) of the vehicle within the interpolation delay time T _X , and the finally obtained inertial navigation starting position coordinates are: (λ ₀ , L ₀ , H ₀ )=(λ′ ₀ +Δλ, L′ ₀ +ΔL, H′ ₀ +ΔH).

After the vehicle obtains the navigation starting point, the inertial navigation high-precision positioning method of the present invention needs to use the selected reference point to establish a relative coordinate system to achieve binocular high-precision positioning. The present invention provides a method for selecting a binocular high-precision positioning reference point.

While retaining the important features of image graphics, corner points can effectively reduce the amount of information data, make the information content very high, effectively improve the speed of calculation, and be conducive to reliable image matching, making real-time processing possible. There are many options for selecting reference objects for binocular positioning of vehicles in indoor parking lots. The present invention selects corners or corners with significant corner features that are ubiquitous in indoor parking lots and are easy to identify and feature extraction as reference points for binocular high-precision positioning. When the vehicle enters the indoor parking lot from the garage, the binocular camera on the vehicle selects the nearest object with significant corner features in the driving direction of the vehicle as the reference object for binocular positioning to extract feature points. The improved stereo matching method of the SIFT algorithm extracts feature points, selects the closest point in the feature points as the reference point for binocular positioning, and obtains the three-dimensional coordinates of this point in the camera coordinate system. Error correction of inertial navigation is performed later using the coordinate information of this point.

Next, the binocular stereo vision system extracts the feature points of the surrounding environment and serves as the reference point for the accurate positioning of the binocular in the present invention, and the extraction and positioning method of the binocular positioning feature point. The present invention applies an improved SIFT matching method based on indoor parking, that is, according to the positional relationship of the wall corner in the image, the ROI (Region of Interest) region is defined autonomously, that is, the matching is performed in the way of the region of interest, and the vehicle travels by accumulating the position , can complete the positioning and tracking of the ROI area. After that, only the images in the ROI area can be matched, which greatly improves the accuracy, speed and accuracy of the matching, and also greatly improves the stability and robustness of the matching. An improved stereo matching method based on SIFT algorithm is applied to feature point extraction and positioning of indoor parking navigation. Figure 6 is a schematic diagram of the steps of the improved stereo matching method based on SIFT algorithm. Select the target area ROI from the left image, then reduce the dimension of the feature vector to reduce the time-consuming, use the BBF algorithm based on the KD tree to speed up the matching speed, and finally use the RANSAC algorithm to remove the mismatch.

在t₁时刻，双目视觉系统检测到第1个角点特征显著的物体的角点在摄像机坐标系中的位置信息(x_c1,y_c1,z_c1)；在t₂时刻，惯性导航高精度定位系统有上一时刻解算得到的车辆在室内停车场高精度地图中的位置信息(λ₁，L₁，H₁)为起点，继续完成以上步骤的数据采集，当在t_n时刻双目立体视觉系统检测到车辆的位置离第1个角点特征显著的物体的距离s_n<p，其中p为根据实际要求设定的一个阈值，t_n时刻双目立体视觉系统检测到第1个角点特征显著的物体角点的在摄像机坐标系中的位置信息为(x_cn,y_cn,z_cn)，并在此时刻获取下一个距离最近的角点特征显著的物体的角点位置信息，即第2个角点特征显著的物体的位置信息(x_cn′,y_cn′,z_cn′)。在室内高精度地图中的位置信息由前一时刻t_n-1时刻的车辆位置信息进行更新得到，为(λ_n，L_n，H_n)。在t_n+1时刻双目立体视觉系统获取的第二个角点特征显著的物体角点位置信息(x_cn+1,y_cn+1,z_cn+1)和上一时刻获取的位置信息(x_cn′,y_cn′,z_cn′)，通过坐标系之间的转换，得到各特征点在世界坐标系(导航坐标系)中的位置信息，并通过数据处理，更新t_n+1时刻车辆的位置信息(λ_n+1,L_n+1,H_n+1)。后面时刻中，当车辆行驶到离角点特征显著的物体小于一个设定的阈值的时候，通过以上算法进行更换角点特征显著的物体参考角点，直到车辆到达目标车位。For the update selection of the binocular high-precision positioning reference point during the entire process of the vehicle parking, the following specific method is provided with reference to Fig. 4 : At the moment t ₀ =0, the binocular vision system detects the nearest object with significant corner point features , that is, the position information ( _{x c0} , y _c0 , z _c0 ) of the corner of the first object with significant corner features in the camera coordinate system; Position (λ ₀ , L ₀ , H ₀ ) is the starting point to start navigation, and at time t ₁ , the accelerations a _x , a _y , a _z and attitude angles γ, θ, and γ of each axis in the carrier coordinate system are obtained.

At time t ₁ , the binocular vision system detects the position information (x _c1 , y _c1 , z _c1 ) of the corner of the first object with significant corner features in the camera coordinate system; at time t ₂ , the inertial navigation high The precision positioning system has the position information (λ ₁ , L ₁ , H ₁ ) of the vehicle in the high-precision map of the indoor parking lot calculated at the previous _moment as the starting point, and continues to complete the data collection of the above steps. The distance between the position of the vehicle detected by the binocular stereo vision system and the first object with significant corner features s _n <p, where p is a threshold set according to actual requirements, and the binocular stereo vision system detects the first object at time t _n . The position information of the corners of each object with significant corner features in the camera coordinate system is (x _cn , y _cn , z _cn ), and at this moment, the position of the next closest object with significant corner features is obtained. information, that is, the position information (x _cn ′, y _cn ′, z _cn ′) of the object with significant second corner features. The position information in the indoor high-precision map is obtained by updating the vehicle position information at the previous time t _n-1 , which is (λ _n , L _n , H _n ). The corner position information (x _cn+1 , y _cn+1 , z _cn+1 ) of the second object with significant corner features obtained by the binocular stereo vision system at time t _n+1 and the position information obtained at the previous moment (x _cn ′, y _cn ′, z _cn ′), through the transformation between coordinate systems, the position information of each feature point in the world coordinate system (navigation coordinate system) is obtained, and through data processing, t _n+1 is updated Position information of the vehicle at time (λ _n+1 , L _n+1 , H _n+1 ). In the following moments, when the vehicle travels to an object with significant off-corner characteristics smaller than a set threshold, the above algorithm is used to replace the reference corner of the object with significant corner characteristics until the vehicle reaches the target parking space.

Referring to Figure 2, the selection and design of the high-precision inertial navigation coordinate system in the present invention consists of the following parts:

First, the earth coordinate system O _e -x _e y _e z _e , the earth coordinate system rotates synchronously with the earth, the z _e axis points to the regional rotation axis, the x _e axis points to the intersection of the prime meridian and the equator, and the y _e axis is at the equator In the plane, it forms a right-handed coordinate system with the x _e axis and the z _e axis;

Second, the navigation coordinate system O _n -x _n y _n z _n , its origin O coincides with the center of gravity of the carrier, that is, the vehicle, and the coordinate axes point to the north, east and sky directions respectively;

Third, the carrier coordinate system O _b -x _b y _b z _b , a coordinate system created with the center of gravity of the vehicle as the origin, where the x _b axis points to the front of the carrier, the y _b axis is along the longitudinal direction of the carrier, and the z _b axis points to the vertical axis of the carrier direction;

Fourth, the camera coordinate system O _c -x _c y _c z _c takes the optical center of the camera as the coordinate origin, the z _c axis is perpendicular to the plane of the image coordinate system, and the y _c axis is vertically downward;

Fifth, the world coordinate system O _w -x _w y _w z _w , the world coordinate system describes the relative positional relationship of objects in the actual environment, which associates the image pixel coordinate system with the physical coordinate system, the camera coordinate system and space objects, The selection of the world coordinate system in the present invention coincides with the navigation coordinate system in the inertial navigation system.

的大小。本发明采用四元数法进行姿态的更新，通过求解转换矩阵

即可实现载体坐标系与导航坐标系的转换，得到在t_i(i＝0，1，2…)时刻车辆在导航坐标系下的位置信息(x_ni，y_ni，z_ni)；双目视觉系统摄像机坐标系与世界坐标系的转换，由t_i(i＝0，1,2…)时刻双目视觉系统检测到的角点特征显著的物体角点在摄像机坐标系中的位置信息(x_ci,y_ci,z_ci)，得到在世界坐标系O_w-x_wy_wz_w下角点的位置(x_wi，y_wi，z_wi)。界坐标系描述实际环境中物体的相对位置关系，它将图像像素坐标系和物理坐标系、摄像机坐标系以及空间物体关联在一起。空间特征点在世界坐标系和摄像机坐标系下的齐次坐标分别为(x_w，y_w，z_w，1)和(x_c，y_c，z_c，1)，其中齐次坐标系就是把维数为n的向量用(n+1)维的向量来表示，世界坐标系的原点在摄像机坐标系中的坐标值为t＝(t_x，t_y，t_z)^T，这个3维的向量为平移向量，方向矢量R为摄像机坐标系在世界坐标系中用来描述二者关系的方向矢量，由三个姿态角的正余弦关系组合起来3×3的正交矩阵，则世界坐标系与摄像头坐标系之间的关系为

其中O^T＝(0，0，0)；地球坐标系与导航坐标系的转换，首先将地球坐标系O_ex_ey_ez_e绕z_e轴旋转λ角，得到O_e1-x_e1y_e1z_e1。然后绕y_e1轴旋转90°-L角，得到O_e2-x_e2y_e2z_e2。最后绕z_e2轴旋90°。经过以上步骤可以实现地球坐标系O_e-x_ey_ez_e和导航坐标系O_n-x_ny_nz_n，两坐标系之间的变换可通过矩阵

来表示，式中L,λ分别代表当地纬度和当地经度。Aiming at the conversion between each coordinate system in the present invention, a conversion method of high-precision inertial navigation coordinate system is provided, which mainly includes: coordinate transformation of the inertial navigation system, attitude calculation, speed update and position update from acquisition of acceleration and attitude angle : The instantaneous velocity v _x , v _y and v _z and the instantaneous position x, y and z of the carrier motion are obtained by integral respectively. The attitude of the carrier is the three rotations of the carrier relative to the navigation coordinate system when the carrier is in the body coordinate system. horn

the size of. The invention adopts the quaternion method to update the attitude, and solves the transformation matrix by solving the transformation matrix.

The conversion between the carrier coordinate system and the navigation coordinate system can be realized, and the position information (x _ni , y _ni , z _ni ) of the vehicle in the navigation coordinate system at time t _i (i=0, 1, 2...) can be obtained; binocular The conversion between the camera coordinate system of the vision system and the world coordinate system, the position information of the corner points of the object with significant corner features detected by the binocular vision system at the time t _i (i=0, 1, 2...) in the camera coordinate system ( x _ci , y _ci , z _ci ), obtain the position (x _wi , y _wi , z _wi ) of the lower corner point in the world coordinate system O _w -x _w y _w z _w . The bounded coordinate system describes the relative positional relationship of objects in the actual environment, which associates the image pixel coordinate system with the physical coordinate system, the camera coordinate system, and space objects. The homogeneous coordinates of the spatial feature points in the world coordinate system and the camera coordinate system are (x _w , y _w , z _w , 1) and (x _c , y _c , z _c , 1) respectively, where the homogeneous coordinate system is A vector of dimension n is represented by a vector of (n+1) dimension, and the coordinate value of the origin of the world coordinate system in the camera coordinate system is t=(t _x , _ty , t _z ) ^T , this 3-dimensional The vector of is the translation vector, and the direction vector R is the direction vector used by the camera coordinate system to describe the relationship between the two in the world coordinate system. Combining the sine and cosine relationship of the three attitude angles is a 3×3 orthogonal matrix, then the world coordinate The relationship between the system and the camera coordinate system is

Where O ^T = (0, 0, 0); for the conversion between the earth coordinate system and the navigation coordinate system, first rotate the earth coordinate system O _e x _e y _e z _e around the z _e axis by an angle of λ to obtain O _e1 -x _e1 y _e1 z _e1 . Then rotate 90°-L around the y _e1 axis to get O _e2 -x _e2 y _e2 z _e2 . Finally, rotate 90° around the z _e2 axis. After the above steps, the earth coordinate system O _e -x _e y _e z _e and the navigation coordinate system O _n -x _n y _n z _n can be realized, and the transformation between the two coordinate systems can be achieved through the matrix

where L and λ represent the local latitude and local longitude, respectively.

Aiming at the position error obtained by the vehicle using the inertial navigation system indoors, the present invention provides a method for eliminating the accumulated error of the inertial navigation, the implementation of which includes the following steps:

First, referring to the schematic diagram of the composition of the high-precision inertial navigation system and the structure of the reference coordinate system in Fig. 2, a dynamic coordinate system is established with the reference corner point ⑧ as the coordinate origin. Assume that at the moment t ₀ =0, the position information of the vehicle at the entrance of the garage obtained by high-precision GPS is (λ ₀ , L ₀ , H ₀ ), and at this moment, the binocular vision system detects that the first corner has significant features At the same time, the coordinates (x _w0 , y _w0 , z _w0 ) of the corner points of the object with significant corner characteristics in the world coordinate system are calculated, then in the coordinate system established with the characteristic corner points as the coordinates , the current position of the vehicle is (-x _w0 ,-y _w0 ,-z _w0 ), and in the inertial navigation system at the current moment, the coordinates of the vehicle in the navigation coordinate system are (x _n0 , y _n0 , z _n0 ). After a period of time, at time t ₁ , the binocular vision system detects the position of the corner point ⑧ of the object with significant corner features, and calculates that the corner point of the object with significant corner features at time t ₁ is in the world coordinate system The coordinates of (x _w1 , y _w1 , z _w1 ), in the coordinate system established with the characteristic corners as the coordinates, the current position of the vehicle is (-x _w1 ,-y _w1 ,-z _w1 ), at this moment the inertial The navigation system obtains the position of the vehicle (x _n1 , y _n1 , z _n1 ). The position information of the previous two times t ₀ and t ₁ is obtained, then the position information obtained by the binocular stereo vision system obtains the position information of the vehicle in the world coordinate system at time t _{1 at} time t 1, namely (x _w1 -x _w0 , y _w1 -y _w0 , z _w1 -z _w0 ). In the present invention, the selected world coordinate system and the navigation coordinate system are the same coordinate system, then the coordinates of the vehicle in the navigation coordinate system measured by the binocular vision system are (x _n1 ′, y _n1 ′, z _n1 ′)= (x _w1 -x _w0 ,y _w1 -y _w0 ,z _w1 -z _w0 ). The position information (x _n1 ′, y _n1 ′, z _n1 ′) in the navigation coordinate system is used to replace the position information (x _n1 , y _n1 , z _n1 ) in the navigation coordinate system of the inertial navigation system vehicle at time t ₁ , that is, the updated The position information of the inertial navigation system vehicle in the navigation coordinate system, so as to eliminate the accumulated error of inertial navigation from time _t0 to time _t1 . Through the transformation between the earth coordinate system and the navigation coordinate system, the position information (λ ₁ , L ₁ , H ₁ ) of the vehicle in the high-precision map of the indoor parking lot is obtained.

Second, the vehicle continues to drive, and at time t ₂ , the location (λ ₁ , L ₁ , H ₁ ) in the high-precision map of the indoor parking lot is used as the starting point, and (λ ₁ , L ₁ , H ₁ ) is used as the origin to establish Relative to the earth coordinate system, the same method is used to eliminate the accumulated error of the inertial navigation system at time t ₂ , and the position (λ ₂ , L ₂ , H ₂ ) of the vehicle in the high-precision map of the indoor parking lot at time t ₂ is also obtained. The method of eliminating the accumulated error of the inertial navigation system at the later time is the same as the method at the previous time, wherein the intervals of time t ₀ , t ₁ , and t ₂ are equal.

Thirdly, when the binocular stereo vision system detects the distance between the position of the vehicle and the first object with significant corner features at time t _n , s _n < p, where p is a threshold set according to actual requirements, as shown in Figure 4 As shown in the dynamic coordinate system transformation diagram of the reference point of the high-precision inertial navigation system, the position information of the corner point of the first object detected by the binocular stereo vision system at time t _n is (x _wn , y _wn , z _wn ), and obtain the corner position information of the next closest object with significant corner features at this moment, that is, the position information of the second object with significant corner features (x _wn ′, y _wn ′, z _wn ′ ). The position information in the indoor high-precision map is obtained by updating the vehicle position information at the previous time t _n-1 , which is (λ _n , L _n , H _n ). The corner position information (x _wn+1 , y _wn+1 , z _wn+1 ) of the second object with significant corner features obtained by the binocular stereo vision system at time t _n+1 and the position information obtained at the previous moment (x _wn ′, y _wn ′, z _wn ′), and the position information (λ _n+1 , L _n+1 , H _n+1 ) of the vehicle at time t _n+1 is updated. In the following moments, when the vehicle travels to an object with significant off-corner characteristics that is smaller than a set threshold, the above algorithm is used to replace the reference object and reference corner.

The position of the vehicle in the indoor high-precision map at time t _i (i=0, 1, 2...) is continuously updated until the vehicle reaches the target parking space.

The above embodiments should be understood as only for illustrating the present invention and not for limiting the protection scope of the present invention. After reading the contents of the description of the present invention, the skilled person can make various changes or modifications to the present invention, and these equivalent changes and modifications also fall within the scope defined by the claims of the present invention.

Claims (6)

1. a kind of inertial navigation high-precision positioning method based on binocular, acceleration and gyroscope, is characterized in that, comprises the following steps: Step 1. Establish a high-precision inertial system based on an inertial navigation module, a binocular stereo vision system and a vehicle, and establish a reference coordinate system structure; Step 2. Obtaining the starting coordinates of inertial navigation: The vehicle uses the starting coordinate point as the starting point for inertial navigation, and adopts the improved stereo matching method based on the SIFT algorithm, and selects according to the positional relationship of the points with significant corner features in the image. The area of interest, the vehicle completes the positioning and tracking of the area of interest through continuous position accumulation during the driving process, and only performs image matching on the area of interest in the subsequent process to complete the feature point extraction of binocular vision at the initial moment. To achieve the purpose of high matching accuracy and fast speed; Step 3. Selection of the reference point for binocular high-precision positioning: select the corner or corner with significant corner features in the indoor parking lot as the reference point for binocular high-precision positioning, and use the reference point as the origin to establish a reference coordinate system; Step 4. Selection and design of high-precision inertial navigation coordinate system: select the coordinate system including the earth coordinate system, navigation coordinate system, carrier coordinate system, camera coordinate system, and world coordinate system in the navigation process, so as to satisfy the final pair of relevant coordinates. the solution; Step 5. Conversion of high-precision inertial navigation coordinate system: obtain the position information of the feature point in the world coordinate system, the position information of the vehicle in the inertial navigation and navigation coordinate system, and the conversion relationship between the world coordinate system and the inertial navigation and navigation coordinate system ; Step 6. Elimination of accumulated errors of inertial navigation: Determine the three-dimensional coordinates of the inertial navigation reference position feature points based on binocular positioning and high-precision GPS information, and use the feature point coordinate values as reference points to update the three-dimensional inertial navigation coordinates in real time to eliminate inertial navigation due to Accumulated errors generated by integration, realize precise positioning of vehicles based on inertial navigation and binocular positioning combined with auxiliary indoor parking lot; The steps of the improved stereo matching method based on the SIFT algorithm in the step 2 specifically include: firstly, by performing epipolar constraints on the collected left and right images, and secondly, selecting the ROI area from the left image, and reducing the dimension of the feature vector to reduce time-consuming, using the method based on The BBF algorithm of the KD tree speeds up the matching speed, and finally uses the RANSAC algorithm to remove the mismatch; The elimination of the accumulated error of inertial navigation in the step 6: the coordinates of the starting point of inertial navigation are determined by binocular stereo vision positioning and high-precision GPS information. When the vehicle enters an area with no GPS signal or a very weak GPS signal, the inertial navigation mode is turned on and the The binocular stereo vision system obtains the location information of the corners or inflection points with significant corner features, and uses the location information of the feature points as a reference point to establish a three-dimensional reference coordinate system. The difference between the coordinate conversion and the vehicle position at the adjacent time is used to update the vehicle position; the vehicle selects whether to replace the reference object and the reference point by judging the distance of the reference point and the set minimum distance threshold of the reference point during the driving process. Finally, the precise positioning of vehicles based on inertial navigation and binocular positioning combined with auxiliary indoor parking lot is realized. 2. a kind of inertial navigation high-precision positioning method based on binocular, acceleration and gyroscope according to claim 1, is characterized in that, the inertial navigation module of described step 1 is composed of three-axis acceleration sensor, three-axis gyroscope It is composed of a three-axis magnetometer, and the parameters of each sensor are measured, and the coordinate system transformation in the inertial navigation system is obtained through the quaternion algorithm. The three-dimensional coordinates of the inertial navigation reference position feature points are determined by visual correlation algorithm, and the three-dimensional coordinates of the inertial navigation are updated in real time by using the difference between the coordinate values of the feature points at the current moment and the previous moment as the reference point to eliminate the inertial navigation caused by the visual image information. The cumulative error generated by the integration. 3. a kind of inertial navigation high-precision positioning method based on binocular, acceleration and gyroscope according to claim 1, is characterized in that, the acquisition of described step 2 starting coordinate point specifically comprises: when entering without GPS signal or In an indoor garage with a very weak GPS signal, the vehicle judges whether the binocular stereo vision system on the vehicle can obtain the position information of the characteristic corner at the starting point based on the last valid GPS information obtained, and compares the delay time of this process with the vehicle. The speed, heading angle information, etc., are interpolated for position information to obtain the initial coordinates of inertial navigation and navigation, and the vehicle uses the initial coordinates as the initial position for inertial navigation. 4. a kind of inertial navigation high-precision positioning method based on binocular, acceleration and gyroscope according to claim 3, is characterized in that, described step 3 utilizes the feature of corner point, namely corner point is the curvature on the target contour. The local maximum point is the decisive feature of the object contour, and the corner point also has rotation invariance. When the vehicle enters the indoor parking lot from the garage, the binocular camera on the vehicle selects the nearest corner point feature in the driving direction of the vehicle. Significant objects are used as reference objects for binocular positioning to extract feature points, and the acquired corner points are used as reference points for binocular high-precision positioning. 5. a kind of inertial navigation high-precision positioning method based on binocular, acceleration and gyroscope according to claim 1, is characterized in that, described step 5 also comprises the conversion between following coordinate systems: First, the conversion of the carrier coordinate system and the navigation coordinate system: the attitude conversion of the carrier in space is analogous to the carrier coordinate system b to the navigation coordinate system n after three basic rotation transformations around the pitch axis, roll axis and heading axis:

Second, the conversion between the camera coordinate system and the world coordinate system: select the world coordinate system to coincide with the navigation coordinate system, and the coordinates (x _c , y _c , z _c ) of the spatial feature points in the camera coordinate system are converted to obtain spatial features The coordinates of the point in the world coordinate system (x _w , y _w , z _w ), that is, the coordinate points (x _n , y _n , z _n ) of the spatial feature point in the navigation coordinate system are obtained: Third, the conversion between the earth coordinate system and the navigation coordinate system: first, rotate the earth coordinate system O _e -x _e y _e z _e around the z _e axis by an angle of λ to obtain O _e1 -x _e1 y _e1 z _e1 , and then rotate around the z e axis. The y _e1 axis is rotated by 90°-L angle to obtain O _e2 -x _e2 y _e2 z _e2 ; finally, it is rotated by 90° around the z _e2 axis. After the above steps, the earth coordinate system O _e -x _e y _e z _e can be converted to Navigation coordinate system O _n -x _n y _n z _n , the mutual transformation between the two coordinate systems can be done through the matrix

To represent. 6. a kind of inertial navigation high-precision positioning method based on binocular, acceleration and gyroscope according to claim 5 is characterized in that, the coordinates (x _c , y c , y _c , z _c ) After transformation, the coordinates (x _w , y _w , z _w ) of the spatial feature points in the world coordinate system are obtained, including: The homogeneous coordinates of the spatial feature points in the world coordinate system and the camera coordinate system are (x _w , y _w , z _w , 1) and (x _c , y _c , z _c , 1) respectively, where the homogeneous coordinate system is A vector of dimension n is represented by a vector of n+1 dimension, and the coordinate value of the origin of the world coordinate system in the camera coordinate system is t=(t _x , _ty , t _z ) ^T , this 3-dimensional vector is the translation vector, and the direction vector R is the direction vector used by the camera coordinate system to describe the relationship between the two in the world coordinate system. Combining the sine and cosine relationships of the three attitude angles to form a 3×3 orthogonal matrix, the world coordinate system and The relationship between the camera coordinate systems is

where R represents a 3×3 rotation matrix, O ^T =(0,0,0).

Images