A Preset-Time Visual Servo Control Method and System for a Dual-Arm Robot Based on Homography Matrix
By constructing rotation and translation error vectors based on the homography matrix and designing a preset time performance boundary, the coupling problem between visual error and coordinated control in visual servoing of dual-arm robots is solved. This enables preset time visual pose adjustment without relying on the feasibility of initial error, thereby improving the visual servoing control effect of collaborative operation of dual-arm robots.
Patent Information
- Application Number
- CN202611585343.8
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2026-10-14
- Publication Date
- 2026-11-10
AI Technical Summary
In collaborative visual servoing of dual-arm robots, there is a clear coupling between visual error and dual-arm coordinated control. Existing methods are difficult to achieve visual pose adjustment for a preset time without relying on the feasibility of initial error. Furthermore, traditional preset performance control methods require the initial error to fall within a fixed performance boundary, making it difficult to adapt to task changes.
By using a homography matrix-based method, the image homography matrix is estimated and converted into an Euclidean homography matrix. Rotation and translation error vectors are constructed, a preset time performance boundary and its obstacle function transformation are designed, and a mapping relationship between object velocity and joint velocity is established to achieve preset time visual servo adjustment.
It can achieve small steady-state error and error convergence within a preset time without the initial error falling into a fixed boundary. It is suitable for a variety of dual-arm collaborative operation scenarios and improves the transient performance and steady-state accuracy of visual servo control.
Smart Images

Figure CN123060732A_ABST
Abstract
Description
Technical Field
[0001] This invention belongs to the field of robot vision servo control and dual-arm robot collaborative operation technology, and in particular relates to a preset time vision servo control method and system for dual-arm robots based on homography matrix. Background Technology
[0002] Compared to single-arm robots, dual-arm robots offer greater load capacity and dexterity. However, in practical tasks, the initial pose of the manipulated object and environmental conditions are uncertain, making it difficult to guarantee operational accuracy through offline planning alone. Therefore, dual-arm robots typically require visual sensors to acquire object image features and achieve online adjustment through visual servoing. However, in dual-arm cooperative visual servoing, visual errors are usually defined at the object level, while control commands need to be distributed to both robotic arms, and the object's motion is generated jointly by both arms, resulting in significant coupling between visual feedback adjustment and dual-arm coordinated control.
[0003] Existing positional visual servoing methods typically rely on complete pose reconstruction, where image features may deviate from the camera's field of view. Image-based visual servoing methods directly adjust for image errors, but may suffer from problems such as image Jacobian singularities, local minima, and non-ideal Cartesian trajectories. Homography-based visual servoing can reconstruct relative rotation and partial translational geometry using homography relationships between planar visual features, offering both image error feedback and geometric interpretation. However, current research on homography-based visual servoing largely focuses on single-arm systems, making it difficult to directly apply to dual-arm collaborative grasping scenarios.
[0004] Furthermore, in high-precision dual-arm collaborative operation tasks, the control system not only needs a small final steady-state error but also requires clear constraints on the error convergence process and settling time. The goal of preset-time performance control is to allow the user to directly specify time parameters during the controller design phase, ensuring that the visual error enters a preset residual range within a specified time and remains within the allowable accuracy range after that time. Compared to general asymptotic convergence or finite-time convergence methods, preset-time performance constraints are more convenient for setting performance indicators based on task cycle time, operational safety, and collaborative execution efficiency. However, traditional preset-time performance control methods typically require the initial error to fall within a pre-defined performance boundary. In contrast, the initial object pose, grasping configuration, and desired visual reference in dual-arm visual servoing tasks often change with the task, making it difficult for the initial error to meet the feasibility conditions of fixed performance boundaries. Therefore, a visual servoing control method for dual-arm robots is urgently needed to achieve preset-time visual pose adjustment without relying on the feasibility conditions of initial error. Summary of the Invention
[0005] To address the above technical problems, this invention provides a method and system for pre-set time visual servo control of a dual-arm robot based on homography matrix.
[0006] The technical solution adopted by this invention to solve its technical problem is: In a first aspect, the present invention provides a pre-set time visual servo control method for a dual-arm robot based on homography matrix, the method comprising the following steps: S100: Acquire the image of the current visual sign plane, obtain the corresponding feature points of the current visual sign plane and the fixed expected visual sign plane, and estimate the image homography matrix based on the camera intrinsic parameter matrix; S200: Based on the camera intrinsic parameter matrix, the image homography matrix is converted into a Euclidean homography matrix, and the Euclidean homography matrix is decomposed. The physically feasible solution is selected from the candidate solutions obtained by decomposition to obtain the relative rotation parameters of the current visual label plane relative to the desired visual label plane. S300: Establish the kinematic relationship of rigid grasping of the dual-arm robot and construct the mapping relationship from the object velocity to the joint velocity of the dual-arm robot; S400: Based on the relative rotation parameters, a rotation error vector is constructed using the angular axis representation. An extended translation error vector is constructed based on the image coordinates and depth information of the visual identifier center point. The original error channel is then constructed based on the rotation error vector and the extended translation error vector. S500: Construct a preset time performance boundary and its corresponding barrier function transformation for each original error channel, and convert each original error channel into a transformed error channel; S600: Design object angular velocity control law and object translation velocity control law according to each transformation error channel, and convert the combined object velocity command into dual-arm robot joint velocity command through mapping relationship to drive the robot to achieve preset time visual servo adjustment.
[0007] Preferably, S100 includes: S110: Obtain at least four pairs of non-collinear corresponding feature points on the current visual identity plane and the desired visual identity plane, and denote their pixel homogeneous coordinates as follows: and ,in, The index is the sequence number of the feature point. ,and , This represents the total number of selected corresponding feature point pairs. For the current visual identity plane, the first The pixel homogeneous coordinate vector of each feature point These are the first two visual identifiers on the current visual identity plane. The x and y coordinates of each feature point in the image coordinate system For the desired visual identity on the plane, the first The pixel homogeneous coordinate vector of each feature point These are the first two lines on the desired visual identity plane. The x and y coordinates of each feature point in the image coordinate system; S120: In the camera intrinsic parameter matrix Given the given conditions, the least squares homography matrix estimation method is used to solve for the image homography matrix. And establish the projection homography constraint relationship between the current feature point and the expected feature point: (1) in, Scale factor; It is the Euclidean homography matrix. Preferably, S200 includes: S210: Euclidean homography matrix Scale normalization is performed, and Euclidean homography matrix decomposition based on singular value decomposition is adopted to... Decompose into satisfying The set of candidate solutions, where This is the relative rotation matrix of the current visual signage plane relative to the desired visual signage plane; It is a scale translation vector; Let be the unit normal vector of the desired visual identity plane; S220: Physical feasibility screening of candidate solutions obtained from Euclidean homography matrix decomposition: First, candidate solutions that do not satisfy the positive depth constraint of feature points and the plane normal direction constraint are eliminated; when multiple candidate solutions still exist after screening, a physically feasible solution is selected from the remaining candidate solutions based on the continuity of decomposition results at adjacent time steps, thus obtaining the relative rotation matrix of the current visual sign plane relative to the desired visual sign plane. Scale translation vector and the unit normal vector of the desired visual identity plane Among them, positive depth constraint means that the visual feature points corresponding to the decomposition result have positive depth in the camera coordinate system; plane normal direction constraint means that the direction of the unit normal vector obtained by decomposition is consistent with the normal direction of the expected visual label plane; continuity means that there is no abrupt change between the decomposition result at the current time and the decomposition result at the previous time.
[0008] Preferably, S300 includes: S310: Let the joint vectors of the dual-arm robot be... ,in and These are the joint vectors for the left and right robotic arms, respectively; the corresponding joint velocity vectors are... ,in and These are the velocity vectors of the left and right robotic arm joints, respectively; the end effector velocities of the left and right robotic arms are respectively... and Its synthesis rate is ; S320: Let the object's pose be... The object's velocity is ,in Let the object's translational velocity be... Let be the angular velocity of the object. In the base coordinate system of the dual-arm robot, the end effector velocity and joint velocity of the dual-arm robot satisfy the following: (2) in, For the Jacobian matrix of the end effector of a dual-arm robot, and These are the Jacobian matrices for the left and right robotic arm end effectors, respectively. Represents a block diagonal matrix; S330: When the two robotic arms maintain a rigid grip on the object and there is no relative slippage at the gripping point, the end-effector velocity of the robotic arm and the velocity of the object satisfy the following: (3) in, For dual-arm grasping matrix, and These are the grasping sub-matrices corresponding to the left and right robotic arms, used to describe the rigid motion constraint relationship between the object's velocity and the end-effector velocities of each robotic arm; S340: From equations (2) and (3), we can obtain the mapping relationship between the object velocity and the joint velocity of the dual-arm robot: (4) in, for Moore-Penrose pseudoinverse, symbol Representing the Moore-Penrose pseudo-inverse operation, equation (4) is used to convert the object velocity command output by the object layer visual servo controller. Convert to joint velocity commands for dual-arm robot This drives the left and right robotic arms to work together to complete the desired object movement.
[0009] Preferably, S400 includes: S410: Based on relative rotation matrix The rotation error vector is defined using the angular axis: (5) in, Let be the rotation error vector. For the unit axis of rotation, Let be the relative rotation angle between the current visual signage plane and the desired visual signage plane, and satisfy . Relative rotation angle As rotation angle error; and It is calculated by the following formula: (6) in, Represents the trace of a matrix. Let represent an antisymmetric matrix constructed from vectors, when When the time is right, it means that there is no relative rotation error between the current visual signage plane and the desired visual signage plane; S420: Select the center point of the visual sign as the reference point for translation error, and let the extended translation vector of the current visual sign center point be... The expected translation vector for the expansion of the visual identifier's center point is: ,in, and These represent the translation vectors of the current and desired visual identifier center points, respectively. and The image coordinates of the current visual identifier's center point. The depth of the current visual identifier center point relative to the camera coordinate system; , and Let be the image coordinates of the desired visual identifier center point and the desired depth, respectively; define the extended translation error as: (7) in, To expand the translation error vector, , and They are defined as follows: (8) in, The depth ratio between the desired depth and the current depth. and These are the horizontal pixel error and the vertical pixel error, respectively. This is the depth ratio related error.
[0010] Preferably, S500 includes: S510: For each error channel ,set up They represent , , and ,in Indicates rotation angle error. , and These represent horizontal pixel error, vertical pixel error, and depth scale-related error, respectively. S520: Given design parameters Preset time Upper bound of target residual accuracy and scaling parameters ,in Used to adjust the error normalization scale. Used to specify the time when the error enters the target residual region. Used to specify the corresponding error channel The target residual accuracy at that time Used to adjust the rate of change of the preset time scaling function; the performance boundary generation function is defined as: (9) in, As an auxiliary variable, Used to generate performance boundaries for error channels; S530: Based on the upper limit of the target residual accuracy Calculate the final value of the preset time scaling function: (10) in, Indicates the error channel The corresponding scaling function at the preset time The final value that is then maintained makes ; S540: Construct a preset time scaling function: (11) in, For error channel The corresponding preset time scaling function, and satisfies and in Keep as ; Further define the barrier function transformation: (12) in, To normalize the error, As an intermediate variable, This refers to the error in the barrier function transformation. S550: Through equation (12), the original error channel Convert to transformation error channel ;because The performance boundary corresponding to the initial moment tends to infinity in the limiting sense. Therefore, this performance transformation does not require the initial error to fall within the finite performance boundary in advance.
[0011] Preferably, in S600, the object angular velocity control law and the object translational velocity control law are designed according to each transformation error channel, including: S610: For rotation angle error in the rotary channel According to equation (12), , and Define auxiliary variables: (13) in, This is the rotation error normalization term. This is the transformation term of the obstacle function for the rotating channel; S620: Define a Jacobian-like matrix for: (14) in, It is the identity matrix. It is the singer function, and ; S630: Select positive design parameters The angular velocity control law for the object is designed as follows: (15) in, The command for the object's angular velocity. Preset a time scaling function for the rotating channel. Let be the rotation matrix from the base coordinate system of the dual-arm robot to the camera coordinate system. for The first derivative; S640: For translation channels, the definition is: (16) in, To extend the translation error, the obstacle function transforms the error vector. These represent the barrier function transformation errors corresponding to the horizontal pixel error channel, the vertical pixel error channel, and the depth scale-related error channel, respectively. For the normalized error vector, These are the normalized errors corresponding to each translation error channel. A vector composed of the derivatives of the time scaling function for the translation channel. These are the first derivatives of the preset time scaling functions corresponding to each translation error channel; S650: Define the matrix: (17) in, This is the diagonal matrix of the transformation terms of the translation channel obstacle function. These correspond to the barrier function transformation terms for the horizontal pixel error channel, the vertical pixel error channel, and the depth scale-related error channel, respectively. Represents a diagonal matrix. This is the auxiliary gain matrix for the translation channel. The diagonal elements of the translation channel auxiliary gain matrix correspond to the product of the preset time scaling function and the error normalization term for each translation error channel, and for... have: (18) in, For each translation error channel, the barrier function transformation term is... These are intermediate variables defined for each translation error channel in the barrier function transformation. This is the error normalization term for each translation error channel; S660: Set ,in and Let be the image coordinates of the current visual identifier's center point; The matrix is determined by the camera's extrinsic parameters. for The third line defines the extended interaction matrix: (19) in, Let be the camera intrinsic parameter matrix, take The first three columns constitute a submatrix and set ,in , , Positive design parameters; The control law for the translational velocity of the object is designed as follows: (20) in, This is the command for the translation speed of the object. The depth of the current visual identifier center point relative to the camera coordinate system. for Moore-Penrose pseudo-reverse, for The inverse matrix, for The inverse matrix, This represents the Hadamard element-wise product.
[0012] Preferably, in S600, the combined object velocity command is converted into a dual-arm robot joint velocity command through a mapping relationship, including: Combine the object angular velocity command obtained from equation (15) and the object translational velocity command obtained from equation (20) into an object velocity command: (twenty one) Substituting equation (21) into equation (4), we obtain the joint speed command for the dual-arm robot: (twenty two) The controller will Decomposed into left arm joint velocity command and right arm joint speed command The data is then sent to the servo execution units of the left and right robotic arms, respectively, so that the two robotic arms can jointly generate the desired object speed, thereby achieving preset time visual servo pose adjustment of the manipulated object relative to the desired visual reference.
[0013] Secondly, the present invention provides a pre-set time visual servo control system for a dual-arm robot based on a homography matrix, comprising the steps of executing a pre-set time visual servo control method for a dual-arm robot based on a homography matrix, including: The image homography matrix estimation module is used to acquire the image of the current visual sign plane, obtain the corresponding feature points of the current visual sign plane and the fixed expected visual sign plane, and estimate the image homography matrix based on the camera intrinsic parameter matrix; The relative rotation parameter determination module is used to convert the image homography matrix into an Euclidean homography matrix based on the camera intrinsic parameter matrix, decompose the Euclidean homography matrix, select a physically feasible solution from the candidate solutions obtained by decomposition, and obtain the relative rotation parameters of the current visual label plane relative to the desired visual label plane. The kinematic mapping module establishes the kinematic relationship of rigid grasping by the dual-arm robot and constructs the mapping relationship between the object velocity and the joint velocity of the dual-arm robot. The error construction module is used to construct a rotation error vector based on the relative rotation parameters using the angular axis representation, and to construct an extended translation error vector based on the image coordinates and depth information of the visual identifier center point. The original error channel is then constructed based on the rotation error vector and the extended translation error vector. The transformation module is used to construct a preset time performance boundary and its corresponding barrier function transformation for each original error channel, and to convert each original error channel into a transformed error channel. The control module is used to design object angular velocity control laws and object translation velocity control laws according to each transformation error channel, and convert the combined object velocity command into dual-arm robot joint velocity command through mapping relationship to drive the robot to achieve preset time visual servo adjustment.
[0014] Thirdly, the present invention provides a computer device, including a memory and a processor, wherein the memory stores a computer program, and the processor executes the computer program to implement the steps of a preset time visual servo control method for a dual-arm robot based on homography matrix.
[0015] The aforementioned method and system for pre-set time visual servo control of a dual-arm robot based on homography matrix decomposition obtains the relative rotation parameters between visual marker planes. Combining the image coordinates and depth information of the visual marker center point, it constructs rotational and extended translational errors. This eliminates the need for complete 3D pose reconstruction of the manipulated object, exhibiting good environmental adaptability and versatility. Pre-set time performance boundaries and obstacle function transformations are constructed for the original error channels, converting constrained original errors into unconstrained transformation errors. This allows for pre-setting of error convergence time and steady-state accuracy, without requiring the initial error to fall within finite boundaries, significantly improving the transient performance and steady-state accuracy of visual servo control. Furthermore, by converting object velocity commands into joint velocity commands through rigid grasping kinematic mapping of the dual-arm robot, a complete mapping from object-level control laws to joint space is achieved. This enables the two robotic arms to collaboratively complete the pre-set time visual servo pose adjustment of the manipulated object, making it suitable for various dual-arm collaborative operation scenarios. Attached Figure Description
[0016] Figure 1 A flowchart illustrating a pre-set time visual servo control method for a dual-arm robot based on a homography matrix, provided as an embodiment of the present invention; Figure 2 This is an overall framework diagram of a pre-set time visual servo control method for a dual-arm robot based on homography matrix, provided in an embodiment of the present invention. Detailed Implementation
[0017] To enable those skilled in the art to better understand the technical solution of the present invention, the present invention will be further described in detail below with reference to the accompanying drawings.
[0018] In one embodiment, the flow of a pre-set time visual servo control method for a dual-arm robot based on homography matrix is as follows: Figure 1 As shown in the diagram, the overall framework is as follows: Figure 2 As shown, the method includes the following steps: S100: Acquire the image of the current visual sign plane, obtain the corresponding feature points of the current visual sign plane and the fixed expected visual sign plane, and estimate the image homography matrix based on the camera intrinsic parameter matrix; S200: Based on the camera intrinsic parameter matrix, the image homography matrix is converted into a Euclidean homography matrix, and the Euclidean homography matrix is decomposed. The physically feasible solution is selected from the candidate solutions obtained by decomposition to obtain the relative rotation parameters of the current visual label plane relative to the desired visual label plane. S300: Establish the kinematic relationship of rigid grasping of the dual-arm robot and construct the mapping relationship from the object velocity to the joint velocity of the dual-arm robot; S400: Based on the relative rotation parameters, a rotation error vector is constructed using the angular axis representation. An extended translation error vector is constructed based on the image coordinates and depth information of the visual identifier center point. The original error channel is then constructed based on the rotation error vector and the extended translation error vector. S500: Construct a preset time performance boundary and its corresponding barrier function transformation for each original error channel, and convert each original error channel into a transformed error channel; S600: Design object angular velocity control law and object translation velocity control law according to each transformation error channel, and convert the combined object velocity command into dual-arm robot joint velocity command through mapping relationship to drive the robot to achieve preset time visual servo adjustment.
[0019] This method acquires the current feature points and fixed desired feature points of the planar visual identifiers on the object being grasped by dual arms using a camera. It estimates and decomposes the homography matrix to extract relative rotation information. Based on this relative rotation information, a rotation error vector is constructed, and an extended translation error is constructed by combining the image coordinates and depth information of the visual identifier's center point. On this basis, original error channels are constructed based on the rotation error vector and the extended translation error vector. A preset time performance transformation is performed on each original error channel, and object layer angular velocity control laws and translation velocity control laws are designed. Finally, joint velocity commands for the dual-arm robot are generated through dual-arm grasping kinematic mapping, achieving preset time visual pose adjustment under the desired visual reference.
[0020] In one embodiment, S100 includes: S110: Obtain at least four pairs of non-collinear corresponding feature points on the current visual identity plane and the desired visual identity plane, and denote their pixel homogeneous coordinates as follows: and ,in, The index is the sequence number of the feature point. ,and , This represents the total number of selected corresponding feature point pairs. For the current visual identity plane, the first The pixel homogeneous coordinate vector of each feature point These are the first two visual identifiers on the current visual identity plane. The x and y coordinates of each feature point in the image coordinate system For the desired visual identity on the plane, the first The pixel homogeneous coordinate vector of each feature point These are the first two lines on the desired visual identity plane. The x and y coordinates of each feature point in the image coordinate system; S120: In the camera intrinsic parameter matrix Given the given conditions, the least squares homography matrix estimation method is used to solve for the image homography matrix. And establish the projection homography constraint relationship between the current feature point and the expected feature point: (1) in, Scale factor; It is the Euclidean homography matrix.
[0021] Through the above steps, the image homography matrix and Euclidean homography matrix can be obtained from the corresponding feature points of the current visual labeling plane and the desired visual labeling plane, providing a geometric information basis for the subsequent decomposition of relative motion parameters and construction of visual errors.
[0022] In one embodiment, S200 includes: S210: Euclidean homography matrix Scale normalization is performed, and Euclidean homography matrix decomposition based on singular value decomposition is adopted to... Decompose into satisfying The set of candidate solutions, where This is the relative rotation matrix of the current visual signage plane relative to the desired visual signage plane. For scale translation vector, Let be the unit normal vector of the desired visual identity plane; S220: Physical feasibility screening of candidate solutions obtained from Euclidean homography matrix decomposition: First, candidate solutions that do not satisfy the positive depth constraint of feature points and the plane normal direction constraint are eliminated; when multiple candidate solutions still exist after screening, a physically feasible solution is selected from the remaining candidate solutions based on the continuity of decomposition results at adjacent time steps, thus obtaining the relative rotation matrix of the current visual sign plane relative to the desired visual sign plane. Scale translation vector and the unit normal vector of the desired visual identity plane Among them, positive depth constraint means that the visual feature points corresponding to the decomposition result have positive depth in the camera coordinate system; plane normal direction constraint means that the direction of the unit normal vector obtained by decomposition is consistent with the normal direction of the expected visual label plane; continuity means that there is no abrupt change between the decomposition result at the current time and the decomposition result at the previous time.
[0023] By screening the candidate solutions described above, decomposition results that do not satisfy the actual imaging relationship can be eliminated, and relative rotation parameters that have physical meaning and change continuously in adjacent time intervals can be obtained, providing a basis for the stable construction of rotation error.
[0024] In one embodiment, the rotation error vector includes a rotation angle error, and the extended translation error vector includes horizontal pixel error, vertical pixel error, and depth scale-related error. The rotation angle error, horizontal pixel error, vertical pixel error, and depth scale-related error constitute each original error channel; S300 includes: S310: Let the joint vectors of the dual-arm robot be... ,in and These are the joint vectors for the left and right robotic arms, respectively; the corresponding joint velocity vectors are... ,in and These are the velocity vectors of the left and right robotic arm joints, respectively; the end effector velocities of the left and right robotic arms are respectively... and Its synthesis rate is ; S320: Let the object's pose be... The object's velocity is ,in Let the object's translational velocity be... Let be the angular velocity of the object. In the base coordinate system of the dual-arm robot, the end effector velocity and joint velocity of the dual-arm robot satisfy the following: (2) in, For the Jacobian matrix of the end effector of a dual-arm robot, and These are the Jacobian matrices for the left and right robotic arm end effectors, respectively. Represents a block diagonal matrix; S330: When the two robotic arms maintain a rigid grip on the object and there is no relative slippage at the gripping point, the end-effector velocity of the robotic arm and the velocity of the object satisfy the following: (3) in, For dual-arm grasping matrix, and These are the grasping sub-matrices corresponding to the left and right robotic arms, used to describe the rigid motion constraint relationship between the object's velocity and the end-effector velocities of each robotic arm; S340: From equations (2) and (3), we can obtain the mapping relationship between the object velocity and the joint velocity of the dual-arm robot: (4) in, for Moore-Penrose pseudoinverse, symbol Representing the Moore-Penrose pseudo-inverse operation, equation (4) is used to convert the object velocity command output by the object layer visual servo controller. Convert to joint velocity commands for dual-arm robot This drives the left and right robotic arms to work together to complete the desired object movement.
[0025] By establishing a mapping relationship between the object's velocity and the joint velocities of the dual-arm robot, the object-layer visual servo control commands can be converted into joint velocity commands for the left and right robotic arms, enabling the two robotic arms to collaboratively drive the movement of the manipulated object under rigid grasping constraints. In one embodiment, S400 includes: S410: Based on relative rotation matrix The rotation error vector is defined using the angular axis: (5) in, Let be the rotation error vector. For the unit axis of rotation, Let be the relative rotation angle between the current visual signage plane and the desired visual signage plane, and satisfy . Relative rotation angle As rotation angle error; and It is calculated by the following formula: (6) in, Represents the trace of a matrix. Let represent an antisymmetric matrix constructed from vectors, when When the time is right, it means that there is no relative rotation error between the current visual signage plane and the desired visual signage plane; S420: Select the center point of the visual sign as the reference point for translation error, and let the extended translation vector of the current visual sign center point be... The expected translation vector for the expansion of the visual identifier's center point is: ,in, and These represent the translation vectors of the current and desired visual identifier center points, respectively. and The image coordinates of the current visual identifier's center point. The depth of the current visual identifier center point relative to the camera coordinate system; , and Let be the image coordinates of the desired visual identifier center point and the desired depth, respectively; define the extended translation error as: (7) in, To expand the translation error vector, , and They are defined as follows: (8) in, The depth ratio between the desired depth and the current depth. and These are the horizontal pixel error and the vertical pixel error, respectively. This is the depth ratio related error.
[0026] Through the above error construction, the rotational deviation, image plane position deviation, and depth deviation of the manipulated object relative to the desired visual reference are represented as the corresponding original error channels, providing a basis for applying preset time performance constraints to each error channel.
[0027] In one embodiment, S500 includes: S510: For each error channel ,set up They represent , , and ,in Indicates rotation angle error. , and These represent horizontal pixel error, vertical pixel error, and depth scale-related error, respectively. S520: Given design parameters Preset time Upper bound of target residual accuracy and scaling parameters ,in Used to adjust the error normalization scale. Used to specify the time when the error enters the target residual region. Used to specify the corresponding error channel The target residual accuracy at that time Used to adjust the rate of change of the preset time scaling function; the performance boundary generation function is defined as: (9) in, As an auxiliary variable, Used to generate performance boundaries for error channels; S530: Based on the upper limit of the target residual accuracy Calculate the final value of the preset time scaling function: (10) in, Indicates the error channel The corresponding scaling function at the preset time The final value that is then maintained makes ; S540: Construct a preset time scaling function: (11) in, For error channel The corresponding preset time scaling function, and satisfies and in Keep as ; Further define the barrier function transformation: (12) in, To normalize the error, As an intermediate variable, This refers to the error in the barrier function transformation. S550: Through equation (12), the original error channel Convert to transformation error channel ;because The performance boundary corresponding to the initial moment tends to infinity in the limiting sense. Therefore, this performance transformation does not require the initial error to fall within the finite performance boundary in advance.
[0028] Through the above performance boundary and barrier function transformation, each original error channel can be converted into a corresponding transformed error channel, and the error convergence time and target residual range can be preset by design parameters, while avoiding the imposition of finite performance boundary conditions on the initial error.
[0029] In one embodiment, S600 designs the object angular velocity control law and the object translation velocity control law according to each transformation error channel, including: S610: For rotation angle error in the rotary channel According to equation (12), , and Define auxiliary variables: (13) in, This is the rotation error normalization term. This is the transformation term of the obstacle function for the rotating channel; S620: Define a Jacobian-like matrix for: (14) in, It is the identity matrix. It is the singer function, and ; S630: Select positive design parameters The angular velocity control law for the object is designed as follows: (15) in, The command for the object's angular velocity. Preset a time scaling function for the rotating channel. Let be the rotation matrix from the base coordinate system of the dual-arm robot to the camera coordinate system. for The first derivative; S640: For translation channels, the definition is: (16) in, To extend the translation error, the obstacle function transforms the error vector. These represent the barrier function transformation errors corresponding to the horizontal pixel error channel, the vertical pixel error channel, and the depth scale-related error channel, respectively. For the normalized error vector, These are the normalized errors corresponding to each translation error channel. A vector composed of the derivatives of the time scaling function for the translation channel. These are the first derivatives of the preset time scaling functions corresponding to each translation error channel; S650: Define the matrix: (17) in, This is the diagonal matrix of the transformation terms of the translation channel obstacle function. These correspond to the barrier function transformation terms for the horizontal pixel error channel, the vertical pixel error channel, and the depth scale-related error channel, respectively. Represents a diagonal matrix. This is the auxiliary gain matrix for the translation channel. The diagonal elements of the translation channel auxiliary gain matrix correspond to the product of the preset time scaling function and the error normalization term for each translation error channel, and for... have: (18) in, For each translation error channel, the barrier function transformation term is... These are intermediate variables defined for each translation error channel in the barrier function transformation. This is the error normalization term for each translation error channel; S660: Set ,in and Let be the image coordinates of the current visual identifier's center point; The matrix is determined by the camera's extrinsic parameters. for The third line defines the extended interaction matrix: (19) in, Let be the camera intrinsic parameter matrix, take The first three columns constitute a submatrix and set ,in , , Positive design parameters; The control law for the translational velocity of the object is designed as follows: (20) in, This is the command for the translation speed of the object. The depth of the current visual identifier center point relative to the camera coordinate system. for Moore-Penrose pseudo-reverse, for The inverse matrix, for The inverse matrix, This represents the Hadamard element-wise product.
[0030] In one embodiment, S600 converts the combined object velocity command into a dual-arm robot joint velocity command through a mapping relationship, including: Combine the object angular velocity command obtained from equation (15) and the object translational velocity command obtained from equation (20) into an object velocity command: (twenty one) Substituting equation (21) into equation (4), we obtain the joint speed command for the dual-arm robot: (twenty two) The controller will Decomposed into left arm joint velocity command and right arm joint speed command The data is then sent to the servo execution units of the left and right robotic arms, respectively, so that the two robotic arms can jointly generate the desired object speed, thereby achieving preset time visual servo pose adjustment of the manipulated object relative to the desired visual reference.
[0031] Through the aforementioned control laws and kinematic mapping, a closed-loop control process can be formed, from visual error feedback to object velocity command, and then to the velocity command of the two arm joints. This allows each visual error channel to enter the corresponding target residual range within a preset time, thereby achieving pose adjustment of the manipulated object relative to the desired visual reference.
[0032] In one embodiment, a pre-set time visual servoing control system for a dual-arm robot based on a homography matrix is also provided, comprising: The image homography matrix estimation module is used to acquire the image of the current visual sign plane, obtain the corresponding feature points of the current visual sign plane and the fixed expected visual sign plane, and estimate the image homography matrix based on the camera intrinsic parameter matrix; The relative rotation parameter determination module is used to convert the image homography matrix into an Euclidean homography matrix based on the camera intrinsic parameter matrix, decompose the Euclidean homography matrix, select a physically feasible solution from the candidate solutions obtained by decomposition, and obtain the relative rotation parameters of the current visual label plane relative to the desired visual label plane. The kinematic mapping module establishes the kinematic relationship of rigid grasping by the dual-arm robot and constructs the mapping relationship between the object velocity and the joint velocity of the dual-arm robot. The error construction module is used to construct a rotation error vector based on the relative rotation parameters using the angular axis representation, and to construct an extended translation error vector based on the image coordinates and depth information of the visual identifier center point. The original error channel is then constructed based on the rotation error vector and the extended translation error vector. The transformation module is used to construct a preset time performance boundary and its corresponding barrier function transformation for each original error channel, and to convert each original error channel into a transformed error channel. The control module is used to design object angular velocity control laws and object translation velocity control laws according to each transformation error channel, and convert the combined object velocity command into dual-arm robot joint velocity command through mapping relationship to drive the robot to achieve preset time visual servo adjustment.
[0033] Specific limitations regarding the pre-set time visual servoing control system for a dual-arm robot based on homography matrix can be found in the above-described limitations of the pre-set time visual servoing control method for a dual-arm robot based on homography matrix, and will not be repeated here. Each module in the aforementioned pre-set time visual servoing control system for a dual-arm robot based on homography matrix can be implemented entirely or partially through software, hardware, or a combination thereof. These modules can be embedded in or independent of the processor in a computer device, or stored in the memory of a computer device as software, so that the processor can call and execute the corresponding operations of each module.
[0034] In one embodiment, a computer device is also provided, including a memory and a processor, the memory storing a computer program, the processor executing the computer program to implement the steps of a preset-time visual servo control method for a dual-arm robot based on a homography matrix.
[0035] Those skilled in the art will understand that all or part of the processes in the methods of the above embodiments can be implemented by a computer program instructing related hardware. The computer program can be stored in a non-volatile computer-readable storage medium, and when executed, it can include the processes of the embodiments of the methods described above. Any references to memory, storage, databases, or other media used in the embodiments provided in this application can include at least one of non-volatile and volatile memory. Non-volatile memory can include read-only memory (ROM), magnetic tape, floppy disk, flash memory, or optical storage, etc. Volatile memory can include random access memory (RAM) or external cache memory. By way of illustration and not limitation, RAM can be in various forms, such as static random access memory (SRAM) or dynamic random access memory (DRAM), etc.
[0036] Compared with the prior art, the present invention has the following beneficial effects: 1) This invention combines homography matrix visual servoing with dual-arm grasping kinematics, constructs visual errors at the object level and designs speed control laws, and then maps the object speed commands to the left and right robotic arm joint speed commands through dual-arm grasping kinematics, so that the visual adjustment at the object level and the collaborative execution at the dual-arm joint level form a unified control framework, which is suitable for visual pose adjustment in dual-arm robot collaborative grasping scenarios.
[0037] 2) This invention constructs a preset time performance constraint by transforming the performance boundary generation function, the preset time scaling function, and the barrier function, so that the rotation angle error, horizontal pixel error, vertical pixel error, and depth ratio related error can enter the corresponding target residual range within a preset time. At the same time, it avoids the dependence of traditional preset performance methods on the feasibility conditions of initial errors, and makes it easy to set the adjustment time and channelized residual upper bound according to the task requirements.
[0038] The foregoing has provided a detailed description of a pre-set time visual servo control method and system for a dual-arm robot based on homography matrix, as provided by this invention. Specific examples have been used to illustrate the principles and implementation methods of this invention. The descriptions of the embodiments above are merely for the purpose of helping to understand the core ideas of this invention. It should be noted that those skilled in the art can make various improvements and modifications to this invention without departing from its principles, and these improvements and modifications also fall within the protection scope of the claims of this invention.
Claims
1. A pre-set time visual servo control method for a dual-arm robot based on homography matrix, characterized in that, The method includes the following steps: S100: Acquire the image of the current visual sign plane, obtain the corresponding feature points of the current visual sign plane and the fixed expected visual sign plane, and estimate the image homography matrix based on the camera intrinsic parameter matrix; S200: Based on the camera intrinsic parameter matrix, the image homography matrix is converted into a Euclidean homography matrix, and the Euclidean homography matrix is decomposed. The physically feasible solution is selected from the candidate solutions obtained by decomposition to obtain the relative rotation parameters of the current visual label plane relative to the desired visual label plane. S300: Establish the kinematic relationship of rigid grasping of the dual-arm robot and construct the mapping relationship from the object velocity to the joint velocity of the dual-arm robot; S400: Based on the relative rotation parameters, a rotation error vector is constructed using the angular axis representation. An extended translation error vector is constructed based on the image coordinates and depth information of the visual identifier center point. The original error channel is then constructed based on the rotation error vector and the extended translation error vector. S500: Construct a preset time performance boundary and its corresponding barrier function transformation for each original error channel, and convert each original error channel into a transformed error channel; S600: Design object angular velocity control law and object translation velocity control law according to each transformation error channel, and convert the combined object velocity command into dual-arm robot joint velocity command through mapping relationship to drive the robot to achieve preset time visual servo adjustment.
2. The method according to claim 1, characterized in that, S100 includes: S110: Obtain at least four pairs of non-collinear corresponding feature points on the current visual identity plane and the desired visual identity plane, and denote their pixel homogeneous coordinates as follows: and ,in, The index is the sequence number of the feature point. ,and , This represents the total number of selected corresponding feature point pairs. For the current visual identity plane, the first The pixel homogeneous coordinate vector of each feature point These are the first two visual identifiers on the current visual identity plane. The x and y coordinates of each feature point in the image coordinate system For the desired visual identity on the plane, the first The pixel homogeneous coordinate vector of each feature point These are the first two lines on the desired visual identity plane. The x and y coordinates of each feature point in the image coordinate system; S120: In the camera intrinsic parameter matrix Given the given conditions, the least squares homography matrix estimation method is used to solve for the image homography matrix. And establish the projection homography constraint relationship between the current feature point and the expected feature point: (1) in, Scale factor; It is the Euclidean homography matrix.
3. The method according to claim 2, characterized in that, S200 includes: S210: Euclidean homography matrix Scale normalization is performed, and Euclidean homography matrix decomposition based on singular value decomposition is adopted to... Decompose into satisfying The set of candidate solutions, where This is the relative rotation matrix of the current visual signage plane relative to the desired visual signage plane. For scale translation vector, Let be the unit normal vector of the desired visual identity plane; S220: Physical feasibility screening of candidate solutions obtained from Euclidean homography matrix decomposition: First, candidate solutions that do not satisfy the positive depth constraint of feature points and the plane normal direction constraint are eliminated; when multiple candidate solutions still exist after screening, a physically feasible solution is selected from the remaining candidate solutions based on the continuity of decomposition results at adjacent time steps, thus obtaining the relative rotation matrix of the current visual sign plane relative to the desired visual sign plane. Scale translation vector and the unit normal vector of the desired visual identity plane Among them, positive depth constraint means that the visual feature points corresponding to the decomposition result have positive depth in the camera coordinate system; plane normal direction constraint means that the direction of the unit normal vector obtained by decomposition is consistent with the normal direction of the expected visual label plane; continuity means that there is no abrupt change between the decomposition result at the current time and the decomposition result at the previous time.
4. The method according to claim 3, characterized in that, The S300 includes: S310: Let the joint vectors of the dual-arm robot be... ,in and These are the joint vectors for the left and right robotic arms, respectively; the corresponding joint velocity vectors are... ,in and These are the velocity vectors of the left and right robotic arm joints, respectively; the end effector velocities of the left and right robotic arms are respectively... and Its synthesis rate is ; S320: Let the object's pose be... The object's velocity is ,in Let the object's translational velocity be... Let be the angular velocity of the object. In the base coordinate system of the dual-arm robot, the end effector velocity and joint velocity of the dual-arm robot satisfy the following: (2) in, For the Jacobian matrix of the end effector of a dual-arm robot, and These are the Jacobian matrices for the left and right robotic arm end effectors, respectively. Represents a block diagonal matrix; S330: When the two robotic arms maintain a rigid grip on the object and there is no relative slippage at the gripping point, the end-effector velocity of the robotic arm and the velocity of the object satisfy the following: (3) in, For dual-arm grasping matrix, and These are the grasping sub-matrices corresponding to the left and right robotic arms, used to describe the rigid motion constraint relationship between the object's velocity and the end-effector velocities of each robotic arm; S340: From equations (2) and (3), we can obtain the mapping relationship between the object velocity and the joint velocity of the dual-arm robot: (4) in, for Moore-Penrose pseudoinverse, symbol Representing the Moore-Penrose pseudo-inverse operation, equation (4) is used to convert the object velocity command output by the object layer visual servo controller. Convert to joint velocity commands for dual-arm robot This drives the left and right robotic arms to work together to complete the desired object movement.
5. The method according to claim 4, characterized in that, The S400 includes: S410: Based on relative rotation matrix The rotation error vector is defined using the angular axis: (5) in, Let be the rotation error vector. Unit axis of rotation Let be the relative rotation angle between the current visual signage plane and the desired visual signage plane, and satisfy . Relative rotation angle As rotation angle error; and It is calculated by the following formula: (6) in, Represents the trace of a matrix. Let represent an antisymmetric matrix constructed from vectors, when When the time is right, it means that there is no relative rotation error between the current visual signage plane and the desired visual signage plane; S420: Select the center point of the visual sign as the reference point for translation error, and let the extended translation vector of the current visual sign center point be... The expected translation vector for the expansion of the visual identifier's center point is: ,in, and These represent the translation vectors of the current and desired visual identifier center points, respectively. and The image coordinates of the current visual identifier's center point. The depth of the current visual identifier center point relative to the camera coordinate system; , and Let be the image coordinates of the desired visual identifier center point and the desired depth, respectively; define the extended translation error as: (7) in, To expand the translation error vector, , and They are defined as follows: (8) in, The depth ratio between the desired depth and the current depth. and These are the horizontal pixel error and the vertical pixel error, respectively. This is the depth ratio related error.
6. The method according to claim 5, characterized in that, The S500 includes: S510: For each error channel ,set up They represent , , and ,in Indicates rotation angle error. , and These represent horizontal pixel error, vertical pixel error, and depth scale-related error, respectively. S520: Given design parameters Preset time Upper bound of target residual accuracy and scaling parameters ,in Used to adjust the error normalization scale. Used to specify the time when the error enters the target residual region. Used to specify the corresponding error channel The target residual accuracy at that time Used to adjust the rate of change of the preset time scaling function; the performance boundary generation function is defined as: (9) in, As an auxiliary variable, Used to generate performance boundaries for error channels; S530: Based on the upper limit of the target residual accuracy Calculate the final value of the preset time scaling function: (10) in, Indicates the error channel The corresponding scaling function at the preset time The final value that is then maintained makes ; S540: Construct a preset time scaling function: (11) in, For error channel The corresponding preset time scaling function, and satisfies and in Keep as ; Further define the barrier function transformation: (12) in, To normalize the error, As an intermediate variable, This refers to the error in the barrier function transformation. S550: Through equation (12), the original error channel Convert to transformation error channel ;because The performance boundary corresponding to the initial moment tends to infinity in the limiting sense. Therefore, this performance transformation does not require the initial error to fall within the finite performance boundary in advance.
7. The method according to claim 6, characterized in that, In S600, the object's angular velocity control law and translational velocity control law are designed separately for each transformation error channel, including: S610: For rotation angle error in the rotary channel According to equation (12), , and Define auxiliary variables: (13) in, This is the rotation error normalization term. This is the transformation term of the obstacle function for the rotating channel; S620: Define a Jacobian-like matrix for: (14) in, It is the identity matrix. It is the singer function, and ; S630: Select positive design parameters The angular velocity control law for the object is designed as follows: (15) in, The command for the object's angular velocity. Preset a time scaling function for the rotating channel. Let be the rotation matrix from the base coordinate system to the camera coordinate system of the dual-arm robot. for The first derivative; S640: For translation channels, the definition is: (16) in, To extend the translation error, the obstacle function transforms the error vector. These represent the barrier function transformation errors corresponding to the horizontal pixel error channel, the vertical pixel error channel, and the depth scale-related error channel, respectively. For the normalized error vector, These are the normalized errors corresponding to each translation error channel. A vector composed of the derivatives of the time scaling function for the translation channel. These are the first derivatives of the preset time scaling functions corresponding to each translation error channel; S650: Define the matrix: (17) in, This is the diagonal matrix of the transformation terms of the translation channel obstacle function. These correspond to the barrier function transformation terms for the horizontal pixel error channel, the vertical pixel error channel, and the depth scale-related error channel, respectively. Represents a diagonal matrix. This is the auxiliary gain matrix for the translation channel. The diagonal elements of the translation channel auxiliary gain matrix correspond to the product of the preset time scaling function and the error normalization term for each translation error channel, and for... have: (18) in, For each translation error channel, the barrier function transformation term is... These are intermediate variables defined for each translation error channel in the barrier function transformation. This is the error normalization term for each translation error channel; S660: Set ,in and Let be the image coordinates of the current visual identifier's center point; The matrix is determined by the camera's extrinsic parameters. for The third line defines the extended interaction matrix: (19) in, Let be the camera intrinsic parameter matrix, take The first three columns constitute a submatrix and set ,in , , Positive design parameters; The control law for the translational velocity of the object is designed as follows: (20) in, This is the command for the translation speed of the object. The depth of the current visual identifier center point relative to the camera coordinate system. for Moore-Penrose pseudo-reverse, for The inverse matrix, for The inverse matrix, This represents the element-wise product of Hadamard.
8. The method according to claim 7, characterized in that, In S600, the combined object velocity commands are converted into joint velocity commands for the dual-arm robot through a mapping relationship, including: Combine the object angular velocity command obtained from equation (15) and the object translational velocity command obtained from equation (20) into an object velocity command: (21) Substituting equation (21) into equation (4), we obtain the joint speed command for the dual-arm robot: (22) The controller will Decomposed into left arm joint velocity command and right arm joint speed command The data is then sent to the servo execution units of the left and right robotic arms, respectively, so that the two robotic arms can jointly generate the desired object speed, thereby achieving preset time visual servo pose adjustment of the manipulated object relative to the desired visual reference.
9. A pre-set time visual servo control system for a dual-arm robot based on homography matrix, used to execute the steps of the method as described in any one of claims 1-8, characterized in that, include: The image homography matrix estimation module is used to acquire the image of the current visual sign plane, obtain the corresponding feature points of the current visual sign plane and the fixed expected visual sign plane, and estimate the image homography matrix based on the camera intrinsic parameter matrix; The relative rotation parameter determination module is used to convert the image homography matrix into an Euclidean homography matrix based on the camera intrinsic parameter matrix, decompose the Euclidean homography matrix, select a physically feasible solution from the candidate solutions obtained by decomposition, and obtain the relative rotation parameters of the current visual labeling plane relative to the desired visual labeling plane. The kinematic mapping module establishes the kinematic relationship of rigid grasping by the dual-arm robot and constructs the mapping relationship between the object velocity and the joint velocity of the dual-arm robot. The error construction module is used to construct a rotation error vector based on the relative rotation parameters using the angular axis representation, and to construct an extended translation error vector based on the image coordinates and depth information of the visual identifier center point. The original error channel is then constructed based on the rotation error vector and the extended translation error vector. The transformation module is used to construct a preset time performance boundary and its corresponding barrier function transformation for each original error channel, and to convert each original error channel into a transformed error channel. The control module is used to design object angular velocity control laws and object translation velocity control laws according to each transformation error channel, and convert the combined object velocity command into dual-arm robot joint velocity command through mapping relationship to drive the robot to achieve preset time visual servo adjustment.
10. A computer device comprising a memory and a processor, wherein the memory stores a computer program, characterized in that, When the processor executes the computer program, it implements the steps of the method according to any one of claims 1 to 8.
