Robot state estimation method and device and electronic equipment

Abstract

The invention provides a robot state estimation method and device and electronic equipment, and the method comprises the steps: obtaining a plurality of pieces of state data, collected by a plurality of sensors in a target robot, at the current moment, and obtaining a historical state queue of the target robot, the historical state queue comprises optimal pose estimation values of the target robot at a plurality of historical moments before the current moment, a preset pose prediction model is adopted to predict the historical state queue, and a pose state prediction value of the target robot at the current moment is obtained, and determining an optimal pose estimation value of the target robot at the current moment according to the pose state prediction value of the target robot at the current moment and the plurality of observation values at the current moment. According to the method provided by the invention, the continuity and accuracy of pose estimation are realized by combining the time sequence information of the historical state queue and the look-ahead ability of the preset prediction model through the plurality of state data at the current moment acquired by the plurality of sensors and then through fusion optimization of the plurality of observation values.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of robots, in particular to a robot state estimation method, device and electronic equipment. BACKGROUND

[0002] Autonomous mobile robot technology is increasingly widely used in outdoor service fields. Among them, autonomous mowing robots have become the core equipment of garden maintenance automation due to their efficient and convenient operation advantages. Precise and reliable positioning and navigation capabilities are the core foundation for autonomous mowing robots to achieve full-coverage lawn mowing, dynamic obstacle avoidance and operation path optimization, and directly determine the operation quality and safety performance of the equipment. However, the typical operation scene of autonomous mowing robots is an outdoor unstructured garden environment, which has characteristics such as dynamic changes in light intensity (such as direct sunlight, tree shade), sparse and uneven ground texture (such as large homogeneous lawns), undulating terrain, and random distribution of weeds, stones and other obstacles, which makes sensor data susceptible to interference and poses a serious environmental challenge to state estimation.

[0003] At present, the positioning and navigation system of outdoor mowing robots mostly adopts a multi-sensor fusion scheme, and the mainstream configuration takes binocular vision, wheel speed meter and inertial measurement unit as the core, combined with traditional filtering algorithms to realize state estimation. Among them, binocular vision directly recovers the three-dimensional scale information of the environment through stereo matching, which is the key to visual positioning; the wheel speed meter provides mileage information by integrating the wheel speed, making up for the cumulative error of visual positioning; the inertial measurement unit can quickly respond to changes in the robot's attitude and provide high-frequency motion state data. However, the existing technical solutions have significant defects: on the one hand, the performance of each sensor is greatly affected by the environment, binocular vision is prone to matching failure in sparse texture areas such as lawns or complex lighting conditions such as strong light and shadow, wheel speed meters will output incorrect mileage information when the ground is slippery, and inertial measurement units have inherent zero drift and noise interference; on the other hand, traditional fusion algorithms (such as extended Kalman filter) use fixed parameter settings and lack adaptive adjustment capabilities for sensor dynamic failure or data anomalies. When a single sensor data is distorted or multiple sensor data conflicts, it cannot effectively identify faults and optimize the fusion strategy, ultimately leading to divergence of the state estimation result, seriously affecting the operation accuracy and reliability of the mowing robot. SUMMARY

[0004] The purpose of the present application is to provide a robot state estimation method, device and electronic equipment to solve the problems in the prior art, which realizes the continuity and accuracy of pose estimation by combining the multiple state data collected by multiple sensors at the current time, the time sequence information of the historical state queue and the foresight ability of the preset prediction model, and then optimizing multiple observation values.

[0005] To achieve the above object, the technical scheme adopted by the embodiments of the present application is as follows: In a first aspect, the embodiments of the present application provide a robot state estimation method, comprising: obtaining a plurality of state data collected by a plurality of sensors of a target robot at a current time; determining a plurality of observation values at the current time according to the plurality of state data at the current time; obtaining a historical state queue of the target robot, the historical state queue comprising optimal pose estimation values of the target robot at a plurality of historical times before the current time; predicting the historical state queue by using a preset pose prediction model to obtain a pose state prediction value of the target robot at the current time; determining an optimal pose estimation value of the target robot at the current time according to the pose state prediction value of the target robot at the current time and the plurality of observation values at the current time.

[0006] In an optional implementation, the step of predicting the historical state queue by using the preset pose prediction model to obtain the pose state prediction value of the target robot at the current time comprises: predicting the historical state queue by using the preset pose prediction model to obtain a relative pose increment of the target robot at the current time; determining the pose state prediction value of the target robot at the current time according to the relative pose increment of the target robot at the current time and an optimal pose estimation value of the target robot at a previous time.

[0007] In an optional implementation, the step of determining the optimal pose estimation value of the target robot at the current time according to the pose state prediction value of the target robot at the current time and the plurality of observation values at the current time comprises: determining a chi-square test value of the plurality of sensors according to the pose state prediction value at the current time and the plurality of observation values at the current time; determining a credibility weight of the plurality of sensors based on the chi-square test value of the plurality of sensors; determining an observation noise covariance according to the credibility weight of the plurality of sensors; determining the optimal pose estimation value of the target robot at the current time according to the observation noise covariance, the pose state prediction value of the target robot at the current time and the plurality of observation values at the current time.

[0008] In an optional implementation, determining the chi-square test value of the multiple sensors based on the predicted pose state of the target robot at the current moment and multiple observations at the current moment includes: Based on the predicted pose state of the target robot at the current moment and multiple observations at the current moment, the standardized residuals of the multiple sensors are determined; Based on the standardized residuals, the global chi-square check value of the multiple sensors is determined; Based on the global chi-square check value of the multiple sensors, determine whether the multiple sensors are faulty; If it is determined that the multiple sensors are faulty, then the chi-square test value of the multiple sensors is determined based on the predicted pose state of the target robot at the current moment and the multiple observation values ​​at the current moment.

[0009] In an optional implementation, determining the reputation weights of the plurality of sensors based on the chi-square test values ​​of the plurality of sensors includes: Based on the predicted pose state of the target robot at the current moment and multiple observations at the current moment, determine the Mahalanobis distances corresponding to multiple sensors; Based on the chi-square test values ​​of the multiple sensors, and according to the Mahalanobis distances corresponding to the multiple sensors, the reputation weights of the multiple sensors are determined.

[0010] In an optional implementation, determining the optimal pose estimate of the target robot at the current time based on the observation noise covariance, the predicted pose state of the target robot at the current time, and multiple observations at the current time includes: The error covariance at the current moment is determined based on the target robot's error covariance, state transition matrix, and process noise at the previous moment. The gain matrix is ​​determined based on the error covariance at the current moment, the preset coefficient matrix, and the observation noise covariance. The error state is determined based on the gain matrix, the predicted pose state of the target robot at the current moment, and multiple observations at the current moment. Based on the predicted pose state of the target robot at the current moment and the error state, the optimal pose estimate of the target robot at the current moment is determined.

[0011] In an optional implementation, before using the preset pose prediction model to predict the historical state queue and obtain the relative pose increment of the target robot at the current moment, the method further includes: Obtain sample pairs, the sample pairs including: the actual pose increment of the target robot at a first moment, and pose estimates at multiple second moments prior to the first moment; Based on the pose estimation values ​​at the multiple second time points, the initial pose prediction model is used to predict the pose increment at the first time point, thereby obtaining the predicted pose increment at the first time point. Based on the predicted pose increment and the actual pose increment at the first moment, the parameters of the initial pose prediction model are adjusted to obtain the preset pose prediction model.

[0012] In an optional implementation, the preset pose prediction model includes: a convolutional layer, a temporal prediction layer, and an output layer. The step of using the preset pose prediction model to predict the historical state queue to obtain the relative pose increment of the target robot at the current moment includes: The convolutional layer is used to extract features from the historical state queue to obtain a local feature sequence; The local feature sequence is predicted using the temporal prediction layer to obtain the hidden state information at the current time. The output layer is used to map the hidden state information at the current moment to obtain the relative pose increment of the target robot at the current moment.

[0013] Secondly, embodiments of this application also provide a robot state estimation device, the device comprising: The acquisition module is used to acquire multiple state data at the current moment collected by multiple sensors in the target robot; The determination module is used to determine multiple observation values ​​at the current time based on multiple state data at the current time. The acquisition module is further configured to acquire the historical state queue of the target robot, the historical state queue including: the optimal pose estimation values ​​of the target robot at multiple historical moments before the current moment; The prediction module is used to predict the historical state queue using a preset pose prediction model to obtain the predicted pose state value of the target robot at the current moment. The determining module is further configured to determine the optimal pose estimate of the target robot at the current time based on the predicted pose state of the target robot at the current time and multiple observation values ​​at the current time.

[0014] Thirdly, embodiments of this application also provide an electronic device, including: a processor, a storage medium, and a bus, wherein the storage medium stores program instructions executable by the processor, and when the electronic device is running, the processor communicates with the storage medium via the bus, and the processor executes the program instructions to perform the steps of the robot state estimation method as described in any of the first aspects.

[0015] Fourthly, embodiments of this application also provide a computer-readable storage medium storing a computer program, which, when executed by a processor, performs the steps of the robot state estimation method as described in any of the first aspects.

[0016] The beneficial effects of this application are: This application provides a robot state estimation method, apparatus, and electronic device. The method includes: acquiring multiple state data at the current moment collected by multiple sensors in a target robot; acquiring a historical state queue of the target robot, wherein the historical state queue includes: optimal pose estimates of the target robot at multiple historical moments prior to the current moment; predicting the historical state queue using a preset pose prediction model to obtain a predicted pose state value of the target robot at the current moment; and determining the optimal pose estimate of the target robot at the current moment based on the predicted pose state value of the target robot at the current moment and multiple observations at the current moment. This method, by combining multiple state data at the current moment collected by multiple sensors with the temporal information of the historical state queue and the look-ahead capability of the preset prediction model, and further optimizing through the fusion of multiple observations, achieves continuity and accuracy in pose estimation, improves the reliability of the target robot's positioning and navigation in complex outdoor scenarios, and provides stable core support for full-coverage operations and precise obstacle avoidance. Attached Figure Description

[0017] To more clearly illustrate the technical solutions of the embodiments of the present invention, the accompanying drawings used in the embodiments will be briefly introduced below. It should be understood that the following drawings only show some embodiments of the present invention and should not be regarded as a limitation on the scope. For those skilled in the art, other related drawings can be obtained based on these drawings without creative effort.

[0018] Figure 1 One of the flowcharts for a robot state estimation method provided in this application embodiment; Figure 2 A second schematic flowchart illustrating a robot state estimation method provided in this application embodiment; Figure 3 The third schematic flowchart of a robot state estimation method provided in this application embodiment; Figure 4 The fourth flowchart illustrates a robot state estimation method provided in this application embodiment; Figure 5 Fifth of a flowchart illustrating a robot state estimation method provided in this application embodiment; Figure 6 A flowchart illustrating a robot state estimation method provided in this application is shown in Figure 6. Figure 7 The seventh flowchart illustrating a robot state estimation method provided in this application embodiment; Figure 8 This is the eighth flowchart illustrating a robot state estimation method provided in this application embodiment; Figure 9 A schematic diagram of the functional modules of a robot state estimation device provided in an embodiment of this application; Figure 10 This is a schematic diagram of an electronic device provided in an embodiment of this application. Detailed Implementation

[0019] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are some embodiments of the present invention, but not all embodiments.

[0020] Therefore, the following detailed description of the embodiments of this application provided in the accompanying drawings is not intended to limit the scope of the claimed application, but merely to illustrate selected embodiments of the application. All other embodiments obtained by those skilled in the art based on the embodiments of this application without inventive effort are within the scope of protection of this application.

[0021] In the description of this application, it should be noted that if the terms "upper", "lower", etc. appear to indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings, or the orientation or positional relationship that the product of this application is usually placed in, it is only for the convenience of describing this application and simplifying the description, and does not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation, and therefore should not be construed as a limitation of this application.

[0022] Furthermore, the terms "first," "second," etc., used in the specification, claims, and accompanying drawings of this invention are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that such data can be interchanged where appropriate so that embodiments of the invention described herein can be implemented in orders other than those illustrated or described herein. Additionally, the terms "comprising" and "having," and any variations thereof, are intended to cover a non-exclusive inclusion; for example, a process, method, system, product, or apparatus that comprises a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such processes, methods, products, or apparatus.

[0023] It should be noted that, where there is no conflict, the features in the embodiments of this application can be combined with each other.

[0024] The robot state estimation method provided in this application will be explained in detail below with reference to the accompanying drawings and specific examples. The robot state estimation method provided in this application can be implemented by an electronic device pre-installed with a preset robot state estimation algorithm or detection software, by running the algorithm or software. The electronic device can be, for example, a server or a terminal, and the terminal can be a user computer. Figure 1 This is one of the flowcharts illustrating a robot state estimation method provided in an embodiment of this application; as shown below. Figure 1 As shown, the method includes: S101. Acquire multiple state data at the current moment collected by multiple sensors in the target robot.

[0025] In this embodiment, the target robot can be a lawnmower robot, and it is equipped with multiple sensors, such as a binocular camera, a wheel speedometer, and an inertial measurement unit (IMU). The binocular sensor can be a global shutter CMOS camera, and the wheel speedometer can be a high-precision photoelectric encoder. A 10ms period hardware synchronization pulse is generated by a control module in the target robot, such as a programmable logic device (FPGA), to trigger simultaneous sampling by the binocular camera, wheel speedometer, and IMU, obtaining multiple state data at the current moment.

[0026] The current state data captured by the binocular cameras includes two images, one to the left and one to the right, showing the target robot's direction of travel. The current state data captured by the wheel speedometer includes the number of pulses from the encoders of the left and right drive wheels as the target robot moves, and these pulses are converted into linear and angular velocities. Specifically, encoders are mounted on the target robot's wheels; each wheel rotation generates a certain number of pulses. By counting the pulses, the number of wheel rotations is determined. Based on the wheel circumference and the number of pulses per unit time, the forward or backward speed of the target robot can be calculated. By comparing the speed difference between the left and right wheels, the angular velocity of the target robot during a turn can be calculated.

[0027] The current state data collected by the IMU includes: the three-axis acceleration and three-axis angular velocity of the target robot during its movement. The three-axis acceleration is the acceleration of the target robot in the X (forward and backward), Y (left and right), and Z (up and down) directions, and the three-axis angular velocity is the angular velocity of the target robot around the X, Y, and Z axes.

[0028] S102. Based on the multiple state data at the current moment, determine the multiple observation values ​​at the current moment.

[0029] Specifically, for the current state data acquired by the binocular cameras, the OrientedFAST and RotatedBRIEF (ORB) algorithm is used to extract features from the left and right images. A matcher is used to perform feature matching to obtain valid matching points. Then, triangulation and motion estimation are performed: based on the pre-calibrated binocular camera parameters (baseline B, focal length f) and the matching feature points, the depth information of the feature points is calculated, and then the relative pose transformation of the camera is solved using the PnP algorithm. Then, based on the optimal pose estimate from the previous moment... and relative pose transformation Determine the first observation at the current moment. , is represented as: .

[0030] For the current state data collected by the wheel speed sensor, the relative pose transformation is obtained by inference based on the differential driving model and integration. Then, based on the optimal pose estimate from the previous moment... and relative pose transformation Determine the second observation at the current time. , is represented as: The differential drive model is a simple robot kinematics model. It assumes that the wheels move in an arc around an instantaneous center of rotation in a very short time. Based on the difference in the distance traveled by the left and right wheels, the angle (angular displacement) and the distance traveled (linear displacement) of the target robot during this time can be calculated. By integrating the motion of the differential drive model from the previous moment to the current moment, the relative pose transformation based on the wheel speedometer is obtained. .

[0031] For the current state data acquired by the IMU, IMU pre-integration is performed from the previous time step to the current time step to obtain the relative pose transformation. Then, based on the optimal pose estimate from the previous moment... and relative pose transformation Determine the third observation at the current moment. , is represented as: The pre-integration technique involves pre-integrating all IMU data from the previous moment to the current moment into a relative motion increment. This increment depends only on the IMU's measurements during this time period and is independent of the absolute pose from the previous moment. The motion increment obtained through pre-integration is the relative pose transformation calculated by the IMU. .

[0032] If the multiple sensors include a binocular camera, a wheel speedometer, and an IMU, then the multiple observations at the current moment include: the first observation. Second observation and the third observation .

[0033] S103. Obtain the historical state queue of the target robot.

[0034] The historical state queue includes the optimal pose estimates of the target robot at multiple historical moments prior to the current moment.

[0035] Specifically, the historical state queue is a fixed-length state queue of length L. Represented as: Where the current time is time t, then the historical state queue It contains the optimal pose estimates from time t-1 to time tL, with each state parameterized by a 6-dimensional vector, specifically represented as follows: , They represent three-dimensional positions respectively. These are represented as 3 Euler angles.

[0036] S104. Use a preset pose prediction model to predict the historical state queue and obtain the predicted pose state of the target robot at the current moment.

[0037] To eliminate the influence of dimensions and accelerate model convergence, the historical state queue is standardized, as follows:

[0038] in, and These are the three position and three attitude angle state variables obtained from statistics in each dimension. x, y, z, ， ， The mean and standard deviation vector of the sequence, and the standardized sequence. As input to the preset pose prediction model.

[0039] The standardized queue is predicted using a pre-defined pose prediction model. Make a prediction to obtain the target robot's pose state at the current moment. .

[0040] S105. Based on the predicted pose state of the target robot at the current moment and multiple observations at the current moment, determine the optimal pose estimate of the target robot at the current moment.

[0041] Finally, based on the predicted pose state of the target robot at the current moment... The first observation is the sum of multiple observations at the current time. Second observation and the third observation Determine the optimal pose estimate of the target robot at the current moment. .

[0042] In summary, this application provides a robot state estimation method. The method includes: acquiring multiple state data points collected by multiple sensors in a target robot at the current moment; acquiring a historical state queue of the target robot, wherein the historical state queue includes optimal pose estimates of the target robot at multiple historical moments prior to the current moment; using a preset pose prediction model to predict the historical state queue to obtain a predicted pose state value of the target robot at the current moment; and determining the optimal pose estimate of the target robot at the current moment based on the predicted pose state value of the target robot at the current moment and multiple observations at the current moment. This method, by combining multiple state data points collected by multiple sensors at the current moment with the temporal information of the historical state queue and the look-ahead capability of the preset prediction model, and further optimizing through the fusion of multiple observations, achieves continuity and accuracy in pose estimation, improves the reliability of the target robot's positioning and navigation in complex outdoor scenarios, and provides stable core support for full-coverage operations and precise obstacle avoidance.

[0043] This application also provides another possible implementation of the robot state estimation method. Figure 2 This is a second flowchart illustrating a robot state estimation method provided in an embodiment of this application, as shown below. Figure 2 As shown, a preset pose prediction model is used to predict the historical state queue to obtain the predicted pose state of the target robot at the current moment, including: S201. A preset pose prediction model is used to predict the historical state queue to obtain the relative pose increment of the target robot at the current moment.

[0044] In this embodiment, the preset pose prediction model is a hybrid pose prediction model, including a convolutional neural network (CNN) and a long short-term memory (LSTM) network.

[0045] The historical state queue is standardized, and the standardized queue is then processed. Input into the preset pose prediction model To make a prediction, it is represented as:

[0046] in, This represents the relative pose increment predicted by the model from time t-1 to time t. Represented as network parameters, these are parameters obtained through offline training and optimization of a preset pose prediction model.

[0047] S202. Based on the relative pose increment of the target robot at the current moment and the optimal pose estimate of the target robot at the previous moment, determine the predicted pose state of the target robot at the current moment.

[0048] The relative pose increment obtained through network prediction The value is transformed to a Lie group through an exponential mapping and compared with the optimal pose estimate from the previous time step. By combining the results, we obtain the prior state prediction, i.e., the pose state prediction value at the current moment. , is represented as:

[0049] Where exp is the exponential mapping. The output is the predicted pose state value. This serves as a benchmark for evaluating the reliability of subsequent filter predictions and sensor observations.

[0050] The method provided in this application derives the predicted pose state value by combining relative pose increment prediction and the optimal estimate of the previous moment, thus overcoming the limitations of traditional uniform or simple kinematic models. By leveraging the in-depth mining of historical state queues through a preset pose prediction model, complex motion characteristics such as slippage and vibration of the target robot can be accurately captured, and the output relative pose increment more closely matches the actual motion law. Furthermore, by combining the optimal pose optimized from the previous moment, the predicted value at the current moment possesses both temporal continuity and numerical accuracy, providing a high-quality prior benchmark for subsequent fault diagnosis and fusion calculation, and effectively reducing the interference of abnormal observations on the final estimation result.

[0051] This application also provides another possible implementation of the robot state estimation method. Figure 3 This is the third flowchart illustrating a robot state estimation method provided in this application embodiment, as shown below. Figure 3 As shown, based on the predicted pose state of the target robot at the current moment and multiple observations at the current moment, the optimal pose estimate of the target robot at the current moment is determined, including: S301. Based on the current pose state prediction value and multiple observation values ​​at the current time, determine the chi-square test values ​​of multiple sensors.

[0052] In this embodiment, the chi-square test value of each sensor is determined based on the current pose state prediction value and multiple observation values ​​at the current time, which is used to determine whether each sensor is faulty.

[0053] Figure 4 This is the fourth flowchart illustrating a robot state estimation method provided in this application embodiment, as shown below. Figure 4 As shown, step S301 specifically includes: S401. Based on the predicted pose state of the target robot at the current moment and multiple observations at the current moment, determine the standardized residuals of multiple sensors.

[0054] Specifically, the standardized residual is expressed as:

[0055] in, Represented as multiple observations at the current moment, Represented as a residual matrix, it represents the error between the predicted pose state of the target robot at the current time and multiple observations at the current time. Represented as a covariance matrix, Represented as a coefficient matrix Represented as the error covariance matrix, Represented as the noise matrix corresponding to each of the multiple sensors. Represented as standardized residuals, Each element should follow a standard normal distribution N(0,1), and the entire vector should follow a multivariate standard normal distribution.

[0056] S402. Determine the global chi-square check value for multiple sensors based on the standardized residuals.

[0057] S403. Determine whether multiple sensors are faulty based on the global chi-square check value of multiple sensors.

[0058] Specifically, the global chi-square test calculation formula is expressed as follows:

[0059] Global chi-square check value It follows a chi-square distribution with dim(rt) degrees of freedom. If the global chi-square check value exceeds the statistical threshold, it is determined that there is a fault among multiple sensors, and the isolation procedure is triggered.

[0060] S404. If it is determined that multiple sensors are faulty, then the chi-square test values ​​of the multiple sensors are determined based on the predicted pose state of the target robot at the current moment and the multiple observation values ​​at the current moment.

[0061] The formula for calculating the chi-square test value of multiple sensors is expressed as follows:

[0062] and Residual matrix Covariance Matrix The block corresponds to the i-th sensor. When a fault is detected by the global inspection, the source of the fault needs to be located. Therefore, a chi-square test is performed on each sensor individually to obtain the chi-square test value for each sensor.

[0063] If the chi-square test value of only one sensor exceeds the preset threshold, that sensor is isolated as the source of the fault. If the chi-square test values ​​of multiple sensors simultaneously exceed the preset threshold, it may indicate that multiple faults have occurred, or that the fault originates from the model itself.

[0064] S302. Determine the reputation weights of multiple sensors based on the chi-square test values ​​of multiple sensors.

[0065] S303. Determine the observation noise covariance based on the reputation weights of multiple sensors.

[0066] Specifically, the reputation weights of multiple sensors are determined based on the chi-square test values ​​of multiple sensors. The observation noise covariance is updated based on the reputation weights of multiple sensors, specifically as follows:

[0067] The observation noise covariance is adaptively adjusted based on reputation weights. It is a very small positive number to prevent division by zero errors. When the weight is low, i.e., the corresponding sensor observation is unreliable, the observation noise covariance R is amplified, thereby reducing the weight of that observation during fusion.

[0068] S304. Based on the observation noise covariance, the predicted pose state of the target robot at the current moment, and multiple observations at the current moment, determine the optimal pose estimate of the target robot at the current moment.

[0069] Finally, the predicted pose state of the target robot at the current moment is fused with multiple observations at the current moment based on the observation noise covariance to obtain the optimal pose estimate of the target robot at the current moment.

[0070] The method provided in this application establishes a dynamic correlation between sensor reliability and noise weights by logically deriving the observation noise covariance through chi-square test value calculation, reputation weight allocation, and noise covariance adjustment. The chi-square test value can accurately identify sensor observation anomalies based on mathematical statistics, while the reputation weights quantify the real-time reliability of each sensor. The adjusted observation noise covariance has a clearer physical meaning and stronger adaptability. This process achieves adaptive dynamic optimization of observation noise, avoiding the inadequacy of fixed noise models in complex environments and providing realistic noise parameters for subsequent fusion calculations, further improving the accuracy and anti-interference capability of pose state estimation.

[0071] This application also provides another possible implementation of the robot state estimation method. Figure 5 This is the fifth flowchart illustrating a robot state estimation method provided in this application embodiment, as shown below. Figure 5 As shown, the reputation weights of multiple sensors are determined based on the chi-square test values ​​of multiple sensors, including: S501. Based on the predicted pose state of the target robot at the current moment and multiple observation values ​​at the current moment, determine the Mahalanobis distances corresponding to multiple sensors.

[0072] In this embodiment, the Mahalanobis distance calculation formula is expressed as:

[0073] in, For each observation With pose state prediction value Mahalanobis distance between them Let be the observation noise covariance matrix of the i-th sensor under normal operating conditions. The larger the Mahalanobis distance, the greater the probability that the observation is an outlier.

[0074] S502. Based on the chi-square test values ​​of multiple sensors, determine the reputation weights of multiple sensors according to the Mahalanobis distances corresponding to the multiple sensors.

[0075] Specifically, the formula for calculating credibility weight is as follows:

[0076] in, ∈[0,1] represents the dynamic reputation weight of the i-th sensor. To adjust parameters and control the rate of weight decay, the first formula in the reputation weight calculation formula represents the reputation weight of a sensor determined to be a fault source when the chi-square test values ​​of multiple sensors are used. The second formula represents the reputation weight of each sensor when the overall test shows no fault. The third equation represents the determination of the reputation weight of each sensor when it is determined that there is a fault in the global inspection, but the faulty sensor cannot be identified. The smaller the Mahalanobis distance, the higher the weight of the chi-square test, indicating that the sensor is more reliable.

[0077] The method provided in this application combines the chi-square test value and Mahalanobis distance to determine the sensor reputation weights, achieving a multi-dimensional and accurate assessment of sensor reliability. Mahalanobis distance effectively measures the deviation between observed and predicted values. Combined with the fault determination result of the chi-square test value, the weight allocation considers both the consistency of observed data and the sensor's fault state. Compared to single-dimensional weight allocation methods, the reputation weights obtained in this step are more comprehensive and objective, accurately reflecting the sensor's real-time operating status. This provides a scientific basis for the adaptive adjustment of observation noise covariance, ensuring that reliable sensors receive higher weights in fusion and improving the stability of pose state estimation.

[0078] This application also provides another possible implementation of the robot state estimation method. Figure 6 This is the sixth flowchart illustrating a robot state estimation method provided in this application embodiment, as shown below. Figure 6 As shown, based on the observation noise covariance, the predicted pose state of the target robot at the current moment, and multiple observations at the current moment, the optimal pose estimate of the target robot at the current moment is determined, including: S601. Determine the error covariance at the current moment based on the target robot's error covariance, state transition matrix, and process noise at the previous moment.

[0079] The formula for calculating the error covariance at the current moment is as follows:

[0080] Represented as a state transition matrix, Represented as process noise, it characterizes the uncertainty of the noise. It is expressed as the error covariance of the previous time step.

[0081] S602. Determine the gain matrix based on the error covariance at the current time, the preset coefficient matrix, and the observation noise covariance.

[0082] In this embodiment, the robust fusion based on error state Kalman filtering (ESKF) is expressed by the following formula for calculating the gain matrix:

[0083] in, This is expressed as the error covariance at the current time. Represented as a preset coefficient matrix, If we express the observation noise covariance, then we determine the gain matrix. .

[0084] S603. Determine the error state based on the gain matrix, the predicted pose state of the target robot at the current moment, and multiple observations at the current moment.

[0085] Specifically, the formula for calculating the error state is as follows:

[0086] It should be noted that, based on the error covariance at the current moment... Preset coefficient matrix and gain matrix Update the error covariance matrix at the current time step. This is used to calculate the error covariance at the next time step, and the specific formula is as follows:

[0087] in, It is represented as an identity matrix.

[0088] S604. Based on the predicted pose state and error state of the target robot at the current moment, determine the optimal pose estimate of the target robot at the current moment.

[0089] The formula for calculating the optimal pose estimate at the current moment is expressed as:

[0090] Since the observation noise covariance is redefined based on the reputation weights of multiple sensors, the gain matrix and error state are also updated accordingly. Therefore, based on the predicted pose state and error state of the target robot at the current moment, the optimal pose estimate of the target robot at the current moment can be obtained.

[0091] The method provided in this application refines the implementation process of optimal pose estimation through gain matrix calculation, error state update, and pose optimization. The gain matrix calculation comprehensively considers the current error covariance, the observation matrix, and the adjusted observation noise covariance, dynamically balancing the confidence levels of the predicted and observed values. The error state update accurately corrects the deviation of the predicted values, and finally, pose optimization is achieved through exponential mapping on a Lie group, ensuring the mathematical rigor of the pose update. This step fully utilizes the prior information of the predicted values ​​and the real-time information of the observed values, while rigorous mathematical operations ensure the accuracy of the estimation results, making the final output optimal pose both accurate and smooth, meeting the real-time and reliability requirements of the target robot's navigation control.

[0092] This application also provides another possible implementation of the robot state estimation method. Figure 7 This is the seventh flowchart illustrating a robot state estimation method provided in this application embodiment, as shown below. Figure 7 As shown, before using a preset pose prediction model to predict the historical state queue and obtaining the relative pose increment of the target robot at the current moment, the method also includes: S701. Obtain sample pairs.

[0093] The sample pairs include: the target robot's actual pose increment at the first moment, and pose estimates at multiple second moments prior to the first moment.

[0094] S702. Based on the pose estimation values ​​of multiple second time moments, the initial pose prediction model is used to predict the pose increment of the first time moment, so as to obtain the predicted pose increment of the first time moment.

[0095] The initial pose prediction model includes convolutional layers, temporal prediction layers, and an output layer. The convolutional layers are convolutional neural networks (CNN), the temporal prediction layers are long short-term memory networks (LSTM), and the output layer is a fully connected regression layer.

[0096] A one-dimensional convolutional layer is used to extract features from the pose estimates at multiple second time steps to obtain local feature sequences corresponding to the pose estimates at multiple second time steps. Then, a temporal prediction layer is used to encode the local feature sequences corresponding to the pose estimates at multiple second time steps to obtain the hidden state information corresponding to the local feature sequences. Finally, the output layer is used to map the local feature sequences to obtain the predicted pose increment at the first time step.

[0097] S703. Based on the predicted pose increment and the actual pose increment at the first moment, the parameters of the initial pose prediction model are adjusted to obtain the preset pose prediction model.

[0098] We employ a weighted mean squared error (MSE) loss function, simultaneously optimizing both translation and rotation increments. The loss function is expressed as:

[0099] in, (For example ), and The predicted and true quaternions corresponding to the rotation increment can be obtained through Euler angle transformation. Represented as the predicted pose increment at the first moment. This represents the true pose increment at time-1. The loss function is minimized using the Adam optimizer on a large dataset. The parameter set is iteratively updated using the backpropagation algorithm. All weights and biases are adjusted until the model converges. After training, a set of optimal, fixed parameters is obtained. Save this set of parameters along with the model structure to obtain the preset pose prediction model.

[0100] The method provided in this application trains a preset pose prediction model through a process of sample pair acquisition, initial model prediction, and parameter adjustment, providing high-performance model support for pose increment prediction. The constructed sample pairs ensure the accuracy and scene coverage of the training data; the weighted mean square error loss function is used to simultaneously optimize translation and rotation increments, enabling the model to learn the key features of pose changes in a balanced manner; iterative training and early stopping strategies ensure the model's convergence and generalization ability. The trained model can accurately learn the motion patterns of the target robot in different scenarios, and the accuracy of the output predicted pose increments far exceeds that of traditional models, laying a solid foundation for subsequent pose prediction and fusion calculation.

[0101] This application also provides another possible implementation of the robot state estimation method, wherein the preset pose prediction model includes: a convolutional layer, a temporal prediction layer, and an output layer. Figure 8 This is the eighth flowchart illustrating a robot state estimation method provided in this application embodiment, as shown below. Figure 8 As shown, a preset pose prediction model is used to predict the historical state queue to obtain the relative pose increment of the target robot at the current moment, including: S801. Use convolutional layers to extract features from the historical state queue to obtain local feature sequences.

[0102] In this embodiment, a convolutional layer is used to extract features from the historical state queue, specifically as follows:

[0103] in, This represents a one-dimensional convolution operation, where 1 is the layer index. is the kernel weight, and b is the bias. Multiple one-dimensional convolutional kernels are slid along the length of the historical state queue to extract local motion patterns (such as constant speed, acceleration, turning, etc.). ReLU is the activation function, and C(1) is the output local feature sequence.

[0104] S802. A temporal prediction layer is used to predict the local feature sequence to obtain the hidden state information at the current time.

[0105] The local feature sequence C(1) extracted by the convolutional layer is used as input, and long-term dependencies in the sequence are learned through its internal gating mechanism (forget gate, input gate, output gate). The LSTM layer outputs the hidden state information at the current time step. It encodes condensed information of the entire historical state queue and serves as input for subsequent fully connected regression layers.

[0106] S803. The output layer is used to map the hidden state information at the current moment to obtain the relative pose increment of the target robot at the current moment.

[0107] Specifically, the output layer is a fully connected regression layer, represented as:

[0108] in, and Both are weight matrices. and Both are bias vectors, and ReLU is an activation function. This layer outputs the hidden state information of the LSTM at the current time step. Mapped to 6-dimensional relative pose increments from time t-1 to time t .

[0109] The method provided in this application achieves relative pose increment prediction through the collaborative action of convolutional layers, temporal prediction layers, and output layers, fully leveraging the feature extraction and temporal modeling capabilities of deep learning models. Convolutional layers effectively capture local motion patterns in historical state queues, while the temporal prediction layer learns long-term dependencies in the sequence through a gating mechanism. The output layer maps abstract hidden states to specific 6-dimensional pose increments. This structure balances the efficiency of local motion feature extraction with the effective transmission of temporal information, enabling the model to accurately uncover key patterns in historical states. The output relative pose increments exhibit extremely high accuracy and timeliness, meeting the real-time requirements of embedded platforms and providing high-quality core input for pose state prediction.

[0110] The robot state estimation device and electronic device provided in any of the above embodiments of this application will be explained below. The specific implementation process and the resulting technical effects are the same as those in the corresponding method embodiments. For the sake of brevity, the parts not mentioned in this embodiment can be referred to the corresponding content in the method embodiment.

[0111] Figure 9 This is a schematic diagram of the functional modules of a robot state estimation device provided in an embodiment of this application. Figure 9 As shown, the robot state estimation device 100 includes: The acquisition module 110 is used to acquire multiple state data at the current moment collected by multiple sensors in the target robot; The determination module 120 is used to determine multiple observations at the current time based on multiple state data at the current time. The acquisition module 110 is also used to acquire the historical state queue of the target robot, which includes the optimal pose estimate of the target robot at multiple historical moments before the current moment. The prediction module 130 is used to predict the historical state queue using a preset pose prediction model to obtain the predicted pose state of the target robot at the current moment. The determination module 120 is also used to determine the optimal pose estimate of the target robot at the current moment based on the predicted pose state of the target robot at the current moment and multiple observations at the current moment.

[0112] Optionally, the prediction module 130 is further configured to use a preset pose prediction model to predict the historical state queue, thereby obtaining the relative pose increment of the target robot at the current moment; and to determine the predicted pose state of the target robot at the current moment based on the relative pose increment of the target robot at the current moment and the optimal pose estimate of the target robot at the previous moment.

[0113] Optionally, the determining module 120 is further configured to determine the chi-square test values ​​of multiple sensors based on the predicted pose state value at the current time and multiple observation values ​​at the current time; determine the reputation weights of multiple sensors based on the chi-square test values ​​of multiple sensors; determine the observation noise covariance based on the reputation weights of multiple sensors; and determine the optimal pose estimate of the target robot at the current time based on the observation noise covariance, the predicted pose state value of the target robot at the current time, and multiple observation values ​​at the current time.

[0114] Optionally, the determining module 120 is further configured to determine the standardized residuals of multiple sensors based on the predicted pose state of the target robot at the current moment and multiple observations at the current moment; determine the global chi-square check value of multiple sensors based on the standardized residuals; determine whether multiple sensors are faulty based on the global chi-square check value of multiple sensors; if it is determined that multiple sensors are faulty, then determine the chi-square check value of multiple sensors based on the predicted pose state of the target robot at the current moment and multiple observations at the current moment.

[0115] Optionally, the determining module 120 is further configured to determine the Mahalanobis distances corresponding to multiple sensors based on the predicted pose state of the target robot at the current moment and multiple observations at the current moment; and to determine the reputation weights of multiple sensors based on the chi-square test values ​​of multiple sensors and the Mahalanobis distances corresponding to multiple sensors.

[0116] Optionally, the determining module 120 is further configured to: determine the error covariance at the current moment based on the error covariance, state transition matrix, and process noise of the target robot at the previous moment; determine the gain matrix based on the error covariance at the current moment, the preset coefficient matrix, and the observation noise covariance; determine the error state based on the gain matrix, the predicted pose state of the target robot at the current moment, and multiple observations at the current moment; and determine the optimal pose estimate of the target robot at the current moment based on the predicted pose state of the target robot at the current moment and the error state.

[0117] Optionally, the device further includes: The acquisition module 110 is also used to acquire sample pairs, which include: the actual pose increment of the target robot at the first moment, and the pose estimates of multiple second moments before the first moment; The prediction module 130 is used to predict the pose increment of the first time step based on the pose estimation values ​​of multiple second time steps using an initial pose prediction model, so as to obtain the predicted pose increment of the first time step. The parameter tuning module is used to tune the initial pose prediction model based on the predicted pose increment and the actual pose increment at the first moment, so as to obtain the preset pose prediction model.

[0118] Optionally, the preset pose prediction model includes a convolutional layer, a temporal prediction layer, and an output layer. The prediction module 130 is used to extract features from the historical state queue using the convolutional layer to obtain a local feature sequence; to perform prediction processing on the local feature sequence using the temporal prediction layer to obtain the hidden state information at the current time; and to map the hidden state information at the current time using the output layer to obtain the relative pose increment of the target robot at the current time.

[0119] The above-described device is used to execute the method provided in the foregoing embodiments, and its implementation principle and technical effect are similar, so they will not be described again here.

[0120] These modules can be one or more integrated circuits configured to implement the above methods, such as one or more Application Specific Integrated Circuits (ASICs), one or more microprocessors, or one or more Field Programmable Gate Arrays (FPGAs). Alternatively, when a module is implemented using processing element scheduler code, the processing element can be a general-purpose processor, such as a Central Processing Unit (CPU) or other processor capable of calling program code. Furthermore, these modules can be integrated together as a system-on-a-chip (SOC).

[0121] Figure 10 This is a schematic diagram of an electronic device provided in an embodiment of this application. This electronic device can be used for robot state estimation. Figure 10 As shown, the electronic device includes: a processor 210, a storage medium 220, and a bus 230.

[0122] Storage medium 220 stores machine-readable instructions executable by processor 210. When the electronic device is running, processor 210 communicates with storage medium 220 via bus 230, and processor 210 executes the machine-readable instructions to perform the steps of the above method embodiment. The specific implementation and technical effects are similar and will not be described again here.

[0123] Optionally, this application also provides a storage medium 220, on which a computer program is stored. When the computer program is run by a processor, it executes the steps of the above-described method embodiments. The specific implementation and technical effects are similar, and will not be repeated here.

[0124] In the several embodiments provided by this invention, it should be understood that the disclosed apparatus and methods can be implemented in other ways. For example, the apparatus embodiments described above are merely illustrative; for instance, the division of 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 apparatuses or units may be electrical, mechanical, or other forms.

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

[0126] Furthermore, the functional units in the various embodiments of the present invention 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 in the form of hardware plus software functional units.

[0127] The integrated units implemented as software functional units described above can be stored in a computer-readable storage medium. These software functional units, stored in a storage medium, include several instructions to cause an electronic device (which may be a personal computer, server, or network device, etc.) or processor to execute some steps of the methods described in the various embodiments of the present invention. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0128] The above are merely specific embodiments of the present invention, but the scope of protection of the present invention is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the technical scope disclosed in the present invention should be included within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims.

Claims

1. A robot state estimation method, characterized in that, include: Acquire multiple state data at the current moment from multiple sensors in the target robot; Based on the multiple state data at the current moment, determine multiple observation values ​​at the current moment; Obtain the historical state queue of the target robot, the historical state queue including: the optimal pose estimation values ​​of the target robot at multiple historical moments before the current moment; A preset pose prediction model is used to predict the historical state queue to obtain the predicted pose state of the target robot at the current moment. Based on the predicted pose state of the target robot at the current moment and multiple observations at the current moment, the optimal pose estimate of the target robot at the current moment is determined.

2. The method according to claim 1, characterized in that, The step of using a preset pose prediction model to predict the historical state queue to obtain the predicted pose state value of the target robot at the current moment includes: The preset pose prediction model is used to predict the historical state queue to obtain the relative pose increment of the target robot at the current moment; Based on the relative pose increment of the target robot at the current moment and the optimal pose estimate of the target robot at the previous moment, the predicted pose state of the target robot at the current moment is determined.

3. The method according to claim 1, characterized in that, The step of determining the optimal pose estimate of the target robot at the current moment based on the predicted pose state of the target robot at the current moment and multiple observations at the current moment includes: Based on the current pose state prediction value and multiple observation values ​​at the current time, determine the chi-square test value of the multiple sensors; The reputation weights of the multiple sensors are determined based on the chi-square test values ​​of the multiple sensors. The observation noise covariance is determined based on the reputation weights of the multiple sensors. Based on the observation noise covariance, the predicted pose state of the target robot at the current moment, and multiple observations at the current moment, the optimal pose estimate of the target robot at the current moment is determined.

4. The method according to claim 3, characterized in that, The step of determining the chi-square test values ​​of the multiple sensors based on the predicted pose state of the target robot at the current moment and multiple observation values ​​at the current moment includes: Based on the predicted pose state of the target robot at the current moment and multiple observations at the current moment, the standardized residuals of the multiple sensors are determined; Based on the standardized residuals, the global chi-square check value of the multiple sensors is determined; Based on the global chi-square check value of the multiple sensors, determine whether the multiple sensors are faulty; If it is determined that the multiple sensors are faulty, then the chi-square test value of the multiple sensors is determined based on the predicted pose state of the target robot at the current moment and the multiple observation values ​​at the current moment.

5. The method according to claim 3, characterized in that, The determination of the reputation weights of the multiple sensors based on the chi-square test values ​​of the multiple sensors includes: Based on the predicted pose state of the target robot at the current moment and multiple observations at the current moment, determine the Mahalanobis distances corresponding to multiple sensors; Based on the chi-square test values ​​of the multiple sensors, and according to the Mahalanobis distances corresponding to the multiple sensors, the reputation weights of the multiple sensors are determined.

6. The method according to claim 3, characterized in that, The step of determining the optimal pose estimate of the target robot at the current time based on the observation noise covariance, the predicted pose state of the target robot at the current time, and multiple observations at the current time includes: The error covariance at the current moment is determined based on the target robot's error covariance, state transition matrix, and process noise at the previous moment. The gain matrix is ​​determined based on the error covariance at the current moment, the preset coefficient matrix, and the observation noise covariance. The error state is determined based on the gain matrix, the predicted pose state of the target robot at the current moment, and multiple observations at the current moment. Based on the predicted pose state of the target robot at the current moment and the error state, the optimal pose estimate of the target robot at the current moment is determined.

7. The method according to claim 2, characterized in that, Before using the preset pose prediction model to predict the historical state queue and obtain the relative pose increment of the target robot at the current moment, the method further includes: Obtain sample pairs, the sample pairs including: the actual pose increment of the target robot at a first moment, and pose estimates at multiple second moments prior to the first moment; Based on the pose estimation values ​​at the multiple second time points, the initial pose prediction model is used to predict the pose increment at the first time point, thereby obtaining the predicted pose increment at the first time point. Based on the predicted pose increment and the actual pose increment at the first moment, the parameters of the initial pose prediction model are adjusted to obtain the preset pose prediction model.

8. The method according to claim 2, characterized in that, The preset pose prediction model includes a convolutional layer, a temporal prediction layer, and an output layer. The step of using the preset pose prediction model to predict the historical state queue to obtain the relative pose increment of the target robot at the current moment includes: The convolutional layer is used to extract features from the historical state queue to obtain a local feature sequence; The local feature sequence is predicted using the temporal prediction layer to obtain the hidden state information at the current time. The output layer is used to map the hidden state information at the current moment to obtain the relative pose increment of the target robot at the current moment.

9. A robot state estimation device, characterized in that, The device includes: The acquisition module is used to acquire multiple state data at the current moment collected by multiple sensors in the target robot; The determination module is used to determine multiple observation values ​​at the current time based on multiple state data at the current time. The acquisition module is further configured to acquire the historical state queue of the target robot, the historical state queue including: the optimal pose estimation values ​​of the target robot at multiple historical moments before the current moment; The prediction module is used to predict the historical state queue using a preset pose prediction model to obtain the predicted pose state value of the target robot at the current moment. The determining module is further configured to determine the optimal pose estimate of the target robot at the current time based on the predicted pose state of the target robot at the current time and multiple observation values ​​at the current time.

10. An electronic device, characterized in that, include: The device includes a processor, a storage medium, and a bus. The storage medium stores program instructions executable by the processor. When the electronic device is running, the processor communicates with the storage medium via the bus, and the processor executes the program instructions to perform the steps of the robot state estimation method as described in any one of claims 1-8.

Images