CN114946403A - Tea picking robot based on calibration-free visual servo and tea picking control method thereof - Google Patents

Abstract

The invention discloses a tea picking robot based on uncalibrated visual servo and a tea picking control method, which comprise the following steps: the system comprises a camera, a picking hand, a visual controller and a Delta parallel mechanism, wherein the visual controller is used for obtaining uncalibrated visual servo control information through an uncalibrated visual servo control model according to a tender shoot image obtained by the camera; and the Delta parallel mechanism is used for controlling the picking hands to pick tea according to the uncalibrated visual servo control information. By adopting the technical scheme of the invention, the convergence time of picking calculation can be shortened, the working efficiency is improved, and the tea picking robot can accurately finish picking work.

Description

A tea picking robot based on uncalibrated visual servo and its tea picking control method

technical field

The invention belongs to the technical field of tea picking, and in particular relates to a tea picking robot based on non-calibration visual servo and a tea picking control method.

Background technique

At present, the control of tea picking robots often relies on machine vision to obtain image information, and the picking coordinate points are obtained through the image processing algorithm in the upper computer, and then sent to the lower computer to make the picking hand move. In this way, the tea picking robot is divided into two parts: image recognition and trajectory operation, but its accuracy and efficiency are insufficient. Therefore, the control method of visual servoing has emerged. The image feature information is obtained through the visual sensor installed on the robot, and it is used as feedback information to drive the picking hand to approach the target position, which saves the time for reading and transmitting coordinate points, and the accuracy is also improved. will improve. According to the different feedback information, visual servoing can be divided into three modes: position-based (PBVS), image-based (IBVS) and hybrid-based (HBVS). PBVS constitutes a closed-loop control system in 3D Cartesian space, which is highly dependent on the precision calibration of vision sensors and accurate geometric models. Model calibration is difficult, and because the image feature signal is outside the control loop, the target may be located outside the field of view. . IBVS forms a closed-loop system in a two-dimensional image space, and designs a feedback control strategy according to the error signal defined by the image features. HBVS is also known as 2.5D visual servo, which includes 3D space and 2D space, and requires a large amount of calculation. If the calculation accuracy is insufficient, the performance of the system will also decrease. Compared with the other two methods, IBVS has high precision and low design difficulty.

Traditional visual servoing relies heavily on the accurate calibration of cameras, robots and "hand-eye", and image errors often occur during the calibration process. In the traditional method, the state of the picking hands is affected by two aspects, one is the image information obtained by the camera in the tea picking machine, and the other is the internal parameters determined by the system through calibration. The latter is easily interfered by external factors such as the environment, which makes the positioning inaccurate. Therefore, the traditional visual servo needs to be recalibrated, and the calibration and maintenance costs will increase, which does not meet the cost control requirements of the tea picking robot, and also increases the amount of calculation.

SUMMARY OF THE INVENTION

The technical problem to be solved by the present invention is to provide a tea picking robot based on non-calibration visual servo and a tea picking control method thereof, which can shorten the convergence time of picking calculation, improve the work efficiency, and enable the tea picking robot to accurately complete the picking work.

For achieving the above object, the present invention adopts the following technical scheme:

A tea picking robot based on uncalibrated visual servo, comprising: a camera, a picking hand, a vision controller and a Delta parallel mechanism, wherein,

a vision controller for obtaining uncalibrated visual servo control information through the uncalibrated visual servo control model according to the shoot image obtained by the camera;

The Delta parallel mechanism is used for controlling the picking hands to pick tea leaves according to the uncalibrated visual servo control information.

Preferably, the vision controller includes:

The acquisition module is used to acquire the sprout image captured by the camera;

The servo control module is used for comparing the characteristics of the sprout image with the characteristics of the target desired image through the uncalibrated visual servo task function and the extreme learning machine algorithm based on genetic optimization to obtain uncalibrated visual servo control information.

Preferably, the Delta parallel mechanism includes:

a calculation module, used for obtaining the picking trajectory according to the uncalibrated visual servo control information and the Jacobian matrix; wherein, the Jacobian matrix is the relationship matrix between the movement speed of the picking hand and the rotational speed of the motor;

The picking module controls the picking hands to pick tea leaves according to the picking track.

Preferably, the degree of freedom of the delta parallel mechanism is 3DOF, and the motion of the static platform of the delta parallel mechanism is kept in translation in space.

The present invention also provides a tea picking control method based on a non-calibrated visual servo tea picking robot, comprising the following steps:

Step S1, obtaining the uncalibrated visual servo control information through the uncalibrated visual servo control model according to the shoot image obtained by the camera through the vision controller;

Step S2, controlling the picking hands to pick tea leaves according to the uncalibrated visual servo control information through the Delta parallel mechanism.

Preferably, step S1 includes:

Get the sprout image captured by the camera;

The features of the sprout image are compared with the features of the target desired image through the uncalibrated visual servo task function and the extreme learning machine algorithm based on genetic optimization, and the uncalibrated visual servo control information is obtained.

Preferably, step S2 includes:

According to the uncalibrated visual servo control information and the Jacobian matrix, the picking trajectory is obtained; wherein, the Jacobian matrix is the relationship matrix between the movement speed of the picking hand and the rotational speed of the motor;

According to the picking track, the picking hands are controlled to pick tea leaves.

Preferably, the degree of freedom of the delta parallel mechanism is 3DOF, and the motion of the static platform of the delta parallel mechanism is kept in translation in space.

The invention obtains visual servo control information through a visual controller through a non-calibrated visual servo task function and an extreme learning machine algorithm based on genetic optimization; and controls the picker to pick tea leaves according to the visual servo control information through a delta parallel mechanism. By adopting the technical scheme of the present invention, the convergence time of the picking calculation can be shortened, the work efficiency can be improved, and the tea picking robot can accurately complete the picking work.

Description of drawings

Fig. 1 is the structure schematic diagram of the tea picking robot based on uncalibrated visual servo of the present invention;

Figure 2(a) is a schematic diagram of the simplified model of the Delta parallel mechanism;

Figure 2(b) is a schematic diagram of the single branch chain model of the Delta parallel mechanism;

Fig. 3 is the structure diagram of the vision controller;

Figure 4 is a schematic diagram of a pinhole model;

Figure 5 is a flowchart of the GA-ELM algorithm.

Detailed ways

In order to make the purposes, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments These are some embodiments of the present invention, but not all embodiments. The components of the embodiments of the invention generally described and illustrated in the drawings herein may be arranged and designed in a variety of different configurations

Example 1:

As shown in FIG. 1 , the present invention provides a tea picking robot based on uncalibrated visual servoing, including: a camera, a picking hand, a vision controller and a Delta parallel mechanism, wherein,

The vision controller is used to obtain the uncalibrated visual servo control information through the uncalibrated visual servo control model according to the shoot image obtained by the camera; the delta parallel mechanism is used to control the uncalibrated visual servo control information according to the uncalibrated visual servo control information. Pickers carry out tea picking.

As an implementation of this embodiment, the vision controller includes:

The acquisition module is used to acquire the sprout image captured by the camera;

The servo control module is used for comparing the characteristics of the sprout image with the characteristics of the target desired image through the uncalibrated visual servo task function and the extreme learning machine algorithm based on genetic optimization to obtain uncalibrated visual servo control information.

As an implementation of the embodiment of the present invention, the Delta parallel mechanism includes:

a calculation module, used for obtaining the picking trajectory according to the uncalibrated visual servo control information and the Jacobian matrix; wherein, the Jacobian matrix is the relationship matrix between the movement speed of the picking hand and the rotational speed of the motor;

The picking module controls the picking hands to pick tea leaves according to the picking track.

The workflow of leaf picking by the tea picking robot is as follows:

(1) Under the guidance of the camera, the tea picking robot moves to the initial position of the picking work, and the vision controller prepares for work. After receiving the tender bud image, the tea picking robot starts to work in a closed loop.

(2) The camera captures and obtains the tender bud image within the working range, and then transmits it to the PC of the tea picking robot. On the one hand, the PC obtains the coordinates and feature points of the tender bud, and on the other hand, the tender bud image is retained as the target. On a PC, compare with subsequent captured images.

(3) The tea picking robot is guided by the vision controller based on the uncalibrated visual servo, and the Delta parallel mechanism controls the picking hand to move the camera to the target point while taking pictures. After cutting the bud picking points by hand, the leaf buds are adsorbed into the collection box by the flow of gas in the negative pressure straw; through the above picking track, the picked picking points that were photographed are worked in place one by one.

(4) When the images in this space are compared correctly, the tea picking robot moves to the next tea ridge under the guidance of the "eye" unit, and repeats step (2) until the task in the current tea ridge is completed, and then runs step ( 1).

The image is captured by the camera, and when all the captured targets are picked, this process is defined as a work cycle in this paper. The process of picking hand from one picking point to another picking point is defined as picking cycle.

Further, the degree of freedom of the Delta parallel mechanism is 3DOF, and the motion of the static platform is kept in translation in space.

It can be seen from Figure 2(a) that the center point of the static platform of the Delta parallel mechanism is O, and the center point of the moving platform is O'. The length of the master arm is expressed as |A _i B _i |=L ₁ , the slave arm is |B _i C _i |=L ₂ , and so on, |OA _i |=R, |C _i O′|=r, where i=1,2,3. It can be seen from Figure 2(b) that O in the static platform is the base point of the coordinate system, and a rectangular coordinate system O-xyz located in space is set. In the O-xy plane, the angle between OA _i and the x-axis is α ₁ =0, α ₂ =2π/3, α ₃ =4π/3, the angle between the driver and the O-xy plane is expressed as θ _i , i=1,2,3, and O' is expressed as the coordinates (x, y ,z).

By the closed-loop vector method, according to the geometric relationship of the Delta mechanism, the kinematic position equation of the Delta mechanism established based on the geometric method can be obtained, as shown in formula (1):

Let ΔR=r-R, then let

That is, the final expression of the motor rotation angle θ _i is:

Considering the characteristics of the mechanism and the environmental constraints under the specific tea picking workspace, θ _i takes a positive solution.

A necessary condition for uncalibrated visual servoing is the establishment of Jacobian matrix, which can relate the movement speed of the picking hand to the motor speed.

Let the kinematic equation of the delta parallel mechanism be

P=f(θ) (4)

P is the mapping relationship matrix between the position point of the end of the mechanism and the rotation angle of each axis. The two sides of equation (4) are derived at the same time for the time t, and the mapping relationship between the motion speed of the end of the mechanism in the working space and the speed of each motor is obtained:

为机构末端在工作空间内的速度矢量，

表示驱动关节速度矢量，J(θ)是一个偏导数矩阵，即目标速度雅克比矩阵，也即采摘手的速度雅克比矩阵。in,

is the velocity vector of the end of the mechanism in the workspace,

Represents the driving joint velocity vector, J(θ) is a partial derivative matrix, namely the target velocity Jacobian matrix, that is, the picker's velocity Jacobian matrix.

Let _{Di denote B i C i}

By formulas (6) and (7), the end speed law required by the work is converted into the speed law required by the three rotating motors, so as to complete the precise speed control.

Further, the control objective of uncalibrated visual servoing can be described by the task function equation (8):

E(t)=f(m(t),a)-f ^* (8)

where f and f ^* are the current and desired states of the system, respectively, m(t) is the image measurement, and a is the relevant model parameter, such as the camera focal length. The control objective is to minimize the objective function.

In IBUVS, the performance of uncalibrated servo depends on two parts, one is the estimation and calculation of the image interaction matrix, and the other is the choice of controller gain. Among them, the solution of the image interaction matrix inverse matrix representing the mapping relationship from 3D space to 2D image plane plays an important role in the IBUVS system [20-23]. Difficulty and singularity problems for inversion of interaction matrix. The embodiment of the present invention adopts an intelligent neural network convergence unit as a vision controller, and an extreme learning machine (GA-ELM) based on genetic optimization is designed. The fixed gain will make the convergence speed of the characteristic error converge to zero in the system and the working speed of the picker mutually restrict. If the gain value is large, the convergence speed will increase, and the speed of the picker will also increase, and there is a risk of exceeding the constraint. On the contrary, the convergence speed will be slowed down, and the computational efficiency of the system will be reduced. Therefore, the embodiment of the present invention defines a controller based on a fuzzy logic (FL) unit, which replaces the fixed gain with a variable gain λ _a , and the input of the calculation is the L ₂ norm of the task function and its derivative to obtain an appropriate gain.

As shown in Fig. 3, the vision controller is used as the uncalibrated servo control model, and the task function is constructed according to the comparison between the desired characteristics of the target and the current image characteristics. After processing the tea image information, it enters the servo system to circulate. The image information is intelligently approximated by a neural network as a vision controller to solve the inverse of the image interaction matrix. During one picking cycle, the L ₂ norm (||E|| ₂ , d||E|| ₂ /dt) of the error and error derivative in the image will be taken as input to calculate the proposed variable gain λ _a . The inverse matrix and variable gain work together in the robot kinematic controller based on fuzzy logic (FL) unit, and the robot will follow the constraints of the image field of view, so as to obtain the Jacobian matrix of the delta parallel mechanism and its inverse. After that, on the one hand, the movement of the picking hand is driven by the rotation angle of the motor, and on the other hand, the variable gain λ _a is affected by the actual working constraint M(J). The camera moves with the picker and compares the captured image with the expected image to minimize the task function. The embodiments of the present invention extract features from images of tea sprouts in the form of point features.

Figure 4 depicts the pinhole model, a coordinate point in three-dimensional space, represented as P(X, Y, Z) in the camera's coordinate system. The analytical derivation of the interaction matrix adopts the point feature of the pinhole model, where λ is the focal length of the camera.

In Figure 4, there is a point p(u, v) on the projection surface, and the relationship between the actual target point P(X, Y, Z) and its coordinates can be expressed by formula (9):

The relationship between the projection speed of the point feature on the image plane and the speed of the picking hand is given by equation (10):

代替L_s，Z值使用了期望值f^*的Z值。上式仅为一个点特征，须对每个特征点分别求得^[24-26]。Among them, L _s is the interaction matrix, Z is the position depth of point P in space, and the complexity of acquisition is high. For this, use

Instead of L _s , the Z value of the expected value f ^* is used. The above formula is only a point feature, which must be obtained separately for each feature point ^[24-26] .

Following the design steps of the controller, the speed is transmitted to the servo system as a control signal. After the task function is determined, the relationship between the error and the speed is expressed as formula (11):

The characteristic error of the task function is reduced exponentially, and Equation (12) is obtained:

Simultaneously formula (11) and formula (12), finally obtain the definition formula (13) of speed:

收敛则会产生固定输出值。并且

的解析存在一些障碍，不仅仅是矩阵的奇异性，还有相机以及特征图像中的噪声都会使其解析难度增加。收敛单元的输入为任务函数，收敛得到交互逆矩阵与误差向量的乘积函数。由此，便能得到6个与特征点数量无关且仅影响线速度和角速度的输出。L _s ⁺ is the inverse of the interaction matrix, λ is the gain value, and v _K is the vector of the camera's linear and angular velocities in the reference frame. The interaction matrix is defined by L _s ∈ R ^k×6 , and the output value after the convergence of the inverse matrix changes with the number of point features, but using

Convergence produces a fixed output value. and

There are some obstacles to the analysis of , not only the singularity of the matrix, but also the noise in the camera and the feature image will make the analysis more difficult. The input of the convergence unit is the task function, and the product function of the interaction inverse matrix and the error vector is obtained by convergence. As a result, six outputs that are independent of the number of feature points and only affect the linear and angular velocities can be obtained.

The focus of the vision controller's extreme learning machine (GA-ELM) algorithm based on genetic optimization is: ELM, as a single hidden layer algorithm with fast learning speed, must minimize the error output after learning. Before its learning, the GA algorithm is used to optimize its input weight W, output weight β and bias b to avoid improper selection of algorithm parameters and promote the ELM algorithm to improve the output accuracy. The process is shown in Figure 5,

1), GA-ELM initialization parameters, encoding the parameters of the ELM network;

2), through the initial population and evaluation of adaptability, to obtain the best parameters in GA;

3) After S-C-M, the optimal parameters of ELM are obtained;

4), put the optimal parameters into the learning, the network starts to learn, and the inverse of the interaction matrix is fitted and calculated

Further, in actual work, the movement of the picking hand, insufficient contrast of the target background, and the buds are blocked by other leaf buds, all of which may cause the loss of image features, thus making the servo task impossible to complete. In addition, there may be noise in the transmission of the head after the image is captured, and there will be errors in the image processing, thereby affecting the accuracy of the system. According to this problem, the embodiment of the present invention uses a homography matrix to construct a task function. The solution of the homography matrix depends on the feature points in the image, and the number of feature points is not less than 4 pairs. In the image of tea buds, the feature points are more clear. When the number of identified feature points increases, the image noise has a more suitable robustness with the help of the task function in the system. The number of feature points will not affect the dimensions of the homography matrix and the task function, nor will it affect the real-time performance of the system.

The task function of conventional uncalibrated visual servoing is defined as Eq. (14):

是估计的单应矩阵的第i行。in,

is the ith row of the estimated homography matrix.

The error vector of the system is expressed as Equation (15):

以获得每次迭代中当前和所需特征点之间的唯一投影单应矩阵

可以证明e＝0当且仅当旋转矩阵R＝I_3×3和位置向量t＝0。where h ₀ is constructed by stacking the rows of the identity matrix I _3×3 . The embodiment of the present invention can be considered as making the current camera frame F and F ^* coincide. To guarantee this, constraints are defined

to obtain a unique projected homography matrix between the current and desired feature points in each iteration

It can be shown that e=0 if and only if the rotation matrix R=I _3×3 and the position vector t=0.

其中h₄是单应矩阵的最后一个元素，并且引入了直接线性变换(DLT)来估计单应矩阵。首先，有必要对两幅图像分别进行归一化，具体如下:In this embodiment of the present invention, constraints are defined

where _h4 is the last element of the homography matrix and a direct linear transform (DLT) is introduced to estimate the homography matrix. First, it is necessary to normalize the two images separately, as follows:

和

保证这些特征点的质心在原点；translate feature points to

and

Ensure that the centroids of these feature points are at the origin;

和

以使它们与原点的平均距离等于

Scale the feature points to

and

so that their average distance from the origin equals

Based on the properties of homogeneous coordinates, the transformed pixel coordinates can be used to construct homogeneous simultaneous equations, such as equation (16):

为

的第i行；上述归一化操作的目的是防止系数矩阵A因图像噪声而病态。where A is the coefficient matrix,

for

The ith row of ; the purpose of the above normalization operation is to prevent the coefficient matrix A from being ill-conditioned by image noise.

的比例为

新的任务函数构造为式(17)：therefore,

The ratio is

The new task function is constructed as formula (17):

反映了单应矩阵的四个自由度。in

Reflects the four degrees of freedom of the homography matrix.

The error vector of the system is defined as:

where e=0 if and only if the rotation matrix R=0 and the position vector t=0.

This new task function takes full advantage of the four-degree-of-freedom property of the homography matrix. This not only reduces the dimension of the task function, but also simplifies the state space of the visual servo system that needs to be estimated online, thereby further improving the real-time performance of the servo system.

In traditional servo systems, the speed of convergence of the task function is considered the most important performance criterion, so that the speed limit of the picker is not taken into account. The servo system should converge faster within the speed limit, a variable gain λ _a is defined, which is obtained from the input of the error of the image and the norm of the derivative of the error, which is then looped into the FL unit based system motion controller. The change of λ _a has an impact on the running speed of the picker, and important constraints such as the working boundary need to be considered in the trajectory planning of the Delta parallel mechanism. Therefore, a constraint is established in the designed non-calibration servo system, which is the restriction on the movement of the picking hand, and is also the total evaluation function that it can stop at any time and avoid singularity, which is defined as formula (19):

Among them, J(θ) is the speed Jacobian matrix of the picking hand. After it is determined, it enters the system loop, and this function is also used as the input of the FL unit.

Example 2:

The present invention also provides a tea picking control method of a tea picking robot, comprising the following steps:

Step S1, obtaining the uncalibrated visual servo control information through the uncalibrated visual servo control model according to the shoot image obtained by the camera through the vision controller;

Step S2, controlling the picking hands to pick tea leaves according to the uncalibrated visual servo control information through the Delta parallel mechanism.

As an implementation of this embodiment, step S1 includes:

Obtain the sprout image captured by the camera;

The features of the sprout image are compared with the features of the target desired image through the uncalibrated visual servo task function and the extreme learning machine algorithm based on genetic optimization, and the uncalibrated visual servo control information is obtained.

As an implementation manner of the embodiment of the present invention, step S2 includes:

According to the uncalibrated visual servo control information and the Jacobian matrix, the picking trajectory is obtained; wherein, the Jacobian matrix is the relationship matrix between the movement speed of the picking hand and the rotational speed of the motor;

According to the picking track, the picking hands are controlled to pick tea leaves.

Further, through the guidance of the vision controller to the tea picking robot, the Delta parallel mechanism controls the picking hand to move the camera to the target point while taking pictures, and compares it with the help of the task function until it reaches the working point, and cuts the tender bud picking point by the picking hand. After that, the leaf buds are adsorbed into the collection box by the flow of gas in the negative pressure straw.

Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, but not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that: The technical solutions described in the foregoing embodiments can still be modified, or some or all of the technical features thereof can be equivalently replaced; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the technical solutions of the embodiments of the present invention. scope.

Claims (8)

1. A tea-picking robot based on uncalibrated visual servoing is characterized by comprising: a camera, a picking hand, a vision controller and a Delta parallel mechanism, wherein,

the visual controller is used for obtaining uncalibrated visual servo control information through an uncalibrated visual servo control model according to the tender shoot image obtained by the camera;

and the Delta parallel mechanism is used for controlling the picking hands to pick tea according to the uncalibrated visual servo control information.

2. The tea-plucking robot based on uncalibrated visual servoing of claim 1, wherein the visual controller comprises:

the acquisition module is used for acquiring a tender shoot image shot by the camera;

and the servo control module is used for comparing the characteristics of the tender shoot image with the characteristics of the target expected image through an uncalibrated visual servo task function and an extreme learning machine algorithm based on genetic optimization to obtain visual servo control information.

3. The tea-plucking robot based on uncalibrated visual servoing of claim 2, wherein the Delta parallel mechanism comprises:

the calculation module is used for obtaining a picking track according to the uncalibrated visual servo control information and the Jacobian matrix; the Jacobian matrix is a relational matrix of the motion speed of the picking hand and the rotating speed of the motor;

and the picking module is used for controlling the picking hands to pick tea leaves according to the picking track.

4. The tea-plucking robot based on uncalibrated visual servoing of claim 3, wherein the degree of freedom of the Delta parallel mechanism is 3DOF, and the motion of the static platform of the Delta parallel mechanism is kept in translation in space.

5. A tea picking control method based on a non-calibration visual servo tea picking robot is characterized by comprising the following steps:

step S1, obtaining uncalibrated visual servo control information through the uncalibrated visual servo control model according to the tender shoot image obtained by the camera through the visual controller;

and step S2, controlling a picking hand to pick tea leaves through a Delta parallel mechanism according to the uncalibrated visual servo control information.

6. The tea-plucking control method based on the uncalibrated vision servo tea-plucking robot of claim 5, wherein the step S1 comprises:

acquiring a tender shoot image shot by a camera;

and comparing the characteristics of the tender shoot image with the characteristics of the target expected image through an uncalibrated visual servo task function and an extreme learning machine algorithm based on genetic optimization to obtain visual servo control information.

7. The tea-plucking control method based on the uncalibrated vision servo tea-plucking robot of claim 6, wherein the step S2 comprises:

obtaining a picking track according to the visual servo control information and the Jacobian matrix; the Jacobian matrix is a relational matrix of the motion speed of the picking hand and the rotating speed of the motor;

and controlling the picking hands to pick tea leaves according to the picking track.

8. The tea-plucking control method based on the uncalibrated visual servo tea-plucking robot according to claim 7, wherein the degree of freedom of the Delta parallel mechanism is 3DOF, and the motion of the static platform of the Delta parallel mechanism is kept in translation in space.

Images