Control method and system for mechanical arm of distribution network line operation platform

Abstract

The invention belongs to the technical field of distribution network line operation, and provides a distribution network line operation platform mechanical arm control method and system, and the method comprises the steps: determining a cable, a cable clamp obstacle and an operation platform through image recognition according to a cable cluster, a cable clamp cluster, a mechanical arm cluster and an obstacle cluster; according to the method, extraction and identification of a cable, a cable clamp, a mechanical arm and the like are realized, and on the basis, the center line of the identified cable is extracted through principal component analysis; outward expansion is conducted with the center line as the reference, and a safety forbidden zone of the mechanical arm is obtained; according to the safety forbidden zone, a preset path planning strategy and a trajectory tracking strategy, the mechanical arm is controlled; when the mechanical arm is controlled, the expected position of the tail end is compensated, so that the actual position of the tail end of the mechanical arm in a world coordinate system is attached to the expected position of the tail end, the influence of shaking of a working platform is offset, the problem of position change caused by external force influence such as cable shaking in the control process is solved, and effective obstacle detection is achieved; and the safety is improved.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of distribution network line operation, and particularly relates to a distribution network line operation platform mechanical arm control method and system. BACKGROUND

[0002] The distribution network line operation platform comprising a walking mechanism and a mechanical arm can realize clamping and wiring of cables through the mechanical arm.

[0003] The distribution network operation environment has a high degree of unstructured characteristics, such as a complex spatial relationship between phase cables, cable sag changes with working conditions, and the like. These dynamic uncertain factors bring severe challenges to stable operation of the mechanical arm in the operation platform. The traditional mechanical arm control adopts a static environment modeling and absolute coordinate calibration scheme, which is difficult to cope with the problem of base platform position changes caused by cable shaking and other external force influences when the mechanical arm is deployed for operation on overhead lines. In addition, a single sensor detection scheme cannot effectively detect obstacles and non-operation cables, which brings great hidden dangers to operation safety. SUMMARY

[0004] To solve the above problems, the application provides a distribution network line operation platform mechanical arm control method and system. The application realizes extraction and identification of cables, clamps and mechanical arms, and on this basis, extracts a cable center line through principal component analysis for the identified cable; expands outward based on the center line to obtain a safety prohibited area of the mechanical arm; controls the mechanical arm according to the safety prohibited area and preset path planning and trajectory tracking strategies; and compensates the end desired position when controlling the mechanical arm, so that the actual position of the mechanical arm end in the world coordinate system fits the end desired position, offsets the influence of the operation platform shaking, solves the problem of position changes caused by cable shaking and other external force influences in the control process, realizes effective detection of obstacles, and improves safety.

[0005] To achieve the above purpose, the application is implemented through the following technical scheme: In a first aspect, the application provides a distribution network line operation platform mechanical arm control method, comprising: Obtaining relevant laser data and image data in an operation area; Fusing and processing the laser data and the image data; performing point cloud segmentation and feature extraction on the fused and processed laser data and image data to obtain cable clusters, clamp clusters, mechanical arm clusters and obstacle clusters; Determining cables, clamps, obstacles and operation platforms through image recognition according to the cable clusters, the clamp clusters, the mechanical arm clusters and the obstacle clusters; Extracting a cable center line through principal component analysis for the identified cable; expanding outward based on the center line to obtain a safety prohibited area of the mechanical arm; According to the safety forbidden zone, and the preset path planning strategy and trajectory tracking strategy, the mechanical arm is controlled; wherein, when the mechanical arm is controlled, the end desired position is compensated, so that the actual position of the mechanical arm end in the world coordinate system is consistent with the end desired position, and the influence of the work platform shaking is offset.

[0006] Further, when acquiring the relevant laser data and image data in the work area, the laser radar and the camera acquiring the laser data and the image data are offline coordinate calibrated: the calibration board is placed in the field of view of the laser radar and the camera at the same time, the radar obtains the point cloud of the calibration board plane, the camera identifies the corner points of the calibration board, the internal parameters of the camera are calibrated through Zhang's calibration method, the distortion coefficient of the camera image is obtained and the internal parameters are optimized; in the external parameter calibration, the corresponding relationship of the observation data of the same object by the laser radar and the camera is found, the optimal rotation matrix and translation vector are found, so that the re-projection error between the laser radar point cloud projected on the image plane and the corresponding feature points in the camera image is minimized.

[0007] Further, each three-dimensional point cloud coordinate of the laser radar is transformed into the camera coordinate system, and then projected onto the two-dimensional image through the camera internal parameter.

[0008] Further, the laser data and the image data are sequentially subjected to straight-through filtering, statistical outlier removal and voxel grid filtering: the straight-through filtering removes the point cloud data in the region not of interest through the given filtering field and value range, reduces the data size, and retains the cylindrical region with the longest extension distance of the mechanical arm as the center and a preset safety distance; the statistical outlier removal removes the points whose average distance from other points exceeds the preset multiple standard deviation range of the mean value; the voxel grid filtering divides the three-dimensional space into uniform voxels.

[0009] Further, the point cloud segmentation and feature extraction include: according to the coordinate range converted from the geometric parameters of the work platform, the point cloud data is removed; according to the joint angle information of the mechanical arm and the geometric information of the end effector, the real-time pose of each connecting rod in the radar coordinate system is determined one by one, and the axis-aligned bounding box of each connecting rod is generated; all point clouds are traversed, and the points falling within any connecting rod bounding box are classified as mechanical arm candidate clusters; For the remaining point cloud, use K-D tree search to classify the points with a spatial distance not greater than a first preset distance into a pre-grouped cluster; randomly extract multiple points with an x-axis span greater than a preset span in a single pre-grouped cluster, select a straight line model or a quadratic curve model, and solve the model parameters; obtain the perpendicular distance of all segments to the model, and count the inner points with a distance less than a second preset distance; according to this method, multiple iterations are performed, the model with the most inner points is selected, and the inner points are classified as cable candidate clusters; subsequently, the cable model is fitted in the remaining point cloud cluster grouping to obtain the remaining cable candidate clusters; Based on the segmented cable clusters and their fitted trajectories, multiple clamp detection segments are defined. Centered on the cable trajectory, these segments extend in a direction perpendicular to the cable, forming cylindrical candidate regions. Combining the candidate point cloud of clamps after removing candidate point cloud clusters of robotic arms and cables, a KD tree is used to determine each point. Point clouds with density features matching the clamps are selected to obtain candidate clamp points. Euclidean clustering is performed on the candidate points to obtain candidate clamp clusters. Euclidean distance clustering is then performed on the remaining point clouds to initially classify them into obstacle clusters.

[0010] Furthermore, the determination of the safety restricted area includes: determining the extension direction and cross-sectional plane of the working phase cable through principal component analysis; assuming the point cloud of the non-working phase cable is... The radar coordinates of each point are: Move the center of the point cloud to the origin of the coordinate system:

[0011] in, The first in the point cloud of non-operating phase cables Radar coordinates of the original point; This represents the total number of points contained in the point cloud of the non-operating phase cable. Let this be the geometric center of the point cloud of the non-operating phase cable.

[0012] At this time, the centralized point cloud The center is the origin; calculate the covariance matrix C (3×3): ; Using the eigenvalues ​​of the covariance matrix C ( ) and eigenvectors ( Describes the magnitude and direction of the variance of the principal components; largest eigenvalue. corresponding feature vector This refers to the direction of cable extension, and the eigenvectors corresponding to other eigenvalues ​​together constitute the cross-sectional plane of the conductor; The cable centerline is the projection center of the point cloud on a cross section perpendicular to the extension direction. The projection length is obtained by projecting the center point onto the extension direction. The projection length is arranged by size and averaged by interval to obtain discrete points of the centerline in the original radar coordinate system. The discrete points are curve fitted to obtain a smooth centerline trajectory. Using the centerline extracted by principal component analysis as the axis and the preset distribution network safety regulations as the radius, a three-dimensional tubular model is generated as the safety no-go zone for the movement of the robotic arm.

[0013] Further, the safety forbidden area of the non-working phase line is represented by a geometric envelope body, the mechanical arm model and the safety forbidden area are discretized into a series of capsule bodies distributed along the center line of the conductor, each link of the mechanical arm is enveloped by the capsule bodies to obtain hierarchical bounding boxes, a larger hierarchical bounding box capable of completely enclosing all the hierarchical bounding boxes is constructed for the mechanical arm and the safety forbidden area, and the larger hierarchical bounding box is regarded as the root node of the recursive level in the collision detection; all objects are sorted according to the coordinate values of the centroids on the segmentation axis; the sorted list is divided into two from the middle, the front half is attributed to the left child node, and the back half is attributed to the right child node; the binary tree is constructed through the recursive loop, and each leaf node only contains one hierarchical bounding box; the distance between the leaf node pairs intersected by the hierarchical bounding boxes is ensured to be greater than the safety threshold value, so that the mechanical arm is ensured to be outside the safety forbidden area; during the movement of the mechanical arm, the safety forbidden area is regarded as a potential field generating a repulsive force, and the distance between the leaf node pairs intersected by the hierarchical bounding boxes is inversely proportional to the size of the repulsive force; when the distance is less than the set safety threshold value, the repulsive force is converted into a speed instruction of the mechanical arm to drive the mechanical arm away from the safety forbidden area.

[0014] Further, the path planning strategy and the trajectory tracking strategy include: a sampling planning algorithm is used to find a collision-free path from the starting point to the target point in the configuration space of the mechanical arm; and a smooth trajectory executed by the mechanical arm is generated by assigning time stamps, speed and acceleration information to each point through polynomial interpolation.

[0015] Further, the compensation for the end position includes: the end position is compensated by a 4*4 homogeneous transformation matrix The pose of the base coordinate system B relative to the world coordinate system W is described as: ; wherein, represents a rotation matrix of the base coordinate system relative to the world coordinate system; represents a translation vector of the base coordinate system origin relative to the world coordinate system origin; the end position is compensated as: ; The compensated end position is substituted into the inverse kinematics of the mechanical arm, the joint expected posture is updated, the torque controller is calculated in real time to track, and the actual position of the mechanical arm end in the world coordinate system is fitted to the end position , so as to offset the influence of the platform shaking.

[0016] In a second aspect, the application further provides a distribution network line operation platform mechanical arm control system, comprising: a data acquisition module configured to acquire relevant laser data and image data in an operation area; The fusion module is configured to fuse and process the laser data and the image data, and to perform point cloud segmentation and feature extraction on the fused and processed laser data and image data to obtain a cable cluster, a clamp cluster, a mechanical arm cluster and an obstacle cluster. The recognition module is configured to determine cables, clamps, obstacles and a work platform through image recognition according to the cable cluster, the clamp cluster, the mechanical arm cluster and the obstacle cluster. The safety forbidden area determination module is configured to extract a cable center line through principal component analysis for the recognized cable, and to obtain a safety forbidden area of the mechanical arm by extending outward based on the center line. The control module is configured to control the mechanical arm according to the safety forbidden area, a preset path planning strategy and a trajectory tracking strategy, and to compensate an end desired position when the mechanical arm is controlled, so that an actual position of the mechanical arm end in a world coordinate system is fitted to the end desired position, and the influence of the work platform shaking is offset.

[0017] In a third aspect, the present application further provides a computer readable storage medium, which stores a computer program, and the program is executed by a processor to implement the steps of the method for controlling a mechanical arm of a distribution line work platform according to the first aspect.

[0018] In a fourth aspect, the present application further provides an electronic device, which comprises a memory, a processor and a computer program stored in the memory and capable of running on the processor, and the processor implements the steps of the method for controlling a mechanical arm of a distribution line work platform according to the first aspect when executing the program.

[0019] In a fifth aspect, the present application further provides a computer program product, which comprises a computer program, and the computer program is executed by a processor to implement the steps of the method for controlling a mechanical arm of a distribution line work platform according to the first aspect.

[0020] Compared with the prior art, the present application has the following beneficial effects: 1、The cable cluster, the clamp cluster, the mechanical arm cluster and the obstacle cluster are obtained by fusing and processing laser data and image data in the application; the cable, the clamp obstacle and the operation platform are determined through image recognition according to the cable cluster, the clamp cluster, the mechanical arm cluster and the obstacle cluster; the extraction and recognition of the cable, the clamp and the mechanical arm are realized, on the basis of which, the center line of the cable is extracted through principal component analysis; the safety forbidden area of the mechanical arm is obtained by extending outward with the center line as a reference; the mechanical arm is controlled according to the safety forbidden area and preset path planning strategies and trajectory tracking strategies; when the mechanical arm is controlled, the end desired position is compensated, so that the actual position of the mechanical arm end in the world coordinate system is consistent with the end desired position, the influence of the operation platform shaking is offset, the position change problem caused by the influence of external force such as cable shaking in the control process is solved, the obstacle effective detection is realized, and the safety is improved. BRIEF DESCRIPTION OF DRAWINGS

[0021] The accompanying drawings, which form a part of this specification, are included to provide a further understanding of the embodiments of the application and are incorporated in and constitute a part of this specification. The embodiments of these drawings are shown by way of illustration of the embodiments of the application, and therefore, are not to be considered as limiting the embodiments of the application.

[0022] Figure 1 It is a schematic diagram of the network wiring line operation platform of the embodiment 1 of the application. Figure 2 It is a method flowchart of the embodiment 1 of the application. Wherein, 1, platform main body;2, mechanical arm;3, mechanical claw. DETAILED DESCRIPTION

[0023] The application will be further described below in conjunction with the drawings and embodiments.

[0024] It should be noted that the following detailed description is exemplary and is intended to provide further explanation of the present application. Unless otherwise specified, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which the present application belongs.

[0025] Embodiment 1: As Figure 1As shown, one of the network wiring line operation platforms includes a platform body 1, a mechanical arm 2 arranged on the platform body 1, and a mechanical gripper 3 arranged on the mechanical arm 2; the mechanical gripper 3 can adopt a wire shearing and clamping mechanism and the like. The network wiring line operation platform including the mechanical arm needs to complete fine operation tasks in a high-voltage, strong electric field and unstructured overhead line environment, and the premise is to construct a mechanical arm platform operation space in a dynamic environment in real time, and to ensure the safety of the mechanical arm operation. The overhead line is not an ideal static parallel line, and when the mechanical arm and its tool are hung on the cable, the additional load will cause the current cable sag to change, thereby changing the relative position and distance of the cable to the adjacent live cable; if no active anti-collision means is adopted, only passive sensing obstacle avoidance is relied on, the sensing obstacle avoidance will be triggered constantly in the operation process, affecting the operation efficiency; even if the sensing obstacle avoidance detects an obstacle to stop, the distance to the cable may be further approached due to the change of the environment, increasing the operation risk.

[0026] To solve at least one of the above problems, the embodiment provides a network wiring line operation platform mechanical arm control method, which constructs a real-time dynamic environment model that can accurately represent the geometric structure and understand the physical semantics, and provides an information basis for subsequent safety planning; a sensing scheme in which a laser radar and an RGB camera work cooperatively is adopted, the laser radar is installed at an upper position on the right end of the operation platform, 360° dead angle-free detection is realized in the vertical direction, and accurate three-dimensional geometric information that is not affected by light is provided; the camera is fixed in parallel above the radar to supplement the texture color and resolution of the laser radar detection result. The relative position is fixed through rigid connection, so that the point cloud of the laser radar can be accurately projected on the camera image in the subsequent process, and data association and fusion are realized. As shown in the figure, Figure 2 The method includes the following steps: S1, offline coordinate calibration: A plurality of sensors such as laser radars and cameras collect the working area from different directions to obtain image and depth information of the current environment. In order to fuse a plurality of sensing data, a unified coordinate language needs to be established through offline one-time calibration. The mathematical transformation relationship between the laser radar coordinate system, the camera coordinate system and the final mechanical arm base coordinate system is determined. This step includes camera intrinsic calibration and multi-sensor extrinsic calibration, and is the cornerstone of all data fusion and space calculation. Finally, the following relationship is obtained: ; Wherein, s is a scale factor; represents a pixel coordinate; represents a point in the calibration plate coordinate system coordinate (Z=0); K is an intrinsic matrix, R and t are extrinsic parameters.

[0027] The calibration board is placed in the field of view of both the radar and the camera, the radar obtains the point cloud of the calibration board plane, and the camera identifies the corner points of the calibration board. The camera's internal parameters are calibrated by Zhang's calibration method, the distortion coefficients of the camera image are obtained, and the internal parameters are optimized, so that the camera picture can truly reflect the working environment. The principle is to use the pinhole imaging model of the camera. By shooting different direction checkerboard calibration board, using the known world coordinates of the checkerboard corner points and their pixel coordinates in the image, the internal parameter matrix of the camera (including focal length, principal point coordinates) and distortion coefficient are solved. This process is essentially solving a linear equation set, minimizing the re-projection error.

[0028] Remember , H is a 3x3 homogeneous matrix, by substituting the pixel coordinates and world coordinates of multiple calibration board corner points on the same picture, the matrix H corresponding to the picture can be obtained. By letting , the internal parameter constraint can be converted into a linear equation set about the symmetric matrix B. Each picture can provide two equations, at least 3 different attitude calibration board pictures are needed, and B can be linearly solved. Then the internal parameter matrix K is decomposed from B by Cholesky decomposition method.

[0029] The principle of external parameter calibration is to find the corresponding relationship of the observation data of the same object by two sensors, and the essence is to solve a nonlinear optimization problem, to find the optimal rotation matrix R and translation vector T, so that the re-projection error between the laser radar point cloud projected to the image plane and the corresponding feature points in the camera image is minimized.

[0030] Once the internal parameter matrix K is known, the external parameter can be solved by the following formula: ; Where λ is a scaling factor.

[0031] The parameters obtained by the above steps are the preliminary solution. Next, all parameters (internal parameters, distortion coefficients, and external parameters of each picture) are substituted into a nonlinear optimization problem to minimize the re-projection error. The re-projection error is the difference between the pixel coordinates observed by the calibration board corner points and the pixel coordinates projected back by the camera model according to the current parameter estimation. The optimization goal is to minimize the sum of squares of all point re-projection errors.

[0032] ; Where, is the "actual detected 2D pixel coordinates" of the th 3D space point in the th image; is the re-projection function, which projects the 3D space point through the camera internal parameters, external parameters, distortion coefficients, etc. to get the "predicted 2D pixel coordinates" on the 2D image. is the camera intrinsic matrix; is the camera distortion coefficient; , is the extrinsic parameter, representing the rotation matrix and translation vector respectively; is the coordinate of the th 3D space point.

[0033] The Levenberg-Marquardt (LM) algorithm is used to iteratively optimize this process.

[0034] S2, Real-time online processing: S2.1, Data synchronization and fusion: Using the calibrated extrinsic matrix, each three-dimensional point cloud coordinate of the lidar is transformed into the camera coordinate system, and then projected onto the two-dimensional image through the camera intrinsic parameter. In this way, each three-dimensional point finds its corresponding pixel in the two-dimensional image, thereby obtaining the color and texture information of the point.

[0035] S2.2, Point cloud preprocessing: The original point cloud data includes invalid information such as noise. After processing by noise reduction and filtering algorithms, a clean data set is obtained. In order to balance data quality and computational efficiency, straight-through filtering, statistical outlier removal, and voxel grid filtering are performed in sequence.

[0036] Straight-through filtering removes point cloud data in the region of interest by given filtering fields and value ranges, reducing data size. For overhead line operation scenarios, the cylindrical region with the longest mechanical arm extension distance plus a certain safety distance as the center is retained.

[0037] The principle of statistical outlier removal is based on the assumption of data distribution. Effective point cloud data is dense, while noise points are sparse and far from the main body. Points whose average distance from other points exceeds ±2 times the standard deviation of the mean are considered outliers and are removed. This can effectively remove obvious, sparse outliers, significantly improving data quality.

[0038] Voxel grid filtering reduces data size to improve the efficiency of subsequent processing algorithms while preserving the overall geometry of the scene as much as possible. The three-dimensional space is divided into uniform microcubes (voxels). All points within each voxel are represented by a central point. This can significantly reduce the number of points while preserving the overall geometry of the scene, improving the computational efficiency of subsequent algorithms.

[0039] S2.3, Point cloud segmentation and feature extraction: For unstructured environments such as cables, clamps, obstacles, and work platforms in live-line work of power distribution networks, the core logic is to prioritize strong geometric features, assist with density differences, and eliminate interference before segmenting targets to complete the segmentation of cable clusters, clamp clusters, robotic arm clusters, and obstacle clusters.

[0040] Point cloud cluster segmentation for the work platform and robotic arm relies on known platform structural information and kinematic calculations. The geometric parameters of the work platform are converted into coordinate ranges and removed from the point cloud data. Based on the robotic arm joint angle information obtained from the controller and the geometric information of the end effector, the real-time pose of each link in the radar coordinate system is calculated one by one, generating an axis-aligned bounding box (AABB) for each link. All point clouds are traversed, and points falling within any link's bounding box are grouped into candidate robotic arm clusters.

[0041] The cable's geometric characteristics are the clearest continuous strip shape, segmented using range clipping + RANSAC fitting + density filtering. For the remaining point cloud, a KD-tree search is used to group points with a spatial distance ≤ 0.1m into a pre-grouped cluster. Within each pre-grouped cluster, three points with an x-axis span greater than 0.3m are randomly selected, based on... z selects either a linear model or a quadratic curve model and solves for the model parameters. Further, it calculates the vertical distance from all segments to the model and counts interior points with a distance less than 0.008m. This method is iterated multiple times, selecting the model with the most interior points, and its interior points are grouped into the cable candidate cluster. Subsequently, cable models are fitted sequentially into the remaining point cloud clusters to obtain the remaining cable candidate clusters.

[0042] As rigid targets attached to cables, cable clamps are segmented using a core logic that first locks down the area surrounding the cable, then filters using high-density, small-volume features. Based on the segmented cable clusters and their fitted trajectories, multiple cable clamp detection segments are defined, extending perpendicularly to the cable trajectory to form cylindrical candidate regions. Combining the candidate point cloud of cable clamps after removing candidate point cloud clusters for robotic arms and cables, a KD-tree is used to calculate the number of points within a 0.02m radius neighborhood for each point. Point clouds with density features matching those of cable clamps (cable clamp density feature: 3000 points / cubic meter corresponds to ≥6 points in a 0.02m neighborhood) are selected, resulting in high-density candidate points for cable clamps. Euclidean clustering is performed on these high-density candidate points to obtain candidate clusters of cable clamps. Further validation of these candidate clusters is based on the geometric features of the cable clamps, resulting in the output of the cable clamp clusters. The remaining point cloud is then subjected to Euclidean distance clustering, initially classifying it into obstacle clusters.

[0043] S2.4, Semantic Recognition: For each segmented point cloud cluster, it is also necessary to assign it a processable identity information so that the system can distinguish whether each cluster is a live wire (operating phase cable) or an obstacle.

[0044] The embodiment adopts a vision-based method, classifies and recognizes a two-dimensional image collected by a camera through a YOLO image recognition model, recognizes four types of targets in the image, i.e., cables, clamps, obstacles and work platforms, and assigns the recognition result to a three-dimensional point cloud cluster corresponding to a two-dimensional pixel point.

[0045] S3, dynamic protection area construction modeling: For the complex scene of overhead cable operation, the safe operation space of the mechanical arm is not fixed, and in the reachable space, there can be other cables, branches and other objects, which can change their spatial positions under the influence of external forces. Therefore, the dynamic changes of the environment are monitored in real time through the laser radar and the camera, so as to update the safe space of the mechanical arm operation in real time.

[0046] For each identified live wire, the center line thereof is extracted through principal component analysis (PCA). According to the safety regulations (such as 0.7 meters), a three-dimensional and tubular safety exclusion zone model is generated by expanding outward based on the center line. This model can be understood as a safety sleeve wrapped around the wire.

[0047] The wire point cloud is in an elongated linear distribution, and the data has the maximum variance in the extension direction and the minimum variance in the perpendicular direction. The principal component analysis finds the extension direction and the cross-sectional plane through mathematical calculation. Let the point cloud of the non-operation phase cable be , and the radar coordinates of each point are . The center of the point cloud is moved to the coordinate origin through the following formula, so that the PCA focuses on the distribution form of the point cloud.

[0048]

[0049] , wherein is the radar coordinate of the th original point in the non-operation phase cable point cloud; is the total number of points contained in the non-operation phase cable point cloud; is the geometric center of the non-operation phase cable point cloud.

[0050] At this time, the center of the centralized point cloud is the coordinate origin. Further, the covariance matrix C(3x3) is calculated through the following formula: ; The eigenvalues ( ) and eigenvectors ( ) of the covariance matrix C can describe the variance size and direction of the principal component.

[0051] The maximum eigenvalue corresponding to the eigenvector is the extension direction of the cable, and the corresponding are two orthogonal direction vectors perpendicular to , which together constitute the cross-sectional plane of the cable.

[0052] The cable centerline is the projection center of the cable point cloud on the cross section perpendicular to the extension direction, the centralization point is projected onto the extension direction , and the projection length is obtained. The projection length is arranged in size and the discrete points of the centerline in the original radar coordinate system are obtained by interval averaging : ; wherein, is the average value of all in the kth group. Curve fitting is performed on the discrete points to obtain a smooth centerline trajectory. A three-dimensional tubular model is generated with the centerline extracted by PCA as the axis and the safety regulation (0.7m) as the radius, which is regarded as the safety forbidden zone for the motion of the mechanical arm.

[0053] For the safety forbidden zone of the non-operation phase line, a hierarchical and efficient distance calculation and collision detection method is used in the control process of the mechanical arm to determine whether the mechanical arm is in the forbidden zone.

[0054] First, the mechanical arm model and the tubular safety forbidden zone are represented by simple geometric envelopes, and the cable safety forbidden zone is discretized into a series of capsule bodies distributed along the cable centerline. Each link of the mechanical arm is enveloped by a capsule body, which is a hierarchical bounding volume. Further, a hierarchical bounding volume tree (BVH) is constructed, that is, a larger hierarchical bounding volume is constructed for the mechanical arm and the safety forbidden zone, which can completely enclose all hierarchical bounding volumes, and is regarded as the root node of the recursive level in collision detection. Subsequently, based on the surface area heuristic method (SAH), the child nodes are divided, and in this process, all objects are sorted according to the coordinate values of their centroids on the splitting axis. The sorted list is divided into two from the middle, and the first half is assigned to the left child node and the second half is assigned to the right child node. Through recursive iteration, a complete binary tree is constructed, and each leaf node contains only one hierarchical bounding volume.

[0055] In this way, the collision detection is simplified from the cumbersome distance calculation of the six links and all safety forbidden zone segments to a fast hierarchical bounding volume intersection test, resulting in a small list of potential collision pairs. Finally, only the leaf node pairs that intersect the hierarchical bounding volume are subjected to precise distance calculation to ensure that the mechanical arm is outside the safety forbidden zone. This hierarchical space management is the algorithm cornerstone of the system to achieve real-time safety control in complex environments, which ensures that the system can meet the extremely high real-time requirements under limited computing power.

[0056] During the movement of the robot arm, the safety forbidden zone is regarded as a potential field that generates repulsive force. The distance between the leaf nodes of the hierarchical bounding box intersection is inversely proportional to the size of the repulsive force. The closer the distance, the stronger the repulsive force. When the distance is less than the set safety threshold, the virtual repulsive force is converted into a speed instruction of the robot arm to drive the robot arm away from the forbidden zone.

[0057] S4, layered early warning: To further ensure safety, ultrasonic sensors and electric field effect sensors are arranged at each node of the robot arm, and a multi-modal sensor decision logic is constructed.

[0058] Primary warning: According to the time difference (ToF) between the emission and reception of electromagnetic waves measured by the ultrasonic sensor, the distance is calculated. It is used to determine whether there is an object around the node.

[0059] Secondary warning: The electric field effect sensor quantifies the electric field strength by measuring the micro-current generated by the induced charge, further confirming the danger level of the object. Only when both independent sensors are triggered at the same time is the highest level of danger determined, which greatly reduces the false positive rate.

[0060] S5, path planning and trajectory tracking: Before the operation starts, based on the current environment model, combined with the initial position of the robot arm end and the coordinates of the operation point, the sampling planning algorithm RRT* (rapidly exploring random tree) is used to find a collision-free path from the starting point to the target point in the configuration space of the robot arm.

[0061] Through five polynomial interpolation, each point is given a timestamp, speed and acceleration information, generating a smooth trajectory that the robot arm can stably execute, ensuring continuous speed and acceleration during movement, avoiding wear and impact due to sudden force on the joints.

[0062]

[0063] where, represents the angle of the th joint at time ; and represents the polynomial coefficient of the th segment of the path.

[0064] The core of trajectory tracking is to make the robot arm joints accurately follow the expected trajectory ”, which needs to solve two major problems: the nonlinearity of robot arm dynamics; and the base offset caused by platform sway. By calculating the torque controller (CTC) to actively offset the nonlinearity of inertia, Coriolis force, etc., and coordinate system transformation to compensate for sway, the end position error is reduced.

[0065] Based on the robot arm dynamics model, a torque controller (CTC) is designed to actively offset the nonlinearity of inertia, Coriolis force, etc.

[0066] The 6-DOF robot dynamics equation is:

[0067] where, is the inertia matrix, reflecting the effect of joint angle on inertia resistance; is the Coriolis / centrifugal force matrix, reflecting the coupling effect of joint angular velocity and angle; is the gravity vector, reflecting the effect of joint angle on gravity moment; is the joint driving torque vector output by the controller.

[0068] The CTC control law realizes tracking by feedforward compensation of nonlinearity + PD feedback correction of error.

[0069]

[0070] where, is the feedforward term, used to offset the dynamics nonlinearity, making the system approximately a linear second-order system; is the PD feedback term, correcting the angle and speed error, and suppressing overshoot; , is the PD gain matrix.

[0071] The core of trajectory tracking lies in the controller design. The basic level adopts computed torque control, which is based on the robot dynamics model and can calculate the joint torque that offsets the effects of Coriolis force, centrifugal force and other nonlinear dynamics in real time, providing a foundation for accurate tracking of the desired trajectory.

[0072] The distribution network line operation platform is mounted on the cable and is easily affected by wind and load changes, resulting in shaking, which causes the base coordinate system {B} to deviate from the world coordinate system {W}. If the desired trajectory in the world coordinate system {W} is directly tracked, the end will deviate from the target. Compensation needs to be achieved through IMU / visual fusion estimation of the transformation matrix + trajectory coordinate transformation.

[0073] World coordinate system {W}: fixed near the work point (such as the cable suspension point), Z axis perpendicular to the ground, X axis pointing to the work direction.

[0074] Base coordinate system {B}: fixed on the robot base (rigidly connected with the platform), shaking with the platform, X axis along the initial stretching direction of the robot arm.

[0075] The pose of the base coordinate system {B} relative to the world coordinate system {W} is described by the 4x4 homogeneous transformation matrix .

[0076] ; where, R B W represents the rotation matrix of base coordinate system {B} relative to world coordinate system {W}, measured by IMU quaternion conversion, conversion formula: ; wherein, R B W represents the translation vector of the origin of the base coordinate system {B} relative to the origin of the world coordinate system {W}, obtained by feature point matching from camera data through adjacent frame displacement.

[0077] During the platform operation process, the IMU and the depth camera are fused to output in real time. The end desired position is compensated: ; The compensated end desired position is substituted into the inverse kinematics of the robot arm to update the joint desired attitude, and the torque controller is calculated in real time to track, so that the actual position of the robot arm end in the world coordinate system will accurately fit the end desired position , offsetting the influence of platform shaking.

[0078] Embodiment 2: A distribution network line operation platform robot arm control system, comprising: a data acquisition module configured to acquire relevant laser data and image data in an operation area; a fusion module configured to fuse and process the laser data and the image data; and perform point cloud segmentation and feature extraction on the fused and processed laser data and image data to obtain cable clusters, clamp clusters, robot arm clusters and obstacle clusters; an identification module configured to determine cables, clamps, obstacles and operation platforms through image recognition according to the cable clusters, clamp clusters, robot arm clusters and obstacle clusters; a safety exclusion zone determination module configured to extract a cable center line by principal component analysis based on the identified cable; and expand outward based on the center line to obtain a safety exclusion zone of the robot arm; a control module configured to control the robot arm according to the safety exclusion zone, and a preset path planning strategy and trajectory tracking strategy; wherein, when the robot arm is controlled, the end desired position is compensated so that the actual position of the robot arm end in the world coordinate system fits the end desired position, offsetting the influence of platform shaking.

[0079] The working method of the system is the same as the distribution network line operation platform robot arm control method of embodiment 1, which will not be repeated here.

[0080] Embodiment 3: The embodiment provides a computer readable storage medium, which stores a computer program, and the program is executed by a processor to implement steps of the network distribution line operation platform mechanical arm control method in the embodiment 1.

[0081] Embodiment 4: The embodiment provides an electronic device, which comprises a memory, a processor, and a computer program stored in the memory and capable of running on the processor, and the processor implements steps of the network distribution line operation platform mechanical arm control method in the embodiment 1 when executing the program.

[0082] Embodiment 5: The embodiment provides a computer program product, which comprises a computer program, and the computer program is executed by a processor to implement steps of the network distribution line operation platform mechanical arm control method in the embodiment 1.

[0083] The above merely provides preferred embodiments of the embodiment, and is not intended to limit the embodiment. The embodiment can have various modifications and changes for those skilled in the art. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the embodiment shall be included in the protection scope of the embodiment.

Claims

1. A method for controlling a mechanical arm of a network wiring line operation platform, characterized in that, The method comprises the following steps: acquiring relevant laser data and image data in a work area; fusing and processing the laser data and the image data; performing point cloud segmentation and feature extraction on the fused and processed laser data and image data to obtain cable clusters, clamp clusters, mechanical arm clusters and obstacle clusters; determining cables, clamps and obstacles and a work platform through image recognition according to the cable clusters, the clamp clusters, the mechanical arm clusters and the obstacle clusters; extracting a cable center line through principal component analysis for the recognized cables; extending outward based on the center line to obtain a safety forbidden area of the mechanical arm; controlling the mechanical arm according to the safety forbidden area and preset path planning strategies and trajectory tracking strategies; wherein, when the mechanical arm is controlled, an end desired position is compensated so that an actual position of the mechanical arm end in a world coordinate system is fitted to the end desired position, and the influence of the work platform shaking is offset.

2. The method of claim 1, wherein: When the relevant laser data and image data in the work area are acquired, the laser radar and the camera acquiring the laser data and the image data are calibrated offline: a calibration board is placed in the field of view of the laser radar and the camera at the same time, the laser radar obtains the point cloud of the plane of the calibration board, the camera recognizes the corner points of the calibration board, the internal parameters of the camera are calibrated through Zhang's calibration method, the distortion coefficient of the camera image is obtained and the internal parameters are optimized; when the external parameters are calibrated, the corresponding relationship of the observation data of the same object by the laser radar and the camera is found, the optimal rotation matrix and translation vector are found, and the re-projection error between the laser radar point cloud projected onto the image plane and the corresponding feature points in the camera image is minimized.

3. The method of claim 2, wherein: Each three-dimensional point cloud coordinate of the laser radar is transformed into the camera coordinate system, and then projected onto the two-dimensional image through the camera internal parameters.

4. The method of claim 1, wherein: The laser data and the image data are sequentially subjected to straight-through filtering, statistical outlier removal and voxel grid filtering: the straight-through filtering removes the point cloud data of the region not of interest through the given filtering field and value range, reduces the data size, and retains a cylindrical region with the longest stretching distance of the mechanical arm plus a preset safety distance as the center; the statistical outlier removal removes the points whose average distance to other points exceeds the preset multiple standard deviation range of the mean value; the voxel grid filtering divides the three-dimensional space into uniform voxels.

5. The method of claim 1, wherein, The point cloud segmentation and feature extraction comprise the following steps: the coordinate range is converted according to the geometric parameters of the work platform, and the point cloud data is removed; the real-time pose of each connecting rod in the radar coordinate system is determined one by one according to the joint angle information of the mechanical arm and the geometric information of the end effector, and the axis-aligned bounding box of each connecting rod is generated; all point clouds are traversed, and the points falling within any connecting rod bounding box are classified as mechanical arm candidate clusters; for the remaining point clouds, the K-D tree search is used to classify the points with a spatial distance not greater than a first preset distance into a pre-grouping cluster; a plurality of points with an x-axis span greater than a preset span are randomly extracted in a single pre-grouping cluster, a straight line model or a quadratic curve model is selected, and the model parameters are solved; the perpendicular distance of all segments to the model is obtained, and the inliers with a distance less than a second preset distance are counted; the model with the most inliers is selected through multiple iterations according to the above method, and the inliers are classified as cable candidate clusters; subsequently, cable models are fitted in the remaining point cloud cluster groups to obtain the remaining cable candidate clusters; Based on the segmented cable cluster and the fitted trajectory of the cable cluster, a plurality of clamp detection segments are divided, a cylindrical candidate area is formed along the direction perpendicular to the cable with the cable trajectory as the center, and the clamp candidate point cloud is combined with the removed robot arm and cable candidate point cloud to determine each point using a K-D tree to screen the point cloud of the points that meet the density characteristics of the clamp to obtain clamp candidate points; the candidate points are subjected to Euclidean clustering to obtain clamp candidate clusters; and the remaining point cloud is subjected to even clustering to preliminarily divide the obstacle clusters.

6. The method of claim 1, wherein: The safety forbidden zone determination comprises: determining the working phase cable extension direction and cross section plane by principal component analysis; setting the point cloud of the non-working phase cable as , wherein the radar coordinates of each point are ; and moving the point cloud center to the coordinate system origin. wherein, is the radar coordinate of the i-th original point in the non-working phase cable point cloud; is the radar coordinate of the i-th original point in the non-working phase cable point cloud; is the total number of points contained in the non-working phase cable point cloud; is the geometric center of the non-working phase cable point cloud; The center of the point cloud at this time with the center as the coordinate origin; calculate the covariance matrix C (3 x 3): ； The principal components are described by the eigenvalues ( ) and eigenvectors ( ) of the covariance matrix C; the largest eigenvalue corresponds to the eigenvector , i.e. the direction of the cable extension, and the other eigenvectors correspond to the eigenvectors of the cross-sectional plane of the cable; The cable center line is the projection center of the point cloud on the cross section perpendicular to the extension direction, the center point is projected onto the extension direction to obtain a projection length, the projection lengths are arranged according to the size and averaged according to the interval to obtain discrete points of the center line in the original radar coordinate system, and curve fitting is performed on the discrete points to obtain a smooth center line trajectory; a three-dimensional tubular model is generated with the center line extracted by principal component analysis as an axis and a predetermined network safety regulation as a radius, and the three-dimensional tubular model is used as a safety forbidden area for the movement of the robot arm.

7. The method of claim 6, wherein: For the safety forbidden area of the non-operation phase line, the robot arm model and the safety forbidden area are represented by a geometric envelope, the safety forbidden area is discretized into a series of capsule bodies distributed along the conductor center line, each link of the robot arm is enveloped by a capsule body to obtain a hierarchical bounding box, a larger hierarchical bounding box that can completely surround all the hierarchical bounding boxes is constructed for the robot arm and the safety forbidden area, and the larger hierarchical bounding box is regarded as the root node of the recursive level in the collision detection; all objects are sorted according to the coordinate values of the centroids on the segmentation axis; the sorted list is divided into two from the middle, the front half is attributed to the left child node, and the back half is attributed to the right child node; a binary tree is constructed through a recursive loop, and each leaf node only contains one hierarchical bounding box; the distance between the leaf node pairs intersected by the hierarchical bounding boxes is used to ensure that the robot arm is outside the safety forbidden area; during the movement of the robot arm, the safety forbidden area is regarded as a potential field that generates a repulsive force, the distance between the leaf node pairs intersected by the hierarchical bounding boxes is inversely proportional to the size of the repulsive force; when the distance is less than a set safety threshold, the repulsive force is converted into a speed command of the robot arm to drive the robot arm away from the safety forbidden area.

8. The method of claim 1, wherein: The path planning strategy and the trajectory tracking strategy include: a sampling planning algorithm is used to find a collision-free path from the starting point to the target point in the configuration space of the robot arm; a polynomial interpolation is used to assign time stamps, velocities and acceleration information to each point to generate a smooth trajectory for the robot arm to execute.

9. The method of claim 8, wherein: Compensating for the end goal position comprises: by a 4x4 homogeneous transformation matrix Describing the pose of the base coordinate system B with respect to the world coordinate system W: ； wherein, represents a rotation matrix of the base coordinate frame relative to the world coordinate frame; represents a translation vector of the base coordinate frame origin relative to the world coordinate frame origin; for end-effector desired position compensate for: ； Compensated end-effector desired position Substitute the inverse kinematics of the robot arm, update the joint desired pose, calculate the torque controller in real time to track, so that the actual position of the end-effector of the robot arm in the world coordinate system is consistent with the end-effector desired position , offset the influence of platform shaking.

10. A network wiring line work platform mechanical arm control system, characterized in that, The method comprises the following steps: a data acquisition module configured to acquire relevant laser data and image data in the operation area; a fusion module configured to fuse and process the laser data and the image data, and to perform point cloud segmentation and feature extraction on the fused and processed laser data and image data to obtain cable clusters, clamp clusters, robot arm clusters and obstacle clusters; an identification module configured to determine cables, clamps, obstacles and operation platforms through image recognition based on the cable clusters, the clamp clusters, the robot arm clusters and the obstacle clusters; a safety forbidden area determination module configured to extract a cable center line through principal component analysis based on the identified cable; and a safety forbidden area is expanded outward based on the center line to obtain a safety forbidden area of the robot arm. The control module is configured to control the robot arm according to the safety forbidden area and a preset path planning strategy and trajectory tracking strategy; wherein when the robot arm is controlled, the end desired position is compensated so that the actual position of the robot arm end in the world coordinate system is consistent with the end desired position, and the influence of the work platform shaking is offset.

Images