Research on Control Algorithm of Visual Servo Grasping Operating System for Unmanned Aerial Vehicle Robot Arm for Transmission Line Inspection

When applying the UAV robotic arm visual servo grasping operating system for transmission line inspection, the Cartesian space trajectory planning robotic arm end position information is used. Although finally for the robotic arm control also finally need to inverse solution to each joint in the joint space to control, but the task performed is not the same goal, the end position of the Cartesian space trajectory planning is a function of time, this subsection will be introduced to the straight line trajectory planning [23] and circular arc trajectory planning [24] two Cartesian space trajectory planning methods.

The common interpolation methods are interpolation according to timed length and fixed distance interpolation according to distance. Timed interpolation refers to interpolation according to a certain time step, but this method also has a certain problem, that is, if the teaching distance is too large, it will lead to the interpolation distance is far away and therefore the accuracy is reduced. Fixed-distance interpolation refers to interpolating according to a certain distance step, and for high accuracy, the interpolation distance only needs to be small.

If two points in Cartesian space need to be planned in a straight line, it is actually planning the position of the middle point, i.e., position and attitude. In practice, the linear motion process does not need to change the attitude, that is, often need to consider the position planning.

Spatial arc trajectory planning is different from spatial linear planning, because it involves changes in the coordinate system, so it is relatively more complex. First of all, the default robotic arm needs from A to B to C points of the arc trajectory planning, the use of spatial geometry knowledge can be known, over the space of the three points can get a fixed circle, so you can get the center of the arc, the specified direction of rotation, you can know the arc of the arc of the arc through the three points of the arc of the arc, the center of the circle and the three points can be set up a coordinate system, so you can get the coordinate system with respect to the base coordinate system coordinate conversion relationship, so the arc coordinate system to get, the need to plan out the arc coordinate system, so it is relatively more complex. Relationships, so this arc coordinate system to get, the need to plan out the arc coordinate system in the coordinates through the coordinate transformation relationship to get the actual point in the base coordinate system trajectory relationship.

Sampling planning and optimal planning are common planning problems for robotic arms. In practice, especially in the transmission line inspection environment, there are generally other obstacles in the operating space of the robotic arm, so it is necessary to pay attention to the collision-free motion planning problem of the robotic arm, which is also known as the sampling planning problem. Nowadays, the two common and most common algorithms are Probabilistic Graph Method (PRM) and Rapidly Expanded Random Tree (RRT) [25].

The algorithms in this paper are all based on the RRT algorithm, which is a single-query planning algorithm, as can also be seen from its name, which is a way of slowly and continuously expanding from the root node until the end point is found, whereupon the path that is found is the connected path that is planned. As PRM needs modeling, its computation is large, time-consuming and inefficient, while the RRT speed for PRM has a significant improvement, more efficient, which is often RRT in the robotic arm collision-free motion planning tends to be more widely used.

The 3D trajectory planning problem is the main concern is the speed problem, in order to improve the speed of spatial modeling, collision detection is taken for sampling points. For the search problem in high dimensional space, in order to find the beginning and end of the planned path, try to utilize in the transmission line inspection state space, guiding the blank place. This approach has very good search results for high dimensional spaces.

RRT uses a single-query approach whose goal is to find a connectable path quickly. Its search process can be simply understood as a tree structure, i.e., spreading outward section by section. As a random sampling algorithm, which is also adapted in high-dimensional space, this approach is mainly aimed at speed, and the starting path is not optimal, according to the previous mobile chassis path planning is used in the bidirectional A * algorithm, naturally also thought of the feasibility of having a bidirectional search. This is obviously for transmission line inspection speed will have a great improvement. The next section will be about the bidirectional RRT-Connect.

RRT-Connect relative to the RRT is a two-way search, in the front to the starting point as the starting point, the end as their own goal point, while standing in the end of the point of view, the end of the end as the starting point, the starting point among the end of the end of the results of this two-way search if there is a final intersection, then the speed will be significantly improved, the success of this expansion to the connection can be seen as a success of the planning. As RRT-Connect adds heuristic steps, the search efficiency is faster and better for narrow channels. However, RRT-Connect is still a single-query approach, and its planned trajectory is not yet optimal, but in fact, the optimization of the trajectory is not particularly important in the robotic arm operation of transmission line inspection tasks, and speed is often the most important factor.

In the robotic arm transmission line inspection for grasping motion control method, this paper adopts the position-based visual servo control method (PBVS), the method is through the camera feedback to the robotic arm target object position information to control the robotic arm on the transmission line inspection when the target object of the precise positioning and grasping. The basic flow of the PBVS is shown in Figure 1.

Figure 1.

PBVS basic process

According to the dynamic binocular vision calibration method the position of the target can be obtained so that the PBVS system can be constructed. After the construction of the system is completed, the motion speed of each joint needs to be obtained so as to control the joint motion for grasping.

The error of PBVS can be expressed by equation (1), where s is the state quantity obtained from visual measurement at the t moment, l(t) is the 3D spatial position information at the t moment, r is the 3D spatial pose information, and S^ is the ideal state quantity. 1e(t)=s(l(t),r)−s^

Let r = [R_x, R_y, R_z]^T, whose modulus is the magnitude of the rotation angle θ, then θ and r can be expressed by equations (2) and (3). 2θ=arccos(r11+r22+r33−12) 3r=12sinθ[ r32−r23r13−r31r21−r12 ]

In the ideal state, s^=[0,0]T , let the rotation matrix and translation vector of the target coordinates and the end coordinates of the robotic arm be ^t R_e and T_e, then Eq. (1) can be rewritten as Eq. (4). ^t R_e and ^t T_e can be obtained from dynamic binocular vision calibration. 4e(t)=(tTe,r)T

Let the linear and angular velocity vectors at the end of the robotic arm be v_e and ω_e, respectively, then the velocity at the end is V_e = (v_e, ω_e), and its transformation relationship with the state quantity is shown in Equation (5). Where, L_s is the characteristic Jacobi matrix and its expression is shown in equation (6). 5s = LsVe 6Ls=[ teRe00Lr ]

Let L_e = L_s, the derivative of the error can be obtained from Eq. (5) and Eq. (6) as shown in Eq. (7). 7e˙=LeVe

Assuming an exponential reduction in error, the velocity of the jaws is shown in equation (8), where λ is the scale factor and Lc+ is the full rank matrix of L_c. 8Ve=−λLc+e

According to Eq. (4) and Eq. (6) Eq. (8) can be rewritten as shown in Eq. (9). 9Ve=−λ[ tRe00Lr−1 ][ tTer ]

The linear velocity v_e and angular velocity ω_e at the end of the robot arm can be obtained from equation (9) as shown in equation (10). 10{ ve=−λtRetTeωe=−λr

The position error e(t), linear velocity v_e and angular velocity ω_e are sent to the robotic arm controller, which solves the movement angle and velocity of each joint to control the robotic arm to accurately grasp the target.

Although PBVS can accurately control the gripping position of the robotic arm, it is more sensitive to light changes and view angle changes. Therefore, a position estimation algorithm based on Extended Kalman Smooth Variable Structure Filtering (EK-SVSF) is added to the visual servo control system in this paper.

The Kalman filter provides an optimal estimate of the system state by predicting and updating the state of the system. The state prediction ω^kk−1 , covariance prediction P_kk–1 and prediction measurement Y^kk−1 of the visual servo system can be represented by equation (11), where A is the system dynamic matrix, ω^k−1 is the a posteriori state estimation vector, P_k–1 is the a posteriori error covariance matrix, Q_k–1 is the system noise covariance matrix and f is the image mapping function. 11{ ω^k/k−1=Aω^k−1Pkk−1=APk−1AT+Qk−1Y^kk−1=f(ω^k/k−1)

According to Eq. (11), the prediction error e_z,kk–1 can be obtained as shown in Eq. (12), where ξ_k–1 is the camera measurement. 12ez,kk−1=ξk−1−Y^kk−1

This results in the smoothed boundary value ψ_k, as shown in Equation (13), where F_k is the observation matrix, S_k is the measurement error covariance matrix, and Ē is the diagonal matrix consisting of E, whose expression is shown in Equation (14), where γ is the rate of convergence of the SVSF. 13ψk=(E¯−1FkPkk−1FkTSk1)−1 14E=| ez,k/k−1 |+γ| ez,k−1/k−1 |

The expression for the gain of the EK-SVSF is shown in (15), with F^† being the pseudo-inverse of the measurement matrix F, diag representing the diagonal, representing the Hadamard product, and sat representing the logistic satisfiability condition for e_z,k/k–1/ψ_k in the SVSF. 15{ Kk=F†diag[ E∘sat(ez,kk−1/ψk) ]diag(ez,kk−1)−1,ψk>ψcKk=Pkk−1FkTSk1,ψk<ψc

The state prediction ω^kk and covariance prediction P_k of the updated system state are calculated based on K_k, as shown in Eq. (16). 16{ ω^k/k=ω^kk−1+Kkez,k/k−1Pk=(I−KkFk)Pk/k−1(I−KkFk)T+KkRk−1KkT

In order to improve the robustness and stability of the system, data fusion techniques are usually used to integrate data originating from multiple sensors in a specific way. In this paper, the sensors in the vision servo system include left and right cameras, when the master arm performs the grasping task, the master eye will collect the target information and perform the position estimation, and the slave arm will reach the side of the master arm to collect the target information and the information of the actuator at the end of the master arm and perform the position estimation, so in this paper, we will perform data fusion of the position estimation so as to get the optimal position estimation.

The data fusion technique is divided into two ways: centralized and distributed. Centralized data fusion is to concentrate the information from multiple data sources into a central location for integration and analysis. Distributed data fusion is to process the information from multiple data sources according to their own needs and conditions before further integration and analysis. This approach reduces the computation time and data storage burden, and is very friendly for processing information in real-time dynamic environments. Therefore, in this paper, distributed data fusion is used to obtain the optimal bit position, as shown in Fig. 2.

Figure 2.

Distributed data fusion is the optimal position estimation

In order to obtain better position estimation, this paper utilizes the ordered weighted average (OWA) operator to fuse the KF-SVSF position estimation results originating from two cameras.The OWA operator is based on the weighted average of the sequential relationship between the nodes to obtain the aggregated results.The distributed data fusion process based on the OWA is shown in Fig. 3.

Figure 3.

Owa distributed data fusion process

The weights of the OWA are determined based on the covariance matrix P_k/k calculated from the a posteriori error e_k/k, and the computational expression is shown in Equation (17), where e_k/k is the difference between the current state ω_k and the state prediction ω^k/k . Each time the error estimation is performed, its error value can be obtained from the diagonal elements of the covariance matrix P_k/k, which can be used to update the weights. 17Pk/k=E[ ek/kek/kT ]

The lower the estimation error the higher the weights and vice versa the lower the weights. Assuming that the first elements of the two covariance matrices P_k/k are P₁ and P₂, the weights are shown in Equation (18), and the optimal bit position estimation can be obtained by applying the obtained weights to the corresponding data and weighting the average. 18{ w1=1/P11/P1+1/P2=P2P1+P2w2=1/P21/P1+1/P2=P1P1+P2

The system utilizes a UR3 six-degree-of-freedom robotic arm and a DH AG95 two-finger parallel gripper to implement the gripping action. The communication between the systems is realized through the robotic arm operating system, which ensures efficient data exchange between the upper computer and the robotic arm. The upper computer integrates the feature detector, IBVS controller and visualization interface developed independently in this paper, which is a complete set of facilities to support the processing of the data coming from the robotic arm and send feedback control signals to it. The connection between the robotic arm and the upper computer is realized through Ethernet, which ensures the stability and speed of data transmission.

The grasping performance was analyzed using four metrics: grasping success rate, servo success rate, positioning error and attitude similarity.

Grasping Success Rate (GSR): Grasping an object and keeping the object from falling is regarded as a successful grasping. The grasping success rate is the ratio of the number of successful grasps to the total number of grasps.

Servo Success Rate (SSR): Project the target keypoints of the camera's desired pose and final pose onto the image plane respectively, and if the root-mean-square (RMS) errors of all the keypoints are within the threshold, it is regarded as one successful visual servoing.

Positioning Error (PE): indicates the displacement error between the final position and the desired position of the end-effector. The localization error measures the planar grasping accuracy of the algorithm.

Grasping Posture Similarity (GPS): as the method proposed in this paper guides 6D grasping through 2D information, this chapter can quantify the degree of deviation between the final and desired postures of the end-effector of a robotic arm as a reflection of the accuracy of the 6DoF grasping by mapping the rotational and translational similarities to a uniform metric space of [0,1] and weighting the sum.

In the grasping experiments, the performance of the proposed method is compared with five visual servo grasping models, IBVS, SIFT, Deep MPCVS, KOVIS, and PID-IBVS. Among them, IBVS utilizes Ar Uco markers from OpenCV vision library as a feature extraction tool, and combines with a proportional controller to realize basic image visual servo grasping. SIFT extracts stable feature points to guide the robotic arm grasping by combining scale-invariant feature transformation technique with a proportional controller. The two methods, DeepMPCVS and PID-IBVS, respectively, employ respectively a Model Predictive Control and PID control strategies, both identify the bounding box of the target through RTMDet network and use the four corner points of the bounding box as the control features to achieve accurate visual servo grasping. KOVIS, on the other hand, is a scale-free grasping method based on learning the feature representation of the object from the selfencoder.

The following section shows a successful grasping example of the proposed method under transmission line inspection. From the external camera viewpoint, the robotic arm is able to approach the target gradually and complete the grasping task successfully. And from the camera view of the robotic arm, both the target bounding box and the key points are accurately captured and tracked throughout the grasping process. The key performance curve of the grasping process is recorded. Figure 8 shows the error convergence curve under the control method of this paper, and the image feature error can be smoothly reduced to below the set threshold, which proves that the system has successfully realized the target convergence. Figure 9 shows the feature point trajectory of the robotic arm, in which the blue stars indicate the initial image features and the orange triangles indicate the desired image features, and it can be seen from the feature trajectory that the feature trajectory of the proposed method can reach the desired position smoothly.

Figure 8.

Error convergence curve

Figure 9.

Characteristic trajectory

Table 1 shows the comparison of the grasping performance of six visual servoing methods in static cluttered scenes of transmission line inspection. In terms of grasping success rate (GSR), the IBVS method is only 0.49, and all other deep learning-based visual servoing methods perform well. The control method proposed in this paper and the KOVIS method achieve the optimal and sub-optimal grasping success rate of 0.93 and 0.91, respectively. In terms of servo success rate (SSR) index, the proposed method in this paper is the optimal 0.95, which indicates that the controller of the proposed method has strong convergence. In addition, the average errors of the proposed method in x-axis, y-axis and z-axis are 0.19 cm, 1.11 cm and 0.18 cm, respectively, which are optimal and show good grasping accuracy. In addition, the grasping similarity result of the proposed method is 0.975, which shows that it can restore the grasping attitude better. By analyzing the above four indexes, it can be seen that the proposed method performs better in terms of comprehensive performance, which demonstrates its stable and efficient grasping ability in dealing with static cluttered scenes in transmission line inspection.