Robot path planning method and device and robot

Abstract

The application provides a path planning method and device of a robot and the robot, and relates to the technical field of robot control. The method comprises the following steps: acquiring sampling information of a human obstacle, and determining a plurality of collision query points of the robot on a current planning path; performing batch collision detection on the collision query points by using a signed distance field model based on the sampling information, so as to obtain a collision distance corresponding to each collision query point; if the collision distance of any collision query point does not meet a set condition, a new planning path is determined again based on a current joint configuration of the robot and the sampling information, so that the robot moves according to the new planning path. The application can improve the path updating frequency of the robot, so as to meet the real-time requirement in a human-robot cooperation scene.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of robot control, and in particular to a robot path planning method, device and robot. BACKGROUND

[0002] In the existing robot motion planning technology, most methods are designed based on static or quasi-static environment assumption. The core of such methods is to plan a collision-free path from the starting point to the target point under the fixed obstacle layout through a serial collision detection mechanism.

[0003] However, in the dynamic scenario of human-robot collaboration, the robot needs to share the workspace with the workers, and the robot must avoid obstacles in real time. The human body posture usually changes continuously at a high frequency (for example, 30Hz), which requires the robot to complete perception, decision-making and path regeneration in a very short time window (such as 100-200 milliseconds) to achieve safe and smooth collaboration.

[0004] However, when the above static path planning method using a serial collision detection mechanism is directly applied to the dynamic scenario of human-robot collaboration, the path update frequency is low, which cannot meet the real-time requirements of high-frequency updates of the human body posture, and thus cannot achieve obstacle avoidance response to dynamic human obstacles, which poses a potential safety hazard. SUMMARY

[0005] Embodiments of the present application provide a robot path planning method, device and robot to solve the problem of low path update frequency of the robot, which cannot meet the real-time requirements in the human-robot collaboration scenario and is prone to collision.

[0006] In a first aspect, embodiments of the present application provide a robot path planning method, comprising:

[0007] Obtaining sampling information of a human obstacle and determining a plurality of collision query points of the robot on a current planning path;

[0008] Based on the sampling information, performing batch collision detection on the collision query points using a signed distance field model to obtain collision distances corresponding to each collision query point; the signed distance field model is used to encode features of the sampling information, determine obstacle features, and perform parallel decoding operation on the collision query points based on the obstacle features to obtain collision distances corresponding to each collision query point;

[0009] If the collision distance of any collision query point does not meet the set condition, a new planning path is determined based on the current joint configuration of the robot and the sampling information, so that the robot moves according to the new planning path.

[0010] In a possible implementation, the sampling information comprises: surface sampling points and skeleton key points of the human obstacle;

[0011] The signed distance field model comprises: a first encoder, a second encoder, a feature splicing module, and a decoding module.

[0012] The first encoder is configured to perform feature extraction on the surface sampling points to obtain surface geometric features.

[0013] The second encoder is configured to perform feature extraction on the skeleton key points to obtain human posture features.

[0014] The feature splicing module is configured to perform feature splicing on the surface geometric features and the human posture features to obtain obstacle features.

[0015] The decoding module is configured to perform parallel decoding operations on the plurality of collision query points based on the obstacle features to obtain collision distances corresponding to each collision query point.

[0016] In a possible implementation, the re-determination of the new planning path based on the current joint configuration of the robot and the sampling information comprises:

[0017] Feature encoding is performed on the sampling information by using a signed distance field model to determine obstacle features.

[0018] An initial search tree, accumulated costs of nodes in the search tree, and an optimal path cost are initialized; the nodes in the search tree are used to represent joint configurations of the robot.

[0019] Based on the current joint configuration, a preset target joint configuration, and the optimal path cost, an ellipsoid sampling region is determined, and a plurality of candidate samples are batch-generated in the ellipsoid sampling region.

[0020] Distance information between all nodes in the search tree and all candidate samples is batch-calculated to obtain a distance matrix; the distance matrix comprises distance information between each node and each candidate sample.

[0021] Based on the distance matrix and the accumulated costs of the nodes in the search tree, a candidate parent node is determined for each candidate sample from all nodes in the search tree.

[0022] Based on each candidate sample and its corresponding candidate parent node, a group of interpolation points are determined, and parallel decoding operations are performed on each group of interpolation points and its corresponding candidate sample based on the obstacle features by using the signed distance field model to obtain collision distances corresponding to each group of interpolation points and its corresponding candidate sample.

[0023] If the collision distance corresponding to the interpolation point and the corresponding candidate sample meets the set condition, the candidate sample is determined as a new node in the search tree, and the accumulated cost of the new node is determined;

[0024] Based on the accumulated cost of the new node, the optimal path cost is updated, and based on the updated optimal path cost, the step of determining an ellipsoid sampling region based on the current joint configuration, a preset target joint configuration and the optimal path cost is performed, and when the algorithm termination condition is reached, the path corresponding to the current optimal path cost is determined as a new planning path.

[0025] In a possible implementation, the step of determining a set of interpolation points based on each candidate sample and the corresponding candidate parent node includes:

[0026] The line between each candidate sample and the corresponding candidate parent node is determined as a candidate edge;

[0027] A plurality of interpolation points are inserted at equal intervals on the candidate edge, and the plurality of interpolation points are determined as a set of interpolation points;

[0028] The joint configuration corresponding to the interpolation point is: ;

[0029] Wherein, represents the joint configuration corresponding to the interpolation point between the candidate sample and the candidate parent node, represents the joint configuration corresponding to the candidate parent node, represents the joint configuration corresponding to the candidate sample, represents the total number of interpolation points between the candidate sample and the candidate parent node.

[0030] In a possible implementation, the step of determining a candidate parent node for each candidate sample from all nodes in the search tree based on the distance matrix and the accumulated cost of each node in the search tree includes:

[0031] For each candidate sample, a set of neighboring nodes adjacent to the candidate sample is determined from all nodes in the search tree based on the distance matrix;

[0032] For each node in the set of neighboring nodes, the accumulated cost corresponding to the candidate sample when the node is used as a candidate parent node is calculated;

[0033] The node corresponding to the minimum value of the accumulated cost is determined as the candidate parent node corresponding to the candidate sample.

[0034] ​In a possible implementation, the operation of decoding each group of interpolation points and the corresponding candidate samples based on the obstacle feature by using the signed distance field model to obtain the collision distances corresponding to each group of interpolation points and the corresponding candidate samples comprises the following operations.

[0035] The operation of converting each group of interpolation points and the corresponding candidate samples into a collision query point matrix by using the batch forward kinematics algorithm comprises the following operations.

[0036] The operation of inputting the collision query point matrix into the signed distance field model to enable the signed distance field model to decode the collision query point matrix based on the obstacle feature to obtain a distance vector corresponding to the collision query point matrix comprises the following operations.

[0037] In a possible implementation, the operation of converting each group of interpolation points and the corresponding candidate samples into a collision query point matrix comprises the following operations.

[0038] The operation of aggregating the joint configurations corresponding to each group of interpolation points and the corresponding candidate samples into a joint configuration matrix comprises the following operations.

[0039] The operation of determining the transformation matrix corresponding to each joint in the robot under all joint configurations based on the joint angles of each joint in the joint configuration matrix comprises the following operations.

[0040] The operation of extracting the three-dimensional space coordinates of the plurality of collision detection points under all joint configurations from the transformation matrix corresponding to each joint in the robot under all joint configurations to obtain the collision query point matrix comprises the following operations.

[0041] In a possible implementation, the operation of determining the transformation matrix corresponding to each joint in the robot under all joint configurations based on the joint angles of each joint in the joint configuration matrix comprises the following operations.

[0042] The operation of calculating the relative transformation of each joint under all joint configurations based on the joint angle of the joint under all joint configurations comprises the following operations.

[0043] The operation of determining the transformation matrix of each joint under all joint configurations based on the transformation matrix of the previous joint and the relative transformation of the joint under all joint configurations comprises the following operations.

[0044] In a second aspect, an embodiment of the present application provides a path planning device of a robot, comprising:

[0045] an acquisition module configured to acquire sampling information of a human obstacle and determine a plurality of collision query points of the robot on a current planning path;

[0046] a detection module configured to perform batch collision detection on the collision query points based on the sampling information by using a signed distance field model to obtain collision distances corresponding to the collision query points; the signed distance field model is configured to perform feature coding on the sampling information, determine obstacle features, and perform parallel decoding operation on the collision query points based on the obstacle features to obtain the collision distances corresponding to the collision query points;

[0047] a planning module configured to, if the collision distance of any collision query point does not meet a set condition, determine a new planning path based on a current joint configuration of the robot and the sampling information, so that the robot moves according to the new planning path.

[0048] In a third aspect, an embodiment of the present application provides a robot, comprising a memory and a processor, the memory stores a computer program, and the processor implements the method in the first aspect or any possible implementation manner of the first aspect when executing the computer program.

[0049] Compared with the prior art, the signed distance field model in the embodiment of the present application encodes the sampling information of the human obstacle and performs parallel decoding operation on all collision query points according to the obstacle features, so that all collision query points can share the obstacle features, thereby determining the collision distances of the collision query points in parallel, greatly reducing the time consumption of collision detection, and further improving the path updating efficiency of the robot, so as to adapt to human-robot collaboration scenarios and avoid human-robot collision risks.

[0050] Compared with other signed distance field models (for example, a discretized signed distance field model based on a voxel grid or an octree), the signed distance field model provided in the present application determines obstacle features by encoding, and all collision query points perform decoding operation in parallel based on common obstacle features (equivalent to aggregating the collision detection of all collision query points into one batch forward propagation), so that the time consumption of collision detection presents a sub-linear characteristic, that is, the time consumption of collision detection does not increase linearly with the number of collision detection points, but remains approximately constant within the parallel processing capacity, finally realizing the purpose of improving the path updating frequency and meeting the real-time requirement in the human-robot collaboration scenario. BRIEF DESCRIPTION OF DRAWINGS

[0051] Figure 1 is an implementation flowchart of the path planning method of the robot provided by the embodiment of the present application;

[0052] Figure 2 is a comparison diagram of the traditional serial collision detection method and the batch collision detection method in the embodiment of the application provided by the embodiment of the application;

[0053] Figure 3 is a collision detection time schematic diagram of the signed distance field model provided by the embodiment of the application;

[0054] Figure 4 is an architecture schematic diagram of the signed distance field model provided by the embodiment of the application;

[0055] Figure 5 is an implementation flowchart of the path planning process provided by the embodiment of the application;

[0056] Figure 6 is a flowchart of the path planning process provided by the embodiment of the application;

[0057] Figure 7 is a flowchart of the batch collision detection provided by the embodiment of the application;

[0058] Figure 8 is a calling time and calling frequency schematic diagram of the signed distance field model under different versions provided by the embodiment of the application;

[0059] Figure 9 is a path planning time schematic diagram under different versions provided by the embodiment of the application;

[0060] Figure 10 is a time consumption schematic diagram of each algorithm module under different versions provided by the embodiment of the application;

[0061] Figure 11 is a time consumption proportion schematic diagram of each algorithm module under different versions provided by the embodiment of the application;

[0062] Figure 12 is a verification scene diagram of the path planning method of the robot provided by the embodiment of the application;

[0063] Figure 13 is a structure schematic diagram of the path planning device of the robot provided by the embodiment of the application. DETAILED DESCRIPTION

[0064] The embodiments of the application will be described in detail below with reference to the accompanying drawings.

[0065] To improve the path update frequency, meet the real-time demand in the dynamic scene, and realize real-time obstacle avoidance for dynamic human obstacles, the embodiment of the application samples the human obstacles, determines the collision query points on the current path of the robot, then performs batch collision detection on the collision query points based on the sampling information by using the signed distance field model, obtains the collision distance, and re-plans the path when the collision distance does not meet the set condition. Here, the signed distance field model adopts a one-time encoding and parallel decoding mode to realize collision detection, so that the collision detection operations of multiple collision query points can be processed in parallel, greatly reducing the time consumption of collision detection, and then improving the path update efficiency of the robot, so as to adapt to the human-robot collaboration scene and avoid human-robot collision risks.

[0066] Referring to Figure 1 which shows an implementation flowchart of the path planning method of the robot provided by the embodiment of the application, and is described in detail as follows:

[0067] Step 101, sampling information of a human obstacle is obtained, and multiple collision query points of the robot on the current planning path are determined.

[0068] Here, the sampling information can include surface sampling points and skeletal key points of the human obstacle, which are used to reflect the current state of the human obstacle, so that subsequent collision detection can be performed based on the current state of the human obstacle.

[0069] In the field of path planning of the robot, the joint configuration in the robot configuration space is usually used to adjust the pose of the robot, and then the path planning is realized. In essence, the planning path of the robot is a plurality of continuous joint configurations. The starting point of the planning path is the initial joint configuration of the robot, and the end point of the planning path is the preset target joint configuration. The planning path connects the initial joint configuration and the preset target joint configuration through a plurality of continuous joint configurations.

[0070] Each joint configuration includes the joint angle of each joint inside the robot. Taking a common six-axis link robot as an example, it has 6 joints inside, and accordingly, a joint configuration can adopt the form of a 6-dimensional vector to represent the joint angle of each joint.

[0071] It can be understood that the human obstacle is located in a three-dimensional space (also referred to as Cartesian space), while the planning path of the robot is composed of joint configurations of the robot configuration space. Therefore, when detecting whether there is a collision risk in the planning path of the robot, the joint configuration in the planning path of the robot can be first converted into a collision query point in the three-dimensional space, so as to perform collision detection in the same space (i.e., the three-dimensional space).

[0072] Here, a batched forward kinematic (FK) algorithm can be used to convert the joint configurations in the planned path into collision query points in three-dimensional space. The embodiment of the present application can uniformly input all joint configurations in the planned path into the batched FK algorithm to uniformly obtain all collision query points of the robot on the current planned path. The specific conversion process of the batched FK algorithm is not expanded here, and will be described later.

[0073] The collision query points of the robot on the current planned path are three-dimensional coordinate points that reflect the pose of the robot in three-dimensional space. The embodiment of the present application detects whether there is a collision risk on the current planned path of the robot by determining the sampling information of the human obstacle and the collision query points of the robot.

[0074] In step 102, based on the sampling information, a batch collision detection is performed on the collision query points using a signed distance field (SDF) model to obtain the collision distance corresponding to each collision query point.

[0075] The mathematical definition of the signed distance field can be expressed as:

[0076] (1)

[0077] wherein, SDF represents a function of the signed distance field, represents the Euclidean distance between the collision query point and the boundary of the obstacle region.

[0078] The above Euclidean distance can be expressed as:

[0079] (2)

[0080] wherein, OB represents the boundary of the obstacle region, represents a point on the boundary of the obstacle region.

[0081] The existing signed distance field model usually adopts a discretized representation method, such as a voxel grid. The distance of each collision query point is determined by calculating the voxel index, interpolation, and reading the distance value of the adjacent voxel. This makes it necessary to independently and completely perform the distance query process for each collision query point, i.e., a serial detection process. That is, the collision detection task of each collision query point is an independent, irregular memory access micro-task when the existing signed distance field model performs collision detection, which leads to an increase in the time consumption of collision detection with the number of collision detection points, i.e., the time consumption of the traditional model presents a linear characteristic. ​

[0082] The embodiment of the present application can input the sampling information of the human obstacle and all the collision query points into the signed distance field model, so that the signed distance field model can perform batch detection on all the collision query points, output the collision distance corresponding to each collision query point, and further determine whether each collision query point has a collision risk.

[0083] The signed distance field model in the embodiment of the present application is used for feature coding of the sampling information, determination of obstacle features, and parallel decoding operation of the collision query points based on the obstacle features to obtain the collision distance corresponding to each collision query point.

[0084] The signed distance field model in the embodiment of the present application mainly includes an encoding stage and a decoding stage. The encoding stage is used for feature coding of the sampling information and determination of obstacle features. The decoding stage is used for parallel decoding operation of all the collision query points according to the obstacle features to obtain the collision distance corresponding to each collision query point. Here, the collision query points can be combined to form a collision query point matrix. The collision query point matrix contains the three-dimensional space coordinates of each collision query point.

[0085] The decoding stage in the embodiment of the present application only needs to decode the collision query point matrix according to the obstacle features. The decoding operation is essentially a forward propagation of a neural network, and the essence of the neural network forward propagation is a cascade matrix multiplication. Therefore, the signed distance field model in the embodiment of the present application can run on a graphics processing unit (GPU), and the parallel processing architecture of the GPU can perform parallel processing on the cascade matrix multiplication, so that all the collision query points can share the same obstacle feature and perform decoding processing synchronously, thereby greatly reducing the time consumption of collision detection and further improving the path update frequency to adapt to the real-time requirements in human-robot collaboration scenarios.

[0086] Referring to Figure 2 In the traditional serial detection process, the FK algorithm needs to be executed once for each joint configuration, and the collision query points output by the FK algorithm are input into the traditional signed distance field model (such as a voxel grid or an octree) to realize one collision detection. For n The joint configuration needs to repeatedly call n times of the traditional signed distance field model, which costs n times of GPU call overhead. However, the embodiment of the present application can uniformly execute a batch FK algorithm once for n times of joint configurations, and input the collision query points output by the FK algorithm into the signed distance field model of the embodiment of the present application to realize batch collision detection. For n times of joint configurations, only 1 time of signed distance field model needs to be called, which costs 1 time of GPU call overhead.

[0087] Since the signed distance field in the embodiment of the application adopts the one-time encoding and parallel decoding mode, and is matched with the GPU parallel architecture, when the parallel processing capacity of the GPU is below, the collision query time of the signed distance field model in the embodiment of the application is approximately constant (i.e. presents sub-linear characteristics):

[0088] (3)

[0089] wherein, represents the query time of the signed distance field provided by the embodiment of the application when the number of collision query points is represents the basic reasoning delay, i.e. the time required for the signed distance field model provided by the embodiment of the application to query one collision query point, represents the number of collision query points corresponding to the parallel processing capacity of the GPU, and in general cases, is in the order of 10 4 .

[0090] Referring to Figure 3 , it is verified by experiments that when the number of collision query points increases from 16 to 10000 within the parallel processing capacity, the number of collision query points increases by 625 times, and the decoding execution time only increases by 1.20 times (0.49ms→0.59ms). That is, the signed distance field model provided by the embodiment of the application has sub-linear characteristics, wherein the GPU parallelism absorbs the additional computational load with minimum delay overhead.

[0091] In some embodiments, the signed distance field model can include a first encoder, a second encoder, a feature splicing module and a decoding module.

[0092] The first encoder can be a PointNet-lite encoder, which is used for feature extraction of surface sampling points to obtain surface geometric features. The surface geometric features are used to reflect the geometric information such as height, fatness and thinness of the human body.

[0093] The second encoder can be a Multi-Layer Perceptron (MLP) encoder, which is used for feature extraction of skeletal key points to obtain human posture features. The human posture features are used to reflect the action posture information of the human body.

[0094] The feature splicing module adopts a hierarchical fusion strategy to splice the surface geometric features and the human posture features to generate obstacle features.

[0095] The decoding module is used for parallel decoding operation of a plurality of collision query points based on the obstacle features to obtain the collision distances corresponding to each collision query point. ​

[0096] The first encoder (PointNet-lite encoder) adopts a lightweight PointNet architecture, which is adapted to the disordered features of surface sampling points. Through three sequentially connected 1D convolutional layers (3→32→64→64) and max pooling layers, the surface sampling points are mapped to 64-dimensional surface geometric features. The first encoder captures the geometric topological information of the human body surface. The second encoder (MLP encoder) adopts a multi-layer fully connected network architecture, specifically three fully connected layers (96→128→64→32), combined with LayerNorm normalization, to output 32-dimensional human pose features. To depict information about changes in human posture.

[0097] The feature stitching module includes a first stitching layer, a fusion MLP layer, and a second stitching layer connected in sequence: the first stitching layer stitches 64-dimensional surface geometric features. and 32-dimensional human posture features The features are concatenated along the feature dimension to obtain a 96-dimensional joint feature. The MLP layer is then used to perform feature fusion and nonlinear mapping on the joint feature to extract cross-modal correlation information and output a 64-dimensional latent code z. Here, the architecture of the MLP layer is: Linear(96,128)→ReLU→Linear(128,64).

[0098] The second splicing layer combines the 64-dimensional hidden code z and the 32-dimensional human pose features. The vectors are concatenated to generate a 96-dimensional conditional vector. This enables deep fusion of geometric and pose information, ensuring that pose information participates in feature representation at multiple levels.

[0099] This condition vector These are the obstacle features, used for batch decoding operations in subsequent decoding modules. The obstacle feature vectors obtained during the encoding process are cached for reuse in the decoding process. For each frame of human obstacle surface sampling points and skeletal keypoints sampled, an encoding process is performed to update the cached obstacle feature vectors; the encoding process takes approximately 0.4ms.

[0100] The decoding module includes a Fieature-wise Linear Modulation (FiLM) parameter generator and a FiLM conditional decoder. The FiLM parameter generator consists of two sequentially connected MLP layers; the FiLM parameter generator is used to generate parameters based on a conditional vector. c Generate 5 sets of scaling parameters and offset parameters (128 dimensions per group), used to modulate the 5 FiLM modulation layers in the FiLM conditional decoder.

[0101] The FiLM conditioned decoder comprises an input projection layer, 5 FiLM modulation layers connected in sequence and an output head. Each FiLM modulation layer is 128-dimensional. The FiLM conditioned decoder first maps the 3-dimensional space query point (i.e. the collision query point) to the 128-dimensional feature space through the input projection layer, each FiLM modulation layer linearly modulates the features in turn, and finally maps the final hidden state to a scalar signed distance value (i.e. a distance vector) through the output head (128→64→1). The decoding operation is always based on the parallel decoding of the obstacle features in the cache, and the time overhead of each decoding process is about 0.5 ms.

[0102] Exemplarily, the surface sampling points in the embodiment of the present application can be represented as , i.e. a total of 3000 surface sampling points, each surface sampling point corresponding to a set of three-dimensional space coordinates. The skeletal key points can be represented as , i.e. a total of 32 skeletal key points, each skeletal key point corresponding to a set of three-dimensional space coordinates. The collision query point matrix composed of all collision query points can be represented as . Wherein, represents the number of collision query points.

[0103] The mathematical expression of the signed distance field model in the embodiment of the present application can be represented as:

[0104] (4)

[0105] Wherein, represents the distance vector, which contains the collision distance corresponding to each collision query point, represents the signed distance field model provided by the embodiment of the present application.

[0106] Referring to Figure 4 , the signed distance field model provided by the embodiment of the present application encodes the surface sampling points and the skeletal key points of the human obstacle using the PointNet encoder and the MLP encoder respectively in the encoding process, and uses the feature splicing module to splice the features to obtain the feature vector for representing the obstacle features. The feature vector obtained in the encoding process is cached for use in the decoding process. In the decoding process, the batch query points (i.e. all collision query points in the collision query point matrix) and the cached feature vector can be input into the decoding module to obtain the batch query value, i.e. the collision distance corresponding to each collision query point.

[0107] Here, it's important to clarify that for each frame of human obstacle surface sampling points and skeletal keyword points sampled, an encoding process is performed to update the feature vector in the cache. Each encoding process takes approximately 0.4ms. Decoding operations are always performed in parallel based on the cached feature vectors. Each decoding process takes approximately 0.5ms.

[0108] Step 103: If the collision distance of any collision query point does not meet the set conditions, a new planning path is re-determined based on the robot's current joint configuration and sampling information so that the robot can move according to the new planning path.

[0109] The set conditions in this embodiment of the invention can be expressed as:

[0110] (5)

[0111] in, Indicates the first The collision distance corresponding to each collision query point This indicates the preset collision safety margin. For example, You can take 4cm.

[0112] If any collision query point in the robot's current planned path does not conform to the above formula (5), it can be determined that the current planned path has a collision risk. In this embodiment of the invention, a new planned path without collision risk can be re-determined based on the robot's current joint configuration and human obstacle sampling information.

[0113] This invention can periodically acquire sampling information of human obstacles at a fixed frequency (e.g., 30Hz) and perform collision detection to determine whether a path update is needed. When a path update is determined to be necessary, a path update operation is performed to achieve real-time adjustment of the robot's planned path based on human obstacles. Experimental verification shows that the method provided by this invention can support real-time distance monitoring up to 100Hz.

[0114] Compared to existing technologies, the signed distance field model in this embodiment of the invention encodes the sampling information of human obstacles and performs parallel decoding operations on all collision query points according to the obstacle features, so that all collision query points can share obstacle features and thus determine the collision distance of each collision query point in parallel, thereby significantly reducing the time consumption of collision detection and improving the path update efficiency of the robot, so as to adapt it to human-machine collaboration scenarios and avoid the risk of human-machine collision.

[0115] Compared with other signed distance field models (for example, voxel grid or octree, etc.), the signed distance field model provided in the application determines the obstacle feature through encoding, all collision query points perform decoding operations in parallel based on the common obstacle feature (equivalent to aggregating the collision detection of all collision query points into a batch forward propagation), so that the time overhead of collision detection presents a sub-linear characteristic, that is, the time overhead of collision detection does not increase linearly with the number of collision detection points, but remains approximately constant within the parallel processing capacity, ultimately achieving the purpose of improving the path update frequency, meeting the real-time demand in the human-robot collaboration scenario.

[0116] In the related art, the Batch Informed Trees (BIT*) algorithm is mostly used for path planning. The traditional BIT* algorithm is a "sampling-detection-expansion" loop architecture used for searching an optimal planning path. In order to adapt to the sub-linear characteristic of the signed distance field model provided in the embodiments of the application and save time overhead, the embodiments of the application improve the traditional BIT* algorithm to form a "batch sampling-batch detection-batch expansion" loop architecture. The improved BIT* algorithm is defined as the Batch-BIT* algorithm in the embodiments of the application.

[0117] The following will be described in combination with Figure 5 The Batch-BIT* algorithm (i.e., the path planning method of the robot) will be described in detail.

[0118] In step 501, the sampling information is encoded by using the signed distance field model to determine the obstacle feature.

[0119] Here, considering that in the process of planning the path of the robot, collision detection needs to be performed for each candidate sample (i.e., a candidate point to be included in the planning path), and the candidate sample is included in the planning path only after it is determined that there is no collision risk, therefore, the embodiments of the application can pre-call the signed distance field to encode the sampling information of the human obstacle, and cache the obtained obstacle feature, so that the obstacle feature can be called and decoded in real time during the algorithm iteration process to realize collision detection, without repeatedly encoding in each algorithm iteration process, thereby saving time overhead.

[0120] In step 502, the search tree, the cumulative cost of each node in the search tree, and the optimal path cost are initialized.

[0121] Like the traditional BIT* algorithm, the Batch-BIT* algorithm also uses the method of updating the search tree to continuously expand the joint configuration in the planning path, and ultimately obtains a plurality of complete planning paths, and determines the optimal planning path from the planning paths according to the path cost of each planning path.

[0122] Here, the nodes in the search tree are used to represent the joint configurations of the robot, and by constantly expanding the nodes in the search tree, the purpose of constantly expanding the planned path can be achieved. In the algorithm initialization stage, the current joint configuration of the robot can be initialized as the root node of the search tree. The cumulative cost of the root node can be initialized as 0. The optimal path cost can be initialized as .

[0123] Step 503, based on the current joint configuration, the preset target joint configuration and the optimal path cost, determine the ellipsoid sampling region, and batch generate a plurality of candidate samples in the ellipsoid sampling region.

[0124] The current joint configuration is the starting point of the planned path, that is, the root node in the search tree. The preset target joint configuration is the end point of the planned path, that is, the target node in the search tree. In the embodiment of the application, the current joint configuration and the preset target joint configuration can be taken as the focus, and the optimal path cost is taken as the constraint, so as to determine an ellipsoid sampling region:

[0125] (6)

[0126] Wherein, represents the ellipsoid sampling region, represents the joint configuration point in the ellipsoid sampling region, represents the current joint configuration to the joint configuration point between the distance, represents the distance between the joint configuration point and the preset target joint configuration , represents the optimal path cost. In the embodiment of the application, the cumulative sum of the distance between each two adjacent joint configuration points in the planned path is taken as the path cost of the planned path.

[0127] In order to adapt to the batch detection characteristics of the signed distance field model, which is different from the traditional BIT* algorithm, the embodiment of the application uniformly generates candidate samples (i.e. joint configuration points) in the ellipsoid sampling region, so as to realize batch sampling:

[0128] (7)

[0129] Wherein, represents the candidate sample set, represents the first candidate sample, represents the second candidate sample, represents the candidate sample, , for example, can be 50, representing the first candidate sample, .

[0130] Step 504, batch computing distance information between all nodes in the search tree and all candidate samples, to obtain a distance matrix.

[0131] Different from the tree operation method of point-by-point nearest neighbor search of the traditional BIT* algorithm, the embodiment of the application adopts a vectorized tree operation method for batch nearest neighbor search in a vectorized manner, which specifically comprises: batch computing distance information between all nodes in the search tree and all candidate samples to obtain a distance matrix, and determining a candidate parent node for each candidate sample based on the distance matrix.

[0132] Here, the distance matrix can be represented as:

[0133] (8)

[0134] wherein, the distance matrix contains distance information between each node and each candidate sample, the candidate sample vector composed of all candidate samples contains the joint configuration corresponding to each candidate sample, the node vector composed of all nodes contains the joint configuration corresponding to each node in the search tree, the number of nodes in the search tree.

[0135] Step 505, based on the distance matrix and the cumulative cost of each node in the search tree, determining a candidate parent node for each candidate sample from all nodes in the search tree.

[0136] In the embodiment of the application, for each candidate sample, a set of neighboring nodes adjacent to the candidate sample can be determined from all nodes in the search tree based on the distance matrix; then, for each node in the set of neighboring nodes, the cumulative cost corresponding to the candidate sample when the node is the candidate parent node is calculated; finally, the node corresponding to the minimum value of the cumulative cost is determined as the candidate parent node corresponding to the candidate sample.

[0137] Here, for each candidate sample, the nodes with distance information less than a set value from the candidate sample can be determined according to the distance matrix, and the nodes are included in the set of neighboring nodes as the neighboring nodes of the candidate sample.

[0138] In the embodiment of the application, the candidate parent node corresponding to each candidate sample can be determined according to the following formula:

[0139] (9)

[0140] wherein, represents a candidate parent node corresponding to the i th candidate sample, represents a candidate parent node corresponding to the i th candidate sample, represents a set of neighboring nodes corresponding to the i th candidate sample, represents a cumulative cost of the i th node, represents an element value in the i th row and the j th column of the distance matrix, that is, distance information between the i th candidate sample and the j th node.

[0141] Step 506: Based on each candidate sample and the candidate parent node corresponding thereto, a group of interpolation points is determined, and a parallel decoding operation is performed on each group of interpolation points and the candidate sample corresponding thereto based on the obstacle feature by using the signed distance field model, so as to obtain the collision distance corresponding to each group of interpolation points and the candidate sample corresponding thereto.

[0142] Different from the traditional BIT* algorithm, in the embodiment of the present application, a batch interpolation manner is adopted when interpolating between the sample and the candidate parent node corresponding thereto, so as to adapt to the batch detection characteristic of the signed distance field model.

[0143] In some embodiments, a connection line between each candidate sample and the candidate parent node corresponding thereto can be determined as a candidate edge, then a plurality of interpolation points are inserted at equal intervals on the candidate edge, and the plurality of interpolation points are taken as a group of interpolation points.

[0144] The joint configuration corresponding to the interpolation point can be represented as:

[0145] (10)

[0146] wherein, represents a joint configuration corresponding to the i th interpolation point between the candidate sample and the candidate parent node, represents a joint configuration corresponding to the candidate parent node, represents a joint configuration corresponding to the candidate sample, represents a joint configuration corresponding to the candidate sample, represents the total number of interpolation points between the candidate sample and the candidate parent node.

[0147] In the embodiment of the present application, all interpolation points and candidate samples are aggregated and uniformly subjected to batch collision detection. Before batch collision detection, the batch FK algorithm can be used to convert all interpolation points and candidate samples into collision query points in a three-dimensional space.

[0148] ​​​​​In some embodiments, the interpolation points and the corresponding candidate samples are converted into a collision query point matrix by using a batched forward kinematics algorithm, and the collision query point matrix is input into the signed distance field model, so that the signed distance field model performs a parallel decoding operation on the collision query point matrix based on the obstacle features to obtain a distance vector corresponding to the collision query point matrix. Here, the distance vector contains the collision distances corresponding to each group of interpolation points and the corresponding candidate samples.

[0149] It should be noted that each interpolation point or candidate sample corresponds to a group of joint configurations in the robot configuration space, and each group of joint configurations in the robot configuration space corresponds to a plurality of collision query points in the three-dimensional space. The embodiments of the present application can use a batched FK algorithm to convert each interpolation point and candidate sample into a collision query point in the three-dimensional space and form a collision query point matrix. Here, the collision query point matrix contains a plurality of collision query points and their corresponding three-dimensional space coordinates.

[0150] In the embodiments of the present application, all the detected collision query points are merged into the same batch, i.e., a complete collision query point matrix, and are input into the decoding module in the signed distance field model for parallel decoding operation to realize batch collision detection and finally obtain the collision distances corresponding to each collision query point. It can be understood that the collision query points in the three-dimensional space correspond to the interpolation points and candidate samples in the robot configuration space, and based on the collision distances corresponding to each collision query point, the collision distances corresponding to the interpolation points and candidate samples can be determined.

[0151] In step 507, if the collision distances corresponding to the interpolation points and the corresponding candidate samples meet the set conditions, the candidate sample is determined as a new node in the search tree, and the cumulative cost of the new node is determined.

[0152] It can be understood that each candidate sample corresponds to a group of interpolation points, and if the collision distances of the candidate sample and the corresponding interpolation points both meet the set conditions, i.e., the above formula (5), the candidate sample can be expanded as a new node in the search tree, and the cumulative cost of the new node is calculated.

[0153] Here, the cumulative cost of the new node is the sum of the cumulative cost of the corresponding candidate parent node and the distance information between the node and the candidate parent node. The formula is represented as: . Wherein, represents the cumulative cost of the candidate parent node, represents the distance information between the new node and the candidate parent node.

[0154] At step 508, based on the accumulated cost of the new node, the optimal path cost is updated, and based on the updated optimal path cost, the step of determining the ellipsoid sampling region based on the current joint configuration, the preset target joint configuration and the optimal path cost is jumped to perform until the algorithm termination condition is reached, the path corresponding to the current optimal path cost is determined as the new planning path.

[0155] For each new node, the distance information between the new node and the target node can be calculated respectively, and the sum of the accumulated cost of the new node and the distance information is taken as the path cost of the planning path corresponding to the new node. In the embodiment of the application, it is detected whether the minimum value in the path cost is less than the current optimal path cost, if yes, the minimum value in the path cost is updated as the new optimal path cost, and the ellipsoid sampling region is re-determined based on the new optimal path cost, and the iteration is performed until the algorithm termination condition is reached, and the path corresponding to the current optimal path cost is determined as the new planning path.

[0156] Here, it needs to be clear that if the distance information between any new node and the target node is greater than the set threshold, it is determined that the node cannot be directly connected with the target node, at this time, the next iteration needs to be entered to continue to expand the new node, until there is a new node that can be connected with the target node, it is determined that the algorithm termination condition is reached. At this time, the path corresponding to the optimal path cost is selected from all the paths that can connect the target node as the new planning path.

[0157] After obtaining the new planning path, path pruning and B-spline post-processing can be used for path smoothing to ensure that the new planning path trajectory is smooth and executable.

[0158] Referring to Figure 6 The Batch-BIT* algorithm in the embodiment of the application initializes the ellipsoid sampling region, batch-samples the candidate samples, calls the batch FK algorithm to perform vectorization calculation to obtain the collision query point matrix, calls the SDF model to perform batch collision detection, and performs vectorization tree operation to expand the nodes in batches to update the planning path until the target node is reached, and the optimal path is output. The Batch-BIT* algorithm in the embodiment of the application improves the traditional BIT* algorithm, and can realize batch sampling, batch collision detection, batch nearest neighbor search and batch node updating based on the BIT* algorithm, so that the Batch-BIT* algorithm can adapt to the batch detection characteristics of the signed distance field model, save time overhead, and further improve the path updating efficiency.

[0159] In some embodiments, the specific steps of converting each group of interpolation points and the corresponding candidate samples into a collision query point matrix by using the batch forward kinematics algorithm are as follows:

[0160] Step A, aggregate the joint configurations corresponding to each set of interpolation points and their corresponding candidate samples into a joint configuration matrix.

[0161] Here, the joint configuration matrix contains multiple sets of joint configurations, each set of joint configurations corresponding to an interpolation point or a candidate sample. Each set of joint configurations contains the joint angles of each joint inside the robot; taking a common six-axis link robot as an example, it has six joints inside, accordingly, a set of joint configurations can take the form of a 6-dimensional vector, representing the joint angles of each joint.

[0162] In the embodiments of the present application, the joint configuration matrix can be expressed as:

[0163] (11)

[0164] In the formula, represents the joint configuration matrix, represents the number of joint configurations, represents the number of joints inside the robot, respectively represent the first set of joint configurations, the second set of joint configurations, and the set of joint configurations, represents the transpose factor, represents the first set of joint configurations, respectively represent the joint angles of each joint inside the robot.

[0165] Step B, based on the joint angles of each joint inside the robot in the joint configuration matrix, determine the transformation matrix corresponding to each joint inside the robot under all joint configurations.

[0166] In some embodiments, for each joint inside the robot, based on the joint angles of the joint in all joint configurations, calculate the relative transformation of the joint in all joint configurations, and then based on the transformation matrix of the previous joint and the relative transformation of the joint in all joint configurations, determine the transformation matrix of the joint under all joint configurations.

[0167] Here, for each joint inside the robot, the relative transformation of the joint in all joint configurations can be calculated by vectorizing the trigonometric function.

[0168] The embodiments of the present application pre-set the base coordinate system, that is, the unit transformation matrix of all joints:

[0169] (12)

[0170] In the formula, represents the base coordinate system, represents the dimension of The identity matrix. Here, the identity transformation matrix for each joint configuration is a 4×4 identity matrix, and the identity transformation matrices for all joint configurations together constitute the aforementioned base coordinate system.

[0171] Next, the transformation matrix for each joint is calculated recursively:

[0172] (13)

[0173] In the formula, Indicates the first joint configuration among all joint configurations. Transformation matrix of each joint, Indicates the first joint configuration among all joint configurations. Transformation matrix of each joint, This indicates that the first joint configuration is calculated using vectorized trigonometric functions. The relative transformation of each joint.

[0174] Step C: Extract the three-dimensional spatial coordinates of multiple collision detection points under all joint configurations from the transformation matrices corresponding to each joint inside the robot under all joint configurations, and obtain the collision query point matrix.

[0175] In this embodiment of the invention, the transformation matrix corresponding to each joint inside the robot under all joint configurations can be represented as follows: In the embodiments of the present invention, Defined as a batch link transformation matrix.

[0176] Collision query points can be extracted from the above batch link transformation matrix through vectorization operations. For example, in this embodiment of the invention, the link center point, tool axis point and intermediate point are extracted for each group of joint configurations as the corresponding collision query points for each group of joint configurations.

[0177] Formula for extracting the center point of the connecting rod: (14)

[0178] In the formula, This represents the three-dimensional spatial coordinates of the center point of the link in all joint configurations. This represents the set of link indices to be detected. This is used to extract the three-dimensional spatial coordinates of the center point of each link in the link index set of all joint configurations. Here, each link can be simply understood as a joint inside the robot.

[0179] Tool pivot point extraction formula: (15)

[0180] In the formula, This represents the three-dimensional spatial coordinates of the tool axis point in all joint configurations. Indicates the first Index of the joint, i.e., index of the end link, in the six-link robot = 6, Coordinates of the origin of the end link coordinate system are extracted, Denotes the tool axis interpolation weight, , Denotes the tool length.

[0181] Intermediate point extraction formula: (16)

[0182] In the formula, Denotes the three-dimensional space coordinates of the intermediate point in all joint configurations, where the intermediate point refers to the midpoint coordinates of the line connecting the origins of the adjacent two links, which is used to approximately represent the geometric position of the middle part of the link, The origin coordinates of the first link to the last link, The origin coordinates of the 0th link to the second last link are extracted.

[0183] In the embodiment of the present application, the above batch FK algorithm can be used to correspondingly output a collision query point matrix . In the formula, Denotes the number of collision query points corresponding to each group of joint configurations.

[0184] Referring to Figure 7 , the embodiment of the present application inputs group of joint configurations into the batch FK algorithm for parallel calculation, and correspondingly obtains the collision detection point matrix output by the FK algorithm. The collision detection point matrix is aggregated as a single batch to be sent to the SDF model for batch collision detection to obtain the collision detection result. Exemplarily, Figure 7 Each group of joint configurations corresponds to 16 collision query points.

[0185] Based on 5 versions of path planning methods x 3 path planning scenes x 300 times / configuration, 4500 experiments are performed to illustrate the effect of the path planning method provided by the embodiment of the present application. Among them, the 5 versions of path planning methods respectively include:

[0186] • V0-Baseline: traditional serial signed distance field model + traditional FK algorithm + traditional BIT* algorithm;

[0187] • V1-VectorizedSampling: based on the V0 version, the batch sampling improvement for the BIT* algorithm is added;

[0188] • V2-VectorizedTree: based on the V1 version, the batch nearest neighbor search improvement for the BIT* algorithm is added;

[0189] • V3-VecFK: On the basis of V2 version, the batched FK algorithm is added, and the batched generation of interpolation points of BIT* algorithm is improved

[0190] • V4-Full: On the basis of V3, the batched collision detection improvement of signed distance field model provided by the embodiment of the application is added, that is, the path planning method provided by the embodiment of the application.

[0191] The final comprehensive ablation experiment conclusion is as follows:

[0192] 1. Significant overall acceleration: path planning time is reduced from 2,645 ms to 119 ms, achieving 22.2 times acceleration;

[0193] 2. Extreme collision detection acceleration: collision detection time is reduced from 2,199 ms to 19 ms, achieving 116 times acceleration;

[0194] 3. SDF model call is greatly reduced: SDF model call times are reduced from 3,006 times to 16 times, 188 times reduction; SDF call time and call times under different versions can be seen in Figure 8 .

[0195] 4. Bottleneck identification is crucial: V3-VecFK only optimizes FK (180 ms→22 ms, 8 times) and only obtains 1.27 times overall acceleration because SDF still accounts for 96%; only V4-Full realizes 22 times acceleration through batched SDF; path planning time under different versions can be seen in Figure 9 .

[0196] 5. Computing load balancing: V4-Full realizes balanced load distribution—FK algorithm (32%), other (34%), search tree operation (18%), SDF model (16%); time overhead and proportion of each algorithm module under different versions can be seen in Figure 10 and Figure 11 .

[0197] 6. Support dynamic scene: batched SDF query combined with pose estimation supports 30Hz real-time obstacle monitoring;

[0198] 7. Improve planning success rate: success rate is improved from 88.1% to 92.4% (faster planning allows more iterations before timeout);

[0199] 8. Path quality remains: all versions produce comparable path quality (joint space length is about 4.87-5.05 rad).

[0200] Reference Figure 12, the six-axis link robot moves from point A to point B, and the human body is located in the middle of the path and raises the arm to form an obstacle. Based on the path planning method provided by the embodiment of the application, the robot successfully completes re-planning within 119 milliseconds, and the robot safely reaches the target point along the bypass trajectory, maintaining a safety distance of more than 4 cm from the surface of the human body throughout the journey.

[0201] It should be understood that the size of the serial number of each step in the above embodiment does not mean the order of execution, and the execution order of each process should be determined according to its function and internal logic, and should not constitute any limitation on the implementation process of the embodiment of the application.

[0202] The following is a device embodiment of the application, and for details not described in detail, reference can be made to the corresponding method embodiments described above.

[0203] Figure 13 The structure of the path planning device of the robot provided by the embodiment of the application is shown, only the parts related to the embodiment of the application are shown for the convenience of description, and the details are as follows:

[0204] As shown in Figure 13 , the path planning device 13 of the robot includes an acquisition module 131, a detection module 132 and a planning module 133.

[0205] The acquisition module 131 is configured to acquire sampling information of a human obstacle and determine a plurality of collision query points of the robot on a current planning path.

[0206] The detection module 132 is configured to perform batch collision detection on the collision query points based on the sampling information by using a signed distance field model to obtain collision distances corresponding to the collision query points; the signed distance field model is configured to encode features of the sampling information, determine obstacle features, and perform parallel decoding operations on the collision query points based on the obstacle features to obtain the collision distances corresponding to the collision query points.

[0207] The planning module 133 is configured to, if the collision distance of any collision query point does not meet a set condition, determine a new planning path based on a current joint configuration of the robot and the sampling information, so that the robot moves according to the new planning path.

[0208] In a possible implementation, the sampling information includes surface sampling points and skeletal key points of the human obstacle.

[0209] The signed distance field model includes a first encoder, a second encoder, a feature splicing module and a decoding module.

[0210] The first encoder is configured to extract features of the surface sampling points to obtain surface geometric features.

[0211] a second encoder configured to perform feature extraction on the skeleton key points to obtain human pose features;

[0212] a feature splicing module configured to splice the surface geometric features and the human pose features to obtain obstacle features;

[0213] a decoding module configured to perform parallel decoding operations on the plurality of collision query points based on the obstacle features to obtain collision distances corresponding to the collision query points.

[0214] In a possible implementation, the planning module 133 is specifically configured to:

[0215] perform feature encoding on the sampling information by using a signed distance field model to determine obstacle features;

[0216] initialize a search tree, accumulated costs of nodes in the search tree, and an optimal path cost; the nodes in the search tree are used to represent joint configurations of the robot;

[0217] determine an ellipsoid sampling region based on a current joint configuration, a preset target joint configuration, and the optimal path cost, and batch generate a plurality of candidate samples in the ellipsoid sampling region;

[0218] batch calculate distance information between all nodes in the search tree and all candidate samples to obtain a distance matrix; the distance matrix includes distance information between each node and each candidate sample;

[0219] determine a candidate parent node for each candidate sample from all nodes in the search tree based on the distance matrix and the accumulated costs of the nodes in the search tree;

[0220] based on each candidate sample and the corresponding candidate parent node, determine a group of interpolation points, and perform parallel decoding operations on the groups of interpolation points and the corresponding candidate samples based on the obstacle features by using the signed distance field model to obtain collision distances corresponding to the groups of interpolation points and the corresponding candidate samples;

[0221] if the collision distances corresponding to the interpolation points and the corresponding candidate samples meet a set condition, the candidate sample is determined as a new node in the search tree, and an accumulated cost of the new node is determined;

[0222] update the optimal path cost based on the accumulated cost of the new node, and jump to perform the step of determining the ellipsoid sampling region based on the current joint configuration, the preset target joint configuration, and the optimal path cost based on the updated optimal path cost until an algorithm termination condition is reached, and a path corresponding to the current optimal path cost is determined as a new planning path.

[0223] In a possible implementation, the planning module 133 is specifically configured to:

[0224] a connection between each candidate sample and its corresponding candidate parent node is determined as a candidate edge;

[0225] a plurality of interpolation points are inserted into the candidate edge at equal intervals, and the plurality of interpolation points are taken as a set of interpolation points;

[0226] the interpolation points correspond to joint configurations, and the joint configurations are determined as: ;

[0227] wherein, represents a joint configuration corresponding to an i-th interpolation point between the candidate sample and the candidate parent node, represents a joint configuration corresponding to the candidate parent node, represents a joint configuration corresponding to the candidate sample, represents a joint configuration corresponding to the candidate sample, represents a total number of interpolation points between the candidate sample and the candidate parent node.

[0228] In a possible implementation, the planning module 133 is specifically configured to:

[0229] for each candidate sample, determine a set of neighboring nodes adjacent to the candidate sample from all nodes of the search tree based on the distance matrix;

[0230] for each node in the set of neighboring nodes, calculate a cumulative cost corresponding to the candidate sample when the node is taken as a candidate parent node;

[0231] determine a node corresponding to a minimum value of the cumulative cost as the candidate parent node corresponding to the candidate sample.

[0232] In a possible implementation, the planning module 133 is specifically configured to:

[0233] convert each set of interpolation points and its corresponding candidate sample into a collision query point matrix by using a batched forward kinematics algorithm; each interpolation point or candidate sample corresponds to a set of joint configurations in a robot configuration space, and each set of joint configurations in the robot configuration space corresponds to a plurality of collision query points in a three-dimensional space; the collision query point matrix comprises a plurality of collision query points and their corresponding three-dimensional space coordinates;

[0234] input the collision query point matrix into the signed distance field model, so that the signed distance field model performs a parallel decoding operation on the collision query point matrix based on obstacle features to obtain a distance vector corresponding to the collision query point matrix; the distance vector comprises collision distances corresponding to each set of interpolation points and its corresponding candidate sample.

[0235] In a possible implementation, the planning module 133 is specifically configured to:

[0236] The joint configurations corresponding to each group of interpolation points and the corresponding candidate samples are aggregated into a joint configuration matrix; the joint configuration matrix contains multiple groups of joint configurations, and each group of joint configurations contains joint angles of joints inside the robot;

[0237] Based on the joint angles of the joints inside the robot in the joint configuration matrix, the transformation matrices corresponding to the joints inside the robot under all joint configurations are determined;

[0238] From the transformation matrices corresponding to the joints inside the robot under all joint configurations, the three-dimensional space coordinates of the plurality of collision detection points under all joint configurations are extracted to obtain a collision query point matrix.

[0239] In a possible implementation, the planning module 133 is specifically configured to:

[0240] For each joint inside the robot, based on the joint angles of the joint in all joint configurations, the relative transformation of the joint in all joint configurations is calculated;

[0241] Based on the transformation matrix of the previous joint and the relative transformation of the joint in all joint configurations, the transformation matrix of the joint under all joint configurations is determined.

[0242] The device embodiment can be used to implement the above method embodiments, and the technical principles and implementation effects are the same as those of the above method embodiments, which will not be described here.

[0243] The embodiment of the application also provides a robot comprising a memory and a processor, the memory stores a computer program, and the processor implements the method in the above method embodiment when executing the computer program.

[0244] In the above embodiments, the description of each embodiment has its own emphasis, and the parts not described or recorded in a certain embodiment can be referred to the related description of other embodiments. If there is no special description and no logical conflict, the terms and / or descriptions of different embodiments are consistent and can be mutually referenced. The technical features in different embodiments can be combined to form new embodiments according to their inherent logical relationship.

[0245] The above-described embodiments are only used to illustrate the technical solutions of the present application, rather than limit them; although the present application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that they can modify the technical solutions recorded in the foregoing embodiments, or make equivalent replacements for part of the technical features; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the present application, and should be included in the protection scope of the present application.

Claims

1. A method of path planning for a robot, characterized by, The method comprises the following steps: Obtaining sampling information of a human obstacle and determining a plurality of collision query points of the robot on a current planning path; Based on the sampling information, using a signed distance field model to perform batch collision detection on the collision query points to obtain the collision distance corresponding to each collision query point; the signed distance field model is used to encode the features of the sampling information, determine the obstacle features, and perform parallel decoding operation on the collision query points based on the obstacle features to obtain the collision distance corresponding to each collision query point; If the collision distance of any collision query point does not meet the set condition, a new planning path is determined based on the current joint configuration of the robot and the sampling information, so that the robot moves according to the new planning path; The sampling information includes surface sampling points and skeletal key points of the human obstacle; The signed distance field model includes a first encoder, a second encoder, a feature splicing module and a decoding module; The first encoder is used to extract features of the surface sampling points to obtain surface geometric features; The second encoder is used to extract features of the skeletal key points to obtain human posture features; The feature splicing module is used to splice the surface geometric features and the human posture features to obtain the obstacle features; The decoding module is used to perform parallel decoding operation on the plurality of collision query points based on the obstacle features to obtain the collision distance corresponding to each collision query point; The new planning path is determined based on the current joint configuration of the robot and the sampling information, which comprises: Using a signed distance field model to encode the features of the sampling information to determine the obstacle features; Initializing a search tree, the cumulative cost of each node in the search tree, and the optimal path cost; the nodes in the search tree are used to represent the joint configuration of the robot; Based on the current joint configuration, the preset target joint configuration and the optimal path cost, an ellipsoid sampling region is determined, and a plurality of candidate samples are generated in the ellipsoid sampling region; Batch calculating the distance information between all nodes in the search tree and all candidate samples to obtain a distance matrix; the distance matrix contains the distance information between each node and each candidate sample; Based on the distance matrix and the cumulative cost of each node in the search tree, a candidate parent node is determined for each candidate sample from all nodes in the search tree; Based on each candidate sample and its corresponding candidate parent node, a group of interpolation points are determined, and the signed distance field model is used to perform parallel decoding operation on each group of interpolation points and its corresponding candidate sample based on the obstacle features to obtain the collision distance corresponding to each group of interpolation points and its corresponding candidate sample; If the collision distance corresponding to the interpolation point and its corresponding candidate sample meets the set condition, the candidate sample is determined as a new node in the search tree, and the cumulative cost of the new node is determined. updating the optimal path cost based on the accumulated cost of the new node, and performing, based on the updated optimal path cost, the step of determining the ellipsoid sampling region based on the current joint configuration, a preset target joint configuration, and the optimal path cost until an algorithm termination condition is reached, and determining a path corresponding to the current optimal path cost as a new planning path.

2. The method of path planning for a robot according to claim 1, wherein, The step of determining a set of interpolation points based on each candidate sample and its corresponding candidate parent node comprises: determining a connection between each candidate sample and its corresponding candidate parent node as a candidate edge; inserting multiple interpolation points at equal intervals on the candidate edge, and taking the multiple interpolation points as a set of interpolation points; The joint configuration corresponding to the interpolation point is: ; wherein, represents the joint configuration of the candidate parent node, represents the joint configuration of the candidate sample, represents the joint configuration of the candidate parent node, represents the joint configuration of the candidate sample, represents the total number of interpolation points between the candidate sample and the candidate parent node.

3. The method of claim 1, wherein, The step of determining a candidate parent node for each candidate sample from all nodes of the search tree based on the distance matrix and the accumulated cost of each node in the search tree comprises: For each candidate sample, determining a set of neighboring nodes adjacent to the candidate sample from all nodes of the search tree based on the distance matrix; For each node in the set of neighboring nodes, calculating the accumulated cost of the candidate sample when the node is used as a candidate parent node; determining the node corresponding to the minimum value of the accumulated cost as the candidate parent node corresponding to the candidate sample.

4. The method of claim 1, wherein, The step of performing parallel decoding operations on each set of interpolation points and its corresponding candidate sample based on the obstacle feature using the signed distance field model to obtain the collision distance corresponding to each set of interpolation points and its corresponding candidate sample comprises: using a batch forward kinematics algorithm to convert each set of interpolation points and its corresponding candidate sample into a collision query point matrix; each interpolation point or candidate sample corresponds to a set of joint configurations in the robot configuration space, and each set of joint configurations in the robot configuration space corresponds to a plurality of collision query points in the three-dimensional space; the collision query point matrix contains a plurality of collision query points and their corresponding three-dimensional space coordinates; inputting the collision query point matrix into the signed distance field model to enable the signed distance field model to perform parallel decoding operations on the collision query point matrix based on the obstacle feature to obtain a distance vector corresponding to the collision query point matrix; the distance vector contains the collision distance corresponding to each set of interpolation points and its corresponding candidate sample.

5. The method of path planning for a robot according to claim 4, wherein, The step of converting each set of interpolation points and its corresponding candidate sample into a collision query point matrix using a batch forward kinematics algorithm comprises: aggregating the joint configurations corresponding to each set of interpolation points and its corresponding candidate sample into a joint configuration matrix; the joint configuration matrix contains a plurality of sets of joint configurations, and each set of joint configurations contains the joint angles of the joints inside the robot; determining the transformation matrix corresponding to each joint inside the robot under all joint configurations based on the joint angles of the joints inside the robot in the joint configuration matrix; extracting the three-dimensional space coordinates of the plurality of collision detection points under all joint configurations from the transformation matrix corresponding to each joint inside the robot to obtain the collision query point matrix.

6. The method of path planning for a robot according to claim 5, wherein, The step of determining the transformation matrix corresponding to each joint inside the robot under all joint configurations based on the joint angles of the joints inside the robot in the joint configuration matrix comprises: For each joint inside the robot, based on the joint angle of the joint in all joint configurations, a relative transformation of the joint in all joint configurations is calculated; Based on the transformation matrix of the previous joint and the relative transformation of the joint in all joint configurations, a transformation matrix of the joint in all joint configurations is determined.

7. A path planning device of a robot characterized by comprising: Comprise: An acquisition module is configured to acquire sampling information of a human obstacle and determine a plurality of collision query points of the robot on a current planning path; A detection module is configured to perform batch collision detection on the collision query points based on the sampling information by using a signed distance field model to obtain collision distances corresponding to each collision query point; the signed distance field model is configured to encode features of the sampling information, determine obstacle features, and perform parallel decoding operations on the collision query points based on the obstacle features to obtain collision distances corresponding to each collision query point; A planning module is configured to, if the collision distance of any collision query point does not meet a set condition, determine a new planning path based on a current joint configuration of the robot and the sampling information, so that the robot moves according to the new planning path; The sampling information comprises surface sampling points and skeletal key points of the human obstacle; The signed distance field model comprises a first encoder, a second encoder, a feature splicing module, and a decoding module; The first encoder is configured to extract features of the surface sampling points to obtain surface geometric features; The second encoder is configured to extract features of the skeletal key points to obtain human posture features; The feature splicing module is configured to splice the surface geometric features and the human posture features to obtain the obstacle features; The decoding module is configured to perform parallel decoding operations on the plurality of collision query points based on the obstacle features to obtain collision distances corresponding to each collision query point; The determination of the new planning path based on the current joint configuration of the robot and the sampling information comprises: Encoding features of the sampling information by using the signed distance field model to determine obstacle features; Initializing a search tree, accumulated costs of nodes in the search tree, and an optimal path cost; the nodes in the search tree are used to represent joint configurations of the robot; Determining an ellipsoid sampling region based on the current joint configuration, a preset target joint configuration, and the optimal path cost, and batch generating a plurality of candidate samples in the ellipsoid sampling region; Batch calculating distance information between all nodes in the search tree and all candidate samples to obtain a distance matrix; the distance matrix comprises distance information between each node and each candidate sample; Based on the distance matrix and the accumulated costs of the nodes in the search tree, determining a candidate parent node for each candidate sample from all nodes in the search tree; Based on each candidate sample and its corresponding candidate parent node, a group of interpolation points are determined, and parallel decoding operations are performed on each group of interpolation points and its corresponding candidate sample based on the obstacle features by using the signed distance field model to obtain collision distances corresponding to each group of interpolation points and its corresponding candidate sample. If the collision distance corresponding to the interpolation point and the corresponding candidate sample meets the set condition, the candidate sample is determined as a new node in the search tree, and the accumulated cost of the new node is determined; Based on the accumulated cost of the new node, the optimal path cost is updated, and based on the updated optimal path cost, the step of determining the ellipsoid sampling region based on the current joint configuration, the preset target joint configuration and the optimal path cost is performed until the algorithm termination condition is reached, and the path corresponding to the current optimal path cost is determined as a new planning path.

8. A robot, characterized in that A computer readable storage medium storing a computer program, wherein the computer program is configured to implement the method according to any one of claims 1 to 6 when executed by a processor. A computer readable storage medium storing a computer program, wherein the computer program is configured to implement the method according to any one of claims 1 to 6 when executed by a processor.

Images