CN106945043B - A multi-arm collaborative control system for a master-slave teleoperated surgical robot - Google Patents

A multi-arm collaborative control system for a master-slave teleoperated surgical robot Download PDF

Info

Publication number
CN106945043B
CN106945043B CN201710254171.0A CN201710254171A CN106945043B CN 106945043 B CN106945043 B CN 106945043B CN 201710254171 A CN201710254171 A CN 201710254171A CN 106945043 B CN106945043 B CN 106945043B
Authority
CN
China
Prior art keywords
joint
arm
control system
industrial
term
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.)
Expired - Fee Related
Application number
CN201710254171.0A
Other languages
Chinese (zh)
Other versions
CN106945043A (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.)
Chongqing Institute of Green and Intelligent Technology of CAS
Original Assignee
Chongqing Institute of Green and Intelligent Technology of CAS
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 Chongqing Institute of Green and Intelligent Technology of CAS filed Critical Chongqing Institute of Green and Intelligent Technology of CAS
Priority to CN201710254171.0A priority Critical patent/CN106945043B/en
Publication of CN106945043A publication Critical patent/CN106945043A/en
Application granted granted Critical
Publication of CN106945043B publication Critical patent/CN106945043B/en
Expired - Fee Related legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1679—Programme controls characterised by the tasks executed
    • B25J9/1682—Dual arm manipulator; Coordination of several manipulators
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Manipulator (AREA)

Abstract

发明公开了一种主从式遥操作手术机器人多臂协同控制系统，包括工业PC系统和底层控制系统，所述工业PC系统包括工业PC、以太网、CAN总线适配卡；所述底层控制系统包括数字量与模拟量采集卡、伺服驱动器；工业PC利用以太网接受来自其他系统的信号，利用CAN总线适配卡连接伺服驱动器控制机械臂运动，利用数字量/模拟量采集卡采集和处理来自多维力传感器的信号。本发明完成了硬件架构的搭建和机械臂运动学算法和多臂协同控制软件开发，实现了手术机器人的多臂协同控制功能。

Figure 201710254171

The invention discloses a multi-arm collaborative control system of a master-slave teleoperated surgical robot, comprising an industrial PC system and a bottom control system, wherein the industrial PC system includes an industrial PC, an Ethernet, and a CAN bus adapter card; the bottom control system Including digital and analog acquisition cards, servo drives; industrial PCs use Ethernet to receive signals from other systems, use CAN bus adapter cards to connect servo drives to control the movement of the robotic arm, use digital/analog acquisition cards to collect and process data from Signals from multidimensional force sensors. The invention completes the construction of the hardware structure and the development of the kinematic algorithm of the mechanical arm and the multi-arm cooperative control software, and realizes the multi-arm cooperative control function of the surgical robot.

Figure 201710254171

Description

一种主从式遥操作手术机器人多臂协同控制系统A multi-arm collaborative control system for a master-slave teleoperated surgical robot

技术领域technical field

本发明属于手术机器人领域，具体涉及一种主从式遥操作手术机器人多臂协同控制系统，属于人机交互领域。The invention belongs to the field of surgical robots, in particular to a multi-arm collaborative control system of a master-slave teleoperated surgical robot, which belongs to the field of human-computer interaction.

背景技术Background technique

主从式遥操作手术机器人控制系统通常由一个主手操作端和若干个从手执行臂组成。从手执行臂安装于手术台旁边，在其末端可以安装内窥镜以及各种手术器械，通过微小创口到达病人体内病灶处。医生操作主手操作，即可控制从手末端器械完成各种手术操作，为外科医生提供了传统手术的操作环境，可以协助医生完成更精细的手术动作，减少手术时由于疲劳产生的误操作或手部震颤造成的损伤。多臂机器人是髙阶强耦合的复杂非线性系统，同时由于工作环境的多变性，对机器人系统控制方法的鲁棒性、控制系统的实时性提出了很髙要求。基于对单臂机器人的运动学优化求解，无法满足机器人末端受环境约束并需要与环境交互的应用需求。在针对非结构化环境设计的多臂协作机器人比传统的单臂机器人具有更加灵活的特点，能实现人与机器人之间以及各机器人之间的互动功能。目前国外已研制出主从式遥操作手术机器人，但尚不具备力反馈功能及多从臂协同控制功能。The control system of the master-slave teleoperated surgical robot usually consists of a master-hand operating end and several slave-hand execution arms. The hand-operated arm is installed next to the operating table, and an endoscope and various surgical instruments can be installed at its end to reach the lesions in the patient's body through tiny wounds. By operating the main hand, the doctor can control the instruments from the end of the hand to complete various surgical operations, providing the surgeon with an operating environment for traditional surgery, assisting the doctor in completing more delicate surgical actions, and reducing misoperation or misoperation caused by fatigue during surgery. Injuries from tremors in the hands. The multi-arm robot is a complex nonlinear system with high-order strong coupling. At the same time, due to the variability of the working environment, high requirements are placed on the robustness of the robot system control method and the real-time performance of the control system. Based on the kinematic optimization solution of the single-arm robot, it cannot meet the application requirements that the robot end is constrained by the environment and needs to interact with the environment. Multi-arm collaborative robots designed for unstructured environments are more flexible than traditional single-arm robots, and can realize interactive functions between humans and robots and between robots. At present, master-slave teleoperated surgical robots have been developed abroad, but they do not have the function of force feedback and cooperative control of multi-slave arms.

发明内容SUMMARY OF THE INVENTION

有鉴于此，本发明提供一种主从式遥操作手术机器人多臂协同控制系统，利用滑模控制系统及多运动控制器并行解算，可以实现各机械臂之间的协同作业功能。In view of this, the present invention provides a multi-arm cooperative control system of a master-slave teleoperated surgical robot, which can realize the cooperative operation function between each manipulator by using a sliding mode control system and a multi-motion controller to solve in parallel.

本发明的目的是通过这样的技术方案实现的，一种主从式遥操作手术机器人多臂协同控制系统，用于对多臂手术机器人进行控制，多臂手术机器人包括主手操作端和从手执行端，主手操作端为7DOF串联式机器人，从手执行端由两个7DOF串联式机械臂和两个6DOF串联式机械臂组成，每个机械臂末端均安装多维力传感器和运动控制器，其特征在于：该控制系统包括工业PC系统和底层控制系统，所述工业PC系统包括工业PC、以太网、CAN总线适配卡；所述底层控制系统包括数字量与模拟量采集卡、伺服驱动器；工业PC利用以太网接受来自其他系统的信号，利用CAN总线适配卡连接伺服驱动器控制机械臂运动，利用数字量/模拟量采集卡采集和处理来自多维力传感器的信号；各机械臂接收主手操作端传递过来的位置控制指令，通过逆运动学解算得到各关节期望位置；通过多维力传感器感知与环境间的接触力及机械臂之间的接触力，经过逆动力学求解得到各关节力和力矩信息，并将接触力反馈至手术机器人主手；通过绝对值位置传感器获取各关节实际位置；由工业PC协调各运动控制器的数据传递及信号传递，完成多机械臂协同控制功能。The purpose of the present invention is achieved through such a technical solution, a master-slave teleoperated surgical robot multi-arm collaborative control system for controlling the multi-arm surgical robot. The multi-arm surgical robot includes a master hand operating end and a slave hand. The execution end, the master hand operation end is a 7DOF tandem robot, and the slave hand execution end consists of two 7DOF tandem manipulator arms and two 6DOF tandem manipulator arms. The end of each manipulator is equipped with a multi-dimensional force sensor and a motion controller. It is characterized in that: the control system includes an industrial PC system and a bottom control system, the industrial PC system includes an industrial PC, an Ethernet, and a CAN bus adapter card; the bottom control system includes a digital and analog acquisition card, a servo driver ;The industrial PC uses Ethernet to receive signals from other systems, uses the CAN bus adapter card to connect the servo driver to control the movement of the manipulator, and uses the digital/analog acquisition card to collect and process the signals from the multi-dimensional force sensor; each manipulator receives the main The position control command transmitted from the hand operation terminal is used to obtain the desired position of each joint through the inverse kinematics solution; the contact force between the sensor and the environment and the contact force between the robotic arms are sensed by the multi-dimensional force sensor, and each joint is obtained through the inverse kinematics solution. Force and torque information, and feedback the contact force to the main hand of the surgical robot; obtain the actual position of each joint through the absolute value position sensor; the industrial PC coordinates the data transmission and signal transmission of each motion controller to complete the multi-arm collaborative control function.

进一步，该手术机器人多臂协同控制系统包括滑模控制器、自适应模糊逻辑控制器和非线性观测器；所述滑模控制器，用于精确获得系统的动力学参数；所述自适应模糊逻辑控制器，用于提高控制精度；所述非线性观测器，用于补偿外部环境对系统稳定新和精度的影响。Further, the multi-arm cooperative control system of the surgical robot includes a sliding mode controller, an adaptive fuzzy logic controller and a nonlinear observer; the sliding mode controller is used to accurately obtain the dynamic parameters of the system; the adaptive fuzzy logic controller The logic controller is used to improve the control accuracy; the nonlinear observer is used to compensate the influence of the external environment on the stability and accuracy of the system.

进一步，所述滑模控制器的控制律为τ＝τeq+τsm，其中τsm为滑模控制项，

Figure BDA0001272912810000021
为机器人的动力学等效模型，M(q)为惯量矩阵，J-1(q)为逆雅可比矩阵，
Figure BDA0001272912810000022
为机器人末端笛卡尔空间加速度，
Figure BDA0001272912810000023
为雅可比矩阵的导数，
Figure BDA00012729128100000220
为关节空间速度，
Figure BDA0001272912810000025
为惯性项和科氏项，G(q)为重力项；选择滑模面
Figure BDA0001272912810000026
其中s为6×1维向量，c为正定对角矩阵，e＝qid-qi，
Figure BDA0001272912810000027
表示e的导数，qid为关节期望位置，qi为关节实际位置；所述滑模项为
Figure BDA0001272912810000028
k1为6×6的正定对角矩阵；M(q)为惯性矩阵；JT(q)为雅可比矩阵的转秩矩阵，s＝[s1,s2,…,si]为模糊逻辑输入量，γ＝[γ1,γ2,…,γi]为模糊逻辑输出量。Further, the control law of the sliding mode controller is τ=τ eq +τ sm , where τ sm is the sliding mode control term,
Figure BDA0001272912810000021
is the dynamic equivalent model of the robot, M(q) is the inertia matrix, J -1 (q) is the inverse Jacobian matrix,
Figure BDA0001272912810000022
is the Cartesian space acceleration of the robot end,
Figure BDA0001272912810000023
is the derivative of the Jacobian matrix,
Figure BDA00012729128100000220
is the joint space velocity,
Figure BDA0001272912810000025
is the inertia term and the Coriolis term, G(q) is the gravity term; select the sliding surface
Figure BDA0001272912810000026
where s is a 6×1-dimensional vector, c is a positive definite diagonal matrix, e=q id -q i ,
Figure BDA0001272912810000027
Represents the derivative of e, q id is the desired position of the joint, and qi is the actual position of the joint; the sliding mode term is
Figure BDA0001272912810000028
k 1 is a 6×6 positive definite diagonal matrix; M(q) is an inertial matrix; J T (q) is a rank-transformed matrix of the Jacobian matrix, and s=[s 1 ,s 2 ,...,s i ] is a fuzzy Logic input, γ=[γ 1 ,γ 2 ,...,γ i ] is the output of fuzzy logic.

进一步，所述非线性观测器的模型为：

Figure BDA0001272912810000029
Further, the model of the nonlinear observer is:
Figure BDA0001272912810000029

Figure BDA00012729128100000210
其中K1与K2是正定矩阵，
Figure BDA00012729128100000211
表示外部转矩扰动，
Figure BDA00012729128100000212
表示外部转矩扰动的估计，
Figure BDA00012729128100000213
Figure BDA00012729128100000214
为关节速度误差向量，
Figure BDA00012729128100000215
为对关节速度误差
Figure BDA00012729128100000216
的估计量，
Figure BDA00012729128100000217
表示关节加速度误差；M-1表示逆惯量矩阵，C表示惯性项和科氏项，
Figure BDA00012729128100000218
表示关节空间速度，G表示重力项，
Figure BDA00012729128100000219
为关节输入加速度，τ∈R6为关节输入力矩。
Figure BDA00012729128100000210
where K 1 and K 2 are positive definite matrices,
Figure BDA00012729128100000211
represents the external torque disturbance,
Figure BDA00012729128100000212
represents the estimate of the external torque disturbance,
Figure BDA00012729128100000213
Figure BDA00012729128100000214
is the joint velocity error vector,
Figure BDA00012729128100000215
for the joint velocity error
Figure BDA00012729128100000216
an estimate of ,
Figure BDA00012729128100000217
represents the joint acceleration error; M -1 represents the inverse inertia matrix, C represents the inertia term and the Coriolis term,
Figure BDA00012729128100000218
represents the joint space velocity, G represents the gravity term,
Figure BDA00012729128100000219
is the joint input acceleration and τ∈R6 is the joint input torque.

由于采用了上述技术方案，本发明具有如下的优点：Owing to adopting the above-mentioned technical scheme, the present invention has the following advantages:

本发明完成了硬件架构的搭建和机械臂运动学算法和多臂协同控制软件开发，实现了手术机器人的多臂协同控制功能。The invention completes the construction of the hardware structure and the development of the kinematic algorithm of the mechanical arm and the multi-arm cooperative control software, and realizes the multi-arm cooperative control function of the surgical robot.

附图说明Description of drawings

为了使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作进一步的详细描述，其中：In order to make the objects, technical solutions and advantages of the present invention clearer, the present invention will be described in further detail below in conjunction with the accompanying drawings, wherein:

图1为本发明主从式手术机器人控制系统示意图；1 is a schematic diagram of a master-slave surgical robot control system of the present invention;

图2为本发明动力学控制策略示意图；Fig. 2 is the schematic diagram of the dynamic control strategy of the present invention;

图3为本发明多臂协同作业控制系统示意图。FIG. 3 is a schematic diagram of the multi-arm cooperative operation control system of the present invention.

具体实施方式Detailed ways

以下将结合附图，对本发明的优选实施例进行详细的描述；应当理解，优选实施例仅为了说明本发明，而不是为了限制本发明的保护范围。The preferred embodiments of the present invention will be described in detail below with reference to the accompanying drawings; it should be understood that the preferred embodiments are only for illustrating the present invention, rather than for limiting the protection scope of the present invention.

附图1为本发明的控制系统硬件架构，主手操作端和从手执行端，主手操作端为7DOF串联式机器人，从手执行端由两个7DOF串联式机械臂和两个6DOF串联式机械臂组成，每个机械臂末端均安装多维力传感器。所述的机械臂的控制系统硬件包括工业PC系统和底层控制系统。所述工业PC系统包括可视化人机界面、工业PC、以太网卡、CAN总线适配卡；所述底层控制系统包括多个运动控制器、数字量与模拟量采集卡、伺服驱动器及电机、多维力传感器、绝对式编码器。工业PC利用以太网接受来自其他系统的信号，利用CAN总线适配卡连接伺服驱动器控制机械臂运动，利用数字量/模拟量采集卡采集和处理来自多维力传感器与其他传感器的信号。Accompanying drawing 1 is the hardware structure of the control system of the present invention, the master-hand operation end and the slave-hand execution end, the master-hand operation end is a 7DOF series robot, and the slave-hand execution end is composed of two 7DOF series robot arms and two 6DOF series robots. It consists of robotic arms, and a multi-dimensional force sensor is installed at the end of each robotic arm. The control system hardware of the robotic arm includes an industrial PC system and a bottom control system. The industrial PC system includes a visual human-machine interface, an industrial PC, an Ethernet card, and a CAN bus adapter card; the underlying control system includes a plurality of motion controllers, digital and analog acquisition cards, servo drives and motors, multi-dimensional force Sensors, absolute encoders. The industrial PC uses Ethernet to receive signals from other systems, uses the CAN bus adapter card to connect the servo driver to control the movement of the robotic arm, and uses the digital/analog acquisition card to collect and process signals from multi-dimensional force sensors and other sensors.

附图3为本发明的多臂协同作业控制系统示意图，各机械臂接收主手操作端传递过来的位置控制指令，通过逆运动学解算得到各关节期望位置；通过多维力传感器感知与环境间的接触力及机械臂之间的接触力，经过逆动力学求解得到各关节力和力矩信息，并将接触力反馈至手术机器人主手；通过绝对值位置传感器获取各关节实际位置；采用滑模控制器对各关节进行控制，实现多臂协同作业控制。3 is a schematic diagram of the multi-arm cooperative operation control system of the present invention. Each mechanical arm receives the position control command transmitted from the main hand operation end, and obtains the desired position of each joint through inverse kinematics calculation; The contact force and the contact force between the robotic arms are obtained through inverse dynamics solution to obtain the force and moment information of each joint, and the contact force is fed back to the main hand of the surgical robot; the actual position of each joint is obtained through the absolute value position sensor; the sliding mode is adopted The controller controls each joint to realize multi-arm cooperative operation control.

主从式遥操作手术机器人多臂协同控制系统包括滑模控制器、自适应模糊逻辑控制器和非线性观测器；所述滑模控制器，用于精确获得系统的动力学参数；所述自适应模糊逻辑控制器，用于提高控制精度；所述非线性观测器，用于补偿外部环境对系统稳定新和精度的影响。The multi-arm cooperative control system of a master-slave teleoperated surgical robot includes a sliding mode controller, an adaptive fuzzy logic controller and a nonlinear observer; the sliding mode controller is used to accurately obtain the dynamic parameters of the system; the automatic The adaptive fuzzy logic controller is used to improve the control accuracy; the nonlinear observer is used to compensate the influence of the external environment on the stability and accuracy of the system.

所述滑模控制器的控制律为τ＝τeq+τsm，其中τsm为滑模控制项，

Figure BDA0001272912810000031
为机器人的动力学等效模型，M(q)为惯量矩阵，J-1(q)为逆雅可比矩阵，
Figure BDA0001272912810000032
为机器人末端笛卡尔空间加速度，
Figure BDA0001272912810000033
为雅可比矩阵的导数，
Figure BDA0001272912810000038
为关节空间速度，
Figure BDA0001272912810000035
为惯性项和科氏项，G(q)为重力项；选择滑模面
Figure BDA0001272912810000036
Figure BDA0001272912810000037
表示e的导数，其中s为6×1维向量，c为正定对角矩阵，e＝qid-qi，qid为关节期望位置，qi为关节实际位置；滑模项τsm可设计为
Figure BDA0001272912810000041
k1,k2为6×6的正定对角矩阵。The control law of the sliding mode controller is τ=τ eq +τ sm , where τ sm is the sliding mode control term,
Figure BDA0001272912810000031
is the dynamic equivalent model of the robot, M(q) is the inertia matrix, J -1 (q) is the inverse Jacobian matrix,
Figure BDA0001272912810000032
is the Cartesian space acceleration of the robot end,
Figure BDA0001272912810000033
is the derivative of the Jacobian matrix,
Figure BDA0001272912810000038
is the joint space velocity,
Figure BDA0001272912810000035
is the inertia term and the Coriolis term, G(q) is the gravity term; select the sliding surface
Figure BDA0001272912810000036
Figure BDA0001272912810000037
Represents the derivative of e, where s is a 6×1-dimensional vector, c is a positive definite diagonal matrix, e=q id -q i , q id is the desired position of the joint, and q i is the actual position of the joint; the sliding mode term τ sm can be designed for
Figure BDA0001272912810000041
k 1 , k 2 are 6×6 positive definite diagonal matrices.

引入模糊逻辑控制策略，模糊逻辑输出量γ只与s有关，γ＝FLC(s)，FLC(s)为模糊语言决策集的函数，s＝[s1,s2,…,si]为模糊逻辑输入量，γ＝[γ1,γ2,…,γi]为模糊逻辑输出量，

Figure BDA0001272912810000042
定义
Figure BDA0001272912810000043
Figure BDA0001272912810000044
表示当前参数，
Figure BDA0001272912810000045
表示初始参数，
Figure BDA0001272912810000046
Figure BDA0001272912810000047
表示
Figure BDA0001272912810000048
的估计值，ψki(si)表示模糊基函数，
Figure BDA0001272912810000049
为不确定性补偿项；于是可以将滑模项τsm修正为
Figure BDA00012729128100000410
k1,k2为6×6的正定对角矩阵；M(q)∈R6×6,为惯性矩阵；JT(q)为雅可比矩阵的转秩矩阵，s＝[s1,s2,…,si]为模糊逻辑输入量。Introducing the fuzzy logic control strategy, the fuzzy logic output γ is only related to s, γ=FLC(s), FLC(s) is the function of the fuzzy language decision set, s=[s 1 , s 2 ,...,s i ] is Fuzzy logic input, γ=[γ 1 ,γ 2 ,...,γ i ] is the fuzzy logic output,
Figure BDA0001272912810000042
definition
Figure BDA0001272912810000043
Figure BDA0001272912810000044
represents the current parameter,
Figure BDA0001272912810000045
represents the initial parameter,
Figure BDA0001272912810000046
Figure BDA0001272912810000047
express
Figure BDA0001272912810000048
The estimated value of , ψ ki (s i ) represents the fuzzy basis function,
Figure BDA0001272912810000049
is the uncertainty compensation term; then the sliding mode term τ sm can be modified as
Figure BDA00012729128100000410
k 1 , k 2 is a 6×6 positive definite diagonal matrix; M(q)∈R 6×6 , is an inertia matrix; J T (q) is a rank-transformed matrix of Jacobian matrix, s=[s 1 ,s 2 ,…,s i ] is the input of fuzzy logic.

所述非线性观测器的模型为：

Figure BDA00012729128100000411
其中K1与K2是正定矩阵，
Figure BDA00012729128100000412
表示外部转矩扰动的估计，
Figure BDA00012729128100000413
表示关节加速度误差，
Figure BDA00012729128100000414
Figure BDA00012729128100000415
为关节速度误差向量，
Figure BDA00012729128100000416
为对关节速度误差
Figure BDA00012729128100000417
的估计量，；
Figure BDA00012729128100000418
为加速度，M-1表示逆惯量矩阵，C表示惯性项和科氏项，
Figure BDA00012729128100000419
表示关节速度，G表示重力项，
Figure BDA00012729128100000420
表示外部转矩扰动，τ∈R6为关节输入力矩。The model of the nonlinear observer is:
Figure BDA00012729128100000411
where K 1 and K 2 are positive definite matrices,
Figure BDA00012729128100000412
represents the estimate of the external torque disturbance,
Figure BDA00012729128100000413
represents the joint acceleration error,
Figure BDA00012729128100000414
Figure BDA00012729128100000415
is the joint velocity error vector,
Figure BDA00012729128100000416
for the joint velocity error
Figure BDA00012729128100000417
an estimate of ,
Figure BDA00012729128100000418
is the acceleration, M -1 represents the inverse inertia matrix, C represents the inertia term and the Coriolis term,
Figure BDA00012729128100000419
represents the joint velocity, G represents the gravity term,
Figure BDA00012729128100000420
represents the external torque disturbance, and τ∈R6 is the joint input torque.

所述非线性观测器的设计方法为，定义位置误差e＝q-qd，q表示关节当前位置，qd表示关节目标位置，求一阶导数和二阶导数为

Figure BDA00012729128100000421
qd,
Figure BDA00012729128100000422
为关节角位置、速度与加速度；定义
Figure BDA00012729128100000423
Figure BDA00012729128100000424
为关节速度误差向量，
Figure BDA00012729128100000425
为对关节速度误差
Figure BDA00012729128100000426
的估计量；定义
Figure BDA00012729128100000427
Figure BDA00012729128100000428
为对外部干扰力矩的估计，
Figure BDA00012729128100000429
为干扰误差的估计；将所述e,
Figure BDA00012729128100000430
的表达式代入动力学方程得到
Figure BDA00012729128100000431
基于迭代算法的非线性观测器设计为
Figure BDA00012729128100000432
其中K1与K2是正定矩阵。The design method of the nonlinear observer is to define the position error e=qq d , q represents the current position of the joint, q d represents the target position of the joint, and the first and second derivatives are calculated as
Figure BDA00012729128100000421
q d ,
Figure BDA00012729128100000422
are joint angular positions, velocities, and accelerations; define
Figure BDA00012729128100000423
Figure BDA00012729128100000424
is the joint velocity error vector,
Figure BDA00012729128100000425
for the joint velocity error
Figure BDA00012729128100000426
estimator; definition
Figure BDA00012729128100000427
Figure BDA00012729128100000428
is an estimate of the external disturbance torque,
Figure BDA00012729128100000429
is the estimation of the interference error; the e,
Figure BDA00012729128100000430
Substitute the expression into the kinetic equation to get
Figure BDA00012729128100000431
The nonlinear observer based on iterative algorithm is designed as
Figure BDA00012729128100000432
where K 1 and K 2 are positive definite matrices.

所述的动力学方程为：The kinetic equation described is:

Figure BDA00012729128100000433
其中
Figure BDA00012729128100000434
为关节角位置、速度加速度，M(q)∈R6×6为惯性矩阵，
Figure BDA00012729128100000435
为科氏力和向心力矩阵，G(q)∈R6为重力矩阵，τ∈R6为关节输入力矩，τd∈R6为机械臂所受外力矩，将
Figure BDA00012729128100000436
代入机械臂的关节动力学方程得到
Figure BDA00012729128100000437
将机械臂末端期望位姿
Figure BDA00012729128100000438
代替
Figure BDA00012729128100000439
得到动力学等效方程为
Figure BDA00012729128100000440
Figure BDA00012729128100000433
in
Figure BDA00012729128100000434
is the joint angular position, velocity acceleration, M(q)∈R 6 × 6 is the inertia matrix,
Figure BDA00012729128100000435
is the Coriolis force and centripetal force matrix, G(q) ∈ R 6 is the gravity matrix, τ ∈ R 6 is the joint input torque, τ d ∈ R 6 is the external torque received by the manipulator.
Figure BDA00012729128100000436
Substitute into the joint dynamics equation of the manipulator to get
Figure BDA00012729128100000437
Set the desired pose of the end of the robotic arm
Figure BDA00012729128100000438
replace
Figure BDA00012729128100000439
The kinetic equivalent equation is obtained as
Figure BDA00012729128100000440

由机械臂的DH参数建立串联机器人的齐次变换矩阵，将机械臂的正运动学方程表示为x(t)＝φ(q)，x(t)∈R6，q(t)∈R6，其中x(t)为笛卡尔空间中机械臂末端位姿，q(t)为关节空间中各关节位置；对正运动学方程求导得到速度方程为

Figure BDA0001272912810000051
其中，
Figure BDA0001272912810000052
为雅可比矩阵；对速度方程求导得到加速度方程
Figure BDA0001272912810000053
于是
Figure BDA0001272912810000054
The homogeneous transformation matrix of the serial robot is established from the DH parameters of the manipulator, and the forward kinematic equation of the manipulator is expressed as x(t)=φ(q), x(t)∈R 6 , q(t)∈R 6 , where x(t) is the pose of the end of the manipulator in Cartesian space, and q(t) is the position of each joint in the joint space; derivation of the forward kinematics equation gives the velocity equation as
Figure BDA0001272912810000051
in,
Figure BDA0001272912810000052
is the Jacobian matrix; the acceleration equation is obtained by deriving the velocity equation
Figure BDA0001272912810000053
then
Figure BDA0001272912810000054

本发明提出了一种主从式遥操作手术机器人多臂协同控制系统，按照所述流程搭建控制系统硬件并编写控制程序实现了多臂协同作业控制功能。The invention proposes a multi-arm cooperative control system of a master-slave teleoperated surgical robot. According to the process, the control system hardware is built and the control program is written to realize the multi-arm cooperative operation control function.

以上所述仅为本发明的优选实施例，并不用于限制本发明，显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。The above descriptions are only preferred embodiments of the present invention and are not intended to limit the present invention. Obviously, those skilled in the art can make various changes and modifications to the present invention without departing from the spirit and scope of the present invention. Thus, provided that these modifications and variations of the present invention fall within the scope of the claims of the present invention and their equivalents, the present invention is also intended to include these modifications and variations.

Claims (1)

1.一种主从式遥操作手术机器人多臂协同控制系统，用于对多臂手术机器人进行控制，多臂手术机器人包括主手操作端和从手执行端，主手操作端为7DOF串联式机器人，从手执行端由两个7DOF串联式机械臂和两个6DOF串联式机械臂组成，每个机械臂末端均安装多维力传感器和运动控制器，其特征在于：该控制系统包括工业PC系统和底层控制系统，所述工业PC系统包括工业PC、以太网、CAN总线适配卡；所述底层控制系统包括数字量与模拟量采集卡、伺服驱动器；工业PC利用以太网接受来自其他系统的信号，利用CAN总线适配卡连接伺服驱动器控制机械臂运动，利用数字量/模拟量采集卡采集和处理来自多维力传感器的信号；各机械臂接收主手操作端传递过来的位置控制指令，通过逆运动学解算得到各关节期望位置；通过多维力传感器感知与环境间的接触力及机械臂之间的接触力，经过逆动力学求解得到各关节力和力矩信息，并将接触力反馈至手术机器人主手；通过绝对值位置传感器获取各关节实际位置；由工业PC协调各运动控制器的数据传递及信号传递，完成多机械臂协同控制功能；该手术机器人多臂协同控制系统包括滑模控制器、自适应模糊逻辑控制器和非线性观测器；所述滑模控制器，用于精确获得系统的动力学参数；所述自适应模糊逻辑控制器，用于提高控制精度；所述非线性观测器，用于补偿外部环境对系统稳定新和精度的影响；1. A master-slave teleoperated surgical robot multi-arm collaborative control system for controlling the multi-arm surgical robot. The multi-arm surgical robot includes a master hand operation end and a slave hand execution end, and the master hand operation end is a 7DOF series type. The robot is composed of two 7DOF serial manipulator arms and two 6DOF serial manipulator arms, and the end of each manipulator is equipped with a multi-dimensional force sensor and a motion controller. It is characterized in that: the control system includes an industrial PC system and bottom control system, the industrial PC system includes industrial PC, Ethernet, CAN bus adapter card; the bottom control system includes digital and analog acquisition cards, servo drives; the industrial PC uses Ethernet to accept data from other systems Signal, use the CAN bus adapter card to connect the servo driver to control the movement of the manipulator, and use the digital/analog acquisition card to collect and process the signals from the multi-dimensional force sensor; each manipulator receives the position control command transmitted from the main hand operation end, through the The desired position of each joint is obtained by inverse kinematics solution; the contact force with the environment and the contact force between the manipulators are sensed by multi-dimensional force sensors, and the force and torque information of each joint are obtained through inverse dynamics solution, and the contact force is fed back to The main hand of the surgical robot; the actual position of each joint is obtained through the absolute value position sensor; the data transmission and signal transmission of each motion controller are coordinated by the industrial PC to complete the multi-arm collaborative control function; the multi-arm collaborative control system of the surgical robot includes a sliding mode controller, adaptive fuzzy logic controller and nonlinear observer; the sliding mode controller is used to accurately obtain the dynamic parameters of the system; the adaptive fuzzy logic controller is used to improve the control accuracy; Linear observer, used to compensate the influence of external environment on system stability and accuracy; 所述滑模控制器的控制律为τ＝τeq+τsm，其中τsm为滑模控制项，
Figure FDA0002282812070000011
为机器人的动力学等效模型，M(q)为惯量矩阵，J-1(q)为逆雅可比矩阵，
Figure FDA0002282812070000012
为机器人末端笛卡尔空间加速度，
Figure FDA0002282812070000013
为雅可比矩阵的导数，
Figure FDA0002282812070000014
为关节空间速度，
Figure FDA0002282812070000015
为惯性项和科氏项，G(q)为重力项；选择滑模面
Figure FDA0002282812070000016
其中s为6×1维向量，c为正定对角矩阵，e＝qid-qi，
Figure FDA0002282812070000017
表示e的导数，qid为关节期望位置，qi为关节实际位置；所述滑模控制项为
Figure FDA0002282812070000018
k1为6×6的正定对角矩阵；M(q)为惯性矩阵；JT(q)为雅可比矩阵的转秩矩阵，s＝[s1,s2,…,si]为模糊逻辑输入量，γ＝[γ1,γ2,…,γi]为模糊逻辑输出量；The control law of the sliding mode controller is τ=τ eq +τ sm , where τ sm is the sliding mode control term,
Figure FDA0002282812070000011
is the dynamic equivalent model of the robot, M(q) is the inertia matrix, J -1 (q) is the inverse Jacobian matrix,
Figure FDA0002282812070000012
is the Cartesian space acceleration of the robot end,
Figure FDA0002282812070000013
is the derivative of the Jacobian matrix,
Figure FDA0002282812070000014
is the joint space velocity,
Figure FDA0002282812070000015
is the inertia term and the Coriolis term, G(q) is the gravity term; select the sliding surface
Figure FDA0002282812070000016
where s is a 6×1-dimensional vector, c is a positive definite diagonal matrix, e=q id -q i ,
Figure FDA0002282812070000017
represents the derivative of e, q id is the desired position of the joint, and qi is the actual position of the joint; the sliding mode control term is
Figure FDA0002282812070000018
k 1 is a 6×6 positive definite diagonal matrix; M(q) is an inertial matrix; J T (q) is a rank-transformed matrix of the Jacobian matrix, and s=[s 1 ,s 2 ,...,s i ] is a fuzzy Logic input, γ=[γ 1 ,γ 2 ,...,γ i ] is the fuzzy logic output; 所述非线性观测器的模型为：
Figure FDA0002282812070000019
其中K1与K2是正定矩阵，
Figure FDA00022828120700000110
表示外部转矩扰动，
Figure FDA00022828120700000111
表示外部转矩扰动的估计，
Figure FDA00022828120700000112
Figure FDA00022828120700000113
为关节速度误差向量，
Figure FDA00022828120700000114
为对关节速度误差
Figure FDA00022828120700000115
的估计量，
Figure FDA00022828120700000116
表示关节加速度误差；M-1表示逆惯量矩阵，C表示惯性项和科氏项，
Figure FDA00022828120700000117
表示关节空间速度，G表示重力项，
Figure FDA00022828120700000118
为关节输入加速度，τ∈R6为关节输入力矩。The model of the nonlinear observer is:
Figure FDA0002282812070000019
where K 1 and K 2 are positive definite matrices,
Figure FDA00022828120700000110
represents the external torque disturbance,
Figure FDA00022828120700000111
represents the estimate of the external torque disturbance,
Figure FDA00022828120700000112
Figure FDA00022828120700000113
is the joint velocity error vector,
Figure FDA00022828120700000114
for the joint velocity error
Figure FDA00022828120700000115
an estimate of ,
Figure FDA00022828120700000116
represents the joint acceleration error; M -1 represents the inverse inertia matrix, C represents the inertia term and the Coriolis term,
Figure FDA00022828120700000117
represents the joint space velocity, G represents the gravity term,
Figure FDA00022828120700000118
is the joint input acceleration and τ∈R6 is the joint input torque.
CN201710254171.0A 2017-04-18 2017-04-18 A multi-arm collaborative control system for a master-slave teleoperated surgical robot Expired - Fee Related CN106945043B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710254171.0A CN106945043B (en) 2017-04-18 2017-04-18 A multi-arm collaborative control system for a master-slave teleoperated surgical robot

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710254171.0A CN106945043B (en) 2017-04-18 2017-04-18 A multi-arm collaborative control system for a master-slave teleoperated surgical robot

Publications (2)

Publication Number Publication Date
CN106945043A CN106945043A (en) 2017-07-14
CN106945043B true CN106945043B (en) 2020-05-08

Family

ID=59476291

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710254171.0A Expired - Fee Related CN106945043B (en) 2017-04-18 2017-04-18 A multi-arm collaborative control system for a master-slave teleoperated surgical robot

Country Status (1)

Country Link
CN (1) CN106945043B (en)

Families Citing this family (17)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107351088B (en) * 2017-08-31 2020-02-14 南京埃斯顿机器人工程有限公司 Robot external motion path control method
CN108214445B (en) * 2018-01-24 2020-12-25 哈尔滨工业大学 ROS-based master-slave heterogeneous teleoperation control system
CN108406766B (en) * 2018-02-11 2020-08-04 浙江工业大学 A Synchronous Control Method of Multi-Manipulator System Based on Composite Integral Sliding Mode
CN108598965A (en) * 2018-04-11 2018-09-28 南京理工大学 A kind of hot line robot isolation switch replacing options based on force feedback master ＆ slave control
CN108646562B (en) * 2018-05-15 2021-05-18 浙江工业大学 Cross-coupling-based finite time parameter identification and position synchronization control method for multi-mechanical-arm system
CN108656111B (en) * 2018-05-15 2020-12-01 浙江工业大学 Finite time parameter identification and position synchronization control method for dual manipulator system
CN109015634B (en) * 2018-07-24 2021-07-06 西北工业大学 Force/position hybrid control method for multi-arm teleoperated robot based on performance function
CN109358506B (en) * 2018-10-26 2021-07-23 南京理工大学 An Adaptive Fuzzy Teleoperation Control Method Based on Disturbance Observer
CN111374778A (en) * 2018-12-29 2020-07-07 深圳市达科为智能医学有限公司 Surgical robot topology loop control system
CN110181510B (en) * 2019-05-21 2021-12-21 南京航空航天大学 Mechanical arm trajectory tracking control method based on time delay estimation and fuzzy logic
CN110794678B (en) * 2019-11-05 2021-07-30 燕山大学 A four-channel teleoperated force feedback control method with limited hysteresis nonlinearity
CN111515951A (en) * 2020-04-29 2020-08-11 江苏集萃华科智能装备科技有限公司 Teleoperation system and teleoperation control method for robot
CN114167725B (en) * 2021-11-30 2025-02-11 荆楚理工学院 Collaborative robot trajectory tracking control method and system
CN114378796B (en) * 2022-03-04 2023-09-05 国网智能科技股份有限公司 Master-slave and autonomous operation integrated manipulator system, robot system and method
CN115570568B (en) * 2022-10-11 2024-01-30 江苏高倍智能装备有限公司 Multi-manipulator cooperative control method and system
CN116079733A (en) * 2023-02-10 2023-05-09 上海电气集团股份有限公司 Master-slave robot manpower feedback control method, system, equipment and medium
CN116214524B (en) * 2023-05-08 2023-10-03 国网浙江省电力有限公司宁波供电公司 UAV grabbing method, device and storage medium for oil sample recovery

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103862459A (en) * 2012-12-11 2014-06-18 天津工业大学 Design method of position and attitude observer for airborne parallel platform
CN105319972A (en) * 2015-11-27 2016-02-10 燕山大学 Remote operating robot fixed time control method based on rapid terminal sliding mode
CN106375421A (en) * 2016-08-30 2017-02-01 上海交通大学 Robot-assisted intelligent maintenance system based on remote control

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103862459A (en) * 2012-12-11 2014-06-18 天津工业大学 Design method of position and attitude observer for airborne parallel platform
CN105319972A (en) * 2015-11-27 2016-02-10 燕山大学 Remote operating robot fixed time control method based on rapid terminal sliding mode
CN106375421A (en) * 2016-08-30 2017-02-01 上海交通大学 Robot-assisted intelligent maintenance system based on remote control

Also Published As

Publication number Publication date
CN106945043A (en) 2017-07-14

Similar Documents

Publication Publication Date Title
CN106945043B (en) A multi-arm collaborative control system for a master-slave teleoperated surgical robot
JP7556920B2 (en) METHOD FOR CONTROLLING A MECHANICAL SYSTEM, CONTROLLER FOR A MECHANICAL SYSTEM, ROBOT MANIPULATOR, AND NON-TRANSITORY COMPUTER-READABLE STORAGE MEDIUM
CN109358506B (en) An Adaptive Fuzzy Teleoperation Control Method Based on Disturbance Observer
CN110035871B (en) System and method for indicating a robot
CN106003034B (en) A kind of robot controller control system and control method
CN104440864B (en) A kind of master-slave mode remote operating industrial robot system and its control method
CN109968361B (en) A variable impedance remote operation control device and method based on real-time force feedback
CN107028663A (en) A kind of new master-slave mode operating robot control method
CN111230873B (en) Teaching learning-based collaborative handling control system and method
US10335946B2 (en) Compositional impedance programming for robots
JP2015529163A (en) Constraints on robot manipulators with redundant degrees of freedom
Huang et al. Cooperative manipulation of deformable objects by single-leader–dual-follower teleoperation
CN106406098B (en) A Human-Computer Interaction Control Method of Robot System in Unknown Environment
Si et al. Adaptive compliant skill learning for contact-rich manipulation with human in the loop
Liang et al. An Augmented Discrete‐Time Approach for Human‐Robot Collaboration
Lv et al. GuLiM: A hybrid motion mapping technique for teleoperation of medical assistive robot in combating the COVID-19 pandemic
Park et al. Integration of an exoskeleton robotic system into a digital twin for industrial manufacturing applications
Tan et al. Toward unified adaptive teleoperation based on damping ZNN for robot manipulators with unknown kinematics
CN111168680B (en) A Neural Dynamics Method Based Soft Robot Control Method
Parvin et al. Human-Machine Interface (HMI) Robotic Arm Controlled by Gyroscopically Acceleration
Almusawi et al. Online teaching of robotic arm by human–robot interaction: end effector force/torque sensing
CN107553485B (en) Method for generating dynamic virtual clamp in human-computer interaction process
CN114179089A (en) Robust region tracking control method for mechanical arm
CN113500597A (en) Multi-end teleoperation sliding mode impedance control method based on force translation mechanism
CN112894827A (en) Mechanical arm motion control method, system and device and readable storage medium

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
CF01 Termination of patent right due to non-payment of annual fee
CF01 Termination of patent right due to non-payment of annual fee

Granted publication date: 20200508