Power transmission mechanical arm operation posture analysis method and system fusing visual perception

Abstract

The application relates to the technical field of visual perception, and particularly discloses a power transmission mechanical arm operation posture analysis method and system fusing visual perception, joint motion data and target object visual pose data are collected and time-aligned; a recursive prior state estimation value and an uncertainty measure thereof are estimated, and the validity of visual observation is determined according to visual residual errors; when visual lock is lost, an amplitude upper limit constraint is applied to the uncertainty measure, disturbance acceleration is identified by using torque feedback and compensated to state recursion; when visual recovery, the end pose, speed and acceleration after disturbance compensation are used as initial boundaries, and the visual pose after recovery is used as a terminal constraint to generate a transition state sequence satisfying continuous acceleration change rate; during the transition period, the uncertainty measure is exponentially attenuated and contracted to a steady-state nominal value, and then state update recursion based on the visual pose is recovered; the application realizes smooth transition and accurate positioning of posture analysis under visual interference.

Description

Technical Field

[0001] This invention relates to the field of visual perception technology, and more specifically to a method and system for analyzing the working posture of a power transmission robotic arm that integrates visual perception. Background Technology

[0002] With the advancement of smart grid construction, using target objects to replace manual labor for high-altitude live-line work on transmission lines has become an industry trend. In typical tasks such as insulator string replacement, lead wire crimping, pin insertion, and bolt tightening, the end effector unit of the target object needs to perform millimeter-level precision target positioning and operation in strong winds at altitudes ranging from several meters to tens of meters. To achieve these precise operations, existing technologies generally employ visual perception to acquire the pose information of the target object in the work scene. This visual pose data serves as a crucial input for the motion planning and attitude control of the end effector unit, enabling the target object to autonomously approach and manipulate the target fittings through visual guidance.

[0003] When visual pose data of the target object in existing power transmission operations is lost due to interference from rain, fog, or lighting, the attitude analysis process has two major defects: during the visual lockout period, it only relies on the integral recursion of the joint encoder, which cannot suppress the cumulative drift of the end effector pose caused by high-altitude wind vibration; during visual recovery, the covariance expansion causes a step change in the end effector unit, resulting in positioning deviation and impact load, which makes it difficult to meet the requirements of smooth end effector movement and positioning reliability for high-precision operations such as pin insertion. Summary of the Invention

[0004] The purpose of this invention is to provide a method and system for analyzing the working posture of a power transmission robotic arm that integrates visual perception, so as to solve the problems mentioned above.

[0005] The objective of this invention can be achieved through the following technical solutions: A method for analyzing the operational posture of a power transmission robotic arm that integrates visual perception includes the following steps: S1: Collect angular displacement, angular velocity, and torque feedback data of each joint of the target object, and simultaneously acquire left and right view images of the target object in the work scene using a binocular stereo vision sensor set near the end effector unit; project coded structured light stripes onto the surface of the target object using a line structured light projector, execute stereo matching algorithm and structured light decoding algorithm based on the left and right view images, and combine with the target object's preset geometric prior information to calculate the target object's three-dimensional spatial coordinates and rotation angle to obtain the target object's visual pose data; align the angular displacement, angular velocity, and torque feedback data with the visual pose data according to a unified time reference, and use it as input for subsequent state prediction; S2: Based on the angular displacement, angular velocity, torque feedback data and the state estimate of the previous moment, the prior state estimate of the current moment and its corresponding uncertainty measure are recursively obtained; the visual pose data of the current moment is subtracted from the predicted position and predicted posture angular components in the prior state estimate to obtain the observation residual sequence; the validity state of the current visual observation is determined according to the residual abnormality of the observation residual sequence. S3: If the validity state is visual unlocking, then apply an upper bound constraint on the amplitude of the uncertainty measure, and use the torque feedback data to identify the current disturbance acceleration acting on the end effector unit. Add the disturbance acceleration as a compensation to the integral term of the state recursion to obtain the prior state estimate after disturbance compensation and the uncertainty measure after constraint. S4: When the validity state switches from visual unlocking to visual recovery, the actual end pose, velocity and acceleration corresponding to the perturbation-compensated prior state estimate are used as initial boundary conditions, and the recovered visual pose data are used as endpoint constraints to generate a temporary transition state sequence that satisfies the continuous constraint of the rate of change of acceleration, which serves as the expected pose command during the transition period. S5: During the execution of the temporary transition state sequence, the uncertainty metric after being restricted is subjected to exponential decay shrinkage processing, so that the shrinkage uncertainty metric approaches the preset steady-state nominal value at the end of the transition. Then, the state update recursion based on visual pose data is restored using the steady-state nominal value to complete the entire pose analysis process.

[0006] As a further aspect of the present invention: the determination of the validity status of the current visual observation specifically includes: The observation residual sequence is obtained by subtracting the visual pose data at the current moment from the prior state estimate; Take the normal observation residuals of multiple consecutive frames in the historical time, calculate their drift mean and dispersion, and use the sum of the multiples of the drift mean and dispersion as the upper limit of the judgment, and the difference of the multiples of the drift mean and dispersion as the lower limit of the judgment. The upper limit and the lower limit of the judgment constitute the adaptive judgment interval. The observation residual at the current moment is compared with the adaptive decision interval. If it exceeds the adaptive decision interval, the current visual observation is determined to be in an abnormal state; otherwise, it is determined to be in a normal state.

[0007] As a further aspect of the present invention: the step of taking the normal observation residuals of multiple consecutive frames in historical time and calculating their drift mean and dispersion specifically includes: Take the observation residuals at historical moments under normal conditions, arrange them in chronological order, and construct a sliding residual sequence; The weighted residual sequence is obtained by assigning a corresponding weight value to each residual member based on the time interval between each residual member and the current time. The residual member closer to the current time has a larger weight value, and the residual member farther away from the current time has a smaller weight value. Perform a weighted summation operation on the weighted residual sequence and divide by the sum of the weights to obtain the drift mean; Based on the deviation magnitude of each residual member in the weighted residual sequence relative to the drift mean, the degree of dispersion is obtained by summing the weighted squares according to the weight value corresponding to each residual member.

[0008] As a further aspect of the present invention: S3 specifically includes: Using the current angular displacement and angular velocity as input, and combining them with preset joint inertia parameters, calculate the theoretical torque of each joint in the current motion state; The actual torque feedback data of each joint is subtracted from the corresponding theoretical torque for each joint to obtain the torque residual of each joint; Using the torque residuals of each joint as input, and based on the spatial coupling relationship between the motion axes of each joint, the torque residuals of each joint are converted into the equivalent disturbance acceleration of the end effector unit in the work space, which is used as a compensation quantity.

[0009] As a further aspect of the present invention: the calculation of the theoretical torque of each joint in the current motion state specifically includes: Get the angular displacement and angular velocity of the current joint, and also get the angular displacement and angular velocity of the adjacent parent joint. Calculate the joint's own inertial torque based on the current joint's angular displacement and angular velocity, and simultaneously calculate the coupling torque generated by the adjacent joint's motion on the current joint based on the adjacent previous-level joint's angular displacement and angular velocity. The theoretical torque of the joint in its current motion state is obtained by superimposing its own inertial torque with the coupling torque.

[0010] As a further aspect of the present invention: the process of obtaining the desired pose command during the transition period is as follows: Obtain the actual pose, actual velocity, and actual acceleration of the end point in the initial boundary conditions, as well as the end point pose in the end point constraints, and set the total transition time to a preset time window; The preset time window is divided into three consecutive time segments in chronological order: the initial adjustment segment, the intermediate transition segment, and the terminal approach segment. The duration ratio of each time segment is a preset fixed ratio. With the initial boundary conditions satisfied at the starting point of the initial adjustment segment and the endpoint constraints satisfied at the endpoint constraints with both the endpoint velocity and the endpoint acceleration being zero, the variation law of the end pose with time in each time segment is constructed respectively, so that the pose, velocity, acceleration and acceleration change rate at the connection point of adjacent time segments are continuous. The variation laws of each time segment are sequentially connected to obtain a temporary transition state sequence that satisfies the continuous acceleration change rate constraint.

[0011] As a further aspect of the present invention: S5 specifically includes: Obtain the current actual amplitude of the uncertainty measure after the constraint, and obtain the preset steady-state nominal value, and calculate the deviation amplitude between the actual amplitude and the steady-state nominal value; Obtain the execution duration of the temporary transition state sequence from the start time to the current time, and obtain the total duration of the temporary transition state sequence. Calculate the ratio of the execution duration to the total duration. Using the ratio as an indicator of the decay process, a decay coefficient with the natural constant as the base and a negative multiple of the ratio as the exponent is constructed. The deviation amplitude is multiplied by the decay coefficient to obtain the deviation to be contracted at the current moment. The sum of the steady-state nominal value and the deviation to be contracted is used as the uncertainty measure after contraction at the current moment, until the temporary transition state sequence is completed, so that the uncertainty measure after contraction approaches the steady-state nominal value at the end of the transition.

[0012] As a further aspect of the present invention: the process of obtaining the deviation amplitude is as follows: The constrained uncertainty measure is decomposed into components along each coordinate direction of the workspace, and the components corresponding to each coordinate direction of the steady-state nominal value are obtained. Based on the motion sensitivity of the end effector unit along each coordinate direction in the current work process, corresponding weight coefficients are set for each coordinate direction. The deviation components in each coordinate direction are obtained by subtracting the actual components from the nominal components in each direction. The deviation components in each coordinate direction are weighted and summed according to their respective weighting coefficients to obtain the deviation amplitude.

[0013] A power transmission robotic arm posture analysis system integrating visual perception includes: The data acquisition module collects angular displacement, angular velocity and torque feedback data of each joint of the target object, and simultaneously collects visual pose data of the target object in the operation scene. After aligning the two types of data according to a unified time reference, they are used as input for subsequent state prediction. The state recursion and validity determination module recursively obtains the prior state estimate and its corresponding uncertainty measure at the current moment based on the feedback data of angular displacement, angular velocity, and torque, as well as the state estimate at the previous moment. At the same time, it determines the validity state of the current visual observation based on the degree of residual abnormality between the visual pose data and the prior state estimate. The amplitude constraint and disturbance compensation module applies an upper bound constraint on the uncertainty measure if the validity state is visual unlocking. It also uses torque feedback data to identify the disturbance acceleration currently acting on the end execution unit and adds the disturbance acceleration as a compensation quantity to the integral term of the state recursion to obtain the prior state estimate after disturbance compensation and the uncertainty measure after constraint. The transition trajectory generation module, when the validity state switches from visual unlocking to visual recovery, uses the actual end pose, velocity and acceleration corresponding to the prior state estimate after perturbation compensation as the initial boundary conditions, and the recovered visual pose data as the endpoint constraint to generate a temporary transition state sequence that satisfies the continuous constraint of the rate of change of acceleration, as the expected pose command during the transition period. The uncertainty contraction and state recovery module performs exponential decay contraction on the restricted uncertainty metric during the execution of the temporary transition state sequence, so that the contracted uncertainty metric approaches the preset steady-state nominal value at the end of the transition. Then, the state update recursion based on visual pose data is restored using the steady-state nominal value to complete the entire pose analysis process.

[0014] The beneficial effects of this invention are: (1) By constructing a temporary transition state sequence that satisfies the continuous constraint of the rate of change of acceleration, and with the exponential decay contraction of the uncertainty measure, the end-effector of the target object can approach the target pose with a smooth and gradual motion trajectory at the moment when the visual signal recovers from being unlocked. This effectively avoids the end-effector step pose jump caused by the covariance mutation in the traditional filtering method, thereby significantly reducing the risk of shear fracture caused by impact load in thin-walled pin insertion operations.

[0015] (2) By introducing a disturbance acceleration identification and compensation mechanism based on torque feedback data during visual loss of lock, the identified equivalent disturbance acceleration is superimposed on the integral term of state recursion, so that the state estimate in the pure prediction stage can track the changes of actual wind vibration disturbance in real time, effectively suppressing the cumulative drift of end pose during the visual loss period, ensuring that the deviation between the actual pose and the target pose is within a controllable range when vision is restored, and improving the pose maintenance capability of continuous operation in high-altitude strong wind environment. Attached Figure Description

[0016] The invention will now be further described with reference to the accompanying drawings.

[0017] Figure 1 This is a flowchart of the method of the present invention; Figure 2 This is a flowchart of the process for determining the validity status of the current visual observation in this invention; Figure 3 This is a flowchart illustrating the specific implementation process of step S3 in this invention; Figure 4 This is a system block diagram of the present invention. Detailed Implementation

[0018] 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.

[0019] Please see Figure 1 As shown, this invention is a method for analyzing the working posture of a power transmission robotic arm that integrates visual perception, including the following steps: S1: Collect angular displacement, angular velocity and torque feedback data of each joint of the target object, and simultaneously collect visual pose data of the target object in the operation scene. After aligning the two types of data according to a unified time reference, they are used as input for subsequent state prediction. S2: Based on the feedback data of angular displacement, angular velocity, and torque, as well as the state estimate of the previous moment, the prior state estimate of the current moment and its corresponding uncertainty measure are obtained recursively. At the same time, the validity of the current visual observation is determined based on the degree of residual abnormality between the visual pose data and the prior state estimate. S3: If the validity state is visual unlocking, then apply an upper bound constraint on the amplitude of the uncertainty measure, and use the torque feedback data to identify the current disturbance acceleration acting on the end execution unit. The disturbance acceleration is added as a compensation to the integral term of the state recursion to obtain the prior state estimate after disturbance compensation and the uncertainty measure after constraint. S4: When the validity state switches from visual unlocking to visual recovery, the actual end pose, velocity and acceleration corresponding to the prior state estimate after perturbation compensation are used as the initial boundary conditions, and the recovered visual pose data are used as the endpoint constraint to generate a temporary transition state sequence that satisfies the continuous constraint of the rate of change of acceleration, which serves as the expected pose command during the transition period. S5: During the execution of the temporary transition state sequence, the uncertainty metric after being restricted is subjected to exponential decay shrinkage processing, so that the shrinkage uncertainty metric approaches the preset steady-state nominal value at the end of the transition. Then, the state update recursion based on visual pose data is restored using the steady-state nominal value to complete the entire pose resolution process.

[0020] In S1, angular displacement, angular velocity, and torque feedback data of each joint of the target object are collected, and visual pose data of the target object in the work scene are collected simultaneously. The two types of data are aligned according to a unified time reference and used as input for subsequent state prediction, specifically including: An angular displacement sensor, an angular velocity sensor, and a torque sensor are respectively installed at each joint of the target object. The angular displacement sensor, using a photoelectric encoder or a rotary transformer, is installed at the output end of the drive shaft of each joint to sense the joint's rotation angle in real time and output angular displacement data. The angular velocity sensor, using a DC tachogenerator or a digital rate gyroscope, is installed on the side of the drive shaft of each joint to sense the joint's rotation rate in real time and output angular velocity data. The torque sensor, using a strain gauge torque sensor, is installed between the reducer input end and the drive motor output end of each joint to sense the driving torque borne by the joint in real time and output torque feedback data.

[0021] The three sensors mentioned above collect data synchronously at a sampling frequency of 1 kHz, and the collected data is transmitted to the central processing unit via fieldbus.

[0022] A binocular stereo vision sensor is configured near the end-effector of the target object. The sensor consists of two industrial cameras with a fixed baseline distance. Both industrial cameras are equipped with global shutter type image sensors to synchronously acquire left and right view images of the work scene at a frame rate of 30 frames per second.

[0023] Simultaneously, a line structured light projector is configured next to the binocular stereo vision sensor. This projector projects coded structured light stripes onto the surface of the target object to enhance the discernible features of the target object's surface under weak texture conditions. The left and right view images acquired by the binocular stereo vision sensor are transmitted to the image processing unit via transmission cables. The image processing unit calculates the three-dimensional spatial coordinates of the target object from the left and right view images based on stereo matching and structured light decoding algorithms. Combining this with the target object's preset geometric prior information, it further calculates the rotation angle of the target object relative to the coordinate system of the binocular stereo vision sensor. By combining the three-dimensional spatial coordinates and the rotation angle, the visual pose data of the target object is obtained.

[0024] Since angular displacement data, angular velocity data, torque feedback data, and visual pose data come from different sensors and have different sampling frequencies, a field-programmable gate array (FPGA) is used as the timing synchronization master station to provide time-consistent data input for subsequent state prediction processing. This master station is connected to the data acquisition units of each joint sensor and the image acquisition unit of the binocular stereo vision sensor via hard-wired synchronization signals. At the beginning of each sampling period, the timing synchronization master station simultaneously sends trigger pulses to each data acquisition unit and image acquisition unit. Upon receiving the trigger pulse, each unit records the system timestamp of the current moment and binds this timestamp to the acquired data. For visual pose data, due to the processing delay between image acquisition and pose calculation, the image processing unit appends the timestamp of the original exposure moment to the visual pose data after completing the pose calculation.

[0025] After receiving all timestamped data, the central processing unit selects angular displacement data, angular velocity data, torque feedback data, and visual pose data with timestamp deviations within a preset allowable range, using the timestamps as indices. These data are then grouped into data sets for the same processing cycle. For processing cycles with missing visual pose data, the visual pose data from the previous valid cycle is used until new data arrives, thus forming a time-aligned data sequence as input for subsequent state prediction.

[0026] Please see Figure 2 As shown, in S2, based on the angular displacement, angular velocity, torque feedback data and the state estimate from the previous moment, the prior state estimate for the current moment and its corresponding uncertainty metric are recursively obtained. Simultaneously, based on the degree of residual anomaly between the visual pose data and the prior state estimate, the validity of the current visual observation is determined, specifically including: At the start of each processing cycle, the central processing unit (CPU) acquires the updated posterior state estimate from the previous cycle. This posterior state estimate includes the three-dimensional position coordinates, three-dimensional attitude angles, and angular displacements and angular velocities of each joint of the end effector unit in the workspace. Using this posterior state estimate as the starting point for the current cycle, and simultaneously inputting the angular displacement data, angular velocity data, and torque feedback data collected in the current cycle, the CPU recursively applies the posterior state estimate from the previous cycle to the current moment according to the kinematic equations of the end effector unit, obtaining the prior state estimate for the current moment. This prior state estimate includes the predicted position, predicted attitude angles, and predicted angular displacements and angular velocities of each joint of the end effector unit at the current moment.

[0027] While recursively calculating the prior state estimate, the variance and covariance of the prediction error of each state variable are recorded during the recursion process, forming a two-dimensional numerical matrix. The diagonal elements of this numerical matrix are the variance of the prediction error of each state variable, and the off-diagonal elements are the covariance between different state variables. This numerical matrix serves as a measure of the uncertainty associated with the prior state estimate.

[0028] After obtaining the prior state estimate at the current moment, the visual pose data collected in the current cycle is subtracted from the predicted position and predicted attitude angle components in the prior state estimate. Specifically, the visual pose data includes three position coordinate components and three attitude angle components of the target object in the workspace. The predicted position in the prior state estimate includes the corresponding three position coordinate components, and the predicted attitude angle includes the corresponding three attitude angle components. The components in the same coordinate direction of the two are subtracted one by one to obtain six residual components. These six residual components are arranged in a preset order to form the observation residual sequence at the current moment.

[0029] To determine whether the visual pose data at the current moment is usable, a sliding residual buffer is maintained in the central processing unit. This buffer stores a series of consecutive observation residual sequences that have been determined to be in a normal state from historical moments. The buffer depth is set to a fixed value of 20, meaning that the buffer always contains the observation residual sequences of the most recent 20 frames in a normal state. In each processing cycle, all normal observation residual sequences are retrieved from the buffer. Based on the time distance of each sequence from the current moment, a weight value is assigned to each residual sequence: the weight value corresponding to the residual sequence closest to the current moment is set to 1, and for each subsequent frame forward, the weight value decreases by 0.05 until the weight value of the furthest frame in the buffer decreases to 0.05, resulting in a weighted residual sequence set.

[0030] A weighted average operation is performed on the set of weighted residual sequences. This involves multiplying each of the six residual components in each residual sequence by its corresponding weight value, summing all products, and dividing by the sum of the weight values ​​to obtain the weighted average residual sequence. This weighted average residual sequence contains the mean of the six components, which is used as the drift mean at the current time step. For each residual sequence in the set of weighted residual sequences, the deviation of each of its six residual components relative to the corresponding component in the drift mean is calculated. Each deviation is squared and multiplied by its corresponding weight value. All weighted squares are summed to obtain the weighted sum of squares, which is used as the degree of dispersion.

[0031] An adaptive decision interval is constructed using the drift mean as the center and the product of the dispersion multiplied by a preset multiple as the offset. Specifically, the lower bound component is obtained by subtracting the product of the dispersion and the preset multiple from each component of the drift mean, and the upper bound component is obtained by adding the product of the dispersion and the preset multiple to each component of the drift mean. The preset multiple is set to 3. All lower bound and upper bound components are combined to form the lower and upper bounds of the adaptive decision interval.

[0032] The six residual components in the current observation residual sequence are compared one by one with the corresponding components in the adaptive decision interval. If all six residual components are between the corresponding lower and upper decision bound components, the current visual observation is determined to be in a normal state. The current observation residual sequence is stored in the sliding residual buffer, and the oldest residual sequence in the buffer is removed to update the buffer. If any one of the six residual components exceeds the corresponding lower or upper decision bound component, the current visual observation is determined to be in an abnormal state. The current observation residual sequence is not stored in the buffer, the contents of the buffer remain unchanged, and an abnormal state flag is output for subsequent steps.

[0033] Please see Figure 3 As shown, in S3, if the validity state is visual unlocking, an upper bound constraint on the amplitude of the uncertainty measure is applied, and the disturbance acceleration currently acting on the end effector is identified using torque feedback data. The disturbance acceleration is then added as a compensation to the integral term of the state recursion to obtain the disturbance-compensated prior state estimate and the constrained uncertainty measure, specifically including: When the output validity status is marked as an abnormal state, the central processing unit performs two parallel processes. The first process is to apply an upper bound constraint on the amplitude of the uncertainty measure accompanying the prior state estimate at the current moment. The second process is to identify the disturbance acceleration currently acting on the end effector unit using the torque feedback data of each joint.

[0034] For the first step, the central processing unit first calculates the square root of the sum of the squares of all diagonal elements in the current uncertainty metric matrix, using this square root as the current amplitude. A preset upper bound of 100 is set. If the current amplitude is less than or equal to 100, the current uncertainty metric matrix remains unchanged and is used as the constrained uncertainty metric. If the current amplitude is greater than 100, a scaling factor is calculated, which is equal to 100 divided by the current amplitude. Then, each element in the current uncertainty metric matrix is ​​multiplied by this scaling factor to obtain the constrained uncertainty metric. The calculation process of the scaling factor is expressed mathematically as follows: ,in This represents the uncertainty measure matrix after constraints are applied. This indicates the preset upper limit of the amplitude, with a value of 100. A represents the current amplitude. This represents the current uncertainty measurement matrix.

[0035] For the second processing step, the central processing unit acquires the angular displacement data, angular velocity data, and torque feedback data of each joint at the current moment. Combining this with the equivalent moment of inertia parameters and inter-joint coupling coefficient parameters of each joint pre-stored in the central processing unit, it calculates the theoretical torque of each joint in its current motion state. For the second step... There are 1 joint, among which This represents the joint number, which is a positive integer. The central processing unit retrieves the angular velocity, angular acceleration, and angular velocity of the adjacent upstream joint of this joint. The angular acceleration is obtained by dividing the difference between the current angular velocity and the previous angular velocity by the sampling interval.

[0036] Multiplying the equivalent moment of inertia of the joint by its angular acceleration yields the joint's own inertial torque. Multiplying the joint's coupling coefficient by its own angular velocity and then by the angular velocity of its adjacent parent joint yields the coupling torque generated by the movement of the adjacent parent joint on the current joint. Adding the joint's own inertial torque to the coupling torque yields the theoretical torque of the joint in its current motion state. The above calculation process can be expressed mathematically as follows: ,in Indicates the first The theoretical torque of each joint Indicates the first The equivalent rotational inertia of each joint Indicates the first angular acceleration of each joint, Indicates the first The coupling coefficient between a joint and its adjacent parent joint. Indicates the first angular velocity of each joint This represents the angular velocity of the adjacent upstream joint.

[0037] After performing the theoretical torque calculation on each joint, the central processing unit subtracts the actual torque feedback data of each joint from the corresponding theoretical torque for each joint to obtain the torque residual of each joint. Then, using the torque residual of each joint as input, the central processing unit converts the torque residual of each joint into the equivalent disturbance acceleration of the end effector in the workspace according to the pre-set Jacobian transpose matrix. The conversion process is as follows: multiply the inverse of the Jacobian transpose matrix by the column vector composed of the torque residuals of each joint to obtain the equivalent disturbance acceleration components of the end effector in the three coordinate directions of the workspace. Combine the equivalent disturbance acceleration components in the three coordinate directions to obtain the equivalent disturbance acceleration vector, which is used as the compensation amount.

[0038] The central processing unit adds the compensation amount to the integral term of the state recursion, that is, replaces the unknown disturbance term in the state recursion equation with the compensation amount, and re-executes the state recursion to obtain the disturbance-compensated prior state estimate. The disturbance-compensated prior state estimate and the constrained uncertainty measure obtained from the first processing are output together for subsequent steps.

[0039] In S4, when the validity state switches from visual unlocking to visual recovery, the actual end pose, velocity, and acceleration corresponding to the perturbation-compensated prior state estimate are used as initial boundary conditions, and the recovered visual pose data are used as endpoint constraints. A temporary transition state sequence satisfying the continuous acceleration rate of change constraint is generated as the expected pose command during the transition period, specifically including: When the validity status indicator output by S2 switches from an abnormal state to a normal state, the central processing unit first obtains the disturbance-compensated prior state estimate output by S3. From this prior state estimate, it extracts the three position coordinate components of the end effector in the workspace as the actual pose of the end effector, extracts the three linear velocity components of the end effector in the workspace as the actual velocity of the end effector, and extracts the three linear acceleration components of the end effector in the workspace as the actual acceleration of the end effector. The actual pose, actual velocity, and actual acceleration of the end effector are combined to form the initial boundary conditions.

[0040] The central processing unit acquires the visual pose data at the current moment, extracts the three position coordinate components of the target object in the work space as the endpoint pose, and sets the total transition time as a preset time window. The value of the preset time window is between 50 milliseconds and 80 milliseconds. The central processing unit determines the specific value of the preset time window based on the magnitude of the actual velocity of the end effector at the current moment. The larger the magnitude of the actual velocity of the end effector, the larger the value of the preset time window, and the smaller the magnitude, the smaller the value of the preset time window.

[0041] The central processing unit (CPU) divides the preset time window into three consecutive time segments in chronological order: an initial adjustment segment, an intermediate transition segment, and a terminal approximation segment. The ratio of the total duration of the preset time window to the duration of each time segment is stored in the CPU's non-volatile memory. This ratio is fixed at 2:3:2, meaning the initial adjustment segment accounts for 2 / 7 of the total preset time window duration, the intermediate transition segment accounts for 3 / 7, and the terminal approximation segment accounts for 2 / 7. Based on this ratio, the CPU calculates the start and end times of each time segment, with the start time of the preset time window as time zero, the end time of the initial adjustment segment as 2 / 7 of the total preset time window duration, the end time of the intermediate transition segment as 5 / 7 of the total preset time window duration, and the end time of the terminal approximation segment as the total preset time window duration.

[0042] The central processing unit (CPU) constructs the time-varying characteristics of the three position coordinate components of the end effector within the initial adjustment segment, intermediate transition segment, and terminal approximation segment. For each position coordinate component, the CPU uses the pose, velocity, and acceleration of that coordinate component at the beginning of the corresponding time segment as the initial constraint for that segment, and the pose, velocity, and acceleration of that coordinate component at the end of the corresponding time segment as the termination constraint for that segment. For the initial adjustment segment, the initial constraint at its starting point is provided by the actual end effector pose, actual end effector velocity, and actual end effector acceleration in the initial boundary conditions. The pose, velocity, and acceleration of that coordinate component at the end of the initial adjustment segment are set as undetermined parameters, and there are three such undetermined parameters. For the intermediate transition segment, the pose, velocity, and acceleration at its starting point are equal to the undetermined parameters corresponding to the end of the initial adjustment segment. The pose, velocity, and acceleration at its end are set as the second set of undetermined parameters, and there are three such undetermined parameters.

[0043] For the terminal approximation segment, the pose, velocity, and acceleration at the starting point are equal to the second set of undetermined parameters at the end point of the intermediate transition segment, the pose at the end point is equal to the corresponding coordinate component in the end point pose, the velocity at the end point is set to zero, and the acceleration at the end point is set to zero.

[0044] Based on the aforementioned constraints, the central processing unit (CPU) imposes a constraint on the continuity of the rate of acceleration change of the coordinate component within each time segment. Specifically, at the junction of the initial adjustment segment and the intermediate transition segment, the rate of acceleration change of the coordinate component is equal on both sides of the junction point; similarly, at the junction of the intermediate transition segment and the final approximation segment, the rate of acceleration change of the coordinate component is equal on both sides of the junction point. Through these constraints, the CPU determines the rate of change of the coordinate component within each time segment, using time as the variable. This rate of change is described by a fifth-order polynomial, meaning the coordinate component changes with time according to a fifth-order power function of time.

[0045] The central processing unit (CPU) independently executes the above construction process for each of the three position coordinate components, obtaining the independent variation law of each coordinate component within each time segment. The variation laws of the three coordinate components at the same moment are combined to obtain the complete pose of the end-effector at that moment. Following a preset control cycle, the CPU calculates the complete pose point-by-point at each moment using a preset control step size, starting from the beginning of a preset time window, until the end of the preset time window. The calculated complete poses at all moments are then arranged in temporal order to obtain a temporary transition state sequence that satisfies the continuous acceleration rate constraint. This temporary transition state sequence is output as the desired pose command for the transition period, for subsequent steps to call.

[0046] In S5, during the execution of the temporary transition state sequence, the constrained uncertainty metric undergoes exponential decay shrinkage processing, causing the shrunken uncertainty metric to approach a preset steady-state nominal value at the end of the transition. Then, the state update recursion based on visual pose data is restored using the steady-state nominal value, completing the entire pose resolution process. Specifically, this includes: When the temporary transition state sequence generated in step four begins execution, the central processing unit (CPU) initiates an exponentially decaying contraction process. This process first acquires the constrained uncertainty metric matrix output in step three. This matrix is ​​a 9x9 two-dimensional numerical matrix, where the first three diagonal elements correspond to the position prediction error variance of the end effector in the three coordinate directions of the workspace, the middle three diagonal elements correspond to the velocity prediction error variance in the three coordinate directions, and the last three diagonal elements correspond to the acceleration prediction error variance in the three coordinate directions. The CPU extracts the first three diagonal elements from this matrix, i.e., the position prediction error variance in the three coordinate directions, as the actual components of the constrained uncertainty metric in each coordinate direction of the workspace. Simultaneously, the CPU acquires preset steady-state nominal values. These are three nominal variance values ​​pre-stored in the CPU's non-volatile storage area, corresponding to the steady-state position prediction error variance values ​​in the three coordinate directions of the workspace. The values ​​of these three nominal variance values ​​are 0.01, 0.01, and 0.02, all in millimeters squared.

[0047] The central processing unit (CPU) acquires the motion sensitivity parameters of the end effector in the current operation. These parameters include three weighting coefficients, each corresponding to one of the three coordinate directions in the workspace. When the current operation is a pin insertion operation, the three weighting coefficients are set to 0.8, 0.1, and 0.1, respectively. 0.8 corresponds to the pin axis direction, and 0.1 corresponds to the two directions perpendicular to the pin axis. The CPU subtracts the actual component from the corresponding nominal component in each coordinate direction to obtain the deviation component for each direction. Then, the absolute value of the deviation component is multiplied by the corresponding weighting coefficient, and the three products are summed to obtain the deviation amplitude.

[0048] The central processing unit (CPU) acquires the execution duration of the temporary transitional state sequence from the start time to the current time in each control cycle, and also acquires the total duration of the temporary transitional state sequence. The CPU calculates the ratio of the execution duration to the total duration, with this ratio ranging from 0 to 1. Using this ratio as an indicator of the decay process, the CPU constructs a decay coefficient. This decay coefficient is constructed by using the natural constant e as the base and negative five times the ratio as the exponent, and the resulting exponential function value is used as the decay coefficient. The CPU multiplies the deviation amplitude by this decay coefficient to obtain the current deviation to be contracted. The CPU adds the three nominal components of the steady-state nominal value to the deviation to be contracted along the corresponding coordinate directions to obtain the three coordinate direction uncertainty measurement components after contraction at the current time. These components are then combined to form the uncertainty measurement after contraction at the current time.

[0049] The central processing unit (CPU) continuously executes the aforementioned deviation amplitude calculation, ratio calculation, attenuation coefficient construction, and post-contraction uncertainty metric calculation in each control cycle until the temporary transition state sequence is completed. At the end of the temporary transition state sequence, the executed time equals the total time, the ratio equals one, the attenuation coefficient equals the natural constant e to the power of negative five, and the deviation to be contracted is approximately 0.67% of the initial deviation amplitude. The post-contraction uncertainty metric has approached the steady-state nominal value. After the temporary transition state sequence is completed, the CPU uses the steady-state nominal value as the posterior uncertainty metric for the current moment, and uses the endpoint pose of the temporary transition state sequence output in step four as the posterior state estimate for the current moment. It then resumes the state update recursion based on the visual pose data. That is, in each subsequent processing cycle, the collected visual pose data and the prior state estimate are used to calculate the residual, and the posterior state estimate is updated to complete the entire pose resolution process.

[0050] Please see Figure 4 As shown, the power transmission robotic arm's operational posture analysis system, which integrates visual perception, includes: The data acquisition module collects angular displacement, angular velocity and torque feedback data of each joint of the target object, and simultaneously collects visual pose data of the target object in the operation scene. After aligning the two types of data according to a unified time reference, they are used as input for subsequent state prediction. The state recursion and validity determination module recursively obtains the prior state estimate and its corresponding uncertainty measure at the current moment based on the feedback data of angular displacement, angular velocity, and torque, as well as the state estimate at the previous moment. At the same time, it determines the validity state of the current visual observation based on the degree of residual abnormality between the visual pose data and the prior state estimate. The amplitude constraint and disturbance compensation module applies an upper bound constraint on the uncertainty measure if the validity state is visual unlocking. It also uses torque feedback data to identify the disturbance acceleration currently acting on the end execution unit and adds the disturbance acceleration as a compensation quantity to the integral term of the state recursion to obtain the prior state estimate after disturbance compensation and the uncertainty measure after constraint. The transition trajectory generation module, when the validity state switches from visual unlocking to visual recovery, uses the actual end pose, velocity and acceleration corresponding to the prior state estimate after perturbation compensation as the initial boundary conditions, and the recovered visual pose data as the endpoint constraint to generate a temporary transition state sequence that satisfies the continuous constraint of the rate of change of acceleration, as the expected pose command during the transition period. The uncertainty contraction and state recovery module performs exponential decay contraction on the restricted uncertainty metric during the execution of the temporary transition state sequence, so that the contracted uncertainty metric approaches the preset steady-state nominal value at the end of the transition. Then, the state update recursion based on visual pose data is restored using the steady-state nominal value to complete the entire pose analysis process.

[0051] The working principle of this invention is as follows: Angular displacement, angular velocity, and torque feedback data of each joint of the target object are collected, and visual pose data of the target object in the work scene are collected simultaneously. The two types of data are aligned according to a unified time reference. Based on the aligned data and the state estimate of the previous moment, the prior state estimate and its uncertainty measure are recursively derived. The validity of the visual observation is determined based on the degree of residual abnormality between the visual pose data and the prior state estimate. When visual lock is lost, an upper bound constraint on the amplitude of the uncertainty measure is applied, and the torque feedback data is used to identify the disturbance acceleration experienced by the end effector. The disturbance acceleration is then added as a compensation to the state recursion. In the integral term, the prior state estimate after perturbation compensation and the uncertainty measure after constraint are obtained. When vision is restored, the actual pose, velocity and acceleration of the end point corresponding to the prior state estimate after perturbation compensation are used as the initial boundary conditions, and the restored visual pose data is used as the endpoint constraint to generate a temporary transition state sequence that satisfies the continuous constraint of the rate of change of acceleration as the expected pose command during the transition period. During the execution of the transition state sequence, the uncertainty measure after constraint is subjected to exponential decay contraction processing so that it approaches the preset steady-state nominal value at the end of the transition. Then, the state update recursion based on the visual pose data is restored using the steady-state nominal value to complete the attitude resolution.

[0052] In this embodiment, the "target object" is not limited to a specific type of mechanical actuator; it can be any object with a joint structure or that can be characterized as a multi-degree-of-freedom motion unit. Specifically, the target object can be the robotic arm body and its end effector, or other work equipment, operating components, or observed target entities with associated motion relationships, such as insulator strings, hardware assemblies, connectors, clamp structures, or other target objects with spatial pose change characteristics in power transmission operations.

[0053] Furthermore, when the target object is a non-robotic arm-like entity, its "angular displacement", "angular velocity" and "torque feedback data" can be equivalently obtained or indirectly calculated through image perception, visual tracking or motion estimation based on multi-frame image sequences, thereby uniformly constructing state variable parameters describing the spatial motion state of the target object for subsequent visual pose analysis and state prediction processing.

[0054] With the above settings, the present invention can uniformly model and analyze target objects with spatial pose change characteristics in the work scene without relying on specific mechanical structures, thereby enhancing the versatility and applicability of the method in various visual perception application scenarios.

[0055] The foregoing has provided a detailed description of one embodiment of the present invention, but this description is merely a preferred embodiment and should not be construed as limiting the scope of the invention. All equivalent variations and modifications made within the scope of the claims of this invention should still fall within the patent coverage of this invention.

Claims

1. A method for analyzing the working posture of a power transmission robotic arm integrating visual perception, characterized in that, Includes the following steps: S1: Collect angular displacement, angular velocity, and torque feedback data of each joint of the target object, and simultaneously collect left and right view images of the target object in the work scene through a binocular stereo vision sensor set near the end effector unit; project coded structured light stripes onto the surface of the target object using a line structured light projector, execute stereo matching algorithm and structured light decoding algorithm based on the left and right view images, and combine the preset geometric prior information of the target object to calculate the three-dimensional spatial coordinates and rotation angle of the target object to obtain the visual pose data of the target object; The angular displacement, angular velocity, and torque feedback data are aligned with the visual pose data according to a unified time reference and then used as input for subsequent state prediction. S2: Based on the angular displacement, angular velocity, torque feedback data and the state estimate of the previous moment, the prior state estimate of the current moment and its corresponding uncertainty measure are recursively obtained; the visual pose data of the current moment is subtracted from the predicted position and predicted posture angular components in the prior state estimate to obtain the observation residual sequence; the validity state of the current visual observation is determined according to the residual abnormality of the observation residual sequence. S3: If the validity state is visual unlocking, then apply an upper bound constraint on the amplitude of the uncertainty measure, and use the torque feedback data to identify the current disturbance acceleration acting on the end effector unit. Add the disturbance acceleration as a compensation to the integral term of the state recursion to obtain the prior state estimate after disturbance compensation and the uncertainty measure after constraint. S4: When the validity state switches from visual unlocking to visual recovery, the actual end pose, velocity and acceleration corresponding to the perturbation-compensated prior state estimate are used as initial boundary conditions, and the recovered visual pose data are used as endpoint constraints to generate a temporary transition state sequence that satisfies the continuous constraint of the rate of change of acceleration, which serves as the expected pose command during the transition period. S5: During the execution of the temporary transition state sequence, the uncertainty metric after being restricted is subjected to exponential decay shrinkage processing, so that the shrinkage uncertainty metric approaches the preset steady-state nominal value at the end of the transition. Then, the state update recursion based on visual pose data is restored using the steady-state nominal value to complete the entire pose analysis process.

2. The method for analyzing the working posture of a power transmission robotic arm based on fused visual perception as described in claim 1, characterized in that, The determination of the validity status of the current visual observation specifically includes: The observation residual sequence is obtained by subtracting the visual pose data at the current moment from the prior state estimate; Take the normal observation residuals of multiple consecutive frames in the historical time, calculate their drift mean and dispersion, and use the sum of the multiples of the drift mean and dispersion as the upper limit of the judgment, and the difference of the multiples of the drift mean and dispersion as the lower limit of the judgment. The upper limit and the lower limit of the judgment constitute the adaptive judgment interval. The observation residual at the current moment is compared with the adaptive decision interval. If it exceeds the adaptive decision interval, the current visual observation is determined to be in an abnormal state; otherwise, it is determined to be in a normal state.

3. The method for analyzing the working posture of a power transmission robotic arm based on fused visual perception according to claim 2, characterized in that, The step of taking the normal observation residuals from multiple consecutive frames in historical time and calculating their drift mean and dispersion specifically includes: Take the observation residuals at historical moments under normal conditions, arrange them in chronological order, and construct a sliding residual sequence; The weighted residual sequence is obtained by assigning a corresponding weight value to each residual member based on the time interval between each residual member and the current time. The residual member closer to the current time has a larger weight value, and the residual member farther away from the current time has a smaller weight value. Perform a weighted summation operation on the weighted residual sequence and divide by the sum of the weights to obtain the drift mean; Based on the deviation magnitude of each residual member in the weighted residual sequence relative to the drift mean, the degree of dispersion is obtained by summing the weighted squares according to the weight value corresponding to each residual member.

4. The method for analyzing the working posture of a power transmission robotic arm based on fused visual perception according to claim 1, characterized in that, S3 specifically includes: Using the current angular displacement and angular velocity as input, and combining them with preset joint inertia parameters, calculate the theoretical torque of each joint in the current motion state; The actual torque feedback data of each joint is subtracted from the corresponding theoretical torque for each joint to obtain the torque residual of each joint; Using the torque residuals of each joint as input, and based on the spatial coupling relationship between the motion axes of each joint, the torque residuals of each joint are converted into the equivalent disturbance acceleration of the end effector unit in the work space, which is used as a compensation quantity.

5. The method for analyzing the working posture of a power transmission robotic arm based on fused visual perception according to claim 4, characterized in that, The calculation of the theoretical torque of each joint in the current motion state specifically includes: Get the angular displacement and angular velocity of the current joint, and also get the angular displacement and angular velocity of the adjacent parent joint. Calculate the joint's own inertial torque based on the current joint's angular displacement and angular velocity, and simultaneously calculate the coupling torque generated by the adjacent joint's motion on the current joint based on the adjacent previous-level joint's angular displacement and angular velocity. The theoretical torque of the joint in its current motion state is obtained by superimposing its own inertial torque with the coupling torque.

6. The method for analyzing the working posture of a power transmission robotic arm based on fused visual perception according to claim 1, characterized in that, The process of obtaining the desired pose command during the transition period is as follows: Obtain the actual pose, actual velocity, and actual acceleration of the end point in the initial boundary conditions, as well as the end point pose in the end point constraints, and set the total transition time to a preset time window; The preset time window is divided into three consecutive time segments in chronological order: the initial adjustment segment, the intermediate transition segment, and the terminal approach segment. The duration ratio of each time segment is a preset fixed ratio. With the initial boundary conditions satisfied at the starting point of the initial adjustment segment and the endpoint constraints satisfied at the endpoint constraints with both the endpoint velocity and the endpoint acceleration being zero, the variation law of the end pose with time in each time segment is constructed respectively, so that the pose, velocity, acceleration and acceleration change rate at the connection point of adjacent time segments are continuous. The variation laws of each time segment are sequentially connected to obtain a temporary transition state sequence that satisfies the continuous acceleration change rate constraint.

7. The method for analyzing the working posture of a power transmission robotic arm based on fused visual perception according to claim 1, characterized in that, S5 specifically includes: Obtain the current actual amplitude of the uncertainty measure after the constraint, and obtain the preset steady-state nominal value, and calculate the deviation amplitude between the actual amplitude and the steady-state nominal value; Obtain the execution duration of the temporary transition state sequence from the start time to the current time, and obtain the total duration of the temporary transition state sequence. Calculate the ratio of the execution duration to the total duration. Using the ratio as an indicator of the decay process, a decay coefficient with the natural constant as the base and a negative multiple of the ratio as the exponent is constructed. The deviation amplitude is multiplied by the decay coefficient to obtain the deviation to be contracted at the current moment. The sum of the steady-state nominal value and the deviation to be contracted is used as the uncertainty measure after contraction at the current moment, until the temporary transition state sequence is completed, so that the uncertainty measure after contraction approaches the steady-state nominal value at the end of the transition.

8. The method for analyzing the working posture of a power transmission robotic arm based on fused visual perception according to claim 7, characterized in that, The process for obtaining the deviation amplitude is as follows: The constrained uncertainty measure is decomposed into components along each coordinate direction of the workspace, and the components corresponding to each coordinate direction of the steady-state nominal value are obtained. Based on the motion sensitivity of the end effector unit along each coordinate direction in the current work process, corresponding weight coefficients are set for each coordinate direction. The deviation components in each coordinate direction are obtained by subtracting the actual components from the nominal components in each direction. The deviation components in each coordinate direction are weighted and summed according to their respective weighting coefficients to obtain the deviation amplitude.

9. A power transmission robotic arm posture analysis system integrating visual perception, characterized in that, The method for performing the power transmission robotic arm operation posture analysis method fused with visual perception as described in any one of claims 1-8 includes: The data acquisition module collects angular displacement, angular velocity and torque feedback data of each joint of the target object, and simultaneously collects visual pose data of the target object in the operation scene. After aligning the two types of data according to a unified time reference, they are used as input for subsequent state prediction. The state recursion and validity determination module recursively obtains the prior state estimate and its corresponding uncertainty measure at the current moment based on the feedback data of angular displacement, angular velocity, and torque, as well as the state estimate at the previous moment. At the same time, it determines the validity state of the current visual observation based on the degree of residual abnormality between the visual pose data and the prior state estimate. The amplitude constraint and disturbance compensation module applies an upper bound constraint on the uncertainty measure if the validity state is visual unlocking. It also uses torque feedback data to identify the disturbance acceleration currently acting on the end execution unit and adds the disturbance acceleration as a compensation quantity to the integral term of the state recursion to obtain the prior state estimate after disturbance compensation and the uncertainty measure after constraint. The transition trajectory generation module, when the validity state switches from visual unlocking to visual recovery, uses the actual end pose, velocity and acceleration corresponding to the prior state estimate after perturbation compensation as the initial boundary conditions, and the recovered visual pose data as the endpoint constraint to generate a temporary transition state sequence that satisfies the continuous constraint of the rate of change of acceleration, as the expected pose command during the transition period. The uncertainty contraction and state recovery module performs exponential decay contraction on the restricted uncertainty metric during the execution of the temporary transition state sequence, so that the contracted uncertainty metric approaches the preset steady-state nominal value at the end of the transition. Then, the state update recursion based on visual pose data is restored using the steady-state nominal value to complete the entire pose analysis process.

Images