CN103433924A

CN103433924A - High-accuracy position control method for serial robot

Info

Publication number: CN103433924A
Application number: CN2013103219800A
Authority: CN
Inventors: 白瑞林; 闫文才; 李新; 吉峰
Original assignee: XINJE ELECTRONIC CO Ltd; Jiangnan University
Current assignee: XINJE ELECTRONIC CO Ltd; Jiangnan University
Priority date: 2013-07-26
Filing date: 2013-07-26
Publication date: 2013-12-11

Abstract

Provide an improved serial robot control method: no need to know the specific mathematical model of the controlled object; it has strong robustness and high tracking accuracy; and improves the torque jump and speed jump caused by a wide range of initial pose deviations question. The sliding mode method based on the calculation moment method is adopted to ensure the strong robustness in the control; the exponential reaching law is introduced to eliminate the chattering problem in the sliding mode control; Estimate the sliding mode switching gain, enhance its adaptability to uncertain factors, and eliminate the chattering phenomenon of the output torque in sliding mode control; use another fuzzy adaptive controller to correct the coefficient of the exponential reaching law, To improve the large moment and velocity jump problems caused by a large range of initial pose deviations.

Description

High precision position control method for tandem robot

技术领域technical field

本发明涉及串联机器人的位置控制领域，具体是指一种通过模糊自适应滑模控制方法实现对串联机器人的高精度位置跟踪以及机器人启动时力矩和速度跳变问题的改善方法。The invention relates to the field of position control of series robots, in particular to a fuzzy self-adaptive sliding mode control method to realize high-precision position tracking of series robots and an improvement method for torque and speed jump problems when the robots start.

背景技术Background technique

机器人技术是集机构学、电子技术、计算机技术、传感技术、控制论、人工智能和仿生学等多学科于一体的高新技术。Robotics is a high-tech that integrates multiple disciplines such as mechanism, electronic technology, computer technology, sensor technology, cybernetics, artificial intelligence and bionics.

机器人位置控制是机器人技术的一个重要领域。工业机器人是一个复杂的多输入多输出的非线性系统，具有强耦合、时变以及非线性等动力学特性，其控制过程复杂。由于机器人参数测量与建模的不精确，加上机器人负载以及工业外部干扰的不确定性，实际中无法获取机器人完整、精确的对象模型，工业机器人的特定应用环境，决定它必须面对各种不确定因素的存在。Robot position control is an important field of robotics. Industrial robot is a complex multi-input and multi-output nonlinear system with strong coupling, time-varying and nonlinear dynamic characteristics, and its control process is complex. Due to the imprecise measurement and modeling of robot parameters, coupled with the uncertainty of robot load and external industrial interference, it is impossible to obtain a complete and accurate object model of the robot in practice. The specific application environment of industrial robots determines that it must face various The existence of uncertain factors.

对于机器人来说，其控制器设计分为两类：一类是按照机器人实际轨迹与期望轨迹间的偏差进行负反馈控制。这类方法称为“运动控制”，主要优点是控制律简单，易于实现。但对于控制高速高精度机器人来说，这类方法有两个明显的缺点：一是难于保证受控机器人具有良好的动态和静态品质；二是需要较大的控制能量。另一类控制器设计称为“动态控制”。这类方法是根据机器人动力学模型的性质设计出更精细的非线性控制律，所以又常称为“以模型为基础的控制”。用动态控制方法设计的控制器可使被控机器人具有良好的动态和静态品质，克服了运动控制方法的缺点。For the robot, its controller design is divided into two categories: one is to carry out negative feedback control according to the deviation between the actual trajectory and the expected trajectory of the robot. This type of method is called "motion control", and the main advantage is that the control law is simple and easy to implement. However, for controlling high-speed and high-precision robots, this method has two obvious disadvantages: one is that it is difficult to ensure that the controlled robot has good dynamic and static qualities; the other is that it requires a large amount of control energy. Another class of controller designs is called "dynamic control." This type of method is based on the nature of the robot dynamics model to design a more sophisticated nonlinear control law, so it is often called "model-based control". The controller designed by the dynamic control method can make the controlled robot have good dynamic and static qualities, and overcome the shortcomings of the motion control method.

滑模控制不需要知道被控对象的数学模型，但控制中容易出现斗振问题，为了进一步提高滑模控制效果，可以采用自适应模糊滑模控制，自适应调节滑模控制的增益，增强对随机不确定性的适应能力，来消除在滑模控制中的输入抖振现象。但值得关注的是，在跟踪误差突变时控制器的大力矩和速度跳变问题，给实际的机器人控制带来很大弊端，非常容易损坏各关节的伺服电机。Sliding mode control does not need to know the mathematical model of the controlled object, but the bucket vibration problem is easy to appear in the control. In order to further improve the effect of sliding mode control, adaptive fuzzy sliding mode control can be used to adaptively adjust the gain of sliding mode control to enhance the control effect. Adaptability to stochastic uncertainties to eliminate input chattering in sliding mode control. However, it is worth noting that the large torque and speed jump of the controller when the tracking error changes suddenly brings great disadvantages to the actual robot control, and it is very easy to damage the servo motors of each joint.

发明内容Contents of the invention

本发明的目的在于基于双模糊自适应滑模控制技术，设计一种跟踪效果好、速度输出平滑的机器人位置控制算法。很好地改善由于大的初始位姿产生的偏差而引起的大力矩和速度的跳变问题。The purpose of the present invention is to design a robot position control algorithm with good tracking effect and smooth speed output based on double-fuzzy self-adaptive sliding mode control technology. It can well improve the problem of large torque and speed jump caused by the deviation caused by the large initial pose.

为达到此目的，本发明的技术方案如下：基于计算力矩法的滑模控制技术，建立机器人的连杆坐标系，获取它的D-H参数，得到机器人的动力学方程。根据D-H参数估算各关节的惯性力项、哥氏力项和重力项，最后得出各关节的力矩估算公式。通过各关节的位置误差建立滑模面，利用基于计算力矩法的滑模控制技术来进行各关节的位置控制。为了减少滑模控制中的抖振现象，加入了指数趋近律，对于其中的滑模切换增益K采用自适应模糊控制在线进行估计。为了减弱大的初始偏差带来的力矩跳变和速度跳变问题，采用另一个模糊控制来估算指数趋近律的系数A，确定最优参数。整个流程包括：动力学估算模块、建立滑模面模块、滑模切换增益估算模块、指数趋近律估算模块、控制力矩计算模块。To achieve this goal, the technical solution of the present invention is as follows: based on the sliding mode control technology of the calculated moment method, the link coordinate system of the robot is established, its D-H parameters are obtained, and the dynamic equation of the robot is obtained. The inertial force term, Coriolis force term and gravity term of each joint are estimated according to the D-H parameters, and finally the torque estimation formula of each joint is obtained. The sliding mode surface is established through the position error of each joint, and the position control of each joint is carried out by using the sliding mode control technology based on the calculation torque method. In order to reduce the chattering phenomenon in the sliding mode control, the exponential reaching law is added, and the sliding mode switching gain K is estimated online by adaptive fuzzy control. In order to weaken the torque jump and speed jump problems caused by the large initial deviation, another fuzzy control is used to estimate the coefficient A of the exponential reaching law and determine the optimal parameters. The whole process includes: a dynamics estimation module, a sliding mode surface establishment module, a sliding mode switching gain estimation module, an exponential reaching law estimation module, and a control moment calculation module.

第一步，建立机器人各连杆坐标系，确定各连杆的D-H参数(a_i，α_i，d_i，θ_i)。由拉格朗日方程：

i＝1，2，...，n，推导出动力学方程：

T_{i} = Σ_{j = 1}^{n} D_{ij} {\overset{\cdot \cdot}{q}}_{j} + I_{ai} {\overset{\cdot \cdot}{q}}_{i} + Σ_{j = 1}^{6} Σ_{k = 1}^{6} D_{ijk} {\overset{\cdot}{q}}_{j} {\overset{\cdot}{q}}_{k} + D_{i}

The first step is to establish the coordinate system of each link of the robot, and determine the DH parameters (a _i , α _i , d _i , θ _i ) of each link. From the Lagrange equation:

i=1, 2,..., n, derive the kinetic equation:

T_{i} = Σ_{j = 1}^{no} {D.}_{ij} {\overset{\cdot \cdot}{q}}_{j} + I_{ai} {\overset{&Center Dot; &Center Dot;}{q}}_{i} + Σ_{j = 1}^{6} Σ_{k = 1}^{6} {D.}_{ijk} {\overset{\cdot}{q}}_{j} {\overset{\cdot}{q}}_{k} + {D.}_{i}

根据动力学方程估算出惯性力项、哥氏力项和重力项最后得出各轴的力矩估算公式： $\hat{τ} = \hat{H} {\overset{\cdot \cdot}{q}}_{r} + \hat{C} {\overset{\cdot}{q}}_{r} + \hat{G} .$ Estimate the inertial force term, Coriolis force term and gravity term according to the dynamic equation Finally, the torque estimation formula of each axis is obtained: $\hat{τ} = \hat{h} {\overset{&Center Dot; \cdot}{q}}_{r} + \hat{C} {\overset{\cdot}{q}}_{r} + \hat{G} .$

第二步，通过计算各关节的位置误差e和误差变化率

建立滑模面

其中Λ＝diag[λ₁，…λ_l…λ_n]，λ_l＞0。In the second step, by calculating the position error e and error change rate of each joint

Create a sliding surface

Wherein Λ=diag[λ ₁ , . . . λ _l . . . λ _n ], λ _l >0.

并定义： ${\overset{\cdot}{q}}_{r} = \overset{\cdot}{q} - s = {\overset{\cdot}{q}}_{d} - Λe, {\overset{\cdot \cdot}{q}}_{r} = \overset{\cdot \cdot}{q} - \overset{\cdot}{s} = {\overset{\cdot \cdot}{q}}_{d} - Λ \overset{\cdot}{e}$ and define: ${\overset{&Center Dot;}{q}}_{r} = \overset{\cdot}{q} - the s = {\overset{\cdot}{q}}_{d} - Λ e, {\overset{\cdot \cdot}{q}}_{r} = \overset{&Center Dot; &Center Dot;}{q} - \overset{&Center Dot;}{the s} = {\overset{&Center Dot; \cdot}{q}}_{d} - Λ \overset{\cdot}{e}$

设计控制律为： $τ = \hat{τ} - As - Ksgns,$ $\hat{τ} = \hat{H} {\overset{\cdot \cdot}{q}}_{r} + \hat{C} {\overset{\cdot}{q}}_{r} + \hat{G}$ The design control law is: $τ = \hat{τ} - As - Ksgns,$ $\hat{τ} = \hat{h} {\overset{\cdot \cdot}{q}}_{r} + \hat{C} {\overset{&Center Dot;}{q}}_{r} + \hat{G}$

其中，

为等效控制，As为指数趋近律，K sgns为切换控制。

分别为H，C，G的估计值，K＝diag[K₁₁，…K_ii，…K_nn]、A＝diag[a₁，…，a_i，…，a_n]为正定矩阵。in,

is the equivalent control, As is the exponential reaching law, and K sgns is the switching control.

are estimated values of H, C, and G respectively, K=diag[K ₁₁ ,...K _ii ,...K _nn ], A=diag[a ₁ ,...,a _i ,..., _an ] are positive definite matrices.

第三步，用模糊控制自适应逼近滑模控制律的增益K。采用乘积推理机、单值模糊器和中心平均解模糊器来设计模糊控制系统，系统的控制输出为：The third step is to use fuzzy control to adaptively approximate the gain K of the sliding mode control law. The fuzzy control system is designed by using product reasoning machine, single value fuzzer and central average defuzzifier. The control output of the system is:

${k k}_{i i} = = \frac{{Σ Σ}_{m m = = 11}^{M m} {ϵ ϵ}_{{k k}_{i i}}^{m m} {μ μ}_{{A A}^{m m}} (({S S}_{i i}))}{{Σ Σ}_{m m = = 11}^{M m} {μ μ}_{{A A}^{m m}} (({S S}_{i i}))} = = {ϵ ϵ}_{{k k}_{i i}}^{T T} {Ψ Ψ}_{{k k}_{i i}} (({S S}_{i i})),,$

用于表示模糊集的隶属函数设计为：The membership functions used to represent fuzzy sets are designed as:

${μ μ}_{A A} (({x x}_{i i})) = = exp exp [[- - {((\frac{{x x}_{i i} - - α α}{σ σ}))}^{22}]]$

选取自适应律为： Choose the adaptive law as:

第四步，对滑模控制的指数控制项系数进行模糊控制，通过调节A来达到：当误差及误差变化率大的时候尽量减小控制量；反之，则增加控制量。从而保留原控制算法好的跟踪效果，同时改善机器人启动时大力矩和速度跳变问题。The fourth step is to perform fuzzy control on the coefficient of the exponential control item of the sliding mode control, and adjust A to achieve: when the error and the error rate of change are large, the control amount should be reduced as much as possible; otherwise, the control amount should be increased. In this way, the good tracking effect of the original control algorithm is preserved, and at the same time, the problem of large torque and speed jump when the robot starts is improved.

第五步，将上面几部分组合，关节的力矩控制输入为：The fifth step is to combine the above parts, and the torque control input of the joint is:

$τ τ = = \overset{^^}{τ τ} - - As As - - Ksgns Ksgns,, \overset{^^}{τ τ} = = \overset{^^}{H h} {\overset{\cdot &Center Dot; \cdot &Center Dot;}{q q}}_{r r} + + \overset{^^}{C C} {\overset{\cdot &Center Dot;}{q q}}_{r r} + + \overset{^^}{G G} . .$

其中，

分别为H，C，G的估计值，s为滑模面，通过第三步的自适应滑模控制方法来估算参数K，利用第四步的滑模控制方法来估算参数A。in,

are the estimated values of H, C, and G respectively, and s is the sliding mode surface. The parameter K is estimated by the adaptive sliding mode control method in the third step, and the parameter A is estimated by the sliding mode control method in the fourth step.

本发明的有益效果：提供了一种基于双模糊自适应滑模的机器人位置控制方法，用于提高串联机器人跟踪精度并改善力矩、速度跳变问题。滑模控制本质上是一类特殊的非线性控制，因具有强鲁棒性而成为一种有效的控制方法；在滑模控制的基础上引入指数趋近律，有效地消除抖振问题；采用一个自适应模糊控制器，根据滑模到达条件对滑模切换增益进行估算，增强其对不确定性因素的适应能力，消除了在滑模控制中输出力矩的抖振现象；采用另一个模糊自适应控制器对指数趋近律的系数进行修正，来改善由于大范围的初始位姿偏差变化而引起的大力矩和速度跳变问题。The beneficial effect of the present invention is to provide a robot position control method based on double-fuzzy adaptive sliding mode, which is used to improve the tracking accuracy of serial robots and improve the problems of torque and speed jumps. Sliding mode control is essentially a special kind of nonlinear control, and it has become an effective control method because of its strong robustness; on the basis of sliding mode control, an exponential reaching law is introduced to effectively eliminate the chattering problem; An adaptive fuzzy controller estimates the sliding mode switching gain according to the sliding mode arrival condition, enhances its adaptability to uncertain factors, and eliminates the chattering phenomenon of the output torque in the sliding mode control; another fuzzy automatic The adaptive controller modifies the coefficients of the exponential reaching law to improve the problem of large torque and velocity jumps caused by a wide range of initial pose deviation changes.

附图说明Description of drawings

图1连杆坐标系示意图；Fig. 1 Schematic diagram of connecting rod coordinate system;

图2本发明整体示意图。Fig. 2 overall schematic diagram of the present invention.

具体实施方式Detailed ways

为使本发明的目的、技术方案和优点更加清楚明白，下面结合具体实施例，并参照附图，对本发明作进一步详细说明。In order to make the object, technical solution and advantages of the present invention clearer, the present invention will be further described in detail below in conjunction with specific embodiments and with reference to the accompanying drawings.

本发明的基本思路是：提供了一种改进的机器人的控制方法：它不需要知道被控对象的具体数学模型；而且具有强鲁棒性、高跟踪精度；并且改善由于大范围的初始位姿偏差而引起的力矩跳变和速度跳变问题。本发明首先对机器人进行建模估算它的动力学模型，采用基于计算力矩法的滑模非线性控制方法，来保证控制中的强鲁棒性；机器人滑模控制会出现抖振现象，因此本发明引入指数趋近律，有效地消除抖振问题。同时，本发明采用一个自适应模糊控制器，根据滑模到达条件对滑模切换增益进行估算，增强其对不确定性因素的适应能力，消除在滑模控制中输出力矩的抖振现象；采用另一个模糊自适应控制器对指数趋近律的系数进行修正，来改善由于大范围的初始位姿偏差变化而引起的大力矩和速度跳变问题。The basic idea of the present invention is to provide an improved robot control method: it does not need to know the specific mathematical model of the controlled object; it has strong robustness and high tracking accuracy; The problem of torque jump and speed jump caused by deviation. The present invention first models and estimates the dynamics model of the robot, and adopts the sliding mode nonlinear control method based on the calculated torque method to ensure strong robustness in the control; the chattering phenomenon will occur in the sliding mode control of the robot, so the present invention The invention introduces exponential reaching law to effectively eliminate the chattering problem. At the same time, the present invention adopts an adaptive fuzzy controller to estimate the sliding mode switching gain according to the sliding mode arrival condition, enhances its adaptability to uncertain factors, and eliminates the chattering phenomenon of the output torque in the sliding mode control; Another fuzzy adaptive controller modifies the coefficients of the exponential reaching law to improve the problem of large torque and velocity jumps caused by a wide range of initial pose deviation changes.

附图2为本发明的整体控制框图。动力学估算模块1通过建立机器人的连杆坐标系，获取它的D-H参数，得到机器人的动力学方程，根据D-H参数估算各关节的惯性力项、哥氏力项和重力项，最后得出各关节的力矩估算公式。建立滑模面模块2通过各关节的位置误差建立滑模面，利用基于计算力矩法的滑模控制技术来进行各关节的位置控制。滑模切换增益估算模块3为减少滑模控制中的抖振现象，加入指数趋近律，对于其中滑模切换增益K采用自适应模糊控制来在线进行估计。指数趋近律估算模块4为减弱大的初始偏差带来的大力矩与速度跳变问题，采用另一个模糊控制来估算指数趋近律系数，确定最优参数。控制力矩计算模块5最后算出各关节的控制输入τ_i来完成机器人的位置控制。Accompanying drawing 2 is the overall control block diagram of the present invention. The dynamics estimation module 1 establishes the link coordinate system of the robot, obtains its DH parameters, obtains the dynamic equation of the robot, estimates the inertial force, Coriolis force and gravity of each joint according to the DH parameters, and finally obtains each Joint moment estimation formula. Establishing the sliding mode surface module 2 establishes the sliding mode surface through the position error of each joint, and uses the sliding mode control technology based on the calculation torque method to control the position of each joint. Sliding mode switching gain estimation module 3 adds exponential reaching law in order to reduce the chattering phenomenon in sliding mode control, and uses adaptive fuzzy control to estimate the sliding mode switching gain K online. The exponential reaching law estimation module 4 uses another fuzzy control to estimate the exponential reaching law coefficients and determine the optimal parameters in order to reduce the large torque and speed jump caused by the large initial deviation. The control torque calculation module 5 finally calculates the control input τ _i of each joint to complete the position control of the robot.

进一步，所述动力学估算模块1具体为：Further, the kinetic estimation module 1 is specifically:

(1.1)D-H参数的获得：(1.1) Obtaining of D-H parameters:

在各连杆上分别固接一个坐标系，与基座固接的坐标系记为{0}，与连杆i固接的坐标系记为{i}，D-H法用两个参数连杆扭角α_i和连杆长度α_i来描述任意连杆i，用连杆偏置d_i和关节角θ_i来描述相邻连杆的关系。4个连杆参数可以分别定义为：α_i--绕X_i轴，Z_i-1轴到Z_i轴的角度；a_i--沿X_i轴，Z_i-1轴到Z_i轴的距离；d_i--沿Z_i-1轴，X_i-1轴到X_i轴的距离；θ_i--绕Z_i-1轴，X_i-1轴到X_i轴的角度。如图1。A coordinate system is respectively fixed on each connecting rod, and the coordinate system fixed to the base is marked as {0}, and the coordinate system fixed to the connecting rod i is marked as {i}. The DH method uses two parameters of the connecting rod torsion Angle α _i and link length α _i are used to describe any link i, and link offset d _i and joint angle θ _i are used to describe the relationship between adjacent links. The four connecting rod parameters can be respectively defined as: α _i -- the angle around the X _i axis, the Z _i-1 axis to the Z _i axis; a _i -- the angle along the X _i axis, the Z _i-1 axis to the Z _i axis Distance; d _i -- along the Z _i-1 axis, the distance from the X _i-1 axis to the X _i axis; θ _i -- the angle around the Z _i-1 axis, from the X _i-1 axis to the X _i axis. Figure 1.

(1.2)求取运动学方程：(1.2) Find the kinematic equation:

确定了D-H参数后，通过两个平移运动和两个旋转运动来建立相邻连杆i-1和i之间的相对关系，连杆变换^i-1T_i表示连杆坐标系{i}相对于坐标系{i-1}的变换，可分解为四个步骤：After the DH parameters are determined, the relative relationship between adjacent links i-1 and i is established through two translational movements and two rotational movements, and the link transformation ^i-1 T _i means that the link coordinate system {i} is relative to The transformation of the coordinate system {i-1} can be decomposed into four steps:

a)绕Z_i-1轴旋转θ_i角，使X_i-1轴转到与X_i同一平面内；a) Rotate the angle θ _i around the Z _i-1 axis, so that the X _i-1 axis turns to the same plane as X _i ;

b)沿轴Z_i-1平移一段距离d_i，把X_i-1移动到与X_i同一直线上；b) Translate for a distance d _i along the axis Z _i-1 , and move Xi _-1 to the same straight line as _Xi ;

c)沿轴X_i-1平移距离a_i，使得两坐标系的原点重叠；c) Translate the distance a _i along the axis X _i-1 , so that the origins of the two coordinate systems overlap;

d)绕轴X_i-1旋转α_i角，使得两坐标系完全重叠。d) Rotate the angle α _i around the axis X _i-1 , so that the two coordinate systems overlap completely.

如此，连杆坐标系{i}相对于连杆坐标系{i-1}的位姿可以用齐次变换矩阵^i-1T_i表示为：In this way, the pose of the link coordinate system {i} relative to the link coordinate system {i-1} can be expressed by the homogeneous transformation matrix ^i-1 T _i as:

${T T}_{i i}^{i i - - 11} = = {T T}_{rot rot} (({Z Z}_{i i - - 11},, {θ θ}_{i i})) {T T}_{tran tran} (({Z Z}_{i i - - 11},, {d d}_{i i})) {T T}_{tran tran} (({X x}_{i i - - 11},, {a a}_{i i})) {T T}_{rot rot} (({X x}_{i i - - 11},, {α α}_{i i}))$

$= = [\begin{matrix} {cos cos θ θ}_{i i} & - - {sin sin θ θ}_{i i} {cos cos α α}_{i i} & {sin sin θ θ}_{i i} {sin sin α α}_{i i} & {a a}_{i i} {cos cos θ θ}_{i i} \\ {sin sin θ θ}_{i i} & {cos cos θ θ}_{i i} {cos cos α α}_{i i} & {- - cos cos θ θ}_{i i} {sin sin α α}_{i i} & {a a}_{i i} {sin sin θ θ}_{i i} \\ 00 & {sin sin α α}_{i i} & {cos cos α α}_{i i} & {d d}_{i i} \\ 00 & 00 & 00 & 11 \end{matrix}]$

运动学方程为：The kinematic equation is:

⁰T₆＝⁰T₁ ¹T₂ ²T₃ ³T₄ ⁴T₅ ⁵T₆ ⁰ T ₆ = ⁰ T ₁ ¹ T ₂ ² T ₃ ³ T ₄ ⁴ T ₅ ⁵ T ₆

(1.3)系统动力学方程式，即拉格朗日方程如下：

i＝1，2，…，n机械臂的精确动力学模型为：(1.3) The system dynamics equation, that is, the Lagrangian equation is as follows:

i=1, 2,..., n The exact dynamic model of the manipulator is:

${τ τ}_{i i} = = {Σ Σ}_{j j = = 11}^{n no} {D D.}_{ij ij} {\overset{\cdot &Center Dot; \cdot &Center Dot;}{q q}}_{j j} + + {I I}_{ai ai} {\overset{\cdot &Center Dot; \cdot &Center Dot;}{q q}}_{i i} + + {Σ Σ}_{j j = = 11}^{66} {Σ Σ}_{k k = = 11}^{66} {D D.}_{ijk ijk} {\overset{\cdot \cdot}{q q}}_{j j} {\overset{\cdot &Center Dot;}{q q}}_{k k} + + {D D.}_{i i}$

对于三关节机械臂：For a three-joint robot arm:

${D D.}_{ij ij} = = {Σ Σ}_{p p = = max max i i,, j j}^{33} Trace Trace ((\frac{{&PartialD; &PartialD; T T}_{p p}}{{&PartialD; &PartialD; q q}_{j j}} {I I}_{p p} \frac{{&PartialD; &PartialD; T T}_{p p}^{T T}}{{&PartialD; &PartialD; q q}_{i i}}))$

${D D.}_{ijk ijk} = = {Σ Σ}_{p p = = max max i i,, j j,, k k}^{33} Trace Trace ((\frac{{&PartialD; &PartialD;}^{22} {T T}_{p p}}{{&PartialD; &PartialD; q q}_{j j} {&PartialD; &PartialD; q q}_{k k}} {I I}_{i i} \frac{{&PartialD; &PartialD; T T}_{p p}^{T T}}{{&PartialD; &PartialD; q q}_{i i}}))$

${D D.}_{i i} = = {Σ Σ}_{p p = = i i}^{33} {- - m m}_{p p} {g g}^{T T} \frac{{&PartialD; &PartialD; T T}_{p p}}{{&PartialD; &PartialD; q q}_{i i}} {r r}_{p p}^{p p}$

I_ai为传动装置的等效转动惯量，一般可忽略不计。根据动力学方程估算出各关节惯性力项、哥氏力项和重力项

最后得出各轴的力矩估算公式：

\hat{τ} = \hat{H} {\overset{\cdot \cdot}{q}}_{r} + \hat{C} {\overset{\cdot}{q}}_{r} + \hat{G} .

I _ai is the equivalent moment of inertia of the transmission device, which is generally negligible. Estimate the inertial force term, Coriolis force term and gravity term of each joint according to the dynamic equation

Finally, the torque estimation formula of each axis is obtained:

\hat{τ} = \hat{h} {\overset{&Center Dot; &Center Dot;}{q}}_{r} + \hat{C} {\overset{\cdot}{q}}_{r} + \hat{G} .

所述滑模控制模块2具体为：The sliding mode control module 2 is specifically:

(2.1)滑模面的设计(2.1) Design of sliding surface

定义机械臂的位置跟踪误差为e＝q_d-q，其中q_d为关节期望位置，q为实际位置。定义误差函数为：其中Λ＝diag[λ₁，…，λ_i，…，λ_n]，λ_i＞0。Define the position tracking error of the manipulator as e=q _d -q, where q _d is the expected position of the joint, and q is the actual position. Define the error function as: Wherein Λ=diag[λ ₁ , . . . , λ _i , . . . , λ _n ], λ _i >0.

(2.2)滑模控制律的设计(2.2) Design of sliding mode control law

定义： ${\overset{\cdot}{q}}_{r} = \overset{\cdot}{q} - s = {\overset{\cdot}{q}}_{d} - Λe, {\overset{\cdot \cdot}{q}}_{r} = \overset{\cdot \cdot}{q} - \overset{\cdot}{s} = {\overset{\cdot \cdot}{q}}_{d} - Λ \overset{\cdot}{e}$ definition: ${\overset{&Center Dot;}{q}}_{r} = \overset{&Center Dot;}{q} - the s = {\overset{\cdot}{q}}_{d} - Λ e, {\overset{&Center Dot; &Center Dot;}{q}}_{r} = \overset{&Center Dot; &Center Dot;}{q} - \overset{&Center Dot;}{the s} = {\overset{\cdot \cdot}{q}}_{d} - Λ \overset{&Center Dot;}{e}$

设计控制律为： $τ = \hat{τ} - As - Ksgns, \hat{τ} = \hat{H} {\overset{\cdot \cdot}{q}}_{r} + \hat{C} {\overset{\cdot}{q}}_{r} + \hat{G}$ The design control law is: $τ = \hat{τ} - As - Ksgns, \hat{τ} = \hat{h} {\overset{\cdot \cdot}{q}}_{r} + \hat{C} {\overset{&Center Dot;}{q}}_{r} + \hat{G}$

其中，

为等效控制，As为指数趋近律，Ksgns为切换控制。

分别为H，C，G的估计值，K＝diag[K₁₁，…，K_ii，…K_nn]、A＝diag[a₁，…，a_i，…，a_n]为正定矩阵。in,

is equivalent control, As is exponential reaching law, and Ksgns is switching control.

are estimated values of H, C, and G respectively, K=diag[K ₁₁ ,...,K _ii ,...K _nn ], A=diag[a ₁ ,...,a _i ,..., _an ] are positive definite matrices.

所述滑模切换增益估算模块3具体为：The sliding mode switching gain estimation module 3 is specifically:

(3.1)模糊规则的设计(3.1) Design of fuzzy rules

基于模糊增益调整的控制律设计为：其中K＝[k₁，…，k_i，…，k_n]，k_i为第i个模糊系统的输出。The control law based on fuzzy gain adjustment is designed as: Wherein K=[k ₁ , ..., _ki , ..., k _n ], _ki is the output of the i-th fuzzy system.

如果增益K采用模糊控制进行逼近，并且定义Lyapunov函数：

\overset{\cdot}{V} = \frac{1}{2} [{\overset{\cdot}{s}}^{T} Hs + s^{T} \overset{\cdot}{H} s + s^{T} H \overset{\cdot}{s}] = \frac{1}{2} [{2 s}^{T} H \overset{\cdot}{s} + s^{T} \overset{\cdot}{H} s] = s^{T} [- (C + A) s + Δf - K + Cs] =

s^{T} [- As + Δf - K] = s^{T} [Δf - K] - s^{T} As = Σ_{i = 1}^{n} [s_{i} {Δf}_{i} - s_{i} k_{i}] - s^{T} As

If the gain K is approximated by fuzzy control, and the Lyapunov function is defined:

\overset{&Center Dot;}{V} = \frac{1}{2} [{\overset{&Center Dot;}{the s}}^{T} Hs + {the s}^{T} \overset{&Center Dot;}{h} the s + {the s}^{T} h \overset{\cdot}{the s}] = \frac{1}{2} [{2 the s}^{T} h \overset{&Center Dot;}{the s} + {the s}^{T} \overset{&Center Dot;}{h} the s] = {the s}^{T} [- (C + A) the s + Δf - K + Cs] =

{the s}^{T} [- As + Δ f - K] = {the s}^{T} [Δf - K] - {the s}^{T} As = Σ_{i = 1}^{no} [{the s}_{i} {Δf}_{i} - {the s}_{i} k_{i}] - {the s}^{T} As

由此可见，为保证

为负，应使s_ik_i≥O，即保证s_i与k_i符号相同。同时，考虑s_iΔf_i-s_ik_i，当|s_i|较大时，为保证

为较大的负数，希望|k_i|较大；当|s_i|较小时，|k_i|保持较小的值，就可保证

为负数。It can be seen that, to ensure

Negative, should make s _i k _i ≥ O, that is to ensure that s _i and _ki have the same sign. At the same time, considering s _i Δf _i -s _i k _i , when |s _i | is large, to ensure

is a large negative number, we hope that |k _i | is larger; when |s _i | is small, |k _i | keeps a small value, which guarantees

is a negative number.

(3.2)模糊系统设计(3.2) Fuzzy system design

采用乘积推理机、单值模糊器和中心平均解模糊器来设计模糊控制系统，系统的控制输出为：模糊系统的输出为：The fuzzy control system is designed by using product reasoning machine, single value fuzzer and central average defuzzifier. The control output of the system is: The output of the fuzzy system is:

${k k}_{i i} = = \frac{{Σ Σ}_{m m = = 11}^{M m} {ϵ ϵ}_{{k k}_{i i}}^{m m} {μ μ}_{{A A}^{m m}} (({s the s}_{i i}))}{{Σ Σ}_{m m = = 11}^{M m} {μ μ}_{{A A}^{m m}} (({s the s}_{i i}))} = = {ϵ ϵ}_{{k k}_{i i}}^{T T} {Ψ Ψ}_{{k k}_{i i}} (({s the s}_{i i}))$

其中： $ϵ_{k_{i}} = {[ϵ_{k_{i}}^{1}, \cdot \cdot \cdot, ϵ_{k_{i}}^{m}, \cdot \cdot \cdot, ϵ_{k_{i}}^{M}]}^{T}, Ψ_{k_{i}} (s_{i}) = {[φ_{k_{i}}^{1} (s_{i}), \cdot \cdot \cdot, φ_{k_{i}}^{m} (s_{i}), \cdot \cdot \cdot, φ_{k_{i}}^{M} (s_{i})]}^{T},$ in: $ϵ_{k_{i}} = {[ϵ_{k_{i}}^{1}, &Center Dot; &Center Dot; &Center Dot;, ϵ_{k_{i}}^{m}, \cdot \cdot \cdot, ϵ_{k_{i}}^{m}]}^{T}, Ψ_{k_{i}} ({the s}_{i}) = {[φ_{k_{i}}^{1} ({the s}_{i}), &Center Dot; &Center Dot; \cdot, φ_{k_{i}}^{m} ({the s}_{i}), &Center Dot; &Center Dot; &Center Dot;, φ_{k_{i}}^{m} ({the s}_{i})]}^{T},$

${φ φ}^{m m} ((x x)) = = \frac{{Π Π}_{i i = = 11}^{n no} {μ μ}_{{A A}_{i i}^{m m}} (({x x}_{i i}^{* *}))}{{Σ Σ}_{m m = = 11}^{M m} {Π Π}_{i i = = 11}^{n no} {μ μ}_{{A A}_{i i}^{m m}} (({x x}_{i i}^{* *}))},,$

(3.3)自适应模糊控制律的设计(3.3) Design of adaptive fuzzy control law

上面已得到： $H \overset{\cdot}{s} = - (C + A) s + Δf - k - - - (1)$ The above has been obtained: $h \overset{&Center Dot;}{the s} = - (C + A) the s + Δf - k - - - (1)$

取

为理想Δf_i的逼近，根据万能逼近定理，存在ω_i＞0，有：Pick

is the approximation of the ideal Δf _i , according to the universal approximation theorem, there exists ω _i >0, we have:

$| | {Δf Δ f}_{i i} - - {θ θ}_{{k k}_{id id}}^{T T} {Ψ Ψ}_{{k k}_{i i}} (({s the s}_{i i})) | | \leq \leq {ω ω}_{i i} - - - - - - ((22))$

定义Lyapunov函数： $V = \frac{1}{2} s^{T} Hs + \frac{1}{2} Σ_{i = 1}^{n} ({\tilde{θ}}_{k_{i}}^{T} {\tilde{θ}}_{k_{i}}) .$ 其中 ${\tilde{θ}}_{k_{i}} = θ_{k_{i}} - θ_{k_{id}} .$ 则Define the Lyapunov function: $V = \frac{1}{2} {the s}^{T} Hs + \frac{1}{2} Σ_{i = 1}^{no} ({\tilde{θ}}_{k_{i}}^{T} {\tilde{θ}}_{k_{i}}) .$ in ${\tilde{θ}}_{k_{i}} = θ_{k_{i}} - θ_{k_{id}} .$ but

$\overset{\cdot \cdot}{V V} = = \frac{11}{22} [[{\overset{\cdot \cdot}{s the s}}^{T T} Hs Hs + + {s the s}^{T T} \overset{\cdot \cdot}{H h} s the s + + {s the s}^{T T} H h \overset{\cdot \cdot}{s the s}]] + + \frac{11}{22} {Σ Σ}_{i i = = 11}^{n no} (({\overset{\cdot &Center Dot;}{\overset{~ ~}{θ θ}}}_{{k k}_{i i}}^{T T} {\overset{~ ~}{θ θ}}_{{k k}_{i i}} + + {\overset{~ ~}{θ θ}}_{{k k}_{i i}}^{T T} {\overset{\cdot &Center Dot;}{\overset{~ ~}{θ θ}}}_{{k k}_{i i}}))$

$= = \frac{11}{22} [[{22 s the s}^{T T} H h \overset{\cdot &Center Dot;}{s the s} + + {s the s}^{T T} \overset{\cdot &Center Dot;}{H h} s the s]] + + \frac{11}{22} {Σ Σ}_{i i = = 11}^{n no} 22 {\overset{~ ~}{θ θ}}_{{k k}_{i i}}^{T T} {\overset{\cdot &Center Dot;}{\overset{~ ~}{θ θ}}}_{{k k}_{i i}} = = {s the s}^{T T} [[H h \overset{\cdot &Center Dot;}{s the s} + + Cs Cs]] + + {Σ Σ}_{i i = = 11}^{n no} {\overset{~ ~}{θ θ}}_{{k k}_{i i}}^{T T} {\overset{\cdot &Center Dot;}{\overset{~ ~}{θ θ}}}_{{k k}_{i i}}$

$= = {s the s}^{T T} [[- - ((C C + + A A)) s the s + + Δf Δ f - - k k + + Cs Cs]] + + {Σ Σ}_{i i = = 11}^{n no} {\overset{~ ~}{θ θ}}_{{k k}_{i i}}^{T T} {\overset{\cdot &Center Dot;}{\overset{~ ~}{θ θ}}}_{{k k}_{i i}}$

$= = {s the s}^{T T} [[- - As As + + Δf Δ f - - k k]] + + {Σ Σ}_{i i = = 11}^{n no} {\overset{~ ~}{θ θ}}_{{k k}_{i i}}^{T T} {\overset{\cdot \cdot}{\overset{~ ~}{θ θ}}}_{{k k}_{i i}} = = - - {s the s}^{T T} As As + + {s the s}^{T T} [[Δf Δf - - k k]] + + {Σ Σ}_{i i = = 11}^{n no} {\overset{~ ~}{θ θ}}_{{k k}_{i i}}^{T T} {\overset{\cdot \cdot}{\overset{~ ~}{θ θ}}}_{{k k}_{i i}}$

$= = - - {s the s}^{T T} As As + + {Σ Σ}_{i i = = 11}^{n no} {s the s}_{i i} [[Δf Δ f - - {k k}_{i i}]] + + {Σ Σ}_{i i = = 11}^{n no} {\overset{~ ~}{θ θ}}_{{k k}_{i i}}^{T T} {\overset{\cdot \cdot}{\overset{~ ~}{θ θ}}}_{{k k}_{i i}}$

由于 $k_{i} = {\tilde{θ}}_{k_{i}}^{T} Ψ_{k_{i}} (s_{i}) + θ_{k_{id}}^{T} Ψ_{k_{i}} (s_{i}),$ 则：because $k_{i} = {\tilde{θ}}_{k_{i}}^{T} Ψ_{k_{i}} ({the s}_{i}) + θ_{k_{id}}^{T} Ψ_{k_{i}} ({the s}_{i}),$ but:

$\overset{\cdot \cdot}{V V} = = {- - s the s}^{T T} As As + + {Σ Σ}_{i i = = 11}^{n no} {s the s}_{i i} [[{Δf Δ f}_{i i} - - (({\overset{~ ~}{θ θ}}_{{k k}_{i i}}^{T T} {Ψ Ψ}_{{k k}_{i i}} (({s the s}_{i i})) + + {θ θ}_{{k k}_{id id}}^{T T} {Ψ Ψ}_{{k k}_{i i}} (({s the s}_{i i}))))]] + + {Σ Σ}_{i i = = 11}^{n no} {\overset{~ ~}{θ θ}}_{{k k}_{i i}}^{T T} {\overset{\cdot &Center Dot;}{\overset{~ ~}{θ θ}}}_{{k k}_{i i}}$

$= = - - {s the s}^{T T} As As + + {Σ Σ}_{i i = = 11}^{n no} {s the s}_{i i} [[{Δf Δ f}_{i i} - - {θ θ}_{{k k}_{id id}}^{T T} {Ψ Ψ}_{{k k}_{i i}} (({s the s}_{i i}))]] + + {Σ Σ}_{i i = = 11}^{n no} (({- - s the s}_{i i} {\overset{~ ~}{θ θ}}_{{k k}_{i i}}^{T T} {Ψ Ψ}_{{k k}_{i i}} (({s the s}_{i i})))) + + {Σ Σ}_{i i = = 11}^{n no} {\overset{~ ~}{θ θ}}_{{k k}_{i i}}^{T T} {\overset{\cdot \cdot}{\overset{~ ~}{θ θ}}}_{{k k}_{i i}}$

$= = - - {s the s}^{T T} As As + + {Σ Σ}_{i i = = 11}^{n no} {s the s}_{i i} [[{Δf Δ f}_{i i} - - {θ θ}_{{k k}_{id id}}^{T T} {Ψ Ψ}_{{k k}_{i i}} (({s the s}_{i i}))]] + + {Σ Σ}_{i i = = 11}^{n no} (({- - s the s}_{i i} {\overset{~ ~}{θ θ}}_{{k k}_{i i}}^{T T} {Ψ Ψ}_{{k k}_{i i}} (({s the s}_{i i})) + + {\overset{~ ~}{θ θ}}_{{k k}_{i i}}^{T T} {\overset{\cdot &Center Dot;}{\overset{~ ~}{θ θ}}}_{{k k}_{i i}})) - - - - - - ((33))$

$= = {- - s the s}^{T T} As As + + {Σ Σ}_{i i = = 11}^{n no} {s the s}_{i i} [[{Δf Δf}_{i i} - - {θ θ}_{{k k}_{id id}}^{T T} {Ψ Ψ}_{{k k}_{i i}} (({s the s}_{i i}))]] + + {Σ Σ}_{i i = = 11}^{n no} {\overset{~ ~}{θ θ}}_{{k k}_{i i}}^{T T} (({- - s the s}_{i i} {Ψ Ψ}_{{k k}_{i i}} (({s the s}_{i i})) + + {\overset{\cdot \cdot}{\overset{~ ~}{θ θ}}}_{{k k}_{i i}}))$

选取自适应律为： ${\overset{\overset{\cdot}{~}}{θ}}_{k_{i}} = s_{i} Ψ_{k_{i}} (s_{i}) - - - (4)$ Choose the adaptive law as: ${\overset{\overset{\cdot}{~}}{θ}}_{k_{i}} = {the s}_{i} Ψ_{k_{i}} ({the s}_{i}) - - - (4)$

并代入(3)式得： $\overset{\cdot}{V} = - s^{T} As + Σ_{i = 1}^{n} s_{i} [{Δf}_{i} - θ_{k_{id}}^{T} Ψ_{k_{i}} (s_{i})]$ And substitute into formula (3) to get: $\overset{&Center Dot;}{V} = - {the s}^{T} As + Σ_{i = 1}^{no} {the s}_{i} [{Δf}_{i} - θ_{k_{id}}^{T} Ψ_{k_{i}} ({the s}_{i})]$

存在正实数γ_i，使得(2)式满足：

其中0＜γ_i＜1。则：

s_{i} | {Δf}_{i} - θ_{k_{id}}^{T} Ψ_{k_{i}} (s_{i}) {| \leq γ_{i} | s_{i} |}^{2} = γ_{i} s_{i}^{2}

There is a positive real number γ _i , so that formula (2) satisfies:

where 0<γ _i <1. but:

{the s}_{i} | {Δf}_{i} - θ_{k_{id}}^{T} Ψ_{k_{i}} ({the s}_{i}) {| \leq γ_{i} | {the s}_{i} |}^{2} = γ_{i} {the s}_{i}^{2}

$\overset{\cdot &Center Dot;}{V V} \leq \leq - - {s the s}^{T T} As As + + {Σ Σ}_{i i = = 11}^{n no} {γ γ}_{i i} {s the s}_{i i}^{22} = = {Σ Σ}_{i i = = 11}^{n no} ((- - {a a}_{i i} {s the s}_{i i}^{22} + + {γ γ}_{i i} {s the s}_{i i}^{22})) = = {Σ Σ}_{i i = = 11}^{n no} (({γ γ}_{i i} - - {a a}_{i i})) {s the s}_{i i}^{22} \leq \leq 00 - - - - - - ((55))$

其中γ＝diag[γ₁，…，γ_i，…γ_n]，a_i＞γ_i。由式(5)可见，仅当s＝0时，

自适应律(4)渐进收敛。得出结论为：

即

\lim_{t &RightArrow; \infty} \overset{\cdot}{q} = {\overset{\cdot}{q}}_{d} .

Where γ=diag[γ ₁ , . . . , γ _i , . . . γ _n ], a _i >γ _i . It can be seen from formula (5), only when s=0,

The adaptive law (4) converges asymptotically. It is concluded that:

Right now

\lim_{t &Right Arrow; \infty} \overset{&Center Dot;}{q} = {\overset{\cdot}{q}}_{d} .

所述指数趋近律估算模块4具体为：The exponential reaching law estimation module 4 is specifically:

(4.1)由于机器人刚启动时会产生一个比较大的误差和误差变化率，所以这时的控制器会产生一个比较大的输出。为了减小这种情况，在滑模控制的基础上，对滑模控制的指数控制项系数进行模糊控制，通过调节A来达到：当误差及误差变化率大的时候尽量减小控制量；反之，则增加控制量。从而改善机器人启动时的大力矩与速度跳变问题。(4.1) Since the robot will generate a relatively large error and error rate of change when it is first started, the controller at this time will generate a relatively large output. In order to reduce this situation, on the basis of sliding mode control, fuzzy control is performed on the index control item coefficient of sliding mode control, and by adjusting A to achieve: when the error and error change rate are large, the control amount should be reduced as much as possible; otherwise , then increase the amount of control. Thereby improving the problem of large torque and speed jump when the robot starts.

(4.2)控制规则可自调节的模糊控制器的实现：(4.2) Realization of fuzzy controller with self-adjusting control rules:

一个模糊控制器性能的好坏在很大程度上取决于它的模糊控制规则，如果采用固定的模糊控制规则，则模糊控制器一旦形成，语言规则与合成推理就是确定、不可调的。但在有些控制场景中，为了使已有模糊控制器具有更强的应变性，以适应于不同的控制对象，就要求控制规则具有一定的自调节功能。The performance of a fuzzy controller depends largely on its fuzzy control rules. If fixed fuzzy control rules are adopted, once the fuzzy controller is formed, the language rules and synthetic reasoning are deterministic and non-adjustable. However, in some control scenarios, in order to make the existing fuzzy controller more adaptable to different control objects, the control rules are required to have a certain self-regulation function.

对于一个二维的模糊控制器当其输入变量E、EC和输出量U的论域划分等级相同时，所引入的描述控制规则表达式为：For a two-dimensional fuzzy controller, when its input variables E, EC and output U have the same domain division level, the introduced description control rule expression is:

$U u = = [[αE αE + + ((11 - - α α)) EC EC]],, α α &Exists; &Exists; &Element; &Element; ((0,1 0,1))$

通过调节α值便可以对控制规则进行调节。By adjusting the value of α, the control rules can be adjusted.

所述控制力矩计算模块5具体为：The control torque calculation module 5 is specifically:

(5)将上面的几部分组合，关节的力矩控制输入为：

其中，

分别为H，C，G的估计值，s为滑模面，通过第三步的自适应滑模控制方法来估算参数K，利用第四步的滑模控制方法来估算参数A。将计算的τ作为关节的控制输入。(5) Combining the above parts, the torque control input of the joint is:

in,

are the estimated values of H, C, and G respectively, and s is the sliding mode surface. The parameter K is estimated by the adaptive sliding mode control method in the third step, and the parameter A is estimated by the sliding mode control method in the fourth step. The calculated τ is used as the control input of the joint.

Claims

1. Provides an improved serial robot control method: it does not need to know the specific mathematical model of the controlled object; it has strong robustness, high tracking accuracy; and improves the torque caused by a wide range of initial pose deviations jump and speed jump problem; the present invention first models the robot, estimates its dynamic model, and adopts a sliding mode nonlinear control method based on the calculation torque method to ensure strong robustness in control; the robot sliding mode control There will be chattering, so the present invention introduces an exponential reaching law to effectively eliminate the chattering problem; at the same time, the present invention uses an adaptive fuzzy controller to estimate the sliding mode switching gain according to the sliding mode arrival condition, so as to enhance its The adaptability of uncertainty factors eliminates the chattering phenomenon of the output torque in sliding mode control; another fuzzy adaptive controller is used to correct the coefficient of the exponential reaching law to improve the variation due to a wide range of initial pose deviations The large torque and speed jump problems caused by it;

The dynamics estimation module 1 establishes the link coordinate system of the robot, obtains its D-H parameters, obtains the dynamic equation of the robot, estimates the inertial force, Coriolis force and gravity of each joint according to the D-H parameters, and finally obtains each Joint torque estimation formula;

Establish the sliding mode surface module 2 to establish the sliding mode surface through the position error of each joint, and use the sliding mode control technology based on the calculation torque method to control the position of each joint;

In order to reduce the chattering phenomenon in the sliding mode control, the sliding mode switching gain estimation module 3 adds an exponential reaching law, and uses adaptive fuzzy control to estimate the constant speed approaching term coefficient K online;

The exponential reaching law estimation module 4 uses another fuzzy control to estimate the exponential reaching law coefficients to determine the optimal parameters in order to weaken the large torque and speed jump caused by the large initial deviation;

The control torque calculation module 5 finally calculates the control input τ _i of each joint to complete the position control of the robot; to realize high-precision tracking of the robot position and improve the problem of torque jump and speed jump.

2. according to the described double-fuzzy self-adaptive sliding mode control method based on calculation moment method of claim 1, it is characterized in that: described DH parameter seeks and dynamics estimation module, establishes each connecting rod coordinate system of robot, determines each joint DH parameters (a _i , α _i , d _i , θ _i ); by Lagrange equation:

i=1, 2,..., n, derive the kinetic equation:

Estimate the inertial force term, Coriolis force term and gravity term according to the dynamic equation

Finally, the torque estimation formula of each joint is obtained:

3. according to the described double-fuzzy self-adaptive sliding mode control method based on calculation moment method of claim 1, it is characterized in that: the described sliding mode surface module is set up, by calculating the position error e and the error rate of change of each joint

Create a sliding surface

Wherein Λ=diag[λ ₁ ,...,λ _i ,...,λ _n ], λ _i >0;

and define:

The design control law is:

in,

is equivalent control, As is exponential reaching law, and Ksgns is switching control;

4. according to the described double-fuzzy self-adaptive sliding mode control method based on calculating moment method according to claim 1, it is characterized in that: described sliding mode switching gain estimation module uses the gain K of fuzzy control self-adaptive approximation sliding mode control law; Adopt The fuzzy control system is designed by product inference machine, single value fuzzer and central average defuzzifier. The control output of the system is:

The membership function used to represent fuzzy sets is designed as:

Choose the adaptive law as:

5. according to the described double-fuzzy self-adaptive sliding mode control method based on calculation moment method of claim 1, it is characterized in that: described exponential reaching law estimation module carries out fuzzy control to the exponential control item coefficient of sliding mode control, by adjusting A to achieve: when the error and error rate of change are large, reduce the control amount as much as possible; on the contrary, increase the control amount, thereby retaining the good tracking effect of the original control algorithm, and at the same time improving the large torque problem when the robot starts. the

6. according to the described double-fuzzy self-adaptive sliding mode control method based on calculation moment method of claim 1, it is characterized in that: described control moment calculation module, with the combination of several parts above, the moment control input of joint is:

in, are the estimated values of H, C, and G respectively, and s is the sliding mode surface. The parameter K is estimated by the adaptive sliding mode control method in the third step, and the parameter A is estimated by the sliding mode control method in the fourth step; the calculated τ is used as the control input of the joint.