CN103019239A - Trajectory tracking sliding mode control system and control method for spraying mobile robot - Google Patents

Abstract

The invention discloses a trajectory tracking sliding mode control method for a spraying mobile robot. The method comprises the following steps of: performing mechanism analysis on a mobile robot, and establishing a mobile robot kinematic model with non-integrity constraint; establishing a controlled object mathematical model of each branch controller of a wheeled mobile robot provided with a motor driving shaft disturbance term; identifying a traveling path by utilizing a computer vision system, and determining an expected motion track of each branch driving motor according to the kinematic model deduced in the previous step; detecting the rotating speed of the motor, calculating the actual motion angular velocity and actual motion angular acceleration of left and right driving motors of the mobile robot, and calculating the deviation and deviation derivative between the expected angular velocity and the actual angular velocity of each driving motor; establishing a sliding mode switching function which meets the speed control requirement of the driving motor; determining the sliding mode controller control quantity of the left and right driving motors of the mobile robot on the basis of the sliding mode surface function s; and respectively transmitting the control quantity of the motor of the mobile robot to the left and right driving motors.

Description

The track following System with Sliding Mode Controller and the control method that are used for spray medicine mobile robot

Technical field

The present invention relates to a kind of mobile robot's Trajectory Tracking Control, relate in particular to the construction method of following the tracks of the sliding mode controller of control for two-wheel drive spray medicine mobile robot trace under the greenhouse.

Background technology

Since the seventies in last century, China progressively promotes the plastic greenhouse cultivation technology, and obtains aobvious economic and social benefit, and nowadays, China has become maximum crops hothouse production country.Yet the hothouse production management level of China are compared with advanced country with automaticity and are still had a certain distance.Aspect hothouse production equipment, still carry out the crops dispenser with hand sprayer.For effectively improving the agricultural chemicals utilization factor, reduce labour intensity, and reduce agricultural chemicals to operating personnel's injury, be necessary to improve the automatization level of China's hothouse production equipment, the requirement of producing to satisfy modern precision agriculture.

Spraying machine device people is very important modern greenhouse management equipment, and increasing country has used greenhouse spraying machine device people in agricultural production, and relevant technical merit is also improving constantly.Yet, crop-planting is intensive in the greenhouse, earth bulging and spatial obstacle are also deposited, and under this destructuring environment crop are implemented precisely spray medicine for realizing spray medicine mobile robot, and Trajectory Tracking Control is that spraying machine device people realizes that crops precisely spray the key of medicine accurately.

Document " Image-based Trajectory Tracking with Fuzzy Control for Nonholonomic MobileRobots (Tatsuya Kato, et.al, IECON 2011-37th Annual Conference on IEEE Industrial ElectronicsSociety, 2011, pp.3299-3304.) introduce a kind of Trajectory Tracking Control that realizes the mobile robot by the fuzzy controller of processing based on image.The machine-independent people's of model image coordinate system is mapped to image coordinate system with the information of robot camera, by the information indirect control state of the control plane of delineation.Gradient and intercept by the target trajectory under the graphics process computed image coordinate system therefrom obtain the desired trajectory objective function.Designing a fuzzy controller controls course angle and the speed of robot, this fuzzy controller be the 6I-2O structure, input quantity is gradient under the image coordinate system of processing based on image and derivative, course angle, the pace of intercept, gradient and intercept), output quantity is the regulated quantity of course angle and pace.This controller can be by the information indirect control state of the control plane of delineation.Although the software and hardware technology development of at present image processing is swift and violent, but the calculating of visual identification algorithm is consuming time still very important reliably, and the operating environment in the greenhouse is sometimes more weaker than the operating environment in field, the factors such as closeness intensive such as the interior space and surface facility, crop is larger than the field, indoor light is sufficient have not increased again the identification difficulty, to such an extent as in time perception robot pose variation of vision system; The mobile robot advances and when turning, can sliding friction occur with ground inevitably so that the output controlled quentity controlled variable of system can not accurate response in control system, thereby directly affect the performance of robotic tracking control; In addition, adopt fuzzy controller that the mobile robot is carried out Trajectory Tracking Control in the literary composition, and fuzzy logic needs complete human experience, because input quantity is more, its inference rule and fuzzy table sharply increase; Document " improve the AKF of wheeled mobile robot performance and sliding formwork combine control method " (Cao Zhengcai etc., control and decision-making, 10 phases of the 26th volume in 2011, the 1409-1503 page or leaf) in real work, is subject to inevitably the problem of such environmental effects for wheeled mobile robot, adopt the Sage-Husa adaptive Kalman filter that the reference locus with white noise is estimated, to improve the authenticity of metrical information; Sliding Mode Algorithm be used for control speed and turn to, realize the tracking control of robot and suppress external interference.Utilize the Backstepping method to select to be used for the velocity control law of tracking, select PI type sliding-mode surface, determine the equivalent control amount by Dynamic Models of Robot Manipulators, when there is external interference in system, consider the switching controls amount.In addition, introducing speed change function vs (s, ε) replaces sgn (s) to weaken jitter phenomenon.The subject matter of the method is structure and the design process complexity of controller, and requires robot that fully large acceleration can be provided, and is difficult to satisfy the requirement of real-time of greenhouse spray medicine mobile robot control.

Application number is CN201110169879.9, name be called " variable structure control method of wheeled mobile robot " Patent Application Publication a kind of variable structure control method of wheeled mobile robot, adopt the Multi-Mode PID control method to come the traveling priority of control; The working direction that the control method that adopts PID control to combine with rule control is proofreaied and correct dolly; These two kinds of control modes are changed by the variation of deflection and center offset, the turning control of robot comes control to turn in original place right-angled bend mode by turn direction information and positional information, adopt different control algolithms and control accordingly parameter for the residing different conditions of robot, more traditional PID control method has certain intelligent.But should invent in essence still a kind of pid control algorithm, because greenhouse spray medicine mobile robot is the complication system of a plurality of variablees such as collection crop area to be painted, disease and pest degree, range information, the speed of travel, spray value, liquor strength and one, and each variable influences each other.In addition, the existence that the factor such as temperature, humidity, illuminance, ground flat degree, driving wheel foozle and some crop, Keelof green house, pipeline and road adjunct etc. disturb in the greenhouse, so that being difficult to control, above-mentioned PID control method travels along the path of planning, actual path easily departs from desirable operation route, therefore can produce in the operating area to respray more greatly and the drain spray zone.

In addition, consult domestic Patent aspect Control of Wheeled Mobile Robots as seen, the mobile robot control patent mostly is the control system structural design, because a variety of causes such as the manufacturing of mechanism body, assembling, wearing and tearing and complicated destructuring farmland scene will certainly affect the tracking accuracy of Greenhouse Robot to the Accuracy that sensor information gathers.

Can find out that from above-mentioned Greenhouse Robot Trajectory Tracking Control correlation technique existing research has obtained some achievements, but have certain limitation.Fuzzy control method lacks self-learning capability, in the mobile robot trace tracking control system, can not adjust online fuzzy control rule, and select with certain subjectivity at the subordinate function of input quantity output quantity, adaptive ability is limited, be difficult to obtain desirable track following effect, in addition, fuzzy rule depends on operating personnel's experience, for the greenhouse spray medicine Mobile Robot Control System with Multiinputoutput amount, its inference rule and fuzzy control table are complicated; The PID control method is only applicable to linear system or simple nonlinear system, and for multivariate, nonlinearity and uncertain factor and interference and the complicated greenhouse spray medicine mobile-robot system deposited, its control effect is unsatisfactory; And be structure and the design process complexity of controller based on the subject matter that the sliding formwork of Backstepping method is controlled, and require robot that fully large acceleration can be provided, be difficult to satisfy the requirement of real-time of greenhouse spray medicine mobile robot control.

Summary of the invention

The objective of the invention is for overcoming above-mentioned the deficiencies in the prior art, for greenhouse spray medicine mobile robot, propose a kind of sliding-mode control based on integral weighting gain Reaching Law and realize its Trajectory Tracking Control, the absolute value that comprises switching function s integration in this control algolithm gain term, when s levels off to zero the time, the gain of switching item levels off to zero, buffets thereby eliminate; When system state during not at sliding mode, although the s value is larger because integral weighting coefficient k f is for negative, can effectively avoid when system not in the increase of sliding mode handoff gain during the stage.The designed sliding mode controller of the present invention does not rely on the accurate mathematical model of controlled device, have response fast, insensitive to parameter and environmental change, need not the advantages such as system's on-line identification, physics realization is simple, and the intrinsic buffeting problem of System with Sliding Mode Controller can be effectively eliminated in the gain introducing of Reaching Law of integral weighting.Under the condition that does not increase the system hardware cost, improve mobile robot's tracking accuracy and antijamming capability by the method for software control, thereby realize that further Greenhouse Robot is precisely spraying medicine to crop enforcement in the ranks.

The technical solution used in the present invention is that the track following System with Sliding Mode Controller for spray medicine mobile robot is comprised of car body pose and driving wheel speed mapping block, revolver control system and right cranking wheel control system;

Described car body pose and driving wheel speed mapping block, be used for basis with the controlled device mathematical model of each driving wheel controller of wheeled mobile robot of motor driving shaft distracter and the walking path that computer vision system identifies, determine the desired speed track ω d of each branch road drive motor in realizing spray medicine mobile Robot process, input respectively revolver control system and right cranking wheel control system;

Described revolver control system comprises sliding mode controller, driver, direct current generator, gear case and revolver, also is provided with scrambler between the output terminal of described direct current generator and the input end of described sliding mode controller; Described sliding mode controller sends to driver with controlled quentity controlled variable as driving instruction, driver is regulated DC generator speed by the different duty of regulating pwm signal, direct current generator is input to gear case with rotatablely moving of axle, the output shaft drive machines people revolver of gear case rotates, described scrambler adopts Measuring Frequency Method that the rotating speed of motor is detected, in the situation that the consideration driving wheel trackslips, calculate the actual motion angular velocity omega of mobile robot left and right sides driving motor, the actual motion angular acceleration

And calculate deviation e and the deviation derivative of each driving motor expectation angular velocity and actual angular speed

Input to described sliding mode controller; Described sliding mode controller is according to deviation e, deviation derivative With sliding-mode surface gain of parameter sliding mode controller switching function s, again according to sliding mode controller switching function s and controlled device mathematical model, determine the controlled quentity controlled variable of mobile robot's driving motor;

The structure of described right cranking wheel control system is identical with the structure of revolver control system.

As a further improvement on the present invention, the controlled quentity controlled variable of described driving motor is determined according to following formula:

$\left\{\begin{array}{c}u=-\frac{1}{g\left(x\right)}[c\stackrel{\&CenterDot;}{e}-{\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&omega;}}}_d+f\left(x\right)+g\left(x\right)u+d\left(t\right)+\mathrm{ks}+{\mathrm{<figure-callout\; id="2"\; label="k"\; filenames="HDA00002474057900023.png"\; state="\{\{state\}\}">k</figure-callout>}}_2|\mathrm{\&rho;}\left|\mathrm{sgn}\left(s\right)\right]\\ \mathrm{\&rho;}={\&Integral;}_0^t(k_f\mathrm{\&rho;}+s)\mathrm{dt}\end{array}\right.$

Wherein, u is controller output, is the command voltage (unit is v) that sends to servoamplifier; F (x) and g (x) are the mathematic(al) representations relevant with motor parameter with the motor drive shaft setting; The first order derivative of drive motor angular velocity error; It is the second derivative of drive motor desired motion angular velocity; D (t) is the uncertain part of Greenhouse Robot system, comprises Parameter Perturbation, load disturbance, sensor measurement error and other external disturbance; S is the sliding-mode surface function; K and k _wThe positive weighted integral gain Reaching Law parameter that to set, usually k and k _wValue arranges larger, and system can enter sliding-mode surface with very fast speed like this; k _fBe negative weighting coefficient; Sgn (s) is the sign function about sliding-mode surface s.

Another technical scheme that realizes the object of the invention is: be used for spray medicine mobile robot's track following sliding formwork control control method, comprise the steps:

Step 1 is carried out analysis of mechanism to the mobile robot, sets up the moveable robot movement with nonholonomic restriction and learns model;

Step 2 considers that Greenhouse Robot travels and requires and design feature and the greenhouse factor of effect with it that foundation is with the controlled device mathematical model of each branch controller of wheeled mobile robot of motor driving shaft distracter;

Step 3 utilizes computer vision system to identify the border in the capable or zone, ridge of crops in greenhouse as walking path, according to the kinematics model that step 1 is derived, determine the desired motion track of each branch road drive motor in realizing spray medicine mobile robot trace tracing process;

Step 4 is utilized incremental optical-electricity encoder, adopts Measuring Frequency Method that the rotating speed of motor is calculated, and detects the actual motion state of two wheel mobile robot left and right sides driving motors;

Step 5 is considered mobile robot's drive motor characteristic and rate request, selects suitable sliding-mode surface parameter, makes up to satisfy the sliding formwork switching function that the control of drive motor speed requires;

Step 6 design is for the weighting type storage gain Reaching Law of drive motor, and the controlled device Design of Mathematical Model of setting up according to step 2 calculates the controlled quentity controlled variable of two wheel mobile robot left and right sides driving motors accordingly based on the sliding mode control algorithm of Reaching Law;

Step 7 sends to respectively left and right sides motor driver with above-mentioned direct motor drive controlled quentity controlled variable, realizes the track following under the spray medicine mobile robot greenhouse.

The present invention is directed to two-wheel drive greenhouse spray medicine mobile robot trace and follow the tracks of control, propose first a kind of sliding-mode control based on integral weighting gain Reaching Law, its characteristics and beneficial effect are:

1, wheeled mobile robot is the non-holonomic Constraints of a nonholonomic constraint, has non-linear, strong coupling, multivariate characteristic, particularly surface condition and constantly changes in steering procedure, is difficult to set up the controlled device mathematical models.And the complicacy of greenhouse is so that have uncertainty in the tracing process, be subject to temperature in the greenhouse, humidity, illuminance, ground flat degree, the driving wheel measuring error that the impact of factor produces such as trackslip such as Parameter Perturbation and load disturbance and sensor, can cause that all the track of the actual walking of mobile robot departs from ideal path.Sliding mode variable structure control based on weighted integral gain Reaching Law proposed by the invention is a kind of special nonlinear Control, its realization does not rely on the controlled device mathematical models, and to the parameter of above-mentioned Greenhouse Robot system change and the uncertain factor such as external interference insensitive, by selecting suitable Reaching Law parameter, can guarantee that the mobile robot departs from actual path in the situation of desirable operation route with the fastest speed convergence expectation route, realizes rapidity and the accuracy of track following.

2, spraying medicine mobile robot actual path in the process of desired trajectory convergence, because the factors such as inner parameter perturbation, external disturbance, measuring error and measurement noise, the particularly startup of robot, stop, turning waits and can produce larger tracking error fast, causes the gain of switching item larger, so that the controlled quentity controlled variable amplitude fluctuations is larger, follow the tracks of dynamic property thereby reduce the robot trajectory, also can damage equipment when serious.Control algolithm Reaching Law of the present invention, in the integration item, introduce negative weighted value, can effectively avoid the increase when the handoff gain of system state during not at sliding-mode surface, so that the controller output quantity is level and smooth, and then so that in the greenhouse spray medicine mobile robot's actual motion track follow in the process of desirable operation route and can not produce larger fluctuation, therefore can avoid producing in the operating area more serious respraying and the drain spray phenomenon.

Description of drawings

Below in conjunction with the drawings and specific embodiments the present invention is described in further detail.

Fig. 1 is two-wheel drive greenhouse spray medicine mobile apparatus human model synoptic diagram;

Fig. 2 is the schematic diagram that greenhouse spray medicine mobile robot trace is followed the tracks of the integral weighting gain Reaching Law sliding-mode control of control;

Fig. 3 is drive motor speed trace plot;

Fig. 4 is drive motor speed tracking error curve figure;

Fig. 5 is the driving controlled quentity controlled variable of drive motor;

Fig. 6 is greenhouse spray medicine mobile robot straight line path tracing figure, and wherein, 6a is the straight line path tracing figure, and 6b is in the straight line path tracking situation, left and right sides drive motor angular velocity response diagram;

Fig. 7 is greenhouse spray medicine mobile robot circular path tracing figure, and wherein, 7a is the circular path tracing figure, and 7b is in the circular path tracking situation, left and right sides drive motor angular velocity response diagram;

Embodiment

Such as Fig. 2, at first, the two-wheel drive mobile robot is carried out analysis of mechanism, in conjunction with synoptic diagram shown in Figure 1, set up the moveable robot movement with nonholonomic restriction and learn model; Secondly, consider that Greenhouse Robot travels to require and design feature and the soil isothermal chamber environmental factor of effect with it that foundation is with the controlled device mathematical model of each branch controller of wheeled mobile robot of motor driving shaft distracter; Then, utilize border that computer vision system identifies the zone such as the capable or ridge of crops in greenhouse as walking path, the desired speed track ω d(desired motion track of determining each branch road drive motor in realizing spray medicine mobile Robot process refers to the speed track of the expectation of drive motor); Utilize incremental optical-electricity encoder, adopt Measuring Frequency Method that the rotating speed of motor is calculated, detect the actual motion state ω of mobile robot left and right sides driving motor, and calculate the deviation e of each driving motor expectation angular velocity and actual angular speed; Next consider mobile robot's drive motor characteristic and rate request, select suitable sliding-mode surface parameter, make up and satisfy the sliding formwork switching function that the control of drive motor speed requires

For the storage gain Reaching Law of drive motor design with negative weighted value Wherein, k _f＜0, based on the design sliding formwork control law of this Reaching Law $u=-1/g\left(x\right)[c\stackrel{\&CenterDot;}{e}-{\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&omega;}}}_d+f\left(x\right)+g\left(x\right)u+d\left(t\right)+\mathrm{ks}+k_w|\mathrm{\&rho;}\left|\mathrm{sgn}\left(s\right)\right]$ By selecting suitable sliding-mode surface and Reaching Law parameter, guaranteeing that the controller output quantity is stably under the prerequisite, reduce fast owing to greenhouse, driving wheel are made the track following error that the factors such as alignment error, measuring error and other external disturbance cause, thereby guarantee to spray medicine mobile robot actual motion track and within the fastest time, follow smoothly desirable working path, avoid in more serious the respraying and the drain spray phenomenon of operating area generation; Calculate the controlled quentity controlled variable of mobile robot left and right sides driving motor according to designed integral weighting gain Reaching Law sliding mode control algorithm, send to respectively left and right sides motor driver, drive the orbit tracking movement under the spray medicine mobile robot greenhouse.Concrete grammar is as follows:

1, greenhouse spray medicine mobile robot is carried out analysis of mechanism, set up the greenhouse spray medicine moveable robot movement with nonholonomic restriction and learn model

The kinematics model of greenhouse spray medicine mobile robot continuous system:

$\left[\begin{array}{c}{\mathrm{\&omega;}}_L\\ {\mathrm{\&omega;}}_R\end{array}\right]=\left[\begin{array}{ccc}\frac{R}{i}\&CenterDot;\frac{1}{\cos \mathrm{\&theta;}}& 0& -L\\ 0& \frac{R}{i}\&CenterDot;\frac{1}{\sin \mathrm{\&theta;}}& L\end{array}\right]\left[\begin{array}{c}\stackrel{\&CenterDot;}{x}\\ \stackrel{\&CenterDot;}{y}\\ \stackrel{\&CenterDot;}{\mathrm{\&theta;}}\end{array}\right]---\left(1\right)$

Wherein, ω _LAnd ω _RBe respectively robot left and right wheels drive motor reason wheel angular velocity (unit is rad/s); Q=[x, y, θ] be the state of system, i.e. mobile robot's pose amount, (x, y) is the position coordinates (unit be mm) of robot in the XY direction, θ is the attitude angle (unit is rad) of robot; R is robot driving wheel radius (unit is mm), and L is half (unit is mm) of two driving wheel wheels distance in the heart.

Because the impact of greenhouse floor environment, the mobile robot advances and when turning, can sliding friction occur with ground inevitably, for so that the speed of mobile operating speed and actual requirement coincide preferably, determine the linear relation between expectation angular velocity and the theoretical angular velocity

ω＝a·ω _s+b (2)

Wherein, ω is actual speed, ω _sBe theoretical velocity, a and b are coefficient.

2, foundation is with the controlled device mathematical model of each branch controller of wheeled mobile robot of motor driving shaft distracter

For considering characteristics that the greenhouse travels and convenience and the dirigibility of operation in the ranks, the three-wheel walking is adopted in greenhouse spraying machine device people's running gear design, the scheme that two-wheel drive, direct current generator drive.The variation of surface condition, load increase and other factor affect to some extent on the travel speed of robot, take into full account the greenhouse in the ranks travelling of walking robot require and design feature, take state space equation set up with each control branch road controlled device mathematical model of mobile robot of motor drive shaft distracter as:

$\left\{\begin{array}{c}{\stackrel{\&CenterDot;}{x}}_1=x_2\\ {\stackrel{\&CenterDot;}{x}}_2=f\left(x\right)+g\left(x\right)u+d\left(t\right)\end{array}\right.---\left(3\right)$

Wherein, x ₁=ω ∈ R is the actual motion angular velocity of drive motor, and unit is (unit is rad/s); U ∈ R is the control inputs of system, and the branch road that namely sends to the motor servo amplifier drives controlled quentity controlled variable, and unit is V; F (x) and g (x) are the functions with corresponding dimension, sliding formwork control has very strong anti-interference and robustness, so this system has insensitivity for the system parameter variations in the certain limit, so f (x) and g (x) can directly determine according to motor drive shaft setting and motor parameter; X=[x ₁, x ₂] ∈ R ²It is system state;

X _iFirst order derivative, i=1 wherein, 2; D (t) is the uncertain part of Greenhouse Robot system, comprises Parameter Perturbation, load disturbance, sensor measurement error and other external disturbance.

3, utilize computer vision system to identify the border in the zone such as the capable or ridge of crops in greenhouse as walking path, according to the kinematics model that step 1 is derived, determine the desired motion track of each branch road drive motor in realizing spray medicine mobile robot trace tracing process

Greenhouse spray medicine mobile robot utilizes monopod video camera to obtain guidance path, monopod video camera be installed in robot two front axle central points directly over, apart from ground 0.5m, the depression angle of video camera is 25 degree during collection, and the resolution of obtaining image is 752 * 582 pixels.Gather the crop plant image under the heliogreenhouse environment, crop is by the ridge plantation, and the ground between two ridges is the actual path of robot ambulation.Process at the Lab color space, crop is identified from Soil Background.By the maximum variance thresholding method image is converted into bianry image, and utilizes wave trough position to determine separatrix, ridge, the left and right sides.Obtain the discrete point that navigates according to left and right sides ridge culture thing plant position, obtain guidance path by the Hough conversion.Then determine the desired motion angular velocity omega of each branch road drive motor in realizing spray medicine mobile Robot process according to kinematics model shown in formula (1), (2) _d(rad/s of unit) and desired motion angular acceleration

(the rad/s of unit ²);

4, detect the actual motion state of having considered the mobile robot left and right sides driving motor that driving wheel trackslips

Consider the situation that greenhouse floor and driving wheel trackslip, actual slippage rate and the theoretical velocity of setting, load and surface state are relevant, and itself and theoretical velocity are approximated to linear relationship and are

σ＝c·ω _s+d

ω＝(1-σ)ω _t (4)

Wherein, σ is the driving wheel slippage rate, ω _sBe theoretical angular velocity (rad/s of unit) that ω is actual angular speed (rad/s of unit), ω _tBe encoder feedback value (rad/s of unit) that c and d are coefficient.

Utilize incremental optical-electricity encoder, adopt Measuring Frequency Method that the rotating speed of motor is detected, calculate the actual motion angular velocity omega (rad/s of unit) of mobile robot left and right sides driving motor in the situation that the consideration driving wheel trackslips, the actual motion angular acceleration

(unit is rad/s ²), and calculate deviation e and the deviation derivative that each driving motor is expected angular velocity and actual angular speed

5, consider mobile robot's drive motor characteristic and rate request, select suitable sliding-mode surface parameter, make up and satisfy the sliding formwork switching function that the control of drive motor speed requires

Greenhouse Robot is realized its track following by the speed control of left and right wheels drive motor, considers drive motor characteristic and rate request, designs the sliding mode controller switching function for the control of direct current drive motor speed:

$s=\mathrm{ce}+\stackrel{\&CenterDot;}{e}---\left(5\right)$

In the formula (5), e=ω-ω _dBe the angular velocity error (rad/s of unit) of mobile robot left and right sides drive motor motion,

Be the e first order derivative;

Be the sliding-mode surface function; C gets normal number, satisfies drive motor speed control requirement, guarantees simultaneously polynomial expression

Satisfy Hull dimension thatch (Hurwitz) stability criterion, thereby guarantee the existence of sliding mode.

6, on definite sliding-mode surface function s basis, design weighted integral gain Reaching Law, determine the sliding mode controller controlled quentity controlled variable of mobile robot left and right sides driving motor;

On the designed drive motor speed sliding formwork control switching function s basis of step 5, design weighted integral gain Reaching Law

$\stackrel{\&CenterDot;}{s}=-\mathrm{ks}-k_w\left|\mathrm{\&rho;}\right|\mathrm{sgn}\left(s\right)$

$\mathrm{\&rho;}={\&Integral;}_0^t(k_f\mathrm{\&rho;}+s)\mathrm{dt},k_f<0---\left(6\right)$

In the formula, k and k _wThe weighted integral gain Reaching Law parameter that can set, in the expression formula of integration item ρ, k _fBe the weighting coefficient of bearing, satisfy, as ρ 0 the time, k _fρ＜0, when ρ＜0, k _fρ〉0.Convolution (3) controlled device mathematical model and formula (5) sliding formwork switching function are determined the controlled quentity controlled variable of mobile robot's driving motor

$\left\{\begin{array}{c}u=-\frac{1}{g\left(x\right)}[c\stackrel{\&CenterDot;}{e}-{\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&omega;}}}_d+f\left(x\right)+g\left(x\right)u+d\left(t\right)+\mathrm{ks}+k_w|\mathrm{\&rho;}\left|\mathrm{sgn}\left(s\right)\right]\\ \mathrm{\&rho;}={\&Integral;}_0^t(k_f\mathrm{\&rho;}+s)\mathrm{dt}\end{array}\right.---\left(7\right)$

Wherein,

The second derivative of drive motor motion angular velocity, k and k _wBe normal number, usually k and k _wValue arranges larger, and system can enter sliding-mode surface with very fast speed like this.k _fBe negative constant, sgn (s) is the sign function about sliding-mode surface s.

In the Reaching Law that this sliding mode control algorithm adopts, replace s with new integration phase ρ, level off to zero the time as s in the sliding mode stage, the integration of s also levels off to zero, and the ρ value also trends towards 0, and finally can eliminate sgn (s) item; When system during not at sliding-mode surface, select suitable weighting coefficient k _fCan greatly reduce ρ, so that the ρ value trends towards 0 fast, effectively avoided the increase of handoff gain, so that the controller output quantity is level and smooth, and then spray medicine mobile robot's actual motion track can steadily be followed desirable operation route in the assurance greenhouse, improves greenhouse spraying machine device people track following performance.

7, drive mobile robot's left and right wheels with the drive and control of electric machine amount

Determined the controlled quentity controlled variable of left and right sides driving motor by step 6, see formula (7), this controlled quentity controlled variable is come regulating electric machine speed as the motor servo driver that drives instruction and send to each branch road by the different duty of regulating pwm signal.Direct current generator is input to gear case with rotatablely moving of axle, the output shaft control wheel turns of gear case, and control direct motor drive machine people left and right wheels, thus drive the orbit tracking movement that sprays under the medicine mobile robot greenhouse.

One embodiment of the present of invention below are provided.

Embodiment

Control method of the present invention is mainly put forth effort on a kind of sliding-mode control based on integral weighting gain Reaching Law and is improved spray medicine mobile robot trace tracking control performance under the greenhouse.If the artificial two-wheel drive of spray medicine mobile apparatus robot is driven by DC servo-motor, its path control system block diagram as shown in Figure 2.

Be used for spray medicine mobile robot's track following System with Sliding Mode Controller, formed by car body pose and driving wheel speed mapping block, revolver control system and right cranking wheel control system;

Car body pose and driving wheel speed mapping block, be used for basis with the controlled device mathematical model of each driving wheel controller of wheeled mobile robot of motor driving shaft distracter and the walking path that computer vision system identifies, determine the desired speed track ω of each branch road drive motor in realizing spray medicine mobile Robot process _d, input respectively revolver control system and right cranking wheel control system;

The revolver control system comprises sliding mode controller, driver, direct current generator, gear case and revolver, also is provided with scrambler between the output terminal of direct current generator and the input end of sliding mode controller; Sliding mode controller sends to driver with controlled quentity controlled variable as driving instruction, driver is regulated DC generator speed by the different duty of regulating pwm signal, direct current generator is input to gear case with rotatablely moving of axle, the output shaft drive machines people revolver of gear case rotates, scrambler adopts Measuring Frequency Method that the rotating speed of motor is detected, in the situation that the consideration driving wheel trackslips, calculate the actual motion angular velocity omega of mobile robot left and right sides driving motor, the actual motion angular acceleration And calculate deviation e and the deviation derivative of each driving motor expectation angular velocity and actual angular speed Input to described sliding mode controller; Sliding mode controller is according to deviation e, deviation derivative

With sliding-mode surface gain of parameter sliding mode controller switching function s, again according to sliding mode controller switching function s and controlled device mathematical model, determine the controlled quentity controlled variable of mobile robot's driving motor; The structure of right cranking wheel control system is identical with the structure of revolver control system.

The embodiment of the control method of this control system is as follows:

1, the mobile robot is carried out analysis of mechanism, set up the moveable robot movement with nonholonomic restriction and learn model

If mobile robot mechanism parameter: R=105mm, L=205mm, i=33:1, kinematics model according to mobile robot's continuous system shown in the formula (8), can determine mobile robot's free-position amount q=[x, y, θ] the theoretical angular velocity omega of corresponding left and right wheels drive motor _LAnd ω _R(unit is rad/s).

$\left[\begin{array}{c}{\mathrm{\&omega;}}_L\\ {\mathrm{\&omega;}}_R\end{array}\right]=\left[\begin{array}{ccc}\frac{105}{33}\&CenterDot;\frac{1}{\cos \mathrm{\&theta;}}& 0& -205\\ 0& \frac{105}{33}\&CenterDot;\frac{1}{\sin \mathrm{\&theta;}}& 205\end{array}\right]\left[\begin{array}{c}\stackrel{\&CenterDot;}{x}\\ \stackrel{\&CenterDot;}{y}\\ \stackrel{\&CenterDot;}{\mathrm{\&theta;}}\end{array}\right]---\left(8\right)$

Consider that the mobile robot advances and when turning and the sliding friction of ground generation, determine linear relation between drive motor actual angular speed and the theoretical angular velocity according to formula (2)

ω＝0.9305·ω _s+0.0076 (9)

2, foundation is with the controlled device mathematical model of each branch controller of wheeled mobile robot of motor driving shaft distracter

The key of setting up the controlled device mathematical model of each branch controller of mobile robot is to determine f (x) and the g (x) in the formula (3).Each branch road is established the DC servo-motor driver and is set to speed control mode take motor driver and motor as controlled device, and its winding resistance is r _a(unit is Ω), the winding equivalent inductance is L _a(unit is mH), total moment of inertia is that (unit is gcm to J on the DC servo-motor axle ²), back EMF coefficient is K _e(unit is V/ (rad)), torque constant is that K unit is Nm/A).Consider that the system that adopts the sliding formwork control technology has insensitivity to system parameter variations after forming sliding formwork, then the controlled device mathematical model of each branch controller of mobile robot can be simplified and is established as:

${\stackrel{\&CenterDot;}{x}}_2=-\frac{r_a{B}_v+K_e{K}_T}{L_{a}J}{x}_1-\frac{r_{a}J+L_a{B}_v}{L_{a}J}{x}_2+\frac{K_T}{L_{a}J}u+d\left(t\right)---\left(10\right)$

Wherein, Bv is for being ratio of damping, and u is controller output, is the command voltage (unit is v) that sends to servoamplifier; X is the angular velocity (unit is rad/s) of each branch road drive motor of mobile robot; D (t) is the uncertain part of Greenhouse Robot system, comprises Parameter Perturbation, load disturbance, sensor measurement error and other external disturbance, need not during modeling to determine, sliding formwork control has stronger robustness to this.

Contrast formula (3) has:

$f\left(x\right)=-\frac{r_a{B}_v+K_e{K}_T}{L_{a}J}{x}_1-\frac{r_{a}J+L_a{B}_v}{L_a\mathrm{<figure-callout\; id="2"\; label="J\; x"\; filenames="HDA00002474057900023.png"\; state="\{\{state\}\}">J</figure-callout>}}{x}_{}{}_2$

(11)

$g\left(x\right)=\frac{K_T}{L_{a}J}$

According to driver setting and the parameter of electric machine, establish that the drive motor parameter is in the formula (10): r _a=0.628 Ω, L _a=100mH, J=0.0602gcm ², K _T=0.0255Nm/A, K _e=0.067V/ (rad) can determine f (x) and g (x) thus.

3, determine the desired motion track of each branch road drive motor in realizing spray medicine mobile Robot process

The mobile apparatus human motion is generally by its pose amount q=[x, y, θ] expression, establish the actual needs mobile robot and finish following motion: 1. robot carries out linear uniform motion in 2s.If the origin coordinates of straight line path is [0,0], with the angle of X-axis be 45 °, the robot initial pose is [0,0,0], the left and right wheels initial velocity is 0; 2. adopt same control structure and parameter setting, establish robot and carry out the uniform circular motion that radius is 400mm in 4s, the robot initial pose is that starting point is [800,400, pi/2], and the left and right wheels initial velocity is 0.Determine mobile robot's left and right wheels drive motor expectation angular velocity omega according to formula (8) and formula (9) _LdAnd ω _Rd(unit is rad/s).

4, detect the actual motion state of mobile robot left and right sides driving motor

Consider the situation that greenhouse floor and driving wheel trackslip, determine actual slippage rate according to formula (4)

σ＝0.046·ω _s+0.0266

(12)

ω＝(1-σ)ω _t

Utilize 2000 line incremental optical-electricity encoders, adopt Measuring Frequency Method that the rotating speed of motor is detected, according to formula (12), calculate the actual motion angular velocity omega (rad/s of unit) of mobile robot left and right sides driving motor in the situation that the consideration driving wheel trackslips, the actual motion angular acceleration

(unit is rad/s ²), and calculate deviation e and the deviation derivative that each driving motor is expected angular velocity and actual angular speed

5, consider mobile robot's direct-drive motor characteristic and rate request, select the sliding-mode surface parameter, make up and satisfy the sliding formwork switching function that the control of drive motor speed requires

Consider drive motor characteristic and rate request, the sliding mode controller switching function of the control of direct current drive motor speed shown in the design formula (13):

$s=\mathrm{ce}+\stackrel{\&CenterDot;}{e}---\left(13\right)$

For satisfying drive motor speed control requirement, the c value is larger, among the present invention, and c 〉=500; E=ω-ω _dBe the angular velocity error (rad/s of unit) of mobile robot left and right sides drive motor motion,

Be the e first order derivative;

Be the sliding-mode surface function;

6, on definite sliding-mode surface function s basis, design weighted integral gain Reaching Law, determine the controlled quentity controlled variable of mobile robot left and right sides driving motor

On the designed drive motor speed sliding formwork control switching function s basis of step 5, design weighted integral gain Reaching Law

$\stackrel{\&CenterDot;}{s}=-\mathrm{ks}-k_w\left|\mathrm{\&rho;}\right|\mathrm{sgn}\left(s\right)$

$\mathrm{\&rho;}={\&Integral;}_0^t(k_f\mathrm{\&rho;}+s)\mathrm{dt},k_f<0---\left(14\right)$

In the formula, k and k _wBe weighted integral gain Reaching Law parameter, k, k _wValue should arrange larger, and the assurance system enters sliding-mode surface with speed faster, gets k=2000, k _w=100; In the expression formula of integration item ρ, k _fBe weighting coefficient, get k _f=-5.

Determine the controlled quentity controlled variable of mobile robot's driving motor according to sliding formwork switching function shown in controlled device mathematical model shown in formula (10) and (11) and the formula (13)

$\left\{\begin{array}{c}u=-\frac{1}{g\left(x\right)}[c\stackrel{\&CenterDot;}{e}-{\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&omega;}}}_d+f\left(x\right)+g\left(x\right)u+d\left(t\right)+\mathrm{ks}+k_w|\mathrm{\&rho;}\left|\mathrm{sgn}\left(s\right)\right]\\ \mathrm{\&rho;}={\&Integral;}_0^t(k_f\mathrm{\&rho;}+s)\mathrm{dt}\end{array}\right.---\left(15\right)$

Wherein,

Be the second derivative of drive motor motion angular velocity, sgn (s) is the sign function about sliding-mode surface s, coefficient c, k, k _fAnd k _wCan by Computer Simulation determine or practical application before test further adjust.

Speed control for mobile robot's drive motor, owing to introduce novel weighted integral gain Reaching Law in the sliding mode controller, its actual angular speed is followed expectation angular velocity fully in 0.015s, this controller output quantity is level and smooth, the actual motion track that has guaranteed spray medicine mobile robot in the greenhouse is followed in the process of desirable operation route can not produce larger fluctuation, has improved greenhouse spraying machine device people track following performance.

7, the controlled quentity controlled variable with left and right sides driving motor drives mobile robot's left and right wheels

Determined the controlled quentity controlled variable of left and right sides driving motor by step 6, see formula (7), this controlled quentity controlled variable is come regulating electric machine speed as the motor servo driver that drives instruction and send to each branch road by the different duty of regulating pwm signal.Direct current generator is input to gear case with rotatablely moving of axle, the output shaft control wheel turns of gear case, and control direct motor drive machine people left and right wheels, thus drive accurate path trace and the location of spraying under the medicine mobile robot greenhouse.

Mobile robot's drive motor velocity-response curve and graph of errors are as shown in Figure 3 and Figure 4; The controller curve of output as shown in Figure 5; Fig. 3-Fig. 5 shows that under the adjusting of weighted integral gain Reaching Law sliding mode controller, system namely reaches steady state (SS) in 0.02m, and the speed tracking error converges to zero; The controller curve of output is more smooth, and the high frequency in the sliding formwork control switches a sgn (s) and do not appear in the controlled quentity controlled variable, can effectively solve the buffeting problem of sliding moding structure with this.Fig. 6 and 7 is respectively robot straight line and circular path tracking results, and Fig. 6 and Fig. 7 show the designed control law of employing this paper, and the mobile robot is the tracing preset path locus preferably, and tracking error goes to zero.

Spray the designed weighted integral gain Reaching Law sliding mode controller of medicine mobile robot for the greenhouse among the present invention, its realization does not rely on the controlled device mathematical models, and to the parameter of greenhouse factor, mobile-robot system change and external interference etc. insensitive, by selecting suitable Reaching Law parameter, can guarantee that the mobile robot departs from actual path in the situation of desirable operation route with the fastest speed convergence expectation route.In addition, this controller output quantity is level and smooth, and then can steadily follow desirable operation route so that spray medicine mobile robot's actual motion track in the greenhouse, therefore can avoid in more serious the respraying and the drain spray phenomenon of operating area generation.

Claims (5)

1. be used for spray medicine mobile robot's track following sliding-mode control, it is characterized in that, the method adopts following steps:

1) mobile robot is carried out analysis of mechanism, set up the moveable robot movement with nonholonomic restriction and learn model;

2) consider that Greenhouse Robot travels and require and design feature and the soil greenhouse factor of effect with it that foundation is with the controlled device mathematical model of each branch controller of wheeled mobile robot of motor driving shaft distracter;

3) utilize computer vision system to identify the border in the zone such as the capable or ridge of crops in greenhouse as walking path, according to the kinematics model that step 1) is derived, determine the desired motion track of each branch road drive motor in realizing spray medicine mobile Robot process;

4) utilize incremental optical-electricity encoder, adopt Measuring Frequency Method that the rotating speed of motor is detected, considering to calculate the actual motion angular velocity omega of mobile robot left and right sides driving motor, the actual motion angular acceleration in the situation that driving wheel trackslips And calculate deviation e and the deviation derivative of each driving motor expectation angular velocity and actual angular speed

5) consider mobile robot's drive motor characteristic and rate request, select suitable sliding-mode surface parameter, make up and satisfy the sliding formwork switching function that the control of drive motor speed requires;

6) on definite sliding-mode surface function s basis, design weighted integral gain Reaching Law is determined the sliding mode controller controlled quentity controlled variable of mobile robot left and right sides driving motor;

7) mobile robot's drive and control of electric machine amount is sent to respectively left and right sides motor driver, realize the track following under the spray medicine mobile robot greenhouse.

2. mould control method according to claim 1 is characterized in that:

In the step 4), consider the situation that greenhouse floor and driving wheel trackslip, actual slippage rate and the theoretical velocity of setting, load and surface state are relevant, and itself and theoretical velocity are approximated to linear relationship and are

σ＝c·ω _s+d

ω＝(1-σ)ω _t

Wherein, σ is the driving wheel slippage rate, ω _sBe theoretical angular velocity, ω is actual angular speed, ω _tBe the encoder feedback value, c and d are coefficient.

3. control method according to claim 1 is characterized in that:

Described step 6) weighted integral gain Reaching Law is in:

$\stackrel{\&CenterDot;}{s}=-\mathrm{ks}-k_w\left|\mathrm{\&rho;}\right|\mathrm{sgn}\left(s\right)$

$\mathrm{\&rho;}={\&Integral;}_0^t(k_f\mathrm{\&rho;}+s)\mathrm{dt},k_f<0$

In the formula, s is sliding formwork control switching function, and ρ is the weighted integral gain term, and sgn (s) is the sign function about sliding-mode surface s, k and k _wThe weighted integral gain Reaching Law parameter that can set, in the expression formula of integration item ρ, k _fThe weighting coefficient of bearing, as ρ〉0 the time, k _fρ＜0, when ρ＜0, k _fρ〉0;

The controlled quentity controlled variable of described mobile robot's driving motor is determined according to following formula:

$\left\{\begin{array}{c}u=-\frac{1}{g\left(x\right)}[c\stackrel{\&CenterDot;}{e}-{\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&omega;}}}_d+f\left(x\right)+g\left(x\right)u+d\left(t\right)+\mathrm{ks}+k_w|\mathrm{\&rho;}\left|\mathrm{sgn}\left(s\right)\right]\\ \mathrm{\&rho;}={\&Integral;}_0^t(k_f\mathrm{\&rho;}+s)\mathrm{dt}\end{array}\right.$

Wherein, u is controller output, is the command voltage (unit is v) that sends to servoamplifier; F (x) and g (x) be with

Motor drive shaft arranges the mathematic(al) representation relevant with motor parameter;

The first order derivative of drive motor angular velocity error;

It is the second derivative of drive motor desired motion angular velocity; D (t) is the uncertain part of Greenhouse Robot system, comprises Parameter Perturbation, load disturbance, sensor measurement error and other external disturbance; S is the sliding-mode surface function; K and k _wIt is the positive weighted integral gain Reaching Law parameter that to set; k _fBe negative weighting coefficient; Sgn (s) is the sign function about sliding-mode surface s.

4. be used for spray medicine mobile robot's track following System with Sliding Mode Controller, formed by car body pose and driving wheel speed mapping block, revolver control system and right cranking wheel control system; It is characterized in that,

Described car body pose and driving wheel speed mapping block, be used for basis with the controlled device mathematical model of each driving wheel controller of wheeled mobile robot of motor driving shaft distracter and the walking path that computer vision system identifies, determine the desired speed track ω of each branch road drive motor in realizing spray medicine mobile Robot process _d, input respectively revolver control system and right cranking wheel control system;

Described revolver control system comprises sliding mode controller, driver, direct current generator, gear case and revolver, also is provided with scrambler between the output terminal of described direct current generator and the input end of described sliding mode controller; Described sliding mode controller sends to driver with controlled quentity controlled variable as driving instruction, driver is regulated DC generator speed by the different duty of regulating pwm signal, direct current generator is input to gear case with rotatablely moving of axle, the output shaft drive machines people revolver of gear case rotates, described scrambler adopts Measuring Frequency Method that the rotating speed of motor is detected, in the situation that the consideration driving wheel trackslips, calculate the actual motion angular velocity omega of mobile robot left and right sides driving motor, the actual motion angular acceleration

And calculate deviation e and the deviation derivative of each driving motor expectation angular velocity and actual angular speed

Input to described sliding mode controller; Described sliding mode controller is according to deviation e, deviation derivative

With sliding-mode surface gain of parameter sliding mode controller switching function s, again according to sliding mode controller switching function s and controlled device mathematical model, determine the controlled quentity controlled variable of mobile robot's driving motor;

The structure of described right cranking wheel control system is identical with the structure of revolver control system.

5. track following System with Sliding Mode Controller according to claim 4 is characterized in that,

The controlled quentity controlled variable of described driving motor is determined according to following formula:

$\left\{\begin{array}{c}u=-\frac{1}{g\left(x\right)}[c\stackrel{\&CenterDot;}{e}-{\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&omega;}}}_d+f\left(x\right)+g\left(x\right)u+d\left(t\right)+\mathrm{ks}+k_w|\mathrm{\&rho;}\left|\mathrm{sgn}\left(s\right)\right]\\ \mathrm{\&rho;}={\&Integral;}_0^t(k_f\mathrm{\&rho;}+s)\mathrm{dt}\end{array}\right.$

Wherein, u is controller output, is the command voltage (unit is v) that sends to servoamplifier; F (x) and g (x) are the mathematic(al) representations relevant with motor parameter with the motor drive shaft setting;

The first order derivative of drive motor angular velocity error;

It is the second derivative of drive motor desired motion angular velocity; D (t) is the uncertain part of Greenhouse Robot system, comprises Parameter Perturbation, load disturbance, sensor measurement error and other external disturbance; S is the sliding-mode surface function; K and k _wIt is the positive weighted integral gain Reaching Law parameter that to set; k _fBe negative weighting coefficient; Sgn (s) is the sign function about sliding-mode surface s.

Images