的偏差轉(zhuǎn)化為直流電機(jī)Y的加速度、速度和位置指令值，F(xiàn)PGA再結(jié)合電流傳感器C2和電機(jī)光電編碼器的反饋，生成驅(qū)動直流電機(jī)Y的PWM波，通過直流電機(jī)Y的伺服控制調(diào)整直流無刷電機(jī)X的行走方向角度，加速度計Al、陀螺儀Gl和方向傳感器Dl實時記錄滅火機(jī)器人的加速度、速度和角度信號并送給FPGA，F(xiàn)PGA記錄并儲存滅火機(jī)器人的瞬時加速度、速度和位置信息。在下一個采樣周期到來時，F(xiàn)PGA根據(jù)直流電機(jī)Y角度偏差再微調(diào)直流電機(jī)Y的PWM波輸入，通過此方式可以精確調(diào)整滅火機(jī)器人的姿態(tài)，使其重新回到設(shè)定中心位置并沿著設(shè)定方向前進(jìn)，行走速度全程滿足探尋需求。當(dāng)滅火機(jī)器人在加速度計Al、陀螺儀Gl和方向傳感器Dl的控制下精確運(yùn)動一格到達(dá)新地址時，微處理器將更新其坐標(biāo)。
[0048]如果在坐標(biāo)(X，Y)時的方向為北，在更新其坐標(biāo)為(X，Y+1)，新坐標(biāo)方向依舊為北；如果在坐標(biāo)(X，Y)時的方向為東，在更新其坐標(biāo)為(Χ+1，Y)，新坐標(biāo)方向依舊為東；如果在坐標(biāo)(Χ，γ)時的方向為南，在更新其坐標(biāo)為(X，Υ-1)，新坐標(biāo)方向依舊為南；如果在坐標(biāo)(X，Y)時的方向為西，在更新其坐標(biāo)為(x-l，Y)，新坐標(biāo)方向依舊為西；
光電傳感器S7開始工作，對新坐標(biāo)下的光源進(jìn)行判斷，如果光電傳感器捕捉到信號將通知控制器發(fā)現(xiàn)目標(biāo)，控制器會讓光電傳感器禁止工作2秒工作，2秒后重新開啟光電傳感器，如果再次捕捉到光電信號，控制器會開啟攜帶的干冰滅火器的電磁閥，開始對蠟燭進(jìn)行噴灑干冰直至光源消失，然后控制器調(diào)出滅火機(jī)器人已經(jīng)搜索的路徑，并舍棄未搜尋的目標(biāo)，通過洪水算法找出已經(jīng)搜尋過房間的最佳路徑，然后滅火機(jī)器人按照此路徑快速回到搜尋起點(diǎn)；
如果光電傳感器S7沒有捕捉到新坐標(biāo)下的光源，滅火機(jī)器人將離開目前房間格，將繼續(xù)搜尋并更新其坐標(biāo)；
在滅火機(jī)器人沿著當(dāng)前方向向前運(yùn)動過程中如果傳感器SI和S6判斷前方有擋墻進(jìn)入運(yùn)動范圍，并且此時傳感器S2、S3、S4、S5分別判斷左右都有擋墻時，滅火機(jī)器人進(jìn)入所謂的“死胡同”，控制器將存儲此時坐標(biāo)(X，Y)，根據(jù)傳感器SI和S6的反饋計算出向前運(yùn)動停車的位置參數(shù)YS1，F(xiàn)PGA根據(jù)搜尋控制器速度和加速度要求生成直流無刷電機(jī)X要運(yùn)行的位置、速度和加速度指令值，F(xiàn)PGA再結(jié)合電流傳感器Cl和電機(jī)光電編碼器的反饋生成控制直流無刷電機(jī)X的PWM控制信號，控制信號經(jīng)驅(qū)動橋放大后驅(qū)動直流無刷電機(jī)X向前運(yùn)動，控制器實時采集其速度大小，并結(jié)合光電編碼器Cl的反饋實時調(diào)整其大小，F(xiàn)PGA會根據(jù)直流無刷電機(jī)X的速度自動調(diào)節(jié)電機(jī)M的伺服控制，自動改變真空吸盤對地面的吸附能力。在沿著當(dāng)前房間格向前停車過程中，滅火機(jī)器人根據(jù)前進(jìn)方向左右擋墻的房間信息進(jìn)入雙墻導(dǎo)航模式，然后再結(jié)合設(shè)定的左右擋墻導(dǎo)航閥值，加速度計Al、陀螺儀Gl和方向傳感器Dl實時記錄滅火機(jī)器人的加速度、速度和角度信號并送給FPGA，F(xiàn)PGA記錄并儲存滅火機(jī)器人的瞬時加速度、速度和位置信息。當(dāng)滅火機(jī)器人快速探索脫離了設(shè)定中心位置時，微處理器把離開中心位置的偏差轉(zhuǎn)化為直流電機(jī)Y的加速度、速度和位置指令值，，F(xiàn)PGA再結(jié)合直流電機(jī)Y的電流C2和光電編碼器反饋，生成驅(qū)動直流電機(jī)Y的PffM波，通過直流電機(jī)Y的伺服控制調(diào)整直流無刷電機(jī)X的行走方向角度，加速度計Al、陀螺儀Gl和方向傳感器Dl實時記錄滅火機(jī)器人的加速度、速度和角度信號并送給FPGA，F(xiàn)PGA記錄并儲存滅火機(jī)器人的瞬時加速度、速度和位置信息。在下一個采樣周期到來時，微處理器根據(jù)直流電機(jī)Y角度偏差再微調(diào)直流電機(jī)Y的PWM波輸入，通過此方式可以精確調(diào)整滅火機(jī)器人的姿態(tài)，使其重新回到設(shè)定中心位置并沿著設(shè)定方向前進(jìn)。當(dāng)滅火機(jī)器人進(jìn)入死胡同停車后，STM32F407開啟另外一套傳感器，然后滅火機(jī)器人沿著原先相反的方向運(yùn)動，滅火機(jī)器人開始更新其坐標(biāo)信息。
[0049]如果在坐標(biāo)(X，Y)時的方向為北，則更新其坐標(biāo)為(X，Y)，新坐標(biāo)方向為南；如果在坐標(biāo)(X，Y)時的方向為東，則更新其坐標(biāo)為(XI，Y)，新坐標(biāo)方向為西；如果在坐標(biāo)(X，Y)時的方向為南，則更新其坐標(biāo)為(X，Y)，新坐標(biāo)方向為北；如果在坐標(biāo)(X，Y)時的方向為西，則更新其坐標(biāo)為(X，Y)，新坐標(biāo)方向為東；
在滅火機(jī)器人沿著當(dāng)前方向向前運(yùn)動過程中如果有擋墻進(jìn)入前方的運(yùn)動范圍，并且此時左右的傳感器S2和S3判斷左邊有擋墻，而S4、S5判斷右方?jīng)]有擋墻時，滅火機(jī)器人將存儲此時坐標(biāo)(X，Y)，然后滅火機(jī)器人將按照圖7的曲線行走；
在右轉(zhuǎn)彎時，F(xiàn)PGA首先把行走直線很短的距離R90_Leading按照控制器不同搜尋速度和加速度要求生成直流無刷電機(jī)X要運(yùn)行的位置、速度和加速度指令值，F(xiàn)PGA再結(jié)合電流傳感器Cl和電機(jī)光電編碼器的反饋生成控制直流無刷電機(jī)X的PffM控制信號，控制信號經(jīng)驅(qū)動橋放大后驅(qū)動直流無刷電機(jī)X向前運(yùn)動，控制器實時采集其速度大小，并結(jié)合光電編碼器Cl的反饋實時調(diào)整其大小，F(xiàn)PGA會根據(jù)直流無刷電機(jī)X的速度自動調(diào)節(jié)電機(jī)M的伺服控制，自動改變真空吸盤對地面的吸附能力。在沿著當(dāng)前房間格向前運(yùn)動過程中，滅火機(jī)器人根據(jù)前進(jìn)方向左擋墻的房間信息進(jìn)入單左墻導(dǎo)航模式，然后再結(jié)合設(shè)定的左擋墻導(dǎo)航閥值，加速度計Al、陀螺儀Gl和方向傳感器Dl實時記錄滅火機(jī)器人的加速度、速度和角度信號并送給FPGA，F(xiàn)PGA記錄并儲存滅火機(jī)器人的瞬時加速度、速度和位置信息。當(dāng)滅火機(jī)器人快速探索脫離了設(shè)定中心位置時，微處理器把離開中心位置的偏差轉(zhuǎn)化為直流電機(jī)Y的加速度、速度和位置指令值，，F(xiàn)PGA再結(jié)合直流電機(jī)Y的電流C2和光電編碼器反饋，生成驅(qū)動直流電機(jī)Y的PWM波，通過直流電機(jī)Y的伺服控制調(diào)整直流無刷電機(jī)X的行走方向角度，加速度計Al、陀螺儀Gl和方向傳感器Dl實時記錄滅火機(jī)器人的加速度、速度和角度信號并送給FPGA，F(xiàn)PGA記錄并儲存滅火機(jī)器人的瞬時加速度、速度和位置信息。在下一個采樣周期到來時，微處理器根據(jù)直流電機(jī)Y角度偏差再微調(diào)直流電機(jī)Y的PWM波輸入，通過此方式可以精確調(diào)整滅火機(jī)器人的姿態(tài)，使其重新回到設(shè)定中心位置并沿著設(shè)定方向前進(jìn)。
[0050]當(dāng)?shù)竭_(dá)既定目標(biāo)時，傳感器參考值R90_FrontWallRef開始工作，防止外界干擾開始做誤差補(bǔ)償。誤差補(bǔ)償結(jié)束后開始調(diào)整直流無刷電機(jī)X的速度和方向使其完成弧度ARC的運(yùn)動曲線。
[0051]FPGA首先把弧度ARC的角度按照控制器不同搜尋速度和加速度要求生成直流無刷電機(jī)X的速度大小和直流電機(jī)Y要運(yùn)行的位置、速度和加速度指令值，F(xiàn)PGA再結(jié)合電機(jī)Y光電編碼器和電流傳感器C2的反饋生成驅(qū)動直流電機(jī)Y的PffM波驅(qū)動信號，然后驅(qū)動信號放大后驅(qū)動直流電機(jī)Y運(yùn)動，電機(jī)Y通過機(jī)械裝置調(diào)整直流無刷電機(jī)X的運(yùn)動角度。在運(yùn)動過程中，控制器根據(jù)直流無刷電機(jī)X的光電編碼器的反饋實時調(diào)整其PWM波輸出，使其速度滿足探尋轉(zhuǎn)彎需要，F(xiàn)PGA會根據(jù)直流無刷電機(jī)X的速度自動調(diào)節(jié)電機(jī)M的伺服控制，自動改變真空吸盤對地面的吸附能力。在轉(zhuǎn)彎過程中，方向傳感器Dl實時記錄滅火機(jī)器人的角度，然后控制器就可以得到真實轉(zhuǎn)彎角度與設(shè)定角度之間的偏差；當(dāng)滅火機(jī)器人快速搜尋轉(zhuǎn)彎脫離了設(shè)定位置時，在新的采樣周期內(nèi)，角度偏差大小被FPGA轉(zhuǎn)化為直流電機(jī)Y新的位置、速度和加速度指令值，，F(xiàn)PGA再結(jié)合電機(jī)Y的光電編碼器和電流傳感器C2的反饋生成控制直流電機(jī)Y的PWM波控制信號，然后驅(qū)動信號放大后驅(qū)動直流電機(jī)Y運(yùn)動，電機(jī)Y通過機(jī)械裝置再次調(diào)整直流無刷電機(jī)X的運(yùn)動角度，使滅火機(jī)器人轉(zhuǎn)彎的角度逼近于設(shè)定值，通過此方法可以準(zhǔn)確調(diào)整滅火機(jī)器人在弧度ARC時的姿態(tài)，最終完成90度轉(zhuǎn)彎。
[0052]當(dāng)?shù)竭_(dá)既定目標(biāo)后，系統(tǒng)依靠傳感器S2、S3開始導(dǎo)航，控制器把直線行走很短的距離R90_Passing按照控制器不同搜尋速度和加速度要求生成直流無刷電機(jī)X要運(yùn)行的位置、速度和加速度指令值，F(xiàn)PGA再結(jié)合電流傳感器Cl和電機(jī)光電編碼器的反饋生成控制直流無刷電機(jī)X的PWM控制信號，控制信號經(jīng)驅(qū)動橋放大后驅(qū)動直流無刷電機(jī)X向前運(yùn)動，控制器實時采集其速度大小，并結(jié)合光電編碼器Cl的反饋實時調(diào)整其大小，F(xiàn)PGA會根據(jù)直流無刷電機(jī)X的速度自動調(diào)節(jié)電機(jī)M的伺服控制，自動改變真空吸盤對地面的吸附能力。在沿著當(dāng)前房間格向前運(yùn)動過程中，滅火機(jī)器人根據(jù)前進(jìn)方向左擋墻的房間信息進(jìn)入單左墻導(dǎo)航模式，然后再結(jié)合設(shè)定的左擋墻導(dǎo)航閥值，加速度計Al、陀螺儀Gl和方向傳感器Dl實時記錄滅火機(jī)器人的加速度、速度和角度信號并送給FPGA，F(xiàn)PGA記錄并儲存滅火機(jī)器人的瞬時加速度、速度和位置信息。當(dāng)滅火機(jī)器人快速探索脫離了設(shè)定中心位置時，微處理器把離開中心位置的偏差轉(zhuǎn)化為直流電機(jī)Y的加速度、速度和位置指令值，，F(xiàn)PGA再結(jié)合直流電機(jī)Y的電流C2和光電編碼器反饋，生成驅(qū)動直流電機(jī)Y的PWM波，通過直流電機(jī)Y的伺服控制調(diào)整直流無刷電機(jī)X的行走方向角度，加速度計Al、陀螺儀Gl和方向傳感器Dl實時記錄滅火機(jī)器人的加速度、速度和角度信號并送給FPGA，F(xiàn)PGA記錄并儲存滅火機(jī)器人的瞬時加速度、速度和位置信息。在下一個采樣周期到來時，微處理器根據(jù)直流電機(jī)Y角度偏差再微調(diào)直流電機(jī)Y的PWM波輸入，通過此方式可以精確調(diào)整滅火機(jī)器人的姿態(tài)，使其重新回到設(shè)定中心位置并沿著設(shè)定方向前進(jìn)。當(dāng)?shù)竭_(dá)既定目標(biāo)后完成整個右轉(zhuǎn)彎的軌跡曲線運(yùn)動，然后控制其開始更新其坐標(biāo)和方向。