Vehicle pose updating method, device, equipment, medium, program product and vehicle

Abstract

The invention relates to the technical field of vehicle automatic driving, and discloses a vehicle pose updating method, device and equipment, a medium, a program product and a vehicle, and the method comprises the steps: obtaining pose data of a current frame of the vehicle, and the acceleration, angular velocity, wheel speed and timestamp of each frame in a preset frame number; the preset frame number comprises a current frame and a plurality of continuous historical frame numbers taking the current frame as an end point; based on a complementary filtering principle, calculating an angular velocity compensation amount according to the pose data of the current frame and the acceleration, the angular velocity, the wheel speed and the timestamp of the preset frame number; and updating the pose data of the current frame based on the angular velocity compensation amount. According to the method, the angular velocity is compensated according to the historical data, the problem of error accumulation can be solved, the accuracy of the angular velocity is improved, and therefore angle divergence of the pitch angle and the roll angle of the vehicle is restrained, and the accuracy and stability of vehicle pose estimation in the dynamic environment are improved.

Description

Technical Field

[0001] The present invention relates to the field of vehicle automatic driving technology, and in particular to a vehicle posture updating method, device, equipment, medium, program product and vehicle. Background Art

[0002] With the rapid development of intelligent transportation and autonomous driving technologies, high-precision vehicle pose estimation has become a key link in achieving safe and effective navigation and control. Traditional pose estimation methods are often limited by the influence of inertial measurement unit (IMU) sensor errors and dynamic environment. Gyroscopes can provide high-precision angular velocity measurements, but there are problems of error accumulation. After long-term operation, there are problems such as vehicle pitch and roll angle divergence, which leads to errors in pose updates. Summary of the invention

[0003] In view of this, the present invention provides a vehicle posture updating method, device, equipment, medium, program product and vehicle to solve the problem of inaccurate posture updating.

[0004] In a first aspect, the present invention provides a vehicle posture updating method, the method comprising:

[0005] Obtain the vehicle's current frame posture data, as well as the acceleration, angular velocity, wheel speed and timestamp of each frame in a preset number of frames, where the preset number of frames includes the current frame and multiple consecutive historical frames with the current frame as the end point;

[0006] Based on the complementary filtering principle, the angular velocity compensation is calculated according to the posture data of the current frame and the acceleration, angular velocity, wheel speed and timestamp of the preset frame number;

[0007] Update the pose data of the current frame based on the angular velocity compensation amount.

[0008] The vehicle posture updating method provided by the embodiment of the present invention calculates the angular velocity compensation amount according to the posture data of the current frame of the vehicle and the acceleration, angular velocity, wheel speed and timestamp of a preset number of frames based on the complementary filtering principle, and updates the posture data of the current frame based on the angular velocity compensation amount. The present invention can solve the problem of error accumulation and improve the accuracy of angular velocity by compensating the angular velocity according to historical data, thereby suppressing the angular divergence of the vehicle's pitch angle and roll angle, and improving the accuracy and stability of vehicle posture estimation in a dynamic environment.

[0009] In an optional embodiment, based on the complementary filtering principle, the angular velocity compensation amount is calculated according to the posture data of the current frame, and the acceleration, angular velocity, wheel speed and timestamp of a preset frame number, including: calculating the first gravity acceleration according to the acceleration, angular velocity, wheel speed and timestamp of the preset frame number; determining the second gravity acceleration corresponding to the posture data of the current frame, and calculating the error between the first gravity acceleration and the second gravity acceleration; calculating the product of the error and the preset compensation coefficient to obtain the angular velocity compensation amount.

[0010] The present invention can calculate the angular velocity compensation amount according to the acceleration, angular velocity and wheel speed, without adding other observation and sensing data. Only the output of the gyroscope and accelerometer of the sensor IMU and the vehicle wheel speed data are needed to improve the posture update accuracy, achieving low cost and high return.

[0011] In an optional embodiment, the first gravitational acceleration is calculated based on the acceleration, angular velocity, wheel speed and timestamp of a preset frame number, including: averaging the acceleration, angular velocity, wheel speed and timestamp of the preset frame number to obtain a mean acceleration, a mean angular velocity, a mean wheel speed and a mean time interval; calculating the linear acceleration of the vehicle based on the wheel speed and timestamp corresponding to the preset frame interval in the preset frame number, and calculating the Coriolis acceleration of the vehicle based on the mean angular velocity and the mean wheel speed; subtracting the linear acceleration and the Coriolis acceleration from the mean acceleration to obtain the first gravitational acceleration.

[0012] The present invention can consider the influence of the linear acceleration generated by the vehicle movement and the Coriolis acceleration generated by the rotation on the observation of the gravitational acceleration when compensating the angular velocity, obtain a pure gravitational acceleration value, improve the compensation effect, and obtain a more accurate angular velocity. At the same time, the data of the preset frame number are averaged, which can reduce the volatility of the data, improve the stability of the data, and provide a more reliable input for subsequent processing.

[0013] In an optional embodiment, the second gravitational acceleration corresponding to the posture data of the current frame is determined, and the error between the first gravitational acceleration and the second gravitational acceleration is calculated, including: taking the product of the posture data represented in quaternion form and a preset unit vector as the normalized second gravitational acceleration; normalizing the first gravitational acceleration; calculating the product of the normalized first gravitational acceleration and the normalized second gravitational acceleration to obtain the error.

[0014] The present invention can reflect the difference between the actual posture and theoretical posture of the vehicle by calculating the error between the gravitational acceleration generated by the current posture and the observed gravitational acceleration obtained from relevant data, so as to obtain the corresponding angular velocity compensation amount according to the difference.

[0015] In an optional embodiment, the posture data includes position data and attitude data, and the posture data of the current frame is updated based on the angular velocity compensation, including: adding the angular velocity compensation to the mean angular velocity to obtain the compensated angular velocity; converting the compensated angular velocity into a rotation angle through an integral operation, and converting the rotation angle into an adjusted rotation matrix through an exponential lifting operation; calculating the product of the adjusted rotation matrix and the rotation matrix corresponding to the posture data to obtain updated posture data; updating the position data of the current frame according to the updated posture data to obtain updated position data.

[0016] The present invention uses the compensated angular velocity to perform track calculation and posture update, which can achieve high-precision real-time update of the vehicle's posture, provide more reliable and accurate posture information for advanced technical fields such as autonomous driving, unmanned vehicles, and intelligent robot navigation, ensure driving safety, and improve user satisfaction.

[0017] In a second aspect, the present invention provides a vehicle posture updating device, which includes: a data acquisition module, used to obtain the posture data of the vehicle in a current frame, and the acceleration, angular velocity, wheel speed and timestamp of each frame in a preset number of frames, the preset number of frames including the current frame and multiple consecutive historical frames with the current frame as the end point; a compensation calculation module, used to calculate the angular velocity compensation amount based on the complementary filtering principle according to the posture data of the current frame and the acceleration, angular velocity, wheel speed and timestamp of a preset number of frames; a posture update module, used to update the posture data of the current frame based on the angular velocity compensation amount.

[0018] In a third aspect, the present invention provides a computer device, comprising: a memory and a processor, the memory and the processor are communicatively connected to each other, computer instructions are stored in the memory, and the processor executes the vehicle posture updating method of the first aspect or any corresponding embodiment thereof by executing the computer instructions.

[0019] In a fourth aspect, the present invention provides a computer-readable storage medium having computer instructions stored thereon, the computer instructions being used to enable a computer to execute the vehicle posture updating method of the first aspect or any corresponding embodiment thereof.

[0020] In a fifth aspect, the present invention provides a computer program product, comprising computer instructions, wherein the computer instructions are used to enable a computer to execute the vehicle posture updating method of the above-mentioned first aspect or any corresponding embodiment thereof.

[0021] In a sixth aspect, the present invention provides a vehicle, comprising: an inertial measurement unit for collecting vehicle posture data, acceleration and angular velocity; a wheel speed sensor for collecting the wheel speed of the vehicle; and a controller connected to the inertial measurement unit and the wheel speed sensor, for executing the vehicle posture update method of the first aspect or any corresponding embodiment thereof.

[0022] Beneficial effects of the present invention:

[0023] (1) The present invention can solve the problem of error accumulation and improve the accuracy of angular velocity by calculating the angular velocity compensation amount according to historical frame data, thereby suppressing the angular divergence of the vehicle's pitch angle and roll angle, and improving the accuracy and stability of vehicle posture estimation in a dynamic environment;

[0024] (2) The present invention calculates the angular velocity compensation amount according to the acceleration, angular velocity and wheel speed, and can improve the accuracy of posture update by relying only on the output of the gyroscope and accelerometer of the sensor IMU and the vehicle wheel speed data, without adding other observation and sensor data, thus achieving low cost and high return;

[0025] (3) The present invention can obtain a pure gravitational acceleration value by considering the influence of the linear acceleration generated by the vehicle motion and the Coriolis acceleration generated by the rotation on the gravitational acceleration observation during angular velocity compensation, thereby improving the compensation effect and obtaining a more accurate angular velocity. BRIEF DESCRIPTION OF THE DRAWINGS

[0026] In order to more clearly illustrate the specific implementation methods of the present invention or the technical solutions in the prior art, the drawings required for use in the specific implementation methods or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are some implementation methods of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0027] Figure 1 is a schematic flow chart of a vehicle posture updating method according to an embodiment of the present invention;

[0028] Figure 2 is a schematic diagram of the operation flow of a vehicle posture updating method according to an embodiment of the present invention;

[0029] Figure 3 is a flow chart of another vehicle posture updating system according to an embodiment of the present invention;

[0030] Figure 4 is a flow chart of another vehicle posture updating method according to an embodiment of the present invention;

[0031] Figure 5 is a structural block diagram of a vehicle posture updating device according to an embodiment of the present invention;

[0032] Figure 6 It is a schematic diagram of the hardware structure of a computer device according to an embodiment of the present invention. DETAILED DESCRIPTION

[0033] In order to make the purpose, technical solution and advantages of the embodiments of the present invention clearer, the technical solution in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative work are within the scope of protection of the present invention.

[0034] The embodiment of the present invention is applicable to the scenario of high-precision real-time update of vehicle posture during vehicle driving to achieve safe and effective navigation and control, taking a multi-storey underground parking lot as an example. The embodiment of the present invention provides a vehicle posture update method, which calculates the angular velocity compensation amount according to the historical frame data to achieve the effect of improving the accuracy of posture data.

[0035] According to an embodiment of the present invention, an embodiment of a vehicle posture updating method is provided. It should be noted that the steps shown in the flowchart of the accompanying drawings can be executed in a computer system such as a set of computer executable instructions, and although a logical order is shown in the flowchart, in some cases, the steps shown or described can be executed in an order different from that shown here.

[0036] In this embodiment, a vehicle posture updating method is provided, which can be used in a controller of a vehicle. Figure 1 is a flow chart of a vehicle posture updating method according to an embodiment of the present invention. Figure 1 As shown, the process includes the following steps:

[0037] Step S101, obtaining the posture data of the vehicle in the current frame, and the acceleration, angular velocity, wheel speed and timestamp of each frame in a preset number of frames, where the preset number of frames includes the current frame and multiple consecutive historical frames with the current frame as the end point.

[0038] Specifically, in the embodiment of the present invention, Figure 2As shown, during the operation of the vehicle, the three-axis acceleration and angular velocity in each frame of data collected in real time by the inertial measurement unit IMU pre-deployed in the vehicle, and the posture data calculated based on the angular velocity and acceleration are obtained. Among them, the inertial measurement unit IMU is a device for measuring the three-axis attitude angle (or angular rate) and acceleration of an object. The IMU and the vehicle body are connected, that is, the inertial sensitive elements (gyroscopes and accelerometers) are directly "bound" to the vehicle body, thereby realizing the guidance and navigation of the vehicle. Generally, an IMU includes three single-axis accelerometers and three single-axis gyroscopes. The accelerometer detects the acceleration signal of the object in the independent three axes of the carrier coordinate system, and the gyroscope detects the angular velocity signal of the carrier relative to the navigation coordinate system, measures the angular velocity and acceleration of the object in three-dimensional space, and calculates the attitude of the object. Among them, the angular velocity output by the gyroscope is an instantaneous quantity, and the angular velocity cannot be used directly in attitude balance. The angular velocity and time integral are required to calculate the angle, and the obtained angle change is added to the initial angle to obtain the target angle. For a moving vehicle, the target angles generally include pitch, roll and yaw. Pitch is the angle between the vehicle body and the ground plane, which is defined as the inclination angle between the front of the vehicle and the horizontal plane; roll is the angle between the longitudinal axis of the vehicle body and the horizontal plane, which is defined as the angle of rotation of the vehicle body around the longitudinal axis; yaw is the angle between the longitudinal axis of the vehicle body and the true north direction, which is defined as the direction in which the front of the vehicle is pointing.

[0039] In some optional embodiments, taking the scenario of a vehicle driving towards a multi-story underground parking lot as an example, in order to accurately determine the number of floors of the parking lot where the vehicle is located, the three-axis acceleration a obtained by the embodiment of the present invention is a vector signal composed of three-axis acceleration signals, and the angular velocity ω focuses on the angular velocity signal corresponding to the pitch angle. In addition, due to the characteristics of the sensor itself or environmental factors, the angular velocity data output by the IMU may have a fixed deviation, namely, a static zero bias. In order to eliminate this deviation, the embodiment of the present invention averages the data over a period of time to obtain a reference value. This reference value is then subtracted from the original data to obtain the angular velocity ω after removing the static zero bias. The specific process is a conventional technical means in this field and will not be repeated here.

[0040] In some optional implementations, the vehicle wheel speed sensor acquires wheel speed data in real time, and the embodiment of the present invention acquires the wheel speed v of each frame from the vehicle CAN bus. At the same time, the acceleration a, angular velocity ω and wheel speed v are all accompanied by a timestamp t. Since the vehicle IMU and the wheel speed sensor have completed time synchronization, the timestamps of the data have been accurately aligned to ensure that all data are collected at the same time point.

[0041] In some optional implementations, in order to update the posture in real time with high precision, after obtaining the posture data, acceleration, angular velocity, wheel speed and timestamp corresponding to the current frame of each new frame, the embodiment of the present invention uses this frame as the current frame, and takes the acceleration, angular velocity, wheel speed and timestamp corresponding to each historical frame of n-1 frames forward based on the current frame, so that the data finally obtained includes the posture data of the current frame and the acceleration a of n frames. i , angular velocity ω i , wheel speed v i and timestamp t i .

[0042] Step S102, based on the complementary filtering principle, the angular velocity compensation is calculated according to the posture data of the current frame and the acceleration, angular velocity, wheel speed and timestamp of a preset number of frames.

[0043] Specifically, in the embodiment of the present invention, Figure 2 As shown, high-precision real-time posture is solved based on the complementary filtering principle, wherein the complementary filtering principle is to use the characteristics of the accelerometer and gyroscope in angle calculation, that is, the accelerometer is more accurate for a long time, and the gyroscope is more accurate for a short time, so the acceleration is low-pass filtered to filter out high-frequency noise, and the gyroscope is high-pass filtered to filter out low-frequency noise. These two filters are complementary to the characteristics of the two sensors and can achieve better results. Therefore, based on the complementary filtering principle, the embodiment of the present invention calculates the cumulative error of the angular velocity in this time period based on the acceleration, angular velocity, wheel speed and timestamp in the time period corresponding to the preset frame number, and uses this as the angular velocity compensation amount.

[0044] Step S103, updating the posture data of the current frame based on the angular velocity compensation amount.

[0045] Specifically, in an embodiment of the present invention, the angular velocity is corrected using the angular velocity compensation, so that the compensated angular velocity can be used to perform track calculation and posture update to obtain accurate posture data of the vehicle. After the posture data update of the current frame is completed, the current frame becomes a historical frame, the next frame data obtained is used as the data of the current frame, and the posture data is updated again according to the above steps, so that the accurate posture data of the vehicle at each moment can be obtained, and the vehicle posture can be updated in real time with high precision, ensuring safe and effective navigation and control of the vehicle, and realizing smarter and more reliable automatic driving.

[0046] The vehicle posture updating method provided by the embodiment of the present invention calculates the angular velocity compensation amount according to the posture data of the current frame of the vehicle and the acceleration, angular velocity, wheel speed and timestamp of a preset number of frames based on the complementary filtering principle, and updates the posture data of the current frame based on the angular velocity compensation amount. The present invention can solve the problem of error accumulation and improve the accuracy of angular velocity by compensating the angular velocity according to historical data, thereby suppressing the angular divergence of the vehicle's pitch angle and roll angle, and improving the accuracy and stability of vehicle posture estimation in a dynamic environment.

[0047] In this embodiment, a vehicle posture updating method is provided, which can be used in a controller of a vehicle. Figure 3 is a flow chart of a vehicle posture updating method according to an embodiment of the present invention. Figure 3 As shown, the process includes the following steps:

[0048] Step S301, obtain the vehicle's current frame posture data, and the acceleration, angular velocity, wheel speed and timestamp of each frame in a preset number of frames, the preset number of frames includes the current frame and multiple consecutive historical frames with the current frame as the end point. For details, please see Figure 1 Step S101 of the illustrated embodiment will not be described in detail here.

[0049] Step S302, based on the complementary filtering principle, the angular velocity compensation is calculated according to the posture data of the current frame and the acceleration, angular velocity, wheel speed and timestamp of a preset number of frames.

[0050] Specifically, the above step S302 includes:

[0051] Step S3021, calculating the first gravitational acceleration according to the acceleration, angular velocity, wheel speed and timestamp of a preset number of frames.

[0052] Specifically, in the embodiment of the present invention, during the driving process of the vehicle, due to the influence of the vehicle speed and the vehicle rotation, the observed gravity acceleration will be different from the actual gravity acceleration. i , angular velocity ω i , wheel speed v i and timestamp t i Calculate the observed value of gravitational acceleration g observe , taking this as the first gravitational acceleration.

[0053] In some optional implementations, the above step S3021 includes:

[0054] Step a1, averaging the acceleration, angular velocity, wheel speed and time stamp of a preset number of frames to obtain the mean acceleration, mean angular velocity, mean wheel speed and mean time interval.

[0055] Step a2, calculating the linear acceleration of the vehicle according to the wheel speed and timestamp corresponding to the preset frame interval in the preset frame number, and calculating the Coriolis acceleration of the vehicle according to the mean angular velocity and the mean wheel speed.

[0056] Step a3: subtract the linear acceleration and the Coriolis acceleration from the mean acceleration to obtain a first gravitational acceleration.

[0057] Specifically, in the embodiment of the present invention, in order to reduce the volatility of data, improve the stability of data, and provide more reliable input for subsequent processing, the acquired n frames of acceleration a are respectively i , angular velocity ω i , wheel speed v i and timestamp t i Take the average and get the mean acceleration Mean angular velocity Mean wheel speed and mean time interval The calculation formula is as follows:

[0058]

[0059] In some optional implementations, the mean acceleration obtained by the embodiment of the present invention is It is the average of the acceleration directly measured by the IMU, including the acceleration of gravity, the linear acceleration of the vehicle, and the Coriolis acceleration caused by the rotation of the vehicle. Therefore, the average acceleration needs to be Only by removing these two parts can a more accurate gravity acceleration observation value be obtained. Among them, the linear acceleration of the vehicle is obtained by comparing the change rate of the wheel speed data of the previous and next frames, which reflects the motion state of the vehicle. In the embodiment of the present invention, for the wheel speed v of the n frame i and timestamp t i , with n / 2 frames as the preset frame interval, according to the wheel speed v corresponding to the n / 2 frame interval in the n frame data i and timestamp t i Calculate the linear acceleration a′ of the vehicle, for example, the wheel speed v at 10 frames i and timestamp t i , the wheel speed differences and time differences between the 6th frame and the 1st frame, the 7th frame and the 2nd frame, the 8th frame and the 3rd frame, the 9th frame and the 4th frame, and the 10th frame and the 5th frame are calculated in sequence, and the ratio of the sum of the wheel speed differences to the sum of the time differences is used as the linear speed. This is only an example and is not limited to this. The corresponding calculation formula is as follows:

[0060]

[0061] In some optional embodiments, the Coriolis acceleration is the acceleration formed by the interaction and coupling of the relative motion and the relative motion while the object is performing the associated motion. and mean wheel speed Calculate the vehicle's Coriolis acceleration a″ using the following formula:

[0062]

[0063] Therefore, the final observed value of gravitational acceleration is This gravitational acceleration observation value is a pure gravitational acceleration value and can provide an accurate reference for subsequent calculations.

[0064] Step S3022, determining the second gravitational acceleration corresponding to the posture data of the current frame, and calculating the error between the first gravitational acceleration and the second gravitational acceleration.

[0065] Specifically, in the embodiment of the present invention, the error between the second gravity acceleration corresponding to the posture data and the calculated gravity acceleration observation value reflects the difference between the actual posture of the vehicle and the theoretical posture. By compensating for this error, angular velocity compensation can be achieved.

[0066] In some optional implementations, the above step S3022 includes:

[0067] Step b1, taking the product of the posture data in the form of quaternion representation and the preset unit vector as the normalized second gravitational acceleration.

[0068] Step b2, normalizing the first gravitational acceleration.

[0069] Step b3, calculating the product of the normalized first gravitational acceleration and the normalized second gravitational acceleration to obtain an error.

[0070] Specifically, in the embodiment of the present invention, the representation of the posture data includes: attitude angle, quaternion, Euler angle, rotation matrix and posture matrix, wherein quaternion is an effective method for representing and manipulating the direction of objects in three-dimensional space, which avoids the universal lock problem and can use spherical interpolation to achieve smooth rotation transition. The embodiment of the present invention calculates the product of the posture data q represented by the quaternion and the unit vector vec = [0,0,1] to obtain the second gravity acceleration q*vec generated by the current frame posture data after normalization, and at the same time converts the gravity acceleration observation value g observe Normalize: g observe / ||g observe||, calculate the product of the normalized first gravitational acceleration and the normalized second gravitational acceleration to get the error e, the calculation formula is as follows:

[0071] e=(g observe / ||g observe ||)×q*vec

[0072] Step S3023, calculating the product of the error and the preset compensation coefficient to obtain the angular velocity compensation amount.

[0073] Specifically, in the embodiment of the present invention, the error e obtained above is multiplied by a preset compensation coefficient k p , the acceleration compensation k can be obtained p e, where the compensation coefficient k p It is related to the performance of the sensor and can be determined through debugging in actual operation. The value of 0.2 in the present invention is only used as an example and is not limited to this.

[0074] Step S303: Update the posture data of the current frame based on the angular velocity compensation. Figure 1 Step S103 of the illustrated embodiment will not be described in detail here.

[0075] The vehicle posture updating method provided by the embodiment of the present invention calculates the angular velocity compensation amount according to the posture data of the current frame of the vehicle and the acceleration, angular velocity, wheel speed and timestamp of a preset number of frames based on the complementary filtering principle, and updates the posture data of the current frame based on the angular velocity compensation amount. The present invention can solve the problem of error accumulation and improve the accuracy of angular velocity by compensating the angular velocity according to historical data, thereby suppressing the angular divergence of the vehicle's pitch angle and roll angle, and improving the accuracy and stability of vehicle posture estimation in a dynamic environment.

[0076] In this embodiment, a vehicle posture updating method is provided, which can be used in a controller of a vehicle. Figure 4 is a flow chart of a vehicle posture updating method according to an embodiment of the present invention. Figure 4 As shown, the process includes the following steps:

[0077] Step S401, obtain the vehicle's current frame posture data, and the acceleration, angular velocity, wheel speed and timestamp of each frame in a preset number of frames, the preset number of frames includes the current frame and multiple consecutive historical frames with the current frame as the end point. For details, please see Figure 3 Step S301 of the illustrated embodiment will not be described in detail here.

[0078] Step S402, based on the complementary filtering principle, calculate the angular velocity compensation amount according to the posture data of the current frame and the acceleration, angular velocity, wheel speed and timestamp of the preset frame number. For details, please refer to Figure 3Step S302 of the illustrated embodiment will not be described in detail here.

[0079] Step S403: updating the posture data of the current frame based on the angular velocity compensation amount.

[0080] Specifically, the above step S403 includes:

[0081] Step S4031, adding the angular velocity compensation amount to the mean angular velocity to obtain the compensated angular velocity.

[0082] Specifically, in the embodiment of the present invention, in order to reduce the volatility of data and improve the stability of data, the mean angular velocity As the benchmark, add the angular velocity compensation on this basis to get the compensated angular velocity ω, that is,

[0083] Step S4032, converting the compensated angular velocity into a rotation angle through an integral operation, and converting the rotation angle into an adjustment rotation matrix through an exponential lifting operation, calculating the product of the adjustment rotation matrix and the rotation matrix corresponding to the posture data, and obtaining the updated posture data.

[0084] Specifically, in the embodiment of the present invention, the posture data of the vehicle includes position data and attitude data, so it is necessary to perform dead reckoning according to the angular velocity compensation, and update the position data and attitude data respectively. The compensated angular velocity ω is converted into a rotation angle by an integral operation, and the rotation angle is converted into an adjustment rotation matrix by an exponential lifting operation, and the product of the adjustment rotation matrix and the rotation matrix corresponding to the posture data is calculated to obtain the updated posture data. The calculation formula is as follows:

[0085] R=R*SO3Exp(ω*dt)

[0086] Among them, SO3Exp() is the exponential mapping operation from Lie algebra to Lie group, which is used to convert the Lie algebra elements representing the rotation in the three-dimensional space into an actual rotation matrix through exponential mapping. This rotation matrix performs the actual rotation operation in the three-dimensional space. The embodiment of the present invention converts the compensated angular velocity corresponding to the compensated angular velocity into a rotation matrix, thereby adjusting the rotation matrix of the current frame posture data to obtain an updated rotation matrix, that is, the updated posture data.

[0087] Step S4033, updating the position data of the current frame according to the updated posture data to obtain updated position data.

[0088] Specifically, in an embodiment of the present invention, the position data of the current frame is updated based on the updated posture data, and the formula is as follows:

[0089] P=P+R*SO3Exp(ω*dt)

[0090] In some optional implementations, the embodiments of the present invention can further calculate the position and speed of the vehicle at each moment by combining the acceleration data and the wheel speed data.

[0091] The vehicle posture updating method provided by the embodiment of the present invention calculates the angular velocity compensation amount according to the posture data of the current frame of the vehicle and the acceleration, angular velocity, wheel speed and timestamp of a preset number of frames based on the complementary filtering principle, and updates the posture data of the current frame based on the angular velocity compensation amount. The present invention can solve the problem of error accumulation and improve the accuracy of angular velocity by compensating the angular velocity according to historical data, thereby suppressing the angular divergence of the vehicle's pitch angle and roll angle, and improving the accuracy and stability of vehicle posture estimation in a dynamic environment.

[0092] In this embodiment, a vehicle posture updating device is also provided, which is used to implement the above-mentioned embodiments and preferred implementation modes, and will not be repeated hereafter. As used below, the term "module" can implement a combination of software and / or hardware of a predetermined function. Although the devices described in the following embodiments are preferably implemented in software, the implementation of hardware, or a combination of software and hardware, is also possible and conceivable.

[0093] This embodiment provides a vehicle posture updating device, such as Figure 5 As shown, including:

[0094] The data acquisition module 501 is used to obtain the posture data of the vehicle in the current frame, and the acceleration, angular velocity, wheel speed and timestamp of each frame in a preset number of frames. The preset number of frames includes the current frame and multiple consecutive historical frames with the current frame as the end point.

[0095] The compensation calculation module 502 is used to calculate the angular velocity compensation amount based on the complementary filtering principle according to the posture data of the current frame and the acceleration, angular velocity, wheel speed and timestamp of a preset number of frames.

[0096] The posture updating module 503 is used to update the posture data of the current frame based on the angular velocity compensation amount.

[0097] In some optional implementations, the compensation calculation module 502 includes:

[0098] The first gravity acceleration calculation unit is used to calculate the first gravity acceleration according to the acceleration, angular velocity, wheel speed and timestamp of a preset frame number.

[0099] The error calculation unit is used to determine the second gravitational acceleration corresponding to the posture data of the current frame, and calculate the error between the first gravitational acceleration and the second gravitational acceleration.

[0100] The angular velocity compensation amount calculation unit is used to calculate the product of the error and the preset compensation coefficient to obtain the angular velocity compensation amount.

[0101] In some optional implementations, the first gravity acceleration calculation unit includes:

[0102] The first calculation subunit is used to average the acceleration, angular velocity, wheel speed and time stamp of a preset number of frames to obtain a mean acceleration, a mean angular velocity, a mean wheel speed and a mean time interval.

[0103] The second calculation subunit is used to calculate the linear acceleration of the vehicle according to the wheel speed and time stamp corresponding to the preset frame number interval in the preset frame number, and calculate the Coriolis acceleration of the vehicle according to the mean angular velocity and the mean wheel speed.

[0104] The third calculation subunit is used to subtract the linear acceleration and the Coriolis acceleration from the mean acceleration to obtain a first gravitational acceleration.

[0105] In some optional implementations, the error calculation unit includes:

[0106] The fourth calculation subunit is used to use the product of the posture data in the form of quaternion representation and the preset unit vector as the normalized second gravitational acceleration.

[0107] The fifth calculation subunit is used to normalize the first gravitational acceleration.

[0108] The sixth calculation subunit is used to calculate the product of the normalized first gravitational acceleration and the normalized second gravitational acceleration to obtain an error.

[0109] In some optional implementations, the posture data includes position data and posture data, and the posture update module 503 includes:

[0110] The angular velocity compensation unit is used to add the angular velocity compensation amount to the mean angular velocity to obtain the compensated angular velocity.

[0111] The posture update unit is used to convert the compensated angular velocity into a rotation angle through an integral operation, and to convert the rotation angle into an adjustment rotation matrix through an exponential lifting operation, and to calculate the product of the adjustment rotation matrix and the rotation matrix corresponding to the posture data to obtain the updated posture data.

[0112] The position updating unit is used to update the position data of the current frame according to the updated posture data to obtain the updated position data.

[0113] The further functional description of each of the above modules and units is the same as that of the above corresponding embodiments and will not be repeated here.

[0114] The vehicle posture updating device in this embodiment is presented in the form of a functional unit, where the unit refers to an ASIC (Application Specific Integrated Circuit) circuit, a processor and memory that executes one or more software or fixed programs, and / or other devices that can provide the above functions.

[0115] The embodiment of the present invention also provides a computer device having the above Figure 5 The vehicle posture updating device shown.

[0116] See also Figure 6 , Figure 6 is a schematic diagram of the structure of a computer device provided by an optional embodiment of the present invention, such as Figure 6 As shown, the computer device includes: one or more processors 10, a memory 20, and interfaces for connecting various components, including high-speed interfaces and low-speed interfaces. Various components are connected to each other using different buses for communication, and can be installed on a common mainboard or installed in other ways as needed. The processor can process the instructions executed in the computer device, including instructions stored in or on the memory to display the graphical information of the GUI on an external input / output device (such as, a display device coupled to the interface). In some optional embodiments, if necessary, multiple processors and / or multiple buses can be used together with multiple memories and multiple memories. Similarly, multiple computer devices can be connected, and each device provides some necessary operations (for example, as a server array, a group of blade servers, or a multi-processor system). Figure 6 A processor 10 is taken as an example.

[0117] The processor 10 may be a central processing unit, a network processor or a combination thereof. The processor 10 may further include a hardware chip. The hardware chip may be a dedicated integrated circuit, a programmable logic device or a combination thereof. The programmable logic device may be a complex programmable logic device, a field programmable gate array, a general purpose array logic or any combination thereof.

[0118] The memory 20 stores instructions executable by at least one processor 10, so that the at least one processor 10 executes the method shown in the above embodiment.

[0119] The memory 20 may include a program storage area and a data storage area, wherein the program storage area may store an operating system, an application required for at least one function; the data storage area may store data created according to the use of the computer device, etc. In addition, the memory 20 may include a high-speed random access memory, and may also include a non-transient memory, such as at least one disk storage device, a flash memory device, or other non-transient solid-state storage device. In some optional embodiments, the memory 20 may optionally include a memory remotely arranged relative to the processor 10, and these remote memories may be connected to the computer device via a network. Examples of the above-mentioned network include, but are not limited to, the Internet, an intranet, a local area network, a mobile communication network, and combinations thereof.

[0120] The memory 20 may include a volatile memory, such as a random access memory; the memory may also include a non-volatile memory, such as a flash memory, a hard disk or a solid state drive; the memory 20 may also include a combination of the above types of memory.

[0121] The computer device further comprises a communication interface 30 for the computer device to communicate with other devices or a communication network.

[0122] The embodiment of the present invention also provides a computer-readable storage medium. The method according to the embodiment of the present invention can be implemented in hardware, firmware, or can be implemented as a computer code that can be recorded in a storage medium, or can be implemented as a computer code that is originally stored in a remote storage medium or a non-temporary machine-readable storage medium and will be stored in a local storage medium through a network download, so that the method described herein can be stored in such software processing on a storage medium using a general-purpose computer, a dedicated processor, or programmable or dedicated hardware. Among them, the storage medium can be a magnetic disk, an optical disk, a read-only storage memory, a random access memory, a flash memory, a hard disk or a solid-state hard disk, etc.; further, the storage medium can also include a combination of the above types of memories. It can be understood that a computer, a processor, a microprocessor controller, or programmable hardware includes a storage component that can store or receive software or computer code. When the software or computer code is accessed and executed by a computer, a processor, or hardware, the method shown in the above embodiment is implemented.

[0123] A part of the present invention may be applied as a computer program product, such as a computer program instruction, which, when executed by a computer, can call or provide the method and / or technical solution according to the present invention through the operation of the computer. Those skilled in the art should understand that the existence of the computer program instruction in a computer-readable medium includes, but is not limited to, a source file, an executable file, an installation package file, etc., and accordingly, the way in which the computer program instruction is executed by the computer includes, but is not limited to: the computer directly executes the instruction, or the computer compiles the instruction and then executes the corresponding compiled program, or the computer reads and executes the instruction, or the computer reads and installs the instruction and then executes the corresponding installed program. Here, the computer-readable medium may be any available computer-readable storage medium or communication medium accessible to the computer.

[0124] The embodiment of the present invention further provides a vehicle, comprising: an inertial measurement unit, for collecting posture data, acceleration and angular velocity of the vehicle; a wheel speed sensor, for collecting wheel speed of the vehicle; a controller, connected to the inertial measurement unit and the wheel speed sensor, for executing the above Figure 1 , Figure 3 or Figure 4 The vehicle pose update method shown.

[0125] Although the embodiments of the present invention have been described in conjunction with the accompanying drawings, those skilled in the art may make various modifications and variations without departing from the spirit and scope of the present invention, and such modifications and variations are all within the scope defined by the appended claims.

Claims

1. A vehicle posture updating method, characterized in that: The method comprises: Obtaining the posture data of the vehicle in the current frame, and the acceleration, angular velocity, wheel speed and timestamp of each frame in a preset number of frames, wherein the preset number of frames includes the current frame and a plurality of consecutive historical frames with the current frame as the end point; Based on the complementary filtering principle, the angular velocity compensation amount is calculated according to the posture data of the current frame and the acceleration, angular velocity, wheel speed and timestamp of the preset number of frames; The posture data of the current frame is updated based on the angular velocity compensation amount.

2. The method according to claim 1, characterized in that The method of calculating the angular velocity compensation amount based on the complementary filtering principle according to the posture data of the current frame and the acceleration, angular velocity, wheel speed and timestamp of the preset number of frames includes: Calculating a first gravitational acceleration according to the acceleration, angular velocity, wheel speed and timestamp of the preset number of frames; Determine a second gravitational acceleration corresponding to the posture data of the current frame, and calculate an error between the first gravitational acceleration and the second gravitational acceleration; The product of the error and a preset compensation coefficient is calculated to obtain the angular velocity compensation amount.

3. The method according to claim 2, characterized in that The calculating the first gravity acceleration according to the acceleration, angular velocity, wheel speed and timestamp of the preset number of frames includes: The acceleration, the angular velocity, the wheel speed and the timestamp of the preset number of frames are respectively averaged to obtain a mean acceleration, a mean angular velocity, a mean wheel speed and a mean time interval; Calculating the linear acceleration of the vehicle according to the wheel speed corresponding to the preset frame number interval in the preset frame number and the timestamp, and calculating the Coriolis acceleration of the vehicle according to the mean angular velocity and the mean wheel speed; The first gravitational acceleration is obtained by subtracting the linear acceleration and the Coriolis acceleration from the mean acceleration.

4. The method according to claim 3, characterized in that The determining the second gravitational acceleration corresponding to the posture data of the current frame, and calculating the error between the first gravitational acceleration and the second gravitational acceleration, comprises: The product of the posture data in the form of quaternion representation and a preset unit vector is used as the normalized second gravitational acceleration; Normalizing the first gravitational acceleration; The product of the normalized first gravitational acceleration and the normalized second gravitational acceleration is calculated to obtain the error.

5. The method according to claim 3, characterized in that: The posture data includes position data and posture data, and the updating of the posture data of the current frame based on the angular velocity compensation amount includes: Adding the angular velocity compensation amount to the mean angular velocity to obtain a compensated angular velocity; Converting the compensated angular velocity into a rotation angle by an integral operation, converting the rotation angle into an adjustment rotation matrix by an exponential lifting operation, calculating the product of the adjustment rotation matrix and the rotation matrix corresponding to the posture data, and obtaining updated posture data; The position data of the current frame is updated according to the updated posture data to obtain updated position data.

6. A vehicle posture updating device, characterized in that: The device comprises: A data acquisition module, used to acquire the posture data of the vehicle in the current frame, and the acceleration, angular velocity, wheel speed and timestamp of each frame in a preset number of frames, wherein the preset number of frames includes the current frame and a plurality of consecutive historical frames with the current frame as the end point; A compensation calculation module, configured to calculate an angular velocity compensation amount based on the complementary filtering principle and according to the posture data of the current frame and the acceleration, angular velocity, wheel speed and timestamp of the preset number of frames; The posture updating module is used to update the posture data of the current frame based on the angular velocity compensation amount.

7. A computer device, characterized in that: include: A memory and a processor, wherein the memory and the processor are communicatively connected to each other, the memory stores computer instructions, and the processor executes the vehicle posture updating method according to any one of claims 1 to 5 by executing the computer instructions.

8. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores computer instructions, and the computer instructions are used to enable a computer to execute the vehicle posture updating method according to any one of claims 1 to 5.

9. A computer program product, characterized in that The method comprises computer instructions for causing a computer to execute the vehicle posture updating method according to any one of claims 1 to 5.

10. A vehicle, characterized in that: include: Inertial measurement unit, used to collect vehicle posture data, acceleration and angular velocity; A wheel speed sensor, used to collect the wheel speed of the vehicle; A controller is connected to the inertial measurement unit and the wheel speed sensor, and is used to execute the vehicle posture updating method according to any one of claims 1 to 5.

Images