笛卡儿空间机器人轨迹规划方法

第３卷第３期

２００２年６月北华大学学报（自然科学版）ＪＯｕＲＮＡＬｏＦＢＥＩＨｕＡＵＮＩＶＥＲＳＩＴＹ（ＮａｔｕｒａｌＳｃｉｅｎｃｅ）Ｖｏ【３Ｎｏ３Ｊｕｎ．２００２

文章编号：１００９＿４８２２（２００２）０３０２６９．０３

笛卡儿空间机器人轨迹规划方法

孙羽１，张汝波２

（ｊ湛江海洋大学计算机系，广东湛江盟４０８８；

２．哈尔滨工程大学计算机系，黑龙江哈尔滨Ｊ５００００）

摘要：提出了笛卡儿空间机器人连续路径（ＣＰ）轨迹规螂方法．移动轨迹在笛卡儿坐标系中由ｌ蛆参数方程说明

时间间隔［０，Ｔ］分为ｍ段．在每个时间段，多项式的系数可以用递归的形式求出

美健词：机器人；轨迹规划；参数方程

中图分类号：ＴＰ２４２文献标识码：Ａ

大部分机器人应用需要感应器沿着指定路线完成任务和避开障碍物．实际上，典型的机器人任务包括移动感应器指明笛卡儿位置或者指出笛卡儿路径，ｃＰ机器人应用在焊接、灭火、搜寻等方面，机器人轨迹规划取决于前一时刻位置、速度以及加速度．要完成一个平滑移动不仅需要指定移动中每一个中间点的位置而且需要指定速度轨迹规划可从关节变量集得到也可由笛卡儿空间得到．然而，笛卡儿空间路径规划需要把笛卡儿坐标转化成关节坐标，由于机器人移动控制是在关节进行的，路径规划和坐标变换上计算的复杂问题已经妨碍了笛卡儿路径规划为解决这一问题，许多科学工作者提出了一系列解决方法Ｙｏｕｃｅｆ—Ｔｏｕｍｉ…提出了机器人高速轨迹控制方法．Ｋａｈｎ和Ｒｏｔｈ【２１研究机器人ＰＴＰ移动轨迹时间最优规划问题；Ｐａｕｐ］提出一种技术平稳地把手从直线一个分段移动到另一段，在片段之间手的速度和加速度通过转换成关节坐标得到控制后来，Ｔａｙｌｏｒ【４’通过使用二元函数，四元数表示手的位置，改进了Ｐａｎｌ’ｓ方法．Ｋｉｎ和ｓｈｉｎ【５Ｊ提出了关节空间的最优时间路径规划然而他们所提出的方法太复杂只能离线完成．Ｌｕｈ和Ｌｉｎｌ６Ｊ提出三次样条函数法来描述手的ｃＰ运动，他们试图寻找一系列时间划分，使整个移动过程时间最优，可是如果中间插入点增加，会增加大量的计算．

１机器人ＣＰ移动轨迹规划

ｃＰ移动特点是感应器不仅要到达最终目标点，而且必须按照预想的路径移动这里我们用笛卡儿空间表示轨迹规划．假设当前位置之前时间、速度和加速度已确定，相应的关节置换、速度和加速度可通过感应器的位置和方向通过运动学反解计算得到设下是机器人ｃＰ运动的时间，ｆｏ和｝ｒ分别表示开始时间和终止时间时间间隔［Ｏ，了１］分为［ｆｏ，￡１］，［ｆｌ，‘２卜…‘，［ｒ一１，ｒ。】，。ｏ＝０，｝。＝‘，，。，一￡ｏ＝丁，△￡，＝￡：一￡。一１（ｉ＝１，２，…，ｍ），感应器移动轨迹在笛卡尔空间用六维向量ｒ（￡）表示如下：

ｒ＝【ｚ，√，ｚ，ｄ，卢，廿］１＝ｆ尸１，Ｒ１］。

Ｐ＝［ｚ，ｙ，ｚ］１为位置向量，Ｒ＝［ｎ．ｐ，≯］１为方向向量，ａ，ｐ，ｐ是笛卡儿坐标系感应器方向角度相应的关节向量表示为：ｑ＝［ｑ１，ｑ２……，ｑ”ｊ７，假设连续路径可由一组参数方程表示如下：

ｚ＝，ｌ（５）；

ｙ＝，２（５）；

ｚ＝／３（ｓ）；

５＝５（￡）

收稿日期：２００１．１１—１６（１）（２）（３）（４）

作者简介：孙羽（１９６３一），男，硕士，副教授，主要从事人工智能的研究

万方数据　

２７０北华大学学报（自然科学版）第３卷ｚ，ｙ，ｚ表示感应器的位置。ｓ表示移动路径长度

一般地，感应器移动时，方向角也改变．为了精确地指出感应器（机器人）移动轨迹，我们必须知道方向角度的变化．因此设ｎ，，Ｂ，以表示感应器的方向角度，设

口，＝。ｏ＋（“，一ｎｏ）×“／丁；

且＝卢。十（卢，一卢ｏ）×ｆ，／Ｔ；

咖＝驴ｏ＋（妒，一妒ｏ）×岛／Ｔ．（５）（６）（７）

ａｏ，ａ，，岛，卢，，‰，咖分别表示感应器初始和终止点方向角度每一时间间隔ｆ，２￡，一－＋△ｆ。，感应器坐标变换矩阵Ｔ。根据路径方程，位置和方向限制条件首先计算，然后关节变量和它们的增量通过运动学反向解得到，相应的关节轨迹向量ｑ（ｆ）可由反向解得到．增量移动和Ｊ矩阵相关，因此曲（ｆ）＝Ｊ。毋（￡）有

弓（ｚ）＝，－１｝（￡），

ｑ（ｆ）＝，＿１［ｒ（ｆ）一（ｄｊ／ｄｆ）ｒ（ｒ）］

，假设不是单一的，其反函数Ｊ－１可用数值形式或符号形式求得，或用反向求解方程求微分得到．考虑感应器从Ｐｔ移到Ｐ，＋，，假设移动距离很小，关节ｉ点加速度不变，这种情况下，每点速度和加速度用下面公式计算出来

ｑ。＝（ｑｆ十１。，一矶）／△ｆ，＝△口“／△￡。，

ｑ。＝（％一吼一２，，）／（△￡。）２．（８）（９）

其中，△ｑ。＝ｑ。一ｑ。一¨，净Ｏ，ｌ…，Ⅲｊ＝１，２，…，ｎ（ｎ：ＤｏＦ值）．关节在ｆ，一ｌ和ｒ，时刻的位置和速度确定之后，可以用３次（或４次或５次）多项式表示，每１间隔［ｒ，一１，ｆ，］轨迹在没有限制速度的情况下（不包括起始点和终止点），３．３。３．…．３—５轨迹可以使用，这是一个３次样条曲线（适合于除最后一段之外所有轨迹段），这里规定￡为标准划时间变量，ｆ∈［Ｏ，１］，ｒ为实际时间，

ｒ∈ｎ，一１，ｆＪ，｝＝（ｒ—ｆ。一１）／（￡。一ｆ｜－１）＝（ｒ一￡Ｈ）／＾¨＾。＝△ｏ＝ｒ。一￡，。ｌ，

Ｌ为第ｉ轨迹段上实际时间，每个时问段上每个关节变量表示如下：

９１（ｆ）＝４１０＋４ｌｌｆ＋口ｉ２ｔ２＋ｄ１３ｆ３，（１０）

ｑ，（￡）＝口ｆｏ＋口，１ｆ＋ｎ。２ｆ２＋４玛｝３，１＜ｆ＜优，（１１）

口。（￡）＝４。ｏ＋ｎ。。１ｆ＋ｎ。。２ｆ２＋Ⅱ。３ｒ３＋口，４ｒ４＋ｄ。．５￡５（１２）

如果关节在开始和终止点（初始条件）ｑｏ，ｑ，的速度和加速度已知，中间置换点给出，可得到全部系数，根据公式（１０）有

ｊ１（ｆ）＝（口１１＋２Ⅱ１２ｒ＋３口１３￡２）／＾１，

ｑ１（ｆ）＝（２口１２＋６ｎ１３ｆ）肌１２

由初始条件

可得ｑ１（ｏ）＝ｏ，÷１（ｏ）＝；ｏ，ｑ１（ｏ）＝口ｏ，９１（１）＝ｑ１口１０＝ｑｏ，Ⅱ１ｌ＝毒ｏ＾１，口１２＝ｑｏ＾１２／２，ｎ１３＝ｑ，一ｇｏ一奇ｏ＾ｌ—ｑｏ＾；／２

ｎ．ｏ＝ｑ，一ｌ，。订＝ｑ。一ｌ＾，，口，２＝ｑ＿一１＾。２／２，ｎ。３＝口。一ｑｉ一１一ｑｉ—ｌ＾ｉ～ｑ。一１＾。。／２．用同样的方式可得到公式（儿）多项式的所有系数

其中１＜ｉ＜Ⅲ一１．

根据初始条件公式（１２）

ｑ。（Ｏ）＝ｑ。一ｌ，ｑ。．（０）＝ｑ。一１，ｑ。，（０）＝ｑ一１，

‰（１）＝ｑ，，ｊ。，（１）＝ｊ

可得ｒ，ｑ。。（１）＝ｑ，

“舢＝‰一ｌ，ｎ，Ⅳ１＝ｑⅢ一ｌ＾ｍ，ａ”１２＝ｑⅢ一１＾”ｚ２／２，ｄＭ３＝１０‘１—４‘２＋‘３／２，“Ⅲ４＝７‘２—１５‘１一‘３

万方数据　

第３期

ｎ。，５＝６ｃ１—３ｃ２＋ｃ孙羽，等：笛卡儿空间机器人轨迹规划方法２７１３／２，ｆ１＝ｑ，一Ⅱ。，ｏ一Ⅱ。，ｌ—ｎ。２，ｃ２＝ｊ，＾。，一ｎ。１～２ｎ。２，ｃ３＝ｑｒ＾。：２—２ｄ。２因此所有间隔关节轨迹可规划如果考虑中间点的关节速度的限制，可以用４—３—３一－一３—３—４轨迹，这种轨迹第１个分段是４次多项式轨迹从起始点到初动点，之后除最后轨迹分段外（最后分段用４次多项式表示）都用３次多项表示多项式方程中每个关节变量（每个时间段上）表示为

ｇｌ（￡）：。ｌｏ＋口１１￡＋ｎ１２ｆ２＋ｎ１３ｆ３＋口１４￡４，（１３）

吼（ｆ）＝４，ｏ＋ｎ。１ｆ＋ｄ，２ｆ２＋ａ门￡３１＜ｉ＜研（１４）

ｑｍ（｝）＝口”ｌｏ＋口。１ｆ＋ｎ。２￡２＋ｄ。，３￡３＋。。４ｆ４

初始条件（１５）

ｑ１（ｏ）＝ｑｏ，ｑ（１）＝ｑｌ，毒１（ｏ）＝毒ｏ，÷１（１）＝ｊ１．口ｌ（ｏ）＝ｑｏ，ｑ。（ｏ）＝ｑ。一ｌ，口ｆ（１）＝ｑ，，

ｊｆ（ｏ）＝÷。一ｌｊ。（１）＝ｊ，，ｑ。（ｏ）＝ｑ。Ⅲ口。（１）＝ｑ，，ｊ。（ｏ）＝÷一１，ｊ。（１）＝ｊｒ，；。（１）＝；，，由这些初始条件，可以得到公式（１３）～（１５）所有系数

。１０＝ｑｏ，。１１＝ｑｏ＾１，８１２＝口ｏ＾１。／２，。”＝４。Ｉ一。２，。“＝。２—３。１，

‘１＝ｇｌ一８１０一。１１—２４１２，。２＝＾１ｑｌ一。１１—２８１２．

因为＜

｜ｌ＝

因为３三。瓤ｍ个ｍ＝时有礼阿∞段ｑ有３８ｆｏ一２ｎ。ｌ一口正。，ｄ。３＝ｑ＾，一２吼＋２ｎ。ｏ十口。Ｉ

ｄ。ｏ＝ｑｍ一１，Ⅱ。１＝ｑ。一ｌ＾ｍ，口。，２＝６Ｄｌ一３Ｄ２＋Ｄ３／２，口。。３＝４Ｄ２—８Ｄ１一Ｄ３／２，