CN103716399A - Remote interaction fruit picking cooperative asynchronous control system and method based on wireless network - Google Patents

Abstract

The invention discloses a remote interaction fruit picking cooperative asynchronous control system based on a wireless network, and belongs to the field of data identification. The remote interaction fruit picking cooperative asynchronous control system comprises a visual information collection module, a robot posture and position information collection module, a robot motion control module, a network transmission module, a man-machine interaction module, a three-dimensional coordinate calculation module and a robot task list module. According to the remote interaction fruit picking cooperative asynchronous control system, due to cooperative asynchronous control based on the wireless network, fruit identification operation, locating operation and picking execution operation of fruit picking robots can be asynchronous, and efficiency is improved; meanwhile, the remote interaction fruit picking cooperative asynchronous control system gives play to the advantage of the identification capacity of humans in an unstructured environment and the advantage of accurate locating of the robots, the identification space range is wide, the identification type is not limited, and whether fruits are ripe or not can be judged.

Description

The collaborative asynchronous control system of remote interaction picking fruit and method based on wireless network

Technical field

What the present invention relates to is a kind of system and method for field of data recognition, specifically a kind of remote interaction picking fruit cooperative control system and method based on wireless network.

Background technology

The development of wireless network, the particularly development of IEEE802.11 standard, constantly increase transmission rate, communication distance, makes robot controlled in wireless become possibility.WLAN (wireless local area network) based on 802.11 standards (Wlan) is most popular wireless local area network technology, and its cost performance is high, networking flexibility convenient, and this makes it in mechanization of agriculture and intellectuality, have important use value.

Fruit picking robot is for this custom-designed robot of specific task of picking fruit.At present, fruit identification and the locating module of research and the fruit picking robot of having developed are all positioned on robot body.The Silsoe research institute of Britain has developed mushroom harvesting (1994), plucks success rate in 75% left and right, and picking rate is 6.7/s, and it is to pluck failed main cause that growth is tilted.Holland's agricultural environment Graduate School of Engineering is developed cucumber harvesting robot (1996), and cucumber is detected as power and is greater than 90%, plucks success rate approximately 80%, but picking rate is about 54s/, the time length can not meet commercialization.QingBei, Korea university has developed apple-picking machine (1998) and has reached 85% from the discrimination of the outside identification of tree crown apple, and speed reaches 5/s, and success rate can not meet application requirements.Its new grade of Cao of Shanghai Communications University is developed Strawberry recognition algorithm (2008), and in laboratory environment, average distinguishing speed is 1s, and carpopodium False Rate is 7%, and harvesting process causes damage to 5% fruit.The people such as China Agricultural University's discipline is super, Li Wei develop cucumber picking robot, and the success rate of gathering reaches 85%, and single cucumber is plucked 28.6s consuming time, has higher practicality.Be subject to the restriction of robot operational capability, identification and low, the consuming time length of location link accuracy rate, can not drop into production application.

Through the retrieval of prior art is found, Chinese patent literature CN102682286A, open day 2012.09.19, a kind of picking robot fruit recognition methods technology based on laser vision system is disclosed, the summary of this technology is: it is laser vision system that laser range finder and rectilinear translation mechanical mechanism are closed to structure, obtain fruit tree local distance information, utilize the 3-D view of the far and near feature of agreement relation generation mark scene of distance value and gray value.After level and smooth this 3-D view, centre of form coordinate and the radius of fruit in employing chain code following technology and at random round detection method computed image.

But defect and deficiency that this technology is compared with the present invention are: identification spatial dimension has limitation: only can process part, be difficult to the whole fruits on fruit tree effectively to be identified; Identification kind scope has limitation: can only process circular fruit; Can not judge the maturity of fruit, make follow-up harvesting operation pluck green fruit, cause economic loss.

Summary of the invention

The present invention is directed to prior art above shortcomings, a kind of remote interaction picking fruit cooperative control system and method based on wireless network is provided, make fruit identification, location and the execution harvesting of fruit picking robot asynchronous, raise the efficiency.

The present invention is achieved by the following technical solutions:

The present invention relates to the collaborative asynchronous control system of a kind of remote interaction picking fruit based on wireless network, comprise: visual information acquisition module, robot pose and positional information acquisition module, robot motion control module, network transmission module, human-computer interaction module, three-dimensional coordinate resolve module and robot task list block, wherein:

Visual information acquisition module, robot pose and positional information acquisition module, robot motion control module and robot task list block are arranged in robot, and human-computer interaction module and three-dimensional coordinate resolve module and be arranged at main control computer;

Robot task list block is sent timestamp to visual information module and robot pose and positional information acquisition module, the image information of visual information module collecting fruit also together transfers to network transmission module with timestamp, robot pose and positional information acquisition module are that timestamp is set up message breakpoint, network transmission module is sent to image information and timestamp the human-computer interaction module of main control computer, human-computer interaction module is determined the fruit scope of image information, by three-dimensional coordinate, resolve module and determine that each fruit in this fruit scope is with respect to robot camera and the three-dimensional world coordinate sequence corresponding with timestamp, each three-dimensional world coordinate sequence and timestamp return to robot task list block by network transmission module, robot task list block reads according to the rule of first in first out the three-dimensional world coordinate sequence that timestamp is corresponding, transformation matrix determines that fruit to be plucked is with respect to the coordinate sequence of current binocular camera shooting head coordinate system, motion-control module resolves and sends control signal according to this coordinate sequence is contrary, the harvesting action of control.

Described visual information acquisition module comprises: two are arranged at camera in robot and for the image compression module of compressed image information.

Described network transmission module comprises: application layer, network layer, transport layer and physical layer, wherein: application layer adopts custom protocol, network layer adopts Transmission Control Protocol, transport layer adopts IP protocol family, physical layer comprises: wireless omnidirectional access point group, hub and the wireless directional relay of mutual communication, access point Zu Yu robot of wireless omnidirectional communicates, and wireless directional relay and main control computer communicate.

Described custom protocol specifically comprises: timestamp, information type, stem check portion, instruction department.

Described robot task list block comprises: for sending the newly-built TU task unit of timestamp, be used for reading, coordinate transforming sequence and the abortive task management unit that sorts, for discharging the resource releasing unit of the shared hardware resource of finishing the work.

The present invention relates to the control method of said system, the method creation-time stamp, the image information of correspondent time collecting fruit and robot pose and positional information, for image information is set up coordinate sequence as the task list of robot, action is plucked according to this task list by robot, comprises the following steps:

Step 1, robot task list block are sent timestamp to visual information module and robot pose and positional information acquisition module, the image information of visual information module collecting fruit also together transfers to network transmission module with timestamp, robot pose and positional information acquisition module are that timestamp is set up message breakpoint, and network transmission module is sent to main control computer by image information and timestamp;

The human-computer interaction module of step 2, main control computer is determined the fruit scope of image information, resolves module determine that each fruit in this fruit scope is with respect to robot camera and the three-dimensional world coordinate sequence corresponding with timestamp by three-dimensional coordinate;

Step 3, each three-dimensional world coordinate sequence and timestamp return to robot task list block by network transmission module, robot task list block reads according to the rule of first in first out the three-dimensional world coordinate sequence that timestamp is corresponding, transformation matrix determines that fruit to be plucked is with respect to the coordinate sequence of current two camera coordinate systems, motion-control module is according to this coordinate sequence against resolving and send control signal, and the harvesting of control is moved.

Described step 1 specifically comprises the following steps:

1) the task list module of robot is sent timestamp to visual information acquisition module, robot pose and positional information acquisition module;

2) vision collecting module is used its two video cameras that are arranged at robot binocular position to take and compress the figure frame of two-way image information, transfers in the lump network transmission module with corresponding timestamp;

3) network transmission module packaging time stamp and figure frame, be transferred to main control computer;

4) robot pose and positional information acquisition module gather attitude and positional information and are that the timestamp receiving is set up breakpoint sign.

Described attitude and positional information comprise: for describing height and the angle of pitch of The Cloud Terrace at robot location's compass output information, driver motor encoder output information and binocular camera shooting head place.

Described breakpoint is masked as and points to the attitude of time of receipt (T of R) during stamp and the pointer of positional information memory address.

Described step 2 specifically comprises the following steps:

1) application layer of network transmission module is unpacked the information of obtaining, and obtains the image information after timestamp and compression;

2) image information display after decompress(ion) is in the visual interface of human-computer interaction module, determine the fruit to be plucked on the two width images that two cameras take, in two width images of same frame, match fruit position, by three-dimensional coordinate, resolve the three-dimensional world coordinate (x that module is calculated each fruit to be plucked _wy _wz _w), coupling has been calculated fruits all on image successively;

The three-dimensional world coordinate computational process of each described fruit to be plucked specifically comprises the following steps:

2.1) fruit is with respect to the two-dimensional coordinate (x, y) of single camera:

$Z_c\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]=\begin{array}{c}\left[\begin{array}{cccc}\frac{f}{\mathrm{dx}}& s& u_0& 0\\ 0& \frac{f}{\mathrm{dy}}& v_0& 0\\ 0& 0& 1& 0\end{array}\right]\end{array}\left[\begin{array}{cc}R& T\\ 0^T& 1\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]=M_1{M}_2\begin{array}{c}\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]\end{array}=M\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right];$

Wherein: (x, y) be under image coordinate system fruit with respect to the two-dimensional coordinate of single camera; Z _cfor Description Image coordinate is tied to the Coordinate Conversion factor of the transformation relation of world coordinate system; M ₁for intrinsic parameters of the camera; M ₂for video camera external parameter; M=M ₁m ₂; F represents video camera effective focal length; Dx, dy represents horizontal and vertical pixel unit length; (u ₀, v ₀) origin of coordinates of presentation video coordinate origin under computer coordinate system, unit is millimeter; S represents the distortion parameter in non-linear camera model;

Intrinsic parameters of the camera M ₁for computer coordinate is tied to the transformation relation of image coordinate system:

$\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]=\left[\begin{array}{ccc}\frac{1}{\mathrm{dx}}& s& u_0\\ 0& \frac{1}{\mathrm{dy}}& v_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}u\\ v\\ 1\end{array}\right];$

The external parameter M of video camera ₂for the transformation relation of world coordinate system and computer coordinate system, R represents a definite quadrature spin matrix, and T represents a definite translation matrix:

$\left[\begin{array}{c}u\\ v\\ w\\ 1\end{array}\right]=\left[\begin{array}{cc}R& T\\ 0^T& 1\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right];$

2.2) show that fruit is with respect to the depth z of binocular camera shooting head: establish C ₁, C ₂be respectively the optical centre of two video cameras, b is C ₁, C ₂horizontal range; F is the focal length of camera, P ₁and P ₂for the mapping point of space geometry point P in two video camera imaging planes; Cross C ₁and C ₂to camera coordinates, be that plane is done to slaver over, intersection point is A ₁and A ₂; Crossing P is that plane is made intersection point to camera coordinates, and intersection point is B, meets at an E with imaging plane; If A ₁c ₁=i _a, A ₂c ₂=i _b; ?

2.3) spatial point x _wy _wz _w ^tprojection on the image of two video cameras is expressed as:

$Z_{c1}\left[\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right]=M^{\′}\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]=\left[\begin{array}{cccc}{m}_{11}^1& m_{12}^1& m_{13}^1& m_{14}^1\\ m_{21}^1& m_{22}^1& m_{23}^1& m_{24}^1\\ m_{31}^1& m_{32}^1& m_{33}^1& m_{34}^1\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right];$

$Z_{c2}\left[\begin{array}{c}{u}_2\\ v_2\\ 1\end{array}\right]=M^{\′\′}\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]=\left[\begin{array}{cccc}{m}_{11}^2& m_{12}^2& m_{13}^2& m_{14}^2\\ m_{21}^2& m_{22}^2& m_{23}^2& m_{24}^2\\ m_{31}^2& m_{32}^2& m_{33}^2& m_{34}^2\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right];$

Wherein: Z _c1and Z _c2the image coordinate that is respectively two video cameras acquisitions is tied to the Coordinate Conversion factor of world coordinate system, and M ', M ' ' are respectively the product of two intrinsic parameters of the camera and external parameter,

k=1,2, i=1,2,3,4, j=1,2,3 representing matrix elements;

2.4) establish the x of world coordinate system _wo _wy _wplane overlaps with the image coordinate system of one of them video camera, x _waxle is optical axis, by the position relationship of two video cameras, calculates spatial point x _wy _wz _w ^t, wherein, R _cthe quadrature spin matrix that represents the position relationship of two video cameras, T _cthe translation matrix that represents the position relationship of two video cameras: $\left[\begin{array}{c}{x}_{c2}\\ y_{c2}\\ z_{c2}\\ 1\end{array}\right]=\left[\begin{array}{cc}{R}_c& T_c\\ 0^T& 1\end{array}\right]\left[\begin{array}{c}{x}_{c1}\\ y_{c1}\\ z_{c1}\\ 1\end{array}\right].$

The transformation matrix of described step 3 refers to: attitude and positional information acquisition module are that waiting task timestamp is established message breakpoint, according to this message breakpoint place attitude and location expression matrix, current attitude and location expression matrix, obtains fruit to be plucked with respect to the coordinate sequence of two camera coordinate systems.

Described step 3 specifically comprises step:

1) the three-dimensional world coordinate of each fruit to be plucked and timestamp return to robot task list block by network transmission module;

2) transformation matrix: attitude and the location expression matrix of establishing message breakpoint place robot are S ₀, the attitude of current robot and location expression matrix are S _p, transformation matrix S _c, as shown in the formula expression:

S _p=S _cs ₀,

X _wy _wz _wthe three-dimensional world coordinate that represents each fruit to be plucked, fruit to be plucked relatively with the coordinate system x of current two video cameras _p, y _p, z _pfor:

$\left[\begin{array}{c}{x}_p\\ y_p\\ z_p\end{array}\right]=S_c\·\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\end{array}\right]=S_p\·S_0^{-1}\·\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\end{array}\right];$

The attitude of described robot and location expression matrix S ₀attitude and location expression matrix S with current robot _pcomprise: compass output information, driving wheel are clicked height and the angle of pitch of the The Cloud Terrace at encoder output information and binocular camera shooting head place.

Technique effect

The present invention is based on the collaborative asynchronous control of wireless network, make fruit identification, location and the execution harvesting of fruit picking robot asynchronous, raise the efficiency; Meanwhile, native system has been brought into play the people's accurate location of identification capability and robot advantage separately under destructuring environment, and identification spatial dimension is wide, and identification kind is not limit, and can whether judge fruit maturation.Relative with the conventional fruit recognizer based on machine vision, can effectively avoid illumination shakiness and block the problem of bringing.

Accompanying drawing explanation

Fig. 1 is the system connection diagram of embodiment 1;

Fig. 2 is the custom protocol of application layer.

Fig. 3 is the topological structure schematic diagram of physical layer;

Fig. 4 is the method step schematic diagram of embodiment 2;

Fig. 5 is the coordinate system schematic diagram of embodiment 2;

Fig. 6 is the vision range finding amount principle schematic of embodiment 2.

Embodiment

Below embodiments of the invention are elaborated, the present embodiment is implemented take technical solution of the present invention under prerequisite, provided detailed execution mode and concrete operating process, but protection scope of the present invention is not limited to following embodiment.

Embodiment 1

As shown in Figure 1, the present embodiment comprises: visual information acquisition module, robot pose and positional information acquisition module, robot motion control module, network transmission module, human-computer interaction module, three-dimensional coordinate resolve module and robot task list block, wherein:

Visual information acquisition module, robot pose and positional information acquisition module, robot motion control module and robot task list block are arranged in robot, and human-computer interaction module and three-dimensional coordinate resolve module and be arranged at main control computer;

Robot task list block is sent timestamp to visual information module and robot pose and positional information acquisition module, the image information of visual information module collecting fruit also together transfers to network transmission module with timestamp, robot pose and positional information acquisition module are that timestamp is set up message breakpoint, network transmission module is sent to image information and timestamp the human-computer interaction module of main control computer, human-computer interaction interface is determined the fruit scope of image information, by three-dimensional coordinate, resolve module and determine that each fruit in this fruit scope is with respect to robot camera and the three-dimensional world coordinate sequence corresponding with timestamp, each three-dimensional world coordinate sequence and timestamp return to robot task list block by network transmission module, robot task list block reads according to the rule of first in first out the three-dimensional world coordinate sequence that timestamp is corresponding, transformation matrix determines that fruit to be plucked is with respect to the coordinate sequence of current binocular camera shooting head coordinate system, motion-control module resolves and sends control signal according to this coordinate sequence is contrary, the harvesting action of control.

Described visual information acquisition module comprises: be arranged at binocular camera shooting head in robot and for the image compression module of compressed image information.

As shown in Figure 2, described custom protocol specifically comprises: timestamp DD, HH, MM, SS, MS, information type TYPE, stem check portion, instruction department data.

Described network transmission module comprises: application layer, network layer, transport layer and physical layer, and wherein: application layer adopts custom protocol, network layer adopts Transmission Control Protocol, and transport layer adopts IP protocol family,

As shown in Figure 3, physical layer comprises: wireless omnidirectional access point group, hub and the wireless directional relay of mutual communication, and access point Zu Yu robot of wireless omnidirectional communicates, and wireless directional relay and main control computer communicate.

Described robot task list block comprises: for sending the newly-built TU task unit of timestamp, be used for reading, coordinate transforming sequence and the abortive task management unit that sorts, for discharging the resource releasing unit of the shared hardware resource of finishing the work.

Embodiment 2

As shown in Figure 4, the present embodiment comprises the following steps:

1, by robot task list block, send timestamp, visual information module collection receives after timestamp, and the image information collecting is transferred to network transmission module.With this simultaneously, robot pose and positional information acquisition module are that timestamp is set up message breakpoint.Network transmission module package image and timestamp are sent to the network transmission module of main control computer.

1.1, the task list module of robot is sent timestamp to visual information acquisition module, robot pose and positional information acquisition module.

1.2, vision collecting module is used its two binocular cameras that are arranged at robot binocular position to take and compress the figure frame of two-way image information, gives in the lump network transmission module with corresponding timestamp.

1.3, network transmission module packaging time stamp and figure frame, be transferred to main control computer;

1.4, robot pose and positional information acquisition module gather attitude and positional information and are that the timestamp receiving is set up breakpoint sign.

Described attitude and positional information comprise: for describing height and the angle of pitch of the The Cloud Terrace at robot location's compass output information and driver motor encoder output information, binocular camera shooting head place.

Described breakpoint is masked as and points to the attitude of time of receipt (T of R) during stamp and the pointer of positional information memory address.

2, the application layer of network transmission module is unpacked the information of obtaining, obtain the image information after timestamp and compression, image information display after decompress(ion) is in the visual interface of human-computer interaction module, by user, by masterplate frame, select method to click successively the fruit to be plucked on two width images, three-dimensional coordinate resolves module and determines that in two field picture, fruit is with respect to the three-dimensional coordinate point sequence of camera, and the application layer of network transmission module returns to robot by timestamp and three-dimensional coordinate sequence of packets.

2.1, the application layer of main control computer network transmission module is unpacked the information of obtaining, and obtains the image information after timestamp and compression.

2.2, the image after decompress(ion) is shown in the visual interface of human-computer interaction module, fruit to be plucked on the two width images that selected method to click successively to be taken by binocular camera shooting head respectively by masterplate frame by user, in two width images of same frame, match fruit position, transfer to three-dimensional coordinate in another thread to resolve module and calculate the coordinate with respect to binocular camera shooting head of each fruit to be plucked (x y z), coupling has been calculated fruits all on image successively.

Computational process is as follows: as shown in Figure 5, according to the external parameter of the inner parameter of video camera, video camera, obtain fruit with respect to the two-dimensional coordinate (x, y) of single camera; By vision range finding amount principle, obtain fruit with respect to the depth z of binocular camera shooting head; By the geometrical relationship between two cameras, obtained plucking the three-dimensional world coordinate system coordinate (x y z) of fruit.

Described masterplate frame selects method to be: select circular shuttering, rectangle template and the arc template of prepackage as mouse pointer, by mouse roller, change in proportion template size, by right button, add mouse roller combination and change Aspect Ratio, long-time by the arc radian of left mouse button towing change, make staff indicate fruit region, comprise the possible region that is blocked.

Described fruit is represented by formula 1 with respect to the two-dimensional coordinate (x, y) of single camera:

$Z_c\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]=\begin{array}{c}\left[\begin{array}{cccc}\frac{f}{\mathrm{dx}}& s& u_0& 0\\ 0& \frac{f}{\mathrm{dy}}& v_0& 0\\ 0& 0& 1& 0\end{array}\right]\end{array}\left[\begin{array}{cc}R& T\\ 0^T& 1\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]=M_1{M}_2\begin{array}{c}\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]\end{array}=M\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right];$

Wherein: x, y be under image coordinate system fruit with respect to the two-dimensional coordinate of single camera; Z _cfor Description Image coordinate is tied to the Coordinate Conversion factor of the transformation relation of world coordinate system; M ₁for intrinsic parameters of the camera; M ₂for video camera external parameter; M=M ₁m ₂; F represents video camera effective focal length; Dx, dy represents horizontal and vertical pixel unit length; u ₀, v ₀the origin of coordinates of presentation video coordinate origin under computer coordinate system, unit is millimeter; S represents the distortion parameter in non-linear camera model.

Described intrinsic parameters of the camera M ₁by the two-step method based on radial arrangement restraint, demarcate and draw.

Described computer coordinate is the cartesian coordinate system of arrangement mode of the image pixel of the capable v of u row, and its initial point is defined as first pixel in the image upper left corner, and unit is pixel; Described image coordinate system is cartesian coordinate systems coplanar from computer coordinate system but different initial points, (the u that initial point is ₀, v ₀), unit is millimeter.

As shown in Figure 5, binocular camera coordinate system is respectively two three-dimensional system of coordinates, wherein x _c1o _c1y _c1, x _c2o _c2y _c2represent respectively two planes, plane x _c1o _c1y _c1with image coordinate system xO ₁y is parallel, ideally z _caxle is the optical axis of video camera, with computer coordinate system uO ₀v meets at O ₁, focal distance f is the distance between two coordinate systems, in reality, optical axis is not orthogonal to x-y plane, with the distortion parameter s in non-linear camera model, describes;

Consider distortion parameter, computer coordinate is tied to the transformation relation of image coordinate system and describes with following formula:

$\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]=\left[\begin{array}{ccc}\frac{1}{\mathrm{dx}}& s& u_0\\ 0& \frac{1}{\mathrm{dy}}& v_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}u\\ v\\ 1\end{array}\right];$

The external parameter M of described video camera ₂for the transformation relation of world coordinate system and computer coordinate system, also by the two-step method based on radial arrangement restraint, demarcate and draw; For selected world coordinate system, coordinate system transformation relation can be provided by a definite quadrature spin matrix R and a definite translation matrix T, coordinate system transformation relation as shown in the formula:

$\left[\begin{array}{c}u\\ v\\ 0\\ 1\end{array}\right]=\left[\begin{array}{cc}R& T\\ 0^T& 1\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right];$

As shown in Figure 6, show that fruit is with respect to the vision range finding amount principle of the depth z of binocular camera shooting head: C wherein ₁, C ₂be respectively the optical centre of binocular camera shooting head, b is C ₁, C ₂horizontal range; F is the focal length of camera, P ₁and P ₂for the mapping point of space geometry point world point P on two camera imaging planes; Cross C ₁and C ₂to camera coordinates, be that plane is done to slaver over, intersection point is A ₁and A ₂; Crossing P is that plane is made intersection point to camera coordinates, and intersection point is B, meets at an E with imaging plane; If A ₁c ₁=i _a, A ₂c ₂=i _b; If z is that space geometry point world point P is to the distance of camera plane; Easily know Δ PEP ₂∽ Δ PBC ₂, Δ PEP ₁∽ Δ PBC ₁thereby, draw:

$\left\{\begin{array}{c}\frac{z-f}{z}=\frac{a}{a+i_b}\\ \frac{z-f}{z}=\frac{a+b-i_a+i_b}{b+i_b+a}\end{array}\right.;$

Cancellation intermediate variable a, obtains

$z=\frac{\mathrm{bf}}{i_a-i_b}.$

From formula 1:

Spatial point x _wy _wz _w ^tprojection on the image of video camera 1 is as shown in Equation 2:

$Z_{c1}\left[\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right]=M^{\′}\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]=\left[\begin{array}{cccc}{m}_{11}^1& m_{12}^1& m_{13}^1& m_{14}^1\\ m_{21}^1& m_{22}^1& m_{23}^1& m_{24}^1\\ m_{31}^1& m_{32}^1& m_{33}^1& m_{34}^1\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right];$

Spatial point x _wy _wz _w ^tprojection on the image of video camera 2 is as shown in Equation 3:

$Z_{c2}\left[\begin{array}{c}{u}_2\\ v_2\\ 1\end{array}\right]=M^{\′\′}\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]=\left[\begin{array}{cccc}{m}_{11}^2& m_{12}^2& m_{13}^2& m_{14}^2\\ m_{21}^2& m_{22}^2& m_{23}^2& m_{24}^2\\ m_{31}^2& m_{32}^2& m_{33}^2& m_{34}^2\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right];$

Z _c1and Z _c2the image coordinate that is respectively two video cameras acquisitions is tied to the Coordinate Conversion factor of world coordinate system, and M ', M ' ' are respectively the product of two intrinsic parameters of the camera and external parameter,

k=1,2, i=1,2,3,4, j=1,2,3 representing matrix elements, establish the x of world coordinate system _wo _wy _wplane overlaps with the image coordinate system of one of them video camera 2, x _waxle is optical axis, as shown in Figure 5,, by the position relationship of two video cameras, can calculate spatial point x _wy _wz _w ^t;

The position relationship of two video cameras is also by quadrature spin matrix R _cwith translation matrix T _cprovide, the position relationship of two video cameras, as shown in Equation 4: $\left[\begin{array}{c}{x}_{c2}\\ y_{c2}\\ z_{c2}\\ 1\end{array}\right]=\left[\begin{array}{cc}{R}_c& T_c\\ 0^T& 1\end{array}\right]\left[\begin{array}{c}{x}_{c1}\\ y_{c1}\\ z_{c1}\\ 1\end{array}\right];$

Simultaneous formula 2,3 and 4 cancellation Z _c1and Z _c2, can be about x _wy _wz _wlinear equation.

3, the timestamp of coordinate sequence and original image transfers to network transmission module to return to robot task list block.Task list reads by the rule of first in first out the three-dimensional coordinate sequence that this timestamp is corresponding, determines the three-dimensional coordinate sequence of the relatively current binocular camera shooting head of fruit to be plucked coordinate system through video camera transformation matrix.Motion-control module is according to the contrary control signal of sending that there emerged a driver of resolving of this three-dimensional coordinate sequence, the harvesting action of control.When plucking operation, repeat fruit space coordinates above-mentioned to be plucked and obtain, form fruit identification, location and carry out pluck asynchronous, improve picking efficiency.

3.1, the timestamp of coordinate sequence and original image transfers to network transmission module to return to the task list module of robot.

3.2, described video camera transformation matrix, be specially: waiting task timestamp is at the message breakpoint of attitude and positional information acquisition module, according to this breakpoint attitude and location expression matrix and current attitude, location expression matrix, release the transformation matrix of binocular camera coordinate system:

If message breakpoint place, the attitude of robot and location expression matrix are S ₀, the attitude of current robot and location expression matrix are S _pre, video camera transformation matrix S _c, as shown in the formula expression:

S _p=S _cs ₀,

Fruit to be plucked relatively with the three-dimensional coordinate (x of current binocular camera shooting head coordinate system _p, y _p, z _p), (x _wy _wz _w) represent the three-dimensional world coordinate of each fruit to be plucked of correspondent time

$\left[\begin{array}{c}{x}_p\\ y_p\\ z_p\end{array}\right]=S_c\·\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\end{array}\right]=S_{\mathrm{pre}}\·S_0^{-1}\·\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\end{array}\right];$

Height and the angle of pitch of the The Cloud Terrace that wherein, attitude and location expression matrix comprise compass output information, driving wheel click encoder output information and binocular camera shooting head place.

Claims (9)

1. the remote interaction picking fruit based on wireless network is worked in coordination with asynchronous control system, it is characterized in that, comprise: visual information acquisition module, robot pose and positional information acquisition module, robot motion control module, network transmission module, human-computer interaction module, three-dimensional coordinate resolve module and robot task list block, wherein:

Visual information acquisition module, robot pose and positional information acquisition module, robot motion control module and robot task list block are arranged in robot, and human-computer interaction module and three-dimensional coordinate resolve module and be arranged at main control computer;

Robot task list block is sent timestamp to visual information module and robot pose and positional information acquisition module, the image information of visual information module collecting fruit also together transfers to network transmission module with timestamp, robot pose and positional information acquisition module are that timestamp is set up message breakpoint, network transmission module is sent to image information and timestamp the human-computer interaction module of main control computer, human-computer interaction module is determined the fruit scope of image information, by three-dimensional coordinate, resolve module and determine that each fruit in this fruit scope is with respect to robot camera and the three-dimensional world coordinate sequence corresponding with timestamp, each three-dimensional world coordinate sequence and timestamp return to robot task list block by network transmission module, robot task list block reads according to the rule of first in first out the three-dimensional world coordinate sequence that timestamp is corresponding, transformation matrix determines that fruit to be plucked is with respect to the coordinate sequence of binocular camera shooting head coordinate system, motion-control module resolves and sends control signal according to this coordinate sequence is contrary, the harvesting action of control.

2. system according to claim 1, is characterized in that, described visual information acquisition module comprises: two are arranged at camera in robot and for the image compression module of compressed image information.

3. system according to claim 1, it is characterized in that, described robot task list block comprises: for sending the newly-built TU task unit of timestamp, be used for reading, coordinate transforming sequence and the abortive task management unit that sorts, for discharging the resource releasing unit of the shared hardware resource of finishing the work.

4. the control method for system described in claim 1-3 any one, it is characterized in that, the method creation-time stamp, the image information of correspondent time collecting fruit and robot pose and positional information, for image information is set up coordinate sequence as the task list of robot, action is plucked according to this task list by robot, comprises the following steps:

Step 1, robot task list block are sent timestamp to visual information module and robot pose and positional information acquisition module, the image information of visual information module collecting fruit also together transfers to network transmission module with timestamp, robot pose and positional information acquisition module are that timestamp is set up message breakpoint, and network transmission module is sent to main control computer by image information and timestamp;

The human-computer interaction module of step 2, main control computer is determined the fruit scope of image information, resolves module determine that each fruit in this fruit scope is with respect to robot camera and the three-dimensional world coordinate sequence corresponding with timestamp by three-dimensional coordinate;

Step 3, each three-dimensional world coordinate sequence and timestamp return to robot task list block by network transmission module, robot task list block reads according to the rule of first in first out the three-dimensional world coordinate sequence that timestamp is corresponding, transformation matrix determines that fruit to be plucked is with respect to the coordinate sequence of two camera coordinate systems, motion-control module is according to this coordinate sequence against resolving and send control signal, and the harvesting of control is moved.

5. method according to claim 4, is characterized in that, described step 1 specifically comprises the following steps:

1) the task list module of robot is sent timestamp to visual information acquisition module, robot pose and positional information acquisition module;

2) vision collecting module is used its two video cameras that are arranged at robot binocular position to take and compress the figure frame of two-way image information, transfers in the lump network transmission module with corresponding timestamp;

3) network transmission module packaging time stamp and figure frame, be transferred to main control computer;

4) robot pose and positional information acquisition module gather attitude and positional information and are that the timestamp receiving is set up breakpoint mark

Will.

6. according to the method described in claim 4 or 5, it is characterized in that, described step 2 specifically comprises the following steps:

1) application layer of network transmission module is unpacked the information of obtaining, and obtains the image information after timestamp and compression;

2) image information display after decompress(ion) is in the visual interface of human-computer interaction module, determine the fruit to be plucked on the two width images that two cameras take, in two width images of same frame, match fruit position, by three-dimensional coordinate, resolve the three-dimensional world coordinate (x that module is calculated each fruit to be plucked _wy _wz _w), coupling has been calculated fruits all on image successively.

7. system according to claim 6, is characterized in that, the three-dimensional world coordinate computational process of each described fruit to be plucked specifically comprises the following steps:

2.1) fruit is with respect to the two-dimensional coordinate (x, y) of single camera:

$Z_c\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]=\left[\begin{array}{cccc}\frac{f}{\mathrm{dx}}& s& u_0& 1\\ 0& \frac{f}{\mathrm{dx}}& v_0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{cc}R& T\\ 0^T& 1\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]=M_1{M}_2\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]=M\begin{array}{c}\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]\end{array};$

Wherein: x, y be under image coordinate system fruit with respect to the two-dimensional coordinate of single camera; Z _cfor Description Image coordinate is tied to the Coordinate Conversion factor of the transformation relation of world coordinate system; M ₁for intrinsic parameters of the camera; M ₂for video camera external parameter; M=M ₁m ₂; F represents video camera effective focal length; Dx, dy represents horizontal and vertical pixel unit length; u ₀, v ₀the origin of coordinates of presentation video coordinate origin under computer coordinate system, unit is millimeter; S represents the distortion parameter in non-linear camera model;

Intrinsic parameters of the camera M ₁for computer coordinate is tied to the transformation relation of image coordinate system:

$\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]=\left[\begin{array}{ccc}\frac{1}{\mathrm{dx}}& s& u_0\\ 0& \frac{1}{\mathrm{dy}}& v_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}u\\ v\\ 1\end{array}\right];$

The external parameter M of video camera ₂for the transformation relation of world coordinate system and computer coordinate system, R represents a definite quadrature spin matrix, and T represents a definite translation matrix:

$\left[\begin{array}{c}u\\ v\\ 0\\ 1\end{array}\right]=\left[\begin{array}{cc}R& T\\ 0^T& 1\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right];$

2.2) show that fruit is with respect to the depth z of binocular camera shooting head: establish C ₁, C ₂be respectively the optical centre of two video cameras, b is C ₁, C ₂horizontal range; F is the focal length of camera, P ₁and P ₂for the mapping point of space geometry point world point P in two video camera imaging planes; Cross C ₁and C ₂to camera coordinates, be that plane is done to slaver over, intersection point is A ₁and A ₂; Crossing P is that plane is made intersection point to camera coordinates, and intersection point is B, meets at an E with imaging plane; If A ₁c ₁=i _a, A ₂c ₂=i _b; If w is the distance that space geometry point world point P arrives camera plane, i.e. depth z:

2.3) spatial point x _wy _wz _w ^tprojection on the image of two video cameras is expressed as:

$Z_{c1}\left[\begin{array}{c}{u}_1\\ v_1\\ 1\end{array}\right]=M^{\′}\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]=\left[\begin{array}{cccc}{m}_{11}^1& m_{12}^1& m_{13}^1& m_{14}^1\\ m_{21}^1& m_{22}^1& m_{23}^1& m_{24}^1\\ m_{31}^1& m_{32}^1& m_{33}^1& m_{34}^1\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right];$

$Z_{c2}\left[\begin{array}{c}{u}_2\\ v_2\\ 1\end{array}\right]=M^{\′\′}\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]=\left[\begin{array}{cccc}{m}_{11}^2& m_{12}^2& m_{13}^2& m_{14}^2\\ m_{21}^2& m_{22}^2& m_{23}^2& m_{24}^2\\ m_{31}^2& m_{32}^2& m_{33}^2& m_{34}^2\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right];$

Wherein: Z _c1and Z _c2the image coordinate that is respectively two video cameras acquisitions is tied to the Coordinate Conversion factor of world coordinate system, and M ', M ' ' are respectively the product of two intrinsic parameters of the camera and external parameter,

k=1,2, i=1,2,3,4, j=1,2,3 representing matrix elements;

2.4) establish the x of world coordinate system _wo _wy _wplane overlaps with the image coordinate system of one of them video camera, x _waxle is optical axis, by the position relationship of two video cameras, calculates spatial point x _wy _wz _w ^t, wherein, R _cthe quadrature spin matrix that represents the position relationship of two video cameras, T _cthe translation matrix that represents the position relationship of two video cameras: $\left[\begin{array}{c}{x}_2\\ y_2\\ z_2\\ 1\end{array}\right]=\left[\begin{array}{cc}{R}_c& T_c\\ 0^T& 1\end{array}\right]\left[\begin{array}{c}{x}_1\\ y_1\\ z_1\\ 1\end{array}\right].$

8. according to the method described in claim 4 or 5, it is characterized in that, the transformation matrix of described step 3 refers to: attitude and positional information acquisition module are that waiting task timestamp is established message breakpoint, according to this message breakpoint place attitude and location expression matrix, current attitude and location expression matrix, obtains fruit to be plucked with respect to the coordinate sequence of two camera coordinate systems.

9. method according to claim 8, is characterized in that, described step 3 specifically comprises step:

1) the three-dimensional world coordinate of each fruit to be plucked and timestamp return to robot task list block by network transmission module;

2) transformation matrix: attitude and the location expression matrix of establishing message breakpoint place robot are S ₀, the attitude of current robot and location expression matrix are S _p, video camera transformation matrix Sc, as shown in the formula expression:

S _p=ScS0,

y _wz _wthe three-dimensional world coordinate that represents each fruit to be plucked, fruit to be plucked relatively with the coordinate system x of current two video cameras _p, y _p, z _pfor:

$\left[\begin{array}{c}{x}_p\\ y_p\\ z_p\end{array}\right]=S_c\·\begin{array}{c}\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\end{array}\right]{=S}_p\end{array}\·S_0^{-1}\·\begin{array}{}\end{array}\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\end{array}\right];$

The attitude of message breakpoint place robot and location expression matrix S ₀attitude and location expression matrix S with current robot _pcomprise: compass output information, driving wheel are clicked height and the angle of pitch of the The Cloud Terrace at encoder output information and binocular camera shooting head place.

Images