球形電動(dòng)機(jī)三維旋轉(zhuǎn)運(yùn)動(dòng)軌跡插值算法

張宗楠，吳鳳英，張冰嬌

（天津理工大學(xué) 電氣電子工程學(xué)院，天津300384）

三自由度永磁球形電動(dòng)機(jī)作為新型電動(dòng)機(jī)簡(jiǎn)化了機(jī)械結(jié)構(gòu).具有占用體積小，使用便捷，穩(wěn)定性好等優(yōu)勢(shì)，且球形電動(dòng)機(jī)運(yùn)動(dòng)復(fù)雜，所以有必要對(duì)其控制進(jìn)行研究，而其中的軌跡規(guī)劃及實(shí)現(xiàn)是其中重要一環(huán).

球形電機(jī)運(yùn)動(dòng)軌跡是旋轉(zhuǎn)運(yùn)動(dòng)軌跡，其軌跡表示方法、軌跡規(guī)劃方法以及實(shí)現(xiàn)方法與傳統(tǒng)機(jī)器人表示方法都不相同，現(xiàn)在針對(duì)球形電機(jī)軌跡規(guī)劃研究相關(guān)文獻(xiàn)較多，如安徽工業(yè)大學(xué)提出的B 樣條軌跡規(guī)劃、時(shí)間與能耗最優(yōu)軌跡規(guī)劃.天津大學(xué)提出的時(shí)間最優(yōu)軌跡規(guī)劃、避障軌跡規(guī)劃[1-3].而對(duì)具體實(shí)現(xiàn)運(yùn)動(dòng)軌跡的插值算法研究較少，且傳統(tǒng)的插值算法只用于實(shí)現(xiàn)二維或三維空間的平移運(yùn)動(dòng)軌跡，如機(jī)器人中常用的D-H 模型所用的直線插補(bǔ)算法、空間圓弧插補(bǔ)算法、B 樣條插補(bǔ)算法[4-6].但是此類傳統(tǒng)插值算法不能直接應(yīng)用于球形電動(dòng)機(jī)三維旋轉(zhuǎn)運(yùn)動(dòng)的插值.所以本文提出一種基于歐拉角數(shù)字積分角度插補(bǔ)算法，此插值算法以傳統(tǒng)數(shù)控系統(tǒng)中的數(shù)字積分法為基礎(chǔ)，首先對(duì)球形電機(jī)建立運(yùn)動(dòng)學(xué)模型，通過(guò)逆運(yùn)動(dòng)學(xué)模型，求解出球形電機(jī)運(yùn)動(dòng)的歐拉角，并分析了歐拉角的奇異性問(wèn)題，最后對(duì)三個(gè)歐拉角角度依次進(jìn)行插補(bǔ)實(shí)現(xiàn)球形電機(jī)的運(yùn)動(dòng)控制.

1 三自由度永磁球形電機(jī)結(jié)構(gòu)與工作原理

1.1 永磁球形電機(jī)裝置結(jié)構(gòu)

三自由度永磁球形電動(dòng)機(jī)裝置結(jié)構(gòu)示意圖如圖1 所示.永磁球形電動(dòng)機(jī)由半殼定子和球形轉(zhuǎn)子構(gòu)成，在球形轉(zhuǎn)子的表面赤道平面裝有p 對(duì)永磁極，且按照N、S 極依次分布，法蘭安裝在球形轉(zhuǎn)子頂部，用以連接轉(zhuǎn)子輸出周和負(fù)載.定子為鋁制空球殼，三層獨(dú)立且平行的的定子線圈安裝在定子球殼內(nèi)部，每個(gè)線圈具有相同的空心結(jié)構(gòu)和匝數(shù)，第二層的線圈放置在赤道位置，第一層和第三層的線圈對(duì)稱的分布在第二層線圈兩側(cè)，也就是對(duì)稱分布在赤道兩側(cè)，赤道兩側(cè)線圈的給個(gè)線圈軸線和赤道具有相同的夾角.

圖1 球形電動(dòng)機(jī)裝置結(jié)構(gòu)示意圖Fig.1 Schematic diagram of spherical motor device

1.2 球形電機(jī)工作基本原理

球形電機(jī)運(yùn)動(dòng)原理如圖2 所示.當(dāng)定子線圈通電后，定子線圈就會(huì)產(chǎn)生旋轉(zhuǎn)磁場(chǎng)，定子線圈產(chǎn)生的旋轉(zhuǎn)磁場(chǎng)與轉(zhuǎn)子上永磁體產(chǎn)生的靜態(tài)磁場(chǎng)相互作用，從而產(chǎn)生一定的電磁轉(zhuǎn)矩驅(qū)動(dòng)永磁球形電動(dòng)機(jī)的運(yùn)動(dòng)，通過(guò)控制大小不同的線圈電流，可以產(chǎn)生各種方向和不同大小的電磁轉(zhuǎn)矩，轉(zhuǎn)子輸出軸可以做出自轉(zhuǎn)、俯仰、偏航動(dòng)作，從而實(shí)現(xiàn)球形電動(dòng)機(jī)的三自由度運(yùn)動(dòng).

圖2 球形電動(dòng)機(jī)運(yùn)行原理圖Fig.2 Spherical motor operation schematic

2 建立球形電動(dòng)機(jī)運(yùn)動(dòng)學(xué)模型

2.1 球形電動(dòng)機(jī)正運(yùn)動(dòng)學(xué)

為了便于描述球形電動(dòng)機(jī)球形轉(zhuǎn)子的運(yùn)動(dòng)狀況，需要對(duì)其剛體建立正逆運(yùn)動(dòng)學(xué)模型.本文采用歐拉角ZYX 旋轉(zhuǎn)方式進(jìn)行位姿描述，因?yàn)闅W拉角表示法具有參數(shù)少、直觀、物理意義明確，易于理解和應(yīng)用等優(yōu)點(diǎn).

設(shè)定子的坐標(biāo)系為xyz，球形轉(zhuǎn)子的的坐標(biāo)為abc，兩個(gè)坐標(biāo)的原點(diǎn)相重合，其中，定子坐標(biāo)系為參考坐標(biāo)系，轉(zhuǎn)子坐標(biāo)系為運(yùn)動(dòng)坐標(biāo)系，球形轉(zhuǎn)子的運(yùn)動(dòng)可以看做是運(yùn)動(dòng)坐標(biāo)系繞參考坐標(biāo)系旋轉(zhuǎn)形成的，即轉(zhuǎn)子坐標(biāo)系abc 第一次旋轉(zhuǎn)是繞c 軸從abc 坐標(biāo)系旋轉(zhuǎn)α 角，到達(dá)坐標(biāo)系a1b1c1，第二旋轉(zhuǎn)繞a1軸從坐標(biāo)系旋轉(zhuǎn)β 角，到達(dá)坐標(biāo)系a2b2c2，第三次旋轉(zhuǎn)繞c2 軸從坐標(biāo)系a2b2c2 坐標(biāo)系旋轉(zhuǎn)γ 角到達(dá)坐標(biāo)系abc.這三次旋轉(zhuǎn)的變化矩陣為：

式（1）縮寫符號(hào)分別為：角度余弦cos 簡(jiǎn)記為c，角度正弦sin 簡(jiǎn)記為s.

坐標(biāo)變化關(guān)系示意圖如圖3 所示.

圖3 笛卡爾坐標(biāo)變換關(guān)系示意圖Fig.3 Schematic diagram of Cartesian coordinate transformation

2.2 球形電動(dòng)機(jī)正運(yùn)動(dòng)學(xué)

根據(jù)球形電動(dòng)機(jī)逆運(yùn)動(dòng)學(xué)中已知旋轉(zhuǎn)矩陣，可以求解出對(duì)應(yīng)的歐拉角，將旋轉(zhuǎn)矩陣表示如下：

則可以表示如下歐拉角：

2.3 歐拉角奇異性分析

在數(shù)學(xué)中，奇異性通常指的是在一個(gè)點(diǎn)處，某個(gè)數(shù)學(xué)對(duì)象未被定義，或者一個(gè)集合中的某個(gè)點(diǎn)處的某個(gè)數(shù)學(xué)特性不好，比如不可微.在歐拉角描述鋼體位姿的情況跟下也存在奇異性問(wèn)題，所以在選取歐拉角描述的方式時(shí)，應(yīng)當(dāng)避開這種情況的出現(xiàn).以前面所采用的ZYX 旋轉(zhuǎn)方法為例，給定球形電動(dòng)機(jī)一個(gè)姿態(tài)，是否只有一種旋轉(zhuǎn)方法可以讓基座坐標(biāo)系和轉(zhuǎn)子坐標(biāo)系想重合，答案是否定的.當(dāng)球形電動(dòng)機(jī)Y 軸所指豎直向上時(shí)，此時(shí)但是α、γ 有多種方式，即存在不唯一解，比如不過(guò)球形轉(zhuǎn)子的旋轉(zhuǎn)運(yùn)動(dòng)被外面定子殼體所限制，各個(gè)歐拉角的角度旋轉(zhuǎn)范圍為180°≤α≤180°和-45°≤β、γ≤45°，所以采用ZYX 旋轉(zhuǎn)方式并不會(huì)在球形電動(dòng)機(jī)出現(xiàn)歐拉角的奇異性問(wèn)題.

3 轉(zhuǎn)子輸出軸數(shù)字積分法角度插補(bǔ)原理

3.1 數(shù)學(xué)模型

已知在參考坐標(biāo)系xyz 下轉(zhuǎn)子輸出軸的初始位姿是P，然后以Z-X-Z 歐拉角旋轉(zhuǎn)方式分別旋轉(zhuǎn)α、β、γ 角后，轉(zhuǎn)子輸出軸到達(dá)目標(biāo)位姿點(diǎn)P′，根據(jù)Axis-Angle 理論，轉(zhuǎn)子輸出軸的轉(zhuǎn)動(dòng)可以看做是繞某一軸旋轉(zhuǎn)θ 角后到達(dá)目標(biāo)位姿，若以恒定的角速度ω 發(fā)生偏轉(zhuǎn)，對(duì)應(yīng)歐拉角α、β、γ 角以角速度ωα，ωβ，ωγ發(fā)生偏轉(zhuǎn)，它們的關(guān)系式為：

式中，K為比例系數(shù).

在Δt時(shí)間內(nèi)歐拉角α、β、γ 角度增量Δα，Δβ，Δγ 應(yīng)為：

轉(zhuǎn)子輸出軸從起始位姿轉(zhuǎn)向目標(biāo)位姿的過(guò)程，可以看做是歐拉角α、β、γ 每一個(gè)單位時(shí)間間隔Δt分別以增量Kα、Kβ、Kγ 同時(shí)累加的結(jié)果.經(jīng)過(guò)m次累加后，歐拉角α、β、γ 分別到達(dá)終點(diǎn).

式中，K，α，β，γ 為常數(shù)，若取一單位時(shí)間間隔，Δt=1，則：

若這三式成立，那么：

上述所示公式表明了累加次數(shù)m和比例常數(shù)K的反比例關(guān)系上，如果m確定了，那么K也就確定了，不過(guò)累加次數(shù)m必須為整數(shù)，所以K一定是小數(shù).K的取值主要考慮每次增量Δα、Δβ、Δγ 不大于1，以保證轉(zhuǎn)子坐標(biāo)系abc 每次分配給脈沖不超過(guò)一個(gè)，也就是說(shuō)每次角度增量只產(chǎn)生一個(gè)單位步距角，即：

其中，Kα、Kβ、Kγ 為被積函數(shù).

3.2 進(jìn)給脈沖產(chǎn)生的方法

歐拉角α、β、γ 的最大容許值受系統(tǒng)中寄存器容量的限制，假如寄存器有n位，則α、β、γ 的最大允許寄存器的容量為2n-1（α、β、γ 為二進(jìn)制數(shù)），滿足Kα＜1、Kβ＜1、Kγ＜1，即：

由積分器框圖4 可以看出，圖中被積函數(shù)寄存器JωZ寄存Kα，累加寄存器JRZ寄存余數(shù). 累加結(jié)果大于1 時(shí)，整數(shù)部分溢出作為進(jìn)給Δα，小數(shù)部分存于累加寄存器JRZ中，待下次累加.

圖4 積分框圖及邏輯圖Fig.4 Integration block diagram and logic diagram

顯然，在做歐拉角角度插補(bǔ)時(shí)應(yīng)實(shí)現(xiàn)的、和的積分運(yùn)算.需要同時(shí)采用三個(gè)積分器同時(shí)分別進(jìn)行，這樣組成的數(shù)字積分器角度插補(bǔ)框圖如圖5 所示.

圖5 數(shù)字積分器角度插補(bǔ)框圖Fig.5 Digital integrator angle interpolation block diagram

4 轉(zhuǎn)子輸出軸插補(bǔ)流程及仿真驗(yàn)證

4.1 轉(zhuǎn)子輸出軸插補(bǔ)流程

已知定子殼體的靜坐標(biāo)系xyz 上起始位姿為，球形轉(zhuǎn)子的動(dòng)坐標(biāo)系abc上的目標(biāo)位姿為，經(jīng)式（3）可以反解出球形電動(dòng)機(jī)的歐拉角，再進(jìn)行數(shù)字積分原理歐拉角軌跡插補(bǔ)算法.其算法流程如圖6 所示.

圖6 球形轉(zhuǎn)子插補(bǔ)流程圖Fig.6 Spherical rotor interpolation flow chart

4.2 MATLAB 仿真

球形電機(jī)轉(zhuǎn)子輸出軸的轉(zhuǎn)動(dòng)可以分為三類，第一類是歐拉角α、β、γ 單獨(dú)轉(zhuǎn)動(dòng)，因?yàn)橹皇抢@不同的軸旋轉(zhuǎn)，而旋轉(zhuǎn)的原理則是相同的，所以只對(duì)歐拉角α 進(jìn)行仿真插值，第二類是歐拉角α、β、γ 兩兩進(jìn)行轉(zhuǎn)動(dòng)，同上原則一樣，所以只選取α、β 的組合進(jìn)行仿真插值，第三類是三個(gè)歐拉角α、β、γ 同時(shí)進(jìn)行插值.

第一類歐拉角α 單獨(dú)旋轉(zhuǎn)，已知球形轉(zhuǎn)子其實(shí)位姿為Rs1，目標(biāo)位姿位姿為Re1.

根據(jù)圖6 所示的球形轉(zhuǎn)子插補(bǔ)流程圖進(jìn)行MATLAB 仿真，則球形轉(zhuǎn)子軌跡插值仿真如圖7 所示.

第二類歐拉角α、β 單獨(dú)旋轉(zhuǎn)，已知球形轉(zhuǎn)子其實(shí)位姿為Rs1，目標(biāo)位姿位姿為Re2.

則球形轉(zhuǎn)子軌跡插值仿真如圖8 所示.

圖7 歐拉角α 角度插值仿真圖Fig.7 Euler angle α angle interpolation simulation diagram

圖8 歐拉角α、β 角度插補(bǔ)仿真圖Fig.8 Euler angle α、β angle interpolation simulation diagram

第三類歐拉角α、β、γ 同時(shí)旋轉(zhuǎn)，已知球形轉(zhuǎn)子其實(shí)位姿為Rs1，目標(biāo)位姿位姿為Re3.

則球形轉(zhuǎn)子軌跡插值仿真如圖9 所示.

由MATLAB 仿真圖可知，在三種情況下，歐拉角角度數(shù)字積分法的可行性，把三張仿真圖相互比較可知，歐拉角各自角的插補(bǔ)過(guò)程是相互獨(dú)立的，由圖7 和圖8 可知，當(dāng)插值角度大時(shí)，插補(bǔ)速度快，插值角度小時(shí)，插補(bǔ)速度慢.這是因?yàn)榻嵌鹊牟逖a(bǔ)都是由m=2n次的累加運(yùn)算，也就是插值時(shí)間是一定的，插補(bǔ)的角度越小則誤差越小.

圖9 歐拉角α、β、γ 角度仿真圖Fig.9 Euler angle α、β、γ angle interpolation simulation diagram

5 結(jié) 論

本文提出了一種三自由度永磁球形電動(dòng)機(jī)的數(shù)字積分角度插補(bǔ)算法，根據(jù)球形電動(dòng)機(jī)的特殊的運(yùn)動(dòng)形式，并建立正逆運(yùn)動(dòng)學(xué)模型，詳細(xì)的介紹了角度數(shù)字積分法的原理，在MATLAB 仿真平臺(tái)上驗(yàn)證了球形電動(dòng)機(jī)轉(zhuǎn)子輸出軸運(yùn)行的三種不同運(yùn)動(dòng)情況，從而證明該方法的可行性，為后續(xù)的球形電動(dòng)機(jī)的閉環(huán)控制提供了理論基礎(chǔ).