WO2023220972A1 - Mobile device, pose estimation method and apparatus therefor, and storage medium - Google Patents

Abstract

The present disclosure relates to the technical field of mobile robots, and specifically provides a mobile device, a pose estimation method and apparatus therefor, and a storage medium. The pose estimation method comprises: obtaining a first pose set and a second pose set by means of a first sensor and a second sensor of a mobile device; determining a first pose change rate according to the first pose set, determining a second pose change rate according to the second pose set, and according to a difference between the first pose change rate and the second pose change rate, determining a first pose weight value and a second pose weight value of the mobile device at the current time; and according to the first pose weight value and the second pose weight value, performing fusion processing on first pose data and second pose data at the current time to obtain a target pose. In the implementation of the present disclosure, the weight values of the sensors can be dynamically adjusted, so that the influence of signal interference is reduced, and the pose estimation accuracy is improved.

Description

Mobile device and pose estimation method, device and storage medium thereof Technical field

The present disclosure relates to the technical field of mobile robots, and specifically to a mobile device and its pose estimation method, device, and storage medium.

Background technique

The pose estimation of a mobile device is an important prerequisite for autonomous navigation. In related technologies, an inertial measurement unit (IMU) can usually be used to obtain the pose information of a mobile device through a multi-sensor fusion method. However, in related technologies, the autonomous navigation system of mobile devices is susceptible to interference and influence from complex environments, which causes the mobile device's pose estimation to drift and affects the pose estimation accuracy.

Contents of the invention

In order to improve the pose estimation accuracy of a mobile device, embodiments of the present disclosure provide a mobile device and its pose estimation method, device, and storage medium.

In a first aspect, embodiments of the present disclosure provide a pose estimation method, which is applied to mobile devices. The method includes:

The first pose set of the mobile device at the current moment is obtained through the first sensor of the mobile device, and the second pose set of the mobile device at the current moment is obtained through the second sensor of the mobile device; wherein, The first pose set and the second pose set include pose data at the current moment and several previous sampling moments;

Determine a first posture change rate of the mobile device based on the first posture set, and determine a second posture change rate of the mobile device based on the second posture set;

Determine the first posture weight and the second posture weight of the mobile device at the current moment based on the difference between the first posture change rate and the second posture change rate;

According to the first posture weight and the second posture weight, the first posture data and the second posture data at the current moment are fused to obtain the target posture of the mobile device at the current moment.

In some embodiments, obtaining the first attitude set of the mobile device at the current moment through the first sensor of the mobile device includes:

For each sampling moment, obtain the first motion data at the sampling moment based on the first sensor collection;

Determine the first attitude change data of the mobile device according to the first motion data at the sampling time, the first motion data at the previous sampling time, and the sampling period;

Determine the first attitude data of the mobile device at the sampling moment based on the first attitude data of the previous sampling moment and the first attitude change data;

The first attitude set is obtained based on the first attitude data at the current time and the first attitude data at a preset number of sampling times before the current time.

In some embodiments, obtaining the second pose set of the mobile device at the current moment through the second sensor of the mobile device includes:

For each sampling moment, obtain the second motion data of the sampling moment based on the second sensor collection;

Determine the second pose data of the mobile device at the sampling moment according to the second motion data;

The second pose set is obtained based on the second pose data at the current time and the second pose data at a preset number of sampling times before the current time.

In some embodiments, the first pose change rate of the mobile device is determined based on the first pose set, and the second pose change rate of the mobile device is determined based on the second pose set, include:

Perform boundary numerical processing based on each first pose data included in the first pose set to obtain a numerically continuous first target set; perform boundary processing based on each second pose data included in the second pose set Data processing to obtain a numerically continuous second target set;

Perform linear regression based on each element included in the first target set to obtain the first slope of the first straight line corresponding to the first target set; perform linear regression based on each element included in the second target set to obtain the the second slope of the second straight line corresponding to the first target set;

The first slope is determined as the first posture change rate, and the second slope is determined as the second posture change rate.

In some embodiments, the process of performing the boundary numerical processing on any one of the first pose set and the second pose set includes:

In response to each element in the pose set including both positive and negative values, calculate an average of the absolute values of all elements;

Determine the target boundary corresponding to the pose set according to the relationship between the first absolute value of the difference between the average value and the first boundary, and the magnitude relationship between the second absolute value of the difference between the average value and the second boundary;

In response to the target boundary being the preset boundary among the first boundary and the second boundary, numerical conversion is performed on each element in the pose set to obtain a numerically continuous target set.

In some embodiments, in response to the target boundary being a preset boundary among the first boundary and the second boundary, numerical conversion is performed on each element in the pose set to obtain a numerically continuous target set. ,include:

In response to the target boundary being a preset boundary among the first boundary and the second boundary, determining a first number of positive values and a second number of negative values in the pose set;

In response to the first quantity being greater than the second quantity, increasing all negative element values by a preset value to obtain the target set;

In response to the first quantity not being greater than the second quantity, the element value of the positive value is reduced by a preset value to obtain the target set.

In some embodiments, determining the first posture weight of the mobile device at the current moment based on the difference between the first posture change rate and the second posture change rate includes:

In response to the difference between the first attitude change rate and the second attitude change rate being greater than the preset threshold, Kalman filtering is performed on the variance of the first attitude data at several sampling moments before the current moment to obtain the current moment. The first position weight.

In some implementations, the second sensor includes a magnetometer; determining the second pose weight of the mobile device at the current moment includes:

Using the magnetometer to collect the second pose data at the current moment;

The geomagnetic mode length is calculated based on the second pose data at the current moment;

The second pose weight is determined according to the geomagnetic mode length.

In some embodiments, the first posture data and the second posture data at the current moment are fused according to the first posture weight value and the second posture weight value to obtain the movement The target pose of the device at the current moment, including:

According to the first posture weight and the second posture weight, Kalman filtering is performed on the first posture data and the second posture data at the current moment to obtain the target position of the mobile device at the current moment. posture.

In some embodiments, the first sensor includes a gyroscope and the second sensor includes a magnetometer.

In a second aspect, embodiments of the present disclosure provide a pose estimation device, which is applied to mobile devices. The device includes:

The acquisition module is configured to obtain the first attitude set of the mobile device at the current moment through the first sensor of the mobile device, and obtain the second attitude set of the mobile device at the current moment through the second sensor of the mobile device. A pose set; wherein the first pose set and the second pose set include pose data at the current moment and several previous sampling moments;

a change rate determination module configured to determine a first posture change rate of the mobile device according to the first posture set, and determine a second posture change rate of the mobile device according to the second posture set;

A weight determination module configured to determine the first posture weight and the second posture weight of the mobile device at the current moment based on the difference between the first posture change rate and the second posture change rate. value;

The data fusion module is configured to perform fusion processing on the first posture data and the second posture data at the current moment according to the first posture weight value and the second posture weight value to obtain the mobile device. The target pose at the current moment.

In some implementations, the acquisition module is configured to:

For each sampling moment, obtain the first motion data at the sampling moment based on the first sensor collection;

Determine the first attitude change data of the mobile device according to the first motion data at the sampling time, the first motion data at the previous sampling time, and the sampling period;

Determine the first attitude data of the mobile device at the sampling moment based on the first attitude data of the previous sampling moment and the first attitude change data;

The first attitude set is obtained based on the first attitude data at the current time and the first attitude data at a preset number of sampling times before the current time.

In some implementations, the acquisition module is configured to:

For each sampling moment, obtain the second motion data of the sampling moment based on the second sensor collection;

Determine the second pose data of the mobile device at the sampling moment according to the second motion data;

The second pose set is obtained based on the second pose data at the current time and the second pose data at a preset number of sampling times before the current time.

In some embodiments, the rate of change determination module is configured to:

Perform boundary numerical processing based on each first pose data included in the first pose set to obtain a numerically continuous first target set; perform boundary processing based on each second pose data included in the second pose set Data processing to obtain a numerically continuous second target set;

Perform linear regression based on each element included in the first target set to obtain the first slope of the first straight line corresponding to the first target set; perform linear regression based on each element included in the second target set to obtain the the second slope of the second straight line corresponding to the first target set;

The first slope is determined as the first posture change rate, and the second slope is determined as the second posture change rate.

In some embodiments, the rate of change determination module is configured to:

In response to each element in the pose set including both positive and negative values, calculate an average of the absolute values of all elements;

Determine the target boundary corresponding to the pose set according to the relationship between the first absolute value of the difference between the average value and the first boundary, and the magnitude relationship between the second absolute value of the difference between the average value and the second boundary;

In response to the target boundary being the preset boundary among the first boundary and the second boundary, numerical conversion is performed on each element in the pose set to obtain a numerically continuous target set.

In some embodiments, the rate of change determination module is configured to:

In response to the target boundary being a preset boundary among the first boundary and the second boundary, determining a first number of positive values and a second number of negative values in the pose set;

In response to the first quantity being greater than the second quantity, increasing all negative element values by a preset value to obtain the target set;

In response to the first quantity not being greater than the second quantity, the element value of the positive value is reduced by a preset value to obtain the target set.

In some implementations, the weight determination module is configured to:

In response to the difference between the first attitude change rate and the second attitude change rate being greater than the preset threshold, Kalman filtering is performed on the variance of the first attitude data at several sampling moments before the current moment to obtain the current moment. The first position weight.

In some implementations, the second sensor includes a magnetometer; the weight determination module is configured to:

Using the magnetometer to collect the second pose data at the current moment;

The geomagnetic mode length is calculated based on the second pose data at the current moment;

The second pose weight is determined according to the geomagnetic mode length.

In some embodiments, the data fusion module is configured to:

According to the first posture weight and the second posture weight, Kalman filtering is performed on the first posture data and the second posture data at the current moment to obtain the target position of the mobile device at the current moment. posture.

In some embodiments, the first sensor includes a gyroscope and the second sensor includes a magnetometer.

In a third aspect, an embodiment of the present disclosure provides a mobile device, including:

a first sensor and a second sensor;

processor; and

A memory stores computer instructions for causing the processor to execute the method according to any implementation manner of the first aspect.

In a fourth aspect, an embodiment of the present disclosure provides a storage medium that stores computer instructions, and the computer instructions are used to cause a computer to execute the method according to any embodiment of the first aspect.

The pose estimation method in the embodiment of the present disclosure includes obtaining the first pose set at the current time through the first sensor and determining the first pose change rate, and obtaining the second pose set at the current time through the second sensor and determining the second pose estimation method. The pose change rate, based on the difference between the first pose change rate and the second pose change rate, determines the first pose weight and the second pose weight, based on the first pose weight and the second pose weight The weight fuses the first pose data and the second pose data at the current moment to obtain the target pose of the mobile device at the current moment. In the embodiment of the present disclosure, by comparing the changing trends of the sampling data of the first sensor and the second sensor within a current period of time, the reliability of the two at the current moment is determined, and the pose weights of the two are dynamically adjusted based on the reliability. This can effectively eliminate or alleviate the defects of data distortion caused by signal interference, and improve the accuracy of the target pose obtained after data fusion.

Description of the drawings

In order to more clearly explain the specific embodiments of the present disclosure or the technical solutions in the prior art, the drawings that need to be used in the description of the specific embodiments or the prior art will be briefly introduced below. Obviously, the drawings in the following description The drawings illustrate some embodiments of the present disclosure. For those of ordinary skill in the art, other drawings can be obtained based on these drawings without exerting creative efforts.

Figure 1 is a structural block diagram of a mobile device according to some embodiments of the present disclosure.

Figure 2 is a flowchart of a pose estimation method according to some embodiments of the present disclosure.

Figure 3 is a flowchart of a pose estimation method according to some embodiments of the present disclosure.

Figure 4 is a flowchart of a pose estimation method according to some embodiments of the present disclosure.

Figure 5 is a flowchart of a pose estimation method according to some embodiments of the present disclosure.

Figure 6 is a schematic diagram of a pose estimation method according to some embodiments of the present disclosure.

Figure 7 is a flowchart of a pose estimation method according to some embodiments of the present disclosure.

Figure 8 is a flowchart of a pose estimation method according to some embodiments of the present disclosure.

Figure 9 is a flowchart of a pose estimation method according to some embodiments of the present disclosure.

Figure 10 is a structural block diagram of a pose estimation device according to some embodiments of the present disclosure.

Detailed ways

The technical solution of the present disclosure will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are part of the embodiments of the present disclosure, but not all of them. Based on the embodiments in this disclosure, all other embodiments obtained by those of ordinary skill in the art without creative efforts fall within the scope of protection of this disclosure. In addition, the technical features involved in different embodiments of the present disclosure described below can be combined with each other as long as they do not conflict with each other.

The pose estimation of a mobile device is an important prerequisite for autonomous navigation. In related technologies, an inertial measurement unit (IMU) can usually be used to obtain the pose information of a mobile device through a multi-sensor fusion method.

For example, taking the pose estimation scenario of a ground mobile robot as an example, in a simple implementation, the robot's pose can be directly integrated over time through the robot's gyroscope, accelerometer and other inertial measurement sensors. This method is feasible in a short time, but after the robot moves for a long time, the drift of the gyroscope itself and the influence of the body vibration on the gyroscope will cause distortion of the pose data.

In order to solve the problem of gyroscope distortion, in related technologies, other types of sensor data can be integrated to continuously correct the gyroscope to avoid gyroscope drift. However, methods in related technologies are difficult to satisfy the pose estimation of complex scenes.

For example, taking the fusion of magnetometer and gyroscope as an example, this method is suitable for scenarios where the magnetic field environment of the body is single and there is no electromagnetic interference, such as high-altitude flight scenarios of drones. For ground mobile robots, the surrounding magnetic field environment is complex and changeable, which will have a great impact on the magnetometer, resulting in inaccurate estimation of the robot's pose.

As another example, take the fusion of GPS satellite positioning and gyroscope as an example. This method is suitable for scenarios where the body is outdoors, such as outdoor robots, drones, etc. For robots that move indoors, the GPS signal is weak, the positioning is inaccurate, and it cannot provide accurate pose correction, resulting in inaccurate pose estimation.

Therefore, in related technologies, in order to achieve accurate posture estimation of indoor robots, more types and quantities of sensor data can only be integrated, doubling the cost of the machine and the complexity of the algorithm.

Based on the deficiencies in the above related technologies, embodiments of the present disclosure provide a mobile device and its pose estimation method, device, and storage medium, aiming to improve the pose estimation accuracy of the mobile device and reduce the impact of complex environments on sensor accuracy. Impact, without the need to fuse more sensor data, the current environmental conditions of the device can be judged and identified, and the fusion algorithm can be adjusted accordingly to improve the pose estimation accuracy of the device in complex environments.

The pose estimation method in the embodiment of the present disclosure can be applied to mobile devices. Mobile devices refer to electronic devices that can be driven to produce position movement, such as mobile robots, drones, etc. Figure 1 shows a structural block diagram of a mobile device in some embodiments of the present disclosure, which will be described below with reference to Figure 1 .

As shown in Figure 1, in some implementations, a mobile device 600 of an example of the present disclosure includes a processor 601, a memory 602, a first sensor 604, a second sensor 605, and a driving device 606.

The processor 601, the memory 602, the first sensor 604, the second sensor 605 and the driving device 606 establish a communicable connection between any two through the bus 603.

The processor 601 can be any type of processor with one or more processing cores. It can perform single-threaded or multi-threaded operations and is used to parse instructions to perform operations such as obtaining data, performing logical operation functions, and issuing operation processing results.

Memory 602 may include non-volatile computer-readable storage media, such as at least one disk storage device, flash memory device, distributed storage device remotely located relative to processor 601, or other non-volatile solid-state storage device. The memory may have a program storage area for storing non-volatile software programs, non-volatile computer executable programs and modules for the processor 601 to call to cause the processor 601 to perform one or more method steps. The memory 602 may also include a storage part such as a volatile random access storage medium or a hard disk as a data storage area for storing operation processing results and data output by the processor 601.

The first sensor 604 may be an inertial measurement unit provided on the mobile device. For example, the first sensor 604 may include a gyroscope, an accelerometer, an angular velocity meter, etc. During the movement of the mobile device 600, the first sensor 604 may detect the mobile device. The first pose data.

The second sensor 605 may be a different type of measurement unit than the first sensor 604 provided on the mobile device. For example, the second sensor 605 may include a magnetometer. During the movement of the mobile device 600, the second sensor 605 may detect the mobile device. The second pose data.

It can be understood that in the embodiment of the disclosure, the second sensor 605 is used to correct the observation data of the first sensor 604 to obtain more accurate pose data. The process of data fusion will be specifically described in the method below of the disclosure. .

The driving device 606 is the power system of the mobile device and is used to drive the mobile device 600 to generate displacement. In some embodiments, the driving device 606 may include mechanical structures such as a motor, a transmission mechanism, and a roller. Those skilled in the art will undoubtedly understand and fully implement this, and will not be described in detail in this disclosure.

On the basis of the mobile device shown in Figure 1, embodiments of the present disclosure provide a pose estimation method for real-time estimation of the pose of the mobile device 600 when it is moving. This method can be used by the processor of the mobile device 600. 601 is executed, which will be described below with reference to the implementation in Figure 2 .

As shown in Figure 2, in some implementations, the pose estimation method of the present disclosure includes:

S210. Obtain the first pose set of the mobile device at the current moment through the first sensor of the mobile device, and obtain the second pose set of the mobile device at the current moment through the second sensor of the mobile device.

It can be understood that when the mobile device is moving, the first sensor and the second sensor will collect the pose data at the current moment in a fixed sampling period, so that the target pose at the current moment can be obtained in real time through a multi-sensor fusion algorithm. For example, in one example, the sampling frequency of the first sensor and the second sensor is 10 Hz, that is, pose data is collected 10 times per second, so the processor of the mobile device also performs pose estimation 10 times per second.

In the embodiment of the present disclosure, when solving the target pose at the current moment, data fusion is not directly based on the pose data of the first sensor and the second sensor. This is because, considering that in a complex environment, the detection signals of the first sensor and/or the second sensor will be interfered, resulting in large errors in the collected pose data. For example, taking the magnetometer as an example, in a scene with a complex magnetic field environment, the detection signal of the magnetometer is greatly interfered, resulting in a large deviation between the pose data collected by the magnetometer and the actual pose, which in turn causes the data to be fused. There is a large error in the obtained target pose.

Therefore, in the embodiment of the present disclosure, before solving the target pose at the current moment, first the environmental conditions of the mobile robot are currently judged and identified, thereby determining whether the working status of the first sensor and the second sensor is reliable. And based on the current reliability of the two, the weight parameters of the first pose data and the second pose data during the data fusion process are dynamically adjusted in real time to ensure the accuracy of the target pose of the mobile device obtained by data fusion and reduce Interference of environmental factors on the sensor.

Specifically, at each sampling moment, the first attitude data of each sampling moment can be calculated based on the sampling data of the first sensor. Similarly, the second attitude data of each sampling moment can be calculated based on the sampling data of the second sensor. pose data.

In the embodiment of the present disclosure, taking the current time t as an example, firstly, the first pose set can be established based on the first pose data of the current time t and several previous sampling moments, which can be expressed as:

In formula (1),

represents the first pose set,

Represents the first pose data at the current time t,

Represents the first attitude data at the sampling time t-1... and so on, so that the first attitude set includes a total of N first attitude data at the sampling time.

Secondly, the second pose set can be established based on the second pose data of the current time t and several previous sampling moments, expressed as:

In formula (2),

represents the second pose set,

Represents the second pose data at the current time t,

Represents the second pose data at the sampling time t-1... and so on, so that the second pose set includes a total of N second pose data at the sampling time.

It is worth noting that the first pose set and the second pose set represent a set of pose data for a period of time before the current time. The embodiment of the present disclosure does not place a limit on the number N of elements in the set, which can be arbitrarily greater than 0. Positive integers, such as N=10, 20, 50, etc., this disclosure does not limit this.

S220. Determine the first posture change rate of the mobile device based on the first posture set, and determine the second posture change rate of the mobile device based on the second posture set.

It can be understood that the first pose set represents the pose data of the mobile device for a period of time before the current moment calculated based on the data collected by the first sensor, which can reflect the changing trend of the pose data sampled by the first sensor.

Similarly, the first pose set represents the pose data of the mobile device for a period of time before the current moment calculated based on the data collected by the second sensor, which can reflect the changing trend of the pose data sampled by the second sensor.

The first pose set and the second pose set correspond to the movement process of the mobile device within the same period of time. Therefore, in the absence of signal interference, theoretically the change trends of the two should be consistent or close. And if one or both parties are subject to signal interference, the changing trends of the two should be quite different.

Based on this principle, in the embodiment of the present disclosure, the first posture change rate can be used to reflect the change trend of each first posture data in the first posture set, and the first posture change rate can be used to reflect each second posture data in the second posture set. The difference between the second posture change rate of the change trend determines whether the current first sensor and/or the second sensor are in a reliable state.

In some embodiments, linear regression processing can be performed based on each first attitude data included in the first attitude set, the first straight line is obtained by fitting, and the first slope of the first straight line is used as the first attitude change. Rate. Similarly, linear regression processing can be performed based on each second pose data included in the second pose set, a second straight line is obtained by fitting, and the second slope of the second straight line is used as the second pose change rate. The present disclosure will be specifically described in the following embodiments and will not be described in detail here.

S230. Determine the first posture weight and the second posture weight of the mobile device at the current moment based on the difference between the first posture change rate and the second posture change rate.

Based on the foregoing, it can be seen that the first attitude change rate can reflect the changing trend of the sampling data of the first sensor, and the second attitude change rate can reflect the changing trend of the sampling data of the second sensor.

In some embodiments, if the difference between the first posture change rate and the second posture change rate is very small, it means that the trends of the posture data calculated by the first sensor and the second sensor are the same, and it also indicates that the current trend is the same. Mobile devices operate in an environment with few distractions.

In other embodiments, if the difference between the first posture change rate and the second posture change rate is large, it means that the trends of the posture data calculated by the first sensor and the second sensor are not the same, or Indicates that the current environment where the mobile device is located is very disruptive.

In the embodiment of the present disclosure, different pose weights of the first sensor and the second sensor can be dynamically adjusted based on the above different situations. The pose weights can be understood as the influence of the sensor's pose data on the target pose.

For example, in an example, during the data fusion process, if the weight of the first pose is larger and the weight of the second pose is smaller, the calculated target pose is more inclined to the first pose data; conversely, if the weight of the second pose is smaller The weight of one pose is very small, the weight of the second pose is large, and the calculated target pose is more inclined to the second pose data.

Thus, in some embodiments, it is assumed that the first sensor is a gyroscope and the second sensor is a magnetometer. If the difference between the first pose change rate and the second pose change rate is very small, it means that the signal interference of the environment at the current moment is very small, and only the second pose weight of the second pose data can be adjusted. This is due to the higher reliability of the gyroscope relative to the magnetometer when neither the gyroscope nor the magnetometer is disturbed.

If the difference between the first attitude change rate and the second attitude change rate is large, indicating that the signal interference of the environment at the current moment is large, the first attitude weight and the second attitude weight of the first attitude data can be adjusted at the same time. The second pose weight of the pose data reduces the impact of signal interference.

The process of determining the first posture weight and the second posture weight will be described in detail in the following embodiments of the present disclosure, which will not be elaborated here.

S240. According to the first posture weight and the second posture weight, fuse the first posture data and the second posture data at the current moment to obtain the target position of the mobile device at the current moment.

Based on the above, it can be seen that for the current time t, the first posture data can be calculated through the data collected by the first sensor, and the second posture data can be calculated through the data collected by the second sensor. After the first pose weight and the second pose weight corresponding to the two are determined through the aforementioned process, the first pose data and the second pose data can be fused based on the pose weight, so that Determine the target pose of the mobile device at the current time t.

In some implementations, a Kalman filter method can be used to fuse the first pose data and the second pose data to obtain the target pose. This will be described in the following embodiments of the present disclosure and will not be described in detail here.

As can be seen from the above, in the embodiment of the present disclosure, the reliability of the first sensor and the second sensor at the current moment is determined by comparing the changing trends of the sampling data of the first sensor and the second sensor, and the positions of the two are dynamically adjusted based on the reliability. pose weights, which can effectively eliminate or alleviate the defects of data distortion caused by signal interference, and improve the accuracy of the target pose obtained after data fusion.

It is worth explaining that the pose of an object in three-dimensional space can be represented by three-dimensional position coordinates (x, y, z) and three-dimensional attitude (ψ _roll , ψ _pitch , ψ _yaw ), ψ _roll represents the roll angle, and ψ _pitch represents Pitch angle, ψ _yaw represents the yaw angle.

For the three-dimensional attitude estimation of ground mobile equipment, the roll angle ψ _roll and pitch angle ψ _pitch themselves have very small motion amplitudes, and can be corrected by the accelerometer. The accelerometer is almost not affected by environmental factors, and the obtained pose results can be compared It converges well, so the difficulty in its three-dimensional pose estimation lies in the estimation of the yaw angle ψ _yaw .

In the following embodiments of the disclosure, the pose estimation method of the embodiments of the disclosure will be described by taking the pose estimation of the yaw angle ψ _yaw of the ground mobile device as an example. However, it should be understood that the pose estimation method in the embodiment of the present disclosure is not limited to this, and can be applied to any other pose estimation, which will not be described in detail in the present disclosure.

In addition, in the following embodiments, the first sensor of the mobile device is a gyroscope, and the second sensor is a magnetometer. The specific working principles of gyroscopes and magnetometers can undoubtedly be understood and fully implemented by those skilled in the art, and will not be described in detail in this disclosure.

As shown in Figure 3, in some implementations, according to the pose estimation method of the present disclosure, the process of obtaining the first pose set includes:

S310. For each sampling moment, based on the first motion data at the sampling moment collected by the first sensor.

It can be understood that the first sensor collects motion data of the mobile device every fixed sampling period, which is defined as the first motion data. For example, in the embodiment of the disclosure, the first sensor is a gyroscope, and the gyroscope samples the yaw (yaw) angle data of the mobile device every 100 ms, which is the first motion data described in the disclosure.

Taking the sampling time t as an example, the first motion data collected by the first sensor can be expressed as ω _z,t , that is, the angular velocity of the yaw angle of the fuselage at the sampling time t.

S320. Determine the first posture change data of the mobile device based on the first motion data at the sampling time, the first motion data at the previous sampling time, and the sampling period.

S330. Determine the first attitude data of the mobile device at the sampling time based on the first attitude data and the first attitude change data at the previous sampling time.

After the first motion data of the current moment is obtained through the first sensor sampling, the motion information of the current moment needs to be accumulated to the pose result of the previous moment, so as to update the pose information in real time.

Specifically, still taking the sampling time t as an example, the corresponding first motion data is ω _{z, t} . The previous sampling time is t-1, and the corresponding first motion data is ω _{z, t-1.} , then the first attitude data corresponding to the sampling time t is expressed as:

In formula (3),

Represents the yaw angle of the fuselage at the sampling time t obtained by gyroscope integration, that is, the first attitude data.

Represents the yaw angle of the fuselage at the previous sampling time t-1. Δt represents the sampling period of the gyroscope.

Referring to the above formula (3), ω _z,t represents the angular velocity of the yaw angle sampled at time t, and ω _z,t-1 represents the angular velocity of the yaw angle sampled at time t-1. thereby

That is, it represents the average angular velocity of the fuselage yaw angle during the movement from time t-1 to time t. The product of it and the sampling period Δt is the movement angle of the fuselage yaw angle from time t-1 to time t, which is the disclosure The first posture change data described in the embodiment. Then the first attitude change data is accumulated into the yaw angle data at time t-1, and the yaw angle data at time t can be obtained.

For each sampling moment, the calculation is performed through the above formula (3), and the first attitude data corresponding to each sampling moment can be obtained.

S340. Obtain the first attitude set based on the first attitude data at the current time and the first attitude data at a preset number of sampling times before the current time.

In the embodiment of the present disclosure, it is necessary to construct the first posture set based on the first posture data for a period of time before the current time.

Taking the current time t as an example, you can obtain the current time t, the previous sampling time t-1, the first two sampling times t-1,..., the first N sampling times tN, and the first position corresponding to a total of N sampling times. pose data, the first pose set is obtained, expressed as shown in Equation (1):

As shown in Figure 4, in some implementations, according to the pose estimation method of the present disclosure, the process of obtaining the second pose set includes:

S410. For each sampling moment, obtain the second motion data at the sampling moment based on the second sensor collection.

It can be understood that the second sensor collects motion data of the mobile device every fixed sampling period, which is defined as second motion data. For example, in the embodiment of the present disclosure, the second sensor is a magnetometer, and the magnetometer samples the magnetic field vector and pose angle data around the mobile device every 100 ms, which is the second motion data described in the present disclosure.

S420. Determine the second pose data of the mobile device at the sampling time based on the second motion data.

In the embodiment of the present disclosure, the process of the magnetometer calculating the yaw angle at each sampling moment can be expressed as:

In formula (4),

represents the yaw angle of the fuselage calculated by the magnetometer, and B=[B _x , By _y , B _z ] ^T represents the magnetic field vector detected by the magnetometer. φ represents the fuselage roll angle, and θ represents the fuselage pitch angle, which can be obtained using, for example, the Mahony attitude solution algorithm.

For each sampling moment, calculation is performed through the above formula (4), and the second pose data corresponding to each sampling moment can be obtained.

S430. Obtain a second pose set based on the second pose data at the current time and the second pose data at a preset number of sampling times before the current time.

In the embodiment of the present disclosure, it is necessary to construct a second pose set based on the second pose data for a period of time before the current time.

Taking the current time t as an example, you can obtain the current time t, the previous sampling time t-1, the first two sampling times t-1,..., the first N sampling times tN, and the second bit corresponding to a total of N sampling times. pose data to obtain the second pose set, expressed as shown in Equation (2):

In some embodiments, after obtaining the first pose set and the second pose set, linear regression can be performed on the two pose sets respectively to obtain the slope corresponding to the fitting straight line, and the changing trend of the two poses is determined based on the slope. Are they consistent or similar. The following description will be made with reference to the embodiment of FIG. 5 .

As shown in Figure 5, in some embodiments, according to the pose estimation method of the present disclosure, the process of determining the first pose change rate and the second pose change rate includes:

S510. Perform boundary numerical processing based on each first pose data included in the first pose set to obtain a numerically continuous first target set; perform boundary processing based on each second pose data included in the second pose set to obtain A numerically consecutive second set of targets.

It should be noted that in the real moving scene of the mobile device, the movement process of the yaw angle is continuous, that is, the mobile device can continuously rotate within the range of 0° to 360°. However, in mathematical representation, the yaw angle range of mobile devices is -180° to 180°, which is shown in Figure 6, which will lead to discontinuity at the 180° boundary.

For example, the actual motion process of the mobile device from time t-N to time t is: rotation from 175° to 185°. This is continuous in terms of the objective movement of the device, but in terms of the mathematical expression shown in Figure 6, the movement process is: movement from 175° to 180°, and then movement from -180° to -175°. It can be seen that there is a boundary crossing from 180° to -180 in this process, resulting in discontinuity in mathematical expression.

The discontinuity of mathematical expression will lead to the discontinuity of each element in the pose set. In this way, before performing linear regression on the pose set, it is necessary to perform boundary numerical processing on the element values that have boundary crossings, so that each element in the pose set The values of the elements are consecutive.

For example, the elements in the pose set are expressed as: [175,177,179,-179,-177,-175]. The real movement process represented by this pose set is: the body moves from 175° to 185°, but the first pose set The elements in are not numerically continuous. Through boundary numerical processing, the pose set can be expressed as: [175,177,179,181,183,185], which means that each element in the pose set is numerically continuous.

After the above boundary numerical processing, the first pose set can be converted into the first target set, and the second pose set can be converted into the second target set. The process of performing boundary numerical processing on the first pose set and the second pose set will be specifically described in the embodiment of FIG. 7 below in this disclosure, and will not be described in detail here.

S520. Perform linear regression based on each element included in the first target set to obtain the first slope of the first straight line corresponding to the first target set; perform linear regression based on each element included in the second target set to obtain the first slope corresponding to the first target set. The second slope of the second straight line.

In the embodiment of the present disclosure, after performing boundary numerical processing, linear regression can be performed on the N elements included in each of the first target set and the second target set, and the corresponding first straight line and second straight line can be obtained by fitting.

In some implementations, the least squares method can be used to perform linear regression on the first target set and the second target set. Any one of the target sets will be used as an example for illustration.

First, linear regression is performed on N elements in the target set, expressed as:

A＝[X,I] (5)

In formula (5), X is a time vector, expressed as X=[0,1,...,N-1] ^T . I is an N*1 column matrix with all elements being 1, expressed as

Based on equation (5) we can get:

H＝(A ^T A) ^-1 A ^T Y (6)

In formula (6), Y=ψ _N , which is the element in the target set. H=[k,b] ^T , k represents the slope of the straight line obtained by fitting, and b represents the intercept of the straight line obtained by fitting.

Through the above process, linear regression can be performed on the first target set to obtain the first slope k ^g of the corresponding first straight line, and linear regression can be performed on the second target set to obtain the second slope k ^m of the corresponding second straight line.

S530. Determine the first slope as the first posture change rate, and determine the second slope as the second posture change rate.

In the embodiment of the present disclosure, the first slope k ^g is determined as the first pose change rate corresponding to the first pose set, and the second slope is determined as the second pose change rate corresponding to the second pose set.

It can be understood that the first attitude change rate is the slope of the first straight line fitted according to each first attitude data of the first attitude set, which can reflect each first attitude data in the first attitude set. changing trends. Similarly, the second pose change rate is the slope of the second straight line fitted according to each second pose data in the second pose set, which can reflect the change rate of each second pose data in the second pose set. Trend.

After obtaining the first posture change rate and the second posture change rate, the trend consistency of the two can be determined through the difference between the first posture change rate and the second posture change rate. In the following embodiments of the present disclosure, Provide a detailed explanation of this process.

It can be seen from the above that in the embodiment of the present disclosure, the changing trend of the data collected by the first sensor and the second sensor is reflected through the pose data for a period of time before the current time, so that the current environment of the mobile device can be judged, and the third sensor can be used to determine the current environment of the mobile device. The current reliability of the first sensor and the second sensor is quantitatively expressed.

As shown in Figure 7, in some embodiments, the process of performing boundary numerical processing on any one of the first pose set and the second pose set includes:

S710. In response to each element in the pose set including both positive and negative values, calculate the average of the absolute values of all elements.

As shown in Figure 6, for a certain pose set, if the elements included in it have both positive and negative values, it means that the values of each element in the pose set may be discontinuous.

For example, in an example, the pose set is expressed as [170,171,172,173,174,175], which means that the yaw angle of the mobile device is rotated from 170° to 175°. In this pose set, all element values are positive, and each element is numerically continuous.

For example, in an example, the pose set is represented as [-175,-174,-173,-172,-171,-170], which indicates that the yaw angle of the mobile device is rotated from 185° to 190°. In this pose set, all element values are negative, and each element is numerically continuous.

It can be seen that if all elements in the pose set are positive values, or all elements are negative values, it means that each element in the pose set is numerically continuous, that is, it does not cross the 0° boundary or the 180° boundary. , there is no need to perform boundary numerical processing, and the above linear regression process can be directly performed.

However, if each element in the pose set includes both positive and negative values, it does not mean that the values of each element must be discontinuous.

For example, in an example, the pose set is expressed as [175,177,179,-179,-177,-175], which means that the yaw angle of the mobile device is rotated from 175° to 185°. In this pose set, including both positive and negative values, the values of each element are not continuous because they cross the 180° boundary, and 181° will be represented as -179, resulting in discontinuous values.

For example, in an example, the pose set is represented as [-5,-3,-1,1,3,5], which means that the yaw angle of the mobile device is rotated from 355° to 5°. In this pose set, although it also includes both positive and negative elements, the value of each element is continuous because it spans the 0° boundary. Even if the value gradually changes from negative to positive, it is still continuously.

Therefore, in this step, it is first determined whether the values of each element in the pose set include both positive and negative values. If so, it is necessary to further determine whether the boundary spanned by the pose set is the 0° boundary or the 180° boundary. In the embodiment of the present disclosure, the 0° boundary is defined as the first boundary, and the 180° boundary is defined as the second boundary.

In the embodiment of the present disclosure, if the pose set includes both positive elements and negative elements, the absolute values of all elements can be taken and the average value can be calculated, expressed as:

In formula (7), aver_N represents the average of the absolute values of N elements in the pose set, and ψ _N represents each element in the pose set.

S720. Determine the target boundary corresponding to the pose set based on the relationship between the first absolute value of the difference between the average value and the first boundary, and the magnitude relationship between the second absolute value of the difference between the average value and the second boundary.

Based on the foregoing, it can be seen that when the pose set includes both positive elements and negative elements, it is necessary to further determine whether the pose set spans the first boundary or the second boundary. The first boundary is the 0° boundary, and the second boundary The boundary is the 180° boundary.

Specifically, the average value obtained above and the absolute value of each boundary difference can be calculated respectively. By comparing the two sizes, the target boundary spanned by the pose set can be determined.

For example, the absolute value of the first difference between the average value and the first boundary is expressed as: |aver_N-180|, and the absolute value of the second difference between the average value and the second boundary is expressed as: |aver_N-0|.

In some implementations, if |aver_N-180|<|aver_N-0|, it means that the elements in the pose set are closer to the 0° boundary, that is, the first boundary, so that the first boundary is determined as the target boundary.

In other implementations, if |aver_N-180|>|aver_N-0|, it means that the elements in the pose set are closer to the 180° boundary, that is, the second boundary, so that the second boundary is determined as the target boundary.

S730. In response to the target boundary being the preset boundary among the first boundary and the second boundary, perform numerical conversion on each element in the pose set to obtain a numerically continuous target set.

Based on the foregoing, it can be seen that only when the pose set crosses the 180° boundary (that is, the second boundary), each element in the pose set is numerically discontinuous. Therefore, in the embodiment of the present disclosure, the preset boundary is the second boundary. boundary.

In some implementations, if the target boundary is the first boundary, it means that the pose set spans the 0° boundary, so that each element in the pose set is numerically continuous, so there is no need to perform numerical boundary processing, and the above-mentioned process is performed directly. Linear regression process is sufficient.

In other embodiments, if the target boundary is the second boundary, it means that the pose set spans the 180° boundary. Therefore, each element in the pose set is numerically discontinuous, and linear regression cannot be directly performed. Each element needs to be The element values are numerically converted to obtain a target set with continuous values. The process of numerical conversion is specifically described in the embodiment of FIG. 8 of the present disclosure below.

As can be seen from the foregoing, in the embodiments of the present disclosure, by judging the numerical continuity of each element in the pose set, linear regression can be effectively performed on a pose set with discontinuous mathematical expressions, which is suitable for pose estimation in any scenario.

As shown in Figure 8, in some implementations, the pose estimation method of the present disclosure example, the process of numerical conversion of each element in the pose set includes:

S810. In response to the target boundary being the preset boundary among the first boundary and the second boundary, determine the first number of positive values and the second number of negative values in the pose set.

S820. In response to the first quantity being greater than the second quantity, increase all negative element values by the preset value to obtain the target set.

S830. In response to the first quantity not being greater than the second quantity, reduce the positive element value by the preset value to obtain the target set.

Based on the above, it can be seen that if the target boundary corresponding to the pose set is the second boundary, it means that the pose set spans the 180° boundary. Each element in the pose set is numerically discontinuous, and discontinuous values need to be converted into continuous values.

When converting numerical values, you can convert all negative values into positive values uniformly, or you can convert all positive values into negative values uniformly. In some implementations, in order to reduce the amount of calculation, the number of positive values and negative values can be determined, and the smaller number can be converted into the larger number, thereby reducing the number of numerical values to be converted and improving calculation efficiency.

For example, in an example, the pose set is expressed as [173,175,177,179,-179,-177], in which the positive values include 173, 175, 177 and 179, a total of 4, that is, the first number is 4. Negative values include -179 and -177, a total of 2, that is, the second number is 2.

In this example, the first quantity is greater than the second quantity, so all negative values need to be converted into positive values by increasing the preset value. In the example of this disclosure, the preset value may be 360. For example, -179+360=181, -177+360=183, so the target set obtained after numerical conversion is expressed as [173,175,177,179,181,183]. It can be seen that each element in the target set is numerically continuous.

For example, in another example, the pose set is expressed as [177,179,-179,-177,-175,-173], in which the positive values include 177 and 179, that is, the first number is 2. The negative values include -179, -177, -175 and -173, a total of 4, that is, the second number is 4.

In this example, the first quantity is smaller than the second quantity, so all positive values need to be reduced by the preset value and converted into negative values. In the example of this disclosure, the preset value may be 360. For example, 177-360=-183, 179-360=-181, so the target set obtained after numerical conversion is expressed as [-183,-181,-179,-177,-175,-173], as you can see, Each element in the target set is already numerically continuous.

Through the above process, boundary numerical processing is performed on the first pose set and the second pose set shown in equations (1) and (2) respectively, and the first target set and the second target set can be obtained, and then based on the aforementioned figure The linear regression process described in 5, respectively, obtains the first pose change rate corresponding to the first pose set and the second pose change rate corresponding to the second pose set, which will not be described in detail in this disclosure.

It can be seen from the above that in the embodiments of the present disclosure, by performing numerical conversion on each element in the pose set and converting each element in the pose set into a continuous numerical value, linear regression can be effectively performed on a pose set with discontinuous mathematical expression.

As described in conjunction with the embodiment of FIG. 5 , the first attitude change rate represents the changing trend of the data collected by the first sensor, and the second posture change rate represents the changing trend of the data collected by the second sensor. Based on the foregoing, it can be seen that under normal working conditions of the first sensor and the second sensor, theoretically the changing trends of the first attitude change rate and the second attitude change rate should be consistent or very close. If the change trends of the two are significantly different, , it means that there is large interference in the first sensor and/or the second sensor. Therefore, in the embodiment of the present disclosure, the current sensor reliability of the mobile device can be determined based on the difference between the first attitude change rate and the second attitude change rate.

In the embodiment of the present disclosure, a preset threshold may be set in advance, and the preset threshold represents a critical value at which the difference between the first posture change rate and the second posture change rate is large. When the difference between the first pose change rate and the second pose change rate |k ^g -k ^m | is not greater than the preset threshold, it means that the change trends of the first pose set and the second pose set are consistent, that is, the first sensor and the second sensor are not interfered with and have high reliability. When the difference between the first pose change rate and the second pose change rate |k ^g -k ^m | is greater than the preset threshold, it means that the change trends of the first pose combination and the second pose set are inconsistent, that is, the second sensor and /Or there is interference in the second sensor and its reliability is poor. As for the specific value of the preset threshold, those skilled in the art can set it according to specific scenarios, and this disclosure does not limit this.

In some embodiments, when the difference between the first attitude change rate and the second attitude change rate is not greater than a preset threshold, it means that neither the first sensor nor the second sensor is interfered, and there is no need to monitor both. To perform interference adjustment on the weight value, you only need to maintain the original update process.

In other embodiments, the first sensor is a gyroscope, and the second sensor is a magnetometer. When determining that the changing trends of the collected data of the gyroscope and the magnetometer are consistent, considering that the gyroscope has higher reliability than the magnetometer, only the posture weight of the magnetometer can be adjusted without adjusting the position of the gyroscope. posture weight. When it is determined that the changing trends of the data collected by the gyroscope and the magnetometer are inconsistent, the posture weights of the gyroscope and the magnetometer are adjusted at the same time. Each is explained below.

As shown in Figure 9, in some embodiments, according to the pose estimation method of the present disclosure, the process of determining the second pose weight of the magnetometer includes:

S910: Use the magnetometer to collect the second pose data at the current moment.

S920. Calculate the geomagnetic module length based on the second pose data at the current moment.

S930. Determine the second pose weight according to the geomagnetic mode length.

It can be understood that the magnetometer is a sensor that measures the strength of the environmental magnetic field. The magnetic field strength can be measured by the magnetometer to obtain the required three-axis azimuth angle data. When the surrounding magnetic field environment is disturbed, the geomagnetic mode length B measured by the magnetometer will change, and the greater the degree of interference can be measured by the rate of change of the geomagnetic mode length.

In the embodiment of the present disclosure, after using a magnetometer to collect the second pose data at the current time t, the second pose data can be normalized to obtain the geomagnetic mode length B corresponding to the current time t, which is expressed as:

In formula (8),

represents the second pose weight of the magnetometer under ideal conditions, B represents the geomagnetic mode length,

Indicates the second pose weight of the magnetometer at the current time t.

It can be understood that under ideal conditions the geomagnetic mode length is 1. The stronger the surrounding magnetic field interference, the smaller the calculated geomagnetic mode length is less than 1. Therefore, the second pose weight of the magnetometer at each sampling moment can be calculated in real time through the above equation (8), and as the surrounding magnetic field environment changes, the second pose weight can be dynamically adjusted, so that the surrounding magnetic field interferes The greater the degree, the smaller the second pose weight of the magnetometer.

For the gyroscope, when the difference between the first attitude change rate and the second attitude change rate is not greater than the preset threshold, the original first attitude weight update process can be maintained, and this disclosure will no longer Repeat.

When the difference between the first attitude change rate and the second attitude change rate is greater than the preset threshold, it means that the current gyroscope may be affected by factors such as vibration, resulting in reduced reliability of the gyroscope, thus requiring maintenance of the gyroscope. The first posture weight is adjusted. In some implementations, the first attitude weight can be adjusted based on Kalman filtering, which will be described in detail below.

First, the first pose weight of the current moment and the previous M sampling moments can be obtained, expressed as:

And so on. It can be understood that the specific value of M can be set according to scene requirements, for example, M=5, 10, 20, etc., and this disclosure does not limit this.

Then, based on the first attitude weight of M sampling moments, the corresponding variance can be calculated, expressed as

That is, the variance of the first posture weight of the past M sampling moments. The process of adjusting the first pose weight at the current time t based on Kalman filtering can be expressed as:

In formula (9),

Indicates the first posture weight corresponding to the current moment t,

Represents the variance of the first pose weight of the past M sampling moments,

Indicates the first posture weight corresponding to the previous sampling time t-1. K _M represents the Kalman gain, expressed as:

In formula (10),

Represents the confidence of the variance of the past M sampling moments,

Represents the confidence of the pose weight of the first pose data.

Based on the above process, the first attitude data of the current time t can be obtained

The corresponding first pose weight

Get the second pose data at the current time t

The corresponding second pose weight

It can be seen from the above that in the embodiments of the present disclosure, based on the quantitative judgment of the reliability of the first sensor and the second sensor, the pose weights of each sensor during the data fusion process can be dynamically adjusted in real time, thereby eliminating or reducing the external environment. Interference with the sensor improves pose estimation accuracy.

In the embodiment of the present disclosure, after obtaining the first posture data of the current time t

Second pose data

First position weight

and the second pose weight

After that, based on the first posture weight

and the second pose weight

To the first attitude data

and the second pose data

Perform data fusion processing to obtain the target pose of the mobile device.

In some implementations, the first pose data and the second pose data can be fused based on the Kalman filter algorithm, expressed as:

In formula (11), ψ _t represents the target pose of the mobile device at the current time t, ψ _t-1 represents the target pose of the previous sampling time t-1,

Represents the first pose data at the current time t,

Represents the second pose data at the current time t. K is the Kalman gain, expressed as:

In formula (11),

Represents the first posture weight,

Represents the second pose weight.

Based on the above process, the target pose of the mobile device corresponding to the current time t can be calculated, which is the final yaw angle of the mobile device in the above example. It can be understood that in the embodiments of the present disclosure, even if the signal environment around the mobile device changes, for example, the magnetometer and/or gyroscope is interfered, the pose weight corresponding to the sensor can be dynamically adjusted through the above process to ensure the final data. The accuracy of the target pose obtained by fusion.

As can be seen from the above, in the embodiment of the present disclosure, the reliability of the first sensor and the second sensor at the current moment is determined by comparing the changing trends of the sampling data of the first sensor and the second sensor, and the positions of the two are dynamically adjusted based on the reliability. pose weights, which can effectively eliminate or alleviate the defects of data distortion caused by signal interference, and improve the accuracy of the target pose obtained after data fusion.

Embodiments of the present disclosure provide a pose estimation device, which can be applied to, for example, the mobile device 600 shown in FIG. 1 , and is used to estimate the pose of the mobile device 600 in real time when it is moving.

As shown in Figure 10, in some implementations, the pose estimation device of the present disclosure includes:

The acquisition module 10 is configured to obtain the first attitude set of the mobile device at the current moment through the first sensor of the mobile device, and obtain the third attitude set of the mobile device at the current moment through the second sensor of the mobile device. Two pose sets; wherein, the first pose set and the second pose set include pose data at the current moment and several previous sampling moments;

The change rate determination module 20 is configured to determine a first posture change rate of the mobile device according to the first posture set, and determine a second posture change rate of the mobile device according to the second posture set. ;

The weight determination module 30 is configured to determine the first posture weight and the second posture of the mobile device at the current moment based on the difference between the first posture change rate and the second posture change rate. weight;

The data fusion module 40 is configured to perform fusion processing on the first posture data and the second posture data at the current moment according to the first posture weight value and the second posture weight value to obtain the movement The target pose of the device at the current moment.

As can be seen from the above, in the embodiment of the present disclosure, the reliability of the first sensor and the second sensor at the current moment is determined by comparing the changing trends of the sampling data of the first sensor and the second sensor, and the positions of the two are dynamically adjusted based on the reliability. pose weights, which can effectively eliminate or alleviate the defects of data distortion caused by signal interference, and improve the accuracy of the target pose obtained after data fusion.

In some implementations, the acquisition module 10 is configured to:

For each sampling moment, obtain the first motion data at the sampling moment based on the first sensor collection;

Determine the first attitude change data of the mobile device according to the first motion data at the sampling time, the first motion data at the previous sampling time, and the sampling period;

Determine the first attitude data of the mobile device at the sampling moment based on the first attitude data of the previous sampling moment and the first attitude change data;

The first attitude set is obtained based on the first attitude data at the current time and the first attitude data at a preset number of sampling times before the current time.

In some implementations, the acquisition module 10 is configured to:

For each sampling moment, obtain the second motion data of the sampling moment based on the second sensor collection;

Determine the second pose data of the mobile device at the sampling moment according to the second motion data;

The second pose set is obtained based on the second pose data at the current time and the second pose data at a preset number of sampling times before the current time.

In some embodiments, the change rate determination module 20 is configured to:

Perform boundary numerical processing based on each first pose data included in the first pose set to obtain a numerically continuous first target set; perform boundary processing based on each second pose data included in the second pose set Data processing to obtain a numerically continuous second target set;

Perform linear regression based on each element included in the first target set to obtain the first slope of the first straight line corresponding to the first target set; perform linear regression based on each element included in the second target set to obtain the the second slope of the second straight line corresponding to the first target set;

The first slope is determined as the first posture change rate, and the second slope is determined as the second posture change rate.

In some embodiments, the change rate determination module 20 is configured to:

In response to each element in the pose set including both positive and negative values, calculate an average of the absolute values of all elements;

Determine the target boundary corresponding to the pose set according to the relationship between the first absolute value of the difference between the average value and the first boundary, and the magnitude relationship between the second absolute value of the difference between the average value and the second boundary;

In response to the target boundary being the preset boundary among the first boundary and the second boundary, numerical conversion is performed on each element in the pose set to obtain a numerically continuous target set.

In some embodiments, the change rate determination module 20 is configured to:

In response to the target boundary being a preset boundary among the first boundary and the second boundary, determining a first number of positive values and a second number of negative values in the pose set;

In response to the first quantity being greater than the second quantity, all negative element values are increased by a preset value to obtain the target set;

In response to the first quantity not being greater than the second quantity, the element value of the positive value is reduced by a preset value to obtain the target set.

In some implementations, the weight determination module 30 is configured as:

In response to the difference between the first attitude change rate and the second attitude change rate being greater than the preset threshold, Kalman filtering is performed on the variance of the first attitude data at several sampling moments before the current moment to obtain the current moment. The first position weight.

In some embodiments, the second sensor includes a magnetometer; the weight determination module 30 is configured to:

Using the magnetometer to collect the second pose data at the current moment;

The geomagnetic mode length is calculated based on the second pose data at the current moment;

The second pose weight is determined according to the geomagnetic mode length.

In some embodiments, the data fusion module 40 is configured to:

According to the first posture weight and the second posture weight, Kalman filtering is performed on the first posture data and the second posture data at the current moment to obtain the target position of the mobile device at the current moment. posture.

In some embodiments, the first sensor includes a gyroscope and the second sensor includes a magnetometer.

As can be seen from the above, in the embodiment of the present disclosure, the reliability of the first sensor and the second sensor at the current moment is determined by comparing the changing trends of the sampling data of the first sensor and the second sensor, and the positions of the two are dynamically adjusted based on the reliability. pose weights, which can effectively eliminate or alleviate the defects of data distortion caused by signal interference, and improve the accuracy of the target pose obtained after data fusion.

In some embodiments, the present disclosure provides a mobile device including:

a first sensor and a second sensor;

processor; and

A memory stores computer instructions for causing the processor to execute the method according to any implementation manner of the first aspect.

For the mobile device in the embodiment of the present disclosure, reference can be made to the structure and principle of the mobile device 600 shown in FIG. 1 , which will not be described again in the present disclosure.

In a fourth aspect, an embodiment of the present disclosure provides a storage medium that stores computer instructions, and the computer instructions are used to cause a computer to execute the method according to any embodiment of the first aspect.

As can be seen from the above, in the embodiment of the present disclosure, the reliability of the first sensor and the second sensor at the current moment is determined by comparing the changing trends of the sampling data of the first sensor and the second sensor, and the positions of the two are dynamically adjusted based on the reliability. pose weights, which can effectively eliminate or alleviate the defects of data distortion caused by signal interference, and improve the accuracy of the target pose obtained after data fusion.

Obviously, the above-mentioned embodiments are only examples for clear explanation and are not limitations of the embodiments. For those of ordinary skill in the art, other different forms of changes or modifications can be made based on the above description. An exhaustive list of all implementations is neither necessary nor possible. The obvious changes or modifications derived therefrom are still within the protection scope of the present invention.

Claims (13)

1. A pose estimation method, characterized in that it is applied to mobile devices, and the method includes:

   The first pose set of the mobile device at the current moment is obtained through the first sensor of the mobile device, and the second pose set of the mobile device at the current moment is obtained through the second sensor of the mobile device; wherein, The first pose set and the second pose set include pose data at the current moment and several previous sampling moments;

   Determine a first posture change rate of the mobile device based on the first posture set, and determine a second posture change rate of the mobile device based on the second posture set;

   Determine the first posture weight and the second posture weight of the mobile device at the current moment based on the difference between the first posture change rate and the second posture change rate;

   According to the first posture weight and the second posture weight, the first posture data and the second posture data at the current moment are fused to obtain the target posture of the mobile device at the current moment.
2. The method of claim 1, wherein obtaining the first attitude set of the mobile device at the current moment through the first sensor of the mobile device includes:

   For each sampling moment, obtain the first motion data at the sampling moment based on the first sensor collection;

   Determine the first attitude change data of the mobile device according to the first motion data at the sampling time, the first motion data at the previous sampling time, and the sampling period;

   Determine the first attitude data of the mobile device at the sampling moment based on the first attitude data of the previous sampling moment and the first attitude change data;

   The first attitude set is obtained based on the first attitude data at the current time and the first attitude data at a preset number of sampling times before the current time.
3. The method according to claim 1, wherein obtaining the second pose set of the mobile device at the current moment through the second sensor of the mobile device includes:

   For each sampling moment, obtain the second motion data of the sampling moment based on the second sensor collection;

   Determine the second pose data of the mobile device at the sampling moment according to the second motion data;

   The second pose set is obtained based on the second pose data at the current time and the second pose data at a preset number of sampling times before the current time.
4. The method according to any one of claims 1 to 3, characterized in that the first attitude change rate of the mobile device is determined according to the first attitude set, and the first attitude change rate is determined according to the second attitude set. The second pose change rate of the mobile device includes:

   Perform boundary numerical processing based on each first pose data included in the first pose set to obtain a numerically continuous first target set; perform boundary processing based on each second pose data included in the second pose set Data processing to obtain a numerically continuous second target set;

   Perform linear regression based on each element included in the first target set to obtain the first slope of the first straight line corresponding to the first target set; perform linear regression based on each element included in the second target set to obtain the the second slope of the second straight line corresponding to the first target set;

   The first slope is determined as the first posture change rate, and the second slope is determined as the second posture change rate.
5. The method according to claim 4, characterized in that the process of performing the boundary numerical processing on any one of the first pose set and the second pose set includes:

   In response to each element in the pose set including both positive and negative values, calculate an average of the absolute values of all elements;

   Determine the target boundary corresponding to the pose set according to the relationship between the first absolute value of the difference between the average value and the first boundary, and the magnitude relationship between the second absolute value of the difference between the average value and the second boundary;

   In response to the target boundary being the preset boundary among the first boundary and the second boundary, numerical conversion is performed on each element in the pose set to obtain a numerically continuous target set.
6. The method according to claim 5, characterized in that, in response to the target boundary being a preset boundary among the first boundary and the second boundary, numerical conversion is performed on each element in the pose set to obtain A numerically continuous set of targets, including:

   In response to the target boundary being a preset boundary among the first boundary and the second boundary, determining a first number of positive values and a second number of negative values in the pose set;

   In response to the first quantity being greater than the second quantity, increasing all negative element values by a preset value to obtain the target set;

   In response to the first quantity not being greater than the second quantity, the element value of the positive value is reduced by a preset value to obtain the target set.
7. The method of claim 1, wherein the first posture weight of the mobile device at the current moment is determined based on the difference between the first posture change rate and the second posture change rate, include:

   In response to the difference between the first attitude change rate and the second attitude change rate being greater than the preset threshold, Kalman filtering is performed on the variance of the first attitude data at several sampling moments before the current moment to obtain the current moment. The first position weight.
8. The method according to claim 1 or 7, wherein the second sensor includes a magnetometer; determining the second pose weight of the mobile device at the current moment includes:

   Using the magnetometer to collect the second pose data at the current moment;

   The geomagnetic mode length is calculated based on the second pose data at the current moment;

   The second pose weight is determined according to the geomagnetic mode length.
9. The method according to claim 1, characterized in that, based on the first posture weight and the second posture weight, the first posture data and the second posture data at the current moment are processed. Fusion processing to obtain the target pose of the mobile device at the current moment, including:

   According to the first posture weight and the second posture weight, Kalman filtering is performed on the first posture data and the second posture data at the current moment to obtain the target position of the mobile device at the current moment. posture.
10. The method according to claim 1, characterized in that:

    The first sensor includes a gyroscope and the second sensor includes a magnetometer.
11. A pose estimation device, characterized in that it is applied to mobile equipment, and the device includes:

    The acquisition module is configured to obtain the first attitude set of the mobile device at the current moment through the first sensor of the mobile device, and obtain the second attitude set of the mobile device at the current moment through the second sensor of the mobile device. A pose set; wherein the first pose set and the second pose set include pose data at the current moment and several previous sampling moments;

    a change rate determination module configured to determine a first posture change rate of the mobile device according to the first posture set, and determine a second posture change rate of the mobile device according to the second posture set;

    A weight determination module configured to determine the first posture weight and the second posture weight of the mobile device at the current moment based on the difference between the first posture change rate and the second posture change rate. value;

    The data fusion module is configured to perform fusion processing on the first posture data and the second posture data at the current moment according to the first posture weight value and the second posture weight value to obtain the mobile device. The target pose at the current moment.
12. A mobile device, characterized by including:

    a first sensor and a second sensor;

    processor; and

    A memory storing computer instructions for causing the processor to execute the method according to any one of claims 1 to 10.
13. A storage medium, characterized in that computer instructions are stored, and the computer instructions are used to cause a computer to execute the method according to any one of claims 1 to 10.

Images