CN105964567A - Sorting control system for glass bottles in household garbage - Google Patents

Abstract

The invention discloses a sorting control system for glass bottles in household garbage. The system comprises a bottle posture receiving module, a multi-manipulator dispatch control module, a plurality of path control modules and manipulators with a gravity detection function. The multi-manipulator dispatch control module judges whether the multiple glass bottles enter the working areas of the unoccupied manipulators or not synchronously; the coordinate and attitude angle information of the glass bottles is transmitted to the manipulator path control modules and manipulators corresponding to the unoccupied manipulators; the manipulator path control modules control the manipulators to grab the bottles and judge the weights of the bottles; and if the glass bottle weight features are met, the grabbed bottles are placed in a material container, or the bottles are put back onto a conveyor belt. According to the sorting control system, multiple sorting tasks are distributed to the multiple manipulators simultaneously, besides, each manipulator is additionally provided with a weight sensor for extracting the bottle weight features, and the defect that a machine visual identification system cannot identify the material of a target object is overcome.

Description

A kind of for the vial sorting control system in house refuse

Technical field

The present invention relates to a kind of for the vial sorting control system in house refuse, belong to environmental protection automated arm neck Territory.

Background technology

In the front end of house refuse remanufacture line, need that the vial in rubbish is carried out sorting and reclaim, prevent from crushing Glass follow-up apparatus for production line is damaged.It is all to use manual sorting that vial in existing house refuse reclaims Method, there is certain danger, and sort that person works's bad environments, labor intensity is high and low the lacking of work efficiency Point.The sorting of vial can be used the method that mechanical hand captures, mechanical hand capture has that efficiency is high, low cost and work continuously The advantage made, is therefore used widely.

At present, in house refuse, vial sorting system framework is that conveyer belt front end disposes Vision Builder for Automated Inspection, by right The rubbish paved on conveyer belt carries out image acquisition and image procossing, sends bottle class shape and the posture information of identification to machinery Hand controls, Manipulator Controller control arm end arrival the coordinate points specified is grabbed attitude with certain hands and is carried out glass The crawl of bottle.

But, existing manipulator control major part is all the motor control to separate unit mechanical hand, even multiple stage is mechanical The control of hands is also for different types of sorting target sorting task in different operations.Produce mobile for multiple target The practical situations of the garbage sorting on line, the easy missing inspection of this method for sorting, efficiency is low.It addition, existing sorting identifies Technology, can only identification by the method for image procossing typically using CCD optical camera as the image acquisition element of identification object The shape of sorting object, it is impossible to its material of identification.Vial and plastic bottle are the bottle class sorting objects being easier to obscure, if Realize the sorting to vial it is necessary to while mechanical hand captures sorting object, detect its weight, thus pick out glass Bottle.General mechanical hand cannot be distinguished by whether captured object is vial.

Summary of the invention

The technology of the present invention solves problem: overcome the deficiencies in the prior art, it is provided that a kind of for the glass in house refuse Glass bottle sorting control system, solves the vial fast automatic sorting problem in house refuse, improves garbage sorting efficiency, reduces The labor intensity of workman, improves the automaticity of garbage sorting production line.

The technical solution of the present invention: a kind of multi-robot for the vial in house refuse sorts and controls system System, this system includes bottle pose receiver module, multi-robot dispatching control module, N number of mechanical hand, N number of control mechanical hand TRAJECTORY CONTROL module and weight sensor, mechanical hand and TRAJECTORY CONTROL module one_to_one corresponding, weight sensor is used for collection machinery Hands paw and the joint weight capturing thing, wherein:

Bottle pose receiver module, the bottle posture information frame receiving outside input is deposited into receiving in relief area, institute State bottle posture information frame and include bottle coordinate under imaging plane coordinate system, attitude angle and shooting time；

Multi-robot dispatching control module, extracts the bottle posture information frame received in relief area, calculates bottle pose letter In breath frame, bottle is at t₂The prediction coordinate being engraved in during+Δ T under conveyer belt plane coordinate system, described t₂For current time, Δ T is machine Tool hands receives instruction to the time grabbed required for bottle, it is judged that whether this prediction coordinate falls into the work of any one mechanical hand Making space, if entering the work space of certain mechanical hand, then judging the duty whether " idle " of this mechanical hand, if " idle ", then the mechanical hand that the coordinate under the imaging plane coordinate system of this bottle is distributed to the free time with attitude angle information is corresponding Mechanical hand TRAJECTORY CONTROL module；

Mechanical hand TRAJECTORY CONTROL module, during initialization, controls mechanical hand and is positioned at default ready position；Receive bottle becoming After coordinate information under photo coordinate system, duty is set to " doing ", coordinate information and the attitude angle of this bottle are entered Row-coordinate is changed, and obtains this bottle coordinate under this mechanical hand coordinate system, controls mechanical hand and moves to vial position, whirler The angle of tool hands is consistent with attitude angle, captures bottle, when bottle leaves conveyer belt surface, reads weight sensor values, according to Weight judges whether the bottle captured has the weight characteristics of vial, if it is, deliver to bottle refer to beyond conveyer belt Fixed material memory block, if not the weight characteristics of vial, is then put back into bottle on conveyer belt, afterwards, controls machinery Hands returns to the ready position preset, and duty is set to " idle ".

According to bottle weight, described mechanical hand TRAJECTORY CONTROL module judges that whether the bottle that captured is the method for vial For:

Judge that weight, whether in preset threshold range, if it is, be judged to vial, is otherwise judged to other bottle Class, described threshold value is determined by the statistical weight sum of manipulator paw with vial.

Described N number of mechanical hand arranges along conveyer belt direction, serial number 1～N, and multi-robot dispatching control module circulates Execution following steps:

(1) judge that current time receives whether relief area has new bottle posture information frame, if it has, by new bottle position Appearance information frame is stored in current crawl queue, enters step (2), otherwise, is directly entered step (2)；

(2) according to the principle of first in first out, first the bottle posture information frame captured in queue is chosen；

(3) bottle coordinate information (x under imaging plane coordinate system in bottle posture information frame is resolved_{P_0},y_{P_0}) and clap Take the photograph time t₁；

(4) by bottle coordinate information (x in imaging plane coordinate system_{P_0},y_{P_0}) carry out Coordinate Conversion, obtain conveyer belt and put down Coordinate information (x under areal coordinate system_{C_0},y_{C_0})；

(5) bottle is calculated at t₂Prediction coordinate (the x under conveyer belt plane coordinate system in+Δ T moment_{C_target}, y_{C_target})；

(6) current time judges that coordinate (x predicted by this bottle successively_{C_target},y_{C_target}) whether enter the 1st mechanical hand ～the work space of n-th mechanical hand, if this bottle enters the work space of some mechanical hand, it is determined that this mechanical hand is Mechanical hand to be grabbed, enters step (7), otherwise, is saved in temporary queue by this bottle posture information frame, enters step (8)；

(7) robot work state to be grabbed whether " idle " is judged, if " idle ", then by this bottle posture information frame Imaging plane coordinate system coordinate be sent to the TRAJECTORY CONTROL module that this mechanical hand is corresponding with attitude angle information, enter step (8)； If duty is " hurrying ", then this bottle posture information frame is saved in temporary queue, enters step (8)；

(8) choose next bottle posture information frame, re-execute step (3)～step (7), until having judged to capture team All bottle posture information frames in row, enter step (9)；

(9) empty crawl queue, all bottle posture information frames in temporary queue are stored in crawl queue.

Described step (5) calculates bottle at t by resolving equation below group₂It is engraved in during+Δ T under conveyer belt plane coordinate system Prediction coordinate (x_{C_target},y_{C_target}):

x_{C_target}=x_{C_0}+v×(t₂+ΔT-t₁)

y_{C_target}=y_{C_0}

$\mathrm{\Δ}T=\sqrt{{(x_{R\_t\arg et}-x_{R\_\mathrm{int}})}^2+{(y_{R\_t\arg et}-y_{R\_\mathrm{int}})}^2+{(z_{R\_t\arg et}-z_{R\_\mathrm{int}})}^2}/{V_{R\_}}_{hand}$

$\left[\begin{array}{c}{x}_{R\_t\arg et}\\ y_{R\_t\arg et}\\ 1\end{array}\right]=M_{C\_R\_i}\·\left[\begin{array}{c}{x}_{C\_t\arg et}\\ y_{C\_t\arg et}\\ 1\end{array}\right]$

z_{R_target}=0

Wherein, (x_{R_int}, y_{R_int}, z_{R_int}) it is the ready position of the i-th mechanical hand seat under this robot coordinate system Mark；V_{R_hand}For mechanical hand along the pre-set velocity of point-to-point linear motion, v is the speed of conveyer belt, M_{C_R_i}Sit for conveyer belt plane Mark is tied to the coordinate conversion matrix of i-th robot coordinate system, t₂For current time.

Described multi-robot dispatching control module judges that coordinate (x predicted by this bottle_{C_target},y_{C_target}) whether enter i-th The method of the operation interval of individual mechanical hand is:

(6a) by the i-th mechanical hand at i-th mechanical hand plane coordinate system X_RiO_RiY_RiUnder zero (0,0) sit Mark conversion, obtains this zero coordinate (x under conveyer belt plane coordinate system_{C_i},y_{C_i})；

(6b) (x is judged_{C_target},y_{C_target}) whether meet condition:

Time, mechanical hand Work space be radius be the sector region of R, meet, then it is assumed that (x_{C_target},y_{C_target}) fall into the work of i-th mechanical hand In interval, otherwise it is assumed that (x_{C_target},y_{C_target}) not in the operation interval of this mechanical hand.

In described step (1) middle crawl queue, bottle posture information frame is according to the most regularly arranged: the different shooting moment Bottle posture information frame, arranges according to time order and function order, and the bottle posture information frame in same shooting moment, according to the position of bottle Putting arrangement, the posture information frame near the bottle of mechanical hand is arranged in front.

The present invention compared with prior art has the advantages that

1, the present invention arranges that rubbish is sorted by multiple mechanical hand, makes full use of the sorting ability of every mechanical hand, will Multiple sorting tasks are simultaneously allocated to multiple stage mechanical hand and sort, and can realize the most target of whole system and capture the sorting energy of thing Power, work efficiency is high.

2, invention defines fixing conveyer belt plane coordinate system, whether coordinate falls into the meter of the crawl scope of mechanical hand Calculating is all to calculate at conveyer belt plane coordinate system, advantageously reduces amount of calculation, improves computational efficiency.

3, invention defines multiple robot coordinate system, robot coordinate system changes along with the installation site of mechanical hand, Reduce the process complexity of mechanical hand TRAJECTORY CONTROL module, it addition, multi-robot control module has only to put down bottle imaging The coordinate of areal coordinate system is sent to each mechanical hand TRAJECTORY CONTROL module, each mechanical hand TRAJECTORY CONTROL module be responsible for each needing The coordinate of the bottle captured carries out Coordinate Conversion, simplifies the complexity of multi-robot control module.

4, the present invention solves general sorting identification link and can only identify sorting object profile, it is impossible to identification sorting object It is vial or plastic bottle problem, it is proposed that the weight characteristics of detection sorting object when mechanical hand captures sorting object, makees For judging to capture the condition whether object is vial sorting thing, vial is carried out automatic sorting, is effectively prevented broken The destruction that follow-up apparatus for production line is caused by glass.

Accompanying drawing explanation

Fig. 1 is the system construction drawing of the present invention；

Fig. 2 is the control method flow chart of the present invention；

Fig. 3 sorting manipulator distribution mode.

Detailed description of the invention

The present invention is described in further detail with specific embodiment below in conjunction with the accompanying drawings:

The embodiment of the present invention relates to following several coordinate system, is defined as follows:

(1) imaging plane coordinate system

With the intersection point of photographic head optical axis and conveyer belt plane as zero, imaging plane along conveyer belt moving direction is X_PAxle, with X_PThe direction that direction of principal axis is vertical is Y_PAxle, imaging plane is conveyer belt plane.

(2) conveyer belt plane coordinate system

The initial point of conveyer belt plane coordinate system is a fixing point O at conveyer belt edge_C, X_CAxle is moving along conveyer belt Direction, Y_CIt is positioned at conveyer belt plane and and X_CAxle is vertical.

(3) N number of robot coordinate system

The robot coordinate system of i-th mechanical hand is X_RiO_RiY_Ri, initial point is the pedestal friendship with conveyer belt plane of mechanical hand Point, axle Z_RVertical and conveyer belt plane, axle X_R, axle Y_RRespectively with the axle X of conveyer belt plane coordinate system_CWith axle Y_CDirection is consistent.

Imaging plane coordinate system converts along with imaging plane, and multiple robot coordinate systems become along with the installation site of mechanical hand Change, difficulty and the process complexity of mechanical hand TRAJECTORY CONTROL module that imager coordinate obtains can be reduced.

Fig. 1 is a kind of for the vial sorting control system structured flowchart in house refuse.As it can be seen, this system bag Include bottle pose receiver module, multi-robot dispatching control module, N number of mechanical hand, the TRAJECTORY CONTROL module of N number of control mechanical hand And weight sensor, mechanical hand and TRAJECTORY CONTROL module one_to_one corresponding, weight sensor is used for collection machinery hands paw and crawl The joint weight of thing, is arranged on directly over manipulator paw, and the stress axis of gravity sensor is through the center of gravity of paw, its In:

Bottle posture information receiver module, receives the bottle posture information frame of outside input by serial communication mode, will It is stored in reception relief area, and transmission standard can be RS232, RS485, RS422, and described bottle posture information frame includes bottle Coordinate, attitude angle and shooting time under imaging plane coordinate system；In described posture information, coordinate is that the minimum of bottle is external The Geometric center coordinates of rectangle, described attitude angle information is the long limit angle with X-axis of the minimum enclosed rectangle of bottle；Bottle position The posture information of appearance information frame is with imaging plane coordinate system as reference, and content is brief and contains the whole letters required for crawl Breath, is conducive to shortening delivery time and the data volume of transmission.

Its multiple mechanical hands controlled are scheduling controlling, with mechanical hand track control by multi-robot dispatching control module The communication of module can realize in the way of using industrial field bus.Multi-robot dispatching control module, extracts and receives buffering Bottle posture information frame in district, in calculating bottle posture information frame, bottle is at t₂It is engraved in during+Δ T under conveyer belt plane coordinate system Prediction coordinate, described t₂For current time, Δ T is that mechanical hand receives instruction to the time grabbed required for bottle, it is judged that Whether this prediction coordinate enters the work space of any one mechanical hand, if entering the work space of certain mechanical hand, then sentences The duty of this mechanical hand disconnected whether " idle ", if " idle ", then by the coordinate under the imaging plane coordinate system of this bottle The mechanical hand TRAJECTORY CONTROL module that the mechanical hand of free time is corresponding is distributed to attitude angle information.Multi-robot dispatching control module with The running status controlling each mechanical hand of cycle polling of Millisecond, carries out task distribution to idle mechanical hand in time, it is ensured that Bottle is captured by mechanical hand efficiently.

Mechanical hand TRAJECTORY CONTROL module, during initialization, controls mechanical hand and is positioned at default ready position；Receive the one-tenth of bottle After coordinate information under photo coordinate system, duty is set to " doing ", coordinate information and the attitude angle of this bottle are entered Row-coordinate is changed, and obtains this bottle coordinate under this mechanical hand coordinate system, controls mechanical hand and moves the position to bottle, rotates The angle of mechanical hand is consistent with attitude angle, captures bottle, after bottle leaves conveyer belt plane, inspecting manipuator paw and crawl The joint weight of thing, it is judged that whether its weight, in certain threshold range, if it is, be judged to vial, is sent to The material memory block specified beyond conveyer belt, otherwise, it is determined that be other material bottle, unclamps paw, is placed back into conveyer belt, institute State threshold value to be determined by the statistical weight sum of manipulator paw with vial.Afterwards, the preparation position that mechanical hand returns to preset is controlled Put, duty is set to " idle ".Described default ready position can be the surface of conveyer belt plane, and it is put down at conveyer belt Projection on face is positioned at conveyer belt at Y_PAxial center.This position is conducive to mechanical hand quickly to arrive conveyer belt surface Capture.

Multi-robot control module has only at the coordinate of imaging plane coordinate system, bottle is sent to each mechanical hand track Control module, each mechanical hand TRAJECTORY CONTROL module the coordinate being responsible for each needing the bottle captured carries out Coordinate Conversion, letter Change the complexity of multi-robot control module.

N number of mechanical hand arranges along conveyer belt direction, serial number 1～N, and the circulation of multi-robot dispatching control module performs Following steps:

(1) judge to receive whether relief area has new bottle posture information frame, if it has, by new bottle posture information frame It is stored in current crawl queue, enters step (2), otherwise, be directly entered step (2)

(2) according to the principle of first in first out, first the bottle posture information frame captured in queue is chosen；

(3) bottle posture information frame coordinate information (x under imaging plane coordinate system is resolved_{P_0},y_{P_0}) and shooting time t₁；

(4) by bottle coordinate information (x in imaging plane coordinate system_{P_0},y_{P_0}) carry out Coordinate Conversion, obtain conveyer belt and put down Coordinate information (x under areal coordinate system_{C_0},y_{C_0})；

$\left[\begin{array}{c}{x}_{C\_0}\\ y_{C\_0}\\ 1\end{array}\right]=M_{P\_C}\·\left[\begin{array}{c}{x}_{P\_0}\\ y_{P\_0}\\ 1\end{array}\right]---\left(1\right)$

M_{P_C}The coordinate conversion matrix of conveyer belt plane coordinate system it is tied to for imaging plane coordinate；

Initial point O at conveyer belt plane coordinate system_CAnd X_CAxle and Y_CObvious physical label is set on any point on axle, logical Cross these obvious physical labels at the coordinate figure corresponding to imaging plane coordinate system and the coordinate under conveyer belt plane coordinate system Value can calculate imaging plane coordinate and be tied to the coordinate conversion matrix M of conveyer belt plane coordinate system_{P_C}。

(5) by resolving equation below group calculating target crawl thing at t₂It is engraved in during+Δ T under conveyer belt plane coordinate system Prediction coordinate (x_{C_target},y_{C_target}):

x_{C_target}=x_{C_0}+v×(t₂+ΔT-t₁) (2)

y_{C_target}=y_{C_0} (3)

$\mathrm{\Δ}T=\sqrt{{(x_{R\_t\arg et}-x_{R\_\mathrm{int}})}^2+{(y_{R\_t\arg et}-y_{R\_\mathrm{int}})}^2+{(z_{R\_t\arg et}-z_{R\_\mathrm{int}})}^2}/{V_{R\_}}_{hand}---\left(4\right)$

$\left[\begin{array}{c}{x}_{R\_t\arg et}\\ y_{R\_t\arg et}\\ 1\end{array}\right]=M_{C\_R\_i}\·\left[\begin{array}{c}{x}_{C\_i\arg et}\\ y_{C\_t\arg et}\\ 1\end{array}\right]---\left(5\right)$

z_{R_target}=0 (6)

Wherein, (x_{R_int}, y_{R_int}, z_{R_int}) it is the ready position of the i-th mechanical hand seat under this robot coordinate system Mark；V_{R_hand}For mechanical hand along the pre-set velocity of point-to-point linear motion, v is the speed of conveyer belt, t₂For current time, M_{C_R_i} The coordinate conversion matrix of conveyer belt plane coordinate system, i=1～N it is tied to for i-th mechanical coordinate.

The taper frock installed with arm end carries out point cantact with three particular points on conveyer belt, determines mechanical hand Coordinate system X_RiO_RiY_Ri, plane X_RiO_RiY_RiWith conveyer belt planes overlapping.Precision optical instrument is used to measure conveyer belt plane coordinates The initial point of system and along X_CAxle and along Y_CThe coordinate figure of any on axle, then measure machinery bradawl shape frock with precision optical instrument Cusp is at three the not coordinate figure of collinear position and coordinate figures under robot coordinate system thereof.Can be resolved by such as co-relation Go out transition matrix M_{C_R_i}。

The above-mentioned coordinates of targets Forecasting Methodology when mechanical hand arrives crawl position, is conducive to target is captured object location Accurate judgement, increase the degree of accuracy captured.Avoiding calculating error causes target crawl thing not exist when mechanical hand captures In its work space, cause capturing unsuccessfully.

(6) current time judges that this target captures thing prediction coordinate (x successively_{C_target},y_{C_target}) whether enter the 1st Mechanical hand～the work space of n-th mechanical hand, if this target captures thing and enters the work space of some mechanical hand, the most really This mechanical hand fixed is mechanical hand to be grabbed, and enters step (7), otherwise, is saved in temporary queue by this object pose information frame, enters Enter step (8)；

Multi-robot dispatching control module judges that this target captures thing prediction coordinate (x_{C_target},y_{C_target}) whether enter The method of the operation interval of i-th mechanical hand is:

(6a) by the i-th mechanical hand at i-th robot coordinate system X_RiO_RiY_RiUnder zero (0,0) carry out coordinate and turn Change, obtain this zero coordinate (x under conveyer belt plane coordinate system_{C_i},y_{C_i})；

$\left[\begin{array}{c}{x}_{C\_i}\\ y_{C\_i}\\ 1\end{array}\right]=M_{P\_R\_i}\·\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]---\left(7\right)$

(6b) (x is judged_{C_target},y_{C_target}) whether meet condition:

Time, mechanical hand Work space be radius be the sector region of R, meet, then it is assumed that (x_{C_target},y_{C_target}) fall into the work of i-th mechanical hand In region, otherwise it is assumed that (x_{C_target},y_{C_target}) not in the working region of this mechanical hand.

(7) robot work state to be grabbed whether " idle " is judged, if " idle ", then by this object pose information frame Imaging plane coordinate system coordinate be sent to the TRAJECTORY CONTROL module that this mechanical hand is corresponding with attitude angle information, enter step (8)； If duty is " hurrying ", then this object pose information frame is saved in temporary queue, enters step (8)；

(8) according to the principle of first in first out, choose next object pose information frame, re-execute step (3)～step (7), until having chosen all object pose information frames in crawl queue, step (9) is entered；

(9) empty crawl queue, all object pose information frames in temporary queue are stored in crawl queue.

When capturing object and leaving conveyer belt, mechanical hand TRAJECTORY CONTROL module reads the detected value of weight sensor, it is judged that Whether it is in threshold range set in advance, if it is will capture thing and place material container, if it is not, then by paw pine Open, capture object and drop back into conveyer belt.

It is all to calculate at conveyer belt plane coordinate system that above-mentioned coordinate calculates major part, advantageously reduces amount of calculation, Improve computational efficiency.

Embodiment:

A kind of for the vial sorting control system in house refuse configuring 3 mechanical hands, as shown in Figure 3.Machinery Hands is arranged in order along conveyer belt direction of transfer, and wherein, mechanical hand 1 and mechanical hand 2 are positioned at conveyer belt the same side, and mechanical hand 3 is positioned at Opposite side.The initial point of conveyer belt plane coordinate system is 1 O at conveyer belt edge_C, X_CAxle is the direction of motion along conveyer belt, Y_C It is positioned at conveyer belt plane and and X_CAxle is vertical.

In the present embodiment, imaging plane coordinate system overlaps with conveyer belt plane coordinate system, then,

$M_{P\_C}=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]$

The robot coordinate system of mechanical hand 1 is X_RY_RZ_RO_RInitial point is the pedestal of mechanical hand and is positioned at conveyer belt plane, axle Z_R Vertical and conveyer belt plane, axle X_R, axle Y_RRespectively with axle X_CWith axle Y_CDirection is consistent.

1., 2., 3. and 4. bottle to be captured moves along conveyer belt direction, wherein, is 1. plastic bottle, and remaining is glass Bottle, before bottle enters the work space of mechanical hand 1, the information frame of this bottle has transmitted to posture information receiver module. All information frames received are stored in reception relief area by posture information receiver module, and multi-robot scheduler module can be west MENZI S7-300 series of programmable logic controller (PLC).Each program at PLC performs the cycle (within 1ms) to circulation Execution following steps:.

(1) judge to receive buffering takes whether there is new bottle posture information frame, if it has, by new bottle posture information frame It is stored in and currently captures queue, enter step (2), otherwise, be directly entered step (2).Current crawl in queue shown in Fig. 2 has 4 Individual bottle posture information frame.In described crawl queue, bottle posture information frame is according to the most regularly arranged: the different shooting moment Bottle posture information frame, arranges according to time order and function order, and the bottle posture information frame in same shooting moment, according to bottle pose The position arrangement of information frame, the bottle posture information frame near mechanical hand is arranged in front.

(2) the bottle 1. information frame captured in queue is chosen.

(3) bottle 1. information frame coordinate information (x under imaging plane coordinate system is resolved_{P_0},y_{P_0})=(0,300) and shooting Time t₁=1；

(4) the 1. coordinate information (x in imaging plane coordinate system by bottle_{P_0},y_{P_0})=(0,300) carry out Coordinate Conversion, Obtain the coordinate information (x under conveyer belt plane coordinate system_{C_0},y_{C_0})；

$\left[\begin{array}{c}{x}_{C\_0}\\ y_{C\_0}\\ 1\end{array}\right]=M_{P\_C}\·\left[\begin{array}{c}{x}_{P\_0}\\ y_{P\_0}\\ 1\end{array}\right]$

Wherein,

$M_{P\_C}=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]$

M_{P_C}Be tied to the coordinate conversion matrix of conveyer belt plane coordinate system for imaging plane coordinate, Two coordinate system overlaps, i.e. (x_{C_0},y_{C_0})=(0,300)；

(5) current time t is set₂=3s, the ready position of the 1st mechanical hand coordinate under this robot coordinate system (x_{R_int},y_{R_int},z_{R_int})=(0,600,500)；Δ T be mechanical hand receive instruction to grab target crawl thing required for Time, V_{R_hand}=2000, speed v=1000 of conveyer belt, M_{C_R_i}It is tied to i-th robot coordinate for conveyer belt plane coordinates The coordinate conversion matrix of system；

The coordinate conversion matrix that 1st robot coordinate is tied to conveyer belt plane coordinate system is:

$M_{C\_R\_1}=\left[\begin{array}{ccc}1& 0& 2000\\ 0& 1& -300\\ 0& 0& 1\end{array}\right]$

Two coordinate system is translation relation.

Resolve equation group (formula (2)～formula (6)) and obtain target crawl thing at t₂+ Δ T the moment in conveyer belt plane Prediction coordinate (x under coordinate system_{C_target},y_{C_target}):

Wherein, z_{R_target}=0, x_{C_0}=0, y_{C_0}=y_{C_target}=300

It is calculated (x_{C_target},y_{C_target})=(2288.675,300), Δ T=0.289.

(6) current time judges that coordinate (x predicted by this bottle successively_{C_target},y_{C_target}) whether enter the 1st mechanical hand ～the work space of the 3rd mechanical hand, if this target captures thing and enters the work space of some mechanical hand, it is determined that this machine Tool hands is mechanical hand to be grabbed, and enters step (7), otherwise, is saved in temporary queue by this bottle posture information frame, enters step (8)；

Judge (x_{C_target},y_{C_target}) whether at the Work Space Range of mechanical hand 1, if mechanical hand 1 is in the work of conveyer belt Make space radius R=1000mm.

(x_{C_1},y_{C_1})=(2000 ,-300)

$x_{C\_1}+\sqrt{R^2-{(y_{C\_t\arg et}-y_{C\_1})}^2}=2800$

$x_{C\_1}-\sqrt{R^2-{(y_{C\_t\arg et}-y_{C\_1})}^2}=1200$

x_{C_target}Meet Relation, therefore, 1. bottle is to be in the work space of mechanical hand 1, enters step (7)；

(7) mechanical hand 1 duty to be grabbed whether " idle " is judged, if " idle ", then by this object pose information frame In imaging plane coordinate system coordinate information be sent to the TRAJECTORY CONTROL module that this mechanical hand is corresponding, enter step (8)；If work Making state is " hurrying ", then be saved in temporary queue by this object pose information frame, enters step (9)；

(8) mechanical hand captures bottle, and when 1. bottle leaves conveyer belt, mechanical hand TRAJECTORY CONTROL module sense weight senses The input quantity of device, it is judged that whether it is in default threshold value, in the present embodiment, uses the gripper that super light material is made, gripper Weight and the weight summation of general vial generally lie between 800～850g, therefore, threshold range takes [800,850], This bottle is empty plastic bottle, it should at about 420g, hence it is evident that not in threshold range, controls paw and unclamps, and 1. bottle drops back into Conveyer belt.

(9) according to the principle of first in first out, multi-robot dispatching control module chooses next bottle posture information frame, weight Newly performing step (3)～step (7), until having chosen all bottle posture information frames in crawl queue, entering step (10)；

(10) multi-robot dispatching control module empties crawl queue, by all bottle posture information frames in temporary queue It is stored in crawl queue.

The present invention arranges that the bottle in rubbish is sorted by multiple mechanical hand, can make full use of dividing of every mechanical hand Pick ability, multiple sorting tasks are simultaneously allocated to multiple stage mechanical hand and sort, the most target of whole system can be realized and capture The sorting ability of thing, work efficiency is high.Owing to the speed of conveyer belt is slow, and the loop control speed of multi-robot control module Hurry up, for same target captures thing, the work space that chance will be had repeatedly to enter multiple mechanical hands, can reach very Low loss.

It addition, present invention weight characteristics of detection sorting object when mechanical hand captures sorting object, as judging to capture Whether object is the condition of vial sorting thing, vial is carried out automatic sorting, is effectively prevented broken glass to follow-up The destruction caused of apparatus for production line.

The above, the detailed description of the invention that only present invention is optimal, but protection scope of the present invention is not limited thereto, Any those familiar with the art in the technical scope that the invention discloses, the change that can readily occur in or replacement, All should contain within protection scope of the present invention.

The content not being described in detail in description of the invention belongs to the known technology of professional and technical personnel in the field.

Claims (6)

1. one kind for the vial sorting control system in house refuse, it is characterised in that include bottle pose receiver module, Multi-robot dispatching control module, N number of mechanical hand, the TRAJECTORY CONTROL module of N number of control mechanical hand and weight sensor, mechanical hand With TRAJECTORY CONTROL module one_to_one corresponding, weight sensor is used for collection machinery hands paw and captures the joint weight of thing, wherein:

Bottle pose receiver module, the bottle posture information frame receiving outside input is deposited into receiving in relief area, described bottle Sub-posture information frame includes bottle coordinate under imaging plane coordinate system, attitude angle and shooting time；

Multi-robot dispatching control module, extracts the bottle posture information frame received in relief area, calculates bottle posture information frame Middle bottle is at t₂The prediction coordinate being engraved in during+Δ T under conveyer belt plane coordinate system, described t₂For current time, Δ T is mechanical hand Receive instruction to the time grabbed required for bottle, it is judged that the work whether this prediction coordinate falls into any one mechanical hand is empty Between, if entering the work space of certain mechanical hand, then judge the duty whether " idle " of this mechanical hand, if " empty Not busy ", then the coordinate under the imaging plane coordinate system of this bottle is distributed to attitude angle information the machine that the mechanical hand of free time is corresponding Tool hands TRAJECTORY CONTROL module；

Mechanical hand TRAJECTORY CONTROL module, during initialization, controls mechanical hand and is positioned at default ready position；Receive bottle to put down in imaging After coordinate information under areal coordinate system, duty is set to " doing ", coordinate information and the attitude angle of this bottle are sat Mark conversion, obtains this bottle coordinate under this mechanical hand coordinate system, controls mechanical hand and moves to vial position, rotating machine arm Angle consistent with attitude angle, capture bottle, when bottle leaves conveyer belt surface, read weight sensor values, according to weight Judge whether the bottle that captured has the weight characteristics of vial, if it is, deliver to bottle to specify beyond conveyer belt Material memory block, if not the weight characteristics of vial, is then put back into bottle on conveyer belt, afterwards, controls mechanical hand and returns To the ready position preset, duty is set to " idle ".

The most according to claim 1 a kind of for the vial sorting control system in house refuse, it is characterised in that institute State mechanical hand TRAJECTORY CONTROL module and judge that whether the bottle that captured is that the method for vial is according to bottle weight:

Judge that weight, whether in preset threshold range, if it is, be judged to vial, is otherwise judged to other bottle of class, institute State threshold value to be determined by the statistical weight sum of manipulator paw with vial.

The most according to claim 1 a kind of for the vial sorting control system in house refuse, it is characterised in that institute Stating N number of mechanical hand to arrange along conveyer belt direction, serial number 1～N, the circulation of multi-robot dispatching control module performs following step Rapid:

(1) judge that current time receives whether relief area has new bottle posture information frame, if it has, by new bottle pose letter Breath frame is stored in current crawl queue, enters step (2), otherwise, is directly entered step (2)；

(2) according to the principle of first in first out, first the bottle posture information frame captured in queue is chosen；

(3) bottle coordinate information (x under imaging plane coordinate system in bottle posture information frame is resolved_{P_0},y_{P_0}) and shooting time Between t₁；

(4) by bottle coordinate information (x in imaging plane coordinate system_{P_0},y_{P_0}) carry out Coordinate Conversion, obtain conveyer belt plane and sit Coordinate information (x under mark system_{C_0},y_{C_0})；

(5) bottle is calculated at t₂Prediction coordinate (the x under conveyer belt plane coordinate system in+Δ T moment_{C_target},y_{C_target})；

(6) current time judges that coordinate (x predicted by this bottle successively_{C_target},y_{C_target}) whether enter the 1st mechanical hand～N The work space of individual mechanical hand, if this bottle enters the work space of some mechanical hand, it is determined that this mechanical hand is for waiting to grab Mechanical hand, enters step (7), otherwise, is saved in temporary queue by this bottle posture information frame, enters step (8)；

(7) robot work state to be grabbed whether " idle " is judged, if " idle ", then by the one-tenth in this bottle posture information frame Photo coordinate system coordinate is sent to the TRAJECTORY CONTROL module that this mechanical hand is corresponding with attitude angle information, enters step (8)；If Duty is " hurrying ", then be saved in temporary queue by this bottle posture information frame, enters step (8)；

(8) according to the principle of first in first out, choose next object pose information frame, re-execute step (3)～step (7), Until having chosen all object pose information frames in crawl queue, enter step (9)；

(9) empty crawl queue, all object pose information frames in temporary queue are stored in crawl queue.

The most according to claim 3 a kind of for the vial sorting control system in house refuse, it is characterised in that institute State step (5) and calculate bottle at t by resolving equation below group₂The prediction coordinate being engraved in during+Δ T under conveyer belt plane coordinate system (x_{C_target},y_{C_target}):

x_{C_target}=x_{C_0}+v×(t₂+ΔT-t₁)

y_{C_target}=y_{C_0}

$\mathrm{\Δ}T=\sqrt{{(x_{R\_t\arg et}-x_{R\_\mathrm{int}})}^2+{(y_{R\_t\arg et}-y_{R\_\mathrm{int}})}^2+{(z_{R\_t\arg et}-z_{R\_\mathrm{int}})}^2}/V_{R\_hand}$

$\left[\begin{array}{c}{x}_{R\_t\arg et}\\ y_{R\_t\arg et}\\ 1\end{array}\right]=M_{C\_R\_i}\·\left[\begin{array}{c}{x}_{C\_t\arg et}\\ y_{C\_t\arg et}\\ 1\end{array}\right]$

z_{R_target}=0

Wherein, (x_{R_int}, y_{R_int}, z_{R_int}) it is the ready position of the i-th mechanical hand coordinate under this robot coordinate system； V_{R_hand}For mechanical hand along the pre-set velocity of point-to-point linear motion, v is the speed of conveyer belt, M_{C_R_i}For conveyer belt plane coordinates It is tied to the coordinate conversion matrix of i-th robot coordinate system, t₂For current time.

The most according to claim 3 a kind of for the vial sorting control system in house refuse, it is characterised in that institute State multi-robot dispatching control module and judge that coordinate (x predicted by this bottle_{C_target},y_{C_target}) whether enter i-th mechanical hand The method of operation interval is:

(6a) by the i-th mechanical hand at i-th mechanical hand plane coordinate system X_RiO_RiY_RiUnder zero (0,0) carry out coordinate and turn Change, obtain this zero coordinate (x under conveyer belt plane coordinate system_{C_i},y_{C_i})；

(6b) (x is judged_{C_target},y_{C_target}) whether meet condition:

Time, the work of mechanical hand Space be radius be the sector region of R, meet, then it is assumed that (x_{C_target},y_{C_target}) fall into the operation interval of i-th mechanical hand In, otherwise it is assumed that (x_{C_target},y_{C_target}) not in the operation interval of this mechanical hand.

The most according to claim 3 a kind of for the vial sorting control system in house refuse, it is characterised in that institute State and step (1) captures in queue bottle posture information frame according to the most regularly arranged: the bottle poses letter in different shooting moment Breath frame, arranges according to time order and function order, the bottle posture information frame in same shooting moment, arranges according to the position of bottle, leans on The posture information frame of the bottle of nearly mechanical hand is arranged in front.