ArUco code-based power secondary equipment mechanical arm positioning method

Abstract

The application relates to a power secondary equipment mechanical arm positioning method based on an ArUco code and belongs to the technical field of mechanical arm control. The technical scheme is as follows: a depth camera is installed at the end of a mechanical arm and kept parallel to the gripper of the mechanical arm, the camera pose is continuously adjusted through ArUco code identification, the Z-axis of the camera coordinate system is ensured to be parallel to the Z-axis of a target coordinate system, three-dimensional coordinates of the target under the base coordinate system of the mechanical arm are obtained according to hand-eye calibration, six joint angles of an RML63 humanoid collaborative mechanical arm are solved through inverse kinematics, and finally, the mechanical arm movement is controlled through a ROS (Robot Operating System) to complete the target grabbing task. The application has the beneficial effects that under the condition of low use cost, the influence of external environmental factors on the visual positioning effect can be minimized, the positioning accuracy is high, the real-time identification and grabbing of the mechanical arm on the power equipment cabinet door handle target can be realized, and the demand of the current power inspection robot autonomous operation can be met.

Description

TECHNICAL FIELD

[0001] The application relates to an ArUco code-based power secondary equipment mechanical arm positioning method and belongs to the technical field of mechanical arm control. BACKGROUND

[0002] Equipment inspection in a power system is a very important routine work for timely and accurately finding out equipment safety hazards, ensuring normal work of power grid equipment, and safe and stable operation of a substation. In particular, in the environment of a substation with a large number of equipment, if a hidden danger is not timely eliminated, the substation cannot normally operate, or even the normal production and life of a large area around the substation is affected, or even personal injury is caused. As the power grid system is continuously improved, the automation level of the power inspection system is higher and higher. The manual inspection method cannot meet the current and future intelligent development needs of the power grid. In this background, intelligent inspection robots emerge as the times require. The intelligent inspection robot integrates bionic robot technology, multi-sensor fusion technology, intelligent control technology, navigation and positioning technology, and Internet of Things technology, and can realize all-weather, all-around, and full-autonomous intelligent inspection and monitoring of power grid equipment, greatly reducing the labor intensity of power grid workers and the operation and maintenance cost of the power grid system. However, the current research and development of the inspection robot mainly stays in remote measurement and monitoring of power equipment, and does not have the ability to directly operate the power equipment, which is far from the expected intelligent level. With the development of automation and intelligence, combining a mechanical arm with a machine vision image recognition technology to realize autonomous operation of the robot under different regulations is a key development direction of the future inspection robot. The traditional autonomous operation method of the mechanical arm generally adopts a drag teaching method, drags the robot arm to a specified position set in advance, and then stores the trajectory completed by the robot to repeat the operation. Since there is a lack of visual positioning information, the method is highly dependent on the external environment, the target feature state, and the grasping task. Once a change occurs, the method needs to be reconfigured, and the flexibility is poor. The autonomous operation of the mechanical arm based on machine vision mainly uses image detection to obtain the three-dimensional coordinates of the target in the mechanical arm base coordinate system. In recent years, with the rapid development of deep learning vision technology, the combination of deep learning technology and robot operation has a wide application. However, the target detection method is easily affected by light changes, occlusion, and other conditions, and has problems such as low recognition and positioning rate, poor robustness, and long running time in the actual autonomous operation process of the mechanical arm. SUMMARY

[0003] The application aims to provide an ArUco code-based power secondary equipment mechanical arm positioning method, which can minimize the influence of external environmental factors on the visual positioning effect at a lower use cost, has high positioning accuracy, can realize real-time recognition and grabbing of the mechanical arm on the power equipment cabinet door handle target, can meet the current demand of power inspection robot autonomous operation, and solves the problems in the background technology.

[0004] The technical scheme of the application is:

[0005] An ArUco code-based power secondary equipment mechanical arm positioning method, comprising the following steps: installing a depth camera at the end of a mechanical arm, keeping parallel with the clamping jaw of the mechanical arm, constantly adjusting the camera pose by recognizing the ArUco code, ensuring that the Z-axis of the camera coordinate system is parallel to the Z-axis of the target coordinate system, obtaining the three-dimensional coordinates of the target under the mechanical arm base coordinate system according to the hand-eye calibration, obtaining the six joint angles of the RML63 humanoid collaborative mechanical arm through inverse kinematics, and finally controlling the movement of the mechanical arm through the ROS robot operating system to complete the target grabbing task.

[0006] The specific steps are as follows:

[0007] ①The ArUco code is estimated in pose through image recognition technology or a camera, and the coordinate conversion relationship between the ArUco code image coordinate system and the camera coordinate system is established;

[0008] ②The relative pose relationship between the RML63 humanoid collaborative mechanical arm coordinate system and the RealSense D455 depth camera coordinate system is measured and calculated by using the hand-eye calibration model based on dual quaternions, so as to determine the three-dimensional space coordinate values of the object to be grabbed under the robot coordinate system, and then the RRT-Connect trajectory planning algorithm is used to plan through the robot kinematics equation, so as to determine the travel trajectory and realize the autonomous operation of the mechanical arm;

[0009] The mechanical arm hand-eye relationship matrix X and the mechanical arm base coordinate system and the world coordinate system orientation relationship Y can be expressed as a matrix equation

[0010] AX=YB (1)

[0011] Wherein, A represents the pose transformation matrix of the world coordinate system to the camera coordinate system in different poses, B represents the pose transformation matrix of the mechanical arm tool coordinate system in different poses to the mechanical arm base coordinate system, and R A , R X , R Y , R B respectively represent the rotation part in the 4×4 pose transformation matrix of A, X, Y and B, t A , t X , t Y , t BLet t, t represent the translation part in the 4x4 transformation matrix of A, X, Y, B respectively, then the homogeneous matrix equation (1) can be expressed as:

[0012]

[0013] Decoupling and expanding the above formula into a separate form of rotation and translation are as follows:

[0014] R A R X = R Y R B (3)

[0015] R A t X +t A = R Y t B +t Y (4)

[0016] The above formula is expressed by the dual quaternion parameterization method, and vectors a and b represent the quaternions of two consecutive finite motions, respectively. According to the commutativity of quaternion matrix multiplication, the corresponding rotation matrix and translation vector can be solved by using the LM least square method, and the transformation matrix Y between the robot coordinate system and the world coordinate system is determined, so that the three-dimensional coordinate value of the ArUco code in the mechanical arm coordinate system is determined;

[0017]

[0018] Wherein, W(a′, b′) represents a 4x8 matrix composed of the dual parts a′ and b′ of the dual quaternion q(x, y) can be solved by singular value decomposition, and q′(x, y) is the dual part of the dual quaternion corresponding to the 3x3 translation vector t X , t Y , and V(a i , b i ) and W(a i ′, b i ′) are obtained by n(n≥3) measurements, a 4n x 8 matrix L′ = [W(a1′, b1′), W(a2′, b2′), …, W(a n ′, b n ′)] and L are established, and the above formula is converted into a homogeneous equation group:

[0019] L q′ (x, y) + L′q(x, y) = 0 (6)

[0020] Using the unit row and orthogonal restriction conditions of the dual quaternion, the equation group (6) can be expressed as an optimization problem with constraints:

[0021]

[0022] Wherein, ||·|| represents the matrix F-norm, by using Lagrange multiplication and nonlinear optimization algorithm, the multivariate function optimization problem in formula (7) can be well converged to extreme value;The to-be-grabbed target and the ArUco code are in the same plane, the center point of the ArUco code is used as the world coordinate system, the base point of the to-be-grabbed target is used as the object coordinate system, the axis of the to-be-grabbed target coordinate system is parallel to the axis of the world coordinate system in turn, and the rotation matrix of the target object is constrained, then the coordinate transformation relationship between the world coordinate system and the mechanical arm base coordinate system of the ArUco code is determined through formula (7), and finally the pose information of the to-be-grabbed target under the mechanical arm can be obtained through the translation matrix between the world coordinate system and the target coordinate system.

[0023] The number of the ArUco codes is four, and the code disc composed of the four ArUco codes is arranged around the electric equipment cabinet door handle, the code in the upper left corner is the first detection code, and when the code is shielded, the other codes are detected clockwise, after the ArUco code is identified, the electric equipment cabinet door handle target is searched from the field of view and is grabbed.

[0024] The beneficial effects of the present application are: low equipment cost, no need for expensive three-dimensional positioning equipment, only ordinary cameras and cheap two-dimensional code stickers are needed to realize power secondary equipment target recognition and accurate positioning;High running efficiency, without complex binocular stereo vision algorithm, the electric equipment cabinet door handle small target can be grabbed, the ArUco two-dimensional code image has good saliency, is easy to encode and detect, and can meet the real-time operation demand of embedded development board;Good algorithm robustness, in the case of low use cost, the influence of external environmental factors on visual positioning effect can be minimized, and the reliability of the autonomous operation of the power inspection robot is enhanced. BRIEF DESCRIPTION OF DRAWINGS

[0025] Figure 1 The present application is an autonomous grabbing system overall composition and working principle;

[0026] Figure 2 It is the internal id information diagram of the ArUco code of the present application;

[0027] Figure 3 It is the black frame diagram of the ArUco code of the present application;

[0028] Figure 4 It is the ArUco code diagram of the present application;

[0029] Figure 5 It is the hand-eye relationship model diagram of the present application;

[0030] Figure 6 It is the mechanical arm grabbing experiment platform schematic diagram of the present application;

[0031] Figure 7 Figure 1 is a schematic diagram of the process of the RML63 mechanical arm grabbing the handle of the power cabinet door according to the present application. DETAILED DESCRIPTION

[0032] The present application will be further described by examples in conjunction with the accompanying drawings.

[0033] A power secondary equipment mechanical arm positioning method based on ArUco code, comprising the following steps: installing a depth camera at the end of the mechanical arm, keeping parallel with the gripper of the mechanical arm, constantly adjusting the camera pose by recognizing the ArUco code, ensuring that the Z axis of the camera coordinate system is parallel to the Z axis of the target coordinate system, obtaining the three-dimensional coordinates of the target under the base coordinate system of the mechanical arm according to the hand-eye calibration, solving the six joint angles of the RML63 humanoid collaborative mechanical arm through inverse kinematics, and finally completing the target grabbing task through the ROS robot operating system to control the movement of the mechanical arm.

[0034] The specific steps are as follows:

[0035] ①The ArUco code is estimated in pose by image recognition technology or a camera, and the coordinate conversion relationship between the ArUco code image coordinate system and the camera coordinate system is established; considering that the model-based pose estimation method mainly compares the real image and the model image, calculates the target pose, and is very dependent on the accurate detection of local features, when the noise is large and the accurate local features cannot be extracted, the stability of the pose estimation method will be affected. The learning-based pose estimation method generally matches and analyzes the global features of the image, but needs to define the pose category in advance, and trains the pose classifier through the pattern classification method to realize pose recognition, so the robustness of this method depends to a great extent on the sufficiency of the training samples. Therefore, the ArUco code (see Figure 2 、 3 , 4) is used as an aid for visual detection to complete the position and pose determination of small targets such as power equipment cabinet door handles;

[0036] ②The relative pose relationship between the TML63 humanoid collaborative mechanical arm coordinate system and the RealSense D455 depth camera coordinate system is accurately measured and calculated by using the hand-eye calibration model based on dual quaternions, so as to determine the three-dimensional space coordinate values of the object to be grabbed under the robot coordinate system, and then the RRT-Connect trajectory planning algorithm is used to plan and determine the travel trajectory through the robot kinematics equation (Denavit-Hartenberg parameters), so as to realize the autonomous operation of the mechanical arm;

[0037] Refer to Figure 5, the hand-eye relationship matrix X of the robot arm and the orientation relationship Y between the robot arm base coordinate system and the world coordinate system can be expressed as a matrix equation

[0038] AX=YB (1)

[0039] wherein A represents the pose transformation matrix of the world coordinate system to the camera coordinate system of different poses, B represents the pose transformation matrix of the robot arm tool coordinate system to the robot arm base coordinate system of different poses, let R A , R X , R Y , R B respectively represent the rotation part in the 4x4 pose transformation matrix of A, X, Y, B, t A , t X , t Y , t B respectively represent the translation part in the 4x4 transformation matrix of A, X, Y, B, then the homogeneous matrix equation (1) can be expressed as:

[0040]

[0041] The above formula is decoupled and expanded into a form of rotation and translation separated:

[0042] R A R X =R Y R B (3)

[0043] R A t X +t A =R Y t B +t Y (4)

[0044] The above formula is expressed by using the dual quaternion parameterization method, and vectors a and b represent the quaternions of two consecutive finite motions, then by the commutativity of quaternion matrix multiplication, the corresponding rotation matrix and translation vector can be solved by using the LM least square method, the transformation matrix Y between the robot coordinate system and the world coordinate system is determined, and the three-dimensional coordinate value of the ArUco code in the robot arm coordinate system is determined;

[0045]

[0046] wherein W(a′, b′) represents a 4x8 matrix composed of the dual parts a′ and b′ in the dual quaternion q(x, y) can be solved by singular value decomposition, and q′(x, y) is a 3x3 translation vector t X , t YThe dual part in the corresponding dual quaternion is obtained through n (n is greater than or equal to 3) measurements, V(a i , b i ) and W(a i ', b i ') are obtained, a 4n*8 order matrix L'=[W(a1',b1'), W(a2',b2'), …, W(a n ', b n ')] and L are established, and the above formula is converted into a homogeneous equation group:

[0047] L q′ (x, y)+L'q(x, y)=0 (6)

[0048] The equation group (6) can be expressed as an optimization problem with a constraint condition by using the unit row and orthogonal constraint condition of the dual quaternion:

[0049]

[0050] Wherein, ||·|| represents the matrix F-norm, the multivariate function optimization problem in formula (7) can be well converged to an extreme value by using Lagrange multiplication and a nonlinear optimization algorithm; considering that the target to be grabbed and the ArUco code are in the same plane, the center point of the ArUco code is taken as a world coordinate system, the base point of the target to be grabbed is taken as an object coordinate system, the axis of the target to be grabbed coordinate system is parallel to the axis of the world coordinate system in turn, the rotation matrix of the target object is constrained, the coordinate transformation relationship between the world coordinate system where the ArUco code is located and the base coordinate system of the mechanical arm is determined through formula (7), and finally the pose information of the target to be grabbed under the mechanical arm can be obtained through the translation matrix between the world coordinate system and the target coordinate system.

[0051] Considering that a single ArUco code is easy to be shielded during data acquisition and has low robustness, the number of ArUco codes in the application is four, and the code disc composed of the four ArUco codes surrounds the electric equipment cabinet door handle, the code in the upper left corner is the first detection code, and when the code is shielded, the code is detected clockwise to other codes, after the ArUco code is recognized, the electric equipment cabinet door handle target is searched from the field of view and is grabbed.

[0052] Referring to the drawings Figure 6, a Rieman RML63 humanoid collaborative robot arm, an Intel RealSense D455 depth camera, and an ArUco code are used to build a robot arm grabbing experiment system to position and grab the handle of a power equipment cabinet door. The humanoid collaborative robot arm has a repeat positioning accuracy of ±0.05 mm, a working radius of 638.5 mm, a camera resolution of 1280×720 pixel, and a field of view angle of 87°×58°×95°. Before the experiment, the camera is fixed on the flange plate of the robot arm end effector. The host computer is configured as an NVIDIA JETSON TX2 embedded development board, a 384-core NVIDIA Volta TM GPU with 48 Tensor Cores, 8 GB 128-bit LPDDR4 memory, and a system running Linux version number Ubuntu 18.04. The controller is placed inside the robot arm chassis. The grabbing process is shown in Figure 7 , and the experimental process data is shown in Table 1,

[0053] Table 1 Experimental results table

[0054]

[0055] The grabbing is one-time grabbing, and the robot arm grabs the door handle and opens it once, which is recorded as one cycle of grabbing. A total of 10 cycles of grabbing are performed, and the success rate of grabbing is recorded. The average recognition time in Table 1 is 1.98 s, which includes the time for detecting the ArUco code from the picture and predicting its pose relative to the robot arm. The average navigation time is 15.92 s, which includes the time for the robot arm to move to the picture point of the power equipment door handle. The picture point is the pose at which the robot arm moves to a suitable pose to make the door handle image in the center of the robot arm end camera field of view. The average positioning time is 1.18 s, which refers to the time for calculating the pose of the power equipment door handle relative to the robot arm, which facilitates subsequent robot arm grabbing and door opening operations. Under the NVIDIA JETSON TX2 embedded processor, the average time required for recognition, navigation, positioning, grabbing, and return of the grabbing target is 48.3 s from the start of detection to the end of opening the cabinet door. This can meet the needs of autonomous operation of a power inspection robot.

[0056] The application firstly analyzes ArUco code image recognition technology, obtains the position of the ArUco code in the camera coordinate system, then combines the pose estimation and the robot hand-eye calibration algorithm to solve the spatial coordinates of the target to be grabbed in the robot base coordinate, and finally controls the movement of the mechanical arm through the ROS robot operating system to complete the target grabbing task. The application has low equipment cost, does not need expensive three-dimensional positioning equipment (such as laser radar, structured light or binocular camera), and can realize power secondary equipment target recognition and accurate positioning only by using a common camera and a cheap two-dimensional code sticker; the application has high running efficiency, can realize the grabbing of the small target of the power equipment cabinet door handle without complex binocular stereo vision algorithm, the ArUco two-dimensional code image has good saliency, is composed of a wide black frame and an internal binary matrix, is easy to encode and detect, can meet the real-time operation demand of the embedded development board, and has good algorithm robustness. Whether the traditional vision or the deep learning vision target detection method is affected by light changes, shielding and other conditions, and has poor flexibility, while the power secondary equipment target positioning method based on the ArUco code can minimize the influence of external environmental factors on the visual positioning effect under the condition of low use cost, and enhance the reliability of the autonomous operation of the power inspection robot.

Claims

1. A method for positioning a robotic arm of a power secondary equipment based on ArUco codes, characterized in that... The process includes the following steps: mounting a depth camera at the end of the robotic arm, keeping it parallel to the gripper of the robotic arm; continuously adjusting the camera pose by recognizing ArUco codes to ensure that the Z-axis of the camera coordinate system is parallel to the Z-axis of the target coordinate system; obtaining the three-dimensional coordinates of the target in the robotic arm's base coordinate system based on hand-eye calibration; obtaining the six joint angles of the RML63 humanoid collaborative robotic arm through inverse kinematics; and finally controlling the movement of the robotic arm through the ROS robot operating system to complete the target grasping task. The specific steps are as follows: ① Use a camera to estimate the pose of the ArUco code and establish the coordinate transformation relationship between the ArUco code image coordinate system and the camera coordinate system; ② Using a hand-eye calibration model based on dual quaternions, the relative pose relationship between the RML63 humanoid collaborative robot coordinate system and the RealSense D455 depth camera coordinate system is calculated, thereby determining the three-dimensional spatial coordinates of the object to be grasped in the robot coordinate system. Then, through the robot kinematic equations, the RRT-Connect trajectory planning algorithm is used to plan and determine the travel trajectory to achieve autonomous operation of the robot arm. The hand-eye relationship matrix X of the robotic arm and the orientation relationship between the robotic arm's base coordinate system and the world coordinate system Y can be represented by matrix equations. (1) in, A This represents the pose transformation matrix from the world coordinate system to the camera coordinate system at different poses. B Let be the pose transformation matrix from the robot arm tool coordinate system to the robot arm base coordinate system in different postures. R A , R X , R Y , R B They represent A , X , Y , B The rotation part of a 4×4 pose transformation matrix. t A , t X , t Y , t B They represent A , X , Y , B The translation part of the 4×4 transformation matrix, then the homogeneous matrix equation (1) can be expressed as: (2) Decoupling the above equation and expanding it into a form that separates rotation and translation is: (3) (4) The above formula is expressed using the dual quaternion parameterization method. Vectors a and b represent the quaternions of two consecutive finite motions, respectively. Then, by the commutativity of quaternion matrix multiplication, we can obtain the corresponding rotation matrix and translation vector by using the LM least squares method. This allows us to determine the transformation matrix Y between the robot coordinate system and the world coordinate system, thereby determining the three-dimensional spatial coordinates of the ArUco code in the robot arm coordinate system. (5) in, W ( a′ , b′ ) represents the dual quaternion The dual part a ′, b The 4×8 matrix formed by the '' q ( x , y It can be solved by singular value decomposition. It is a 3×3 translation vector t X , t Y The dual part of the corresponding dual quaternion is obtained through n (n≥3) measurements. V ( a i , b i )and W ( a i ′, b i Construct a 4n×8 matrix. L ′=[ W ( a 1 ′, b 1 ′), W( a 2 ′, b 2 ′),…, W( a n ′, b n ')]and L Transform the above equation into a system of homogeneous equations: (6) Using the unit row and orthogonality constraints of dual quaternions, the system of equations (6) can be expressed as an optimization problem with constraints: (7) Where, ||•|| represents the matrix F-norm. Using Lagrange multiplication and nonlinear optimization algorithms, the multivariate function optimization problem in equation (7) can converge well to the extreme value. The target to be grasped and the ArUco code are on the same plane. The center point of the ArUco code is used as the world coordinate system, and the base point of the target to be grasped is used as the object coordinate system. The axis of the target coordinate system is parallel to the axis of the world coordinate system in turn. At the same time, the rotation matrix of the target object is constrained. Then, the coordinate transformation relationship between the world coordinate system where the ArUco code is located and the base coordinate system of the robotic arm is determined by equation (7). Finally, the pose information of the target to be grasped under the robotic arm can be obtained by the translation matrix between the world coordinate system and the target coordinate system.

2. The method for positioning a robotic arm of a power secondary equipment based on ArUco codes according to claim 1, characterized in that: The number of ArUco codes is four. The code disk composed of the four ArUco codes surrounds the door handle of the power equipment cabinet. The code in the upper left corner is the first detection code. If there is an obstruction, the other codes are detected in turn. After the ArUco code is identified, the target of the door handle of the power equipment cabinet will be found in the field of view and grasped.

Images