Robot assembly space obstacle avoidance method and system applied to 3D perception

Abstract

The invention provides a robot assembly space obstacle avoidance method and system applied to 3D perception, and relates to the technical field of robot automatic assemblation.The method comprises the steps that 3D assembly space data of a robot assembly area are obtained firstly, and the 3D assembly space data comprise assembly targets, static obstacles, dynamic obstacles and related information of a robot end effector; extracting spatial obstacle features from the data, establishing an initial obstacle association relationship, dynamically updating the initial association relationship, generating a real-time obstacle association relationship, and generating obstacle avoidance path constraints sorted according to an execution sequence according to the real-time association relationship and motion range information of an end effector; and finally, on the basis of the constraints and the three-dimensional contour information of the assembly target, generating a dynamic obstacle avoidance track of an end effector of the robot, including a continuous position coordinate sequence and a corresponding attitude angle sequence, so that efficient and safe obstacle avoidance of the robot is realized.

Description

Technical Field

[0001] This invention relates to the field of robotic automated assembly technology, and more specifically, to a method and system for obstacle avoidance in robot assembly space using 3D perception. Background Technology

[0002] In the field of robotic automated assembly, 3D perception technology plays a crucial role in improving assembly accuracy and efficiency. As industrial production demands increasing levels of intelligence and automation, robots need to accurately and efficiently complete assembly tasks in complex assembly spaces, and obstacle avoidance capability is a vital guarantee for achieving this goal.

[0003] Currently, existing obstacle avoidance methods for robot assembly spaces have several limitations. Firstly, many methods only acquire partial information about the assembly space, such as considering only the position of static obstacles while ignoring the real-time changes of dynamic obstacles. This leads to the robot's inability to respond promptly to interference from dynamic obstacles, increasing the risk of collisions. Secondly, some methods, when dealing with spatial obstacle relationships, simply treat obstacles as independent entities without deeply analyzing the spatial positional dependencies between static obstacles and between static and dynamic obstacles, making it difficult to generate comprehensive and accurate obstacle avoidance strategies. Furthermore, the obstacle avoidance paths generated by existing methods often lack proper planning of the execution sequence, potentially prioritizing path length while neglecting collision avoidance. This exposes the robot to a higher risk of collisions during operation, hindering the smooth progress of the assembly task. Summary of the Invention

[0004] In view of the aforementioned problems, and in conjunction with the first aspect of the present invention, embodiments of the present invention provide a robot assembly space obstacle avoidance method applied to 3D perception, the method comprising: Acquire 3D assembly space data of the robot assembly area. The 3D assembly space data includes the three-dimensional contour information of the assembly target, the spatial distribution information of static obstacles, the real-time position information of dynamic obstacles, and the motion range information of the robot end effector. Spatial obstacle features are extracted from the 3D assembly space data, and initial obstacle associations are established based on the spatial obstacle features. The spatial obstacle features include the three-dimensional size features, surface curvature features, and relative position features with the assembly target of static obstacles, as well as the motion direction features and motion speed features of dynamic obstacles. The initial obstacle associations are used to represent the spatial positional dependencies between static obstacles and between static obstacles and dynamic obstacles. The initial obstacle association is dynamically updated by adjusting the strength of the association edges based on the real-time location information of the dynamic obstacles to generate real-time obstacle associations. Based on the real-time obstacle association and the motion range information of the robot end effector, obstacle avoidance path constraints are generated in the order of execution. The obstacle avoidance path constraints in the order of execution include collision avoidance constraints executed first, motion stability constraints executed after collision avoidance constraints, and path length constraints executed last. The collision avoidance constraints include the minimum distance limit between the path and the obstacle, and the motion stability constraints include the limit on the continuous change of the path direction. Based on the obstacle avoidance path constraints ordered by execution sequence and the three-dimensional contour information of the assembly target, a dynamic obstacle avoidance trajectory of the robot end effector is generated. The dynamic obstacle avoidance trajectory includes a continuous sequence of position coordinates and a corresponding sequence of attitude angles.

[0005] Furthermore, embodiments of the present invention also provide an obstacle avoidance system for robot assembly space applied to 3D perception, characterized in that it includes: A processor; a machine-readable storage medium for storing machine-executable instructions of the processor; wherein the processor is configured to execute the above-described obstacle avoidance method for robot assembly space applied to 3D perception by executing the machine-executable instructions.

[0006] In another aspect, embodiments of the present invention also provide a computer program product, the computer program product including machine-executable instructions, the machine-executable instructions being stored in a computer-readable storage medium, a processor of a computer device reading the machine-executable instructions from the computer-readable storage medium, the processor executing the machine-executable instructions, causing the computer device to execute the above-described obstacle avoidance method for robot assembly space applied to 3D perception.

[0007] Based on the above, by comprehensively acquiring 3D assembly space data of the robot assembly area, covering assembly targets, static obstacles, dynamic obstacles, and information from the robot's end effector, spatial obstacle features are extracted from this 3D assembly space data, and initial obstacle relationships are established. The spatial positional dependencies between static obstacles and between static and dynamic obstacles are analyzed in depth. The initial obstacle relationships are dynamically updated, allowing for timely adjustment of the associated edge strength based on real-time changes in the position information of dynamic obstacles, generating real-time obstacle relationships. This ensures that the robot can monitor changes in obstacles within the assembly space in real time. Based on the real-time obstacle relationships and the motion range information of the robot's end effector, obstacle avoidance path constraints are generated in order of execution. From collision avoidance and motion stability to path length, orderly planning is implemented, ensuring both safe robot operation and considering motion stability and path rationality. Finally, based on these constraints and the 3D contour information of the assembly target, a dynamic obstacle avoidance trajectory for the robot's end effector is generated, providing the robot with precise and feasible motion guidance. This effectively improves the robot's obstacle avoidance capability and assembly efficiency in complex assembly spaces, reduces collision risks, and ensures the successful completion of assembly tasks. Attached Figure Description

[0008] Figure 1 This is a schematic diagram of the execution flow of the obstacle avoidance method for robot assembly space applied to 3D perception provided in an embodiment of the present invention.

[0009] Figure 2 This is a schematic diagram of exemplary hardware and software components of a robot assembly space obstacle avoidance system for 3D perception provided in an embodiment of the present invention. Detailed Implementation

[0010] The present invention will now be described in detail with reference to the accompanying drawings. Figure 1 This is a flowchart illustrating a robot assembly space obstacle avoidance method applied to 3D perception, provided by an embodiment of the present invention. The following is a detailed description of this robot assembly space obstacle avoidance method applied to 3D perception.

[0011] Step S110: Obtain 3D assembly space data of the robot assembly area. The 3D assembly space data includes the three-dimensional contour information of the assembly target, the spatial distribution information of static obstacles, the real-time position information of dynamic obstacles, and the motion range information of the robot end effector.

[0012] In this embodiment, the assembly area refers to the spatial range within the robotic arm assembly workstation. 3D assembly space data is acquired through multiple 3D LiDAR sensors distributed around the workstation and at its top, covering the entire assembly area. The assembly target is the mechanical part to be assembled; its three-dimensional contour information is obtained through 3D scanning, including the coordinate points of each surface and surface normal vectors. Static obstacles are fixed equipment and tool racks within the workstation; their spatial distribution information is acquired through sensor data collection and identification, including the position coordinates and shape parameters of each static obstacle. Dynamic obstacles are moving material boxes and other temporarily moving equipment within the workstation; their real-time position information is acquired and updated in real-time by sensors, including the position coordinates and direction of movement of each dynamic obstacle at different times. The motion range information of the robot's end effector includes parameters such as the maximum distance traveled and the maximum rotation angle along the three spatial coordinate axes. These parameters are set by the robot's design parameters and control system.

[0013] Step S120: Extract spatial obstacle features from the 3D assembly space data, and establish initial obstacle associations based on the spatial obstacle features.

[0014] In this embodiment, the process of extracting spatial obstacle features from 3D assembly space data first involves feature extraction for both static and dynamic obstacles. For static obstacles, their position coordinates, shape parameters, and surface normal vectors are extracted as spatial obstacle features. For dynamic obstacles, their real-time position coordinates, movement speed, and movement direction are extracted as spatial obstacle features. Then, initial obstacle relationships are established based on the extracted spatial obstacle features. The establishment of these relationships is based on factors such as the positional distance and relative direction between obstacles.

[0015] Step S121: Perform multi-resolution region division processing on the 3D assembly space data. Set the division resolution according to the proportion of obstacles at different locations in the assembly area to obtain multiple non-overlapping three-dimensional sub-regions. Each three-dimensional sub-region corresponds to a local space in the assembly area. Regions with an obstacle proportion greater than a preset proportion threshold are divided with high resolution, while regions with an obstacle proportion less than or equal to the preset proportion threshold are divided with low resolution.

[0016] In this embodiment, when performing multi-resolution region segmentation on the 3D assembly space data, the proportion of obstacles at different locations within the assembly area is first calculated. The obstacle proportion is calculated by counting the number of obstacles within a certain range at each location and then dividing by the total volume of the space within that range. A preset proportion threshold is set based on the actual application scenario. For regions where the obstacle proportion is greater than the preset proportion threshold, high-resolution segmentation is used, meaning the volume of the segmented 3D sub-regions is smaller. For regions where the obstacle proportion is less than or equal to the preset proportion threshold, low-resolution segmentation is used, meaning the volume of the segmented 3D sub-regions is larger. The resulting multiple non-overlapping 3D sub-regions each correspond to a local space within the assembly area.

[0017] Step S122: Identify static and dynamic obstacles in each three-dimensional sub-region. For static obstacles, extract their length parameters in the three spatial coordinate axes as the three-dimensional size features of the static obstacles.

[0018] In this embodiment, when identifying static and dynamic obstacles in each three-dimensional sub-region, a target recognition algorithm is used to identify and classify objects within each sub-region. The target recognition algorithm identifies objects based on their features, classifying them into static and dynamic obstacles. For identified static obstacles, their length parameters along the three spatial coordinate axes are extracted as the three-dimensional dimensional features of the static obstacles.

[0019] Step S123: Perform layered sampling processing on the surface of each static obstacle, set the sampling density according to the surface curvature change trend of the static obstacle, obtain the coordinate information of multiple surface sampling points, increase the number of sampling points in areas where the curvature change is greater than the preset curvature change threshold, and reduce the number of sampling points in areas where the curvature change is less than or equal to the preset curvature change threshold.

[0020] In this embodiment, when performing layered sampling processing on the surface of each static obstacle, the curvature change trend of the static obstacle surface is first analyzed. The curvature change trend analysis is achieved by calculating the curvature values ​​at different points on the surface, based on the surface's geometry and normal vector information. A preset curvature change threshold is set according to the actual application scenario. For regions where the curvature change is greater than the preset curvature change threshold, the number of sampling points is increased. For regions where the curvature change is less than or equal to the preset curvature change threshold, the number of sampling points is reduced. Through layered sampling processing, the coordinate information of multiple surface sampling points is obtained.

[0021] Step S124: For each surface sampling point, set a local neighborhood range that adapts to the surface curvature around it, filter out other sampling points located within the local neighborhood range from the coordinate information of all surface sampling points, and form a local neighborhood sampling point set for that sampling point. The local neighborhood range is narrowed for regions with curvature values ​​greater than a preset curvature threshold, and the local neighborhood range is expanded for regions with curvature values ​​less than or equal to the preset curvature threshold.

[0022] In this embodiment, when setting the local neighborhood range for each surface sampling point, it is based on the surface curvature value at the location of the sampling point. The preset curvature threshold is set according to the actual application scenario. For regions with curvature values ​​greater than the preset curvature threshold, the local neighborhood range is reduced. For regions with curvature values ​​less than or equal to the preset curvature threshold, the local neighborhood range is expanded. Then, other sampling points located within the local neighborhood range are selected from the coordinate information of all surface sampling points to form a local neighborhood sampling point set for that sampling point.

[0023] Step S125: Based on the coordinate information of the local neighborhood sampling point set, the equation of the local tangent plane passing through the sampling point is fitted using the least squares method. The deviation angle between the normal vector at the sampling point and the normal vector of the local tangent plane equation is calculated. Combining the distance parameters from each point in the local neighborhood sampling point set to the local tangent plane, the curvature value at the sampling point is determined using the curvature calculation formula.

[0024] In this embodiment, when fitting the local tangent plane equation based on the coordinate information of the local neighborhood sampling point set, the least squares method is used for fitting. The least squares method determines the tangent plane equation by minimizing the sum of squared distances from the sampling points to the tangent plane. After fitting the local tangent plane equation, the deviation angle between the normal vector at the sampling point and the normal vector of the local tangent plane equation is calculated. The deviation angle is calculated based on the dot product formula of vectors, by calculating the dot product of the two normal vectors and combining it with the magnitude of the vectors. Then, combined with the distance parameters from each point in the local neighborhood sampling point set to the local tangent plane, the curvature value at the sampling point is determined using the curvature calculation formula.

[0025] Step S126: Perform statistical analysis on the curvature values ​​of all sampling points, calculate the maximum, minimum, average and variance of the curvature values, and use them together as the surface curvature characteristics of the static obstacle.

[0026] In this embodiment, when performing statistical analysis on the curvature values ​​of all sampling points, the curvature values ​​of all sampling points are first collected. Then, the maximum, minimum, average, and variance of the curvature values ​​are calculated. The maximum value is the largest curvature value among all sampling points, the minimum value is the smallest curvature value among all sampling points, the average value is the sum of the curvature values ​​of all sampling points divided by the number of sampling points, and the variance is the sum of the squares of the differences between each sampling point's curvature value and the average value divided by the number of sampling points. These statistical characteristics collectively constitute the surface curvature characteristics of this static barrier.

[0027] Step S127: Determine the reference coordinate position of the assembly target in the 3D assembly space. The reference coordinate position is the geometric center coordinate of the assembly target. Calculate the vector difference between the geometric center coordinate of each static obstacle and the reference coordinate position. The vector difference contains magnitude and direction information. Use the vector difference as the relative position feature between the static obstacle and the assembly target.

[0028] In this embodiment, when determining the reference coordinate position of the assembly target in the 3D assembly space, the geometric center coordinates of the assembly target are first found. The geometric center coordinates are calculated based on the three-dimensional contour information of the assembly target, by calculating the average value of all surface coordinate points of the assembly target. Then, the vector difference between the geometric center coordinates of each static obstacle and the reference coordinate position is calculated. The vector difference is calculated by subtracting the reference coordinate position from the geometric center coordinates of the static obstacle. The vector difference includes magnitude and direction information: the magnitude is the distance between the two coordinate points, and the direction is the direction from the reference coordinate position to the geometric center coordinates of the static obstacle. This vector difference is used as the relative position feature between the static obstacle and the assembly target.

[0029] Step S128: For dynamic obstacles, extract their position coordinate information at two consecutive acquisition times, calculate the vector difference between the two position coordinate information, use the direction of the vector difference as the motion direction feature of the dynamic obstacle, and use the ratio of the magnitude of the vector difference to the time interval between the two acquisition times as the motion speed feature of the dynamic obstacle.

[0030] In this embodiment, when extracting motion direction and velocity features from a dynamic obstacle, the position coordinates of the obstacle at two consecutive acquisition times are first obtained. The time interval between acquisition times is set according to the sensor's acquisition frequency. Then, the vector difference between the two position coordinates is calculated by subtracting the position coordinates of the previous acquisition time from the position coordinates of the later acquisition time. The direction of this vector difference is taken as the motion direction feature of the dynamic obstacle. The ratio of the magnitude of this vector difference to the time interval between the two acquisition times is taken as the motion velocity feature of the dynamic obstacle.

[0031] Step S129: Collect the three-dimensional size features, surface curvature features, and relative position features with the assembly target of all static obstacles, as well as the motion direction features and motion speed features of all dynamic obstacles, to form the spatial obstacle features corresponding to each obstacle.

[0032] In this embodiment, when collecting features of all static and dynamic obstacles, the three-dimensional size features, surface curvature features, and relative position features with respect to the assembly target of each static obstacle are first collected, as well as the motion direction features and motion speed features of each dynamic obstacle. Then, the above features are integrated to form the spatial obstacle features corresponding to each obstacle.

[0033] Step S1210: Analyze the relative position features in the spatial obstacle features of different static obstacles, calculate the spatial distance parameter and the directional angle parameter between any two static obstacles. If the spatial distance parameter is less than the preset associated distance threshold and the directional angle parameter is within the preset associated angle range, it is determined that there is a direct spatial occlusion relationship between the two static obstacles, and an associated edge is established between the two static obstacles.

[0034] In this embodiment, when analyzing the relative positional characteristics of different static obstacles, the relative positional characteristics of each static obstacle are first obtained, i.e., the vector difference between its relative position and the assembly target. Then, the spatial distance parameter between any two static obstacles is calculated. This spatial distance parameter is based on the geometric center coordinates of the two static obstacles and is obtained by calculating the distance between the two coordinate points. The directional angle parameter is also calculated, based on the vector difference between the relative positions of the two static obstacles and is obtained by calculating the angle between the two vectors. Preset association distance thresholds and association angle ranges are set according to the actual application scenario. If the spatial distance parameter is less than the preset association distance threshold and the directional angle parameter is within the preset association angle range, a direct spatial occlusion relationship is determined between the two static obstacles, and an association edge is established between the two static obstacles.

[0035] Step S1211: Analyze the spatial obstacle characteristics of static and dynamic obstacles, calculate the spatial distance parameter between the geometric center coordinates of the static obstacle and the current position coordinates of the dynamic obstacle, and determine whether the dynamic obstacle has a tendency to move towards the static obstacle by combining the movement direction characteristics of the dynamic obstacle. If the spatial distance parameter is less than the associated distance threshold and the dynamic obstacle has a tendency to move towards the static obstacle, then establish the associated edge between the static obstacle and the dynamic obstacle.

[0036] In this embodiment, when analyzing the spatial obstacle characteristics of static and dynamic obstacles, the spatial distance parameter between the geometric center coordinates of the static obstacle and the current position coordinates of the dynamic obstacle is first calculated. Then, the movement direction characteristics of the dynamic obstacle are considered to determine whether the dynamic obstacle has a tendency to move towards the static obstacle. The determination method is to analyze whether the movement direction of the dynamic obstacle points towards the direction of the static obstacle. If the spatial distance parameter is less than the associated distance threshold and the dynamic obstacle has a tendency to move towards the static obstacle, then an associated edge is established between the static obstacle and the dynamic obstacle.

[0037] Step S1212: Treat all static and dynamic obstacles as nodes, connect the nodes with associated edges, assign an initial association strength value to each associated edge, the initial association strength value is negatively correlated with the corresponding spatial distance parameter, forming an initial obstacle association relationship that represents the spatial positional dependence between static obstacles and between static and dynamic obstacles.

[0038] In this embodiment, all static and dynamic obstacles are treated as nodes, and nodes with associated edges are connected. An initial association strength value is assigned to each associated edge, based on the corresponding spatial distance parameter; the smaller the spatial distance parameter, the larger the initial association strength value. This forms initial obstacle association relationships representing the spatial positional dependencies between static obstacles and between static and dynamic obstacles.

[0039] Step S130: Dynamically update the initial obstacle association relationship, adjust the strength of the association edges in the association relationship according to the real-time location information of the dynamic obstacles, and generate a real-time obstacle association relationship.

[0040] In this embodiment, when dynamically updating the initial obstacle association, the positional changes of the dynamic obstacles are first acquired in real time. Then, the strength of the association edges in the association is adjusted according to the positional changes of the dynamic obstacles. The adjustment method is as follows: if the dynamic obstacle moves towards the associated static obstacle, the spatial distance parameter is decreased, and the strength of the association edge is increased; if the dynamic obstacle moves away from the associated static obstacle, the spatial distance parameter is increased, and the strength of the association edge is decreased. Real-time obstacle association is generated.

[0041] Step S131: Continuously collect the real-time location information of dynamic obstacles according to the set dynamic update time interval, and record the timestamp and corresponding location coordinates of each collection.

[0042] In this embodiment, real-time location information of dynamic obstacles is continuously collected according to a set dynamic update time interval. The time interval is set according to the actual application scenario. The timestamp and corresponding location coordinates of each collection are recorded. The timestamp is used to identify the time sequence of collection, and the location coordinates are used for subsequent analysis of the positional changes of dynamic obstacles.

[0043] Step S132: For each dynamic obstacle, calculate the position offset between its current real-time position coordinates and the position coordinates collected last time, and update the motion direction characteristics and motion speed characteristics of the dynamic obstacle according to the position offset.

[0044] In this embodiment, for each dynamic obstacle, the position offset between its current real-time position coordinates and the previously acquired position coordinates is calculated. The position offset is calculated by subtracting the previously acquired position coordinates from the current real-time position coordinates. The motion direction feature and motion velocity feature of the dynamic obstacle are updated based on the position offset. The motion direction feature is the direction of the position offset, and the motion velocity feature is the ratio of the magnitude of the position offset to the time interval.

[0045] Step S133: Traverse all associated edges in the initial obstacle association relationship that are associated with the dynamic obstacle, including the associated edges between the dynamic obstacle and other static obstacles, and the associated edges between the dynamic obstacle and other dynamic obstacles.

[0046] In this embodiment, all associated edges in the initial obstacle association relationship related to the dynamic obstacle are traversed. Associated edges include those between the dynamic obstacle and other static obstacles, and those between the dynamic obstacle and other dynamic obstacles. During the traversal, each associated edge is processed one by one.

[0047] Step S134: For each associated edge, recalculate the current spatial distance parameter between the two obstacles connected by the associated edge. If the associated edge connects a dynamic obstacle and a static obstacle, calculate the spatial distance parameter between the current real-time position coordinates of the dynamic obstacle and the geometric center coordinates of the static obstacle. If the associated edge connects two dynamic obstacles, calculate the spatial distance parameter between the current real-time position coordinates of the two dynamic obstacles.

[0048] In this embodiment, for each associated edge, the current spatial distance parameter between the two obstacles connected by the associated edge is recalculated. If the associated edge connects a dynamic obstacle and a static obstacle, the spatial distance parameter between the current real-time position coordinates of the dynamic obstacle and the geometric center coordinates of the static obstacle is calculated. If the associated edge connects two dynamic obstacles, the spatial distance parameter between the current real-time position coordinates of the two dynamic obstacles is calculated.

[0049] Step S135: Adjust the association strength value of the corresponding associated edge according to the recalculated spatial distance parameter. If the spatial distance parameter decreases, increase the association strength value; if the spatial distance parameter increases, decrease the association strength value; if the spatial distance parameter is greater than the association distance threshold, adjust the association strength value to zero and disconnect the associated edge.

[0050] In this embodiment, the association strength value of the corresponding associated edge is adjusted according to the recalculated spatial distance parameter. If the spatial distance parameter decreases, the association strength value is increased; if the spatial distance parameter increases, the association strength value is decreased; if the spatial distance parameter is greater than the association distance threshold, the association strength value is adjusted to zero, and the associated edge is disconnected.

[0051] Step S136: Check if there are any new combinations of obstacles that need to establish associated edges, calculate the current spatial distance parameters between obstacles that have not established associated edges, and determine whether the association conditions are met by combining the movement direction characteristics of dynamic obstacles. If they are met, establish new associated edges and assign initial association strength values.

[0052] In this embodiment, it is checked whether there are new obstacle combinations that require establishing associated edges. The current spatial distance parameter between obstacles that have not yet established associated edges is calculated, and the motion direction characteristics of dynamic obstacles are combined to determine whether the association condition is met. The association condition is that the spatial distance parameter is less than the association distance threshold and the dynamic obstacle shows a tendency to move towards another obstacle. If the conditions are met, a new associated edge is established and an initial association strength value is assigned.

[0053] Step S137: After normalizing the association strength values ​​of all associated edges, the adjusted nodes, associated edges and association strength values ​​are integrated to form real-time obstacle association relationships. The real-time obstacle association relationships can reflect the spatial positional dependencies between all obstacles at the current moment.

[0054] In this embodiment, the association strength values ​​of all associated edges are normalized by converting them to a specific numerical range. Then, the adjusted nodes, associated edges, and association strength values ​​are integrated to form real-time obstacle association relationships. These real-time obstacle association relationships reflect the spatial positional dependencies between all obstacles at the current moment.

[0055] Step S140: Generate obstacle avoidance path constraints ordered by execution order based on the real-time obstacle association relationship and the motion range information of the robot end effector. The obstacle avoidance path constraints ordered by execution order include collision avoidance constraints executed first, motion stability constraints executed after collision avoidance constraints, and path length constraints executed last. The collision avoidance constraints include the minimum distance limit between the path and the obstacle, and the motion stability constraints include the limit on the continuous change of the path direction.

[0056] In this embodiment, obstacle avoidance path constraints are generated in order of execution based on real-time obstacle associations and the motion range information of the robot's end effector. First, collision avoidance constraints are generated, including a minimum distance limit between the path and obstacles to ensure the robot's motion path maintains a certain distance from obstacles. Then, motion stability constraints are generated, including restrictions on continuous changes in path direction to ensure smooth changes in the robot's motion path direction. Finally, path length constraints are generated, including restrictions on the path length to ensure the length of the robot's motion path is within a reasonable range.

[0057] Step S141: Analyze the real-time obstacle association relationship, identify the obstacle combination connected by the association edge whose association strength value is greater than the preset strength threshold, and divide the obstacle combination into multiple obstacle clusters.

[0058] In this embodiment, real-time obstacle relationships are analyzed to identify obstacle combinations connected by edges whose association strength values ​​are greater than a preset strength threshold. The preset strength threshold is set according to the actual application scenario. These obstacle combinations are divided into multiple obstacle clusters, each containing interconnected obstacles.

[0059] Step S1411: Initialize the cluster identifier counter, set the initial value of the cluster identifier counter to zero, and create an empty cluster list to store the partitioned obstacle clusters.

[0060] In this embodiment, the initial value of the cluster identifier counter is set to zero when initializing it. An empty cluster list is created to store the obstacle clusters obtained from subsequent partitioning.

[0061] Step S1412: Select an obstacle that has not been assigned a cluster from all nodes in the real-time obstacle association relationship as the starting node, use the current value of the cluster identifier counter as the cluster identifier of the starting node, and add the starting node to the temporary cluster set.

[0062] In this embodiment, an obstacle that has not been assigned a cluster is selected from all nodes in the real-time obstacle association relationship as the starting node. The current value of the cluster identifier counter is used as the cluster identifier of the starting node, and then the starting node is added to the temporary cluster set.

[0063] Step S1413: Traverse all associated edges connected to the starting node in the real-time obstacle association relationship, check whether the association strength value of each associated edge is greater than the preset strength threshold. If the association strength value is greater than the strength threshold, obtain the associated node at the other end of the associated edge.

[0064] In this embodiment, all associated edges connected to the starting node in the real-time obstacle association relationship are traversed. The association strength value of each associated edge is checked to see if it is greater than a preset strength threshold. If the association strength value is greater than the strength threshold, the associated node at the other end of that associated edge is obtained.

[0065] Step S1414: Check whether the associated node has been assigned to a cluster. If it has not been assigned to a cluster, set the cluster identifier of the associated node to the value of the current cluster identifier counter and add the associated node to the temporary cluster set.

[0066] In this embodiment, it is checked whether the associated node has been assigned to a cluster. If it has not been assigned to a cluster, the cluster identifier of the associated node is set to the value of the current cluster identifier counter, and then the associated node is added to the temporary cluster set.

[0067] Step S1415: Continue to traverse the associated edges of all newly added nodes in the temporary cluster set, repeating the above steps of checking the association strength value, obtaining associated nodes, and assigning cluster identifiers, until no new nodes are added to the temporary cluster set.

[0068] In this embodiment, the associated edges of all newly added nodes in the temporary cluster set are traversed. The steps of checking the association strength value, obtaining associated nodes, and assigning cluster identifiers are repeated until no new nodes are added to the temporary cluster set.

[0069] Step S1416: Treat all nodes in the temporary cluster set as an obstacle cluster, assign the value of the current cluster identifier counter to the obstacle cluster, and add the obstacle cluster to the cluster list.

[0070] In this embodiment, all nodes in the temporary cluster set are treated as a single obstacle cluster. A current cluster identifier counter value is assigned to this obstacle cluster, and then the obstacle cluster is added to the cluster list.

[0071] Step S1417: Increment the value of the cluster identifier counter by one, clear the temporary cluster set, select the next unassigned cluster obstacle from all nodes of the real-time obstacle association relationship as the new starting node, and repeat the above steps.

[0072] In this embodiment, the cluster identifier counter is incremented by one. The temporary cluster set is cleared, and the next obstacle without a cluster is selected from all nodes in the real-time obstacle association as the new starting node. The above steps are repeated.

[0073] Step S1418: If there are isolated nodes in the real-time obstacle association relationship that have not been assigned a cluster, then create an obstacle cluster for each isolated node, assign an independent cluster identifier, and add it to the cluster list.

[0074] In this embodiment, if there are isolated nodes in the real-time obstacle association relationship that have not been assigned a cluster, an obstacle cluster is created for each isolated node, an independent cluster identifier is assigned, and the node is added to the cluster list.

[0075] Step S1419: Traverse all obstacle clusters in the cluster list and check if there are any associated edges between the clusters. If there are associated edges between nodes in two obstacle clusters with an associated strength value greater than the strength threshold, then merge the two obstacle clusters into a new obstacle cluster and update the cluster identifier and cluster list.

[0076] In this embodiment, all obstacle clusters in the cluster list are traversed. It checks for any associated edges between clusters. If there is an associated edge between nodes in two obstacle clusters with a strength value greater than a threshold, the two obstacle clusters are merged into a new obstacle cluster, and the cluster identifier and cluster list are updated.

[0077] Step S1420: Repeat the above merging steps until there are no association edges with an association strength value greater than the strength threshold between all obstacle clusters in the cluster list, and finally obtain multiple obstacle clusters.

[0078] In this embodiment, the above merging steps are repeated until there are no association edges with an association strength value greater than the strength threshold between all obstacle clusters in the cluster list, and finally multiple obstacle clusters are obtained.

[0079] Step S142: For each obstacle cluster, collect the spatial obstacle features of all obstacles in the cluster. If the cluster contains static obstacles, extract the three-dimensional size features and surface curvature features of the static obstacles. If the cluster contains dynamic obstacles, extract the motion direction features, motion speed features, and current real-time position coordinates of the dynamic obstacles.

[0080] In this embodiment, for each obstacle cluster, the spatial obstacle features of all obstacles within the cluster are collected. If the cluster contains static obstacles, the three-dimensional size features and surface curvature features of the static obstacles are extracted. If the cluster contains dynamic obstacles, the motion direction features, motion speed features, and current real-time position coordinates of the dynamic obstacles are extracted.

[0081] Step S143: Based on the spatial obstacle characteristics of all obstacles in the cluster, determine the overall spatial distribution range of the obstacle cluster. The overall spatial distribution range is the smallest three-dimensional bounding box that includes all obstacles in the cluster. Calculate the length, width, height and geometric center coordinates of the three-dimensional bounding box.

[0082] In this embodiment, the overall spatial distribution range of the obstacle cluster is determined based on the spatial obstacle characteristics of all obstacles within the cluster. The overall spatial distribution range is the smallest three-dimensional bounding box that includes all obstacles within the cluster. The length, width, and height dimensions and the geometric center coordinates of this three-dimensional bounding box are calculated. The length, width, and height dimensions are the lengths of the three-dimensional bounding box along the three spatial coordinate axes, and the geometric center coordinates are the coordinates of the center position of the three-dimensional bounding box.

[0083] Step S144: Based on the three-dimensional size parameters of the end effector in the motion range information of the robot end effector, calculate the safe buffer distance that the robot end effector needs to maintain when approaching the obstacle cluster. The safe buffer distance is the sum of the maximum size value in the three-dimensional size parameters of the end effector and the preset safety margin. The safe buffer distance is used as the minimum distance limit between the path and all obstacles in the obstacle cluster and is included in the collision avoidance constraints executed first.

[0084] In this embodiment, the three-dimensional dimensions of the robot's end effector, derived from its motion range information, are used to calculate the safe buffer distance that the robot's end effector needs to maintain when approaching the obstacle cluster. The safe buffer distance is the sum of the maximum dimension value in the end effector's three-dimensional dimensions and a preset safety margin. This safe buffer distance is then used as the minimum distance limit between the path and all obstacles in the obstacle cluster and incorporated into the first-executed collision avoidance constraints.

[0085] Step S145: Extract the maximum rotation angle parameter and maximum movement acceleration parameter from the motion range information of the robot end effector. Combined with the accuracy requirements of the assembly operation, determine the allowable directional change threshold and acceleration change threshold of the robot end effector during the motion process.

[0086] In this embodiment, the maximum rotation angle parameter and maximum acceleration parameter are extracted from the motion range information of the robot's end effector. Based on the accuracy requirements of the assembly operation, allowable directional change thresholds and acceleration change thresholds are determined for the robot's end effector during motion. The directional change threshold is the maximum allowable value for the change in the robot's motion path direction, and the acceleration change threshold is the maximum allowable value for the change in the robot's motion acceleration.

[0087] Step S146: Set a continuous change limit for the path direction based on the direction change threshold, that is, the angle between the directions of two adjacent path points shall not exceed the direction change threshold; set a continuous change limit for the path acceleration based on the acceleration change threshold, that is, the amount of acceleration change between two adjacent path segments shall not exceed the acceleration change threshold; and incorporate the continuous change limit for the path direction and the continuous change limit for the path acceleration into the motion stability constraint executed after the collision avoidance constraint.

[0088] In this embodiment, a continuous change limit for path direction is set based on a direction change threshold, meaning the angle between the directions of two adjacent path points must not exceed the aforementioned direction change threshold. A continuous change limit for path acceleration is set based on an acceleration change threshold, meaning the change in acceleration between two adjacent path segments must not exceed the aforementioned acceleration change threshold. Both the continuous change limit for path direction and the continuous change limit for path acceleration are incorporated into the motion stability constraints subsequently executed after collision avoidance constraints.

[0089] Step S147: Analyze the spatial gaps between different obstacle clusters and calculate the three-dimensional dimension parameters of each spatial gap. If the three-dimensional dimension parameters of the gap are all greater than the sum of the three-dimensional dimension parameters of the robot end effector and the safety buffer distance, then the gap is identified as a potential path channel. The three-dimensional dimension parameters include the width, height and depth of the gap.

[0090] In this embodiment, the spatial gaps between different obstacle clusters are analyzed. The three-dimensional dimensional parameters of each spatial gap are calculated, including the gap's width, height, and depth. If the three-dimensional dimensional parameters of the gap are all greater than the sum of the three-dimensional dimensional parameters of the robot's end effector and the safety buffer distance, then the gap is identified as a potential path passage.

[0091] Step S148: For each potential path channel, in combination with the minimum distance limit between the path and the obstacle, set the boundary range of the path within the channel. The boundary range is the area where the inner wall of the path channel and the corresponding obstacle cluster maintain a safe buffer distance. The path is always located inside the path channel and maintains the minimum distance limit with the obstacles on both sides of the path channel.

[0092] In this embodiment, for each potential path channel, the boundary range of the path within the channel is set based on the minimum distance limit between the path and obstacles. The boundary range is the area where a safe buffer distance is maintained between the inner wall of the path channel and the corresponding obstacle cluster. The path always lies within the path channel and maintains the aforementioned minimum distance limit with respect to the obstacles on both sides of the path channel.

[0093] Step S149: Based on the total time requirement of the assembly task, set the maximum allowable value of the path length, and use the path length not exceeding the maximum allowable value as the last path length constraint to be executed.

[0094] In this embodiment, a maximum allowable path length is set based on the total time requirement of the assembly task. The total time requirement is set according to the actual assembly task. The path length not exceeding the aforementioned maximum allowable value is used as the final path length constraint.

[0095] Step S1410: The collision avoidance constraints, motion stability constraints, and path length constraints are ordered in the order of execution. The collision avoidance constraints are executed before the motion stability constraints, and the motion stability constraints are executed before the path length constraints. In the subsequent trajectory generation process, if different constraints conflict, the constraints that are executed first are given priority, forming obstacle avoidance path constraints ordered in the order of execution.

[0096] In this embodiment, the collision avoidance constraints, motion stability constraints, and path length constraints are executed in a specific order. Collision avoidance constraints are executed before motion stability constraints, and motion stability constraints are executed before path length constraints. During subsequent trajectory generation, if conflicts arise between different constraints, the constraints executed earlier are prioritized, forming obstacle avoidance path constraints ordered by execution order.

[0097] Step S150: Based on the obstacle avoidance path constraints ordered by execution order and the three-dimensional contour information of the assembly target, generate a dynamic obstacle avoidance trajectory for the robot end effector. The dynamic obstacle avoidance trajectory includes a continuous sequence of position coordinates and a corresponding sequence of attitude angles.

[0098] In this embodiment, a dynamic obstacle avoidance trajectory for the robot's end effector is generated based on obstacle avoidance path constraints ordered by execution sequence and the 3D contour information of the assembly target. First, a sequence of position coordinates for the robot's motion is generated according to the obstacle avoidance path constraints. Then, a corresponding sequence of attitude angles is generated based on the 3D contour information of the assembly target. The dynamic obstacle avoidance trajectory includes a continuous sequence of position coordinates and a corresponding sequence of attitude angles.

[0099] Step S151: Analyze the three-dimensional contour information of the assembly target, and extract the coordinates of the assembly reference point and the assembly direction vector on the assembly target. The coordinates of the assembly reference point are the target position that the robot end effector needs to reach, and the assembly direction vector is the attitude reference direction of the robot end effector when it completes the assembly operation at the target position.

[0100] In this embodiment, the three-dimensional contour information of the assembly target is analyzed, and the coordinates of the assembly reference point and the assembly direction vector on the assembly target are extracted. The coordinates of the assembly reference point are the target position that the robot end effector needs to reach, and the assembly direction vector is the attitude reference direction of the robot end effector when it completes the assembly operation at the target position.

[0101] Step S152: Obtain the current position coordinates and current attitude angle of the robot end effector, take the current position coordinates as the starting point of the dynamic obstacle avoidance trajectory, take the assembly reference point coordinates as the ending point of the dynamic obstacle avoidance trajectory, and initially plan an initial straight path from the starting point to the ending point in the 3D assembly space.

[0102] In this embodiment, the current position coordinates and current attitude angle of the robot's end effector are obtained. The current position coordinates are used as the starting point of the dynamic obstacle avoidance trajectory, and the assembly reference point coordinates are used as the ending point of the dynamic obstacle avoidance trajectory. An initial straight-line path from the starting point to the ending point is initially planned in the 3D assembly space.

[0103] Step S153: Check whether the initial straight path satisfies the collision avoidance constraint executed first among the obstacle avoidance path constraints ordered by execution order. That is, calculate whether the distance between each point on the initial straight path and all obstacle clusters is greater than the corresponding minimum distance limit. If all points meet the distance requirement, the initial straight path is used as a candidate path. If there are points that do not meet the distance requirement, multiple intermediate transition points are set at the intersection of the initial straight path and the overall spatial distribution range of the obstacle clusters. The number of intermediate transition points is determined according to the size of the intersection area and the number of path channels.

[0104] In this embodiment, it is checked whether the initial straight path satisfies the collision avoidance constraint executed first among the obstacle avoidance path constraints ordered by execution sequence. The distances between each point on the initial straight path and all obstacle clusters are calculated to ensure they are all greater than the corresponding minimum distance limit. If all points meet the distance requirements, the initial straight path is selected as a candidate path. If any points do not meet the distance requirements, multiple intermediate transition points are set at the intersection of the initial straight path and the overall spatial distribution range of the obstacle clusters. The number of intermediate transition points is determined based on the size of the intersection area and the number of path channels.

[0105] Step S154: Based on the path channel boundary range in the obstacle avoidance path constraints ordered by execution order, adjust the coordinate position of each intermediate transition point. Each intermediate transition point is located within the corresponding path channel, and the distance between the intermediate transition point and the surrounding obstacle cluster meets the minimum distance limit.

[0106] In this embodiment, the coordinates of each intermediate transition point are adjusted according to the path channel boundary range in the obstacle avoidance path constraints ordered by execution sequence. Each intermediate transition point is located within its corresponding path channel, and the distance between the intermediate transition point and the surrounding obstacle cluster meets the minimum distance constraint.

[0107] Step S155: Using the starting point, intermediate transition point, and ending point as critical path points, construct a path optimization objective function. The path optimization objective function includes a path length term, a path smoothness term, and an attitude adaptation term. The path length term is used to minimize the total path length to meet the path length constraint of the last execution. The path smoothness term is used to minimize the directional change of adjacent path segments to meet the motion stability constraint of the execution following the collision avoidance constraint. The attitude adaptation term is used to minimize the deviation between the attitude of the end effector and the assembly direction vector.

[0108] In this embodiment, a path optimization objective function is constructed using the starting point, intermediate transition points, and ending point as critical path points. The path optimization objective function includes a path length term, a path smoothness term, and an attitude adaptation term. The path length term minimizes the total path length to meet the path length constraint for the last executed operation. The path smoothness term minimizes the directional changes between adjacent path segments to meet the motion stability constraint following the collision avoidance constraint. The attitude adaptation term minimizes the deviation between the end effector's attitude and the assembly direction vector.

[0109] Step S156: Normalize the terms in the path optimization objective function, and set weight coefficients for the terms in the normalized path optimization objective function. The weight coefficients are determined according to the execution order of the obstacle avoidance path constraints ordered by execution order. The weight coefficient of the path smoothness term is greater than the weight coefficient of the path length term, and the weight coefficient of the attitude adaptability term is the same as the weight coefficient of the path smoothness term.

[0110] In this embodiment, the terms in the path optimization objective function are normalized by converting the values ​​of each term to a specific range. Weight coefficients are then assigned to each term in the normalized path optimization objective function, and these weight coefficients are determined based on the execution order of the obstacle avoidance path constraints. The weight coefficient for the path smoothness term is greater than that for the path length term, and the weight coefficient for the attitude adaptation term is the same as that for the path smoothness term.

[0111] Step S157: The particle swarm optimization algorithm is used to solve the path optimization objective function to generate multiple candidate paths, each containing the coordinate information of multiple path points.

[0112] In this embodiment, a particle swarm optimization algorithm is used to solve the path optimization objective function. The particle swarm optimization algorithm finds the optimal solution by simulating the swarm behavior of particles. Multiple candidate paths are generated, each containing the coordinate information of multiple path points.

[0113] Step S1571: Determine the size of the particle swarm, the maximum number of iterations, and the algorithm parameters for inertia weight, cognitive coefficient, and social coefficient. Each particle in the particle swarm represents a candidate path, and each path point contains three spatial coordinates. The particle dimension is the number of path points multiplied by three.

[0114] In this embodiment, algorithm parameters for determining the particle swarm size, maximum number of iterations, inertia weight, cognitive coefficient, and social coefficient are defined. Each particle in the particle swarm represents a candidate path, and each path point contains three spatial coordinates. The particle dimension is three times the number of path points.

[0115] Step S1572: Initialize the position of each particle in the particle swarm. The position of each particle corresponds to the path point coordinates of a candidate path. During initialization, the path point coordinates are randomly generated based on the coordinates of the starting point, intermediate transition points and the ending point. The generated path point coordinates are within a reasonable range of the 3D assembly space.

[0116] In this embodiment, the position of each particle in the particle swarm is initialized. The position of each particle corresponds to the path point coordinates of a candidate path. During initialization, the path point coordinates are randomly generated based on the coordinates of the starting point, intermediate transition points, and the ending point. The generated path point coordinates are within a reasonable range of the 3D assembly space.

[0117] Step S1573: Calculate the path optimization objective function value for each particle. Specifically, calculate the values ​​of the path length term, path smoothness term, and attitude adaptation term based on the particle's position, and combine them with the weighting coefficients to obtain the total value of the path optimization objective function.

[0118] In this embodiment, the path optimization objective function value corresponding to each particle is calculated. Specifically, the values ​​of the path length term, path smoothness term, and attitude adaptation term are calculated based on the particle's position, and the total value of the path optimization objective function is obtained by combining the weighting coefficients.

[0119] Step S1574: Record the historical best position of each particle, that is, the position where the path optimization objective function value of the particle is the minimum in all iterations; record the global best position of the particle swarm, that is, the position where the path optimization objective function value is the minimum among all the historical best positions of all particles.

[0120] In this embodiment, the historical best position of each particle is recorded, that is, the position where the path optimization objective function value of the particle is minimized throughout all iterations. The global best position of the particle swarm is also recorded, that is, the position where the path optimization objective function value is minimized among all the historical best positions of all particles.

[0121] Step S1575: Start iterating according to the preset maximum number of iterations. In each iteration, calculate the particle's velocity vector based on the particle's current position, historical best position, and global best position, combined with inertia weight, cognitive coefficient, and social coefficient.

[0122] In this embodiment, iteration begins with a preset maximum number of iterations. During each iteration, the particle's velocity vector is calculated based on its current position, historical best position, and global best position, combined with inertia weight, cognitive coefficient, and social coefficient.

[0123] Step S1576: Update the particle's position based on the particle's velocity vector to obtain new path point coordinates. During the update process, constrain the particle's position so that the new path point coordinates are located within the corresponding path channel and satisfy the minimum distance limit with the obstacle cluster.

[0124] In this embodiment, the particle's position is updated based on its velocity vector to obtain new path point coordinates. During the update process, the particle's position is constrained, ensuring that the new path point coordinates are within the corresponding path channel and satisfy the minimum distance requirement to the obstacle cluster.

[0125] Step S1577: Calculate the path optimization objective function value for each particle after the update. If the path optimization objective function value is less than the path optimization objective function value corresponding to the historical best position of the particle, then update the historical best position of the particle; if the path optimization objective function value is less than the path optimization objective function value corresponding to the global best position of the particle swarm, then update the global best position of the particle swarm.

[0126] In this embodiment, the path optimization objective function value corresponding to each particle is calculated after the update. If the path optimization objective function value is less than the path optimization objective function value corresponding to the particle's historical best position, then the particle's historical best position is updated; if the path optimization objective function value is less than the path optimization objective function value corresponding to the global best position of the particle swarm, then the global best position of the particle swarm is updated.

[0127] Step S1578: Adjust the value of the inertia weight. Gradually decrease the inertia weight as the number of iterations increases. The particle has a global search capability greater than the set performance in the early stage of the iteration and a local search capability greater than the set performance in the later stage of the iteration.

[0128] In this embodiment, the value of the inertia weight is adjusted. The inertia weight is gradually decreased as the number of iterations increases, giving the particle a stronger global search capability in the early stages of iteration and a stronger local search capability in the later stages.

[0129] Step S1579: Repeat the above steps of velocity calculation, position update, path optimization objective function value calculation and optimal position update until the preset maximum number of iterations is reached or the path optimization objective function value corresponding to the global optimal position of the particle swarm no longer changes after multiple consecutive iterations.

[0130] In this embodiment, the steps of velocity calculation, position update, path optimization objective function value calculation and optimal position update are repeated until the preset maximum number of iterations is reached or the path optimization objective function value corresponding to the global optimal position of the particle swarm no longer changes after multiple consecutive iterations.

[0131] Step S15710: The path point coordinate sequence corresponding to the global optimal position of the particle swarm is taken as the path point coordinate of the optimal path. At the same time, the path point coordinate sequences corresponding to multiple particles with smaller path optimization objective function values ​​during the iteration process are selected together as multiple candidate paths generated.

[0132] In this embodiment, the path point coordinate sequence corresponding to the global optimal position of the particle swarm is used as the path point coordinate sequence of the optimal path. Simultaneously, the path point coordinate sequences corresponding to multiple particles with smaller path optimization objective function values ​​during the iteration process are selected together as multiple candidate paths to be generated.

[0133] Step S158: For each candidate path, for each path point, combine the assembly direction vector in the three-dimensional contour information of the assembly target and the motion range information of the robot end effector to calculate the initial attitude angle sequence corresponding to the path point. The initial attitude angle sequence includes the rotation angles around the first spatial coordinate axis, the second spatial coordinate axis, and the third spatial coordinate axis.

[0134] In this embodiment, for each candidate path, for each path point, the initial attitude angle sequence corresponding to that path point is calculated by combining the assembly direction vector in the 3D contour information of the assembly target and the motion range information of the robot end effector. The initial attitude angle sequence includes rotation angles around the first spatial coordinate axis, the second spatial coordinate axis, and the third spatial coordinate axis.

[0135] Step S159: Construct an attitude optimization objective function, which includes an attitude continuity term and an attitude feasibility term. The attitude continuity term is used to minimize the change in the attitude angle sequence corresponding to adjacent path points, and the attitude change conforms to the maximum rotation angle parameter of the robot end effector. The attitude feasibility term is used to ensure that the attitude angle sequence of each path point is within the motion range of the robot end effector.

[0136] In this embodiment, an attitude optimization objective function is constructed. The attitude optimization objective function includes an attitude continuity term and an attitude feasibility term. The attitude continuity term minimizes the change in the attitude angle sequence corresponding to adjacent path points, ensuring that the attitude change conforms to the maximum rotation angle parameter of the robot's end effector. The attitude feasibility term ensures that the attitude angle sequence of each path point is within the motion range of the robot's end effector.

[0137] Step S1510: Normalize the terms in the attitude optimization objective function, solve the attitude optimization objective function using the gradient descent algorithm, adjust the initial attitude angle sequence of each path point, and obtain the optimized attitude angle sequence.

[0138] In this embodiment, the terms in the attitude optimization objective function are normalized by converting the values ​​of each term to a specific range. The gradient descent algorithm is then used to solve the attitude optimization objective function, which finds the optimal solution by updating parameters along the negative gradient direction of the objective function. The initial attitude angle sequence for each path point is adjusted to obtain the optimized attitude angle sequence.

[0139] Step S1511: Establish a collaborative optimization model for path and attitude, combine the path optimization objective function and the attitude optimization objective function to form a collaborative optimization objective function, and simultaneously adjust the path point coordinates and the corresponding attitude angle sequence through iterative optimization. In each iteration, first adjust the path point coordinates according to the current attitude angle sequence, and then optimize the attitude angle sequence according to the adjusted path point coordinates, until the value of the collaborative optimization objective function converges to the preset convergence threshold.

[0140] In this embodiment, a collaborative optimization model for path and attitude is established, combining the path optimization objective function and the attitude optimization objective function to form a collaborative optimization objective function. The path point coordinates and corresponding attitude angle sequences are simultaneously adjusted through iterative optimization. In each iteration, the path point coordinates are first adjusted based on the current attitude angle sequence, and then the attitude angle sequence is optimized based on the adjusted path point coordinates. This continues until the value of the collaborative optimization objective function converges to a preset convergence threshold, which is set according to the actual application scenario.

[0141] Step S1512: During the collaborative optimization process, continuously check whether the path and attitude satisfy the obstacle avoidance path constraints ordered by execution order. If a constraint violation occurs, adjust the corresponding weight coefficients to prioritize the satisfaction of the constraints executed first.

[0142] In this embodiment, during the collaborative optimization process, it is continuously checked whether the path and attitude satisfy the obstacle avoidance path constraints ordered by execution sequence. If a constraint violation occurs, the corresponding weight coefficient is adjusted, increasing the weight coefficient of the item corresponding to the violated constraint to prioritize the satisfaction of constraints executed earlier.

[0143] Step S1513: After the collaborative optimization is completed, multiple interpolation points are inserted between adjacent path points. The number of interpolation points is determined according to the distance between adjacent path points and the motion stability constraint. The coordinates of the interpolation points are generated by the cubic spline interpolation algorithm, and the path is smooth and continuous.

[0144] In this embodiment, after collaborative optimization, multiple interpolation points are inserted between adjacent path points. The number of interpolation points is determined based on the distance between adjacent path points and motion stability constraints. The coordinates of the interpolation points are generated using a cubic spline interpolation algorithm, which can generate smooth and continuous curves. The path is smooth and continuous.

[0145] Step S1514: For each interpolation point, use a linear interpolation algorithm to calculate its corresponding attitude angle sequence. The attitude angle sequence of the interpolation point transitions continuously with the attitude angle sequence of the adjacent path point.

[0146] In this embodiment, a linear interpolation algorithm is used to calculate the corresponding attitude angle sequence for each interpolation point. The linear interpolation algorithm calculates the attitude angle sequence of the interpolation point by using the attitude angle sequences of adjacent path points. The attitude angle sequence of the interpolation point transitions continuously with the attitude angle sequences of adjacent path points.

[0147] Step S1515: Arrange the starting point, intermediate transition point, interpolation point and ending point in sequence to form a continuous position coordinate sequence. Match the attitude angle sequence corresponding to each coordinate point with the position coordinate sequence to form the dynamic obstacle avoidance trajectory of the robot end effector.

[0148] In this embodiment, the starting point, intermediate transition points, interpolation points, and ending point are arranged in chronological order to form a continuous sequence of position coordinates. The attitude angle sequence corresponding to each coordinate point is matched one-to-one with the position coordinate sequence, and integrated to form the dynamic obstacle avoidance trajectory of the robot's end effector.

[0149] Step S210: Perform virtual pre-simulation verification on the dynamic obstacle avoidance trajectory, simulate the process of the robot end effector moving along the trajectory, detect whether there is a potential collision risk, and obtain the trajectory pre-simulation result.

[0150] In this embodiment, a virtual pre-simulation verification process is performed on the dynamic obstacle avoidance trajectory. The process of the robot's end effector moving along the trajectory is simulated to detect any potential collision risks. The trajectory pre-simulation results are then obtained.

[0151] For example, step S211: Construct a virtual simulation environment for the 3D assembly space, and restore the three-dimensional outline of the assembly target, the spatial distribution of all static obstacles, the real-time position and motion characteristics of dynamic obstacles in the virtual simulation environment. At the same time, import the three-dimensional model of the robot end effector. The size and motion parameters of the three-dimensional model are consistent with the actual robot end effector.

[0152] In this embodiment, a virtual simulation environment for a 3D assembly space is constructed. The virtual simulation environment recreates the 3D contours of the assembly target, the spatial distribution of all static obstacles, and the real-time positions and motion characteristics of dynamic obstacles. Simultaneously, a 3D model of the robot's end effector is imported, with dimensions and motion parameters consistent with the actual robot end effector.

[0153] Step S212: Import the position coordinate sequence of the dynamic obstacle avoidance trajectory and the corresponding attitude angle sequence into the virtual simulation environment, set the time step of the virtual pre-performance, and the time step is adapted to the motion speed parameters of the robot end effector. The pre-performance process is synchronized with the actual motion process.

[0154] In this embodiment, the position coordinate sequence and corresponding attitude angle sequence of the dynamic obstacle avoidance trajectory are imported into the virtual simulation environment. A virtual pre-simulation time step is set, which is adapted to the motion speed parameters of the robot's end effector. The pre-simulation process is synchronized with the actual motion process.

[0155] Step S213: Drive the robot end effector model in the virtual simulation environment to move along the dynamic obstacle avoidance trajectory step by step according to the time step. In each time step, record the current position coordinates and current attitude angle of the robot end effector model.

[0156] In this embodiment, the robot end effector model in the virtual simulation environment is driven to move along a dynamic obstacle avoidance trajectory step by step according to the time step. Within each time step, the current position coordinates and current attitude angle of the robot end effector model are recorded.

[0157] Step S214: Within each time step, calculate the minimum distance between the robot end effector model and all static obstacle models in the virtual simulation environment, and calculate the minimum distance between the robot end effector model and all dynamic obstacle models. The position of the dynamic obstacle model is updated with time step according to its motion direction and speed characteristics.

[0158] In this embodiment, within each time step, the minimum distance between the robot end effector model and all static obstacle models in the virtual simulation environment is calculated. The minimum distance between the robot end effector model and all dynamic obstacle models is also calculated. The positions of the dynamic obstacle models are updated with each time step based on their motion direction and velocity characteristics.

[0159] Step S215: Compare the calculated minimum distance with the minimum distance limit in the obstacle avoidance path constraints ordered by execution order. If the minimum distance in all time steps is greater than the corresponding minimum distance limit, it is determined that the current dynamic obstacle avoidance trajectory has no potential collision risk; if the minimum distance in any time step is less than or equal to the corresponding minimum distance limit, it is determined that the current dynamic obstacle avoidance trajectory has a potential collision risk.

[0160] In this embodiment, the calculated minimum distance is compared with the minimum distance limit in the obstacle avoidance path constraints ordered by execution sequence. If the minimum distance in all time steps is greater than the corresponding minimum distance limit, the current dynamic obstacle avoidance trajectory is determined to have no potential collision risk. If the minimum distance in any time step is less than or equal to the corresponding minimum distance limit, the current dynamic obstacle avoidance trajectory is determined to have a potential collision risk.

[0161] Step S216: For time steps with potential collision risks, record the corresponding collision location coordinates, the type of obstacle involved in the collision, and the posture angle sequence at the time of the collision. Analyze the cause of the collision. If the cause of the collision is a deviation in the prediction of the motion trajectory of the dynamic obstacle, then re-evaluate the motion direction characteristics and motion speed characteristics of the dynamic obstacle. If the cause of the collision is an unreasonable setting of the path point coordinates, then mark the corresponding path point as the target for subsequent adjustment.

[0162] In this embodiment, for time steps with potential collision risks, the corresponding collision location coordinates, the type of obstacle involved in the collision, and the sequence of attitude angles at the time of the collision are recorded. The cause of the collision is analyzed. If the cause is a deviation in the prediction of the motion trajectory of the dynamic obstacle, the motion direction and speed characteristics of the dynamic obstacle are reassessed. If the cause is an unreasonable setting of path point coordinates, the corresponding path points are marked as targets for subsequent adjustments.

[0163] Step S217: Record the entire virtual rehearsal process and generate a rehearsal video file and a rehearsal data report. The rehearsal data report includes the position coordinates, attitude angle, minimum distance to obstacles, and collision risk assessment results for each time step.

[0164] In this embodiment, the entire virtual rehearsal process is recorded. A rehearsal video file and a rehearsal data report are generated. The rehearsal data report includes the position coordinates, attitude angle, minimum distance to obstacles, and collision risk assessment results for each time step.

[0165] Step S218: Combine the collision risk assessment results, collision cause analysis, and pre-simulation data report to form the trajectory pre-simulation results.

[0166] In this embodiment, the trajectory simulation result is formed by combining the collision risk assessment result, the collision cause analysis, and the simulation data report.

[0167] Step S220: If the trajectory pre-simulation result shows no potential collision risk, the dynamic obstacle avoidance trajectory is converted into a trajectory execution instruction that the robot can execute, and the trajectory execution instruction is sent to the robot control system to drive the robot to complete the assembly operation.

[0168] In this embodiment, if the trajectory pre-simulation results show no potential collision risk, the dynamic obstacle avoidance trajectory is converted into a trajectory execution command that the robot can execute. This trajectory execution command is then sent to the robot control system to drive the robot to complete the assembly operation.

[0169] For example, step S221: parse the continuous position coordinate sequence and the corresponding attitude angle sequence in the dynamic obstacle avoidance trajectory, and determine the timestamp information of each coordinate point and the corresponding attitude angle according to the motion speed parameters of the robot end effector. The timestamp information enables the robot end effector to maintain uniform motion from one coordinate point to the next coordinate point, and the motion time between adjacent coordinate points is positively correlated with the distance between the two points.

[0170] In this embodiment, a continuous sequence of position coordinates and corresponding attitude angles in the dynamic obstacle avoidance trajectory are analyzed. The timestamp information for each coordinate point and its corresponding attitude angle is determined based on the motion speed parameters of the robot's end effector. This timestamp information ensures that the robot's end effector maintains a constant speed as it moves from one coordinate point to the next, and that the movement time between adjacent coordinate points is positively correlated with the distance between the two points.

[0171] Step S222: Obtain the kinematic model parameters of the robot, which include the length of each joint, joint type, and joint range of motion limits. Based on the kinematic model parameters, establish the inverse kinematics solution model of the robot.

[0172] In this embodiment, the kinematic model parameters of the robot are obtained. These parameters include the length, joint type, and joint range of motion limitations of each joint. Based on these kinematic model parameters, an inverse kinematics model of the robot is established. This inverse kinematics model is used to solve for joint angles based on the position and orientation of the end effector.

[0173] Step S223: Input the three-dimensional coordinate values ​​of each coordinate point in the dynamic obstacle avoidance trajectory into the inverse kinematics solution model, and obtain the angle parameters corresponding to each joint of the robot through a numerical iterative algorithm. The robot end effector can reach the coordinate point and present the rotation angle in the corresponding posture angle sequence.

[0174] In this embodiment, the three-dimensional coordinates of each point in the dynamic obstacle avoidance trajectory are input into the inverse kinematics solution model. The angle parameters corresponding to each joint of the robot are obtained through a numerical iterative algorithm. The robot's end effector can reach the coordinate point and exhibit the rotation angle in the corresponding posture angle sequence.

[0175] Step S224: Check whether the joint angle parameters obtained by the solution are within the corresponding joint motion range limit. If all joint angle parameters are within the limit, retain the joint angle parameter. If any joint angle parameter exceeds the limit, adjust the corresponding posture angle sequence and re-input it into the inverse kinematics solution model for solution until joint angle parameters that meet the joint motion range limit are obtained.

[0176] In this embodiment, it is checked whether the obtained joint angle parameters are within the corresponding joint motion range limits. If all joint angle parameters are within the limits, the joint angle parameter is retained. If any joint angle parameter exceeds the limits, the corresponding posture angle sequence is adjusted and re-input into the inverse kinematics solution model for solving until joint angle parameters that meet the joint motion range limits are obtained.

[0177] Step S225: Integrate the joint angle parameters corresponding to each timestamp with the rotation angle parameters and coordinate point information in the posture angle sequence to form an instruction unit containing timestamp, joint angle parameters, rotation angle parameters and coordinate point information. Each instruction unit corresponds to a robot motion control instruction for one time step.

[0178] In this embodiment, the joint angle parameters corresponding to each timestamp are integrated with the rotation angle parameters and coordinate point information in the attitude angle sequence. This forms an instruction unit that includes timestamps, joint angle parameters, rotation angle parameters, and coordinate point information. Each instruction unit corresponds to a robot motion control instruction for one time step.

[0179] Step S226: Arrange all instruction units according to the order of timestamp information to form an ordered trajectory execution instruction sequence. Check whether the time interval between adjacent instruction units in the sequence is uniform. If there is an uneven time interval, adjust the corresponding timestamp information so that the time interval between adjacent instruction units is equal.

[0180] In this embodiment, all instruction units are arranged according to the chronological order of their timestamp information to form an ordered sequence of execution instructions. The time intervals between adjacent instruction units in the sequence are checked for uniformity. If uneven time intervals exist, the corresponding timestamp information is adjusted to ensure that the time intervals between adjacent instruction units are equal.

[0181] Step S227: Obtain the instruction format requirements of the robot control system. The instruction format requirements include the data encoding method, instruction field order, and check code calculation rules. Perform format conversion processing on the trajectory execution instruction sequence according to the instruction format requirements, encode the data in the instruction unit according to the specified encoding method, arrange the data according to the specified field order, and calculate the check code of each instruction unit.

[0182] In this embodiment, the instruction format requirements of the robot control system are obtained. These requirements include data encoding methods, instruction field order, and checksum calculation rules. The trajectory execution instruction sequence is then converted according to these requirements. Data in each instruction unit is encoded using the specified encoding method, arranged according to the specified field order, and a checksum is calculated for each instruction unit.

[0183] Step S228: Perform an integrity check on the execution instruction sequence of the converted trajectory. All instruction units contain field information and have correct check codes. If there are instruction units with missing fields or incorrect check codes, the format conversion process is repeated.

[0184] In this embodiment, an integrity check is performed on the converted trajectory execution instruction sequence. All instruction units contain field information and have correct checksums. If any instruction unit has missing fields or an incorrect checksum, the format conversion process is repeated.

[0185] Step S229: Send the converted trajectory execution instruction sequence to the robot control system through the communication interface between the robot and the control system. Data packet transmission is used during the transmission process.

[0186] In this embodiment, the converted trajectory execution command sequence is sent to the robot control system via the communication interface between the robot and the control system. During transmission, a data packetization method is used, dividing the command sequence into multiple data packets for transmission.

[0187] Step S2210: After receiving the trajectory execution instruction sequence, the robot control system parses and verifies the instruction sequence, executes each instruction unit in sequence according to the timestamp in the instruction sequence, drives each joint of the robot to move according to the corresponding joint angle parameters, and makes the robot end effector move along the dynamic obstacle avoidance trajectory to complete the assembly operation.

[0188] In this embodiment, after receiving the trajectory execution command sequence, the robot control system parses and verifies the command sequence. Each command unit is executed sequentially according to the timestamp in the command sequence, driving each joint of the robot to move according to the corresponding joint angle parameters. This causes the robot's end effector to move along a dynamic obstacle avoidance trajectory, completing the assembly operation.

[0189] Step S2211: During the assembly operation, the robot continuously receives real-time position and attitude information from the robot control system and compares it with the preset information of the dynamic obstacle avoidance trajectory. If the deviation is within the allowable range, the robot continues to execute; if the deviation exceeds the allowable range, the robot pauses execution and issues an adjustment command.

[0190] In this embodiment, during the robot's assembly operation, it continuously receives real-time position and attitude information from the robot control system. This information is compared with preset information of the dynamic obstacle avoidance trajectory. If the deviation is within the allowable range, execution continues; if the deviation exceeds the allowable range, execution is paused and an adjustment command is issued.

[0191] Step S230: If the trajectory pre-simulation result shows that there is a potential collision risk, return to the step of generating obstacle avoidance path constraints ordered by execution order, adjust the constraint parameters and regenerate the dynamic obstacle avoidance trajectory.

[0192] In this embodiment, if the trajectory simulation results indicate a potential collision risk, the process returns to the step of generating obstacle avoidance path constraints ordered by execution sequence. The constraint parameters are then adjusted, and the dynamic obstacle avoidance trajectory is regenerated.

[0193] For example, step S231: Analyze the collision information in the trajectory pre-simulation results to determine the collision location coordinates, the obstacle clusters involved in the collision, and the motion parameters at the time of the collision.

[0194] In this embodiment, collision information in the trajectory pre-simulation results is analyzed. The coordinates of potential collision locations, the clusters of obstacles involved in the collision, and the motion parameters at the time of the collision are determined.

[0195] Step S232: Analyze the cause of the collision. If the cause is that the minimum distance limit between the path and the obstacle is set too small, then increase the minimum distance limit corresponding to the obstacle cluster by a preset adjustment increment. The size of the adjustment increment is determined based on the difference between the minimum distance at the time of the collision and the original minimum distance limit. If the cause is that the continuous change limit of the path direction is set too loosely, causing the path to be too tortuous and close to the obstacle, then reduce the direction change threshold to make the path direction change smoother. If the cause is that the movement trend prediction of the dynamic obstacle is biased, then recalculate its movement direction characteristics and movement speed characteristics based on the real-time movement data of the dynamic obstacle and update the real-time obstacle association relationship.

[0196] In this embodiment, the causes of collisions are analyzed. If the collision is caused by an excessively small minimum distance limit between the path and the obstacle, a preset adjustment increment is added to the minimum distance limit corresponding to the obstacle cluster. The magnitude of the adjustment increment is determined based on the difference between the minimum distance at the time of collision and the original minimum distance limit. If the collision is caused by an excessively loose limit on the continuous change of path direction, resulting in an overly tortuous path close to the obstacle, the direction change threshold is reduced to make the path direction change smoother. If the collision is caused by a deviation in the prediction of the motion trend of dynamic obstacles, the motion direction and speed characteristics of the dynamic obstacles are recalculated based on the real-time motion data of the dynamic obstacles, and the real-time obstacle association is updated.

[0197] Step S233: Based on the adjusted minimum distance limit, direction change threshold, or updated real-time obstacle association, return to the step of generating obstacle avoidance path constraints ordered by execution order, and redetermine the overall spatial distribution range of the obstacle cluster, the boundary range of the path channel, and the parameters of each constraint.

[0198] In this embodiment, based on the adjusted minimum distance limit, direction change threshold, or updated real-time obstacle association, the process returns to the step of generating obstacle avoidance path constraints ordered by execution sequence. The overall spatial distribution range of the obstacle cluster, the boundary range of the path passage, and the parameters of each constraint are then redefined.

[0199] Step S234: Repeat the steps of generating dynamic obstacle avoidance trajectory according to the regenerated obstacle avoidance path constraints ordered by execution order. During the path generation process, focus on areas with previous collision risks, increase the number of path points in the area, and optimize the path direction.

[0200] In this embodiment, the step of generating a dynamic obstacle avoidance trajectory is repeated according to the regenerated obstacle avoidance path constraints ordered by execution sequence. During path generation, special attention is paid to areas with previous collision risks, increasing the number of path points in these areas and optimizing the path direction.

[0201] Step S235: Perform virtual pre-simulation verification on the regenerated dynamic obstacle avoidance trajectory again to detect whether there are still potential collision risks.

[0202] In this embodiment, the regenerated dynamic obstacle avoidance trajectory is again subjected to virtual pre-simulation verification to detect whether any potential collision risks still exist.

[0203] Step S236: If the re-simulation result shows no potential collision risk, continue to execute the subsequent trajectory execution command conversion and sending steps; if the re-simulation result still shows a potential collision risk, repeat the above steps of adjusting constraint parameters, regenerating trajectory, and re-simulation verification until the generated dynamic obstacle avoidance trajectory passes the re-simulation verification.

[0204] In this embodiment, if the re-simulation result shows no potential collision risk, the subsequent trajectory execution command conversion and transmission steps continue. If the re-simulation result still shows a potential collision risk, the steps of adjusting constraint parameters, regenerating the trajectory, and re-simulating verification are repeated until the generated dynamic obstacle avoidance trajectory passes the re-simulation verification.

[0205] Step S237: Record the constraint parameters, trajectory changes, and pre-simulation results for each adjustment to form an adjustment log.

[0206] In this embodiment, the constraint parameters, trajectory changes, and pre-simulation results of each adjustment are recorded to form an adjustment log.

[0207] Step S238: If a collision-free trajectory cannot be generated after multiple adjustments, assess whether the current obstacle distribution in the assembly space is suitable for continuing the assembly task. If the obstacle distribution results in no feasible path, issue an obstacle clearing prompt. After the obstacle clearing is completed, reacquire the 3D assembly space data and start a new round of obstacle avoidance trajectory generation process.

[0208] In this embodiment, if a collision-free trajectory cannot be generated after multiple adjustments, the obstacle distribution in the current assembly space is assessed to determine if it is suitable to continue the assembly task. If the obstacle distribution results in no feasible path, an obstacle clearing prompt is issued. After the obstacle clearing is completed, the 3D assembly space data is reacquired, and a new round of obstacle avoidance trajectory generation begins.

[0209] Based on the same inventive concept, please refer to Figure 2 This paper shows a schematic block diagram of a robot assembly space obstacle avoidance system 100 for 3D perception, which is provided in an embodiment of this application for executing the above-described robot assembly space obstacle avoidance method for 3D perception. The robot assembly space obstacle avoidance system 100 for 3D perception may include a communication unit 110, a machine-readable storage medium 120, and a processor 130.

[0210] In this embodiment, both the machine-readable storage medium 120 and the processor 130 are located within the robot assembly space obstacle avoidance system 100 for 3D perception and are separately configured. However, it should be understood that the machine-readable storage medium 120 may also be independent of the robot assembly space obstacle avoidance system 100 for 3D perception and may be accessed by the processor 130 via a bus interface. Alternatively, the machine-readable storage medium 120 may be integrated into the processor 130 and may communicate with external systems via the communication unit 110.

[0211] The processor 130 is the control center of the 3D perception robot assembly space obstacle avoidance system 100. It connects to various parts of the system via various interfaces and lines. By running or executing software programs and / or modules stored in the machine-readable storage medium 120, and by calling data stored in the machine-readable storage medium 120, it performs various functions and processes data of the 3D perception robot assembly space obstacle avoidance system 100, thereby providing overall monitoring of the system. Optionally, the processor 130 may include one or more processing cores; for example, the processor 130 may integrate an application processor and a modem processor, wherein the application processor mainly handles the operating system, user interface, and applications, and the modem processor mainly handles wireless communication. It is understood that the modem processor may also not be integrated into the processor. The machine-readable storage medium 120 is used to store machine-executable instructions for executing the scheme of this application, and the processor 130 is used to execute the machine-executable instructions stored in the machine-readable storage medium 120 to implement the obstacle avoidance method for robot assembly space applied to 3D perception provided in the aforementioned method embodiment.

[0212] It should be noted that, in order to simplify the description of the present invention and thus help to understand one or more embodiments of the invention, multiple features may sometimes be grouped into one embodiment, drawing or description thereof in the foregoing description of the embodiments of the present invention.

Claims

1. A method for obstacle avoidance in robot assembly space applied to 3D perception, characterized in that, The method includes: Acquire 3D assembly space data of the robot assembly area. The 3D assembly space data includes the three-dimensional contour information of the assembly target, the spatial distribution information of static obstacles, the real-time position information of dynamic obstacles, and the motion range information of the robot end effector. Spatial obstacle features are extracted from the 3D assembly space data, and initial obstacle associations are established based on the spatial obstacle features. The spatial obstacle features include the three-dimensional size features, surface curvature features, and relative position features with the assembly target of static obstacles, as well as the motion direction features and motion speed features of dynamic obstacles. The initial obstacle associations are used to represent the spatial positional dependencies between static obstacles and between static obstacles and dynamic obstacles. The initial obstacle association is dynamically updated by adjusting the strength of the association edges based on the real-time location information of the dynamic obstacles to generate real-time obstacle associations. Based on the real-time obstacle association and the motion range information of the robot end effector, obstacle avoidance path constraints are generated in the order of execution. The obstacle avoidance path constraints in the order of execution include collision avoidance constraints executed first, motion stability constraints executed after collision avoidance constraints, and path length constraints executed last. The collision avoidance constraints include the minimum distance limit between the path and the obstacle, and the motion stability constraints include the limit on the continuous change of the path direction. Based on the obstacle avoidance path constraints ordered by execution sequence and the three-dimensional contour information of the assembly target, a dynamic obstacle avoidance trajectory of the robot end effector is generated. The dynamic obstacle avoidance trajectory includes a continuous sequence of position coordinates and a corresponding sequence of attitude angles.

2. The obstacle avoidance method for robot assembly space applied to 3D perception as described in claim 1, characterized in that, The step of extracting spatial obstacle features from the 3D assembly space data and establishing initial obstacle associations based on the spatial obstacle features includes: The 3D assembly space data is divided into multiple resolution regions. The division resolution is set according to the proportion of obstacles at different locations in the assembly area to obtain multiple non-overlapping three-dimensional sub-regions. Each three-dimensional sub-region corresponds to a local space in the assembly area. Regions with a proportion of obstacles greater than a preset proportion threshold are divided with high resolution, while regions with a proportion of obstacles less than or equal to the preset proportion threshold are divided with low resolution. Static and dynamic obstacles are identified in each three-dimensional sub-region. For static obstacles, their length parameters in the three spatial coordinate axes are extracted as the three-dimensional size features of the static obstacles. The surface of each static obstacle is sampled in layers. The sampling density is set according to the curvature change trend of the static obstacle surface. The coordinate information of multiple surface sampling points is obtained. The number of sampling points is increased in areas where the curvature change is greater than the preset curvature change threshold, and the number of sampling points is reduced in areas where the curvature change is less than or equal to the preset curvature change threshold. For each surface sampling point, a local neighborhood range adapted to the surface curvature is set around it. Other sampling points located within this local neighborhood range are selected from the coordinate information of all surface sampling points to form a local neighborhood sampling point set for that sampling point. The local neighborhood range is narrowed for regions with curvature values ​​greater than a preset curvature threshold, and the local neighborhood range is expanded for regions with curvature values ​​less than or equal to the preset curvature threshold. Based on the coordinate information of the local neighborhood sampling point set, the equation of the local tangent plane passing through the sampling point is fitted using the least squares method. The deviation angle between the normal vector at the sampling point and the normal vector of the local tangent plane equation is calculated. Combined with the distance parameters from each point in the local neighborhood sampling point set to the local tangent plane, the curvature value at the sampling point is determined by the curvature calculation formula. Statistical analysis was performed on the curvature values ​​of all sampling points to calculate the maximum, minimum, average and variance of the curvature values, which together were used as the surface curvature characteristics of the static barrier. The reference coordinate position of the assembly target in the 3D assembly space is determined. The reference coordinate position is the geometric center coordinate of the assembly target. The vector difference between the geometric center coordinate of each static obstacle and the reference coordinate position is calculated. The vector difference contains magnitude and direction information. The vector difference is used as the relative position feature between the static obstacle and the assembly target. For dynamic obstacles, extract their position coordinate information at two consecutive acquisition times, calculate the vector difference between the two position coordinate information, use the direction of the vector difference as the motion direction feature of the dynamic obstacle, and use the ratio of the magnitude of the vector difference to the time interval between the two acquisition times as the motion speed feature of the dynamic obstacle. Collect the three-dimensional size features, surface curvature features, and relative position features with the assembly target of all static obstacles, as well as the motion direction features and motion speed features of all dynamic obstacles, to form the spatial obstacle features corresponding to each obstacle; Analyze the relative position features of spatial obstacles in different static obstacles, calculate the spatial distance parameter and the directional angle parameter between any two static obstacles. If the spatial distance parameter is less than the preset associated distance threshold and the directional angle parameter is within the preset associated angle range, it is determined that there is a direct spatial occlusion relationship between the two static obstacles, and an associated edge is established between the two static obstacles. Analyze the spatial obstacle characteristics of static and dynamic obstacles, calculate the spatial distance parameter between the geometric center coordinates of the static obstacle and the current position coordinates of the dynamic obstacle, and determine whether the dynamic obstacle has a tendency to move towards the static obstacle by combining the movement direction characteristics of the dynamic obstacle. If the spatial distance parameter is less than the association distance threshold and the dynamic obstacle has a tendency to move towards the static obstacle, then establish the association edge between the static obstacle and the dynamic obstacle. All static and dynamic obstacles are treated as nodes. Nodes with associated edges are connected together. An initial association strength value is assigned to each associated edge. The initial association strength value is negatively correlated with the corresponding spatial distance parameter, forming an initial obstacle association relationship that represents the spatial positional dependence between static obstacles and between static and dynamic obstacles.

3. The obstacle avoidance method for robot assembly space applied to 3D perception as described in claim 2, characterized in that, The step of dynamically updating the initial obstacle association relationship, adjusting the strength of the association edges in the relationship based on the real-time location information changes of the dynamic obstacles, and generating a real-time obstacle association relationship includes: The system continuously collects real-time location information of dynamic obstacles according to the set dynamic update time interval, and records the timestamp and corresponding location coordinates of each collection. For each dynamic obstacle, calculate the position offset between its current real-time position coordinates and the position coordinates of the last acquisition, and update the motion direction characteristics and motion speed characteristics of the dynamic obstacle according to the position offset. Traverse all associated edges in the initial obstacle association relationship that are associated with the dynamic obstacle, including the associated edges between the dynamic obstacle and other static obstacles, and the associated edges between the dynamic obstacle and other dynamic obstacles; For each associated edge, recalculate the current spatial distance parameter between the two obstacles connected by the associated edge. If the associated edge connects a dynamic obstacle and a static obstacle, calculate the spatial distance parameter between the current real-time position coordinates of the dynamic obstacle and the geometric center coordinates of the static obstacle. If the associated edge connects two dynamic obstacles, calculate the spatial distance parameter between the current real-time position coordinates of the two dynamic obstacles. The association strength value of the corresponding associated edge is adjusted according to the recalculated spatial distance parameter. If the spatial distance parameter decreases, the association strength value is increased; if the spatial distance parameter increases, the association strength value is decreased; if the spatial distance parameter is greater than the association distance threshold, the association strength value is adjusted to zero and the associated edge is disconnected. Check if there are any new combinations of obstacles that need to be associated, calculate the current spatial distance parameters between obstacles that have not been associated, and determine whether the association conditions are met by combining the movement direction characteristics of dynamic obstacles. If they are met, establish new associated edges and assign initial association strength values. After normalizing the association strength values ​​of all associated edges, the adjusted nodes, associated edges, and association strength values ​​are integrated to form real-time obstacle association relationships. These real-time obstacle association relationships can reflect the spatial positional dependencies between all obstacles at the current moment.

4. The obstacle avoidance method for robot assembly space applied to 3D perception as described in claim 1, characterized in that, The step of generating obstacle avoidance path constraints ordered by execution sequence based on the real-time obstacle association and the motion range information of the robot's end effector includes: The real-time obstacle association is analyzed to identify the obstacle combination connected by the association edge whose association strength value is greater than a preset strength threshold, and the obstacle combination is divided into multiple obstacle clusters. For each obstacle cluster, the spatial obstacle features of all obstacles within the cluster are collected. If the cluster contains static obstacles, the three-dimensional size features and surface curvature features of the static obstacles are extracted. If the cluster contains dynamic obstacles, the motion direction features, motion speed features, and current real-time position coordinates of the dynamic obstacles are extracted. Based on the spatial obstacle characteristics of all obstacles within the cluster, the overall spatial distribution range of the obstacle cluster is determined. The overall spatial distribution range is the smallest three-dimensional bounding box that includes all obstacles within the cluster. The length, width, height, and geometric center coordinates of the three-dimensional bounding box are calculated. Combining the three-dimensional dimension parameters of the end effector in the motion range information of the robot end effector, the safe buffer distance that the robot end effector needs to maintain when approaching the obstacle cluster is calculated. The safe buffer distance is the sum of the maximum dimension value in the three-dimensional dimension parameters of the end effector and the preset safety margin. The safe buffer distance is used as the minimum distance limit between the path and all obstacles in the obstacle cluster and is incorporated into the collision avoidance constraints executed first. Extract the maximum rotation angle parameter and maximum translation acceleration parameter from the motion range information of the robot end effector, and combine them with the accuracy requirements of the assembly operation to determine the allowable directional change threshold and acceleration change threshold of the robot end effector during the motion process; The continuous change limit of the path direction is set according to the direction change threshold, that is, the direction angle between two adjacent path points shall not exceed the direction change threshold; the continuous change limit of the path acceleration is set according to the acceleration change threshold, that is, the acceleration change between two adjacent path segments shall not exceed the acceleration change threshold. The continuous change limit of the path direction and the continuous change limit of the path acceleration are jointly incorporated into the motion stability constraint executed after the collision avoidance constraint. Analyze the spatial gaps between different obstacle clusters, calculate the three-dimensional dimensional parameters of each spatial gap, and if the three-dimensional dimensional parameters of the gap are all greater than the sum of the three-dimensional dimensional parameters of the robot end effector and the safety buffer distance, then the gap is identified as a potential path channel. The three-dimensional dimensional parameters include the width, height and depth of the gap. For each potential path passage, the boundary range of the path within the passage is set in combination with the minimum distance limit between the path and the obstacle. The boundary range is the area where a safe buffer distance is maintained between the inner wall of the path passage and the corresponding obstacle cluster. The path is always located inside the path passage and maintains the minimum distance limit with the obstacles on both sides of the path passage. Based on the total time requirement of the assembly task, a maximum allowable value for the path length is set, and the path length not exceeding the maximum allowable value is used as the final path length constraint. The collision avoidance constraints, motion stability constraints, and path length constraints are ordered in that order of execution. Collision avoidance constraints are executed before motion stability constraints, and motion stability constraints are executed before path length constraints. In the subsequent trajectory generation process, if different constraints conflict, the constraints that are executed first are given priority, thus forming obstacle avoidance path constraints ordered in the order of execution.

5. The obstacle avoidance method for robot assembly space applied to 3D perception according to claim 4, characterized in that, The process involves parsing the real-time obstacle relationships, identifying obstacle combinations connected by edges whose association strength values ​​are greater than a preset strength threshold, and dividing these obstacle combinations into multiple obstacle clusters, including: Initialize the cluster identifier counter, set the initial value of the cluster identifier counter to zero, and create an empty cluster list to store the partitioned obstacle clusters; Select an obstacle that has not been assigned a cluster from all nodes in the real-time obstacle association relationship as the starting node, use the current value of the cluster identifier counter as the cluster identifier of the starting node, and add the starting node to the temporary cluster set. Traverse all associated edges connected to the starting node in the real-time obstacle association relationship, check whether the association strength value of each associated edge is greater than the preset strength threshold. If the association strength value is greater than the strength threshold, obtain the associated node at the other end of the associated edge. Check whether the associated node has been assigned to a cluster. If it has not been assigned to a cluster, set the cluster identifier of the associated node to the value of the current cluster identifier counter and add the associated node to the temporary cluster set. Continue traversing all the associated edges of newly added nodes in the temporary cluster set, repeating the steps of checking the association strength value, obtaining associated nodes, and assigning cluster identifiers, until no more new nodes are added to the temporary cluster set; All nodes in the temporary cluster set are treated as a barrier cluster. The current cluster identifier counter value is assigned to the barrier cluster, and the barrier cluster is added to the cluster list. Increment the value of the cluster identifier counter by one, clear the temporary cluster set, select the next unassigned cluster obstacle from all nodes of the real-time obstacle association as the new starting node, and repeat the above steps. If there are isolated nodes in the real-time obstacle association that have not been assigned a cluster, then create a separate obstacle cluster for each isolated node, assign an independent cluster identifier, and add it to the cluster list. Iterate through all obstacle clusters in the cluster list and check if there are any associated edges between the clusters. If there are associated edges between nodes in two obstacle clusters with an associated strength value greater than the strength threshold, then merge the two obstacle clusters into a new obstacle cluster and update the cluster identifier and cluster list. Repeat the above merging steps until there are no more associated edges with an association strength value greater than the strength threshold between all obstacle clusters in the cluster list, and finally obtain multiple obstacle clusters.

6. The obstacle avoidance method for robot assembly space applied to 3D perception according to claim 1, characterized in that, The generation of the dynamic obstacle avoidance trajectory for the robot's end effector based on the obstacle avoidance path constraints ordered by execution sequence and the three-dimensional contour information of the assembly target includes: The three-dimensional contour information of the assembly target is analyzed, and the coordinates of the assembly reference point and the assembly direction vector on the assembly target are extracted. The coordinates of the assembly reference point are the target position that the robot end effector needs to reach, and the assembly direction vector is the attitude reference direction of the robot end effector when it completes the assembly operation at the target position. The current position coordinates and current attitude angle of the robot end effector are obtained. The current position coordinates are used as the starting point of the dynamic obstacle avoidance trajectory, and the assembly reference point coordinates are used as the ending point of the dynamic obstacle avoidance trajectory. An initial straight path from the starting point to the ending point is initially planned in the 3D assembly space. Check whether the initial straight path satisfies the collision avoidance constraint executed first among the obstacle avoidance path constraints ordered by execution order. That is, calculate whether the distance between each point on the initial straight path and all obstacle clusters is greater than the corresponding minimum distance limit. If all points meet the distance requirement, the initial straight path is used as a candidate path. If there are points that do not meet the distance requirement, multiple intermediate transition points are set at the intersection of the initial straight path and the overall spatial distribution range of the obstacle clusters. The number of intermediate transition points is determined according to the size of the intersection area and the number of path channels. Based on the path channel boundary range in the obstacle avoidance path constraints ordered by execution order, adjust the coordinate position of each intermediate transition point. Each intermediate transition point is located within the corresponding path channel, and the distance between the intermediate transition point and the surrounding obstacle cluster meets the minimum distance limit. Using the starting point, intermediate transition point, and ending point as critical path points, a path optimization objective function is constructed. The path optimization objective function includes a path length term, a path smoothness term, and an attitude adaptation term. The path length term is used to minimize the total path length to meet the path length constraint of the last execution. The path smoothness term is used to minimize the directional change of adjacent path segments to meet the motion stability constraint of the execution following the collision avoidance constraint. The attitude adaptation term is used to minimize the deviation between the attitude of the end effector and the assembly direction vector. Normalize the terms in the path optimization objective function and set weight coefficients for each term in the normalized path optimization objective function. The weight coefficients are determined according to the execution order of the obstacle avoidance path constraints ordered by execution order. The weight coefficient of the path smoothness term is greater than the weight coefficient of the path length term, and the weight coefficient of the attitude adaptability term is the same as the weight coefficient of the path smoothness term. The particle swarm optimization algorithm is used to solve the path optimization objective function, generating multiple candidate paths, each containing the coordinate information of multiple path points; For each candidate path, for each path point, the initial attitude angle sequence corresponding to the path point is calculated by combining the assembly direction vector in the three-dimensional contour information of the assembly target and the motion range information of the robot end effector. The initial attitude angle sequence includes the rotation angles around the first spatial coordinate axis, the second spatial coordinate axis, and the third spatial coordinate axis. A posture optimization objective function is constructed, which includes a posture continuity term and a posture feasibility term. The posture continuity term is used to minimize the change in the posture angle sequence corresponding to adjacent path points, and the posture change conforms to the maximum rotation angle parameter of the robot end effector. The posture feasibility term is used to ensure that the posture angle sequence of each path point is within the motion range of the robot end effector. The terms in the attitude optimization objective function are normalized, and the gradient descent algorithm is used to solve the attitude optimization objective function. The initial attitude angle sequence of each path point is adjusted to obtain the optimized attitude angle sequence. A collaborative optimization model for path and attitude is established, which combines the objective function of path optimization with the objective function of attitude optimization to form a collaborative optimization objective function. The path point coordinates and the corresponding attitude angle sequence are adjusted simultaneously through iterative optimization. In each iteration, the path point coordinates are first adjusted according to the current attitude angle sequence, and then the attitude angle sequence is optimized according to the adjusted path point coordinates until the value of the collaborative optimization objective function converges to the preset convergence threshold. During the collaborative optimization process, we continuously check whether the path and attitude satisfy the obstacle avoidance path constraints ordered by execution order. If a constraint violation occurs, we adjust the corresponding weight coefficients to ensure that the constraints executed first are satisfied. After the collaborative optimization is completed, multiple interpolation points are inserted between adjacent path points. The number of interpolation points is determined according to the distance between adjacent path points and the motion stability constraint. The coordinates of the interpolation points are generated by the cubic spline interpolation algorithm, and the path is smooth and continuous. For each interpolation point, a linear interpolation algorithm is used to calculate its corresponding attitude angle sequence, and the attitude angle sequence of the interpolation point transitions continuously with the attitude angle sequence of the adjacent path point. The starting point, intermediate transition points, interpolation points, and ending point are arranged in chronological order to form a continuous sequence of position coordinates. The attitude angle sequence corresponding to each coordinate point is matched one-to-one with the position coordinate sequence, and integrated to form the dynamic obstacle avoidance trajectory of the robot's end effector.

7. The obstacle avoidance method for robot assembly space applied to 3D perception according to claim 6, characterized in that, The particle swarm optimization algorithm is used to solve the path optimization objective function, generating multiple candidate paths, including: Determine the algorithm parameters for particle swarm size, maximum number of iterations, inertia weight, cognitive coefficient, and social coefficient. Each particle in the particle swarm represents a candidate path, and each path point contains three spatial coordinates. The particle dimension is the number of path points multiplied by three. Initialize the position of each particle in the particle swarm. The position of each particle corresponds to the path point coordinates of a candidate path. During initialization, the path point coordinates are randomly generated based on the coordinates of the starting point, intermediate transition points, and the ending point. The generated path point coordinates are within a reasonable range of the 3D assembly space. Calculate the path optimization objective function value for each particle. Specifically, calculate the values ​​of the path length term, path smoothness term, and attitude adaptation term based on the particle's position, and combine them with weighting coefficients to obtain the total value of the path optimization objective function. Record the historical best position of each particle, that is, the position where the path optimization objective function value of the particle is minimized in all iterations; record the global best position of the particle swarm, that is, the position where the path optimization objective function value is minimized among all the historical best positions of all particles; The iteration begins with the preset maximum number of iterations. During each iteration, the particle's velocity vector is calculated based on its current position, historical best position, and global best position, combined with inertia weight, cognitive coefficient, and social coefficient. The particle's position is updated based on its velocity vector to obtain new path point coordinates. During the update process, the particle's position is constrained so that the new path point coordinates are located within the corresponding path channel and satisfy the minimum distance limit with the obstacle cluster. Calculate the path optimization objective function value for each particle after the update. If the path optimization objective function value is less than the path optimization objective function value corresponding to the particle's historical best position, then update the particle's historical best position. If the path optimization objective function value is less than the path optimization objective function value corresponding to the global best position of the particle swarm, then update the global best position of the particle swarm. The value of the inertia weight is adjusted, and the inertia weight is gradually reduced as the number of iterations increases. In the early stage of the iteration, the particle has a global search capability that is greater than the set performance, and in the later stage of the iteration, it has a local search capability that is greater than the set performance. Repeat the above steps of velocity calculation, position update, path optimization objective function value calculation and optimal position update until the preset maximum number of iterations is reached or the path optimization objective function value corresponding to the global optimal position of the particle swarm no longer changes after multiple consecutive iterations; The path point coordinate sequence corresponding to the global optimal position of the particle swarm is used as the path point coordinate of the optimal path. At the same time, the path point coordinate sequences corresponding to multiple particles whose path optimization objective function value is less than the set function value are selected during the iteration process and used together as multiple candidate paths to generate.

8. The obstacle avoidance method for robot assembly space applied to 3D perception according to claim 1, characterized in that, The method further includes: The dynamic obstacle avoidance trajectory is subjected to virtual pre-simulation verification processing to simulate the process of the robot's end effector moving along the trajectory, detect whether there are potential collision risks, and obtain trajectory pre-simulation results; If the trajectory pre-simulation result shows no potential collision risk, the dynamic obstacle avoidance trajectory is converted into a trajectory execution instruction that the robot can execute, and the trajectory execution instruction is sent to the robot control system to drive the robot to complete the assembly operation; if the trajectory pre-simulation result shows a potential collision risk, the process returns to the step of generating obstacle avoidance path constraints ordered by execution order, and the dynamic obstacle avoidance trajectory is regenerated after adjusting the constraint parameters.

9. A robot assembly space obstacle avoidance system applied to 3D perception, characterized in that, include: processor; A machine-readable storage medium for storing machine-executable instructions of the processor; The processor is configured to execute the robot assembly space obstacle avoidance method for 3D perception as described in any one of claims 1 to 8 by executing the machine-executable instructions.

10. A computer program product, characterized in that, The computer program product includes machine-executable instructions stored in a computer-readable storage medium. A processor of a computer device reads the machine-executable instructions from the computer-readable storage medium and executes the machine-executable instructions, causing the computer device to perform the obstacle avoidance method for robot assembly space applied to 3D perception as described in any one of claims 1 to 8.

Images