Mobile equipment, pose estimation method and device thereof and storage medium
Patent Information
- Application Number
- CN202280004235.0
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2022-05-18
- Publication Date
- 2024-11-26
- Estimated Expiration
- 2042-05-18
AI Technical Summary
The pose estimation accuracy of mobile devices in complex environments is low, and interference causes drift. Existing technologies need to fuse more sensor data to improve accuracy, but this increases cost and algorithm complexity.
Collect pose data through the first sensor and the second sensor, calculate the pose change rate and weights, dynamically adjust the sensor weights during the data fusion process, eliminate signal interference, and improve pose estimation accuracy.
Improve the pose estimation accuracy of mobile devices in complex environments, reduce the impact of interference on sensor accuracy, and avoid increasing the complexity of sensor data fusion.
Smart Images

Figure CN119032254A_ABST
Abstract
Description
Mobile device and posture estimation method, device, and storage medium thereof Technical Field
[0001] The present disclosure relates to the technical field of mobile robots, and in particular to a mobile device and a method, apparatus, and storage medium for estimating its posture. Background Art
[0002] Estimating the pose of a mobile device is a crucial prerequisite for autonomous navigation. In related technologies, an inertial measurement unit (IMU) is typically used to obtain this information through multi-sensor fusion. However, autonomous navigation systems in mobile devices are susceptible to interference and influence from complex environments, causing drift in the estimated pose and affecting the accuracy of the pose.
[0003] Summary of the Invention
[0004] To improve the accuracy of posture estimation of a mobile device, embodiments of the present disclosure provide a mobile device and a posture estimation method, apparatus, and storage medium thereof.
[0005] In a first aspect, an embodiment of the present disclosure provides a posture estimation method applied to a mobile device, the method comprising:
[0006] Obtaining a first pose set of the mobile device at a current moment through a first sensor of the mobile device, and obtaining a second pose set of the mobile device at a current moment through a second sensor of the mobile device; wherein the first pose set and the second pose set include pose data at a current moment and at a plurality of previous sampling moments;
[0007] 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;
[0008] Determining a first posture weight and a second posture weight of the mobile device at a current moment according to a difference between the first posture change rate and the second posture change rate;
[0009] The first pose data and the second pose data at the current moment are fused according to the first pose weight and the second pose weight to obtain the target pose of the mobile device at the current moment.
[0010] In some implementations, obtaining, by means of a first sensor of the mobile device, a first pose set of the mobile device at a current moment includes:
[0011] At each sampling moment, first motion data at the sampling moment is acquired based on the first sensor;
[0012] Determining first position change data of the mobile device according to the first motion data at the sampling moment, the first motion data at the previous sampling moment, and a sampling period;
[0013] Determining the first posture data of the mobile device at the sampling moment according to the first posture data at the previous sampling moment and the first posture change data;
[0014] The first posture set is obtained according to the first posture data at the current moment and the first posture data at a preset number of sampling moments before the current moment.
[0015] In some implementations, obtaining a second position set of the mobile device at a current moment by using a second sensor of the mobile device includes:
[0016] At each sampling moment, obtaining second motion data at the sampling moment based on the second sensor;
[0017] Determining second posture data of the mobile device at the sampling moment according to the second motion data;
[0018] The second posture set is obtained according to the second posture data at the current moment and the second posture data at a preset number of sampling moments before the current moment.
[0019] In some embodiments, determining a first pose change rate of the mobile device according to the first pose set and determining a second pose change rate of the mobile device according to the second pose set includes:
[0020] Performing boundary numerical processing based on each first pose data included in the first pose set to obtain a first target set that is numerically continuous; performing boundary data processing based on each second pose data included in the second pose set to obtain a second target set that is numerically continuous;
[0021] Performing linear regression on each element included in the first target set to obtain a first slope of a first straight line corresponding to the first target set; performing linear regression on each element included in the second target set to obtain a second slope of a second straight line corresponding to the first target set;
[0022] The first slope is determined as the first posture change rate, and the second slope is determined as the second posture change rate.
[0023] 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:
[0024] In response to each element in the pose set including both positive and negative values, calculating an average of the absolute values of all elements;
[0025] Determining a target boundary corresponding to the pose set according to a first absolute value of a difference between the average value and the first boundary, and a second absolute value of a difference between the average value and the second boundary;
[0026] In response to the target boundary being a preset boundary between the first boundary and the second boundary, numerical conversion is performed on each element in the posture set to obtain a numerically continuous target set.
[0027] In some embodiments, in response to the target boundary being a preset boundary between the first boundary and the second boundary, performing numerical conversion on each element in the pose set to obtain a numerically continuous target set includes:
[0028] In response to the target boundary being a predetermined 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;
[0029] In response to the first number being greater than the second number, increasing the values of all negative elements by a preset value to obtain the target set;
[0030] In response to the first number being not greater than the second number, the value of the positive element is reduced by a preset value to obtain the target set.
[0031] In some embodiments, determining the first pose weight of the mobile device at a current moment based on a difference between the first pose change rate and the second pose change rate includes:
[0032] In response to the difference between the first posture change rate and the second posture change rate being greater than a preset threshold, Kalman filtering is performed on the variance of the first posture data at several sampling moments before the current moment to obtain the first posture weight at the current moment.
[0033] In some embodiments, the second sensor includes a magnetometer; and determining a second posture weight of the mobile device at a current moment includes:
[0034] Using the magnetometer to collect and obtain second posture data at the current moment;
[0035] The geomagnetic modulus length is calculated based on the second posture data at the current moment;
[0036] The second posture weight is determined according to the geomagnetic modulus.
[0037] In some embodiments, 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:
[0038] Kalman filtering is performed on 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.
[0039] In some embodiments, the first sensor includes a gyroscope and the second sensor includes a magnetometer.
[0040] In a second aspect, an embodiment of the present disclosure provides a posture estimation device, applied to a mobile device, comprising:
[0041] an acquisition module configured to obtain a first pose set of the mobile device at a current moment through a first sensor of the mobile device, and obtain a second pose set of the mobile device at a 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 a plurality of previous sampling moments;
[0042] a change rate determination module, configured to determine a first pose change rate of the mobile device according to the first pose set, and to determine a second pose change rate of the mobile device according to the second pose set;
[0043] a weight determination module configured to determine a first posture weight and a second posture weight of the mobile device at a current moment based on a difference between the first posture change rate and the second posture change rate;
[0044] 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.
[0045] In some embodiments, the acquisition module is configured to:
[0046] At each sampling moment, first motion data at the sampling moment is acquired based on the first sensor;
[0047] Determining first position change data of the mobile device according to the first motion data at the sampling moment, the first motion data at the previous sampling moment, and a sampling period;
[0048] Determining the first posture data of the mobile device at the sampling moment according to the first posture data at the previous sampling moment and the first posture change data;
[0049] The first posture set is obtained according to the first posture data at the current moment and the first posture data at a preset number of sampling moments before the current moment.
[0050] In some embodiments, the acquisition module is configured to:
[0051] At each sampling moment, obtaining second motion data at the sampling moment based on the second sensor;
[0052] Determining second posture data of the mobile device at the sampling moment according to the second motion data;
[0053] The second posture set is obtained according to the second posture data at the current moment and the second posture data at a preset number of sampling moments before the current moment.
[0054] In some embodiments, the rate of change determination module is configured to:
[0055] Performing boundary numerical processing based on each first pose data included in the first pose set to obtain a first target set that is numerically continuous; performing boundary data processing based on each second pose data included in the second pose set to obtain a second target set that is numerically continuous;
[0056] Performing linear regression on each element included in the first target set to obtain a first slope of a first straight line corresponding to the first target set; performing linear regression on each element included in the second target set to obtain a second slope of a second straight line corresponding to the first target set;
[0057] The first slope is determined as the first posture change rate, and the second slope is determined as the second posture change rate.
[0058] In some embodiments, the rate of change determination module is configured to:
[0059] In response to each element in the pose set including both positive and negative values, calculating an average of the absolute values of all elements;
[0060] Determining a target boundary corresponding to the pose set according to a first absolute value of a difference between the average value and the first boundary, and a second absolute value of a difference between the average value and the second boundary;
[0061] In response to the target boundary being a preset boundary between the first boundary and the second boundary, numerical conversion is performed on each element in the posture set to obtain a numerically continuous target set.
[0062] In some embodiments, the rate of change determination module is configured to:
[0063] In response to the target boundary being a predetermined 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;
[0064] In response to the first number being greater than the second number, increasing the values of all negative elements by a preset value to obtain the target set;
[0065] In response to the first number being not greater than the second number, the value of the positive element is reduced by a preset value to obtain the target set.
[0066] In some embodiments, the weight determination module is configured to:
[0067] In response to the difference between the first posture change rate and the second posture change rate being greater than a preset threshold, Kalman filtering is performed on the variance of the first posture data at several sampling moments before the current moment to obtain the first posture weight at the current moment.
[0068] In some embodiments, the second sensor includes a magnetometer; and the weight determination module is configured to:
[0069] Using the magnetometer to collect and obtain second posture data at the current moment;
[0070] The geomagnetic modulus length is calculated based on the second posture data at the current moment;
[0071] The second posture weight is determined according to the geomagnetic modulus.
[0072] In some embodiments, the data fusion module is configured to:
[0073] Kalman filtering is performed on 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.
[0074] In some embodiments, the first sensor includes a gyroscope and the second sensor includes a magnetometer.
[0075] In a third aspect, an embodiment of the present disclosure provides a mobile device, including:
[0076] a first sensor and a second sensor;
[0077] processor; and
[0078] A memory storing computer instructions, wherein the computer instructions are used to enable the processor to execute the method according to any embodiment of the first aspect.
[0079] In a fourth aspect, an embodiment of the present disclosure provides a storage medium storing computer instructions, wherein the computer instructions are used to enable a computer to execute the method according to any embodiment of the first aspect.
[0080] The posture estimation method of the embodiment of the present disclosure includes obtaining the first posture set at the current moment and determining the first posture change rate through a first sensor, obtaining the second posture set at the current moment and determining the second posture change rate through a second sensor, determining the first posture weight and the second posture weight according to the difference between the first posture change rate and the second posture change rate, fusing the first posture data and the second posture data at the current moment according to the first posture weight and the second posture weight to obtain the target posture of the mobile device at the current moment. In the embodiment of the present disclosure, by comparing the change 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 posture weights of the two are dynamically adjusted based on the reliability, thereby effectively eliminating or alleviating the defects of data distortion caused by signal interference and improving the accuracy of the target posture obtained after data fusion. BRIEF DESCRIPTION OF THE DRAWINGS
[0081] In order to more clearly illustrate the specific embodiments of the present disclosure or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the specific embodiments or the description of the prior art. Obviously, the drawings described below are some embodiments of the present disclosure. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.
[0082] FIG1 is a structural block diagram of a mobile device according to some embodiments of the present disclosure.
[0083] FIG2 is a flowchart of a pose estimation method according to some embodiments of the present disclosure.
[0084] FIG3 is a flowchart of a method for pose estimation according to some embodiments of the present disclosure.
[0085] FIG4 is a flowchart of a pose estimation method according to some embodiments of the present disclosure.
[0086] FIG5 is a flowchart of a method for estimating a posture according to some embodiments of the present disclosure.
[0087] FIG6 is a schematic diagram of a pose estimation method according to some embodiments of the present disclosure.
[0088] FIG7 is a flowchart of a method for pose estimation according to some embodiments of the present disclosure.
[0089] FIG8 is a flowchart of a method for estimating a posture according to some embodiments of the present disclosure.
[0090] FIG9 is a flowchart of a method for pose estimation according to some embodiments of the present disclosure.
[0091] FIG10 is a structural block diagram of a posture estimation apparatus according to some embodiments of the present disclosure. DETAILED DESCRIPTION
[0092] The technical solutions of the present disclosure will be described clearly and completely below in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present disclosure, not all of the embodiments. Based on the embodiments of the present disclosure, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present disclosure. In addition, the technical features involved in the different embodiments of the present disclosure described below can be combined with each other as long as they do not conflict with each other.
[0093] Estimating the pose 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 multi-sensor fusion.
[0094] For example, in a scenario involving the pose estimation of a ground-based mobile robot, a simple implementation involves directly integrating the robot's inertial measurement sensors, such as its gyroscope and accelerometer, over time to derive the robot's pose. This approach works for a short period of time, but after prolonged robot movement, the gyroscope's own drift and the effects of the robot's body vibrations can distort the pose data.
[0095] To address the gyroscope distortion issue, related technologies can fuse data from other types of sensors to continuously correct the gyroscope and prevent gyroscope drift. However, these methods are difficult to meet the requirements of pose estimation in complex scenes.
[0096] For example, the fusion of a magnetometer and a gyroscope is suitable for scenarios where the aircraft's magnetic field is uniform and free of electromagnetic interference, such as high-altitude drone flight. However, for ground-based mobile robots, the complex and changing magnetic field can significantly affect the magnetometer, leading to inaccurate pose estimation.
[0097] For example, the fusion of GPS satellite positioning and gyroscopes is suitable for outdoor scenarios, such as outdoor robots and drones. However, for robots moving indoors, the GPS signal is weak, positioning is inaccurate, and accurate pose correction cannot be provided, resulting in inaccurate pose estimation.
[0098] 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, resulting in a doubling of the body cost and algorithm complexity.
[0099] Based on the defects existing in the above-mentioned related technologies, the embodiments of the present disclosure provide a mobile device and its posture estimation method, device, and storage medium, aiming to improve the posture estimation accuracy of the mobile device and reduce the impact of complex environments on sensor accuracy. Without the need to fuse more sensor data, the current environmental conditions of the device are judged and identified, and the fusion algorithm is adjusted accordingly to improve the posture estimation accuracy of the device in complex environments.
[0100] The pose estimation method of the embodiments of the present disclosure can be applied to mobile devices, which are electronic devices that can be driven to move, such as mobile robots and drones. FIG1 shows a block diagram of the structure of a mobile device in some embodiments of the present disclosure, which will be described below in conjunction with FIG1.
[0101] As shown in FIG. 1 , in some embodiments, a mobile device 600 according to 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 .
[0102] The processor 601 , the memory 602 , the first sensor 604 , the second sensor 605 and the driving device 606 establish a communicative connection between any two of them via the bus 603 .
[0103] The processor 601 may be any type of processor having one or more processing cores, and may execute single-threaded or multi-threaded operations, and may be used to parse instructions to execute operations such as acquiring data, performing logical operations, and delivering processing results.
[0104] The memory 602 may include non-volatile computer-readable storage media, such as at least one disk storage device, a flash memory device, a distributed storage device remotely located from the processor 601, or other non-volatile solid-state storage devices. The memory may include a program storage area for storing non-volatile software programs, non-volatile computer executable programs, and modules, which are called by the processor 601 to cause the processor 601 to perform one or more method steps. The memory 602 may also include a volatile random access memory medium or a storage portion such as a hard disk as a data storage area for storing operation processing results and data output by the processor 601.
[0105] 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. When the mobile device 600 moves, the first sensor 604 may detect the first position data of the mobile device.
[0106] The second sensor 605 may be a measurement unit of a different type from the first sensor 604 , which is provided on the mobile device. For example, the second sensor 605 may include a magnetometer, etc. When the mobile device 600 moves, the second sensor 605 may detect the second posture data of the mobile device.
[0107] It can be understood that in the embodiment of the present disclosure, the second sensor 605 is used to correct the observation data of the first sensor 604 to obtain more accurate posture data. The data fusion process is specifically described in the method below in this disclosure.
[0108] The driving device 606 is a power system of the mobile device, which 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 this disclosure will not elaborate on this.
[0109] Based on the mobile device shown in Figure 1, an embodiment of the present disclosure provides a posture estimation method for real-time estimation of the posture of the mobile device 600 during movement. The method can be executed by the processor 601 of the mobile device 600 and is described below in conjunction with the embodiment of Figure 2.
[0110] As shown in FIG2 , in some embodiments, the pose estimation method of the present disclosure includes:
[0111] S210: Obtain a first pose set of the mobile device at the current moment through a first sensor of the mobile device, and obtain a second pose set of the mobile device at the current moment through a second sensor of the mobile device.
[0112] It is understood that when a mobile device is moving, the first and second sensors collect current pose data at a fixed sampling period, thereby using a multi-sensor fusion algorithm to calculate the current target pose in real time. For example, in one example, the sampling frequency of the first and second sensors is 10 Hz, meaning that pose data is collected 10 times per second. Therefore, the mobile device's processor also performs pose estimation 10 times per second.
[0113] In the embodiment of the present disclosure, when solving the target posture at the current moment, data fusion is not performed directly based on the posture data of the first sensor and the second sensor. This is because, considering that in a complex environment, the detection signal of the first sensor and / or the second sensor may be interfered with, resulting in a large error in the collected posture 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 subject to greater interference, resulting in a large deviation between the posture data collected by the magnetometer and the actual posture, which in turn causes a large error in the target posture obtained after data fusion.
[0114] Therefore, in the embodiment of the present disclosure, before solving the target posture at the current moment, the current environmental conditions of the mobile robot are first judged and identified to determine whether the working status of the first sensor and the second sensor is reliable, and according to the current reliability of the two, the weight parameters of the first posture data and the second posture data in the data fusion process are dynamically adjusted in real time to ensure the accuracy of the target posture of the mobile device obtained by data fusion and reduce the interference of environmental factors on the sensors.
[0115] Specifically, at each sampling moment, the first pose data at each sampling moment can be calculated based on the sampling data of the first sensor. Similarly, the second pose data at each sampling moment can be calculated based on the sampling data of the second sensor.
[0116] In the embodiment of the present disclosure, taking the current time t as an example, first, the first pose set can be established based on the first pose data of the current time t and several previous sampling times, which can be expressed as:
[0117]
[0118] In formula (1), represents the first pose set, Represents the first position data at the current time t, Represents the first bit pose data at sampling time t-1... and so on, so that the first bit pose set includes the first bit pose data at N sampling times.
[0119] Secondly, the second pose set can be established based on the second pose data at the current time t and several previous sampling times, which can be expressed as:
[0120]
[0121] In formula (2), represents the second pose set, represents the second pose data at the current time t, 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 in total.
[0122] 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 moment. The embodiment of the present disclosure does not limit the number N of elements in the set, which can be any positive integer greater than 0, such as N = 10, 20, 50, etc., and the present disclosure does not impose any restrictions on this.
[0123] S220: Determine a first pose change rate of the mobile device according to the first pose set, and determine a second pose change rate of the mobile device according to the second pose set.
[0124] It can be understood that the first pose set represents the pose data of the mobile device a period of time before the current moment calculated based on the data collected by the first sensor, which can reflect the change trend of the pose data sampled by the first sensor.
[0125] Similarly, the first pose set represents the pose data of the mobile device for a period of time before the current moment, which is calculated based on the data collected by the second sensor, and can reflect the change trend of the pose data sampled by the second sensor.
[0126] The first and second pose sets correspond to the movement of the mobile device over the same period of time. Therefore, in the absence of signal interference, their change trends should theoretically be consistent or similar. However, if one or both of them are subject to signal interference, their change trends should differ significantly.
[0127] Based on this principle, in the embodiment of the present disclosure, whether the current first sensor and / or second sensor is in a reliable state can be determined by the difference between the first posture change rate reflecting the change trend of each first posture data in the first posture set, and the second posture change rate reflecting the change trend of each second posture data in the second posture set.
[0128] In some embodiments, a linear regression process can be performed based on each first pose data included in the first pose set to obtain a first straight line, and the first slope of the first straight line can be used as the first pose change rate. Similarly, a linear regression process can be performed based on each second pose data included in the second pose set to obtain a second straight line, and the second slope of the second straight line can be used as the second pose change rate. This is specifically described in the following embodiments of the present disclosure and is not described in detail here.
[0129] S230: Determine the first pose weight and the second pose weight of the mobile device at the current moment according to the difference between the first pose change rate and the second pose change rate.
[0130] Based on the foregoing, it can be seen that the first pose change rate can reflect the change trend of the sampling data of the first sensor, and the second pose change rate can reflect the change trend of the sampling data of the second sensor.
[0131] In some embodiments, if the difference between the first posture change rate and the second posture change rate is very small, it indicates that the posture data calculated by the first sensor and the second sensor have the same trend, which also indicates that the current environment of the mobile device has little interference.
[0132] 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 different, which also indicates that the environment in which the current mobile device is located is highly disturbed.
[0133] In the embodiment of the present disclosure, the different posture weights of the first sensor and the second sensor can be dynamically adjusted based on the above different situations. The posture weight can be understood as the influence of the sensor's posture data on the target posture.
[0134] 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 first pose is very small and the weight of the second pose is larger, the calculated target pose is more inclined to the second pose data.
[0135] Therefore, in some embodiments, assuming the first sensor is a gyroscope and the second sensor is a magnetometer, if the difference between the first and second pose change rates is small, it indicates that the signal interference in the current environment is minimal, and only the second pose weight of the second pose data can be adjusted. This is because, when both the gyroscope and the magnetometer are free of interference, the gyroscope has higher reliability than the magnetometer.
[0136] If the difference between the first pose change rate and the second pose change rate is large, it means that the signal interference in the current environment is large. 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 at the same time to reduce the impact of signal interference.
[0137] The process of determining the first pose weight and the second pose weight is described in detail in the following embodiments of the present disclosure, which will not be expanded here.
[0138] S240 , 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 position of the mobile device at the current moment.
[0139] Based on the above, it can be seen that at 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 first pose weight and the second pose weight corresponding to the two through the above process, the first pose data and the second pose data can be fused based on the pose weights to determine the target pose of the mobile device at the current time t.
[0140] In some embodiments, a Kalman filter method can be used to fuse the first pose data and the second pose data to obtain the target pose. This is described in the following embodiments of the present disclosure and will not be described in detail here.
[0141] From the above, it can be seen that in the embodiment of the present disclosure, by comparing the change trends of the sampling data of the first sensor and the second sensor in the current period of time, the reliability of the two at the current moment is determined, and the posture weights of the two are dynamically adjusted based on the reliability, thereby effectively eliminating or alleviating the defects of data distortion caused by signal interference and improving the accuracy of the target posture obtained after data fusion.
[0142] It is worth noting that the position of an object in three-dimensional space can be expressed by its three-dimensional position coordinates (x, y, z) and its three-dimensional posture (ψ roll , ψ pitch , ψ yaw ) to express, ψ roll represents the roll angle, ψ pitch represents the pitch angle, ψ yaw Indicates the yaw angle.
[0143] For the three-dimensional attitude estimation of ground mobile equipment, its roll angle ψ roll and the pitch angle ψ pitch The movement amplitude itself is very small and can be corrected by the accelerometer. The accelerometer is almost unaffected by environmental factors, and the obtained pose results can converge well. Therefore, the difficulty of its three-dimensional pose estimation lies in the yaw angle ψ yaw Estimates.
[0144] In the following embodiments of the present disclosure, the yaw angle ψ of the ground mobile device is yaw Taking pose estimation as an example, the pose estimation method of the embodiment of the present disclosure is described. However, it should be understood that the pose estimation method of the embodiment of the present disclosure is not limited to this, and can be applied to any other pose estimation, and this disclosure will not elaborate on this.
[0145] 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 the gyroscope and magnetometer are undoubtedly understood and fully implemented by those skilled in the art, and will not be elaborated in this disclosure.
[0146] As shown in FIG3 , in some embodiments, the pose estimation method of the present disclosure example, the process of obtaining the first pose set includes:
[0147] S310 : For each sampling moment, first motion data at the sampling moment is acquired based on the first sensor.
[0148] It is understood that the first sensor collects motion data of the mobile device at a fixed sampling period, which is defined as the first motion data. For example, in the embodiment of the present disclosure, the first sensor is a gyroscope, which samples the yaw angle data of the mobile device every 100ms, which is also the first motion data described in the present disclosure.
[0149] Taking 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 fuselage yaw angle at sampling time t.
[0150] S320: Determine the first position change data of the mobile device according to the first motion data at the sampling moment, the first motion data at the previous sampling moment, and the sampling period.
[0151] S330: Determine the first pose data of the mobile device at the sampling moment according to the first pose data and the first pose change data at the previous sampling moment.
[0152] 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 accumulated to the posture result at the previous moment, so as to update the posture information in real time.
[0153] 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 pose data corresponding to the sampling time t is expressed as:
[0154]
[0155] In formula (3), It represents the yaw angle of the fuselage at sampling time t obtained by gyroscope integration, that is, the first pose data. represents the yaw angle of the fuselage at the previous sampling time t-1. Δt represents the sampling period of the gyroscope.
[0156] Refer to the above formula (3), ω z,t Represents the angular velocity of the yaw angle sampled at time t, ω z,t-1 represents the angular velocity of the yaw angle sampled at time t-1. This represents the average angular velocity of the fuselage's yaw angle during the motion from time t-1 to time t. The product of this velocity and the sampling period Δt is the motion angle of the fuselage's yaw angle from time t-1 to time t, which is also the first position change data described in the embodiments of the present disclosure. The first position change data is then added to the yaw angle data at time t-1 to obtain the yaw angle data at time t.
[0157] For each sampling moment, the above formula (3) is used for calculation to obtain the first position data corresponding to each sampling moment.
[0158] S340 , obtaining a first position pose set according to the first position pose data at the current moment and the first position pose data at a preset number of sampling moments before the current moment.
[0159] In the disclosed embodiment, it is necessary to construct a first pose set based on the first pose data of a period of time before the current moment.
[0160] Taking the current time t as an example, we can obtain the first position data corresponding to the current time t, the previous sampling time t-1, the previous two sampling times t-1, ..., the previous N sampling times tN, a total of N sampling times, and obtain the first position set, which is expressed as shown in formula (1):
[0161] As shown in FIG4 , in some embodiments, the pose estimation method of the present disclosure example, the process of obtaining the second pose set includes:
[0162] S410 : At each sampling moment, obtain second motion data at the sampling moment based on the second sensor.
[0163] It is understood that the second sensor collects motion data of the mobile device at a fixed sampling period, which is defined as the second motion data. For example, in the embodiment of the present disclosure, the second sensor is a magnetometer, which samples the magnetic field vector and posture angle data around the mobile device every 100ms, which is also the second motion data described in the present disclosure.
[0164] S420: Determine second posture data of the mobile device at the sampling moment according to the second motion data.
[0165] In the embodiment of the present disclosure, the process of the magnetometer calculating the yaw angle at each sampling moment can be expressed as:
[0166]
[0167] In formula (4), represents the fuselage yaw angle calculated by the magnetometer, B=[B x ,By ,B z ] T represents the magnetic field vector detected by the magnetometer. φ represents the roll angle of the fuselage, and θ represents the pitch angle of the fuselage, which can be obtained using, for example, the Mahony attitude solution algorithm.
[0168] For each sampling moment, the above formula (4) is used for calculation to obtain the second posture data corresponding to each sampling moment.
[0169] S430: Obtain a second posture set according to the second posture data at the current moment and the second posture data at a preset number of sampling moments before the current moment.
[0170] In the embodiment of the present disclosure, it is necessary to construct a second posture set based on the second posture data of a period of time before the current moment.
[0171] Taking the current time t as an example, the second pose data corresponding to the current time t, the previous sampling time t-1, the previous two sampling times t-1, ..., the previous N sampling times tN, a total of N sampling times, can be obtained to obtain the second pose set, which is expressed as shown in formula (2):
[0172] In some embodiments, after obtaining the first pose set and the second pose set, linear regression can be performed on each of the two pose sets to obtain the slope corresponding to the fitted line. Based on the slope, it can be determined whether the change trends of the two pose sets are consistent or similar. This is described below with reference to the embodiment of FIG5 .
[0173] As shown in FIG5 , in some embodiments, the pose estimation method of the present disclosure example, the process of determining the first pose change rate and the second pose change rate includes:
[0174] 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 continuous second target set.
[0175] It's important to note that in real-world mobile scenarios, the yaw angle of a mobile device is continuous, meaning it can rotate continuously within a range of 0° to 360°. However, mathematically, the yaw angle range of a mobile device is -180° to 180°, as shown in Figure 6. This results in a discontinuity at the 180° boundary.
[0176] For example, the actual motion of a mobile device from time tN to time t is from 175° to 185°. This is continuous in terms of the device's objective motion. However, in the mathematical representation shown in Figure 6, this motion is from 175° to 180°, and then from -180° to -175°. As can be seen, this process involves a sudden change from 180° to -180°, resulting in a discontinuity in the mathematical representation.
[0177] The discontinuity of 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 numerical processing on the values of elements that cross boundaries so that the value of each element in the pose set is continuous.
[0178] For example, the elements in the pose set are represented as: [175, 177, 179, -179, -177, -175]. The actual motion process represented by this pose set is: the body moves from 175° to 185°. However, the elements in the first pose set are not numerically continuous. Through boundary numerical processing, the pose set can be represented as: [175, 177, 179, 181, 183, 185], which means that the elements in the pose set are numerically continuous.
[0179] After the above-mentioned 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 is specifically described in the embodiment of Figure 7 below of this disclosure and will not be described in detail here.
[0180] S520. Perform linear regression based on each element included in the first target set to obtain a first slope of a first straight line corresponding to the first target set; perform linear regression based on each element included in the second target set to obtain a second slope of a second straight line corresponding to the first target set.
[0181] In the embodiment of the present disclosure, after performing boundary numerical processing, linear regression can be performed on the N elements respectively included in the first target set and the second target set to obtain the corresponding first and second straight lines.
[0182] In some implementations, the least squares method may be used to perform linear regression on the first target set and the second target set, and any one of the target sets is used as an example for illustration.
[0183] First, linear regression is performed on the N elements in the target set, which can be expressed as:
[0184] A=[X,I] (5)
[0185] In formula (5), X is the time vector, expressed as X = [0, 1, ..., N-1] T I is an N*1 column matrix, all elements are 1, expressed as Based on formula (5), we can get:
[0186] H=(A T A) -1 A T Y (6)
[0187] In formula (6), Y = ψ N , which is the element in the target set. H=[k,b] T , k represents the slope of the fitted straight line, and b represents the intercept of the fitted straight line.
[0188] Through the above process, the first target set can be linearly regressed to obtain the first slope k of the corresponding first straight line. g , perform linear regression on the second target set to obtain the second slope k of the corresponding second straight line m .
[0189] S530: Determine the first slope as the first posture change rate, and determine the second slope as the second posture change rate.
[0190] In the embodiment of the present disclosure, the first slope k g The first pose change rate corresponding to the first pose set is determined, and the second slope is determined as the second pose change rate corresponding to the second pose set.
[0191] It can be understood that the first pose change rate is the slope of the first straight line fitted to the first pose data of each first pose set, which can reflect the change trend of each first pose data in the first pose set. Similarly, the second pose change rate is the slope of the second straight line fitted to the second pose data of each second pose set, which can reflect the change trend of each second pose data in the second pose set.
[0192] 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 is specifically described in the following embodiments of the present disclosure.
[0193] From the above, it can be seen that in the embodiment of the present disclosure, the posture data in a period of time before the current moment is used to reflect the changing trend of the data collected by the first sensor and the second sensor, so as to judge the current environment of the mobile device and quantify the current reliability of the first sensor and the second sensor.
[0194] As shown in FIG7 , in some embodiments, the process of performing boundary numerical processing on any one of the first pose set and the second pose set includes:
[0195] S710 . In response to each element in the posture set including both positive and negative values, calculate and obtain the average of the absolute values of all elements.
[0196] As shown in FIG6 , for a certain posture set, if the elements included therein have both positive and negative values, it means that the values of the elements in the posture set may be discontinuous.
[0197] For example, a 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 elements are positive and continuous.
[0198] For example, a 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 elements are negative and the values of each element are continuous.
[0199] It can be seen from this that 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° boundary or the 180° boundary. There is no need to perform boundary numerical processing, and the above linear regression process can be directly performed.
[0200] 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.
[0201] For example, a 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 in the pose set are not continuous because they cross the 180° boundary. 181° will be represented as -179, resulting in a discontinuous value.
[0202] For example, a 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.
[0203] Therefore, in this step, we first determine whether the values of each element in the pose set include both positive and negative values. If so, we need to further determine whether the boundary spanned by the pose set is a 0° boundary or a 180° boundary. In the disclosed embodiment, the 0° boundary is defined as the first boundary, and the 180° boundary is defined as the second boundary.
[0204] In the embodiment of the present disclosure, if the pose set includes both positive and negative elements, the absolute values of all elements can be taken and the average value can be calculated, which can be expressed as:
[0205]
[0206] In formula (7), aver_N represents the average value of the absolute values of N elements in the pose set, ψ N Represents each element in the pose set.
[0207] S720: Determine the target boundary corresponding to the pose set based on a first absolute value of a difference between the average value and the first boundary, and a second absolute value of a difference between the average value and the second boundary.
[0208] Based on the above, when the posture set includes both positive and negative elements, it is necessary to further determine whether the posture set crosses the first boundary or the second boundary. The first boundary is the 0° boundary, and the second boundary is the 180° boundary.
[0209] Specifically, the absolute value of the difference between the average value obtained above and each boundary value can be calculated respectively. By comparing the two, the target boundary crossed by the pose set can be determined.
[0210] For example, the absolute value of a first difference between the average value and the first boundary is expressed as |aver_N-180|, and the absolute value of a second difference between the average value and the second boundary is expressed as |aver_N-0|.
[0211] In some implementations, if |aver_N-180|<|aver_N-0|, it indicates that the element in the pose set is closer to the 0° boundary, that is, the first boundary, and thus the first boundary is determined as the target boundary.
[0212] In other implementations, if |aver_N-180|>|aver_N-0|, it means that the element in the pose set is closer to the 180° boundary, that is, the second boundary, and the second boundary is determined as the target boundary.
[0213] S730: In response to the target boundary being a preset boundary between the first boundary and the second boundary, perform numerical conversion on each element in the posture set to obtain a numerically continuous target set.
[0214] Based on the above, it can be seen that only when the posture set crosses the 180° boundary (that is, the second boundary) are the elements in the posture set numerically discontinuous. Therefore, in the embodiment of the present disclosure, the preset boundary is the second boundary.
[0215] In some embodiments, if the target boundary is the first boundary, it means that the pose set spans the 0° boundary, so that the elements in the pose set are numerically continuous, and there is no need to perform numerical boundary processing, and the above-mentioned linear regression process can be directly performed.
[0216] In other embodiments, if the target boundary is the second boundary, it means that the pose set spans a 180° boundary. As a result, the elements in the pose set are numerically discontinuous, making direct linear regression impossible. It is necessary to perform numerical conversion on the values of each element to obtain a numerically continuous target set. The numerical conversion process is described in detail in the embodiment of FIG8 below.
[0217] From the foregoing, it can be seen that 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 the pose set with discontinuous mathematical expression, which is applicable to pose estimation in any scenario.
[0218] As shown in FIG8 , in some embodiments, the pose estimation method of the present disclosure example, the process of performing numerical conversion on each element in the pose set includes:
[0219] S810: In response to the target boundary being a preset boundary between the first boundary and the second boundary, determine a first number of positive values and a second number of negative values in the pose set.
[0220] S820: In response to the first number being greater than the second number, increase the values of all negative elements by a preset value to obtain a target set.
[0221] S830: In response to the first number being not greater than the second number, reducing the value of the positive elements by a preset value to obtain a target set.
[0222] Based on the above, if the target boundary corresponding to the pose set is the second boundary, it means that the pose set crosses the 180° boundary, and the values of the elements in the pose set are discontinuous, and the discontinuous values need to be converted into continuous values.
[0223] When converting values, all negative values can be converted to positive values, or all positive values can be converted to negative values. In some embodiments, to reduce the amount of computation, the number of positive and negative values can be determined, and the number of positive and negative values can be converted to the number of negative values, thereby reducing the number of values to be converted and improving computational efficiency.
[0224] For example, in one example, the pose set is represented as [173, 175, 177, 179, -179, -177], where the positive values include 173, 175, 177, and 179, totaling 4, that is, the first number is 4. The negative values include -179 and -177, totaling 2, that is, the second number is 2.
[0225] In this example, the first number is greater than the second number, so all negative values need to be converted to positive values by adding a preset value. In this example, the preset value may 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]. It can be seen that the elements in the target set are already continuous in value.
[0226] For example, in another example, the pose set is represented as [177, 179, -179, -177, -175, -173], where the positive values include 177 and 179, totaling 2, that is, the first number is 2. The negative values include -179, -177, -175, and -173, totaling 4, that is, the second number is 4.
[0227] In this example, the first number is smaller than the second number, so all positive values need to be converted to negative values by reducing the preset value. In this example, the preset value may be 360. For example, 177-360=-183, 179-360=-181, and the target set obtained after the numerical conversion is represented as [-183,-181,-179,-177,-175,-173]. It can be seen that the elements in the target set are already continuous in value.
[0228] Through the above process, the first pose set and the second pose set shown in formula (1) and formula (2) are subjected to boundary numerical processing respectively, and the first target set and the second target set can be obtained. Then, based on the linear regression process described in Figure 5 above, the first pose change rate corresponding to the first pose set and the second pose change rate corresponding to the second pose set are obtained respectively, which will not be repeated in this disclosure.
[0229] From the above, it can be seen that in the embodiments of the present disclosure, by performing numerical conversion on each element in the posture set and converting each element in the posture set into a continuous numerical value, linear regression can be effectively performed on the posture set with discontinuous mathematical expression.
[0230] In conjunction with the embodiment described in FIG5 , the first posture change rate represents the change trend of the data collected by the first sensor, and the second posture change rate represents the change trend of the data collected by the second sensor. Based on the foregoing, it can be seen that when the first sensor and the second sensor are in normal working condition, theoretically, the change trends of the first posture change rate and the second posture change rate should be consistent or very close. If the difference between the change trends of the two is large, it means that there is a 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 posture change rate and the second posture change rate.
[0231] In the embodiment of the present disclosure, a preset threshold value can be pre-set, which represents a critical value at which the difference between the first posture change rate and the second posture change rate is large. g -k m | is not greater than the preset threshold, indicating 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 the reliability is high. When the difference between the first pose change rate and the second pose change rate |k g -k m If the value is greater than a preset threshold, it indicates that the change trends of the first pose combination and the second pose set are inconsistent, indicating that the second sensor and / or the second sensor are interfering and have poor reliability. The specific value of the preset threshold can be set by those skilled in the art based on specific scenarios, and this disclosure does not impose any limitations thereon.
[0232] In some embodiments, when the difference between the first posture change rate and the second posture change rate is not greater than a preset threshold, it indicates that neither the first sensor nor the second sensor is interfered with, so there is no need to interfere and adjust the weight values of the two, and it is only necessary to maintain the original update process.
[0233] In other embodiments, the first sensor is a gyroscope, and the second sensor is a magnetometer. When determining that the trends of the data collected by the gyroscope and the magnetometer are consistent, considering the higher reliability of the gyroscope compared to the magnetometer, only the magnetometer's posture weight may be adjusted, while the gyroscope's posture weight remains unchanged. When determining that the trends of the data collected by the gyroscope and the magnetometer are inconsistent, the posture weights of both the gyroscope and the magnetometer are adjusted simultaneously. Each of these is explained below.
[0234] As shown in FIG9 , in some embodiments, the pose estimation method of the present disclosure example, the process of determining the second pose weight of the magnetometer includes:
[0235] S910: Use the magnetometer to collect and obtain the second posture data at the current moment.
[0236] S920. Calculate the geomagnetic modulus length based on the second posture data at the current moment.
[0237] S930: Determine a second posture weight according to the geomagnetic modulus.
[0238] As you can understand, a magnetometer is a sensor that measures the strength of the ambient magnetic field. This magnetic field strength is used to obtain the required three-axis azimuth data. When the surrounding magnetic field is disturbed, the geomagnetic modulus B measured by the magnetometer will change. The degree of disturbance can be measured by the rate of change of the geomagnetic modulus.
[0239] In the embodiment of the present disclosure, after the second posture data at the current time t is acquired by using the magnetometer, the second posture data can be normalized to obtain the geomagnetic modulus length B corresponding to the current time t, which is expressed as:
[0240]
[0241] In formula (8), represents the second pose weight of the magnetometer under ideal conditions, B represents the geomagnetic modulus, Represents the second pose weight of the magnetometer at the current time t.
[0242] It can be understood that under ideal conditions, the geomagnetic modulus is 1. The stronger the surrounding magnetic field interference, the smaller the calculated geomagnetic modulus is. Therefore, the second pose weight of the magnetometer at each sampling moment can be calculated in real time through the above formula (8), and as the surrounding magnetic field environment changes, the second pose weight can be dynamically adjusted, so that the greater the degree of surrounding magnetic field interference, the smaller the second pose weight of the magnetometer.
[0243] For the gyroscope, when the difference between the first posture change rate and the second posture change rate is not greater than the preset threshold, the original first posture weight update process can be maintained, and this disclosure will not elaborate on this.
[0244] If the difference between the first and second pose change rates is greater than a preset threshold, this indicates that the current gyroscope may be affected by factors such as vibration, resulting in reduced gyroscope reliability, and the first pose weight of the gyroscope needs to be adjusted. In some embodiments, the first pose weight can be adjusted based on a Kalman filter, as described in detail below.
[0245] First, the first pose weight of the current moment and the previous M sampling moments can be obtained, which is expressed as: It can be understood that the specific value of M can be set according to the scene requirements, for example, M=5, 10, 20, etc., and this disclosure does not limit this.
[0246] Then, based on the first pose weight at M sampling moments, the corresponding variance can be calculated and expressed as That is, the variance of the first pose weight at the past M sampling moments. The process of adjusting the first pose weight at the current moment t based on Kalman filtering can be expressed as:
[0247]
[0248] In formula (9), Indicates the first pose weight corresponding to the current time t, represents the variance of the first pose weight at the past M sampling moments, K represents the first pose weight corresponding to the previous sampling time t-1. M represents the Kalman gain, which is expressed as:
[0249]
[0250] In formula (10), represents the confidence level of the variance of the past M sampling moments, Indicates the confidence of the pose weight of the first pose data.
[0251] Based on the above process, the first position data at 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
[0252] From the above, it can be seen 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 posture weights of each sensor in the data fusion process can be dynamically adjusted in real time, thereby eliminating or reducing the interference of the external environment on the sensor and improving the accuracy of posture estimation.
[0253] In the embodiment of the present disclosure, after obtaining the first position data of the current time t Second pose data First pose weight And the second pose weight After that, we can use the first pose weight and the second pose weight First pose data and the second pose data Perform data fusion processing to obtain the target pose of the mobile device.
[0254] In some implementations, the first pose data and the second pose data may be fused based on a Kalman filter algorithm, which can be expressed as:
[0255]
[0256] In formula (11), ψ t represents the target pose of the mobile device at the current time t, ψ t-1 represents the target pose at the previous sampling time t-1, Represents the first position data at the current time t, represents the second pose data at the current time t. K is the Kalman gain, which is expressed as:
[0257]
[0258] In formula (11), represents the first pose weight, Represents the second pose weight.
[0259] Based on the above process, the target pose of the mobile device corresponding to the current time t can be calculated, that is, the final yaw angle of the mobile device in the above example. It will be understood that in the embodiments of the present disclosure, 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 by the final data fusion.
[0260] From the above, it can be seen that in the embodiment of the present disclosure, by comparing the change trends of the sampling data of the first sensor and the second sensor in the current period of time, the reliability of the two at the current moment is determined, and the posture weights of the two are dynamically adjusted based on the reliability, thereby effectively eliminating or alleviating the defects of data distortion caused by signal interference and improving the accuracy of the target posture obtained after data fusion.
[0261] An embodiment of the present disclosure provides a posture estimation device, which can be applied to, for example, the mobile device 600 shown in FIG1 , for real-time estimation of the posture of the mobile device 600 during movement.
[0262] As shown in FIG10 , in some embodiments, the pose estimation apparatus of the present disclosure example includes:
[0263] An acquisition module 10 is configured to obtain a first pose set of the mobile device at a current moment through a first sensor of the mobile device, and obtain a second pose set of the mobile device at a 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 a plurality of previous sampling moments;
[0264] a change rate determination module 20, configured to determine a first pose change rate of the mobile device according to the first pose set, and to determine a second pose change rate of the mobile device according to the second pose set;
[0265] a weight determination module 30 configured to determine a first posture weight and a second posture weight of the mobile device at a current moment based on a difference between the first posture change rate and the second posture change rate;
[0266] 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.
[0267] From the above, it can be seen that in the embodiment of the present disclosure, by comparing the change trends of the sampling data of the first sensor and the second sensor in the current period of time, the reliability of the two at the current moment is determined, and the posture weights of the two are dynamically adjusted based on the reliability, thereby effectively eliminating or alleviating the defects of data distortion caused by signal interference and improving the accuracy of the target posture obtained after data fusion.
[0268] In some embodiments, the acquisition module 10 is configured to:
[0269] At each sampling moment, first motion data at the sampling moment is acquired based on the first sensor;
[0270] Determining first position change data of the mobile device according to the first motion data at the sampling moment, the first motion data at the previous sampling moment, and a sampling period;
[0271] Determining the first posture data of the mobile device at the sampling moment according to the first posture data at the previous sampling moment and the first posture change data;
[0272] The first posture set is obtained according to the first posture data at the current moment and the first posture data at a preset number of sampling moments before the current moment.
[0273] In some embodiments, the acquisition module 10 is configured to:
[0274] At each sampling moment, obtaining second motion data at the sampling moment based on the second sensor;
[0275] Determining second posture data of the mobile device at the sampling moment according to the second motion data;
[0276] The second posture set is obtained according to the second posture data at the current moment and the second posture data at a preset number of sampling moments before the current moment.
[0277] In some embodiments, the change rate determination module 20 is configured to:
[0278] Performing boundary numerical processing based on each first pose data included in the first pose set to obtain a first target set that is numerically continuous; performing boundary data processing based on each second pose data included in the second pose set to obtain a second target set that is numerically continuous;
[0279] Performing linear regression on each element included in the first target set to obtain a first slope of a first straight line corresponding to the first target set; performing linear regression on each element included in the second target set to obtain a second slope of a second straight line corresponding to the first target set;
[0280] The first slope is determined as the first posture change rate, and the second slope is determined as the second posture change rate.
[0281] In some embodiments, the change rate determination module 20 is configured to:
[0282] In response to each element in the pose set including both positive and negative values, calculating an average of the absolute values of all elements;
[0283] Determining a target boundary corresponding to the pose set according to a first absolute value of a difference between the average value and the first boundary, and a second absolute value of a difference between the average value and the second boundary;
[0284] In response to the target boundary being a preset boundary between the first boundary and the second boundary, numerical conversion is performed on each element in the posture set to obtain a numerically continuous target set.
[0285] In some embodiments, the change rate determination module 20 is configured to:
[0286] In response to the target boundary being a predetermined 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;
[0287] In response to the first number being greater than the second number, increasing the values of all negative elements by a preset value to obtain the target set;
[0288] In response to the first number being not greater than the second number, the value of the positive element is reduced by a preset value to obtain the target set.
[0289] In some implementations, the weight determination module 30 is configured to:
[0290] In response to the difference between the first posture change rate and the second posture change rate being greater than a preset threshold, Kalman filtering is performed on the variance of the first posture data at several sampling moments before the current moment to obtain the first posture weight at the current moment.
[0291] In some embodiments, the second sensor includes a magnetometer; and the weight determination module 30 is configured to:
[0292] Using the magnetometer to collect and obtain second posture data at the current moment;
[0293] The geomagnetic modulus length is calculated based on the second posture data at the current moment;
[0294] The second posture weight is determined according to the geomagnetic modulus.
[0295] In some embodiments, the data fusion module 40 is configured to:
[0296] Kalman filtering is performed on 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.
[0297] In some embodiments, the first sensor includes a gyroscope and the second sensor includes a magnetometer.
[0298] From the above, it can be seen that in the embodiment of the present disclosure, by comparing the change trends of the sampling data of the first sensor and the second sensor in the current period of time, the reliability of the two at the current moment is determined, and the posture weights of the two are dynamically adjusted based on the reliability, thereby effectively eliminating or alleviating the defects of data distortion caused by signal interference and improving the accuracy of the target posture obtained after data fusion.
[0299] In some embodiments, the present disclosure provides a mobile device comprising:
[0300] a first sensor and a second sensor;
[0301] processor; and
[0302] A memory storing computer instructions, wherein the computer instructions are used to enable the processor to execute the method according to any embodiment of the first aspect.
[0303] The mobile device of the embodiment of the present disclosure may refer to the structure and principle of the mobile device 600 shown in FIG1 , and the present disclosure will not elaborate on this.
[0304] In a fourth aspect, an embodiment of the present disclosure provides a storage medium storing computer instructions, wherein the computer instructions are used to enable a computer to execute the method according to any embodiment of the first aspect.
[0305] From the above, it can be seen that in the embodiment of the present disclosure, by comparing the change trends of the sampling data of the first sensor and the second sensor in the current period of time, the reliability of the two at the current moment is determined, and the posture weights of the two are dynamically adjusted based on the reliability, thereby effectively eliminating or alleviating the defects of data distortion caused by signal interference and improving the accuracy of the target posture obtained after data fusion.
[0306] Obviously, the above embodiments are merely examples for clarity of explanation and are not intended to limit the embodiments. Those skilled in the art will appreciate that other variations or modifications can be made based on the above description. It is not necessary and impossible to enumerate all embodiments here. Obvious variations or modifications arising therefrom remain within the scope of protection of the present disclosure.
Claims
1. A pose estimation method, characterized in that: Applied to a mobile device, the method includes: Obtaining a first pose set of the mobile device at a current moment through a first sensor of the mobile device, and obtaining a second pose set of the mobile device at a current moment through a second sensor of the mobile device; wherein the first pose set and the second pose set include pose data at a current moment and at a plurality of previous sampling moments; 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; Determining a first posture weight and a second posture weight of the mobile device at a current moment according to a difference between the first posture change rate and the second posture change rate; The first pose data and the second pose data at the current moment are fused according to the first pose weight and the second pose weight to obtain the target pose of the mobile device at the current moment.
2. The method according to claim 1, characterized in that The obtaining, by using the first sensor of the mobile device, a first pose set of the mobile device at the current moment, includes: At each sampling moment, first motion data at the sampling moment is acquired based on the first sensor; Determining first position change data of the mobile device according to the first motion data at the sampling moment, the first motion data at the previous sampling moment, and a sampling period; Determining the first posture data of the mobile device at the sampling moment according to the first posture data at the previous sampling moment and the first posture change data; The first posture set is obtained according to the first posture data at the current moment and the first posture data at a preset number of sampling moments before the current moment.
3. The method according to claim 1, characterized in that The obtaining, by using a second sensor of the mobile device, a second position set of the mobile device at a current moment, includes: At each sampling moment, obtaining second motion data at the sampling moment based on the second sensor; Determining second posture data of the mobile device at the sampling moment according to the second motion data; The second posture set is obtained according to the second posture data at the current moment and the second posture 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 Determining a first posture change rate of the mobile device according to the first posture set, and determining a second posture change rate of the mobile device according to the second posture set, includes: Performing boundary numerical processing based on each first pose data included in the first pose set to obtain a numerically continuous first target set; performing boundary data processing based on each second pose data included in the second pose set to obtain a numerically continuous second target set; Performing linear regression on each element included in the first target set to obtain a first slope of a first straight line corresponding to the first target set; performing linear regression on each element included in the second target set to obtain a second slope of a 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 posture set and the second posture set includes: In response to each element in the pose set including both positive and negative values, calculating an average of the absolute values of all elements; Determining a target boundary corresponding to the pose set according to a first absolute value of a difference between the average value and the first boundary, and a second absolute value of a difference between the average value and the second boundary; In response to the target boundary being a preset boundary between the first boundary and the second boundary, numerical conversion is performed on each element in the posture 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 of the first boundary and the second boundary, performing numerical conversion on each element in the pose set to obtain a numerically continuous target set, including: In response to the target boundary being a predetermined 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 number being greater than the second number, increasing the values of all negative elements by a preset value to obtain the target set; In response to the first number being not greater than the second number, the value of the positive element is reduced by a preset value to obtain the target set.
7. The method according to claim 1, characterized in that Determining a first posture weight of the mobile device at a current moment according to a difference between the first posture change rate and the second posture change rate includes: In response to the difference between the first posture change rate and the second posture change rate being greater than a preset threshold, Kalman filtering is performed on the variance of the first posture data at several sampling moments before the current moment to obtain the first posture weight at the current moment.
8. The method according to claim 1 or 7, characterized in that The second sensor includes a magnetometer; and determining a second posture weight of the mobile device at a current moment includes: Using the magnetometer to collect and obtain second posture data at the current moment; The geomagnetic modulus length is calculated based on the second posture data at the current moment; The second posture weight is determined according to the geomagnetic modulus.
9. The method according to claim 1, characterized in that The 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: Kalman filtering is performed on 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.
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 posture estimation device, characterized in that: Applied to a mobile device, the device comprises: an acquisition module configured to obtain a first pose set of the mobile device at a current moment through a first sensor of the mobile device, and obtain a second pose set of the mobile device at a 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 a plurality of previous sampling moments; a change rate determination module, configured to determine a first pose change rate of the mobile device according to the first pose set, and to determine a second pose change rate of the mobile device according to the second pose set; a weight determination module configured to determine a first posture weight and a second posture weight of the mobile device at a current moment based on a difference between the first posture change rate and the second posture 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: a first sensor and a second sensor; processor; as well as A memory storing computer instructions, wherein the computer instructions are used to enable 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 make a computer execute the method according to any one of claims 1 to 10.
Citation Information
Patent Citations
Course angle acquiring method and device
CN106153042A
Attitude angle calculating and positioning method and fusion sensor thereof
CN109001787A
Method and device for adjusting holographic content and computer-readable storage medium
CN109218709A
Method and system for optimizing sensor attitude fusion measurement method
CN109631875A
Pose information processing method and device, equipment and storage medium
CN110530356A
