CN106874887A

CN106874887A - Based on the farm machinery navigation control method that farm environment is perceived

Info

Publication number: CN106874887A
Application number: CN201710141810.2A
Authority: CN
Inventors: 张瑞宏; 奚小波; 金亦富; 张剑峰; 单翔; 蔡广林; 孙福华; 叶伟伟; 史扬杰; 马国梁
Original assignee: NANJING WOYANG MACHINERY TECHNOLOGY Co Ltd
Current assignee: NANJING WOYANG MACHINERY TECHNOLOGY Co Ltd
Priority date: 2017-03-10
Filing date: 2017-03-10
Publication date: 2017-06-20
Also published as: CN108082181A; CN108082181B

Abstract

The invention provides a kind of farm machinery navigation control method perceived based on farm environment in agricultural machinery control technology field, following steps, step 1 are specifically included：Before agricultural machinery working, video camera is demarcated, radar and visual information are spatially merged；Step 2：During agricultural machinery working, the height change between the detection radar of distance detection device one and ground, the height change between the detection video camera of distance detection device two and ground, the conversion of real-time adjustment radar and camera coordinates makes radar spatially synchronous with video camera；Step 3：Industrial computer resolves the millimetre-wave radar data for receiving, and effective target is determined, it is determined that most risk object, synchronous acquisition camera review；Step 4：Industrial computer judges most risk object state and plans walking path according to radar information, according to the view data that radar and camera acquisition are arrived, judges the type of most risk object, navigation case control agricultural machinery action；Data fusion high precision in the present invention, improves the degree of accuracy of cognitive disorders thing.

Description

Based on the farm machinery navigation control method that farm environment is perceived

Technical field

The present invention relates to a kind of air navigation aid, more particularly to a kind of unmanned middle air navigation aid for using of agricultural machinery.

Background technology

Precision agriculture technology is considered as the forward position of 21 century development in agricultural science and technology, is that science and technology contains highest, integrated comprehensive One of most strong modern agricultural production administrative skill.Precision agriculture technology is according to spatial variability, positioning, timing, quantitative implementation A set of modernization farming operating technology and the system for managing, are information technology and a kind of comprehensive novel agricultural for combining of agricultural production Technology.

Precision agriculture is applied to fast development, can fully excavate the maximum productive potentialities in farmland, rationally utilize liquid manure Resource, reduction environmental pollution, increase substantially agricultural output and quality.

Development precision agriculture technology is to solve China's agricultural from traditional agriculture to being faced in agricultural modernization evolution Ensure agricultural product total amount, transform agricultural production, improve quality of agricultural product and quality, resource wretched insufficiency and utilization rate Effective settling mode of the problems such as low, environmental pollution, is also the only way which must be passed of Chinese agricultural modernization development and transition and upgrade.

Satellite Navigation Technique is one of basic composition of precision agriculture technology, agricultural machinery is realized automatic running, agricultural machinery working Before only need to set parameter after, navigation system just guide agricultural machinery enter automatic job pattern, proceed by straight line farming.In agriculture During machine self-navigation, may there is electric pole, ridge, soil in the bad environments and complexity in farmland in large-scale farmland Mound, livestock and the work personnel that occur at any time etc., these factors are all for the realization of unmanned agricultural machinery proposes new challenge. In the prior art, agricultural machinery automatically walk in farmland can be realized using Satellite Navigation Technique, but agricultural machinery cannot be accurately identified Barrier in front of agricultural machinery, i.e. agricultural machinery cannot sense farm environment, not to mention done automatically according to the farm environment for perceiving and stop Car is waited and is also to continue with the treatment such as traveling, must have driver assistance to manipulate the action of agricultural machinery during agricultural machinery working, without notice, Agricultural machinery will be collided with preceding object thing；Therefore in the urgent need to working out a set of navigation control method perceived based on farm environment Make the standby ability perceived to surrounding enviroment of unpiloted agricultural machinery and implement, once there is electric pole, field in running into above-mentioned farmland The ridge, mound, livestock and occur at any time work personnel situations such as, can in time take the emergency processings such as parking waiting, avoidance.

The content of the invention

For defect of the prior art, it is an object of the invention to overcome above-mentioned weak point of the prior art, solution Certainly unpiloted agricultural machinery cannot perceive the technical problem of farm environment in the prior art, there is provided one kind is unmanned for agricultural machinery Farm environment cognitive method, the present invention realizes the perception of farm environment, and perceived accuracy is high, identification agricultural machinery preceding object thing The degree of accuracy is high, reliability when raising agricultural machinery is unmanned.

The object of the present invention is achieved like this：A kind of farm machinery navigation control method perceived based on farm environment, specifically Comprise the following steps,

Step 1：Before agricultural machinery working, video camera is demarcated, video camera space coordinate transformation, then radar vision is combined Demarcate so that radar and visual information are spatially merged；

Step 2：During agricultural machinery working, the height change △ h between the real-time detection radar of distance detection device one and ground_st, away from Height change △ h between the real-time detection video camera of detection means two and ground_ct, industrial computer carries out data processing real-time adjustment thunder Up to the transformational relation with camera coordinates, make that radar and video camera realize spatially under operating condition is synchronous；

Step 3：Industrial computer resolves the millimetre-wave radar data for receiving, and determines effective target, selects agricultural machinery working front Radar region interested, it is determined that most risk object, synchronously carries out the collection of camera review；

Step 4：Information according to radar judges the motion state of most risk object, and industrial computer is according to most risk object Motion state planning agricultural machinery travel path, the view data of the most risk object collected according to radar and camera is judged most The type of risk object, the action command that industrial computer will be parsed is transferred to navigation case, and navigation case control agricultural machinery does accordingly Action；

Wherein, during agricultural machinery working, the travel speed of agricultural machinery is at the uniform velocity；

The distance detection device one is identical with the structure of distance detection device two, and distance detection device one is arranged on agricultural machinery Front side and it is arranged on immediately below radar, distance detection device two is arranged on agricultural machinery downside and is arranged on video camera position directly below； The distance detection device one includes thering is accommodating chamber and retractable housing, and accommodating chamber is cylindrical shape, is set in the housing Having can do the sliding block for moving along a straight line up and down along inner walls, and detection slider top is provided with housing with lateral extent in inner walls The range sensor of change, 2 connectors are connected between the sliding block and inner walls top, and connector is on sliding block in width Central Symmetry on degree direction is set；The connector includes fairlead, and being provided with the fairlead can be along fairlead inwall The guide post of slip, being left between the front and rear both sides of sliding block and inner walls can accommodate the gap of guide post, the fairlead Bottom is provided with pilot hole and is provided with limit rank of the restricted guidance bar away from fairlead, and the guide post is passed through outside pilot hole and sliding block Side is connected, and at least one air-vent, the bottom of the sliding block are provided with corresponding housing directly over the accommodating chamber of the fairlead The universal rolling wheel that can be rolled on the ground is installed；

Industrial computer receives the data-signal that the range sensor sends over and carries out data processing.

In order to realize radar and video camera preliminary synchronisation spatially, being converted into vehicle coordinate in the step 1 Image pixel coordinates specifically include following steps,

Step 1.1：Before agricultural machinery working, ground is defaulted as level, by millimetre-wave radar be fixedly mounted on agricultural machinery front side and Positioned at agricultural machinery longitudinal central axis, radar emission is faced out, and makes radar emission face perpendicular to the ground；Make video camera when video camera is installed Optical axis is parallel to the ground；

Step 1.2：Radar fix system 0 is set up by origin of the center of radar₀-X₀Y₀Z₀, plane where millimetre-wave radar by X₀Axle and Y₀Axle determines and and Z₀Axle is perpendicular, Z₀Axle it is parallel to the ground and with agricultural machinery central axes；Set up camera coordinates It is Oc-XcYcZc, is origin Oc with the center of video camera, parallel to the imaging plane of video camera, Zc axles is to take the photograph to plane XcOcYc The optical axis and perpendicular to imaging plane of finding a view of camera；Set up the center and car of vehicle axis system Ow-XwYwZw, Ow for agricultural machinery rear axle Central axis intersection point, Xw axles level to the right and perpendicular to the longitudinal central axis line of agricultural machinery, Zw it is horizontal forward and with agricultural machinery in Heart axis overlap, Yw axles perpendicular to the water surface ground upwardly, the X of radar fix system₀O₀Z₀Plane is put down with the XwOwZw of vehicle axis system Face is parallel；

Step 1.3：The point that optical axis intersects with imaging plane is image principal point O ', and vehicle coordinate is by spin matrix R peace The amount of shifting to s_cCamera coordinates (x is obtained after conversion_c, y_c, z_c, 1)^T, the vehicle coordinate of arbitrfary point P is (x_w, y_w, z_w, 1)^T, by car Coordinate Conversion is camera coordinates, and specific transformational relation is as follows,

In formula (1-1), R is an orthogonal matrices for the row of three row three, s_c(x_c0, y_c0, z_c0) it is vehicle under primary condition Coordinate is tied to the 1*3 translation matrix of camera coordinate system, x_c0Central shaft and the straight line of vehicle center axis two where video camera Distance, y_c0For under primary condition video camera apart from ground height, z_c0It is video camera apart from the distance of agricultural machinery rear axle；

Step 1.4：By camera coordinates (x_c, y_c, z_c, 1)^TIt is transformed into image physical coordinates (x₁, y₁)^T, specific conversion Relation is as follows,

In formula (1-2), f is the focal length of video camera, and focal length unit is mm；

Step 1.5：By image physical coordinates (x₁, y₁)^TImage pixel coordinates (u, v) are transformed into, specific transformational relation is such as Under：

Wherein, dx, dy represent that each pixel is horizontally and vertically going up unit-sized, u respectively₀、v₀Respectively image pixel is sat The transverse and longitudinal coordinate of the lower camera optical axis of mark system and imaging plane intersection point, coordinate unit is pixel；

Step 1.6：The conversion that image pixel coordinates are tied to vehicle axis system is obtained according to above formula (1-1)~(1-3) Formula, be specifically,

Step 1.7：In order that radar and visual information are spatially merged, by the coordinate transformation relation in step 1.6 more It is newly,

Wherein, s=s_c+s₀, s₀Coordinate be set to (x_s0,y_s0,z_s0), x_s0=0, y_s0It is distance by radar ground under primary condition The height in face, z_s0It is radar and the distance of agricultural machinery rear axle.

In order to improve the fusion accuracy of radar and video camera during agricultural machinery working, the industrial computer in the step 2 is carried out The transformational relation of data processing real-time adjustment radar and camera coordinates, is specifically that the actual road conditions according to agricultural machinery are adjusted in real time Whole translation vector s, the translation vector s after being adjusted under scan period t_t=s_c+s₀+△s_t, real-time vehicle coordinate and image pixel The transformational relation of coordinate, be specifically,

Wherein, △ h_ctIt is video camera under scan period t and the changing value of ground level, △ h_stIt is radar under scan period t With the changing value of ground level, j is scan period number, (u_t,v_t) it is real-time update meter under scan period t during agricultural machinery working The image pixel coordinates for obtaining.

In order to further improve environment sensing precision, △ is obtained in the translation vector in the step 2 after self-adaptative adjustment h_tThe step of it is as follows,

△ h are obtained in translation vector in the step 2 after real-time adjustment_tThe step of it is as follows,

Step 2.1：The high variable quantity between radar and ground and the height change between video camera and ground are calculated in real time Amount, be specifically,

I moment and i-1 moment radar and ground level distance change value are △ h in scan period t_sti, using method of average meter High variable quantity △ h of the radar with respect to ground in calculation scan period t_st,

Assuming that video camera and ground level changing value are △ h under sampling instant i and sampling instant i-1 in scan period t_cti, The high variable quantity △ h on video camera and ground in scan period t are calculated using the method for average_ct,

Step 2.2：The translation vector s after self-adaptative adjustment under scan period t is calculated in real time, is specifically,

Wherein, k is the sum of the sampled point in a scan period；

In this design, the height by the height change and video camera on real-time detection distance by radar ground apart from ground becomes Change, real-time update translation vector s, improve the precision of video camera and radar spatial synchronization.

The accuracy of radar data is resolved to further improve, the resolving radar data in the step 3 determines effective Target, specifically includes following steps,

Step 3.1：The data received to radar are resolved according to millimetre-wave radar agreement, obtain objects in front relative The angle [alpha] of radar, apart from r, relative velocity v, the reflected intensity of objects in front and be each Target Assignment only one ID；

Step 3.2：Random noise signal is filtered, it is ensured that the continuous effective of radar data, is specifically, defined Z=[r, α, v]^TIt is the measured value of radar, z (k) is the measured value of millimetre-wave radar kth time output,

d²=S (z (k)-z (k-1)) (z (k)-z (k-1))^T＜ r_s ² (3-1)

Filter out the data-signal for not being inconsistent box-like (3-1)；Wherein, d is adding between adjacent measurement vector z (k), z (k-1) Power Euclidean distance, S is weighting matrix, r_sIt is the threshold value of setting；

Step 3.3：Judge target whether agricultural machinery traveling track in, when radar objects in front meets di≤ds, mesh It is marked in agricultural machinery traveling lane, otherwise, target is elected as effectively not in agricultural machinery traveling lane at the beginning of the target in agricultural machinery traveling lane Target, and numbering is ranked up according to criterion from the close-by examples to those far off to it；Target outside agricultural machinery traveling lane is benign target, Excluded；Wherein, ds is safe distance threshold value, and ds=L/2+ks, di are the target and Z that are measured under i sampled points₀Between axle Distance, L is the plough tool width hung on agricultural machinery, and ks is the safe clearance of setting；

Step 3.4：Effective target to primary election carries out validity check, finally determines effective target；

Step 3.5：According to the effective target for determining, the minimum distance barrier obtained by millimetre-wave radar is defined as The most risk object of candidate, if dj≤dmin, dj be the agricultural machinery that millimetre-wave radar is obtained and ID be between the effective target of j away from From dmin is the distance of acquired agricultural machinery within one scan period of millimetre-wave radar and nearest effective target, then ID is j Effective target is most risk object；

In this design, first the random noise signal that interference and noise signal are produced is filtered, improve radar data solution The accuracy of calculation；By the differentiation to agricultural machinery traveling lane, the obstacle target outside agricultural machinery traveling lane is excluded, it is preliminary selected same Barrier in track is effective target, and primary election effective target is tested to further determine that effective target, improves effective The accuracy of target identification；Order to effective target according to distance from the near to the remote is rule, it is determined that most risk object；

Validity check is carried out to the effective target of primary election in the step 3.4 and specifically includes following steps,

Step 3.4.1：Effective target to primary election is predicted, and chooses state Sn=[d_n,v_n,a_n], primary election effective target Status predication equation be,

Wherein, d_(n+1,n)、v_(n+1,n)、a_(n+1,n)It is the state letter of effective obstacle target of upper scan period prediction Breath, d_n,v_n,a_nThe relative distance of the effective obstacle target measured in the detection cycle of millimetre-wave radar n-th, relative is represented respectively Speed, relative acceleration, t are the scan periods of millimetre-wave radar；

Step 3.4.2：By the status information and radar of the (n+1)th cycle effective target of comparison prediction it is actually measured The status information of n+1 cycle effective targets, it is specific as follows,

Wherein, d₀、v₀、a₀It is the error threshold between the effective obstacle target measured value and predicted value for setting；

Step 3.4.3：Effective obstacle target in the scan period of radar by continuous probe to more than m times, meanwhile, expire The effective target of formula (3-3) is consistent with primary election effective target in sufficient step 3.4.2, then the relative distance of more fresh target, relative Speed, relative angle, number information；Otherwise, the effective target of primary election is not in millimetre-wave radar detection target, using effective Target prediction information is tracked to the effective target of primary election, if the effective target of primary election is in next scan period of radar Still it is not detected, then stops using corresponding primary election effective target information, updates effective target information, and return to step 3.4.1 circulation is performed；

In this design, contrasted with the carrying out for testing out by the status information of upper one effective target of scanning prediction, Judge whether effective target information is consistent, and false target is further excluded with this, the determination of effective target is obtained further Ensure.

As a further improvement on the present invention, judge in the step 4 most risk object sound state specifically include with Lower step,

The sound state of most risk object is judged in the step 4, industrial computer is advised according to the motion state of most risk object Draw agricultural machinery travel path and specifically include following steps,

Step 4.1：According in step 3.5 determine most risk object, constantly update most risk object relative velocity and Whether relative distance information, judges the distance of most risk object and radar in the range of stopping distance, i.e. z_d>z_min(4-1), z_dFor Radar and the relative distance of most risk object that millimetre-wave radar is detected, z_minIt is the stopping distance threshold value of setting, most dangerous mesh When mark meets formula (4-1), agricultural machinery continues to travel；

Step 4.2：The sound state of most risk object is judged according to relative velocity size, it is specific as follows,

v≠v_Car (4-2)

Within the continuous scan period, when (4-2) formula is set up all the time, the state for judging target is dynamic, now, industrial computer Send sound and light alarm, z_d≤z_minWhen, parking waiting instruction is sent to navigation case by industrial computer, and navigation case control agricultural machinery stops Etc. pending；Otherwise, agricultural machinery continues to travel, and is back to step 3.1 circulation execution, wherein, v is the speed of radar relative target Size, v_CarIt is the travel speed of agricultural machinery；When (4-2) formula is invalid all the time, judge that target is static state, then industrial computer is made at avoidance Reason, is specifically that camera-scanning goes out the edge contour of barrier, plough tool width and agricultural machinery minimum turn of the industrial computer according to agricultural machinery Curved radius sets an avoidance path；Industrial computer parses the front wheel angle of agricultural machinery according to the avoidance path for setting, and will be dynamic Navigation case is sent to as instruction, the front wheel angle of navigation case control agricultural machinery makes agricultural machinery be walked according to the avoidance path of setting；

The left and right sides center of agricultural machinery is separately installed with radar one and radar two；Agricultural machinery during avoidance, radar One and radar two continually scan for whether the agricultural machinery left and right sides has barrier, the relative distance for setting radar one and barrier is d₁, The relative distance that radar two is set with barrier is d₂, judge whether agricultural machinery continues according to avoidance path row according to below equation Walk,

d₁<d_s0 (4-3)

d₂<d_s0 (4-4)

When (4-3) or (4-4) any one formula is set up, industrial computer makes parking waiting decision-making, navigation case control agricultural machinery Stop；Otherwise, agricultural machinery continues to be walked according to avoidance path；

Wherein, d_s0It is the turning security distance of setting, angular transducer is installed on agricultural machinery, front wheel steering angle is passed by angle Sensor is measured, and the steering angle signal that angular transducer will be detected is transferred to industrial computer；

In this design, judge that the sound state principle of most risk object is simple, improve response speed；According to most risk object Dynamic quiescent conditions are made agricultural machinery and are further walked decision-making, if agricultural machinery is static state, agricultural machinery is walked according to the avoidance path of setting.

The reliability in theoretical avoidance path is obtained to further improve, the avoidance path tool in the step 4.2 Body is,

Characteristic circle is done by the center of circle of the center of barrier, the radius of characteristic circle is r_min+ w/2, avoidance path is by arc section First, straightway one, arc section two, straightway two and arc section three are constituted, one end and the original straight line path of agricultural machinery of arc section one Footpath is tangent, and the other end of arc section one is tangent with one end of straightway one, the other end of straightway one and one end of straightway two Tangent with arc section two respectively, the other end of straightway two is tangent with arc section three, and arc section two is characterized a section on circle, circle The center line of segmental arc one and arc section three on arc section two is symmetrical arranged, and agricultural machinery sequentially passes through arc section one, straightway one, circle Segmental arc two, straightway two and the cut-through thing of arc section three, wherein, r_minIt is the min. turning radius of agricultural machinery, w is the work of agricultural machinery Industry width, the circumradius of barrier is less than min. turning radius r_min；The radius of arc section one is r_min, the arc section three Radius be r_min, the starting point of arc section one is designated as H points, and the center of circle of arc section one is designated as O₁Point, straightway one is original with agricultural machinery The joining of straight line path is designated as J, and straightway one is designated as D, agricultural machinery original path and characteristic circle with the points of tangency of arc section two Joining is designated as K and K ' respectively, and JK=w/2, the center of circle of arc section two is designated as O points, and the coordinate of O is set to (a, b), arc section two Central point is designated as B points, and the coordinate of J points is designated as (x1, y1), and the equation of JD can be write as：

Y=k (x-x₁)+y₁(4-5)；

The equation of characteristic circle can be write as：

(x-a)²+(y-b)²=r²

R=r_min+w/2 (4-6)

K can be obtained by (4-5) and (4-6), D points are the joining of JD and characteristic circle, and D point coordinates is solved with this；

Set up an office O₁Coordinate be (x₂,y₂), then point O₁Distance to straight line JD is：

y₂=y₁+r_min (4-8)

O is obtained according to formula (4-7) and (4-8)₁Coordinate；Then the coordinate of H points is (x₂, y₁), the coordinate of B points is (a, b+ r)。

As a further improvement on the present invention, the most risk object for being collected according to radar and camera in the step 4 View data, judges the type of most risk object, and navigation case control agricultural machinery does corresponding action, specifically includes following steps,

Step 4.1a：In the case of most risk object is for dynamic, video camera is identified to most risk object, and video camera is obtained The image of most risk object is taken, image is carried out into matching with the human sample training storehouse for training compares, and exports target identification knot Really；

Step 4.2a：According to the target identification output control agricultural machinery action of output, if non-human, then navigate navigation case case Sound and light alarm is sent, and controls agricultural machinery to do parking waiting and processed；If target identification result is human body, navigation case sends acousto-optic report It is alert, judge whether human body deviates agricultural machinery traveling lane or human body and moved to away from agricultural machinery direction, judged with below equation,

z_wn+1>z_wn (4-3)

di>ds (4-4)

If the human body target that radar detection is arrived meets (4-3) or (4-4), agricultural machinery continues to move forward, and otherwise navigate case Control agricultural machinery does parking waiting treatment；z_wnIt is the distance of the n-th detection scanning cycle radar most risk object relatively, z_w(n+1)For under The distance of one scan period radar most risk object relatively；

In this design, the sound state of most risk object is first judged, if most risk object is always static, then it is assumed that most endanger Dangerous target is the non-life bodies such as electric pole, trees, otherwise it is assumed that most risk object is farming personnel or livestock, by shooting The image of machine collection most risk object simultaneously identifies whether most risk object is human body, exports target identification result, if human body, Then navigation case sends sound and light alarm, because work personnel itself have hedging to realize, work personnel are after the alarm for hearing agricultural machinery Agricultural machinery traveling lane may be walked out or toward away from the walking of the agricultural machinery direction of motion, be set using the habituation reaction of the personnel of working Determining program, adaptability is good, and agricultural machinery is avoided also reminding the farming in front of agricultural machinery while electric pole, livestock etc. are non-human automatically Personnel feel and avoid, and do continuation traveling or parking waiting treatment according to the behavior of farming personnel.

As a further improvement on the present invention, the range sensor is arranged on the center of slider top, range sensor Just to inner walls top.

As a further improvement on the present invention, the range sensor is arranged on the center at the top of inner walls, and distance is passed Sensor is just to the center of slider top.

Before agricultural machinery working, video camera is carried out with the demarcation of radar under conditions of level ground；And during agricultural machinery working, Farmland ground relief, the same position of agricultural machinery is not installed on due to radar and video camera, and radar and video camera are relatively The height in face is different and as landform changes；The course of work of distance detection device one is specifically, universal rolling wheel along Broken terrain is rolled, and when ground is protruded, protrusion ground gives universal rolling wheel upward active force, and sliding block is along in housing Wall upward sliding, slipper push guide post upward sliding, range sensor detects the climb of sliding block, i.e., between radar and ground High variable quantity；Convex ground gradually level when, sliding block is slided gradually downward；When being sunken into, sliding block exists for locality In the presence of deadweight, sliding block slide downward, until universal rolling wheel is contacted with ground, range sensor detection sliding block and inner walls Between height change, the height change value between radar that range sensor will be detected in real time and Current terrestrial is sent to industry control Machine；The operation principle of distance detection device two is identical with the operation principle of distance detection device one, and distance detection device two is real-time Height change value between the video camera and Current terrestrial that will detect is sent to industrial computer；

Compared with prior art, millimetre-wave radar and video camera are combined perception farm environment to the present invention by the present invention, High variable quantity is added to radar and camera coordinates conversion by real-time detection radar and video camera apart from the height change on ground Translation vector in, during agricultural machinery working, video camera and radar is spatially realized real synchronization, improve video camera and radar Fusion accuracy；The random noise signal that noise and interference signal are produced is filtered, the accurate of radar detection signal is improved Property；Agricultural machinery course according to setting is defined as agricultural machinery traveling lane, will be elected as at the beginning of the obstacle target in agricultural machinery traveling lane Effective target, then further checked to the effective target of primary election, to further determine that effective target, improve radar perceive it is same The validity and accuracy of obstacle target in track；Most risk object and tracking most risk object are chosen, video camera is most endangering Target identification is carried out based on the sound state of dangerous target, if most risk object is dynamic, it is only necessary to whether identify dynamic object It is human body, it is not necessary to identify particular type, reduces operand, improve response speed, navigation case is according to image recognition knot Fruit control agricultural machinery action, it is to avoid agricultural machinery is collided when unmanned with barrier；If recognition result is human body, navigate case acousto-optic Warning reminding work personnel avoid agricultural machinery, and using this characteristic of the custom thinking of people, constantly whether detection human body deviates agricultural machinery row Sail track or human body to be moved to away from agricultural machinery direction, navigation case controls whether agricultural machinery is done at parking waiting according to testing result Reason, adaptability is good；If most risk object is static state, camera-scanning goes out the profile of barrier, obtains avoidance path, controls agricultural machinery Front wheel steering angle make agricultural machinery according to setting avoidance path walk, meanwhile, radar one and radar two detection agricultural machinery avoidance process In whether with the presence of new barrier, if so, agricultural machinery parking waiting, to ensure the safety work of agricultural machinery；Present invention can apply to When agricultural machinery is unmanned in the navigation work of farm environment automatic sensing.

Brief description of the drawings

Fig. 1 is the flow chart of perception farm environment method of the present invention based on millimetre-wave radar and video camera.

Fig. 2 is the relation schematic diagram of camera coordinate system and vehicle axis system in the present invention.

Fig. 3 is the relation schematic diagram of camera coordinate system and image physical coordinates system in the present invention.

Fig. 4 is the relation schematic diagram of image physical coordinates system and image pixel coordinates system in the present invention.

Fig. 5 for the present invention in agricultural machinery run over farmland environment schematic in journey.

Fig. 6 for the present invention in agricultural machinery run over lane discriminating schematic diagram in journey.

Fig. 7 is primary election effective target to be tested to further determine that the flow chart of effective target in the present invention.

Fig. 8 is the avoidance path locus figure in the present invention.

Fig. 9 is a structural representation of distance detection device one in the present invention.

Figure 10 is an A-A direction view of distance detection device one in the present invention.

Figure 11 is another structural representation of distance detection device in the present invention.

Wherein, 1 sliding block, 2 housings, 3 air-vents, 4 fairleads, 5 guide posts, 6 limit ranks, 7 accommodating chambers, 8 range sensors, 9 universal rolling wheels.

Specific embodiment

The present invention is further illustrated below in conjunction with the accompanying drawings.

Embodiment 1

A kind of farm machinery navigation control method perceived based on farm environment as shown in Fig. 1~10, specifically includes following step Suddenly：

Distance detection device one is identical with the structure of distance detection device two, and distance detection device one is arranged on agricultural machinery front side And be arranged on immediately below radar, distance detection device two is arranged on agricultural machinery downside and is arranged on video camera position directly below；Such as Fig. 9 With shown in Figure 10, distance detection device one includes thering is accommodating chamber 7 and retractable housing 2, and accommodating chamber 7 is cylindrical shape, shell Being provided with body 2 can do the sliding block 1 for moving along a straight line up and down along the inwall of housing 2, and sliding block 1 is in rectangular-shape, the center at the top of sliding block 1 Range sensor 8 is installed, range sensor 8 is just to the inwall of housing 2 top；It is connected between sliding block 1 and the inwall of housing 2 top 2 connectors, Central Symmetry of the connector on sliding block 1 in the direction of the width is set；Connector includes fairlead 4, fairlead 4 The guide post that can be slided along the inwall of fairlead 4 is inside provided with, is left and can be accommodated between the front and rear both sides of sliding block 1 and the inwall of housing 2 The gap of guide post, the bottom of fairlead 4 is provided with pilot hole and is provided with limit rank 6 of the restricted guidance bar away from fairlead 4, is oriented to Bar is connected through pilot hole with the outside of sliding block 1, and it is saturating to be provided with least one on the corresponding housing 2 in the surface of accommodating chamber 7 of fairlead 4 Stomata 3, the bottom of sliding block 1 is provided with the universal rolling wheel 9 that can be rolled on the ground；

Industrial computer receives the data-signal that sends over of range sensor 8 and carries out data processing；

In order to realize video camera and millimetre-wave radar preliminary synchronisation spatially, as shown in figs. 2 to 4, the general in step 1 Vehicle coordinate is converted into image pixel coordinates and specifically includes following steps,

Vehicle coordinate be converted into image pixel coordinates specifically include following steps in step 1,

Wherein, s=s_c+s₀, s₀Coordinate be set to (x_s0,y_s0,z_s0), x_s0=0, y_s0It is distance by radar ground under primary condition The height in face, z_s0It is radar and the distance of agricultural machinery rear axle；

Radar fix is converted into image coordinate by shared vehicle axis system, radar data is through three-dimensional coordinate inversion Change, complete target information and be matched to visual information, radar is asked for i.e. with the relative position of camera space by vehicle axis system Can；Real-time adjustment image pixel coordinates and the transformational relation of vehicle coordinate in step 2, be specifically, according to the reality of agricultural machinery Road conditions real-time adjustment translation vector s, the translation vector s after being adjusted under scan period t_t=s_c+s₀+△s_t, real-time vehicle coordinate With the transformational relation of image pixel coordinates, it is specifically,

Wherein, △ h_ctIt is video camera under scan period t and the changing value of ground level, △ h_stIt is radar under scan period t With the changing value of ground level, j is scan period number；

△ h are obtained in translation vector in step 2 after real-time adjustment_tThe step of it is as follows,

Wherein, k is the sum of the sampled point in a scan period.

Resolving radar data in step 3 determines effective target, specifically includes following steps,

d²=S (z (k)-z (k-1)) (z (k)-z (k-1))^T＜ r_s ² (3-1)

It is exemplified below, from figure 5 it can be seen that the fore-and-aft distance at this 2 obstacle distance agricultural machinery centers of B, C is more than Ds, outside agricultural machinery traveling lane；A, D this 2 fore-and-aft distances at obstacle distance agricultural machinery center are less than ds, in agricultural machinery traveling lane It is interior, then elect effective target as at the beginning of A and D；

It is display when barrier is in traveling lane in Fig. 6, barrier E is apart from agricultural machinery center O_{Agricultural machinery}Distance be less than L/2 + ks, E are in agricultural machinery traveling lane；

Validity check is carried out to the effective target of primary election in step 3.4 and specifically includes following steps,

The sound state of most risk object is judged in step 4, industrial computer plans agriculture according to the motion state of most risk object Machine walking path specifically includes following steps,

Step 4.1：According in step 2.5 determine most risk object, constantly update most risk object relative velocity and Whether relative distance information, judges the distance of most risk object and radar in the range of stopping distance, i.e. z_d>z_min(4-1), z_dFor Radar and the relative distance of most risk object that millimetre-wave radar is detected, z_minIt is the stopping distance threshold value of setting, most dangerous mesh When mark meets formula (4-1), agricultural machinery continues to travel；

v≠v_Car (4-2)

d₁<d_s0 (4-3)

d₂<d_s0 (4-4)

The view data of the most risk object collected according to radar and camera in step 4, judges most risk object Type specifically includes following steps,

Step 4.1a：If most risk object is always static, navigation case control agricultural machinery does parking waiting treatment；Otherwise, Video camera is identified to most risk object；

Step 4.2a：Video camera obtains the image of most risk object, and image is entered with the human sample training storehouse for training Row matching is compared, and exports target identification result；

Step 4.3a：According to the target identification output control agricultural machinery action of output, if non-human, then navigate navigation case case Sound and light alarm is sent, and controls agricultural machinery to do parking waiting and processed；If target identification result is human body, navigation case sends acousto-optic report It is alert, judge whether human body deviates agricultural machinery traveling lane or human body and moved to away from agricultural machinery direction, judged with below equation,

z_wn+1>z_wn (4-3)

di>ds (4-4)

As shown in figure 8, the avoidance path in step 4.2 is specifically,

Characteristic circle is done by center of circle O of the center of barrier, the radius of characteristic circle is r_min+ w/2, avoidance path is by arc section First, straightway one, arc section two, straightway two and arc section three are constituted, one end and the original straight line path of agricultural machinery of arc section one Footpath is tangent, and the other end of arc section one is tangent with one end of straightway one, the other end of straightway one and one end of straightway two Tangent with arc section two respectively, the other end of straightway two is tangent with arc section three, and arc section two is characterized a section on circle, circle The center line of segmental arc one and arc section three on arc section two is symmetrical arranged, and agricultural machinery sequentially passes through arc section one, straightway one, circle Segmental arc two, straightway two and the cut-through thing of arc section three；Wherein, r_minIt is the min. turning radius of agricultural machinery, w is the work of agricultural machinery Industry width, the circumradius of barrier is less than min. turning radius r_min；The radius of arc section one is r_min, the half of arc section three Footpath is r_min, the starting point of arc section one is designated as H points, and the center of circle of arc section one is designated as O₁Point, straightway one and the original straight line of agricultural machinery The joining in path is designated as J, and straightway one is designated as D with the points of tangency of arc section two, and agricultural machinery original path intersects with characteristic circle Point is designated as K and K ' respectively, and JK=w/2, the center of circle of arc section two is designated as O points, and the coordinate of O is set to (a, b), the center of arc section two Point is designated as B points, and the coordinate of J points is designated as (x1, y1), and the equation of JD can be write as：

Y=k (x-x₁)+y₁(4-5)；

The equation of characteristic circle can be write as：

(x-a)²+(y-b)²=r²

R=r_min+w/2 (4-6)

y₂=y₁+r_min (4-8)

O is obtained according to formula (4-7) and (4-8)₁Coordinate；Then the coordinate of H points is (x₂, y₁), the coordinate of B points is (a, b+ r)；

The view data of the most risk object collected according to radar and camera in step 4, judges most risk object Type, navigation case control agricultural machinery does corresponding action, specifically includes following steps,

z_wn+1>z_wn (4-3)

di>ds (4-4)

Before agricultural machinery working, video camera is carried out with the demarcation of radar under conditions of level ground；And during agricultural machinery working, Farmland ground relief, the same position of agricultural machinery is not installed on due to radar and video camera, and radar and video camera are relatively The height in face is different and as landform changes；The course of work of distance detection device one is specifically, universal rolling wheel 9 along Broken terrain is rolled, and when ground is protruded, protrusion ground gives universal rolling wheel 9 upward active force, and sliding block 1 is along housing 2 inwall upward slidings, sliding block 1 promote guide post upward sliding, range sensor 8 detect sliding block 1 climb, i.e., radar with High variable quantity between ground；Convex ground gradually level when, sliding block 1 is slided gradually downward；Locality when being sunken into, Sliding block 1 in the presence of deadweight, the slide downward of sliding block 1, until universal rolling wheel 9 is contacted with ground, the detection sliding block of range sensor 8 Height change between 1 and the inwall of housing 2, the height change between the real-time radar that will be detected of range sensor 8 and Current terrestrial Value is sent to industrial computer；The operation principle of distance detection device two is identical with the operation principle of distance detection device one, distance inspection The height change value surveyed between the real-time video camera that will be detected of device two and Current terrestrial is sent to industrial computer；

Compared with prior art, millimetre-wave radar and video camera are combined perception farm environment to the present invention by the present invention, The height of real-time detection radar and video camera apart from ground is distinguished by the setting of distance detection device one and distance detection device two Degree change, high variable quantity is added in the translation vector of radar and camera coordinates conversion；Distance detection device one and away from From the smart structural design of detection means two, accuracy of detection is high；During agricultural machinery working, video camera and radar is set spatially to realize very Positive synchronization, improves the fusion accuracy of video camera and radar；The random noise signal that noise and interference signal are produced is filtered Remove, improve the accuracy of radar detection signal；Agricultural machinery course according to setting is defined as agricultural machinery traveling lane, and agricultural machinery is travelled Elect effective target at the beginning of obstacle target in track as, then the effective target of primary election is further checked, with further really Determine effective target, improve radar and perceive with the validity and accuracy of obstacle target in track；Choose most risk object and with Track most risk object, video camera carries out target identification based on the sound state of most risk object, if most risk object is dynamic, Only need to identify whether dynamic object is human body, it is not necessary to identify particular type, reduce operand, improve response speed Degree, navigation case controls agricultural machinery action according to image recognition result, it is to avoid agricultural machinery is collided when unmanned with barrier；If identification When result is human body, navigation case sound and light alarm reminds work personnel to avoid agricultural machinery, using this characteristic of the custom thinking of people, constantly Whether detection human body deviates agricultural machinery traveling lane or human body is moved to away from agricultural machinery direction, and navigation case is according to testing result control Whether agricultural machinery does parking waiting treatment, and adaptability is good；If most risk object is static state, camera-scanning goes out the profile of barrier, Avoidance path is obtained, controlling the front wheel steering angle of agricultural machinery makes agricultural machinery be walked according to the avoidance path of setting, meanwhile, radar one and thunder Whether with the presence of new barrier during up to two detection agricultural machinery avoidances, if so, agricultural machinery parking waiting, to ensure the safety of agricultural machinery Operation；Present invention can apply to agricultural machinery when unmanned in the navigation work of farm environment automatic sensing.

Embodiment 2

Difference with embodiment 1 is that as shown in figure 11, range sensor 8 is arranged at the top of the inwall of housing 22 Center, range sensor 8 is just to the center at the top of sliding block 1.

The invention is not limited in above-described embodiment, on the basis of technical scheme disclosed by the invention, the skill of this area Art personnel are according to disclosed technology contents, it is not necessary to which performing creative labour just can make one to some of which technical characteristic A little to replace and deform, these are replaced and deformation all falls in the scope of protection of the present invention.

Claims

1. it is a kind of based on farm environment perceive farm machinery navigation control method, it is characterised in that specifically include following steps,

Step 1：Before agricultural machinery working, video camera is demarcated, video camera space coordinate transformation, then mark is combined to radar vision It is fixed so that radar and visual information are spatially merged；

Step 2：During agricultural machinery working, the height change △ h between the real-time detection radar of distance detection device one and ground_st, distance inspection Survey the height change △ h between the real-time detection video camera of device two and ground_ct, industrial computer carry out data processing real-time adjustment radar with The transformational relation of camera coordinates, make that radar and video camera realize spatially under operating condition is synchronous；

Step 4：Information according to radar judges the motion state of most risk object, motion of the industrial computer according to most risk object State planning agricultural machinery travel path, the view data of the most risk object collected according to radar and camera is judged most dangerous The type of target, the action command that industrial computer will be parsed is transferred to navigation case, and navigation case control agricultural machinery does corresponding action；

The distance detection device one is identical with the structure of distance detection device two, and distance detection device one is arranged on agricultural machinery front side And be arranged on immediately below radar, distance detection device two is arranged on agricultural machinery downside and is arranged on video camera position directly below；It is described Distance detection device one includes thering is accommodating chamber and retractable housing, and accommodating chamber is cylindrical shape, and being provided with the housing can The sliding block for moving along a straight line up and down is done along inner walls, detection slider top is provided with housing with inner walls upside distance change Range sensor, the sliding block and inner walls top between be connected with 2 connectors, connector is on sliding block in width side Upward Central Symmetry is set；The connector includes fairlead, and being provided with the fairlead can slide along fairlead inwall Guide post, being left between the front and rear both sides of sliding block and inner walls can accommodate the gap of guide post, the bottom of the fairlead It is provided with pilot hole and is provided with limit rank of the restricted guidance bar away from fairlead, the guide post connects through pilot hole with sliding block outside Connect, be provided with least one air-vent directly over the accommodating chamber of the fairlead on corresponding housing, the bottom of the sliding block is installed There is the universal rolling wheel that can be rolled on the ground；

2. it is according to claim 1 based on farm environment perceive farm machinery navigation control method, it is characterised in that the step Radar fix be converted into image pixel coordinates specifically include following steps in rapid 1,

Step 1.1：Before agricultural machinery working, ground is defaulted as level, and millimetre-wave radar one is fixedly mounted on the front side and position of agricultural machinery In agricultural machinery longitudinal central axis, radar emission is faced out, and makes radar emission face perpendicular to the ground；Make the light of video camera when video camera is installed Axle is parallel to the ground；

Step 1.2：Radar fix system 0 is set up by origin of the center of radar₀-X₀Y₀Z₀, plane is by X where millimetre-wave radar₀Axle And Y₀Axle determines and and Z₀Axle is perpendicular, Z₀Axle it is parallel to the ground and with agricultural machinery central axes；Set up camera coordinate system Oc-XcYcZc, is origin Oc with the center of video camera, and parallel to the imaging plane of video camera, Zc axles are shootings to plane XcOcYc The optical axis and perpendicular to imaging plane of finding a view of machine；Set up the center and vehicle of vehicle axis system Ow-XwYwZw, Ow for agricultural machinery rear axle Central axis intersection point, Xw axles level to the right and perpendicular to the longitudinal central axis line of agricultural machinery, Zw it is horizontal forward and with agricultural machinery center Axis overlap, Yw axles perpendicular to the water surface ground upwardly, the X of radar fix system₀O₀Z₀The XwOwZw planes of plane and vehicle axis system It is parallel；

Step 1.3：The point that optical axis intersects with imaging plane is image principal point O ', and vehicle coordinate is by spin matrix R and is translated towards Amount s_cCamera coordinates (x is obtained after conversion_c, y_c, z_c, 1)^T, the vehicle coordinate of arbitrfary point P is (x_w, y_w, z_w, 1)^T, vehicle is sat Mark is converted to camera coordinates, and specific transformational relation is as follows,

In formula (1-1), R is an orthogonal matrices for the row of three row three, s_c(x_c0, y_c0, z_c0) it is vehicle coordinate under primary condition It is tied to the 1*3 translation matrix of camera coordinate system, x_c0Central shaft where video camera and the straight line of vehicle center axis two away from From y_c0For under primary condition video camera apart from ground height, z_c0It is video camera apart from the distance of agricultural machinery rear axle；

Step 1.4：By camera coordinates (x_c, y_c, z_c, 1)^TIt is transformed into image physical coordinates (x₁, y₁)^T, specific transformational relation is such as Under,

Step 1.5：By image physical coordinates (x₁, y₁)^TImage pixel coordinates (u, v) are transformed into, specific transformational relation is as follows：

Wherein, dx, dy represent that each pixel is horizontally and vertically going up unit-sized, u respectively₀、v₀Respectively image pixel coordinates system The transverse and longitudinal coordinate of lower camera optical axis and imaging plane intersection point, coordinate unit is pixel；

Step 1.6：The conversion formula that image pixel coordinates are tied to vehicle axis system is obtained according to above formula (1-1)~(1-3), It is specifically,

Step 1.7：In order that radar and visual information are spatially merged, the coordinate transformation relation in step 1.6 is updated to,

Wherein, s=s_c+s₀, s₀Coordinate be set to (x_s0,y_s0,z_s0), x_s0=0, y_s0It is the height on distance by radar ground under primary condition Degree, z_s0It is radar and the distance of agricultural machinery rear axle.

3. it is according to claim 2 based on farm environment perceive farm machinery navigation control method, it is characterised in that the step Industrial computer in rapid 2 carries out the transformational relation of data processing real-time adjustment radar and camera coordinates, is specifically, according to agricultural machinery Actual road conditions real-time adjustment translation vector s, under scan period t adjust after translation vector s_t=s_c+s₀+△s_t, real-time car Coordinate and the transformational relation of image pixel coordinates, be specifically,

Wherein, △ h_ctIt is video camera under scan period t and the changing value of ground level, △ h_stIt is radar under scan period t and ground The changing value of face height, j is scan period number, (u_t,v_t) under scan period t during agricultural machinery working real-time update calculate The image pixel coordinates for arriving.

4. it is according to claim 3 based on farm environment perceive farm machinery navigation control method, it is characterised in that

Step 2.1：The high variable quantity between radar and ground and the high variable quantity between video camera and ground, tool are calculated in real time Body is,

I moment and i-1 moment radar and ground level distance change value are △ h in scan period t_sti, calculated using the method for average and swept Retouch high variable quantity △ h of the radar with respect to ground in cycle t_st,

Assuming that video camera and ground level changing value are △ h under sampling instant i and sampling instant i-1 in scan period t_cti, use The method of average calculates the high variable quantity △ h on video camera and ground in scan period t_ct,

Wherein, k is the sum of the sampled point in a scan period.

5. it is according to claim 4 based on farm environment perceive farm machinery navigation control method, it is characterised in that the step Resolving radar data in rapid 3 determines effective target, specifically includes following steps,

Step 3.1：The data received to radar are resolved according to millimetre-wave radar agreement, obtain objects in front with respect to radar Angle [alpha], apart from r, relative velocity v, the reflected intensity of objects in front and be each Target Assignment only one ID；

Step 3.2：Random noise signal is filtered, it is ensured that the continuous effective of radar data, is specifically, define z= [r,α,v]^TIt is the measured value of radar, z (k) is the measured value of millimetre-wave radar kth time output,

d²=S (z (k)-z (k-1)) (z (k)-z (k-1))^T＜ r_s ² (3-1)

Filter out the data-signal for not being inconsistent box-like (3-1)；Wherein, d is the weighting Europe between adjacent measurement vector z (k), z (k-1) Family name's distance, S is weighting matrix, r_sIt is the threshold value of setting；

Step 3.3：Whether target is judged in the track of agricultural machinery traveling, and when radar objects in front meets di≤ds, target exists In agricultural machinery traveling lane, otherwise, target elects effective mesh as not in agricultural machinery traveling lane at the beginning of the target in agricultural machinery traveling lane Mark, and numbering is ranked up according to criterion from the close-by examples to those far off to it；Target outside agricultural machinery traveling lane is benign target, will It is excluded；Wherein, ds is safe distance threshold value, and ds=L/2+ks, di are the target and Z that are measured under i sampled points₀Between axle away from From L is the plough tool width hung on agricultural machinery, and ks is the safe clearance of setting；

Step 3.5：According to the effective target for determining, the minimum distance barrier obtained by millimetre-wave radar is defined as candidate Most risk object, if it is the distance between effective target of j that dj≤dmin, dj are agricultural machinery and the ID that millimetre-wave radar is obtained, Dmin is the distance of acquired agricultural machinery within one scan period of millimetre-wave radar and nearest effective target, then ID is having for j Effect target is most risk object；

Wherein, validity check is carried out to the effective target of primary election in the step 3.4 and specifically includes following steps,

Step 3.4.1：Effective target to primary election is predicted, and chooses state Sn=[d_n,v_n,a_n], the shape of primary election effective target State predictive equation is,

Wherein, d_(n+1,n)、v_(n+1,n)、a_(n+1,n)It is the status information of effective obstacle target of upper scan period prediction, d_n, v_n,a_nRelative distance, relative velocity, the phase of the effective obstacle target measured in the detection cycle of millimetre-wave radar n-th are represented respectively To acceleration, t is the scan period of millimetre-wave radar；

Step 3.4.2：By the status information and radar of the (n+1)th cycle effective target of comparison prediction it is actually measured (n+1)th The status information of cycle effective target, it is specific as follows,

Step 3.4.3：Effective obstacle target in the scan period of radar by continuous probe to more than m time, meanwhile, satisfaction is walked The effective target of formula (3-3) is consistent with primary election effective target in rapid 3.4.2, then the relative distance of more fresh target, relative velocity, Relative angle, number information；Otherwise, the effective target of primary election is not in millimetre-wave radar detection target, uses effective target Information of forecasting is tracked to the effective target of primary election, if the effective target of primary election in next scan period of radar still It is not detected, then stops using corresponding primary election effective target information, updates effective target information, and return to step 3.4.1 circulation is performed.

6. it is according to claim 5 based on farm environment perceive farm machinery navigation control method, it is characterised in that the step Judge that the sound state of most risk object specifically includes following steps in rapid 4,

Step 4.1：According to the most risk object determined in step 3.5, the relative velocity of most risk object and relative is constantly updated Whether range information, judges the distance of most risk object and radar in the range of stopping distance, i.e. z_d>z_min(4-1), z_dIt is millimeter Radar and the relative distance of most risk object that ripple radar detection is arrived, z_minIt is the stopping distance threshold value of setting, most risk object is expired During sufficient formula (4-1), agricultural machinery continues to travel；

v≠v_Car (4-2)

Within the continuous scan period, when (4-2) formula is set up all the time, the state for judging target is dynamic, and now, industrial computer sends Sound and light alarm, z_d≤z_minWhen, parking waiting instruction is sent to navigation case by industrial computer, and navigation case control agricultural machinery does parking waiting Treatment；Otherwise, agricultural machinery continues to travel, and is back to step 3.1 circulation execution, wherein, v is big for the speed of radar relative target It is small, v_CarIt is the travel speed of agricultural machinery；When (4-2) formula is invalid all the time, judge that target is static state, then industrial computer is made at avoidance Reason, is specifically that camera-scanning goes out the edge contour of barrier, plough tool width and agricultural machinery minimum turn of the industrial computer according to agricultural machinery Curved radius sets an avoidance path；Industrial computer parses the front wheel angle of agricultural machinery according to the avoidance path for setting, and will be dynamic Navigation case is sent to as instruction, navigation case control agricultural machinery is walked according to the avoidance path of setting；

The left and right sides center of agricultural machinery is separately installed with radar one and radar two；Agricultural machinery during avoidance, the He of radar one Radar two continually scans for whether the agricultural machinery left and right sides has barrier, and the relative distance for setting radar one with barrier is d₁, setting Radar two is d with the relative distance of barrier₂, judge whether agricultural machinery continues to be walked according to avoidance path according to below equation,

d₁<d_s0 (4-3)

d₂<d_s0 (4-4)

When (4-3) or (4-4) any one formula is set up, industrial computer makes parking waiting decision-making, and navigation case control agricultural machinery stops； Otherwise, agricultural machinery continues to be walked according to avoidance path；

Wherein, d_s0It is the turning security distance of setting.

7. it is according to claim 5 based on farm environment perceive farm machinery navigation control method, it is characterised in that the step Avoidance path in rapid 4.2 is specifically,

Characteristic circle is done by the center of circle of the center of barrier, the radius of characteristic circle is r_min+ w/2, avoidance path is by arc section one, straight Line segment one, arc section two, straightway two and arc section three are constituted, one end of arc section one straight line path phase original with agricultural machinery Cut, the other end of arc section one is tangent with one end of straightway one, the other end of straightway one and one end of straightway two are distinguished Tangent with arc section two, the other end of straightway two is tangent with arc section three, and arc section two is characterized a section on circle, arc section One and center line of the arc section three on arc section two be symmetrical arranged, agricultural machinery sequentially passes through arc section one, straightway one, arc section 2nd, straightway two and the cut-through thing of arc section three, wherein, r_minIt is the min. turning radius of agricultural machinery, w is wide for the operation of agricultural machinery Degree, the circumradius of barrier is less than min. turning radius r_min；The radius of arc section one is r_min, the half of the arc section three Footpath is r_min, the starting point of arc section one is designated as H points, and the center of circle of arc section one is designated as O₁Point, straightway one and the original straight line of agricultural machinery The joining in path is designated as J, and straightway one is designated as D with the points of tangency of arc section two, and agricultural machinery original path intersects with characteristic circle Point is designated as K and K ' respectively, and JK=w/2, the center of circle of arc section two is designated as O points, and the coordinate of O is set to (a, b), the center of arc section two Point is designated as B points, and the coordinate of J points is designated as (x1, y1), and the equation of JD can be write as：

Y=k (x-x₁)+y₁(4-5)；

The equation of characteristic circle can be write as：

R=r_min+w/2 (4-6)

y₂=y₁+r_min (4-8)

O is obtained according to formula (4-7) and (4-8)₁Coordinate；Then the coordinate of H points is (x₂, y₁), the coordinate of B points is (a, b+r).

8. it is according to claim 7 based on farm environment perceive farm machinery navigation control method, it is characterised in that the step The view data of the most risk object collected according to radar and camera in rapid 4, judges the type of most risk object, and navigate case Control agricultural machinery does corresponding action, specifically includes following steps,

Step 4.1a：In the case of most risk object is for dynamic, video camera is identified to most risk object, and video camera is obtained most The image of risk object, carries out image matching and compares with the human sample training storehouse for training, and exports target identification result；

Step 4.2a：According to the target identification output control agricultural machinery action of output, if non-human, then the case that navigates sends navigation case Sound and light alarm, and control agricultural machinery to do parking waiting treatment；If target identification result is human body, navigation case sends sound and light alarm, Judge whether human body deviates agricultural machinery traveling lane or human body and moved to away from agricultural machinery direction, judged with below equation,

z_wn+1>z_wn (4-3)

di>ds (4-4)

If the human body target that radar detection is arrived meets (4-3) or (4-4), agricultural machinery continues to move forward, case control of otherwise navigating Agricultural machinery does parking waiting treatment；z_wnIt is the distance of the n-th detection scanning cycle radar most risk object relatively, z_w(n+1)For next The distance of scan period radar most risk object relatively.

9. the farm machinery navigation control method perceived based on farm environment according to right wants 1~8 any one, its feature exists In the range sensor is arranged on the center of slider top, and range sensor is just to inner walls top.

10. according to any one of claim 1~8 based on farm environment perceive farm machinery navigation control method, its feature It is that the range sensor is arranged on the center at the top of inner walls, and range sensor is just to the center of slider top.