當(dāng)前位置：首頁 > 编程资源 > 编程问答 >内容正文

编程问答

多传感器融合定位二-3D激光里程计其二：NDT

發(fā)布時間：2024/1/18 编程问答 39 豆豆

生活随笔收集整理的這篇文章主要介紹了多传感器融合定位二-3D激光里程计其二：NDT 小編覺得挺不錯的,現(xiàn)在分享給大家,幫大家做個參考.

多傳感器融合定位二-3D激光里程計其二：NDT

1. 經(jīng)典NDT
2. 計算方式
- 2.1 2D場景求解:
- 2.2 3D場景求解：
3. 其他 NDT

Reference:

深藍(lán)學(xué)院-多傳感器融合

多傳感器融合定位理論基礎(chǔ)

文章跳轉(zhuǎn)：

多傳感器融合定位一-3D激光里程計其一：ICP

多傳感器融合定位二-3D激光里程計其二：NDT

多傳感器融合定位三-3D激光里程計其三：點云畸變補償

多傳感器融合定位四-3D激光里程計其四：點云線面特征提取

多傳感器融合定位五-點云地圖構(gòu)建及定位

多傳感器融合定位六-慣性導(dǎo)航原理及誤差分析

多傳感器融合定位七-慣性導(dǎo)航解算及誤差分析其一

多傳感器融合定位八-慣性導(dǎo)航解算及誤差分析其二

多傳感器融合定位九-基于濾波的融合方法Ⅰ其一

多傳感器融合定位十-基于濾波的融合方法Ⅰ其二

多傳感器融合定位十一-基于濾波的融合方法Ⅱ

多傳感器融合定位十二-基于圖優(yōu)化的建圖方法其一

多傳感器融合定位十三-基于圖優(yōu)化的建圖方法其二

多傳感器融合定位十四-基于圖優(yōu)化的定位方法

多傳感器融合定位十五-多傳感器時空標(biāo)定(綜述)

1. 經(jīng)典NDT

NDT 核心思想：基于概率的匹配。目標(biāo)是將點集 $Y$ 匹配到固定的點集 $X$ 中。這里的聯(lián)合概率說的是將 $X$ 劃分成柵格(如下圖)，每個柵格里面都有很多點，這些點可以計算一個均值&&協(xié)方差，這是一個概率的概念。而聯(lián)合概率是： $Y$ 往 $X$ 上旋轉(zhuǎn)的時候，它落在柵格中的哪個格子里是知道的。根據(jù)落在格子里的點，本身 $X$ 格子已經(jīng)有了一個均值/協(xié)方差，而 $Y$ 的這些落在格子里的點可以形成一個聯(lián)合概率。這個聯(lián)合概率會作為有沒匹配好的一個指標(biāo)。

點集：
$\begin{aligned} & X=\left\{x_1, x_2, \cdots, x_{N_x}\right\} \\ & Y=\left\{y_1, y_2, \cdots, y_{N_y}\right\} \end{aligned}$ 目標(biāo): $\max \Psi=\max \prod_{i=1}^{N_y} f\left(X, T\left(p, y_i\right)\right)$ (與ICP這里有些區(qū)別，ICP的是點到點的距離，這里變成了聯(lián)合概率)
2D模型: $\quad p=p_3=\left[\begin{array}{lll}t_x & t_y & \phi_z\end{array}\right]^{\mathrm{T}}$
3D模型: $\quad p=p_6=\left[\begin{array}{llllll}t_x & t_y & t_z & \phi_x & \phi_y & \phi_z\end{array}\right]^{\mathrm{T}}$

2. 計算方式

均值和協(xié)方差：
$\mu=\frac{1}{N_x} \sum_{i=1}^{N_x} x_i, \quad \boldsymbol{\Sigma}=\frac{1}{N_x-1} \sum_{i=1}^{N_x}\left(x_i-\mu\right)\left(x_i-\mu\right)^{\mathrm{T}}$ 根據(jù)預(yù)測的位姿，對點進(jìn)行旋轉(zhuǎn)和平移(這里的旋轉(zhuǎn)和平移是一個初始預(yù)測值)：
$y_i^{\prime}=T\left(p, y_i\right)=R y_i+t$ 旋轉(zhuǎn)和平移后的點與目標(biāo)點集中的點在同一坐標(biāo)系下，此時可計算各點的聯(lián)合概率：
$f\left(X, y_i^{\prime}\right)=\frac{1}{\sqrt{2 \pi} \sqrt{|\boldsymbol{\Sigma}|}} \exp \left(-\frac{\left(y_i^{\prime}-\mu\right)^{\mathrm{T}} \boldsymbol{\Sigma}^{-1}\left(y_i^{\prime}-\mu\right)}{2}\right)$ 所有點的聯(lián)合概率(就是所有點的概率乘一起)：
$\begin{aligned} \Psi & =\prod_{i=1}^{N_y} f\left(X, T\left(p, y_i\right)\right) \\ & =\prod_{i=1}^{N_y} \frac{1}{\sqrt{2 \pi} \sqrt{|\mathbf{\Sigma}|}} \exp \left(-\frac{\left(y_i^{\prime}-\mu\right)^{\mathrm{T}} \boldsymbol{\Sigma}^{-1}\left(y_i^{\prime}-\mu\right)}{2}\right) \end{aligned}$ 目標(biāo)是讓所有點的聯(lián)合概率最大。但是上式中的 R 和 t 是在 $y'_i$ 內(nèi)的。這里還有一個exp，這個指數(shù)項會讓公式變得復(fù)雜，需要消掉指數(shù)項。這時去對數(shù)可以解決。
取對數(shù)，簡化問題(目標(biāo)是讓所有點的聯(lián)合概率最大)：
$\ln \Psi=\sum_{i=1}^{N_y}\left(-\frac{\left(y_i^{\prime}-\mu\right)^{\mathrm{T}} \boldsymbol{\Sigma}^{-1}\left(y_i^{\prime}-\mu\right)}{2}+\ln \left(\frac{1}{\sqrt{2 \pi} \sqrt{|\boldsymbol{\Sigma}|}}\right)\right)$ 去除常數(shù)項 $\ln \left(\frac{1}{\sqrt{2 \pi} \sqrt{|\boldsymbol{\Sigma}|}}\right)$ ，得到：
$\max \Psi=\max \ln \Psi=\min \Psi_1=\min \sum_{i=1}^{N_y}\left(y_i^{\prime}-\mu\right)^{\mathrm{T}} \boldsymbol{\Sigma}^{-1}\left(y_i^{\prime}-\mu\right)$ 因為之前是負(fù)數(shù)，我們上式只需要求最小值就行。(這里又變成馬氏距離了)

這時就變成了一個優(yōu)化問題了：
目標(biāo)函數(shù)： $\min \sum_{i=1}^{N_y}\left(y_i^{\prime}-\mu\right)^{\mathrm{T}} \boldsymbol{\Sigma}^{-1}\left(y_i^{\prime}-\mu\right)$
$\quad \quad \quad \quad y_i^{\prime}=T\left(p, y_i\right)=R y_i+t$
待求參數(shù)： $R, t$
定義殘差函數(shù)： $f_i(p)=y_i^{\prime}-\mu$
按照高斯牛頓法的流程，只需計算殘差函數(shù)關(guān)于待求參數(shù)的雅可比，便可迭代優(yōu)化。
$J_i=\frac{d f_i(p)}{d p}$

2.1 2D場景求解:

$\begin{aligned} p & =\left[\begin{array}{lll} t_x & t_y & \phi_z \end{array}\right]^{\mathrm{T}} \\ \\ y_i^{\prime} & =T\left(p, y_i\right) \\ & =\left[\begin{array}{cc} \cos \phi_z & -\sin \phi_z \\ \sin \phi_z & \cos \phi_z \end{array}\right] y_i+\left[\begin{array}{l} t_x \\ t_y \end{array}\right] \end{aligned}$ 雅可比：
$J_i=\frac{d f_i(p)}{d p}=\left[\begin{array}{ccc}1 & 0 & -y_{i 1} \sin \phi_z-y_{i 2} \cos \phi_z \\ 0 & 1 & y_{i 1} \cos \phi_z-y_{i 2} \sin \phi_z\end{array}\right]$

2.2 3D場景求解：

$\begin{aligned} & p=\left[\begin{array}{llllll} t_x & t_y & t_z & \phi_x & \phi_y & \phi_z \end{array}\right]^{\mathrm{T}} \\ & y_i^{\prime}=T\left(p, y_i\right)=R_x R_y R_z y_i+t=\left[\begin{array}{ccc} c_y c_z & -c_y s_z & s_y \\ c_x s_z+s_x s_y c_z & c_x c_z-s_x s_y s_z & -s_x c_y \\ s_x s_z-c_x s_y c_z & c_x s_y s_z+s_x c_z & c_x c_y \end{array}\right] y_i+\left[\begin{array}{c} t_x \\ t_y \\ t_z \end{array}\right] \end{aligned}$ 雅可比：
$\begin{aligned} J_i=\left[\begin{array}{llllll} 1 & 0 & 0 & 0 & c & f \\ 0 & 1 & 0 & a & d & g \\ 0 & 0 & 1 & b & e & h \end{array}\right] \end{aligned}$ 其中：
$\begin{aligned} & a=y_{i 1}\left(-s_x s_z+c_x s_y c_z\right)+y_{i 2}\left(-s_x c_z-c_x s_y s_z\right)+y_{i 3}\left(-c_x c_y\right) \\ & b=y_{i 1}\left(c_x s_z+s_x s_y c_z\right)+y_{i 2}\left(-s_x s_y s_z+c_x c_z\right)+y_{i 3}\left(-s_x c_y\right) \\ & c=y_{i 1}\left(-s_y c_z\right)+y_{i 2}\left(s_y s_z\right)+y_{i 3}\left(c_y\right) \\ & d=y_{i 1}\left(s_x c_y c_z\right)+y_{i 2}\left(-s_x c_y s_z\right)+y_{i 3}\left(s_x s_y\right) \\ & e=y_{i 1}\left(-c_x c_y c_z\right)+y_{i 2}\left(c_x c_y s_z\right)+y_{i 3}\left(-c_x s_y\right) \\ & f=y_{i 1}\left(-c_y s_z\right)+y_{i 2}\left(-c_y c_z\right) \\ & g=y_{i 1}\left(c_x c_z-s_x s_y s_z\right)+y_{i 2}\left(-c_x s_z-s_x s_y c_z\right) \\ & h=y_{i 1}\left(s_x c_z+c_x s_y s_z\right)+y_{i 2}\left(c_x s_y c_z-s_x s_z\right) \end{aligned}$ 上面是使用旋轉(zhuǎn)矩陣求導(dǎo)，也可以使用李代數(shù)求解。

3. 其他 NDT

總結(jié)

以上是生活随笔為你收集整理的多传感器融合定位二-3D激光里程计其二：NDT的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。

上一篇： svs文件转换为tiff文件
下一篇： Manipulation Attacks