從一個坐標系的點變換到另一個坐標系的點，旋轉(zhuǎn)矩陣的角度我們不能直接知道，但是可以通過兩個坐標系之間的旋轉(zhuǎn)來間接得到。
如：
世界坐標系有一個點P，我們要描述它，就得給他一個坐標系原點，如果他放在車身坐標系，坐標值為P1(2,-1,z)，怎么知道該點P1在另一個imu坐標系中的坐標值呢？

這里說明一下，imu坐標系和車身坐標系原點重合，x軸和y軸方向不一樣，z軸重合(方向都向上)。

先說結(jié)論：

由于車身坐標系和imu坐標系只是旋轉(zhuǎn)90°的關(guān)系，所以可以很容易得到車身坐標系的點坐標在imu坐標系的點坐標為：

Ximu= -1 * Ycar Yimu= Xcar

---

以下是俯視圖，z軸向上，在圖中沒畫出來，用圓圈內(nèi)一個實心點表示向上。

從圖中可以看出，這個點P1在對應(yīng)的imu坐標系中的坐標為P2(1,2,z)。

當然，這是人眼看出來的，怎么用算法求解出來呢？

坐標變換需要用到變換矩陣，這個變換矩陣在這里就是旋轉(zhuǎn)矩陣，需要知道旋轉(zhuǎn)矩陣的角度，就得借助坐標系的旋轉(zhuǎn)了。

---

記住一句話：坐標系旋轉(zhuǎn)是坐標變換的逆過程。

先解釋這句話：坐標系旋轉(zhuǎn)是坐標變換的逆過程。 即，如果從A坐標系繞Z軸逆時針旋轉(zhuǎn)$\alpha °$，可以得到B坐標系；那么在A坐標系的某點$P_A$，該點在B坐標系的點坐標為：在A坐標系下將$P_A$點通過旋轉(zhuǎn)矩陣(角度是繞Z軸逆時針旋轉(zhuǎn)$?\alpha °$)計算得到，即 $P_B=T(?\alpha )?P_A$ ，其中$T(?\alpha )$表示旋轉(zhuǎn)矩陣逆時針旋轉(zhuǎn)$?\alpha °$。

用實例來證明：

將車身坐標系繞Z軸逆時針旋轉(zhuǎn)-90°(即順時針旋轉(zhuǎn)90°，會描述成逆時針旋轉(zhuǎn)-90°。)，就得到了imu坐標系。那么，車身坐標系的點P1(2,-1,z)，在imu坐標系中坐標值是多少呢？

將車身坐標系繞Z軸右手法則正方向旋轉(zhuǎn)-90°，就得到了imu坐標系。那么，車身坐標系的點P1(2,-1,z)，在imu坐標系中坐標值是多少呢？

車身坐標系的某點P1(2,-1,z)，在對應(yīng)的imu坐標系中的坐標為P2(1,2,z)，就有T*P1=P2，可以解得旋轉(zhuǎn)矩陣T為(1)式：

我們知道，繞Z軸旋轉(zhuǎn)的旋轉(zhuǎn)矩陣是:

由于(1) = (2)，解出旋轉(zhuǎn)角度 $\alpha =90°$。坐標變換就是將該點P1在自身車身坐標系中右手定則正方向旋轉(zhuǎn)90°，得到了在imu坐標系中的坐標值。

上面沒考慮z軸方向的值，因為z軸重合，z值一樣。下面是考慮z軸方向的推導(dǎo)過程：

注意，這里由車身坐標系的P1到imu坐標系中P2點旋轉(zhuǎn)矩陣的旋轉(zhuǎn)角度是右手法則繞Z軸正方向旋轉(zhuǎn)90°，但是從車身坐標系旋轉(zhuǎn)到imu坐標系是繞Z軸右手法則正方向旋轉(zhuǎn)-90°。

已知車身坐標系的點P1， 我們的目的是求P1在IMU坐標系的坐標P2。
我們只能通過旋轉(zhuǎn)矩陣來計算出來P2，并不是描述坐標軸旋轉(zhuǎn) $\alpha °$，因為你旋轉(zhuǎn) $\alpha °$還是得通過旋轉(zhuǎn)矩陣才能得到具體結(jié)果坐標。那么，前面計算得到的車輛坐標系下的旋轉(zhuǎn)矩陣是右手法則繞Z軸正方向旋轉(zhuǎn)90°，而從車輛坐標系到imu坐標系需要繞車輛坐標系的Z軸右手法則正方向旋轉(zhuǎn)-90°，正好一正一負，即坐標系旋轉(zhuǎn)是坐標變換的逆過程。

對于坐標軸旋轉(zhuǎn)需要旋轉(zhuǎn)兩次的情況，可以自己做一下測試，應(yīng)該還是一樣順序，例如坐標軸先繞X軸旋轉(zhuǎn)a°，再繞Z軸旋轉(zhuǎn)b°，那么坐標變換就是旋轉(zhuǎn)矩陣先繞X軸旋轉(zhuǎn)-a°，再繞Z軸旋轉(zhuǎn)-b°，而不是旋轉(zhuǎn)矩陣先繞Z軸旋轉(zhuǎn)-b°，再繞X軸旋轉(zhuǎn)-a°。我這里沒驗證，按照我代碼里面的應(yīng)該是這樣。

---

關(guān)于右手坐標系與右手法則：

如果不明白這兩個概念，可以參考右手坐標系與右手定則。

• 右手坐標系是用來指定X、Y、Z軸的正方向，并不能隨意指定X、Y、Z軸的正方向；
• 右手定則是用來規(guī)定繞某軸旋轉(zhuǎn)的正方向。

---

右手法則的四指旋轉(zhuǎn)方向為正。

三維空間下，逆時針旋轉(zhuǎn)不一定是正方向，所以只能描述為右手定則的繞某軸正方向旋轉(zhuǎn)：

• 比如你右手握成拳頭，大拇指指向下，那么你四個手指的指向就是順時針，即正方向，那么逆時針就不是正方向了，你就不能套用逆時針是正方向的理論。

• 所謂的逆時針方向為正，是由于人們平時的習慣，約定俗成的。如，打牌時是逆時針出牌、坐標系象限是逆時針命名一二三四象限，角度也是。

• 我們平時在三維空間的旋轉(zhuǎn)，也習慣稱呼順時針旋轉(zhuǎn)與逆時針旋轉(zhuǎn)，是因為，右手定則的繞某軸旋轉(zhuǎn)的旋轉(zhuǎn)方向不好描述(例如，你說，“右手握住Z軸，大拇指指向Z軸正方向，繞著四指指向的方向旋轉(zhuǎn)30度”，這樣描述太麻煩，不如一句，“繞Z軸逆(或順)時針旋轉(zhuǎn)30度”，描述的簡潔清晰)，所以使用順時針逆時針旋轉(zhuǎn)來描述更容易讓人明白是怎樣旋轉(zhuǎn)的，和順時針逆時針旋轉(zhuǎn)的正反無關(guān)。

---

將兩條結(jié)論列出來：

• 將車身坐標系繞Z軸右手法則正方向旋轉(zhuǎn)-90°，就得到了imu坐標系；
• 車身坐標系點P1經(jīng)過旋轉(zhuǎn)矩陣T得到imu坐標系點P2,這個變換矩陣T是將車身坐標系點P1繞車身坐標系Z軸右手法則正方向旋轉(zhuǎn)90°。

看明白沒，一個是坐標系的坐標軸繞Z軸右手法則正方向旋轉(zhuǎn)-90°得到另一個坐標系，一個是坐標點繞Z軸右手法則正方向旋轉(zhuǎn)90°得到在另一個坐標系下對應(yīng)的坐標值。

變換矩陣是將某點在自己的坐標系內(nèi)通過變換矩陣進行變換，但是得到的旋轉(zhuǎn)后的坐標值是其他坐標系的坐標值。

需要注意，不要混淆的是，這里的坐標系原點和該坐標系某點P是相對不動的，如果是一個相對坐標系運動的物體，那就是自己在自己坐標系的坐標已經(jīng)變了,此時如果需要預(yù)測運動后的坐標值，需要用到車的速度、角度等odometry的信息進行同步。和本文討論的坐標變換不一樣。

平移向量

對于坐標系平移，也是逆過程。
例如，兩個坐標系A(chǔ)和坐標系B的各個軸方向一樣，只是原點不在一起。
坐標系B的原點(0,0,0)在坐標系A(chǔ)中的坐標為(-4,-2,0),那么，坐標系A(chǔ)中的某點$P(P_x,P_y,P_z)$在坐標系B中的坐標為多少？
一畫圖就知道結(jié)果了，A坐標系沿X正向平移了-4，沿Y正向平移了-2，那么A中的某點$P(P_x,P_y,P_z)$在坐標系B中的坐標為$(P_x+4,P_y+2,P_z)$ ，即相當于A坐標系移動到B坐標系的逆過程。

總結(jié)

以上是生活随笔為你收集整理的【自动驾驶】29.坐标变换与坐标轴旋转的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

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