張吉穩 段小明 黃旭東
0 引言自動化碼頭中，AGV 體積龐大（長寬大於45 英尺貨櫃）、運動靈活、運動密集，某些工況下，AGV與其他設備之間的間距不足20 cm。因此，在自動化碼頭作業過程中，為了避免與設施及其他AGV發生碰撞，AGV的安全防撞以及高精度的防撞能力顯得尤為重要。
根據車輛管理系統VMS 的需求，結合導航系統的能力，推算出防撞傳感器的左右偏轉角標定誤差要控制在0.05°以內，左右偏移量誤差要控制在2 mm 以內。
由於雷射防撞傳感器探測到的每個障礙點都要統一到AGV 車身坐標系中，因此，必須通過精確標定準確獲取雷射防撞傳感器與AGV 的精確位姿關係。
選用德國Sick 公司的LMS511 作為雷射防撞傳感器，該傳感器可以探測從-5°到185°內的一個平面。在防撞傳感器相對於AGV車身坐標系的6 個自由度中，僅相對車身的橫向偏移和左右偏轉角兩個自由度對防撞精度影響最大。因此，本文主要對這兩個自由度進行自動標定，其他4 個自由度將採用設計值或根據水平尺等普通工具安裝及測量而得到。
目前，常用的標定方法主要是對空間三個及以上的同名控制點單次進行標定[1-3]，由於受到雷射入射角、物體表面粗糙度及表面材質等多因素的影響，LMS511本身就存在幾釐米的測量誤差，該標定方法誤差相對較大。陶雪嬌等[4] 通過最小二乘擬合方法提高標定精度，但是仍難以滿足本文中的精度要求。本文結合自動化碼頭實際情況，提出一種全新的高精度自動標定方法。
1 自動標定系統流程選用一條平整的車道作為標定車道，將標定板精確安裝在平行於車道的中心線且距離車道中心線a 的位置，然後用Leica 全站儀獲取標定板相對於車道的精確位置關係。將AGV 駛入標定車道的標定位置，同時保證AGV 的停車誤差在允許範圍內。選定車頭或車尾的LMS511 雷射防撞傳感器，啟動一鍵自動標定，待標定結果達到收斂條件，自動停止標定，並發出自動標定結束的標識。若超過一定的次數仍然沒有達到收斂條件，發出標定失敗的標識。自動標定流程如圖1 所示。
圖 1 自動標定流程
2 偏轉角自動標定方法2.1 坐標系定義、標定板放置及停車如圖2 所示，以AGV 中心作為坐標系的原點，x軸正方向指向AGV 車頭，y 軸正方向指向AGV 左端。前後雷射坐標系的原點是AGV 坐標系的x 軸與AGV前後車體外端的交點，雷射坐標系的x 軸正方向都指向AGV 車體外端。前方雷射坐標系的y 軸正方向指向AGV 左端，後方雷射坐標系的y 軸正方向指向AGV 的右端。
將AGV 精確停靠在車道上，然後在距離AGV 車道中心線為a 的位置，精確放置兩塊標定板。
圖2 坐標系定義及標定板的放置
2.2 求取標定板在AGV 坐標系的直行方程通過Leica 全站儀測量標定板上若干點在全局坐標系中的坐標，記為
將AGV 準確停放在預先埋設於地面並寫入編號及坐標的磁釘上，安裝在AGV 前後的RFID 感應天線可以探測到天線下方的磁釘在Global 坐標系中的坐標，同時還可以獲取到磁釘在前後RFID 感應天線坐標系中的坐標。根據磁釘與AGV 前後RFID 感應天線中心的位置關係，結合磁釘在Global 坐標系中的坐標，可以計算出AGV 前後RFID 天線中心點在Global 坐標系中的坐標，分別記為(x RFID_F_Global，y RFID_F_Global) 和(x RFID_R_Global，y RFID_R_Global)，由於前後RFID 感應天線相對於AGV 中心對稱安裝，AGV 中心點即為前後天線中心線的中點，記為(xAGV_Global，yAGV_Global )，前後RFID 中心線與Global坐標系x 軸的夾角記為α ，於是從Global 坐標系轉換到AGV 坐標系的轉換矩陣為
假設AGV 到前後雷射傳感器的位姿轉換矩陣分別記為
和
。於是，標定板上若干點在 AGV 坐標系中的坐標為
將標定板在Sick 雷射坐標係數據點坐標
和
Sick A 先後通過最小二乘法線性擬合得到標定板在前後Sick 雷射坐標系中的直線方程。假設AB 、CD 兩個標定板轉換成前後Sick 雷射坐標系中的直線方程，分別記為
2.3 標定參數的實時計算1）查找反光板的雷射有效探測範圍在標定過程中，要求放置標定板一側不能有人和物體進入。根據距離值的跳變情況，自動查找前後雷射的有效探測範圍。
2）雷射測量值轉換為理想坐標 系中的坐標前後雷射返回的測量值分別用極坐標（d F, θ F）和（d R, θ R）表示，前後雷射實際安裝位置相對於理想坐標系的x 軸偏移量、y 軸偏移量、z 軸偏轉角分別用Δx F、Δy F、Δθ F 和Δx R、Δy R、Δθ R 表示， 其中Δx F 和Δx R 選用設計值，Δy F、Δy R、Δθ F 和Δθ R 的初始值全部為0，於是前後雷射測量值分別轉換成理想LMS511 直角坐標系中的坐標的轉換關係為
3）初步擬合標定板的直線方程假設待擬合的標定板的直線方程為
前後雷射探測到的標定板的N 組極坐標數據, 經過式(5) 或式(6) 轉換為對應的理想坐標系的坐標( ) i i x，y後，利用最小二乘法進行線性擬合，可以得到斜率k 和截距b 的最佳估算值為
即初步擬合後的直線方程為
4）根據距離的3σ 法則濾波後再擬合反光板上( ) i i x，y 被擬合點到擬合直線的距離為
假設距離i d 的均值和標準偏差分別用d 和表示，即
當3 i d > σ 時，認為該點屬於幹擾點，剔除該點。經過濾波後，針對剩餘的點，採用上述相同的方式進行最小二乘法直線擬合，計算出前後雷射探測出的標定板的直線方程為
5）計算新的標定參數結合式(4) 和式(12)，可以得到雷射偏移現有標定參數的偏移量分別為
將1 000 次相對於現有標定參數的偏移量的結果求取均值後，作為新的現有可行標定參數的偏移量，分別用
表示。
2.4 自動計算最終標定結果若在現有可行標定參數的基礎上, 再次標定的可行標定參數滿足下列條件，則代表達到收斂條件，標定結束。
如果前部（或後部）雷射的前後兩次可行標定參數的偏移量不滿足式(14) 或式(15), 需要對標定參數進行更新。若超過20 次還沒有達到式(14) 或式(15) 的收斂條件，則表示標定失敗。
假設上一次可行的標定參數分別用Δy F、Δy R、Δθ F 和Δθ R 表示。那麼，經過本輪上述1 000 次標定後，新的可行的標定參數更新為
3 自動標定實驗如圖3 所示，將AGV 行駛到標定位置，並確保AGV 停車誤差在規定範圍內。選定前方雷射防傳感器，啟動一鍵自動標定，標定程序自動提取圖3b 下方的數據作為標定板的有效探測範圍。通過對第6 次可行標定參數和第5 次可行標定參數的比較，達到收斂條件，結束標定。最終擬合的直線方程為y = 0.000291x 1.63
圖3 標定測試
根據上述方法，最終達到收斂條件時，自動標定計算得出的左右偏轉角為-0.001 745 rad，偏移量為0.006 m。
4 結論及誤差分析如表1 所示，通過10 次成功標定數據比較，最大偏轉角誤差的絕對值控制在0.04°，最大偏移量誤差的絕對值控制在0.001 m。
該自動標定方法已經在國內某些全自動化碼頭使用，通過上述自動標定方法，可以將y 軸偏移量的誤差控制在1 mm 以內，左右偏轉角度的誤差控制在0.05°以內，達到VMS 需求。