本發(fā)明涉及碰撞檢測(cè)，特別涉及一種基于動(dòng)力學(xué)模型與加速度調(diào)節(jié)閾值的碰撞檢測(cè)方法及系統(tǒng)。

背景技術(shù)：

1、晶圓傳輸機(jī)器人是專門應(yīng)用于ic制造業(yè)的半導(dǎo)體機(jī)器人，其主要用于晶圓的傳輸和搬運(yùn)，由于晶圓易碎且成本極高，且機(jī)器人手指也難于加工，發(fā)生碎片后會(huì)對(duì)整個(gè)真空環(huán)境造成污染，影響整個(gè)生產(chǎn)線的效率，因此晶圓傳輸機(jī)器人的安全問題尤為重要，而碰撞檢測(cè)屬于保障安全的核心之一。

2、碰撞檢測(cè)本質(zhì)上是對(duì)外界干擾力的檢測(cè)，方法主要可以分為兩大類：依靠外部傳感器的碰撞檢測(cè)方法與無外部傳感器的碰撞檢測(cè)方法。

3、基于外部傳感器的這類方法,通過安裝傳感器（如力/力矩傳感器，加速度傳感器等）感知外界碰撞力矩進(jìn)行碰撞檢測(cè)，進(jìn)而對(duì)外界碰撞做出判斷，但此項(xiàng)技術(shù)增加了系統(tǒng)成本，且檢測(cè)區(qū)域有限。

4、無外部傳感器的碰撞檢測(cè)方法,通過建立機(jī)器人的動(dòng)力學(xué)模型，將采集到的軌跡代入機(jī)器人動(dòng)力學(xué)方程作為機(jī)器人的理論力矩，同時(shí)采集機(jī)器人各關(guān)節(jié)對(duì)應(yīng)電機(jī)的電流，將電流轉(zhuǎn)換成力矩后與理論力矩作差后取絕對(duì)值，并設(shè)定固定閾值或動(dòng)態(tài)閾值，超出設(shè)定的閾值則判定碰撞發(fā)生，但設(shè)定固定的閾值面臨兩個(gè)問題：第一，閾值過大會(huì)導(dǎo)致較小的碰撞檢測(cè)不到；第二，閾值過小會(huì)導(dǎo)致碰撞檢測(cè)過于靈敏，可能由于電機(jī)加減速造成力矩的突變而誤判為發(fā)生碰撞，導(dǎo)致停機(jī)影響工作效率。

技術(shù)實(shí)現(xiàn)思路

1、本發(fā)明目的是：提供一種基于動(dòng)力學(xué)模型與加速度調(diào)節(jié)閾值的碰撞檢測(cè)方法及系統(tǒng)，以解決現(xiàn)有技術(shù)中碰撞閾值的設(shè)定受限而使得碰撞無法被檢測(cè)或者檢測(cè)過于靈敏的問題。

2、本發(fā)明的技術(shù)方案是：一種基于動(dòng)力學(xué)模型與加速度調(diào)節(jié)閾值的碰撞檢測(cè)方法，包括：

3、建立含摩擦的機(jī)器人動(dòng)力學(xué)模型；

4、設(shè)定實(shí)時(shí)碰撞閾值，計(jì)算實(shí)際力矩與含摩擦的理論力矩之間的絕對(duì)差值，將絕對(duì)差值與實(shí)時(shí)碰撞閾值對(duì)比，判斷碰撞是否發(fā)生；

5、其中，實(shí)時(shí)碰撞閾值的設(shè)定方法為：

6、a、設(shè)定初始碰撞閾值，以及調(diào)整閾值；

7、b、確定機(jī)器人的運(yùn)行軌跡，并基于機(jī)器人的當(dāng)前運(yùn)行狀態(tài)對(duì)運(yùn)行軌跡進(jìn)行實(shí)時(shí)分割，包括加速路徑、勻速路徑及減速路徑；

8、c、獲取機(jī)器人距離就近目標(biāo)位的實(shí)時(shí)軌跡距離；

9、d、基于機(jī)器人在不同路徑的加速度設(shè)定運(yùn)行軌跡對(duì)應(yīng)的閾值調(diào)節(jié)臨界點(diǎn)，并采用調(diào)整閾值對(duì)初始碰撞閾值進(jìn)行修正，獲得實(shí)時(shí)碰撞閾值。

10、優(yōu)選的，機(jī)器人的運(yùn)行軌跡對(duì)應(yīng)的兩端點(diǎn)分別為第一目標(biāo)位及第二目標(biāo)位；

11、機(jī)器人的當(dāng)前運(yùn)行狀態(tài)包括運(yùn)行方向相反的第一狀態(tài)和第二狀態(tài)：

12、第一狀態(tài)下機(jī)器人沿運(yùn)行軌跡由第一目標(biāo)位至第二目標(biāo)位運(yùn)動(dòng)，并將運(yùn)行軌跡由第一目標(biāo)位至第二目標(biāo)位分割為加速路徑、勻速路徑及減速路徑；

13、第二狀態(tài)下機(jī)器人沿運(yùn)行軌跡由第二目標(biāo)位至第一目標(biāo)位運(yùn)動(dòng)，并將運(yùn)行軌跡由第一目標(biāo)位至第二目標(biāo)位分割為減速路徑、勻速路徑及加速路徑。

14、優(yōu)選的，機(jī)器人在加速路徑上具有第一加速度，在減速路徑上具有第二加速度；

15、所述閾值調(diào)節(jié)臨界點(diǎn)與就近目標(biāo)位之間的距離與所述加速度的絕對(duì)值呈正相關(guān)。

16、優(yōu)選的，每個(gè)運(yùn)行狀態(tài)下，所述運(yùn)行軌跡上均對(duì)應(yīng)兩個(gè)閾值調(diào)節(jié)臨界點(diǎn)，兩個(gè)所述閾值調(diào)節(jié)臨界點(diǎn)分別處在加速路徑和減速路徑上，并分別定義為加速閾值調(diào)節(jié)臨界點(diǎn)及減速閾值調(diào)節(jié)臨界點(diǎn)。

17、優(yōu)選的，機(jī)器人在所述加速閾值調(diào)節(jié)臨界點(diǎn)與就近的目標(biāo)位之間，以及減速閾值調(diào)節(jié)臨界點(diǎn)與就近的目標(biāo)位之間運(yùn)行時(shí)對(duì)應(yīng)的實(shí)時(shí)碰撞閾值為初始碰撞閾值與調(diào)整閾值之差；

18、機(jī)器人在加速閾值調(diào)節(jié)臨界點(diǎn)與減速閾值調(diào)節(jié)臨界點(diǎn)之間運(yùn)行時(shí)對(duì)應(yīng)的實(shí)時(shí)碰撞閾值為初始碰撞閾值與調(diào)整閾值之和。

19、優(yōu)選的，設(shè)定加速路徑與勻速路徑之間為第一交界點(diǎn)，減速路徑與勻速路徑之間為第二交界點(diǎn)；

20、機(jī)器人在所述加速閾值調(diào)節(jié)臨界點(diǎn)與就近的目標(biāo)位之間，以及減速閾值調(diào)節(jié)臨界點(diǎn)與就近的目標(biāo)位之間運(yùn)行時(shí)對(duì)應(yīng)的實(shí)時(shí)碰撞閾值為初始碰撞閾值與調(diào)整閾值之差；

21、機(jī)器人在所述加速閾值調(diào)節(jié)臨界點(diǎn)與第一交界點(diǎn)之間，以及減速閾值調(diào)節(jié)臨界點(diǎn)與第二交界點(diǎn)之間運(yùn)行時(shí)對(duì)應(yīng)的實(shí)時(shí)碰撞閾值為初始碰撞閾值；

22、機(jī)器人在所述勻速路徑運(yùn)行時(shí)對(duì)應(yīng)的實(shí)時(shí)碰撞閾值為初始碰撞閾值與調(diào)整閾值之和。

23、優(yōu)選的，含摩擦的機(jī)器人動(dòng)力學(xué)模型的建立方法為：

24、建立機(jī)器人的動(dòng)力學(xué)模型；

25、采集機(jī)器人各個(gè)關(guān)節(jié)的實(shí)際力矩，并根據(jù)采集的機(jī)器人位置計(jì)算得到無摩擦的理論力矩；

26、確定機(jī)器人各個(gè)關(guān)節(jié)的摩擦系數(shù)，辨識(shí)出來的摩擦系數(shù)使理論力矩與實(shí)際力矩之間誤差的平方和最小，進(jìn)而建立出含摩擦的機(jī)器人動(dòng)力學(xué)模型。

27、優(yōu)選的，機(jī)器人各個(gè)關(guān)節(jié)的實(shí)際力矩的獲取方法為：

28、采集各個(gè)關(guān)節(jié)對(duì)應(yīng)電機(jī)的電流，并將電流轉(zhuǎn)換成實(shí)際力矩；

29、轉(zhuǎn)換關(guān)系如下：

30、；

31、其中，為實(shí)際力矩，為電機(jī)的轉(zhuǎn)矩常數(shù)，為采集到的電流；

32、機(jī)器人各個(gè)關(guān)節(jié)的無摩擦理論力矩的獲取方法為：

33、采集機(jī)器人的編碼數(shù)據(jù)，通過中心差分法將下發(fā)的編碼值轉(zhuǎn)換為機(jī)器人的位置、速度、加速度數(shù)據(jù)，并帶入機(jī)器人動(dòng)力學(xué)模型，計(jì)算出機(jī)器人各個(gè)關(guān)節(jié)的無摩擦的理論力矩。

34、優(yōu)選的，含摩擦的機(jī)器人動(dòng)力學(xué)模型建立過程中，采用最小二乘法辨識(shí)出機(jī)器人各個(gè)關(guān)節(jié)的摩擦力模型中的摩擦系數(shù)，在機(jī)器人無碰撞情景下，辨識(shí)出來的摩擦系數(shù)應(yīng)使理論力矩與實(shí)際力矩之間誤差的平方和最小，將摩擦辨識(shí)轉(zhuǎn)換成了最優(yōu)化問題：

35、；

36、其中，n為樣本總數(shù)，為理論力矩，為實(shí)際力矩；

37、表示的約束條件，即為理論力矩與實(shí)際力矩之間誤差的平方和；

38、最優(yōu)化問題即為使最小的靜摩擦系數(shù)和動(dòng)摩擦系數(shù)；即辨識(shí)出了機(jī)器人各個(gè)關(guān)節(jié)的摩擦系數(shù)，建立出含摩擦的機(jī)器人動(dòng)力學(xué)模型。

39、本申請(qǐng)還公開了一種基于動(dòng)力學(xué)模型與加速度調(diào)節(jié)閾值的碰撞檢測(cè)系統(tǒng)，用于執(zhí)行上述的基于動(dòng)力學(xué)模型與加速度調(diào)節(jié)閾值的碰撞檢測(cè)方法。

40、與現(xiàn)有技術(shù)相比，本發(fā)明的優(yōu)點(diǎn)是：

41、（1）本發(fā)明對(duì)機(jī)器人的運(yùn)行軌跡進(jìn)行分割，在機(jī)器人靠近目標(biāo)物易發(fā)生碰撞的過程中，通過對(duì)初始碰撞閾值進(jìn)行調(diào)整，降低設(shè)定的初始碰撞閾值，從而實(shí)現(xiàn)碰撞檢測(cè)靈敏度的增加；而在其他路徑段，發(fā)生碰撞的概率較小，避免受到一些環(huán)境干擾等因素的影響，通過增加設(shè)定的初始碰撞閾值，降低碰撞檢測(cè)的靈敏度從而避免因閾值設(shè)定的過小造成停機(jī)。

42、（2）考慮到機(jī)器人運(yùn)行中發(fā)生碰撞暫停的可能，以及可能存在勻速路段很短甚至沒有的情況，本發(fā)明還結(jié)合了機(jī)器人與目標(biāo)物的實(shí)時(shí)軌跡距離進(jìn)行閾值調(diào)節(jié)，這種綜合考慮加速度和軌跡距離的雙重調(diào)節(jié)機(jī)制，使機(jī)器人在復(fù)雜的工況下也能準(zhǔn)確地設(shè)置合適的碰撞閾值，進(jìn)一步提高了機(jī)器人的適應(yīng)性和可靠性。

技術(shù)特征：

1.一種基于動(dòng)力學(xué)模型與加速度調(diào)節(jié)閾值的碰撞檢測(cè)方法，其特征在于，包括：

2.根據(jù)權(quán)利要求1所述的基于動(dòng)力學(xué)模型與加速度調(diào)節(jié)閾值的碰撞檢測(cè)方法，其特征在于，機(jī)器人的運(yùn)行軌跡對(duì)應(yīng)的兩端點(diǎn)分別為第一目標(biāo)位及第二目標(biāo)位；

3.根據(jù)權(quán)利要求2所述的基于動(dòng)力學(xué)模型與加速度調(diào)節(jié)閾值的碰撞檢測(cè)方法，其特征在于：機(jī)器人在加速路徑上具有第一加速度，在減速路徑上具有第二加速度；

4.根據(jù)權(quán)利要求2所述的基于動(dòng)力學(xué)模型與加速度調(diào)節(jié)閾值的碰撞檢測(cè)方法，其特征在于：每個(gè)運(yùn)行狀態(tài)下，所述運(yùn)行軌跡上均對(duì)應(yīng)兩個(gè)閾值調(diào)節(jié)臨界點(diǎn)，兩個(gè)所述閾值調(diào)節(jié)臨界點(diǎn)分別處在加速路徑和減速路徑上，并分別定義為加速閾值調(diào)節(jié)臨界點(diǎn)及減速閾值調(diào)節(jié)臨界點(diǎn)。

5.根據(jù)權(quán)利要求4所述的基于動(dòng)力學(xué)模型與加速度調(diào)節(jié)閾值的碰撞檢測(cè)方法，其特征在于：機(jī)器人在所述加速閾值調(diào)節(jié)臨界點(diǎn)與就近的目標(biāo)位之間，以及減速閾值調(diào)節(jié)臨界點(diǎn)與就近的目標(biāo)位之間運(yùn)行時(shí)對(duì)應(yīng)的實(shí)時(shí)碰撞閾值為初始碰撞閾值與調(diào)整閾值之差；

6.根據(jù)權(quán)利要求4所述的基于動(dòng)力學(xué)模型與加速度調(diào)節(jié)閾值的碰撞檢測(cè)方法，其特征在于：設(shè)定加速路徑與勻速路徑之間為第一交界點(diǎn)，減速路徑與勻速路徑之間為第二交界點(diǎn)；

7.根據(jù)權(quán)利要求1所述的基于動(dòng)力學(xué)模型與加速度調(diào)節(jié)閾值的碰撞檢測(cè)方法，其特征在于，含摩擦的機(jī)器人動(dòng)力學(xué)模型的建立方法為：

8.根據(jù)權(quán)利要求7所述的基于動(dòng)力學(xué)模型與加速度調(diào)節(jié)閾值的碰撞檢測(cè)方法，其特征在于，機(jī)器人各個(gè)關(guān)節(jié)的實(shí)際力矩的獲取方法為：

9.根據(jù)權(quán)利要求7所述的基于動(dòng)力學(xué)模型與加速度調(diào)節(jié)閾值的碰撞檢測(cè)方法，其特征在于，含摩擦的機(jī)器人動(dòng)力學(xué)模型建立過程中，采用最小二乘法辨識(shí)出機(jī)器人各個(gè)關(guān)節(jié)的摩擦力模型中的摩擦系數(shù)，在機(jī)器人無碰撞情景下，辨識(shí)出來的摩擦系數(shù)應(yīng)使理論力矩與實(shí)際力矩之間誤差的平方和最小，將摩擦辨識(shí)轉(zhuǎn)換成了最優(yōu)化問題：

10.基于動(dòng)力學(xué)模型與加速度調(diào)節(jié)閾值的碰撞檢測(cè)系統(tǒng)，其特征在于：用于執(zhí)行權(quán)利要求1-9任一項(xiàng)所述的基于動(dòng)力學(xué)模型與加速度調(diào)節(jié)閾值的碰撞檢測(cè)方法。

技術(shù)總結(jié)
本發(fā)明涉及碰撞檢測(cè)技術(shù)領(lǐng)域，具體涉及一種基于動(dòng)力學(xué)模型與加速度調(diào)節(jié)閾值的碰撞檢測(cè)方法及系統(tǒng)，碰撞檢測(cè)方法包括：建立含摩擦的機(jī)器人動(dòng)力學(xué)模型；設(shè)定實(shí)時(shí)碰撞閾值，計(jì)算實(shí)際力矩與含摩擦的理論力矩之間的絕對(duì)差值，將絕對(duì)差值與實(shí)時(shí)碰撞閾值對(duì)比，判斷碰撞是否發(fā)生。本發(fā)明對(duì)機(jī)器人的運(yùn)行軌跡進(jìn)行分割，在機(jī)器人靠近目標(biāo)物易發(fā)生碰撞的過程中，通過對(duì)初始碰撞閾值進(jìn)行調(diào)整，降低設(shè)定的初始碰撞閾值，從而實(shí)現(xiàn)碰撞檢測(cè)靈敏度的增加；而在其他路徑段，發(fā)生碰撞的概率較小，增加設(shè)定的初始碰撞閾值，降低碰撞檢測(cè)的靈敏度從而避免因閾值設(shè)定的過小造成停機(jī)。

技術(shù)研發(fā)人員：王彭,方興雨,丁文華
受保護(hù)的技術(shù)使用者：泓滸（蘇州）半導(dǎo)體科技有限公司
技術(shù)研發(fā)日：
技術(shù)公布日：2025/5/15