RRT法的冗余机械手路径规划(快速扩展随机树)

( f ( qd + e) - f ( qd ) )
(5)
记
λ j
为
J
( qd
+ e) J T ( qd
+ e)的最小特征值 ,
λ k
为
K的最小特征值 ,
因此
λ j
> 0,λk
> 0,式
( 5)变为
V ( e,
t)
≤
-
λλ kf
‖f
(
Байду номын сангаас
qd
+ e)
-
f ( qd ) ‖2
-
qTd J T ( qd ) ( f ( qd + e) - f ( qd ) )
零 。当采用计算机控制时 , 速度项可表示为单位时
间里角度增量 , 此时控制算法 q ( t) = KJ T ( pd - p ) 可表示为 Δq ( t) = KJ TΔpΔt, 其中 , Δt为采 样周
期 。注意到该算法要求期望关节角速度均可到达 ,
但是由于关节角度自身的限制以及障碍物的存在 ,
(6)
由于 qd 有界 , ‖qd ‖·‖J ( qd ) ‖ <M ,式 ( 6)变为
V ( e,
t)
≤
-
λλ kf
‖f
(
qd
+ e)
-
f ( qd ) ‖2
+
M ‖ ( f ( qd + e) - f ( qd ) ‖
(7)
从式
( 7) 可看出 ,
如果
λ k
足够大
,
可保证
V ( e, t)负定 , 由稳定性判定准则可知 , e ( t)趋向于
记关节角度跟踪误差为 e ( t) = q ( t) - qd ( t) , 对 e ( t)两边求导得到 : e(k) = q(k) - qd (k) = KJ T (q) (f (qd ) - f (q) ) - qd ( t) = - KJ T ( qd + e) ( f ( qd + e) - f ( qd ) ) - qd ( t) ( 3)
1 引 言
实时环境中冗余多关节机械手的路径规划问题 是机器人智能控制领域中一项重要课题 。针对这一 问题 , 研究学者提出了各种规划算法 , 如概率路标 图法 ( PRM ) [ 124 ] , 快速搜索随机树法 ( RRT) [ 527 ] ,人 工势场法 [ 829 ] ,单元分解法 [ 10 ] ,拓扑降维法 等 。 [ 11212 ]
2009年 11月 第 16卷第 6期
控制工程 Control Engineering of China
Nov. 2 0 0 9 Vol. 16, No. 6
文章编号 : 167127848 (2009) 0620756204
基于雅可比转秩 2RRT法的冗余机械手路径规划
周 芳 1 , 朱齐丹 1 , 张新福 2 , 吴叶斌 1
3 雅可比转秩控制算法
记机械手关节角度为 q = [ q1 , …, q6 ]T , 末端工 具 A 点的位置和姿态表示为 p ( t) , 由运动学得知 p ( t)可表示成 q ( t)的函数 :
p ( t) = f ( q ( t) )
(1)
关节速度与末端工具 A 点的运动速度可表示为
p ( t) = J q ( t) 。设跟踪期望轨迹为 pd ( t)且有界 。
由于核反应检修机械手结构上的特殊性 , 使其 具有冗余机械手的特性 。在实际规划中 , 目标点在
工作空间内给定 , 为解决目标点在位形空间内的不 确定性 , 提出利用雅可比转秩控制算法与 RRT结 合起来对该冗余机械手进行路径规划 , 并对机械手 进行仿真研究 。
2 机械手模型描述与碰撞检测
本文所描述的核反应堆检修用机械手共有 6个 关节 , 全部为旋转关节 , 如图 1所示 。
1) 机械手碰撞检测 为了方便机械手的碰撞 检测 , 需要对机械手的结构描述作某种简化 。将机 械手的各关节视为圆柱杆件 , 该杆件可简化为两端 为半径为 R 的球 , 中间为半径为 R 的圆柱体 。由 于各杆件在 1 /4的球形空间内运动 , 只需要对圆柱 两端点进行检测 。同时 , 只针对可能发生碰撞的关 节进行检测 , 如 AC 段和 FG段 , 把两关节之间的 碰撞检测转化为两圆柱体之间的最短距离来求解 。
(1. 哈尔滨工程大学 自动化学院 , 黑龙江 哈尔滨 150001; 2. 海军后勤技术装备研究所 , 北京 100072)
摘 要 : 针对核反应堆检修用机械手在末端任务给定的无碰撞路径规划问题进行研究 。 提出一种将雅可比转秩控制算法与快速搜索随机树法相结合的混合算法 , 通过选择距目标点 工作空间距离最近的位姿点作为树的扩充节点 , 避免对逆运动学进行求解 。利用雅可比转秩 控制算法计算出最优扩充方向 , 采用二分梯度下降扩充方法对末端工具速度加以限制 , 避免 关节速度发生突变 。仿真结果表明该混合算法的快速性和有效性 。 关 键 词 : 冗余机械手 ; 快速搜索随机树 ; 路径规划 ; 工作空间 ; 雅可比转秩 ; 逆运动学 中图分类号 : TP 27 文献标识码 : A
2) 运动学求解 由于核反应堆检修用机械手 的工作需要 , 使得机械手末端安装了偏心工具 , 而 且工作时 , 末端姿态 3个旋转自由度中有一个自由 度可任意 , 这使得该机械手的运动学逆解问题成为 冗余机械手逆解问题 。针对这类问题 , 一般采用数 值优化 [16 ]的方法进行搜索可行解 , 但是需要消耗 大量的搜索时间 。
人工势场法由于其算法简单易实现等特点 , 一 般应用于局部路径规划 。单元分解法主要用于低维 空间内的路径规划 。而基于采样技术的搜索算法 ( PRM , RRT) 能很好地解决高维空间内搜索问题 。 PRM[ 3 ]算法主要用于静态环境下搜索问题 , 预先构 建路标图 , 在实时规划中通过路标图的搜索获取一 条可行路径 。RRT[ 6 ]从初始点出发产生搜索树 , 在 位形空间内随机采样位姿点进行树的扩充 , 直到该 搜索树连接到目标点为止 。
从上述算法可看出 , 在规划前需要在关节空间 中预先给定目标点 。在实际中 , 目标点通常在工作 空间中给定 , 此时需要进行逆运动学解算 。但对于 多关节机械手而言 , 逆解求取非常复杂且存在不惟 一性 , 如本文所研究的机械手即为该情况 。同时基 本 RRT算法在选择树的节点时采用的是随机选取 的方式 , 这种方式导致树的扩充较慢 。
使得关节速度不能朝着任意方向变化 。为解决这一
问题 , 本文对 A 点运动速度以及关节速度加以限
制 , 利用二分法选择 A 点移动距离 , 并判断关节速
度是否可行 。
4 基于雅可比转秩的 RRT算法
本文在 RRT算法的基础上进行改进 , 并用于 对六自由度机械手进行路径规划 。RRT算法的基本 原理可描述如下 :
Abstract: The p roblem of collision2free path p lanning for nuclear reactor repairing manipulator with the given end path is considered. A hybrid algorithm of combining the transpose of Jacobian control algorithm w ith rap idly2exp loring random tree method is p resented. The nearest configuration point to the workspace w ith goal point is taken as the expanded node of the tree to avoid the solving of inverse kine2 matics. The best expanding direction is calculated w ith the jacobian transpose control algorithm. And the velocity of the end tool of the manipulator is restricted by using the bisection gradient2decent extend algorithm to avoid the occurrence of joint velocity mutation. The simulation results show the rap idity and validity of the p roposed hybrid algorithm. Key words: redundant manipulator; rap idly2exp loring random tree; path p lanning; workspace; the transpose of Jacobian; inverse ki2 nem a tic s
定义如下 Lyapunov函数 V ( e, t) :
V ( e,
t)
=
1 2
( f ( qd
+ e)
-
f ( qd ) ) T ( f ( qd
+ e)
-
f ( qd ) )
(4)
当 e≠0时 , V ( e, t) > 0, 并对 V ( e, t)求微分得
到:
V ( e, t) = - ( f ( qd + e) - qTd J T ( qd ) ( f ( qd + e) f ( qd ) ) - f ( qd ) ) T J ( qd + e) KJ T ( qd + e) ×
Path Planning App roach for Redundant Manipulator Based on Jacobian Transpose2RRT A lgorithm
ZHOU Fang1 , ZHU Q i2dan1 , ZHAN G X in2fu2 , WU Ye2bin1
( 1. School of Automation, Harbin Engineering University, Harbin 150001, China; 2. Navy Logistic Technology and Equipment Institute, Beijing 100072, China)
雅可比转秩序控制算法 ,如图 2所示 。
图 2 雅可比转秩控制算法
F ig12 The a lgor ithm of the tran spose of the Jacob ian
考虑图 2中控制算法 , 随着控制增益 K增大 , q ( t)逐渐收敛到期望值 qd ( t) 。
根据图 2所示控制算法有下式成立 : q ( k) = KJ T ( pd - p) = KJ T ( q) ( f ( qd ) - f ( q) ) ( 2)
第 6期 周 芳等 : 基于雅可比转秩 2RRT法的冗余机械手路径规划
·757·
其中 , 6个关节的中心点分别为 B , C, D, E, F, G。
当机械手 各 关 节 均 伸 长 时 将 达 到 球 形 半 径 的 115倍 , 这将导致机械手在 1 /4 球形内始终以一种 收缩弯曲的姿态运动 (见图 1) 。本文将机械手的碰 撞检测分为 , 机械手与环境间碰撞检测与各关节间 互碰检测 。
首先从初始点 qinit出发产生搜索树 T ree, 并将 其作为搜索树的父节点 。其次 , 以一定的概率 pr 在未访问的位形空间内随机采样位姿点 qrand作为搜 索树的下一扩充节点 。并从搜索树中选择距 qrand最 近的节点 qnear。最后 , 从 qrand处朝着 qnear的方向选 择新节点 qnew作为 qrand的邻接节点 。如果两点之间 存在障碍物 , 则停止本次扩充 , 进入下一次随机选 点过程 , 直到该搜索树连接到目标位姿点为止 。
图 1 机械手运动姿态示意图 F ig11 The m otion posture f igure of man ipula tor
收稿日期 : 2009206217; 收修定稿日期 : 2009207220 基金项目 : 省级自然科学基金资助项目 (60875025) ; 哈尔滨工程大学校基金资助项目 (002040260746) 作者简介 : 周 芳 (19822) , 男 , 江西南昌人 , 博士 , 主要研究方向为机器人与智能控制等 ; 朱齐丹 (19632) , 男 , 教授 , 博士生导师 。