TWI714462B - External force estimation system for delta robot and method thereof - Google Patents

External force estimation system for delta robot and method thereof Download PDF

Info

Publication number
TWI714462B
TWI714462B TW109105755A TW109105755A TWI714462B TW I714462 B TWI714462 B TW I714462B TW 109105755 A TW109105755 A TW 109105755A TW 109105755 A TW109105755 A TW 109105755A TW I714462 B TWI714462 B TW I714462B
Authority
TW
Taiwan
Prior art keywords
parallel manipulator
parallel
external force
manipulator
matrix
Prior art date
Application number
TW109105755A
Other languages
Chinese (zh)
Other versions
TW202132069A (en
Inventor
藍振洋
劉孟昆
郭翰
Original Assignee
國立臺灣科技大學
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 國立臺灣科技大學 filed Critical 國立臺灣科技大學
Priority to TW109105755A priority Critical patent/TWI714462B/en
Application granted granted Critical
Publication of TWI714462B publication Critical patent/TWI714462B/en
Publication of TW202132069A publication Critical patent/TW202132069A/en

Links

Images

Landscapes

  • Manipulator (AREA)

Abstract

An external force estimation system for delta robot and a method thereof are provided. Position, speed, and acceleration of delta robot are analyzed through kinematic by control device and delta robot is analyzed through dynamics by control device to obtain dynamic model of delta robot. An impedance controller is established through mass-damped-spring second-order system with delta robot and external force by control device. External force to delta robot is calculated by the impedance controller with actual and theoretical trajectories of delta robot deviation. Therefore, the efficiency of dynamic model and impedance controller of delta robot to calculate external force to delta robot may be achieved.

Description

並聯式機械手臂外力估測系統及其方法Parallel mechanical arm external force estimation system and method

一種外力估測系統及其方法，尤其是指一種透過建立並聯式機械手臂的動態模型以及阻抗控制器藉以計算反推並聯式機械手臂所承受到的外力的並聯式機械手臂外力估測系統及其方法。 An external force estimation system and method thereof, in particular to a parallel mechanical arm external force estimation system and its method for calculating the external force of the reverse push parallel robot arm by establishing a dynamic model of the parallel robot arm and an impedance controller method.

機械手臂依結構可分為兩類，分別是串聯式機械手臂(serial manipulator)及並聯式機械手臂(parallel manipulator)。串聯式的機械手臂為開放結構，各關節可獨立運作，串聯式機械手臂的工作空間大且自由度高；而並聯式則為封閉結構，其工作空間較小。但並聯式機械手臂具有許多優點，最重要的一是可將致動器固定於基座上，從而大幅減少機械手臂的慣量，且因其封閉式結構，相較串聯式機械手臂擁有更好的剛性，意味著其能完成精確及快速操作。 Robotic arms can be divided into two types according to their structures, namely serial manipulators and parallel manipulators. The series robot arm has an open structure, and each joint can operate independently. The series robot arm has a large working space and a high degree of freedom; while the parallel type has a closed structure and has a small working space. However, the parallel robot arm has many advantages. The most important one is that the actuator can be fixed on the base, thereby greatly reducing the inertia of the robot arm. Because of its closed structure, it has better advantages than the series robot arm. Rigidity means that it can complete precise and fast operations.

無論是串聯型或並聯型的機械手臂，若要完成任務勢必要搭配一套控制法則，而傳統的機械手臂控制大多著重於自由空間中的位置控制，例如：噴漆、焊接、拾起-放置...等，這類工作通常不會與環境產生接觸力。對於一些特定的製程，例如：研磨、拋光、組裝...等，位置控制即不再適用。原 因在於位置控制是透過回授位置來決定控制量(control effort)，在這樣的控制法則下，位置控制器會將機械手臂與環境的接觸力視為外部干擾，進而產生更大的接觸力。而當接觸力過大時，機械手臂的控制系統便有可能變得不穩定，即使機械手臂的控制系統保持穩定，致動器也會因為無法克服接觸力而達到飽和，而另外一種可想而知的結果就是機械手臂因過大的接觸力而損壞。 Whether it is a series or parallel type of robotic arms, it is necessary to match a set of control rules to complete the task. The traditional robotic arm control mostly focuses on position control in free space, such as: painting, welding, pick-up and place. .. etc. This type of work usually does not produce contact with the environment. For some specific manufacturing processes, such as: grinding, polishing, assembly, etc., position control is no longer applicable. original The reason is that position control determines the control effort through feedback position. Under such a control law, the position controller regards the contact force of the robot arm with the environment as an external disturbance, and thus generates a greater contact force. When the contact force is too large, the control system of the robotic arm may become unstable. Even if the control system of the robotic arm remains stable, the actuator will be saturated because it cannot overcome the contact force. Another one can be imagined. The result is that the mechanical arm is damaged due to excessive contact force.

為了使機械手臂能夠完成上述所提到與環境產生相互作用的製程，讓機械手臂具有順應性(compliance)也逐漸變成必要的要件。順應性指的是機械手臂對於環境接觸力的反應能力，而順應運動(compliant motion)則是指與環境相互作時機械手臂末端點(end-point)運動軌跡因受接觸力作用而產生變動的過程。換句話說，順應運動即為順應性的具體表現，而順應運動控制(compliant motion control)則是一種為使機械手臂在受限的環境中運動的一種控制方法。順應運動又可分為主動式與被動式的順應運動控制，主動式是透過控制法則來達成，而被動式則是在末端點安裝彈性元件來達成。 In order to enable the robotic arm to complete the aforementioned process of interacting with the environment, it is gradually becoming a necessary requirement for the robotic arm to have compliance. Compliance refers to the ability of the robotic arm to respond to the contact force of the environment, while compliant motion refers to the end-point motion trajectory of the robotic arm that changes due to the contact force when interacting with the environment. process. In other words, compliant motion is a concrete manifestation of compliance, and compliant motion control is a control method for manipulators to move in a restricted environment. The compliant motion can be divided into active and passive compliant motion control. The active type is achieved through control laws, and the passive type is achieved by installing elastic elements at the end points.

綜上所述，可知先前技術中長期以來一直存在機械手臂對於外力的偵測需要使用額外的感測器致成本增加以及影響機械手臂工作空間的問題，因此有必要提出改進的技術手段，來解決此一問題。 In summary, it can be seen that in the prior art there has been a long-standing problem that the robot arm needs to use additional sensors to detect external forces, which increases the cost and affects the working space of the robot arm. Therefore, it is necessary to propose improved technical means to solve the problem. This question.

有鑒於先前技術存在機械手臂對於外力的偵測需要使用額外的感測器致成本增加以及影響機械手臂工作空間的問題，本發明遂揭露一種機械手臂對於外力的偵測需要使用額外的感測器致成本增加以及影響機械手臂工作空間系統及其方法，其中： 本發明所揭露的並聯式機械手臂外力估測系統，其包含：並聯式機械手臂以及控制裝置；控制裝置更包含：動態模型建立模組、阻抗控制建立模組以及外力估測模組。 In view of the problems in the prior art that the robot arm needs to use additional sensors for external force detection, which increases the cost and affects the working space of the robot arm, the present invention discloses a robot arm that requires additional sensors for external force detection. Causes cost increase and affects the robotic arm workspace system and its methods, including: The system for estimating the external force of the parallel manipulator disclosed in the present invention includes: a parallel manipulator and a control device; the control device further includes: a dynamic model establishment module, an impedance control establishment module, and an external force estimation module.

並聯式機械手臂包含有三個馬達、與各馬達相連接的第一連桿、與各第一連桿相連接的第二連桿以及與各第二連桿相連接的操作平台；及控制裝置與並聯式機械手臂的馬達連接，控制裝置自馬達接收量測到的力矩值以及轉動位置，控制裝置生成控制訊號以對並聯式機械手臂的馬達進行控制。 The parallel robot arm includes three motors, a first connecting rod connected to each motor, a second connecting rod connected to each first connecting rod, and an operating platform connected to each second connecting rod; and a control device and The motor of the parallel manipulator is connected, the control device receives the measured torque value and rotation position from the motor, and the control device generates a control signal to control the motor of the parallel manipulator.

控制裝置的動態模型建立模組是透過對並聯式機械手臂的位置、速度以及加速度進行運動學分析，再對並聯式機械手臂進行動力學分析，以得到並聯式機械手臂的動態模型，並聯式機械手臂的動態模型由接收到的力矩值以及轉動位置所識別與計算出；控制裝置的阻抗控制建立模組是透過質量阻尼彈簧二階系統建立並聯式機械手臂與外力的阻抗控制器；及控制裝置的外力估測模組是將並聯式機械手臂的實際軌跡以及並聯式機械手臂的理論軌跡的誤差代入阻抗控制器以計算得到並聯式機械手臂所承受到的外力。 The dynamic model establishment module of the control device is through the kinematic analysis of the position, speed and acceleration of the parallel manipulator, and then the dynamic analysis of the parallel manipulator to obtain the dynamic model of the parallel manipulator. The dynamic model of the arm is identified and calculated by the received torque value and rotation position; the impedance control establishment module of the control device is to establish the impedance controller of the parallel robot arm and the external force through the second-order system of mass damping spring; and the control device The external force estimation module substitutes the actual trajectory of the parallel robot arm and the error of the theoretical trajectory of the parallel robot arm into the impedance controller to calculate the external force that the parallel robot arm bears.

本發明所揭露的並聯式機械手臂外力估測方法，其包含下列步驟：首先，並聯式機械手臂包含有三個馬達、與各馬達相連接的第一連桿、與各第一連桿相連接的第二連桿以及與各第二連桿相連接的操作平台；接著，控制裝置與並聯式機械手臂的馬達連接，控制裝置自馬達接收量測到的力矩值以及轉動位置，控制裝置生成控制訊號以對並聯式機械手臂的馬達進行控制；接著，控制裝置透過對並聯式機械手臂的位置、速度以及加速度進行運動學分析，再對並聯式機械手臂進行動力學分析，以得到並聯式機械手臂的動 態模型，並聯式機械手臂的動態模型由接收到的力矩值以及轉動位置所識別與計算出；接著，控制裝置透過質量阻尼彈簧二階系統建立並聯式機械手臂與外力的阻抗控制器；最後，控制裝置將並聯式機械手臂的實際軌跡以及並聯式機械手臂的理論軌跡的誤差代入阻抗控制器以計算得到並聯式機械手臂所承受到的外力。 The method for estimating the external force of a parallel manipulator disclosed in the present invention includes the following steps: First, the parallel manipulator includes three motors, a first link connected to each motor, and a first link connected to each first link. The second link and the operating platform connected to each second link; then, the control device is connected to the motor of the parallel robot arm, the control device receives the measured torque value and rotation position from the motor, and the control device generates a control signal To control the motors of the parallel manipulator; then, the control device analyzes the position, speed and acceleration of the parallel manipulator by kinematics analysis, and then performs dynamic analysis on the parallel manipulator to obtain the parallel manipulator move The dynamic model of the parallel manipulator is identified and calculated by the received torque value and rotation position; then, the control device establishes the impedance controller of the parallel manipulator and the external force through the second-order system of mass damping spring; finally, control The device substitutes the error of the actual trajectory of the parallel robot arm and the theoretical trajectory of the parallel robot arm into the impedance controller to calculate the external force that the parallel robot arm bears.

本發明所揭露的系統及方法如上，與先前技術之間的差異在於控制裝置透過對並聯式機械手臂的位置、速度以及加速度進行運動學分析，再對並聯式機械手臂進行動力學分析，以得到並聯式機械手臂的動態模型，控制裝置透過質量阻尼彈簧二階系統建立並聯式機械手臂與外力的阻抗控制器，控制裝置將並聯式機械手臂的實際軌跡以及並聯式機械手臂的理論軌跡的誤差代入阻抗控制器以計算得到並聯式機械手臂所承受到的外力。 The system and method disclosed in the present invention are as above. The difference between the control device and the prior art is that the control device performs kinematic analysis on the position, speed and acceleration of the parallel manipulator, and then performs dynamic analysis on the parallel manipulator to obtain The dynamic model of the parallel manipulator. The control device establishes the impedance controller of the parallel manipulator and the external force through the second-order system of mass damping spring. The control device substitutes the actual trajectory of the parallel manipulator and the error of the theoretical trajectory of the parallel manipulator into the impedance The controller calculates the external force that the parallel manipulator bears.

透過上述的技術手段，本發明可以達成透過建立並聯式機械手臂的動態模型以及阻抗控制器藉以計算反推並聯式機械手臂所承受到的外力的技術功效。 Through the above-mentioned technical means, the present invention can achieve the technical effect of calculating the external force that the parallel robot arm bears by establishing the dynamic model of the parallel robot arm and the impedance controller.

100:並聯式機械手臂 100: Parallel robot arm

101:馬達 101: Motor

102:第一連桿 102: first link

103:第二連桿 103: second link

104:操作平台 104: operating platform

200:控制裝置 200: control device

201:動態模型建立模組 201: Dynamic model creation module

202:阻抗控制建立模組 202: Impedance control establishment module

203:外力估測模組 203: External force estimation module

301:阻抗控制器 301: Impedance Controller

302:微分器 302: Differentiator

303:低通濾波器 303: Low-pass filter

304:正向運動學 304: Positive Kinematics

305:雅可比矩陣 305: Jacobian Matrix

306:外力感測器 306: External force sensor

步驟101:並聯式機械手臂包含有三個馬達、與各馬達相連接的第一連桿、與各第一連桿相連接的第二連桿以及與各第二連桿相連接的操作平台 Step 101: The parallel robot arm includes three motors, a first link connected to each motor, a second link connected to each first link, and an operating platform connected to each second link

步驟102:控制裝置與並聯式機械手臂的馬達連接，控制裝置自馬達接收量測到的力矩值以及轉動位置，控制裝置生成控制訊號以對並聯式機械手臂的馬達進行控制 Step 102: The control device is connected to the motor of the parallel manipulator. The control device receives the measured torque value and rotation position from the motor. The control device generates a control signal to control the motor of the parallel manipulator.

步驟103:控制裝置透過對並聯式機械手臂的位置、速度以及加速度進行運動學分析，再對並聯式機械手臂進行動力學分析，以得到並聯式機械手臂的動態模型，並聯式機械手臂的動態模型由接收到的力矩值以及轉動位置所識別與計算出 Step 103: The control device performs kinematics analysis on the position, speed and acceleration of the parallel manipulator, and then performs dynamic analysis on the parallel manipulator to obtain the dynamic model of the parallel manipulator, and the dynamic model of the parallel manipulator Recognized and calculated by the received torque value and rotation position

步驟104:控制裝置透過質量阻尼彈簧二階系統建立並聯式機械手臂與外力的阻抗控制器 Step 104: The control device establishes the impedance controller of the parallel robot arm and external force through the second-order system of mass damping spring

步驟105:控制裝置將並聯式機械手臂的實際軌跡以及並聯式機械手臂的理論軌跡的誤差代入阻抗控制器以計算得到並聯式機械手臂所承受到的外力 Step 105: The control device substitutes the actual trajectory of the parallel robot arm and the error of the theoretical trajectory of the parallel robot arm into the impedance controller to calculate the external force that the parallel robot arm bears

第1圖繪示為本發明並聯式機械手臂外力估測系統的並聯式機械手臂立體圖。 Figure 1 is a perspective view of the parallel robot arm of the parallel robot arm external force estimation system of the present invention.

第2圖繪示為本發明並聯式機械手臂外力估測系統的系統方塊圖。 Figure 2 is a system block diagram of the external force estimation system of the parallel robot arm of the present invention.

第3圖繪示為本發明並聯式機械手臂外力估測系統的阻抗控制架構圖。 Figure 3 is a diagram showing the impedance control architecture of the parallel robot arm external force estimation system of the present invention.

第4圖繪示為本發明並聯式機械手臂外力估測方法的方法流程圖。 Figure 4 is a flowchart of the method for estimating the external force of the parallel manipulator according to the present invention.

以下將配合圖式及實施例來詳細說明本發明的實施方式，藉此對本發明如何應用技術手段來解決技術問題並達成技術功效的實現過程能充分理解並據以實施。 The following describes the implementation of the present invention in detail with the drawings and embodiments, so as to fully understand and implement the implementation process of how the present invention uses technical means to solve technical problems and achieve technical effects.

以下首先要說明本發明所揭露的並聯式機械手臂故障偵測系統，並請參考「第1圖」所示，「第1圖」繪示為本發明並聯式機械手臂外力估測系統的並聯式機械手臂立體圖。 The following first describes the fault detection system of the parallel manipulator disclosed in the present invention, and please refer to "Figure 1". "Figure 1" shows the parallel manipulator external force estimation system of the present invention. Three-dimensional view of a robotic arm.

並聯式機械手臂100包含有三個馬達101、與各馬達101相連接的第一連桿102、與各第一連桿102相連接的第二連桿103以及與各第二連桿103相連接的操作平台104，值得注意的是，馬達101是固定於固定架(圖中未繪示)上，並且馬達101固定於固定架上是呈現正三角形的配置。 The parallel robot arm 100 includes three motors 101, a first link 102 connected to each motor 101, a second link 103 connected to each first link 102, and a second link 103 connected to each second link 103. For the operating platform 104, it is worth noting that the motor 101 is fixed on a fixing frame (not shown in the figure), and the motor 101 is fixed on the fixing frame in an equilateral triangle configuration.

請參考「第2圖」所示，「第2圖」繪示為本發明並聯式機械手臂外力估測系統的系統方塊圖。 Please refer to "Figure 2". "Figure 2" is a system block diagram of the parallel robot arm external force estimation system of the present invention.

控制裝置200與並聯式機械手臂100的馬達101連接，控制裝置200自並聯式機械手臂100的馬達101接收量測到的力矩值以及轉動位置，控制裝置200生成控制訊號以對並聯式機械手臂100的馬達101進行控制。 The control device 200 is connected to the motor 101 of the parallel manipulator 100. The control device 200 receives the measured torque value and rotation position from the motor 101 of the parallel manipulator 100. The control device 200 generates a control signal to control the parallel manipulator 100. The motor 101 is controlled.

本發明所揭露的並聯式機械手臂外力估測系統，其包含：並聯式機械手臂100以及控制裝置200；控制裝置200更包含：動態模型建立模組201、阻抗控制建立模組202以及外力估測模組203。 The system for estimating the external force of a parallel manipulator disclosed in the present invention includes: a parallel manipulator 100 and a control device 200; the control device 200 further includes: a dynamic model establishment module 201, an impedance control establishment module 202, and external force estimation Module 203.

控制裝置200的動態模型建立模組201是透過對並聯式機械手臂100的位置、速度以及加速度進行運動學分析，再對並聯式機械手臂100進行動 力學分析，以得到並聯式機械手臂100的動態模型，並聯式機械手臂100的動態模型由接收到的力矩值以及轉動位置所識別與計算出，控制裝置200的動態模型建立模組201得到的並聯式機械手臂100的動態模型如下：

Figure 109105755-A0305-02-0008-2
The dynamic model establishment module 201 of the control device 200 performs kinematics analysis on the position, speed and acceleration of the parallel manipulator 100, and then performs dynamic analysis on the parallel manipulator 100 to obtain the dynamics of the parallel manipulator 100 Model, the dynamic model of the parallel manipulator 100 is identified and calculated from the received torque value and rotation position, and the dynamic model of the parallel manipulator 100 obtained by the dynamic model establishment module 201 of the control device 200 is as follows:
Figure 109105755-A0305-02-0008-2

其中，τ m 為並聯式機械手臂的馬達扭矩；q為並聯式機械手臂各節點的角度向量；

Figure 109105755-A0305-02-0008-28
為並聯式機械手臂各節點的角速度向量；
Figure 109105755-A0305-02-0008-29
為並聯式機械手臂各節點的角加速度向量；D為並聯式機械手臂的慣性矩陣；C為並聯式機械手臂的離心力矩陣；G為並聯式機械手臂的重力項矩陣；F為並聯式機械手臂的馬達摩擦力；J T 為雅可比(Jacobian)矩陣的轉置矩陣；及F ext 為接觸外力。 Among them, τ m is the motor torque of the parallel manipulator; q is the angle vector of each node of the parallel manipulator;
Figure 109105755-A0305-02-0008-28
Is the angular velocity vector of each node of the parallel manipulator;
Figure 109105755-A0305-02-0008-29
Is the angular acceleration vector of each node of the parallel manipulator; D is the inertia matrix of the parallel manipulator; C is the centrifugal force matrix of the parallel manipulator; G is the gravity term matrix of the parallel manipulator; F is the parallel manipulator’s Motor friction; J T is the transposed matrix of the Jacobian matrix; and F ext is the contact external force.

將並聯式機械手臂100的動態模型以反向動力學的形式即可改寫如下：

Figure 109105755-A0305-02-0008-3
The dynamic model of the parallel robot arm 100 can be rewritten as follows in the form of reverse dynamics:
Figure 109105755-A0305-02-0008-3

接著，控制裝置200的阻抗控制建立模組202是透過質量阻尼彈簧二階系統建立並聯式機械手臂100與外力的阻抗控制器，質量阻尼彈簧二階系統如下：

Figure 109105755-A0305-02-0008-4
Next, the impedance control establishment module 202 of the control device 200 establishes the impedance controller of the parallel manipulator 100 and the external force through the second-order mass damping spring system. The second-order mass damping spring system is as follows:
Figure 109105755-A0305-02-0008-4

其中，P為並聯式機械手臂的真實位移；

Figure 109105755-A0305-02-0008-5
為並聯式機械手臂的真實速度；
Figure 109105755-A0305-02-0008-7
為並聯式機械手臂的真實加速度；Pd為並聯式機械手臂的期望位移；
Figure 109105755-A0305-02-0008-6
為並聯式機械手臂的期望速度；
Figure 109105755-A0305-02-0008-1
為並聯式機械手臂的期望加速度；Mm為目標阻抗的質量；Bm為目標阻抗的阻尼；及Km為目標阻抗的彈簧。 Among them, P is the true displacement of the parallel robot arm;
Figure 109105755-A0305-02-0008-5
Is the true speed of the parallel robot arm;
Figure 109105755-A0305-02-0008-7
Is the true acceleration of the parallel manipulator; P d is the expected displacement of the parallel manipulator;
Figure 109105755-A0305-02-0008-6
Is the expected speed of the parallel manipulator;
Figure 109105755-A0305-02-0008-1
Is the expected acceleration of the parallel manipulator; M m is the mass of the target impedance; B m is the damping of the target impedance; and K m is the spring of the target impedance.

將質量阻尼彈簧二階系統以反向動力學的形式即可改寫如下：

Figure 109105755-A0305-02-0008-8
The second-order mass damping spring system can be rewritten as follows in the form of reverse dynamics:
Figure 109105755-A0305-02-0008-8

將以反向動力學改寫後的動態模型以及質量阻尼彈簧二階系統代入

Figure 109105755-A0305-02-0009-9
，即可得到阻抗控制器如下：
Figure 109105755-A0305-02-0009-10
 Substitute the dynamic model rewritten with reverse dynamics and the second-order mass damping spring system
Figure 109105755-A0305-02-0009-9
, You can get the impedance controller as follows:
Figure 109105755-A0305-02-0009-10

其中，τm為並聯式機械手臂的馬達扭矩；W-1為JD-1的逆矩陣；J為雅可比矩陣；

Figure 109105755-A0305-02-0009-23
為雅可比矩陣的微分矩陣；D-1為並聯式機械手臂的慣性矩陣的逆矩陣；
Figure 109105755-A0305-02-0009-24
為並聯式機械手臂各節點的角速度向量；P為並聯式機械手臂的真實位移；
Figure 109105755-A0305-02-0009-25
為並聯式機械手臂的真實速度；
Figure 109105755-A0305-02-0009-20
為並聯式機械手臂的真實加速度；Pd為並聯式機械手臂的期望位移；
Figure 109105755-A0305-02-0009-21
為並聯式機械手臂的期望速度；
Figure 109105755-A0305-02-0009-22
為並聯式機械手臂的期望加速度；Mm -1為目標阻抗的質量的逆矩陣；Bm為目標阻抗的阻尼；Km為目標阻抗的彈簧；C為並聯式機械手臂的離心力矩陣；G為並聯式機械手臂的重力項矩陣；F為並聯式機械手臂的馬達摩擦力；JT為雅可比(Jacobian)矩陣的轉置矩陣；及Fext為接觸外力。 Among them, τ m is the motor torque of the parallel manipulator; W -1 is the inverse matrix of JD -1 ; J is the Jacobian matrix;
Figure 109105755-A0305-02-0009-23
Is the differential matrix of the Jacobian matrix; D -1 is the inverse matrix of the inertia matrix of the parallel manipulator;
Figure 109105755-A0305-02-0009-24
Is the angular velocity vector of each node of the parallel manipulator; P is the true displacement of the parallel manipulator;
Figure 109105755-A0305-02-0009-25
Is the true speed of the parallel robot arm;
Figure 109105755-A0305-02-0009-20
Is the true acceleration of the parallel manipulator; P d is the expected displacement of the parallel manipulator;
Figure 109105755-A0305-02-0009-21
Is the expected speed of the parallel manipulator;
Figure 109105755-A0305-02-0009-22
Is the expected acceleration of the parallel manipulator; M m -1 is the inverse matrix of the mass of the target impedance; B m is the damping of the target impedance; K m is the spring of the target impedance; C is the centrifugal force matrix of the parallel manipulator; G is The gravity term matrix of the parallel manipulator; F is the motor friction of the parallel manipulator; J T is the transposed matrix of the Jacobian matrix; and F ext is the contact external force.

接著，請同時參考「第2圖」以及「第3圖」所示，「第3圖」繪示為本發明並聯式機械手臂外力估測系統的阻抗控制架構圖。 Then, please refer to both "Figure 2" and "Figure 3". "Figure 3" shows the impedance control architecture diagram of the parallel robot arm external force estimation system of the present invention.

阻抗控制架構為阻抗控制器301、並聯式機械手臂100、微分器302、低通濾波器303、正向運動學304、雅可比(Jacobian)矩陣305以及外力感測器306所構成，阻抗控制器301接收並聯式機械手臂的期望位移、並聯式機械手臂的期望速度、並聯式機械手臂的期望加速度以及外力，阻抗控制器301計算並輸出並聯式機械手臂的扭矩並結合外力所造成的等校扭矩提供至並聯式機械手臂100，阻抗控制器301計算並輸出目標阻抗的阻尼以及目標阻抗的彈簧至外力感測器306，並聯式機械手臂100輸出位置至微分器302以及正向運動 學304，微分器302配合低通濾波器303計算並輸出並聯式機械手臂各節點的角速度向量至雅可比矩陣305，正向運動學304計算並輸出並聯式機械手臂的真實位移至外力感測器306，雅可比矩陣305計算並輸出並聯式機械手臂的真實速度至外力感測器306，外力感測器306即可計算並輸出外力至阻抗控制器301。 The impedance control architecture is composed of impedance controller 301, parallel manipulator 100, differentiator 302, low-pass filter 303, forward kinematics 304, Jacobian matrix 305 and external force sensor 306, impedance controller 301 receives the expected displacement of the parallel robot arm, the expected speed of the parallel robot arm, the expected acceleration of the parallel robot arm, and the external force. The impedance controller 301 calculates and outputs the torque of the parallel robot arm and combines the equalized torque caused by the external force. Provided to the parallel robot arm 100, the impedance controller 301 calculates and outputs the damping of the target impedance and the spring of the target impedance to the external force sensor 306, and the parallel robot arm 100 outputs the position to the differentiator 302 and forward movement School 304, the differentiator 302 cooperates with the low-pass filter 303 to calculate and output the angular velocity vector of each node of the parallel manipulator to the Jacobian matrix 305, the forward kinematics 304 calculates and outputs the true displacement of the parallel manipulator to the external force sensor 306. The Jacobian matrix 305 calculates and outputs the real speed of the parallel manipulator to the external force sensor 306, and the external force sensor 306 can calculate and output the external force to the impedance controller 301.

由上述質量阻尼彈簧二階系統可以知道若並聯式機械手臂100受到外力時，將會產生上述質量阻尼彈簧二階系統，因此相反而言，當並聯式機械手臂100出現不屬於動態模型的響應時，即可透過質量阻尼彈簧二階系統的阻抗控制反推出並聯式機械手臂100所承受到的外力大小，即可由此反推得到外力估測器。 From the above-mentioned second-order mass damping spring system, it can be known that if the parallel manipulator 100 receives an external force, the above-mentioned second-order mass damping spring system will be generated. Therefore, on the contrary, when the parallel manipulator 100 has a response that does not belong to the dynamic model, namely The magnitude of the external force received by the parallel manipulator 100 can be deduced through the impedance control of the second-order system of the mass damping spring, and the external force estimator can be obtained by reverse deduction.

當並聯式機械手臂100的實際軌跡與並聯式機械手臂100的理論軌跡出現誤差時，就代表並聯式機械手臂100承受到外力，而外力的大小即為質量阻尼彈簧二階系統的響應，亦即外力的大小即為實際軌跡與理想軌跡間的誤差關係，然而透過質量阻尼彈簧二階系統的響應反推外力需要得知並聯式機械手臂100未端的位置、速度以及加速度的資訊，由於加速度資訊取得需要額外的感測器，估而控制裝置200的外力估測模組203是將並聯式機械手臂100的實際軌跡以及並聯式機械手臂的理論軌跡的誤差代入阻抗控制器並將之簡化為下列公式以計算得到並聯式機械手臂100所承受到的外力：

Figure 109105755-A0305-02-0010-11
When there is an error between the actual trajectory of the parallel manipulator 100 and the theoretical trajectory of the parallel manipulator 100, it means that the parallel manipulator 100 has received an external force, and the magnitude of the external force is the response of the second-order mass damping spring system, that is, the external force The magnitude of is the error relationship between the actual trajectory and the ideal trajectory. However, through the response of the mass damping spring second-order system to reverse the external force, it is necessary to know the position, velocity and acceleration information of the end of the parallel manipulator 100. The acceleration information requires additional The external force estimating module 203 of the control device 200 is to substitute the actual trajectory of the parallel manipulator 100 and the error of the theoretical trajectory of the parallel manipulator into the impedance controller and simplify it into the following formula for calculation Obtain the external force endured by the parallel manipulator 100:
Figure 109105755-A0305-02-0010-11

其中，F ext 為接觸外力；P為並聯式機械手臂的真實位移；

Figure 109105755-A0305-02-0010-27
為並聯式機械手臂的真實速度；Pd為並聯式機械手臂的期望位移；
Figure 109105755-A0305-02-0010-26
為並聯式機械手臂的期望速度；Bm為目標阻抗的阻尼；及Km為目標阻抗的彈簧。 Among them, F ext is the contact external force; P is the true displacement of the parallel manipulator;
Figure 109105755-A0305-02-0010-27
Is the real speed of the parallel manipulator; P d is the expected displacement of the parallel manipulator;
Figure 109105755-A0305-02-0010-26
Is the desired speed of the parallel manipulator; B m is the damping of the target impedance; and K m is the spring of the target impedance.

請同時參考「第4圖」所示，「第4圖」繪示為本發明並聯式機械手臂外力估測方法的方法流程圖。 Please also refer to the "Figure 4" which is a flowchart of the method for estimating the external force of the parallel manipulator of the present invention.

首先，並聯式機械手臂包含有三個馬達、與各馬達相連接的第一連桿、與各第一連桿相連接的第二連桿以及與各第二連桿相連接的操作平台(步驟101)；接著，控制裝置與並聯式機械手臂的馬達連接，控制裝置自馬達接收量測到的力矩值以及轉動位置，控制裝置生成控制訊號以對並聯式機械手臂的馬達進行控制(步驟102)；接著，控制裝置透過對並聯式機械手臂的位置、速度以及加速度進行運動學分析，再對並聯式機械手臂進行動力學分析，以得到並聯式機械手臂的動態模型，並聯式機械手臂的動態模型由接收到的力矩值以及轉動位置所識別與計算出(步驟103)；接著，控制裝置透過質量阻尼彈簧二階系統建立並聯式機械手臂與外力的阻抗控制器(步驟104)；最後，控制裝置將並聯式機械手臂的實際軌跡以及並聯式機械手臂的理論軌跡的誤差代入阻抗控制器以計算得到並聯式機械手臂所承受到的外力(步驟105)。 First, the parallel robot arm includes three motors, a first link connected to each motor, a second link connected to each first link, and an operating platform connected to each second link (step 101 ); Next, the control device is connected to the motor of the parallel manipulator, the control device receives the measured torque value and rotation position from the motor, and the control device generates a control signal to control the motor of the parallel manipulator (step 102); Then, the control device analyzes the position, velocity and acceleration of the parallel robot arm through kinematics analysis, and then performs dynamic analysis on the parallel robot arm to obtain the dynamic model of the parallel robot arm. The dynamic model of the parallel robot arm is determined by The received torque value and the rotational position are identified and calculated (step 103); then, the control device establishes the impedance controller of the parallel robot arm and the external force through the mass damping spring second-order system (step 104); finally, the control device is connected in parallel The error of the actual trajectory of the parallel manipulator and the theoretical trajectory of the parallel manipulator is substituted into the impedance controller to calculate the external force that the parallel manipulator bears (step 105).

綜上所述，可知本發明與先前技術之間的差異在於控制裝置透過對並聯式機械手臂的位置、速度以及加速度進行運動學分析，再對並聯式機械手臂進行動力學分析，以得到並聯式機械手臂的動態模型，控制裝置透過質量阻尼彈簧二階系統建立並聯式機械手臂與外力的阻抗控制器，控制裝置將並聯式機械手臂的實際軌跡以及並聯式機械手臂的理論軌跡的誤差代入阻抗控制器以計算得到並聯式機械手臂所承受到的外力。 In summary, it can be seen that the difference between the present invention and the prior art is that the control device analyzes the position, speed and acceleration of the parallel robot arm through kinematics analysis, and then performs dynamic analysis on the parallel robot arm to obtain the parallel type. The dynamic model of the manipulator, the control device establishes the impedance controller of the parallel manipulator and the external force through the mass damping spring second-order system. The control device substitutes the actual trajectory of the parallel manipulator and the error of the theoretical trajectory of the parallel manipulator into the impedance controller Calculate the external force that the parallel robot arm bears.

藉由此一技術手段可以來解決先前技術所存在機械手臂對於外力的偵測需要使用額外的感測器致成本增加以及影響機械手臂工作空間的問 題，進而達成透過建立並聯式機械手臂的動態模型以及阻抗控制器藉以計算反推並聯式機械手臂所承受到的外力的技術功效。 This technical method can solve the problem that the robot arm in the prior art needs to use additional sensors for external force detection, which causes the increase in cost and affects the working space of the robot arm. To solve the problem, the technical effect of calculating the external force that the parallel robot is subjected to is calculated by establishing the dynamic model of the parallel robot arm and the impedance controller.

雖然本發明所揭露的實施方式如上，惟所述的內容並非用以直接限定本發明的專利保護範圍。任何本發明所屬技術領域中具有通常知識者，在不脫離本發明所揭露的精神和範圍的前提下，可以在實施的形式上及細節上作些許的更動。本發明的專利保護範圍，仍須以所附的申請專利範圍所界定者為準。 Although the embodiments disclosed in the present invention are as above, the content described is not used to directly limit the scope of patent protection of the present invention. Anyone with ordinary knowledge in the technical field to which the present invention belongs can make slight changes in the form and details of the implementation without departing from the spirit and scope of the present invention. The patent protection scope of the present invention shall still be subject to those defined by the attached patent application scope.

100:並聯式機械手臂 100: Parallel robot arm

200:控制裝置 200: control device

201:動態模型建立模組 201: Dynamic model creation module

202:阻抗控制建立模組 202: Impedance control establishment module

203:外力估測模組 203: External force estimation module

Claims (10)

一種並聯式機械手臂外力估測系統，其包含：一並聯式機械手臂，所述並聯式機械手臂包含有三個馬達、與各馬達相連接的第一連桿、與各第一連桿相連接的第二連桿以及與各第二連桿相連接的操作平台；及一控制裝置，所述控制裝置與所述並聯式機械手臂的所述馬達連接，所述控制裝置自所述馬達接收量測到的力矩值以及轉動位置，所述控制裝置生成控制訊號以對所述並聯式機械手臂的所述馬達進行控制，所述控制裝置更包含：一動態模型建立模組，透過對並聯式機械手臂的位置、速度以及加速度進行運動學分析，再對並聯式機械手臂進行動力學分析，以得到所述並聯式機械手臂的動態模型，所述並聯式機械手臂的動態模型由接收到的力矩值以及轉動位置所識別與計算出；一阻抗控制建立模組，透過質量阻尼彈簧二階系統建立所述並聯式機械手臂與外力的一阻抗控制器；及一外力估測模組，將所述並聯式機械手臂的實際軌跡以及所述並聯式機械手臂的理論軌跡的誤差代入所述阻抗控制器以計算得到並聯式機械手臂所承受到的外力。 A system for estimating the external force of a parallel manipulator, comprising: a parallel manipulator, the parallel manipulator including three motors, a first link connected to each motor, and a first link connected to each first link A second link and an operating platform connected to each of the second links; and a control device connected to the motor of the parallel robot arm, the control device receiving measurements from the motor The control device generates a control signal to control the motor of the parallel manipulator. The control device further includes: a dynamic model building module, through which the parallel manipulator Perform kinematics analysis on the position, velocity and acceleration of the parallel manipulator, and then perform dynamic analysis on the parallel manipulator to obtain the dynamic model of the parallel manipulator. The dynamic model of the parallel manipulator consists of the received torque value and The rotational position is identified and calculated; an impedance control establishment module, which establishes an impedance controller of the parallel manipulator and external force through the second-order mass damping spring system; and an external force estimation module, which connects the parallel mechanical The error of the actual trajectory of the arm and the theoretical trajectory of the parallel manipulator is substituted into the impedance controller to calculate the external force that the parallel manipulator bears. 如請求項1所述的並聯式機械手臂外力估測系統，其中所述動態模型為下列公式：
Figure 109105755-A0305-02-0014-12
 其中，τ m 為並聯式機械手臂的馬達扭矩；q為並聯式機械手臂各節點的角度向量；
Figure 109105755-A0305-02-0015-30
為並聯式機械手臂各節點的角速度向量；
Figure 109105755-A0305-02-0015-31
為並聯式機械手臂各節點的角加速度向量；D為並聯式機械手臂的慣性矩陣；C為並聯式機械手臂的離心力矩陣；G為並聯式機械手臂的重力項矩陣；F為並聯式機械手臂的馬達摩擦力；J T 為雅可比(Jacobian)矩陣的轉置矩陣；及F ext 為接觸外力。 The system for estimating the external force of a parallel manipulator according to claim 1, wherein the dynamic model is the following formula:
Figure 109105755-A0305-02-0014-12
 Among them, τ m is the motor torque of the parallel manipulator; q is the angle vector of each node of the parallel manipulator;
Figure 109105755-A0305-02-0015-30
Is the angular velocity vector of each node of the parallel manipulator;
Figure 109105755-A0305-02-0015-31
Is the angular acceleration vector of each node of the parallel manipulator; D is the inertia matrix of the parallel manipulator; C is the centrifugal force matrix of the parallel manipulator; G is the gravity term matrix of the parallel manipulator; F is the parallel manipulator’s Motor friction; J T is the transposed matrix of the Jacobian matrix; and F ext is the contact external force. 如請求項1所述的並聯式機械手臂外力估測系統，其中所述質量阻尼彈簧二階系統為下列公式：
Figure 109105755-A0305-02-0015-13
 其中，P為並聯式機械手臂的真實位移；
Figure 109105755-A0305-02-0015-32
為並聯式機械手臂的真實速度；
Figure 109105755-A0305-02-0015-33
為並聯式機械手臂的真實加速度；Pd為並聯式機械手臂的期望位移；
Figure 109105755-A0305-02-0015-36
為並聯式機械手臂的期望速度；
Figure 109105755-A0305-02-0015-37
為並聯式機械手臂的期望加速度；Mm為目標阻抗的質量； Bm為目標阻抗的阻尼；及Km為目標阻抗的彈簧。 The external force estimation system of the parallel manipulator according to claim 1, wherein the second-order mass damping spring system is the following formula:
Figure 109105755-A0305-02-0015-13
 Among them, P is the true displacement of the parallel robot arm;
Figure 109105755-A0305-02-0015-32
Is the true speed of the parallel robot arm;
Figure 109105755-A0305-02-0015-33
Is the true acceleration of the parallel manipulator; P d is the expected displacement of the parallel manipulator;
Figure 109105755-A0305-02-0015-36
Is the expected speed of the parallel manipulator;
Figure 109105755-A0305-02-0015-37
Is the expected acceleration of the parallel manipulator; M m is the mass of the target impedance; B m is the damping of the target impedance; and K m is the spring of the target impedance. 如請求項1所述的並聯式機械手臂外力估測系統，其中所述阻抗控制器為下列公式：
Figure 109105755-A0305-02-0016-14
 其中，τ m 為並聯式機械手臂的馬達扭矩；W -1為JD -1的逆矩陣；J為雅可比矩陣；j為雅可比矩陣的微分矩陣；D -1為並聯式機械手臂的慣性矩陣的逆矩陣；
Figure 109105755-A0305-02-0016-38
為並聯式機械手臂各節點的角速度向量；P為並聯式機械手臂的真實位移；
Figure 109105755-A0305-02-0016-39
為並聯式機械手臂的真實速度；
Figure 109105755-A0305-02-0016-40
為並聯式機械手臂的真實加速度；Pd為並聯式機械手臂的期望位移；
Figure 109105755-A0305-02-0016-41
為並聯式機械手臂的期望速度；
Figure 109105755-A0305-02-0016-42
為並聯式機械手臂的期望加速度；M m -1為目標阻抗的質量的逆矩陣； Bm為目標阻抗的阻尼；Km為目標阻抗的彈簧；C為並聯式機械手臂的離心力矩陣；G為並聯式機械手臂的重力項矩陣；F為並聯式機械手臂的馬達摩擦力；J T 為雅可比(Jacobian)矩陣的轉置矩陣；及F ext 為接觸外力。 The parallel robot arm external force estimation system according to claim 1, wherein the impedance controller is the following formula:
Figure 109105755-A0305-02-0016-14
 Among them, τ m is the motor torque of the parallel manipulator; W -1 is the inverse matrix of JD -1 ; J is the Jacobian matrix; j is the differential matrix of the Jacobian matrix; D -1 is the inertia matrix of the parallel manipulator Inverse matrix
Figure 109105755-A0305-02-0016-38
Is the angular velocity vector of each node of the parallel manipulator; P is the true displacement of the parallel manipulator;
Figure 109105755-A0305-02-0016-39
Is the true speed of the parallel robot arm;
Figure 109105755-A0305-02-0016-40
Is the true acceleration of the parallel manipulator; P d is the expected displacement of the parallel manipulator;
Figure 109105755-A0305-02-0016-41
Is the expected speed of the parallel manipulator;
Figure 109105755-A0305-02-0016-42
Is the expected acceleration of the parallel manipulator; M m -1 is the inverse matrix of the mass of the target impedance; B m is the damping of the target impedance; K m is the spring of the target impedance; C is the centrifugal force matrix of the parallel manipulator; G is The gravity term matrix of the parallel manipulator; F is the motor friction force of the parallel manipulator; J T is the transposed matrix of the Jacobian matrix; and F ext is the contact external force. 如請求項1所述的並聯式機械手臂外力估測系統，其中所述外力估測模組透過下列公式以計算得到並聯式機械手臂所承受到的外力：
Figure 109105755-A0305-02-0017-15
 其中，F ext 為接觸外力；P為並聯式機械手臂的真實位移；
Figure 109105755-A0305-02-0017-43
為並聯式機械手臂的真實速度；Pd為並聯式機械手臂的期望位移；
Figure 109105755-A0305-02-0017-45
為並聯式機械手臂的期望速度；Bm為目標阻抗的阻尼；及Km為目標阻抗的彈簧。 The system for estimating the external force of the parallel manipulator according to claim 1, wherein the external force estimation module calculates the external force that the parallel manipulator bears through the following formula:
Figure 109105755-A0305-02-0017-15
 Among them, F ext is the contact external force; P is the true displacement of the parallel manipulator;
Figure 109105755-A0305-02-0017-43
Is the real speed of the parallel manipulator; P d is the expected displacement of the parallel manipulator;
Figure 109105755-A0305-02-0017-45
Is the desired speed of the parallel manipulator; B m is the damping of the target impedance; and K m is the spring of the target impedance. 一種並聯式機械手臂外力估測方法，其包含下列步驟：一並聯式機械手臂包含有三個馬達、與各馬達相連接的第一連桿、與各第一連桿相連接的第二連桿以及與各第二連桿相連接的操作平台； 一控制裝置與所述並聯式機械手臂的所述馬達連接，所述控制裝置自所述馬達接收量測到的力矩值以及轉動位置，所述控制裝置生成控制訊號以對所述並聯式機械手臂的所述馬達進行控制；所述控制裝置透過對並聯式機械手臂的位置、速度以及加速度進行運動學分析，再對並聯式機械手臂進行動力學分析，以得到並聯式機械手臂的動態模型，所述並聯式機械手臂的動態模型由接收到的力矩值以及轉動位置所識別與計算出；所述控制裝置透過質量阻尼彈簧二階系統建立並聯式機械手臂與外力的一阻抗控制器；及所述控制裝置將並聯式機械手臂的實際軌跡以及並聯式機械手臂的理論軌跡的誤差代入所述阻抗控制器以計算得到並聯式機械手臂所承受到的外力。 A method for estimating the external force of a parallel manipulator includes the following steps: a parallel manipulator includes three motors, a first link connected to each motor, a second link connected to each first link, and Operating platform connected with each second connecting rod; A control device is connected to the motor of the parallel manipulator, the control device receives the measured torque value and the rotational position from the motor, and the control device generates a control signal to control the parallel manipulator. The motor is controlled; the control device performs kinematic analysis on the position, speed and acceleration of the parallel robot arm, and then performs dynamic analysis on the parallel robot arm to obtain the dynamic model of the parallel robot arm. The dynamic model of the parallel manipulator is identified and calculated by the received torque value and rotation position; the control device establishes an impedance controller for the parallel manipulator and external force through the second-order system of mass damping springs; and the control The device substitutes the error of the actual trajectory of the parallel robot arm and the theoretical trajectory of the parallel robot arm into the impedance controller to calculate the external force that the parallel robot arm bears. 如請求項6所述的並聯式機械手臂外力估測方法，其中所述動態模型為下列公式：
Figure 109105755-A0305-02-0018-16
 其中，τ m 為並聯式機械手臂的馬達扭矩；q為並聯式機械手臂各節點的角度向量；
Figure 109105755-A0305-02-0018-46
為並聯式機械手臂各節點的角速度向量；
Figure 109105755-A0305-02-0018-47
為並聯式機械手臂各節點的角加速度向量；D為並聯式機械手臂的慣性矩陣； C為並聯式機械手臂的離心力矩陣；G為並聯式機械手臂的重力項矩陣；F為並聯式機械手臂的馬達摩擦力；J T 為雅可比(Jacobian)矩陣的轉置矩陣；及F ext 為接觸外力。 The method for estimating the external force of a parallel manipulator according to claim 6, wherein the dynamic model is the following formula:
Figure 109105755-A0305-02-0018-16
 Among them, τ m is the motor torque of the parallel manipulator; q is the angle vector of each node of the parallel manipulator;
Figure 109105755-A0305-02-0018-46
Is the angular velocity vector of each node of the parallel manipulator;
Figure 109105755-A0305-02-0018-47
Is the angular acceleration vector of each node of the parallel manipulator; D is the inertia matrix of the parallel manipulator; C is the centrifugal force matrix of the parallel manipulator; G is the gravity term matrix of the parallel manipulator; F is the parallel manipulator’s Motor friction; J T is the transposed matrix of the Jacobian matrix; and F ext is the contact external force. 如請求項6所述的並聯式機械手臂外力估測方法，其中所述質量阻尼彈簧二階系統為下列公式：
Figure 109105755-A0305-02-0019-17
 其中，P為並聯式機械手臂的真實位移；
Figure 109105755-A0305-02-0019-50
為並聯式機械手臂的真實速度；
Figure 109105755-A0305-02-0019-51
為並聯式機械手臂的真實加速度；Pd為並聯式機械手臂的期望位移；
Figure 109105755-A0305-02-0019-53
為並聯式機械手臂的期望速度；
Figure 109105755-A0305-02-0019-54
為並聯式機械手臂的期望加速度；Mm為目標阻抗的質量；Bm為目標阻抗的阻尼；及Km為目標阻抗的彈簧。 The method for estimating the external force of a parallel manipulator according to claim 6, wherein the second-order mass damping spring system is the following formula:
Figure 109105755-A0305-02-0019-17
 Among them, P is the true displacement of the parallel robot arm;
Figure 109105755-A0305-02-0019-50
Is the true speed of the parallel robot arm;
Figure 109105755-A0305-02-0019-51
Is the true acceleration of the parallel manipulator; P d is the expected displacement of the parallel manipulator;
Figure 109105755-A0305-02-0019-53
Is the expected speed of the parallel manipulator;
Figure 109105755-A0305-02-0019-54
Is the expected acceleration of the parallel manipulator; M m is the mass of the target impedance; B m is the damping of the target impedance; and K m is the spring of the target impedance. 如請求項6所述的並聯式機械手臂外力估測方法，其中所述阻抗控制器為下列公式：
Figure 109105755-A0305-02-0020-18
 其中，τ m 為並聯式機械手臂的馬達扭矩；W -1為JD -1的逆矩陣；J為雅可比矩陣；j為雅可比矩陣的微分矩陣；D -1為並聯式機械手臂的慣性矩陣的逆矩陣；
Figure 109105755-A0305-02-0020-55
為並聯式機械手臂各節點的角速度向量；P為並聯式機械手臂的真實位移；
Figure 109105755-A0305-02-0020-56
為並聯式機械手臂的真實速度；
Figure 109105755-A0305-02-0020-57
為並聯式機械手臂的真實加速度；Pd為並聯式機械手臂的期望位移；
Figure 109105755-A0305-02-0020-60
為並聯式機械手臂的期望速度；
Figure 109105755-A0305-02-0020-61
為並聯式機械手臂的期望加速度；M m -1為目標阻抗的質量的逆矩陣；Bm為目標阻抗的阻尼；Km為目標阻抗的彈簧；C為並聯式機械手臂的離心力矩陣；G為並聯式機械手臂的重力項矩陣； F為並聯式機械手臂的馬達摩擦力；J T 為雅可比(Jacobian)矩陣的轉置矩陣；及F ext 為接觸外力。 The method for estimating the external force of a parallel manipulator according to claim 6, wherein the impedance controller is the following formula:
Figure 109105755-A0305-02-0020-18
 Among them, τ m is the motor torque of the parallel manipulator; W -1 is the inverse matrix of JD -1 ; J is the Jacobian matrix; j is the differential matrix of the Jacobian matrix; D -1 is the inertia matrix of the parallel manipulator Inverse matrix
Figure 109105755-A0305-02-0020-55
Is the angular velocity vector of each node of the parallel manipulator; P is the true displacement of the parallel manipulator;
Figure 109105755-A0305-02-0020-56
Is the true speed of the parallel robot arm;
Figure 109105755-A0305-02-0020-57
Is the true acceleration of the parallel manipulator; P d is the expected displacement of the parallel manipulator;
Figure 109105755-A0305-02-0020-60
Is the expected speed of the parallel manipulator;
Figure 109105755-A0305-02-0020-61
Is the expected acceleration of the parallel manipulator; M m -1 is the inverse matrix of the mass of the target impedance; B m is the damping of the target impedance; K m is the spring of the target impedance; C is the centrifugal force matrix of the parallel manipulator; G is The gravity term matrix of the parallel manipulator; F is the motor friction force of the parallel manipulator; J T is the transposed matrix of the Jacobian matrix; and F ext is the contact external force. 如請求項6所述的並聯式機械手臂外力估測方法，其中所述控制裝置透過下列公式以計算得到並聯式機械手臂所承受到的外力：
Figure 109105755-A0305-02-0021-19
 其中，F ext 為接觸外力；P為並聯式機械手臂的真實位移；
Figure 109105755-A0305-02-0021-62
為並聯式機械手臂的真實速度；Pd為並聯式機械手臂的期望位移；
Figure 109105755-A0305-02-0021-63
為並聯式機械手臂的期望速度；Bm為目標阻抗的阻尼；及Km為目標阻抗的彈簧。 The method for estimating the external force of the parallel manipulator according to claim 6, wherein the control device calculates the external force that the parallel manipulator bears through the following formula:
Figure 109105755-A0305-02-0021-19
 Among them, F ext is the contact external force; P is the true displacement of the parallel manipulator;
Figure 109105755-A0305-02-0021-62
Is the real speed of the parallel manipulator; P d is the expected displacement of the parallel manipulator;
Figure 109105755-A0305-02-0021-63
Is the desired speed of the parallel manipulator; B m is the damping of the target impedance; and K m is the spring of the target impedance.
TW109105755A 2020-02-21 2020-02-21 External force estimation system for delta robot and method thereof TWI714462B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
TW109105755A TWI714462B (en) 2020-02-21 2020-02-21 External force estimation system for delta robot and method thereof

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
TW109105755A TWI714462B (en) 2020-02-21 2020-02-21 External force estimation system for delta robot and method thereof

Publications (2)

Publication Number Publication Date
TWI714462B true TWI714462B (en) 2020-12-21
TW202132069A TW202132069A (en) 2021-09-01

Family

ID=74669761

Family Applications (1)

Application Number Title Priority Date Filing Date
TW109105755A TWI714462B (en) 2020-02-21 2020-02-21 External force estimation system for delta robot and method thereof

Country Status (1)

Country Link
TW (1) TWI714462B (en)

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7367771B2 (en) * 2002-07-09 2008-05-06 Amir Khajepour Light weight parallel manipulators using active/passive cables
CN102049776A (en) * 2009-10-26 2011-05-11 发那科株式会社 Parallel link robot
CN103034123A (en) * 2012-12-11 2013-04-10 中国科学技术大学 Dynamic model parameter identification based parallel robot control method
CN109333530A (en) * 2018-10-08 2019-02-15 浙江工业大学 A kind of six articulated mechanical arm Study on Contact Force Control based on elastic actuator of connecting

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7367771B2 (en) * 2002-07-09 2008-05-06 Amir Khajepour Light weight parallel manipulators using active/passive cables
CN102049776A (en) * 2009-10-26 2011-05-11 发那科株式会社 Parallel link robot
CN103034123A (en) * 2012-12-11 2013-04-10 中国科学技术大学 Dynamic model parameter identification based parallel robot control method
CN109333530A (en) * 2018-10-08 2019-02-15 浙江工业大学 A kind of six articulated mechanical arm Study on Contact Force Control based on elastic actuator of connecting

Also Published As

Publication number Publication date
TW202132069A (en) 2021-09-01

Similar Documents

Publication Publication Date Title
CN111136633B (en) All-state control method for flexible master-slave robot system under time-varying delay
CN106483964B (en) Robot compliance control method based on contact force observer
CN109015658B (en) Space double-arm robot control method for capturing rolling target
JP3124519B2 (en) Robot controller with control system mode switching function
WO2023071126A1 (en) End force soft measurement method for hydraulic manipulator
Morel et al. The precise control of manipulators with joint friction: A base force/torque sensor method
Broenink et al. Peg-in-hole assembly using impedance control with a 6 dof robot
Zou et al. An approach for peg-in-hole assembling based on force feedback control
TWI714462B (en) External force estimation system for delta robot and method thereof
Xia et al. Hybrid force/position control of industrial robotic manipulator based on Kalman filter
Park et al. Multi-link multi-contact force control for manipulators
Lotfi et al. Global adaptive estimation of joint velocities in robotic manipulators
Yang et al. Dynamic modeling based on real-time deflection measurement and compensation control for flexible multi-link manipulators
CN114355771A (en) Cooperative robot force and position hybrid control method and system
Kövecses et al. Dynamic modeling and analysis of a robot manipulator intercepting and capturing a moving object, with the consideration of structural flexibility
Natale et al. Robust hybrid force/position control with experiments on an industrial robot
Spiller et al. Superimposed force/torque-control of cooperating robots
Yao et al. Hybrid position, posture, force and moment control with impedance characteristics for robot manipulators
Ott et al. Admittance Control using a Base Force/Torque Sensor.
Sintov et al. Longitudinal regrasping of elongated objects
Lara-Molina et al. Generalized predictive control of parallel robots
Bascetta et al. Task space visual servoing of eye-in-hand flexible manipulators
TWI755688B (en) Fault detection system for delta robot and method thereof
Sahay et al. Simulation of robot arm dynamics using NE method of 2-link manipulator
Zhao et al. Stiffness characteristics and kinematics analysis of parallel 3-DOF mechanism with flexible joints