六由度并聯機器人位姿控制分析與仿真

王栿棟++鄒華勇

摘 要：利用剛體空間六個自由度運動原理，以Stewart平臺為例，介紹了并聯機構正反解的基本原理。并針對并聯機構更困難的正解問題，利用Matlab設計一個圖形加數字坐標仿真程序，可以方便地求得并聯機構符合實際客觀條件的位置解，并直觀地觀察各桿件之間的空間關系。

關鍵詞：正解；反解；坐標；位姿；仿真

引言

機器人從20世紀誕生以來，以迅速成為一門應用十分廣泛的技術。機器人技術在工程中的應用日益廣泛，即工業機器人。其中研究比較早，目前工程界應用比較成熟的是串聯機器人。它是一種能夠自動定位控制、多自由度、各大大小小的機械臂以串聯的形式聯接起來。這種典型的開環機構具有結構簡單，成本低，控制簡單，運動空間大等優點。但伴隨著其結構特性的缺點則是承載能力有限、機構剛度差，各運動機構疊加誤差大，運動慣性大。而在80年代以來隨機計算機技術的發展，并聯機器人的研究成為了新的熱點。并聯機器人與串聯機器人正好可以形成互補，它的優點極為串聯機器人的缺點，相應它的缺點則是串聯機器人的優點，因此并聯機器人在飛行模擬器，空間對接器、裝配生產線等需要高精度，高穩定性，高速的場合應用越來越廣泛。但串聯機器人與并聯機器人的基礎理論基本一樣，所以文章以六自由度并聯機器人最典型的結構——Stewart平臺簡析六自由度機器手入手，在運動學的位置正反解。文章總結的內容中略去了有關機器人的數學基礎知識和基礎力學知識，假設已經具備一定的矢量代數，矩陣論，和工學力學知識，直接進入對機器人本身的運動進行了分析。并約定，一般黑色字體變量為矢量，非黑字體變量為標量。

1 六自由度并聯機器人運動學

研究操作手的幾何學要區分兩個問題，即在同一系統下的運動學正問題和逆問題，所謂的正問題就是給定關節變量前提下，求機器人執行終端的位姿，這個求解過程就叫正解。而逆問題反過來，設定定機器人執行終端的位置，反求各關節變量，這個求解過程叫反解。

見圖1a所示并聯操作手，關于這個并聯機構的反解其實是容易的，這里不再敘述，有興趣者可參閱本文列出得參考文獻。現在主要對其行運動學正問題分析，但在應用到如圖1的并聯機器人中時，他的正解運動學卻并不那么簡單。

在圖1a中，考慮三角形AiSiBi，i=1～3，角標i表示第i對腿。當6個腿的長度固定，把平臺M去掉，三角形AiSiBi只能繞AiBi軸旋轉。這樣，我們可以用一個長度為li的腿代替長度為qia和qib的一對腿，這條腿通過一個繞AiBi的轉動關節和六角基座平臺β相連接。結構簡化后的結果如圖2所示，它在運動學上的等價于圖1a的原來結構。

圖1 6自由度飛行器模擬器

a）基本結構 b）三角形活動平臺、六角形固定平臺

然后引入坐標系Fi，它的原點設在第i個腿和基座平臺β連接的點口Oi處，并作如下約定：

對i=1，2，3，

Oi是連接旋轉關節的中心的集合；

Xi的方向是從Ai指向Bi

Yi的選擇是由Zi垂直于6邊形固定平臺指向上來確定的，也就是X與Y坐標向量的矢量積方向朝上。

之后來確定從六邊形的中心O到移動平臺的3個頂點S1，S2 和S3的位置矢量。

我們需要確定li和Oi。參考圖2和圖3，其中ai和bi表示Ai，和Bi相對O為原點基座坐標的矢量。對于i=1，2，3，有

因此ui是由Ai指向Bi的單位矢量。而且，原點Oi的位置由矢量oi確定，oi的表述如下：

（1）

進而，設si為在坐標系Fi（Oi，Xi，Yi，Zi）中Si的位置矢量，則

（2）

圖2 等效簡化機構

圖3 一條腿與一對腿的等效 圖4 坐標系F0與坐標系Fi的關系

現在，將一個坐標系空間F0定義為原點 O和X，Y，軸在六邊形固定平面上，其上的 Xi和 Yi的關系如圖4所示.當在坐標系F0中表達，si的形式如下

（2.3）

這里，[Ri]0是在坐標系F0中表達的從坐標系F0到坐標系Fi的旋轉矩陣，給出如下

（2.4）

參考圖（4）有

（2.5）

（2.6）

將式（4）～式（6）代入式（3）得到

（2.7）

這里，Oi由式（8）給出。

因為活動三角平臺三個頂點之間的距離是固定的，所以位置矢量S1，S2，S3必須滿足以下約束

（8a） （8b） （8c）

展開后，式（8a-c）取形式

（9a）

（9b）

（9c）

Di，Ei，Fi都是已知數據，i為1～5， 他們可以通過計算得到，經過整理后具體形式如下。

其中 ，是一個2*2反對稱矩陣，這樣三個方程已經

列好，并對應三個未知數？準1，？準2，？準3。方程數與未知數數量一致，在理論上是可以有解的，但這個數據運算也好，方程本身的復雜度也好，要把它迅速正確求出代數解是很困難的。如果？準1，？準2，？準3三個角符合實際情況的解（0～？仔）能夠得到，則通過式（7）可得到確定三角運動平臺三個頂點的矢量，從而根據三點確定一面的原理，來確定并聯機器人執行端（三角活動平臺）的空間位姿。接下去就是如何快速正確地解這方程。

2 仿真程序

2.1 相關仿真參數簡述

按照上述原理，編寫計算程序，實現坐標顯示和仿真圖示。如圖5a所示，綠色的正六邊形代表固定底盤，其大小由邊長b確定。上方正三角形是活動平臺，其大小由邊長a確定。L1～L6表示六條機器腿的長度，同一顏色為一組。a1～a3表示活動平臺三個頂點，其坐標通過計算已顯示在左上角。通過計算還可以識別活動平臺法線朝向，如果是朝上則顯示青色（如圖5a），如果朝下則顯示品紅色（如圖5b），一般多數Stewart平臺的活動平臺正常使用情況桿多為法線方向一致朝上。而在此處是為了分析活動平臺所有可能的情況而設計的仿真程序。

除此之外還可以利用圖形界面的放大和旋轉功能來觀察各桿件之間的位置關系，從而判斷其互相干涉情況。

2.2 仿真過程羅列

請看圖6。整個仿真過程，分為仿真計算、仿真圖示兩個大部分。其中仿真計算是最根本，而且執行過程相對耗時，仿真圖示是根據初始輸入參數把計算結果來顯示出來。

圖6 仿真計算結構簡圖

2.3 仿真計算結構

為了使得整個仿真過程快速度，作者采用了數值迭代法。因為在理論上采用符號計算（復數域求解）是最全最精確的，而事實上在matlab中符號計算對于參數較多，相對復雜的方程組，計算相當耗時。而且返回的結果不一定齊全。

為了使仿真計算結果沒有遺漏，作者在數值迭代計算中合理分布了三個方程共9個初值，使得計算結果沒有遺漏，并且避免三角函數因周期而產生的重復解。

為了使仿真計算結果符合客觀實際情況，作者采用了數值篩選，目的之一是去除不符合客觀物理空間復數解。目的之二是去除不符合并聯機構實際結構的負數解。

整個計算機構請看圖7所示。中間計算解值是指圖（3）中α1～α3三個角度。

圖3.3仿真計算結構

2.4 仿真舉例

當輸入參數為a=6，b=9，L1=8，L2=8，L3=8，L4=8，L5=13，L6=15.時候仿真結果如下，見圖8，結果顯示，本初始條件系下，并聯結構有兩個位姿解，其中一個法線朝上，一個法線朝下。相應的活動平臺頂點坐標也顯示在左上角。

3 結束語

通過對并聯機構運動學的分析，設計用來求并聯機構正解位置的仿真程序，這一來不但可以快速正確地求得在不同機構參數下的位置解，而且可以通過圖示清楚直觀地了解各工作臺面的和機器腿在三維空間中的實際姿態。

參考文獻

[1]宋偉剛.機器人學[J].科學出版社，2007.

[2]黃真，孔令富，方躍法.并聯機器人機構學理論及控制[J].機械工業出版社，1997.

[3]唐錫寬，金德聞.機械動力學[J].高等教育出版社，1983.endprint

摘 要：利用剛體空間六個自由度運動原理，以Stewart平臺為例，介紹了并聯機構正反解的基本原理。并針對并聯機構更困難的正解問題，利用Matlab設計一個圖形加數字坐標仿真程序，可以方便地求得并聯機構符合實際客觀條件的位置解，并直觀地觀察各桿件之間的空間關系。

關鍵詞：正解；反解；坐標；位姿；仿真

引言

機器人從20世紀誕生以來，以迅速成為一門應用十分廣泛的技術。機器人技術在工程中的應用日益廣泛，即工業機器人。其中研究比較早，目前工程界應用比較成熟的是串聯機器人。它是一種能夠自動定位控制、多自由度、各大大小小的機械臂以串聯的形式聯接起來。這種典型的開環機構具有結構簡單，成本低，控制簡單，運動空間大等優點。但伴隨著其結構特性的缺點則是承載能力有限、機構剛度差，各運動機構疊加誤差大，運動慣性大。而在80年代以來隨機計算機技術的發展，并聯機器人的研究成為了新的熱點。并聯機器人與串聯機器人正好可以形成互補，它的優點極為串聯機器人的缺點，相應它的缺點則是串聯機器人的優點，因此并聯機器人在飛行模擬器，空間對接器、裝配生產線等需要高精度，高穩定性，高速的場合應用越來越廣泛。但串聯機器人與并聯機器人的基礎理論基本一樣，所以文章以六自由度并聯機器人最典型的結構——Stewart平臺簡析六自由度機器手入手，在運動學的位置正反解。文章總結的內容中略去了有關機器人的數學基礎知識和基礎力學知識，假設已經具備一定的矢量代數，矩陣論，和工學力學知識，直接進入對機器人本身的運動進行了分析。并約定，一般黑色字體變量為矢量，非黑字體變量為標量。

1 六自由度并聯機器人運動學

研究操作手的幾何學要區分兩個問題，即在同一系統下的運動學正問題和逆問題，所謂的正問題就是給定關節變量前提下，求機器人執行終端的位姿，這個求解過程就叫正解。而逆問題反過來，設定定機器人執行終端的位置，反求各關節變量，這個求解過程叫反解。

見圖1a所示并聯操作手，關于這個并聯機構的反解其實是容易的，這里不再敘述，有興趣者可參閱本文列出得參考文獻。現在主要對其行運動學正問題分析，但在應用到如圖1的并聯機器人中時，他的正解運動學卻并不那么簡單。

在圖1a中，考慮三角形AiSiBi，i=1～3，角標i表示第i對腿。當6個腿的長度固定，把平臺M去掉，三角形AiSiBi只能繞AiBi軸旋轉。這樣，我們可以用一個長度為li的腿代替長度為qia和qib的一對腿，這條腿通過一個繞AiBi的轉動關節和六角基座平臺β相連接。結構簡化后的結果如圖2所示，它在運動學上的等價于圖1a的原來結構。

圖1 6自由度飛行器模擬器

a）基本結構 b）三角形活動平臺、六角形固定平臺

然后引入坐標系Fi，它的原點設在第i個腿和基座平臺β連接的點口Oi處，并作如下約定：

對i=1，2，3，

Oi是連接旋轉關節的中心的集合；

Xi的方向是從Ai指向Bi

Yi的選擇是由Zi垂直于6邊形固定平臺指向上來確定的，也就是X與Y坐標向量的矢量積方向朝上。

之后來確定從六邊形的中心O到移動平臺的3個頂點S1，S2 和S3的位置矢量。

我們需要確定li和Oi。參考圖2和圖3，其中ai和bi表示Ai，和Bi相對O為原點基座坐標的矢量。對于i=1，2，3，有

因此ui是由Ai指向Bi的單位矢量。而且，原點Oi的位置由矢量oi確定，oi的表述如下：

（1）

進而，設si為在坐標系Fi（Oi，Xi，Yi，Zi）中Si的位置矢量，則

（2）

圖2 等效簡化機構

圖3 一條腿與一對腿的等效 圖4 坐標系F0與坐標系Fi的關系

現在，將一個坐標系空間F0定義為原點 O和X，Y，軸在六邊形固定平面上，其上的 Xi和 Yi的關系如圖4所示.當在坐標系F0中表達，si的形式如下

（2.3）

這里，[Ri]0是在坐標系F0中表達的從坐標系F0到坐標系Fi的旋轉矩陣，給出如下

（2.4）

參考圖（4）有

（2.5）

（2.6）

將式（4）～式（6）代入式（3）得到

（2.7）

這里，Oi由式（8）給出。

因為活動三角平臺三個頂點之間的距離是固定的，所以位置矢量S1，S2，S3必須滿足以下約束

（8a） （8b） （8c）

展開后，式（8a-c）取形式

（9a）

（9b）

（9c）

Di，Ei，Fi都是已知數據，i為1～5， 他們可以通過計算得到，經過整理后具體形式如下。

其中 ，是一個2*2反對稱矩陣，這樣三個方程已經

列好，并對應三個未知數？準1，？準2，？準3。方程數與未知數數量一致，在理論上是可以有解的，但這個數據運算也好，方程本身的復雜度也好，要把它迅速正確求出代數解是很困難的。如果？準1，？準2，？準3三個角符合實際情況的解（0～？仔）能夠得到，則通過式（7）可得到確定三角運動平臺三個頂點的矢量，從而根據三點確定一面的原理，來確定并聯機器人執行端（三角活動平臺）的空間位姿。接下去就是如何快速正確地解這方程。

2 仿真程序

2.1 相關仿真參數簡述

按照上述原理，編寫計算程序，實現坐標顯示和仿真圖示。如圖5a所示，綠色的正六邊形代表固定底盤，其大小由邊長b確定。上方正三角形是活動平臺，其大小由邊長a確定。L1～L6表示六條機器腿的長度，同一顏色為一組。a1～a3表示活動平臺三個頂點，其坐標通過計算已顯示在左上角。通過計算還可以識別活動平臺法線朝向，如果是朝上則顯示青色（如圖5a），如果朝下則顯示品紅色（如圖5b），一般多數Stewart平臺的活動平臺正常使用情況桿多為法線方向一致朝上。而在此處是為了分析活動平臺所有可能的情況而設計的仿真程序。

除此之外還可以利用圖形界面的放大和旋轉功能來觀察各桿件之間的位置關系，從而判斷其互相干涉情況。

2.2 仿真過程羅列

請看圖6。整個仿真過程，分為仿真計算、仿真圖示兩個大部分。其中仿真計算是最根本，而且執行過程相對耗時，仿真圖示是根據初始輸入參數把計算結果來顯示出來。

圖6 仿真計算結構簡圖

2.3 仿真計算結構

為了使得整個仿真過程快速度，作者采用了數值迭代法。因為在理論上采用符號計算（復數域求解）是最全最精確的，而事實上在matlab中符號計算對于參數較多，相對復雜的方程組，計算相當耗時。而且返回的結果不一定齊全。

為了使仿真計算結果沒有遺漏，作者在數值迭代計算中合理分布了三個方程共9個初值，使得計算結果沒有遺漏，并且避免三角函數因周期而產生的重復解。

為了使仿真計算結果符合客觀實際情況，作者采用了數值篩選，目的之一是去除不符合客觀物理空間復數解。目的之二是去除不符合并聯機構實際結構的負數解。

整個計算機構請看圖7所示。中間計算解值是指圖（3）中α1～α3三個角度。

圖3.3仿真計算結構

2.4 仿真舉例

當輸入參數為a=6，b=9，L1=8，L2=8，L3=8，L4=8，L5=13，L6=15.時候仿真結果如下，見圖8，結果顯示，本初始條件系下，并聯結構有兩個位姿解，其中一個法線朝上，一個法線朝下。相應的活動平臺頂點坐標也顯示在左上角。

3 結束語

通過對并聯機構運動學的分析，設計用來求并聯機構正解位置的仿真程序，這一來不但可以快速正確地求得在不同機構參數下的位置解，而且可以通過圖示清楚直觀地了解各工作臺面的和機器腿在三維空間中的實際姿態。

參考文獻

[1]宋偉剛.機器人學[J].科學出版社，2007.

[2]黃真，孔令富，方躍法.并聯機器人機構學理論及控制[J].機械工業出版社，1997.

[3]唐錫寬，金德聞.機械動力學[J].高等教育出版社，1983.endprint

摘 要：利用剛體空間六個自由度運動原理，以Stewart平臺為例，介紹了并聯機構正反解的基本原理。并針對并聯機構更困難的正解問題，利用Matlab設計一個圖形加數字坐標仿真程序，可以方便地求得并聯機構符合實際客觀條件的位置解，并直觀地觀察各桿件之間的空間關系。

關鍵詞：正解；反解；坐標；位姿；仿真

引言

機器人從20世紀誕生以來，以迅速成為一門應用十分廣泛的技術。機器人技術在工程中的應用日益廣泛，即工業機器人。其中研究比較早，目前工程界應用比較成熟的是串聯機器人。它是一種能夠自動定位控制、多自由度、各大大小小的機械臂以串聯的形式聯接起來。這種典型的開環機構具有結構簡單，成本低，控制簡單，運動空間大等優點。但伴隨著其結構特性的缺點則是承載能力有限、機構剛度差，各運動機構疊加誤差大，運動慣性大。而在80年代以來隨機計算機技術的發展，并聯機器人的研究成為了新的熱點。并聯機器人與串聯機器人正好可以形成互補，它的優點極為串聯機器人的缺點，相應它的缺點則是串聯機器人的優點，因此并聯機器人在飛行模擬器，空間對接器、裝配生產線等需要高精度，高穩定性，高速的場合應用越來越廣泛。但串聯機器人與并聯機器人的基礎理論基本一樣，所以文章以六自由度并聯機器人最典型的結構——Stewart平臺簡析六自由度機器手入手，在運動學的位置正反解。文章總結的內容中略去了有關機器人的數學基礎知識和基礎力學知識，假設已經具備一定的矢量代數，矩陣論，和工學力學知識，直接進入對機器人本身的運動進行了分析。并約定，一般黑色字體變量為矢量，非黑字體變量為標量。

1 六自由度并聯機器人運動學

研究操作手的幾何學要區分兩個問題，即在同一系統下的運動學正問題和逆問題，所謂的正問題就是給定關節變量前提下，求機器人執行終端的位姿，這個求解過程就叫正解。而逆問題反過來，設定定機器人執行終端的位置，反求各關節變量，這個求解過程叫反解。

見圖1a所示并聯操作手，關于這個并聯機構的反解其實是容易的，這里不再敘述，有興趣者可參閱本文列出得參考文獻。現在主要對其行運動學正問題分析，但在應用到如圖1的并聯機器人中時，他的正解運動學卻并不那么簡單。

在圖1a中，考慮三角形AiSiBi，i=1～3，角標i表示第i對腿。當6個腿的長度固定，把平臺M去掉，三角形AiSiBi只能繞AiBi軸旋轉。這樣，我們可以用一個長度為li的腿代替長度為qia和qib的一對腿，這條腿通過一個繞AiBi的轉動關節和六角基座平臺β相連接。結構簡化后的結果如圖2所示，它在運動學上的等價于圖1a的原來結構。

圖1 6自由度飛行器模擬器

a）基本結構 b）三角形活動平臺、六角形固定平臺

然后引入坐標系Fi，它的原點設在第i個腿和基座平臺β連接的點口Oi處，并作如下約定：

對i=1，2，3，

Oi是連接旋轉關節的中心的集合；

Xi的方向是從Ai指向Bi

Yi的選擇是由Zi垂直于6邊形固定平臺指向上來確定的，也就是X與Y坐標向量的矢量積方向朝上。

之后來確定從六邊形的中心O到移動平臺的3個頂點S1，S2 和S3的位置矢量。

我們需要確定li和Oi。參考圖2和圖3，其中ai和bi表示Ai，和Bi相對O為原點基座坐標的矢量。對于i=1，2，3，有

因此ui是由Ai指向Bi的單位矢量。而且，原點Oi的位置由矢量oi確定，oi的表述如下：

（1）

進而，設si為在坐標系Fi（Oi，Xi，Yi，Zi）中Si的位置矢量，則

（2）

圖2 等效簡化機構

圖3 一條腿與一對腿的等效 圖4 坐標系F0與坐標系Fi的關系

現在，將一個坐標系空間F0定義為原點 O和X，Y，軸在六邊形固定平面上，其上的 Xi和 Yi的關系如圖4所示.當在坐標系F0中表達，si的形式如下

（2.3）

這里，[Ri]0是在坐標系F0中表達的從坐標系F0到坐標系Fi的旋轉矩陣，給出如下

（2.4）

參考圖（4）有

（2.5）

（2.6）

將式（4）～式（6）代入式（3）得到

（2.7）

這里，Oi由式（8）給出。

因為活動三角平臺三個頂點之間的距離是固定的，所以位置矢量S1，S2，S3必須滿足以下約束

（8a） （8b） （8c）

展開后，式（8a-c）取形式

（9a）

（9b）

（9c）

Di，Ei，Fi都是已知數據，i為1～5， 他們可以通過計算得到，經過整理后具體形式如下。

其中 ，是一個2*2反對稱矩陣，這樣三個方程已經

列好，并對應三個未知數？準1，？準2，？準3。方程數與未知數數量一致，在理論上是可以有解的，但這個數據運算也好，方程本身的復雜度也好，要把它迅速正確求出代數解是很困難的。如果？準1，？準2，？準3三個角符合實際情況的解（0～？仔）能夠得到，則通過式（7）可得到確定三角運動平臺三個頂點的矢量，從而根據三點確定一面的原理，來確定并聯機器人執行端（三角活動平臺）的空間位姿。接下去就是如何快速正確地解這方程。

2 仿真程序

2.1 相關仿真參數簡述

按照上述原理，編寫計算程序，實現坐標顯示和仿真圖示。如圖5a所示，綠色的正六邊形代表固定底盤，其大小由邊長b確定。上方正三角形是活動平臺，其大小由邊長a確定。L1～L6表示六條機器腿的長度，同一顏色為一組。a1～a3表示活動平臺三個頂點，其坐標通過計算已顯示在左上角。通過計算還可以識別活動平臺法線朝向，如果是朝上則顯示青色（如圖5a），如果朝下則顯示品紅色（如圖5b），一般多數Stewart平臺的活動平臺正常使用情況桿多為法線方向一致朝上。而在此處是為了分析活動平臺所有可能的情況而設計的仿真程序。

除此之外還可以利用圖形界面的放大和旋轉功能來觀察各桿件之間的位置關系，從而判斷其互相干涉情況。

2.2 仿真過程羅列

請看圖6。整個仿真過程，分為仿真計算、仿真圖示兩個大部分。其中仿真計算是最根本，而且執行過程相對耗時，仿真圖示是根據初始輸入參數把計算結果來顯示出來。

圖6 仿真計算結構簡圖

2.3 仿真計算結構

為了使得整個仿真過程快速度，作者采用了數值迭代法。因為在理論上采用符號計算（復數域求解）是最全最精確的，而事實上在matlab中符號計算對于參數較多，相對復雜的方程組，計算相當耗時。而且返回的結果不一定齊全。

為了使仿真計算結果沒有遺漏，作者在數值迭代計算中合理分布了三個方程共9個初值，使得計算結果沒有遺漏，并且避免三角函數因周期而產生的重復解。

為了使仿真計算結果符合客觀實際情況，作者采用了數值篩選，目的之一是去除不符合客觀物理空間復數解。目的之二是去除不符合并聯機構實際結構的負數解。

整個計算機構請看圖7所示。中間計算解值是指圖（3）中α1～α3三個角度。

圖3.3仿真計算結構

2.4 仿真舉例

當輸入參數為a=6，b=9，L1=8，L2=8，L3=8，L4=8，L5=13，L6=15.時候仿真結果如下，見圖8，結果顯示，本初始條件系下，并聯結構有兩個位姿解，其中一個法線朝上，一個法線朝下。相應的活動平臺頂點坐標也顯示在左上角。

3 結束語

通過對并聯機構運動學的分析，設計用來求并聯機構正解位置的仿真程序，這一來不但可以快速正確地求得在不同機構參數下的位置解，而且可以通過圖示清楚直觀地了解各工作臺面的和機器腿在三維空間中的實際姿態。

參考文獻

[1]宋偉剛.機器人學[J].科學出版社，2007.

[2]黃真，孔令富，方躍法.并聯機器人機構學理論及控制[J].機械工業出版社，1997.

[3]唐錫寬，金德聞.機械動力學[J].高等教育出版社，1983.endprint