七自由度串聯(lián)機(jī)器人位姿軌跡規(guī)劃算法與仿真

陳愛文，黃忠明，黃鳳良

（1.揚(yáng)州職業(yè)大學(xué)電子工程學(xué)院，揚(yáng)州 225009；2.南京師范大學(xué) 南瑞電氣與自動(dòng)化學(xué)院，南京 210042）

0 引言

位姿軌跡規(guī)劃化是工業(yè)機(jī)器人執(zhí)行作業(yè)任務(wù)中的一個(gè)重要環(huán)節(jié)，通過給出有限個(gè)示教點(diǎn)規(guī)劃得到笛卡爾空間下的目標(biāo)軌跡，為實(shí)現(xiàn)機(jī)器人高效、準(zhǔn)確、平穩(wěn)地執(zhí)行作業(yè)任務(wù)奠定基礎(chǔ)、減少繁重的機(jī)器人示教工作。工業(yè)機(jī)器人的位姿軌跡規(guī)劃包括位置軌跡規(guī)劃和姿態(tài)軌跡規(guī)劃，只有位置軌跡與姿態(tài)軌跡同時(shí)光滑連續(xù)且同步運(yùn)行，才能保證機(jī)器人末端執(zhí)行器的任務(wù)軌跡連續(xù)光滑。

本文以笛卡爾空間下的直線軌跡為目標(biāo)位置軌跡，單位四元數(shù)表示姿態(tài)，結(jié)合正弦加減速控制算法，分別規(guī)劃位置軌跡和姿態(tài)軌跡，并對兩種軌跡進(jìn)行位姿同步處理，以獲得位置和姿態(tài)軌跡均連續(xù)光滑且同步變化的位姿軌跡。

1 位姿軌跡規(guī)劃算法

機(jī)器人末端執(zhí)行器從起始端到末端平滑過渡，需要對機(jī)器人的整個(gè)運(yùn)行過程進(jìn)行加減速控制，選擇易于實(shí)現(xiàn)、曲線平滑性較好的正弦加減速控制算法，實(shí)現(xiàn)機(jī)器人在各關(guān)節(jié)位移、速度、加速度、加加速度等的連續(xù)光滑，提高機(jī)器人運(yùn)動(dòng)軌跡的精度和運(yùn)動(dòng)的穩(wěn)定性。

1.1 正弦加減速控制算法

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

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

1）加速階段（0＜t≤t1），加速度a隨時(shí)間按正弦規(guī)律變化，從0變化到最大值，再變化為0。整個(gè)加速階段，加速度的值始終位于y坐標(biāo)軸的正半軸，而速度也會(huì)在此階段從0達(dá)到最大值，即勻速階段速度vm。

2）勻速階段（t1＜t≤t2），以恒定速度vm運(yùn)行至減速階段，此階段加速度為0，軌跡按恒定速度直線運(yùn)動(dòng)，該階段也是機(jī)器人末端執(zhí)行器運(yùn)行的主要階段。

3）減速階段（t2＜t≤T），加速度a隨時(shí)間按正弦規(guī)律變化，從0變化到最小值，再變化為0。整個(gè)加速階段，加速度的值始終位于y坐標(biāo)軸的負(fù)半軸，而速度也會(huì)在勻速階段速度vm減小到0，當(dāng)t=T時(shí)機(jī)器人末端執(zhí)行器走完預(yù)設(shè)軌跡，到達(dá)目標(biāo)位置，運(yùn)行結(jié)束。

通過給定若干個(gè)示教點(diǎn)，設(shè)定最大速度vm，最大加速度am，則整個(gè)過程加速度表達(dá)式為：

積分得到速度表達(dá)式為：

積分得到位移表達(dá)式為：

1.2 基于正弦加減速控制的直線定時(shí)插補(bǔ)位置軌跡規(guī)劃算法

以直線軌跡為目標(biāo)軌跡，采用定時(shí)插補(bǔ)的方式，加入正弦加減速控制算法，使機(jī)器人末端執(zhí)行器在起停階段光滑過渡，從而構(gòu)造出從起點(diǎn)到終點(diǎn)的平滑位置軌跡。

如圖2所示，機(jī)器人末端執(zhí)行器從Ps點(diǎn)沿直線運(yùn)動(dòng)到Pe點(diǎn)，起始點(diǎn)Ps坐標(biāo)為（xs，ys，zs），終點(diǎn)Pe坐標(biāo)為（xe，ye，ze），設(shè)插補(bǔ)周期為Δtt，運(yùn)行速度為v，從起點(diǎn)到第i個(gè)插補(bǔ)點(diǎn)的位移可表示為si=iΔts=ivΔtt。

將直線PsPe表示成是時(shí)間t的參數(shù)方程的形式，則第i個(gè)插補(bǔ)點(diǎn)Pi的（xi，yi，zi）坐標(biāo)分別為：

圖2 空間直線

插補(bǔ)點(diǎn)Pi的坐標(biāo)（xi，yi，zi）為第i個(gè)插補(bǔ)周期t=iΔtt時(shí)刻末端執(zhí)行器的三維坐標(biāo)。

給定若干個(gè)示教點(diǎn)，設(shè)定最大速度vm，第i個(gè)插補(bǔ)周期Δtt的位移為si，最大加速度am，勻速段速度為vc，則整個(gè)過程設(shè)加速度的表達(dá)式為：

積分得到速度表達(dá)式為：

積分得到位移表達(dá)式為：

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

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

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

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

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

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

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

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

待各參數(shù)確定后，需校驗(yàn)勻速段速度vc是否“可達(dá)”，即在給定的加速度下機(jī)器人末端執(zhí)行器經(jīng)歷加速階段、勻速階段、減速階段的完整過程。

當(dāng)t1=t2時(shí)，整個(gè)運(yùn)動(dòng)過程處于臨界狀態(tài)。已知加速度段的位移為：

當(dāng)t1=t2時(shí)，有，從而得臨界速度為：。

當(dāng)vc≤vset時(shí)，勻速段速度vc“可達(dá)”，即運(yùn)動(dòng)過程有勻速段，否則需要重新設(shè)定最大運(yùn)行速度vc或者最大加速度am。

1.3 基于正弦加減速控制的四元數(shù)球面線性插值的姿態(tài)軌跡規(guī)劃算法

采用單位四元數(shù)的球面線性插值的方法，實(shí)現(xiàn)兩姿態(tài)間的軌跡插值，加入正弦加減速控制算法，對姿態(tài)角的變化過程進(jìn)行加減速控制，構(gòu)建出姿態(tài)平滑的軌跡。

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

由式(7)得兩姿態(tài)間角位移θ隨時(shí)間t得變化關(guān)系為：

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

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

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

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

2 位姿軌跡規(guī)劃算法同步

機(jī)器人運(yùn)行過程中末端執(zhí)行器的位置和姿態(tài)一般是同步變化的的，它們的變化同時(shí)開始同時(shí)結(jié)束。由于位置軌跡規(guī)劃和姿態(tài)軌跡規(guī)劃算法是分開進(jìn)行的，必須要使二者的運(yùn)行時(shí)間同步，以得到同步變化的位姿軌跡。

設(shè)位置軌跡運(yùn)行總時(shí)間和姿態(tài)軌跡運(yùn)行總時(shí)間分別為Tpos和Tort，根據(jù)Tpos和Tort的大小關(guān)系分三種情況進(jìn)行同步處理：

1）Tpos=Tort：位置軌跡與姿態(tài)軌跡運(yùn)行時(shí)間相等，滿足位姿同步的要求。

2）Tpos＞Tort：位置軌跡運(yùn)行時(shí)間大于姿態(tài)軌跡運(yùn)行時(shí)間，此時(shí)增加Tort，使Tort=Tpos，計(jì)算相應(yīng)的勻速段角速度wc，重新規(guī)劃姿態(tài)軌跡。

3）Tpos＜Tort：位置軌跡運(yùn)行時(shí)間小于姿態(tài)軌跡運(yùn)行時(shí)間，此時(shí)增加Tpos，使Tpos=Tort，計(jì)算相應(yīng)的勻速段速度vc或角速度wc，重新規(guī)劃位置軌跡。

3 位姿軌跡規(guī)劃算法仿真

以Cyton Gamma300七自由度機(jī)器人為實(shí)驗(yàn)對象，在MATLAB環(huán)境下驗(yàn)證位姿軌跡規(guī)劃算法。

3.1 直線位姿軌跡算法仿真

位姿軌跡規(guī)劃算法能夠根據(jù)示教點(diǎn)使機(jī)器人運(yùn)行出預(yù)設(shè)任務(wù)軌跡，解析出機(jī)器人運(yùn)行過程中的位置軌跡變化和姿態(tài)軌跡變化，同時(shí)對位姿軌跡規(guī)劃算法中的位置軌跡插補(bǔ)算法、姿態(tài)軌跡插補(bǔ)算法的有效性進(jìn)行分析。

直線軌跡的起始點(diǎn)Ts和終止點(diǎn)Te，其對應(yīng)的位姿矩陣分別為：

設(shè)定機(jī)器人末端執(zhí)行的最大運(yùn)行速度為300mm/s，最大運(yùn)行加速度為500mm/s2，最大姿態(tài)角速度為0.3rad/s，最大姿態(tài)角加速度為0.5rad/s2，插補(bǔ)周期為2ms，位姿矩陣中位置坐標(biāo)的單位為毫米（mm），姿態(tài)的單位為弧度（rad），得到機(jī)器人的位姿軌跡如圖3所示。

圖3 直線位姿軌跡曲線

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

末端執(zhí)行器在直線運(yùn)動(dòng)過程中位置軌跡如圖4所示。

圖4 直線位置軌跡曲線

末端執(zhí)行器在直線運(yùn)動(dòng)過程中，其位移、速度、加速度隨時(shí)間變化的關(guān)系如圖5所示。

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

由圖5可知，末端執(zhí)行器在直線運(yùn)動(dòng)啟停加速階段、勻速階段、減速階段的變化均符合足正弦加減速控制策略的控制規(guī)律，整個(gè)過程位移、速度及加速度連續(xù)均連續(xù)變化，符合位置規(guī)劃算法的預(yù)期目標(biāo)。從圖5中可以看出，直線位置軌跡最大運(yùn)行速度并不是設(shè)定的0.3m/s，這是因?yàn)樵谶M(jìn)行位姿同步處理時(shí)姿態(tài)軌跡運(yùn)行時(shí)間大于位置軌跡運(yùn)行時(shí)間，此時(shí)同步處理算法通過改變最大運(yùn)行段速度重新對位置軌跡進(jìn)行插補(bǔ)，從而使位置軌跡運(yùn)行時(shí)間和姿態(tài)軌跡運(yùn)行時(shí)間相等，達(dá)到位姿同步的目的。

3.3 直線姿態(tài)軌跡算法仿真分析

末端執(zhí)行器在直線運(yùn)動(dòng)過程中姿態(tài)軌跡如圖6所示。

末端執(zhí)行器在直線運(yùn)動(dòng)過程中，其角位移、角速度、角加速度隨時(shí)間變化關(guān)系如圖7所示。

圖6 直線姿態(tài)軌跡曲線

圖7 直線姿態(tài)軌跡位移、速度及加速度變化曲線

從圖7可知，姿態(tài)軌跡規(guī)劃算法可以實(shí)現(xiàn)啟停階段姿態(tài)角速度和角加速度變化的連續(xù)性，符合姿態(tài)軌跡規(guī)劃的預(yù)期目標(biāo)。同時(shí)從圖7中可以看出，姿態(tài)軌跡的運(yùn)行時(shí)間為2.85秒，與圖5中所示的位置軌跡運(yùn)行時(shí)間相等，驗(yàn)證了位姿同步處理算法的可行性。

4 結(jié)論

通過以Cyton Gamma300七自由度機(jī)器人為實(shí)驗(yàn)對象，研究了工業(yè)機(jī)器人笛卡爾空間下的位姿軌跡規(guī)劃算法。以直線軌跡為目標(biāo)軌跡，結(jié)合正弦加減速控制算法，研究了位置軌跡規(guī)劃算法；以單位四元數(shù)表示末端執(zhí)行器的姿態(tài)，結(jié)合正弦加減速控制算法，研究了姿態(tài)軌跡規(guī)劃算法，并對位置軌跡和姿態(tài)軌跡進(jìn)行同步處理，獲得了使機(jī)器人從啟到停位置與姿態(tài)均連續(xù)光滑變化的位姿軌跡軌跡規(guī)劃算法。