SLAM導航機器人零基礎實戰系列：（四）差分底盤設計——2.stm32主控軟件設計

摘要 ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?

運動底盤是移動機器人的重要組成部分，不像激光雷達、IMU、麥克風、音響、攝像頭這些通用部件可以直接買到，很難買到通用的底盤。一方面是因為底盤的尺寸結構和參數是要與具體機器人匹配的；另一方面是因為底盤包含軟硬件整套解決方案，是很多機器人公司的核心技術，一般不會隨便公開。出于強烈的求知欲與學習熱情，我想自己DIY一整套兩輪差分底盤，并且將完整的設計過程公開出去供大家學習。說干就干，本章節主要內容：

1.stm32主控硬件設計

2.stm32主控軟件設計

3.底盤通信協議

4.底盤ROS驅動開發

5.底盤PID控制參數整定

6.底盤里程計標

---

溫馨提示：

本篇文章已經收錄在我最新出版的書籍《機器人SLAM導航核心技術與實戰》，感興趣的讀者可以購買紙質書籍來進行更加深入和系統性的學習，購買鏈接如下：

點這里購買：《機器人SLAM導航核心技術與實戰》購買鏈接

---

2.stm32主控軟件設計 ? ? ? ? ? ? ? ?

上一節搭建好了底盤的stm32主控硬件，現在就來說說怎么開發配套的stm32軟件。關于建立stm32工程、使用stm32開發庫、stm32軟件調試方法等基礎知識就不多說了，有需要的可以查閱相關資料學習，我覺得http://www.openedv.com《正點原子》的開發資料寫的還可以。我就直接從底盤控制的項目入手，直接進行項目中各個功能需求開始分析講解，如圖11，是我的底盤控制stm32工程項目。

（圖11）底盤控制stm32工程項目

2.1.電機控制 ? ? ? ? ? ? ? ?

電機控制分為兩個部分（電機轉向控制、電機轉速控制），這些都集成在了電機驅動芯片TB6612FNG里面，所以只需要用單片機的IO口產生控制轉向的高低電平和控制轉速的PWM波就能實現。

首先，初始化IO口作為輸出腳，用于產生高低電平輸出來控制轉向，實例代碼如圖12。

（圖12）電機轉向控制IO口初始化

然后，用通用定時器TIM4的通道CH1和CH2分別產生兩路PWM輸出用于兩個電機的轉速控制，定時器默認引腳分配如圖13。

（圖13）stm32定時器通道默認引腳分配

初始化通用定時器TIM4的通道CH1和CH2為PWM輸出，實例代碼如14。

（圖14）電機轉速控制IO口初始化

最后，將電機轉向和速度控制的操作封裝在一個函數中，便于其它地方調用，實例代碼如圖15。

（圖15）電機轉向和速度控制封裝

2.2.編碼器數據讀取 ? ? ? ? ??

編碼器對底盤來說至關重要，一方面底盤通過編碼器的反饋進行PID閉環速度控制，另一方面底盤通過編碼器進行航跡推演得到里程計用于后續的定位與導航等高級算法中。這里用到的編碼器是正交編碼器，所以直接使用通用定時器的輸入捕獲中的編碼器模式來讀取編碼器。采用通用定時器TIM2的通道CH1和CH2捕獲encoder1的A相和B相脈沖，采用通用定時器TIM3的通道CH1和CH2捕獲encoder2的A相和B相脈沖。

先初始化TIM2作為編碼器encoder1的捕獲，實例代碼如圖16。

（圖16）初始化TIM2作為編碼器encoder1的捕獲

然后，將讀取編碼器計數值的操作封裝在一個函數中，便于其它地方調用，實例代碼如圖17。

（圖17）讀取編碼器encoder1計數值封裝

最后，編寫TIM2計數溢出時的中斷處理函數，實例代碼如圖18。

（圖18）TIM2計數溢出中斷處理函數

同理可得TIM3捕獲encoder2的代碼實現，這里就不在贅述了。

2.3.串口數據收發 ? ? ? ? ? ?

串口2是數據接口，負責接收上位機發送過來的控制指令，同時將編碼器值返回給上位機；串口1是debug接口，負責接收上位機發送過來的版本信息請求、PIDm默認值恢復、PID值設定等調試指令，同時將程序中的debug打印信息返回給上位機。但是在底盤正常工作時，只需要連接串口2；串口1是預留出來給有需要自己動手修改PID參數使用的。

首先，配置串口1，先對串口1的輸出進行printf函數打印支持，實例代碼如圖19。

（圖19）串口1的輸出進行printf函數打印支持

然后，初始化串口1，實例代碼如圖20。

（圖20）初始化串口1

最后，編寫串口1接收中斷處理函數，此函數主要進行對上位機發過來的數據進行協議解析，實例代碼如圖21。

（圖21）串口1接收中斷處理函數

接下來，介紹串口2，初始化串口2，實例代碼如圖22。

（圖22）初始化串口2

然后，將串口2發送數據的操作封裝到函數中，便于其它地方調用，實例代碼如圖23。

（圖23）串口2發送數據封裝

最后，編寫串口2接收中斷處理函數，此函數主要進行對上位機發過來的數據進行協議解析，實例代碼如圖24。

（圖24）串口2接收中斷處理函數

到這里，串口有1和串口2的數據發送與接收都編寫好了，依據我們定義的usart2數據通信協議和usart1調試通信協議，上位機就可以編寫對應的程序來跟底盤的串口2和串口1進行通信了。關于通信協議的具體內容，將在后續做展開。

2.4.電機速度PID控制 ? ? ? ??

我在底盤中采用的是增量型PID算法，編程涉及到的數學表達式有3個，分別是：

e(k) = target_value - current_value

delta_u(k) = Kp*[e(k)-e(k-1)] + Ki*e(k) + Kd*[e(k)-2*e(k-1)+e(k-2)]

u(k) = u(k-1) + delta_u(k)

將這3個數學表達式封裝到函數中，便于其它地方調用，實例代碼如圖25。

（圖25）串口2接收中斷處理函數

?電機1與電機2采用同樣的PID算法，所以電機2的PID算法代碼實現就不贅述了。關于PID參數的整定方法，將在后續做展開。

2.5.周期性控制 ? ? ? ? ??

通過上面的講解，各個模塊的驅動代碼都準備就緒了，現在需要產生一個周期性的過程，在里面實現編碼器計數值采樣、PID控制等具體實現。這里采用定時器TIM1產生一個周期性的中斷，在中斷處理函數中實現各模塊的具體操作。

首先，配置定時器TIM1，實例代碼如圖26。

（圖26）配置定時器TIM1

然后，編寫中斷處理函數，實例代碼如圖27。

（圖27）TIM1中斷處理函數

2.6.stm32主控軟件整體框圖

通過上面的講解，對底盤控制的stm32程序實現有了一定的了解，接下來就來做一個總結。

先來看看main()函數實現，如圖28。

（圖28）main()函數實現

結合上面TIM1中斷處理函數，不難發現，整個stm32程序的執行過程：

a.在main()函數中初始化各個模塊；

b.TIM1中斷處理函數周期性的讀取編碼器值、反饋獲取的編碼值、PID控制；

c.剩下的就是串口1和串口2的通信交互。

具體stm32主控軟件整體框圖如圖29。

（圖29）stm32主控軟件整體框圖

需要說明的是，在周期性循環體中，要首先讀取編碼器的值，來保證嚴格的等間隔采樣。

后記 ? ? ? ? ? ? ? ?

如果大家對博文的相關類容感興趣，或有什么技術疑問，歡迎加QQ技術交流群（117698356）

參考文獻

[1] 張虎，機器人SLAM導航核心技術與實戰[M]. 機械工業出版社，2022.

?購書鏈接：https://item.jd.com/13041503.html

下載更多資料：www.xiihoo.com

QQ技術討論群： 117698356

B站視頻教程：https://space.bilibili.com/66815220

Github源碼：https://github.com/xiihoo/Books_Robot_SLAM_Navigation

Gitee源碼（國內訪問速度快）：https://gitee.com/xiihoo-robot/Books_Robot_SLAM_Navigation

序

前言

編程基礎篇

第1章 ROS入門必備知識

1.1 ROS簡介 2

1.1.1 ROS的性能特色 2

1.1.2 ROS的發行版本 3

1.1.3 ROS的學習方法 3

1.2 ROS開發環境的搭建 3

1.2.1 ROS的安裝 4

1.2.2 ROS文件的組織方式 4

1.2.3 ROS網絡通信配置 5

1.2.4 集成開發工具 5

1.3 ROS系統架構 5

1.3.1 從計算圖視角理解ROS架構 6

1.3.2 從文件系統視角理解ROS架構 7

1.3.3 從開源社區視角理解ROS架構 8

1.4 ROS調試工具 8

1.4.1 命令行工具 9

1.4.2 可視化工具 9

1.5 ROS節點通信 10

1.5.1 話題通信方式 12

1.5.2 服務通信方式 15

1.5.3 動作通信方式 19

1.6 ROS的其他重要概念 25

1.7 ROS 2.0展望 28

1.8 本章小結 28

第2章 C++編程范式

2.1 C++工程的組織結構 29

2.1.1 C++工程的一般組織結構 29

2.1.2 C++工程在機器人中的組織結構 29

2.2 C++代碼的編譯方法 30

2.2.1 使用g++編譯代碼 31

2.2.2 使用make編譯代碼 32

2.2.3 使用CMake編譯代碼 32

2.3 C++編程風格指南 33

2.4 本章小結 34

第3章 OpenCV圖像處理

3.1 認識圖像數據 35

3.1.1 獲取圖像數據 35

3.1.2 訪問圖像數據 36

3.2 圖像濾波 37

3.2.1 線性濾波 37

3.2.2 非線性濾波 38

3.2.3 形態學濾波 39

3.3 圖像變換 40

3.3.1 射影變換 40

3.3.2 霍夫變換 42

3.3.3 邊緣檢測 42

3.3.4 直方圖均衡 43

3.4 圖像特征點提取 44

3.4.1 SIFT特征點 44

3.4.2 SURF特征點 50

3.4.3 ORB特征點 52

3.5 本章小結 54

硬件基礎篇

第4章 機器人傳感器

4.1 慣性測量單元 56

4.1.1 工作原理 56

4.1.2 原始數據采集 60

4.1.3 參數標定 65

4.1.4 數據濾波 73

4.1.5 姿態融合 75

4.2 激光雷達 91

4.2.1 工作原理 92

4.2.2 性能參數 94

4.2.3 數據處理 96

4.3 相機 100

4.3.1 單目相機 101

4.3.2 雙目相機 107

4.3.3 RGB-D相機 109

4.4 帶編碼器的減速電機 111

4.4.1 電機 111

4.4.2 電機驅動電路 112

4.4.3 電機控制主板 113

4.4.4 輪式里程計 117

4.5 本章小結 118

第5章 機器人主機

5.1 X86與ARM主機對比 119

5.2 ARM主機樹莓派3B+ 120

5.2.1 安裝Ubuntu MATE 18.04 120

5.2.2 安裝ROS melodic 122

5.2.3 裝機軟件與系統設置 122

5.3 ARM主機RK3399 127

5.4 ARM主機Jetson-tx2 128

5.5 分布式架構主機 129

5.5.1 ROS網絡通信 130

5.5.2 機器人程序的遠程開發 130

5.6 本章小結 131

第6章 機器人底盤

6.1 底盤運動學模型 132

6.1.1 兩輪差速模型 132

6.1.2 四輪差速模型 136

6.1.3 阿克曼模型 140

6.1.4 全向模型 144

6.1.5 其他模型 148

6.2 底盤性能指標 148

6.2.1 載重能力 148

6.2.2 動力性能 148

6.2.3 控制精度 150

6.2.4 里程計精度 150

6.3 典型機器人底盤搭建 151

6.3.1 底盤運動學模型選擇 152

6.3.2 傳感器選擇 152

6.3.3 主機選擇 153

6.4 本章小結 155

SLAM篇

第7章 SLAM中的數學基礎

7.1 SLAM發展簡史 158

7.1.1 數據關聯、收斂和一致性 160

7.1.2 SLAM的基本理論 161

7.2 SLAM中的概率理論 163

7.2.1 狀態估計問題 164

7.2.2 概率運動模型 166

7.2.3 概率觀測模型 171

7.2.4 概率圖模型 173

7.3 估計理論 182

7.3.1 估計量的性質 182

7.3.2 估計量的構建 183

7.3.3 各估計量對比 190

7.4 基于貝葉斯網絡的狀態估計 193

7.4.1 貝葉斯估計 194

7.4.2 參數化實現 196

7.4.3 非參數化實現 202

7.5 基于因子圖的狀態估計 206

7.5.1 非線性最小二乘估計 206

7.5.2 直接求解方法 206

7.5.3 優化方法 208

7.5.4 各優化方法對比 218

7.5.5 常用優化工具 219

7.6 典型SLAM算法 221

7.7 本章小結 221

第8章 激光SLAM系統

8.1 Gmapping算法 223

8.1.1 原理分析 223

8.1.2 源碼解讀 228

8.1.3 安裝與運行 233

8.2 Cartographer算法 240

8.2.1 原理分析 240

8.2.2 源碼解讀 247

8.2.3 安裝與運行 258

8.3 LOAM算法 266

8.3.1 原理分析 266

8.3.2 源碼解讀 267

8.3.3 安裝與運行 270

8.4 本章小結 270

第9章 視覺SLAM系統

9.1 ORB-SLAM2算法 274

9.1.1 原理分析 274

9.1.2 源碼解讀 310

9.1.3 安裝與運行 319

9.1.4 拓展 327

9.2 LSD-SLAM算法 329

9.2.1 原理分析 329

9.2.2 源碼解讀 334

9.2.3 安裝與運行 337

9.3 SVO算法 338

9.3.1 原理分析 338

9.3.2 源碼解讀 341

9.4 本章小結 341

第10章 其他SLAM系統

10.1 RTABMAP算法 344

10.1.1 原理分析 344

10.1.2 源碼解讀 351

10.1.3 安裝與運行 357

10.2 VINS算法 362

10.2.1 原理分析 364

10.2.2 源碼解讀 373

10.2.3 安裝與運行 376

10.3 機器學習與SLAM 379

10.3.1 機器學習 379

10.3.2 CNN-SLAM算法 411

10.3.3 DeepVO算法 413

10.4 本章小結 414

自主導航篇

第11章 自主導航中的數學基礎

11.1 自主導航 418

11.2 環境感知 420

11.2.1 實時定位 420

11.2.2 環境建模 421

11.2.3 語義理解 422

11.3 路徑規劃 422

11.3.1 常見的路徑規劃算法 423

11.3.2 帶約束的路徑規劃算法 430

11.3.3 覆蓋的路徑規劃算法 434

11.4 運動控制 435

11.4.1 基于PID的運動控制 437

11.4.2 基于MPC的運動控制 438

11.4.3 基于強化學習的運動控制 441

11.5 強化學習與自主導航 442

11.5.1 強化學習 443

11.5.2 基于強化學習的自主導航 465

11.6 本章小結 467

第12章 典型自主導航系統

12.1 ros-navigation導航系統 470

12.1.1 原理分析 470

12.1.2 源碼解讀 475

12.1.3 安裝與運行 479

12.1.4 路徑規劃改進 492

12.1.5 環境探索 496

12.2 riskrrt導航系統 498

12.3 autoware導航系統 499

12.4 導航系統面臨的一些挑戰 500

12.5 本章小結 500

第13章 機器人SLAM導航綜合實戰

13.1 運行機器人上的傳感器 502

13.1.1 運行底盤的ROS驅動 503

13.1.2 運行激光雷達的ROS驅動 503

13.1.3 運行IMU的ROS驅動 504

13.1.4 運行相機的ROS驅動 504

13.1.5 運行底盤的urdf模型 505

13.1.6 傳感器一鍵啟動 506

13.2 運行SLAM建圖功能 506

13.2.1 運行激光SLAM建圖功能 507

13.2.2 運行視覺SLAM建圖功能 508

13.2.3 運行激光與視覺聯合建圖功能 508

13.3 運行自主導航 509

13.4 基于自主導航的應用 510

13.5 本章小結 511

附錄A Linux與SLAM性能優化的探討

附錄B 習題

總結

以上是生活随笔為你收集整理的SLAM导航机器人零基础实战系列：（四）差分底盘设计——2.stm32主控软件设计的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。