移動測量平臺激光雷達旋轉與平移參數計算方法
【專利摘要】本發明提出了一種移動測量平臺激光雷達旋轉與平移參數計算方法，包括步驟：選定M個控制點；建立N個狀態變量且第i個記為Qi，令迭代次數n＝0；建立N個速度變量且第i個記為Vi；建立N個局部最優解變量且第i個記為建立N個局部最優變量對應成本且第i個記為Fi；計算成本函數如果令計算最小成本Fm，如果第m個變量的為全局最優解，則Fbest＝Fm，如果Fbest＜ε則退出，輸出Qbest；構造速度變化值，更新速度變量和變量狀態，令n＝n+1，返回計算成本函數。該方法魯莽性強，不需要精確的對值進行初始化也可以收斂；其公式全部是正向計算，不需要進行矩陣求導等復雜運算，提高了計算速度。
【專利說明】移動測量平臺激光雷達旋轉與平移參數計算方法
【技術領域】[0001]本發明涉及一種移動測量平臺系統標定的方法，尤其是對GNSS (全球定位系統)，IMU (慣性導航單元)與Lidar (激光雷達)組成的移動測量平臺，求解激光雷達相對于POS坐標系的旋轉與平移參數的方法。
【背景技術】
[0002]移動測量系統集成激光雷達、GNSS全球定位系統、IMU慣性導航單元等傳感器，可以快速采集道路周邊高精度的點云數據。在移動測量系統中，激光雷達、GNSS、IMU傳感器安裝在一個剛性平臺上，整個平臺安裝在移動車輛上。在車輛前進過程中，同步采集激光點云數據，GNSS原始數據，MU的三軸加速度與三軸角速度，各種數據通過時間進行嚴格同步。在數據處理過程中，把GNSS原始數據與IMU的原始數據首先進行POS解算，獲得一定時間間隔周期的POS數據(位置與姿態數據，包括WGS84坐標與該時刻平臺的姿態)。因為激光雷達采集的數據是用極坐標表示的相對于激光雷達本身設備坐標系的點云坐標，因此，需要把激光雷達坐標系下的點云坐標轉換為WGS84坐標系下的坐標或者東北高坐標系下的坐標。在這一轉換過程中，需要已知激光雷達相對于POS坐標系的旋轉矩陣與平移向量。
[0003]在求解以上問題時，目前用的比較多的是最小二乘法，比如文獻《車載系統激光與POS總體檢較方法》中使用最小二乘法來求解。但是這種方法實現困難，需要對矩陣進行偏微分計算，工作量大，易出錯；另外，在收斂時需要指定初始值，存在不收斂的可能，需要通過多次改變初值進行計算以確保結果的準確性。
【發明內容】
[0004]為了克服上述現有技術中存在的缺陷，本發明的目的是提供一種移動測量平臺激光雷達旋轉與平移參數計算方法，該方法魯莽性強，不需要精確的對值進行初始化，也可以收斂；該方法內的公式全部是正向計算，不需要進行矩陣求導等復雜運算，提高了計算速度。
[0005]為了實現本發明的上述目的，本發明提供了一種移動測量平臺激光雷達旋轉與平移參數計算方法，包括如下步驟:
[0006]SI，選定M個控制點，M為正整數，Pj = (Xj, Yj, Zj)表示激光掃描儀坐標系中第j個
控制點的點坐標，P」點在東北高坐標系中的坐標為巧=(?，^)，Pj點采集時刻平臺的POS
為POSj = (X，y, z, pitch, roll, yaw), I≤j≤M ;所述變量POSj前三個分量表示GNSS天線相位中心在東北高坐標系中的位置，后三個分量表示平臺姿態，既俯仰、橫滾、航向角度；
[0007]S2，建立N個狀態變量，所述N為正整數，第i個狀態變量記為Qi =(Xi, Yi, Zi, Θ i； Qi, Ψ),其中,Xi, yi, Zi表示第i個變量的位置分量，Qi, Qi, Ijri為旋轉角度分量，其中，N,令迭代的次數η = O;
[0008]S3，建立N個速度變量，所述N為正整數，第i個速度變量記為Vi =(ΥΛ Vi7, Vi2, Vi ",V/), I ^ i SN;其中，前三個分量表示位置變化的速度，后三個分量表示旋轉角度變化的速度；
[0009]S4，建立N個局部最優解變量，所述N為正整數，第i個局部最優解變量記為
【權利要求】
1.一種移動測量平臺激光雷達旋轉與平移參數計算方法，其特征在于，包括如下步驟: SI，選定M個控制點，M為正整數，Pj = (Xj, Yj, Zj.)表示激光掃描儀坐標系中第j個控制點的點坐標，Pj點在東北高坐標系中的坐標為Pj，Pj點采集時刻平臺的POS為POSj = (X，y, z, pitch, roll, yaw), I≤j≤Μ,所述變量POSj前三個分量表示GNSS天線相位中心在東北高坐標系中的位置，后三個分量表示平臺姿態，包括俯仰、橫滾、航向角度； S2，建立N個狀態變量，所述N為正整數，第i個狀態變量記為Qi =(Xi, Yi, Zi, Θ i； Qi, Ψ),其中,Xi, Yi, Zi表示第i個變量的位置分量，Qi, Qi, Vi為旋轉角度分量，其中，N,令迭代的次數η = O; S3，建立N個速度變量，所述N為正整數，第i個速度變量記為Vi =(VA Vi7, Vi2, Vi ",V/), I ^ i SN，其中，前三個分量表示位置變化的速度，后三個分量表示旋轉角度變化的速度； S4,建立N個局部最優解變量，所述N為正整數，第i個局部最優解變量記為QfT = Qi，建立N個局部最優變量對應成本，第i個局部最優變量對應成本Fi = + c?，其中，KiSN; S5，計算每個狀態變量Qi的成本函數
2.如權利要求1所述的移動測量平臺激光雷達旋轉與平移參數計算方法，其特征在于，步驟S7的具體方法為: S71，計算速度變量變化值
3.如權利要求2所述的移動測量平臺激光雷達旋轉與平移參數計算方法，其特征在于，I≤C1≤C2≤2，T1, r2 e [O, I]的隨機數.
4.如權利要求3所述的移動測量平臺激光雷達旋轉與平移參數計算方法，其特征在于，C1 = 1.5, C2 = 2。
5.如權利要求2所述的移動測量平臺激光雷達旋轉與平移參數計算方法，其特征在于，I ≤ C3 ≤ 10，50≤ c4≤200。
6.如權利要求5所述的移動測量平臺激光雷達旋轉與平移參數計算方法，其特征在于，c3=2, c4=100。
7.如權利要求2所述的移動測量平臺激光雷達旋轉與平移參數計算方法，其特征在于，C5 < 10。
8.如權利要求7所述的移動測量平臺激光雷達旋轉與平移參數計算方法，其特征在于，C5=L 5。
9.如權利要求1所述的移動測量平臺激光雷達旋轉與平移參數計算方法，其特征在
于，步驟S5中轉換函數
10.如權利要求1所述的移動測量平臺激光雷達旋轉與平移參數計算方法，其特征在于，ε =le_60
【文檔編號】G01C21/20GK103644917SQ201310648382
【公開日】2014年3月19日 申請日期:2013年12月4日 優先權日:2013年12月4日
【發明者】陳翰新, 向澤君, 朱圣, 徐占華, 呂楠, 羅再謙, 龍川, 殷飛, 汪明, 楊元, 王滿杰, 陳漢, 鄭良, 饒鳴, 滕德貴, 梁建國, 劉穎 申請人:重慶數字城市科技有限公司, 重慶市勘測院