A Robust Visual Servo Control Method for an Anthropomorphic Manipulator with Field of View Constraints

Abstract

The present invention discloses a robust visual servo control method for an anthropomorphic manipulator with a field of view constraint. A camera sensor installed on the robot wrist collects the QR code image on the workpiece and sends it to the industrial control computer. The industrial control computer real-time identifies the image features and generates the 7DOF robot control signal, and sends this control signal to the robot to complete the tracking of the visual feature points. The shoulder, elbow, and wrist joints of the 7DOF anthropomorphic manipulator are defined by imitating the 7-degree-of-freedom human arm structure, and the arm angles are calculated. The obstacle Lyapunov function is combined with the arm angles to design a visual servo field of view constraint controller with anthropomorphic characteristics, indirectly constraining the trajectory of the feature points during tracking to remain within the camera field of view. At the same time, the manipulator can achieve anthropomorphic motion based on the rotation of the arm angles. A sliding mode torque controller is designed to drive the robot to the desired feature position. Finally, the feature points are constrained not to exceed the camera field of view, improving the success rate of the visual servo task and realizing the anthropomorphic arm angle rotation motion.

Description

Technical Field

[0001] The present invention relates to the technical field of robot vision guidance, and particularly to a robust visual servo control method for an anthropomorphic manipulator with a field of view constraint. Background Art

[0002] Many research works have been proposed for the problem of visual feature loss during the visual servo task. Some research works attempt to use path planning to avoid the image feature trajectory exceeding the camera FOV. Some scholars have designed the trajectory of the image moment defined in the virtual image plane to solve the image-based control task of quadrotors. Other scholars have overcome the disadvantages of visual servo by parameterizing the velocity screw of the camera and providing accurate initial depth through the proposed depth estimation technology, and developed a trajectory planning algorithm. Hybrid visual servo schemes have also been applied to constrain the feature points from exceeding the camera field of view. Some scholars have designed a control framework that combines the advantages of position-based and image-based visual servo to control an aircraft equipped with a robotic arm without losing features during operation. Some scholars have proposed a new wireless hybrid control algorithm for the visual servo of mobile robots, which uses the position-based visual servo (PBVS) method for global routing and the IBVS method for fine navigation. The above strategies can ensure the field of view constraint during the visual servo task, but there are a large number of online non-linear optimization problems, and their real-time feasibility in the robot system is questionable.

[0003] Barrier Lyapunov functions have been widely used in recent years to constrain the performance of non-linear systems, and the control law is directly given during the Lyapunov stability analysis. Some scholars have used BLF to ensure that a single input-output non-linear system remains within the output constraint range. Inspired by these ideas, to solve the above defects and ensure the success of the visual servo task, it is considered to combine BLF with image-based visual servo to constrain the image feature points to stay within the FOV and ensure the stability of the visual servo system.

[0004] Another problem is the system dynamics uncertainty caused by inaccurate modeling of the robot system, which affects the stability and control accuracy of the visual servo control system. Various solutions have been proposed for this problem. Sliding mode control is a feasible method to improve the robustness of the control system. Some scholars have proposed a control method that combines sliding mode control (SMC) and Takagi-Sugeno fuzzy system model, which can effectively compensate for the non-linear disturbances and uncertainties of the robot system. Some scholars have designed a fast integrated terminal sliding mode control to handle the system uncertainty of a robotic manipulator. In this study, an SMC control law combined with IBVS is proposed to handle the uncertainty of a 7-DOF redundant robot manipulator.

[0005] In view of the problems in the prior art that visual servoing has the loss of visual features, which leads to the failure of visual guidance tasks, and the uncertainty problems brought about by system modeling, the patent of the present application is proposed. Summary of the Invention

[0006] In view of the above technical problems, the present invention provides a robust visual servo control method for an anthropomorphic manipulator with a field of view constraint.

[0007] The technical solution adopted by the present invention to solve its technical problems is as follows:

[0008] A robust visual servo control method for an anthropomorphic manipulator with a field of view constraint, the method comprising the following steps:

[0009] Step S100: Calibrate the visual servo system by using the N-point method to obtain the conversion relationship between the camera and the end coordinate system;

[0010] Step S200: Establish an overall dynamic model of the robot visual servo system by combining the dynamic model of the robotic arm, the dynamic model of the visual system, and the conversion relationship between the camera and the end coordinate system;

[0011] Step S300: The camera real-time acquires the QR code image on the workpiece, sends the QR code image to the industrial control computer, the industrial control computer extracts the four corner points of the QR code image as the current feature points, and obtains the feature point position error vector according to the current feature points and the preset expected feature points;

[0012] Step S400: Select the shoulder joint, elbow joint, and wrist joint of the robotic arm by imitating the characteristics of a 7DOF human arm, calculate the magnitude of the arm angle, project the arm angle into the null space of the robotic arm by using null space projection, obtain the null space joint angular velocity through the mapping relationship between the arm angle and the null space joint angular velocity, and design a visual servo field of view constraint controller with anthropomorphic characteristics according to the null space joint angular velocity and the feature point position error vector to obtain the desired joint angular velocity;

[0013] Step S500: Acquire the current joint angular velocity, obtain the joint angular velocity error vector according to the desired joint angular velocity and the current joint angular velocity, design a robot torque controller according to the desired joint angular velocity and the current joint angular velocity error vector by combining the barrier Lyapunov function, and introduce sliding mode control to compensate for the unknown parameter variables of the robot model to obtain the control signal;

[0014] Step S600: Send the control signal to the robot to drive the robot to reach the desired feature point position and ensure that the feature point trajectory does not exceed the camera field of view range, and the robotic arm simultaneously performs a rotational movement imitating the human arm angle during the visual guidance process.

[0015] Preferably, step S100 includes:

[0016] Step S110: Prepare a calibration board with N points;

[0017] Step S120: Place the calibration board randomly under the camera;

[0018] Step S130: Use the RealSense camera to capture an image of the calibration board;

[0019] Step S140: Calculate the center pixel coordinates of the N circular dots in the image;

[0020] Step S150: Record the three-dimensional pose of the end effector of the Sawyer robotic arm in the form of quaternions;

[0021] Step S160: Repeat steps S120 - S150 a total of 12 times;

[0022] Step S170: Solve the 2D - 3D data to obtain a set of coordinate transformation relationships between the calibration board and the camera c T o ;

[0023] Step S180: According to c T o , calculate the transformation relationship between the camera and the end - effector coordinate system of the robotic arm, specifically:

[0024] B T e2 × e T c2 × c2 T o = B T e1 × e T c1 × c1 T o

[0025]

[0026] Among them, B T e1 and B T e2 are respectively the coordinate transformation relationship matrices of the two end - effector poses of the robotic arm relative to the robotic arm base, c1 T o and c2 T o are respectively the coordinate transformation relationships between the camera and the calibration board under the two different end - effector poses of the robotic arm, e T c1 and e T c2They are respectively the coordinate transformation relationships between the end effector of the robotic arm and the camera to be obtained, satisfying e T c1 = e T c2 ;

[0027] Step S190: Obtain the transformation relationship between the end of the robotic arm and the camera coordinate system according to the coordinate transformation relationship equation e T c Specifically:

[0028]

[0029] Preferably, step S200 includes:

[0030] Step S210: Obtain the transformation relationship matrix between the camera speed and the end speed through the transformation relationship between the end of the robotic arm and the camera coordinate system. Specifically:

[0031]

[0032] Step S220: There is the following relationship between the feature point speed and the camera speed:

[0033]

[0034] Among them, is the feature point speed, L is the image interaction matrix, V c is the camera speed;

[0035] There is a transformation relationship between the robotic arm joint angular velocity and the end speed. Specifically:

[0036]

[0037] Among them, V e is the speed of the end effector of the robotic arm, J q ∈R 6×n is the Jacobian matrix of the robotic arm; through the speed transformation matrix W ce between the end of the robotic arm and the camera, there is V c =W ce V e , and the dynamic model of the vision system can be obtained. Specifically:

[0038]

[0039] Among them, J s =LW ce J q is the task Jacobian matrix;

[0040] Step S230: The dynamic model of an N-link rigid robot system. Specifically:

[0041]

[0042] wherein, respectively represent the joint angle position, joint angular velocity, and joint angular acceleration variables, and τ ∈ R n is the torque control input variable, and M(q) ∈ R n×n is the inertia matrix of the robot, is the centripetal force and Coriolis force matrix, and G(q) ∈ R n represents the gravity matrix;

[0043] The dynamic model of the robotic arm can be expressed as:

[0044]

[0045] Step S240: Combine the dynamic model of the vision system to obtain the overall dynamic model of the visual servo system, specifically:

[0046]

[0047] where z is the depth information from the camera to the feature point;

[0048] Preferably, in step S300, the feature point position error vector is obtained based on the current feature point and the preset desired feature point, specifically:

[0049] z1 = s - s d

[0050] s = (u i , v i ) T , s d = (u id , v id ) T , i = 1, 2,..., m

[0051] where z1 is the feature point position error vector, s is the current feature point, and s d is the desired feature point.

[0052] Preferably, step S400 includes:

[0053] Step S410: Select the obstacle Lyapunov candidate function V1 as:

[0054]

[0055] where, k a1 = [k a11 , k a12 ,..., k a1i T ​, where \(i = 1, 2, \cdots, 2m\) is the error vector of the image feature points \(z_1=[z 11 ,z 12 ,\cdots,z 1v T \in\mathbb{R} v , and \(v = 2m\) is the constraint boundary;

[0056] Step S420: Differentiate \(V_1\) with respect to time to obtain:

[0057]

[0058] The derivative of the error of the feature point \(z_1\) with respect to time is Substitute into and introduce the image feature position constraint, and design \(\alpha\) as:

[0059]

[0060] where is the Moore - Penrose pseudoinverse form of the task Jacobian matrix \(J s \), and \(k_1=[k 11 ,k 12 ,\cdots,k 1i T , where \(i = 1, 2, \cdots, n\) are positive constants;

[0061] Step S430: Select joint 1 of the Sawyer seven - degree - of - freedom anthropomorphic robotic arm as the shoulder joint (S), joint 3 as the elbow joint (E), and joint 6 as the wrist joint (W), and calculate the arm angle \(\psi\):

[0062]

[0063] where represents the vector from the base of the robotic arm to the shoulder, is the vector of the robotic arm from the shoulder to the elbow, is the vector of the robotic arm from the shoulder to the wrist, is the vector from the elbow to the wrist of the robot;

[0064] Step S440: Calculate the linear velocity direction vector \(l\) of the arm angle plane formed by the three joints mentioned in Step S430 ψ \in\mathbb{R} 3×1 , specifically:

[0065]

[0066] Step S450: Use the mapping relationship \(J E \) between the arm angle and the null - space joint angular velocity to obtain the null - space joint angular velocity expression:​​

[0067]

[0068] Among them, J E ∈R 3×3 represents the mapping relationship between the arm angle and the null-space joint velocity. This mapping relationship J E is calculated from the elbow of the robot to the base, and is the differential of the arm angle ψ;

[0069] Step S460: Through the null-space projection relationship of the robot Jacobian matrix, obtain a visual servo field-of-view constraint controller with arm-angle motion, and rewrite the visual servo field-of-view constraint controller α as:

[0070]

[0071] where I ∈ R 6×n is the identity matrix;

[0072] Preferably, step S500 includes:

[0073] Step S510: Select the obstacle Lyapunov candidate function V2 as follows:

[0074]

[0075] Step S520: Differentiate V2 with respect to time to obtain an expression for analyzing the system stability and subsequent design of the torque controller:

[0076]

[0077] Step S530: According to the desired joint angular velocity and the current joint angular velocity error vector, design the robot torque controller τ in combination with the obstacle Lyapunov function to stabilize the system:

[0078]

[0079] Step S540: Use the visual servo-based sliding mode control to compensate for the unknown parameter terms of the robot in the torque controller to enhance the system robustness. Design a sliding surface based on the visual servo field-of-view constraint controller, and the sliding mode torque controller is designed as:

[0080]

[0081] where, and k2 = [k 21 , k 22 ,..., k 2i T , i = 1, 2,..., n are positive constants.​

[0082] The above-mentioned robust visual servo control method for an anthropomorphic manipulator with a field of view constraint, where the camera sensor is used to collect the QR code image on the workpiece and send it to the industrial control computer; the industrial control computer real-time identifies the image features and generates 7DOF robot control signals, and sends the control signals to the robot through the local area network to complete the tracking of the visual feature points; in order to endow the robotic arm with human-like characteristics, imitate the 7-degree-of-freedom arm structure of humans, define the shoulder joint, elbow joint and wrist joint of the 7DOF anthropomorphic robotic arm, and calculate the arm angle; combine the obstacle Lyapunov function with the arm angle to design a visual servo field of view constraint controller with human-like characteristics, indirectly constrain the feature point trajectory during tracking to remain within the camera field of view, and at the same time the robotic arm can achieve human-like motion based on the rotation of the arm angle; design a sliding mode (SMC) torque controller to drive the robot to the desired feature position, and the sliding mode control is used to handle the system uncertainties brought by the robot modeling and the camera system model reconstruction, enhancing the robustness of the system; finally, the present invention can constrain the feature points not to exceed the camera field of view, improve the success rate of the visual servo task, and achieve human-like arm angle rotation motion, providing more possibilities for human-robot collaboration. Description of the Drawings

[0083] Figure 1 It is a flowchart of a robust visual servo control method for an anthropomorphic manipulator with a field of view constraint in an embodiment of the present invention;

[0084] Figure 2 It is an architecture diagram of the software and hardware platform of the visual servo system in an embodiment of the present invention;

[0085] Figure 3 It is a schematic diagram of the arm angle defined on the Sawyer robot in an embodiment of the present invention;

[0086] Figure 4 It is a schematic diagram of the execution effect of the visual servo field of view constraint control method in an embodiment of the present invention. Among them, Figure (a) is the execution effect under the classical visual servo control method and the PID control method, and Figure (b) is the execution effect under the visual servo field of view constraint controller and the sliding mode torque controller;

[0087] Figure 5 It is a curve of the change of the arm angle value in an embodiment of the present invention. Detailed Embodiments

[0088] In order to enable those skilled in the art to better understand the technical solutions of the present invention, the present invention will be further described in detail below with reference to the accompanying drawings.

[0089] In one embodiment, as Figure 1 shown, a robust visual servo control method for an anthropomorphic manipulator with a field of view constraint, the method includes the following steps:

[0090] Step S100: Calibrate the visual servo system using the N - point method to obtain the transformation relationship between the camera and the end - effector coordinate system.

[0091] In one embodiment, step S100 includes:

[0092] Step S110: Prepare a calibration board with N points;

[0093] Step S120: Place the calibration board randomly under the camera;

[0094] Step S130: Use the RealSense camera to capture an image of the calibration board;

[0095] Step S140: Calculate the pixel coordinates of the centers of the N circular dots in the image;

[0096] Step S150: Record the three - dimensional pose of the end - effector of the Sawyer manipulator in the form of quaternions at this time;

[0097] Step S160: Repeat steps S120 - S150 a total of 12 times;

[0098] Step S170: Solve the 2D - 3D data to obtain a set of coordinate transformation relationships between the calibration board and the camera c T o ;

[0099] Step S180: According to c T o , calculate the transformation relationship between the camera and the end - effector coordinate system of the manipulator. Specifically:

[0100] B T e2 × e T c2 × c2 T o = B T e1 × e T c1 × c1 T o

[0101]

[0102] Among them, B T e1 and B T e2 are respectively the coordinate transformation relationship matrices of the two end - effector poses of the manipulator relative to the manipulator base, c1 T o and c2 T oThey are the coordinate transformation relationships between the camera and the calibration board under two different end poses of the robotic arm respectively. e T c1 and e T c2 They are the coordinate transformation relationships between the end effector of the robotic arm and the camera to be obtained, satisfying e T c1 = e T c2 .

[0103] Step S190: Obtain the transformation relationship between the end of the robotic arm and the camera coordinate system according to the coordinate transformation relationship equation e T c , specifically:

[0104]

[0105] Furthermore, before step S100, there is also step S000: Build the software and hardware platform of the visual servo system. Specifically, as Figure 2 shown, the end effector of the Sawyer robot is equipped with a RealSense D435i camera, adopting the eye-in-hand structure. The vision part includes a RealSense D435i camera, an AprilTag two-dimensional code, and a visual servo platform (ViSP). The AprilTag image is captured online by the RealSense D435i camera and sent to ViSP. ViSP decodes the tag, extracts the corner coordinates of the AprilTag as feature points, and calculates the control law of the vision part. The torque level controller of the Sawyer robot is implemented through Intera_SDK. Intera_SDK and ViSP run on a computer equipped with Ubuntu 18.04 and ROS melodic. Due to programming language incompatibility, ROS topics are used to transmit the data interacted between the two programming platforms.

[0106] Step S200: Establish the overall dynamic model of the robot visual servo system by combining the dynamic model of the robotic arm, the dynamic model of the visual system, and the transformation relationship between the camera and the end coordinate system.

[0107] In one embodiment, step S200 includes:

[0108] Step S210: Obtain the transformation relationship matrix between the camera speed and the end speed through the transformation relationship between the end of the robotic arm and the camera coordinate system, specifically:

[0109]

[0110] Step S220: There is the following relationship between the feature point speed and the camera speed:

[0111]

[0112] Among them, is the velocity of the feature point, L is the image interaction matrix, and V c is the camera velocity;

[0113] There is a conversion relationship between the angular velocity of the robotic arm joint and the end velocity, specifically:

[0114]

[0115] Among them, V e is the velocity of the end effector of the robotic arm, and J q ∈R 6×n is the Jacobian matrix of the robotic arm; through the velocity conversion matrix W ce between the end of the robotic arm and the camera, there is V c =W ce V e , and the dynamic model of the vision system can be obtained, specifically:

[0116]

[0117] Among them, J s =LW ce J q is the task Jacobian matrix;

[0118] Step S230: The dynamic model of an N-link rigid robotic system, specifically:

[0119]

[0120] Among them, respectively represent the joint angle position, joint angular velocity, and joint angular acceleration variables, τ∈R n is the torque control input variable, M(q)∈R n×n is the inertia matrix of the robot, is the centripetal force and Coriolis force matrix, and G(q)∈R n represents the gravity matrix;

[0121] The dynamic model of the robotic arm can be expressed as:

[0122]

[0123] Step S240: Combining the dynamic model of the vision system, the overall dynamic model of the visual servo system is obtained, specifically:

[0124]

[0125] Among them, z is the depth information from the camera to the feature point;

[0126] Step S300: The camera obtains the QR code image on the workpiece in real time, sends the QR code image to the industrial control computer. The industrial control computer extracts the four corner points of the QR code image as the current feature points, and obtains the feature point position error vector according to the current feature points and the preset expected feature points.

[0127] In one embodiment, in step S300, obtaining the feature point position error vector according to the current feature points and the preset expected feature points is specifically:

[0128] z1 = s - s d

[0129] s = (u i , v i ) T , s d = (u id , v id ) T , i = 1, 2,..., m. Where z1 is the feature point position error vector, s is the current feature point, and s d is the expected feature point.

[0130] Specifically, z1 = [z 11 , z 12 ,..., z 1v ) T ∈ R v , v = 2m, where z 1(v-1) = u m - u md , z 1v = v m - v md , so the dimension of s = (u i , v i ) T is m, and the dimension of z is 2m.

[0131] For the QR code image captured under the camera's field of view, use the VISP visual servo algorithm library to process the AprilTag image and extract the four corner points of the QR code as feature points.

[0132] Step S400: Select the shoulder joint, elbow joint, and wrist joint of the robotic arm by imitating the characteristics of a 7DOF human arm, calculate the magnitude of the arm angle, project the arm angle into the null space of the robotic arm using null space projection, obtain the null space joint angular velocity through the mapping relationship between the arm angle and the null space joint angular velocity, and design a vision servo field-of-view constraint controller with human-like characteristics according to the null space joint angular velocity and the feature point position error vector to obtain the desired joint angular velocity.

[0133] In one embodiment, step S400 includes:

[0134] Step S410: Select the obstacle Lyapunov candidate function V1 as:

[0135]

[0136] where k a1 = [k a11 , k a12 ,..., k a1i T , i = 1, 2,..., 2m is the image feature point error vector z1 = [z 11 , z 12 ,..., z 1v T ∈ R v , and v = 2m is the constraint boundary.

[0137] Specifically, to promote the process of the constrained IBVS task, an IBVS control strategy is designed using the obstacle Lyapunov function and a BLF candidate function is selected.

[0138] Step S420: Differentiate V1 with respect to time to obtain:

[0139]

[0140] The derivative of the feature point z1 error with respect to time is Substitute it into and introduce the image feature position constraint to design α as:

[0141]

[0142] where is the Moore-Penrose pseudoinverse form of the task Jacobian matrix J s , k1 = [k 11 , k 12 ,..., k 1i T , i = 1, 2,..., n are positive constants;

[0143] Step S430: Select joint 1 of the Sawyer seven-degree-of-freedom anthropomorphic robotic arm as the shoulder joint (S), joint 3 as the elbow joint (E), and joint 6 as the wrist joint (W), and calculate the arm angle ψ:

[0144]

[0145] where represents the vector from the base of the robotic arm to the shoulder, is the vector of the robotic arm from the shoulder to the elbow, is the vector of the robotic arm from the shoulder to the wrist,​​​ is the vector from the elbow to the wrist of the robot.

[0146] Specifically, Figure 3 For the schematic diagram of the arm angle, in order to make the redundant manipulator have humanoid characteristics while completing the visual servo task with FOV constraints, the arm angle is used to complete humanoid control. The arm angle ψ is composed of the included angle formed by the actual plane (SEW) and the reference plane (BSW) around the axis formed.

[0147] Step S440: Calculate the linear velocity direction vector l of the arm angle plane formed by the three joints mentioned in step S430 ψ ∈R 3×1 , specifically:

[0148]

[0149] Step S450: Utilize the mapping relationship J E between the arm angle and the null space joint angular velocity to obtain the null space joint angular velocity expression:

[0150]

[0151] where J E ∈R 3×3 represents the mapping relationship between the arm angle and the null space joint velocity. This mapping relationship J E is calculated from the elbow of the robot to the base, and is the differential of the arm angle ψ.

[0152] For the visual servo task, a visual servo field-of-view constraint controller is proposed to track the required feature point coordinates and limit them within the camera FOV, which is expressed as:

[0153]

[0154] where k1 = [k 11 , k 12 ,..., k 1i T , i = 1, 2,..., n are positive constants, represents the joint velocity required to drive the manipulator to the specified position.

[0155] Step S460: Through the null space projection relationship of the robot Jacobian matrix, obtain the visual servo field-of-view constraint controller with arm angle motion, and rewrite the visual servo field-of-view constraint controller α as:

[0156]

[0157] ​where \(I\in R\) 6×n is the identity matrix; specifically, a visual servo field constraint controller with humanoid arm angle motion is designed as above, which can stabilize the visual servo system and constrain the image feature points in the camera field of view, and introduce humanoid arm angle rotation motion.

[0158] Step S500: Obtain the current joint angular velocity, obtain the joint angular velocity error vector based on the desired joint angular velocity and the current joint angular velocity, design a robot torque controller in combination with the obstacle Lyapunov function according to the desired joint angular velocity and the current joint angular velocity error vector, and introduce sliding mode control to compensate for the unknown parameter variables of the robot model to obtain a control signal.

[0159] Specifically, obtaining the joint angular velocity error vector based on the desired joint angular velocity and the current joint angular velocity is specifically:

[0160]

[0161] where is the current joint angular velocity, \(\alpha\) is the desired joint angular velocity, and \(z_2\) is the joint angular velocity error vector.

[0162] In one embodiment, step S500 includes:

[0163] Step S510: Select the obstacle Lyapunov candidate function \(V_2\) as follows:

[0164]

[0165] Step S520: Differentiate \(V_2\) with respect to time to obtain an expression for analyzing the system stability and subsequent design of the torque controller:

[0166]

[0167] Step S530: Design a robot torque controller \(\tau\) to stabilize the system in combination with the desired joint angular velocity and the joint angular velocity error vector and the obstacle Lyapunov function:

[0168]

[0169] Specifically, the term in the torque controller is unknown, which will lead to poor control performance. There is a parameter vector based on the robot manipulator to satisfy where is the regression matrix of known joint variables, with an upper bound satisfying is the unknown constant parameter vector describing the mass of the robotic arm, with an upper bound satisfying To overcome this problem and improve the control accuracy, define as the sliding mode surface.

[0170] Step S540: Use the sliding mode control based on visual servo to compensate for the unknown parameter terms of the robot in the torque controller Enhance the system robustness, and design the sliding mode surface based on the visual servo field-of-view constraint controller as The sliding mode torque controller is designed as:

[0171]

[0172] where and k2 = [k 21 , k 22 ,..., k 2i T , i = 1, 2,..., n are positive constants.

[0173] Step S600: Send the control signal to the robot, drive the robot to reach the desired feature point position and ensure that the feature point trajectory does not exceed the camera field-of-view range, and the robotic arm simultaneously performs a rotational motion imitating the human arm angle during the visual guidance process.

[0174] Specifically, write the relevant program in the industrial control computer according to the designed controller to calculate the control variable. First, pack the currently extracted feature point image coordinates and the image interaction matrix L in the form of a ROS topic and send them to the Intera_SDK programming platform, write a visual servo field-of-view constraint controller and a sliding mode torque controller with humanoid behavior, debug the robot to find the appropriate control parameters and constraint boundaries, and finally send the control quantity to the robotic arm through the local area network to control the robot to reach the desired feature point position and simultaneously complete the humanoid arm angle rotational motion.

[0175] Figure 4 This is the experimental result display. Figure 4 (a) represents the visual servo guidance experiment using the classical visual servo and PID torque controller. The dashed box represents the camera field-of-view range. It can be seen that both feature points 1 and 2 exceed the field-of-view range, indicating the failure of the visual servo task. Figure 4 (b) is the verification result of the present invention. All feature points are constrained within the camera field-of-view range, verifying the effectiveness of the present invention. Figure 5 This is the curve of the arm angle value during the arm angle movement.

[0176] ​Compared with the prior art, the advantages of the present invention are as follows: (1) Develop a humanoid control algorithm combined with image-based visual servo (IBVS) control. Through the manipulator kinematics, the rotational motion similar to that of a human arm is realized, enabling human-robot collaboration to be applied to most typical industrial serial manipulators; (2) An obstacle Lyapunov function is innovatively introduced into the design of visual servo control. Using a field of view constraint controller, the control variables of the visual servo system are all restricted within the constraint range formed by the BLF while satisfying the field of view (FOV) constraint, improving the success rate of visual servo tasks; (3) Aiming at the dynamic uncertainty existing in the 7-degree-of-freedom redundant manipulator system, while improving the stability of the visual servo system, an IBVS combined sliding mode control law is proposed, improving the robustness of the robot visual servo system.

[0177] The present invention discloses a robust visual servo control method for an anthropomorphic manipulator applicable to field of view constraints and arm angle motion. The system consists of a 7-degree-of-freedom (DOF) manipulator, a camera sensor (eye-in-hand) installed on the end effector, and an industrial control computer. The camera sensor is used to collect the QR code image on the workpiece and send it to the industrial control computer; the industrial control computer real-time identifies the image features and generates 7DOF robot control signals, and sends the control signals to the robot through a local area network to complete the tracking of visual feature points; in order to endow the manipulator with humanoid characteristics and imitate the 7-degree-of-freedom arm structure of humans, the shoulder joint, elbow joint, and wrist joint of the 7DOF anthropomorphic manipulator are defined, and the arm angle is calculated; an obstacle Lyapunov function is combined with the arm angle to design a visual servo field of view constraint controller with humanoid characteristics, indirectly constraining the feature point trajectory to remain within the camera field of view during the tracking process, and at the same time the manipulator can realize humanoid motion based on arm angle rotation; a sliding mode (SMC) torque controller is designed to drive the robot to the desired feature position, and the sliding mode control is used to handle the system uncertainties brought by robot modeling and camera system model reconstruction, enhancing the robustness of the system; finally, the present invention can constrain the feature points not to exceed the camera field of view, improve the success rate of visual servo tasks, and realize humanoid arm angle rotation motion, providing more possibilities for human-robot collaboration.

[0178] The above has introduced in detail a robust visual servo control method for an anthropomorphic manipulator with field of view constraints provided by the present invention. Specific examples are used in this article to elaborate on the principle and implementation manner of the present invention. The description of the above embodiments is only used to help understand the core idea of the present invention. It should be noted that for those of ordinary skill in the art of this technology, without departing from the principle of the present invention, several improvements and modifications can be made to the present invention, and these improvements and modifications also fall within the protection scope of the claims of the present invention.

Claims

1. A robust visual servo control method for an anthropomorphic manipulator with a field of view constraint, characterized in that The method includes the following steps: Step S100: Calibrate the visual servo system using the N-point method to obtain the transformation relationship between the camera and the end-effector coordinate system; Step S200: Establish the overall dynamic model of the robot visual servo system by combining the dynamic model of the robotic arm, the dynamic model of the vision system, and the transformation relationship between the camera and the end-effector coordinate system; Step S300: The camera real-time acquires the QR code image on the workpiece, sends the QR code image to the industrial control computer, the industrial control computer extracts the four corner points of the QR code image as the current feature points, and obtains the feature point position error vector according to the current feature points and the preset desired feature points; Step S400: Select the shoulder joint, elbow joint, and wrist joint of the robotic arm by imitating the characteristics of a 7DOF human arm, calculate the magnitude of the arm angle, project the arm angle into the null space of the robotic arm using null space projection, obtain the null space joint angular velocity through the mapping relationship between the arm angle and the null space joint angular velocity, and design a vision servo field-of-view constraint controller with human-like characteristics according to the null space joint angular velocity and the feature point position error vector to obtain the desired joint angular velocity; Step S500: Acquire the current joint angular velocity, obtain the current joint angular velocity error vector according to the desired joint angular velocity and the current joint angular velocity, design a robot torque controller according to the desired joint angular velocity and the current joint angular velocity error vector by combining the barrier Lyapunov function, and introduce sliding mode control to compensate for the unknown parameter variables of the robot model to obtain the control signal; Step S600: Send the control signal to the robot, drive the robot to reach the desired feature point position and ensure that the feature point trajectory does not exceed the camera field-of-view range, and the robotic arm simultaneously performs a rotational motion imitating the human arm angle during the visual guidance process.

2. The method according to claim 1, characterized in that, Step S100 includes: Step S110: Prepare a calibration board with N points; Step S120: Place the calibration board randomly under the camera; Step S130: Use the RealSense camera to capture the calibration board image; Step S140: Calculate the center pixel coordinates of the N circular dots in the image; Step S150: Record the three-dimensional pose of the end-effector of the Sawyer robotic arm in the form of quaternion; Step S160: Repeat steps S120 - S150 a total of 12 times; Step S170: Solve the 2D-3D data to obtain a set of coordinate transformation relationships between the calibration board and the camera c T o ; Step S180: According to the c T o , calculate the conversion relationship between the camera and the end - effector coordinate system of the robotic arm, specifically: B T e2 × e T c2 × c2 T o = B T e1 × e T c1 × c1 T o Among them, B T e1 and B T e2 are respectively the coordinate transformation relation matrices of the end poses of two groups of the robotic arm with respect to the robotic arm base, c1 T o and c2 T o are respectively the coordinate transformation relations between the camera and the calibration board under two different end poses of the robotic arm, e T c1 and e T c2 are respectively the coordinate transformation relations between the end effector of the robotic arm and the camera to be obtained, satisfying e T c1 = e T c2 ; Step S190: Obtain the conversion relationship between the end of the robotic arm and the camera coordinate system according to the coordinate transformation relation equation e T c , specifically:

3. The method according to claim 1, characterized in that Step S200 includes: Step S210: Obtain the transformation relationship matrix between the camera velocity and the end-effector velocity through the transformation relationship between the end of the robotic arm and the camera coordinate system, specifically: Step S220: There is the following relationship between the feature point velocity and the camera velocity: Among them, is the feature point velocity, L is the image interaction matrix, V c is the camera velocity; There is a transformation relationship between the robotic arm joint angular velocity and the end-effector velocity, specifically: Among them, V e is the speed of the end effector of the robotic arm, and J q ∈R 6×n is the Jacobian matrix of the robotic arm; through the velocity transformation matrix W ce between the end of the robotic arm and the camera, we have V c = W ce V e , and the dynamic model of the vision system can be obtained, specifically as follows: Among them, J s = LW ce J q and J s is the task Jacobian matrix; Step S230: The dynamic model of the N-link rigid robot system, specifically: Among them, respectively represent the joint angle position, joint angular velocity, and joint angular acceleration variables, τ ∈ R n is the torque control input variable, M(q) ∈ R n×n is the inertia matrix of the robot, is the centripetal force and Coriolis force matrix, G(q) ∈ R n represents the gravity matrix; The dynamic model of the robotic arm can be expressed as: Step S240: Combine the dynamic model of the vision system to obtain the overall dynamic model of the visual servo system, specifically: where z is the depth variable from the camera to the feature point.

4. The method according to claim 3, wherein In step S300, a feature point position error vector is obtained based on the current feature point and a preset desired feature point, specifically as follows: z1 = s - s d s=(u i ,v i ), T ,s d =(u id ,v id ), T ,i = 1, 2, ..., m Among them, z1 is the feature point position error vector, s is the current feature point, and s d is the expected feature point.

5. The method according to claim 4, wherein Step S400 includes: Step S410: Select the obstacle Lyapunov candidate function V1 as: where k a1 =[k a11 , k a12 ,..., k a1i T , i = 1, 2,..., 2m is the constraint boundary of the image feature point error vector z1 = [z 11 , z 12 ,..., z 1v T ∈R v , v = 2m;​​ Step S420: Differentiate V1 with respect to time to obtain: The derivative of the feature point z1 error with respect to time is Substitute into And introduce the image feature position constraint, and design α as: Among them is the Moore-Penrose pseudoinverse form of the task Jacobian matrix J s , k1 = [k 11 , k 12 ,..., k 1i T , where i = 1, 2,..., n are positive constants;​ Step S430: Select joint 1 of the Sawyer seven-degree-of-freedom anthropomorphic robotic arm as the shoulder joint (S), joint 3 as the elbow joint (E), and joint 6 as the wrist joint (W), and calculate the arm angle ψ: Among them, represents the vector from the base of the robotic arm to the shoulder, is the vector of the robotic arm from the shoulder to the elbow, is the vector of the robotic arm from the shoulder to the wrist, is the vector from the elbow of the robot to the wrist; Step S440: Calculate the linear velocity direction vector l of the arm angle plane formed by the three joints mentioned in step S430 ψ ∈R 3×1 , specifically: Step S450: Using the mapping relationship J between the arm angle and the null space joint angular velocity E , obtain the null space joint angular velocity expression: where, J E ∈R 3×3 represents the mapping relationship between the arm angle and the null space joint velocity, and this mapping relationship J E is calculated from the elbow of the robot to the base, is the differential of the arm angle ψ; Step S460: Through the null space projection relationship of the robot Jacobian matrix, obtain a visual servo field-of-view constraint controller with arm angle motion, and rewrite the visual servo field-of-view constraint controller α as: where \(I\in\mathbb{R}\) 6×n is the identity matrix.

6. The method according to claim 5, wherein Step S500 includes: Step S510: Select the obstacle Lyapunov candidate function V2 as follows: Step S520: Differentiate V2 with respect to time to obtain an expression for analyzing system stability and subsequent design of the torque controller: Step S530: According to the desired joint angular velocity and the current joint angular velocity error vector, design a robotic arm torque controller τ in combination with the obstacle Lyapunov function to stabilize the system: Step S540: Compensate for the unknown parameter terms of the robot in the torque controller using vision servo-based sliding mode control Enhance the system robustness. Design a sliding mode surface based on the vision servo field-of-view constraint controller. The sliding mode torque controller is designed as follows: Among them, and k2 = [k 21 , k 22 ,..., k 2i T , where i = 1, 2,..., n are positive constants.​

Images