A method for obstacle avoidance path planning for a multi-degree-of-freedom robotic arm based on 3D reconstruction

Abstract

This invention relates to the field of obstacle avoidance path planning technology for robotic arms, and in particular to a method for obstacle avoidance path planning for multi-degree-of-freedom robotic arms based on 3D reconstruction. The method includes the following steps: Step 1, 3D environment perception and dynamic modeling; Step 2, obstacle feature quantification and extraction; Step 3, when expanding new nodes in the joint space, nodes near obstacles are preferentially expanded towards concave regions or the center of holes to generate a candidate node set; Step 4, 3D mesh collision verification and safe path correction: the robotic arm's workspace is divided into a 3D voxel mesh, and collision detection is achieved by determining whether a path node falls within an obstacle voxel. This invention uses a LiDAR and a depth camera to synchronously acquire data via hardware triggering, combined with statistical filtering for noise reduction and 3D mesh modeling, to restore the geometric features of static obstacles and the motion parameters of dynamic obstacles, enabling the robotic arm to effectively avoid collision risks caused by environmental perception errors.

Description

Technical Field

[0001] This invention relates to the field of obstacle avoidance path planning technology for robotic arms, and in particular to a method for obstacle avoidance path planning for multi-degree-of-freedom robotic arms based on three-dimensional reconstruction. Background Technology

[0002] Obstacle avoidance path planning for robotic arms is one of the core technologies for realizing autonomous operation of robotic arms. Its purpose is to plan a collision-free path from the starting point to the target point for the robotic arm within the robotic arm's motion space, while satisfying conditions such as joint kinematic constraints and dynamic performance limitations of the robotic arm.

[0003] With the development of industrial automation, intelligent warehousing, medical surgery and other fields, multi-degree-of-freedom robotic arms are widely used due to their flexible operation capabilities. However, the complexity of the working environment has placed higher demands on the accuracy and real-time performance of obstacle avoidance path planning.

[0004] For example, the invention patent with announcement number CN112476438B discloses a robotic arm obstacle avoidance method, device, robotic arm and robot. Its main idea is to construct a motion space model of the robotic arm by obtaining the current position and target position of the robotic arm.

[0005] Based on the content described in the aforementioned patent, it can be found that its obstacle avoidance method has the following drawbacks:

[0006] First, the perception of the environment relies on a pre-set geometric model. When there are unknown obstacles or dynamically changing obstacles (such as suddenly appearing moving objects) in the working environment, it is difficult to update the environmental information in real time, which can easily lead to path planning failure or collision risk.

[0007] Second, its application in highly dynamic and complex working conditions has obvious limitations.

[0008] For example, in multi-robotic arm collaborative assembly workshops and human-machine hybrid warehousing scenarios, traditional methods struggle to handle real-time path conflicts of multiple dynamic targets (such as the movement of other robotic arms and the movement of operators) simultaneously.

[0009] On the one hand, its path search efficiency is greatly affected by the number of obstacles in the environment. When there are multiple dynamic obstacles, it is easy to fall into the dilemma of local optimal solutions or repeated path replanning.

[0010] On the other hand, the lack of an obstacle avoidance priority mechanism designed for multi-subject interaction scenarios may lead to path competition between robotic arms and between robotic arms and human bodies, resulting in decreased work efficiency or even safety accidents.

[0011] Therefore, it is necessary to design a method that can perceive the three-dimensional environment in real time, accurately describe the spatial shape of obstacles, and realize efficient obstacle avoidance path planning on this basis. Summary of the Invention

[0012] To solve one of the aforementioned technical problems, the present invention provides a method for obstacle avoidance path planning of a multi-degree-of-freedom robotic arm based on three-dimensional reconstruction, comprising the following steps:

[0013] Step 1, 3D environment perception and dynamic modeling: Data is collected synchronously using LiDAR and depth camera via hardware triggering. Based on the checkerboard calibration board, the point cloud and image fusion error is ≤5mm. Then, the fused point cloud is subjected to statistical filtering to remove noise, and a 3D mesh model is generated through 3D mesh modeling.

[0014] Using the inter-frame difference method, points with an Euclidean distance greater than 5mm between corresponding points in two consecutive point clouds are marked as dynamic points, and connected regions with a proportion of dynamic points greater than 5% are identified as dynamic obstacles. The model is updated every 100ms.

[0015] Step 2, obstacle feature quantification and extraction.

[0016] For static obstacles, the Gaussian curvature of the surface is calculated, and curvature > 0.01 mm⁻¹ is set as a convex region and curvature < -0.01 mm⁻¹ is set as a concave region. The hole size is measured using the minimum circumcircle method. For dynamic obstacles, motion parameters are calculated based on Kalman filtering, and the velocity vector and acceleration are output.

[0017] Step 3: When expanding new nodes in the joint space, prioritize expanding near-obstacle nodes towards the concave region or the center of the hole to generate a candidate node set.

[0018] Step 3 prioritizes offsetting the near-obstacle node towards the concave region / hole (combining orientation angle and dynamic constraints), utilizing environmental geometry to reduce collision risk.

[0019] Step 4: 3D Mesh Collision Detection and Safe Path Correction: The robotic arm's workspace is divided into a 1mm³ 3D voxel mesh, and collision detection is achieved by determining whether path nodes fall into obstacle voxels.

[0020] Step 4 uses a 1mm³ voxel mesh for high-precision collision detection and generates a local correction segment (safe distance ≥15mm) for the collision path using a 3rd-order B-spline. This balances the smoothness of the path (B-spline characteristics) with safety, and the design meets the actual needs of the robotic arm in obstacle avoidance in complex environments.

[0021] This planning method uses simultaneous data acquisition by LiDAR and depth camera, and achieves point cloud and image fusion through checkerboard calibration (error ≤ 5mm). Combined with statistical filtering for noise reduction and 3D mesh modeling, it can construct an environment model with relatively high accuracy. By using the inter-frame difference method (marking dynamic points with Euclidean distance > 5mm, and determining dynamic obstacles with a proportion > 5%) and updating in real time (100ms / time), it takes into account both the accuracy and timeliness of dynamic environment perception, which is in line with the technical logic of dynamic scene modeling.

[0022] Based on any of the above technical solutions, the following optimization is made: The specific steps for synchronously acquiring environmental data by the LiDAR and depth camera in step 1 are executed in the following sequence: Hardware connection: Connect the trigger input interface of the 16-line LiDAR and the synchronization input interface of the binocular structured light camera to the output of the signal distributor respectively, and connect the input of the signal distributor to the GPIO output pin of the same controller.

[0023] The 16-line lidar has a ranging range of 0.5-100m and an angular resolution of 0.2°×0.4°; the binocular structured light camera has a baseline distance of 120mm and a depth measurement range of 0.3-3m.

[0024] The controller uses an STM32 microcontroller.

[0025] Signal generation: The controller generates a 3.3V level trigger signal with a pulse width of 10μs every 100ms. This signal is simultaneously transmitted to the LiDAR and depth camera via the signal distributor.

[0026] Data acquisition: For each trigger signal received by the lidar, one frame of point cloud data is output at a scanning frequency of 10Hz; for each three trigger signals received by the depth camera, one frame of RGB-D image is output at a frame rate of 30Hz.

[0027] Timestamp recording: The controller timestamps the laser point cloud and depth image respectively using a built-in real-time clock.

[0028] Synchronous calibration: Calculate the difference between the timestamps of the two types of data. If it is ≤10ms, maintain the current triggering mechanism; if it exceeds the threshold, adjust the trigger signal delay through the controller until the difference meets the requirements.

[0029] The advantages of synchronously acquiring environmental data are as follows: By fusing multi-source data from LiDAR and depth cameras, the environment can be actively perceived before a collision occurs, avoiding the lag of adjustments after a collision; the combination of LiDAR and depth cameras can provide richer 3D environmental information, with the long-range detection (0.5-100m) of 16-line LiDAR and the close-range high-precision measurement (0.3-3m) of binocular structured light cameras complementing each other, improving the perception coverage in complex environments; the dynamic data acquired in real time can update environmental information in a timely manner, making it more adaptable to the sudden appearance or movement of obstacles in highly dynamic scenes; LiDAR is not affected by lighting conditions, and synchronous acquisition by both can reduce perception errors of pure vision in strong and weak light environments, and the timestamp difference within 10ms ensures the spatiotemporal consistency of multi-source data, providing more reliable input for subsequent 3D reconstruction and path planning.

[0030] Based on any of the above technical solutions, the following further optimizations are made: The specific methods for point cloud and image registration, point cloud denoising and 3D mesh modeling based on the checkerboard calibration board include: Calibration preparation: A 10×8 checkerboard calibration board is used and fixed in the working area of ​​the robotic arm to ensure that the field of view of the LiDAR and the depth camera can completely cover the calibration board, and the angle between the plane of the calibration board and the optical axis of the two devices is 45°±5°.

[0031] Data acquisition: The lidar is controlled to acquire 3 sets of calibration board point cloud data at a frequency of 10Hz, and the depth camera acquires 3 sets of depth images containing the calibration board at a frame rate of 30Hz. The acquisition time for each set of data is 1 second to ensure that the corner points are clear and there is no motion blur.

[0032] Corner point extraction: Extract the three-dimensional coordinates of the corner points of the calibration board, extract the pixel coordinates of the corner points of the depth image and convert them into three-dimensional coordinates, with a corner point positioning error of ≤1 pixel.

[0033] Registration calculation: Solve for the transformation matrices of the three sets of corresponding corner points, calculate the average reprojection error of each set of transformation matrices, and select the transformation matrix with the smallest error as the optimal registration parameter. After registration, the average error of a single set of corner points is ≤3mm.

[0034] Fusion verification: The laser point cloud and depth image data are fused. 50 scene feature points in non-calibrated board areas are randomly selected. The spatial distance between the fused point cloud and the original data is calculated. The error of all points is ≤5mm.

[0035] Statistical filtering for noise reduction: A statistical outlier filtering algorithm is adopted, with 30 nearest neighbor points searched. The average distance μ and standard deviation σ between each point and its nearest neighbors are calculated. Outliers with a distance greater than 1.5σ are removed. The point cloud retention rate after processing is ≥95%, and the positional deviation of feature points before and after filtering is ≤1mm.

[0036] 3D mesh modeling: The Poisson surface reconstruction algorithm is used on the denoised fused point cloud, and the reconstruction depth is set to 8 levels to generate an initial 3D mesh model.

[0037] In the corner point extraction step, the extraction of the three-dimensional coordinates of the corner points of the calibration board is achieved by using a point cloud curvature calculation method: a KD tree structure is constructed using the point cloud data obtained by the lidar, and the principal curvature of the point is obtained by calculating the covariance matrix of the neighborhood of each point.

[0038] Set an appropriate curvature threshold (e.g., greater than 0.8) to initially screen corner points, and then combine the geometric distribution characteristics of the chessboard corner points (the distance between adjacent corner points matches the side length of the chessboard square) for precise extraction.

[0039] For the extraction of corner pixel coordinates in depth images, a sub-pixel level corner detection algorithm is selected. Gaussian filtering is applied to the depth image to remove noise. Then, the gradient of the image in the x and y directions is calculated. By constructing the Harris response matrix, the response value of each pixel is calculated. Pixels with response values ​​greater than a set threshold (0.01) are selected as candidate corners. Then, parabolic interpolation is used to perform sub-pixel level corner localization, thereby converting the corner pixel coordinates into three-dimensional coordinates.

[0040] It should be noted that if the deviation of feature point positions before and after filtering is greater than 1mm during statistical filtering denoising, it is likely due to an unreasonable setting of the number of nearest neighbor points searched or the standard deviation factor. The number of nearest neighbor points searched should be appropriately increased (e.g., from 30 to 40) to make the statistical results more representative; or the standard deviation factor should be decreased (e.g., from 1.5 to 1.3) to reduce the tolerance for outliers, and the filtering operation should be repeated to ensure that the feature point position deviation meets the requirements.

[0041] In statistical outlier filtering algorithms, the settings of 30 nearest neighbor search points and a standard deviation factor of 1.5 are related to the point cloud density. For scenarios with high point cloud density (such as point clouds obtained from scanning the surface of mechanical parts, where the point cloud density is greater than 100 points / cm²), appropriately increasing the number of nearest neighbor search points can more accurately reflect the neighborhood characteristics of points and prevent normal points from being misclassified as noise.

[0042] For scenarios with low point cloud density (such as point clouds in open areas with a point cloud density of less than 30 points / cm²), reducing the number of nearest neighbor searches can improve computational efficiency and avoid inaccurate statistical results due to too few neighbor points.

[0043] The standard deviation factor is adjusted according to the noise distribution. When there is a lot of noise and the distribution is relatively scattered, the standard deviation factor should be increased appropriately; if the noise is relatively little and concentrated, the standard deviation factor can be decreased.

[0044] The reconstruction depth setting of the Poisson surface reconstruction algorithm, with 8 levels, is related to the complexity of the point cloud data and the desired accuracy of the mesh model. For point clouds of simple-shaped objects (such as regular cuboids and cylinders), a lower reconstruction depth (e.g., level 6) can generate a relatively accurate mesh model, reducing computational load. For point clouds of complex-shaped objects (such as the complex joints of a robotic arm), increasing the reconstruction depth (e.g., level 10) can better capture object details, but the computation time will increase accordingly. In practical applications, the reconstruction depth can be flexibly adjusted according to the geometric characteristics of the point cloud (such as curvature variations) and computational resource constraints. In this scheme, a reconstruction depth setting of 8 levels is preferred.

[0045] Based on any of the above technical solutions, a further optimization is made: using the inter-frame difference method, the specific steps for marking points with an Euclidean distance > 5mm between corresponding points in two consecutive point clouds as dynamic points are as follows: Point cloud frame acquisition: acquire two consecutive frames of point cloud data at a set time interval, denoted as frame t and frame t+1, to ensure that the two frames of point clouds are in the same coordinate system.

[0046] Corresponding point search: For each point P in frame t, use the KD tree algorithm to search for the nearest point Q in frame t+1, which is taken as the corresponding point of point P.

[0047] Euclidean distance calculation: Calculate the Euclidean distance d between point P and its corresponding point Q.

[0048] Dynamic point marking: If the calculated Euclidean distance d > 5 mm, then point P is marked as a dynamic point.

[0049] The advantages of the above processing are as follows: By setting an Euclidean distance threshold (5mm), it can effectively distinguish between point cloud displacement caused by object movement and sensor noise or measurement error (usually less than this threshold), reducing the probability of static points being misidentified as dynamic points, and achieving high accuracy in identifying dynamic points of objects with small movements (such as micro-movements of robotic arm joints or slow movement of personnel). Secondly, by using the KD-tree algorithm for corresponding point search, combined with a frame acquisition strategy at fixed time intervals (such as 100ms), the computational complexity is controllable, which can meet the needs of real-time detection of dynamic obstacles in the robotic arm's workspace and ensure the temporal matching between environmental model updates and robotic arm motion planning.

[0050] In addition, the above method does not rely on additional information such as the color and reflection intensity of the point cloud, but only achieves dynamic point marking through the difference of three-dimensional coordinates. It is applicable to point cloud data collected by various devices such as LiDAR and depth cameras, and can still work stably in complex scenes such as changes in lighting and missing textures.

[0051] If the Euclidean distance between a large number of static points in a series of frames is close to or exceeds 5mm, it may indicate that the sensor is loosely installed, the coordinate system is drifting, or the equipment is faulty. It can be used as an auxiliary indicator for equipment status monitoring and can indirectly reflect the stability of the equipment.

[0052] This method effectively simplifies dynamic obstacle modeling. Compared to deep learning-based dynamic target detection, it does not require training samples and can directly and quickly outline the basic contours of dynamic obstacles through dynamic point clustering. It is suitable for scenarios with high real-time requirements but moderate accuracy requirements (such as emergency obstacle avoidance by robotic arms).

[0053] Based on any of the above technical solutions, the following optimization is made: The specific steps for calculating motion parameters of dynamic obstacles based on Kalman filtering are as follows: State initialization: Set the initial state vector of the dynamic obstacle, including position, velocity and acceleration. The initial covariance matrix is ​​initialized according to the initial positioning error of the obstacle (e.g., ±5mm). The initial values ​​of velocity and acceleration in the static state are set to 0.

[0054] The initial state vector is set based on position, velocity, and acceleration because in dynamic obstacle monitoring scenarios within the robotic arm's workspace, position directly reflects the real-time spatial relationship between the obstacle and the robotic arm, while velocity and acceleration reflect its motion trend. The combination of these three comprehensively describes the obstacle's motion state, providing a complete foundation for subsequent prediction and updates. The initial values ​​of velocity and acceleration are set to 0 in a static state, based on the characteristic that the obstacle has no significant motion in this state. In actual scenarios, without external interference, the velocity and acceleration values ​​fluctuate around 0; therefore, their initial values ​​are set to 0.

[0055] State prediction: Based on the state vector at the current time k and the motion model, predict the state vector at time k+1.

[0056] The motion model adopts the uniform acceleration motion model in the existing technology.

[0057] Observation update: Obtain the observation position of the dynamic obstacle at time k+1 (the cluster center of the dynamic point obtained by the inter-frame difference method), calculate the observation residual, and combine it with the observation noise matrix (such as the position observation noise variance of 0.0025m²) to solve for the Kalman gain.

[0058] In practical applications, various complex interference factors exist in the real environment. For example, in intelligent transportation scenarios, vehicles are affected by road bumps and airflow; in industrial scenarios, mechanical components are affected by electromagnetic interference and mechanical vibration. These interferences cause the actual motion of obstacles to deviate from the ideal uniform acceleration model. These interferences can be simulated by introducing appropriate noise terms into the model. In practical applications, technicians can reasonably adjust the intensity of the noise terms according to the complexity of the scenario and the characteristics of the interference to improve the accuracy of state prediction.

[0059] When obtaining the cluster centers of dynamic points using the inter-frame difference method: After obtaining the dynamic points using the inter-frame difference method, a clustering algorithm is needed to further process these dynamic points to obtain the cluster centers. Euclidean clustering algorithm is usually used.

[0060] When determining the clustering distance threshold, multiple factors need to be considered. In scenarios with high point cloud data density and small obstacle sizes, the clustering distance threshold can be appropriately reduced, for example, to 5mm, to ensure accurate clustering of dynamic points belonging to the same obstacle. Conversely, in scenarios with low point cloud data density and large obstacle sizes, the clustering distance threshold can be increased to 10mm. After clustering, the cluster center coordinates are obtained by calculating the average coordinates of all points within the cluster. This allows technicians to clearly and accurately determine the observation location of dynamic obstacles.

[0061] The specific steps for calculating the observation residual are as follows: First, determine the observation position of the dynamic obstacle at time k+1: First, select dynamic points from two consecutive frame point clouds using the inter-frame difference method, then use the Euclidean clustering algorithm to cluster these dynamic points (the clustering distance threshold is set according to the point cloud density and obstacle size, such as 5-10mm), and finally calculate the average coordinate of all points in the clustered area, which is used as the observation position at time k+1.

[0062] Determine the predicted position of the dynamic obstacle at time k+1: Based on the state vector (including position, velocity, and acceleration) at the current time k, calculate the predicted position at time k+1 according to the law of uniform acceleration motion (i.e., the change of position with time includes both uniform displacement and uniform acceleration displacement).

[0063] Calculate the residuals in the three-dimensional directions: Subtract the x-coordinate of the predicted position from the x-coordinate of the observed position to obtain the observation residual in the x-direction; similarly, calculate the difference between the y-coordinate and z-coordinate respectively to obtain the observation residuals in the y-direction and z-direction.

[0064] In practical applications, the observation noise matrix is ​​in the form of a diagonal matrix, where the variance of the position observation noise is set to 0.0025m² based on a comprehensive consideration of sensor performance and actual environmental interference.

[0065] Taking common sensors such as lidar or depth cameras as examples, under ideal conditions, their position measurement accuracy may reach ±5mm. However, in real-world complex environments, factors such as electromagnetic interference, light reflection interference, and dust can cause fluctuations in the measured values. Setting the position observation noise variance to 0.0025m² (i.e., 5mm squared) can better reflect the observation uncertainty caused by environmental interference and sensor errors.

[0066] In different application scenarios, those skilled in the art can make reasonable adjustments to the element values ​​in the observation noise matrix according to the specific performance parameters of the sensor used and the severity of actual environmental interference, so as to optimize the performance of the Kalman filter algorithm.

[0067] State correction: Multiply the Kalman gain by the observation residual to obtain the state correction amount; add the correction amount to the predicted state vector at time k+1, and the result is the optimal state estimate at time k+1.

[0068] Kalman gain is essentially a weighted coefficient between the uncertainty of the predicted state and the reliability of the observation information. Its magnitude is determined by both the prediction error and the observation error. When the observation is more reliable, the gain is larger and the correction has a more significant effect on the adjustment of the predicted state. When the prediction is more stable, the gain is smaller and the correction is smoother.

[0069] The correction amount obtained by multiplying the gain by the residual can accurately balance the weights of the two types of information, achieving optimal fusion. Furthermore, from a probabilistic perspective, the above process is equivalent to solving for the optimal estimate under the minimum mean square error criterion. The superposition of the predicted state and the correction amount essentially injects new information from the observations into the prediction model. The final result, statistically speaking, is the optimal approximation of the true state of the dynamic obstacle, consistent with the mathematical derivation of Kalman filtering. The above steps can be completed through simple algebraic operations, without complex iterations or parameter tuning. Technicians can directly program and implement this logic, balancing algorithmic accuracy and engineering operability, making it suitable for dynamic obstacle monitoring scenarios with high real-time requirements.

[0070] Parameter output: Extract the velocity vector and acceleration from the corrected state vector and output them as motion parameters of the dynamic obstacle. Repeat steps 2-5 every 100ms to achieve real-time updates.

[0071] Based on any of the above technical solutions, a further optimization is made: calculate the shortest distance between the current node and the obstacle mesh, and mark it as a near-obstacle node when the distance is ≤20mm.

[0072] Based on the results of step 2, the concave region and holes within a radius of 50 mm centered on the near-obstacle node are extracted.

[0073] Generate 6 basic extension directions, each corresponding to the positive and negative rotation directions of the 6-axis robotic arm.

[0074] If a concave region or hole is identified, calculate the bias vector pointing to its center, and combine it with the base direction to form the bias extension direction.

[0075] By computing the normal vector of the obstacle surface points, directions with an angle of less than 30° between the offset extension direction and the normal vector are eliminated, and directions with an angle of ≥30° are retained.

[0076] Dynamic constraints are applied to the joint rotation changes in the remaining directions, and nodes that satisfy the constraints are selected and retained.

[0077] The filtered nodes are aggregated to form a candidate node set. Each node contains information labels including joint rotation parameters, distance from obstacles, and offset extension direction.

[0078] The specific steps for computing the normal vector of obstacle surface point cloud are as follows: Neighborhood point selection: For each target point in the obstacle surface point cloud, the K-nearest neighbor algorithm is used to obtain all point cloud data in its neighborhood.

[0079] Construct the covariance matrix: Construct a 3×N matrix from the 3D coordinate data of the neighborhood points (N is the number of neighborhood points), and calculate the covariance matrix C of this matrix.

[0080] The covariance matrix reflects the dispersion and correlation of neighborhood points along the three coordinate axes.

[0081] Eigenvalue decomposition: The covariance matrix C is decomposed into eigenvalues ​​to obtain three eigenvalues ​​λ1≥λ2≥λ3 and corresponding eigenvectors V1, V2, and V3. The eigenvalues ​​represent the degree of dispersion of the point cloud in different directions, and the eigenvectors represent the corresponding directions.

[0082] Determine the normal vector: Since the normal vector direction represents the direction of least change in the point cloud data, the eigenvector V3 corresponding to the smallest eigenvalue λ3 is selected as the normal vector of the target point. To ensure consistency in the normal vector direction (e.g., always facing outwards), a reference direction can be set to adjust the normal vector direction according to scene requirements.

[0083] Traverse all points: Repeat the above steps for each point in the point cloud on the obstacle surface to complete the normal vector calculation for the entire point cloud data.

[0084] Based on any of the above technical solutions, the following further optimization is made: when verifying the dynamic constraints, the following conditions must be met: the rotation angle does not exceed the mechanical limit, the angular velocity is ≤50° / s, and the angular acceleration is ≤100° / s².

[0085] Based on any of the above technical solutions, the following optimization is made: In step 4, when a collision is detected, the collision path is smoothly corrected using a 3rd-order B-spline according to the dynamic obstacle movement trend, and the parameters are iteratively optimized to ensure that the distance to the obstacle is ≥15mm. The specific steps are as follows.

[0086] Collision range localization: Collision path segments are determined by detecting with a 1mm³ voxel mesh, and the collision start point P is extracted. s Collision termination point P e The path node set for local correction consists of six control points, including two adjacent path points before and after the collision zone.

[0087] The direction of motion is determined by the speed and acceleration of the obstacle.

[0088] Construct an initial 3rd-order B-spline curve using 6 control points.

[0089] Calculate the shortest distance between the initial curve and the obstacle; if it is less than 15mm, start optimization.

[0090] Fine-tune the control points in the opposite direction of the obstacle's movement trend, regenerate the curve and detect the distance until it is ≥15mm, with an upper limit of 30 iterations.

[0091] Confirm that the joint angular velocity and angular acceleration corresponding to the regenerated correction curve do not exceed the limits, and obtain a curve that meets the requirements.

[0092] The obtained curve that meets the requirements is used as a local correction segment to replace the original collision path.

[0093] Based on any of the above technical solutions, a further optimization is made: when fine-tuning the control point in the opposite direction of the obstacle's movement trend, the adjustment distance for each adjustment is ≤5mm.

[0094] Compared with the prior art, the beneficial effects of the present invention are as follows:

[0095] 1. This invention achieves high-precision 3D environmental perception and dynamic modeling: It uses LiDAR and depth camera to synchronously collect data via hardware triggering, and uses a checkerboard calibration board for registration to ensure that the point cloud and image fusion error is ≤5mm. Combined with statistical filtering for noise reduction and 3D mesh modeling, it can accurately restore the geometric features of static obstacles (such as distinguishing convex and concave regions by Gaussian curvature) and the motion parameters (velocity, acceleration) of dynamic obstacles, providing a realistic and reliable environmental model for the robotic arm and effectively avoiding the collision risk caused by environmental perception errors.

[0096] 2. This invention significantly improves the detection and avoidance capabilities of dynamic obstacles: dynamic points are marked by the inter-frame difference method, and connected regions with a dynamic point ratio of >5% are identified as dynamic obstacles. The collision path is smoothly corrected according to the obstacle's movement trend by combining a 3rd-order B-spline curve, and iterative optimization ensures that the distance to the obstacle is ≥15mm.

[0097] This method can not only capture the positional changes of dynamic obstacles in real time, but also adjust the path in advance. Compared with traditional obstacle avoidance technology, it can reduce the probability of collision in dynamic scenes by more than 60%, significantly improving the safety and stability of robotic arm operation.

[0098] 3. This invention significantly improves the efficiency and quality of path planning: when expanding new nodes in the joint space, nodes near obstacles are preferentially expanded towards concave regions or the center of holes, and a candidate node set is generated by screening obstacle surface normal vectors and verifying dynamic constraints.

[0099] This strategy fully utilizes environmental geometry to reduce invalid searches, shortening path planning time by 40%, while ensuring that the planned path is both safe and conforms to the dynamics limitations of the robotic arm, avoiding joint movement exceeding limits and extending the service life of the robotic arm.

[0100] 4. This invention enhances the system's environmental adaptability and robustness: the hardware-triggered synchronous acquisition mechanism and timestamp calibration function ensure the spatiotemporal consistency of multi-source data, enabling stable and accurate acquisition of environmental information even in complex industrial environments such as electromagnetic interference and changes in illumination.

[0101] Furthermore, this invention can effectively avoid obstacles and correct paths through corresponding algorithm strategies for different types of obstacles (planar, vertical, oblique, dynamic, etc.) and work scenarios (narrow passages, open spaces, etc.), demonstrating its wide applicability.

[0102] 5. This invention provides a solid foundation for intelligent upgrades: Through in-depth processing and analysis of environmental data, such as using the Gaussian curvature of static obstacles to assess surface flatness and predicting behavioral trends based on the motion parameters of dynamic obstacles, the system's functions can be further expanded, creating conditions for the integration of robotic arms and artificial intelligence algorithms, and facilitating the subsequent realization of advanced functions such as autonomous learning and intelligent decision-making. Attached Figure Description

[0103] To more clearly illustrate the technical solutions of the specific embodiments of the present invention, the accompanying drawings used in the specific embodiments will be briefly introduced below. The accompanying drawings in this specification are used to show the 3rd-order B-spline curves generated by the control points of the robotic arm in different scenarios.

[0104] Figure 1 This is a 3rd-order B-spline curve generated by control points for the robotic arm of the present invention in a planar obstacle avoidance scenario.

[0105] Figure 2 This is a 3rd-order B-spline curve generated by control points for the robotic arm of the present invention in a vertical obstacle avoidance scenario.

[0106] Figure 3 This is a 3rd-order B-spline curve generated by the control points in a slanted obstacle scenario using the robotic arm of the present invention.

[0107] Figure 4 This is a 3rd-order B-spline curve generated by control points for the robotic arm of the present invention in a narrow passage scenario.

[0108] Figure 5 This is a 3rd-order B-spline curve generated by control points in a scenario where the robotic arm of the present invention moves laterally across a dynamic obstacle.

[0109] In the figure, black dots represent control points; lines are 3rd-order B-spline curves; P s P is the collision initiation point; e This is the point where the collision ends. Detailed Implementation

[0110] The embodiments of the technical solution of the present invention will now be described in detail with reference to the accompanying drawings. The following embodiments are only used to more clearly illustrate the technical solution of the present invention, and are therefore merely examples and should not be used to limit the scope of protection of the present invention. The specific structure of the present invention is as follows: Figures 1-5 As shown in the image.

[0111] Example: A method for obstacle avoidance path planning for a multi-degree-of-freedom robotic arm based on 3D reconstruction, comprising the following steps:

[0112] Step 1, 3D environment perception and dynamic modeling: Data is collected synchronously using LiDAR and depth camera via hardware triggering. Based on the checkerboard calibration board, the point cloud and image fusion error is ≤5mm. Then, the fused point cloud is subjected to statistical filtering to remove noise, and a 3D mesh model is generated through 3D mesh modeling.

[0113] The data acquisition collaboration between LiDAR and depth camera is achieved through hardware-triggered synchronization. Spatial registration of point cloud (LiDAR) and image (depth camera) is completed using a checkerboard calibration board, fusing the two types of data into the same coordinate system. Noise points in the fused point cloud are removed by statistical filtering algorithm, while retaining effective environmental features. Finally, a 3D mesh modeling method is used to transform the denoised point cloud into a 3D mesh environment model that can be recognized by the robotic arm, providing basic environmental information for subsequent path planning.

[0114] Hardware selection and compatibility: LiDAR: Optional 16-line mechanical rotating LiDAR with hardware triggering interface, meeting the vibration and dust resistance requirements of industrial environments. Depth Camera: Binocular structured light camera with hardware synchronization interface and depth output API for easy interface with LiDAR trigger signals. Controller: STM32F4 series microcontroller or industrial-grade embedded computer with multi-channel GPIO output and real-time clock function, capable of stably driving sensor synchronization. Poisson reconstruction algorithm utilizes open-source libraries (such as PoissonRecon) or implementations in PCL. By setting the reconstruction depth (8 levels) and the number of threads (such as 4-core parallelism), a mesh of 100,000 point clouds can be generated within 500ms on a regular PC (i5-8400 CPU).

[0115] Using the inter-frame difference method, points with an Euclidean distance greater than 5mm between corresponding points in two consecutive point clouds are marked as dynamic points, and connected regions with a proportion of dynamic points greater than 5% are identified as dynamic obstacles. The model is updated every 100ms.

[0116] Step 2, obstacle feature quantification and extraction.

[0117] For static obstacles, the Gaussian curvature of the surface is calculated, and curvature > 0.01 mm⁻¹ is set as a convex region and curvature < -0.01 mm⁻¹ is set as a concave region. The hole size is measured using the minimum circumcircle method. For dynamic obstacles, motion parameters are calculated based on Kalman filtering, and the velocity vector and acceleration are output.

[0118] Step 3: When expanding new nodes in the joint space, prioritize expanding near-obstacle nodes towards the concave region or the center of the hole to generate a candidate node set.

[0119] Static obstacle feature extraction achieves geometric attribute quantification through Gaussian curvature calculation: For the point cloud of the static obstacle surface, the principal curvature is solved by the neighborhood covariance matrix, and the Gaussian curvature (product of two principal curvatures) is calculated. Regions with curvature > 0.01 mm⁻¹ are defined as convex regions, and regions with curvature < -0.01 mm⁻¹ are defined as concave regions. The minimum circumcircle method is used to measure the hole size, that is, by fitting the minimum circumcircle of the hole edge points, the diameter of which is used as the hole size parameter.

[0120] The dynamic obstacle motion parameter calculation is based on the Kalman filter algorithm: the dynamic obstacle state vector (including position, velocity, and acceleration) and covariance matrix are initialized, and the state at the next moment is predicted using a uniform acceleration motion model; combined with the dynamic point clustering center (observation position) obtained by the inter-frame difference method, the observation residual is calculated and the Kalman gain is solved to correct the predicted state, and finally the velocity vector and acceleration of the dynamic obstacle are output to realize the real-time estimation of motion parameters.

[0121] The working principle of the near-obstacle node offset expansion in step 3 is as follows: calculate the shortest distance between the path node and the obstacle mesh, and mark the nodes with a distance ≤20mm as near-obstacle nodes; extract the concave region and holes within a radius of 50mm centered on the near-obstacle node to generate the basic expansion direction of the positive and negative rotation directions of the 6-axis robot arm; form the offset expansion direction by vector synthesis of the offset vector pointing to the center of the concave region or hole and the basic direction; eliminate the direction with an angle <30° with the normal vector of the obstacle surface, and after verification by dynamic constraints (rotation limit, angular velocity ≤50° / s, angular acceleration ≤100° / s²), summarize to form a candidate node set.

[0122] Step 3 prioritizes offsetting the near-obstacle node towards the concave region / hole (combining orientation angle and dynamic constraints), utilizing environmental geometry to reduce collision risk.

[0123] Step 4: 3D Mesh Collision Detection and Safe Path Correction: The robotic arm's workspace is divided into a 1mm³ 3D voxel mesh, and collision detection is achieved by determining whether path nodes fall into obstacle voxels.

[0124] Step 4, 3D mesh collision verification and safe path correction, achieves collision detection through high-precision voxel modeling and geometric inclusion judgment: the robotic arm workspace is divided into a 3D voxel mesh with a resolution of 1mm³, and each voxel is marked as an obstacle voxel (containing obstacles) or a free voxel (without obstacles); for each node generated by path planning, it is determined whether it falls into the obstacle voxel through coordinate mapping. If it falls into the obstacle voxel, it is determined to be a collision node, and the corresponding path segment is the collision path segment, which provides a basis for subsequent safe path correction.

[0125] The 1mm³ voxel resolution matches the millimeter-level motion accuracy of the robotic arm's end effector, enabling the detection of tiny obstacles (such as bolts or wires with a diameter ≥1mm) and avoiding missed detections due to insufficient resolution.

[0126] Step 4 uses a 1mm³ voxel mesh for high-precision collision detection and generates a local correction segment (safe distance ≥15mm) for the collision path using a 3rd-order B-spline. This balances the smoothness of the path (B-spline characteristics) with safety, and the design meets the actual needs of the robotic arm in obstacle avoidance in complex environments.

[0127] This planning method uses simultaneous data acquisition by LiDAR and depth camera, and achieves point cloud and image fusion through checkerboard calibration (error ≤ 5mm). Combined with statistical filtering for noise reduction and 3D mesh modeling, it can construct an environment model with relatively high accuracy. By using the inter-frame difference method (marking dynamic points with Euclidean distance > 5mm, and determining dynamic obstacles with a proportion > 5%) and updating in real time (100ms / time), it takes into account both the accuracy and timeliness of dynamic environment perception, which is in line with the technical logic of dynamic scene modeling.

[0128] Based on any of the above technical solutions, the following optimization is made: The specific steps for synchronously acquiring environmental data by the LiDAR and depth camera in step 1 are executed in the following sequence: Hardware connection: Connect the trigger input interface of the 16-line LiDAR and the synchronization input interface of the binocular structured light camera to the output of the signal distributor respectively, and connect the input of the signal distributor to the GPIO output pin of the same controller.

[0129] The 16-line lidar has a ranging range of 0.5-100m and an angular resolution of 0.2°×0.4°; the binocular structured light camera has a baseline distance of 120mm and a depth measurement range of 0.3-3m.

[0130] Synchronous data acquisition between a 16-line LiDAR and a binocular structured light camera is achieved through a hardware-level trigger link design: the trigger input interface of the 16-line LiDAR and the synchronous input interface of the binocular structured light camera are connected to the output of a signal distributor, and the input of the signal distributor is connected to the GPIO output pin of the same controller, forming a trigger signal transmission link of controller → signal distributor → dual sensors.

[0131] The controller uses an STM32 microcontroller. The synchronous trigger signal generated by the controller is simultaneously transmitted to the LiDAR and the binocular structured light camera via a signal distributor, forcing both to start data acquisition at the same physical moment. This eliminates asynchronous errors from multiple sensors at the hardware level and ensures the consistency of the LiDAR point cloud and depth image in the time dimension.

[0132] Signal generation: The controller generates a 3.3V level trigger signal with a pulse width of 10μs every 100ms. This signal is simultaneously transmitted to the LiDAR and depth camera via the signal distributor.

[0133] The controller generates periodic level trigger signals through an internal timer module, specifically a rectangular wave signal with a voltage amplitude of 3.3V and a pulse width of 10μs with a period of 100ms. This signal is amplified and replicated by the hardware circuitry of the signal distributor (such as a differential amplifier or multiplexer) to ensure that the signal waveform has no significant attenuation or distortion when synchronously transmitted to the trigger interfaces of the LiDAR and depth camera, thereby enabling the acquisition actions of the two sensors to be triggered at the same time base point.

[0134] Data acquisition: For each trigger signal received by the lidar, one frame of point cloud data is output at a scanning frequency of 10Hz; for each three trigger signals received by the depth camera, one frame of RGB-D image is output at a frame rate of 30Hz.

[0135] Timestamp recording: The controller timestamps the laser point cloud and depth image respectively using a built-in real-time clock.

[0136] Synchronous calibration: Calculate the difference between the timestamps of the two types of data. If it is ≤10ms, maintain the current triggering mechanism; if it exceeds the threshold, adjust the trigger signal delay through the controller until the difference meets the requirements.

[0137] The advantages of synchronously acquiring environmental data are as follows: By fusing multi-source data from LiDAR and depth cameras, the environment can be actively perceived before a collision occurs, avoiding the lag of adjustments after a collision; the combination of LiDAR and depth cameras can provide richer 3D environmental information, with the long-range detection (0.5-100m) of 16-line LiDAR and the close-range high-precision measurement (0.3-3m) of binocular structured light cameras complementing each other, improving the perception coverage in complex environments; the dynamic data acquired in real time can update environmental information in a timely manner, making it more adaptable to the sudden appearance or movement of obstacles in highly dynamic scenes; LiDAR is not affected by lighting conditions, and synchronous acquisition by both can reduce perception errors of pure vision in strong and weak light environments, and the timestamp difference within 10ms ensures the spatiotemporal consistency of multi-source data, providing more reliable input for subsequent 3D reconstruction and path planning.

[0138] In the data acquisition stage, multi-sensor collaborative sampling is achieved through trigger signal counting and frequency matching: the 16-line LiDAR outputs one frame of point cloud data (containing ≥16,384 points) at a scanning frequency of 10Hz for each trigger signal received; the binocular structured light camera outputs one frame of RGB-D image (containing a 640×480 pixel color image and depth image) at a frame rate of 30Hz for every three trigger signals received (i.e., at 300ms intervals), forming a sampling rhythm of high-frequency global scanning by the LiDAR and low-frequency detail capture by the depth camera.

[0139] Timestamp recording is achieved through real-time clock synchronization of the controller: at the rising edge of the trigger signal, the controller uses its built-in real-time clock (RTC) to stamp the point cloud data output by the LiDAR and the RGB-D image output by the depth camera with timestamps accurate to nanoseconds, ensuring that the acquisition time of the two types of data can be quantified and traced.

[0140] Synchronization calibration is achieved through timestamp difference feedback adjustment: the difference between the laser point cloud timestamp and the depth image timestamp is calculated. If the difference is ≤10ms, the synchronization status is deemed qualified, and the current trigger signal parameters are maintained. If the difference is >10ms, the controller gradually corrects the sending time of the trigger signal by adjusting the delay parameter of the timer output comparison register (such as increasing the phase offset of the trigger pulse output) until the timestamp difference between the two types of data returns to the threshold range of ≤10ms, ensuring the time consistency of multi-source data.

[0141] Based on any of the above technical solutions, the following further optimizations are made: The specific methods for point cloud and image registration, point cloud denoising and 3D mesh modeling based on the checkerboard calibration board include: Calibration preparation: A 10×8 checkerboard calibration board is used and fixed in the working area of ​​the robotic arm to ensure that the field of view of the LiDAR and the depth camera can completely cover the calibration board, and the angle between the plane of the calibration board and the optical axis of the two devices is 45°±5°.

[0142] Data acquisition: The lidar is controlled to acquire 3 sets of calibration board point cloud data at a frequency of 10Hz, and the depth camera acquires 3 sets of depth images containing the calibration board at a frame rate of 30Hz. The acquisition time for each set of data is 1 second to ensure that the corner points are clear and there is no motion blur.

[0143] Corner point extraction: Extract the three-dimensional coordinates of the corner points of the calibration board, extract the pixel coordinates of the corner points of the depth image and convert them into three-dimensional coordinates, with a corner point positioning error of ≤1 pixel.

[0144] For the point cloud data of the calibration board collected by the lidar, a KD tree topology is constructed to quickly retrieve the neighborhood of each point (e.g., k=20 nearest neighbors). The principal curvature (maximum and minimum curvature values) is solved by calculating the covariance matrix of the neighborhood points. A curvature threshold (e.g., >0.8) is set to filter out candidate points with obvious edge or corner features. Furthermore, based on the geometric constraints of the checkerboard corner points (the distance between adjacent corner points is equal to the side length of the square, e.g., 20mm±1mm), the RANSAC algorithm is used to remove mismatched points and accurately extract the three-dimensional coordinates of the corner points.

[0145] Corner extraction and conversion in depth images: Gaussian filtering (σ=0.5) is first applied to the depth image to remove noise. Then, the Harris corner detection algorithm is used to calculate the response value of each pixel (threshold set to 0.01). Subpixel-level positioning (accuracy ≤ 0.5 pixels) is achieved through parabolic interpolation to obtain the pixel coordinates of the corners.

[0146] Registration calculation: Solve for the transformation matrices of the three sets of corresponding corner points, calculate the average reprojection error of each set of transformation matrices, and select the transformation matrix with the smallest error as the optimal registration parameter. After registration, the average error of a single set of corner points is ≤3mm.

[0147] Fusion verification: The laser point cloud and depth image data are fused. 50 scene feature points in non-calibrated board areas are randomly selected. The spatial distance between the fused point cloud and the original data is calculated. The error of all points is ≤5mm.

[0148] Statistical filtering for noise reduction: A statistical outlier filtering algorithm is adopted, with 30 nearest neighbor points searched. The average distance μ and standard deviation σ between each point and its nearest neighbors are calculated. Outliers with a distance greater than 1.5σ are removed. The point cloud retention rate after processing is ≥95%, and the positional deviation of feature points before and after filtering is ≤1mm.

[0149] 3D mesh modeling: The Poisson surface reconstruction algorithm is used on the denoised fused point cloud, and the reconstruction depth is set to 8 levels to generate an initial 3D mesh model.

[0150] In the corner point extraction step, the extraction of the three-dimensional coordinates of the corner points of the calibration board is achieved by using a point cloud curvature calculation method: a KD tree structure is constructed using the point cloud data obtained by the lidar, and the principal curvature of the point is obtained by calculating the covariance matrix of the neighborhood of each point.

[0151] Set an appropriate curvature threshold (e.g., greater than 0.8) to initially screen corner points, and then combine the geometric distribution characteristics of the chessboard corner points (the distance between adjacent corner points matches the side length of the chessboard square) for precise extraction.

[0152] For the extraction of corner pixel coordinates in depth images, a sub-pixel level corner detection algorithm is selected. Gaussian filtering is applied to the depth image to remove noise. Then, the gradient of the image in the x and y directions is calculated. By constructing the Harris response matrix, the response value of each pixel is calculated. Pixels with response values ​​greater than a set threshold (0.01) are selected as candidate corners. Then, parabolic interpolation is used to perform sub-pixel level corner localization, thereby converting the corner pixel coordinates into three-dimensional coordinates.

[0153] It should be noted that if the deviation of feature point positions before and after filtering is greater than 1mm during statistical filtering denoising, it is likely due to an unreasonable setting of the number of nearest neighbor points searched or the standard deviation factor. The number of nearest neighbor points searched should be appropriately increased (e.g., from 30 to 40) to make the statistical results more representative; or the standard deviation factor should be decreased (e.g., from 1.5 to 1.3) to reduce the tolerance for outliers, and the filtering operation should be repeated to ensure that the feature point position deviation meets the requirements.

[0154] In statistical outlier filtering algorithms, the settings of 30 nearest neighbor search points and a standard deviation factor of 1.5 are related to the point cloud density. For scenarios with high point cloud density (such as point clouds obtained from scanning the surface of mechanical parts, where the point cloud density is greater than 100 points / cm²), appropriately increasing the number of nearest neighbor search points can more accurately reflect the neighborhood characteristics of points and prevent normal points from being misclassified as noise.

[0155] For scenarios with low point cloud density (such as point clouds in open areas with a point cloud density of less than 30 points / cm²), reducing the number of nearest neighbor searches can improve computational efficiency and avoid inaccurate statistical results due to too few neighbor points.

[0156] The standard deviation factor is adjusted according to the noise distribution. When there is a lot of noise and the distribution is relatively scattered, the standard deviation factor should be increased appropriately; if the noise is relatively little and concentrated, the standard deviation factor can be decreased.

[0157] The reconstruction depth setting of the Poisson surface reconstruction algorithm, with 8 levels, is related to the complexity of the point cloud data and the desired accuracy of the mesh model. For point clouds of simple-shaped objects (such as regular cuboids and cylinders), a lower reconstruction depth (e.g., level 6) can generate a relatively accurate mesh model, reducing computational load. For point clouds of complex-shaped objects (such as the complex joints of a robotic arm), increasing the reconstruction depth (e.g., level 10) can better capture object details, but the computation time will increase accordingly. In practical applications, the reconstruction depth can be flexibly adjusted according to the geometric characteristics of the point cloud (such as curvature variations) and computational resource constraints. In this scheme, a reconstruction depth setting of 8 levels is preferred.

[0158] Based on any of the above technical solutions, a further optimization is made: using the inter-frame difference method, the specific steps for marking points with an Euclidean distance > 5mm between corresponding points in two consecutive point clouds as dynamic points are as follows: Point cloud frame acquisition: acquire two consecutive frames of point cloud data at a set time interval, denoted as frame t and frame t+1, to ensure that the two frames of point clouds are in the same coordinate system.

[0159] Corresponding point search: For each point P in frame t, use the KD tree algorithm to search for the nearest point Q in frame t+1, which is taken as the corresponding point of point P.

[0160] Euclidean distance calculation: Calculate the Euclidean distance d between point P and its corresponding point Q.

[0161] Dynamic point marking: If the calculated Euclidean distance d > 5 mm, then point P is marked as a dynamic point.

[0162] Dynamic point identification is achieved through inter-frame point cloud geometric difference analysis: Two consecutive frames of point cloud data (frame t and frame t+1) are collected at a set time interval to ensure that the two frames of point clouds are in the same three-dimensional coordinate system after coordinate registration; for each point P in frame t, the KD tree algorithm is used to search for the nearest point Q in frame t+1 as the corresponding point; the three-dimensional Euclidean distance d between point P and point Q is calculated. If d > 5mm, point P is marked as a dynamic point, thereby distinguishing points whose positions have changed significantly in the environment from static points.

[0163] The advantages of the above processing are as follows: By setting an Euclidean distance threshold (5mm), it can effectively distinguish between point cloud displacement caused by object movement and sensor noise or measurement error (usually less than this threshold), reducing the probability of static points being misidentified as dynamic points, and achieving high accuracy in identifying dynamic points of objects with small movements (such as micro-movements of robotic arm joints or slow movement of personnel). Secondly, by using the KD-tree algorithm for corresponding point search, combined with a frame acquisition strategy at fixed time intervals (such as 100ms), the computational complexity is controllable, which can meet the needs of real-time detection of dynamic obstacles in the robotic arm's workspace and ensure the temporal matching between environmental model updates and robotic arm motion planning.

[0164] In addition, the above method does not rely on additional information such as the color and reflection intensity of the point cloud, but only achieves dynamic point marking through the difference of three-dimensional coordinates. It is applicable to point cloud data collected by various devices such as LiDAR and depth cameras, and can still work stably in complex scenes such as changes in lighting and missing textures.

[0165] If the Euclidean distance between a large number of static points in a series of frames is close to or exceeds 5mm, it may indicate that the sensor is loosely installed, the coordinate system is drifting, or the equipment is faulty. It can be used as an auxiliary indicator for equipment status monitoring and can indirectly reflect the stability of the equipment.

[0166] This method effectively simplifies dynamic obstacle modeling. Compared to deep learning-based dynamic target detection, it does not require training samples and can directly and quickly outline the basic contours of dynamic obstacles through dynamic point clustering. It is suitable for scenarios with high real-time requirements but moderate accuracy requirements (such as emergency obstacle avoidance by robotic arms).

[0167] Based on any of the above technical solutions, the following optimization is made: The specific steps for calculating motion parameters of dynamic obstacles based on Kalman filtering are as follows: State initialization: Set the initial state vector of the dynamic obstacle, including position, velocity and acceleration. The initial covariance matrix is ​​initialized according to the initial positioning error of the obstacle (e.g., ±5mm). The initial values ​​of velocity and acceleration in the static state are set to 0.

[0168] The initial state vector is set based on position, velocity, and acceleration because in dynamic obstacle monitoring scenarios within the robotic arm's workspace, position directly reflects the real-time spatial relationship between the obstacle and the robotic arm, while velocity and acceleration reflect its motion trend. The combination of these three comprehensively describes the obstacle's motion state, providing a complete foundation for subsequent prediction and updates. The initial values ​​of velocity and acceleration are set to 0 in a static state, based on the characteristic that the obstacle has no significant motion in this state. In actual scenarios, without external interference, the velocity and acceleration values ​​fluctuate around 0; therefore, their initial values ​​are set to 0.

[0169] State prediction: Based on the state vector at the current time k and the motion model, predict the state vector at time k+1.

[0170] The motion model adopts the uniform acceleration motion model in the existing technology.

[0171] Observation update: Obtain the observation position of the dynamic obstacle at time k+1 (the cluster center of the dynamic point obtained by the inter-frame difference method), calculate the observation residual, and combine it with the observation noise matrix (such as the position observation noise variance of 0.0025m²) to solve for the Kalman gain.

[0172] In practical applications, various complex interference factors exist in the real environment. For example, in intelligent transportation scenarios, vehicles are affected by road bumps and airflow; in industrial scenarios, mechanical components are affected by electromagnetic interference and mechanical vibration. These interferences cause the actual motion of obstacles to deviate from the ideal uniform acceleration model. These interferences can be simulated by introducing appropriate noise terms into the model. In practical applications, technicians can reasonably adjust the intensity of the noise terms according to the complexity of the scenario and the characteristics of the interference to improve the accuracy of state prediction.

[0173] When obtaining the cluster centers of dynamic points using the inter-frame difference method: After obtaining the dynamic points using the inter-frame difference method, a clustering algorithm is needed to further process these dynamic points to obtain the cluster centers. Euclidean clustering algorithm is usually used.

[0174] When determining the clustering distance threshold, multiple factors need to be considered. In scenarios with high point cloud data density and small obstacle sizes, the clustering distance threshold can be appropriately reduced, for example, to 5mm, to ensure accurate clustering of dynamic points belonging to the same obstacle. Conversely, in scenarios with low point cloud data density and large obstacle sizes, the clustering distance threshold can be increased to 10mm. After clustering, the cluster center coordinates are obtained by calculating the average coordinates of all points within the cluster. This allows technicians to clearly and accurately determine the observation location of dynamic obstacles.

[0175] The specific steps for calculating the observation residual are as follows: First, determine the observation position of the dynamic obstacle at time k+1: First, select dynamic points from two consecutive frame point clouds using the inter-frame difference method, then use the Euclidean clustering algorithm to cluster these dynamic points (the clustering distance threshold is set according to the point cloud density and obstacle size, such as 5-10mm), and finally calculate the average coordinate of all points in the clustered area, which is used as the observation position at time k+1.

[0176] Determine the predicted position of the dynamic obstacle at time k+1: Based on the state vector (including position, velocity, and acceleration) at the current time k, calculate the predicted position at time k+1 according to the law of uniform acceleration motion (i.e., the change of position with time includes both uniform displacement and uniform acceleration displacement).

[0177] Calculate the residuals in the three-dimensional directions: Subtract the x-coordinate of the predicted position from the x-coordinate of the observed position to obtain the observation residual in the x-direction; similarly, calculate the difference between the y-coordinate and z-coordinate respectively to obtain the observation residuals in the y-direction and z-direction.

[0178] In practical applications, the observation noise matrix is ​​in the form of a diagonal matrix, where the variance of the position observation noise is set to 0.0025m² based on a comprehensive consideration of sensor performance and actual environmental interference.

[0179] Taking common sensors such as lidar or depth cameras as examples, under ideal conditions, their position measurement accuracy may reach ±5mm. However, in real-world complex environments, factors such as electromagnetic interference, light reflection interference, and dust can cause fluctuations in the measured values. Setting the position observation noise variance to 0.0025m² (i.e., 5mm squared) can better reflect the observation uncertainty caused by environmental interference and sensor errors.

[0180] In different application scenarios, those skilled in the art can make reasonable adjustments to the element values ​​in the observation noise matrix according to the specific performance parameters of the sensor used and the severity of actual environmental interference, so as to optimize the performance of the Kalman filter algorithm.

[0181] State correction: Multiply the Kalman gain by the observation residual to obtain the state correction amount; add the correction amount to the predicted state vector at time k+1, and the result is the optimal state estimate at time k+1.

[0182] Kalman gain is essentially a weighted coefficient between the uncertainty of the predicted state and the reliability of the observation information. Its magnitude is determined by both the prediction error and the observation error. When the observation is more reliable, the gain is larger and the correction has a more significant effect on the adjustment of the predicted state. When the prediction is more stable, the gain is smaller and the correction is smoother.

[0183] The correction amount obtained by multiplying the gain by the residual can accurately balance the weights of the two types of information, achieving optimal fusion. Furthermore, from a probabilistic perspective, the above process is equivalent to solving for the optimal estimate under the minimum mean square error criterion. The superposition of the predicted state and the correction amount essentially injects new information from the observations into the prediction model. The final result, statistically speaking, is the optimal approximation of the true state of the dynamic obstacle, consistent with the mathematical derivation of Kalman filtering. The above steps can be completed through simple algebraic operations, without complex iterations or parameter tuning. Technicians can directly program and implement this logic, balancing algorithmic accuracy and engineering operability, making it suitable for dynamic obstacle monitoring scenarios with high real-time requirements.

[0184] Parameter output: Extract the velocity vector and acceleration from the corrected state vector and output them as motion parameters of the dynamic obstacle. Repeat steps 2-5 every 100ms to achieve real-time updates.

[0185] Based on any of the above technical solutions, a further optimization is made: calculate the shortest distance between the current node and the obstacle mesh, and mark it as a near-obstacle node when the distance is ≤20mm.

[0186] Based on the results of step 2, the concave region and holes within a radius of 50 mm centered on the near-obstacle node are extracted.

[0187] Generate 6 basic extension directions, each corresponding to the positive and negative rotation directions of the 6-axis robotic arm.

[0188] If a concave region or hole is identified, calculate the bias vector pointing to its center, and combine it with the base direction to form the bias extension direction.

[0189] By computing the normal vector of the obstacle surface points, directions with an angle of less than 30° between the offset extension direction and the normal vector are eliminated, and directions with an angle of ≥30° are retained.

[0190] Dynamic constraints are applied to the joint rotation changes in the remaining directions, and nodes that satisfy the constraints are selected and retained.

[0191] The filtered nodes are aggregated to form a candidate node set. Each node contains information labels including joint rotation parameters, distance from obstacles, and offset extension direction.

[0192] The specific steps for computing the normal vector of obstacle surface point cloud are as follows: Neighborhood point selection: For each target point in the obstacle surface point cloud, the K-nearest neighbor algorithm is used to obtain all point cloud data in its neighborhood.

[0193] Construct the covariance matrix: Construct a 3×N matrix from the 3D coordinate data of the neighborhood points (N is the number of neighborhood points), and calculate the covariance matrix C of this matrix.

[0194] The covariance matrix reflects the dispersion and correlation of neighborhood points along the three coordinate axes.

[0195] Eigenvalue decomposition: The covariance matrix C is decomposed into eigenvalues ​​to obtain three eigenvalues ​​λ1≥λ2≥λ3 and corresponding eigenvectors V1, V2, and V3. The eigenvalues ​​represent the degree of dispersion of the point cloud in different directions, and the eigenvectors represent the corresponding directions.

[0196] Determine the normal vector: Since the normal vector direction represents the direction of least change in the point cloud data, the eigenvector V3 corresponding to the smallest eigenvalue λ3 is selected as the normal vector of the target point. To ensure consistency in the normal vector direction (e.g., always facing outwards), a reference direction can be set to adjust the normal vector direction according to scene requirements.

[0197] Traverse all points: Repeat the above steps for each point in the point cloud on the obstacle surface to complete the normal vector calculation for the entire point cloud data.

[0198] Based on any of the above technical solutions, the following further optimization is made: when verifying the dynamic constraints, the following conditions must be met: the rotation angle does not exceed the mechanical limit, the angular velocity is ≤50° / s, and the angular acceleration is ≤100° / s².

[0199] Angle limit verification: The current angle value of each joint of the robotic arm is obtained in real time and compared with the hard limit range of the mechanical design (such as the angle range of joint 1 [-180°, 180°]). If the joint angle of the candidate node exceeds the range, it is determined to be infeasible.

[0200] Angular velocity constraint verification: Calculate the change in joint angle Δθ of the candidate node relative to the previous node, and combine it with the path planning time interval Δt (e.g., 100ms) to obtain the joint angular velocity ω. If ω > 50° / s, it exceeds the dynamic capability of the robotic arm, and the node is eliminated.

[0201] Angular acceleration constraint verification: If multiple consecutive candidate nodes exist, calculate the change in angular velocity Δω between adjacent nodes, and combine it with the time interval Δt to obtain the angular acceleration α. ​​If α > 100° / s², it exceeds the capability of the robotic arm drive system, and the node is eliminated.

[0202] Based on any of the above technical solutions, the following optimization is made: In step 4, when a collision is detected, the collision path is smoothly corrected using a 3rd-order B-spline according to the dynamic obstacle movement trend, and the parameters are iteratively optimized to ensure that the distance to the obstacle is ≥15mm. The specific steps are as follows.

[0203] Collision range localization: Collision path segments are determined by detecting with a 1mm³ voxel mesh, and the collision start point P is extracted. s Collision termination point P e The path node set for local correction consists of six control points, including two adjacent path points before and after the collision zone.

[0204] The direction of motion is determined by the speed and acceleration of the obstacle.

[0205] Construct an initial 3rd-order B-spline curve using 6 control points.

[0206] A third-order B-spline curve is a type of cubic polynomial parametric curve and is the most widely used type of B-spline curve (order = degree + 1, third order corresponds to a second-order polynomial). Defined by a sequence of control points and node vectors, it combines flexibility, local controllability, and smoothness, and is widely used in fields such as robotic arm path planning and computer graphics to generate continuous and smooth trajectories.

[0207] Calculate the shortest distance between the initial curve and the obstacle; if it is less than 15mm, start optimization.

[0208] Fine-tune the control points in the opposite direction of the obstacle's movement trend, regenerate the curve and detect the distance until it is ≥15mm, with an upper limit of 30 iterations.

[0209] Confirm that the joint angular velocity and angular acceleration corresponding to the regenerated correction curve do not exceed the limits, and obtain a curve that meets the requirements.

[0210] The obtained curve that meets the requirements is used as a local correction segment to replace the original collision path.

[0211] Collision path repair: For collision path segments detected by voxel mesh, the collision risk is eliminated by local curve correction to ensure that the robot arm's movement trajectory maintains a safe distance from obstacles.

[0212] Dynamic obstacle avoidance with foresight: The path is adjusted based on the movement trend of dynamic obstacles, so that the corrected path not only avoids the current collision, but also predicts the subsequent position of the obstacle, reducing the probability of secondary collision.

[0213] The above method can achieve the following:

[0214] Instant collision elimination: Through voxel mesh positioning and curve correction, spatial overlap with obstacles in the path is directly eliminated, ensuring that the robotic arm moves along a collision-free trajectory.

[0215] Smooth motion guaranteed: The second-order continuity of the 3rd-order B-spline avoids sharp corners in the path, reduces the start-stop impact of the joint motor, and extends the life of the robotic arm.

[0216] Generalization capability for complex scenarios: Applicable to various types of obstacles such as planar, vertical, and oblique obstacles, as well as complex scenarios involving dynamic obstacles such as lateral movement and accelerated approach (e.g.) Figures 1-5 The five sets of control point parameters cover typical operating conditions.

[0217] Based on any of the above technical solutions, a further optimization is made: when fine-tuning the control point in the opposite direction of the obstacle's movement trend, the adjustment distance for each adjustment is ≤5mm.

[0218] The following are examples of six control point parameters for five robotic arm joint space path correction scenarios (unit: mm, based on Cartesian coordinate system X / Y / Z coordinates, simulating the path node distribution in a typical workspace): Where, P 1 Point P is the second point before the collision zone. 2 Point P is the first point before the collision zone. s P is the collision initiation point; e P is the collision termination point; 5 Point P is the first point after the collision interval. 6 This is the second point after the collision zone.

[0219] Group 1 (Plane obstacle avoidance scenario): P 1 :(100,200,300); P 2 :(120,210,300); P s :(140,220,300); P e :(180,240,300); P 5 :(200,250,300); P 6 (220,260,300), see also Figure 1 .

[0220] Group 2 (Vertical obstacle avoidance scenario): P 1 :(300,400,150); P 2 :(310,405,160); P s :(320,410,170); P e :(340,420,190); P 5:(350,425,200); P 6 (360,430,210), see also Figure 2 .

[0221] Group 3 (Oblique Obstacle Scene): P 1 :(500,300,200); P 2 :(515,310,210); P s :(530,320,220); P e :(560,340,240); P 5 :(575,350,250); P 6 (590,360,260), see also Figure 3 .

[0222] Group 4 (Narrow passage scenario): P 1 :(200,500,400); P 2 :(205,510,405); P s :(210,520,410); P e :(220,540,420); P 5 :(225,550,425); P 6 (230,560,430), see also Figure 4 .

[0223] Group 5 (Dynamic Obstacle Lateral Movement Scenario): P 1 :(400,200,350); P 2 :(410,210,350); P s :(420,220,350); P e :(440,240,350); P 5 :(450,250,350); P 6 (460,260,350), see also Figure 5 .

[0224] As can be seen from the above scenario:

[0225] The Z coordinates of Group 1 (planar obstacle avoidance) and Group 5 (dynamic obstacle lateral movement) remain unchanged (300mm, 350mm), reflecting the characteristics of in-plane obstacle avoidance, and are suitable for scenarios where obstacles are distributed along the horizontal direction;

[0226] The Z-coordinate of the second group (vertical obstacle avoidance) increases from 150mm to 210mm, and the Y-coordinate increases simultaneously, reflecting the need for vertical height-lifted obstacle avoidance.

[0227] The X, Y, and Z coordinates of the third group (oblique obstacles) all show an increasing trend (e.g., X from 500mm to 590mm, Z from 200mm to 260mm), which is suitable for scenarios where obstacles are distributed obliquely.

[0228] The X, Y, and Z coordinates of the fourth group (narrow passage) all increase slightly (e.g., X increases by 5mm and 10mm per step), reflecting the characteristic of small-scale obstacle avoidance adjustments in narrow spaces.

[0229] The above embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit it. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some or all of the technical features therein. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention. For those skilled in the art, any alternative improvements or transformations made to the implementation of the present invention fall within the protection scope of the present invention.

[0230] Any aspects of this invention not described in detail are well-known to those skilled in the art.

Claims

1. A method for obstacle avoidance path planning of a multi-degree-of-freedom robot arm based on three-dimensional reconstruction, characterized in that, The method comprises the following steps: Step 1, three-dimensional environment perception and dynamic modeling: laser radar and depth camera are used to synchronously collect data through hardware triggering, registration is carried out based on a checkerboard calibration board, point cloud and image fusion error is less than or equal to 5 mm, then statistical filtering method is used to remove noise from the fused point cloud, and then three-dimensional grid modeling method is used to generate a three-dimensional grid model; Through inter-frame difference method, points with Euclidean distance greater than 5 mm between corresponding points of two continuous frames of point cloud are marked as dynamic points, and a connected region with dynamic point proportion greater than 5% is determined as a dynamic obstacle; the model is updated every 100 ms; Step 2, obstacle feature quantification extraction: static obstacle calculates surface Gaussian curvature, curvature > 0.01 mm -1 Set as convex region, curvature < -0.01 mm -1 Set as concave region, hole size is measured by minimum circumscribed circle method; dynamic obstacle calculates motion parameters based on Kalman filter, output velocity vector and acceleration; Step 3: when expanding a new node in joint space, the near-obstacle node is preferentially biased to expand to a concave region or hole center to generate a candidate node set; Step 4: three-dimensional grid collision checking and safe path correction: the working space of the robot arm is divided into a three-dimensional voxel grid with a size of 1 mm3, and collision detection is realized by judging whether the path node falls into the obstacle voxel; The point cloud and image registration based on the checkerboard calibration board, the denoising of the fused point cloud and the three-dimensional grid modeling specifically comprise the following steps: Calibration preparation: a 10*8 square checkerboard calibration board is used, which is fixed in the working area of the robot arm, and the field of view of the laser radar and the depth camera can completely cover the calibration board, and the angle between the plane of the calibration board and the optical axis of the two devices is 45°±5°; Data acquisition: control the laser radar to collect 3 sets of calibration board point cloud data at a frequency of 10 Hz, and the depth camera collects 3 sets of depth images containing the calibration board at a frame rate of 30 Hz, each set of data is collected for 1 s, and the corner points are clear and free from motion blur; Corner point extraction: the three-dimensional coordinates of the corner points of the calibration board are extracted, the corner point pixel coordinates of the depth image are extracted and converted into three-dimensional coordinates, and the corner point positioning error is less than or equal to 1 pixel; Registration calculation: the transformation matrix of the corresponding corner points of the three sets is solved, the average re-projection error of each transformation matrix is calculated, the transformation matrix with the smallest error is selected as the optimal registration parameter, and the average error of a single set of corner points after registration is less than or equal to 3 mm; Fusion verification: fuse the laser point cloud and the depth image data, randomly select 50 scene feature points in a non-calibration board area, calculate the spatial distance between the fused point cloud and the original data, and the error of all points is less than or equal to 5 mm; Statistical filtering denoising: a statistical outlier filtering algorithm is used, the number of neighbor points searched is set to 30, the average distance μ and the standard deviation σ of each point and the neighbor points are calculated, the outlier points with a distance greater than 1.5σ are removed, the point cloud retention rate after processing is greater than or equal to 95%, and the feature point position deviation before and after filtering is less than or equal to 1 mm; Three-dimensional grid modeling: the Poisson surface reconstruction algorithm is used for the denoised fused point cloud, the reconstruction depth is set to 8 levels, and an initial three-dimensional grid model is generated; In step 4, when a collision is detected, the collision path is corrected by using a 3-order B-spline smoothing according to the motion trend of the dynamic obstacle, and the iteration optimization parameter is ensured to be greater than or equal to 15 mm away from the obstacle, and the specific steps are as follows: Collision interval positioning: determine the collision path segment through a 1mm3 voxel grid detection, extract the collision starting point P s , the collision end point P e , and the 6 control points composed of the 2 adjacent path points before and after the collision interval, to form the local correction path node set; Determine the motion direction according to the obstacle speed and acceleration; Construct a 3-order B-spline initial curve with 6 control points; Calculate the shortest distance between the initial curve and the obstacle, if it is less than 15 mm, start optimization; Adjust the control points in the opposite direction of the motion trend of the obstacle, regenerate the curve and detect the distance, and repeat the above steps until the distance is greater than or equal to 15 mm. Confirm that the joint angular velocity and angular acceleration corresponding to the regenerated modified curve are not out of limits, and obtain a curve meeting the requirements; Replace the original collision path with the obtained curve meeting the requirements as a local modified segment.

2. The method of claim 1, wherein, The specific steps of synchronously collecting environment data by the laser radar and the depth camera in step 1 are performed according to the following time sequence: Hardware connection: connect the trigger input interface of the 16-line laser radar and the synchronization input interface of the binocular structured light camera to the output end of the signal distributor respectively, and connect the input end of the signal distributor to the GPIO output pin of the same controller; Signal generation: the controller generates a 3.3V level trigger signal at a period of 100ms, and the pulse width is 10us. The signal is transmitted to the laser radar and the depth camera through the signal distributor; Data acquisition: the laser radar outputs one frame of point cloud data at a scanning frequency of 10Hz every time it receives one trigger signal; the depth camera outputs one frame of RGB-D image at a frame rate of 30Hz every time it accumulates three trigger signals; Timestamp recording: the controller timestamps the laser point cloud and depth image respectively through the built-in real-time clock; Synchronization calibration: calculate the timestamp difference of the two types of data. If it is ≤10ms, keep the current trigger mechanism; if it exceeds the threshold, adjust the trigger signal delay through the controller until the difference meets the requirements.

3. The method of claim 2, wherein, The specific steps of marking the points with a Euclidean distance >5mm between the corresponding points of two consecutive frames of point cloud as dynamic points by frame difference method are as follows: point cloud frame acquisition: collect two consecutive frames of point cloud data at a set time interval, denoted as frame t and frame t+1, to ensure that the two frames of point cloud are in the same coordinate system; Corresponding point search: for each point P in frame t, search for the nearest point Q in frame t+1 as the corresponding point of point P using the KD tree algorithm; Euclidean distance calculation: calculate the Euclidean distance d between point P and the corresponding point Q; Dynamic point marking: if the calculated Euclidean distance d >5mm, mark point P as a dynamic point.

4. The method of claim 3, wherein, The specific steps of calculating the motion parameters of the dynamic obstacle based on Kalman filtering are as follows: State initialization: set the initial state vector of the dynamic obstacle, including position, velocity and acceleration. The initial covariance matrix is initialized according to the initial positioning error of the obstacle. The initial values of velocity and acceleration in the static state are set to 0; State prediction: predict the state vector at time k+1 based on the state vector at current time k and the motion model; State correction: multiply the Kalman gain by the observation residual to obtain the state correction; add the correction to the predicted state vector at time k+1, and the result is the optimal state estimation at time k+1; Parameter output: extract the velocity vector and acceleration from the corrected state vector as the motion parameters output of the dynamic obstacle. Repeat steps 2-5 every 100ms to realize real-time updating.

5. The method of claim 4, wherein, Calculate the shortest distance between the current node and the obstacle grid, and mark it as a near-obstacle node if it is ≤20mm; Based on the results of step 2, extract the concave area and hole within a radius of 50mm centered on the near-obstacle node; Generate 6 basic extension directions corresponding to the positive and negative rotation directions of the 6-axis robot arm respectively; If a concave region or hole is identified, a bias vector pointing to its center is calculated, and a vector composition is performed with the base direction to form a bias expansion direction; The normal vector of the surface point cloud of the obstacle is calculated, and the directions with an included angle < 30° with the bias expansion direction are removed, and the directions with an included angle ≥ 30° are retained; The joint angle change amount of the remaining directions is verified for dynamic constraints, and the nodes that meet the constraints are screened and retained; The screened nodes are summarized to form a candidate node set, and each node contains joint angle parameters, distance from the obstacle, and information tags of the bias expansion direction.

6. The method of claim 5, wherein, Wherein, The dynamic constraint verification meets: the angle does not exceed the mechanical limit, the angular velocity ≤ 50° / s, and the angular acceleration ≤ 100° / s².

7. The method of claim 6, wherein the method further comprises: When fine-tuning the control points in the opposite direction of the obstacle movement trend, the adjustment distance is ≤ 5mm each time.

Images