A mobile robot path planning and compliant docking method for automatic installation of circuit breakers

Abstract

The application discloses a mobile robot path planning and compliant docking method for automatic installation of circuit breakers, comprising: acquiring work environment information and constructing a navigation model for mobile decision-making, evaluating the mobile path in combination with the circuit breaker installation task, and generating an optimal mobile path to a target position; controlling the mobile robot to run along the optimal mobile path to the vicinity of the target position, and determining the relative pose relationship between the circuit breaker guide wheel and the guide groove based on environment perception information; in the process of contact between the guide wheel and the guide groove, real-time collection of mechanical interaction information and contact state identification are performed to obtain the corresponding contact stage; according to the contact stage, the system damping parameters are adaptively adjusted, and the dynamic control of the end effector movement is performed, so that the guide wheel realizes continuous introduction along the guide groove, thereby completing the compliant docking of the circuit breaker.

Description

Technical Field

[0001] This invention relates to the field of compliant docking technology, and in particular to a mobile robot path planning and compliant docking method for automatic installation of circuit breakers. Background Technology

[0002] In the automated installation of circuit breakers in distribution cabinets, it is typically necessary to carry or drive a mobile robot along the cabinet's guide structure to ensure precise alignment of the circuit breaker's guide wheels and guide slots. This task not only relies on the robot's ability to reach the target location, but more importantly, it requires sufficient stopping accuracy and relative posture consistency after arriving at the target installation position to ensure the assemblability and safety of subsequent assembly processes.

[0003] Most existing mobile robot path planning methods primarily optimize obstacle avoidance, accessibility, and path length / time efficiency. However, in environments like power distribution rooms—where equipment is dense, passageways are narrow, and workstations are scattered—simply ensuring the robot "reaches the target point" is often insufficient to meet circuit breaker installation requirements. This is because: firstly, the boundaries of cabinets, doors, cable trays, and temporarily placed tools or carts within the power distribution room create complex geometric constraints. The environment contains not only "point obstacles" but also "collision upon approach" gap constraints. While the robot may be modeled as accessible at the planning level, the lack of explicit constraints on robot size and safety margins leads to a tendency to traverse along edges or choose theoretically feasible but impractical routes at narrow corners. Secondly, some methods use planar grids or coarse-resolution maps for searching, which fail to accurately reflect local height differences and occlusion structures, resulting in geometric mismatches between the robot and actual obstacles during execution. This leads to increased collision risk, greater posture deviation, and even emergency stops when the robot enters narrow passages, turns, or approaches cabinet entrances, thus affecting task continuity.

[0004] In multi-station scenarios, robots need to move repeatedly. If the planning lacks a structured representation and connectivity guarantee for continuous free space, the robot may encounter path detours, dead ends, or insufficient ability to enter the docking area when switching between different workstations. As a result, the robot still needs to make significant secondary adjustments after entering the workstation neighborhood (e.g., repeated fine-tuning and correction), increasing the requirements for positioning accuracy, control stability, and assembly tolerance. If the docking posture deviation is large, subsequent docking control is prone to generating additional lateral impacts and torque fluctuations, affecting the smoothness of the docking process, thus leading to equipment safety and assembly consistency issues.

[0005] In the automatic installation of circuit breakers, the key to docking control is to smoothly guide the guide wheel into the guide groove. This process typically involves multiple mechanical stages, including approach, single-sided contact, alignment, and introduction / pushing, with the stress state changing significantly with each stage. Existing compliance control methods mostly employ impedance / admittance control with fixed stiffness and damping parameters, making it difficult to adapt to the dynamic mechanical characteristics of different stages. This can easily lead to oscillations or response lags in certain stages, affecting docking success rate and equipment safety. While some methods attempt to use force thresholds to switch stages and adjust parameters, the thresholds are difficult to set, and switching can easily cause abrupt parameter changes. Furthermore, the lack of modeling support for the contact geometry and mechanical mechanism of the guide wheel-guide groove results in control parameter adjustment relying on experience and exhibiting poor adaptability.

[0006] The purpose of this invention is to address the problems existing in the prior art by designing a mobile robot path planning and compliant docking method for automatic circuit breaker installation. Summary of the Invention

[0007] In view of this, the purpose of this invention is to propose a mobile robot path planning and compliant docking method for automatic circuit breaker installation, which can solve the above-mentioned problems.

[0008] This invention provides a method for path planning and compliant docking of a mobile robot for automatic circuit breaker installation, comprising: S1 acquires operational environment information and constructs a navigation model for movement decision-making. It then evaluates the movement path in conjunction with the circuit breaker installation task and generates the optimal movement path to the target location. S2 controls the mobile robot to move along the optimal movement path to the vicinity of the target position, and determines the relative pose relationship between the circuit breaker guide wheel and the guide groove based on environmental perception information; During the contact process between the guide wheel and the guide groove, S3 collects mechanical interaction information in real time and identifies the contact state to obtain the corresponding contact stage. S4 adaptively adjusts the system damping parameters and dynamically controls the movement of the end effector according to the contact stage, so that the guide wheel can be continuously guided along the guide groove, thereby completing the smooth docking of the circuit breaker.

[0009] The beneficial effects of this invention are: First, the nominal pose of the target installation station is simultaneously converted into the actual spatial pose under a unified global coordinate system and integrated into the path search space as a constraint condition. A graph search algorithm is then used to generate the optimal movement path. This allows the robot to more accurately meet the relative docking geometry conditions with the guide groove entrance when it reaches the station's neighborhood. This significantly reduces the relative pose deviation and attitude offset in the initial stage of guide wheel-guide groove docking, lowers the correction magnitude and time overhead required for subsequent pose calculation and recursive approximation, reduces the risk of lateral impact, torque fluctuations, and guide wheel misalignment, and improves the success rate and assembly reliability of circuit breaker docking.

[0010] Secondly, the actual pose of the target workstation is mapped into the aforementioned structured planning space, ensuring that path planning not only satisfies obstacle avoidance and shortest distance requirements but also guarantees the robot's accessibility to the docking area and the pre-defined posture conditions. This reduces the probability of detours and jams caused by environmental modeling errors, geometric mismatches, and changes in channel topology in scenarios with dense equipment, narrow passageways, and dispersed workstations in power distribution rooms. It also significantly reduces the amount of secondary corrections required after the robot reaches the workstation, thereby improving overall operational efficiency and the consistency and robustness of continuous assembly across workstations.

[0011] Third, by introducing time-frequency analysis of force / torque signals, multi-dimensional features such as total energy, energy centroid frequency, energy change rate, and correlation coefficient are extracted. Combined with the differences in physical mechanisms at each contact stage, the online identification of the guide wheel-guide groove contact state is achieved, transforming the continuous and complex contact process into distinguishable discrete stages (accurately identifying key processes such as contact instant, unilateral contact, and alignment). This solves the problem that traditional methods rely on experience or single thresholds and are difficult to make stable judgments. Furthermore, based on the identification results, the damping parameters of the corresponding stages are obtained. A phased adaptive adjustment is adopted, combined with a smooth transition mechanism to avoid parameter abrupt changes. Under the impedance control framework, the dynamic action is applied to the motion control of the end effector, achieving both rapid response and vibration suppression in the contact process. For example, in the unilateral contact stage, damping is increased to suppress impact, and in the lead-in stage, damping is reduced to ensure smooth propulsion and avoid jamming and impact. Attached Figure Description

[0012] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the accompanying drawings required in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0013] Figure 1 This is the overall method flowchart of this embodiment.

[0014] Figure 2 This is a flowchart of methods S3 and S4 in this embodiment. Detailed Implementation

[0015] To facilitate understanding by those skilled in the art, the structure of the present invention will now be described in further detail with reference to the accompanying drawings. It should be understood that, unless otherwise specified, the order of the steps mentioned in this embodiment can be adjusted according to actual needs, and they can even be executed simultaneously or partially simultaneously.

[0016] like Figure 1As shown, this embodiment of the invention provides a method for path planning and compliant docking of a mobile robot for automatic circuit breaker installation, including: S1 acquires work environment information and constructs a navigation model for movement decision-making. It evaluates the movement path in conjunction with the circuit breaker installation task and generates the optimal movement path to the target installation station. S101 scans the working environment using a LiDAR mounted on the mobile robot to acquire raw 3D point cloud data. It then filters and voxels the raw 3D point cloud data to obtain preprocessed point cloud data. S102 performs multi-frame point cloud registration and pose estimation based on preprocessed point cloud data to obtain the robot's motion trajectory. It then performs spatial modeling on the registered point cloud data to construct an occupied grid map and obtain environmental map data. S103 classifies the grid cells of the environmental map data according to the preset occupancy probability threshold, and obtains obstacle grids and free grids. It performs connected component analysis on the free grids to extract continuous free space regions. S104 performs morphological dilation or erosion on the continuous free space region to introduce robot size constraints, obtain the passable region, extract the skeleton of the passable region, and generate a spatial structure representation that can be used for path planning. S105 reads the preset circuit breaker installation task information, obtains the nominal pose data of the target installation station, and converts the nominal pose data to the global coordinate system established by the environmental map data to obtain the actual spatial pose of the target installation station. S106 uses the passable area data and the actual spatial pose of the target installation station as joint inputs to construct a path search space. With the shortest path as the objective, a graph search algorithm is used to solve the path search space to obtain the optimal movement path from the current position of the mobile robot to the target installation station.

[0017] In this step, the working environment is scanned in three dimensions using LiDAR to obtain environmental point cloud data. The data quality and computational efficiency are improved by filtering and voxel sampling. Based on this, environmental mapping is achieved through multi-frame point cloud registration and pose estimation to construct an occupied grid map, thereby obtaining a structured environmental representation.

[0018] For example, in a power distribution room with numerous rows of switch cabinets and narrow passageways, LiDAR can accurately acquire information on cabinet boundaries and passageway widths, which, after processing, forms an environmental map suitable for navigation. Furthermore, continuous free space is extracted through grid classification and connected component analysis, and morphological expansion of the space is performed based on the robot's own dimensions (such as chassis width and safety clearance) to obtain a truly passable area, thus avoiding planned paths traversing narrow but impassable areas.

[0019] Simultaneously, by introducing the target workstation pose information from the circuit breaker installation task and uniformly converting it to the global coordinate system corresponding to the environmental map, the fusion of environmental and task information is achieved. For example, the installation location of a circuit breaker in a specified distribution cabinet is mapped to a specific coordinate location on the map. Based on the above information, a path search space is constructed, and a graph search algorithm is used to generate the optimal movement path, enabling the robot to travel safely along the channel and accurately reach the target installation workstation.

[0020] It effectively solves the problems of inaccurate environmental representation, failure to consider equipment size constraints, and lack of task objective guidance in traditional path planning, thereby improving the reliability of path planning and its adaptability to actual operations.

[0021] S2 controls the mobile robot to move along the optimal movement path to the vicinity of the target installation station, and determines the relative pose relationship between the circuit breaker guide wheel and the guide groove based on environmental perception information; S201 controls the mobile robot to move along the optimal movement path and corrects its own pose in real time through the positioning module. During the movement, it acquires the robot's real-time pose and calculates the error with the optimal movement path. Based on the error result, it performs closed-loop adjustment of the movement. S202 acquires the current pose information in real time, and determines whether the current pose is within the approach range based on the actual spatial pose of the target installation station. If so, it collects laser distance data and installation area images, and obtains the edge data of the installation area through the installation area images. S203 constructs a pose calculation equation based on laser distance data and installation area edge data. The pose calculation equation is solved through nonlinear optimization to obtain the pose deviation of the robot end relative to the installation reference. S204 combines the structural features of the guide wheel and guide groove to perform coordinate transformation on the pose deviation, obtain the relative pose parameters of the guide wheel relative to the guide groove, convert the relative pose parameters into the target adjustment pose of the end effector, control the actuator to perform recursive pose adjustment, so that the end effector gradually approaches the target pose.

[0022] In this step, a closed-loop motion control mechanism is constructed by controlling the mobile robot to move along the planned path and using the positioning module to correct the robot's pose in real time, thereby reducing path tracking errors. In the narrow passage environment of the power distribution room, the mobile robot may accumulate positioning errors due to uneven ground or tire slippage during its movement. Through real-time pose feedback and path error correction, the robot can be guaranteed to stably reach the vicinity of the target installation position.

[0023] After the robot approaches the target workstation, it integrates laser ranging data and visual image information to perform fine perception and edge extraction of the installation area, thereby improving the accuracy of local environmental perception. For example, when there is occlusion at the edge of the guide slot opening or changes in lighting at the circuit breaker installation location, a single sensor may not be able to accurately identify the position of the guide slot. Multi-source information fusion can improve the reliability of boundary detection.

[0024] Based on this, high-precision pose deviation estimation of the end effector relative to the mounting reference is achieved by constructing pose calculation equations and solving them using nonlinear optimization methods. By incorporating the distance constraints obtained from laser ranging and the guide groove edge features extracted from the image into the optimization model, millimeter-level position and attitude deviation results can be obtained.

[0025] By further considering the structural constraints between the guide wheel and the guide groove, the aforementioned pose deviation is converted into relative pose parameters with actual physical meaning and mapped to adjustment commands for the end effector. The target pose is then gradually approximated through recursive adjustments. For example, when a lateral offset of the guide wheel relative to the guide groove is detected, the end effector is controlled to perform a small lateral movement and attitude adjustment, so that the guide wheel gradually aligns with the center line of the guide groove.

[0026] It achieves a smooth transition from global navigation and positioning to local high-precision alignment, effectively solving the docking difficulties caused by the accumulation of positioning errors and the high precision requirements of the guide wheel and guide groove during installation, and providing accurate and reliable initial alignment conditions for subsequent compliant contact and stable introduction.

[0027] like Figure 2 As shown, during the contact process between the guide wheel and the guide groove, S3 collects mechanical interaction information in real time and identifies the contact state to obtain the corresponding contact stage. During the contact process between the guide wheel and the guide groove, real-time acquisition and analysis of mechanical interaction information enables online identification of the contact state. This discretizes the continuous docking process into multiple contact stages with distinct mechanical characteristics, providing a basis for subsequent staged control strategies and addressing the problems of inaccurate perception of the contact process state and lack of targeted control strategies in existing technologies. The specific steps are as follows: During the docking process between the guide wheel and the guide groove, the S301 uses a torque sensor installed on the end effector to collect the contact force signal in real time. The calculation formula is as follows: , in, Indicates contact force signal, This represents the axial thrust along the guide groove direction. This represents the lateral force perpendicular to the direction of the guide groove. Indicates vertical force. The torque representing the axial thrust along the guide groove direction. The torque representing the lateral force perpendicular to the direction of the guide groove. The torque representing the vertical force; S302 performs a short-time Fourier transform on the torques of the lateral and vertical forces based on a sliding time window to obtain the characteristics of the lateral contact state between the guide wheel and the guide groove. In this step, a six-dimensional force / torque sensor is used to acquire multi-dimensional contact force information between the guide wheel and the guide groove, constructing a complete mechanical observation vector to achieve synchronous perception of axial propulsion force, lateral force, and torque information.

[0028] By performing a short-time Fourier transform on the torque signals of lateral and vertical forces through a sliding time window, the time-domain signals are converted into a time-frequency domain representation, and spectral features reflecting dynamic changes in contact are extracted. This solves the technical problem that traditional time-domain analysis is difficult to characterize the transient impact and vibration characteristics of contact, making different contact stages distinguishable in the frequency domain, thereby improving the stage identification capability.

[0029] Window length L Corresponding time ,Pick The window sliding step size is S = L / 4. The lateral force within the window... and torque about the vertical axis Perform a short-time Fourier transform, because these two components best reflect the lateral contact state between the guide wheel and the guide groove.

[0030] S303 extracts several time-frequency features of the current time window by analyzing the lateral contact state characteristics between the guide wheel and the guide groove. S3031 calculates the total energy within the time window by integrating the time-spectrum energy of the signal within the time window. The calculation formula is as follows: , in, This represents the total energy within the time window t. Indicates time angular frequency Spectral amplitude, This represents the energy at the corresponding frequency. and Indicates the frequency integral range; In this step, this feature characterizes the overall signal strength during the contact process. During the approach phase, the guide wheels are not in contact, and the energy is very low, mainly consisting of ambient noise. At the moment of single-sided contact, the lateral force suddenly increases, causing a surge in energy. During the centering phase, the energy remains at a certain level. After introduction, the vehicle moves smoothly forward, with energy changing gradually. The sudden energy surge allows for precise detection of the contact moment. This feature effectively detects the moment of contact and distinguishes energy level changes at different contact stages, solving the problem of accurately identifying the contact initiation point.

[0031] S3032 calculates the weighted average frequency of the spectral energy as the energy centroid frequency, using the following formula: , in, This represents the energy centroid frequency within the time window t; In this step, the energy centroid frequency characterizes the position of the center of signal energy distribution on the frequency axis, reflecting the dominant frequency characteristics of contact vibration. The numerator is the frequency ω multiplied by the energy at that frequency, and then integrated over the frequency to obtain the energy-weighted frequency sum. The denominator is the total energy, i.e., the sum of energy at all frequencies.

[0032] During the circuit breaker connection process, the single-sided contact stage is mainly characterized by low-frequency impact (e.g., about 5–20Hz), corresponding to a lower energy center frequency. During the centering stage, due to the slight sliding and frictional vibration of the guide wheel in the guide groove, the frequency component shifts to the mid-frequency region (e.g., about 50–200Hz), causing the energy center frequency to increase. After entering the lead-in stage, the movement tends to be stable, and the frequency decreases again.

[0033] S3033 calculates the rate of energy change by measuring the energy difference between adjacent time windows, using the following formula: , in, This represents the rate of energy change at time window t. This represents the total energy of the current window. This represents the total energy in the previous time window. Indicates the window sliding step size; In this step, the rate of energy change characterizes the dynamic rate of change during the contact process. At the instant of contact, the rate of energy change increases significantly due to the impact; during the centering phase, the rate of change gradually decreases; and during the introduction phase, it tends to stabilize, with the rate of change approaching zero.

[0034] S3034 uses the Pearson correlation coefficient to calculate the correlation coefficient between the moments of the lateral force and the vertical force. The calculation formula is as follows: , in, Indicates time window t The correlation coefficient within, Indicates lateral force Torque of vertical force covariance, Indicates lateral force standard deviation Torque representing vertical force The standard deviation.

[0035] In this step, the coupling relationship between the two is characterized. In the single-sided contact stage, the guide wheel contacts one side of the guide groove, the contact point is off-center, and the lateral force and torque satisfy an approximately linear relationship (ignoring friction). The two have a strong correlation, with a correlation coefficient close to 1. In the centering or double-sided contact stage, the force distribution is more symmetrical, the force and torque relationship becomes more complex, and the correlation decreases. The correlation coefficient can be used to determine whether it is in a single-sided contact state.

[0036] S304 determines the current contact stage by matching the time-frequency characteristics of the current time window with the reference characteristics of each contact stage based on a pre-established contact stage feature model. S3041 generates force / torque signal data under different contact stages through experimental acquisition or simulation, and extracts the corresponding time-frequency characteristic parameters, including: total energy within the window, energy centroid frequency, energy change rate, and correlation coefficient. S3042 labels each feature vector with a corresponding contact stage phase label. The contact stage phase labels include: approach stage, unilateral contact stage, centering stage, introduction stage, and push-in stage. The feature vectors under each phase category are statistically processed to calculate their mean vector, which is used as a reference template for that phase. S3043 calculates the Euclidean distance between the current feature vector and each phase reference template, and selects the phase with the smallest distance as the current recognition result. The calculation formula is as follows: ， in, This represents several time-frequency characteristics of the current time window. Indicates the reference template for each phase, Describing the Euclidean norm, This indicates the contact phase corresponding to the current time window.

[0037] In this step, the continuously changing contact process is transformed into a standardized classification problem, which avoids the instability caused by relying on empirical thresholds for stage division in traditional methods, and improves the accuracy and consistency of the identification results.

[0038] S305 performs filtering or consistency processing on the contact phase recognition results of multiple consecutive windows to obtain stable phase estimation.

[0039] This step ensures the continuity and reliability of the contact phase in time, avoids frequent phase jumps, and thus provides a stable input for the smooth switching of subsequent damping parameters, improving the overall control stability of the system.

[0040] S4 adaptively adjusts the system damping parameters and dynamically controls the movement of the end effector according to the contact stage, so that the guide wheel can be continuously guided along the guide groove, thereby completing the smooth docking of the circuit breaker.

[0041] By dividing the contact process into multiple stages with different mechanical properties and employing matched damping parameters for each stage, the problem of traditional fixed-parameter control being unable to adapt to multi-stage contact changes is effectively solved, thereby reducing impact and vibration and improving the stability and success rate of the docking process. The specific steps are as follows: S401 divides the contact stages according to the contact state between the guide wheel and the guide groove during the automated alignment process, and determines the corresponding damping parameters based on the contact stages. In this step, contact phase segmentation is used to limit the selection of subsequent damping parameters within a control strategy range that matches the current contact state. Because the force constraints on the system differ under different contact states, the dynamic response characteristics (e.g., oscillation tendency, overshoot risk, and convergence speed) of the system during alignment vary. Therefore, the phase identifiers obtained through contact phase segmentation serve as the basis for selecting damping parameters, enabling the system to employ different damping intervals during single-sided contact, alignment, and insertion / push-in processes, thereby improving alignment efficiency while ensuring stability.

[0042] S4011 When the lateral force perpendicular to the guide groove direction exceeds the lateral force threshold and the torque of the lateral force is proportional to the torque of the vertical force, it is the single-sided contact stage, and the system damping parameter is set to the high damping parameter range. In this step, under single-sided contact, the guide wheel is primarily constrained by one side wall of the guide groove. Since the constraint mainly originates from this side wall, the magnitude of the lateral force is usually significant, exhibiting a stable proportional relationship or correlation with the torque around the vertical axis. This proportional relationship indicates that the lateral deviation is not symmetrically constrained; therefore, under single-sided contact, there is a potential risk of oscillation or overshoot due to unresolved deviation. Therefore, a high-damping parameter range is used in the single-sided contact stage to enhance the energy dissipation capacity of the impedance / force control response, thereby suppressing oscillation and rapidly reducing lateral deviation, thus mitigating the instability risk caused by misalignment or jamming.

[0043] The high-damping parameter range can be calculated using the following steps: Analyze the geometric relationship between the circuit breaker guide wheel (cylinder) and the distribution cabinet guide groove (V-shaped inlet + straight groove). Let the radius of the guide wheel be R, and the included angle of the V-shaped portion of the guide groove be 2°. (Typically 90°), the width of the straight groove is slightly larger than the diameter of the guide wheel. When there is a lateral deviation y between the center of the guide wheel and the center of the guide groove, the contact point between the guide wheel and the guide groove can be determined by geometric relationships.

[0044] The guide wheel contacts only one side of the V-shaped surface. The contact force is perpendicular to the contact surface and can be decomposed into axial and lateral components. The system can be simplified as a single-degree-of-freedom spring-mass-damped system, with the following dynamic equations: , in, This indicates the lateral displacement of the guide wheel center. This represents the first derivative of the lateral displacement with respect to time. This represents the second derivative of the lateral displacement with respect to time. The equivalent mass, including the mass of the guide wheel and its connecting components, can be calculated using a CAD model or identified experimentally. This represents the equivalent damping parameter, which is composed of mechanical friction and control system damping. The stiffness of a single-sided contact is determined by the elastic modulus of the materials of the guide wheel and guide groove, as well as the contact geometry, and can be estimated using Hertzian contact theory. Indicates lateral force; The critical damping ratio of this second-order system is: The corresponding critical damping coefficient is:

[0045] Actual damping ratio .when The system is underdamped and prone to oscillations. Over-damping results in a slow response. Therefore, in the single-sided contact phase, the damping coefficient b should be close to... This is to quickly suppress the impact while avoiding excessive oscillation.

[0046] S4012 When the lateral force perpendicular to the guide groove direction is less than the lateral force threshold, the torque of the vertical force is less than the torque threshold, the rate of change of the axial force is less than the rate of change of the axial force threshold, and the stabilization time window is greater than the stabilization time window threshold, it is the centering stage, and the system damping parameters are set to the medium damping parameter range. When the guide wheel enters the V-shaped guide groove, two different physical contact states may occur. Both of these states can lead to variations in lateral force. (Or the resultant lateral force) decreases, but the inherent risks are different.

[0047] Scenario A: Ideal alignment (target state) When the centerline of the guide wheel coincides with the centerline of the guide groove, the guide wheel and both sides of the V-shaped guide groove are in contact simultaneously and are in an ideally aligned state. In this state, due to the basically symmetrical forces on the left and right sides, the resultant force of the lateral forces is... The torque approaches zero, and the torque about the vertical axis also approaches zero.

[0048] Situation B: Disconnection / Stuck (Dangerous Condition) When the guide wheel is stuck near the entrance of the guide groove due to angular deviation, geometric interference, or structural limitations, the guide wheel may not have actually entered the guide groove. However, due to hard contact or the recovery of elastic deformation, the resultant lateral force may still decrease, thereby... This manifests as being close to or even below the threshold. If only based on " If the force is determined to be less than the lateral force threshold and forced propulsion is executed, the propulsion force may continue to be applied under this dangerous condition, which may lead to equipment damage or further jamming.

[0049] When the guide wheel contacts both sides of the V-shaped guide groove, the system state should not be directly judged solely by the magnitude of the resultant lateral force. A more reliable criterion can be characterized by the following features: Force distribution characteristics: Under ideal alignment, even if the left and right lateral forces are not strictly equal due to factors such as friction and differences in surface roughness, the resultant force of the left and right lateral forces should still be close to zero.

[0050] Torque characteristics: Under ideal alignment, the torque about the vertical axis should be close to zero; however, in cases of jamming or asymmetrical contact points, although the resultant lateral force may be close to zero, it may still be non-zero or significantly deviate from zero.

[0051] The relationship between axial force and displacement (propulsion) is characterized as follows: Under aligned conditions, the axial force increases steadily with the propulsion displacement (or follows the expected trend); however, under jammed conditions, it may increase abnormally, exhibit a step-like increase, or deviate from the expected trend. Based on these characteristics, reliable alignment judgment can usually be achieved without establishing a complete bilateral contact dynamics model. Preferably, this step can construct criteria based on measurable physical quantities, thereby reducing the risk of misjudgment.

[0052] Lateral force threshold Typically, this is taken as 5~10N (slightly higher than the sensor noise). Torque threshold. This value is typically taken as 0.2 ~ 0.5 Nm. This condition is crucial, as it eliminates the possibility of "stuck but with zero net force." Axial force change rate threshold. Typically, a value of 30~50 N / s is used to eliminate hard jamming. (Stability time window threshold) Typically, a damping parameter of 0.1 to 0.2 s is used to eliminate transient disturbances. The damping parameter is slightly lower than the high damping parameter range for single-sided contact to allow for centering motion.

[0053] S4013 When the rate of change of the lateral force perpendicular to the guide groove direction is less than the threshold of the lateral force change and the rate of change of the torque of the vertical force is less than the threshold of the torque of the vertical force, it is the introduction stage and the pushing stage, and the system damping parameters are set to the medium damping parameter range.

[0054] In this step, during the introduction / push-in phase, the key objective of the system shifts from "establishing alignment" to "maintaining stable introduction motion under the guidance of the guide groove." During this phase, even if the absolute value of the lateral force or torque changes, if both the lateral force and torque exhibit a controlled trend of change (e.g., the rate of change is below a threshold), it indicates that the system will not experience abrupt oscillations caused by introduction instability, and the introduction process remains within stable constraints.

[0055] S402 stores the damping parameters corresponding to each contact stage in association, forming a stage-damping parameter mapping relationship. Based on the contact stage identification results, it selects the damping parameters of the corresponding stage in real time. When S403 detects a change in the contact phase, it introduces a smooth transition function to interpolate the damping parameters, so that the current damping parameters smoothly transition from the parameter values ​​of the previous phase to the target damping parameters of the current phase. S4031 extends the damping parameters in each degree of freedom direction into a diagonal form of the damping matrix, and defines a smooth transition function for the damping matrix, calculated as follows: , in, This represents the damping matrix of the previous contact stage. This represents the target damping matrix for the current contact phase. This represents the transition factor, generated through a first-order inertial element; If the contact stage identifiers obtained from two consecutive samplings are different, S4032 determines that the current contact stage has changed, initializes the transition factor, and uses the target damping parameter corresponding to the current stage as the final transition value. S4033 calculates the damping parameters in real time based on the smooth transition function, and completes the switching of damping parameters when the transition factor approaches 1.

[0056] In this step, if the damping parameter changes instantaneously when the guide wheel transitions from single-sided contact to the centering stage, it may trigger a new impact; a smooth transition allows the system response to change continuously, improving control stability.

[0057] The obtained phase damping coefficients Establish a phase-damping mapping table. For multi-degree-of-freedom cases, this can be extended to a damping matrix. ,in For axial damping, For lateral damping, This is rotational damping. The damping parameters for each degree of freedom can be calculated separately based on the equivalent dynamic model in the corresponding direction.

[0058] The transition factor is generated using a first-order inertial element: ,in, The phase switching time is T, which is the transition time constant (taken as 50-100 ms) to avoid control shock caused by sudden parameter changes.

[0059] Under impedance control, the S404 introduces the real-time updated damping parameters into the control model, and calculates the speed command or displacement correction of the end effector by combining the error between the desired pose and the actual pose. S4041 constructs an impedance control model within the end effector's task space, and the calculation formula is as follows: , in, Indicates the desired pose increment at the end point (lateral translation) and rotation about the vertical axis ), This represents the second derivative of the expected position increment at the endpoint with respect to time. This represents the first derivative of the expected position increment at the endpoint with respect to time. This represents the set virtual mass matrix (taking a fixed value, such as...). ), This represents a dynamically adjusted virtual damping matrix. This represents the virtual stiffness matrix (taking a smaller value to ensure compliance, such as...). )), This represents the components of the measured contact force / torque in the task space (obtained by conversion from a six-dimensional force sensor). This represents the desired contact force (usually set to zero vector; only during the pushing phase can the axial force be set to a small normal force to maintain contact). S4042 maps the damping parameters after smooth transition to a virtual damping matrix in the impedance control model, and solves for the desired end acceleration based on the force error or pose error driving the control model. S4043 performs numerical integration on the desired acceleration to obtain the desired velocity and displacement increment, and converts the displacement increment into joint control commands for the actuator. In this step, the contact force and pose error are converted into the desired acceleration, and the velocity and displacement commands are obtained through integration, realizing the mapping from force control to motion control. This enables the robot to have compliant characteristics similar to a "spring-damped system" during the contact process, such as automatically buffering and adjusting when the guide wheels deviate slightly, thereby improving the fault tolerance of the docking process.

[0060] If, within N consecutive sampling periods, the S4044 detects a lateral force exceeding a lateral force threshold and a displacement less than a displacement threshold, then the contact state satisfies the preset jamming condition, triggering safety protection control.

[0061] In this step, when the guide wheel gets stuck at the edge of the guide groove due to excessive deviation, it will show a continuous increase in lateral force but a very small change in displacement. This criterion can trigger protection measures in time to avoid equipment damage or structural jamming and improve system safety.

[0062] During the docking process, S405 continuously calculates the motion control quantity of the end effector based on the contact phase identification results and the real-time updated damping parameters. When a change in the contact phase is detected during the docking process, the control parameters are dynamically adjusted and the control output is updated, so that the motion state of the guide wheel transitions continuously between each phase until the docking is completed and a stable docking state is achieved.

[0063] The method of the present invention will be described using the automatic installation process of a certain type of 12kV vacuum circuit breaker as an example.

[0064] I. System Configuration A six-dimensional force / torque sensor (ATI Mini-45, 1kHz sampling rate) is integrated at the end of the circuit breaker transfer platform to collect real-time contact force information between the guide wheel and the guide groove. The end-effector micro-motion actuator consists of a linear motor and a rotary motor, with a position resolution of 1μm, for achieving fine adjustment at the end.

[0065] II. Offline Parameter Acquisition and Feature Library Establishment Based on the structural parameters of the circuit breaker and actuator, the equivalent mass of the system was calculated to be m = 45 kg using a CAD model; the equivalent contact stiffness between the guide wheel and the guide groove was measured to be k = 1.5 × 10⁻⁶ kg using an indentation test. N / m. The calculated critical damping for single-sided contact is... ≈520 N·s / m.

[0066] Fifty simulated docking experiments were conducted, force signals were collected at each phase, feature vectors were extracted and labeled, and a feature library was established. The reference features for each phase are as follows: near: , , , Meaningless; Unilateral contact: It surged to 10-50. great, ; To China: Maintain 5-20, medium, ; Import: Maintain stability.

[0067] Pushing in: The axial force continues to increase, while the lateral force is almost zero.

[0068] III. Online Operation Process During the actual installation process, the transfer platform advances at a speed of 5mm / s and extracts feature parameters in real time with a sliding time window of 50ms.

[0069] The initial phase is set to "close" and the damping is set to a low value. .

[0070] At 2.3 seconds (corresponding to the 46th window), the energy surges to 45, the main frequency is 15Hz, and the correlation coefficient is 0.96, indicating entry into the "single-sided contact" phase. Damping smoothly transitions to [a later state] after 80ms. .

[0071] At the 3.1-second mark (corresponding to the 62nd window), the energy drops to 18, the main frequency rises to 75Hz, and the correlation coefficient is 0.25, indicating entry into the "centering" phase. A smooth damping transition then occurs. .

[0072] At the 4.5-second mark (corresponding to the 90th window), the energy stabilizes at 5, the main frequency is 10Hz, and the correlation coefficient is 0, indicating entry into the "introduction" phase. A smooth damping transition then begins. .

[0073] At 6.0 seconds (corresponding to the 120th window), the guide wheel fully enters the straight groove, completing the alignment stage, with a cumulative advance displacement of 30mm. The system then switches to position control mode and continues to advance the remaining 200mm of travel at a speed of 10mm / s, with a total installation time of approximately 26 seconds.

[0074] IV. Effect Verification The method of this invention is combined with the fixed damping control method (( Comparative experiments were conducted, and the results showed that when the method of the present invention was used, the peak value of the lateral force during the contact process between the guide wheel and the guide groove was reduced by about 38%, the time consumed in the centering stage was shortened by about 25%, and no obvious oscillation or jamming occurred during the entire docking process.

[0075] Those skilled in the art will understand that embodiments of the present invention can be provided as methods, systems, or computer program products. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention can take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0076] This invention is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart illustrations and / or block diagrams. Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0077] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0078] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0079] It should be noted that any reference signs placed between parentheses in the claims should not be construed as limiting the claims. The word "comprising" does not exclude the presence of components or steps not listed in the claims. The word "a" or "an" preceding a component does not exclude the presence of a plurality of such components. The invention can be implemented by means of hardware comprising several different components and by means of a suitably programmed computer. In a unit claim enumerating several means, several of these means may be embodied by the same item of hardware. The words first, second, and third, etc., do not indicate any order. These words can be interpreted as names.

[0080] Although preferred embodiments of the invention have been described, those skilled in the art, upon learning the basic inventive concept, can make other changes and modifications to these embodiments. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments as well as all changes and modifications falling within the scope of the invention.

[0081] Obviously, those skilled in the art can make various modifications and variations to this invention without departing from its spirit and scope. Therefore, if these modifications and variations fall within the scope of the claims of this invention and their equivalents, this invention also intends to include these modifications and variations.

[0082] In this invention, unless otherwise explicitly specified and limited, the terms "installation," "connection," "linking," and "fixing," etc., should be interpreted broadly. For example, they can refer to a fixed connection, a detachable connection, or an integral part; they can refer to a mechanical connection or an electrical connection; they can refer to a direct connection or an indirect connection through an intermediate medium; they can refer to the internal communication of two components or the interaction between two components. Those skilled in the art can understand the specific meaning of the above terms in this invention according to the specific circumstances.

[0083] In the description of this specification, the references to terms such as "one embodiment," "some embodiments," "example," "specific example," or "some examples," etc., indicate that a specific feature, structure, material, or characteristic described in connection with that embodiment or example is included in at least one embodiment or example of the present invention. In this specification, the illustrative expressions of the above terms should not be construed as necessarily referring to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples. Moreover, without contradiction, those skilled in the art can combine and integrate the different embodiments or examples described in this specification, as well as the features of different embodiments or examples.

Claims

1. A method for path planning and compliant docking of a mobile robot for automatic installation of circuit breakers, characterized in that, include: S1 acquires operational environment information and constructs a navigation model for movement decision-making. It then evaluates the movement path in conjunction with the circuit breaker installation task and generates the optimal movement path to the target location. S2 controls the mobile robot to move along the optimal movement path to the vicinity of the target position, and determines the relative pose relationship between the circuit breaker guide wheel and the guide groove based on environmental perception information; During the contact process between the guide wheel and the guide groove, S3 collects mechanical interaction information in real time and identifies the contact state to obtain the corresponding contact stage, including: During the docking process between the guide wheel and the guide groove, the S301 uses a torque sensor installed on the end effector to collect the contact force signal in real time. The calculation formula is as follows: ， in, Indicates contact force signal, This represents the axial thrust along the guide groove direction. This represents the lateral force perpendicular to the direction of the guide groove. Indicates vertical force. The torque representing the axial thrust along the guide groove direction. The torque representing the lateral force perpendicular to the direction of the guide groove. The torque representing the vertical force; S302 performs a short-time Fourier transform on the torques of the lateral and vertical forces based on a sliding time window to obtain the characteristics of the lateral contact state between the guide wheel and the guide groove. S303 extracts several time-frequency features of the current time window by analyzing the lateral contact state characteristics between the guide wheel and the guide groove. S304 determines the current contact stage by matching the time-frequency characteristics of the current time window with the reference characteristics of each contact stage based on a pre-established contact stage feature model. S305 performs filtering or consistency processing on the contact phase recognition results of multiple consecutive windows to obtain a stable phase estimate; S4 adaptively adjusts the system damping parameters and dynamically controls the movement of the end effector according to the contact stage, so that the guide wheel can be continuously guided along the guide groove, thereby completing the smooth docking of the circuit breaker.

2. The method for path planning and compliant docking of a mobile robot for automatic installation of circuit breakers according to claim 1, characterized in that, The process of acquiring work environment information and constructing a navigation model for movement decision-making, evaluating the movement path in conjunction with the circuit breaker installation task, and generating the optimal movement path to the target location includes: S101 scans the working environment using a LiDAR mounted on the mobile robot to acquire raw 3D point cloud data. It then filters and voxels the raw 3D point cloud data to obtain preprocessed point cloud data. S102 performs multi-frame point cloud registration and pose estimation based on preprocessed point cloud data to obtain the robot's motion trajectory. It then performs spatial modeling on the registered point cloud data to construct an occupied grid map and obtain environmental map data. S103 classifies the grid cells of the environmental map data according to the preset occupancy probability threshold, and obtains obstacle grids and free grids. It performs connected component analysis on the free grids to extract continuous free space regions. S104 performs morphological dilation or erosion on the continuous free space region to introduce robot size constraints, obtain the passable region, extract the skeleton of the passable region, and generate a spatial structure representation that can be used for path planning. S105 reads the preset circuit breaker installation task information, obtains the nominal pose data of the target installation station, and converts the nominal pose data to the global coordinate system established by the environmental map data to obtain the actual spatial pose of the target installation station. S106 uses the passable area data and the actual spatial pose of the target installation station as joint inputs to construct a path search space. With the shortest path as the objective, a graph search algorithm is used to solve the path search space to obtain the optimal movement path from the current position of the mobile robot to the target installation station.

3. The method for path planning and compliant docking of a mobile robot for automatic installation of circuit breakers according to claim 1, characterized in that, The process of controlling the mobile robot to move along the optimal movement path to the vicinity of the target position and determining the relative pose relationship between the circuit breaker guide wheel and the guide groove based on environmental perception information includes: S201 controls the mobile robot to move along the optimal movement path and corrects its own pose in real time through the positioning module. During the movement, it acquires the robot's real-time pose and calculates the error with the optimal movement path. Based on the error result, it performs closed-loop adjustment of the movement. S202 acquires the current pose information in real time, and determines whether the current pose is within the approach range based on the actual spatial pose of the target installation station. If so, it collects laser distance data and installation area images, and obtains the edge data of the installation area through the installation area images. S203 constructs a pose calculation equation based on laser distance data and installation area edge data. The pose calculation equation is solved through nonlinear optimization to obtain the pose deviation of the robot end relative to the installation reference. S204 combines the structural features of the guide wheel and guide groove to perform coordinate transformation on the pose deviation, obtain the relative pose parameters of the guide wheel relative to the guide groove, convert the relative pose parameters into the target adjustment pose of the end effector, control the actuator to perform recursive pose adjustment, so that the end effector gradually approaches the target pose.

4. The method for path planning and compliant docking of a mobile robot for automatic installation of circuit breakers according to claim 1, characterized in that, The extraction of several time-frequency features of the current time window based on the lateral contact state characteristics between the guide wheel and the guide groove includes: S3031 calculates the total energy within the time window by integrating the time-spectrum energy of the signal within the time window. The calculation formula is as follows: ， in, This represents the total energy within the time window t. Indicates time Location, frequency Spectral amplitude, This represents the energy at the corresponding frequency. and Indicates the frequency integral range; S3032 calculates the weighted average frequency of the spectral energy as the energy centroid frequency, using the following formula: ， in, This represents the energy centroid frequency within the time window t; S3033 calculates the energy change rate by the energy difference between adjacent time windows, using the following formula: ， in, This represents the rate of energy change at time window t. This represents the total energy of the current window. This represents the total energy in the previous time window. Indicates the window sliding step size; The correlation coefficient between the lateral force and the vertical force is calculated using the Pearson correlation coefficient for S3034. The calculation formula is as follows: ， in, This represents the correlation coefficient within the time window t. Indicates lateral force Torque of vertical force covariance, Indicates lateral force standard deviation Torque representing vertical force The standard deviation.

5. The method for path planning and compliant docking of a mobile robot for automatic installation of circuit breakers according to claim 1, characterized in that, The process, based on a pre-established contact stage feature model, matches the time-frequency features of the current time window with the reference features of each contact stage to determine the current contact stage, including: S3041 generates force / torque signal data under different contact stages through experimental acquisition or simulation, and extracts the corresponding time-frequency characteristic parameters, including: total energy within the window, energy centroid frequency, energy change rate, and correlation coefficient. S3042 labels each feature vector with a corresponding contact stage phase label, which includes: approach stage, unilateral contact stage, centering stage, introduction stage, and push-in stage. The feature vectors under each phase category are statistically processed to calculate their mean vector, which is used as a reference template for that phase. S3043 calculates the Euclidean distance between the current feature vector and each phase reference template, and selects the phase with the smallest distance as the current recognition result. The calculation formula is as follows: ， in, This represents several time-frequency characteristics of the current time window. Indicates the reference template for each phase, Denotes the Euclidean norm. This indicates the contact phase corresponding to the current time window.

6. The method for path planning and compliant docking of a mobile robot for automatic installation of circuit breakers according to claim 1, characterized in that, The process of adaptively adjusting the system damping parameters and dynamically controlling the movement of the end effector according to the contact stage, so that the guide wheel can be continuously guided along the guide groove, thereby completing the compliant docking of the circuit breaker, includes: S401 divides the contact stages according to the contact state between the guide wheel and the guide groove during the automated alignment process, and determines the corresponding damping parameters based on the contact stages. S402 stores the damping parameters corresponding to each contact stage in association, forming a stage-damping parameter mapping relationship. Based on the contact stage identification results, it selects the damping parameters of the corresponding stage in real time. When S403 detects a change in the contact phase, it introduces a smooth transition function to interpolate the damping parameters, so that the current damping parameters smoothly transition from the parameter values ​​of the previous phase to the target damping parameters of the current phase. Under impedance control, the S404 introduces the real-time updated damping parameters into the control model, and calculates the speed command or displacement correction of the end effector by combining the error between the desired pose and the actual pose. During the docking process, S405 continuously calculates the motion control quantity of the end effector based on the contact phase identification results and the real-time updated damping parameters. When a change in the contact phase is detected during the docking process, the control parameters are dynamically adjusted and the control output is updated, so that the motion state of the guide wheel transitions continuously between each phase until the docking is completed and a stable docking state is achieved.

7. The method for path planning and compliant docking of a mobile robot for automatic installation of circuit breakers according to claim 6, characterized in that, The process of dividing the contact phase based on the contact state between the guide wheel and the guide groove during the automated alignment process, and determining the corresponding damping parameters based on the contact phase, includes: S4011 When the lateral force perpendicular to the guide groove direction exceeds the lateral force threshold and the torque of the lateral force is proportional to the torque of the vertical force, it is the single-sided contact stage, and the system damping parameter is set to the high damping parameter range. S4012 When the lateral force perpendicular to the guide groove direction is less than the lateral force threshold, the torque of the vertical force is less than the torque threshold, the rate of change of the axial force is less than the rate of change of the axial force threshold, and the stabilization time window is greater than the stabilization time window threshold, it is the centering stage, and the system damping parameters are set to the medium damping parameter range. S4013 When the rate of change of the lateral force perpendicular to the guide groove direction is less than the threshold of the lateral force change and the rate of change of the torque of the vertical force is less than the threshold of the torque of the vertical force, it is the introduction stage and the pushing stage, and the system damping parameters are set to the medium damping parameter range.

8. The method for path planning and compliant docking of a mobile robot for automatic installation of circuit breakers according to claim 6, characterized in that, When a change in the contact phase is detected, a smooth transition function is introduced to interpolate the damping parameters, so that the current damping parameters smoothly transition from the parameter values ​​of the previous phase to the target damping parameters of the current phase. This includes: S4031 extends the damping parameters in each degree of freedom direction into a diagonal form of the damping matrix, and defines a smooth transition function for the damping matrix, calculated as follows: ， in, This represents the damping matrix of the previous contact stage. This represents the target damping matrix for the current contact phase. This represents the transition factor, generated through a first-order inertial element; If the contact stage identifiers obtained from two consecutive samplings are different, S4032 determines that the current contact stage has changed, initializes the transition factor, and uses the target damping parameter corresponding to the current stage as the final transition value. S4033 calculates the damping parameters in real time based on the smooth transition function, and completes the switching of damping parameters when the transition factor approaches 1.

9. A method for path planning and compliant docking of a mobile robot for automatic installation of circuit breakers according to claim 6, characterized in that, Under impedance control, the damping parameters updated in real time are introduced into the control model. Combined with the error between the desired pose and the actual pose, the velocity command or displacement correction of the end effector is calculated, including: S4041 constructs an impedance control model within the end effector's task space, and the calculation formula is as follows: ， in, This represents the desired pose increment at the end point. This represents the second derivative of the expected position increment at the endpoint with respect to time. This represents the first derivative of the expected position increment at the endpoint with respect to time. This represents the set virtual mass matrix. This represents a dynamically adjusted virtual damping matrix. Represents the virtual stiffness matrix. This represents the component of the measured contact force / torque in the task space. Indicates the desired contact force; S4042 maps the damping parameters after smooth transition to a virtual damping matrix in the impedance control model, and solves for the desired end acceleration based on the force error or pose error driving the control model. S4043 performs numerical integration on the desired acceleration to obtain the desired velocity and displacement increment, and converts the displacement increment into joint control commands for the actuator. Within N consecutive sampling periods, if the detected lateral force exceeds the lateral force threshold and the displacement is less than the displacement threshold, the contact state meets the preset jamming condition, triggering the safety protection control.

Images