移動機(jī)器人基于激光測距和單目視覺的室內(nèi)同時定位和地圖構(gòu)建

1、 930 . 自 動 化 學(xué) 報(bào) , 31 卷 性 基 于 陸 標(biāo) 的拓 撲地 圖 的 優(yōu) 點(diǎn)在 于 其 只 要求 維持 地 圖 全 局 拓 撲 的 一 致性 而 避 免 了 幾 何 地 圖 保證 一 致 性 的 問 題 但 其 缺點(diǎn) 是 由 于 定位 空 間 的 離散 化而 無 法 保 證 定 位 的 精 度 柵 格 地 圖 容 易創(chuàng) 建和 維 護(hù) 但提 高這 種 方法 的 分辨 率會 導(dǎo) 致 計(jì) 算復(fù) 雜 性 的 迅速 增 加 使 其在 自 主 移動機(jī) 器 人上 進(jìn)行 實(shí) 時 處 理 變得 困 難 側(cè) s r r t r d n 由于 本 文 進(jìn) 行 實(shí) 驗(yàn) 的 走 廊 和 辦 公 室 為

2、 部 分 結(jié) 構(gòu) 化 室 內(nèi) 環(huán) 境 ( 而 t u c u e i d o e S e 。 t n 所 以 在 S L A M 的 地 圖構(gòu) 建 研 究 中 以 幾 何 地 圖構(gòu)建 方 法為 主 這 樣 表 達(dá)簡 單 vr n i 節(jié)省存 儲 空 間 具 體 構(gòu)建 步 驟 包 括基 于 激 光 和 視 覺 傳感 器進(jìn) 行 的 環(huán) 境特 征 提取 線段 特征 的 擬 合 合并 及 匹 配 激 光構(gòu) 建 的二 維平 面 地 圖 與從 視 覺 傳感 器 提 取 的 墻 角和 門的垂 直邊 緣 可在 特 征 層 次 上 進(jìn) 一 步融 合 成功 匹 配 的 高 級 環(huán) 境 特 征 可 視 為 自然 陸

3、標(biāo) 并 可 作為 地 圖 中拓 撲節(jié) 點(diǎn) 的 選取 依 據(jù) 這 種在 局 部 采 用 兒 何地 圖 表 示 全 局 使用 拓 撲表 示 的 地 圖 構(gòu) 建方 法 在 實(shí) 際 應(yīng)用 中具 有 更 好 的 實(shí)用 性和 魯 棒性 4 2 全 局 線 段 特 征在 機(jī) 器人 觀 測坐 標(biāo) 下 的 表 示 。 丙 。 全 局 地 圖 中 的 水 平 線 段特 征 向量 可 表 示 為 L 二 ( Q T 而 位 姿 下 x ( 的 測量 方 程 喲 * , h ( x L 依 據(jù) 文 獻(xiàn) 中介 紹 的 轉(zhuǎn) 換 模 型 局 部 地 圖 中 的 線 段 特 征 向量可 表 述 為 為 0 . . , , .

4、 、 , , , . 、 . , , . , . . , , , . , : L 。 ! 一 , ( ! , a ! 一 “無 ( X , L 一 ( g “ 一 v任 不石 與 百c o s 口 ( 一a g 一 0 , 1 ( 5 。, 其 中 p 和 p 分 別 為全 局 坐 標(biāo) 原 點(diǎn)和 局 部 坐 標(biāo) 原 點(diǎn) 到 特征 線 段 的垂 直 距 離 。 分 別 為 p 和 p 與 各 自坐 標(biāo) 系 橫 軸 的交 角 上 述方 程 可 近 似 線性 的 表 示 為 . 而 和 山 則 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 二 的協(xié) 方 差矩 陣 為 二 幾 , 一 洲 周川 . T + 氣 , , 1 ( 7 1 公 式 ( 7 中 p 為 當(dāng) 前位 姿 的 協(xié) 方差 矩 陣 與 全 局 地 圖 中水平 線段 特 征 的轉(zhuǎn) 換 相 似 全 局 。 地 圖 中垂直 邊 緣 特 征 角 滬 也 能相 應(yīng) 轉(zhuǎn) 換 為機(jī) 器 人 坐 標(biāo) 系 下 的

6、 特 征 角 滬 從 而 為 E K F 定 位 做好 技術(shù) 準(zhǔn) 備 . 4 3 擴(kuò) 展 卡 爾 里濾 波 定位 由 于 采 用 弧 線建 模 的 里 程 計(jì) 模 型 無 法 進(jìn) 行 近 似 的 線性 表示 所 以 本 文在 機(jī) 器 人 自主定 位 中采 用 E K F 算 法 E K F 是 迭 代調(diào) 用 算 法 并 且 只 有 新 息才 被 當(dāng) 前計(jì) 算所 使 用 所 以 該 算 法 不 需 要大 量 的 數(shù) 據(jù)存 儲 且 易 于 實(shí) 現(xiàn) E K F 算法 包括 如 下 4 個 步 驟 lk 1 * 2 狀 態(tài) 預(yù) 測 系 統(tǒng) 狀態(tài) 的 一 步 預(yù) 測 x 解 l 及 其 協(xié) 方差 尸 解

7、 可 由前 述 公式 ( 和 ( 3 計(jì) 算得 到 測 量 預(yù) 測 全 局 地 圖 的 線 段 特 征 可 由 4 2 節(jié) 所介 紹 方法 轉(zhuǎn)換 到 機(jī)器 人 坐 標(biāo) 系 下 以 完成 對觀 測 的預(yù) 估 匹 配 全 局 線 段 特征 在 當(dāng) 前 位 姿 下 進(jìn) 行 坐 標(biāo)轉(zhuǎn) 換 得 到 的 線段 特征 與局 部 地 圖 中 通 過 測 到 的 線 段 特 征 構(gòu)成 一組 特 征 對 如果 其 滿 足 M a a a o is 準(zhǔn) 則 則 可 以 認(rèn) 為其 是 最 優(yōu) 量得 h ln b 的 匹配 一 占 占 ( 石T s ( 石 < x 己n ( (1 5 . , . , , . :

8、. . . . . . , , . 其 中 占L 二 L 一 元 而 , s一 是 新 息協(xié) 方差 矩 陣 . 6 期 : 莊 嚴(yán)等 : 開 移 動機(jī) 器 人 基 于 激 光測 距 和 單 目視覺 的室 內(nèi) 同時 定位 和 地 圖 構(gòu) 建 , , , 93 1 ll ,。 * 狀 態(tài) 更 新 特 征 對 的 下 標(biāo)記 為 仁 = 1 M 并且 x k + , I p k * 測 量 預(yù) 估 與 觀 測 間 的 新 息 協(xié) 方 差 矩 陣為 + , ll ,。 、 二 x k + 1 ; , p k + 二 : & + 1, = 城p 、+ 1 一, , 、 可 、 + 幾 , , +

9、幾 : , (1 9 新 息協(xié) 方差 具體 包括 機(jī)器 人 位 姿預(yù) 測 的 協(xié) 方 差 全 局 線段 特征 協(xié) 方 差 和 機(jī) 器 人 觀 測 噪 聲 三 部分 機(jī)器 人 迭 代 位姿 更 新 公式 和 位 姿協(xié) 方差 更 新公 式 為 . 瓜 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 , 經(jīng) 過成 功 匹 配 M 組 線 段

10、對 后 可 使 用 上面 公式 去 更 新機(jī) 器 人 位 姿 及 其協(xié) 方 差 1 1 : 1 * : 1 . 環(huán) 結(jié) 束 時 可 得 到 x 解 解 = x k * M 凡 十 + 一 八 + M + , , . 在循 , , . 4 4 sL A M 算法 的 具 體 實(shí) 現(xiàn) . 算 法 在機(jī) 器 人 導(dǎo) 航 控 制 軟 件 的體 系 結(jié) 構(gòu) 中 處 于 核 心 地 位 當(dāng) 激 光 測 距 儀 和 單 目視 覺 的 數(shù) 據(jù) 經(jīng)過 預(yù) 處 理 特 征提 取 和 數(shù) 據(jù) 融合 后 水平 線 段 和 垂 直 邊 緣 這 樣 的初 級 特 征 以 及 墻 角 門等 高 級 特 征被 轉(zhuǎn) 換 到 全

11、 局 坐 標(biāo) 系 下 以 預(yù) 定 義 的 抽 象 對 象 形 式 存儲 于 全 局 地 圖 中 當(dāng) 機(jī) 器 人 進(jìn) 行定 位 時 全 局 地 圖 中的 特 征 再轉(zhuǎn) 換 到 當(dāng) 前機(jī) 器 人 所 在 坐 標(biāo) 系下 通過 1 M a a a o i 準(zhǔn) 則 將 地 圖 中 的 特 征 與 機(jī) 器 人 當(dāng) 前 觀 測 的特 征進(jìn) 行 匹 配 處 理 利 用 公式 ( 5 h ln b s 進(jìn) 行 特 征對 的 匹 配時 從 激 光 傳感 器 數(shù) 據(jù) 中提取 的 特 定平 面 線 段 特征 和 從 視 覺 數(shù) 據(jù) 中 獲取 的 垂 直 邊 緣 特征 分別進(jìn) 行 匹 配 匹 配 成功 的 特 征 對

12、所提 供 的 位 姿校 正 量 被 送 入機(jī) 器 人 位 姿及 速 度控 制 模塊進(jìn) 行 更 新 、 L S AM 、 , . , , . “ ” , . “ ” . 多 傳感 器信 息在 特征 層 次 上 匹 配 所得 到 的 高級 環(huán) 境特 征 被作 為 自然 陸標(biāo) 使 用 自然 陸 標(biāo)的 確 定 已 經(jīng)包 含 了激 光 和 視 覺 傳感 器 的 特 征 匹配 和 信 息 融 合 而 且 自然 陸標(biāo)相 對 于 普 通 線段 特 征更 易 于 辨 識 ( 如 走 廊 中房 門 的寬 度 基 本 相 同 而 且 分 布也有 一 定 的 規(guī) 律 可 尋 例 所 以 我 們在 實(shí) 際 應(yīng) 用 中利

13、 用 自然 陸標(biāo)進(jìn) 行 二 次 定位 這 樣經(jīng) 過 普 通 線段 特 征 定 位的機(jī) 器 人 即 便 因 為動 態(tài) 噪 聲 的 干 擾 而發(fā) 生 誤 定 位 仍然 可 以 根 據(jù) 自然 陸 標(biāo) 進(jìn) 行 再次 定 位 校 正 避 免 了不 可 恢 復(fù) 的 定 位 失 敗 從 而 保 證 導(dǎo) 航 策 略 的 正 確 進(jìn) 行 地 圖 構(gòu) 建 和 基 于 E K F 的 定位 算 r 法在 s m a tR O B 2 移 動機(jī) 器人 平 臺 所 使用 的 X O / 2 嵌入 式 實(shí) 時操 作 系 統(tǒng) 中 以 實(shí) 時 任 務(wù) 的 形 式周 期 進(jìn)行 調(diào) 用 由 于 機(jī) 器 人 視 角 等 因 素 的

14、制 約 基 于 自然 陸 標(biāo) 定 位 的 頻 率要 低 于 基 于 普 通 線段 特 征 定 位 的 頻率 , . , . , , , , . 一 . , . 5 實(shí)驗(yàn) 結(jié) 果 S . o c PI t r l a 統(tǒng) 該 系 統(tǒng) 采 用 C m p a t C 工 業(yè) 標(biāo)準(zhǔn) 由 M o o o M P C 8 4 單 一 中 央處 理器 控 制 使 2 0 2 強(qiáng) 實(shí)時 嵌入 式 操 作 系 統(tǒng) t R O B 2 機(jī) 器 人 系 統(tǒng) 配 備 有激 光 Sm a r 用x o / 視覺 等 多 種 傳感 , , . a m t r R OB 2 一 移 動機(jī)器 人 是 從 瑞士 引 進(jìn) 的一

15、 種 功 能 強(qiáng) 大 的 多 用 途 中型 嵌 入 式 機(jī) 器 人系 一 、 器 , 并具 有完 整 的 控制 軟 件和 開 發(fā) 工 具 從 而 為開 展 自主 機(jī) 器 人 研 究提 供 了 適 宜 的平 臺和 . , 有力 的技術(shù) 支 持 5 1 M 實(shí)驗(yàn)結(jié) 果 自主 移 動機(jī)器 人 S L A M 實(shí)驗(yàn) 的 實(shí) 際 環(huán) 境 包括 辦 公 室 走 廊 和 門 廳 幾 部 分 其 中走 廊 為 實(shí) 驗(yàn) 的 主 體 環(huán) 境 在 S L A M 實(shí)驗(yàn) 中 我 們可 通 過 基 于 網(wǎng)絡(luò) 的 控 制 界 面 ( 圖 3 對 機(jī) 器 人 的 見 S LA 、 , . , 93 2 自 動 化 學(xué) 、

16、報(bào) 31 卷 圖 3 基 于 網(wǎng)絡(luò)的 控制 界 面 Fig . 3 b W七 ba s e d e o n so le in t er a f ee 地 圖 構(gòu) 建 定 位和 導(dǎo) 航 進(jìn) 行 實(shí) 時 的監(jiān) 控 和 結(jié) 果 分 S L A M 實(shí)驗(yàn) 的 析 隨著 機(jī)器 人 位 姿 的不 斷 更 新 局 部 地 圖 視 覺場 景 效果 同 時 反 映 在 軟 件界 面 的 n 和 全 局 地 圖 中 圖 4 是 機(jī) 器 人 在 27 寬 的 走廊 區(qū) S L A M 實(shí)驗(yàn) 結(jié) 果 域 的 一組 圖 中 機(jī) 器人 位 姿 用 圓 圈 來 表示 圓圈 中短 線 為機(jī) 器 人 運(yùn) 動方 向 圖 4 中

17、相鄰 兩 網(wǎng) 格 點(diǎn) 間 距 為 1 二 在 機(jī)器 人 運(yùn) 動 環(huán) 境 中有 墻 壁 開 或 關(guān) 閉 的 門 以 及 角 落 等 不 同環(huán) 境 特 征 隨 著 機(jī)器 人 的 不 斷運(yùn) 動 走 廊 環(huán)境 的 全 局 地 圖 被 逐漸 構(gòu) 建 出 來 機(jī) 器 人 在 完 全 未 知 的環(huán) 境 中根 據(jù) 提取 的 特 征 同 時 進(jìn) 行 自主 定 位 定 位 周 期 為 1 5 , 實(shí)驗(yàn) 中機(jī)器 人 速 度 為 0 2 m / . , 、 . . , . . 、 . , . , . . . . 圖 Fig . 4 SL A M ex 實(shí)驗(yàn) 結(jié) 果 e ri 圖 lt F ig . S SL A M

18、o e il ro 實(shí)驗(yàn) 中機(jī) 器 人位 姿誤 差 曲線 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 . 數(shù) 據(jù) 分析 與 結(jié) 果 討論 圖 5 . 為 S L A M 實(shí) 驗(yàn) 中機(jī) 器 人 位 姿誤 差 曲線 橫 軸 為 運(yùn) 動過 程 中 定 位次 數(shù) 縱 軸為 機(jī) 器 x 二 , 試網(wǎng) 格 間距 代 表 Z m 和 0 網(wǎng) 格 間 距 人位姿 ( c 0 的 校 正 量 從 上 到 下 分別 為 ( x o c 代表 2 在 多 次 S L A M 實(shí) 驗(yàn) 的 定 位

19、過 程 中 和 夕 的 校正 量 保 持 在 士s m 之 內(nèi) 0 的 校正 o 量 在 士4 之 內(nèi) 由 圖 5 可 見 機(jī)器 人 局 部 位 姿 有 些 時刻 校 正 量 較大 這 主 要 基 于 下述 原 因 , , , , 、 . , , . , : , 較 大 是 因 為 機(jī) 器 人 由 靜 止 到 運(yùn) 動對 控制 器 有較 大 沖擊 造 成 的 2 地 l 啟動 階段 ; 面 狀 況 的 局 部 不 平整 也 會造 成位 姿 誤差 的 突增 3 當(dāng) 機(jī) 器 人 運(yùn) 動 中 轉(zhuǎn) 向時 受 非 完 整 約 束 x , 0 的 校 正 量 同 時增 大 ; 4 移動 機(jī)器 人 平 臺 點(diǎn)

20、 鎮(zhèn) 定控 制 器控 制 算法 的 影 響 機(jī) 器 人 位 姿 x x 由于 機(jī) 器 人 運(yùn) 動 中 以 沿 方 向運(yùn) 行 為主 所 以 位 姿 在 方 向 的 誤差 要 明 顯 大 于 , 方 向的 誤差值 x 、 ; , , , , , . 在 圖 6 中 隨 機(jī) 給 出機(jī) 器 人 在 不 同 位 姿 下 的 幾 組 誤差 橢 圓 如 圖 所 示 外 面 的誤 差 橢 k 圓 代表 機(jī) 器 人 經(jīng) 過 一 步 狀 態(tài) 預(yù) 測 后 的 狀 態(tài) 協(xié) 方 差 矩 陣 p ( + 1 / 勸 里 面 的誤 差 橢 圓 為 經(jīng) 過 E K F 狀 態(tài) 更新 后 的 狀 態(tài) 協(xié) 方 差 矩 陣 尸 (

21、 十 1 / k 十 1 在 第 二 組誤 差 橢 圓 中 被 E K F 更 新 后 k 的 誤差 橢 圓 顯 著變 小 表 明機(jī) 器 人 自定 位 使 得 位 姿不 確 定 性 得 到 改 善 其 余 幾 組 誤 差 橢 圓 , . , , . , , . 的 變 化不 是 很 明顯 這 是 由于 l 機(jī)器 人 運(yùn) 動 速 度 有 限 而 定 位 頻 率 相 對 較 快 這 樣在 短 ; 距 離 直 線運(yùn) 動 中 根 據(jù) 里程 計(jì) 模型 進(jìn) 行 的一 步 狀 態(tài) 估 計(jì) 值本 身 誤差 就 有 限 2 在 E K F 定 5 m 的 特 征 進(jìn)行 匹 配 位 算 法 中 只 選 用長 度

22、大 于 0 這 雖 提高 了 定 位可 靠 性 但 也 導(dǎo) 致 機(jī) 器 人 在 某 些 位置 可 用 于 定位 的 環(huán) 境 特 征 減 少 所 以 校正 效 果 不 明 顯 , , : , , . , , , . 6 期 莊 嚴(yán)等 : 開 移動 機(jī)器 人 基 于 激 光測距 和單 目視覺 的室 內(nèi) 同 時 定位 和 地 圖 構(gòu)建 933 P ( + 無 1/ k + l 圖 F ig 6 . 6 k 機(jī)器 人 在 尸 ( 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 結(jié)論 . 本 文 的研 究 重 點(diǎn)在 于 如何 從 激 光 和 視 覺 傳 感 器 獲 取 的 數(shù) 據(jù) 中高效 率 的 提 取 環(huán) 境 特 征 并 合理 的 進(jìn) 行幾 何地 圖 構(gòu)建 本文 提 出 的 機(jī) 器 人 同 時 定 位 和 地 圖 構(gòu) 建算 法 已 應(yīng) 用 于 全 自主 移動機(jī) 器 人 在 部 分 結(jié)構(gòu) 化 室 內(nèi)環(huán) 境 下的 導(dǎo) 航 運(yùn) 動 中 實(shí) 驗(yàn)表 明 該 s L A M 算法 可 以 實(shí)時 完 成 機(jī) 器 人 的 全 局 地 圖構(gòu) 建 和 即 時 精 確 定 位 任務(wù) 并 且 易 于 在 嵌

24、入 式 移 動 機(jī)器 人 平 臺 上 實(shí) 現(xiàn) 如 何 進(jìn) 一 步 提 高 地 圖 構(gòu) 建 和 定 位 的 精 度 并 消 除 實(shí)際 環(huán) 境 中 動態(tài) 物 體 對 定 位 和 地 圖 構(gòu) 建 的影 響 是下 一 步 研 究 工 作 的 重 點(diǎn) . , . . 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