移动机器人基于激光测距和单目视觉的室内同时定位和地图构建

1、 930 . 自 动 化 学 报 , 31 卷 性 基 于 陆 标 的拓 扑地 图 的 优 点在 于 其 只 要求 维持 地 图 全 局 拓 扑 的 一 致性 而 避 免 了 几 何 地 图 保证 一 致 性 的 问 题 但 其 缺点 是 由 于 定位 空 间 的 离散 化而 无 法 保 证 定 位 的 精 度 栅 格 地 图 容 易创 建和 维 护 但提 高这 种 方法 的 分辨 率会 导 致 计 算复 杂 性 的 迅速 增 加 使 其在 自 主 移动机 器 人上 进行 实 时 处 理 变得 困 难 侧 s r r t r d n 由于 本 文 进 行 实 验 的 走 廊 和 办 公 室 为

2、 部 分 结 构 化 室 内 环 境 ( 而 t u c u e i d o e S e 。 t n 所 以 在 S L A M 的 地 图构 建 研 究 中 以 几 何 地 图构建 方 法为 主 这 样 表 达简 单 vr n i 节省存 储 空 间 具 体 构建 步 骤 包 括基 于 激 光 和 视 觉 传感 器进 行 的 环 境特 征 提取 线段 特征 的 拟 合 合并 及 匹 配 激 光构 建 的二 维平 面 地 图 与从 视 觉 传感 器 提 取 的 墙 角和 门的垂 直边 缘 可在 特 征 层 次 上 进 一 步融 合 成功 匹 配 的 高 级 环 境 特 征 可 视 为 自然 陆

3、标 并 可 作为 地 图 中拓 扑节 点 的 选取 依 据 这 种在 局 部 采 用 儿 何地 图 表 示 全 局 使用 拓 扑表 示 的 地 图 构 建方 法 在 实 际 应用 中具 有 更 好 的 实用 性和 鲁 棒性 4 2 全 局 线 段 特 征在 机 器人 观 测坐 标 下 的 表 示 。 丙 。 全 局 地 图 中 的 水 平 线 段特 征 向量 可 表 示 为 L 二 ( Q T 而 位 姿 下 x ( 的 测量 方 程 哟 * , h ( x L 依 据 文 献 中介 绍 的 转 换 模 型 局 部 地 图 中 的 线 段 特 征 向量可 表 述 为 为 0 . . , , .

4、 、 , , , . 、 . , , . , . . , , , . , : L 。 ! 一 , ( ! , a ! 一 “无 ( X , L 一 ( g “ 一 v任 不石 与 百c o s 口 ( 一a g 一 0 , 1 ( 5 。, 其 中 p 和 p 分 别 为全 局 坐 标 原 点和 局 部 坐 标 原 点 到 特征 线 段 的垂 直 距 离 。 分 别 为 p 和 p 与 各 自坐 标 系 横 轴 的交 角 上 述方 程 可 近 似 线性 的 表 示 为 . 而 和 山 则 l L 二 从X 1 I J + L g 1 (6 其中 二 万1 2 一 今 L U 。s H12 0

5、0 一1 a 。 , _ 幻g , 1 一 1 L口g 丙 1 。 _ ll 一 eo s 口 ( 一 , a , 一 翌s in ( 一 口 r ta n a 。 一矍 : 口 ( 一 。 n 口 + 三 i ( 一 , , 一 、 厄 孕不 石 口一 。r e (, z 二 的协 方 差矩 阵 为 二 几 , 一 洲 周川 . T + 气 , , 1 ( 7 1 公 式 ( 7 中 p 为 当 前位 姿 的 协 方差 矩 阵 与 全 局 地 图 中水平 线段 特 征 的转 换 相 似 全 局 。 地 图 中垂直 边 缘 特 征 角 沪 也 能相 应 转 换 为机 器 人 坐 标 系 下 的

6、 特 征 角 沪 从 而 为 E K F 定 位 做好 技术 准 备 . 4 3 扩 展 卡 尔 里滤 波 定位 由 于 采 用 弧 线建 模 的 里 程 计 模 型 无 法 进 行 近 似 的 线性 表示 所 以 本 文在 机 器 人 自主定 位 中采 用 E K F 算 法 E K F 是 迭 代调 用 算 法 并 且 只 有 新 息才 被 当 前计 算所 使 用 所 以 该 算 法 不 需 要大 量 的 数 据存 储 且 易 于 实 现 E K F 算法 包括 如 下 4 个 步 骤 lk 1 * 2 状 态 预 测 系 统 状态 的 一 步 预 测 x 解 l 及 其 协 方差 尸 解

7、 可 由前 述 公式 ( 和 ( 3 计 算得 到 测 量 预 测 全 局 地 图 的 线 段 特 征 可 由 4 2 节 所介 绍 方法 转换 到 机器 人 坐 标 系 下 以 完成 对观 测 的预 估 匹 配 全 局 线 段 特征 在 当 前 位 姿 下 进 行 坐 标转 换 得 到 的 线段 特征 与局 部 地 图 中 通 过 测 到 的 线 段 特 征 构成 一组 特 征 对 如果 其 满 足 M a a a o is 准 则 则 可 以 认 为其 是 最 优 量得 h ln b 的 匹配 一 占 占 ( 石T s ( 石 < x 己n ( (1 5 . , . , , . :

8、. . . . . . , , . 其 中 占L 二 L 一 元 而 , s一 是 新 息协 方差 矩 阵 . 6 期 : 庄 严等 : 开 移 动机 器 人 基 于 激 光测 距 和 单 目视觉 的室 内 同时 定位 和 地 图 构 建 , , , 93 1 ll ,。 * 状 态 更 新 特 征 对 的 下 标记 为 仁 = 1 M 并且 x k + , I p k * 测 量 预 估 与 观 测 间 的 新 息 协 方 差 矩 阵为 + , ll ,。 、 二 x k + 1 ; , p k + 二 : & + 1, = 城p 、+ 1 一, , 、 可 、 + 几 , , +

9、几 : , (1 9 新 息协 方差 具体 包括 机器 人 位 姿预 测 的 协 方 差 全 局 线段 特征 协 方 差 和 机 器 人 观 测 噪 声 三 部分 机器 人 迭 代 位姿 更 新 公式 和 位 姿协 方差 更 新公 式 为 . 瓜 X + 1 , 一 , 几+ = 1 一, 1 * 一 , 、+ 1 1 、 1 , X + 、 一 、 11 : , 可暇 + 一 1, 2 (0 , K 、+ + 1 , i L ( , , 一 L , 2 ( 1 2 ( 2 凡+ = 几 , + 11 1 * 一 凡 & + 1 + : K 1 , 经 过成 功 匹 配 M 组 线 段

10、对 后 可 使 用 上面 公式 去 更 新机 器 人 位 姿 及 其协 方 差 1 1 : 1 * : 1 . 环 结 束 时 可 得 到 x 解 解 = x k * M 凡 十 + 一 八 + M + , , . 在循 , , . 4 4 sL A M 算法 的 具 体 实 现 . 算 法 在机 器 人 导 航 控 制 软 件 的体 系 结 构 中 处 于 核 心 地 位 当 激 光 测 距 仪 和 单 目视 觉 的 数 据 经过 预 处 理 特 征提 取 和 数 据 融合 后 水平 线 段 和 垂 直 边 缘 这 样 的初 级 特 征 以 及 墙 角 门等 高 级 特 征被 转 换 到 全

11、 局 坐 标 系 下 以 预 定 义 的 抽 象 对 象 形 式 存储 于 全 局 地 图 中 当 机 器 人 进 行定 位 时 全 局 地 图 中的 特 征 再转 换 到 当 前机 器 人 所 在 坐 标 系下 通过 1 M a a a o i 准 则 将 地 图 中 的 特 征 与 机 器 人 当 前 观 测 的特 征进 行 匹 配 处 理 利 用 公式 ( 5 h ln b s 进 行 特 征对 的 匹 配时 从 激 光 传感 器 数 据 中提取 的 特 定平 面 线 段 特征 和 从 视 觉 数 据 中 获取 的 垂 直 边 缘 特征 分别进 行 匹 配 匹 配 成功 的 特 征 对

12、所提 供 的 位 姿校 正 量 被 送 入机 器 人 位 姿及 速 度控 制 模块进 行 更 新 、 L S AM 、 , . , , . “ ” , . “ ” . 多 传感 器信 息在 特征 层 次 上 匹 配 所得 到 的 高级 环 境特 征 被作 为 自然 陆标 使 用 自然 陆 标的 确 定 已 经包 含 了激 光 和 视 觉 传感 器 的 特 征 匹配 和 信 息 融 合 而 且 自然 陆标相 对 于 普 通 线段 特 征更 易 于 辨 识 ( 如 走 廊 中房 门 的宽 度 基 本 相 同 而 且 分 布也有 一 定 的 规 律 可 寻 例 所 以 我 们在 实 际 应 用 中利

13、 用 自然 陆标进 行 二 次 定位 这 样经 过 普 通 线段 特 征 定 位的机 器 人 即 便 因 为动 态 噪 声 的 干 扰 而发 生 误 定 位 仍然 可 以 根 据 自然 陆 标 进 行 再次 定 位 校 正 避 免 了不 可 恢 复 的 定 位 失 败 从 而 保 证 导 航 策 略 的 正 确 进 行 地 图 构 建 和 基 于 E K F 的 定位 算 r 法在 s m a tR O B 2 移 动机 器人 平 台 所 使用 的 X O / 2 嵌入 式 实 时操 作 系 统 中 以 实 时 任 务 的 形 式周 期 进行 调 用 由 于 机 器 人 视 角 等 因 素 的

14、制 约 基 于 自然 陆 标 定 位 的 频 率要 低 于 基 于 普 通 线段 特 征 定 位 的 频率 , . , . , , , , . 一 . , . 5 实验 结 果 S . o c PI t r l a 统 该 系 统 采 用 C m p a t C 工 业 标准 由 M o o o M P C 8 4 单 一 中 央处 理器 控 制 使 2 0 2 强 实时 嵌入 式 操 作 系 统 t R O B 2 机 器 人 系 统 配 备 有激 光 Sm a r 用x o / 视觉 等 多 种 传感 , , . a m t r R OB 2 一 移 动机器 人 是 从 瑞士 引 进 的一

15、 种 功 能 强 大 的 多 用 途 中型 嵌 入 式 机 器 人系 一 、 器 , 并具 有完 整 的 控制 软 件和 开 发 工 具 从 而 为开 展 自主 机 器 人 研 究提 供 了 适 宜 的平 台和 . , 有力 的技术 支 持 5 1 M 实验结 果 自主 移 动机器 人 S L A M 实验 的 实 际 环 境 包括 办 公 室 走 廊 和 门 厅 几 部 分 其 中走 廊 为 实 验 的 主 体 环 境 在 S L A M 实验 中 我 们可 通 过 基 于 网络 的 控 制 界 面 ( 图 3 对 机 器 人 的 见 S LA 、 , . , 93 2 自 动 化 学 、

16、报 31 卷 图 3 基 于 网络的 控制 界 面 Fig . 3 b W七 ba s e d e o n so le in t er a f ee 地 图 构 建 定 位和 导 航 进 行 实 时 的监 控 和 结 果 分 S L A M 实验 的 析 随着 机器 人 位 姿 的不 断 更 新 局 部 地 图 视 觉场 景 效果 同 时 反 映 在 软 件界 面 的 n 和 全 局 地 图 中 图 4 是 机 器 人 在 27 宽 的 走廊 区 S L A M 实验 结 果 域 的 一组 图 中 机 器人 位 姿 用 圆 圈 来 表示 圆圈 中短 线 为机 器 人 运 动方 向 图 4 中

17、相邻 两 网 格 点 间 距 为 1 二 在 机器 人 运 动 环 境 中有 墙 壁 开 或 关 闭 的 门 以 及 角 落 等 不 同环 境 特 征 随 着 机器 人 的 不 断运 动 走 廊 环境 的 全 局 地 图 被 逐渐 构 建 出 来 机 器 人 在 完 全 未 知 的环 境 中根 据 提取 的 特 征 同 时 进 行 自主 定 位 定 位 周 期 为 1 5 , 实验 中机器 人 速 度 为 0 2 m / . , 、 . . , . . 、 . , . , . . . . 图 Fig . 4 SL A M ex 实验 结 果 e ri 图 lt F ig . S SL A M

18、o e il ro 实验 中机 器 人位 姿误 差 曲线 o o se eo 4 SL A M p m e t n re s u 5 M b b t 5 p r ee o ti n in x , , , theta 5 2 . 数 据 分析 与 结 果 讨论 图 5 . 为 S L A M 实 验 中机 器 人 位 姿误 差 曲线 横 轴 为 运 动过 程 中 定 位次 数 纵 轴为 机 器 x 二 , 试网 格 间距 代 表 Z m 和 0 网 格 间 距 人位姿 ( c 0 的 校 正 量 从 上 到 下 分别 为 ( x o c 代表 2 在 多 次 S L A M 实 验 的 定 位

19、过 程 中 和 夕 的 校正 量 保 持 在 士s m 之 内 0 的 校正 o 量 在 士4 之 内 由 图 5 可 见 机器 人 局 部 位 姿 有 些 时刻 校 正 量 较大 这 主 要 基 于 下述 原 因 , , , , 、 . , , . , : , 较 大 是 因 为 机 器 人 由 静 止 到 运 动对 控制 器 有较 大 冲击 造 成 的 2 地 l 启动 阶段 ; 面 状 况 的 局 部 不 平整 也 会造 成位 姿 误差 的 突增 3 当 机 器 人 运 动 中 转 向时 受 非 完 整 约 束 x , 0 的 校 正 量 同 时增 大 ; 4 移动 机器 人 平 台 点

20、 镇 定控 制 器控 制 算法 的 影 响 机 器 人 位 姿 x x 由于 机 器 人 运 动 中 以 沿 方 向运 行 为主 所 以 位 姿 在 方 向 的 误差 要 明 显 大 于 , 方 向的 误差值 x 、 ; , , , , , . 在 图 6 中 随 机 给 出机 器 人 在 不 同 位 姿 下 的 几 组 误差 椭 圆 如 图 所 示 外 面 的误 差 椭 k 圆 代表 机 器 人 经 过 一 步 状 态 预 测 后 的 状 态 协 方 差 矩 阵 p ( + 1 / 劝 里 面 的误 差 椭 圆 为 经 过 E K F 状 态 更新 后 的 状 态 协 方 差 矩 阵 尸 (

21、 十 1 / k 十 1 在 第 二 组误 差 椭 圆 中 被 E K F 更 新 后 k 的 误差 椭 圆 显 著变 小 表 明机 器 人 自定 位 使 得 位 姿不 确 定 性 得 到 改 善 其 余 几 组 误 差 椭 圆 , . , , . , , . 的 变 化不 是 很 明显 这 是 由于 l 机器 人 运 动 速 度 有 限 而 定 位 频 率 相 对 较 快 这 样在 短 ; 距 离 直 线运 动 中 根 据 里程 计 模型 进 行 的一 步 状 态 估 计 值本 身 误差 就 有 限 2 在 E K F 定 5 m 的 特 征 进行 匹 配 位 算 法 中 只 选 用长 度

22、大 于 0 这 虽 提高 了 定 位可 靠 性 但 也 导 致 机 器 人 在 某 些 位置 可 用 于 定位 的 环 境 特 征 减 少 所 以 校正 效 果 不 明 显 , , : , , . , , , . 6 期 庄 严等 : 开 移动 机器 人 基 于 激 光测距 和单 目视觉 的室 内 同 时 定位 和 地 图 构建 933 P ( + 无 1/ k + l 图 F ig 6 . 6 k 机器 人 在 尸 ( b il e ro 十 se / l 幻和 e rr o r e 尸( + k l ip se / l k + k ( o M b o t,5 p o in p + 1 /k

23、a l 下 的 位 姿误 差 椭圆 n d p k + z 无+ ( / i 6 结论 . 本 文 的研 究 重 点在 于 如何 从 激 光 和 视 觉 传 感 器 获 取 的 数 据 中高效 率 的 提 取 环 境 特 征 并 合理 的 进 行几 何地 图 构建 本文 提 出 的 机 器 人 同 时 定 位 和 地 图 构 建算 法 已 应 用 于 全 自主 移动机 器 人 在 部 分 结构 化 室 内环 境 下的 导 航 运 动 中 实 验表 明 该 s L A M 算法 可 以 实时 完 成 机 器 人 的 全 局 地 图构 建 和 即 时 精 确 定 位 任务 并 且 易 于 在 嵌

24、入 式 移 动 机器 人 平 台 上 实 现 如 何 进 一 步 提 高 地 图 构 建 和 定 位 的 精 度 并 消 除 实际 环 境 中 动态 物 体 对 定 位 和 地 图 构 建 的影 响 是下 一 步 研 究 工 作 的 重 点 . , . . 1 LI L , e Y , . T , T , a n M : , Ch e n X J 476、480 . Pr e s u e t n st t a e n a d f tu u r e o d e v l Pm e e t n o f m o b il e t ro b o t te e hn o o l y g r es e r

25、a io e h . R 2 A O bo 亡 2 00 2 24 ( 5 u rr 留 K o 0 , , n I R 3 In : bo ro e 乙c j s n a o T , m a is N e t t d A o , t e s e J n f lt P C 留t e l l 工EE E u P C h r is t e n e d in g s n e s f In t N i 即二 er n a o . H I A e t iv e t io n a M lt i s s y m u 吕 te l a . se , o s n0 1 20 n . r o , - i n t

26、h e f y lo 34 2 ( a 一 ea l z a io i t n : p r e e is i o n an d re l i 3 1 3 1 、1 4 3 : b a , il t y f i o r e t P 8 P li a ns l g o ba l l o e l a iz t1 lC o n ee fr u t t e c A r i五 i l I n e a r n o o f r e o m l ig l a e b il . ro b e n e e r s S u to e t s g o u in kh l m o eo u s , u m S e 4 5

27、 G in t i v a ll o u ns ao JA s o 乃a a t j n c J N eb , e ra J R o , 丁 r do 乞 s J D M u lt is e t ns t o E r o n o , b o tj s c t an d A e D u . rr a n t W hy ro e . . H F Si . m o a 云 n 沁 o s n 200 1 , u f s io , n o f im lt a n a n o l e w e lt iP l h yP o h e is t r a k 1 t s C n g . d e n 1999 a

28、 n . 13、22 l a iz a t io n d n m a P b u il in g d . 1 7 6 : ( 9 0 8 、9 1 4 6 7 8 9 P t e d 罗 t n llig n A u e t t X v n i m n e o s as m i u S nyd in 6i aVlis iie e ir o n ( 0 a5 8 1 、5e8l8 r s e e o se tr e r se a n n n s J e n sf l P e t g t m m 朗k in g u in g la s s n a d Ch i t 朋 n H 1 P v n e

29、t o m nl m d a tj 七 f s R o b o 幻。 d A u忿 2 0 0 1 1 7 ( : 1 3 8 、 1 4 7 2 汪沼 石 刃阴 刀 朋 m n o s aon r e c 吕 n o e r o o t o e z o n c a n n s o o r ra n d P i i l t ge A r r a K O T m a is N I m Pr o v n g r b u s t i s o s g O t n o i m b il b l a i a i 妙 u in l映 s e n eu a r s o n i f n d in g a n d

30、 m o l i In : P r o e e d in g 日 o f T hir E u r P e a n 、b r k 、 s P o n A d v a n e e d M o b ile R o b o t i v d o o n o h Z u r ie h S w it z e r l d 1 9 9 9 1 7 7 、 1 8 5 n a n n o e ro o o r e s o a P d r n a X J W 么 g Y F Z h a g Y M厄 g W M b il b t m a b u i ld in g b 助e n im P o d a g l h i

31、 t g m u n P e v n e u a e m et d l d b o t 2 0 0 3 2 5 7 : 6 8 0 、6 8 5 r n a o h ( t s a 叫 O R r se o r n o r o 叭/ n g a i f r m a i n P r o e e s i n g in m o t o s e i s W H C H E N W D X l Y G U n c ta n n P a b u ild in f m o il o t g b e b td o o r . e n e In : in o u m fI t i e a n eo u s lo e l t a 返 a io o n o n d m P a b . u il in g d n ee , u s in g t a u