CN105094134B - A kind of AGV stopping a train at a target point methods based on image line walking - Google Patents
A kind of AGV stopping a train at a target point methods based on image line walking Download PDFInfo
- Publication number
- CN105094134B CN105094134B CN201510528410.8A CN201510528410A CN105094134B CN 105094134 B CN105094134 B CN 105094134B CN 201510528410 A CN201510528410 A CN 201510528410A CN 105094134 B CN105094134 B CN 105094134B
- Authority
- CN
- China
- Prior art keywords
- mrow
- mtd
- mtr
- mtable
- mfenced
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Fee Related
Links
- 238000000034 method Methods 0.000 title claims abstract description 34
- 238000001914 filtration Methods 0.000 claims abstract description 17
- 238000000605 extraction Methods 0.000 claims abstract description 10
- 238000006243 chemical reaction Methods 0.000 claims abstract description 5
- 238000000926 separation method Methods 0.000 claims abstract description 4
- 239000000284 extract Substances 0.000 claims description 16
- 230000008569 process Effects 0.000 claims description 16
- 239000011159 matrix material Substances 0.000 claims description 15
- 238000005259 measurement Methods 0.000 claims description 13
- 238000010276 construction Methods 0.000 claims description 3
- 238000011217 control strategy Methods 0.000 claims description 3
- 238000009826 distribution Methods 0.000 claims description 3
- 239000007787 solid Substances 0.000 claims description 3
- 230000009466 transformation Effects 0.000 claims description 3
- 230000007704 transition Effects 0.000 claims description 3
- 241000208340 Araliaceae Species 0.000 claims 1
- 235000005035 Panax pseudoginseng ssp. pseudoginseng Nutrition 0.000 claims 1
- 235000003140 Panax quinquefolius Nutrition 0.000 claims 1
- 230000002146 bilateral effect Effects 0.000 claims 1
- 235000008434 ginseng Nutrition 0.000 claims 1
- 238000010586 diagram Methods 0.000 description 8
- 238000005516 engineering process Methods 0.000 description 6
- 230000008901 benefit Effects 0.000 description 3
- 238000003860 storage Methods 0.000 description 3
- 235000013399 edible fruits Nutrition 0.000 description 2
- 230000008859 change Effects 0.000 description 1
- 239000003086 colorant Substances 0.000 description 1
- 238000007796 conventional method Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 230000002452 interceptive effect Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 239000000463 material Substances 0.000 description 1
- 230000035800 maturation Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000005192 partition Methods 0.000 description 1
- 230000004044 response Effects 0.000 description 1
- 230000011218 segmentation Effects 0.000 description 1
Landscapes
- Navigation (AREA)
- Train Traffic Observation, Control, And Security (AREA)
Abstract
A kind of AGV stopping a train at a target point methods based on image line walking, medium filtering is carried out by the image collected;HSV data formats are converted the image into, the stopping terrestrial reference of setpoint color in image is extracted, obtains containing stoppingly target bianry image;Computing successively is opened and closed to bianry image;Judge that obtained landmark region is errorless, each connected region barycentric coodinates is extracted respectively and average coordinates are tried to achieve;Average longitudinal image coordinate will be obtained by existing Coordinate Conversion form and be converted to real ground distance information dy;By dyWith the speed progress Kalman filtering read on AGV encoders;By ground distance information dyAs input quantity, it is input in automatic disturbance rejection controller, calculates the AGV output speeds for obtaining subsequent time, forms a position-force control device;Repeat the stopping terrestrial reference in above step, real-time separation and Extraction image and control AGV high accuracy positionings to stop, until vehicle stops.Control accuracy of the present invention is higher, have good stability.
Description
Technical field
The present invention is applied to the AGV control fields based on image line walking, is related to a kind of suitable for the AGV based on image line walking
Stopping a train at a target point method.
Background technology
AGV (Automated Guided Vehicle) namely automatical pilot transportation vehicle is realized in automatic factory
One key areas, this technology is mainly used in the material handling storage work in the bulk storage plant with a large amount of shelf, relates to
And multiple related skills such as sensor data acquisition, data filtering, routing information extraction, motion control, motor driving, data transfer
Art field.
With the inexorable trend of developing rapidly for autonomous driving vehicle technology in recent years, and the production automation so that should
The market demand for the automatical pilot transportation vehicle of bulk storage plant is also more and more stronger.In automatical pilot transportation vehicle control technology
The stopping a train at a target point technology of efficiently and accurately be one of them highly important link.At present, the sensing of general identification path of navigation
Scheme has magnetic navigation, optical navigation etc., and these schemes are relatively easy and maturation, but be due to sensor self information amount it is very few and
Lead to not realize accurate stopping a train at a target point.Conventional method is stopped using digital output modul mostly, that is, when vehicle-mounted sensing
Device detects stop sign and begins to ramp to stop, so can not often vehicle is accurately rested in precalculated position.And be based on
The automatical pilot transportation vehicle of image procossing, which can then be realized, is accurately positioned parking, and cost of implementation is relatively low.
At present, the quick pinpoint stopping technical in the AGV control technologies based on image line walking for AGV rarely has and related to
And, and the mode of general vehicle positioning stop is the speed planning that Each point in time is carried out to AGV speeds, enables AGV relatively accurate
Stopping.But required precision of this mode to AGV speed closed loops is very harsh, but in practice because AGV is unloaded and full
System performance during load has significant change, causes speed ring to be extremely difficult to satisfied precision.So, if can solve to be based on image
The quick pinpoint parking problem of closed loop, then can further excavate image-guidance be applied to AVG fields advantage, improve
AGV control accuracies and stability.
The content of the invention
In order to overcome, the control accuracy of the existing AGV stopping a train at a target point modes based on image line walking is relatively low, less stable
The problem of, the invention provides a kind of control accuracy is higher, the AGV stopping a train at a target point sides based on image line walking that have good stability
Method, while ensureing to accurately identify ground parking landmark information, having again can be by landmark information feedback control AGV speed
Degree, realizes the characteristics of closed loop is stopped.
The present invention solves above-mentioned technical problem and is achieved through the following technical solutions:
A kind of AGV stopping a train at a target point methods based on image line walking, comprise the following steps:
1) image collected is subjected to medium filtering;
2) filtered image is converted into HSV data formats, adjusts H and S threshold value, extract setpoint color in image
Stop terrestrial reference, obtain containing stoppingly target bianry image;
3) to step 2) in obtained bianry image computing successively is opened and closed, obtain stopping landmark region;
4) to step 3) the obtained stopping quantity of terrestrial reference range statistics connected region, each region area, calculate its external
The length-width ratio of rectangle and the relation of navigation guide line;If the above feature meets preparatory condition, then it is assumed that obtained terrestrial reference
Region is errorless, and extracts each connected region barycentric coodinates respectively and try to achieve average coordinates, while performing step 5 backward);Otherwise
The frame image data is abandoned, and updates camera acquisition image and is then back to execution step 1);
5) by existing Coordinate Conversion form by step 4) obtain average longitudinal image coordinate and be converted to real ground
Range information dy, by dyWith the speed progress Kalman filtering read on AGV encoders;
6) by ground distance information dyAs input quantity, it is input in automatic disturbance rejection controller, calculating obtains subsequent time
AGV output speeds, form a position-force control device;
7) repeat step 1)~6), stopping terrestrial reference in real-time separation and Extraction image simultaneously controls AGV high accuracy positionings to stop,
Until vehicle stops.
Further, in step 2) described in stopping terrestrial reference include following feature:
Stoppingly target color is different from AGV navigation guide lines, and is limited among yellow, green, red and blueness
Selection, setting is stoppingly designated as the wide 5cm of 20cm two long rectangular solid region, and is symmetrically distributed in the two of navigation guide line
Side;Meanwhile, setting stops terrestrial reference and is separated by 15cm with navigation guide line, stops the long side of terrestrial reference and navigation guide line vertical distribution.
Further, in step 4) in, described stopping terrestrial reference Region Feature Extraction and decision process is as follows:
4.1) two pieces of maximum regions of area in all connected domains of image are chosen, the area in two pieces of regions is calculated, and by two
Individual area value obtains difference as difference, then takes the absolute value of difference divided by the area in two pieces of regions and obtain result for n, this is tied
Fruit n is compared with a threshold value N set in advance, if less than threshold value N, i.e. n<N, then it is assumed that extract correct in landmark region;
If 4.2) meet condition 4.1), the boundary rectangle length-width ratio in two pieces of regions is calculated, by rectangular aspect ratio and reality
The stoppingly target boundary rectangle length and width on border ground are compared to difference and obtain difference, and take the absolute value m of difference, equally by result m
Compared with threshold value M set in advance, if less than threshold value M, i.e. m<M, then it is assumed that extract correct in landmark region.
If 4.2) 4.1) all met with condition 4.2), the barycentric coodinates in two pieces of regions are calculated, by its lateral coordinates x1、
x2With the coordinate x of middle leading line0It is y=2*x0-x1-x2Calculating obtains result y, takes y absolute value and threshold value Y set in advance
Compare, if less than threshold value Y, i.e. y<Y, then it is assumed that landmark region extracts correct, two pieces of regions stop terrestrial reference area to be correct
Domain.
Further, in step 5) in, the filtering of described Kalman filtering algorithm is as follows:
5.1) state equation of system is initially set up:
In above formula, Img_dykIt is k moment real ground distance information dy, q_biaskIt is the speed that k moment encoders are measured
Deviation is spent, dt is update cycle, Img_dyk-1It is the ground distance information d measured in k-1 time chart picturesy, q_biask-1It is k-1
The speed deviation that moment encoder is measured, speed_m is the process noise of encoder, and w_Img_dy and w_speed are respectively
The d measured in imageyThe measurement noise of the velocity amplitude measured with encoder;
5.2) measurement equation is set up:Img_ in formula
dykIt is k moment real ground distance information dy, q_biaskIt is the speed deviation that encoder is measured, v_Img_dy is ground
Range information dyWhite noise;
5.3) construction process noise matrix:Wherein Q_Img_dy is ground distance
Information dyProcess noise, Q_speed is the process noise of velocity amplitude that encoder is measured;
5.4) measurement noise matrix is constructed:[R_Img_dy], wherein R_Img_dy are ground distance information dyMeasurement make an uproar
Sound.
5.5) angle is predicted:
5.6) variance is predicted:
Wherein, Pk|k-1For the covariance matrix predicted by k-1 moment quantity of state the k moment;
5.7) kalman gain is calculated:
Wherein,For kalman gain;
5.8) variance updates:
Wherein, E is unit matrix, Pk|kFor the covariance matrix at k moment;
5.9) state estimation:
Formula (1)~(5) are computed repeatedly to obtain apart from dyOptimal result.
In step 6) in, the automatic disturbance rejection controller includes Nonlinear Tracking Differentiator, extended state observer and nonlinear combination,
Nonlinear Tracking Differentiator is that parameter inputs transition process arranging, obtains smooth input signal, and extract its differential signal, expansion state
Non-linear, uncertain system approximation is linearized and determined using the method transformation object model of two-channel compensation by observer
Property.Nonlinear combination provides the control strategy of controlled device.
The device have the advantages that:Stopping terrestrial reference area can be strengthened by introducing necessary medium filtering and opening and closing operation
The anti-interference of regional partition, it is to avoid noise information is mistaken for stop terrestrial reference;Landmark region and leading line different colours are designed, are led to
Cross hsv color domain and extract corresponding information, can effectively prevent interfering for landmark region and leading line in image.Pass through
Whether wrong stop terrestrial reference extracting, and, which improves and stoppingly marks recognition correct rate, is judged to stoppingly target feature extraction.By terrestrial reference
Image coordinate be converted to actual range, improve the linearity of input data, and Kalman filtering done to it and further carry
The high antijamming capability of range data.Finally can greatly as the controller of position closed loop using Active Disturbance Rejection Control algorithm
Improve the speed and precision of stopping a train at a target point.
Brief description of the drawings
Fig. 1 is containing the gray-scale map for stopping landmark information.
Fig. 2 is HSV Threshold segmentation schematic diagrames.
Fig. 3 is to be partitioned into stoppingly target result schematic diagram.
Fig. 4 is stopping terrestrial reference feature schematic diagram.
Fig. 5 is picturedeep and actual range relation schematic diagram.
Fig. 6 is Active Disturbance Rejection Control algorithm block architecture diagram.
Fig. 7 is stopping a train at a target point control block diagram.
Embodiment
To become apparent from the object, technical solutions and advantages of the present invention, below in conjunction with the accompanying drawings to the technical side of the present invention
Case is further described.
A kind of 1~Fig. 7 of reference picture, AGV stopping a train at a target point methods based on image line walking, methods described comprises the following steps:
1) image collected is subjected to medium filtering, eliminates the noise in image.As shown in Figure 1.To contain leading line
And stoppingly target gray-scale map.
2) filtered image is converted into HSV data formats.H and S threshold value is adjusted, setpoint color in image is extracted
Stop terrestrial reference, obtain containing stoppingly target bianry image.As shown in (a) (b) figure in Fig. 2, choose and only contain stop line information
H domains and S domains part, this makes it possible to extract stop landmark information region.
In step 2) in, the stopping terrestrial reference further describing includes following feature:Stoppingly target color is drawn with AGV navigation
Wire is different, but is only limitted to select among yellow, green, red and blueness.Stoppingly it is designated as the wide 5cm's of 20cm two long
Rectangular solid region, and symmetrically it is distributed in the both sides of navigation guide line.Meanwhile, stop terrestrial reference and be separated by with navigation guide line
15cm, stops the long side of terrestrial reference and navigation guide line vertical distribution.1. signified region is white leading line to label as shown in Figure 1,
Label 2. signified two symmetrical regions for green stoppings terrestrial reference and be distributed in white leading line both sides.
3) to step 2) in obtained bianry image computing successively is opened and closed, further eliminate noise influence, stopped
Only landmark region.Specific effect is as shown in Figure 3.Being capable of complete extraction stoppingly target connected region relatively.
4) to step 3) the obtained stopping quantity of terrestrial reference range statistics connected region, each region area, calculate its external
The length-width ratio of rectangle and the relation of navigation guide line, if the above feature meets condition, then it is assumed that obtained landmark region
It is errorless, and extract each connected region barycentric coodinates respectively and try to achieve average coordinates, while performing step 5 backward).Otherwise abandon
The frame data, and update camera acquisition image be then back to execution step 1).The Partial Feature of terrestrial reference is as shown in right figure 4.
In step 4) in, further describe stopping terrestrial reference Region Feature Extraction and decision process is as follows:
4.1) two pieces of maximum regions of area in all connected domains of image are chosen.The area in two pieces of regions is calculated, and by two
Individual area value obtains difference as difference, and takes the absolute value of difference divided by the area in two pieces of regions and obtain result for n.This is tied
Fruit n is compared with a threshold value N set in advance, if less than threshold value N, i.e. n<N, then it is assumed that extract correct in landmark region.
If 4.2) meet condition 4.1), the boundary rectangle length-width ratio boundary rectangle in two pieces of regions, such as Fig. 4 acceptances of the bid are calculated
It is number 3. shown.Rectangular aspect ratio is obtained into difference with the stoppingly target boundary rectangle length-width ratio (taking 0.25) with actual ground as difference
Value, and take the absolute value m of difference.Equally result m is compared with threshold value M set in advance, if less than threshold value M, i.e. m<M, then
Think that landmark region is extracted correct.
If 4.3) condition 4.1) and 4.2) is all met, the barycentric coodinates in two pieces of regions are calculated, in position of centre of gravity such as Fig. 4
Label is 4. shown, by its lateral coordinates x1、x2With the coordinate x of middle leading line0, as in Fig. 4 label 1. shown in, be y=2*x0-
x1-x2Calculating obtains result y.Y absolute value is taken to be compared with threshold value Y set in advance, if less than threshold value Y, i.e. y<Y, then it is assumed that ground
Mark extracted region correct, two pieces of regions stop landmark region to be correct.
5) by existing Coordinate Conversion form by step 4) obtain average longitudinal image coordinate and be converted to real ground
Range information dy.By dyKalman filtering is carried out with the speed that reads on AGV encoders and further improves terrestrial reference believing with AGV distances
The stability and accuracy of breath.Above-mentioned Coordinate Conversion form is obtained by the position record of actual scale in the picture
's.These forms are converted into chart as shown in Figure 5.Abscissa is actual range in figure, and ordinate is longitudinal pixel in image
Coordinate.
In step 5) in, the filtering for further describing Kalman filtering algorithm is as follows:
5.1) state equation of system is initially set up:
In above formula, Img_dykIt is k moment real ground distance information dy, q_biaskIt is the speed that k moment encoders are measured
Deviation is spent, dt is update cycle, Img_dyk-1It is the ground distance information d measured in k-1 time chart picturesy, q_biask-1It is k-1
The speed deviation that moment encoder is measured, speed_m is the process noise of encoder, and w_Img_dy and w_speed are respectively
The d measured in imageyThe measurement noise of the velocity amplitude measured with encoder;
5.2) measurement equation is set up:Img_ in formula
dykIt is k moment real ground distance information dy, q_biaskIt is the speed deviation that encoder is measured, v_Img_dy is ground
Range information dyWhite noise;
5.3) construction process noise matrix:Wherein Q_Img_dy is ground distance
Information dyProcess noise, Q_speed is the process noise of velocity amplitude that encoder is measured;
5.4) measurement noise matrix is constructed:[R_Img_dy], wherein R_Img_dy are ground distance information dyMeasurement make an uproar
Sound;
5.5) angle is predicted:
5.6) variance is predicted:
Wherein, Pk|k-1For the covariance matrix predicted by k-1 moment quantity of state the k moment;
5.7) kalman gain is calculated:
Wherein,For kalman gain;
5.8) variance updates:
Wherein, E is unit matrix, Pk|kFor the covariance matrix at k moment;
5.9) state estimation:
Formula (1)~(5) are computed repeatedly to obtain apart from dyOptimal result.
6) by ground distance information dyAs input quantity, it is input in automatic disturbance rejection controller, calculating obtains subsequent time
AGV output speeds, form a position-force control device.
Automatic disturbance rejection controller is further described, active disturbance rejection (ADRC) controller develops, taken from PID controller
The core concept of PID error feedback controls.Traditional PID control, which is directly drawn, takes output to be made the difference with reference input as control signal,
Cause response quickly of system occur and the contradiction of overshoot occurs.Automatic disturbance rejection controller is mainly made up of three parts:Nonlinear Tracking Differentiator,
Extended state observer and nonlinear combination.Nonlinear Tracking Differentiator is that parameter inputs transition process arranging, obtains smooth input letter
Number, and extract its differential signal.Extended state observer using two-channel compensation method transformation object model, by it is non-linear,
Uncertain system approximation linearisation and certainty.Nonlinear combination provides the control strategy of controlled device.Active Disturbance Rejection Control
Algorithm block architecture diagram is as shown in Figure 6.
7) repeat step 1)~stopping terrestrial reference that 6) just can in real time in separation and Extraction image and control AGV high accuracy positionings to stop
Car, until vehicle stops.Whole stopping a train at a target point control block diagram is as shown in Figure 7.By the range data and encoder of IMAQ
The speed data fusion of collection, which is input in Kalman filter, obtains more accurate range data, will more accurate distance
Data input controls the rotating speed of brushless electric machine so as to control the present speed of vehicle, final realize is determined into automatic disturbance rejection controller
Point parking.
Claims (5)
1. a kind of AGV stopping a train at a target point methods based on image line walking, it is characterised in that comprise the following steps:
1) image collected is subjected to medium filtering;
2) filtered image is converted into HSV data formats, adjusts H and S threshold value, extract the stopping of setpoint color in image
Terrestrial reference, is obtained containing stoppingly target bianry image;
3) to step 2) in obtained bianry image computing successively is opened and closed, obtain stopping landmark region;
4) to step 3) the obtained stopping quantity of terrestrial reference range statistics connected region, each region area, calculate its boundary rectangle
Length-width ratio and navigation guide line relation;If the above feature meets preparatory condition, then it is assumed that obtained landmark region
It is errorless, and extract each connected region barycentric coodinates respectively and try to achieve average coordinates, while performing step 5 backward);Otherwise abandon
The frame image data, and update camera acquisition image be then back to execution step 1);
5) by existing Coordinate Conversion form by step 4) obtain average longitudinal image coordinate and be converted to real ground distance
Information dy, by dyWith the speed progress Kalman filtering read on AGV encoders;
6) by ground distance information dyAs input quantity, it is input in automatic disturbance rejection controller, the AGV that calculating obtains subsequent time is defeated
Go out speed, form a position-force control device;
7) repeat step 1)~6), stopping terrestrial reference in real-time separation and Extraction image simultaneously controls AGV high accuracy positionings to stop, until
Vehicle stops.
2. a kind of AGV stopping a train at a target point methods based on image line walking as claimed in claim 1, it is characterised in that in step 2)
Described in stopping terrestrial reference include following feature:
Stoppingly target color is different from AGV navigation guide lines, and is limited to select among yellow, green, red and blueness,
Setting is stoppingly designated as the wide 5cm of 20cm two long rectangular solid region, and is symmetrically distributed in the both sides of navigation guide line;Together
When, setting stops terrestrial reference and is separated by 15cm with navigation guide line, stops the long side of terrestrial reference and navigation guide line vertical distribution.
3. a kind of AGV stopping a train at a target point methods based on image line walking as claimed in claim 1 or 2, it is characterised in that in step
4) in, described stopping terrestrial reference Region Feature Extraction and decision process is as follows:
4.1) two pieces of maximum regions of area in all connected domains of image are chosen, the area in two pieces of regions is calculated, and by two faces
Product value obtains difference as difference, then take the absolute value of difference divided by the area in two pieces of regions and, obtain result for n, by result n with
One threshold value N set in advance compares, if less than threshold value N, i.e. n < N, then it is assumed that extract correct in landmark region;
If 4.2) meet condition 4.1), the boundary rectangle length-width ratio in two pieces of regions is calculated, by rectangular aspect ratio and practically
The stoppingly target boundary rectangle length and width in face are compared to difference and obtain difference, and take the absolute value m of difference, equally by result m and in advance
The threshold value M first set compares, if less than threshold value M, i.e. m < M, then it is assumed that extract correct in landmark region;
If 4.3) 4.1) all met with condition 4.2), the barycentric coodinates in two pieces of regions are calculated, by its lateral coordinates x1、x2With
The coordinate x of middle leading line0It is y=2*x0-x1-x2Calculating obtains result y, takes y absolute value and threshold value Y ratios set in advance
Compared with if less than threshold value Y, i.e. y < Y, then it is assumed that landmark region extracts correct, two pieces of regions stop terrestrial reference area to be correct
Domain.
4. a kind of AGV stopping a train at a target point methods based on image line walking as claimed in claim 1 or 2, it is characterised in that in step
5) in, the filtering of described Kalman filtering algorithm is as follows:
5.1) state equation of system is initially set up:
<mfenced open = "" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mi>Im</mi>
<mi>g</mi>
<mo>_</mo>
<mi>d</mi>
<msub>
<mi>y</mi>
<mi>k</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>q</mi>
<mo>_</mo>
<msub>
<mi>bias</mi>
<mi>k</mi>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mi>d</mi>
<mi>t</mi>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mi>Im</mi>
<mi>g</mi>
<mo>_</mo>
<mi>d</mi>
<msub>
<mi>y</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>q</mi>
<mo>_</mo>
<msub>
<mi>bias</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>+</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mi>s</mi>
<mi>p</mi>
<mi>e</mi>
<mi>e</mi>
<mi>d</mi>
<mo>_</mo>
<mi>m</mi>
<mo>*</mo>
<mi>d</mi>
<mi>t</mi>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>+</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mi>w</mi>
<mo>_</mo>
<mi>Im</mi>
<mi>g</mi>
<mo>_</mo>
<mi>d</mi>
<mi>y</mi>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>w</mi>
<mo>_</mo>
<mi>s</mi>
<mi>p</mi>
<mi>e</mi>
<mi>e</mi>
<mi>d</mi>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
1
In above formula, Img_dykIt is k moment real ground distance information dy, q_biaskIt is that the speed that k moment encoders are measured is inclined
Difference, dt is update cycle, Img_dyk-1It is the ground distance information d measured in k-1 time chart picturesy, q_biask-1It is the k-1 moment
The speed deviation that encoder is measured, speed_m is the process noise of encoder, w_Im.G_dy and w_speed are image respectively
In the d that measuresyThe measurement noise of velocity amplitude that measures of measurement noise and encoder;
5.2) measurement equation is set up:Img_dy in formulakIt is
K moment real ground distance information dy, q_biaskIt is the speed deviation that encoder is measured, v_Img_dy is ground distance letter
Cease dyWhite noise;
5.3) construction process noise matrix:Wherein Q_Img_dy is ground distance information dy
Process noise, Q_speed is the process noise of velocity amplitude that encoder is measured;
5.4) measurement noise matrix is constructed:[R_Img_dy], wherein R_Img_dy are ground distance information dyMeasurement noise;
5.5) angle is predicted:
<mrow>
<msub>
<mrow>
<mi>Img</mi>
<mo>_</mo>
<mi>dy</mi>
</mrow>
<mi>k</mi>
</msub>
<mo>=</mo>
<msub>
<mrow>
<mi>Img</mi>
<mo>_</mo>
<mi>dy</mi>
</mrow>
<mi>k</mi>
</msub>
<mo>-</mo>
<msub>
<mi>q</mi>
<msub>
<mi>bias</mi>
<mi>k</mi>
</msub>
</msub>
<mo>*</mo>
<mi>dt</mi>
<mo>+</mo>
<mi>soeed</mi>
<mo>_</mo>
<mi>m</mi>
<mo>*</mo>
<mi>dt</mi>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
</mrow>
5.6) variance is predicted:
<mrow>
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>P</mi>
<mrow>
<mi>k</mi>
<mo>|</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mi>P</mi>
<mn>00</mn>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>P</mi>
<mn>01</mn>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>P</mi>
<mn>10</mn>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>P</mi>
<mn>11</mn>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mi>d</mi>
<mi>t</mi>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mi>P</mi>
<mn>00</mn>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>P</mi>
<mn>01</mn>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>P</mi>
<mn>10</mn>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>P</mi>
<mn>11</mn>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mi>d</mi>
<mi>t</mi>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>+</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mi>Q</mi>
<mo>_</mo>
<mi>Im</mi>
<mi>g</mi>
<mo>_</mo>
<mi>d</mi>
<mi>y</mi>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mi>Q</mi>
<mo>_</mo>
<mi>s</mi>
<mi>p</mi>
<mi>e</mi>
<mi>e</mi>
<mi>d</mi>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
</mtd>
</mtr>
</mtable>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>2</mn>
<mo>)</mo>
</mrow>
</mrow>
Wherein, Pk|k-1For the covariance matrix predicted by k-1 moment quantity of state the k moment;
5.7) kalman gain is calculated:
<mrow>
<mtable>
<mtr>
<mtd>
<mrow>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mi>K</mi>
<mo>_</mo>
<mn>0</mn>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>K</mi>
<mo>_</mo>
<mn>1</mn>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mi>P</mi>
<mn>00</mn>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>P</mi>
<mn>01</mn>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>P</mi>
<mn>10</mn>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>P</mi>
<mn>11</mn>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<msup>
<mrow>
<mo>(</mo>
<mrow>
<mo>&lsqb;</mo>
<mtable>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
<mo>&rsqb;</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mi>P</mi>
<mn>00</mn>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>P</mi>
<mn>01</mn>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>P</mi>
<mn>10</mn>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>P</mi>
<mn>11</mn>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>+</mo>
<mi>R</mi>
<mo>_</mo>
<mi>Im</mi>
<mi>g</mi>
<mo>_</mo>
<mi>d</mi>
<mi>y</mi>
</mrow>
<mo>)</mo>
</mrow>
<mrow>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msup>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mi>P</mi>
<mn>00</mn>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>P</mi>
<mn>10</mn>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>/</mo>
<mrow>
<mo>(</mo>
<mi>P</mi>
<mn>00</mn>
<mo>+</mo>
<mi>R</mi>
<mo>_</mo>
<mi>Im</mi>
<mi>g</mi>
<mo>_</mo>
<mi>d</mi>
<mi>y</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
</mtable>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>3</mn>
<mo>)</mo>
</mrow>
</mrow>
Wherein,For kalman gain;
5.8) variance updates:
<mrow>
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>P</mi>
<mrow>
<mi>k</mi>
<mo>|</mo>
<mi>k</mi>
</mrow>
</msub>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mi>P</mi>
<mn>00</mn>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>P</mi>
<mn>01</mn>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>P</mi>
<mn>10</mn>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>P</mi>
<mn>11</mn>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mrow>
<mo>(</mo>
<mi>E</mi>
<mo>-</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mi>K</mi>
<mo>_</mo>
<mn>0</mn>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>K</mi>
<mo>_</mo>
<mn>1</mn>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>&lsqb;</mo>
<mtable>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
<mo>&rsqb;</mo>
<mo>)</mo>
</mrow>
<msub>
<mi>P</mi>
<mrow>
<mi>k</mi>
<mo>|</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mi>P</mi>
<mn>00</mn>
<mo>-</mo>
<mi>K</mi>
<mo>_</mo>
<mn>0</mn>
<mo>*</mo>
<mi>P</mi>
<mn>00</mn>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>P</mi>
<mn>01</mn>
<mo>-</mo>
<mi>K</mi>
<mo>_</mo>
<mn>0</mn>
<mo>*</mo>
<mi>P</mi>
<mn>01</mn>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>P</mi>
<mn>10</mn>
<mo>-</mo>
<mi>K</mi>
<mo>_</mo>
<mn>1</mn>
<mo>*</mo>
<mi>P</mi>
<mn>00</mn>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>P</mi>
<mn>11</mn>
<mo>-</mo>
<mi>K</mi>
<mo>_</mo>
<mn>1</mn>
<mo>*</mo>
<mi>P</mi>
<mn>01</mn>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
</mtd>
</mtr>
</mtable>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>4</mn>
<mo>)</mo>
</mrow>
</mrow>
Wherein, E is unit matrix, Pk|kFor the covariance matrix at k moment;
5.9) state estimation:
<mrow>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mi>Im</mi>
<mi>g</mi>
<mo>_</mo>
<mi>d</mi>
<mi>y</mi>
<mi>k</mi>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>q</mi>
<mo>_</mo>
<msub>
<mi>bias</mi>
<mi>k</mi>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mi>Im</mi>
<mi>g</mi>
<mo>_</mo>
<msub>
<mi>dy</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>q</mi>
<mo>_</mo>
<msub>
<mi>bias</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>+</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mi>K</mi>
<mo>_</mo>
<mn>0</mn>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>K</mi>
<mo>_</mo>
<mn>1</mn>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>*</mo>
<mrow>
<mo>(</mo>
<mi>Im</mi>
<mi>g</mi>
<mo>_</mo>
<msub>
<mi>dy</mi>
<mi>k</mi>
</msub>
<mo>-</mo>
<mi>s</mi>
<mi>p</mi>
<mi>e</mi>
<mi>e</mi>
<mi>d</mi>
<mo>_</mo>
<mi>m</mi>
<mo>*</mo>
<mi>d</mi>
<mi>t</mi>
<mo>)</mo>
</mrow>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>5</mn>
<mo>)</mo>
</mrow>
</mrow>
2
Formula (1)~(5) are computed repeatedly to obtain apart from dyOptimal result.
5. a kind of AGV stopping a train at a target point methods based on image line walking as claimed in claim 1 or 2, it is characterised in that in step
6) in, the automatic disturbance rejection controller includes Nonlinear Tracking Differentiator, extended state observer and nonlinear combination, and Nonlinear Tracking Differentiator is ginseng
Number input transition process arranging, obtains smooth input signal, and extracts its differential signal, and extended state observer uses bilateral
The method transformation object model of road compensation, by non-linear, uncertain system approximation linearisation and certainty, nonlinear combination
Provide the control strategy of controlled device.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201510528410.8A CN105094134B (en) | 2015-08-25 | 2015-08-25 | A kind of AGV stopping a train at a target point methods based on image line walking |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201510528410.8A CN105094134B (en) | 2015-08-25 | 2015-08-25 | A kind of AGV stopping a train at a target point methods based on image line walking |
Publications (2)
Publication Number | Publication Date |
---|---|
CN105094134A CN105094134A (en) | 2015-11-25 |
CN105094134B true CN105094134B (en) | 2017-10-31 |
Family
ID=54574807
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201510528410.8A Expired - Fee Related CN105094134B (en) | 2015-08-25 | 2015-08-25 | A kind of AGV stopping a train at a target point methods based on image line walking |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN105094134B (en) |
Families Citing this family (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105911989B (en) * | 2016-04-28 | 2021-08-13 | 财团法人车辆研究测试中心 | Decision-making system and method for combined type automatic auxiliary driving |
CN106843214B (en) * | 2017-02-13 | 2020-02-21 | 浙江工业大学 | Tape guidance AGV tracking control method based on active disturbance rejection control |
CN107168310A (en) * | 2017-05-11 | 2017-09-15 | 广州市井源机电设备有限公司 | Control device, system and method that a kind of AGV dollies precisely navigate |
CN107516328B (en) * | 2017-08-23 | 2021-02-12 | 山东非凡智能科技有限公司 | AGV working point positioning method and system |
CN108196545B (en) * | 2018-01-03 | 2021-06-25 | 浙江同筑科技有限公司 | AGV magnetic navigation control method adopting active disturbance rejection control technology |
CN108398946B (en) * | 2018-01-25 | 2021-12-21 | 成都图灵时代科技有限公司 | Intelligent tracking accurate positioning device and method |
CN108268044B (en) * | 2018-01-31 | 2021-06-22 | 浙江国自机器人技术股份有限公司 | Mobile robot in-place precision control method, system, medium and equipment |
CN110333716A (en) * | 2019-04-30 | 2019-10-15 | 深圳市商汤科技有限公司 | A kind of motion control method, device and system |
CN110837814B (en) * | 2019-11-12 | 2022-08-19 | 深圳创维数字技术有限公司 | Vehicle navigation method, device and computer readable storage medium |
CN113570657A (en) * | 2020-04-28 | 2021-10-29 | 富华科精密工业(深圳)有限公司 | Fisheye image-based automatic guiding trolley positioning method and computer device |
CN111679292B (en) * | 2020-06-24 | 2023-04-07 | 昆山同日智能技术有限公司 | Relative positioning method for AGV trolley laser navigation |
CN114137978A (en) * | 2021-11-29 | 2022-03-04 | 佛山市毕佳索智能科技有限公司 | Speed planning and control method for storage AGV |
Family Cites Families (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH07281747A (en) * | 1994-04-04 | 1995-10-27 | Toyota Motor Corp | Traveling controller for automated guided vehicle |
CN1438138A (en) * | 2003-03-12 | 2003-08-27 | 吉林大学 | Vision guiding method of automatic guiding vehicle and automatic guiding electric vehicle |
CN103390259A (en) * | 2012-05-09 | 2013-11-13 | 徐世铭 | Ground image processing method in visual guidance AGV |
CN102788591B (en) * | 2012-08-07 | 2015-05-13 | 郭磊 | Visual information-based robot line-walking navigation method along guide line |
CN102997910B (en) * | 2012-10-31 | 2016-04-13 | 上海交通大学 | A kind of based on road of ground surface target location guidance system and method |
-
2015
- 2015-08-25 CN CN201510528410.8A patent/CN105094134B/en not_active Expired - Fee Related
Also Published As
Publication number | Publication date |
---|---|
CN105094134A (en) | 2015-11-25 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN105094134B (en) | A kind of AGV stopping a train at a target point methods based on image line walking | |
CN104282020B (en) | A kind of vehicle speed detection method based on target trajectory | |
CN103150559B (en) | Head recognition and tracking method based on Kinect three-dimensional depth image | |
CN107167826A (en) | The longitudinal direction of car alignment system and method for Image Feature Detection based on variable grid in a kind of automatic Pilot | |
CN101660908B (en) | Visual locating and navigating method based on single signpost | |
CN107272008A (en) | A kind of AGV Laser navigation systems with inertia compensation | |
CN105405154A (en) | Target object tracking method based on color-structure characteristics | |
CN101574586B (en) | Shuttlecock robot and control method thereof | |
CN111681259B (en) | Vehicle tracking model building method based on Anchor mechanism-free detection network | |
CN106871904A (en) | A kind of mobile robot code-disc positioning correction method based on machine vision | |
CN106990781A (en) | Automatic dock AGV localization methods based on laser radar and image information | |
Pauls et al. | Monocular localization in hd maps by combining semantic segmentation and distance transform | |
CN109844457A (en) | For by aerial image or satellite image and by vehicle detection to data generate supplying digital road model system and method | |
CN109813334A (en) | Real-time high-precision vehicle mileage calculation method based on binocular vision | |
CN109059930A (en) | A kind of method for positioning mobile robot of view-based access control model odometer | |
CN105550675A (en) | Binocular pedestrian detection method based on optimization polymerization integration channel | |
CN103077532A (en) | Real-time video object quick tracking method | |
CN105444741A (en) | Double view window based route characteristic identifying, deviation measuring, and accurate positioning method | |
Huang et al. | Vision-based semantic mapping and localization for autonomous indoor parking | |
CN105303518A (en) | Region feature based video inter-frame splicing method | |
CN112254728A (en) | Method for enhancing EKF-SLAM global optimization based on key road sign | |
CN113538585B (en) | High-precision multi-target intelligent identification, positioning and tracking method and system based on unmanned aerial vehicle | |
CN106153037A (en) | The indoor orientation method of a kind of robot, Apparatus and system | |
CN108196545B (en) | AGV magnetic navigation control method adopting active disturbance rejection control technology | |
Launay et al. | A corridors lights based navigation system including path definition using a topologically corrected map for indoor mobile robots |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant | ||
CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20171031 |