本發(fā)明涉及機(jī)器人控制技術(shù)領(lǐng)域，特別涉及一種機(jī)器人控制方法與裝置。

背景技術(shù)：

現(xiàn)有技術(shù)中，在末端帶有力傳感器的機(jī)器人拖動(dòng)示教方法中，通常有以下三種處理方式，第一種方法是通過(guò)獲取傳感器力矩，獲取機(jī)器人的法蘭工裝末端速度，并將獲取的力矩與法蘭工裝末端速度作為一個(gè)PID(比例、積分、微分)控制環(huán)處理，進(jìn)而得到機(jī)器人各軸關(guān)節(jié)的下發(fā)位置，實(shí)現(xiàn)拖動(dòng)示教的功能。第二種方法是通過(guò)動(dòng)力學(xué)方程直接由獲取的傳感器力矩?fù)Q算出機(jī)器人各關(guān)節(jié)軸的下發(fā)位置。第三種方法是由獲取的傳感器力矩直接規(guī)劃?rùn)C(jī)器人法蘭工裝末端的軌跡，并逆解得到機(jī)器人各軸關(guān)節(jié)的軌跡。

在實(shí)現(xiàn)本發(fā)明的過(guò)程中，發(fā)明人發(fā)現(xiàn)現(xiàn)有技術(shù)中至少存在以下問(wèn)題：第一種方法由于不需要進(jìn)一步規(guī)劃?rùn)C(jī)器人各軸關(guān)節(jié)的速度，因而，在拖動(dòng)機(jī)器人時(shí)，無(wú)法有效的規(guī)避奇異位置。第二種方法直接由獲取的傳感器力矩?fù)Q算出機(jī)器人各關(guān)節(jié)軸的下發(fā)位置，因而，機(jī)器人拖動(dòng)示教的精度比較差。第三種方法由于不通過(guò)獲取的機(jī)器人各軸關(guān)節(jié)的速度實(shí)現(xiàn)軌跡規(guī)劃，因而，機(jī)器人拖動(dòng)示教的精度比較差，且在拖動(dòng)示教的過(guò)程中難以規(guī)避奇異位置。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明實(shí)施方式的目的在于提供一種機(jī)器人控制方法與裝置，使得控制機(jī)器人的過(guò)程精度更高，靈敏度更好。

為解決上述技術(shù)問(wèn)題，本發(fā)明的實(shí)施方式提供了一種機(jī)器人控制方法，包括：設(shè)置追蹤周期；在追蹤周期內(nèi)采集力傳感器信息，根據(jù)采集的力傳感器信息獲取機(jī)器人多軸機(jī)構(gòu)各軸關(guān)節(jié)的追蹤位置，其中，追蹤位置為多軸機(jī)構(gòu)各軸關(guān)節(jié)在下一追蹤周期的位置；記錄追蹤周期與追蹤位置，在追蹤周期內(nèi)對(duì)追蹤位置進(jìn)行軌跡規(guī)劃并下發(fā)至伺服控制系統(tǒng)，控制多軸機(jī)構(gòu)各軸關(guān)節(jié)移動(dòng)至追蹤位置。

本發(fā)明的實(shí)施方式還提供了一種機(jī)器人控制裝置，包括：設(shè)置模塊，用于設(shè)置追蹤周期；獲取模塊，用于在追蹤周期內(nèi)采集力傳感器信息，根據(jù)采集的力傳感器信息獲取機(jī)器人多軸機(jī)構(gòu)各軸關(guān)節(jié)的追蹤位置，其中，追蹤位置為多軸機(jī)構(gòu)各軸關(guān)節(jié)在下一追蹤周期的位置。位置下發(fā)模塊，用于記錄追蹤周期與追蹤位置，在追蹤周期內(nèi)對(duì)追蹤位置進(jìn)行軌跡規(guī)劃并下發(fā)至伺服控制系統(tǒng)，控制多軸機(jī)構(gòu)各軸關(guān)節(jié)移動(dòng)至追蹤位置。

本發(fā)明實(shí)施方式相對(duì)于現(xiàn)有技術(shù)而言，通過(guò)在追蹤周期內(nèi)采集力傳感信息，并由此規(guī)劃?rùn)C(jī)器人多軸機(jī)構(gòu)各軸關(guān)節(jié)的追蹤位置，這樣，有利于在控制機(jī)器人的過(guò)程中提高各關(guān)節(jié)軸運(yùn)動(dòng)的精度，以及提高靈敏度。通過(guò)記錄及下發(fā)各位置下發(fā)周期的下發(fā)位置至伺服控制系統(tǒng)，控制多軸機(jī)構(gòu)各軸關(guān)節(jié)移動(dòng)至各下發(fā)位置，這樣，有利于在拖動(dòng)機(jī)器人移動(dòng)一次后，實(shí)現(xiàn)示教機(jī)器人的功能。

另外，在追蹤周期內(nèi)采集力傳感器信息，根據(jù)采集的力傳感器信息獲取機(jī)器人多軸機(jī)構(gòu)各軸關(guān)節(jié)的追蹤位置，包括：在追蹤周期內(nèi)采集力傳感器信息，根據(jù)采集的力傳感器信息獲取多軸機(jī)構(gòu)的法蘭工裝末端速度；根據(jù)多軸機(jī)構(gòu)的法蘭工裝末端速度，獲取多軸機(jī)構(gòu)各軸關(guān)節(jié)的追蹤速度；根據(jù)多軸機(jī)構(gòu)各軸關(guān)節(jié)的追蹤速度，獲取多軸機(jī)構(gòu)各軸關(guān)節(jié)的追蹤位置。本發(fā)明實(shí)施方式中，根據(jù)采集的力傳感器信息，可以獲取多軸機(jī)構(gòu)各軸關(guān)節(jié)的追蹤速度，這樣，有利用在獲取機(jī)器人多軸機(jī)構(gòu)各軸關(guān)節(jié)的追蹤位置后，在將機(jī)器人拖動(dòng)至追蹤位置的過(guò)程中，提高多軸機(jī)構(gòu)各軸關(guān)節(jié)移動(dòng)的精度以及靈敏度。

另外，根據(jù)多軸機(jī)構(gòu)各軸關(guān)節(jié)的追蹤速度，獲取多軸機(jī)構(gòu)各軸關(guān)節(jié)的追蹤位置包括：在追蹤周期內(nèi)，獲取多軸機(jī)構(gòu)各軸關(guān)節(jié)的速度；根據(jù)多軸機(jī)構(gòu)各軸關(guān)節(jié)的速度以及多軸機(jī)構(gòu)各軸關(guān)節(jié)的追蹤速度，獲取多軸機(jī)構(gòu)各軸關(guān)節(jié)的加速度；根據(jù)多軸機(jī)構(gòu)各軸關(guān)節(jié)的速度、多軸機(jī)構(gòu)各軸關(guān)節(jié)的加速度以及追蹤周期，獲取多軸機(jī)構(gòu)各軸關(guān)節(jié)的追蹤位置。本發(fā)明實(shí)施方式中，根據(jù)獲取的多軸機(jī)構(gòu)各軸關(guān)節(jié)的速度、加速度以及追蹤周期，可以獲取多軸機(jī)構(gòu)各軸關(guān)節(jié)的追蹤位置，這樣，使得獲取的追蹤位置更準(zhǔn)確。

另外，獲取多軸機(jī)構(gòu)各軸關(guān)節(jié)的追蹤速度后，還包括：如果在多軸機(jī)構(gòu)各軸關(guān)節(jié)的追蹤速度中，存在超出預(yù)設(shè)的限制關(guān)節(jié)速度的追蹤速度，則對(duì)追蹤速度超出限制關(guān)節(jié)速度的軸關(guān)節(jié)，進(jìn)行速度修正，將限制關(guān)節(jié)速度作為修正后的追蹤速度，并且其余各軸關(guān)節(jié)的追蹤速度按相同比例降低。本發(fā)明實(shí)施方式中，對(duì)于超出預(yù)設(shè)的限制關(guān)節(jié)速度的追蹤速度，可以進(jìn)行速度修正，并將限制關(guān)節(jié)速度作為修正后的追蹤速度，同時(shí)其余各軸關(guān)節(jié)的追蹤速度可以按相同比例降低，這樣，有利于在拖動(dòng)多軸機(jī)構(gòu)各軸關(guān)節(jié)移動(dòng)的過(guò)程中，有效地規(guī)避奇異位置。

附圖說(shuō)明

圖1是根據(jù)本發(fā)明第一實(shí)施方式的機(jī)器人控制方法的流程圖；

圖2是根據(jù)本發(fā)明第二實(shí)施方式的機(jī)器人控制方法的流程圖；

圖3是根據(jù)本發(fā)明第三實(shí)施方式的機(jī)器人控制裝置的結(jié)構(gòu)圖。

具體實(shí)施方式

為使本發(fā)明的目的、技術(shù)方案和優(yōu)點(diǎn)更加清楚，下面將結(jié)合附圖對(duì)本發(fā)明的各實(shí)施方式進(jìn)行詳細(xì)的闡述。然而，本領(lǐng)域的普通技術(shù)人員可以理解，在本發(fā)明各實(shí)施方式中，為了使讀者更好地理解本申請(qǐng)而提出了許多技術(shù)細(xì)節(jié)。但是，即使沒(méi)有這些技術(shù)細(xì)節(jié)和基于以下各實(shí)施方式的種種變化和修改，也可以實(shí)現(xiàn)本申請(qǐng)所要求保護(hù)的技術(shù)方案。

本發(fā)明的第一實(shí)施方式涉及一種機(jī)器人控制方法，如圖1所示，包括：

步驟101：設(shè)置追蹤周期。具體地說(shuō)，終端可以預(yù)先設(shè)置追蹤周期，其中，追蹤周期為獲取的力以及力矩值的更新周期。

更具體地說(shuō)，力傳感器可以安裝在機(jī)器人多軸機(jī)構(gòu)的法蘭工裝末端上。終端可以預(yù)先設(shè)置追蹤周期，并在追蹤周期內(nèi)多次采集力傳感器信息，同時(shí)，對(duì)采集的力傳感器信息進(jìn)行濾波處理，從而，得到需要更新的力以及力矩值。例如，追蹤周期為0.01秒時(shí)，終端可以在0.01秒內(nèi)多次采集力傳感器信息，得到多個(gè)力以及力矩值，并對(duì)采集到的力傳感器信息進(jìn)行濾波處理。終端可以獲取得到的多個(gè)力以及力矩值的平均值，并將上述平均值作為追蹤周期內(nèi)獲取的力以及力矩值。這樣，終端可以每0.01秒更新一次獲取的力以及力矩值。

步驟102：在追蹤周期內(nèi)采集力傳感器信息，根據(jù)采集的力傳感器信息獲取機(jī)器人多軸機(jī)構(gòu)各軸關(guān)節(jié)的追蹤位置。其中，追蹤位置為多軸機(jī)構(gòu)各軸關(guān)節(jié)在下一追蹤周期的位置。

具體地說(shuō)，終端通過(guò)對(duì)在追蹤周期內(nèi)采集的力傳感器信息進(jìn)行濾波處理，獲取到需要更新的力以及力矩值后，可以在追蹤周期內(nèi)更新上述力以及力矩值，并且，可以根據(jù)更新后的力以及力矩值獲取多軸機(jī)構(gòu)的法蘭工裝末端的速度，進(jìn)而，根據(jù)上述獲取的法蘭工裝末端速度可以獲取機(jī)器人多軸機(jī)構(gòu)各軸關(guān)節(jié)的追蹤位置。

在步驟103：記錄追蹤周期與追蹤位置，在追蹤周期內(nèi)對(duì)追蹤位置進(jìn)行軌跡規(guī)劃并下發(fā)至伺服控制系統(tǒng)，控制多軸機(jī)構(gòu)各軸關(guān)節(jié)移動(dòng)至追蹤位置。具體地說(shuō)，在獲取多軸機(jī)構(gòu)各軸關(guān)節(jié)的追蹤位置后，終端可以記錄上述追蹤位置以及追蹤周期，并且可以根據(jù)上述追蹤位置以及預(yù)設(shè)的位置下發(fā)周期，在追蹤周期內(nèi)對(duì)多軸機(jī)構(gòu)各軸關(guān)節(jié)進(jìn)行軌跡規(guī)劃，得到追蹤周期內(nèi)各位置下發(fā)周期的下發(fā)位置，同時(shí)，可以將各下發(fā)位置下發(fā)至伺服控制系統(tǒng)，控制多軸機(jī)構(gòu)各軸關(guān)節(jié)在各位置下發(fā)周期內(nèi)移動(dòng)至各位置下發(fā)周期的下發(fā)位置，并最終在追蹤周期內(nèi)移動(dòng)至追蹤位置。同時(shí)，終端還可以記錄各位置下發(fā)周期的下發(fā)位置，這樣，機(jī)器人多軸機(jī)構(gòu)各軸關(guān)節(jié)可以復(fù)現(xiàn)上述規(guī)劃軌跡。

需要說(shuō)明的是，預(yù)設(shè)的位置下發(fā)周期可以小于或等于追蹤周期。例如，追蹤周期為0.01秒時(shí)，終端可以將位置下發(fā)周期設(shè)置為0.002秒。根據(jù)獲取的多軸機(jī)構(gòu)各軸關(guān)節(jié)的追蹤位置，終端可以在0.01秒內(nèi)對(duì)多軸機(jī)構(gòu)各軸關(guān)節(jié)進(jìn)行軌跡規(guī)劃，得到每0.002秒內(nèi)多軸機(jī)構(gòu)各軸關(guān)節(jié)需移動(dòng)至的下發(fā)位置，同時(shí)，可以將各下發(fā)位置下發(fā)至伺服控制系統(tǒng)，控制多軸機(jī)構(gòu)在各0.002秒內(nèi)移動(dòng)至相應(yīng)的下發(fā)位置，并最終在0.01秒內(nèi)移動(dòng)至追蹤位置。

本發(fā)明實(shí)施方式相對(duì)于現(xiàn)有技術(shù)而言，通過(guò)在追蹤周期內(nèi)采集力傳感信息，并由此獲取機(jī)器人多軸機(jī)構(gòu)各軸關(guān)節(jié)的追蹤位置，同時(shí)，根據(jù)獲取的追蹤位置在追蹤周期內(nèi)對(duì)多軸機(jī)構(gòu)各軸關(guān)節(jié)進(jìn)行軌跡規(guī)劃，得到多軸機(jī)構(gòu)各軸關(guān)節(jié)的各下發(fā)位置，這樣，有利于在控制機(jī)器人的過(guò)程中提高各關(guān)節(jié)軸運(yùn)動(dòng)的精度，以及提高靈敏度。通過(guò)記錄及下發(fā)各位置下發(fā)周期的下發(fā)位置至伺服控制系統(tǒng)，控制多軸機(jī)構(gòu)各軸關(guān)節(jié)移動(dòng)至各下發(fā)位置，這樣，有利于在拖動(dòng)機(jī)器人移動(dòng)一次后，實(shí)現(xiàn)示教機(jī)器人的功能。

本發(fā)明的第二實(shí)施方式涉及一種機(jī)器人控制方法。第二實(shí)施方式在第一實(shí)施方式的基礎(chǔ)上作了進(jìn)一步的改進(jìn)，主要改進(jìn)之處在于：本發(fā)明第二實(shí)施方式中，提出了一種更為具體的根據(jù)采集的力傳感器信息獲取機(jī)器人多軸機(jī)構(gòu)各軸關(guān)節(jié)的追蹤位置的方法。如圖2所示，包括：

步驟201：設(shè)置追蹤周期。具體地說(shuō)，終端可以預(yù)先設(shè)置追蹤周期，其中，追蹤周期為獲取的力以及力矩值的更新周期。

更具體地說(shuō)，力傳感器可以安裝在機(jī)器人多軸機(jī)構(gòu)的法蘭工裝末端上。終端可以預(yù)先設(shè)置追蹤周期，并在追蹤周期內(nèi)采集力傳感器信息，同時(shí)，對(duì)采集的力傳感器信息進(jìn)行濾波處理，從而，得到需要更新的力以及力矩值。

步驟202：在追蹤周期內(nèi)采集力傳感器信息，根據(jù)采集的力傳感器信息獲取多軸機(jī)構(gòu)的法蘭工裝末端速度。具體地說(shuō)，多軸機(jī)構(gòu)的法蘭工裝末端上安裝的力傳感器可以為6維力傳感器，終端通過(guò)對(duì)在追蹤周期內(nèi)采集的6維力傳感器信息進(jìn)行濾波處理，可以獲取到需要更新的力以及力矩值，其中，上述需要更新的力以及力矩值為6維向量，可以包括X、Y、Z方向的平移力和A、B、C方向的扭矩。進(jìn)一步，終端可以根據(jù)上述平移力、扭矩、預(yù)設(shè)的平移力比例關(guān)系以及扭矩比例關(guān)系，獲取多軸機(jī)構(gòu)的法蘭工裝末端速度，其中，上述多軸機(jī)構(gòu)的法蘭工裝末端速度的表達(dá)式為：

6_v＝[k_px，k_py，k_pz，k_ra，k_rb，k_rc]

其中，x、y、z分別為多軸機(jī)構(gòu)的法蘭工裝末端在X、Y、Z方向的平移力，a、b、c分別為多軸機(jī)構(gòu)的法蘭工裝末端在A、B、C方向的扭矩，k_p與k_r分別為預(yù)設(shè)的平移力比例關(guān)系與扭矩比例關(guān)系。

需要說(shuō)明的是，本步驟中力傳感器相對(duì)于多軸機(jī)構(gòu)的法蘭工裝末端速度也可以是非正比關(guān)系，比如，多軸機(jī)構(gòu)的法蘭工裝末端速度可以為力傳感器所獲力矩的分段函數(shù)。這樣，在力傳感器所獲力矩較小時(shí)，可以獲取更小的法蘭工裝末端速度，有利于提高精度。

此外，本步驟中終端還可以根據(jù)采集的力傳感器信息獲取多軸機(jī)構(gòu)的法蘭工裝末端加速度，并可以通過(guò)上述獲取的加速度得到多軸機(jī)構(gòu)的法蘭工裝末端速度。具體地說(shuō)，終端通過(guò)對(duì)在追蹤周期內(nèi)采集的6維力傳感器信息進(jìn)行濾波處理，可以獲取到需要更新的力以及力矩值，并且由上述力以及力矩值可以得到多軸機(jī)構(gòu)的法蘭工裝末端加速度，進(jìn)一步，可以根據(jù)上述加速度獲取多軸機(jī)構(gòu)的法蘭工裝末端速度。

步驟203：在追蹤周期內(nèi)，獲取多軸機(jī)構(gòu)各軸關(guān)節(jié)的位置。具體地說(shuō)，終端可以在追蹤周期內(nèi)，通過(guò)伺服控制系統(tǒng)獲取多軸機(jī)構(gòu)各軸關(guān)節(jié)當(dāng)前的位置。

步驟204：根據(jù)多軸機(jī)構(gòu)各軸關(guān)節(jié)的位置，獲取雅克比矩陣。具體地說(shuō)，根據(jù)獲取的多軸機(jī)構(gòu)各軸關(guān)節(jié)當(dāng)前的位置，可以獲取雅可比矩陣，其中，多軸機(jī)構(gòu)各軸關(guān)節(jié)的位置坐標(biāo)可以表示為Θ，雅克比矩陣為與Θ相關(guān)的函數(shù)，可以表示為⁶J(Θ)。

步驟205：根據(jù)多軸機(jī)構(gòu)的法蘭工裝末端速度與雅克比矩陣，獲取多軸機(jī)構(gòu)各軸關(guān)節(jié)的追蹤速度。具體地說(shuō)，根據(jù)多軸機(jī)構(gòu)的法蘭工裝末端速度⁶v與雅可比矩陣⁶J(Θ)，并依據(jù)多軸機(jī)構(gòu)各軸關(guān)節(jié)的追蹤速度的公式

通過(guò)求逆解，可以獲取多軸機(jī)構(gòu)各軸關(guān)節(jié)的追蹤速度。其中，⁶v是多軸機(jī)構(gòu)的法蘭工裝末端速度，⁶J(Θ)是雅可比矩陣，是多軸機(jī)構(gòu)各軸關(guān)節(jié)的追蹤速度。

步驟206：在追蹤周期內(nèi)，獲取多軸機(jī)構(gòu)各軸關(guān)節(jié)的速度。具體地說(shuō)，終端可以在追蹤周期內(nèi)，通過(guò)伺服控制系統(tǒng)獲取多軸機(jī)構(gòu)各軸關(guān)節(jié)當(dāng)前的速度。

步驟207：根據(jù)多軸機(jī)構(gòu)各軸關(guān)節(jié)的速度以及多軸機(jī)構(gòu)各軸關(guān)節(jié)的追蹤速度，獲取多軸機(jī)構(gòu)各軸關(guān)節(jié)的加速度。具體地說(shuō)，終端可以根據(jù)獲取的多軸機(jī)構(gòu)各關(guān)節(jié)軸當(dāng)前的速度以及追蹤速度，分別獲取多軸機(jī)構(gòu)各軸關(guān)節(jié)當(dāng)前的角速度以及追蹤角速度，進(jìn)一步，終端可以根據(jù)多軸機(jī)構(gòu)各軸關(guān)節(jié)當(dāng)前的角速度、追蹤角速度、追蹤周期以及公式

獲取多軸機(jī)構(gòu)各軸關(guān)節(jié)的角加速度。其中，以及分別為多軸機(jī)構(gòu)各軸關(guān)節(jié)當(dāng)前的角速度以及追蹤角速度，為多軸機(jī)構(gòu)各軸關(guān)節(jié)的角加速度，T為追蹤周期。

步驟208：根據(jù)多軸機(jī)構(gòu)各軸關(guān)節(jié)的速度、多軸機(jī)構(gòu)各軸關(guān)節(jié)的加速度以及追蹤速度，獲取多軸機(jī)構(gòu)各軸關(guān)節(jié)的追蹤位置。具體地說(shuō)，根據(jù)在追蹤周期內(nèi)獲取的多軸機(jī)構(gòu)各軸關(guān)節(jié)當(dāng)前的速度、多軸機(jī)構(gòu)各軸關(guān)節(jié)的加速度以及追蹤速度，并依據(jù)相關(guān)的運(yùn)動(dòng)學(xué)公式，終端可以獲取多軸機(jī)構(gòu)各軸關(guān)節(jié)的追蹤位置。

步驟209：記錄追蹤周期與追蹤位置，在追蹤周期內(nèi)對(duì)追蹤位置進(jìn)行軌跡規(guī)劃并下發(fā)至伺服控制系統(tǒng)，控制多軸機(jī)構(gòu)各軸關(guān)節(jié)移動(dòng)至追蹤位置。具體地說(shuō)，在獲取多軸機(jī)構(gòu)各關(guān)節(jié)軸的追蹤位置后，終端可以記錄上述追蹤位置以及追蹤周期，并且可以根據(jù)上述追蹤位置以及預(yù)設(shè)的位置下發(fā)周期，在追蹤周期內(nèi)對(duì)多軸機(jī)構(gòu)各軸關(guān)節(jié)進(jìn)行軌跡規(guī)劃，得到追蹤周期內(nèi)各位置下發(fā)周期的下發(fā)位置，同時(shí)，可以將各下發(fā)位置下發(fā)至伺服控制系統(tǒng)，控制多軸機(jī)構(gòu)各軸關(guān)節(jié)在各位置下發(fā)周期內(nèi)移動(dòng)至各位置下發(fā)周期的下發(fā)位置，并最終在追蹤周期內(nèi)移動(dòng)至追蹤位置。同時(shí)，終端還可以記錄各位置下發(fā)周期的下發(fā)位置，這樣，機(jī)器人多軸機(jī)構(gòu)各軸關(guān)節(jié)可以復(fù)現(xiàn)上述規(guī)劃軌跡。其中，預(yù)設(shè)的位置下發(fā)周期可以小于或等于追蹤周期。

需要說(shuō)明的是，在步驟205獲取多軸機(jī)構(gòu)各軸關(guān)節(jié)的追蹤速度后，如果多軸機(jī)構(gòu)各軸關(guān)節(jié)的追蹤速度中，存在超出預(yù)設(shè)的限制關(guān)節(jié)速度的追蹤速度，則對(duì)追蹤速度超出限制關(guān)節(jié)速度的軸關(guān)節(jié)，進(jìn)行速度修正，將限制關(guān)節(jié)速度作為修正后的追蹤速度，并且其余各軸關(guān)節(jié)的追蹤速度按相同比例降低。

此外，在步驟207獲取多軸機(jī)構(gòu)各軸關(guān)節(jié)的加速度后，如果在多軸機(jī)構(gòu)各軸關(guān)節(jié)的加速度中，存在超出預(yù)設(shè)的限制關(guān)節(jié)加速度的加速度，則對(duì)加速度超出限制關(guān)節(jié)加速度的軸關(guān)節(jié)，進(jìn)行加速度修正，將限制關(guān)節(jié)加速度作為修正后的加速度，并且其余各軸關(guān)節(jié)的加速度按相同比例降低。

本發(fā)明實(shí)施方式中，根據(jù)在追蹤周期內(nèi)采集的力傳感器信息，可以獲取多軸機(jī)構(gòu)各軸關(guān)節(jié)的追蹤速度與各軸關(guān)節(jié)的加速度，并由此可以獲取到多軸機(jī)構(gòu)各軸關(guān)節(jié)的追蹤位置。這樣，可以使得獲取的多軸機(jī)構(gòu)各軸關(guān)節(jié)的追蹤位置更準(zhǔn)確，且根據(jù)上述追蹤位置在追蹤周期內(nèi)對(duì)多軸機(jī)構(gòu)各軸關(guān)節(jié)進(jìn)行軌跡規(guī)劃，得到追蹤周期內(nèi)各位置下發(fā)周期的下發(fā)位置，使得拖動(dòng)機(jī)器人多軸機(jī)構(gòu)各軸關(guān)節(jié)移動(dòng)的過(guò)程中，精度與靈敏度都有所提高。同時(shí)在獲取的多軸機(jī)構(gòu)各軸關(guān)節(jié)的追蹤速度與各軸關(guān)節(jié)的加速度后，對(duì)超出限制關(guān)節(jié)速度與限制關(guān)節(jié)加速度的軸關(guān)節(jié)，進(jìn)行修正，有利于在拖動(dòng)機(jī)器人多軸機(jī)構(gòu)各軸關(guān)節(jié)移動(dòng)的過(guò)程中，有效的規(guī)避奇異位置。

上面各種方法的步驟劃分，只是為了描述清楚，實(shí)現(xiàn)時(shí)可以合并為一個(gè)步驟或者對(duì)某些步驟進(jìn)行拆分，分解為多個(gè)步驟，只要包含相同的邏輯關(guān)系，都在本專利的保護(hù)范圍內(nèi)；對(duì)算法中或者流程中添加無(wú)關(guān)緊要的修改或者引入無(wú)關(guān)緊要的設(shè)計(jì)，但不改變其算法和流程的核心設(shè)計(jì)都在該專利的保護(hù)范圍內(nèi)。

本發(fā)明第三實(shí)施方式涉及一種機(jī)器人控制裝置，如圖3所示，機(jī)器人控制裝置300包括設(shè)置模塊301、獲取模塊302、位置下發(fā)模塊303以及追蹤速度修正子模塊304。

設(shè)置模塊301用于設(shè)置追蹤周期。具體地說(shuō)，設(shè)置模塊301可以預(yù)先設(shè)置追蹤周期，其中，追蹤周期為獲取的力以及力矩值的更新周期。

獲取模塊302用于在追蹤周期內(nèi)采集力傳感器信息，根據(jù)采集的力傳感器信息獲取機(jī)器人多軸機(jī)構(gòu)各軸關(guān)節(jié)的追蹤位置，其中，追蹤位置為多軸機(jī)構(gòu)各軸關(guān)節(jié)在下一追蹤周期的位置。具體地說(shuō)，多軸機(jī)構(gòu)的法蘭工裝末端上可以安裝6維力傳感器，獲取模塊302可以在追蹤周期內(nèi)多次采集6維力傳感器信息，同時(shí)，對(duì)采集的力傳感器信息進(jìn)行濾波處理，從而，得到需要更新的力以及力矩值。獲取模塊302在追蹤周期內(nèi)更新上述力以及力矩值后，可以根據(jù)更新后的力以及力矩值獲取多軸機(jī)構(gòu)的法蘭工裝末端的速度，進(jìn)而，根據(jù)上述獲取的法蘭工裝末端速度可以獲取機(jī)器人多軸機(jī)構(gòu)各軸關(guān)節(jié)的追蹤位置。

位置下發(fā)模塊303用于記錄追蹤周期與追蹤位置，在追蹤周期內(nèi)對(duì)追蹤位置進(jìn)行軌跡規(guī)劃并下發(fā)至伺服控制系統(tǒng)，控制多軸機(jī)構(gòu)各軸關(guān)節(jié)移動(dòng)至追蹤位置。具體地說(shuō)，在獲取模塊302獲取多軸機(jī)構(gòu)各關(guān)節(jié)軸的追蹤位置后，位置下發(fā)模塊303可以記錄上述追蹤位置以及追蹤周期，并且可以根據(jù)上述追蹤位置以及預(yù)設(shè)的位置下發(fā)周期，在追蹤周期內(nèi)對(duì)多軸機(jī)構(gòu)各軸關(guān)節(jié)進(jìn)行軌跡規(guī)劃，得到追蹤周期內(nèi)各位置下發(fā)周期的下發(fā)位置，同時(shí)，可以將各下發(fā)位置下發(fā)至伺服控制系統(tǒng)，控制多軸機(jī)構(gòu)各軸關(guān)節(jié)在各位置下發(fā)周期內(nèi)移動(dòng)至各位置下發(fā)周期的下發(fā)位置，并最終在追蹤周期內(nèi)移動(dòng)至追蹤位置。同時(shí)，位置下發(fā)模塊303還可以記錄各位置下發(fā)周期的下發(fā)位置，這樣，機(jī)器人多軸機(jī)構(gòu)各軸關(guān)節(jié)可以復(fù)現(xiàn)上述規(guī)劃軌跡。

追蹤速度修正子模塊304用于當(dāng)多軸機(jī)構(gòu)各軸關(guān)節(jié)的追蹤速度中，存在超出預(yù)設(shè)的限制關(guān)節(jié)速度的追蹤速度時(shí)，對(duì)追蹤速度超出限制關(guān)節(jié)速度的軸關(guān)節(jié)，進(jìn)行速度修正，將限制關(guān)節(jié)速度作為修正后的追蹤速度，并且其余各軸關(guān)節(jié)的追蹤速度按相同比例降低。

獲取模塊302還可以包括法蘭工裝末端速度獲取子模塊3021、追蹤速度獲取子模塊3022以及追蹤位置獲取子模塊3023。

法蘭工裝末端速度獲取子模塊3021用于在追蹤周期內(nèi)采集力傳感器信息，根據(jù)采集的力傳感器信息獲取多軸機(jī)構(gòu)的法蘭工裝末端速度。具體地說(shuō)，多軸機(jī)構(gòu)的法蘭工裝末端上可以安裝6維力傳感器，法蘭工裝末端速度獲取子模塊3021通過(guò)對(duì)在追蹤周期內(nèi)采集的6維力傳感器信息進(jìn)行濾波處理，可以獲取到需要更新的力以及力矩值，并且，可以根據(jù)上述力以及力矩值獲取多軸機(jī)構(gòu)的法蘭工裝末端速度。

追蹤速度獲取子模塊3022用于根據(jù)多軸機(jī)構(gòu)的法蘭工裝末端速度，獲取多軸機(jī)構(gòu)各軸關(guān)節(jié)的追蹤速度。具體地說(shuō)，追蹤速度獲取子模塊3022可以在追蹤周期內(nèi)，通過(guò)伺服控制系統(tǒng)獲取多軸機(jī)構(gòu)各軸關(guān)節(jié)當(dāng)前的位置以及與上述位置相關(guān)的雅可比矩陣。進(jìn)而，可以根據(jù)多軸機(jī)構(gòu)的法蘭工裝末端速度與雅克比矩陣，獲取多軸機(jī)構(gòu)各軸關(guān)節(jié)的追蹤速度。

追蹤位置獲取子模塊3023用于根據(jù)多軸機(jī)構(gòu)各軸關(guān)節(jié)的追蹤速度，獲取多軸機(jī)構(gòu)各軸關(guān)節(jié)的追蹤位置。具體地說(shuō)，追蹤速度獲取子模塊3022還可以在追蹤周期內(nèi)，通過(guò)伺服控制系統(tǒng)獲取多軸機(jī)構(gòu)各軸關(guān)節(jié)當(dāng)前的速度。追蹤位置獲取子模塊3023可以根據(jù)多軸機(jī)構(gòu)各軸關(guān)節(jié)當(dāng)前的速度以及多軸機(jī)構(gòu)各軸關(guān)節(jié)的追蹤速度，獲取多軸機(jī)構(gòu)各軸關(guān)節(jié)的加速度。進(jìn)而，可以根據(jù)多軸機(jī)構(gòu)各軸關(guān)節(jié)當(dāng)前的速度、多軸機(jī)構(gòu)各軸關(guān)節(jié)的加速度以及追蹤速度，獲取多軸機(jī)構(gòu)各軸關(guān)節(jié)的追蹤位置。

本發(fā)明實(shí)施方式中，根據(jù)在追蹤周期內(nèi)采集的力傳感器信息，可以獲取多軸機(jī)構(gòu)各軸關(guān)節(jié)的追蹤速度與各軸關(guān)節(jié)的加速度，并由此可以獲取到多軸機(jī)構(gòu)各軸關(guān)節(jié)的追蹤位置。這樣，可以使得獲取的多軸機(jī)構(gòu)各軸關(guān)節(jié)的追蹤位置更準(zhǔn)確，且根據(jù)上述追蹤位置在追蹤周期內(nèi)對(duì)多軸機(jī)構(gòu)各軸關(guān)節(jié)進(jìn)行軌跡規(guī)劃，得到追蹤周期內(nèi)各位置下發(fā)周期的下發(fā)位置，使得拖動(dòng)機(jī)器人多軸機(jī)構(gòu)各軸關(guān)節(jié)移動(dòng)的過(guò)程中，精度與靈敏度都有所提高。同時(shí)在獲取的多軸機(jī)構(gòu)各軸關(guān)節(jié)的追蹤速度與各軸關(guān)節(jié)的加速度后，對(duì)超出限制關(guān)節(jié)速度與限制關(guān)節(jié)加速度的軸關(guān)節(jié)，進(jìn)行修正，有利于在拖動(dòng)機(jī)器人多軸機(jī)構(gòu)各軸關(guān)節(jié)移動(dòng)的過(guò)程中，有效的規(guī)避奇異位置。

不難發(fā)現(xiàn)，本實(shí)施方式為與第二實(shí)施方式相對(duì)應(yīng)的裝置實(shí)施例，本實(shí)施方式可與第二實(shí)施方式互相配合實(shí)施。第二實(shí)施方式中提到的相關(guān)技術(shù)細(xì)節(jié)在本實(shí)施方式中依然有效，為了減少重復(fù)，這里不再贅述。相應(yīng)地，本實(shí)施方式中提到的相關(guān)技術(shù)細(xì)節(jié)也可應(yīng)用在第二實(shí)施方式中。

值得一提的是，本實(shí)施方式中所涉及到的各模塊均為邏輯模塊，在實(shí)際應(yīng)用中，一個(gè)邏輯單元可以是一個(gè)物理單元，也可以是一個(gè)物理單元的一部分，還可以以多個(gè)物理單元的組合實(shí)現(xiàn)。此外，為了突出本發(fā)明的創(chuàng)新部分，本實(shí)施方式中并沒(méi)有將與解決本發(fā)明所提出的技術(shù)問(wèn)題關(guān)系不太密切的單元引入，但這并不表明本實(shí)施方式中不存在其它的單元。

本領(lǐng)域技術(shù)人員可以理解實(shí)現(xiàn)上述實(shí)施例方法中的全部或部分步驟是可以通過(guò)程序來(lái)指令相關(guān)的硬件來(lái)完成，該程序存儲(chǔ)在一個(gè)存儲(chǔ)介質(zhì)中，包括若干指令用以使得一個(gè)設(shè)備(可以是單片機(jī)，芯片等)或處理器(processor)執(zhí)行本申請(qǐng)各個(gè)實(shí)施例所述方法的全部或部分步驟。而前述的存儲(chǔ)介質(zhì)包括：U盤、移動(dòng)硬盤、只讀存儲(chǔ)器(ROM，Read-Only Memory)、隨機(jī)存取存儲(chǔ)器(RAM，Random Access Memory)、磁碟或者光盤等各種可以存儲(chǔ)程序代碼的介質(zhì)。

本領(lǐng)域的普通技術(shù)人員可以理解，上述各實(shí)施方式是實(shí)現(xiàn)本發(fā)明的具體實(shí)施例，而在實(shí)際應(yīng)用中，可以在形式上和細(xì)節(jié)上對(duì)其作各種改變，而不偏離本發(fā)明的精神和范圍。