機器人學—機器人位置運動學（3）課程回顧機器人位置的正逆運動學

直角坐標、圓柱坐標、球坐標、鏈式坐標機器人姿態(tài)的正逆運動學

滾動、俯仰、偏航角；歐拉角課程回顧包含：2個線性平移運動，1個旋轉運動。

圓柱坐標課程回顧球坐標包含：1個線性運動，2個旋轉運動。課程回顧滾轉角(roll)、俯仰角(pitch)和偏航角(yaw)表示RPY姿態(tài)變化的矩陣：課程回顧滾轉角、俯仰角和偏航角最終姿態(tài)（已知量）待求量用的逆左乘上式兩邊，可得解耦方法：注：這樣等式左邊只含fa，而右邊不含fa，從而實現(xiàn)解耦課程回顧進行矩陣相乘后得滾轉角、俯仰角和偏航角根據(jù)矩陣兩邊對應參數(shù)一一相等可求解出fn，fo，fa課程回顧歐拉角的逆運動學求解與RPY非常類似?？梢允箽W拉方程的兩邊左乘來消去其中一邊的?。讓兩邊的對應元素相等，就可以得到以下方程：歐拉角課程回顧歐拉角課程回顧

D-H坐標

D-H參數(shù)、D-H矩陣機器人正逆運動學分析

運動學方程、位置學運動仿真課程回顧①每個連桿指定坐標系；②確定從一個連桿到另一個相鄰連桿的齊次變換矩陣；③將所有變化結合起來，確定末端桿相對于基座的總變化；④建立運動學方程。思路：用齊次變換矩陣描述兩個相鄰連桿坐標系的空間位姿關系。D-H坐標法建模連桿關節(jié)連桿關節(jié)課程回顧基本步驟建立連桿坐標系列出相鄰坐標系的位姿關系（DH參數(shù)）計算變換矩陣在機器人的基座上，可以從第1個關節(jié)開始變換到第2個關節(jié)，然后到第3個關節(jié)，等等，直到機器人手和末端執(zhí)行器。在機器人基座和手之間總的變換為：（n為關節(jié)數(shù)）qdaa0-11-22-3…n-1~nD-H參數(shù)表1）指定x和z軸時，軸線方向可任選，機器人總變換是一樣的，只是個別矩陣和變量會有差別。2）為了便于理解，可以使用附加坐標系，但未知變量個數(shù)與自由度數(shù)要一致。3）D-H法不使用沿y軸的變換，如果發(fā)現(xiàn)需要沿y軸的變換，則可能出現(xiàn)錯誤，或需要在它們之間附加一個坐標系。4）在實際工程應用中，由于制造和裝配誤差，可能使本來平行的z軸之間存在偏角，需要沿y軸變換，D-H法不能表示該誤差。機器人正運動學方程課程回顧5）也可以使用其它表示方法來建立機器人運動學方程，但為了后續(xù)微分運動學和動力學分析，D-H法更加方便。6）推導的都是機器人基座和末端執(zhí)行器之間的變換。如需表達全局坐標系和末端執(zhí)行器之間的變換，只需左乘基座坐標系與全局坐標系之間的變換。7）D-H法適用任何關節(jié)和連桿構型，如果存在連桿扭曲或關節(jié)偏移，坐標系法將不能使用，這是D-H法的優(yōu)勢。1、機器人正運動學方程課程回顧逆運動學分析：已知末端執(zhí)行器的方向和位置，求解各關節(jié)變量。目的是將末端執(zhí)行器在操作空間的運動轉化為關節(jié)空間運動。2、機器人逆運動學方程課程回顧在一些較為復雜的場景，需要機器人通過逆運動學實時規(guī)劃軌跡3機器人位置運動學一、機器人矩陣表示二、齊次變換矩陣三、機器人坐標變換的表示四、機器人正、逆運動學分析五、機器人位置運動學仿真六、設計項目：四足機器人的運動學分析準確表示機器人在空間中的位姿一、機器人矩陣表示1.1空間向量基礎1、坐標系與點的坐標表示直角坐標系（笛卡爾坐標系）

也可以寫成列向量的形式：

一、機器人矩陣表示2、三維向量及其運算

向量三角形法則

一、機器人矩陣表示1.2位姿矩陣1、位置矩陣

一般情況下，機器人可以認為是由多個剛體所構成，因此機器人的位姿可以通過構成該機器人的全部剛體的位姿來表述。這樣機器人空間位姿的描述問題可以轉換為單個剛體空間位姿的描述問題。

表示。

左上標的A代表參考坐標系{A}，右下標的B代表剛體坐標系{B}位置矩陣描述：

一、機器人矩陣表示2、姿態(tài)矩陣兩個坐標系原點相同，但三個坐標軸的方向不同

矩陣三個互相垂直的單位列矢量滿足：

可知姿態(tài)矩陣為正交矩陣，滿足：一、機器人矩陣表示3、位姿矩陣位置+姿態(tài)將位置矩陣與姿態(tài)矩陣結合描述剛體B的位姿：

3機器人位置運動學一、機器人矩陣表示二、齊次變換矩陣三、機器人坐標變換的表示四、機器人正、逆運動學分析五、機器人位置運動學仿真六、設計項目：四足機器人的運動學分析二、齊次變換矩陣1、齊次坐標

二、齊次變換矩陣2、齊次變換矩陣位姿矩陣

齊次矩陣

最下面一行可以看作是坐標系{B}在參考坐標系{A}中的三根坐標軸向量及坐標原點向量的比例系數(shù)。優(yōu)點：方陣的逆矩陣計算較為容易；兩個相乘的矩陣為方陣的話，就不需要考慮行數(shù)與列數(shù)的問題，可以任意相乘。在坐標變換中，需要將許多矩陣進行連乘，因此使用方陣更為方便。3機器人位置運動學一、機器人矩陣表示二、齊次變換矩陣三、機器人坐標變換的表示四、機器人正、逆運動學分析五、機器人位置運動學仿真六、設計項目：四足機器人的運動學分析三、機器人坐標變換的表示1、平移變換姿態(tài)相同，原點不同平移矢量

平移變換方程齊次坐標、齊次變換矩陣

三、機器人坐標變換的表示2、旋轉變換姿態(tài)不同，原點相同

旋轉變換方程

一些常用機器人繞x、y、z軸旋轉角度θ的旋轉公式：

三、機器人坐標變換的表示姿態(tài)不同，原點不同

3、復合變換（i）矢量計算法{A}指向{B}的向量（ii）采用齊次變換矩陣所有變換都可以轉換為矩陣相乘

三、機器人坐標變換的表示3、復合變換

基于上述兩種思路求解（i）矢量計算法

三、機器人坐標變換的表示3、復合變換

基于上述兩種思路求解（i）矢量計算法

④代入計算可得

三、機器人坐標變換的表示3、復合變換

（ii）采用齊次變換矩陣

三、機器人坐標變換的表示3、復合變換（ii）采用齊次變換矩陣由于所有的變化都是相對于參考坐標系的，總的變換矩陣相當于把每步的變換矩陣依次左乘。三、機器人坐標變換的表示4、相對于當前坐標系的變換

需要注意的是，相對于參考坐標系的坐標變換，都是依次左乘每個變換矩陣得到的。如果相對于運動坐標系或當前坐標系的變換，則需要右乘變換矩陣。復合坐標變換在計算時要注意坐標變換的順序?？偟淖儞Q矩陣為：

3機器人位置運動學一、機器人矩陣表示二、齊次變換矩陣三、機器人坐標變換的表示四、機器人正、逆運動學分析五、機器人位置運動學仿真六、設計項目：四足機器人的運動學分析四、機器人正運動學分析1、位置正運動學機器人正運動學即已知機器人各個關節(jié)的變量，求出機器人末端執(zhí)行器的位姿。（i）直角坐標在直角坐標系中，表示機器人手的正運動學變換矩陣為對于所有驅動機構都是線性的機器人，其有3個沿x、y和z軸的運動，可以用直角坐標對其正運動學矩陣進行表示。參考坐標系機器人手坐標系

四、機器人正運動學分析1、位置正運動學（ii）圓柱坐標當機器人具備兩個線性平移運動和一個旋轉運動時，可用圓柱坐標對其進行描述。參考坐標系機器人手坐標系

這三個變換都是相對參考坐標系，依次左乘；

四、機器人正運動學分析變換以后到達某一位置和姿態(tài)。此時在經(jīng)過繞當前坐標系的一次1、位置正運動學例3.4圓柱坐標機器人經(jīng)過了

旋轉變換后的位姿矩陣如下，求該旋轉變換前的機器人位姿矩陣。

四、機器人正運動學分析變換以后到達某一位置和姿態(tài)。此時在經(jīng)過繞當前坐標系的一次1、位置正運動學例3.4圓柱坐標機器人經(jīng)過了

旋轉變換后的位姿矩陣如下，求該旋轉變換前的機器人位姿矩陣。

四、機器人正運動學分析1、位置正運動學機器人正運動學即已知機器人各個關節(jié)的變量，求出機器人末端執(zhí)行器的位姿。（iii）球坐標球坐標機器人由一個線性運動和兩個旋轉運動組成。

依次左乘；

四、機器人正運動學分析1、位置正運動學

解：

可得：

則：

又有：

因此：

四、機器人正運動學分析2、姿態(tài)正運動學當機器人末端運動坐標系已經(jīng)到達期望的位置上，但其末端仍然不是期望的姿態(tài)。那么，就需要在不改變位置的情況下，對末端坐標系進行旋轉，時使其達到期望的姿態(tài)。這時只能基于當前坐標系進行變換，繞參考坐標系進行變換會改變末端坐標系位置。（1）滾轉角(roll)、俯仰角(pitch)和偏航角(yaw)

RPY旋轉四、機器人正運動學分析2、姿態(tài)正運動學（1）滾轉角(roll)、俯仰角(pitch)和偏航角(yaw)由于是繞末端坐標系運動，即當前坐標系，因此需將三個變換矩陣右乘得到總變換矩陣：

該坐標系相對于參考坐標系的最終位姿是表示位置變化的矩陣和RPY矩陣相乘的結果。例如，假設一個機器人是根據(jù)圓柱坐標與RPY進行設計的，那么這個機器人的位姿變化矩陣為：

四、機器人正運動學分析2、姿態(tài)正運動學（2）歐拉角

右乘

歐拉旋轉Denavit和Hartenberg于1955年提出的機器人機構建模和分析的方法（AKinematicNotationforLower-PairMechanismsBasedonMatrices.JournalofAppliedMechanics，影響因子2.1，被引量3816）。簡單，通用性好；成為串聯(lián)機器人的標準建模方法。第一臺工業(yè)機器人DH坐標法是工業(yè)機器人運動學建模的基礎，至今仍在廣泛使用四、機器人D-H坐標法概述1、D-H坐標法由來四、機器人D-H坐標法概述①每個連桿指定坐標系；②確定從一個連桿到另一個相鄰連桿的齊次變換矩陣；③將所有變化結合起來，確定末端桿相對于基座的總變化；④建立運動學方程。思路：用齊次變換矩陣描述兩個相鄰連桿坐標系的空間位姿關系。2、D-H坐標法思路連桿關節(jié)連桿關節(jié)四、D-H坐標建立方法1、基本步驟建立連桿坐標系列出相鄰坐標系的位姿關系（DH參數(shù)）計算變換矩陣連桿n關節(jié)

n＋1關節(jié)n連桿0（基座）連桿1關節(jié)1連桿2連桿3關節(jié)2關節(jié)3

編號：從基座開始，連桿從0開始標記，關節(jié)從1開始標記。除基座和末端執(zhí)行器外，每個連桿都有兩個關節(jié)。連桿n的前端為關節(jié)n，后端為關節(jié)n+1。

四、D-H坐標建立方法在連桿n的末端設立一個固連坐標系xnynznzn軸:沿(n+1)關節(jié)軸方向（轉動/移動軸向）xn軸:zn-1與zn之間的公垂線，方向由zn-1指向znyn軸:右手法則確定（i）建立連桿坐標四、D-H坐標建立方法1、基本步驟（i）建立連桿坐標如果兩個關節(jié)軸平行，則xn軸可選在與兩個關節(jié)軸垂直的任意位置。如果兩個關節(jié)軸相交，則xn軸可定義為沿著zn-1

zn的方向，而原點位于交點。zn-1znxnxnzn-1znxn四、D-H坐標建立方法1、基本步驟（ii）列出D-H參數(shù)zn+1與zn距離:an+1;連桿

n+1關節(jié)

n+1關節(jié)

n＋2zn+1xn+1dn+1an+1znxn

n+1

n+1D-H參數(shù)xn+1與xn夾角:

n+1；xn+1與xn距離:dn+1;zn+1與zn夾角:

n+1;四、D-H坐標建立方法1、基本步驟（ii）列出D-H參數(shù)每個連桿上的坐標系建好以后，可以建立相鄰兩個坐標系之間齊次變換矩陣。第n+1個坐標系可看作是第n個坐標系經(jīng)過旋轉和平移后得到。連桿

n+1關節(jié)n+1關節(jié)

n＋2znxnynzn+1xn+1yn+1變換矩陣T四、D-H坐標建立方法1、基本步驟（ii）列出D-H參數(shù)①

將第n個坐標系的xn軸繞zn旋轉

n+1角，使xn軸與xn+1軸同向對應的變換矩陣為xn+1連桿

n+1關節(jié)

n+1關節(jié)n＋2zn+1On+1dn+1an+1znxn

n+1

n+1四、D-H坐標建立方法1、基本步驟（ii）列出D-H參數(shù)

②將第n個坐標系沿著zn軸平移dn+1

，使得xn與xn+1重合xn+1連桿

n+1關節(jié)

n+1關節(jié)

n＋2zn+1dn+1an+1znxn

n+1

n+1對應的變換矩陣為四、D-H坐標建立方法1、基本步驟（ii）列出D-H參數(shù)③將第n個坐標系沿著xn+1軸平移an+1，使得原點On與On+1重合對應的變換矩陣為xn+1連桿n+1關節(jié)

n+1關節(jié)

n＋2zn+1dn+1an+1znxn

n+1

n+1四、D-H坐標建立方法1、基本步驟（iii）計算變換矩陣④將第n個坐標系繞xn+1軸旋轉

n+1角，使兩個坐標系完全重合對應的變換矩陣為xn+1連桿

n+1關節(jié)n+1關節(jié)n＋2zn+1dn+1an+1znxn

n+1

n+1四、D-H坐標建立方法總變換矩陣為？？or=Rot(z,qn+1)

╳Trans(0,0,dn+1)╳Trans(an+1,0,0)╳Rot(x,an+1)D-H變換矩陣=Rot(x,an+1)╳Trans(an+1,0,0)╳Trans(0,0,dn+1)╳Rot(z,qn+1)

√╳每步變換都是相對于當前坐標系進行，因此右乘xn+1連桿

n+1關節(jié)

n+1關節(jié)n＋2zn+1On+1dn+1an+1znxn

n+1

n+1四、D-H坐標建立方法1、基本步驟例如關節(jié)2與關節(jié)3之間的變換：在機器人的基座上，可以從第1個關節(jié)開始變換到第2個關節(jié)，然后到第3個關節(jié)，等等，直到機器人手和末端執(zhí)行器。在機器人基座和手之間總的變換為：（n為關節(jié)數(shù)）四、D-H坐標建立方法2、D-H坐標建立方法：D-H參數(shù)、D-H矩陣qdaa0-11-22-3…n-1~nD-H參數(shù)表1、機器人正運動學方程

例3.6如下圖所示的簡單2軸平面機器人，根據(jù)D-H表示法，建立必要的坐標系，填寫D-H參數(shù)表，導出該機器人的正運動學方程。四、基于D-H坐標的運動學方程基座（連桿0）連桿1連桿2關節(jié)1關節(jié)2zUxUyUa1a2步驟：1、機器人正運動學方程四、基于D-H坐標的運動學方程zUxUyU按D-H法建立連桿坐標列D-H參數(shù)表D-H矩陣z0x0連桿末端建立固連坐標系；zn軸方向沿轉軸方向（本例中垂直于紙面）；xn軸:zn-1與zn之間的公垂線，方向由zn-1指向znzHxHz1x1步驟：qdaa0-1q10a101-Hq20a201、機器人正運動學方程四、基于D-H坐標的運動學方程按D-H法建立連桿坐標列D-H參數(shù)表D-H矩陣z0x0z1x1zHxH連桿

n+1關節(jié)

n+1關節(jié)

n＋2zn+1xn+1On+1dn+1an+1znOnxn

n+1

n+1Hna1a2q1q2步驟：1、機器人正運動學方程四、基于D-H坐標的運動學方程按D-H法建立連桿坐標列D-H參數(shù)表D-H矩陣1、機器人正運動學方程四、基于D-H坐標的運動學方程正運動學：矩陣中q1與q2為關節(jié)變量參數(shù)，確定q1、q2即得到末端桿相對于基座的位姿。1、機器人正運動學方程四、基于D-H坐標的運動學方程你能看懂天宮號操作臺嗎？《地心引力》電影中，美國宇航員乘坐中文操作系統(tǒng)的中國飛船返回地球，如今已成部分現(xiàn)實。1、機器人正運動學方程四、基于D-H坐標的運動學方程

MatlabRoboticsToolbox提供了Link函數(shù)，可利用DH參數(shù)表建立可視化機器人系統(tǒng)：qdaa0-1q10a101-Hq20a20%DH參數(shù)為thetaDAalphaL1=Link([0010],'standard');L2=Link([0010],'standard');robot=SerialLink([L1L2],'name','robot');robot.display();theta=[00];robot.plot(theta);robot.teach;

例3.7為下圖中的3自由度機器人指定坐標系，并導出該機器人的正運動學方程。1、機器人正運動學方程四、基于D-H坐標的運動學方程1、機器人正運動學方程四、基于D-H坐標的運動學方程qdaa0-1q10a101-290+q200902-Hq3d300z0x0zHxH如何確定x2y2z2：z2：第3個關節(jié)的轉軸方向x2：兩個關節(jié)軸相交，xn軸可定義為沿著zn-1

zn的方向，而原點位于交點。x2x1z1z2a1d3q1q2q31、機器人正運動學方程四、基于D-H坐標的運動學方程注意：繞z1的旋轉角是90°+q2，而不是q2，這是因為當q2為0時，x1與x2之間有個90°夾角，也是為了確定機器人的零位。z0x0z1x1zHxHq1q2q3z2x2在歸零位置的機器人1、機器人正運動學方程四、基于D-H坐標的運動學方程1、機器人正運動學方程四、基于D-H坐標的運動學方程

其中

例3.8對圖所示的四軸機器人指定坐標系，寫出坐標變換方程（在全局坐標系中的情況）。1、機器人正運動學方程四、基于D-H坐標的運動學方程D-H參數(shù)θdaα0-1θ10l3901-2θ2-l40902-Hθ3l5+l600四、基于D-H坐標的運動學方程1、機器人正運動學方程四、基于D-H坐標的運動學方程華為天才少年：獨造神奇機械臂，遠程操控縫合葡萄皮。6自由度機器人1、機器人正運動學方程四、基于D-H坐標的運動學方程6自由度機器人能夠完全確定空間位姿，是應用最多的一種工業(yè)機器人。

例3.9對于下圖的6自由度機器人，根據(jù)D-H表示法，建立該機器人所需的坐標系，填寫相應的參數(shù)表并導出其正運動學方程。1、機器人正運動學方程四、基于D-H坐標的運動學方程

解：1、機器人正運動學方程四、基于D-H坐標的運動學方程x0z0x1z1x2z2x3z3x4z4x5z5x6z6a2a3a4θdaα0-1θ100901-2θ20a202-3θ30a303-4θ40a4-904-5θ500905-6θ6000D-H參數(shù)

解：1、機器人正運動學方程四、基于D-H坐標的運動學方程機器人基座和手之間的總變換為：

解：1、機器人正運動學方程四、基于D-H坐標的運動學方程1）指定x和z軸時，軸線方向可任選，機器人總變換是一樣的，只是個別矩陣和變量會有差別。2）為了便于理解，可以使用附加坐標系，但未知變量個數(shù)與自由度數(shù)要一致。3）D-H法不使用沿y軸的變換，如果發(fā)現(xiàn)需要沿y軸的變換，則可能出現(xiàn)錯誤，或需要在它們之間附加一個坐標系。4）在實際工程應用中，由于制造和裝配誤差，可能使本來平行的z軸之間存在偏角，需要沿y軸變換，D-H法不能表示該誤差。1、機器人正運動學方程四、基于D-H坐標的運動學方程5）也可以使用其它表示方法來建立機器人運動學方程，但為了后續(xù)微分運動學和動力學分析，D-H法更加方便。6）到目前為止，所有的例子中，推導的都是機器人基座和末端執(zhí)行器之間的變換。如需表達全局坐標系和末端執(zhí)行器之間的變換，只需左乘基座坐標系與全局坐標系之間的變換。7）D-H法適用任何關節(jié)和連桿構型，如果存在連桿扭曲或關節(jié)偏移，坐標系法將不能使用，這是D-H法的優(yōu)勢。1、機器人正運動學方程四、基于D-H坐標的運動學方程1、機器人正運動學方程四、基于D-H坐標的運動學方程斯坦福大學的機器人研究shakey，1966年（世界上第一款智能機器人）經(jīng)典斯坦福機械手，1970s最佳機器人控制論文獎「會滾動的手」,2021

例3.10斯坦福機械手，指定坐標系并填寫參數(shù)表。1、機器人正運動學方程四、基于D-H坐標的運動學方程x0z0z1x1x2z2x3z3x4z4x5z5x6z6d2d3θdaα0-1θ100-901-2θ2d20902-30d3003-4θ400-904-5θ500905-6θ60001、機器人正運動學方程四、基于D-H坐標的運動學方程四、基于D-H坐標的運動學方程1、機器人正運動學方程1、機器人正運動學方程四、基于D-H坐標的運動學方程θdaα0-1θ100-901-2θ2d20902-30d3003-4θ400-904-5θ500905-6θ6000已知未知逆解未知已知正解逆運動學分析：已知末端執(zhí)行器的方向和位置，求解各關節(jié)變量。目的是將末端執(zhí)行器在操作空間的運動轉化為關節(jié)空間運動。2、機器人逆運動學方程四、基于D-H坐標的運動學方程在一些較為復雜的場景，需要機器人通過逆運動學實時規(guī)劃軌跡例3.11找出例3.6中機器人關節(jié)變量的符號表達式。2、機器人逆運動學方程四、基于D-H坐標的運動學方程z0x0z1x1zHxHa1a2q1q2zUxUyU解：列出機器人正運動學方程，假設要將機器人放置到n,o,a,p向量所給定的期望位置和姿態(tài)：

2、機器人逆運動學方程四、基于D-H坐標的運動學方程代數(shù)解法：使方程兩邊的矩陣分量相等對于正向運動學，一旦關節(jié)變量已知，末端執(zhí)行器的位置和旋轉矩陣也是唯一確定的。但逆運動學的情況要復雜得多，因為求解的式子大多是非線性，可能存在多解或無解。

其它解法：方程兩邊右乘2、機器人逆運動學方程四、基于D-H坐標的運動學方程

其它解法：方程兩邊右乘2、機器人逆運動學方程四、基于D-H坐標的運動學方程A1只含q1，A2只含q2，通過上述方式實現(xiàn)q1、q2解耦，可以簡化計算

在例3.9的基礎上討論串聯(lián)式機器人臂的逆運動學一般解法2、機器人逆運動學方程四、基于D-H坐標的運動學方程后文用[RHS](Right-HandSide)簡化表示該矩陣代數(shù)解法？機器人位姿矩陣：四、基于D-H坐標的運動學方程

為求解各關節(jié)角度，選擇左乘方程兩邊的矩陣，首先用左乘兩邊的矩陣得：2、機器人逆運動學方程四、基于D-H坐標的運動學方程得到：嘗試尋找對應元素進行求解：四、基于D-H坐標的運動學方程繼續(xù)左乘矩陣的逆，考慮關節(jié)2，3，4是平行的，單獨左乘、不到解耦結果，需要左乘到四、基于D-H坐標的運動學方程代回最初的矩陣等式：

}消去C2由于q234為已知量，因此可求q3四、基于D-H坐標的運動學方程將q3結果代回前式:

}2、機器人逆運動學方程

}四、基于D-H坐標的運動學方程q2,q3,q234均為已知量q4=q234

-

q2-

q32、機器人逆運動學方程2、機器人逆運動學方程四、基于D-H坐標的運動學方程}四、基于D-H坐標的運動學方程最后左乘A5的逆解耦q6：四、基于D-H坐標的運動學方程解算步驟總結:①左乘解耦q1②繼續(xù)左乘到解耦q234③利用q234計算q2，q3，q4，q5④最后繼續(xù)左乘解耦q6有多組逆解3、機器人逆運動學編程四、基于D-H坐標的運動學方程

為使機器人按預定軌跡運動，必須在規(guī)定時間內反復計算關節(jié)變量，需要用到前面得到的逆解。中間插值點一般每秒要反算位置50~200次，如果單次計算耗時過長，則將丟失精度或無法按路徑運動。3、機器人逆運動學編程四、基于D-H坐標的運動學方程%建立機器人模型%thetadaalphaoffsetL1=Link([000pi/20]);L2=Link([000.500]);L3=Link([000.500]);L4=Link([000.2-pi/20]);L5=Link([000pi/20]);L6=Link([00000]);robot=SerialLink([L1L2L3L4L5L6],'name','6aix');T1=transl(1,0,0.5);%起始點位姿T2=transl(0.3,0.6,0.5);%終止點位姿q1=robot.ikine(T1);%根據(jù)起始點位姿反解關節(jié)角q2=robot.ikine(T2);%根據(jù)終止點位姿反解關節(jié)角[q,qd,qdd]=jtraj(q1,q2,50);%五次多項式插值利用Ma