機器人學中的狀態估計第八章——迭代式求解ICP問題部分公式推導

原創

Lance Yu

2019-04-24 05:20

本文主要記錄在研讀高博翻譯的機器人學中的狀態估計一書中公式的推導，不理解之處。

式（8.83）推導——迭代式使用旋轉矩陣求解ICP問題：

首先我們有關於旋轉矩陣的目標函數——式(8.81)：

$J(C)=\frac{1}{2}\sum\limits_{j=1}^M{w_j((y_j-y)-C(p_j-p))^T((y_j-y)-C(p_j-p))}$

根據高斯-牛頓的思想，使用擾動形式處理旋轉矩陣有式（8.82）：

$C=\exp(\psi^\wedge)C_{op}\approx (1+\psi^\wedge)C_{op}$

將式（8.82）代入到式(8.81)中，則可將目標函數轉換成下式：

$J(C)=\frac{1}{2}\sum\limits_{j=1}^M{w_j((y_j-y)-(1+\psi^\wedge)C_{op}(p_j-p))^T((y_j-y)-(1+\psi^\wedge)C_{op}(p_j-p))}$

爲了方便書寫，現只考慮目標函數中的一項。將上式展開有：

$[J(C)]_j=((1+\psi^\wedge)C_{op}(p_j-p))^T((1+\psi^\wedge)C_{op}(p_j-p))-((1+\psi^\wedge)C_{op}(p_j-p))^T(y_j-y)-(y_j-y)^T((1+\psi^\wedge)C_{op}(p_j-p))+(y_j-y)^T(y_j-y)$

對上式兩邊同時對擾動求導，並令導數爲0.同時注意到上式和擾動相關的只有前三部分。因此，首先處理擾動的二次項有：

$((1+\psi^\wedge)C_{op}(p_j-p))^T((1+\psi^\wedge)C_{op}(p_j-p))\\ &=(C_{op}(p_j-p))^T(1+\psi^\wedge)^T(1+\psi^\wedge)C_{op}(p_j-p)\\ &=(C_{op}(p_j-p))^T(1-\psi^\wedge)(1+\psi^\wedge)(C_{op}(p_j-p))\\ &=(C_{op}(p_j-p))^T(1-\psi^\wedge\psi^\wedge)(C_{op}(p_j-p))\\ &=(C_{op}(p_j-p))^T(C_{op}(p_j-p))-(C_{op}(p_j-p))^T\psi^\wedge\psi^\wedge(C_{op}(p_j-p))$

其中對於上式的處理第2步中用到了反對稱矩陣的性質。

$(1+\psi^\wedge)^T=(1-\psi^\wedge)$

從上式可以看出擾動的二次項又可以分離出和擾動無關的項，求導時不予考慮。因此，對於上式後半部分按如下處理。

$-(C_{op}(p_j-p))^T\psi^\wedge\psi^\wedge(C_{op}(p_j-p))\\ &=-\psi^T(C_{op}(p_j-p))^\wedge(C_{op}(p_j-p))^\wedge\psi\\ &=-\psi^TC_{op}(p_j-p)^\wedge{C_{op}}^TC_{op}(p_j-p)^\wedge{C_{op}}^T\psi\\ &=-\psi^TC_{op}(p_j-p)^\wedge(p_j-p)^\wedge{C_{op}}^T\psi$

此時根據矩陣求導的知識便可以很容易的得到式（8.83）中等號的左邊。

然後處理關於擾動的一次項，同樣忽略和擾動無關項。

$-((1+\psi^\wedge)C_{op}(p_j-p))^T(y_j-y)\\ &\Rightarrow -(\psi^\wedge C_{op}(p_j-p))^T(y_j-y)\\ &\Rightarrow ((C_{op}(p_j-p))^\wedge\psi)^T(y_j-y)\\ &\Rightarrow -\psi^T(C_{op}(p_j-p))^\wedge(y_j-y)\\&\Rightarrow \psi^T(y_j-y)^\wedge C_{op}(p_j-p)$