機器人運動學參數誤差優化補償方法及裝置與流程
2023-06-30 23:35:01
本發明涉及機器人參數校正技術領域，具體而言，涉及一種機器人運動學參數誤差優化補償方法及裝置。
背景技術：
隨著技術的發展，工業機器人的應用逐漸普及。目前，工業機器人的重複精度相對比較高，但定位精度卻相對比較差。因此在很多應用中需要對機器人的相關參數進行標定。所謂標定是指運用相關的測量手段和適當的參數識別方法辨識出機器人模型的準確參數。
機器人運動學標定是比較常用的機器人參數標定方法，該方法通過建模、測量、辨識和補償等步驟對機器人的運動學參數進行重新辨識和校正，以減小機器人在生產、裝配和使用過程中產生的幾何參數誤差，從而達到提高機器人絕對定位精度的目的。
在對機器人運動學參數標定的過程中，如何將辨識出的參數誤差補償進機器人控制器是一個很重要的問題。發明在研究中發現，現有的將辨識出的參數誤差補償進機器人的方式存在導致標定失敗的概率比較大，可能不僅無法提高機器人的定位精度，而且有可能使機器人運動學逆解析失敗。
技術實現要素：
本發明的目的在於提供機器人運動學參數誤差優化補償方法及裝置，以解決運動學參數補償後導致的運動學逆解解析失敗的問題。
第一方面，本發明實施例提供了一種機器人運動學參數誤差優化補償方法，包括：
根據機器人標定前和標定後的運動學參數，分別構建機器人運動學名義值模型和機器人運動學標定值模型，其中，在所述機器人運動學名義值模型和機器人運動學標定值模型中，採用末端執行器坐標系原點的位姿向量表示末端執行器坐標系與基坐標系之間的位姿轉換關係；
在預設關節空間位置下，根據所述運動學標定值模型計算末端執行器坐標系原點的位姿；
構建參數補償約束優化模型，其中，所述參數補償約束優化模型用於約束根據運動學標定值模型計算得到的末端執行器坐標系原點的位姿與預設目標位姿之間的差值最小；
求解所述參數補償約束優化模型，得到經過優化補償後的關節空間坐標值；
將所述經過優化補償後的關節空間坐標值替代由所述機器人運動學名義值模型進行運動學逆計算得到的關節空間坐標。
結合第一方面，在第一方面的第一種可能的實施方式中，所述在預設關節空間位置下，根據所述運動學標定值模型計算末端執行器坐標系原點的位姿，包括：
根據所述運動學標定值模型，計算所述預設關節空間位置對應的末端執行器坐標系在笛卡爾空間中相對於基坐標系的齊次變換矩陣；
根據所述齊次變換矩陣，確定所述末端執行器坐標系原點的位姿。
結合第一方面或第一方面的第一種可能的實施方式，在本發明實施例的第二種可能的實施方式中，所述參數補償約束優化模型為：
minω(θ)＝||Pn-fc(θ)||2
其中，ω(θ)表示位姿誤差向量的歐氏範數；Pn表示所述末端執行器坐標系轉換矩陣計算得到的指定位姿向量[px py pz α β γ]T；fc(θ)表示根據標定值模型和關節空間位置θ計算得到的實際位姿向量；和分別表示關節i的最小限位和最大限位，在移動關節中，該約束表示為關節在軸向上能夠移動的最大位移。
結合第一方面的第二種可能的實施方式，在第一方面的第三種可能的實施方式中，所述求解所述參數補償約束優化模型，得到經過優化補償後的關節空間坐標值，包括：
採用內點法求解所述參數補償約束優化模型，得到經過優化補償後的關節空間坐標值。
結合第一方面的第三種可能的實施方式，在第一方面的第四種可能的實施方式中，所述採用內點法求解所述參數補償約束優化模型，得到經過優化補償後的關節空間坐標值，包括：
根據所述參數補償約束優化模型構建無約束目標函數：
式中，r(k)是懲罰因子，是一個遞減的正數序列，即r(0)＞r(1)＞...r(k)，且有gi(θ)是所述參數補償約束優化模型中的約束條件；
對所述無約束目標函數進行迭代，得到所述經過優化補償後的關節空間坐標值。
第二方面，本發明實施例提供了一種機器人運動學參數誤差優化補償裝置，包括：
第一模型構建單元，用於根據機器人標定前和標定後的運動學參數，分別構建機器人運動學名義值模型和機器人運動學標定值模型，其中，在所述機器人運動學名義值模型和機器人運動學標定值模型中，採用末端執行器坐標系原點的位姿向量表示末端執行器坐標系與基坐標系之間的位姿轉換關係；
第一計算單元，用於在預設關節空間位置下，根據所述運動學標定值模型計算末端執行器坐標系原點的位姿；
第二模型構建單元，用於構建參數補償約束優化模型，其中，所述參數補償約束優化模型用於約束根據運動學標定值模型計算得到的末端執行器坐標系原點的位姿與預設目標位姿之間的差值最小；
第二計算單元，用於求解所述參數補償約束優化模型，得到經過優化補償後的關節空間坐標值；
補償單元，用於將所述經過優化補償後的關節空間坐標值替代由所述機器人運動學名義值模型進行運動學逆計算得到的關節空間坐標。
結合第二方面，在第二方面的第一種可能的實施方式中，所述第一計算單元在預設關節空間位置下，根據所述運動學標定值模型計算末端執行器坐標系原點的位姿，具體包括執行：
根據所述運動學標定值模型，計算所述預設關節空間位置對應的末端執行器坐標系在笛卡爾空間中相對於基坐標系的齊次變換矩陣；
根據所述齊次變換矩陣，確定所述末端執行器坐標系原點的位姿。
結合第二方面或者第二方面的第一種可能的實施方式，在第二方面的第二種可能的實施方式中，所述參數補償約束優化模型為：
minω(θ)＝||Pn-fc(θ)||2
其中，ω(θ)表示位姿誤差向量的歐氏範數；Pn表示所述末端執行器坐標系轉換矩陣計算得到的指定位姿向量[px py pz α β γ]T；fc(θ)表示根據標定值模型和關節空間位置θ計算得到的實際位姿向量；和分別表示關節i的最小限位和最大限位，在移動關節中，該約束表示為關節在軸向上能夠移動的最大位移。
結合第二方面的第二種可能的實施方式，在第二方面的第三種可能的實施方式中，所述第二計算單元求解所述參數補償約束優化模型，得到經過優化補償後的關節空間坐標值，具體包括執行：
採用內點法求解所述參數補償約束優化模型，得到經過優化補償後的關節空間坐標值。
結合第二方面的第三種可能的實施方式，在第二方面的第四種可能的實施方式中，所述補償單元採用內點法求解所述參數補償約束優化模型，得到經過優化補償後的關節空間坐標值，具體包括執行：
根據所述參數補償約束優化模型構建無約束目標函數：
式中，r(k)是懲罰因子，是一個遞減的正數序列，即r(0)＞r(1)＞...r(k)，且有gi(θ)是所述參數補償約束優化模型中的約束條件；
對所述無約束目標函數進行迭代，得到所述經過優化補償後的關節空間坐標值。
在本發明實施例方案中，利用機器人標定前和標定後的運動學參數分別構建機器人運動學名義值模型和標定值模型，並且在預設關節空間位置下，基於運動學標定值模型計算末端執行器坐標系原點的位姿，之後利用參數補償約束優化模型使上述計算得到的末端執行器坐標系原點的位姿與預設目標位姿之間的差值最小，並且通過求解上述參數補償約束優化模型得到經過優化補償後的關節空間坐標值；得到經過優化補償後的關節空間坐標值後，將經過優化補償後的關節空間坐標值替換由所述機器人運動學名義值模型進行運動學逆計算得到的關節空間坐標。本發明實施例方案，標定方式簡單，而且能夠解決運動學參數補償後導致的運動學逆解解析失敗的問題。
為使本發明的上述目的、特徵和優點能更明顯易懂，下文特舉較佳實施例，並配合所附附圖，作詳細說明如下。
附圖說明
為了更清楚地說明本發明實施例的技術方案，下面將對實施例中所需要使用的附圖作簡單地介紹，應當理解，以下附圖僅示出了本發明的某些實施例，因此不應被看作是對範圍的限定，對於本領域普通技術人員來講，在不付出創造性勞動的前提下，還可以根據這些附圖獲得其他相關的附圖。
圖1是本發明實施例機器人運動學參數誤差優化補償方法的流程圖；
圖2是本發明實施例構建機器人運動學模型的效果示意圖；
圖3是本發明實施例機器人運動學參數誤差優化補償裝置的結構示意圖。
具體實施方式
下面將結合本發明實施例中附圖，對本發明實施例中的技術方案進行清楚、完整地描述，顯然，所描述的實施例僅僅是本發明一部分實施例，而不是全部的實施例。通常在此處附圖中描述和示出的本發明實施例的組件可以以各種不同的配置來布置和設計。因此，以下對在附圖中提供的本發明的實施例的詳細描述並非旨在限制要求保護的本發明的範圍，而是僅僅表示本發明的選定實施例。基於本發明的實施例，本領域技術人員在沒有做出創造性勞動的前提下所獲得的所有其他實施例，都屬於本發明保護的範圍。
現有技術中，將辨識出的參數誤差補償進入控制器的過程通常是通過修改機器人控制器中的機器人模型來完成，機器人模型修改的成功與否直接決定了運動學標定的成敗。隨著機器人普及化程度的提高，大多數非專業用戶並不擁有機器人控制器的修改權限，因而難以將辨識出的補償參數寫入機器人控制器，這使得參數辨識的結果無法應用到實際操作中，也就無法實現機器人絕對定位精度的提高。
另一方面，即使用戶擁有機器人控制器的修改權限，如果經過標定的運動學參數與名義值不符，例如經過標定的機器人腕關節三個軸坐標系的運動學參數與名義值不符，可能會造成補償後的機器人模型中的腕關節三個軸線不再相交於一點，由此會導致機器人運動學逆解的解析失敗。
為了解決現有技術中存在的運動學參數補償後導致的運動學逆解解析失敗的問題，本發明實施例提供了一種機器人運動學參數誤差優化補償方法。
圖1是本發明實施例機器人運動學參數誤差優化補償方法的流程圖。圖1所示方法的執行主體為機器人中的控制器，所述方法包括：
步驟S101：根據機器人標定前和標定後的運動學參數，分別構建機器人運動學名義值模型和機器人運動學標定值模型。
在所述機器人運動學名義值模型和機器人運動學標定值模型中，採用末端執行器坐標系原點的位姿向量表示末端執行器坐標系與基坐標系之間的位姿轉換關係。
在本發明實施例方案中，採用D-H參數構建上述的機器人運動學名義值模型和機器人運動學標定值模型。經典D-H參數常用於描述機器人運動學模型，是機器人連杆和關節建模的一種簡單方法，可用於任何機器人構型。
本發明實施例方案中，採用D-H參數構建機器人運動學模型的方法包括：
(1)如圖2所示，在機器人關節處以關節軸線為Z軸，以與該關節連接的連杆起點為坐標原點，建立每個關節的坐標系，其中，建立的每個關節的坐標系稱為基坐標系。
(2)採用一個由機器人幾何結構參數構造的4×4的齊次變換矩陣描述相鄰兩連杆的空間轉換關係。
(3)根據相鄰關節坐標系轉換推導出末端執行器坐標系相對於基坐標系的等價齊次變換矩陣。
在規定了全部連杆的坐標系後，就可以確定用於表示坐標系i-1和坐標系i之間相對位置和方位關係的4個參數，即杆件長度ai-1，杆件扭角ɑi-1，關節距離di和關節轉角θi，其中，杆件長度ai-1，杆件扭角ɑi-1，關節距離di和關節轉角θi稱為經典D-H參數。利用基本的齊次轉動和齊次平移變換矩陣，可以得到相鄰坐標系i-1和i的齊次變換矩陣i-1iT，即：
其中cɑi-1表示cos(ɑi-1)，sɑi-1表示sin(ɑi-1)，cθi表示cos(θi)，sθi表示sin(θi)。
因而，機器人基坐標繫到末端執行器坐標系的齊次變換矩陣可以表示為：
其中n，o，a和p分別表示齊次變換矩陣中的元素。據此，可分別利用標定前運動學參數的名義值和標定後的標定值，建立機器人運動學名義值模型Rn和標定值模型Rc。其中名義值模型表示在控制器中末端坐標系相對於基坐標系的轉換關係，其腕關節三個坐標系的軸線相交於一點，可根據指定末端執行器坐標系位姿利用運動學逆解得到對應的關節空間位置的解析解；而在經過運動學參數標定後的標定值模型表示經過標定後末端坐標系相對於基坐標系的準確轉換關係，其杆件扭角不一定是0或90°，造成腕關節三個軸不一定相交，因而無法通過解析解得到其逆解。
根據RPY角方法將齊次變換矩陣轉化為末端執行器坐標系原點在基坐標系中的位姿P，則x，y和z坐標分別為px，py和pz，而末端執行器坐標系繞基坐標系坐標軸x，y，z旋轉的偏轉角ɑ，俯仰角β和橫滾角γ可分別計算為：
至此，可以使用末端執行器坐標系原點的位姿向量Pn＝[px py pz α β γ]T來表示末端執行器坐標系與基坐標系之間的位姿轉換關係。
步驟S102：在預設關節空間位置下，根據所述運動學標定值模型計算末端執行器坐標系原點的位姿。
其中，具體實施時，機器人控制器根據所述運動學標定值模型，計算所述預設關節空間位置對應的末端執行器坐標系在笛卡爾空間中相對於基坐標系的齊次變換矩陣；根據所述齊次變換矩陣，確定所述末端執行器坐標系原點的位姿。
步驟S103：構建參數補償約束優化模型，其中，所述參數補償約束優化模型用於約束根據運動學標定值模型計算得到的末端執行器坐標系原點的位姿與預設目標位姿之間的差值最小。
在本發明實施例方案中，構建參數補償約束優化模型的步驟包括：
(1)由第一步得到的標定值模型Rc，在給定關節空間位置的前提下，可以計算該關節位置對應的末端執行器坐標系在笛卡爾空間中相對於基坐標系的齊次變換矩陣，進而求得末端執行器坐標系原點的位姿。在此約束優化問題中，標定值模型中的關節位置作為待優化變量，其優化目標是使由標定值模型計算得到的末端執行器坐標系原點的位姿儘可能地接近指定的目標位姿，即最小化位姿誤差向量的歐氏範數。
據此可構建參數補償約束優化模型：
minω(θ)＝||Pn-fc(θ)||2
其中，ω(θ)表示位姿誤差向量的歐氏範數，即誤差向量內積的平方根；Pn表示根據指定的末端執行器坐標系轉換矩陣計算得到的指定位姿向量[px py pz α β γ]T；fc(θ)表示根據標定值模型和關節空間位置θ計算得到的實際位姿向量；和分別表示關節i的最小限位和最大限位，在移動關節中，該約束表示為關節在軸向上能夠移動的最大位移。
步驟S104：求解所述參數補償約束優化模型，得到經過優化補償後的關節空間坐標值。
在本發明實施例中，可以採用內點法求解所述參數補償約束優化模型，得到經過優化補償後的關節空間坐標值。其中，內點法具有計算速度快，算法比較成熟，結果可信度高等優點。
具體的，採用內點法求解所述參數補償約束優化模型得到經過優化補償後的關節空間坐標值的步驟包括：
一、根據上述參數補償約束優化模型構造一個新的無約束目標函數，即懲罰函數
式中，r(k)是懲罰因子，是一個遞減的正數序列，即r(0)＞r(1)＞...r(k)，且有懲罰因子可以大幅提高模型收斂的速度；gi(θ)是原優化模型中關於關節空間位置的12個約束條件，即等，均為小於或等於0的不等式。
二、對懲罰函數進行迭代，在可行域內逐步逼近原約束優化問題的最優解，具體的包括：
(1)取初始懲罰因子r(0)＞0，允許誤差ε＞0；
(2)在可行域D內取初始點θ(0)，令k＝1，初始點可由運動學名義值模型根據給定的末端執行器位姿求逆解得到；
(3)從θ(k-1)點出發，用無約束優化方法(如最速下降法或牛頓法)求解懲罰函數的極值點θ*(r(k))；
(4)檢查迭代終止條件，如果滿足
或
||θ*(r(k))-θ*(r(k-1))||≤ε2，
則終止迭代計算，並以θ*(r(k))為原優化函數ω(θ)的約束最優解，否則轉入下一步；
(5)取r(k+1)＝Cr(k)，θ(0)＝θ*(r(k))，k＝k+1，轉入步驟3)。遞減係數C＝0.1-0.5，常取0.1。
經過迭代計算，可得到參數補償約束優化模型的最優解，即經過優化補償後的關節空間坐標值。
步驟S105：將所述經過優化補償後的關節空間坐標值替代由所述機器人運動學名義值模型進行運動學逆計算得到的關節空間坐標。
在本發明實施例方案中，利用標定前後機器人的運動學參數分別構建機器人運動學名義值模型和標定值模型；根據指定的機器人末端執行器位姿和標定值模型，將指定位姿和由標定值模型計算到位姿的差值向量的最小歐氏範數作為優化目標，將各運動關節的關節運動範圍作為約束條件，構建約束優化模型；利用非線性多元函數優化算法對模型進行求解，得到補償後的關節空間坐標值，將該組值替代由名義值模型的運動學逆解計算得到的關節空間坐標並由控制器操作機器人運動。
本發明實施例方法的機器人運動學參數誤差優化補償方法中，運動學參數補償不需要修改機器人控制器中的原有參數，方便標定；另外，本發明實施例方法解決了機器人運動學參數標定可能造成的腕關節三個軸線不再相交於一個點而導致的逆解解析失效的問題。
圖3是本發明實施例提供的一種機器人運動學參數誤差優化補償裝置。如圖3所示，該裝置包括：第一模型構建單元201、第一計算單元202、第二模型構建單元203、第二計算單元204和補償單元205；其中，
第一模型構建單元201，用於根據機器人標定前和標定後的運動學參數，分別構建機器人運動學名義值模型和機器人運動學標定值模型，其中，在所述機器人運動學名義值模型和機器人運動學標定值模型中，採用末端執行器坐標系原點的位姿向量表示末端執行器坐標系與基坐標系之間的位姿轉換關係；
第一計算單元202，用於在預設關節空間位置下，根據所述運動學標定值模型計算末端執行器坐標系原點的位姿；
第二模型構建單元203，用於構建參數補償約束優化模型，其中，所述參數補償約束優化模型用於約束根據運動學標定值模型計算得到的末端執行器坐標系原點的位姿與預設目標位姿之間的差值最小；
第二計算單元204，用於求解所述參數補償約束優化模型，得到經過優化補償後的關節空間坐標值；
補償單元205，用於將所述經過優化補償後的關節空間坐標值替代由所述機器人運動學名義值模型進行運動學逆計算得到的關節空間坐標。
在一種可能的設計中，所述第一計算單元202在預設關節空間位置下，根據所述運動學標定值模型計算末端執行器坐標系原點的位姿，具體包括執行：
根據所述運動學標定值模型，計算所述預設關節空間位置對應的末端執行器坐標系在笛卡爾空間中相對於基坐標系的齊次變換矩陣；
根據所述齊次變換矩陣，確定所述末端執行器坐標系原點的位姿。
在一種可能的設計中，所述參數補償約束優化模型為：
minω(θ)＝||Pn-fc(θ)||2
其中，ω(θ)表示位姿誤差向量的歐氏範數；Pn表示所述末端執行器坐標系轉換矩陣計算得到的指定位姿向量[px py pz α β γ]T；fc(θ)表示根據標定值模型和關節空間位置θ計算得到的實際位姿向量；和分別表示關節i的最小限位和最大限位，在移動關節中，該約束表示為關節在軸向上能夠移動的最大位移。
在一種可能的設計中，所述第二計算單元204求解所述參數補償約束優化模型，得到經過優化補償後的關節空間坐標值，具體包括執行：
採用內點法求解所述參數補償約束優化模型，得到經過優化補償後的關節空間坐標值。
在一種可能的設計中，所述補償單元205採用內點法求解所述參數補償約束優化模型，得到經過優化補償後的關節空間坐標值，具體包括執行：
根據所述參數補償約束優化模型構建無約束目標函數：
式中，r(k)是懲罰因子，是一個遞減的正數序列，即r(0)＞r(1)＞...r(k)，且有gi(θ)是所述參數補償約束優化模型中的約束條件；
對所述無約束目標函數進行迭代，得到所述經過優化補償後的關節空間坐標值。
本發明實施例所提供的進行機器人運動學參數誤差優化補償方法的電腦程式產品，包括存儲了程序代碼的計算機可讀存儲介質，所述程序代碼包括的指令可用於執行前面方法實施例中所述的方法，具體實現可參見方法實施例，在此不再贅述。
所屬領域的技術人員可以清楚地了解到，為描述的方便和簡潔，上述描述的系統、裝置和單元的具體工作過程，可以參考前述方法實施例中的對應過程，在此不再贅述。
在本申請所提供的幾個實施例中，應該理解到，所揭露的系統、裝置和方法，可以通過其它的方式實現。以上所描述的裝置實施例僅僅是示意性的，例如，所述單元的劃分，僅僅為一種邏輯功能劃分，實際實現時可以有另外的劃分方式，又例如，多個單元或組件可以結合或者可以集成到另一個系統，或一些特徵可以忽略，或不執行。另一點，所顯示或討論的相互之間的耦合或直接耦合或通信連接可以是通過一些通信接口，裝置或單元的間接耦合或通信連接，可以是電性，機械或其它的形式。
所述功能如果以軟體功能單元的形式實現並作為獨立的產品銷售或使用時，可以存儲在一個計算機可讀取存儲介質中。基於這樣的理解，本發明的技術方案本質上或者說對現有技術做出貢獻的部分或者該技術方案的部分可以以軟體產品的形式體現出來，該計算機軟體產品存儲在一個存儲介質中，包括若干指令用以使得一臺計算機設備(可以是個人計算機，伺服器，或者網絡設備等)執行本發明各個實施例所述方法的全部或部分步驟。而前述的存儲介質包括：U盤、移動硬碟、只讀存儲器(ROM，Read-Only Memory)、隨機存取存儲器(RAM，Random Access Memory)、磁碟或者光碟等各種可以存儲程序代碼的介質。
以上所述，僅為本發明的具體實施方式，但本發明的保護範圍並不局限於此，任何熟悉本技術領域的技術人員在本發明揭露的技術範圍內，可輕易想到變化或替換，都應涵蓋在本發明的保護範圍之內。因此，本發明的保護範圍應所述以權利要求的保護範圍為準。