A mechanical arm trajectory obstacle avoidance control method for a complex distribution network operation environment

Abstract

The present application relates to the technical field of robot control, and especially relates to a mechanical arm trajectory obstacle avoidance control method for complex distribution network operation environment, comprising the following steps: collecting operation environment, mechanical arm body posture and geometric information of a grabbed target object through a multi-modal sensor, and performing time synchronization and coordinate system calibration on the information to obtain environment data in a unified reference system; based on the environment data, discretizing the operation space into a three-dimensional voxel grid, calculating the occupation probability of each voxel, and combining dynamic obstacle identification and motion prediction to generate a probability occupation model. In the present application, multi-modal perception is performed and unified reference system modeling is performed, which improves the problem that traditional mechanical arm trajectory control methods mostly adopt static environment assumption, and dynamic obstacle avoidance fails due to the inability to perceive real-time environmental dynamic changes and unify multi-source information benchmarks.

Description

Technical Field

[0001] This invention relates to the field of robot control technology, and in particular to a method for controlling the trajectory obstacle avoidance of a robotic arm in complex power distribution network operation environments. Background Technology

[0002] The working environment in power distribution networks is typically characterized by confined spaces, diverse and dynamically changing obstacles, and the overall space occupied by a robotic arm during cargo-carrying operations changes significantly due to variations in the size and posture of the object being grasped. Existing robotic arm trajectory control methods are mostly based on static environment modeling and the robotic arm's geometric model, lacking unified fusion processing of multi-source environmental information and effective prediction of dynamic obstacles, making it difficult to accurately assess the space occupancy during cargo-carrying operations.

[0003] Traditional robotic arm trajectory control methods mostly rely on static environment assumptions. Because they cannot perceive dynamic environmental changes in real time and provide a unified multi-source information benchmark, they suffer from dynamic obstacle avoidance failure. Summary of the Invention

[0004] To overcome the above shortcomings, this invention provides a robotic arm trajectory obstacle avoidance control method for complex power distribution network operation environments. It aims to improve the problem that traditional robotic arm trajectory control methods mostly adopt static environment assumptions, which cannot perceive dynamic environmental changes in real time and unify multi-source information benchmarks, thus causing dynamic obstacle avoidance failure.

[0005] In a first aspect, the present invention provides the following technical solution: a method for controlling the trajectory obstacle avoidance of a robotic arm in complex power distribution network operation environments, comprising the following steps:

[0006] S1. Perform perception and synchronization processing, including collecting the working environment, the posture of the robotic arm and the geometric information of the target object being grasped through multimodal sensors, and performing time synchronization and coordinate system calibration on the information to obtain environmental data under a unified reference system.

[0007] S2. Based on the environmental data, the work space is discretized into a three-dimensional voxel grid, the occupancy probability of each voxel is calculated, and a probability occupancy model is generated by combining dynamic obstacle recognition and motion prediction.

[0008] S3. Based on the robot arm's posture and the target object's geometric information, construct a joint envelope model of the robot arm and the target object, and perform multi-resolution representation;

[0009] S4. Based on the static electronic fence range of the pre-defined operation specifications, the joint envelope model, and the dynamic obstacle prediction results, generate a dynamic electronic fence that includes the object sweeping space.

[0010] S5. Using the probability occupancy model, joint envelope model and dynamic electronic fence as input, establish a trajectory optimization model, and solve for candidate control command sequences under constraints.

[0011] S6. Perform a secure projection on the candidate control command sequence to generate a final control command sequence that satisfies the discrete probability security constraint; if a control command sequence that satisfies the constraint cannot be generated, execute a preset backoff strategy;

[0012] S7. Execute the final control instruction sequence and repeat each step in each control cycle until the task is completed.

[0013] Preferably, the sensing and synchronization includes:

[0014] Point cloud data of the workspace is acquired using 3D LiDAR;

[0015] Acquire depth information of obstacle surfaces using a depth camera;

[0016] The joint angle and force information of the robotic arm are obtained by using a joint encoder and an end effector torque sensor.

[0017] The multimodal sensor data is synchronized in time, and a unified coordinate transformation is performed based on the coordinate system of the robotic arm base to obtain environmental observation data under a unified reference system.

[0018] Preferably, the environmental data and motion prediction include:

[0019] Map the environmental data to the three-dimensional voxel mesh;

[0020] The occupancy probability of each voxel is calculated recursively based on the Bayesian update method;

[0021] Identify dynamic obstacles based on changes in occupancy probability and extract their movement trajectories;

[0022] The future position of the dynamic obstacle is predicted based on the motion prediction model and updated to the probability occupancy model.

[0023] Preferably, the joint envelope modeling includes:

[0024] The geometric model of the robotic arm body is registered and merged with the geometric model of the grasped target object to generate the joint envelope model.

[0025] The joint envelope is mapped to a coarse-resolution voxel space for fast collision detection;

[0026] In areas with potential collision risks, a high-resolution model is invoked to perform accurate distance calculations.

[0027] Preferably, the generation of the dynamic electronic fence includes:

[0028] The static electronic fence range is used as the initial fence.

[0029] Based on the joint envelope model and the dynamic obstacle prediction results, the sweep space of the joint envelope within the prediction time window is calculated;

[0030] The dynamic electronic fence is generated by performing a spatial union operation between the swept space and the static electronic fence, and then performing a safety expansion of the resulting union area according to the perception error and prediction uncertainty.

[0031] Preferably, the trajectory optimization includes:

[0032] A trajectory optimization model is constructed based on the discretized dynamics model of the robotic arm;

[0033] The probability occupancy model, the joint envelope model, and the dynamic electronic fence are used as optimization input constraints.

[0034] The constraint condition is introduced such that the probability of intersection between the joint envelope and the dynamic electronic fence at any time is not lower than a preset safety threshold.

[0035] The candidate control command sequence is obtained by using the rolling time-domain model predictive control method.

[0036] Preferably, the rolling time-domain model predictive control method includes:

[0037] The solution result of the previous control cycle is used as the initial value for optimization in the current control cycle;

[0038] Within the prediction time domain, the state at each future moment is predicted based on the discretized dynamic model of the robotic arm, and the optimal control input sequence is calculated under the conditions of satisfying the probabilistic obstacle avoidance constraints and dynamic constraints.

[0039] The first control input in the control input sequence is applied to the robotic arm actuator to drive the robotic arm to move, and the above solution process is repeated in the next control cycle based on the latest environmental data and the state of the robotic arm.

[0040] Preferably, the final control command sequence includes:

[0041] A quadratic programming problem is constructed based on the candidate control command sequence, with the objective of minimizing the deviation between the control input and the candidate control command sequence;

[0042] The discrete probability safety constraint is introduced into the quadratic programming constraint to keep the trajectory safety probability not lower than the preset safety threshold.

[0043] Solve the quadratic programming problem to obtain the final control command sequence.

[0044] Preferably, the rollback strategy includes:

[0045] When the quadratic programming problem has no feasible solution, a preset backoff strategy is executed. The backoff strategy includes deceleration, maintaining the current posture, or moving to a preset safe position.

[0046] Secondly, this invention provides the following technical solution: a robotic arm trajectory obstacle avoidance control system for complex power distribution network operation environments, comprising the following modules:

[0047] The multimodal perception module is used for perception and synchronization processing, including collecting geometric information of the working environment, the posture of the robotic arm and the target object being grasped through multimodal sensors, and performing time synchronization and coordinate system calibration on the information to obtain environmental data under a unified reference system.

[0048] The environment modeling module is used to discretize the work space into a three-dimensional voxel grid based on the environmental data, calculate the occupancy probability of each voxel, and generate a probability occupancy model by combining dynamic obstacle recognition and motion prediction.

[0049] The envelope modeling module is used to construct a joint envelope model of the robotic arm and the target object based on the robotic arm's posture and the target object's geometric information, and to perform multi-resolution representation.

[0050] The electronic fence generation module is used to generate a dynamic electronic fence that includes the space for carrying objects and sweeping, based on the static electronic fence range of the pre-defined operation, the joint envelope model, and the dynamic obstacle prediction results.

[0051] The trajectory optimization module is used to take the probability occupancy model, the joint envelope model and the dynamic electronic fence as inputs, establish a trajectory optimization model and solve for candidate control command sequences under constraints.

[0052] The safety projection module is used to perform safety projection on the candidate control command sequence to generate a final control command sequence that satisfies the discrete probability safety constraints; when a control command sequence that satisfies the constraints cannot be generated, a preset backoff strategy is executed.

[0053] The execution module is used to execute the final control instruction sequence and repeat each step in each control cycle until the task is completed.

[0054] The present invention has the following beneficial effects:

[0055] 1. In this invention, multimodal perception and unified reference system modeling are performed, and then the closed-loop control of trajectory optimization and safe projection is achieved by combining probabilistic occupancy model, dynamic obstacle prediction, joint envelope and dynamic electronic fence. This improves the problem that most traditional robotic arm trajectory control methods adopt static environment assumptions and cannot perceive dynamic changes in the environment in real time and unify multi-source information benchmarks, thus causing dynamic obstacle avoidance failure.

[0056] 2. In this invention, the working space is discretized into a three-dimensional voxel grid, the occupancy probability of each voxel is calculated, and a probability occupancy model is generated by combining dynamic obstacle motion prediction. This model is used as the input constraint for trajectory optimization, so that the robotic arm trajectory planning can make decisions based on the probability distribution of environmental occupancy. This improves the problem that traditional trajectory obstacle avoidance methods mostly use deterministic environmental modeling, which does not consider the uncertainty of environmental perception, resulting in insufficient obstacle avoidance safety.

[0057] 3. In this invention, a joint envelope model is constructed based on the posture of the robotic arm and the geometric information of the target object, and multi-resolution representation is performed. This model is then used for dynamic electronic fence generation and trajectory optimization constraints, thereby achieving accurate collision detection of the overall spatial occupancy of the robotic arm and the target object being grasped. This improves upon the problem that traditional obstacle avoidance control methods mostly only consider the geometric information of the robotic arm itself, and that the impact of the target object being grasped on spatial occupancy is ignored, which increases the risk of collision during object-carrying operations.

[0058] 4. In this invention, a dynamic electronic fence containing the sweeping space is generated based on the static electronic fence combined with the envelope model and the dynamic obstacle prediction results, and is used as a trajectory optimization constraint. This allows for the definition of a safety boundary that dynamically adjusts with the environment and working state for the robotic arm operation. This improves upon the problem that traditional electronic fence control methods mostly use fixed boundary settings, which cannot adapt to the robotic arm's carrying motion and dynamic obstacle changes, resulting in low utilization of the working space or failure of the safety boundary. Attached Figure Description

[0059] Figure 1 This is a flowchart of a robotic arm trajectory obstacle avoidance control method for complex power distribution network operation environments proposed in this invention.

[0060] Figure 2 This is a schematic diagram of the multimodal perception and synchronization process of a robotic arm trajectory obstacle avoidance control method for complex power distribution network operation environments proposed in this invention.

[0061] Figure 3 This is a schematic diagram of the environmental modeling and dynamic prediction process for a robotic arm trajectory obstacle avoidance control method for complex power distribution network operation environments proposed in this invention.

[0062] Figure 4This is a schematic diagram of the joint envelope modeling and collision detection process for a robotic arm trajectory obstacle avoidance control method for complex power distribution network operation environments proposed in this invention.

[0063] Figure 5 This is a schematic diagram of the dynamic electronic fence generation process for a robotic arm trajectory obstacle avoidance control method for complex power distribution network operation environments proposed in this invention.

[0064] Figure 6 This is a schematic diagram of the trajectory optimization and safety projection process of a robotic arm trajectory obstacle avoidance control method for complex power distribution network operation environments proposed in this invention.

[0065] Figure 7 This is a modular architecture diagram of a robotic arm trajectory obstacle avoidance control system for complex power distribution network operation environments proposed in this invention. Detailed Implementation

[0066] The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0067] Example 1

[0068] In a first embodiment of the present invention, the present invention provides a robotic arm trajectory obstacle avoidance control method for complex power distribution network operation environments, such as... Figures 1-6 As shown, it includes the following steps:

[0069] S1. Perform perception and synchronization processing, including collecting the working environment, the posture of the robotic arm and the geometric information of the target object being grasped through multimodal sensors, and performing time synchronization and coordinate system calibration on the information to obtain environmental data under a unified reference system.

[0070] Furthermore, perception and synchronization include:

[0071] Point cloud data of the workspace is acquired using 3D LiDAR;

[0072] Acquire depth information of obstacle surfaces using a depth camera;

[0073] The joint angle and force information of the robotic arm are obtained by using a joint encoder and an end effector torque sensor.

[0074] Multimodal sensor data is synchronized in time, and a unified coordinate transformation is performed based on the coordinate system of the robotic arm base to obtain environmental observation data under a unified reference system.

[0075] Specifically, a 3D LiDAR is used to scan the workspace to acquire 3D point cloud data describing the spatial structure of the work area. This data includes the 3D position coordinates of points in the LiDAR coordinate system and reflection intensity information, which can be used to reconstruct the spatial distribution and shape characteristics of objects in the environment. This information is the foundation for building a spatial geometric model and identifying obstacle boundaries.

[0076] A depth camera is used to image the surface of obstacles in the workspace, obtaining the depth value corresponding to each pixel. This information can provide fine geometric details of the obstacle surface at close range, which can be used to supplement the insufficient accuracy of 3D LiDAR when detecting specific materials or low-reflectivity surfaces, thereby improving the completeness and accuracy of the overall environment model.

[0077] The robot arm's attitude information at the current moment is obtained by acquiring the angle values ​​of each joint through joint encoders; simultaneously, the force and torque values ​​of the end effector in the three axes are acquired through end effector force and torque sensors. This data can be used to calculate the robot arm's specific position in the workspace and its relative attitude to the grasped target object, and to provide accurate geometric input for subsequent joint envelope modeling.

[0078] Time synchronization processing is performed on the data streams output by multimodal sensors to ensure that environmental and attitude information from different sensors reflects the operational status at the same moment. Time synchronization avoids spatial registration errors caused by sampling delays, thereby ensuring the accuracy of multi-source data fusion.

[0079] The time-synchronized multimodal sensor data is transformed into a unified reference coordinate system with the robotic arm base as the origin. This process utilizes known sensor extrinsic parameters to perform spatial transformation between coordinate systems, ensuring that the data output by all sensors is expressed under the same coordinate reference, which facilitates subsequent spatial occupancy modeling and collision detection.

[0080] After the above processing, an environmental observation dataset under a unified reference frame is obtained, which includes three-dimensional point cloud information of the workspace, obstacle surface depth information, robotic arm posture information, and end-effector force information. This data serves as the input for subsequent environmental modeling and trajectory optimization control, directly affecting the safety and reliability of trajectory obstacle avoidance.

[0081] By acquiring 3D structural information of the workspace, obstacle surface depth information, and robotic arm posture and end-effector force information through 3D LiDAR, depth cameras, joint encoders, and end-effector force and torque sensors, and through time synchronization and unified coordinate system calibration, the multi-source perception results are unified into a coordinate system with the robotic arm base as the reference, thus forming an environmental observation dataset that is consistent in both space and time. This dataset provides an accurate and complete input foundation for environmental modeling, dynamic obstacle prediction, joint envelope generation, and trajectory optimization, ensuring the effectiveness and reliability of obstacle avoidance planning in dynamic and complex work environments.

[0082] S2. Based on environmental data, the work space is discretized into a three-dimensional voxel grid, the occupancy probability of each voxel is calculated, and a probability occupancy model is generated by combining dynamic obstacle recognition and motion prediction.

[0083] Furthermore, environmental data and motion prediction include:

[0084] Map environmental data onto the three-dimensional voxel mesh;

[0085] The occupancy probability of each voxel is calculated recursively based on the Bayesian update method;

[0086] Identify dynamic obstacles based on changes in occupancy probability and extract their movement trajectories;

[0087] The future positions of dynamic obstacles are predicted based on the motion prediction model and then updated to the probabilistic occupancy model.

[0088] Specifically, under a unified reference system, environmental observation data of the workspace is transformed into a probabilistic occupancy model that can be used for trajectory obstacle avoidance planning, and dynamic obstacle identification and future position prediction are realized to support subsequent trajectory optimization and dynamic electronic fence generation.

[0089] Environmental observation data under a unified reference frame is mapped onto a three-dimensional voxel mesh, and the operating space is defined. According to a fixed resolution Divided into voxel units ,in Voxels in Orientation index. Each voxel stores the occupancy probability information for that spatial cell. Input is environmental observation data. (Includes 3D point cloud and depth information); output is a 3D voxel mesh structure. .

[0090] The occupancy probability is calculated recursively, and the Bayesian update method is used to recursively estimate the voxel occupancy state. Let the ... Time voxels The probability of occupancy is The likelihood of the occupancy state of this voxel based on the observation data is: The updated formula is: ;in This indicates the probability that the voxel is observed to be occupied; This indicates the probability that the observed voxel is empty; This represents the occupancy probability at the previous time step. This update process is repeated within each control cycle to obtain the probability distribution of job space occupancy at the current time step. The input is the occupancy probability at the previous time step. Current observation data The output is the updated occupancy probability. .

[0091] Dynamic obstacle recognition and trajectory extraction analyze the changes in occupancy probability over multiple consecutive time points: if the occupancy probability of a voxel or set of voxels changes significantly and continuously over time, a dynamic obstacle is identified at its corresponding spatial location; the spatial locations identified as the same obstacle at adjacent time points are connected to form a sequence of the obstacle's motion trajectory. ;in These represent the times when the obstacle was first and last detected, respectively. The input is the voxel occupancy probability distribution at each time step; the output is a set of dynamic obstacle trajectories. .

[0092] Dynamic obstacle motion prediction, based on the motion trajectory of dynamic obstacles, uses a motion prediction model to predict the obstacle's position over multiple future control cycles. Assume the obstacle is in... The position of the moment is ,predict The time position is Then the prediction model can be expressed as: ;in This is a motion prediction function, which can be implemented based on a linear motion model, a uniform acceleration model, or other dynamic models. The prediction results are updated in voxel form to the probabilistic occupancy model: for the voxels covered by the predicted location, their occupancy probability is adjusted accordingly within the prediction time window to reflect the space that the obstacle may occupy in the future. The input is the dynamic obstacle trajectory. Predict the time window length; the output is the updated probability occupancy model. .

[0093] After completing the above steps, the probability occupancy model for the current time and the prediction time window is obtained:

[0094] This model provides probabilistic spatial constraints for subsequent trajectory optimization and dynamic electronic fence generation, and can simultaneously consider the uncertain location distribution of static and dynamic obstacles in obstacle avoidance planning.

[0095] S3. Based on the robot arm's posture and the target object's geometric information, construct a joint envelope model of the robot arm and the target object, and perform multi-resolution representation;

[0096] Furthermore, joint envelope modeling includes:

[0097] The geometric model of the robotic arm body is registered and merged with the geometric model of the target object being grasped to generate a joint envelope model.

[0098] Map the joint envelope to a coarse-resolution voxel space for fast collision detection;

[0099] In areas with potential collision risks, a high-resolution model is invoked to perform accurate distance calculations.

[0100] Specifically, the geometric model of the robotic arm consists of multiple links and joints, and its geometry is defined in the coordinate system of the robotic arm base. The following can be represented as a set of polyhedra:

[0101] ;in Indicates the first A three-dimensional geometric model of a link. The number of links. The current posture of the robotic arm is determined by the joint angle vector. It is determined that the forward kinematic transformation matrix... It can calculate the spatial position and orientation of each link.

[0102] The geometric model of the target object, the geometric model of the target object being grabbed in the object coordinate system. The following is a set of polyhedra: ;in For the geometric components of an object, This represents the number of geometric components. The relative position of the object and the end effector of the robotic arm is determined by the gripper's attitude matrix. Provided.

[0103] Coordinate registration and merging transforms the geometric model of an object using a transformation matrix. From the object coordinate system Transform to the robot arm base coordinate system : The joint envelope model is represented as: The input is the robot arm's posture vector. 1. Geometric model of the robotic arm Geometric model of the target object gripper attitude matrix The output is a joint envelope model. .

[0104] Coarse-resolution voxelization will be used to model the joint envelope. Mapped to a resolution of 3D voxel mesh : ; where 1 indicates that it is occupied and 0 indicates that it is free.

[0105] Fast collision detection, using coarse resolution voxels Perform spatial Boolean operations with the environment probability occupancy model to determine if voxel overlap exists. If the overlapping voxel occupancy probability exceeds a set threshold... If the condition is met, then the region is determined to have a potential collision risk. The input is a joint envelope model. voxel resolution Environmental probability occupancy model; output is a set of potential collision risk areas. .

[0106] High-resolution, accurate distance calculation for potential collision risk areas Calling resolution is ( High-resolution geometric model This allows for precise collision detection and minimum distance calculation. The minimum distance calculation formula is: ;in Represents the point set on the surface of environmental obstacles. Representing the joint envelope model The three-dimensional coordinate vector of any point in space, Represents the set of environmental obstacles The coordinate vector of any surface point in the vector. If If the location is not specified, it is considered a real collision risk, and the relevant voxel position is passed to the trajectory optimization module as an obstacle avoidance constraint. The input is the risk area. High-resolution model Environmental obstacle point set Safe distance threshold The output is collision risk markers and constraint data.

[0107] By registering and merging the geometric information of the robotic arm and the grasped target, a joint envelope model that can be updated according to the robotic arm's posture is constructed. A collision detection strategy of first coarse detection and then precise calculation is implemented through a multi-resolution method. While ensuring collision detection accuracy, the number of high-resolution calculations is reduced, improving the overall computational efficiency and providing spatial constraint information that balances accuracy and real-time performance for trajectory optimization.

[0108] S4. Based on the static electronic fence range of the pre-defined operation specifications, the joint envelope model, and the dynamic obstacle prediction results, a dynamic electronic fence including the object sweeping space is generated.

[0109] Furthermore, the generation of dynamic electronic fences includes:

[0110] Use the static electronic fence area as the initial fence;

[0111] Based on the joint envelope model and dynamic obstacle prediction results, the sweep space of the joint envelope within the prediction time window is calculated.

[0112] The swept space and the static electronic fence are spatially joined, and the resulting joined area is safely expanded according to the perception error and prediction uncertainty to generate a dynamic electronic fence.

[0113] Specifically, static electronic fences are defined by pre-setting static electronic fence areas according to operational safety regulations. This area is used to limit the range of motion of the robotic arm and the object it carries. This area is defined in the robotic arm's base coordinate system. The following is a set of closed spaces with fixed boundaries. The input is the static electronic fence boundary data provided by the pre-defined work specifications; the output is the static electronic fence spatial set. .

[0114] Joint envelope trajectory generation based on the current robot arm posture and candidate trajectories In the prediction time window Internal calculation of the joint envelope model at each time step: ;Sweep space definition sweep space This represents the set of all spatial locations covered by the joint envelope within the prediction time window: The joint envelope position simultaneously considers the impact of dynamic obstacle prediction results on the trajectory, i.e., the trajectory Need to avoid predicted obstacle locations The input is a joint envelope model. Dynamic obstacle prediction results Prediction Time Window The output is a swept space. .

[0115] The union operation formula will sweep the space. Compared with static electronic fences Perform a spatial union operation to obtain the expanded fence space: This calculation ensures that the fence covers the entire reachable space of the robotic arm within the prediction time window, including all possible poses when carrying an object. The input is... , The output is the expanded fence space. .

[0116] The expansion radius is defined by considering the uncertainties of sensor perception errors and dynamic obstacle prediction, and a safety expansion radius is introduced. From the upper bound of the perception error Upper bound of the predicted position error composition: The expansion calculation formula is used to expand the fenced space. Perform a three-dimensional dilation operation to obtain a dynamic electronic fence. :

[0117] The output is the expanded fence space. Expansion radius The output is a dynamic electronic fence. .

[0118] By combining the potential future space occupied by the robotic arm and the carried object with the safe operating range, a dynamic electronic fence is formed that can adapt to changes in the operating status in real time. Union operations ensure that the entire space for the carried object's movement is contained within the fence, and safety expansion processing improves the obstacle avoidance safety margin under conditions of perception and prediction uncertainty, providing dynamic and safe spatial boundary constraints for the trajectory optimization module.

[0119] S5. Using the probabilistic occupancy model, the joint envelope model, and the dynamic electronic fence as inputs, a trajectory optimization model is established, and candidate control command sequences are obtained by solving under constraints.

[0120] Furthermore, trajectory optimization includes:

[0121] A trajectory optimization model is constructed based on the discretized dynamics model of the robotic arm;

[0122] The probability occupancy model, the joint envelope model, and the dynamic electronic fence are used as optimization input constraints.

[0123] The constraint condition is introduced: the probability of the intersection between the joint envelope and the dynamic electronic fence at any time is not lower than a preset safety threshold.

[0124] Candidate control command sequences are obtained by using the rolling time-domain model predictive control method.

[0125] The rolling time-domain model predictive control method includes:

[0126] The solution result of the previous control cycle is used as the initial value for optimization in the current control cycle;

[0127] Within the prediction time domain, the state at each future moment is predicted based on the discretized dynamic model of the robotic arm, and the optimal control input sequence is calculated under the conditions of satisfying the probabilistic obstacle avoidance constraints and dynamic constraints.

[0128] The first control input in the control input sequence is applied to the robotic arm actuator to drive the robotic arm to move, and the above solution process is repeated in the next control cycle based on the latest environmental data and the state of the robotic arm.

[0129] Specifically, by using the probability occupation model Joint envelope model and dynamic electronic fences As an input constraint for the trajectory optimization model, it can simultaneously consider the overall geometric dimensions of the robotic arm and the target object being grasped, the distribution of static and dynamic obstacles in the working environment, and the spatial range of the electronic fence during the trajectory planning process, thereby constraining and controlling possible collision risks during the planning stage.

[0130] The trajectory optimization model is established based on the discretized dynamics model of the robotic arm, ensuring that the optimization results satisfy the kinematic and dynamic constraints of the robotic arm and guaranteeing the physical feasibility of the output trajectory. The discretized dynamics model can be expressed as: ;in Let k be the state vector at time k, including the joint positions. and joint velocity ; The control input at time k; This is the state update function obtained by discretizing the dynamic equations of the robotic arm. The optimization framework established through this dynamic model ensures that the planning results satisfy the kinematic and dynamic characteristics of the robotic arm, guaranteeing the effectiveness of the trajectory in terms of physical feasibility.

[0131] Objective function definition: The objective function for trajectory optimization comprehensively controls energy consumption and trajectory smoothness.

[0132] ;in To control the input weight matrix; This is the trajectory smoothness weight matrix; To predict the off-track number in the time domain. This objective function ensures that, while satisfying obstacle avoidance constraints, the trajectory has a low execution load and continuous variation, reducing the impact on the drive system.

[0133] Dynamic consistency, ;

[0134] Probabilistic obstacle avoidance constraints ;in A preset safety probability threshold is set. This constraint ensures that trajectory planning not only avoids obstacles in geometric space, but also guarantees the safety of the trajectory execution process in a probabilistic sense, adapting to perception errors and prediction uncertainties.

[0135] Joint and input limitations ; ; The inputs are environmental model data, the range of motion of the robotic arm, and the physical limits of the actuator; the output is a set of constraints to ensure that the trajectory is executed under the requirements of physical feasibility and safety probability.

[0136] To improve real-time performance, a rolling time-domain model predictive control (MPC) method is employed for solution. In each control cycle:

[0137] The optimal solution from the previous control cycle is used as the initial value for the current optimization to improve the convergence speed. Based on the current state of the robotic arm and environmental perception data, the state trajectory at the next N time steps is predicted, and the optimal control input sequence is calculated under the conditions of satisfying dynamic constraints and probabilistic obstacle avoidance constraints. The first control variable in the obtained control input sequence is used as the initial value. It acts on the robotic arm's actuator to drive the robotic arm's movement; the process is repeated in the next cycle to achieve dynamic adjustment of the trajectory.

[0138] The input data includes an environmental probability occupancy model, a joint envelope model, a dynamic electronic fence range, the current state vector of the robotic arm, and historical optimization solutions. The output data consists of a sequence of candidate control commands that satisfy both probabilistic safety constraints and dynamic constraints. This method enables the trajectory optimization process to dynamically adapt to changes in the environment and operational state, improving the safety and feasibility of robotic arm trajectory execution in complex power distribution network environments.

[0139] S6. Perform a secure projection on the candidate control command sequence to generate a final control command sequence that satisfies the discrete probability security constraint; if a control command sequence that satisfies the constraint cannot be generated, execute a preset backoff strategy;

[0140] Furthermore, the final control command sequence includes:

[0141] A quadratic programming problem is constructed based on the candidate control command sequence, with the objective of minimizing the deviation between the control input and the candidate control command sequence;

[0142] Discrete probability safety constraints are introduced into the quadratic programming constraints to ensure that the trajectory safety probability is not lower than a preset safety threshold.

[0143] Solve the quadratic programming problem to obtain the final control command sequence.

[0144] Rollback strategies include:

[0145] When the quadratic programming problem has no feasible solution, a preset backoff strategy is executed. The backoff strategy includes deceleration, maintaining the current posture, or moving to a preset safe position.

[0146] Specifically, by performing safety corrections on the candidate control command sequence output by the trajectory optimization module, it is ensured that the final executed control command meets the preset safety threshold in a probabilistic sense, and potential risks are reduced through a backoff strategy when the constraints cannot be met.

[0147] By constructing a quadratic programming problem aimed at minimizing the deviation between the final control input and the candidate control command sequence, the trajectory can be adjusted as necessary to meet safety constraints while maintaining its feasibility. The introduction of discrete probability safety constraints ensures that, even in the presence of perceptual uncertainties or dynamic changes in obstacles, the safety probability of the robotic arm's trajectory is not lower than a preset safety threshold, thus ensuring that collision risks are controlled during operation.

[0148] When the secondary planning problem has no feasible solution, a backoff strategy is implemented, including deceleration, maintaining the current attitude, or moving to a preset safe position. This can quickly switch to a low-risk state when safety requirements cannot be met, preventing potential collisions or instability risks.

[0149] Input, candidate control instruction sequence This is a preliminary planned instruction sequence that needs to be revised to meet safety requirements; safety probability threshold. This is used to define acceptable levels of security risk; probability occupancy model This reflects the probability of each region in the environment being occupied; the joint envelope model. Integrating the geometric information of the robotic arm and the target object being grasped for collision detection-related analysis; dynamic electronic fence. It provides dynamically changing safety boundary constraints. The output is the final control command sequence. After being processed by a safety correction or rollback strategy, it is used to drive the robotic arm to operate safely.

[0150] The logic for constructing the quadratic programming model is to modify the control commands to meet safety requirements without significantly altering the original trajectory planning results, and to construct the following optimization problem: ;in: The current optimized control input corresponds to an element in the final control command sequence. These are candidate control inputs, i.e., instructions for the initial planning; The weight matrix is ​​used to balance the importance of each control component, ensuring the "fine-tuning" characteristic of the original plan during the correction process. This secondary planning aims to minimize the deviation between the final control input and the candidate control command, so that the corrected command is physically feasible and safe, while conforming to the initial planned trajectory to the greatest extent, reducing the impact on work efficiency and path consistency.

[0151] Probabilistic safety constraints This constraint defines the joint envelope model of the robotic arm and the object being grasped. Dynamic electronic fence and probability occupation model Deep integration. This requires that at any point during trajectory execution... The intersection probability of the joint envelope and the dynamic electronic fence must not be lower than the set safety threshold. Even with perception errors and uncertainties in predicting dynamic obstacles, probability constraints can effectively mitigate collision risks and ensure the safety of robotic arm operations.

[0152] Dynamics and input constraints ;in Represents the state vector of the robotic arm. This constraint, based on the physical characteristics of the robotic arm, limits the range of values ​​for state and control inputs, ensuring that commands remain within the actual executable capabilities of the robotic arm and preventing excessive movements that could damage the equipment or cause danger.

[0153] When the quadratic programming problem has no feasible solution, a backoff strategy is implemented to allow the robotic arm to quickly enter a low-risk state:

[0154] Deceleration: Gradually reduce the speed of each joint until the robotic arm reaches a safe state of motion. This reduces the possibility of collision by decreasing kinetic energy and narrowing the range of motion.

[0155] Maintain current posture: Freeze joint positions to keep the robotic arm in its current spatial pose, avoid new risks caused by blind movement, and create a stable environment for subsequent processing.

[0156] Move to a preset safe position: Move the robotic arm to a predefined collision-free safe posture to completely avoid risks from a spatial perspective, creating conditions for re-operation or troubleshooting.

[0157] The generation and value realization of results, and the final output of the control command sequence. follow:

[0158] This sequence is used to drive the robotic arm to perform operations. Through the logic of "planning correction + safety fallback", it not only ensures the physical feasibility and safety of the control commands during the trajectory execution stage, but also switches to the safety mode in a timely manner when the planning result is not feasible. This significantly improves the operational stability and safety of the robotic arm in complex power distribution network operation environments, and helps the robotic arm reliably complete tasks such as power distribution network operation and maintenance.

[0159] S7. Execute the final control instruction sequence and repeat each step in each control cycle until the task is completed.

[0160] Specifically, a continuous closed-loop control process is constructed by repeatedly executing each step and the final control command sequence in each control cycle. Based on real-time updates of multimodal perception, the process continuously adapts to the dynamic environment through steps such as environmental modeling, envelope construction, electronic fence generation, and trajectory optimization, ensuring that control commands are always safe and effective. This supports the robotic arm in complex power distribution network operation environments, from the start of the operation to the completion of the task, providing dynamic environmental adaptability and operational continuity, and ensuring the reliable completion of the operation task.

[0161] Example 2:

[0162] In complex power distribution network line inspection scenarios, intelligent inspection robots equipped with robotic arms need to perform visual inspections and simple maintenance on equipment such as insulators and clamps. The working environment includes dynamic obstacles caused by surrounding construction and vehicle traffic, as well as moving objects such as tree branches and passing birds, and static power distribution towers and building facilities. However, current technologies suffer from asynchronous data collection times and inconsistent coordinate systems from multiple sensors, making it difficult to fuse environmental information and accurately construct real-time dynamic environment models and predict the trajectories of dynamic obstacles. Furthermore, the combined envelope of the robotic arm and gripping tool struggles to accurately perform collision detection and safe distance control when operating in confined spaces within the power distribution network. Trajectory optimization struggles to balance work efficiency and safety probability constraints, and control commands are prone to incompatibility issues between physical feasibility and obstacle avoidance safety. Moreover, if the retreat strategy is not executed promptly or reasonably during sudden environmental changes, it can easily lead to the robotic arm colliding with equipment or interrupting the operation. These technical problems need to be solved to ensure operational continuity and safety. To address these issues, this invention provides a robotic arm trajectory obstacle avoidance control system for complex power distribution network working environments, the structure of which is as follows: Figure 7 As shown. The specific implementation process of this system is as follows:

[0163] The multimodal perception module is used for perception and synchronization processing, including collecting geometric information of the working environment, the posture of the robotic arm and the target object being grasped through multimodal sensors, and performing time synchronization and coordinate system calibration on the information to obtain environmental data under a unified reference system.

[0164] The environment modeling module is used to discretize the workspace into a three-dimensional voxel mesh based on environmental data, calculate the occupancy probability of each voxel, and generate a probabilistic occupancy model by combining dynamic obstacle recognition and motion prediction.

[0165] The envelope modeling module is used to construct a joint envelope model of the robotic arm and the target object based on the robotic arm's posture and the target object's geometric information, and to perform multi-resolution representation.

[0166] The electronic fence generation module is used to generate a dynamic electronic fence that includes the space for carrying objects and sweeping, based on the static electronic fence range of the pre-defined operation, the joint envelope model, and the dynamic obstacle prediction results.

[0167] The trajectory optimization module is used to take the probabilistic occupancy model, the joint envelope model and the dynamic electronic fence as inputs, establish a trajectory optimization model and solve for candidate control command sequences under constraints.

[0168] The safety projection module is used to perform safety projection on the candidate control command sequence to generate a final control command sequence that satisfies the discrete probability safety constraints; when a control command sequence that satisfies the constraints cannot be generated, a preset backoff strategy is executed.

[0169] The execution module is used to execute the final control instruction sequence and repeat each step in each control cycle until the job task is completed.

[0170] Specifically, the multimodal perception module uses multimodal sensors to collect geometric information about the working environment, the robot arm's posture, and the object being grasped. Through time synchronization and coordinate system calibration, it outputs environmental data in a unified reference frame. Its role is to provide accurate and consistent basic data for subsequent environmental modeling and envelope construction, ensuring that each stage processes data based on the same spatiotemporal reference. This guarantees the accuracy and coordination of the system's perception of the working scene and serves as the fundamental support for the data input of the entire technical solution.

[0171] The environment modeling module, based on the environmental data output by the multimodal perception module, discretizes the workspace into a three-dimensional voxel mesh, calculates the occupancy probability of each voxel, and generates a probabilistic occupancy model by combining dynamic obstacle recognition and motion prediction. This module, by constructing a model that reflects both static and dynamic changes in the environment, provides a basis for subsequently determining which areas in the robotic arm's workspace are passable and which contain obstacles. It is a core component of environmental cognition for achieving obstacle avoidance by the robotic arm, enabling the system to grasp the static occupancy status and dynamic trends of the work environment.

[0172] The envelope modeling module constructs a joint envelope model of the robotic arm and the target object based on the robotic arm's posture and the target object's geometric information, and performs multi-resolution representation. Its function is to integrate the robotic arm and the work object into a unified spatial positioning model. This multi-resolution representation can be flexibly invoked in different demand scenarios, providing accurate spatial positioning data for subsequent assessment of collision risks between the robotic arm / work object and environmental obstacles. It is a key model support for collision detection and safe operation.

[0173] The electronic fence generation module generates a dynamic electronic fence that includes the space for carrying and sweeping objects, based on the static electronic fence range specified in the pre-defined operation plan, the joint envelope model, and the dynamic obstacle prediction results. This module integrates the static safety boundary with the spatial sweeping behavior of the robotic arm and target object during dynamic operations, as well as the influence of dynamic obstacles, to construct a dynamically changing safe operating boundary. This defines a real-time, precise safe area for the robotic arm's operation, and is a core component in ensuring that the robotic arm does not overstep boundaries or collide with objects.

[0174] The trajectory optimization module takes the probabilistic occupancy model, the joint envelope model, and the dynamic electronic fence as inputs to establish a trajectory optimization model and solve for candidate control command sequences. Its function is to plan the trajectory commands for the robotic arm's operation while satisfying environmental and safety constraints, providing a preliminary, theoretically feasible sequence of motion commands for the robotic arm to perform its tasks. This is a crucial step in realizing autonomous trajectory planning for the robotic arm.

[0175] The safety projection module performs safety projection on the candidate control command sequence to generate the final control command sequence that satisfies the discrete probability safety constraints. This module further constrains the control commands to ensure that they can cope with perception errors, model uncertainties, and other situations during actual execution, truly possessing physical feasibility and safety reliability. It is a key correction step in transforming theoretically feasible trajectory commands into practically executable and safe commands.

[0176] The execution module executes the final control command sequence and repeats each step in each control cycle until the task is completed. On the one hand, it drives the robotic arm to complete the task by executing commands; on the other hand, the periodic repetition of each step enables the system to continuously acquire new environmental data, update the environmental model, and adjust the electronic fence and control commands to cope with the dynamic changes in the environment during the operation. This ensures that the robotic arm can safely and accurately perform the task throughout the entire operation cycle, which is the core link to achieve closed-loop execution and dynamic adaptation of the task.

[0177] Finally, it should be noted that the above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art can still modify the technical solutions described in the foregoing embodiments or make equivalent substitutions for some of the technical features. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A method for controlling the trajectory obstacle avoidance of a robotic arm in complex power distribution network operation environments, characterized in that, Includes the following steps: S1. Perform perception and synchronization processing, including collecting the working environment, the posture of the robotic arm and the geometric information of the target object being grasped through multimodal sensors, and performing time synchronization and coordinate system calibration on the information to obtain environmental data under a unified reference system. S2. Based on the environmental data, the work space is discretized into a three-dimensional voxel grid, the occupancy probability of each voxel is calculated, and a probability occupancy model is generated by combining dynamic obstacle recognition and motion prediction. S3. Based on the robot arm's posture and the target object's geometric information, construct a joint envelope model of the robot arm and the target object, and perform multi-resolution representation; S4. Based on the static electronic fence range of the pre-defined operation specifications, the joint envelope model, and the dynamic obstacle prediction results, generate a dynamic electronic fence that includes the object sweeping space. S5. Using the probability occupancy model, joint envelope model and dynamic electronic fence as input, establish a trajectory optimization model, and solve for candidate control command sequences under constraints. S6. Perform a secure projection on the candidate control command sequence to generate a final control command sequence that satisfies the discrete probability security constraint; if a control command sequence that satisfies the constraint cannot be generated, execute a preset backoff strategy; S7. Execute the final control instruction sequence, and repeat each step in each control cycle until the task is completed; The environmental data and motion predictions include: Map the environmental data to the three-dimensional voxel mesh; The occupancy probability of each voxel is calculated recursively based on the Bayesian update method; Identify dynamic obstacles based on changes in occupancy probability and extract their movement trajectories; The future position of the dynamic obstacle is predicted based on the motion prediction model and updated to the probability occupancy model; The joint envelope modeling includes: The geometric model of the robotic arm body is registered and merged with the geometric model of the grasped target object to generate the joint envelope model. The joint envelope is mapped to a coarse-resolution voxel space for fast collision detection; In areas with potential collision risks, a high-resolution model is used to perform accurate distance calculations. The generation of the dynamic electronic fence includes: The static electronic fence range is used as the initial fence. Based on the joint envelope model and the dynamic obstacle prediction results, the sweep space of the joint envelope within the prediction time window is calculated; The dynamic electronic fence is generated by performing a spatial union operation between the swept space and the static electronic fence, and then performing a safety expansion of the resulting union area according to the perception error and prediction uncertainty.

2. The robotic arm trajectory obstacle avoidance control method for complex power distribution network operation environments according to claim 1, characterized in that, The sensing and synchronization include: Point cloud data of the workspace is acquired using 3D LiDAR; Acquire depth information of obstacle surfaces using a depth camera; The joint angle and force information of the robotic arm are obtained by using a joint encoder and an end effector torque sensor. The multimodal sensor data is synchronized in time, and a unified coordinate transformation is performed based on the coordinate system of the robotic arm base to obtain environmental observation data under a unified reference system.

3. The robotic arm trajectory obstacle avoidance control method for complex power distribution network operation environments according to claim 1, characterized in that, The trajectory optimization includes: A trajectory optimization model is constructed based on the discretized dynamics model of the robotic arm; The probability occupancy model, the joint envelope model, and the dynamic electronic fence are used as optimization input constraints. The constraint condition is introduced such that the probability of intersection between the joint envelope and the dynamic electronic fence at any time is not lower than a preset safety threshold. The candidate control command sequence is obtained by using the rolling time-domain model predictive control method.

4. The robotic arm trajectory obstacle avoidance control method for complex power distribution network operation environments according to claim 3, characterized in that, The rolling time-domain model predictive control method includes: The solution result of the previous control cycle is used as the initial value for optimization in the current control cycle; Within the prediction time domain, the state at each future moment is predicted based on the discretized dynamic model of the robotic arm, and the optimal control input sequence is calculated under the conditions of satisfying the probabilistic obstacle avoidance constraints and dynamic constraints. The first control input in the control input sequence is applied to the robotic arm actuator to drive the robotic arm to move, and the above solution process is repeated in the next control cycle based on the latest environmental data and the state of the robotic arm.

5. The robotic arm trajectory obstacle avoidance control method for complex power distribution network operation environments according to claim 3, characterized in that, The final control command sequence includes: A quadratic programming problem is constructed based on the candidate control command sequence, with the objective of minimizing the deviation between the control input and the candidate control command sequence; The discrete probability safety constraint is introduced into the quadratic programming constraint to keep the trajectory safety probability not lower than the preset safety threshold. Solve the quadratic programming problem to obtain the final control command sequence.

6. The robotic arm trajectory obstacle avoidance control method for complex power distribution network operation environments according to claim 1, characterized in that, The execution of the preset rollback strategy includes: When the quadratic programming problem has no feasible solution, a preset backoff strategy is executed. The backoff strategy includes deceleration, maintaining the current posture, or moving to a preset safe position.

7. A robotic arm trajectory obstacle avoidance control system for complex power distribution network operation environments, characterized in that, A robotic arm trajectory obstacle avoidance control method for complex power distribution network operation environments, as described in any one of claims 1-6, includes the following modules: The multimodal perception module is used for perception and synchronization processing, including collecting geometric information of the working environment, the posture of the robotic arm and the target object being grasped through multimodal sensors, and performing time synchronization and coordinate system calibration on the information to obtain environmental data under a unified reference system. The environment modeling module is used to discretize the work space into a three-dimensional voxel grid based on the environmental data, calculate the occupancy probability of each voxel, and generate a probability occupancy model by combining dynamic obstacle recognition and motion prediction. The envelope modeling module is used to construct a joint envelope model of the robotic arm and the target object based on the robotic arm's posture and the target object's geometric information, and to perform multi-resolution representation. The electronic fence generation module is used to generate a dynamic electronic fence that includes the space for carrying objects and sweeping, based on the static electronic fence range of the pre-defined operation, the joint envelope model, and the dynamic obstacle prediction results. The trajectory optimization module is used to take the probability occupancy model, the joint envelope model and the dynamic electronic fence as inputs, establish a trajectory optimization model and solve for candidate control command sequences under constraints. The safety projection module is used to perform safety projection on the candidate control command sequence to generate a final control command sequence that satisfies the discrete probability safety constraints; when a control command sequence that satisfies the constraints cannot be generated, a preset backoff strategy is executed. The execution module is used to execute the final control instruction sequence and repeat each step in each control cycle until the task is completed.

Images