Vehicle positioning method, positioning device, positioning system and vehicle

Abstract

The present application provides a vehicle positioning method, positioning device, positioning system and vehicle, comprising: obtaining image data collected by a camera, constructing a reprojection error constraint based on the image data; obtaining raw IMU data collected by an IMU, constructing an IMU pre-integration constraint between IMU data corresponding to adjacent moments based on the raw IMU data; obtaining raw wheel speedometer data collected by a wheel speedometer, constructing a wheel speedometer pre-integration constraint between wheel speedometer data corresponding to adjacent moments based on the raw wheel speedometer data; adjusting three weights corresponding to the camera, IMU and wheel speedometer respectively based on the reprojection error constraint, the IMU pre-integration constraint and the wheel speedometer pre-integration constraint, and optimizing the target vehicle's position information based on the adjusted three weights to obtain the target position information. The present application solves the technical problem in the related art that vehicles on bumpy roads are prone to sudden increase in IMU excitation, abnormal IMU data, and sudden changes in the vehicle's positioning state.

Description

Technical Field

[0001] The present application relates to the automotive field, and in particular to a vehicle positioning method, a positioning device, a positioning system, and a vehicle. Background Art

[0002] SLAM is key to autonomous navigation in unknown environments. In environments without satellite positioning signals, SLAM can be implemented using a variety of sensors, including lidar, cameras, and integrated inertial navigation systems. Laser SLAM offers high accuracy, but its high cost hinders its large-scale application in autonomous vehicles. In contrast, cameras are inexpensive and compact, making vSLAM highly popular. However, using only cameras to estimate vehicle pose can significantly impact the accuracy and robustness of vSLAM in situations with significant lighting fluctuations, rapid rotational motion, and transient image loss.

[0003] In the related technology, although the visual-inertial SLAM method has been relatively perfected, it cannot be well applied to ground vehicles. This is mainly due to the following reasons: (1) When the vehicle is on a bumpy road, it is easy to cause a sudden increase in the IMU excitation. When encountering bumpy terrain such as potholes or speed bumps, the IMU data is abnormal. The IMU pre-integration and its constraints in the existing visual-inertial positioning system are no longer reliable, but there is no effective method to deal with this part of the abnormal data, resulting in a sharp decrease in the accuracy of the visual-inertial positioning system, and even causing a sudden change in the vehicle's positioning state. (2) Ground vehicles are often in a uniform linear or approximately uniform linear motion. At this time, the excitation of the IMU is insufficient, resulting in scale drift and reduced positioning accuracy.

[0004] Therefore, based on the above problems existing in the related art, no effective solution has been proposed yet. Summary of the Invention

[0005] The main purpose of this application is to provide a vehicle positioning method, positioning device, positioning system and vehicle to solve the technical problem in the related art that the IMU excitation of the vehicle on a bumpy road is easily increased suddenly, the IMU data is abnormal, and the accuracy of the visual inertial positioning system is sharply reduced, and even the positioning state of the vehicle is suddenly changed.

[0006] According to one aspect of an embodiment of the present invention, a vehicle positioning method is provided, the method comprising: obtaining image data collected by a camera, constructing a reprojection error constraint based on the image data, wherein the camera is set on a target vehicle; obtaining original IMU data collected by an IMU, constructing an IMU pre-integration constraint between IMU data corresponding to adjacent moments based on the original IMU data, wherein the IMU is set on the target vehicle; obtaining original wheel speedometer data collected by a wheel speedometer, constructing a wheel speedometer pre-integration constraint between wheel speedometer data corresponding to adjacent moments based on the original wheel speedometer data, wherein the wheel speedometer is set on the target vehicle; adjusting three weights corresponding to the camera, the IMU, and the wheel speedometer, respectively, based on the reprojection error constraint, the IMU pre-integration constraint, and the wheel speedometer pre-integration constraint, and optimizing the position information of the target vehicle based on the adjusted three weights to obtain the target position information.

[0007] Optionally, based on the reprojection error constraint, the IMU pre-integration constraint, and the wheel speed meter pre-integration constraint, the three weights corresponding to the camera, the IMU, and the wheel speed meter are adjusted, including: based on the reprojection error constraint, the IMU pre-integration constraint, and the wheel speed meter pre-integration constraint, determining the road condition on which the target vehicle is traveling through a sliding window optimization method; and adjusting the three weights based on the road condition.

[0008] Optionally, image data captured by a camera on the target vehicle is obtained, and a reprojection error constraint is constructed based on the image data, including: obtaining the i-th frame image and the j-th frame image captured by the camera, and performing optical flow tracking on the i-th frame image and the j-th frame image, where the i-th frame image is the current frame image captured by the camera, and the j-th frame image is the previous frame image captured by the camera; determining target feature points based on optical flow tracking, wherein the target feature points are feature points that exist on both the i-th frame image and the j-th frame image; calculating the projection coordinate values ​​of the target feature points in the i-th frame image projected onto the camera coordinate system corresponding to the j-th frame image; and constructing a reprojection error constraint based on the projection coordinate values.

[0009] Optionally, calculating the projection coordinate value of the target feature point in the i-th frame image projected onto the camera coordinate system corresponding to the j-th frame image includes: calculating the projection coordinate value using Formula 1, where Formula 1 is:

[0010] Among them, u ci 、v ci They are used to represent the normalized plane coordinates of the target feature points in the i-th frame image, T wbi is the transformation matrix from the IMU coordinate system to the world coordinate system corresponding to the i-th frame image, is the inverse matrix of the transformation matrix from the IMU coordinate system to the world coordinate system corresponding to the j-th frame image, T bc is the external parameter between the camera and the IMU, Tbc The corresponding inverse matrix, λ is the inverse depth of the target feature point in the first observation image frame in the sliding window.

[0011] Optionally, construct reprojection error constraints based on the projection coordinate values, including:

[0012] The reprojection error constraint is: Among them, x cj 、y cj 、z cj is the projection coordinate value, u cj 、v cj The normalized plane coordinate value in the j-th frame image after using stream tracking for the target feature point in the i-th frame image.

[0013] Optionally, IMU data collected by an IMU on the target vehicle is obtained, and an IMU pre-integration constraint between IMU data corresponding to adjacent moments is constructed based on the IMU data, including: constructing a first position pre-integral of the IMU with respect to position based on the IMU data, where the first position pre-integral is: Among them, α bibj is the first position pre-integral between the state quantities corresponding to time i and time j, is the relative rotation from time t to time i, is the measured value of acceleration in the IMU coordinate system at time t, δt is the time difference between two frames of IMU data, time i is the time when the camera captures the i-th frame image, and time j is the time when the camera captures the j-th frame image. Based on the IMU data, the velocity pre-integral of the IMU about the velocity is constructed, and the velocity pre-integral is: Among them, β bibj is the velocity pre-integral between the state quantities corresponding to time i and time j, is the relative rotation from time t to time i, is the measured value of acceleration in the IMU coordinate system at time t, and δt is the time difference between two frames of IMU data. Based on the IMU data, the first rotation pre-integral of the IMU about rotation is constructed. The first rotation pre-integral is: Among them, q bibj is the first rotation pre-integral between the state quantities corresponding to time i and time j, is the relative rotation from time t to time i, and δt is the time difference between two frames of IMU data. The IMU pre-integration constraint is constructed based on the first position pre-integration, velocity pre-integration, and first rotation pre-integration.

[0014] Optionally, constructing an IMU pre-integration constraint based on the first position pre-integration, the velocity pre-integration, and the first rotation pre-integration includes: constructing an IMU pre-integration constraint through a second formula, where the IMU pre-integration constraint is: Among them, r p is the residual error of IMU about position, r q is the residual error of IMU about rotation, r v is the residual error of IMU about velocity, r ba is the residual of the IMU acceleration bias, r bg is the residual of the IMU angular velocity bias, q bjw is the rotation transformation from the world coordinate system to the IMU coordinate system at time t, is the translation transformation value from the IMU coordinate system to the world coordinate system at time j, is the translation change from the IMU coordinate system to the world coordinate system at time i, Δt is the time difference between time i and time j, g w is the gravity vector in the world coordinate system, is the inverse pre-integration of the first rotation pre-integration, is the multiplication between quaternions, [.] xyz is the real part of the quaternion, is the acceleration bias of the accelerometer at time j, is the acceleration bias of the accelerometer at time i, is the angular velocity zero bias of the gyroscope at time j, is the angular velocity bias of the gyroscope at time i, α bibj is the first position pre-integration, β bibj is the velocity pre-integral, q bibj Pre-integrate for the first rotation.

[0015] Optionally, original tachometer data collected by the tachometer is obtained, and a tachometer pre-integral constraint between tachometer data corresponding to adjacent moments is constructed based on the original tachometer data, including: constructing a second position pre-integral of the tachometer with respect to position, where the second position pre-integral is: in, is the pre-integral of the wheel speedometer position between the wheel speedometer data corresponding to time i and time j, is the relative rotation from time t to time i, q be is the rotational external parameter between the wheel speed meter and the IMU, v et is the speed value calculated by the wheel speed meter at time t, δt is the time difference between two frames of wheel speed meter data, time i is the time when the camera captures the i-th frame image, and time j is the time when the camera captures the j-th frame image;

[0016] Based on the second position pre-integration, the wheel speed meter pre-integration constraint is constructed. The pre-integration constraint is: Among them, q bjw is the rotation transformation from the world coordinate system to the IMU coordinate system at time t, is the translation transformation value from the IMU coordinate system to the world coordinate system at time j, is the translation change from the IMU coordinate system to the world coordinate system at time i, Pre-integrate for the second position.

[0017] Optionally, based on the reprojection error constraint, the IMU pre-integration constraint, and the wheel speed meter pre-integration constraint, a sliding window optimization method is used to determine the road condition on which the target vehicle is traveling, including: obtaining multiple first rotation pre-integrals between multiple adjacent image frames in the sliding window, determining multiple relative rotation angles based on the multiple first rotation pre-integrals, and determining an average rotation angle of the multiple relative rotation angles. Acquire multiple first position pre-integrals between multiple adjacent image frames in the sliding window, determine multiple relative translation distances based on the multiple first position pre-integrals, and determine a first average translation distance corresponding to the multiple relative translation distances Acquire multiple second position pre-integrals between multiple adjacent image frames in the sliding window, determine multiple relative translation distances based on the multiple second position pre-integrals, and determine second average translation distances corresponding to the multiple relative translation distances Get the current IMU rotation pre-integral and determine the relative rotation angle θ based on the rotation pre-integral b ; Get the IMU position pre-integration α at the current moment b , using the wheel speed position pre-integral α′ b ;

[0018] Determine the first parameter, the second parameter and the third parameter, the first parameter is The second parameter is The third parameter is A road condition is determined based on the first parameter, the second parameter, and the third parameter.

[0019] Optionally, the road surface condition is determined based on the first parameter, the second parameter and the third parameter, including: when the first parameter satisfies the first condition, the second parameter satisfies the second condition and the third parameter satisfies the third condition, the road surface condition is determined to be an abnormal condition, wherein the first condition is that f1 is greater than or equal to the first threshold, the second condition is that f2 is greater than or equal to the first threshold, and the third condition is that f3 is less than or equal to the second threshold, and the abnormal condition is the condition where the flatness of the road surface is less than a preset flatness; when the three parameters do not satisfy the three corresponding conditions at the same time, the road surface condition is determined to be a normal condition.

[0020] Optionally, the three weights are adjusted according to the road conditions, including: when the road conditions are abnormal, reducing the weight corresponding to the IMU to the first preset weight, and increasing the weight corresponding to the camera to the second preset weight, and increasing the weight corresponding to the wheel speed meter to the third preset weight.

[0021] Optionally, according to the road condition, the three weights are adjusted, including: when the road condition is normal, the three weights are optimized to calculate the fourth parameter, the fifth parameter and the sixth parameter, and the fourth parameter is α bibj is the pre-integration of IMU with respect to position, is the pre-integral of the wheel speed meter with respect to position, and the fifth parameter is q bibj is the pre-integration of IMU about rotation, and the sixth parameter is α bibj is the pre-integration of IMU about position, Th θ =2.3; the three weights are optimized through the fourth parameter, the fifth parameter and the sixth parameter.

[0022] Optionally, the three weights are optimized by the fourth parameter, the fifth parameter, and the sixth parameter, including: optimizing the weight corresponding to the wheel speed meter by a third formula according to the fourth parameter and the fifth parameter, the third formula being: T e =t1(1-t2); Based on the fourth parameter and the fifth parameter, the weight corresponding to the IMU is optimized by the fourth formula, which is: T I =t2(1-t1); Based on the sixth parameter, the weight corresponding to the camera is optimized by the fifth formula, which is: T c =1-t3.

[0023] According to another aspect of an embodiment of the present invention, a vehicle positioning device is also provided, which includes: a first acquisition unit, used to acquire image data collected by a camera, and construct a reprojection error constraint based on the image data, wherein the camera is set on a target vehicle; a second acquisition unit, used to acquire original IMU data collected by an IMU, and construct an IMU pre-integration constraint between IMU data corresponding to adjacent moments based on the original IMU data, wherein the IMU is set on the target vehicle; a third acquisition unit, used to acquire original wheel speedometer data collected by a wheel speedometer, and construct a wheel speedometer pre-integration constraint between wheel speedometer data corresponding to adjacent moments based on the original wheel speedometer data, wherein the wheel speedometer is set on the target vehicle; an adjustment unit, used to adjust three weights corresponding to the camera, the IMU and the wheel speedometer respectively based on the reprojection error constraint, the IMU pre-integration constraint and the wheel speedometer pre-integration constraint, and optimize the position information of the target vehicle based on the adjusted three weights to obtain the target position information.

[0024] According to another aspect of an embodiment of the present invention, a computer-readable storage medium is provided. The computer-readable storage medium includes a stored program, wherein the program executes any one of the above methods.

[0025] According to another aspect of an embodiment of the present invention, a processor is further provided, and the processor is configured to run a program, wherein the program executes any one of the above methods when running.

[0026] According to another aspect of an embodiment of the present invention, a positioning system is provided, including a camera, an IMU, a wheel speed meter, and a vehicle positioning device, wherein the vehicle positioning device is configured to execute any one of the above methods.

[0027] According to another aspect of an embodiment of the present invention, a vehicle is provided, a positioning system including a camera, an IMU, and a wheel speed meter; a controller of the positioning system including one or more processors, a memory, and one or more programs, wherein the one or more programs are stored in the memory and configured to be executed by the one or more processors, and the one or more programs include a method for executing any one of the above methods.

[0028] The technical solution of the present application is adopted, by obtaining image data collected by a camera, constructing a reprojection error constraint based on the image data, and the camera is set on the target vehicle; obtaining original IMU data collected by an IMU, constructing an IMU pre-integration constraint between IMU data corresponding to adjacent moments based on the original IMU data, and the IMU is set on the target vehicle; obtaining original wheel speedometer data collected by a wheel speedometer, constructing a wheel speedometer pre-integration constraint between wheel speedometer data corresponding to adjacent moments based on the original wheel speedometer data, and the wheel speedometer is set on the target vehicle; according to the reprojection error constraint, the IMU pre-integration constraint and the wheel speedometer pre-integration constraint, the three weights corresponding to the camera, the IMU and the wheel speedometer are adjusted respectively, and according to the adjusted three weights, the pose information of the target vehicle is optimized to obtain the target pose information, which solves the technical problem in the related art that the vehicle is prone to a sudden increase in IMU excitation and abnormal IMU data on bumpy roads, which leads to a sharp decrease in the accuracy of the visual inertial positioning system and even a sudden change in the positioning state of the vehicle, and achieves the technical effect of continuously providing accurate and highly robust positioning information. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] The drawings that constitute part of this application are used to provide a further understanding of this application. The illustrative embodiments of this application and their descriptions are used to explain this application and do not constitute an improper limitation on this application. In the drawings:

[0030] Figure 1 is a flow chart of a vehicle positioning method provided according to an embodiment of the present application;

[0031] Figure 2 A flowchart for constructing reprojection error constraints provided in an embodiment of the present application;

[0032] Figure 3A flowchart of a method for determining the initial position of a vehicle at the next moment is provided for an embodiment of the present application;

[0033] Figure 4 A flowchart of a method for constructing IMU pre-integration constraints provided in this application;

[0034] Figure 5 A flowchart of a method for constructing a wheel speed meter pre-integration constraint provided in an embodiment of the present application;

[0035] Figure 6 A flowchart of a method for determining a road condition on which a target vehicle is traveling, provided in an embodiment of the present application;

[0036] Figure 7 A flowchart of a method for adjusting the weights of a camera, an IMU, and wheel speeds provided in this application;

[0037] Figure 8 A flowchart of a closed-loop detection thread provided by this application;

[0038] Figure 9 A specific schematic diagram of the closed-loop detection thread provided by this application;

[0039] Figure 10 Schematic diagram of a vehicle positioning device provided according to an embodiment of the present application. DETAILED DESCRIPTION

[0040] It should be noted that, in the absence of conflict, the embodiments and features of the embodiments in this application can be combined with each other. The present application will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.

[0041] In order to enable those skilled in the art to better understand the present invention, the following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments in the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts should fall within the scope of protection of this application.

[0042] It should be noted that the terms "first", "second", etc. in the specification and claims of the present application and the above-mentioned drawings are used to distinguish similar objects and are not necessarily used to describe a specific order or sequential order. It should be understood that the data used in this way can be interchanged where appropriate, so that the embodiments of the present application described here. In addition, the terms "including" and "having" and any of their variations are intended to cover non-exclusive inclusions. For example, a process, method, system, product or device that includes a series of steps or units is not necessarily limited to those steps or units clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or devices.

[0043] It should be understood that when an element (such as a layer, film, region, or substrate) is described as being "on" another element, the element may be directly on the other element or intervening elements may be present. Moreover, in the specification and claims, when it is described that an element is "connected to" another element, the element may be "directly connected to" the other element or "connected to" the other element through a third element.

[0044] As mentioned in the background technology, there is a technical problem in the prior art that a vehicle on a bumpy road may easily cause a sudden increase in IMU excitation, abnormal IMU data, and a sharp decrease in the accuracy of the visual inertial positioning system, and even cause a sudden change in the vehicle's positioning state. In order to solve the above problem, a typical embodiment of the present application provides a vehicle positioning method, a positioning device, a positioning system and a vehicle.

[0045] Figure 1 This is a flow chart of a vehicle positioning method provided according to an embodiment of the present application. Figure 1 As shown, the method includes the following steps:

[0046] Step S101 , obtaining image data collected by a camera, and constructing a reprojection error constraint based on the image data, wherein the camera is set on a target vehicle.

[0047] Step S102: obtaining raw IMU data collected by the IMU, and constructing IMU pre-integration constraints between IMU data corresponding to adjacent moments based on the raw IMU data. The IMU is set on the target vehicle.

[0048] Step S103 , obtaining original wheel speed meter data collected by a wheel speed meter, and constructing a wheel speed meter pre-integration constraint between wheel speed meter data corresponding to adjacent moments based on the original wheel speed meter data, wherein the wheel speed meter is set on the target vehicle.

[0049] In step S104, the three weights corresponding to the camera, IMU, and wheel speed meter are adjusted according to the reprojection error constraint, the IMU pre-integration constraint, and the wheel speed meter pre-integration constraint, and the target vehicle's pose information is optimized based on the adjusted three weights to obtain the target pose information.

[0050] The present application provides a vehicle positioning method, in which a front-end odometer thread is provided for performing optical flow tracking on adjacent image frames captured by a camera, and integrating IMU data and wheel speed meter data on a manifold to obtain the current position of the vehicle in real time.

[0051] In this application, the road conditions on which the target vehicle is traveling are determined through a sliding window optimization method based on the reprojection error constraints, the IMU pre-integration constraints, and the wheel speed meter pre-integration constraints; and the three weights are adjusted based on the road conditions.

[0052] This method also provides a back-end optimization thread that tightly couples the data collected by the camera, IMU, and wheel speedometer, pre-integrates the raw data of the IMU and wheel speedometer, establishes inter-frame constraints, tightly couples the data of the camera, IMU, and wheel speedometer in a sliding window, and detects whether there is abnormal data. The abnormal data is used to determine that the road condition on which the vehicle is located is an abnormal condition, and adaptively adjusts the corresponding weights of the camera, IMU, and wheel speedometer according to the determined road condition, and optimizes the current posture information based on the adjusted weights.

[0053] Specifically, in practical applications, the camera collects images in real time, and performs optical flow tracking on the current frame image and the previous frame image to construct the reprojection error constraint, such as Figure 2 As shown, Figure 2 The flowchart for constructing the reprojection error constraint includes the following steps:

[0054] Step S201: obtaining the i-th frame image and the j-th frame image captured by the camera, and performing optical flow tracking on the i-th frame image and the j-th frame image, where the i-th frame image is the current frame image captured by the camera, and the j-th frame image is the previous frame image captured by the camera;

[0055] Step S202: determining target feature points based on optical flow tracking, wherein the target feature points are feature points that exist on both the i-th frame image and the j-th frame image;

[0056] Step S203: Calculate the projection coordinate value of the target feature point in the i-th frame image projected onto the camera coordinate system corresponding to the j-th frame image, where the projection coordinate value is: in, are used to represent the normalized plane coordinates of the target feature points in the i-th frame image, is the transformation matrix from the IMU coordinate system to the world coordinate system corresponding to the i-th frame image, is the inverse matrix of the transformation matrix from the IMU coordinate system to the world coordinate system corresponding to the j-th frame image, T bc is the external parameter between the camera and the IMU, T bc The corresponding inverse matrix, λ is the inverse depth of the target feature point in the first observation image frame in the sliding window;

[0057] Step S204: Construct a reprojection error constraint based on the projection coordinate value, wherein the reprojection error constraint is: in, is the projection coordinate value, The normalized plane coordinate value in the j-th frame image after using stream tracking for the target feature point in the i-th frame image.

[0058] This application also provides a method for determining the initial position of the vehicle at the next moment, the specific flow chart is as follows: Figure 3 As shown, the following steps are included:

[0059] Step S301: After adaptive local histogram equalization of the current frame image (i-th frame image), optical flow tracking is performed between the previous frame image and the current frame image based on the image pyramid;

[0060] Step S302: using the basic matrix to eliminate outliers (points where tracking failed) after optical flow tracking on the current frame image;

[0061] Step S303: setting a mask around the inliers retained in the current frame image (setting the mask to make the feature points uniform), and extracting a required number of feature points outside the mask;

[0062] Step S304: Integrate the IMU data and wheel speed meter data on the manifold to obtain the initial posture at the next moment.

[0063] The present application also provides a method for constructing IMU pre-integration constraints, which constructs IMU pre-integration constraints between adjacent time state quantities by pre-integrating IMU raw data. The IMU pre-integration constraints include constructing pre-integration parts about position, pre-integration about velocity, and pre-integration about rotation. The main steps are as follows: Figure 4 As shown, the specific steps include:

[0064] Step S401: Construct the first position pre-integral of the IMU with respect to the position based on the IMU data: in, is the first position pre-integral between the state quantities corresponding to time i and time j, is the relative rotation from time t to time i, is the measured value of acceleration in the IMU coordinate system at time t, δt is the time difference between two frames of IMU data, time i is the time when the camera captures the i-th frame image, and time j is the time when the camera captures the j-th frame image;

[0065] Step S402: Based on the IMU data, construct the IMU velocity pre-integral about velocity: in, is the velocity pre-integral between the state quantities corresponding to time i and time j, is the relative rotation from time t to time i, is the measured value of acceleration in the IMU coordinate system at time t, and δt is the time difference between two frames of IMU data;

[0066] Step S403: Construct the first rotation pre-integral of the IMU with respect to rotation according to the IMU data:

[0067] in, is the first rotation pre-integral between the state quantities corresponding to time i and time j, is the relative rotation from time t to time i, and δt is the time difference between two frames of IMU data;

[0068] Step S404: Constructing IMU pre-integration constraints based on the first position pre-integration, velocity pre-integration, and first rotation pre-integration, where:

[0069] The IMU pre-integration constraints are: Among them, r p is the residual error of IMU about position, r q is the residual error of IMU about rotation, r v is the residual error of IMU about velocity, r ba is the residual of the IMU acceleration bias, r bg is the residual of the IMU angular velocity bias, is the rotation transformation from the world coordinate system to the IMU coordinate system at time t, is the translation transformation value from the IMU coordinate system to the world coordinate system at time j, is the translation change from the IMU coordinate system to the world coordinate system at time i, Δt is the time difference between time i and time j, g w is the gravity vector in the world coordinate system, is the inverse pre-integration of the first rotation pre-integration, is the multiplication between quaternions, [.] xyz is the real part of the quaternion, is the acceleration bias of the accelerometer at time j, is the acceleration bias of the accelerometer at time i, is the angular velocity zero bias of the gyroscope at time j, is the angular velocity bias of the gyroscope at time i, is the first position pre-integration, is the velocity pre-integral, Pre-integrate for the first rotation.

[0070] This application also provides a method for constructing a wheel speed meter pre-integration constraint, the flow chart of which is as follows: Figure 5 As shown, the specific steps include:

[0071] Step S501: Construct the second position pre-integral of the wheel speed meter with respect to the position: in, is the pre-integral of the wheel speedometer position between the wheel speedometer data corresponding to time i and time j, is the relative rotation from time t to time i, q be is the rotational external parameter between the wheel speed meter and the IMU, v et is the speed value calculated by the wheel speed meter at time t, δt is the time difference between two frames of wheel speed meter data, time i is the time when the camera captures the i-th frame image, and time j is the time when the camera captures the j-th frame image;

[0072] Step S502: Constructing a wheel speed meter pre-integration constraint based on the second position pre-integration. The pre-integration constraint is: in, is the rotation transformation from the world coordinate system to the IMU coordinate system at time t, is the translation transformation value from the IMU coordinate system to the world coordinate system at time j, is the translation change from the IMU coordinate system to the world coordinate system at time i, Pre-integrate for the second position.

[0073] It should be noted that after constructing the pre-integration constraints on the IMU data, the pre-integration constraints on the wheel speed meter data, and the reprojection error constraints, the data collected by the camera, IMU, and wheel speed meter are initialized in a loosely coupled manner for trajectory alignment. The specific diagram is shown in the figure below. Figure 5 As shown, Figure 5 A schematic diagram of the trajectory alignment of camera data, IMU data, and wheel speed meter data provided in this application.

[0074] In practical applications, if the rotation extrinsic parameters between the camera and IMU, or between the IMU and the wheel speed meter are unknown, the rotation extrinsic parameter constraint is used to estimate the rotation extrinsic parameter R between the camera and IMU. bc , the rotational external reference R between the wheel speed meter and the IMU be ; Use rotation constraints to estimate the zero bias b of the gyroscope in the IMU g; Use the translation constraint to estimate the scale factor and the direction of gravity in the initial coordinate system c0; calculate the rotation matrix between c0 and the world coordinate system, and align the trajectory to the world coordinate system.

[0075] After aligning the data collected by the camera, IMU and wheel speed meter, this application adopts the sliding window optimization method to determine the state quantity to be optimized in the sliding window, X=[x0,x1.....x n ,x bc ,x be ,λ0,λ1,...λ m ],in, x k Indicates the position of the IMU body in the world coordinate system at time k speed attitude And the acceleration bias estimate b in the IMU body coordinate system a and the angular velocity bias estimate b g , n represents a key frame. x bc =[p bc ,q bc ] represents the external parameter from camera to IMU, x be =[p be ,q be ] represents the external parameter from the wheel speed meter to the IMU, λ j is the inverse depth of the j-th feature point in the first observed image frame in the sliding window, and m is the number of landmark points observed in all key frames in the sliding window, where the landmark points are the 3D points corresponding to the feature points in the three-dimensional real space.

[0076] This application provides a method for determining the road condition of a target vehicle by using a sliding window optimization method based on reprojection error constraints, IMU pre-integration constraints, and wheel speedometer pre-integration constraints. Figure 6 A flow chart of a method for determining the road conditions on which a target vehicle is traveling, wherein the specific steps are as follows:

[0077] Step S601: Obtaining the IMU rotation pre-integration between adjacent image frames in the sliding window and position pre-integration α bibj , the relative rotation angle θ is obtained by calculation i , and for θ i Find the average and get By α bibj Calculate the relative translation size α i , and find α i The corresponding average

[0078] Step S602: Obtaining the pre-integral position of the wheel speedometer between adjacent image frames in the sliding window Depend on Calculate the relative translation size α′ i , and find α′ i The corresponding average

[0079] Step S603: Obtain the IMU rotation pre-integral q at the current moment b , and according to q b Calculate the relative rotation angle θ b , get the IMU position pre-integration α at the current moment b and the position pre-integral α′ of the wheel speed meter b ;

[0080] Step S604: Another first parameter Second parameter The third parameter If f1 is greater than or equal to a first threshold, f2 is greater than or equal to a second threshold, and f3 is less than or equal to a third threshold, the road condition is determined to be a bumpy condition, such as a pothole, a speed bump, etc. In a specific embodiment, the first threshold is equal to the second threshold, equal to 3, and the third threshold is 0.3.

[0081] Step S605: When the three parameters do not satisfy the three corresponding conditions at the same time, it is determined that the road surface working condition is a normal working condition.

[0082] This application also provides a method for adjusting the weights of the camera, IMU, and wheel speed, as follows: Figure 7 As shown, the following steps are included:

[0083] Step S701: When the road condition is bumpy, the IMU pre-integration constraint is no longer credible, so the weight of the IMU pre-integration constraint is reduced. In a specific embodiment, T e / T I =50, where T e is the weight factor after wheel speed meter optimization, T I is the optimized weight factor of IMU.

[0084] Step S702: If the three conditions cannot be met simultaneously, since the IMU is a self-closing sensor and its estimated value of motion is more accurate than the wheel speed meter in a short period of time, the optimization weight factor is calculated based on the IMU increment value:

[0085] T e =t1(1-t2), T I =t2(1-t1), T c =1-t3

[0086] This application also provides a closed-loop detection thread for closed-loop detection and global pose graph optimization to eliminate the errors accumulated in the above two threads. The process is as follows Figure 8 As shown, it specifically includes the following steps:

[0087] Step S801: performing similarity detection in historical key frames based on the bag-of-words model to obtain closed-loop candidate key frames;

[0088] Step S802: Perform temporal and spatial consistency detection based on the closed-loop candidate key frames to obtain closed-loop key frames;

[0089] Step S803: Optimize the vehicle pose information in the global pose graph.

[0090] The specific diagram of the thread of the above closed-loop detection is as follows Figure 9 shown.

[0091] The present application provides a vehicle positioning method, which is a multi-sensor fusion vehicle positioning method based on a tightly coupled camera, IMU, and wheel speedometer, and can provide ground vehicles with positioning effects with excellent accuracy and robustness. Unlike existing visual inertial positioning methods, when the vehicle is in bumpy terrain such as potholes or speed bumps, or in uniform linear motion or approximately uniform linear motion, the excitation to the IMU may suddenly increase, or the excitation may be insufficient, and the weight can be adaptively adjusted. The present invention can adaptively adjust and optimize the weight factor based on the historical data of the IMU and wheel speedometer in the current and sliding windows, especially when abnormal data is detected, so that the present invention can effectively cope with special working conditions such as bumpy terrain, uniform linear motion, and stillness.

[0092] Based on the above key points, the present invention includes three parallel threads: a front-end odometry thread, which performs optical flow tracking on adjacent image frames and integrates IMU and wheel speedometer data on the manifold to obtain the real-time vehicle's current pose. A back-end optimization thread initializes the tightly coupled fusion localization method, pre-integrates the raw data from the IMU and wheel speedometer, establishes inter-frame constraints, tightly couples the camera, IMU, and wheel speedometer data in a sliding window, adjusts the optimization weights of each sensor, constructs a factor graph optimization framework, and optimizes and updates the state variables in the sliding window. A closed-loop detection thread is used for closed-loop detection and global pose graph optimization, eliminating the errors accumulated in the aforementioned two threads.

[0093] It should be noted that the steps shown in the flowcharts of the accompanying drawings can be executed in a computer system such as a set of computer-executable instructions, and that, although a logical order is shown in the flowcharts, in some cases, the steps shown or described can be executed in an order different from that shown here.

[0094] The embodiment of the present application further provides a vehicle positioning device. It should be noted that the vehicle positioning device provided in the embodiment of the present application can be used to execute the vehicle positioning method provided in the embodiment of the present application. The vehicle positioning device provided in the embodiment of the present application is introduced below.

[0095] Figure 10 Schematic diagram of a vehicle positioning device provided according to an embodiment of the present application. Figure 10 As shown, the device includes:

[0096] The first construction unit 101 is used to obtain image data collected by a camera and construct a reprojection error constraint based on the image data, and the camera is set on the target vehicle; the second construction unit 102 is used to obtain original IMU data collected by an IMU and construct an IMU pre-integration constraint between IMU data corresponding to adjacent moments based on the original IMU data, and the IMU is set on the target vehicle; the third construction unit 103 is used to obtain original wheel speedometer data collected by a wheel speedometer and construct a wheel speedometer pre-integration constraint between wheel speedometer data corresponding to adjacent moments based on the original wheel speedometer data, and the wheel speedometer is set on the target vehicle; the adjustment unit 104 is used to adjust the three weights corresponding to the camera, the IMU and the wheel speedometer respectively according to the reprojection error constraint, the IMU pre-integration constraint and the wheel speedometer pre-integration constraint, and optimize the position information of the target vehicle according to the adjusted three weights to obtain the target position information.

[0097] Optionally, the adjustment unit includes: a first determination subunit, used to determine the road conditions on which the target vehicle is traveling based on the reprojection error constraint, the IMU pre-integration constraint, and the wheel speed meter pre-integration constraint through a sliding window optimization method; and a first adjustment subunit, used to adjust the three weights based on the road conditions.

[0098] Optionally, the first construction unit includes: a tracking subunit, used to obtain the i-th frame image and the j-th frame image captured by the camera, and perform optical flow tracking on the i-th frame image and the j-th frame image, the i-th frame image is the current frame image captured by the camera, and the j-th frame image is the previous frame image captured by the camera; a second determination subunit, used to determine the target feature points based on optical flow tracking, wherein the target feature points are feature points that exist on both the i-th frame image and the j-th frame image; a calculation subunit, used to calculate the projection coordinate values ​​of the target feature points in the i-th frame image projected onto the camera coordinate system corresponding to the j-th frame image; and a first construction subunit, used to construct a reprojection error constraint based on the projection coordinate values.

[0099] Optionally, the calculation subunit includes: a calculation module, configured to calculate the projection coordinate value using Formula 1, where Formula 1 is:

[0100] in, are used to represent the normalized plane coordinates of the target feature points in the i-th frame image, is the transformation matrix from the IMU coordinate system to the world coordinate system corresponding to the i-th frame image, is the inverse matrix of the transformation matrix from the IMU coordinate system to the world coordinate system corresponding to the j-th frame image, T bc is the external parameter between the camera and the IMU, T bc The corresponding inverse matrix, λ is the inverse depth of the target feature point in the first observation image frame in the sliding window.

[0101] Optionally, the reprojection error constraint is: in, is the projection coordinate value, The normalized plane coordinate value in the j-th frame image after using stream tracking for the target feature point in the i-th frame image.

[0102] Optionally, the second construction unit includes: a second construction subunit, configured to construct a first position pre-integral of the IMU with respect to the position based on the IMU data, where the first position pre-integral is: in, is the first position pre-integral between the state quantities corresponding to time i and time j, is the relative rotation from time t to time i, is the measured value of acceleration in the IMU coordinate system at time t, δt is the time difference between two frames of IMU data, time i is the time when the camera captures the i-th frame image, and time j is the time when the camera captures the j-th frame image. Based on the IMU data, the velocity pre-integral of the IMU about the velocity is constructed, and the velocity pre-integral is: in, is the velocity pre-integral between the state quantities corresponding to time i and time j, is the relative rotation from time t to time i, is the measured value of acceleration in the IMU coordinate system at time t, and δt is the time difference between two frames of IMU data; the third construction subunit is used to construct the first rotation pre-integral of the IMU about rotation based on the IMU data. The first rotation pre-integral is: in, is the first rotation pre-integral between the state quantities corresponding to time i and time j, is the relative rotation from time t to time i, and δt is the time difference between two frames of IMU data; the fourth construction subunit is used to construct the IMU pre-integration constraint based on the first position pre-integration, velocity pre-integration and first rotation pre-integration.

[0103] Optionally, the fourth construction subunit includes: a first construction module, configured to construct an IMU pre-integration constraint using a second formula, where the IMU pre-integration constraint is: Among them, r p is the residual error of IMU about position, r q is the residual error of IMU about rotation, r v is the residual error of IMU about velocity, r ba is the residual of the IMU acceleration bias, r bg is the residual of the IMU angular velocity bias, is the rotation transformation from the world coordinate system to the IMU coordinate system at time t, is the translation transformation value from the IMU coordinate system to the world coordinate system at time j, is the translation change from the IMU coordinate system to the world coordinate system at time i, Δt is the time difference between time i and time j, g w is the gravity vector in the world coordinate system, is the inverse pre-integration of the first rotation pre-integration, is the multiplication between quaternions, [.] xyz is the real part of the quaternion, is the acceleration bias of the accelerometer at time j, is the acceleration bias of the accelerometer at time i, is the angular velocity zero bias of the gyroscope at time j, is the angular velocity bias of the gyroscope at time i, is the first position pre-integration, is the velocity pre-integral, Pre-integrate for the first rotation.

[0104] Optionally, the third construction unit includes: a fifth construction subunit, configured to construct a second position pre-integral of the wheel speed meter with respect to the position, the second position pre-integral being: in, is the pre-integral of the wheel speedometer position between the wheel speedometer data corresponding to time i and time j, is the relative rotation from time t to time i, q be is the rotational external parameter between the wheel speed meter and the IMU, v et is the speed value calculated by the wheel speed meter at time t, δt is the time difference between two frames of wheel speed meter data, time i is the time corresponding to the time when the camera captures the i-th frame image, and time j is the time corresponding to the time when the camera captures the j-th frame image; the sixth construction subunit is used to construct the wheel speed meter pre-integration constraint based on the second position pre-integration, and the pre-integration constraint is: in, is the rotation transformation from the world coordinate system to the IMU coordinate system at time t, is the translation transformation value from the IMU coordinate system to the world coordinate system at time j, is the translation change from the IMU coordinate system to the world coordinate system at time i, Pre-integrate for the second position.

[0105] Optionally, the first determining subunit includes: a first acquiring module for acquiring a plurality of first rotation pre-integrals between a plurality of adjacent image frames in the sliding window, determining a plurality of relative rotation angles based on the plurality of first rotation pre-integrals, and determining an average rotation angle of the plurality of relative rotation angles The second acquisition module is used to obtain multiple first position pre-integrals between multiple adjacent image frames in the sliding window, determine multiple relative translation distances based on the multiple first position pre-integrals, and determine a first average translation distance corresponding to the multiple relative translation distances. The third acquisition module is used to obtain multiple second position pre-integrals between multiple adjacent image frames in the sliding window, determine multiple relative translation distances based on the multiple second position pre-integrals, and determine the second average translation distances corresponding to the multiple relative translation distances. The second position pre-integral is the pre-integral of the wheel speed meter with respect to the position; the fourth acquisition module is used to obtain the rotation pre-integral of the IMU at the current moment and determine the relative rotation angle θ based on the rotation pre-integral b ; The fifth acquisition module is used to obtain the IMU position pre-integration α at the current moment b , using the wheel speed position pre-integral α′ b ; The first determination module is used to determine the first parameter, the second parameter and the third parameter, the first parameter is The second parameter is The third parameter is A road condition is determined based on the first parameter, the second parameter, and the third parameter.

[0106] Optionally, the first determination module includes: a first determination submodule, used to determine that the road surface condition is an abnormal condition when the first parameter meets the first condition, the second parameter meets the second condition, and the third parameter meets the third condition, wherein the first condition is that f1 is greater than or equal to the first threshold, the second condition is that f2 is greater than or equal to the second threshold, and the third condition is that f3 is less than or equal to the third threshold, and the abnormal condition is a condition where the flatness of the road surface is less than a preset flatness; a second determination submodule, used to determine that the road surface condition is a normal condition when the three parameters do not meet the three corresponding conditions at the same time.

[0107] Optionally, the first adjustment subunit includes: an adjustment module, which is used to reduce the weight corresponding to the IMU to a first preset weight, and increase the weight corresponding to the camera to a second preset weight, and increase the weight corresponding to the wheel speed meter to a third preset weight when the road condition is an abnormal condition.

[0108] Optionally, the first adjustment subunit includes: a first optimization module for optimizing the three weight calculations to obtain a fourth parameter, a fifth parameter, and a sixth parameter when the road condition is normal. The fourth parameter is is the pre-integration of IMU with respect to position, is the pre-integral of the wheel speed meter with respect to position, and the fifth parameter is is the pre-integration of IMU about rotation, and the sixth parameter is is the pre-integration of IMU about position, Th θ =2.3; a second optimization module, used to optimize the three weights through the fourth parameter, the fifth parameter and the sixth parameter.

[0109] Optionally, the second optimization module includes: a first optimization submodule, configured to optimize the weight corresponding to the wheel speed meter according to the fourth parameter and the fifth parameter by using a third formula, wherein the third formula is: T e =t1(1-t2); The second optimization submodule is used to optimize the weight corresponding to the IMU according to the fourth parameter and the fifth parameter through the fourth formula. The fourth formula is: T I =t2(1-t1); The third optimization submodule is used to optimize the weight corresponding to the camera according to the sixth parameter through the fifth formula. The fifth formula is: T c =1-t3.

[0110] The above-mentioned device for determining the transmission input torque includes a processor and a memory. The above-mentioned first construction unit, second construction unit, third construction unit and adjustment unit are all stored in the memory as program units, and the processor executes the above-mentioned program units stored in the memory to realize corresponding functions.

[0111] The processor includes a kernel, which retrieves the corresponding program unit from the memory. One or more kernels can be configured to adjust kernel parameters to address the issue of inaccurate AMT calibration caused by low input torque accuracy in existing AMTs.

[0112] The memory may include non-permanent memory in a computer-readable medium, random access memory (RAM) and / or non-volatile memory, such as read-only memory (ROM) or flash RAM, and the memory includes at least one memory chip.

[0113] An embodiment of the present invention provides a computer-readable storage medium having a program stored thereon, which implements the above-mentioned vehicle positioning method when executed by a processor.

[0114] An embodiment of the present invention provides a processor, which is used to run a program, wherein the vehicle positioning method is executed when the program is run.

[0115] An embodiment of the present invention provides a device, comprising a processor, a memory, and a program stored in the memory and executable on the processor. When the processor executes the program, at least the following steps are performed:

[0116] Step S101 , obtaining image data collected by a camera, and constructing a reprojection error constraint based on the image data, wherein the camera is set on a target vehicle.

[0117] Step S102: obtaining raw IMU data collected by the IMU, and constructing IMU pre-integration constraints between IMU data corresponding to adjacent moments based on the raw IMU data. The IMU is set on the target vehicle.

[0118] Step S103 , obtaining original wheel speed meter data collected by a wheel speed meter, and constructing a wheel speed meter pre-integration constraint between wheel speed meter data corresponding to adjacent moments based on the original wheel speed meter data, wherein the wheel speed meter is set on the target vehicle.

[0119] In step S104, the three weights corresponding to the camera, IMU, and wheel speed meter are adjusted according to the reprojection error constraint, the IMU pre-integration constraint, and the wheel speed meter pre-integration constraint, and the target vehicle's pose information is optimized based on the adjusted three weights to obtain the target pose information.

[0120] The devices in this article can be servers, PCs, PADs, mobile phones, etc.

[0121] The present application also provides a computer program product, which, when executed on a data processing device, is adapted to execute a program for initializing at least the following method steps:

[0122] Step S101 , obtaining image data collected by a camera, and constructing a reprojection error constraint based on the image data, wherein the camera is set on a target vehicle.

[0123] Step S102: obtaining raw IMU data collected by the IMU, and constructing IMU pre-integration constraints between IMU data corresponding to adjacent moments based on the raw IMU data. The IMU is set on the target vehicle.

[0124] Step S103 , obtaining original wheel speed meter data collected by a wheel speed meter, and constructing a wheel speed meter pre-integration constraint between wheel speed meter data corresponding to adjacent moments based on the original wheel speed meter data, wherein the wheel speed meter is set on the target vehicle.

[0125] In step S104, the three weights corresponding to the camera, IMU, and wheel speed meter are adjusted according to the reprojection error constraint, the IMU pre-integration constraint, and the wheel speed meter pre-integration constraint, and the target vehicle's pose information is optimized based on the adjusted three weights to obtain the target pose information.

[0126] According to another typical embodiment of the present application, a vehicle is also provided, including a positioning system, including a camera, an IMU, and a wheel speed meter; a controller of the positioning system, including one or more processors, a memory, and one or more programs, wherein the one or more programs are stored in the memory and configured to be executed by the one or more processors, and the one or more programs include a method for executing any one of the above methods.

[0127] In the above embodiments of the present invention, the description of each embodiment has its own focus. For parts that are not described in detail in a certain embodiment, reference can be made to the relevant descriptions of other embodiments.

[0128] In the several embodiments provided in this application, it should be understood that the disclosed technical content can be implemented in other ways. Among them, the device embodiments described above are only exemplary. For example, the division of the above-mentioned units can be a logical function division. In actual implementation, there may be other division methods, such as multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be through some interfaces, indirect coupling or communication connection of units or modules, which can be electrical or other forms.

[0129] The units described above as separate components may or may not be physically separate, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed across multiple units. Some or all of the units may be selected according to actual needs to achieve the purpose of the present embodiment.

[0130] In addition, the functional units in the various embodiments of the present invention may be integrated into a single processing unit, each unit may exist physically separately, or two or more units may be integrated into a single unit. The aforementioned integrated units may be implemented in the form of hardware or software functional units.

[0131] If the above-mentioned integrated unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, or the part that contributes to the prior art, or all or part of the technical solution can be embodied in the form of a software product. The computer software product is stored in a storage medium and includes several instructions for enabling a computer device (which can be a personal computer, server or network device, etc.) to execute all or part of the steps of the above-mentioned methods of each embodiment of the present invention. The aforementioned storage medium includes: various media that can store program codes, such as a USB flash drive, a read-only memory (ROM), a random access memory (RAM), a mobile hard disk, a magnetic disk or an optical disk.

[0132] From the above description, it can be seen that the above embodiments of the present application achieve the following technical effects:

[0133] 1) In the positioning method of the above-mentioned vehicle of the present application, by acquiring the image data collected by the camera, constructing the reprojection error constraint based on the image data, the camera is set on the target vehicle; acquiring the original IMU data collected by the IMU, constructing the IMU pre-integration constraint between the IMU data corresponding to adjacent moments based on the original IMU data, the IMU is set on the target vehicle; acquiring the original wheel speedometer data collected by the speedometer, constructing the wheel speedometer pre-integration constraint between the wheel speedometer data corresponding to adjacent moments based on the original wheel speedometer data, the wheel speedometer is set on the target vehicle; based on the reprojection error constraint, the IMU pre-integration constraint and the wheel speedometer pre-integration constraint, adjust the three weights corresponding to the camera, IMU and wheel speedometer respectively, and based on the adjusted three weights, optimize the target vehicle's posture information to obtain the target posture information. On the basis of the existing visual inertial positioning system, the wheel speedometer is tightly coupled, and the wheel speedometer pre-integration and its constraints are added to the adjacent images to jointly construct a factor graph optimization framework, which effectively improves the accuracy and robustness of the positioning system.

[0134] 2) In the positioning device of the above-mentioned vehicle of the present application, a first acquisition unit is used to acquire image data collected by a camera and construct a reprojection error constraint based on the image data, and the camera is set on the target vehicle; a second acquisition unit is used to acquire original IMU data collected by an IMU and construct an IMU pre-integration constraint between IMU data corresponding to adjacent moments based on the original IMU data, and the IMU is set on the target vehicle; a third acquisition unit is used to acquire original wheel speedometer data collected by a wheel speedometer and construct a wheel speedometer pre-integration constraint between wheel speedometer data corresponding to adjacent moments based on the original wheel speedometer data, and the wheel speedometer is set on the target vehicle; an adjustment unit is used to adjust the three weights corresponding to the camera, IMU and wheel speedometer respectively according to the reprojection error constraint, the IMU pre-integration constraint and the wheel speedometer pre-integration constraint, and optimize the position information of the target vehicle according to the adjusted three weights to obtain the target position information, which can determine whether the vehicle is in bumpy terrain such as potholes or speed bumps, uniform straight line or approximately uniform straight line motion, and the present invention can effectively detect abnormal data. By using the rotation, position pre-integration of the IMU and the position pre-integration of the wheel speedometer between the current moment and each historical adjacent image frame in the sliding window, a threshold is set to detect abnormal data.

[0135] 3) The above-mentioned vehicle of the present application includes a positioning system, including a camera, an IMU, and a wheel speedometer; the controller of the positioning system includes one or more processors, a memory, and one or more programs, wherein the one or more programs are stored in the memory and are configured to be executed by the one or more processors, and the one or more programs include a method for executing any of the items. This solves the technical problem in the related art that vehicles on bumpy roads are prone to sudden increases in IMU excitation, abnormal IMU data, and a sharp decrease in the accuracy of the visual inertial positioning system, or even sudden changes in the vehicle's positioning state. It also achieves the technical effect of continuously providing accurate and highly robust positioning information.

[0136] The above are merely preferred embodiments of the present application and are not intended to limit the present application. Those skilled in the art will readily appreciate that various modifications and variations are possible. Any modifications, equivalent substitutions, or improvements made within the spirit and principles of the present application shall be included within the scope of protection of the present application.

Claims

1. A vehicle positioning method, characterized in that: include: Obtaining image data captured by a camera, and constructing a reprojection error constraint based on the image data, wherein the camera is disposed on a target vehicle; Obtaining raw IMU data collected by an IMU, and constructing an IMU pre-integration constraint between IMU data corresponding to adjacent moments based on the raw IMU data, wherein the IMU is set on the target vehicle; Obtaining raw wheel speed meter data collected by a wheel speed meter, and constructing a wheel speed meter pre-integration constraint between wheel speed meter data corresponding to adjacent moments based on the raw wheel speed meter data, wherein the wheel speed meter is provided on the target vehicle; Acquire multiple first rotation pre-integrals between multiple adjacent image frames in the sliding window, determine multiple relative rotation angles based on the multiple first rotation pre-integrals, and determine an average rotation angle of the multiple relative rotation angles Acquire multiple first position pre-integrals between multiple adjacent image frames in the sliding window, determine multiple relative translation distances based on the multiple first position pre-integrals, and determine a first average translation distance corresponding to the multiple relative translation distances Acquire multiple second position pre-integrals between multiple adjacent image frames in the sliding window, determine multiple relative translation distances based on the multiple second position pre-integrals, and determine second average translation distances corresponding to the multiple relative translation distances The second position pre-integral is the pre-integral of the wheel speed meter with respect to the position; the rotation pre-integral of the IMU at the current moment is obtained, and the relative rotation angle θ is determined based on the rotation pre-integral b ; Get the position pre-integral α of the IMU at the current moment b , with the position pre-integrated α′ of the wheel speed meter b ; Determine the first parameter, the second parameter and the third parameter, the first parameter is The second parameter is The third parameter is determining a road surface condition based on the first parameter, the second parameter, and the third parameter; According to the road conditions, the three weights corresponding to the camera, the IMU, and the wheel speedometer are adjusted respectively, and the posture information of the target vehicle is optimized based on the adjusted three weights to obtain target posture information.

2. The method according to claim 1, characterized in that Obtain image data captured by a camera on a target vehicle, and construct a reprojection error constraint based on the image data, including: Obtaining an i-th frame image and a j-th frame image captured by the camera, and performing optical flow tracking on the i-th frame image and the j-th frame image, where the i-th frame image is a current frame image captured by the camera, and the j-th frame image is a previous frame image captured by the camera; Determining target feature points based on the optical flow tracking, wherein the target feature points are feature points that exist simultaneously on the i-th frame image and the j-th frame image; Calculate the projection coordinate value of the target feature point in the i-th frame image projected onto the camera coordinate system corresponding to the j-th frame image; The reprojection error constraint is constructed according to the projection coordinate value.

3. The method according to claim 2, characterized in that Calculating the projection coordinate value of the target feature point in the i-th frame image projected onto the camera coordinate system corresponding to the j-th frame image, including: The projection coordinate value is calculated by formula 1, which is: in, are respectively used to represent the normalized plane coordinates of the target feature points in the i-th frame image, is the transformation matrix from the IMU coordinate system to the world coordinate system corresponding to the i-th frame image, is the inverse matrix of the transformation matrix from the IMU coordinate system to the world coordinate system corresponding to the j-th frame image, T bc is the external parameter between the camera and the IMU, T bc The corresponding inverse matrix, λ is the inverse depth of the target feature point in the first observation image frame in the sliding window.

4. The method according to claim 3, characterized in that Constructing the reprojection error constraint according to the projection coordinate value includes: The reprojection error constraint is: in, is the projection coordinate value, The normalized plane coordinate value in the j-th frame image after optical flow tracking is used for the target feature point in the i-th frame image.

5. The method according to claim 1, wherein Obtain the IMU data collected by the IMU on the target vehicle, Constructing an IMU pre-integration constraint between IMU data corresponding to adjacent moments based on the IMU data, including: Based on the IMU data, a first position pre-integral of the IMU with respect to position is constructed, where the first position pre-integral is: in, is the first position pre-integral between the state quantities corresponding to time i and time j, is the relative rotation from time t to time i, is the measured value of acceleration in the IMU coordinate system at time t, δt is the time difference between two frames of IMU data, time i is the time corresponding to the camera capturing the i-th frame image, and time j is the time corresponding to the camera capturing the j-th frame image; Based on the IMU data, the velocity pre-integral of the IMU with respect to velocity is constructed, and the velocity pre-integral is: in, is the velocity pre-integral between the state quantities corresponding to the time i and the time j, is the relative rotation from time t to time i, is the measured value of acceleration in the IMU coordinate system at time t, and δt is the time difference between two frames of IMU data; According to the IMU data, a first rotation pre-integral of the IMU with respect to rotation is constructed, where the first rotation pre-integral is: in, is the first rotation pre-integral between the state quantities corresponding to the time i and the time j, is the relative rotation from time t to time i, and δt is the time difference between two frames of IMU data; The IMU pre-integration constraint is constructed based on the first position pre-integration, the velocity pre-integration, and the first rotation pre-integration.

6. The method according to claim 5, characterized in that Constructing the IMU pre-integration constraint based on the first position pre-integration, the velocity pre-integration, and the first rotation pre-integration includes: The IMU pre-integration constraint is constructed by the second formula, and the IMU pre-integration constraint is: Among them, r p is the residual error of the IMU about the position, r q is the residual error of the IMU with respect to rotation, r v is the residual error of the IMU about velocity, r ba is the residual of the IMU acceleration bias, r bg is the residual of the IMU angular velocity bias, is the rotation transformation from the world coordinate system to the IMU coordinate system at time t, is the translation transformation value from the IMU coordinate system to the world coordinate system at time j, is the translation change from the IMU coordinate system to the world coordinate system at time i, Δt is the time difference between the time i and the time j, g w is the gravity vector in the world coordinate system, is the inverse pre-integration of the first rotation pre-integration, is the multiplication between quaternions, [.] xyz is the real part of the quaternion, is the acceleration bias of the accelerometer at time j, is the acceleration bias of the accelerometer at time i, is the angular velocity zero bias of the gyroscope at time j, is the angular velocity zero bias of the gyroscope at the time i, is the first position pre-integration, is the velocity pre-integral, Pre-integrate for the first rotation.

7. The method according to claim 1, characterized in that Obtaining original wheel speed meter data collected by the wheel speed meter, and constructing a wheel speed meter pre-integration constraint between wheel speed meter data corresponding to adjacent moments based on the original wheel speed meter data, including: Construct a second position pre-integral of the wheel speed meter with respect to position, wherein the second position pre-integral is: in, is the pre-integral of the wheel speedometer with respect to position between the wheel speedometer data corresponding to time i and time j, is the relative rotation from time t to time i, q be is the rotational external parameter between the wheel speed meter and the IMU, v et is the speed value calculated by the wheel speed meter at time t, δt is the time difference between two frames of wheel speed meter data, time i is the time corresponding to the time when the camera captures the i-th frame image, and time j is the time corresponding to the time when the camera captures the j-th frame image; According to the second position pre-integration, the wheel speed meter pre-integration constraint is constructed, and the pre-integration constraint is: in, is the rotation transformation from the world coordinate system to the IMU coordinate system at time t, is the translation transformation value from the IMU coordinate system to the world coordinate system at time j, is the translation change from the IMU coordinate system to the world coordinate system at time i, Pre-integrate for the second position.

8. The method according to claim 1, characterized in that Determining the road surface condition based on the first parameter, the second parameter, and the third parameter includes: When the first parameter satisfies a first condition, the second parameter satisfies a second condition, and the third parameter satisfies a third condition, determining that the road surface operating condition is an abnormal operating condition, wherein the first condition is that f1 is greater than or equal to a first threshold, the second condition is that f2 is greater than or equal to a second threshold, and the third condition is that f3 is less than or equal to a third threshold, and the abnormal operating condition is a condition in which the flatness of the road surface is less than a preset flatness; When the three parameters do not satisfy the three corresponding conditions at the same time, it is determined that the road condition is a normal condition.

9. The method according to claim 8, characterized in that According to the road conditions, the three weights are adjusted, including: When the road condition is the abnormal condition, the weight corresponding to the IMU is reduced to the first preset weight, and the weight corresponding to the camera is increased to the second preset weight, and the weight corresponding to the wheel speed meter is increased to the third preset weight.

10. The method according to claim 8, characterized in that According to the road conditions, the three weights are adjusted, including: When the road condition is the normal condition, the three weights are optimized to obtain the fourth parameter, the fifth parameter and the sixth parameter. The fourth parameter is is the pre-integration of the IMU with respect to position, is the pre-integral of the wheel speed meter with respect to position, and the fifth parameter is is the pre-integral of the IMU with respect to rotation, and the sixth parameter is is the pre-integral of the IMU with respect to position, Th θ =2.3; The three weights are optimized using the fourth parameter, the fifth parameter, and the sixth parameter.

11. The method according to claim 10, characterized in that Optimizing the three weights by using the fourth parameter, the fifth parameter, and the sixth parameter includes: According to the fourth parameter and the fifth parameter, the weight corresponding to the wheel speed meter is optimized by the third formula, and the third formula is: e =t1(1-t2); According to the fourth parameter and the fifth parameter, the weight corresponding to the IMU is optimized by the fourth formula, and the fourth formula is: I =t2(1-t1); According to the sixth parameter, the weight corresponding to the camera is optimized by the fifth formula, and the fifth formula is: c =1-t3.

12. A vehicle positioning device, characterized in that: include: a first acquisition unit, configured to acquire image data captured by a camera, and construct a reprojection error constraint based on the image data, wherein the camera is disposed on a target vehicle; A second acquisition unit is configured to acquire raw IMU data collected by an IMU, and construct an IMU pre-integration constraint between IMU data corresponding to adjacent moments based on the raw IMU data, wherein the IMU is disposed on the target vehicle; a third acquisition unit, configured to acquire raw wheel speedometer data collected by a wheel speedometer, and construct a wheel speedometer pre-integration constraint between wheel speedometer data corresponding to adjacent moments based on the raw wheel speedometer data, wherein the wheel speedometer is provided on the target vehicle; An adjustment unit is configured to obtain a plurality of first rotation pre-integrals between a plurality of adjacent image frames in a sliding window, determine a plurality of relative rotation angles based on the plurality of first rotation pre-integrals, and determine an average rotation angle of the plurality of relative rotation angles. Acquire multiple first position pre-integrals between multiple adjacent image frames in the sliding window, determine multiple relative translation distances based on the multiple first position pre-integrals, and determine a first average translation distance corresponding to the multiple relative translation distances Acquire multiple second position pre-integrals between multiple adjacent image frames in the sliding window, determine multiple relative translation distances based on the multiple second position pre-integrals, and determine second average translation distances corresponding to the multiple relative translation distances The second position pre-integral is the pre-integral of the wheel speed meter with respect to the position; Obtain the rotation pre-integral of the IMU at the current moment, and determine the relative rotation angle θ based on the rotation pre-integral b ; Get the position pre-integral α of the IMU at the current moment b , with the position pre-integrated α′ of the wheel speed meter b ; Determine the first parameter, the second parameter and the third parameter, the first parameter is The second parameter is The third parameter is determining a road surface condition based on the first parameter, the second parameter, and the third parameter; According to the road conditions, the three weights corresponding to the camera, the IMU, and the wheel speedometer are adjusted respectively, and the posture information of the target vehicle is optimized based on the adjusted three weights to obtain target posture information.

13. A computer-readable storage medium, characterized in that The computer-readable storage medium includes a stored program, wherein the program executes the method according to any one of claims 1 to 11.

14. A processor, characterized in that: The processor is configured to run a program, wherein the program executes the method according to any one of claims 1 to 11 when running.

15. A positioning system, characterized in that: The vehicle positioning device comprises a camera, an IMU, a wheel speed meter and a vehicle positioning device, wherein the vehicle positioning device is used to execute the method according to any one of claims 1 to 11.

16. A vehicle, characterized in that: include: Positioning system, including camera, IMU, and wheel speedometer; The controller of the positioning system includes one or more processors, a memory and one or more programs, wherein the one or more programs are stored in the memory and are configured to be executed by the one or more processors, and the one or more programs include methods for executing any one of claims 1 to 11.

Images