Multi-degree-of-freedom mechanical arm obstacle avoidance path planning method based on three-dimensional reconstruction

Abstract

The invention relates to the technical field of mechanical arm obstacle avoidance path planning, in particular to a multi-degree-of-freedom mechanical arm obstacle avoidance path planning method based on three-dimensional reconstruction, which comprises the following steps: step 1, three-dimensional environment perception and dynamic modeling; preferentially offsetting and expanding the near-obstacle nodes towards the concave area or the hole center to generate a candidate node set; and 4, three-dimensional grid collision verification and safe path correction are conducted, specifically, the working space of the mechanical arm is divided into three-dimensional voxel grids, and collision detection is achieved by judging whether path nodes fall into obstacle object elements or not. According to the method, the laser radar and the depth camera are adopted to synchronously collect data through hardware triggering, statistical filtering denoising and three-dimensional grid modeling are combined, geometrical characteristics of static obstacles and motion parameters of dynamic obstacles are restored, and the collision risk caused by environmental perception errors of the mechanical arm is effectively avoided.

Description

Technical Field

[0001] The present invention relates to the technical field of robot arm obstacle avoidance path planning, in particular to a multi-degree-of-freedom robot arm obstacle avoidance path planning method based on three-dimensional reconstruction. Background Art

[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 the robotic arm's joint kinematic constraints and dynamic performance limitations.

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

[0004] For example, the invention patent with publication number CN112476438B discloses a robotic arm obstacle avoidance method, device, robotic arm, and robot. The 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] From the contents recorded in the above patent, it can be found that the obstacle avoidance method has the following disadvantages: First, the perception of the environment relies on a preset 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 risks.

[0006] Second, there are obvious limitations when used in highly dynamic and complex working conditions.

[0007] For example, in multi-robot collaborative assembly workshops and warehousing scenarios with mixed human-machine operations, traditional methods find it difficult to simultaneously handle real-time path conflicts among multiple dynamic targets (such as the movement of other robotic arms and the movement of operators).

[0008] 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 solution or repeated path replanning.

[0009] On the other hand, the obstacle avoidance priority mechanism has not been designed for multi-agent interaction scenarios, which may lead to path competition between robotic arms and between robotic arms and human bodies, resulting in reduced operating efficiency or even safety accidents.

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

[0011] The present invention solves one of the above technical problems and adopts a technical solution: a multi-degree-of-freedom robotic arm obstacle avoidance path planning method based on three-dimensional reconstruction, comprising the following steps: Step 1: 3D environment perception and dynamic modeling: Use lidar and depth cameras to synchronously collect data through hardware triggering, and align them based on a checkerboard calibration plate to ensure that the point cloud and image fusion error is ≤5mm. Then, statistical filtering methods are used to remove noise from the fused point cloud, and a 3D mesh model is generated using a 3D mesh modeling method.

[0012] Through the inter-frame difference method, points with a Euclidean distance of more than 5mm between corresponding points in two consecutive frames of point clouds are marked as dynamic points. Connected areas with a dynamic point ratio of more than 5% are determined to be dynamic obstacles, and the model is updated every 100ms.

[0013] Step 2: Quantitative extraction of obstacle features.

[0014] For static obstacles, the Gaussian curvature of the surface is calculated, and curvatures greater than 0.01mm⁻¹ are set as convex areas, and curvatures less than -0.01mm⁻¹ are set as concave areas. The minimum circumscribed circle method is used to measure the hole size. For dynamic obstacles, the motion parameters are calculated based on the Kalman filter, and the velocity vector and acceleration are output.

[0015] Step 3: When expanding new nodes in the joint space, the nodes near obstacles are preferentially expanded toward the concave area or the center of the hole to generate a set of candidate nodes.

[0016] In step 3, nodes near obstacles are preferentially expanded toward concave areas / holes (combining directional angles and dynamic constraints), utilizing the geometric features of the environment to reduce collision risks.

[0017] Step 4: 3D mesh collision check and safe path correction: The robot's workspace is divided into a 1mm³ 3D voxel grid. Collision detection is performed by determining whether the path nodes fall within the obstacle voxels.

[0018] Step 4 uses a 1mm³ voxel grid for high-precision collision detection and a third-order B-spline to generate a local correction segment (safety distance ≥ 15mm) for the collision path. This design balances path smoothness (B-spline characteristics) with safety. This design meets the actual needs of the robotic arm for obstacle avoidance in complex environments.

[0019] This planning method uses lidar and depth cameras to collect data synchronously, realizes point cloud and image fusion through checkerboard calibration plate alignment (error ≤ 5mm), and combines statistical filtering denoising and 3D mesh modeling to construct an environmental model more accurately. Through the inter-frame difference method (Euclidean distance > 5mm marks dynamic points, and proportion > 5% determines dynamic obstacles) and real-time updates (100ms / time), it takes into account the accuracy and timeliness of dynamic environment perception, which conforms to the technical logic of dynamic scene modeling.

[0020] Based on any of the above technical solutions, the following is further optimized: the specific steps of synchronously collecting environmental data by the laser radar and depth camera in step 1 are executed according to the following timing: 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 the input end of the signal distributor is connected to the GPIO output pin of the same controller.

[0021] The 16-line laser radar 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.

[0022] The controller uses STM32 controller.

[0023] Signal generation: The controller generates a 3.3V level trigger signal with a pulse width of 10μs at a period of 100ms. This signal is transmitted to the lidar and depth camera simultaneously through the signal distributor.

[0024] Data acquisition: Each time the lidar receives a trigger signal, it outputs one frame of point cloud data at a scanning frequency of 10Hz; each time the depth camera receives three trigger signals cumulatively, it outputs one frame of RGB-D image at a frame rate of 30Hz.

[0025] Timestamp recording: The controller uses a built-in real-time clock to timestamp the laser point cloud and depth image.

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

[0027] The advantages of the above-mentioned synchronous collection of environmental data are: through the fusion of multi-source data from lidar and depth cameras, the environment can be actively perceived before a collision occurs, avoiding the lag of adjustment after the collision; the combination of lidar and depth cameras can provide richer three-dimensional environmental information, and the long-range detection (0.5-100m) of the 16-line lidar and the close-range high-precision measurement (0.3-3m) of the binocular structured light camera complement each other, improving the perception coverage in complex environments; through real-time synchronous collection of dynamic data, environmental information can be updated in a timely manner, which is more adaptable to the sudden appearance or movement of obstacles in high-dynamic scenes; lidar is not affected by lighting conditions, and the synchronous collection of the two can reduce the perception error of pure vision in strong and low-light environments, and the timestamp difference within 10ms ensures the spatiotemporal consistency of multi-source data, providing more reliable input for subsequent three-dimensional reconstruction and path planning.

[0028] Based on any of the above technical solutions, further optimization is as follows: the specific methods of point cloud and image registration, fused point cloud denoising and 3D mesh modeling based on the checkerboard calibration plate include: Calibration preparation: Use a 10×8 square checkerboard calibration plate and fix it in the working area of ​​the robotic arm to ensure that the field of view of the lidar and depth camera can completely cover the calibration plate, and the angle between the calibration plate plane and the optical axis of the two devices is 45°±5°.

[0029] Data acquisition: The LiDAR is controlled to collect three sets of calibration plate point cloud data at a frequency of 10 Hz. The depth camera collects three sets of depth images including the calibration plate at a frame rate of 30 Hz. Each set of data is collected for 1 second to ensure that the corners are clear and free of motion blur.

[0030] Corner point extraction: Extract the 3D coordinates of the corner points of the calibration plate, extract the pixel coordinates of the corner points of the depth image and convert them into 3D coordinates. The corner point positioning error is ≤ 1 pixel.

[0031] Registration calculation: Solve the transformation matrices of three groups of corresponding corner points, calculate the average reprojection error of each group 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 group of corner points is ≤3mm.

[0032] Fusion verification: Fuse the laser point cloud with the depth image data, randomly select 50 scene feature points in the non-calibration plate area, and calculate the spatial distance between the fused point cloud and the original data. The error of all points is ≤5mm.

[0033] Statistical filtering denoising: Using the statistical outlier filtering algorithm, the number of neighbor point searches is set to 30, the average distance μ and standard deviation σ between each point and its neighbor points are calculated, and outliers with a distance greater than 1.5σ are removed. After processing, the point cloud retention rate is ≥95%, and the position deviation of feature points before and after filtering is ≤1mm.

[0034] 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 the initial 3D mesh model.

[0035] In the corner point extraction step, the three-dimensional coordinates of the corner points of the calibration plate are extracted by a method based on point cloud curvature calculation: a KD tree structure is constructed using the point cloud data obtained by the lidar, and the covariance matrix of the neighborhood of each point is calculated to obtain the principal curvature of the point.

[0036] An appropriate curvature threshold (such as greater than 0.8) is set to preliminarily screen corner points, and then accurate extraction is performed based on the geometric distribution characteristics of the checkerboard corner points (the distance between adjacent corner points is consistent with the side length of the checkerboard squares).

[0037] To extract the pixel coordinates of the corner points in the depth image, a sub-pixel corner detection algorithm is used. The depth image is subjected to Gaussian filtering to remove noise. The gradient of the image in the x and y directions is then calculated. The response value of each pixel is calculated by constructing a Harris response matrix. Pixels with response values ​​greater than the set threshold (0.01) are selected as candidate corner points. The parabolic interpolation method is then used to locate the corner points at the sub-pixel level, thereby converting the pixel coordinates of the corner points into three-dimensional coordinates.

[0038] It should be noted that during statistical filtering and denoising, if the position deviation of feature points before and after filtering is greater than 1mm, it may be caused by an improper setting of the number of neighboring points to search or the standard deviation multiple. It is necessary to appropriately increase the number of neighboring points to search (for example, from 30 to 40) to make the statistical results more representative; or reduce the standard deviation multiple (for example, from 1.5 to 1.3) to reduce the tolerance for outliers, and re-filter to ensure that the feature point position deviation meets the requirements.

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

[0040] For scenes 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 neighboring points to search for can improve computational efficiency while avoiding inaccurate statistical results due to too few neighboring points.

[0041] The standard deviation multiple is adjusted according to the noise distribution. When the noise is large and the distribution is relatively discrete, the standard deviation multiple is appropriately increased; if the noise is relatively small and concentrated, the standard deviation multiple can be reduced.

[0042] The Poisson surface reconstruction algorithm's reconstruction depth setting of 8 levels is related to the complexity of the point cloud data and the desired mesh model accuracy. For point clouds of simple-shaped objects (such as regular cuboids and cylinders), a lower reconstruction depth (such as level 6) can generate a more accurate mesh model, reducing the amount of computation. For point clouds of complex-shaped objects (such as the complex joints of a robotic arm), increasing the reconstruction depth (such as level 10) can better capture object details, but the computation time will increase accordingly. In practical applications, the reconstruction depth can be flexibly adjusted based on the geometric characteristics of the point cloud (such as changes in curvature) and computing resource constraints. In this solution, a reconstruction depth setting of 8 is preferred.

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

[0044] 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 as the corresponding point of point P.

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

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

[0047] The advantages of this processing are: by setting a Euclidean distance threshold (5mm), it can effectively distinguish point cloud displacement caused by object motion from sensor noise or measurement error (usually less than this threshold), reducing the probability of static points being misidentified as dynamic points. This allows for high accuracy in identifying dynamic points of slightly moving objects (such as micro-motions of robotic arm joints or slow movement of people). Secondly, the KD tree algorithm is used for corresponding point search, combined with a frame acquisition strategy with a fixed time interval (e.g., 100ms). This manages computational complexity and meets the requirements for real-time detection of dynamic obstacles within the robotic arm's workspace, ensuring that environmental model updates are time-matched with the robotic arm's motion planning.

[0048] In addition, the above method does not rely on additional information such as the color and reflection intensity of the point cloud, and only realizes dynamic point marking through the difference in three-dimensional coordinates. It is suitable for point cloud data collected by various devices such as lidar and depth cameras, and can still work stably in complex scenes such as lighting changes and texture loss.

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

[0050] This method effectively simplifies dynamic obstacle modeling. Compared with dynamic target detection based on deep learning, the above method does not require training samples and can 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 of robotic arms).

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

[0052] Initial state vector setting rationale: Position, velocity, and acceleration are selected to form the initial state vector because, in dynamic obstacle monitoring scenarios within the robot's workspace, position directly reflects the real-time spatial relationship between the obstacle and the robot, while velocity and acceleration reflect its motion trend. The combination of these three provides a comprehensive description of the obstacle's motion, providing a complete foundation for subsequent predictions and updates. The initial values ​​of velocity and acceleration are set to 0 in the static state because the obstacle exhibits no noticeable motion in this state. In real-world scenarios, in the absence of external interference, velocity and acceleration fluctuate around 0, so their initial values ​​are set to 0.

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

[0054] The motion model adopts the uniform acceleration motion model in the prior art.

[0055] Observation update: Obtain the observed position of the dynamic obstacle at time k+1 (the dynamic point cluster center obtained by the inter-frame difference method), calculate the observation residual, and combine it with the observation noise matrix (for example, the position observation noise variance is 0.0025m²) to solve the Kalman gain; In practical applications, various complex interference factors exist in the real world. For example, in intelligent transportation scenarios, vehicles are affected by road bumps and air currents, while in industrial scenarios, mechanical components are affected by electromagnetic interference and mechanical vibration. These interferences can cause the actual movement of obstacles to deviate from the ideal uniform acceleration model. These interferences are simulated by introducing appropriate noise terms into the model. In actual applications, technicians can adjust the intensity of the noise term based on the complexity of the scenario and the characteristics of the interference to improve the accuracy of state prediction.

[0056] 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 needs to be further used to process these dynamic points to obtain the cluster centers. Usually, the Euclidean clustering algorithm is used.

[0057] When determining the clustering distance threshold, multiple factors must be considered. In scenarios with high point cloud data density and small obstacles, the clustering distance threshold can be appropriately reduced, for example, to 5mm, to ensure that dynamic points belonging to the same obstacle are accurately clustered together. In scenarios with low point cloud data density and large obstacles, the clustering distance threshold can be increased to 10mm. After clustering is complete, the coordinates of the cluster center are calculated by averaging the coordinates of all points within the cluster, allowing technicians to clearly and accurately determine the observed location of dynamic obstacles.

[0058] Specific steps for calculating observation residuals: Determine the observation position of the dynamic obstacle at time k+1: first, use the inter-frame difference method to filter out dynamic points from two consecutive frames of point cloud, 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 value of all points in the cluster area as the observation position at time k+1.

[0059] 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, the predicted position at time k+1 is calculated according to the law of uniform acceleration (that is, the change in position over time includes both uniform displacement and uniform acceleration).

[0060] Calculate the residuals in the three-dimensional direction: 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 the z-coordinate to obtain the observation residuals in the y- and z-directions.

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

[0062] For example, common sensors like lidar and depth cameras can achieve position measurement accuracy of ±5mm under ideal conditions. However, in complex real-world environments, factors such as electromagnetic interference, light reflection interference, and dust can cause measurement fluctuations. Setting the position observation noise variance to 0.0025m² (5mm squared) effectively reflects the measurement uncertainty caused by environmental interference and sensor errors.

[0063] In different application scenarios, technicians in this field can reasonably adjust the element values ​​in the observation noise matrix according to the specific performance parameters of the sensors used and the severity of the actual environmental interference to optimize the performance of the Kalman filter algorithm.

[0064] 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 estimate at time k+1.

[0065] The Kalman gain is essentially a weighted coefficient for the uncertainty of the predicted state and the reliability of the observed information. Its size is determined by both the prediction error and the observation error. When the observation value is more reliable, the gain is greater and the correction amount adjusts the predicted state more significantly; when the prediction is more stable, the gain is smaller and the correction is smoother.

[0066] The correction amount obtained by gain × residual can accurately balance the weights of the two types of information and achieve optimal fusion. In addition, from a probabilistic perspective, the above process is equivalent to solving the optimal estimate under the minimum mean square error criterion. The superposition of the predicted state and the correction amount is essentially to inject the new information contained in the observation into the prediction model. The final result is the optimal approximation of the true state of the dynamic obstacle in a statistical sense, which is consistent with the mathematical derivation of the Kalman filter. The above steps can be completed through simple algebraic operations without the need for complex iterations or parameter debugging. Technicians can directly implement it according to the logic programming, taking into account both algorithm accuracy and engineering operability, and are suitable for dynamic obstacle monitoring scenarios with high real-time requirements.

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

[0068] Based on any of the above technical solutions, further optimization is: calculating the shortest distance between the current node and the obstacle grid, and marking it as a near-obstacle node when it is ≤20mm.

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

[0070] Generate 6 basic extension directions corresponding to the positive and negative rotation directions of the 6-axis robot arm.

[0071] If a concave area or hole is identified, the deflection vector pointing to its center is calculated, and the vector is synthesized with the basic direction to form the biased expansion direction.

[0072] The normal vector of the obstacle surface point cloud is calculated, and the directions with an angle of less than 30° with the offset extension direction are eliminated, and the directions with an angle of ≥30° are retained.

[0073] Perform dynamic constraint verification on the joint angle changes in the remaining directions, and filter and retain the nodes that meet the constraints.

[0074] The filtered nodes are aggregated to form a candidate node set, each of which contains information labels such as joint rotation parameters, distance to obstacles, and offset extension direction.

[0075] Among them, the specific steps of calculating the normal vector through the 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.

[0076] Construct a covariance matrix: Construct the three-dimensional coordinate data of the neighborhood points into a 3×N matrix (N is the number of neighborhood points) and calculate the covariance matrix C of the matrix.

[0077] The covariance matrix reflects the degree of dispersion and correlation of neighborhood points in the directions of the three coordinate axes.

[0078] Eigenvalue decomposition: Perform eigenvalue decomposition on the covariance matrix C to obtain three eigenvalues ​​λ1 ≥ λ2 ≥ λ3 and the 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.

[0079] Determine the normal vector: Since the normal vector direction is the direction with the least change in the point cloud data, the eigenvector V3 corresponding to the minimum eigenvalue λ3 is selected as the normal vector for the target point. To ensure the consistency of the normal vector direction (for example, uniformly pointing outward), the reference direction can be set according to the scene requirements to adjust the normal vector direction.

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

[0081] Based on any of the above technical solutions, further optimization is as follows: wherein, during the dynamic constraint verification, the following conditions are met: the rotation angle does not exceed the mechanical limit, the angular velocity ≤ 50° / s, and the angular acceleration ≤ 100° / s².

[0082] Based on any of the above technical solutions, further optimization is as follows: in step 4, when a collision is detected, the collision path is corrected using a third-order B-spline smoothing method according to the movement trend of the dynamic obstacle, and the parameters are iteratively optimized to ensure that the distance to the obstacle is ≥15mm. The specific steps are as follows.

[0083] Collision interval positioning: Determine the collision path segment through 1mm³ voxel grid detection and extract the collision starting point P s , collision end point P e , and 6 control points consisting of 2 adjacent path points before and after the collision interval, constitute the local corrected path node set; Determine the direction of movement based on the speed and acceleration of the obstacle.

[0084] Construct a 3rd-order B-spline initial curve with 6 control points.

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

[0086] 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. The maximum number of iterations is 30.

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

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

[0089] Based on any of the above technical solutions, further optimization is that when fine-tuning the control point in the opposite direction of the obstacle movement trend, the adjustment distance each time is ≤5mm.

[0090] Compared with the prior art, the present invention has the following beneficial effects: 1. This invention achieves high-precision 3D environmental perception and dynamic modeling: It uses hardware-triggered synchronous data acquisition from a LiDAR and depth camera, and uses checkerboard calibration to ensure that the point cloud and image fusion error is ≤5mm. Combined with statistical filtering and denoising, it can accurately restore the geometric features of static obstacles (such as distinguishing convex and concave areas through Gaussian curvature) and the motion parameters (speed and acceleration) of dynamic obstacles. This provides a realistic and reliable environmental model for the robotic arm, effectively avoiding the risk of collision caused by environmental perception errors.

[0091] 2. This invention significantly improves the ability to detect and avoid dynamic obstacles: it uses inter-frame differencing to mark dynamic points, and identifies connected areas with a dynamic point ratio greater than 5% as dynamic obstacles. It then uses a third-order B-spline curve to smoothly correct the collision path based on the obstacle's motion trend, and iterative optimization ensures a distance to the obstacle of 15 mm or greater.

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

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

[0094] This strategy makes full use of the geometric characteristics of the environment, reduces invalid searches, and shortens path planning time by 40%. At the same time, it ensures that the planned path is both safe and complies with the dynamic limitations of the robot arm, avoids excessive joint movement, and extends the service life of the robot arm.

[0095] 4. The present invention enhances the environmental adaptability and robustness of the system: the hardware-triggered synchronous acquisition mechanism and timestamp calibration function ensure the spatiotemporal consistency of multi-source data, and can stably obtain accurate environmental information even in complex industrial environments such as electromagnetic interference and lighting changes.

[0096] In addition, for different types of obstacles (flat, vertical, diagonal, dynamic, etc.) and working scenarios (narrow passages, open spaces, etc.), the present invention can achieve effective obstacle avoidance and path correction through corresponding algorithm strategies, and has wide applicability.

[0097] 5. This invention provides a good 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 the robotic arm with artificial intelligence algorithms, facilitating the subsequent implementation of advanced functions such as autonomous learning and intelligent decision-making. BRIEF DESCRIPTION OF THE DRAWINGS

[0098] In order to more clearly illustrate the technical solutions of the specific embodiments of the present invention, the following is a brief introduction to the drawings required for the specific embodiments. The drawings in the specification of the present invention are used to show the third-order B-spline curves generated by the control points of the robotic arm in different scenarios.

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

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

[0101] Figure 3 This is a third-order B-spline curve graph generated by the control points of the robotic arm of the present invention in an oblique obstacle scenario.

[0102] Figure 4 This is a third-order B-spline curve graph generated by the control points of the robotic arm of the present invention in a narrow channel scenario.

[0103] Figure 5 This is a third-order B-spline curve graph generated by control points in a scenario where the robotic arm of the present invention moves laterally around a dynamic obstacle.

[0104] In the figure, the black dots are control points; the lines are 3rd order B-spline curves; Ps is the collision starting point; P e The collision end point. DETAILED DESCRIPTION

[0105] The following embodiments of the technical solution of the present invention are 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 only used as examples and are not intended to limit the scope of protection of the present invention. Figure 1-Figure 5 As shown in .

[0106] Embodiment: A multi-degree-of-freedom robotic arm obstacle avoidance path planning method based on three-dimensional reconstruction, comprising the following steps: Step 1: 3D environment perception and dynamic modeling: Use lidar and depth cameras to synchronously collect data through hardware triggering, and align them based on a checkerboard calibration plate to ensure that the point cloud and image fusion error is ≤5mm. Then, statistical filtering methods are used to remove noise from the fused point cloud, and a 3D mesh model is generated using a 3D mesh modeling method.

[0107] Through hardware trigger synchronization, data collection coordination between the lidar and depth camera is achieved, and a checkerboard calibration plate is used to complete the spatial alignment of the point cloud (lidar) and the image (depth camera), fusing the two types of data into the same coordinate system. A statistical filtering algorithm is used to remove noise points in the fused point cloud and retain effective environmental features. Finally, a three-dimensional grid modeling method is used to convert the denoised point cloud into a three-dimensional grid environment model that can be recognized by the robotic arm, providing basic environmental information for subsequent path planning.

[0108] Among them, hardware selection and compatibility: LiDAR: An optional 16-line mechanical rotating radar, the interface supports hardware triggering, and meets the vibration and dustproof requirements of industrial environments. Depth camera: A binocular structured light camera with a hardware synchronization interface and depth output API, which is convenient for docking with the LiDAR trigger signal. Controller: STM32F4 series microcontroller or industrial-grade embedded computer with multi-channel GPIO output and real-time clock function, which can stably drive sensor synchronization. The Poisson reconstruction algorithm calls the open source library (such as PoissonRecon) or the implementation in PCL. By setting the reconstruction depth (8 levels) and the number of threads (such as 4 cores in parallel), the mesh generation of a 100,000 point cloud can be completed within 500ms on an ordinary PC (i5-8400CPU).

[0109] Through the inter-frame difference method, points with a Euclidean distance of more than 5mm between corresponding points in two consecutive frames of point clouds are marked as dynamic points. Connected areas with a dynamic point ratio of more than 5% are determined to be dynamic obstacles, and the model is updated every 100ms.

[0110] Step 2: Quantitative extraction of obstacle features.

[0111] For static obstacles, the Gaussian curvature of the surface is calculated, and curvatures greater than 0.01mm⁻¹ are set as convex areas, and curvatures less than -0.01mm⁻¹ are set as concave areas. The minimum circumscribed circle method is used to measure the hole size. For dynamic obstacles, the motion parameters are calculated based on the Kalman filter, and the velocity vector and acceleration are output.

[0112] Step 3: When expanding new nodes in the joint space, the nodes near obstacles are preferentially expanded toward the concave area or the center of the hole to generate a set of candidate nodes.

[0113] Static obstacle feature extraction achieves geometric attribute quantification through Gaussian curvature calculation: For the surface point cloud of static obstacles, the principal curvature is solved through the neighborhood covariance matrix, and the Gaussian curvature (the product of the two principal curvatures) is calculated. Areas with curvature greater than 0.01mm⁻¹ are defined as convex areas, and areas with curvature less than -0.01mm⁻¹ are defined as concave areas. The minimum circumscribed circle method is used to measure the hole size. That is, the minimum circumscribed circle of the hole edge points is fitted, and its diameter is used as the hole size parameter.

[0114] The calculation of dynamic obstacle motion parameters is based on the Kalman filter algorithm: the dynamic obstacle state vector (including position, velocity, acceleration) and covariance matrix are initialized, and the uniform acceleration motion model is used to predict the state at the next moment. The dynamic point cluster center (observation position) obtained by the inter-frame difference method is combined, the observation residual is calculated and the Kalman gain is solved, the predicted state is corrected, and finally the velocity vector and acceleration of the dynamic obstacle are output to achieve real-time estimation of motion parameters.

[0115] The working principle of the near-obstacle node bias expansion in step 3 is as follows: the shortest distance between the path node and the obstacle grid is calculated, and nodes with a distance ≤ 20 mm are marked as near-obstacle nodes. The concave areas and holes within a radius of 50 mm centered on the near-obstacle node are extracted to generate the basic expansion directions for the positive and negative rotation directions of the 6-axis robot arm. The bias vector pointing to the center of the concave area or hole is synthesized with the basic direction to form the biased expansion direction. The directions with an angle of less than 30° with the normal vector of the obstacle surface are eliminated. After verification by dynamic constraints (angle limit, angular velocity ≤ 50° / s², angular acceleration ≤ 100° / s²), the candidate node set is summarized.

[0116] In step 3, nodes near obstacles are preferentially expanded toward concave areas / holes (combining directional angles and dynamic constraints), utilizing the geometric features of the environment to reduce collision risks.

[0117] Step 4: 3D mesh collision check and safe path correction: The robot's workspace is divided into a 1mm³ 3D voxel grid. Collision detection is performed by determining whether the path nodes fall within the obstacle voxels.

[0118] The 3D mesh collision verification and safe path correction in step 4 implement collision detection through high-precision voxel modeling and geometric containment judgment: the robot arm workspace is divided into a 3D voxel grid with a resolution of 1mm³, and each voxel is marked as an obstacle voxel (contains an obstacle) or a free voxel (no obstacle). For each node generated by the path planning, coordinate mapping is used to determine whether it falls within the obstacle voxel. If it does, it is determined to be a collision node, and the corresponding path segment is the collision path segment, providing a basis for subsequent safe path correction.

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

[0120] Step 4 uses a 1mm³ voxel grid for high-precision collision detection and a third-order B-spline to generate a local correction segment (safety distance ≥ 15mm) for the collision path. This design balances path smoothness (B-spline characteristics) with safety. This design meets the actual needs of the robotic arm for obstacle avoidance in complex environments.

[0121] This planning method uses lidar and depth cameras to collect data synchronously, realizes point cloud and image fusion through checkerboard calibration plate alignment (error ≤ 5mm), and combines statistical filtering denoising and 3D mesh modeling to construct an environmental model more accurately. Through the inter-frame difference method (Euclidean distance > 5mm marks dynamic points, and proportion > 5% determines dynamic obstacles) and real-time updates (100ms / time), it takes into account the accuracy and timeliness of dynamic environment perception, which conforms to the technical logic of dynamic scene modeling.

[0122] Based on any of the above technical solutions, the following is further optimized: the specific steps of synchronously collecting environmental data by the laser radar and depth camera in step 1 are executed according to the following timing: 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 the input end of the signal distributor is connected to the GPIO output pin of the same controller.

[0123] The 16-line laser radar 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.

[0124] Synchronous data acquisition of the 16-line lidar and binocular structured light camera is achieved through hardware-level trigger link design: the trigger input interface of the 16-line lidar and the synchronization input interface of the binocular structured light camera are respectively connected to the output end of the signal distributor, and the input end of the signal distributor is connected to the GPIO output pin of the same controller, forming a trigger signal transmission link from controller to signal distributor to dual sensors.

[0125] The controller uses an STM32. The controller generates a synchronous trigger signal that is transmitted simultaneously to the LiDAR and binocular structured light camera via a signal distributor, forcing both to start data acquisition at the same physical moment. This eliminates asynchronous errors in the multi-source sensors at the hardware level and ensures temporal consistency between the laser point cloud and the depth image.

[0126] Signal generation: The controller generates a 3.3V level trigger signal with a pulse width of 10μs at a period of 100ms. This signal is transmitted to the lidar and depth camera simultaneously through the signal distributor.

[0127] The controller uses an internal timer module to generate a periodic level trigger signal, specifically a rectangular wave signal with a 3.3V voltage amplitude and a 10μs pulse width, outputted at a 100ms period. This signal is amplified and replicated by the signal distributor's hardware circuitry (such as a differential amplifier or multi-channel buffer). This ensures that the signal waveforms are not significantly attenuated or distorted when transmitted simultaneously to the trigger interfaces of the lidar and depth camera, enabling the acquisition of both sensors to be triggered at the same time.

[0128] Data acquisition: Each time the lidar receives a trigger signal, it outputs one frame of point cloud data at a scanning frequency of 10Hz; each time the depth camera receives three trigger signals cumulatively, it outputs one frame of RGB-D image at a frame rate of 30Hz.

[0129] Timestamp recording: The controller uses a built-in real-time clock to timestamp the laser point cloud and depth image.

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

[0131] The advantages of the above-mentioned synchronous collection of environmental data are: through the fusion of multi-source data from lidar and depth cameras, the environment can be actively perceived before a collision occurs, avoiding the lag of adjustment after the collision; the combination of lidar and depth cameras can provide richer three-dimensional environmental information, and the long-range detection (0.5-100m) of the 16-line lidar and the close-range high-precision measurement (0.3-3m) of the binocular structured light camera complement each other, improving the perception coverage in complex environments; through real-time synchronous collection of dynamic data, environmental information can be updated in a timely manner, which is more adaptable to the sudden appearance or movement of obstacles in high-dynamic scenes; lidar is not affected by lighting conditions, and the synchronous collection of the two can reduce the perception error of pure vision in strong and low-light environments, and the timestamp difference within 10ms ensures the spatiotemporal consistency of multi-source data, providing more reliable input for subsequent three-dimensional reconstruction and path planning.

[0132] The data acquisition process achieves multi-sensor collaborative sampling through trigger signal counting and frequency matching: each time the 16-line lidar receives a trigger signal, it outputs one frame of point cloud data at a 10Hz scanning frequency (a single frame of point cloud contains ≥16,384 points). The binocular structured light camera outputs one frame of RGB-D image (containing a 640×480 pixel color image and depth image) at a 30Hz frame rate every time it receives three trigger signals (i.e., a 300ms interval). This forms a sampling rhythm of high-frequency global scanning by the lidar + low-frequency detail capture by the depth camera.

[0133] Timestamp recording is achieved through the controller's real-time clock synchronization mark: at the rising edge of the trigger signal, the controller uses the built-in real-time clock (RTC) to timestamp the point cloud data output by the lidar and the RGB-D image output by the depth camera with nanosecond accuracy, ensuring that the collection time of the two types of data can be quantified and traced.

[0134] Synchronous 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 state is determined to be qualified and the current trigger signal parameters are maintained. If the difference is greater than 10ms, the controller gradually corrects the sending time of the trigger signal by adjusting the delay parameters 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.

[0135] Based on any of the above technical solutions, further optimization is as follows: the specific methods of point cloud and image registration, fused point cloud denoising and 3D mesh modeling based on the checkerboard calibration plate include: Calibration preparation: Use a 10×8 square checkerboard calibration plate and fix it in the working area of ​​the robotic arm to ensure that the field of view of the lidar and depth camera can completely cover the calibration plate, and the angle between the calibration plate plane and the optical axis of the two devices is 45°±5°.

[0136] Data acquisition: The LiDAR is controlled to collect three sets of calibration plate point cloud data at a frequency of 10 Hz. The depth camera collects three sets of depth images including the calibration plate at a frame rate of 30 Hz. Each set of data is collected for 1 second to ensure that the corners are clear and free of motion blur.

[0137] Corner point extraction: Extract the 3D coordinates of the corner points of the calibration plate, extract the pixel coordinates of the corner points of the depth image and convert them into 3D coordinates. The corner point positioning error is ≤ 1 pixel.

[0138] For the calibration plate point cloud data collected by the lidar, a KD tree topology structure 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, and a curvature threshold (e.g., >0.8) is set to screen out candidate points with obvious edge or corner features. Furthermore, based on the geometric constraints of the checkerboard corner points (the spacing between adjacent corner points is equal to the side length of the square, e.g., 20mm±1mm), the RANSAC algorithm is used to eliminate mismatched points and accurately extract the three-dimensional coordinates of the corner points.

[0139] Depth image corner extraction and conversion: The depth image is first subjected to Gaussian filtering (σ=0.5) to remove noise. The Harris corner detection algorithm is then used to calculate the response value of each pixel (the threshold is set to 0.01). Sub-pixel positioning (accuracy ≤ 0.5 pixel) is achieved through parabolic interpolation to obtain the pixel coordinates of the corner points.

[0140] Registration calculation: Solve the transformation matrices of three groups of corresponding corner points, calculate the average reprojection error of each group 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 group of corner points is ≤3mm.

[0141] Fusion verification: Fuse the laser point cloud with the depth image data, randomly select 50 scene feature points in the non-calibration plate area, and calculate the spatial distance between the fused point cloud and the original data. The error of all points is ≤5mm.

[0142] Statistical filtering denoising: Using the statistical outlier filtering algorithm, the number of neighbor point searches is set to 30, the average distance μ and standard deviation σ between each point and its neighbor points are calculated, and outliers with a distance greater than 1.5σ are removed. After processing, the point cloud retention rate is ≥95%, and the position deviation of feature points before and after filtering is ≤1mm.

[0143] 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 the initial 3D mesh model.

[0144] In the corner point extraction step, the three-dimensional coordinates of the corner points of the calibration plate are extracted by a method based on point cloud curvature calculation: a KD tree structure is constructed using the point cloud data obtained by the lidar, and the covariance matrix of the neighborhood of each point is calculated to obtain the principal curvature of the point.

[0145] An appropriate curvature threshold (such as greater than 0.8) is set to preliminarily screen corner points, and then accurate extraction is performed based on the geometric distribution characteristics of the checkerboard corner points (the distance between adjacent corner points is consistent with the side length of the checkerboard squares).

[0146] To extract the pixel coordinates of the corner points in the depth image, a sub-pixel corner detection algorithm is used. The depth image is subjected to Gaussian filtering to remove noise. The gradient of the image in the x and y directions is then calculated. The response value of each pixel is calculated by constructing a Harris response matrix. Pixels with response values ​​greater than the set threshold (0.01) are selected as candidate corner points. The parabolic interpolation method is then used to locate the corner points at the sub-pixel level, thereby converting the pixel coordinates of the corner points into three-dimensional coordinates.

[0147] It should be noted that during statistical filtering and denoising, if the position deviation of feature points before and after filtering is greater than 1mm, it may be caused by an improper setting of the number of neighboring points to search or the standard deviation multiple. It is necessary to appropriately increase the number of neighboring points to search (for example, from 30 to 40) to make the statistical results more representative; or reduce the standard deviation multiple (for example, from 1.5 to 1.3) to reduce the tolerance for outliers, and re-filter to ensure that the feature point position deviation meets the requirements.

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

[0149] For scenes 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 neighboring points to search for can improve computational efficiency while avoiding inaccurate statistical results due to too few neighboring points.

[0150] The standard deviation multiple is adjusted according to the noise distribution. When the noise is large and the distribution is relatively discrete, the standard deviation multiple is appropriately increased; if the noise is relatively small and concentrated, the standard deviation multiple can be reduced.

[0151] The Poisson surface reconstruction algorithm's reconstruction depth setting of 8 levels is related to the complexity of the point cloud data and the desired mesh model accuracy. For point clouds of simple-shaped objects (such as regular cuboids and cylinders), a lower reconstruction depth (such as level 6) can generate a more accurate mesh model, reducing the amount of computation. For point clouds of complex-shaped objects (such as the complex joints of a robotic arm), increasing the reconstruction depth (such as level 10) can better capture object details, but the computation time will increase accordingly. In practical applications, the reconstruction depth can be flexibly adjusted based on the geometric characteristics of the point cloud (such as changes in curvature) and computing resource constraints. In this solution, a reconstruction depth setting of 8 is preferred.

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

[0153] 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 as the corresponding point of point P.

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

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

[0156] Dynamic point recognition 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 cloud are in the same three-dimensional coordinate system after coordinate alignment; 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 is greater than 5mm, point P is marked as a dynamic point, thereby distinguishing points with significant position changes in the environment from static points.

[0157] The advantages of this processing are: by setting a Euclidean distance threshold (5mm), it can effectively distinguish point cloud displacement caused by object motion from sensor noise or measurement error (usually less than this threshold), reducing the probability of static points being misidentified as dynamic points. This allows for high accuracy in identifying dynamic points of slightly moving objects (such as micro-motions of robotic arm joints or slow movement of people). Secondly, the KD tree algorithm is used for corresponding point search, combined with a frame acquisition strategy with a fixed time interval (e.g., 100ms). This manages computational complexity and meets the requirements for real-time detection of dynamic obstacles within the robotic arm's workspace, ensuring that environmental model updates are time-matched with the robotic arm's motion planning.

[0158] In addition, the above method does not rely on additional information such as the color and reflection intensity of the point cloud, and only realizes dynamic point marking through the difference in three-dimensional coordinates. It is suitable for point cloud data collected by various devices such as lidar and depth cameras, and can still work stably in complex scenes such as lighting changes and texture loss.

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

[0160] This method effectively simplifies dynamic obstacle modeling. Compared with dynamic target detection based on deep learning, the above method does not require training samples and can 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 of robotic arms).

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

[0162] Initial state vector setting rationale: Position, velocity, and acceleration are selected to form the initial state vector because, in dynamic obstacle monitoring scenarios within the robot's workspace, position directly reflects the real-time spatial relationship between the obstacle and the robot, while velocity and acceleration reflect its motion trend. The combination of these three provides a comprehensive description of the obstacle's motion, providing a complete foundation for subsequent predictions and updates. The initial values ​​of velocity and acceleration are set to 0 in the static state because the obstacle exhibits no noticeable motion in this state. In real-world scenarios, in the absence of external interference, velocity and acceleration fluctuate around 0, so their initial values ​​are set to 0.

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

[0164] The motion model adopts the uniform acceleration motion model in the prior art.

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

[0166] In practical applications, various complex interference factors exist in the real world. For example, in intelligent transportation scenarios, vehicles are affected by road bumps and air currents, while in industrial scenarios, mechanical components are affected by electromagnetic interference and mechanical vibration. These interferences can cause the actual movement of obstacles to deviate from the ideal uniform acceleration model. These interferences are simulated by introducing appropriate noise terms into the model. In actual applications, technicians can adjust the intensity of the noise term based on the complexity of the scenario and the characteristics of the interference to improve the accuracy of state prediction.

[0167] 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 needs to be further used to process these dynamic points to obtain the cluster centers. Usually, the Euclidean clustering algorithm is used.

[0168] When determining the clustering distance threshold, multiple factors must be considered. In scenarios with high point cloud data density and small obstacles, the clustering distance threshold can be appropriately reduced, for example, to 5mm, to ensure that dynamic points belonging to the same obstacle are accurately clustered together. In scenarios with low point cloud data density and large obstacles, the clustering distance threshold can be increased to 10mm. After clustering is complete, the coordinates of the cluster center are calculated by averaging the coordinates of all points within the cluster, allowing technicians to clearly and accurately determine the observed location of dynamic obstacles.

[0169] Specific steps for calculating observation residuals: Determine the observation position of the dynamic obstacle at time k+1: first, use the inter-frame difference method to filter out dynamic points from two consecutive frames of point cloud, 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 value of all points in the cluster area as the observation position at time k+1.

[0170] 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, the predicted position at time k+1 is calculated according to the law of uniform acceleration (that is, the change in position over time includes both uniform displacement and uniform acceleration).

[0171] Calculate the residuals in the three-dimensional direction: 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 the z-coordinate to obtain the observation residuals in the y- and z-directions.

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

[0173] For example, common sensors like lidar and depth cameras can achieve position measurement accuracy of ±5mm under ideal conditions. However, in complex real-world environments, factors such as electromagnetic interference, light reflection interference, and dust can cause measurement fluctuations. Setting the position observation noise variance to 0.0025m² (5mm squared) effectively reflects the measurement uncertainty caused by environmental interference and sensor errors.

[0174] In different application scenarios, technicians in this field can reasonably adjust the element values ​​in the observation noise matrix according to the specific performance parameters of the sensors used and the severity of the actual environmental interference to optimize the performance of the Kalman filter algorithm.

[0175] 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 estimate at time k+1.

[0176] The Kalman gain is essentially a weighted coefficient for the uncertainty of the predicted state and the reliability of the observed information. Its size is determined by both the prediction error and the observation error. When the observation value is more reliable, the gain is greater and the correction amount adjusts the predicted state more significantly; when the prediction is more stable, the gain is smaller and the correction is smoother.

[0177] The correction amount obtained by gain × residual can accurately balance the weights of the two types of information and achieve optimal fusion. In addition, from a probabilistic perspective, the above process is equivalent to solving the optimal estimate under the minimum mean square error criterion. The superposition of the predicted state and the correction amount is essentially to inject the new information contained in the observation into the prediction model. The final result is the optimal approximation of the true state of the dynamic obstacle in a statistical sense, which is consistent with the mathematical derivation of the Kalman filter. The above steps can be completed through simple algebraic operations without the need for complex iterations or parameter debugging. Technicians can directly implement it according to the logic programming, taking into account both algorithm accuracy and engineering operability, and are suitable for dynamic obstacle monitoring scenarios with high real-time requirements.

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

[0179] Based on any of the above technical solutions, further optimization is: calculating the shortest distance between the current node and the obstacle grid, and marking it as a near-obstacle node when it is ≤20mm.

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

[0181] Generate 6 basic extension directions corresponding to the positive and negative rotation directions of the 6-axis robot arm.

[0182] If a concave area or hole is identified, the deflection vector pointing to its center is calculated, and the vector is synthesized with the basic direction to form the biased expansion direction.

[0183] The normal vector of the obstacle surface point cloud is calculated, and the directions with an angle of less than 30° with the offset extension direction are eliminated, and the directions with an angle of ≥30° are retained.

[0184] Perform dynamic constraint verification on the joint angle changes in the remaining directions, and filter and retain the nodes that meet the constraints.

[0185] The filtered nodes are aggregated to form a candidate node set, each of which contains information labels such as joint rotation parameters, distance to obstacles, and offset extension direction.

[0186] Among them, the specific steps of calculating the normal vector through the 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.

[0187] Construct a covariance matrix: Construct the three-dimensional coordinate data of the neighborhood points into a 3×N matrix (N is the number of neighborhood points) and calculate the covariance matrix C of the matrix.

[0188] The covariance matrix reflects the degree of dispersion and correlation of neighborhood points in the directions of the three coordinate axes.

[0189] Eigenvalue decomposition: Perform eigenvalue decomposition on the covariance matrix C to obtain three eigenvalues ​​λ1 ≥ λ2 ≥ λ3 and the 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.

[0190] Determine the normal vector: Since the normal vector direction is the direction with the least change in the point cloud data, the eigenvector V3 corresponding to the minimum eigenvalue λ3 is selected as the normal vector for the target point. To ensure the consistency of the normal vector direction (for example, uniformly pointing outward), the reference direction can be set according to the scene requirements to adjust the normal vector direction.

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

[0192] Based on any of the above technical solutions, further optimization is as follows: wherein, during the dynamic constraint verification, the following conditions are met: the rotation angle does not exceed the mechanical limit, the angular velocity ≤ 50° / s, and the angular acceleration ≤ 100° / s².

[0193] Angle limit check: The current angle value of each joint of the robot 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 this range, it is judged as infeasible.

[0194] Angular velocity constraint verification: Calculate the change in joint angle Δθ of the candidate node relative to the previous node. Combined with the path planning time interval Δt (e.g., 100ms), the joint angular velocity ω is calculated. If ω > 50° / s, the robot exceeds its dynamic capabilities and the node is eliminated.

[0195] Angular acceleration constraint check: If there are multiple consecutive candidate nodes, calculate the angular velocity change Δω between adjacent nodes and combine it with the time interval Δt to obtain the angular acceleration α. ​​If α > 100° / s², the robot arm drive system exceeds its capabilities and the node is removed.

[0196] Based on any of the above technical solutions, further optimization is as follows: in step 4, when a collision is detected, the collision path is corrected using a third-order B-spline smoothing method according to the movement trend of the dynamic obstacle, and the parameters are iteratively optimized to ensure that the distance to the obstacle is ≥15mm. The specific steps are as follows.

[0197] Collision interval positioning: Determine the collision path segment through 1mm³ voxel grid detection and extract the collision starting point P s , collision end point P e , and 6 control points consisting of 2 adjacent path points before and after the collision interval, constitute the local corrected path node set; Determine the direction of movement based on the speed and acceleration of the obstacle.

[0198] Construct a 3rd-order B-spline initial curve with 6 control points.

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

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

[0201] 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. The maximum number of iterations is 30.

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

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

[0204] Collision path repair: For the collision path segments detected by the voxel grid, local curve correction is used to eliminate collision risks and ensure that the robot arm's motion trajectory maintains a safe distance from obstacles.

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

[0206] The above method can achieve: Instant collision elimination: Through voxel grid positioning and curve correction, spatial overlap with obstacles in the path is directly eliminated, ensuring that the robot arm moves along a collision-free trajectory.

[0207] Guaranteeing smooth motion: The second-order continuity of the third-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.

[0208] Complex scene generalization capability: Applicable to multiple types of obstacles such as flat, vertical, and oblique obstacles, as well as complex scenes such as dynamic obstacles moving laterally and accelerating approaching (such as Figure 1-Figure 5 The 5 groups of control point parameters in the figure cover typical working conditions).

[0209] Based on any of the above technical solutions, further optimization is that when fine-tuning the control point in the opposite direction of the obstacle movement trend, the adjustment distance each time is ≤5mm.

[0210] The following are examples of 6 control point parameters for 5 sets of robot arm joint space path correction scenarios (unit: mm, based on the X / Y / Z coordinates of the Cartesian coordinate system, simulating the path node distribution in a typical workspace): 1 The second point before the collision interval; P 2 is the first point before the collision interval; P s is the collision starting point; P e is the collision end point; P 5 is the first point after the collision interval; P 6 It is the second point after the collision interval.

[0211] 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 Figure 1 .

[0212] 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 Figure 2 .

[0213] 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 Figure 3 .

[0214] Group 4 (narrow passage scene): 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 Figure 4 .

[0215] Group 5 (Dynamic obstacle lateral movement scene): 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 Figure 5 .

[0216] From the above scenario, we can see that: The Z coordinates of Group 1 (planar obstacle avoidance) and Group 5 (lateral movement of dynamic obstacles) remain unchanged (300mm and 350mm), reflecting the in-plane obstacle avoidance feature and are suitable for scenarios where obstacles are distributed along the horizontal direction. The Z coordinate of the second group (vertical obstacle avoidance) increases from 150mm to 210mm, and the Y coordinate increases synchronously, reflecting the need to raise the height in the vertical direction to avoid obstacles; The X, Y, and Z coordinates of the third group (oblique obstacles) all show an increasing trend (for example, X from 500mm to 590mm, Z from 200mm to 260mm), which is suitable for scenarios where obstacles are distributed along an oblique direction. The X, Y, and Z coordinates of the fourth group (narrow passages) all increase slightly (e.g., X increases by 5mm and 10mm per step), reflecting the characteristic of small adjustments for obstacle avoidance in narrow spaces.

[0217] The above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the above embodiments, those skilled in the art should understand that the technical solutions described in the above embodiments can still be modified, or some or all of the technical features therein can be replaced by equivalents. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the scope of the technical solutions of the embodiments of the present invention. For those skilled in the art, any replacement improvements or changes made to the implementation methods of the present invention fall within the scope of protection of the present invention.

[0218] Any matters not described in detail in the present invention are well-known technologies to those skilled in the art.

Claims

1. A multi-degree-of-freedom robotic arm obstacle avoidance path planning method based on three-dimensional reconstruction, characterized in that: The following steps are involved: Step 1: 3D environment perception and dynamic modeling: The laser radar and depth camera are used to synchronously collect data through hardware triggering. The data is then registered based on a checkerboard calibration plate to ensure that the error between the point cloud and the image is ≤5mm. The fused point cloud is then subjected to statistical filtering to remove noise, and a 3D mesh model is generated using a 3D mesh modeling method. Using the inter-frame difference method, points with a Euclidean distance greater than 5 mm between two consecutive point cloud frames are marked as dynamic points. Connected areas with a dynamic point ratio greater than 5% are considered dynamic obstacles. The model is updated every 100 ms. Step 2: Quantitative extraction of obstacle features: Calculate the Gaussian curvature of the surface of static obstacles and set the curvature to > 0.01mm -1 Set it as a convex area and set the curvature to <-0.01mm -1 The concave area is set and the minimum circumscribed circle method is used to measure the hole size. The motion parameters of dynamic obstacles are calculated based on the Kalman filter, and the velocity vector and acceleration are output. Step 3: When expanding new nodes in the joint space, the nodes near obstacles are preferentially expanded toward the concave area or the center of the hole to generate a set of candidate nodes; Step 4: 3D mesh collision check and safe path correction: The robot's workspace is divided into a 1mm³ 3D voxel grid. Collision detection is performed by determining whether the path nodes fall within the obstacle voxels.

2. The method for obstacle avoidance path planning of a multi-degree-of-freedom manipulator based on three-dimensional reconstruction according to claim 1, characterized in that: The specific steps for synchronously collecting environmental data by the lidar and depth camera in step 1 are performed in the following 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. The input end of the signal distributor is connected to the GPIO output pin of the same controller. Signal generation: The controller generates a 3.3V level trigger signal with a pulse width of 10μs at a period of 100ms. This signal is transmitted to the lidar and depth camera simultaneously through the signal distributor. Data acquisition: Each time the LiDAR receives a trigger signal, it outputs one frame of point cloud data at a 10Hz scanning frequency; each time the depth camera receives three trigger signals cumulatively, it outputs one frame of RGB-D image at a 30Hz frame rate; Timestamp recording: The controller uses a built-in real-time clock to timestamp the laser point cloud and depth image respectively; Synchronous calibration: Calculate the difference between the timestamps of the two types of data. If it is ≤10ms, maintain 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 for obstacle avoidance path planning of a multi-degree-of-freedom manipulator based on three-dimensional reconstruction according to claim 2, characterized in that: The specific methods for point cloud and image registration, fused point cloud denoising, and 3D mesh modeling based on the checkerboard calibration plate include: Calibration preparation: Use a 10×8 checkerboard calibration plate and fix it in the working area of ​​the robotic arm. Ensure that the fields of view of the lidar and depth camera can completely cover the calibration plate, and that the angle between the calibration plate plane and the optical axis of the two devices is 45°±5°. Data acquisition: Control the LiDAR to collect three sets of calibration plate point cloud data at a frequency of 10 Hz, and the depth camera to collect three sets of depth images including the calibration plate at a frame rate of 30 Hz. Each set of data acquisition takes 1 second to ensure that the corners are clear and free of motion blur. Corner point extraction: extract the 3D coordinates of the corner points of the calibration plate, extract the pixel coordinates of the corner points of the depth image and convert them into 3D coordinates. The corner point positioning error is ≤ 1 pixel. Registration calculation: Solve the transformation matrices of three groups of corresponding corner points, calculate the average reprojection error of each group 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 group of corner points is ≤3mm; Fusion verification: Fuse the laser point cloud with the depth image data, randomly select 50 scene feature points in the non-calibration plate area, and calculate the spatial distance between the fused point cloud and the original data. The error of all points is ≤5mm; Statistical filtering denoising: Using the statistical outlier filtering algorithm, the number of neighbor point searches is set to 30, the average distance μ and standard deviation σ between each point and its neighbor points are calculated, and outliers with a distance greater than 1.5σ are removed. After processing, the point cloud retention rate is ≥95%, and the position deviation of feature points before and after filtering is ≤1mm; 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 the initial 3D mesh model.

4. The method for obstacle avoidance path planning of a multi-degree-of-freedom manipulator based on three-dimensional reconstruction according to claim 3, characterized in that: The specific steps of marking the points with Euclidean distance of more than 5 mm between the corresponding points of two consecutive point cloud frames as dynamic points by using the inter-frame difference method are as follows: Point cloud frame acquisition: two consecutive frames of point cloud data are collected at a set time interval, recorded as frame t and frame t+1, ensuring that the two frames of point cloud are in the same coordinate system; 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 as the corresponding point of point P; 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, point P is marked as a dynamic point.

5. The method for obstacle avoidance path planning of a multi-degree-of-freedom manipulator based on three-dimensional reconstruction according to claim 4, characterized in that: The specific steps for calculating motion parameters of dynamic obstacles based on Kalman filtering are: 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: Based on the state vector at the current time k and the motion model, predict the state vector at time k+1; State correction: Multiply the Kalman gain by the observation residual to obtain the state correction; add this correction to the predicted state vector at time k+1, and the result is the optimal state estimate at time k+1; Parameter output: Extract the velocity vector and acceleration from the corrected state vector as the motion parameter output of the dynamic obstacle. Repeat steps 2-5 every 100ms to achieve real-time updates.

6. The method for obstacle avoidance path planning of a multi-degree-of-freedom manipulator based on three-dimensional reconstruction according to claim 5, characterized in that: Calculate the shortest distance between the current node and the obstacle grid. If the distance is ≤20mm, mark it as a near-obstacle node. Based on the results of step 2, extract the concave area and holes 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; If a concave area or hole is identified, the deflection vector pointing to its center is calculated and synthesized with the basic direction to form the biased expansion direction; By computing the normal vector of the obstacle surface point cloud, the directions with an angle less than 30° between the offset extension direction and the obstacle surface are eliminated, and the directions with an angle greater than or equal to 30° are retained. Perform dynamic constraint verification on the joint angle changes in the remaining directions, and select and retain the nodes that meet the constraints; The filtered nodes are aggregated to form a candidate node set, each of which contains information labels such as joint rotation parameters, distance to obstacles, and offset extension direction.

7. The method for obstacle avoidance path planning of a multi-degree-of-freedom manipulator based on three-dimensional reconstruction according to claim 6, characterized in that: in, Dynamic constraint verification meets the following requirements: rotation angle does not exceed mechanical limit, angular velocity ≤ 50° / s, angular acceleration ≤ 100° / s².

8. The method for obstacle avoidance path planning of a multi-degree-of-freedom manipulator based on three-dimensional reconstruction according to claim 7, characterized in that: In step 4, when a collision is detected, the collision path is corrected using a 3rd-order B-spline smoothing algorithm based on the dynamic obstacle's motion trend. The parameters are iteratively optimized to ensure that the distance to the obstacle is ≥15 mm. The specific steps are as follows: Collision interval positioning: Determine the collision path segment through 1mm³ voxel grid detection and extract the collision starting point P s , collision end point P e , and 6 control points consisting of 2 adjacent path points before and after the collision interval, constitute the local corrected path node set; Determine the direction of movement based on the speed and acceleration of the obstacle; Construct a 3rd 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 15mm, start optimization. Fine-tune the control point in the opposite direction of the obstacle's movement, regenerate the curve, and detect the distance until it is ≥15mm; Confirm that the joint angular velocity and angular acceleration corresponding to the regenerated correction curve do not exceed the limit, and obtain a curve that meets the requirements; The obtained curve that meets the requirements is used as the local correction segment to replace the original collision path.

9. The method for obstacle avoidance path planning of a multi-degree-of-freedom robotic arm based on three-dimensional reconstruction according to claim 8, characterized in that: When fine-tuning the control point in the opposite direction of the obstacle's movement, the adjustment distance each time should be ≤5mm.

Images