智能 汽车 如何 眼观六路 、耳听八方 ？传感器 融合 与 卡尔曼滤波 技术
各位 同學 大家 好 我 是 李永樂 老師
前 一段時間 我講 了 一期 關於 自動 駕駛 的 話題
有 小朋友 就 跟 我 說
他 最近 就 買 了 一輛 自動 駕駛 的 汽車 小鵬 P7
它 擁有 多個 攝像頭 毫米波 雷達
超聲波 雷達 分米級 的 高德 地圖
以及 厘米 級 的 高精度 定位
還有 互為 冗余 的雙 計算 平臺
他 想問 我
這 麽 一大堆 聽不懂 的 名詞 到底 是 什 麽 意思 呢
今天 我們 就 來 聊 一聊
小鵬 汽車 的 NGP 自動 導航 輔助 駕駛 功能
為 了 聊 這個 話題
我們 首先 來講 一講 傳感器 的 融合 傳感器 融合
我們 在 上 一次 講 自動 駕駛 的 時候 說過
一個 汽車 要 想 自動 駕駛 的話
它 必須 能夠 自動 地 感知 外界 的 環境
同時 進行 決策 以及 執行
在 感知 的 層面 就 需要 一大堆 傳感器 進行 配合
汽車 的 傳感器 就 相當於 人 的 感官
人 的 感官 不 只有 一種
比如說 亞 裏士多德 說 人 的 感官 有 五種
就是 眼睛 鼻子 舌頭 耳朵 還有 身體
分別 對應 了 視覺 嗅覺 味覺 聽覺
還有 觸覺 是 吧
每 一種 感覺 都 有 它 的 特點
比如說 大部分 情況 下
視覺 的 範圍 是 比較 廣泛 的
但是 如果 你 遇到 了 雨雪 天氣
也許 聽覺 能為 我們 提供 更 準確 的 信息
只有 幾種 感官 相互配合
人們 才能 準確 地 判斷 外界 的 環境
那 麽 對於 汽車 來講
同樣 是 如此
使用 一種 傳感器 不能 準確 地 判斷 外界 環境
所有 的 自動 駕駛 汽車
必須 使用 多種 傳感器 相互配合
這 就是 所謂 的 傳感器 融合 技術
它 能夠 讓 汽車 眼觀六路 耳聽八方
回答 兩個 重要 的 問題
那 就是 我 周圍 有什 麽 以及 我 在 哪裏
我們 首先 來看 第一個 問題
說 汽車 可以 通過 傳感器 回答
說 自己 的 周圍 到底 有什 麽 這個 話題 是 吧
一般來講
在 具有 高級 自動 駕駛 功能 的 汽車 上
它 在 高速行駛 的 時候
有 三種 傳感器 發揮 了 這個 作用
這 三種 傳感器 分別 是 激光雷達
以及 以前 我們 講過 的 叫 毫米波 雷達
毫米波 雷達 還有 就是 攝像頭 攝像頭
每 一種 傳感器 都 有 自己 不同 的 特點
我們 來 解釋一下 畫個 表格
表格 畫完 了
我們 首先 來說 一說 激光雷達
激光雷達 的 原理
就是 發射 一束 激光
遇到 障礙物 然後 反射 接收 到 回波
計算 發射 和 接收 的 時間差
從而 判斷 障礙物 的 距離
激光雷達 又 分為 兩種
有 一種 叫 機械式 激光雷達
就是 它 可以 向 不同 的 方向 360 度 的 旋轉
然後 掃描 是 吧
這樣 就 可以 判斷
說 周圍 的 障礙物 離 自己 都 有 多 遠 了
可以 獲得 一個 3D 點陣
就 可以 勾繪 出 周圍 的 這個 情況
還有 另外 一種 激光雷達 叫做 固體 激光雷達
它 就是 由 很 多個 光源 形成 一個 陣列
通過 調相 的 方法
就 可以 實現 向 不同 的 方向 發射 激光
它 不 需要 旋轉
而且 這種 雷達 它 相比 來講 造價 比較 低
這個 壽命 也 比較 長
可能 是 未來 激光雷達 的 這個 發展 方向
激光雷達 有 很多 很 好 的 特征
比如說 首先 它 的 定位 非常 準確
它 可以 準確 地 判斷 這個 障礙物 離 我們 有多遠
而且 是 什 麽 方向 的
它 都 可以 準確 地 判斷 出來
同時 激光雷達 是 自己 發出 電磁波 接收 回波
它 不 需要 有 外界 的 光
所以 夜晚 也 可以 使用
而且 激光 的 波長 很 短
它 可以 判斷 這個 物體 的 表面 的 形狀 是 吧
這個 物體 是 一個 平面 還是 一個 凸起
它 都 可以 判斷 出來
我們 稱之為 可以 3D 建模
但是 激光雷達 也 有 它 的 缺點
比如說 因為 激光 波長 比較 短
很難 穿透 雨雪 雲霧
到 雨雪 雲霧 的 天氣
激光雷達 就 不能 用 了 對 吧
再 比如說 激光雷達 是 靠 發射 激光 接收 回波
測量 物體 的 距離
它 沒有 辦法 判斷
這個 物體 是 紅色 還是 綠色 的 是 吧
它 沒有 辦法 判斷 物體 的 顏色
所以 它 看不出來 紅綠燈 是 吧
而且 它 也 沒有 辦法 區分 哪個 車道 線
是不是 這是 激光雷達 的 一個 缺點
那 麽 為 了 克服 這些 缺點
人們 還 需要 其他 雷達
比如說 毫米波 雷達
毫米波 雷達 和 激光雷達 原理 類似
它 只不過 波 長長 一些
它 也 可以 實現 比較 準確 的 定位
在 夜晚 也 可以 使用
它 還有 一個 優點
就是 因為 波 長長 可以 穿透 雨雪 是 吧
所以 也 稱 它 為 全天候 雷達
白天 晚上 都 能 用 雨雪 天氣 也 能 用
但是 它 也 有 缺點
首先 它 跟 激光雷達 一樣
不 能夠 區分 物體 的 顏色
而且 因為 它 波長 比較 長
就 沒有 辦法 準確 地 描繪出
這個 物體 到底 是 長 什 麽 樣子
沒有 辦法 進行 3D 建模 這是 它 的 一 缺點
那 麽 怎 麽 辦 呢
人們 又 提出 我們 還 可以 用 攝像頭
攝像頭 就 相當於 汽車 的 眼睛
它 跟 雷達 不 一樣
雷達 是 發射 電磁波 接收 回波
而 這個 攝像頭 它 只 接收 外界 的 光 信號
它 不 往外 發射 對 不 對
所以 它 的 特征 首先 它 可以 判斷 顏色 對 吧
它 拍攝 了 很多很多 的 照片
然後 把 這些 照片 輸入 計算機 進行 機器 學習
它 就 可以 知道 這個 是 紅綠燈 那個 是 斑馬線
這個 是 人 那個 是車 對 吧
它 就 可以 知道 這些 信息 了
但是 攝像頭 也 有 它 的 缺點
首先 來講 晚上 沒光 的 時候
攝像頭 肯定 是 用 不了 的 對 不 對
雨雪 天氣 這個 光 也 很 容易 被 遮擋
所以 也 沒有 辦法 判斷 前方 的 物體 了
如果 你 是 雙目 攝像頭
你 倒 是 可以 進行 簡單 的 定位
但是 攝像頭 它 是 有 畸變 的
所以 它 的 定位 準確度 遠遠 沒有 雷達 好
如果 是 單目 攝像頭 那 就 沒有 辦法 實現 定位 了
同時 攝像頭 也 沒有 辦法 進行 3D 建模
你 這樣 一看
每 一種 傳感器 其實 都 有 它 的 優點 和 它 的 缺點
所以 我們 得 把 多種 傳感器 配合起來 使用
才 能夠 準確 地 判斷 出 外界 的 環境
你 比如說 你 用 這個 激光雷達 進行 定位 是 吧
定位 完 了 之後
用 它 勾勒 出 這個 物體 的 這個 形狀 來
最後 我們 再用 這個 攝像頭 進行 上色
我 就 可以 知道 是 什 麽 了
如果 攝像頭 發現 前面 有 一只 老虎
結果 激光雷達 判斷 這是 一個 平面 的 是 吧
那 我們 就 可以 知道 這是 一張 老虎 的 年畫
我 就 可以 準確 地 進行 判斷 了
那小鵬 P7 就是 利用 了 這樣 的 技術
它 有 5 個 毫米波 雷達
還有 14 個 攝像頭 進行 融合
那 自動 駕駛 汽車 除了 要 知道 周圍 有什 麽 以外
還有 一個 就是 自己 在 哪裏
或者說 要 實現 這個 高精度 的 定位
那 我們 現在 定位 用 導航 是不是
我們 用 手機 打開 百度 地圖 或者 高德 地圖
我們 就 可以 導航 了
那種 地圖 它 叫 導航 級 地圖
導航 級 地圖 精確度 不高 大概 是 米級
就是說 我們 使用 導航 級 地圖
它 的 誤差 在 幾米 的 這個 量級
我 沒有 辦法 區分
自己 的 汽車 到底 是 在 主 路上 還是 在 輔 路上
到底 是 在 第一條 車道 還是 在 第二條 車道
那 麽 這種 導航 及 地圖
其實 對於 自動 駕駛 汽車 是 不夠 的
自動 駕駛 汽車 必須 使用 高精度 的 地圖
高精 地圖
這個 高精 地圖 要 精到 什 麽 程度 呢
要 能夠 對 汽車 進行 分米級
甚至 厘米 級 的 這種 定位
它 的 誤差 要 在 分米 甚至 是 厘米 的 量級
那 怎 麽 才能 做到 這 麽 高 的 精度 呢
我給 大家 介紹 一下 這個 方法
首先 我們 知道 一個 汽車 上面
它會 有 一些 這個 定位 的 模塊
它 可以 和 這個 衛星 進行 聯絡
比如說 我們 可以 用 GPS 或者 北鬥 這個 導航系統
用 這個 導航系統 跟 衛星 進行 聯絡
理論 上 來講 四顆 衛星 就 可以 把 你 定位 了
這 就是 第一步
就是 通過 這個 北鬥 或者 GPS 我們 可以 進行 定位
但是 這樣 定位 它 是 存在 一定 的 誤差 的
比如說 因為 雲霧 的 影響 造成 的 這個 光速 的 變化
等等 會 有 誤差
那 麽 這個 誤差 怎 麽 辦 呢
我們 可以 在 地面 上 建立 一系列 的 基站 進行 校準
我們 不光 要 你 和 這個 衛星 聯絡
你 還要 和 地面 的 基站 進行 校準 聯絡
很多 公司 就 做 這個 事
比如說 像 這個 千尋 定位系統
千尋 定位 就是 在 地面 上 安裝 了 一大堆 的 基站
這樣 它 就 可以 提高 你 定位 的 精度
比如說 像 導航系統 只能 給你 達到 米 量級
而 這個 千尋 定位 就 可以 輔助 你
達到 分 米 量級 的 定位
但是 我們 怎 麽 樣 才能
進一步提高 這個 定位 的 精度 呢
還有 方法
比如說 像 這個 高德 地圖
他 做 這個 高精度 地圖 的 方法 是 這樣 的
首先 他 在 做 地圖 的 時候
他 就 會派 一輛 帶有 這個 激光雷達 的 汽車 上路
這個 激光雷達 的 汽車
它 就 會 把 周圍 的 環境 都 拍下來
周圍 可能 有什 麽 呢
周圍 可能 有 一個 比如說 一個 房子
也 可能 是 有 一棵 大樹
也 可能 這邊 有 一個 紅綠燈
也 可能 這邊 還有 一個 什 麽 花壇 之類 的
那 麽 這些 東西
都 會 被 這個 激光雷達 拍攝 下來
拍攝 下來 之後
它 就 把 這個 數據 都 記錄下來 了
然後 放到 這個 高精 地圖 上面 是 吧
這個 就是 高德 的 這個 高精 地圖
然後 當 我們 的 這個 自動 駕駛 汽車
去 使用 這個 地圖 的 時候
它 自動 駕駛 汽車 不是 也 有 這些 傳感器 嗎
它 也 會 拍攝 周圍 的 環境
它會 把 周圍 的 環境
再 和 高精 地圖 中 已經 拍攝 好 的 這個 環境
進行 對比
它 一 對比 就 知道 了
你 看 我 現在 在 主 路上 我 在 輔 路上
我 在 第一條 道 還是 在 第二條 道
我 就 可以 達到 厘米 的 量級 了
這個 小鵬 汽車 就 采用 了 這樣 的 一個 技術
首先 通過 這個 定位系統
以及 這個 千尋 定位系統
達到 了 一個 分米級 的 定位
然後 再 通過 這個 高精 地圖
跟 周圍 的 環境 進行 比對
達到 這個 厘米 級 的 定位
這 就是 自動 駕駛 汽車 的 一個 定位 的 原理
那剛 才 我們 介紹 了 傳感器 融合 的 作用
但 傳感器 究竟 是 怎 麽 融合 到 一塊 的 呢
這是 一個 很 復 雜的 技術 問題
舉 個例 子 來講
不同 的 傳感器 它們 的 獲得 信息 是 不 一樣 的
這裏 面 面臨 兩個 方面 的 問題
第一個 問題 叫做 空間 定標
就是 你 不同 的 傳感器
你 是 安裝 在 車上 不同 的 位置 的
所以 角度 也 不 一樣
這樣 你 會 看到 不同 的 情景 拍攝 到 不同 的 畫面
你 怎 麽 通過 空間 坐 標的 方法
把 這些 個 畫面 給 融合 到 一塊
這就 叫 空間 定標 是 吧
第二個 是 時間 定標
每 一個 傳感器 它 對 外界 環境 的 感知 頻率 不 一樣
比如說 攝像頭 它 可能 每秒鐘 拍攝 25 張 照片
而 這個 毫米波 雷達
它 可能 每秒鐘 只是 探測 10 次
這樣一來 攝像頭 拍照片 的 時候
可能 這個 毫米波 雷達 沒有 信息 傳 回來
所以 我要 通過 一些 方法
把 這個 不同 時間 的 信號 又 把 它 融合 起來
這個 就 叫 所謂 的 時間 定標
除了 空間 定標 和 時間 定標 以外
更為 重要 的 是
有 的 時候 傳感器 獲得 的 信息 是 彼此 矛盾 的
比如說 攝像頭 告訴 我們 前方 有 一堵 墻
而 毫米波 雷達 告訴 我們 前面 是 個 斜坡
我們 到底 相信 誰 呢
比如說 定位系統 告訴 我們 現在 在 主 路上
但是 這個 雷達 告訴 我們 現在 我們 在 輔 路上
我們 又 該 相信 誰 呢
因為 每 一種 傳感器 它 都 是 有 誤差 的
所以 這樣 的 矛盾 是 每時每刻 都 在 發生 的
我們 該 怎 麽 辦 呢
這裏 我們 就 不得不 提 一位
在 工程界 家喻戶曉 的 人物 了
他 是 一位 出生 在 奧地利 的 美國 數學家 和 工程師
名字 叫做 魯道夫 · 卡爾曼
他 提出 了 卡爾曼濾波
卡爾曼濾波 曾經 幫助 阿波羅 飛船 登上 了 月球
現在 也 在 每時每刻 都 在 幫助 我們 的 GPS 導航系統
我們 利用 卡爾曼濾波
就 可以 把 不同 傳感器 的 信息 融合 到 一塊 了
我們 舉 一個 例子
比如說 我們 通過 定位系統
比如 GPS 定位 或者 是 什 麽 其他 的 定位系統
我們 知道 了 這個 汽車 的 位置
這個 位置 就是 z₁
我 告訴 你 汽車 是 在 z₁ 的 位置
但是 因為 GPS 是 有 誤差 的
它 不能 準確 地 告訴 你 在 什 麽 位置
而 只能 告訴 你 在 一個 範圍 內
它 還有 一個 誤差 或者說 方差 σ₁²
z₁ 只是 表示
你 可能 在 的 位置 中 的 這個 平均 位置
或者 叫 期望
而 這個 σ₁² 就 代表 了 你 的 誤差 大小
如果 這個 σ₁² 特別 特別 小
就 表示 你 的 定位 特別 準確
σ₁² 特別 大
就 表示 你 的 定位 不太 準確 誤差 特別 大
一般來講 我們 認為
這個 GPS 定位 也好 其他 的 定位 也好
它 的 這個 誤差 情況
都 滿足 一個 高斯分布 或者 正態 分布
以前 我們 講過 高斯分布 是 什 麽 意思 呢
畫 一個 圖像
縱 坐標 表示 的 是 概率密度
就 出現 在 某 一個 位置 的 概率密度
橫坐標 是 表示 你 的 位置
如果說 GPS 告訴 我們
你 的 位置 是 以 z₁ 為 平均 位置
而 標準差 是 σ₁
我們 可以 畫出 一個 圖像 來
它 就 長 這個 樣子 是 吧
它 這個 最大 概率 的 位置 就是 z₁
這個 圖像 就 告訴 我們
你 的 位置 最有 可能 是 在 z₁ 的 位置
你 離 z₁ 位置 越近 你 出現 的 概率 就 越 大
你 離 z₁ 位置 越遠 你 出現 的 概率 就 越 小
如果 這個 函數 特別 的 瘦高
那 就 意味著 你給 我 的 信息 特別 準確
你 分布 的 位置 都 是 在 z₁ 附近 的
如果 這個 函數 特別 的 矮胖
那 就 說明 你 誤差 特別 大
我 也 不 太 清楚 你 到底 是 在 什 麽 位置
這 就是 所謂 的 這種 高斯分布 或者說 正態 分布
這裏 邊的 這個 σ
其實 就 表示 它 瘦高 還是 矮胖 的 程度
如果 σ 特別 小 那 這個 函數 就 特別 瘦高
就 表示 比較 準確
σ 特別 大 就 說明 你 比較 矮胖
好 總而言之
這個 GPS 定位 告訴 了 我們 這個 位置 的 信息
但是 我們 不是 還有 其他 傳感器 嗎
什 麽 雷達 什 麽 的 攝像頭
我們 這些 傳感器 它 也 可以 告訴 我們 位置 的 信息
比如 它 告訴 我們 汽車 的 位置 是 在 z₂
而且 這個 方差 是 σ₂²
我 也 可以 再 畫出 這個 傳感器 告訴 我們 的 情況
比如說 它 有 可能 是 長 這個 樣子 的 長 這個 樣子
這個 樣子 就 告訴 我們
傳感器 認為 我們 的 位置 更加 靠前 一點 是不是
而且 傳感器 提供 的 信息 可能 更加 準確
它 的 這個 標準差 σ₂ 比較 小
而剛 才 這個 GPS 告訴 我們 的 這個 標準差
σ₁ 它 是 比較 大 的
現在 我們 得到 兩個 不同 的 信息
我們 如何 把 它 融合 到 一塊 呢
這 卡爾曼 就 說 了
經過 數學計算 你 有 一個 最優 的 估計
最優 的 估計 最優 的 估計 是 什 麽 呢
那 就是 這有 一個 公式
這 公式 告訴 我們 你 最優 估計 的 位置 等於 什 麽 呢
=σ₂²z₁/(σ₁²+σ₂²)+σ₁²z₂/(σ₁²+σ₂²)
有 的 同學們 說什 麽 亂七八糟 的
其實 這個 問題 並 不是 難 理解 的
咱們 仔細 看
z₁ 和 z₂ 是 兩個 傳感器 告訴 我們 的 位置
我們 最後 估計 的 位置
一定 是 要 把 它 融合 到 一塊 得出 一個 結論
但是 誰 更 準確 我 就 更 相信 誰
你 看 這 兩個 圖像 你 就 應該 知道
這個 σ₂ 更 小 一些
說明 z₂ 這個 信息 更加 準確
所以 我 在 計算 權重 的 時候
我 應該 讓 z₂ 的 這個 權重 更大 一些
我 怎 麽 才能 讓 z₂ 的 權重 更大 一些 呢
我 讓 它 前面 的 權重 是
σ₁²/(σ₁²+σ₂²)
你 σ₁ 比較 大 所以 這個 權重 就 大
大家 能 明白 嗎 對 不 對
同樣 道理 你 這個 z₁ 它 的 標準差 比較 大
說明 它 不是 那 麽 的 準確
所以 它 的 權重 就要 小 一些 是 吧
所以 你 可以 簡單 地 理解 一下
這個 其實 就是 把 兩個 數據 融合 到 一塊 了
在 融合 的 時候 誰 準確 誰 的 權重 就 會 大 一些
同時 我 也 可以 知道 新 的 這個 方差
1/σ²=1/σ₁²+1/σ₂²
這 就是 卡爾曼 告訴 我們 的 最優 結論
你 會 發現 這個 新 的 標準差 的 平方 或者說 方差
它 是 比 原來 的 兩個 方差 都 要 小 的
所以 最終 我們 會得 出來 一個 新 的 估計 情況
它 是 長 這個 樣子 的
它 首先 它 的 均值 是 介於 兩個 估計值 之間 的
而且 它 的 方差 比 兩個 估計值 都 要 小
或者說 我們 獲得 了 一個 更加 準確 的 估計
那 麽 假如 這輛 汽車 還有 其他 傳感器
比如說 有什 麽 速度計 加速度計
你還 可以 繼續 進行 卡爾曼濾波
這樣 就 會 獲得 一個 更加 準確 的 結果 了
我們 剛才 介紹 的 這個 模型
其實 是 最 簡單 的 一個 卡爾曼濾波 模型
在 實際 的 情況 下 卡爾曼濾波 不光 是 多維 的
而且 還是 跟 時間 有關 的 它 是 一個 時續 的
我們 來看 一下 這個 卡爾曼濾波
它 在 航海 上 有 什 麽 應用 在 航海 上 應用
其實 人類 在 大 航海 時代 開始 的 時候
就 已經 在 不知不覺 中 使用 這種 濾波 的 方法 了
比如說 一個 有 經驗 的 船長
他 在 t₁ 的 時刻 他會 有 一個 位置
他 知道 自己 的 位置
那 麽 他 會 有 一個 航向 或者 是 航速 表 是 吧
他 有 航向 還有 航速 表
所以 他 通過 航向 和 航速 他 可以 估計
說 t₂ 的 時刻 它 有 一個 估計 的 位置
但是 他並 不會 把 這個 位置 作為 最終 自己 的 位置
他會 怎 麽 樣
他會用 六分儀 去測
在 六分儀 測量 的 時候
它 又會有 一個 估計 的 位置
這 兩個 估計 的 位置 可能 是 不 一樣 的
於是 它會 怎 麽 樣
它會 通過 一定 的 方法 把 它 融合 起來
得到 t₂ 的 位置
那 現在 我們 就 知道 了
其實 你 最佳 的 融合 方法 就是 卡爾曼濾波 對 吧
你 t₂ 的 位置 有 了 之後
你還 可以 繼續 通過 航向 和 航速 對 不 對
你還 可以 繼續 得 出來 一個 估計 的 位置
然後 你 再 通過 六分儀
你 再 得到 一個 估計 的 位置
如此 你 把 這 兩個 估計 的 位置 再 去 算
你 就 會 得到 t₃ 時刻 的
這個 最好 的 一個 估計 位置
所以 這 就是 在 航海 過程 中
經常 使用 的 一種 方法
它 的 原理 其實 跟 卡爾曼濾波 是 一樣 的
多 傳感器 融合 的 原理 同樣 是 如此
就是 每 一種 傳感器
它 提取 的 數據 獲得 的 特征 以及 做出 的 判斷
本質 上 都 是 一組 帶有 誤差 的 測量 值
那 麽 我們 可以 通過 卡爾曼濾波 的 方法
把 這些 測量 值 融合 起來
得到 一個 最 準確 的 結果
宇宙飛船 登月 的 時候
需要 通過 各種 方法 來 判斷 自身 的 位置
修正 軌道
現在 的 自動 駕駛 汽車
也 要 通過 各種 傳感器 來給 自己 定位
同時 判斷 周圍 的 環境
所 使用 的 原理 其實 與 早期 的 航海家 是 一樣 的
只不過 我們 現在 有 了 更加 高精尖 的 設備
以及 更好 的 數學 理論
中國 古人 說 兼聽則明 偏聽則暗
兼 聽 就是 多 傳感器 融合
從 兼 聽到 明所 需要 的 就是 卡爾曼濾波
小鵬 汽車 NGP 就是 這樣
通過 多 傳感器 融合 和 卡爾曼濾波 技術
能夠 對 自己 的 位置 和 周圍 的 環境
做出 更加 準確 的 判斷
從而 讓 駕駛 變得 更加 安全 和 高效
大家 如果 喜歡 我 的 視頻
可以 在 YouTube 帳號 李永樂 老師 裏 訂閱 我
點擊 小 鈴鐺 可以 第一 時間 獲得 更新 信息
