Robot mechanical arm collision avoidance planning method and system, medium and equipment

Abstract

The invention relates to the technical field of man-machine interaction, and provides a robot mechanical arm collision avoidance planning method and system, a medium and equipment, and the method comprises the steps: obtaining posture features through the processing of a feature coding layer and an encoder for the observed posture features of a human body, and carrying out the inverse discrete cosine transform after the posture features are processed through a decoder and a feature decoding layer, obtaining human body posture features at future moments; for observed human body track features, track features are obtained through processing of a feature encoding layer and an encoder, feature fusion is carried out on the track features and posture features through a multi-head attention layer, after the fused track features are processed through a decoder and a feature decoding layer, inverse discrete cosine transform is carried out, and human body track features at the future moment are obtained; calculating the human body posture features at the future moment into a human body model, and performing point cloudization on the human body model to obtain predicted dynamic human body point cloud; and obtaining an object point cloud, taking the object point cloud and the dynamic human body point cloud as an obstacle, and performing collision avoidance trajectory planning.

Description

Technical Field

[0001] The present invention belongs to the technical field of human-computer interaction, and particularly relates to a method, system, medium and device for collision avoidance planning of a robot manipulator. Background Technique

[0002] The statements in this part only provide background technical information related to the present invention, and do not necessarily constitute prior art.

[0003] Human pose prediction has wide application value in the fields of human-computer interaction, motion capture, virtual reality, etc. The purpose of human pose prediction is to predict the motion trend of the human body in the future for a period of time through a given historical motion sequence. With the development of deep learning technology, significant progress has been made in human pose prediction models based on neural networks. However, there are still certain challenges in terms of real-time performance and accuracy. At the same time, it is difficult to meet the fineness requirements using a geometric envelope model to model the human body.

[0004] During the human-computer interaction process, avoiding collisions between the manipulator and the human body is an important goal. There are many collision avoidance planning methods, such as sampling-based methods, optimization-based methods, learning-based methods, etc.

[0005] Due to the advantages of being able to handle non-linear systems and high-dimensional state spaces and being able to adjust in real time online in a dynamic environment, sampling-based Model Predictive Control (MPPI) has gradually become the mainstream method for collision avoidance path planning. However, traditional sampling-based model predictive control algorithms are not optimized for human collision avoidance, and in a dynamic environment, it is difficult for the manipulator to maintain real-time performance when facing dynamic obstacles. Summary of the Invention

[0006] In order to solve the technical problems existing in the above background technique, the present invention provides a method, system, medium and device for collision avoidance planning of a robot manipulator. Based on the predicted human trajectory features and human pose features, the human pose prediction network is jointly optimized to improve the effect of human pose prediction. Moreover, the predicted human pose sequence is calculated into a human model, which is point-clouded and added to the robot manipulator collision avoidance, so that the cost calculation of the manipulator sampling trajectory is related not only to the current human body and object point clouds, but also to the predicted future human body point clouds, making the collision avoidance trajectory of the manipulator more forward-looking.

[0007] In order to achieve the above object, the present invention adopts the following technical solutions: The first aspect of the present invention provides a method for collision avoidance planning of a robot manipulator, which includes: Obtain the observed human pose features and observed human trajectory features at a plurality of historical moments; For observing human body posture features, after processing by the feature encoding layer and the encoder, posture features are obtained. After processing the posture features by the decoder and the feature decoding layer, inverse discrete cosine transform is performed to obtain the human body posture features at future moments; for observing human body trajectory features, after processing by the feature encoding layer and the encoder, trajectory features are obtained. The trajectory features and the posture features are subjected to feature fusion through the multi-head attention layer. After the fused trajectory features are processed by the decoder and the feature decoding layer, inverse discrete cosine transform is performed to obtain the human body trajectory features at future moments; the human body trajectory features and the human body posture features at future moments are combined to optimize the feature encoding layer, the encoder, the decoder, and the feature decoding layer; The human body posture features at future moments are calculated into a human body model, and the human body model is point-clouded to obtain the predicted dynamic human body point cloud; The object point cloud is obtained and used as an obstacle together with the dynamic human body point cloud to perform collision avoidance trajectory planning.

[0008] Further, the steps of the multi-head attention layer for feature fusion include: Based on the trajectory features and the posture features , the query Q , the key K , and the value V are calculated, where , , and are the weights of three linear layers; Based on the query, the key, and the value, the associated feature V is calculated, where is used for normalization processing; The associated feature is matrix-multiplied with the posture feature and then matrix-concatenated to obtain the fused trajectory feature.

[0009] Further, the calculation of the human body model uses a linear model for human body modeling.

[0010] Further, the collision avoidance trajectory planning uses sampling-based model predictive control.

[0011] Further, the feature encoding layer includes a discrete cosine transform, a fully-connected layer, and a transposed layer connected in sequence.

[0012] Further, the encoder uses a multi-layer perceptron encapsulated by multiple linear layers and normalization layers.

[0013] Further, the feature decoding layer has the same structure as the feature encoding layer; the decoder has the same structure as the encoder.

[0014] The second aspect of the present invention provides a robot manipulator collision avoidance planning system, which includes: A data acquisition module, which is configured to: acquire observed human body pose features and observed human body trajectory features at a plurality of historical moments; A feature extraction module, which is configured to: for the observed human body pose features, through a feature encoding layer and an encoder for processing, obtain pose features, and after processing the pose features through a decoder and a feature decoding layer, perform inverse discrete cosine transform to obtain future moment human body pose features; for the observed human body trajectory features, through a feature encoding layer and an encoder for processing, obtain trajectory features, perform feature fusion on the trajectory features and the pose features through a multi-head attention layer, and after processing the fused trajectory features through a decoder and a feature decoding layer, perform inverse discrete cosine transform to obtain future moment human body trajectory features; combine the future moment human body trajectory features and the human body pose features to optimize the feature encoding layer, the encoder, the decoder, and the feature decoding layer; A point cloud generation module, which is configured to: calculate the future moment human body pose features into a human body model, and generate a point cloud of the human body model to obtain a predicted dynamic human body point cloud; A trajectory planning module, which is configured to: acquire an object point cloud, and use it together with the dynamic human body point cloud as obstacles to perform collision avoidance trajectory planning.

[0015] A third aspect of the present invention provides a computer-readable storage medium, on which a computer program is stored, and when the program is executed by a processor, it implements the steps in a robot manipulator collision avoidance planning method as described above.

[0016] A fourth aspect of the present invention provides a computer device, including a computer-readable storage medium, a processor, and a computer program stored on the computer-readable storage medium and executable on the processor, and when the processor executes the program, it implements the steps in a robot manipulator collision avoidance planning method as described above.

[0017] Compared with the prior art, the beneficial effects of the present invention are: The present invention calculates the predicted human body pose sequence into a human body model through the SMPL model, and generates a point cloud of the human body model, and adds it to the mechanical arm collision avoidance based on MPPI, so that the cost calculation of the mechanical arm sampling trajectory is not only related to the current human body and object point clouds, but also related to the predicted future human body point clouds, enabling the mechanical arm to select a more appropriate control input, and also having a certain degree of forward-looking in the collision avoidance process.

[0018] The present invention considers that a person's pose is largely affected by the position (trajectory) of the hip, predicts the human body trajectory features, and fuses them with the pose features through a multi-head attention mechanism. Based on the predicted human body trajectory features and human body pose features, jointly optimize the human body pose prediction network, which can better improve the effect of human body pose prediction. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] The accompanying drawings forming a part of this invention are used to provide a further understanding of the invention. The schematic embodiments of the invention and their descriptions are used to explain the invention and do not constitute an improper limitation of the invention.

[0020] Figure 1 is a flowchart of a method for collision avoidance planning of a robotic manipulator according to Embodiment 1 of the present invention; Figure 2 is a schematic diagram of a human pose prediction network according to Embodiment 1 of the present invention; Figure 3 is a schematic diagram of the structure of a multi-head attention layer according to Embodiment 1 of the present invention; Figure 4 is a schematic diagram of point cloudifying a human body using SMPL according to Embodiment 1 of the present invention; Figure 5 is a schematic diagram of the manipulator applying MPPI to select the first control input with the lowest cost according to Embodiment 1 of the present invention; Figure 6 is a schematic diagram of a sampling-based model predictive control planning process according to Embodiment 1 of the present invention; Figure 7 is a schematic diagram of the structure of a computer device according to Embodiment 4 of the present invention. Detailed implementation manners

[0021] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention.

[0022] It should be noted that the following detailed descriptions are all illustrative and are intended to provide further explanations of the present invention. Unless otherwise specified, all technical and scientific terms used herein have the same meaning as commonly understood by those of ordinary skill in the technical field to which the present invention belongs.

[0023] Embodiment 1 This embodiment provides a method for collision avoidance planning of a robotic manipulator.

[0024] The method for collision avoidance planning of a robotic manipulator provided in this embodiment, on the basis of MPPI planning for collision avoidance, adds a human pose point cloud prediction network, calculates the predicted future human pose sequence into a human model using the SMPL (a linear model for human body modeling) model, point cloudifies the human model, and uses the point cloud in the environment and the predicted point cloud together for collision avoidance planning by the MPPI algorithm.

[0025] The method for collision avoidance planning of a robotic manipulator provided in this embodiment, as Figure 1 shown, includes the following steps: Step 1: Predict the future human body posture and trajectory through a human body posture prediction network.

[0026] In step 1 of this embodiment, a multi-layer perceptron human body posture point cloud prediction sub-network and a human body trajectory prediction sub-network combined with a multi-head attention layer are provided.

[0027] As Figure 2 shown, the two prediction sub-networks have the same structure, both including: a feature encoding layer, an encoder, a decoder, and a feature decoding layer, and the two sub-networks are connected through a multi-head attention layer.

[0028] Step 101: Obtain the user's observed posture through a depth camera, that is, the observed human body posture features at the past historical moments and the observed human body trajectory features; Step 102: Perform discrete cosine transform (DCT) on the observed human body posture features to encode the action time series, and after encoding it is .

[0029] Step 103: Input into a single-layer linear layer (fully connected layer) to process the features in the action space, and use a transpose layer to transpose the features to obtain the output ; where and are the learnable parameters of the single-layer linear layer (fully connected layer); Step 104: Input the feature into the posture encoder, and the posture encoder contains m perceptron modules.

[0030] Input the feature into a multi-layer perceptron module encapsulated by a multi-layer linear layer (fully connected layer) and a normalization layer to encode the posture features in terms of time.

[0031] Use a structure similar to a residual network between adjacent two-layer perceptron modules. The residual network only needs to learn the residual amount of the front and back features. Use the formula , where represents the normalization layer, and are the learnable parameters of the i-th fully connected layer, is the output of the i-th fully connected layer, and the output feature of the (i - 1)-th perceptron module, after passing through the i-th perceptron module, cross-layer combines the original feature to obtain the output feature of the i-th perceptron module.

[0032] The output of the pose encoder is pose features , where m is the number of multi-layer perceptrons in the pose encoder, is the output feature of the (m - 1)-th layer perceptron module.

[0033] Step 105: Input the pose features into a pose decoder with the same structure as the pose encoder, and decode the pose features in the time dimension; transpose the decoded pose features to ; input into a single-layer linear layer to extract spatial features.

[0034] Step 106: Use the inverse discrete cosine transform to transform the output back to the original pose representation, and obtain the human pose features at the future 1 moment.

[0035] Step 107: Input the observed human trajectory features into the discrete cosine transform to encode the action time series, and after encoding it is .

[0036] Step 108: Input into a single-layer linear layer to process the features in the action space, and use a transpose layer to transpose the features to obtain the output .

[0037] Step 109: Input the feature into a multi-layer perceptron module with the same structure as above to encode the trajectory features in time.

[0038] Step 110: Perform feature fusion on the obtained pose features and the trajectory features , and input them into a multi-head attention layer.

[0039] As Figure 3 shown, the inputs of the multi-head attention layer are query Q, key K, and value V, which are , , respectively; among them, , and are the weights of three linear layers; through the formula V, is used for normalization processing to obtain the associated feature of the pose feature and the trajectory feature ; multiply the associated feature with the pose feature and then perform matrix splicing to obtain the new trajectory feature .

[0040] Step 111: Input the fused trajectory features into a trajectory decoder with the same structure as the trajectory encoder, and decode the trajectory features in the time dimension; transpose the decoded trajectory features to ; input into a single-layer linear layer to extract spatial features; use the inverse discrete cosine transform to transform the output back to the original trajectory representation to obtain the human trajectory features for the next 1 . .

[0041] Since a person's pose is largely affected by the position (trajectory) of the hips, predicting the human trajectory features and fusing them with the pose features through the multi-head attention mechanism, and jointly optimizing the human pose prediction network based on the predicted human trajectory features and human pose features can better improve the effect of human pose prediction.

[0042] Step 2: Calculate the predicted human pose features into a human body model, and point-cloudify the human body model to obtain the predicted dynamic human point cloud.

[0043] In step 2 of this embodiment, use SMPL as shown in Figure 4 to convert the predicted human pose features into a human body model, and point-cloudify the human body model.

[0044] Step 3: Construct a sampling-based model predictive control (MPPI) to achieve collision avoidance path planning in an environment with obstacle point clouds.

[0045] In step 3 of this embodiment, provide a mechanical arm collision avoidance planning method based on MPPI, as shown in Figure 6 , including:[[]] (1) Select the state of the model predictive control as , where q,[[]] ,[[]] are the angle, angular velocity, and angular acceleration of the robotic arm, respectively.

[0046] (2) Model the motion of the robotic arm as the optimal control of a discrete stochastic dynamic system , where is the state at time t transitions to the state at time under the control of the input with probability.

[0047] (3) Sample batches of control sequences of length from the current input distribution , for each batch, the sampled control sequence is input into the approximate dynamic model of the robotic arm to obtain the trajectory and the corresponding state sequence and the cost sequence . Among them, the batch of the state sequence and the corresponding cost sequence is N, and the length is H.

[0048] In this embodiment, the cost of each batch is calculated in parallel , where is the terminal cost outside the length , is the discount factor, which is used to bias the immediate cost; the sampling batch is N, is the cost obtained by inputting a batch of control sequences, is the h-th control of the control sequence with length H at time t in the current batch. Similarly, is the h-th state of the state sequence with length H at time t in the current batch, is the parameter of the terminal cost, which is a reasonable parameter specified by humans.

[0049] (4) As Figure 5 shown, select the first input of the control sequence with the lowest cost from the batch of control sequences as the input of the current state, that is, the optimal input.

[0050] It should be noted that what is obtained is a control sequence, which is a control sequence with a vision length of H, and the first of the control sequences is selected as the input.

[0051] (5) Use the currently sampled control sequence to update the distribution of the next sampling, update the variance and mean of the distribution, and use it for the next sampling. Mean update equation: , covariance update equation: , and are the update steps, and the calculation equation of the weight : ; among them, is the mean of the sampling distribution at the h-th position of the entire sequence at time t, is the covariance of the sampling distribution at the h-th position of the entire sequence at time t; in the calculation formula of the weight , represents the control at the h-th position of the i-th batch sequence, represents the state at the h-th position of the i-th batch sequence, is the parameter of the terminal cost.

[0052] (6) Sample the control sequence from the distribution again, calculate the control input, and update the input distribution at the same time, and loop in this way until the target point is reached. ​​​​​​​​​​​​​​​​​​​

[0053] Step 4: Use the predicted dynamic human body point cloud and the real-time object point cloud of the environment as obstacles and add them together to the sampling-based model predictive control collision avoidance planning.

[0054] Obstacles will affect the cost of collision, and thus affect the total cost. The cost will affect the weights in the update equation, thereby affecting the sampling distribution.

[0055] In step 4 of this embodiment, the human body pose prediction point cloud is integrated into the sampling-based model predictive control to provide a collision avoidance planning method based on MPPI combined with the human body pose prediction point cloud.

[0056] During the motion planning of the robotic arm based on sampling-based model predictive control, the point clouds of real-time objects and the human body are regarded as obstacles.

[0057] The sampling trajectory of the robotic arm calculates the collision avoidance cost of the obstacles to obtain the control input with the lowest cost, and controls the next movement of the robotic arm.

[0058] As Figure 1 shown, the dynamic human body point cloud predicted by the prediction network is combined with the real-time point cloud and added to the motion planning of the robotic arm. Each sampling trajectory of the robotic arm not only avoids the real-time point cloud of the human body, but also considers the predicted dynamic human body point cloud, making the collision avoidance trajectory of the robotic arm more forward-looking.

[0059] Basic grid collision detection is used in the collision avoidance planning of sampling-based model predictive control.

[0060] Step 5: Plan a more forward-looking collision avoidance trajectory through sampling-based model predictive control, and the robotic arm completes the motion.

[0061] A robotic arm collision avoidance planning method provided by this embodiment calculates the human body model from the predicted human body pose sequence through the SMPL model, point-cloudifies the human body model, and adds it to the robotic arm collision avoidance based on MPPI, so that the cost calculation of the robotic arm sampling trajectory is not only related to the current human body and object point clouds, but also related to the predicted future human body point clouds, enabling the robotic arm to select a more appropriate control input and also having a certain degree of forward-looking during the collision avoidance process.

[0062] Embodiment 2 This embodiment provides a robotic arm collision avoidance planning system, which specifically includes: A data acquisition module, which is configured to: acquire the observed human body pose features and observed human body trajectory features at several historical moments; A feature extraction module, which is configured to: for the observed human body posture features, through the feature encoding layer and the encoder for processing to obtain the posture features, after processing the posture features through the decoder and the feature decoding layer, perform inverse discrete cosine transform to obtain the human body posture features at future moments; for the observed human body trajectory features, through the feature encoding layer and the encoder for processing to obtain the trajectory features, perform feature fusion on the trajectory features and the posture features through the multi-head attention layer, and after processing the fused trajectory features through the decoder and the feature decoding layer, perform inverse discrete cosine transform to obtain the human body trajectory features at future moments; combine the human body trajectory features and the human body posture features at future moments to optimize the feature encoding layer, the encoder, the decoder, and the feature decoding layer; A point cloud generation module, which is configured to: calculate the human body posture features at future moments into a human body model, and generate a point cloud of the human body model to obtain the predicted dynamic human body point cloud; A trajectory planning module, which is configured to: obtain the object point cloud, and use it together with the dynamic human body point cloud as obstacles to perform collision avoidance trajectory planning.

[0063] It should be noted here that each module in this embodiment corresponds one by one to each step in Embodiment 1, and the specific implementation process is the same, so it will not be repeated here.

[0064] Embodiment 3 This embodiment provides a computer-readable storage medium, on which a computer program is stored, and when the program is executed by a processor, it implements the steps in a robot manipulator collision avoidance planning method as described in Embodiment 1 above.

[0065] Embodiment 4 This embodiment provides a computer device, as Figure 7 shown, including a computer-readable storage medium 1003, a processor 1001, a communication interface 1002, and a computer program stored on the computer-readable storage medium 1003 and executable on the processor 1001. Among them, the processor 1001, the communication interface 1002, and the computer-readable storage medium 1003 can be connected through a bus or other means. Among them, the communication interface 1002 is used to receive and send data, and when the processor 1001 executes the program, it implements the steps in a robot manipulator collision avoidance planning method as described in Embodiment 1 above.

[0066] The above are only the preferred embodiments of the present invention and are not used to limit the present invention. For those skilled in the art, the present invention can have various changes and modifications. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A method for collision avoidance planning of a robotic manipulator, characterized in that, Including: Obtain the observed human body pose features and observed human body trajectory features at several historical moments; For the observed human body pose features, through the feature encoding layer and the encoder for processing, obtain the pose features. After processing the pose features through the decoder and the feature decoding layer, perform inverse discrete cosine transform to obtain the human body pose features at future moments; For the observed human body trajectory features, through the feature encoding layer and the encoder for processing, obtain the trajectory features. Perform feature fusion on the trajectory features and the pose features through the multi-head attention layer. After processing the fused trajectory features through the decoder and the feature decoding layer, perform inverse discrete cosine transform to obtain the human body trajectory features at future moments; Combine the human body trajectory features and human body pose features at future moments to optimize the feature encoding layer, encoder, decoder, and feature decoding layer; Calculate the human body pose features at future moments into a human body model, and point-cloudify the human body model to obtain the predicted dynamic human body point cloud; Obtain the object point cloud, and use it together with the dynamic human body point cloud as obstacles to perform collision avoidance trajectory planning.

2. The robotic manipulator collision avoidance planning method according to claim 1, wherein The steps for the multi-head attention layer to perform feature fusion include: Based on trajectory features and pose features , calculate query Q , key K and value V , where , and are the weights of three linear layers; Calculate the associated features based on the query, key, and value V, where is used for normalization processing; Multiply the associated features and the pose features by matrix and then perform matrix splicing to obtain the fused trajectory features.

3. A robot manipulator collision avoidance planning method according to claim 1, characterized in that, The calculation of the human body model uses a linear model for human body modeling.

4. A robot manipulator collision avoidance planning method according to claim 1, characterized in that, The collision avoidance trajectory planning uses sampling-based model predictive control.

5. A method for collision avoidance planning of a robotic manipulator according to claim 1, characterized in that, The feature encoding layer includes a discrete cosine transform, a fully connected layer, and a transposed layer connected in sequence.

6. The robot manipulator collision avoidance planning method according to claim 1, wherein The encoder uses a multi-layer perceptron encapsulated by a multi-layer linear layer and a normalization layer.

7. The method for collision avoidance planning of a robotic manipulator according to claim 1, characterized in that The feature decoding layer has the same structure as the feature encoding layer; the decoder has the same structure as the encoder.

8. A robot manipulator collision avoidance planning system, characterized in that, Including: A data acquisition module, which is configured to: Obtain the observed human body pose features and observed human body trajectory features at several historical moments; A feature extraction module, which is configured to: For the observed human body pose features, through the feature encoding layer and the encoder for processing, obtain the pose features. After processing the pose features through the decoder and the feature decoding layer, perform inverse discrete cosine transform to obtain the human body pose features at future moments; For the observed human body trajectory features, through the feature encoding layer and the encoder for processing, obtain the trajectory features. Perform feature fusion on the trajectory features and the pose features through the multi-head attention layer. After processing the fused trajectory features through the decoder and the feature decoding layer, perform inverse discrete cosine transform to obtain the human body trajectory features at future moments; Combine the human body trajectory features and human body pose features at future moments to optimize the feature encoding layer, encoder, decoder, and feature decoding layer; A point-cloudification module, which is configured to: Calculate the human body pose features at future moments into a human body model, and point-cloudify the human body model to obtain the predicted dynamic human body point cloud; A trajectory planning module, which is configured to: Obtain the object point cloud, and use it together with the dynamic human body point cloud as obstacles to perform collision avoidance trajectory planning.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the program is executed by a processor, it implements the steps in a robot manipulator collision avoidance planning method as described in any one of claims 1-7.

10. A computer device, comprising a computer-readable storage medium, a processor, and a computer program stored on the computer-readable storage medium and executable on the processor, characterized in that, When the processor executes the program, it implements the steps in a robot manipulator collision avoidance planning method as described in any one of claims 1-7.

Images