機械臂的軌跡規(guī)劃

1、機械臂運動的軌跡規(guī)劃摘要空間機械臂是一個機、電、熱、 控一體化的高集成的空間機械系統(tǒng)。隨著科技的發(fā)展，特別是航空飛機、機器人等的誕生得到了廣泛的應用，空間機械臂作為在軌跡的支持、服務等以備受人們的關注。本文將以空間機械臂為研究對象，針對空間機械臂的直線運動、關節(jié)的規(guī)劃、空間直線以及弧線的軌跡規(guī)劃幾個方面進行研究，對機械臂運動和工作空間進行了分析，同時對機械臂的軌跡規(guī)劃進行了驗證，利用MATLA瞰件對機械臂的軌跡進行仿真， 驗證算法的正確性和可行性， 同時此路徑規(guī)劃方法可以提高機械臂的作業(yè)效率，為機械臂操作提高理論指導，為機器人更復雜的運動仿真與路徑規(guī)劃打下基礎。本文一共分為四章：第一章，首先總

2、結了機械臂運動控制與軌跡規(guī)劃問題的研究現(xiàn)狀及研究方法， 歸納了各種軌跡規(guī)劃的算法及其優(yōu)化方法，闡述了機械臂的研究背景和主要內容。第二章， 對機械臂的空間運動進行分析研究，采用抽樣求解數(shù)值法蒙特卡洛方法，進行機械臂工作空間求解，同時在 MATLAB中進行仿真，直觀展示機械臂工作范圍，為下一章的軌跡規(guī)劃提供理論基礎；同時通過D-H 參數(shù)法對機械臂的正、逆運動分析求解，分析兩者的區(qū)別和聯(lián)系。第三章， 主要針對軌跡規(guī)劃的一般性問題進行分析，利用笛卡爾空間的軌跡規(guī)劃方法對機械臂進行軌跡規(guī)劃,同時利用MATLAB對空間直線和空間圓弧進行軌跡規(guī)劃，通過仿真驗證算法的正確性和可行性。第四章，總結全文，分析本文

3、應用到機械臂中的控制算法，通過MATLAB結果可以得出本文所建立的算法正確性，能夠對機械臂運動提供有效的路徑，而且改進了其他應用于空間機械臂的路徑規(guī)劃問題。關鍵詞 】 運動分析工作空間算法研究軌跡規(guī)劃ABSTRACTSpace manipulator is a machine, electricity, heat, charged with high integration of space mechanical system integration. With the development of science and technology, especially the birth of

4、 aviation aircraft, a robot has been widely used, the trajectory of space manipulator as the support and services to peoples attention. This article will space manipulator as the research object, according to the linear motion of the space manipulator, joint planning, space of the straight line and

5、curve, the trajectory planning of several aspects of mechanical arm movement and working space are analyzed, and the trajectory planning of manipulator is verified, the trajectory of manipulator is to make use of MATLAB software simulation, verify the correctness and feasibility of the algorithm, at

6、 the same time this path planning method can improve the efficiency of mechanical arm, improve the theoretical guidance for mechanical arm operation, simulation and path planning for robot more complicated movement.This article is divided into four chapters altogether:The first chapter, first summar

7、izes the mechanical arm motion control and path planning problem research status and research methods, summarizes the variety of trajectory planning algorithm and the method of optimization, and expounds the research background and main content of mechanical arm.The second chapter, the paper studied

8、 the space motion of mechanical arm, the numerical method, monte carlo method are deduced with the method of sampling, the workspace for mechanical arm is, at the same time the simulation in MATLAB, intuitive display mechanical arm work scope, providing theoretical basis for the next chapter of traj

9、ectory planning. At the same time through d-h method of positive and inverse kinematic analysis of the mechanical arm, analyze the difference and contact.The third chapter, mainly aims at the general problem of trajectory planning is analyzed, using cartesian space trajectory planning method for tra

10、jectory planning, mechanical arm at the same time, MATLAB is used to analyse the spatial straight line and arc trajectory planning, through the simulation verify the correctness and feasibility of the algorithm.The fourth chapter, summarizes the full text, analysis of the control algorithm is applie

11、d to the mechanical arm in this paper, through the MATLAB results can be concluded that the correctness of algorithm, can provide effective path of mechanical arm movement, and improved the other used in space manipulator path planning problem.key words motion analysis,work space,trajectory planning

12、,algorithm research目錄摘 要 -.1. -ABSTRAC.T-.2. -第一章緒論 -.5.-第一節(jié)研究背景及意義-.5-第二節(jié)國內外發(fā)展現(xiàn)狀-.6-一、國內現(xiàn)狀-.6.-二、國外現(xiàn)狀-.6.-第二章機械臂的運動分析-.8-第一節(jié)機械臂的正運動學分析-.8-第二節(jié)機械臂的逆運動學求解-.10-第三章五軸機械臂軌跡規(guī)劃與仿真-.11-第一節(jié)軌跡規(guī)劃一般問題-.11-第二節(jié)關節(jié)空間的軌跡規(guī)劃-.12-一、三次多項式插值法-.12-二、五次多項式插值-.15-第三節(jié)笛卡爾空間的軌跡規(guī)劃-.17-一、空間直線軌跡規(guī)劃-.18-二、空間圓弧的軌跡規(guī)劃-.21-三、一般空間軌跡規(guī)劃-.

13、25-第四章 總結與展望-.3. 0 -參考文獻-.3.1 -第一章 緒論第一節(jié) 研究背景及意義隨著宇宙空間的開發(fā)，70 年代美國提出了在宇宙空間利用機器人系統(tǒng)的概念， 并且在航天飛機上實施。當初的空間機器人是由航天飛機艙內的宇航員通過電視畫面操縱的。隨著空間技術的進一步發(fā)展使得未來空間操作任務急劇增加，空間站的建立、維修，衛(wèi)星的回收、釋放等工作會越來越多。如果所有這些工作都依靠宇航員來完成，其成本將十分高昂，也是十分危險的，因為惡劣的太空環(huán)境會給宇航員的空間作業(yè)帶來巨大的威脅。宇航員的艙外作業(yè)需要龐大而復雜的環(huán)境控制系統(tǒng)、生命保障系統(tǒng)、物質供給系統(tǒng)、救生系統(tǒng)等的支持，這些系統(tǒng)不但具有很高的技

14、術難度，而且成本巨大。用空間機器人代替宇航員進行太空作業(yè)不僅可以使宇航員避免在惡劣太空環(huán)境中工作時可能受到的傷害，還可以降低成本， 提高空間探索的效益?？臻g機械臂是空間機器人的一種，已被考慮在未來的空間活動中承擔大型空間站的在軌安裝及對失效飛行器的的捕捉與維修，土壤和巖石的取樣等；并期望其在無人狀態(tài)下承擔未來空間實驗室或工廠的日常工作。根據(jù)空間作業(yè)的需要，空間機器人上一般都安裝了一個或多個模仿人手臂的多自由度機器臂。隨著我國國民經(jīng)濟與國防工業(yè)技術的迅速發(fā)展，對航天器的需求量日益增加，對其能力的要求日臻提高。特別是空間站在軌服務、深空探測等空間技術領域的迅速發(fā)展，對于空間機械臂技術的需求越來越迫

15、切，而且對其工作能力和性能要求越來越高，對其安全性、壽命等方面也提出了越來越高的要求。此外， 受國外在高技術領域的技術限制與封鎖，使得我們必須堅持自力更生、獨立自主的高技術研發(fā)道路，堅持自主創(chuàng)新的思想，加速并加強空間機械臂技術的研發(fā)工作1。將機器人用于空間服務，一項關鍵技術就是路徑規(guī)劃。路徑規(guī)劃研究是機器人研究領域中的一個重要分支,是機器人導航中最重要的任務之一。對已知靜態(tài)環(huán)境中機器人路徑規(guī)劃的研究已經(jīng)進行了將近40 年， 路徑規(guī)劃問題的研究有很大的價值。多年的研究工作在取得進展的同時，愈加證明了路徑規(guī)劃是一個復雜路徑規(guī)劃算法的計算量取決于任務、環(huán)境的復雜性以及對規(guī)劃路徑質量的要求， 一個好的

16、路徑規(guī)劃算法應該兼顧對規(guī)劃速度和路徑質量的期望。隨著研究的深入，各種新的路徑規(guī)劃方法層出不窮，使路徑規(guī)劃研究一直活躍在機器人學領域。目前國內對空間機械臂研究還處于起步階段，因此開展空間機械臂相關領域的研究將極大促進我國空間科學試驗、空間維護與建設、深空探測等空間技術的發(fā)展。 本論文根據(jù)課題的技術要求，將空間機械臂路徑規(guī)劃作為切入點，研究路徑規(guī)劃問題，其研究成果具有重要的理論指導意義和工程應用價值。第二節(jié) 國內外發(fā)展現(xiàn)狀一、國內現(xiàn)狀我國的工業(yè)機器人從80 年代“七五”科技攻關開始起步，目前已基本掌握了機器人操作機的設計制造技術、控制系統(tǒng)硬件和軟件設計技術、運動學和軌跡規(guī)劃技術，生產(chǎn)了部分機器人關

17、鍵元件，開發(fā)出噴漆、弧焊、點焊、裝配、搬運等機器人；但總的來看，我國工業(yè)機器人技術及其工程應用的水平和國外比還是有一定的距離，如：可靠性低于國外產(chǎn)品；機器人應用工程起步較晚，應用領域窄，生產(chǎn)線系統(tǒng)技術與國外比有差距。我國的智能機器人和特種機器人在“ 863” 計劃的支持下，也取得不少成果。其中最突出的是水下機器人，6000 米水下無纜機器人的成果居世界領先水平，還開發(fā)出直接遙控機器人、雙臂協(xié)調控制機器人、爬壁機器人、管道機器人等機種； 在機器人視覺、力覺、 觸覺、 聲覺等基礎技術的開發(fā)應用上開展了不少工作，有一定的發(fā)展基礎。但是在多傳感器信息融合控制技術、遙控加局部自主系統(tǒng)遙控機器人、只能裝配

18、機器人、機器人化機械等的開發(fā)應用方面則剛剛起步，與國外先進水平差距較大。二、國外現(xiàn)狀美國是機器人的誕生地，早在1962 年就研制出世界上第一臺工業(yè)機器人，比起號稱“機器人王國”的日本起步至少早五六了年。 1971 年，通用汽車公司又第一次用機器人進行點焊。西歐時僅次于日美機器人的生產(chǎn)基地，也是日美機器人的重要市場。早在1966 年，美國Unimation 公司的尤尼曼特機器人和AMF 公司的沃莎特蘭機器人就進入英國市場。接著，英國Hall Automation 公司研制出自己的機器人RAMP。德國工業(yè)機器人的總數(shù)占世界第三。德國對于一些有危險、有毒、 有害的工作崗位，必須以機器人替代普通人的勞

19、動。同時提出了1985 年以后要向高級的、帶感覺的智能型機器人轉移目標。1954 年：美國人戴沃爾制造了世界第一臺可編程的機械手。1959 年：戴沃爾與美國發(fā)明家英格伯格聯(lián)手制造出第一臺工業(yè)機器人。1962 年：美國AFM 公司生產(chǎn)出萬能搬運機器人，與Unimation 公司生產(chǎn)的萬能伙伴機器人一樣成為真正商業(yè)化的工業(yè)機器人。1967 年：日本川崎重工公司與豐田公司分別從美國購買了工業(yè)機器人Unimat和Verstran的生產(chǎn)許可，開始對機器人的研究和制造。1968 年：美國斯坦福研究所公布他們研制的機器人Shakey。1973年：世界上機器人和小型計算機第一次攜手合作，誕生了機器人T3。19

20、79 年：日本山梨大學發(fā)明了平面關節(jié)機器人SCARA。1984年：英格伯格在此推出機器人 Helpmate,這種機器人能在醫(yī)院為病人送飯送藥和送郵件。1996年：本田公司推出仿人型機器人 P2,雙足行走機器人的研究達到了一個新的高度。2002年：美國iRobot公司推出了吸塵器機器人 Roombar,為世界上商業(yè)化 最成功的家用機器人。2006年：微軟公司推出Microsoft Robitics Studio機器人，從此機器人模塊化 平臺同一化的趨勢越來越明顯。在工業(yè)機器人技術方面，工業(yè)機器人有操作機（機械本體）、控制器、伺服驅動系統(tǒng)和檢測傳感器裝置構成，是一種仿人操作、自動控制、可重復編程、

21、能在三維空間完成各種作業(yè)的機電一體化自動化生產(chǎn)設備。第二章 機械臂的運動分析機械臂的運動是其軌跡出現(xiàn)的直接原因。所以軌跡規(guī)劃的前提是機械臂的運動分析1。本文通過對機械臂的正運動學和逆運動學進行求解，分析兩者的區(qū)別和聯(lián)系。 通過對五軸機械臂關于坐標系幾何關系，針對常見軌跡規(guī)劃方案中起始和終止階段進行研究，分析研究結果。第一節(jié) 機械臂的正運動學分析機械臂從關節(jié)空間到末端笛卡爾空間的變換是正向運動學描述。由坐標系中已知的各個關節(jié)角度，求解機械臂末端相對應于原點坐標系的位置和位姿。設矩陣 A 表示機械臂連桿的齊次變換：AiRot(Z, i )Trans ( i,0,ldi)Trans(an 1,0,0

22、)Rot(X, i)cos isin i0ai 1sin i cos i 1cos i cos i 1sin i 1sin i 1dii i 1i i 1i 1i 1 i(2-1)sin i sin i 1cos i sin i 1cos i 1cos i 1di0001由于機械臂全是旋轉關節(jié)。對于文中采用的機械臂而言有五個其次變換矩陣，則末端連桿坐標系相對于基坐標系的齊次變換矩陣r11r12r13px0r21r22r23py05TA1A2A3A4A5212223 y(2-2)r31r32 r33pz0001式即為機械臂的運動方程，它反應各關節(jié)變量與機械臂末端位姿之間的關系，上式左邊的五個矩陣

23、含有五個關節(jié)變量q 1, 2, 3, 4, 5。方程右邊為描述機械臂末端關節(jié)位置和姿態(tài)的齊次矩陣，由剛體姿態(tài)的描述可知r11，r12，r13，21,22, 3,31,32,33分別為機械臂末關節(jié)坐標系的三個坐標軸與機械臂基坐標系三個坐標軸的方向余弦，Px, py, Pz為機械臂末關節(jié)的坐標原點在機械 臂基坐標系中的三維坐標。機械臂正運動學求解就是已知各連桿的關節(jié)變量求解末端連桿的位姿矩陣。即已知關節(jié)變量q 1, 2, 3, 4, 5T ,求解上式機械臂運動學方程中等式右邊矩10將上式中的機械臂五個關節(jié)的齊次變換矩陣帶入，即計算出50T 中各元素值 為：r11r12r13px(2-3)0r21r

24、22r23py5Tr31r32r33pz0001其中：r11c1c234c5s1s5r12c1c234s5s1c5r13c1c23s4c1s23c4r21s1c234 c5c1s5r22s1c234s5c1c5s1c23s4s1s23c4r31s234 c5r32s234 s5r11c234pxc1c23a3c1c2a2s1ld 2c1a1pys1c23a3s1c2a2c1ld 2s1 a1pzs23a3s2a2ld1其中，c1cos 1 ， s1sin 1 ， c234 cos( 24) ， s234 sin( 24) 。第二節(jié)機械臂的逆運動學求解機械臂的逆運動學解是對其運動學正解的反解，因

25、而已知量和求解量相反， 即已知機械臂末端的位置姿態(tài)對機械臂進行驅動，使各個關節(jié)從此刻的姿態(tài)運動到與末端位姿相對應的位置，進而得到關節(jié)變量 11。機械臂的運動學正、逆求解實質是機械臂關節(jié)空間與工作空間之間的非線性 映射關系，兩者可相互轉換。關系圖如下所小。桿件參數(shù)關節(jié)角量運動學正解J端執(zhí)行器位姿_關節(jié)角量u逆解選取y運動學逆解lzT桿件參數(shù)圖1關節(jié)空間與工作空間的關系機械臂的逆運動學問題，指已知機械臂的末端位姿，即已知齊次變換矩陣0T ,求解各轉動關節(jié)的角度i。機械臂的逆運行學問題，可以理解為通過運動學方程：0_ 0 _1 _ 2_345T 1 T( 1)2T( 2)3T( 3)4T( 4)5T

26、( 5)(2-4)求解i。整理式，將含有1的部分移到方程的左邊0_10_ 1 _ 2_3_4_0T(1) 15T 2 T( 2)3T( 3)4T( 4)4T( 5)(2-5)S1PxC1Py(2-7)將10T轉置，上式可以表達成為:C1S100r11r12r13PxSiC100r21r22r23Py1 -(2-6)01d15 0r31r32r33d100010001假設上式的兩邊元素和式相等，得到:可以得出1的解第三章五軸機械臂軌跡規(guī)劃與仿真目前關于空間軌跡規(guī)劃的方法主要有三種， 三次多項式插值，高階多項式插 值以及樣條曲線等方法。主要討論軌跡在關節(jié)空間中的位移、 速度與加速度等變 量的關系。

27、規(guī)劃實質是根據(jù)需求，計算出預定的軌跡曲線，在軌跡規(guī)劃時可以再 運動學與動力學的基礎上進行規(guī)劃，所以規(guī)劃是建立在運動學和動力學基礎上 的。圖2機械臂的matlab生成第一節(jié)軌跡規(guī)劃一般問題軌跡規(guī)劃的一般方法是在機械臂末端的初始和目標位置之間用多項式函數(shù)“內插”來抵近給定的路徑，并沿著時間軸產(chǎn)生一系列的可供操作機使用的“控制設定點” 3。其中關節(jié)坐標和笛卡爾坐標都可以對路徑端點進行給出。一般是 在笛卡爾坐標中給出，由于在笛卡爾坐標中機械臂末端形態(tài)更容易觀察。所以通常采用笛卡爾方法。在給定的兩端之間，常有多條可能路徑。可以沿著直線和光滑多項式曲線運 動。本文將討論插值法，研究滿足路徑約束的簡單軌跡規(guī)

28、劃3。第二節(jié) 關節(jié)空間的軌跡規(guī)劃機械臂關節(jié)空間的軌跡規(guī)劃解決機械臂從起始位姿到終止位姿去取放物體的問題 .機械臂末端移動的過程并不重要，只要求運動是平滑的且沒有碰撞產(chǎn)生.在關節(jié)空間中進行軌跡規(guī)劃時，算法簡單、工具移動效率高、關節(jié)空間與直角坐標空間連續(xù)的對應關系是不存在的，因此機構的奇異性問題一般不會發(fā)生。對于無路徑的要求，應盡量在關節(jié)空間進行軌跡規(guī)劃。一、三次多項式插值法三次多項式與其一階導數(shù)函數(shù)，總計有四個待定系數(shù)，對起始點和目標點兩者的角度、角角速度同時給出約束條件，本文采用的是三次多項式插值法5?？梢詫νㄟ^空間的n 個點進行分析并進行軌跡規(guī)劃，讓速度和加速度在運動過程中保持軌跡平滑。本文

29、算法可以實現(xiàn)對(n 1)段中的每一段三次多項式系數(shù)求解，為了方便，對其進行歸一化處理。(1)時間標準化算法根據(jù)三次多項式軌跡規(guī)劃流程，對每個關節(jié)進行軌跡規(guī)劃時需要對(n 1)段的軌跡進行設計，為了能對(n 1)個軌跡規(guī)劃方程進行同樣處理，本文首先設計了時間標準化算法將時間進行處理，經(jīng)過處理后的時間t 0,1。首先定義：t：標準化時間變量，t 0,1;：未標準化時間，單位為秒；i ：第 i 段軌跡規(guī)劃結束的未標準化時間，i i i 1 ；機械臂執(zhí)行第i 段軌跡所需要的實際時間：t ( i 1)/( i i 1) ，其中i 1, i,t 0,1。時間歸一化后的三次多項式為：y A0 A1t A2t

30、2 A3t3(2)機械臂軌跡規(guī)劃算法實現(xiàn)過程已知初始位置為1 ；給定初始速度為0；已知第一個中間點位置2，它也是第一運動段三次多項式軌跡的終點；為了保證運動的連續(xù)性，需要設定 2所在點為三次多項式軌跡的起點， 以確保運動的連續(xù)；為了保證2處速度連續(xù)，三次多項式在2處一階可導；為了保證2處加速度連續(xù)，三次多項式在2處二階可導；以此類推，每一個中間點的位置 i(2 i (n 1),都一定要在其原運動 段軌跡的終點，并且也是它后運動段的起點。 i 1 的速度保持連續(xù)； i 1 的加速度保持連續(xù)；點位置no給定終點速度，設其為00(3)約束條件第一個三次曲線為：(t)a10a11ta12t 2a13t

31、3第二個三次曲線為：(t )a20a21ta22ta23t第三個三次曲線為：(t )a30a31ta32t2a33t 3第 ( n 1) 個三次曲線為：(t) a(n 1)0 a(n 1)1t a(n 1)2ta(n 1)3t在同一時間段內，三次曲線每次的起始時刻t 0 ，停止時刻ttn ，其中i1.n 。在標準化時間t0處，設定1 為第一條三次多項式運動段的起點，可以得出：110；在標準化時間t 0處，三次多項式運動段第一條的初始速度是已知變量，所以得出：1 a110；第一中間點位置2與第一條三次多項式運動段在標準化時間t tn時的終點相同，所以可以得出：2 a10 a11tf1 a12tf

32、1 a13tf1 ；第一中間點位置2與第一運動段在標準化時間t 0時起點相同，所以得出： 2a20 ；三次多項式在2處一階可導，因此可得出：2a11 2a12tf1 3a13tf21a21；三次多項式在2處二階可導，因此可得出：22a12 6a13tf12a22；第二個空間點的位置3與第二運動段在標準化時間tl2時的終點相同，所以有:233a20a21t f 2a22t f2a23t f 2 ；第二個中間點的位置3 應與第三運動段在標準化時間t 0 時起點相同，所以有：3a30 ；三次多項式在3處一階可導，從而有：3a21 2a22t f 2 3a23t2f2a31；三次多項式在3 處二階可導

33、，從而有：32a22 6a23t f 2 2a32；? 第 (n 2) 個中間點位置n 1和第 (n 1)運動段在標準化時間tf(n 2)時的終點相同，所以有：23n 1 a(n 2)0a(n 2)1t f(n 2)a(n 2)2tf (n 2)a(n 2)3t f (n 2)? 第 (n 2) 個中間點位置n 1 應與下一運動段在標準化時間t 0時的起點位置相同，所以有n 1a(n 1)0；? 三次多項式在第(n 2)個中間點處一階可導，從而：(3-1)(3-2)2(n 1)a(n 2)12a(n 2)2tf (n 2)3a(n 2)3tf(n 2)a( n 1)1? 三次多項式在第(n 2

34、)個中間點處二階可導，從而：( n 1)2a(n 2) 2 6a(n 2)3t f (n 2) 2a(n 1)2?因此可以得出所有軌跡終點在標準化時間tn時的位置n為:(3-3)(3-4)2 3na(n 1)0a(n 1)1tfna(n 1)2tfna(n 1)3tfn?因此可以得出所有軌跡終點在標準化時間tn時的速度n為:2n a(n 1)12a(n 1)2tfn3a(n 1)3tfn以上公式改寫為矩陣為：C M1。由該矩陣計算M1可以求出軌跡規(guī)劃的全部參數(shù)，（由五軸機械臂運動學逆解求出）于是求得（n 1）段的運動方 程，從而使五軸機械臂末端執(zhí)行器經(jīng)過所給定的位置坐標。通過以上分析可以確定機

35、械臂在滿足速度要求的兩個位姿之間運動時各個 關節(jié)軸的角度變化曲線。如下圖3所示是MATLAB仿真分析三次多項式插值：機 械臂某關節(jié)角在4秒內由初始點A經(jīng)過中間點B到達終點C的變化情況。三個 位置點的速度和角速度如下所示：A 30B 60C 40A 20B 30C 20圖中實線為角度變化曲線，虛線為角速度變化曲線。關節(jié)角度曲線平滑，而 速度曲線在中間點B處出現(xiàn)突變。圖3三次多項式插值法、五次多項式插值五次多項式共有六個待定系數(shù)，要想六個系數(shù)得到確定，至少需要六個條件五次多項式可以看作是關節(jié)角度的時間函數(shù)，因此其一階可導和二階可導分別可以看作是關節(jié)角速度和關節(jié)角加速度的時間函數(shù)。五次多項式及一階、

36、二階導數(shù)公式如下：(t) c0 cit c2t2cj3c4c5t5 Cl 2c2t 3c3t2 4c4t3 5c5t4 2c2 6c3t 12C4t2 20C5t3(3-5)(3-6)(3-7)為了求得待定系數(shù)c。, c3, c4,c5，對起始點和目標點同時給出關于角度和角加速度的約束條件:(to)c。cit。c?t；c3t3 c c5t5(3-8)(tf)c。Gtfc42 c cfcj：(3-9)(%)ci3c3t24c.3 5c5t04(3-10)(tf)G 2c2tf 3c3t2 4c4t3 5c5t4(3-11)(t。)2c2 6c山 12c4to 224；(3-12)(3-13)始點

37、和目標點的關節(jié)角速度,c320 f 20 0c2(8 f。212。)tf_2(3 0 f)tf2t3(3-14)c,c530 0 30 f (14 f 16 0)tf (3。2 f)t22t4_212 f 12。(6 f 6 0)tf (。 f)tf2t5為了對比三次多項式關節(jié)插值算法和五次多項式插值算法的效果，同樣要求 機械臂從起始點開始運動，經(jīng)過4秒到達終點，仿真時起始點和目標點的關節(jié)角 速度為00中間點的關節(jié)角加速度還可以對相鄰兩段軌跡角加速度進行平均值求 解，使該值為中間點的瞬時加速度12。利用MATLAB對五次多項式插值進行仿真, 將結果與三次多項式插值進行對比，發(fā)現(xiàn)三個位置點的速度

38、、角速度兩種方法相 同，同時增加角加速度約束:A30B60C40a20b30c20a 2 b 4 c 2仿真結果如圖4所示，圖中實線和虛線分別表示角度變化曲線、 角速度變化 曲線。點線則表示角加速度曲線。其中關節(jié)角度和角速度曲線顯示的都相對平滑， 而角加速度曲線在中間點B處變化稍大。結果分析得出，多項式插值法雖然計算 量有所增加，但是其關節(jié)空間軌跡平滑、運動穩(wěn)定，且階數(shù)越高滿足的約束項越 多。001-100-150 -00.511.522.533.54Time(s)50圖4五次多項式插值法第三節(jié) 笛卡爾空間的軌跡規(guī)劃在機械臂的笛卡爾空間軌跡規(guī)劃中，中間點即插補點的坐標可以通過插補算 法得到。得

39、到中間點后，在把中間點的位姿轉換成相應的關節(jié)角， 再通過對關節(jié) 角的控制，使得機械臂的末端能按照預先規(guī)劃的路徑運動。機械臂的笛卡爾空間軌跡規(guī)劃位姿控制過程大致如下所示:圖5機械臂笛卡爾空間軌跡規(guī)劃控制過程空間直線和空間弧線的軌跡規(guī)劃是笛卡爾空間中不可或缺的兩部分。因為空間的曲線可以分割為許多直線和弧線；但是也有會出現(xiàn)直線或弧線連接處尖角問 題，為了使運動軌跡連續(xù)平滑，本文采用圓弧過度來平滑尖角。在笛卡爾空間中， 空間直線和空間弧線的軌跡規(guī)劃是最常見的兩部分，其他空間曲線可以通過這兩者來逼近。一、空間直線軌跡規(guī)劃所謂空間直線插補就是在該直線起始點位姿已知的情況下，對軌跡中間點 (插補點)的位姿坐

40、標進行求解。直線插補法：設已知起始點的位置坐標分別為：Po(xo, yo,Zo) , Pf (Xf, yf, Zf) , p0和Pf為相對基礎坐標系計算其長度：、2、2.、2L . (xo Xf) (yo yf) (zo Zf)(3-15)求間隔內行程，需要分勻速、加速、減速三種情況進行討論：勻速：設速度為v ,則插補周期Ts內行程為di vTs ；加速：設加速度為 a ,起始點速度為v ,則在插補周期內的行程為:1d2 VoTs -aTs2 ；整個加速度的路程為:2白，時間記為i2計算總時間：tti t2 t3 ；計算插補點數(shù)：N ;Ts對插補點所在段進行判斷(勻速段、加速段、減速段)，使各

41、軸的增量得到確定，對各插補點坐標進行實時計算根據(jù)坐標值，通過運動學逆解求出各關節(jié)角。利用五次多項式插值法對關節(jié)角的插值計算。從以上各式分析可以看出，機械臂完成一個空間軌跡的過程，是實現(xiàn)估計 離散點的過程。讓其盡量逼近，使機械臂軌跡盡可能的符合規(guī)劃好的運動軌跡， 本次采用定時插補法。為了使機械臂的性能更好，讓末端執(zhí)行器的軌跡更平滑，在相鄰兩個插值點的關節(jié)角間選取插補函數(shù)使關節(jié)軸運動更加穩(wěn)定。此方法將笛卡爾空間、關節(jié)空間相結合。如： 工具末端沿著一個直線運動，通過上面的計算把直線段上插補199 次即整體直線軌跡分為200 個點， 每個坐標點進行逆運動學求解得到200 組關節(jié)角度值。最后通過關節(jié)空間

42、軌跡規(guī)劃的方法將相鄰的兩組關節(jié)角之間進行角度插補， 從而使工具末端的軌跡平滑且能很好的控制每個關節(jié)的角速度和角加速度 8。在MATLAB中利用上述直線插補方法對機械臂進行正方形軌跡規(guī)劃仿真，機械臂的末端由起始點A,經(jīng)過B點、C 點、 D 點返回A 點。其中點A、 B、 C、 D的位姿分別用齊次變換矩陣表示為:10TA0001000010410601801TB100001000010290601801(3-16)TC10000100001029060 TD1801100001000010410601801(3-17)正方形的每個邊長為120mm,每個邊上插補30步，總仿真時間為120s。正方形軌

43、跡的仿真結果如圖6 所示， 通過運動學求解得到五個關節(jié)角的位移數(shù)據(jù)并生成相關的數(shù)據(jù)曲線，如圖7 所示。位姿1時關節(jié)角1軌跡規(guī)劃曲線11.522.53時間t/s0246co 024680 - - - - 111112- - - - - -,5 O圖6關節(jié)角位移軌跡曲線關節(jié)角1時關節(jié)角1速度軌跡曲線50403020fo度速節(jié)關的1角節(jié)關O-1 2位姿1時關節(jié)角1加速度軌跡曲線0.511.5時間t/s22.5O-2o o Oo o o o O6 5 4 3 22戶O度速加的1角節(jié)關-30 0圖7關節(jié)角速度與加速度軌跡仿真圖由上述仿真圖可以看出，每個關節(jié)角度曲線均可劃分為4段，每段關節(jié)角度 變化平穩(wěn)光

44、滑，只在正方形四個頂點出變化最大，故還需要對頂點附近的關節(jié)角 進行空間軌跡規(guī)劃。二、空間圓弧的軌跡規(guī)劃在笛卡爾空間圓弧軌跡規(guī)劃中，為了計算方便，運用坐標變換，即先在圓弧 所在平面建立一個新的直角坐標系，在這個直角坐標系中計算圓弧的各插補點在 新坐標系中的值。然后再將這些值返回到原來的坐標系中， 算出各插補點在原來 坐標系中的值。圓弧插補的位移曲線也是采用拋物線過度的線性函數(shù)，歸一化因子的求解與上述一樣 網(wǎng)。三點確定一段弧。設機械臂末端執(zhí)行器從起始位置P1經(jīng)過中間點P2到達終點P3,如果這三點不共線，就一定存在從起始點P經(jīng)過中間點P2到達終點P3的圓弧軌跡規(guī)劃算法。具體算法如下：先求得圓弧的圓心

45、Po(Xo,yo,Zo)和半徑r。P1(X1,y1，Z1)、P2(X2, y2, Z2)和 P3(X3, y3,Z3)三點確定平面 M,其方程為:xX3yy3z Z3將其展開可得:Xi X3 yi X2 X3 y2(yi y3)(Z2 Z3)也 (X2 X3)(Zi Z3) 0 (Xi X3)(y2 y3)(y3zi Z30yZ2 Z3y3)(Zi Z3)(x X3)X3)(Z2 Z3)(y V3X3)(yi y3)(Z Z3)(3-i8)(3-i9)圖8空間圓弧插補示意圖過PiP2的中點且與PiP2垂直的平面T的方程為:yi)(3-20)r ii,、，,X -(XiX2)(X2 Xi) y

46、二(yiy2)( y222.i ，Z -(Zi Z2)(Z2 Zi) 02過點P2P3的中點且垂直P2P3的平面S的方程為: ii ,X -(X2 X3)(X3 X2) y 二(y2 y3)(y3 y) 222i(3-2i)Z -(Z2 Z3)( Z3 Z2) 0 2聯(lián)立上式，求得圓心P0(X0,y0,Z0)。圓弧的半徑為：r .(X。 Xi)2 (y yi)2 (z /(3-22)以圓心P0(X0,y0,Z0)為原點建立圓弧所在平面的新坐標 Or UVW , U軸為 坐標系原點P與點P3的連線。單位方向向量為u PB ;P0P3W軸為平面T與平面S的交線，其單位方向向量為:PRP2P3PRP

47、2P3據(jù)右手法則，V軸在W軸和U軸的叉乘方向，其單位向量為13.v w u根據(jù)齊次坐標變換可得齊次坐標矩陣Tr為：Ux Vx Wx XoUy Vy Wy YoI RUz Vz Wz Zo0001其逆矩陣Tr1可以根據(jù)齊次變換矩陣求解逆得到:(3-23)(3-24)可以得到：Tr1rT0將點P、P2、uxvxwxR Uy Vy Wy RUzVzWzRTPo1X。Yo z。(3-25)P3以及圓心Po從原來坐標系中的值轉換到圓心所在用 UVW新坐標系中。設原來的坐標系中的值分別為(x1, y1 ,z1) (x2,y2,z2)、(x3,y3,z3)、(xo, yo, zo),在新坐標中值分別為(U1

48、,V),W1)、(U2,V2, W2)與(U3,V3, W3),則求解：U1XU2x2U3*3V1Tr1Y1V2Tr1Y2V311 RY3(3-26)W14W2z2W3z3111111由上式推到知Uo VoW0W1W2W30,U1 r ；求圓弧角度 。由于在 MATLAB中內部函數(shù)(x,y)的求解范圍在-1800-1800 之間。M：v r sin當 V3 0 時，則 3 A tan 2(V3, U3)3 u r cos(3-27)w 0將插補結果返回到原坐標中，設點p在原坐標系中坐標值為（x, y,z）,則有:(3-28)vTrw由以上結果可以得到圓弧上各插補點的位置，各插補點的三個位姿角度

49、可以 各自按照位移曲線為拋物線過度的線性函數(shù)求得。 把每個插補點的位姿通過運動 學逆解，就可以得到各插補點對應的關節(jié)角。空間三點的位姿Pi、p2和p3可以分別用下式表示:00Tpi 1001001000360604001Tp2001001001000330403001Tp300100100100028060 (3-29)2001通過空間弧線的插補法,插補步數(shù)設為N=200,仿真時間40s,在 MATLAB中進行空間圓弧的仿真，如圖9所示，同時獲得機械臂關節(jié)角度的數(shù)據(jù)曲線， 如圖10所示。機械臂的自由度數(shù)目應與所要完成的任務相匹配，空間圓弧在M平面上，機械臂的關節(jié)數(shù)對于空間圓弧軌跡而言是冗余的。

50、第五關節(jié)軸線與末端的 工具軸線重合，關節(jié)五的角度并不影響空間圓弧的軌跡規(guī)劃中工具的位置和姿態(tài)，所以圖10中沒有第五關節(jié)的角度曲線。三、一般空間軌跡規(guī)劃在大多數(shù)情況下，可將 TCP的目標運動軌跡劃分成若干段圓弧軌跡和直線軌跡的連接，從而在每個對應的小區(qū)段使用直線插值或圓弧插值的方法完成整條TCP的目標運動軌跡的插值。但是，對于復雜程度相對較高的目標軌跡曲線，直線段加上圓弧段的組合在曲線精度方面并不理想，常常難以滿足用戶需求。而且，將復雜程度高的軌跡曲線劃分為若干段微小的直線和圓弧的組合，是非常困難的工作， 特別是還要保證直線段和圓弧段交接點處過渡平滑的條件。因此， 對于任意復雜的軌跡，使用更高級

51、的插值方法勢在必行。較為常用的復雜軌跡插值方法有：多項式插值法、分段線性插值法、分段多項式法、B 樣條插值法等等。要保證較高的插值精度，往往需要給定更密集的插值點序列，而且， 如果使用的是多項式插值法，為獲得高精度，多項式的次數(shù)也需要高。但是，當次數(shù)高時，會產(chǎn)生龍格現(xiàn)象，即在插值區(qū)間兩端，會產(chǎn)生劇烈的震蕩現(xiàn)象，導致插值點不收斂于目標軌跡。三次樣條就是通過全部樣點且具有連續(xù)二階導的函數(shù)，因此， 選擇三次樣條插值對機械臂軌跡曲線進行規(guī)劃10。若三次樣條曲線所經(jīng)過的n 1個插值樣點序列的X軸坐標為X。、X1、.、xn 。則待求的三次樣條曲線參數(shù)方程x s(u) 是通過所有樣點，而且具有連續(xù)二階導數(shù)的分段三次多項式，即滿足條件：(1)s(u)在每兩個樣點之間的函數(shù)表達式Sj(U)是小于或等于三次多項式；(2) s(u) 在除起點和終點外的所有內