沈友建，黃孝鵬，肖建東，陳煊之

　?。ㄉ綎|科技大學(xué) 電氣與自動(dòng)化工程學(xué)院，山東 青島 266590）

       摘要：針對(duì)類車機(jī)器人自主移動(dòng)的問(wèn)題，首先在非完整約束系統(tǒng)下建立類車機(jī)器人低速移動(dòng)過(guò)程的運(yùn)動(dòng)學(xué)模型和動(dòng)力學(xué)模型，選用適合基礎(chǔ)性類車移動(dòng)機(jī)器人研究的自行車模型進(jìn)行狀態(tài)分析；在混合式體系結(jié)構(gòu)下用STM32作為機(jī)器人自主移動(dòng)控制系統(tǒng)的核心，給出控制系統(tǒng)框圖，完成硬件設(shè)計(jì)；同時(shí)完成環(huán)境定位與建圖，構(gòu)建動(dòng)態(tài)貝葉斯網(wǎng)絡(luò)，最終綜合實(shí)現(xiàn)類車機(jī)器人自主移動(dòng)的功能。

　　關(guān)鍵詞：自主移動(dòng)；運(yùn)動(dòng)學(xué)模型；STM32控制系統(tǒng)；SLAM

0引言

　　從2013年德國(guó)提出“工業(yè)4.0”開始，機(jī)器人的智能化過(guò)程將會(huì)越來(lái)越迅速，而“人機(jī)協(xié)作、與人共融”是下一步機(jī)器人產(chǎn)業(yè)亟待解決的難題，目前的工業(yè)機(jī)器人或?qū)I(yè)服務(wù)機(jī)器人的移動(dòng)能力都不夠完善，操作方式不夠靈活，自主化程度也都不高。

　　基于以上在機(jī)器人領(lǐng)域出現(xiàn)的新情況和遇到的新問(wèn)題，本文對(duì)基于STM32的機(jī)器人自主移動(dòng)技術(shù)研究進(jìn)行簡(jiǎn)單的論述。本文的研究對(duì)象為類車移動(dòng)機(jī)器人，為論述簡(jiǎn)潔，下文中出現(xiàn)的機(jī)器人、移動(dòng)機(jī)器人都代指類車移動(dòng)機(jī)器人，不另注明。

　　類車移動(dòng)機(jī)器人自主移動(dòng)是指在無(wú)人操作情況下，在隨機(jī)環(huán)境中，為完成特定事件或一系列動(dòng)作，通過(guò)機(jī)器人自身搭載的控制系統(tǒng)并應(yīng)用環(huán)境感知技術(shù)進(jìn)行多種數(shù)據(jù)信息的處理，最終實(shí)現(xiàn)機(jī)器人的自主決策、獨(dú)立執(zhí)行，要求是能夠躲避各種隨機(jī)障礙、規(guī)避潛在風(fēng)險(xiǎn)［1］。目前，類車機(jī)器人自主移動(dòng)的主要功能需求為：（1）在未知環(huán)境中實(shí)現(xiàn)自身定位，并進(jìn)行環(huán)境感知與建圖；（2）未知環(huán)境下的整體規(guī)劃與行為決策；（3）搭載具有學(xué)習(xí)進(jìn)化能力的機(jī)器人移動(dòng)控制系統(tǒng)設(shè)計(jì)；（4）各機(jī)器人之間實(shí)現(xiàn)網(wǎng)絡(luò)化、協(xié)同化；（5）最終目的是實(shí)現(xiàn)“人機(jī)協(xié)作、與人共融”。

　　本文論述的機(jī)器人自主移動(dòng)技術(shù)分為3個(gè)部分：（1）機(jī)器人低速自主移動(dòng)數(shù)學(xué)模型的建立；（2）基于STM32的控制系統(tǒng)設(shè)計(jì)；（3）機(jī)器人自主移動(dòng)SLAM模型及原理。

1移動(dòng)過(guò)程中機(jī)器人建模

　　在類車移動(dòng)機(jī)器人受非完整約束的條件下建立其低速自主移動(dòng)數(shù)學(xué)模型，包括低速移動(dòng)運(yùn)動(dòng)學(xué)模型和動(dòng)力學(xué)模型，同時(shí)引入自行車模型來(lái)簡(jiǎn)化對(duì)自主移動(dòng)機(jī)器人建模的問(wèn)題。

　　1.1非完整約束和完整約束

　　完整約束和非完整約束是構(gòu)成系統(tǒng)約束的全部?jī)?nèi)容。完整約束（Holonomic Constraint）是指系統(tǒng)約束方程中不含確定系統(tǒng)位置的坐標(biāo)的微商，或雖含坐標(biāo)的微商但不利用動(dòng)力學(xué)方程就可直接積分成為不含坐標(biāo)微商的約束；非完整約束（Nonholonomic Constrain）是指系統(tǒng)約束方程中含有確定系統(tǒng)位置的坐標(biāo)的微商且不利用動(dòng)力學(xué)方程不能直接積分為不含坐標(biāo)微商的約束，非完整約束包括運(yùn)動(dòng)約束和動(dòng)態(tài)約束兩種［2］。類車移動(dòng)機(jī)器人就是典型的受非完整約束的系統(tǒng)，簡(jiǎn)稱非完整系統(tǒng)。

　　1.2移動(dòng)機(jī)器人的數(shù)學(xué)模型

　　對(duì)于移動(dòng)機(jī)器人，為了對(duì)其運(yùn)動(dòng)特性進(jìn)行分析、整理和優(yōu)化，需要建立經(jīng)過(guò)合理簡(jiǎn)化和抽象的數(shù)學(xué)模型。本文采用運(yùn)動(dòng)學(xué)模型和動(dòng)力學(xué)模型對(duì)類車移動(dòng)機(jī)器人進(jìn)行建模分析研究，通過(guò)這種模型，研究類車移動(dòng)機(jī)器人在時(shí)間、空間中的運(yùn)動(dòng)規(guī)律并進(jìn)行控制系統(tǒng)設(shè)計(jì)。

　　根據(jù)現(xiàn)有移動(dòng)機(jī)器人情況的統(tǒng)計(jì)研究，大多數(shù)移動(dòng)機(jī)器人速度都低于30 km/h，而在轉(zhuǎn)彎的過(guò)程中機(jī)器人還會(huì)有減速過(guò)程，在這種低速轉(zhuǎn)彎的過(guò)程中，側(cè)向加速度會(huì)很小，因此認(rèn)為移動(dòng)機(jī)器人的輪子相對(duì)于地面滿足非完整約束。如圖1所示，此時(shí)類車移動(dòng)機(jī)器人的瞬時(shí)轉(zhuǎn)向中心一直位于機(jī)器人后軸的橫向延長(zhǎng)線上，也就是說(shuō)后軸上各點(diǎn)的速度方向始終垂直于后軸，而整個(gè)后軸及后車輪在橫向軸心方向上速度為零，這就是類車移動(dòng)機(jī)器人的非完整約束。即對(duì)于機(jī)器人后軸各點(diǎn)處的速度應(yīng)該滿足如下要求：

　　其中，xc、yc代表機(jī)器人后軸上各點(diǎn)的坐標(biāo)。此種情況下，對(duì)機(jī)器人質(zhì)心列寫運(yùn)動(dòng)學(xué)方程組如下：

　　其中，υ為整體質(zhì)心速度；L為前軸到后軸的距離；α為機(jī)器人的質(zhì)心側(cè)偏角；β為機(jī)器人的橫擺角。因?yàn)轭愜囈苿?dòng)機(jī)器人受到的是非完整約束，必須使用關(guān)于這些約束的相應(yīng)的廣義坐標(biāo)，所以在圖1中，x和y為機(jī)器人各部分質(zhì)點(diǎn)所屬質(zhì)點(diǎn)系質(zhì)量分布的平均位置在廣義坐標(biāo)系下的坐標(biāo)；R為機(jī)器人瞬時(shí)轉(zhuǎn)彎半徑；S為后軸長(zhǎng)度；p、q為機(jī)器人質(zhì)心分別距前軸和后軸的距離；φl(shuí)、φr、φ分別為機(jī)器人左前輪、右前輪以及前軸中心點(diǎn)的對(duì)應(yīng)轉(zhuǎn)向角［3］。

為了盡量得到更加適應(yīng)于研究類車機(jī)器人的規(guī)劃及控制問(wèn)題的數(shù)學(xué)模型，應(yīng)該使得類車移動(dòng)機(jī)器人低速行駛的運(yùn)動(dòng)學(xué)模型的形式盡量簡(jiǎn)單，因此要求該模型只反映機(jī)器人低速行駛時(shí)其運(yùn)動(dòng)的主要特征，為此，在保證車輪相對(duì)于地面滿足非完整約束的情況下，可以忽略車輪的側(cè)向運(yùn)動(dòng)。

　　雖然上述運(yùn)動(dòng)學(xué)模型可以反映移動(dòng)機(jī)器人的運(yùn)動(dòng)情況，但是為了滿足非完整約束的條件，還需要知道車輪的側(cè)偏角，所以僅僅依靠運(yùn)動(dòng)學(xué)模型還不足以完成對(duì)移動(dòng)機(jī)器人的完整數(shù)學(xué)描述，以而引入了動(dòng)力學(xué)模型的知識(shí)。通過(guò)建立動(dòng)力學(xué)模型可以計(jì)算出移動(dòng)機(jī)器人車輪所受的側(cè)向力，進(jìn)而得到車輪側(cè)偏角的具體數(shù)據(jù)，進(jìn)一步補(bǔ)充之前的運(yùn)動(dòng)學(xué)模型。根據(jù)研究問(wèn)題的范疇和實(shí)際情況，將兩種模型結(jié)合應(yīng)用，就可以對(duì)移動(dòng)機(jī)器人較好地建立模型。

　　本文中提到的移動(dòng)機(jī)器人動(dòng)力學(xué)問(wèn)題是指移動(dòng)的平穩(wěn)性和機(jī)器人控制系統(tǒng)的實(shí)際控制效果這兩大領(lǐng)域。本節(jié)的內(nèi)容屬于機(jī)器人控制系統(tǒng)的實(shí)際控制效果的一部分。

　　目前操縱穩(wěn)定性模型種類廣泛，從簡(jiǎn)單的低自由度模型到復(fù)雜18自由度模型都有相對(duì)應(yīng)的理論研究。高復(fù)雜度的動(dòng)力學(xué)模型優(yōu)勢(shì)在于反映移動(dòng)機(jī)器人的非線性特性和動(dòng)態(tài)響應(yīng)特性，但同時(shí)，高自由度模型含有較多的無(wú)光變量，優(yōu)化速度較慢；簡(jiǎn)單的低自由度模型含有較少的無(wú)關(guān)變量，優(yōu)化速度較快。理論研究結(jié)合實(shí)際條件，確定自行車模型為理想化的動(dòng)力學(xué)模型，如圖2所示。

　　圖2中，υx、υy分別為移動(dòng)機(jī)器人質(zhì)心的縱向速度和側(cè)向速度；α為車輛的質(zhì)心偏角；β·為機(jī)器人相對(duì)車縱軸線的橫擺角速度；φ為前輪轉(zhuǎn)角；εa、εb分別為前輪和后輪的輪胎側(cè)偏角；Fxa、Fxb分別為前輪和后輪的縱向力；Fya、Fyb 分別為前輪和后輪的側(cè)向力；p、q為移動(dòng)機(jī)器人質(zhì)心分別距前軸和后軸的距離；R為移動(dòng)機(jī)器人質(zhì)心處的瞬時(shí)轉(zhuǎn)彎半徑。

　　自行車模型是對(duì)類車移動(dòng)機(jī)器人的簡(jiǎn)化建模，根據(jù)類車機(jī)器人的結(jié)構(gòu)特點(diǎn)，在車輪軸向方向上，用該模型的前輪代替移動(dòng)機(jī)器人的全部前車輪，用該模型的后輪代替移動(dòng)機(jī)器人的全部后車輪，雖然模型結(jié)構(gòu)簡(jiǎn)單，但是依舊能夠清晰反映類車移動(dòng)機(jī)器人縱向、側(cè)向以及橫擺運(yùn)動(dòng)特性，非常適合用于基礎(chǔ)性類車移動(dòng)機(jī)器人的動(dòng)力學(xué)研究。自行車模型的動(dòng)力學(xué)方程組如式（3）所示［3］：

　　其中，m為半車質(zhì)量：Jz為機(jī)器人自主移動(dòng)時(shí)繞z軸的轉(zhuǎn)動(dòng)慣量，其他變量與圖2中相同。

2基于STM 32的運(yùn)動(dòng)控制系統(tǒng)設(shè)計(jì)

　　嵌入式控制器件具有結(jié)構(gòu)簡(jiǎn)單、易于控制、成本相對(duì)較低的優(yōu)勢(shì)，越來(lái)越為人們所青睞；ARM是開源環(huán)境，可借鑒應(yīng)用的研究技術(shù)豐富；STM32單片機(jī)具有性價(jià)比高、性能好、成本低、功耗低的特點(diǎn)，非常適合作為數(shù)據(jù)處理和控制單元，而且豐富的多功能電機(jī)控制接口方便實(shí)現(xiàn)對(duì)各種驅(qū)動(dòng)電機(jī)的精確控制。因此選用STM32單片機(jī)作為控制系統(tǒng)的核進(jìn)行控制系統(tǒng)設(shè)計(jì)。

　　2.1控制系統(tǒng)基本組成

　　類車機(jī)器人自主移動(dòng)控制系統(tǒng)包括兩部分：（1）完成機(jī)器人的運(yùn)動(dòng)控制功能；（2）進(jìn)行自主環(huán)境感知。這當(dāng)中的關(guān)鍵技術(shù)包括運(yùn)動(dòng)控制技術(shù)、傳感器技術(shù)、機(jī)器人視覺技術(shù)、多傳感器信息融合技術(shù)、無(wú)線網(wǎng)絡(luò)通信技術(shù)以及集傳感器、控制與通信于一體的系統(tǒng)集成技術(shù)。圖3是一種機(jī)器人自主移動(dòng)控制系統(tǒng)框圖。

　　機(jī)器人自主移動(dòng)控制系統(tǒng)的一種典型的硬件組成如圖4所示。整體結(jié)構(gòu)包括含有工控機(jī)的上位機(jī)和由STM32芯片作為控制核心的下位系統(tǒng)。上位機(jī)不斷地發(fā)送指令和編碼器的反饋信號(hào)給下位機(jī)，下位機(jī)以此來(lái)控制移動(dòng)機(jī)器人的具體動(dòng)作，同時(shí)將機(jī)器人的狀態(tài)信息和傳感器數(shù)據(jù)反饋到上位機(jī)，包括機(jī)器人的矢量速度、多種傳感器信息、視頻信息等。上位機(jī)采用處理能力強(qiáng)大的工控機(jī)，本文不做詳細(xì)描述。

　　2.2控制系統(tǒng)的體系結(jié)構(gòu)

　　體系結(jié)構(gòu)是機(jī)器人的物理框架和智能控制系統(tǒng)的載體，包括慎思式、反應(yīng)式和混合式［4］。慎思式體系結(jié)構(gòu)有較強(qiáng)的總體控制能力，但是反應(yīng)性差；反應(yīng)式體系結(jié)構(gòu)缺點(diǎn)是對(duì)于全局性任務(wù)的管理能力較差，優(yōu)點(diǎn)則是其魯棒性/抗變換性（Robustness）好、動(dòng)態(tài)響應(yīng)能力比較強(qiáng)；混合式體系結(jié)構(gòu)結(jié)合以上兩種結(jié)構(gòu)的特點(diǎn)，既有較高的機(jī)器人動(dòng)態(tài)響應(yīng)能力，又具有基較強(qiáng)的系統(tǒng)規(guī)劃能力，特別適合不確定環(huán)境下的機(jī)器人的自主移動(dòng)。

　　機(jī)器人自主移動(dòng)需要較強(qiáng)的任務(wù)管理能力和動(dòng)態(tài)環(huán)境響應(yīng)能力來(lái)確保能在各種不確定環(huán)境下完成決策任務(wù)。針對(duì)機(jī)器人的這種功能需求，確定在混合式體系的框架下設(shè)計(jì)機(jī)器人的控制系統(tǒng)，以此來(lái)達(dá)到結(jié)構(gòu)簡(jiǎn)單、魯棒性/抗變換性好、擴(kuò)展能力強(qiáng)的目的。機(jī)器人的控制系統(tǒng)體系結(jié)構(gòu)如圖5所示［5］。

　　STM32控制系統(tǒng)的功能屬于慎思層，其中任務(wù)規(guī)劃模塊具有最高的層次，負(fù)責(zé)實(shí)現(xiàn)人機(jī)交互以及遠(yuǎn)程服務(wù)器交互。傳感器管理模塊監(jiān)測(cè)傳感器的數(shù)據(jù)，保證傳感器正常運(yùn)行。序列發(fā)生器和執(zhí)行監(jiān)控模塊作為過(guò)渡層，作用是分解任務(wù)、監(jiān)督執(zhí)行器執(zhí)行。行為控制模塊完成行為層的功能。機(jī)器人感知模塊主要采集和處理各種傳感器數(shù)據(jù)，既包括直接傳送到行為庫(kù)的數(shù)據(jù)，也包括傳送到STM32控制系統(tǒng)用于環(huán)境建圖的信息。執(zhí)行器負(fù)責(zé)最終完成工作。機(jī)器人的控制系統(tǒng)結(jié)構(gòu)整體采用多線程的軟件工作模式，從而減少慎思處理過(guò)程對(duì)機(jī)器人整體響應(yīng)能力的限制。

3機(jī)器人自主移動(dòng)SLAM模型及原理

　　環(huán)境定位與建圖可以看做是實(shí)現(xiàn)機(jī)器人自主移動(dòng)技術(shù)的關(guān)鍵。移動(dòng)機(jī)器人同時(shí)定位與建圖（Simultaneous Localization And Mapping, SLAM）問(wèn)題可以描述為：機(jī)器人在未知環(huán)境中開始移動(dòng)時(shí)，其初始位置未知，而在其移動(dòng)的過(guò)程中，可以不斷根據(jù)位置估計(jì)和地圖信息實(shí)現(xiàn)自身定位，在自身定位的基礎(chǔ)上同時(shí)建造增量式地圖，實(shí)現(xiàn)機(jī)器人的自主定位和導(dǎo)航，定位與建圖同時(shí)進(jìn)行、相輔相成。

　　圖6SLAM理論體系展開圖移動(dòng)機(jī)器人SLAM問(wèn)題含兩方面的內(nèi)容：一是機(jī)器人移動(dòng)過(guò)程中的自身定位與狀態(tài)控制，二是機(jī)器人所處環(huán)境的信息搜集。當(dāng)機(jī)器人在未知環(huán)境中移動(dòng)時(shí)，不斷地通過(guò)系統(tǒng)配置的多種傳感器和搭載的攝像設(shè)備感知所處環(huán)境，這包括探測(cè)路障、檢測(cè)路標(biāo)、收集環(huán)境圖像信息、自身環(huán)境定位。移動(dòng)機(jī)器人SLAM的系統(tǒng)工作過(guò)程如圖7所示。

　　該類車移動(dòng)機(jī)器人為一定范圍內(nèi)的全局感知型。在圖中，R為機(jī)器人的感知半徑；Xt表示t時(shí)刻移動(dòng)機(jī)器人的位姿狀態(tài)向量；wt為t時(shí)刻路標(biāo)與機(jī)器人形成的觀測(cè)向量；sj表示第j個(gè)路標(biāo)的位置狀態(tài)向量；ut為從t-1時(shí)刻到t時(shí)刻的機(jī)器人控制系統(tǒng)的輸入向量。

　　很明顯，根據(jù)運(yùn)動(dòng)過(guò)程的連續(xù)性，系統(tǒng)當(dāng)前狀態(tài)與之前的系統(tǒng)狀態(tài)、觀測(cè)信息以及輸入有關(guān)，即：

　　類比于系統(tǒng)的狀態(tài)可觀測(cè)性，假設(shè)當(dāng)前的機(jī)器人的移動(dòng)狀態(tài)是之前所有狀態(tài)變量經(jīng)過(guò)任意形式的運(yùn)動(dòng)變化之和，則可以得到系統(tǒng)當(dāng)前移動(dòng)狀態(tài)的分布概率［6］為：

　　由系統(tǒng)狀態(tài)分布公式進(jìn)而得到觀測(cè)信息的相對(duì)公式為：

公式（4）與公式（5）共同組成了移動(dòng)機(jī)器人和環(huán)境的動(dòng)態(tài)貝葉斯網(wǎng)絡(luò)（Dynamic Bayes network,DBN）。這個(gè)網(wǎng)絡(luò)將會(huì)隨著毗鄰時(shí)間步驟把不同變量聯(lián)系起來(lái)，另外還可以應(yīng)用大數(shù)據(jù)挖掘技術(shù)，提取出機(jī)器人自主移動(dòng)過(guò)程中的大量特性數(shù)據(jù)，為類車機(jī)器人的自主移動(dòng)技術(shù)的研究提供基礎(chǔ)，最終使得類車移動(dòng)機(jī)器人具有進(jìn)行全局性整體規(guī)劃的性能。

4結(jié)論

　　本控制系統(tǒng)以STM32作為自主移動(dòng)機(jī)器人的控制芯片，在設(shè)計(jì)實(shí)現(xiàn)的過(guò)程中，加入運(yùn)動(dòng)學(xué)模型和動(dòng)力學(xué)模型，可為機(jī)器人的機(jī)械結(jié)構(gòu)設(shè)計(jì)、動(dòng)力裝置配備以及路徑規(guī)劃等提供參考數(shù)據(jù)；融合SLAM技術(shù)，初步實(shí)現(xiàn)了機(jī)器人自主移動(dòng)。今后在自主移動(dòng)的基礎(chǔ)上，再加入傳感器技術(shù)、電子技術(shù)、計(jì)算機(jī)技術(shù)、控制技術(shù)及人工智能技術(shù)等多學(xué)科多種技術(shù)，使移動(dòng)機(jī)器人更自主、更智能，甚至具有情感，以更好地服務(wù)于人類。

　　參考文獻(xiàn)

　?。?］ 沈林成,徐昕,朱華勇,等.移動(dòng)機(jī)器人自主控制技術(shù)理論與技術(shù)［M］.北京： 科學(xué)出版社，2011.

　?。?］ 黃用華，廖啟征，魏世民，等.一種前輪驅(qū)動(dòng)自行車機(jī)器人的非完整約束［J］.中南大學(xué)學(xué)報(bào)（自然科學(xué)版），2011，42（增刊1）：512-518.

　?。?］ 沈林成,徐昕,朱華勇,等.移動(dòng)機(jī)器人自主控制技術(shù)理論與技術(shù)［M］.北京:科學(xué)出版社，2011.

　?。?］ 秦志斌,錢徽,朱淼良.自主移動(dòng)機(jī)器人混合式體系結(jié)構(gòu)的一種Multiagent實(shí)現(xiàn)方法［J］.機(jī)器人，2006，28(5):479-481.

　?。?］ 宋永端,李丹勇,蔡文川.移動(dòng)機(jī)器人機(jī)器自主化技術(shù)［M］.北京:機(jī)械工業(yè)出版社，2012.

　?。?］ 陳白帆.動(dòng)態(tài)環(huán)境下移動(dòng)機(jī)器人同時(shí)定位與建圖研究［D］.長(zhǎng)沙:中南大學(xué)，2009.