CN106249742A - The method and system that robot ridge row identification guides are realized based on laser radar detection - Google Patents

Abstract

The invention discloses and realize, based on laser radar detection, the method and system that robot ridge row identification guides, step: the laser radar that robot carries gathers the row left and right sides, the ridge distance away from plant protection robot and polar angle；Set marginal value, use extreme value filter method to remove the interference information in the distance collected；Carry out coordinate transform, polar coordinate are converted into rectangular coordinate；After coordinate transform, split data into two classes: carry out being divided into two classes by the distance of the row left and right sides, ridge, utilize categorical data respectively border, the row left and right sides, ridge envelope to be carried out fitting a straight line, obtain border, row both sides, ridge linear equation；The centrage matching in dual-side boundary line: according to the linear equation of linear equation matching ridge, border, row both sides, the ridge row centrage obtained；Using ridge row centrage as leading line, the course deviation of calculating robot and trajector deviation；The guiding to robot ridge row identification is realized by course deviation and trajector deviation.Path identification method of the present invention is relatively simple, efficient.

Description

The method and system that robot ridge row identification guides are realized based on laser radar detection

Technical field

The present invention relates to realize, based on laser radar detection, the method and system that robot ridge row identification guides.

Background technology

Traditional and more classical Path Recognition mode is to use machine vision, and machine vision is for the identification of barrier It it is feature based recognition methods.But, unstructured road environment is more complicated, does not has specific recognition marks, it is impossible to provide Barrier range information accurately.Therefore, the identification of the detection of barrier in unstructured road with path is become by machine vision Obtain helpless.

Summary of the invention

The purpose of the present invention is contemplated to solve the problems referred to above, it is provided that a kind of detection based on laser radar realizes robot ridge Row identifies the method and system guided, and it utilizes laser radar to gather ridge row environmental information, decoded, filtering, coordinate transform, limit The processes such as boundary's matching, navigation center's line drawing, it is achieved the local paths planning in row region, ridge.And with the trajector deviation asked for and boat To deviation as the input quantity of controller, using theoretical velocity and theoretical course angle as output, and then control actuator.

To achieve these goals, the present invention adopts the following technical scheme that

Realize, based on laser radar detection, the method that robot ridge row identification guides, comprise the steps:

Step (1): the laser radar that plant protection robot carries gather the row left and right sides, the ridge distance away from plant protection robot and Polar angle；

Step (2): Filtering Processing: set marginal value, uses extreme value filter method to remove the interference letter in the distance collected Breath；

Step (3): coordinate transform；Set up the geometric coordinate relation of the distance after processing after filtering and robot location, Carry out coordinate transform, polar coordinate are converted into rectangular coordinate；

Step (4): after coordinate transform, splits data into two classes: carry out being divided into two classes by the distance of the row left and right sides, ridge, Side is a class；

Step (5): both sides edge fitting: utilize categorical data respectively border, the row left and right sides, ridge envelope to be carried out straight line Matching, obtains border, row both sides, ridge linear equation；

Step (6): the centrage matching in dual-side boundary line: according in linear equation matching ridge, border, row both sides, the ridge row obtained The linear equation of heart line；

Step (7): using ridge row centrage as leading line, the course deviation of calculating robot and trajector deviation；By boat The guiding to robot ridge row identification is realized to deviation and trajector deviation.

Realize, based on laser radar detection, the method that robot ridge row identification guides, also include:

Step (8): using the course deviation asked for and trajector deviation as the input quantity of the PID controller of robot, PID is controlled Device processed exports theoretical travel speed Vd and theoretical deflection angle γ d after processing；PID controller is according to theoretical travel speed Vd and theory Robot is controlled by deflection angle γ d；

Meanwhile, velocity sensor is obtained by plant protection robot actual speed V and the actual rotational angle that rotary angle transmitter obtains θ, input is to PID controller, it is achieved robot speed and the feedback of deflection angle, repeats step (1)-step (8) until completing machine The row identification of device people ridge guides.

Also include between described step (1) and step (2):

Step (10): character code and decoding: for improving the transfer rate of information, the distance collected is carried out at coding Reason；

The step of described step (10) is: inside laser radar, and each distance collected is all with 18 two System represents, adds 30H by every 6 respectively as one group, and then unification sends；Laser radar is to obtain actual distance, right Receiving decoding data, be individually subtracted 30H, the most every three groups are combined into a distance value.

The marginal value of described step (2) refers to: the line space L of ridge row and the absolute value of the difference of robot width D.

The interference information of described step (2) refers to: the distance more than marginal value and the distance less than ridge line space 1/3.

Polar coordinate are converted into shown in the transformation process such as formula (1) of rectangular coordinate by described step (3):

$\left\{\begin{array}{c}x=\mathrm{\ρ}cos\mathrm{\α}\\ y=\mathrm{\ρ}sin\mathrm{\α}\end{array}\right.---\left(1\right)$

Wherein, α is the angular range of laser radar detection；ρ is the distance value that under polar coordinate system, laser radar collects；x For the abscissa value under the rectangular coordinate system of acquisition after coordinate transform；Y is the rectangular coordinate obtained after coordinate transform Ordinate value under Xi.

The formula of the fitting a straight line of described step (5) is:

$\left\{\begin{array}{c}y1=k1*x1+b1\\ y2=k2*x2+b2\end{array}\right.---\left(2\right)$

K1 is the slope of the straight line after the row matching of ridge, side；B1 is the intercept of the straight line after the row matching of ridge, side；X1 is one The abscissa of the point on straight line after the row matching of ridge, side, y1 is the vertical coordinate of the point on the straight line after the row matching of ridge, side；

K2 is the slope of the straight line after the row matching of opposite side ridge；B2 is the intercept of the straight line after the row matching of opposite side ridge；x2 For the abscissa of the point on the straight line after the row matching of opposite side ridge, y2 is the vertical seat of the point on the straight line after the row matching of opposite side ridge Mark.

The step of described step (6) is:

Step (61) first asks for the slope k 3 of ridge row centrage:

K3=(k1+k2)/2； (3)

The depth of parallelism of step (62) boundary line judges:

If two boundary straight line is parallel, then

B3=(b1+b2)/2； (4)

B3 is the intercept of the ridge row centrage asked for；

If two boundary straight line is not parallel, then ask for the intersecting point coordinate (x3, y3) of two straight lines:

B3=y3-x3*k3； (5)

K3 is the slope of ridge row centrage；B3 is the intercept of the ridge row centrage asked for；

(x3, y3) is the intersection coordinate value of two boundary lines of matching；

To sum up, the linear equation of ridge row centrage is:

Y'=k3*x'+b3； (6)；

Wherein, x' is the abscissa of the point of ridge row centrage, and y' is the vertical coordinate of the point of ridge row centrage.

Described step (7) calculates the formula of course deviation:

Δ ψ=arctank3 (7)

Described step (7) calculates the formula of trajector deviation:

$\mathrm{\Δ}d=\left|b3\right|/\sqrt{{\left(k3\right)}^2+1}---\left(8\right)$

Output course deviation Δ ψ and the value Δ d of trajector deviation.

The system that robot ridge row identification guides is realized based on laser radar detection, including:

Data acquisition unit: plant protection robot carry laser radar gather the row left and right sides, ridge away from plant protection robot away from From and polar angle；

Filter processing unit: set marginal value, uses extreme value filter method to remove the interference information in the distance collected；

Coordinate transformation unit: set up the geometric coordinate relation of the distance after processing after filtering and robot location, carry out Polar coordinate are converted into rectangular coordinate by coordinate transform；

Data sorting unit: after coordinate transform, splits data into two classes: the distance of the row left and right sides, ridge be divided into Two classes, side is a class；

Edge fitting unit: both sides edge fitting: utilize categorical data respectively border, the row left and right sides, ridge envelope to be entered Row fitting a straight line, obtains border, row both sides, ridge linear equation；

Centrage fitting unit: the centrage matching in dual-side boundary line: intend according to border, row both sides, the ridge linear equation obtained Close the linear equation of ridge row centrage；

Deviation computing unit: using ridge row centrage as leading line, the course deviation of calculating robot and trajector deviation；Logical Cross course deviation and trajector deviation realizes the guiding to robot ridge row identification.

Realize, based on laser radar detection, the method that robot ridge row identification guides, also include:

Control unit: using the course deviation asked for and trajector deviation as the input quantity of the PID controller of robot, PID Controller exports theoretical travel speed Vd and theoretical deflection angle γ d after processing；PID controller is according to theoretical travel speed Vd and reason Robot is controlled by opinion deflection angle γ d；

Meanwhile, velocity sensor is obtained by plant protection robot actual speed V and the actual rotational angle that rotary angle transmitter obtains θ, input is to PID controller, it is achieved robot speed and the feedback of deflection angle, guides until completing robot ridge row identification.

Also include between described data acquisition unit and filter processing unit:

Character code and decoding unit: for improving the transfer rate of information, the distance collected is carried out coded treatment.

Beneficial effects of the present invention:

1, path identification method is relatively simple, efficient；

2, improve the precision of path navigation；

3, improve adaptive capacity to environment, round-the-clock strong adaptability；

4, laser radar is that Laser emission is gone out and passes through to measure transmitting light and from body surface by the illuminator by rotation Time difference between reflection light is found range.Laser radar can obtain polar angle and pole span two amount, and the angle model measured simultaneously Enclosing bigger, the measurement time meets the requirement of real-time of motion.Owing to laser radar has a round-the-clock strong adaptability, sensitivity and The features such as resolution is high, informative.

Accompanying drawing explanation

Fig. 1 is that laser radar gathers ridge row environmental information schematic diagram；

Fig. 2 is laser radar data collection and process chart；

Fig. 3 is to extract ridge row bound line and the envelope schematic diagram of centrage；

Fig. 4 is ridge row centrage and motor control relation schematic diagram；

Fig. 5 robot hardware connects figure.

Detailed description of the invention

The invention will be further described with embodiment below in conjunction with the accompanying drawings.

Laser radar range is as the prerequisite of environment sensing, and it is every certain angle one distance value of detection, and And the environmental information gathered represents in polar mode.The ridge row environmental information gathered represents such as Fig. 1 in rectangular coordinate system Shown in.Local paths planning is carried out, based on laser radar data collection and process chart such as by means of laser radar sensor Shown in Fig. 2.

The present invention uses laser radar to gather ambient condition information.After connecting hardware, electrifying startup, laser radar is first First completing initialization procedure, then controller is periodically sent out instruction to laser radar, obtains the data that laser radar gathers；For Improve the efficiency of transmission of data, it will usually setting transmission protocol, therefore after controller receives data, need data Decoding.

On the basis of data acquisition and parsing, owing to being affected by extraneous adverse circumstances, it may appear that a lot Noise spot.These put and are not belonging to useful obstacle information point, need to be filtered processing to the signal gathered.Screening is effectively Data message, remove interference information.The method using extreme value filtering removes the interference to this ridge row data acquisition of other ridge row. Using the difference of the line-spacing L of ridge row and robot width D as the marginal value of filtering.

After filtering, gather the geometric coordinate relation of data and robot for setting up ridge row, first need to carry out coordinate transform, will Polar coordinate are converted into rectangular coordinate (α is the angular range that laser radar detects).Shown in transformation process such as formula (1):

$\left\{\begin{array}{c}x=\mathrm{\ρ}cos\mathrm{\α}\\ y=\mathrm{\ρ}sin\mathrm{\α}\end{array}\right.---\left(1\right)$

After coordinate transform, these data are divided into two classes, as using the distance value of-30 degree to 60 degree as the side of ridge row, Using 120 degree to the 210 degree of opposite sides as ridge row, carry out the fitting a straight line of border, the row left and right sides, ridge envelope respectively.And ask Take linear equation to be respectively as follows: $\left\{\begin{array}{c}y1=k1*x1+b1\\ y2=k2*x2+b2\end{array}\right.---\left(2\right)$

After extracting row dual-side boundary line, ridge, two linear equation matching ridge row centrage equations according to obtaining:

1) slope of ridge row centrage is first asked for:

K3=(k1+k2)/2 (3)

2) depth of parallelism of boundary line judges: if the two of matching boundary straight line are parallel, then

B3=(b1+b2)/2 (4)；

If two boundary straight line is not parallel, the intersecting point coordinate of two straight lines can be asked for, be set to (x3, y3).Because of Long Hang center Line must be through the intersection point of two straight lines, so can try to achieve:

B3=y3-x3*k3 (5)；

To sum up, the linear equation trying to achieve ridge row centrage is:

Y'=k3*x'+b3 (6).

Finally, the laser radar pose relative to ridge row centrage, geometric coordinate relation such as Fig. 3 between the two need to be determined Shown in: course deviation:

Δ ψ=arctank3 (7)

Lateral deviation:

$\mathrm{\Δ}d=\left|b3\right|/\sqrt{{\left(k3\right)}^2+1}---\left(8\right)$

Output course deviation Δ ψ and the value Δ d of lateral deviation.

As shown in Figures 3 and 4, after obtaining the value of Δ ψ and Δ d, as the input of PID controller based on motion model Amount, via controller exports theoretical travel speed Vd and the control signal of theoretical deflection angle γ d, and transports robot after processing Dynamic control.Gathering actual speed V and the actual rotational angle α of robot, the data these gathered input to STM32 monolithic simultaneously Machine, after PID controller processes, it is achieved robot speed and the feedback control of deflection angle.

As it is shown in figure 5, robot is connected with laser radar, rotary angle transmitter, velocity sensor and STM32 single-chip microcomputer, institute State STM32 single-chip microcomputer and be provided with PID controller.

Although the detailed description of the invention of the present invention is described by the above-mentioned accompanying drawing that combines, but not the present invention is protected model The restriction enclosed, one of ordinary skill in the art should be understood that on the basis of technical scheme, and those skilled in the art are not Need to pay various amendments or deformation that creative work can make still within protection scope of the present invention.

Claims (10)

1. realize, based on laser radar detection, the method that robot ridge row identification guides, it is characterized in that, comprise the steps:

Step (1): the laser radar that plant protection robot carries gathers the row left and right sides, the ridge distance away from plant protection robot and polar angle；

Step (2): Filtering Processing: set marginal value, uses extreme value filter method to remove the interference information in the distance collected；

Step (3): coordinate transform；Set up the geometric coordinate relation of the distance after processing after filtering and robot location, carry out Polar coordinate are converted into rectangular coordinate by coordinate transform；

Step (4): after coordinate transform, splits data into two classes: carry out being divided into two classes, side by the distance of the row left and right sides, ridge It it is a class；

Step (5): both sides edge fitting: utilize categorical data respectively border, the row left and right sides, ridge envelope to be carried out straight line plan Close, obtain border, row both sides, ridge linear equation；

Step (6): the centrage matching in dual-side boundary line: according to linear equation matching ridge, border, row both sides, the ridge row centrage obtained Linear equation；

Step (7): using ridge row centrage as leading line, the course deviation of calculating robot and trajector deviation；Inclined by course Difference and trajector deviation realize the guiding to robot ridge row identification.

2. realize, based on laser radar detection, the method that robot ridge row identification guides as claimed in claim 1, it is characterized in that, Also include:

Step (8): using the course deviation asked for and trajector deviation as the input quantity of the PID controller of robot, PID controller Theoretical travel speed Vd of output and theoretical deflection angle γ d after process；PID controller is according to theoretical travel speed Vd and theoretical deflection Robot is controlled by angle γ d；

Meanwhile, actual speed V that velocity sensor is obtained by plant protection robot and the actual rotational angle θ that rotary angle transmitter obtains, defeated Entering to PID controller, it is achieved robot speed and the feedback of deflection angle, repeating step (1)-step (8) until completing robot Ridge row identification guides.

3. realize the method that robot ridge row identification guides, its feature based on laser radar detection as claimed in claim 1 or 2 It is also to include between described step (1) and step (2):

Step (10): character code and decoding: for improving the transfer rate of information, the distance collected is carried out coded treatment.

4. realize, based on laser radar detection, the method that robot ridge row identification guides as claimed in claim 3, it is characterized in that,

The step of described step (10) is: inside laser radar, and each distance collected is all the binary system with 18 Representing, adding 30H by every 6 respectively as one group, then unification sends；Laser radar is to obtain actual distance, to reception Decoding data, is individually subtracted 30H, and the most every three groups are combined into a distance value.

5. realize the method that robot ridge row identification guides, its feature based on laser radar detection as claimed in claim 1 or 2 It is that the marginal value of described step (2) refers to: the line space L of ridge row and the absolute value of the difference of robot width D.

6. realize the method that robot ridge row identification guides, its feature based on laser radar detection as claimed in claim 1 or 2 It is that the interference information of described step (2) refers to: the distance more than marginal value and the distance less than ridge line space 1/3.

7. realize the method that robot ridge row identification guides, its feature based on laser radar detection as claimed in claim 1 or 2 It is that described step (7) calculates the formula of course deviation and is:

Δ ψ=arctank3； (7)

Described step (7) calculates the formula of trajector deviation:

$\mathrm{\Δ}d=\left|b3\right|/\sqrt{{\left(k3\right)}^2+1};---\left(8\right)$

Output course deviation Δ ψ and the value Δ d of trajector deviation.

8. realize, based on laser radar detection, the system that robot ridge row identification guides, it is characterized in that, including:

Data acquisition unit: the laser radar that plant protection robot carries gather the row left and right sides, the ridge distance away from plant protection robot and Polar angle；

Filter processing unit: set marginal value, uses extreme value filter method to remove the interference information in the distance collected；

Coordinate transformation unit: set up the geometric coordinate relation of the distance after processing after filtering and robot location, carry out coordinate Polar coordinate are converted into rectangular coordinate by conversion；

Data sorting unit: after coordinate transform, splits data into two classes: carry out being divided into two by the distance of the row left and right sides, ridge Class, side is a class；

Edge fitting unit: both sides edge fitting: utilize categorical data respectively border, the row left and right sides, ridge envelope to be carried out directly Line matching, obtains border, row both sides, ridge linear equation；

Centrage fitting unit: the centrage matching in dual-side boundary line: according to the linear equation matching ridge, border, row both sides, ridge obtained The linear equation of row centrage；

Deviation computing unit: using ridge row centrage as leading line, the course deviation of calculating robot and trajector deviation；By boat The guiding to robot ridge row identification is realized to deviation and trajector deviation.

9. realize, based on laser radar detection, the method that robot ridge row identification guides as claimed in claim 8, it is characterized in that, Also include:

Control unit: using the course deviation asked for and trajector deviation as the input quantity of the PID controller of robot, PID controls Device exports theoretical travel speed Vd and theoretical deflection angle γ d after processing；PID controller is inclined according to theoretical travel speed Vd and theory Robot is controlled by corner γ d；

Meanwhile, actual speed V that velocity sensor is obtained by plant protection robot and the actual rotational angle θ that rotary angle transmitter obtains, defeated Entering to PID controller, it is achieved robot speed and the feedback of deflection angle, guiding until completing robot ridge row identification.

10. realize the method that robot ridge row identification guides, its feature based on laser radar detection as claimed in claim 8 It is,

Also include between described data acquisition unit and filter processing unit:

Character code and decoding unit: for improving the transfer rate of information, the distance collected is carried out coded treatment.