第2章機(jī)器人運動學(xué)2.1節(jié)位姿描述與齊次變換第1-15周，星期二，16:40-18:15，（五）103機(jī)器人技術(shù)基礎(chǔ)22.1.1位姿描述2.1.2坐標(biāo)變換2.1.3其他姿態(tài)描述三角度姿態(tài)法等效軸-角法四元數(shù)

法本節(jié)目錄3大寫斜體加粗：矩陣R、T小寫斜體加粗：矢量p、默認(rèn)矢量為列向量；小寫斜體不加粗：標(biāo)量a，p左上標(biāo)：變量所在坐標(biāo)系

符號約定4

符號約定5如何描述機(jī)器人某構(gòu)件，例如末端手爪的空間狀態(tài)建立一個世界坐標(biāo)系{A}；在末端手爪某處，例如兩手指尖端中點建立一個坐標(biāo)系{B}，其原點為P；坐標(biāo)系{B}與手爪固聯(lián)，隨手爪運動；坐標(biāo)系{B}相對于坐標(biāo)系{A}的描述，就唯一確定了手爪的空間狀態(tài)——位姿P位姿描述6空間中某點P在坐標(biāo)系{A}中的描述

位置描述7

結(jié)論：矢量與某坐標(biāo)系各坐標(biāo)軸單位向量的點積，就得到矢量在該坐標(biāo)系中的表達(dá)姿態(tài)描述8

因為上述向量均為單位向量，所以：

姿態(tài)描述92025/6/1

姿態(tài)描述10將坐標(biāo)系{B}的各坐標(biāo)軸在{A}中的表達(dá)組成一個矩陣：矩陣中各元素均是{A}、{B}兩個坐標(biāo)系各坐標(biāo)軸之間夾角的余弦又稱為方向余弦矩陣（directioncosinematrix）。姿態(tài)描述11

姿態(tài)描述12將坐標(biāo)系{A}的各坐標(biāo)軸在{B}中的表達(dá)組成一個矩陣：{A}系的3個坐標(biāo)軸相對{B}系的坐標(biāo)就是其在{B}系三個坐標(biāo)軸上的投影。{A}、{B}兩坐標(biāo)系各軸夾角的大小與坐標(biāo)軸向量在哪個坐標(biāo)系表達(dá)無關(guān)姿態(tài)描述13結(jié)論：姿態(tài)描述14

在工業(yè)機(jī)器人領(lǐng)域，為了形象地描述機(jī)器人（俗稱機(jī)械臂、操作臂等，manipulator）的姿態(tài)，姿態(tài)矩陣一般寫成如下形式：a為接近矢量（approachvector），表示手爪接近物體的方向o為方位矢量（orientationvector），表示手爪中的一個手指指向另一個手指的方向n為法向矢量（normalvector）姿態(tài)描述15位置和姿態(tài)合稱位姿圖中代表手爪位姿的坐標(biāo)系{B}，可表示為：其中：表示姿態(tài)表示位置工業(yè)機(jī)器人的位姿描述姿態(tài)描述16位姿圖的說明矢量箭頭從一個坐標(biāo)原點指向另一個坐標(biāo)系原點矢量指明它表示的是箭頭處坐標(biāo)系相對于箭尾坐標(biāo)系的相對關(guān)系例如：{B}相對于{A}機(jī)器人學(xué)中，機(jī)器人末端的位姿（矩陣）通常也稱作機(jī)器人的位形（configuration）。姿態(tài)描述172.1.1位姿描述2.1.2坐標(biāo)變換2.1.3其他姿態(tài)描述三角度姿態(tài)法等效軸-角法四元數(shù)

法本節(jié)目錄18坐標(biāo)變換把一個矢量在{B}坐標(biāo)系中的表達(dá)轉(zhuǎn)換到{A}坐標(biāo)系中；矢量本身沒有變化，但是在不同坐標(biāo)系中的值不同；坐標(biāo)變換的本質(zhì)所在，即描述的是坐標(biāo)系之間的變換而不是對象本身。坐標(biāo)變換（Transformation,映射Mapping）坐標(biāo)變換19平移變換坐標(biāo)系{B}相對于{A}僅有平移：已知矢量P在{B}中的表達(dá)：則矢量P在{A}中的表達(dá)：只有{A}、{B}姿態(tài)相同時，上式才成立20旋轉(zhuǎn)變換坐標(biāo)系{B}相對于{A}僅有旋轉(zhuǎn)：由姿態(tài)矩陣的定義和性質(zhì)：可知：坐標(biāo)變換21旋轉(zhuǎn)變換已知矢量p在{B}中的表達(dá)：待求解矢量p在{A}中的表達(dá)：也即，點P在{A}坐標(biāo)系各軸上的投影可利用{A}的各坐標(biāo)軸在{B}中的表達(dá)與的點積來計算，即：只有在同一個坐標(biāo)系中表達(dá)的兩個矢量才能執(zhí)行運算。點積結(jié)果是標(biāo)量，與該矢量在哪個坐標(biāo)系表達(dá)無關(guān)！BpApBp坐標(biāo)變換22旋轉(zhuǎn)變換由于{B}相對于{A}的旋轉(zhuǎn)矩陣所以：訣竅：坐標(biāo)變換23旋轉(zhuǎn)變換——實例解：可得：

注意：繞某一軸旋轉(zhuǎn)，規(guī)定按照右手定則，逆時針為正坐標(biāo)變換24旋轉(zhuǎn)變換——實例解：

又已知點P在{B}系中的表達(dá)：求：注意：映射變換不改變向量本身，只是在不同坐標(biāo)系描述向量，或者說求向量在不同坐標(biāo)系中的坐標(biāo)坐標(biāo)變換25繞各坐標(biāo)軸的旋轉(zhuǎn)矩陣

繞z軸有夾角θ：繞x軸有夾角θ：繞y軸有夾角θ：

坐標(biāo)變換坐標(biāo)變換的一般情況26坐標(biāo)變換的一般情況

解：首先建立一個中間坐標(biāo)系{C}，它與{A}姿態(tài)相同，與{B}原點重合

顯然：于是：因為{C}與{A}僅存在平移關(guān)系，所以：

齊次變換矩陣27一般坐標(biāo)變換的表達(dá)一般情況下的坐標(biāo)變換，可由下式計算：為了使表達(dá)更簡潔，引入齊次變換矩陣：原3×1坐標(biāo)向量增加一行，變成4×1的齊次坐標(biāo)向量齊次變換矩陣的性質(zhì)28齊次變換矩陣的性質(zhì)

齊次變換：舉例29一般坐標(biāo)變換——實例

解：{B}相對于{A}的齊次變換矩陣為：其中：最后，可得：

30

逆變換解：其中旋轉(zhuǎn)矩陣部分，根據(jù)單位正交矩陣的性質(zhì)直接寫出：利用一般變換的映射公式：顯然：由此，可得：

逆變換31解：于是最后，可得

逆變換：舉例32

復(fù)合變換：連續(xù)旋轉(zhuǎn)解：

如何求{C}系相對于{A}系的姿態(tài)？

則作為將{B}系映射到{A}系中的旋轉(zhuǎn)矩陣，這樣可將參考坐標(biāo)系從{B}變到{A}，結(jié)果可變成了{(lán)C}系（相對于{A}系）的姿態(tài)。連續(xù)旋轉(zhuǎn)可通過矩陣相乘得到，即滿足旋轉(zhuǎn)矩陣的合成法則復(fù)合變換：連續(xù)旋轉(zhuǎn)33

復(fù)合變換：連續(xù)齊次變換解：

3.聯(lián)立上述兩式，得4.由此，可得注意：展開得變換方程（1）34

利用齊次變換的遞推特性，求不直接關(guān)聯(lián)兩坐標(biāo)系的關(guān)系，或未知變換。根據(jù)第1個變換路徑，可得：從第2個變換路徑，也可得：前面兩式可構(gòu)造一個變換方程：據(jù)此，可求得：變換方程（2）352025/6/1如右圖，注意{D}鄰近的兩坐標(biāo)系與{D}的相對關(guān)系與前例相反。利用齊次變換的遞推特性，求不直接關(guān)聯(lián)兩坐標(biāo)系的關(guān)系，或未知變換。

從第2個變換路徑，也可得：根據(jù)前面兩式，可求解鏈路中的其他變換，例如：變換方程（3）36

變換方程的實際用途根據(jù)右圖中的變換路徑，可得：變換方程（4）37

變換方程的實際用途根據(jù)右圖中的變換路徑，可得：式中，，為用戶給定的變換，由機(jī)器人正運動學(xué)模型得到

38【例】如下圖所示，一輪式移動機(jī)器人上搭載機(jī)械手在房間內(nèi)進(jìn)行拾取木塊的作業(yè)，天花板上安放一攝像頭用作機(jī)器人的視覺反饋系統(tǒng)。各坐標(biāo)系如圖所示，其中，{W}為參考坐標(biāo)系，{B}和{T}分別為附著在輪式移動機(jī)器人和機(jī)械手末端上的物體坐標(biāo)系，{C}為攝像頭坐標(biāo)系，{S}為附著在木塊上的物體坐標(biāo)系。通過視覺傳感器測量得到通過關(guān)節(jié)角度測量裝置標(biāo)定得到預(yù)先已知求：木塊相對機(jī)械手的位形變換方程（5）39自由矢量與線矢量的變換物理效果與作用點無關(guān)的矢量——自由矢量（freevector）線速度、力（偶）矩等物理效果與作用點有關(guān)的矢量——線矢量（linevector）角速度、力等兩坐標(biāo)系間的自由矢量變換，僅涉及到旋轉(zhuǎn)線速度力（偶）矩兩坐標(biāo)系間的線矢量變換，需要考慮坐標(biāo)系原點偏移的影響本章目錄402025/6/12.1.1位姿描述2.1.2映射與算子2.1.3其他姿態(tài)描述三角度姿態(tài)法等效軸-角法四元數(shù)

法三角度姿態(tài)法41R有9個元素，是否一定需要9個變量才能唯一確定旋轉(zhuǎn)矩陣？再次考察旋轉(zhuǎn)變換矩陣R由于R是單位正交矩陣，所以存在6個約束條件：3個列向量是單位向量：3個正交條件：因此，R中只有3個獨立變量，也即用3個參數(shù)即可表示姿態(tài)。三角度姿態(tài)法42線性代數(shù)中的凱萊公式指出，對于任何一個正交陣R存在一個反對稱矩陣，滿足：再次考察旋轉(zhuǎn)變換矩陣R其中：這再次說明，可用3個參數(shù)表示姿態(tài)。

三角度姿態(tài)法43旋轉(zhuǎn)變換一般不滿足交換律，也即：再次考察旋轉(zhuǎn)變換矩陣R

注意：旋轉(zhuǎn)不滿足交換律，那么必須要使用三個有順序的參數(shù)才能準(zhǔn)確描述姿態(tài)采用3個獨立的姿態(tài)角來描述3個姿態(tài)角的任意組合有33=27種形式，但是為了保持3個姿態(tài)角的獨立性，需要保證兩個連續(xù)旋轉(zhuǎn)軸的軸線不能平行，因此3姿態(tài)角存在12中形式。3*2*2=12X-Y-Z、X-Z-Y、Y-X-Z、Y-Z-X、Z-X-Y、Z-Y-X、Z-Y-Z、Z-X-Z、Y-Z-Y、Y-X-Y、X-Y-X、X-Z-XRPY（繞3個定軸的旋轉(zhuǎn)）歐拉角（繞3個動軸的旋轉(zhuǎn)）三角度姿態(tài)法4412×2=24種三角度姿態(tài)法45X-Y-Z固定角

繞固定坐標(biāo)系三個軸的三次轉(zhuǎn)動，得到的三個轉(zhuǎn)角（

,

,

）稱為X-Y-Z固定角。在描述運動物體時，例如：飛機(jī)，它們又被稱為橫滾角（Roll）、俯仰角（Pitch）和偏航角（Yaw）——R-P-Y角

ZYaw三角度姿態(tài)法46X-Y-Z固定角

三角度姿態(tài)法47X-Y-Z固定角復(fù)合變換：計算，得：注意：繞固定坐標(biāo)軸的連續(xù)變換，按變換順序“左乘”得到最終變換矩陣三角度姿態(tài)法48X-Y-Z固定角已知旋轉(zhuǎn)矩陣R，求對應(yīng)的X-Y-Z固定角（

,

,

）在實現(xiàn)機(jī)器人連續(xù)運動控制的姿態(tài)插補(bǔ)時，經(jīng)常需要根據(jù)已知旋轉(zhuǎn)矩陣求解姿態(tài)角已知：根據(jù)：三角度姿態(tài)法492025/6/1X-Y-Z固定角Atan2(y,x)—“四象限反正切函數(shù)”，內(nèi)置于大多數(shù)編程語言，可根據(jù)x、y的符號給出不同的角度值，例如：Atan2(-2.0,-2.0)=-135°Atan2(2.0,2.0)=45°三角度姿態(tài)法50關(guān)于

角的說明計算中，通常?。海?0°≤

≤90°若

＝±90°，則cos

＝0，此時，

和

的值無法計算規(guī)定：或51歐拉角是瑞士數(shù)學(xué)家歐拉（Euler，1707-1783）提出的一種采用繞動坐標(biāo)系3個坐標(biāo)軸的轉(zhuǎn)角組合描述剛體姿態(tài)的方法。A.Z-Y-X歐拉角B.Z-Y-Z（Z-X-Z）歐拉角A.Z-Y-X歐拉角將{B}繞其z軸旋轉(zhuǎn)角度

繞{B}的新y軸旋轉(zhuǎn)角度

繞{B}的新x軸旋轉(zhuǎn)角度

B.Z-Y-Z歐拉角將{B}繞其z軸旋轉(zhuǎn)角度

繞{B}的新y軸旋轉(zhuǎn)角度

繞{B}的新z軸旋轉(zhuǎn)角度

進(jìn)動角章動角自旋角三角度姿態(tài)法三角度姿態(tài)法52Z-Y-X歐拉角坐標(biāo)系{B}相對{A}的姿態(tài)的另一種表示法，是假想相對運動坐標(biāo)系軸連續(xù)轉(zhuǎn)動，并利用旋轉(zhuǎn)映射得出旋轉(zhuǎn)矩陣。首先，假設(shè)初始{B}與{A}重合。

將{B}繞其z軸旋轉(zhuǎn)角度

繞{B}的新y軸旋轉(zhuǎn)角度

繞{B}的新x軸旋轉(zhuǎn)角度

三角度姿態(tài)法53Z-Y-X歐拉角

也即：注意：繞運動坐標(biāo)軸的連續(xù)變換，按變換順序“右乘”得到最終變換矩陣三角度姿態(tài)法54Z-Y-X歐拉角Z-Y-X歐拉角定義的位姿矩陣為：上述結(jié)果與繞固定軸X-Y-Z旋轉(zhuǎn)得到的位姿矩陣相等！三角度姿態(tài)法55X-Y-Z固定角與Z-Y-X歐拉角Z-Y-X歐拉角X-Y-Z固定角結(jié)論：坐標(biāo)系{B}相對于坐標(biāo)系{A}的姿態(tài)可以假想繞三個坐標(biāo)軸依次旋轉(zhuǎn)得到繞固定坐標(biāo)系三個軸的連續(xù)旋轉(zhuǎn)與繞運動軸以相反順序旋轉(zhuǎn)的結(jié)果相同沿固定坐標(biāo)系的固定角連續(xù)變換，按旋轉(zhuǎn)順序連續(xù)“左乘”沿運動坐標(biāo)系的歐拉角連續(xù)變換，按旋轉(zhuǎn)順序連續(xù)“右乘”三角度姿態(tài)法56常用的Z-Y-Z歐拉角變換與機(jī)器人末端工具姿態(tài)描述常采用Z-Y-Z歐拉角描述，這樣可與腕部三個垂直正交旋轉(zhuǎn)關(guān)節(jié)的轉(zhuǎn)角直接對應(yīng)用Z-Y-Z歐拉角描述的旋轉(zhuǎn)矩陣：若已知旋轉(zhuǎn)矩陣：則，三個歐拉角為：顯然，此時

角不能等于0°或180°。若出現(xiàn)此情況，則取

＝0°三角度姿態(tài)法57三角度位姿描述中的奇異點（SingularPoint）問題無論采用歐拉角還是固定角表示位姿，當(dāng)中間軸轉(zhuǎn)角等于±90°或0°、180°時，總會出現(xiàn)無法求解的情況例如：Z-Y-Z歐拉角，

＝0°或180°Z-Y-X歐拉角，

＝±90°奇異發(fā)生在第一次轉(zhuǎn)動軸線與最后一次轉(zhuǎn)動軸線共線的位置第一次和最后一次旋轉(zhuǎn)的轉(zhuǎn)軸重合（即中間軸的轉(zhuǎn)角

=±90°）時，導(dǎo)致繞第一、第三軸的轉(zhuǎn)角無法計算，歐拉角描述的姿態(tài)發(fā)生奇異；對應(yīng)的位形或位姿，稱為奇異位形（singularconfiguration）58三角度位姿描述中的奇異點問題從實際物理意義上來說，此種情況意味著第1、3軸重合，導(dǎo)致繞第1、3軸的轉(zhuǎn)角無法計算，稱為奇異現(xiàn)象對應(yīng)的位姿點（由姿態(tài)角元素構(gòu)成的點），稱為奇異點Z-Y-X歐拉角，

＝±90°三角度姿態(tài)法59位姿奇異現(xiàn)象的具體案例飛行器中的萬向節(jié)死鎖（Gimballock）問題陀螺儀：X軸控制偏航（右圖中的藍(lán)色圖示），Y軸控制俯仰（右圖中的紅色圖示），Z軸控制橫滾（右圖中的綠色圖示）。俯仰角

=±

/2時發(fā)生奇異發(fā)生萬向節(jié)鎖死時，俯仰角和航偏角沒影響，橫滾角度受影響。工程上一般在發(fā)生奇異時，人為設(shè)定橫滾角

=0。/hanjuefu5827/article/details/80659343?depth_1-utm_source=distribute.pc_relevant.none-task&utm_source=distribute.pc_relevant.none-task三角度姿態(tài)法60位姿奇異現(xiàn)象的具體案例機(jī)槍轉(zhuǎn)塔跟蹤過頂飛機(jī)目標(biāo)飛機(jī)過頂飛行；轉(zhuǎn)塔方位跟蹤速度趨向于無窮大。當(dāng)接近方位角為90

的位置，它的工作性能越來越不理想。為跟蹤飛過飛機(jī)頭頂?shù)哪繕?biāo)，槍手需要操控機(jī)槍以非?？斓乃俣壤@方位角轉(zhuǎn)動。如果目標(biāo)直接飛過槍手頭頂，對方位角的跟蹤速度趨向于無窮大。三角度姿態(tài)法等效軸-角法612025/6/1能否用一次旋轉(zhuǎn)變換描述{B}相對于{A}的姿態(tài)？歐拉旋轉(zhuǎn)定理：在三維空間里，剛體的任意旋轉(zhuǎn)等價于一個繞著某固定軸的旋轉(zhuǎn)（簡化描述）假設(shè){B}與{A}初始狀態(tài)重合，將{B}繞過原點的任意單位向量

按右手定則旋轉(zhuǎn)θ角，可到達(dá){B}的實際姿態(tài)此旋轉(zhuǎn)記為：右手定則等效軸-角法62

等效軸-角法63等效軸-角旋轉(zhuǎn)矩陣表達(dá)式的推導(dǎo)

64等效軸角旋轉(zhuǎn)矩陣表達(dá)式的推導(dǎo)于是，根據(jù)旋轉(zhuǎn)變換方程可得：

由旋轉(zhuǎn)矩陣的正交性，可得：于是：根據(jù)假設(shè)：等效軸-角法652025/6/1等效軸-角旋轉(zhuǎn)矩陣表達(dá)式的推導(dǎo)可得將上式右端相乘，并利用可得等效軸-角法662025/6/1根據(jù)旋轉(zhuǎn)矩陣求解等效軸-角若已知旋轉(zhuǎn)矩陣R：可求解等效軸-角：注意：θ不能等于0°或180°，否則出現(xiàn)奇異現(xiàn)象，無法確定轉(zhuǎn)軸等效軸-角法反對稱矩陣67羅德里格斯（Rodrigues）公式等效軸-角法歐拉定理：任一旋轉(zhuǎn)矩陣R總可以等效為繞某一固定軸的旋轉(zhuǎn)運動。68羅德里格斯（Rodrigues）公式等效軸-角法四元數(shù)的定義692025/6/1

以超復(fù)數(shù)表達(dá)的四元數(shù)以四元向量表達(dá)的四元數(shù)威廉·若宛·哈密頓（WilliamRowanHamilton1805-1865）愛爾蘭數(shù)學(xué)家，他提出了四元數(shù)。四元數(shù)虛數(shù)單位i,j,k的乘法規(guī)則以（標(biāo)量+向量）表達(dá)的四元數(shù)四元數(shù)的運算70加法乘法共軛四元數(shù)逆四元數(shù)的模模為1的四元數(shù)稱為單位四元數(shù)單位四元數(shù)利用單位四元數(shù)實現(xiàn)姿態(tài)旋轉(zhuǎn)71歐拉參數(shù)表示的四元數(shù)

利用單位四元數(shù)實現(xiàn)旋轉(zhuǎn)

72利用單位四元數(shù)實現(xiàn)連續(xù)旋轉(zhuǎn)利用單位四元數(shù)實現(xiàn)姿態(tài)旋轉(zhuǎn)用單位四元數(shù)實現(xiàn)連續(xù)兩次旋轉(zhuǎn)，可以先計算單位四元數(shù)的乘積（復(fù)合旋轉(zhuǎn)），再與被旋轉(zhuǎn)向量相乘它等價于：

兩個旋轉(zhuǎn)矩陣的復(fù)合，涉及27次乘法和18次加法兩個單位四元數(shù)的復(fù)合，僅需要16次乘法和12次加法單位四元數(shù)在實現(xiàn)連續(xù)旋轉(zhuǎn)時，計算效率高73姿態(tài)及旋轉(zhuǎn)描述方法匯總物體坐標(biāo)系{B}最初與固定坐標(biāo)系{A}重合。令{B}繞過坐標(biāo)原點的單位向量

轉(zhuǎn)動120

，求當(dāng)前坐標(biāo)系{B}相對固定坐標(biāo)系的姿態(tài)矩陣?！纠俊纠恳阎藨B(tài)矩陣

，求對應(yīng)的等效轉(zhuǎn)軸和轉(zhuǎn)角。習(xí)題74已知單位四元數(shù)為【例】【例】已知姿態(tài)矩陣

，求對應(yīng)的等效歐拉參數(shù)。，求對應(yīng)的旋轉(zhuǎn)矩陣和等效轉(zhuǎn)軸。該四元數(shù)的物理意義是：繞z軸旋轉(zhuǎn)120

。75習(xí)題已知單位四元數(shù)為【例】該四元數(shù)的物理意義是：由前面的兩個例子可知，這兩個分解的運動分別為z軸轉(zhuǎn)動和x軸轉(zhuǎn)動。因此該轉(zhuǎn)動可看作是先繞x軸旋轉(zhuǎn)半周，再繞z軸旋轉(zhuǎn)120

。76，求對應(yīng)的旋轉(zhuǎn)矩陣和等效轉(zhuǎn)軸。解：歐拉參數(shù)：，出現(xiàn)奇異。因此，無法直接給出其等效轉(zhuǎn)軸，不過可看作是兩個連續(xù)轉(zhuǎn)動的復(fù)合運動：習(xí)題第二章結(jié)束第2章機(jī)器人運動學(xué)2.2節(jié)機(jī)器人（操作臂）正運動學(xué)第1-15周，星期二，16:40-18:15，（五）103機(jī)器人技術(shù)基礎(chǔ)792.2.1正運動學(xué)2.2.2連桿參數(shù)與連桿坐標(biāo)系2.2.3操作臂的運動學(xué)方程2.2.4典型工業(yè)機(jī)器人的運動學(xué)模型2.2.5坐標(biāo)系的命名本節(jié)目錄正運動學(xué)（ForwardKinematics）802025/6/1腕2基座{B}腰大臂小臂腕1腕3{W}工具T正運動學(xué)問題θ1θ2θ3θ4θ5θ6已知：關(guān)節(jié)變量[θ1,…,θn]T,

4×4求解：末端（或中間連桿）的空間位姿

關(guān)節(jié)變量[θ1,…,θn]T正運動學(xué)模型——從關(guān)節(jié)變量到末端工具位姿的映射關(guān)系812025/6/1{5}{0}{1}{2}{3}{4}{6}θ1θ2θ3θ4θ5θ6構(gòu)型：從基座到末端的開鏈驅(qū)動效果：從基座到末端，關(guān)節(jié)驅(qū)動變量逐次作用于后續(xù)剛體，兩相鄰剛體之間的相對位姿，僅取決于它們之間的連接關(guān)節(jié)串聯(lián)機(jī)器人正運動學(xué)問題的特點建立串聯(lián)機(jī)器人正運動學(xué)模型的思路

正運動學(xué)822025/6/1構(gòu)建機(jī)器人學(xué)的數(shù)學(xué)描述體系，是后續(xù)逆運動學(xué)、速度分析、動力學(xué)分析的理論基礎(chǔ)；在設(shè)計階段根據(jù)關(guān)節(jié)驅(qū)動電機(jī)特性和結(jié)構(gòu)參數(shù)評估機(jī)器人工作空間、末端速度和加速度研究機(jī)器人正運動學(xué)問題的意義工具坐標(biāo)系腕部坐標(biāo)系基坐標(biāo)系固定坐標(biāo)系目標(biāo)坐標(biāo)系銷釘正運動學(xué)832.2.1正運動學(xué)2.2.2連桿參數(shù)與連桿坐標(biāo)系2.2.3操作臂的運動學(xué)方程2.2.4典型工業(yè)機(jī)器人的運動學(xué)模型2.2.5坐標(biāo)系的命名本節(jié)目錄連桿參數(shù)842025/6/1

DH參數(shù)法的由來JacquesDenavit(1930-2012)RichardHartenberg(1907-1997)后置坐標(biāo)系前置坐標(biāo)系如果剛體坐標(biāo)系建立在前向關(guān)節(jié)上，則稱為前置坐標(biāo)系（ModifiedDHConvention）連桿參數(shù)852025/6/1

連桿的結(jié)構(gòu)參數(shù)，Linkparameters（2個）862025/6/1機(jī)器人中間某連桿，兩端為旋轉(zhuǎn)關(guān)節(jié)，關(guān)節(jié)分布及尺寸如圖連桿的結(jié)構(gòu)參數(shù)——實例

連桿i-150mm50mm50mm125mm50mm75mm連桿參數(shù)872025/6/1

連桿的連接參數(shù)，Linkparameters（2個）連桿參數(shù)882025/6/10號連桿（基座）的參數(shù)

n號連桿（末端）的參數(shù)

連桿參數(shù)892025/6/1

連桿i-1的固聯(lián)坐標(biāo)系定義在關(guān)節(jié)i-1上連桿坐標(biāo)系定義——中間連桿IntermediateLink

連桿坐標(biāo)系902025/6/1

連桿坐標(biāo)系定義——基座Firstlink連桿坐標(biāo)系912025/6/1

連桿坐標(biāo)系定義——末端連桿，Lastlink連桿坐標(biāo)系922025/6/1

連桿坐標(biāo)系定義小結(jié)——參數(shù)連桿坐標(biāo)系932025/6/1連桿坐標(biāo)系定義小結(jié)——步驟

連桿坐標(biāo)系942025/6/1連桿坐標(biāo)系定義——實例1為右圖所示3自由度平面機(jī)械臂（3R機(jī)器人）建立連桿坐標(biāo)系。連桿坐標(biāo)系連桿參數(shù)表連桿坐標(biāo)系952025/6/1連桿坐標(biāo)系定義——實例2為右圖所示3自由度空間機(jī)械臂建立連桿坐標(biāo)系。連桿坐標(biāo)系連桿參數(shù)表連桿坐標(biāo)系962.2.1正運動學(xué)2.2.2連桿參數(shù)與連桿坐標(biāo)系2.2.3操作臂的運動學(xué)方程2.2.4典型工業(yè)機(jī)器人的運動學(xué)模型2.2.5坐標(biāo)系的命名本節(jié)目錄連桿坐標(biāo)變換972025/6/1

坐標(biāo)系{i}相對于{i-1}的變換

982025/6/1坐標(biāo)系{i}相對于{i-1}的變換

上式等價于：

于是：

上式中的每個變換矩陣都是簡單的平移或繞坐標(biāo)軸旋轉(zhuǎn)變換，可寫成：

連桿坐標(biāo)變換992025/6/1坐標(biāo)系{i}相對于{i-1}的變換,LinkTransformation其中：

于是：連桿坐標(biāo)變換1002025/6/1連桿坐標(biāo)變換——實例1建立下圖所示平面3R機(jī)器人各連桿坐標(biāo)系的齊次變換矩陣（前置坐標(biāo)系）對應(yīng)各連桿坐標(biāo)系的齊次變換矩陣連桿坐標(biāo)系連桿參數(shù)表連桿坐標(biāo)變換101操作臂的運動學(xué)方程由于操作臂可以看成是由一系列桿件通過關(guān)節(jié)連接而成的，因此可以將各連桿變換矩陣順序序相乘，便可得到末端桿坐標(biāo)系{n}相對于基坐標(biāo)系{0}的齊次變換矩陣是關(guān)節(jié)變量di或θi的函數(shù)

由于機(jī)械手（或末端）的位姿可以由齊次矩陣描述，因此上式（2.2.8）稱為機(jī)器人的運動學(xué)方程，表示機(jī)械手位姿與各關(guān)節(jié)變量之間的關(guān)系。2025/6/1102關(guān)節(jié)空間、笛卡爾空間和驅(qū)動空間操作臂的連桿位置可由一組n個關(guān)節(jié)變量確定，這樣一組變量稱為n×1的的關(guān)節(jié)向量。所有關(guān)節(jié)矢量組成的空間稱為關(guān)節(jié)空間。當(dāng)機(jī)械手的位姿是在直角坐標(biāo)空間描述時，這個空間稱為笛卡爾空間，有時稱為任務(wù)空間或操作空間。把關(guān)節(jié)矢量表示成一組驅(qū)動器函數(shù)時，這個矢量稱為驅(qū)動器向量，這個空間稱為驅(qū)動空間。操作臂運動學(xué)正問題：驅(qū)動空間→關(guān)節(jié)空間→笛卡爾空間的描述。操作臂運動學(xué)逆問題：笛卡爾空間→關(guān)節(jié)空間→驅(qū)動空間的描述。對于串聯(lián)機(jī)器人，運動學(xué)正問題求解比逆問題容易；而對于并聯(lián)機(jī)器人，則相反。運動學(xué)方程的應(yīng)用：工作空間分析1032025/6/1機(jī)器人的工作空間工作空間（Workspace）——機(jī)器人末端所能達(dá)到的范圍，機(jī)器人的重要性能指標(biāo)，包含可達(dá)工作空間和靈巧工作空間兩個概念靈巧工作空間（DexterousWorkspace）——可達(dá)工作空間中的某個區(qū)域，在該區(qū)域中，對于任意位置點，機(jī)器人能從各個方向（以任意姿態(tài)）到達(dá)可達(dá)工作空間（ReachableWorkspace）——機(jī)器人至少能從一個方向（以一種姿態(tài)）達(dá)到的位置點構(gòu)成的空間利用正運動學(xué)模型，對各關(guān)節(jié)變量遍歷求解可獲得可達(dá)工作空間對靈巧工作空間中的某位置點，直觀上，平面機(jī)器人末端可繞該點做圓周運動，空間機(jī)器人末端可繞該點做球面運動1042025/6/1例：平面2R機(jī)器人的工作空間若l1=l2=l，則可達(dá)工作空間為半徑為2l的圓（含內(nèi)部），靈活工作空間為圓心點，圓周上的點對應(yīng)唯一關(guān)節(jié)解，其他位置各點存在兩個解若l1≠l2，則可達(dá)工作空間為內(nèi)徑為|l1-l2|、外徑為（l1+l2）的圓環(huán)，靈活工作空間為空集顯然，當(dāng)靈活工作空間為一點或空集時，其運動靈活性比較差若想提高機(jī)器人的靈活性，可增加一個R關(guān)節(jié)，變成平面3R機(jī)器人l1=l2l1

l2運動學(xué)方程的應(yīng)用：工作空間分析1052025/6/1例：平面3R機(jī)器人的工作空間設(shè)l1＞l2，l2＞l3，l1≤l2＋l3可達(dá)工作空間是半徑為l1＋l2＋l3的圓靈活工作空間是內(nèi)徑為l1－l2＋l3、外徑為l1＋l2－l3的圓環(huán)可見，通過增加一個關(guān)節(jié)，能夠有效增加靈活工作空間運動學(xué)方程的應(yīng)用：工作空間分析1062025/6/1串聯(lián)機(jī)器人關(guān)節(jié)配置的一般原則定位與定向分離靠近基座的關(guān)節(jié)用于定位，平面機(jī)器人2自由度，空間機(jī)器人3自由度靠近末端的關(guān)節(jié)用于定向，平面機(jī)器人1自由度，空間機(jī)器人3自由度空間機(jī)器人定向關(guān)節(jié)的3個軸線交于一點（腕心），理想情況下相互正交（即：末端定向機(jī)構(gòu)等價于一個主動球鉸，也叫球腕）定位定向定位定向運動學(xué)方程的應(yīng)用：工作空間分析1072.2.1正運動學(xué)2.2.2連桿參數(shù)與連桿坐標(biāo)系2.2.3操作臂的運動學(xué)方程2.2.4典型工業(yè)機(jī)器人的運動學(xué)模型2.2.5坐標(biāo)系的命名本節(jié)目錄典型工業(yè)機(jī)器人的運動學(xué)模型1082025/6/1利用D-H參數(shù)法對PUMA560機(jī)器人進(jìn)行正向位移求解（前置坐標(biāo)系）z1z2x2z3x3z4x4z5x5z6x6x0x1y01092025/6/1利用D-H參數(shù)法對PUMA560機(jī)器人進(jìn)行正向位移求解（前置坐標(biāo)系）連桿iαi-1ai-1di變量θi變量范圍10°00θ1-160°~160°2

90°0d2θ2-225°~45°30°a20θ3-45°~225°4

90°a3d4θ4-110°~170°590°00θ5-100°~100°6

90°00θ6-266°~266°典型工業(yè)機(jī)器人的運動學(xué)模型1102025/6/1利用D-H參數(shù)法對PUMA560機(jī)器人進(jìn)行正向位移求解（前置坐標(biāo)系）機(jī)器人的位移正運動學(xué)模型：根據(jù)關(guān)節(jié)變量求解末端位姿的過程稱為解析位移正解（Forwarddisplacementanalysis）典型工業(yè)機(jī)器人的運動學(xué)模型1112025/6/1

正運動學(xué)的建模步驟典型工業(yè)機(jī)器人的運動學(xué)模型1122.2.1正運動學(xué)2.2.2連桿參數(shù)與連桿坐標(biāo)系2.2.3操作臂的運動學(xué)方程2.2.4典型工業(yè)機(jī)器人的運動學(xué)模型2.2.5坐標(biāo)系的命名本節(jié)目錄坐標(biāo)系的命名1132025/6/1典型工業(yè)機(jī)器人的坐標(biāo)系命名工具坐標(biāo)系{T}腕部坐標(biāo)系{W}基坐標(biāo)系{B}固定坐標(biāo)系{S}目標(biāo)坐標(biāo)系{G}銷釘基坐標(biāo)系{B}：坐標(biāo)系{0}，位于機(jī)器人基座固定坐標(biāo)系{S}：與任務(wù)相關(guān)，通常固定在工作臺的一角，也稱工作臺坐標(biāo)系腕部坐標(biāo)系{W}：坐標(biāo)系{W}，固聯(lián)在機(jī)器人末端連桿上，原點通常在腕部法蘭中心1142025/6/1典型工業(yè)機(jī)器人的坐標(biāo)系命名工具坐標(biāo)系{T}腕部坐標(biāo)系{W}基坐標(biāo)系{B}固定坐標(biāo)系{S}目標(biāo)坐標(biāo)系{G}銷釘工具坐標(biāo)系{T}：固聯(lián)于工具末端，手指沒有抓持工具時，位于兩個指尖中間目標(biāo)坐標(biāo)系{G}：描述工具的目標(biāo)位姿，當(dāng)機(jī)器人運動結(jié)束時，工具坐標(biāo)系{T}應(yīng)當(dāng)與目標(biāo)坐標(biāo)系{G}重合{T}相對于{G}的變換矩陣：坐標(biāo)系的命名第2章機(jī)器人運動學(xué)2.3節(jié)機(jī)器人（操作臂）逆運動學(xué)第1-15周，星期二，16:40-18:15，（五）103機(jī)器人技術(shù)基礎(chǔ)1162.3.1解的存在性問題2.3.2運動學(xué)方程的解法2.3.3操作臂逆運動學(xué)計算實例本節(jié)目錄逆運動學(xué)（InverseKinematics）1172025/6/1基座{B}工具{T}逆運動學(xué)問題θ1θ2θ3θ4θ5θ6

求解關(guān)節(jié)向量[θ1,θ2,…,θn]T逆運動學(xué)模型——從末端位姿矩陣到關(guān)節(jié)向量的映射機(jī)器人應(yīng)用的基礎(chǔ)——通常給定末端工具位姿，需要求解關(guān)節(jié)變量，然后通過控制關(guān)節(jié)變量到指定值，使得末端工具到達(dá)給定位姿。1182025/6/1逆運動學(xué)解析解的求解過程已知機(jī)器人正運動學(xué)模型

求其反函數(shù)，得到關(guān)節(jié)變量函數(shù)——逆運動學(xué)模型

得到各關(guān)節(jié)變量?i

{W}{B}{S}{G}{T}

逆運動學(xué)1192025/6/1求解逆運動學(xué)方程的特點PUMA560的運動學(xué)正解逆運動學(xué)問題：已知12個方程，求解6個關(guān)節(jié)變量與旋轉(zhuǎn)矩陣相關(guān)的9個方程，存在6個約束條件，只有3個是獨立的實質(zhì)上是根據(jù)6個方程，求解6個變量存在三角函數(shù)，是超越方程(transcendentalequation),含有未知量的超越式(指數(shù)、對數(shù)、三角函數(shù)、反三角函數(shù)等)的方程需要考慮解的存在性、多解問題，以及求解方法逆運動學(xué)1202025/6/1機(jī)器人運動學(xué)逆解的存在性在給定機(jī)器人末端位姿時，需要考慮逆解的存在性只有機(jī)器人末端位置點在可達(dá)工作空間（Reachableworkspace）內(nèi)，且給定姿態(tài)也可達(dá)，才存在逆解對于可達(dá)工作空間邊緣的位置點，與位置相關(guān)的關(guān)節(jié)存在唯一解，且末端可達(dá)姿態(tài)受限對于靈巧工作空間中的位置點，理論上對任意末端姿態(tài)均存在關(guān)節(jié)逆解，但是受限于關(guān)節(jié)轉(zhuǎn)角范圍，通常僅在有限的末端姿態(tài)空間中存在逆解除上述兩個區(qū)域之外，其他可達(dá)空間中的位置點，只有有限的位姿存在關(guān)節(jié)逆解解的存在性1212025/6/1多解情況對于可達(dá)空間中的多數(shù)給定位姿，其逆運動學(xué)問題存在多解例如，對于PUMA機(jī)器人，對應(yīng)著每個末端位姿，理論上都存在8個解對應(yīng)著右圖所示的4個解，把4、6關(guān)節(jié)反轉(zhuǎn)180°，5關(guān)節(jié)取反，又可得到4個解解的存在性1222025/6/1多解情況障礙物的存在、結(jié)構(gòu)尺寸限制、關(guān)節(jié)轉(zhuǎn)角限制，會使某些解無效對于存在多解的情況，求解時，取“最近”關(guān)節(jié)角當(dāng)多個關(guān)節(jié)都存在多解時，以大連桿的“最近”關(guān)節(jié)角優(yōu)先為原則選取解。即：少移動大關(guān)節(jié)（腰、大臂、小臂）、多移動小關(guān)節(jié)（腕）“最近”解&“無效”解“次優(yōu)”解解的存在性1232.3.1解的存在性問題2.3.2運動學(xué)方程的解法2.3.3操作臂逆運動學(xué)計算實例本節(jié)目錄逆解的解法1242025/6/1數(shù)值解（Numericalsolution）封閉解（Analyticalsolution）空間6自由度機(jī)器人具有“封閉解”的充分條件是：相鄰三根關(guān)節(jié)軸交于一點，或相互平行“封閉解”計算效率高，機(jī)器人設(shè)計時，應(yīng)盡量使其具有“封閉解”絕大多數(shù)工業(yè)機(jī)器人采用了這種設(shè)計針對給定末端位姿，以當(dāng)前關(guān)節(jié)角為起點，根據(jù)運動學(xué)正解方程，利用牛頓迭代法求解可以用解析表達(dá)式顯式表達(dá)關(guān)節(jié)變量，也稱解析解相互平行三個相鄰軸交于一點封閉解——實例11252025/6/1平面2R機(jī)器人已知各桿的長度l1和l2和末端參考點B的坐標(biāo)，計算關(guān)節(jié)角

1和

2存在兩組解：正運動學(xué)模型：逆解：封閉解——實例21262025/6/1平面3R機(jī)器人正運動學(xué)模型：逆解：封閉解——實例31272025/6/1空間3R機(jī)器人正運動學(xué)模型：逆解：1282.3.1解的存在性問題2.3.2運動學(xué)方程的解法2.3.3操作臂逆運動學(xué)計算實例本節(jié)目錄PUMA機(jī)器人運動學(xué)逆解1292025/6/1正運動學(xué)模型已知求1302025/6/1思路對于關(guān)節(jié)變量較多的串聯(lián)機(jī)器人而言，由于關(guān)節(jié)耦合嚴(yán)重，需進(jìn)行逐次消元，以達(dá)到簡化求反解的目的。為此，可利用Paul反變換法來實現(xiàn)。左乘從等式兩邊矩陣對應(yīng)的元素中尋找含單關(guān)節(jié)變量的等式，進(jìn)而解出該變量。不斷重復(fù)此過程，直到所有變量解出左乘PUMA機(jī)器人運動學(xué)逆解1312025/6/1需用到的幾個中間變換矩陣PUMA機(jī)器人運動學(xué)逆解1322025/6/1求θ1正解的中間變換矩陣

1已知

PUMA機(jī)器人運動學(xué)逆解1332025/6/1求θ2和θ3

2

3PUMA機(jī)器人運動學(xué)逆解1342025/6/1求θ4和θ5

4

5PUMA機(jī)器人運動學(xué)逆解1352025/6/1求θ6

6理論上全部關(guān)節(jié)值存在8組解，但由于結(jié)構(gòu)限制，部分解可能在實際中并不存在。上文僅介紹了求解方法和步驟，運動學(xué)逆解的解析表達(dá)式見教材。PUMA機(jī)器人運動學(xué)逆解第2章機(jī)器人運動學(xué)2.4節(jié)速度雅可比第1-15周，星期二，16:40-18:15，（五）103機(jī)器人技術(shù)基礎(chǔ)1372.4.1剛體的線速度和角速度2.4.2操作臂連桿的運動速度2.4.3速度雅可比2.4.4奇異性本節(jié)目錄時變位置的符號表示——線速度1382025/6/1若{B}中有一時變矢量BQ（t）設(shè)有兩坐標(biāo)系{A}、{B}{A}{B}Q則，其在{B}中的速度表達(dá)為：其在{A}中的速度表達(dá)為：

（速度是自由矢量）剛體的線速度1392025/6/1{B}原點在{A}中的向量為APBORG；{B}相對于{A}僅有平移運動；{B}中有一向量BQ，相對于{B}原點的線速度為BVQ；求Q相對于{A}的線速度AVQ已知兩剛體，各自的固聯(lián)坐標(biāo)系為{A}和{B}AVQ由兩個速度向量合成得到：AVQ注意：為求解AVQ，所有矢量都必須轉(zhuǎn)換到{A}坐標(biāo)系中表達(dá)AVQ是相對于{A}的速度，需要用到速度矢量合成計算質(zhì)點相對參考坐標(biāo)系的運動坐標(biāo)系之間的相對平動時變位置的符號表示——角速度1402025/6/1點的相對運動只有線速度、而沒有角速度剛體之間的相對運動既有線速度、又有角速度角速度一定指兩個剛體坐標(biāo)系之間的相對轉(zhuǎn)動速度角速度表示符號Ω注意：AΩB為一向量，其方向表示{B}相對于{A}旋轉(zhuǎn)的瞬時旋轉(zhuǎn)軸，其大小為旋轉(zhuǎn)速率（回顧等效軸角姿態(tài)表示）；角速度也可以在不同坐標(biāo)系描述，如：C(AΩB)為{B}相對于{A}的角速度在{C}中的描述；ωC=UΩC表示{C}相對于參考坐標(biāo)系{U}的角速度設(shè)有兩個剛體坐標(biāo)系{A}、{B}，原點重合，僅有相對轉(zhuǎn)動，則它們之間的角速度為：剛體的角速度1412025/6/1{A}、{B}原點重合；{B}相對于{A}僅有旋轉(zhuǎn)運動，旋轉(zhuǎn)角速度為AΩB；{B}中有一向量BQ，相對于{B}原點的線速度為BVQ；求Q相對于{A}的線速度AVQ已知兩剛體，各自的固聯(lián)坐標(biāo)系為{A}和{B}首先假設(shè)Q在{B}中固定不動，即：但是，由于{B}相對于{A}的旋轉(zhuǎn)AΩB，AVQ顯然不等于零1422025/6/1從{A}觀察Q的變化情況，如右圖AQ(t)AQ(t+Δt)AQ(t)繞AΩB右圖軸轉(zhuǎn)動，下一時刻到達(dá)AQ(t+Δt)顯然，AQ(t)和AQ(t+Δt)是以坐標(biāo)系原點為頂點、AΩB為軸線的圓錐上的兩條母線AQ的微分增量Δ一定垂直于AQ和AΩB，且其大小為：其中：θ為AQ與AΩB的夾角于是，AVQ的大小為：方向遵循右手定則，垂直于AQ和AΩB實際上，AVQ垂直等于AQ、AΩB的叉乘：剛體的角速度1432025/6/1如果考慮Q的相對于{B}的變化，即：則：于是，若：則：利用旋轉(zhuǎn)變換，把已知變量變換到{A}坐標(biāo)系，得：質(zhì)點相對參考坐標(biāo)系的運動坐標(biāo)系之間的相對轉(zhuǎn)動回顧叉乘計算方法：剛體的角速度角速度矢量的意義1442025/6/1假設(shè)通過等效軸-角變換獲得該矩陣剛體{B}相對于{A}的姿態(tài)矩陣為R

K為旋轉(zhuǎn)矢量

假設(shè)經(jīng)過小時間間隔Δt，轉(zhuǎn)軸不變，而轉(zhuǎn)角為Δθ則可定義角速度矢量可以把角速度矢量簡單理解為剛體{B}繞瞬時轉(zhuǎn)軸旋轉(zhuǎn)的時，轉(zhuǎn)角對時間的導(dǎo)數(shù)與單位轉(zhuǎn)軸矢量的乘積1452025/6/1

對右圖所示的{i+1}坐標(biāo)系，有：角速度矢量的意義線速度與角速度的綜合1462025/6/1綜合{A}和{B}純平動和純轉(zhuǎn)動：得：AVBORF≠0BVQ≠0AΩB≠0如果兩剛體{A}和{B}之間既有平動，也有轉(zhuǎn)動，且{B}中向量Q相對于{B}有運動，即：

AVBORG

≠0AΩB

≠0BVQ

≠0純平動純轉(zhuǎn)動質(zhì)點相對參考坐標(biāo)系的運動坐標(biāo)系之間的相對轉(zhuǎn)動坐標(biāo)系之間的相對平動注意：此式中，線速度矢量AVBORG、BVQ和位置矢量BQ很容易獲得，而如何獲得角速度矢量卻不明確1472.4.1剛體的線速度和角速度2.4.2操作臂連桿的運動速度2.4.3速度雅可比2.4.4奇異性本節(jié)目錄機(jī)器人連桿運動——迭代計算1482025/6/1以坐標(biāo)系{0}為參考坐標(biāo)系，則各連桿{i}在坐標(biāo)系{0}中的速度表示為：任一瞬時，連桿{i}的速度矢量如下圖所示線速度：υi角速度：ωi圖中線速度和角速度均定義在連桿坐標(biāo)系{i}中線速度：iυi角速度：

iωi連桿{i}軸線{i}機(jī)器人連桿運動——迭代計算1492025/6/1考慮右圖通過轉(zhuǎn)動關(guān)節(jié)連接的兩個連桿：在{i}坐標(biāo)系中考察{i+1}桿的角速度

iωi+1

iωi+1包含兩項：連桿{i}的角速度：

把第二項轉(zhuǎn)換到{i}坐標(biāo)系，兩項相加，可得：機(jī)器人連桿運動——迭代計算1502025/6/1

在{i}坐標(biāo)系中考察{i+1}桿坐標(biāo)原點的線速度

iυi+1

兩項相加，可得：iυi+1包含兩項：連桿{i}的線速度：關(guān)節(jié)i的角速度iωi引起的線速度：轉(zhuǎn)換到{i+1}坐標(biāo)系：機(jī)器人連桿運動——迭代計算1512025/6/1同樣地，可以寫出{i+1}為移動關(guān)節(jié)的速度迭代公式{i+1}為轉(zhuǎn)動關(guān)節(jié)的速度迭代公式根據(jù)上述迭代公式，即可從基坐標(biāo)