從今天開始，我們進(jìn)入核心技術(shù)篇。第一個(gè)要講的問題，是整個(gè)運(yùn)動(dòng)控制的起點(diǎn)：上位機(jī)給出一個(gè)目標(biāo)姿態(tài)，六根桿分別該伸多長(zhǎng)？這個(gè)問題看似簡(jiǎn)單，實(shí)際上它是Stewart平臺(tái)運(yùn)動(dòng)學(xué)中最核心的數(shù)學(xué)問題。控制器必須先回答一個(gè)更具體的問題：為了讓上平臺(tái)到達(dá)這個(gè)目標(biāo)位置和姿態(tài)，六支鏈分別應(yīng)該達(dá)到多長(zhǎng)？這一步就是Stewart平臺(tái)的逆運(yùn)動(dòng)學(xué)求解。
一、先分清正解和逆解
1. 用"調(diào)望遠(yuǎn)鏡"理解兩類問題
想象你在用一架老式望遠(yuǎn)鏡看星星。你想看某顆星，需要把望遠(yuǎn)鏡轉(zhuǎn)到特定角度。
已知目標(biāo)角度，調(diào)整旋鈕------對(duì)應(yīng)逆運(yùn)動(dòng)學(xué)（逆解）：由目標(biāo)位姿求解驅(qū)動(dòng)控制量。
調(diào)整旋鈕后，觀測(cè)當(dāng)前指向------對(duì)應(yīng)正運(yùn)動(dòng)學(xué)（正解）：由驅(qū)動(dòng)支鏈長(zhǎng)度求解動(dòng)平臺(tái)當(dāng)前位姿。
2. 逆運(yùn)動(dòng)學(xué)
已知上平臺(tái)的目標(biāo)位姿：
求六條支鏈的目標(biāo)長(zhǎng)度：
這里的x、y、z表示平移量，φ、θ、ψ分別表示橫滾、俯仰和偏航角。具體角度名稱和正方向必須服從項(xiàng)目采用的坐標(biāo)系定義。
本文所討論的"給定目標(biāo)位姿求六支鏈桿長(zhǎng)"，就是逆運(yùn)動(dòng)學(xué)問題。
3. 正運(yùn)動(dòng)學(xué)
正運(yùn)動(dòng)學(xué)的輸入和輸出恰好相反：已知六條支鏈的實(shí)際長(zhǎng)度，反過來求上平臺(tái)當(dāng)前的六自由度位姿。
正運(yùn)動(dòng)學(xué)通常沒有像逆運(yùn)動(dòng)學(xué)那樣直接、唯一的閉式表達(dá)，需要采用Newton-Raphson、Levenberg-Marquardt等數(shù)值迭代方法。計(jì)算結(jié)果還會(huì)受到初始值、機(jī)構(gòu)構(gòu)型、傳感器誤差和奇異位形的影響。
在實(shí)際系統(tǒng)中：
? 逆解用于把位姿指令轉(zhuǎn)換為執(zhí)行器目標(biāo)；
? 正解可用于狀態(tài)估計(jì)、運(yùn)動(dòng)顯示、標(biāo)定驗(yàn)證和控制結(jié)果校核；
? 若配置外部位姿測(cè)量裝置，還可將正解結(jié)果與測(cè)量結(jié)果進(jìn)行比較。
二、建立兩個(gè)坐標(biāo)系
逆運(yùn)動(dòng)學(xué)計(jì)算的第一步不是寫公式，而是統(tǒng)一坐標(biāo)系。
通常需要建立兩個(gè)右手坐標(biāo)系：
-
固定坐標(biāo)系{B}：安裝在下平臺(tái)或基座上；
-
動(dòng)平臺(tái)坐標(biāo)系{P}：固定在上平臺(tái)上，隨上平臺(tái)一起運(yùn)動(dòng)。
設(shè)第i條支鏈在下平臺(tái)上的鉸點(diǎn)坐標(biāo)為：
上鉸點(diǎn)在動(dòng)平臺(tái)坐標(biāo)系中的坐標(biāo)為：
下鉸點(diǎn)坐標(biāo)始終在固定坐標(biāo)系中表示。上鉸點(diǎn)坐標(biāo)雖然在動(dòng)平臺(tái)局部坐標(biāo)系中保持不變，但它在固定坐標(biāo)系中的位置會(huì)隨平臺(tái)位姿變化。
三、把目標(biāo)位姿轉(zhuǎn)換成旋轉(zhuǎn)矩陣
上平臺(tái)的目標(biāo)位姿可以寫成齊次變換矩陣：
如果項(xiàng)目采用常見的ZYX旋轉(zhuǎn)約定，可寫為：
需要特別強(qiáng)調(diào)：不同系統(tǒng)采用的旋轉(zhuǎn)順序可能不同。
XYZ、ZYX、內(nèi)旋、外旋或者航空坐標(biāo)系之間不能直接混用。即使輸入的三個(gè)角度數(shù)值完全相同，只要旋轉(zhuǎn)順序不同，最終得到的六支鏈長(zhǎng)度就可能不同。
-
使用左手坐標(biāo)系還是右手坐標(biāo)系；
-
三個(gè)平移軸的正方向；
-
Roll、Pitch、Yaw分別繞哪一根軸；
-
采用內(nèi)旋還是外旋；
-
歐拉角的旋轉(zhuǎn)順序；
-
輸入角度單位是度還是弧度。
四、逆解的核心：計(jì)算每條支鏈的空間向量
當(dāng)目標(biāo)平移向量t和旋轉(zhuǎn)矩陣R已知后，第i個(gè)上鉸點(diǎn)在固定坐標(biāo)系中的位置為：
這一步包含兩個(gè)動(dòng)作：先用旋轉(zhuǎn)矩陣改變上鉸點(diǎn)相對(duì)動(dòng)平臺(tái)原點(diǎn)的方向，再加上動(dòng)平臺(tái)原點(diǎn)的平移量。
于是，第i條支鏈的空間向量為：
代入上式：
對(duì)這個(gè)空間向量求歐氏范數(shù)，就得到第i條支鏈的目標(biāo)長(zhǎng)度：
對(duì)i=1到6分別執(zhí)行一次，就能得到完整的六支鏈長(zhǎng)度向量：
一個(gè)簡(jiǎn)化例子
假設(shè)平臺(tái)在零位時(shí)，某根桿的理論長(zhǎng)度是785 mm。現(xiàn)在上平臺(tái)升高了50 mm，其他姿態(tài)不變。由于桿是斜著安裝的（與垂直方向有夾角），桿長(zhǎng)的變化并不等于50 mm，而是要根據(jù)幾何關(guān)系計(jì)算。
如果桿與垂直方向夾角約30°，那么桿長(zhǎng)變化約為：
也就是說，上平臺(tái)升高50 mm，這根桿要伸長(zhǎng)約57.7 mm。這就是逆解在做的事------把平臺(tái)位姿變化翻譯成每根桿的長(zhǎng)度變化。六根桿的夾角各不相同，所以同樣的位姿變化，六根桿的伸縮量也不同。這就是為什么需要精確的運(yùn)動(dòng)學(xué)模型來計(jì)算。
五、桿長(zhǎng)不一定等于電動(dòng)缸控制指令
由逆運(yùn)動(dòng)學(xué)得到的Li，通常表示兩個(gè)鉸點(diǎn)中心之間的幾何距離。但伺服系統(tǒng)實(shí)際需要的可能是電動(dòng)缸伸出量、絲杠或電機(jī)的位置脈沖、相對(duì)于機(jī)械零位的增量，或者相對(duì)于平臺(tái)中位的長(zhǎng)度變化。
如果第i條支鏈的機(jī)械基準(zhǔn)長(zhǎng)度為L(zhǎng)i0，則其目標(biāo)伸縮量可寫為：
若電機(jī)編碼器計(jì)數(shù)與直線位移的換算系數(shù)為Ki，還需要進(jìn)一步轉(zhuǎn)換：
其中Ci0是與機(jī)械零位對(duì)應(yīng)的編碼器基準(zhǔn)值。
任何一個(gè)環(huán)節(jié)的基準(zhǔn)定義不一致，都可能造成平臺(tái)零位偏差、姿態(tài)耦合，甚至觸發(fā)行程或機(jī)構(gòu)干涉。
六、為什么逆解算對(duì)了，平臺(tái)仍可能不能運(yùn)動(dòng)？
逆解公式給出的是幾何結(jié)果，不代表這個(gè)目標(biāo)位姿在實(shí)際設(shè)備上一定可執(zhí)行。控制系統(tǒng)至少還要完成以下檢查。
1. 支鏈行程檢查
只要有一條支鏈超出范圍，整個(gè)目標(biāo)位姿就不能直接執(zhí)行。
2. 速度、加速度與Jerk檢查
支鏈長(zhǎng)度隨時(shí)間變化后，還要繼續(xù)計(jì)算速度、加速度和Jerk，防止指令在數(shù)學(xué)上連續(xù)、在機(jī)械上卻過于激烈。
3. 鉸鏈角度檢查
支鏈長(zhǎng)度沒有超限，不代表球鉸或虎克鉸一定處于允許偏轉(zhuǎn)范圍。姿態(tài)較大時(shí)，某個(gè)鉸鏈可能先達(dá)到機(jī)械角度極限。
4. 機(jī)構(gòu)干涉檢查
六條支鏈之間、支鏈與平臺(tái)之間、負(fù)載與周邊設(shè)備之間都可能發(fā)生干涉。
5. 奇異性檢查
當(dāng)機(jī)構(gòu)接近奇異位形時(shí)，平臺(tái)可能出現(xiàn)局部失控、驅(qū)動(dòng)力急劇增大或某些方向剛度下降。逆解仍可能返回一組桿長(zhǎng)，但這組桿長(zhǎng)不一定對(duì)應(yīng)安全、穩(wěn)定、可控的狀態(tài)。
6. 工作空間檢查
Stewart平臺(tái)的六個(gè)位姿分量彼此耦合。"X方向可以移動(dòng)某個(gè)距離"和"俯仰可以達(dá)到某個(gè)角度"，不代表兩者能夠同時(shí)達(dá)到各自最大值。
七、正運(yùn)動(dòng)學(xué)為什么更復(fù)雜？
逆運(yùn)動(dòng)學(xué)中，上平臺(tái)位姿已知，每個(gè)上鉸點(diǎn)經(jīng)過坐標(biāo)變換后都能直接得到位置，再計(jì)算與對(duì)應(yīng)下鉸點(diǎn)之間的距離即可。
正運(yùn)動(dòng)學(xué)則相反。已知的是六個(gè)標(biāo)量長(zhǎng)度，需要反推出三個(gè)平移量和三個(gè)旋轉(zhuǎn)量。其約束方程可以寫為：
六條支鏈形成六個(gè)非線性方程：
實(shí)際求解通常包括以下過程：
如果初始值距離真實(shí)位姿過遠(yuǎn)，或者平臺(tái)接近奇異區(qū)域，迭代可能收斂緩慢、收斂到錯(cuò)誤構(gòu)型，甚至無法收斂。
工程系統(tǒng)通常利用上一控制周期的位姿作為下一周期正解的初始值，因?yàn)檫B續(xù)運(yùn)動(dòng)過程中，相鄰周期的位姿變化通常較小。南京全控的控制系統(tǒng)中，正運(yùn)動(dòng)學(xué)求解采用Newton-Raphson迭代法，精度可達(dá)1×10??，一般迭代2---3次即可收斂。
八、從數(shù)學(xué)模型走向工程系統(tǒng)，還差三步
第一步：準(zhǔn)確獲得機(jī)構(gòu)幾何參數(shù)
理論模型使用設(shè)計(jì)圖紙中的上下鉸點(diǎn)坐標(biāo)，實(shí)際設(shè)備則會(huì)受到加工、裝配和鉸鏈安裝誤差影響。對(duì)于精度要求較高的平臺(tái)，不能只依賴名義尺寸，需要通過測(cè)量和運(yùn)動(dòng)學(xué)標(biāo)定識(shí)別更接近真實(shí)機(jī)構(gòu)的幾何參數(shù)。
第二步：統(tǒng)一整個(gè)系統(tǒng)的數(shù)據(jù)定義
運(yùn)動(dòng)控制器、仿真軟件、上位機(jī)、游戲引擎和測(cè)試系統(tǒng)之間必須使用一致的數(shù)據(jù)協(xié)議，尤其要統(tǒng)一坐標(biāo)軸、角度順序、位姿參考點(diǎn)、零位定義、數(shù)據(jù)周期和超限處理邏輯。
第三步：把逆解放入實(shí)時(shí)控制鏈路
真實(shí)平臺(tái)不會(huì)只計(jì)算一個(gè)靜態(tài)目標(biāo)點(diǎn)?？刂葡到y(tǒng)需要在固定周期內(nèi)持續(xù)完成：
-
接收目標(biāo)位姿；
-
進(jìn)行軌跡規(guī)劃或數(shù)據(jù)濾波；
-
執(zhí)行運(yùn)動(dòng)學(xué)逆解；
-
檢查工作空間和安全邊界；
-
計(jì)算六軸同步目標(biāo)；
-
下發(fā)到伺服驅(qū)動(dòng)器；
-
采集編碼器與系統(tǒng)狀態(tài)；
-
進(jìn)行報(bào)警、記錄或閉環(huán)修正。
因此，逆運(yùn)動(dòng)學(xué)公式本身并不長(zhǎng)，但穩(wěn)定的工程系統(tǒng)需要機(jī)械設(shè)計(jì)、幾何標(biāo)定、實(shí)時(shí)控制、總線通訊、安全策略和軟件架構(gòu)共同配合。
結(jié)語
給定Stewart平臺(tái)的目標(biāo)位姿，求解六支鏈桿長(zhǎng)，本質(zhì)上可以歸納為三個(gè)步驟：
核心公式是：
但在工程應(yīng)用中，必須繼續(xù)驗(yàn)證行程、速度、加速度、鉸鏈偏角、機(jī)構(gòu)干涉、工作空間和奇異性，并通過零位標(biāo)定將幾何桿長(zhǎng)轉(zhuǎn)換為真實(shí)執(zhí)行器指令。
項(xiàng)目需求建議先明確這些信息
如果您正在進(jìn)行Stewart平臺(tái)、六自由度運(yùn)動(dòng)平臺(tái)或模擬器運(yùn)動(dòng)系統(tǒng)的方案設(shè)計(jì)，建議提前整理：
-
負(fù)載及質(zhì)心位置；
-
上平臺(tái)尺寸與安裝方式；
-
目標(biāo)平移與轉(zhuǎn)動(dòng)范圍；
-
速度、加速度及運(yùn)動(dòng)頻率；
-
控制周期和通訊接口；
-
位姿數(shù)據(jù)來源；
-
精度與驗(yàn)收要求；
-
安裝空間和安全邊界。
南京全控航空科技有限公司可根據(jù)具體項(xiàng)目需求開展機(jī)構(gòu)構(gòu)型、工作空間、運(yùn)動(dòng)學(xué)可行性及控制系統(tǒng)方案分析。所有產(chǎn)品參數(shù)和接口能力以具體方案及最終技術(shù)協(xié)議為準(zhǔn)。
