機(jī)械臂運(yùn)動路徑設(shè)計分析

1、機(jī)械臂運(yùn)動路徑設(shè)計分析摘要本文探討了六自由度機(jī)械臂從一點(diǎn)到另一點(diǎn)沿任意軌跡移動路徑、一點(diǎn)到另一點(diǎn)沿著給定軌跡移動路徑、以及無碰撞路徑規(guī)劃問題，并討論了設(shè)計參數(shù)對機(jī)械臂靈活性和使用范圍的影響，同時給出了建議。問題一：（1）首先確定初始坐標(biāo)均為零時機(jī)械臂姿態(tài)，建立多級坐標(biāo)系，利用空間解析幾何的變換基本原理及相對坐標(biāo)系的齊次坐標(biāo)變換的矩陣解析方法，來建立機(jī)器人的運(yùn)動系統(tǒng)的多級變換方程。通過逆運(yùn)動學(xué)解法和構(gòu)建規(guī)劃，來求優(yōu)化指令（2）假定機(jī)械臂初始姿態(tài)為0，曲線離散化，每個離散點(diǎn)作為末端位置，通過得到的相鄰兩點(diǎn)的姿態(tài)，利用（1）中算法計算所有相鄰兩點(diǎn)間的增量指令，將滿足精度要求的指令序列記錄下來。（3）

2、通過將障礙物理想化為球體，將躲避問題就轉(zhuǎn)化成保證機(jī)械手臂上的點(diǎn)與障礙球球心距離始終大于r的問題。進(jìn)而通過迭代法和指令檢驗(yàn)法，剔除不符合要求的指令，從而實(shí)現(xiàn)避障的目的 問題二：將問題二中的實(shí)例應(yīng)用到問題一中的相對應(yīng)的算法中，部分結(jié)果見附錄 問題三：靈活性與適用范圍相互制約，只能根據(jù)權(quán)重求得較優(yōu)連桿長度。關(guān)鍵詞：多級坐標(biāo)變換 逆運(yùn)動學(xué)解法 優(yōu)化 指令檢驗(yàn) 一、問題重述1.1 問題背景 某型號機(jī)器人（圖示和簡化圖略）一共有6個自由度，分別由六個旋轉(zhuǎn)軸（關(guān)節(jié)）實(shí)現(xiàn)，使機(jī)器人的末端可以靈活地在三維空間中運(yùn)動。機(jī)器人關(guān)于六個自由度的每一個組合，表示機(jī)械臂的一個姿態(tài)，顯然每個姿態(tài)確定頂端指尖的空間位置X：。

3、假定機(jī)器人控制系統(tǒng)只能夠接收改變各個關(guān)節(jié)的姿態(tài)的關(guān)于連桿角度的增量指令（機(jī)器指令），使得指尖（指尖具有夾工具、焊接、擰螺絲等多種功能，不過在這里不要求考慮這方面的控制細(xì)節(jié)）移動到空間點(diǎn)X，其中各個增量只能取到-2, -1.9, -1.8, ，1.8, 1.9, 2這41個離散值（即精度為0.1°，絕對值不超過2°）。通過一系列的指令序列可以將指尖依次到達(dá)位置X0，X1，Xn，則稱X0，X1，Xn為從指尖初始位置X0到達(dá)目標(biāo)位置Xn的一條路徑（運(yùn)動軌跡）。根據(jù)具體的目標(biāo)和約束條件計算出合理、便捷、有效的指令序列是機(jī)器人控制中的一個重要問題。假設(shè)機(jī)器人的初始位置在y-z平面上，

4、約定直角坐標(biāo)系的原點(diǎn)設(shè)在圖示的A點(diǎn)，z軸取為AB方向，x軸垂直紙面而y軸則在基座所固定的水平臺面上 1.2 目標(biāo)任務(wù) 問題一：設(shè)計一個通用的算法，用來計算執(zhí)行下面指定動作所要求的指令序列，并要求對算法的適用范圍、計算效率以及近似算法所造成的誤差和增量離散取值所造成的誤差大小進(jìn)行討論（不考慮其他原因造成的誤差）：（1）已知初始姿態(tài)0和一個可達(dá)目標(biāo)點(diǎn)的空間位置（Ox, Oy, Oz），計算指尖到達(dá)目標(biāo)點(diǎn)的指令序列。（2）要求指尖沿著預(yù)先指定的一條空間曲線x = x(s), y = y(s), z = z(s), a s b 移動，計算滿足要求的指令序列。（3）在第個問題中，假設(shè)在初始位置與目標(biāo)位置

5、之間的區(qū)域中有若干個已知大小、形狀、方向和位置的障礙物，要求機(jī)械臂在運(yùn)動中始終不能與障礙物相碰，否則會損壞機(jī)器。這個問題稱機(jī)械臂避碰問題，要求機(jī)械臂末端在誤差范圍內(nèi)到達(dá)目標(biāo)點(diǎn)并且整個機(jī)械臂不碰到障礙物（機(jī)械臂連桿的粗細(xì)自己設(shè)定）。問題二：應(yīng)用你的算法就下面具體的數(shù)據(jù)給出計算結(jié)果，并將計算結(jié)果以三組六維的指令序列（每行6個數(shù)據(jù)）形式存放在Excel文件里，文件名定為answer1.xls，answer2.xls和answer3.xls。假設(shè)在機(jī)械臂的旁邊有一個待加工的中空圓臺形工件，上部開口。工件高180mm，下底外半徑168mm，上底外半徑96mm，壁厚8mm。豎立地固定在xy平面的操作臺上，

6、底部的中心在 (210, 0, 0)。要求機(jī)械臂（指尖）從初始位置移動到工具箱所在位置的 (20,200, 120) 處，以夾取要用的工具。如果圓臺形工件外表面與平面x =2z的交線是一條裂紋需要焊接，請你給出機(jī)械臂指尖繞這條曲線一周的指令序列。有一項任務(wù)是在工件內(nèi)壁點(diǎn)焊四個小零件，它們在內(nèi)表面上的位置到x-y平面的投影為（320,-104）、（120,106）、（190,-125）和（255,88）。要求機(jī)械臂從圓臺的上部開口處伸進(jìn)去到達(dá)這些點(diǎn)進(jìn)行加工，為簡捷起見，不妨不計焊條等的長度，只考慮指尖的軌跡問題三：制造廠家希望通過修改各條連桿的相對長度以及各關(guān)節(jié)最大旋轉(zhuǎn)角度等設(shè)計參數(shù)提高機(jī)械臂的

7、靈活性和適用范圍。請根據(jù)你們的計算模型給他們提供合理的建議。二、問題分析機(jī)械臂運(yùn)動路徑設(shè)計問題主要涉及到相對坐標(biāo)系坐標(biāo)變換、機(jī)器人正運(yùn)動學(xué)分析、逆運(yùn)動學(xué)求解、優(yōu)化以及機(jī)器人避碰問題。1運(yùn)動方程的建立從機(jī)構(gòu)學(xué)觀點(diǎn)來看，機(jī)器人屬于空間機(jī)構(gòu)范疇。桿件每次轉(zhuǎn)動因此，采用空間解析幾何的變換基本原理及坐標(biāo)變換的矩陣解析方法，來建立機(jī)器人的運(yùn)動系統(tǒng)的多級變換方程。由于旋轉(zhuǎn)軸涉及到平行連桿和垂直連桿兩類，因此對于各類旋轉(zhuǎn)變換，所使用的變換矩陣也不相同。同時，此題中涉及的機(jī)器人有6個自由度，則從工件的坐標(biāo)位置到固定坐標(biāo)系的變換要經(jīng)過多級坐標(biāo)變換。采用多級坐標(biāo)變換的方法。由上述三點(diǎn)，便可建立機(jī)器人運(yùn)動系統(tǒng)的坐標(biāo)變

8、換關(guān)系式。2尖端軌跡曲線模型的建立對于已給定一條空間曲線x=(s),y=(y),z=(z),可將其看成一個點(diǎn)的集合。因此機(jī)械臂實(shí)現(xiàn)一個空間軌跡的過程是實(shí)現(xiàn)軌跡離散的過程。如果這些離散點(diǎn)間距很大，機(jī)械臂軌跡與要求的軌跡就有較大誤差。只有這些離散點(diǎn)彼此很接近，才有可能使機(jī)械臂的軌跡以滿足要求精確度逼近要求的軌跡。連續(xù)軌跡的控制實(shí)際上就是在多次執(zhí)行離散點(diǎn)間的點(diǎn)位控制，離散點(diǎn)點(diǎn)越密集越能逼近要求的運(yùn)動曲線3避碰問題假設(shè)空間障礙物為半徑為r的球體，則這些球體空間便形成了機(jī)械手臂的約束。而躲避問題就轉(zhuǎn)化成保證機(jī)械手臂上的點(diǎn)與障礙球球心距離始終大于r的問題。根據(jù)運(yùn)動規(guī)律，不難知道手臂相對于初始位置的姿態(tài)決定

9、于之前執(zhí)行的所有指令的和。已知連桿上的點(diǎn)L在其所在的相對坐標(biāo)系中的坐標(biāo)及轉(zhuǎn)動的指令，根據(jù)齊次坐標(biāo)變換矩陣就可得到L在固定坐標(biāo)系中的坐標(biāo)，然后可計算距離。所以 可以用問題1-1算法產(chǎn)生點(diǎn)到點(diǎn)的指令，可以利用迭代法從初始位置開始提前檢驗(yàn)每個指令，不滿足要求的無法執(zhí)行。4.自由度分析：由題中指出的：指尖E點(diǎn)，具有夾工具、焊接、擰螺絲等多種功能，不過在這里不要求考慮這方面的控制細(xì)節(jié)。則將情況理想化，不考慮第6個自由度對運(yùn)動的影響。 三、模型的假設(shè)1.各關(guān)節(jié)連桿在輸入指令后同時開始轉(zhuǎn)動，速度為低速，各桿件之間無摩擦，臂各旋轉(zhuǎn)軸最大運(yùn)動速度相同2.在軌跡規(guī)劃中不考慮機(jī)械臂關(guān)節(jié)轉(zhuǎn)角的限制。3.不考慮機(jī)械臂結(jié)

10、構(gòu)和裝配產(chǎn)生的誤差。4.連桿為剛體，不會發(fā)生形變5.設(shè)機(jī)器人的初始位置是在一個平面上的（y-z 平面）。6.關(guān)節(jié)視為質(zhì)點(diǎn)，它們所占的面積可忽略不計。四、符號說明：坐標(biāo)系，為固定坐標(biāo)系，其余為固定在桿上的相對坐標(biāo)系：到的坐標(biāo)系變換矩陣:尖端移動終點(diǎn)在固定坐標(biāo)系中的x方向坐標(biāo)：尖端移動終點(diǎn)在固定坐標(biāo)系中的y方向坐標(biāo) ：尖端移動終點(diǎn)在固定坐標(biāo)系中的z方向坐標(biāo) ：尖端轉(zhuǎn)動起點(diǎn)在固定坐標(biāo)系中初始x方向坐標(biāo)：尖端轉(zhuǎn)動起點(diǎn)在固定坐標(biāo)系中初始y方向坐標(biāo) ：尖端轉(zhuǎn)動起點(diǎn)在固定坐標(biāo)系中初始z方向坐標(biāo)-第i個自由度的相對于初始姿態(tài)轉(zhuǎn)角(i=1,2,3,4,5,6)-第i個自由度轉(zhuǎn)角的增量(i=1,2,3,4,5,6

11、)si-sin ci-cos五、模型建立與求解5.1運(yùn)動學(xué)模型建立與求解5.1.1機(jī)器人運(yùn)動方程的建立 為了產(chǎn)生點(diǎn)到點(diǎn)的運(yùn)動序列, 首先我們先作出初始角度均為0時的姿態(tài)，以每個節(jié)點(diǎn)為原點(diǎn)建立坐標(biāo)系到如圖5.1.1。圖-5.1.1從圖-5.1.2可以看出F點(diǎn)相當(dāng)于將繞Z軸旋轉(zhuǎn)得到，同理其他節(jié)點(diǎn)分別相當(dāng)于將前一坐標(biāo)系繞X,X,Z,X,Z軸轉(zhuǎn)動?？傻玫礁髯鴺?biāo)系相對于前一個的坐標(biāo)系的坐標(biāo)變換公式。從而得到機(jī)械臂各個關(guān)節(jié)的變換矩陣旋轉(zhuǎn)矩陣：位置矢量：圖-5.1.2：機(jī)器人連桿參數(shù)（見表格1）： 其中2確定固定坐標(biāo)系中E點(diǎn)位置由題設(shè)中，已知固定坐標(biāo)系原點(diǎn)，根據(jù)給定的連桿長度和角度，易計算得出，在固定坐標(biāo)系

12、中E位置： Ex=-65(-cos(s1)sin(s4)+(-sin(s1)cos(s2)cos(s3)+sin(s1)sin(s2)sin(s3)cos(s4)sin(s5)+65(sin(s1)cos(s2)sin(s3)+sin(s1)sin(s2)cos(s3)cos(s5)+255sin(s1)cos(s2)sin(s3)+255sin(s1)sin(s2)cos(s3)+255sin(s1)sin(s2)Ey=-65(-sin(s1)sin(s4)+(cos(s1)cos(s2)cos(s3)-cos(s1)sin(s2)sin(s3)cos(s4)sin(s5)+65(-cos(

13、s1)cos(s2)sin(s3)-cos(s1)sin(s2)cos(s3)cos(s5)-255cos(s1)cos(s2)*sin(s3)-255cos(s1)sin(s2)cos(s3)-255cos(s1)sin(s2) Ez=140-65(sin(s2)cos(s3)+cos(s2)sin(s3)cos(s4)sin(s5)+65(-sin(s2)sin(s3)+cos(s2)cos(s3)cos(s5)-255sin(s2)sin(s3)+255cos(s2)cos(s3)+255cos(s2)這樣便得到了運(yùn)動學(xué)方程。5.1.2 利用逆運(yùn)動學(xué)方法求解（1）求可用逆變換左乘運(yùn)動方程

14、式兩邊得：根據(jù)對性元素相等可解的解得:（2）求由：解得:（3）求 （1） （2）解得：其中 （4）求 或（5）求由，可以解得：或（6）求；或5.2問題11的模型在已有六自由度手臂運(yùn)動方程和逆運(yùn)動學(xué)解法的基礎(chǔ)上，若已知機(jī)械臂末端轉(zhuǎn)動終點(diǎn)坐標(biāo)和轉(zhuǎn)動起點(diǎn)坐標(biāo)已知，就可以得到各關(guān)節(jié)的坐標(biāo)，連桿的相對于初始狀態(tài)的角度變化及機(jī)械臂的姿態(tài)。5.2.1機(jī)械臂最佳姿態(tài)的確定對于一般六自由度機(jī)械臂，帶入末端坐標(biāo)（x,y,z）會得到16組，考慮用機(jī)械臂指尖實(shí)際到達(dá)的空間位置（,）到理想位置（x,y,z）的偏差距離與移動距離的比值最少來衡量機(jī)械臂是否是最佳姿態(tài)，所以我們通過定義一個參量偏移系數(shù)來尋找最佳指令，其數(shù)學(xué)模

15、型可表示為：Min :尖端移動終點(diǎn)在固定坐標(biāo)系中的x方向坐標(biāo)：尖端移動終點(diǎn)在固定坐標(biāo)系中的y方向坐標(biāo) ：尖端移動終點(diǎn)在固定坐標(biāo)系中的z方向坐標(biāo) ：尖端轉(zhuǎn)動起點(diǎn)在固定坐標(biāo)系中初始x方向坐標(biāo)：尖端轉(zhuǎn)動起點(diǎn)在固定坐標(biāo)系中初始y方向坐標(biāo) ：尖端轉(zhuǎn)動起點(diǎn)在固定坐標(biāo)系中初始z方向坐標(biāo)當(dāng)尖端移動終點(diǎn)坐標(biāo)（x，y，z）已知情況下，利用MATLAB Robotics Tool可得到T矩陣，利用逆運(yùn)動學(xué)解法，可得到，因指令中各個增量只能取到-2, -1.9, -1.8, ，1.8, 1.9, 2這41個離散值（即精度為0.1°，絕對值不超過2°），所以最終轉(zhuǎn)動的均只保留小數(shù)點(diǎn)后一位。所以對得到

16、的取位處理，然后回代入正方向運(yùn)動學(xué)方程，解得的（,）才為實(shí)際位置。通過對M的比較得到最佳。5.2.2 指令生成在已知（i=1,2,3,4,5，6）后，只需要設(shè)計一種方式使機(jī)械臂從轉(zhuǎn)到。這里根據(jù)點(diǎn)位機(jī)械臂運(yùn)動特點(diǎn)選用加速勻速減速三段式控制方式。如圖52中的方式二。規(guī)定一個加速時間T(T<L/2),當(dāng)t=T,機(jī)器人以勻速運(yùn)動，最后在目標(biāo)點(diǎn)前T時刻開始減速。這種實(shí)時控制計算量小，易于控制。在我們的程序中不防假設(shè)前10個指令加速，后10個指令減速，其他勻速運(yùn)動。程序見附錄2。（假設(shè)變速過程極快）5.3問題12的模型機(jī)械臂連續(xù)軌跡的控制問題曲線對于已給定一條空間曲線x=(s),y=(y),z=(z

17、),可將其看成一個點(diǎn)的集合。因此機(jī)械臂實(shí)現(xiàn)一個空間軌跡的過程是實(shí)現(xiàn)軌跡離散的過程。如果這些離散點(diǎn)間距很大，機(jī)械臂軌跡與要求的軌跡就有較大誤差。只有這些離散點(diǎn)彼此很接近，才有可能使機(jī)械臂的軌跡以滿足要求精確度逼近要求的軌跡。連續(xù)軌跡的控制實(shí)際上就是在多次執(zhí)行離散點(diǎn)間的點(diǎn)位控制，離散點(diǎn)點(diǎn)越密集越能逼近要求的運(yùn)動曲線。對于問題12預(yù)先指定的曲線我們先要把它離散化，把得到點(diǎn)的坐標(biāo)帶入逆運(yùn)動方程中，然后根據(jù)一定的路徑約束和實(shí)際情況的約束找出通過點(diǎn)時機(jī)械臂的姿態(tài)，然后的姿態(tài)作為初始位置問題11的算法中得到移動到的最佳指令由于問題11算法中假定各關(guān)節(jié)在一次轉(zhuǎn)動中產(chǎn)生等量指令，且各關(guān)節(jié)同時轉(zhuǎn)動所以機(jī)械臂尖端在

18、，之間運(yùn)動路徑基本相同，再加之取點(diǎn)較多，所以此處對于，之間，尖端的路徑曲線不予以考慮因此曲線運(yùn)動的最佳指令為c= 5.4問題13的模型機(jī)械手臂避障問題（指令檢驗(yàn)法）假設(shè)空間障礙物為半徑為r的球體，則這些球體空間便形成了機(jī)械手臂的約束。而躲避問題就轉(zhuǎn)化成保證機(jī)械手臂上的點(diǎn)與障礙球球心距離始終大于r的問題。根據(jù)運(yùn)動規(guī)律，不難知道手臂相對于初始位置的姿態(tài)決定于之前執(zhí)行的所有指令的和。已知連桿上的點(diǎn)L在其所在的相對坐標(biāo)系中的坐標(biāo)及轉(zhuǎn)動的指令，根據(jù)齊次坐標(biāo)變換矩陣就可得到L在固定坐標(biāo)系中的坐標(biāo)，然后可計算距離。所以 可以用問題1-1算法產(chǎn)生點(diǎn)到點(diǎn)的指令，可以利用迭代法從初始位置開始提前檢驗(yàn)每個指令，不滿

19、足要求的無法執(zhí)行。根據(jù)這些要求建立執(zhí)行檢驗(yàn)?zāi)Ｐ腿缦拢海?15）此模型的目標(biāo)函數(shù)是一個符合的函數(shù)，目標(biāo)函數(shù)表征機(jī)械臂上的點(diǎn)與目標(biāo)點(diǎn)的距離，若與內(nèi)壁的距離小于r，就會突然變大，從而不滿足目標(biāo)函數(shù)。六、模型的驗(yàn)證6.1 問題2-1的求解根據(jù)問題11的模型算法，計算得出以連桿DE從空間起始點(diǎn)指向工具箱位置（20，-200，120）的姿態(tài)，通過機(jī)械臂逆運(yùn)動學(xué)計算得出它到這點(diǎn)的角度為（-84.30,61.00,-43.30,0,-17.60）。把得到的角度代入指令生成算法中得到：（0，0，-1.6，0，-1,6，0）；（-0.1，-0.1，-1.6，0，-1,6，0）；（-0.2，0.1，-1.6，0，-

20、1,6，0）等，詳見answer1.xls。MATLAB Robotic仿真如下（程序見附件2）。表61 關(guān)節(jié)角度誤差6.2.2對模型二的仿真對圖中曲線采用1-2模型解得指令見answer2，（程序見附件3）七、模型的評價1、基于齊次坐標(biāo)變換法的點(diǎn)位控制模型評價：該模型可以適用于所有已知目標(biāo)位置和姿態(tài)的點(diǎn)位控制問題。算法的計算效率非常的高，并且實(shí)時控制性好。算法誤差產(chǎn)生于多次三角變換求解所帶來的舍入誤差。2、連續(xù)軌跡控制模型評價：該模型適用于所有參數(shù)方程的空間曲線生成機(jī)器指令。適用于機(jī)器人在低速運(yùn)動的情況。但離散點(diǎn)數(shù)量較少時，會產(chǎn)生誤差。3、指令檢驗(yàn)法模型評價：該模型精度高，易于理解。但設(shè)計算

21、法上較為復(fù)雜。參考文獻(xiàn)參考文獻(xiàn)按正文中的引用次序列出，其中：1機(jī)械運(yùn)動學(xué)教程2 機(jī)器人手臂下棋運(yùn)動的逆運(yùn)動學(xué)分析與仿真 趙艷云機(jī)械臂參數(shù)關(guān)節(jié)變量符號初始位置變量范圍10± 1802-90± 12530± 13840± 2705-90-120 +133.560± 270表 1：附件：問題1-1nx=T06(1,1);ny=T06(2,1);nz=T06(3,1);ox=T06(1,2);oy=T06(2,2);oz=T06(3,2); ax=T06(1,3);ay=T06(2,3);az=T06(3,3); px=T06(1,4);py=T06(

22、2,4);pz=T06(3,4); n=nx ny nz; o=ox oy oz; a=ax ay az; p=px py pz;theta=zeros(6,100);for i=2:100 %theta1 theta11=atan(py-65*ay)/(px-65*ax); theta12=pi+atan(py-65*ay)/(px-65*ax); if theta11-theta(i-1,1)>=theta12-theta(i-1,1) theta(i,1) =theta12; else theta(i,1) =theta11; end %theta3theta31=asin(1)+

23、atan(1/6);theta32=pi-asin(1)+atan(1/6);if theta31-theta(i-1,3)>=theta32-theta(i-1,3) theta(i,3) = theta32; else theta(i,3) = theta31;end %theta2 w=atan(c1(i,j)*px+s1(i,j)*py-65*(c1(i,j)*ax+s1(i,j)*ay)/(pz-65*az); B=(255*c3(i,j)/sqrt(c1(i,j)*px+s1(i,j)*py-65*c1(i,j)*ax-65*s1(i,j)*ay)2+(pz-p*az)2);

24、 theta21=asin(B-w-theta(i,3); theta22=pi-asin(B)-w-theta(i,3); if theta21-theta(i-1,2)>=theta22-theta(i-1,2) theta(i,2) = theta22; else theta(i,2) = theta21; end %theta5theta51=acos(s231(i,j)*(c1(i,j)*ax+s1(i,j)*ay)-c231(i,j)*az);theta52=2*pi-acos(s232(i,j)*(c1(i,j)*ax+s1(i,j)*ay)-c232(i,j)*az);i

25、f theta51-theta(i-1,5)>=b5-theta(i-1,5) theta(i,5) = theta52; else theta(i,5) = theta51;end %theta4 theta41=asin(s511(i,j)*ax-c1(i,j)*ay)/s511(i,j); theta42=pi-asin(s1(i,j)*ax-c1(i,j)*ay)/s511(i,j); if atheta41-theta(i-1,4)>=theta42-theta(i-1,4) theta(i,4) = theta42; else theta(i,4) = theta41;

26、 end %theta6 theta61=asin(sin(c1(i,j)*ox+s1(i,j)*oy)-c231(i,j)*oz)/s511(i,j); theta62=pi-asin(s231(i,j)*(c1(i,j)*ox+s1(i,j)*oy)-c231(i,j)*oz)/s511(i,j); if theta61-theta(i-1,6)>=theta62-theta(i-1,6) theta(i,6) = theta62; else theta(i,6) = theta61; end end問題2-1T06=transl(20,-200,120); nx=T06(1,1);

27、ny=T06(2,1);nz=T06(3,1); ox=T06(1,2);oy=T06(2,2);oz=T06(3,2); ax=T06(1,3);ay=T06(2,3);az=T06(3,3); px=T06(1,4);py=T06(2,4);pz=T06(3,4); n=nx ny nz; o=ox oy oz; a=ax ay az; p=px py pz;theta=zeros(7,6);for i=2:100 %theta1 theta11=atan(py-65*ay)/(px-65*ax); theta12=pi+atan(py-65*ay)/(px-65*ax); if thet

28、a11-theta(i-1,1)>=theta12-theta(i-1,1) theta(i,1) =theta12; else theta(i,1) =theta11; end %theta3 theta31=asin(1)+atan(1/6); theta32=pi-asin(1)+atan(1/6); if theta31-theta(i-1,3)>=theta32-theta(i-1,3) theta(i,3) = theta32; else theta(i,3) = theta31; end c1(i)=cos(theta(i,1); s1(i)=sin(theta(i,

29、1); c3(i)=cos(theta(i,3); s3(i)=sin(theta(i,3); c23(i)=cos(theta(i,1)+theta(i,3); s23(i)=sin(theta(i,1)+theta(i,3); %theta2 w(i)=atan(c1(i)*px+s1(i)*py-65*(c1(i)*ax+s1(i)*ay)/(pz-65*az); B(i)=(255*c3(i)/sqrt(c1(i)*px+s1(i)*py-65*c1(i)*ax-65*s1(i)*ay)2+(pz-1*az)2); theta21=asin(B(i)-w(i)-theta(i,3);

30、theta22=pi-asin(B(i)-w(i)-theta(i,3); if theta21-theta(i-1,2)>=theta22-theta(i-1,2) theta(i,2) = theta22; else theta(i,2) = theta21; end %theta5theta51=acos(s23(i)*(c1(i)*ax+s1(i)*ay)-c23(i)*az);theta52=2*pi-acos(s23(i)*(c1(i)*ax+s1(i)*ay)-c23(i)*az);if theta51-theta(i-1,5)>=theta52-theta(i-1,5) theta(i,5) = theta52; else theta(i,5) = theta51;end c5(i)=cos(theta(i,