一種考慮末端運動誤差的六自由度機器人軌跡規劃方法與流程
2023-12-09 07:26:56
本發明涉及到一種考慮運動誤差的六自由度機器人軌跡規劃方法，屬於機器人運動控制研究領域，具體涉及通過結合旋量理論、三次樣條插值算法以及粒子群優化算法對機器人末端連續軌跡進行規劃，進而達到滿足追蹤精度要求、提高規劃效率同時獲得光滑運動軌跡的研究目的。
背景技術：
軌跡規劃作為機器人控制研究的基礎，對機器人的綜合運動性能具有重要影響。按規劃空間進行劃分，規劃方法一般包括關節空間規劃和操作空間規劃。關節空間規劃方法是指直接對關節變量進行插值規劃，最終建立關節變量隨時間的變化曲線，但由於無法預知運動過程中末端的軌跡變化情況，只適用於末端點到點的簡單操作任務。對於末端具有連續軌跡的操作任務，則需採用操作空間規劃方法，該方法得到的運動軌跡雖然具有較高的追蹤精度，但無法保證運動平穩性。為了能夠在追蹤末端軌跡的基礎上保證運動的柔順性，一些學者首先將在操作空間內選取能夠保證末端軌跡的關鍵路徑點，然後基於運動學逆解模型計算各關節空間內的軌跡節點，再在關節空間內進行軌跡規劃，從而同時保證了基本末端軌跡和運動平穩性。在該類方法中規劃軌跡僅能夠保證關鍵路徑點處的位置精度，由於關節插值帶來的連續軌跡的追蹤誤差依然無法控制。
在軌跡規劃中，末端關鍵路徑點個數越多越能夠保證追蹤精度，但路徑點越多關節角位置間距越小，過小的間距難以體現插值算法在平滑軌跡方面起到的真實作用，且逆解次數越多，軌跡規劃計算越複雜，從而降低規劃效率。其次插值算法不僅影響著軌跡曲線，其算法關鍵參數(如時間間隔)也對追蹤精度也有一定影響。為了提升規劃效率，本專利基於旋量理論建立了正逆運動學模型，與傳統的D-H模型相比，具有幾何意義清晰、表達無奇異及運算量很少等優點。為獲得平滑的運動，關節空間軌跡規劃採用三次樣條插值算法，該方法規劃得到的關節角速度，角加速度連續。為使追蹤誤差在滿足要求的條件下儘量小，本專利採用粒子群優化算法對軌跡進行優化，能夠通過獲取合理的關鍵路徑點個數N和合適的時間間隔組合達到減小追蹤誤差的目的。
技術實現要素：
本發明旨在提供一種考慮運動誤差的六自由度機器人軌跡規劃方法。該方法通過在末端連續軌跡上等距離取關鍵路徑點，對採用旋量運動學模型逆解得到的各關節角位置進行插值規劃，首先以關鍵點個數為變量將末端跟蹤誤差控制在要求範圍以內，再以各段時間間隔為設計變量，各關節最大角速度、角加速度及角加加速度為約束條件，以追蹤誤差最小為優化目標對軌跡進行優化，從而獲得規劃效率高、跟蹤誤差小且運動平滑的規劃軌跡。
本發明是採用以下技術手段實現的：
S1、基於旋量理論，對機器人模型建立正逆運動學模型。
S2、在末端的連續軌跡上等距取N+1個關鍵路徑點，得到N個軌跡段，N表示軌跡段的數量；並通過運動學逆解模型得到各關節的軌跡節點，採用三次修正樣條曲線進行插值規劃，從而得到各關節角位移、速度、加速度及加加速度隨時間的變化曲線。
S3、在角位移曲線上每隔20ms取點，並通過正運動學模型計算末端位置，建立跟蹤誤差模型，計算各位置點的追蹤誤差，並提取最大軌跡跟蹤誤差max(Em)。Em為軌跡跟蹤誤差，m為
S4、Tn表示第n軌跡段的時間間隔，取Tn＝t,1≤n≤N，按照以上步驟進行規劃軌跡，計算最大跟蹤誤差max(Em)，如果不滿足精度要求max(Em)＜Emax Emax為根據操作任務限定的最大跟蹤誤差，取N+1，再計算誤差，依次循環計算，直到跟蹤誤差滿足條件。時間間隔t與運動精度限制Emax根據具體任務要求確定。
S5、當N確定之後，以各段時間間隔為設計變量，以各關節角速度、加速度及加加速度為約束條件，以跟蹤誤差最小為優化目標得到優化軌跡。
本發明的特點是通過在末端連續軌跡上等距離取關鍵路徑點，對採用旋量運動學模型逆解得到的各關節角位置進行插值規劃，首先以關鍵點個數為變量將末端跟蹤誤差控制在要求範圍以內，再以各段時間間隔為設計變量、各關節最大角速度、角加速度及角加加速度為約束條件，以追蹤誤差最小為優化目標對軌跡進行優化，從而獲得規劃效率高、跟蹤誤差小且運動平滑的規劃軌跡。
附圖說明
圖1機器人末端軌跡規劃流程圖；
圖2某六軸工業機器人參數坐標系；
圖3各關節角速度、角加速度及角加速度曲線圖；
圖4末端追蹤誤差曲線；
具體實施方式
步驟(1)基於旋量理論建立機器人正逆運動學模型
正運動學模型
如圖2所示，已知該機器人在初始狀態第i個關節所在位置矢量ri及旋轉矢量ωi如下：
根據旋量理論，關節間的轉換矩陣表示為指數積形式，
式中表示第i個關節旋量，θi為第i個關節角位移；可由ωi＝[ω1 ω2 ω3]定義為則νi是第i個關節運動的旋轉線速度，νi＝-ωi×ri。
則機器人正運動學模型gst(θ)表示如下：
逆運動學模型
本方法將各關節角的求解轉化為三個Paden-Kahan子問題，由於該機器人末端的位置決定於關節1,2和3，而其位姿決定於關節4,5和6。首先將前三個關節的逆向運動描述為：末端位置向量re繞關節1旋轉-θ1至re1，再繞關節2旋轉-θ2至re2，然後繞關節3旋轉-θ3至r5，則θ1，θ2和θ3分別通過以下三個表達式求出，其中式(5)屬於子問題1，而式(6)和(7)屬於子問題3。
其中re1可以由末端位置向量re＝[x y z]決定；δ2,δ3為定距離，δ2＝||re1-r2||，δ3＝||re-r3||。
其次θ4，θ5和θ6分別通過以下三個表達式求出，其中式(8)屬於子問題2，式(9)屬於子問題1，
其中r04在旋轉軸6上而不在旋轉軸4和5上，取r04＝[0 744 0]；r06不在旋轉軸6上，取r06＝[0 150 860]；
步驟(2)求取各關節軌跡節點並進行插值規劃
將末端連續軌跡曲線定義如下，其姿態保持Ω＝[0 0 1]，且操作任務總時間T不大於1min。
其中0≤α≤360°，為等距離取N+1個關鍵路徑點，取α＝(360n/N)°,n＝0,1,2,…N。
將得到的末端位姿代入逆運動學模型可求得各關節軌跡節點N+1個。本專利採用三次樣條曲線對關節軌跡節點進行插值運算，對某一關節而言，關節軌跡被分為N個子段，第n段(t∈[tn-1,tn])軌跡角位移Sn(t)、角速度S′n(t)、角加速度S″n(t)可表示如下：
其中Tn為第n段時間間隔；θn,分別為時間tn所對應的關節角位移，角速度和角加速度，且可通過邊界條件Sn(tn-1)＝θn-1,Sn(tn)＝θn及S′n(tn)＝S′n+1(tn)計算得到。
步驟(3)建立末端追蹤誤差模型
將規劃得到的各關節角位移曲線上每隔20ms取值，得到M＝T/0.02個關節點組，並通過正運動學模型計算各關節點組對應的末端位姿如下，
其中θ1m…θ6m為第m個關節點組；提取gm中對應的位置矢量Pm。
建立末端誤差模型Em如下，
Em＝||Pm-O||-R (15)
其中O為軌跡圓心的位置矢量，R為連續軌跡上各點曲率半徑，R＝150mm。
步驟(4)確定末端關鍵路徑點個數n
取Tn＝2，對末端軌跡按照步驟2對各關節進行軌跡規劃，再按照步驟3求出末端最大誤差值max(Em)，該任務要求末端誤差滿足以下條件，max(Em)≤Emax＝1mm，若計算結果不滿足任務要求，則增大路徑點個數N，繼續規劃並計算末端誤差，依次循環計算直到滿足末端精度要求，從而確定N＝24。
步驟(5)以末端誤差最小為目標函數優化軌跡
根據步驟(2)求得各關節角速度，角加速度和角加加速度最大值如下，
S′max＝max(|{S′n(t),1≤n≤24}|) (19)
S″max＝max(|{S″n(t),1≤n≤24}|) (20)
S″′max＝max(|{S″′n(t),1≤n≤24}|) (21)
則優化約束條件可表示如下，
其中時間間隔Tn範圍根據具體任務確定，和為第i個關節所允許的最大角速度、角加速度和角加加速度值，具體取值如下，
以Tn,1≤n≤24為設計變量，優化目標函數表示為，
f＝min(Emax) (23)
本專利採用PSO優化算法對以上問題進行優化求解，優化結果如下：
末端最大誤差為0.60044mm，與初始時間間隔下的誤差0.8019mm相比分別減小了25.12％，總時間由48s減小至47.1487s，且各關節角速度、角加速度及角加加速度均遠小於限定值。