基于正弦加減速控制算法的工業機器人位姿軌跡規劃研究

黃忠明，陳愛文，黃鳳良

（1.南京師范大學 電氣與自動化工程學院，南京 210042；2.揚州職業大學 電子工程學院，揚州 225009）

0 引言

位姿軌跡規劃是通過給出有限個示教點規劃得到笛卡爾空間下的目標軌跡，是實現機器人高效、準確、平穩地執行作業任務、減少繁重的機器人示教工作的重要手段。位姿軌跡規劃包括位置軌跡規劃和姿態軌跡規劃，二種軌跡規劃一般分開進行，也有有一些學者試圖將二者同時規劃，但不管如何規劃，只有位置軌跡與姿態軌跡同時光滑連續且同步運行，才能保證機器人末端執行器的任務軌跡連續光滑。位姿軌跡在啟停階段會存在速度階躍的情況，這會對機器人機械本體造成沖擊，關節上的驅動器無法補償如此巨大的力而導致電流過載，同時，機器人本體的振動和沖擊會影響工作質量、造成本體磨損影響機器人壽命[1]。杜亮、張鐵[2]采用位姿分離的方式，先插補機器人的位置點，再在各位置點上以歐拉角的方式插補機器人姿態，這種方法簡單方便，實現了位姿軌跡同步規劃，提高了機器人控制的實時性，但無法實現啟停階段位置軌跡和姿態軌跡的（角）速度與（角）加速度的連續性。宋金華[3]采用7段S曲線加減速算法與三次樣條插值算法結合的方法，根據速度變化將軌跡分段，每段軌跡均采用三次樣條插值，并以此作為插值曲線來規劃末端執行器的直線軌跡和圓弧軌跡，但7段S曲線加減速算法速度分段較多、算法復雜，且未提及位置規劃過程中的姿態處理方法。陳偉華、張鐵[4]研究了三次均勻有理B樣條在自由曲線軌跡中的應用，曲線插補過程中同樣進行了加減速控制，但其采用的線性加減速控制并不能使運動加速度連續。

本文采用正弦加減速控制算法，以笛卡爾空間下的直線軌跡為目標位置軌跡，單位四元數表示姿態，分別規劃位置軌跡和姿態軌跡，并對兩種軌跡進行位姿同步規劃處理，獲得位置和姿態軌跡均連續光滑且同步變化的位姿軌跡，以解決啟停階段（角）速度和（角）加速度不連續的問題。

1 位姿軌跡規劃算法研究

1.1 位置軌跡規劃

設機器人末端執行器從Ps點沿直線運動到Pe點，起始點Ps坐標為（xs，ys，zs），終點Pe坐標為（xe，ye，ze），采用定時插補的方式，設插補周期為Δt，運行速度為v，從起點到第i個插補點的位移可表示為si=iΔs=ivΔt。將直線PsPe表示為關于是時間t的參數方程的形式，則第i個插補點的X，Y，Z坐標分別為：

式中，Δx=xe-xs，Δy=ye-ys，Δz=ze-zs，。（xi，yi，zi）即為第i個插補周期t=iΔt時刻末端執行器的三維坐標。

本文采用正弦加減速控制算法對運動過程進行加減速分段，原理如下：

1）加速階段：加速度a隨時間按正弦規律變化，從0變化到最大值，再變化為0。整個加速階段，加速度的值始終位于y坐標軸的正半軸，而速度也會在此階段從0達到最大值，即勻速階段速度cm。

2）勻速階段：以恒定速度vm運行至減速階段，該階段也是機器人末端執行器運行的主要階段。

3）減速階段：加速度a隨時間按正弦規律變化，從0變化到最小值，再變化為0。整個加速階段，加速度的值始終位于y坐標軸的負半軸，而速度也會在勻速階段速度vm減小到0，此時機器人末端執行器走完預設軌跡，到達目標位置，運行結束。

給定若干個示教點，設定最大速度vm，最大加速度am，則整個過程加速度的表達式為：

由式(2)積分得到速度表達式為：

由式(3)積分得到位移表達式為：

在不考慮關節空間下對笛卡爾空間預設軌跡進一步細分的情況下，關節驅動器驅動各關節運動的進給量就是一個插補周期內機器人末端執行器沿X，Y，Z三個方向上的變化量對應的關節角變化量，由式(1)及si=iΔs可知，的大小與運行完第i個插補周期t的位移si相關，因此，直線段的加減速劃分考察的是直線沿X，Y，Z三個方向的位移s隨運行時間t和運行速度v的變化情況。設機器人運行完第i個插補周期t的位移為si，勻速段速度為vc，最大加速度為am，則由式(4)得加速、勻速和減速三個階段對應的位移si分別為：

加速階段（0＜t≤t1）：

勻速階段（t1＜t≤t2）：

減速階段（t2＜t≤Tpos）：

各參數確定后，需校驗勻速段速度vc是否“可達”，即在給定的加速度下機器人末端執行器可以經歷加速、勻速和減速的完整過程，當整個運動過程處于臨界狀態時（末端執行器加速到速度最大值時直接進入減速階段），t1=Tpos-t1=t2。已知加速度段的位移為：

當vc≤vcmax時，勻速段速度vc“可達”，即運動過程有勻速段，否則需要重新設定最大運行速度vc或者最大加速度am。

1.2 姿態軌跡規劃

四元數表示的兩姿態間的軌跡規劃通常采用單位四元數球面線性插值的方法，設末端執行器的起始Ps和結束點Pe對應姿態的單位四元數分別表示為Q0和Q1，二者間的角位移為，總運動時間為Tort，插補周期為Δtt，則球面插值曲線為：

式中，Slerp函數的返回值Qu表示末端執行器在起始姿態和結束姿態間的一個中間姿態，0≤u≤1，u的取值為，其中n=Tort/Δt。

四元數球面線性插值雖然可以實現兩姿態間的軌跡插值，但在啟停階段存在姿態角速度不連續的情況，因此論文同樣采用正弦加減速控制算法，對姿態角的變化過程進行加減速控制。設定最大角速度wc，最大角加速度am，參照正弦加減速控制算法在位置軌跡規劃中的應用，得不同階段兩姿態間角位移θ隨時間的變化關系為：

速度規劃的本質是控制角位移的變化，即對球面線性插值公式slerp函數中的u的控制。設兩姿態間的角位移為，使代入slerp，得到隨插補周期t變化的四元數序列，即起始點姿態向結束點姿態變化過程中的姿態點序列，其余參數的確定方法與位置軌跡中的確定方法相同。最后將四元數序列轉化為姿態矩陣序列[5,6]。

1.3 位姿同步規劃

機器人運行過程中末端執行器的位置和姿態一般是同步變化的的，以保證末端執行器平穩運行，因此需要對位置軌跡和姿態軌跡進行同步規劃處理。設位置軌跡運行總時間和姿態軌跡運行總時間分別為Tpos和Tort，當Tpos≠Tort時，通過調整位置軌跡或姿態軌跡中一方的最大速度或最大角速度值，重新規劃位置或姿態軌跡，使兩個軌跡的運行時間相等。根據Tpos和Tort的大小關系分三種情況進行同步處理：

1）Tpos=Tort：位置軌跡與姿態軌跡運行時間相等，滿足位姿同步的要求。

2）Tpos＞Tort：為之軌跡運行時間大于姿態軌跡運行時間，此時增加Tort，使Tort=Tpos，計算相應的勻速段角速度wc，重新規劃姿態軌跡。

3）Tpos＜Tort：位置軌跡運行時間小于姿態軌跡運行時間，此時增加Tpos，使Tpos=Tort，計算相應的勻速段速度vc或角速度wc，重新規劃位置軌跡。

2 仿真結果與分析

以Cyton Gamma300七自由度機器人為實驗對象，取末端執行器的兩個示教點分別作為起始點和終止點，對應的起始點位姿矩陣Ts和終止點位姿矩陣Te分別為：

在MATALB環境下驗證上述基于正弦加減速控制算法的位姿軌跡規劃算法。

位姿矩陣中位置坐標的單位為毫米（mm），姿態的單位為弧度（rad），以起始點為例，機器人末端執行器的位置和姿態在矩陣中分別對應為：

設定機器人末端執行的最大運行速度為300mm/s，最大運行加速度為500mm/s2，最大姿態角速度為0.3rad/s，最大姿態角加速度為0.5rad/s2，插補周期為2ms，經本文的位姿軌跡規劃算法得到機器人的不同視角下的位姿軌跡如圖1所示。

軌跡中位移、速度及加速度隨時間變化的關系如圖2所示。

圖1 直線位姿軌跡

圖2 直線位置軌跡位移、速度及加速度變化

由圖2可知，末端執行器在直線運動啟停加減速階段、勻速階段的變化均符合足正弦加減速控制策略的控制規律，整個過程位移、速度及加速度連續均連續變化，符合位置規劃算法的預期目標。從圖2中可以看出，直線位置軌跡最大運行速度并不是設定的0.3m/s，這是因為在進行位姿同步處理時姿態軌跡運行時間大于位置軌跡運行時間，此時同步處理算法通過改變最大運行段速度重新對位置軌跡進行插補，從而使位置軌跡運行時間和姿態軌跡運行時間相等，達到位姿同步的目的。

直線軌跡中還包含了姿態軌跡的變化，姿態的角位移、角速度及角加速度雖時間的變化關系如圖3所示。

圖3 姿態軌跡位移、速度及加速度變化

從圖3可知，姿態軌跡規劃算法可以實現啟停階段姿態角速度和角加速度變化的連續性，符合姿態軌跡規劃的預期目標。同時從圖3中可以看出，姿態軌跡的運行時間為2.85秒，與圖2中所示的位置軌跡運行時間相等，驗證了位姿同步處理算法的可行性。

3 結論

本文提出了基于正弦加減速控制算法的工業機器人位姿軌跡規劃算法，通過將正弦加減速控制策略應用到機器人末端執行器的位置軌跡和姿態軌跡的同步規劃中，使其在啟動階段、勻速運行階段及停止階段間平穩光滑過渡。仿真結果表明，啟停階段末端執行器的（角）速度和（角）加速度連續變化，整個運行軌跡連續、平滑。