改進(jìn)的基于測(cè)地線的機(jī)器人軌跡規(guī)劃方法

張延恒，孫漢旭，賈慶軒

（北京郵電大學(xué) 自動(dòng)化學(xué)院，北京，100876）

改進(jìn)的基于測(cè)地線的機(jī)器人軌跡規(guī)劃方法

張延恒，孫漢旭，賈慶軒

（北京郵電大學(xué) 自動(dòng)化學(xué)院，北京，100876）

針對(duì)機(jī)器人運(yùn)動(dòng)規(guī)劃過程中測(cè)地線微分方程初始條件的確定問題，提出一種基于微分運(yùn)動(dòng)的初始條件求解方法。該方法通過對(duì)比分析黎曼空間與三維空間中機(jī)器人末端運(yùn)動(dòng)軌跡，確定兩者之間的空間對(duì)應(yīng)關(guān)系，在此基礎(chǔ)上建立測(cè)地線微分方程初始條件的求解模型。最后，以2自由度機(jī)器人為例，對(duì)比驗(yàn)證該方法的有效性。研究結(jié)果表明：該模型解決了以往采用線性搜索方法求解微分方程初始條件所造成的求解不確定性問題。

機(jī)器人；軌跡規(guī)劃；測(cè)地線

機(jī)器人運(yùn)動(dòng)規(guī)劃是機(jī)器人研究首先要面對(duì)的問題。目前，關(guān)于機(jī)器人運(yùn)動(dòng)規(guī)劃的研究很多，但大多是以運(yùn)動(dòng)學(xué)逆解和多項(xiàng)式插值為基礎(chǔ)進(jìn)行的規(guī)劃研究[1?5]。隨著研究手段的發(fā)展，李群李代數(shù)及微分幾何等數(shù)學(xué)工具被越來越多地運(yùn)用到機(jī)器人運(yùn)動(dòng)規(guī)劃的研究中[6?12]。特別地，張連東等[6?8]采用微分幾何中活動(dòng)標(biāo)架方法，對(duì)機(jī)器人的操作能力、運(yùn)動(dòng)規(guī)劃等問題進(jìn)行了研究，所采用的基于黎曼曲面上的測(cè)地線進(jìn)行機(jī)器人最優(yōu)軌跡規(guī)劃的方法，具有非時(shí)間參考軌跡規(guī)劃的優(yōu)點(diǎn)。但他們?cè)谇蠼鉁y(cè)地線微分方程時(shí)，采用的是線性搜索的方法，該方法通過設(shè)定關(guān)節(jié)初始廣義速度范圍，在小步長(zhǎng)增量下，設(shè)定搜索誤差范圍；當(dāng)搜索到的目標(biāo)曲線上任意點(diǎn)與理想終點(diǎn)之間的誤差小于設(shè)定的搜索誤差時(shí)，認(rèn)為搜索點(diǎn)滿足條件，即由該搜索點(diǎn)（廣義速度）作為測(cè)地線微分方程組的初始廣義速度，從而完成對(duì)微分方程組的求解。這種方法存在2個(gè)缺點(diǎn)：一是在設(shè)定搜索范圍時(shí)，較難確定廣義初始速度的取值范圍；二是這種搜索方法并不是一個(gè)準(zhǔn)確的求解方法，在關(guān)節(jié)變量較多時(shí)搜索計(jì)算量大，而且搜索收斂性下降。針對(duì)此問題，本文作者提出了一種采用微分運(yùn)動(dòng)求解測(cè)地線微分方程初始條件的方法。該方法通過建立測(cè)地線初始方向與關(guān)節(jié)速度間的空間映射關(guān)系，從而實(shí)現(xiàn)精確的求解微分方程初始條件，簡(jiǎn)化了測(cè)地線微分方程的求解過程。

1 黎曼度量與測(cè)地線

黎曼度量是黎曼曲面上一個(gè)對(duì)稱正定的張量場(chǎng)[13]，對(duì)于機(jī)器人末端運(yùn)動(dòng)曲面，其運(yùn)動(dòng)的軌跡弧長(zhǎng)的平方是一個(gè)正定對(duì)稱的二次形式，因而就構(gòu)成了一個(gè)黎曼度量。此黎曼度量是黎曼曲面上2點(diǎn)之間距離的度量方式。

在黎曼曲面上的測(cè)地線為曲線上每一個(gè)點(diǎn)的測(cè)地曲率為0的曲線，即沿測(cè)地線方向曲面上2個(gè)點(diǎn)之間的距離最短。因此，對(duì)機(jī)器人末端進(jìn)行軌跡規(guī)劃時(shí)，首先要將機(jī)器人的運(yùn)動(dòng)學(xué)特性進(jìn)行幾何化，并將其映射成黎曼曲面的形式，建立黎曼曲面后，就可以對(duì)曲面進(jìn)行分析，從而脫離機(jī)器人實(shí)體，而單純通過幾何方法來研究機(jī)器人的運(yùn)動(dòng)，即通過研究機(jī)器人運(yùn)動(dòng)曲面上的測(cè)地線來對(duì)機(jī)器人進(jìn)行最優(yōu)軌跡規(guī)劃。在利用測(cè)地線對(duì)機(jī)器人進(jìn)行運(yùn)動(dòng)規(guī)劃時(shí)，機(jī)器人各關(guān)節(jié)之間的運(yùn)動(dòng)不再是獨(dú)立的，它們之間的耦合關(guān)系由測(cè)地線微分方程決定。

2 改進(jìn)的機(jī)器人軌跡規(guī)劃方法

2.1 兩自由度機(jī)器人黎曼度量建模

平面兩自由度機(jī)器人如圖1所示，其操作臂末端位置可表示為：

式中：l1和l2分別為連桿1和連桿2的桿長(zhǎng)；q1和q2分別為關(guān)節(jié)1和關(guān)節(jié)2的轉(zhuǎn)角。

圖1 兩自由度機(jī)器人模型Fig.1 Model of two-degree robot

據(jù)此建立以弧長(zhǎng)平方為度量的機(jī)器人黎曼度量：

其中：s為軌跡弧長(zhǎng)；a1,a2和a3為與系統(tǒng)結(jié)構(gòu)及狀態(tài)有關(guān)的參數(shù)。將其寫成黎曼度量系數(shù)矩陣的形式為：

根據(jù)式（1）確定了黎曼度量系數(shù)矩陣后，機(jī)器人的運(yùn)動(dòng)就從真實(shí)的物理空間轉(zhuǎn)換到了幾何空間，即完成了對(duì)機(jī)器人運(yùn)動(dòng)性質(zhì)的幾何化。對(duì)機(jī)器人運(yùn)動(dòng)的研究就轉(zhuǎn)換為對(duì)該幾何空間中對(duì)應(yīng)的黎曼曲面的研究。機(jī)器人末端執(zhí)行器2點(diǎn)之間的最短運(yùn)動(dòng)路徑對(duì)應(yīng)于該黎曼曲面上的測(cè)地線[14]，所得到的測(cè)地線微分方程如下：

式中：Γ為克里斯托弗系數(shù)。

為求解此二元二次微分方程組，將此微分方程組進(jìn)行降階處理，轉(zhuǎn)換成標(biāo)準(zhǔn)的狀態(tài)方程的形式，可得：

2.2 確定微分方程初始條件

由式（2）可看到，要對(duì)該微分方程進(jìn)行求解，必須已知微分方程求解的初始條件，即初始角度和廣義速度。對(duì)于機(jī)器人運(yùn)動(dòng)規(guī)劃，通常情況下初始時(shí)各關(guān)節(jié)角度為已知量，因而還必須確定初始時(shí)的廣義速度，該廣義速度是關(guān)于末端軌跡弧長(zhǎng)的微分，其表示式為：

對(duì)于機(jī)器人，其末端運(yùn)動(dòng)的所有可達(dá)空間構(gòu)成了抽象的空間曲面——黎曼曲面，在進(jìn)行機(jī)器人空間2點(diǎn)之間的運(yùn)動(dòng)規(guī)劃時(shí)，采用微分幾何中的測(cè)地線方法。測(cè)地線為黎曼曲面上2點(diǎn)之間長(zhǎng)度最短的曲線，因此，可以將黎曼曲面上的測(cè)地線等距對(duì)應(yīng)于三維歐氏空間中的曲線。在三維歐氏空間中，2點(diǎn)之間的最短曲線即為直線。該最短直線與黎曼曲面上的測(cè)地線對(duì)應(yīng)。由此可知，對(duì)應(yīng)于機(jī)器人末端執(zhí)行器最短運(yùn)動(dòng)距離，末端執(zhí)行器必須沿空間運(yùn)動(dòng)始末2點(diǎn)之間的連線運(yùn)動(dòng)，因而可以通過如下方法確定運(yùn)動(dòng)初始廣義速度。

對(duì)于空間2點(diǎn)X1和X2（其中X1為運(yùn)動(dòng)起始點(diǎn)，X2為運(yùn)動(dòng)終止點(diǎn)），其運(yùn)動(dòng)矢量為：

對(duì)式（3）所表示的矢量單位化，可得到：

在此設(shè)定末端運(yùn)動(dòng)初始速度v，則機(jī)器人末端運(yùn)動(dòng)速度矢量v可表示為：

根據(jù)機(jī)器人運(yùn)動(dòng)學(xué)關(guān)系[15]可以得到機(jī)器人運(yùn)動(dòng)雅可比矩陣J（該雅可比矩陣必須為滿秩矩陣，對(duì)于冗余度機(jī)器人，可通過設(shè)定約束條件使其成為滿秩），則由末端運(yùn)動(dòng)速度與關(guān)節(jié)運(yùn)動(dòng)速度之間的雅可比矩陣可以得到此速度所對(duì)應(yīng)的關(guān)節(jié)速度：

根據(jù)測(cè)地線微分方程可知該微分方程所解得的關(guān)節(jié)角度是關(guān)于末端運(yùn)動(dòng)距離s的函數(shù)，即：

由于機(jī)器人末端執(zhí)行器運(yùn)動(dòng)距離從0逐漸增大，因此，距離增量ds為正值，有：

將式（5）和（7）代入式（6）可得到最終形式的機(jī)器人關(guān)節(jié)速度：

由式（8）可以看出：對(duì)于給定機(jī)器人末端執(zhí)行器運(yùn)動(dòng)始末2點(diǎn)，其關(guān)節(jié)角度關(guān)于末端運(yùn)動(dòng)弧長(zhǎng)的微分在給定點(diǎn)處的大小只與該點(diǎn)處的運(yùn)動(dòng)雅可比矩陣及2點(diǎn)之間的方向矢量有關(guān)，而與末端執(zhí)行器在此位置時(shí)的實(shí)際運(yùn)動(dòng)速度無關(guān)。對(duì)應(yīng)于測(cè)地線微分方程，可以看出：對(duì)于黎曼曲面上的給定點(diǎn)，存在無數(shù)條測(cè)地線，而對(duì)于給定的末端執(zhí)行器運(yùn)動(dòng)終止點(diǎn)，對(duì)應(yīng)于黎曼曲面上的測(cè)地線只存在1條。

3 方法驗(yàn)證

對(duì)于圖1所示兩自由度機(jī)器人，機(jī)械臂各連桿桿長(zhǎng)為1 m。處于圖示位置時(shí)，各關(guān)節(jié)轉(zhuǎn)角q1=π/3，q2=?2π/3。機(jī)械臂由當(dāng)前位置P1=（1, 0）T運(yùn)動(dòng)至空間點(diǎn)即沿方向運(yùn)動(dòng)個(gè)單位，所得到的仿真結(jié)果如圖2和圖3所示。

與文獻(xiàn)[6?7]所得到的軌跡規(guī)劃結(jié)果進(jìn)行對(duì)比可知：本文所得結(jié)果與文獻(xiàn)[6?7]中所得結(jié)果一致，證明了本文所提出的確定測(cè)地線微分方程初始條件的方法的正確性。本文所采用的基于微分運(yùn)動(dòng)的測(cè)地線微分方程初始條件求解方法，可以精確求解測(cè)地線微分方程的初始方向（廣義速度）。

圖2 關(guān)節(jié)角度規(guī)劃結(jié)果Fig.2 Planning results of joint angle

圖3 角度耦合關(guān)系Fig.3 Relationship of joint angles

4 結(jié)論

（1） 本文所提出的計(jì)算方法解決了以往采用線性搜索方法求解微分方程初始條件所造成的求解不確定性問題。

（2） 在給定規(guī)劃起始點(diǎn)和終止點(diǎn)時(shí)，測(cè)地線微分方程的初始方向僅與該點(diǎn)處的運(yùn)動(dòng)雅可比矩陣及2點(diǎn)之間的方向矢量有關(guān)，而與末端執(zhí)行器在此位置時(shí)的實(shí)際運(yùn)動(dòng)速度無關(guān)。

[1] 李東潔, 邱江艷, 尤波. 一種機(jī)器人軌跡規(guī)劃的優(yōu)化算法[J].電機(jī)與控制學(xué)報(bào), 2009, 13（1）： 123?127.

LI Dong-jie, QIU Jiang-yan, YOU Bo. Optimal algorithm for trajectory planning of the robot[J]. Electric Machines and Control, 2009, 13（1）： 123?127.

[2] Park J J, Kim J H, Song J B. Path planning for a robot manipulator based on probabilistic roadmap and reinforcement learning[J]. International Journal of Control, Automation and Systems, 2007, 5（6）： 674?680.

[3] Blackmore L, Williams B. Optimal manipulator path planning with obstacles using disjunctive programming[C]//Proceedings of the 2006 American Control Conference. Minneapolis, 2006： 3200?3202.

[4] 譚冠政, 徐雄, 肖宏峰. 工業(yè)機(jī)器人實(shí)時(shí)高精度路徑跟蹤與軌跡規(guī)劃[J]. 中南大學(xué)學(xué)報(bào)： 自然科學(xué)版, 2005, 36（1）： 102?107.

TAN Guan-zheng, XU Xiong, XIAO Hong-feng. Real-time and accurate hand path tracking and joint trajectory planning for industrial robots[J]. Journal of Central South University： Science and Technology, 2005, 36（1）： 102?107.

[5] 黃志雄, 何清華, 吳萬榮, 等. 大型水輪機(jī)葉形現(xiàn)場(chǎng)檢測(cè)機(jī)械臂運(yùn)動(dòng)學(xué)分析[J]. 中南工業(yè)大學(xué)學(xué)報(bào)： 自然科學(xué)版, 2003, 34（2）： 170?172.

HUANG Zhi-xiong, HE Qing-hua, WU Wan-rong, et al. Kinematics analysis of field measuring manipulator of large turbine blades[J]. Journal of Central South University： Science and Technology, 2003, 34（2）： 170?172.

[6] 張連東, 王德倫. 一種基于測(cè)地線的機(jī)器人軌跡規(guī)劃方法[J].機(jī)器人, 2004, 26（1）： 83?86.

ZHANG Lian-dong, WANG De-lun. Robot trajectory planning based on geodesics[J]. Robot, 2004, 26（1）： 83?86.

[7] 張連東. 基于微分幾何學(xué)的機(jī)器人操作性能的研究[D]. 大連：大連理工大學(xué)機(jī)械工程學(xué)院, 2004： 53?54.

ZHANG Lian-dong. Robot manipulability research based on differential geometry[D]. Dalian： Dalian University of Technology. School of Mechanical Engineering, 2004： 53?54.

[8] ZHANG Lian-dong, ZHOU Chang-jiu. Robot optimal trajectory planning based on geodesics[C]//2007 IEEE International Conference on Control and Automation. Guangzhou, 2007： 2433?2436.

[9] Altafini C. Redundant robotic chains on riemannian manifolds[C]//Proceedings of the 41st IEEE Conference on Decision and Control. Lasvegas, 2002： 1534?1539.

[10] Altafini C. Geometric motion control for a kinematically redundant robotic chain： Application to a holonomic mobile manipulator[J]. Journal of Robotic Systems, 2003, 20（5）： 211?227.

[11] Rodnay G, Rimon E. Isometric visualization of configuration spaces of two degrees of freedom mechanisms[C]//Proceeding of the 1998 IEEE International Conference on Robotics and Automation, Belgium, 1998： 2928?2934.

[12] Zefran M, Kumar V, Croke C B. On the generation of smooth three dimensional rigid body motions[J]. IEEE Transactions on Robotics and Automation, 1998, 14（4）： 576?589.

[13] 陳省身, 陳維桓. 微分幾何講義[M]. 北京： 北京大學(xué)出版社, 2001： 129?139.

CEHN Xing-shen, CHEN Wei-huan. Lectures on differential geometry[M]. Beijing： Beijing University Press, 2001： 129?139.

[14] 梅向明, 黃敬之. 微分幾何[M]. 北京： 高等教育出版社, 2003： 213?227.

MEI Xiang-ming, HUANG Jing-zhi. Differential geometry[M]. Beijing： Higher Education Press, 2003： 213?227.

[15] 熊有倫, 丁漢, 劉恩滄. 機(jī)器人學(xué)[M]. 北京： 機(jī)械工業(yè)出版社, 1993： 87?100.

XIONG You-lun, DING Han, LIU En-cang. Robotics[M]. Beijing： China Machine Press, 1993： 87?100.

（編輯 趙?。?/p>

An improved robot trajectory planning method based on geodesics

ZHANG Yan-heng, SUN Han-xu, JIA Qing-xuan
（Automation School, Beijing University of Posts and Telecommunications, Beijing 100876, China）

To solve the problem of the initial conditions of geodesics differential equations, a novel method based on differential motion was proposed. By way of analyzing the motion trajectory of robot between Riemannian space and three-dimensional space, their spatial mapping relationship was confirmed and a model used to compute the initial conditions was developed. Finally, a two-link manipulator was used to verify the application of this method. The results show that this model avoids the uncertainty problem existing in the linear search method used to solve initial conditions of the geodesics differential equations.

robot; trajectory planning; geodesic

TP242

A

1672?7207（2011）05?1344?04

2010?01?28；

2010?05?10

國(guó)家自然科學(xué)基金青年科學(xué)基金資助項(xiàng)目（50905019）；中央高?；究蒲袠I(yè)務(wù)費(fèi)專項(xiàng)資金資助項(xiàng)目（2010PTB-07-01）

張延恒（1978?），男，山東煙臺(tái)人，博士，講師，從事機(jī)器人運(yùn)動(dòng)學(xué)、動(dòng)力學(xué)及控制研究；電話：010-62281368；E-mail： zyh620@163.com