2010年 8月 农 机 化 研 究 第 8期 基 于 遗 传 算 法 的 采 摘 机 器 人 轨 迹 规 划 陈天 宏’，崔 天 时 ，李 广 军。 (1．东 北 农业 大学 工 程 学 院 ，哈 尔滨 150030；2．尚 志 市 电业 局 ，黑 龙 江 尚 志 1 50600) 摘 要 ： 以总距 离最 短 作 为 性 能 指 标 ，以 6 自由度 采 摘 机 器 人 作 为 研 究 对 象 ，在 考 虑 各 关 节 速 度 、加 速 度 和 加 速 度 变 化 约束 的基 础 上 ，采 用 遗 传 算 法 对 机 器 人 所 经 过 各 点 的 进 行 路 径 规 划 。运 用 robotics toolbox工 具 箱 进 行 仿 真 ，结 果 表 明 ：所设 计 的机 器 人 轨 迹规 划 方 法 既 能 保 证 总路 径 最 优 ，又可 使 机 器 人 的轨 迹平 滑 。 关 键 词 ：采 摘 机 器 人 ；轨 迹 规 划 ；robotics t~lbox；遗 传 算 法 中 图分 类 号 ：TP242；$126 文献 标 识 码 ：A 文章 编 号 ：1003—188X(2010)08—0031—04 0 引言 1 经 过各路径 点 的坐标 随着 新 农业 生 产 模 式 和新 技 术 的 应 用 ，机 器 人 将 果 实 的识 别 和 定 位 是 果 实 采 摘 机 器 人 的难 点 ，同 越来 越 多 地 应 用 到 农 业 生 产 中 。农 业 机 器 人 在 工 作 时 识别 和 定 位 的准 确 性 关 系 到 采 摘 机 器 人 工 作 效 率 。 时环 境 复 杂 ，需 要 实 时 的 判 断 果 实 的 位 置 。 另 外 ，农 实 现机 器 人 对 果 实 采 摘 的关 键 是 要 获 得 果 实 在 空 间 业 机 器人 的作 业一 般 是 在 移 动 的 同 时 进 行 ，其 作 业 路 的 准确 位 置 ，从 而 为 机 器 人 提 供 空 间 位 置 参 数 。 目 径 不 是连 接 出 发点 和 终 点 的最 短距 离 ，而是 具 有 狭 窄 前 ，双 目视觉 的识 别 和定 位 取得 的效 果是 比较 好 的 。 的范 围 、较 长 的距 离 及 遍 及 整 个 lit问 表 面 等 特 点 。为 本 文采 用 的是 针 孔 成 像 模 型 ，各 坐 标 系 转 化 关 系 提 高 其 采 摘 的 效 率 ，应 该 对 其 进 行 路 径 规 划 ，找 到 最 如 图 1所示 。 的最优，又要保证作业的路径最短，因此需采用合理 l L—．J L…．J 1． ．1 ． I 薹 ．．一 周 ．网 ．网 的方法对机器人的作业轨迹进行规划 ，使机器人按照 优 化 的轨 迹 进 行 。一 条 优 化 的轨 迹 是 指 在 满 足 机 器 l 标 l I 抓系 l l—I6枷 系l l 杯系I 图I 坐标系转换关系图 人动力学和运动学约束 的前提下 ，使机器人 的运 动时 Fig ． 1 The figure of coordinate t fn lat．0n relation． 间最短的轨迹 。最优路 径是指如何调整各路 径段 ，使 根据图 1可得 总 的路 径最 短 ，并 满 足 速 度 、加 速 度 、加 速 度 变 化 和力 矩约束。 厂1 ] ， r“] I d tt0 IFf 0 0] 1 ， l 方法 爻 制 1 f1 L0 1。0J I= 方法只能优化机器人各关节的轨迹曲线，并不能缩短 L j l d， l I l 机器 人 的工 作 时 问 。为 了提 高机 器 人 的 工 作 效 率 ，很 L0 0 1 J 多人将神经 网络 和模 糊控 制等算 法应用 到轨迹 规划 rX ] 其 算 法复 杂 ，计算量大。如果采用遗传算法对机器人 进行最 优轨迹规 划 ，通 过计算机来 进行决 策和选择 ， 会使离线规划变得简单 ，且总路径是最短 的… 。 基金项目：东北农业大学博士肩动基金(2o07—2oo9) 作者简介：陈天宏(’ 4_)，男，黑龙江依兰人，硕 {：研究生·(E—mai¨ ethl9841102@ l63．eom l u u ILI I z I L1．j ㈩ 式 中 一 正交 旋转 矩 阵 ； 卜 平 移 向 量 ； ／ 焦 距 ； X yt ， Z．．一 摄 像 机 坐标 系的 坐标 ； ’ ’ ’’… ’ ’ ～ … … 通讯作者：崔天时(1967一)，男，哈尔滨人，副教授，硕士， 导师，(E— D y_一像 坐 标 系 。 mail)tse@fleau．edu 图像 OUV坐标 系 以像 素 为单 位 ，设 成 像 平 面 坐 标 

2010年 8月 农 机 化 研 究 第 8期 图像 OUV坐标 系 以像 素 为单 位 ，设 成 像 平 面坐 标 个 随 机路 径 。利 用 rt×N矩 阵 存储 凡个 随 机 群体 产 生 系 的原 点 0在 “一 坐 标 系 中 的坐 标 为 (Ⅱ。， 。)，X轴 初 始 群体 。 和 y轴方 向上单 位 物理 尺 寸上 的像 素 数 目为 d 和 d 。 2．2 路 径点 位 置距 离 和适 应 度 函数 的确 定 如果 标定 Ⅳ个 点 ，采 用 奇 异值 分 析 的方 法 求 得 最 节点 的位 置为 编译 前 指定 ，也 可 以使 用 随 机 生 成 小二 乘 解 ，就 可得 到 摄 像 机 的 内部 参 数 、外 部 参 数 和 的坐标 参数 。距离 矩 阵 使 用 一 个 N×N 矩 阵 D存 储 ， 其他 参 数 。 图 2 双 目模 型 Fig．2 Configuration of two cameras D(i一1，i)代表 节 点 i一1和节 点 之 间 的距 离 。 D( 一1，i)=√( — —1) 一(Y —Y —1) (2) 在该 问 题 的 求 解 中 ，用 距 离 的 总 和 来 衡 量 适 应 度 ，距离 的总和越大 ，适应度越小 ，进而探讨求解结果 是否最优。每条路径距离 总合计算公式 ，即其适应度 函数 为 n—t ∑ ^／／( — ) 一()， —Y ) (3) 利 用 适 应 度 函 数 选 择 个 体 ，将 较 小 路 径 (适 应 度 较 大 )个 体选 择 下来 。 2．3 交叉 交叉操作可以得到新一代个体 ，新个体组合 了父 辈 个体 的特性 。将 群 体 内 的各 个 个 体 随 机 搭 配 成对 ， 对 每一 个个 体 ，以某 一 概 率 交换 他 们 之 间 的部 分 染 色 体 。本 文 中交 叉采 用 部 分 匹 配 交叉 策 略 ，其 基 本 实 现 根据 图 2可 得到 的 步骤 是 ： “l一 “o —=—一， l一 o —：__ C1 Cl a 2( l—b) a),2Y 1 “2 一 0 ————=————一 ’ 2 一 o —=—一 。 c1 ‘cI 1)随 机选 取两 个 交叉 点 ； 2)将 两交 叉点 中间 的基 因段互 换 ； 3)将 互 换 的基 因 段 以 外 的 部 分 中 与 互换 后 基 因 段 中元 素 冲突 的用 另 一 父 代 的相 应 位 置 代 替 ，直 到 没 式 中 a a a 0记分 别 是 摄 像 机 内 部 参 数 。 有 冲突 。 (“。， ，)和 (“ ， )为 P点 的 图像 坐标 。由上 面 4个 式 2．4 变异 子 可求 得点 P 的三 维 坐 标 ，然 后 根 据 下 式 就 可 到 点 P 变异操作通过随机改变个体 中某些个体的某些基 的世界 坐标 ，即[至]= ： 。 y‘．． Z 。 1 。 由此 就 可 得 到所 经 过点 的坐标 ，然 后进 行轨 迹 规划 J。 2 最优 轨迹规划 因而 产 生 新 个 体 ，有 助 于增 加 种 群 的多 样 性 ，避 免 早 熟 收敛 (非 全 局最 优 )。本 文 中 变 异 操 作 使 用 互 换 操 作 算子 ，即随 机交 换 染 色 体 中两 个 不 同基 因编 码 的 位 置 ，对 于 一个 11路径 的染色 体 的变 异操 作 ，依 变 异 概 率 确定 是否 变 异 ，随机 选 择 路 径 上 的两 个 路 径 点 进 行 交换 。 2．5 群体 的 跟新 和 终止 条件 最 优 路径 轨迹 规 划是 指在 机 器人 的起点 和终 点 间 离散 出一 些 中 间点 。对 两 点 问 的 路 径 进 行 优 化 ，使 得 末 端 执行 器从 起 点 到终 点 的访 问路 程 最 短 ，且 要 考 虑 末 端 执行 器 在 运 动 过 程 中速 度 和 加 速 度 变 化 的 物 理 约束。本文采用遗传算法对路径进行规划。 为保证计 算 随迭代 次 数 的增加 ，最优 解越 来 越 好 。本文 种 每 次 变 异 后 将 每 次 的 上 次 迭 代 最 优 解 加 入群 体 ，防 止 因 交叉 或 变 异 而 使 最 优 解 失 去 ，出现 退 化现 象 。为保 持 群体 数 目不 变 ，变 异后 产 生 随机 解 加 人 群 体 。最 常用 终止 条 件 ：一 是 事 先 给定 一 个 最 大进 化步数 ；二是判断最优化值是否连续—杆 ：步没有 明显 2．1 路 径点 编 码与 随机 和初 始 群体 生成 变 化 。本 文 只选择 了第一 种 方法 H J。 给所有路径点编码 ，以路径的次序作为遗传算法 的编码 。在 MATLAB中使 用 randperm(Ⅳ)产 生 一个 1 3 结 果 分 析 及 仿 真 实 验 ×N 的矩 阵 (』】v为 所 有节 点 的个 数 ，本例 中为 11)为 一 设路 径 点 坐 标 分 别 为 1(0．08，0．3)；2(0．09， 

2010年 8月 农 机 化 研 究 第 8期 0．29)；3(0．13，0．28)；4(0．15，0．26)；5(0．15， 0．25)；6(0．18，0．23)；7(0．21，0．19)；8(0．22， 0．13)；9(0．20，0．14)；10(0．23，0．16)；1 1(0．24， 0．151． 运行 后 得 到结 果 如 图 3所 示 ，最 优 路径 R为 1，3， 4，6，7，10，l1，8 9，5，2，最 短 总距 离 为 0．4734。 采用 D—H方 法 在 每一 个 连 杆 的初 始 端 建 立 附体 坐 标 系 ，并 根 据所 建 立 的连 杆 坐标 系 可 以 确 定 连 杆 坐 标参 数 。为 了求解 运 动 学 方 程 式 ，应 用 齐 次 变 换 矩 阵 图4 运动仿真初始图 i一1 T来描 述 第 i坐标 系相 对 于第 i坐 标 系 相对 于 i一 Fig．4 The picture of initial motion simulation 1坐标 系 的位 置 和方 位 。将 各 连 杆 变换 矩 阵相 乘 就 得 到 了末端 执 行 器 坐 标 系 相 对 于 基 坐 标 系 的 齐 次 变 换 阵 。该 矩 阵 是 运 动 学 正 解 方 程 描 述 了末 端 执 行 器 在 基 坐标 系 中 的 位 置 和 姿 态 。将 运 动 方 程 式 的 两 端 依 次 左乘 各 矩 阵 的逆 矩 阵 ，并 使 两 端 相 等矩 阵 的 对 应 元 素相 等 ，即可求 得 各 关 节变 量 ，得 到 运 动学 逆 解 。 0 叭 ∞ 0 0 叫 0 图 5 运 动 到 1点 的 三 维 图 Fig．5 Th e three—dimension image of robot at point 1 图 3 轨 迹 规 划 Fig．3 The figure of trajectory planning 本 文 采用 了点 到点 运 动 的 轨迹 规 划 。设 它 的起 始 点 为 A(0，0，0)，终 点是 标 定 求 得 的 物 体 三 维 坐 标 。 运 动 到 1点 完 成 一定 任 务后 ，再 将 1点 视 为起 始点 ，运 动 到 3点完 成 了预定 工 作任 务 ；然后 ，又将 3点 视 为 起 始点 ，依规 划 的路 径 继 续 运 动 。这 里 ，对 于 形 如 A 点 到 1点 、1点 到 3点 的 运 动 ，它 们 之 间 不存 在 任 何 的 中 问点 。运 用 Matlab软 件 中 的 robotics toolbox工 具 箱 ， 建立 机 械 手 的三 维 仿 真 图 。 图 4是 初 始 位 置 仿 真 图 ， 6 末 端 关 节 的 运 动 轨 迹 Fig．6 The trajectory of the last link 4 结 论 1)本 文 以最 优路 径 作 为性 能指 标 ，并 在 满足 各 种 约 束 的 条件 下 进行 机 器 人 轨 迹 规 划 ，旨在 使 机 器 人 末 端 执 行器 在 两 工 作 点 之 间 或 沿 规 定 轨 迹 运 动 的路 径 图 5是 运 动到 1点 时 的仿 真 图 ，图 6是从 A点 到 1点 最 短 。 的坐 标 变化 图 ，图 7是 各 关 节 变 量 变 化 曲线 图 。这 里 2)通 过 遗 传算 法 路 径进 行 优 化 ，并 利用 机 器 人工 只给 出 了运 动 到 1点 的仿 真 图 ，并 通 过 控 制 滑 块 驱 动 具 箱进 行 仿 真 ，对优 化 的结 果进 行 验 证 。 机 械 手 运 动 ，运 动 情 况 满 足 预 定 要 求 ，从 而 验 证 了连 3)仿 真结果表明 ，该 规划算 法是 可行的 ，并且与 杆 坐 标 系 的正确 性 。在 运 动 过 程 中 ，各 关 节 角 连 续 变 传统 算 法相 比有 着 计 算 量 小 和 效 率 高 的优 点 ，为 后 续 化 ，末 端 执行 器 可 以实 现 预 期 的运 动 一 。 实 际采 摘实 验 做 理论 指 导 。 · 33 · 

2010年 8月 农 机 化 研 究 第 8期 程 jP ：lI< 搬 IP ：II< 毂 秘 1997：52—60． 装 备技 术 ，2006(1)：8—1o． [2] 马颂 德 ，张正 友．计 算 机 视觉 [M]．北 京 ：科技 出版 社 ， [3] 刘小 勇．番茄收获机械手 机构 分析及 双 目定位 系统 的研 究 [D]．哈尔 滨 ：东北农 业大学 ，2006． [4] 李东 洁 ，邱江艳 ，尤波 ．一种 机器人 轨迹 规划 的优化 算法 [J]．电机 与控制学报 ，2009(1)：123—127． [5] 任敬轶 ，孙汉旭 ．一种 新颖 的笛 卡 尔空 间轨 迹 规划 方法 [J]．机器 人 ，2002，24(3)：217—221． [6] 孙 村栋 ，曲彦宾．遗传算 法在 机器 人路径 规划 中的应用 研 [J]．西北工业 大学学报 ，1998(2)：79—83． 时 间 ／s [7] 孟庆鑫 ，王晓东 ．机器人技 术基础 [M]．哈尔 滨 ：哈尔滨 肇 13 05 1 0 1．5 20 时间／ s 图 7 关 节 变 量 变 化 曲 线 工业大学 出版 社 ，2006(9)：52—66． Fig．7 The curves of joint variables [8] 罗家佳 ，胡 国清 ．基 于 MATLAB的机 器人 运动仿 真 研究 参考 文献 ： [J]．厦 门大学学报 ，2005(5)：640—644． [9] Corke P I．A robotics toolbox for MATLAB[J]．IEEERobot· [1] 姜丽萍 ，陈树人．果实采摘 机器人 的研究综 述 [J]．农业 ics and Automation Magazine，1996，3(1)：24—32． Path Planning of Fruit Picking Robot Based on G enetic Algorithm s Chen Tianhong ，Cui Tianshi ，Li Guangjun。 (1．College of Engineering，Northeast Agricultural University，Harbin 1 50030，China；2．Shangzhi Electric Power Bu— reau，Shangzhi 1 50600，China) Abstract：Taking the shortest distance as the performance index，6一DOF fruit picking robot as an object of research， the various points of the paths that robot passed are planned with genetic algorithm in this paper．The trajectory planning was established considering the joint velocity，acceleration and acceleration restriction．The results indicate that with ro— botics toolbox，the design of robot trajectory planning method not only ensures the path is optimal，but also makes the trajectory of robot smooth． Key words：picking robot；path planning；robotics toolbox；genetic algorithms (上 接第 30页 ) Abstract ID：1003—188x(2010)08-0028一EA RPROP to the Prediction of W ater Table of Sanjiang Plain — — Based on the M ethod of BP Nerve Net Guo W ei，Zhao Jie，Wang Fulin (Engineering College，Northeast Agricultural University，Harbin 1 50030，China) Abstract：Due to the importance of the prediction of the water table of Sanjiang Plain，the thesis introduces the algorithm concept and mathematics model of the BP nerve net of RPROP in detail，and instructs the prediction model of the flexible BP nerve net of RPROP according to the specific conditions of the wter table of Sanjing Plain．Th e thesis also makes a specific prediction and analysis taking a well as the sample． The results prove the accuracy and effectiveness of the approach to the prediction of the buried depth of water table of the Sanjiang Plain and also prove the application prospect of this theory and approach to the dynamic prediction of the water table． Key words：observation of the water table；buried dept of the wate table；BP nerve net；RPROP；Sanjiang Plain · 34 ·