未知環(huán)境下基于VFH+物理仿真耦合模型的 機(jī)器人避障

孫遠(yuǎn)敬，王 帥，李 鑫，郭 鷹

（遼寧工程技術(shù)大學(xué) 機(jī)械工程學(xué)院，遼寧 阜新 123000）

0 引言

機(jī)器人路徑規(guī)劃問(wèn)題在消防救援、交通運(yùn)輸、航空航天、物流倉(cāng)儲(chǔ)等行業(yè)中有很大研究?jī)r(jià)值和發(fā)展?jié)摿?路徑規(guī)劃作為構(gòu)建起始點(diǎn)與目標(biāo)點(diǎn)之間理論最優(yōu)路徑的策略問(wèn)題，可分為對(duì)預(yù)設(shè)環(huán)境和未知環(huán)境的規(guī)劃.基于這兩種環(huán)境下的路徑規(guī)劃問(wèn)題，為克服各種算法存在的局限性，相關(guān)學(xué)者對(duì)避障算法的優(yōu)化改進(jìn)和組合策略做了大量研究.莊宇輝[1]等基于向量場(chǎng)直方圖法（Vector Field Histogram, VFH）提出考慮運(yùn)動(dòng)和環(huán)境特性，引入閾值評(píng)價(jià)函數(shù)對(duì)各行進(jìn)方向進(jìn)行綜合評(píng)價(jià)，選取最合適的閾值范圍通行.張亞蘭[2]等將改進(jìn)的雙向A*算法與VFH算法聯(lián)合優(yōu)化應(yīng)用在避障問(wèn)題中，所提方法規(guī)劃的路徑長(zhǎng)度短、耗時(shí)少，并能有效避免環(huán)境變化帶來(lái)的碰撞風(fēng)險(xiǎn).李衛(wèi)碩[3]等提出將反向傳播（Back Propagation，BP）神經(jīng)網(wǎng)絡(luò)應(yīng)用在機(jī)器人避障中，選取合適的輸入、得分選項(xiàng)，設(shè)置合適的條件概率訓(xùn)練神經(jīng)網(wǎng)絡(luò)模型.軌跡較人工勢(shì)場(chǎng)法短，且被證明能夠應(yīng)用于較復(fù)雜的預(yù)設(shè)障礙物環(huán)境中.

本文對(duì)傳統(tǒng)的機(jī)器人避障實(shí)驗(yàn)流程，針對(duì)某款移動(dòng)機(jī)器人在矩陣實(shí)驗(yàn)室（Matlab）環(huán)境創(chuàng)建向量場(chǎng)直方圖算法（Vector Field Histogram+）模型，在Matlab中驗(yàn)證算法的有效性后，再基于Simulink的機(jī)器人物理仿真耦合模型，進(jìn)行耦合算法的仿真實(shí)驗(yàn)，實(shí)現(xiàn)避障機(jī)器人對(duì)障礙物的預(yù)警和規(guī)避.簡(jiǎn)化實(shí)驗(yàn)流程的同時(shí)解決了控制算法模型影響后期實(shí)驗(yàn)階段機(jī)器人的路徑精度問(wèn)題，避免出現(xiàn)機(jī)器人偏離規(guī)劃路徑甚至發(fā)生碰撞風(fēng)險(xiǎn).

1 改進(jìn)向量場(chǎng)直方圖算法

與VFH算法相比，VFH+算法由于其在迭代過(guò)程中將機(jī)器人的尺寸因素和最小安全距離考慮到了算法模型內(nèi)，從而使路徑具有更高的穩(wěn)定性和平滑性.因此得到的路徑軌跡比原始的VFH算法效果更好[4].VFH+算法使用柵格地圖表示在此處存在障礙物的可信度（Certainty Value，CV）作為輸入.高的CV值表示此處較大可能存在障礙物.活動(dòng)窗口內(nèi)每個(gè)柵格的CV值cij的大小可按經(jīng)驗(yàn)方程確定.

1.1 主極坐標(biāo)直方圖的建立

初始階段，將柵格地圖的活動(dòng)區(qū)域映射到主直方圖.活動(dòng)區(qū)域是隨機(jī)器人移動(dòng)的半徑為ws的圓形窗口，柵格地圖中的每個(gè)柵格的內(nèi)容作為障礙物向量，障礙向量的向量方向βij由柵格到機(jī)器人中心的方向決定[5].

式中，x0、y0為機(jī)器人中心點(diǎn)的當(dāng)前坐標(biāo)；xj、yj為活動(dòng)區(qū)域中障礙物柵格單元cij的坐標(biāo).

柵格單元中障礙向量模數(shù)為

式中，a，b為常數(shù)；dij為柵格單元cij到機(jī)器人中心的距離，m.根據(jù)建立的障礙向量建立主極坐標(biāo)直方圖Hp，Hp中可自由選擇角分辨率α，活動(dòng)窗口分區(qū)后得到的區(qū)間數(shù)s=360/α，本次實(shí)驗(yàn)中，將α設(shè)置為5，共72個(gè)區(qū)間.VFH方法未考慮機(jī)器人尺寸因素和最小安全距離的影響，而VFH+方法則通過(guò)低通濾波器來(lái)補(bǔ)償機(jī)器人的尺寸因素可能會(huì)對(duì)實(shí)驗(yàn)造成的額外影響.另一方面，VFH+算法通過(guò)在構(gòu)建主極坐標(biāo)直方圖時(shí)放大障礙物柵格范圍，可以有效地實(shí)現(xiàn)寬度補(bǔ)償保證了算法的穩(wěn)定性.此外，VFH+算法還更新了所有與放大后的障礙物柵格相重合的直方圖區(qū)間.圖1顯示了一個(gè)障礙物柵格單元被放大rr+s倍，在柵格地圖中，被放大后的障礙物柵格區(qū)域表明機(jī)器人在該區(qū)域內(nèi)為不能行走狀態(tài)，實(shí)現(xiàn)寬度補(bǔ)償，保證算法穩(wěn)定性.此外，VFH+算法還會(huì)實(shí)時(shí)更新所有與放大后的障礙物柵相重合的直方圖區(qū)間.

圖1 放大后的障礙物柵格單元示例 Fig.1 example of an enlarged barrier grid unit

對(duì)于區(qū)間k在極坐標(biāo)下的障礙密度為

式中，為濾波系數(shù)，為保證主極坐標(biāo)直方圖平順性，該系數(shù)只考慮機(jī)器人視覺(jué)范圍內(nèi)障礙物；ijr為放大障礙物柵格以機(jī)器人質(zhì)點(diǎn)為中心放大角，°.

1.2 二值極坐標(biāo)直方圖的建立

VFH+算法設(shè)置τlow和τhigh兩個(gè)參數(shù)增加遲滯作用以改善VFH算法中機(jī)器人某些時(shí)刻與障礙物距離太近、安全區(qū)域太小等缺陷，本實(shí)驗(yàn)τlow取500，τhigh取1 400.根據(jù)主極坐標(biāo)直方圖Hp和τlow、τhigh以及二值極坐標(biāo)直方圖中的每個(gè)區(qū)間的狀態(tài)值Hb，用來(lái)記錄機(jī)器人在該時(shí)刻下可以自由通行的方向.狀態(tài)值Hb分為自由通行和禁止通過(guò)兩種情況，分別由0和1表示.當(dāng)區(qū)域k內(nèi)的障礙頻次大于τhigh和τlow時(shí)，Hp通過(guò)以下規(guī)則更新二值極坐標(biāo)直方圖.

某一時(shí)刻，Hp通過(guò)以下規(guī)則更新二值極坐標(biāo)為

假設(shè)機(jī)器人右轉(zhuǎn)向半徑為rr，左轉(zhuǎn)為rl.在障礙物柵格地圖中，機(jī)器人的左右轉(zhuǎn)向圓圓心與當(dāng)前機(jī)器人中心的相對(duì)距離關(guān)系為

式中，Δx為轉(zhuǎn)向圓圓心在柵格地圖中相對(duì)機(jī)器人中心在x軸方向的偏移距離，m；Δy為轉(zhuǎn)向圓圓心在柵格地圖中相對(duì)機(jī)器人中在y軸方向的偏移距離，m；θ為機(jī)器人的軌跡轉(zhuǎn)角，°.

當(dāng)左轉(zhuǎn)向圓與將柵格地圖中的障礙物柵格A放大了rr+s倍后的區(qū)域有部分重疊時(shí)，此時(shí)滿足

式中，dl為柵格單元cij到左轉(zhuǎn)向圓中心的距離，m；Δx（j）、Δy（i）為柵格單元cij到機(jī)器人中心的相對(duì)x方向、y方向的距離，m.

二者覆蓋的區(qū)域此時(shí)都被Hb認(rèn)為是禁止通過(guò)區(qū)域，相反，由于障礙柵格B與右轉(zhuǎn)向圓無(wú)重疊區(qū)域，除放大后的柵格B區(qū)域外，其他區(qū)域都被認(rèn)為是自由通行區(qū)域[6]為

圖2中，放大的障礙物柵格A禁止機(jī)器人向左側(cè)方向行進(jìn).而障礙物B沒(méi)有阻擋右側(cè)方向.若點(diǎn)s在圖示位置，VFH算法由于沒(méi)有考慮機(jī)器人尺寸、障礙物尺寸以及最小安全距離等，會(huì)將機(jī)器人引導(dǎo)至左側(cè)并可能會(huì)使機(jī)器人與障礙物A相撞.而在VFH +算法中，由于轉(zhuǎn)向圓區(qū)域與放大后的障礙物柵格區(qū)域重疊，使得機(jī)器人在A前右轉(zhuǎn)直行，不再向左轉(zhuǎn)向，直至兩區(qū)域不再有重疊部分.

圖2 基于VFH+算法的轉(zhuǎn)向原理示意 Fig.2 schematic diagram of steering principle based on VFH+algorithm

1.3 轉(zhuǎn)向方向的選擇

根據(jù)Hp和Hb中的值行進(jìn)可以最大限度避免障礙物碰撞，但可能會(huì)偏離實(shí)驗(yàn)的目標(biāo)方向.因此，建立一個(gè)在行進(jìn)代價(jià)和目標(biāo)方向中做出取舍的平衡函數(shù).由該函數(shù)選擇全局最合適的行進(jìn)方向[7].

本次VFH+算法模型的平衡函數(shù)為

式中，μ1，μ2，μ3為權(quán)衡系數(shù)決定了算法的優(yōu)劣性，一般情況下應(yīng)滿足μ1＞μ2+μ3；Δ（c,kt）表示候選方向與目標(biāo)方向間的差角；Δ（c,θi/α）為候選方向與行進(jìn)方向之間的差角；Δ（c,kn,i-1）為當(dāng)前候選方向與前一次選擇方向的角度變化值.

2 基于Matlab的VFH+避障算法模型

利用Matlab軟件建立基于VFH+算法的機(jī)器人避障算法模型，障礙物地圖模型范圍為10 m× 10 m環(huán)境，A（1，4）為出發(fā)點(diǎn)，D（9，7）為目標(biāo)點(diǎn).N1時(shí)刻，機(jī)器人從出發(fā)點(diǎn)A到達(dá)點(diǎn)B（3，5）見(jiàn)圖3.

圖3 避障機(jī)器人到達(dá)點(diǎn)B示意 Fig.3 schematic of obstacle avoidance robot reaching point B

N1時(shí)刻下的二值極坐標(biāo)直方圖見(jiàn)圖4.此時(shí)區(qū)間0～8，28～42的狀態(tài)值Hb為0，表明該區(qū)域發(fā)生碰撞的可能性極低.此時(shí)代表兩個(gè)候選寬域區(qū)間內(nèi)的任意方向均可自由通行[8].

圖4 機(jī)器人到達(dá)點(diǎn)B時(shí)的二值極坐標(biāo)直方圖 Fig. 4 binary polar coordinate histogram when the robot reaches point B

二值坐標(biāo)直方圖中的Hb規(guī)劃了兩個(gè)候選寬域區(qū)間的方向范圍之后，再根據(jù)主極坐標(biāo)圖中的Hp和平衡函數(shù)確定最終的轉(zhuǎn)角大小.Hp中不僅記錄了每個(gè)候選區(qū)間自由通行和禁止通過(guò)兩種狀態(tài)值，還記錄了圖3中點(diǎn)B到達(dá)目標(biāo)點(diǎn)D的目標(biāo)方向，根據(jù)圖5中Hp標(biāo)注的目標(biāo)方向和平衡函數(shù)計(jì)算出下一時(shí)刻機(jī)器人行進(jìn)的轉(zhuǎn)角大小和方向，此時(shí)機(jī)器人在點(diǎn)B選擇沿初始前進(jìn)方向偏左11°行進(jìn).

圖5 機(jī)器人到達(dá)點(diǎn)B時(shí)的主極坐標(biāo)直方圖 Fig.5 histogram of main polar coordinates when the robot reaches point B

N2時(shí)刻，機(jī)器人從點(diǎn)B移動(dòng)到點(diǎn)C，見(jiàn)圖6.達(dá)到點(diǎn)C時(shí)，圖7中的二值極坐標(biāo)直方圖會(huì)實(shí)時(shí)記錄72個(gè)區(qū)間的自由通行和禁止通過(guò)的狀態(tài)值.在到達(dá)點(diǎn)C時(shí)，區(qū)間0～16，28～47，51～72的狀態(tài)值Hb等于0，表明這三個(gè)寬域區(qū)間內(nèi)的任意方向均可自由通行.

圖6 避障機(jī)器人到達(dá)點(diǎn)C示意 Fig.6 schematic of obstacle avoidance robot reaching point C

圖7 機(jī)器人到達(dá)點(diǎn)C時(shí)的二值極坐標(biāo)直方圖 Fig.7 binary polar coordinate histogram when the robot reaches point C

二值極坐標(biāo)直方圖中Hb規(guī)劃了三個(gè)候選區(qū)域后，主極坐標(biāo)直方圖中也記錄了從當(dāng)前位置C到達(dá)目標(biāo)點(diǎn)D的目標(biāo)方向，見(jiàn)圖8.此時(shí)區(qū)間0～3的障礙頻次Hp在820至980左右，這是由于如果繼續(xù)按照左轉(zhuǎn)11°的方向行進(jìn)，右側(cè)放大的障礙物柵格會(huì)與機(jī)器人的右轉(zhuǎn)向圓重合，導(dǎo)致安全距離太小產(chǎn)生碰撞風(fēng)險(xiǎn).因此在平衡函數(shù)和障礙密度的指導(dǎo)下，并考慮目標(biāo)方向.在區(qū)間0～16候選區(qū)域中，機(jī)器人選擇初始前進(jìn)方向偏左15°的方向繼續(xù)向點(diǎn)D行進(jìn).

圖8 機(jī)器人到達(dá)點(diǎn)C時(shí)的主極坐標(biāo)直方圖 Fig.8 histogram of main polar coordinates when the robot reaches point C

在行進(jìn)過(guò)程的每個(gè)時(shí)刻，區(qū)域狀態(tài)值Hb都能夠?qū)崟r(shí)計(jì)算并記錄下行進(jìn)過(guò)程中符合條件的所有候選方向，基于VFH+算法的頻次Hp結(jié)合目標(biāo)方向和平衡函數(shù)實(shí)時(shí)計(jì)算出機(jī)器人在不同方向上的行進(jìn)代價(jià)，選擇最優(yōu)的行進(jìn)路線，最終避障機(jī)器人成功到達(dá)目標(biāo)點(diǎn).

3 避障機(jī)器人模型搭建及實(shí)驗(yàn)

3.1 Simulink模塊下的動(dòng)力學(xué)模型搭建

Simulink模塊下的Simscape提供了不同模塊來(lái)代表機(jī)械系統(tǒng)中的實(shí)體零件、載荷約束、空間關(guān)系等.用戶可通過(guò)這些模塊定義機(jī)械系統(tǒng)內(nèi)部的運(yùn)動(dòng)關(guān)系、約束及受力情況，從而建立物理仿真模型[9].

避障試驗(yàn)中使用的是基于 Arduino UNO 主板和L298N驅(qū)動(dòng)模塊為基礎(chǔ)的自制機(jī)器人.搭載HC-SR04超聲波測(cè)距模塊用于測(cè)量障礙物位置信息.該機(jī)器人的相關(guān)參數(shù)見(jiàn)表1.

表1 避障機(jī)器人的相關(guān)尺寸及參數(shù) Tab.1 related dimensions and motion parameters of obstacle avoidance robots

機(jī)器人的物理仿真模型主要由機(jī)器人左右輪與機(jī)器人主體之間的約束關(guān)系、機(jī)器人與地面環(huán)境的相對(duì)位置關(guān)系和Stateflow對(duì)機(jī)器人轉(zhuǎn)向系統(tǒng)和電機(jī)狀態(tài)的控制邏輯等組成，將建立的VFH+算法模型導(dǎo)入到物理仿真模型中實(shí)現(xiàn)模型耦合.在Multibody進(jìn)行動(dòng)力學(xué)仿真時(shí)根據(jù)VFH+算法實(shí)時(shí)計(jì)算的結(jié)果，stateflow可以實(shí)現(xiàn)對(duì)機(jī)器人運(yùn)動(dòng)方向和速度的實(shí)時(shí)控制，解決復(fù)雜的控制邏輯問(wèn)題.將stateflow控制邏輯模塊放入物理仿真耦合模型中[9].基于Simulink建立的物理耦合仿真模型見(jiàn)圖9.

圖9 避障機(jī)器人的物理仿真耦合模型示意 Fig.9 coupling of physical simulation for obstacle avoidance robot

3.2 模型性能驗(yàn)證

基于Matlab和Simulink建立物理仿真耦合模型后，進(jìn)行實(shí)際環(huán)境下的耦合模型有效性驗(yàn)證的避障實(shí)驗(yàn).Simulink不僅可以創(chuàng)建機(jī)器人系統(tǒng)的物理仿真模型，還直接提供了硬件接口實(shí)現(xiàn)對(duì)各主流機(jī)器人主板系統(tǒng)的連接和控制，包括樹(shù)莓派、樂(lè)高等平臺(tái)[10].實(shí)現(xiàn)了對(duì)目標(biāo)問(wèn)題的動(dòng)力學(xué)物理仿真模型搭建、基于算法的邏輯模型的建立和實(shí)現(xiàn)對(duì)硬件系統(tǒng)的直接控制，耦合模型對(duì)機(jī)器人運(yùn)動(dòng)狀態(tài)的直接控制，避免了將大量實(shí)驗(yàn)時(shí)間耗費(fèi)在編寫(xiě)代碼指令上，簡(jiǎn)化了實(shí)驗(yàn)流程.對(duì)基于Simulink模塊下的耦合仿真模型直接控制的機(jī)器人系統(tǒng)進(jìn)行基于VFH與VFH+算法的實(shí)地避障實(shí)驗(yàn)測(cè)試耦合模型的有效性和機(jī)器人的避障性能.

由圖10兩種算法模型的路徑對(duì)比可以看出，基于VFH算法的藍(lán)色路徑軌跡的距離與基于VFH+算法綠色軌跡的距離相比差別不大，但是在行進(jìn)途中與障礙物的安全距離過(guò)小，極易發(fā)生碰撞，而考慮了機(jī)器人尺寸因素與VFH+算法耦合模型的機(jī)器人,在相同的障礙物地圖中，其規(guī)劃路徑距離更短，安全距離更大.綜上，基于Simulink物理仿真耦合模型的有效性得到了驗(yàn)證，VFH+算法性能在全局路徑規(guī)劃和局部實(shí)時(shí)避障方面均優(yōu)于VFH算法.

圖10 兩種算法的耦合模型路徑對(duì)比 Fig.10 comparison of the coupling model path of two algorithms

4 結(jié)論

本文基于MATLAB創(chuàng)建了VFH+算法模型，基于Simulink下的Simscape模塊建立了某款機(jī)器人的物理模型.實(shí)現(xiàn)耦合模型對(duì)避障過(guò)程中的機(jī)器人運(yùn)動(dòng)狀態(tài)直接控制.可以得到以下結(jié)論：

（1）基于Matlab建立的VFH+算法模型可以實(shí)現(xiàn)對(duì)機(jī)器人避障實(shí)驗(yàn)的前期仿真實(shí)驗(yàn)，檢驗(yàn)避障算法的性能是否符合實(shí)驗(yàn)要求，提高了避障實(shí)驗(yàn)效率.

（2）采用Stateflow對(duì)機(jī)器人轉(zhuǎn)向系統(tǒng)和電機(jī)狀態(tài)實(shí)現(xiàn)控制，簡(jiǎn)化了傳統(tǒng)的代碼編寫(xiě)過(guò)程，降低了代碼錯(cuò)誤造成的實(shí)驗(yàn)風(fēng)險(xiǎn).

（3）利用Simulink提供的機(jī)器人主板系統(tǒng)平臺(tái)的硬件支持包，實(shí)現(xiàn)了耦合模型對(duì)機(jī)器人運(yùn)動(dòng)狀態(tài)的直接控制，避免了代碼移植過(guò)程中代碼缺失等因素對(duì)避障試驗(yàn)的影響，簡(jiǎn)化了避障試驗(yàn)的實(shí)驗(yàn)流程，為機(jī)器人避障實(shí)驗(yàn)研究提供了新的思路與方法.