A robot impact detection method, chip and robot

Abstract

The present application discloses a method, a chip and a robot for detecting impact on a robot. The method for detecting impact on a robot comprises: obtaining an observed speed value and an observed speed direction, obtaining a predicted speed value at a current moment and a predicted speed direction at a current moment; calculating the absolute value of the difference between the observed speed value and the predicted speed value 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 to obtain the 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 less than the preset cosine distance threshold, performing classification processing using a target impact perception network model to obtain a first predicted probability and a second predicted probability that the robot is impacted at the current moment; 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.

Description

Technical Field

[0001] The present application relates to the field of robot control algorithms, and more particularly to a robot impact detection method, a chip, and a 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 laser radar-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 raster map as the final pose. However, the accuracy of this algorithm is heavily dependent on the confidence of the initial search pose.

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

[0004] However, in a complex dynamic environment, when a robot moves on a low-friction surface, if the robot's tire adhesion is low, it will easily slip. Robot slip generally refers to the phenomenon that the robot's wheels undergo lateral displacement (i.e., axial displacement), which is considered as the robot being impacted. The robot is instantly moved to another position without the sensor's perception, that is, the robot is instantly moved to another position without prior information. In the extended Kalman filter (EKF) framework, the sensing data based on the rotary encoder on the robot's wheel will process incorrect posture information (calculate incorrect state estimation results), affecting the robot's detection of its impact.

[0005] On the other hand, the robot is not impacted in three scenarios: (1) the robot moves on a smooth surface; (2) the robot actively hits an obstacle; (3) the robot passes over 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, then 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, the situation where the robot is not impacted is excluded, 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 robot's impact situation; 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 of impact. Summary of the invention

[0007] The present application discloses a robot impact detection method, a chip and a robot. The specific technical solution is as follows:

[0008] A method for detecting impact on a robot comprises: 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, and obtaining 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, and obtaining the 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, using a target impact perception network model to classify a sequence of sensed measurement values, and obtaining 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; wherein the target impact perception network model is a trained impact perception neural network; wherein the sequence of sensed measurement values ​​is derived from data collected by a sensing device of the robot; the sensing device of the robot comprises 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 determining that the robot is impacted.

[0009] 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 the trigger detection starting point, 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.

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

[0011] A robot, wherein the chip is built-in; 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

[0012] Figure 1 The present invention discloses a flow chart of a method for detecting impact on a robot according to an embodiment of the present application.

[0013] Figure 2 Another embodiment of the present application discloses a method flow chart of classifying a sequence of perceived measurement values ​​using a target impact perception network model.

[0014] Figure 3 It is a flowchart of a method for constructing a target impact perception network model disclosed in another embodiment of the present application.

[0015] Figure 4 It 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

[0016] In order to make the purpose, technical scheme and advantages of the present invention clearer, the present invention will be described and illustrated in conjunction with 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 used to limit the present invention. In addition, it can also be understood that for ordinary technicians in the field, some changes such as design, manufacturing or production on the technical content disclosed in the present invention are just conventional technical means, and should not be understood as the content disclosed in this application is insufficient.

[0017] 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 kind of", "the" and the like involved in this application do not represent quantitative restrictions and may 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 comprising 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 for objects.

[0018] In the process of simultaneous localization and mapping (SLAM), wheeled robots use the Correlative Scan Matching (CSM) algorithm to construct a laser point cloud map. The accuracy of this algorithm depends on the confidence of the initial search pose to predict the initial search pose of the prior information, and then the pose of each position node occupied by the robot in its motion trajectory is calculated by fusing the inertial measurement unit (IMU) and the rotary encoder using the standard extended Kalman filter (EKF) framework. However, wheeled robots are very susceptible to side slipping due to impact, 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 observation information of the rotary encoder will obtain an erroneous state estimation result, that is, it cannot reach or approach the filtering accuracy of the measurement sequence processing. In addition, in a scenario where the robot actively collides with an obstacle or crosses an obstacle and body vibration occurs, 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, then when the Mahalanobis distance between two batches of acceleration measurement value samples is used for threshold judgment, it will be determined in advance that the robot is not impacted, resulting in a misjudgment.

[0019] In view of the above problems, the present application discloses a method for detecting impact on a robot. The robot is implemented by a robot with an inertial measurement unit installed on the body and rotary encoders installed on the left and right wheels. In essence, it is a wheeled robot with two odometers installed. The two odometers respectively sense the posture state of the body and the rotation state of the wheel axles on both sides of the body. Figure 1As shown, the robot impact detection method includes:

[0020] Step A, obtaining the observed speed value and the observed speed direction, and then obtaining the predicted speed value and the predicted speed direction at the current moment based on the Kalman filter algorithm, and then executing step B. Among them, the predicted speed value at the current moment and the predicted speed direction at the current moment can constitute the predicted speed vector at the current moment; the observed speed value and the observed speed direction can constitute the observed speed vector, and are output by a preset correlation scan matching algorithm, but due to the operation delay of the correlation scan matching algorithm, the generation timestamp of the observed speed vector is not necessarily the current moment, for example, the generation timestamp of the observed speed vector is delayed for a period of time to obtain the current moment. Specifically, the observed speed value and the observed speed direction may be speed feature information obtained by searching and matching 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 may 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).

[0021] Step B, calculate the absolute value of the difference between the observed speed value and the predicted speed value at the current moment, and obtain the Euclidean distance corresponding to the current speed change, so as to judge the size 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, and obtain the cosine distance corresponding to the current speed change, so as to judge the size between the cosine distance corresponding to the current speed change and the preset cosine distance threshold; then execute step C.

[0022] 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 the cosine distance (also called cosine similarity) between the difference observed speed in the speed direction 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.

[0023] 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 difference in size and direction between the predicted speed vector and the observed speed vector widens; conversely, the predicted speed vector is closer to the observed speed vector.

[0024] 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 sequence of measured values ​​to be sensed to obtain a model probability that can represent an impacted state or a model probability that is not impacted.

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

[0026] It is worth noting that this embodiment chooses to classify the sequence of perceived measurement values ​​using the target impact perception network model 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 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.

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

[0028] It should be noted that the non-impact state can be divided into three forms during the actual walking process of the robot, including the robot walking on a plane without obstacles; the robot colliding with obstacles; the robot climbing over low obstacles and experiencing body vibration, which can be used as negative samples to participate in the training of the impact perception network model. Therefore, it is necessary to use the target impact perception network model to classify the sequence of measured values ​​to be sensed formed in various states. Of course, the initial impact perception network model / impact perception network also needs to be trained in advance using the sequence of measured values ​​to be sensed to improve the classification extraction accuracy.

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

[0030] 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, 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 perceived measurement value sequence, then 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, and 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 are all from the same target impact perception network model.

[0031] 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, which 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.

[0032] 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 determine 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, and then execute step D; then step D determines that the robot is impacted, which can be understood as currently being impacted.

[0033] In the step C, the method of using the target impact perception network model to classify the perceived 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 is as follows: Figure 2 As shown, including:

[0034] Step C1, extracting the sequence of measurement values ​​to be sensed from the raw data collected by the sensing device of the robot using a sliding window. Then execute step C2. The extraction of the sequence of measurement values ​​to be sensed is obtained by orderly scanning using the sliding window technology, generally by sliding extraction along the robot motion time axis, and obtaining multiple sequences of measurement values ​​to be sensed in sequence, wherein a 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 of a sensor type; specifically, in the process of sliding the sliding window through the raw data, multiple sequences of measurement values ​​to be sensed are extracted in sequence according to the order of collection. Among them, 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) in 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.

[0035] For example, starting from the initial moment, the robot measures and records the raw data through the sensing device every other 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. It can also be understood that: within the current traversal area, the sliding window can extract the same type of measurement values ​​to be sensed of the preset number of frames that are most recently cached by the sensing device from the raw data to form a sequence of measurement values ​​to be sensed.

[0036] Preferably, the sequence of measured values ​​to be sensed and the raw data both include vector magnitude and vector direction angle; the robot's sensing device includes an inertial measurement unit and a rotary encoder (equivalent to a wheel odometer), both of which are robot proprioceptive sensors. The sequence of measured values ​​to be sensed is an angular velocity measurement value sequence rotating around each coordinate axis direction, an acceleration measurement value sequence pointing to each coordinate axis direction, a left wheel rotation measurement value sequence, or a right wheel rotation measurement value sequence. 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.

[0037] Step C2, input the measurement value sequence 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 measurement value sequence to be sensed generates input features in the connection layer. Specifically, the robot connects all types of measurement value sequences to be sensed currently extracted by the sliding window along the channels of the corresponding dimensions, respectively, and merges them into the input features to input into the target impact perception network model. 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, and 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 sequentially connected to the same channel 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; preferably, the length of a time period is the window length of a sliding window, as the length of a measurement value sequence to be sensed.

[0038] 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 at least four dimensions of channels 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 realizes the sample set marked as recognized by the target impact perception network model.

[0039] Step C3, controlling the input features to perform convolution operations in multiple convolution layers in sequence, which is equivalent to controlling the input features to be encoded in the continuously stacked multi-layer convolution layers, and obtaining discriminant features, which are the encoding results output by the last convolution layer (traversing layer by layer in the continuously stacked multiple convolution layers) or the last convolution layer (traversing from the direction in which the multiple convolution layers are connected in sequence), and exist in the form of a matrix; and determining to complete the multi-layer convolution operation 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 the target classification features, that is, the target classification features are calculated by integrating the feature information of multiple convolution layers in the target impact perception network model and the feature information of a pooling layer, and the target classification features are equivalent to the target state of the features to be classified, and it is determined to complete the pooling operation in the target impact perception network model; then step C4 is executed. Among them, the target impact perception network model includes multiple convolution layers and a pooling layer.

[0040] 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, determine that multi-layer convolution operations are completed in the target impact perception network model, and determine 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 sequentially represented as a first convolutional layer (regarded as the first convolutional layer in the target impact perception network model), a second convolutional layer (regarded as the second convolutional layer in the target impact perception network model), a third convolutional layer (regarded as the third convolutional layer in the target impact perception network model) and a fourth convolutional layer (regarded 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-coding results of the corresponding output layers; the fourth convolutional layer outputs the encoding result as the discriminant feature in the form of a matrix. Then the discriminant feature is input into the pooling layer, and then the discriminant feature is controlled to perform average pooling processing in the pooling layer to obtain the feature to be classified. From the perspective of mathematical operation, it is understood that the average value of each element in the matrix where the discriminant feature is located is obtained to obtain the feature to be classified, and it is determined to complete the pooling operation in the target impact perception network model, and it is determined to complete the pooling operation by integrating 4 convolution layers and pooling layers. In this way, the convolution results in 4 1-dimensional convolution layers and the average value taken by 1 pooling layer are integrated into the feature to be classified, and the order of magnitude of the parameters of the target impact perception network model is also reduced.

[0041] Step C4, using the softmax algorithm to classify the target classification feature, 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, so that the target classification feature is connected to two nodes; the classification process here includes probability prediction for each classification situation, each classification situation is the situation where the measurement value sequence to be sensed or the feature to be classified is divided into a corresponding classification by the softmax algorithm; then execute step D disclosed in the above embodiment to make a final judgment on whether the robot is impacted at the current moment, rather than a preliminary judgment. In this embodiment, the first predicted probability of the robot being impacted at the current moment is the probability that the measurement value sequence to be sensed is classified as the measurement value sequence extracted when the robot is currently in a state of being impacted; the second predicted probability of the robot being impacted at the current moment is the probability that the measurement value sequence to be sensed is classified as the measurement value sequence extracted when the robot is currently not in a state of being impacted.

[0042] 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 connected to it include a first node (indicating 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 (indicating the position point of the robot not being impacted, which is a negative sample required for training the target impact perception network model). In this embodiment, 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, so as to form the probability of the first classification generated by the softmax classifier, and also the probability that the current position of the robot falls into the first node; 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 not in the state of being impacted, so as to form the probability of the second classification generated by the softmax classifier, and also the probability that the current position of the robot falls into the second node. This improves the feature extraction capability of the target impact perception network model and reduces misjudgment.

[0043] 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 robot impact detection efficiency, computing resources, and robot impact detection accuracy of the target impact perception network model or impact perception neural network.

[0044] When the robot is climbing over low obstacles such as chair legs and table legs, the robot does not experience impact but body vibration. In the time series of sensor data collected by the robot, the acceleration (sensed by the inertial measurement unit) and the angular velocity change greatly 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 a significant change in the sequence of measured values ​​to be sensed in the associated dimension, which is prone to misjudgment. In order to detect the situation that the robot is impacted in the scene where the robot is climbing over low obstacles such as chair legs and table legs and the scene where the robot is impacted and lateral displacement occurs, 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 obtains 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.

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

[0046] 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, first execute step C1, and after extracting the measurement value sequence to be sensed in step C1, execute step C2, wherein the moment when the robot's sensing device collects raw data may be earlier than or synchronized with the moment when the observed speed value and the observed speed direction are obtained in step A; after generating the input feature in step C2, execute step C3; after obtaining the target classification feature in step C3, execute step C4; 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 C4, execute step D.

[0047] Preferably, before executing the aforementioned step D, it also 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 measured values ​​to be sensed 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 measured 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.

[0048] 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 binary classification network. Compared with the prior art, the convolutional neural network used in this embodiment is simplified to form a lightweight neural network model.

[0049] In step C5, the method of updating the model parameters of the target impact perception network model based on the loss function includes using the Adam algorithm to update the weights, that is, using the gradient descent algorithm to update the weight matrix, and then gradually iterating and solving by gradient descent, continuously updating the target impact perception network model after the weight update to the target impact perception network model, so as to obtain the minimized loss value, and realize the deviation correction of the perceived measurement value sequence and the weight. Thereby improving the feature extraction accuracy of the target impact perception network model, and further improving the feasibility of using the target impact perception network model to detect whether the robot is impacted.

[0050] 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 is as follows: Figure 3 As shown, including:

[0051] Step 1, extract the measurement value sequence to be sensed from the raw data collected by the sensing device of the robot using a sliding window, and then execute step 2. The extraction in step 1 is obtained by orderly scanning using the sliding window technology, generally sliding extraction along the robot motion time axis, and sequentially obtaining multiple measurement value sequences to be sensed, wherein a measurement value sequence to be sensed scanned by the sliding window is a type of measurement value sequence to be sensed, representing a measurement value sequence to be sensed of a sensor type; specifically, in the process of sliding the sliding window in the raw data, multiple measurement value sequences to be sensed are sequentially extracted according to the order of acquisition. Among them, 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) in 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.

[0052] For example, starting from the initial moment, the robot measures and records the raw data through the sensing device every other collection cycle, that is, collects the raw data; in this collection 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.

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

[0054] Preferably, the sequence of measured values ​​to be sensed and the raw data both include vector magnitude and vector direction angle; the robot's sensing device includes an inertial measurement unit and a rotary encoder (equivalent to a wheel odometer), both of which are robot proprioceptive sensors. The sequence of measured values ​​to be sensed is an angular velocity measurement value sequence rotating around each coordinate axis direction, an acceleration measurement value sequence pointing to each coordinate axis direction, a left wheel rotation measurement value sequence, or a right wheel rotation measurement value sequence. 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.

[0055] Step 2, connect the measurement value sequence to be sensed along the channel of the corresponding dimension to generate input features; then execute step 3. In some embodiments, the robot connects all types of measurement value sequences 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 sensing neural network. 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 measurements 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 sequentially connected to the same channel 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; 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.

[0056] Step 3, control the input features to perform multiple layers of 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. 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, which is equivalent to multiplying each element in the input features of the corresponding convolution layer by the weight of the same convolution layer in mathematical operations 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.

[0057] Preferably, the pre-constructed impact-perceiving neural network is constructed according to the construction method of a convolutional neural network. The impact-perceiving neural network includes multiple convolutional layers and a pooling layer. The multiple convolutional layers are connected in sequence, and the output layer and the input layer of two adjacent convolutional layers are aligned and connected (the depth between the connected output layer and input layer is equal). The impact-perceiving neural network constructed in this embodiment belongs to a convolutional neural network; the impact-perceiving neural network also includes a connection layer. Before all the measurement value sequences to be sensed are input into the impact-perceiving neural network, the measurement value sequences to be sensed from all dimensions of the inertial measurement unit and the measurement value sequences to be sensed from all dimensions of the rotary encoder are connected along the channels of the corresponding dimensions to generate input features, which can constitute a connection layer in the convolutional neural network. After the input features are generated, it can be understood as inputting the sequence of measured values ​​to be sensed into the impact-perceiving neural network, and it can be understood as inputting the labeled samples to be classified into the first convolutional layer through the corresponding dimensional channels, so as to adapt 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 convolutional layers stacked and connected.

[0058] 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 impact perception neural network, it is necessary to design at least four dimensions of channels in the convolutional neural network and connect the measurement value sequence to be sensed. At this time, the measurement value sequence to be sensed after connecting the channels of the 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 realizes the sample set marked as recognized by the convolutional neural network. Further, if the angular velocity measurement value sequence includes an angular velocity of one dimension obtained by rotating around the yaw angle direction of the robot, an angular velocity of one dimension obtained by rotating around the roll angle direction and an angular velocity of one dimension obtained by rotating around the pitch angle direction, the dimension of the channel designed in the impact perception neural network will increase, and the dimension of the input feature will also increase.

[0059] 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-coding results of the corresponding output layers; the fourth convolutional layer outputs the encoding result as the discriminant feature in the form of a matrix. Then the discriminant feature is input into the pooling layer, and then the discriminant feature is controlled to perform average pooling processing in the pooling layer to obtain the feature to be classified. From the perspective of mathematical operation, it is understood that the average value of each element in the matrix where the discriminant feature is located is obtained to obtain the feature to be classified, and it is determined to complete the pooling operation in the impact-aware neural network, and it is determined to complete the pooling operation by integrating 4 convolution layers and pooling layers. In this way, the convolution results in 4 1-dimensional convolution layers and the average value taken by 1 pooling layer are integrated into the feature to be classified, and the order of magnitude of the neural network parameters is also reduced.

[0060] Preferably, this embodiment selects the appropriate number of convolutional layers and pooling layers, the size of the convolutional layer filter, the moving step of the convolutional layer filter and the pooling operation mode according to the dimension of the measured value sequence 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 perception neural network. Among them, the pooling operation has two operation modes: taking the maximum value and taking the average value.

[0061] Step 4, using the softmax algorithm to classify the features to be classified in step 3, obtain the first predicted probability of being impacted and the second predicted probability of being impacted, and connect the features to be classified to two nodes, where the classification process includes probability prediction for each classification situation, each classification situation is a sequence of measured values ​​to be sensed or the features to be classified are divided into a corresponding classification by the softmax algorithm; then perform step 5. The softmax algorithm in step 4 divides the features to be classified into two categories, and fully connects the matrix output after multi-layer convolution and one-layer 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 multi-layer convolution and pooling operations, and the two nodes to which they are connected include the first node (the position point indicating the robot is impacted, which is a positive sample required for the training of the impact-perceiving neural network) and the second node (the position point indicating the robot is not impacted, which is a negative sample required for the training of the impact-perceiving neural network). In this embodiment, 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 from the robot in the impact 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 measured value sequence to be sensed or the feature to be classified is classified as the measured value sequence extracted from the robot's current non-impact 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.

[0062] 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; then step 6 is executed. Among them, the weights that need to be updated in the current step 5 are the weights of the convolution layer, that is, the weights of the network. 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 the prediction results of two classifications in the loss function; the impact-aware neural network is designed as a two-classification network. Compared with the prior art, the convolutional neural network trained in this embodiment is simplified to form a lightweight neural network model.

[0063] In step 5, the weight update method of the impact perception neural network based on the loss function includes using the Adam algorithm to update the weight, that is, using the gradient descent algorithm to update the weight matrix, and then gradually iterating and solving by gradient descent, continuously updating the impact perception neural network after weight update to the impact perception neural network, so as to obtain the minimized loss value in step 6, and realize the deviation correction of the sensed measurement value sequence and the weight. Thereby improving the feature extraction accuracy of the impact perception neural network, and further improving the feasibility of using the impact perception neural network to detect whether the robot is impacted.

[0064] 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, 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 second predicted probability of being impacted; when the first predicted probability of being impacted obtained in step 4 p p and the predicted probability of the second impact p np When the label true value p changes gtp The loss value may change, causing the loss value to adapt according to the loss function; in the process of updating the weights using the gradient descent algorithm (Adam algorithm), the loss value becomes smaller, thereby improving the optimization effect of the loss function on the impact-perceiving neural network.

[0065] Step 6, repeating steps 2 to 5, so as to realize that each input feature generated by the sequence of measured values ​​to be sensed is input into the impact-perceiving neural network after the weight is updated, until the latest calculated loss value is less than the preset loss threshold, and then the impact-perceiving neural network after the weight is updated is updated to the impact-perceiving neural network. Specifically, when the weight is updated once, the impact-perceiving neural network is updated once, and the updated impact-perceiving neural network uses the same sequence of measured values ​​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 is preferably achieved by repeatedly executing steps 2 to 5 to minimize the loss function, and then marking the impact-perceiving neural network updated in the latest step 5 as the target impact-perceiving network model. Thus, the training constitutes a complete convolutional neural network model.

[0066] 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 and it is determined that the robot is impacted as the current prejudgment result; otherwise, the true value of the label required by the loss function is set to 0 and it is determined that the robot is not impacted as the current prejudgment result. It is worth noting that before the calculated loss value is less than the preset loss threshold, the impact situation of the robot obtained 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, that is, the current prejudgment result. Therefore, the process in which the calculated loss value changes from being greater than or equal to the preset loss threshold to being less than the preset loss threshold is regarded as the process of training the impact perception neural network, and is also the 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 learning rate of the network and the number of each dimensional vector within the sequence of measured values ​​to be sensed are adjusted. When the learning rate is set to 0.001, the number of samples in a single set of measured value sequences to be sensed is set to 8711, and 228176 sets of measured value sequences to be sensed are continuously collected, the update of the aforementioned weights can be iterated 140 times, and the impact-based neural network shows the highest precision and recall rate. The number of iterations here can be appropriately increased or decreased according to the need to improve accuracy and considering time cost.

[0067] After executing steps 1 to 6 above, the present application successively performs multi-layer convolution operations, pooling operations, softmax algorithm classification processing, calculates loss values ​​and weight updates, and trains 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 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.

[0068] On the basis of the above embodiment, in the process of classifying the perceived measurement value sequence using the target impact perception network model, 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 calculated, which is equivalent to updating the first predicted probability of being impacted to the first predicted probability of the robot being impacted at the current moment, and updating the second predicted probability of being impacted to the second predicted probability of the robot being impacted at the current moment.

[0069] 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:

[0070] 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, and after extracting the measured value sequence to be sensed in step 1, execute step 2; after generating the 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.

[0071] 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 latest executed step 5 returns to step 2, and then steps 2, 3, 4 and 5 are executed in sequence to form a round of repeated execution of steps 2 to 5, and the weights of the impact-perceiving neural network are 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 latest 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.

[0072] As an embodiment, the construction method of the impact perception neural network includes: connecting four 1-dimensional convolutional layers in sequence to form four convolutional layers stacked continuously, and then connecting a pooling layer, and configuring the kernel length of each 1-dimensional convolutional layer to be 3, and the moving step length of the convolutional layer filter in each 1-dimensional convolutional layer can be configured to be 1, and the pooling operation in the pooling layer is configured as an average operation 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 weights 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, the impact perception neural network is constructed layer by layer according to the construction method of the convolutional neural network, forming a convolutional neural network with multiple convolutional layers stacked and connected. Therefore, this embodiment selects the appropriate number of convolutional layers and pooling layers, the size of the convolutional layer filters, the moving step size of the convolutional layer filters and the method of pooling operation 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 rotational 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.).

[0073] As an embodiment, the method for generating the input feature includes: according to the type of data collected by the robot's sensing device (sensor type), connecting each type of measurement value sequence to be sensed along the channel of the corresponding dimension to generate the input feature; specifically, in the process of sliding the sliding window through the original data, a plurality of the measurement value sequences to be sensed are extracted in sequence according to the order of collection. 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 in the original data, that is, one type of measurement value sequence to be sensed; and there are at least one dimension (coordinate axis dimension) of measurement value sequences to be sensed of the same type. On this basis, the robot connects all types of currently extracted measurement value sequences to be sensed along the channels of corresponding dimensions, respectively, and merges them into the input features to be input into the impact sensing 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, and 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, as the length of a measurement value sequence to be sensed.

[0074] In this embodiment, the sensing device of the robot 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 rotary encoder for sensing left wheel rotation measurement values ​​and a right rotary 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 in 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, and the 1-axis measurement value sequence is a measurement value sequence obtained in the yaw angle direction, the roll angle direction, or the 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 being aligned with the timestamp of the measurement value sequence to be sensed collected in real time by the sensing device, the measurement value sequence to be sensed is marked as being impacted or not impacted by executing steps A to D.

[0075] Preferably, the target impact perception network model includes 4 1D convolutional layers; the 4 1D 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 the 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.

[0076] In this preferred example, 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.

[0077] 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, which can be understood as being conducive to calculating the first impact prediction probability and the second impact prediction probability using the softmax algorithm in the impact-aware neural network, and reducing the number of channels between the convolutional layer and the pooling layer.

[0078] The sliding window keeps the window length unchanged 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 a measurement value sequence to be sensed at 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.

[0079] 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 left wheel rotation distance or the left wheel rotation rate of the robot; the right wheel rotation measurement value includes the right wheel rotation distance or the right wheel rotation rate of the robot.

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

[0081] 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. These two model probability information are compared with the prior art. 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 to form a lightweight network model, which speeds up the detection of the robot's impact, and can also be deployed in resource-constrained embedded systems.

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

[0083] 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 outputted 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, so that the measurement from the same target state at an earlier moment arrives 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 sensing error of the robot's inertial navigation sensor will increase rapidly when the traditional Kalman system-based filtering method uses the delayed measurement data. 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.

[0084] Based on the above technical defects, the present application discloses an embodiment, in which the execution method of step A is as follows: Figure 4 As shown, including:

[0085] Step A1, based on the preset positioning algorithm, the robot obtains the current posture observation data and its noise covariance matrix, and configures the current posture observation data to exist at the current moment, and the current posture observation data is also recorded as disordered position observation data. At the same time, it is determined 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 preset positioning algorithm calculates the current posture observation data, as the solution moment of the current posture observation data. Then the current posture observation data can be understood as delayed output posture data and sensor data processed by the positioning algorithm, wherein the original sensor data includes inertial data collected by the inertial measurement unit and point cloud data collected by the laser sensor; after the original sensor data is processed by the positioning algorithm, the current posture observation data is obtained, and the distance sensor collection data has been delayed for a period of time, but the association relationship on the timeline will be recorded, including the length of the delay time period. Then the current posture observation data is subjected to data association processing to obtain historical posture observation data. Then execute step A2.

[0086] In addition, 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, and the pose data output by the positioning algorithm includes the current pose observation data; the measurement error includes the systematic measurement error of the inertial measurement unit, the systematic measurement error of the rotary encoder, the systematic measurement error of the laser sensor, etc., to determine the initial state of the noise covariance matrix of the current pose observation data; the positioning algorithm is preferably a correlation scan matching algorithm.

[0087] 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 times; 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 time; the timestamp of the historical posture observation data can be approximated to an integer collection time; 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 time 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.

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

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

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

[0091] 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 indicates the angular velocity bias value of the gyroscope installed on the robot; (ω i -ω b )·d represents the Euler angle that the robot has deflected 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. Quaternion is used to represent the current posture angle of the robot.

[0092] 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 convert (a 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.

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

[0094] 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 id at the observation moment The speed of the observation time id and the timestamp of the observation time id to predict the prior displacement The current posture observation data is composed of observation time id, observation time id is the timestamp of the current posture observation data, and d is the time interval between the current time i and observation time id. It should be noted that within the time interval d, the gravity acceleration, bias acceleration and bias angular velocity are considered unchanged, but they are allowed to be updated after the robot is detected to be in a stationary state.

[0095] 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 a 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 attitude angle through the numerical integration method in the framework of the Kalman filter algorithm.

[0096] On the other hand, in the step A2, the method for calculating the prior covariance matrix of the current posture prediction data based on the state equation includes: It 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 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 is ​​equivalent to the state transfer matrix required by the Kalman filter algorithm; the pose prediction data of id at the observation time In the state equation, it can be regarded as the a posteriori state of the robot; F 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 is determined by the IMU measurement noise, Q i The initial value Q 0 It is determined by the measurement error of the inertial measurement unit in distance and orientation, and can form a diagonal matrix.

[0097] Step A3, based on the delayed out-of-order observation equation, calculate the Kalman gain matrix using the noise covariance matrix of the current posture observation data and the prior covariance matrix of the current posture prediction data; then execute step A4.

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

[0099] 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 posture prediction data; U is the noise covariance matrix of the current posture observation data; 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, 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 posture 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 iis the Kalman gain matrix; i is the current moment.

[0100] 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, wherein the correction here can be at least one correction or the correction operation performed after at least one execution of step A3. And 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 prior covariance matrix of the current posture prediction data is corrected. 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 described in step A and the observed velocity direction described in step A constitute the observed velocity vector.

[0101] The method for correcting the current posture prediction data in step A4 includes: in, is the current posture 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 posture 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 It is the residual between the current pose observation data and the historical pose observation data.

[0102] 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 stationary, it can be directly updated in the next moment as I is the identity matrix.

[0103] 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 pre-set 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.

[0104] 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 making full use of 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 perceived measurement value sequence using the target impact perception network model.

[0105] 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:

[0106] 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, in the process of repeatedly executing steps A3 to A4, the current posture observation data and its noise covariance matrix are not updated correspondingly to the result of the latest output 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 can be updated without the measurement error carried in the posture data output by the positioning algorithm at the next moment i+1; 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 preset. 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.

[0107] Alternatively, within a correction cycle, 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, the Kalman gain matrix calculated in step A3 is updated, and 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 updated noise covariance matrix of the current posture observation data; 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 a round of iterative correction or a round of iterative update is completed.

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

[0109] Starting from the initial moment, N groups of measurement values ​​are collected in sequence in one collection cycle, where the timing start point of one collection cycle is moment m, and the timing end point of the same collection cycle is moment m+N-1; if multiple collection cycles are set, N groups of measurement values ​​are collected in each collection cycle, and a time series in a sliding window is formed in one collection cycle. A group of measurement values ​​includes acceleration measurement values, angular velocity measurement values, left wheel rotation measurement values, and right wheel rotation measurement values; when the initial moment is configured as the moment when the robot starts walking, the timing start point of the first collection cycle.

[0110] A weighted mean square error is calculated for N groups of measurement values ​​collected within an acquisition cycle, and then it is determined whether the weighted mean square error is lower than a preset statistical threshold; wherein the preset statistical threshold is a value determined by the inventor 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 a range of change reflected by a weighted mean square threshold calculated from 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, and then determine whether the robot is in a stationary state or has started to move.

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

[0112]

[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 integer m to integer m+N-1, and 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 preset; the time interval from time m to time m+N-1 is a collection cycle; g is the gravitational acceleration; ω 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 is preset; 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 be consistent with The direction of the accelerometer is parallel to that of thek is equal 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 is equal to the value 0.

[0115] In some embodiments, when it is determined that the robot is in a stationary state, the robot directly uses the Kalman filter algorithm to correct the current posture 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, in which the initial search posture in the stationary state is used as 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 a 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 starts 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 kinematic equations will be updated, for example, the current posture angle is updated. Update current speed And update the current predicted prior displacement Therefore, the bias acceleration and the bias angular velocity are updated respectively by using the average measurement values ​​of the accelerometer and the gyroscope when the robot is in a stationary state (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 planned cleaning, the average measurement values ​​of the accelerometer and the 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 pose prediction data and its prior covariance matrix to correct the drift of the inertial measurement unit, thereby achieving 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 the rotation distance is obtained after the filtering is completed, and the robot The speed error is corrected to obtain the corrected speed in the three-dimensional direction, that is, the speed component in the three coordinate axis directions of the current speed calculated in step A2 after correction by the Kalman filter algorithm; then the position error is updated using the corrected speed in the three-dimensional direction to obtain the observed speed value and the observed speed direction, and the predicted speed value and the 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, and a target impact perception network model that takes into account multiple types of measured value sequences to be perceived is constructed to improve the classification and recognition effect of the target impact perception network model. Therefore, through the combination of the Kalman filter algorithm and the impact perception neural network, the advantages are complemented, which is conducive to improving the accuracy of the robot's impact detection, and also solves the problem of errors in the processing of the sensing data of the rotary encoder on the robot's wheel; then, when the robot is detected to be impacted, the corrected speed prediction vector in the three-dimensional direction 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, and both support being solved into 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 observed state data of the left wheel and the observed state data of the right wheel are calculated respectively, and then the predicted state data of the left wheel and its prior covariance matrix are calculated from the observed state data of the left wheel and its prior covariance matrix based on the state equation; and the predicted state data of the right wheel and its prior covariance matrix are calculated from the observed state data of the right wheel and its prior covariance matrix based on the state equation; the predicted state data of the left wheel and its prior covariance matrix are calculated based on the state equation, and the predicted state data of the right wheel and its prior covariance matrix are calculated based on the state equation; then, by introducing the robot motion speed constraint condition, the Kalman filter correction is performed on the predicted state data of the left wheel and the predicted state data of the right wheel respectively, and the corrected predicted state data of the left wheel and the corrected predicted state data of the right wheel are obtained to form an incomplete constraint correction; then the corrected predicted state data of the left wheel is updated to the left wheel rotation measurement value, and the corrected predicted state data of the right wheel is updated to the right wheel rotation measurement value, so that: when it is detected that the robot is impacted, the corrected predicted state data of the left wheel and the corrected predicted state data of the right wheel are used to correct the position error of the robot, including the lateral speed error of the robot.

[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-complete 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 a value of 0, and the magnitude of the velocity component of the robot's current velocity perpendicular to the robot's walking plane is equal to a value of 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 lateral displacement of the robot.

[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 constraints on state increments between the speed of the accelerometer and gyroscope and the preset inertial error estimate, thereby improving the accuracy of the Kalman filter algorithm's posture prediction 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 value of the system within a certain range, provided that the parameter setting needs 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 to correct 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 matching 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 the 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, and obtaining 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, and obtaining the corresponding 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 measured values ​​to be sensed, 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 measured values ​​to be sensed is data collected from 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, after obtaining the predicted speed value and the predicted speed direction at the current moment in step A, 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 determining 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 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 are used to trigger the detection starting point, triggering the target impact perception network model to classify the data collected by the inertial measurement unit and the rotary encoder, taking into account the necessity of the target impact perception network model classification processing, and integrating the prediction information of at least two types of sensor collection data under the Kalman filter framework, so as to improve the reliability and accuracy of the target impact perception network model classification; and then only using the size relationship between the two prediction probabilities to make a judgment, overcoming the misjudgment problem caused by the sudden change of the measured value of the inertial measurement unit, and improving the accuracy of the impact detection. Among them, the target impact perception network model, as a lightweight neural network model, reduces the dependence on the built-in storage resources of the chip and speeds up the detection speed of the chip.

[0129] The present application also discloses a robot, wherein the chip is built-in, so that the robot executes the robot impact detection method disclosed in the above embodiment; 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. Then the robot is essentially a wheeled robot equipped with two odometers, and the two odometers respectively sense the posture state of the body and the rotation state of the wheel axles on both sides of the body. Among them, the gyroscope is used to measure the angular velocity; the accelerometer is used to measure the acceleration. The left wheel of the robot is equipped with a left-rotating encoder, and the left-rotating encoder is used to sense the left wheel rotation measurement value; the right wheel of the robot is equipped with a right-rotating encoder, and the right-rotating encoder is used to sense the right wheel rotation measurement value. Regardless of whether the robot is in a stationary state, by executing the robot impact detection method, the misjudgment problem caused by the sudden change of the measured value of the inertial measurement unit is overcome, and the accuracy of impact detection is improved.

[0130] Those skilled in the art will appreciate that the embodiments of the present application may be provided as methods, systems, or computer program products. Therefore, the present application may 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 may adopt the form of a computer program product implemented in one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) that include 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 generate a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0132] These computer program instructions may also be stored in a computer-readable memory capable of directing 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 an article of manufacture comprising an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[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, thereby providing instructions for implementing the process. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0134] Finally, it should be noted that the above embodiments are only used to illustrate the technical solution of the present invention rather than to limit it. Although the present invention has been described in detail with reference to the 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 solution of the present invention, which should be included in the scope of the technical solution for protection of the present invention.

Claims

1. A method for detecting impact of a robot, It is characterized in that The robot impact detection methods include: Step A, obtaining the observed speed value and the observed speed direction, and then obtaining the predicted speed value and the predicted speed direction at the current moment based on the 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, and obtaining 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, and obtaining the cosine distance corresponding to the current speed change; 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; 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; 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 latest calculated loss value is less than a preset loss threshold, marking the shock-perceiving neural network after the weight update obtained in the latest 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 latest executed step 5 is a trained shock-perceiving neural network; 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, using the softmax algorithm to classify the target classification features to obtain a first predicted probability that the robot is currently impacted and a second predicted probability that the robot is currently impacted; wherein the first predicted probability that the robot is currently impacted 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 a state of being impacted; the second predicted probability that the robot is currently impacted 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 a state of not being impacted.

2. The robot impact detection method according to claim 1, It is 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.

3. The robot impact detection method according to claim 1, It is characterized in that Before executing step C, the target impact perception network model is constructed.

4. The robot impact detection method according to claim 3, It is 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; Among them, 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 from the robot in the impact state, 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 from the robot in the non-impact state, so as to form the probability of the second classification generated by the softmax algorithm.

5. The robot impact detection method according to claim 4, It is 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.

6. The robot impact detection method according to claim 3, It is 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.

7. The robot impact detection method according to claim 6, It is 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.

8. The robot impact detection method according to claim 1, It is 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, the sequences of measured values ​​to be sensed of each type are connected 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 at least equal to 8; wherein the input feature is set in the form of a matrix.

9. The robot impact detection method according to claim 8, It is 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.

10. The robot impact detection method according to claim 8, It is 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.

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

12. The robot impact detection method according to claim 8, It is 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 by the positioning algorithm to obtain historical posture observation data matches the original data required to be collected by 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.

13. The robot impact detection method according to claim 12, It is 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; K i is the Kalman gain matrix; i is the current moment.

14. The robot impact detection method according to claim 13, It is 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.

15. The robot impact detection method according to claim 12, It is 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 The current posture prediction data is composed of 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.

16. The robot impact detection method according to claim 15, It is 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.

17. The robot impact detection method according to claim 15, It is 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; 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.

18. The robot impact detection method according to claim 17, It is 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.

19. A chip for storing programs; It is 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 18.

20. A robot, It is characterized in that The robot is equipped with the chip described in claim 19; 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