基于深度視覺的AGV載具高精度位姿計算方法研究

廣 告 | 點擊 查 看

摘要：為解決AGV（Automated Guided Vehicle）在非結構化場景下對偏移托盤的精確定位與自主叉取問題，本文提出一種基于低成本深度相機傳感器的視覺高精度位姿計算方法。該方法依賴于前序感知模塊輸出的托盤插孔像素坐標。首先，通過張正友標定法獲取相機內參，并結合深度圖像，將插孔的2D像素坐標映射到3D相機坐標系。繼而，根據三個托盤支撐柱中心的3D空間坐標，構建坐標系轉換和姿態計算，以實現對末端作業載具端面中心點相對于貨叉上平面的高度進行精準測量，計算托盤相對于AGV的精確位置（X,Z）、偏航角（θ）和高度。實車實驗表明，在1.2m至1.7m距離、±30°偏角、±600mm偏移的多種工況下，該系統最終叉取的位置誤差能穩定在10mm以內，角度誤差小于1°，顯著提升了AGV末端作業的精度與成功率；對于各類不同高度的載具，其測量誤差能夠控制在±10毫米以內，有效解決了AGV在無固定高度智能物流應用場景中的關鍵問題。

關鍵詞：AGV；托盤定位；位姿估計；深度視覺；路徑規劃；貝塞爾曲線

作者：張玉1 袁正1,2

1安徽合力股份有限公司

2合力工業車輛（上海）有限公司

題眼廣 告 | 點擊查看

一

引言

自動導引運輸車（Automated Guided Vehicle,AGV）作為一類智能工業機器人，廣泛應用于倉儲物流中的裝卸搬運作業[1-3]。其具備自動化任務調度、自主導航控制、路徑動態規劃以及實時安全避障等優勢功能[4-5]。隨著技術的持續演進，AGV系統進一步引入了末端作業精確定位、實時動態避障與路徑智能優化等新技術[6]，

不斷拓展其應用場景邊界。其中，對AGV末端作業載具的精準定位尤為關鍵，需在復雜場景中實時檢測載具的位置、偏航角及高度信息，從而實現AGV在高難度、非結構化場景中的可靠應用。

在AGV末端作業定位技術方面，現有研究多集中于標準化、結構化地面環境下的托盤識別問題，常采用深度視覺、激光雷達或毫米波雷達等傳感器方案，能夠在限定作業范圍內實現對托盤的識別與定位[7]。基于傳統方法的托盤位姿估計技術主要依賴特征提取與幾何建模，通過分析圖像或點云中的托盤形狀特征來實現位姿估計[8-10]。隨著深度相機與視覺成像技術的發展，基于視覺的托盤檢測方法逐漸普及。該方法利用深度相機采集環境數據，并依據托盤的結構特征實現目標與背景的有效分離。例如，武文漢[11]采用TOF相機設計了倉儲托盤相似度匹配算法，借助多相機系統檢測AprilTag標簽以實現托盤位姿定位，但該方法存在點云處理耗時較長、標簽易損壞且維護復雜等問題；Bellomo等人[12]提出了激光雷達與視覺數據融合的算法，通過實時生成高連續性路徑提升目標識別精度，不僅顯著提高了識別準確率，也增強了系統在動態環境中的穩定性；Kato等人[13]構建了一種基于主動雙目視覺傳感器的三維姿態估計算法，該方法融合了點云預處理與自適應高斯加權特征提取等技術，能夠在復雜多變場景下實現精準的托盤姿態估計；王明俊等人[14]則提出一種融合二維碼標識、RGB-D數據與自適應位姿鎮定算法的系統，有效提升了叉車AGV在自動運輸過程中的檢測精度、魯棒性與搬運效率。

隨著AGV叉車應用場景逐漸向混合、復雜的搬運環境拓展，其末端作業面臨更高要求，尤其是在軟包多層堆疊場景中，常出現位置偏移、姿態傾斜等不確定狀態。現有方法多需預設堆疊高度或依賴輔助標簽進行定位，作業時AGV按固定高度抬升取貨，適應性有限，難以應對高度動態變化的實際場景。因此，實現托盤高度的實時精準測量，已成為推動多層堆疊智能物流系統落地的關鍵技術挑戰。

為此，本文基于低成本深度相機，提出一種視覺高精度位姿計算方法，旨在解決托盤相對于AGV的精確定位與姿態估計問題。該方法首先通過張正友標定法獲取相機內參，并依據深度圖像信息將托盤插孔的二維像素坐標映射至三維相機坐標系。進而，根據三個托盤支撐柱中心在三維空間中的坐標，構建坐標系變換與姿態解算模型，最終實現末端作業載具端面中心點相對于貨叉上平面的高度精確測量。

二

AGV末端作業流程

為解決多層堆疊末端作業應用場景，本文采用低成本深度相機作為傳感器，安裝于貨叉的根部。如圖1所示，采用低成本深度相機方案獲取RGB圖像和深度數據D數據，實現復雜場景下的多融合處理方案定位末端作業對象，形成統一的姿態計算和局部路徑規劃方法，控制車體運動完成復雜末端作業過程。

圖1 末端作業對象識別、定位與路徑規劃系統流程

首先，在離線階段完成模型的訓練與數據準備工作，包括數據集的構建與標注、模型設計，并將訓練完成的識別模型與分割模型分別部署至國產芯片RK3568的識別模塊與分割模塊中。同時，設計相機自動標定算法，預先完成相機安裝后相對于車體的外參標定。

其次，當AGV叉車行駛至取貨或放貨作業的前置位置時，通過深度相機采集待操作托盤所在區域的RGB圖像及深度數據；對RGB圖像進行托盤識別與區域分割，提取圖像中托盤支撐柱區域的中心點像素坐標。

最后，基于中心點像素坐標及其對應深度信息，計算托盤相對于相機光心水平面的高度；結合貨叉與相機的相對安裝高度，準確計算多層堆疊場景下托盤插孔中心相對于地面的實際高度；依據AGV車體叉尖的實時高度，動態計算貨叉需舉升或下降的位移量，從而在叉取貨物過程中實時調整叉尖高度，完成貨物拾取。該方法通過多模態數據融合與場景理解，實現對末端作業對象的精準定位，并結合姿態計算與局部路徑規劃形成統一控制策略，指導車體運動以完成復雜環境下的末端作業任務。

三

高精度位姿計算

1.相機外參標定

以車體中心為參照物，標定深度相機的外參，包含相機相對車體中心的水平偏移dx、高度偏移dy（又稱h）、垂直偏移dz、俯仰角(pitch)、滾動角(roll)、偏航角(yaw)。相機外參表現為計算相機坐標與車體中心坐標系變換關系，即求解兩個三維坐標系間的變換為剛體運動，另一種表示為外參用旋轉矩陣R和平移矩陣T表示。

為保證無人叉車在取貨點時能夠采集到末端作業對象插孔區域的全部畫面，深度相機盡可能安裝于叉車叉尖中心和車體交點位置，高度也盡可能與叉尖同高。考慮不同AGV車體結構的差異，相機安裝到AGV固定位置，需要進行相關參數標定，外參數標定是求解相機坐標系間變換關系的過程。包括相機相對車體中心的6個自由度變量。本文采用棋盤格標定裝置，放置在貨叉之間的車體中心位置，標定板平面與中心對齊并垂直于貨叉。

考慮不同AGV車體結構的差異，相機安裝到AGV固定位置，需要進行相關參數標定，包含以下步驟：

（1）構造棋盤格標定板，標定板放于車體貨叉垂直，與車體中心位置系重合或者平行（平行時需要測量偏移量?z，重合時為0）；

（2）采集棋盤格圖像，根據圖像處理方法完成棋盤格角點檢測，得到多個棋盤格角點坐標（采集12個棋盤格角點，如圖2所示）；考慮深度相機測量誤差，每個點取周圍包括自身9個點深度平均值作為當前棋盤格點的深度，并根據相機內參，轉化為相機坐標系下的坐標；

圖2 黑白棋盤標定板示意圖

（3）選取棋盤格在同一高度的點A和B的坐標，通過深度信息計算兩點在垂直地面方向的高度差?H，結合A與B點測量物理距離L，即可計算相機滾動角(roll)，公式如下：

其中dA和dB分別表征已知A點和B點標定位置的像素深度值，通過相機深度信息獲取；pxA和pxB分別表征已知A點和B點標定位置的水平方向的像素坐標。fx表征深度相機的水平像素焦距，Cx表征深度相機光心在像素坐標系中的坐標。

（4）選取棋盤格在同一垂直的點A和C的坐標，計算相機相對車體中心的俯仰角(pitch)、偏航角(yaw)，公式如下：

dA和dC分別表征已知A點和C點標定位置的像素深度值（標定位置和相機平面的距離），pxA和pxC分別表征已知A點和C點標定位置的水平方向的像素坐標，pyA和pyC分別表征已知A點和C點標定位置的垂直方向的像素坐標，fx和fy分別表征深度相機的水平和垂直方向的像素焦距，Cx和Cy分別表征深度相機光心在像素坐標系中的坐標。

（5）根據測定俯仰角(pitch)、滾動角(roll)、偏航角(yaw)，選取棋盤格中的任意一點計算水平偏移（dx）、相對高度h（dy）和dz，利用公式（4）～公式（6）。

為減少外參標定時存在的誤差，重復以上步驟多次，通過求取平均值等方式得到最終外參。

2.載具位姿計算

深度相機中數據包含圖像RGB數據和每個像素的深度數據。設P為三維空間內任意一點，在深度相機下的像素坐標為（u，v）其深度值為d。由于物體在圖像中的坐標位置以像素為單位，需要將點P的相機坐標值轉換為像素坐標值，這個過程采用小孔成像模型來描述。

在相機坐標系下，設P點三維空間坐標為(Xc，Yc，Zc)，

其中，u和v分別為P點在相機圖像像素橫坐標和縱坐標，d為P點在相機坐標系測量的深度值；fx和fy分別表征深度相機的水平和垂直方向的像素焦距，Cx和Cy分別表征深度相機光心在像素坐標系中的坐標。按照矩陣方法表示：

即為相機內參矩陣，是相機自身固有系數，論文直接使用相機廠家給定，也可以通過張正友標定方法進行標定獲取。

在車體坐標下，設P點三維坐標為（XA，YA，ZA），則在相機坐標系和車體坐標系下P點的兩個三維坐標系間的變換為剛體運動，用旋轉矩陣和平移矩陣表示：

其中，R是3×3矩陣，表示車體坐標系相對于相機坐標系下選擇矩陣；T是三維向量，表示車體坐標系相對于相機坐標系的平移關系；[R|T]表示車體與相機間的外參矩陣。

根據上一節中深度相機外參標定的6個參數，轉化為旋轉矩陣和平移矩陣，具體計算方法如下：

設相機相對車體中心的空間三個軸x，y，z的歐拉角分別為θx(roll)、θy(pitch)、θz(yaw)，采用內旋的方式沿著z-y-x的順序旋轉，正弦值、余弦值分別為sx、cx、sy、cy、sz、cz那么旋轉矩陣為：

根據相機相對車體中心的水平偏移（dx）、相對高度h（dy）和垂直偏移（dz+?z，?z為標定板中心與車體中心距離，標定時測量得到），得到平移矩陣為

由公式（4）和（6）計算三維空間中P點的像素坐標和深度信息與車體坐標轉化關系如下：

綜合已知的相機內參K、相機相對車體中心的旋轉矩陣R和平移矩陣T，將托盤中心點像素坐標O(u,v)及深度d，通過代入公式（17），得到該點在車體坐標系下的3D坐標，得到托盤腿中心點O在車體坐標下的坐標O(XA,YA, ZA)。

在實際工程場景，根據導航控制技術，AGV車體實時定位在世界坐標系下位姿信息，考慮車體行駛水平面情況，通常只考慮車體的二維平面的位置和航向角，即（x,y,θ）三個信息，即車體中心相對世界坐標的計算簡單計算為二維平面的旋轉和平移問題。當AGV車體到達取放貨的前置點時，末端作業系統根據車體當前在世界坐標下的（x,y,θ），計算車體坐標下的托盤坐標O(XA,YA,ZA)中二維坐標O(XA,YA)在世界坐標下位置O(Xw,Yw)；ZA表示在車體坐標系下，托盤中心點相對地面高度，與世界坐標系的高度相同。

在相機坐標系下，托盤中心點相對于相機中心的高度（?H）受相機俯仰角(pitch)、滾動角(roll)的影響，測量：

表1 末端作業AGV插入托盤目標檢測結果

其中，d為托盤中間腿的中心O點深度值；pitch和roll分別為相機相對車體的俯仰角和標定滾動角，在安裝標定時確定；px和py是托盤中間腿的中心O點的像素坐標，fx和fy分別表征深度相機的水平和垂直方向的像素焦距，Cx和Cy分別表征深度相機光心在像素坐。

四

實驗測試

1.實驗設置

實驗平臺選用國產低成本邊緣計算終端RK3568，通過RKNN模型轉換工具實現算法驗證、測試和實測搭載。選用Eagle-M4深度相機，分辨率為960×540，幀率為15，深度數據測量有效范圍為500mm至4500mm。如圖3所示，為驗證所提算法的適用性，本實驗構建了貼合實際物流場景特征、具備高度不確定性與多偏轉角度的托盤多層堆疊工況。

圖3 無固定高度與多偏轉角度堆疊實驗場景

2.載具位姿計算

為驗證末端作業中，所測量的托盤相對車體的位姿的精度，托盤中心與車體中心之間的距離設置1.2m、1.5m和1.7m三種位點，在每一個位點托盤偏轉角度設置-30°、-20°、-10°、0°、10°、20°、30°共7種情形。如表2，在不同距離和托盤偏轉角度的場景下，AGV末端作業系統通過多項實驗測試。當AGV完成末端作業、貨叉進入托盤后，測量貨叉相對托盤中心點左右偏差以及貨叉垂直進叉的角度誤差。通過表2可以看出，基于深度學習與圖像處理分割設計的托盤識別和定位方法，能夠控制左右距離誤差10mm之內，角度誤差控制在1°之內，滿足各類復雜AGV末端作業精度。

3.載具高度測量

考慮相機傳感器本身隨著距離增加，深度值測量誤差也增大。考慮到相機的測量范圍，實驗選取標準托盤作為載具，在三種常用距離環境下進行測試，分別選取測試托盤中心與相機中心的距離為1.8m、2.4m和2.8m。

為排除其他誤差干擾，實驗將托載具于相機正前方進行測試，即載具無偏轉角度、無左右偏移距離。載具高度在100mm至1500mm范圍內，以100mm為間隔進行高度誤差測量。考慮相機單幀數據測量存在較大誤差，如圖4所示，實驗中分別通過單幀深度數據與融合10幀深度數據的分布對比進行驗證。在圖4中，d表示單幀深度數據高度測量誤差情況；optimize d表示融合10幀深度數據的高度測量誤差情況。

圖4 單幀和多幀融合的載具高度測量誤差

（1）單幀深度數據情況：當距離為2800mm時，高度測量誤差較大，誤差絕對值集中在12～20mm；當距離為2400mm（實際AGV末端作業常見距離）時，誤差絕對值集中在10mm以內；當距離為1800mm時，誤差絕對值集中在5mm以內。

（2）融合10幀深度數據：通過多幀數據融合優化后的試驗，能夠大大降低高度測量誤差，從圖4中可以看出各個距離段高度測量誤差能夠控制在5mm以內。

考慮到實際作業中托盤相對于車體中心存在位置偏移與旋轉角度，實驗選取常見AGV作業距離范圍，即d=2400mm，并在該距離下對不同高度的托盤進行高度測量。由于相機安裝于貨叉根部中間位置，與車體中心線對齊，托盤相對車體中心線左右對稱偏移。實驗選取一側偏移600mm、300mm和0mm三個位置點，在每個位置點進行托盤偏轉角度為-30°、-20°、-10°、0°、10°、20°和30°的實驗驗證，測試結果如圖5所示。

圖5 各種偏移量和旋轉角度載具的高度測量誤差

從圖5（a）可知，當載具相對車體中心的水平偏移距離為600mm時，旋轉角度的絕對值越大，高度測量誤差越大，最大誤差在20mm之內；旋轉角度在±10°和0°，測量高度誤差較小。從圖5（b）可知，當載具相對車體中心的水平偏移距離為300mm時，旋轉角度的絕對值越大，高度測量誤差越大，最大誤差在15mm之內；旋轉角度在±10°和0°，測量高度誤差在10mm之內；從圖5（c）可知，當載具相對車體中心的水平無偏移時，旋轉角度的絕對值越大，高度測量誤差越大，最大誤差在15mm之內；旋轉角度在±10°和0°，測量高度誤差在5mm之內。

實際AGV作業場景上，AGV在末端作業的前置點，載具的水平偏移距離在300mm之內，旋轉角度在±10°之內，從各項試驗表面，測量誤差能夠控制在10mm之內，滿足各項無固定載具的高度測量要求。

五

結論

本文針對非結構化場景下AGV對偏移托盤的定位與自主叉取問題，提出了一種基于低成本深度相機的視覺高精度位姿計算方法。實驗結果表明，在1.2m至1.7m距離、±30°偏角及±600mm偏移等多種工況下，該系統叉取位置誤差穩定在10mm以內，角度誤差小于1°，能夠適應不同高度的載具，測量誤差控制在±10mm以內。該方法有效提升了AGV在復雜場景下的作業精度與成功率，解決了AGV在非標準庫位下的叉取難題，為無固定高度智能物流應用提供了可行的技術解決方案，提升AGV在柔性化物流場景中的適應性具有重要價值。

參考文獻:

[1] 陳凱,陳韋志.智能倉儲認證引領物流行業高質量發展[J].質量與認證,2024, (05):30-31.

[2] 郝艷莉.面向“工業 4.0”智慧園區智能倉儲物流建設的發展對策[J].電氣時代,2022, (08):95-97.

[3] 張紅瓊,白國星.基于非線性模型預測控制的無人叉車路徑跟蹤[J].現代制造工程,2025,(04):45-52.

[4] 程樂平,李歡.智能倉儲物流中機器人技術的應用與發展[J].信息系統工程, 2023, (07):43-46.

[5] 白斌.基于數字孿生智慧電表倉儲系統設計與AGV路徑規劃的研究[D].浙江:杭州電子科技大學,2025.

[6] Qian Libing,XU Jinbao,XU Hongli,et al. A method for measuring the height of AGV end vehicle based on depth vision[J].High Technology Letters,2025,31(02):204-210.

[7] 呂盛強.基于視覺的叉車托盤定位與取放應用研究[D].成都:西華大學, 2022:13-29.

[8] de Morais M V B, dos Santos S D, Pires R. A computer vision system for pallets verification in quality control[J]. International Journal of Precision Engineering and Manufacturing, 2023, 24(7): 1221-1234.

[9] Shen K, Qiu Q, Wu Q, et al. Research on the development status of AGV parking robot based on patent analysis[C]//Journal of Physics: Conference Series. IOP Publishing, 2021, 1905(1): 012018.

[10] Mohamed, Alessio Capitanelli, Fulvio Mastrogiovanni, et al. Detection, localisation and tracking of pallets using machine learning techniques and 2D range data[J]. Neural Computing and Applications,2019,32(13).

[11] 武文漢.基于視覺的倉儲托盤檢測方法研究[D].上海交通大學,2018.

[12] Bellomo N, Marcuzzi E, Baglivo L, et al. Pallet pose estimation with LIDAR and vision for autonomous forklifts[J]. IFAC Proceedings Volumes, 2009, 42(4): 612-617.

[13] Kato T, Morisawa M. ICP-Based Pallet Tracking for Unloading on Inclined Surfaces by Autonomous Forklifts[C]//2024 IEEE/SICE International Symposium on System Integration (SII). IEEE, 2024: 1504-1510.

[14] 王明俊.基于視覺的倉儲托盤檢測與移動機器人自適應位姿鎮定[D].浙江:杭州電子科技大學,2022.

———— 物流技術與應用融媒 ————

編輯、排版：王茜

本文內容源自

歡迎文末分享、點贊、在看！轉載請聯系后臺。

廣告宣傳