Visual servo control method for power robot

Abstract

The invention relates to a visual servo control method for an electric robot, and the method comprises the following steps: obtaining a color image around a mechanical arm of the electric robot in real time, and the color image comprises an Aruco code of a target position; acquiring internal parameters of a camera, and calculating target position pixel coordinates under a coordinate system of the power robot by combining the image information of the target and the pose of an end effector of the power robot; detecting an Aruco code in the color image in real time, and calculating a new pose of the end effector of the power robot according to the target position pixel coordinate and the current position pixel coordinate under the coordinate system of the power robot, so that the end effector of the power robot moves from the current position; and all the steps are repeated until an end condition is met, and the control process is completed. Compared with the prior art, the working efficiency of the power robot is improved.

Description

Technical Field

[0001] The invention relates to the technical field of robot control, and in particular to a visual servo control method for an electric robot. Background Art

[0002] As the scale of the power grid continues to increase in high voltage and high magnetic field environments, misoperation increases, personnel capacity is insufficient, and safety risks become more prominent, threatening personnel safety. In this context, power inspection robots are being applied to a wider range of power scenarios. The traditional human-machine co-existence in an insulated cabin operation is no longer able to meet the requirements of control performance and accuracy in live working environments.

[0003] Existing remote-controlled robots face many challenges. For example, the uncertainty problem in visual servoing. In the operation links such as positioning, grasping and moving, the target object occlusion leads to positioning failure. At the same time, it is restricted by the accuracy of the sensor and the stability of the control system, and the fine work cannot be completed. When dealing with inspection conditions, it is difficult for workers to adapt to the noise interference in different inspection environments and different working environments during inspections. Visual feedback may have delays or errors, and the dynamic planning obstacle avoidance ability is poor and it is difficult to cope with scene changes and emergencies, which will affect the robot's ability to perceive the environment in real time and respond. At present, in the process of remote control switching operation, only visual images can be provided to the control end, which will make the operator of the control end overly dependent on visual images and easily cause visual fatigue. In addition, due to the deviation of manual observation of visual images, the operator of the control end may make mistakes, which will not only reduce the operator's work efficiency, but also may cause accidents. For example, when the operator of the control end remotely controls the robot to perform switching operations based on visual images, if the operating tool continues to rub after contacting the high-voltage cabinet, the operator will not be able to continue to control the robot based on the visual image, which will not only reduce the operator's work efficiency, but also have the potential to cause safety accidents. More importantly, existing technical solutions cannot achieve autonomous operation of robots, and robots rely too much on human intervention during operation, which greatly limits the working efficiency and application scope of robots working with live wires. At present, robots working with live wires have significant shortcomings in comprehensive processing capabilities.

[0004] At present, substation operation and maintenance lacks efficient and reliable intelligent systems and safety guarantees. Summary of the invention

[0005] The purpose of the present invention is to provide an operating method for visual servo control of an electric robot to improve the working efficiency of the electric robot in a scene where depth information changes.

[0006] The purpose of the present invention can be achieved by the following technical solutions:

[0007] A visual servo control method for an electric robot comprises the following steps:

[0008] Acquire a color image around the power robot arm in real time, wherein the color image contains an Aruco code of a target position;

[0009] Obtaining the camera internal parameters, and calculating the pixel coordinates of the target position in the electric robot coordinate system by combining the image information of the target and the position and posture of the electric robot end effector;

[0010] Detecting the Aruco code in the color image in real time, and calculating the new posture of the end effector of the electric robot according to the pixel coordinates of the target position and the pixel coordinates of the current position in the electric robot coordinate system, so that the end effector of the electric robot moves from the current position;

[0011] Repeat all the above steps until the end condition is reached and the control process is completed.

[0012] Furthermore, the step of calculating the pixel coordinates of the target position in the electric robot coordinate system includes:

[0013] Based on the camera intrinsic parameters, calculate the target position coordinates in the camera coordinate system;

[0014] According to the posture of the end effector of the electric robot, the target position coordinates in the camera coordinate system are converted into the target position pixel coordinates in the electric robot coordinate system through the inverse transformation matrix, wherein the expression of the inverse transformation matrix is:

[0015]

[0016] In the formula, is the inverse transformation matrix, R is the rotation matrix, and t is the translation vector.

[0017] Furthermore, the step of calculating the target position coordinates in the camera coordinate system includes:

[0018] According to the depth value and the pixel coordinates (u, v) of the target position, the mapping relationship is:

[0019]

[0020] According to the camera intrinsic parameters, the pixel coordinates (u, v) of the target position are converted to the camera coordinate system to obtain the target position coordinates in the camera coordinate system:

[0021]

[0022] Z=z

[0023] Where (X, Y, Z) is the three-dimensional coordinate in the camera coordinate system, f x 、f y 、u 0 、v0 is the camera internal parameter, where f x 、f y is the focal length of the camera, u 0 、v 0 are the horizontal and vertical coordinates of the principal point of the image, and z is the distance from the camera to the target position.

[0024] Furthermore, the step of calculating the new posture of the end effector of the electric robot includes:

[0025] Calculating the error between the pixel coordinates of the target position and the pixel coordinates of the current position in the electric robot coordinate system;

[0026] Based on the error, calculating a control gain using an adaptive gain controller;

[0027] Calculate the motion increment that the end effector of the electric robot needs to perform based on the camera parameters and the control gain;

[0028] The motion increment is converted into a posture increment of the end effector of the electric robot, and a new posture of the end effector of the electric robot is obtained according to the posture increment.

[0029] Furthermore, the Euclidean distance is used to calculate the error, and the calculation expression of the error is:

[0030] norm_error=||(u c -u goal ,v c -v goal )|| 2

[0031] Where norm_error is the error, (u c ,v c ) is the pixel coordinate of the current position, (u goal ,v goal ) is the pixel coordinate of the target position.

[0032] Furthermore, the calculation expression of the control gain is:

[0033]

[0034] In the formula, λ x is the control gain, λ 0 is the initial gain, also known as control strength, λ ∞ is the lower limit of the gain, λ' ∞ is the adjustment factor of the control gain, and norm_error is the error.

[0035] Furthermore, the expression of the action increment is:

[0036]

[0037] In the formula, a is the increment, Δu v is the action increment, C ij is a coefficient matrix, which represents the effect of the change of the camera coordinate system on the pixel coordinates, f x 、f y 、u 0 、v 0 is the camera internal parameter, where f x 、f y is the focal length of the camera, u 0 、v 0 are the horizontal and vertical coordinates of the main point of the image, z is the distance from the camera to the target position, (u c ,v c ) is the pixel coordinate of the current position.

[0038] Furthermore, the expression of the new posture of the end effector of the electric robot is:

[0039] T robot_new =T robot_current ΔT robot

[0040] Where, T robot_new is the new position, T robot_current is the current posture, ΔT robot is the pose increment.

[0041] Furthermore, the end condition is that the error between the pixel coordinates of the target position and the pixel coordinates of the current position in the electric robot coordinate system is less than or equal to a set threshold. When the end condition is met, it means that the end effector of the electric robot moves to the target position, wherein the expression of the end condition is:

[0042]

[0043] Where end_location is the end condition, u c [i][j] are the coordinates of the corner point at the current position, u goal [i][j] are the coordinates of the corner points of the target position, and threshold is the set threshold.

[0044] Furthermore, when the robot end effector moves from the current position, linear interpolation is used for smooth transition:

[0045] T robot_final =T robot +ΔT robot

[0046] Where, T robot_finalis the final position of the robot end effector, T robot is the rotation matrix, Δt robot is the pose increment.

[0047] Compared with the prior art, the present invention has the following beneficial effects:

[0048] (1) To solve the problem of target tracking with constantly changing depth information, the present invention adopts depth images to obtain depth values ​​and color images to continuously detect the Aruco code of the target position, so that the electric robot end effector can continuously update its posture toward the target position, thereby improving the flexibility of the electric robot end effector and further improving work efficiency.

[0049] (2) During the posture updating process of the present invention, the parameters of the control system can be dynamically adjusted according to the real-time motion state of the target object and environmental changes. The introduction of the adaptive gain controller is real-time compared to the traditional gain setting. It can automatically optimize the control gain by detecting system errors and environmental changes in real time, effectively avoiding control failures caused by environmental changes or error accumulation, thereby achieving more accurate trajectory tracking and dynamic adjustment. This control strategy can not only ensure that the robot arm can work efficiently under different tasks and operating conditions, but also maintain stability under fast movement or large interference. Especially in a changeable industrial environment, it can effectively improve the robustness and response speed of the system, ensuring that the robot arm can accurately control the target posture.

[0050] (3) The present invention adopts uncalibrated visual servo control, adaptive parameter estimation, and depth-independent Jacobian matrix estimation methods, which can not only compensate for the uncalibrated camera parameters and the depth information of unknown changing motion, and improve the robustness of motion in power scenes, but also have no constraints on depth information, reduce redundant motion, and help reduce the time it takes for the camera to reach the target, thereby improving the accuracy and efficiency of control. BRIEF DESCRIPTION OF THE DRAWINGS

[0051] Figure 1 It is a schematic diagram of the method flow of the present invention. DETAILED DESCRIPTION

[0052] The present invention is described in detail below in conjunction with the accompanying drawings and specific embodiments. This embodiment is implemented based on the technical solution of the present invention, and provides a detailed implementation method and specific operation process, but the protection scope of the present invention is not limited to the following embodiments.

[0053] This embodiment provides a visual servo control method for an electric robot. Figure 1 As shown, the method comprises the following steps:

[0054] Step S1: Initialize the JAKAS7 robotic arm and the RealSense Depth camera D457 to capture visual information around the robotic arm in real time.

[0055] The specific steps are as follows:

[0056] Step S1, connect the JAKAS7 robot controller through the jkrc library. Use Python scripts to communicate with the robot arm and collect the robot's joint angle and posture information in real time. The robot's TCP (work center point) posture can be updated and controlled in real time in the program, and converted into meters until the current position of the robot's end effector is printed out. Initialize the RealSense camera, and then set the camera parameters to ensure that the depth stream and RGB video stream frame rates are consistent. Use the API provided by the camera SDK to collect target depth information and color images. The depth image contains the Aruco code of the target position, and then use Gaussian filtering to blur the image and reduce noise to obtain a stable image of the sensor and ensure data quality.

[0057] Step S2: Start the robot control and camera stream, and set the recognition dictionary and detection parameters of the Aruco code mark.

[0058] Step S2, in order to set the recognition dictionary and detection parameters of the Aruco code mark, the coordinates of the Aruco code corner points are marked in real time in each frame of the color image for subsequent calculation. In the face of the problem that strong reflections may affect the stability of corner point detection in the working environment of the power cabinet, the detection model needs to meet real-time requirements while ensuring high accuracy.

[0059] As a pattern-based two-dimensional marker, ARUCO code has high robustness and good detection accuracy, and can provide stable pose estimation in dynamic or complex environments. In a dynamic environment, the target object may be affected by occlusion, illumination changes or other interference sources. By utilizing the high robustness of ARUCO code positioning, the system can ensure stable detection and tracking of the target even in more complex environments. In practical applications, by utilizing the unique pattern and encoding method of ARUCO code, the system can accurately and quickly extract the position and orientation of the target object from the camera image. Compared with traditional visual servoing methods, ARUCO code can not only greatly improve positioning accuracy, but also overcome interference factors such as illumination changes and background complexity, especially in the field of industrial automation. Combining ARUCO code with the visual servo system enables the system to achieve efficient target positioning and precise control in a larger workspace.

[0060] Step S3: Obtain camera internal parameters.

[0061] In this section, the camera internal parameters of this embodiment include focal length and principal point position. The pixel coordinates of the target marker are obtained by the camera and converted into three-dimensional coordinates in the camera coordinate system. The key formula is as follows:

[0062] 1) Calculate the target position in the camera coordinate system

[0063] According to the obtained pixel coordinates (u, v) of the target marker point, the mapping relationship is:

[0064]

[0065] Where: z is the distance from the camera to the target position (unit: meter), f x 、f y is the focal length of the camera (unit: pixel), u 0 、v 0 are the horizontal and vertical coordinates of the principal point of the image, respectively.

[0066] Through the camera internal parameter f x 、f y 、u 0 、v 0 , we can convert the pixel coordinates (u, v) into three-dimensional coordinates (X, Y, Z) in the camera coordinate system:

[0067]

[0068] Z=z

[0069] 2) Convert to robot coordinate system

[0070] Assume the position of the robot end effector is (x, y, z, rx, ry, rz), where x, y, z are the position of the robot end effector and rx, ry, rz are its posture. Convert the camera coordinate system to the robot coordinate system:

[0071]

[0072] Where: R is the rotation matrix and t is the translation vector.

[0073] Step S4, perform the positioning task, continuously detect the Aruco code mark, calculate the pixel error and drive the robot arm to move to the target position.

[0074] After reaching the designated location area, the robot will provide real-time feedback and adjustments based on the calculated increments until the predetermined accuracy is achieved.

[0075] (1) Calculation error: First, we calculate the current pixel (u c ,v c ) and the target pixel (u goal,v goal ). This error can be obtained by calculating the Euclidean distance of pixel coordinates:

[0076] norm_error=Π(u c -u goal ,v c -v goal )Π 2

[0077] This error measures the distance between the current pixel and the target pixel. A larger error means more adjustments are needed.

[0078] (2) Calculate the control gain: According to the error size, use the adaptive gain controller to calculate the control gain λ x :

[0079]

[0080] Where: 0 is the initial gain (control strength), λ ∞ is the lower limit of the gain, λ' ∞ is the adjustment factor for the control gain.

[0081] This formula determines how much adjustment the robot needs. The larger the error, the greater the gain, and the faster the robot adjusts.

[0082] The adaptive gain controller of the present invention can dynamically adjust the parameters of the control system according to the real-time motion state of the target object and environmental changes. Compared with the traditional gain setting, the introduction of the adaptive gain controller is real-time, and can automatically optimize the control gain by detecting system errors and environmental changes in real time, effectively avoiding control failures caused by environmental changes or error accumulation, thereby achieving more accurate trajectory tracking and dynamic adjustment. This control strategy can not only ensure that the robot arm can work efficiently under different tasks and operating conditions, but also maintain stability in the case of rapid movement or large interference, especially in a changeable industrial environment. It can effectively improve the robustness and response speed of the system and ensure that the robot arm can accurately control the target posture.

[0083] (3) Calculate pixel coordinate increment: Use camera intrinsic parameters and calculated gain λ x , calculate the action increment that the robot end effector needs to do (expressed in pixel coordinate system). Define a coefficient matrix C ij ", which shows the effect of changes in the camera coordinate system on pixel coordinates:

[0084]

[0085] The coefficient matrix is ​​a Jacobian matrix, which is based on the pixel coordinate increments converted from the color image information.

[0086] Then, find the pseudo-inverse of the matrix:

[0087]

[0088] In the formula, a is the increment, and this action increment Δu v represents the required motion of the robot end effector in the camera coordinate system, (u c ,v c ) is the pixel coordinate of the current position.

[0089] (4) Convert the increment to the robot end pose: We use the calculated motion increment Δu v As input, it is converted into the position increment ΔT of the robot end robot The ultimate goal is to convert the increment in the camera coordinate system into the motion command of the robot end effector. Assuming that the action increment a is the pose increment calculated by the robot, the new pose of the robot end effector can be obtained by the following formula:

[0090] T robot_new =T robot_current ΔT robot

[0091] In the formula, ΔT robot is the pose increment.

[0092] The robot will provide real-time feedback and adjustments based on the calculated increments until the predetermined accuracy is achieved.

[0093] Step S5: When the positioning error is less than the set threshold, the positioning is completed, the program stops and prints the final robot posture.

[0094] Step S5: After each action is performed, check the current pixel (u c , v c ) and the target pixel (u goal , v goal ) to determine whether the positioning is completed. The termination condition is that the error is less than a set threshold.

[0095]

[0096] The formula is the current pixel coordinates and the target pixel coordinates obtained by the color image processing method, and the movement of the robot arm is controlled by the difference between the two.

[0097] When the differences in all pixel coordinates are less than the set threshold, the positioning ends.

[0098] Calculate the robot's end position: After each adjustment, the robot's end position can be obtained by reverse calculation. Assuming the robot's current end position is (x, y, z, rx, ry, rz), then:

[0099]

[0100] Among them, R robot is the rotation matrix, t robot is the translation vector.

[0101] When the target position is calculated, the robot end effector translates from the current position to the target position, usually using linear interpolation for a smooth transition:

[0102] T robot_final =T robot +ΔT robot

[0103] The robot then performs this pose change by linear interpolation.

[0104] The present invention tightly integrates the position and posture positioning information of the target object with the robot control algorithm to form a closed-loop control system. Through the precise position and posture data provided by the ARUCO code, combined with the real-time adjustment of the adaptive gain controller, the system can continuously adjust the movement of the robot arm so that it can accurately track the target with a positioning accuracy of less than 1 pixel. In practical applications, this method can not only accurately locate the target object, but also ensure that the robot arm maintains stable target tracking during movement, avoiding deviations caused by error accumulation or external interference. The system can perform well in tasks requiring high speed and high precision, and is particularly suitable for automated assembly, precision operations and other industrial application scenarios that require high-precision positioning.

[0105] In summary, the invention of robotic arm visual servo positioning combined with ARUCO code and adaptive gain controller can provide a high-precision, high-robustness and adaptive adjustment visual servo system, which is suitable for various complex and dynamic industrial application scenarios and provides new solutions and directions for technological innovation in related fields.

[0106] Although the preferred embodiments of the present invention have been described, those skilled in the art may make other changes and modifications to these embodiments once they have learned the basic creative concept. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments and all changes and modifications that fall within the scope of the present invention.

[0107] Obviously, those skilled in the art can make various changes and modifications to the present invention without departing from the spirit and scope of the present invention. Thus, if these modifications and variations of the present invention fall within the scope of the claims of the present invention and their equivalents, the present invention is also intended to include these modifications and variations.

[0108] Although the preferred embodiments of the present invention have been described, those skilled in the art may make other changes and modifications to these embodiments once they have learned the basic creative concept. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments and all changes and modifications that fall within the scope of the present invention.

[0109] Obviously, those skilled in the art can make various changes and modifications to the present invention without departing from the spirit and scope of the present invention. Thus, if these modifications and variations of the present invention fall within the scope of the claims of the present invention and their equivalents, the present invention is also intended to include these modifications and variations.

Claims

1. A visual servo control method for an electric robot, characterized in that: The following steps are involved: Acquire a color image around the power robot arm in real time, wherein the color image contains an Aruco code of a target position; Obtaining the camera internal parameters, and calculating the pixel coordinates of the target position in the electric robot coordinate system by combining the image information of the target and the position and posture of the electric robot end effector; Detecting the Aruco code in the color image in real time, and calculating the new posture of the end effector of the electric robot according to the pixel coordinates of the target position and the pixel coordinates of the current position in the electric robot coordinate system, so that the end effector of the electric robot moves from the current position; Repeat all the above steps until the end condition is reached and the control process is completed.

2. A visual servo control method for an electric robot according to claim 1, characterized in that: The step of calculating the pixel coordinates of the target position in the electric robot coordinate system comprises: Based on the camera intrinsic parameters, calculate the target position coordinates in the camera coordinate system; According to the posture of the end effector of the electric robot, the target position coordinates in the camera coordinate system are converted into the target position pixel coordinates in the electric robot coordinate system through the inverse transformation matrix, wherein the expression of the inverse transformation matrix is: In the formula, is the inverse transformation matrix, R is the rotation matrix, and t is the translation vector.

3. A visual servo control method for an electric robot according to claim 2, characterized in that: The step of calculating the target position coordinates in the camera coordinate system comprises: According to the depth value and the pixel coordinates (u, v) of the target position, the mapping relationship is: According to the camera intrinsic parameters, the pixel coordinates (u, v) of the target position are converted to the camera coordinate system to obtain the target position coordinates in the camera coordinate system: Z=z Where (X, Y, Z) is the three-dimensional coordinate in the camera coordinate system, f x 、f y , u0, v0 are the camera internal parameters, where f x 、f y is the focal length of the camera, u0 and v0 are the horizontal and vertical coordinates of the principal point of the image, and z is the distance from the camera to the target position.

4. The visual servo control method for an electric robot according to claim 1, characterized in that: The step of calculating the new posture of the end effector of the electric robot comprises: Calculating the error between the pixel coordinates of the target position and the pixel coordinates of the current position in the electric robot coordinate system; Based on the error, calculating a control gain using an adaptive gain controller; Calculate the motion increment that the end effector of the electric robot needs to perform based on the camera parameters and the control gain; The motion increment is converted into a posture increment of the end effector of the electric robot, and a new posture of the end effector of the electric robot is obtained according to the posture increment.

5. A visual servo control method for an electric robot according to claim 4, characterized in that: The error is calculated using the Euclidean distance, and the error calculation expression is: norm_error=||(u c -u goal ,v c -v goal )||2 Where norm_error is the error, (u c ,v c ) is the pixel coordinate of the current position, (u goal ,v goal ) is the pixel coordinate of the target position.

6. A visual servo control method for an electric robot according to claim 4, characterized in that: The calculation expression of the control gain is: In the formula, λ x is the control gain, λ0 is the initial gain, also known as the control strength, λ ∞ is the lower limit of the gain, λ' ∞ is the adjustment factor of the control gain, and norm_error is the error.

7. The visual servo control method for an electric robot according to claim 4, characterized in that: The expression of the action increment is: In the formula, a is the increment, Δu v is the action increment, C ij is a coefficient matrix, which represents the effect of the change of the camera coordinate system on the pixel coordinates, f x 、f y , u0, v0 are the camera internal parameters, where f x 、f y is the focal length of the camera, u0 and v0 are the horizontal and vertical coordinates of the principal point of the image, respectively, and z is the distance from the camera to the target position. c ,v c ) is the pixel coordinate of the current position.

8. The visual servo control method for an electric robot according to claim 4, characterized in that: The expression of the new posture of the end effector of the electric robot is: T robot_new =T robot_current ·ΔT robot Where, T robot_new is the new position, T robot_current is the current posture, ΔT robot is the pose increment.

9. The visual servo control method for an electric robot according to claim 1, characterized in that: The end condition is that the error between the pixel coordinates of the target position and the pixel coordinates of the current position in the electric robot coordinate system is less than or equal to a set threshold. When the end condition is met, it means that the end effector of the electric robot moves to the target position, wherein the expression of the end condition is: Where end_location is the end condition, u c [i][j] are the coordinates of the current corner point, u goal [i][j] are the coordinates of the corner points of the target position, and threshold is the set threshold.

10. The visual servo control method for an electric robot according to claim 1, characterized in that: When the robot end effector moves from the current position, linear interpolation is used for smooth transition: T robot_final =T robot +ΔT robot Where, T robot_final is the final position of the robot end effector, T robot is the rotation matrix, ΔT robot is the pose increment.

Images