[go: up one dir, main page]

CN116069044B - Multi-robot cooperative transportation capacity hybrid control method - Google Patents

Multi-robot cooperative transportation capacity hybrid control method Download PDF

Info

Publication number
CN116069044B
CN116069044B CN202310321732.XA CN202310321732A CN116069044B CN 116069044 B CN116069044 B CN 116069044B CN 202310321732 A CN202310321732 A CN 202310321732A CN 116069044 B CN116069044 B CN 116069044B
Authority
CN
China
Prior art keywords
robot
error
model
end effector
dynamics model
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202310321732.XA
Other languages
Chinese (zh)
Other versions
CN116069044A (en
Inventor
毛建旭
张振国
谭浩然
王耀南
江一鸣
冯运
晁陈卓蕾
谢家胤
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Hunan University
Original Assignee
Hunan University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Hunan University filed Critical Hunan University
Priority to CN202310321732.XA priority Critical patent/CN116069044B/en
Publication of CN116069044A publication Critical patent/CN116069044A/en
Application granted granted Critical
Publication of CN116069044B publication Critical patent/CN116069044B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0219—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory ensuring the processing of the whole working surface
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
    • Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
    • Y02P90/02—Total factory control, e.g. smart factories, flexible manufacturing systems [FMS] or integrated manufacturing systems [IMS]

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Feedback Control In General (AREA)

Abstract

The invention discloses a multi-robot cooperative transportation capacity hybrid control method, which comprises the steps of firstly, establishing a cooperative transportation dynamics model of a plurality of robots; setting a robot position error, introducing an error conversion function to perform error conversion, processing the position error after the robot conversion, and combining a collaborative handling dynamics model to obtain an error transfer dynamics model; rewriting an error transfer dynamics model, setting a sliding mode function and a disturbance estimation error, designing a specified performance controller according to the rewritten error transfer dynamics model, the sliding mode function and the disturbance estimation error, and calculating the input torque of the robot; then presetting an impedance model, a spring model and environmental stiffness estimation, and calculating to obtain the contact force estimation and the position of the robot end effector; and finally, constructing a mathematical simulation model, and verifying the effectiveness of the multi-robot cooperative transportation control method. The method can ensure the precision and the safety of a plurality of robots in the cooperative transportation process.

Description

一种多机器人协同搬运力位混合控制方法A hybrid force-position control method for multi-robot collaborative handling

技术领域Technical Field

本发明涉及多机器人协同搬运控制技术领域，尤其是涉及一种多机器人协同搬运力位混合控制方法。The present invention relates to the technical field of multi-robot collaborative handling control, and in particular to a multi-robot collaborative handling force-position hybrid control method.

背景技术Background Art

近年来，以机器人为代表的智能制造技术正逐渐成为重大装备大型复杂部件高品质制造的新趋势。相比于数控机床，机器人或机器人化装备具有运动灵活、工作空间大、并行协调作业能力强等优点，且易于集成多类型传感器，能够适应复杂的加工环境，由一定规模的单体机器人化装备组成的多机器人制造系统能够进一步增加机器人作业的工作空间与灵巧度，因此设计多机器人高精度、高安全性自主控制方法对智能制造具有重要意义。In recent years, intelligent manufacturing technology represented by robots is gradually becoming a new trend in high-quality manufacturing of large and complex components of major equipment. Compared with CNC machine tools, robots or robotized equipment have the advantages of flexible movement, large workspace, strong parallel and coordinated operation capabilities, and are easy to integrate multiple types of sensors and can adapt to complex processing environments. A multi-robot manufacturing system composed of a certain scale of single robotized equipment can further increase the workspace and dexterity of robot operations. Therefore, designing a high-precision, high-safety autonomous control method for multiple robots is of great significance to intelligent manufacturing.

多移动机器人实现无人驾驶的方式从遥控驾驶，到机载计算机自主控制。移动机器人已经是成熟的移动平台，可以在移动平台上搭载不同的组件应用在不同领域。比如，状态检测、目标跟踪等领域都有移动机器人应用的潜能。其中，这些应用需要在移动平台上搭载机械臂，将二者结合起来就是移动机器人，如此高端的设备可使工业获得很大的便利。随着研究人员对这块领域的深入，已经有学者实现了移动机器人搭载机械臂在实际中的应用。例如灵活的完成抓取和装配作业任务、代替力传感器完成接触力测量工作、借助并联机械臂完成仿生工作。The ways to achieve unmanned driving of multiple mobile robots range from remote control to autonomous control by onboard computers. Mobile robots are already mature mobile platforms, and different components can be installed on the mobile platform for application in different fields. For example, there is potential for the application of mobile robots in fields such as state detection and target tracking. Among them, these applications require the installation of a robotic arm on the mobile platform. Combining the two is a mobile robot. Such high-end equipment can greatly facilitate the industry. As researchers delve deeper into this field, some scholars have realized the practical application of mobile robots equipped with robotic arms. For example, flexible completion of grasping and assembly tasks, replacement of force sensors to complete contact force measurement, and use of parallel robotic arms to complete bionic work.

多机器人协同搬运作为智能制造产业中必不可少的一个环节，尽管已经有一些学者对其进行了一定程度的研究，但是仍然存在一些技术难点需要攻克。在机器人运动过程中，外部扰动作用环境下机器人之间的相互干涉问题无疑是当今的研究热点之一；其次，在仅有位置控制下机械臂对搬运物体产生的挤压等也会对物体与机器人本身产生一定程度的损害。Multi-robot collaborative handling is an indispensable part of the intelligent manufacturing industry. Although some scholars have conducted some research on it, there are still some technical difficulties to be overcome. During the robot movement, the mutual interference between robots in the external disturbance environment is undoubtedly one of the current research hotspots; secondly, the squeezing of the robot arm on the object under position control alone will also cause a certain degree of damage to the object and the robot itself.

发明内容Summary of the invention

本发明要解决的技术问题在于考虑到多个机器人协同搬运过程的精度需要和安全性，提供了一种多机器人协同搬运力位混合控制方法。The technical problem to be solved by the present invention is to provide a multi-robot collaborative handling force-position hybrid control method taking into account the accuracy requirements and safety of the multi-robot collaborative handling process.

一种多机器人协同搬运力位混合控制方法，包括如下步骤：A multi-robot collaborative handling force-position hybrid control method comprises the following steps:

S1、建立机器人的搬运动力学模型，根据机器人的搬运动力学模型建立多个机器人组成的机器人系统的协同搬运动力学模型；S1. Establish a robot handling dynamics model, and establish a collaborative handling dynamics model of a robot system composed of multiple robots based on the robot handling dynamics model;

S2、设置机器人位置误差，引入误差转换函数对机器人位置误差进行转换，得到机器人转换后位置误差，根据机器人转换后位置误差和协同搬运动力学模型得到误差传递动力学模型；S2. Set the robot position error, introduce the error conversion function to convert the robot position error, obtain the robot position error after conversion, and obtain the error transfer dynamics model according to the robot position error after conversion and the collaborative handling dynamics model;

S3、重写误差传递动力学模型，得到重写后的误差传递动力学模型，设置滑模函数和扰动估计误差，根据重写后的误差传递动力学模型、滑模函数和扰动估计误差设计规定性能控制器，根据规定性能控制器计算出机器人系统的输入力矩；S3, rewriting the error transfer dynamics model to obtain the rewritten error transfer dynamics model, setting a sliding mode function and a disturbance estimation error, designing a specified performance controller according to the rewritten error transfer dynamics model, the sliding mode function and the disturbance estimation error, and calculating the input torque of the robot system according to the specified performance controller;

S4、预设阻抗模型、弹簧模型和环境刚度估计，根据阻抗模型、弹簧模型和环境刚度估计设计阻抗控制方法，计算出机器人系统末端执行器的接触力估计和位置；S4, presetting an impedance model, a spring model and an estimated environmental stiffness, designing an impedance control method according to the impedance model, the spring model and the estimated environmental stiffness, and calculating a contact force estimate and a position of an end effector of the robot system;

S5、根据协同搬运动力学模型、误差传递动力学模型和规定性能控制器搭建数学仿真模型，将计算出的机器人系统的输入力矩、机器人系统末端执行器的接触力估计以及位置输入至仿真模型中，验证机器人系统的协同搬运控制方法的有效性。S5. Build a mathematical simulation model based on the collaborative handling dynamics model, error transmission dynamics model and specified performance controller, input the calculated input torque of the robot system, the contact force estimation of the end effector of the robot system and the position into the simulation model to verify the effectiveness of the collaborative handling control method of the robot system.

优选地，S1中的协同搬运动力学模型具体为：Preferably, the collaborative transport dynamics model in S1 is specifically:

Figure SMS_1
Figure SMS_1

其中，

Figure SMS_2
in,
Figure SMS_2

Figure SMS_3
Figure SMS_3

Figure SMS_4
Figure SMS_4

Figure SMS_5
Figure SMS_5

Figure SMS_6
Figure SMS_6

式中，

Figure SMS_10
为机器人系统的对称正定惯性矩阵，
Figure SMS_12
为机器人系统的离心项和克利奥利项矩阵，
Figure SMS_17
为机器人系统在建模过程产生的总摩擦力，
Figure SMS_8
为从机器人系统的关节矢量至工作空间的速度雅可比矩阵，
Figure SMS_13
为重力加速度矩阵，
Figure SMS_15
为机器人系统的关节矢量，
Figure SMS_19
和
Figure SMS_7
分别为机器人系统的关节矢量
Figure SMS_11
的一阶导数和二阶导数，
Figure SMS_16
为机器人系统的自由度，
Figure SMS_21
，
Figure SMS_9
为第
Figure SMS_14
个机器人的自由度，
Figure SMS_18
为机器人系统末端执行器的实际接触力，
Figure SMS_20
为机器人系统的输入力矩。In the formula,
Figure SMS_10
is the symmetric positive definite inertia matrix of the robot system,
Figure SMS_12
is the centrifugal and Creole term matrices of the robot system,
Figure SMS_17
is the total friction force generated by the robot system during the modeling process,
Figure SMS_8
is the velocity Jacobian matrix from the joint vectors of the robot system to the workspace,
Figure SMS_13
is the gravitational acceleration matrix,
Figure SMS_15
is the joint vector of the robot system,
Figure SMS_19
and
Figure SMS_7
are the joint vectors of the robot system
Figure SMS_11
The first and second derivatives of
Figure SMS_16
is the degree of freedom of the robot system,
Figure SMS_21
,
Figure SMS_9
For the
Figure SMS_14
degrees of freedom of the robot,
Figure SMS_18
is the actual contact force of the end effector of the robot system,
Figure SMS_20
is the input torque of the robot system.

优选地，S2具体包括：Preferably, S2 specifically includes:

S21、设置机器人的规定性能和性能函数，根据规定性能和性能函数确定机器人的位置误差；S21, setting the prescribed performance and performance function of the robot, and determining the position error of the robot according to the prescribed performance and performance function;

S22、设置误差转换函数，使用误差转换函数对机器人的位置误差进行转换，得到机器人转换后位置误差；S22, setting an error conversion function, and using the error conversion function to convert the position error of the robot to obtain the position error of the robot after conversion;

S23、根据机器人转换后位置误差建立机器人系统转换后位置误差，对机器人系统转换后位置误差进行处理，并结合协同搬运动力学模型，得到误差传递动力学模型。S23. Establishing the position error of the robot system after conversion according to the position error of the robot after conversion, processing the position error of the robot system after conversion, and combining it with the collaborative handling dynamics model to obtain the error transfer dynamics model.

优选地，S23中的误差传递动力学模型，具体公式为：Preferably, the error transmission dynamics model in S23 has a specific formula:

Figure SMS_22
Figure SMS_22

其中，

Figure SMS_23
in,
Figure SMS_23

Figure SMS_24
Figure SMS_24

Figure SMS_25
Figure SMS_25

Figure SMS_26
Figure SMS_26

Figure SMS_27
Figure SMS_27

Figure SMS_28
Figure SMS_28

Figure SMS_29
Figure SMS_29

式中，

Figure SMS_33
为机器人系统转换后位置误差
Figure SMS_35
的二阶导数，
Figure SMS_38
为机器人系统的对称正定惯性矩阵，
Figure SMS_32
为机器人系统的离心项和克利奥利项矩阵，
Figure SMS_36
为从机器人系统的关节矢量至工作空间的速度雅可比矩阵，
Figure SMS_40
为重力加速度矩阵，
Figure SMS_45
和
Figure SMS_31
表示第i个机器人系统规定性能的上下界，
Figure SMS_37
表示第i个机器人的性能函数，
Figure SMS_41
为机器人系统的输入力矩，
Figure SMS_42
为机器人系统的位置误差
Figure SMS_46
的一阶导数，
Figure SMS_48
为第i个机器人的位置误差，
Figure SMS_50
为机器人系统的关节矢量
Figure SMS_52
的一阶导数，
Figure SMS_47
为第i个机器人的关节矢量，
Figure SMS_49
为机器人系统的期望位置
Figure SMS_51
的二阶导数，
Figure SMS_53
为第i个机器人的希望位置，
Figure SMS_30
为机器人系统末端执行器的实际接触力，
Figure SMS_34
、
Figure SMS_39
、
Figure SMS_44
、
Figure SMS_43
为误差传递动力学模型的中间变量。In the formula,
Figure SMS_33
is the position error of the robot system after conversion
Figure SMS_35
The second-order derivative of
Figure SMS_38
is the symmetric positive definite inertia matrix of the robot system,
Figure SMS_32
is the centrifugal and Creole term matrices of the robot system,
Figure SMS_36
is the velocity Jacobian matrix from the joint vectors of the robot system to the workspace,
Figure SMS_40
is the gravitational acceleration matrix,
Figure SMS_45
and
Figure SMS_31
represents the upper and lower bounds of the performance requirements of the ith robot system,
Figure SMS_37
represents the performance function of the ith robot,
Figure SMS_41
is the input torque of the robot system,
Figure SMS_42
is the position error of the robot system
Figure SMS_46
The first-order derivative of
Figure SMS_48
is the position error of the ith robot,
Figure SMS_50
is the joint vector of the robot system
Figure SMS_52
The first-order derivative of
Figure SMS_47
is the joint vector of the ith robot,
Figure SMS_49
is the desired position of the robot system
Figure SMS_51
The second-order derivative of
Figure SMS_53
is the desired position of the ith robot,
Figure SMS_30
is the actual contact force of the end effector of the robot system,
Figure SMS_34
,
Figure SMS_39
,
Figure SMS_44
,
Figure SMS_43
It is the intermediate variable of the error transfer dynamics model.

优选地，S3具体包括：Preferably, S3 specifically includes:

S31、对误差传递动力学模型进行重写，得到重写后的误差传递动力学模型，重写后的误差传递动力学模型中包含规定性能控制器；S31, rewriting the error transmission dynamics model to obtain a rewritten error transmission dynamics model, wherein the rewritten error transmission dynamics model includes a specified performance controller;

S32、根据步骤S23中机器人系统转换后位置误差设置滑模函数；S32, setting a sliding mode function according to the position error after the robot system conversion in step S23;

S33、设置扰动估计误差，根据滑模函数和扰动估计误差设置第一李雅普诺夫函数；S33, setting a disturbance estimation error, and setting a first Lyapunov function according to the sliding mode function and the disturbance estimation error;

S34、根据第一李雅普诺夫函数判定误差传递动力学模型的稳定性，并设计误差传递动力学模型稳定时对应的规定性能控制器。S34. Determine the stability of the error transmission dynamics model according to the first Lyapunov function, and design a controller with a specified performance corresponding to the stability of the error transmission dynamics model.

优选地，S34中的规定性能控制器，具体可用公式表示为：Preferably, the specified performance controller in S34 can be specifically expressed by the formula:

Figure SMS_54
Figure SMS_54

式中，

Figure SMS_58
为机器人系统的输入力矩，
Figure SMS_59
为机器人系统的对称正定惯性矩阵，
Figure SMS_63
为滑模函数，
Figure SMS_57
为规定性能控制器增益，
Figure SMS_60
为扰动估计，也就是实际扰动
Figure SMS_65
的估计值，
Figure SMS_67
为机器人系统转换后位置误差的一阶导数，
Figure SMS_55
为对角增益矩阵，
Figure SMS_61
为机器人系统的位置误差
Figure SMS_66
的一阶导数，
Figure SMS_68
为机器人系统转换后位置误差
Figure SMS_56
的一阶导数，
Figure SMS_62
、
Figure SMS_64
为误差传递动力学模型的中间变量。In the formula,
Figure SMS_58
is the input torque of the robot system,
Figure SMS_59
is the symmetric positive definite inertia matrix of the robot system,
Figure SMS_63
is the sliding mode function,
Figure SMS_57
To specify the performance controller gain,
Figure SMS_60
is the disturbance estimate, that is, the actual disturbance
Figure SMS_65
The estimated value of
Figure SMS_67
is the first-order derivative of the position error of the robot system after conversion,
Figure SMS_55
is the diagonal gain matrix,
Figure SMS_61
is the position error of the robot system
Figure SMS_66
The first-order derivative of
Figure SMS_68
is the position error of the robot system after conversion
Figure SMS_56
The first-order derivative of
Figure SMS_62
,
Figure SMS_64
It is the intermediate variable of the error transfer dynamics model.

优选地，S4具体包括：Preferably, S4 specifically includes:

S41、预设阻抗模型和弹簧模型，根据阻抗模型和弹簧模型推导出末端执行器的接触力误差；S41, presetting an impedance model and a spring model, and deriving a contact force error of the end effector according to the impedance model and the spring model;

S42、根据末端执行器的接触力误差和阻抗模型得到力跟踪误差传递动力学模型；S42, obtaining a force tracking error transmission dynamics model according to the contact force error and impedance model of the end effector;

S43、设计环境刚度估计，根据力跟踪误差传递动力学模型和环境刚度估计设计第二和第三李雅普诺夫函数，通过第二和第三李雅普诺夫函数推导出环境刚度估计的一阶导数；S43, designing an environmental stiffness estimation, designing a second and a third Lyapunov function according to a force tracking error transmission dynamics model and the environmental stiffness estimation, and deriving a first-order derivative of the environmental stiffness estimation through the second and the third Lyapunov function;

S44、根据环境刚度估计的一阶导数和弹簧模型得出机器人系统末端执行器的接触力估计；S44, deriving a contact force estimate of the end effector of the robot system based on the first-order derivative of the environmental stiffness estimate and the spring model;

S45、根据末端执行器的接触力误差和阻抗模型得到机器人系统末端执行器的位置。S45. Obtain the position of the end effector of the robot system according to the contact force error and impedance model of the end effector.

优选地，S42中的力跟踪误差传递动力学模型具体为：Preferably, the force tracking error transmission dynamics model in S42 is specifically:

Figure SMS_69
Figure SMS_69

式中，

Figure SMS_72
、
Figure SMS_74
、
Figure SMS_78
分别为第i个机器人的阻抗模型的惯性、阻尼和刚度，
Figure SMS_71
为第i个机器人环境刚度
Figure SMS_73
的估计值，
Figure SMS_76
为第i个机器人末端执行器的接触力误差，
Figure SMS_79
和
Figure SMS_70
分别为第i个机器人末端执行器的接触力误差
Figure SMS_75
的一阶导数和二阶导数，
Figure SMS_77
。In the formula,
Figure SMS_72
,
Figure SMS_74
,
Figure SMS_78
are the inertia, damping and stiffness of the impedance model of the ith robot,
Figure SMS_71
is the stiffness of the environment of the ith robot
Figure SMS_73
The estimated value of
Figure SMS_76
is the contact force error of the i-th robot end effector,
Figure SMS_79
and
Figure SMS_70
are the contact force errors of the i-th robot end effector
Figure SMS_75
The first and second derivatives of
Figure SMS_77
.

优选地，S44中机器人系统末端执行器的接触力估计，具体公式为：Preferably, the contact force estimation of the end effector of the robot system in S44 is specifically formulated as follows:

Figure SMS_80
Figure SMS_80

式中，

Figure SMS_81
为第i个机器人末端执行器的接触力估计，
Figure SMS_82
为第i个机器人环境刚度
Figure SMS_83
的估计，
Figure SMS_84
为第i个机器人末端执行器的位置，
Figure SMS_85
为目标物体的位置。In the formula,
Figure SMS_81
is the contact force estimate of the i-th robot end effector,
Figure SMS_82
is the stiffness of the environment of the ith robot
Figure SMS_83
The estimate,
Figure SMS_84
is the position of the i-th robot end effector,
Figure SMS_85
is the position of the target object.

优选地，S45中机器人系统末端执行器的位置，具体公式为：Preferably, the position of the end effector of the robot system in S45 is specifically calculated as follows:

Figure SMS_86
Figure SMS_86

式中，

Figure SMS_89
为阻抗模型输出的第i个机器人末端执行器的位置，
Figure SMS_92
为阻抗模型输出的第i个机器人末端执行器位置的一阶导数，
Figure SMS_95
为阻抗模型输入的第i个机器人末端执行器位置，
Figure SMS_87
，
Figure SMS_91
、
Figure SMS_93
分别为第i个阻抗模型的惯性、阻尼和刚度，
Figure SMS_96
和
Figure SMS_88
分别为阻抗模型输入的第i个机器人末端执行器位置的一阶导数和二阶导数，
Figure SMS_90
为第i个机器人末端执行器的位置误差，
Figure SMS_94
为第i个机器人环境刚度
Figure SMS_97
的估计。In the formula,
Figure SMS_89
is the position of the i-th robot end effector output by the impedance model,
Figure SMS_92
is the first-order derivative of the position of the i-th robot end effector output by the impedance model,
Figure SMS_95
is the position of the i-th robot end effector input to the impedance model,
Figure SMS_87
,
Figure SMS_91
,
Figure SMS_93
are the inertia, damping and stiffness of the ith impedance model,
Figure SMS_96
and
Figure SMS_88
are the first-order derivative and second-order derivative of the position of the i-th robot end effector input to the impedance model, respectively.
Figure SMS_90
is the position error of the i-th robot end effector,
Figure SMS_94
is the stiffness of the environment of the ith robot
Figure SMS_97
Estimates.

上述一种多机器人协同搬运力位混合控制方法，通过设计规定性能控制器，实现多个机器人相互之间严格的误差控制，保证在协同搬运过程中的精度；采用自适应阻抗力控制方法设计机器人末端执行器的接触力估计方法以及阻抗控制，保证了机器人在搬运过程中的安全性。The above-mentioned multi-robot collaborative handling force-position hybrid control method realizes strict error control between multiple robots by designing a specified performance controller, thereby ensuring the accuracy during the collaborative handling process; and adopts an adaptive impedance force control method to design the contact force estimation method and impedance control of the robot end effector, thereby ensuring the safety of the robot during the handling process.

附图说明BRIEF DESCRIPTION OF THE DRAWINGS

图1是本发明一实施例中一种多机器人协同搬运力位混合控制方法的流程图；FIG1 is a flow chart of a multi-robot collaborative handling force-position hybrid control method according to an embodiment of the present invention;

图2是本发明一实施例中多机器人协同搬运的俯视图；FIG2 is a top view of multi-robot collaborative handling in one embodiment of the present invention;

图3是本发明一实施例中多机器人协同搬运的场景图；FIG3 is a scene diagram of multi-robot collaborative handling in one embodiment of the present invention;

图4是本发明一实施例中一种多机器人协同搬运力位混合控制方法示意图；FIG4 is a schematic diagram of a multi-robot collaborative handling force-position hybrid control method according to an embodiment of the present invention;

图5是本发明一实施例中三台机器人协同搬运过程中机器人1的x轴分量控制误差；FIG5 is a diagram showing the x-axis component control error of robot 1 during the collaborative handling process of three robots in one embodiment of the present invention;

图6是本发明一实施例中三台机器人协同搬运过程中机器人2的x轴分量控制误差；FIG6 is a diagram showing the x-axis component control error of robot 2 during the collaborative handling process of three robots in one embodiment of the present invention;

图7是本发明一实施例中三台机器人协同搬运过程中机器人3的x轴分量控制误差；7 is a diagram showing the x-axis component control error of robot 3 during the collaborative handling of three robots in one embodiment of the present invention;

图8是本发明一实施例中三台机器人协同搬运过程的效果图；FIG8 is a diagram showing the effect of a collaborative handling process of three robots in one embodiment of the present invention;

图9是本发明一实施例中三台机器人协同搬运过程机器人1的力跟踪效果图；FIG9 is a diagram showing the force tracking effect of robot 1 during the collaborative handling process of three robots in one embodiment of the present invention;

图10是本发明一实施例中三台机器人协同搬运过程机器人2的力跟踪效果图；10 is a diagram showing the force tracking effect of robot 2 during the collaborative handling process of three robots in one embodiment of the present invention;

图11是本发明一实施例中三台机器人协同搬运过程机器人3的力跟踪效果图。FIG. 11 is a diagram showing the force tracking effect of robot 3 during the collaborative handling process of three robots in one embodiment of the present invention.

具体实施方式DETAILED DESCRIPTION

为了使本技术领域的人员更好地理解本发明的技术方案，下面结合附图对本发明作进一步的详细说明。In order to enable those skilled in the art to better understand the technical solution of the present invention, the present invention is further described in detail below in conjunction with the accompanying drawings.

一种多机器人协同搬运力位混合控制方法，具体包括：A multi-robot collaborative handling force-position hybrid control method specifically includes:

S1、建立机器人的搬运动力学模型，根据机器人的搬运动力学模型建立多个机器人组成的机器人系统的协同搬运动力学模型；S1. Establish a robot handling dynamics model, and establish a collaborative handling dynamics model of a robot system composed of multiple robots based on the robot handling dynamics model;

S2、设置机器人位置误差，引入误差转换函数对机器人位置误差进行转换，得到机器人转换后位置误差，根据机器人转换后位置误差和协同搬运动力学模型得到误差传递动力学模型；S2. Set the robot position error, introduce the error conversion function to convert the robot position error, obtain the robot position error after conversion, and obtain the error transfer dynamics model according to the robot position error after conversion and the collaborative handling dynamics model;

S3、重写误差传递动力学模型，得到重写后的误差传递动力学模型，设置滑模函数和扰动估计误差，根据重写后的误差传递动力学模型、滑模函数和扰动估计误差设计规定性能控制器，根据规定性能控制器计算出机器人系统的输入力矩；S3, rewriting the error transfer dynamics model to obtain the rewritten error transfer dynamics model, setting a sliding mode function and a disturbance estimation error, designing a specified performance controller according to the rewritten error transfer dynamics model, the sliding mode function and the disturbance estimation error, and calculating the input torque of the robot system according to the specified performance controller;

S4、预设阻抗模型、弹簧模型和环境刚度估计，根据阻抗模型、弹簧模型和环境刚度估计设计阻抗控制方法，计算出机器人系统末端执行器的接触力估计和位置；S4, presetting an impedance model, a spring model and an estimated environmental stiffness, designing an impedance control method according to the impedance model, the spring model and the estimated environmental stiffness, and calculating a contact force estimate and a position of an end effector of the robot system;

S5、根据协同搬运动力学模型、误差传递动力学模型和规定性能控制器搭建数学仿真模型，将计算出的机器人系统的输入力矩、机器人系统末端执行器的接触力估计以及位置输入至仿真模型中，验证机器人系统的协同搬运控制方法的有效性。S5. Build a mathematical simulation model based on the collaborative handling dynamics model, error transmission dynamics model and specified performance controller, input the calculated input torque of the robot system, the contact force estimation of the end effector of the robot system and the position into the simulation model to verify the effectiveness of the collaborative handling control method of the robot system.

具体地，参见图1、图2、图3和图4，图1为本发明一实施例中的多机器人协同搬运力位混合控制方法流程图；图2为本发明一实施例中多机器人协同搬运的俯视图；图3为本发明一实施例中多机器人协同搬运的场景图；图4为本发明一实施例中一种多机器人协同搬运力位混合控制方法示意图。Specifically, referring to Figures 1, 2, 3 and 4, Figure 1 is a flow chart of a force-position hybrid control method for multi-robot collaborative handling in one embodiment of the present invention; Figure 2 is a top view of multi-robot collaborative handling in one embodiment of the present invention; Figure 3 is a scene diagram of multi-robot collaborative handling in one embodiment of the present invention; Figure 4 is a schematic diagram of a force-position hybrid control method for multi-robot collaborative handling in one embodiment of the present invention.

一种多机器人协同搬运力位混合控制方法，首先建立单个机器人的搬动动力学模型，在此基础上建立多个机器人组成的机器人系统的协同搬运动力学模型；然后设置机器人位置误差，并引入误差转换函数对机器人位置误差进行转换，根据机器人转换后位置误差和协同搬运动力学模型设计误差传递方法，建立误差传递动力学模型；接着对误差传递动力学模型进行重写，设置滑模函数和扰动估计误差，根据重写后的误差传递动力学模型、滑模函数和扰动估计误差设计规定性能控制器并计算得出输入力矩；接着预设阻抗模型、弹簧模型和环境刚度估计，根据阻抗模型、弹簧模型和环境刚度估计设计阻抗控制方法，计算得出阻抗位置输出，阻抗位置输出包括机器人系统末端执行器的接触力估计和位置；最后根据协同搬运动力学模型、误差传递动力学模型、规定性能控制器以及阻抗控制器搭建数学仿真模型，验证多机器人的协同搬运方法的有效性。A hybrid force-position control method for collaborative handling of multiple robots is disclosed. First, a handling dynamics model of a single robot is established, and on this basis, a collaborative handling dynamics model of a robot system composed of multiple robots is established; then, the robot position error is set, and an error conversion function is introduced to convert the robot position error, an error transfer method is designed according to the position error after the robot conversion and the collaborative handling dynamics model, and an error transfer dynamics model is established; then, the error transfer dynamics model is rewritten, a sliding mode function and a disturbance estimation error are set, and a specified performance controller is designed according to the rewritten error transfer dynamics model, the sliding mode function and the disturbance estimation error, and the input torque is calculated; then, an impedance model, a spring model and an environmental stiffness estimate are preset, and an impedance control method is designed according to the impedance model, the spring model and the environmental stiffness estimate, and the impedance position output is calculated, and the impedance position output includes the contact force estimate and position of the end effector of the robot system; finally, a mathematical simulation model is built according to the collaborative handling dynamics model, the error transmission dynamics model, the specified performance controller and the impedance controller to verify the effectiveness of the collaborative handling method of multiple robots.

在一个实施例中，S1中的协同搬运动力学模型具体为：In one embodiment, the collaborative transport dynamics model in S1 is specifically:

Figure SMS_98
Figure SMS_98

其中，

Figure SMS_99
in,
Figure SMS_99

Figure SMS_100
Figure SMS_100

Figure SMS_101
Figure SMS_101

Figure SMS_102
Figure SMS_102

Figure SMS_103
Figure SMS_103

式中，

Figure SMS_106
为机器人系统的对称正定惯性矩阵，
Figure SMS_108
为机器人系统的离心项和克利奥利项矩阵，
Figure SMS_112
为机器人系统在建模过程产生的总摩擦力，
Figure SMS_107
为从机器人系统的关节矢量至工作空间的速度雅可比矩阵，
Figure SMS_109
为重力加速度矩阵，
Figure SMS_113
为机器人系统的关节矢量，
Figure SMS_118
和
Figure SMS_104
分别为机器人系统的关节矢量
Figure SMS_110
的一阶导数和二阶导数，
Figure SMS_114
为机器人系统的自由度，
Figure SMS_117
，
Figure SMS_105
为第
Figure SMS_111
个机器人的自由度，
Figure SMS_115
为机器人系统末端执行器的实际接触力，
Figure SMS_116
为机器人系统的输入力矩。In the formula,
Figure SMS_106
is the symmetric positive definite inertia matrix of the robot system,
Figure SMS_108
is the centrifugal and Creole term matrices of the robot system,
Figure SMS_112
is the total friction force generated by the robot system during the modeling process,
Figure SMS_107
is the velocity Jacobian matrix from the joint vectors of the robot system to the workspace,
Figure SMS_109
is the gravitational acceleration matrix,
Figure SMS_113
is the joint vector of the robot system,
Figure SMS_118
and
Figure SMS_104
are the joint vectors of the robot system
Figure SMS_110
The first and second derivatives of
Figure SMS_114
is the degree of freedom of the robot system,
Figure SMS_117
,
Figure SMS_105
For the
Figure SMS_111
degrees of freedom of the robot,
Figure SMS_115
is the actual contact force of the end effector of the robot system,
Figure SMS_116
is the input torque of the robot system.

具体地，建立机器人系统的协同搬运动力学模型包括如下步骤：Specifically, establishing the collaborative handling dynamics model of the robot system includes the following steps:

1）建立单个机器人的动力学模型：1) Establish a dynamic model of a single robot:

Figure SMS_119
（1）
Figure SMS_119
(1)

其中，

Figure SMS_120
in,
Figure SMS_120

Figure SMS_121
Figure SMS_121

式中，

Figure SMS_123
为第i个机器人的对称正定惯性矩阵，
Figure SMS_127
为第i个机器人的离心项和克利奥利项矩阵，
Figure SMS_130
为第i个机器人建模过程产生的摩擦力，
Figure SMS_125
第i个机器人的重力加速度矩阵，
Figure SMS_128
为第i个机器人的重力加速度矢量，
Figure SMS_133
为第i个机器人的关节矢量（包括移动端和机械臂），
Figure SMS_135
和
Figure SMS_122
分别为第i个机器人的关节矢量
Figure SMS_129
的一阶导数和二阶导数，
Figure SMS_132
为第i个机器人的输入力矩，
Figure SMS_134
为第i个机器人的末端执行器受到的力，
Figure SMS_124
为从第i个机器人的关节矢量
Figure SMS_126
到工作空间
Figure SMS_131
的速度雅可比矩阵，可由力传递过程得到，具体如下：In the formula,
Figure SMS_123
is the symmetric positive definite inertia matrix of the ith robot,
Figure SMS_127
is the centrifugal term and Creole term matrix of the ith robot,
Figure SMS_130
The friction generated by the modeling process of the i-th robot,
Figure SMS_125
The gravity acceleration matrix of the ith robot,
Figure SMS_128
is the gravity acceleration vector of the ith robot,
Figure SMS_133
is the joint vector of the ith robot (including the mobile terminal and the robotic arm),
Figure SMS_135
and
Figure SMS_122
are the joint vectors of the i-th robot
Figure SMS_129
The first and second derivatives of
Figure SMS_132
is the input torque of the ith robot,
Figure SMS_134
is the force on the end effector of the ith robot,
Figure SMS_124
is the joint vector from the ith robot
Figure SMS_126
To Workspace
Figure SMS_131
The velocity Jacobian matrix of can be obtained from the force transfer process, as follows:

Figure SMS_136
，
Figure SMS_137
为第i个机器人末端执行器坐标，也就是第i个机器人的工作空间，一般情况下
Figure SMS_138
，通过对第i个机器人末端执行器坐标
Figure SMS_139
求一阶导数可以得到：
Figure SMS_136
,
Figure SMS_137
is the coordinate of the end effector of the ith robot, that is, the workspace of the ith robot. Generally,
Figure SMS_138
, by calculating the coordinates of the end effector of the i-th robot
Figure SMS_139
Taking the first-order derivative we get:

Figure SMS_140
，因此，取
Figure SMS_141
。
Figure SMS_140
, therefore, take
Figure SMS_141
.

则机器人系统的工作空间为：

Figure SMS_142
。Then the working space of the robot system is:
Figure SMS_142
.

2）建立由多个机器人组成机器人系统的协同搬运动力学模型：2) Establish a collaborative handling dynamics model of a robot system consisting of multiple robots:

Figure SMS_143
（2）
Figure SMS_143
(2)

其中，

Figure SMS_144
in,
Figure SMS_144

Figure SMS_145
Figure SMS_145

Figure SMS_146
Figure SMS_146

Figure SMS_147
Figure SMS_147

Figure SMS_148
Figure SMS_148

Figure SMS_149
Figure SMS_149

式中，

Figure SMS_151
为机器人系统的对称正定惯性矩阵，
Figure SMS_153
为机器人系统的离心项和克利奥利项矩阵，
Figure SMS_154
为机器人系统在建模过程产生的总摩擦力，
Figure SMS_150
为机器人系统的关节矢量，
Figure SMS_155
为从机器人系统的关节矢量至工作空间的速度雅可比矩阵，
Figure SMS_157
为机器人系统的总自由度，
Figure SMS_158
为第
Figure SMS_152
个机器人的自由度，
Figure SMS_156
表示实数域。In the formula,
Figure SMS_151
is the symmetric positive definite inertia matrix of the robot system,
Figure SMS_153
is the centrifugal and Creole term matrices of the robot system,
Figure SMS_154
is the total friction force generated by the robot system during the modeling process,
Figure SMS_150
is the joint vector of the robot system,
Figure SMS_155
is the velocity Jacobian matrix from the joint vectors of the robot system to the workspace,
Figure SMS_157
is the total degree of freedom of the robot system,
Figure SMS_158
For the
Figure SMS_152
degrees of freedom of the robot,
Figure SMS_156
Represents the field of real numbers.

在一个实施例中，S2具体包括：In one embodiment, S2 specifically includes:

S21、设置机器人的规定性能和性能函数，根据规定性能和性能函数确定机器人的位置误差；S21, setting the prescribed performance and performance function of the robot, and determining the position error of the robot according to the prescribed performance and performance function;

S22、设置误差转换函数，使用误差转换函数对机器人的位置误差进行转换，得到机器人转换后位置误差；S22, setting an error conversion function, and using the error conversion function to convert the position error of the robot to obtain the position error of the robot after conversion;

S23、根据机器人转换后位置误差建立机器人系统转换后位置误差，对机器人系统转换后位置误差进行处理，并结合协同搬运动力学模型，得到误差传递动力学模型。S23. Establishing the position error of the robot system after conversion according to the position error of the robot after conversion, processing the position error of the robot system after conversion, and combining it with the collaborative handling dynamics model to obtain the error transfer dynamics model.

在一个实施例中，S23中的误差传递动力学模型，具体公式为：In one embodiment, the error transmission dynamics model in S23 is specifically formulated as follows:

Figure SMS_159
Figure SMS_159

其中，

Figure SMS_160
in,
Figure SMS_160

Figure SMS_161
Figure SMS_161

Figure SMS_162
Figure SMS_162

Figure SMS_163
Figure SMS_163

Figure SMS_164
Figure SMS_164

Figure SMS_165
Figure SMS_165

Figure SMS_166
Figure SMS_166

式中，

Figure SMS_181
为机器人系统转换后位置误差
Figure SMS_186
的二阶导数，
Figure SMS_188
为机器人系统的对称正定惯性矩阵，
Figure SMS_168
为机器人系统的离心项和克利奥利项矩阵，
Figure SMS_172
为从机器人系统的关节矢量至工作空间的速度雅可比矩阵，
Figure SMS_177
为重力加速度矩阵，
Figure SMS_179
和
Figure SMS_169
表示第i个机器人系统规定性能的上下界，
Figure SMS_174
表示第i个机器人的性能函数，
Figure SMS_182
为机器人系统的输入力矩，
Figure SMS_185
为机器人系统的位置误差
Figure SMS_170
的一阶导数，
Figure SMS_173
为第i个机器人的位置误差，
Figure SMS_175
为机器人系统的关节矢量
Figure SMS_178
的一阶导数，
Figure SMS_184
为第i个机器人的关节矢量，
Figure SMS_187
为机器人系统的期望位置
Figure SMS_189
的二阶导数，
Figure SMS_190
为第i个机器人的希望位置，
Figure SMS_167
为机器人系统末端执行器的实际接触力，
Figure SMS_171
、
Figure SMS_176
、
Figure SMS_180
、
Figure SMS_183
为误差传递动力学模型的中间变量。In the formula,
Figure SMS_181
is the position error of the robot system after conversion
Figure SMS_186
The second-order derivative of
Figure SMS_188
is the symmetric positive definite inertia matrix of the robot system,
Figure SMS_168
is the centrifugal and Creole term matrices of the robot system,
Figure SMS_172
is the velocity Jacobian matrix from the joint vectors of the robot system to the workspace,
Figure SMS_177
is the gravitational acceleration matrix,
Figure SMS_179
and
Figure SMS_169
represents the upper and lower bounds of the performance requirements of the ith robot system,
Figure SMS_174
represents the performance function of the ith robot,
Figure SMS_182
is the input torque of the robot system,
Figure SMS_185
is the position error of the robot system
Figure SMS_170
The first-order derivative of
Figure SMS_173
is the position error of the ith robot,
Figure SMS_175
is the joint vector of the robot system
Figure SMS_178
The first-order derivative of
Figure SMS_184
is the joint vector of the ith robot,
Figure SMS_187
is the desired position of the robot system
Figure SMS_189
The second-order derivative of
Figure SMS_190
is the desired position of the ith robot,
Figure SMS_167
is the actual contact force of the end effector of the robot system,
Figure SMS_171
,
Figure SMS_176
,
Figure SMS_180
,
Figure SMS_183
It is the intermediate variable of the error transfer dynamics model.

具体地，由于每个机器人的期望位置是有界的，可基于此定义机器人的位置误差：Specifically, since the desired position of each robot is bounded, the position error of the robot can be defined based on this:

Figure SMS_191
（3）
Figure SMS_191
(3)

式中，

Figure SMS_192
为第i个机器人的位置误差，
Figure SMS_193
为第i个机器人的关节矢量，
Figure SMS_194
为第i个机器人的期望位置。In the formula,
Figure SMS_192
is the position error of the ith robot,
Figure SMS_193
is the joint vector of the ith robot,
Figure SMS_194
is the expected position of the ith robot.

定义机器人规定性能的上、下界和性能函数，根据规定性能的上下界以及性能函数设置机器人的位置误差范围：Define the upper and lower bounds of the robot's specified performance and the performance function, and set the robot's position error range according to the upper and lower bounds of the specified performance and the performance function:

Figure SMS_195
（4）
Figure SMS_195
(4)

其中，

Figure SMS_196
in,
Figure SMS_196

式中，

Figure SMS_198
和
Figure SMS_202
分别为第i个机器人规定性能的上、下界，
Figure SMS_205
为第i个机器人的性能函数，
Figure SMS_199
、
Figure SMS_201
、
Figure SMS_206
均为正常数，
Figure SMS_208
和
Figure SMS_200
分别表示性能函数
Figure SMS_203
在
Figure SMS_207
和
Figure SMS_209
时的值，且
Figure SMS_197
，
Figure SMS_204
表示第i个机器人的性能函数的逼近速度。In the formula,
Figure SMS_198
and
Figure SMS_202
are the upper and lower bounds of the performance of the ith robot,
Figure SMS_205
is the performance function of the ith robot,
Figure SMS_199
,
Figure SMS_201
,
Figure SMS_206
are all normal numbers,
Figure SMS_208
and
Figure SMS_200
Represents performance function
Figure SMS_203
exist
Figure SMS_207
and
Figure SMS_209
The value of
Figure SMS_197
,
Figure SMS_204
represents the approximation speed of the performance function of the ith robot.

为了实现对多个机器人的规定性能控制，需要设计误差传递动力学模型，设计过程如下：In order to achieve the specified performance control of multiple robots, it is necessary to design an error transmission dynamics model. The design process is as follows:

1）首先引入误差转换函数对每个机器人的位置误差进行误差转换，得到机器人转换后位置误差：1) First, the error conversion function is introduced to convert the position error of each robot to obtain the position error of the robot after conversion:

设置误差转换函数

Figure SMS_210
，
Figure SMS_211
，该误差转换函数满足
Figure SMS_212
。Setting the error transfer function
Figure SMS_210
,
Figure SMS_211
, the error transfer function satisfies
Figure SMS_212
.

经过误差转换，得到机器人转换后位置误差：After error conversion, the position error of the robot after conversion is obtained:

Figure SMS_213
（5）
Figure SMS_213
(5)

其中，

Figure SMS_214
in,
Figure SMS_214

式中，

Figure SMS_215
为第i个机器人转换后位置误差，
Figure SMS_216
和
Figure SMS_217
分别为第i个机器人规定性能的上、下界，
Figure SMS_218
为第i个机器人的位置误差，
Figure SMS_219
为第i个机器人的性能函数，
Figure SMS_220
为中间变量。In the formula,
Figure SMS_215
is the position error of the i-th robot after transformation,
Figure SMS_216
and
Figure SMS_217
are the upper and lower bounds of the performance of the ith robot,
Figure SMS_218
is the position error of the ith robot,
Figure SMS_219
is the performance function of the ith robot,
Figure SMS_220
is an intermediate variable.

在此基础上，机器人系统转换后位置误差可表示为：On this basis, the position error of the robot system after conversion can be expressed as:

Figure SMS_221
（6）
Figure SMS_221
(6)

2）对机器人系统转换后位置误差求一阶导数：2) Calculate the first-order derivative of the position error after the robot system conversion:

Figure SMS_222
（7）
Figure SMS_222
(7)

Figure SMS_223
Figure SMS_223

Figure SMS_224
Figure SMS_224

其中，

Figure SMS_225
in,
Figure SMS_225

Figure SMS_226
Figure SMS_226

式中，

Figure SMS_227
、
Figure SMS_228
为中间变量。In the formula,
Figure SMS_227
,
Figure SMS_228
is an intermediate variable.

3）对机器人系统转换后位置误差求二阶导数，并结合协同搬运动力学模型得到误差传递动力学模型：3) Calculate the second-order derivative of the position error after the robot system conversion, and combine it with the collaborative handling dynamics model to obtain the error transmission dynamics model:

对机器人系统转换后位置误差求二阶导数：Calculate the second-order derivative of the position error after the robot system transformation:

Figure SMS_229
（8）
Figure SMS_229
(8)

将协同搬运动力学模型公式（2）代入上式（8），可以得出误差传递动力学模型，具体公式如下：Substituting the cooperative transport dynamics model formula (2) into the above formula (8), the error transmission dynamics model can be obtained. The specific formula is as follows:

Figure SMS_230
（9）
Figure SMS_230
(9)

其中，

Figure SMS_231
in,
Figure SMS_231

式中，

Figure SMS_232
为机器人系统转换后位置误差的二阶导数，
Figure SMS_233
为机器人系统的对称正定惯性矩阵，
Figure SMS_234
为重力加速度矩阵，
Figure SMS_235
为机器人系统的输入力矩，
Figure SMS_236
为机器人系统的期望位置的二阶导数，
Figure SMS_237
为中间变量。In the formula,
Figure SMS_232
is the second-order derivative of the position error of the robot system after transformation,
Figure SMS_233
is the symmetric positive definite inertia matrix of the robot system,
Figure SMS_234
is the gravitational acceleration matrix,
Figure SMS_235
is the input torque of the robot system,
Figure SMS_236
is the second-order derivative of the desired position of the robot system,
Figure SMS_237
is an intermediate variable.

在一个实施例中，S3具体包括：In one embodiment, S3 specifically includes:

S31、对误差传递动力学模型进行重写，得到重写后的误差传递动力学模型，重写后的误差传递动力学模型中包含规定性能控制器；S31, rewriting the error transmission dynamics model to obtain a rewritten error transmission dynamics model, wherein the rewritten error transmission dynamics model includes a specified performance controller;

S32、根据步骤S23中机器人系统转换后位置误差设置滑模函数；S32, setting a sliding mode function according to the position error after the robot system conversion in step S23;

S33、设置扰动估计误差，根据滑模函数和扰动估计误差设置第一李雅普诺夫函数；S33, setting a disturbance estimation error, and setting a first Lyapunov function according to the sliding mode function and the disturbance estimation error;

S34、根据第一李雅普诺夫函数判定误差传递动力学模型的稳定性，并设计误差传递动力学模型稳定时对应的规定性能控制器。S34. Determine the stability of the error transmission dynamics model according to the first Lyapunov function, and design a controller with a specified performance corresponding to the stability of the error transmission dynamics model.

在一个实施例中，S34中的规定性能控制器，具体可用公式表示为：In one embodiment, the specified performance controller in S34 can be specifically expressed by the formula:

Figure SMS_238
Figure SMS_238

式中，

Figure SMS_242
为机器人系统的输入力矩，
Figure SMS_243
为机器人系统的对称正定惯性矩阵，
Figure SMS_246
为滑模函数，
Figure SMS_239
为规定性能控制器增益，
Figure SMS_245
为扰动估计，也就是实际扰动
Figure SMS_249
的估计值，
Figure SMS_251
为机器人系统转换后位置误差的一阶导数，
Figure SMS_240
为对角增益矩阵，
Figure SMS_247
为机器人系统的位置误差
Figure SMS_250
的一阶导数，
Figure SMS_252
为机器人系统转换后位置误差
Figure SMS_241
的一阶导数，
Figure SMS_244
、
Figure SMS_248
为误差传递动力学模型的中间变量。In the formula,
Figure SMS_242
is the input torque of the robot system,
Figure SMS_243
is the symmetric positive definite inertia matrix of the robot system,
Figure SMS_246
is the sliding mode function,
Figure SMS_239
To specify the performance controller gain,
Figure SMS_245
is the disturbance estimate, that is, the actual disturbance
Figure SMS_249
The estimated value of
Figure SMS_251
is the first-order derivative of the position error of the robot system after conversion,
Figure SMS_240
is the diagonal gain matrix,
Figure SMS_247
is the position error of the robot system
Figure SMS_250
The first-order derivative of
Figure SMS_252
is the position error of the robot system after conversion
Figure SMS_241
The first-order derivative of
Figure SMS_244
,
Figure SMS_248
It is the intermediate variable of the error transfer dynamics model.

具体地，根据误差传递动力学模型设计规定性能控制器，过程如下：Specifically, the specified performance controller is designed according to the error transmission dynamics model, and the process is as follows:

1）对误差传递动力学模型公式进行重写，得到重写后的误差传递动力学模型1) Rewrite the error transmission dynamics model formula to obtain the rewritten error transmission dynamics model

Figure SMS_253
（10）
Figure SMS_253
(10)

其中，

Figure SMS_254
（11）in,
Figure SMS_254
(11)

Figure SMS_255
（12）
Figure SMS_255
(12)

式中，

Figure SMS_256
为控制输入，是一个中间变量，用来通过公式（11）进行
Figure SMS_257
的解算。
Figure SMS_258
为实际扰动，即操纵过程中机器人末端执行器受到的力和内外扰动力之和。In the formula,
Figure SMS_256
is the control input, which is an intermediate variable used to perform
Figure SMS_257
The solution.
Figure SMS_258
is the actual disturbance, that is, the sum of the force on the robot end effector and the internal and external disturbance forces during the manipulation process.

2）根据机器人系统转换后位置误差及其一阶导数设置滑模函数2) Set the sliding mode function according to the position error and its first-order derivative after the robot system conversion

Figure SMS_259
（13）
Figure SMS_259
(13)

式中，

Figure SMS_260
为滑模函数，
Figure SMS_261
为对角增益矩阵，
Figure SMS_262
为机器人系统转换后位置误差，
Figure SMS_263
为机器人系统转换后位置误差的一阶导数。In the formula,
Figure SMS_260
is the sliding mode function,
Figure SMS_261
is the diagonal gain matrix,
Figure SMS_262
is the position error of the robot system after conversion,
Figure SMS_263
is the first-order derivative of the position error of the robot system after conversion.

3）对滑模函数求一阶导数，结合重写后的误差传递动力学模型（10），得到滑模函数的一阶导数：3) Calculate the first-order derivative of the sliding mode function and combine it with the rewritten error transmission dynamics model (10) to obtain the first-order derivative of the sliding mode function:

Figure SMS_264
（14）
Figure SMS_264
(14)

式中，

Figure SMS_265
为滑模函数的一阶导数。In the formula,
Figure SMS_265
is the first-order derivative of the sliding mode function.

4）根据外部扰动

Figure SMS_266
、扰动估计
Figure SMS_267
计算扰动估计误差
Figure SMS_268
：4) According to external disturbance
Figure SMS_266
, disturbance estimation
Figure SMS_267
Calculate the perturbation estimate error
Figure SMS_268
:

Figure SMS_269
（15）
Figure SMS_269
(15)

为了验证误差传递动力学模型、重写后的误差传递动力学模型以及误差扰动估计误差模型的稳定性，引入李雅普诺夫函数并对相关参数进行求解，具体过程如下：In order to verify the stability of the error propagation dynamics model, the rewritten error propagation dynamics model and the error disturbance estimation error model, the Lyapunov function is introduced and the relevant parameters are solved. The specific process is as follows:

1）在重写后的误差传递动力学模型（10）的基础上，考虑滑模函数

Figure SMS_270
与扰动估计误差
Figure SMS_271
，设置第一李雅普诺夫函数：1) Based on the rewritten error propagation dynamics model (10), the sliding mode function is considered
Figure SMS_270
and disturbance estimation error
Figure SMS_271
, set the first Lyapunov function:

Figure SMS_272
（16）
Figure SMS_272
(16)

式中，

Figure SMS_273
为第一李雅普诺夫函数，
Figure SMS_274
为扰动估计误差，
Figure SMS_275
为滑模函数。In the formula,
Figure SMS_273
is the first Lyapunov function,
Figure SMS_274
is the disturbance estimation error,
Figure SMS_275
is a sliding mode function.

对公式（16）中的第一李雅普诺夫函数求一阶导数，并将公式（14）代入可得：Taking the first-order derivative of the first Lyapunov function in formula (16) and substituting formula (14) into it, we can obtain:

Figure SMS_276
（17）
Figure SMS_276
(17)

式中，

Figure SMS_277
为第一李雅普诺夫函数的一阶导数，
Figure SMS_278
为实际扰动，
Figure SMS_279
为扰动估计误差，
Figure SMS_280
为控制输入，是一个中间变量，
Figure SMS_281
为扰动估计误差的一阶导数，
Figure SMS_282
为滑模函数。In the formula,
Figure SMS_277
is the first derivative of the first Lyapunov function,
Figure SMS_278
is the actual disturbance,
Figure SMS_279
is the disturbance estimation error,
Figure SMS_280
is the control input, an intermediate variable,
Figure SMS_281
is the first-order derivative of the disturbance estimation error,
Figure SMS_282
is a sliding mode function.

2）根据第一李雅普诺夫函数的一阶导数设计规定性能控制器：2) Design a controller with specified performance based on the first derivative of the first Lyapunov function:

当第一李雅普诺夫函数的一阶导数不大于0，即

Figure SMS_283
时，说明前面得到的误差传递动力学模型、重写后的误差传递动力学模型以及扰动估计误差是稳定性的。因此，通过计算得出在
Figure SMS_284
时，需要将对应的规定性能控制器设计为：When the first derivative of the first Lyapunov function is not greater than 0, that is
Figure SMS_283
, it shows that the error transmission dynamics model obtained previously, the rewritten error transmission dynamics model and the disturbance estimation error are stable. Therefore, it is calculated that
Figure SMS_284
When , the corresponding specified performance controller needs to be designed as:

Figure SMS_285
（18）
Figure SMS_285
(18)

因此可以通过公式（18）进行

Figure SMS_286
的解算，在此基础上，通过公式（11）计算出机器人系统的输入力矩
Figure SMS_287
，其中，
Figure SMS_288
可使用干扰观测器得出，具体公式如下：Therefore, we can use formula (18) to
Figure SMS_286
On this basis, the input torque of the robot system is calculated by formula (11):
Figure SMS_287
,in,
Figure SMS_288
It can be obtained by using the disturbance observer, the specific formula is as follows:

Figure SMS_289
（19）
Figure SMS_289
(19)

式中，

Figure SMS_290
为滑模函数，
Figure SMS_291
为规定性能控制器增益，
Figure SMS_292
为扰动估计，也就是实际扰动
Figure SMS_293
的估计值，
Figure SMS_294
为正增益矩阵。In the formula,
Figure SMS_290
is the sliding mode function,
Figure SMS_291
To specify the performance controller gain,
Figure SMS_292
is the disturbance estimate, that is, the actual disturbance
Figure SMS_293
The estimated value of
Figure SMS_294
is a positive gain matrix.

3）根据公式（19）求扰动估计

Figure SMS_295
的一阶导数：3) According to formula (19), the disturbance estimate is obtained
Figure SMS_295
The first derivative of :

Figure SMS_296
（20）
Figure SMS_296
(20)

式中，

Figure SMS_297
为扰动估计的一阶导数。In the formula,
Figure SMS_297
is the first derivative of the disturbance estimate.

4）根据公式（15）和公式（20），通过扰动估计误差

Figure SMS_298
和扰动估计的一阶导数
Figure SMS_299
计算扰动估计误差的一阶导数，具体公式为：4) According to formula (15) and formula (20), the error is estimated by perturbation
Figure SMS_298
and the first derivative of the perturbation estimate
Figure SMS_299
Calculate the first-order derivative of the disturbance estimation error. The specific formula is:

Figure SMS_300
（21）
Figure SMS_300
(twenty one)

式中，

Figure SMS_301
为扰动估计误差，
Figure SMS_302
为扰动估计误差的一阶导数，
Figure SMS_303
为正增益矩阵。In the formula,
Figure SMS_301
is the disturbance estimation error,
Figure SMS_302
is the first-order derivative of the disturbance estimation error,
Figure SMS_303
is a positive gain matrix.

5）将公式（18）和（21）代入公式（17），计算得到第一李雅普诺夫函数的一阶导数：5) Substitute formula (18) and (21) into formula (17) to calculate the first-order derivative of the first Lyapunov function:

Figure SMS_304
（22）
Figure SMS_304
(twenty two)

通过分析公式（22）可知，为了使

Figure SMS_305
，就应该使
Figure SMS_306
。假设由于机器人的性能约束，即使在搬运过程中，外部扰动的变化率
Figure SMS_307
仍然可以认为是未知有界的，即
Figure SMS_308
。由不等式可以得出：By analyzing formula (22), we can know that in order to make
Figure SMS_305
, we should make
Figure SMS_306
Assume that due to the performance constraints of the robot, even during the handling process, the rate of change of the external disturbance
Figure SMS_307
It can still be considered as unknown and bounded, that is,
Figure SMS_308
. From the inequality we can conclude that:

Figure SMS_309
（23）
Figure SMS_309
(twenty three)

将上式（23）代入到公式（22），可以得出：Substituting the above formula (23) into formula (22), we can get:

Figure SMS_310
（24）
Figure SMS_310
(twenty four)

其中，

Figure SMS_311
in,
Figure SMS_311

式中，

Figure SMS_312
为规定性能控制器增益，
Figure SMS_313
为正增益矩阵，
Figure SMS_314
为单位向量。In the formula,
Figure SMS_312
To specify the performance controller gain,
Figure SMS_313
is the positive gain matrix,
Figure SMS_314
is a unit vector.

由上述可以得出：当第一李雅普诺夫函数导数

Figure SMS_315
小于零，就意味着机器人系统转换后位置误差
Figure SMS_316
趋向于0，渐近稳定。From the above, we can conclude that when the first Lyapunov function derivative
Figure SMS_315
Less than zero, it means that the position error of the robot system after conversion
Figure SMS_316
tends to 0 and is asymptotically stable.

由

Figure SMS_317
以及公式（18）计算得到规定性能控制器，具体可用公式表示为：Depend on
Figure SMS_317
And formula (18) is used to calculate the specified performance controller, which can be expressed by the formula:

Figure SMS_318
（25）
Figure SMS_318
(25)

式中，

Figure SMS_319
为机器人系统的输入力矩。In the formula,
Figure SMS_319
is the input torque of the robot system.

在一个实施例中，S4具体包括：In one embodiment, S4 specifically includes:

S41、预设阻抗模型和弹簧模型，根据阻抗模型和弹簧模型推导出末端执行器的接触力误差；S41, presetting an impedance model and a spring model, and deriving a contact force error of the end effector according to the impedance model and the spring model;

S42、根据末端执行器的接触力误差和阻抗模型得到力跟踪误差传递动力学模型；S42, obtaining a force tracking error transmission dynamics model according to the contact force error and impedance model of the end effector;

S43、设计环境刚度估计，根据力跟踪误差传递动力学模型和环境刚度估计设计第二和第三李雅普诺夫函数，通过第二和第三李雅普诺夫函数推导出环境刚度估计的一阶导数；S43, designing an environmental stiffness estimation, designing a second and a third Lyapunov function according to a force tracking error transmission dynamics model and the environmental stiffness estimation, and deriving a first-order derivative of the environmental stiffness estimation through the second and the third Lyapunov function;

S44、根据环境刚度估计的一阶导数和弹簧模型得出机器人系统末端执行器的接触力估计；S44, deriving a contact force estimate of the end effector of the robot system based on the first-order derivative of the environmental stiffness estimate and the spring model;

S45、根据末端执行器的接触力误差和阻抗模型得到机器人系统末端执行器的位置。S45. Obtain the position of the end effector of the robot system according to the contact force error and impedance model of the end effector.

在一个实施例中，S42中的力跟踪误差传递动力学模型具体为：In one embodiment, the force tracking error transmission dynamics model in S42 is specifically:

Figure SMS_320
Figure SMS_320

式中，

Figure SMS_321
、
Figure SMS_326
、
Figure SMS_329
分别为第i个机器人的阻抗模型的惯性、阻尼和刚度，
Figure SMS_323
为第i个机器人环境刚度
Figure SMS_324
的估计值，
Figure SMS_327
为第i个机器人末端执行器的接触力误差，
Figure SMS_330
和
Figure SMS_322
分别为第i个机器人末端执行器的接触力误差
Figure SMS_325
的一阶导数和二阶导数，
Figure SMS_328
。In the formula,
Figure SMS_321
,
Figure SMS_326
,
Figure SMS_329
are the inertia, damping and stiffness of the impedance model of the ith robot,
Figure SMS_323
is the stiffness of the environment of the ith robot
Figure SMS_324
The estimated value of
Figure SMS_327
is the contact force error of the i-th robot end effector,
Figure SMS_330
and
Figure SMS_322
are the contact force errors of the i-th robot end effector
Figure SMS_325
The first and second derivatives of
Figure SMS_328
.

在一个实施例中，S44中机器人系统末端执行器的接触力估计，具体公式为：In one embodiment, the contact force of the end effector of the robot system in S44 is estimated by the following formula:

Figure SMS_331
Figure SMS_331

式中，

Figure SMS_332
为第i个机器人末端执行器的接触力估计，
Figure SMS_333
为第i个机器人环境刚度
Figure SMS_334
的估计，
Figure SMS_335
为第i个机器人末端执行器的位置，
Figure SMS_336
为目标物体的位置。In the formula,
Figure SMS_332
is the contact force estimate of the i-th robot end effector,
Figure SMS_333
is the stiffness of the environment of the ith robot
Figure SMS_334
The estimate,
Figure SMS_335
is the position of the i-th robot end effector,
Figure SMS_336
is the position of the target object.

在一个实施例中，S45中机器人系统末端执行器的位置，具体公式为：In one embodiment, the position of the end effector of the robot system in S45 is specifically expressed as:

Figure SMS_337
Figure SMS_337

式中，

Figure SMS_340
为阻抗模型输出的第i个机器人末端执行器的位置，
Figure SMS_343
为阻抗模型输出的第i个机器人末端执行器位置的一阶导数，
Figure SMS_346
为阻抗模型输入的第i个机器人末端执行器位置，
Figure SMS_339
，
Figure SMS_341
、
Figure SMS_344
分别为第i个阻抗模型的惯性、阻尼和刚度，
Figure SMS_347
和
Figure SMS_338
分别为阻抗模型输入的第i个机器人末端执行器位置的一阶导数和二阶导数，
Figure SMS_342
为第i个机器人末端执行器的位置误差，
Figure SMS_345
为第i个机器人环境刚度
Figure SMS_348
的估计。In the formula,
Figure SMS_340
is the position of the i-th robot end effector output by the impedance model,
Figure SMS_343
is the first-order derivative of the position of the i-th robot end effector output by the impedance model,
Figure SMS_346
is the position of the i-th robot end effector input to the impedance model,
Figure SMS_339
,
Figure SMS_341
,
Figure SMS_344
are the inertia, damping and stiffness of the ith impedance model,
Figure SMS_347
and
Figure SMS_338
are the first-order derivative and second-order derivative of the position of the i-th robot end effector input to the impedance model, respectively.
Figure SMS_342
is the position error of the i-th robot end effector,
Figure SMS_345
is the stiffness of the environment of the ith robot
Figure SMS_348
Estimates.

具体地，考虑机器人系统末端执行器的搬运安全性，设计阻抗控制方法，计算得出机器人系统末端执行器的接触力估计与机器人系统末端执行器的位置，过程如下：Specifically, considering the handling safety of the end effector of the robot system, an impedance control method is designed to calculate the contact force estimation of the end effector of the robot system and the position of the end effector of the robot system. The process is as follows:

1）定义广义目标阻抗模型1) Define the generalized target impedance model

Figure SMS_349
（26）
Figure SMS_349
(26)

式中，

Figure SMS_350
为第i个机器人阻抗模型输出的末端执行器位置，也就是阻抗参考位置输出，
Figure SMS_351
为第i个机器人阻抗模型输入的末端执行器位置，
Figure SMS_352
，
Figure SMS_353
、
Figure SMS_354
分别为第i个阻抗模型的惯性、阻尼和刚度，
Figure SMS_355
为第i个机器人末端执行器的接触力误差。In the formula,
Figure SMS_350
is the end effector position output by the impedance model of the ith robot, that is, the impedance reference position output,
Figure SMS_351
is the end effector position input to the impedance model of the ith robot,
Figure SMS_352
,
Figure SMS_353
,
Figure SMS_354
are the inertia, damping and stiffness of the ith impedance model,
Figure SMS_355
is the contact force error of the i-th robot end effector.

2）定义末端执行器的接触力误差：2) Define the contact force error of the end effector:

Figure SMS_356
（27）
Figure SMS_356
(27)

式中，

Figure SMS_357
为第i个机器人末端执行器的接触力误差，
Figure SMS_358
为第i个机器人末端执行器的参考接触力（简称为参考力），
Figure SMS_359
为第i个机器人末端执行器的实际接触力（简称为实际力）。在实际应用中，末端执行器的实际力
Figure SMS_360
可以从弹簧模型中获得，弹簧模型可以表示为：In the formula,
Figure SMS_357
is the contact force error of the i-th robot end effector,
Figure SMS_358
is the reference contact force of the i-th robot end effector (referred to as reference force),
Figure SMS_359
is the actual contact force of the end effector of the ith robot (referred to as actual force). In practical applications, the actual force of the end effector
Figure SMS_360
It can be obtained from the spring model, which can be expressed as:

Figure SMS_361
（28）
Figure SMS_361
(28)

式中，

Figure SMS_362
为第i个机器人末端执行器的位置，
Figure SMS_363
为目标物体的位置，
Figure SMS_364
第i个机器人的环境刚度，
Figure SMS_365
。In the formula,
Figure SMS_362
is the position of the i-th robot end effector,
Figure SMS_363
is the position of the target object,
Figure SMS_364
The environmental stiffness of the ith robot,
Figure SMS_365
.

3）由公式（27）和（28）计算得出机器人末端执行器的位置：3) The position of the robot end effector is calculated by formulas (27) and (28):

Figure SMS_366
（29）
Figure SMS_366
(29)

假设机器人末端执行器的位置达到阻抗模型输出的末端执行器的位置，即

Figure SMS_367
，根据公式（28）和（29）可以得出：Assume that the position of the robot end effector reaches the position of the end effector output by the impedance model, that is,
Figure SMS_367
According to formulas (28) and (29), we can get:

Figure SMS_368
（30）
Figure SMS_368
(30)

式中，

Figure SMS_369
为第i个机器人末端执行器的接触力误差，
Figure SMS_370
为第i个阻抗模型的刚度，
Figure SMS_371
为第i个机器人末端执行器的参考力，
Figure SMS_372
为阻抗模型输入的第i个机器人末端执行器位置，
Figure SMS_373
为第i个机器人环境刚度。In the formula,
Figure SMS_369
is the contact force error of the i-th robot end effector,
Figure SMS_370
is the stiffness of the ith impedance model,
Figure SMS_371
is the reference force of the i-th robot end effector,
Figure SMS_372
is the position of the i-th robot end effector input to the impedance model,
Figure SMS_373
is the environmental stiffness of the ith robot.

由上式（30）可知，一旦机器人系统达到稳态，为了使稳态时第i个机器人末端执行器的接触力误差

Figure SMS_374
等于0（即
Figure SMS_375
），必须满足以下条件：From the above formula (30), it can be seen that once the robot system reaches a steady state, in order to make the contact force error of the i-th robot end effector in the steady state
Figure SMS_374
Equal to 0 (i.e.
Figure SMS_375
), the following conditions must be met:

Figure SMS_376
（31）
Figure SMS_376
(31)

设

Figure SMS_377
为第i个机器人环境刚度
Figure SMS_378
的估计值，用环境刚度估计值
Figure SMS_379
分别取代公式（29）和（31）中的环境刚度：set up
Figure SMS_377
is the stiffness of the environment of the ith robot
Figure SMS_378
The estimated value of the environmental stiffness is
Figure SMS_379
Replace the ambient stiffness in formulas (29) and (31) respectively:

Figure SMS_380
（32）
Figure SMS_380
(32)

Figure SMS_381
（33）
Figure SMS_381
(33)

定义

Figure SMS_382
为第i个机器人末端执行器的位置误差，
Figure SMS_383
，将上述公式（32）和（33）相减可以得出末端执行器的接触力误差与末端执行器的位置误差的关系： definition
Figure SMS_382
is the position error of the i-th robot end effector,
Figure SMS_383
, subtracting the above formulas (32) and (33) can yield the relationship between the contact force error of the end effector and the position error of the end effector:

Figure SMS_384
（34）
Figure SMS_384
(34)

式中，

Figure SMS_385
为第i个机器人末端执行器的接触力误差，
Figure SMS_386
为第i个机器人末端执行器的位置误差，
Figure SMS_387
为第i个机器人环境刚度的估计值。In the formula,
Figure SMS_385
is the contact force error of the i-th robot end effector,
Figure SMS_386
is the position error of the i-th robot end effector,
Figure SMS_387
is the estimated value of the stiffness of the environment of the ith robot.

将公式（34）代入公式（26）中的阻抗模型，得到力跟踪误差传递动力学模型：Substituting formula (34) into the impedance model in formula (26), the force tracking error transmission dynamic model is obtained:

Figure SMS_388
（35）
Figure SMS_388
(35)

定义环境刚度估计误差：Define the environmental stiffness estimation error:

Figure SMS_389
（36）
Figure SMS_389
(36)

式中，

Figure SMS_390
为第i个机器人环境刚度估计误差。In the formula,
Figure SMS_390
is the estimation error of the stiffness of the environment of the i-th robot.

基于公式（35）中的力跟踪误差传递动力学模型，考虑第i个机器人末端执行器的接触力误差

Figure SMS_391
与环境刚度估计误差
Figure SMS_392
，设置第二李雅普诺夫函数：Based on the force tracking error transmission dynamics model in formula (35), the contact force error of the i-th robot end effector is considered
Figure SMS_391
Estimation error of stiffness with environment
Figure SMS_392
, set the second Lyapunov function:

Figure SMS_393
（37）
Figure SMS_393
(37)

式中，

Figure SMS_394
为第二李雅普诺夫函数，
Figure SMS_395
为数学符号，表示求矩阵里面的对角线上元素的和。In the formula,
Figure SMS_394
is the second Lyapunov function,
Figure SMS_395
It is a mathematical symbol, which means to find the sum of the elements on the diagonal of the matrix.

求解第二李雅普诺夫函数的一阶导数：Solve for the first derivative of the second Lyapunov function:

Figure SMS_396
（38）
Figure SMS_396
(38)

设置第三李雅普诺夫函数：Set up the third Lyapunov function:

Figure SMS_397
（39）
Figure SMS_397
(39)

式中，

Figure SMS_398
为第三李雅普诺夫函数。In the formula,
Figure SMS_398
is the third Lyapunov function.

求解第三李雅普诺夫函数的一阶导数：Solve for the first derivative of the third Lyapunov function:

Figure SMS_399
（40）
Figure SMS_399
(40)

对第二李雅普诺夫函数的一阶导数

Figure SMS_400
和第三李雅普诺夫函数的一阶导数
Figure SMS_401
求和可以得出：The first derivative of the second Lyapunov function
Figure SMS_400
and the first derivative of the third Lyapunov function
Figure SMS_401
The sum gives:

Figure SMS_402
（41）
Figure SMS_402
(41)

通过分析公式（41）可知，为了使末端执行器的接触力误差

Figure SMS_403
收敛，应当使
Figure SMS_404
，因此设计环境刚度估计的一阶导数如下：By analyzing formula (41), it can be seen that in order to make the contact force error of the end effector
Figure SMS_403
Convergence should be
Figure SMS_404
, so the first-order derivative of the design environment stiffness estimate is as follows:

Figure SMS_405
（42）
Figure SMS_405
(42)

将公式（42）代入公式（41），可以得出：Substituting formula (42) into formula (41), we can obtain:

Figure SMS_406
（43）
Figure SMS_406
(43)

综上，随着时间趋于无穷，第i个机器人末端执行器的接触力误差

Figure SMS_407
In summary, as time approaches infinity, the contact force error of the i-th robot end effector
Figure SMS_407

根据公式（42）设计的环境刚度估计的一阶导数和公式（28）中的弹簧模型，可以得出机器人末端执行器的接触力估计：According to the first-order derivative of the estimated environmental stiffness designed by equation (42) and the spring model in equation (28), the contact force estimate of the robot end effector can be obtained:

Figure SMS_408
（42）
Figure SMS_408
(42)

根据公式（26）中的阻抗模型和公式（34）中的末端执行器的接触力误差

Figure SMS_409
，可以计算机器人末端执行器的位置，具体公式为：According to the impedance model in formula (26) and the contact force error of the end effector in formula (34),
Figure SMS_409
, the position of the robot end effector can be calculated. The specific formula is:

Figure SMS_410
（43）
Figure SMS_410
(43)

式中，

Figure SMS_413
为阻抗模型输出的第i个机器人末端执行器的位置，也就是阻抗参考位置输出，
Figure SMS_414
为阻抗模型输出的第i个机器人末端执行器位置的一阶导数，
Figure SMS_417
为阻抗模型输入的第i个机器人末端执行器位置，
Figure SMS_411
，
Figure SMS_416
、
Figure SMS_419
分别为第i个阻抗模型的惯性、阻尼和刚度，
Figure SMS_421
和
Figure SMS_412
分别为阻抗模型输入的第i个机器人末端执行器位置的一阶导数和二阶导数，
Figure SMS_415
为第i个机器人末端执行器的位置误差，
Figure SMS_418
为第i个机器人环境刚度
Figure SMS_420
的估计。In the formula,
Figure SMS_413
is the position of the i-th robot end effector output by the impedance model, that is, the impedance reference position output,
Figure SMS_414
is the first-order derivative of the position of the i-th robot end effector output by the impedance model,
Figure SMS_417
is the position of the i-th robot end effector input to the impedance model,
Figure SMS_411
,
Figure SMS_416
,
Figure SMS_419
are the inertia, damping and stiffness of the ith impedance model,
Figure SMS_421
and
Figure SMS_412
are the first-order derivative and second-order derivative of the position of the i-th robot end effector input to the impedance model, respectively.
Figure SMS_415
is the position error of the i-th robot end effector,
Figure SMS_418
is the stiffness of the environment of the ith robot
Figure SMS_420
Estimates.

最后，根据协同搬运动力学模型、误差传递动力学模型和规定性能控制器搭建数学仿真模型，将计算得出的机器人系统的输入力矩、机器人末端执行器的接触力估计以及位置输入至仿真模型中，验证机器人系统的协同搬运控制方法的有效性。主要步骤为：Finally, a mathematical simulation model is built based on the collaborative handling dynamics model, error transmission dynamics model and specified performance controller, and the calculated input torque of the robot system, the contact force estimation of the robot end effector and the position are input into the simulation model to verify the effectiveness of the collaborative handling control method of the robot system. The main steps are:

在机器人自由运动过程中，由机器人的参考位置与实际位置计算机器人位置误差

Figure SMS_422
，通过误差转换函数对机器人位置误差进行转换，得到机器人系统转换后位置误差，根据机器人系统转换后位置误差和协同搬运动力学模型得到误差传递动力学模型，设计规定性能控制器并计算得出机器人系统运动需要的输入力矩
Figure SMS_423
，之后将输入力矩
Figure SMS_424
输入到协同搬运动力学模型中，使多个机器人进行协同搬运，在协同搬运过程，由机器人末端执行器的位置计算公式计算出机器人末端执行器的位置，将其输入到机器人系统计算位置误差
Figure SMS_425
，从而实现机器人位置控制。During the free motion of the robot, the robot position error is calculated from the reference position and actual position of the robot.
Figure SMS_422
, the robot position error is converted through the error conversion function to obtain the position error of the robot system after conversion. The error transmission dynamics model is obtained based on the position error of the robot system after conversion and the collaborative handling dynamics model. The specified performance controller is designed and the input torque required for the robot system movement is calculated.
Figure SMS_423
, then input the torque
Figure SMS_424
Input it into the collaborative handling dynamics model to enable multiple robots to perform collaborative handling. During the collaborative handling process, the position of the robot end effector is calculated by the position calculation formula of the robot end effector, and it is input into the robot system to calculate the position error.
Figure SMS_425
, thereby realizing the robot position control.

具体地，仿真曲线验证了位置跟踪性能与力估计性能。参见图5至图11，图5、图6和图7分别为本发明一实施例中三台机器人协同搬运过程中机器人1、机器人2和机器人3的x轴分量控制误差；图8是本发明一实施例中三台机器人协同搬运过程的效果图；图9、图10和图11分别为本发明一实施例中三台机器人协同搬运过程机器人1、机器人2和机器人3的力跟踪效果图。Specifically, the simulation curve verifies the position tracking performance and force estimation performance. Referring to Figures 5 to 11, Figures 5, 6 and 7 are respectively the x-axis component control errors of robot 1, robot 2 and robot 3 in the collaborative handling process of three robots in one embodiment of the present invention; Figure 8 is a rendering of the collaborative handling process of three robots in one embodiment of the present invention; Figures 9, 10 and 11 are respectively the force tracking renderings of robot 1, robot 2 and robot 3 in the collaborative handling process of three robots in one embodiment of the present invention.

由图5至图7可以看出，机器人移动平台在搬运过程中被严格限制在误差安全范围内，保证了搬运过程的精度和安全性；图8为三台机器人协同搬运过程效果图，由图8可以看出，机器人可以在较小的误差下完成轨迹跟踪；在图9至图11中，机械臂可以完成对期望力的跟踪控制，保证了在搬运中的安全性。It can be seen from Figures 5 to 7 that the robot mobile platform is strictly limited within the error safety range during the handling process, ensuring the accuracy and safety of the handling process; Figure 8 is a rendering of the collaborative handling process of three robots. It can be seen from Figure 8 that the robot can complete trajectory tracking with a smaller error; in Figures 9 to 11, the robotic arm can complete the tracking control of the desired force, ensuring safety during handling.

采用上述一种多机器人协同搬运力位混合控制方法，具有如下优点：The above-mentioned multi-robot collaborative handling force-position hybrid control method has the following advantages:

1.通过设计规定性能控制器，实现多个机器人相互之间严格的误差控制，保证了搬运过程中的精度；1. By designing a specified performance controller, strict error control between multiple robots is achieved to ensure the accuracy of the handling process;

2.采用自适应阻抗力控制方法设计机器人末端执行器的接触力估计以及阻抗控制，保证了机器人在搬运过程中的安全性能。2. The adaptive impedance force control method is used to design the contact force estimation and impedance control of the robot's end effector, ensuring the safety performance of the robot during the handling process.

以上对本发明所提供的一种多机器人协同搬运力位混合控制方法进行了详细介绍。本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的核心思想。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以对本发明进行若干改进和修饰，这些改进和修饰也落入本发明权利要求的保护范围内。The above is a detailed introduction to a multi-robot collaborative handling force-position hybrid control method provided by the present invention. This article uses specific examples to illustrate the principles and implementation methods of the present invention. The description of the above embodiments is only used to help understand the core idea of the present invention. It should be pointed out that for ordinary technicians in this technical field, without departing from the principles of the present invention, several improvements and modifications can be made to the present invention, and these improvements and modifications also fall within the scope of protection of the claims of the present invention.

Claims (8)

1. A multi-robot cooperative transport capacity hybrid control method, the method comprising:
s1, establishing a carrying dynamics model of a robot, and establishing a cooperative carrying dynamics model of a robot system formed by a plurality of robots according to the carrying dynamics model of the robot;
s2, setting a robot position error, and converting the robot position error by introducing an error conversion function to obtain a robot converted position error, and obtaining an error transfer dynamics model according to the robot converted position error and the collaborative handling dynamics model;
s3, rewriting the error transfer dynamics model to obtain a rewritten error transfer dynamics model, setting a sliding mode function and a disturbance estimation error, designing a specified performance controller according to the rewritten error transfer dynamics model, the sliding mode function and the disturbance estimation error, and calculating the input torque of the robot system according to the specified performance controller;
s4, presetting an impedance model, a spring model and environmental stiffness estimation, designing an impedance control method according to the impedance model, the spring model and the environmental stiffness estimation, and calculating the contact force estimation and the position of the end effector of the robot system;
s5, constructing a mathematical simulation model according to the cooperative transportation dynamics model, the error transfer dynamics model and the specified performance controller, inputting the calculated input moment of the robot system, the contact force estimation and the position of the end effector of the robot system into the simulation model, and verifying the effectiveness of the cooperative transportation control method of the robot system;
the step S3 specifically comprises the following steps:
s31, rewriting the error transfer dynamics model to obtain a rewritten error transfer dynamics model, wherein the rewritten error transfer dynamics model comprises a specified performance controller;
s32, setting a sliding mode function according to the position error after the conversion of the robot system in the step S23;
s33, setting a disturbance estimation error, and setting a first Lyapunov function according to the sliding mode function and the disturbance estimation error;
s34, judging the stability of the error transfer dynamics model according to the first Lyapunov function, and designing a corresponding specified performance controller when the error transfer dynamics model is stable;
the specific performance controller in S34 may be specifically expressed as:
Figure QLYQS_2
in (1) the->
Figure QLYQS_7
For the input torque of the robot system, +.>
Figure QLYQS_11
For symmetrical positive determination of the inertial matrix of the robotic system, < >>
Figure QLYQS_4
For the sliding mode function, +.>
Figure QLYQS_10
To prescribe performance controller gain, +.>
Figure QLYQS_13
For disturbance estimation, i.e. the actual disturbance +.>
Figure QLYQS_15
Estimated value of ∈10->
Figure QLYQS_1
First derivative of position error after conversion for robotic system,/->
Figure QLYQS_3
For the diagonal gain matrix>
Figure QLYQS_6
Position error for robot system +.>
Figure QLYQS_8
First derivative of>
Figure QLYQS_5
Position error after conversion for robot system +.>
Figure QLYQS_9
First derivative of>
Figure QLYQS_12
、
Figure QLYQS_14
Is an intermediate variable of the error transfer dynamics model.
2. The multi-robot cooperative transportation capacity bit mixture control method according to claim 1, wherein the cooperative transportation dynamics model in S1 is specifically:
Figure QLYQS_27
wherein (1)>
Figure QLYQS_17
Figure QLYQS_23
Figure QLYQS_19
Figure QLYQS_22
Figure QLYQS_26
In (1) the->
Figure QLYQS_32
For symmetrical positive determination of the inertial matrix of the robotic system, < >>
Figure QLYQS_25
For the centrifugal term and the kroot term matrix of the robot system, < >>
Figure QLYQS_28
For the total friction force generated by the robotic system during modeling>
Figure QLYQS_16
For the velocity jacobian from the joint vector of the robot system to the working space +.>
Figure QLYQS_21
Gravitational acceleration matrix>
Figure QLYQS_30
Is a joint vector of the robotic system, +.>
Figure QLYQS_35
And->
Figure QLYQS_31
Joint vectors of the robot system, respectively +.>
Figure QLYQS_34
First and second derivatives of +.>
Figure QLYQS_24
For the degree of freedom of the robotic system, +.>
Figure QLYQS_29
，
Figure QLYQS_33
Is->
Figure QLYQS_36
Degree of freedom of the personal robot, < >>
Figure QLYQS_18
For the actual contact force of the end effector of the robotic system,/->
Figure QLYQS_20
Is the input torque of the robot system.
3. The multi-robot cooperative transportation capacity bit mixture control method according to claim 2, wherein S2 specifically comprises:
s21, setting specified performance and performance function of the robot, and determining position error of the robot according to the specified performance and performance function;
s22, setting an error conversion function, and converting the position error of the robot by using the error conversion function to obtain the position error after the robot is converted;
s23, building a robot system post-conversion position error according to the robot post-conversion position error, processing the robot system post-conversion position error, and combining the collaborative handling dynamics model to obtain an error transfer dynamics model.
4. The multi-robot cooperative transportation capacity bit mixture control method of claim 3, wherein the error transfer dynamics model in S23 has a specific formula:
Figure QLYQS_55
wherein (1)>
Figure QLYQS_59
Figure QLYQS_62
Figure QLYQS_38
Figure QLYQS_42
Figure QLYQS_47
Figure QLYQS_50
Figure QLYQS_43
In (1) the->
Figure QLYQS_46
Position error after conversion for robot system +.>
Figure QLYQS_51
Second derivative of>
Figure QLYQS_54
For symmetrical positive determination of the inertial matrix of the robotic system, < >>
Figure QLYQS_57
For the centrifugal term and the kroot term matrix of the robot system, < >>
Figure QLYQS_60
For the velocity jacobian from the joint vector of the robot system to the working space +.>
Figure QLYQS_63
Gravitational acceleration matrix>
Figure QLYQS_64
And->
Figure QLYQS_53
Upper and lower bounds representing specified performance of the ith robotic system,/->
Figure QLYQS_65
Representing the performance function of the i-th robot, < ->
Figure QLYQS_67
For the input torque of the robot system, +.>
Figure QLYQS_68
Position error for robot system +.>
Figure QLYQS_37
First derivative of>
Figure QLYQS_41
For the position error of the ith robot, < +.>
Figure QLYQS_45
Joint vector for robot system +.>
Figure QLYQS_49
First derivative of>
Figure QLYQS_40
For the joint vector of the ith robot, < +.>
Figure QLYQS_44
For the desired position of the robot system +.>
Figure QLYQS_48
Second derivative of>
Figure QLYQS_52
For the desired position of the ith robot, < +.>
Figure QLYQS_56
For the actual contact force of the end effector of the robotic system,/->
Figure QLYQS_58
、
Figure QLYQS_61
、
Figure QLYQS_66
、
Figure QLYQS_39
Is an intermediate variable of the error transfer dynamics model.
5. The multi-robot cooperative transportation capacity bit mixture control method according to claim 1, wherein S4 specifically comprises:
s41, presetting an impedance model and a spring model, and deducing a contact force error of the end effector according to the impedance model and the spring model;
s42, obtaining a force tracking error transfer dynamics model according to the contact force error of the end effector and the impedance model;
s43, designing an environmental stiffness estimation, designing second and third Lyapunov functions according to the force tracking error transfer dynamics model and the environmental stiffness estimation, and deriving a first derivative of the environmental stiffness estimation through the second and third Lyapunov functions;
s44, obtaining a contact force estimation of the end effector of the robot system according to the first derivative of the environmental stiffness estimation and the spring model;
s45, obtaining the position of the end effector of the robot system according to the contact force error of the end effector and the impedance model.
6. The multi-robot cooperative transportation capacity bit mixture control method according to claim 5, wherein the force tracking error transfer dynamics model in S42 is specifically:
Figure QLYQS_69
in (1) the->
Figure QLYQS_74
、
Figure QLYQS_76
、
Figure QLYQS_70
Inertia, damping and stiffness of the impedance model of the i-th robot, respectively,/i>
Figure QLYQS_73
For the i-th robot environmental stiffness +.>
Figure QLYQS_75
Estimated value of ∈10->
Figure QLYQS_78
Error of contact force for the ith robot end effector, +.>
Figure QLYQS_71
And->
Figure QLYQS_72
Contact force errors of the i-th robotic end effector, respectively +.>
Figure QLYQS_77
First and second derivatives of +.>
Figure QLYQS_79
。
7. The method for controlling the hybrid of the cooperative conveyance capacities of multiple robots as claimed in claim 6, wherein the contact force estimation of the end effector of the robot system in S44 is as follows:
Figure QLYQS_80
in (1) the->
Figure QLYQS_81
Estimating for the contact force of the ith robot end effector,/for the contact force of the ith robot end effector>
Figure QLYQS_82
For the i-th robot environmental stiffness +.>
Figure QLYQS_83
Estimate of->
Figure QLYQS_84
For the position of the i-th robotic end effector,/->
Figure QLYQS_85
Is the position of the target object.
8. The method for controlling the hybrid of the cooperative transportation capacity of multiple robots according to claim 7, wherein the position of the end effector of the robot system in S45 is as follows:
Figure QLYQS_89
in (1) the->
Figure QLYQS_93
Position of i-th robot end effector output for impedance model, +.>
Figure QLYQS_95
First derivative of the i-th robot end effector position output for the impedance model, +.>
Figure QLYQS_90
The i-th robot end effector position input for impedance model, < >>
Figure QLYQS_94
，
Figure QLYQS_96
、
Figure QLYQS_97
Inertia, damping and stiffness of the ith impedance model, respectively,/->
Figure QLYQS_86
And->
Figure QLYQS_87
First and second derivatives of the ith robot end effector position, respectively, input for the impedance model,/->
Figure QLYQS_88
For the position error of the ith robot end effector,/->
Figure QLYQS_92
For the i-th robot environmental stiffness +.>
Figure QLYQS_91
Is a function of the estimate of (2).
CN202310321732.XA 2023-03-29 2023-03-29 Multi-robot cooperative transportation capacity hybrid control method Active CN116069044B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310321732.XA CN116069044B (en) 2023-03-29 2023-03-29 Multi-robot cooperative transportation capacity hybrid control method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310321732.XA CN116069044B (en) 2023-03-29 2023-03-29 Multi-robot cooperative transportation capacity hybrid control method

Publications (2)

Publication Number Publication Date
CN116069044A CN116069044A (en) 2023-05-05
CN116069044B true CN116069044B (en) 2023-06-16

Family

ID=86182257

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310321732.XA Active CN116069044B (en) 2023-03-29 2023-03-29 Multi-robot cooperative transportation capacity hybrid control method

Country Status (1)

Country Link
CN (1) CN116069044B (en)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117601117B (en) * 2023-11-02 2024-05-24 同济大学 A multi-robot coordinated handling control method based on cooperative impedance
CN117921666B (en) * 2024-01-31 2024-07-23 广东工业大学 A variable impedance safe interaction method for a robotic arm

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110434858B (en) * 2019-09-11 2020-11-17 青岛大学 Force/position hybrid control method of multi-mechanical-arm system based on command filtering
CN113427483A (en) * 2021-05-19 2021-09-24 广州中国科学院先进技术研究所 Double-machine manpower/bit multivariate data driving method based on reinforcement learning
CN114115248B (en) * 2021-11-12 2024-10-25 湖南大学 Multi-robot cooperative transportation method and system in warehouse environment
CN115533915B (en) * 2022-10-21 2024-12-06 湖南大学 An active contact detection and control method for aerial working robots in uncertain environments
CN115625711B (en) * 2022-11-08 2024-04-30 沈阳工业大学 A collaborative control method for dual-arm robots considering end force

Also Published As

Publication number Publication date
CN116069044A (en) 2023-05-05

Similar Documents

Publication Publication Date Title
CN116069044B (en) Multi-robot cooperative transportation capacity hybrid control method
CN110275436B (en) RBF neural network self-adaptive control method of multi-single-arm manipulator
CN110154028A (en) A Model-Free Adaptive Integral Terminal Sliding Mode Control Method for Manipulators
CN115157238B (en) A dynamic modeling and trajectory tracking method for multi-degree-of-freedom robots
CN115625711B (en) A collaborative control method for dual-arm robots considering end force
CN113103237B (en) A reconfigurable manipulator control method and system for unknown environmental constraints
CN114571444A (en) Q-learning-based impedance control method for slagging-off robot
CN115890735B (en) Robotic arm system, robotic arm and control method thereof, controller and storage medium
CN113814983A (en) Multi-single-arm manipulator system control method and system
CN114942593A (en) An adaptive sliding mode control method for manipulator based on disturbance observer compensation
CN112809666A (en) 5-DOF mechanical arm force and position tracking algorithm based on neural network
CN113954077B (en) Underwater swimming mechanical arm trajectory tracking control method and device with energy optimization function
Raouf et al. Workspace trajectory tracking control for two-flexible-link manipulator through output redefinition
CN111427264B (en) A Neural Adaptive Fixed Time Control Method for Complex Telemanipulation Technology
CN111590561B (en) Robustness preset performance control method for distributed mechanical arm system
CN117733860A (en) Sliding mode control method of tail end track of double-arm robot based on position dynamics
CN116000919B (en) A full-state constraint control method for a single-link manipulator system with dead zone
CN116945229A (en) Robot high-precision shaft hole assembly method, system, terminal and storage medium
CN118544363B (en) A collaborative handling control method for multiple mobile robots based on adaptive impedance
CN110480641B (en) A Recursive Distributed Fast Convergence Robust Control Method for Manipulators
CN113927591A (en) A finite-time adaptive robot force-position hybrid control method
CN119089665B (en) Multi-branch spacecraft design evaluation system and method for on-orbit servicing
CN115582838B (en) H∞ Consistency Control Method for Predefined Time of Multi-manipulator Based on Preset Performance
Sun et al. Model-Free Non-Singular Fast Terminal Sliding Mode Controller Using Extended Sliding Mode Observer for Series Elastic Actuator-Based Robotic Arm
Wang et al. Dynamic Modelling and Continuous Trajectory Tracking Control of Space Robots Based on Lie Group SE (3)

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant