CN102788591B - Visual information-based robot line-walking navigation method along guide line - Google Patents

Abstract

The invention discloses a visual information-based robot line-walking navigation method along a guide line, comprising the following steps of: (a) obtaining an environment image by a visual sensor arranged on a robot; (b) obtaining route information from the environment image, and transmitting the obtained route information to a controller; and (c) setting the linear velocity and the angular velocity of the robot by the controller according to the route information. According to the method, the robot can walk along an appointed route.

Description

The Robot guide line line walking air navigation aid of view-based access control model information

Technical field

The present invention relates to a kind of air navigation aid, particularly a kind of Robot guide line line walking air navigation aid of view-based access control model information.

Background technology

Airmanship is the basis that removable smart machine completes a lot of sophisticated functions, has a wide range of applications in fields such as commercial production, life, national defence.Such as, in the workplace of bad environments, can be used for guided robot and complete hot mission; In the occasion such as workshop, harbour, can be used for transport task; At field of traffic, can be used for realizing adaptive cruise etc.

At present conventional navigation means has following several: the hybrid navigation of the navigation based on fixed routes such as tracks, the navigation based on range radar, the navigation based on inertial gyroscope, the navigation based on GPS and various method.Above method has respective relative merits: the Navigation Control based on fixed route is got up simply, but needs to lay track or solenoid circuit, very inconvenient when installing and change; Navigation based on range radar is comparatively large by the impact of surrounding environment change, and needs to arrange a large amount of reflective sigh; Based on the navigation simple and flexible of inertial gyroscope, but deviation accumulation can occur by the impact of various interference, long-time running cannot be able to work because of the too large youngster of error; Navigation based on GPS is easy to use, and degree of freedom is large, but due to limited precision, is not suitable for the precision navigation of regional area.

In recent years, along with popularizing of high quality and low cost vision sensor and improving constantly of robot processing power, vision sensor has become the vitals of robot perception external environment condition; This makes to realize navigation by vision becomes possibility.But, also do not occur navigation product and the method for view-based access control model information in the prior art, and the navigation of view-based access control model has very wide application prospect.Those need the place realizing high precision navigation in the region that space is little, environment is more complicated, path is fixed, and such as factory, harbour, station etc. have needed the place of amount transport task, and the navigation of view-based access control model can play very important effect.

Summary of the invention

For the deficiencies in the prior art, the object of the present invention is to provide a kind of Robot guide line line walking air navigation aid of the view-based access control model information that Robot given line can be made to walk.

Technical scheme of the present invention is achieved in that the Robot guide line line walking air navigation aid of view-based access control model information, comprises the steps: that (a) obtains ambient image by the vision sensor that robot is installed; B () be acquisition approach information from ambient image, and the routing information of acquisition is transferred to controller; C () controller is according to the linear velocity of path information setting robot and angular velocity.

The invention has the beneficial effects as follows: the present invention only need draw out guide line on the ground can realize navigation, does not need the support of the software and hardwares such as track, gyroscope, GPS or electronic chart, install and change all very convenient.Positioning precision can reach centimetre-sized easily.And fix due to object of reference, avoid the cumulative errors introduced because of various external factor.The technology of binding pattern identification, can make robot automatically perform corresponding actions after specific identifier being detected, so greatly expand the range of application of vision guide line.

The present invention can be applied in those places needing to realize high precision navigation in the region that space is little, environment is more complicated, path is fixed, and such as factory, harbour, station etc. have needed the place of amount transport task.

Simulation result of the present invention can sufficient proof, as long as parameter is suitable, the controller using parallel distribution compensation fuzzy control algorithm design to realize, can obtain good control effects.Even if when control cycle is long, this controller still can control effectively to robot, and whole process is very steady.

Accompanying drawing explanation

Fig. 1 is ambient image;

Ambient image is mapped to the image after HSV space by Fig. 2;

Fig. 3 does the image after binaryzation to H territory;

Fig. 4 does the image after medium filtering to the image after binaryzation;

Image after Fig. 5 processes further to the image after medium filtering;

The process schematic of Fig. 6 route searching;

Result after Fig. 7 route searching;

Fig. 8 makes the setting value of linear velocity record actual linear velocity response by low frequency change;

Fig. 9 makes the setting value of linear velocity record actual linear velocity response by high-frequency change;

Figure 10 makes the setting value of angular velocity record actual linear velocity response by low frequency change;

Figure 11 makes the setting value of angular velocity record actual linear velocity response by high-frequency change;

The response curve that parameter after Figure 12 adopts initialization obtains;

A kind of ideal step response of Figure 13;

Figure 14 adopts the emulation route [solid line is the guide line preset, and the dotted line of round dot composition represents emulation route] obtained under first group of parameter (V=1m/s, delta_t=16.67ms, delta_w=10 °/s) during the pid algorithm of preset parameter;

Figure 15 adopts the emulation route [solid line is the guide line preset, and the dotted line of round dot composition represents emulation route] obtained under second group of parameter (V=3m/s, delta_t=16.67ms, delta_w=10 °/s) during the pid algorithm of preset parameter;

Figure 16 adopts the emulation route [solid line is the guide line preset, and the dotted line of round dot composition represents emulation route] obtained under the 3rd group of parameter (V=1m/s delta_t=100ms, delta_w=10 °/s) during the pid algorithm of preset parameter;

Figure 17 is the angular velocity curve that Figure 14 is corresponding;

Figure 18 is the angular velocity curve that Figure 15 is corresponding;

Figure 19 is the angular velocity curve that Figure 16 is corresponding;

At first group of parameter (v=1m/s when Figure 20 adopts FUZZY ALGORITHMS FOR CONTROL based on parallel distribution compensation, delta_t=16.67m, delta_w=10 °/s) under obtain emulation route [solid line is the guide line preset, round dot composition dotted line represent emulation route];

At second group of parameter (v=1m/s when Figure 21 adopts FUZZY ALGORITHMS FOR CONTROL based on parallel distribution compensation, delta_t=100ms, delta_w=10 °/s) under obtain emulation route [solid line is the guide line preset, round dot composition dotted line represent emulation route];

Figure 22 is the angular velocity curve that Figure 20 is corresponding;

Figure 23 is the angular velocity curve that Figure 21 is corresponding;

Figure 24 is the structured flowchart of control algolithm.

1-guide line, 2-road surface, 3-Pioneer3-AT type mobile robot, 4-vision sensor obtains the scope of ambient image.

Embodiment

The present embodiment adopts the Pioneer3-AT type mobile robot of MobileRobots company, and the machine people of this model is four-wheel drive, turns to mechanism similar to creeper truck; Be furnished with the sensors such as the varifocal camera of CCD, sonar ring, laser radar, gyroscope, and possess vehicle-mounted computer, dominant frequency 1.5GHz.The operating system that vehicle-mounted computer uses is Windows XP, belongs to multitask non-real time operating system.User is undertaken alternately by AIRA (ActivMedia RoboticsInterface for Application) application programming interfaces and robot, such as setting speed, reading state etc.The OO C++ of AIRA writes, and supports multithreading.User both by bottom order Direct Drive Robot, also can realize more senior function by loading self-defining task.Guide line is yellow solid-line curve or white solid-line curve that road draws.

The Robot guide line line walking air navigation aid of the present embodiment view-based access control model information, comprises the steps:

A () obtains ambient image by the vision sensor that robot is installed;

B () be acquisition approach information from ambient image, and the routing information of acquisition is transferred to controller;

First the partitioning algorithm based on minimum spanning tree is adopted to extract path from ambient image, step is as follows: first ambient image (as Fig. 1) is mapped to HSV space (as Fig. 2), further process is done: because the background of experimental situation is more complicated in H territory, a lot of tone and the close assorted point of guide line is had near guide line, if directly carry out Iamge Segmentation in H territory, the diffusion phenomena in region can be very serious; First according to the tone value of guide line, binaryzation (as Fig. 3) is done to H territory, again medium filtering (as Fig. 4) is done to the image of binaryzation afterwards, finally adopt quick figure partitioning algorithm based on minimum spanning tree by region disconnecting corresponding for guide line; Add up the stain number of the regional after separation, region stain ratio being greater than certain threshold value is chosen to be region corresponding to guide line, and dyed black, final result as shown in Figure 5;

Then employing path search algorithm obtains the path point in ambient image, and the broken line being break in order to these path point represents path; Can operand be reduced like this, strengthen antijamming capability simultaneously; Step is as follows: the first step: the sequence number num of point is set to 1; Second step: line by line scan from nearest a line, when detecting that width meets the black region of guide line width range in certain a line, using the path point that the mid point in this region searches as first, records position P (num); The prediction direction pre_th (num) of this point is set to 90 degree, and state state (num) is designated as 1, enters second step afterwards; If do not search the black region that width meets guide line width range, then repeat the first step; 3rd step: judge whether next point likely exceeds image range according to current path point P (num); If exceed, turn to the 3rd step, if do not exceed, then with P (num) for the center of circle, with certain length in pixels for radius, search in the scope of each certain angle in pre-measuring angle pre_th (num) left and right; If search the black region that width meets guide line width range, then the mid point of black region is designated as next path point P (num+1); If search multiple path point, select the path point closest to prediction direction pre_th (num), using the pre-measuring angle of the deflection of P (num) to P (num+1) as the n-th um+1 path point, and the state of state (num+1) is designated as 1; If do not search path point, using the Searching point in prediction direction as the n-th um+1 path point, its state state (num+1) is designated as 0, its prediction direction pre_th (num+1) is designated as pre_th (num); Repeat the 3rd step; 4th step: search for complete; As shown in Figure 6, the result after route searching as shown in Figure 7 for the process of route searching.

Finally utilizing three rank perspective transformation matrixs to obtain geographic position corresponding to each path point: path point to be mapped to robot center for initial point, is longitudinally in the earth axes of Y-axis; If any one path point P coordinate is in the picture (x, y), the coordinate in geographic coordinate system is (u, v), then there is matrix mat and make all there is corresponding t for arbitrary P and meet:

(u*t,v*t,t) ^T=mat*(x,y,1) ^T（2-1）

T is transpose of a matrix in linear algebra, is a known mathematical operator; In order to determine perspective transformation matrix mat, four calibration points can be selected from image, recording corresponding geographic coordinate respectively; Bring these four calibration points into matrix equation (2-1), form a Matrix division, can mat be obtained by this Matrix division; After mat matrix is determined, for each path point P (num) searched out in the picture, corresponding geographic coordinate GP (num) can be obtained with formula (2-1); Represented by path in the ambient image broken line that several discrete points are formed, if search point_num point altogether, the geographic coordinate that these points are corresponding is GP (num), and state is state (num).

Through actual test, the path extraction of the present embodiment and method for expressing still can reliably workings in background complexity and when being subject to various interference, separated by guide line exactly.When photo resolution is 160*120, when CPU frequency is 1.5GHz, the working time (including the whole process from image acquisition to path expression) of whole algorithm is about 200ms.Compared with the control cycle of robot 100ms, the working time of this algorithm can accept.In order to solve frequency acquisition and the nonsynchronous problem of robot controlling frequency of routing information, the routing information that a certain moment obtains is used for the several control cycle of continuous print, can estimate current routing information according to the state change of robot in each control cycle, calculate controlled quentity controlled variable by this estimated value.

C () controller is according to the linear velocity of path information setting robot and angular velocity.

The motion model of robot:

The steering mechanism of Pioneer3-AT type mobile robot is consistent with track vehicle, and namely the wheel velocity on left side and right side is equal respectively.When the speed of both sides wheel remains unchanged, the movement locus of robot is straight line or circular arc.This mobile robot can be simplified to two-wheeled motion model, and the axis of two-wheeled is in the centre of robot front and back wheel axis.

In order to test machine people is to the dynamic response characteristic of linear velocity and angular velocity setting value, the present embodiment has done following experiment: make the setting value of linear velocity by low frequency and high-frequency vary in two ways, record actual linear velocity response as shown in Figure 8 and Figure 9: from above result, if the setting value of linear velocity and actual value difference larger, then the correction of first control cycle is less, with constant maximal rate, linear velocity is revised from second control cycle, when setting value is less than threshold value to actual value difference, the dynamic response of linear velocity is similar to one order inertia system.

Make the setting value of angular velocity by low frequency and high-frequency vary in two ways, record actual linear velocity response as shown in Figure 10 and Figure 11: from above result, similar with the dynamic response of linear velocity, if angular velocity setting value and actual value difference larger, then the correction of first control cycle is less, revise angular velocity with constant maximal rate from second control cycle, when setting value is less than threshold value to actual value difference, the dynamic response of angular velocity is similar to one order inertia system.

Approximate discrete model is set up according to conclusion above, as follows:

$V=\frac{\mathrm{Vl}+\mathrm{Vr}}{2}---(3-1)$

$W=\frac{\mathrm{Vr}-\mathrm{Vl}}{\mathrm{width}}---(3-2)$

$x(t+1)=x\left(t\right)+V\left(t\right)*\mathrm{delta}\_t*\cos (\mathrm{angle}\left(t\right)+\frac{W\left(t\right)*\mathrm{delta}\_t}{2})---(3-3)$

$y(t+1)=y\left(t\right)+V\left(t\right)*\mathrm{delta}\_t*\sin (\mathrm{angle}\left(t\right)+\frac{W\left(t\right)*\mathrm{delta}\_t}{2})---(3-4)$

$\left\{\begin{array}{cc}V(t+1)=v\_\mathrm{para}*V\_\mathrm{set}\left(t\right)+V\left(t\right)& \left(\right|V\_\mathrm{set}\left(t\right)-V\left(t\right)|<\mathrm{delta}\_v)(3-5)\\ V(t+1)=V\left(t\right)+\mathrm{delta}\_v& (V\_\mathrm{set}\left(t\right)-V\left(t\right)>\mathrm{delta}\_v)(3-6)\\ V(t+1)=V\left(t\right)-\mathrm{delta}\_v& (V\_\mathrm{set}\left(t\right)-V\left(t\right)<-\mathrm{delta}\_v)(3-7)\end{array}\right.$

$\left\{\begin{array}{cc}W(t+1)=w\_\mathrm{para}*W\_\mathrm{set}\left(t\right)+W\left(t\right)& \left(\right|W\_\mathrm{set}\left(t\right)-W\left(t\right)|<\mathrm{delta}\_w)(3-8)\\ W(t+1)=W\left(t\right)+\mathrm{delta}\_w& (W\_\mathrm{set}\left(t\right)-W\left(t\right)>\mathrm{delta}\_w)(3-9)\\ W(t+1)=W\left(t\right)-\mathrm{delta}\_w& (W\_\mathrm{set}\left(t\right)-W\left(t\right)<-\mathrm{delta}\_w)(3-10)\end{array}\right.$

Wherein V is linear velocity, and Vl, Vr are respectively left and right speed of taking turns, and v_para is the wheel line velocity coefficient relevant with robot motion's system inertia.W is angular velocity, the coordinate that x (t), y (t) are robot, the deflection (with x-axis positive dirction for start line, being just counterclockwise) that angle (t) is robot, w_para is the angular speed of wheel coefficient relevant with robot motion's system inertia.V_set and W_set is respectively the setting value of linear velocity and angular velocity.

Can be obtained drawing a conclusion by analysis above: in the ideal case, if position is determined, the movement locus of robot in future time instance is determined by V and W completely, therefore realizes by control V and W the control of moving of car state.Because V and W changes not quite in the control cycle of 100ms, can think that robot moves with constant V and W in a control cycle, track is circular arc.

In addition, when the setting value of V and W and actual value difference larger time, the actual value of V and W approaches setting value with constant speed respectively, when the setting value of V and W and actual value difference less time, the similar inertia system of response of actual value.When the load of robot changes in rated range, the change of systematic parameter is less, can ignore.From experimental data, the maximum pace of change of linear velocity is 300mm/s, and the maximum pace of change of angular velocity is 100 °/s, and inertia is larger.Meanwhile, there is uncertain time lag in the kinematic system of robot, time lag on the one hand from the command response system with robot itself, the sequential that another aspect causes from multi-thread environment uncertain.

First the speed control algorithm relevant to path flexibility is adopted to control [to consider the main task Shi Shi robot line walking of vision guide to robot speed's size, less demanding to speed], method is as follows: connecting GP (num) with the angle of x-axis positive dirction in the broken line of GP (num+1) and geographic coordinate system is Gth (num), then degree of crook can be expressed as:

$W={\mathrm{\&Sigma;}}_{i=1}^n\left|\mathrm{\&theta;i}\right|---(3-11)$

Wherein θ 1=Gth (1), θ i=Gth (i)-Gth (i-1) (i >=2); According to W speed setting value is divided into stop, low, neutralize high four gears;

Then controller adopts the FUZZY ALGORITHMS FOR CONTROL based on parallel distribution compensation to control robot speed direction again: in each control cycle, chooses a bit on the guide line of front, and obtains the tangent line at this some place; Calculate according to FUZZY ALGORITHMS FOR CONTROL the controlled quentity controlled variable that robot of sening as an envoy to approaches this tangent line as early as possible, thus reach the object making the steady line walking of robot; Robot belongs to nonlinear system, can be similar to the weighting of two linear systems; Utilize Linear-Quadratic Problem method to these two linear system devise optimum controllers respectively, the weighting of these two optimal controllers is exactly final controller; Specifically comprise the steps:

Step 1: the differential degree cur_vlr choosing robot movement, the angle l_angle of robot car body and tangent line and the distance l_y of robot car body center and tangent line tri-parameters are as quantity of state; Linear-Quadratic Problem method is utilized to take out the state equation of robot movement; Its concrete implementation is as follows:

(1a) the dynamic response model one order inertia model of Schemes of Angular Velocity Estimation for Robots is similar to;

(1b) choose the differential degree cur_vlr of robot movement, the angle l_angle of robot car body and tangent line and the distance l_y of robot car body center and tangent line tri-parameters are as quantity of state;

(1c) Linear-Quadratic Problem method is utilized to take out the state equation of robot movement, as shown in formula (3-16) to (3-18):

cur_vlr(t+1)=v_para*u(t)+(1-v_para)*cur_vlr(t) (3-16)

$l\_\mathrm{angle}(t+1)=l\_\mathrm{angle}\left(t\right)-\frac{\mathrm{cur}\_\mathrm{vlr}\left(t\right)}{\mathrm{width}}*\mathrm{delta}\_t---(3-17)$

$l\_y(t+1)=l\_y\left(t\right)+v*\mathrm{delta}\_t*\sin (l-\mathrm{angle}\left(t\right)-\frac{\mathrm{cur}\_\mathrm{vlr}\left(t\right)*\mathrm{delta}\_t}{2*\mathrm{width}})---(3-18)$

Wherein, cur_vlr is the differential degree of left and right wheels, and v is the linear velocity of robot, and width is width of the carbody, and delta_t is control cycle;

Step 2: the motion model due to robot is nonlinear, comprises sin function; According to the state equation of robot movement, take out two state space linear models simplified and be similar to; Its concrete implementation is as follows:

(2a) because the motion model of robot comprises sin function, so be nonlinear;

If $p\left(t\right)=l\_\mathrm{angle}\left(t\right)-\frac{\mathrm{cur}\_\mathrm{vlr}\left(t\right)*\mathrm{delta}\_t}{2*\mathrm{width}}---(3-19);$

(2b) when p (t) is near zero, linear model is as shown in formula (3-20):

x(t+1)=A1*x(t)+B1*u(t) (3-20)；

(2c) when p (t) is near positive and negative pi, linear model is as shown in formula (3-21):

x(t+1)=A2*x(t)+B2*u(t) (3-21)；

(2d) formula (3-20) is with formula (3-21):

$A1=\left(\begin{array}{ccc}1-\mathrm{speed}\_\mathrm{para}& 0& 0\\ -\frac{\mathrm{delta}\_t}{\mathrm{width}}& 1& 0\\ -\frac{V*\mathrm{delta}\_t^2}{2*\mathrm{width}}& V*\mathrm{delta}\_t& 1\end{array}\right)$

B1=(speed_para 0 0)

$A2=\left(\begin{array}{ccc}1-\mathrm{speed}\_\mathrm{para}& 0& 0\\ -\frac{\mathrm{delta}\_t}{\mathrm{width}}& 1& 0\\ -\frac{g*V*\mathrm{delta}\_t^2}{2*\mathrm{width}}& g*V*\mathrm{delta}\_t& 1\end{array}\right)$

B2=(speed_para 0 0)

g=0.01/pi

The implication of Speed_para: Speed_para is the overall wire velocity coefficient relevant with robot motion's system inertia.

(2e) set w1 (p (t)), w2 (p (t)) as p (t) is for the membership function of two linear models, as shown in formula (3-22), formula (3-23):

$w1\left(p\left(t\right)\right)=\frac{\sin \left(p\left(t\right)\right)-g*p\left(t\right)}{p\left(t\right)*(1-g)}---(3-22)$

$w2\left(p\left(t\right)\right)=\frac{\left(p\left(t\right)\right)-\sin \left(p\left(t\right)\right)}{p\left(t\right)*(1-g)}---(3-23)$

Then fuzzy model is as shown in formula (3-24):

x(t+1)=A(p(t))*x(t)+B(p(t))*u(t) （3-24）

Wherein:

$A\left(p\left(t\right)\right)=\frac{w1\left(p\left(t\right)\right)*A1+w2\left(p\left(t\right)\right)*A2}{w1\left(p\left(t\right)\right)+w2\left(p\left(t\right)\right)}---(3-25)$

$B\left(p\left(t\right)\right)=\frac{w1\left(p\left(t\right)\right)*B1+w2\left(p\left(t\right)\right)*B2}{w1\left(p\left(t\right)\right)+w2\left(p\left(t\right)\right)}---(3-26)$

As-179.4270<p (t) <179.4270, this fuzzy model illustrates nonlinear model accurately;

Step 3: use Linear-Quadratic Problem method design optimal controller respectively for two linear models, its concrete implementation is as follows:

(3a) use Linear-Quadratic Problem method respectively for two linear models, obtain control rate as shown in formula (3-27), formula (3-28):

When p (t) is near zero,

u(t)=f1*x(t) (3-27)

When p (t) is near positive and negative pi

u(t)=f2*x(t) (3-28)；

(3b) final controlled quentity controlled variable is as shown in formula (3-30):

$u\left(t\right)=\frac{w1\left(p\left(t\right)\right)*f1+w2\left(p\left(t\right)\right)*f2}{w1\left(p\left(t\right)\right)+w2\left(p\left(t\right)\right)}*x\left(t\right)---(3-29)$

$W\left(t\right)=-\frac{u\left(t\right)}{\mathrm{width}}---(3-30);$

Step 4: when with Linear-Quadratic Problem method design optimal controller, need given weighting matrices Q and R, solve by off-line the parameter that the Riccati differential equation obtains controller; Q and R is very large on the impact of control effects; In order to improve the efficiency of setting parameter, determined the approximate range of parameter by emulation, then by actual test, fine setting is made to parameter; Its concrete implementation is as follows:

(4a) in MATLAB, establish the response model of each state diagonal angle speed setting value of a robot when constant airspeed; Q and R has reacted the differential degree cur_vlr of robot movement respectively, the angle l_angle of robot car body and tangent line, the distance l_y of robot car body center and tangent line and controlled quentity controlled variable, to the contribution of Control platform, therefore can determine the adjustment direction of parameter according to the response results of robot under given original state.Such as, the parameter after adopting initialization can obtain the response curve shown in Figure 12.The speed restrained by the known position deviation of result is comparatively slow, can suitably increase the weights that in Q matrix, l_y is corresponding, continues test.The overall principle is, which state convergence is comparatively slow, just suitably increases this state weights corresponding in Q matrix, until response index reaches requirement.Figure 13 shows a kind of ideal step response, can find out that the position deviation of robot is tending towards 0 very soon, and whole process is more stable.Now corresponding parameter is accurate Optimal Parameters corresponding to this speed.

It is to be noted: for the ease of calculating the Riccati differential equation, needing supposition linear velocity to be steady state value.When change of line speed, the optimal value of the parameter of controller also can change.In order to address this problem, need the optimal controller parameter that several linear velocities of calculated off-line are corresponding, estimating parameter corresponding to other linear velocities by data fitting.This method workload is comparatively large, but can reach best control effects.The present embodiment, in order to complexity of equalization and control performance, is not adopted in this way.

(4b) for the ease of the validity of verification algorithm, simultaneously for setting parameter provides convenient, MATLAB GUI is used to build a simulated environment: the parameters value in this environment real-time display device people motion process, can arbitrarily suspend, return Last status, and can show historical data and analyze;

(4c) emulation experiment is carried out:

The experimental result of preset parameter pid algorithm is as shown in Figure 14 to Figure 19:

The emulation route that Figure 14 to Figure 16 obtains respectively when being and adopting the pid algorithm of preset parameter under 3 groups of parameters, Figure 17 to Figure 19 is corresponding angular velocity curve, and wherein horizontal ordinate is control cycle number, ordinate is angular velocity.Often organizing under parameter, more suitable value of all pid parameter having been adjusted to.Result shows, when control cycle is shorter, the method is more effective.And when control cycle is longer, even if adjustment parameter, control effects is still very poor.Standard control cycle due to robot is 100ms, and the method is unavailable in theory.

Based on the experimental result of the fuzzy algorithm of parallel distribution compensation as shown in Figure 20 to Figure 23:

The emulation route that Figure 20 and Figure 21 obtains respectively when being and adopting the FUZZY ALGORITHMS FOR CONTROL based on parallel distribution compensation under 2 groups of parameters, Figure 22 and Figure 23 is corresponding angular velocity curve.

Simulation result can sufficient proof, as long as parameter is suitable, this algorithm just can obtain good control effects; Even if when control cycle is long, this algorithm still can control effectively to robot, and whole process is very steady;

Can obtain as drawn a conclusion by the result of emulation experiment:

(1) the pid algorithm control effects of preset parameter is very limited, should not adopt.

(2) fuzzy algorithm based on parallel distribution compensation can obtain good control effects.

Step 5: make fine setting by reality test to parameter, its concrete implementation is as follows:

(5a) emulated data that (4c) obtains is applied in true experiment;

(5b) according to truly testing, the value of adjustment individual parameters;

(5c) step (5a) ~ (5b) is repeated, until reach Optimal Control effect.

Preset parameter PID method and the fuzzy algorithm experimental result based on parallel distribution compensation as follows:

(1) pid algorithm of preset parameter

The pid algorithm of preset parameter is effective when speed very low (about 0.3m/s), will be unstable when speed is slightly high; And adjusting of parameter is cumbersome.

(2) based on the fuzzy algorithm of parallel distribution compensation

When adopting the fuzzy algorithm based on parallel distribution compensation, need to adjust comparatively multiparameter, need to do more experiment to the determination of controller parameter.In order to raise the efficiency, first can determine accurate Optimal Parameters in the state-space model of MATLAB emulation, then by actual test, parameter be finely tuned.For the ease of comparing, unified by parallel with guide line for the longitudinal direction of robot initial time in reality test, center and the guide line of robot depart from 30cm.According to the transit time experienced before robot stabilized line walking and oscillation degree evaluation control quality, and corresponding adjustment is made to parameter.

Under parameter comparatively reasonably situation, robot can be transitioned into line walking state fast and stable, and whole process is not shaken.When the actual linear velocity of robot is close to the ideal line speed that parameter is corresponding, the control effects of this algorithm is optimum.But when the perfect condition deviation corresponding with parameter when the actual linear velocity of robot is larger, the effect of this algorithm can degradation.In order to address this problem, need for low, neutralize high three gears and measure controling parameters respectively, obtain controling parameters corresponding to each gear speed by the method for data fitting.

The pid algorithm of preset parameter: the state current according to robot selects a line (i.e. certain a line in robot front) from image, using the reference point of guide line in the position of this row as robot motion.Using the difference E of this point and row mid point as deviation, by increment PI D formulae discovery angular velocity setting value.Computing formula is as shown in (3-12):

W_set(t+1)=W_set(t)+Kp*(E(t)-E(t-1))+Ki*E(t)+Kd*(E(t)+E(t-2)-2*E(t-1))（3-12）

The maximum feature of this algorithm is the model not needing to know controlled device, and computing is simple, and shortcoming is that control effects places one's entire reliance upon parameter, and when the state (as linear velocity, angular velocity, forward path) of controlled device is different, optimal parameter is often different.For control objectives Shi Shi robot line walking when speed is not high, do not have too high request to Control platform, preset parameter pid algorithm is applicable equally.

Claims (2)

1. the Robot guide line line walking air navigation aid of view-based access control model information, is characterized in that, comprise the steps:

A () obtains ambient image by the vision sensor that robot is installed;

B () be acquisition approach information from ambient image, and the routing information of acquisition is transferred to controller;

C () controller is according to the linear velocity of path information setting robot and angular velocity;

In step (b): first adopt the partitioning algorithm based on minimum spanning tree to extract path from ambient image, then employing path search algorithm obtains the path point in ambient image, finally utilizes three rank perspective transformation matrixs to obtain geographic position corresponding to each path point;

In step (c): first controller adopts the speed control algorithm relevant to path flexibility to control robot speed's size, then controller adopts the pid algorithm of preset parameter again or controls robot speed direction based on the FUZZY ALGORITHMS FOR CONTROL of parallel distribution compensation;

In step (b):

Adopt that from ambient image, to extract the step in path based on the partitioning algorithm of minimum spanning tree as follows: first ambient image is mapped to HSV space, further process is done: first according to the tone value of guide line, binaryzation is done to H territory in H territory, again medium filtering is done to the image of binaryzation afterwards, finally adopt quick figure partitioning algorithm based on minimum spanning tree by region disconnecting corresponding for guide line; Add up the stain number of the regional after separation, region stain ratio being greater than certain threshold value is chosen to be region corresponding to guide line, is dyed black;

Employing path search algorithm obtains the path point in ambient image, and the broken line being break in order to these path point represents path, and step is as follows: the first step: the sequence number num of point is set to 1; Second step: line by line scan from nearest a line, when detecting that width meets the black region of guide line width range in certain a line, using the path point that the mid point in this region searches as first, records position P (num); The prediction direction pre_th (num) of this point is set to 90 degree, and state state (num) is designated as 1, enters second step afterwards; If do not search the black region that width meets guide line width range, then repeat the first step; 3rd step: judge whether next point likely exceeds image range according to current path point P (num); If exceed, turn to the 3rd step, if do not exceed, then with P (num) for the center of circle, with certain length in pixels for radius, search in pre-measuring angle pre_th (num); If search the black region that width meets guide line width range, then the mid point of black region is designated as next path point P (num+1); If search multiple path point, select the path point closest to prediction direction pre_th (num), using the pre-measuring angle of the deflection of P (num) to P (num+1) as the n-th um+1 path point, and the state of state (num+1) is designated as 1; If do not search path point, using the Searching point in prediction direction as the n-th um+1 path point, its state state (num+1) is designated as 0, its prediction direction pre_th (num+1) is designated as pre_th (num); Repeat the 3rd step; 4th step: search for complete;

Utilizing three rank perspective transformation matrixs to obtain geographic position corresponding to each path point: path point to be mapped to robot center for initial point, is longitudinally in the earth axes of Y-axis; If any one path point P coordinate is in the picture (x, y), the coordinate in geographic coordinate system is (u, v), then there is matrix mat and make all there is corresponding t for arbitrary P and meet:

(u*t,v*t,t) ^T＝mat*(x,y,1) ^T(2-1)，

T is transpose of a matrix in linear algebra, is a known mathematical operator; In order to determine perspective transformation matrix mat, from image, selecting four calibration points, record corresponding geographic coordinate respectively; Bring these four calibration points into matrix equation (2-1), form a Matrix division, can mat be obtained by this Matrix division; After mat matrix is determined, for each path point P (num) searched out in the picture, corresponding geographic coordinate GP (num) can be obtained with formula (2-1); Represented by path in the ambient image broken line that several discrete points are formed, if search point_num point altogether, the geographic coordinate that these points are corresponding is GP (num), and state is state (num).

2. the Robot guide line line walking air navigation aid of view-based access control model information according to claim 1, it is characterized in that, in step (c): the method adopting the speed control algorithm relevant to path flexibility to control robot speed's size is as follows: the angle connecting x-axis positive dirction in the broken line of GP (num) and GP (num+1) and geographic coordinate system is Gth (num), then degree of crook is expressed as:

$W={\mathrm{\&Sigma;}}_{i=1}^n\left|\mathrm{\&theta;i}\right|---(3-11)$

Wherein θ 1=Gth (1), θ i=Gth (i)-Gth (i-1), i >=2; W is angular velocity, according to W speed setting value is divided into stop, low, neutralize high four gears;

Then controller adopts the FUZZY ALGORITHMS FOR CONTROL based on parallel distribution compensation to control robot speed direction again: in each control cycle, chooses a bit on the guide line of front, and obtains the tangent line at this some place; Calculate according to FUZZY ALGORITHMS FOR CONTROL the controlled quentity controlled variable that robot of sening as an envoy to approaches this tangent line as early as possible, thus reach the object making the steady line walking of robot; Robot belongs to nonlinear system, is similar to the weighting of two linear systems; Utilize Linear-Quadratic Problem method to these two linear system devise optimum controllers respectively, the weighting of these two optimal controllers is exactly final controller; Specifically comprise the steps:

Step 1: the differential degree cur_vlr choosing robot movement, the angle l_angle of robot car body and tangent line and the distance l_y of robot car body center and tangent line tri-parameters are as quantity of state; Linear-Quadratic Problem method is utilized to take out the state equation of robot movement; Its concrete implementation is as follows:

(1a) the dynamic response model one order inertia model of Schemes of Angular Velocity Estimation for Robots is similar to;

(1b) choose the differential degree cur_vlr of robot movement, the angle l_angle of robot car body and tangent line and the distance l_y of robot car body center and tangent line tri-parameters are as quantity of state;

(1c) Linear-Quadratic Problem method is utilized to take out the state equation of robot movement, as shown in formula (3-16) to (3-18):

cur_vlr(t+1)＝v_para*u(t)+(1-v_para)*cur_vlr(t) (3-16)

$l\_\mathrm{angle}(t+1)=l\_\mathrm{angle}\left(t\right)-\frac{\mathrm{cur}\_\mathrm{vlr}\left(t\right)}{\mathrm{width}}*\mathrm{delta}\_t---(3-17)$

$l\_y(t+1)=l\_y\left(t\right)+v*\mathrm{delta}\_t*\sin (l\_\mathrm{angle}\left(t\right)-\frac{\mathrm{cur}\_\mathrm{vlr}\left(t\right)*\mathrm{delta}\_t}{2*\mathrm{width}})---(3-18)$

Wherein, v_para is the coefficient relevant with robot motion's system inertia, and cur_vlr is the differential degree of left and right wheels, and v is the linear velocity of robot, and width is width of the carbody, and delta_t is control cycle;

Step 2: the motion model due to robot is nonlinear, comprises sin function; According to the state equation of robot movement, take out two state space linear models simplified and be similar to; Its concrete implementation is as follows:

(2a) because the motion model of robot comprises sin function, so be nonlinear;

If $p\left(t\right)=l\_\mathrm{angle}\left(t\right)-\frac{\mathrm{cur}\_\mathrm{vlr}\left(t\right)*\mathrm{delta}\_t}{2*\mathrm{width}}---(3-19);$

(2b) when p (t) is near zero, linear model is as shown in formula (3-20):

x(t+1)＝A1*x(t)+B1*u(t) (3-20)；

(2c) when p (t) is near positive and negative pi, linear model is as shown in formula (3-21):

x(t+1)＝A2*x(t)+B2*u(t) (3-21)；

(2d) formula (3-20) is with (3-21):

$\text{A1=}\left(\begin{array}{ccc}1-\mathrm{speed}\_\mathrm{para}& 0& 0\\ -\frac{\mathrm{delta}\_t}{\mathrm{width}}& 1& 0\\ -\frac{V*\mathrm{delta}\_t^2}{2*\mathrm{width}}& V*\mathrm{delta}\_t& 1\end{array}\right)$

B1＝(speed_para 0 0)

$A2=\left(\begin{array}{ccc}1-\mathrm{speed}\_\mathrm{para}& 0& 0\\ -\frac{\mathrm{delta}\_t}{\mathrm{width}}& 1& 0\\ -\frac{g*V*\mathrm{delta}\_t^2}{2*\mathrm{width}}& g*V*\mathrm{delta}\_t& 1\end{array}\right)$

B2＝(speed_para 0 0)

g＝0.01/pi

(2e) set w1 (p (t)), w2 (p (t)) as p (t) is for the membership function of two linear models, as shown in formula (3-22), formula (3-23):

$w1\left(p\left(t\right)\right)=\frac{\sin \left(p\left(t\right)\right)-g*p\left(t\right)}{p\left(t\right)*(1-g)}---(3-22)$

$w2\left(p\left(t\right)\right)=\frac{p\left(\text{t}\right)-\sin \left(p\left(t\right)\right)}{p\left(t\right)*(1-g)}---(3-23)$

Then fuzzy model is as shown in formula (3-24):

x(t+1)＝A(p(t))*x(t)+B(p(t))*u(t) (3-24)

Wherein:

$A\left(p\left(t\right)\right)=\frac{w1\left(p\left(t\right)\right)*A1+w2\left(p\left(t\right)\right)*A2}{w1\left(p\left(t\right)\right)+w2\left(p\left(t\right)\right)}---(3-25)$

$B\left(p\left(t\right)\right)=\frac{w1\left(p\left(t\right)\right)*B1+w2\left(p\left(t\right)\right)*B2}{w1\left(p\left(t\right)\right)+w2\left(p\left(t\right)\right)}---(3-26)$

As-179.4270<p (t) <179.4270, this fuzzy model illustrates nonlinear model accurately;

Step 3: use Linear-Quadratic Problem method design optimal controller respectively for two linear models, its concrete implementation is as follows:

(3a) use Linear-Quadratic Problem method respectively for two linear models, obtain control rate as shown in formula (3-27), formula (3-28):

When p (t) is near zero,

u(t)＝f1*x(t) (3-27)

When p (t) is near positive and negative pi

u(t)＝f2*x(t) (3-28)；

(3b) final controlled quentity controlled variable is as shown in formula (3-30):

$u\left(t\right)=\frac{w1\left(p\left(t\right)\right)*f1+w2\left(p\left(t\right)\right)*f2}{w1\left(p\left(t\right)\right)+w2\left(p\left(t\right)\right)}*x\left(t\right)---(3-29)$

$W\left(t\right)=-\frac{u\left(t\right)}{\mathrm{width}}---(3-30)$

Step 4: when with Linear-Quadratic Problem method design optimal controller, need given weighting matrices Q and R, solve by off-line the parameter that the Riccati differential equation obtains controller; Q and R is very large on the impact of control effects; The approximate range of parameter is determined by emulation; Its concrete implementation is as follows:

(4a) in MATLAB, establish the response model of each state diagonal angle speed setting value of a robot when constant airspeed; Q and R has reacted the differential degree cur_vlr of robot movement respectively, and the angle l_angle of robot car body and tangent line, the distance l_y of robot car body center and tangent line and controlled quentity controlled variable are to the contribution of Control platform; The adjustment direction of parameter can be determined according to the response results of robot under given original state;

(4b) use MATLAB GUI to build a simulated environment: the parameters value in this environment real-time display device people motion process, arbitrarily can suspend, return Last status, and can show historical data and analyze;

(4c) emulation experiment is carried out: simulation result can sufficient proof, as long as parameter is suitable, this algorithm just can obtain good control effects; Even if when control cycle is long, this algorithm still can control effectively to robot, and whole process is very steady;

Step 5: make fine setting by reality test to parameter, its concrete implementation is as follows:

(5a) emulated data that (4c) obtains is applied in true experiment;

(5b) according to truly testing, the value of adjustment individual parameters;

(5c) step (5a) ~ (5b) is repeated, until reach Optimal Control effect.