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

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

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

3、應(yīng)用到機(jī)械臂中的控制算法，通過(guò)MATLAB結(jié)果可以得出本文所建立的算法正確性，能夠?qū)C(jī)械臂運(yùn)動(dòng)提供有效的路徑，而且改進(jìn)了其他應(yīng)用于空間機(jī)械臂的路徑規(guī)劃問(wèn)題。關(guān)鍵詞 】 運(yùn)動(dòng)分析工作空間算法研究軌跡規(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é)國(guó)內(nèi)外發(fā)展現(xiàn)狀-.6-一、國(guó)內(nèi)現(xiàn)狀-.6.-二、國(guó)外現(xiàn)狀-.6.-第二章機(jī)械臂的運(yùn)動(dòng)分析-.8-第一節(jié)機(jī)械臂的正運(yùn)動(dòng)學(xué)分析-.8-第二節(jié)機(jī)械臂的逆運(yùn)動(dòng)學(xué)求解-.10-第三章五軸機(jī)械臂軌跡規(guī)劃與仿真-.11-第一節(jié)軌跡規(guī)劃一般問(wèn)題-.11-第二節(jié)關(guān)節(jié)空間的軌跡規(guī)劃-.12-一、三次多項(xiàng)式插值法-.12-二、五次多項(xiàng)式插值-.15-第三節(jié)笛卡爾空間的軌跡規(guī)劃-.17-一、空間直線軌跡規(guī)劃-.18-二、空間圓弧的軌跡規(guī)劃-.21-三、一般空間軌跡規(guī)劃-.

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

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

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

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

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

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

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

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

21、能在三維空間完成各種作業(yè)的機(jī)電一體化自動(dòng)化生產(chǎn)設(shè)備。第二章 機(jī)械臂的運(yùn)動(dòng)分析機(jī)械臂的運(yùn)動(dòng)是其軌跡出現(xiàn)的直接原因。所以軌跡規(guī)劃的前提是機(jī)械臂的運(yùn)動(dòng)分析1。本文通過(guò)對(duì)機(jī)械臂的正運(yùn)動(dòng)學(xué)和逆運(yùn)動(dòng)學(xué)進(jìn)行求解，分析兩者的區(qū)別和聯(lián)系。 通過(guò)對(duì)五軸機(jī)械臂關(guān)于坐標(biāo)系幾何關(guān)系，針對(duì)常見軌跡規(guī)劃方案中起始和終止階段進(jìn)行研究，分析研究結(jié)果。第一節(jié) 機(jī)械臂的正運(yùn)動(dòng)學(xué)分析機(jī)械臂從關(guān)節(jié)空間到末端笛卡爾空間的變換是正向運(yùn)動(dòng)學(xué)描述。由坐標(biāo)系中已知的各個(gè)關(guān)節(jié)角度，求解機(jī)械臂末端相對(duì)應(yīng)于原點(diǎn)坐標(biāo)系的位置和位姿。設(shè)矩陣 A 表示機(jī)械臂連桿的齊次變換：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由于機(jī)械臂全是旋轉(zhuǎn)關(guān)節(jié)。對(duì)于文中采用的機(jī)械臂而言有五個(gè)其次變換矩陣，則末端連桿坐標(biāo)系相對(duì)于基坐標(biāo)系的齊次變換矩陣r11r12r13px0r21r22r23py05TA1A2A3A4A5212223 y(2-2)r31r32 r33pz0001式即為機(jī)械臂的運(yùn)動(dòng)方程，它反應(yīng)各關(guān)節(jié)變量與機(jī)械臂末端位姿之間的關(guān)系，上式左邊的五個(gè)矩陣

23、含有五個(gè)關(guān)節(jié)變量q 1, 2, 3, 4, 5。方程右邊為描述機(jī)械臂末端關(guān)節(jié)位置和姿態(tài)的齊次矩陣，由剛體姿態(tài)的描述可知r11，r12，r13，21,22, 3,31,32,33分別為機(jī)械臂末關(guān)節(jié)坐標(biāo)系的三個(gè)坐標(biāo)軸與機(jī)械臂基坐標(biāo)系三個(gè)坐標(biāo)軸的方向余弦，Px, py, Pz為機(jī)械臂末關(guān)節(jié)的坐標(biāo)原點(diǎn)在機(jī)械 臂基坐標(biāo)系中的三維坐標(biāo)。機(jī)械臂正運(yùn)動(dòng)學(xué)求解就是已知各連桿的關(guān)節(jié)變量求解末端連桿的位姿矩陣。即已知關(guān)節(jié)變量q 1, 2, 3, 4, 5T ,求解上式機(jī)械臂運(yùn)動(dòng)學(xué)方程中等式右邊矩10將上式中的機(jī)械臂五個(gè)關(guān)節(jié)的齊次變換矩陣帶入，即計(jì)算出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é)機(jī)械臂的逆運(yùn)動(dòng)學(xué)求解機(jī)械臂的逆運(yùn)動(dòng)學(xué)解是對(duì)其運(yùn)動(dòng)學(xué)正解的反解，因

25、而已知量和求解量相反， 即已知機(jī)械臂末端的位置姿態(tài)對(duì)機(jī)械臂進(jìn)行驅(qū)動(dòng)，使各個(gè)關(guān)節(jié)從此刻的姿態(tài)運(yùn)動(dòng)到與末端位姿相對(duì)應(yīng)的位置，進(jìn)而得到關(guān)節(jié)變量 11。機(jī)械臂的運(yùn)動(dòng)學(xué)正、逆求解實(shí)質(zhì)是機(jī)械臂關(guān)節(jié)空間與工作空間之間的非線性 映射關(guān)系，兩者可相互轉(zhuǎn)換。關(guān)系圖如下所小。桿件參數(shù)關(guān)節(jié)角量運(yùn)動(dòng)學(xué)正解J端執(zhí)行器位姿_關(guān)節(jié)角量u逆解選取y運(yùn)動(dòng)學(xué)逆解lzT桿件參數(shù)圖1關(guān)節(jié)空間與工作空間的關(guān)系機(jī)械臂的逆運(yùn)動(dòng)學(xué)問(wèn)題，指已知機(jī)械臂的末端位姿，即已知齊次變換矩陣0T ,求解各轉(zhuǎn)動(dòng)關(guān)節(jié)的角度i。機(jī)械臂的逆運(yùn)行學(xué)問(wèn)題，可以理解為通過(guò)運(yùn)動(dòng)學(xué)方程：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轉(zhuǎn)置，上式可以表達(dá)成為:C1S100r11r12r13PxSiC100r21r22r23Py1 -(2-6)01d15 0r31r32r33d100010001假設(shè)上式的兩邊元素和式相等，得到:可以得出1的解第三章五軸機(jī)械臂軌跡規(guī)劃與仿真目前關(guān)于空間軌跡規(guī)劃的方法主要有三種， 三次多項(xiàng)式插值，高階多項(xiàng)式插 值以及樣條曲線等方法。主要討論軌跡在關(guān)節(jié)空間中的位移、 速度與加速度等變 量的關(guān)系。

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

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

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

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

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

32、1 a13tf1 ；第一中間點(diǎn)位置2與第一運(yùn)動(dòng)段在標(biāo)準(zhǔn)化時(shí)間t 0時(shí)起點(diǎn)相同，所以得出： 2a20 ；三次多項(xiàng)式在2處一階可導(dǎo)，因此可得出：2a11 2a12tf1 3a13tf21a21；三次多項(xiàng)式在2處二階可導(dǎo)，因此可得出：22a12 6a13tf12a22；第二個(gè)空間點(diǎn)的位置3與第二運(yùn)動(dòng)段在標(biāo)準(zhǔn)化時(shí)間tl2時(shí)的終點(diǎn)相同，所以有:233a20a21t f 2a22t f2a23t f 2 ；第二個(gè)中間點(diǎn)的位置3 應(yīng)與第三運(yùn)動(dòng)段在標(biāo)準(zhǔn)化時(shí)間t 0 時(shí)起點(diǎn)相同，所以有：3a30 ；三次多項(xiàng)式在3處一階可導(dǎo)，從而有：3a21 2a22t f 2 3a23t2f2a31；三次多項(xiàng)式在3 處二階可導(dǎo)

33、，從而有：32a22 6a23t f 2 2a32；? 第 (n 2) 個(gè)中間點(diǎn)位置n 1和第 (n 1)運(yùn)動(dòng)段在標(biāo)準(zhǔn)化時(shí)間tf(n 2)時(shí)的終點(diǎn)相同，所以有：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) 個(gè)中間點(diǎn)位置n 1 應(yīng)與下一運(yùn)動(dòng)段在標(biāo)準(zhǔn)化時(shí)間t 0時(shí)的起點(diǎn)位置相同，所以有n 1a(n 1)0；? 三次多項(xiàng)式在第(n 2)個(gè)中間點(diǎn)處一階可導(dǎo)，從而：(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? 三次多項(xiàng)式在第(n 2

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

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

36、二階導(dǎo)數(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，對(duì)起始點(diǎn)和目標(biāo)點(diǎn)同時(shí)給出關(guān)于角度和角加速度的約束條件:(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)始點(diǎn)

37、和目標(biāo)點(diǎn)的關(guān)節(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為了對(duì)比三次多項(xiàng)式關(guān)節(jié)插值算法和五次多項(xiàng)式插值算法的效果，同樣要求 機(jī)械臂從起始點(diǎn)開始運(yùn)動(dòng)，經(jīng)過(guò)4秒到達(dá)終點(diǎn)，仿真時(shí)起始點(diǎn)和目標(biāo)點(diǎn)的關(guān)節(jié)角 速度為00中間點(diǎn)的關(guān)節(jié)角加速度還可以對(duì)相鄰兩段軌跡角加速度進(jìn)行平均值求 解，使該值為中間點(diǎn)的瞬時(shí)加速度12。利用MATLAB對(duì)五次多項(xiàng)式插值進(jìn)行仿真, 將結(jié)果與三次多項(xiàng)式插值進(jìn)行對(duì)比，發(fā)現(xiàn)三個(gè)位置點(diǎn)的速度

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

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

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

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

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

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

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

45、Po(Xo,yo,Zo)和半徑r。P1(X1,y1，Z1)、P2(X2, y2, Z2)和 P3(X3, y3,Z3)三點(diǎn)確定平面 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空間圓弧插補(bǔ)示意圖過(guò)PiP2的中點(diǎn)且與PiP2垂直的平面T的方程為:yi)(3-20)r ii,、，,X -(XiX2)(X2 Xi) y

46、二(yiy2)( y222.i ，Z -(Zi Z2)(Z2 Zi) 02過(guò)點(diǎn)P2P3的中點(diǎn)且垂直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)為原點(diǎn)建立圓弧所在平面的新坐標(biāo) Or UVW , U軸為 坐標(biāo)系原點(diǎn)P與點(diǎn)P3的連線。單位方向向量為u PB ;P0P3W軸為平面T與平面S的交線，其單位方向向量為:PRP2P3PRP

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

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

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

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

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