Rail mobile path planning method and system for multi-station valve detection

Abstract

This invention relates to the fields of valve inspection automation and industrial robot control technology, specifically a method and system for planning a ground-rail movement path for multi-station valve inspection. It includes path planning, ground-rail positioning, torque monitoring, and posture compensation. The system obtains the target station and current position, determines the movement path based on obstacle avoidance and positioning constraints, and locks the ground rail. Its core is to obtain the real-time torque value of the robotic arm's end effector during valve opening and closing tests, calculate the end effector's elastic deformation displacement based on a pre-calibrated joint flexibility matrix, and convert it to the station coordinate system to generate posture compensation commands, controlling the servo motors of each joint to perform compensation movements. This invention achieves dynamic correction of end effector posture deviations during the testing process, effectively eliminating lateral forces in low-temperature environments, improving alignment stability, and avoiding structural damage.

Description

Technical Field

[0001] This invention relates to the fields of valve inspection automation and industrial robot control technology, specifically to a ground-rail movement path planning method and system for multi-station valve inspection. Background Technology

[0002] Existing multi-station valve testing equipment mainly relies on the positioning accuracy of the ground rail and the static teaching posture of the robotic arm to complete docking and opening / closing tests. However, it suffers from the following technical drawbacks under low-temperature conditions:

[0003] Low-temperature environments can easily cause deviations between the valve installation position and the preset position. Furthermore, when the robotic arm outputs a large test torque, its reducer and transmission structure will elastically yield. Even if the ground rail mechanism is in place normally, the end effector and the valve drive shaft may still experience positional misalignment, force eccentricity, or misalignment. The multi-station switching, in-station alignment, and test force compensation processes of existing equipment are independent of each other, making it impossible to dynamically correct the robotic arm end effector's posture based on real-time torque changes during valve opening and closing. These defects make the valve stem or sealing parts highly susceptible to additional lateral forces during low-temperature cyclic testing, resulting in poor alignment stability and structural damage. Summary of the Invention

[0004] The purpose of this invention is to provide a method and system for planning the track movement path for multi-station valve inspection, addressing the following technical problems: Existing multi-station valve inspection technologies have shortcomings in addressing installation position deviations under low-temperature conditions and dynamic posture correction during high-torque testing. There is an urgent need to propose a multi-station valve inspection equipment and planning system that integrates compliant alignment within the workstation and real-time dynamic compensation for posture deviations based on force. The objective of this invention can be achieved through the following technical solutions:

[0005] The target valve station is determined, and the current position of the ground rail is obtained; a candidate path is generated based on the current position and the target valve station, and the target movement path is determined based on obstacle avoidance constraints and station positioning constraints; the ground rail is controlled to move to the target valve station according to the target movement path, and the ground rail is positioned and locked by the positioning mechanism set on the ground rail;

[0006] The initial pose of the end effector of the robotic arm mounted on the ground rail in the coordinate system corresponding to the target valve station is obtained. The robotic arm includes multiple joints and servo motors for each joint. The real-time torque value of the end effector during the valve opening and closing test is obtained. Based on the real-time torque value and the pre-calibrated joint flexibility matrix, the elastic deformation displacement of the end effector is determined. The elastic deformation displacement is converted to the coordinate system of the station, and a pose compensation command is generated based on the converted displacement. The compensation displacement of each joint is determined according to the pose compensation command, and the servo motors of each joint are controlled to perform compensation movements.

[0007] Preferably, a coordinate system corresponding to the target valve station is established; pose alignment is performed based on the coordinate system, and during the alignment process, the brakes on each joint axis of the robotic arm are released, and the servo motor of the end effector installed at the end of the robotic arm is controlled to output detection torque; the compliant control of the end effector of the robotic arm is activated according to the detection torque, and the tool center point pose when aligned with the valve drive axis of the target valve at the target valve station is obtained; the tool center point pose is used as the initial pose of the end effector of the robotic arm.

[0008] Preferably, the real-time torque data collected by a torque sensor located at the end of the robotic arm is acquired; preset filtering parameters are acquired, the preset filtering parameters including a first-order hysteresis filter time constant and a sampling period; a filtering coefficient is determined based on the sampling period and the first-order hysteresis filter time constant; and the real-time torque data is subjected to first-order hysteresis filtering according to the filtering coefficient to obtain the real-time torque value.

[0009] Preferably, the real-time torque value is converted into a force vector of the robotic arm end in the tool coordinate system based on the valve drive radius of the target valve; the current actual joint angles of each joint of the robotic arm are obtained, and the Jacobian matrix of the robotic arm end in the tool coordinate system is constructed based on the actual joint angles; based on the force vector, the Jacobian matrix and the joint flexibility matrix, the end Cartesian space displacement vector of the robotic arm end is determined as the elastic deformation displacement.

[0010] Preferably, a preset compensation gain matrix corresponding one-to-one with each component of the elastic deformation displacement is obtained; the product of the elastic deformation displacement and the compensation gain matrix is ​​calculated to obtain a target pose compensation amount including position compensation components and attitude compensation components; the target pose is determined according to the initial pose and the target pose compensation amount; and the pose compensation command is generated according to the target pose.

[0011] Preferably, after obtaining the target pose compensation amount, the modulus of the elastic deformation displacement is calculated as the actual offset distance; a preset safe displacement threshold is obtained; if the actual offset distance is greater than the safe displacement threshold, a stop warning signal is generated, the generation of the pose compensation command is stopped, and the robotic arm is controlled to stop moving and the torque applied to the target valve at the target valve station by the end of the robotic arm is released; if the actual offset distance is not greater than the safe displacement threshold, the pose compensation command continues to be generated according to the target pose compensation amount.

[0012] Preferably, the process of constructing the pre-calibrated joint flexibility matrix is ​​as follows: obtaining the flexibility calibration value of the reducer corresponding to each joint of the robotic arm; constructing a diagonal matrix using the flexibility calibration value of the reducer corresponding to each joint as diagonal elements; and using the diagonal matrix as the joint flexibility matrix.

[0013] Preferably, the system includes a ground rail mechanism, a robotic arm mounted on the ground rail mechanism, a torque sensor disposed at the end of the robotic arm, servo motors for driving each joint of the robotic arm, a processor, and a memory. The processor is connected to the ground rail mechanism, the robotic arm, the torque sensor, and the servo motors for each joint. The memory stores computer program instructions. When the computer program instructions are executed by the processor, the above-mentioned ground rail movement path planning method for multi-position valve detection is implemented.

[0014] Compared with the prior art, the present invention has the following beneficial effects:

[0015] This solution controls the movement and positioning of the ground track based on obstacle avoidance constraints and workstation positioning constraints. During valve opening and closing tests, it determines the elastic deformation displacement based on real-time torque values ​​and pre-calibrated joint flexibility matrices, generating posture compensation commands to control the servo motors of each joint to perform compensating movements. This solution can dynamically correct end-effector posture deviations during testing, effectively eliminate lateral forces on the valve under low-temperature conditions, improve alignment stability, and avoid structural damage. Attached Figure Description

[0016] To more clearly illustrate the technical solutions in this invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of this invention. For those skilled in the art, other drawings can be obtained from these drawings without creative effort.

[0017] Figure 1 This is a flowchart illustrating a ground track movement path planning method for multi-station valve detection, as provided in an embodiment of the present invention.

[0018] Figure 2 This is a schematic diagram of a ground track movement path planning system for multi-station valve detection, provided in an embodiment of the present invention. Detailed Implementation

[0019] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to specific embodiments.

[0020] This embodiment applies to a multi-position valve testing device. The device includes a ground rail mechanism, a robotic arm mounted on the ground rail mechanism, an end effector mounted at the end of the robotic arm, a torque sensor mounted on the end effector, a servo drive component for driving the ground rail mechanism to move, servo motors for driving the joints of the robotic arm to rotate, and a processor for performing calculations and control.

[0021] The processor can be set in a control system consisting of a robot controller and a programmable logic controller. The programmable logic controller is responsible for the station switching control of the ground rail mechanism, while the robot controller is responsible for the robot arm pose calculation, torque processing, and compensation command issuance. The torque sensor is connected to the robot controller via an industrial bus, so that the end force data can participate in the control calculation within the system cycle.

[0022] In the low-temperature valve cyclic opening and closing test, the valve is distributed at multiple testing stations along the direction of movement of the ground rail; each testing station corresponds to a station coordinate system, which is a local reference coordinate system established based on the current valve installation position, and is used to uniformly describe the spatial position and posture of the robotic arm end in the vicinity of the station.

[0023] Because the equipment operates in a low-temperature environment, there may be a deviation between the actual installation position of the valve and the set position at normal temperature. If the docking is still based solely on the positioning accuracy of the ground rail mechanism and the static teaching posture of the robotic arm, the end drive head at the front end of the end actuator is prone to axis deviation due to joint elastic deformation when outputting torque exceeding the preset threshold, thus generating a lateral force on the valve stem. Therefore, this embodiment performs the ground rail station movement, station alignment, and posture compensation during the testing process in sequence.

[0024] like Figure 1 As shown, a ground track movement path planning method for multi-position valve detection includes:

[0025] Determine the target valve position and obtain the current position of the ground rail;

[0026] Candidate paths are generated based on the current location and the target valve station, and the target movement path is determined based on obstacle avoidance constraints and station positioning constraints.

[0027] The control rail moves to the target valve position according to the target movement path, and the positioning mechanism on the rail is used to position and lock the rail.

[0028] The initial pose of the end effector of the robotic arm mounted on the ground rail in the coordinate system corresponding to the target valve station is obtained. The robotic arm includes multiple joints and servo motors for each joint that drive the multiple joints.

[0029] Obtain the real-time torque value of the robotic arm's end effector during the valve opening and closing test process;

[0030] Based on the real-time torque value and the pre-calibrated joint flexibility matrix, the elastic deformation displacement of the end effector of the robotic arm is determined.

[0031] The elastic deformation displacement is transformed to the workstation coordinate system, and a pose compensation command is generated based on the transformed displacement.

[0032] The compensating displacement of each joint is determined according to the pose compensation command, and the servo motors of each joint are controlled to perform the compensating motion.

[0033] When the upper-level scheduling logic provides the valve to be detected, the programmable logic controller determines the location of the valve as the target valve station and reads the current position of the ground rail mechanism; the current position can be obtained by feedback from the absolute encoder of the ground rail, and the feedback result is expressed as the current position data of the ground rail in the track direction;

[0034] The programmable logic controller (PLC) generates candidate paths based on the current position and the target valve station position. Each candidate path represents a feasible movement plan from the current position to the target valve station. The specific generation rule is as follows: starting from the one-dimensional coordinates of the current position of the ground rail and ending at the one-dimensional coordinates of the center of the target valve station, the path is generated according to a preset distance step size that matches the positioning accuracy of the ground rail mechanism. Discretization interpolation is performed within the physical track interval between the start and end points;

[0035] The ground rail moves along the existing track. The candidate path is represented by the positioning scheme along the track direction and the feasible range related to avoidance of surrounding mechanisms. The obstacle avoidance constraint is used to eliminate the path that interferes with the tank boundary, tooling structure or adjacent workstation equipment. The workstation positioning constraint is used to ensure that after the ground rail stops, the working space of the robotic arm can cover the target valve and meet the subsequent locking requirements.

[0036] The specific judgment calculation rules are as follows: the candidate path is discretized into multiple location sampling points along the extension direction of the ground track; for obstacle avoidance constraints, the minimum distance from each location sampling point to the pre-stored three-dimensional surrounding obstacle bounding box is calculated one by one. The three-dimensional surrounding obstacle bounding box is a geometric boundary box pre-constructed based on the computer-aided design three-dimensional model of the peripheral equipment and tooling or the environmental scanning point cloud data, used to enclose the external spatial shape of the obstacle.

[0037] If the minimum distance corresponding to all sampling points is greater than the preset safety margin, the candidate path is determined to meet the avoidance constraint; otherwise, it is determined not to meet the avoidance constraint and is removed from the candidate path set. The safety margin is a distance threshold set based on the motion control system error of the ground rail mechanism and the maximum extension size of the robotic arm. For example, it can be set to a specific value between 50 mm and 150 mm.

[0038] For the station positioning constraint, the absolute value of the deviation between the one-dimensional coordinates of the candidate path endpoint and the one-dimensional coordinates of the ideal center of the target valve station is calculated. If the absolute value of the deviation is less than or equal to the effective locking and capture tolerance distance of the positioning mechanism, the candidate path is determined to meet the positioning constraint; otherwise, it is determined not to meet the positioning constraint. The effective locking and capture tolerance distance refers to the maximum initial position deviation limit allowed by the positioning mechanism such as the pin locking structure to successfully complete the locking action by relying on the guide ramp or its own driving force without mechanical hard interference. After comparing the calculation results of all candidate paths, the programmable logic controller retains the path that simultaneously meets the above two types of constraints as the target movement path. If there are multiple feasible paths, the one with the shortest total movement distance is selected as the uniquely determined target movement path.

[0039] After the target movement path is determined, the programmable logic controller (PLC) sends a movement command to the ground rail servo drive component, controlling the ground rail mechanism to move along the target movement path to the target valve station. When the current position fed back by the encoder meets the station positioning condition, the controller triggers the positioning mechanism to act. The positioning mechanism can be an electrically controlled pin locking structure, the function of which is to fix the ground rail base at the target station, reducing the additional displacement of the ground rail in subsequent opening and closing tests. After the positioning mechanism completes locking, the processor switches the current station to the target valve station and calls the station coordinate system parameters corresponding to the station, providing a unified reference for the subsequent end pose expression.

[0040] Obtain the initial pose of the end effector of the robotic arm mounted on the ground rail in the coordinate system corresponding to the target valve station, including:

[0041] Establish a workstation coordinate system corresponding to the target valve workstation;

[0042] The pose alignment is performed based on the workstation coordinate system, and the brakes of each joint axis of the robotic arm are released during the alignment process to control the servo motor built into the end effector installed at the end of the robotic arm to output the detection torque.

[0043] The tool center point pose is obtained when the end of the robotic arm is compliantly controlled based on the detected torque and aligned with the valve drive axis of the target valve at the target valve station.

[0044] Use the tool center point pose as the initial pose of the robotic arm's end effector.

[0045] The initial pose is the tool center point pose after the end effector of the robot arm is aligned with the valve drive axis at the target valve station, and it serves as the reference pose for subsequent compensation calculations. The robot controller first establishes a station coordinate system corresponding to the target valve station and guides the end effector of the robot arm to a preset approach position. After reaching the approach position, instead of locking the brakes of each joint axis, the brakes of each joint axis are released, so that each joint remains in a servo enabled state.

[0046] At the same time, the robot controller controls the servo motor of the end effector to output the detection torque; the detection torque is a torque that meets the detection threshold range for aligning the valve drive axis, and its magnitude is selected to be sufficient to produce guiding contact without damaging the valve sealing structure.

[0047] The magnitude of the detection torque is determined by obtaining the allowable torque limit of the valve sealing component through prior structural strength analysis or calibration. 5% to 10% of this allowable torque limit is then used as the specific value for the detection torque. After the detection torque is established, the robot controller initiates compliant control at the end effector of the robotic arm. Specifically, this is achieved by enabling an admittance control algorithm to establish the dynamic equations of the end-effector contact force and position deviation. ,in , , These are preset 6×6-dimensional target mass, damping, and stiffness matrices, used to characterize the desired inertia, damping, and stiffness characteristics of the robotic arm's end effector in each degree of freedom in Cartesian space. To detect the actual contact feedback force after torque conversion, and Let the acceleration and velocity be the end-position yield, respectively. The position yield amount for each control cycle is obtained by discretizing and solving this equation. This allows for dynamic correction of the target reference position, enabling the end valve to have displacement clearance in a plane perpendicular to the valve axis.

[0048] At this time, if there is a positional deviation between the end effector and the valve drive, the guiding effect at the contact point will cause the end effector to adjust its position until the end effector and the valve drive axis are aligned. The condition for determining whether the alignment state has been reached is: the robot controller continuously monitors the real-time position change rate of the end effector during the compliant adjustment process. When the position change rate is less than the preset alignment stability threshold in multiple consecutive system cycles, and the detection torque output by the end effector does not change abruptly, it is determined that the guiding sliding process has ended and entered a stable mechanical engagement state, which is confirmed as the alignment state.

[0049] The preset alignment stability threshold is determined as follows: extract the natural position fluctuation data of the device when it is in servo-enabled hovering state under normal working conditions, calculate the standard deviation of the fluctuation, and set 3 times the standard deviation as the alignment stability threshold; when the alignment is confirmed, the robot controller reads the tool center point pose and records the tool center point pose as the initial pose of the end of the robot arm in the coordinate system of the corresponding station of the target valve station.

[0050] Obtain the real-time torque value of the robotic arm's end effector during the valve opening and closing test process, including:

[0051] Acquire real-time torque data from a torque sensor located at the end of the robotic arm;

[0052] Obtain the preset filter parameters, which include the first-order lag filter time constant and the sampling period;

[0053] The filter coefficients are determined based on the sampling period and the first-order hysteresis filter time constant.

[0054] The real-time torque data is subjected to first-order hysteresis filtering based on the filtering coefficients to obtain the real-time torque value.

[0055] During the valve opening and closing test, the end effector outputs test torque to the target valve, and the torque sensor synchronously collects real-time torque data at the end of the robotic arm. The real-time torque data is the raw measurement value output by the torque sensor, which reflects the actual torque change borne by the end during the valve opening and closing process. Since the meshing of the reducer and the change of internal resistance of the valve may introduce high-frequency fluctuations, the robot controller does not directly use the raw measurement value for compensation calculation, but performs filtering processing first.

[0056] During filtering, the robot controller reads preset filtering parameters. The preset filtering parameters include a first-order hysteresis filter time constant and a sampling period. The sampling period corresponds to the update period of torque data in the industrial bus, and the time constant is used to characterize the degree of suppression of high-frequency fluctuations by the filter. The robot controller determines the filtering coefficients based on the ratio of the sampling period to the sum of the sampling period and the first-order hysteresis filter time constant.

[0057] In the specific filtering process, by adjusting the sampling period With this sampling period and first-order hysteresis filter time constant The quotient of the sum is determined as the filter coefficient. The calculation formula is: After the filter coefficients are determined, the robot controller processes the real-time torque data uploaded by the torque sensor cycle by cycle according to the first-order hysteresis filter formula.

[0058] The specific filtering and allocation logic is as follows: The real-time torque data collected in the current cycle is... Multiply by the filter coefficient and the filtered output results from the previous cycle. Multiply With filter coefficients The difference between the two values, when summed, gives the real-time torque value output in the current cycle. ,in, This is the current discrete sampling period number. The first-order lag filter is calculated by using the previous discrete sampling period number as the index, so that the current period's output value contains information from both the current sample value and the filtering result from the previous period. The specific mathematical formula for this first-order lag filter is as follows: ;

[0059] For example, if the sampling period The first-order lag filter time constant is 10 milliseconds. If we set it to 90 milliseconds, then the filter coefficients calculated using the aforementioned proportional formula are... The value is 0.1; the system will take 10% of the current real-time torque data acquisition value and add it to 90% of the filtered output result of the previous cycle to obtain the real-time torque value.

[0060] After obtaining the real-time torque value, the robot controller calculates the elastic deformation displacement of the end effector of the robotic arm by combining the pre-calibrated joint flexibility matrix. The joint flexibility matrix reflects the ability of each joint to generate elastic angular displacement under force, and essentially corresponds to the flexibility characteristics of the output end of each joint reducer. The robot controller converts the real-time torque value into the force input of the end effector of the robotic arm in the tool coordinate system, and then establishes the Jacobian matrix under the current posture based on the actual joint angle fed back by the current encoder of each joint.

[0061] The end-effector force, Jacobian matrix, and joint flexibility matrix are calculated sequentially to obtain the displacement change of the end-effector in Cartesian space. This displacement change represents the position and attitude shift that the end-effector will undergo under the current force conditions, and is therefore determined as the elastic deformation displacement of the end-effector.

[0062] The robot controller will transform the elastic deformation displacement expressed by the tool coordinate system or the end-effector coordinate system to the station coordinate system corresponding to the target valve station. The displacement data obtained after the transformation belongs to the same reference system as the initial pose and can be directly used to generate pose compensation commands.

[0063] The robot controller uses the initial pose as a reference, takes the inverse compensation relationship of the transformed displacement to generate a new target pose, and then converts the target pose into a pose compensation command that the robot arm can execute. After the pose compensation command is solved by inverse kinematics, the compensation displacement of each joint is obtained. The robot controller further sends the compensation displacement of each joint to the servo motor of each joint, so that the servo motor of each joint performs the corresponding compensation movement during the continuous opening and closing of the valve. Thus, the robot arm does not remain stationary, but makes a small adjustment in the direction of force to reduce the deviation of the end effector drive axis from the valve drive axis.

[0064] The transfer of the ground rail station, the compliant alignment within the station, and the torque compensation during the testing process are executed in sequence, and the positional deviation is corrected by real-time force data.

[0065] In this embodiment, the process of obtaining the elastic deformation displacement of the robotic arm end effector, the process of forming the pose compensation command, the safety determination process, and the process of establishing the joint flexibility matrix are described. In this embodiment, the robot controller is still used as the main computing entity, and the encoders, torque sensors, and end effectors of each joint are used as data sources and motion execution components.

[0066] The process of constructing the pre-calibrated joint flexibility matrix includes:

[0067] Obtain the flexibility calibration values ​​of the reducers corresponding to each joint of the robotic arm;

[0068] Construct a diagonal matrix using the compliance calibration values ​​of the reducer corresponding to each joint as diagonal elements;

[0069] Use the diagonal matrix as the joint flexibility matrix.

[0070] The system pre-establishes a joint flexibility matrix; the joint flexibility matrix is ​​obtained from the flexibility calibration values ​​of the reducers corresponding to each joint of the robotic arm. The flexibility calibration values ​​reflect the magnitude of the elastic angular displacement of the joint under unit torque. When establishing the matrix, the flexibility calibration values ​​of the reducers corresponding to each joint are first obtained, and then the flexibility calibration values ​​of each joint are arranged as diagonal elements according to the joint order to construct a diagonal matrix. This diagonal matrix is ​​then stored in the robot controller as the joint flexibility matrix.

[0071] This diagonal matrix reflects the main elastic yield characteristics of each joint's own reducer under force; the compliance calibration value can be derived from the dynamic calibration results before equipment deployment, or from the angular displacement ratio measured after loading a known torque on site. Both correspond to the actual elastic parameters of the mechanical components, rather than abstract set values.

[0072] Based on the real-time torque value and the pre-calibrated joint flexibility matrix, the elastic deformation displacement of the robotic arm's end effector is determined, including:

[0073] Establish the tool coordinate system at the end of the robotic arm, and convert the real-time torque value into the force vector of the end of the robotic arm in the tool coordinate system based on the valve drive radius of the target valve at the target valve station.

[0074] Obtain the current actual joint angles of each joint of the robotic arm, and construct the Jacobian matrix of the robotic arm's end effector in the tool coordinate system based on the actual joint angles;

[0075] Spatial displacement mapping calculations are performed based on the force vector, Jacobian matrix, and joint flexibility matrix to determine the end-effector Cartesian spatial displacement vector of the robotic arm, which serves as the elastic deformation displacement.

[0076] During the valve opening and closing test, after receiving the filtered real-time torque value, the robot controller first converts it in conjunction with the valve drive radius of the target valve. The valve drive radius is the equivalent force arm size when the output torque of the end effector acts on the valve drive part. Its specific value is obtained by the system by reading the pre-stored target valve specification database or calling its computer-aided design parameters. The robot controller constructs a six-dimensional tool coordinate system containing three force components along the coordinate axes and three torque components around the coordinate axes as force vectors.

[0077] Let the rotation axis direction of the end-effector coordinate system, such as the Z-axis, be the driving rotation axis. The robot controller directly uses the real-time torque value as the torque component around this rotation axis to maintain the torque dimension of Newton-meter. At the same time, the real-time torque value is divided by the valve driving radius to calculate the magnitude of the maximum equivalent lateral force caused by eccentric compression at the contact point. The eccentric direction vector calculated based on the real-time rotation angle of the robot arm end is introduced to project the maximum equivalent lateral force onto two orthogonal force components perpendicular to the rotation axis in the tool coordinate system.

[0078] Meanwhile, when constructing the torque component around the rotation axis, the derived torque caused by the product of the eccentric force and the valve drive radius is subtracted from the total measured torque; for the other dimensional components that do not bear resistance exceeding the preset resistance threshold, the value is set to zero, so as to convert the scalar output into a force vector containing the reverse resistance torque and the lateral deviation force.

[0079] After the force vector is determined, the robot controller reads the current actual joint angles of each joint of the robotic arm from the encoders of the servo motors of each joint. The actual joint angles directly reflect the current posture of the robotic arm and are the input for constructing the Jacobian matrix. Based on the set of actual joint angles, the robot controller calls the kinematic model of the robotic arm in the current tool coordinate system and constructs the Jacobian matrix of the end effector in the tool coordinate system. The Jacobian matrix is ​​used to represent the mapping relationship between the joint space and the end effector Cartesian space. In this embodiment, the Jacobian matrix maps the end effector force vector to the equivalent joint torque borne by each joint on the one hand, and maps the joint elastic displacement back to the end effector displacement on the other hand. Therefore, it is a mapping matrix that realizes the conversion between end effector force and end effector deformation.

[0080] After the Jacobian matrix and joint flexibility matrix are determined, the robot controller performs matrix operations in sequence according to the preset calculation relationship: first, the force vector is mapped to the joint side through the transpose of the Jacobian matrix to obtain the force torque corresponding to each joint; then, the force torque of each joint is multiplied by the joint flexibility matrix to obtain the angular displacement of each joint due to elastic yielding.

[0081] The joint angular displacement is then converted into an end-effector Cartesian space displacement vector using the Jacobian matrix; the specific calculation logic is as follows: based on the force vector of the end-effector in the tool coordinate system... Jacobian matrix and its transpose Joint flexibility matrix The displacement vector in the Cartesian space at the end was calculated. The comprehensive mapping relationship formula is as follows: ;

[0082] The specific calculation process is as follows: The robot controller calculates the force vector... Transpose of the Jacobian matrix Multiply by the values ​​to calculate the torque on each joint; then multiply the torque on each joint by the joint flexibility matrix. Multiply by the product to calculate the angular displacement of each joint due to elastic yielding; then multiply the angular displacement of each joint by the Jacobian matrix. Multiplying them together yields the final Cartesian space displacement vector. ;

[0083] For example, when the force vector indicates a lateral force of 10 Newtons at the end, the equivalent joint torque of 0.5 Nm is generated by transforming the force vector to the corresponding joint through the Jacobian matrix. Combined with the joint's calibration flexibility of 0.01 radians / Newton, the elastic angular displacement of the joint is obtained as 0.005 radians. Then, through positive mapping of the Jacobian matrix, the end displacement change of 0.1 mm is obtained, thereby realizing the structured decomposition of data flow and displacement determination.

[0084] The Cartesian space displacement vector of the end contains both position change components and attitude change components, which can characterize the actual yielding trend of the end relative to the ideal alignment position under the current force state. Therefore, it is used as the elastic deformation displacement in the embodiment.

[0085] The elastic deformation displacement is transformed to the workstation coordinate system, and a pose compensation command is generated based on the transformed displacement, including:

[0086] Obtain the preset compensation gain matrix that corresponds one-to-one with the components of elastic deformation displacement;

[0087] Calculate the product of elastic deformation displacement and compensation gain matrix to obtain the target pose compensation amount, which includes position compensation component and attitude compensation component;

[0088] The target pose is determined based on the initial pose and the target pose compensation amount.

[0089] Generate pose compensation commands based on the target pose.

[0090] After obtaining the elastic deformation displacement, the robot controller first transforms the displacement to the coordinate system corresponding to the current workstation. This transformation ensures that the compensation amount and the initial pose are in the same reference coordinate system, thus avoiding directional errors caused by direct superposition under different coordinate expressions. After the transformation, the robot controller calls the preset compensation gain matrix. The compensation gain matrix corresponds one-to-one with each component of the elastic deformation displacement, preferably using a diagonal matrix form, so that the position and attitude components are scaled according to their respective gains. The gain is not directly taken as the full compensation value, but a preset safety margin is reserved under the premise of satisfying the compensation effect, so as to reduce the vibration risk caused by rigid interference.

[0091] The specific method for obtaining the compensation gain matrix is ​​as follows: During the trial operation phase of equipment deployment, a step load is applied to the end of the robotic arm and the settling time required for the end to move from elastic displacement to vibration convergence is recorded; multiple closed-loop compensation tests are conducted by increasing the gain value in preset steps, and the dynamic response performance of each compensation process is recorded. The gain that minimizes the end-to-end settling time and does not produce overshoot oscillation is recorded as the critical gain; 80% to 90% of this critical gain is taken as the values ​​of the diagonal elements corresponding to the position compensation component and the attitude compensation component, thereby ensuring the timeliness of the compensation response and avoiding the excitation of dynamic vibration of the mechanism during actual compensation.

[0092] The robot controller calculates the product of the elastic deformation displacement and the compensation gain matrix to obtain the target pose compensation amount. Specifically, the robot controller performs matrix multiplication on the compensation gain matrix and the elastic deformation displacement to calculate the target pose compensation amount. The target pose compensation amount consists of a position compensation component and an attitude compensation component, wherein the position compensation component is used to correct the spatial coordinates of the end tool center point, and the attitude compensation component is used to correct the angular deviation of the end tool around each axis.

[0093] After obtaining the target pose compensation amount, the following is also included:

[0094] Calculate the modulus of elastic deformation displacement as the actual offset distance;

[0095] Obtain the preset safe displacement threshold;

[0096] If the actual offset distance is greater than the safe displacement threshold, a stop warning signal is generated, the generation of pose compensation commands is stopped, the robotic arm is controlled to stop moving, and the torque applied to the target valve at the target valve station by the end of the robotic arm is released; otherwise, pose compensation commands continue to be generated according to the target pose compensation amount.

[0097] After obtaining the target pose compensation amount, the robot controller does not immediately generate pose compensation instructions, but first performs a safety judgment. Specifically, it calculates the modulus of each component of the elastic deformation displacement to obtain the actual offset distance. Since the elastic deformation displacement contains both position change components and attitude change components, the modulus calculation is as follows: the three position change components in the elastic deformation displacement are extracted and the square root of the sum of squares is calculated, or the three attitude change components are multiplied by the valve drive radius to convert them into position equivalent quantities and then the sum of squares of the original position change components is calculated together to obtain a scalar value with a unified length dimension.

[0098] The actual offset distance reflects the overall degree of end-effector retreat under the current stress conditions. The preset safe displacement threshold is read and compared with the actual offset distance. The safe displacement threshold is derived from the limit deformation calibration result of the robotic arm under test torque plus a safety margin, and is used to distinguish between normal elastic retreat and abnormal interference or jamming. The specific determination method is as follows: the maximum rated test torque allowed by the system design is converted into the end-effector force input into the mapping model of Jacobian matrix and joint flexibility matrix, the theoretical limit modulus is calculated, and the theoretical limit modulus is multiplied by a safety factor of 1.2 to 1.5 as the safe displacement threshold.

[0099] When the actual offset distance exceeds the safe displacement threshold, the robot controller determines that the current state exceeds the normal compensation range, generates a stop warning signal, and sends a stop motion command to the robotic arm control module. At the same time, it sends a control command to release the drive torque to the end effector. This avoids continuing compensation under conditions of mechanical interference, valve freezing, or abnormal jamming, reducing damage to the robotic arm reducer and valve structure. If the actual offset distance is not greater than the safe displacement threshold, it means that the current torque change is still within the allowable compensation range, and the robot controller continues to generate pose compensation commands based on the target pose compensation amount.

[0100] If further compensation is permitted, the robot controller uses the initial pose as a reference and superimposes the target pose compensation onto the initial pose to obtain the target pose. The specific mathematical mapping rule is as follows: the target pose compensation is decomposed, and for the position compensation component, a three-dimensional vector addition is performed with the translation vector in the initial pose to obtain the corrected target translation vector; for the attitude compensation component, it is denoted as a small rotation angle vector. ,in, , , They represent circumference respectively. axis, axis, The minute rotational angular components of the axis, superscript To represent the matrix transpose, we can directly construct the corresponding incremental rotation matrix using the small angle approximation rule. Its specific algebraic construction formula is: ;

[0101] Since the aforementioned target pose compensation amount has been uniformly transformed to the fixed workstation coordinate system reference, the robot controller performs a left multiplication operation by multiplying the incremental rotation matrix by the initial rotation matrix corresponding to the initial pose from the left to obtain the corrected target rotation matrix; the target pose with a homogeneous transformation matrix structure is constructed by recombining the target translation vector and the target rotation matrix.

[0102] The target pose here is the corrected pose that the robotic arm end effector should reach within the current system cycle. Its direction is opposite to the predicted elastic deformation, and it is used to counteract the offset caused by the force. The robot controller then generates a pose compensation command based on the target pose and converts it into compensation displacement for each joint through inverse kinematics, and sends it to the corresponding joint servo motors. After receiving the compensation displacement, each joint servo motor performs linkage adjustment based on the compensation displacement while keeping the servo enabled, so that the end effector can maintain alignment with the valve drive axis as much as possible while continuously outputting valve drive torque.

[0103] The system converts torque into a force relationship, combines the Jacobian matrix and compliance characteristics to determine the compensation amount, and performs displacement threshold determination before generating pose compensation commands.

[0104] like Figure 2 As shown, a ground-rail movement path planning system for multi-station valve detection includes a ground-rail mechanism, a robotic arm mounted on the ground-rail mechanism, a torque sensor located at the end of the robotic arm, servo motors for each joint of the robotic arm, a processor, and a memory.

[0105] The processor is connected to the ground track mechanism, the robotic arm, the torque sensor, and the servo motors of each joint.

[0106] The memory stores computer program instructions, which, when executed by the processor, implement the ground track movement path planning method for multi-position valve detection in the embodiment.

[0107] The system includes a ground rail mechanism, a robotic arm mounted on the ground rail mechanism, a torque sensor located at the end of the robotic arm, servo motors for each joint of the robotic arm, a processor, and a memory. The processor is connected to the ground rail mechanism, the robotic arm, the torque sensor, and the servo motors for each joint. The memory stores computer program instructions. After the processor reads and executes the computer program instructions, it is used to complete the ground rail station movement control, initial pose determination, torque filtering, elastic deformation displacement calculation, pose compensation, and safe shutdown processing in the aforementioned embodiments.

[0108] The system includes a ground rail mechanism to support the robotic arm as it moves between multiple valve inspection stations and locks it at the target station in conjunction with a positioning mechanism; a robotic arm to achieve spatial alignment between the end effector and the valve drive unit after the ground rail is in place, and to perform compensating motion during valve opening and closing; a torque sensor to collect real-time torque data corresponding to the output torque of the end effector, which serves as input for the processor to calculate elastic deformation displacement; servo motors for each joint to drive the robotic arm to complete routine posture adjustments and to perform small joint displacement corrections during the compensation phase; and a memory to store operating parameters such as station coordinate data, joint flexibility matrix, filtering parameters, compensation gain matrix, and safety displacement threshold.

[0109] When the system is working, the processor first determines the target valve station based on the current detection task and reads the current position of the ground rail from the position feedback unit of the ground rail mechanism. Based on the relationship between the current position and the target valve station, combined with obstacle avoidance constraints and station positioning constraints, the processor outputs the control command corresponding to the target movement path to the ground rail mechanism. After the ground rail mechanism reaches the target valve station, the processor controls the positioning mechanism to lock and switches to the station coordinate system corresponding to the target valve station. Then, the processor controls the robotic arm to complete compliant alignment in servo-enabled state, obtains the pose of the end tool center point and uses it as the initial pose.

[0110] After entering the valve opening and closing test process, the torque sensor continuously sends real-time torque data to the processor. The processor processes the real-time torque data according to the preset first-order hysteresis filter parameters in the memory to obtain the real-time torque value. Then, combined with the valve drive radius, the current actual joint angle, the Jacobian matrix, and the joint flexibility matrix, the elastic deformation displacement of the end of the robotic arm is obtained. The processor further transforms the elastic deformation displacement into the work position coordinate system and calculates the target pose compensation amount in combination with the compensation gain matrix.

[0111] If the actual offset distance calculated from the elastic deformation displacement is within the safe displacement threshold, the processor generates a pose compensation command, converts it into compensation displacement for each joint, and sends it to the servo motor of each joint; if the actual offset distance exceeds the safe displacement threshold, the processor outputs a stop warning signal, controls the robotic arm to stop moving, and releases the drive torque applied to the target valve by the end effector.

[0112] The system integrates the functions of ground rail station movement, robotic arm posture adjustment, end effector torque acquisition, and processor calculation. During valve opening and closing tests, the processor uses the objective physical quantities fed back by the torque sensor to calculate the elastic displacement of the robotic arm caused by the force, and then drives the servo motors of each joint to perform corresponding compensation, thereby reducing the offset between the end effector and the valve drive axis. The system is applied to the cyclic opening and closing detection of multi-station valves in low-temperature environments.

[0113] It should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit it. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can be made to the technical solutions of the present invention without departing from the spirit and scope of the technical solutions of the present invention.

Claims

1. A method for planning the track movement path for multi-position valve detection, characterized in that, include: Determine the target valve position and obtain the current position of the ground rail; Candidate paths are generated based on the current position and the target valve station, and the target movement path is determined based on obstacle avoidance constraints and station positioning constraints. The ground rail is controlled to move to the target valve position according to the target movement path, and the ground rail is positioned and locked by the positioning mechanism set on the ground rail; The initial pose of the end effector of the robotic arm mounted on the ground rail in the coordinate system corresponding to the target valve station is obtained. The robotic arm includes multiple joints and servo motors for each joint that drive the multiple joints. Obtain the real-time torque value of the end effector of the robotic arm during the valve opening and closing test process; Based on the real-time torque value and the pre-calibrated joint flexibility matrix, the elastic deformation displacement of the end of the robotic arm is determined. The elastic deformation displacement is converted to the workstation coordinate system, and a pose compensation command is generated based on the converted displacement. The compensating displacement of each joint is determined according to the posture compensation command, and the servo motors of each joint are controlled to perform the compensating motion.

2. The method for planning a ground track movement path for multi-position valve detection according to claim 1, characterized in that, Obtaining the initial pose of the end effector of the robotic arm mounted on the ground rail in the coordinate system corresponding to the target valve station includes: Establish the workstation coordinate system corresponding to the target valve workstation; The pose alignment is performed based on the workstation coordinate system, and the brakes of each joint axis of the robotic arm are released during the alignment process to control the servo motor built into the end effector installed at the end of the robotic arm to output the detection torque. The compliant control of the end effector of the robotic arm is activated based on the detected torque to obtain the tool center point pose when aligned with the valve drive axis of the target valve at the target valve station. The pose of the tool's center point is used as the initial pose of the robotic arm's end effector.

3. The method for planning a ground track movement path for multi-position valve detection according to claim 1, characterized in that, Obtaining the real-time torque value of the robotic arm's end effector during the valve opening and closing test includes: Acquire real-time torque data from a torque sensor located at the end of the robotic arm; Obtain preset filtering parameters, which include a first-order lag filter time constant and a sampling period; The filter coefficients are determined based on the sampling period and the first-order hysteresis filter time constant. The real-time torque data is subjected to first-order hysteresis filtering based on the filtering coefficients to obtain the real-time torque value.

4. The method for planning a ground track movement path for multi-position valve detection according to claim 1, characterized in that, Based on the real-time torque value and the pre-calibrated joint flexibility matrix, the elastic deformation displacement of the end effector of the robotic arm is determined, including: Establish the tool coordinate system at the end of the robotic arm, and convert the real-time torque value into the force vector of the end of the robotic arm in the tool coordinate system based on the valve drive radius of the target valve at the target valve station. Obtain the current actual joint angles of each joint of the robotic arm, and construct the Jacobian matrix of the end effector of the robotic arm in the tool coordinate system based on the actual joint angles; Based on the force vector, the Jacobian matrix, and the joint flexibility matrix, spatial displacement mapping calculation is performed to determine the end-effector Cartesian spatial displacement vector of the robotic arm, which is then used as the elastic deformation displacement.

5. The method for planning a ground track movement path for multi-position valve detection according to claim 1, characterized in that, The elastic deformation displacement is transformed to the workstation coordinate system, and a pose compensation command is generated based on the transformed displacement, including: Obtain a preset compensation gain matrix that corresponds one-to-one with the components of the elastic deformation displacement; Calculate the product of the elastic deformation displacement and the compensation gain matrix to obtain the target pose compensation amount, which includes position compensation components and attitude compensation components; The target pose is determined based on the initial pose and the target pose compensation amount; The pose compensation command is generated based on the target pose.

6. The method for planning a ground track movement path for multi-station valve detection according to claim 5, characterized in that, After obtaining the target pose compensation amount, the method further includes: Calculate the modulus of the elastic deformation displacement as the actual offset distance; Obtain the preset safe displacement threshold; If the actual offset distance is greater than the safe displacement threshold, a stop warning signal is generated, the generation of the pose compensation command is stopped, and the robotic arm is controlled to stop moving and the torque applied to the target valve at the target valve station by the end of the robotic arm is released; otherwise, the pose compensation command continues to be generated according to the target pose compensation amount.

7. The method for planning a ground track movement path for multi-position valve detection according to claim 1, characterized in that, The process of constructing the pre-calibrated joint flexibility matrix includes: Obtain the flexibility calibration value of the reducer corresponding to each joint of the robotic arm; Construct a diagonal matrix using the compliance calibration values ​​of the reducers corresponding to each joint as diagonal elements; The diagonal matrix is ​​used as the joint flexibility matrix.

8. A track-based movement path planning system for multi-position valve detection, characterized in that, It includes a ground rail mechanism, a robotic arm mounted on the ground rail mechanism, a torque sensor located at the end of the robotic arm, servo motors for driving each joint of the robotic arm, a processor, and a memory. The processor is connected to the ground rail mechanism, the robotic arm, the torque sensor, and the servo motors of each joint. The memory stores computer program instructions, which, when executed by the processor, implement the ground track movement path planning method for multi-position valve detection as described in any one of claims 1 to 7.

Images