Posture estimation method and device, electronic equipment and computer readable storage medium

Abstract

Embodiments of the present application disclose a pose estimation method and device, electronic equipment and a computer readable storage medium. The method comprises determining current pose information corresponding to a current frame, and after initialization of a stable magnetic field is completed, if current magnetic field intensity information of the current frame and reference magnetic field intensity information of the stable magnetic field satisfy a first preset condition, then a reference frame is selected from a preset frame set corresponding to the stable magnetic field according to the current pose information, wherein reference pose information corresponding to the reference frame and the current pose information satisfy a second preset condition, and target pose information corresponding to the current pose information is determined according to the reference frame and the current magnetic field intensity information. The accuracy of pose information estimation is improved by selecting a reference frame and optimizing the pose information in combination with the reference frame. The accuracy of pose information estimation is further improved by selecting a reference frame and optimizing the pose information in combination with the reference frame when the stable magnetic field is used.

Description

TECHNICAL FIELD

[0001] Embodiments of the present application relate to the technical field of extended reality, and in particular to a pose estimation method and device, an electronic device, and a computer readable storage medium. BACKGROUND

[0002] Extended reality technology refers to directly superimposing virtual content (text, pictures, etc.) in a real field of view to create a natural and real, fully immersive interactive experience for users through an extended reality device.

[0003] For an extended reality device, accurate 3DoF pose estimation can ensure that virtual content is presented in the correct direction and angle in the user's field of view, seamlessly integrated with the real environment, thereby improving the user's sense of immersion and naturalness of interaction.

[0004] Currently, in order to improve the accuracy of 3DoF pose estimation, a combination of a gyroscope, an accelerometer, and a magnetometer is usually used to estimate the pose. However, when using a gyroscope, an accelerometer, and a magnetometer to estimate the pose, it is susceptible to environmental factors, for example, in the use scenario of the magnetometer, external magnetic field interference is likely to exist, resulting in inaccurate pose estimation. SUMMARY

[0005] Embodiments of the present application provide a pose estimation method, device, electronic device, and computer readable storage medium, which can improve the accuracy of pose estimation.

[0006] In a first aspect, embodiments of the present application provide a pose estimation method, which comprises:

[0007] determining current pose information corresponding to a current frame;

[0008] after initialization of the stable magnetic field is completed, if the current magnetic field intensity information of the current frame and the reference magnetic field intensity information of the stable magnetic field satisfy a first preset condition, then a reference frame is selected from a preset frame set corresponding to the stable magnetic field according to the current pose information, wherein reference pose information corresponding to the reference frame and the current pose information satisfy a second preset condition;

[0009] determining target pose information corresponding to the current pose information according to the reference frame and the current magnetic field intensity information.

[0010] In a second aspect, embodiments of the present application also provide a pose estimation device, which comprises:

[0011] a first determination module configured to determine current pose information corresponding to a current frame;

[0012] The screening module is configured to, after the initialization of the stable magnetic field is completed, if the current magnetic field intensity information of the current frame and the reference magnetic field intensity information of the stable magnetic field satisfy a first preset condition, screen a reference frame from a preset frame set corresponding to the stable magnetic field according to the current attitude information, wherein reference attitude information corresponding to the reference frame satisfies a second preset condition with the current attitude information.

[0013] The second determination module is configured to determine target attitude information corresponding to the current attitude information according to the reference frame and the current magnetic field intensity information.

[0014] Optionally, in some embodiments of the present application, the determination of the target attitude information corresponding to the current attitude information according to the reference frame and the current magnetic field intensity information comprises:

[0015] determination of reference state estimation information and a reference covariance matrix corresponding to the reference frame, wherein the reference state estimation information comprises a posterior state estimation mean for the reference frame;

[0016] determination of target state estimation information of the current frame according to the reference state estimation information, the reference covariance matrix and the current magnetic field intensity information;

[0017] resolution of the target attitude information from the target state estimation information.

[0018] Optionally, in some embodiments of the present application, the determination of the target state estimation information of the current frame according to the reference state estimation information, the reference covariance matrix and the current magnetic field intensity information comprises:

[0019] determination of a joint magnetic field intensity measurement model, wherein the joint magnetic field intensity measurement model is constructed according to the current frame and the reference frame;

[0020] determination of residual information for magnetic field noise according to the joint magnetic field intensity measurement model and the current magnetic field intensity information;

[0021] determination of target gain information according to the reference state estimation information and the reference covariance matrix;

[0022] determination of the target state estimation information according to the residual information and the target gain information.

[0023] Optionally, in some embodiments of the present application, the current attitude information comprises roll angle information, pitch angle information and heading angle information.

[0024] The determination of the target gain information according to the reference state estimation information and the reference covariance matrix comprises:

[0025] obtain transition state estimation information and a transition covariance matrix, the transition state estimation information being posterior estimation information of a pose state of the current frame for the roll angle information and the pitch angle information;

[0026] determine the target gain information according to the transition state estimation information, the transition covariance matrix, the reference state estimation information and the reference covariance matrix;

[0027] determine the target state estimation information according to the residual information and the target gain information, including:

[0028] determine state offset information according to the residual information and the target gain information;

[0029] update the transition state estimation information according to the state offset information to obtain the target state estimation information.

[0030] Optionally, in some embodiments of the present application, the obtaining of the transition state estimation information and the transition covariance matrix includes:

[0031] perform integral prediction processing according to angular velocity information corresponding to the current frame to obtain initial state estimation information and an initial covariance matrix;

[0032] determine a second covariance matrix for target noise according to an acceleration measurement model;

[0033] determine the transition state estimation information and the transition covariance matrix according to the initial state estimation information, the initial covariance matrix, the second covariance matrix and acceleration information corresponding to the current frame.

[0034] Optionally, in some embodiments of the present application, the determining of the target gain information according to the transition state estimation information, the transition covariance matrix, the reference state estimation information and the reference covariance matrix includes:

[0035] construct a reference Jacobian matrix of the residual information for the reference frame according to the transition state estimation information and the reference state estimation information, and construct a transition Jacobian matrix of the residual information for the current frame according to the transition state estimation information and the reference state estimation information, wherein the reference Jacobian matrix and the transition Jacobian matrix are both for the heading angle information;

[0036] determine the target gain information according to the reference Jacobian matrix, the transition Jacobian matrix, the transition covariance matrix and the reference covariance matrix.

[0037] Optionally, in some embodiments of the present application, the filtering the reference frame from the preset frame set corresponding to the stable magnetic field according to the current attitude information comprises:

[0038] determining a 3D grid corresponding to the stable magnetic field, each voxel in the 3D grid corresponding to reference attitude information of an attitude frame respectively;

[0039] filtering a reference voxel from the voxels according to the current attitude information and each reference attitude information;

[0040] setting the attitude frame corresponding to the reference voxel as the reference frame.

[0041] Optionally, in some embodiments of the present application, the voxel of the 3D grid is empty in an initial state;

[0042] after the determining the target attitude information corresponding to the current attitude information according to the reference frame and the current magnetic field intensity information, the method further comprises:

[0043] determining a voxel state of a target voxel corresponding to the target attitude information in the 3D grid;

[0044] if the voxel state is empty, filling the target voxel of the 3D grid.

[0045] Optionally, in some embodiments of the present application, the 3D grid takes roll angle information, pitch angle information and heading angle information as three coordinate axes;

[0046] the determining the voxel state of the target voxel corresponding to the target attitude information in the 3D grid comprises:

[0047] extracting target roll angle information, target pitch angle information and target heading angle information from the target attitude information;

[0048] determining a target identification bit of the target attitude information in the 3D grid according to a voxel edge length of the 3D grid and the target roll angle information, the target pitch angle information and the target heading angle information;

[0049] determining the voxel state according to a voxel filling state of the target voxel corresponding to the target identification bit.

[0050] In a third aspect, the embodiments of the present application further provide an electronic device, which comprises a memory, a processor and a computer program stored in the memory and capable of running on the processor, and the computer program is executed by the processor to implement the steps in the attitude estimation method described above.

[0051] In a fourth aspect, the embodiments of the present application further provide a computer readable storage medium, and the computer readable storage medium stores a computer program. The computer program is executed by a processor to implement the steps of the posture estimation method.

[0052] In a fifth aspect, the embodiments of the present application further provide a computer program product or a computer program. The computer program product or the computer program includes computer instructions stored in a computer readable storage medium. A processor of a computer device reads the computer instructions from the computer readable storage medium, and the processor executes the computer instructions to enable the computer device to perform the method provided in various optional implementation manners of the embodiments of the present application.

[0053] The embodiments of the present application determine current posture information corresponding to a current frame. After initialization of the stable magnetic field is completed, if current magnetic field intensity information of the current frame and reference magnetic field intensity information of the stable magnetic field satisfy a first preset condition, a reference frame is selected from a preset frame set corresponding to the stable magnetic field according to the current posture information, wherein reference posture information corresponding to the reference frame satisfies a second preset condition with the current posture information, and target posture information corresponding to the current posture information is determined according to the reference frame and the current magnetic field intensity information.

[0054] The posture information is optimized by selecting the reference frame and combining the reference frame, and the accuracy of posture information estimation is improved. The reference frame is selected and the posture information is optimized in the stable magnetic field, and the accuracy of posture information estimation is further improved. BRIEF DESCRIPTION OF DRAWINGS

[0055] In order to more clearly illustrate the technical solutions in the present application, the following will briefly introduce the drawings needed in the embodiment description. Obviously, the drawings in the following description are only some embodiments of the present application, and other drawings can be obtained by those skilled in the art without creative labor.

[0056] Figure 1 is a scene schematic diagram of the electronic device executing the posture estimation method provided by the embodiments of the present application;

[0057] Figure 2 is a flow schematic diagram of the posture estimation method provided by the embodiments of the present application;

[0058] Figure 3 is a framework diagram of the posture estimation system provided by the embodiments of the present application;

[0059] Figure 4 is a schematic diagram of the relationship among the acceleration measurement value, the acceleration and the active acceleration provided by the embodiments of the present application;

[0060] Figure 5 is a schematic diagram of the relationship among the measured value of the magnetic field strength, the acceleration, and the active acceleration provided by an embodiment of the present application;

[0061] Figure 6 is a structural schematic diagram of a pose estimation device provided by an embodiment of the present application;

[0062] Figure 7 is a structural schematic diagram of an electronic device provided by an embodiment of the present application.

[0063] BRIEF DESCRIPTION OF THE DRAWINGS

[0064] 10-electronic device; 201-gyroscope prediction module; 202-accelerometer update module; 203-magnetometer update module; 301-first determination module; 302-screening module; 303-second determination module; 401-processor; 402-memory; 403-power supply; 404-input unit. DETAILED DESCRIPTION

[0065] The technical solutions in the present application will be described in detail below with reference to the drawings in the present application. Obviously, the described embodiments are only a part of the embodiments of the present application, rather than all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by a person skilled in the art without creative work fall within the protection scope of the present application.

[0066] The present application provides a pose estimation method and device, an electronic device, and a computer readable storage medium. Specifically, the present application provides a pose estimation device suitable for an electronic device to improve the accuracy of pose estimation. Specifically, the electronic device includes an extended reality device, which includes but is not limited to a head-mounted display, wearable glasses, and the extended reality device can be an integrated extended reality device with a built-in computing processing unit or a split extended reality device connected to an external computing processing unit. The extended reality device includes but is not limited to an onboard optical display system, i.e. a head-up display system, applied to aircraft, cars, ships, and other vehicles, such as AR-HUD (Augmented reality hud-up display) mounted on intelligent networked vehicles, handheld mobile devices such as mobile phones, desktop computers, notebook computers, tablets, and wearable near-eye display systems such as head-mounted displays and smart glasses. When the extended reality device is a wearable head-mounted display or smart glasses, it can be an integrated extended reality device with a built-in computing processing unit or a split extended reality device connected to an external computing processing unit.

[0067] Please refer to Figure 1 , Figure 1is a scene schematic diagram of an electronic device executing the posture estimation method provided in the embodiment of the present application, and a specific execution process of the electronic device executing the posture estimation method is as follows:

[0068] The electronic device 10 determines current posture information corresponding to a current frame. After completion of initialization of the stable magnetic field, if current magnetic field intensity information of the current frame and reference magnetic field intensity information of the stable magnetic field satisfy a first preset condition, the electronic device 10 screens a reference frame from a preset frame set corresponding to the stable magnetic field according to the current posture information, wherein reference posture information corresponding to the reference frame satisfies a second preset condition with the current posture information, and the electronic device 10 determines target posture information corresponding to the current posture information according to the reference frame and the current magnetic field intensity information.

[0069] For example, taking a smart glasses product form of the electronic device as an extended reality device as an example, when a user wears the smart glasses, if there is a demand for posture estimation, current posture information of a current frame is determined, and after completion of initialization of the stable magnetic field, if current magnetic field intensity information of the current frame and reference magnetic field intensity information of the stable magnetic field satisfy a first preset condition, a reference frame is screened from a preset frame set according to the current posture information, and then target posture information is obtained by optimizing the current posture information in combination with the reference frame.

[0070] In summary, the embodiment of the present application optimizes posture information by screening a reference frame and optimizing the posture information in combination with the reference frame, thereby improving the accuracy of posture information estimation. By screening the reference frame and optimizing the posture information in the stable magnetic field, the accuracy of posture information estimation is further improved.

[0071] The following will be described in detail. It should be noted that the order of description of the following embodiments is not limited as the order of priority of the embodiments.

[0072] Please refer to Figure 2 , Figure 2 is a flowchart of a posture estimation method provided in the embodiment of the present application. 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 in the flowchart. Specifically, the flow of the electronic device executing the posture estimation method specifically includes:

[0073] 101, determine current posture information corresponding to a current frame.

[0074] It should be noted that the current frame corresponds to the current moment of motion, and the current frame corresponds to time information, magnetometer measurement values, acceleration measurement values, and gyroscope measurement values. In the embodiments of the present application, the current attitude information is the attitude information corresponding to the current frame, which reflects the rotation information and direction information of the electronic device or the IMU in the electronic device. In the embodiments of the present application, the rotation and direction are represented by the roll angle information (also referred to as the roll angle information), the pitch angle information, and the heading angle information of the electronic device in the current frame, that is, in the embodiments of the present application, the current attitude information includes the roll angle information, the pitch angle information, and the heading angle information corresponding to the current frame.

[0075] 102. After the initialization of the stable magnetic field is completed, if the current magnetic field intensity information of the current frame and the reference magnetic field intensity information of the stable magnetic field satisfy a first preset condition, a reference frame is selected from a preset frame set corresponding to the stable magnetic field according to the current attitude information, wherein the reference attitude information corresponding to the reference frame satisfies a second preset condition with the current attitude information.

[0076] The initialization of the stable magnetic field refers to a process of setting initial parameters, calibrating devices, and adjusting compensation mechanisms to make the magnetic field reach a preset stable state in a specific application scenario. The core goal of this process is to eliminate environmental interference, match system requirements, and provide a reference for subsequent magnetic field regulation. For example, within a time interval of electronic device motion, when the difference between the magnetic field intensity measurement values provided by the continuous m frames of the magnetometer and the low-pass filtered magnetic field intensity measurement values is less than a threshold value, the stable magnetic field is initialized as the low-pass filtered magnetic field intensity measurement values, thereby realizing the initialization of the stable magnetic field.

[0077] The current magnetic field intensity information of the current frame and the reference magnetic field intensity information of the stable magnetic field satisfy the first preset condition, which means that the current frame is within the stable magnetic field range and is suitable for attitude estimation using the characteristics of the stable magnetic field. The first preset condition is a condition set to represent that the difference between the current magnetic field intensity information and the reference magnetic field intensity information is small, for example, when the difference between the magnetic field intensity measurement values of the current magnetic field intensity information and the reference magnetic field intensity information is less than a threshold value, it is considered that the first preset condition is satisfied at this time, wherein the threshold value can be configured based on actual needs, for example, the threshold value includes but is not limited to between 5 μT and 10 mT, etc.

[0078] It should be noted that the preset frame set contains multiple attitude frames for the same stable magnetic field, wherein each attitude frame corresponds to different attitude information, i.e., the roll angle information, the pitch angle information, and the heading angle information of each attitude frame are different. It should be noted that each stable magnetic field corresponds to a preset frame set, i.e., the stable magnetic fields corresponding to different preset frame sets are different.

[0079] The reference attitude information and the current attitude information satisfy a second preset condition, which means that the reference attitude information and the current attitude information have relevance or correlation, for example, the reference attitude information and the current attitude information are close, have reference value for attitude optimization, and the like.

[0080] 103. determining target attitude information corresponding to the current attitude information according to the reference frame and the current magnetic field intensity information.

[0081] For example, a relative constraint between two frames is constructed in a direction of a magnetic field intensity measurement value corresponding to the current magnetic field intensity information and a direction of a magnetic field intensity measurement value of reference magnetic field intensity information corresponding to the reference frame, and then the current attitude information of the current frame is corrected.

[0082] It should be noted that in the prior art, many 3DoF attitude estimation methods adaptively calculate the measurement noise of the accelerometer and the magnetometer, thereby improving the attitude estimation accuracy. However, the existing 3DoF attitude estimation method regards the magnetic field intensity measurement value provided by the magnetometer as an absolute attitude reference quantity for updating, that is, directly determines the heading angle corresponding to a moment according to the direction of the magnetic field intensity measurement value at the moment. In the case where there is no magnetic field interference or the magnetic field interference is accurately estimated, the magnetic field intensity measurement value provided by the magnetometer can be regarded as an absolute attitude reference quantity to provide an accurate heading angle. However, when there is magnetic field interference and the magnetic field interference cannot be accurately estimated, using the magnetic field intensity measurement value provided by the magnetometer as an absolute attitude reference quantity will provide a heading angle with error, and the heading angle error is different at different attitudes, which will reduce the estimation accuracy of the heading angle.

[0083] The embodiments of the present application optimize the attitude information by screening the reference frame and combining the reference frame to improve the accuracy of attitude information estimation. In the embodiments of the present application, the reference frame is screened and the attitude information is optimized in a stable magnetic field to further improve the accuracy of attitude information estimation.

[0084] In the embodiments of the present application, considering the environmental noise, an extended Kalman filtering algorithm is used to describe the device state through a state vector (such as attitude quaternion and angular velocity), to quantify the uncertainty of state estimation through a covariance matrix, and to iteratively update the state and covariance through local linearization (Jacobian matrix) of a nonlinear model, thereby realizing optimal attitude estimation in a noise environment. The technical solutions of the embodiments of the present application are described in detail below.

[0085] In the embodiments of the present application, since the current attitude information is corrected in combination with the reference frame, based on the characteristics of the state vector and the covariance matrix of the extended Kalman filtering algorithm, the reference state estimation information and the reference covariance matrix of the reference frame are combined to optimize the state estimation of the current frame, and then the target attitude information optimized for the current attitude information is solved. That is, in some embodiments of the present application, the step of "determining the target attitude information corresponding to the current attitude information according to the reference frame and the current magnetic field intensity information" comprises:

[0086] determining reference state estimation information and a reference covariance matrix corresponding to the reference frame, the reference state estimation information comprising a posterior state estimation mean value for the reference frame;

[0087] determining target state estimation information of the current frame according to the reference state estimation information, the reference covariance matrix and the current magnetic field intensity information;

[0088] extracting the target attitude information corresponding to the current attitude information from the target state estimation information.

[0089] The reference state estimation information is the state estimation posterior mean value (i.e., the posterior state estimation mean value) for the reference frame, comprising the rotation of the IMU corresponding to the three-axis orthogonal body coordinate system to the world coordinate system at the time of the reference frame. The reference covariance matrix is used to quantify the uncertainty of the reference state estimation information of the reference frame.

[0090] The target state estimation information of the current frame is determined by combining the reference state estimation information and the reference covariance matrix of the reference frame, which improves the accuracy of the target state estimation information.

[0091] Correspondingly, after determining the target state estimation information of the current frame, the target attitude information corresponding to the current attitude information, i.e., the corrected roll angle information, pitch angle information and heading angle information, can be extracted from the target state estimation information.

[0092] When the current attitude information of the current frame is corrected based on the reference frame, the joint magnetic field intensity measurement model for the reference frame and the current frame can be constructed, that is, in some embodiments of the present application, the step of "determining the target state estimation information of the current frame according to the reference state estimation information, the reference covariance matrix and the current magnetic field intensity information" comprises:

[0093] determining a joint magnetic field intensity measurement model, the joint magnetic field intensity measurement model being constructed in combination with the current frame and the reference frame;

[0094] determining residual information for the magnetic field noise according to the joint magnetic field intensity measurement model and the current magnetic field intensity information;

[0095] determining target gain information according to the reference state estimation information and the reference covariance matrix;

[0096] determining the target state estimation information according to the residual information and the target gain information.

[0097] It should be noted that the joint magnetic field strength measurement model is constructed based on the reference attitude information of the reference frame, the reference magnetic field strength information and the current attitude information of the current frame and the current magnetic field strength information, and therefore, the residual information reflects the comprehensive difference between the current frame and the reference frame after joint consideration.

[0098] The target gain information is also referred to as Kalman gain, which is used to weigh the weights of the reference frame and the current frame in the embodiments of the present application, for example, the contributions of the reference frame and the current frame are adjusted according to the reference covariance matrix of the reference frame and the covariance matrix corresponding to the measurement value of the current frame, so that the fusion of the reference attitude information of the reference frame and the current attitude information of the current frame is more reasonable, and therefore, the target state estimation information is determined based on the residual information and the target gain information, so that the state estimation obtained is more accurate.

[0099] The reference frame determined based on the stable magnetic field is mainly used to correct the heading angle of the current frame, and the correction of the roll angle and the pitch angle mainly depends on the angular velocity measurement value of the gyroscope and the acceleration measurement value of the accelerometer.

[0100] In order to ensure the accuracy of the target attitude information obtained finally, the embodiments of the present application further correct the heading angle in combination with the reference frame on the basis of the roll angle and the pitch angle correction based on the angular velocity measurement value of the gyroscope and the acceleration measurement value of the accelerometer. That is, in some embodiments of the present application, the current attitude information includes roll angle information, pitch angle information and heading angle information, and the step of “determining target gain information according to the reference state estimation information and the reference covariance matrix” includes:

[0101] obtaining transition state estimation information and transition covariance matrix, the transition state estimation information being the posteriori estimation information of the attitude state of the current frame for the roll angle information and the pitch angle information;

[0102] determine the target gain information according to the transition state estimation information, the transition covariance matrix, the reference state estimation information and the reference covariance matrix;

[0103] determining the target state estimation information according to the residual information and the target gain information, comprises:

[0104] determining state offset information according to the residual information and the target gain information;

[0105] updating the transition state estimation information according to the state offset information to obtain the target state estimation information.

[0106] Wherein, by combining the state posterior vector based on the roll angle information and the pitch angle information and the corresponding covariance matrix to calculate the target gain information, the accuracy of the target gain information calculation is improved, and the accuracy of the target state estimation information is improved.

[0107] Wherein, after determining the residual information, the target gain information can be calculated according to the Jacobian matrix of the current frame and the Jacobian matrix of the reference frame, and the accuracy of the target gain information is improved. That is, in some embodiments of the present application, the step of "determining the target gain information according to the transition state estimation information, the transition covariance matrix, the reference state estimation information and the reference covariance matrix" comprises:

[0108] constructing a reference Jacobian matrix of the residual information for the reference frame according to the transition state estimation information and the reference state estimation information, and constructing a transition Jacobian matrix of the residual information for the current frame according to the transition state estimation information and the reference state estimation information, wherein the reference Jacobian matrix and the transition Jacobian matrix are both for the heading angle information;

[0109] determining the target gain information according to the reference Jacobian matrix, the transition Jacobian matrix, the transition covariance matrix and the reference covariance matrix.

[0110] Wherein, the transition state estimation information and the transition covariance matrix are calculated by the angular velocity measurement value of the gyroscope and the acceleration measurement value of the accelerometer, that is, in some embodiments of the present application, the step of "obtaining the transition state estimation information and the transition covariance matrix" comprises:

[0111] integrating and predicting according to the angular velocity information corresponding to the current frame to obtain initial state estimation information and initial covariance matrix;

[0112] determining a second covariance matrix for the target noise according to an acceleration measurement model;

[0113] The transition state estimation information and the transition covariance matrix are determined according to the initial state estimation information, the initial covariance matrix, the second covariance matrix, and acceleration information corresponding to the current frame.

[0114] Wherein, the target noise refers to noise when the roll angle and the pitch angle are updated.

[0115] Specifically, the gyroscope can provide accurate attitude estimation in a short time, but due to the existence of noise and gyroscope zero offset, the integral error of the angular velocity provided by the gyroscope will continuously accumulate over time. Complementarily with the gyroscope, when the main acceleration perceived by the accelerometer is the gravitational acceleration (when stationary, uniform motion, acceleration along the direction of gravity), the absolute roll and pitch angles can be provided according to the component distribution of the gravitational acceleration on the accelerometer coordinate axis. Therefore, the embodiment of the application corrects the roll and pitch angle error accumulated by long-time integration by using the accelerometer measurement value, so that the roll angle and the pitch angle in the current attitude information of the current frame are corrected.

[0116] Wherein, in order to more clearly describe the attitude estimation method in the embodiment of the application, the following will describe the scheme according to the updating order of the roll angle, the pitch angle and the heading angle. Specifically, please refer to Figure 3 , Figure 3 is the framework diagram of the attitude estimation system provided by the embodiment of the application, wherein the attitude estimation system comprises a gyroscope prediction module 201, an accelerometer update module 202 and a magnetometer update module based on loop correction 203. Specifically:

[0117] The gyroscope prediction module 201 uses the angular velocity information provided by the gyroscope to perform integration to provide accurate attitude estimation in a short time.

[0118] Wherein, the accurate attitude estimation in a short time includes initial state estimation information and an initial covariance matrix.

[0119] The accelerometer update module 202 corrects the roll and pitch angles by using the acceleration measurement value provided by the accelerometer, and by adaptively calculating the acceleration measurement value uncertainty, the correction of the roll and pitch angles in the low acceleration stage can be effectively utilized, and the roll and pitch angle error caused by acceleration impact is reduced.

[0120] Wherein, the correction of the roll and pitch angles obtains the transition state estimation information, and the transition covariance matrix is obtained by adaptively calculating the acceleration measurement value uncertainty.

[0121] Magnetometer update module 203 constructs historical keyframe information under a stable magnetic field environment, and detects historical keyframes that form a loop with the current frame when re-moving near the historical keyframes. Using the attitude of the looping historical keyframes as a reference, it constructs the relative attitude constraints between the two frames by using the direction of the magnetic field strength measurement value of the looping historical keyframes and the direction of the magnetic field strength measurement value of the current frame, thereby correcting the heading angle of the current frame.

[0122] Accordingly, by using the current magnetic field strength measurement value (corresponding to the current magnetic field strength information) of the current frame as a relative attitude reference to update the heading angle, the problem of providing an erroneous heading angle when using the magnetic field strength measurement value as an absolute attitude reference value in the presence of magnetic field interference and when the magnetic field interference cannot be accurately estimated is effectively avoided, thereby improving the heading angle estimation accuracy. In addition, by adaptively calculating the uncertainty of the magnetic field strength measurement value, the heading angle can be effectively corrected using a stable magnetic field, and the heading angle error caused by magnetic field interference can be reduced.

[0123] Each historical keyframe corresponds to an attitude frame in the preset frame set in this application embodiment. The loop closure historical keyframe determined based on loop closure detection is the reference frame selected from the preset frame set based on the current attitude information in this application embodiment.

[0124] Specifically, the following provides a more detailed description of each module. Before the description, the state vector to be estimated in this embodiment of the application will be explained or defined, for example, the current time corresponding to the current frame. For example, the state vector to be estimated includes:

[0125]

[0126] in, Indicates the current time The state estimation information is called the active state. This represents the body coordinate system of the IMU at the current moment, where the three axes are mutually orthogonal. To the world coordinate system rotation, This indicates that the gyroscope has zero bias. The attitude state representing a key historical moment (assuming there are n key historical moments) (corresponding to the reference state estimation information of each attitude frame in the preset frame set in this application embodiment) is called the Schmidt state. This represents the body coordinate system corresponding to the IMU at the first historical critical moment, where the three axes are mutually orthogonal. To the world coordinate system The rotation. represents the rotation of the IMU sensor corresponding three-axis mutually orthogonal body frame coordinate system at the n-th historical key moment to the world coordinate system .

[0127] The state covariance matrix is represented as:

[0128]

[0129] wherein, represents the covariance matrix of the active state, and represents the covariance matrix between the active state and the Schmidt state, represents the covariance matrix of the Schmidt state.

[0130] Next, based on the above description or definition of the state vector to be estimated, the attitude estimation system of the embodiments of the present application is described, in particular:

[0131] The gyroscope prediction module 201 specifically includes: integrating the angular velocity measurement value provided by the gyroscope based on the state posterior mean and covariance of the previous moment , and further obtaining the state prior mean of the current moment (i.e. corresponding to the initial state estimation information in the embodiments of the present application) and the covariance matrix (i.e. corresponding to the initial covariance matrix in the embodiments of the present application). Specifically:

[0132]

[0133] wherein, represents the rotation posterior mean of the IMU corresponding three-axis mutually orthogonal body coordinate system to the world coordinate system estimated at the previous moment , represents the rotation prior mean of the IMU corresponding three-axis mutually orthogonal body coordinate system to the world coordinate system estimated at the current moment , represents the gyroscope zero offset posterior mean estimated at the previous moment , represents the gyroscope zero offset prior mean estimated at the current moment , represents the time interval between the current moment and the previous moment ,​ denotes the current time instant estimated Schmitt state prior mean, denotes the previous time instant estimated Schmitt state posterior mean. denotes the exponential map that transforms a rotation vector into a rotation matrix. is the state transition matrix, where denotes the 3x3 identity matrix, denotes the noise gain matrix, denotes the noise matrix, where, denotes the gyroscope noise covariance matrix, denotes the gyroscope bias noise covariance matrix. denotes the previous time instant estimated active state posterior covariance matrix, and denotes the previous time instant estimated active state and Schmitt state posterior covariance matrix, denotes the previous time instant estimated Schmitt state posterior covariance matrix.

[0134] where the angular velocity measurement is obtained by coordinate transformation, for example, the gyroscope angular velocity measurement at the current time instant provided by the gyroscope is transformed to the IMU corresponding three-axis orthogonal body coordinate system to obtain the angular velocity measurement , where the coordinate transformation formula is specifically:

[0135]

[0136] where, denotes the non-orthogonality between the axes of the gyroscope, i.e., the mutual interference between the axes due to manufacturing process, etc., denotes the gyroscope cross-axis coupling error obtained according to the IMU calibration, where denotes the projection ratio of the motion in the z-axis direction on the y-axis channel, denotes the projection ratio of the motion in the y-axis direction on the z-axis channel, denotes the projection ratio of the motion in the z-axis direction on the x-axis channel, denotes the projection ratio of the motion in the x-axis direction on the z-axis channel, denotes the projection ratio of the motion in the y-axis direction on the x-axis channel, denotes the projection ratio of the motion in the x-axis direction on the y-axis channel; denotes the proportional relationship between the measured value and the actual angular velocity, also known as the proportional factor, and is used to correct the scaling error of the measured data, wherein 、 、 denote the proportional factors of the gyroscope on the x-axis, y-axis and z-axis respectively; denotes the output value of the gyroscope when it is stationary, i.e. the bias, wherein 、 、 denote the x-axis, y-axis and z-axis components of the bias respectively.

[0137] wherein the angular velocity measurement model provided by the gyroscope is:

[0138]

[0139] wherein, denotes the angular velocity measurement value provided by the gyroscope under the body coordinate system corresponding to the IMU at the current time , denotes the true value of the angular velocity under the body coordinate system corresponding to the IMU at the current time , denotes the bias of the gyroscope that changes over time, denotes the noise of the gyroscope, denotes the random walk noise of the gyroscope. The accelerometer updating module 202 specifically comprises: combining the state prior mean value (i.e. corresponding to the initial state estimation information in the embodiment of the application) and the covariance matrix

[0140] (i.e. corresponding to the initial covariance matrix in the embodiment of the application) obtained by the gyroscope prediction module 201 at the current time , using the acceleration measurement information provided by the accelerometer to update the state, to obtain the first-stage state posterior mean value (i.e. corresponding to the transition state estimation information in the embodiment of the application) and the covariance matrix (i.e. corresponding to the transition covariance matrix in the embodiment of the application).

[0141]

[0142] wherein, denotes the prior covariance matrix of the active state estimated by the gyroscope prediction module at the current time , denotes the residual constructed by the acceleration information, denotes the Jacobian matrix of the residual constructed by the acceleration information with respect to the active state, ​​denotes the noise covariance matrix of the acceleration measurements provided by the accelerometer when performing the roll, pitch angle update, denotes the Kalman gain used to weigh the prior estimate of the gyroscope prediction module against the measurements provided by the accelerometer. denotes the prior mean of the active state estimated by the gyroscope prediction module at the current time denotes the prior mean of the active state estimated by the gyroscope prediction module at the current time denotes the first stage active state posterior mean at the current time denotes the prior mean of the Schmitt state estimated by the gyroscope prediction module at the current time denotes the prior mean of the Schmitt state estimated by the gyroscope prediction module at the current time denotes the first stage Schmitt state posterior mean at the current time denotes the first stage Schmitt state posterior mean at the current time denotes the first stage Schmitt state posterior mean at the current time denotes the first stage Schmitt state posterior mean at the current time denotes the first stage Schmitt state posterior mean at the current time denotes the first stage Schmitt state posterior mean at the current time denotes the prior covariance matrix of the active state estimated by the gyroscope prediction module at the current time denotes the prior covariance matrix of the active state estimated by the gyroscope prediction module at the current time denotes the prior covariance matrix of the active state estimated by the gyroscope prediction module at the current time denotes the prior covariance matrix of the active state estimated by the gyroscope prediction module at the current time denotes the prior covariance matrix of the active state estimated by the gyroscope prediction module at the current time denotes the prior covariance matrix of the active state estimated by the gyroscope prediction module at the current time denotes the prior covariance matrix of the active state estimated by the gyroscope prediction module at the current time

[0143] wherein the acceleration measurement model is denoted as:

[0144]

[0145] wherein, denotes the acceleration measurements provided by the accelerometer in the IMU corresponding body frame at the current time denotes the true acceleration in the IMU corresponding body frame at the current time denotes the true acceleration in the IMU corresponding body frame at the current time denotes the accelerometer noise. Wherein the acceleration measurements provided by the accelerometer at the current time are transformed into the acceleration measurements in the IMU corresponding body frame are denoted as:

[0146]

[0147] wherein the accelerometer cross-axis coupling error represents the non-orthogonality between the axes of the accelerometer, i.e. the mutual interference between the axes due to manufacturing processes and the like, wherein represents the projection ratio of the motion in the z-axis direction on the y-axis channel, represents the projection ratio of the motion in the y-axis direction on the z-axis channel, represents the projection ratio of the motion in the x-axis direction on the z-axis channel, the scale factor represents the scale relationship between the measured value and the actual acceleration, used to correct the scaling error of the measurement data, wherein , , respectively represent the scale factors of the accelerometer in the x-axis, y-axis and z-axis, the zero offset represents the output value of the accelerometer when it is at rest, wherein , , respectively represent the x-axis, y-axis and z-axis components of the zero offset.

[0148] wherein the true value of acceleration can be decomposed into the acceleration caused by the gravitational acceleration and the active acceleration, so the acceleration measurement model is specifically represented as:

[0149]

[0150] wherein, represents the rotation of the body coordinate system corresponding to the three axes of the IMU at the current moment to the world coordinate system , represents the gravitational acceleration in the world coordinate system, represents the active acceleration. It is considered together constitute the noise of the acceleration measurement value when updating the roll and pitch angles. Only the direction of gravity will contribute to the roll and pitch angles, so we normalize the above formula to obtain:

[0151]

[0152] wherein, , i.e. the target noise in the foregoing, is directed to the noise when updating the roll and pitch angles.

[0153] Further, the covariance matrix of the noise of the acceleration measurement value provided by the accelerometer when updating the roll and pitch angles is:

[0154]

[0155] in combination with Figure 4 the The relationship between the active state and the Schmitt state is shown in the following equation: , The covariance matrix can be expressed as:

[0156]

[0157] wherein, , and denote the coefficients. Thus, the expression of the covariance matrix is:

[0158]

[0159] wherein, denotes the accelerometer noise. And, based on the acceleration measurement model, the residual information is expressed as:

[0160]

[0161] wherein, denotes the IMU corresponding three-axis mutually orthogonal body coordinate system estimated by the gyroscope prediction module at the current time to the world coordinate system . The residual information relative to the Jacobian matrix of the active state is:

[0162]

[0163] wherein, denotes the skew-symmetric matrix.

[0164] And, the Jacobian matrix of the residual information relative to the Schmitt state is .

[0165] wherein, in the Jacobian matrix of the residual information relative to the active state , since , the last column elements of are all zero, thereby effectively ensuring that the acceleration measurement model only changes the roll and pitch angles, and does not change the heading angle. Thus, by adaptively calculating the accelerometer uncertainty (i.e., the covariance matrix corresponding to the current frame), the correction of the roll and pitch angles in the low acceleration stage can be effectively utilized to reduce the roll and pitch angle errors caused by the acceleration impact.

[0166] The magnetometer update module 203 is specifically configured to determine a reference frame (also referred to as a key frame forming a loop with the current frame), and determine the first stage state posterior mean (i.e., the transition state estimation information in the embodiments of the present application) and the covariance matrix (i.e., the transition covariance matrix corresponding to the embodiments of this application), and update the state using the current magnetic field strength information provided by the magnetometer, to obtain the posterior mean of the second-stage state. (i.e., the target state estimation information corresponding to the embodiments of this application) and covariance matrix .

[0167]

[0168] in, This indicates the current time estimated by the accelerometer update module. The first-stage posterior covariance matrix of the active state. and This indicates the current time estimated by the accelerometer update module. The first-stage posterior covariance matrix between the active state and the Schmidt state. This indicates the current time estimated by the accelerometer update module. The first-stage posterior covariance matrix of the Schmidt state, This represents the residual constructed from the magnetometer information. This represents the Jacobian matrix representing the residuals constructed from magnetometer information relative to the active state. The Jacobian matrix representing the residual constructed from magnetometer information relative to the Schmidt state. This represents the noise covariance matrix of the magnetic field strength measurement provided by the magnetometer when updating the heading angle. This represents the Kalman gain, used to balance the first-stage posterior value estimated by the accelerometer update module with the measurement provided by the magnetometer. (The right side of the equation...) Indicates the current time The posterior mean of the first active state, the left side of the equals sign Indicates the current time The posterior mean of the second-stage active state. (The right side of the equation is...) Indicates the current time The posterior mean of the first-stage Schmidt state, the left side of the equation Indicates the current time The posterior mean of the second-stage Schmidt state.

[0169] Among them, based on keyframes (Reference frame), determine the joint magnetic field strength measurement model for the keyframe and the current frame, expressed as:

[0170]

[0171] in, Indicates the current frame The measured value of the magnetic field strength, Keyframe The measured value of the magnetic field strength, Indicates the current frame A body coordinate system with three mutually orthogonal axes To the world coordinate system rotation, Keyframe A body coordinate system with three mutually orthogonal axes To the world coordinate system rotation, This indicates an external magnetic field disturbance. This indicates the magnetometer noise. Specifically, it refers to the current time of the magnetometer reading. Provided magnetic field strength measurement values Transform to the body coordinate system corresponding to the IMU, where the three axes are mutually orthogonal. Down:

[0172]

[0173] Among them, the magnetometer calibration matrix This indicates various calibration factors of the magnetometer, including the non-orthogonality between axes, the scaling factor, and the offset. This indicates the output value of the magnetometer in a zero magnetic field environment.

[0174] Among them, it is believed These factors together constitute the noise in the magnetic field strength measurement during heading angle updates. Furthermore, since only the direction of the magnetic field strength contributes to the heading angle update, we normalize the above equation to obtain:

[0175]

[0176] in, .

[0177] Furthermore, the noise in the current magnetic field strength measurement provided by the magnetometer during heading angle updates... The covariance matrix is:

[0178]

[0179] Combination Figure 5 shown and , The relationship between the two variables determines that the covariance matrix can be expressed as follows:

[0180]

[0181] in, , and denotes the coefficients. From this, the covariance matrix is expressed as:

[0182]

[0183] And, combined with the joint magnetic field intensity measurement model, the residual information is expressed as:

[0184]

[0185] wherein, denotes the first stage posterior mean of the rotation of the current frame corresponding to the body coordinate system with three mutually orthogonal axes to the world coordinate system estimated by the accelerometer update module, denotes the first stage posterior mean of the rotation of the key frame corresponding to the body coordinate system with three mutually orthogonal axes to the world coordinate system estimated by the accelerometer update module, denotes the direction of the magnetic field intensity measurement value of the current frame , and denotes the direction of the magnetic field intensity measurement value of the key frame . The Jacobian matrix of the residual information with respect to the active state is:

[0186]

[0187] wherein, denotes the skew-symmetric matrix.

[0188] And the Jacobian matrix of the residual information with respect to the Schmidt state is:

[0189]

[0190] Since the magnetic field intensity measurement value can only provide heading angle information, it is not desirable for the magnetic field intensity measurement value to change the roll and pitch angles, so the Jacobian matrix of the residual information with respect to the active state is modified as:

[0191]

[0192] And the Jacobian matrix of the residual information with respect to the Schmidt state is modified as:

[0193]

[0194] Correspondingly, in the embodiments of the present application, the preset frame set is constructed in advance, based on the characteristics that the reference attitude information of each attitude frame includes three angle information of roll angle information, pitch angle information and heading angle information, a 3D grid (the preset frame set) for storing each attitude frame is constructed, and then based on the different characteristics of the roll angle information, the pitch angle information and the heading angle information of each attitude frame, different voxels in the 3D grid are used to correspondingly store the reference attitude information of different attitude frames.

[0195] For example, the 3D grid takes the roll angle information, the pitch angle information and the heading angle information as three coordinate axes, assuming that the reference attitude information of a certain attitude frame corresponds to the roll angle information, the pitch angle information and the heading angle information of , the voxel length of the 3D grid is , the identification bits of the roll coordinate axis, the pitch coordinate axis and the heading angle coordinate axis of the reference attitude information of the attitude frame in the 3D grid are respectively , that is, the voxel coordinate of the reference attitude information of the attitude frame in the 3D grid is (x, y, z). ).

[0196] Correspondingly, in the embodiments of the present application, the reference voxel can be selected according to the current attitude information and the reference attitude information corresponding to each voxel in the 3D grid, that is, the reference frame is selected, that is, in some embodiments of the present application, the step of "selecting the reference frame from the preset frame set corresponding to the stable magnetic field according to the current attitude information" comprises:

[0197] determining the 3D grid corresponding to the stable magnetic field, each voxel in the 3D grid corresponding to the reference attitude information of an attitude frame;

[0198] selecting the reference voxel from the voxels according to the current attitude information and each reference attitude information;

[0199] setting the attitude frame corresponding to the reference voxel as the reference frame.

[0200] For example, taking a 3*3 Rubik's cube as an example, the 9 blocks (equivalent to the voxels in the 3D grid) composing the Rubik's cube correspond to different reference attitude information, and each reference attitude information corresponds to a different attitude frame, so each block corresponds to a different attitude frame.

[0201] For example, in the embodiments of the present application, the voxels close to the current voxel are selected as the reference voxels. For example, based on the current attitude information, the current voxel in the 3D grid is determined, and the reference voxels are selected from the vicinity of the current voxel in the 3D grid. It should be noted that when selecting the corresponding reference voxel, the current voxel is determined based on the current attitude information to be corrected, not based on the target attitude information to be corrected.

[0202] In the embodiments of the present application, in order to improve the accuracy of the reference voxel screening, the size of the included angle between the direction of the magnetic field intensity measurement value corresponding to the current voxel and the direction of the magnetic field intensity measurement value corresponding to the screened voxel and the difference of the magnetic field intensity measurement value are used to determine whether the screened voxel is the reference voxel. That is, in the embodiments of the present application, if the included angle between the direction of the current magnetic field intensity measurement value corresponding to the current voxel (corresponding to the current frame) and the direction of the candidate magnetic field intensity measurement value of the candidate voxel screened from the vicinity of the current voxel is less than a threshold value, and the difference between the modulus of the current magnetic field intensity measurement value and the modulus of the candidate magnetic field intensity measurement value of the candidate voxel is less than a threshold value, then the candidate voxel is considered as the reference voxel of the current voxel, that is, the pose frame corresponding to the reference voxel is the reference frame corresponding to the current frame.

[0203] It should be noted that the reference pose information stored in the voxel of the 3D grid is corrected pose information, for example, the target pose information corresponding to the current pose information determined based on the pose estimation scheme of the embodiments of the present application. After determining the target pose information, the target pose information corresponding to the current frame is stored in the target voxel of the 3D grid, that is, in some embodiments of the present application, the voxel of the 3D grid is initially empty, and after the step of "determining the target pose information corresponding to the current pose information according to the reference frame and the current magnetic field intensity information", the method further comprises:

[0204] determining the voxel state of the target voxel corresponding to the target pose information in the 3D grid;

[0205] if the voxel state is empty, filling the target voxel of the 3D grid.

[0206] In the embodiments of the present application, the target pose information corresponding to the current frame is stored in the target voxel of the 3D grid, so filling the target voxel can be understood as storing the target pose information of the current frame in the target voxel.

[0207] In the embodiments of the present application, the target pose information corresponding to the current frame is stored in the target voxel of the 3D grid, so filling the target voxel can be understood as storing the target pose information of the current frame in the target voxel. determining a target identification bit from the 3D grid, wherein the voxel corresponding to the target identification bit is the target voxel.

[0208] In the embodiments of the present application, the target pose information corresponding to the current frame is stored in the target voxel of the 3D grid, so filling the target voxel can be understood as storing the target pose information of the current frame in the target voxel.

[0209]

[0210] Further, at other time instants, the current frame and the corresponding target pose information are then the candidates for the pose estimation process at the other time instants, for example, as the pose frames stored in the 3D grid, or as the Schmitt states of the historical key time instants.

[0211] Further, the covariance matrix of the current frame is augmented as: wherein denotes the first three columns of the posterior covariance matrix estimated at the current time instant denotes the first three rows of the posterior covariance matrix estimated at the current time instant denotes the first three rows and the first three columns of the posterior covariance matrix estimated at the current time instant denotes the first three rows and the first three columns of the posterior covariance matrix estimated at the current time instant denotes the first three rows and the first three columns of the posterior covariance matrix

[0212] In the embodiments of the present application, if the current magnetic field has a large change compared with the stable magnetic field, and the difference between the magnetic field strength measurement value provided by the continuous m frames of magnetometer and the low-pass filtered magnetic field strength measurement value is less than a threshold value in a motion interval, it is considered that the surrounding magnetic field has a stable change, and the low-pass filtered magnetic field strength measurement value is used to update the stable magnetic field, and the pose frame (such as a key frame) information corresponding to the previous stable magnetic field is stored in a container.

[0213] If the new stable magnetic field has appeared before, the related key frame information is extracted from the container (3D grid) corresponding to the new stable magnetic field and is used again, for example, for the new stable magnetic field and the new current frame, the reference frame corresponding to the new current frame is selected from the new stable magnetic field to estimate the target pose information of the current frame. And when the target pose information of the new current frame is empty in the voxel corresponding to the 3D grid of the new stable magnetic field, the key frame information corresponding to the new stable magnetic field is constructed according to the new current frame and the corresponding target pose information, that is, the preset frame set of the new stable magnetic field is constructed.

[0214] ​In summary, by constructing the historical key frame information (i.e., constructing the corresponding preset frame set) in a stable magnetic field environment, and detecting the historical key frame (i.e., the reference frame corresponding to the current frame in the embodiment of the application) constituting a loop with the current frame when moving to the vicinity of the historical key frame, and taking the attitude of the looped historical key frame as a reference, the direction of the magnetic field intensity measurement value of the looped historical key frame and the direction of the magnetic field intensity measurement value of the current frame are used to construct the attitude relative constraint between the two frames, and then the heading angle of the current frame is corrected. The problem that the heading angle provided with errors when the magnetic field intensity measurement value is used as an absolute attitude reference quantity in the case where the magnetic field interference exists and cannot be accurately estimated is effectively avoided, and the heading angle estimation accuracy is improved.

[0215] In addition, by adaptively calculating the uncertainty of the magnetic field intensity measurement value (i.e., the second-stage covariance matrix of the target attitude information ), the stable magnetic field can be effectively used to correct the heading angle, and the heading angle error caused by the magnetic field interference is reduced.

[0216] In addition, since the gyro zero offset (the angular velocity output value of the gyroscope when stationary) changes over time, when it is detected that the device is stationary, the gyro zero offset is updated using the low-pass filtered angular velocity measurement information, so that the system can timely track the slow change of the zero offset, thereby improving the accuracy of the angular velocity information provided by the gyroscope in subsequent measurements.

[0217] Therefore, the embodiment of the application also relates to updating the gyro zero offset, which specifically includes: when the sensor is in a stationary state, combining the second-stage state posterior mean and the covariance matrix at the current time , using the gyro zero offset measurement value to update the state to obtain the third-stage state posterior mean and the covariance matrix . Specifically,

[0218]

[0219] wherein, represents the second-stage posterior covariance matrix of the active state at the current time estimated by the magnetometer update module, and represents the second-stage posterior covariance matrix between the active state and the Schmidt state at the current time estimated by the magnetometer update module, represents the second-stage posterior covariance matrix of the Schmidt state at the current time estimated by the magnetometer update module, represents the residual error constructed by the gyro zero offset information, denotes the Jacobian matrix of the residual error with respect to the active state, denotes the gyroscope bias measurement noise covariance matrix, denotes the Kalman gain for weighting the second stage posterior value estimated by the magnetometer update module and the gyroscope bias information. The right side of the equation denotes the second stage active state posterior mean value at the current time , and the left side of the equation denotes the third stage active state posterior mean value at the current time . The right side of the equation denotes the second stage Schmitt state posterior mean value at the current time , and the left side of the equation denotes the third stage Schmitt state posterior mean value at the current time . Wherein, the gyroscope bias measurement model is:

[0220]

[0221] wherein, denotes the low-pass filtered angular velocity measurement value, denotes the gyroscope bias measurement noise. Based on the gyroscope bias measurement model, the residual error information is , the Jacobian matrix of the residual error information with respect to the active state is , and the Jacobian matrix of the residual error with respect to the Schmitt state (i.e. the reference state estimation information in the foregoing) is .

[0222] wherein, in the embodiments of the present application, it is assumed that denotes the low-pass filtered angular velocity measurement value, denotes the low-pass filtered acceleration measurement value, and the stationary condition judgment condition of the sensor (MARG) is:

[0223] Condition 1: ;

[0224] Condition 2: ;

[0225] Condition 3: .

[0226] wherein , , are adjustable threshold values, denotes the angular velocity measurement value provided by the gyroscope at the current time , and denotes the angular velocity measurement value provided by the gyroscope at the current time The accelerometer provides acceleration measurements. We consider that the sensor is in a stationary state if and only if the angular velocity and acceleration measurements at consecutive m instants satisfy the above conditions 1, 2 and 3.

[0227] It can be understood that the prior art all uses the magnetic field strength measurement as an absolute attitude reference quantity, that is, directly corrects the heading angle corresponding to a moment according to the direction of the magnetic field strength measurement at the moment.

[0228] The embodiment of the present application updates the heading angle by taking the magnetic field strength measurement as a relative attitude reference quantity, that is, when re-moved to the vicinity of a historical key frame, detects a historical key frame (a reference frame corresponding to the current frame) constituting a loop with the current frame, and takes the attitude of the loop historical key frame as a reference, uses the direction of the magnetic field strength measurement of the loop historical key frame and the direction of the magnetic field strength measurement of the current frame to construct the attitude relative constraint between the two frames, and then corrects the heading angle of the current frame.

[0229] Among them, the present scheme effectively avoids the problem that when the magnetic field interference exists and the magnetic field interference cannot be accurately estimated, the magnetic field strength measurement is used as an absolute attitude reference quantity, which will provide a heading angle with error, thereby improving the heading angle estimation accuracy.

[0230] In addition, by adaptively calculating the acceleration measurement, the correction of the roll and pitch angle in the low acceleration stage can be effectively utilized to reduce the roll and pitch angle error caused by acceleration impact; by adaptively calculating the uncertainty of the magnetic field strength measurement, the stable magnetic field correction of the heading angle can be effectively utilized, and the heading angle error caused by the magnetic field interference can be reduced.

[0231] In order to better implement the attitude estimation method of the present application, the present application also provides an attitude estimation device based on the above attitude estimation method. The meanings of the terms are the same as in the above attitude estimation method, and the specific implementation details can be referred to the description in the method embodiment.

[0232] Please refer to Figure 6 , Figure 6 is a structural schematic diagram of the attitude estimation device provided by the embodiment of the present application. The attitude estimation device can be specifically as follows:

[0233] The first determination module 301 is configured to determine the current attitude information corresponding to the current frame.

[0234] The screening module 302 is configured to, after the stable magnetic field initialization is completed, if the current magnetic field strength information of the current frame and the reference magnetic field strength information of the stable magnetic field satisfy a first preset condition, screen a reference frame from a preset frame set corresponding to the stable magnetic field according to the current attitude information, wherein the reference attitude information corresponding to the reference frame and the current attitude information satisfy a second preset condition.

[0235] The second determining module 303 is configured to determine target attitude information corresponding to the current attitude information according to the reference frame and the current magnetic field intensity information.

[0236] Optionally, in some embodiments of the present application, the determination of the target attitude information corresponding to the current attitude information according to the reference frame and the current magnetic field intensity information comprises:

[0237] determining reference state estimation information and a reference covariance matrix corresponding to the reference frame, the reference state estimation information comprising a posterior state estimation mean for the reference frame;

[0238] determining target state estimation information of the current frame according to the reference state estimation information, the reference covariance matrix and the current magnetic field intensity information;

[0239] resolving the target attitude information from the target state estimation information.

[0240] Optionally, in some embodiments of the present application, the determination of the target state estimation information of the current frame according to the reference state estimation information, the reference covariance matrix and the current magnetic field intensity information comprises:

[0241] determining a joint magnetic field intensity measurement model, the joint magnetic field intensity measurement model being constructed according to the current frame and the reference frame jointly;

[0242] determining residual information for magnetic field noise according to the joint magnetic field intensity measurement model and the current magnetic field intensity information;

[0243] determining target gain information according to the reference state estimation information and the reference covariance matrix;

[0244] determining the target state estimation information according to the residual information and the target gain information.

[0245] Optionally, in some embodiments of the present application, the current attitude information comprises roll angle information, pitch angle information and heading angle information;

[0246] The determination of the target gain information according to the reference state estimation information and the reference covariance matrix comprises:

[0247] obtaining transition state estimation information and a transition covariance matrix, the transition state estimation information being posterior estimation information of attitude states of the current frame for the roll angle information and the pitch angle information;

[0248] determine the target state estimation information according to the residual information and the target gain information;

[0249] The determining the target state estimation information according to the residual information and the target gain information comprises:

[0250] determining state offset information according to the residual information and the target gain information;

[0251] updating the transition state estimation information according to the state offset information to obtain the target state estimation information.

[0252] Optionally, in some embodiments of the present application, the obtaining the transition state estimation information and the transition covariance matrix comprises:

[0253] performing integral prediction processing according to angular velocity information corresponding to the current frame to obtain initial state estimation information and an initial covariance matrix;

[0254] determining a second covariance matrix for target noise according to an acceleration measurement model;

[0255] determining the transition state estimation information and the transition covariance matrix according to the initial state estimation information, the initial covariance matrix, the second covariance matrix, and acceleration information corresponding to the current frame.

[0256] Optionally, in some embodiments of the present application, the determining the target gain information according to the transition state estimation information, the transition covariance matrix, the reference state estimation information, and the reference covariance matrix comprises:

[0257] constructing a reference Jacobian matrix of the residual information for the reference frame according to the transition state estimation information and the reference state estimation information, and constructing a transition Jacobian matrix of the residual information for the current frame according to the transition state estimation information and the reference state estimation information, wherein the reference Jacobian matrix and the transition Jacobian matrix are both for the yaw angle information;

[0258] determining the target gain information according to the reference Jacobian matrix, the transition Jacobian matrix, the transition covariance matrix, and the reference covariance matrix.

[0259] Optionally, in some embodiments of the present application, the selecting the reference frame from the preset frame set corresponding to the stable magnetic field according to the current attitude information comprises:

[0260] determining a 3D grid corresponding to the stable magnetic field, each voxel in the 3D grid corresponding to reference attitude information of an attitude frame.

[0261] screening a reference voxel from the voxels according to the current attitude information and each reference attitude information;

[0262] setting an attitude frame corresponding to the reference voxel as the reference frame.

[0263] Optionally, in some embodiments of the present application, the voxels of the 3D grid are initially empty;

[0264] After determining the target attitude information corresponding to the current attitude information according to the reference frame and the current magnetic field intensity information, the method further comprises:

[0265] determining a voxel state of a target voxel corresponding to the target attitude information in the 3D grid;

[0266] if the voxel state is empty, filling the target voxel of the 3D grid.

[0267] Optionally, in some embodiments of the present application, the 3D grid takes roll angle information, pitch angle information and heading angle information as three coordinate axes;

[0268] The determination of the voxel state of the target voxel corresponding to the target attitude information in the 3D grid comprises:

[0269] extracting target roll angle information, target pitch angle information and target heading angle information from the target attitude information;

[0270] determining a target identification bit of the target attitude information in the 3D grid according to a voxel edge length of the 3D grid and the target roll angle information, the target pitch angle information and the target heading angle information;

[0271] determining the voxel state according to a voxel filling state of a target voxel corresponding to the target identification bit.

[0272] After the initialization of the stable magnetic field is completed, the first determination module 301 determines the current attitude information corresponding to the current frame if the current magnetic field intensity information of the current frame and the reference magnetic field intensity information of the stable magnetic field satisfy a first preset condition, the screening module 302 screens a reference frame from a preset frame set corresponding to the stable magnetic field according to the current attitude information, wherein the reference attitude information corresponding to the reference frame and the current attitude information satisfy a second preset condition, and the second determination module 303 determines the target attitude information corresponding to the current attitude information according to the reference frame and the current magnetic field intensity information.

[0273] In summary, the embodiment of the present application optimizes the attitude information by screening the reference frame and combining the reference frame, and improves the accuracy of attitude information estimation. Wherein, the accuracy of attitude information estimation is further improved by screening the reference frame and optimizing the attitude information in the stable magnetic field.

[0274] In addition, the present application also provides an electronic device, as shown in the figure, which shows the structural schematic diagram of the electronic device related to the present application, in particular: Figure 7

[0275] The electronic device can include a processor 401 with one or more processing cores, a memory 402 with one or more computer readable storage media, a power supply 403, and an input unit 404, etc. Those skilled in the art can understand that the structure of the electronic device shown in the figure does not constitute a limitation on the electronic device, and can include more or fewer components than the figure, or combine certain components, or different component arrangements. Among them: Figure 7

[0276] The processor 401 is the control center of the electronic device, which connects all parts of the electronic device through various interfaces and lines, executes the software programs and / or modules stored in the memory 402, and calls the data stored in the memory 402, executes various functions and processes data of the electronic device, and thus monitors the whole electronic device. Optionally, the processor 401 can include one or more processing cores; preferably, the processor 401 can integrate an application processor and a modem processor, wherein the application processor mainly processes the operating system, user interface and application program, etc., and the modem processor mainly processes wireless communication. It can be understood that the above-mentioned modem processor can also not be integrated into the processor 401.

[0277] The memory 402 can be used to store software programs and modules, and the processor 401 executes various functions and attitude estimation by running the software programs and modules stored in the memory 402. The memory 402 can mainly include a program storage area and a data storage area, wherein the program storage area can store the operating system, at least one application program required by the function (such as sound playing function, image playing function, etc.) and the like; the data storage area can store the data created according to the use of the electronic device and the like. In addition, the memory 402 can include a high-speed random access memory, and can also include a non-volatile memory, such as at least one magnetic disk storage device, flash memory device, or other volatile solid-state memory device. Accordingly, the memory 402 can also include a memory controller to provide the processor 401 with access to the memory 402.

[0278] ​​The electronic device also includes a power supply 403 for powering the various components. Preferably, the power supply 403 is logically connected to the processor 401 through a power management system, so that the power management system can manage charging, discharging, and power consumption management, etc. The power supply 403 can also include one or more DC or AC power sources, recharging systems, power supply device debugging circuits, power converters or inverters, power status indicators, etc.

[0279] The electronic device can also include an input unit 404, which can be used to receive input digital or character information, and generate keyboard, mouse, joystick, optical or trackball signal inputs related to user settings and function controls.

[0280] Although not shown, the electronic device can also include a display unit, etc., which will not be described here. In particular, in the present embodiment, the processor 401 in the electronic device will load one or more executable files corresponding to the processes of one or more application programs into the memory 402 according to the following instructions, and run the application programs stored in the memory 402 by the processor 401, so as to implement the steps in any of the posture estimation methods provided in the present application.

[0281] In the present application, the electronic device (terminal device) can also be an extended reality device. The extended reality device includes a natural glasses-shaped smart glasses, which at least has the functions of extended reality display, wearing state detection, biometric identification, human-computer interaction, data processing, etc. It includes a frame, a leg, a processor, a sensor, an optical display component, a microphone, a speaker, etc. The leg and the frame have a cavity inside, which contains circuits and electronic components. The sensor includes a camera, an eye tracker, an iris instrument, an IMU, a gyroscope, etc. The optical display component includes a micro projection light machine and an optical coupler. The micro projection light machine can be based on Micro-Oled, Micro-Led, LCOS, or LBS. The optical coupler can be an optical lens or an optical waveguide sheet. The processor can be an XR professional processor or a general-purpose processor. The frame and the leg are the supporting structure of the entire glasses. The leg has a certain elasticity, and the length and clamping force can be adjusted to fit different head shapes of users. The camera can take pictures of the user's hands, face, eyes, etc. The microphone can listen to the user's voice, and the processor can calculate and process various data.

[0282] The electronic device of the embodiments of this application determines the current attitude information corresponding to the current frame after the initialization of the stable magnetic field is completed, filters a reference frame from the preset frame set corresponding to the stable magnetic field according to the current attitude information, wherein the reference attitude information corresponding to the reference frame satisfies the second preset condition with the current attitude information, and determines the target attitude information corresponding to the current attitude information according to the reference frame and the current magnetic field intensity information.

[0283] The reference frame is filtered and the attitude information is optimized in combination with the reference frame, thereby improving the accuracy of the attitude information estimation. The filtering of the reference frame and the optimization of the attitude information are performed in the stable magnetic field, thereby further improving the accuracy of the attitude information estimation.

[0284] The specific implementation of the above operations can be referred to the foregoing embodiments, which will not be described herein.

[0285] Those skilled in the art can understand that all or part of the steps in the various methods of the above embodiments can be completed by instructions, or by related hardware controlled by the instructions, which can be stored in a computer readable storage medium and loaded and executed by a processor.

[0286] To this end, the present application provides a computer readable storage medium, which stores a computer program capable of being loaded by a processor to execute the steps in any one of the attitude estimation methods provided by the present application.

[0287] The specific implementation of the above operations can be referred to the foregoing embodiments, which will not be described herein.

[0288] The computer readable storage medium can include a read only memory (ROM), a random access memory (RAM), a magnetic disk or an optical disk, etc.

[0289] Since the instructions stored in the computer readable storage medium can execute the steps in any one of the attitude estimation methods provided by the present application, the beneficial effects of any one of the attitude estimation methods provided by the present application can be achieved, which will be described in detail in the foregoing embodiments, which will not be described herein.

[0290] The above describes in detail the posture estimation method, device, electronic device, and computer readable storage medium provided by the present application. The principles and implementation manners of the present application are described by using specific examples. The above example description is only used to help understand the method of the present application and its core idea. Meanwhile, for those skilled in the art, according to the idea of the present application, the specific implementation manners and application ranges can be changed. In conclusion, the content of the specification should not be understood as a limitation of the present application.

Claims

1. A pose estimation method, characterized in that, The method includes: Determine the current attitude information corresponding to the current frame; After the stable magnetic field initialization is completed, if the current magnetic field strength information of the current frame and the reference magnetic field strength information of the stable magnetic field satisfy the first preset condition, then a reference frame is selected from the preset frame set corresponding to the stable magnetic field according to the current attitude information, wherein the reference attitude information corresponding to the reference frame and the current attitude information satisfy the second preset condition. Determine the target attitude information corresponding to the current attitude information based on the reference frame and the current magnetic field strength information; Determining the target attitude information corresponding to the current attitude information based on the reference frame and the current magnetic field strength information includes: Determine the reference state estimation information and reference covariance matrix corresponding to the reference frame. The reference state estimation information includes the posterior state estimation mean of the attitude state for the roll angle information, pitch angle information and yaw angle information of the reference frame. The reference covariance matrix reflects the error propagation characteristics between the roll angle information, pitch angle information, yaw angle information and gyroscope zero bias at the reference frame time. A joint magnetic field strength measurement model is determined, which is constructed jointly based on the current frame and the reference frame. The joint magnetic field strength measurement model is expressed as follows: ； in, Indicates the current frame The measured value of the magnetic field strength, Indicates reference frame The measured value of the magnetic field strength, Indicates the current frame A body coordinate system with three mutually orthogonal axes To the world coordinate system rotation, Indicates reference frame A body coordinate system with three mutually orthogonal axes To the world coordinate system rotation, This indicates an external magnetic field disturbance. Indicates magnetometer noise; Based on the joint magnetic field strength measurement model and the current magnetic field strength information, residual information for magnetic field noise is determined; The target gain information is determined based on the reference state estimation information and the reference covariance matrix; The target state estimation information is determined based on the residual information and the target gain information; The target attitude information is parsed from the target state estimation information.

2. The attitude estimation method according to claim 1, characterized in that, The current attitude information includes roll angle information, pitch angle information, and yaw angle information; Determining the target gain information based on the reference state estimation information and the reference covariance matrix includes: The transition state estimation information and the transition covariance matrix are obtained. The transition state estimation information is the posterior estimation information of the attitude state of the current frame in relation to the roll angle information and the pitch angle information. The transition covariance matrix reflects the error propagation characteristics between the roll angle information and the pitch angle information at the current frame time. The target gain information is determined based on the transition state estimation information, the transition covariance matrix, the reference state estimation information, and the reference covariance matrix; Determining the target state estimation information based on the residual information and the target gain information includes: The state offset information is determined based on the residual information and the target gain information; The transition state estimation information is updated based on the state offset information to obtain the target state estimation information.

3. The attitude estimation method according to claim 2, characterized in that, The acquisition of transition state estimation information and transition covariance matrix includes: Based on the angular velocity information corresponding to the current frame, integral prediction processing is performed to obtain initial state estimation information and initial covariance matrix; Determine the second covariance matrix for the target noise based on the acceleration measurement model; The transition state estimation information and the transition covariance matrix are determined based on the initial state estimation information, the initial covariance matrix, the second covariance matrix, and the acceleration information corresponding to the current frame.

4. The attitude estimation method according to claim 2, characterized in that, Determining the target gain information based on the transition state estimation information, the transition covariance matrix, the reference state estimation information, and the reference covariance matrix includes: The reference Jacobian matrix of the residual information for the reference frame is constructed based on the transition state estimation information and the reference state estimation information, and the transition Jacobian matrix of the residual information for the current frame is constructed based on the transition state estimation information and the reference state estimation information, wherein both the reference Jacobian matrix and the transition Jacobian matrix are for the heading angle information; The target gain information is determined based on the reference Jacobian matrix, the transition Jacobian matrix, the transition covariance matrix, and the reference covariance matrix.

5. The attitude estimation method according to claim 1, characterized in that, The step of selecting reference frames from the preset frame set corresponding to the stable magnetic field based on the current attitude information includes: The 3D mesh corresponding to the stable magnetic field is determined, and each voxel in the 3D mesh corresponds to the reference attitude information of an attitude frame. Reference voxels are selected from the voxels based on the current pose information and each of the reference pose information; Set the pose frame corresponding to the reference voxel as the reference frame.

6. The attitude estimation method according to claim 1, characterized in that, The voxels of the 3D mesh are initially empty; After determining the target attitude information corresponding to the current attitude information based on the reference frame and the current magnetic field strength information, the method further includes: Determine the voxel state of the target voxel corresponding to the target pose information in the 3D mesh; If the voxel state is empty, then the target voxel of the 3D mesh is filled.

7. The attitude estimation method according to claim 6, characterized in that, The 3D mesh uses roll angle information, pitch angle information, and yaw angle information as three coordinate axes; Determining the voxel state of the target voxel corresponding to the target pose information in the 3D mesh includes: The target roll angle, target pitch angle, and target heading angle are extracted from the target attitude information. Based on the voxel side length of the 3D mesh and the target roll angle, target pitch angle and target heading angle, the target attitude information corresponding to the target identifier in the 3D mesh is determined; The voxel state is determined based on the voxel filling state of the target voxel corresponding to the target identifier bit.

8. An attitude estimation device, characterized in that, The device includes: The first determining module is used to determine the current attitude information corresponding to the current frame; The filtering module is used to filter reference frames from the preset frame set corresponding to the stable magnetic field according to the current attitude information after the stable magnetic field initialization is completed, if the current magnetic field strength information of the current frame and the reference magnetic field strength information of the stable magnetic field satisfy a first preset condition. The reference attitude information corresponding to the reference frame and the current attitude information satisfy a second preset condition. The second determining module is used to determine the target attitude information corresponding to the current attitude information based on the reference frame and the current magnetic field strength information. Determining the target attitude information corresponding to the current attitude information based on the reference frame and the current magnetic field strength information includes: Determine the reference state estimation information and reference covariance matrix corresponding to the reference frame. The reference state estimation information includes the posterior state estimation mean of the attitude state for the roll angle information, pitch angle information and yaw angle information of the reference frame. The reference covariance matrix reflects the error propagation characteristics between the roll angle information, pitch angle information, yaw angle information and gyroscope zero bias at the reference frame time. A joint magnetic field strength measurement model is determined, which is constructed jointly based on the current frame and the reference frame. The joint magnetic field strength measurement model is expressed as follows: ； in, Indicates the current frame The measured value of the magnetic field strength, Indicates reference frame The measured value of the magnetic field strength, Indicates the current frame A body coordinate system with three mutually orthogonal axes To the world coordinate system rotation, Indicates reference frame A body coordinate system with three mutually orthogonal axes To the world coordinate system rotation, This indicates an external magnetic field disturbance. Indicates magnetometer noise; Based on the joint magnetic field strength measurement model and the current magnetic field strength information, residual information for magnetic field noise is determined; The target gain information is determined based on the reference state estimation information and the reference covariance matrix; The target state estimation information is determined based on the residual information and the target gain information; The target attitude information is parsed from the target state estimation information.

9. An electronic device, characterized in that, It includes a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the steps of the attitude estimation method as described in any one of claims 1-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, implements the steps of the attitude estimation method as described in any one of claims 1-7.

Images