七自由度串聯機器人位姿軌跡規劃算法與仿真

陳愛文，黃忠明，黃鳳良

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

0 引言

位姿軌跡規劃化是工業機器人執行作業任務中的一個重要環節，通過給出有限個示教點規劃得到笛卡爾空間下的目標軌跡，為實現機器人高效、準確、平穩地執行作業任務奠定基礎、減少繁重的機器人示教工作。工業機器人的位姿軌跡規劃包括位置軌跡規劃和姿態軌跡規劃，只有位置軌跡與姿態軌跡同時光滑連續且同步運行，才能保證機器人末端執行器的任務軌跡連續光滑。

本文以笛卡爾空間下的直線軌跡為目標位置軌跡，單位四元數表示姿態，結合正弦加減速控制算法，分別規劃位置軌跡和姿態軌跡，并對兩種軌跡進行位姿同步處理，以獲得位置和姿態軌跡均連續光滑且同步變化的位姿軌跡。

1 位姿軌跡規劃算法

機器人末端執行器從起始端到末端平滑過渡，需要對機器人的整個運行過程進行加減速控制，選擇易于實現、曲線平滑性較好的正弦加減速控制算法，實現機器人在各關節位移、速度、加速度、加加速度等的連續光滑，提高機器人運動軌跡的精度和運動的穩定性。

1.1 正弦加減速控制算法

正弦加減速控制算法的速度曲線分為加速度段、勻速度段和減速度段三個階段，其加速度按正弦規律變化，如圖1所示。

圖1 正弦加減速型加速度曲線

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

2）勻速階段（t1＜t≤t2），以恒定速度vm運行至減速階段，此階段加速度為0，軌跡按恒定速度直線運動，該階段也是機器人末端執行器運行的主要階段。

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

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

積分得到速度表達式為：

積分得到位移表達式為：

1.2 基于正弦加減速控制的直線定時插補位置軌跡規劃算法

以直線軌跡為目標軌跡，采用定時插補的方式，加入正弦加減速控制算法，使機器人末端執行器在起停階段光滑過渡，從而構造出從起點到終點的平滑位置軌跡。

如圖2所示，機器人末端執行器從Ps點沿直線運動到Pe點，起始點Ps坐標為（xs，ys，zs），終點Pe坐標為（xe，ye，ze），設插補周期為Δtt，運行速度為v，從起點到第i個插補點的位移可表示為si=iΔts=ivΔtt。

將直線PsPe表示成是時間t的參數方程的形式，則第i個插補點Pi的（xi，yi，zi）坐標分別為：

圖2 空間直線

插補點Pi的坐標（xi，yi，zi）為第i個插補周期t=iΔtt時刻末端執行器的三維坐標。

給定若干個示教點，設定最大速度vm，第i個插補周期Δtt的位移為si，最大加速度am，勻速段速度為vc，則整個過程設加速度的表達式為：

積分得到速度表達式為：

積分得到位移表達式為：

則由式(7)得加速階段、勻速階段、減速階段對應的位移si分別為：

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

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

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

式中：s1是加速階段iΔtt=t1時的位移，

s2是整個勻速階段的位移，即s2=vc(t2-t1)。

將不同階段的si代入式(7)，就可得到加減速劃分后的第i個插補周期末端執行器的三維坐標。

由式(5)加速度的變化規律，wt1=π，從而得。

待各參數確定后，需校驗勻速段速度vc是否“可達”，即在給定的加速度下機器人末端執行器經歷加速階段、勻速階段、減速階段的完整過程。

當t1=t2時，整個運動過程處于臨界狀態。已知加速度段的位移為：

當t1=t2時，有，從而得臨界速度為：。

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

1.3 基于正弦加減速控制的四元數球面線性插值的姿態軌跡規劃算法

采用單位四元數的球面線性插值的方法，實現兩姿態間的軌跡插值，加入正弦加減速控制算法，對姿態角的變化過程進行加減速控制，構建出姿態平滑的軌跡。

令末端執行器的起始姿態和結束姿態對應的單位四元數分別為Q0和Q1，總角位移為 ?=arcos(Q0·Q1)，總運動時間為T，插補周期為Δtt，u的取值為，其中n=T/Δtt，0≤ ?=arcos(Q0·Q1)≤π，0≤u≤1，則球面插值曲線為：

由式(7)得兩姿態間角位移θ隨時間t得變化關系為：

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

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

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

速度規劃的本質是控制角位移θ的變化，即對球面線性插值公式slerp（）函數中的u的控制。把u=θi/?代入slerp（）函數，得到插補周期Δtt變化的四元數序列，最后將四元數序列轉化為姿態旋轉矩陣序列。

2 位姿軌跡規劃算法同步

機器人運行過程中末端執行器的位置和姿態一般是同步變化的的，它們的變化同時開始同時結束。由于位置軌跡規劃和姿態軌跡規劃算法是分開進行的，必須要使二者的運行時間同步，以得到同步變化的位姿軌跡。

設位置軌跡運行總時間和姿態軌跡運行總時間分別為Tpos和Tort，根據Tpos和Tort的大小關系分三種情況進行同步處理：

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

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

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

3 位姿軌跡規劃算法仿真

以Cyton Gamma300七自由度機器人為實驗對象，在MATLAB環境下驗證位姿軌跡規劃算法。

3.1 直線位姿軌跡算法仿真

位姿軌跡規劃算法能夠根據示教點使機器人運行出預設任務軌跡，解析出機器人運行過程中的位置軌跡變化和姿態軌跡變化，同時對位姿軌跡規劃算法中的位置軌跡插補算法、姿態軌跡插補算法的有效性進行分析。

直線軌跡的起始點Ts和終止點Te，其對應的位姿矩陣分別為：

設定機器人末端執行的最大運行速度為300mm/s，最大運行加速度為500mm/s2，最大姿態角速度為0.3rad/s，最大姿態角加速度為0.5rad/s2，插補周期為2ms，位姿矩陣中位置坐標的單位為毫米（mm），姿態的單位為弧度（rad），得到機器人的位姿軌跡如圖3所示。

圖3 直線位姿軌跡曲線

3.2 直線位置軌跡算法仿真分析

末端執行器在直線運動過程中位置軌跡如圖4所示。

圖4 直線位置軌跡曲線

末端執行器在直線運動過程中，其位移、速度、加速度隨時間變化的關系如圖5所示。

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

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

3.3 直線姿態軌跡算法仿真分析

末端執行器在直線運動過程中姿態軌跡如圖6所示。

末端執行器在直線運動過程中，其角位移、角速度、角加速度隨時間變化關系如圖7所示。

圖6 直線姿態軌跡曲線

圖7 直線姿態軌跡位移、速度及加速度變化曲線

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

4 結論

通過以Cyton Gamma300七自由度機器人為實驗對象，研究了工業機器人笛卡爾空間下的位姿軌跡規劃算法。以直線軌跡為目標軌跡，結合正弦加減速控制算法，研究了位置軌跡規劃算法；以單位四元數表示末端執行器的姿態，結合正弦加減速控制算法，研究了姿態軌跡規劃算法，并對位置軌跡和姿態軌跡進行同步處理，獲得了使機器人從啟到停位置與姿態均連續光滑變化的位姿軌跡軌跡規劃算法。