Positioning method based on imu-odometer, parking method of vehicle, vehicle

Abstract

The application discloses a positioning method based on an IMU-wheel speed meter, a parking method of a vehicle and the vehicle. The positioning method based on the IMU-wheel speed meter comprises the following steps: constructing a prediction model according to historical acquisition data of an inertial sensor IMU, and constructing an observation model according to historical acquisition data of a wheel speed meter; obtaining a nominal state quantity according to current acquisition data of the IMU by using the prediction model, and obtaining an error quantity according to current acquisition data of the wheel speed meter by using the observation model; and obtaining a fusion positioning result according to the nominal state quantity and the error quantity. The method can guarantee the stability and reliability of positioning.

Description

Technical Field

[0001] This invention relates to the field of vehicle technology, and in particular to a positioning method based on an IMU (Inertial Measurement Unit) wheel speedometer, a vehicle parking method, and a vehicle. Background Technology

[0002] The stability and reliability of multi-sensor fusion positioning methods in related technologies become difficult to guarantee over time. Summary of the Invention

[0003] This invention aims to at least partially solve one of the technical problems in related technologies. Therefore, the first objective of this invention is to propose a positioning method based on an IMU-wheel speedometer to improve the stability and reliability of fused positioning.

[0004] The second objective of this invention is to provide a method for parking a vehicle.

[0005] A third objective of this invention is to provide a computer-readable storage medium.

[0006] The fourth objective of this invention is to provide a vehicle.

[0007] To achieve the above objectives, a first aspect of the present invention proposes a positioning method based on an IMU-wheel speed meter. The method includes: constructing a prediction model based on historical acquisition data of the inertial sensor (IMU); constructing an observation model based on historical acquisition data of the wheel speed meter; obtaining a nominal state quantity using the prediction model based on the current acquisition data of the IMU, and obtaining an error quantity using the observation model based on the current acquisition data of the wheel speed meter; and obtaining a fused positioning result based on the nominal state quantity and the error quantity.

[0008] The IMU-wheel speed meter-based positioning method of this invention first constructs a prediction model based on historical data collected by the inertial sensor (IMU), and then constructs an observation model based on historical data collected by the wheel speed meter. Next, the prediction model is used to obtain a nominal state quantity based on the current data collected by the IMU, and the observation model is used to obtain an error quantity based on the current data collected by the wheel speed meter. Finally, based on the nominal state quantity and the error quantity, a fused positioning result is obtained. Therefore, it is possible to fuse information from the IMU and wheel speed meter while considering the error quantity during fusion, ensuring the stability and reliability of the positioning.

[0009] To achieve the above objectives, a second aspect of the present invention provides a parking method for a vehicle, wherein the vehicle is equipped with an IMU and a wheel speed meter, the method comprising: when a parking command is received, obtaining a fusion positioning result of the vehicle using the above method; and performing parking control on the vehicle based on the fusion positioning result.

[0010] The parking method of the vehicle of the embodiment of the present application can improve the accuracy and efficiency of parking by using the above-mentioned positioning method based on the IMU-odometer to obtain the fusion positioning result of the vehicle.

[0011] To achieve the above-mentioned purpose, the third aspect of the present application provides a computer readable storage medium, which stores a computer program, and the computer program is executed by a processor to implement the above-mentioned positioning method based on the IMU-odometer or the above-mentioned parking method of the vehicle.

[0012] The computer readable storage medium of the embodiment of the present application, when implementing the above-mentioned positioning method based on the IMU-odometer, first constructs a prediction model according to the historical acquisition data of the inertial sensor IMU, constructs an observation model according to the historical acquisition data of the odometer, then obtains the nominal state quantity according to the current acquisition data of the IMU by using the prediction model, and obtains the error quantity according to the current acquisition data of the odometer by using the observation model, so as to obtain the fusion positioning result according to the nominal state quantity and the error quantity. Thus, the information of the IMU and the odometer can be fused, and the error quantity is considered during fusion, which ensures the stability and reliability of positioning. When implementing the above-mentioned parking method of the vehicle, the fusion positioning result of the vehicle is obtained by using the above-mentioned positioning method based on the IMU-odometer to park, which can improve the accuracy and efficiency of parking.

[0013] To achieve the above-mentioned purpose, the fourth aspect of the present application provides a vehicle, which comprises an IMU, an odometer and a controller, the controller is connected with the IMU and the odometer respectively, the controller comprises a memory, a processor and a computer program stored in the memory, and the computer program is executed by the processor to implement the above-mentioned positioning method based on the IMU-odometer or the above-mentioned parking method of the vehicle.

[0014] The vehicle of the embodiment of the present application, when implementing the above-mentioned positioning method based on the IMU-odometer, first constructs a prediction model according to the historical acquisition data of the inertial sensor IMU, constructs an observation model according to the historical acquisition data of the odometer, then obtains the nominal state quantity according to the current acquisition data of the IMU by using the prediction model, and obtains the error quantity according to the current acquisition data of the odometer by using the observation model, so as to obtain the fusion positioning result according to the nominal state quantity and the error quantity. Thus, the information of the IMU and the odometer can be fused, and the error quantity is considered during fusion, which ensures the stability and reliability of positioning. When implementing the above-mentioned parking method of the vehicle, the fusion positioning result of the vehicle is obtained by using the above-mentioned positioning method based on the IMU-odometer to park, which can improve the accuracy and efficiency of parking.

[0015] Additional aspects and advantages of the present application will be made apparent from the following description. BRIEF DESCRIPTION OF DRAWINGS

[0016] Figure 1 is a flowchart of an IMU-odometry based positioning method according to an embodiment of the present application;

[0017] Figure 2 is a schematic diagram of a vehicle kinematic model according to an embodiment of the present application;

[0018] Figure 3 is a flowchart of an IMU-odometry based positioning method according to an embodiment of the present application;

[0019] Figure 4 is a flowchart of a parking method of a vehicle according to an embodiment of the present application. DETAILED DESCRIPTION

[0020] Embodiments of the present application are described in detail below with reference to the accompanying drawings, in which like or similar elements are denoted by the same or similar reference signs, and examples of the embodiments are shown in the drawings. The embodiments described below by reference to the drawings are exemplary and are intended to explain the present application, and are not to be understood as limiting the present application.

[0021] An IMU-odometry based positioning method, a parking method of a vehicle, and a vehicle according to embodiments of the present application are described below with reference to the accompanying drawings.

[0022] Figure 1 is a flowchart of an IMU-odometry based positioning method according to an embodiment of the present application.

[0023] As shown in Figure 1 , the IMU-odometry based positioning method comprises:

[0024] S11, constructing a prediction model according to historical acquisition data of an inertial sensor IMU.

[0025] S12, constructing an observation model according to historical acquisition data of an odometry.

[0026] Specifically, when the positioning module receives a start parking instruction sent by a user, parameter initialization is first performed, including loading external parameters of the IMU and the odometry, noise covariance matrix, sensor fault threshold, and front and rear distance of the wheels.

[0027] After the parameter initialization, firstly, the current acquisition data of the IMU is acquired, including the timestamp, three-axis acceleration and angular velocity of the IMU, and the current acquisition data of the IMU is checked by using data bit consistency and consistency of the timestamps of the front and rear frames with the frequency of the IMU. The specific checking method is: if the current acquisition data is lost or the difference between the timestamps of the previous frame acquisition data is greater than the difference threshold value through the data bit and the timestamps of the front and rear frames, it is determined that the current acquisition data checking is not passed, and the next frame acquisition data is received.

[0028] The difference threshold value can be obtained from the IMU publishing frequency, that is, if the frequency of the two frames of timestamps is too different from the IMU publishing frequency, an IMU data abnormality alarm is sent.

[0029] Therefore, the current acquisition data of the IMU can be checked.

[0030] After the current acquisition data of the IMU is checked, the IMU needs to be judged for damage according to the prediction model and the current acquisition data of the IMU. Therefore, the first noise covariance matrix, the first state transition matrix and the prior covariance need to be calculated.

[0031] Specifically, the first state transition matrix and the first noise covariance matrix are obtained according to the prediction model and the current acquisition data of the IMU; the prior covariance is obtained according to the first state transition matrix and the first noise covariance matrix; if the prior covariance does not converge to the first threshold value and is greater than the second threshold value, it is determined that the IMU is damaged, otherwise it is determined that the IMU is normal.

[0032] The prediction model is represented by the following formula:

[0033]

[0034] Wherein, δp, δv, δq, δb a δb w , δg respectively represent position error, velocity error, rotation error, acceleration error, angular velocity error, gravity error, η v represents velocity noise, η q represents angle noise, η g represents gravity noise, η a represents acceleration noise, and Δt is the sampling period of the IMU, R represents the nominal rotation amount, a and w respectively represent the current measurement acceleration and the current measurement angle in the current acquisition data of the IMU.

[0035] After the prediction model is obtained, the first state transition matrix F is obtained by the following formula:

[0036]

[0037]

[0038] Wherein, the A matrix can be obtained from the above prediction model.

[0039] And the first noise covariance matrix Q can also be obtained by the following formula:

[0040]

[0041] Wherein, n w is the angular velocity noise, b a is the acceleration random walk, b w is the angular velocity random walk, and k is a proportional coefficient.

[0042] After obtaining the first state transition matrix F and the first noise covariance matrix Q, the prior covariance can be obtained by the following formula:

[0043]

[0044] Wherein, represents the posterior covariance at the i-1 time, represents the prior covariance at the i time.

[0045] If the current prior covariance does not converge to the first threshold value and is greater than the second threshold value, it is considered that the IMU is damaged, otherwise it is considered that the IMU is normal and continues to wait for the next frame. Wherein, the value of the above-mentioned second threshold value can be 3 times the preset first initial value.

[0046] Therefore, the damage of the IMU can be judged after the current collection data of the IMU is verified.

[0047] At the same time of obtaining the current collection data of the IMU, the current collection data of the wheel speed meter can also be obtained synchronously, including the steering angle and speed data.

[0048] The wheel speed and the front wheel steering angle can be obtained by the wheel speed encoder, so as to update the position and the angle. Assuming that the positioning method based on IMU-wheel speed meter of the application is used for vehicle parking, and the parking is in a horizontal plane, wherein the position and the angle can be derived by the vehicle kinematics model, as shown in the following formula: Figure 2 Wherein, the wheel speed is v, the vehicle body rotation angle is θ, the front and rear wheel distance is l, x W is the vehicle head direction at the starting time, y w is the left side of the vehicle at the starting time, is the front wheel steering angle, and the motion recursive process is as follows:

[0049] x i = x i-1 + v i cosθΔt,

[0050] y i = y i-1 + v i cos θ Δt,

[0051]

[0052] The above formula is the observation model.

[0053] It should be noted that the above constructing the prediction model and constructing the observation model can be based on the error state Kalman filter ESKF algorithm.

[0054] S13, using the prediction model to obtain the nominal state quantity according to the current collection data of the IMU, and using the observation model to obtain the error quantity according to the current collection data of the wheel speed meter.

[0055] Specifically, the above nominal state quantity includes a nominal rotation quantity, a nominal velocity and a nominal position, wherein,

[0056] The nominal rotation quantity at the i-th moment is predicted by the following formula:

[0057]

[0058] Wherein, The intermediate prediction rotation quantity from the (i-1)-th moment to the i-th moment is represented by q i The prediction rotation quantity at the i-th moment is represented by q

[0059] The nominal velocity at the i-th moment is predicted by the following formula:

[0060] V1 = R i *(a-b a )+g

[0061]

[0062] V2 = V3

[0063] V4 = R i-1 *(a-b a )+g

[0064]

[0065] Wherein, g is the acceleration of gravity.

[0066] The nominal position at the i-th moment is predicted by the following formula:

[0067] P1 = Vi-1​​​

[0068]

[0069]

[0070]

[0071]

[0072] Moreover, after obtaining the observation model and the current collection data of the wheel speed meter, damage of the wheel speed meter can be judged according to the observation model and the current collection data of the wheel speed meter. For this purpose, the current collection data of the wheel speed meter needs to be verified before damage of the wheel speed meter is judged. The specific verification method can refer to the method for verifying the current collection data of the IMU.

[0073] After the current collection data of the wheel speed meter is verified, damage of the wheel speed meter can be judged according to the observation model and the current collection data of the wheel speed meter, specifically including: obtaining a second state transition matrix and a second noise covariance matrix according to the observation model and the current collection data of the wheel speed meter; obtaining a posterior covariance according to the second state transition matrix and the second noise covariance matrix; if the posterior covariance does not converge to a third threshold value and is greater than a fourth threshold value, it is determined that the wheel speed meter is damaged, otherwise it is determined that the wheel speed meter is normal.

[0074] Firstly, the corresponding and is the position change amount of the wheel speed meter from the i-1 time to the i time, is the angle change amount of the wheel speed meter from the i-1 time to the i time, and then the motion distance and angle of the wheel are transferred to the IMU coordinate system of the first frame. The motion equation needs to be differentiated with respect to the error state quantity to obtain the second state transition matrix H.

[0075] The second state transition matrix H is obtained by the following formula:

[0076]

[0077] wherein represents the rotation of the wheel speed meter coordinate system to the IMU coordinate system, represents the position anti-symmetric matrix of the wheel speed meter in the inertial sensor coordinate system, represents the anti-symmetric matrix of the angular velocity of the wheel speed meter in the inertial sensor, represents the rotation quaternion representation of the wheel speed meter coordinate system to the inertial sensor coordinate system, represents the rotation matrix quaternion representation of the position of the wheel speed meter from the i-1 time to the i time, R{} denotes a right multiplication form of a rotation quaternion of an inertial sensor coordinate system to a wheel speedometer coordinate system, and R{} denotes a rotation quaternion conversion rotation matrix.

[0078] Considering the influence of vehicle slip, the position slip is decomposed into two directions to obtain a second noise covariance matrix R, and the second noise covariance matrix R is obtained by the following formula:

[0079]

[0080] Wherein, k1, k2, k3 respectively represent the first, second and third proportional coefficients, Δl represents the position offset from the i-1 time to the i time, Δv represents the speed offset from the i-1 time to the i time, The error amount of the angle from the i-1 time to the i time is represented.

[0081] Further, the update process of the error state quantity needs to calculate the corresponding Kalman gain and posterior covariance, and finally calculate the estimation of the corresponding error state quantity. Specifically, the posterior covariance is obtained by the following formula:

[0082]

[0083] Wherein, The prior covariance at the i time is represented. The posterior covariance at the i time is represented.

[0084] After obtaining the posterior covariance, if the current posterior covariance does not converge to the third threshold value and is greater than the fourth threshold value, it is considered that the wheel speedometer is damaged and an alarm is given, otherwise it is considered that the wheel speedometer is normal. The fourth threshold value can be 3 times the preset second initial value.

[0085] The error amount is obtained by the following formula:

[0086] δx=K(z-h(x))∈R 18*1 ,

[0087] Wherein, The Kalman gain is represented, z represents the wheel speed observation value, h(x) is the nominal value, and x is the input value of the IMU.

[0088] Optionally, as shown in the example of Figure 3 After the calculation of the error amount is completed, it can also be judged whether the wheel speedometer is damaged.

[0089] S14, according to the nominal state quantity and the error quantity, the fusion positioning result is obtained.

[0090] Specifically, the fusion positioning result is obtained by the following formula:

[0091]

[0092]

[0093]

[0094] (b a ) i =(b a ) i-1 +δx ba

[0095] (b g ) i =(b g ) i-1 +δx bg

[0096] g i =g i-1 +δx g ,

[0097] where δx p ,δx v ,δx R ,δb ba ,δb bg ,δx g are error quantities, the fused result is obtained by adding the error quantities to the previously predicted nominal quantities, and the above ( a ) i =(b a ) i-1 +x ba , g ) i =(b g ) i-1 +x bg , g i = i-1 +x g may be obtained by using the ESKF method.

[0098] In summary, the positioning method based on the IMU- wheel speed meter in the embodiment of the application firstly constructs a prediction model according to historical collection data of the inertial sensor IMU, constructs an observation model according to historical collection data of the wheel speed meter, then obtains a nominal state quantity according to current collection data of the IMU by using the prediction model, and obtains an error quantity according to current collection data of the wheel speed meter by using the observation model, so as to obtain a fusion positioning result according to the nominal state quantity and the error quantity. In this way, the information of the IMU and the wheel speed meter can be fused, and the error quantity is considered during the fusion, so that the stability and reliability of positioning are ensured. In addition, whether the IMU and the wheel speed meter are damaged is judged, so that the error term is prevented from continuously expanding beyond a threshold value due to damage, the stability and reliability of fusion are further ensured, and the sensor does not need to use professional equipment, which is convenient and fast.

[0099] Further, the application provides a parking method of a vehicle.

[0100] Figure 4 is a flowchart of the parking method of the vehicle in the embodiment of the application.

[0101] In the embodiment of the application, the IMU and the wheel speed meter are arranged on the vehicle. Specifically, since most of the vehicle-mounted devices are equipped with the wheel speed meter and the IMU sensor, the price is low, and the two sensors do not depend on the external signal source of the vehicle body, so that the fusion positioning can be effectively performed. Since the reliability of a single sensor cannot be guaranteed, the reliability of the sensor needs to be judged during the fusion positioning.

[0102] Considering that the sampling periods of the wheel speed meter and the IMU sensor are inconsistent, the above-mentioned positioning method based on the IMU- wheel speed meter is used to obtain the current position and attitude by integrating the sensor data; and since the data collection coordinate systems and positions of the wheel speed meter and the IMU are inconsistent, the wheel speed meter needs to be converted to the IMU coordinate system. When the covariance term of the fusion algorithm continuously converges to the expected range, it is considered that both sensors are effective, otherwise, the damaged sensor is judged according to the convergence of the covariance term, and an alarm is given for the sensor judged as damaged.

[0103] In addition, the fusion positioning algorithm based on the error state Kalman filter ESKF algorithm can avoid the singularity of adding an additional constraint after multi-parameterization.

[0104] As shown in Figure 4 , the parking method of the vehicle comprises:

[0105] S41, when the parking instruction is received, the fusion positioning result of the vehicle is obtained by using the above-mentioned positioning method based on the IMU- wheel speed meter.

[0106] S42, the vehicle is controlled to park according to the fusion positioning result.

[0107] The parking method of the vehicle of the embodiment of the present application can improve the precision and efficiency of parking by using the fusion positioning result of the vehicle obtained by the positioning method of the IMU- wheel speed meter.

[0108] Further, the present application provides a computer readable storage medium.

[0109] In the embodiment of the present application, the computer readable storage medium stores a computer program, and the computer program is executed by the processor to implement the positioning method based on the IMU- wheel speed meter or the parking method of the vehicle.

[0110] The computer readable storage medium of the embodiment of the present application, when implementing the positioning method based on the IMU- wheel speed meter, first constructs a prediction model according to the historical acquisition data of the inertial sensor IMU, constructs an observation model according to the historical acquisition data of the wheel speed meter, then obtains the nominal state quantity according to the current acquisition data of the IMU by using the prediction model, and obtains the error quantity according to the current acquisition data of the wheel speed meter by using the observation model, so as to obtain the fusion positioning result according to the nominal state quantity and the error quantity. Therefore, the information of the IMU and the wheel speed meter can be fused, and the error quantity is considered during the fusion, so as to ensure the stability and reliability of the positioning. Moreover, whether the IMU and the wheel speed meter are damaged is judged, so as to prevent the error term from continuously expanding beyond the threshold value due to the damage, further ensure the stability and reliability of the fusion, and make the sensor not need to use professional equipment, which is convenient and fast. When implementing the parking method of the vehicle, the fusion positioning result of the vehicle obtained by the positioning method of the IMU- wheel speed meter is used for parking, so as to improve the precision and efficiency of parking.

[0111] Further, the present application provides a vehicle.

[0112] In the embodiment of the present application, the vehicle includes an IMU, a wheel speed meter and a controller, the controller is connected with the IMU and the wheel speed meter respectively, the controller includes a memory, a processor and a computer program stored in the memory, and the computer program is executed by the processor to implement the positioning method based on the IMU- wheel speed meter or the parking method of the vehicle.

[0113] The vehicle of the embodiment of the application, when implementing the above-mentioned positioning method based on the IMU-odometer, firstly constructs a prediction model according to historical acquisition data of the inertial sensor IMU, constructs an observation model according to historical acquisition data of the odometer, then obtains a nominal state quantity according to current acquisition data of the IMU by using the prediction model, and obtains an error quantity according to current acquisition data of the odometer by using the observation model, so as to obtain a fusion positioning result according to the nominal state quantity and the error quantity. Thus, the information of the IMU and the odometer can be fused, and the error quantity is considered when fusing, so that the stability and reliability of positioning are ensured. Moreover, whether the IMU and the odometer are damaged is judged, so that the error term is prevented from continuously expanding beyond a threshold value due to damage, the stability and reliability of fusion are further ensured, and the sensor does not need to use professional equipment, which is convenient and fast. When implementing the above-mentioned parking method of the vehicle, the fusion positioning result of the vehicle is obtained by using the above-mentioned positioning method based on the IMU-odometer to park, so that the accuracy and efficiency of parking can be improved.

[0114] It should be noted that the logic and / or steps represented in the flowcharts and / or otherwise described herein, for example, can be considered as a list of executable instructions for implementing logic functions and can be specifically embodied in any computer-readable medium for use by or in connection with an instruction execution system, apparatus, or device, such as a computer-based system, processor- based system, or other system that can fetch the instructions from the instruction execution system, apparatus, or device and execute the instructions, or in conjunction with which the instructions can be executed. For the purposes of this specification, a "computer-readable medium" can be any apparatus that can contain, store, communicate, propagate, or transport the program for use by or in connection with the instruction execution system, apparatus, or device. More specific examples (a non-exhaustive list) of the computer-readable medium include the following: an electrical connection having one or more wires (electrical apparatus), a portable computer diskette (magnetic apparatus), a random-access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or Flash memory), an optical fiber (optical apparatus), and a portable compact disc read-only memory (CDROM). In addition, the computer-readable medium can even be paper or another suitable medium upon which the program is printed, because the program can be electronically obtained, for example, by optical scanning of the paper or other medium, followed by electronic

[0115] It should be understood that aspects of the application can be implemented in hardware, software, firmware or combinations thereof. In the embodiments described above, various steps or methods can be implemented, for example, through software or firmware in storage media which are executable by a suitable instruction execution system. For example, if implemented in hardware, as in another embodiment, any of the following technologies, known in the art, or their combinations, can be employed: discrete logic circuitry having logic gates for implementing logic functions upon an application of data signals, application-specific integrated circuits having appropriate combinational logic gates, programmable gate arrays (PGA), field-programmable gate arrays (FPGA), and so on.

[0116] In the description of the present application, the description of the terms "one embodiment", "some embodiments", "an example", "a specific example", or "some examples" etc. means that the specific features, structures, materials or characteristics described in connection with the embodiment or example are contained in at least one embodiment or example of the present application. In the present description, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described can be combined in any one or more embodiments or examples in a suitable manner.

[0117] In the description of the present application, it should be understood that the terms "center", "longitudinal", "transverse", "length", "width", "thickness", "upper", "lower", "front", "rear", "left", "right", "vertical", "horizontal", "top", "bottom", "inner", "outer", "clockwise", "counterclockwise", "axial", "radial", "circumferential" and the like indicate the orientation or positional relationship based on the orientation or positional relationship shown in the drawings, and are only for the convenience of describing the present application and simplifying the description, and do not indicate or imply that the devices or elements referred to must have a particular orientation, be constructed and operated in a particular orientation, and therefore cannot be understood as limiting the present application.

[0118] In addition, the terms "first", "second" are only for descriptive purposes and cannot be understood as indicating or implying relative importance or implicitly indicating the number of the technical features indicated. Therefore, the features defined with "first", "second" can explicitly or implicitly include at least one of the features. In the description of the present application, the meaning of "a plurality of" is at least two, for example, two, three, etc., unless otherwise specifically limited.

[0119] In the present application, unless specifically defined otherwise, the terms "mounting", "connected", "connecting", "fixed", and the like should be construed broadly, for example, can be fixed connection, can also be detachable connection, or integral; can be mechanical connection, can also be electrical connection; can be directly connected, can also be indirectly connected through an intermediate medium, can be the internal communication of two elements or the interaction relationship of two elements, unless specifically defined otherwise. For those skilled in the art, the specific meaning of the above terms in the present application can be understood according to the specific circumstances.

[0120] In the present application, unless specifically defined otherwise, the first feature is "on" or "under" the second feature can be that the first and second features are in direct contact, or the first and second features are in indirect contact through an intermediate medium. Moreover, the first feature "above", "over" and "on" the second feature can be that the first feature is directly above or obliquely above the second feature, or only indicates that the horizontal height of the first feature is higher than that of the second feature. The first feature "below", "under" and "under" the second feature can be that the first feature is directly below or obliquely below the second feature, or only indicates that the horizontal height of the first feature is less than that of the second feature.

[0121] Although the embodiments of the present application have been shown and described above, it can be understood that the above embodiments are exemplary and cannot be construed as limiting the present application, and those skilled in the art can make changes, modifications, replacements and variations to the above embodiments within the scope of the present application.

Claims

1. A positioning method based on an IMU-wheel speedometer, characterized in that, The method includes: A predictive model is built based on historical data collected by the inertial measurement unit (IMU). An observation model was constructed based on historical data collected from the wheel speed gauge. The nominal state quantity is obtained using the prediction model based on the current data collected by the IMU, and the error quantity is obtained using the observation model based on the current data collected by the wheel speed meter. The fused positioning result is obtained based on the nominal state quantity and the error quantity; The method further includes: Before obtaining the nominal state quantity, it is determined whether the IMU is damaged. If the IMU is not damaged, the step of obtaining the nominal state quantity using the prediction model based on the current data collected by the IMU is executed. Before obtaining the error amount, it is determined whether the wheel speed gauge is damaged. If the wheel speed gauge is not damaged, the step of obtaining the error amount based on the current collected data of the wheel speed gauge using the observation model is executed. The determination of whether the IMU is damaged includes: Based on the prediction model and the current data collected by the IMU, the first state transition matrix and the first noise covariance matrix are obtained. The prior covariance is obtained based on the first state transition matrix and the first noise covariance matrix; If the prior covariance does not converge to the first threshold and is greater than the second threshold, the IMU is determined to be damaged; otherwise, the IMU is determined to be normal.

2. The method according to claim 1, characterized in that, The method further includes: Before determining whether the IMU is damaged, the currently acquired data of the IMU is verified; Before determining whether the wheel speed gauge is damaged, the currently collected data of the wheel speed gauge is verified.

3. The method according to claim 2, characterized in that, Verification of the currently collected data includes: If the currently collected data is lost, or if the difference between the timestamp of the currently collected data and the timestamp of the previous frame of collected data is greater than the difference threshold, then the verification of the currently collected data is determined to be unsuccessful, and the system waits to receive the next frame of collected data.

4. The method according to claim 1, characterized in that, The determination of whether the wheel speed gauge is damaged includes: Based on the observation model and the current data collected by the wheel speed meter, the second state transition matrix and the second noise covariance matrix are obtained; Based on the second state transition matrix and the second noise covariance matrix, the posterior covariance is obtained; If the posterior covariance does not converge to the third threshold and is greater than the fourth threshold, the wheel speed gauge is determined to be damaged; otherwise, the wheel speed gauge is determined to be normal.

5. The method according to claim 1, characterized in that, The prediction model is expressed by the following formula: ， in, These represent position error, velocity error, rotational error, acceleration error, angular velocity error, and gravity error, respectively. Indicates speed noise, Indicates angular noise, Indicates gravity noise, Indicates acceleration noise. R represents the sampling period of the IMU, R represents the nominal rotation amount, and a and w represent the current measured acceleration and current measured angle in the current data collected by the IMU, respectively.

6. The method according to claim 5, characterized in that, The first noise covariance matrix Q is obtained by the following formula: ， in, For angular velocity noise, To accelerate random walk, For random walks with angular velocity, This is the proportionality coefficient; The first state transition matrix F is obtained by the following formula: .

7. The method according to claim 6, characterized in that, The prior covariance is obtained by the following formula: ， in, Let i be the posterior covariance at time i-1. Let represent the prior covariance at time i.

8. The method according to claim 6, characterized in that, The nominal state quantities include nominal rotation, nominal velocity, and nominal position, wherein, The nominal rotation at time i is expressed by the following formula. Make a prediction: ， in, , This represents the intermediate predicted rotation amount from time i-1 to time i. This represents the predicted rotation amount at time i. The nominal velocity at time i is expressed by the following formula. Make a prediction: ， Where g is the acceleration due to gravity; The nominal position at time i is obtained by the following formula. Make a prediction: 。 9. The method according to claim 4, characterized in that, The second state transition matrix H is obtained by the following formula: ， in This indicates that the external parameter wheel speedometer coordinate system has been rotated to the IMU coordinate system. The antisymmetric matrix representing the position of the wheel speedometer in the inertial sensor coordinate system. An antisymmetric matrix representing the angular velocity of the wheel speedometer under the inertial sensor. The rotational quaternion representation of the wheel speedometer coordinate system to the inertial sensor coordinate system. The rotation matrix quaternion representation of the position of the wheel speed gauge from time i-1 to time i. R{} represents the right-multiplication form of the rotation quaternion from the inertial sensor coordinate system to the wheel speedometer coordinate system, where R{} represents the quaternion-to-rotation matrix. The second noise covariance matrix R is obtained by the following formula: ， in, , , These represent the first, second, and third proportionality coefficients, respectively. This represents the position offset from time i-1 to time i. This represents the velocity offset from time i-1 to time i. This represents the error in the angle from time i-1 to time i.

10. The method according to claim 9, characterized in that, The posterior covariance is obtained using the following formula: in, Let i represent the prior covariance at time i. Let i represent the posterior covariance at time i. , representing the Kalman gain.

11. The method according to claim 10, characterized in that, The error amount is obtained by the following formula: ， Where z represents the observed wheel speed. is the nominal value, and x is the input value of the IMU.

12. The method according to claim 11, characterized in that, The fusion localization result is obtained using the following formula: ， in, Let be the error quantity.

13. A method for parking a vehicle, characterized in that, The vehicle is equipped with an IMU and a wheel speed meter, and the method includes: When a parking instruction is received, the vehicle's fusion positioning result is obtained using the method according to any one of claims 1-12; Parking control is performed on the vehicle based on the fusion positioning results.

14. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by a processor, it implements the method as described in any one of claims 1-12.

15. A vehicle, characterized in that, The device includes an IMU, a wheel speed meter, and a controller, wherein the controller is connected to the IMU and the wheel speed meter respectively, and the controller includes a memory, a processor, and a computer program stored in the memory. When the computer program is executed by the processor, it implements the method as described in any one of claims 1-12.

Images