Mobile device and method, apparatus, and storage medium for pose estimation thereof

Abstract

This disclosure relates to the field of mobile robot technology, specifically providing a mobile device and its pose estimation method, apparatus, and storage medium. A pose estimation method includes: obtaining a first pose set and a second pose set using a first sensor and a second sensor of the mobile device; determining a first pose change rate based on the first pose set, determining a second pose change rate based on the second pose set, and determining the first pose weight and second pose weight of the mobile device at the current moment based on the difference between the two; and fusing the first pose data and the second pose data at the current moment based on the first pose weight and the second pose weight to obtain a target pose. In this embodiment, the sensor weights can be dynamically adjusted, thereby reducing the impact of signal interference and improving the accuracy of pose estimation.

Description

Technical Field

[0001] This disclosure relates to the field of mobile robot technology, specifically to a mobile device and its pose estimation method, apparatus, and storage medium. Background Technology

[0002] Pose estimation for mobile devices is a crucial prerequisite for autonomous navigation. In related technologies, inertial measurement units (IMUs) are typically used to obtain pose information through multi-sensor fusion. However, in these technologies, autonomous navigation systems for mobile devices are susceptible to interference and influence from complex environments, leading to pose estimation drift and affecting its accuracy. Summary of the Invention

[0003] To improve the pose estimation accuracy of mobile devices, this disclosure provides a mobile device and a pose estimation method, apparatus, and storage medium thereof.

[0004] In a first aspect, embodiments of this disclosure provide a pose estimation method applied to a mobile device, the method comprising:

[0005] 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;

[0006] 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.

[0007] Based on the difference between the first pose change rate and the second pose change rate, the first pose weight and the second pose weight of the mobile device at the current moment are determined.

[0008] Based on the first pose weight and the second pose weight, the first pose data and the second pose data at the current moment are fused to obtain the target pose of the mobile device at the current moment.

[0009] In some implementations, obtaining the first pose set of the mobile device at the current moment through the first sensor of the mobile device includes:

[0010] For each sampling time, the first motion data for that sampling time is obtained based on the first sensor;

[0011] Based on the first motion data at the sampling time, the first motion data at the previous sampling time, and the sampling period, the first pose change data of the mobile device is determined;

[0012] Based on the first pose data and the first pose change data at the previous sampling time, the first pose data of the mobile device at the sampling time is determined;

[0013] The first pose set is obtained based on the first pose data at the current moment and the first pose data at a preset number of sampling moments before the current moment.

[0014] In some implementations, obtaining the second pose set of the mobile device at the current moment through the second sensor of the mobile device includes:

[0015] For each sampling time, second motion data for that sampling time is acquired based on the second sensor;

[0016] The second pose data of the mobile device at the sampling time is determined based on the second motion data;

[0017] The second pose set is obtained based on the second pose data at the current moment and the second pose data at a preset number of sampling moments before the current moment.

[0018] In some implementations, determining the first pose change rate of the mobile device based on the first pose set and determining the second pose change rate of the mobile device based on the second pose set includes:

[0019] Boundary numerical processing is performed on each first pose data included in the first pose set to obtain a numerically continuous first target set; boundary data processing is performed on each second pose data included in the second pose set to obtain a numerically continuous second target set.

[0020] A linear regression is performed on each element of the first target set to obtain the first slope of the first straight line corresponding to the first target set; a linear regression is performed on each element of the second target set to obtain the second slope of the second straight line corresponding to the first target set.

[0021] The first slope is determined as the first pose change rate, and the second slope is determined as the second pose change rate.

[0022] In some implementations, the process of performing the boundary value processing on any one of the first pose set and the second pose set includes:

[0023] In response to the fact that each element in the pose set includes both positive and negative values, the average value of the absolute values ​​of all elements is calculated.

[0024] The target boundary corresponding to the pose set is determined based on the relationship between the absolute value of the first difference between the average value and the first boundary, and the absolute value of the second difference between the average value and the second boundary.

[0025] In response to the target boundary being a preset boundary between the first boundary and the second boundary, the elements in the pose set are numerically transformed to obtain a numerically continuous target set.

[0026] In some implementations, the step of numerically transforming each element in the pose set in response to the target boundary being a preset boundary between a first boundary and a second boundary to obtain a numerically continuous target set includes:

[0027] In response to the target boundary being a preset boundary between a first boundary and a second boundary, a first number of positive values ​​and a second number of negative values ​​are determined in the pose set;

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

[0029] In response to the first quantity not being greater than the second quantity, the positive element values ​​are reduced by a preset value to obtain the target set.

[0030] In some implementations, determining the first pose weight of the mobile device at the current moment based on the difference between the first pose change rate and the second pose change rate includes:

[0031] In response to the difference between the first pose change rate and the second pose change rate being greater than a preset threshold, Kalman filtering is performed on the variance of the first pose data from several sampling times prior to the current time to obtain the first pose weight at the current time.

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

[0033] The magnetometer is used to acquire the second pose data at the current moment;

[0034] The geomagnetic modulus length is calculated based on the second pose data at the current moment.

[0035] The second pose weight is determined based on the geomagnetic modulus.

[0036] In some implementations, the step of fusing the first pose data and the second pose data at the current moment based on the first pose weight and the second pose weight to obtain the target pose of the mobile device at the current moment includes:

[0037] Based on the first pose weight and the second pose weight, Kalman filtering is performed on the first pose data and the second pose data at the current time to obtain the target pose of the mobile device at the current time.

[0038] In some implementations, the first sensor includes a gyroscope, and the second sensor includes a magnetometer.

[0039] Secondly, embodiments of this disclosure provide a pose estimation device applied to a mobile device, the device comprising:

[0040] The acquisition module is configured to obtain a first pose set of the mobile device at the current moment through a first sensor of the mobile device, and to obtain a second pose set of the mobile device at the current moment through a 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;

[0041] The rate of change determination module is configured to determine the first pose change rate of the mobile device based on the first pose set, and to determine the second pose change rate of the mobile device based on the second pose set.

[0042] The weight determination module is configured to determine the first pose weight and the second pose weight of the mobile device at the current moment based on the difference between the first pose change rate and the second pose change rate.

[0043] The data fusion module is configured to fuse the first pose data and the second pose data at the current moment according to the first pose weight and the second pose weight to obtain the target pose of the mobile device at the current moment.

[0044] In some implementations, the acquisition module is configured to:

[0045] For each sampling time, the first motion data for that sampling time is obtained based on the first sensor;

[0046] Based on the first motion data at the sampling time, the first motion data at the previous sampling time, and the sampling period, the first pose change data of the mobile device is determined;

[0047] Based on the first pose data and the first pose change data at the previous sampling time, the first pose data of the mobile device at the sampling time is determined;

[0048] The first pose set is obtained based on the first pose data at the current moment and the first pose data at a preset number of sampling moments before the current moment.

[0049] In some implementations, the acquisition module is configured to:

[0050] For each sampling time, second motion data for that sampling time is acquired based on the second sensor;

[0051] The second pose data of the mobile device at the sampling time is determined based on the second motion data;

[0052] The second pose set is obtained based on the second pose data at the current moment and the second pose data at a preset number of sampling moments before the current moment.

[0053] In some implementations, the rate of change determination module is configured to:

[0054] Boundary numerical processing is performed on each first pose data included in the first pose set to obtain a numerically continuous first target set; boundary data processing is performed on each second pose data included in the second pose set to obtain a numerically continuous second target set.

[0055] A linear regression is performed on each element of the first target set to obtain the first slope of the first straight line corresponding to the first target set; a linear regression is performed on each element of the second target set to obtain the second slope of the second straight line corresponding to the first target set.

[0056] The first slope is determined as the first pose change rate, and the second slope is determined as the second pose change rate.

[0057] In some implementations, the rate of change determination module is configured to:

[0058] In response to the fact that each element in the pose set includes both positive and negative values, the average value of the absolute values ​​of all elements is calculated.

[0059] The target boundary corresponding to the pose set is determined based on the relationship between the absolute value of the first difference between the average value and the first boundary, and the absolute value of the second difference between the average value and the second boundary.

[0060] In response to the target boundary being a preset boundary between the first boundary and the second boundary, the elements in the pose set are numerically transformed to obtain a numerically continuous target set.

[0061] In some implementations, the rate of change determination module is configured to:

[0062] In response to the target boundary being a preset boundary between a first boundary and a second boundary, a first number of positive values ​​and a second number of negative values ​​are determined in the pose set;

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

[0064] In response to the first quantity not being greater than the second quantity, the positive element values ​​are reduced by a preset value to obtain the target set.

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

[0066] In response to the difference between the first pose change rate and the second pose change rate being greater than a preset threshold, Kalman filtering is performed on the variance of the first pose data from several sampling times prior to the current time to obtain the first pose weight at the current time.

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

[0068] The magnetometer is used to acquire the second pose data at the current moment;

[0069] The geomagnetic modulus length is calculated based on the second pose data at the current moment.

[0070] The second pose weight is determined based on the geomagnetic modulus.

[0071] In some implementations, the data fusion module is configured to:

[0072] Based on the first pose weight and the second pose weight, Kalman filtering is performed on the first pose data and the second pose data at the current time to obtain the target pose of the mobile device at the current time.

[0073] In some implementations, the first sensor includes a gyroscope, and the second sensor includes a magnetometer.

[0074] Thirdly, embodiments of this disclosure provide a mobile device, including:

[0075] First sensor and second sensor;

[0076] Processor; and

[0077] A memory storing computer instructions for causing the processor to perform the method according to any embodiment of the first aspect.

[0078] Fourthly, embodiments of this disclosure provide a storage medium storing computer instructions for causing a computer to perform the method described according to any embodiment of the first aspect.

[0079] The pose estimation method of this disclosure includes obtaining a first pose set at the current moment through a first sensor and determining the first pose change rate; obtaining a second pose set at the current moment through a second sensor and determining the second pose change rate; determining a first pose weight and a second pose weight based on the difference between the first pose change rate and the second pose change rate; and fusing the first pose data and the second pose data at the current moment based on the first pose weight and the second pose weight to obtain the target pose of the mobile device at the current moment. In this disclosure, by comparing the change trends of the sampling data from the first sensor and the second sensor over a current period, the reliability of both at the current moment is determined, and the pose weights of both are dynamically adjusted based on the reliability. This effectively eliminates or mitigates the data distortion caused by signal interference and improves the accuracy of the target pose obtained after data fusion. Attached Figure Description

[0080] To more clearly illustrate the technical solutions in the specific embodiments of this disclosure or the prior art, the drawings used in the description of the specific embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of this disclosure. For those skilled in the art, other drawings can be obtained from these drawings without creative effort.

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

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

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

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

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

[0086] Figure 6 This is a schematic diagram of the pose estimation method according to some embodiments of this disclosure.

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

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

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

[0090] Figure 10 This is a structural block diagram of a pose estimation device according to some embodiments of the present disclosure. Detailed Implementation

[0091] The technical solutions of this disclosure will now be clearly and completely described with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of this disclosure, not all of them. All other embodiments obtained by those skilled in the art based on the embodiments of this disclosure without inventive effort are within the scope of protection of this disclosure. Furthermore, the technical features involved in the different embodiments of this disclosure described below can be combined with each other as long as they do not conflict with each other.

[0092] Pose estimation of mobile devices is an important prerequisite for achieving autonomous navigation. In related technologies, inertial measurement units (IMUs) can usually be used to obtain the pose information of mobile devices through multi-sensor fusion.

[0093] For example, in the scenario of pose estimation for a ground-based mobile robot, a simple implementation can directly obtain the robot's pose by integrating over time using inertial measurement sensors such as gyroscopes and accelerometers. This method is feasible in the short term, but after the robot has been moving for a long time, the drift of the gyroscope itself and the influence of body vibration on the gyroscope will lead to distortion of the pose data.

[0094] To address the gyroscope distortion problem, related technologies can fuse data from other types of sensors to continuously correct the gyroscope and prevent gyroscope drift. However, these methods are insufficient for pose estimation in complex scenarios.

[0095] For example, using the fusion of magnetometer and gyroscope, this method is suitable for scenarios where the robot is in a simple magnetic field environment without electromagnetic interference, such as high-altitude flight scenarios for drones. However, for ground-based mobile robots, the surrounding magnetic field environment is complex and variable, which can greatly affect the magnetometer and lead to inaccurate robot pose estimation.

[0096] For example, taking the fusion of GPS satellite positioning and gyroscope data as an example, this method is suitable for outdoor scenarios, such as outdoor robots and drones. However, for robots moving indoors, GPS signals are weak, positioning is inaccurate, and they cannot provide accurate pose correction, resulting in inaccurate pose estimation.

[0097] Therefore, in order to achieve accurate pose estimation for indoor robots, related technologies can only integrate more types and quantities of sensor data, which leads to a significant increase in the cost of the robot body and the complexity of the algorithm.

[0098] Based on the deficiencies in the aforementioned related technologies, this disclosure provides a mobile device and its pose estimation method, apparatus, and storage medium, aiming to improve the pose estimation accuracy of the mobile device and reduce the impact of complex environments on sensor accuracy. Without fusing more sensor data, it judges and identifies the current environmental conditions of the device and adjusts the fusion algorithm accordingly to improve the pose estimation accuracy of the device in complex environments.

[0099] The pose estimation method of this disclosure can be applied to mobile devices, which are electronic devices that can be driven to move positions, such as mobile robots and drones. Figure 1 The present disclosure shows structural block diagrams of mobile devices in some embodiments. The following is a description of these diagrams in conjunction with... Figure 1 Please provide an explanation.

[0100] like Figure 1 As shown, in some embodiments, the mobile device 600 of this disclosure includes a processor 601, a memory 602, a first sensor 604, a second sensor 605, and a drive device 606.

[0101] The processor 601, memory 602, first sensor 604, second sensor 605, and drive device 606 establish a communicable connection between any two of them via bus 603.

[0102] The processor 601 can be any type of processor with one or more processing cores. It can execute single-threaded or multi-threaded operations, used for parsing instructions to perform operations such as data acquisition, logical operations, and outputting processing results.

[0103] The memory 602 may include a non-volatile computer-readable storage medium, such as at least one disk storage device, flash memory device, distributed storage device remotely located relative to the 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, which can be invoked by the processor 601 to cause the processor 601 to execute one or more method steps. The memory 602 may also include a volatile random access storage medium, or a storage portion such as a hard disk, as a data storage area for storing the processing results and data output by the processor 601.

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

[0105] The second sensor 605 may be a measurement unit of a different type from the first sensor 604, which may be installed on the mobile device. For example, the second sensor 605 may include a magnetometer, etc. During the movement of the mobile device 600, the second sensor 605 can detect the second pose data of the mobile device.

[0106] It is understood that in the embodiments of this disclosure, the second sensor 605 is used to correct the observation data of the first sensor 604, thereby obtaining more accurate pose data. The data fusion process will be specifically described in the following method of this disclosure.

[0107] The drive device 606 is the power system for the mobile device, used to drive the mobile device 600 to generate displacement. In some embodiments, the drive device 606 may include mechanical structures such as motors, transmission mechanisms, and rollers, which will undoubtedly be understood and fully implemented by those skilled in the art, and will not be described in detail here.

[0108] exist Figure 1 Based on the mobile device shown, this disclosure provides a pose estimation method for real-time estimation of the pose of the mobile device 600 during movement. This method can be executed by the processor 601 of the mobile device 600. The following describes the method in conjunction with... Figure 2 The implementation method is described below.

[0109] like Figure 2 As shown, in some embodiments, the pose estimation method of this disclosure includes:

[0110] 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.

[0111] It is understandable that during the movement of a mobile device, the first and second sensors collect pose data at a fixed sampling period. This data is then used in real-time through a multi-sensor fusion algorithm to calculate the target pose at that moment. For example, in one scenario, the sampling frequency of the first and second sensors is 10Hz, meaning that pose data is collected 10 times per second. Consequently, the mobile device's processor also performs pose estimation 10 times per second.

[0112] In this embodiment of the disclosure, when calculating the target pose at the current moment, data fusion is not directly performed based on the pose data of the first and second sensors. This is because, considering that in complex environments, the detection signals of the first and / or second sensors may be subject to interference, resulting in significant errors in the acquired pose data. For example, taking a magnetometer as an example, in a scenario with a complex magnetic field environment, the magnetometer's detection signal is subject to significant interference, causing a large deviation between the pose data acquired by the magnetometer and the actual pose, thus resulting in a large error in the target pose obtained after data fusion.

[0113] Therefore, in this embodiment of the present disclosure, before calculating the target pose at the current moment, the current environmental conditions of the mobile robot are first judged and identified to determine whether the working state of the first sensor and the second sensor is reliable. Based on the current reliability of the two sensors, the weight parameters of the first pose data and the second pose data are dynamically adjusted in real time during the data fusion process to ensure the accuracy of the target pose of the mobile device obtained by data fusion and reduce the interference of environmental factors on the sensors.

[0114] Specifically, at each sampling moment, the first pose data at each sampling moment can be calculated based on the sampling data from the first sensor, and similarly, the second pose data at each sampling moment can be calculated based on the sampling data from the second sensor.

[0115] In this embodiment of the disclosure, taking the current time t as an example, the first pose set can first be established based on the first pose data of the current time t and several previous sampling times, which can be represented as:

[0116]

[0117] In equation (1), Represents the first pose set. This represents the first pose data at the current time t. This represents the first pose data at sampling time t-1... and so on, so that the first pose set includes the first pose data at N sampling times.

[0118] Secondly, a second pose set can be established based on the second pose data from the current time t and several previous sampling times, represented as:

[0119]

[0120] In equation (2), This represents the second pose set. This represents the second pose data at the current time t. This represents the second pose data at sampling time t-1... and so on, so that the second pose set includes the second pose data at N sampling times.

[0121] It is worth noting that the first pose set and the second pose set represent the set of pose data for a period of time before the current time. In this embodiment of the present disclosure, the number N of elements in the set is not limited, and it can take any positive integer greater than 0, such as N = 10, 20, 50, etc. This disclosure does not impose any restrictions on this.

[0122] S220. Determine the first pose change rate of the mobile device based on the first pose set, and determine the second pose change rate of the mobile device based on the second pose set.

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

[0124] Similarly, the first pose set represents the pose data of the mobile device at a certain time before the current moment, calculated based on the data collected by the second sensor. It can reflect the changing trend of the pose data sampled by the second sensor.

[0125] The first pose set and the second pose set correspond to the movement process of the mobile device within the same time period. Therefore, in the absence of signal interference, theoretically, their trends should be consistent or similar. However, if one or both are subject to signal interference, their trends should differ significantly.

[0126] Based on this principle, in this embodiment of the disclosure, the difference between the first pose change rate, which reflects the changing trend of each first pose data in the first pose set, and the second pose change rate, which reflects the changing trend of each second pose data in the second pose set, can be used to determine whether the first sensor and / or the second sensor are in a reliable state.

[0127] In some embodiments, linear regression can be performed based on the first pose data included in the first pose set to fit a first straight line, and the first slope of the first straight line can be used as the first pose change rate. Similarly, linear regression can be performed based on the second pose data included in the second pose set to fit a second straight line, and the second slope of the second straight line can be used as the second pose change rate. This disclosure will be described in detail in the embodiments below, and will not be elaborated further here.

[0128] S230. Based on the difference between the first pose change rate and the second pose change rate, determine the first pose weight and the second pose weight of the mobile device at the current moment.

[0129] As can be seen from the foregoing, the first pose change rate can reflect the changing trend of the sampling data of the first sensor, and the second pose change rate can reflect the changing trend of the sampling data of the second sensor.

[0130] In some implementations, if the difference between the first pose change rate and the second pose change rate is small, it indicates that the pose data calculated by the first sensor and the second sensor have the same trend, and also indicates that the current mobile device is in an environment with little interference.

[0131] In other implementations, if there is a large difference between the first pose change rate and the second pose change rate, it indicates that the trends of the pose data calculated by the first sensor and the second sensor are not the same, and it also indicates that the current mobile device is in a highly disturbed environment.

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

[0133] For example, in the data fusion process, if the weight of the first pose is large and the weight of the second pose is small, the calculated target pose will be more inclined to the first pose data; conversely, if the weight of the first pose is small and the weight of the second pose is large, the calculated target pose will be more inclined to the second pose data.

[0134] Therefore, in some implementations, 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 small, it indicates that the signal interference in the environment at the current moment is minimal, and the adjustment can be made only for the second pose weights of the second pose data. This is because, when both the gyroscope and the magnetometer are unaffected by interference, the gyroscope has higher reliability than the magnetometer.

[0135] If there is a large difference between the first pose change rate and the second pose change rate, it indicates that there is significant signal interference in the environment at the current moment. In this case, the first pose weight of the first pose data and the second pose weight of the second pose data can be adjusted simultaneously to reduce the impact of signal interference.

[0136] The process of determining the first pose weight and the second pose weight is described in detail in the following embodiments of this disclosure, and will not be elaborated here.

[0137] S240. Based on the first pose weight and the second pose weight, the first pose data and the second pose data at the current moment are fused to obtain the target position of the mobile device at the current moment.

[0138] As described above, for the current time t, the first pose data can be calculated using the data collected by the first sensor, and the second pose data can be calculated using the data collected by the second sensor. After determining the corresponding first pose weights and second pose weights through the aforementioned process, data fusion processing can be performed on the first pose data and second pose data based on the pose weights to determine the target pose of the mobile device at the current time t.

[0139] In some implementations, a Kalman filter can be used to fuse the first pose data and the second pose data to obtain the target pose. This disclosure will describe this in the following embodiments, but will not be detailed here.

[0140] As can be seen from the above, in this embodiment of the present disclosure, by comparing the changing trends of the sampling data of the first sensor and the second sensor over a period of time, the reliability of the two sensors at the current moment is determined, and the pose weights of the two sensors 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.

[0141] It is worth noting that the pose of an object in three-dimensional space can be determined by its three-dimensional position coordinates (x, y, z) and its three-dimensional orientation (ψ). roll , ψ pitch , ψ yaw ) to represent, ψ roll Indicates the roll angle, ψ pitch ψ represents the pitch angle. yaw Indicates the yaw angle.

[0142] For three-dimensional attitude estimation of ground mobile devices, its roll angle ψ roll and pitch angle ψ pitch Since the motion amplitude itself is very small and can be corrected by an accelerometer, which is almost unaffected by environmental factors, the resulting pose can converge well. Therefore, the difficulty in its three-dimensional pose estimation lies in the yaw angle ψ.yaw The estimate.

[0143] In the embodiments described below in this disclosure, the yaw angle ψ of the ground mobile device will be used as the reference. yaw The pose estimation method of this disclosure is described using pose estimation as an example. However, it should be understood that the pose estimation method of this disclosure is not limited to this, and can be applied to any other pose estimation, which will not be elaborated upon here.

[0144] Furthermore, in the embodiments described below, the first sensor of the mobile device is a gyroscope, and the second sensor is a magnetometer. Those skilled in the art can undoubtedly understand and fully implement the specific working principles of the gyroscope and magnetometer, and these will not be elaborated upon here.

[0145] like Figure 3 As shown, in some embodiments, the pose estimation method of this disclosure, the process of obtaining the first pose set includes:

[0146] S310. For each sampling time, the first motion data at the sampling time is obtained based on the first sensor.

[0147] It is 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 this embodiment of the disclosure, the first sensor is a gyroscope, which samples the yaw angle data of the mobile device every 100ms, which is the first motion data described in this disclosure.

[0148] Taking sampling time t as an example, the first motion data acquired by the first sensor can be expressed as ω. z，t That is, the angular velocity of the fuselage yaw angle at sampling time t.

[0149] S320. Based on the first motion data at the sampling time, the first motion data at the previous sampling time, and the sampling period, determine the first pose change data of the mobile device.

[0150] S330. Based on the first pose data and the first pose change data at the previous sampling time, determine the first pose data of the mobile device at the sampling time.

[0151] After obtaining the first motion data at the current moment through sampling by the first sensor, the motion information at the current moment needs to be added to the pose result at the previous moment, so as to update the pose information in real time.

[0152] Specifically, taking sampling time t as an example, the corresponding first motion data is ω. z，t The previous sampling time is t-1, and its corresponding first motion data is ω. z，t-1Then the first pose data corresponding to sampling time t is represented as:

[0153]

[0154] In equation (3), This represents the fuselage yaw angle at sampling time t, obtained through integration using the gyroscope, which is also the first pose data. Δt represents the fuselage yaw angle at the previous sampling time t-1. Δt represents the sampling period of the gyroscope.

[0155] See equation (3) above, ω z，t ω represents the angular velocity of the yaw angle sampled at time t. z，t-1 This represents the angular velocity of the yaw angle sampled at time t-1. Therefore... This means that the average angular velocity of the fuselage yaw angle during the motion from time t-1 to time t is multiplied by the sampling period Δt, which is the motion angle of the fuselage yaw angle from time t-1 to time t, that is, the first attitude change data described in this embodiment. Then, the first attitude change data is added to the yaw angle data at time t-1 to obtain the yaw angle data at time t.

[0156] For each sampling time, the first pose data corresponding to each sampling time can be obtained by calculating using the above formula (3).

[0157] S340. Based on the first pose data at the current moment and the first pose data at the preset number of sampling moments before the current moment, obtain the first pose set.

[0158] In this embodiment of the disclosure, it is necessary to construct a set of first poses based on the first pose data of a period of time prior to the current moment.

[0159] Taking the current time t as an example, we can obtain the first pose data corresponding to the current time t, the previous sampling time t-1, the two previous sampling times t-1, ..., the N previous sampling times tN, for a total of N sampling times, and obtain the first pose set, which is represented as shown in equation (1):

[0160] like Figure 4 As shown, in some embodiments, the pose estimation method of this disclosure, in the process of obtaining the second pose set, includes:

[0161] S410. For each sampling time, the second motion data of the sampling time is obtained based on the second sensor.

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

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

[0164] In this embodiment of the disclosure, the process of the magnetometer calculating the yaw angle at each sampling time can be expressed as:

[0165]

[0166] In equation (4), This represents the fuselage yaw angle calculated by the magnetometer, B = [B x B y B z ] T This 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 algorithms such as Mahony's attitude calculation algorithm.

[0167] For each sampling time, the second pose data corresponding to each sampling time can be obtained by calculating using the above equation (4).

[0168] S430. Based on the second pose data at the current time and the second pose data at the preset number of sampling times before the current time, obtain the second pose set.

[0169] In this embodiment of the disclosure, it is necessary to construct a second pose set based on the second pose data of a period of time prior to the current moment.

[0170] Taking the current time t as an example, we can obtain the second pose data corresponding to the current time t, the previous sampling time t-1, the two previous sampling times t-1, ..., the N previous sampling times tN, for a total of N sampling times, and obtain the second pose set, which is represented as shown in equation (2):

[0171] In some implementations, 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 of the fitted line. Based on the slope, it can be determined whether the changing trends of the two are consistent or similar. The following combines... Figure 5 The implementation method is described below.

[0172] like Figure 5As shown, in some embodiments, the pose estimation method of this disclosure, the process of determining the first pose change rate and the second pose change rate includes:

[0173] S510. Perform boundary numerical processing on each first pose data included in the first pose set to obtain a numerically continuous first target set; perform boundary processing on each second pose data included in the second pose set to obtain a numerically continuous second target set.

[0174] It's important to note that in real-world mobile scenarios, the movement of the yaw angle is continuous, meaning the mobile device can rotate continuously within the range of 0° to 360°. However, mathematically, the yaw angle of a mobile device ranges from -180° to 180°, which is as follows: Figure 6 As shown, this leads to discontinuities at the 180° boundary.

[0175] For example, the actual motion of a mobile device from time tN to time t is: rotating from 175° to 185°. This is continuous in terms of the device's objective motion, but in... Figure 6 In the mathematical representation shown, the motion process is: moving from 175° to 180°, and then from -180° to -175°. It can be seen that this process involves a boundary crossing from 180° to -180°, resulting in a discontinuity in the mathematical expression.

[0176] The discontinuity in mathematical expression will lead to the discontinuity of each element in the pose set. Therefore, before performing linear regression on the pose set, it is necessary to perform boundary value processing on the element values ​​that cross the boundary so that the values ​​of each element in the pose set are continuous.

[0177] For example, the elements in the pose set are represented as [175,177,179,-179,-177,-175]. This pose set represents the actual motion process: the body moves from 175° to 185°. However, the elements in the first pose set are not numerically continuous. Through boundary value processing, this pose set can be represented as [175,177,179,181,183,185], thus making the elements in the pose set numerically continuous.

[0178] Based on 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 boundary numerical processing for the first pose set and the second pose set is described below. Figure 7 The specific implementation method will be described in detail here.

[0179] S520. Perform linear regression on each element of the first target set to obtain the first slope of the first straight line corresponding to the first target set; perform linear regression on each element of the second target set to obtain the second slope of the second straight line corresponding to the first target set.

[0180] In this embodiment of the disclosure, after boundary numerical processing, linear regression can be performed on the N elements included in the first target set and the second target set respectively to fit the corresponding first straight line and second straight line.

[0181] In some implementations, the least squares method can be used to perform linear regression on the first target set and the second target set. Taking either target set as an example, this will be explained.

[0182] First, perform linear regression on the N elements in the target set, as follows:

[0183] A = [X, I] (5)

[0184] In equation (5), X is the time vector, represented as X = [0, 1, ..., N-1]. T Let I be an N*1 column matrix with all elements being 1, represented as... Based on equation (5), we can obtain:

[0185] H = (A T A) -1 A T Y (6)

[0186] In equation (6), Y = ψ N That is, the elements in the target set. H = [k, b] T , k represents the slope of the fitted line, and b represents the intercept of the fitted line.

[0187] Through the above process, the first slope k of the first straight line can be obtained by performing linear regression on the first target set. g The second slope k of the second straight line is obtained by performing linear regression on the second target set. m .

[0188] S530. The first slope is determined as the first pose change rate, and the second slope is determined as the second pose change rate.

[0189] In this embodiment of the disclosure, the first slope k g The first pose change rate is determined as the first pose set, and the second slope is determined as the second pose change rate corresponding to the second pose set.

[0190] It can be understood that the rate of change of the first pose is the slope of the first straight line fitted based on the first pose data of the first pose set, which can reflect the changing trend of each first pose data in the first pose set. Similarly, the rate of change of the second pose is the slope of the second straight line fitted based on the second pose data of the second pose set, which can reflect the changing trend of each second pose data in the second pose set.

[0191] After obtaining the first pose change rate and the second pose change rate, the trend consistency between the two can be determined by the difference between the first pose change rate and the second pose change rate. This process will be specifically described in the following embodiments of this disclosure.

[0192] As can be seen from the above, in this embodiment of the present disclosure, the pose data over a period of time prior to the current moment is used to reflect the changing trend of the data collected by the first and second sensors, which can help determine the current environment of the mobile device and quantify the current reliability of the first and second sensors.

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

[0194] S710. In response to the fact that each element in the pose set includes both positive and negative values, calculate the average of the absolute values ​​of all elements.

[0195] Combination Figure 6 As shown, for a certain pose set, if the elements included contain both positive and negative values, it indicates that the values ​​of the elements in the pose set may be discontinuous.

[0196] For example, in one example, the pose set is represented as [170,171,172,173,174,175], which indicates that the yaw angle of the mobile device rotates from 170° to 175°. In this pose set, all element values ​​are positive, and the elements are numerically consecutive.

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

[0198] Therefore, if all elements in the pose set are positive or all elements are negative, it means that the elements in the pose set are numerically continuous, that is, they do not cross the 0° or 180° boundary. Therefore, there is no need to process the boundary values, and the above linear regression process can be performed directly.

[0199] However, if the elements in the pose set include both positive and negative values, it does not mean that the values ​​of the elements are necessarily discontinuous.

[0200] For example, in one example, the pose set is represented as [175,177,179,-179,-177,-175], which indicates that the yaw angle of the mobile device rotates from 175° to 185°. This pose set includes both positive and negative values, and the values ​​of the elements are not continuous because they cross the 180° boundary; 181° will be represented as -179, resulting in a discontinuity in the values.

[0201] For example, in one example, the pose set is represented as [-5,-3,-1,1,3,5], which indicates that the yaw angle of the mobile device rotates from 355° to 5°. Although this pose set includes both positive and negative elements, the values ​​of each element are continuous because they cross the 0° boundary. Even if the value gradually changes from negative to positive, it remains continuous.

[0202] 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 crossed by the pose set is a 0° boundary or a 180° boundary. In this embodiment, the 0° boundary is defined as the first boundary, and the 180° boundary is defined as the second boundary.

[0203] In this embodiment of the disclosure, if the pose set includes both positive and negative elements, the average value can be calculated by taking the absolute value of all elements, as follows:

[0204]

[0205] In equation (7), aver_N represents the average of the absolute values ​​of the N elements in the pose set, and ψ N This represents each element in the pose set.

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

[0207] As mentioned above, when the pose set includes both positive and negative elements, it is necessary to further determine whether the pose set crosses the first boundary or the second boundary. The first boundary is the 0° boundary, and the second boundary is the 180° boundary.

[0208] Specifically, the absolute value of the average value obtained above and the difference of each boundary can be calculated separately. By comparing the two values, the target boundary crossed by the pose set can be determined.

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

[0210] 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, and thus the first boundary is determined as the target boundary.

[0211] 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, and thus the second boundary is determined as the target boundary.

[0212] S730, in response to the target boundary being a preset boundary between the first boundary and the second boundary, performs numerical transformation on each element in the pose set to obtain a numerically continuous target set.

[0213] As can be seen from the foregoing, the elements in the pose set are not numerically continuous only when the pose set crosses the 180° boundary (i.e., the second boundary). Therefore, in this embodiment of the present disclosure, the preset boundary is the second boundary.

[0214] In some implementations, if the target boundary is the first boundary, it means that the pose set crosses the 0° boundary, and thus the elements in the pose set are numerically continuous. Therefore, there is no need to perform numerical boundary processing, and the above-mentioned linear regression process can be performed directly.

[0215] In other embodiments, if the target boundary is a second boundary, it indicates that the pose set crosses a 180° boundary. Therefore, the elements in the pose set are numerically discontinuous, making direct linear regression impossible. Numerical transformation of each element's value is required to obtain a numerically continuous target set. This disclosure is further detailed below. Figure 8 The implementation method provides a detailed explanation of the numerical conversion process.

[0216] As can be seen from the foregoing, in this embodiment of the present disclosure, by judging the numerical continuity of each element in the pose set, linear regression can be effectively performed on pose sets with discontinuous mathematical expressions, which is applicable to pose estimation in any scenario.

[0217] like Figure 8 As shown, in some embodiments, the pose estimation method of this disclosure, in the process of numerically transforming each element in the pose set, includes:

[0218] S810, in response to the target boundary being a preset boundary between 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.

[0219] S820: In response to the first quantity being greater than the second quantity, increment the values ​​of all negative elements by a preset value to obtain the target set.

[0220] S830, In response to the first quantity not being greater than the second quantity, reduce the positive element values ​​by a preset value to obtain the target set.

[0221] As mentioned above, if the target boundary corresponding to the pose set is the second boundary, it means that the pose set crosses the 180° boundary. The values ​​of each element in the pose set are discontinuous, and it is necessary to convert the discontinuous values ​​into continuous values.

[0222] During numerical conversion, all negative values ​​can be converted to positive values, or all positive values ​​can be converted to negative values. In some implementations, to reduce computational load, the number of positive and negative values ​​can be determined, and the side with fewer values ​​can be converted to the side with more values, thereby reducing the number of values ​​to be converted and improving computational efficiency.

[0223] For example, in one example, the pose set is represented as [173,175,177,179,-179,-177]. The positive values ​​include 173, 175, 177, and 179, a total of 4, which is the first quantity. The negative values ​​include -179 and -177, a total of 2, which is the second quantity.

[0224] In this example, the first quantity is greater than the second quantity, so all negative values ​​need to be converted to positive values ​​by adding a preset value. In this example, the preset value can be 360. For example, -179 + 360 = 181, -177 + 360 = 183, so the target set obtained after the numerical conversion is represented as [173, 175, 177, 179, 181, 183]. As you can see, the elements in the target set are numerically continuous.

[0225] For example, in another example, the pose set is represented as [177,179,-179,-177,-175,-173], where there are 2 positive values, including 177 and 179, which is the first quantity. There are 4 negative values, including -179, -177, -175, and -173, which is the second quantity.

[0226] In this example, the first quantity is less than the second quantity, so all positive values ​​need to be converted to negative values ​​by subtracting a preset value. In this example, the preset value can be 360. For example, 177-360=-183, 179-360=-181, so the target set obtained after the numerical conversion is represented as [-183,-181,-179,-177,-175,-173]. As you can see, the elements in the target set are numerically continuous.

[0227] Through the above process, the first pose set and the second pose set shown in equations (1) and (2) are respectively subjected to boundary numerical processing to obtain the first target set and the second target set. Then, based on the aforementioned... Figure 5 The linear regression process described herein yields 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 elaborated further in this disclosure.

[0228] As can be seen from the above, in this embodiment of the disclosure, by performing numerical conversion on each element in the pose set and converting each element in the pose set into continuous numerical values, linear regression can be effectively performed on the pose set whose mathematical expression is discontinuous.

[0229] Combination Figure 5 As described in the implementation, the first pose change rate represents the trend of change in the data collected by the first sensor, and the second pose change rate represents the trend of change in the data collected by the second sensor. Based on the foregoing, under normal operating conditions of the first and second sensors, theoretically, the trends of change in the first pose change rate and the second pose change rate should be consistent or very close. If the trends differ significantly, it indicates that there is significant interference between the first and / or second sensors. Therefore, in this implementation, the current sensor reliability of the mobile device can be determined based on the difference between the first pose change rate and the second pose change rate.

[0230] In this embodiment, a preset threshold can be pre-set, which represents a critical value where the difference between the first pose change rate and the second pose change rate is large. When the difference between the first pose change rate and the second pose change rate is |k g -k m| If the value is not greater than a preset threshold, it indicates that the change trends of the first pose set and the second pose set are consistent, meaning that the first and second sensors are not interfered with, and the reliability is high. When the difference between the change rate of the first pose and the change rate of the second pose is |k g -k m If the value is greater than a preset threshold, it indicates that the trends of the first pose set and the second pose set are inconsistent, meaning that the second sensor and / or the second sensor is interfering, resulting in poor reliability. Those skilled in the art can set the specific value of the preset threshold according to the specific scenario; this disclosure does not impose any limitations on it.

[0231] In some implementations, if the difference between the first pose change rate and the second pose change rate is not greater than a preset threshold, it indicates that neither the first sensor nor the second sensor has been disturbed. Therefore, there is no need to adjust the weight values ​​of the two sensors, and it is sufficient to maintain the original update process.

[0232] In other embodiments, the first sensor is a gyroscope, and the second sensor is a magnetometer. When the changing trends of the data collected by the gyroscope and magnetometer are consistent, considering the higher reliability of the gyroscope compared to the magnetometer, only the pose weights of the magnetometer can be adjusted, without adjusting the pose weights of the gyroscope. Conversely, when the changing trends of the data collected by the gyroscope and magnetometer are inconsistent, the pose weights of both the gyroscope and magnetometer are adjusted simultaneously. These will be explained separately below.

[0233] like Figure 9 As shown, in some embodiments, the pose estimation method of this disclosure, the process of determining the second pose weights of the magnetometer includes:

[0234] S910: The second pose data at the current moment is obtained by using a magnetometer.

[0235] S920. The geomagnetic modulus length is calculated based on the second pose data at the current moment.

[0236] S930. Determine the second pose weights based on the geomagnetic modulus length.

[0237] As we can understand it, a magnetometer is a sensor that measures the strength of an ambient magnetic field. By measuring the magnetic field strength, the required triaxial azimuth data can be obtained. When the surrounding magnetic field environment is disturbed, the geomagnetic modulus B measured by the magnetometer will change, and the degree of disturbance can be measured by the rate of change of the geomagnetic modulus.

[0238] In this embodiment of the disclosure, after acquiring the second pose data at the current time t using a magnetometer, the second pose data can be normalized to obtain the geomagnetic modulus B corresponding to the current time t, which is expressed as:

[0239]

[0240] In equation (8), This represents the second pose weight of the magnetometer under ideal conditions, where B represents the geomagnetic modulus. This represents the second pose weight of the magnetometer at the current time t.

[0241] It is understandable that, under ideal conditions, the geomagnetic modulus length is 1. The stronger the surrounding magnetic field interference, the smaller the calculated geomagnetic modulus length will be. Therefore, the second pose weight of the magnetometer at each sampling time can be calculated in real time using the above equation (8). Furthermore, the second pose weight can be dynamically adjusted as the surrounding magnetic field environment changes. Thus, the greater the degree of surrounding magnetic field interference, the smaller the second pose weight of the magnetometer will be.

[0242] For a gyroscope, if the difference between the first pose change rate and the second pose change rate is not greater than a preset threshold, the original first pose weight update process can be maintained, and this disclosure will not elaborate further.

[0243] If the difference between the first pose change rate and the second pose change rate exceeds a preset threshold, it indicates that the gyroscope may be affected by factors such as vibration, leading to a decrease in the gyroscope's reliability. Therefore, it is necessary to adjust the first pose weight of the gyroscope. In some implementations, the first pose weight can be adjusted based on Kalman filtering, which will be explained in detail below.

[0244] First, the first pose weight of the current time and the previous M sampling times can be obtained, represented as: And so on. It can be understood that the specific value of M can be set according to the needs of the scenario, such as M=5, 10, 20, etc., and this disclosure does not impose any restrictions on it.

[0245] Then, based on the first pose weight at M sampling times, the corresponding variance can be calculated, expressed as: This refers to the variance of the first pose weight over the past M sampling times. The process of adjusting the first pose weight at the current time t based on Kalman filtering can be expressed as:

[0246]

[0247] In equation (9), This represents the first pose weight corresponding to the current time t. This represents the variance of the first pose weight over the past M sampling times. K represents the first pose weight corresponding to the previous sampling time t-1. M The Kalman gain is expressed as:

[0248]

[0249] In equation (10), This represents the confidence level of the variance over the past M sampling times. This represents the confidence level of the pose weights of the first pose data.

[0250] Based on the above process, the first pose data at the current time t can be obtained. The corresponding first pose weight Obtain the second pose data at the current time t. The corresponding second pose weights

[0251] As can be seen from the above, in this embodiment of the present disclosure, the pose weights of each sensor can be dynamically adjusted in real time during the data fusion process based on the quantitative judgment of the reliability of the first sensor and the second sensor, thereby eliminating or reducing the interference of the external environment on the sensors and improving the pose estimation accuracy.

[0252] In this embodiment of the disclosure, the first pose data at the current time t is obtained. Second pose data First pose weight and the second pose weights Then, based on the first pose weight value Second pose weights For the first pose data Second pose data Data fusion processing is performed to obtain the target pose of the mobile device.

[0253] In some implementations, the first pose data and the second pose data can be fused based on the Kalman filter algorithm, as shown below:

[0254]

[0255] In equation (11), ψ t ψ represents the target pose of the mobile device at the current time t. t-1 This represents the target pose at the previous sampling time t-1. This represents the first pose data at the current time t. This represents the second pose data at the current time t. K is the Kalman gain, expressed as:

[0256]

[0257] In equation (11), This represents the first pose weight. This represents the weight of the second pose.

[0258] Based on the above process, the target pose of the mobile device at the current time t can be calculated, which is the final yaw angle of the mobile device in the example above. It can be understood that in this embodiment, even if the signal environment around the mobile device changes, such as interference with the magnetometer and / or gyroscope, the pose weights corresponding to the sensors can be dynamically adjusted through the above process to ensure the accuracy of the target pose obtained from the final data fusion.

[0259] As can be seen from the above, in this embodiment of the present disclosure, by comparing the changing trends of the sampling data of the first sensor and the second sensor over a period of time, the reliability of the two sensors at the current moment is determined, and the pose weights of the two sensors 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.

[0260] This disclosure provides a pose estimation device that can be applied to, for example... Figure 1 The mobile device 600 shown is used to estimate the pose of the mobile device 600 in real time during movement.

[0261] like Figure 10 As shown, in some embodiments, the pose estimation apparatus of this disclosure includes:

[0262] The acquisition module 10 is configured to obtain the first pose set of the mobile device at the current moment through the first sensor of the mobile device, and to obtain the second pose set of the mobile device at the current moment 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;

[0263] The rate of change determination module 20 is configured to determine the first pose change rate of the mobile device based on the first pose set, and to determine the second pose change rate of the mobile device based on the second pose set.

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

[0265] The data fusion module 40 is configured to fuse the first pose data and the second pose data at the current moment according to the first pose weight and the second pose weight to obtain the target pose of the mobile device at the current moment.

[0266] As can be seen from the above, in this embodiment of the present disclosure, by comparing the changing trends of the sampling data of the first sensor and the second sensor over a period of time, the reliability of the two sensors at the current moment is determined, and the pose weights of the two sensors 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.

[0267] In some embodiments, the acquisition module 10 is configured to:

[0268] For each sampling time, the first motion data for that sampling time is obtained based on the first sensor;

[0269] Based on the first motion data at the sampling time, the first motion data at the previous sampling time, and the sampling period, the first pose change data of the mobile device is determined;

[0270] Based on the first pose data and the first pose change data at the previous sampling time, the first pose data of the mobile device at the sampling time is determined;

[0271] The first pose set is obtained based on the first pose data at the current moment and the first pose data at a preset number of sampling moments before the current moment.

[0272] In some embodiments, the acquisition module 10 is configured to:

[0273] For each sampling time, second motion data for that sampling time is acquired based on the second sensor;

[0274] The second pose data of the mobile device at the sampling time is determined based on the second motion data;

[0275] The second pose set is obtained based on the second pose data at the current moment and the second pose data at a preset number of sampling moments before the current moment.

[0276] In some embodiments, the rate of change determination module 20 is configured to:

[0277] Boundary numerical processing is performed on each first pose data included in the first pose set to obtain a numerically continuous first target set; boundary data processing is performed on each second pose data included in the second pose set to obtain a numerically continuous second target set.

[0278] A linear regression is performed on each element of the first target set to obtain the first slope of the first straight line corresponding to the first target set; a linear regression is performed on each element of the second target set to obtain the second slope of the second straight line corresponding to the first target set.

[0279] The first slope is determined as the first pose change rate, and the second slope is determined as the second pose change rate.

[0280] In some embodiments, the rate of change determination module 20 is configured to:

[0281] In response to the fact that each element in the pose set includes both positive and negative values, the average value of the absolute values ​​of all elements is calculated.

[0282] The target boundary corresponding to the pose set is determined based on the relationship between the absolute value of the first difference between the average value and the first boundary, and the absolute value of the second difference between the average value and the second boundary.

[0283] In response to the target boundary being a preset boundary between the first boundary and the second boundary, the elements in the pose set are numerically transformed to obtain a numerically continuous target set.

[0284] In some embodiments, the rate of change determination module 20 is configured to:

[0285] In response to the target boundary being a preset boundary between a first boundary and a second boundary, a first number of positive values ​​and a second number of negative values ​​are determined in the pose set;

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

[0287] In response to the first quantity not being greater than the second quantity, the positive element values ​​are reduced by a preset value to obtain the target set.

[0288] In some implementations, the weight determination module 30 is configured to:

[0289] In response to the difference between the first pose change rate and the second pose change rate being greater than a preset threshold, Kalman filtering is performed on the variance of the first pose data from several sampling times prior to the current time to obtain the first pose weight at the current time.

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

[0291] The magnetometer is used to acquire the second pose data at the current moment;

[0292] The geomagnetic modulus length is calculated based on the second pose data at the current moment.

[0293] The second pose weight is determined based on the geomagnetic modulus.

[0294] In some implementations, the data fusion module 40 is configured to:

[0295] Based on the first pose weight and the second pose weight, Kalman filtering is performed on the first pose data and the second pose data at the current time to obtain the target pose of the mobile device at the current time.

[0296] In some implementations, the first sensor includes a gyroscope, and the second sensor includes a magnetometer.

[0297] As can be seen from the above, in this embodiment of the present disclosure, by comparing the changing trends of the sampling data of the first sensor and the second sensor over a period of time, the reliability of the two sensors at the current moment is determined, and the pose weights of the two sensors 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.

[0298] In some embodiments, this disclosure provides a mobile device, including:

[0299] First sensor and second sensor;

[0300] Processor; and

[0301] A memory storing computer instructions for causing the processor to perform the method according to any embodiment of the first aspect.

[0302] The mobile device described in this disclosure can be referred to the foregoing. Figure 1 The structure and principle of the mobile device 600 shown are not described in detail in this disclosure.

[0303] Fourthly, embodiments of this disclosure provide a storage medium storing computer instructions for causing a computer to perform the method described according to any embodiment of the first aspect.

[0304] As can be seen from the above, in this embodiment of the present disclosure, by comparing the changing trends of the sampling data of the first sensor and the second sensor over a period of time, the reliability of the two sensors at the current moment is determined, and the pose weights of the two sensors 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.

[0305] Obviously, the above embodiments are merely examples for clear illustration and are not intended to limit the embodiments. Those skilled in the art will recognize that other variations or modifications can be made based on the above description. It is neither necessary nor possible to exhaustively list all embodiments here. However, obvious variations or modifications derived therefrom are still within the scope of protection of this disclosure.

Claims

1. A pose estimation method, characterized in that, 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 multiple previous sampling moments; The first pose change rate of the mobile device is determined based on the first pose set, where the first pose change rate represents the change trend of the first pose data at each sampling time in the first pose set; the second pose change rate of the mobile device is determined based on the second pose set, where the second pose change rate represents the change trend of the second pose data at each sampling time in the second pose set. Based on the difference between the first pose change rate and the second pose change rate, the first pose weight and the second pose weight of the mobile device at the current moment are determined. Based on the first pose weight and the second pose weight, the first pose data and the second pose data at the current moment are fused to obtain the target pose of the mobile device at the current moment.

2. The method according to claim 1, characterized in that, The step of obtaining the first pose set of the mobile device at the current moment through the first sensor of the mobile device includes: For each sampling time, the first motion data for that sampling time is obtained based on the first sensor; Based on the first motion data at the sampling time, the first motion data at the previous sampling time, and the sampling period, the first pose change data of the mobile device is determined; Based on the first pose data and the first pose change data at the previous sampling time, the first pose data of the mobile device at the sampling time is determined; The first pose set is obtained based on the first pose data at the current moment and the first pose data at a preset number of sampling moments before the current moment.

3. The method according to claim 1, characterized in that, The step of 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 time, second motion data for that sampling time is acquired based on the second sensor; The second pose data of the mobile device at the sampling time is determined based on the second motion data; The second pose set is obtained based on the second pose data at the current moment and the second pose data at a preset number of sampling moments before the current moment.

4. The method according to any one of claims 1 to 3, characterized in that, The step of determining the first pose change rate of the mobile device based on the first pose set and determining the second pose change rate of the mobile device based on the second pose set includes: Boundary numerical processing is performed on each first pose data included in the first pose set to obtain a numerically continuous first target set; boundary data processing is performed on each second pose data included in the second pose set to obtain a numerically continuous second target set. A linear regression is performed on each element of the first target set to obtain the first slope of the first straight line corresponding to the first target set; a linear regression is performed on each element of the second target set to obtain the second slope of the second straight line corresponding to the second target set. The first slope is determined as the first pose change rate, and the second slope is determined as the second pose change rate.

5. The method according to claim 4, characterized in that, The process of performing the boundary value processing on any one of the first pose set and the second pose set includes: In response to the fact that each element in the pose set includes both positive and negative values, the average value of the absolute values ​​of all elements is calculated. The target boundary corresponding to the pose set is determined based on the relationship between the absolute value of the first difference between the average value and the first boundary, and the absolute value of the second difference between the average value and the second boundary; wherein, the first boundary represents the 0° boundary in the heading angle direction, and the second boundary represents the 180° boundary in the heading angle direction. In response to the target boundary being a preset boundary between the first boundary and the second boundary, the elements in the pose set are numerically transformed 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 between a first boundary and a second boundary, the elements in the pose set are numerically transformed to obtain a numerically continuous target set, including: In response to the target boundary being a preset boundary between a first boundary and a second boundary, a first number of positive values ​​and a second number of negative values ​​are determined in the pose set; In response to the first quantity being greater than the second quantity, all negative element values ​​are incremented by a preset value to obtain the target set; In response to the first quantity not being greater than the second quantity, the positive element values ​​are reduced by a preset value to obtain the target set.

7. The method according to claim 1, characterized in that, Based on the difference between the first pose change rate and the second pose change rate, the first pose weight of the mobile device at the current moment is determined, including: In response to the difference between the first pose change rate and the second pose change rate being greater than a preset threshold, Kalman filtering is performed on the variance of the first pose data from several sampling times prior to the current time to obtain the first pose weight at the current time.

8. The method according to claim 1 or 7, characterized in that, The second sensor includes a magnetometer; determining the second pose weights of the mobile device at the current moment includes: The magnetometer is used to acquire the second pose data at the current moment; The geomagnetic modulus length is calculated based on the second pose data at the current moment. The second pose weight is determined based on the geomagnetic modulus.

9. The method according to claim 1, characterized in that, The step of fusing the first pose data and the second pose data at the current moment according to the first pose weight and the second pose weight to obtain the target pose of the mobile device at the current moment includes: Based on the first pose weight and the second pose weight, Kalman filtering is performed on the first pose data and the second pose data at the current time to obtain the target pose of the mobile device at the current time.

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, Applied to a mobile device, the device includes: The acquisition module is configured to obtain a first pose set of the mobile device at the current moment through a first sensor of the mobile device, and to obtain a second pose set of the mobile device at the current moment through a second sensor of the mobile device; wherein the first pose set and the second pose set include pose data at the current moment and at multiple previous sampling moments; The rate of change determination module is configured to determine the first pose change rate of the mobile device based on the first pose set and to determine the second pose change rate of the mobile device based on the second pose set. The first pose change rate represents the change trend of the first pose data at each sampling time in the first pose set, and the second pose change rate represents the change trend of the second pose data at each sampling time in the second pose set. The weight determination module is configured to determine the first pose weight and the second pose weight of the mobile device at the current moment based on the difference between the first pose change rate and the second pose change rate. The data fusion module is configured to fuse the first pose data and the second pose data at the current moment according to the first pose weight and the second pose weight to obtain the target pose of the mobile device at the current moment.

12. A mobile device, characterized in that, include: First sensor and second sensor; processor; as well as A memory storing computer instructions for causing the processor to perform the method according to any one of claims 1 to 10.

13. A storage medium, characterized in that, The computer contains computer instructions for causing the computer to perform the method according to any one of claims 1 to 10.

Images