An electric work robot mechanical arm positioning and trajectory optimization device

Abstract

This invention proposes a positioning and trajectory optimization device for a live-line working robot arm. An environmental perception module collects environmental parameters for high-altitude live-line work; a multi-source fusion positioning module collects various real-time position signals of the robot arm and fuses these signals; a trajectory optimization control module receives positioning information and environmental parameters, and plans and optimizes the robot arm's motion trajectory based on these parameters and a preset target position; and an execution adjustment module receives trajectory control commands and drives the robot arm to move along the optimized trajectory, enabling it to reach the work position. The modules work together seamlessly, requiring no manual intervention, shortening work time, and improving the overall efficiency of live-line work. This device is suitable for various high-altitude live-line work scenarios, improving the efficiency and reliability of live-line working robot arm positioning and trajectory optimization.

Description

Technical Field

[0001] This invention relates to the field of live-line working robots, and in particular to a device for positioning and trajectory optimization of the robotic arm of a live-line working robot. Background Technology

[0002] Live-line working is a crucial aspect of power system operation and maintenance. It is primarily used to inspect, maintain, and troubleshoot power equipment such as transmission and distribution lines, towers, and insulators without interrupting power supply. With the continuous development of power systems, the requirements for safety and efficiency in live-line working are constantly increasing. Live-line working robots are gradually replacing manual labor in performing high-altitude and high-risk live-line work tasks. Among these robots, the robotic arm, as the core execution component, directly determines the quality and safety of the work due to its positioning accuracy and trajectory control performance.

[0003] Currently, the positioning accuracy and trajectory control technology of existing live-line working robot arms still have the following shortcomings: (1) Robotic arm positioning relies on manual labor and automatic positioning accuracy is low. There are two main ways for existing robotic arms to reach the work position. One is to rely on manual remote control positioning. Manual control is affected by factors such as operator experience and visual fatigue, resulting in low positioning accuracy and efficiency, and there is a risk of misoperation. The other is to use a single automatic positioning method (such as visual positioning or laser positioning). It is affected by complex high-altitude working environment, such as strong light, rainstorm, fog, electromagnetic interference, etc. The positioning signal is prone to drift and distortion, resulting in positioning accuracy that cannot meet the work requirements, and even causing work safety accidents.

[0004] (2) The working trajectory of the robotic arm is unreasonable, and its motion trajectory has problems such as redundant movements, motion jamming, and unreasonable path. The existing trajectory planning of robotic arms mostly adopts fixed paths or simple algorithms, which do not fully combine the real-time changes of the working environment and the motion characteristics of the robotic arm. This results in the robotic arm having redundant movements during the movement, unstable movement speed, and jamming. In addition, the path planning does not avoid obstacles such as live lines, towers, and insulators, which may cause the robotic arm to collide with the above-mentioned equipment, causing equipment damage and work interruption. In severe cases, it may also cause safety accidents such as electric shock and equipment short circuit.

[0005] In view of the shortcomings of the existing technologies, there is an urgent need to design a device that can achieve precise positioning of the robotic arm, optimize the motion trajectory, and improve the safety and efficiency of operation, so as to solve the problems existing in the current technologies. Summary of the Invention

[0006] In order to solve the problems existing in the prior art, this invention innovatively proposes a positioning and trajectory optimization device for a live-line working robot arm, which effectively solves the problem of low efficiency and reliability of live-line working robot arm positioning and trajectory optimization caused by the prior art, and effectively improves the efficiency and reliability of live-line working robot arm positioning and trajectory optimization.

[0007] The first aspect of this invention provides a positioning and trajectory optimization device for a live-line working robot arm, comprising: an environmental perception module, a multi-source fusion positioning module, a trajectory optimization control module, and an execution adjustment module; the environmental perception module is used to collect environmental parameters for high-altitude live-line work, including the position of line components and the intensity of environmental interference signals within the work area, and transmits the collected environmental parameters to the trajectory optimization control module; the multi-source fusion positioning module is used to collect multiple real-time position signals of the robot arm, fuse the multiple real-time position signals, and output positioning information to the trajectory optimization control module; the trajectory optimization control module is used to receive the positioning information and environmental parameters, and based on the positioning information, environmental parameters, and a preset work target position, plan and optimize the motion trajectory of the robot arm, generate trajectory control commands, and transmit them to the execution adjustment module; the execution adjustment module is used to receive the trajectory control commands, drive the robot arm to move along the optimized trajectory, and achieve precise arrival of the robot arm at the work position.

[0008] The technical solution adopted in this invention has the following technical effects: 1. The environmental perception module, multi-source fusion positioning module, trajectory optimization control module, and execution adjustment module in the technical solution of this invention work together to achieve full automation of the robotic arm from positioning and trajectory planning to execution, without human intervention, shortening operation time, improving the overall efficiency of live-line work, and applicable to various high-altitude live-line work scenarios; effectively solving the problem of low efficiency and reliability of positioning and trajectory optimization of live-line work robot arms caused by existing technologies, and effectively improving the efficiency and reliability of positioning and trajectory optimization of live-line work robot arms.

[0009] 2. The environmental perception module in the technical solution of this invention includes a camera, a lidar, and an ultrasonic sensor. The camera is used to collect the position parameters, contour parameters, and type parameters of the circuit components in the work area, the feature point parameters of the preset work target, and the image boundary parameters of the work area. The lidar is used to measure the distance parameters between the robotic arm and surrounding obstacles, charged bodies, and grounded bodies, the three-dimensional point cloud data of the work area, and the size and depth parameters of the obstacles. The ultrasonic sensor is used to measure the supplementary distance parameters of nearby obstacles and the obstacle detection parameters around the joints of the robotic arm, ensuring the reliability of the positioning and trajectory optimization of the robotic arm of the live-line working robot.

[0010] 3. The multi-source fusion positioning module in the technical solution of this invention includes an integrated visual positioning unit, a laser positioning unit, and an inertial navigation positioning unit. The multi-source fusion positioning module uses a Kalman filter algorithm to fuse multiple real-time position signals and output positioning information. It integrates visual, laser, and inertial navigation positioning methods and eliminates signal drift and distortion through the Kalman filter algorithm. This solves the problems of high environmental interference and low positioning accuracy of single automatic positioning methods. At the same time, it replaces manual remote control positioning, realizes autonomous and accurate positioning of the robotic arm, improves positioning efficiency and accuracy, and reduces the risk of manual operation.

[0011] 4. In the technical solution of this invention, the trajectory optimization control module combines environmental perception information and robotic arm motion constraints, through improved... Algorithms and particle swarm optimization algorithms plan and optimize motion trajectories, eliminate redundant movements and motion stutters, avoid obstacles such as power lines, towers, and insulators, effectively prevent collisions between the robotic arm and equipment, improve the safety of live-line work, and reduce equipment damage and work interruptions.

[0012] 5. In response to the safety requirements and motion characteristics of the robotic arm in the live-line operation scenario, the technical solution of this invention adopts a 7-segment S-shaped speed curve based on safety level segmented adaptive adjustment to achieve precise adjustment of the motion speed curve. This solves the problems of fixed speed curve, easy jamming, and uncontrollable speed when close to the live body in the prior art, and further improves the reliability of the positioning and trajectory optimization of the robotic arm of the live-line operation robot.

[0013] It should be understood that the above general description and the following detailed description are exemplary and explanatory only, and are not intended to limit the invention. Attached Figure Description

[0014] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, for those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0015] Figure 1 This is a schematic diagram of the communication structure of each module in the device of Embodiment 1 of the present invention; Figure 2 This is a schematic diagram showing the positional distribution of each module in the device of Embodiment 1 of the present invention; Figure 3 This is a flowchart illustrating the trajectory optimization control module in the device of Embodiment 1 of the present invention; Figure 4 This is another flowchart illustrating the trajectory optimization control module in the device of Embodiment 1 of the present invention. Detailed Implementation

[0016] To clearly illustrate the technical features of this solution, the invention will be described in detail below through specific embodiments and in conjunction with the accompanying drawings. The following disclosure provides many different embodiments or examples for implementing different structures of the invention. To simplify the disclosure of the invention, components and arrangements of specific examples are described below. Furthermore, reference numerals and / or letters may be repeated in different examples. This repetition is for simplification and clarity and does not in itself indicate a relationship between the various embodiments and / or arrangements discussed. It should be noted that the components illustrated in the drawings are not necessarily drawn to scale. Descriptions of well-known components, processing techniques, and processes are omitted in this invention to avoid unnecessarily limiting the invention.

[0017] Example 1 like Figures 1-2 As shown, this invention provides a positioning and trajectory optimization device for a live-line working robot arm, comprising: an environmental perception module 2, a multi-source fusion positioning module 3, a trajectory optimization control module 4, and an execution adjustment module 5. The environmental perception module 2 is used to collect environmental parameters for high-altitude live-line work, including the position of line components and the intensity of environmental interference signals within the work area, and transmits the collected environmental parameters to the trajectory optimization control module 4. The multi-source fusion positioning module 3 is used to collect multiple real-time position signals of the robot arm 1, fuse these signals, and output positioning information to the trajectory optimization control module 4. The trajectory optimization control module 4 is used to receive the positioning information and environmental parameters, plan and optimize the motion trajectory of the robot arm 1 based on the positioning information, environmental parameters, and a preset work target position, generate trajectory control commands, and transmit them to the execution adjustment module 5. The execution adjustment module 5 is used to receive the trajectory control commands, drive the robot arm 1 to move along the optimized trajectory, and enable the robot arm 1 to reach the work position.

[0018] The environmental perception module 2 includes a camera, a lidar, and an ultrasonic sensor. The camera is installed at the end of the robotic arm to collect the position, contour, and type parameters of the circuit components within the work area, the feature point parameters of the preset work targets, and the image boundary parameters of the work area. The circuit components include obstacles such as live lines, poles, crossarms, insulators, fittings, and switches. The lidar is installed on the top of the robot body to measure the distance parameters between the robotic arm and surrounding obstacles, live conductors, and grounded conductors, as well as the 3D point cloud data of the work area and the size and depth parameters of obstacles. The ultrasonic sensor is installed at the joints of the robotic arm to measure the supplementary distance parameters of nearby obstacles and the obstacle detection parameters around the joints of the robotic arm.

[0019] The camera can be an industrial-grade high-definition camera with a resolution of 1920×1080 and a frame rate of 30fps. It is resistant to strong light and waterproof, and is installed at the end of the robotic arm. The lidar can be a 2D lidar with a measurement range of 0.1-50m and a measurement accuracy of ±1cm, and is installed on the top of the robot body. The ultrasonic sensor is a waterproof ultrasonic sensor with a measurement range of 0.02-5m, and is installed at the joint of the robotic arm. The three sensors work together to collect environmental parameters, which are then pre-processed and transmitted to the trajectory optimization and control module.

[0020] The multi-source fusion positioning module 3 includes an integrated visual positioning unit, a laser positioning unit, and an inertial navigation positioning unit. The visual positioning unit captures the image features of the target through a camera to achieve the initial positioning of the robotic arm. The laser positioning unit measures the distance and relative position between the robotic arm and the target through laser ranging technology. The inertial navigation positioning unit collects the motion posture, speed, and acceleration information of the robotic arm in real time.

[0021] Specifically, the visual positioning unit can use a high-definition camera shared with the environmental perception module. It extracts feature points of the target using the SIFT image recognition algorithm (with a feature point extraction threshold of 0.03), and converts the image coordinates into world coordinates using a perspective transformation algorithm to achieve preliminary positioning of the robotic arm, with the positioning error controlled within ±2cm. The laser positioning unit uses a laser rangefinder sensor, which works in conjunction with the visual positioning unit to accurately measure the distance and relative position between the robotic arm and the target using the triangulation principle, with a ranging error of ±0.1cm. The inertial navigation positioning unit uses a MEMS inertial measurement unit with a sampling frequency of 100Hz to collect the robotic arm's attitude angle, angular velocity, and acceleration information in real time. The collected data is then processed for noise reduction using a complementary filtering algorithm.

[0022] The multi-source fusion positioning module 3 uses the Kalman filter algorithm to fuse multiple real-time position signals and output positioning information; among them, the real-time position signals include visual positioning signals, laser positioning signals and inertial navigation positioning signals.

[0023] Specifically, the Kalman filter algorithm is set as follows: The state equation (system state prediction equation) is X(k) = A × X(k-1) + B × U(k) + W(k); The observation equation (system observation update equation) is Z(k) = H × X(k) + V(k); The state matrix A is a 3×3 matrix, the observation matrix H is a 3×3 matrix, and the process noise W(k) and observation noise V(k) both follow a Gaussian distribution with variances of 0.01 and 0.001, respectively. The algorithm fuses the three positioning signals to effectively eliminate signal drift and distortion, and outputs real-time positioning information with a positioning accuracy of ±0.5cm.

[0024] The core function of the Kalman filter algorithm in this scheme is to optimally fuse the output results of the three positioning methods: visual positioning, laser positioning, and inertial navigation positioning, to eliminate the drift and distortion of individual signals, and finally output positioning information with an accuracy of ±0.5cm at the end of the robotic arm.

[0025] The specific definitions of the physical meaning of all parameters are shown in Table 1 below: Table 1: Physical Meaning of Kalman Filter Algorithm Parameters

[0026] The Kalman filter algorithm in this scheme achieves complementary advantages of the three positioning methods through two core stages: prediction and update, thus overcoming the shortcomings of single signals. The specific correlation logic is as follows: 1. Prediction Stage: Relying on inertial navigation positioning to address the insufficient dynamic response of visual / laser positioning. This stage uses only the state equation, based on the optimal position estimate at time k-1, combined with the velocity increment U(k) output by the inertial navigation positioning unit, to predict the prior estimate of the robot arm's position at time k. Core Function: Leveraging the advantages of high sampling frequency and fast dynamic response of inertial navigation positioning, it compensates for the shortcomings of low sampling frequency and inability to track rapid robot arm movements in visual and laser positioning. Simultaneously, it quantifies the inherent drift error of inertial navigation through process noise W(k), providing a basis for subsequent correction.

[0027] 2. Update Phase: Relying on visual / laser positioning to solve the problem of cumulative drift in inertial navigation. This phase uses observation equations to first calculate the observed prediction value corresponding to the prior estimate, and compare it with the actual observed value Z(k) of visual + laser positioning to obtain the observation residual; then, the prior estimate is corrected by Kalman gain to obtain the optimal posterior estimate of the robot arm position at time k (i.e., the final output precise positioning information). Core function: Utilizing the advantages of high absolute position accuracy and no cumulative drift of visual and laser positioning, the cumulative error of inertial navigation positioning is corrected. At the same time, the impact of environmental interference on visual and laser positioning is quantified by observation noise V(k), achieving optimal fusion of the two absolute positioning methods.

[0028] 3. The collaborative closed-loop relationship among the three: Inertial navigation provides dynamic prediction basis for the algorithm, and vision + laser positioning provides absolute position correction benchmark for the algorithm. The two equations achieve complementary advantages of the three positioning signals through the quantization of Gaussian noise and the optimal estimation of minimum mean square error. Finally, in complex environments such as high altitude strong electromagnetic fields and strong light, stable positioning information with an accuracy of ±0.5cm is output.

[0029] Therefore, the multi-source fusion positioning module 3 of this solution can accurately locate live lines, towers, insulators, hardware, switches, and various fixed / dynamic obstacles, in addition to the work target and the robotic arm, with a positioning accuracy of no less than ±1cm. The specific implementation method is as follows: 1. Hardware reuse and target recognition: The multi-source fusion positioning module and the environmental perception module share the high-definition camera and LiDAR acquisition unit. Through the YOLO target detection algorithm and SIFT feature extraction algorithm, the contours, types and pixel coordinates of power lines, towers, insulators, hardware, switches and obstacles are identified; at the same time, the depth and distance point cloud data of the target are acquired through LiDAR.

[0030] 2. Coordinate Transformation and Coarse Positioning: By using camera calibration parameters and perspective transformation algorithms, the image pixel coordinates are transformed into three-dimensional coarse positioning coordinates in the world coordinate system. Combined with LiDAR point cloud data, depth correction is performed to obtain the three-dimensional coarse positioning information of the target (error ≤ ±2cm).

[0031] 3. Fusion Filtering and Precise Positioning: The coarse positioning information is used as the observation value. Combined with the robot body posture compensation data output by the inertial navigation unit (to eliminate coordinate deviation caused by high-altitude platform shaking), the Kalman filter algorithm is used for optimization to eliminate coordinate drift caused by environmental interference. Finally, the precise three-dimensional positioning information, contour boundary and size parameters of the target are output.

[0032] Among them, in trajectory optimization control module 4, such as Figure 3 As shown, based on positioning information, environmental parameters, and preset target positions, the motion trajectory of the robotic arm is planned and optimized, and trajectory control commands are generated, specifically including: S1, based on the preset work targets and obstacle location information and contour parameters of the live-line work area, input a 3D grid map of the work space. According to the live-line work regulations, define the spatial boundaries, dividing the area into different zones including restricted areas, warning zones, and safety zones. Based on the spatial boundaries of these different zones, use the improved... The algorithm plans and generates the initial motion trajectory; S2, the preset constraints are used as the input to the fitness function of the particle swarm optimization algorithm, and the initial motion trajectory parameters are optimized by the particle swarm optimization algorithm; S3: During the operation, the positioning information of the target in the live working area is continuously updated. If the swing of the conductor or the displacement of the obstacle is detected, resulting in the minimum distance from the current movement trajectory being less than the safety threshold, the trajectory adjustment process is immediately triggered.

[0033] Specifically, step S1 includes: S11, preprocess the location information and contour parameters of the target in the live-line working area collected by the environmental perception module, and extract structured data that can be used for trajectory planning; Specifically, in step S11, this solution addresses the special scenarios of live-line work involving high altitude, high risk, and strong electromagnetic interference. It uses the multi-dimensional parameters collected by the environmental sensing module 2 as its core foundation, and employs environmental parameter preprocessing, digital modeling of the work space, and improvement techniques. The entire process of algorithm-based heuristic search and trajectory feasibility verification generates an initial motion trajectory that complies with live-line working safety regulations and is adapted to on-site working conditions.

[0034] Step S11 standardizes the raw multidimensional parameters collected by the environmental perception module 2, extracting structured data that can be used for trajectory planning, providing accurate input for initial trajectory generation. Specific details are as follows: (1) Source and type of multidimensional parameters: All parameters come from the three acquisition units of the environmental perception module, including: High-definition camera captures: the position, outline, and type parameters of power lines, towers, crossarms, insulators, hardware, and switches; the feature point parameters of the target; and the image boundary parameters of the work area. LiDAR data acquisition: distance parameters between the robotic arm and surrounding obstacles, charged bodies, and grounded bodies; 3D point cloud data of the work area; size and depth parameters of obstacles. Ultrasonic sensor acquisition: supplementary distance parameters of obstacles at close range (0.02-5m), and obstacle detection data around the robotic arm joints.

[0035] (2) Preprocessing procedure: Data denoising: statistical filtering is performed on lidar point clouds to remove outliers, strong light suppression and distortion correction are performed on images, and mean filtering is performed on ultrasonic data to eliminate random errors; Target fusion and registration: The target contour and type identified in the image are registered with the LiDAR point cloud data to match the three-dimensional coordinates, contour boundary, size, and attributes (charged body / insulator / grounded body / obstacle) of each target (such as power line, tower, obstacle, etc.) to form a structured target parameter set; Coordinate unification: All target parameters, the current position of the robotic arm, and the position of the work target are uniformly transformed into the world coordinate system to eliminate coordinate deviations between different acquisition units and ensure spatial consistency.

[0036] S12, based on the preprocessed structured data, construct a three-dimensional raster map of the work space, and combine it with the safety specifications for live-line work to classify the safety level, generating spatial boundaries and constraints for different areas for the initial trajectory; Specifically, step S12 includes: S121, with the robotic arm base as the origin and the maximum working radius of the robotic arm as the boundary, constructs a three-dimensional cubic working space and divides the three-dimensional cubic working space into several three-dimensional grids; Specifically, a three-dimensional cubic working space is constructed with the robotic arm base as the origin and the maximum working radius of the robotic arm as the boundary. This space is then divided into several three-dimensional grids with a side length of 0.5cm (matching the ±0.5cm positioning accuracy of this solution).

[0037] S122, based on the structured target parameter set, assign a label to each three-dimensional grid. The three-dimensional grid located within the preset target outline is labeled as an obstacle grid, and the three-dimensional grid in free space is labeled as a free grid. Specifically, based on the structured target parameter set, each grid is assigned a value: grids located within the target outline are labeled as "obstacle grids", and grids in free space are labeled as "free grids".

[0038] S123, based on the live-line working specifications, combines the target attributes corresponding to the three-dimensional grid to classify the safety level of the free grid, and sets differentiated path cost values. Specifically, strictly following the 10kV live-line working safety regulations, and combining the target attributes corresponding to the grid, the free grid is divided into safety levels, and differentiated path costs are set. After division, it includes restricted areas, warning areas, and safe areas.

[0039] The restricted area includes all obstacle grids, as well as free grids connecting different phase charged bodies <0m and connecting charged bodies and grounded bodies <0m (areas where different phase charged bodies are connected or where charged bodies and grounded bodies are connected, i.e., areas with a distance <0m). The path cost is set to infinity, and entry into the trajectory is absolutely prohibited. Warning zone: Free grids 0m to 0.4m away from charged or grounded objects, with path cost set to 10 times the base cost value, and priority should be given to avoiding them during trajectory planning; Safe Zone: A free grid cell located more than 0.4m (first preset distance) from charged or grounded conductors. The path cost is set to a base cost of 1, making it a priority area for trajectory planning. This cost setting directly determines the direction of subsequent heuristic searches, ensuring that the initial trajectory is generated primarily within the safe zone, thus mitigating the risk of electric shock and collisions at the source.

[0040] At the same time, the location coordinates of obstacles, charged bodies, and grounded bodies are used as improvements. The core parameter of the algorithm's path cost function is that the closer the trajectory node is to the restricted area, the higher the path cost, ensuring that the generated initial trajectory meets the safety requirements for live-line work throughout, with no risk of collision or electric shock.

[0041] S13, based on the constructed 3D raster map, spatial boundaries and constraints of different regions, uses an improved... The algorithm plans and generates the initial motion trajectory; Specifically, step S13 includes: S131, using the current 3D coordinates of the robotic arm's end effector as the planning start point S and the preset 3D coordinates of the target object as the planning end point G, initialize... Algorithm parameters; Specifically, based on the constructed 3D raster map, improvements were made to this solution. The algorithm initializes its parameters. All core parameters are derived from the multi-dimensional parameters of environmental perception and the constraints of the live-line working scenario. Algorithm parameter initialization: Starting point S: The current three-dimensional coordinates of the robotic arm's end effector, derived from precise positioning information from the multi-source fusion positioning module; Endpoint G: The three-dimensional coordinates of the target, derived from precise positioning information obtained through environmental perception and multi-source fusion optimization; Open list / closed list initialization: The open list initially contains only the starting point S, and the closed list is initially empty, used for heuristic search.

[0042] S132, Determine the improvements The algorithm's path cost function and heuristic function; the path cost function includes the actual cost and the heuristic function value, which is a weighted fusion of Euclidean distance and Manhattan distance; Specifically, the improvements in this solution are: The algorithm path cost function f(n) is: f(n) = g(n) + h(n); Wherein, the actual cost g(n) is the sum of the cost of all the grids passed from the starting point S to the current node n. The grids passing through the warning zone will have a high cost added to ensure that the algorithm prioritizes the safe zone path. Heuristic function h(n): This solution is designed for live-line working scenarios and uses a weighted fusion of Euclidean distance and Manhattan distance, as shown in the formula: Among them, (x n ,y n ,z n (x) represents the coordinates of the current node n. G ,y G ,z G The coordinates of the endpoint G are shown below. The weighting coefficients of 0.6 / 0.4 are the optimal values ​​verified by extensive simulations in live-line working scenarios, ensuring search efficiency while avoiding the trajectory from intersecting between live conductors. Simultaneously, the weighting coefficients can be dynamically adjusted based on the obstacle distribution perceived by the environment: in densely populated areas with live lines, the Manhattan distance weight is increased to 0.5~0.6 to enhance axial constraints and improve safety; in open areas, the Euclidean distance weight is increased to 0.7~0.8 to shorten the trajectory length and improve efficiency.

[0043] S133, Based on the workspace parameters, set algorithm search constraints, the algorithm search constraints are: the change in joint angle of adjacent nodes does not exceed a preset percentage (10%) of the maximum angle of a single joint; the search range only includes the space reachable by the robotic arm to avoid generating unreachable trajectories; S134, based on the initialized algorithm parameters, executes the improved... The algorithm uses heuristic search to generate an initial sequence of trajectory nodes.

[0044] Specifically, based on the initialized algorithm parameters, the improved algorithm is executed. The algorithm uses heuristic search to generate an initial sequence of trajectory nodes. The specific process is as follows: (1) Take the node with the smallest total path cost f(n) from the open list, use it as the current search node, and move it into the closed list; (2) Traverse all three-dimensional neighboring nodes of the current node and verify each neighboring node: if it is a restricted area grid or is already in the closed list, skip it directly; if it is not in the open list, calculate its g(n) value, set the parent node and add it to the open list; if it is already in the open list and the new g(n) value is smaller, update its cost and parent node. (3) Repeat the above steps until the endpoint G is added to the open list (search successful) or the open list is empty (search failed, readjust the region division and search again). (4) After a successful search, backtrack from the endpoint G along the parent node to the starting point S to obtain the complete node sequence, which is the initial motion trajectory of the robotic arm.

[0045] S14, perform a feasibility check on the generated initial trajectory for live-line work to ensure that the trajectory meets safety specifications and robotic arm movement requirements; the feasibility check includes safety distance check, robotic arm accessibility check, and trajectory continuity check.

[0046] Specifically, the generated initial trajectory undergoes a feasibility check specifically for live-line work to ensure that the trajectory complies with safety regulations and the robotic arm's movement requirements: (1) Safety distance verification: Traverse all trajectory nodes, calculate their distances to the nearest charged body and obstacle, ensure that all nodes meet the minimum safety distance requirements, and fine-tune the coordinates of nodes that do not meet the requirements; (2) Robotic arm reachability verification: Perform inverse kinematics solution for each node to verify whether the joint rotation angle is within the allowable range, and adjust nodes that exceed the limit; (3) Trajectory continuity verification: Verify whether the position changes of adjacent nodes are continuous, remove redundant nodes, and ensure that the trajectory is smooth and continuous.

[0047] Through the above entire process, this solution generates an initial motion trajectory that conforms to safety standards, adapts to on-site conditions, and is reachable by the robotic arm, based entirely on the multi-dimensional parameters of the live-line working site collected by the environmental perception module 2. This solves the technical problem that general trajectory planning algorithms do not consider the safety constraints of live-line work and are prone to generating risky trajectories.

[0048] In step S2, the particle swarm optimization algorithm of this scheme, addressing the core requirements of prioritizing safety while balancing smoothness and efficiency in live-line working scenarios, designs a multi-objective weighted fusion fitness function. This function is the core basis for algorithm iterative optimization, directly determining the direction and effect of trajectory optimization. In the global trajectory optimization stage: core constraints determine the optimization direction and effect. This type of positioning information is the core input of the particle swarm optimization algorithm's fitness function: Safety constraint optimization: The positioning information of live conductors, grounding conductors, and obstacles is used as safety constraint sub-items of the fitness function. During optimization, the distance between trajectory nodes and the nearest live conductor is calculated in real time. If it is less than the safety threshold, a high penalty is applied to ensure that the optimized trajectory always maintains a sufficient safe distance; Smoothness optimization: Combining the positioning and contours of towers, crossarms, insulators, hardware, and switches, the inflection points and curvature of the trajectory are optimized to avoid sharp turns around live equipment, reducing redundant actions and stuttering; Efficiency optimization: Combining the positioning information of obstacle distribution, under the premise of safety constraints, the total trajectory length is optimized first to reduce invalid actions and improve work efficiency. Specific scenario settings are as follows: The priority of the fitness function in this scheme, from high to low, is: safety constraints > trajectory smoothness > work efficiency > robotic arm motion constraints. By weighted fusion of multiple objective sub-items, multiple objectives of trajectory optimization are quantified into a single fitness value. The smaller the fitness value, the better the trajectory corresponding to the particle.

[0049] Specifically, the fitness function is as follows: F=ω1×F safe +ω2×F smooth +ω3×F eff +ω4×F limit Where F is the total fitness value of the particle; ω1 is the weighting coefficient of the safety constraint term; F safe For safety constraints; ω2 is the weighting coefficient for trajectory smoothness; F smooth For trajectory smoothness, ω3 is the weighting coefficient for the operation efficiency sub-item; F eff ω4 is the weighting coefficient for the motion constraint sub-item; F is the sub-item for work efficiency. limit For motion constraint sub-items; and, satisfying ω1+ω2+ω3+ω4=1, combined with the live-line working scenario, this scheme sets ω1=0.5 (highest safety priority), ω2=0.25, ω3=0.15, ω4=0.1; Among them, the safety constraint sub-item F safe The specific calculation method is as follows:

[0050] Where N is the total number of nodes in the optimized trajectory; D th The minimum safe distance threshold for live-line working is set at 0.4m for live lines (minimum safe distance between tools and live conductors), and 0.1m for grounded conductors / ordinary obstacles. i λ is the minimum distance from the i-th node of the trajectory to the nearest charged body / obstacle; λ is the penalty coefficient (set to 100 in this scheme. When the distance between trajectory nodes is less than the minimum safe distance threshold for live-line work, a high squared penalty is triggered, causing the fitness value of the particle to increase sharply and be quickly eliminated in the iteration, thus avoiding safety risks from the root). This sub-item is the core of the fitness function, used to quantify the safety performance of the trajectory and ensure that the trajectory maintains a sufficient safe distance from charged bodies and obstacles throughout the entire process.

[0051] Among them, the trajectory smoothness sub-term F smooth The specific calculation method is as follows:

[0052] in, Let be the angular acceleration of the robotic arm joint corresponding to the i-th node of the trajectory, which is obtained by the second-order difference of the joint rotation angles of the three adjacent nodes. =θ i+1 2θ i +θ i 1, θ i is the joint angle vector corresponding to the i-th node; || is the 2-norm of the vector, representing the fluctuation amplitude of the joint angular acceleration; this sub-item is used to eliminate redundant movements and motion jams in the trajectory, ensuring the smooth operation of the robotic arm at height and avoiding insufficient safety distance due to shaking. The smaller the value of this sub-item, the smaller the fluctuation of the joint angular acceleration in the trajectory, the smoother the trajectory, and the robotic arm movement without speed changes, jams, or impacts, fully meeting the stability requirements of high-altitude live-line operations.

[0053] Among them, the work efficiency sub-item F eff The specific calculation method is as follows:

[0054] Where L is the total length of the current trajectory, L max T represents the total length of the initial trajectory (normalized baseline); T represents the total operation time of the current trajectory. maxThe total operation time of the initial trajectory (normalized baseline) is used to shorten the total trajectory length and operation time, reduce redundant actions, and improve the efficiency of live-line work. The smaller the value of this sub-item, the shorter the total trajectory length, the less operation time, and the higher the operation efficiency, which solves the problem of many redundant trajectory actions and long operation time in the existing technology.

[0055] Among them, the motion constraint sub-term F limit The specific calculation method is as follows:

[0056] Where M is the total number of joints in the robotic arm; Let be the rotation angle of the j-th joint corresponding to the i-th node of the trajectory. The maximum rotation angle of the j-th joint. Let be the minimum rotation angle of the j-th joint; Let be the angular velocity of the j-th joint corresponding to the i-th node of the trajectory. represents the maximum angular velocity of the j-th joint. This sub-item ensures that the trajectory conforms to the physical constraints of the robotic arm, avoiding unexecutable actions due to joint angle or velocity exceeding limits. When the joint angle or angular velocity of a trajectory node exceeds the limit, a penalty term is triggered, increasing the fitness value to ensure that the optimized trajectory fully conforms to the motion constraints of the robotic arm and can be executed precisely.

[0057] The execution logic of the fitness function in the algorithm: (1) Particle encoding: Each particle corresponds to a trajectory to be optimized. The particle dimension is the number of trajectory nodes × the number of robotic arm joints. The value of each dimension corresponds to the joint rotation angle of the trajectory node, realizing a one-to-one mapping between the trajectory and the particle. (2) Fitness calculation: After the particle swarm is initialized, the values ​​of the above four sub-items are calculated for the trajectory corresponding to each particle, and the weighted fusion is used to obtain the total fitness value F; (3) Iterative optimization: In each iteration, the particle updates its velocity and position based on the individual best (pbest) and the global best (gbest), recalculates the fitness value, and updates the optimal solution until the preset number of 100 iterations is reached; (4) Optimal trajectory output: After the iteration is completed, the trajectory of the particle corresponding to the global optimal position (with the smallest fitness value) is the optimized optimal operation trajectory.

[0058] In step S3, during the operation, the positioning information of the target in the live-line work area is continuously updated. If a conductor swing or obstacle displacement is detected, causing the minimum distance from the current trajectory to be less than the safety threshold, the trajectory adjustment process is immediately triggered. During the operation, the multi-source fusion positioning module 3 continuously updates the positioning information of the live conductor, grounding conductor, and obstacles. If a conductor swing or obstacle displacement is detected, causing the minimum distance from the current trajectory to be less than the safety threshold, the trajectory adjustment process is immediately triggered. Simultaneously, the updated positioning information is synchronized in real time to the 3D grid map, serving as the core benchmark for local trajectory optimization or global replanning, ensuring that the adjusted trajectory always avoids risk areas. The trajectory adjustment process specifically includes: S31, obtain the basis for adjustment and the type of environmental change, and divide the trajectory adjustment into trajectory adjustment for static environmental changes and trajectory adjustment for dynamic environmental changes according to the type of environmental change; Specifically, this solution addresses the complex and ever-changing nature of high-altitude environments for live-line work by constructing a dynamic trajectory adjustment mechanism that integrates "real-time perception, risk assessment, tiered adjustment, and closed-loop verification." This mechanism enables precise trajectory adjustments based on environmental changes and the different types and risk levels of obstacles, ensuring safety throughout the entire operation.

[0059] The trajectory adjustment in this plan is based on the core principle of live-line working safety, and comprehensively considers multi-dimensional real-time data as the basis for adjustment, specifically including: (1) Legal red line basis: the minimum safe distance threshold specified in the 10kV live working procedure, any adjusted trajectory must meet this requirement; (2) Real-time perception basis: static environmental change parameters (newly identified fixed obstacles), dynamic environmental change parameters (position, speed and direction of swinging wires and drifting foreign objects), and real-time distance parameters between the robotic arm and the target are collected in real time by the environmental perception module. (3) Positioning data basis: real-time updates of the current position of the robotic arm, the position of the target, the precise positioning information of obstacles / power lines, and the motion trajectory prediction data of dynamic obstacles by the multi-source fusion positioning module; (4) Robotic arm constraint basis: physical constraints such as the joint rotation range, maximum movement speed, and reachable space of the robotic arm to ensure that the adjusted trajectory can be executed accurately; (5) Trajectory status basis: the currently executed optimal trajectory node sequence, velocity curve parameters, and the current motion state of the robotic arm.

[0060] Based on the type of environmental change, this plan divides trajectory adjustments into two main categories: trajectory adjustments for static environmental changes and trajectory adjustments for dynamic environmental changes.

[0061] S32, Based on the trajectory adjustment type and the current risk trigger level under that trajectory adjustment type, determine the trajectory adjustment strategy for different risk trigger levels.

[0062] Specifically, the trajectory adjustment for static environmental changes: Static environmental changes refer to newly identified fixed static obstacles (such as hardware, clamps, fixed equipment, etc. that were not modeled in advance) during the operation. These obstacles are fixed in position and do not move, which is a common type of environmental change at live-line working sites. The specific adjustment process is as follows: Step S3211, Adjust the trigger and risk level determination: (1) Triggering conditions: The environmental perception module continuously scans the work space, identifies new static obstacles, and the multi-source fusion positioning module calculates that the minimum distance between the obstacle and the current trajectory is less than 1.0 times the safety distance threshold, or the obstacle is located on the current trajectory path and there is a risk of collision, and the adjustment process is immediately triggered.

[0063] (2) Risk level determination: Level 1 risk: Minimum distance < 0.4m, with risk of direct collision / electric shock / short circuit; Level 2 risk: 0.4m ≤ minimum distance < 0.7m, indicating a risk of insufficient safe distance; Level 3 risk: Minimum distance ≥ 0.7m, no direct risk, only continuous monitoring required.

[0064] Step S3212, Real-time updating of the environmental map: (1) The multi-source fusion positioning module accurately locates newly identified obstacles and outputs their three-dimensional coordinates, contour boundaries, and size parameters. The positioning accuracy is ≥ ±1cm. (2) Update obstacle parameters to the three-dimensional grid map of the work space in real time, re-divide restricted areas, warning areas and safe areas, and update grid path value synchronously.

[0065] Step S3213, graded trajectory adjustment is executed: (1) Level 1 risk adjustment strategy: immediate emergency stop + global trajectory replanning.

[0066] Step 1: Immediately issue an emergency stop command to drive the robotic arm to stop moving and trigger an audible and visual warning to prevent accidents from occurring; Step 2: Using the current stopping position of the robotic arm as the new starting point and the original target position as the ending point, the improved A* algorithm is called again to generate a new initial trajectory based on the updated grid map; Step 3: Use the particle swarm optimization algorithm to optimize the new initial trajectory and generate the globally optimal trajectory and velocity curve that meet the safety constraints; Step 4: Perform collision detection, safety distance verification, and accessibility verification on the new trajectory. Once all verifications are passed, issue control commands to drive the robotic arm to operate along the new trajectory.

[0067] (2) Secondary risk adjustment strategy: local trajectory optimization adjustment.

[0068] Step 1: The robotic arm maintains its current motion state while reducing the speed of the corresponding segment, allowing time for adjustment; Step 2: Lock the local trajectory segments of 5 trajectory nodes before and after the risk point. Using the start and end points of the local segments as fixed boundaries, call the particle swarm optimization algorithm to adjust the coordinates of the local nodes, avoid the risk area, and at the same time ensure a smooth connection with the global trajectory. Step 3: Perform a safety check on the adjusted local trajectory. Once it passes the check, update it to the global trajectory and then send it out for execution.

[0069] (3) Level 3 risk adjustment strategy: continuous tracking, without adjusting the trajectory.

[0070] Continuously track the distance between the obstacle and the current trajectory, and only fine-tune the speed curve of the corresponding segment to reduce the movement speed, being prepared to make adjustments at any time, until the robotic arm completely passes through the area.

[0071] Step S3214: Closed-loop verification after adjustment.

[0072] After the adjustment is completed, during the continuous verification of the adjusted trajectory execution, the minimum distance to obstacles always meets the safety requirements. At the same time, the adjustment data is recorded and the operation rule base is updated to provide a reference for similar scenarios in the future.

[0073] On the other hand, in the trajectory adjustment for dynamic environmental changes, dynamic environmental changes refer to targets whose motion state changes in real time during the operation (such as conductor swaying caused by wind, foreign objects drifting from high altitudes, equipment displacement caused by platform shaking, etc.). These targets are the core safety risk sources at live-line work sites, and the specific adjustment process is as follows: Step S3221, Adjust triggering and dynamic target tracking prediction: (1) Triggering conditions: The environmental perception module identifies the dynamic target through continuous frame data, calculates its speed and direction, and the multi-source fusion positioning module predicts its future trajectory. If there is a collision intersection with the current trajectory of the robotic arm, or the minimum distance is less than 1.5 times the safety threshold, the adjustment process is immediately triggered.

[0074] (2) Dynamic target tracking and prediction: The Kalman filter algorithm is used to process the continuous frame positioning data of dynamic targets, update their position, velocity, and acceleration parameters in real time, predict their motion trajectory within the next 3 seconds, and the prediction frequency is ≥30Hz. The minimum distance and collision time (TTC) between the dynamic target and the current trajectory are calculated as the core basis for risk level determination.

[0075] Step S3222, Dynamic Risk Level Determination: The criteria for determining different risk levels are shown in Table 2 below.

[0076] Table 2: Criteria for Determining Different Risk Levels

[0077] Step S3223, hierarchical dynamic trajectory adjustment is executed: (1) Level 1 risk adjustment strategy: emergency obstacle avoidance + emergency stop.

[0078] Step 1: Immediately issue an emergency obstacle avoidance command, drive the robotic arm to quickly retreat to the safe zone in the opposite direction of the current trajectory, and at the same time reduce the speed to 0 and trigger an audible and visual warning; Step 2: Continuously track the dynamic target until the risk is completely eliminated and the position is stable. Then, re-execute the global trajectory replanning to generate a new optimal trajectory and continue the operation. Step 3: If the dynamic target persists and the risk cannot be eliminated, issue a termination command to drive the robotic arm back to the initial safe position.

[0079] (2) Secondary risk adjustment strategy: real-time local trajectory rolling optimization.

[0080] Step 1: The robotic arm reduces its current speed to less than 0.1 m / s, while continuously updating the position and predicted trajectory of the dynamic target at a frequency of 30 Hz; Step 2: The Model Predictive Control (MPC) algorithm is adopted, with the predicted trajectory of the dynamic target as the rolling constraint, the current position of the robotic arm as the starting point, and the next node of the original trajectory as the temporary end point. Within each 10ms control cycle, the local trajectory nodes for the next 5 control cycles are re-optimized to ensure that the minimum distance between the optimized trajectory and the predicted trajectory of the dynamic target is always ≥1.0 times the safety threshold. Step 3: Simultaneously optimize the velocity curve of the local trajectory, reduce the motion speed and acceleration, and ensure that the robotic arm can respond to changes in the target at any time; Step 4: Perform a safety check on the optimized trajectory within each control cycle. Once the check passes, immediately send it out for execution to achieve real-time rolling optimization of the trajectory. Step 5: Once the risk is eliminated, if the current position of the robotic arm deviates slightly from the original trajectory, it will smoothly transition back to the original trajectory; if the deviation is large, global replanning will be performed again.

[0081] (3) Three-level risk adjustment strategy: speed fine-tuning + continuous tracking. Without changing the current global trajectory, only the velocity curve of the corresponding segment is finely adjusted to reduce the maximum speed to within 0.2 m / s, thereby improving the response margin; Continuously track the dynamic target status. If the risk level escalates, immediately switch to the corresponding adjustment strategy; if the risk is eliminated, resume the original speed curve and trajectory execution.

[0082] Step S3224: Dynamically adjusted closed-loop tracking and optimization.

[0083] During the adjustment process, the system continuously updates the dynamic target status at 10ms intervals, repeatedly executing the closed-loop process of "prediction-judgment-optimization-execution" until the robotic arm reaches the task target or the risk is completely eliminated. After the task is completed, the entire adjustment process data is recorded, the dynamic target prediction algorithm and trajectory adjustment parameters are optimized, and the system's responsiveness to dynamic environments is improved.

[0084] Preferably, in the trajectory optimization control module 4, such as Figure 4 As shown, based on positioning information, environmental parameters, and preset target positions, the motion trajectory of the robotic arm is planned and optimized, and trajectory control commands are generated. Specifically, this may also include: S4, after optimizing the motion trajectory of the robotic arm, uses a 7-segment S-shaped speed curve to adjust the motion speed curve of the robotic arm.

[0085] Specifically, the adjustment of the robotic arm's motion speed curve is achieved by using a 7-segment S-shaped speed curve, including: S41, Based on the electrified scenario, determine the adjustment criteria for the 7-segment S-shaped velocity curves; the adjustment criteria specifically include: core safety criteria, trajectory geometry criteria, robotic arm constraint criteria, real-time environment criteria, and operational accuracy criteria; the 7-segment S-shaped velocity curves include acceleration segment velocity curve, uniform acceleration segment velocity curve, deceleration segment velocity curve, uniform speed segment velocity curve, acceleration / deceleration segment velocity curve, uniform deceleration segment velocity curve, and deceleration / deceleration segment velocity curve; Specifically, this solution addresses the safety requirements of live-line working scenarios and the motion characteristics of the robotic arm by employing a 7-segment S-shaped speed curve based on safety level segmentation and self-adaptation. This enables precise adjustment of the motion speed curve and solves the problems of fixed speed curves, easy jamming, and uncontrollable speed when approaching live objects in existing technologies.

[0086] The speed curve adjustment in this solution is entirely based on the core requirements of live-line working scenarios, prioritizing safety and incorporating multiple parameters as the basis for adjustment. Specifically, it includes: (1) Core safety basis: The minimum safe distance threshold of the 10kV live-line working safety regulations. The minimum distance between the trajectory node and the live conductor or obstacle is the core basis. The closer to the live conductor, the lower the speed limit, to ensure the safety of the operation; (2) Geometric basis of trajectory: curvature and inflection point distribution of the optimized trajectory. The greater the curvature and the greater the change in turning angle, the lower the upper limit of speed and the smaller the jerk, so as to avoid impact and jamming in the movement of the robotic arm; (3) Robotic arm constraint basis: The maximum angular velocity, angular acceleration and jerk of each joint of the robotic arm are hard constraints. All parameters of the speed curve must not exceed the limit to avoid motor overload and motion loss. (4) Real-time environmental basis: obstacle distance and environmental interference data collected in real time by the environmental perception module. If an obstacle is detected to be approaching or environmental interference is detected to be increasing, the speed of the corresponding segment will be reduced immediately, or even an emergency stop will be triggered; (5) Operational accuracy basis: The positioning accuracy requirement of this scheme is ±0.5cm. In the operation area near the target, an extremely low speed limit is set to avoid overshoot and positioning deviation due to excessive speed.

[0087] S42, based on the adjustment criteria, the optimized motion trajectory is segmented, and a corresponding velocity constraint is set for each segment; for each trajectory segment, the total length, maximum curvature, and minimum distance to the charged body are extracted as the basic parameters for generating the velocity curve; Specifically, the acceleration curve is continuous and without abrupt changes, which can completely avoid the robot arm's movement jamming and impact. Based on the above adjustment criteria, the globally optimal trajectory after particle swarm optimization is segmented, and a corresponding velocity constraint is set for each segment. Specifically: (1) Trajectory segmentation: Based on safety distance and curvature characteristics, the trajectory is divided into 4 types of segments: Safe section: A smooth section of track with a distance of >0.7m between the track node and the charged body, a curvature of <0.1rad / m, and no obstacles or inflections; Warning section: The distance between the trajectory node and the charged body is 0.4m~0.7m, or the curvature is 0.1~0.5rad / m, and there are obstacles to bypass or small-angle turning points in the road section; Inflection point / curvature change section: The track curvature is >0.5 rad / m, and there are road sections with large-angle turns and sudden changes in joint angles; Work section: The distance between the trajectory node and the work target is less than 0.1m, which is the precise positioning section for the final operation.

[0088] (2) Feature extraction: For each trajectory segment, extract the total length, maximum curvature, minimum distance to the charged body, and starting / ending velocity constraints as the basic parameters for generating the velocity curve.

[0089] S43, based on the adjustment criteria, sets differentiated speed constraint parameters for each type of trajectory segment; Specifically, based on the adjustment criteria, differentiated speed constraint parameters were set for each type of trajectory segment. All parameters were verified through 10kV live-line working field simulation and testing, as shown in Table 3 below: Table 3: Correspondence Table of Trajectory Segmentation and Trajectory Velocity Constraints

[0090] It should be noted that the parameters in the table can be dynamically adjusted according to the operating voltage level and the model of the robotic arm, and this embodiment does not impose any restrictions on them.

[0091] S44, for each trajectory segment, based on the set velocity constraints, total length of the trajectory segment, and starting / ending velocity constraints, solve for the time parameters of each segment of the 7 S-shaped velocity curves; Specifically, for each trajectory segment, based on the set velocity constraints, total trajectory segment length, and start / end point velocity constraints, the time parameters of each segment of the 7 S-shaped velocity curves are solved. The specific solution logic is as follows: (1) Boundary conditions are set: the starting velocity of each segment is the ending velocity of the previous segment, and the ending velocity is the starting velocity of the next segment, so as to achieve smooth connection of velocities between segments; the total displacement of the trajectory segment is equal to the total length of the trajectory segment.

[0092] (2) Solving by scenario: If the trajectory segment is long enough, the preset maximum speed v can be achieved. max Then, a complete 7-segment S-shaped velocity curve is generated, and the acceleration time t1, uniform acceleration time t2, deceleration time t3, uniform speed time t4, acceleration / deceleration time t5, uniform deceleration time t6, and deceleration / deceleration time t7 are solved to satisfy the total displacement constraint and the limit constraints of velocity, acceleration, and jerk. If the trajectory segment is too short to reach the preset maximum speed, the uniform speed segment t4 is omitted, and a symmetrical 6-segment S-shaped velocity curve is generated to ensure that the total displacement matches the trajectory segment length. If the trajectory segment is extremely short (such as the inflection point segment), the uniform acceleration and deceleration segments are omitted, and only the acceleration, deceleration, acceleration-deceleration, and deceleration segments are retained to generate four S-shaped velocity curves, ensuring a smooth transition of velocity without abrupt changes.

[0093] S45. After the solution is completed, check whether the acceleration and jerk of the velocity curve are continuous to avoid the robot arm movement from getting stuck. Specifically, after the solution is completed, a smoothness check is performed, that is, to check whether the acceleration and jerk of the velocity curve are continuous, to ensure that there are no sudden changes and to avoid the robot arm's movement from getting stuck.

[0094] S46 splices the velocity curves of each segment in the trajectory sequence to ensure that the starting / ending velocity and acceleration of adjacent segments are completely consistent, achieving a smooth connection of the velocity curves of the entire trajectory without any jumps between segments; S47 performs a global verification of the spliced ​​full-track velocity curve. Based on the full-track velocity curve, it generates the corresponding robotic arm end position, velocity, and acceleration commands, as well as joint space rotation, angular velocity, and angular acceleration commands for each interpolation cycle.

[0095] Specifically, the spliced ​​full-track velocity curve is globally verified. Under the premise of not exceeding safety constraints and the limits of the robotic arm, the velocity parameters of the corresponding segments are finely adjusted (according to the preset adjustment step size) to balance safety and work efficiency. Based on the full-track velocity curve, the position, velocity, and acceleration commands of the robotic arm end effector, as well as the rotation angle, angular velocity, and angular acceleration commands of the joint space, are generated for each interpolation cycle at a 1ms interpolation cycle, providing a basis for subsequent execution.

[0096] Furthermore, this embodiment achieves precise execution and dynamic adjustment of the speed curve through a fully closed-loop execution method of "command issuance - closed-loop feedback - real-time correction". The specific process is as follows: (1) Pre-execution: Issuance and preprocessing of control commands: The trajectory optimization control module 4 sends the joint space interpolation command corresponding to the generated full trajectory speed curve to the drive motor controller of the execution adjustment module 5 via the CAN bus (baud rate 500kbps). The controller performs limit verification on the command and prepares to execute it after the verification is passed.

[0097] (2) Closed-loop execution: Position-velocity dual closed-loop feedback control: The execution adjustment module 5 adopts a dual closed-loop PID control algorithm with a position loop and a speed loop to achieve precise execution of the speed curve. The control cycle is consistent with the interpolation cycle (1ms). Position loop: The photoelectric encoder collects the actual rotation angle of each joint of the robotic arm in real time, compares it with the target rotation angle of the interpolation command to obtain the position deviation, and outputs the speed command through the position loop PID; Speed ​​Loop: The speed command output by the position loop is compared with the actual rotational speed collected by the motor encoder to obtain the speed deviation. The speed loop PID output current command drives the servo motor to move according to the preset speed curve. Synchronous tracking: Dual closed-loop control ensures that the actual motion trajectory and speed of the robotic arm are perfectly matched with the preset optimized trajectory and speed curve, with a positioning accuracy of ±0.5cm.

[0098] (3) Dynamic adjustment: Real-time speed correction based on environmental changes: During operation, the environmental sensing module continuously collects environmental parameters. If environmental changes occur, the speed curve is dynamically adjusted immediately. If a change in the position of an obstacle or charged body is detected ahead of the trajectory, and the distance to the warning zone is approaching, the velocity curve parameters of the corresponding segment are immediately adjusted to the warning zone constraint parameters, the maximum speed is reduced, and the interpolation command is updated synchronously. If a collision risk is detected and the vehicle is about to enter a restricted area, an emergency stop command will be immediately triggered, rapidly reducing the target speed to 0, while simultaneously issuing an audible and visual warning. If the actual position of the robotic arm deviates from the target position by more than ±0.5cm, the speed curve of the corresponding segment is immediately fine-tuned to reduce the movement speed and correct the position command to ensure positioning accuracy.

[0099] (4) Post-execution review and parameter optimization: After the task is completed, the system records the execution data of the speed curve (actual speed, acceleration, position deviation, etc.), analyzes and optimizes the speed constraint parameters of each segment, and continuously improves the adaptability and execution accuracy of the speed curve.

[0100] It should be noted that the trajectory optimization control module can use a microprocessor with a main frequency of 480MHz, possessing high-speed computing capabilities and the ability to quickly process real-time data transmitted from multiple modules; among which the improvements... The algorithm is specifically improved by introducing a heuristic function. The algorithm optimizes the search strategy through this heuristic function, effectively shortening the trajectory planning time and avoiding invalid searches. The specific parameters of the particle swarm optimization algorithm are set as follows: the number of particles is 50, the number of iterations is 100, the initial value of the inertia weight is 0.9, which is linearly decreased to 0.4 during the iteration process, and the learning factors c1 and c2 are both 2.0. This algorithm optimizes the motion speed, joint angle, motion time and other parameters of the initial trajectory, adjusts the motion speed curve (using an S-shaped speed curve to avoid stuttering caused by sudden speed changes), eliminates redundant actions, and transmits the generated control commands to the execution and adjustment module through the CAN bus with a transmission baud rate of 500kbps to ensure the real-time performance and stability of the command transmission.

[0101] Preferably, the execution adjustment module 5 includes a drive motor, a reduction mechanism, and a position feedback unit, which is mechanically connected to the robotic arm of the live-line working robot and electrically connected to the trajectory optimization control module. The drive motor receives trajectory control commands and drives the movement of each joint of the robotic arm through the reduction mechanism, so that the robotic arm moves along the optimized trajectory. The position feedback unit collects the actual movement position and posture information of the robotic arm in real time and feeds it back to the trajectory optimization control module to form a closed-loop control, ensuring that the robotic arm can accurately reach the working position and avoid positioning deviation. In the execution adjustment module 5, the drive motor is a servo motor with a rated power of 100W and a rated speed of 3000rpm, equipped with a planetary reduction mechanism with a reduction ratio of 1:50. The position feedback unit uses a photoelectric encoder with a resolution of 1024 lines to collect the joint angle and position information of the robotic arm in real time and feeds it back to the trajectory optimization control module to form a closed-loop control.

[0102] Preferably, the device may further include a power supply module, using a lithium battery pack as the power source, equipped with a charging management unit and a power detection unit, which are electrically connected to the multi-source fusion positioning module, trajectory optimization control module, execution adjustment module, and environmental sensing module, respectively. The charging management unit is used to protect the lithium battery pack during charging, extending battery life; the power detection unit monitors the battery level in real time, and when the level falls below a preset threshold, it issues an alarm signal to remind staff to charge the battery promptly, ensuring continuous and stable operation of the device. The power supply module uses a 12V / 20Ah lithium battery pack, and the charging management unit has overcharge, over-discharge, and short-circuit protection functions; the power detection unit uses current and voltage sensors to monitor the battery level in real time, and when the level falls below 20%, it issues an alarm via indicator light and wireless signal.

[0103] The working process between the various modules in this embodiment is as follows: (1). After the device is started, the power supply module supplies power to each module, and the environmental perception module begins to collect environmental parameters of high-altitude operations, including the position of live lines, towers, insulators and surrounding obstacles, and transmits them to the trajectory optimization control module. (2). The multi-source fusion positioning module collects the visual positioning signal, laser positioning signal and inertial navigation positioning signal of the robotic arm, and processes them through Kalman filtering algorithm to output accurate positioning information to the trajectory optimization control module; (3) The trajectory optimization control module receives precise positioning information and environmental parameters, and based on the preset target position, optimizes the trajectory by improving... The algorithm plans the initial motion trajectory, then optimizes the trajectory using a particle swarm optimization algorithm, and generates trajectory control commands. (4). The execution adjustment module receives the trajectory control command, drives the servo motor to move the robotic arm along the optimized trajectory, and the position feedback unit collects the actual position information of the robotic arm in real time and feeds it back to the trajectory optimization control module to adjust the control command and ensure that the robotic arm accurately reaches the working position. (5) During operation, the environmental perception module continuously collects environmental parameters. If environmental changes or obstacles are detected, the trajectory optimization control module adjusts the motion trajectory in real time to avoid collisions. The power supply module detects the power in real time to ensure stable operation of the device.

[0104] The environmental perception module, multi-source fusion positioning module, trajectory optimization control module, and execution adjustment module of this invention work together to automate the entire process of the robotic arm from positioning and trajectory planning to execution, eliminating the need for manual intervention, shortening operation time, and improving the overall efficiency of live-line work. It is applicable to various high-altitude live-line work scenarios. It effectively solves the problem of low efficiency and reliability of positioning and trajectory optimization of live-line work robot arms caused by existing technologies, and effectively improves the efficiency and reliability of positioning and trajectory optimization of live-line work robot arms.

[0105] The environmental perception module in this invention includes a camera, a lidar, and an ultrasonic sensor. The camera is used to collect the position parameters, contour parameters, and type parameters of the circuit components within the work area, the feature point parameters of the preset work target, and the image boundary parameters of the work area. The lidar is used to measure the distance parameters between the robotic arm and surrounding obstacles, charged bodies, and grounded bodies, the three-dimensional point cloud data of the work area, and the size and depth parameters of the obstacles. The ultrasonic sensor is used to measure the supplementary distance parameters of nearby obstacles and the obstacle detection parameters around the joints of the robotic arm, ensuring the reliability of the positioning and trajectory optimization of the robotic arm of the live-line working robot.

[0106] The multi-source fusion positioning module in this invention includes an integrated visual positioning unit, a laser positioning unit, and an inertial navigation positioning unit. The multi-source fusion positioning module uses a Kalman filter algorithm to fuse multiple real-time position signals and output positioning information. It integrates visual, laser, and inertial navigation positioning methods and eliminates signal drift and distortion through the Kalman filter algorithm, solving the problems of high environmental interference and low positioning accuracy of single automatic positioning methods. At the same time, it replaces manual remote control positioning, realizes autonomous and accurate positioning of the robotic arm, improves positioning efficiency and accuracy, and reduces the risk of manual operation.

[0107] In the technical solution of this invention, the trajectory optimization control module combines environmental perception information and robotic arm motion constraints, through improved... Algorithms and particle swarm optimization algorithms plan and optimize motion trajectories, eliminate redundant movements and motion stutters, avoid obstacles such as power lines, towers, and insulators, effectively prevent collisions between the robotic arm and equipment, improve the safety of live-line work, and reduce equipment damage and work interruptions.

[0108] In response to the complex and ever-changing nature of high-altitude environments during live-line work, the technical solution of this invention constructs a dynamic trajectory adjustment mechanism that integrates real-time perception, risk assessment, graded adjustment, and closed-loop verification. This mechanism can achieve precise trajectory adjustment based on environmental changes and the different types and risk levels of obstacles, ensuring safety throughout the entire operation.

[0109] In response to the safety requirements and motion characteristics of the robotic arm in live-line working scenarios, the technical solution of this invention adopts a 7-segment S-shaped speed curve based on safety level segmentation and self-adaptation to achieve precise adjustment of the motion speed curve. This solves the problems of fixed speed curves, easy jamming, and uncontrollable speed when approaching live objects in the prior art, and further improves the reliability of the positioning and trajectory optimization of the robotic arm in live-line working scenarios.

[0110] While the specific embodiments of the present invention have been described above in conjunction with the accompanying drawings, this is not intended to limit the scope of protection of the present invention. Those skilled in the art should understand that various modifications or variations that can be made by those skilled in the art without creative effort based on the technical solutions of the present invention are still within the scope of protection of the present invention.

Claims

1. A device for positioning and trajectory optimization of a robotic arm for live-line working, characterized in that, include: Environmental perception module, multi-source fusion positioning module, trajectory optimization and control module, execution adjustment module; The environmental perception module is used to collect environmental parameters for high-altitude live-line operations, including the location of line components and the intensity of environmental interference signals within the work area, and transmits the collected environmental parameters to the trajectory optimization control module; the multi-source fusion positioning module is used to collect multiple real-time position signals of the robotic arm, fuse the multiple real-time position signals, and output positioning information to the trajectory optimization control module. The trajectory optimization control module is used to receive positioning information and environmental parameters, and based on the positioning information, environmental parameters, and preset target position, to plan and optimize the motion trajectory of the robotic arm, generate trajectory control commands, and transmit them to the execution adjustment module; the execution adjustment module is used to receive the trajectory control commands, drive the robotic arm to move along the optimized trajectory, and enable the robotic arm to reach the work position.

2. The live-line working robot arm positioning and trajectory optimization device according to claim 1, characterized in that, The environmental perception module includes a camera, a lidar, and an ultrasonic sensor. The camera is installed at the end of the robotic arm and is used to collect the position parameters, contour parameters, and type parameters of the circuit components within the work area, the feature point parameters of the preset work targets, and the image boundary parameters of the work area. The circuit components include live lines, poles, crossarms, insulators, fittings, and switches. The lidar is installed on the top of the robot body and is used to measure the distance parameters between the robotic arm and surrounding obstacles, live conductors, and grounded conductors, as well as the three-dimensional point cloud data of the work area and the size and depth parameters of obstacles. The ultrasonic sensor is installed at the joints of the robotic arm and is used to measure the supplementary distance parameters of nearby obstacles and the obstacle detection parameters around the joints of the robotic arm.

3. The live-line working robot arm positioning and trajectory optimization device according to claim 2, characterized in that, The multi-source fusion positioning module includes an integrated visual positioning unit, a laser positioning unit, and an inertial navigation positioning unit. The visual positioning unit captures image features of the target using a camera to achieve initial positioning of the robotic arm. The laser positioning unit measures the distance and relative position between the robotic arm and the target using laser ranging technology. The inertial navigation positioning unit collects the robotic arm's motion posture, velocity, and acceleration information in real time. The multi-source fusion positioning module uses a Kalman filter algorithm to fuse multiple real-time position signals and outputs positioning information. The real-time position signals include visual positioning signals, laser positioning signals, and inertial navigation positioning signals.

4. The live-line working robot arm positioning and trajectory optimization device according to claim 2, characterized in that, Based on positioning information, environmental parameters, and preset target positions, the robot arm's motion trajectory is planned and optimized, and trajectory control commands are generated, specifically including: Based on the preset work targets and the location and contour parameters of obstacles in the live-line work area, a 3D raster map of the work space is input. According to the live-line work regulations, spatial boundaries are defined, dividing the area into different zones including restricted areas, warning zones, and safety zones. Based on the spatial boundaries of these different zones, an improved [system / mechanism] is used. The algorithm plans and generates the initial motion trajectory; The preset constraints are used as input to the fitness function of the particle swarm optimization algorithm, and the initial motion trajectory parameters are optimized by the particle swarm optimization algorithm. During the operation, the positioning information of the target in the live working area is continuously updated. If the swing of the conductor or the displacement of the obstacle is detected, resulting in the minimum distance from the current movement trajectory being less than the safety threshold, the trajectory adjustment process is immediately triggered.

5. The live-line working robot arm positioning and trajectory optimization device according to claim 4, characterized in that, Based on the location information and contour parameters of the target in the live-line working area, a 3D raster map of the working space is input. According to the live-line working procedures, spatial boundaries are defined, dividing the area into different zones including restricted areas, warning zones, and safety zones. Based on the spatial boundaries of these different zones, an improved [system / mechanism] is used. The algorithm for planning and generating the initial motion trajectory specifically includes: The positioning information and contour parameters of the target in the live-line working area collected by the environmental perception module are preprocessed to extract structured data that can be used for trajectory planning; Based on the preprocessed structured data, a three-dimensional raster map of the work space is constructed, and safety levels are classified in conjunction with the live-line work safety regulations to generate spatial boundaries and constraints for different areas of the initial trajectory. Based on the constructed 3D raster map, spatial boundaries and constraints of different regions, and using the improved... The algorithm plans and generates the initial motion trajectory; The feasibility of live-line work is verified on the generated initial trajectory to ensure that the trajectory meets safety specifications and robotic arm movement requirements; the feasibility verification includes safety distance verification, robotic arm accessibility verification, and trajectory continuity verification.

6. The live-line working robot arm positioning and trajectory optimization device according to claim 5, characterized in that, Based on the preprocessed structured data, a 3D raster map of the work space is constructed, and safety levels are classified according to the live-line working safety regulations. Specifically, spatial boundaries and constraints for different areas are generated for the initial trajectory, including: With the robotic arm base as the origin and the maximum working radius of the robotic arm as the boundary, a three-dimensional cubic working space is constructed, and the three-dimensional cubic working space is divided into several three-dimensional grids; Based on the structured target parameter set, each three-dimensional grid is assigned a label. Three-dimensional grids located within the preset target outline are labeled as obstacle grids, and three-dimensional grids in free space are labeled as free grids. Based on the live-line working specifications and combined with the target attributes corresponding to the three-dimensional grid, the safety level of the free grid is divided, and a differentiated path cost is set. The restricted area includes all obstacle grids, as well as three-dimensional grids connecting different phase charged bodies and free grids connecting charged bodies and grounded bodies. The path cost is set to infinity, and entry into the trajectory is absolutely prohibited. The warning zone includes free grids that are less than a first preset distance from charged or grounded objects. The path cost is set to 10 times the base cost value, and these grids are prioritized for avoidance during trajectory planning. The safe zone includes free grids that are more than a first preset distance from charged or grounded objects. The path cost is set to a base cost of 1, which is the priority area for trajectory planning.

7. The live-line working robot arm positioning and trajectory optimization device according to claim 5, characterized in that, Based on the constructed 3D raster map, spatial boundaries and constraints of different regions, and using the improved... The algorithm for planning and generating the initial motion trajectory specifically includes: Using the current 3D coordinates of the robotic arm's end effector as the planning start point S and the preset 3D coordinates of the target task as the planning end point G, initialize... Algorithm parameters; Determine improvements The algorithm's path cost function and heuristic function; the path cost function includes the actual cost and the heuristic function value, which is a weighted fusion of Euclidean distance and Manhattan distance; Based on the workspace parameters, algorithm search constraints are set, namely: the change in joint angle between adjacent nodes does not exceed a preset percentage of the maximum angle of a single joint; the search range only includes the reachable space of the robotic arm to avoid generating unreachable trajectories. Based on the initialized algorithm parameters, perform the improved... The algorithm uses heuristic search to generate an initial sequence of trajectory nodes.

8. The live-line working robot arm positioning and trajectory optimization device according to claim 4, characterized in that, The fitness function is as follows: F=ω1×F safe +ω2×F smooth +ω3×F eff +ω4×F limit Where F is the total fitness value of the particle; ω1 is the weighting coefficient of the safety constraint sub-item; F safe For safety constraints; ω2 is the weighting coefficient for trajectory smoothness; F smooth For trajectory smoothness, ω3 is the weighting coefficient for the operation efficiency sub-item; F eff ω4 is the weighting coefficient for the motion constraint sub-item; F is the sub-item for work efficiency. limit For motion constraint sub-items; Among them, the safety constraint sub-item F safe The specific calculation method is as follows: Where N is the total number of nodes in the optimized trajectory; D th D is the minimum safe distance threshold for live-line working. i λ is the minimum distance from the i-th node of the trajectory to the nearest charged body / obstacle; λ is the penalty coefficient. Among them, the trajectory smoothness sub-term F smooth The specific calculation method is as follows: in, Let θ be the angular acceleration of the robotic arm joint corresponding to the i-th node of the trajectory. i Let |i| be the joint angle vector corresponding to the i-th node; ||| is the 2-norm of the vector, representing the fluctuation amplitude of the joint angular acceleration; Among them, the work efficiency sub-item F eff The specific calculation method is as follows: Where L is the total length of the current trajectory, L max T represents the total length of the initial trajectory; T represents the total operation time of the current trajectory. max The total operation time for the initial trajectory; Among them, the motion constraint sub-term F limit The specific calculation method is as follows: Where M is the total number of joints in the robotic arm; Let be the rotation angle of the j-th joint corresponding to the i-th node of the trajectory. The maximum rotation angle of the j-th joint. Let be the minimum rotation angle of the j-th joint; Let be the angular velocity of the j-th joint corresponding to the i-th node of the trajectory. Let be the maximum angular velocity of the j-th joint.

9. The live-line working robot arm positioning and trajectory optimization device according to claim 4, characterized in that, The trajectory adjustment process specifically includes: Obtain the basis for adjustment and the type of environmental change, and based on the type of environmental change, divide the trajectory adjustment into trajectory adjustment for static environmental changes and trajectory adjustment for dynamic environmental changes; Based on the trajectory adjustment type and the current risk trigger level under that trajectory adjustment type, determine the trajectory adjustment strategy for different risk trigger levels.

10. A positioning and trajectory optimization device for a live-line working robot arm according to any one of claims 1-9, characterized in that, After optimizing the robotic arm's motion trajectory, the trajectory optimization control module uses a 7-segment S-shaped velocity curve to adjust the robotic arm's motion speed curve. Specifically, this adjustment includes: The adjustment criteria for the seven S-shaped velocity curves are determined based on the electrified scenario. The adjustment criteria specifically include: core safety criteria, trajectory geometry criteria, robotic arm constraint criteria, real-time environment criteria, and operational accuracy criteria. The seven S-shaped velocity curves include acceleration segment velocity curve, uniform acceleration segment velocity curve, deceleration segment velocity curve, uniform speed segment velocity curve, acceleration and deceleration segment velocity curve, uniform deceleration segment velocity curve, and deceleration and deceleration segment velocity curve. Based on the adjustment criteria, the optimized motion trajectory is segmented, and corresponding velocity constraints are set for each segment. For each trajectory segment, the total length, maximum curvature, and minimum distance to the charged body are extracted as the basic parameters for generating the velocity curve. Based on the adjustment criteria, differentiated velocity constraint parameters are set for each type of trajectory segment; For each trajectory segment, based on the set velocity constraints, total length of the trajectory segment, and starting / ending velocity constraints, the time parameters of each segment of the 7 S-shaped velocity curves are solved; After the solution is completed, check whether the acceleration and jerk of the velocity curve are continuous to avoid the robot arm from getting stuck. The velocity curves of each segment are spliced ​​together in the order of the trajectory to ensure that the starting / ending velocities and accelerations of adjacent segments are completely consistent, so as to achieve a smooth connection of the velocity curves of the entire trajectory without any jumps between segments; A global verification is performed on the spliced ​​full trajectory velocity curve. Based on the full trajectory velocity curve, the position, velocity, and acceleration commands of the robotic arm end effector, as well as the rotation angle, angular velocity, and angular acceleration commands in the joint space, are generated for each interpolation cycle.

Images