Vehicle three-dimensional posture positioning estimation method, system, device and storage medium

Abstract

The present invention discloses a method, system, device and storage medium for estimating the three-dimensional posture positioning of a vehicle. The method includes: performing IMU trajectory derivation based on accelerometer zero bias and gyroscope zero bias, constructing a nominal discrete time motion equation of a three-dimensional posture nominal state variable; determining a three-dimensional posture error state variable corresponding to the three-dimensional posture nominal state variable, and obtaining a prediction error covariance matrix based on the three-dimensional posture error state variable; and obtaining a real-time three-dimensional posture result of the vehicle through ESKF fusion positioning based on three-dimensional posture semantic observations, velocity observations, nominal discrete time motion equations and prediction error covariance matrix. Compared with the prior art that only realizes the two-dimensional posture estimation of the vehicle and cannot meet the complex parking scene, the present invention fuses multi-source information based on the error state Kalman filter algorithm to obtain the three-dimensional posture estimation of the vehicle, meeting the needs of complex parking scenes.

Description

Technical Field

[0001] The present invention belongs to the technical field of autonomous driving, and in particular relates to a method, system, device and storage medium for estimating a three-dimensional position and posture of a vehicle. Background Art

[0002] With the development of electric vehicles, driverless technology and the increasing penetration of intelligent driving software, memory parking has gradually become a hot technology pursued by the automotive market. Memory parking means that after the user drives the car to the designated parking space for the first time, the car records its driving trajectory and detected road semantics through the on-board sensors to build a global map (this process is referred to as mapping). When the user sends a parking request next time, the car can follow the last path to complete automatic parking in or out (this process is referred to as tracking). The application scenario of memory parking is usually in underground parking lots with weak or no global navigation satellite system (GNSS) signals. How to accurately estimate the position and posture (referred to as posture) of the vehicle in the absence of a positioning source becomes the difficulty of this function.

[0003] At present, the mainstream low-cost solution in single-vehicle intelligence is to realize two-dimensional pose estimation based on the extended Kalman filter (EKF) (that is, the vehicle's motion is assumed to be two-dimensional plane motion, and the two-dimensional position and heading angle of the vehicle are estimated). It relies on two wheel speed encoders for track calculation as the prediction process of filtering; it relies on IMU and camera to observe the heading angle and pose respectively as the update process of the filter. The assumption of two-dimensional plane motion is difficult to meet the scenarios such as uphill and downhill and cross-layer. Therefore, how to accurately obtain the real-time three-dimensional pose results of the vehicle to meet the needs of complex parking scenarios has become an urgent problem to be solved.

[0004] The above contents are only used to assist in understanding the technical solution of the present invention and do not constitute an admission that the above contents are prior art. Summary of the invention

[0005] The main purpose of the present invention is to provide a vehicle three-dimensional posture positioning estimation method, system, device and storage medium, aiming to solve the technical problem of how to accurately obtain the real-time three-dimensional posture results of the vehicle to meet the needs of complex parking scenarios.

[0006] To achieve the above object, the present invention provides a method for estimating a three-dimensional position and posture of a vehicle, the method comprising:

[0007] Determine the vehicle's accelerometer and gyroscope biases;

[0008] Based on the accelerometer bias and the gyroscope bias, an IMU trajectory is derived to construct a nominal discrete-time motion equation of a three-dimensional posture nominal state variable;

[0009] Determine a three-dimensional posture error state variable corresponding to the three-dimensional posture nominal state variable, and obtain a prediction error covariance matrix based on the three-dimensional posture error state variable;

[0010] Semantically registering the local semantic map of the vehicle with the global semantic map to obtain a three-dimensional position and posture semantic observation;

[0011] The real-time three-dimensional pose result of the vehicle is obtained by fusion positioning through ESKF according to the three-dimensional pose semantic observation quantity, speed observation quantity, the nominal discrete time motion equation and the prediction error covariance matrix.

[0012] Optionally, the step of performing IMU trajectory derivation based on the accelerometer bias and the gyroscope bias to construct a nominal discrete-time motion equation of a three-dimensional posture nominal state variable comprises:

[0013] Based on the accelerometer bias and the gyroscope bias, an IMU trajectory is derived to obtain the vehicle's displacement, velocity, rotation data, gravitational acceleration, the original velocity of the accelerometer, and the original data of the gyroscope;

[0014] Constructing a three-dimensional posture nominal state variable according to the accelerometer bias, the gyroscope bias, the displacement, the velocity, the rotation data and the gravitational acceleration;

[0015] A nominal discrete-time motion equation is constructed according to the three-dimensional posture nominal state variables, the original velocity of the accelerometer and the original data of the gyroscope.

[0016] Optionally, the nominal discrete-time equation of motion is:

[0017]

[0018] In the formula, p(t+Δt) is the displacement at time t+Δt, p(t) is the displacement at time t, v(t) is the velocity at time t, Δt is the time increment, and R(t) is the rotation data at time t. is the original data of the accelerometer at time t, b a (t) is the accelerometer zero bias at time t, g(t) is the gravity acceleration at time t, v(t+Δt) is the velocity at time t+Δt, R(t+Δt) is the rotation data at time t+Δt, is the original data of the gyroscope at time t, b g (t) is the gyro bias at time t, b g (t+Δt) is the gyro zero bias at time t+Δt, b a (t+Δt) is the accelerometer zero bias at time t+Δt, and g(t+Δt) is the gravitational acceleration at time t+Δt.

[0019] Optionally, the step of obtaining a prediction error covariance matrix based on the three-dimensional pose error state variable comprises:

[0020] Constructing an error linearization equation based on the three-dimensional posture error state variable;

[0021] The error linearization equation is:

[0022] δx(t+Δt)=F(t)δx(t)+ω,ω~N(0,Q)

[0023]

[0024] Where δx(t) is the three-dimensional posture error state variable at time t, ω is noise, N is normal distribution, Q is process noise, F(t) is the Jacobian matrix, and I3 is a three-dimensional matrix;

[0025] The prediction error covariance matrix is ​​determined according to the error linearization equation.

[0026] Optionally, the step of determining the prediction error covariance matrix according to the error linearization equation comprises:

[0027] The error linearization equation is updated by using the Jacobian matrix to obtain an error covariance update formula;

[0028] The error covariance update formula is:

[0029] P(t+Δt)=F(t)P(t)F(t) T +Q

[0030] Where P(t+Δt) is the prediction error covariance matrix at time t+Δt, P(t) is the error covariance matrix at time t, and F(t) T is the transpose of the Jacobian matrix;

[0031] The prediction error covariance matrix is ​​determined according to the error covariance update formula.

[0032] Optionally, the step of obtaining the real-time three-dimensional pose result of the vehicle by fusion positioning through ESKF according to the three-dimensional pose semantic observation, the velocity observation, the nominal discrete time motion equation and the prediction error covariance matrix includes:

[0033] Constructing a state variable update equation according to the three-dimensional pose semantic observation, the velocity observation, the nominal discrete time motion equation and the prediction error covariance matrix;

[0034] Based on the state variable update equation, ESKF fusion positioning is performed to obtain a corrected nominal covariance matrix;

[0035] The real-time three-dimensional position result of the vehicle is determined according to the modified nominal covariance matrix.

[0036] Optionally, the state variable update equation is:

[0037]

[0038] Where K is the Kalman gain, P (t+Δt) is the forecast error covariance matrix, is the corrected error covariance matrix, x (t+Δt) is the nominal discrete-time equation of motion, is the corrected nominal covariance matrix, Φ is the covariance matrix of the observation noise, z is the observed quantity, h is the observation state transfer matrix, H is the Jacobian matrix of the observation equation relative to the error state variable, H T is the transpose of the Jacobian matrix of the observation equation relative to the error state variable, and I is the identity matrix.

[0039] In addition, to achieve the above-mentioned purpose, the present invention also proposes a vehicle three-dimensional posture positioning estimation system, the vehicle three-dimensional posture positioning estimation system comprising:

[0040] A determination module, used for determining the accelerometer bias and the gyroscope bias of the vehicle;

[0041] A construction module is used to perform IMU trajectory derivation based on the accelerometer bias and the gyroscope bias, and construct a nominal discrete time motion equation of the nominal state variables of the three-dimensional posture;

[0042] A prediction module, used to determine a three-dimensional posture error state variable corresponding to the three-dimensional posture nominal state variable, and obtain a prediction error covariance matrix based on the three-dimensional posture error state variable;

[0043] A registration module, used for semantically registering the local semantic map and the global semantic map of the vehicle to obtain a three-dimensional position and posture semantic observation;

[0044] The output module is used to obtain the real-time three-dimensional pose result of the vehicle through ESKF fusion positioning according to the three-dimensional pose semantic observation, velocity observation, the nominal discrete time motion equation and the prediction error covariance matrix.

[0045] In addition, to achieve the above-mentioned purpose, the present invention also proposes a vehicle three-dimensional posture positioning estimation device, which includes: a memory, a processor, and a vehicle three-dimensional posture positioning estimation program stored in the memory and executable on the processor, and the vehicle three-dimensional posture positioning estimation program is configured to implement the steps of the vehicle three-dimensional posture positioning estimation method as described above.

[0046] In addition, to achieve the above-mentioned purpose, the present invention also proposes a storage medium, on which a vehicle three-dimensional posture positioning estimation program is stored. When the vehicle three-dimensional posture positioning estimation program is executed by a processor, the steps of the vehicle three-dimensional posture positioning estimation method as described above are implemented.

[0047] The present invention first determines the accelerometer bias and gyroscope bias of the vehicle, then derives the IMU trajectory based on the accelerometer bias and gyroscope bias, constructs the nominal discrete time motion equation of the three-dimensional posture nominal state variable, then determines the three-dimensional posture error state variable corresponding to the three-dimensional posture nominal state variable, and obtains the prediction error covariance matrix based on the three-dimensional posture error state variable, semantically aligns the local semantic map and the global semantic map of the vehicle to obtain the three-dimensional posture semantic observation, and finally obtains the real-time three-dimensional posture result of the vehicle through the fusion positioning of the three-dimensional posture semantic observation, the speed observation, the nominal discrete time motion equation and the prediction error covariance matrix through the ESKF. Compared with the prior art that only realizes the two-dimensional posture estimation of the vehicle, resulting in inaccurate positioning, which cannot meet the complex parking scene, this embodiment fuses multi-source information based on the error state Kalman filter algorithm to obtain the three-dimensional posture estimation of the vehicle, which meets the needs of complex parking scenes. BRIEF DESCRIPTION OF THE DRAWINGS

[0048] Figure 1 It is a structural schematic diagram of a vehicle three-dimensional posture positioning estimation device in a hardware operating environment involved in an embodiment of the present invention;

[0049] Figure 2 A schematic diagram of a flow chart of a first embodiment of a method for estimating a vehicle's three-dimensional posture positioning according to the present invention;

[0050] Figure 3 This is a fusion positioning process framework diagram of the first embodiment of the vehicle three-dimensional posture positioning estimation method of the present invention;

[0051] Figure 4 This is a structural block diagram of the first embodiment of the vehicle three-dimensional posture positioning estimation system of the present invention.

[0052] The realization of the purpose, functional features and advantages of the present invention will be further explained in conjunction with embodiments and with reference to the accompanying drawings. DETAILED DESCRIPTION

[0053] It should be understood that the specific embodiments described herein are only used to explain the present invention, and are not used to limit the present invention.

[0054] Reference Figure 1 , Figure 1 The present invention is a schematic diagram of the structure of a vehicle three-dimensional posture positioning estimation device in the hardware operating environment involved in the embodiment of the present invention.

[0055] like Figure 1 As shown, the vehicle three-dimensional posture positioning estimation device may include: a processor 1001, such as a central processing unit (CPU), a communication bus 1002, a user interface 1003, a network interface 1004, and a memory 1005. Among them, the communication bus 1002 is used to realize the connection and communication between these components. The user interface 1003 may include a display screen (Display), an input unit such as a keyboard (Keyboard), and the optional user interface 1003 may also include a standard wired interface and a wireless interface. The network interface 1004 may optionally include a standard wired interface and a wireless interface (such as a wireless fidelity (Wireless-Fidelity, Wi-Fi) interface). The memory 1005 may be a high-speed random access memory (Random Access Memory, RAM), or a stable non-volatile memory (Non-Volatile Memory, NVM), such as a disk storage. The memory 1005 may also be a storage system independent of the aforementioned processor 1001.

[0056] Those skilled in the art will understand that Figure 1 The structure shown in the figure does not constitute a limitation on the vehicle three-dimensional posture positioning estimation device, and may include more or fewer components than shown in the figure, or a combination of certain components, or a different arrangement of components.

[0057] like Figure 1 As shown, the memory 1005 as a storage medium may include an operating system, a network communication module, a user interface module, and a vehicle three-dimensional posture positioning estimation program.

[0058] exist Figure 1 In the vehicle three-dimensional posture positioning estimation device shown, the network interface 1004 is mainly used for data communication with the network server; the user interface 1003 is mainly used for data interaction with the user; the processor 1001 and the memory 1005 in the vehicle three-dimensional posture positioning estimation device of the present invention can be set in the vehicle three-dimensional posture positioning estimation device, and the vehicle three-dimensional posture positioning estimation device calls the vehicle three-dimensional posture positioning estimation program stored in the memory 1005 through the processor 1001, and executes the vehicle three-dimensional posture positioning estimation method provided by the embodiment of the present invention.

[0059] The embodiment of the present invention provides a method for estimating the three-dimensional position and posture of a vehicle, referring to Figure 2 , Figure 2 The figure is a flow chart of the first embodiment of the method for estimating the three-dimensional posture of a vehicle according to the present invention.

[0060] In this embodiment, the vehicle three-dimensional posture positioning estimation method includes the following steps:

[0061] Step S10: Determine the accelerometer bias and gyroscope bias of the vehicle.

[0062] It is easy to understand that the execution subject of this embodiment can be a vehicle three-dimensional posture positioning estimation system with functions such as data processing, network communication and program running, or it can be other computer equipment with similar functions, etc., and this embodiment is not limited.

[0063] It should be noted that since the zero bias of the accelerometer and gyroscope is extremely easily affected by environmental factors such as temperature and power on and off, it is necessary to ensure that the vehicle is stationary for 1 to 2 seconds before each use to calculate the vehicle's accelerometer zero bias and gyroscope zero bias.

[0064] Step S20: deriving the IMU trajectory based on the accelerometer bias and the gyroscope bias, and constructing a nominal discrete-time motion equation of the three-dimensional posture nominal state variables.

[0065] It should also be noted that in ESKF, the state variable to be determined is usually called the nominal state variable, the error of the state variable is called the error state variable, and the sum of the two is called the true value. ESKF is a variation of Kalman filtering. Like the traditional EKF model, it includes the prediction and update process of the state quantity, but its operation object is the error state variable.

[0066] In this embodiment, a minimized parameter expression may be used to deal with the rotation processing, and the value of the state quantity may be always kept small, thereby simplifying the calculation and avoiding singular values.

[0067] The nominal state variables are: x = [p, v, R, b g ,b a ,g] T , where p is displacement, v is velocity, R is rotation, and b g is the gyroscope bias, b a is the zero bias of the accelerometer, g is the acceleration due to gravity, and each quantity is a three-dimensional vector.

[0068] Relationship between nominal state variable and error state variable: x t =x+δx, where x t is true value.

[0069] In the specific implementation, the state quantity and covariance need to be initialized. The state quantity x and δx are both initialized to 18x1 dimensional zero matrices, and the covariance P is initialized to 18x18 dimensional unit matrix.

[0070] The initialization formula of process noise Q (18x18 dimensional matrix) is as follows:

[0071] Q=diag(03,(σ a *σ a )3,(σ g *σ g )3,(σ bg *σ bg )3,(σ ba *σ ba )3,03)

[0072] Among them, σ g ,σ a Denote the measurement noise standard deviation of the gyroscope and accelerometer, σ bg ,σ ba Represents the standard deviation of the zero bias wander of the gyroscope and accelerometer, respectively. diag represents a matrix constructed by diagonal elements, and (*)3 represents a three-dimensional matrix with * as the diagonal.

[0073] It should be understood that the measurement noise standard deviation and random walk standard deviation of the gyroscope and accelerometer are generally given by experience, and in actual use, the parameters need to be manually adjusted according to the fusion effect.

[0074] It should also be noted that the observation noise needs to be initialized. For the speed observation obtained by wheel speed pulse calculation, its noise Φ encoder The initialization formula for (3x3 dimensional matrix) is:

[0075] Φ encoder =(σ encoder *σ encoder )3

[0076] Among them, σ encoder Represents the standard deviation of wheel speed pulse measurement noise.

[0077] For the 3D pose observations calculated by semantic registration, the noise Φ camera The initialization formula for (6x6 dimensional matrix) is:

[0078] Φ camera =diag((σ p *σ p )3,(σ r *σ r )3)

[0079] Among them, σ p and σ r represent the standard deviation of the measurement noise of semantic registration in displacement and rotation, respectively.

[0080] In this embodiment, the IMU trajectory is derived based on the accelerometer bias and the gyroscope bias to obtain the vehicle's displacement, velocity, rotation data, gravitational acceleration, the original velocity of the accelerometer and the original data of the gyroscope; the three-dimensional posture nominal state variables are constructed according to the accelerometer bias, the gyroscope bias, the displacement, the velocity, the rotation data and the gravitational acceleration; the nominal discrete-time motion equation is constructed according to the three-dimensional posture nominal state variables, the original velocity of the accelerometer and the original data of the gyroscope.

[0081] The nominal discrete-time equation of motion is:

[0082]

[0083] In the formula, p(t+Δt) is the displacement at time t+Δt, p(t) is the displacement at time t, v(t) is the velocity at time t, Δt is the time increment, and R(t) is the rotation data at time t. is the original data of the accelerometer at time t, b a (t) is the accelerometer zero bias at time t, g(t) is the gravity acceleration at time t, v(t+Δt) is the velocity at time t+Δt, R(t+Δt) is the rotation data at time t+Δt, is the original data of the gyroscope at time t, b g (t) is the gyro bias at time t, b g (t+Δt) is the gyro zero bias at time t+Δt, b a (t+Δt) is the accelerometer zero bias at time t+Δt, g(t+Δt) is the gravitational acceleration at time t+Δt, Exp represents the index mapping in the Lie group, and the formulas about rotation are all expressed in Lie group.

[0084] Step S30: Determine the three-dimensional posture error state variable corresponding to the three-dimensional posture nominal state variable, and obtain a prediction error covariance matrix based on the three-dimensional posture error state variable.

[0085] Furthermore, an error linearization equation is constructed based on the three-dimensional posture error state variables; and the prediction error covariance matrix is ​​determined according to the error linearization equation.

[0086] The error linearization equation is:

[0087]

[0088] Where δx(t) is the three-dimensional posture error state variable at time t, ω is the noise, N is the normal distribution, Q is the process noise, F(t) is the linearized Jacobian matrix, and I3 is the three-dimensional matrix.

[0089] The processing method for determining the prediction error covariance matrix according to the error linearization equation is to update the error linearization equation through the Jacobian matrix to obtain the error covariance update formula; and determine the prediction error covariance matrix according to the error covariance update formula.

[0090] The error covariance update formula is:

[0091] P(t+Δt)=F(t)P(t)F(t) T +Q

[0092] Where P(t+Δt) is the prediction error covariance matrix at time t+Δt, P(t) is the error covariance matrix at time t, and F(t) T is the transpose of the Jacobian matrix.

[0093] Step S40: semantically aligning the local semantic map and the global semantic map of the vehicle to obtain a three-dimensional position and posture semantic observation.

[0094] In this embodiment, the observation-related variables are adjusted accordingly with the change of the sensor. This embodiment uses two observation quantities, one is the speed observation based on the wheel speed encoder, and the other is the three-dimensional posture observation based on semantic registration.

[0095] It should also be noted that, during the preprocessing of the wheel speed encoder, the change in the number of pulses is calculated based on bit operations to avoid jumps near the maximum number of pulses.

[0096] Speed ​​Observation:

[0097] The observed quantity z obtained based on the wheel speed pulse is [Rv observe ] T , where v observe represents the wheel speed calculated from the wheel speed pulse, and R is the posture estimated by the current filter.

[0098] The corresponding update amounts are as follows:

[0099] zh(x)=[Rv observe -v] T

[0100] The corresponding Jacobian matrix is:

[0101] H=[0 I3 0 0 0 0]

[0102] 3D pose observation:

[0103] Based on the semantic alignment of the local semantic map and the global semantic map of the vehicle's current position, the observation z = [p observe ,R observe ] T , that is, the three-dimensional translation and rotation.

[0104] The corresponding update amount is expressed in manifold as follows:

[0105] zh(x)=[p observe -p,Log(R T R observe )] T

[0106] The corresponding Jacobian matrix is:

[0107]

[0108] It should also be noted that the semantic information in the vehicle's surrounding environment (such as lane lines, arrows, parking space lines, pillars, signs, etc.) is extracted through the camera to generate a local semantic map; the position and posture of the semantics in the global map are solved based on the output position and posture of the previous frame fusion positioning; the local semantic map is aligned with the global semantic map obtained by mapping, and the position and posture of the local semantic map are corrected.

[0109] Step S50: According to the three-dimensional pose semantic observation, the velocity observation, the nominal discrete-time motion equation and the prediction error covariance matrix, the real-time three-dimensional pose result of the vehicle is obtained through ESKF fusion positioning.

[0110] In the specific implementation, refer to Figure 3 , Figure 3 This is a fusion positioning process framework diagram of the first embodiment of the vehicle three-dimensional posture positioning estimation method of the present invention. The state variable update equation is constructed according to the three-dimensional posture semantic observation quantity, the velocity observation quantity, the nominal discrete time motion equation and the prediction error covariance matrix; the ESKF fusion positioning is performed based on the state variable update equation to obtain the corrected nominal covariance matrix; the real-time three-dimensional posture result of the vehicle is determined according to the corrected nominal covariance matrix, and the real-time three-dimensional posture result of the vehicle is [p, R].

[0111] The state variable update equation is:

[0112]

[0113] Where K is the Kalman gain, P (t+Δt) is the forecast error covariance matrix, is the corrected error covariance matrix, x (t+Δt) is the nominal discrete-time equation of motion, is the corrected nominal covariance matrix, Φ is the covariance matrix of the observation noise, z is the observed quantity, h is the observation state transfer matrix, H is the Jacobian matrix of the observation equation relative to the error state variable, H T is the transpose of the Jacobian matrix of the observation equation relative to the error state variable, and I is the identity matrix.

[0114] It should also be noted that

[0115]

[0116] in, is the observation noise, and Φ is the covariance matrix of the observation noise.

[0117] It should also be understood that after the prediction and update, the estimate of the error state is corrected. Next, it is only necessary to classify the error state into the nominal state and reset the mean of the ESKF. The specific formula is as follows:

[0118] δx=0

[0119] In order to save computational effort, the covariance adjustment is ignored. Directly used as the starting value for the next iteration.

[0120] In this embodiment, the accelerometer bias and gyroscope bias of the vehicle are first determined, and then the IMU trajectory is derived based on the accelerometer bias and gyroscope bias, and the nominal discrete time motion equation of the three-dimensional posture nominal state variable is constructed. Then, the three-dimensional posture error state variable corresponding to the three-dimensional posture nominal state variable is determined, and the prediction error covariance matrix is ​​obtained based on the three-dimensional posture error state variable, and the local semantic map of the vehicle and the global semantic map are semantically aligned to obtain the three-dimensional posture semantic observation quantity, and finally, the real-time three-dimensional posture result of the vehicle is obtained through the fusion positioning of the three-dimensional posture semantic observation quantity, the speed observation quantity, the nominal discrete time motion equation and the prediction error covariance matrix through ESKF. Compared with the prior art that only realizes the two-dimensional posture estimation of the vehicle, resulting in inaccurate positioning and unable to meet complex parking scenarios, this embodiment only requires IMU, camera, and wheel speed encoder to reduce costs; at the same time, based on the error state Kalman filter algorithm, multi-source information is fused to obtain the three-dimensional posture estimation of the vehicle, and elevation, roll angle, and pitch angle can be additionally provided to cope with complex scenarios such as uphill and downhill and cross-layer.

[0121] Reference Figure 4 , Figure 4 This is a structural block diagram of the first embodiment of the vehicle three-dimensional posture positioning estimation system of the present invention.

[0122] like Figure 4 As shown, the vehicle three-dimensional posture positioning estimation system proposed in the embodiment of the present invention includes:

[0123] A determination module 4001 is used to determine the accelerometer bias and gyroscope bias of the vehicle;

[0124] A construction module 4002 is used to perform IMU trajectory derivation based on the accelerometer bias and the gyroscope bias, and construct a nominal discrete time motion equation of a three-dimensional posture nominal state variable;

[0125] A prediction module 4003 is used to determine a three-dimensional posture error state variable corresponding to the three-dimensional posture nominal state variable, and obtain a prediction error covariance matrix based on the three-dimensional posture error state variable;

[0126] A registration module 4004 is used to perform semantic registration on the local semantic map and the global semantic map of the vehicle to obtain a three-dimensional position and posture semantic observation;

[0127] The output module 4005 is used to obtain the real-time three-dimensional pose result of the vehicle through ESKF fusion positioning based on the three-dimensional pose semantic observation, velocity observation, the nominal discrete time motion equation and the prediction error covariance matrix.

[0128] Other embodiments or specific implementations of the vehicle three-dimensional posture positioning estimation system of the present invention can refer to the above-mentioned method embodiments and will not be repeated here.

[0129] It should be noted that, in this article, the terms "include", "comprises" or any other variations thereof are intended to cover non-exclusive inclusion, so that a process, method, article or system including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or system. In the absence of further restrictions, an element defined by the sentence "comprises a ..." does not exclude the existence of other identical elements in the process, method, article or system including the element.

[0130] The serial numbers of the above embodiments of the present invention are only for description and do not represent the advantages or disadvantages of the embodiments.

[0131] Through the description of the above implementation methods, those skilled in the art can clearly understand that the above-mentioned embodiment methods can be implemented by means of software plus a necessary general hardware platform, and of course by hardware, but in many cases the former is a better implementation method. Based on such an understanding, the technical solution of the present invention, or the part that contributes to the prior art, can be embodied in the form of a software product, which is stored in a storage medium (such as a read-only memory / random access memory, a magnetic disk, or an optical disk), and includes a number of instructions for a terminal device (which can be a mobile phone, a computer, a server, or a network device, etc.) to execute the methods described in each embodiment of the present invention.

[0132] The above are only preferred embodiments of the present invention, and are not intended to limit the patent scope of the present invention. Any equivalent structure or equivalent process transformation made using the contents of the present invention specification and drawings, or directly or indirectly applied in other related technical fields, are also included in the patent protection scope of the present invention.

Claims

1. A method for estimating a vehicle's three-dimensional posture positioning, characterized in that: The vehicle three-dimensional posture positioning and estimation method comprises the following steps: Determine the vehicle's accelerometer and gyroscope biases; Based on the accelerometer bias and the gyroscope bias, an IMU trajectory is derived to construct a nominal discrete-time motion equation of a three-dimensional posture nominal state variable; Determine a three-dimensional posture error state variable corresponding to the three-dimensional posture nominal state variable, and obtain a prediction error covariance matrix based on the three-dimensional posture error state variable; Semantically registering the local semantic map of the vehicle with the global semantic map to obtain a three-dimensional position and posture semantic observation; The real-time three-dimensional pose result of the vehicle is obtained by fusion positioning through ESKF according to the three-dimensional pose semantic observation quantity, speed observation quantity, the nominal discrete time motion equation and the prediction error covariance matrix.

2. The method according to claim 1, characterized in that The step of performing IMU trajectory derivation based on the accelerometer bias and the gyroscope bias to construct a nominal discrete-time motion equation of a three-dimensional posture nominal state variable comprises: Based on the accelerometer bias and the gyroscope bias, an IMU trajectory is derived to obtain the displacement, velocity, rotation data, gravity acceleration, original velocity of the accelerometer, and original data of the gyroscope of the vehicle; Constructing a three-dimensional posture nominal state variable according to the accelerometer bias, the gyroscope bias, the displacement, the velocity, the rotation data and the gravitational acceleration; A nominal discrete-time motion equation is constructed according to the three-dimensional posture nominal state variables, the original velocity of the accelerometer and the original data of the gyroscope.

3. The method according to claim 2, characterized in that The nominal discrete-time equation of motion is: In the formula, p(t+Δt) is the displacement at time t+Δt, p(t) is the displacement at time t, v(t) is the velocity at time t, Δt is the time increment, and R(t) is the rotation data at time t. is the original data of the accelerometer at time t, b a (t) is the accelerometer zero bias at time t, g(t) is the gravity acceleration at time t, v(t+Δt) is the velocity at time t+Δt, R(t+Δt) is the rotation data at time t+Δt, is the original data of the gyroscope at time t, b g (t) is the gyro zero bias at time t, b g (t+Δt) is the gyro zero bias at time t+Δt, b a (t+Δt) is the accelerometer zero bias at time t+Δt, and g(t+Δt) is the gravitational acceleration at time t+Δt.

4. The method according to claim 3, characterized in that The step of obtaining a prediction error covariance matrix based on the three-dimensional posture error state variable comprises: Constructing an error linearization equation based on the three-dimensional posture error state variable; The error linearization equation is: δx(t+Δt)=F(t)δx(t)+ω,ω~N(0,Q) Where δx(t) is the three-dimensional posture error state variable at time t, ω is noise, N is normal distribution, Q is process noise, F(t) is the Jacobian matrix, and I3 is a three-dimensional matrix; The prediction error covariance matrix is ​​determined according to the error linearization equation.

5. The method according to claim 4, characterized in that The step of determining the prediction error covariance matrix according to the error linearization equation comprises: The error linearization equation is updated by using the Jacobian matrix to obtain an error covariance update formula; The error covariance update formula is: P(t+Δt)=F(t)P(t)F(t) T +Q Where P(t+Δt) is the prediction error covariance matrix at time t+Δt, P(t) is the error covariance matrix at time t, and F(t) T is the transpose of the Jacobian matrix; The prediction error covariance matrix is ​​determined according to the error covariance update formula.

6. The method according to claim 5, characterized in that The step of obtaining the real-time three-dimensional pose result of the vehicle through ESKF fusion positioning according to the three-dimensional pose semantic observation, the speed observation, the nominal discrete time motion equation and the prediction error covariance matrix includes: Constructing a state variable update equation according to the three-dimensional pose semantic observation, the velocity observation, the nominal discrete time motion equation and the prediction error covariance matrix; Based on the state variable update equation, ESKF fusion positioning is performed to obtain a corrected nominal covariance matrix; The real-time three-dimensional position result of the vehicle is determined according to the modified nominal covariance matrix.

7. The method according to claim 6, characterized in that The state variable update equation is: K=P (t+Δt) H T (HP (t+Δt) H T +Φ) -1 δx=K(z-h(x (t+Δt) )) Where K is the Kalman gain, P (t+Δt) is the forecast error covariance matrix, is the corrected error covariance matrix, x (t+Δt) is the nominal discrete-time equation of motion, is the corrected nominal covariance matrix, Φ is the covariance matrix of the observation noise, z is the observed quantity, h is the observation state transfer matrix, H is the Jacobian matrix of the observation equation relative to the error state variable, H T is the transpose of the Jacobian matrix of the observation equation relative to the error state variable, and I is the identity matrix.

8. A vehicle three-dimensional posture positioning estimation system, characterized in that: The vehicle three-dimensional posture positioning estimation system comprises: A determination module, used for determining the accelerometer bias and the gyroscope bias of the vehicle; A construction module is used to perform IMU trajectory derivation based on the accelerometer bias and the gyroscope bias, and construct a nominal discrete time motion equation of the nominal state variables of the three-dimensional posture; A prediction module, used to determine a three-dimensional posture error state variable corresponding to the three-dimensional posture nominal state variable, and obtain a prediction error covariance matrix based on the three-dimensional posture error state variable; A registration module, used for semantically registering the local semantic map and the global semantic map of the vehicle to obtain a three-dimensional position and posture semantic observation; The output module is used to obtain the real-time three-dimensional pose result of the vehicle through ESKF fusion positioning according to the three-dimensional pose semantic observation, velocity observation, the nominal discrete time motion equation and the prediction error covariance matrix.

9. A vehicle three-dimensional posture positioning estimation device, characterized in that: The device includes: a memory, a processor, and a vehicle three-dimensional pose positioning estimation program stored in the memory and executable on the processor, wherein the vehicle three-dimensional pose positioning estimation program is configured to implement the steps of the vehicle three-dimensional pose positioning estimation method as described in any one of claims 1 to 7.

10. A storage medium, characterized in that: The storage medium stores a vehicle three-dimensional posture positioning estimation program, and when the vehicle three-dimensional posture positioning estimation program is executed by the processor, the steps of the vehicle three-dimensional posture positioning estimation method as described in any one of claims 1 to 7 are implemented.

Images