机器人运动轨迹规划分析与算法

机器人运动轨迹规划分析与算法 徐向荣马香峰 华东 冶金学院机械系 , 马鞍山市) (北京钢铁学院机 器 人研究所) 摘要本文在i L n 和L u h等人工作的 基础上 , 提出了一种新的机器人 C P运动轨迹规划算法 。 在该算法中 , 连续路 径由一 组直角坐标下 的参数方 程来描述 。 时间区间。 , T)被分成 m段 , 并且各轨迹 段 的拟合多项式系数采用递推方式 求得 , 这不仅易 子计 纤机实现 , 而且计算全也较少 . 另 外本文对时间最优轨迹规划问题也作了 分析 和研究 , 并提出了一 种有效的算法 . 关镜词 : 操作器 , 轨迹规划 , 递推 , 多项式 . 1 引言 轨迹规划是机器人研究中一个非常 重要课 题 , 国内外已有不少学者在这方面做了大量工 作 “ 一。 。 K a h n 和R oth “早 在 197 1年就研究 过机器人作 P T P 运动 时时间最优轨迹规划 问 题 , 但作了不少假设和近似 。 P a u l “ 曾提出 一种机器人手臂沿空间直 线段运动的关节轨迹 规划方法 , 他首先利用齐次变换矩阵将手部在 直角 坐标下的位置 、 速度和加速度变换成各关 节的位移 、 速度和加速度 , 然后 用插 补法将每 一轨迹段 上关节的位移 、 速度和加速度规划成 二次平滑函数 。 P a u l方法易于 理解和应用 , 但 计算量非常大 。 T a y l o r 咭 改进了 P a u l 的方 法 。 他采用四元数表示法来描述手部位置 , 为 减小轨迹误差 , 他将轨迹离散 成一定数量的点 , 然后再逐步计算出这些离散点上关节的位移 、 速度和加速度 。 后来K im和 si h n “ 提出一种 时间最短轨迹规划方法 , 这 种方法是基于关节 空间的 , 并考虑了各种实际约束条件其中也包 括动力学约束 , 但这种方法较复杂 , 且只 能离 线完成 。 后来 i L n 和 L u h等人提出 了规划机器 人 CP 运动轨迹 的三次样条函数方法 6 ,7 , 这 种方法的优点是可得到优化 的关节运动规律 , 但当轨迹中间路径点个数 n 较多时 , 此法所需 计算量也就较大 。 另外这 种方法还要求在轨迹 规划前计算出整个路径 上所有中间点 , 这也使 得这种方法具有局限性 。 本文在 i L n 和 L u h 等 人工作的 基础 上 , 通过对轨迹规 划的研究 , 得 收 到本文的 时 间是 19 87年4月2。日 . 到一种机器人CP运动 的轨迹规 划算法 , 在 这 种算法中 , 各轨迹段 的拟合多项式系数采用递 推方式求得 , 这不仅 易于计算机实现 , 而且计 算量也较少 。 2 机器 人CP运动的轨迹规划算法 机器人 CP 运动也即连 续轨迹 运 动的特点 在于 : 我们不仅要求机器人到达目标点 , 而且 必须 沿着我们所希望 的路径在一定精度范围内 移动 。 本 文采用基 于直 角坐标方案来 规 划 CP 运动轨迹 , 这种方案就是 : 轨迹是以机器人手 的直角坐标位 置和姿势对 )时 间 t 的函数来描述 的 。 为规划轨迹 , 我们进行如下考虑 : 假定机器人完成C P运动所需时 间为T , 将 区间。 , T分 成m 段(m与精度有关 , m 越 大 , 则精度越高) : 工 。, t , , 才 :, 公: , , t 。 _ , t、 , r。= o , t , = T 。 每个子 区I e d长度 为 : 乙t 二r、 一亡 一 , (i = 1 , , 。) , 在 每 一时 刻 t (亡、 =艺 一, + 尔 ) , 首先根据 连续轨 迹的方程和手部姿势要求 , 计算出机器人手部 坐标系相对 基座参照 坐标系的位置和姿势 , 再 进 行运动 学反解计算 , 以求得期望 的关节转角 (或位移)和关节角增量 , 这样共要进行 m 十 l次运动学反解计算 。 当求得在 t 、一 , 和 t;两点 时各关节变量值和 速度值 时 , 再用一多项 式来 拟 合在区间t ; 一 , “内关节的 运 动 。 因此 , 机器人 CP运动轨迹的规划比PT P 运动轨迹规 划所需的计算量要大得多 。 当机器人手部沿连续轨迹运动时 , 除了位 置要 求外 , 往往还有速度和加速度要 求 , 因此 为了使规划出的 各关节运动 规律还 能满足手部 2 卷 6 期机器人运动轨迹规划分析与算法 在直角坐标下的速度和加速 度要求 , 还应进行 运动学速度和加速度反解计算 , 一般方法是 : 先求出雅可比矩 阵 J , 然后对其求逆和求导 , 再由下面两式进行计算 。 将手部速度和加速度 的约束变换为对关节速度和加速度的约束 : 甲i =切。+ _望 少二望几 _。 T (9) g 二J一 (Q) . y q 二J 一 (q) 下+J 一 (q)y (1) (2) 式中 : J(q ) 为雅可比矩阵 ; y 为手部在直角坐 标下 的速度矢量 , 丫为手部在直角 坐标下的加 速度矢量 。 由于求雅可比矩阵及其求导 、 求逆运算极 某复杂 , 因此这种方法所需计算量很大 。 为此 本文 采用一种近似方法来考虑机器人 cP 运动 时手部有速度和加速度约束的情况 , 其条件是 连续轨迹可用参数方程表示 , 并且参数能反映 手部速度和加速度的变化 。 现设连续轨迹是以如下参数方程表示 : x= f ; ( s ) 夕= f : ( s) z二 f 3 (s) (3) (4) (5) (6) 山 、 口 、 , 、 分别表示手部与基 座参照 坐标系 X 、 Y 、 Z轴的夹角或欧拉角 。 为求出各关节的运动规律 q ,二 q , (约 ( j 二 1 , , N , N为 自由度) , 对区间0 , T中 每一时刻 t i ( = O , 1 , , m ; t 。 二O,t、二 T) , 先由式 (3)(9)求出( x 、 y 、 z; 、 a* 、 尽 、 甲 ) , 再据此计算出手部坐标 系相对基座参照坐标系的变换矩阵 T寿 i( 二 o , 1 , , m) , 然后再由T介进行运动学反解计 算求 出对应的 各关节变量值 , 由于运动学反解 的多解性 , 对每一个 T寿可 能有好几组关节变 量值与之对应 , 这时可根据各关节工作范围将 无意义解去掉 , 在剩下的 几组有效解中 , 再选 择一组与当前形态最接近的解作为各关节变矗 值 口 , 、 q* 2 、 、 g ; N 。 另外当 乙r ( =:- 才、 一、 )很小时 , 可用下述近似公式计算各关节 速度和加速度 : 叮 , 亡 (q *一召、一:, )/乙t ; = 乙叮 , /乙 t (10) = s ( t) r.ls e s e 、s e l. .I . . 其中 : X 、 y 、 z 是连续轨迹的直角坐标分量 ; : 是时间t的函数 , 它是用来反映手部速度和加 速度变化的 , 其意义随具体情况而定 , 可以是 长度或角度等 。 譬如当机器人手部沿直线运动 时 , 可取 s 为运动路径长度 。 由于机器人CP运动时 , 其手部姿势往往 也随着变化 , 为能准确描述机器人运动 , 除了 知道手部位置变化外 , 还应知道手部姿势的变 化 。 设在连续轨迹的始点和终点手部的姿势角 为 ( a 。, 尽 。, 甲 。 ) 、 ( a , , 月 r , 甲r) , 我们假 定姿势角随时 间作线性变化 , 因 此在时刻 仁 ( 0 簇 t, 镇) T 手部姿势角为 : q、 , (q 、 ,一q、 一: , )/(乙忿) “ (1 1) 式中: 乙q *s 口 ,一 g 一:,; i= 0 , 1 , , 爪 , j = 1 , , N 。 当求得在亡 一 : 和 :两点时的各关节的关节 变量值和速度值时 , 便可规划 亡; 一, t内关 节运动 , 为了减少计算量以及使规 划出的关节 运动平稳 , 在每一子 区 间艺 一: , t 内用一多 项式来拟合各关节运动 , 并近似认为在公 一 , 艺、 内机器 人作P T P运动 。 现设在t * 一, , t内 拟合关节运动 的多项式为 : q ;,“ q , (七) “a ;。+a 门t+ + ai 。 忿 ” “二 “ 。 + ” 产 : “ 二 “ 。+ 五并与 ( 7 ) 式中 艺 一 1 (1 2) 。为无量纲时间 , :二( T一 “ 一: ) / (云 - 丁一亡亩一 l 入 艺任0 , 1 , 儿、二乙玄; 二 ( 8 ) t、一艺、 一 ,; T为实际 时间, 月 . T任艺一 , t 由于区间 0 , T共被分成m个子 区间: 0 19 88年 得中间各轨迹段多项 式系数 。 第艺段多项式系数的确定( 2 镇 i ( m 一1 ) 叼 ; ( t ) = a ; 。 + a ,七+ a;:艺“+a;3才“ 由于叮 、 一: 、 口; 已知 , 而口 、一, 口, 一工又可 用 前面方法递推求出 , 因此该多项 式系数可由条 件 : q (0) =q、 一 , g (1) = q 、, q (0) = q、 一: 及 q (0) =口* 一 , 求得如下 : 了. .r百l e s ll.e e . J l e e l.w e .、 七, 亡 , , 云 2 , , t 。:一 , 才 。 , 因此对于一 个关节来说需要构造 m个多项式 。 下面我们 采 用 两种方法来规 划各关节的运动 , 一种是 3 一 3 - 3 - 一 3 一 5轨迹 规划法 , 另一种是4 一 3 一 3 - 一 3 一 4 轨迹规划法 。 2 . 13 一 3 一 3 - 。 一 3 一 5 轨迹的规划 当对各关节的运动速度无 约束时(始终点 除外) , 可用这种方法来规 划轨迹 。 这种方案 就是将最后一个轨迹段用一五次多 项式来 拟 合 , 其余各轨迹段 用三次 多项式来拟合 , 即 : 第 i 段 : q , () =a , 。 + a ,; t+ a 12+ a ,3艺“ (13) 第i段 : 口 , (。) = a,。+ a门亡+ a, :云2+a,。艺“ (1 4) 第 ,竹段: 口二(: ) =a 二。 +a 。, + a。:“+ +o茄s才3+ a二4艺+a。 t “ (一5) 由于关节在始 点 和终点的位移 、 速度 、 加速度 010= q f 一 I Q 2 =q ; 一1几 1 二一)采 一 q 一 In 了 乙 a 门=口*一口、 一; 一q; 一, h ;一 咬 一 g 一 ,九若 乙 第m段多项式系数的确定 口 二 (艺) =a 。 + a。 ,云+ a二:t“+ a 。 3艺3+ + a二、艺4+ a 。艺5 q o、 变 f q o、 值口 q o, gf 、 qf 、 q , 及各中间点的关节 q :, , q 。 一; 是已知 的 , 据此可递 由于口 二一: 递推求出 、 叮 ,。、 口 。 、 口 。 已知 , 而q 。一, 、 口 二一, 可 , 据此可得 : 推求出各轨迹段多项式 系数 。 第 1段多项式系数的确定 由 于 叮l(t)= a ,。 +a ,艺+ 。 1: 、“+ a ,。t3 ., (0) = q 二一;, q 。 (0) = q ,。 一 ;, 口 二 (0) = = q 。一, (1) 二叼r , g 。 (1) =qr , 口 , (1) = qf 爪 m q 口 了1 1 ,/、| | 1 . 口:(亡)= ( a , +Za , : 艺+3a ;。艺2 )/孔 : ; 口,(公) = (Za ,: + 6a ,: 忿) /h圣 。 由已知条件 : g : (0) 由这6个条件可求出 6 个多项式系数如下 : 口。 o = q 二一 1 q : ( 0) = q 。 , 各系数如下 : 口, (o) 三口。;g , (1) =q a, q 。衅 = q o ; 可得 阮 如 q 0 一 一 一一 i 一 2 一 几n 00 瓦 一 二 口 q 一 ,19l 口 一一 一一 20口 .占.占 a 口 . . IJ少 了、! ! l 因此 , 口 , = g , (1) , q , 二q: (1) , 再以 q l 、 q l 、 q , 和 q : 为已知参数 , 迹段多项 式 q。(t)的各系数 , 便可求得第 2 轨 以此 递推下去可 a 。 z =q 。一1几二 。二: 一三 一 石 。一 , 、点 2 1 a明 3 =。“ : 一4“ 2 + 寸 “ 3 a。 4 =7c 2 一15c l 一c 3 “ 一 6c l 一3c 2 + 一 ; 一 c 3 c l = q r一a oo 一a ol 一a o Z cZ“ Qr耘 。 一a ,。 1 一 Za 二2 c: = 召fh二一Za 。2 求出各轨迹段多项式系数后 , 关节的运 动 2卷 6期 机器人运动轨迹规划分析与算法 也就规划出来了 。 2 . 2 4 一 3 一 3一, . .一3 一 3 一 4轨迹的规 划 当对各关节在中间点时的速度有约束时 , 可采用这种 轨迹规划 法 , 此法 就是将起始段和 结束段均用一四次 多项式来拟合 , 而中间各段 用三次 多项式来拟合 。 对机器人手端运动速度 的约束 , 可 由式 ( 1) 或(10)进行速度反解 计算变换为对关节速度的 约束 , 这样通过对各 关节速度进行 限制 , 就可达到控 制机器 人手端 运动速度的 目的 。 下面利 用已知条件导出求各 轨迹段多项式系数的递推公式 。 第 1段多项式系数的确定 叮; (t) = a ,。 + a ;: t+ a : : 艺“+ a , 3才“+a: 艺峨 第 1段多项式的边界条件是 : ai。= = q ; h i一 Zq + Za ; o +a、 a 第m段多项式系数的确定 q 二 (才) = a。 。 + a。 : 云+ a。Zt“+a二。亡3+a。 艺4 该段多项式要满足 的边界条件有 : q 二 (0) = q 二一: ; q 二 (1) = q ,; a 。 (0) = a ,。一 , ; a 。 (1) = qr;口二(1) 二 q r 由这些条件可求得各系数如下 : q ; ( 一 ) =q 、 q , (1) =q , q , (0) = q 。 由此可得各系数如下 : a; n 0= q 。 一 l a。 1 = q 二一1凡二 a ,2 =6 D r一 3D : +0 . S D s a。 s =4 D 2 一SD x 一D 3 a, 4 = qr+ZD , 一D Z+ 0 . 5 D 3 D 一= q f一a二 o 一a 二l D Z = 凡 , q r a 。l D 3= 嵘 q , Q2 0= q o a 一 z = q o 九 1 1 a ,: = 该 , 口。瓦 2 a 、3 = 4c l 一c : a 24 = c Z 一3c l Cz= Q I 一Q一 。一Q 11 一Q 1 2 : = 九 1 q z 一a zl 一 2a 1 2 第i段多项 式系数的确定( 2( i簇。一 1 ) . Q i (玄) = ai。+ai ,艺+ ai:t“+ai 3t“ 该段多项式 的边界条件为 : q i (0) 二qi 一, ; q ; (1) = q i q ; (0) = q、 一 , ; q i (1) =口 由这四个条件可求得各系数如下 : 口10= q i一 1 口门 Q币么 = q卜 l h = 3叹、 一3a o 一Za i x 一q 儿 求出各关节各轨迹段多项 式系数后 , 各关 节随 时间的运动规律i q (诊i ( 二1 , , N , O岌 T ()T 也就被确定 。 2 . 3 步骤 综 上所述 , 可得机器人 CP 运动轨迹规划 算法 (CP T A)如下 : (1)输入连续轨迹方程(直 线 、 圆 、 圆 弧或椭圆等) : x = f ; ( s ) , , = f : ( s ) , z = f 3 ( s ) , s = s (七) ; (2)输入始终点的各关节速度和加速度 并给定机器人沿连续轨迹运动 时手部姿势变化 规律 :a=a (零) , 尽=尽(云) , , =中(艺); ( 3) 输入运动时间 T , 根据精度要求将 0 , T分 成l n 个子 区间 : 0 , 艺; , t , t Z , , t 二一;, 才 。 (七 。: 共T); (4)置i = 0; ( 5 )根据连续轨迹方程和手部姿势变化 规律求出时刻 t时手部坐标系相对基座参照 坐 标系的变换矩阵T么 ( 6) 由 T介进行运动学反解计算 , 求出 对应 的各关节 . 变量 值 q 1, q : , , a 。 , 再 8 年2 2 机器人 1 988 年 由式 ( 1) 或( 10 ) 计算对应的各岁亩速度 q 门 , q 2, 5 q i对; ( 7 )利用3 一 3 一 3 - 一 3 一 5 或 4 一 3 一 3 - 一 3 一 3 一 4 轨迹规划递推公式求出区间以 一 , 亡、 (艺二 1 , , m) 内各关节轨迹段多项式 系数 , (8)置艺二艺+ 1 , 如果 云毛川 , 则返回到 步骤 5 , 否则向下继 续; ( 9 )输出各关节运 动规律 。 2 . 4 CP T几的特点 本文的机器人 CP 运动轨迹规划算法具有 如下特点 : (1) 由于求轨迹段多项式系数的公式是 以递推形式给出的 , 因此在规划轨迹前 , 不必 计算出所有中间点 , 而是仅算出前面几点 , 然 后边走边算 , 递推地求出后面各段 的多项式 系 数 , 这为实时计算提供了可能性 ; ( 2 )本文算法比iL n 和 L u h等人提出的 三次样条函数方法所需计算量要少 。 因为 iL n 和L u h 的方法要求在规 划轨迹前计算出整个路 径上所有中间点 , 而且需要解一个( n 一 2 ) x (n 一 2 )的三对 角方程组 ( 。 为中 间路径点个 数) , 故当 灯 较大时 , 所需计算量也较大 。 由 于本文算法所需计算量相对较小 , 又是以递推 形式给出 , 因此很易于计算机实现 。 3 轨迹 的优化处理 及m段 的确定 . 首先我们定义 : 在满足各关节速度 、 加速 度 、 跃动( je 1 k )和力矩约束条件下 , 在运动 过程中耗时最少的关节 运动轨迹为时间最优轨 迹 。 时间最 优轨迹的规划是一个非常 一 复杂问 题 , 目前国外有不少学 者仍在致力于这个问题 的 研究 。 为求这个问 题 , 本文采用一种 与6 不同的方法 : 设机器人完成CP运 动所需时问为 T , 与前 万 ,i才 斗 !同 , 将时间区间 O , 了 分成 。; 段 : 艺 。, t , , t , t : , , t 二一, t价, 运用前面 算法C P TA 一可规划 出 区间艺 、 一 , 才 内各关节 的运动规律 : 口i(t) (i 二1 , 2 , , m ; j = l , 2 , , N) 。 现设各关节速度 、 加 速度 、 跃 动 、 力矩变化的最大极限值分别 为 : V . ;口 .么 、 a,m :二 、 J 5m .二 、。 :二 (j = =1, 2 , , N) 为使规划的轨迹有效 , q i 和关节力矩: ;i须 满 足 : 叮 , 毛V *二 ;二 q , ( a i。 。二 1口 ; s】 落J j二 ,二 i=1 . , 2 , , ,、 1 :s 簇Q s二 。二 夕 = 1 , 2 , , N 下面再设法求出在区间 , 一 , , 内N个关节 中速度 、 加速度和跃动的最大值 , 对于用4 一 3 - 3 - . . 一 3 一 3 一 4或3一3 一 3 - 一 3 一 3 一 5方法规划轨 迹 时 , 除第 1段和第m段外 , 中间各段都是 3 次 多项式 , 因 此若设在第i区间才 一, , 玄内第 j关节的最大速度 、 最大加速度 、 最大跃动和 最大力矩分别为q s口 。:、 q ,。 :、 q, 。 。 二 丁s二 : , 则 q 11口 “ 二 m a x 】 口;i m ax q ; ( t) 1 = s (云 一, ) 式中 t任t* 一 , 2 , , N ; t i = 0 0 】 q ;, (云)】 , q , (“ ) (16) t, 艺=1 , 2 , 一 , m ; 了 “ 1 , 任t 一, , t、 , 且满足 口s(t ) 由于在第 2 至第m 一 1段区间中 , 关节轨 迹是 3 次多项式 , 所以关节加速度是时间 t 的 线性函数 , 故 q i萝。 :二 = m ax !q 二 m ax lq (t) l = = (t 一 , )! , ! q ;, ( , )! (1 7) 式中 : t 任t : 一、 , t: 。 由于跃动是加速度的变化速率 , 故有 : 二 . _ q ; ( 、 ) 一q , (“ 一 , )1 _ 甘f了m a二 一1一 - - 一 百了 一一 一l歹 1几 f (几 = t、一艺 一: )(1 8) 机器人关节力矩 可 根据动力学方程求出 , 而机 器人 动力学方程可 表示成 : 2 卷 6 期机器人运动轨迹规划分析与算法 T = D ( q ) q + H ( q , q)+G (q) 式中了为N 火 l广义关节力矩(力)矢量;D(动 为N 又 N 惯性矩阵 ; Hq ( , c 1 ) 是 N 冰 1哥氏力 和离心力矢量 , G(动是N x l 重力矢量 。 由于 机器人动力学方程是非线性和强藕合的 , 因此 关节最大力矩 : 。 :二 的求解十分复杂 , 本文不作 具体讨论 。 现假设第 艺轨迹段第了 关节的最大力 矩为: i。 。: , 并令 : = 平尹 Xq“ 。 二/V , 口 二 = m aX口sm 。: / as。 。: (19) (20) 6 3 1= m ax q 犷户 d 、 = = m ax 了 ; Y矛 m。 : /J m。 : /Q 争口 a盆 j口 扭 盆 (21) (2 2) i二z , 2 , , m ; j 二 1 , 2 , , N 若时 间区间以 ; 一工, 亡, 间隔 儿;(=云,一 公; 一 , )变化成 dh ; , 则关节速度 、 加速度 、 跃 动将分别增大或缩小工/J , l /d “, l / d 名倍” , 因 此 , 令 : d ;“ m aX d , , d;宁 , d;宁 , d ; 、 (乞= = 1 , 2 , 二。”, )(23) 得到 d ; 后 , 用新 的时 间区间(d , h , d Z孔2, , d扭启 取代原时 间 区 间仇 1, 阮2 , , 儿n ) z , 得到新 的时 一间区 间后 , 再用前面算法 CP T A重新规划轨迹 , 如此反复进行 直 至 d , (i 之 l , 2 , , m) 近似等于 1为止 。 d 、 刘1意 味着机器人在第 i 轨迹段 内关节运动或关节力 矩已接近极限值 , 也即 目标函数 T= 艺 h 已 接近最小值 , 此时的关节轨迹就是时 间最优轨 迹 。 本文求时 间最优轨迹 的方法 与文 献 6中 从 n和 L u h的方法不同之处在于 : 文献6中每 一区间大小的变化会导 致其它所有区间都发生 变化 , 而本文中每一区间大小的变化则是相对 独立的 , 不影响其 它区间大小的变化 。 譬如假 设在区间艺 一 l, 嚣、 中 , 关节夕的最大 速度 a ,i。 。二 大于V i。 。: , 则几=q i。 a二 /V ,二 。二 l 1 , 文献 6 的处理方法是将所有子 区间都扩 大人倍 , 时间区间变成以执 , 久h Z, , 久风 , 而实际上除t 一 , ; 外 , 其它区 间不一定需 要变化 , 这样处理的结果往往导致规划 出的关 节轨迹并不是时间最优轨迹 。 而本文处理方法 则是 : 仅仅将区间t 一 , t ; 间 隔h 扩大成 从 , 其它区间不变化 , 因 此新 的 时间区间 就 是执 2, h: , , 入瓦 , 儿 , * , , 儿二, 这样 处理易于使规划出的轨迹更 逼近于 时间最优轨 迹 。 时间最优轨迹规划算法可概述如下 : 步骤1 : 输入各种已知参数 , 并假设运动 时间区间为h :, h Z, , h , ; 步骤 2 : 利用第 2 节 c PTA算法规划 各关 节 轨迹 ; 步骤 3 : 利用 公式( 16)(2 3)确定d 、 (艺= = 1 , 2 , , m) ; 步骤 4 : 用区l b Jd内 : , d声 :, , d声 , : 取代原区f l Ah :, h Z, , h ” , ; 步骤 5 : 如果6 , 侣 1“ = 1 , 2 , , ” ; ) , 则进行步骤6 , 否则 返回到步骤2 ; 步骤 6 : 输出时间最优关节运动规律 。 下面再讨论分段数邢的确定问题 。 为确定 m , 我们可 采用如下近似方法 : 设机器人运动 时轨迹误差最大允许值为 。 。, 先任意取既为一值 , 并将时 间区间0 , T 分成。段 , 根据连续轨迹方程和手部姿势变化 规律可相应得到l n + 1 个路径点在直角坐标下 的位置和 姿势矢量 : p 。, p ;, , 氏 , p ;= 。 , 夕* , z、,a, 月 , 印), 通过运)Z 学 反解计算可得到。 + 1 组关节变量矢量 : q 。, a , , 。 q = (al , 。玉 , , 。介), 至 此可 利 用如下算法来最后 确定m : 步骤 1 : 利用 CP TA 算法规划各关节在 各 子区间t 一; , t 一 l “ 的 轨迹 : q i 。 ) (乞 = 1 , 2 , “ , 爪 ; 夕=1 步骤 2 : 步骤 3 : , 2 , 置艺二 N) ; 瓦=0 ; 置 t 1 二 不 _ , 十刃 ; 少: Z 24 机器人 1 9 as 年 步骤 4 : 计算 q “ = = Q , (t 。 ) , 口 2(t : ) , , q 对 (艺 。 ); 步骤 5 : 用 q“ 进行运动学计算 , 得到直 角坐标下相应 的路径点位置 与姿势矢量 p 。 *; 步骤 6 : 计算 p 。 与实际运动路径之间误 差 6* ; 步骤 7 : 如果d * 簇d 。, 则进行步 骤 8 ; 否 则 , 在 p 一 ; 与 p ; 之 间增加一中 间点 p 。 ; , 并 笠 : p 。 “ (X( 艺 。 ) , Y(才 。 、 ) , Z(艺 。 ) ,a (艺 。 ) , 尽(t“ ) , 切(t 。 ) ) , 瓦= =丸+ 1; 步骤 8 : 如果 艺m , 置 艺二不+1 , 返回到 步骤 3 ; 否 则向下继续进行 , 步骤 9 : 置m = 爪+ k , 并输出m值 。 4 结束语 通过前面分析可知 : 与 Li n 和 L uh 等人方 法相比 , 本文算法不仅易于计算机实现 , 而且 计算量 也较少 。 参考文献 1 Cr aigJJ . In计od u etion 七o r o bo ti es: m eehan i e s an d eontr O I . Ma ssa e hu, t ts , A d dis on 一We sl e y P u bl ih si ng Co皿p a ny , 10 56 2 K ahn ME , Ro thB . The n e a r一mini mum tim e e ont ro l o f open f o opa r tie ula t e d kie nma ti ee ha - 10 5 . A SME , T r an s 丁 our na l o fDy n a血e Sys t - e皿s, e M a s u e rme nt,an d Co n o t r l , 1。了1,。 3(3): 16 4一172 3 P a u lRP . M姐ipulao t r Ca rte城an path IE EET r ansa etion scn Sys e t ms , M an , e o n trol - an d Cy - be r n etie s, l。了。; SMC 一。 ( 2 1) : 702一7一1 4 Taylo r R H . Pla nn i nga n d exee u桩ono f s t r砚- ghti l ne 口 an ip u l a o t r tr ajeeto过e s . IBMJo u r nal o fRe努 a c rh an d D e讹O I pm e ot,xo7。;2 3:42一连36 5 Ki mBK , Shi n KG . A ne fici e nt minim u垃- i tm ero bot pa thpla nningun d e rr eai l s tie eo n i d - 桩o ns . In : Pr o e o f 10 5 4 Am ei re an C ontr o lCo n - f ee rn ee, Sa n Dieg , 10 54:2 0 。一5 03 6 L in CS , Ch a ng PR , a n dL u hJYS 。 F o r m - u lai t o nan d o p七imtz aito no f e u bi e polyn omial ji o nt tra j ectoi res f o r indu stal 。ro b os t . I EEE Tra n sa etionon A u tomatieCo ntro l , x日5 3;A C - 2 8(12) : 工066一107 3 7 L u hJ YS , L i n CS . APp rox im ate j o i nt tra - j e eto rie s f o reo nt ro l o fi n du stria l ro bo t sa l o ng C a re t 溢an p aths . IEEET r an s Sy ,te田, M an a n dCvbe r ne tie s, 2 9 54; SMC 一 24(3) : 4 44一4 5 0 5 L u hJ YS , Li n CS , optim u m pa thpla nn i ng fo r m eehanie al m a nipu la t or s . AS M E Tra n s Jo u rn a l o fDy n amie Sv stems, Me a su e rm e nt, a n dC on t ro f , 10 5 1;102 : 24 2一1 51 。 , C ha n dS , DotyKL . 0 皿一iln e polyn o 血 a l “ tr a - j eet i o r es fo r o rb ot m a刀ipu l毗