CN106272436A - A kind of service robot self-adaptation control method based on varying load - Google Patents

Abstract

A kind of service robot self-adaptation control method based on varying load, relate to the adaptive control technology field of service robot, solving existing wheeled service robot, to there is steady-state error big, it is difficult to meet the application scenarios that control accuracy is higher, uses following steps: 1) set up kinetics equation and the Nonholonomic Constraint Equations of service robot；2) mathematical model of service robot is set up；3) according to angular error and angular velocity error, in conjunction with robot system quality, parameter attribute amount is calculated；4) according to parameter attribute amount, in conjunction with control rate, controller output controls the driving motor torque of robot.Use a kind of expert PID self-adaptation control method, rate of change according to error and error adjusts control parameter in time, makes control system operate in optimum state all the time, thus improves service-delivery machine task efficiency, improve the kinetic stability of service robot, improve precision and the motility of system.

Description

A kind of service robot self-adaptation control method based on varying load

Technical field

The present invention relates to the adaptive control technology field of service robot, particularly with regard to wheeled service robot During at Parameter Perturbation with by external disturbance, the most in the course of the work, under load situation of change, the motor control of service robot System strategy and two-wheel differential speed control method.

Background technology

Service robot is as an important branch of robot field, and varied different types of service robot is just It is developed for meeting national economy and the every necks of national defense construction such as family, business, military affairs, national defence, the anti-terrorism disaster relief, medical treatment amusement The demand in territory.Dividing according to move mode, service robot can be divided into the polytypes such as wheeled, lower limb formula, wheel lower limb are hybrid.Wheeled Mobile robot, owing to, in structured environment, efficiency is higher, is widely used in service robot product.

Wheeled service robot is a kind of Nonholonomic Constraints Systems, in the course of the work, due to ground, humidity, temperature, machine The factor impacts such as tool abrasion, operating mode change, cause plant model accurate not, and the problems such as external disturbance is unpredictable make Obtain traditional control method real-time, robustness and feasibility in actual service robot system and all suffer from strong Challenge.At present, many scientific research personnel achieve some achievements in terms of the motor control of wheeled mobile robot.Traditional machine Device people's motor control is frequently with PID controller, and the method is full-fledged, simple and practical.Someone utilizes the ratio of fuzzy tuning micro- Dividing control algolithm, the motion to wheeled robot is controlled.The wheeled mobile robot controlled for two-wheel difference, someone carries Go out control algolithm based on fuzzy logic.Traditional PID control is combined by someone with fuzzy control, by PID control realization The accuracy controlled, utilizes fuzzy control to improve the rapidity controlled.Fuzzy logic control has stronger robustness and rapidity, Although the PID being better than routine controls, but system causes wheeled service robot to be difficult to meet the applied field that control accuracy is higher Scape.

Summary of the invention

In sum, to there is steady-state error big for existing wheeled service robot, it is difficult to meet control accuracy higher should By scene, and a kind of service robot self-adaptation control method based on varying load is proposed.

For solving technical problem proposed by the invention, the technical scheme of employing is: a kind of server based on varying load Device people's self-adaptation control method, it is characterised in that described method employing following steps:

1) kinetics equation and the Nonholonomic Constraint Equations of service robot are set up；

2) mathematical model of service robot is set up；

3) according to angular error and angular velocity error, in conjunction with robot system quality, parameter attribute amount is calculated；

4) according to parameter attribute amount, in conjunction with control rate, controller output controls the driving motor torque of robot.

Described kinetic model equation is as follows:

$M\left(q\right)\stackrel{\·\·}{q}+V(q,\stackrel{\·}{q})\stackrel{\·}{q}+G\left(q\right)=B\left(q\right)\mathrm{\Γ}$

Wherein, q=(x_r,y_r,θ_r)^TFor generalized coordinates；M (q) is symmetric positive definite inertia matrix；For

Centrifugal force and coriolis force matrix；G (q) is gravity influence matrix；B (q) is input transition matrix；Γ is power

Square matrix；

Described Nonholonomic Constraint Equations is as follows:

$A\left(q\right)\stackrel{\·}{q}=0$

Wherein, A (q)=[-sin (θ_r),cos(θ_r), 0], for the matrix relevant to nonholonomic restriction.

Introducing Lagrange operator λ, described mathematical model is expressed as:

$M\left(q\right)\stackrel{\·\·}{q}+V(q,\stackrel{\·}{q})\stackrel{\·}{q}+G\left(q\right)=B\left(q\right)\mathrm{\Γ}+A^T\left(q\right)\mathrm{\λ}$

Parameter attribute is recognized as:

$\left\{\begin{array}{c}e\left(k\right)=(q-\stackrel{\‾}{q})/m\\ \stackrel{\·}{e}\left(k\right)=(\stackrel{\·}{q}-\stackrel{\‾}{\stackrel{\·}{q}})/m\end{array}\right.$

Wherein, m is to include being supported on interior robot total quality；

According to parameter attribute identification, adaptive controller is:

1) as | e (k) | > D_e1, the absolute value of specification error is the biggest, regardless of error change trend, all should examine Considering and allow controller make maximum output, with rapid alignment error, make Error Absolute Value reduce with maximum speed, control rate is:

U (k)=Γ_max；

2) whenTime, specification error is in the direction change increased towards absolute value, or error is certain constant value, not Change.Now, control rate is:

$\begin{array}{c}u\left(k\right)=u(k-1)+\\ k_p\[e\left(k\right)-e(k-1)\]+\\ k_{i}e\left(k\right)+\\ k_d\[e\left(k\right)-2e(k-1)+e(k-2)\]\end{array};$

3)Or during e (k)=0, specification error absolute value becomes towards the direction reduced

Change, or reached poised state.Now, control rate is:

U (k)=u (k-1)；

4) whenTime, specification error is in extreme value state.Now, control rate is:

U (k)=u (k-1)+k_pe(k)；

Comprehensive above four kinds of situations, the control rate of adaptive controller is:

$\begin{array}{c}u\left(k\right)=k_1\·u(k-1)+\\ k_2\·k_p\[e\left(k\right)-e(k-1)\]+\\ k_3\·k_{i}e\left(k\right)+\\ k_3\·k_d\[e\left(k\right)-2e(k-1)+e(k-2)\]\\ +k_4\·{\mathrm{\Γ}}_{\max }\end{array}.$

The invention have the benefit that the present invention compares traditional services robot control strategy, for service-delivery machine Radix Ginseng Number perturbation, or the situation of external disturbance, particularly varying load, carry out feature identification to the rate of change of error and error, use one Planting expert PID self-adaptation control method, parameter tuning is then by Implementation of Expert System, and control strategy is given by PID controller.Expert System on-line tracing and adjustment controller process, adjust control parameter in time according to the rate of change of error and error, make control system System operates in optimum state all the time, thus improves service-delivery machine task efficiency, improves the kinetic stability of service robot, The precision of improvement system and motility.

Accompanying drawing explanation

Fig. 1 is service robot wheeled construction figure of the present invention；

Fig. 2 is service robot control structure block diagram of the present invention.

Detailed description of the invention

Structure below in conjunction with accompanying drawing and the currently preferred specific embodiment present invention is further described.

Present invention includes service robot mathematical model, parameter attribute identification, adaptive controller three part.

Service-delivery machine human occupant dynamic model includes Nonholonomic Constraint Equations, including the robot total quality of load, motor Moment, kinematic parameter and the mechanics parameter such as robot location, speed, acceleration.

Service-delivery machine human occupant dynamic model equation is as follows:

$M\left(q\right)\stackrel{\·\·}{q}+V(q,\stackrel{\·}{q})\stackrel{\·}{q}+G\left(q\right)=B\left(q\right)\mathrm{\Γ}$

Wherein, q=(x_r,y_r,θ_r)^TFor generalized coordinates；M (q) is symmetric positive definite inertia matrix；For centrifugal force and brother Family name's moment battle array；G (q) is gravity influence matrix；B (q) is input transition matrix；Γ is moment matrix.Its nonholonomic restriction is:

The Nonholonomic Constraint Equations of wheeled service robot is as follows:

$A\left(q\right)\stackrel{\·}{q}=0$

Wherein, A (q)=[-sin (θ_r),cos(θ_r), 0], for the matrix relevant to nonholonomic restriction.

Introducing Lagrange operator λ, the mathematical model of wheeled service robot can be expressed as:

$M\left(q\right)\stackrel{\·\·}{q}+V(q,\stackrel{\·}{q})\stackrel{\·}{q}+G\left(q\right)=B\left(q\right)\mathrm{\Γ}+A^T\left(q\right)\mathrm{\λ}$

Parameter attribute is recognized as:

$\left\{\begin{array}{c}e\left(k\right)=(q-\stackrel{\‾}{q})/m\\ \stackrel{\·}{e}\left(k\right)=(\stackrel{\·}{q}-\stackrel{\‾}{\stackrel{\·}{q}})/m\end{array}\right.$

Wherein, m is to include being supported on interior robot total quality.

According to parameter attribute identification, adaptive controller is:

1) as | e (k) | > D_e1, the absolute value of specification error is the biggest, regardless of error change trend, all should examine Considering and allow controller make maximum output, with rapid alignment error, make Error Absolute Value reduce with maximum speed, control rate is:

U (k)=Γ_max；

2) whenTime, specification error is in the direction change increased towards absolute value, or error is certain constant value, not Change.Now, control rate is:

$\begin{array}{c}u\left(k\right)=u(k-1)+\\ k_p\[e\left(k\right)-e(k-1)\]+\\ k_{i}e\left(k\right)+\\ k_d\[e\left(k\right)-2e(k-1)+e(k-2)\]\end{array};$

3)Or during e (k)=0, specification error absolute value changes towards the direction reduced, Or reach poised state.Now, control rate is:

U (k)=u (k-1)；

4) whenTime, specification error is in extreme value state.Now, control rate is:

U (k)=u (k-1)+k_pe(k)；

Comprehensive above four kinds of situations, the control rate of adaptive controller is:

$\begin{array}{c}u\left(k\right)=k_1\·u(k-1)+\\ k_2\·k_p\[e\left(k\right)-e(k-1)\]+\\ k_3\·k_{i}e\left(k\right)+\\ k_3\·k_d\[e\left(k\right)-2e(k-1)+e(k-2)\]\\ +k_4\·{\mathrm{\Γ}}_{\max }\end{array}.$

This patent is described further according to concrete case:

1) according to the wheeled service robot structure chart shown in Fig. 1, the kinetics equation of service robot and non-complete is set up Whole constraint equation；

2) mathematical model of service robot is set up；

3) according to angular error and angular velocity error, in conjunction with robot system quality, parameter attribute amount is calculated；

4) according to parameter attribute amount, in conjunction with control rate, controller output controls the driving motor torque of robot.

Such as two-wheel differential service robot system, service robot total quality within 100kg, the driving moment of motor Within 0.6m/s, D_e1=0.1m, k_p=10, k_i=0.1, k_d=0.8.Service robot in the course of the work, due to m be become Changing, system control rate is:

As | e (k) | > 0.1, u (k)=0.6Nm；

WhenTime, u (k)=u (k-1)+10 [e (k)-e (k-1)]+0.1e (k)+0.8 [e (k)-2e (k- 1)+e(k-2)]；

WhenOr during e (k)=0, u (k)=u (k-1)；

WhenTime, u (k)=u (k-1)+10e (k).

Claims (3)

1. a service robot self-adaptation control method based on varying load, it is characterised in that described method uses following step Rapid:

1) kinetics equation and the Nonholonomic Constraint Equations of service robot are set up；

2) mathematical model of service robot is set up；

3) according to angular error and angular velocity error, in conjunction with robot system quality, parameter attribute amount is calculated；

4) according to parameter attribute amount, in conjunction with control rate, controller output controls the driving motor torque of robot.

A kind of service robot self-adaptation control method based on varying load the most according to claim 1, it is characterised in that:

Described kinetic model equation is as follows:

$M\left(q\right)\stackrel{\·\·}{q}+V(q,\stackrel{\·}{q})\stackrel{\·}{q}+G\left(q\right)=B\left(q\right)\mathrm{\Γ}$

Wherein, q=(x_r,y_r,θ_r)^TFor generalized coordinates；M (q) is symmetric positive definite inertia matrix；For centrifugal force and coriolis force Matrix；G (q) is gravity influence matrix；B (q) is input transition matrix；Γ is moment matrix；

Described Nonholonomic Constraint Equations is as follows:

$A\left(q\right)\stackrel{\·}{q}=0$

Wherein, A (q)=[-sin (θ_r),cos(θ_r), 0], for the matrix relevant to nonholonomic restriction.

A kind of service robot self-adaptation control method based on varying load the most according to claim 1 and 2,

It is characterized in that: introducing Lagrange operator λ, described mathematical model is expressed as:

$M\left(q\right)\stackrel{\·\·}{q}+V(q,\stackrel{\·}{q})\stackrel{\·}{q}+G\left(q\right)=B\left(q\right)\mathrm{\Γ}+A^T\left(q\right)\mathrm{\λ}$

Parameter attribute is recognized as:

$\left\{\begin{array}{c}e\left(k\right)=(q-\stackrel{\‾}{q})/m\\ \stackrel{\·}{e}\left(k\right)=(\stackrel{\·}{q}-\stackrel{\‾}{\stackrel{\·}{q}})/m\end{array}\right.$

Wherein, m is to include being supported on interior robot total quality；

According to parameter attribute identification, adaptive controller is:

1) as | e (k) | > D_e1, the absolute value of specification error is the biggest, regardless of error change trend, all it is also contemplated that allow Maximum output made by controller, with rapid alignment error, makes Error Absolute Value reduce with maximum speed, and control rate is:

U (k)=Γ_max；

2) whenTime, specification error is in the direction change increased towards absolute value, or error is certain constant value, does not occurs Change.Now, control rate is:

$\begin{array}{c}u\left(k\right)=u(k-1)+\\ k_p\[e\left(k\right)-e(k-1)\]+\\ k_{i}e\left(k\right)+\\ k_d\[e\left(k\right)-2e(k-1)+e(k-2)\]\end{array};$

3)Or during e (k)=0, specification error absolute value changes towards the direction reduced, or Reach poised state.Now, control rate is:

U (k)=u (k-1)；

4) whenTime, specification error is in extreme value state.Now, control rate is:

U (k)=u (k-1)+k_pe(k)；

Comprehensive above four kinds of situations, the control rate of adaptive controller is:

$\begin{array}{c}u\left(k\right)=k_1\·u(k-1)+\\ k_2\·k_p\[e\left(k\right)-e(k-1)\]+\\ k_3\·k_{i}e\left(k\right)+\\ k_3\·k_d\[e\left(k\right)-2e(k-1)+e(k-2)\]\\ +k_4\·{\mathrm{\Γ}}_{\max }\end{array}.$