A horizontal attitude initialization method and initialization device

Abstract

The application provides a horizontal attitude initialization method and device, comprising: acquiring acceleration data, angular velocity data and wheel speed data of a carrier in a predetermined time period in time synchronization when the carrier is in a motion working condition; determining a pre-integral quantity of acceleration from a previous sampling time to the sampling time for each predetermined sampling time; determining a difference value between the pre-integral quantity of acceleration from the previous sampling time to the sampling time and a wheel speed pre-integral quantity from the previous sampling time to the sampling time, and determining an integral quantity of gravitational acceleration of the carrier in the predetermined time period based on the difference value; and determining an attitude quaternion of a terminal sampling time in the predetermined time period based on the integral quantity of gravitational acceleration of the carrier in the predetermined time period. The initialization method can reduce the precision error of the horizontal attitude angle calculation of an object in a motion working condition.

Description

Technical Field

[0001] This application relates to the field of inertial navigation and positioning technology, and in particular to a method and apparatus for initializing horizontal attitude. Background Technology

[0002] IMU sensors are currently widely used in emerging industries such as vehicles, robotics, and the Internet of Things to provide high-precision information on the horizontal attitude, velocity, and position of objects. An IMU sensor is a six-axis sensor that uses changes in inertia to measure the acceleration and rotation of an object. It contains a three-axis accelerometer and a three-axis angular velocity meter (gyroscope). By using an IMU sensor to measure the acceleration and angular velocity of an object, the object's attitude quaternion can be calculated, and the object's horizontal attitude can be calculated based on the attitude quaternion.

[0003] In existing technologies, the horizontal attitude initialization of an object is generally performed under static conditions using acceleration and angular velocity data. However, when the object is in motion, the collected acceleration data includes both motion acceleration and gravitational acceleration. Since motion acceleration affects the calculation of the attitude quaternion, this existing method of horizontal attitude initialization results in significant errors in calculating the horizontal attitude angle of a moving object. This, in turn, affects the calculation of other parameters, such as velocity and position, thus impacting the object's application in motion. Summary of the Invention

[0004] In view of this, the purpose of this application is to provide a method and apparatus for initializing horizontal attitude, so as to reduce the accuracy error in calculating the horizontal attitude angle of an object under motion conditions.

[0005] In a first aspect, embodiments of this application provide a method for initializing a horizontal attitude, the initialization method comprising:

[0006] When the carrier is in motion, acquire the acceleration data, angular velocity data and wheel speed data of the carrier within a predetermined time period.

[0007] For each predetermined sampling time after the initial sampling time within the predetermined time period, based on the acceleration data and angular velocity data from the initial sampling time to the predetermined sampling time, the pre-integral amount of the acceleration from the previous time to the predetermined sampling time and the attitude integral amount from the initial sampling time to the final sampling time are determined.

[0008] The difference between the pre-integral value of the acceleration from the moment before the predetermined sampling time to the predetermined sampling time and the pre-integral value of the wheel speed from the moment before the predetermined sampling time to the predetermined sampling time is determined, and the integral value of the gravitational acceleration of the carrier within the predetermined time period is determined based on the difference value; wherein, the pre-integral value of the wheel speed is determined based on the wheel speed data;

[0009] Based on the integral of the gravitational acceleration of the carrier during the predetermined time period, the initial value of the attitude quaternion at the initial sampling time is corrected to obtain the corrected value of the attitude quaternion at the initial sampling time.

[0010] Based on the attitude quaternion correction value at the initial sampling time and the attitude integral from the initial sampling time to the final sampling time, the attitude quaternion at the final sampling time within the predetermined time period is determined, and the horizontal attitude of the carrier is initialized based on the attitude quaternion at the final sampling time within the predetermined time period.

[0011] Optionally, for each predetermined sampling time after the initial sampling time within the predetermined time period, determining the pre-integral amount of the acceleration from the previous time to the predetermined sampling time and the attitude integral amount from the initial sampling time to the final sampling time, based on the acceleration data and angular velocity data from the initial sampling time to the predetermined sampling time, includes:

[0012] Obtain the initial values ​​of the attitude quaternions at the initial sampling time within the predetermined time period;

[0013] For each predetermined sampling time after the initial sampling time within the predetermined time period, based on the acceleration data, angular velocity data and the initial value of the attitude quaternion from the initial sampling time to the predetermined sampling time, the pre-integral amount of the acceleration from the previous time to the predetermined sampling time is determined.

[0014] For the period from the initial sampling time to the final sampling time within the predetermined time period, the attitude integral from the initial sampling time to the final sampling time is determined based on the angular velocity data from the initial sampling time to the final sampling time.

[0015] Optionally, for each predetermined sampling time after the initial sampling time within the predetermined time period, determining the integral of the acceleration from the previous time to the predetermined sampling time based on the acceleration data, angular velocity data, and the initial value of the attitude quaternion at the initial sampling time at the predetermined sampling time includes:

[0016] For each predetermined sampling time after the initial sampling time within the predetermined time period, the attitude quaternion for that predetermined sampling time is determined based on the angular velocity data from the initial sampling time to that predetermined sampling time and the initial value of the attitude quaternion at the initial sampling time.

[0017] Based on the attitude quaternion at the predetermined sampling time and the acceleration data at the predetermined sampling time, the pre-integral quantity of the acceleration from the previous moment to the predetermined sampling time is determined.

[0018] Optionally, determining the attitude quaternion for each predetermined sampling time after the initial sampling time within the predetermined time period, based on the angular velocity data at that predetermined sampling time and the initial value of the attitude quaternion at the initial sampling time, includes:

[0019] For each predetermined sampling time after the initial sampling time within the predetermined time period, the angular velocity data from the time before the predetermined sampling time to the predetermined sampling time is pre-integrated to obtain the attitude pre-integrated quantity from the time before the predetermined sampling time to the predetermined sampling time.

[0020] Based on the attitude pre-integral value from the moment before each predetermined sampling moment after the initial sampling moment within the predetermined time period to that predetermined sampling moment and the initial value of the attitude quaternion at the initial sampling moment, the attitude quaternion at that predetermined sampling moment is determined.

[0021] Optionally, determining the attitude quaternion for the predetermined sampling time based on the attitude pre-integral value from the time before each predetermined sampling time after the initial sampling time within the predetermined time period and the initial value of the attitude quaternion at the initial sampling time includes:

[0022] Based on the attitude pre-integration from the moment before each predetermined sampling moment after the initial sampling moment within the predetermined time period, the attitude integral from the initial sampling moment to the predetermined sampling moment within the predetermined time period is determined.

[0023] Based on the attitude integral from the initial sampling time to the predetermined sampling time within the predetermined time period and the initial value of the attitude quaternion at the initial sampling time, the attitude quaternion at the predetermined sampling time is determined.

[0024] Optionally, determining the attitude integral from the initial sampling time to the final sampling time based on the angular velocity data from the initial sampling time to the final sampling time within the predetermined time period includes:

[0025] For each predetermined sampling time from the initial sampling time to the final sampling time within the predetermined time period, the angular velocity data from the previous time to the current predetermined sampling time are pre-integrated to obtain the attitude pre-integration from the previous time to the current predetermined sampling time. Based on the attitude pre-integration from the previous time to the current predetermined sampling time for each predetermined sampling time within the predetermined time period, the attitude integral from the initial sampling time to the final sampling time within the predetermined time period is determined.

[0026] Optionally, the step of correcting the initial value of the attitude quaternion at the initial sampling time based on the integral of the gravitational acceleration of the carrier during the predetermined time period, and obtaining the corrected value of the attitude quaternion at the initial sampling time, includes:

[0027] Obtain the gravitational acceleration vector;

[0028] Based on the gravity acceleration vector and the integral of the gravity acceleration of the carrier within the predetermined time period, the attitude quaternion correction value at the initial sampling moment is determined.

[0029] Secondly, embodiments of this application provide a horizontal attitude initialization device, the initialization device comprising:

[0030] The acquisition module is used to acquire the acceleration data, angular velocity data and wheel speed data of the carrier within a predetermined time period when the carrier is in motion.

[0031] The integration module is used to determine, for each predetermined sampling time after the initial sampling time within the predetermined time period, the pre-integral amount of acceleration from the previous moment to the predetermined sampling time and the attitude integral amount from the initial sampling time to the final sampling time, based on the acceleration data and angular velocity data from the initial sampling time to the predetermined sampling time.

[0032] The difference determination module is used to determine the difference between the pre-integral amount of acceleration from the moment before the predetermined sampling time to the predetermined sampling time and the pre-integral amount of wheel speed from the moment before the predetermined sampling time to the predetermined sampling time, and to determine the integral amount of the gravitational acceleration of the carrier within the predetermined time period based on the difference value; wherein, the pre-integral amount of wheel speed is determined based on the wheel speed data;

[0033] The correction module is used to correct the initial value of the attitude quaternion at the initial sampling time based on the integral of the gravitational acceleration of the carrier during the predetermined time period, and to obtain the corrected value of the attitude quaternion at the initial sampling time.

[0034] An initialization module is used to determine the attitude quaternion at the end of the predetermined time period based on the attitude quaternion correction value at the initial sampling time and the attitude integral from the initial sampling time to the end sampling time, and to initialize the horizontal attitude of the carrier based on the attitude quaternion at the end of the predetermined time period.

[0035] Thirdly, embodiments of this application provide an electronic device, including: a processor, a memory, and a bus. The memory stores machine-readable instructions executable by the processor. When the electronic device is running, the processor communicates with the memory via the bus. When the machine-readable instructions are executed by the processor, the steps of the horizontal attitude initialization method described above are performed.

[0036] Fourthly, embodiments of this application provide a computer-readable storage medium storing a computer program that, when executed by a processor, performs the steps of the horizontal attitude initialization method described above.

[0037] This application provides a method and apparatus for initializing horizontal attitude, comprising: acquiring, when a carrier is in motion, time-synchronized acceleration data, angular velocity data, and wheel speed data of the carrier within a predetermined time period; for each predetermined sampling time after the initial sampling time within the predetermined time period, determining, based on the acceleration data and angular velocity data from the initial sampling time to the predetermined sampling time, the pre-integral amount of acceleration from the previous time to the predetermined sampling time and the attitude integral amount from the initial sampling time to the final sampling time; and determining, based on the pre-integral amount of acceleration from the previous time to the predetermined sampling time and the predetermined time, the attitude integral amount; and, based on the pre-integral amount of acceleration from the previous time to the predetermined sampling time and the predetermined time, the attitude integral amount. The difference between the pre-integral value of the wheel speed from the moment before the sampling time to the predetermined sampling time is used to determine the integral value of the gravitational acceleration of the carrier within the predetermined time period. Based on the integral value of the gravitational acceleration of the carrier within the predetermined time period, the initial value of the attitude quaternion at the initial sampling time is corrected to obtain the corrected value of the attitude quaternion at the initial sampling time. Based on the corrected value of the attitude quaternion at the initial sampling time and the attitude integral value from the initial sampling time to the final sampling time, the attitude quaternion at the final sampling time within the predetermined time period is determined. The horizontal attitude of the carrier is initialized based on the attitude quaternion at the final sampling time within the predetermined time period.

[0038] The method for initializing horizontal attitude provided in this application, compared with the prior art which only collects acceleration data and angular velocity data, also collects wheel speed data. In subsequent steps, the difference between the pre-integral value of acceleration and the pre-integral value of wheel speed is used to eliminate motion acceleration that is harmful to the calculation of attitude quaternions. Therefore, the influence of motion acceleration on the calculation of attitude quaternions is avoided, thereby reducing the accuracy error of calculating the horizontal attitude angle of an object under motion conditions, and realizing the initialization of horizontal attitude under motion conditions.

[0039] To make the above-mentioned objectives, features and advantages of this application more apparent and understandable, preferred embodiments are described below in detail with reference to the accompanying drawings. Attached Figure Description

[0040] To more clearly illustrate the technical solutions of the embodiments of this application, the accompanying drawings used in the embodiments will be briefly introduced below. It should be understood that the following drawings only show some embodiments of this application and should not be regarded as a limitation of the scope. For those skilled in the art, other related drawings can be obtained based on these drawings without creative effort.

[0041] Figure 1 A flowchart illustrating an exemplary embodiment of this application for initializing a horizontal attitude is shown.

[0042] Figure 2 A schematic diagram of the structure of a horizontal attitude initialization device provided in an exemplary embodiment of this application is shown;

[0043] Figure 3 A schematic diagram of the structure of an electronic device provided by an exemplary embodiment of this application is shown. Detailed Implementation

[0044] To make the objectives, technical solutions, and advantages of the embodiments of this application clearer, the technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, and not all embodiments. The components of the embodiments of this application described and shown in the accompanying drawings can generally be arranged and designed in various different configurations. Therefore, the following detailed description of the embodiments of this application provided in the accompanying drawings is not intended to limit the scope of the claimed application, but merely represents selected embodiments of this application. Based on the embodiments of this application, every other embodiment obtained by those skilled in the art without inventive effort falls within the scope of protection of this application.

[0045] In existing technologies, the horizontal attitude initialization of an object is generally performed under static conditions using acceleration and angular velocity data. However, when the object is in motion, the collected acceleration data includes both motion acceleration and gravitational acceleration. Since motion acceleration affects the calculation of the attitude quaternion, this existing method of horizontal attitude initialization results in significant errors in calculating the horizontal attitude angle of a moving object. This, in turn, affects the calculation of other parameters, such as velocity and position, thus impacting the object's application in motion.

[0046] Based on this, the horizontal attitude initialization method provided in this application, compared with the prior art which only collects acceleration data and angular velocity data, also acquires wheel speed data, and in subsequent steps, eliminates motion acceleration that is harmful to the calculation of attitude quaternions by using the difference between the pre-integral value of acceleration and the pre-integral value of wheel speed. Therefore, the influence of motion acceleration on the calculation of attitude quaternions is avoided, thereby reducing the accuracy error of horizontal attitude angle calculation for objects under motion conditions, and realizing horizontal attitude initialization under motion conditions.

[0047] Please see Figure 1 , Figure 1 A flowchart of a horizontal attitude initialization method provided in an embodiment of this application is shown.

[0048] like Figure 1 As shown in the figure, an embodiment of this application provides a method for initializing a horizontal attitude, which includes the following steps:

[0049] S100. When the carrier is in motion, acquire the acceleration data, angular velocity data and wheel speed data of the carrier within a predetermined time period.

[0050] As an example, a carrier is a vehicle that can move by means of wheel-shaped rotating parts, such as a car, truck, electric vehicle, or robot with wheels.

[0051] As an example, motion conditions can include smooth motion conditions and non-smooth motion conditions. For instance, smooth motion conditions can include the vehicle traveling on a flat asphalt road, while non-smooth motion conditions can include the vehicle traveling on uneven surfaces such as slopes or gravel roads.

[0052] The predetermined time period is set in advance based on the actual situation. In practical applications, the length of the predetermined time period can be set differently depending on the motion condition of the carrier. For example, the length of the predetermined time period when the carrier is in a smooth motion condition can be shorter than the length of the predetermined time period when the carrier is in a non-smooth motion condition. With a fixed sampling frequency, setting a longer predetermined time period, i.e., a longer sampling time, allows for the acquisition of more sampling data. Thus, compared to a smooth motion condition, when the carrier is in a non-smooth motion condition, a longer predetermined time period allows for the acquisition of more acceleration, angular velocity, and wheel speed data, which helps reduce errors in subsequent calculations.

[0053] As an example, the carrier can be equipped with an IMU sensor and a wheel speed meter. The IMU sensor collects the carrier's acceleration and angular velocity data, and the wheel speed meter collects the carrier's wheel speed data. Alternatively, the carrier can be equipped with an accelerometer, a gyroscope, and a wheel speed meter. The accelerometer collects the carrier's acceleration data, the gyroscope collects the carrier's angular velocity data, and the wheel speed meter collects the carrier's wheel speed data.

[0054] Here, the acceleration, angular velocity, and wheel speed data can be data in the carrier coordinate system. The carrier coordinate system is fixed to the carrier and moves with it; its origin coincides with the carrier's center of mass. The orientation of the carrier coordinate system can be defined according to the actual situation; for example, it can be defined as a right-front-upper coordinate system.

[0055] Here, acceleration data includes motion acceleration and gravitational acceleration.

[0056] Here, acceleration data, angular velocity data, and wheel speed data all include time, and time-synchronized acceleration data, angular velocity data, and wheel speed data are obtained through time synchronization.

[0057] S200. For each predetermined sampling time after the initial sampling time within the predetermined time period, based on the acceleration data and angular velocity data from the initial sampling time to the predetermined sampling time, determine the pre-integral amount of the acceleration from the previous time to the predetermined sampling time and the attitude integral amount from the initial sampling time to the final sampling time.

[0058] Understandably, acceleration data, angular velocity data, and wheel speed data all include time. Within a predetermined time period, the time of the first synchronization is the initial sampling time, the time of any subsequent synchronization is the predetermined sampling time, and the last predetermined sampling time is the termination sampling time.

[0059] As an example, step S200 may include:

[0060] S210. Obtain the initial value of the attitude quaternion at the initial sampling time within the predetermined time period;

[0061] Here, the initial values ​​of the attitude quaternions can be the initial values ​​of the attitude quaternions converted from the carrier coordinate system to the local navigation coordinate system. As an example, the direction of the local navigation coordinate system can be defined according to the actual situation; for example, the local navigation coordinate system can be defined as the Northeast-Sky coordinate system.

[0062] Here, the initial values ​​of the attitude quaternions can be predefined according to the actual situation. For example, the initial values ​​of the attitude quaternions... It can be: [1.0,0.0,0.0,0.0], where b represents the carrier coordinate system, n represents the local navigation coordinate system, and 0 represents the initial sampling time.

[0063] S220. For each predetermined sampling time after the initial sampling time within the predetermined time period, based on the acceleration data, angular velocity data and the initial value of the attitude quaternion from the initial sampling time to the predetermined sampling time, determine the pre-integral amount of the acceleration from the previous time to the predetermined sampling time.

[0064] As an example, step S220 may include the following steps:

[0065] S221. For each predetermined sampling time after the initial sampling time within the predetermined time period, determine the attitude quaternion for the predetermined sampling time based on the angular velocity data from the initial sampling time to the predetermined sampling time and the initial value of the attitude quaternion at the initial sampling time.

[0066] As an example, step S221 may include the following steps:

[0067] S2211. For each predetermined sampling time after the initial sampling time within the predetermined time period, pre-integrate the angular velocity data from the time before the predetermined sampling time to the predetermined sampling time to obtain the attitude pre-integration quantity from the time before the predetermined sampling time to the predetermined sampling time.

[0068] For example, the attitude pre-integral from the moment before the predetermined sampling time to the predetermined sampling time can be obtained using the following formula:

[0069]

[0070] in, W represents the attitude pre-integral quantity from the (i-1)th predetermined sampling time to the ith predetermined sampling time. iLet dt be the angular velocity data at the i-th predetermined sampling time, and dt be the time interval from the (i-1)-th predetermined sampling time to the i-th predetermined sampling time, where 1 ≤ i ≤ k, and k is the end sampling time of the predetermined time interval.

[0071] S2212. Based on the attitude pre-integral quantity from the moment before each predetermined sampling moment after the initial sampling moment within the predetermined time period to the predetermined sampling moment and the initial value of the attitude quaternion at the initial sampling moment, determine the attitude quaternion at the predetermined sampling moment.

[0072] As an example, step S2212 may include the following steps:

[0073] First, based on the attitude pre-integration from the moment before each predetermined sampling moment after the initial sampling moment within the predetermined time period, the attitude integral from the initial sampling moment to the predetermined sampling moment within the predetermined time period is determined.

[0074] For example, the attitude integral from the initial sampling time to the predetermined sampling time within the predetermined time period can be determined using the following formula:

[0075]

[0076] in, The attitude integral from the initial sampling time to the i-th predetermined sampling time within the predetermined time period is given. The attitude pre-integration is the distance from the (i-1)th predetermined sampling time to the ith predetermined sampling time.

[0077] Furthermore, for example, the attitude integral from the initial sampling time to the third predetermined sampling time within a predetermined time period is:

[0078]

[0079] Then, based on the attitude integral from the initial sampling time to the predetermined sampling time within the predetermined time period and the initial value of the attitude quaternion at the initial sampling time, the attitude quaternion at the predetermined sampling time is determined.

[0080] For example, the attitude quaternion at the predetermined sampling time can be determined using the following formula:

[0081]

[0082] in, Let be the attitude quaternion at the i-th predetermined sampling time. The initial value of the attitude quaternion at the initial sampling time is given. The attitude integral from the initial sampling time to the i-th predetermined sampling time within the predetermined time period.

[0083] Here, when the acquired acceleration, angular velocity, and wheel speed data are in the vehicle coordinate system, and the initial attitude quaternion value at the initial sampling time is the initial attitude quaternion value converted from the vehicle coordinate system to the local navigation coordinate system, the attitude quaternion determined in this step at the predetermined sampling time... It can be an attitude quaternion that is converted from the carrier coordinate system to the navigation coordinate system.

[0084] S222. Based on the attitude quaternion at the predetermined sampling time and the acceleration data at the predetermined sampling time, determine the pre-integral amount of the acceleration from the previous moment to the predetermined sampling time.

[0085] For example, the pre-integral of the acceleration from the moment before the predetermined sampling time to the predetermined sampling time can be determined using the following formula:

[0086]

[0087] in, The pre-integral value of the acceleration from the (i-1)th predetermined sampling time to the ith predetermined sampling time is... Let a be the attitude quaternion at the i-th predetermined sampling time. i Let dt be the acceleration data at the i-th predetermined sampling time, and dt be the time interval between the (i-1)-th predetermined sampling time and the i-th predetermined sampling time.

[0088] S230. For the period from the initial sampling time to the final sampling time within the predetermined time period, determine the attitude integral from the initial sampling time to the final sampling time based on the angular velocity data from the initial sampling time to the final sampling time.

[0089] As an example, step S230 may include the following steps:

[0090] S231. For each predetermined sampling time from the initial sampling time to the final sampling time within the predetermined time period, pre-integrate the angular velocity data from the time before each predetermined sampling time to the predetermined sampling time to obtain the attitude pre-integration quantity from the time before each predetermined sampling time to the predetermined sampling time.

[0091] S232. Based on the attitude pre-integration from the moment before each predetermined sampling moment to the predetermined sampling moment within the predetermined time period, determine the attitude integration from the initial sampling moment to the final sampling moment within the predetermined time period.

[0092] For example, the attitude integral from the initial sampling time to the final sampling time within the predetermined time period can be determined using the following formula:

[0093]

[0094] in, The attitude integral from the initial sampling time to the final sampling time within the predetermined time period. The attitude pre-integration is the distance from the (i-1)th predetermined sampling time to the ith predetermined sampling time.

[0095] It is understood that in step S220 above, when determining the acceleration pre-integral amount from the moment before the predetermined sampling time to the predetermined sampling time, it is necessary to first determine the attitude integral amount from the initial sampling time to the predetermined sampling time within the predetermined time period. The predetermined sampling time includes the termination sampling time. Thus, when determining the acceleration pre-integral amount from the moment before the termination sampling time to the termination sampling time through step S220, the attitude integral amount from the initial sampling time to the termination sampling time has already been determined. Therefore, step S230 can be omitted. When executing step S2212, when the predetermined sampling time is the termination sampling time, the attitude integral amount from the initial sampling time to the termination sampling time within the predetermined time period can be determined.

[0096] It can be understood that the pre-integral value of the acceleration from the moment preceding the predetermined sampling time to the predetermined sampling time includes the pre-integral value of the motion acceleration from the moment preceding the predetermined sampling time to the predetermined sampling time and the pre-integral value of the gravitational acceleration from the moment preceding the predetermined sampling time to the predetermined sampling time. Since motion acceleration is detrimental to the calculation of horizontal attitude, the pre-integral value of motion acceleration is also detrimental to the calculation of horizontal attitude and needs to be discarded; only the pre-integral value of gravitational acceleration needs to be retained.

[0097] S300, determine the difference between the pre-integral amount of acceleration from the moment before the predetermined sampling time to the predetermined sampling time and the pre-integral amount of wheel speed from the moment before the predetermined sampling time to the predetermined sampling time, and determine the integral amount of gravitational acceleration of the carrier within the predetermined time period based on the difference value;

[0098] The wheel speed pre-integral value is determined based on the wheel speed data. As an example, for each predetermined sampling time, the difference between the wheel speed data at the predetermined sampling time and the wheel speed data at the previous predetermined sampling time can be determined as the wheel speed pre-integral value from the time before the predetermined sampling time to the current predetermined sampling time.

[0099]

[0100] in, The pre-integral value of the wheel velocity from the (i-1)th sampling time to the ith sampling time; velData i This represents the wheel speed data at the i-th sampling time; velData i-1 This represents the wheel speed data at the (i-1)th sampling time.

[0101] Furthermore, the integral of the gravitational acceleration of the carrier during the predetermined time period is determined based on the difference value. For example, the integral of the gravitational acceleration of the carrier during the predetermined time period can be determined using the following formula:

[0102]

[0103] in, It is the integral of the gravitational acceleration of the carrier during the predetermined time period; The pre-integral value of the wheel speed from the (i-1)th sampling time to the ith sampling time; It is the pre-integral value of the acceleration from the (i-1)th sampling time to the ith sampling time.

[0104] Here, the acceleration data, angular velocity data, and wheel speed data obtained are data in the carrier coordinate system. The integral of the gravitational acceleration of the carrier during the predetermined time period is determined in this step. It can be an integral quantity in the carrier coordinate system.

[0105] Here, the magnitude of the pre-integral of the wheel speed from the moment before the predetermined sampling time to the predetermined sampling time is basically the same as the magnitude of the pre-integral of the motion acceleration from the moment before the predetermined sampling time to the predetermined sampling time. Therefore, the pre-integral of the motion acceleration can be eliminated by the difference between the pre-integral of the acceleration and the pre-integral of the wheel speed, while retaining the pre-integral of the gravitational acceleration. Furthermore, the integral of the gravitational acceleration can be obtained by using the pre-integral of the gravitational acceleration.

[0106] S400. Based on the integral of the gravitational acceleration of the carrier during the predetermined time period, correct the initial value of the attitude quaternion at the initial sampling time to obtain the corrected value of the attitude quaternion at the initial sampling time.

[0107] As an example, step S400 may include the following steps:

[0108] S410, Obtain the gravitational acceleration vector;

[0109] Here, the gravitational acceleration vector g n It can be [0,0,g], which is the gravitational acceleration vector in the navigation coordinate system; where g is the gravitational acceleration value.

[0110] S420. Based on the gravity acceleration vector and the integral of the gravity acceleration of the carrier within the predetermined time period, determine the attitude quaternion correction value at the initial sampling moment.

[0111] For example, the attitude quaternion correction value at the initial sampling time can be determined using the following formula:

[0112]

[0113] in, The attitude quaternion correction value at the initial sampling time. g is the integral of the gravitational acceleration of the carrier during the predetermined time period. n This is the vector of gravitational acceleration.

[0114] S500. Based on the attitude quaternion correction value at the initial sampling time and the attitude integral from the initial sampling time to the termination sampling time, determine the attitude quaternion at the termination sampling time within the predetermined time period, and initialize the horizontal attitude of the carrier based on the attitude quaternion at the termination sampling time within the predetermined time period.

[0115] Based on the attitude quaternion correction value at the initial sampling time and the attitude integral from the initial sampling time to the final sampling time within the predetermined time period, the attitude quaternion at the final sampling time within the predetermined time period is determined.

[0116] For example, the attitude quaternion at the end of the predetermined sampling time within the predetermined time period can be determined using the following formula:

[0117]

[0118] in, The attitude quaternion at the end of the sampling time within the predetermined time period; The attitude quaternion correction value at the initial sampling time; The attitude integral from the initial sampling time to the final sampling time within the predetermined time period.

[0119] Here, the steps for initializing the horizontal attitude of the carrier based on the attitude quaternion at the end sampling time within the predetermined time period are existing technologies and will not be described in detail here.

[0120] The method for initializing horizontal attitude provided in this application, compared with the prior art which only collects acceleration data and angular velocity data, also collects wheel speed data. In subsequent steps, the difference between the pre-integral value of acceleration and the pre-integral value of wheel speed is used to eliminate motion acceleration that is harmful to the calculation of attitude quaternions. Therefore, the influence of motion acceleration on the calculation of attitude quaternions is avoided, thereby reducing the accuracy error of calculating the horizontal attitude angle of an object under motion conditions, and realizing the initialization of horizontal attitude under motion conditions.

[0121] Based on the same inventive concept, this application also provides a horizontal attitude initialization device corresponding to the above-mentioned horizontal attitude initialization method. Since the principle of the device in this application is similar to that of the method in this application, the implementation of the device can refer to the implementation of the method, and the repeated parts will not be described again.

[0122] Figure 2 A schematic diagram of the structure of a horizontal attitude initialization device provided in an exemplary embodiment of this application is shown.

[0123] like Figure 2 As shown, the initialization device 200 includes:

[0124] The acquisition module 210 is used to acquire the acceleration data, angular velocity data and wheel speed data of the carrier within a predetermined time period when the carrier is in motion.

[0125] The integration module 220 is used to determine, for each predetermined sampling time after the initial sampling time within the predetermined time period, the pre-integral amount of acceleration from the previous moment to the predetermined sampling time and the attitude integral amount from the initial sampling time to the final sampling time, based on the acceleration data and angular velocity data from the initial sampling time to the predetermined sampling time.

[0126] The difference determination module 230 is used to determine the difference between the pre-integral amount of acceleration from the moment before the predetermined sampling time to the predetermined sampling time and the pre-integral amount of wheel speed from the moment before the predetermined sampling time to the predetermined sampling time, and to determine the integral amount of the gravitational acceleration of the carrier within the predetermined time period based on the difference value; wherein, the pre-integral amount of wheel speed is determined based on the wheel speed data;

[0127] The correction module 240 is used to correct the initial value of the attitude quaternion at the initial sampling time based on the integral of the gravitational acceleration of the carrier during the predetermined time period, and to obtain the corrected value of the attitude quaternion at the initial sampling time.

[0128] The initialization module 250 is used to determine the attitude quaternion at the end of the predetermined time period based on the attitude quaternion correction value at the initial sampling time and the attitude integral from the initial sampling time to the end sampling time, and to initialize the horizontal attitude of the carrier based on the attitude quaternion at the end of the predetermined time period.

[0129] Optionally, the integration module 220 is specifically used for:

[0130] Obtain the initial values ​​of the attitude quaternions at the initial sampling time within the predetermined time period;

[0131] For each predetermined sampling time after the initial sampling time within the predetermined time period, based on the acceleration data, angular velocity data and the initial value of the attitude quaternion from the initial sampling time to the predetermined sampling time, the pre-integral amount of the acceleration from the previous time to the predetermined sampling time is determined.

[0132] For the period from the initial sampling time to the final sampling time within the predetermined time period, the attitude integral from the initial sampling time to the final sampling time is determined based on the angular velocity data from the initial sampling time to the final sampling time.

[0133] Optionally, the integration module 220 is specifically used for:

[0134] For each predetermined sampling time after the initial sampling time within the predetermined time period, the attitude quaternion for that predetermined sampling time is determined based on the angular velocity data from the initial sampling time to that predetermined sampling time and the initial value of the attitude quaternion at the initial sampling time.

[0135] Based on the attitude quaternion at the predetermined sampling time and the acceleration data at the predetermined sampling time, the pre-integral quantity of the acceleration from the previous moment to the predetermined sampling time is determined.

[0136] Optionally, the integration module 220 is specifically used for:

[0137] For each predetermined sampling time after the initial sampling time within the predetermined time period, the angular velocity data from the time before the predetermined sampling time to the predetermined sampling time is pre-integrated to obtain the attitude pre-integrated quantity from the time before the predetermined sampling time to the predetermined sampling time.

[0138] Based on the attitude pre-integral value from the moment before each predetermined sampling moment after the initial sampling moment within the predetermined time period to that predetermined sampling moment and the initial value of the attitude quaternion at the initial sampling moment, the attitude quaternion at that predetermined sampling moment is determined.

[0139] Optionally, the integration module 220 is specifically used for:

[0140] Based on the attitude pre-integration from the moment before each predetermined sampling moment after the initial sampling moment within the predetermined time period, the attitude integral from the initial sampling moment to the predetermined sampling moment within the predetermined time period is determined.

[0141] Based on the attitude integral from the initial sampling time to the predetermined sampling time within the predetermined time period and the initial value of the attitude quaternion at the initial sampling time, the attitude quaternion at the predetermined sampling time is determined.

[0142] Optionally, the integration module 220 is specifically used for:

[0143] For each predetermined sampling time from the initial sampling time to the final sampling time within the predetermined time period, the angular velocity data from the previous time to the current predetermined sampling time are pre-integrated to obtain the attitude pre-integration from the previous time to the current predetermined sampling time. Based on the attitude pre-integration from the previous time to the current predetermined sampling time for each predetermined sampling time within the predetermined time period, the attitude integral from the initial sampling time to the final sampling time within the predetermined time period is determined.

[0144] Optionally, the correction module 240 is specifically used for:

[0145] Obtain the gravitational acceleration vector;

[0146] Based on the gravity acceleration vector and the integral of the gravity acceleration of the carrier within the predetermined time period, the attitude quaternion correction value at the initial sampling moment is determined.

[0147] The horizontal attitude initialization device provided in this application, compared with the prior art which only collects acceleration data and angular velocity data, also collects wheel speed data. In subsequent steps, the difference between the pre-integral value of acceleration and the pre-integral value of wheel speed is used to eliminate motion acceleration that is harmful to the calculation of attitude quaternions. Therefore, the influence of motion acceleration on the calculation of attitude quaternions is avoided, thereby reducing the accuracy error of horizontal attitude angle calculation for objects under motion conditions, and realizing horizontal attitude initialization under motion conditions.

[0148] Please see Figure 3 , Figure 3 This is a schematic diagram of the structure of an electronic device provided in an embodiment of this application. Figure 3 As shown, the electronic device 300 includes a processor 310, a memory 320, and a bus 330.

[0149] The memory 320 stores machine-readable instructions that can be executed by the processor 310. When the electronic device 300 is running, the processor 310 and the memory 320 communicate via the bus 330. When the machine-readable instructions are executed by the processor 310, the steps of the horizontal attitude initialization method as described in the above method embodiment can be executed. For specific implementation details, please refer to the method embodiment, which will not be repeated here.

[0150] This application also provides a computer-readable storage medium storing a computer program. When the computer program is run by a processor, it can execute the steps of the horizontal attitude initialization method as described in the above method embodiments. For specific implementation details, please refer to the method embodiments, which will not be repeated here.

[0151] Those skilled in the art will understand that, for the sake of convenience and brevity, the specific working processes of the systems, devices, and units described above can be referred to the corresponding processes in the foregoing method embodiments, and will not be repeated here.

[0152] In the several embodiments provided in this application, it should be understood that the disclosed systems, apparatuses, and methods can be implemented in other ways. The apparatus embodiments described above are merely illustrative. For example, the division of units is only a logical functional division, and in actual implementation, there may be other division methods. Furthermore, multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed. Additionally, the shown or discussed mutual couplings, direct couplings, or communication connections may be through some communication interfaces; indirect couplings or communication connections between devices or units may be electrical, mechanical, or other forms.

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

[0154] In addition, the functional units in the various embodiments of this application can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit.

[0155] If the aforementioned functions are implemented as software functional units and sold or used as independent products, they can be stored in a processor-executable, non-volatile, computer-readable storage medium. Based on this understanding, the technical solution of this application, in essence, or the part that contributes to the prior art, or a portion of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of this application. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0156] Finally, it should be noted that the above-described embodiments are merely specific implementations of this application, used to illustrate the technical solutions of this application, and not to limit them. The scope of protection of this application is not limited thereto. Although this application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that any person skilled in the art can still modify or easily conceive of changes to the technical solutions described in the foregoing embodiments, or make equivalent substitutions for some of the technical features, within the scope of the technology disclosed in this application. Such modifications, changes, or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of this application, and should all be covered within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.

Claims

1. A method for initializing horizontal attitude, characterized in that, The initialization method includes: When the carrier is in motion, acquire the acceleration data, angular velocity data and wheel speed data of the carrier within a predetermined time period. For each predetermined sampling time after the initial sampling time within the predetermined time period, based on the acceleration data and angular velocity data from the initial sampling time to the predetermined sampling time, the pre-integral amount of the acceleration from the previous time to the predetermined sampling time and the attitude integral amount from the initial sampling time to the final sampling time are determined. The difference between the pre-integral value of the acceleration from the moment before the predetermined sampling time to the predetermined sampling time and the pre-integral value of the wheel speed from the moment before the predetermined sampling time to the predetermined sampling time is determined, and the integral value of the gravitational acceleration of the carrier within the predetermined time period is determined based on the difference value; wherein, the pre-integral value of the wheel speed is determined based on the wheel speed data; Based on the integral of the gravitational acceleration of the carrier during the predetermined time period, the initial value of the attitude quaternion at the initial sampling time is corrected to obtain the corrected value of the attitude quaternion at the initial sampling time. Based on the attitude quaternion correction value at the initial sampling time and the attitude integral from the initial sampling time to the final sampling time, the attitude quaternion at the final sampling time within the predetermined time period is determined, and the horizontal attitude of the carrier is initialized based on the attitude quaternion at the final sampling time within the predetermined time period.

2. The initialization method according to claim 1, characterized in that, For each predetermined sampling time following the initial sampling time within the predetermined time period, based on the acceleration and angular velocity data from the initial sampling time to that predetermined sampling time, the pre-integral amount of the acceleration from the previous time to that predetermined sampling time and the attitude integral amount from the initial sampling time to the final sampling time are determined, including: Obtain the initial values ​​of the attitude quaternions at the initial sampling time within the predetermined time period; For each predetermined sampling time after the initial sampling time within the predetermined time period, based on the acceleration data, angular velocity data and the initial value of the attitude quaternion from the initial sampling time to the predetermined sampling time, the pre-integral amount of the acceleration from the previous time to the predetermined sampling time is determined. For the period from the initial sampling time to the final sampling time within the predetermined time period, the attitude integral from the initial sampling time to the final sampling time is determined based on the angular velocity data from the initial sampling time to the final sampling time.

3. The initialization method according to claim 2, characterized in that, For each predetermined sampling time following the initial sampling time within the predetermined time period, based on the acceleration data, angular velocity data, and the initial value of the attitude quaternion at the initial sampling time, the pre-integral value of the acceleration from the previous time to the predetermined sampling time is determined, including: For each predetermined sampling time after the initial sampling time within the predetermined time period, the attitude quaternion for that predetermined sampling time is determined based on the angular velocity data from the initial sampling time to that predetermined sampling time and the initial value of the attitude quaternion at the initial sampling time. Based on the attitude quaternion at the predetermined sampling time and the acceleration data at the predetermined sampling time, the pre-integral quantity of the acceleration from the previous moment to the predetermined sampling time is determined.

4. The initialization method according to claim 3, characterized in that, For each predetermined sampling time following the initial sampling time within the predetermined time period, the attitude quaternion for that predetermined sampling time is determined based on the angular velocity data from the initial sampling time to that predetermined sampling time and the initial value of the attitude quaternion at the initial sampling time, including: For each predetermined sampling time after the initial sampling time within the predetermined time period, the angular velocity data from the time before the predetermined sampling time to the predetermined sampling time is pre-integrated to obtain the attitude pre-integrated quantity from the time before the predetermined sampling time to the predetermined sampling time. Based on the attitude pre-integral value from the moment before each predetermined sampling moment after the initial sampling moment within the predetermined time period to that predetermined sampling moment and the initial value of the attitude quaternion at the initial sampling moment, the attitude quaternion at the predetermined sampling moment is determined.

5. The initialization method according to claim 4, characterized in that, The determination of the attitude quaternion at the predetermined sampling time based on the attitude pre-integral value from the time before each predetermined sampling time after the initial sampling time within the predetermined time period and the initial value of the attitude quaternion at the initial sampling time includes: Based on the attitude pre-integration from the moment before each predetermined sampling moment after the initial sampling moment within the predetermined time period, the attitude integral from the initial sampling moment to the predetermined sampling moment within the predetermined time period is determined. Based on the attitude integral from the initial sampling time to the predetermined sampling time within the predetermined time period and the initial value of the attitude quaternion at the initial sampling time, the attitude quaternion at the predetermined sampling time is determined.

6. The initialization method according to claim 2, characterized in that, The step of determining the attitude integral from the initial sampling time to the final sampling time within the predetermined time period, based on the angular velocity data from the initial sampling time to the final sampling time, includes: For each predetermined sampling time from the initial sampling time to the final sampling time within the predetermined time period, the angular velocity data from the previous time to the current predetermined sampling time are pre-integrated to obtain the attitude pre-integration from the previous time to the current predetermined sampling time. Based on the attitude pre-integration from the previous time to the current predetermined sampling time for each predetermined sampling time within the predetermined time period, the attitude integral from the initial sampling time to the final sampling time within the predetermined time period is determined.

7. The initialization method according to claim 1, characterized in that, The step of correcting the initial value of the attitude quaternion at the initial sampling time based on the integral of the gravitational acceleration of the carrier during the predetermined time period, and obtaining the corrected value of the attitude quaternion at the initial sampling time, includes: Obtain the gravitational acceleration vector; Based on the gravity acceleration vector and the integral of the gravity acceleration of the carrier within the predetermined time period, the attitude quaternion correction value at the initial sampling moment is determined.

8. A horizontal attitude initialization device, characterized in that, The initialization device includes: The acquisition module is used to acquire the acceleration data, angular velocity data and wheel speed data of the carrier within a predetermined time period when the carrier is in motion. The integration module is used to determine, for each predetermined sampling time after the initial sampling time within the predetermined time period, the pre-integral amount of acceleration from the previous moment to the predetermined sampling time and the attitude integral amount from the initial sampling time to the final sampling time, based on the acceleration data and angular velocity data from the initial sampling time to the predetermined sampling time. The difference determination module is used to determine the difference between the pre-integral amount of acceleration from the moment before the predetermined sampling time to the predetermined sampling time and the pre-integral amount of wheel speed from the moment before the predetermined sampling time to the predetermined sampling time, and to determine the integral amount of the gravitational acceleration of the carrier within the predetermined time period based on the difference value; wherein, the pre-integral amount of wheel speed is determined based on the wheel speed data; The correction module is used to correct the initial value of the attitude quaternion at the initial sampling time based on the integral of the gravitational acceleration of the carrier during the predetermined time period, and to obtain the corrected value of the attitude quaternion at the initial sampling time. An initialization module is used to determine the attitude quaternion at the end of the predetermined time period based on the attitude quaternion correction value at the initial sampling time and the attitude integral from the initial sampling time to the end sampling time, and to initialize the horizontal attitude of the carrier based on the attitude quaternion at the end of the predetermined time period.

9. An electronic device, characterized in that, include: The device includes a processor, a memory, and a bus. The memory stores machine-readable instructions executable by the processor. When the electronic device is running, the processor communicates with the memory via the bus. The machine-readable instructions are executed by the processor to perform the steps of the horizontal attitude initialization method as described in any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a computer program that, when executed by a processor, performs the steps of the horizontal attitude initialization method as described in any one of claims 1 to 7.

Images