1. 旋轉(zhuǎn)矩陣

• 由于激光雷達(dá)獲取的點(diǎn)云數(shù)據(jù)的坐標(biāo)是相對(duì)于激光雷達(dá)坐標(biāo)系的，為了使車(chē)最終得到的點(diǎn)云數(shù)據(jù)坐標(biāo)是在車(chē)坐標(biāo)系下的，我們需要對(duì)點(diǎn)云中每一個(gè)點(diǎn)的坐標(biāo)進(jìn)行坐標(biāo)轉(zhuǎn)換。
• 首先是需要對(duì)坐標(biāo)系進(jìn)行旋轉(zhuǎn)變換，先以二維平面的單位向量坐標(biāo)轉(zhuǎn)換為例，假設(shè)兩坐標(biāo)系中的旋轉(zhuǎn)矩陣為R，旋轉(zhuǎn)角度為$\theta$,點(diǎn)P在$x_{1}o{y}_1$坐標(biāo)(車(chē)坐標(biāo)系)下的坐標(biāo)為$(x_1,y_1)$；點(diǎn)P在$x_{2}o{y}_2$坐標(biāo)（激光雷達(dá)坐標(biāo)系）下的坐標(biāo)為$(x_2,y_2)$,已知點(diǎn)在激光雷達(dá)坐標(biāo)系下的坐標(biāo)$(x_2,y_2)$，可以由以下的坐標(biāo)系的轉(zhuǎn)換關(guān)系得到$(x_1,y_1)$：

旋轉(zhuǎn)矩陣R:將點(diǎn)P在旋轉(zhuǎn)后的坐標(biāo)系下的坐標(biāo)轉(zhuǎn)換為旋轉(zhuǎn)前的坐標(biāo)系下的坐標(biāo)

$$\left[\begin{array}{c}{x}_1\\ y_1\end{array}\right]=R\left[\begin{array}{c}{x}_2\\ y_2\end{array}\right]\text{\hspace{1em}(1)}$$

• 根據(jù)旋轉(zhuǎn)角度$\theta$,由上圖1可得$x_1$和$y_1$為：
  $$\begin{gathered} x_1=x_{2}cos\theta ?y_{2}sin\theta \\ {y}_1=x_{2}sin\theta +y_{2}cos\theta \text{\hspace{1em}(2)} \end{gathered}$$
• 由此可以得出以下的坐標(biāo)轉(zhuǎn)換矩陣等式：
  $$\left[\begin{array}{c}{x}_1\\ y_1\end{array}\right]=\left[\begin{array}{c}cos\theta ,?sin\theta \\ sin\theta ,cos\theta \end{array}\right]\left[\begin{array}{c}{x}_2\\ y_2\end{array}\right]\text{\hspace{1em}(3)}$$
• 旋轉(zhuǎn)矩陣R即為：
  $$R=\left[\begin{array}{c}cos\theta ,?sin\theta \\ sin\theta ,cos\theta \end{array}\right]\text{\hspace{1em}(4)}$$
• 由二維推廣至三維，由右手定則可以想象在上圖1中的O點(diǎn)處有一條垂直于XOY平面指向屏幕的的Z軸：
  

• 那么上述的二維空間內(nèi)的坐標(biāo)旋轉(zhuǎn)便可推廣至三維空間中繞z軸的旋轉(zhuǎn)，旋轉(zhuǎn)角度$\theta$便是歐拉角中的航向角（也稱(chēng)偏航角yaw），由于旋轉(zhuǎn)前后z軸沒(méi)有發(fā)生變換，上述公式（3）可以寫(xiě)為以下形式：
  [ x 1 y 1 z 1 ] = [ c o s θ ? s i n θ 0 s i n θ c o s θ 0 0 0 1 ] [ x 2 y 2 z 2 ] (5) \begin{bmatrix} x_1\\y_1\\z_1 \end{bmatrix} =\begin{bmatrix} cos\theta&-sin\theta&0\\ sin\theta&cos\theta&0\\ 0&0&1\\ \end{bmatrix} \begin{bmatrix} x_2\\y_2\\z_2 \end{bmatrix} \tag{5} ?x1?y1?z1?? ?= ?cosθsinθ0??sinθcosθ0?001? ? ?x2?y2?z2?? ?(5)
• 由此可得航向角（也稱(chēng)偏航角yaw）旋轉(zhuǎn)矩陣$R_{yaw}$為：
  R y a w = [ c o s θ ? s i n θ 0 s i n θ c o s θ 0 0 0 1 ] (6) R_{yaw}=\begin{bmatrix} cos\theta&-sin\theta&0\\ sin\theta&cos\theta&0\\ 0&0&1\\ \end{bmatrix} \tag{6} Ryaw?= ?cosθsinθ0??sinθcosθ0?001? ?(6)
  

• 由上述公式（6）同理可以推導(dǎo)出繞坐標(biāo)軸y軸旋轉(zhuǎn)$\beta$（俯仰角pictch）的公式如下:
  [ x 1 y 1 z 1 ] = [ c o s β 0 s i n β 0 1 0 ? s i n β 0 c o s β ] [ x 2 y 2 z 2 ] (7) \begin{bmatrix} x_1\\y_1\\z_1 \end{bmatrix} =\begin{bmatrix} cos\beta&0&sin\beta\\ 0&1&0\\ -sin\beta&0&cos\beta\\ \end{bmatrix} \begin{bmatrix} x_2\\y_2\\z_2 \end{bmatrix} \tag{7} ?x1?y1?z1?? ?= ?cosβ0?sinβ?010?sinβ0cosβ? ? ?x2?y2?z2?? ?(7)
• 由此可得俯仰角pictch旋轉(zhuǎn)矩陣$R_{pictch}$為：
  R p i t c h = [ c o s β 0 s i n β 0 1 0 ? s i n β 0 c o s β ] (8) R_{pitch}=\begin{bmatrix} cos\beta&0&sin\beta\\ 0&1&0\\ -sin\beta&0&cos\beta\\ \end{bmatrix} \tag{8} Rpitch?= ?cosβ0?sinβ?010?sinβ0cosβ? ?(8)
  

• 也可以推導(dǎo)出繞坐標(biāo)軸x軸旋轉(zhuǎn)$\gamma$（橫滾角roll）的公式如下:
  [ x 1 y 1 z 1 ] = [ 1 0 0 0 c o s γ ? s i n γ 0 s i n γ c o s γ ] [ x 2 y 2 z 2 ] (9) \begin{bmatrix} x_1\\y_1\\z_1 \end{bmatrix} =\begin{bmatrix} 1&0&0\\ 0&cos\gamma&-sin\gamma\\ 0&sin\gamma&cos\gamma\\ \end{bmatrix} \begin{bmatrix} x_2\\y_2\\z_2 \end{bmatrix} \tag{9} ?x1?y1?z1?? ?= ?100?0cosγsinγ?0?sinγcosγ? ? ?x2?y2?z2?? ?(9)
• 由此可得橫滾角roll旋轉(zhuǎn)矩陣$R_{roll}$為：
  R r o l l = [ 1 0 0 0 c o s γ ? s i n γ 0 s i n γ c o s γ ] (10) R_{roll}=\begin{bmatrix} 1&0&0\\ 0&cos\gamma&-sin\gamma\\ 0&sin\gamma&cos\gamma\\ \end{bmatrix} \tag{10} Rroll?= ?100?0cosγsinγ?0?sinγcosγ? ?(10)
• 按照不同的順序?qū)ψ鴺?biāo)軸進(jìn)行旋轉(zhuǎn)可以得到不同的旋轉(zhuǎn)矩陣R，R共有六種形式，分別為
  $$\begin{gathered} R=R_{yaw}{R}_{pitch}{R}_{roll}R=R_{yaw}{R}_{roll}{R}_{pitch} \\ R=R_{pitch}{R}_{yaw}{R}_{roll}R=R_{pitch}{R}_{yaw}{R}_{roll} \\ R=R_{roll}{R}_{pitch}{R}_{yaw}R=R_{roll}{R}_{yaw}{R}_{pitch}\text{\hspace{1em}(11)} \end{gathered}$$
• 按照歐拉角的測(cè)量方式，每次旋轉(zhuǎn)按照車(chē)底盤(pán)坐標(biāo)系的坐標(biāo)軸進(jìn)行旋轉(zhuǎn)（即外旋），外旋旋轉(zhuǎn)矩陣是左乘矩陣，即按照X-Y-Z的順序進(jìn)行旋轉(zhuǎn)的話(huà)，得到的旋轉(zhuǎn)矩陣是$R=R_Z{R}_Y{R}_X$,通過(guò)測(cè)量的得到的歐拉角的角度值就可以計(jì)算出對(duì)應(yīng)的旋轉(zhuǎn)矩陣。

外旋（左乘）：每次旋轉(zhuǎn)繞固定軸旋轉(zhuǎn)
內(nèi)旋（右乘）：每次旋轉(zhuǎn)繞自身旋轉(zhuǎn)后的軸旋轉(zhuǎn)

2. 平移矩陣

已知激光雷達(dá)相對(duì)于車(chē)坐標(biāo)原點(diǎn)的三維坐標(biāo)x,y,z，可以得到激光雷達(dá)與車(chē)坐標(biāo)系的平移矩陣為：
T = [ x y z ] (12) T=\begin{bmatrix} x\\y\\z \end{bmatrix} \tag{12} T= ?xyz? ?(12)

3. 坐標(biāo)系的轉(zhuǎn)換

根據(jù)上述計(jì)算得到的旋轉(zhuǎn)矩陣R和平移矩陣T。設(shè)車(chē)坐標(biāo)下點(diǎn)的坐標(biāo)為$(x_c,y_c,z_c)$激光雷達(dá)下點(diǎn)的坐標(biāo)為$(x_l,y_l,z_l)$即最終的變換形式為：
[ x c y c z c ] = R [ x l y l z l ] + T (13) \begin{bmatrix} x_c \\ y_c\\ z_c \end{bmatrix} =R\begin{bmatrix} x_l \\ y_l\\ z_l \end{bmatrix} +T \tag{13} ?xc?yc?zc?? ?=R ?xl?yl?zl?? ?+T(13)文章來(lái)源地址http://www.zghlxwxcb.cn/news/detail-414967.html

4. 坐標(biāo)轉(zhuǎn)換代碼

• 構(gòu)造旋轉(zhuǎn)平移矩陣對(duì)點(diǎn)云中的點(diǎn)進(jìn)行轉(zhuǎn)換

//坐標(biāo)變換將激光雷達(dá)坐標(biāo)系下的點(diǎn)轉(zhuǎn)換到小車(chē)坐標(biāo)系下
void MainWindow::changePoint(pcl::PointCloud<pcl::PointXYZ>::Ptr lidarCloud,
                 pcl::PointCloud<pcl::PointXYZ>::Ptr carCloud,
                 double yaw,double pitch,double roll,double x,double y,double z)
{
    Eigen::Matrix4f transform=Eigen::Matrix4f::Identity();
    Eigen::Matrix4f transformYaw;
    Eigen::Matrix4f transformPitch;
    Eigen::Matrix4f transformRoll;
    
    //航向角
    transformYaw<<cos(yaw),-sin(yaw),0,0,\
            sin(yaw),cos(yaw),0,0,\
            0,0,1,0,\
            0,0,0,1;
    //俯仰角
    transformPitch<<cos(pitch),0,sin(pitch),0,\
            0,1,0,0,\
            -sin(pitch),0,cos(pitch),0,\
            0,0,0,1;
    //橫滾角
    transformRoll<<1,0,0,0,\
            0,cos(roll),-sin(roll),0,\
            0,sin(roll),cos(roll),0,\
            0,0,0,1;
            
    //旋轉(zhuǎn)矩陣
    transform=transformRoll*transformPitch*transformYaw;
    
    //平移矩陣
    transform(0,3)=x;
    transform(1,3)=y;
    transform(2,3)=z;
    
    //坐標(biāo)轉(zhuǎn)換
    pcl::transformPointCloud(*lidarCloud,*carCloud,transform);
}

到了這里，關(guān)于激光雷達(dá)標(biāo)定(坐標(biāo)系轉(zhuǎn)換)的文章就介紹完了。如果您還想了解更多內(nèi)容，請(qǐng)?jiān)谟疑辖撬阉鱐OY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！