CN105382841B - A kind of remote operating bilateral control method communicated based on the dual pathways - Google Patents

Abstract

The invention discloses a kind of remote operating bilateral control method communicated based on the dual pathways, is comprised the following steps：1) main handss and the kinetic model from handss are set up, and 2) are set up from bracelet border and operator's model, 3) system closed loop power model 4) are drawn at main handss end and from handss end introducing controller respectively；The present invention is required for four communication channels and comes while transmitting position and force information in main handss and from handss end, and need to only adopt dual pathways communication meet the requirement of control system stability contorting, so as to reach simplified control system method for designing, the cost such as valuable communication bandwidth during saving remote operating；In addition, the present invention can play the control method to the uncertain adaptation sexual clorminance of ambient parameter, so that bilateral control system has more preferable capacity of resisting disturbance in the controls by introducing impedance/combination control method.

Description

A kind of remote operating bilateral control method communicated based on the dual pathways

【Technical field】

The invention belongs to teleoperation field, is related to bilateral control and Technology of ZO control, it is specifically a kind of based on double The remote operating bilateral control method of channel communications.

【Background technology】

In the medical field, medical worker can be made across apart from upper restriction, being patient's exclusion by teleoperation Difficult miscellaneous diseases are even performed the operation；In the rescue under hazardous environment, remote operating can avoid fireman from directly participating in danger Rescue under environment, so as to ensure the life security of fireman, while also contributing to make up fire-fighting robot degree of intelligence inadequate And be difficult to deal with the autonomous rescue problem of complicated unstructured moving grids.Remote operating process proposes very high request to operator, And the stability in its operating process and the transparency are particularly subject to the concern of researcher.Typical Space teleoperation system typically by Five parts constitute, including operator namely main handss, main handss operation equipment, communication channel, from handss, and from handss local environment.It is distant The main purpose of operation control system design is exactly to make system have the preferably transparency and stability.It is widely used at present Remote control system controller design and analysis are four-way control structures, i.e., main handss and from simultaneously transmit between handss position and Force signal, although this structure can make system obtain the preferably transparency, system design is complex.Additionally, in four-way Assume in the control system in road that system is preferable linear system, but during actual remote operating, main handss and between handss Generally there are complicated coupling nonlinear kinetics, it is assumed that be not necessarily within zone of reasonableness, so as to cause to system design Certain difficulty.

【The content of the invention】

Deficiency of the present invention for existing four-way remote operating control system, using Technology of ZO control, proposes a kind of Based on the remote operating bilateral control method that the dual pathways is communicated, the method adopts dual pathways communication modes, will move desired position Information is passed to from handss by main handss, and the force information with environmental interaction is fed back to main handss and operator from handss.Additionally, system design In also contemplate the parameter uncertainty and environmental disturbances of operating environment.Compare existing four-way remote operating control system to have more Good Lu Bangxing, can better adapt to the uncertainty of ambient parameter, while with higher capacity of resisting disturbance.

To achieve these goals, the present invention is employed the following technical solutions and is achieved：

A kind of remote operating bilateral control method communicated based on the dual pathways, is comprised the following steps：

1) main handss and the kinetic model from handss are set up

Consideration end effector does translational motion, and Manipulator Dynamic is expressed as in cartesian space coordinate system

Wherein, X ∈ R³Represent mechanical arm tail end position, q ∈ RⁿRepresent each joint angle of mechanical arm, D_x(q)∈R^3×3Expression machine Tool arm inertia matrix,Including Coriolis, centrifugal force and gravity, F suffered by mechanical arm_e∈R³Represent robot end To environment force, F_dist∈R³Expression acts on the external disturbance power above executor；F∈R³It is control input, according to resistance Anti-/direct union control method, impedance control are restrained and are：

Wherein, K_pAnd K_iIt is diagonal matrix；WithIt is D respectively_xAnd H_xEstimated value；And have：

Hypothesis environment is a linear quality spring-damp system；Under Constrained moving condition, reference locus X_rCan be written as：

K_eq=K_t(K_e+K_t)^-1K_e (5)

Wherein, X_eo∈R³Represent object of study initial position in the environment, K_e∈R^3×3Represent environment rigidity coefficient square Battle array, is diagonal matrix, F_d∈R³Represent expected force, M_t,B_t,K_t∈R³It is the inertia of institute's object of study, damping, stiffness coefficient square Battle array；The impedance of object of study is defined as：

And have：

E_x=X_r-X (7)

E_f=F_d-F_e (8)

According to (1) formula, ignore its exterior perturbed force, set up main handss and the kinetic model from handss respectively

Wherein, D_xmAnd D_xsMain handss and the inertia matrix from handss, H are represented respectively_xmAnd H_xsRepresent that main handss are corresponding with from handss respectively In the H of (1) formula_x, X_mAnd X_sMain handss and the position from handss, F are represented respectively_hAnd F_eRepresent that operator is applied to respectively to lead on hand Power, the power that environment is applied to from handss, F_mcAnd F_scMain handss and the control input amount from handss are represented respectively；

2) set up from bracelet border and operator's model

Environmental model Linear Time Invariant quality spring-damp system model is represented

F_e(s)=(M_es²+B_es+K_e)(X_s(s)-X_eo(s)) (11)

M_e, B_eAnd K_e∈R^3×3It is diagonal matrix, represents the quality of environmental model, damping, stiffness coefficient matrix respectively；Will behaviour Author is also equivalent to Linear Time Invariant quality spring-damp system, and its kinetic model can be expressed as in frequency domain：

Here, V_hThe chirokinesthetic speed of operator operation master is represented,It is power that operator is applied to main handss；Z_hIt is operation The impedance matrix of person, is defined as：

M_h, B_hAnd K_h∈R^3×3The quality of operator, damping and stiffness coefficient matrix are represented respectively；

3) respectively at main handss end and from handss end introducing controller

Under conditions of Constrained motion, F is used_edThe expected force in the environment from handss effect is represented, then has F_ed=F_h；According to (2) formula, is introducing following controller from handss end

Wherein：

E_fs=F_h-F_e (15)

E_xs=X_rs-X_s (17)

Various middle subscript s represents the parameter item from handss above；

For main handss end, X is used_mdThe desired locations of main handss are represented, then has X_md=X_s, using a kind of based on site error Controller compensation controling power：

Wherein：

E_ms=X_s-X_m (20)

Subscript m represents main handss, diagonal matrix K_vm, K_pmAnd K_imIt is the control parameter of main handss；

4) draw system closed loop power model

By comparison expression (9) and formula (19), formula (10) and formula (14), system closed loop power equation is obtained；

For main handss end has：

For having from handss end：

Laplace transform is taken respectively to formula (21) and formula (22), then main handss end and the closed loop power from handss end are in frequency domain It is expressed as：

The component form of homography in lowercase letter formula (21) and formula (22).

Compared with prior art, the invention has the advantages that：

Control method proposed by the present invention is required for four communication channels and comes while transmitting position in main handss and from handss end And force information, and need to only adopt dual pathways communication meet the requirement of control system stability contorting, so as to reach simplify control Design method, the cost such as valuable communication bandwidth during saving remote operating；In addition, the present invention passes through in the controls Impedance/combination control method is introduced, the control method can be played to the uncertain adaptation sexual clorminance of ambient parameter, so that double Side control system has more preferable capacity of resisting disturbance.

【Description of the drawings】

Fig. 1 is the main handss of analog representation and the slide bar mechanical construction drawing from handss；

Fig. 2 is bilateral control system simulation module figure.

【Specific embodiment】

The present invention will be further described in detail with reference to the accompanying drawings and examples.

The impedance arrived used in the present invention/direct union control method.

Consideration end effector does translational motion, and general Manipulator Dynamic is in cartesian space coordinate system Can be expressed as

Wherein, X ∈ R³Represent mechanical arm tail end position, q ∈ RⁿRepresent each joint angle of mechanical arm, D_x(q)∈R^3×3Expression machine Tool arm inertia matrix,Including Coriolis, centrifugal force and gravity, F suffered by mechanical arm_e∈R³Represent robot end To environment force, F_dist∈R³Expression acts on the external disturbance power above executor.F∈R³It is control input, according to resistance Anti-/direct union control method, impedance control are restrained and are

Wherein, K_pAnd K_iIt is diagonal matrix.WithIt is D respectively_xAnd H_xEstimated value.And have

Here, suppose that environment is a linear quality spring-damp system.Under Constrained moving condition, reference locus X_rCan It is written as

K_eq=K_t(K_e+K_t)^-1K_e (5)

Wherein X_eo∈R³Represent object of study initial position in the environment, K_e∈R^3×3Environment rigidity coefficient matrix is represented, For diagonal matrix, F_d∈R³Represent expected force, M_t,B_t,K_t∈R³It is the inertia of institute's object of study, damping, stiffness coefficient matrix.Will The impedance of object of study is defined as

And have

E_x=X_r-X (7)

E_f=F_d-F_e (8)

This impedance/direct union control method combines the advantage of impedance control and side-jet control method.By giving It is suitably introduced in impedance controller based on PI type controller balancing force errors, can preferably improves the uncertainty of model and outer Portion disturbs the impact to system, while also having played the advantage of side-jet control method.

Referring to Fig. 1 and Fig. 2, the present invention is comprised the following steps：

The first step：Set up equivalent simulation system

In order to illustrate the present invention application flow, set up slide bar frame for movement as shown in Figure 1, here by main handss end and from Handss end simplifies and is equivalent to one and has main handss and from handss single-degree-of-freedom sliding bar mechanism, and coordinate system and kinesiology variable are as shown in Figure 1. Coulomb friction power and viscous force are not considered, the kinetics equation of slide bar can be expressed as

Second step：Set each parameter item

Wherein each parameter is set to：

D (q)=(7.10cos²q+2.58)×10^-4kgm²,Condition impedance Parameter is set to m_e=0.14kg, K_e=400N/m, b_e=1；Each parameter of main handss is set to k_pm=1.8 × 10⁴,k_vm=10, k_im= 10⁴, x is set to from the initial position of handss_eo=122.32mm, is set to k from each parameter of handss_ps=90, k_is=80, m_ts=1, b_ts=10.

3rd step：Set up Simulink analogue systems to be verified

Kinetics equation and above-mentioned parameter according to main handss in the content of the invention and from handss, sets up such as Fig. 2 in Simulink Shown bilateral control analogue system, simulation result show that main handss preferably can be tracked from the environmental forces suffered by handss, average to miss Difference can also preferably track main hand position from hand position less than 0.1N, and mean error is less than 1mm.The dual pathways is reached Communication obtains the purpose of the remote control system transparency and stability.

Above content technological thought only to illustrate the invention, it is impossible to which protection scope of the present invention is limited with this, it is every to press According to technological thought proposed by the present invention, any change done on the basis of technical scheme, claims of the present invention is each fallen within Protection domain within.

Claims (1)

1. it is a kind of based on the dual pathways communicate remote operating bilateral control method, it is characterised in that comprise the following steps：

1) main handss and the kinetic model from handss are set up

Consideration end effector does translational motion, and Manipulator Dynamic is expressed as in cartesian space coordinate system

$D_x\left(q\right)\stackrel{\·\·}{X}+H_x(q,\stackrel{\·}{q})=F-F_e+F_{dist}---\left(1\right)$

Wherein, X ∈ R³Represent mechanical arm tail end position, q ∈ RⁿRepresent each joint angle of mechanical arm, D_x(q)∈R^3×3Represent mechanical arm Inertia matrix,Including Coriolis, centrifugal force and gravity, F suffered by mechanical arm_e∈R³Represent robot end to environment Active force, F_dist∈R³Expression acts on the external disturbance power above executor；F∈R³It is control input, according to impedance/directly Combination control method, impedance control are restrained and are：

$F={\hat{D}}_x(U+K_p{E}_f+K_i{\∫}_0^t{E}_{f}dt)+{\hat{H}}_x+F_e---\left(2\right)$

Wherein, K_pAnd K_iIt is diagonal matrix；WithIt is D respectively_xAnd H_xEstimated value；And have：

$U={\stackrel{\·\·}{X}}_r+M_t^{-1}(B_t{\stackrel{\·}{E}}_x+K_t{E}_x-F_e)---\left(3\right)$

Hypothesis environment is a linear quality spring-damp system；Under Constrained moving condition, reference locus X_rCan be written as：

$X_r=X_{eo}+K_{eq}^{-1}{F}_d---\left(4\right)$

K_eq=K_t(K_e+K_t)^-1K_e (5)

Wherein, X_eo∈R³Represent object of study initial position in the environment, K_e∈R^3×3Environment rigidity coefficient matrix is represented, is Diagonal matrix, F_d∈R³Represent expected force, M_t,B_t,K_t∈R³It is the inertia of institute's object of study, damping, stiffness coefficient matrix；To grind The impedance for studying carefully object is defined as：

$M_t{\stackrel{\·\·}{E}}_x+B_t{\stackrel{\·}{E}}_x+K_t{E}_x=F_e---\left(6\right)$

And have：

E_x=X_r-X (7)

E_f=F_d-F_e (8)

According to (1) formula, ignore its exterior perturbed force, set up main handss and the kinetic model from handss respectively

$F_{mc}=D_{xm}{\stackrel{\·\·}{X}}_m+H_{xm}-F_h---\left(9\right)$

$F_{sc}=D_{xs}{\stackrel{\·\·}{X}}_s+H_{xs}+F_e---\left(10\right)$

Wherein, D_xmAnd D_xsMain handss and the inertia matrix from handss, H are represented respectively_xmAnd H_xsMain handss are represented respectively and are corresponded to from handss (1) H of formula_x, X_mAnd X_sMain handss and the position from handss, F are represented respectively_hAnd F_eRepresent respectively operator be applied to main power on hand, The power of environment, F are applied to from handss_mcAnd F_scMain handss and the control input amount from handss are represented respectively；

2) set up from bracelet border and operator's model

Environmental model Linear Time Invariant quality spring-damp system model is represented

F_e(s)=(M_es²+B_es+K_e)(X_s(s)-X_eo(s)) (11)

M_e, B_eAnd K_e∈R^3×3It is diagonal matrix, represents the quality of environmental model, damping, stiffness coefficient matrix respectively；By operator Also Linear Time Invariant quality spring-damp system is equivalent to, its kinetic model can be expressed as in frequency domain：

$F_h\left(s\right)=F_h^{*}\left(s\right)-Z_h{V}_h\left(s\right)---\left(12\right)$

Here, V_hThe chirokinesthetic speed of operator operation master is represented,It is power that operator is applied to main handss；Z_hIt is operator Impedance matrix, is defined as：

$Z_h=M_{h}s+B_h+\frac{K_h}{s}---\left(13\right)$

M_h, B_hAnd K_h∈R^3×3The quality of operator, damping and stiffness coefficient matrix are represented respectively；

3) respectively at main handss end and from handss end introducing controller

Under conditions of Constrained motion, F is used_edThe expected force in the environment from handss effect is represented, then has F_ed=F_h；According to (2) Formula, is introducing following controller from handss end

$F_{sc}=D_{xs}(U_s+K_{ps}{E}_{fs}+K_{is}{\∫}_0^t{E}_{fs}dt)+H_{xs}+F_e---\left(14\right)$

Wherein：

E_fs=F_h-F_e (15)

$U_s=M_{ts}^{-1}(B_{ts}{\stackrel{\·}{E}}_{xs}+K_{ts}{E}_{xs}-F_e)---\left(16\right)$

E_xs=X_rs-X_s (17)

$X_{rs}=X_{eo}+K_e^{-1}(K_e+K_{ts})K_{ts}^{-1}{F}_h---\left(18\right)$

Various middle subscript s represents the parameter item from handss above；

For main handss end, X is used_mdThe desired locations of main handss are represented, then has X_md=X_s, using a kind of control based on site error Device compensates controling power：

$F_{mc}=D_{xm}\[{\stackrel{\·\·}{X}}_s+K_{vm}{\stackrel{\·}{E}}_{ms}+K_{pm}{E}_{ms}+K_{im}{\∫}_0^t{E}_{ms}dt\]+H_{xm}-F_h---\left(19\right)$

Wherein：

E_ms=X_s-X_m (20)

Subscript m represents main handss, diagonal matrix K_vm, K_pmAnd K_imIt is the control parameter of main handss；

4) draw system closed loop power model

By comparison expression (9) and formula (19), formula (10) and formula (14), system closed loop power equation is obtained；

For main handss end has：

${\stackrel{\·\·}{X}}_s-{\stackrel{\·\·}{X}}_m+K_{vm}({\stackrel{\·}{X}}_s-{\stackrel{\·}{X}}_m)+K_{pm}(X_s-X_m)+K_{im}{\∫}_0^t(X_s-X_m)dt=0---\left(21\right)$

For having from handss end：

$\begin{array}{c}(M_{ts}^{-1}{K}_{ts}{K}_{eqs}^{-1}+K_{ps})(F_h-F_e)+K_{is}{\∫}_0^t(F_h-F_e)dt\\ =M_{ts}^{-1}(M_{ts}+M_e-K_{ts}{K}_{eqs}^{-1}{M}_e){\stackrel{\·\·}{X}}_s+M_{ts}^{-1}(B_{ts}+B_e-K_{ts}{K}_{eqs}^{-1}{B}_e){\stackrel{\·}{X}}_s\end{array}---\left(22\right)$

Laplace transform is taken respectively to formula (21) and formula (22), then main handss end and the closed loop power from handss end are distinguished in frequency domain It is expressed as：

$\begin{array}{c}(X_s-X_m)s^2+k_{vm}(X_s-X_m)s+k_{pm}(X_s-X_m)+k_{im}(X_s-X_m)\frac{1}{s}=0---\left(23\right)\\ \begin{array}{c}\left(\frac{k_{ts}}{m_{ts}{k}_{eqs}}+k_{ps}+\frac{k_{is}}{s}\right)(F_h-F_e)\\ =\left(1+\frac{m_e}{m_{ts}}-\frac{m_e{k}_{ts}}{m_{ts}{k}_{eqs}}\right)s^2{X}_s+\left(\frac{b_{ts}}{m_{ts}}+\frac{b_e}{m_{ts}}-\frac{b_e{k}_{ts}}{m_{ts}{k}_{eqs}}\right){\mathrm{sX}}_s\end{array}---\left(24\right)\end{array}$

The component form of homography in lowercase letter formula (21) and formula (22).