Driving state monitoring control method and system based on deformation compensation

Abstract

The application belongs to the technical field of robot control, and discloses a driving state monitoring control method and system based on deformation compensation, which comprises the following steps: dividing a robot into a driving section and a non-driving section, constructing a positive motion model of the driving section of the robot according to the geometric parameters and kinematic relations of the driving section of the robot; collecting a target end trajectory, and inversely calculating a theoretical take-up displacement in combination with the positive motion model of the driving section; collecting an actual take-up displacement, and calculating a total load of the driving end and a change rate thereof in combination with pre-calibrated elastic parameters; collecting an environmental image sequence, and obtaining a measured displacement speed through feature extraction and matching; collecting motor currents, and performing state discrimination in combination with the currents, the speed, the load and the change rate; and performing a hierarchical response according to the discrimination result; and the application improves the state monitoring accuracy and control reliability of a rope-driven flexible robot in a complex pipeline environment.

Description

Technical Field

[0001] This invention relates to the field of robot control technology, and more specifically, to a drive state monitoring and control method and system based on deformation compensation. Background Technology

[0002] In the drive control of the rope-driven flexible pipe endoscopy robot, the drive motor drives the robot body to move along the pipe by retracting and extending the rope.

[0003] Current status monitoring methods typically rely on the amplitude of the drive motor current to determine whether jamming has occurred, identifying an anomaly when the current exceeds a preset threshold. However, ropes themselves are elastic, and under load, they undergo non-negligible elastic deformation, resulting in a dynamic deviation between the actual retraction / extension displacement at the drive end and the theoretical retraction / extension displacement calculated based on kinematic models. Current technologies do not collect and utilize this deviation, thus failing to sense the actual tension of the rope online and accurately estimate the total load borne by the drive motor output. Under these circumstances, when the robot encounters bending resistance or jamming, the current threshold alone cannot distinguish the source of the resistance: both internal mechanical jamming in the drive section and axial self-locking in the non-drive section will cause an increase in current, but their physical mechanisms are different, requiring diametrically opposed response strategies. The former requires an emergency stop, while the latter can be unlocked through reverse micro-retraction.

[0004] Existing technologies are prone to misjudging non-drive section self-locking as drive section jamming, triggering unnecessary emergency shutdowns, or failing to detect hidden failure states such as rope slippage, affecting the continuity of testing tasks and equipment safety.

[0005] In view of this, the present invention proposes a drive state monitoring and control method and system based on deformation compensation to solve the above problems. Summary of the Invention

[0006] To overcome the aforementioned deficiencies of the prior art, the present invention provides the following technical solution: a drive state monitoring and control method based on deformation compensation, comprising:

[0007] The robot is divided into driven and non-driven segments. A forward motion model of the driven segment is constructed based on the geometric parameters and kinematic relationships of the driven segment. The target end effector trajectory of the robot is collected, and the target end effector trajectory is combined with the forward motion model of the driven segment for reverse calculation to obtain the theoretical extension and retraction displacement.

[0008] The actual extension and retraction displacement of the robot's rope is collected. Based on the actual extension and retraction displacement and theoretical extension and retraction displacement, as well as the pre-calibrated rope elasticity parameters, the total load on the robot's drive end and the rate of change of the total load on the drive end are calculated.

[0009] The robot captures a sequence of images, extracts feature points from adjacent frames, and obtains the measured displacement velocity of the robot's end effector in the direction of motion by matching feature points and estimating inter-frame motion in adjacent frames.

[0010] The robot's drive motor current is collected, and the drive state is determined based on the drive motor current, measured displacement speed, total load at the drive end, and the rate of change of the total load at the drive end, so as to obtain the robot's state determination result.

[0011] A graded response is made based on the state discrimination results.

[0012] A drive state monitoring and control system based on deformation compensation, used to implement the drive state monitoring and control method based on deformation compensation, includes:

[0013] The theoretical analysis module is used to divide the robot into driven and non-driven segments, construct the forward motion model of the driven segment based on the geometric parameters and kinematic relationships of the driven segment, collect the target end-effector motion trajectory of the robot, and perform reverse calculation by combining the target end-effector motion trajectory with the forward motion model of the driven segment to obtain the theoretical extension and retraction displacement.

[0014] The load analysis module is used to collect the actual extension and retraction displacement of the robot's ropes, and calculate the total load on the robot's drive end and the rate of change of the total load on the drive end based on the actual extension and retraction displacement, theoretical extension and retraction displacement, and pre-calibrated rope elasticity parameters.

[0015] The velocity analysis module is used to acquire image sequences captured by the robot, extract feature points from adjacent frames, and obtain the measured displacement velocity of the robot's end effector in the direction of motion by performing feature point matching and inter-frame motion estimation on adjacent frames.

[0016] The state discrimination module is used to collect the drive motor current of the robot, and to perform drive state discrimination based on the drive motor current, measured displacement speed, total load of the drive end and the rate of change of the total load of the drive end, so as to obtain the state discrimination result of the robot.

[0017] The graded response module is used to perform graded responses based on the state determination results.

[0018] The technical effects and advantages of the drive state monitoring and control method and system based on deformation compensation of this invention are as follows:

[0019] By inversely solving the target end trajectory to obtain the theoretical release and take-off displacement, and subtracting it from the actual release and take-off displacement collected by the encoder to obtain the apparent deformation, the total load at the drive end can be estimated based on the rope elastic parameters. This transforms the imperceptible rope elastic deformation in existing technologies into a quantifiable load characterization, thereby enabling online identification of the total load at the drive end without adding additional force sensors, providing a direct mechanical basis for state determination.

[0020] By inputting three types of data—total load at the drive end and its rate of change, measured displacement velocity, and drive motor current—into the state discrimination logic, it can distinguish between five states: normal motion, non-drive segment jamming, drive segment jamming, rope slippage, and rope breakage. This avoids the misjudgment caused by existing technologies that rely solely on current thresholds. When the total load at the drive end exceeds the threshold but the current is not severely overloaded, it is determined to be non-drive segment jamming and reverse unlocking is performed. When the current is severely overloaded, it is determined to be drive segment jamming and emergency shutdown is performed, thus solving the problem of confusion in response strategies for different jamming types.

[0021] By obtaining the measured displacement velocity of the robot end effector in the direction of motion through visual odometry and cross-validating it with the total load change rate of the drive end, the stationary state can be confirmed when the visual velocity is continuously lower than the minimum measurable velocity and the load change rate is abnormal. This avoids false triggering caused by relying solely on load or current signals in working conditions such as turning corners, thereby improving the reliability of state judgment.

[0022] By executing graded responses based on state discrimination results, the control process can be transformed from static threshold triggering to closed-loop control that is dynamically corrected based on load and speed. When the non-drive section is self-locked, it can automatically release the jam and reduce speed to resume operation. When the drive section jams or the rope breaks, it can stop in time, thereby reducing the risk of equipment damage and improving the continuity and safety of the detection task.

[0023] In summary, this application organizes rope elastic deformation compensation, online estimation of total load at the drive end, visual velocity fusion discrimination and graded response into a closed processing chain, which improves the shortcomings of existing technologies in distinguishing jamming types and identifying hidden failures, thereby enhancing the accuracy of state monitoring and control reliability of rope-driven flexible robots in complex pipeline environments. Attached Figure Description

[0024] Figure 1 This is a schematic diagram of the drive state monitoring and control method based on deformation compensation according to the present invention.

[0025] Figure 2 This is a schematic diagram of the method for obtaining the theoretical expansion and contraction displacement according to the present invention;

[0026] Figure 3 This is a schematic flowchart of the method for obtaining the total load of the drive end and the rate of change of the total load of the drive end according to the present invention.

[0027] Figure 4 This is a schematic diagram of the drive state monitoring and control system based on deformation compensation according to the present invention. Detailed Implementation

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

[0029] Example 1:

[0030] Please see Figure 1 As shown, this embodiment provides a drive state monitoring and control method based on deformation compensation, including:

[0031] The robot is divided into a driven segment and a non-driven segment. A forward motion model of the driven segment is constructed based on the geometric parameters and kinematic relationships of the driven segment. The target end effector trajectory of the robot is collected, and the target end effector trajectory is combined with the forward motion model of the driven segment for reverse calculation to obtain the theoretical extension and retraction displacement. In this embodiment, the target end effector refers to the robot's front-end detection part used for state monitoring and trajectory control; the driven segment end effector refers to the connection end between the driven segment and the non-driven segment; the target end effector position is used to generate the target end effector trajectory, and the driven segment end effector position is used to establish the forward kinematic model of the driven segment.

[0032] In a specific implementation, refer to Figure 2 Methods for obtaining the theoretical expansion and contraction displacements include:

[0033] The robot is divided into driven and non-driven segments along its length. Specifically, the driven segment refers to the joint segment directly driven by the cable, and there is a clear kinematic mapping relationship between its input and output, that is, there is a clear kinematic mapping relationship between the cable displacement and the joint bending angle. The non-driven segment refers to the transition segment between the driven segment and the end effector, or the segment that is passively bent due to the geometric constraints of the pipeline, and has no independent driving source.

[0034] Taking a rope-driven continuum robot as an example, the drive segment consists of M segments with a length of... The system consists of series-connected joint units, each driven by three cables evenly distributed at 120° intervals; a base coordinate system B and a joint unit coordinate system are established. Both the end-effector coordinate system and the end-effector coordinate system E are right-handed rectangular coordinate systems, with the X-axis pointing in the robot's forward direction; the Z-axis is perpendicular to the bending plane, and the Y-axis is determined by the right-hand rule; the base coordinate system B is fixed to the center of the starting end face of the drive segment, with its origin denoted as . , The axis points to the end of the robot along its axis; joint element coordinate system Fixed at the first The center of the output end face of each joint unit =1,2,...,M, where M is the total number of joint units contained in the driving segment; origin Located at the center of the output end face of the k-th joint unit, The axis points to the end along the centerline of the joint unit; the end coordinate system E is fixed at the end of the driving segment, i.e., the starting end of the non-driving segment, with the origin at... It is located at the center of the end face of the drive section.

[0035] Each joint element has two rotational degrees of freedom: pitch and yaw. The pitch and yaw angles of the k-th joint element are defined as two bending angles, where the pitch angle is the angle at which the element bends in the XY plane when rotating around the Z-axis; the yaw angle is the angle at which the joint element bends in the XZ plane when rotating around the Y-axis. The bending motion of the joint element is controlled by the differential displacement of three ropes, which are located on a circle with a radius equal to the radius of the rope distribution circle, at angles of 0°, 120°, and 240°, respectively.

[0036] The total camber angle and camber direction angle are calculated based on the pitch and yaw angles corresponding to the joint elements; specifically, the total camber angle is the vector sum of the pitch and yaw angles, i.e., the total camber angle corresponding to the k-th joint element. ,in, For the first The pitch angle corresponding to each joint unit; For the first The yaw angle corresponding to each joint element; the arc length of the element centerline remains constant, denoted as . The tangent of the bending direction angle is the ratio of the yaw angle to the pitch angle, that is, the bending direction angle corresponding to the k-th joint element satisfies... ;when and hour, , Use the bending direction angle from the previous sampling time or take the initial calibration direction angle.

[0037] The theoretical release and retraction displacements of the three ropes are calculated based on the radius of the rope distribution circle, the total bending angle corresponding to the joint unit, and the bending direction angle. Specifically, positive values ​​represent rope tightening, and negative values ​​represent rope release. The formulas for calculating the theoretical release and retraction displacements of the three ropes are as follows: The theoretical release and retraction displacement of the first rope is the negative of the product of the cosine of the bending direction angle, the radius of the rope distribution circle, and the total bending angle; the theoretical release and retraction displacement of the second rope is the negative of the product of the cosine of the bending direction angle minus 120°, the radius of the rope distribution circle, and the total bending angle; the theoretical release and retraction displacement of the third rope is the negative of the product of the cosine of the bending direction angle minus 240°, the radius of the rope distribution circle, and the total bending angle; for example, the theoretical release and retraction displacement of the first rope... The second rope theory of release and recovery displacement The third rope theory of release and recovery displacement ;in, The radius of the rope distribution circle; The bending direction angle.

[0038] Based on the length of the joint unit, the first The pitch and yaw angles corresponding to the nth joint unit are calculated to obtain the nth joint unit. The transformation matrix of the joint unit, such as the first joint unit. The transformation matrix of each joint element is: ;in, Indicates translation along the X-axis , Indicates rotation about the Y-axis horn, Indicates rotation about the Z-axis Angle; the rotation sequence is first around the Y-axis and then around the Z-axis.

[0039] Calculate the product of the transformation matrices of M joint elements to obtain the homogeneous transformation matrix from the base coordinate system to the end coordinate system. Based on the homogeneous transformation matrix, obtain the theoretical position of the end of the drive segment in the base coordinate system. Obtain the X-axis component of the theoretical position as the theoretical displacement of the end of the drive segment. The above calculation process from rope displacement to end position is the forward kinematics model.

[0040] The method for acquiring the target end effector motion trajectory of a robot includes: obtaining the target position sequence of the target end effector in the base coordinate system; when describing general motion, the target end effector motion trajectory also includes the pitch angle target sequence and yaw angle target sequence of the target end effector relative to the base coordinate system; when there is only displacement in the X-axis direction and no pitch angle target change or yaw angle target change, it is determined to be pure axial motion, which is usually the target position sequence of the camera position in the motion direction. It can be obtained by manual command generation, preset path loading, or visual servo autonomous generation. All three methods output the target position sequence arranged according to the sampling time. The target position sequence includes the sampling time, target X-axis position, target pitch angle, and target yaw angle; specifically, in manual command generation, a speed command is issued by manual control, and the target position sequence is calculated based on the speed command and displacement formula; in preset path loading, the target position sequence is obtained at the current time by linear interpolation based on the pre-stored path point sequence; in visual servo autonomous generation, the target speed is set, and the target position sequence is calculated based on the target speed and displacement formula.

[0041] When there is a non-drive segment length between the target end and the drive segment end, the target end trajectory is first converted into the drive segment end target trajectory based on the known length of the non-drive segment, attitude constraints, or calibration extrinsic parameters. When the non-drive segment is approximately a rigid transition segment of fixed length, the target position at the drive segment end is determined. according to Calculate, where, The target's final location. The target end attitude matrix, This refers to the length of the non-drive segment; when the extrinsic parameter matrix of the target end coordinate system relative to the drive segment end coordinate system is obtained through the camera extrinsic parameters. At that time, the target pose at the end of the drive segment according to calculate; Let be the homogeneous transformation matrix of the target pose at the target end in the base coordinate system. Then, the target trajectory at the end of the driving segment is substituted into the forward kinematics model of the driving segment for inverse calculation.

[0042] The target trajectory at the end of the drive segment is substituted into the forward kinematics model of the drive segment for inverse calculation. When the target end coincides with the end of the drive segment, the target end trajectory is directly used as the target trajectory at the end of the drive segment. The total bending angle of each joint unit is iteratively solved using the Jacobi pseudo-inverse method or geometric analytical method. Then, the theoretical retraction and extension displacement formula of each rope is substituted into the formula to calculate the theoretical retraction and extension displacement of each rope. Specifically, the current motion type is determined based on the target end trajectory of the robot. If the target end only produces displacement along the X-axis of the base coordinate system, i.e., the direction of robot motion, without any change in pitch or yaw angle, it is determined to be pure axial motion. If the target end produces both axial displacement and pitch or yaw angle changes, it is determined to be general motion.

[0043] For pure axial motion: when the target's end-effector trajectory only includes the target position in the X-axis direction of the base coordinate system, and does not include pitch angle or yaw angle changes, the current motion type is determined to be pure axial motion. In this case, axial position constraints are used for reverse calculation, that is, the error between the theoretical position of the drive section end in the X-axis direction of the base coordinate system and the target end-effector position is used as the constraint quantity to solve for the theoretical release and retraction displacements of each rope that satisfy the corresponding axial position constraints.

[0044] Specifically, in the pure axial motion state, the bending direction angle of each joint unit is uniformly set to a preset bending direction angle. The same amplitude of test tightening is applied to the three ropes in sequence, and the deflection direction of the end of the drive section is recorded. The deflection direction with the smallest error from the pure axial retraction target is taken as the preset bending direction angle. In this embodiment, after the above calibration, the preset bending direction angle is taken as 0, and the total bending angle of each joint unit is solved according to the same value. The difference between the maximum extension length of the drive section in the fully extended state and the target end position in the target end motion trajectory is calculated to obtain the end retraction amount. Based on the circular arc geometry of the drive section, a constraint equation between the end retraction amount and the total bending angle is established, and the Newton iteration method is used to solve the total bending angle. During the iteration process, the theoretical X-axis position of the drive section end calculated from the current total bending angle is compared with the target end position. When the axial position error between the two is less than the preset convergence accuracy, the iteration is determined to be converged.

[0045] After iterative convergence, the theoretical release and take-up displacements of each rope are calculated using the theoretical release and take-up displacement formula based on the total bending angle and the preset bending direction angle. After the calculation, the obtained theoretical release and take-up displacements of the ropes are substituted into the forward motion model of the drive segment for consistency verification. If the error between the theoretical X-axis position of the end of the drive segment and the target end position obtained by the verification is within the convergence accuracy, the theoretical release and take-up displacements of each rope are output; if the error is greater than the convergence accuracy, the iterative solution is re-executed or the Jacobian pseudo-inverse solution process for general motion is switched to.

[0046] The target trajectory at the end of the drive segment is substituted into the forward kinematics model of the drive segment, and the theoretical expansion and contraction displacement is solved using a numerical iterative method based on the Jacobi pseudo-inverse. When the target end coincides with the end of the drive segment, the trajectory of the target end is directly used as the target trajectory at the end of the drive segment. The specific iterative process is as follows:

[0047] The initial value of the rope displacement vector is set to zero. Based on the current rope displacement vector, the forward kinematics model of the drive segment is called to calculate the current position of the end of the drive segment, and the position error vector between the current position of the end of the drive segment and the target position of the end of the drive segment is calculated.

[0048] The Jacobian matrix is ​​calculated from the rope displacement to the end position. The Jacobian matrix describes the influence of small changes in rope displacement on the end position. The numerical calculation method of the Jacobian matrix is ​​as follows: a preset small perturbation displacement is applied to each rope, and the change in end position caused by the small perturbation displacement is calculated through the forward kinematics model. The ratio of the change to the perturbation displacement is used as the corresponding column of the Jacobian matrix. The above operation is performed on the three ropes in sequence to obtain the complete Jacobian matrix. The method for setting the small perturbation displacement is based on the encoder resolution and numerical stability requirements.

[0049] The adjustment increment of the rope displacement is obtained by multiplying the position error vector with the pseudo-inverse of the Jacobian matrix, where the pseudo-inverse is calculated using the generalized inverse matrix in the least squares sense. The adjustment increment is then added to the current rope displacement vector to obtain the updated rope displacement vector.

[0050] Repeat the above process of calculating the position error vector, calculating the Jacobian matrix, calculating the adjustment increment, and updating the rope displacement until the magnitude of the position error vector is less than the preset convergence accuracy or the number of iterations reaches the preset upper limit. After iterative convergence, each component of the current rope displacement vector is the theoretical release and retraction displacement of each rope. The preset convergence accuracy or preset upper limit is set according to the real-time requirements. The convergence accuracy is not greater than the equivalent displacement after the encoder displacement resolution is converted to the end position. The upper limit of the number of iterations is determined based on the maximum number of model calls that can be executed within the control cycle.

[0051] The actual rope release and take-off displacement is collected. Based on the actual and theoretical release and take-off displacements, as well as the pre-calibrated rope elasticity parameters, the total load and the rate of change of the total load at the drive end are calculated. The actual rope release and take-off displacement can be obtained by converting the motor shaft encoder in combination with the rope wheel radius and transmission ratio. When used for rope breakage condition determination, it is preferable to directly collect the data using an independent displacement encoder on the rope wheel side to form a transmission consistency verification with the motor rotation angle collected by the motor shaft encoder. When no independent displacement encoder on the rope wheel side is set, the rope breakage condition based on the incremental ratio of the two is not executed.

[0052] In a specific implementation, refer to Figure 3 Methods for obtaining the total load on the driver and the rate of change of the total load on the driver include:

[0053] The actual take-up and release displacement of the robot's ropes is obtained by high-resolution encoders installed at the rear end of each drive motor of the robot. A high-resolution encoder is an encoder whose displacement resolution meets the minimum identifiable requirement of the rope's differential elastic deformation. Each rope corresponds to an actual take-up and release displacement value, and the actual take-up and release displacements of all ropes constitute the actual take-up and release displacement vector. The encoder resolution is set as follows: the encoder's displacement resolution is not greater than the preset upper limit of displacement resolution, which is determined based on the minimum identifiable requirement of the rope's differential elastic deformation. If a higher resolution encoder is used, an appropriate digital low-pass filter should be used to preprocess the original signal to suppress the interference of high-frequency noise from the sensor on the calculation of elastic deformation. The encoder sampling frequency is set as follows: it is not lower than the encoder's minimum sampling frequency to meet the requirements for rapid detection of jamming events.

[0054] The difference between the actual release and retraction displacement of each rope and the corresponding theoretical release and retraction displacement is calculated to obtain the apparent deformation of each rope. The apparent deformation of all ropes constitutes an apparent deformation vector. The sources of apparent deformation include the elastic deformation of the rope under axial tension, the displacement deviation caused by the sliding between the rope and the guide mechanism, and the hysteresis displacement caused by the friction between the rope and the pipe wall or guide wheel. Among them, the apparent deformation of each rope is the component of the apparent deformation vector. In the actual process, assuming that the rope is under normal tension and the friction coefficient of the guide mechanism is low, the displacement difference caused by the sliding between the rope and the guide mechanism and the frictional resistance between the rope and the pipe wall and guide wheel can be ignored. At this time, the apparent deformation is approximately equal to the elastic deformation of the rope under axial tension.

[0055] The equivalent tension of each rope is calculated based on the pre-calibrated rope elastic parameters, the apparent deformation of each rope, and the initial preload deformation of each rope. The total load at the drive end is then obtained based on the equivalent tension of each rope. The rope elastic parameters include the rope elastic modulus, rope cross-sectional area, and rope equivalent effective length. Specifically, for the first... Calculate the elastic modulus of a rope. With rope cross-sectional area The product of the products divided by the equivalent effective length of the rope , obtained the The equivalent stiffness coefficient of the rope ,in ; Calculate the first Actual release and retraction displacement of the rope Corresponding theoretical expansion and contraction displacement The difference is used to obtain the first... apparent deformation of the root rope ,in ; Read the first Initial preload deformation of a rope under calibrated tension Calculate the first Effective elastic deformation of the rope ,in ;when When it is less than 0, Set to 0; when the first If the first rope does not meet the rope slippage condition described later and does not meet the rope breakage condition described later, calculate the first rope. The equivalent tension of a rope ,in When the first When a rope meets the candidate conditions for rope slippage or rope breakage, the apparent deformation of the corresponding rope is not used as the actual elastic elongation for tension calculation. Instead, the corresponding rope is marked as an abnormal rope, and the corresponding apparent deformation is used as the rope state discrimination feature. The equivalent tension of each rope not marked as an abnormal rope is summed to obtain the total load at the drive end. When all ropes are marked as abnormal ropes, the output of the calculated value of the total load at the drive end is stopped, and the process of judging the abnormal rope state is initiated. The total load at the drive end is used to characterize the equivalent resistance borne by the output end of the drive motor. Its sources include the axial resistance of the non-drive section and the internal resistance of the drive section. The axial resistance of the non-drive section includes the frictional resistance between the non-drive section and the pipe wall, the axial resistance caused by the normal constraint at the bend, and the kink self-locking resistance. The internal resistance of the drive section includes the joint frictional resistance of the drive section, the frictional resistance between the rope and the guide wheel, and the resistance generated by the rope bending in the drive section.

[0056] The calibration method for the elastic modulus of a rope is as follows: a tensile testing machine is used to perform a cyclic loading-unloading test on the rope under test. According to the relevant standard for determining the actual elastic modulus of wire rope, the ratio of stress to strain is measured during the elastic deformation stage, and the average value of three measurements is taken as the elastic modulus of the rope.

[0057] The method for measuring the cross-sectional area of ​​a rope is as follows: measure the rope diameter using a micrometer screw gauge, and calculate the rope cross-sectional area based on the rope diameter and the area formula.

[0058] The method for calibrating the equivalent effective length of a rope is as follows: apply a known tension to the driving end of the rope, measure the elastic theoretical expansion and contraction displacement of the driving end, calculate the product of the rope's elastic modulus, the rope's cross-sectional area, and the elastic theoretical expansion and contraction displacement, and then divide it by the known tension to obtain the effective length. Take the average of the three measured effective lengths as the equivalent effective length of the rope.

[0059] The ratio of the difference between two consecutive measurements of the total load at the drive end to the time interval between the two consecutive measurements is used to obtain the rate of change of the total load at the drive end.

[0060] The robot acquires image sequences and extracts feature points from adjacent frames. By matching feature points and estimating inter-frame motion in adjacent frames, the robot's end effector obtains the measured displacement velocity in the direction of motion.

[0061] In specific implementations, methods for obtaining measured displacement velocity include:

[0062] The robot acquires image sequences of the driving environment, namely the image sequence of the inner wall of the pipe, using a forward-facing high-definition camera mounted on its end effector. Gaussian filtering and histogram equalization are then applied to the images in the sequence to enhance the texture features of the inner wall of the pipe, resulting in a pre-processed image.

[0063] A fast corner detection algorithm is used to extract feature points from the preprocessed image, and an image pyramid is constructed based on a preset number of layers and a preset scale factor. The preset number of layers is determined by the ratio of the minimum identifiable texture size to the maximum identifiable texture size. The preset scale factor is set as the scaling ratio of the side lengths of neighboring layers. The value of the preset scale factor ensures the continuity of scale space sampling and avoids missed feature points. Feature points are extracted for each layer of the preprocessed image, and an oriented fast rotation descriptor is calculated. The oriented fast rotation descriptor is a binary vector of a preset number of bits, generated by comparing the gray values ​​of random pixel pairs in the neighborhood of the feature point. The preset bit depth is set based on the feature point discrimination and computational resource limitations. The descriptor generation method is as follows: a preset neighborhood window is selected centered on the feature point, and pixels of a preset bit depth are selected within the preset neighborhood window according to a preset random sampling mode. The grayscale comparison result of each pair of pixels constitutes one bit of the descriptor. The descriptor is used for subsequent similarity measurement between feature points. The preset neighborhood window is set based on the expression requirements of the local texture of the feature point. The preset random sampling mode is set based on the standard sampling mode of the directional fast rotation descriptor algorithm, using the preset bit depth pixel coordinates generated by a fixed random seed.

[0064] For preprocessed images of adjacent frames, a brute-force matching method using Hamming distance is used for initial matching. Specifically, for each feature point in the k-th frame of the image sequence, the Hamming distance between the corresponding Oriented Fast Rotation descriptor and the Oriented Fast Rotation descriptors corresponding to all feature points in the (k+1)-th frame is calculated. The Hamming distance is defined as the number of bits that differ in value at the same position between two binary strings. For each feature point in the current frame, the two matching points with the smallest and second smallest Hamming distances are selected from the candidate matching points of the feature point in the current frame. The ratio of the smallest Hamming distance to the second smallest Hamming distance is calculated and denoted as the Hamming ratio. If the Hamming ratio is less than a preset ratio threshold, the matching pair corresponding to the smallest Hamming distance is accepted; otherwise, all candidate matching points of the current feature point are discarded. The preset ratio threshold is set by means of normal straight pipe segment samples: continuous image frames are collected in a normal straight pipe segment, and the maximum Hamming ratio of the correctly matched point pair and the minimum Hamming ratio of the incorrectly matched point pair are calculated. When there is an interval between the two, the median value is taken as the ratio threshold. When there is no interval between the two, it is determined that the current texture condition is not suitable for using the corresponding feature matching result.

[0065] Before performing homography matrix estimation, a matching point adjacency graph is established based on the position of the preliminary matching points in the previous frame of the image. The matching point adjacency graph is established by taking the feature points of the previous frame in each pair of matching points filtered by Hamming ratio as nodes, performing triangulation on all nodes, and determining the two nodes with a common edge in the triangulation as adjacent matching points. The corresponding adjacency relationship is determined by the coordinate distribution of the matching points in the image plane.

[0066] For an adjacent edge in the matching point adjacency graph, the two triangles located on both sides of the corresponding adjacent edge and sharing the same edge in the triangulation are respectively regarded as local regions on both sides; the local matching point sets on both sides are respectively composed of the vertices of the corresponding triangle and the vertices of the adjacent triangles sharing the same edge with the corresponding triangle; if the corresponding adjacent edge is located on the outer boundary of the triangulation and there are no local regions on both sides, then the corresponding adjacent edge is not used for the judgment of the bending abrupt boundary, but only for the limitation of the connected region boundary.

[0067] For each adjacent edge in the matching point adjacency graph, the set of local matching points on both sides of the corresponding adjacent edge is obtained, and two types of reprojection residuals are calculated respectively: the first type is the joint reprojection residual obtained by merging the sets of local matching points on both sides of the corresponding adjacent edge and fitting them with the same candidate homography matrix; the second type is the separate reprojection residual obtained by fitting the sets of local matching points on both sides of the corresponding adjacent edge with their respective candidate homography matrices. If the separate reprojection residuals on both sides of the adjacent edge are not greater than the preset error threshold, but the joint reprojection residual is greater than the preset error threshold, then it is determined that the matching points on both sides of the corresponding adjacent edge do not satisfy the same local approximate planar motion constraint, and the corresponding adjacent edge is determined as the pipe bending abrupt boundary. The preset error threshold is set according to the statistical results of camera calibration error, image resolution, and normal straight pipe section image matching error.

[0068] For each adjacent edge in the matching point adjacency graph, a strong occlusion boundary is determined based on the continuity of feature point matching in the local regions on both sides of the corresponding adjacent edge. Specifically, if there are valid matching point sets on both sides of the corresponding adjacent edge with a number not less than the minimum number threshold, but the matching success rate in the region crossed by the corresponding adjacent edge is lower than the matching success rate threshold, or if a valid matching point set with a number not less than the minimum number threshold cannot be formed after crossing the corresponding adjacent edge, then the corresponding adjacent edge is determined as a strong occlusion boundary. The matching success rate is the ratio of the number of successfully matched point pairs to the total number of feature points in the corresponding local region of the previous frame. The minimum number threshold is set by calculating the minimum number of matching point pairs required based on the homography matrix. The matching success rate threshold is set by setting the typical matching success rate under normal pipe inner wall texture conditions.

[0069] The adjacent edges identified as pipe bend abrupt boundary or strong occlusion boundary are deleted from the matching point adjacency graph, and the connected components formed by the remaining adjacent edges are determined as candidate motion estimation regions. The corresponding pipe bend abrupt boundary line or strong occlusion boundary line is formed by sequentially connecting the midpoints of the deleted adjacent edges. The candidate homography matrix is ​​estimated for the matching point pairs in each candidate motion estimation region, and the corresponding reprojection residual is calculated. If the matching points in the candidate motion estimation region are continuously distributed, the number of matching points is not less than the minimum number threshold, and the reprojection residual corresponding to the candidate homography matrix is ​​not greater than the preset error threshold, then the corresponding candidate motion estimation region is determined as a motion estimation region that satisfies the local approximate plane condition. If the candidate motion estimation region crosses the pipe bend abrupt boundary or strong occlusion boundary, the number of matching points is less than the minimum number threshold, or the reprojection residual corresponding to the candidate homography matrix is ​​greater than the preset error threshold, then the corresponding candidate motion estimation region is removed and not used for subsequent homography matrix decomposition.

[0070] When there is no motion estimation region that satisfies the local approximate plane condition, or when the number of matching points that satisfy the local approximate plane condition is less than the minimum number threshold, the homography model is deemed inapplicable and the visual odometry unreliable determination process is initiated. When there is a motion estimation region that satisfies the local approximate plane condition, the random sampling consistency algorithm and homography matrix estimation are performed based on the matching point pairs within the corresponding motion estimation region, and the corresponding region does not cross the pipe bending abrupt boundary or strong occlusion boundary.

[0071] After determining the motion estimation region that satisfies the local approximate plane condition, the random sampling consensus algorithm is used to filter the matching point pairs in the corresponding motion estimation region to obtain a reliable set of matching point pairs, and at the same time obtain the homography matrix between adjacent frames. The method of the random sampling consensus algorithm to eliminate mismatches is as follows: randomly select at least 4 sets of matching point pairs, calculate the candidate homography matrix U. The method of calculating the candidate homography matrix is ​​an existing technology and will not be described in detail here. Its basic steps include solving the linear equation system by the direct linear transformation method to obtain the initial estimate of the candidate homography matrix U, and then performing nonlinear optimization by the Levenberg-Marquardt algorithm to minimize the reprojection error and obtain the candidate homography matrix U.

[0072] The number of interior points that satisfy the candidate homography matrix U is counted. An interior point is a matching point pair whose projection error of the candidate homography matrix U is less than a preset error threshold. This process is repeated a preset number of times. The candidate homography matrix U with the largest number of interior points is selected as the homography matrix V. Matching point pairs that do not satisfy the homography matrix V are removed as exterior points. The remaining interior points form the set of matching point pairs. The preset number of iterations is determined based on the current number of matching point pairs and the minimum number of matching point pairs required for each homography matrix estimation, and is limited by the maximum number of model calls that can be executed within the control period. The iteration ends when the number of interior points does not increase after two consecutive iterations and random coverage of all matching point pairs has been completed.

[0073] During continuous tracking, if the number of reliable matching point pairs between the current frame and the previous frame is lower than the preset minimum number threshold, visual tracking is determined to be lost. If the number of effective feature points in the current frame is not lower than the feature point number threshold, feature point extraction is re-executed in the current frame and re-matched with the previous frame. If the number of effective feature points in the current frame is lower than the feature point number threshold, the system waits for the next frame image and then re-initializes. The minimum number threshold is set by calculating the minimum number of matching point pairs required based on the homography matrix.

[0074] The homography matrix V is decomposed into a rotation matrix using singular value decomposition. Translation vector .

[0075] Since the camera is fixed to the robot's end effector, the extrinsic parameter matrix between the camera coordinate system and the robot's end effector coordinate system is obtained through pre-calibration. The first rotation matrix, used for the transformation of displacement direction between adjacent frames, is included in the calculation. Because the displacement between adjacent frames is a difference component, the first translation vector cancels out in the difference calculation and does not participate in the transformation of displacement direction between adjacent frames. Through coordinate system transformation, the translation vector obtained from the camera motion decomposition is transformed into the translation vector of the robot's end effector in the base coordinate system. The specific transformation formula is: the robot's end effector translation vector equals the first rotation matrix multiplied by the camera translation vector. The X-axis component of the robot's end effector translation vector is taken as the displacement between adjacent frames. The product of the displacement between adjacent frames and the scale factor is calculated to obtain the actual physical displacement. The scale factor is set as follows: when the robot passes through a straight pipe section of known length, the ratio of the known length to the cumulative displacement estimated by the visual odometry is calculated. When there is no straight pipe section of known length on site, the scale factor is calculated through the camera intrinsic parameters, the pipe inner diameter calibration value, and the pixel scale relationship of the pipe boundary in the image. When neither of the above two methods is available, the visually measured displacement velocity is not output, and a backup velocity estimation method is enabled.

[0076] Calculate the ratio of the actual physical displacement to the time interval between adjacent frames to obtain the measured displacement velocity.

[0077] To ensure the accuracy of subsequent state determination, the reliability of the measured displacement velocity output by the visual odometry needs to be assessed. If any of the following conditions are met, the measured displacement velocity is deemed unreliable, and a backup velocity estimation method is activated. These conditions include:

[0078] Condition 1: When the number of valid feature points extracted within the consecutive image frame number threshold is less than the feature point number threshold; the method for setting the consecutive image frame number threshold is: based on the visual odometry's tolerance to brief occlusion; the method for setting the feature point number threshold is: based on the minimum number of matching point pairs required to solve the homography matrix.

[0079] Condition 2: The feature point matching success rate between adjacent frames, that is, the ratio of the number of successfully matched point pairs to the total number of feature points in the previous frame, is lower than the matching success rate threshold; the matching success rate threshold is set according to the typical matching success rate under normal pipe inner wall texture conditions.

[0080] Condition 3: The measured displacement velocity exceeds the maximum allowable velocity threshold, or the absolute value of the rate of change of the measured displacement velocity between adjacent frames exceeds the maximum allowable acceleration threshold. The rate of change of the measured displacement velocity between adjacent frames is obtained by calculating the ratio of the difference in the measured displacement velocity between adjacent frames to the time interval between adjacent frames. The difference in the measured displacement velocity between adjacent frames is the difference between the measured displacement velocity of the current frame and the measured displacement velocity of the previous frame. The maximum allowable velocity threshold is set according to the maximum design speed of the robot drive system. The maximum allowable acceleration threshold is set as follows: when the comparison object is the rate of change of the measured displacement velocity between adjacent frames, the maximum allowable acceleration threshold is taken as the maximum design acceleration of the robot drive system; when the comparison object is the velocity difference between adjacent frames, the maximum allowable velocity difference threshold is taken as the product of the maximum design acceleration and the image acquisition interval.

[0081] Condition 4: The rotation matrix obtained by decomposing the homography matrix V does not satisfy orthogonality, that is, the Frobenius norm between the decomposition matrix and the identity matrix is ​​greater than the preset orthogonality error threshold. The result of the homography matrix decomposition is determined to be unreliable, and the decomposition matrix is ​​an orthogonal matrix formed by the product of the transpose of the rotation matrix and the rotation matrix. The orthogonality error threshold is set according to the rounding error of the numerical calculation.

[0082] Downgrade strategy:

[0083] When the visual odometry is determined to be unreliable, the backup plan should be activated according to the following priority:

[0084] First priority: Triaxial acceleration is acquired by the inertial measurement unit installed at the end, the gravity component is removed, and the result is integrated once to obtain the backup velocity estimate. The integration noise is suppressed by a sliding window averaging filter. The initial value of the integration is the measured displacement velocity output by the most recent reliable visual odometry. The backup velocity estimate is used to replace the measured displacement velocity output by the visual odometry in the state determination.

[0085] Second priority: If IMU data is unavailable, select a reference segment of known length from the pipe sections that have been traversed, such as a straight pipe section of standard length, and estimate the average velocity at the end by time integration. Specifically, calculate the average velocity by the ratio of the reference segment length to the passage time.

[0086] Third priority: If none of the above backup schemes are available, the priority is downgraded to relying solely on the total load of the drive end and the current of the drive motor for state determination. In this case, the judgment condition involving the measured displacement speed in subsequent steps is modified to: the total load of the drive end continues to rise beyond the stagnation time threshold and the actual retraction and extension displacement is still increasing. That is, the end is indirectly determined to be stagnant by the mode of continuous increase in resistance but the motor is still driving.

[0087] The drive motor current is collected, and the drive state is determined based on the drive motor current, measured displacement speed, total load at the drive end, and the rate of change of the total load at the drive end, so as to obtain the state determination result.

[0088] In specific implementations, the methods for obtaining state discrimination results include:

[0089] The system detects whether a person has entered a bend section based on preset bend entry detection conditions.

[0090] The preset elbow inlet detection conditions include:

[0091] Condition A: The total load on the drive end increases from less than the normal resistance threshold to more than S times the jamming resistance threshold within the first consecutive time threshold.

[0092] Condition B: The measured displacement velocity decreases by more than a first percentage within a continuous first time threshold.

[0093] Condition C: The measured displacement velocity is greater than the minimum measurable velocity.

[0094] Among them, the first time threshold is the ratio of the characteristic length of the pipe section to the normal movement speed; the normal resistance threshold is the maximum value of the steady-state value of the total load at the drive end in the normal uniform speed operation sample of the straight pipe section; the resistance change rate threshold is the maximum value of the absolute value of the change rate of the total load at the drive end in the normal uniform speed operation sample of the straight pipe section; the jamming resistance threshold is the minimum value of the peak value of the total load at the drive end when self-locking occurs in the axial self-locking calibration sample of the non-drive section; when the first difference of the apparent deformation in the next sampling interval changes from positive to non-positive, it is determined as the inflection point of the apparent deformation; the setting method of S is: based on the sensitivity setting of the elbow inlet judgment, the value range is a positive number less than 1.

[0095] When all preset bend entry detection conditions are met, entry into the bend segment is determined. The time corresponding to the fulfillment of all preset bend entry detection conditions is recorded as the bend entry time, and adaptive threshold adjustment of the bend segment is initiated. This involves adaptively adjusting the preset jamming resistance threshold and resistance change rate threshold to obtain the adjusted jamming resistance threshold and adjusted resistance change rate threshold. Specifically, the product of the jamming resistance threshold and the bend amplification factor is calculated to obtain the adjusted jamming resistance threshold; the product of the resistance change rate threshold and the bend change rate amplification factor is calculated to obtain the adjusted resistance change rate threshold. The bend amplification factor is set as follows: the maximum value of the ratio of the peak value of the total drive-end load to the jamming resistance threshold in the normal bend sample is taken; the bend change rate amplification factor is set as follows: the maximum value of the ratio of the peak value of the total drive-end load change rate to the resistance change rate threshold is taken; the calculation results are rounded according to the acquisition resolution of the total drive-end load and the total drive-end load change rate.

[0096] When all preset elbow exit detection conditions are met, the elbow section is exited. The time corresponding to the satisfaction of all preset elbow exit detection conditions is recorded as the elbow exit time. The preset elbow exit detection conditions include:

[0097] The total load on the drive end decreases by more than the second percentage within the first continuous time threshold; the measured displacement speed recovers to more than Kv times the normal movement speed, where the normal movement speed is the average measured displacement speed within the first time threshold before entering the bend entrance; the second percentage is set based on experimental data of the axial resistance drop characteristics after passing the bend; Kv is a positive number less than 1, and its setting method is determined based on the proportion range of the speed recovering to the stable operating speed after normal bend passage.

[0098] Within the bend section, from the moment of bend inlet to the moment of bend outlet determination, in addition to using the adjustment of the jamming resistance threshold and the adjustment of the resistance change rate threshold, a double confirmation is also required based on the measured displacement velocity: if the total load on the drive end exceeds the adjustment of the jamming resistance threshold, and the measured displacement velocity is continuously less than the minimum measurable velocity exceeding the stagnation time threshold, then it is determined to be a jamming candidate state; when the drive motor current is not greater than the first upper current threshold, the jamming candidate state is determined to be a non-drive section jamming state; when the drive motor current is greater than the second lower current threshold, the jamming candidate state is determined to be a drive section jamming state. Status; if the total load on the drive end exceeds the adjustment threshold for jamming resistance, but the measured displacement speed remains greater than the minimum measurable speed, it is determined to be normal cornering and no jamming alarm is triggered; the method for setting the stagnation time threshold is: taking the ratio of the drive segment length to the normal movement speed; the method for setting the normal speed threshold is: setting it according to the robot's normal working speed range, taking the lower limit or average value of the normal working speed as a preset ratio, the method for setting the preset ratio is: setting it according to the safety margin requirements of the speed threshold; the method for setting the preset upper limit value is: setting it according to the system's tolerance for long-term stagnation.

[0099] Within the bend section, when comparing the total load change rate of the drive end, the adjustment resistance change rate threshold is used instead of the resistance change rate threshold. When the total load change rate of the drive end continuously exceeds the adjustment resistance change rate threshold within the second time threshold, and the measured displacement speed is continuously less than the minimum measurable speed exceeding the stagnation confirmation time threshold, the corresponding comparison result is used as an auxiliary criterion for distinguishing between the non-drive section jamming state and the drive section jamming state. The stagnation confirmation time threshold is less than or equal to the stagnation time threshold, and its setting method is: take the time required to complete the speed calculation of two adjacent frames in the image sequence.

[0100] When the total load on the drive end is not greater than the normal resistance threshold and the measured displacement speed is not lower than the minimum measurable speed, it is judged to be in a normal state.

[0101] The current jamming threshold is determined based on whether the robot is in a bend. When the robot is not in a bend, the jamming resistance threshold is used as the current jamming threshold. When the robot is in a bend, the jamming resistance threshold is adjusted and used as the current jamming threshold. When the total load on the drive end exceeds the current jamming threshold, the duration of the measured displacement speed being continuously less than the minimum measurable speed exceeds the stagnation time threshold, and the drive motor current is not greater than the first current upper limit threshold, it is determined to be a non-drive segment jamming state. When the rate of change of the total load on the drive end continuously exceeds the resistance change rate threshold, it is only used as an enhanced confirmation condition for the jamming candidate state. The final distinction between the non-drive segment jamming state and the drive segment jamming state is still based on whether the drive motor current exceeds the corresponding current threshold. The method for setting the second time threshold is: based on the signal sampling frequency and the mechanical system... The dynamic response time of the system is set to cover a preset number of sampling points to ensure statistical stability. The no-load current is set as follows: when the robot is not in the pipe and is in free space, each motor is driven to rotate forward and backward a preset number of times at normal operating speed. The driving current value during each stable operation is recorded, and the arithmetic mean of all measured values ​​is taken as the no-load current. The first current upper limit threshold is set as follows: when the robot is not in the pipe and is in free space, each motor is driven to rotate forward and backward a preset number of times at normal operating speed. The driving current value during stable operation is recorded, and the arithmetic mean is taken as the no-load current. Then, based on the upper limit of the fluctuation of the driving motor current relative to the no-load current in the axial self-locking experiment of the non-drive section, the first current upper limit threshold is determined. The first current upper limit threshold is used to distinguish the non-drive section jamming situation where the driving motor is not close to stall but the total load at the drive end increases.

[0102] The current jamming threshold is determined based on whether the robot is in a bend. When the robot is not currently in a bend, the jamming resistance threshold is used as the current jamming threshold. When the robot is currently in a bend, the jamming resistance threshold is adjusted and used as the current jamming threshold. When the total load on the drive end is greater than the current jamming threshold, the duration of the measured displacement speed being less than the minimum measurable speed exceeds the stagnation time threshold, and the drive motor current is greater than the second current lower limit threshold, the drive segment is judged to be in a jamming state. The second current lower limit threshold is set based on the drive motor stall characteristics and drive segment mechanical jamming experimental data. When the drive motor current is greater than the second current lower limit threshold, the drive motor is judged to be in a near-stall load state, which is used to distinguish the drive segment jamming state.

[0103] The first upper current threshold is less than or equal to the second lower current threshold. When the drive motor current is greater than the first upper current threshold but not greater than the second lower current threshold, the state judgment result is determined as a stuck state awaiting review, and a second judgment is performed based on the total load change rate and duration of the drive end. In the second judgment, within the second time threshold, if the total load change rate of the drive end continuously exceeds the current resistance change rate threshold, and the maximum value of the drive motor current rises to be greater than the second lower current threshold, the stuck state awaiting review is updated to the drive segment stuck state. If the total load change rate of the drive end continuously exceeds the current resistance change rate threshold, and the drive motor current is never greater than the second lower current threshold within the second time threshold, the stuck state awaiting review is updated to the non-drive segment stuck state. If the total load change rate of the drive end does not continuously exceed the current resistance change rate threshold, the stuck state awaiting review is maintained and a first-level response is executed. Among these, when in a non-bend section, the current resistance change rate threshold is the resistance change rate threshold, and when in a bend section, the current resistance change rate threshold is the adjustment resistance change rate threshold.

[0104] When all preset rope slippage conditions are met, the rope is determined to be in a slippage state. The preset rope slippage conditions include: the absolute value of the total load change rate at the drive end is less than D1 times the resistance change rate threshold within the third time threshold, or the total load change rate at the drive end decreases; the measured displacement velocity is continuously less than the minimum measurable velocity exceeding the stagnation time threshold; the drive motor current is less than or equal to the first current upper limit threshold; and the actual retraction and extension displacement is still increasing, but the minimum apparent deformation is continuously lower than the lower limit threshold of apparent deformation, where the minimum apparent deformation is the minimum value of all rope apparent deformations; among which, the third... The time threshold is the stagnation time threshold; D1 is set by taking the ratio of the maximum absolute value of the total load change rate of the drive end in the slippage sample to the resistance change rate threshold; the apparent deformation lower limit threshold is set by continuously collecting the steady-state value of the apparent deformation when the robot is unloaded and moving at a constant speed in a straight pipe section, calculating its arithmetic mean as the normal apparent deformation benchmark value, and taking P times the normal apparent deformation benchmark value as the apparent deformation lower limit threshold. P is set by taking the ratio of the minimum apparent deformation maximum value in the slippage sample to the normal apparent deformation benchmark value.

[0105] A rope is considered broken when all preset rope breakage conditions are met. These preset conditions include: the total load change rate at the drive end decreases by more than D2 times the resistance change rate threshold within the second time threshold; the measured displacement speed remains below the minimum measurable speed for more than the dwell time threshold; the drive motor current is less than the no-load current; and the ratio of the actual increase in retraction / release displacement to the increase in motor angle exceeds the normal threshold. The actual retraction / release displacement is collected by an independent displacement encoder on the rope wheel side, and the motor angle is collected by a motor shaft encoder. The normal threshold is set by taking the maximum value of the ratio of the actual increase in retraction / release displacement to the increase in motor angle in the normal transmission calibration sample. D2 is set by statistically analyzing the boundary between normal operation samples and samples showing rope breakage, with a value range of positive numbers less than 1.

[0106] A graded response is made based on the state discrimination results.

[0107] In a specific implementation, the method for hierarchical response based on state discrimination results includes:

[0108] When the total load on the drive end exceeds the jamming resistance threshold but the duration is less than the warning time threshold, a first-level response is executed. The first-level response includes issuing a warning and reducing the forward drive speed. When the state judgment result is that the rope is slipping, an automatic tensioning response is executed. The warning time threshold is set by taking half of the stagnation time threshold.

[0109] When the duration of the non-drive segment stuck state exceeds the stagnation time threshold, a secondary response is initiated, that is, the preset self-unlocking control strategy is automatically executed. The self-unlocking control strategy includes:

[0110] Control 1: Stop the robot's forward drive.

[0111] Control 2: Execute the reverse drive of the robot. The step size of the reverse drive is less than the upper limit of the reverse adjustment step size. The method for setting the upper limit of the reverse adjustment step size is as follows: gradually increase the reverse step size in a straight pipe section without jamming, record the minimum step size when the robot end begins to generate a measurable reverse displacement, and record the maximum reverse step size that will not cause the target detection position to be lost. Select the upper limit of the reverse adjustment step size between the two.

[0112] Control 3: Monitor the total load change at the drive end. If the total load at the drive end decreases with the reverse drive, it is determined that the self-unlocking has been completed.

[0113] Control 4: If the total load on the drive end after reverse drive is less than or equal to the normal resistance threshold, resume forward drive and reduce the speed by a preset speed ratio. The preset speed ratio is set based on the minimum speed margin that can still maintain effective forward movement after deceleration.

[0114] Control 5: If the total load at the drive end is still greater than the jamming resistance threshold after reverse drive, oscillation drive is executed, that is, forward drive and reverse drive are alternated. The single step size is the oscillation step size, and the alternation frequency is the oscillation frequency. The oscillation step size is set according to the minimum displacement resolution of the drive segment joint unit and the requirement to avoid excessive impact. The oscillation frequency is set by performing low-amplitude reciprocating drive tests under the prototype's no-load and pipeline load conditions, recording the curve of the total load fluctuation amplitude at the drive end changing with the drive frequency, avoiding the frequency range where the load fluctuation amplitude peaks, and selecting the frequency that can reduce the total load at the drive end within the remaining frequency range as the oscillation frequency.

[0115] When the drive section is stuck or the rope is broken, a preset three-level response is executed. When the rope is slipping, an automatic tensioning response is executed first. The automatic tensioning response includes stopping the forward drive, controlling the rope drive end to perform tensioning action according to the tensioning step, and re-executing the rope slippage judgment. Specifically, the target tensioning rope is determined. The target tensioning rope is a rope that meets the rope slippage condition and whose apparent deformation is lower than the lower limit threshold of apparent deformation. When there are multiple ropes that meet the conditions, the tensioning step is calculated for each target tensioning rope.

[0116] Based on the difference between the actual release and take-up displacement of each target tension rope and the corresponding theoretical release and take-up displacement, the current apparent deformation of the corresponding rope is obtained; the positive difference between the lower limit threshold of the apparent deformation and the current apparent deformation is calculated as the apparent deformation to be compensated for the corresponding rope; the apparent deformation to be compensated is rounded up according to the minimum displacement resolution executable by the corresponding rope drive end to obtain the tensioning step of the corresponding rope; if the apparent deformation to be compensated is not greater than zero, no tensioning action is performed on the corresponding rope; the minimum displacement resolution executable by the rope drive end is obtained by conversion based on the rope wheel radius, the number of pulses per revolution of the motor encoder, and the drive transmission ratio; when an independent displacement encoder is set on the rope wheel side, the minimum displacement resolution executable by the rope drive end is directly taken as the displacement resolution of the independent displacement encoder on the rope wheel side.

[0117] After the control rope drive end tightens the target tensioned rope according to the tension step, the actual rope release and release displacement, drive motor current, and measured displacement speed are collected again, and the rope slippage judgment is re-executed. If the re-judgment result no longer meets the rope slippage condition, the tensioning action is stopped and the forward drive is resumed. If the re-judgment result still meets the rope slippage condition, the tension step is recalculated again according to the above method within the allowable number of tensioning attempts, and the tensioning action is executed again. If the rope slippage state is still determined after reaching the tensioning attempt number, the emergency stop is upgraded and a three-level response is executed. The three-level response includes: when the drive section is determined to be stuck, all drive motors of the robot are stopped, a three-level alarm is issued, and a fault log is recorded.

[0118] When a rope breakage is detected, all drive motors of the robot are stopped, and the suspected breakage section is marked in the fault log. The method for determining the suspected breakage section is as follows: identify the target rope that meets the rope breakage conditions, and determine the breakage determination time when the actual release and take-up displacement of the target rope changes abruptly or the apparent deformation drops abruptly. Read the cumulative release and take-up length of the target rope corresponding to the breakage determination time, and combine it with the calibrated arrangement length of the target rope in the drive path to mark the breakage risk location of the target rope as a relative length range, rather than a unique breakage point. When a rope slippage is detected, a level one warning response is issued, that is, a warning is issued and an automatic tensioning response is executed, with a maximum of a preset number of tensioning attempts. If the rope slippage is still detected after the attempts, the system is upgraded to an emergency stop and a level three response is executed. The method for setting the number of tensioning attempts is: take the number of target tensioning ropes. When the rope slippage condition is still met after each target tensioning rope has been tensioned once, the automatic tensioning is determined to have failed.

[0119] The fault log records all sensor data within the fault log recording duration before and after the current moment; the method for setting the fault log recording duration is based on the analysis needs of the transient process before and after the fault and the engineering balance of storage capacity.

[0120] Example 2:

[0121] Please see Figure 4 As shown, this embodiment provides a drive state monitoring and control system based on deformation compensation, including:

[0122] The theoretical analysis module is used to divide the robot into driven and non-driven segments, construct the forward motion model of the driven segment based on the geometric parameters and kinematic relationships of the driven segment, collect the target end-effector motion trajectory of the robot, and perform reverse calculation by combining the target end-effector motion trajectory with the forward motion model of the driven segment to obtain the theoretical extension and retraction displacement.

[0123] The load analysis module is used to collect the actual release and retraction displacement of the robot's ropes, and calculate the total load on the robot's drive end and the rate of change of the total load on the drive end based on the actual release and retraction displacement, the theoretical release and retraction displacement, and the pre-calibrated rope elasticity parameters.

[0124] The velocity analysis module is used to acquire image sequences captured by the robot, extract feature points from adjacent frames, and obtain the measured displacement velocity of the robot's end effector in the direction of motion by performing feature point matching and inter-frame motion estimation on adjacent frames.

[0125] The state discrimination module is used to collect the drive motor current of the robot, and to perform drive state discrimination based on the drive motor current, measured displacement speed, total load of the drive end and the rate of change of the total load of the drive end, so as to obtain the state discrimination result of the robot.

[0126] The graded response module is used to perform graded responses based on the state determination results.

[0127] The above description is merely a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the technical scope disclosed in the present invention should be included within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims.

Claims

1. A drive state monitoring and control method based on deformation compensation, characterized in that, include: The robot is divided into driven and non-driven segments, and a forward motion model of the driven segment is constructed based on the geometric parameters and kinematic relationships of the driven segment. Collect the target end-effector motion trajectory of the robot, combine the target end-effector motion trajectory with the forward motion model of the drive segment to perform reverse calculation, and obtain the theoretical extension and retraction displacement. The actual extension and retraction displacement of the robot's rope is collected. Based on the actual extension and retraction displacement and theoretical extension and retraction displacement, as well as the pre-calibrated rope elasticity parameters, the total load on the robot's drive end and the rate of change of the total load on the drive end are calculated. The robot captures a sequence of images, extracts feature points from adjacent frames, and obtains the measured displacement velocity of the robot's end effector in the direction of motion by matching feature points and estimating inter-frame motion in adjacent frames. The robot's drive motor current is collected, and the drive state is determined based on the drive motor current, measured displacement speed, total load at the drive end, and the rate of change of the total load at the drive end, so as to obtain the robot's state determination result. A graded response is made based on the state discrimination results.

2. The drive state monitoring and control method based on deformation compensation according to claim 1, characterized in that, Methods for obtaining theoretical expansion and contraction displacements include: The motion type is determined based on the target end motion trajectory. For motion types determined to be pure axial motion, the target end motion trajectory is converted into the target trajectory at the end of the drive segment. The bending direction angle of each joint unit is set as the target bending direction angle obtained by calibrating the rope arrangement relationship. The difference between the theoretical X-axis position of the drive segment end output by the drive segment positive motion model and the target position of the drive segment end is taken as the axial position error. The total bending angle of each joint unit is solved by the Newton iteration method or the Jacobi pseudo-inverse method. When the axial position error is less than the convergence accuracy, the theoretical release and retraction displacement of each rope is calculated based on the total bending angle and the bending direction angle. When the target end does not coincide with the drive segment end, the target end motion trajectory is converted into the target trajectory at the end of the drive segment based on the length of the non-drive segment, the attitude constraint relationship of the non-drive segment, and the camera extrinsic parameters.

3. The drive state monitoring and control method based on deformation compensation according to claim 1, characterized in that, Methods for calculating the total load on the driver include: The actual extension and retraction displacement of the robot's rope is collected, and the apparent deformation is calculated based on the actual and theoretical extension and retraction displacements. The total load on the robot's drive end is calculated based on the pre-calibrated rope elasticity parameters and apparent deformation.

4. The drive state monitoring and control method based on deformation compensation according to claim 1, characterized in that, Methods for obtaining measured displacement velocity include: Fast corner points are extracted from adjacent frame images and directional fast rotation descriptors are calculated. Hamming distance brute-force matching is used to obtain initial matching point pairs. The initial matching point pairs are filtered using a random sampling consensus algorithm to obtain a set of matching point pairs. For the set of matching point pairs, the displacement between adjacent frames is obtained through homography matrix decomposition. The product of the displacement between adjacent frames and the scale factor is calculated to obtain the actual physical displacement. The ratio of the actual physical displacement to the time interval between adjacent frames is calculated to obtain the measured displacement velocity.

5. The drive state monitoring and control method based on deformation compensation according to claim 1, characterized in that, Methods for obtaining the non-driving segment stuck state in the state discrimination result include: The current jamming threshold is determined based on whether the robot is in a bend. When the robot is not currently in a bend, the jamming resistance threshold is used as the current jamming threshold. When the robot is currently in a bend, the jamming resistance threshold is adjusted and used as the current jamming threshold. When the total load on the drive end exceeds the current jamming threshold, the duration for which the measured displacement speed is continuously less than the minimum measurable speed exceeds the stagnation time threshold, and the drive motor current is not greater than the first current upper limit threshold, the state judgment result is determined to be a non-drive segment jamming state.

6. The drive state monitoring and control method based on deformation compensation according to claim 5, characterized in that, Methods for obtaining the stuck state of the drive segment in the state discrimination result include: The current jamming threshold is determined based on whether the robot is in a bend. When the robot is not currently in a bend, the jamming resistance threshold is used as the current jamming threshold. When the robot is currently in a bend, the jamming resistance threshold is adjusted and used as the current jamming threshold. When the total load on the drive end is greater than the current jamming threshold, the duration of the measured displacement speed being less than the minimum measurable speed exceeds the stagnation time threshold, and the drive motor current is greater than the second lower current limit threshold, the state judgment result is determined to be a drive segment jamming state. The first upper current limit threshold is less than or equal to the second lower current limit threshold. When the drive motor current is greater than the first upper current limit threshold but not greater than the second lower current limit threshold, the state judgment result is determined to be a jamming state awaiting review, and a secondary judgment is performed based on the change rate and duration of the total load on the drive end.

7. The drive state monitoring and control method based on deformation compensation according to claim 1, characterized in that, Methods for obtaining the normal state in the state discrimination results include: When the total load on the drive end is not greater than the normal resistance threshold and the measured displacement speed is not lower than the minimum measurable speed, the state judgment result is judged as normal.

8. The drive state monitoring and control method based on deformation compensation according to claim 1, characterized in that, Methods for obtaining the rope state in the state discrimination result include: When all preset rope slippage conditions are met, the rope is determined to be in a slippage state. The preset rope slippage conditions include: the total load change rate at the drive end does not increase significantly, the measured displacement speed is continuously less than the minimum measurable speed, the drive motor current is not greater than the first current upper limit threshold, the actual release and retraction displacement still increases and the minimum apparent deformation is lower than the apparent deformation lower limit threshold. When all preset rope breakage conditions are met, the rope is determined to be in a broken state. The preset rope breakage conditions include a decrease in the rate of change of the total load at the drive end, a continuous decrease in the measured displacement speed below the minimum measurable speed, a decrease in the drive motor current below the no-load current, and a ratio of the actual increase in retraction and extension displacement to the increase in motor rotation angle exceeding the normal threshold.

9. The drive state monitoring and control method based on deformation compensation according to claim 1, characterized in that, The tiered response includes: a Level 1 response is executed when the total load on the drive end exceeds the jamming resistance threshold but the duration is less than the warning time threshold; a Level 2 response is executed when the duration of jamming in the non-drive segment exceeds the stagnation time threshold; a preset Level 3 response is executed when the drive segment is jammed or the rope is broken; an automatic tensioning response is executed when the rope is slipping, and if the rope is still determined to be slipping after the automatic tensioning response, it is upgraded to an emergency stop and a Level 3 response is executed; the Level 3 response includes stopping all drive motors of the robot, issuing a Level 3 alarm signal and recording a fault log; the automatic tensioning response includes stopping the forward drive, controlling the rope drive end to perform tensioning actions according to the tensioning step and re-executing the rope slipping judgment.

10. A drive state monitoring and control system based on deformation compensation, used to implement the drive state monitoring and control method based on deformation compensation as described in any one of claims 1-9, characterized in that, include: The theoretical analysis module is used to divide the robot into driven and non-driven segments, construct the forward motion model of the driven segment based on the geometric parameters and kinematic relationships of the driven segment, collect the target end-effector motion trajectory of the robot, and perform reverse calculation by combining the target end-effector motion trajectory with the forward motion model of the driven segment to obtain the theoretical extension and retraction displacement. The load analysis module is used to collect the actual extension and retraction displacement of the robot's ropes, and calculate the total load on the robot's drive end and the rate of change of the total load on the drive end based on the actual extension and retraction displacement, theoretical extension and retraction displacement, and pre-calibrated rope elasticity parameters. The velocity analysis module is used to acquire image sequences captured by the robot, extract feature points from adjacent frames, and obtain the measured displacement velocity of the robot's end effector in the direction of motion by performing feature point matching and inter-frame motion estimation on adjacent frames. The state discrimination module is used to collect the drive motor current of the robot, and to perform drive state discrimination based on the drive motor current, measured displacement speed, total load of the drive end and the rate of change of the total load of the drive end, so as to obtain the state discrimination result of the robot. The graded response module is used to perform graded responses based on the state determination results.

Images