Position estimation method, device, equipment and medium

By combining the data of the wheel speedometer and the gyroscope, the wheel speedometer model and the vehicle kinematic model are used to perform pose estimation, which solves the problem of high-quality IMU high cost and achieves low-cost high-precision pose estimation, suitable for non-existence Wide application of human-driving vehicles.

CN114659521BActive Publication Date: 2024-11-15UISEE TECH BEIJING LTD
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202210334193.9
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2022-03-30
Publication Date
2024-11-15
Estimated Expiration
2042-03-30

AI Technical Summary

Technical Problem

The existing position estimation scheme requires high-quality IMUs, which leads to high costs and is difficult to popularize low-end models, limiting the widespread application of driverless vehicles.

Method used

By obtaining the wheel speed measured by the wheel speed meter and the angular velocity measured by the gyroscope, combined with the preset wheel speed meter model and vehicle kinematic model, numerical integration and error propagation algorithms are performed to achieve vehicle position estimation without the need for high-quality IMU.

Benefits of technology

Multi-sensor coupling based on low-cost gyroscopes and wheel speedometers is realized, which improves the universality and accuracy of position estimation, reduces costs, and is suitable for a wide range of applications.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN114659521B_ABST
    Figure CN114659521B_ABST
Patent Text Reader

Abstract

The present disclosure relates to a method, device, equipment and medium for posture estimation, which includes: obtaining target observation data of a vehicle between time T and time T+1; obtaining the time derivative of the nominal state of the vehicle represented by the target observation data according to a preset wheel speed meter model and a vehicle kinematic model; numerically integrating the time derivative of the nominal state of the vehicle represented by the target observation data, and obtaining the nominal state increment of the vehicle between time T and time T+1 based on the integral result; obtaining the target error confidence corresponding to time T and time T+1 according to an error propagation algorithm, a wheel speed meter model, a vehicle kinematic model and the time derivative of the nominal state of the vehicle; estimating the real posture of the vehicle at time T and time T+1 according to the observation data of a designated sensor at time T and the observation data at time T+1, the nominal state increment corresponding to the target observation data and the target error confidence. The above method has strong universality and is convenient for wide application.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present disclosure relates to the field of information technology, and in particular to a method, device, equipment and medium for posture estimation. Background Art

[0002] In unmanned driving scenarios, in order to achieve high-precision positioning and navigation of unmanned vehicles, in addition to relying on map information, it is also necessary to be able to accurately estimate the position and posture of the unmanned vehicle.

[0003] Most of the existing pose estimation solutions require the unmanned vehicle to be equipped with a high-quality IMU (Inertial Measurement Unit). However, high-quality IMUs are expensive to manufacture and difficult to popularize for vehicles such as low-end models. They have poor universality and are difficult to be widely used. Summary of the invention

[0004] In order to solve the above technical problems or at least partially solve the above technical problems, the present disclosure provides a posture estimation method, device, equipment and medium.

[0005] In a first aspect, an embodiment of the present disclosure provides a posture estimation method for acquiring target observation data of a vehicle between time T and time T+1; wherein the target observation data includes the wheel speed measured by a wheel speed meter and the angular velocity measured by a gyroscope; the time T and the time T+1 are two adjacent times when a designated sensor performs data observation, and the designated sensor is a sensor provided on the vehicle for observing the posture of the vehicle; according to a preset wheel speed meter model and a vehicle kinematic model, a time derivative of a nominal state of the vehicle represented by the target observation data is acquired; the time derivative of the nominal state of the vehicle represented by the target observation data is numerically integrated, and the nominal state of the vehicle between the time T and the time T+1 is acquired based on the integration result. state increment; the nominal state increment includes the vehicle posture increment and the gyroscope bias increment obtained based on the target observation data; according to the error propagation algorithm, the wheel speed meter model, the vehicle kinematic model and the time derivative of the vehicle nominal state, the target error confidence corresponding to the time T and the time T+1 is obtained; wherein the target error confidence is the confidence of the error between the real state increment and the nominal state increment between the time T and the time T+1; according to the observation data of the designated sensor at the time T and the observation data at the time T+1, the nominal state increment corresponding to the target observation data and the target error confidence, the real posture of the vehicle at the time T and the time T+1 is estimated.

[0006] In a second aspect, an embodiment of the present disclosure further provides a posture estimation device, comprising: a data acquisition module, used to acquire target observation data of a vehicle between time T and time T+1; wherein the target observation data includes the wheel speed measured by a wheel speed meter and the angular velocity measured by a gyroscope; the time T and the time T+1 are two adjacent times when a designated sensor performs data observation, and the designated sensor is a sensor provided on the vehicle for observing the posture of the vehicle; a state acquisition module, used to acquire the time derivative of the nominal state of the vehicle represented by the target observation data according to a preset wheel speed meter model and a vehicle kinematic model; an integration module, used to numerically integrate the time derivative of the nominal state of the vehicle represented by the target observation data, and acquire the time derivative of the nominal state of the vehicle between time T and time T+1 based on the integration result. The nominal state increment between the time T and the time T+1; the nominal state increment includes the vehicle posture increment and the gyroscope bias increment obtained based on the target observation data; a confidence acquisition module, used to obtain the target error confidence corresponding to the time T and the time T+1 according to the error propagation algorithm, the wheel speed meter model, the vehicle kinematic model and the time derivative of the vehicle nominal state; wherein the target error confidence is the confidence of the error between the real state increment and the nominal state increment between the time T and the time T+1; a posture estimation module, used to estimate the real posture of the vehicle at the time T and the time T+1 according to the observation data of the designated sensor at the time T and the observation data at the time T+1, the nominal state increment corresponding to the target observation data and the target error confidence.

[0007] In a third aspect, an embodiment of the present disclosure further provides an electronic device, comprising: a processor; a memory for storing executable instructions of the processor; the processor is used to read the executable instructions from the memory and execute the instructions to implement a posture estimation method as provided in an embodiment of the present disclosure.

[0008] In a fourth aspect, an embodiment of the present disclosure further provides a computer-readable storage medium, wherein the storage medium stores a computer program, and the computer program is used to execute the posture estimation method provided in the embodiment of the present disclosure.

[0009] The above technical solution provided by the embodiment of the present disclosure determines the time derivative of the nominal state of the vehicle represented by the tachometer observation data according to the target observation data of the vehicle between time T and time T+1 (the wheel speed measured by the tachometer and the angular velocity measured by the gyroscope), the tachometer model and the vehicle kinematic model, and further obtains the nominal state increment of the vehicle between time T and time T+1 by numerically integrating the time derivative of the nominal state of the vehicle represented by the target observation data, and obtains the target error confidence according to the error propagation algorithm, the tachometer model, the vehicle kinematic model and the time derivative of the nominal state of the vehicle. The observation data of the specified sensor at time T and time T+1, the nominal state increment corresponding to the speedometer observation data, and the target error confidence are used to estimate the real posture of the vehicle at time T and time T+1. The above method does not require a high-quality IMU sensor, but can be achieved by multi-sensor coupling based on a low-cost gyroscope and a speedometer. The posture estimation can be performed by combining the measurement data of the low-cost gyroscope and the speedometer which are usually more accurate (that is, the wheel speed measured by the speedometer and the angular velocity measured by the gyroscope) and other sensors used to observe the vehicle posture. The method has strong universality and is convenient for wide application.

[0010] It should be understood that the content described in this section is not intended to identify the key or important features of the embodiments of the present disclosure, nor is it intended to limit the scope of the present disclosure. Other features of the present disclosure will become easily understood through the following description. BRIEF DESCRIPTION OF THE DRAWINGS

[0011] The accompanying drawings, which are incorporated in and constitute a part of this specification, illustrate embodiments consistent with the present disclosure and, together with the description, serve to explain the principles of the present disclosure.

[0012] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, for ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative labor.

[0013] Figure 1 A flowchart of a posture estimation method provided by an embodiment of the present disclosure;

[0014] Figure 2 A schematic diagram of the structure of a posture estimation device provided in an embodiment of the present disclosure;

[0015] Figure 3 A schematic diagram of the structure of an electronic device provided in an embodiment of the present disclosure. DETAILED DESCRIPTION

[0016] In order to more clearly understand the above-mentioned objectives, features and advantages of the present disclosure, the scheme of the present disclosure will be further described below. It should be noted that the embodiments of the present disclosure and the features in the embodiments can be combined with each other without conflict.

[0017] In the following description, many specific details are set forth to facilitate a full understanding of the present disclosure, but the present disclosure may also be implemented in other ways different from those described herein; it is obvious that the embodiments in the specification are only part of the embodiments of the present disclosure, rather than all of the embodiments.

[0018] Mapping and positioning technology is one of the core technologies in the field of autonomous driving. For unmanned driving scenarios, it is usually necessary to first map the environment and then implement the positioning function in the built map. However, the map built in advance may become outdated as the environment changes and time passes, and gradually fail to provide sufficient information for vehicle positioning. Therefore, in order to achieve high-precision positioning, in addition to relying on map information, a robust odometer is also required, that is, the vehicle's posture needs to be estimated based on the sensors on the vehicle. The accuracy of the posture estimation will directly affect the vehicle positioning.

[0019] Most of the current posture estimation schemes require the use of high-precision IMU observation data and observation data of other sensors configured on the vehicle, such as laser radar / visual sensors, to be tightly coupled (fused). However, this scheme has high requirements on the accuracy of the IMU and requires the unmanned vehicle to be equipped with a high-quality IMU. However, high-quality IMUs are very expensive, so most low-end unmanned vehicles are difficult to adopt the above-mentioned tightly coupled scheme. In contrast, most vehicles are equipped with a wheel speed meter and a low-cost IMU. The disclosed embodiment can fully combine the low-cost IMU with the measurement data of the wheel speed meter with generally high accuracy (that is, the wheel speed measured by the wheel speed meter and the angular velocity measured by the gyroscope) and the observation data of other sensors for coupling, that is, only the gyroscope angular velocity with generally high measurement accuracy in the low-cost IMU and the wheel speed with generally high measurement accuracy in the wheel speed meter are used. The acceleration noise obtained by the low-cost IMU is large and the measurement accuracy is poor. The deflection angle obtained by the wheel speed meter has large noise and poor measurement accuracy. Therefore, the disclosed embodiment does not use the acceleration obtained by the low-cost IMU and the deflection angle obtained by the wheel speed meter. Since the wheel speed of the tachometer and the angular velocity of the gyroscope (the gyroscope of a low-cost IMU can satisfy the requirement) are very easy to obtain, the inventors have studied a solution for performing pose estimation using the wheel speed measured by the tachometer, the angular velocity measured by the gyroscope, and the observation data of other sensors such as cameras and radars. This solution can also be called a tightly coupled solution of multi-sensor fusion based on a tachometer and a low-cost IMU. For ease of understanding, it is explained in detail below.

[0020] Figure 1The present invention provides a flow chart of a method for posture estimation provided by an embodiment of the present invention. The method can be performed by a posture estimation device, wherein the device can be implemented by software and / or hardware and can generally be integrated in an electronic device. Figure 1 As shown, the method mainly includes the following steps S102 to S110:

[0021] Step S102, obtaining the target observation data of the vehicle between time T and time T+1; wherein the target observation data includes the wheel speed measured by the wheel speed meter and the angular velocity measured by the gyroscope; time T and time T+1 are two adjacent times when the designated sensor is performing data observation, and the designated sensor is a sensor provided on the vehicle for observing the vehicle posture.

[0022] In some implementation examples, the T moment and the T+1 moment may be any two adjacent moments in a preset time period. In actual applications, a time window (that is, corresponding to the aforementioned preset time period) may be set, and the time window may include X moments when a specified sensor performs data observation. The T moment and the T+1 moment are any two adjacent moments among the X moments.

[0023] In the disclosed embodiment, the wheel speed and angular velocity between time T and time T+1 are mainly obtained, and the wheel speed is measured by a wheel speed meter, and the angular velocity is measured by a gyroscope, and the gyroscope can be satisfied by using a low-cost IMU. Since most vehicles are equipped with low-cost IMUs and wheel speed meters, the wheel speed measured by the wheel speed meter and the angular velocity measured by the gyroscope are very easy to obtain, and these two data are the most accurate data in the observation data of the wheel speed meter and the gyroscope, respectively, which helps to further ensure the accuracy of posture estimation.

[0024] The number of the above-mentioned designated sensors may be one or more, and the present disclosure does not limit the type of designated sensors, and any sensor that can be used to estimate the position and posture may be used, such as a laser radar, an ultrasonic sensor, a visual sensor, etc. Assume that there are a total of M designated sensors, and the M designated sensors may include sensors of the same or different types, such as the number of designated sensors is 2 (that is, M=2), the first designated sensor is a laser radar, and the second designated sensor is a visual sensor; for another example, the number of designated sensors is 4, the first designated sensor and the second designated sensor are both ultrasonic sensors (set at different positions of the vehicle), the third designated sensor is a visual sensor, and the fourth designated sensor is a laser radar.

[0025] Step S104, according to the preset wheel speed meter model and vehicle kinematic model, obtain the time derivative of the nominal state of the vehicle represented by the target observation data. The nominal state of the vehicle includes the nominal position and attitude of the vehicle and the nominal gyroscope bias. The nominal position and attitude of the vehicle is the nominal vehicle position and attitude, which can also be understood as the nominal vehicle displacement and orientation.

[0026] The embodiments of the present disclosure do not limit the wheel speed meter model, such as a differential wheel speed meter, an Ackermann model, a single vehicle model, etc. The wheel speed meter model to be used can be preset according to actual needs. The vehicle kinematic model is a formula that uses the collected vehicle-related data to calculate the changes in the vehicle position and posture over time. The embodiments of the present disclosure can be directly implemented using the vehicle kinematic model in the related technology.

[0027] On the basis of the wheel speed meter model and the vehicle kinematic model, the target observation data can be used to characterize the time derivative of the nominal state of the vehicle. Specifically, the target observation data represented by the wheel speed meter model is substituted into the vehicle kinematic model to characterize the time derivative of the nominal state of the vehicle. It should be noted that in actual situations, no matter how advanced the wheel speed meter is, it will carry measurement noise, but the noise carried varies in size, and it is impossible to achieve the ideal state of no noise at all. Therefore, the wheel speed meter observation data is usually a measurement value carrying noise, so the vehicle state characterized by it is only the nominal state of the vehicle, that is, the vehicle state obtained by using the nominal observation data.

[0028] Step S106, numerically integrate the time derivative of the nominal state of the vehicle represented by the target observation data, and obtain the nominal state increment of the vehicle between time T and time T+1 based on the integration result; the nominal state increment includes the vehicle posture increment and gyroscope bias increment obtained based on the target observation data.

[0029] In the disclosed embodiment, by numerically integrating the time derivative of the nominal state of the vehicle represented by the target observation data, the transfer equation of the nominal state of the vehicle can be obtained, and then the nominal state increment obtained based on the target observation data can be further obtained according to the transfer equation of the nominal state of the vehicle, and the nominal state increment includes the vehicle posture increment (also known as the nominal posture increment) and the gyroscope bias increment (also known as the nominal gyroscope bias increment). Exemplarily, recursive processing is performed based on the transfer equation of the nominal state of the vehicle to obtain the nominal state increment. The disclosed embodiment does not limit the specific method of numerical integration, such as the numerical integration can be Euler integration, median integration or high-order Runge-Kutta integration, etc., and the required numerical integration method can be flexibly selected according to the actual situation.

[0030] Step S108, according to the error propagation algorithm, the wheel speed meter model, the vehicle kinematic model and the time derivative of the vehicle nominal state, obtain the target error confidence corresponding to the time T and the time T+1; wherein the target error confidence is the confidence of the error between the real state increment and the nominal state increment between the time T and the time T+1.

[0031] In the error propagation algorithm, it is assumed that the true state of the vehicle can be obtained under ideal conditions, that is, the state without any error; and the error between the nominal state and the true state is the error state. At any time, the mean of the error state can be added to the nominal state to calculate the current estimate of the true state. When the embodiment of the present disclosure is implemented, the corresponding target error confidence between time T and time T+1 can be obtained based on the error propagation algorithm, the wheel speed meter model, the vehicle kinematic model and the time derivative of the vehicle nominal state. The higher the confidence, the higher the credibility of the state estimation. In a specific implementation example, the time derivative of the true state of the vehicle can be first obtained based on the wheel speed meter model and the vehicle kinematic model; then, according to the error propagation algorithm, the time derivative of the true state of the vehicle is combined with the time derivative of the nominal state of the vehicle to determine the time derivative of the error between the true state increment and the nominal state increment between time T and time T+1. Specifically, the error propagation algorithm characterizes that the true state minus the nominal state equals the error state. According to the error propagation algorithm, the time derivative of the error state can be obtained by subtracting the time derivative of the vehicle's nominal state from the time derivative of the vehicle's true state, which can also be used to obtain the time derivative of the error state between the true state increment and the nominal state increment between the above-mentioned time T and time T+1. Then, the time derivative of the error is numerically integrated to obtain the error transfer equation; based on the error transfer equation, the corresponding target error confidence between the true state increment and the nominal state increment between time T and time T+1 is recursively obtained.

[0032] In some specific implementation examples, the covariance of the error between the real state increment and the nominal state increment between time T and time T+1 is recursively obtained based on the error transfer equation, and the covariance of the error is used to characterize the corresponding target error confidence between time T and time T+1. Assuming that the covariance of the error is P, the confidence of the wheel speed meter error can be P-1, and the larger the covariance, the smaller the confidence. Based on the nominal state increment corresponding to the known target observation data and the target error confidence, it is coupled with the observation data of other specified sensors.

[0033] Step S110, estimates the actual position and posture of the vehicle at time T and time T+1 based on the observation data of the specified sensor at time T and the observation data at time T+1, the nominal state increment corresponding to the target observation data, and the target error confidence.

[0034] In some specific implementation examples, the first nominal position and first sensor error confidence of the vehicle at time T, as well as the second nominal position and second sensor error confidence of the vehicle at time T+1 can be obtained based on the observation data of the specified sensor at time T and the observation data at time T+1. The above-mentioned method of obtaining the nominal position and corresponding error confidence corresponding to the specified sensor at each time can be implemented with reference to the relevant technology and is not limited here.

[0035] Then, based on the nominal state increment and target error confidence corresponding to the wheel speed meter observation data, as well as the first nominal pose, first sensor error confidence, second nominal pose and second sensor error confidence corresponding to the specified sensor, the actual pose of the vehicle at time T and time T+1 is estimated.

[0036] In the disclosed embodiment, the state of the vehicle can be calculated not only through the target observation data, but also through other designated sensors such as laser radar, visual sensors, etc. Each sensor corresponds to an error confidence. Finally, when the target observation data and the observation data of other designated sensors are tightly coupled for pose estimation, the confidence of all sensors including the wheel speed meter will be considered. The higher the confidence of the sensor, the closer the estimated result of the real state will be to the estimated result corresponding to the sensor. In practical applications, the estimated results of multiple sensors are weighted averaged to obtain the estimated result of the real state, and the weights used in the weighted average are determined based on the error confidence. In the above manner, a relatively reliable real state estimation result can be obtained.

[0037] In summary, the above method provided by the embodiment of the present disclosure does not require a high-quality IMU sensor, and can be achieved only by multi-sensor coupling based on a low-cost gyroscope and a wheel speed meter. By combining the measurement data of a low-cost gyroscope and a wheel speed meter, which usually have higher accuracy (that is, the wheel speed measured by the wheel speed meter and the angular velocity measured by the gyroscope), and other sensors for observing the vehicle posture, posture estimation can be performed. It has strong universality and is convenient for wide application.

[0038] To facilitate further understanding of the above-mentioned posture estimation method, the present disclosure takes the preset wheel speed meter model as a bicycle model as an example, and provides a specific implementation example of the above-mentioned posture estimation method, which can be implemented by referring to the following core steps:

[0039] Step 1: Obtain the target observation data of the vehicle between time T and time T+1 (wheel speed measured by the tachometer and angular velocity measured by the gyroscope), and obtain the time derivative of the nominal state of the vehicle represented by the target observation data based on the single vehicle model and the vehicle kinematic model.

[0040] In the bicycle model, the two front wheels of the autonomous driving vehicle are simplified as the front wheels of the bicycle model, and the two rear wheels are simplified as the rear wheels of the bicycle model. The front wheels of the bicycle model control the direction of movement through steering, and the front wheels or rear wheels provide driving force for the vehicle. Here, front-wheel drive is used as an example to illustrate. In this model, the wheel speed meter observation data involves two parameters. Parameter 1 is the timestamp, which indicates the time t of data collection; parameter 2 is the wheel speed u of the driving wheel, which is the front wheel speed here. A positive wheel speed indicates that the vehicle is moving forward, and a negative wheel speed indicates that the vehicle is moving backward. For the convenience of description, it can be assumed that the wheel speed unit is meter per second, and it is assumed that the distance between the front and rear axles of the vehicle is, and it is also assumed that the angular velocity measured by the gyroscope is Where i represents the IMU coordinate system. Assume that the external parameters from the vehicle coordinate system to the IMU coordinate system are known, and the rotation part of the external parameters is R vi , v represents the vehicle coordinate system. Here, it is assumed that the vehicle coordinate system takes the center of the rear axle as the origin, the front of the vehicle is facing the positive direction of the y-axis, and the right along the rear axle is the positive direction of the x-axis. Based on this, the angular velocity of the vehicle can be directly obtained by coordinate system conversion:

[0041]

[0042] In the posture estimation method provided in the embodiment of the present disclosure, the vehicle state to be estimated includes the vehicle's position vector, attitude, and gyroscope bias. Based on the single-vehicle model and the vehicle kinematic model, the time derivative of the vehicle's nominal state represented by the target observation data can be expressed as:

[0043]

[0044]

[0045] in, represents the nominal position vector; represents the nominal vehicle velocity vector; v y , represents the nominal vehicle speed in the Y-axis direction of the vehicle coordinate system at time T; is the unit vector in the Y-axis direction of the vehicle coordinate system at time T; is the unit vector in the Z-axis direction of the vehicle coordinate system at time T; R{q} represents the rotation matrix of the quaternion q; the quaternion q represents the nominal posture of the vehicle; u represents the wheel speed of the vehicle driving wheel; φ represents the wheel deflection angle; u m Indicates the wheel speed measurement value; R viRepresents the rotation matrix from the IMU coordinate system to the vehicle coordinate system; Represents the nominal angular velocity vector measured by the gyroscope; represents the gyroscope bias vector; represents the nominal angular velocity vector in the vehicle coordinate system; Represents the nominal angular velocity vector in the IMU coordinate system; represents the Gaussian white noise of the gyroscope bias; l represents the wheelbase between the front and rear axles of the vehicle.

[0046] It should be noted that ucosφ is the rear wheel speed of the vehicle body calculated by the front wheel speed. If the vehicle is rear-wheel drive, this step is not required. In addition, it should be noted that the deflection angle of the vehicle body in the disclosed embodiment is not read by a wheel speed meter, but is obtained by obtaining the angular velocity of the vehicle through a gyroscope and multiplying it by the wheelbase of the vehicle.

[0047] Step 2: Perform numerical integration (ie, pre-integration) based on the target observation data, and obtain the nominal state increment of the vehicle between time T and time T+1 based on the integration result.

[0048] The ultimate goal of pre-integration is to fuse the target observation data with the observation data of other specified sensors, so as to estimate the real posture. It should be noted that the T and T+1 moments mentioned in the embodiments of the present disclosure refer to the time of collecting two consecutive frames of other sensor data, such as two adjacent moments when the visual sensor collects two frames of images. Generally, there will be multiple frames of target observation data corresponding to the wheel speed meter and the gyroscope between the two frames of images, that is, the wheel speed meter and the gyroscope collect target observation data at N moments between the T moment and the T+1 moment, and t is any other moment except the last moment in the N moments. Pre-integration refers to integration based on the target observation data. Specifically, the time derivative of the nominal state of the vehicle represented by these target observation data is integrated to obtain the vehicle posture increment (that is, the vehicle posture change) and gyroscope bias increment (that is, the gyroscope bias change) obtained from the wheel speed of the wheel speed meter and the gyroscope angular velocity from the T moment to the T+1 moment, that is, through integration, it can be known how much displacement, rotation and gyroscope bias the vehicle has undergone. In practical applications, pre-integration may also be used to calculate quantities related to posture changes, but in the disclosed embodiment, the changes in position, attitude, and gyroscope bias are mainly calculated.

[0049] In some specific implementations, the time derivative of the nominal state of the vehicle represented by the target observation data can be numerically integrated, and then the nominal state increment of the vehicle between time T and time T+1 can be obtained based on the integration result; wherein the nominal state increment includes the vehicle posture increment and the gyroscope bias increment obtained based on the target observation data. It can be understood that the position and posture of the vehicle state are unknown at the beginning. After obtaining the time derivative of the nominal state of the vehicle, the state of the vehicle at time t+1 can be recursively obtained by the state and observation of the vehicle at time t. This process requires numerical integration of the speed and angular velocity of the vehicle (body). Euler integration, median integration or high-order Runge-Kutta integration can be selected. The disclosed embodiments do not limit the form of numerical integration. The disclosed embodiments are illustrated using the median integration as an example.

[0050] By performing median integration on the speed measured by the wheel speed meter and the angular velocity measured by the gyroscope, the integral result is expressed as:

[0051]

[0052]

[0053]

[0054] Wherein, the N moments between the T moment and the T+1 moment correspond to target observation data respectively, t is any moment other than the last moment among the N moments; δt is the interval between the tth moment and the t+1th moment; is the unit vector in the Y-axis direction of the vehicle coordinate system at time T; is the unit vector in the Z-axis direction of the vehicle coordinate system at time T; Representing quaternions The rotation matrix of vi Represents the rotation matrix from the IMU coordinate system to the vehicle coordinate system; E mid represents the state quantity E at the intermediate moment between time t and time t+1. In other words, E t represents the state quantity E at time t, E t+1 Represents the state quantity E at time t+1; the state quantity E is u m , q θ , q, θ or Among them, u m Indicates the wheel speed measurement value; Represents the nominal angular velocity measured by the gyroscope; represents the gyroscope bias; q represents the quaternion, θ represents the yaw angle, and both the quaternion q and the yaw angle θ are used to characterize the nominal attitude of the vehicle; q θRepresents the quaternion corresponding to the rotation matrix corresponding to the yaw angle θ; represents the nominal position vector; l represents the wheelbase between the front and rear axles of the vehicle.

[0055] In the above formula, the superscript mid indicates the value of the corresponding state quantity at the middle time between t and t+1, the superscript t indicates the value of the corresponding state quantity at time t, and t+1 indicates the value of the corresponding state quantity at time t+1. It should be noted that the superscript T of the matrix indicates the transposition operation of the matrix in mathematical operations, such as, express The transposed matrix of .

[0056] In addition, in the above formula The angle represented in the quaternion is half the actual rotation angle.

[0057] In the above formula provided in the embodiment of the present disclosure, the gyroscope bias uses the value at time t, because it is generally believed that the gyroscope bias will not change much in a short time. In the process of calculating the rear wheel speed of the vehicle, the gyroscope measurement value can be first subtracted from the bias at that time, and converted to the vehicle body coordinate system through the external parameter matrix, and then only the angular velocity of the third dimension, that is, the angular velocity of the yaw angle (yaw angle θ), is taken.

[0058] It should be noted that the above-mentioned driving wheels are all taken as examples of front wheels. They are the unit vectors in the X, Y, and Z axes of the vehicle coordinate system. Specifically, they are the three unit vectors pointing right, forward, and upward with the center of the rear axle of the wheel as the origin. All vectors, quaternions, and rotation matrices represented by quaternions in the above formulas are based on the vehicle's position at time T as the reference system, that is, the coordinate system where the road surface tangent plane is located at time T. The same is true for subsequent formulas, so I will not repeat them here.

[0059] By numerically integrating the time derivative of the vehicle's nominal state represented by the target observation data, the above-mentioned integration result is the transfer equation of the vehicle's nominal state. By recursively deducing based on the transfer equation of the vehicle's nominal state, the nominal state increment of the vehicle between time T and time T+1 can be obtained. The nominal state increment includes the vehicle posture increment and the gyroscope bias increment obtained based on the target observation data.

[0060] Step 3: Based on the error propagation algorithm, the wheel speed meter model, the vehicle kinematic model, and the time derivative of the vehicle nominal state, the covariance of the error between the true state increment and the nominal state increment between time T and time T+1 is obtained. This covariance can be used to characterize the target error confidence.

[0061] The error propagation algorithm involves nominal state, true state and error state. The relationship between the states can be seen in Table 1:

[0062]

[0063] Table 1

[0064] Among them, pos represents position, speed represents the wheel speed of the vehicle driving wheel, angularvelocity represents angular velocity, gyroscopebias represents gyroscope bias; vel represents vehicle speed, quat represents quaternion, rot represents rotation matrix, anglevector represents angle vector, truestate represents true state, nominalstate represents nominal state, errorstate represents error state, composition represents the combination method, specifically the relationship between the true state, nominal state and error state, measure represents measurement value, and noise represents noise value.

[0065] In specific implementation, the time derivative of the real state of the vehicle can be first obtained based on the wheel speed meter model and the vehicle kinematic model; then, according to the error propagation algorithm, the time derivative of the real state of the vehicle is combined with the time derivative of the nominal state of the vehicle to determine the time derivative of the error between the real state increment and the nominal state increment between time T and time T+1; then, the time derivative of the error is numerically integrated to obtain the error transfer equation; finally, the covariance of the error between the real state increment and the nominal state increment between time T and time T+1 is recursively obtained based on the error transfer equation, and the covariance of the error is used to represent the corresponding target error confidence between time T and time T+1. That is, the target error confidence is represented by the covariance of the error between the real posture increment and the nominal posture increment between time T and time T+1.

[0066] From the above, we can see that the transfer equation of the error covariance is determined based on the error transfer equation, and the error transfer equation is determined based on the time derivative of the error. Therefore, we first need to calculate the time derivative of the error. Among them, the time derivative of the error state is equal to the time derivative of the true state minus the time derivative of the nominal state. Specifically:

[0067] The time derivative of the nominal state is:

[0068]

[0069]

[0070]

[0071]

[0072] The time derivative of the true state is:

[0073]

[0074]

[0075]

[0076] The time derivative of the error state is:

[0077]

[0078]

[0079] Here is the command:

[0080]

[0081]

[0082]

[0083]

[0084] It should be noted that A and B are both scalars, and their physical meanings are the vehicle rear wheel speed obtained by the front wheel speed and the gyroscope, except that B does not include small quantities such as noise and error state. In order to simplify the formula, C is further agreed to be the square of A.

[0085] Put A in (u n ,δω b ,ω n ) are all 0, the above formula is simplified to:

[0086]

[0087] Since A0 is A in (u n ,δω b ,ω n ) are all 0, so A0=B

[0088] Substitute it into the above formula and expand it to get:

[0089]

[0090] in, is a scalar, The product of small amounts such as error or noise can be approximated to 0.

[0091] However, it should be considered that the vehicle will not move in the Z-axis direction of the vehicle coordinate system, so between two adjacent frames of images or radar observations (i.e., between time T and time T+1), the vehicle actually moves in a plane, and the displacement generated is only in the xoy plane. Therefore, different from the traditional fusion algorithm of the gyroscope and the wheel speed meter, the embodiment of the present disclosure only models the movement of the vehicle in the plane, which is more in line with the actual situation of the vehicle moving on the road.

[0092] Therefore, the embodiment of the present disclosure is further improved by determining the displacement parameter of the error based on the X-axis displacement and Y-axis displacement corresponding to the vehicle coordinate system at time T in the six-degree-of-freedom model. That is, only the two-degree-of-freedom planar motion is extracted from the three-degree-of-freedom motion of the vehicle in XYZ, thereby obtaining the nominal state and true state of the displacement state quantity:

[0093]

[0094]

[0095] In the above formula, R θ represents a 3x3 rotation matrix containing only the yaw angle (i.e., the yaw angle θ) rotation component, which represents the yaw rotation that occurs when the vehicle moves in a plane between two frames of observation (i.e., between time T and time T+1). θ It only represents the rotation within the plane, so its error state is only the same as the error state of the 3-DOF rotation The yaw angle component in related, so R θ The relationship between the nominal state, true state and error state is:

[0096]

[0097] The error state can be obtained by defining the nominal state and the true state of the in-plane displacement above:

[0098]

[0099] in,[] x The operator represents the cross product matrix corresponding to the vector. The part after the plus sign in the above equation can be further expanded to obtain:

[0100]

[0101] It should be noted here that R θ The upper left corner matrix is ​​the matrix corresponding to the yaw angle, R θ It does not contain the roll angle or pitch angle components, that is:

[0102]

[0103] therefore Although it is a 3D vector, the third dimension is 0, so directly extracting the first 2 dimensions of the vector is the error state of the displacement in the desired plane:

[0104]

[0105] Still using the above convention:

[0106]

[0107]

[0108] Then the partial derivatives in the above in-plane displacement error state expression are:

[0109]

[0110]

[0111]

[0112] Then the median of each partial derivative at the time between time t and time t+1 is:

[0113]

[0114]

[0115]

[0116]

[0117] in:

[0118]

[0119]

[0120]

[0121]

[0122] In addition, the error state equations for rotation and angular velocity bias are:

[0123]

[0124]

[0125] The error state differential equations are written in matrix form (that is, the time derivative of the error is):

[0126]

[0127] The sub-matrices are:

[0128]

[0129]

[0130]

[0131]

[0132]

[0133]

[0134] The above is the error state equation. The median integral is used to solve the error state transfer equation: First, from start:

[0135]

[0136]

[0137] Then comes the gyro bias:

[0138]

[0139] Finally, the displacement in the plane:

[0140]

[0141]

[0142]

[0143]

[0144]

[0145] The state transfer equation based on the median integral can be written in matrix form (the following is the expression of the error transfer equation):

[0146]

[0147] The sub-matrices are:

[0148]

[0149]

[0150]

[0151]

[0152] f 12 =-R vi δt

[0153]

[0154]

[0155]

[0156] g 24 =δtI 3×3

[0157] in:

[0158]

[0159]

[0160]

[0161]

[0162]

[0163]

[0164]

[0165]

[0166]

[0167]

[0168]

[0169]

[0170]

[0171] Through the above error transfer equation, we can iterate from time T to time T+1, and finally get the error state at time T+1, that is:

[0172]

[0173] The three sub-vectors in the above vector are respectively the displacement error state quantity of 2 degrees of freedom, the rotation error state quantity of 3 degrees of freedom, and the gyroscope bias error state quantity of 3 degrees of freedom.

[0174] And the covariance matrix of the error state at time T+1 can be obtained.

[0175] Specifically, the transfer equation of the error covariance is expressed as:

[0176] P t+1 =(Fδt+I)P t (Fδt+I) T +GδtQ(Gδt) T

[0177] Where Q is the wheel speed meter noise, defined as the following diagonal matrix:

[0178]

[0179] The following is an exemplary explanation of the meanings of the letters involved in the above formula:

[0180] The N moments between the T moment and the T+1 moment correspond to target observation data respectively, t is any moment other than the last moment among the N moments; δt is the interval between the tth moment and the t+1th moment; is the unit vector in the X-axis direction of the vehicle coordinate system at time T; is the unit vector in the Y-axis direction of the vehicle coordinate system at time T; is the unit vector in the Z-axis direction of the vehicle coordinate system at time T; R θ is the rotation matrix containing only the rotation component of the yaw angle θ, R vi Represents the rotation matrix from the IMU coordinate system to the vehicle coordinate system; u m Indicates the wheel speed measurement value; u n Indicates wheel speed noise; Represents the nominal angular velocity measured by the gyroscope; Indicates the gyroscope bias; Gaussian white noise representing the gyroscope bias; represents the gyroscope angular velocity noise; represents the angular velocity of the vehicle rotating around the Z axis of the vehicle coordinate system at time T; q represents the quaternion, θ represents the yaw angle, and both the quaternion q and the yaw angle θ are used to characterize the nominal posture of the vehicle; q θ Represents the quaternion corresponding to the rotation matrix corresponding to the yaw angle θ; represents the nominal position vector; l represents the wheelbase between the front axle and the rear axle of the vehicle; is the nominal position vector obtained in the vehicle coordinate system at time T based on the X-axis displacement and Y-axis displacement in the six-degree-of-freedom model; is the position error increment obtained based on the X-axis displacement and Y-axis displacement in the six-degree-of-freedom model in the vehicle coordinate system at time T; is the attitude error increment obtained based on the six-degree-of-freedom model, is the gyroscope bias error increment obtained based on the six-degree-of-freedom model; E mid The state quantity E, E represents the intermediate time between time t and time t+1. t represents the state quantity E at time t, E t+1 Represents the state quantity E at time t+1; the state quantity E is u m , q θ ,q,θ,R θ , or is the quaternion q corresponding to the yaw angle θ mid The rotation matrix of is the wheel speed noise variance, are the noise variances of the gyroscope angular velocity in the X-axis, Y-axis, and Z-axis directions of the vehicle coordinate system at time T, respectively; are the noise variances of the gyroscope bias random walk in the X-axis, Y-axis, and Z-axis directions of the vehicle coordinate system at time T.

[0181] Step 4: Fusion of wheel speed meter, gyroscope and other sensors based on error covariance

[0182] Through the above steps one to three, the nominal state increment obtained by recursion from the wheel speed measured by the speedometer and the angular velocity measured by the gyroscope between time T and time T+1, as well as the covariance matrix of the error between the true state increment and the nominal state increment (which can be used to characterize the above-mentioned target error confidence) can be obtained. In addition, based on the observation data of the specified sensor at time T and the observation data at time T+1, the first nominal posture and the first sensor error confidence of the vehicle at time T, as well as the second nominal posture and the second sensor error confidence of the vehicle at time T+1 can be obtained.

[0183] In the disclosed embodiment, the real posture of the vehicle at time T and time T+1 can be estimated based on the nominal state increment and target error confidence corresponding to the target observation data, and the first nominal posture, first sensor error confidence, second nominal posture and second sensor error confidence corresponding to the specified sensor. In some specific implementation examples, the real posture of the vehicle at the time T and the time T+1 can be estimated according to the following formula:

[0184]

[0185] in, is the actual state of the vehicle, where is the real position of the vehicle in the world coordinate system, is the gyroscope bias, is the real state of the vehicle at time T, is the real state of the vehicle at time T+1, δξ is the nominal state increment between time T and time T+1 corresponding to the target observation data, P -1 is the target error confidence, is the actual position of the vehicle at time T, is the actual position of the vehicle at time T+1, M is the number of the specified sensors, and the value range of m is [1, M]. is the first nominal pose corresponding to the mth specified sensor at time T, is the second nominal pose corresponding to the mth specified sensor at time T+1, is the first sensor error confidence corresponding to the mth specified sensor at time T, The second sensor error confidence level corresponding to the mth specified sensor at time T+1.

[0186] In the specific implementation, refer to the following formula

[0187]

[0188] in, Represents the displacement vector of the vehicle in the world coordinate system at time T+1; Represents the displacement vector of the vehicle in the world coordinate system at time T; represents the nominal displacement increment of the vehicle in the vehicle coordinate system at time T from time T to time T+1 obtained by pre-integration; Indicates the attitude angle of the vehicle in the world coordinate system at time T, expressed in quaternion; Indicates the attitude angle of the vehicle in the world coordinate system at time T+1, expressed in quaternion; Quaternions The corresponding rotation matrix, [] xy It means extracting the displacement of the vehicle in the XY plane of the vehicle coordinate system at time T; Represents the rotation change of the vehicle from time T to time T+1 obtained by pre-integration; [] xyz Represents the vector part of the quaternion to be extracted; It represents the difference between the change in gyroscope bias from time T to time T+1 and the nominal change obtained by pre-integration, where the nominal change is equal to 0.

[0189] In the above formula, the subscript v represents the vehicle coordinate system, and the subscript w represents the world coordinate system. Because the embodiment of the present disclosure assumes that the vehicle moves in a local plane between time T and time T+1, [] xy Only the displacement in the local plane is extracted. In addition, by [] xyz Extract the vector portion of the quaternion.

[0190] In addition, the difference between the actual pose and the nominal pose of other sensors (i.e., designated sensors) at time T is:

[0191]

[0192] The expression of the difference between the actual posture and the nominal posture of other sensors at time T+1 is similar to the expression of the difference between the actual posture and the nominal posture at time T mentioned above, except that T is changed to T+1. It can also be implemented by referring to related technologies, which will not be repeated here.

[0193] The above equations can be efficiently solved by nonlinear least squares solvers such as g2o and ceres to obtain the optimal vehicle pose estimation result.

[0194] In summary, the above-mentioned posture estimation method provided by the embodiment of the present disclosure has the following characteristics:

[0195] 1) The highest quality measurement data from low-cost gyroscopes and tachometers (angular velocity measured by the gyroscope and wheel speed measured by the tachometer) is proposed, so as to maximize the advantages of the two sensors and achieve the best pose estimation effect.

[0196] 2) A pre-integration model based on a single-vehicle model is proposed, and then the covariance matrix of the error state is obtained based on the angular velocity measured by the gyroscope and the wheel speed measured by the speedometer (which can be used to characterize the confidence of the error between the actual state increment and the nominal state increment of the vehicle between time T and time T+1). This allows the data of the gyroscope and the speedometer to be better coupled with those of other sensors through probabilistic means, that is, a specific method for achieving optimal pose estimation by better optimizing the tight coupling of the speedometer and the speedometer with other sensors.

[0197] 3) Use local plane constraints between frames instead of global plane constraints, so that the model can be applied to scenes with ups and downs or road tilt. Specifically, the 3D displacement of the vehicle is simplified to a 2D plane displacement without additional plane constraints. On the one hand, the local plane constraint assumption between frames is satisfied from the model design, and the necessity of additional plane constraints is eliminated, thereby improving the efficiency of the model. In contrast, although the related technology considers that the movement of the vehicle on the actual road is a plane movement, it adds an additional global plane constraint to the 6-DOF posture: that is, the vehicle is constrained to translate in the Z direction during the entire movement, and the roll and pitch (roll angle and pitch angle) are 0, but there is a strong assumption of the global plane, that is, the entire scene of the vehicle movement is considered to be a plane or close to a plane. However, in actual scenes, there are not only various ups and downs, but also the road may have lateral tilts, and the assumption of the global plane is difficult to hold. In addition, the additional plane assumption means that additional constraints need to be added, which increases the amount of calculation and reduces the efficiency of the calculation. The above-mentioned method of using local plane constraints between frames instead of global plane constraints provided by the embodiments of the present disclosure can effectively solve the problems of limited application scenarios and low computing efficiency caused by the global plane assumption in related technologies.

[0198] Corresponding to the above pose estimation method, Figure 2 This is a schematic diagram of the structure of a posture estimation device provided by an embodiment of the present disclosure. The device can be implemented by software and / or hardware and can generally be integrated in an electronic device, such as Figure 2 As shown, the posture estimation device includes the following modules:

[0199] The data acquisition module 202 is used to acquire the target observation data of the vehicle between time T and time T+1; wherein the target observation data includes the wheel speed measured by the wheel speed meter and the angular velocity measured by the gyroscope; time T and time T+1 are two adjacent times when the designated sensor is performing data observation, and the designated sensor is a sensor set on the vehicle for observing the vehicle posture;

[0200] The state acquisition module 204 is used to acquire the time derivative of the nominal state of the vehicle represented by the target observation data according to the preset wheel speed meter model and the vehicle kinematic model;

[0201] The integration module 206 is used to numerically integrate the time derivative of the nominal state of the vehicle represented by the target observation data, and obtain the nominal state increment of the vehicle between time T and time T+1 based on the integration result; the nominal state increment includes the vehicle posture increment and the gyroscope bias increment obtained based on the target observation data;

[0202] The confidence acquisition module 208 is used to obtain the target error confidence corresponding to the time between time T and time T+1 according to the error propagation algorithm, the wheel speed meter model, the vehicle kinematic model and the time derivative of the vehicle nominal state; wherein the target error confidence is the confidence of the error between the real state increment and the nominal state increment between time T and time T+1;

[0203] The posture estimation module 210 is used to estimate the actual posture of the vehicle at time T and time T+1 based on the observation data of the specified sensor at time T and the observation data at time T+1, the nominal state increment corresponding to the target observation data, and the target error confidence.

[0204] In summary, the above-mentioned device provided by the embodiment of the present disclosure does not require a high-quality IMU sensor, and can be realized only by multi-sensor coupling based on a low-cost gyroscope and a wheel speed meter. By combining the measurement data of a low-cost gyroscope and a wheel speed meter which usually have higher accuracy (that is, the wheel speed measured by the wheel speed meter and the angular velocity measured by the gyroscope), and other sensors for observing the vehicle posture, posture estimation can be performed. It has strong universality and is convenient for wide application.

[0205] In some implementations, the wheel speed meter model includes a single vehicle model, and the integration module 206 is specifically configured to perform a median integration on a time derivative of a nominal state of the vehicle represented by the target observation data.

[0206] In some embodiments, the target error confidence is characterized by the covariance of the error between the actual pose increment and the nominal pose increment between time T and time T+1.

[0207] In some implementations, the displacement parameter of the error is determined based on the displacement in the X-axis direction and the displacement in the Y-axis direction corresponding to the vehicle coordinate system at time T in the six-degree-of-freedom model.

[0208] In some embodiments, the confidence acquisition module 208 is specifically used to: acquire the time derivative of the real state of the vehicle based on the wheel speed meter model and the vehicle kinematic model; determine the time derivative of the error between the real state increment and the nominal state increment between time T and time T+1 by combining the time derivative of the real state of the vehicle with the time derivative of the nominal state of the vehicle according to the error propagation algorithm; numerically integrate the time derivative of the error to obtain the error transfer equation; recursively obtain the covariance of the error between the real state increment and the nominal state increment between time T and time T+1 based on the error transfer equation, and use the error covariance to characterize the corresponding target error confidence between time T and time T+1.

[0209] In some embodiments, the posture estimation module 210 is specifically used to: obtain the first nominal posture and the first sensor error confidence of the vehicle at time T, and the second nominal posture and the second sensor error confidence of the vehicle at time T+1 based on the observation data of the specified sensor at time T and the observation data at time T+1; estimate the actual posture of the vehicle at time T and time T+1 based on the nominal state increment and the target error confidence corresponding to the target observation data, and the first nominal posture, the first sensor error confidence, the second nominal posture, and the second sensor error confidence corresponding to the specified sensor.

[0210] In some implementations, the posture estimation module 210 is specifically used to estimate the actual posture of the vehicle at time T and time T+1 according to the following formula:

[0211]

[0212] in, is the actual state of the vehicle, where is the real position of the vehicle in the world coordinate system, is the gyroscope bias, is the real state of the vehicle at time T, is the real state of the vehicle at time T+1, δξ is the nominal state increment between time T and time T+1 corresponding to the target observation data, P -1 is the target error confidence, is the actual position of the vehicle at time T, is the actual position of the vehicle at time T+1, M is the number of the specified sensors, and the value range of m is [1, M]. is the first nominal pose corresponding to the mth specified sensor at time T, is the second nominal pose corresponding to the mth specified sensor at time T+1, is the first sensor error confidence corresponding to the mth specified sensor at time T, The second sensor error confidence level corresponding to the mth specified sensor at time T+1.

[0213] The posture estimation device provided in the embodiment of the present disclosure can execute the posture estimation method provided in any embodiment of the present disclosure, and has the corresponding functional modules and beneficial effects of the execution method. The formulas involved in the posture estimation device can refer to the contents in the posture estimation method embodiment, and will not be repeated here.

[0214] Those skilled in the art can clearly understand that, for the convenience and brevity of description, the specific working process of the above-described device embodiment can refer to the corresponding process in the method embodiment, and will not be repeated here.

[0215] An embodiment of the present disclosure also provides an electronic device, which includes: a processor; a memory for storing instructions executable by the processor; and a processor for reading executable instructions from the memory and executing the instructions to implement the above-mentioned posture estimation method.

[0216] Figure 3 The structure diagram of an electronic device provided by the embodiment of the present disclosure is shown in FIG. Figure 3 As shown, the electronic device 300 includes one or more processors 301 and a memory 302 .

[0217] The processor 301 may be a central processing unit (CPU) or other forms of processing units having data processing capabilities and / or instruction execution capabilities, and may control other components in the electronic device 300 to perform desired functions.

[0218] The memory 302 may include one or more computer program products, which may include various forms of computer-readable storage media, such as volatile memory and / or non-volatile memory. The volatile memory may include, for example, random access memory (RAM) and / or cache memory (cache), etc. The non-volatile memory may include, for example, read-only memory (ROM), hard disk, flash memory, etc. One or more computer program instructions may be stored on the computer-readable storage medium, and the processor 301 may run the program instructions to implement the pose estimation method of the embodiment of the present disclosure described above and / or other desired functions. Various contents such as input signals, signal components, noise components, etc. may also be stored in the computer-readable storage medium.

[0219] In one example, the electronic device 300 may further include: an input device 303 and an output device 304 , and these components are interconnected via a bus system and / or other forms of connection mechanisms (not shown).

[0220] In addition, the input device 303 may also include, for example, a keyboard, a mouse, and the like.

[0221] The output device 304 can output various information to the outside, including the determined distance information, direction information, etc. The output device 304 can include, for example, a display, a speaker, a printer, a communication network and a remote output device connected thereto, and the like.

[0222] Of course, to simplify, Figure 3 Only some of the components related to the present disclosure in the electronic device 300 are shown, and components such as a bus, an input / output interface, etc. are omitted. In addition, according to specific application situations, the electronic device 300 may also include any other appropriate components.

[0223] In addition to the above-mentioned method and device, the embodiment of the present disclosure may also be a computer program product, which includes computer program instructions, and when the computer program instructions are executed by a processor, the processor executes the pose estimation method provided by the embodiment of the present disclosure.

[0224] The computer program product may be written in any combination of one or more programming languages ​​to write program code for performing the operations of the disclosed embodiments, including object-oriented programming languages ​​such as Java, C++, etc., and conventional procedural programming languages ​​such as "C" or similar programming languages. The program code may be executed entirely on the user computing device, partially on the user device, as a separate software package, partially on the user computing device and partially on a remote computing device, or entirely on a remote computing device or server.

[0225] In addition, the embodiments of the present disclosure may also be a computer-readable storage medium on which computer program instructions are stored. When the computer program instructions are executed by a processor, the processor executes the posture estimation method provided by the embodiments of the present disclosure.

[0226] The computer readable storage medium can adopt any combination of one or more readable media. The readable medium can be a readable signal medium or a readable storage medium. The readable storage medium can include, for example, but is not limited to, a system, device or device of electricity, magnetism, light, electromagnetic, infrared, or semiconductor, or any combination of the above. More specific examples (non-exhaustive list) of readable storage media include: an electrical connection with one or more wires, a portable disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the above.

[0227] The embodiments of the present disclosure also provide a computer program product, including a computer program / instruction, which, when executed by a processor, implements the posture estimation method in the embodiments of the present disclosure.

[0228] It should be noted that, in this article, relational terms such as "first" and "second" are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device 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 device. In the absence of further restrictions, the elements defined by the sentence "comprise a ..." do not exclude the existence of other identical elements in the process, method, article or device including the elements.

[0229] The above description is only a specific embodiment of the present disclosure, so that those skilled in the art can understand or implement the present disclosure. Various modifications to these embodiments will be apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the present disclosure. Therefore, the present disclosure will not be limited to the embodiments described herein, but will conform to the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A pose estimation method, It is characterized in that include: Obtaining target observation data of the vehicle between time T and time T+1; wherein the target observation data includes the wheel speed measured by the wheel speed meter and the angular velocity measured by the gyroscope; the time T and the time T+1 are two adjacent times when the designated sensor is performing data observation, and the designated sensor is a sensor provided on the vehicle for observing the vehicle posture; According to a preset wheel speed meter model and a vehicle kinematic model, a time derivative of a nominal state of the vehicle represented by the target observation data is obtained; Numerically integrating the time derivative of the nominal state of the vehicle represented by the target observation data, and obtaining the nominal state increment of the vehicle between the time T and the time T+1 based on the integration result; the nominal state increment includes the vehicle posture increment and the gyroscope bias increment obtained based on the target observation data; According to the error propagation algorithm, the wheel speed meter model, the vehicle kinematic model and the time derivative of the vehicle nominal state, the target error confidence corresponding to the time T and the time T+1 is obtained; wherein the target error confidence is the confidence of the error between the real state increment and the nominal state increment between the time T and the time T+1; The actual position and posture of the vehicle at the time T and the time T+1 are estimated based on the observation data of the designated sensor at the time T and the observation data at the time T+1, the nominal state increment corresponding to the target observation data, and the target error confidence.

2. The method according to claim 1, It is characterized in that The wheel speed meter model includes a single vehicle model, and the time derivative of the vehicle nominal state is: in, represents the nominal position vector; represents the nominal vehicle velocity vector; v y Indicates the nominal vehicle speed in the Y-axis direction of the vehicle coordinate system at time T; is the unit vector in the Y-axis direction of the vehicle coordinate system at time T; is the unit vector in the Z-axis direction of the vehicle coordinate system at time T; R{q} represents the rotation matrix of the quaternion q; u represents the wheel speed of the vehicle driving wheel; φ represents the wheel deflection angle; u m Indicates the wheel speed measurement value; R vi Represents the rotation matrix from the IMU coordinate system to the vehicle coordinate system; Represents the nominal angular velocity vector measured by the gyroscope; represents the gyroscope bias vector; represents the nominal angular velocity vector in the vehicle coordinate system; Represents the nominal angular velocity vector in the IMU coordinate system; represents the Gaussian white noise of the gyroscope bias; l represents the wheelbase between the front and rear axles of the vehicle.

3. The method according to claim 1, It is characterized in that The wheel speed meter model includes a single vehicle model, and the step of numerically integrating the time derivative of the vehicle nominal state represented by the target observation data includes: The time derivative of the nominal state of the vehicle represented by the target observation data is subjected to median integration, and the integral result is expressed as: Wherein, the N moments between the T moment and the T+1 moment correspond to target observation data respectively, t is any moment other than the last moment among the N moments; δt is the interval between the tth moment and the t+1th moment; is the unit vector in the Y-axis direction of the vehicle coordinate system at time T; is the unit vector in the Z-axis direction of the vehicle coordinate system at time T; Representing quaternions The rotation matrix of vi Represents the rotation matrix from the IMU coordinate system to the vehicle coordinate system; E mid The state quantity E, E represents the intermediate time between time t and time t+1. t represents the state quantity E at time t, E t+1 Represents the state quantity E at time t+1; the state quantity E is u m , q θ , q, θ or Among them, u m Indicates the wheel speed measurement value; Represents the nominal angular velocity measured by the gyroscope; represents the gyroscope bias; q represents the quaternion, θ represents the yaw angle, and both the quaternion q and the yaw angle θ are used to characterize the nominal attitude of the vehicle; q θ Represents the quaternion corresponding to the rotation matrix corresponding to the yaw angle θ; represents the nominal position vector; l represents the wheelbase between the front and rear axles of the vehicle.

4. The method according to claim 1, It is characterized in that The target error confidence is characterized by the covariance of the error between the actual pose increment and the nominal pose increment between the time T and the time T+1.

5. The method according to claim 4, It is characterized in that The displacement parameter of the error is determined based on the X-axis displacement and the Y-axis displacement corresponding to the vehicle coordinate system at time T in the six-degree-of-freedom model.

6. The method according to claim 1, It is characterized in that The step of obtaining the target error confidence corresponding to the time between the time T and the time T+1 according to the error propagation algorithm, the wheel speed meter model, the vehicle kinematic model and the time derivative of the vehicle nominal state comprises: Acquire the time derivative of the real state of the vehicle based on the wheel speed meter model and the vehicle kinematic model; According to the error propagation algorithm, the time derivative of the real state of the vehicle is combined with the time derivative of the nominal state of the vehicle to determine the time derivative of the error between the real state increment and the nominal state increment between the time T and the time T+1; Numerically integrating the time derivative of the error to obtain a transfer equation of the error; The covariance of the error between the true state increment and the nominal state increment between the time T and the time T+1 is recursively obtained based on the error transfer equation, and the covariance of the error is used to characterize the corresponding target error confidence between the time T and the time T+1.

7. The method according to claim 6, It is characterized in that The time derivative of the error is: The error transfer equation is expressed as: f 12 =-R vi δt g 24 =δtI 3×3 in: The expression of the transfer equation of the covariance of the error is: P t+1 =(Fδt+I)P t (Fδt+I) T +GδtQ(Gδt) T Wherein, the N moments between the T moment and the T+1 moment correspond to target observation data respectively, t is any moment other than the last moment among the N moments; δt is the interval between the tth moment and the t+1th moment; is the unit vector in the X-axis direction of the vehicle coordinate system at time T; is the unit vector in the Y-axis direction of the vehicle coordinate system at time T; is the unit vector in the Z-axis direction of the vehicle coordinate system at time T; R θ is the rotation matrix containing only the rotation component of the yaw angle θ, R vi Represents the rotation matrix from the IMU coordinate system to the vehicle coordinate system; u m Indicates the wheel speed measurement value; u n Indicates wheel speed noise; Represents the nominal angular velocity measured by the gyroscope; Indicates the gyroscope bias; Gaussian white noise representing the gyroscope bias; represents the gyroscope angular velocity noise; represents the angular velocity of the vehicle rotating around the Z axis of the vehicle coordinate system at time T; q represents the quaternion, θ represents the yaw angle, and both the quaternion q and the yaw angle θ are used to characterize the nominal posture of the vehicle; q θ Represents the quaternion corresponding to the rotation matrix corresponding to the yaw angle θ; represents the nominal position vector; l represents the wheelbase between the front axle and the rear axle of the vehicle; is the nominal position vector obtained in the vehicle coordinate system at time T based on the X-axis displacement and Y-axis displacement in the six-degree-of-freedom model; is the position error increment obtained based on the X-axis displacement and Y-axis displacement in the six-degree-of-freedom model in the vehicle coordinate system at time T; is the attitude error increment obtained based on the six-degree-of-freedom model, is the gyroscope bias error increment obtained based on the six-degree-of-freedom model; E mid The state quantity E, E represents the intermediate time between time t and time t+1. t represents the state quantity E at time t, E t+1 Represents the state quantity E at time t+1; the state quantity E is u m , q θ ,q,θ,R θ , or R{q mid } θ is the quaternion q corresponding to the yaw angle θ mid The rotation matrix of is the wheel speed noise variance, are the noise variances of the gyroscope angular velocity in the X-axis, Y-axis, and Z-axis directions of the vehicle coordinate system at time T, respectively; are the noise variances of the gyroscope bias random walk in the X-axis, Y-axis, and Z-axis directions of the vehicle coordinate system at time T.

8. The method according to claim 1, It is characterized in that The step of estimating the real posture of the vehicle at the time T and the time T+1 according to the observation data of the designated sensor at the time T and the observation data at the time T+1, the nominal state increment corresponding to the target observation data, and the target error confidence comprises: According to the observation data of the designated sensor at the time T and the observation data at the time T+1, obtaining a first nominal position and a first sensor error confidence of the vehicle at the time T, and a second nominal position and a second sensor error confidence of the vehicle at the time T+1; According to the nominal state increment and the target error confidence corresponding to the target observation data, and the first nominal pose, the first sensor error confidence, the second nominal pose and the second sensor error confidence corresponding to the designated sensor, the actual pose of the vehicle at the time T and the time T+1 is estimated.

9. The method according to claim 8, It is characterized in that The step of estimating the actual posture of the vehicle at the time T and the time T+1 according to the nominal state increment and the target error confidence corresponding to the target observation data, and the first nominal posture, the first sensor error confidence, the second nominal posture and the second sensor error confidence corresponding to the designated sensor, comprises: The actual position of the vehicle at the time T and the time T+1 is estimated according to the following formula: in, is the actual state of the vehicle, where is the real position of the vehicle in the world coordinate system, is the gyroscope bias, is the real state of the vehicle at time T, is the real state of the vehicle at time T+1, δξ is the nominal state increment between time T and time T+1 corresponding to the target observation data, P -1 is the target error confidence, is the actual position of the vehicle at time T, is the actual position of the vehicle at time T+1, M is the number of the specified sensors, and the value range of m is [1, M]. is the first nominal pose corresponding to the mth specified sensor at time T, is the second nominal pose corresponding to the mth specified sensor at time T+1, is the first sensor error confidence corresponding to the mth specified sensor at time T, The second sensor error confidence level corresponding to the mth specified sensor at time T+1.

10. The method according to claim 9, It is characterized in that Refer to the following formula expression in, Represents the displacement vector of the vehicle in the world coordinate system at time T+1; Represents the displacement vector of the vehicle in the world coordinate system at time T; represents the nominal displacement increment of the vehicle in the vehicle coordinate system at time T from time T to time T+1 obtained by pre-integration; Indicates the attitude angle of the vehicle in the world coordinate system at time T, expressed in quaternion; Indicates the attitude angle of the vehicle in the world coordinate system at time T+1, expressed in quaternion; Quaternions The corresponding rotation matrix, [] xy It means extracting the displacement of the vehicle in the XY plane of the vehicle coordinate system at time T; Represents the rotation change of the vehicle from time T to time T+1 obtained by pre-integration; [] xyz Represents the vector part of the quaternion to be extracted; Indicates the difference between the change in gyroscope bias from time T to time T+1 and the nominal change obtained by pre-integration.

11. A posture estimation device, It is characterized in that include: A data acquisition module, used to acquire target observation data of the vehicle between time T and time T+1; wherein the target observation data includes the wheel speed measured by the wheel speed meter and the angular velocity measured by the gyroscope; the time T and the time T+1 are two adjacent times when the designated sensor is performing data observation, and the designated sensor is a sensor provided on the vehicle for observing the vehicle posture; A state acquisition module, used for acquiring the time derivative of the nominal state of the vehicle represented by the target observation data according to a preset wheel speed meter model and a vehicle kinematic model; An integration module, used for numerically integrating the time derivative of the nominal state of the vehicle represented by the target observation data, and obtaining the nominal state increment of the vehicle between the time T and the time T+1 based on the integration result; the nominal state increment includes the vehicle posture increment and the gyroscope bias increment obtained based on the target observation data; A confidence acquisition module, configured to obtain a target error confidence corresponding to the time between the time T and the time T+1 according to an error propagation algorithm, the wheel speed meter model, the vehicle kinematic model, and the time derivative of the vehicle nominal state; wherein the target error confidence is a confidence of an error between a real state increment and the nominal state increment between the time T and the time T+1; The posture estimation module is used to estimate the actual posture of the vehicle at the time T and the time T+1 based on the observation data of the designated sensor at the time T and the observation data at the time T+1, the nominal state increment corresponding to the target observation data, and the target error confidence.

12. An electronic device, It is characterized in that The electronic device comprises: processor; a memory for storing instructions executable by the processor; The processor is used to read the executable instructions from the memory and execute the instructions to implement the posture estimation method described in any one of claims 1-10 above.

13. A computer-readable storage medium, It is characterized in that The storage medium stores a computer program, and the computer program is used to execute the posture estimation method described in any one of claims 1 to 10.