本發(fā)明涉及機(jī)電控制技術(shù)領(lǐng)域，特別涉及一種車體位姿校正方法及裝置。

背景技術(shù)：

agv(automatedguidedvehicle,自動導(dǎo)引運(yùn)輸車)是指裝備有視覺、激光雷達(dá)等傳感器，能夠沿設(shè)定的路徑自動行駛并具有自主避障、承載貨物等功能的輪式移動機(jī)器人設(shè)備。近年，隨著國際、國內(nèi)物流、電商行業(yè)的迅猛發(fā)展，agv在智能無人倉儲、無人配送等應(yīng)用場景中需求巨大。

典型的agv設(shè)備底盤為長方形結(jié)構(gòu)，配備兩套車輪系統(tǒng)，一套為具備導(dǎo)向和承重功能的承重輪系統(tǒng)，主要用于支撐負(fù)載并在agv行進(jìn)過程中起到運(yùn)動導(dǎo)向的作用，其布置于底盤的四個(gè)角上，且四個(gè)車輪在同一平面上；另一套為驅(qū)動輪系統(tǒng)，兩輪分布于底盤中部且關(guān)于車體中軸線左右對稱。通過調(diào)整左右驅(qū)動輪的速度實(shí)現(xiàn)agv設(shè)備的直行、轉(zhuǎn)向、原地掉頭等動作。車體頂面居中位置安裝舉升裝置，用于在運(yùn)動過程中托起貨架。貨架采用四角支撐，貨架底部可容納agv進(jìn)行通過和轉(zhuǎn)向運(yùn)動。車體底面居中位置安裝高速攝像頭，用于快速識別張貼于地面的二維碼導(dǎo)向地標(biāo)并實(shí)時(shí)計(jì)算出當(dāng)前agv的位置和朝向。

技術(shù)實(shí)現(xiàn)要素：

發(fā)明人研究發(fā)現(xiàn)，在現(xiàn)有技術(shù)中，agv在到達(dá)設(shè)定的貨架下方后執(zhí)行以下三個(gè)步驟：第一步，原地轉(zhuǎn)向?qū)④囶^方向轉(zhuǎn)到下一個(gè)指令運(yùn)動方向；第二步，升起舉升裝置將貨架托起；第三步，agv拖帶貨架運(yùn)動到目標(biāo)位置完成貨物運(yùn)送。在以上作業(yè)流程的第一步中，由于運(yùn)動誤差的存在，大部分情況下agv在到達(dá)貨架下方后并沒有嚴(yán)格停在貨架的中心位置，在經(jīng)過轉(zhuǎn)向運(yùn)動后，如果這一誤差仍然沒有消除，將導(dǎo)致在第二步貨架舉升過程中出現(xiàn)貨架頂偏的情況，從而導(dǎo)致agv拖帶貨架運(yùn)行過程中與其它貨架發(fā)生碰撞，嚴(yán)重時(shí)可能導(dǎo)致貨架傾覆造成損失。

本發(fā)明解決的一個(gè)技術(shù)問題是，如何實(shí)現(xiàn)車體在旋轉(zhuǎn)過程中同步校正車體位姿。

根據(jù)本發(fā)明實(shí)施例的一個(gè)方面，提供了一種車體位姿校正方法，包括：將車體的左驅(qū)動輪與右驅(qū)動輪的速度比值確定為隨車體旋轉(zhuǎn)角、車體中心目標(biāo)位置以及車體中心當(dāng)前位置變化的函數(shù)，車體旋轉(zhuǎn)角為車體目標(biāo)前進(jìn)方向的法線方向與車體當(dāng)前前進(jìn)方向之間的夾角；根據(jù)車體旋轉(zhuǎn)角、車體中心目標(biāo)位置以及車體中心當(dāng)前位置的數(shù)值，確定車體的左驅(qū)動輪與右驅(qū)動輪的速度比值，并根據(jù)速度比值為車體的左驅(qū)動輪與右驅(qū)動輪設(shè)定旋轉(zhuǎn)速度。

在一個(gè)實(shí)施例中，將車體的左驅(qū)動輪與右驅(qū)動輪的速度比值確定為隨車體旋轉(zhuǎn)角、車體中心目標(biāo)位置以及車體中心當(dāng)前位置變化的函數(shù)包括：將車體旋轉(zhuǎn)半徑確定為車體旋轉(zhuǎn)角的線性函數(shù)，其中，車體旋轉(zhuǎn)半徑為旋轉(zhuǎn)中心與車體中心之間的距離，車體中心位于車體的左驅(qū)動輪與右驅(qū)動輪的中點(diǎn)；根據(jù)線性函數(shù)、車體的左驅(qū)動輪與右驅(qū)動輪之間的距離，將車體的左驅(qū)動輪與右驅(qū)動輪的速度比值確定為車體旋轉(zhuǎn)角的函數(shù)。

在一個(gè)實(shí)施例中，將車體旋轉(zhuǎn)半徑確定為車體旋轉(zhuǎn)角的線性函數(shù)包括：將車體旋轉(zhuǎn)半徑用車體旋轉(zhuǎn)角、車體旋轉(zhuǎn)角系數(shù)以及常數(shù)項(xiàng)表示；將車體中心的位置隨時(shí)間的變化率確定為車體旋轉(zhuǎn)角的函數(shù)；根據(jù)車體中心的位置隨時(shí)間變化率與車體旋轉(zhuǎn)角的函數(shù)關(guān)系，確定車體中心目標(biāo)位置、車體中心當(dāng)前位置以及車體旋轉(zhuǎn)角的函數(shù)關(guān)系；利用車體中心目標(biāo)位置、車體中心當(dāng)前位置，表示車體旋轉(zhuǎn)角系數(shù)以及常數(shù)項(xiàng)。

在一個(gè)實(shí)施例中，所述車體中心的當(dāng)前位置以及所述車體當(dāng)前前進(jìn)方向通過車體的攝像頭探測確定。

在一個(gè)實(shí)施例中，根據(jù)速度比值為車體的左驅(qū)動輪與右驅(qū)動輪設(shè)定旋轉(zhuǎn)速度包括：將車體的旋轉(zhuǎn)角速度確定為隨時(shí)間變化的函數(shù)；根據(jù)車體旋轉(zhuǎn)角以及車體在各個(gè)時(shí)刻的旋轉(zhuǎn)角速度，確定車體旋轉(zhuǎn)的總時(shí)長；根據(jù)車體旋轉(zhuǎn)的總時(shí)長以及車體旋轉(zhuǎn)的工作周期，確定車體旋轉(zhuǎn)的工作周期數(shù)；在每個(gè)工作周期內(nèi)，根據(jù)速度比值以及車體在各個(gè)時(shí)刻的旋轉(zhuǎn)角速度，為車體的左驅(qū)動輪與右驅(qū)動輪設(shè)定旋轉(zhuǎn)速度。

在一個(gè)實(shí)施例中，將車體的旋轉(zhuǎn)角速度確定為隨時(shí)間變化的函數(shù)包括：根據(jù)車體的屬性，設(shè)定車體勻變速旋轉(zhuǎn)的角加速度以及最大旋轉(zhuǎn)角速度；根據(jù)車體勻變速旋轉(zhuǎn)的角加速度以及最大旋轉(zhuǎn)角速度，確定車體在各個(gè)時(shí)刻的旋轉(zhuǎn)角速度。

根據(jù)本發(fā)明實(shí)施例的另一個(gè)方面，提供了一種車體位姿校正裝置，包括：函數(shù)確定模塊，配置為將車體的左驅(qū)動輪與右驅(qū)動輪的速度比值確定為隨車體旋轉(zhuǎn)角、車體中心目標(biāo)位置以及車體中心當(dāng)前位置變化的函數(shù)，車體旋轉(zhuǎn)角為車體目標(biāo)前進(jìn)方向的法線方向與車體當(dāng)前前進(jìn)方向之間的夾角；速度設(shè)定模塊，配置為根據(jù)車體旋轉(zhuǎn)角、車體中心目標(biāo)位置以及車體中心當(dāng)前位置的數(shù)值，確定車體的左驅(qū)動輪與右驅(qū)動輪的速度比值，并根據(jù)速度比值為車體的左驅(qū)動輪與右驅(qū)動輪設(shè)定旋轉(zhuǎn)速度。

在一個(gè)實(shí)施例中，函數(shù)確定模塊配置為：將車體旋轉(zhuǎn)半徑確定為車體旋轉(zhuǎn)角的線性函數(shù)，其中，車體旋轉(zhuǎn)半徑為旋轉(zhuǎn)中心與車體中心之間的距離，車體中心位于車體的左驅(qū)動輪與右驅(qū)動輪的中點(diǎn)；根據(jù)線性函數(shù)、車體的左驅(qū)動輪與右驅(qū)動輪之間的距離，將車體的左驅(qū)動輪與右驅(qū)動輪的速度比值確定為車體旋轉(zhuǎn)角的函數(shù)。

在一個(gè)實(shí)施例中，函數(shù)確定模塊配置為：將車體旋轉(zhuǎn)半徑用車體旋轉(zhuǎn)角、車體旋轉(zhuǎn)角系數(shù)以及常數(shù)項(xiàng)表示；將車體中心的位置隨時(shí)間的變化率確定為車體旋轉(zhuǎn)角的函數(shù)；根據(jù)車體中心的位置隨時(shí)間變化率與車體旋轉(zhuǎn)角的函數(shù)關(guān)系，確定車體中心目標(biāo)位置、車體中心當(dāng)前位置以及車體旋轉(zhuǎn)角的函數(shù)關(guān)系；利用車體中心目標(biāo)位置、車體中心當(dāng)前位置，表示車體旋轉(zhuǎn)角系數(shù)以及常數(shù)項(xiàng)。

在一個(gè)實(shí)施例中，所述車體中心的當(dāng)前位置以及所述車體當(dāng)前前進(jìn)方向通過車體的攝像頭探測確定。

在一個(gè)實(shí)施例中，速度設(shè)定模塊配置為：將車體的旋轉(zhuǎn)角速度確定為隨時(shí)間變化的函數(shù)；根據(jù)車體旋轉(zhuǎn)角以及車體在各個(gè)時(shí)刻的旋轉(zhuǎn)角速度，確定車體旋轉(zhuǎn)的總時(shí)長；根據(jù)車體旋轉(zhuǎn)的總時(shí)長以及車體旋轉(zhuǎn)的工作周期，確定車體旋轉(zhuǎn)的工作周期數(shù)；在每個(gè)工作周期內(nèi)，根據(jù)速度比值以及車體在各個(gè)時(shí)刻的旋轉(zhuǎn)角速度，為車體的左驅(qū)動輪與右驅(qū)動輪設(shè)定旋轉(zhuǎn)速度。

在一個(gè)實(shí)施例中，速度設(shè)定模塊配置為：根據(jù)車體的屬性，設(shè)定車體勻變速旋轉(zhuǎn)的角加速度以及最大旋轉(zhuǎn)角速度；根據(jù)車體勻變速旋轉(zhuǎn)的角加速度以及最大旋轉(zhuǎn)角速度，確定車體在各個(gè)時(shí)刻的旋轉(zhuǎn)角速度。

根據(jù)本發(fā)明實(shí)施例的又一個(gè)方面，提供了一種車體位姿校正裝置，包括：存儲器；以及耦接至存儲器的處理器，處理器被配置為基于存儲在存儲器中的指令，執(zhí)行前述的車體位姿校正方法。

根據(jù)本發(fā)明實(shí)施例的再一個(gè)方面，提供了一種計(jì)算機(jī)可讀存儲介質(zhì)，計(jì)算機(jī)可讀存儲介質(zhì)存儲有計(jì)算機(jī)指令，指令被處理器執(zhí)行時(shí)實(shí)現(xiàn)前述的車體位姿校正方法。

本發(fā)明提供的車體位姿校正方法，將車體的左驅(qū)動輪與右驅(qū)動輪的速度比值確定為隨車體旋轉(zhuǎn)角、車體中心目標(biāo)位置以及車體中心當(dāng)前位置變化的函數(shù)；根據(jù)車體旋轉(zhuǎn)角、車體中心目標(biāo)位置以及車體中心當(dāng)前位置的數(shù)值，確定車體的左驅(qū)動輪與右驅(qū)動輪的速度比值，并根據(jù)速度比值為車體的左驅(qū)動輪與右驅(qū)動輪設(shè)定旋轉(zhuǎn)速度。從而提供了一種車體位姿校正方法，實(shí)現(xiàn)車體在旋轉(zhuǎn)過程中同步校正車體位姿。

通過以下參照附圖對本發(fā)明的示例性實(shí)施例的詳細(xì)描述，本發(fā)明的其它特征及其優(yōu)點(diǎn)將會變得清楚。

附圖說明

為了更清楚地說明本發(fā)明實(shí)施例或現(xiàn)有技術(shù)中的技術(shù)方案，下面將對實(shí)施例或現(xiàn)有技術(shù)描述中所需要使用的附圖作簡單地介紹，顯而易見地，下面描述中的附圖僅僅是本發(fā)明的一些實(shí)施例，對于本領(lǐng)域普通技術(shù)人員來講，在不付出創(chuàng)造性勞動性的前提下，還可以根據(jù)這些附圖獲得其它的附圖。

圖1示出了車體的結(jié)構(gòu)示意圖。

圖2a示出了現(xiàn)有技術(shù)中車體原地旋轉(zhuǎn)的一個(gè)示意圖。

圖2b示出了現(xiàn)有技術(shù)中車體原地旋轉(zhuǎn)的另一個(gè)示意圖。

圖3a示出了現(xiàn)有技術(shù)中車體進(jìn)行90度旋轉(zhuǎn)后的位姿校正情況。

圖3b示出了現(xiàn)有技術(shù)中車體進(jìn)行180度旋轉(zhuǎn)后的位姿校正情況。

圖4示出本發(fā)明車體位姿校正方法的一個(gè)實(shí)施例的流程示意圖。

圖5a示出本發(fā)明車體角度軌跡的一個(gè)實(shí)施例的示意圖。

圖5b示出本發(fā)明車體角速度軌跡的一個(gè)實(shí)施例的示意圖。

圖6a示出本發(fā)明車體位姿校正過程的一個(gè)實(shí)施例的示意圖。

圖6b示出本發(fā)明車體旋轉(zhuǎn)過程中的一個(gè)實(shí)施例的軌跡示意圖。

圖6c示出本發(fā)明車體旋轉(zhuǎn)過程中的左右驅(qū)動輪的速度變化的一個(gè)實(shí)施例的示意圖。

圖7示出本發(fā)明車體位姿校正裝置的一個(gè)實(shí)施例的結(jié)構(gòu)示意圖。

圖8示出本發(fā)明車體位姿校正裝置的另一個(gè)實(shí)施例的結(jié)構(gòu)示意圖。

圖9示出本發(fā)明車體位姿校正裝置的又一個(gè)實(shí)施例的結(jié)構(gòu)示意圖。

具體實(shí)施方式

下面將結(jié)合本發(fā)明實(shí)施例中的附圖，對本發(fā)明實(shí)施例中的技術(shù)方案進(jìn)行清楚、完整地描述，顯然，所描述的實(shí)施例僅僅是本發(fā)明一部分實(shí)施例，而不是全部的實(shí)施例。以下對至少一個(gè)示例性實(shí)施例的描述實(shí)際上僅僅是說明性的，決不作為對本發(fā)明及其應(yīng)用或使用的任何限制?；诒景l(fā)明中的實(shí)施例，本領(lǐng)域普通技術(shù)人員在沒有作出創(chuàng)造性勞動前提下所獲得的所有其它實(shí)施例，都屬于本發(fā)明保護(hù)的范圍。

發(fā)明人對agv轉(zhuǎn)向的原理進(jìn)行了分析。圖1示出了車體的結(jié)構(gòu)示意圖。如果為agv的左右驅(qū)動輪設(shè)定大小相同方向相反的轉(zhuǎn)動速度，在旋轉(zhuǎn)的任意時(shí)刻，agv中心的運(yùn)動速度恒為0，即agv中心在旋轉(zhuǎn)過程中實(shí)時(shí)保持原位。因此不能在旋轉(zhuǎn)過程中實(shí)現(xiàn)位姿校正。如果agv在旋轉(zhuǎn)之前存在位置誤差，旋轉(zhuǎn)后這一誤差仍然存在且近似保持不變。圖2a以及圖2b示出了現(xiàn)有技術(shù)中車體原地旋轉(zhuǎn)的示意圖。

為改善這一狀況，如果將旋轉(zhuǎn)中心配置在非車體中心位置，調(diào)整旋轉(zhuǎn)過程中車體中心的位置，在agv旋轉(zhuǎn)過程中為左右驅(qū)動輪設(shè)置大小不同的速度，可以在一定程度上實(shí)現(xiàn)agv位姿校正。具體實(shí)施方法為：以agv目標(biāo)中心為原點(diǎn)建立坐標(biāo)系，則agv滿足如下的運(yùn)動學(xué)關(guān)系：

其中，agv旋轉(zhuǎn)中心距agv中心的距離(即旋轉(zhuǎn)半徑)為r，agv中心的當(dāng)前位置為x,y，agv當(dāng)前前進(jìn)方向與agv目標(biāo)前進(jìn)方向的法線方向(即x軸)之間的夾角θ。對公式(1)進(jìn)行積分，可得公式(2)：

由于公式(2)有兩個(gè)線性方程，卻僅有一個(gè)未知變量r，因此在非特殊情況下，不存在已知的r使得以上兩個(gè)方程同時(shí)滿足。因此，本方案僅能保證在旋轉(zhuǎn)過程中校正單個(gè)坐標(biāo)值，無法同時(shí)實(shí)現(xiàn)x,y兩個(gè)坐標(biāo)值的旋轉(zhuǎn)矯正。圖3a示出了車體進(jìn)行90度旋轉(zhuǎn)后的位姿校正情況，圖3b示出了車體進(jìn)行180度旋轉(zhuǎn)后的位姿校正情況?？梢钥闯觯摲桨竷H能實(shí)現(xiàn)某一軸向的位姿校正，無法同時(shí)實(shí)現(xiàn)x,y兩個(gè)軸方向的位姿校正。因此在前述應(yīng)用場景下，該方案仍然無法避免出現(xiàn)貨架頂歪的情況。

發(fā)明人研究認(rèn)為，在根本上需要解決如下數(shù)學(xué)問題：設(shè)agv在任意時(shí)刻的位姿由狀態(tài)三元組(x,y,θ)表示。已知：agv的初始狀態(tài)記為(x0,y0,θ0),期望達(dá)到的終止?fàn)顟B(tài)記為(xf,yf,θf)，這里一般有x0≠0,y0≠0,xf＝0,yf＝0。求：規(guī)劃agv在轉(zhuǎn)動過程中的左右輪速vl(t),vr(t),t∈[t0,tf),使agv由初始狀態(tài)轉(zhuǎn)移至期望的終止?fàn)顟B(tài)。由于agv的運(yùn)動學(xué)模型具有典型的非線性微分方程形式，直觀上很難獲得問題的求解方法，于是需要對agv轉(zhuǎn)向姿態(tài)矯正進(jìn)行運(yùn)動學(xué)分析與建模。

(一)agv轉(zhuǎn)向姿態(tài)矯正的運(yùn)動學(xué)分析

雙輪差動式agv的運(yùn)動學(xué)模型為

其中，v表示agv前進(jìn)方向的線速度，ω表示agv自身旋轉(zhuǎn)的角速度，θ表示當(dāng)前車體前進(jìn)方向與x軸正向的夾角，即車體旋轉(zhuǎn)角。方程(3)是agv運(yùn)動過程滿足的基本運(yùn)動學(xué)關(guān)系，在此方程中假定agv在運(yùn)動過程中不存在橫移、打滑等現(xiàn)象。根據(jù)方程(3),我們可以看出agv的運(yùn)動學(xué)方程是非線性的(即，位移增量(δx,δy)與偏轉(zhuǎn)角θ滿足三角函數(shù)關(guān)系),然而位移增量(δx,δy)關(guān)于運(yùn)動速度v是線性的，這就為實(shí)現(xiàn)本發(fā)明的控制需求提供了條件。

假定agv由狀態(tài)(x0,y0,θ0)轉(zhuǎn)移至狀態(tài)(xf,yf,θf),問題實(shí)際上涉及到了x,y,θ三個(gè)狀態(tài)軌跡的規(guī)劃問題。這里偏轉(zhuǎn)角θ的軌跡可通過事先規(guī)劃獲得，意味著任意時(shí)刻的cos(θ),sin(θ)是已知的，即進(jìn)一步問題可以作為一個(gè)線性動態(tài)問題進(jìn)行處理。本問題中，自由變量為(xf-x0,yf-y0)共2個(gè)，有效方程個(gè)數(shù)也為2個(gè)，因此問題的解是存在的，且存在一個(gè)最簡單解。

(二)agv轉(zhuǎn)向姿態(tài)矯正的運(yùn)動學(xué)建模

以角度θ而非時(shí)間t作為自變量，則有如下關(guān)系成立。記則有

由旋轉(zhuǎn)半徑的定義

agv運(yùn)動學(xué)方程(3)可進(jìn)一步變?yōu)?/p>

即微分方程組(6)變?yōu)殛P(guān)于變量θ的函數(shù)。根據(jù)差動輪agv旋轉(zhuǎn)運(yùn)動的運(yùn)動學(xué)關(guān)系，通過調(diào)整agv旋轉(zhuǎn)過程中的旋轉(zhuǎn)半徑可以實(shí)現(xiàn)旋轉(zhuǎn)歸位的需求。其推導(dǎo)過程如下：令旋轉(zhuǎn)半徑為旋轉(zhuǎn)角θ的線性函數(shù)，記為

r(θ)＝aθ+b,(7)

其中，a,b為待確定的未知常數(shù)?；谇笆龇治?，方程(7)的線性函數(shù)形式是滿足問題要求的最簡單形式的解。對式(6)進(jìn)行積分,即

上述問題進(jìn)一步變?yōu)?/p>

根據(jù)分部積分定理,有如下關(guān)系成立

基于式(10),式(9)進(jìn)一步變?yōu)?/p>

上式中僅常數(shù)(a,b)為未知量，由式(11)為二元一次方程組，因此(a,b)的解是唯一的，分別為

其中，

下面結(jié)合圖4描述本發(fā)明提供的車體位姿校正方法的一個(gè)實(shí)施例。

圖4示出本發(fā)明車體位姿校正方法的一個(gè)實(shí)施例的流程示意圖。如圖4所示，該實(shí)施例的車體位姿校正方法包括：

步驟s402，將車體的左驅(qū)動輪與右驅(qū)動輪的速度比值確定為隨車體旋轉(zhuǎn)角、車體中心目標(biāo)位置以及車體中心當(dāng)前位置變化的函數(shù)。其中，車體旋轉(zhuǎn)角為車體目標(biāo)前進(jìn)方向的法線方向與車體當(dāng)前前進(jìn)方向之間的夾角，左驅(qū)動輪速度為右驅(qū)動輪速度為

在步驟s402中，首先將車體旋轉(zhuǎn)半徑r確定為車體旋轉(zhuǎn)角θ的線性函數(shù)。其中，車體旋轉(zhuǎn)半徑r為旋轉(zhuǎn)中心與車體中心之間的距離，車體中心位于車體的左驅(qū)動輪與右驅(qū)動輪的中點(diǎn)。具體包括：將車體旋轉(zhuǎn)半徑用車體旋轉(zhuǎn)角、車體旋轉(zhuǎn)角系數(shù)以及常數(shù)項(xiàng)表示，例如方程(7)所示；將車體中心的位置隨時(shí)間的變化率確定為車體旋轉(zhuǎn)角的函數(shù)，例如方程(6)所示；根據(jù)車體中心的位置隨時(shí)間變化率與車體旋轉(zhuǎn)角的函數(shù)關(guān)系，確定車體中心目標(biāo)位置、車體中心當(dāng)前位置以及車體旋轉(zhuǎn)角的函數(shù)關(guān)系，例如方程(11)所示；利用車體中心目標(biāo)位置、車體中心當(dāng)前位置，表示車體旋轉(zhuǎn)角系數(shù)以及常數(shù)項(xiàng)，例如方程(12)所示。

然后，根據(jù)線性函數(shù)(7)、車體的左驅(qū)動輪與右驅(qū)動輪之間的距離d/2，將車體的左驅(qū)動輪與右驅(qū)動輪的速度比值確定為車體旋轉(zhuǎn)角的函數(shù)。

步驟s404，根據(jù)車體旋轉(zhuǎn)角、車體中心目標(biāo)位置以及車體中心當(dāng)前位置的數(shù)值，確定車體的左驅(qū)動輪與右驅(qū)動輪的速度比值，并根據(jù)速度比值為車體的左驅(qū)動輪與右驅(qū)動輪設(shè)定旋轉(zhuǎn)速度。

本領(lǐng)域技術(shù)人員應(yīng)理解，車體中心的當(dāng)前位置以及車體當(dāng)前前進(jìn)方向可以通過車體的攝像頭探測確定。

在步驟s404中，可以將車體的旋轉(zhuǎn)角速度確定為隨時(shí)間變化的函數(shù)，然后根據(jù)車體旋轉(zhuǎn)角以及車體在各個(gè)時(shí)刻的旋轉(zhuǎn)角速度，確定車體旋轉(zhuǎn)的總時(shí)長，并根據(jù)車體旋轉(zhuǎn)的總時(shí)長以及車體旋轉(zhuǎn)的工作周期確定車體旋轉(zhuǎn)的工作周期數(shù)，以便在每個(gè)工作周期內(nèi)根據(jù)速度比值以及車體在各個(gè)時(shí)刻的旋轉(zhuǎn)角速度為車體的左驅(qū)動輪與右驅(qū)動輪設(shè)定旋轉(zhuǎn)速度。

例如，可以根據(jù)車體的屬性設(shè)定車體勻變速旋轉(zhuǎn)的角加速度以及最大旋轉(zhuǎn)角速度，并根據(jù)車體勻變速旋轉(zhuǎn)的角加速度以及最大旋轉(zhuǎn)角速度，確定車體在各個(gè)時(shí)刻的旋轉(zhuǎn)角速度。

下面給出車體先勻加速、然后勻速最后勻減速運(yùn)動的應(yīng)用例。

首先輸入以下變量：當(dāng)前agv姿態(tài)(x0,y0,θ0),期望的目標(biāo)姿態(tài)(xf＝0,yf＝0,θf),最大轉(zhuǎn)動角速度ωb,最大轉(zhuǎn)動角加速度aωb,始末轉(zhuǎn)動角速度為0，工作周期為ts。然后，根據(jù)agv的當(dāng)前姿態(tài)和期望姿態(tài)，可以計(jì)算車體需要轉(zhuǎn)動的角度θ；根據(jù)agv的運(yùn)動性能及任務(wù)需求，確定勻變速轉(zhuǎn)動的加減速性能參數(shù)。其中，勻變速轉(zhuǎn)動的加速度不超過aωb，最大轉(zhuǎn)動角速度不超過ωb。通過規(guī)劃旋轉(zhuǎn)角速度ω(t)可以求得的總轉(zhuǎn)動時(shí)間記為t，轉(zhuǎn)向所需的工作周期的個(gè)數(shù)得到角度軌跡θ(t)如圖5a所示，角速度軌跡ω(t)如圖5b所示。需要特別說明的是，由于agv實(shí)際運(yùn)行并不是嚴(yán)格按規(guī)劃的軌跡運(yùn)動，因此為提高矯正精度，需在每一個(gè)工作周期重新計(jì)算新的r(θ)函數(shù)關(guān)系。即，將上一工作周期的狀態(tài)(xi-1,yi-1,θi-1)和期望狀態(tài)(xf,yf,θf)代入方程(7)，獲得當(dāng)前工作周期旋轉(zhuǎn)半徑所滿足的關(guān)系式。

ri(θ)＝aiθ+bi.(14)

其中i表示當(dāng)前工作周期數(shù)。然后，可以通過ccd攝像頭掃描獲得當(dāng)前的狀態(tài)(xi,yi,θi),或通過式運(yùn)動學(xué)積分關(guān)系估計(jì)當(dāng)前的狀態(tài)，即

(xi,yi,θi)＝kinematics(xi-1,yi-1,θi-1,ts)(15)

由式(14)計(jì)算當(dāng)前旋轉(zhuǎn)半徑ri(θi)，并計(jì)算當(dāng)前規(guī)劃的轉(zhuǎn)動角速度ω(its)。設(shè)勻加速運(yùn)動時(shí)間為0～tup，勻速運(yùn)動時(shí)間為tup～tconst，勻減速運(yùn)動時(shí)間為tconst～t。那么，在勻加速運(yùn)動階段(its)＜tup,ω(its)＝ω((i-1)ts)+awbts；在勻速運(yùn)動階段tup＜(its)＜tconst,ω(its)＝ω((i-1)ts)；在勻減速運(yùn)動階段(its)＞tconst,ω(its)＝ω((i-1)ts)-awbts。

規(guī)劃當(dāng)前工作周期內(nèi)左右輪的驅(qū)動速度分別為

輸出vl,vr至agv左右輪的伺服驅(qū)動器，等待ts時(shí)長后進(jìn)行下一個(gè)工作周期?；谝陨戏椒?，可以精確計(jì)算從任意初始位姿pose(t0)到目標(biāo)位姿pose(tf)的轉(zhuǎn)移軌跡。圖6a示出車體位姿校正過程的示意圖，圖6b示出車體轉(zhuǎn)動過程中的軌跡示意圖，圖6c示出左右驅(qū)動輪速度變化的示意圖。

上述實(shí)施例提出的車體位姿校正方法，可以實(shí)現(xiàn)車體在轉(zhuǎn)向過程中同步進(jìn)行誤差消除，使得車體在轉(zhuǎn)向后能精確停在貨架的中心位置，保證貨架在準(zhǔn)確位置頂起貨架，從而避免倉儲系統(tǒng)內(nèi)部發(fā)生事故。與此同時(shí)，由于實(shí)現(xiàn)了車體在旋轉(zhuǎn)過程中同步進(jìn)行誤差消除，無需在旋轉(zhuǎn)過程中停止運(yùn)動，節(jié)省了小車的總運(yùn)動時(shí)長，提高了小車的工作效率。此外，本發(fā)明的方法易于實(shí)現(xiàn)，所建立的計(jì)算模型使得求解過程簡單省時(shí)。

下面結(jié)合圖7描述本發(fā)明一個(gè)實(shí)施例的車體位姿校正裝置。

圖7示出本發(fā)明車體位姿校正裝置的一個(gè)實(shí)施例的結(jié)構(gòu)示意圖。如圖7所示，該實(shí)施例的車體位姿校正裝置70包括：

函數(shù)確定模塊702，配置為將車體的左驅(qū)動輪與右驅(qū)動輪的速度比值確定為隨車體旋轉(zhuǎn)角、車體中心目標(biāo)位置以及車體中心當(dāng)前位置變化的函數(shù)，車體旋轉(zhuǎn)角為車體目標(biāo)前進(jìn)方向的法線方向與車體當(dāng)前前進(jìn)方向之間的夾角。

速度設(shè)定模塊704，配置為根據(jù)車體旋轉(zhuǎn)角、車體中心目標(biāo)位置以及車體中心當(dāng)前位置的數(shù)值，確定車體的左驅(qū)動輪與右驅(qū)動輪的速度比值，并根據(jù)速度比值為車體的左驅(qū)動輪與右驅(qū)動輪設(shè)定旋轉(zhuǎn)速度。

在一個(gè)實(shí)施例中，函數(shù)確定模塊702配置為：

將車體旋轉(zhuǎn)半徑確定為車體旋轉(zhuǎn)角的線性函數(shù)，其中，車體旋轉(zhuǎn)半徑為旋轉(zhuǎn)中心與車體中心之間的距離，車體中心位于車體的左驅(qū)動輪與右驅(qū)動輪的中點(diǎn)；

根據(jù)線性函數(shù)、車體的左驅(qū)動輪與右驅(qū)動輪之間的距離，將車體的左驅(qū)動輪與右驅(qū)動輪的速度比值確定為車體旋轉(zhuǎn)角的函數(shù)。

在一個(gè)實(shí)施例中，函數(shù)確定模塊702配置為：

將車體旋轉(zhuǎn)半徑用車體旋轉(zhuǎn)角、車體旋轉(zhuǎn)角系數(shù)以及常數(shù)項(xiàng)表示；

將車體中心的位置隨時(shí)間的變化率確定為車體旋轉(zhuǎn)角的函數(shù)；

根據(jù)車體中心的位置隨時(shí)間變化率與車體旋轉(zhuǎn)角的函數(shù)關(guān)系，確定車體中心目標(biāo)位置、車體中心當(dāng)前位置以及車體旋轉(zhuǎn)角的函數(shù)關(guān)系；

利用車體中心目標(biāo)位置、車體中心當(dāng)前位置，表示車體旋轉(zhuǎn)角系數(shù)以及常數(shù)項(xiàng)。

在一個(gè)實(shí)施例中，所述車體中心的當(dāng)前位置以及所述車體當(dāng)前前進(jìn)方向通過車體的攝像頭探測確定。

在一個(gè)實(shí)施例中，速度設(shè)定模塊704配置為：

將車體的旋轉(zhuǎn)角速度確定為隨時(shí)間變化的函數(shù)；

根據(jù)車體旋轉(zhuǎn)角以及車體在各個(gè)時(shí)刻的旋轉(zhuǎn)角速度，確定車體旋轉(zhuǎn)的總時(shí)長；

根據(jù)車體旋轉(zhuǎn)的總時(shí)長以及車體旋轉(zhuǎn)的工作周期，確定車體旋轉(zhuǎn)的工作周期數(shù)；

在每個(gè)工作周期內(nèi)，根據(jù)速度比值以及車體在各個(gè)時(shí)刻的旋轉(zhuǎn)角速度，為車體的左驅(qū)動輪與右驅(qū)動輪設(shè)定旋轉(zhuǎn)速度。

在一個(gè)實(shí)施例中，速度設(shè)定模塊704配置為：

根據(jù)車體的屬性，設(shè)定車體勻變速旋轉(zhuǎn)的角加速度以及最大旋轉(zhuǎn)角速度；

根據(jù)車體勻變速旋轉(zhuǎn)的角加速度以及最大旋轉(zhuǎn)角速度，確定車體在各個(gè)時(shí)刻的旋轉(zhuǎn)角速度。

上述實(shí)施例提出的車體位姿校正裝置，可以實(shí)現(xiàn)車體在轉(zhuǎn)向過程中同步進(jìn)行誤差消除，使得車體在轉(zhuǎn)向后能精確停在貨架的中心位置，保證貨架在準(zhǔn)確位置頂起貨架，從而避免倉儲系統(tǒng)內(nèi)部發(fā)生事故。與此同時(shí)，由于實(shí)現(xiàn)了車體在旋轉(zhuǎn)過程中同步進(jìn)行誤差消除，無需在旋轉(zhuǎn)過程中停止運(yùn)動，節(jié)省了小車的總運(yùn)動時(shí)長，提高了小車的工作效率。此外，本發(fā)明的方法易于實(shí)現(xiàn)，所建立的計(jì)算模型使得求解過程簡單省時(shí)。

圖8示出了本發(fā)明車體位姿校正裝置的另一個(gè)實(shí)施例的結(jié)構(gòu)圖。如圖8所示，該實(shí)施例的車體位姿校正裝置80包括：存儲器810以及耦接至該存儲器810的處理器820，處理器820被配置為基于存儲在存儲器810中的指令，執(zhí)行前述任意一個(gè)實(shí)施例中的車體位姿校正方法。

存儲器810例如可以包括系統(tǒng)存儲器、固定非易失性存儲介質(zhì)等。系統(tǒng)存儲器例如存儲有操作系統(tǒng)、應(yīng)用程序、引導(dǎo)裝載程序(bootloader)以及其他程序等。

圖9示出了本發(fā)明車體位姿校正裝置的又一個(gè)實(shí)施例的結(jié)構(gòu)圖。如圖9所示，該實(shí)施例的裝置90包括：存儲器810以及處理器820，還可以包括輸入輸出接口930、網(wǎng)絡(luò)接口940、存儲接口950等。這些接口930，940，950以及存儲器810和處理器820之間例如可以通過總線950連接。其中，輸入輸出接口930為顯示器、鼠標(biāo)、鍵盤、觸摸屏等輸入輸出設(shè)備提供連接接口。網(wǎng)絡(luò)接口940為各種聯(lián)網(wǎng)設(shè)備提供連接接口。存儲接口950為sd卡、u盤等外置存儲設(shè)備提供連接接口。

本發(fā)明還包括一種計(jì)算機(jī)可讀存儲介質(zhì)，其上存儲有計(jì)算機(jī)指令，該指令被處理器執(zhí)行時(shí)實(shí)現(xiàn)前述任意一個(gè)實(shí)施例中的車體位姿校正方法。

本領(lǐng)域內(nèi)的技術(shù)人員應(yīng)明白，本發(fā)明的實(shí)施例可提供為方法、系統(tǒng)、或計(jì)算機(jī)程序產(chǎn)品。因此，本發(fā)明可采用完全硬件實(shí)施例、完全軟件實(shí)施例、或結(jié)合軟件和硬件方面的實(shí)施例的形式。而且，本發(fā)明可采用在一個(gè)或多個(gè)其中包含有計(jì)算機(jī)可用程序代碼的計(jì)算機(jī)可用非瞬時(shí)性存儲介質(zhì)(包括但不限于磁盤存儲器、cd-rom、光學(xué)存儲器等)上實(shí)施的計(jì)算機(jī)程序產(chǎn)品的形式。

本發(fā)明是參照根據(jù)本發(fā)明實(shí)施例的方法、設(shè)備(系統(tǒng))、和計(jì)算機(jī)程序產(chǎn)品的流程圖和/或方框圖來描述的。應(yīng)理解可由計(jì)算機(jī)程序指令實(shí)現(xiàn)流程圖和/或方框圖中的每一流程和/或方框、以及流程圖和/或方框圖中的流程和/或方框的結(jié)合?？商峁┻@些計(jì)算機(jī)程序指令到通用計(jì)算機(jī)、專用計(jì)算機(jī)、嵌入式處理機(jī)或其他可編程數(shù)據(jù)處理設(shè)備的處理器以產(chǎn)生一個(gè)機(jī)器，使得通過計(jì)算機(jī)或其他可編程數(shù)據(jù)處理設(shè)備的處理器執(zhí)行的指令產(chǎn)生用于實(shí)現(xiàn)在流程圖一個(gè)流程或多個(gè)流程和/或方框圖一個(gè)方框或多個(gè)方框中指定的功能的裝置。

這些計(jì)算機(jī)程序指令也可存儲在能引導(dǎo)計(jì)算機(jī)或其他可編程數(shù)據(jù)處理設(shè)備以特定方式工作的計(jì)算機(jī)可讀存儲器中，使得存儲在該計(jì)算機(jī)可讀存儲器中的指令產(chǎn)生包括指令裝置的制造品，該指令裝置實(shí)現(xiàn)在流程圖一個(gè)流程或多個(gè)流程和/或方框圖一個(gè)方框或多個(gè)方框中指定的功能。

這些計(jì)算機(jī)程序指令也可裝載到計(jì)算機(jī)或其他可編程數(shù)據(jù)處理設(shè)備上，使得在計(jì)算機(jī)或其他可編程設(shè)備上執(zhí)行一系列操作步驟以產(chǎn)生計(jì)算機(jī)實(shí)現(xiàn)的處理，從而在計(jì)算機(jī)或其他可編程設(shè)備上執(zhí)行的指令提供用于實(shí)現(xiàn)在流程圖一個(gè)流程或多個(gè)流程和/或方框圖一個(gè)方框或多個(gè)方框中指定的功能的步驟。

以上所述僅為本發(fā)明的較佳實(shí)施例，并不用以限制本發(fā)明，凡在本發(fā)明的精神和原則之內(nèi)，所作的任何修改、等同替換、改進(jìn)等，均應(yīng)包含在本發(fā)明的保護(hù)范圍之內(nèi)。