機械臂運動的軌跡規劃

　摘

　要

　空間機械臂就是一個機、電、熱、控一體化的高集成的空間機械系統。隨著科技的發展,特別就是航空飛機、機器人等的誕生得到了廣泛的應用,空間機械臂作為在軌跡的支持、服務等以備受人們的關注。本文將以空間機械臂為研究對象,針對空間機械臂的直線運動、關節的規劃、空間直線以及弧線的軌跡規劃幾個面進行研究,對機械臂運動與工作空間進行了分析,同時對機械臂的軌跡規劃進行了驗證,利用 MATLAB 軟件對機械臂的軌跡進行仿真,驗證算法的正確性與可行性,同時此路徑規劃法可以提高機械臂的作業效率,為機械臂操作提高理論指導,為機器人更復雜的運動仿真與路徑規劃打下基礎。

　本文一共分為四章: 第一章,首先總結了機械臂運動控制與軌跡規劃問題的研究現狀及研究法,歸納了各種軌跡規劃的算法及其優化法,闡述了機械臂的研究背景與主要容。

　 第二章,對機械臂的空間運動進行分析研究,采用抽樣求解數值法—蒙特卡洛法,進行機械臂工作空間求解,同時在 MATLAB 中進行仿真,直觀展示機械臂工作圍,為下一章的軌跡規劃提供理論基礎;同時通過 D-H 參數法對機械臂的正、逆運動分析求解,分析兩者的區別與聯系。

　第三章,主要針對軌跡規劃的一般性問題進行分析,利用笛卡爾空間的軌跡規劃法對機械臂進行軌跡規劃,同時利用 MATLAB 對空間直線與空間圓弧進行軌跡規劃,通過仿真驗證算法的正確性與可行性。

　第四章,總結全文,分析本文應用到機械臂中的控制算法,通過 MATLAB 結果可以得出本文所建立的算確性,能夠對機械臂運動提供有效的路徑,而且改進了其她應用于空間機械臂的路徑規劃問題。

　【 關鍵詞 】

　運動分析 工作空間 算法研究 軌跡規劃 ABSTRACT

　Space 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 aviation aircraft, a robot has been widely used, the trajectory of space manipulator as the support and services to people"s 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 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 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 summarizes 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 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 trajectory 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 trajectory 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 applied 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,algorithm research

　目錄

　摘 要........................................................... - 1 - ABSTRACT........................................................ - 1 - 第一章 緒論..................................................... - 5 - 第一節 研究背景及意義 ....................................... - 5 - 第二節 國外發展現狀 ......................................... - 6 - 一、國現狀............................................... - 6 - 二、國外現狀............................................. - 6 - 第二章 機械臂的運動分析......................................... - 7 - 第一節 機械臂的正運動學分析 ................................. - 7 - 第二節 機械臂的逆運動學求解 ................................. - 9 - 第三章 五軸機械臂軌跡規劃與仿真................................ - 10 - 第一節 軌跡規劃一般問題 .................................... - 11 - 第二節 關節空間的軌跡規劃 .................................. - 11 - 一、三次多項式插值法.................................... - 11 - 二、五次多項式插值...................................... - 15 - 第三節 笛卡爾空間的軌跡規劃 ................................ - 17 - 一、空間直線軌跡規劃.................................... - 17 - 二、空間圓弧的軌跡規劃.................................. - 20 - 三、一般空間軌跡規劃.................................... - 24 - 第四章 總結與展望.............................................. - 28 - 參考文獻....................................................... - 29 -

　第一章

　緒論

　第一節

　研究背景及意義

　隨著宇宙空間的開發,70 年代美國提出了在宇宙空間利用機器人系統的概念,并且在航天飛機上實施。當初的空間機器人就是由航天飛機艙的宇航員通過電視畫面操縱的。隨著空間技術的進一步發展使得未來空間操作任務急劇增加,空間站的建立、維修,衛星的回收、釋放等工作會越來越多。如果所有這些工作都依靠宇航員來完成,其成本將十分高昂,也就是十分危險的,因為惡劣的太空環境會給宇航員的空間作業帶來巨大的威脅。宇航員的艙外作業需要龐大而復雜的環境控制系統、生命保障系統、物質供給系統、救生系統等的支持,這些系統不但具有很高的技術難度,而且成本巨大。用空間機器人代替宇航員進行太空作業不僅可以使宇航員避免在惡劣太空環境中工作時可能受到的傷害,還可以降低成本,提高空間探索的效益。空間機械臂就是空間機器人的一種,已被考慮在未來的空間活動中承擔大型空間站的在軌安裝及對失效飛行器的的捕捉與維修,土壤與巖的取樣等;并期望其在無人狀態下承擔未來空間實驗室或工廠的日常工作。根據空間作業的需要,空間機器人上一般都安裝了一個或多個模仿人手臂的多自由度機器臂。隨著我國國民經濟與國防工業技術的迅速發展,對航天器的需求量日益增加,對其能力的要求日臻提高。特別就是空間站在軌服務、深空探測等空間技術領域的迅速發展,對于空間機械臂技術的需求越來越迫切,而且對其工作能力與性能要求越來越高,對其安全性、壽命等面也提出了越來越高的要求。此外,受國外在高技術領域的技術限制與封鎖,使得我們必須堅持自力更生、獨立自主的高技術研發道路,堅持自主創新的思想,加速并加強空間機械臂技術的研發工作[1] 。

　將機器人用于空間服務,一項關鍵技術就就是路徑規劃。路徑規劃研究就是機器人研究領域中的一個重要分支,就是機器人導航中最重要的任務之一。對已知靜態環境中機器人路徑規劃的研究已經進行了將近 40 年,路徑規劃問題的研究有很大的價值。多年的研究工作在取得進展的同時,愈加證明了路徑規劃就是一個復雜的難題。路徑規劃算法的計算量取決于任務、環境的復雜性以及對規劃路徑質量的要求,一個好的路徑規劃算法應該兼顧對規劃速度與路徑質量的期望。隨著研究的深入,各種新的路徑規劃法層出不窮,使路徑規劃研究一直活躍在

　機器人學領域。

　 目前國對空間機械臂研究還處于起步階段,因此開展空間機械臂相關領域的研究將極大促進我國空間科學試驗、空間維護與建設、深空探測等空間技術的發展。本論文根據課題的技術要求,將空間機械臂路徑規劃作為切入點,研究路徑規劃問題,其研究成果具有重要的理論指導意義與工程應用價值。

　第二節

　國 外發展現狀

　一、國現狀

　我國的工業機器人從 80 年代“七五”科技攻關開始起步,目前已基本掌握了機器人操作機的設計制造技術、控制系統硬件與軟件設計技術、運動學與軌跡規劃技術,生產了部分機器人關鍵元件,開發出噴漆、弧焊、點焊、裝配、搬運等機器人;但總的來瞧,我國工業機器人技術及其工程應用的水平與國外比還就是有一定的距離,如:可靠性低于國外產品;機器人應用工程起步較晚,應用領域窄,生產線系統技術與國外比有差距。

　我國的智能機器人與特種機器人在“863”計劃的支持下,也取得不少成果。其中最突出的就是水下機器人,6000 米水下無纜機器人的成果居世界領先水平,還開發出直接遙控機器人、雙臂協調控制機器人、爬壁機器人、管道機器人等機種;在機器人視覺、力覺、觸覺、聲覺等基礎技術的開發應用上開展了不少工作,有一定的發展基礎。但就是在多傳感器信息融合控制技術、遙控加局部自主系統遙控機器人、只能裝配機器人、機器人化機械等的開發應用面則剛剛起步,與國外先進水平差距較大。

　二、國外現狀

　美國就是機器人的誕生地,早在 1962 年就研制出世界上第一臺工業機器人,比起號稱“機器人國”的日本起步至少早五六了年。1971 年,通用汽車公司又第一次用機器人進行點焊。

　西歐時僅次于日美機器人的生產基地,也就是日美機器人的重要市場。早在1966 年,美國 Unimation 公司的尤尼曼特機器人與 AMF 公司的沃莎特蘭機器人就進入英國市場。接著,英國 Hall Automation 公司研制出自己的機器人 RAMP。德國工業機器人的總數占世界第三。德國對于一些有危險、有毒、有害的工作崗位,

　必須以機器人替代普通人的勞動。同時提出了 1985 年以后要向高級的、帶感覺的智能型機器人轉移目標。

　1954 年:美國人戴沃爾制造了世界第一臺可編程的機械手。

　1959 年:戴沃爾與美國發明家英格伯格聯手制造出第一臺工業機器人。

　1962 年:美國 AFM 公司生產出萬能搬運機器人,與 Unimation 公司生產的萬能伙伴機器人一樣成為真正商業化的工業機器人。

　1967 年:日本川崎重工公司與豐田公司分別從美國購買了工業機器人Unimat 與 Verstran 的生產可,開始對機器人的研究與制造。

　1968 年:美國斯坦福研究所公布她們研制的機器人 Shakey。

　1973 年:世界上機器人與小型計算機第一次攜手合作,誕生了機器人 T3。

　1979 年:日本山梨大學發明了平面關節機器人 SCARA。

　1984 年:英格伯格在此推出機器人 Helpmate,這種機器人能在醫院為病人送飯送藥與送。

　1996 年:本田公司推出仿人型機器人 P2,雙足行走機器人的研究達到了一個新的高度。

　2002 年:美國 iRobot 公司推出了吸塵器機器人 Roombar,為世界上商業化最成功的家用機器人。

　2006 年:微軟公司推出 Microsoft Robitics Studio 機器人,從此機器人模塊化平臺同一化的趨勢越來越明顯。

　在工業機器人技術面,工業機器人有操作機(機械本體)、控制器、伺服驅動系統與檢測傳感器裝置構成,就是一種仿人操作、自動控制、可重復編程、能在三維空間完成各種作業的機電一體化自動化生產設備。

　第二章

　機械臂的運動分析

　機械臂的運動就是其軌跡出現的直接原因。所以軌跡規劃的前提就是機械臂的運動分析[1] 。本文通過對機械臂的正運動學與逆運動學進行求解,分析兩者的區別與聯系。通過對五軸機械臂關于坐標系幾關系,針對常見軌跡規劃案中起始與終止階段進行研究,分析研究結果。

　第一節

　機械臂的正運動學分析

　機械臂從關節空間到末端笛卡爾空間的變換就是正向運動學描述。由坐標系

　中已知的各個關節角度,求解機械臂末端相對應于原點坐標系的位置與位姿。設矩陣 A 表示機械臂連桿的齊次變換: ) , ( ) 0 , 0 , ( ) , 0 , ( ) , (1 i n i i i iX Rot a Trans ld Trans Z Rot A ? ? ???

　?????????????? ???? ? ? ?? ? ? ??1 0 0 0cos cos sin cos sin sinsin sin cos cos cos sin0 sin cos1 1 1 11 1 1 11i i i i i i ii i i i i i ii i idda? ? ? ? ? ?? ? ? ? ? ?? ?

　(2-1) 由于機械臂全就是旋轉關節。對于文中采用的機械臂而言有五個其次變換矩陣,則末端連桿坐標系相對于基坐標系的齊次變換矩陣 ????????????? ?1 0 0 033 32 3123 22 2113 12 115 4 3 2 105zyxp r r rp r r rp r r rA A A A A T

　 (2-2)

　 式即為機械臂的運動程,它反應各關節變量與機械臂末端位姿之間的關系,上式左邊的五個矩陣含有五個關節變量Tq ] , , , , [5 4 3 2 1? ? ? ? ? ? 。程右邊為描述機械臂 末 端 關 節 位 置 與 姿 態 的 齊 次 矩 陣 , 由 剛 體 姿 態 的 描 述 可 知11r ,12r ,13r ,21r ,22r ,23r ,31r ,32r ,33r 分別為機械臂末關節坐標系的三個坐標軸與機械臂基坐標系三個坐標軸的向余弦,xp ,yp ,zp 為機械臂末關節的坐標原點在機械臂基坐標系中的三維坐標。

　 機械臂正運動學求解就就是已知各連桿的關節變量求解末端連桿的位姿矩陣。即已知關節變量Tq ] , , , , [5 4 3 2 1? ? ? ? ? ? ,求解上式機械臂運動學程中等式右邊矩陣各元素的值[10] 。

　 將上式中的機械臂五個關節的齊次變換矩陣帶入,即計算出 T05中各元素值為: ?????????????1 0 0 033 32 3123 22 2113 12 1105zyxp r r rp r r rp r r rT

　 (2-3) 其中:

　5 1 5 234 1 11s s c c c r ? ?

　5 1 5 234 1 12c s s c c r ? ? ?

　4 23 1 4 23 1 13c s c s c c r ? ?

　5 1 5 234 1 21s c c c s r ? ?

　5 1 5 234 1 22c c s c s r ? ? ?

　4 23 1 4 23 1 23c s s s c s r ? ?

　5 234 31c s r ?

　5 234 32s s r ? ?

　234 11c r ? ?

　1 1 2 1 2 2 1 3 23 1a c ld s a c c a c c p x ? ? ? ?

　1 1 2 1 2 2 1 3 23 1a s ld c a c s a c s p y ? ? ? ?

　1 2 2 3 23ld a s a s p z ? ? ?

　其中,1 1cos ? ? c ,1 1sin ? ? s , ) cos(4 3 2 234? ? ? ? ? ? c , ) sin(4 3 2 234? ? ? ? ? ? s 。

　第二節

　機械臂的逆運動學求解

　機械臂的逆運動學解就是對其運動學正解的反解,因而已知量與求解量相反,即已知機械臂末端的位置姿態對機械臂進行驅動,使各個關節從此刻的姿態運動到與末端位姿相對應的位置,進而得到關節變量[11] 。

　 機械臂的運動學正、逆求解實質就是機械臂關節空間與工作空間之間的非線性映射關系,兩者可相互轉換。關系圖如下所示。

　桿件參數運動學正解 末端執行器位姿運動學逆解 逆解選取關節角量關節角量桿件參數 圖 1 關節空間與工作空間的關系

　機械臂的逆運動學問題,指已知機械臂的末端位姿,即已知齊次變換矩陣 T05,求解各轉動關節的角度i? 。

　 機械臂的逆運行學問題,可以理解為通過運動學程: ) ( ) ( ) ( ) ( ) (545 434 323 212 10105? ? ? ? ? T T T T T T ?

　(2-4) 求解i? 。

　 整理式,將含有1? 的部分移到程的左邊 ) ( ) ( ) ( ) ( )] ( [545 434 323 212051101? ? ? ? ? T T T T T T ??

　(2-5)

　 將 T01轉置,上式可以表達成為: Td r r rp r r rp r r rdc ss cyx151 33 32 3123 22 2113 12 1111 11 11 0 0 0 1 0 0 01 0 00 00 0????????????????????????????

　 (2-6)

　 假設上式的兩邊元素與式相等,得到: 01 1? ? ?y xp c p s

　 (2-7) 可以得出1? 的解。

　第三章

　五軸機械臂軌跡規劃與仿真

　目前關于空間軌跡規劃的法主要有三種,三次多項式插值,高階多項式插值以及樣條曲線等法。主要討論軌跡在關節空間中的位移、速度與加速度等變量的關系。規劃實質就是根據需求,計算出預定的軌跡曲線,在軌跡規劃時可以再運動學與動力學的基礎上進行規劃,所以規劃就是建立在運動學與動力學基礎上的。

　 圖 2 機械臂的 matlab 生成 第一節

　軌跡規劃一般問題

　軌跡規劃的一般法就是在機械臂末端的初始與目標位置之間用多項式函數“插”來抵近給定的路徑,并沿著時間軸產生一系列的可供操作機使用的“控制設定點”[3] 。其中關節坐標與笛卡爾坐標都可以對路徑端點進行給出。一般就是在笛卡爾坐標中給出,由于在笛卡爾坐標中機械臂末端形態更容易觀察。所以通常采用笛卡爾法。

　在給定的兩端之間,常有多條可能路徑。可以沿著直線與光滑多項式曲線運動。本文將討論插值法,研究滿足路徑約束的簡單軌跡規劃[3] 。

　第二節

　關節空間的軌跡規劃

　機械臂關節空間的軌跡規劃解決機械臂從起始位姿到終止位姿去取放物體的問題、機械臂末端移動的過程并不重要,只要求運動就是平滑的且沒有碰撞產生、在關節空間中進行軌跡規劃時,算法簡單、工具移動效率高、關節空間與直角坐標空間連續的對應關系就是不存在的,因此機構的奇異性問題一般不會發生。對于無路徑的要求,應盡量在關節空間進行軌跡規劃。

　一、三次多項式插值法

　三次多項式與其一階導數函數,總計有四個待定系數,對起始點與目標點兩者的角度、角角速度同時給出約束條件,本文采用的就是三次多項式插值法[5] 。可以對通過空間的 n 個點進行分析并進行軌跡規劃,讓速度與加速度在運動過程

　中保持軌跡平滑。本文算法可以實現對 ) 1 ( ? n 段中的每一段三次多項式系數求解,為了便,對其進行歸一化處理。

　 (1)時間標準化算法

　 根據三次多項式軌跡規劃流程,對每個關節進行軌跡規劃時需要對 ) 1 ( ? n 段的軌跡進行設計,為了能對 ) 1 ( ? n 個軌跡規劃程進行同樣處理,本文首先設計了時間標準化算法將時間進行處理,經過處理后的時間 ] 1 , 0 [ ? t 。

　 首先定義:

　 t :標準化時間變量, ] 1 , 0 [ ? t ;

　 ? :未標準化時間,單位為秒;

　 i? :第 i 段軌跡規劃結束的未標準化時間,1 ?? ?i i i? ? ? ;

　 機械臂執行第 i 段軌跡所需要的實際時間: ) /( ) (1 1 ? ?? ? ?i i it ? ? ? ? ,其中1 , 0 , ,1? ??ti i? ? ? 。

　 時間歸一化后的三次多項式為:3322 1 0t A t A t A A y ? ? ? ?

　(2)機械臂軌跡規劃算法實現過程

　 ①已知初始位置為1? ;

　 ②給定初始速度為 0;

　 ③已知第一個中間點位置2? ,它也就是第一運動段三次多項式軌跡的終點;

　 ④為了保證運動的連續性,需要設定2? 所在點為三次多項式軌跡的起點,以確保運動的連續;

　 ⑤為了保證2? 處速度連續,三次多項式在2? 處一階可導;

　 ⑥為了保證2? 處加速度連續,三次多項式在2? 處二階可導;

　 ⑦以此類推,每一個中間點的位置i? )) 1 ( 2 ( ? ? ? n i ,都一定要在其原運動段軌跡的終點,并且也就是它后運動段的起點。

　 ⑧1 ? i? 的速度保持連續;

　⑨1 ? i? 的加速度保持連續;

　 ⑩點位置n? 。給定終點速度,設其為 0。

　 (3)約束條件

　 第一個三次曲線為:313212 11 10) ( t a t a t a a t ? ? ? ? ?

　第二個三次曲線為:323222 21 20) ( t a t a t a a t ? ? ? ? ?

　第三個三次曲線為:333232 31 30) ( t a t a t a a t ? ? ? ? ?

　......

　 第 ) 1 ( ? n 個三次曲線為:33 ) 1 (22 ) 1 ( 1 ) 1 ( 0 ) 1 () ( t a t a t a a tn n n n ? ? ? ?? ? ? ? ?

　在同一時間段,三次曲線每次的起始時刻 0 ? t ,停止時刻nt t ? ,其中 n i ... 1 ? 。

　 ①在標準化時間 0 ? t 處,設定1? 為第一條三次多項式運動段的起點,可以得出:10 1? ? ? ;

　 ②在標準化時間 0 ? t 處,三次多項式運動段第一條的初始速度就是已知變量,所以得出: 011 1? ? a ? ;

　 ③第一中間點位置2? 與第一條三次多項式運動段在標準化時間nt t ? 時的終點相同,所以可以得出:31 1321 12 1 11 10 2 f f ft a t a t a a ? ? ? ? ? ;

　 ④第一中間點位置2? 與第一運動段在標準化時間 0 ? t 時起點相同,所以得出:20 2a ? ? ;

　 ⑤三次多項式在2? 處一階可導,因此可得出:2121 13 1 12 11 23 2 a t a t a af f? ? ? ? ? ? ;

　 ⑥三次多項式在2? 處二階可導,因此可得出:22 1 13 12 22 6 2 a t a af? ? ? ? ? ;

　 ⑦第二個空間點的位置3? 與第二運動段在標準化時間12t 時的終點相同,所以有:32 2322 22 2 21 20 3 f f ft a t a t a a ? ? ? ? ? ;

　 ⑧第二個中間點的位置3? 應與第三運動段在標準化時間 0 ? t 時起點相同,所以有:30 3a ? ? ;

　⑨三次多項式在3? 處一階可導,從而有:3122 23 2 22 21 33 2 a t a t a af f? ? ? ? ? ? ;

　 ⑩三次多項式在3? 處二階可導,從而有:32 2 23 22 32 6 2 a t a af? ? ? ? ? ? ;

　 ......

　 ?第 ) 2 ( ? n 個中間點位置1 ? n? 與第 ) 1 ( ? n 運動段在標準化時間) 2 ( ? n ft 時的終點相同,所以有:3) 2 ( 3 ) 2 (2) 2 ( 2 ) 2 ( ) 2 ( 1 ) 2 ( 0 ) 2 ( 1 ? ? ? ? ? ? ? ?? ? ? ?n f n n f n n f n n nt a t a t a a ? 。

　 ?第 ) 2 ( ? n 個中間點位置1 ? n? 應與下一運動段在標準化時間 0 ? t 時的起點位置相同,所以有0 ) 1 ( 1 ? ??n na ? ;

　 ?三次多項式在第 ) 2 ( ? n 個中間點處一階可導,從而: 1 ) 1 (2) 2 ( 3 ) 2 ( ) 2 ( 2 ) 2 ( 1 ) 2 ( ) 1 (3 2? ? ? ? ? ? ?? ? ? ?n n f n n f n n na t a t a a ? ?

　 (3-1)

　 ?三次多項式在第 ) 2 ( ? n 個中間點處二階可導,從而: 2 ) 1 ( ) 2 ( 3 ) 2 ( 2 ) 2 ( ) 1 (2 6 2? ? ? ? ?? ? ?n n f n n na t a a ? ? ?

　(3-2)

　 ?因此可以得出所有軌跡終點在標準化時間nt 時的位置n? 為: 33 ) 1 (22 ) 1 ( 1 ) 1 ( 0 ) 1 ( fn n fn n fn n n nt a t a t a a? ? ? ?? ? ? ? ?

　 (3-3)

　 ?因此可以得出所有軌跡終點在標準化時間nt 時的速度n? 為: 23 ) 1 ( 2 ) 1 ( 1 ) 1 (3 2fn n fn n n nt a t a a? ? ?? ? ? ? ?

　 (3-4)

　 以上公式改寫為矩陣為: ] [ ] [ ] [1??? M C 。由該矩陣計算1] [?M 可以求出軌跡規劃的全部參數,( ] [ ? 由五軸機械臂運動學逆解求出)于就是求得 ) 1 ( ? n 段的運動程,從而使五軸機械臂末端執行器經過所給定的位置坐標。

　 通過以上分析可以確定機械臂在滿足速度要求的兩個位姿之間運動時各個關節軸的角度變化曲線。如下圖 3 所示就是 MATLAB 仿真分析三次多項式插值:機械臂某關節角在 4 秒由初始點 A 經過中間點 B 到達終點 C 的變化情況。三個位置點的速度與角速度如下所示: 20 30 2040 60 30? ? ?? ? ?C B AC B A? ? ?? ? ?? ? ?

　圖中實線為角度變化曲線,虛線為角速度變化曲線。關節角度曲線平滑,而速度曲線在中間點 B 處出現突變。

　0 0.5 1 1.5 2 2.5 3 3.5 4-30-20-10010203040506070時 間 (s)角度(deg) 圖 3 三次多項式插值法 二、五次多項式插值

　五次多項式共有六個待定系數,要想六個系數得到確定,至少需要六個條件。五次多項式可以瞧作就是關節角度的時間函數,因此其一階可導與二階可導分別可以瞧作就是關節角速度與關節角加速度的時間函數。五次多項式及一階、二階導數公式如下: 55443322 1 0 ) (t C t C t C t C t C Ct? ? ? ? ? ? ?

　 (3-5) 453423 2 1 ) (5 4 3 2 t C t C t C t C Ct? ? ? ? ? ? ?

　 (3-6) 3524 3 2 ) (20 12 6 2 t C t C t C Ct? ? ? ? ? ? ?

　(3-7)

　 為了求得待定系數5 4 3 2 1 0, , , , , C C C C C C ,對起始點與目標點同時給出關于角度與角加速度的約束條件: 50 540 430 320 2 0 1 0 ) ( 0t C t C t C t C t C Ct? ? ? ? ? ? ?

　 (3-8) 55443322 1 0 ) ( f f f f f tt C t C t C t C t C Cf? ? ? ? ? ? ?

　(3-9) 40 530 420 3 0 2 1 ) (5 4 3 20t C t C t C t C Ct? ? ? ? ? ? ?

　(3-10) 453423 2 1 ) (5 4 3 2f f f f tt C t C t C t C Cf? ? ? ? ? ? ?

　(3-11)

　30 520 4 0 3 2 ) (20 12 6 20t C t C t C Ct? ? ? ? ? ? ?

　 (3-12) 3524 3 2 ) (20 12 6 2f f f tt C t C t C Cf? ? ? ? ? ? ?

　 (3-13)

　 式中) ( 0 t? 、) (ft? 分別表示起始點與目標點的關節角,) ( 0 t? ? 、) (ft? ? 分別表示起始點與目標點的關節角速度,) ( 0 t? ? ? 、) (ft? ? ? 分別表示起始點與目標點的關節角加速度。將起始時間設為 0,即 00? t 得到解為: ???????????????? ? ? ? ??? ? ? ? ??? ? ? ? ?????520 0 05420 0 04320 0 03020 10 02) ( ) 6 6 ( 12 122) 2 3 ( ) 16 14 ( 30 302) 3 ( ) 12 8 ( 20 202ff f f f fff f f f fff f f f ftt tCtt tCtt tCCCC? ? ? ? ? ?? ? ? ? ? ?? ? ? ? ? ????? ? ? ? ? ?? ? ? ? ? ?? ? ? ? ? ?? ??

　(3-14)

　 為了對比三次多項式關節插值算法與五次多項式插值算法的效果,同樣要求機械臂從起始點開始運動,經過 4 秒到達終點,仿真時起始點與目標點的關節角速度為 0。中間點的關節角加速度還可以對相鄰兩段軌跡角加速度進行平均值求解,使該值為中間點的瞬時加速度[12] 。利用 MATLAB 對五次多項式插值進行仿真,將結果與三次多項式插值進行對比,發現三個位置點的速度、角速度兩種法相同,同時增加角加速度約束: 2 4 220 30 2040 60 30? ? ?? ? ?? ? ?C B AC B AC B A? ? ?? ? ?? ? ?? ? ? ? ? ?? ?

　 仿真結果如圖 4 所示,圖中實線與虛線分別表示角度變化曲線、角速度變化曲線。點線則表示角加速度曲線。其中關節角度與角速度曲線顯示的都相對平滑,而角加速度曲線在中間點 B 處變化稍大。結果分析得出,多項式插值法雖然計算量有所增加,但就是其關節空間軌跡平滑、運動穩定,且階數越高滿足的約束項越多。

　0 0.5 1 1.5 2 2.5 3 3.5 4-150-100-50050100Time(s)Joint(deg) 圖 4 五次多項式插值法 第三節

　笛卡爾空間的軌跡規劃

　在機械臂的笛卡爾空間軌跡規劃中,中間點即插補點的坐標可以通過插補算法得到。得到中間點后,在把中間點的位姿轉換成相應的關節角,再通過對關節角的控制,使得機械臂的末端能按照預先規劃的路徑運動。機械臂的笛卡爾空間軌跡規劃位姿控制過程大致如下所示: 機械臂軌跡規劃器(插補算法)機械臂軌運動學逆解求解機械臂關節角控制機械臂按照規劃的軌跡運動軌跡上幾個給定點的位姿軌跡中插補點的位姿各關節角 圖 5 機械臂笛卡爾空間軌跡規劃控制過程

　 空間直線與空間弧線的軌跡規劃就是笛卡爾空間中不可或缺的兩部分。因為空間的曲線可以分割為多直線與弧線;但就是也有會出現直線或弧線連接處尖角問題,為了使運動軌跡連續平滑,本文采用圓弧過度來平滑尖角。在笛卡爾空間中,空間直線與空間弧線的軌跡規劃就是最常見的兩部分,其她空間曲線可以通過這兩者來逼近。

　一、空間直線軌跡規劃

　所謂空間直線插補就就是在該直線起始點位姿已知的情況下,對軌跡中間點(插補點)的位姿坐標進行求解[6] 。

　直線插補法:

　 ①設已知起始點的位置坐標分別為: ) , , (0 0 0 0z y x p , ) , , (f f f fz y x p ,0p 與fp為相對基礎坐標系計算其長度: 202020) ( ) ( ) (f f fz z y y x x L ? ? ? ? ? ?

　(3-15)

　 ②求間隔行程,需要分勻速、加速、減速三種情況進行討論:

　 勻速:設速度為 v ,則插補期sT 行程為svT d ?1;

　 加 速 : 設 加 速 度 為 a , 起 始 點 速 度 為0v , 則 在 插 補 期 的 行 程為:20 221s saT T v d ? ? ;整個加速度的路程為:2021vas ? ,時間記為[6] :avt02? ;

　 ③計算總時間:3 2 1t t t t ? ? ? ;

　 ④計算插補點數:sTtN ? ;

　 ⑤對插補點所在段進行判斷(勻速段、加速段、減速段),使各軸的增量得到確定,對各插補點坐標進行實時計算。

　 ⑥根據坐標值,通過運動學逆解求出各關節角。

　 ⑦利用五次多項式插值法對關節角的插值計算。

　 ⑧從以上各式分析可以瞧出,機械臂完成一個空間軌跡的過程,就是實現估計離散點的過程。讓其盡量逼近,使機械臂軌跡盡可能的符合規劃好的運動軌跡,本次采用定時插補法。

　 ⑨為了使機械臂的性能更好,讓末端執行器的軌跡更平滑,在相鄰兩個插值點的關節角間選取插補函數使關節軸運動更加穩定。此法將笛卡爾空間、關節空間相結合。如:工具末端沿著一個直線運動,通過上面的計算把直線段上插補 199次即整體直線軌跡分為 200 個點,每個坐標點進行逆運動學求解得到 200 組關節角度值。最后通過關節空間軌跡規劃的法將相鄰的兩組關節角之間進行角度插補,從而使工具末端的軌跡平滑且能很好的控制每個關節的角速度與角加速度[8] 。

　 在 MATLAB 中利用上述直線插補法對機械臂進行正形軌跡規劃仿真,機械臂的末端由起始點 A,經過 B 點、C 點、D 點返回 A 點。其中點 A、B、C、D 的位姿分別用齊次變換矩陣表示為:

　???????????????1 0 0 0180 1 0 060 0 1 0410 0 0 1AT???????????????1 0 0 0180 1 0 060 0 1 0290 0 0 1BT

　(3-16) ????????????????1 0 0 0180 1 0 060 0 1 0290 0 0 1CT????????????????1 0 0 0180 1 0 060 0 1 0410 0 0 1DT

　(3-17)

　 正形的每個邊長為 120mm,每個邊上插補 30 步,總仿真時間為 120s。正形軌跡的仿真結果如圖 6 所示,通過運動學求解得到五個關節角的位移數據并生成相關的數據曲線,如圖 7 所示。

　0 0.5 1 1.5 2 2.5 3-20-18-16-14-12-10-8-6-4-20時 間 t/s關節變量值 /o位 姿 1時 關 節 角 1軌 跡 規 劃 曲 線 圖 6 關節角位移軌跡曲線

　0 0.5 1 1.5 2 2.5 3-20-1001020304050時 間 t/s關節角1的關節速度 0/s關 節 角 1時 關 節 角 1速 度 軌 跡 曲 線 0 0.5 1 1.5 2 2.5 3-30-20-100102030405060時 間 t/s關節角1的加速度 0/s2位 姿 1時 關 節 角 1加 速 度 軌 跡 曲 線 圖 7 關節角速度與加速度軌跡仿真圖

　 由上述仿真圖可以瞧出,每個關節角度曲線均可劃分為 4 段,每段關節角度變化平穩光滑,只在正形四個頂點出變化最大,故還需要對頂點附近的關節角進行空間軌跡規劃。

　二、空間圓弧的軌跡規劃

　在笛卡爾空間圓弧軌跡規劃中,為了計算便,運用坐標變換,即先在圓弧所在平面建立一個新的直角坐標系,在這個直角坐標系中計算圓弧的各插補點在新坐標系中的值。然后再將這些值返回到原來的坐標系中,算出各插補點在原來坐標系中的值。圓弧插補的位移曲線也就是采用拋物線過度的線性函數,歸一化因子的求解與上述一樣[8] 。

　 三點確定一段弧。設機械臂末端執行器從起始位置1P 經過中間點2P 到達終點3P ,如果這三點不共線,就一定存在從起始點1P 經過中間點2P 到達終點3P 的圓弧軌跡規劃算法。具體算法如下:

　 ①先求得圓弧的圓心 ) , , (0 0 0 0z y x P 與半徑 r 。

　 ) , , (1 1 1 1z y x P 、 ) , , (2 2 2 2z y x P 與 ) , , (3 3 3 3z y x P 三點確定平面 M,其程為: 03 2 3 2 3 23 1 3 1 3 13 3 3?? ? ?? ? ?? ? ?z z y y x xz z y y x xz z y y x x

　(3-18)

　 將 其 展 開 可 得 :0 ) )]( )( ( ) )( [() )]( )( ( ) )( [() )]( )( ( ) )( [(3 3 1 3 2 3 2 3 13 3 2 3 1 3 1 3 23 3 1 3 2 3 2 3 1? ? ? ? ? ? ? ?? ? ? ? ? ? ?? ? ? ? ? ?z z y y x x y y x xy y z z x x z z x xx x z z y y z z y y

　(3-19) XYZOMP1P2P3P0T SWVU 圖 8 空間圓弧插補示意圖

　 過2 1 PP 的中點且與2 1 PP 垂直的平面 T 的程為:

　0 ) )]( (21[) )]( (21[ ) )]( (21[1 2 2 11 2 2 1 1 2 2 1? ? ? ? ?? ? ? ? ? ? ?z z z z zy y y y y x x x x x

　 (3-20)

　 過點3 2 PP 的中點且垂直3 2 PP 的平面 S 的程為: 0 ) )]( (21[) )]( (21[ ) )]( (21[2 3 3 22 3 3 2 2 3 3 2? ? ? ? ?? ? ? ? ? ? ?z z z z zy y y y y x x x x x

　 (3-21)

　 聯立上式,求得圓心 ) , , (0 0 0 0z y x P 。圓弧的半徑為: 21 021 021 0) ( ) ( ) ( z z y y x x r ? ? ? ? ? ?

　(3-22)

　 ②以圓心 ) , , (0 0 0 0z y x P 為原點建立圓弧所在平面的新坐標 UVW O R ? ,U 軸為坐標系原點0P 與點3P 的連線。單位向向量為3 03 0P PP Pu ? ?? ?? ;

　 ③W 軸為平面 T 與平面 S 的交線,其單位向向量為:3 2 2 13 2 2 1P P P PP P P Pw ? ? ? ?? ? ? ???? ;根據右手法則,V 軸在 W 軸與 U 軸的叉乘向,其單位向量為[13] : u w v ? ?

　(3-23)

　 根據齊次坐標變換可得齊次坐標矩陣RT 為: ?????????????1 0 0 0o z z zo y y yo x x xRz w v uy w v ux w v uT

　 (3-24)

　 其逆矩陣1 ?RT 可以根據齊次變換矩陣求解逆得到: ???????????z z zy y yx x xw v uw v uw v uR

　???????????oooozyxP

　(3-25)

　 可以得到:?????? ???1 01 oT TRP R RT ;

　 ④將點1P 、2P 、 3 P 以及圓心oP 從原來坐標系中的值轉換到圓心所在 UVW P ?0

　新坐標系中。設原來的坐標系中的值分別為 ) , , (1 1 1z y x 、 ) , , (2 2 2z y x 、 ) , , (3 3 3z y x 、) , , (o o oz y x ,在新坐標中值分別為 ) , , (1 1 1w v u 、 ) , , (2 2 2w v u 與 ) , , (3 3 3w v u ,則求解: ??????????????????????????1 11111111zyxTwvuR??????????????????????????1 12221222zyxTwvuR??????????????????????????1 13331333zyxTwvuR

　(3-26)

　 由上式推到知 03 2 1 0? ? ? ? ? ? w w w w v uo o, r u ?1;

　 ⑤求圓弧角度 ? 。由于在 MATLAB 中部函數 Math、Atan2(x,y)的求解圍在-1800 -180 0 之間。則:

　 當 03? v 時,則 ) , ( 2 tan3 3 3u v A ? ?

　3?? ? ?

　 ??????? ?? ?0cossinwr ur v??

　 (3-27)

　 ⑥將插補結果返回到原坐標中,設點 p 在原坐標系中坐標值為 ) , , ( z y x ,則有: ?????????????????????????1 1wvuTzyxR

　(3-28)

　 由以上結果可以得到圓弧上各插補點的位置,各插補點的三個位姿角度可以各自按照位移曲線為拋物線過度的線性函數求得。把每個插補點的位姿通過運動學逆解,就可以得到各插補點對應的關節角。

　 空間三點的位姿1p 、2p 與3p 可以分別用下式表示: ??????????????1 0 0 0400 0 0 160 0 1 0360 1 0 01 pT?????????????1 0 0 0300 0 0 140 0 1 0330 1 0 02 pT?????????????1 0 0 0200 0 0 160 0 1 0280 1 0 03 pT

　(3-29)

　 通過空間弧線的插補法,插補步數設為 N=200,仿真時間 40s,在 MATLAB 中進行空間圓弧的仿真,如圖 9 所示,同時獲得機械臂關節角度的數據曲線,如圖 10所示。機械臂的自由度數目應與所要完成的任務相匹配,空間圓弧在 M 平面上,機械臂的關節數對于空間圓弧軌跡而言就是冗余的。第五關節軸線與末端的工具

　軸線重合,關節五的角度并不影響空間圓弧的軌跡規劃中工具的位置與姿態,所以圖 10 中沒有第五關節的角度曲線。

　圖 9 圓弧軌跡插值 三、一般空間軌跡規劃

　在大多數情況下,可將 TCP 的目標運動軌跡劃分成若干段圓弧軌跡與直線軌跡的連接,從而在每個對應的小區段使用直線插值或圓弧插值的法完成整條 TCP的目標運動軌跡的插值。但就是,對于復雜程度相對較高的目標軌跡曲線,直線段加上圓弧段的組合在曲線精度面并不理想,常常難以滿足用戶需求。而且,將復雜程度高的軌跡曲線劃分為若干段微小的直線與圓弧的組合,就是非常困難的工作,特別就是還要保證直線段與圓弧段交接點處過渡平滑的條件。因此,對于任意復雜的軌跡,使用更高級的插值法勢在必行。較為常用的復雜軌跡插值法有:多項式插值法、分段線性插值法、分段多項式法、B 樣條插值法等等。要保證較高的插值精度,往往需要給定更密集的插值點序列,而且,如果使用的就是多項式插值法,為獲得高精度,多項式的次數也需要高。但就是,當次數高時,會產生龍格現象,即在插值區間兩端,會產生劇烈的震蕩現象,導致插值點不收斂于目標軌跡。三次樣條就就是通過全部樣點且具有連續二階導的函數,因此,選擇三次樣條插值對機械臂軌跡曲線進行規劃[10] 。

　 若三次樣條曲線所經過的 1 ? n 個插值樣點序列的X軸坐標為0x 、 1 x 、...、 n x 。則待求的三次樣條曲線參數程 ) (u s x ? 就是通過所有樣點,而且具有連續二階導

　數的分段三次多項式,即滿足條件:

　 (1) ) (u s 在每兩個樣點之間的函數表達式 ) (u s j 就是小于或等于三次多項式;

　 (2) ) (u s 在除起點與終點外的所有點處都有直到二階的連續導數。

　 若 ) (u s 在每兩個插值樣點之間的三次多項 ) (u s j 的表達式為: j j j j jd u c u b u a u s ? ? ? ?2 3) ( ) ,..., 2 , 1 ]; 1 , 0 [ ( n j u ? ?

　 (3-30)

　 其中上式中的系數為常數。

　則: ????????? ?個插值樣點之間 和第 第個插值樣點之間 和第 第個插值樣點之間 和第 第1 ), (....3 2 ), (2 1 ), () (n21n n u su su su s x

　(3-31)

　 所以,需要求得 n 組位置常數系數,才能得到完整的三次樣條參數程表達式。

　 假設在 ) (us? ?每個插值樣點出的值為0M ,1M 、、、nM ,又因為每兩個插值樣點之間的三次多項式 ) (u s j 的二階導 ) (us? ?師一次多項式,所以有: j j juM M u u s ? ? ??1) 1 ( ) ( ? ? ) ... 2 , 1 ]; 1 , 0 [ ( n j u ? ?

　(3-32)

　 對 ) (u s j ? ? 分別做一重與二重積分可得[14] : j j j j jM u M u du u s u s ?~] ) 1 ( [21) ( ) (212? ? ? ? ? ?? ?? ? ?

　 j j j j j ju M u M u du u s u s ? ?~~] ) 1 [(61) ( ) (313? ? ? ? ? ?? ? ?

　(3-33)

　 再把插值樣點的已知坐標1) 0 (??j jx s 與j jx s ? ) 1 ( 分別代入上式中,可得: 1 161? ?? ?j j jM x ? ,j j jM x61? ? ? ,代入上式得出: )61( )61)( 1 ( ] ) 1 [(61) (1 1313j j j j j j jM x u M x u M u M u u s ? ? ? ? ? ? ? ?? ? ?

　(3-34)

　 由求得的易知在函數 ) (u s 中,總共含有 1 ? n 個待求未知數:0M ,1M ,、、、nM 。因此只要求得未知數,并且相互獨立的 1 ? n 個程,才能確定函數 ) (u s 的全部未知數。

　 圖 10 三次樣條軌跡插值

　圖 11 直線與姿態勻速軌跡仿真圖

　 圖 12 圓弧軌跡插值仿真圖

　 圖 13 三樣條軌跡仿真圖

　 通過仿真分析,針對簡單且不復雜的目標軌跡曲線,一般采用直線軌跡規劃或者圓弧軌跡規劃,較復雜的可以采用直線軌跡規劃與圓弧軌跡規劃相結合的規劃法,可以得到較好的效果,但就是對于復雜程度相對較高的目標軌跡曲線,直線段加上圓弧段的組合在曲線精度面并不理想,常常難以滿足用戶需求。而且,將復雜程度高的軌跡曲線劃分為若干段微小的直線與圓弧的組合,就是非常困難的工作,特別就是還要保證直線段與圓弧段交接點處過渡平滑的條件。因此,對于任意復雜的軌跡,使用更高級的插值法勢在必行,要保證較高的插值精度,往往需要給定更密集的插值點序列,而且,如果使用的就是多項式插值法,為獲得高精度,多項式的次數也需要高。但就是,當次數高時,會產生龍格現象,即在插值區間兩端,會產生劇烈的震蕩現象,導致插值點不收斂于目標軌跡。三次樣條就就是通過全部樣點且具有連續二階導的函數,所以采用三次樣條軌跡規劃的法能夠達到要求的效果。

　第四章

　總結與展望

　空間機械臂作為就是一個機、電、熱、控一體化的高集成度的空間機電系統,有著廣闊的應用前景與很強的技術牽引與帶動作用。它的的發展將拓展并支撐空間飛行器的在軌操作與任務完成能力,同時可以帶動相關行業以及技術領域的發展。啟動空間機械臂的工程性研究,對于我國載人航天具有極其重要的意義。本文深入分析空間機械臂的自身運動特點之后,建立了機械臂的運動學模型,分析了機械臂直線運動、末端運動以及基于改進的人工勢場法的避障路徑規劃,設計

　開發了機械臂仿真軟件,并對路徑規劃進行了仿真與驗證。所得到的研究成果主要體現在以下幾個面:

　 1.深入研究了機器人運動學理論,針對課題研究的機械臂,建立了機械臂的正逆運動學程,基于窮舉法提出運動學正逆解的驗證法,用于驗證正逆解算法的精度及準確性。

　 2.研究了機械臂直線運動、末端運動的路徑規劃算法,并提出了衡量直線度的概率統計指標,來描述機械臂直線運動算法的精度及性能。

　 3、 研究了基于改進人工勢場法的避障路徑規劃算法,并在算法仿真基礎上,分析了位置增益系數對規劃算法路徑的影響,實現了機械臂末端障礙規避的路徑規劃。

　 4.基于 VC 與 OpenGL 設計開發了機械臂仿真軟件,實現了路徑規劃算法,并在機械臂仿真軟件平臺上對直線運動、末端運動、避障進行了三維仿真與驗證分析,仿真結果表明該系統能夠滿足課題所要求的功能與性能指標。

　 對應用于空間飛行器上的空間機械臂,本文限于時間與條件限制,在路徑規劃算法中假定飛行器就是固定的,未考慮機械臂運動與運動基座的動力學耦合,這在實際的空間應用中將會增大空間飛行器燃料的消耗;在機械臂的路徑規劃算法面研究更優化的算法;基于改進人工勢場法的避障路徑規劃算法還可在臂桿上設置更多的目標標記點,用于實現機械臂整體結構的避障研究。

　參考文獻

　[1] 于 登 云 , 京 , 馬 興 瑞 . 空 間 機 械 臂 技 術 及 發 展 建 議 . 航 天 器 工程.2007,16(4):1-8. [2] 暢,唐立軍,定祥,賀慧勇,司妞,濤.六軸機械臂在冰箱能耗監測線中的軌跡分析[J].電子科技.2014、04. [3] 紅強.工業機器人的時間最優軌跡規劃[D].:大學,2004. [4] 馬強.六自由度機械臂軌跡規劃研究[D].:工程大學,2007. [5] 亮,馬江,阮曉鋼.六自由度機械臂軌跡規劃與仿真研究[J].控制工程.2010、03. [6] 盧君宜.基于結構參數誤差補償的農業采摘機械臂的軌跡規劃[D].:工業大

　學,2010. [7] 好明.156R 關節型機器人軌跡規劃算法研究及仿真[D].:理工大學,2008. [8] 解 本 銘 , 偉 . 打 磨 機 械 臂 的 軌 跡 規 劃 與 仿 真 [J]. 中 國 民 航 大 學 學報.2010,28[4]:1-2. [9] The Universal Encyclopedia of Machines,1974. [10] 習雷平,自力,田慶民.地面無人作戰系統機械臂運動學建模與仿真.軍械工程學院學報[J].2012,01. [11]Taylor L W 、 Continuum Modeling of the Space Shuttle Remote Manipulator System、 Proc、of IEEE Int、 Conf、 on Decision and Control、 1992:626~631 [12]樹國,蔡鶴皋.空間智能機器人地面實驗綜合平臺實時仿真系統.中國宇航學會 機 器 人 學 術 會 議 ( 空 間 機 器 人 專 題 ) 論 文 集 . 工 業 大 學 機 器 人 研 究所.1992:49~52. [13]新松.機器人學導論.科學技術出版社.1994:511~516,543~554. [14]Brooks R A、 Solving the Find - path problem by good representation of free space、 IEEE Trans on Sys Manand Cybern、 1983,13(3):190~1971 [15]樹棟,曲彥賓.遺傳算法在機器人路徑規劃中的應用研究.西北工業大 學學報.1998,16(1):79~83.

推薦訪問： 軌跡 規劃 機械