一種四輪驅(qū)動(dòng)全向底盤的控制裝置及控制方法
【專利摘要】一種四輪驅(qū)動(dòng)全向底盤的控制裝置及控制方法，適用于機(jī)器人底盤移動(dòng)控制領(lǐng)域，無線串口通信模塊用來接收來自與計(jì)算機(jī)連接的無線串口通信模塊的指令，指令信息包括四輪驅(qū)動(dòng)全向底盤所要移動(dòng)到的目標(biāo)點(diǎn)的位置值以及姿態(tài)角值，將接收到的指令傳給上位機(jī)arm；fpga將計(jì)數(shù)電路和信息采集電路融合，由FPGA對(duì)碼盤計(jì)數(shù)模塊的數(shù)據(jù)進(jìn)行采集與處理，然后通過通用I/O接口傳輸?shù)紸RM，同時(shí)FPGA也作為接口電路，將ARM的控制指令傳輸?shù)街绷麟姍C(jī)控制模塊，完成對(duì)直流電機(jī)的轉(zhuǎn)速以及方向的控制；ARM是整個(gè)控制裝置的核心控制模塊，用以對(duì)接收到的數(shù)據(jù)進(jìn)行融合，實(shí)現(xiàn)對(duì)四輪驅(qū)動(dòng)全向底盤的位姿進(jìn)行實(shí)時(shí)結(jié)算，完成對(duì)四個(gè)直流電機(jī)的實(shí)時(shí)控制。
【專利說明】-種四輪驅(qū)動(dòng)全向底盤的控制裝置及控制方法

【技術(shù)領(lǐng)域】
[0001] 本發(fā)明是一種四輪驅(qū)動(dòng)全向底盤的控制裝置及控制方法，主要適用于機(jī)器人底盤 移動(dòng)控制領(lǐng)域，可用于家用機(jī)器人、工業(yè)用機(jī)器人、軍用機(jī)器人的底盤移動(dòng)控制?？墒箼C(jī)器 人始終沿直線行走，同時(shí)可以改變機(jī)體姿態(tài)，大大縮短機(jī)器人的行走距離，提高了機(jī)器人的 靈活性和機(jī)動(dòng)性。采用這種方法，可以使機(jī)器人在狹小空間內(nèi)較好地完成任務(wù)。

【背景技術(shù)】
[0002] 隨著1962年美國(guó)研制出了第一臺(tái)工業(yè)機(jī)器人，經(jīng)過了近50年的發(fā)展，各國(guó)在機(jī) 器人技術(shù)上投入了 了大量的人力、物力、財(cái)力，機(jī)器人技術(shù)的發(fā)展已經(jīng)經(jīng)歷了翻天覆地的變 化。從最初的一代工業(yè)機(jī)器人，到如今第三代智能機(jī)器人，機(jī)器人技術(shù)水平已有了大大提 升，其中幾個(gè)科技強(qiáng)國(guó)美、日、德、英、法等正走在世界前列，其中美、日兩個(gè)科技超級(jí)大國(guó)更 是在各個(gè)方面均遙遙領(lǐng)先。目前，我國(guó)的機(jī)器人技術(shù)也在穩(wěn)步發(fā)展，正在縮小與發(fā)達(dá)國(guó)家的 差距。
[0003] 目前，機(jī)器人行走移動(dòng)控制已經(jīng)成為機(jī)器人領(lǐng)域中的一個(gè)重要分支，得到了人們 越來越多的關(guān)注。如何提高機(jī)器人移動(dòng)時(shí)的靈活性、機(jī)動(dòng)性，是機(jī)器人領(lǐng)域中人們研究的熱 點(diǎn)。
[0004] 當(dāng)前，地面機(jī)器人的移動(dòng)方式主要分為腿足式、履帶式、輪式等幾種。腿足式主要 依托于仿生學(xué)，其模仿人、動(dòng)物、昆蟲等生物的行走方式，目前發(fā)展迅猛，其機(jī)體的自由度 高，可以適應(yīng)復(fù)雜地形，甚至可以攀爬樓梯，缺點(diǎn)是速度較慢；履帶式適用于較復(fù)雜地形，如 山地等，移動(dòng)速度比腿足式快；輪式機(jī)器人移動(dòng)速度最快，但是通常只能應(yīng)用于較為平坦的 地形。
[0005] 對(duì)于輪式機(jī)器人，其大部分底盤采用的均為兩輪驅(qū)動(dòng)，即兩個(gè)主動(dòng)輪，其余的均為 從動(dòng)輪，但是兩輪驅(qū)動(dòng)的底盤不能直接進(jìn)行橫向的平移，只能進(jìn)行縱向平移、自身旋轉(zhuǎn)，即 兩輪底盤只擁有兩個(gè)自由度，這種驅(qū)動(dòng)方式大大地限制了機(jī)器人的靈活性。因而，為提高機(jī) 器人的靈活性，必須在此基礎(chǔ)上進(jìn)行改進(jìn)。


【發(fā)明內(nèi)容】

[0006] 本發(fā)明是一種四輪驅(qū)動(dòng)全向底盤的控制裝置及控制方法，采用本控制裝置及控制 方法可以使裝有四輪全向底盤的機(jī)器人沿任意方向直線行進(jìn)，并且同時(shí)自身旋轉(zhuǎn)，即在行 駛過程中可以隨意調(diào)節(jié)自身的姿態(tài)，即有三個(gè)自由度，這種驅(qū)動(dòng)方式較兩輪驅(qū)動(dòng)有著更強(qiáng) 的靈活性。
[0007] 四輪驅(qū)動(dòng)相比兩輪驅(qū)動(dòng)的優(yōu)點(diǎn)是靈活性高，自由度更多，但對(duì)于控制上要求也更 高，對(duì)于四輪運(yùn)動(dòng)的協(xié)調(diào)控制是控制上的一大難點(diǎn)。本發(fā)明的核心技術(shù)就是解決四輪運(yùn)動(dòng) 協(xié)調(diào)控制的這一難題。
[0008] 一種四輪驅(qū)動(dòng)全向底盤的控制裝置，包括主控模塊（ARM和FPGA)，無線串口通信 模塊，碼盤計(jì)數(shù)模塊，直流電機(jī)控制模塊。無線串口通信模塊用來接收來自與計(jì)算機(jī)連接的 無線串口通信模塊的指令，指令信息包括四輪驅(qū)動(dòng)全向底盤所要移動(dòng)到的目標(biāo)點(diǎn)的位置值 以及姿態(tài)角值，將接收到的指令傳給上位機(jī)ARM ;FPGA將計(jì)數(shù)電路和信息采集電路融合，由 FPGA對(duì)碼盤計(jì)數(shù)模塊的數(shù)據(jù)進(jìn)行采集與處理，然后通過通用I/O接口傳輸?shù)紸RM，同時(shí)FPGA 也作為接口電路，將ARM的控制指令傳輸?shù)街绷麟姍C(jī)控制模塊，完成對(duì)直流電機(jī)的轉(zhuǎn)速以 及方向的控制;ARM是整個(gè)控制裝置的核心控制模塊，用以對(duì)接收到的數(shù)據(jù)進(jìn)行融合，實(shí)現(xiàn) 對(duì)四輪驅(qū)動(dòng)全向底盤的位姿進(jìn)行實(shí)時(shí)結(jié)算，并完成對(duì)四個(gè)直流電機(jī)的實(shí)時(shí)控制。
[0009] 一種四輪驅(qū)動(dòng)全向底盤的控制方法，包括如上所用的控制裝置，以及用于該裝置 的核心的控制算法。具體步驟如下：
[0010] 設(shè)定四輪驅(qū)動(dòng)全向底盤所要移動(dòng)到的目標(biāo)點(diǎn)的位置值，即X、Y坐標(biāo)，以及機(jī)體自 身所要轉(zhuǎn)動(dòng)的角度Θ，啟動(dòng)開始按鈕，通過無線串口通信方式，四輪驅(qū)動(dòng)全向底盤上安裝的 主控模塊接收到指令后，按照預(yù)先設(shè)定的四輪驅(qū)動(dòng)全向底盤運(yùn)動(dòng)的控制算法，通過直流電 機(jī)控制模塊，控制四個(gè)直流電機(jī)協(xié)調(diào)工作，采用的導(dǎo)航方式為四碼盤導(dǎo)航方式，通過碼盤計(jì) 數(shù)模塊實(shí)時(shí)計(jì)數(shù)，可以記錄下各輪行走的距離，將此數(shù)據(jù)傳給主控模塊，主控模塊再對(duì)四個(gè) 直流電機(jī)進(jìn)行實(shí)時(shí)的PID控制，實(shí)時(shí)計(jì)算當(dāng)前運(yùn)動(dòng)軌跡與預(yù)設(shè)軌跡之間的偏差，不斷的進(jìn) 行偏差糾正，使整個(gè)機(jī)體沿預(yù)設(shè)的軌跡進(jìn)行運(yùn)動(dòng)，實(shí)現(xiàn)在自動(dòng)方向沿直線行走并且實(shí)時(shí)改 變自身姿態(tài)角。

【專利附圖】

【附圖說明】
[0011]為了更清楚地說明本發(fā)明實(shí)施例或現(xiàn)有技術(shù)中的技術(shù)方案，下面將對(duì)實(shí)施例或現(xiàn) 有技術(shù)描述中所需要使用的附圖作簡(jiǎn)單地介紹，顯而易見地，下面描述中的附圖僅僅是本 發(fā)明的一些實(shí)施例，對(duì)于本領(lǐng)域普通技術(shù)人員來講，在不付出創(chuàng)造性勞動(dòng)性的前提下，還可 以根據(jù)這些附圖獲得其他的附圖。
[0012] 圖1為本發(fā)明實(shí)施例提供的四輪全向底盤的結(jié)構(gòu)示意圖；
[0013] 圖2為本發(fā)明實(shí)施例提供的四輪全向底盤運(yùn)動(dòng)方式圖；
[0014] 圖3為本發(fā)明實(shí)施例提供的四輪全向底盤運(yùn)動(dòng)學(xué)分析圖；
[0015] 圖4為本發(fā)明實(shí)施例提供的全向碼盤輪安裝示意圖；
[0016] 圖5為本發(fā)明實(shí)施例提供的控制裝置原理圖；
[0017]圖6為本發(fā)明實(shí)施例提供的四輪全向底盤運(yùn)動(dòng)軌跡示意圖；
[0018]圖7、圖8為本發(fā)明實(shí)施例提供的底盤運(yùn)動(dòng)偏差示意圖；
[0019]圖9為本發(fā)明實(shí)施例提供的PID控制原理圖；
[0020] 圖10為本發(fā)明實(shí)施例提供的四輪全向底盤程序控制流程圖；

【具體實(shí)施方式】
[0021] 下面將結(jié)合本發(fā)明實(shí)施例中的附圖，對(duì)本發(fā)明實(shí)施例中的技術(shù)方案進(jìn)行清楚、完 整地描述，顯然，所描述的實(shí)施例僅僅是本發(fā)明一部分實(shí)施例，而不是全部的實(shí)施例。基于 本發(fā)明中的實(shí)施例，本領(lǐng)域普通技術(shù)人員在沒有做出創(chuàng)造性勞動(dòng)前提下所獲得的所有其他 實(shí)施例，都屬于本發(fā)明保護(hù)的范圍。
[0022]圖1為本發(fā)明實(shí)施例提供的四輪全向底盤的結(jié)構(gòu)示意圖。與兩輪底盤相比，四輪 全向底盤有靈活性高的特點(diǎn)。兩輪驅(qū)動(dòng)的底盤不能直接進(jìn)行橫向的平移，只能進(jìn)行縱向平 移、自身旋轉(zhuǎn)，即兩輪底盤只擁有兩個(gè)自由度。四輪驅(qū)動(dòng)底盤則可沿任意方向行走，并且也 可自身旋轉(zhuǎn)，即四輪驅(qū)動(dòng)底盤具有三個(gè)自由度，這相比兩輪驅(qū)動(dòng)來說靈活性更高。
[0023] 圖2為本發(fā)明實(shí)施例提供的四輪全向底盤運(yùn)動(dòng)方式圖。如圖2所示，四輪全向底 盤沿任意方向直線移動(dòng)，并且在此期間機(jī)體姿態(tài)改變，這是四輪底盤最為突出的特點(diǎn)，也是 控制上的絕對(duì)難點(diǎn)。這種移動(dòng)的方式的優(yōu)點(diǎn)在于，在執(zhí)行復(fù)雜任務(wù)(即要求機(jī)體質(zhì)心由A點(diǎn) 移動(dòng)到B點(diǎn)，同時(shí)要求車體轉(zhuǎn)動(dòng)某個(gè)角度）時(shí)極大地縮短了機(jī)體的運(yùn)動(dòng)距離，在機(jī)體速度確 定的情況下可以縮短到達(dá)目的地的時(shí)間，而這項(xiàng)任務(wù)是兩輪驅(qū)動(dòng)的底盤無法完成的。
[0024] 圖3為本發(fā)明實(shí)施例提供的四輪全向底盤運(yùn)動(dòng)學(xué)分析圖。四輪全向底盤工作空間 為一平面，建立如圖3所示的絕對(duì)坐標(biāo)系xa-ya和四輪全向底盤的坐標(biāo)系Xy-y Y，其中，四輪 全向底盤坐標(biāo)系原點(diǎn)與四輪全向底盤中心重合。a為四輪全向底盤中心到四個(gè)輪間的距離， 中心從A點(diǎn)運(yùn)動(dòng)到B點(diǎn)距離為L(zhǎng)，Θ為質(zhì)心從A點(diǎn)至B點(diǎn)運(yùn)動(dòng)時(shí)期望偏離絕對(duì)坐標(biāo)系橫軸 \的角度，Y為運(yùn)動(dòng)結(jié)束時(shí)χ γ與\的期望夾角（即四輪全向底盤在運(yùn)動(dòng)結(jié)束時(shí)期望自轉(zhuǎn) 的角度)，為運(yùn)動(dòng)過程中實(shí)時(shí)的自轉(zhuǎn)的角度，為期望運(yùn)動(dòng)的速度及自轉(zhuǎn)角速 度，L為所要求的輪子i提供沿驅(qū)動(dòng)方向的速度。
[0025] 通過運(yùn)動(dòng)學(xué)分析，可得如下運(yùn)動(dòng)學(xué)方程：
[0026] - ~xa sin γ' + j>a sips+ αγ
[0027] v2 = -xa siny' + yg cos f -af
[0028] v3 = xa cos γ' + ya sin γ' -af
[0029] \!4 = xu cos γ' + yu sin γ' -\- &f
[0030] 上述四個(gè)方程，則是4個(gè)直流電機(jī)分別對(duì)應(yīng)的轉(zhuǎn)速方程，將尨、p期望運(yùn)動(dòng) 的速度及自轉(zhuǎn)角速度代入上述四個(gè)方程中，我們即可得到4個(gè)直流電機(jī)所要控制的理論轉(zhuǎn) 速Vi。
[0031] 因此，上述四個(gè)方程是本發(fā)明的核心算法。
[0032] 圖4為本發(fā)明實(shí)施例提供的全向碼盤輪安裝示意圖。如圖所示，全向碼盤輪上連 接一個(gè)增量型碼盤計(jì)數(shù)模塊，實(shí)時(shí)記錄下碼盤轉(zhuǎn)動(dòng)過的圈數(shù)，將此數(shù)據(jù)傳到主控模塊，對(duì)全 向輪走過的距離進(jìn)行結(jié)算。
[0033] 圖5為本發(fā)明實(shí)施例提供的控制裝置原理圖。如圖5中所示,無線串口通信模塊與 ARM相連，用來接收來自與計(jì)算機(jī)連接的無線串口通信模塊的指令，指令信息包括四輪驅(qū)動(dòng) 全向底盤所要移動(dòng)到的目標(biāo)點(diǎn)的位置值以及姿態(tài)角值，將接收到的指令傳給上位機(jī)ARM; FPGA與ARM相連，將計(jì)數(shù)電路和信息采集電路融合，由FPGA對(duì)碼盤計(jì)數(shù)模塊的數(shù)據(jù)進(jìn)行采 集與處理，然后通過通用I/O接口傳輸?shù)紸RM，同時(shí)FPGA也作為接口電路，將ARM的控制指 令傳輸?shù)街绷麟姍C(jī)控制模塊，完成對(duì)直流電機(jī)的轉(zhuǎn)速以及方向的控制;ARM是整個(gè)控制裝 置的核心控制模塊，用以對(duì)接收到的數(shù)據(jù)進(jìn)行融合，實(shí)現(xiàn)對(duì)四輪驅(qū)動(dòng)全向底盤的位姿進(jìn)行 實(shí)時(shí)結(jié)算，并完成對(duì)四個(gè)直流電機(jī)的實(shí)時(shí)控制；碼盤計(jì)數(shù)模塊與FPGA相連,當(dāng)碼盤輪轉(zhuǎn)動(dòng)1 圈時(shí)，碼盤計(jì)數(shù)模塊會(huì)記錄1200個(gè)數(shù)，因此可通過碼盤計(jì)數(shù)模塊實(shí)現(xiàn)對(duì)碼盤輪行走的距離 進(jìn)行記錄，再通過碼盤輪與驅(qū)動(dòng)輪的半徑之比，則可得到驅(qū)動(dòng)輪的行走距離；電機(jī)控制模塊 與FPGA相連，其作用是對(duì)直流電機(jī)的電流環(huán)進(jìn)行閉環(huán)控制。
[0034] 圖6為本發(fā)明實(shí)施例提供的四輪全向底盤運(yùn)動(dòng)軌跡示意圖。如圖9所示，按一定 周期對(duì)車的軌跡微分，本周期初始時(shí)刻，車的位姿為（xl，yl，thetal)(已知)，結(jié)束時(shí)車的位 姿為（x2，y2，theta2)(未知)。在這個(gè)周期內(nèi)左輪的位移為sl，右輪的位移為 sr，前輪的位 移為sh，后輪的位移為sb，由于四輪全向運(yùn)動(dòng)時(shí)各輪的側(cè)滑嚴(yán)重，碼盤記的位移只沿著各 個(gè)輪的正方向，不會(huì)記錄各輪側(cè)滑而產(chǎn)生的位移，所以碼盤記的各輪位移（dl，dr，dh，db)是 各輪實(shí)際運(yùn)動(dòng)方向的分量(輪子前進(jìn)方向的分量)，根據(jù)實(shí)驗(yàn)結(jié)果，發(fā)現(xiàn)按以下公式解算車 的質(zhì)心的位移較為精確：
[0035]

【權(quán)利要求】
1· 一種四輪驅(qū)動(dòng)全向底盤的控制裝置，其特征在于包括主控模塊（ARM和FPGA)，無線 串口通信模塊，碼盤計(jì)數(shù)模塊，直流電機(jī)控制模塊；無線串口通信模塊用來接收來自與計(jì) 算機(jī)連接的無線串口通信模塊的指令，指令信息包括四輪驅(qū)動(dòng)全向底盤所要移動(dòng)到的目標(biāo) 點(diǎn)的位置值以及姿態(tài)角值，將接收到的指令傳給上位機(jī)ARM ;FPGA將計(jì)數(shù)電路和信息采集 電路融合，由FPGA對(duì)碼盤計(jì)數(shù)模塊的數(shù)據(jù)進(jìn)行采集與處理，然后通過通用I/O接口傳輸?shù)?ARM，同時(shí)FPGA也作為接口電路，將ARM的控制指令傳輸?shù)街绷麟姍C(jī)控制模塊，完成對(duì)直流 電機(jī)的轉(zhuǎn)速以及方向的控制；ARM是整個(gè)控制裝置的核心控制模塊，用以對(duì)接收到的數(shù)據(jù) 進(jìn)行融合，實(shí)現(xiàn)對(duì)四輪驅(qū)動(dòng)全向底盤的位姿進(jìn)行實(shí)時(shí)結(jié)算，并完成對(duì)四個(gè)直流電機(jī)的實(shí)時(shí) 控制。
2. -種四輪驅(qū)動(dòng)全向底盤的控制方法，其特征在于具體步驟如下： 設(shè)定四輪驅(qū)動(dòng)全向底盤所要移動(dòng)到的目標(biāo)點(diǎn)的位置值，即X、Y坐標(biāo)，以及機(jī)體自身所 要轉(zhuǎn)動(dòng)的角度Θ，啟動(dòng)開始按鈕，通過無線串口通信方式，四輪驅(qū)動(dòng)全向底盤上安裝的主控 模塊接收到指令后，按照預(yù)先設(shè)定的四輪驅(qū)動(dòng)全向底盤運(yùn)動(dòng)的控制算法，通過直流電機(jī)控 制模塊，控制四個(gè)直流電機(jī)協(xié)調(diào)工作，采用的導(dǎo)航方式為四碼盤導(dǎo)航方式，通過碼盤計(jì)數(shù)模 塊實(shí)時(shí)計(jì)數(shù)，可以記錄下各輪行走的距離，將此數(shù)據(jù)傳給主控模塊，主控模塊再對(duì)四個(gè)直流 電機(jī)進(jìn)行實(shí)時(shí)的PID控制，實(shí)時(shí)計(jì)算當(dāng)前運(yùn)動(dòng)軌跡與預(yù)設(shè)軌跡之間的偏差，不斷的進(jìn)行偏 差糾正，使整個(gè)機(jī)體沿預(yù)設(shè)的軌跡進(jìn)行運(yùn)動(dòng)，實(shí)現(xiàn)在自動(dòng)方向沿直線行走并且實(shí)時(shí)改變自 身姿態(tài)角。
3. 根據(jù)權(quán)利要求1所述的一種四輪驅(qū)動(dòng)全向底盤的控制方法，其特征在于無線串口通 信模塊與ARM相連,用來接收來自與計(jì)算機(jī)連接的無線串口通信模塊的指令，指令信息包 括四輪驅(qū)動(dòng)全向底盤所要移動(dòng)到的目標(biāo)點(diǎn)的位置值以及姿態(tài)角值，將接收到的指令傳給上 位機(jī)ARM ;FPGA與ARM相連，將計(jì)數(shù)電路和信息采集電路融合，由FPGA對(duì)碼盤計(jì)數(shù)模塊的數(shù) 據(jù)進(jìn)行采集與處理，然后通過通用I/O接口傳輸?shù)紸RM，同時(shí)FPGA也作為接口電路，將ARM 的控制指令傳輸?shù)街绷麟姍C(jī)控制模塊，完成對(duì)直流電機(jī)的轉(zhuǎn)速以及方向的控制；ARM是整 個(gè)控制裝置的核心控制模塊，用以對(duì)接收到的數(shù)據(jù)進(jìn)行融合，實(shí)現(xiàn)對(duì)四輪驅(qū)動(dòng)全向底盤的 位姿進(jìn)行實(shí)時(shí)結(jié)算，并完成對(duì)四個(gè)直流電機(jī)的實(shí)時(shí)控制；碼盤計(jì)數(shù)模塊與FPGA相連，當(dāng)碼 盤輪轉(zhuǎn)動(dòng)1圈時(shí)，碼盤計(jì)數(shù)模塊會(huì)記錄1200個(gè)數(shù)，因此可通過碼盤計(jì)數(shù)模塊實(shí)現(xiàn)對(duì)碼盤輪 行走的距離進(jìn)行記錄，再通過碼盤輪與驅(qū)動(dòng)輪的半徑之比，則可得到驅(qū)動(dòng)輪的行走距離；電 機(jī)控制模塊與FPGA相連，其作用是對(duì)直流電機(jī)的電流環(huán)進(jìn)行閉環(huán)控制； 按一定周期對(duì)車的軌跡微分，本周期初始時(shí)刻，車的位姿為（xl，yl，thetal)(已知)， 結(jié)束時(shí)車的位姿為（x2, y2, theta2)(未知）；在這個(gè)周期內(nèi)左輪的位移為sl，右輪的位移 為sr，前輪的位移為sh，后輪的位移為sb，由于四輪全向運(yùn)動(dòng)時(shí)各輪的側(cè)滑嚴(yán)重，碼盤記 的位移只沿著各個(gè)輪的正方向，不會(huì)記錄各輪側(cè)滑而產(chǎn)生的位移，所以碼盤記的各輪位移 (dl，dr, dh, db)是各輪實(shí)際運(yùn)動(dòng)方向的分量(輪子前進(jìn)方向的分量)，按以下公式解算車的 質(zhì)心的位移： A_ idz+m2 車轉(zhuǎn)過的角度為： ι(Φ~0 dh-db'' ~~+~?~ 式（4. 27)中，k為車左右輪的碼盤間距，Lbh為車前后輪的碼盤間距； 由Λ Θ可以求得本周期結(jié)束時(shí)車的角度為： theta2=thetal+Δ Θ 當(dāng)這一段位移足夠小時(shí)，軌跡可以近似成一條與水平方向夾角為VelTheta的直線；其 中： Veltheta=thetal+arctan((db+dh)/(dr+dl)) 則有： x2=x1+d>l<cos (Veltheta) y2=yi+d*sin(Veltheta) 至此，由（xl，yl，thetal)推出了（x2, y2, theta2)，在平面運(yùn)動(dòng)中，知道了機(jī)體的初始 位姿，并記錄了機(jī)體的各輪的位移，則四輪全向底盤在任意時(shí)刻的位姿都可以得到； 在四輪全向底盤進(jìn)行直線運(yùn)動(dòng)時(shí)，需要給定目標(biāo)的坐標(biāo)點(diǎn)和直線與場(chǎng)地坐標(biāo)系X軸正 方向的夾角，這樣就可以將此條直線確定下來；機(jī)器人根據(jù)這條目標(biāo)直線和當(dāng)前自身的坐 標(biāo)值和航向角，判斷與目標(biāo)直線的位置和角度偏差，同時(shí)控制四輪差速修正行進(jìn)軌跡； 在四輪全向底盤軌跡的修正過程中，需要調(diào)節(jié)的兩個(gè)偏移量為橫向偏差和航向偏差， 如果想同時(shí)對(duì)這兩個(gè)變量進(jìn)行調(diào)節(jié)，則情況會(huì)比較復(fù)雜，需要根據(jù)機(jī)體在不同的情況給調(diào) 節(jié)參數(shù)賦予不同的符號(hào)控制電機(jī)差速調(diào)節(jié)； 為了同時(shí)調(diào)節(jié)四輪全向底盤行進(jìn)的橫向偏差和航向偏差，提高直線行走收斂的快速 性，減小超調(diào)，引入一個(gè)前置探測(cè)點(diǎn)，即在機(jī)體行進(jìn)方向的前方某一位置上，放置一個(gè)虛擬 點(diǎn)，計(jì)算此點(diǎn)與目標(biāo)直線的偏差，并根據(jù)這個(gè)偏差設(shè)置合適的反饋增益來計(jì)算修正的四個(gè) 輪的差速值，修正機(jī)體行進(jìn)的直線軌跡；前置探測(cè)點(diǎn)長(zhǎng)短的選擇和反饋增益系數(shù)需要根據(jù) 機(jī)器人實(shí)際行走的調(diào)試效果來整定，通過加入前置探測(cè)點(diǎn)，可以將橫向偏差和航向偏差整 合在了一起，同時(shí)相當(dāng)于引入超前校正環(huán)節(jié),可以提高行走精度；對(duì)于機(jī)體運(yùn)動(dòng)時(shí)姿態(tài)角的 控制，只要在這個(gè)基礎(chǔ)上加入姿態(tài)角的偏差修正即可； 位置型PID控制算法的基本算式為： n{k) = Kr \e(k) + - φ-m 增量型PID控制算法的基本算式為： Δ u (k) =KP {[e (k) -e (k-1) ] +Κχθ (k) +KD [e (k) -2e (k-1) +e (k-2) ]} 其中，u(k)--第k次采樣時(shí)計(jì)算機(jī)輸出值；e(k)--第η次采樣時(shí)的偏差值； Au(k)--第k次采樣與第k-1次采樣的計(jì)算機(jī)的差值； 通過觀察上式可知，位置式PID算法輸出與整個(gè)過去狀態(tài)有關(guān)，計(jì)算式中要用到過去 偏差的累加值，容易產(chǎn)生較大的積累誤差，容易產(chǎn)生積分飽和；而增量式只需計(jì)算增量，當(dāng) 存在計(jì)算誤差或精度不足時(shí)，對(duì)控制量計(jì)算的影響較?。? 當(dāng)執(zhí)行部件不帶積分部件，其位置與計(jì)算機(jī)輸出的數(shù)字量是一一對(duì)應(yīng)(如電液伺服 閥)，采用位置式PID控制算法；若執(zhí)行部件帶積分部件(如步進(jìn)電機(jī)、步進(jìn)電機(jī)帶動(dòng)閥門或 帶動(dòng)多圈電位器）時(shí)，通常選用增量式PID控制算法； 底盤的行進(jìn)過程共有三個(gè)階段，分別為加速階段、勻速階段和減速階段；在每個(gè)階段中 都要實(shí)時(shí)計(jì)算與目標(biāo)點(diǎn)的距離，若達(dá)到目標(biāo)點(diǎn)就停止；在加速段中，可采用勻加速的控制方 式，但首先需設(shè)定好一個(gè)最大速度，當(dāng)速度達(dá)到最大速度時(shí)，則進(jìn)入勻速區(qū)，進(jìn)入勻速區(qū)后， 需檢測(cè)是否到達(dá)減速區(qū)，當(dāng)進(jìn)入減速區(qū)時(shí)，為了避免由于突然減速帶來的電機(jī)振動(dòng)問題，減 速區(qū)減速的方式采用的是離目標(biāo)距離與減速區(qū)長(zhǎng)度之比乘以當(dāng)前速度，得到一個(gè)平滑的衰 減過程，最終的速度會(huì)緩慢的到達(dá)零；這種減速的效果不錯(cuò)，減速也很快；實(shí)際過程中，考 慮到時(shí)間的要求以及防止電機(jī)低速轉(zhuǎn)動(dòng)，當(dāng)速度減到10%的最大速度即以此勻速速度行 走，直到到達(dá)目標(biāo)點(diǎn)停止或者直接在程序上進(jìn)入下一段路徑。
【文檔編號(hào)】G05D1/02GK104216406SQ201310219764
【公開日】2014年12月17日 申請(qǐng)日期:2013年6月5日 優(yōu)先權(quán)日:2013年6月5日
【發(fā)明者】滕鑫淼 申請(qǐng)人:中國(guó)石油天然氣集團(tuán)公司, 中國(guó)石油集團(tuán)鉆井工程技術(shù)研究院