Industrial vision-based intelligent robot motion capture control method and system

Abstract

This invention discloses a method and system for intelligent robot motion capture control based on industrial vision, belonging to the field of intelligent control technology for industrial robots. Specifically, the method includes calculating an initial drive sequence based on the spatial mapping relationship between the robot's end effector and the operator's end effector; collecting posture change data of its intermediate joints during robot following; comparing this data with the operator's posture change data to calculate the posture deviation, forming a posture deviation sequence; dynamically fusing the posture deviation, its trend value, and the operator's intermediate joint motion speed data to construct a time-varying constraint coefficient; and combining this with the initial drive sequence to calculate a corrected drive sequence, thereby controlling the robot to perform actions. This invention eliminates the risk of human-robot motion fragmentation and collisions caused by uncontrolled intermediate joint postures, enabling safe, efficient, and natural human-robot collaborative operation of industrial robots in flexible production lines.

Description

Technical Field

[0001] This invention relates to a method and system for intelligent robot motion capture and control based on industrial vision, belonging to the field of intelligent control technology for industrial robots. Background Technology

[0002] Against the backdrop of the rapid development of the intelligent manufacturing equipment industry, industrial robots, as the core execution units of flexible production lines, are widely used in high-end manufacturing scenarios such as automobile final assembly, 3C electronics precision assembly, aerospace component grinding and welding, and new energy battery production. As human-machine collaboration becomes the mainstream mode of intelligent manufacturing, robot natural teaching technology based on motion capture, replacing traditional teach pendant programming, has become a key technology for reducing production barriers and improving changeover efficiency.

[0003] In existing technologies, inverse kinematics calculations are typically used to drive the robot's end effector to follow the operator's movements, or impedance control with fixed parameters and damping injection are used for safety protection. When the operator commands the robot's joint angle to exceed the physical limit, it is truncated to the limit value or the excess is converted into a virtual force, and deceleration is achieved by increasing damping. However, existing technologies have the following problems: the robot's intermediate joint posture is randomly determined by inverse kinematics, resulting in self-motion phenomena that do not conform to the operator's intentions; the impedance control parameters are fixed, which cannot adapt to different deceleration requirements and dynamic switching requirements at different speeds for different actions, making it difficult to maintain the continuity and naturalness of operation while ensuring safety. Summary of the Invention

[0004] The purpose of this invention is to provide an intelligent robot motion capture and control method and system based on industrial vision, so as to solve the problems of human-machine motion pattern separation and collision risk caused by the loss of control of intermediate joint posture of industrial robots in the prior art, and realize safe, efficient and natural human-machine collaborative operation of industrial robots in flexible production lines.

[0005] To solve the above-mentioned technical problems, the present invention is implemented using the following technical solution.

[0006] Industrial vision-based intelligent robot motion capture and control methods include:

[0007] Acquire the first pose change data of the end effector in space and the first pose change data of the intermediate joint in space when the operator performs the action, and form a visual perception acquisition sequence.

[0008] The first pose change data in the visual perception acquisition sequence is used as the main control variable. Based on the spatial mapping relationship between the end effector parts of the robot and the operator, the initial drive quantity of each drive joint of the robot is calculated to obtain the initial drive quantity sequence.

[0009] Collect posture change data of each intermediate joint of the robot during the movement of the robot following the operator, calculate the posture deviation by combining the first posture change data, obtain the posture deviation sequence, and construct the constraint coefficient that changes with time based on the posture deviation sequence and the movement speed data of each intermediate joint of the operator.

[0010] The constraint coefficients are applied to the initial drive sequence to control the robot to follow the pose changes of the operator's end effector and approach the pose of the corresponding intermediate joint.

[0011] Furthermore, the system acquires the first pose change data of the end effector in space and the first pose change data of the intermediate joints in space when the operator performs the action, forming a visual perception acquisition sequence, including:

[0012] During the operation, the infrared visual acquisition system, located at the end of the operator's body and at the intermediate joints, sequentially acquires motion data in time frames, with each time frame set to a fixed time interval. The infrared visual acquisition system includes infrared cameras arranged around the operator.

[0013] The motion data is parsed according to the corresponding data format, and the spatial coordinates and spatial orientation of the end effector are extracted to generate the numerical expression of the first pose change data. The spatial orientation of the intermediate joints is extracted to generate the numerical expression of the first pose change data.

[0014] The parsed first pose change data is aligned with the first pose change data and then merged to obtain a continuous visual perception acquisition sequence.

[0015] Furthermore, the first pose change data in the visual perception acquisition sequence is used as the main control variable. Based on the spatial mapping relationship between the end effectors of the robot and the operator, the initial drive quantities of each drive joint of the robot are calculated to obtain the initial drive quantity sequence, including:

[0016] Based on the spatial coordinates in the first pose change data of the current time frame and the relative distance between the robot workspace boundary and each axis, mapping parameters are generated. The mapping parameters include the axis mapping ratio and the orientation mapping matrix.

[0017] Based on the spatial mapping relationship between the robot's end effector and the operator's end effector, the spatial coordinates and spatial orientation in the first pose change data are used as input, and the desired pose change data of the robot's end effector is obtained by transforming according to the mapping parameters.

[0018] The desired pose change data is used as input. Based on the motion relationship between each drive joint and the end effector of the robot, the initial drive amount of each drive joint is calculated in combination with the drive joint position calculated in the previous time frame.

[0019] The initial drive quantities of each drive joint are arranged in sequence to form an initial drive quantity sequence corresponding to the time frame of the visual perception acquisition sequence.

[0020] Furthermore, based on the spatial mapping relationship between the robot's end effector and the operator's end effector, the spatial coordinates and orientation in the first pose change data are used as input, and the desired pose change data of the robot's end effector is obtained by transforming it according to the mapping parameters, including:

[0021] The axial mapping ratio in the mapping parameters is adjusted according to the relative distance in each axis, and the spatial coordinates in the first pose change data are decomposed into components in different axes. The expected spatial coordinates of the robot end effector in space are calculated by the axial mapping ratio.

[0022] The spatial orientation in the first pose change data is represented by a rotation matrix, and multiplied with the orientation mapping matrix to obtain the desired spatial orientation of the robot's end effector in space.

[0023] The desired spatial coordinates and desired spatial orientation of the robot's end effector are merged to generate desired pose change data.

[0024] Furthermore, the posture change data of each intermediate joint of the robot during its movement following the operator are collected. The posture deviation is calculated by combining this data with the initial posture change data to obtain a posture deviation sequence. Based on this posture deviation sequence and the movement velocity data of each intermediate joint of the operator, time-varying constraint coefficients are constructed, including:

[0025] During the robot's movement while following the operator, the posture change data of each intermediate joint of the robot is collected and time-aligned with the first posture change data in the same time frame of the visual perception acquisition sequence.

[0026] Deviation analysis is performed on the posture change data of each intermediate joint of the robot and the first posture change data to calculate the posture deviation amount and obtain the posture deviation sequence.

[0027] The baseline value of the traction amplitude is set based on the attitude deviation amount in the attitude deviation sequence, and the trend value of the attitude deviation is calculated by the attitude deviation amount of the current time frame and the previous time frame.

[0028] By extracting the motion velocity data of each intermediate joint from the first posture change data, the suppression ratio of the traction amplitude is calculated, and the traction amplitude adjustment direction and adjustment amplitude determined by the change trend value are combined to calculate the constraint coefficient that changes with time.

[0029] Furthermore, by extracting the motion velocity data of each intermediate joint from the first posture change data, the suppression ratio of the traction amplitude is calculated. Combined with the traction amplitude adjustment direction and adjustment amplitude determined by the trend value, the constraint coefficient that changes with time is calculated, including:

[0030] The direction of adjustment of the traction amplitude is determined by the sign of the trend value, and the adjustment amplitude of the traction amplitude is determined by exponential mapping of the absolute value of the trend value. The adjustment amplitude and the sign are combined to form a directional adjustment amount.

[0031] Set the output range of the suppression ratio, take the motion speed data of each intermediate joint of the operator as the input variable, establish the mapping relationship between motion speed data and suppression ratio, and form a dynamic suppression function;

[0032] The motion velocity data of each intermediate joint of the operator is input into the dynamic inhibition function for time-delay integration to obtain the inhibition ratio that changes smoothly over time.

[0033] Nonlinear calculations are performed based on the directional adjustment amount and the suppression ratio. The suppression ratio is used as the exponent of the directional adjustment amount, and the constraint coefficient is calculated in combination with the reference value of the traction amplitude.

[0034] Furthermore, the output range of the suppression ratio is set, and the motion velocity data of each intermediate joint of the operator is used as input variables to establish a mapping relationship between the motion velocity data and the suppression ratio, forming a dynamic suppression function, including:

[0035] Set the output value range of the suppression ratio, and map the zero value and preset maximum value of the movement speed data of each intermediate joint of the operator to the lower limit and upper limit of the output value range, respectively, to form an initial mapping relationship between the suppression ratio and the movement speed data;

[0036] During the continuous process of the robot following the operator's movement, the cumulative change of the operator's motion speed data of each intermediate joint over time is collected and used as a dynamic correction factor.

[0037] The initial mapping relationship is adjusted according to the dynamic correction factor, and the motion velocity data of the current time frame is used as input to establish a dynamic suppression function.

[0038] Furthermore, a nonlinear calculation is performed based on the directional adjustment amount and the suppression ratio. The suppression ratio is used as the exponent of the directional adjustment amount, and the constraint coefficient is calculated in conjunction with the baseline value of the traction amplitude, including:

[0039] The suppression ratio is nonlinearly adjusted based on the difference between the suppression ratio of the current time frame and the suppression ratio of the previous time frame to obtain the dynamic suppression index.

[0040] The constraint coefficient is obtained by exponentially calculating the baseline value of the traction amplitude, the dynamic inhibition index as the exponent of the directional adjustment amount, and the product of the directional adjustment amount and the dynamic inhibition index.

[0041] Furthermore, constraint coefficients are applied to the initial actuation sequence to control the robot to follow the pose changes of the operator's end effector and approach the pose of the corresponding intermediate joint, including:

[0042] Based on the actual posture of each intermediate joint of the robot in the current time frame and the distance to the corresponding physical limit, the decomposition ratio of the constraint coefficient on the driving channel of each intermediate joint is allocated to obtain the sub-constraint coefficients corresponding to each intermediate joint.

[0043] The motion velocity data is used as a modulation parameter to perform a nonlinear mapping on the sub-constraint coefficients, and the sub-constraint coefficients after velocity modulation are calculated.

[0044] The velocity-modulated sub-constraint coefficients are fused with the initial drive quantities of the corresponding intermediate joints in the initial drive quantity sequence. The corrected drive quantity is then calculated by combining the velocity-modulated sub-constraint coefficients of the adjacent joints of the intermediate joints in the current time frame.

[0045] The corrected drive quantities of each intermediate joint are combined with the initial drive quantities of the non-intermediate joints in the initial drive quantity sequence to form a corrected drive quantity sequence. The robot's drive joints are then controlled to perform actions based on the corrected drive quantity sequence.

[0046] An intelligent robot motion capture control system based on industrial vision includes a data acquisition module, a mapping module, a coefficient construction module, and a drive module.

[0047] The data acquisition module is used to acquire the first posture change data of the end effector in space and the first posture change data of the intermediate joint in space when the operator performs the action, forming a visual perception acquisition sequence.

[0048] The mapping module is used to take the first pose change data in the visual perception acquisition sequence as the main control quantity, and calculate the initial driving quantity of each drive joint of the robot according to the spatial mapping relationship between the end effector parts of the robot and the operator, so as to obtain the initial driving quantity sequence.

[0049] The coefficient construction module is used to collect the posture change data of each intermediate joint during the robot's movement following the operator, calculate the posture deviation by combining the first posture change data, obtain the posture deviation sequence, and construct the constraint coefficient that changes with time based on the posture deviation sequence and the movement speed data of each intermediate joint of the operator.

[0050] The drive module is used to apply constraint coefficients to the initial drive quantity sequence, control the robot to follow the pose changes of the operator's end effector, and approach the pose of the corresponding intermediate joint.

[0051] Compared with existing technologies, the beneficial effects achieved by this invention are as follows: By acquiring motion data of the operator's end effector and intermediate joints through an infrared vision acquisition system, a dual control reference system including end effector pose and intermediate joint posture is constructed, achieving collaborative optimization of the tracking accuracy of the industrial robot's end effector and the consistency of the intermediate joint posture; the posture deviation, deviation change trend, and motion speed data of the operator's intermediate joints are dynamically fused using multiple parameters to construct a constraint coefficient that varies with time, enabling adaptive adaptation to control requirements of different operating speeds and conditions in industrial production; the constraint coefficient is coupled with the initial drive quantity to form the final corrected drive quantity; this invention solves the problems of human-machine action pattern fragmentation and collision risks caused by uncontrolled intermediate joint posture in existing technologies. While ensuring that the robot's end effector accurately follows the pose changes of the operator's end effector, it continuously brings the robot's intermediate joint posture closer to the posture of the corresponding intermediate joint of the operator, eliminating industrial production safety hazards caused by uncontrolled intermediate joint posture, avoiding the disruption of robot motion continuity due to control abrupt changes, and significantly improving the naturalness and production efficiency of human-machine collaborative operations. It is applicable to flexible intelligent manufacturing production lines in the automotive, 3C electronics, aerospace, and new energy fields.

[0052] It should be understood that the above general description and the following detailed description are merely exemplary and explanatory, and are not intended to limit the technical solutions of this disclosure. Attached Figure Description

[0053] Figure 1 A flowchart of the intelligent robot motion capture and control method based on industrial vision provided by the present invention;

[0054] Figure 2 The spatial mapping relationship diagram provided by this invention;

[0055] Figure 3 The physical limiting diagram provided by the present invention;

[0056] Figure 4 The diagram shows the structure of the intelligent robot motion capture control system based on industrial vision provided by this invention. Detailed Implementation

[0057] The technical solution of the present invention will be described in detail below with reference to the accompanying drawings and specific embodiments. It should be understood that the embodiments of the present invention and the specific features in the embodiments are detailed descriptions of the technical solution of the present invention, rather than limitations thereof. In the absence of conflict, the embodiments of the present invention and the technical features in the embodiments can be combined with each other.

[0058] The term "and / or" simply describes the relationship between related objects, indicating that three relationships can exist. For example, A and / or B can represent: A alone, A and B simultaneously, or B alone. Additionally, the character " / " generally indicates that the preceding and following related objects have an "or" relationship.

[0059] Example 1:

[0060] Please see Figures 1-3 This invention provides an embodiment of an intelligent robot motion capture and control method based on industrial vision, which includes the following specific steps:

[0061] Step S1: Acquire the first pose change data of the end effector in space and the first pose change data of the intermediate joint in space when the operator performs the action, and form a visual perception acquisition sequence.

[0062] The specific steps of step S1 are as follows:

[0063] Step S101: During the operation of the operator, motion data is collected sequentially in time frames by an infrared vision acquisition system arranged at the end of the operator's body and intermediate joints. The time frames are set to a fixed time interval. The infrared vision acquisition system includes infrared cameras arranged around the operator.

[0064] In this embodiment, before the operator begins to perform an action, a marker component is fixed to the back of the operator's hand as a collection point for the end-effector. The marker component contains at least three non-coplanar marker points. Multiple marker points are fixed to the outer sides of the operator's elbow and shoulder joints as collection points for intermediate joints. Multiple infrared cameras are arranged around the operator and spatially calibrated so that the field of view of each camera covers the operator's range of motion. The data sampling time frame interval of the acquisition device is set to, for example, 0.01s. The acquisition device synchronously triggers all infrared cameras to perform exposure and shooting at this fixed time interval to obtain the two-dimensional pixel coordinates of each marker point in the infrared camera coordinate system. The three-dimensional coordinate data of each marker point in space is calculated through the triangulation relationship between the infrared cameras. All the three-dimensional coordinate data of the marker points collected in the same time frame are grouped and packaged according to the marker point number to form motion data for the corresponding time frame. The motion data of each time frame is output sequentially in chronological order.

[0065] Step S102: Parse the motion data according to the corresponding data format, extract the spatial coordinates and spatial orientation of the end effector to generate the numerical expression of the first pose change data, and extract the spatial orientation of the intermediate joints to generate the numerical expression of the first pose change data.

[0066] In this embodiment, the motion data of each time frame is parsed, and the three-dimensional coordinates of multiple marker points corresponding to the acquisition points of the end effector are extracted from the motion data of that time frame. The three-dimensional coordinates of the marker point on the back of the wrist are used as the spatial coordinates of the end effector. The vector pointing from the marker point on the back of the wrist to the marker point on the back of the hand is used as the first axis vector, and the vector pointing from the marker point on the back of the hand to the reference marker point in the finger direction is used as the second axis vector. The cross product of the first axis vector and the second axis vector is performed to obtain the third axis vector. The cross product of the first axis vector and the third axis vector is performed to correct the second axis vector. The three mutually orthogonal axis vectors are normalized to form a rotation matrix, which is then converted into a quaternion as the spatial orientation of the end effector. The spatial coordinates and spatial orientation are combined as the first pose change data. Value representation: Extract the three-dimensional coordinates of multiple marker points corresponding to the intermediate joint acquisition points from the motion data of this time frame. Perform vector difference operation on the three-dimensional coordinates of the marker points on the outer side of the elbow joint and the three-dimensional coordinates of the marker points on the outer side of the shoulder joint to obtain the upper arm direction vector. Perform vector difference operation on the three-dimensional coordinates of the forearm reference marker point and the three-dimensional coordinates of the marker points on the outer side of the elbow joint to obtain the forearm direction vector. Calculate the spatial orientation of the shoulder joint and the spatial orientation of the elbow joint using the upper arm direction vector and the forearm direction vector respectively. Combine the spatial orientation of the shoulder joint and the spatial orientation of the elbow joint as the numerical representation of the first posture change data of the intermediate joint. The numerical representation of the spatial coordinates of the end effector is, for example, (x, y, z), and the numerical representation of the spatial orientation is, for example, (qw, qx, qy, qz).

[0067] For example, assuming that in motion data acquired at any given time frame, the 3D spatial coordinates of the wrist dorsal marker point are calculated by an infrared camera to be (125.30, 340.50, 860.20), the 3D spatial coordinates of the hand dorsal marker point are (128.10, 342.80, 862.40), and the 3D spatial coordinates of the finger direction reference marker point are (131.50, 341.20, 864.10), performing a vector difference operation between the hand dorsal marker point coordinates and the wrist dorsal marker point coordinates yields the principal axis direction vector as (2.80, 2.30, 2.20), which, after normalization, yields the unit principal axis vector as (0.6123, 0.5034, 0.6098). Performing a vector difference operation between the finger direction reference marker point coordinates and the hand dorsal marker point coordinates yields the secondary axis direction vector as (3.40, -1). The coordinates (.60, 1.70) are normalized to obtain the unit sub-axis vector as (0.8512, -0.4006, 0.3387). The unit principal axis vector and the unit sub-axis vector are cross-multiplied to obtain the third axis unit vector as (0.5291, 0.3982, -0.7495). The unit principal axis vector is used as the first column of the rotation matrix, the unit sub-axis vector as the second column, and the third axis unit vector as the third column to construct the rotation matrix. Using the conversion formula from rotation matrix to quaternion, the spatial orientation quaternion of the end effector is calculated as (0.8521, 0.3824, -0.2513, 0.3012). This quaternion is combined with the spatial coordinates (125.30, 340.50, 860.20) of the wrist dorsal marker point to form the complete numerical expression of the first pose change data in this time frame. Simultaneously, the three-dimensional spatial coordinates of the lateral shoulder joint marker (110.20, 330.40, 870.30), the lateral elbow joint marker (95.30, 350.60, 860.80), and the forearm reference marker (85.10, 340.20, 855.40) were obtained from the motion data packet of this time frame. A vector difference operation was performed between the lateral elbow and shoulder joint marker coordinates to obtain the upper arm direction vector (-14.90, 20.20, -9.50). After normalization, this vector was combined with... The spatial orientation quaternion of the shoulder joint was calculated using the reference coordinate system as (0.6124, -0.3541, 0.5203, 0.4726). The vector difference operation was performed between the coordinates of the forearm reference marker and the coordinates of the lateral elbow marker to obtain the forearm direction vector as (-10.20, -10.40, -5.40). After normalization, the spatial orientation quaternion of the elbow joint was calculated as (0.9248, -0.1832, -0.2874, 0.1563). The spatial orientation of the shoulder joint and the spatial orientation of the elbow joint were combined as the first posture change data of the intermediate joint.

[0068] Step S103: Align the parsed first pose change data with the first pose change data and merge them to obtain a continuous visual perception acquisition sequence.

[0069] In this embodiment, the first pose change data and the first pose change data obtained from parsing in each time frame are matched, and a correspondence is established with the timestamp as the index. For data frames with slight timestamp offsets due to sensor response delay, the data of adjacent frames are interpolated by linear interpolation to complete the first pose change data or first pose change data corresponding to the missing timestamp, so that the first pose change data and the first pose change data exist simultaneously under each timestamp. The first pose change data and the first pose change data of all the completed time frames are arranged in order to form a continuous visual perception acquisition sequence with timestamp as the index, in which each time frame simultaneously contains the spatial coordinates and spatial orientation of the end effector and the spatial orientation of the intermediate joint.

[0070] Step S2: Take the first pose change data in the visual perception acquisition sequence as the main control variable, and calculate the initial drive quantity of each drive joint of the robot according to the spatial mapping relationship between the end effector parts of the robot and the operator to obtain the initial drive quantity sequence.

[0071] The specific steps of step S2 are as follows:

[0072] Step S201: Generate mapping parameters based on the spatial coordinates in the first pose change data of the current time frame and the relative distances between the robot workspace boundary and each axis. The mapping parameters include the axis mapping ratio and the orientation mapping matrix.

[0073] In this embodiment, the first pose change data of the current time frame is extracted from the visual perception acquisition sequence to obtain the spatial coordinates of the end effector. These spatial coordinate values ​​are denoted as x_end, y_end, and z_end, respectively. The robot workspace boundary is set, which is composed of the minimum and maximum boundary values ​​in the x-axis, y-axis, and z-axis directions in Cartesian coordinates. The distances between the spatial coordinates and the minimum and maximum boundary values ​​in the x-axis direction are calculated, and the smaller of these two distances is taken as the boundary approach distance in the x-axis direction. The boundary approach distances in the y-axis and z-axis directions are calculated using the same method. These boundary approach distances are then input into the boundary response function to obtain x_end, y_end, and z_end, respectively. The axial mapping ratio of the x-axis, y-axis and z-axis is used to represent the spatial orientation in the first pose change data of the current time frame using a rotation matrix. The spatial orientation rotation matrix of the robot end effector in the previous time frame is obtained. The orientation deviation rotation matrix from the rotation matrix of the previous time frame to the rotation matrix of the current time frame is calculated. The orientation fusion coefficient is calculated based on the minimum boundary approach distance. When the minimum boundary approach distance is greater than or equal to the distance threshold, the orientation fusion coefficient is 1. When the minimum boundary approach distance is less than the distance threshold, the orientation fusion coefficient is equal to the minimum boundary approach distance divided by the distance threshold. The weighted sum of the orientation fusion coefficient and the identity matrix is ​​calculated. The weighted sum of 1 minus the orientation fusion coefficient and the orientation deviation rotation matrix is ​​added together to obtain the orientation mapping matrix. The orientation mapping matrix is ​​made to approach the identity matrix when the operator approaches the boundary of the workspace to suppress orientation following.It should be noted that the boundary response function is constructed based on the boundary approach distance, using the boundary approach distance as the input variable and the axial mapping ratio as the output variable. When the boundary approach distance is greater than a distance threshold, the output value is 1. When the boundary approach distance is less than the distance threshold, the output value decreases as the boundary approach distance decreases. In the linear decreasing mode, the output variable equals the input variable divided by the distance threshold. In the non-linear decreasing mode, the output variable equals the input variable divided by the square of the distance threshold, or a sinusoidal curve transition is used to gradually accelerate or decelerate the decrease of the output variable as it approaches the boundary, thus adjusting the robot's deceleration characteristics when approaching the boundary. The distance threshold is determined based on the maximum permissible motion speed and braking deceleration of the robot's end effector. The theoretical significance lies in reserving sufficient braking distance for the robot, enabling it to decelerate and stop before touching the workspace boundary. The braking distance of the robot's end effector at its maximum speed is obtained. This braking distance is calculated by dividing the square of the maximum speed by twice the maximum deceleration. Multiplying this braking distance by a safety factor yields the distance threshold value. The maximum speed is typically set to 250mm / s to 500mm / s, and the maximum deceleration is typically set to 1000mm / s² to 2000mm / s². The calculated braking distance is approximately 31.25mm to 125mm. After multiplying by the safety factor, the distance threshold is typically in the range of 50mm to 150mm. The safety factor is between 1.2 and 1.5.

[0074] Step S202: Based on the spatial mapping relationship between the robot end effector and the operator end effector, the spatial coordinates and spatial orientation in the first pose change data are used as input, and the desired pose change data of the robot end effector is obtained by transforming according to the mapping parameters.

[0075] The specific steps of step S202 are as follows:

[0076] Step S2021: Adjust the axial mapping ratio in the mapping parameters according to the relative distance in each axis, decompose the spatial coordinates in the first pose change data into components in different axes, and calculate the expected spatial coordinates of the robot end effector in space through the axial mapping ratio.

[0077] In this embodiment, the axial mapping ratios, including the x-axis mapping ratio, y-axis mapping ratio, and z-axis mapping ratio, are extracted from the mapping parameters. The spatial coordinates of the end effector are extracted from the first pose change data of the current time frame. The values ​​of the spatial coordinates on the x-axis of the spatial rectangular coordinate system are extracted as x-axis components, the values ​​on the y-axis are extracted as y-axis components, and the values ​​on the z-axis are extracted as z-axis components. The x-axis components are multiplied by the x-axis mapping ratio to obtain the expected x-axis components of the robot end effector in the desired spatial coordinates. The expected y-axis components and z-axis components are calculated using the same method. The expected x-axis, y-axis, and z-axis components are combined according to the spatial coordinate format to form the desired spatial coordinates of the robot end effector in space.

[0078] exist Figure 2 In the diagram, the rectangle on the left represents the operator's workspace, and the rectangle on the right represents the robot's workspace. Both are located in a spatial coordinate system. The operator's workspace includes a simplified human figure and a schematic diagram of the motion trajectory of the end effector. The robot's workspace includes a simplified robot figure and a schematic diagram of the motion trajectory of the end effector.

[0079] Step S2022: Represent the spatial orientation in the first pose change data using a rotation matrix, and multiply it with the orientation mapping matrix to obtain the desired spatial orientation of the robot's end effector in space.

[0080] In this embodiment, the spatial orientation of the end effector is extracted from the first pose change data of the current time frame. The spatial orientation is converted into a rotation matrix in three-dimensional space and denoted as the original rotation matrix of the operator's end effector in this time frame. The orientation mapping matrix is ​​extracted from the mapping parameters. The original rotation matrix and the orientation mapping matrix are multiplied by matrix multiplication. Specifically, the product of the orientation mapping matrix and the original rotation matrix is ​​calculated to obtain the rotation matrix expression of the desired spatial orientation of the robot's end effector in space. The rotation matrix is ​​converted into an attitude expression and used as the desired spatial orientation of the robot's end effector in space.

[0081] Step S2023: Merge the desired spatial coordinates and desired spatial orientation of the robot's end effector in space to generate desired pose change data.

[0082] In this embodiment, the desired spatial coordinates and desired spatial orientation are merged according to a data format to obtain a complete desired pose data unit. For example, the robot workspace boundary is set to 0mm to 1000mm in the x-axis direction, 0mm to 1000mm in the y-axis direction, and 0mm to 1000mm in the z-axis direction. The spatial coordinates in the first pose change data acquired in the current time frame are (950mm, 500mm, 400mm). If the boundary distance of the operator's end effector in the x-axis direction is close to 50mm, which is less than the distance threshold of 100mm, then x... The axial mapping ratio is adjusted to 0.5. If the operator's end effector is within 500mm of the upper boundary of the y-axis, which is greater than the distance threshold, the y-axis mapping ratio is 1.0. If the operator's end effector is within 400mm of the upper boundary of the z-axis, which is greater than the distance threshold of 100mm, the z-axis mapping ratio is 1.0. Multiplying the x-axis component 950mm by the x-axis mapping ratio 0.5 yields the desired x-axis component 475mm. Multiplying the y-axis component 500mm by the y-axis mapping ratio 1.0 yields the desired y-axis component 500mm. Multiplying the z-axis component 400mm by the z-axis... Multiplying by a mapping ratio of 1.0 yields a desired z-axis component of 400mm. This gives the desired spatial coordinates of the robot's end effector as (475mm, 500mm, 400mm). The spatial orientation of the operator's end effector in the current time frame is converted into a rotation matrix, denoted as the original rotation matrix. The orientation fusion coefficient, calculated based on the boundary proximity distance, is 0.5. The orientation mapping matrix is ​​calculated as a weighted sum of the identity matrix and the orientation deviation rotation matrix. Assuming the orientation deviation rotation matrix is ​​calculated as a rotation matrix rotating 30 degrees around the z-axis, the orientation mapping matrix is ​​0.5 times the identity matrix plus 0.5. The rotation matrix is ​​doubled by 30 degrees around the z-axis. The original rotation matrix is ​​multiplied by the orientation mapping matrix to obtain the rotation matrix of the desired spatial orientation. After transformation, the corresponding posture expression is obtained. The desired spatial coordinates (475mm, 500mm, 400mm) and the desired spatial orientation are merged to form the desired pose change data of the current time frame. When the operator's end effector approaches the boundary of the workspace, the system will reduce the following amplitude in the x-axis, so that the movement of the robot end effector in the x-axis is reduced to half of the movement of the operator. At the same time, the orientation following force is also weakened accordingly, thereby avoiding the collision between the robot end effector and the boundary of the workspace.

[0083] Step S203: Take the desired pose change data as input, and calculate the initial drive amount of each drive joint based on the motion relationship between each drive joint and the end effector of the robot, combined with the drive joint position calculated in the previous time frame.

[0084] In this embodiment, the desired spatial coordinates and desired spatial orientation in the desired pose change data are combined to obtain the desired pose in the current time frame. Geometric parameters of each drive joint of the robot are obtained, including but not limited to the joint type of each drive joint, the link length between adjacent drive joints, the link torsion angle, and the joint offset. Based on the geometric parameters, a kinematic chain relationship is established from the robot base coordinate system through each drive joint to the end effector. The drive quantity of each drive joint calculated in the previous time frame is used as the initial value for the iteration in the current time frame. The desired pose in the current time frame is used as the solution target. The drive quantity of each drive joint is solved through numerical iteration. During the iteration process, based on the drive quantity of the drive joint in the current iteration, the kinematic chain is further refined. The current calculated pose of the end effector in space is calculated using the kinematic chain relationship. The pose error between the current calculated pose and the desired pose is calculated, where the pose error includes position error and attitude error. Based on the pose error, the correction amount of the driving amount of each drive joint is calculated using the Jacobian matrix. The correction amount is added to the driving amount of the drive joint in the current iteration to form a new driving amount of the drive joint. The iteration is repeated until the pose error is less than a preset convergence threshold. The driving amount of each drive joint obtained in the final iteration is used as the initial driving amount of each drive joint in the current time frame. The convergence threshold is set according to the physical accuracy of the robot body and the application scenario. For example, in scenarios such as handling and grasping, the accuracy requirements are relatively relaxed, and it is set to 0.01mm and 0.001rad.

[0085] For example, suppose the robot consists of two rotary drive joints connected in series. The rotation axis of the first drive joint is vertical, and the rotation axis of the second drive joint is horizontal. The link length between the first and second drive joints is 300mm, and the link length between the second drive joint and the end effector is 200mm. The desired pose data in the current time frame is: desired spatial coordinates (350mm, 0mm, 200mm), with the desired spatial orientation being the end effector pointing directly downwards. The drive amount of the first drive joint calculated in the previous time frame is 0.524rad, and the drive amount of the second drive joint is 0.785rad. Using the drive amounts (0.524rad, 0.785rad) from the previous time frame as the initial values ​​for the current time frame iteration, in the first iteration, the current calculated pose of the end effector in space is calculated based on the initial drive amounts using a kinematic chain relationship. The first drive joint rotates by 0.524rad, causing the link to deflect; the second drive joint rotates by 0.785rad, causing the end effector to swing in the vertical plane. The calculated current spatial coordinates are: For simplification, the z-coordinate is 0 and the rotation axis direction is not considered in the calculation. Normally, the z-coordinate is not 0. Compared with the expected spatial coordinates (350mm, 0mm, 200mm), there is an x-axis deviation of 38.4mm, a y-axis deviation of -343.2mm, and a z-axis deviation of 200mm. The Jacobian matrix is ​​calculated based on the pose error and the correction amount is solved. The correction amount of the first drive joint is 0.087rad, and the correction amount of the second drive joint is -0.174rad. After superposition, the new drive joint drive amount is (0.611rad, 0.611rad). Repeat the iteration until the fourth iteration. When the pose error is less than the preset convergence thresholds of 0.01mm and 0.001rad, the initial drive amount of each drive joint in the current time frame is 0.698rad for the first drive joint and 0.698rad for the second drive joint.

[0086] Step S204: Arrange the initial drive quantities of each drive joint in sequence to form an initial drive quantity sequence corresponding to the time frame of the visual perception acquisition sequence.

[0087] In this embodiment, the initial drive quantities of each drive joint in the current time frame are arranged in order to form the initial drive quantity vector of the time frame. The initial drive quantity vector is associated with the timestamp of the time frame and stored. For each time frame in the visual perception acquisition sequence, the initial drive quantity calculation process is repeated to obtain the initial drive quantity vector corresponding to each time frame. The initial drive quantity vectors of all time frames are arranged in order to form the initial drive quantity sequence. Each element in the initial drive quantity sequence contains the initial drive quantity value of each drive joint in the time frame.

[0088] Step S3: Collect the posture change data of each intermediate joint during the robot's movement following the operator, calculate the posture deviation by combining the first posture change data, obtain the posture deviation sequence, and construct the constraint coefficient that changes with time based on the posture deviation sequence and the movement speed data of each intermediate joint of the operator.

[0089] The specific steps of step S3 are as follows:

[0090] Step S301: During the process of the robot following the operator's movement, collect the posture change data of each intermediate joint of the robot, and align it with the first posture change data in the same time frame in the visual perception acquisition sequence.

[0091] In this embodiment, during the robot's movement following the operator, encoder feedback data of each intermediate joint of the robot is collected at the same time frame interval as the visual perception acquisition sequence. The encoder feedback data is converted into the actual spatial orientation of each intermediate joint in the robot's base coordinate system, thus obtaining the posture change data of each intermediate joint. Each posture change data includes the spatial orientation quaternion of the intermediate joint in that time frame. The first posture change data of each time frame in the visual perception acquisition sequence is read. For each time frame, the acquired posture change data of each intermediate joint of the robot is established with the corresponding first posture change data, so that the two types of data in the same time frame are aligned on the time axis. This ensures that the posture change data of the robot's intermediate joints and the first posture change data exist simultaneously in each time frame, forming a time-aligned data pair.

[0092] Step S302: Perform deviation analysis on the posture change data of each intermediate joint of the robot and the first posture change data, calculate the posture deviation amount, and obtain the posture deviation sequence.

[0093] In this embodiment, the spatial orientation of the robot's intermediate joint posture change data and the first posture change data are represented by quaternions. The relative rotation quaternion required to rotate from the current spatial orientation of the robot's intermediate joint to the spatial orientation of the operator's corresponding intermediate joint is calculated. The calculation method is to first calculate the conjugate quaternion of the robot's spatial orientation quaternion, and then perform a quaternion multiplication operation with the operator's spatial orientation quaternion. The relative rotation quaternion is normalized so that its sum of squares equals 1. The normalized relative rotation quaternion is converted into an axis-angle representation. The rotation angle is extracted as the posture deviation of the intermediate joint in the current time frame. The rotation angle is calculated by taking the inverse cosine value of the real part of the relative rotation quaternion and multiplying it by 2. The value range of the rotation angle is set to, for example, 0 degrees to 180 degrees. When the absolute value of the real part of the relative rotation quaternion is greater than 1, it is truncated to the interval [-1, 1] before performing the inverse cosine operation. The posture deviation of each intermediate joint in each time frame is arranged in order to form a posture deviation sequence.

[0094] Step S303: Set the baseline value of the traction amplitude according to the attitude deviation amount in the attitude deviation sequence, and calculate the trend value of the attitude deviation change through the attitude deviation amount of the current time frame and the previous time frame.

[0095] In this embodiment, the attitude deviation of each intermediate joint in the current time frame is obtained from the attitude deviation sequence. The value of the attitude deviation is used as the reference value of the traction amplitude, and the reference value increases linearly with the increase of the attitude deviation. The attitude deviation of the current time frame and the attitude deviation of the previous time frame are obtained. The difference between the attitude deviation of the current time frame and the attitude deviation of the previous time frame is calculated. This difference is used as the change in attitude deviation. The change in attitude deviation is divided by the time interval between adjacent time frames to obtain the rate of change of attitude deviation. This rate of change is used as the trend value of attitude deviation. When the trend value of attitude deviation is positive, it indicates that the attitude deviation is increasing. When it is negative, it indicates that the attitude deviation is decreasing.

[0096] For example, assuming the robot has an intermediate joint, the time interval between adjacent time frames is 0.02s, and the attitude deviation of the current time frame obtained from the attitude deviation sequence is 15... The attitude deviation in the previous time frame was 10. Therefore, the baseline value for the traction amplitude is set to 15. The change in attitude deviation is calculated to be 15. Subtract 10 equals 5 The trend value of the attitude deviation is 5. Divide by 0.02s, equals 250 That is, 4.38 rad / s, if the attitude deviation in the current time frame is 8 The previous time frame was 12. The baseline value is 8. The change is The trend value is =3.50 rad / s, where the positive or negative sign of the trend value indicates whether the attitude deviation tends to increase or decrease, and its absolute value reflects the rate of change of the deviation. The traction amplitude is dynamically adjusted by the trend value. When the deviation increases, the traction force is increased to suppress the deviation divergence, and when the deviation decreases, the traction force is weakened to avoid over-adjustment.

[0097] Step S304: By extracting the motion velocity data of each intermediate joint in the first posture change data, calculate the suppression ratio of the traction amplitude, and combine the traction amplitude adjustment direction and adjustment amplitude determined by the change trend value to calculate the constraint coefficient that changes with time.

[0098] The specific steps of step S304 are as follows:

[0099] Step S3041: Determine the adjustment direction of the traction amplitude based on the sign of the trend value, perform exponential mapping on the absolute value of the trend value to determine the adjustment amplitude of the traction amplitude, and combine the adjustment amplitude with the sign to form a directional adjustment amount.

[0100] In this embodiment, the trend value of the attitude deviation in the current time frame is obtained, and the sign of the trend value is determined. If the trend value is greater than zero, the adjustment direction of the traction amplitude is determined to be positive; if the trend value is less than zero, the adjustment direction of the traction amplitude is determined to be negative; if the trend value is equal to zero, the adjustment direction is zero. The absolute value of the trend value is taken as the input of the exponential mapping function, where the exponential mapping function uses the natural constant. Using the absolute value of the trend value as the base, the product of the absolute value of the trend value and the scaling factor is used as the exponent for power operation, and then a constant of 1 is subtracted to obtain the mapped adjustment range value. This ensures that the adjustment range increases exponentially with the increase of the absolute value of the trend value. The adjustment direction and adjustment range are combined into a directional adjustment amount. The combination method is as follows: when the adjustment direction is positive, the directional adjustment amount takes a positive value of the adjustment range; when the adjustment direction is negative, the directional adjustment amount takes a negative value of the adjustment range; and when the adjustment direction is zero, the directional adjustment amount takes a zero value. The scaling factor is determined based on the maximum response angular velocity of the robot's intermediate joints and the desired sensitivity setting of the robot control system. It is calculated by multiplying the reciprocal of the maximum response angular velocity by a sensitivity adjustment factor. The range of the scaling factor is, for example, within... to Between these parameters, if a more sensitive response to changes in attitude deviation is desired, i.e., to suppress the expansion of deviation more quickly, a smaller value, such as 0.01, should be chosen. If a smoother system and to avoid overshoot are desired, a larger value, such as 0.05, should be chosen. The sensitivity adjustment factor is set according to the physical response bandwidth of the robot's intermediate joints and the sampling interval of the control system. If the maximum angular acceleration of the robot joint actuator is high and the control cycle is short, the sensitivity adjustment factor can be reduced. If the robot joint has significant transmission backlash or flexibility, or if the operating environment requires smooth movements to avoid impact, the sensitivity adjustment factor should be increased.

[0101] For example, assume the scaling factor is... The trend value of the attitude deviation in the current time frame is 250. Since this value is greater than zero, the adjustment direction is determined to be positive, and the absolute value of the trend value is taken as 250. To calculate the exponential mapping, first calculate the product of the absolute value of the trend value and the preset scaling factor. Then, using the natural constant The result is obtained by exponentiation with 5 as the base. Subtracting the constant 1 yields an adjustment range of approximately 147.41, where the adjustment range is a dimensionless value, representing the change in attitude deviation at a trend value of 250. In this case, the amplification factor of the traction amplitude is 147.41 times. Combining the positive and adjustment amplitudes, the directional adjustment amount is +147.41. If the trend value is... If the direction of adjustment is negative, the absolute value is... The product is , Subtracting 1 gives 53.60, and the directional adjustment amount is -53.60. The sign of this directional adjustment amount determines the direction of increase or decrease in the traction amplitude, and its value determines the intensity of the traction amplitude adjustment. When the attitude deviation increases rapidly, the directional adjustment amount increases exponentially, thereby enhancing the traction force to suppress deviation divergence.

[0102] Step S3042: Set the output range of the suppression ratio, take the motion speed data of each intermediate joint of the operator as the input variable, establish the mapping relationship between the motion speed data and the suppression ratio, and form a dynamic suppression function.

[0103] The specific steps of step S3042 are as follows:

[0104] Step S30421: Set the output value range of the suppression ratio, and map the zero value and preset maximum value of the motion speed data of each intermediate joint of the operator to the lower limit and upper limit of the output value range, respectively, to form an initial mapping relationship between the suppression ratio and the motion speed data.

[0105] In this embodiment, the lower limit of the output value range of the suppression ratio is set to, for example, 0.0, and the upper limit is set to, for example, 1.0, where 0.0 represents no suppression and 1.0 represents complete suppression. This ensures that the suppression ratio is always a dimensionless normalized coefficient. Based on the dynamic response characteristics of human motion and the robot control system, the maximum value of the motion speed data of each intermediate joint of the operator is set, where the maximum value represents the maximum motion speed that may occur during actual operation. Speed ​​values ​​exceeding the maximum motion speed are treated as the maximum value. A linear mapping function from motion speed data to the suppression ratio is established. When the motion speed data is zero, the mapping... The output is the lower limit of the output value range, 0.0. When the motion speed data reaches the maximum value, the mapped output is the upper limit of the output value range, 1.0. When the motion speed data is between zero and the maximum value, the suppression ratio is calculated by linear interpolation. That is, the suppression ratio is equal to the product of the motion speed data divided by the maximum value and the width of the output value range. Since the lower limit is 0, the upper limit is 1, and the width is 1, the suppression ratio is equal to the ratio of the motion speed data to the maximum value. The calculation relationship is recorded as the initial mapping relationship. This initial mapping relationship makes the suppression ratio monotonically increase from 0.0 to 1.0 as the motion speed data increases.

[0106] For example, the maximum value of the operator's intermediate joint motion velocity data is set to 500, and the output value range is a lower limit of 0.0 and an upper limit of 1.0. When the collected operator's intermediate joint motion velocity data is 0, the suppression ratio is calculated through the initial mapping relationship. 0 divided by 500 equals 0.0, which is mapped to the lower limit of 0.0. When the motion velocity data is 250, the suppression ratio is... The mapping output is 0.5. When the motion speed data is 500, the suppression ratio is... The mapping output is 1.0. When the motion speed data exceeds the maximum value, it is processed according to the maximum value. The suppression ratio is 1.0. The initial mapping relationship ensures that the suppression ratio is small when the operator's motion speed is low, and the traction amplitude is basically unsuppressed. When the operator's motion speed is high, the suppression ratio increases, thereby weakening the traction amplitude and avoiding drastic correction of the robot's intermediate joint posture due to rapid movements.

[0107] Step S30422: During the continuous process of the robot following the operator's movement, the cumulative change of the movement speed data of each intermediate joint of the operator in the time dimension is collected and used as a dynamic correction factor.

[0108] In this embodiment, during the continuous process of the robot following the operator's movement, motion speed data of each intermediate joint of the operator is continuously collected at fixed time intervals. The fixed time interval is set according to the output frequency of the robot motion capture system and the frequency of the robot joint servo driver. A motion speed value is obtained in each time frame. The time window length is set, and the time window covers the motion speed data of the most recent several frames. If the time window length is 0.2 seconds and the fixed time interval is 0.01 seconds, then the time window covers historical data of 20 time frames. The motion speed data of all time frames within the time window is extracted, and the cumulative change in motion speed data relative to the starting frame of the time window is calculated. Specifically, the speed difference between two adjacent frames within the time window is accumulated to obtain the algebraic sum of the speed change. The absolute value of the cumulative change is used as a dynamic correction factor, where the dynamic correction factor reflects the severity of the speed fluctuation during the operator's recent movement. The more severe the fluctuation, the larger the dynamic correction factor.

[0109] Step S30423: Adjust the initial mapping relationship according to the dynamic correction factor, and use the motion velocity data of the current time frame as input to establish a dynamic suppression function.

[0110] In this embodiment, a dynamic correction factor is used as an adjustment parameter to modify the initial mapping relationship. The maximum value in the initial mapping relationship is replaced with the sum of the maximum value and the dynamic correction factor, so that the maximum value increases adaptively with the severity of speed fluctuations. At the same time, the upper limit of the output value range is adjusted to a weighted combination of the original upper limit and the dynamic correction factor, so that the upper limit is appropriately reduced when the fluctuations are severe. After modification, a dynamic mapping relationship, i.e., a dynamic suppression function, is obtained. The input of the dynamic suppression function is the motion speed data of the current time frame, and the output is the modified suppression ratio. The motion speed data of the current time frame is input into the dynamic suppression function to calculate the suppression ratio under the current time frame. When the operator's motion speed fluctuates drastically, the suppression ratio is smaller than the value output by the initial mapping relationship, thereby weakening the suppression of the traction amplitude and preventing the loss of attitude following ability due to excessive weakening of traction caused by brief speed changes. Among them, the dynamic correction factor is divided by the physical upper limit of the dynamic correction factor to obtain the normalized ratio. The normalized ratio reflects the proportion of the severity of the current speed fluctuation relative to the maximum allowable fluctuation. The normalized ratio is used as the weight of the dynamic correction factor, and one minus the normalized ratio is used as the weight of the original upper limit value.

[0111] Step S3043: Input the motion velocity data of each intermediate joint of the operator into the dynamic suppression function for time delay integration to obtain the suppression ratio that changes smoothly with time.

[0112] In this embodiment, the motion velocity data of each intermediate joint of the operator is used as the input to the dynamic suppression function. The dynamic suppression function internally maintains a time-delay accumulation, initially set to zero. In each time frame, based on the motion velocity data of the current time frame, the instantaneous suppression ratio is calculated using the dynamic suppression function. The time-delay accumulation of the previous time frame is multiplied by an attenuation coefficient to obtain the attenuated accumulation. The attenuation coefficient is a constant greater than 0 and less than 1, causing the historical accumulation to decay over time. The instantaneous suppression ratio of the current time frame is then weighted and summed with the attenuated accumulation. The weighting coefficient is determined based on the time constant, so that the weight of the instantaneous suppression ratio decreases as the time constant increases, while the weight of the attenuated accumulation increases as the time constant increases. The result of the weighted summation is used as the updated value of the time-delay accumulation for the current time frame. The time-delay accumulation of the current time frame is then input to the nonlinear... The smoothing function, or nonlinear smoothing function, limits the accumulated time delay and outputs it as a suppression ratio, making the change trajectory of the suppression ratio a continuous and smooth curve on the time axis. This avoids sudden changes in the suppression ratio caused by instantaneous jumps in motion speed data. It should be noted that the robot's intermediate joints are usually driven by motors and transmitted through reducers, and have a large moment of inertia. When the operator's motion speed changes abruptly, the robot cannot respond to high-frequency changes instantaneously. The attenuation coefficient reflects the degree to which the system retains historical speed information. Its value is negatively correlated with the magnitude of the robot joint's inertia. The larger the inertia, the slower the robot response, and the more historical information needs to be retained to smooth the command. Therefore, the attenuation coefficient should be set larger. The smaller the inertia, the faster the robot response, and the smaller the attenuation coefficient should be set. The time constant is a physical quantity that describes the speed of the system response and is usually set between 0.05 seconds and 0.5 seconds.

[0113] Step S3044: Perform nonlinear calculations based on the directional adjustment amount and the suppression ratio, use the suppression ratio as the exponent of the directional adjustment amount, and calculate the constraint coefficient in combination with the reference value of the traction amplitude.

[0114] The specific steps of step S3044 are as follows:

[0115] Step S30441: Based on the difference between the suppression ratio of the current time frame and the suppression ratio of the previous time frame, the suppression ratio is nonlinearly adjusted to obtain the dynamic suppression index.

[0116] In this embodiment, the suppression ratio calculated in the current time frame and the suppression ratio in the previous time frame are obtained, and the difference between the two is calculated to obtain the change difference of the suppression ratio. The change difference is input into a nonlinear transformation function. The nonlinear transformation function takes the absolute value of the change difference as the independent variable and maps it through a hyperbolic tangent function to obtain the change characteristic value. The change characteristic value increases approximately linearly when the absolute value of the change difference is small and tends to saturate when the absolute value of the change difference is large. The sign direction of the change characteristic value is determined according to the positive or negative sign of the change difference. The signed change characteristic value is multiplied by a sensitivity coefficient to obtain the adjustment bias. The sensitivity coefficient is determined through experimental debugging and its value is usually between 0 and 1. Specifically, during the process of the robot following the operator's movement, by testing the smoothness of the change of the constraint coefficient under different sensitivity coefficient values, and the response speed of the robot's intermediate joint posture to the operator's posture changes, the optimal value that balances the two is selected. Smaller values ​​are used for joints with slower response speeds to suppress oscillations, and larger values ​​are used for joints with faster response speeds to enhance the sensitivity to changes in traction force. The suppression ratio of the current time frame is added to the adjustment bias to obtain the preliminary adjustment value. The preliminary adjustment value is input into the compression function, such as the sigmoid function, ensuring that the function is monotonically increasing and the value range is (0,1). The compressed output value is used as the dynamic suppression index.

[0117] Step S30442: The constraint coefficient is obtained by weighting the product of the reference value of the traction amplitude, the dynamic inhibition index as the exponent of the directional adjustment amount, and the product of the directional adjustment amount and the dynamic inhibition index.

[0118] In this embodiment, the dynamic suppression index is used as the exponent of the directional adjustment amount and a power operation is performed to calculate the dynamic suppression index power of the directional adjustment amount to obtain the first intermediate result. The directional adjustment amount is multiplied by the dynamic suppression index to obtain the second intermediate result. Weighting coefficients are set for the first intermediate result and the second intermediate result respectively. The weighting coefficient of the first intermediate result corresponds to the power operation result of the directional adjustment amount, which has a nonlinear amplification effect when the attitude deviation changes rapidly, and is used to enhance the rapid traction response under sudden deviation. The weighting coefficient of the second intermediate result corresponds to the product of the directional adjustment amount and the dynamic suppression index, and is used to provide linear deviation change rate compensation. The weighting coefficient corresponding to the reference value of the traction amplitude provides the basic traction force based on the current deviation amount. The first intermediate result, the second intermediate result and the reference value of the traction amplitude are multiplied by their respective weighting coefficients and then summed to obtain a weighted sum. This weighted sum is output as the constraint coefficient in the current time frame.

[0119] Step S4: Apply constraint coefficients to the initial drive sequence to control the robot to follow the pose changes of the operator's end effector and approach the pose of the corresponding intermediate joint.

[0120] The specific steps of step S4 are as follows:

[0121] Step S401: Based on the actual posture of each intermediate joint of the robot in the current time frame and the distance to the corresponding physical limit, allocate the decomposition ratio of the constraint coefficient on the drive channel of each intermediate joint to obtain the sub-constraint coefficient corresponding to each intermediate joint.

[0122] In this embodiment, for each intermediate joint, the actual attitude angle value and physical limit parameters of its current time frame are obtained. The physical limit parameters include, but are not limited to, the minimum allowable angle value and the maximum allowable angle value. A first difference between the actual attitude angle value and the minimum allowable angle value is calculated, and a second difference between the maximum allowable angle value and the actual attitude angle value is calculated. The smaller of the first difference and the second difference is taken as the approach distance between the intermediate joint and the physical limit in the current time frame. A safety distance is set according to the physical braking response time of the robot joint, the minimum reaction delay of the control system, and the dynamic speed adjustment capability of the joint actuator. For example, the safety distance is set to 5% to 15% of the total allowable range of motion of the robot joint. The decomposition ratio of the intermediate joint is set to the maximum value of 1.0. When the approach distance is less than the safety distance, the ratio of the approach distance to the safety distance is used as the basic decomposition ratio, so that the basic decomposition ratio decreases linearly to 0 as the approach distance decreases. The input is fed into a smooth transition function, which uses the basic decomposition ratio as the independent variable and performs a nonlinear transformation in the form of a cubic operation to obtain the decomposition ratio of the intermediate joint. This results in the decomposition ratio exhibiting a curve characteristic of first accelerating and then slowly decreasing as the approach distance gradually decreases from the safety distance threshold to 0. The constraint coefficient is multiplied by the decomposition ratio of the intermediate joint to obtain the sub-constraint coefficient of the intermediate joint. The sub-constraint coefficient decreases as the joint approaches the physical limit, thereby weakening the traction force on the joint. The smooth transition function is constructed, for example, in the form of a power function. When the approach distance equals the safety distance, the function output is 1 and remains continuous of the first order. When the approach distance approaches 0, the function output approaches 0 and the rate of change gradually decreases, making the attenuation process exhibit a characteristic of being rapid at first and then slow. This ensures that the traction force is not affected within the safety distance range, and that it attenuates rapidly when approaching the limit to prevent impact with the limit, while ensuring the smoothness of the attenuation process to avoid oscillations in the control system.

[0123] exist Figure 3 In the design, the physical limit boundaries of the robot's intermediate joints consist of an upper limit and a lower limit. The circular icon in the middle indicates the actual posture position, while the circular icon closer to the upper limit indicates the safety distance.

[0124] Step S402: Use the motion speed data as a modulation parameter to perform nonlinear mapping on the sub-constraint coefficients, and calculate the sub-constraint coefficients after speed modulation.

[0125] In this embodiment, the real-time motion velocity of the intermediate joint in the current time frame is obtained, which is expressed as the rate of change of the joint angle over time. A velocity modulation function is constructed based on the velocity response characteristics of the robot joint actuator and the dynamic adjustment capability of the control system. This velocity modulation function is a gain scheduling function, using velocity as an adjustment variable for control sensitivity to avoid applying excessive traction correction during high-speed joint movement, which could lead to shock, overshoot, or actuator saturation. Low-speed and high-speed thresholds are set for the velocity modulation function. The low-speed threshold corresponds to the initial velocity at which the sub-constraint coefficients begin to be modulated, and the high-speed threshold corresponds to the saturation velocity at which the sub-constraint coefficients are modulated to the maximum extent. A linear or S-shaped curve transition is used between the two thresholds to ensure that the modulation coefficients decrease continuously and smoothly as the velocity increases, thereby guaranteeing the attitude following capability. To prevent control instability under high-speed motion, when the absolute value of the real-time motion speed is less than or equal to the low-speed threshold, the speed modulation coefficient is set to, for example, 1.0, indicating that the sub-constraint coefficient is not modulated. When the absolute value of the real-time motion speed is greater than or equal to the high-speed threshold, the speed modulation coefficient is minimized. The minimum speed modulation coefficient is a constant greater than 0 and less than 1, indicating that the sub-constraint coefficient is weakened to the minimum value. When the absolute value of the real-time motion speed is between the low-speed threshold and the high-speed threshold, the speed modulation coefficient changes linearly from 1.0 to the minimum speed modulation coefficient in a linear decreasing manner. The speed modulation coefficient is multiplied by the sub-constraint coefficient to obtain the speed-modulated sub-constraint coefficient, so that the sub-constraint coefficient is weakened when the joint motion speed is high, avoiding the application of excessive traction correction during rapid joint motion, which would cause impact.

[0126] Step S403: Fuse the velocity-modulated sub-constraint coefficients with the initial drive quantities of the corresponding intermediate joints in the initial drive quantity sequence, and calculate the corrected drive quantity by combining the velocity-modulated sub-constraint coefficients of the adjacent joints of the intermediate joints in the current time frame.

[0127] In this embodiment, the velocity-modulated sub-constraint coefficients are used as the traction strength value of the current joint. The initial drive amount of the current intermediate joint in the corresponding time frame of the initial drive amount sequence is used as the initial drive angle of the current intermediate joint. The velocity-modulated sub-constraint coefficients of the intermediate joints before and after this intermediate joint are obtained. If the current intermediate joint is the first intermediate joint, only the sub-constraint coefficients of the next joint are obtained; if it is the last intermediate joint, only the sub-constraint coefficients of the previous joint are obtained. The traction strength value of the current joint and the traction strength values ​​of its adjacent joints are multiplied by the corresponding coupling weight coefficients. The coupling weight coefficients are determined based on the intermediate joint... The degree of motion coupling between segments is set. The degree of motion coupling is quantified by the transmission ratio or link length ratio between each joint, with a value ranging from 0 to 1. The larger the transmission ratio or the closer the link length ratio is to 1, the higher the degree of motion coupling. The three weighted traction strength values ​​are summed to obtain the comprehensive traction strength. The difference between the initial drive angle of the current intermediate joint and the actual drive angle of the intermediate joint in the previous time frame is calculated to obtain the trend of the drive angle change. The comprehensive traction strength is multiplied by the trend of the drive angle change to obtain the drive correction amount. The initial drive angle of the current intermediate joint is added to the drive correction amount to obtain the corrected drive amount.

[0128] Step S404: Combine the corrected drive amount of each intermediate joint with the initial drive amount of the non-intermediate joints in the initial drive amount sequence to form a corrected drive amount sequence, and control each drive joint of the robot to perform actions according to the corrected drive amount sequence.

[0129] In this embodiment, the corrected drive quantities of all intermediate joints are merged to obtain the corrected drive quantity vector of the intermediate joints. The initial drive quantities of non-intermediate joints in the current time frame are extracted from the initial drive quantity sequence. The non-intermediate joints include, but are not limited to, the robot's end effector joints, base joints, and other drive joints that are not subject to attitude constraints. The initial drive quantities of the non-intermediate joints are merged to obtain the corrected drive quantity vector of the non-intermediate joints. This vector is then combined with the corrected drive quantity vector of the intermediate joints to form the complete corrected drive quantity vector in the current time frame. The corrected drive quantity vector is then sent to the servo controllers of each drive joint of the robot. The servo controllers execute position control mode or speed control mode according to the received drive quantities, so that each drive joint of the robot moves according to the corrected drive quantities. For all elements in the visual perception acquisition sequence, the corrected drive quantity vector is repeatedly generated and continuously sent to the servo controllers of each drive joint of the robot. This enables the robot's end effector to follow the operator's end effector pose change while the pose of each intermediate joint continuously approaches the pose of the corresponding intermediate joint of the operator.

[0130] Example 2:

[0131] Please see Figure 4One embodiment of the present invention provides an intelligent robot motion capture control system based on industrial vision, comprising a data acquisition module, a mapping module, a coefficient construction module, and a drive module.

[0132] The data acquisition module is used to acquire the first posture change data of the end effector in space and the first posture change data of the intermediate joint in space when the operator performs the action, forming a visual perception acquisition sequence.

[0133] The mapping module is used to take the first pose change data in the visual perception acquisition sequence as the main control quantity, and calculate the initial driving quantity of each drive joint of the robot according to the spatial mapping relationship between the end effector parts of the robot and the operator, so as to obtain the initial driving quantity sequence.

[0134] The coefficient construction module is used to collect the posture change data of each intermediate joint during the robot's movement following the operator, calculate the posture deviation by combining the first posture change data, obtain the posture deviation sequence, and construct the constraint coefficient that changes with time based on the posture deviation sequence and the movement speed data of each intermediate joint of the operator.

[0135] The drive module is used to apply constraint coefficients to the initial drive quantity sequence, control the robot to follow the pose changes of the operator's end effector, and approach the pose of the corresponding intermediate joint.

[0136] In addition, the parts of the technical solutions provided in the embodiments of this application that are consistent with the implementation principles of the corresponding technical solutions in the prior art have not been described in detail, so as to avoid excessive elaboration.

[0137] The embodiments of the present invention have been described above with reference to the accompanying drawings. However, the present invention is not limited to the specific embodiments described above. The specific embodiments described above are merely illustrative and not restrictive. Those skilled in the art can make many other forms under the guidance of the present invention without departing from the spirit and scope of the claims. All of these forms are within the protection scope of the present invention.

Claims

1. A method for intelligent robot motion capture and control based on industrial vision, characterized in that, include: Acquire the first pose change data of the end effector in space and the first pose change data of the intermediate joint in space when the operator performs the action, and form a visual perception acquisition sequence. The first pose change data in the visual perception acquisition sequence is used as the main control variable. Based on the spatial mapping relationship between the end effector parts of the robot and the operator, the initial drive quantity of each drive joint of the robot is calculated to obtain the initial drive quantity sequence. Collect posture change data of each intermediate joint of the robot during the movement of the robot following the operator, calculate the posture deviation by combining the first posture change data, obtain the posture deviation sequence, and construct the constraint coefficient that changes with time based on the posture deviation sequence and the movement speed data of each intermediate joint of the operator. The constraint coefficients are applied to the initial drive sequence to control the robot to follow the pose changes of the operator's end effector and approach the pose of the corresponding intermediate joint.

2. The intelligent robot motion capture and control method based on industrial vision according to claim 1, characterized in that, The acquisition of the first pose change data of the end effector in space and the first pose change data of the intermediate joint in space when the operator performs the action, forming a visual perception acquisition sequence, includes: During the operation, the infrared visual acquisition system, located at the end of the operator's body and at the intermediate joints, sequentially acquires motion data in time frames, with each time frame set to a fixed time interval. The infrared visual acquisition system includes infrared cameras arranged around the operator. The motion data is parsed according to the corresponding data format, and the spatial coordinates and spatial orientation of the end effector are extracted to generate the numerical expression of the first pose change data. The spatial orientation of the intermediate joints is extracted to generate the numerical expression of the first pose change data. The parsed first pose change data is aligned with the first pose change data and then merged to obtain a continuous visual perception acquisition sequence.

3. The intelligent robot motion capture and control method based on industrial vision according to claim 1, characterized in that, The first pose change data in the visual perception acquisition sequence is used as the main control variable. Based on the spatial mapping relationship between the end effector parts of the robot and the operator, the initial drive quantity of each drive joint of the robot is calculated to obtain the initial drive quantity sequence, including: Based on the spatial coordinates in the first pose change data of the current time frame and the relative distance between the robot workspace boundary and each axis, mapping parameters are generated. The mapping parameters include the axis mapping ratio and the orientation mapping matrix. Based on the spatial mapping relationship between the robot's end effector and the operator's end effector, the spatial coordinates and spatial orientation in the first pose change data are used as input, and the desired pose change data of the robot's end effector is obtained by transforming according to the mapping parameters. The desired pose change data is used as input. Based on the motion relationship between each drive joint and the end effector of the robot, the initial drive amount of each drive joint is calculated in combination with the drive joint position calculated in the previous time frame. The initial drive quantities of each drive joint are arranged in sequence to form an initial drive quantity sequence corresponding to the time frame of the visual perception acquisition sequence.

4. The intelligent robot motion capture and control method based on industrial vision according to claim 3, characterized in that, The step involves using the spatial coordinates and orientation from the first pose change data as input, based on the spatial mapping relationship between the robot's end effector and the operator's end effector, and transforming them according to the mapping parameters to obtain the desired pose change data of the robot's end effector, including: The axial mapping ratio in the mapping parameters is adjusted according to the relative distance in each axis, and the spatial coordinates in the first pose change data are decomposed into components in different axes. The expected spatial coordinates of the robot end effector in space are calculated by the axial mapping ratio. The spatial orientation in the first pose change data is represented by a rotation matrix, and multiplied with the orientation mapping matrix to obtain the desired spatial orientation of the robot's end effector in space. The desired spatial coordinates and desired spatial orientation of the robot's end effector are merged to generate desired pose change data.

5. The intelligent robot motion capture and control method based on industrial vision according to claim 1, characterized in that, The robot collects posture change data of each intermediate joint during its movement while following the operator. This data, combined with the initial posture change data, is used to calculate the posture deviation, resulting in a posture deviation sequence. Based on this sequence and the operator's motion velocity data for each intermediate joint, time-varying constraint coefficients are constructed, including: During the robot's movement while following the operator, the posture change data of each intermediate joint of the robot is collected and time-aligned with the first posture change data in the same time frame of the visual perception acquisition sequence. Deviation analysis is performed on the posture change data of each intermediate joint of the robot and the first posture change data to calculate the posture deviation amount and obtain the posture deviation sequence. The baseline value of the traction amplitude is set based on the attitude deviation amount in the attitude deviation sequence, and the trend value of the attitude deviation is calculated by the attitude deviation amount of the current time frame and the previous time frame. By extracting the motion velocity data of each intermediate joint from the first posture change data, the suppression ratio of the traction amplitude is calculated, and the traction amplitude adjustment direction and adjustment amplitude determined by the change trend value are combined to calculate the constraint coefficient that changes with time.

6. The intelligent robot motion capture and control method based on industrial vision according to claim 5, characterized in that, The process involves extracting the motion velocity data of each intermediate joint from the first posture change data, calculating the suppression ratio of the traction amplitude, and combining this with the traction amplitude adjustment direction and adjustment amplitude determined by the trend value to calculate the constraint coefficient that changes over time, including: The direction of adjustment of the traction amplitude is determined by the sign of the trend value, and the adjustment amplitude of the traction amplitude is determined by exponential mapping of the absolute value of the trend value. The adjustment amplitude and the sign are combined to form a directional adjustment amount. Set the output range of the suppression ratio, take the motion speed data of each intermediate joint of the operator as the input variable, establish the mapping relationship between motion speed data and suppression ratio, and form a dynamic suppression function; The motion velocity data of each intermediate joint of the operator is input into the dynamic inhibition function for time-delay integration to obtain the inhibition ratio that changes smoothly over time. Nonlinear calculations are performed based on the directional adjustment amount and the suppression ratio. The suppression ratio is used as the exponent of the directional adjustment amount, and the constraint coefficient is calculated in combination with the reference value of the traction amplitude.

7. The intelligent robot motion capture and control method based on industrial vision according to claim 6, characterized in that, The setting of the output value range of the suppression ratio involves using the motion velocity data of each intermediate joint of the operator as input variables to establish a mapping relationship between the motion velocity data and the suppression ratio, forming a dynamic suppression function, including: Set the output value range of the suppression ratio, and map the zero value and preset maximum value of the movement speed data of each intermediate joint of the operator to the lower limit and upper limit of the output value range, respectively, to form an initial mapping relationship between the suppression ratio and the movement speed data; During the continuous process of the robot following the operator's movement, the cumulative change of the operator's motion speed data of each intermediate joint over time is collected and used as a dynamic correction factor. The initial mapping relationship is adjusted according to the dynamic correction factor, and the motion velocity data of the current time frame is used as input to establish a dynamic suppression function.

8. The intelligent robot motion capture and control method based on industrial vision according to claim 6, characterized in that, The nonlinear calculation based on the directional adjustment amount and the suppression ratio, using the suppression ratio as the exponent of the directional adjustment amount, and combining it with the baseline value of the traction amplitude to calculate the constraint coefficient, includes: The suppression ratio is nonlinearly adjusted based on the difference between the suppression ratio of the current time frame and the suppression ratio of the previous time frame to obtain the dynamic suppression index. The constraint coefficient is obtained by exponentially calculating the baseline value of the traction amplitude, the dynamic inhibition index as the exponent of the directional adjustment amount, and the product of the directional adjustment amount and the dynamic inhibition index.

9. The intelligent robot motion capture and control method based on industrial vision according to claim 1, characterized in that, The step of applying constraint coefficients to the initial drive sequence to control the robot to follow the pose changes of the operator's end effector and approach the pose of the corresponding intermediate joint includes: Based on the actual posture of each intermediate joint of the robot in the current time frame and the distance to the corresponding physical limit, the decomposition ratio of the constraint coefficient on the driving channel of each intermediate joint is allocated to obtain the sub-constraint coefficients corresponding to each intermediate joint. The motion velocity data is used as a modulation parameter to perform a nonlinear mapping on the sub-constraint coefficients, and the sub-constraint coefficients after velocity modulation are calculated. The velocity-modulated sub-constraint coefficients are fused with the initial drive quantities of the corresponding intermediate joints in the initial drive quantity sequence. The corrected drive quantity is then calculated by combining the velocity-modulated sub-constraint coefficients of the adjacent joints of the intermediate joints in the current time frame. The corrected drive quantities of each intermediate joint are combined with the initial drive quantities of the non-intermediate joints in the initial drive quantity sequence to form a corrected drive quantity sequence. The robot's drive joints are then controlled to perform actions based on the corrected drive quantity sequence.

10. An intelligent robot motion capture control system based on industrial vision, used to implement the intelligent robot motion capture control method based on industrial vision as described in any one of claims 1-9, characterized in that, It includes a data acquisition module, a mapping module, a coefficient construction module, and a driver module: The data acquisition module is used to acquire the first posture change data of the end effector in space and the first posture change data of the intermediate joint in space when the operator performs the action, forming a visual perception acquisition sequence. The mapping module is used to take the first pose change data in the visual perception acquisition sequence as the main control quantity, and calculate the initial driving quantity of each drive joint of the robot according to the spatial mapping relationship between the end effector parts of the robot and the operator, so as to obtain the initial driving quantity sequence. The coefficient construction module is used to collect the posture change data of each intermediate joint during the robot's movement following the operator, calculate the posture deviation by combining the first posture change data, obtain the posture deviation sequence, and construct the constraint coefficient that changes with time based on the posture deviation sequence and the movement speed data of each intermediate joint of the operator. The drive module is used to apply constraint coefficients to the initial drive quantity sequence, control the robot to follow the pose changes of the operator's end effector, and approach the pose of the corresponding intermediate joint.

Images