Positioning method and device, robot and computer program product

Abstract

The invention is suitable for the technical field of robots, and provides a positioning method and device, a robot and a computer program product. In the positioning method, the robot can obtain a first pose corresponding to the robot according to first data collected by a visual sensor, can obtain a second pose corresponding to the robot according to second data collected by an odometer, and can obtain a third pose corresponding to the robot according to IMU data collected by an IMU. Afterwards, the robot can determine abnormal poses in the first poses and the second poses and can eliminate the abnormal poses so as to adaptively fuse the normal poses of the first poses and the second poses to optimize the third poses predicted based on the IMU data and obtain the target poses corresponding to the robot, the influence of the abnormal poses on the target poses can be eliminated, and the accuracy of the target poses is improved. The accuracy and reliability of poses during multi-sensor fusion are improved, so that the accuracy of robot positioning can be improved, and the user experience is improved.

Description

Technical Field

[0001] This application belongs to the field of robotics technology, and in particular relates to a positioning method, device, robot, and computer program product. Background Technology

[0002] In the field of robot localization technology, multi-sensor fusion is a common method to improve localization accuracy. For example, data from visual sensors, inertial measurement units (IMUs), and odometry can be fused to determine the robot's pose. However, current fusion methods, such as extended Kalman filtering (EKF) or unscented Kalman filtering (UKF), are prone to introducing erroneous data when fusing data from visual sensors, IMUs, and odometry, resulting in poor localization accuracy. Summary of the Invention

[0003] This application provides a positioning method, apparatus, robot, and computer program product that can improve the accuracy of robot positioning.

[0004] In a first aspect, embodiments of this application provide a positioning method applied to a robot, the robot including a vision sensor, an odometry system, and an inertial measurement unit (IMU), the positioning method including:

[0005] Based on the first data collected by the vision sensor, the first pose of the robot is obtained;

[0006] The second pose of the robot is obtained based on the second data collected by the odometer.

[0007] The third pose of the robot is obtained based on the IMU data collected by the inertial measurement unit (IMU).

[0008] The third pose is optimized based on the fourth pose to obtain the target pose corresponding to the robot; the fourth pose includes the first pose and / or the second pose, and the fourth pose is a pose that does not meet a preset condition, which is used to indicate an abnormal pose.

[0009] In the positioning method described above, the robot can obtain its first pose based on first data collected by a visual sensor, its second pose based on second data collected by an odometry sensor, and its third pose based on IMU data collected by an IMU (Integrated Device Unit). After obtaining the first pose, the robot can perform anomaly detection to determine if it is abnormal. Similarly, after obtaining the second pose, the robot can perform anomaly detection to determine if it is abnormal. Subsequently, the robot can optimize the third pose based on a normal fourth pose to obtain the target pose. In other words, when the robot fuses the first pose obtained from the visual sensor and the second pose obtained from the odometry sensor to determine its target pose, it can identify abnormal poses based on anomaly detection. After identifying abnormal poses, it can adaptively fuse normal poses to optimize the third pose predicted based on IMU data, thus obtaining the target pose. This eliminates the influence of abnormal poses on the target pose, improves the accuracy and reliability of pose fusion during multi-sensor fusion, thereby improving the accuracy of robot positioning and enhancing the user experience.

[0010] In some embodiments, optimizing the third pose based on the fourth pose to obtain the target pose corresponding to the robot may include:

[0011] Using the third pose as the predicted pose and the fourth pose as the observed pose, the third pose is optimized by iterative error state Kalman filtering to obtain the target pose corresponding to the robot.

[0012] In some embodiments, the preset conditions are used to indicate one or more of the following: no data, pose error, or pose drift.

[0013] In one embodiment, before optimizing the third pose based on the fourth pose to obtain the target pose corresponding to the robot, the localization method may further include:

[0014] In response to the fact that the data format of the first pose is a preset data format and the range of the first pose is within a preset range, it is determined that the first pose does not meet the preset condition; wherein, the preset data format is the data format corresponding to the normal pose.

[0015] For example, before optimizing the third pose based on the fourth pose to obtain the target pose corresponding to the robot, the localization method may further include:

[0016] In response to the fact that the data format of the first pose is not the preset data format, or that the range of the first pose is not within the preset range, it is determined that the first pose satisfies the preset condition.

[0017] In another embodiment, before optimizing the third pose based on the fourth pose to obtain the target pose corresponding to the robot, the localization method may further include:

[0018] Obtain the first predicted pose corresponding to the first pose, and obtain the residual corresponding to the first pose based on the first pose and the first predicted pose.

[0019] Obtain the residual covariance corresponding to the first pose;

[0020] Based on the residual and the residual covariance, obtain the chi-square statistic corresponding to the first pose;

[0021] In response to the chi-square statistic being less than or equal to a preset threshold, it is determined that the first pose does not meet the preset condition.

[0022] In some embodiments, the IMU data includes waist IMU data of the robot.

[0023] Secondly, embodiments of this application provide a positioning device applied to a robot, the robot including a vision sensor, an odometer, and an inertial measurement unit (IMU), the positioning device including:

[0024] The first pose acquisition module is used to acquire the first pose of the robot based on the first data collected by the vision sensor.

[0025] The second pose acquisition module is used to acquire the second pose of the robot based on the second data collected by the odometer.

[0026] The third pose acquisition module is used to acquire the third pose of the robot based on the IMU data collected by the inertial measurement unit (IMU).

[0027] The pose optimization module is used to optimize the third pose based on the fourth pose to obtain the target pose corresponding to the robot; the fourth pose includes the first pose and / or the second pose, and the fourth pose is a pose that does not meet a preset condition, the preset condition being used to indicate a pose abnormality.

[0028] In some embodiments, the pose optimization module is specifically used to optimize the third pose as the predicted pose and the fourth pose as the observed pose by using an iterative error state Kalman filter to obtain the target pose corresponding to the robot.

[0029] For example, the preset conditions are used to indicate one or more of the following: no data, pose error, or pose drift.

[0030] In some embodiments, the positioning device may further include:

[0031] The first detection module is configured to determine that the first pose does not meet the preset condition in response to the fact that the data format of the first pose is a preset data format and the range of the first pose is within a preset range; wherein the preset data format is the format corresponding to the normal pose.

[0032] In other embodiments, the first detection module is further configured to determine that the first pose satisfies the preset condition in response to the data format of the first pose not being the preset data format, or the range of the first pose not being within the preset range.

[0033] In some embodiments, the positioning device may further include:

[0034] The second detection module is used to obtain the first predicted pose corresponding to the first pose, and obtain the residual corresponding to the first pose based on the first pose and the first predicted pose; obtain the residual covariance corresponding to the first pose; obtain the chi-square statistic corresponding to the first pose based on the residual and the residual covariance; and determine that the first pose does not meet the preset condition in response to the chi-square statistic being less than or equal to a preset threshold.

[0035] In some embodiments, the IMU data includes waist IMU data of the robot.

[0036] Thirdly, embodiments of this application provide a robot, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it enables the robot to implement the positioning method described in any one of the first aspects above.

[0037] Fourthly, embodiments of this application provide a computer-readable storage medium storing a computer program that, when executed by a robot, enables the robot to implement the positioning method described in any one of the first aspects above.

[0038] Fifthly, embodiments of this application provide a computer program product, the computer program product including a computer program, which, when executed by a robot, enables the robot to implement the positioning method described in any one of the first aspects above.

[0039] It is understood that the beneficial effects of the second to fifth aspects mentioned above can be found in the relevant descriptions in the first aspect mentioned above, and will not be repeated here. Attached Figure Description

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

[0041] Figure 1 This is a flowchart illustrating the positioning method provided in the embodiments of this application. Figure 1 ;

[0042] Figure 2 This is a flowchart illustrating the positioning method provided in the embodiments of this application. Figure 2 ;

[0043] Figure 3 This is a schematic diagram of the positioning device provided in the embodiments of this application;

[0044] Figure 4 This is a schematic diagram of the robot provided in the embodiments of this application. Detailed Implementation

[0045] In the following description, specific details such as particular system architectures and techniques are set forth for illustrative purposes and not for limitation, in order to provide a thorough understanding of the embodiments of this application. However, those skilled in the art will understand that this application may also be implemented in other embodiments without these specific details. In other instances, detailed descriptions of well-known systems, apparatuses, circuits, and methods have been omitted so as not to obscure the description of this application with unnecessary detail.

[0046] It should be understood that, when used in this application specification and the appended claims, the term "comprising" indicates the presence of the described features, integrals, steps, operations, elements and / or components, but does not exclude the presence or addition of one or more other features, integrals, steps, operations, elements, components and / or a collection thereof.

[0047] It should also be understood that the term “and / or” as used in this application specification and the appended claims means any combination of one or more of the associated listed items and all possible combinations, and includes such combinations.

[0048] As used in this application specification and the appended claims, the term "if" may be interpreted, depending on the context, as "when," "once," "in response to determination," or "in response to detection." Similarly, the phrase "if determined" or "if detected [the described condition or event]" may be interpreted, depending on the context, as meaning "once determined," "in response to determination," "once detected [the described condition or event]," or "in response to detection [the described condition or event]."

[0049] Furthermore, in the description of this application and the appended claims, the terms "first," "second," "third," etc., are used only to distinguish descriptions and should not be construed as indicating or implying relative importance.

[0050] References to "one embodiment" or "some embodiments" as described in this specification mean that one or more embodiments of this application include a specific feature, structure, or characteristic described in connection with that embodiment. Therefore, the phrases "in one embodiment," "in some embodiments," "in other embodiments," "in still other embodiments," etc., appearing in different parts of this specification do not necessarily refer to the same embodiment, but rather mean "one or more, but not all, embodiments," unless otherwise specifically emphasized. The terms "comprising," "including," "having," and variations thereof mean "including but not limited to," unless otherwise specifically emphasized.

[0051] In the field of robot localization technology, robot pose can generally be determined using methods such as visual simultaneous localization and mapping (VSLAM), IMU (Integrated Measurement Unit), and odometry. VSLAM can estimate pose through visual feature matching. However, VSLAM is prone to failure in weakly textured areas (such as white walls or dark environments) due to feature loss, leading to localization drift or loss and resulting in poor accuracy. Odometry (e.g., leg odometry) can calculate pose based on the robot's leg kinematics. However, during robot walking, uneven ground or gait vibrations can easily generate accumulated errors and jitter, affecting pose smoothness and leading to poor localization accuracy. IMU can estimate pose based on IMU data (e.g., acceleration from accelerometers and angular velocity from gyroscopes). While IMUs can provide high-frequency motion information, they suffer from integral drift, resulting in poor localization accuracy, especially significant errors during long-term operation.

[0052] Multi-sensor fusion is a common method to improve positioning accuracy. Therefore, to improve positioning accuracy, multi-sensor fusion technology is generally used. For example, data from visual sensors, IMUs, and odometry can be fused to determine the robot's pose. Typical fusion methods, such as EKF or UKF, directly fuse the poses determined by VSLAM, IMU, and odometry without detecting pose anomalies. When VSLAM or odometry malfunctions (e.g., sensor failure or sudden environmental changes), directly fusing the poses determined by VSLAM and / or odometry may introduce erroneous data (i.e., incorrect poses), leading to positioning failure and poor accuracy.

[0053] To address the aforementioned technical problems, embodiments of this application provide a positioning method, apparatus, robot, and computer program product. The robot may include a vision sensor, an odometry system, and an IMU (Integrated Device Unit). In this positioning method, the robot can obtain a first pose based on first data collected by the vision sensor, a second pose based on second data collected by the odometry system, and a third pose based on IMU data. After obtaining the first pose, the robot can perform anomaly detection to determine if it is abnormal. After obtaining the second pose, the robot can perform anomaly detection to determine if it is abnormal. Subsequently, the robot can optimize the third pose based on a normal fourth pose to obtain the target pose. In other words, when a robot fuses the first pose obtained from a visual sensor and the second pose obtained from an odometry to determine the robot's target pose, it can identify abnormal poses based on anomaly detection. After identifying abnormal poses, it can adaptively fuse normal poses to optimize the third pose predicted based on IMU data, thus obtaining the robot's target pose. This eliminates the influence of abnormal poses on the target pose, improves the accuracy and reliability of pose during multi-sensor fusion, thereby improving the accuracy of robot localization, enhancing user experience, and demonstrating strong ease of use and practicality.

[0054] Please see Figure 1 , Figure 1 This application illustrates a schematic flowchart of the positioning method provided in an embodiment. Figure 1 This localization method can be applied to robots. For example, it can be applied to humanoid robots. Humanoid robots may include vision sensors, IMUs, and odometry, etc. The localization method may include:

[0055] S101. The robot obtains its first pose based on the first data collected by the vision sensor.

[0056] For example, the robot may include one or more vision sensors. The vision sensor may be a camera. This application embodiment does not limit the specific type of camera and can determine it according to the actual scenario. For example, it may be a pinhole camera, a fisheye camera, or a panoramic camera. The first data may include images captured by the camera, such as images of the robot's environment. That is, the robot can acquire images of its environment through one or more cameras and can analyze the images to determine the robot's corresponding pose (i.e., the first pose). The first pose may include both position and orientation. The subsequent second and third poses may also include both position and orientation.

[0057] It should be understood that the embodiments of this application do not limit the specific method by which the robot obtains its first pose from the image, and can be determined according to the actual scenario. That is, the robot can determine its first pose based on the image using any existing method. For example, the robot can use VSLAM technology to determine its first pose based on images of the robot's environment captured by a camera.

[0058] It should be noted that the above-described method of obtaining the robot's first pose based on images captured by the camera is merely an illustrative explanation and should not be construed as a limitation on the embodiments of this application. In this embodiment, the camera may integrate VSLAM technology. After acquiring images of the robot's environment, the camera can obtain the robot's first pose based on the images and output the first pose to the robot. That is, the robot can directly obtain its first pose through a camera integrated with VSLAM technology.

[0059] S102. The robot obtains its second pose based on the second data collected by the odometer.

[0060] For example, the robot may include one or more odometry devices. The second data may include odometry data collected by the odometry devices. For instance, the robot may include one or more leg odometry devices. The second data may include odometry data collected by the leg odometry devices. For example, the robot can obtain corresponding leg odometry data through one or more leg odometry devices, and can analyze the leg odometry data through leg kinematics to determine the robot's corresponding pose (i.e., the second pose). It should be understood that the specific content of the odometry data can be determined according to the actual scenario, and this application embodiment does not limit this. Furthermore, this application embodiment does not limit the specific method by which the robot analyzes the leg odometry data through leg kinematics to determine the robot's corresponding second pose, and can determine it according to the actual scenario.

[0061] It should be noted that the above-described method of obtaining the robot's second pose based on foot odometry data is merely illustrative and should not be construed as a limitation on the embodiments of this application. In the embodiments of this application, after the foot odometry obtains the robot's corresponding foot odometry data, it can analyze the data based on leg kinematics to obtain the robot's corresponding second pose and output the second pose to the robot. That is, the robot can directly obtain its corresponding second pose through the foot odometry.

[0062] S103. The robot obtains its third pose based on the IMU data collected by the IMU.

[0063] For example, the IMU may include an accelerometer and a gyroscope. The accelerometer can be used to collect the robot's acceleration. The gyroscope can be used to collect the robot's angular velocity. IMU data may include acceleration and angular velocity. After acquiring the acceleration collected by the accelerometer and the angular velocity collected by the gyroscope, the robot can perform inertial calculations based on the acceleration and angular velocity to determine the robot's corresponding pose (i.e., the third pose). It should be understood that the embodiments of this application do not limit the method by which the robot determines its corresponding third pose based on acceleration and angular velocity, and can be determined according to the actual scenario. For example, the robot can perform one integration of the angular velocity to obtain the robot's corresponding posture, and can perform two integrations of the acceleration to obtain the robot's corresponding position.

[0064] In some embodiments, the robot may include one or more IMUs located on its waist. The IMU data may include waist IMU data. After acquiring the waist IMU data, the robot can obtain its corresponding third pose based on this data. Since the waist is typically the most robust and least deformable part of the robot's body, compared to the easily swaying head or the feet that move violently with the legs, waist movement best represents the robot's overall motion intention and state. It can provide stable, high-frequency IMU data unaffected by complex leg movements, resulting in high accuracy and stability of the third pose obtained based on the robot's waist IMU data, thereby improving the accuracy and stability of robot positioning.

[0065] S104. Optimize the third pose based on the fourth pose to obtain the target pose corresponding to the robot; the fourth pose includes the first pose and / or the second pose, and the fourth pose is a pose that does not meet the preset conditions.

[0066] In some embodiments, after acquiring the first pose, the robot can perform anomaly detection on the first pose to determine whether the first pose is abnormal, for example, to determine whether the first pose meets preset conditions. Similarly, after acquiring the second pose, the robot can perform anomaly detection on the second pose to determine whether the second pose is abnormal, that is, to determine whether the second pose meets preset conditions.

[0067] For example, preset conditions can be used to indicate pose anomalies. A pose anomaly can include one or more of the following: no data, pose error, or pose drift. Preset conditions can be used to indicate one or more of the following: no data, pose error, or pose drift.

[0068] In one embodiment, "no data" may include data interruption and / or data delay. Preset conditions may include data interruption or data delay. For example, after acquiring a first pose based on a vision sensor, the robot can determine whether a data interruption has occurred based on the first pose and the historical pose A. For instance, when the first pose is the same as the historical pose A, the robot can determine that a data interruption has occurred. When the first pose is different from the historical pose A, the robot can determine that no data interruption has occurred. Additionally, the robot can determine whether the first pose is delayed based on the time corresponding to the first pose and the time corresponding to the historical pose A. For instance, when the time difference between the time corresponding to the first pose and the time corresponding to the historical pose A is greater than a preset duration A, the robot can determine that the first pose is delayed. When the time difference between the time corresponding to the first pose and the time corresponding to the historical pose A is less than or equal to the preset duration A, the robot can determine that the first pose is not delayed. Here, the historical pose A can be a pose determined based on visual data acquired by the vision sensor, and the historical pose A can be a pose prior to the first pose. The time corresponding to the first pose can be the time when the first data was acquired. The time corresponding to the historical pose A can be the time when the visual data corresponding to the historical pose A was acquired. The preset duration A can be determined based on the actual scenario. For example, it can be determined based on the time difference between the moments corresponding to two adjacent historical poses A.

[0069] Similarly, after acquiring the second pose based on odometry, the robot can determine whether a data interruption has occurred based on the second pose and the historical pose B. For example, if the second pose is the same as the historical pose B, the robot can determine that a data interruption has occurred. If the second pose is different from the historical pose B, the robot can determine that no data interruption has occurred. Furthermore, the robot can determine whether the second pose is delayed based on the time corresponding to the second pose and the time corresponding to the historical pose B. For example, if the time difference between the time corresponding to the second pose and the time corresponding to the historical pose B is greater than a preset duration B, the robot can determine that the second pose is delayed. If the time difference between the time corresponding to the second pose and the time corresponding to the historical pose B is less than or equal to the preset duration B, the robot can determine that the second pose is not delayed. Here, the historical pose B can be a pose determined based on visual data acquired by a visual sensor, and the historical pose B can be a pose prior to the second pose. The time corresponding to the second pose can be the time when the second data was acquired. The time corresponding to the historical pose B can be the time when the odometry data corresponding to the historical pose B was acquired. The preset duration B can be determined specifically according to the actual scenario. For example, it can be determined based on the time difference between the moments corresponding to two adjacent historical poses B. The preset duration A and the preset duration B can be the same.

[0070] It should be noted that the methods for determining data interruption and data delay described above are merely illustrative and should not be construed as limiting the embodiments of this application. The embodiments of this application can determine the methods for determining data interruption and data delay based on actual scenarios.

[0071] In another embodiment, a pose error may include an incorrect data format or an unreasonable range. Preset conditions may include incorrect data format or an unreasonable range. An incorrect data format may mean that the pose data format is not a pre-set specific data format (hereinafter referred to as the preset data format). An unreasonable range may mean that the pose exceeds the physically possible range (hereinafter referred to as the preset range). It should be understood that the preset data format and preset range can be determined according to the actual scenario, and this application embodiment does not impose any limitations on them.

[0072] For example, after acquiring a first pose based on a vision sensor, the robot can acquire the data format and range of the first pose. If the data format of the first pose is a preset data format and the range of the first pose is within a preset range, the robot can determine that the first pose has no errors. If the data format of the first pose is not a preset data format, or the range of the first pose is not within a preset range, the robot can determine that the first pose has errors. Similarly, after acquiring a second pose based on odometry, the robot can acquire the data format and range of the second pose. If the data format of the second pose is a preset data format and the range of the second pose is within a preset range, the robot can determine that the second pose has no errors. If the data format of the second pose is not a preset data format, or the range of the second pose is not within a preset range, the robot can determine that the second pose has errors.

[0073] In another embodiment, the robot can detect pose drift using a chi-square test. That is, the robot can use a chi-square test to detect whether the first pose is drifting, and it can also use a chi-square test to detect whether the second pose is drifting.

[0074] For example, after obtaining the first pose, the robot can also obtain the first predicted pose corresponding to the first pose, and obtain the residual corresponding to the first pose based on the first pose and the first predicted pose. The time corresponding to the first predicted pose can be the same as the time corresponding to the first pose. The first predicted pose can be a pose predicted based on historical visual data. Historical visual data can be visual data collected by the visual sensor before the first data. Among them, when determining the residual corresponding to the first pose, the observation can be the pose increment or the absolute pose, and the error between the two poses can be defined by the logarithmic mapping SE(2) on the manifold. In addition, the robot only needs to consider the position and yaw angle when walking on the ground, that is, it can only consider the three degrees of freedom of x, y and yaw, that is, the residual dimension k can be 3. For example, the residual corresponding to the first pose can be: z can be the first pose, and x can be the first predicted pose. Alternatively, z can be the pose increment corresponding to the first pose, and x can be the pose increment corresponding to the first predicted pose.

[0075] Furthermore, after obtaining the first pose, the robot can obtain the residual covariance corresponding to the first pose. For example, the residual covariance corresponding to the first pose can be: S = HPH T +R. H is the observation matrix. P can be the prediction covariance, and R is the observation noise covariance.

[0076] After determining the residuals and residual covariance, the robot can obtain the chi-square statistic corresponding to the first pose based on the residuals and residual covariance. For example, the chi-square statistic corresponding to the first pose can be: χ² 2 =RT S -1 r. Additionally, the robot can also find the critical value of the chi-square distribution based on the residual dimension (i.e., k=3) and the preset significance level α. (This can be referred to as the preset threshold). Subsequently, the robot can determine whether the chi-square statistic corresponding to the first pose is greater than the preset threshold. When the chi-square statistic corresponding to the first pose is greater than the preset threshold, the robot can determine that the first pose has drifted. When the chi-square statistic corresponding to the first pose is less than or equal to the preset threshold, the robot can determine that the first pose has not drifted.

[0077] Similarly, after acquiring the second pose, the robot can also acquire the second predicted pose corresponding to the second pose, and obtain the residual corresponding to the second pose based on the second pose and the second predicted pose. The time corresponding to the second predicted pose can be the same as the time corresponding to the second pose. The second predicted pose can be a pose predicted based on historical odometry data. The historical odometry data can be odometry data collected before the second odometry data. It should be understood that the process of determining the residual corresponding to the second pose is similar to the process of determining the residual corresponding to the first pose; for details, please refer to the process of determining the residual corresponding to the first pose, which will not be elaborated here.

[0078] After acquiring the second pose, the robot can also acquire the corresponding residual covariance. After determining the residual and residual covariance for the second pose, the robot can obtain the chi-square statistic for the second pose based on these residuals and covariance, and determine whether the chi-square statistic is greater than a preset threshold. When the chi-square statistic is greater than the preset threshold, the robot can determine that the second pose has drifted. When the chi-square statistic is less than or equal to the preset threshold, the robot can determine that the second pose has not drifted.

[0079] In one embodiment, in a scenario where a preset condition indicates no data, and no data includes data interruption and data delay, when a data interruption or delay is determined based on the first pose, the robot can determine that the first pose meets the preset condition, thus identifying the first pose as abnormal. When no data interruption or delay is determined based on the first pose, the robot can determine that the first pose does not meet the preset condition, thus identifying the first pose as normal. Similarly, when a data interruption or delay is determined based on the second pose, the robot can determine that the second pose meets the preset condition, thus identifying the second pose as abnormal. When no data interruption or delay is determined based on the second pose, the robot can determine that the second pose does not meet the preset condition, thus identifying the second pose as normal.

[0080] In another embodiment, in a scenario where preset conditions are used to indicate pose errors, and pose errors include incorrect data format or unreasonable ranges, when it is determined that the first pose has an error—for example, when the data format of the first pose is not a preset data format, or the range of the first pose is not within a preset range—the robot can determine that the first pose meets the preset conditions, thus determining that the first pose is abnormal. When it is determined that the first pose does not have an error—for example, when the data format of the first pose is a preset data format, and the range of the first pose is within a preset range—the robot can determine that the first pose does not meet the preset conditions, thus determining that the first pose is normal. Similarly, when it is determined that the second pose has an error—for example, when the data format of the second pose is not a preset data format, or the range of the second pose is not within a preset range—the robot can determine that the second pose meets the preset conditions, thus determining that the second pose is abnormal. When it is determined that the second pose does not have an error—for example, when the data format of the second pose is a preset data format, and the range of the second pose is within a preset range—the robot can determine that the second pose does not meet the preset conditions, thus determining that the first pose is normal.

[0081] In another embodiment, in a scenario where preset conditions are used to indicate pose drift, when it is determined that the first pose has drifted, for example, when the chi-square statistic corresponding to the first pose is greater than a preset threshold, the robot can determine that the first pose meets the preset conditions, thus identifying the first pose as abnormal. When it is determined that the first pose has not drifted, for example, when the chi-square statistic corresponding to the first pose is less than or equal to the preset threshold, the robot can determine that the first pose does not meet the preset conditions, thus identifying the first pose as normal. Similarly, when it is determined that the second pose has drifted, for example, when the chi-square statistic corresponding to the second pose is greater than a preset threshold, the robot can determine that the second pose meets the preset conditions, thus identifying the second pose as abnormal. When it is determined that the second pose has not drifted, for example, when the chi-square statistic corresponding to the second pose is less than or equal to the preset threshold, the robot can determine that the second pose does not meet the preset conditions, thus identifying the second pose as normal.

[0082] In another embodiment, preset conditions are used to indicate multiple scenarios, including no data, pose error, or pose drift. The following will use preset conditions indicating no data, pose error, and pose drift as examples for illustrative explanation.

[0083] For example, when the robot determines that a data interruption or delay has occurred, or that the first pose is erroneous, or that the first pose is drifting, based on the first pose, the robot can determine that the first pose meets preset conditions, thus identifying the first pose as abnormal. When the robot determines that there is no data interruption or delay, no error, and no drifting based on the first pose, the robot can determine that the first pose does not meet preset conditions, thus identifying the first pose as normal. Similarly, when the robot determines that a data interruption or delay has occurred, or that the second pose is erroneous, or that drifting based on the second pose, the robot can determine that the second pose meets preset conditions, thus identifying the second pose as abnormal. When the robot determines that there is no data interruption or delay, no error, and no drifting based on the second pose, the robot can determine that the second pose does not meet preset conditions, thus identifying the second pose as normal.

[0084] For example, when both the first and second poses do not meet preset conditions (i.e., when both the first and second poses are normal), the fourth pose can include both the first and second poses. The robot can optimize the third pose based on the first and second poses to obtain the robot's target pose. When the first pose does not meet preset conditions but the second pose does (i.e., when the first pose is normal and the second pose is abnormal), the fourth pose can include the first pose. The robot can optimize the third pose based on the first pose to obtain the robot's target pose. When the first pose meets preset conditions but the second pose does not (i.e., when the first pose is abnormal and the second pose is normal), the fourth pose can include the second pose. The robot can optimize the third pose based on the second pose to obtain the robot's target pose.

[0085] For example, when optimizing the third pose based on the fourth pose, the robot can use the third pose as the predicted pose and the fourth pose as the observed pose. It then uses an iterated error-state Kalman filter (IESKF) to optimize the third pose, obtaining the robot's target pose. That is, during pose optimization using IESKF, the third pose can be used as the observed pose, while simultaneously fusing the first and second poses. By minimizing the error covariance, the third pose can be optimized to obtain the robot's target pose. IESKF employs an iterative update method, using multiple linearization processes to handle nonlinear problems, thereby improving pose fusion accuracy. This results in a smooth and robust target pose, enhancing the accuracy of the target pose and improving the accuracy and precision of robot navigation and control.

[0086] In this embodiment, after obtaining the first and second poses, abnormal data from VSLAM and foot odometry can be eliminated in real time through anomaly detection. For example, VSLAM failure data in weak texture areas or abnormal data from foot odometry in vibration environments can be eliminated, preventing pose optimization based on erroneous data and preventing the positioning system from crashing. This can improve the reliability of the positioning system in weak texture areas or dynamic environments, ensure accurate positioning and continuous operation in complex scenarios (such as indoor low light or outdoor dynamic environments), enhance the applicability of the positioning system in complex scenarios, and provide a solid foundation for applications such as autonomous navigation and obstacle avoidance of robots.

[0087] Furthermore, after anomaly detection, adaptive pose fusion can be achieved based on the anomaly situation. This means adaptively optimizing the third pose based on the normal pose to obtain the robot's target pose. When data is normal, multi-source data can be fully utilized for pose optimization; when data is abnormal, normal data can be relied upon for pose optimization, improving positioning accuracy and ensuring continuity. Moreover, IESKF uses IMU data as a reference for pose optimization, combining multiple sensors for observation reduces reliance on a single sensor, decreases cumulative pose error, and reduces pose jitter caused by walking vibrations in humanoid robots. This results in a smooth and accurate target pose, thereby improving the robot's motion stability and control precision. For example, experiments show that the positioning method provided in this application can reduce the root mean square error of pose estimation by approximately 20%. Simultaneously, IESKF's efficient iterative computation ensures real-time performance, meeting the robot's real-time navigation requirements.

[0088] In some embodiments, after acquiring the first pose, the robot can perform callback processing on the first pose. After performing callback processing on the first pose, the robot can perform anomaly detection on the first pose. Similarly, after acquiring IMU data, the robot can perform callback processing on the IMU data. After performing callback processing on the IMU data, the robot can perform inertial calculations on the IMU data to obtain the corresponding third pose.

[0089] It should be noted that callback processing can include tasks such as saving data and adding timestamps.

[0090] For example, please see Figure 2 , Figure 2 This application illustrates a schematic flowchart of the positioning method provided in an embodiment. Figure 2 .

[0091] like Figure 2 As shown, this localization method can include data acquisition, callback processing, anomaly detection, IMU inertial calculation, and pose fusion. In data acquisition, the robot can acquire a first pose based on VSLAM, a second pose based on foot odometry, and IMU data. In callback processing, the robot can perform callback processing on the first pose, second pose, and IMU data respectively. In anomaly detection, the robot can detect whether the first pose is abnormal, for example, whether it meets preset conditions, and can also detect whether the second pose is abnormal, for example, whether it meets preset conditions. In IMU inertial calculation, the robot can perform inertial calculation on the IMU data to obtain the corresponding third pose. In pose fusion, the robot can optimize the third pose based on the normal first pose and / or second pose to obtain the target pose. The time corresponding to the first pose and the second pose can be the same as the time corresponding to the IMU data; that is, in pose fusion, the robot can optimize the pose based on synchronous data (e.g., data from the same time), improving the accuracy of the target pose. It should be understood that the time corresponding to the first pose can refer to the time when the vision sensor acquires the first data corresponding to the first pose, and the time corresponding to the second pose can refer to the time when the odometer acquires the second data corresponding to the second pose. The time corresponding to the IMU data can refer to the time when the IMU acquires the IMU data.

[0092] For example, in anomaly detection, when an anomaly is detected in the first pose, the robot can mark the first pose as unavailable. When a normal first pose is detected, the robot can mark the first pose as available. Similarly, when an anomaly is detected in the second pose, the robot can mark the second pose as unavailable. When a normal second pose is detected, the robot can mark the second pose as available. In pose fusion, the robot can determine whether both the first and second poses are available. When both the first and second poses are available, the robot can use IESKF to fuse the first and second poses to optimize the third pose, obtaining the robot's target pose. When only one of the first and second poses is available, for example, when only the first pose is available, the robot can use IESKF to fuse the first pose to optimize the third pose, obtaining the robot's target pose. For example, when only the second pose is available, the robot can use IESKF to fuse the second pose to optimize the third pose, obtaining the robot's target pose. When both the first and second poses are determined to be unavailable, the robot can use the pose from the previous frame to determine its current target pose and output a warning to prompt relevant personnel to handle the situation. After determining the target pose, the robot can decide whether to continue pose estimation. If it decides to continue, the robot can proceed with determining the pose for the next frame using the aforementioned localization method. If it determines that pose estimation is no longer necessary, the robot can terminate the localization process.

[0093] It should be noted that when odometry data is unavailable, such as when the robot is being lifted, the robot can determine its target pose based solely on its first pose and IMU data. Figure 2 The second pose may not be included in the data acquisition, the callback processing may not include the callback processing of the second pose, and the anomaly detection may not include the anomaly detection of the second pose.

[0094] It should be understood that the sequence number of each step in the above embodiments does not imply the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of this application.

[0095] Corresponding to the positioning method described in the above embodiments, Figure 3 A structural block diagram of a positioning device provided in an embodiment of this application is shown. This positioning device can be applied to a robot, which may include a vision sensor, an IMU, and an odometer. For ease of explanation, only the parts relevant to the embodiments of this application are shown.

[0096] Reference Figure 3 The positioning device includes:

[0097] The first pose acquisition module 301 is used to acquire the first pose of the robot based on the first data collected by the vision sensor.

[0098] The second pose acquisition module 302 is used to acquire the second pose of the robot based on the second data collected by the odometer.

[0099] The third pose acquisition module 303 is used to acquire the third pose of the robot based on the IMU data collected by the inertial measurement unit (IMU).

[0100] The pose optimization module 304 is used to optimize the third pose according to the fourth pose to obtain the target pose corresponding to the robot; the fourth pose includes the first pose and / or the second pose, and the fourth pose is a pose that does not meet a preset condition, the preset condition being used to indicate a pose abnormality.

[0101] In some embodiments, the pose optimization module 304 is specifically used to optimize the third pose using the third pose as the predicted pose and the fourth pose as the observed pose, through iterative error state Kalman filtering, to obtain the target pose corresponding to the robot.

[0102] For example, the preset conditions are used to indicate one or more of the following: no data, pose error, or pose drift.

[0103] In some embodiments, the positioning device may further include:

[0104] The first detection module is configured to determine that the first pose does not meet the preset condition in response to the fact that the data format of the first pose is a preset data format and the range of the first pose is within a preset range; wherein the preset data format is the format corresponding to the normal pose.

[0105] In other embodiments, the first detection module is further configured to determine that the first pose satisfies the preset condition in response to the data format of the first pose not being the preset data format, or the range of the first pose not being within the preset range.

[0106] In some embodiments, the positioning device may further include:

[0107] The second detection module is used to obtain the first predicted pose corresponding to the first pose, and obtain the residual corresponding to the first pose based on the first pose and the first predicted pose; obtain the residual covariance corresponding to the first pose; obtain the chi-square statistic corresponding to the first pose based on the residual and the residual covariance; and determine that the first pose does not meet the preset condition in response to the chi-square statistic being less than or equal to a preset threshold.

[0108] In some embodiments, the IMU data includes waist IMU data of the robot.

[0109] It should be noted that the information interaction and execution process between the above-mentioned devices / units are based on the same concept as the method embodiments of this application. For details on their specific functions and technical effects, please refer to the method embodiments section, and they will not be repeated here.

[0110] Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the above-described division of functional units and modules is merely an example. In practical applications, the above functions can be assigned to different functional units and modules as needed, that is, the internal structure of the device can be divided into different functional units or modules to complete all or part of the functions described above. The functional units and modules in the embodiments can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The integrated unit can be implemented in hardware or as a software functional unit. Furthermore, the specific names of the functional units and modules are only for easy differentiation and are not intended to limit the scope of protection of this application. The specific working process of the units and modules in the above system can be referred to the corresponding process in the foregoing method embodiments, and will not be repeated here.

[0111] Figure 4 A schematic diagram of the robot provided in an embodiment of this application is shown. Figure 4 As shown, the robot 4 in this embodiment includes: at least one vision sensor 43 ( Figure 4 Only one is shown in the image), and at least one odometer 44 ( Figure 4 Only one is shown in the image), at least one IMU 45 ( Figure 4 Only one is shown in the image), at least one processor 40 ( Figure 4 (Only one is shown in the diagram), memory 41, and computer program 42 stored in the memory 41 and executable on the at least one processor 40, wherein the processor 40 executes the computer program 42 to implement the steps in any of the above-described positioning method embodiments.

[0112] The robot 4 may include, but is not limited to, a vision sensor 43, an odometer 44, an IMU 45, a processor 40, and a memory 41. Those skilled in the art will understand that... Figure 4 The example shown is merely of robot 4 and does not constitute a limitation on robot 4. It may include more or fewer parts than shown, or combine certain parts, or different parts, such as input / output devices, network access devices, etc.

[0113] The processor 40 can be a central processing unit (CPU), but it can also be other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. The general-purpose processor can be a microprocessor or any conventional processor.

[0114] In some embodiments, the memory 41 may be an internal storage unit of the robot 4, such as a hard disk or memory of the robot 4. In other embodiments, the memory 41 may be an external storage device of the robot 4, such as a plug-in hard disk, smart media card (SMC), secure digital (SD) card, flash card, etc., equipped on the robot 4. Furthermore, the memory 41 may include both internal storage units and external storage devices of the robot 4. The memory 41 is used to store operating systems, applications, bootloaders, data, and other programs, such as the program code of computer programs. The memory 41 can also be used to temporarily store data that has been output or will be output.

[0115] This application also provides a computer-readable storage medium storing a computer program that, when executed by a robot, causes the robot to perform the steps described in the above-described positioning method embodiments.

[0116] This application provides a computer program product, which includes a computer program that, when executed by a robot, enables the robot to perform the steps described in the various positioning method embodiments above.

[0117] If the integrated unit is implemented as a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, all or part of the processes in the methods of the above embodiments can be implemented by a computer program instructing related hardware. The computer program can be stored in a computer-readable storage medium, and when executed by a processor, it can implement the steps of the various method embodiments described above. The computer program includes computer program code, which can be in the form of source code, object code, executable files, or some intermediate form. The computer-readable storage medium can include at least: any entity or device capable of carrying the computer program code to a device / robot, a recording medium, a computer memory, a read-only memory (ROM), a random access memory (RAM), an electrical carrier signal, a telecommunication signal, and a software distribution medium. Examples include USB flash drives, portable hard drives, magnetic disks, or optical disks.

[0118] In the above embodiments, the descriptions of each embodiment have different focuses. For parts that are not described in detail or recorded in a certain embodiment, please refer to the relevant descriptions of other embodiments.

[0119] Those skilled in the art will recognize that the units and algorithm steps of the various examples described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of this application.

[0120] In the embodiments provided in this application, it should be understood that the disclosed devices / robots and methods can be implemented in other ways. For example, the device / robot embodiments described above are merely illustrative. For instance, the division of modules or units is only a logical functional division, and in actual implementation, there may be other division methods. For example, multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed. Furthermore, the coupling or direct coupling or communication connection shown or discussed may be through some interfaces; the indirect coupling or communication connection between devices or units may be electrical, mechanical, or other forms.

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

[0122] The above-described embodiments are only used to illustrate the technical solutions of this application, and are not intended to limit them. Although this application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of this application, and should all be included within the protection scope of this application.

Claims

1. A positioning method, characterized in that, The method of localization is applied to a robot, which includes a vision sensor, an odometry system, and an inertial measurement unit (IMU). Based on the first data collected by the vision sensor, the first pose of the robot is obtained; The second pose of the robot is obtained based on the second data collected by the odometer. The third pose of the robot is obtained based on the IMU data collected by the inertial measurement unit (IMU). The third pose is optimized based on the fourth pose to obtain the target pose corresponding to the robot; the fourth pose includes the first pose and / or the second pose, and the fourth pose is a pose that does not meet a preset condition, which is used to indicate an abnormal pose.

2. The positioning method according to claim 1, characterized in that, The step of optimizing the third pose based on the fourth pose to obtain the target pose corresponding to the robot includes: Using the third pose as the predicted pose and the fourth pose as the observed pose, the third pose is optimized by iterative error state Kalman filtering to obtain the target pose corresponding to the robot.

3. The positioning method according to claim 1, characterized in that, The preset conditions are used to indicate one or more of the following: no data, incorrect pose, or pose drift.

4. The positioning method according to claim 3, characterized in that, Before optimizing the third pose based on the fourth pose to obtain the target pose corresponding to the robot, the localization method further includes: In response to the fact that the data format of the first pose is a preset data format and the range of the first pose is within a preset range, it is determined that the first pose does not meet the preset condition; wherein, the preset data format is the data format corresponding to the normal pose.

5. The positioning method according to claim 4, characterized in that, Before optimizing the third pose based on the fourth pose to obtain the target pose corresponding to the robot, the localization method further includes: In response to the fact that the data format of the first pose is not the preset data format, or that the range of the first pose is not within the preset range, it is determined that the first pose satisfies the preset condition.

6. The positioning method according to claim 3, characterized in that, Before optimizing the third pose based on the fourth pose to obtain the target pose corresponding to the robot, the localization method further includes: Obtain the first predicted pose corresponding to the first pose, and obtain the residual corresponding to the first pose based on the first pose and the first predicted pose. Obtain the residual covariance corresponding to the first pose; Based on the residual and the residual covariance, obtain the chi-square statistic corresponding to the first pose; In response to the chi-square statistic being less than or equal to a preset threshold, it is determined that the first pose does not meet the preset condition.

7. The positioning method according to any one of claims 1 to 6, characterized in that, The IMU data includes the robot's waist IMU data.

8. A positioning device, characterized in that, The positioning device is applied to a robot, which includes a vision sensor, an odometry system, and an inertial measurement unit (IMU). The first pose acquisition module is used to acquire the first pose of the robot based on the first data collected by the vision sensor. The second pose acquisition module is used to acquire the second pose of the robot based on the second data collected by the odometer. The third pose acquisition module is used to acquire the third pose of the robot based on the IMU data collected by the inertial measurement unit (IMU). The pose optimization module is used to optimize the third pose based on the fourth pose to obtain the target pose corresponding to the robot; the fourth pose includes the first pose and / or the second pose, and the fourth pose is a pose that does not meet a preset condition, the preset condition being used to indicate a pose abnormality.

9. A robot comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the computer program, it causes the robot to implement the positioning method as described in any one of claims 1 to 7.

10. A computer program product, the computer program product comprising a computer program, characterized in that, When the computer program is executed by the robot, the robot performs the positioning method as described in any one of claims 1 to 7.

Images