智能 汽车 如何 眼观六路 、耳听八方 ？传感器 融合 与 卡尔曼滤波 技术

各位 同學 大家 好 我 是 李永樂 老師

前 一段時間 我講 了 一期 關於 自動 駕駛 的 話題

有 小朋友 就 跟 我 說

他 最近 就 買 了 一輛 自動 駕駛 的 汽車 小鵬 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 帳號 李永樂 老師 裏 訂閱 我

點擊 小 鈴鐺 可以 第一 時間 獲得 更新 信息