[發(fā)明專利]一種四輪驅(qū)動全向底盤的控制裝置及控制方法

有效

• 申請?zhí)枺?/td>: 201310219764.5
• 申請日: 2013-06-05
• 公開（公告）號: CN104216406B
• 公開（公告）日: 2017-07-14
• 發(fā)明（設(shè)計(jì)）人: 滕鑫淼
• 申請（專利權(quán)）人: 中國石油天然氣集團(tuán)公司;中國石油集團(tuán)鉆井工程技術(shù)研究院
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 北京三友知識產(chǎn)權(quán)代理有限公司11127
• 代理人: 湯在彥

1.一種四輪驅(qū)動全向底盤的控制裝置，其特征在于包括由ARM和FPGA組成的主控模塊，無線串口通信模塊，碼盤計(jì)數(shù)模塊，直流電機(jī)控制模塊；無線串口通信模塊與ARM相連，設(shè)置于底盤的無線串口通信模塊用來接收來自于計(jì)算機(jī)連接的主控模塊處的無線串口通信模塊的指令，指令包括四輪驅(qū)動全向底盤所要移動到的目標(biāo)點(diǎn)的位置值以及姿態(tài)角值，將接收到的指令傳給上位機(jī)ARM；FPGA與ARM相連，F(xiàn)PGA將計(jì)數(shù)電路和信息采集電路融合，由FPGA對碼盤計(jì)數(shù)模塊的數(shù)據(jù)進(jìn)行采集與處理，然后通過通用I/0接口傳輸?shù)紸RM，同時FPGA也作為接口電路，將ARM的控制指令傳輸?shù)街绷麟姍C(jī)控制模塊，完成對直流電機(jī)的轉(zhuǎn)速以及方向的控制；ARM是整個控制裝置的核心控制模塊，用以對接收到的數(shù)據(jù)進(jìn)行融合，實(shí)現(xiàn)對四輪驅(qū)動全向底盤的位姿進(jìn)行實(shí)時結(jié)算，并完成對四個直流電機(jī)的實(shí)時控制；

碼盤計(jì)數(shù)模塊與FPGA相連，當(dāng)碼盤輪轉(zhuǎn)動1圈時，碼盤計(jì)數(shù)模塊會記錄1200個數(shù)，因此通過碼盤計(jì)數(shù)模塊實(shí)現(xiàn)對碼盤輪行走的距離進(jìn)行記錄，再通過碼盤輪與驅(qū)動輪的半徑之比，得到驅(qū)動輪的行走距離；電機(jī)控制模塊與FPGA相連，其作用是對直流電機(jī)的電流環(huán)進(jìn)行閉環(huán)控制；

按一定周期對車的軌跡微分，本周期初始時刻，車的位姿為已知的x1和y1和thetal，結(jié)束時車的位姿為未知的x2和y2和theta2；在這個周期內(nèi)左輪的位移為dl，右輪的位移為dr，前輪的位移為dh，后輪的位移為db，由于四輪全向運(yùn)動時各輪的側(cè)滑嚴(yán)重，碼盤記的位移只沿著各個輪的正方向，不會記錄各輪側(cè)滑而產(chǎn)生的位移，所以碼盤記的各輪位移dl和dr和dh和db是表示輪子前進(jìn)方向的各輪實(shí)際運(yùn)動方向的分量，按以下公式解算車的質(zhì)心的位移：

$d=\sqrt{{\left(\frac{dl+dr}{2}\right)}^2+{\left(\frac{dh+db}{2}\right)}^2}$

車轉(zhuǎn)過的角度為：

$\mathrm{\Δ}\mathrm{\θ}=\frac{1}{2}(\frac{dr-dl}{L_{lr}}+\frac{dh-db}{L_{bh}})$

上式中，L_1r為車左右輪的碼盤間距，L_bh為車前后輪的碼盤間距；

由Δθ可以求得本周期結(jié)束時車的角度為：

theta2＝thetal+Δθ

當(dāng)這一段位移足夠小時，軌跡可以近似成一條與水平方向夾角為VelTheta的直線；其中：

Veltheta＝thetal+arctan((db+dh)/(dr+dl))

則有：

x₂＝x1+d*cos(Veltheta)

y₂＝y(tǒng)1+d*sin(Veltheta)

至此，在平面運(yùn)動中，知道了機(jī)體的初始位姿，并記錄了機(jī)體的各輪的位移，則四輪全向底盤在任意時刻的位姿都可以得到；

在四輪全向底盤進(jìn)行直線運(yùn)動時，需要給定目標(biāo)的坐標(biāo)點(diǎn)和直線與場地坐標(biāo)系x軸正方向的夾角；機(jī)器人根據(jù)這條目標(biāo)直線和當(dāng)前自身的坐標(biāo)值和航向角，判斷與目標(biāo)直線的位置和角度偏差，同時控制四輪差速修正行進(jìn)軌跡；

在四輪全向底盤軌跡的修正過程中，需要調(diào)節(jié)的兩個偏移量為橫向偏差和航向偏差，當(dāng)同時對這兩個變量進(jìn)行調(diào)節(jié)時，則根據(jù)機(jī)體在不同的情況給調(diào)節(jié)參數(shù)賦予不同的符號控制電機(jī)差速調(diào)節(jié)；

引入一個前置探測點(diǎn)，即在機(jī)體行進(jìn)方向的前方某一位置上，放置一個虛擬點(diǎn)，計(jì)算此點(diǎn)與目標(biāo)直線的偏差，并根據(jù)這個偏差設(shè)置反饋增益來計(jì)算修正的四個輪的差速值，修正機(jī)體行進(jìn)的直線軌跡；前置探測點(diǎn)長短的選擇和反饋增益系數(shù)需要根據(jù)機(jī)器人實(shí)際行走的調(diào)試效果來整定；

位置型PID控制算法的基本算式為：

$u\left(k\right)=K_p\{e\left(k\right)+K_I\underset{j=0}{\overset{k}{\Σ}}e\left(j\right)+K_D\[e\left(k\right)-e(k-1)\]\}$

增量型PID控制算法的基本算式為：

△u(k)＝K_p{[e(k)-e(k-1)]+K_Ie(k)+K_D[e(k)-2e(k-1)+e(k-2)]}

其中，u(k)——第k次采樣時計(jì)算機(jī)輸出值；e(k)——第k次采樣時的偏差值；

Δu(k)——第k次采樣與第k-1次采樣的計(jì)算機(jī)的差值；

當(dāng)執(zhí)行部件不帶積分部件，其位置與計(jì)算機(jī)輸出的數(shù)字量是一一對應(yīng)，采用位置式PID控制算法；當(dāng)執(zhí)行部件帶積分部件時，選用增量式PID控制算法；

底盤的行進(jìn)過程共有三個階段，分別為加速階段、勻速階段和減速階段；在每個階段中都要實(shí)時計(jì)算與目標(biāo)點(diǎn)的距離，若達(dá)到目標(biāo)點(diǎn)就停止；在加速段中，可采用勻加速的控制方式，但首先需設(shè)定好一個最大速度，當(dāng)速度達(dá)到最大速度時，則進(jìn)入勻速區(qū)，進(jìn)入勻速區(qū)后，需檢測是否到達(dá)減速區(qū)，當(dāng)進(jìn)入減速區(qū)時，減速區(qū)減速的方式采用的是離目標(biāo)距離與減速區(qū)長度之比乘以當(dāng)前速度，得到一個平滑的衰減過程，最終的速度會緩慢的到達(dá)零；實(shí)際過程中，考慮到時間的要求以及防止電機(jī)低速轉(zhuǎn)動，當(dāng)速度減到10％的最大速度即以此勻速速度行走，直到到達(dá)目標(biāo)點(diǎn)停止或者直接在程序上進(jìn)入下一段路徑。

2.一種四輪驅(qū)動全向底盤的控制方法，其特征在于具體步驟如下：

設(shè)定四輪驅(qū)動全向底盤所要移動到的目標(biāo)點(diǎn)的位置值，即X、Y坐標(biāo)，以及機(jī)體自身所

要轉(zhuǎn)動的角度θ，啟動開始按鈕，通過無線串口通信方式，四輪驅(qū)動全向底盤上安裝的主控模塊接收到指令后，按照預(yù)先設(shè)定的四輪驅(qū)動全向底盤運(yùn)動的控制算法，通過直流電機(jī)控制模塊，控制四個直流電機(jī)協(xié)調(diào)工作，采用的導(dǎo)航方式為四碼盤導(dǎo)航方式，通過碼盤計(jì)數(shù)模塊實(shí)時計(jì)數(shù)，可以記錄下各輪行走的距離，將此數(shù)據(jù)傳給主控模塊，主控模塊再對四個直流電機(jī)進(jìn)行實(shí)時的PID控制，實(shí)時計(jì)算當(dāng)前運(yùn)動軌跡與預(yù)設(shè)軌跡之間的偏差，不斷的進(jìn)行偏差糾正，使整個機(jī)體沿預(yù)設(shè)的軌跡進(jìn)行運(yùn)動，實(shí)現(xiàn)在自動方向沿直線行走并且實(shí)時改變自身姿態(tài)角；

無線串口通信模塊與ARM相連，用來接收來自于計(jì)算機(jī)連接的無線串口通信模塊的指令，指令信息包括四輪驅(qū)動全向底盤所要移動到的目標(biāo)點(diǎn)的位置值以及姿態(tài)角值，將接收到的指令傳給上位機(jī)ARM；FPGA與ARM相連，將計(jì)數(shù)電路和信息采集電路融合，由FPGA對碼盤計(jì)數(shù)模塊的數(shù)據(jù)進(jìn)行采集與處理，然后通過通用I/0接口傳輸?shù)紸RM，同時FPGA也作為接口電路，將ARM的控制指令傳輸?shù)街绷麟姍C(jī)控制模塊，完成對直流電機(jī)的轉(zhuǎn)速以及方向的控制；ARM是整個控制裝置的核心控制模塊，用以對接收到的數(shù)據(jù)進(jìn)行融合，實(shí)現(xiàn)對四輪驅(qū)動全向底盤的位姿進(jìn)行實(shí)時結(jié)算，并完成對四個直流電機(jī)的實(shí)時控制；碼盤計(jì)數(shù)模塊與FPGA相連，當(dāng)碼盤輪轉(zhuǎn)動1圈時，碼盤計(jì)數(shù)模塊會記錄1200個數(shù)，因此可通過碼盤計(jì)數(shù)模塊實(shí)現(xiàn)對碼盤輪行走的距離進(jìn)行記錄，再通過碼盤輪與驅(qū)動輪的半徑之比，則可得到驅(qū)動輪的行走距離；電機(jī)控制模塊與FPGA相連，其作用是對直流電機(jī)的電流環(huán)進(jìn)行閉環(huán)控制；

按一定周期對車的軌跡微分，本周期初始時刻，車的位姿為已知的x1和y1和thetal，結(jié)束時車的位姿為未知的x2和y2和theta2；在這個周期內(nèi)左輪的位移為dl，右輪的位移為dr，前輪的位移為dh，后輪的位移為db，由于四輪全向運(yùn)動時各輪的側(cè)滑嚴(yán)重，碼盤記的位移只沿著各個輪的正方向，不會記錄各輪側(cè)滑而產(chǎn)生的位移，所以碼盤記的各輪位移dl和dr和dh和db是表示輪子前進(jìn)方向的各輪實(shí)際運(yùn)動方向的分量，按以下公式解算車的質(zhì)心的位移：

$d=\sqrt{{\left(\frac{dl+dr}{2}\right)}^2+{\left(\frac{dh+db}{2}\right)}^2}$

車轉(zhuǎn)過的角度為：

$\mathrm{\Δ}\mathrm{\θ}=\frac{1}{2}(\frac{dr-dl}{L_{lr}}+\frac{dh-db}{L_{bh}})$

上式中，L_1r為車左右輪的碼盤間距，L_bh為車前后輪的碼盤間距；

由Δθ可以求得本周期結(jié)束時車的角度為：

theta2＝thetal+Δθ

當(dāng)這一段位移足夠小時，軌跡可以近似成一條與水平方向夾角為VelTheta的直線；其中：

Veltheta＝thetal+arctan((db+dh)/(dr+dl))

則有：

x₂＝x1+d*cos(Veltheta)

y₂＝y(tǒng)1+d*sin(Veltheta)

至此，在平面運(yùn)動中，知道了機(jī)體的初始位姿，并記錄了機(jī)體的各輪的位移，則四輪全向底盤在任意時刻的位姿都可以得到；

在四輪全向底盤進(jìn)行直線運(yùn)動時，需要給定目標(biāo)的坐標(biāo)點(diǎn)和直線與場地坐標(biāo)系x軸正方向的夾角；機(jī)器人根據(jù)這條目標(biāo)直線和當(dāng)前自身的坐標(biāo)值和航向角，判斷與目標(biāo)直線的位置和角度偏差，同時控制四輪差速修正行進(jìn)軌跡；

在四輪全向底盤軌跡的修正過程中，需要調(diào)節(jié)的兩個偏移量為橫向偏差和航向偏差，當(dāng)同時對這兩個變量進(jìn)行調(diào)節(jié)，則根據(jù)機(jī)體在不同的情況給調(diào)節(jié)參數(shù)賦予不同的符號控制電機(jī)差速調(diào)節(jié)；

引入一個前置探測點(diǎn)，即在機(jī)體行進(jìn)方向的前方某一位置上，放置一個虛擬點(diǎn)，計(jì)算此點(diǎn)與目標(biāo)直線的偏差，并根據(jù)這個偏差設(shè)置反饋增益來計(jì)算修正的四個輪的差速值，修正機(jī)體行進(jìn)的直線軌跡；前置探測點(diǎn)長短的選擇和反饋增益系數(shù)需要根據(jù)機(jī)器人實(shí)際行走的調(diào)試效果來整定；

位置型PID控制算法的基本算式為：

$u\left(k\right)=K_p\{e\left(k\right)+K_I\underset{j=0}{\overset{k}{\Σ}}e\left(j\right)+K_D\[e\left(k\right)-e(k-1)\]\}$

增量型PID控制算法的基本算式為：

△u(k)＝K_p{[e(k)-e(k-1)]+K_Ie(k)+K_D[e(k)-2e(k-1)+e(k-2)]}

其中，u(k)——第k次采樣時計(jì)算機(jī)輸出值；e(k)——第k次采樣時的偏差值；

Δu(k)——第k次采樣與第k-1次采樣的計(jì)算機(jī)的差值；

當(dāng)執(zhí)行部件不帶積分部件，其位置與計(jì)算機(jī)輸出的數(shù)字量是一一對應(yīng)，采用位置式PID控制算法；當(dāng)執(zhí)行部件帶積分部件時，選用增量式PID控制算法；

底盤的行進(jìn)過程共有三個階段，分別為加速階段、勻速階段和減速階段；在每個階段中都要實(shí)時計(jì)算與目標(biāo)點(diǎn)的距離，若達(dá)到目標(biāo)點(diǎn)就停止；在加速段中，可采用勻加速的控制方式，但首先需設(shè)定好一個最大速度，當(dāng)速度達(dá)到最大速度時，則進(jìn)入勻速區(qū)，進(jìn)入勻速區(qū)后，需檢測是否到達(dá)減速區(qū)，當(dāng)進(jìn)入減速區(qū)時，減速區(qū)減速的方式采用的是離目標(biāo)距離與減速區(qū)長度之比乘以當(dāng)前速度，得到一個平滑的衰減過程，最終的速度會緩慢的到達(dá)零；實(shí)際過程中，考慮到時間的要求以及防止電機(jī)低速轉(zhuǎn)動，當(dāng)速度減到10％的最大速度即以此勻速速度行走，直到到達(dá)目標(biāo)點(diǎn)停止或者直接在程序上進(jìn)入下一段路徑。