Method and chip for detecting whether robot is impacted, and robot

Abstract

A method for detecting whether a robot is impacted. The method comprises: obtaining an observed speed value and an observed speed direction, and obtaining a predicted speed value at the current moment and a predicted speed direction at the current moment; calculating the absolute value of the difference between the observed speed value and the predicted speed value to obtain a Euclidean distance corresponding to the current speed change; and calculating the cosine value of the included angle between the observed speed direction and the predicted speed direction to obtain a cosine distance corresponding to the current speed change; when the Euclidean distance corresponding to the current speed change is greater than a preset Euclidean distance threshold and the cosine distance corresponding to the current speed change is smaller than a preset cosine distance threshold, using a target impact sensing network model to perform classification processing to obtain a first prediction probability that a robot is impacted at the current moment and a second prediction probability that the robot is impacted at the current moment; and when the first prediction probability that the robot is impacted at the current moment is greater than the second prediction probability that the robot is impacted at the current moment, determining that the robot is impacted. Also provided are a chip for detecting whether a robot is impacted, and a robot.

Description

A robot impact detection method, chip and robot Technical Field

[0001] The present application relates to the field of robot control algorithms, and more particularly to a robot impact detection method, chip, and robot. Background Art

[0002] In the field of simultaneous localization and mapping (SLAM) for low-cost wheeled robots, the Correlative Scan Matching (CSM) algorithm is a lidar-based scan matching algorithm. Specifically, it combines scan matching with a search window to obtain the pose with the highest probability of being occupied by the laser point cloud in the grid map as the final pose. However, the accuracy of this algorithm is heavily dependent on the confidence level of the initial search pose.

[0003] Existing technologies can also choose to use the initial search pose as prior information, and use the extended Kalman filter (EKF) framework to fuse the sensing data of the inertial measurement unit (IMU) and the sensing data of the rotary encoder to process the final optimized pose of the robot.

[0004] However, in complex dynamic environments, when a robot moves on low-friction surfaces, it is very likely to skid if the robot's tires have low adhesion. Skidding generally refers to the lateral displacement (i.e., axial displacement) of the robot's wheels. This is considered an impact, and the robot is instantly moved to a different position without sensor perception. This means that the robot is instantly moved to a different position without prior information. Under the extended Kalman filter (EKF) framework, the sensor data from the rotary encoders on the robot's wheels will be processed into erroneous pose information (calculating an incorrect state estimate), affecting the robot's ability to detect the impact.

[0005] On the other hand, the robot's unimpeded state can be roughly divided into three scenarios: (1) the robot moves on a smooth surface; (2) the robot actively hits an obstacle; and (3) the robot crosses an obstacle and experiences body vibration. In scenarios (2) and (3), the acceleration measurement values ​​sensed by the inertial measurement unit will change significantly.

[0006] In scenario (2), if the relevant method disclosed in Chinese patent CN113566828A is used to detect impact, the robot is first excluded from the situation where it is not impacted when the Mahalanobis distance between the acceleration sample scanned by the window and the overall sample constructed by the acceleration measurement value is less than or equal to the preset Mahalanobis distance threshold, and then the robot's roll angle and pitch angle are used to calculate the Euclidean distance between the corresponding types of angles to determine the situation where the robot is impacted; however, since the acceleration measurement value sensed by the inertial measurement unit will produce a large change, the robot is prone to misjudgment when detecting the impact situation, that is, scenario (2) is actually in a state where the robot is not impacted, but is judged to be in a state where it is impacted.

[0007] Summary of the Invention

[0008] This application discloses a robot impact detection method, chip, and robot. The specific technical solutions are as follows:

[0009] A method for detecting impact on a robot includes: step A, obtaining an observed speed value and an observed speed direction, and then obtaining a predicted speed value and a predicted speed direction at the current moment based on a Kalman filter algorithm; step B, calculating the absolute value of the difference between the observed speed value and the predicted speed value at the current moment to obtain a Euclidean distance corresponding to the current speed change; calculating the cosine value of the angle between the observed speed direction and the predicted speed direction at the current moment to obtain a cosine distance corresponding to the current speed change; step C, if the Euclidean distance corresponding to the current speed change is greater than a preset Euclidean distance threshold and the cosine distance corresponding to the current speed change is less than the preset cosine distance threshold, classifying a sequence of to-be-sensed measurement values ​​using a target impact perception network model to obtain a first predicted probability that the robot is experiencing an impact at the current moment and a second predicted probability that the robot is experiencing an impact at the current moment; wherein the target impact perception network model is a trained impact perception neural network; wherein the sequence of to-be-sensed measurement values ​​is data collected by a sensing device of the robot; the sensing device of the robot includes an inertial measurement unit and a rotary encoder; and step D, if the first predicted probability that the robot is experiencing an impact at the current moment is greater than the second predicted probability that the robot is experiencing an impact at the current moment, determining that the robot is experiencing an impact.

[0010] In summary, the robot impact detection method designed in steps A to D disclosed in the present application uses the Euclidean distance formed between the observed speed value and the predicted speed value at the current moment and the cosine distance formed between the observed speed direction and the predicted speed direction at the current moment as trigger detection starting points, triggering the target impact perception network model to classify and process the data collected by the inertial measurement unit and the rotary encoder. It not only takes into account the necessity of the target impact perception network model classification processing, but also can integrate the prediction information of at least two types of sensor collection data under the Kalman filter framework, thereby improving the reliability and accuracy of the target impact perception network model classification; and then only uses the size relationship between the two prediction probabilities to make a judgment, thereby overcoming the misjudgment problem caused by the sudden change in the measurement value of the inertial measurement unit and improving the accuracy of impact detection.

[0011] A chip is used to store a program; the program is used to control a robot to execute the robot impact detection method.

[0012] A robot is provided, wherein the chip is built in the robot; wherein the left wheel of the robot is equipped with a left-rotating encoder, and the right wheel of the robot is equipped with a right-rotating encoder; an inertial measurement unit is installed in the body of the robot, and the inertial measurement unit includes an accelerometer and a gyroscope. BRIEF DESCRIPTION OF THE DRAWINGS

[0013] FIG1 is a flow chart of a method for detecting impact on a robot disclosed in an embodiment of the present application.

[0014] FIG2 is a flowchart of a method for classifying a sequence of perceived measurement values ​​using a target impact perception network model disclosed in another embodiment of the present application.

[0015] FIG3 is a flowchart of a method for constructing a target impact perception network model disclosed in another embodiment of the present application.

[0016] FIG4 is a flowchart of a method for correcting current posture prediction data and its prior covariance matrix disclosed in another embodiment of the present application. DETAILED DESCRIPTION

[0017] In order to make the purpose, technical solutions and advantages of the present invention clearer, the present invention will be described and illustrated below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described below are only used to explain the present invention and are not intended to limit the present invention. In addition, it is also understood that for those skilled in the art, some changes in design, manufacturing or production based on the technical content disclosed in the present invention are only conventional technical means and should not be understood as insufficient disclosure of the present application.

[0018] Unless otherwise defined, the technical terms or scientific terms involved in this application should be understood by people with ordinary skills in the technical field to which this application belongs. The words "one", "a", "a", "the" and the like involved in this application do not represent quantitative restrictions and can represent the singular or plural. The terms "include", "comprise", "have" and any of their variations involved in this application are intended to cover non-exclusive inclusions, such as: a process, method, system product or device that includes a series of steps or modules is not limited to the listed steps or units, but may also include steps or modules that are not listed, or may also include other steps or units inherent to these processes, methods, products or devices. The terms "first", "second", "third" and the like involved in this application are merely used to distinguish similar correspondences and do not represent a specific ordering of objects.

[0019] During the simultaneous localization and mapping (SLAM) process, wheeled robots use the Correlative Scan Matching (CSM) algorithm to construct a laser point cloud map. The accuracy of this algorithm relies on the confidence of the initial search pose to predict the initial search pose based on prior information. The algorithm then uses a standard extended Kalman filter (EKF) framework to fuse the inertial measurement unit (IMU) and rotary encoder to infer the pose of each position node occupied by the robot in its motion trajectory. However, wheeled robots are extremely susceptible to impact and skidding, and due to the limited perception of the rotary encoder, the axial displacement of the wheel cannot be observed. The EKF algorithm based on the rotary encoder observation information will obtain an erroneous state estimation result, that is, it cannot reach or approach the filtering accuracy of the sequential measurement processing. In addition, in scenarios where the robot actively collides with or crosses an obstacle and experiences body vibration, the acceleration measurement values ​​collected by the inertial measurement unit will vary significantly within a relatively short time series length. If impact detection is performed according to the method of steps 31 to 33 disclosed in Chinese patent CN113566828A, the Mahalanobis distance between two batches of acceleration measurement value samples will be used for threshold judgment, and it will be determined in advance that the robot is not impacted, resulting in a misjudgment.

[0020] To address the above issues, this application discloses a method for detecting impacts on a robot. The method is implemented by a robot equipped with an inertial measurement unit (IMU) and rotary encoders on both wheels. Essentially, the robot is a wheeled robot equipped with two odometers, which respectively sense the position of the robot and the rotation of the wheel axles on both sides of the robot. As shown in Figure 1, the method includes:

[0021] Step A: Obtain the observed speed value and observed speed direction, then obtain the predicted speed value and predicted speed direction at the current moment based on the Kalman filter algorithm, and then execute step B. The predicted speed value and predicted speed direction at the current moment can constitute a predicted speed vector at the current moment; the observed speed value and observed speed direction can constitute an observed speed vector, which is output by a preset correlation scan matching algorithm. However, due to the operating delay of the correlation scan matching algorithm, the generation timestamp of the observed speed vector may not necessarily be the current moment. For example, the generation timestamp of the observed speed vector may be delayed for a period of time before obtaining the current moment. Specifically, the observed speed value and the observed speed direction can be speed feature information obtained by searching and matching the data collected by the robot's sensing device. For example, in the process of the robot running the Correlative Scan Matching (CSM) algorithm, the posture with the highest probability of being occupied by the laser point cloud in the grid map obtained by scanning and matching in combination with the search window; and the predicted speed value and the predicted speed direction at the current moment can be obtained by correction through the Kalman filter algorithm on the basis of determining the observed speed value and the observed speed direction, and are optimized posture data updated by the Kalman filter. The Kalman filter algorithm mentioned in step A is preferably an extended Kalman filter (EKF).

[0022] Step B: Calculate the absolute value of the difference between the observed speed value and the predicted speed value at the current moment to obtain the Euclidean distance corresponding to the current speed change, and then determine the difference between the Euclidean distance corresponding to the current speed change and the preset Euclidean distance threshold; calculate the cosine value of the angle between the observed speed direction and the predicted speed direction at the current moment to obtain the cosine distance corresponding to the current speed change, and then determine the difference between the cosine distance corresponding to the current speed change and the preset cosine distance threshold; then execute step C.

[0023] Step B calculates the absolute value of the difference between the observed speed value and the predicted speed value at the current moment, which defines the sideslip ratio from the speed magnitude level, that is, the Euclidean distance corresponding to the current speed change is used to represent the sideslip ratio; Step B calculates the cosine value of the angle between the observed speed direction and the predicted speed direction at the current moment, and then the cosine distance (also called cosine similarity) between the difference in speed direction between the observed speed and the predicted speed at the current moment is evaluated to represent the sideslip ratio. The speed or speed measurement value mentioned in this application is generally a speed vector, including speed magnitude and speed direction. Preferably, the timestamp of the observed speed value is aligned with the timestamp of the predicted speed value at the current moment or a mapping relationship is established to improve the utilization rate of the delayed data.

[0024] Among them, the larger the Euclidean distance corresponding to the current speed change and the smaller the cosine distance corresponding to the current speed change, the smaller the similarity between the predicted speed vector and the observed speed vector, that is, the gap between the predicted speed vector and the observed speed vector in size and direction widens; conversely, the predicted speed vector is closer to the observed speed vector.

[0025] Step C: If the Euclidean distance corresponding to the current speed change is greater than the preset Euclidean distance threshold, and the cosine distance corresponding to the current speed change is less than the preset cosine distance threshold, the target impact perception network model is used to classify the perception measurement value sequence to obtain a first predicted probability that the robot is impacted at the current moment and a second predicted probability that the robot is impacted at the current moment. The classification processing process here will calculate / predict the probability value, wherein the target impact perception network model is a trained impact perception neural network; then execute step D. Specifically, when the Euclidean distance corresponding to the current speed change is greater than the preset Euclidean distance threshold, and the cosine distance corresponding to the current speed change is less than the preset cosine distance threshold, the difference between the observed speed vector and the predicted speed vector at the current moment is relatively large. It may be that the robot collides with an obstacle or climbs over an obstacle, causing a sudden change in the acceleration measured by the inertial measurement unit and / or a sudden change in the wheel rotation rate measured by the rotary encoder. Therefore, at this time, it can only be used as a rough judgment result of the robot being impacted to trigger a precise detection of the robot being impacted. The robot still needs to continue to make precise judgments, and the target impact perception network model is used to classify the measurement value sequence to be perceived to obtain a model probability that can represent an impacted state or a model probability that is not impacted.

[0026] In order to form a lightweight impact perception neural network, this embodiment designs the target impact perception network model as a binary classification network to calculate the first predicted probability that the robot is impacted at the current moment and the second predicted probability that the robot is impacted at the current moment, and can also calculate the first predicted probability that the robot is not impacted at the current moment and the second predicted probability that the robot is not impacted at the current moment.

[0027] It is worth noting that this embodiment chooses to use the target impact perception network model to classify the sequence of to-be-perceived measurement values ​​only after determining the Euclidean distance corresponding to the current speed change and the cosine distance corresponding to the current speed change, thereby calculating the first predicted probability that the robot is impacted at the current moment and the second predicted probability that the robot is impacted at the current moment, rather than using the sequence of to-be-perceived measurement values ​​to train the impact perception neural network in real time, thereby reducing the operating burden of the robot and improving the classification accuracy of the target impact perception network model.

[0028] Preferably, the preset Euclidean distance threshold is equal to 0.1; the preset cosine distance threshold is equal to 0.707; so as to leave judgment redundancy in both the distance and the angle cosine value.

[0029] It should be noted that the unimpeded state during actual robot movement can be categorized into three types: unimpeded walking on a flat surface; collision with an obstacle; and climbing over a low obstacle with body vibration. These can serve as negative samples for training the impact perception network model. Therefore, the target impact perception network model is required to classify the sequences of to-be-perceived measurements generated under these various conditions. Of course, the initial impact perception network model / network also needs to be pre-trained using the sequence of to-be-perceived measurements to improve classification and extraction accuracy.

[0030] The sequence of measurement values ​​to be sensed is data collected by the robot's sensing device, including the original posture data of each coordinate axis of the body and the original rotation distance data of the wheels; the robot's sensing device includes an inertial measurement unit and a rotary encoder (equivalent to a wheel odometer), both of which are robot proprioception sensors. The sequence of measurement values ​​to be sensed is a sequence of angular velocity measurement values ​​rotating around each coordinate axis, a sequence of acceleration measurement values ​​pointing in each coordinate axis direction, a sequence of left wheel rotation measurement values, or a sequence of right wheel rotation measurement values. The angular velocity measurement value sequence is collected by the gyroscope in the inertial measurement unit, the acceleration measurement value sequence is collected by the accelerometer in the inertial measurement unit, the left wheel rotation measurement value sequence is collected by the rotary encoder installed on the left wheel, and the right wheel rotation measurement value sequence is collected by the rotary encoder installed on the right wheel.

[0031] Step D: If the first predicted probability that the robot is impacted at the current moment is greater than the second predicted probability that the robot is impacted at the current moment, then it is determined that the robot is impacted, so as to detect that the robot has undergone lateral displacement, thereby completing the impact detection of the robot at the current moment, or it can be understood as predicting the impact detection situation at the current moment. Equivalently, if the target impact perception network model is used to calculate the first predicted probability that the robot is not impacted at the current moment and the second predicted probability that the robot is not impacted at the current moment for the sequence of perceived measurement values, then if the first predicted probability that the robot is not impacted at the current moment is less than the second predicted probability that the robot is not impacted at the current moment, then it is determined that the robot is impacted. Among them, the sum of the first predicted probability that the robot is impacted at the current moment and the first predicted probability that the robot is not impacted at the current moment is equal to the value 1, and the sum of the second predicted probability that the robot is impacted at the current moment and the second predicted probability that the robot is not impacted at the current moment is also equal to the value 1, and these four predicted probabilities all come from the same target impact perception network model.

[0032] In summary, the robot impact detection method designed in steps A to D disclosed in this embodiment uses the Euclidean distance formed between the observed speed value and the predicted speed value at the current moment and the cosine distance formed between the observed speed direction and the predicted speed direction at the current moment as trigger detection starting points, triggering the target impact perception network model to classify and process the data collected by the inertial measurement unit and the rotary encoder. This not only takes into account the necessity of the target impact perception network model's classification processing, but also can integrate the prediction information of at least two types of sensor collection data under the Kalman filter framework, thereby improving the reliability and accuracy of the target impact perception network model's classification; and then only uses the size relationship between the two prediction probabilities to make a judgment, thereby overcoming the misjudgment problem caused by the sudden change in the measurement value of the inertial measurement unit and improving the accuracy of impact detection.

[0033] It should be noted that, in the robot impact detection method disclosed in the present application, the execution order of steps A to D is: first execute step A, and after obtaining the predicted speed value and the predicted speed direction at the current moment in step A, execute step B; after calculating the Euclidean distance corresponding to the current speed change and the cosine distance corresponding to the current speed change in step B, execute step C; after obtaining the first predicted probability of the robot being impacted at the current moment and the second predicted probability of the robot being impacted at the current moment in step C, execute step D or further judge the relationship between the first predicted probability of the robot being impacted at the current moment and the second predicted probability of the robot being impacted at the current moment, execute step D; then step D determines that the robot is impacted, which can be understood as currently being impacted.

[0034] In step C, the method of using the target impact perception network model to classify the perception measurement value sequence to obtain a first predicted probability of the robot being impacted at the current moment and a second predicted probability of the robot being impacted at the current moment, as shown in FIG2 , includes:

[0035] Step C1: Extract a sequence of measurement values ​​to be sensed from the raw data collected by the robot's sensing device using a sliding window. Then, proceed to Step C2. The sequence of measurement values ​​to be sensed is extracted using a sliding window technique for sequential scanning. Generally, the extraction is performed along the robot's motion timeline, sequentially obtaining multiple sequences of measurement values ​​to be sensed. Each sequence of measurement values ​​to be sensed scanned by the sliding window represents a type of sequence of measurement values ​​to be sensed, representing a sequence of measurement values ​​to be sensed for a particular sensor type. Specifically, as the sliding window traverses the raw data, the multiple sequences of measurement values ​​to be sensed are sequentially extracted in the order in which they were acquired. In which, the length of the measurement value sequence to be sensed is preferably the window length of the sliding window, and can also be equal to one half, one quarter or even one eighth of the window length of the sliding window, so that one sliding window frames at least one measurement value sequence to be sensed in the original data, at least one type of measurement value sequence to be sensed, and there are measurement values ​​of at least one dimension (coordinate axis dimension) within the same type of measurement value sequence to be sensed; therefore, the robot can extract multiple types of measurement value sequences to be sensed (i.e., measurement value sequences to be sensed of multiple sensor types) in sequence through one sliding window, or can extract measurement value sequences to be sensed corresponding to one type through multiple sliding windows.

[0036] For example, starting from the initial moment, the robot measures and records the raw data through the sensing device every acquisition cycle, thus acquiring raw data. During this acquisition process, the length of the sliding window is given, and the number of raw data contained in the sliding window is N. Whenever the number of recorded raw data from the same sensor equals N, a sequence of measurement values ​​to be sensed is extracted, wherein the length of the given sliding window is generally equal to N. This can also be understood as follows: within the currently traversed area, the sliding window can extract the same type of measurement values ​​to be sensed, which are the preset number of frames most recently cached by the sensing device, from the raw data to form a sequence of measurement values ​​to be sensed.

[0037] Preferably, the sequence of measurement values ​​to be sensed and the raw data both include vector magnitudes and vector direction angles; the robot's sensing device includes an inertial measurement unit and a rotary encoder (equivalent to a wheel odometer), both of which are robot proprioception sensors. The sequence of measurement values ​​to be sensed is a sequence of angular velocity measurement values ​​rotating around each coordinate axis, a sequence of acceleration measurement values ​​pointing in each coordinate axis direction, a sequence of left wheel rotation measurement values, or a sequence of right wheel rotation measurement values. The angular velocity measurement value sequence is collected by the gyroscope in the inertial measurement unit, the acceleration measurement value sequence is collected by the accelerometer in the inertial measurement unit, the left wheel rotation measurement value sequence is collected by the rotary encoder installed on the left wheel of the robot, and the right wheel rotation measurement value sequence is collected by the rotary encoder installed on the right wheel of the robot.

[0038] Step C2: Input the sequence of measurement values ​​to be sensed into the target impact perception network model to generate input features, and then execute step C3; wherein, the target impact perception network model includes a connection layer, and the sequence of measurement values ​​to be sensed generates input features in the connection layer. Specifically, the robot connects all types of sequence of measurement values ​​to be sensed currently extracted by the sliding window along the channels of the corresponding dimensions, merges them into the input features, and inputs them into the target impact perception network model. Each type of sequence of measurement values ​​to be sensed is a sequence of measurement values ​​to be sensed of the same sensor type collected within a time period, and may include measurement values ​​of multiple dimensions. One type of sequence of measurement values ​​to be sensed may be transmitted by a channel of one dimension, and multiple types of sequence of measurement values ​​to be sensed may be sequentially connected to the same channel to synthesize the input features; then all types of sequence of measurement values ​​to be sensed within a time period may be regarded as synthesizing one input feature; preferably, the length of a time period is the window length of a sliding window, which is the length of a sequence of measurement values ​​to be sensed.

[0039] When the angular velocity measurement value sequence, the acceleration measurement value sequence, the left wheel rotation measurement value sequence and the right wheel rotation measurement value sequence are input into the target impact perception network model, it is necessary to occupy channels of at least four dimensions in the target impact perception network model and connect the measurement value sequence to be perceived. At this time, the measurement value sequence to be perceived after connecting the channels of corresponding dimensions forms the input feature, which is equivalent to the inertial posture feature measurement value sensed by the inertial measurement unit and / or the wheel rotation distance feature measurement value sensed by the rotary encoder, and is marked as a sample set recognized by the target impact perception network model.

[0040] Step C3, controlling the input features to perform convolution operations in multiple convolutional layers in sequence, which is equivalent to controlling the input features to be encoded in multiple convolutional layers stacked continuously, to obtain discriminant features, which are the encoding results output by the last convolutional layer (traversing layer by layer in the multiple convolutional layers stacked continuously) or the last convolutional layer (traversing in the direction of multiple convolutional layers connected in sequence), and exist in the form of a matrix; and determining that the multi-layer convolution operation is completed in the target impact perception network model. Then the discriminant features are input into the pooling layer. Then the discriminant features are controlled to perform average pooling processing in the pooling layer (from the perspective of mathematical operation, it is understood as averaging the elements in the matrix where the discriminant features are located) to obtain target classification features, that is, the target classification features are calculated by integrating the feature information of multiple convolutional layers in the target impact perception network model and the feature information of a pooling layer. The target classification features are equivalent to the target state of the features to be classified, and it is determined that the pooling operation is completed in the target impact perception network model; then step C4 is executed. Wherein, the target impact perception network model includes multiple convolutional layers and a pooling layer.

[0041] When the target impact perception network model includes four one-dimensional convolutional layers, the input features are sequentially convolved in the four one-dimensional convolutional layers to obtain discriminant features, and it is determined that multi-layer convolution operations are completed in the target impact perception network model, and the discriminant features are determined to be the encoding results in the four one-dimensional convolutional layers, that is, the results of continuous multi-layer convolution operations; wherein the four one-dimensional convolutional layers are sequentially represented as a first convolutional layer (considered as the first convolutional layer in the target impact perception network model), a second convolutional layer (considered as the second convolutional layer in the target impact perception network model), a third convolutional layer (considered as the third convolutional layer in the target impact perception network model), and a fourth convolutional layer (considered as the fourth convolutional layer in the target impact perception network model); the first convolutional layer, the second convolutional layer, and the third convolutional layer output intermediate variables to make sub-encoding results of the corresponding output layers; the fourth convolutional layer outputs the encoding result as the discriminant feature in the form of a matrix. The discriminant features are then input into a pooling layer, and then the discriminant features are controlled to undergo average pooling within the pooling layer to obtain the features to be classified. From a mathematical operation perspective, this is understood as averaging the elements in the matrix containing the discriminant features to obtain the features to be classified. The pooling operation is then determined to be completed within the target impact perception network model, and is determined to be completed by integrating four convolutional layers and a pooling layer. This integrates the convolution results of the four one-dimensional convolutional layers and the average value of the one pooling layer into the features to be classified, thereby reducing the order of magnitude of the parameters of the target impact perception network model.

[0042] Step C4: Classify the target classification features using a softmax algorithm to obtain a first predicted probability that the robot is currently experiencing an impact and a second predicted probability that the robot is currently experiencing an impact, thereby connecting the target classification features to two nodes. The classification process includes a probability prediction for each classification scenario, where each classification scenario is a sequence of measured values ​​to be sensed or the feature to be classified into a corresponding classification by the softmax algorithm. Then, step D disclosed in the aforementioned embodiment is executed to make a final judgment, rather than a preliminary judgment, as to whether the robot is currently experiencing an impact. In this embodiment, the first predicted probability that the robot is currently experiencing an impact is the probability that the sequence of measured values ​​to be sensed is classified as a sequence of measured values ​​extracted when the robot is currently experiencing an impact; the second predicted probability that the robot is currently experiencing an impact is the probability that the sequence of measured values ​​to be sensed is classified as a sequence of measured values ​​extracted when the robot is currently experiencing an impact.

[0043] In this embodiment, the softmax algorithm is configured as a softmax classifier in the target impact perception network model; the softmax classifier is used to calculate the first predicted probability of the robot being impacted at the current moment and the second predicted probability of the robot being impacted at the current moment using the target classification feature; the target classification feature is the information output by the two nodes to which it is connected after multi-layer convolution operations and pooling operations, and the two nodes to which it is connected include a first node (representing the position point of the robot being impacted, which is a positive sample required for training the target impact perception network model) and a second node (representing the position point of the robot being not impacted, which is a negative sample required for training the target impact perception network model). In this embodiment, the first predicted probability of the robot being impacted at the current moment is the probability that the sequence of measured values ​​to be sensed is classified as the sequence of measured values ​​extracted when the robot is currently in the state of being impacted, thereby forming the probability of the first classification generated by the softmax classifier, and is also the probability that the robot's current position falls into the first node. The second predicted probability of the robot being impacted at the current moment is the probability that the sequence of measured values ​​to be sensed is classified as the sequence of measured values ​​extracted when the robot is currently in the state of being unimpeded, thereby forming the probability of the second classification generated by the softmax classifier, and is also the probability that the robot's current position falls into the second node. This improves the feature extraction capability of the target impact perception network model and reduces misjudgments.

[0044] In summary, the present application uses the target impact perception network model to perform robot impact detection, which can reduce dependence on computer storage resources, effectively reduce training time, and improve the accuracy of posture feature information classification from the overall convolutional neural network structure of the target impact perception network model. The model probability of evaluating two classification situations (the first predicted probability of the robot being impacted at the current moment and the second predicted probability of the robot being impacted at the current moment) is used to describe the classification results as a judgment parameter for impact detection, so as to better balance the three requirements of the target impact perception network model or impact perception neural network: robot impact detection efficiency, computing resources, and robot impact detection accuracy.

[0045] When the robot climbs over low obstacles such as chair legs and table legs, the robot does not experience impact but experiences body vibration. In the time series of sensor data collected by the robot, the acceleration (sensed by the inertial measurement unit) and angular rate change significantly within a relatively short acquisition time series. For example, the Euclidean distance between the predicted velocity vector and the observed velocity vector is greater than a velocity difference threshold, and the cosine distance between the predicted velocity vector and the observed velocity vector may be less than a cosine function threshold, thereby causing significant changes in the sequence of measured values ​​to be sensed in the associated dimension, which is prone to misjudgment. In order to detect the robot being impacted in scenarios where the robot is climbing over low obstacles such as chair legs and table legs and in scenarios where the robot is impacted and undergoes lateral displacement, the present application uses the target impact perception network model disclosed in the aforementioned embodiment to classify the sequence of measured values ​​to be sensed, and obtain a first predicted probability that the robot is impacted at the current moment and a second predicted probability that the robot is impacted at the current moment. Then, when the first predicted probability that the robot is impacted at the current moment is greater than the second predicted probability that the robot is impacted at the current moment, it is determined that the robot is impacted, so as to effectively detect the lateral displacement of the robot based on a certain accuracy trained by the aforementioned target impact perception network model.

[0046] It should be noted that, in step C, the execution order of steps C1 to C4 includes:

[0047] When the Euclidean distance corresponding to the current speed change is greater than the preset Euclidean distance threshold, and the cosine distance corresponding to the current speed change is less than the preset cosine distance threshold, and the target impact perception network model has been trained, step C1 is first executed. After the sequence of measurement values ​​to be perceived is extracted in step C1, step C2 is executed, wherein the moment when the robot's sensing device collects raw data may be ahead of or synchronized with the moment when the observed speed value and the observed speed direction are obtained in step A; after the input feature is generated in step C2, step C3 is executed; after the target classification feature is obtained in step C3, step C4 is executed; after the first predicted probability of the robot being impacted at the current moment and the second predicted probability of the robot being impacted at the current moment are obtained in step C4, step D is executed.

[0048] Preferably, before executing the aforementioned step D, the method further includes: step C5, based on the loss function, using the first predicted probability that the robot is impacted at the current moment and the second predicted probability that the robot is impacted at the current moment; and updating the model parameters of the target impact perception network model, and then updating the target impact perception network model after the model parameters are updated to the target impact perception network model; then re-inputting the sequence of measurement values ​​to be perceived into the updated target impact perception network model, repeating steps C2 to C5 until the latest calculated loss value is less than the preset loss threshold, and then updating the target impact perception network model after the model parameters are updated to the target impact perception network model. Specifically, when the model parameters are updated once, the target impact perception network model is updated once, and the updated target impact perception network model is trained using the same sequence of measurement values ​​to be perceived or the input features generated by it, including: performing multi-layer convolution operations and pooling operations; using the softmax algorithm for classification processing to calculate the latest first prediction probability and the latest second prediction probability; calculating the loss value based on the loss function; by repeatedly executing steps C2 to C5, preferably minimizing the loss function, and then submitting the calculated latest first prediction probability and the latest second prediction probability to step D for judgment.

[0049] Among them, the loss function is binary cross entropy, and the first prediction probability and the latest second prediction probability are the prediction results of two classifications in the loss function respectively; the target impact perception network model is designed as a two-classification network. Compared with the prior art, the convolutional neural network used in this embodiment is simplified to form a lightweight neural network model.

[0050] In step C5, the target impact perception network model is updated based on the loss function by using the Adam algorithm to update the weights, i.e., by using a gradient descent algorithm to update the weight matrix. The target impact perception network model, after weight update, is then iterated and solved step by step by gradient descent to continuously update the target impact perception network model to the target impact perception network model, thereby minimizing the loss value. Furthermore, the target impact perception network model is corrected for deviations between the sequence of perceived measurement values ​​and the weights. This improves the accuracy of feature extraction from the target impact perception network model and further enhances the feasibility of using the target impact perception network model to detect whether the robot has been impacted.

[0051] As an embodiment, before executing step C or before executing step A, the target impact perception network model is constructed, wherein the target impact perception network model is a trained impact perception neural network. The method for constructing the target impact perception network model, as shown in FIG3 , includes:

[0052] Step 1: Extract the sequence of measurement values ​​to be sensed from the raw data collected by the robot's sensing device using a sliding window, and then proceed to Step 2. The extraction in Step 1 is achieved by sequentially scanning the raw data using a sliding window technique. Generally, the extraction is performed by sliding along the robot's motion timeline, sequentially obtaining multiple sequences of measurement values ​​to be sensed. Each sequence of measurement values ​​to be sensed scanned by the sliding window is a type of sequence of measurement values ​​to be sensed, representing a sequence of measurement values ​​to be sensed for a sensor type. Specifically, as the sliding window slides through the raw data, the multiple sequences of measurement values ​​to be sensed are sequentially extracted in the order in which they were collected. In which, the length of the measurement value sequence to be sensed is preferably the window length of the sliding window, and can also be equal to one half, one quarter or even one eighth of the window length of the sliding window, so that one sliding window frames at least one measurement value sequence to be sensed in the original data, at least one type of measurement value sequence to be sensed, and there are measurement values ​​of at least one dimension (coordinate axis dimension) within the same type of measurement value sequence to be sensed; therefore, the robot can extract multiple types of measurement value sequences to be sensed (i.e., measurement value sequences to be sensed of multiple sensor types) in sequence through one sliding window, or can extract measurement value sequences to be sensed corresponding to one type through multiple sliding windows.

[0053] For example, starting from the initial moment, the robot measures and records the raw data through the sensing device every acquisition cycle, that is, collects the raw data; in this acquisition process, the length of the sliding window is given, and the number of raw data contained in the sliding window is N; whenever the number of raw data recorded from the same sensor is equal to N, a sequence of measurement values ​​to be sensed is extracted, wherein the length of the given sliding window is generally equal to N.

[0054] It can also be understood that: within the current traversal area, the sliding window can extract the preset number of frames of the same type of measurement values ​​to be sensed that are most recently cached by the sensing device from the raw data to form a sequence of measurement values ​​to be sensed.

[0055] Preferably, the sequence of measurement values ​​to be sensed and the raw data both include vector magnitudes and vector direction angles; the robot's sensing device includes an inertial measurement unit and a rotary encoder (equivalent to a wheel odometer), both of which are robot proprioception sensors. The sequence of measurement values ​​to be sensed is a sequence of angular velocity measurement values ​​rotating around each coordinate axis, a sequence of acceleration measurement values ​​pointing in each coordinate axis direction, a sequence of left wheel rotation measurement values, or a sequence of right wheel rotation measurement values. The angular velocity measurement value sequence is collected by the gyroscope in the inertial measurement unit, the acceleration measurement value sequence is collected by the accelerometer in the inertial measurement unit, the left wheel rotation measurement value sequence is collected by the rotary encoder installed on the left wheel of the robot, and the right wheel rotation measurement value sequence is collected by the rotary encoder installed on the right wheel of the robot.

[0056] Step 2: Connect the sequence of measurement values ​​to be sensed along the channels of the corresponding dimensions to generate input features; then execute step 3. In some embodiments, the robot connects all types of sequence of measurement values ​​to be sensed currently extracted by the sliding window along the channels of the corresponding dimensions, merges them into the input features, and inputs them into the impact perception neural network. Each type of sequence of measurement values ​​to be sensed is a sequence of measurement values ​​to be sensed of the same sensor type collected within a time period, and may include measurement values ​​of multiple dimensions. One type of sequence of measurement values ​​to be sensed may be transmitted by a channel of one dimension, and multiple types of sequence of measurement values ​​to be sensed may be sequentially connected to the same channel to synthesize the input features; then all types of sequence of measurement values ​​to be sensed within a time period may be regarded as synthesizing one input feature; preferably, the length of a time period is the window length of a sliding window, which is the length of a sequence of measurement values ​​to be sensed.

[0057] Step 3, control the input features to perform multi-layer convolution operations and one layer of pooling operations in the pre-constructed impact-perceiving neural network to obtain features to be classified, and then execute step 4. Wherein, the pre-constructed impact-perceiving neural network includes multiple convolution layers and one pooling layer. The convolution operation of each convolution layer is to control the angular velocity matrix, acceleration matrix and left and right wheel rotation distance matrix formed by the input features to perform convolution operations with the corresponding weight matrix respectively. In mathematical operations, it can be equivalent to multiplying each element in the input features of the corresponding convolution layer by the weight of the same convolution layer to obtain a matrix of measurement values ​​of each dimension (equivalent to the training samples in the impact-perceiving neural network); the features output by the last convolution layer are transmitted to the pooling layer for a layer of pooling operation; the features to be classified obtained by the pooling operation are the matrices that need to be classified.

[0058] Preferably, the pre-constructed impact perception neural network is constructed according to the construction method of the convolutional neural network. The impact perception neural network includes multiple convolutional layers and a pooling layer. The multiple convolutional layers are connected in sequence, and the output layers and input layers of the two adjacent convolutional layers are aligned and connected (the depth between the connected output layers and input layers is equal). The impact perception neural network constructed in this embodiment belongs to a convolutional neural network; the impact perception neural network also includes a connection layer. Before all the measurement value sequences to be sensed are input into the impact perception neural network, the measurement value sequences to be sensed of all dimensions from the inertial measurement unit and the measurement value sequences to be sensed of all dimensions from the rotary encoder are connected along the channels of the corresponding dimensions to generate input features, which can constitute the connection layer in the convolutional neural network. After the input features are generated, it can be understood as inputting the sequence of measurement values ​​to be perceived into the impact-perceiving neural network, and as inputting the labeled samples to be classified into the first convolutional layer through the corresponding dimensional channels, thereby adapting to the first convolutional layer of the input impact-perceiving neural network. The adapted input here includes the depth adaptation of the network layer of the convolutional neural network and the adaptation of the channels connected in each dimension; based on this, the impact-perceiving neural network is constructed layer by layer according to the construction method of the convolutional neural network to form a convolutional neural network with multiple layers of convolutional layers stacked and connected.

[0059] When a sequence of angular velocity measurements, a sequence of acceleration measurements, a sequence of left wheel rotation measurements, and a sequence of right wheel rotation measurements are input into the impact-sensing neural network, it is necessary to design channels of at least four dimensions in the convolutional neural network and connect the sequences of measurements to be sensed. In this case, the sequence of measurements to be sensed, after connecting the channels of corresponding dimensions, forms the input features, which are equivalent to the inertial posture feature measurements sensed by the inertial measurement unit and / or the wheel rotation distance feature measurements sensed by the rotary encoder, and are thus labeled as a sample set recognized by the convolutional neural network. Furthermore, if the sequence of angular velocity measurements includes one dimension of angular velocity obtained by rotating about the robot's yaw angle, one dimension of angular velocity obtained by rotating about the robot's roll angle, and one dimension of angular velocity obtained by rotating about the robot's pitch angle, the number of channels designed in the impact-sensing neural network increases, and the number of input features also increases.

[0060] Specifically, the step 3 controls the input features to perform convolution operations in the four one-dimensional convolutional layers in sequence to obtain discriminant features, determines that multi-layer convolution operations are completed in the impact-perceiving neural network, and determines that the discriminant features are the encoding results in the four one-dimensional convolutional layers, that is, the results of continuous multi-layer convolution operations; wherein the four one-dimensional convolutional layers are represented in sequence as the first convolutional layer (regarded as the first convolutional layer in the impact-perceiving neural network), the second convolutional layer (regarded as the second convolutional layer in the impact-perceiving neural network), the third convolutional layer (regarded as the third convolutional layer in the impact-perceiving neural network) and the fourth convolutional layer (regarded as the fourth convolutional layer in the impact-perceiving neural network); the first convolutional layer, the second convolutional layer and the third convolutional layer output intermediate variables to make sub-encoding results of the corresponding output layers; the fourth convolutional layer outputs the encoding result as the discriminant feature in the form of a matrix. The discriminant features are then input into a pooling layer, which is then controlled to undergo average pooling within the pooling layer to obtain the features to be classified. From a mathematical perspective, this is understood as averaging the elements in the matrix containing the discriminant features to obtain the features to be classified. The pooling operation is then determined to be completed within the impact-aware neural network, and is determined to be completed by integrating four convolutional layers and a pooling layer. This integrates the convolution results of the four one-dimensional convolutional layers and the average value of the one pooling layer into the features to be classified, reducing the order of magnitude of the neural network parameters.

[0061] Preferably, this embodiment selects the appropriate number of convolutional layers and pooling layers, the size of the convolutional layer filters, the shift step size of the convolutional layer filters, and the pooling operation mode based on the dimension of the sequence of measured values ​​to be sensed (including the components of the same dimensional vector in the directions of each coordinate axis or the rotation components around each coordinate axis) to construct the impact-sensing neural network. The pooling operation has two operation modes: maximum value and average value.

[0062] Step 4: Classify the features to be classified described in Step 3 using a softmax algorithm to obtain a first predicted probability of being impacted and a second predicted probability of being impacted, and connect the features to be classified to two nodes. The classification process includes probability prediction for each classification scenario, where each classification scenario is a sequence of measured values ​​to be sensed or a scenario where the features to be classified are classified into a corresponding category by the softmax algorithm. Then, step 5 is performed. The softmax algorithm in Step 4 divides the features to be classified into two categories, and fully connects the matrix output after multiple convolutions and one pooling operation to the two nodes. Specifically, in Step 4, the softmax algorithm is configured as a softmax classifier in the impact-perceiving neural network. The softmax classifier is used to calculate the first predicted probability of being impacted and the second predicted probability of being impacted using the features to be classified. The features to be classified are the information output by the two nodes to which they are connected after multiple convolutions and pooling operations. The two nodes to which they are connected include a first node (representing the location of the robot in an impacted state, which is a positive sample required for training the impact-perceiving neural network) and a second node (representing the location of the robot in an unimpacted state, which is a negative sample required for training the impact-perceiving neural network). In this embodiment, the first impact prediction probability is the probability that the measurement value sequence to be sensed or the feature to be classified is classified as the measurement value sequence extracted when the robot is in an impacted state, so as to form the probability of the first classification generated by the softmax classifier, and is also the probability that the robot's posture falls into the first node; the second impact prediction probability is the probability that the measurement value sequence to be sensed or the feature to be classified is classified as the measurement value sequence extracted when the robot is currently not in an impacted state, so as to form the probability of the second classification generated by the softmax classifier, and is also the probability that the robot's posture falls into the second node.

[0063] Step 5: Based on the loss function, calculate the loss value using the first predicted probability of being impacted and the second predicted probability of being impacted; update the weights of the impact-aware neural network, and then update the impact-aware neural network after the weight update to the impact-aware neural network; then execute step 6. The weights to be updated in step 5 are the weights of the convolutional layer, that is, the weights of the network. The loss function is binary cross entropy, and the first predicted probability of being impacted and the second predicted probability of being impacted are the prediction results of two categories in the loss function. By designing the impact-aware neural network as a binary classification network, the convolutional neural network trained in this embodiment is simplified compared to the prior art, forming a lightweight neural network model.

[0064] In step 5, the weights of the shock-perceiving neural network are updated based on the loss function using the Adam algorithm, i.e., using a gradient descent algorithm to update the weight matrix. The weight matrix is ​​then updated by iteratively solving the problem step by step through gradient descent, and the shock-perceiving neural network after weight update is continuously updated to the shock-perceiving neural network, so as to obtain a minimized loss value in step 6. Furthermore, the bias correction is performed on the sequence of perceived measurement values ​​and the weights. This improves the accuracy of feature extraction of the shock-perceiving neural network, thereby improving the feasibility of using the shock-perceiving neural network to detect whether the robot has been impacted.

[0065] In the process of introducing specific variables to perform function operations, the loss function includes: L = -(p gtp log(p p )+(1-p gtp )log(p np )), wherein L is a loss value configured as the loss value of the shock-aware neural network; p gtp is the true value of the label, p gtp =1, indicating that the current prediction result is that the robot is impacted; p is the predicted probability of the first shock, p np is the second predicted probability of being impacted; when the first predicted probability of being impacted p obtained in step 4 p and the second predicted probability of being impacted p np When the label value p changes, gtp It may change, causing the loss value to adapt according to the loss function; in the process of updating the weight using the gradient descent algorithm (Adam algorithm), the loss value is caused to become smaller, thereby improving the optimization effect of the loss function on the impact-perceiving neural network.

[0066] Step 6, repeating steps 2 to 5, so as to realize that the input features generated by each of the measurement value sequences to be sensed are input into the impact perception neural network after weight update, until the latest calculated loss value is less than the preset loss threshold, and then the impact perception neural network after weight update is updated to the impact perception neural network. Specifically, when the weight is updated once, the impact perception neural network is updated once, and the updated impact perception neural network uses the same measurement value sequence to be sensed or the input features generated by it for training, including: performing multi-layer convolution operations and pooling operations; using the softmax algorithm for classification processing to calculate the latest first impact prediction probability and the latest second impact prediction probability; and calculating the loss value based on the loss function. Step 6 preferably minimizes the loss function by repeatedly executing steps 2 to 5, and then marks the impact perception neural network updated in the latest executed step 5 as the target impact perception network model. Thus, the training constitutes a complete convolutional neural network model.

[0067] During the repetitive execution of steps 2 to 5, each time step 5 is executed, before using the loss function to calculate the loss value, a determination is made as to whether the first predicted probability of being impacted obtained in step 4 is greater than the second predicted probability of being impacted obtained in step 4. If so, the true value of the label required by the loss function is set to 1, and the current prediction result is determined to be that the robot was impacted. Otherwise, the true value of the label required by the loss function is set to 0, and the current prediction result is determined to be that the robot was not impacted. It is worth noting that before the calculated loss value is less than a preset loss threshold, the impact determination result of the robot, derived based on the magnitude relationship between the first predicted probability of being impacted and the second predicted probability of being impacted, is a rough judgment result, i.e., the current prediction result. Therefore, the process of the calculated loss value changing from being greater than or equal to the preset loss threshold to being less than the preset loss threshold is considered a training process for the impact-sensing neural network, and is also a process of updating the weights, the first predicted probability of being impacted, the second predicted probability of being impacted, the relevant model parameters of the convolutional layer, and the relevant model parameters of the pooling layer. In some training embodiments, in order to perform rigorous model analysis, the network's learning rate and the number of dimensional vectors within the sequence of measured values ​​to be sensed are adjusted. When the learning rate is set to 0.001, the number of samples within a single set of measured values ​​to be sensed is set to 8711, and 228,176 sets of measured values ​​to be sensed are continuously collected, the aforementioned weight update can be iterated 140 times, and the impact-sensing neural network achieves the highest precision and recall. The number of iterations can be increased or decreased as needed to improve accuracy and consider time costs.

[0068] After executing steps 1 to 6 above, the present application successively performs multi-layer convolution operations, pooling operations, softmax algorithm classification processing, calculation of loss values ​​and weight updates to train the target impact perception network model, which not only improves the extraction accuracy and classification accuracy of posture features, but also reduces the order of magnitude of the convolutional neural network (or the impact perception neural network) parameters. The target impact perception network model constructed thereby forms a lightweight network model, which speeds up the detection of impact on the robot.

[0069] On the basis of the above embodiment, in the process of classifying the sequence of perceived measurement values ​​using the target impact perception network model, the first predicted probability that the robot is impacted at the current moment and the second predicted probability that the robot is impacted at the current moment are calculated, which is equivalent to updating the first predicted probability of being impacted to the first predicted probability that the robot is impacted at the current moment, and updating the second predicted probability of being impacted to the second predicted probability that the robot is impacted at the current moment.

[0070] It should be noted that, in the process of constructing the target impact perception network model, the execution order of steps 1 to 6 includes:

[0071] When the robot's sensing device collects raw data (the observed speed value and the observed speed direction can be obtained in advance or synchronously with step A), first execute step 1. After extracting the sequence of measurement values ​​to be sensed in step 1, execute step 2; after generating input features in step 2, execute step 3; after obtaining the features to be classified in step 3, execute step 4; after obtaining the first impact prediction probability and the second impact prediction probability in step 4, execute step 5; after updating the impact perception neural network in step 5, execute step 6 to update the target impact perception network model.

[0072] Among them, each time step 6 is executed, it is first determined whether the latest calculated loss value is less than the preset loss threshold; when it is determined that the latest calculated loss value is greater than or equal to the preset loss threshold, the newly executed step 5 returns to step 2, and then steps 2, 3, 4 and 5 are executed in sequence, forming a round of repeated execution of steps 2 to 5, and then the weight of the impact-perceiving neural network is updated again, and then step 6 is executed. Steps 2 to 5 are repeated in this way until it is determined that the latest calculated loss value is less than the preset loss threshold, and the impact-perceiving neural network after the weight update obtained in the newly executed step 5 is marked as the target impact-perceiving network model, and the target impact-perceiving network model is provided for the subsequent execution of step C.

[0073] As an embodiment, the construction method of the impact perception neural network includes: connecting four one-dimensional convolutional layers in sequence to form four consecutively stacked convolutional layers, and then connecting one pooling layer, and configuring the kernel length of each one-dimensional convolutional layer to be 3, and configuring the moving step size of the convolutional layer filter in each one-dimensional convolutional layer to be 1, and configuring the pooling operation in the pooling layer to be an average operation, so as to construct the impact perception neural network. Among them, the original value of the weight of the convolutional layer is randomly generated; the input features and the weight of the convolutional layer are configured in the convolutional layer for convolution operation. Specifically, the four convolutional layers are connected in sequence, and it is ensured that the output layer and the input layer between two adjacent convolutional layers are aligned and connected (the depth between the connected output layer and input layer is equal). The impact perception neural network constructed in this embodiment belongs to a convolutional neural network; the impact perception neural network also includes a connection layer to integrate and connect the sequence of measured values ​​to be perceived in the same dimension and generate the input features. Thus, according to the construction method of the convolutional neural network, the impact perception neural network is constructed layer by layer to form a convolutional neural network with multiple convolutional layers stacked and connected. Therefore, this embodiment selects the appropriate number of convolution layers and pooling layers, the size of the convolution layer filters, the moving step size of the convolution layer filters, and the pooling operation method according to the dimension of the measurement value sequence to be perceived (including the number of components of the same dimensional vector in the direction of each coordinate axis or the number of rotation components around each coordinate axis) by executing steps 1 to 6, so as to train the target impact perception network model, which can adapt to the measurement value sequences to be perceived of different dimensions (such as angular velocity measurement value sequence, acceleration measurement value sequence, left wheel rotation measurement value sequence, right wheel rotation measurement value sequence, etc.).

[0074] As an embodiment, the method for generating input features includes: connecting each type of measurement value sequence to be sensed along a channel of a corresponding dimension according to the type of data collected by the robot's sensing device (sensor type) to generate the input features; specifically, extracting multiple measurement value sequences to be sensed in sequence according to the order of acquisition as a sliding window slides through the raw data. The length of the measurement value sequence to be sensed is preferably the window length of the sliding window, so that one sliding window frames one measurement value sequence to be sensed within the raw data, i.e., one type of measurement value sequence to be sensed; and the same type of measurement value sequence to be sensed contains measurement values ​​of at least one dimension (coordinate axis dimension). On this basis, the robot connects all types of currently extracted measurement value sequences to be sensed along the channels of corresponding dimensions, merges them into the input features, and inputs them into the impact perception neural network, wherein each type of measurement value sequence to be sensed is a measurement value sequence to be sensed of the same sensor type collected within a time period, which may include measurement values ​​of multiple dimensions, one type of measurement value sequence to be sensed may be transmitted by a channel of one dimension, and multiple types of measurement value sequences to be sensed may be connected to the same channel in sequence to synthesize the input features; then all types of measurement value sequences to be sensed within a time period may be regarded as synthesizing one input feature, and preferably, the length of a time period is the window length of a sliding window, which is the length of a measurement value sequence to be sensed.

[0075] In this embodiment, the robot's sensing device includes an inertial measurement unit and a rotary encoder, the inertial measurement unit includes an accelerometer for sensing acceleration measurement values ​​and a gyroscope for sensing angular velocity measurement values, and the rotary encoder includes a left-rotating encoder for sensing left wheel rotation measurement values ​​and a right-rotating encoder for sensing right wheel rotation measurement values; therefore, the measurement value sequence to be sensed is a 3-axis angular velocity measurement value sequence, a 3-axis acceleration measurement value sequence, a 1-axis left wheel rotation measurement value sequence, or a 1-axis right wheel rotation measurement value sequence, so that the dimension of the input feature is at least equal to 8; wherein the input feature is set in the form of a matrix; specifically, for one of the measurement value sequences to be sensed, especially within the 3-axis angular velocity measurement value sequence and the 3-axis acceleration measurement value sequence, the 1-axis measurement value sequence is connected to the impact perception neural network along a channel of one dimension, the 1-axis measurement value sequence is a measurement value sequence obtained in the yaw angle direction, roll angle direction, or pitch angle direction of the robot; the 1-axis measurement value sequence is a vector sequence, so that the angular velocity measurement value and the acceleration measurement value are both set in the form of vectors, and are arranged into a time series according to a certain time line. Therefore, after aligning the timestamps of the measurement value sequence to be sensed collected in real time by the sensing device, the steps A to D are performed to mark the measurement value sequence to be sensed as being impacted or not impacted.

[0076] Preferably, the target impact perception network model includes 4 one-dimensional convolutional layers; the 4 one-dimensional convolutional layers are sequentially represented as the first convolutional layer, the second convolutional layer, the third convolutional layer and the fourth convolutional layer; the depth of the input layer in the first convolutional layer is 8; the depth of the input layer in the first convolutional layer is equal to the number of column elements of the input feature, wherein the input feature is a vector combination containing measurement values ​​of each coordinate axis in the form of a matrix. The depth of the output layer in the first convolutional layer is equal to the depth of the input layer in the second convolutional layer, the depth of the output layer in the second convolutional layer is equal to the depth of the input layer in the third convolutional layer, and the depth of the output layer in the third convolutional layer is equal to the depth of the input layer in the fourth convolutional layer, so that the first convolutional layer, the second convolutional layer, the third convolutional layer and the fourth convolutional layer are sequentially connected to form 4 continuously stacked convolutional layers in the three-dimensional coordinate system.

[0077] In this preferred embodiment, the ratio of the depth of the input layer in the first convolutional layer to the depth of the output layer in the first convolutional layer is smaller than the ratio of the depth of the input layer in the second convolutional layer to the depth of the output layer in the second convolutional layer; the ratio of the depth of the input layer in the second convolutional layer to the depth of the output layer in the second convolutional layer is smaller than the ratio of the depth of the input layer in the third convolutional layer to the depth of the output layer in the third convolutional layer; the ratio of the depth of the input layer in the third convolutional layer to the depth of the output layer in the third convolutional layer is smaller than the ratio of the depth of the input layer in the fourth convolutional layer to the depth of the output layer in the fourth convolutional layer. The reciprocal of the ratio of the depth of the input layer in the second convolutional layer to the depth of the output layer in the second convolutional layer is the ratio of the depth of the input layer in the third convolutional layer to the depth of the output layer in the third convolutional layer.

[0078] Schematically, the depth of the input layer in the first convolutional layer is 8, the depth of the output layer in the first convolutional layer is 64, the depth of the input layer in the second convolutional layer is 64, the depth of the output layer in the second convolutional layer is 128, the depth of the input layer in the third convolutional layer is 128, the depth of the output layer in the third convolutional layer is 64, the depth of the input layer in the fourth convolutional layer is 64, and the depth of the output layer in the fourth convolutional layer is 2. It can be understood that it is beneficial to use the softmax algorithm in the impact-aware neural network to calculate the first impact prediction probability and the second impact prediction probability, and reduce the number of channels between the convolutional layer and the pooling layer.

[0079] The sliding window maintains a constant window length during the process of traversing the original data; within the sliding window, the traversal time at the right end is consistent with the current time, and the traversal time at the left end is determined by the window length, so that the measurement value sequence to be sensed is configured as a time series; in order to align the two end time points of the measurement value sequence to be sensed with the two ends of the sliding window, the length of the measurement value sequence to be sensed is configured to be equal to the window length of the sliding window, thereby improving the effectiveness of the sliding window in extracting the measurement value sequence to be sensed and reducing the introduction of excessive invalid data.

[0080] It should be noted that the raw data includes the acceleration measurement value and acceleration direction sensed by the accelerometer, the angular velocity measurement value and angular velocity direction sensed by the gyroscope, the left wheel rotation measurement value sensed by the left rotary encoder, and the right wheel rotation measurement value sensed by the right rotary encoder; the left wheel rotation measurement value includes the robot's left wheel rotation distance or left wheel rotation rate; the right wheel rotation measurement value includes the robot's right wheel rotation distance or right wheel rotation rate.

[0081] Preferably, the window length of the sliding window is 30, which is the optimal sequence length for the sequence of measured values ​​to be sensed, to achieve the optimal balance between the training efficiency and classification and recognition accuracy of the impact-sensing neural network, wherein the window length represents the number of samples framed by the sliding window within the time axis. The training efficiency here refers to the efficiency of training the impact-sensing neural network using the sequence of measured values ​​to be sensed in steps 1 to 6; the classification and recognition accuracy here refers to the accuracy of the classification operation of the target impact-sensing network model trained in step 6 on the sequence of measured values ​​to be sensed or the corresponding training features.

[0082] In summary, the target impact perception network model is input with a sequence length of 30 to be sensed measurement value sequence, and classification processing is performed to calculate the first predicted probability of the robot being impacted at the current moment and the second predicted probability of the robot being impacted at the current moment. Two model probability information is obtained. Compared with the existing technology, the order of magnitude of parameters required for the convolutional neural network is reduced, and the target impact perception network model occupies less storage space, forming a lightweight network model, which speeds up the detection of the robot's impact situation and can also be deployed in resource-constrained embedded systems.

[0083] Before step C uses the target impact perception network model to classify the perception measurement value sequence, step A needs to obtain the observed speed value and the observed speed direction, and obtain the predicted speed value and the predicted speed direction at the current moment based on the Kalman filter algorithm.

[0084] On the other hand, the robot has problems such as untimely processing of sensor data based on the Correlative Scan Matching (CSM) algorithm or other positioning algorithms, and information delay in the output posture data. For example, there will be delays in performing window search and matching operations through the correlation scan matching algorithm, resulting in the received measurement data often being delayed and disordered. This makes the measurement from the same target state at an earlier moment arrive at the search center later than the measurement at the current moment. This is called the delayed disordered measurement problem. When the delayed measurement data arrives, the delayed measurement data is used by the traditional Kalman system-based filtering method, causing the sensing error of the robot's inertial navigation sensor to grow rapidly. If only the current moment data is used and the delayed measurement data is directly discarded, the information of this part of the data cannot be effectively utilized, and the robot's posture cannot be effectively corrected.

[0085] Based on the above technical deficiencies, the present application discloses an embodiment. In this embodiment, the method for executing step A, as shown in FIG4 , includes:

[0086] Step A1: Based on a pre-set positioning algorithm, the robot obtains current pose observation data and its noise covariance matrix, and configures the current pose observation data to exist at the current moment. The current pose observation data is also recorded as out-of-order position observation data. Simultaneously, the timestamp of the current pose observation data is determined to be less than the current moment. The timestamp of the current pose observation data is the moment when the pre-set positioning algorithm calculates the current pose observation data, which serves as the solution moment for the current pose observation data. The current pose observation data can be understood as delayed output pose data and sensor data processed by the positioning algorithm. The raw sensor data includes inertial data collected by an inertial measurement unit and point cloud data collected by a laser sensor. After the raw sensor data is processed by the positioning algorithm, the current pose observation data is obtained. The sensor data is delayed for a period of time, but the timeline association is recorded, including the length of the delay period. Data association processing is then performed on the current pose observation data to obtain historical pose observation data. Step A2 is then executed.

[0087] In addition, the noise covariance matrix of the current posture observation data is determined by the measurement error carried in the posture data output by the positioning algorithm, and the posture data output by the positioning algorithm includes the current posture observation data; the measurement error includes the system measurement error of the inertial measurement unit, the system measurement error of the rotary encoder, the system measurement error of the laser sensor, etc., to determine the initial state of the noise covariance matrix of the current posture observation data; the positioning algorithm is preferably a correlation scan matching algorithm.

[0088] The historical posture observation data and the original data required to be collected to obtain the current posture observation data using the positioning algorithm are matched in time, forming a posteriori correlation of states at different moments; for example, if the current posture observation data is the posture observation data obtained by the positioning algorithm in the current round of matching iterative calculation, then the historical posture observation data is the posture observation data obtained by the positioning algorithm in the previous round of matching iterative calculation, and the original data required to be collected to obtain the historical posture observation data using the positioning algorithm and the original data required to be collected to obtain the current posture observation data using the positioning algorithm are matched in time, wherein the previous round of matching iterative calculation and the current round of matching iterative calculation are in two adjacent time periods; if the timestamp of the posture observation data obtained by the previous round of iterative calculation is less than the timestamp of the current posture observation data, then it is determined that the timestamp of the historical posture observation data is less than the current moment; the timestamp of the historical posture observation data can be approximated to an integer collection moment; when the timestamp j of the historical posture observation data is equal to the difference between the timestamp of the current posture observation data and the value 1, the timestamp j of the historical posture observation data can be less than the difference between the current moment i and the value 1. This is to take into account the calculation delay of the positioning algorithm and ensure the effectiveness of the Kalman filter.

[0089] Step A2: Based on the kinematic equation, calculate the current posture prediction data from the current posture observation data; and calculate the prior covariance matrix of the current posture prediction data based on the state equation; then execute step A3.

[0090] In some embodiments, before using the Kalman filter algorithm or other related algorithms to update and correct the current pose prediction data and its prior covariance matrix, the current pose prediction data calculated in step A2 can be regarded as the prediction data of the initial state, and the prior covariance matrix of the current pose prediction data calculated in step A2 can be regarded as the covariance matrix of the initial state.

[0091] Specifically, in step A2, the method for calculating the current posture prediction data from the current posture observation data based on the kinematic equation includes:

[0092] Calculate the current attitude angle: is the kinematic equation used to calculate the robot’s posture angle. i is the current angular velocity measured by the robot's gyroscope, ω b is the preset bias angular velocity, which represents the angular velocity bias value of the gyroscope installed on the robot; (ω i -ω b )·d represents the Euler angle deflected by the robot in the time interval d; q{·} is used to convert the Euler angle into a quaternion, which can be regarded as a conversion into a rotation matrix; Indicates quaternion multiplication to realize Euler transformation of the angle value of rotation around each coordinate axis, which means converting Euler angle into quaternion through exponential mapping; is the attitude angle at the observation time id, is the current attitude angle, and All are set in the form of quaternions. Use quaternion to represent the current posture angle of the robot.

[0093] Calculate the current speed: is the kinematic equation used to calculate the robot's motion speed. i is the current acceleration measured by the robot's accelerometer, a b is the preset bias acceleration, which indicates the acceleration bias value of the accelerometer installed on the robot; is the posterior rotation matrix of the observation time id, Used to i -a b ) is adjusted to be parallel to the direction of gravitational acceleration g; is the speed of observation time id, is the current speed.

[0094] Calculate the current predicted a priori displacement: is the kinematic equation used to calculate the displacement of the robot in the time interval d. is the predicted a priori displacement of the observation time id to represent the predicted position of the robot at the observation time id, is the current predicted prior displacement to represent the predicted position of the robot at the current time i.

[0095] Then, the current attitude angle Current speed and the current predicted prior displacement Composition of current pose prediction data That is, the posture data calculated from the current posture observation data based on the kinematic equation at the current moment; among them, the posture angle of the observation moment id The speed of the id at the observation time and the timestamp of the observation time id to predict the prior displacement The current pose observation data is composed of observation time id, which is the timestamp of the current pose observation data. d is the time interval between the current time i and observation time id. It should be noted that during the time interval d, the gravitational acceleration, bias acceleration, and bias angular velocity are considered constant. However, they are allowed to be updated after the robot is detected to be stationary.

[0096] The step A2 is based on the robot kinematic model, which uses the displacement, velocity and acceleration of the robot at a certain moment to establish the state space, and describes the evolution of the robot's posture state over time based on Newton's laws of motion, so as to predict the prior displacement, velocity and posture angle through the numerical integration method in the framework of the Kalman filter algorithm.

[0097] On the other hand, in step A2, the method for calculating the prior covariance matrix of the current posture prediction data based on the state equation includes: This is equivalent to the state equation required to predict the prior covariance matrix in the Kalman filter algorithm. is the prior covariance matrix of the pose prediction data at the observation time id; F ε Is the posture prediction data of the state equation relative to the observation time id The Jacobian matrix is ​​equivalent to the state transfer matrix required by the Kalman filter algorithm; the pose prediction data of the observation time id In the state equation, it can be regarded as the posterior state of the robot; F i is the Jacobian matrix of the state equation with respect to the inertial measurement unit measurement noise; Q i is the noise covariance matrix of the current pose prediction data; Q i is determined by the IMU measurement noise, Q i The initial value Q0 is determined by the measurement error of the inertial measurement unit in distance and orientation, which can form a diagonal matrix.

[0098] Step A3: Based on the delayed out-of-order observation equation, the Kalman gain matrix is ​​calculated using the noise covariance matrix of the current pose observation data and the prior covariance matrix of the current pose prediction data; then step A4 is executed.

[0099] Specifically, in step A3, the method for calculating the Kalman gain matrix includes:

[0100] is the delayed out-of-order observation equation, which is the equation for calculating the Kalman gain using out-of-order position observation data and its noise covariance matrix. is the prior covariance matrix of the current pose prediction data; U is the noise covariance matrix of the current pose observation data; the noise covariance matrix of the current pose observation data is determined by the measurement error carried in the pose data output by the positioning algorithm, which can be determined by the measurement noise of the inertial measurement unit; H is the Jacobian matrix of the delayed out-of-order observation equation relative to the current pose prediction data, which is equivalent to the observation matrix in the Kalman filter algorithm; T is the transpose of the matrix; This is equivalent to inverting the prediction variance. K i is the Kalman gain matrix; i is the current moment.

[0101] Step A4: Correct the current posture prediction data based on the residual between the current posture observation data and the historical posture observation data to update the velocity vector in the corrected current posture prediction data to the predicted velocity vector at the current moment, and update the velocity vector in the current posture observation data to the observed velocity vector, wherein the correction here can be at least one correction or a correction operation performed after at least one step A3. Correct the prior covariance matrix of the current posture prediction data in combination with the Kalman gain matrix, the prior covariance matrix of the current posture prediction data, and the noise covariance matrix of the current posture observation data. The predicted velocity value at the current moment described in step A and the predicted velocity direction at the current moment described in step A constitute the predicted velocity vector at the current moment, and the observed velocity value at step A and the observed velocity direction at step A constitute the observed velocity vector.

[0102] The method for correcting the current posture prediction data in step A4 includes: in, is the current pose observation data, and is represented by a vector; is the historical posture observation data; j is the timestamp of the historical posture observation data, j is less than i; is the current pose prediction data; is the corrected current pose prediction data, It can be expressed as To express the backward prediction state vector from time i to time id, when the robot is in a stationary state, it can be directly updated in the next moment as is the residual between the current pose observation data and the historical pose observation data.

[0103] The method for correcting the prior covariance matrix of the current posture prediction data in step A4 includes: in, is the prior covariance matrix The matrix obtained after correction is It can be expressed as To express the backward prediction covariance matrix from time i to time id, when the robot is in a stationary state, it can be directly updated in the next moment as I is the identity matrix.

[0104] It should be noted that, in step A, the execution order of steps A1 to A4 includes: when the robot's sensing device collects raw data (which can be synchronized with the raw data collection operation mentioned in step C1 or step 1) and is called by the preset positioning algorithm to output the current posture observation data, step A1 is first executed, and after obtaining the historical posture observation data, the current posture observation data and its noise covariance matrix in step A1, step A2 is executed; after calculating the current posture prediction data and its prior covariance matrix in step A2, step A3 is executed; after calculating the Kalman gain matrix in step A3, step A4 is executed; and in step A4, the predicted velocity vector and the observed velocity vector at the current moment are updated.

[0105] In summary, the aforementioned embodiment uses the calculation result of the delayed output of the preset positioning algorithm as the current posture observation data (delayed measurement data), and then corrects the robot state information based on the aforementioned steps A1 to A4, thereby fully utilizing the posture information and covariance matrix of the delayed measurement data (the aforementioned current posture observation data) at the current moment, constraining the sensor error growth of the robot's inertial measurement unit, effectively correcting the robot's posture information, and being able to provide speed trigger conditions in real time for starting the classification processing of the perception measurement value sequence using the target impact perception network model.

[0106] In some embodiments, after the robot completes a correction of the current posture prediction data and its prior covariance matrix at the current moment, it enters the next moment i+1, and there are the following implementation methods:

[0107] When the robot is in a stationary state, the predicted velocity vector at the current moment and the prior covariance matrix of the current posture prediction data can be updated by repeatedly executing steps A3 and A4 to achieve the effect of iterative correction; wherein, during the process of repeatedly executing steps A3 to A4, the current posture observation data and its noise covariance matrix are not updated corresponding to the latest output result of the positioning algorithm, and the historical posture observation data are not updated in step A; the noise covariance matrix of the current posture observation data does not need to be updated with the measurement error carried in the posture data newly output by the positioning algorithm as it enters the next moment i+1; the iterative calculation is performed in this way until the number of repeated executions reaches a preset target number of iterations; wherein the preset target number of iterations is pre-set. Then, the velocity vector in the corrected current posture prediction data is updated to the predicted velocity vector at the current moment, and the velocity vector in the current posture observation data is updated to the observed velocity vector, and it is determined that one round of iterative correction or one round of iterative update is completed.

[0108] Alternatively, within a correction period, the predicted velocity vector at the current moment and the prior covariance matrix of the current posture prediction data are updated by repeatedly executing steps A2 to A4 to achieve the effect of iterative correction. In the process of repeatedly executing steps A2 to A4, the current posture observation data needs to be updated with the corrected current posture prediction data in the last executed step A4 as it enters the next moment i+1; then the current posture prediction data and its prior covariance matrix calculated in step A2 are updated, and the Kalman gain matrix calculated in step A3 is updated. Then, in step A4, the updated current posture prediction data is corrected according to the residual between the updated current posture observation data and the updated historical posture observation data; and the prior covariance matrix of the updated current posture prediction data is corrected in combination with the updated Kalman gain matrix, the updated prior covariance matrix of the current posture prediction data and the noise covariance matrix of the updated current posture observation data; the iterative calculation is performed in this way until the number of repeated executions reaches the preset target number of iterations; wherein the preset target number of iterations is pre-set. Then, the velocity vector in the corrected current posture prediction data is updated to the predicted velocity vector at the current moment, and the velocity vector in the current posture observation data is updated to the observed velocity vector, and it is determined that a round of iterative correction or a round of iterative update is completed.

[0109] As an embodiment, the robot impact detection method further includes:

[0110] Starting from the initial moment, N sets of measurements are collected sequentially within a collection cycle. The starting point of a collection cycle is time m, and the end point is time m+N-1. If multiple collection cycles are set, N sets of measurements are collected in each cycle, forming a time series within a sliding window. A set of measurements includes acceleration, angular velocity, left wheel rotation, and right wheel rotation. If the initial moment is set to the moment the robot starts walking, the first collection cycle starts.

[0111] A weighted mean square error is calculated for N groups of measurement values ​​collected within an acquisition cycle, and then a determination is made as to whether the weighted mean square error is lower than a pre-set statistical threshold; wherein the pre-set statistical threshold is a value determined by the inventors after multiple experiments to limit a reasonable range of change in acceleration measurement values, a reasonable range of change in angular velocity measurement values, a reasonable range of change in left wheel rotation measurement values, and a reasonable range of change in right wheel rotation measurement values; the reasonable range of change is preferably the range of change reflected by the weighted mean square threshold calculated from the acceleration measurement values, angular velocity measurement values, left wheel rotation measurement values, and right wheel rotation measurement values ​​when the robot is in a stationary state, and is used to detect the degree to which the weighted mean square error deviates from the threshold, thereby determining whether the robot is in a stationary state or has started motion.

[0112] Specifically, the method for calculating the weighted mean square error of N groups of measurement values ​​collected in one collection cycle includes:

[0113] Wherein, γ is the weighted mean square error, which is the weighted mean square error calculated within the acquisition period; a k is the acceleration measurement at time k; is the average value of the N groups of acceleration measurements, used to describe the average acceleration in the time interval from time m to time m+N-1, where time k changes from an integer m to an integer m+N-1. It is suitable for calculating the weighted mean square error γ in the framework of the weighted least squares method. When m changes, the original weighted mean square error γ can be recalculated and updated. σ a is the standard deviation of the accelerometer measurement noise, σ a is pre-set; the time interval from time m to time m+N-1 is a said acquisition cycle; g is the acceleration of gravity; ω k is the angular velocity measurement at time k, σ ω is the standard deviation of the gyroscope measurement noise, σ ω It is pre-set; is the measured value of the left wheel rotation at time k, is the right wheel rotation measurement at time k, σ r is the standard deviation of the measurement noise of the left-hand or right-hand encoder, σ r It is pre-set; thus, the standard deviation of the measurement error distribution of each measurement value is taken as the weight in the process of calculating the weighted mean square error.

[0114] When it is determined that the weighted mean square error is lower than a preset statistical threshold, it is determined that the robot is in a stationary state. When the robot is in a stationary state, is to adjust the direction of gravitational acceleration g to The direction of the accelerometer is parallel to the kEqual to g, ω measured by the gyroscope k The value of is equal to 0, which makes the robot's posture constant; the right-rotating encoder measures The value is equal to 0, and the left-rotating encoder measures The size of is equal to the value 0.

[0115] In some embodiments, when the robot is determined to be in a stationary state, the robot directly uses a Kalman filter algorithm to correct the current pose prediction data and its prior covariance matrix, but does not perform steps A1 to A4. The Kalman filter algorithm here can adopt an extended Kalman filter (EKF) algorithm, wherein the initial search pose in the stationary state is used as the prior information.

[0116] In another embodiment, when it is determined that the robot is in a stationary state, the robot corrects the current posture prediction data and its prior covariance matrix by repeatedly executing steps A3 to A4, and then updates the predicted velocity vector at the current moment. The iterative calculation is performed in this way until the updated / corrected current posture prediction data falls within the posture data range under a target state during the time when the robot is in a stationary state, and then steps B to D are selected for execution.

[0117] In other embodiments, when it is determined that the robot is in a stationary state, the robot uses the average value of N groups of acceleration measurements to update the bias acceleration, and uses the average value of N groups of angular velocity measurements to update the bias angular velocity, and begins to correct the current posture prediction data and its prior covariance matrix by executing steps A2 to A4, thereby updating the predicted velocity vector at the current moment; and then selects to execute steps B to D. In the process of re-executing A2, the current posture prediction data calculated based on the above-mentioned kinematic equations will be updated, for example, the current posture angle will be updated. Update current speed And update the current predicted prior displacement Therefore, the bias acceleration and the bias angular velocity are updated respectively using the average measurement values ​​of the accelerometer and the gyroscope when the robot is stationary (the average values ​​of N groups of acceleration measurement values ​​and the average values ​​of N groups of angular velocity measurement values, respectively).

[0118] Since the wheeled sweeping robot will periodically stop moving during the planned cleaning period, the average measurement values ​​of the accelerometer and gyroscope when the robot is stationary can be used to update the bias acceleration and the bias angular velocity. The Kalman filter algorithm can also be used to correct the current posture prediction data and its prior covariance matrix to correct the drift of the inertial measurement unit, thereby realizing zero-speed detection and zero-speed correction.

[0119] When it is determined that the weighted mean square error is not lower than the preset statistical threshold, it is determined that the robot is not in a stationary state, and preferably steps A1 to A4 are not executed. Then, based on the Kalman filter algorithm, the left wheel rotation measurement value and the right wheel rotation measurement value are used to respectively calculate the left wheel predicted state data and the right wheel predicted state data, and then the left wheel predicted state data and the right wheel predicted state data are respectively corrected to obtain the corrected state and covariance matrix; thereby, adaptive adjustment of the constraint value is achieved according to the actual motion state of the robot under the framework of the Kalman filter algorithm, and the error correction value of the rotation rate or rotation distance is obtained after the filtering is completed, and the robot is The speed error is corrected to obtain the corrected three-dimensional speed, that is, the speed component of the current speed calculated in step A2 in the three coordinate axis directions after correction by the Kalman filter algorithm; the position error is then updated using the corrected three-dimensional speed to obtain the observed speed value and observed speed direction, and the predicted speed value and predicted speed direction at the current moment are obtained based on the Kalman filter algorithm, and then steps B to D are executed to introduce the impact perception neural network, construct a target impact perception network model that takes into account multiple types of measurement value sequences to be perceived, and improve the classification and recognition effect of the target impact perception network model. Therefore, the combination of the Kalman filter algorithm and the impact perception neural network complements each other's advantages, which is conducive to improving the accuracy of the robot's impact detection and solving the problem of error in the processing of sensing data of the rotary encoder on the robot's wheel; then, when the robot is detected to be impacted, the corrected three-dimensional speed prediction vector is used to correct the robot's position error to ensure the accuracy of the navigation and positioning results.

[0120] It should be noted that a rotary encoder is installed on each of the left and right wheels of the robot to measure the rotation amount of the left and right wheels, which are recorded as the left wheel rotation measurement value and the right wheel rotation measurement value respectively. Both support solution to wheel speed observation data to participate in correction within the framework of the Kalman filter algorithm.

[0121] Specifically, after determining that the robot is not in a stationary state, under the calculation framework required by the Kalman filter algorithm, the left wheel rotation measurement value can be used based on the preset wheel speed observation equation. and the right wheel rotation measurement value The left wheel observation state data and the right wheel observation state data are calculated respectively, and then the left wheel predicted state data and its prior covariance matrix are calculated from the left wheel observation state data and its prior covariance matrix based on the state equation; and the right wheel predicted state data and its prior covariance matrix are calculated from the right wheel observation state data and its prior covariance matrix based on the state equation; the left wheel predicted state data and its prior covariance matrix are calculated based on the state equation, and the right wheel predicted state data and its prior covariance matrix are calculated based on the state equation; then, by introducing the robot motion speed constraint condition, Kalman filtering correction is performed on the left wheel predicted state data and the right wheel predicted state data respectively to obtain corrected left wheel predicted state data and corrected right wheel predicted state data to form a non-complete constraint correction; then the corrected left wheel predicted state data is updated to the left wheel rotation measurement value, and the corrected right wheel predicted state data is updated to the right wheel rotation measurement value, so that: when it is detected that the robot is impacted, the robot position error is corrected using the corrected left wheel predicted state data and the corrected right wheel predicted state data, including the robot's lateral speed error.

[0122] In this embodiment, the process from calculating the left wheel observation state data and the right wheel observation state data to updating the corrected left wheel predicted state data and the corrected right wheel predicted state data is equivalent to: using the speed observation value of the rotary encoder, the inertial measurement unit / non-holonomic constraint as an auxiliary source with error input to the Kalman filter, and configuring the wheel rotation rate measured by the rotary encoder as the observation vector, and configuring the current posture prediction data as the state vector, and performing the measurement update process of the Kalman filter.

[0123] Among them, the robot movement speed constraint conditions include the speed constraint conditions required for the robot to undergo lateral displacement; the speed constraint conditions required for the robot to undergo lateral displacement include: the magnitude of the velocity component of the robot's current velocity in the direction of the robot's wheel axis (i.e., the magnitude of the lateral velocity) is not equal to the value 0, and the magnitude of the velocity component of the robot's current velocity perpendicular to the robot's walking plane is equal to the value 0; the velocity component of the robot's current velocity in the direction of the robot's wheel axis is the lateral velocity caused by the robot's lateral displacement.

[0124] After the robot motion speed constraint is introduced into the Kalman filter algorithm, the current posture prediction data can be calculated from the current posture observation data according to the kinematic equation in step A2, and the time interval between the current moment and the observation moment can be restricted according to the robot motion speed constraint and the actual motion state of the robot, for example, the observation moment can be set to the previous moment; according to the kinematic equation in step A2, the robot motion speed constraint not only generates constraints between continuous speed, attitude angle, and displacement, but also generates state increment constraints between the speed of the accelerometer and gyroscope and the preset inertial error estimate, thereby improving the posture prediction accuracy of the Kalman filter algorithm under impact.

[0125] It should be noted that the Kalman filter is insensitive to the parameters of the robot motion model (for example, the kinematic equation in step A2) and the initial values ​​of the system within a certain range, provided that the parameter settings need to ensure that the robot system can effectively "capture the target". Kalman filtering is a system that uses time series information for information fusion. Kalman filtering can weight the observation information through the Kalman gain, thereby correcting the uncertainty of the prediction. This correction of uncertainty accumulates over time. When the correction is in the direction of the true value, the posture prediction data and its prior covariance matrix will gradually converge. In the state transition model (target prior) and the observation model (sensor prior), the degree of match between the model and the actual process determines the accuracy of the final corrected posture estimation.

[0126] A chip for storing a program; characterized in that the program is used to control a robot to execute the robot impact detection method disclosed in the aforementioned embodiment, or the chip is configured to control a robot to execute the robot impact detection method disclosed in the aforementioned embodiment, specifically comprising: step A, obtaining an observed speed value and an observed speed direction, and then obtaining a predicted speed value at the current moment and a predicted speed direction at the current moment based on a Kalman filter algorithm; step B, calculating the absolute value of the difference between the observed speed value and the predicted speed value at the current moment to obtain the Euclidean distance corresponding to the current speed change; calculating the cosine value of the angle between the observed speed direction and the predicted speed direction at the current moment to obtain the angle corresponding to the current speed change. Cosine distance; Step C: If the Euclidean distance corresponding to the current speed change is greater than the preset Euclidean distance threshold, and the cosine distance corresponding to the current speed change is less than the preset cosine distance threshold, the target impact perception network model is used to classify the sequence of measurement values ​​to be perceived, and a first predicted probability that the robot is impacted at the current moment and a second predicted probability that the robot is impacted at the current moment are obtained; wherein the sequence of measurement values ​​to be perceived is data collected by a sensing device of the robot; the sensing device of the robot includes an inertial measurement unit and a rotary encoder; Step D: If the first predicted probability that the robot is impacted at the current moment is greater than the second predicted probability that the robot is impacted at the current moment, it is determined that the robot is impacted.

[0127] It should be noted that, under the control of the chip, the execution order of steps A to D includes: first executing step A, obtaining the predicted speed value and the predicted speed direction at the current moment in step A, and then executing step B; after calculating the Euclidean distance corresponding to the current speed change and the cosine distance corresponding to the current speed change in step B, executing step C; after obtaining the first predicted probability of the robot being impacted at the current moment and the second predicted probability of the robot being impacted at the current moment in step C, executing step D or further judging the relationship between the first predicted probability of the robot being impacted at the current moment and the second predicted probability of the robot being impacted at the current moment, executing step D; then, step D determines that the robot is impacted, which can be understood as the current impact.

[0128] In steps A to D executed by the chip-controlled robot, the Euclidean distance formed between the observed velocity value and the predicted velocity value at the current moment, as well as the cosine distance formed between the observed velocity direction and the predicted velocity direction at the current moment, are used as trigger detection starting points. This triggers the target impact perception network model to classify and process the data collected by the inertial measurement unit and the rotary encoder. This not only considers the necessity of the target impact perception network model's classification processing, but also integrates the prediction information of at least two types of sensor data under the Kalman filter framework to improve the reliability and accuracy of the target impact perception network model's classification. Furthermore, judgments are made solely based on the relationship between the two predicted probabilities, overcoming the problem of misjudgment caused by sudden changes in the inertial measurement unit's measurement values ​​and improving the accuracy of impact detection. The target impact perception network model, as a lightweight neural network model, reduces reliance on the chip's built-in storage resources and accelerates the chip's detection speed.

[0129] The present application also discloses a robot equipped with the aforementioned chip, enabling the robot to execute the robot impact detection method disclosed in the aforementioned embodiments. The robot's left wheel is equipped with a left-rotating encoder, and its right wheel is equipped with a right-rotating encoder. An inertial measurement unit (IMU) is installed within the robot's body, comprising an accelerometer and a gyroscope. The robot is essentially a wheeled robot equipped with two odometers, which respectively sense the body's posture and the rotation of the wheel axles on either side of the body. The gyroscope is used to measure angular velocity, and the accelerometer is used to measure acceleration. The robot's left wheel is equipped with a left-rotating encoder, which senses the left wheel's rotation measurement value; the robot's right wheel is equipped with a right-rotating encoder, which senses the right wheel's rotation measurement value. Regardless of whether the robot is stationary or not, executing the described robot impact detection method overcomes the problem of misjudgment caused by sudden changes in the IMU's measurement values, thereby improving the accuracy of impact detection.

[0130] Those skilled in the art will appreciate that the embodiments of the present application can be provided as methods, systems, or computer program products. Therefore, the present application can adopt the form of a complete hardware embodiment, a complete software embodiment, or an embodiment in combination with software and hardware. Moreover, the present application can adopt the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to magnetic disk storage, CD-ROM, optical storage, etc.) that contain computer-usable program code.

[0131] The present application is described with reference to the flowcharts and / or block diagrams of the methods, devices (systems), and computer program products according to the embodiments of the present application. It should be understood that each process and / or box in the flowchart and / or block diagram, as well as the combination of the processes and / or boxes in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device produce a device for implementing the functions specified in one or more processes in the flowchart and / or one or more boxes in the block diagram.

[0132] These computer program instructions may also be stored in a computer-readable memory that can direct a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer-readable memory produce a product including an instruction device that implements the functions specified in one or more processes in the flowchart and / or one or more boxes in the block diagram.

[0133] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, so that the instructions executed on the computer or other programmable device provide steps for implementing the functions specified in one or more processes in the flowchart and / or one or more boxes in the block diagram.

[0134] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit the present invention. Although the present invention has been described in detail with reference to preferred embodiments, ordinary technicians in the field should understand that the specific implementation methods of the present invention can still be modified or some technical features can be replaced by equivalents without departing from the spirit of the technical solutions of the present invention. They should all be included in the scope of the technical solutions claimed for protection by the present invention.

Claims

1. A method for detecting impact of a robot, characterized in that: The robot impact detection methods include: Step A: The robot obtains the observed speed value and the observed speed direction, and then obtains the predicted speed value and the predicted speed direction at the current moment based on the Kalman filter algorithm; then executes step B; Step B, the robot calculates the absolute value of the difference between the observed speed value and the predicted speed value at the current moment, and obtains the Euclidean distance corresponding to the current speed change; calculates the cosine value of the angle between the observed speed direction and the predicted speed direction at the current moment, and obtains the cosine distance corresponding to the current speed change; and then executes step C; Step C: if the Euclidean distance corresponding to the current speed change is greater than the preset Euclidean distance threshold, and the cosine distance corresponding to the current speed change is less than the preset cosine distance threshold, the target impact perception network model is used to classify the perception measurement value sequence to obtain a first predicted probability of the robot being impacted at the current moment and a second predicted probability of the robot being impacted at the current moment; then step D is executed; The sequence of measured values ​​to be sensed is data collected by a sensing device of the robot; the sensing device of the robot includes an inertial measurement unit and a rotary encoder; Step D: if the first predicted probability that the robot is impacted at the current moment is greater than the second predicted probability that the robot is impacted at the current moment, then it is determined that the robot is impacted; Among them, the first predicted probability that the robot is impacted at the current moment is the probability that the measured value sequence to be sensed is classified as the measured value sequence extracted when the robot is currently in the state of being impacted; the second predicted probability that the robot is impacted at the current moment is the probability that the measured value sequence to be sensed is classified as the measured value sequence extracted when the robot is currently in the state of not being impacted.

2. The robot impact detection method according to claim 1, characterized in that: In the step C, the method of using the target impact perception network model to classify the perception measurement value sequence to obtain the first predicted probability of the robot being impacted at the current moment and the second predicted probability of the robot being impacted at the current moment includes: Step C1, extracting a sequence of measured values ​​to be sensed from raw data collected by a sensing device of the robot using a sliding window; Step C2, inputting the to-be-perceived measurement value sequence into the target impact perception network model to generate input features; Step C3, controlling the input features to perform convolution operations in multiple convolution layers in sequence to obtain discriminant features; and then controlling the discriminant features to perform average pooling processing in a pooling layer to obtain target classification features; wherein the target impact perception network model includes multiple convolution layers and one pooling layer; Step C4: classify the target classification features using a softmax algorithm to obtain a first predicted probability that the robot is impacted at the current moment and a second predicted probability that the robot is impacted at the current moment.

3. The robot impact detection method according to claim 2, characterized in that: The softmax algorithm is configured as a softmax classifier in the target impact perception network model; The softmax classifier is used to calculate a first predicted probability that the robot is impacted at the current moment and a second predicted probability that the robot is impacted at the current moment.

4. The robot impact detection method according to claim 1, characterized in that: Before executing the step C, constructing the target impact perception network model; The method for constructing the target impact perception network model includes: Step 1: extracting a sequence of measured values ​​to be sensed from the raw data collected by the robot's sensing device using a sliding window; Step 2: Connect the sequence of measured values ​​to be sensed along the channels of the corresponding dimensions to generate input features; Step 3: Control the input features to perform multi-layer convolution operations and one-layer pooling operations in a pre-constructed impact-aware neural network to obtain features to be classified; wherein the pre-constructed impact-aware neural network includes multiple convolution layers and one pooling layer; Step 4: Classify the features to be classified in step 3 using a softmax algorithm to obtain a first predicted probability of being impacted and a second predicted probability of being impacted; Step 5: Based on the loss function, the loss value is calculated using the first predicted probability of being impacted and the second predicted probability of being impacted; and the weight of the impact-aware neural network is updated, and then the impact-aware neural network after the weight update is updated as the impact-aware neural network; wherein the loss function is a binary cross entropy, and the first predicted probability of being impacted and the second predicted probability of being impacted are respectively the prediction results of two classifications in the loss function; the weight to be updated in the current step 5 is the weight of the convolutional layer; Step 6, repeating steps 2 to 5 until the most recently calculated loss value is less than a preset loss threshold, marking the shock-perceiving neural network after the weight update obtained in the most recently executed step 5 as the target shock-perceiving network model, and determining that the shock-perceiving neural network after the weight update obtained in the most recently executed step 5 is a trained shock-perceiving neural network.

5. The robot impact detection method according to claim 4, characterized in that: In the process of repeatedly executing steps 2 to 5, each time step 5 is executed, before using the loss function to calculate the loss value, it is determined whether the first predicted probability of being impacted obtained in step 4 is greater than the second predicted probability of being impacted obtained in step 4. If so, the true value of the label required by the loss function is set to 1; otherwise, the true value of the label required by the loss function is set to 0; The first impact prediction probability is the probability that the measured value sequence to be sensed or the feature to be classified is classified as the measured value sequence extracted when the robot is in a state of being impacted, so as to form the probability of the first classification generated by the softmax algorithm; the second impact prediction probability is the probability that the measured value sequence to be sensed or the feature to be classified is classified as the measured value sequence extracted when the robot is not in a state of being impacted, so as to form the probability of the second classification generated by the softmax algorithm. The probability of classification.

6. The robot impact detection method according to claim 5, characterized in that: The loss function includes: L=-(p gtp log(p p )+(1-p gtp )log(p np )), where L is the loss value; p gtp is the true value of the label, p gtp =1, it means that the current prediction result is that the robot is impacted; p is the predicted probability of the first impact, p np is the predicted probability of the second shock; The loss function is binary cross entropy, so as to design the shock-aware neural network as a binary classification network.

7. The robot impact detection method according to claim 4, characterized in that: The construction method of the shock perception neural network includes: Connecting four one-dimensional convolutional layers in sequence to form four continuously stacked convolutional layers, and then connecting one pooling layer, configuring the kernel length of each one-dimensional convolutional layer to be 3, and configuring the pooling operation in the pooling layer to be an average operation, so as to construct the shock-aware neural network; The original value of the weight of the convolution layer is randomly generated; the input features and the weight of the convolution layer are configured in the convolution layer to perform a convolution operation.

8. The robot impact detection method according to claim 7, characterized in that: The operation method of step 3 includes: Controlling the input features to perform convolution operations in the four one-dimensional convolution layers in sequence to obtain discriminant features, and determining to complete multi-layer convolution operations in the impact-aware neural network; and then inputting the discriminant features into the pooling layer; Then, the discriminant features are controlled to be average pooled in the pooling layer to obtain features to be classified, and the pooling operation is determined to be completed in the impact-aware neural network.

9. The robot impact detection method according to claim 2, characterized in that: The method for generating the input features includes: According to the type of data collected by the sensing device of the robot, connecting the various types of measured value sequences to be sensed along the channels of the corresponding dimensions to synthesize the input features; The inertial measurement unit includes an accelerometer for sensing acceleration measurement values ​​and a gyroscope for sensing angular velocity measurement values, and the rotary encoder includes a left rotary encoder for sensing left wheel rotation measurement values ​​and a right rotary encoder for sensing right wheel rotation measurement values; The measurement value sequence to be sensed is a 3-axis angular velocity measurement value sequence, a 3-axis acceleration measurement value sequence, a 1-axis left wheel rotation measurement value sequence or a 1-axis right wheel rotation measurement value sequence, so that the dimension of the input feature is greater than or equal to 8; wherein The input features are set in the form of a matrix.

10. The robot impact detection method according to claim 9, characterized in that: The target impact perception network model includes four 1-dimensional convolutional layers; The four 1-dimensional convolutional layers are sequentially represented as a first convolutional layer, a second convolutional layer, a third convolutional layer and a fourth convolutional layer; The depth of the input layer in the first convolutional layer is 8, and the depth of the input layer in the first convolutional layer is equal to the number of column elements of the input feature; The depth of the output layer in the first convolutional layer is equal to the depth of the input layer in the second convolutional layer, the depth of the output layer in the second convolutional layer is equal to the depth of the input layer in the third convolutional layer, and the depth of the output layer in the third convolutional layer is equal to the depth of the input layer in the fourth convolutional layer, so that the first convolutional layer, the second convolutional layer, the third convolutional layer and the fourth convolutional layer are connected in sequence; wherein a ratio of a depth of an input layer in the first convolutional layer to a depth of an output layer in the first convolutional layer is smaller than a ratio of a depth of an input layer in the second convolutional layer to a depth of an output layer in the second convolutional layer; The ratio of the depth of the input layer in the second convolutional layer to the depth of the output layer in the second convolutional layer is smaller than the ratio of the depth of the input layer in the third convolutional layer to the depth of the output layer in the third convolutional layer; the ratio of the depth of the input layer in the third convolutional layer to the depth of the output layer in the third convolutional layer is smaller than the ratio of the depth of the input layer in the fourth convolutional layer to the depth of the output layer in the fourth convolutional layer.

11. The robot impact detection method according to claim 9, characterized in that: The sliding window keeps the window length unchanged during the process of traversing the original data; In the sliding window, the right end traversal time is consistent with the current time, and the left end traversal time is determined by the window length, so that the sequence of measured values ​​to be sensed is configured as a time series; the length of the sequence of measured values ​​to be sensed is equal to the window length of the sliding window; The raw data includes acceleration sensed by an accelerometer, angular velocity sensed by a gyroscope, a left wheel rotation measurement value sensed by a left rotary encoder, and a right wheel rotation measurement value sensed by a right rotary encoder; The left wheel rotation measurement value includes the robot's left wheel rotation distance or left wheel rotation rate; The right wheel rotation measurement value includes the right wheel rotation distance or the right wheel rotation rate of the robot.

12. The robot impact detection method according to claim 11, characterized in that: The window length of the sliding window is a value of 30.

13. The robot impact detection method according to claim 9, characterized in that: The execution method of step A includes: Step A1, based on a preset positioning algorithm, the robot obtains current posture observation data and its noise covariance matrix, and configures the current posture observation data to exist at the current moment, and at the same time, determines that the timestamp of the current posture observation data is less than the current moment, wherein the timestamp of the current posture observation data is the moment when the positioning algorithm outputs the current posture observation data; then the current posture observation data is associated with each other to obtain historical posture observation data; wherein the original data required to be collected using the positioning algorithm to obtain historical posture observation data matches the original data required to be collected using the positioning algorithm to obtain current posture observation data in time; the noise covariance matrix of the current posture observation data is determined by the measurement error carried in the posture data output by the positioning algorithm; Step A2: Based on the kinematic equation, current posture prediction data is calculated from the current posture observation data; and the prior covariance matrix of the current posture prediction data is calculated based on the state equation; Step A3: Based on the delayed out-of-order observation equation, the Kalman gain matrix is ​​calculated using the noise covariance matrix of the current posture observation data and the prior covariance matrix of the current posture prediction data; Step A4, according to the residual between the current posture observation data and the historical posture observation data, the current posture prediction data is corrected to update the velocity vector in the corrected current posture prediction data to the predicted velocity vector at the current moment, and the velocity vector in the current posture observation data is updated to the observed velocity vector; and the prior covariance matrix of the current posture prediction data is corrected in combination with the Kalman gain matrix, the prior covariance matrix of the current posture prediction data and the noise covariance matrix of the current posture observation data; The predicted speed value at the current moment and the predicted speed direction at the current moment constitute the predicted speed vector at the current moment, and the observed speed value and the observed speed direction constitute the observed speed vector.

14. The robot impact detection method according to claim 13, characterized in that: In step A3, the method for calculating the Kalman gain matrix includes: in, is the prior covariance matrix of the current pose prediction data; U is the noise covariance matrix of the current pose observation data; H is the Jacobian matrix of the delayed out-of-order observation equation relative to the current pose prediction data; T is the transpose of the matrix H; K i is the Kalman gain matrix; i is the current moment.

15. The robot impact detection method according to claim 14, characterized in that: The method for correcting the current posture prediction data in step A4 includes: in, is the current posture observation data; is the historical posture observation data, j is the timestamp of the historical posture observation data, and j is less than i; is the current posture prediction data; is the corrected current pose prediction data; is the residual between the current pose observation data and the historical pose observation data; The method for correcting the prior covariance matrix of the current posture prediction data in step A4 includes: in, is the prior covariance matrix The matrix obtained after correction forms the covariance matrix of the target state at the current moment; I is the unit matrix.

16. The robot impact detection method according to claim 13, characterized in that: In step A2, the method for calculating the current posture prediction data from the current posture observation data based on the kinematic equation includes: Calculate the current attitude angle: Among them, ω i is the current angular velocity measured by the robot’s gyroscope, ω b is the preset bias angular velocity; q{·} is used to convert the Euler angle into a quaternion. Represents quaternion multiplication; is the attitude angle at the observation time id, is the current attitude angle, and All are set in the form of quaternion; Calculate the current speed: Among them, a i is the current acceleration measured by the robot's accelerometer, a b is the preset bias acceleration; is the posterior rotation matrix of the observation time id; is the speed of observation time id, is the current speed; Calculate the current predicted a priori displacement: in, is the predicted prior displacement at the observation time id, is the current predicted a priori displacement; Then, the current attitude angle Current speed and the current predicted prior displacement Composing the current posture prediction data Among them, the attitude angle of observation time id The speed of the observation time id and the timestamp of the observation time id to predict the prior displacement Constituting the current posture observation data; Among them, observation time id is the timestamp of the current posture observation data, and d is the time interval between the current time i and the observation time id.

17. The robot impact detection method according to claim 16, characterized in that: In step A2, the method for calculating the prior covariance matrix of the current posture prediction data based on the state equation includes: in, is the prior covariance matrix of the current pose prediction data; is the prior covariance matrix of the pose prediction data of the observation time id; F ε It is the posture prediction data of the state equation relative to the observation time id The Jacobian matrix of i is the Jacobian matrix of the state equation with respect to the IMU measurement noise; Q i is the noise covariance matrix of the current pose prediction data; Q i The initial value of is determined by the IMU measurement noise.

18. The robot impact detection method according to claim 16, characterized in that: The robot impact detection method also includes: Starting from the initial moment, N groups of measurement values ​​are collected in sequence in one collection cycle, wherein the timing starting point of one collection cycle is moment m, and the timing end point of the same collection cycle is moment m+N-1; one group of measurement values ​​includes acceleration measurement values, angular velocity measurement values, left wheel rotation measurement values, and right wheel rotation measurement values; Calculating a weighted mean square error for the N groups of measurement values ​​collected within one collection cycle, and then determining whether the weighted mean square error is lower than a preset statistical threshold; When it is determined that the weighted mean square error is lower than a preset statistical threshold, the robot is determined to be in a stationary state; then the average value of the N groups of acceleration measurement values ​​is used to update the bias acceleration, and the average value of the N groups of angular velocity measurement values ​​is used to update the bias angular velocity; When it is determined that the weighted mean square error is not lower than a preset statistical threshold, it is determined that the robot is not in a stationary state; then based on the Kalman filter algorithm, the left wheel rotation measurement value and the right wheel rotation measurement value are used to respectively calculate the left wheel predicted state data and the right wheel predicted state data; then the left wheel predicted state data and the right wheel predicted state data are respectively corrected.

19. The robot impact detection method according to claim 18, characterized in that: The method for calculating the weighted mean square error of N groups of measurement values ​​collected in one collection cycle includes: Among them, γ is the weighted mean square error; a k is the acceleration measurement at time k, is the average value of the N groups of acceleration measurements, σ a is the standard deviation of the accelerometer measurement noise, σ a is a preset value; g is the acceleration due to gravity; ω k is the angular velocity measurement at time k, σ ω is the standard deviation of the gyroscope measurement noise, σ ω It is pre-set; is the measured value of the left wheel rotation at time k, is the measured value of the right wheel rotation at time k, σ r is the standard deviation of the measurement noise of the left-hand or right-hand encoder, σ r It is preset.

20. The robot impact detection method according to claim 18, characterized in that: After determining that the robot is not in a stationary state, under the calculation framework required by the Kalman filter algorithm, the left wheel predicted state data and its prior covariance matrix are calculated based on the state equation, and the right wheel predicted state data and its prior covariance matrix are calculated based on the state equation; then, by introducing the robot motion speed constraint condition, Kalman filtering is performed on the left wheel predicted state data and the right wheel predicted state data respectively to obtain the corrected left wheel predicted state data and the corrected right wheel predicted state data; then, the corrected left wheel predicted state data is updated to the left wheel rotation measurement value, and the corrected right wheel predicted state data is updated to the right wheel rotation measurement value; Among them, the robot motion speed constraint conditions include the speed constraint conditions required for the robot to undergo lateral displacement; Among them, the speed constraint conditions required for the robot to undergo lateral displacement include: the speed component of the robot's current speed in the direction of the robot's wheel axis is not equal to 0, and the speed component of the robot's current speed perpendicular to the robot's walking plane is equal to 0.

21. A chip for storing a program; characterized in that: The program is used to control the robot to execute the robot impact detection method according to any one of claims 1 to 20.

22. A robot, characterized in that: The robot is equipped with the chip described in claim 21; wherein, a left-rotating encoder is installed on the left wheel of the robot, and a right-rotating encoder is installed on the right wheel of the robot; an inertial measurement unit is installed in the body of the robot, and the inertial measurement unit includes an accelerometer and a gyroscope.

Images