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

有效

• 申請號: 201310219764.5
• 申請日: 2013-06-05
• 公開（公告）號: CN104216406B
• 公開（公告）日: 2017-07-14
• 發明（設計）人: 滕鑫淼
• 申請（專利權）人: 中國石油天然氣集團公司;中國石油集團鉆井工程技術研究院
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 北京三友知識產權代理有限公司11127
• 代理人: 湯在彥

1.一種四輪驅動全向底盤的控制裝置，其特征在于包括由ARM和FPGA組成的主控模塊，無線串口通信模塊，碼盤計數模塊，直流電機控制模塊；無線串口通信模塊與ARM相連，設置于底盤的無線串口通信模塊用來接收來自于計算機連接的主控模塊處的無線串口通信模塊的指令，指令包括四輪驅動全向底盤所要移動到的目標點的位置值以及姿態角值，將接收到的指令傳給上位機ARM；FPGA與ARM相連，FPGA將計數電路和信息采集電路融合，由FPGA對碼盤計數模塊的數據進行采集與處理，然后通過通用I/0接口傳輸到ARM，同時FPGA也作為接口電路，將ARM的控制指令傳輸到直流電機控制模塊，完成對直流電機的轉速以及方向的控制；ARM是整個控制裝置的核心控制模塊，用以對接收到的數據進行融合，實現對四輪驅動全向底盤的位姿進行實時結算，并完成對四個直流電機的實時控制；

碼盤計數模塊與FPGA相連，當碼盤輪轉動1圈時，碼盤計數模塊會記錄1200個數，因此通過碼盤計數模塊實現對碼盤輪行走的距離進行記錄，再通過碼盤輪與驅動輪的半徑之比，得到驅動輪的行走距離；電機控制模塊與FPGA相連，其作用是對直流電機的電流環進行閉環控制；

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

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

車轉過的角度為：

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

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

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

theta2＝thetal+Δθ

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

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

則有：

x₂＝x1+d*cos(Veltheta)

y₂＝y1+d*sin(Veltheta)

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

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

在四輪全向底盤軌跡的修正過程中，需要調節的兩個偏移量為橫向偏差和航向偏差，當同時對這兩個變量進行調節時，則根據機體在不同的情況給調節參數賦予不同的符號控制電機差速調節；

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

位置型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次采樣時計算機輸出值；e(k)——第k次采樣時的偏差值；

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

當執行部件不帶積分部件，其位置與計算機輸出的數字量是一一對應，采用位置式PID控制算法；當執行部件帶積分部件時，選用增量式PID控制算法；

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

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

設定四輪驅動全向底盤所要移動到的目標點的位置值，即X、Y坐標，以及機體自身所

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

無線串口通信模塊與ARM相連，用來接收來自于計算機連接的無線串口通信模塊的指令，指令信息包括四輪驅動全向底盤所要移動到的目標點的位置值以及姿態角值，將接收到的指令傳給上位機ARM；FPGA與ARM相連，將計數電路和信息采集電路融合，由FPGA對碼盤計數模塊的數據進行采集與處理，然后通過通用I/0接口傳輸到ARM，同時FPGA也作為接口電路，將ARM的控制指令傳輸到直流電機控制模塊，完成對直流電機的轉速以及方向的控制；ARM是整個控制裝置的核心控制模塊，用以對接收到的數據進行融合，實現對四輪驅動全向底盤的位姿進行實時結算，并完成對四個直流電機的實時控制；碼盤計數模塊與FPGA相連，當碼盤輪轉動1圈時，碼盤計數模塊會記錄1200個數，因此可通過碼盤計數模塊實現對碼盤輪行走的距離進行記錄，再通過碼盤輪與驅動輪的半徑之比，則可得到驅動輪的行走距離；電機控制模塊與FPGA相連，其作用是對直流電機的電流環進行閉環控制；

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

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

車轉過的角度為：

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

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

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

theta2＝thetal+Δθ

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

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

則有：

x₂＝x1+d*cos(Veltheta)

y₂＝y1+d*sin(Veltheta)

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

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

在四輪全向底盤軌跡的修正過程中，需要調節的兩個偏移量為橫向偏差和航向偏差，當同時對這兩個變量進行調節，則根據機體在不同的情況給調節參數賦予不同的符號控制電機差速調節；

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

位置型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次采樣時計算機輸出值；e(k)——第k次采樣時的偏差值；

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

當執行部件不帶積分部件，其位置與計算機輸出的數字量是一一對應，采用位置式PID控制算法；當執行部件帶積分部件時，選用增量式PID控制算法；

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