隨著自主機(jī)器的發(fā)展，我們可以在生活中經(jīng)?？吹阶灾鳈C(jī)器的應(yīng)用。有傳統(tǒng)應(yīng)用的倉(cāng)庫(kù)工廠AMR，機(jī)械臂，銀行酒店里面的服務(wù)機(jī)器人，家庭機(jī)器人，無(wú)人物流車(chē)，自主礦卡等等。不同的自主機(jī)器，軟件架構(gòu)的方案也不一樣，但核心的模塊定位，導(dǎo)航，感知，控制等都是相通的。

定位模塊是自主機(jī)器最核心的模塊之一，定位又包括全局定位和局部定位，對(duì)于自主機(jī)器，其精度需要達(dá)到厘米級(jí)別。本文我們將討論全局定位，即確定自主機(jī)器在全局下的位置。傳統(tǒng)的低速自主機(jī)器，類(lèi)似于AMR等，其采用的定位方式通常以SLAM（simultaneous localization and mapping）的方法進(jìn)行同時(shí)建圖和定位，但是該方法實(shí)現(xiàn)代價(jià)高，難度大，并不適用于室外自主機(jī)器，類(lèi)似于無(wú)人物流車(chē)，園區(qū)接駁車(chē)等的實(shí)時(shí)高精度定位需求。這些室外自主機(jī)器行駛速度快，距離遠(yuǎn)，環(huán)境復(fù)雜，使得SLAM的精度下降，同時(shí)遠(yuǎn)距離的行駛將導(dǎo)致實(shí)時(shí)構(gòu)建的地圖偏移過(guò)大。因此，如果在已有高精度的全局地圖地圖的情況下進(jìn)行自主機(jī)器的定位，將極大的簡(jiǎn)化該問(wèn)題。

因此，將問(wèn)題分為獨(dú)立的兩部分：建圖Mapping和定位Matching。NDT是一種點(diǎn)云配準(zhǔn)算法，可同時(shí)用于點(diǎn)云的建圖和定位。

CUDA-NDT

正態(tài)分布變換算法（Normal Distributions Transform， NDT） 同ICP算法的功能一致，即，用于計(jì)算兩幀點(diǎn)云數(shù)據(jù)之間的坐標(biāo)變換矩陣，從而能夠使不同的坐標(biāo)下的點(diǎn)云數(shù)據(jù)合并到同一個(gè)坐標(biāo)系統(tǒng)中。不同的是NDT算法對(duì)初值不敏感，且不需要進(jìn)行對(duì)應(yīng)點(diǎn)的特征計(jì)算，所以速度較快。NDT算法使用應(yīng)用于 3D 點(diǎn)統(tǒng)計(jì)模型的標(biāo)準(zhǔn)優(yōu)化技術(shù)來(lái)確定兩個(gè)點(diǎn)云之間最可能的配準(zhǔn)。NDT算法和 ICP算法可以結(jié)合使用，以提高配準(zhǔn)精度和速度。首先，NDT算法可用于粗配準(zhǔn)，得到轉(zhuǎn)換參數(shù)；然后使用ICP算法結(jié)合參數(shù)進(jìn)行精細(xì)配準(zhǔn)。為了改進(jìn)NDT算法在NVIDIA Jetson上的性能，我們推薦使用基于CUDA加速的CUDA-NDT。

使用CUDA-NDT

以下是CUDA NDT的使用實(shí)例

我們需要初始化相關(guān)的類(lèi)對(duì)象，設(shè)置相關(guān)的參數(shù)，并調(diào)用接口函數(shù)。

cudaNDT ndtTest(nPCountM, nQCountM, stream);
ndtTest.setInputSource(source);
ndtTest.setInputTarget(target);
ndtTest.setResolution(resolution);
ndtTest.setMaximumIterations(nr_iterations);
ndtTest.setTransformationEpsilon(epsilon);
ndtTest.setStepSize(step_size);
    ndtTest.ndt(cloud_source, nPCount,
               cloud_target, nQCount, guess,
               transformation_matrix, stream);

CUDA-NDT 計(jì)算的輸出是 transformation_matrix，代表的含義如下：

源點(diǎn)云(P)* transformation_matrix = 目標(biāo)坐標(biāo)系的點(diǎn)云(Q)
因?yàn)榧す忸?lèi)型的輸出點(diǎn)云的數(shù)量為固定值，所以CUDA-NDT在輸出化的時(shí)候，要求輸入兩幀點(diǎn)云的最大數(shù)量，從而分配計(jì)算資源。

class cudaNDT
{
public:
    /*
       nPCountM and nQCountM are the maximum of count for input clouds
       They are used to pre-allocate memory.
    */
    cudaNDT(int nPCountM, int nQCountM, cudaStream_t stream = 0);
    ~cudaNDT(void);
void setInputSource (void *source);
void setInpuTarget (void *target);
void setResolution (float resolution);
void setMaximumIterations (int nr_iterations);
void setTransformationEpsilon (double epsilon);
void setStepSize (double step_size);
    /*
    cloud_target = transformation_matrix * cloud_source
    When the Epsilon of transformation_matrix is less than threshold,
    the function will return transformation_matrix.
    Input:
        cloud_source, cloud_target: data pointer for points cloud
        nPCount: the points number of cloud_source
        nQCount: the points number of cloud_target
        guess: initial guess of transformation_matrix
        stream: CUDA stream
    Output:
        transformation_matrix: rigid transformation matrix
    */

    void ndt(float *cloud_source, int nPCount,
            float *cloud_target, int nQCount,
            float *guess, void *transformation_matrix,
            cudaStream_t stream = 0);
    void *m_handle = NULL;
};

經(jīng)過(guò)CUDA加速的NDT速度對(duì)比微加速版本提升了4倍左右，請(qǐng)參考以下Table2性能對(duì)比，經(jīng)過(guò)NDT匹配的點(diǎn)云效果對(duì)比請(qǐng)參考Figure1和2。

關(guān)于作者

Lily Li 正在為 NVIDIA 的機(jī)器人團(tuán)隊(duì)處理開(kāi)發(fā)人員關(guān)系。她目前正在 Jetson 生態(tài)系統(tǒng)中開(kāi)發(fā)機(jī)器人技術(shù)解決方案，以幫助創(chuàng)建最佳實(shí)踐。

Haoyu Deng 是 NVIDIA 的CUDA開(kāi)發(fā)工程師。目前，他正與 TSE 中國(guó)團(tuán)隊(duì)合作，通過(guò)CUDA開(kāi)發(fā)優(yōu)化軟件性能的解決方案。

審核編輯：郭婷