【正文】 yield break。 var fault = 。 (replace)。 = 。 yield break。 yield return ()。 寧波大學(xué) 信息科學(xué)與工程學(xué)院 本科畢業(yè)設(shè)計(jì)（論文） 3 圖 把主服務(wù)程的主端口傳遞給窗體。 } public bool paused。為了能夠在窗體中向主服務(wù)程序發(fā)送消息，我們首先需要把主服務(wù)程序的消息端口告訴窗體類，在窗體類中新增一個(gè)全局變量保存主服務(wù)程序的消息端口： 圖 然后，在 中定義所需要的消息體類型： 圖 同時(shí)在服務(wù)端口中新增一個(gè) PauseEngine 消息，將 zmhOperations 改為： 圖 在創(chuàng)建窗體時(shí)，把消息端口傳遞給窗體類中的全局變量 _mainPort。 手動(dòng)控制器的實(shí)現(xiàn) 現(xiàn)在我們 以啟動(dòng) /暫停仿真引擎為例。 （ 7）“加速”鍵： 將加速的速度值預(yù)先通過程序編寫出來 ，然后 當(dāng)按下此鍵時(shí) 實(shí)現(xiàn)速度的增加。 （ 5）“左轉(zhuǎn)”鍵：通過控制機(jī)器人左右輪子的轉(zhuǎn)速來實(shí)現(xiàn)機(jī)器人的轉(zhuǎn)向，向左轉(zhuǎn)時(shí)， 右輪的速度應(yīng)大于 左 輪的速度。 （ 3） “前進(jìn)”鍵： 通過控制機(jī)器人左右輪子的轉(zhuǎn)向來實(shí)現(xiàn)機(jī)器人向前行進(jìn)。 各個(gè)按鍵的功能： （ 1）“自動(dòng) /手動(dòng)”切換鍵：實(shí)現(xiàn)自動(dòng)與手動(dòng)操作之間的靈活切換。如圖 所示。 圖 手動(dòng)控制操作面板示意圖 手動(dòng)控制操作面板上有前進(jìn)，后退，左轉(zhuǎn)，右轉(zhuǎn)，加速，減速，自動(dòng) /手動(dòng)切換和啟動(dòng) /暫停仿真引擎 8 個(gè)按鍵。 我們 設(shè)計(jì)出一個(gè) 可視化 的 操作界面 來 介入機(jī)器人 的 仿真過程，譬如啟動(dòng) /暫停仿真， 自動(dòng) /手動(dòng)的切換， 手工設(shè)置機(jī)器人的速度和 前進(jìn)、后退、左轉(zhuǎn)、右轉(zhuǎn)等 。 } 寧波大學(xué) 信息科學(xué)與工程學(xué)院 本科畢業(yè)設(shè)計(jì)（論文） 24 圖 仿真環(huán)境下的實(shí)時(shí) 監(jiān)控畫面 手動(dòng)控制 器的設(shè)計(jì) 上文講到的都是機(jī)器人自動(dòng)巡視，雖然自動(dòng)巡視具有很多的優(yōu)點(diǎn)，如方便，快捷，自主等，但是也有些不足，比如操作不靈活，功能單一等。 = bmpWebcam。 internal void UpdateWebcam(Bitmap bmp) { bmpWebcam = bmp。這樣就可以實(shí)現(xiàn)不斷的刷新 圖像，從而到達(dá)實(shí)時(shí)顯示的目的。 })。 size = 。 Size size = new Size(0, 0)。 }))。 ()。然后就是在 start 方法中定義一個(gè)函數(shù)來啟動(dòng)窗體： 圖 啟動(dòng)窗體程序 啟動(dòng)窗體 后， 把攝像頭拍攝到的圖像顯示在 窗體里， 接下來 就是 不斷地刷新圖像 來實(shí)時(shí)更新圖像 ， 以便我們更方便更準(zhǔn)確的觀察，因此 需要在窗體類中新添一個(gè)方法 UpdateWebcam供調(diào)用刷新圖像。 } 寧波大學(xué) 信息科學(xué)與工程學(xué)院 本科畢業(yè)設(shè)計(jì)（論文） 22 圖 這段程序是 定義一個(gè)選擇仲裁器，往 WebCam 服務(wù)端口發(fā)送一個(gè) QueryFrame 請(qǐng)求當(dāng)請(qǐng)求成功時(shí)，將數(shù)據(jù)保存到 rgbData 中，并將圖像尺寸保存到 size 中 ； 當(dāng)請(qǐng)求失敗時(shí)，給出出錯(cuò)信息 。 } } Activate((false, _dateTimePortImage, UpdateImage))。 bytesPerPixel == 4)) { if (bytesPerPixel == 3) destBitmap = new Bitmap(srcDataWidth, srcDataHeight, )。 bytesPerPixel == 3) || ( == amp。 bytesPerPixel == 3) || ( == amp。 if (rgbData != null) { || != srcDataWidth || != srcDataHeight || ( == amp。 }, failure = { LogError(())。 yield return ((), success = { rgbData = 。程序代碼如下： 寧波大學(xué) 信息科學(xué)與工程學(xué)院 本科畢業(yè)設(shè)計(jì)（論文） 3 圖 攝像頭數(shù)據(jù)獲取程序 IEnumeratorITask UpdateImage(DateTime dateTime) { byte[] rgbData = null。再 設(shè) 1v ， rv 分別為機(jī)器人左、右輪速 ，根據(jù)機(jī)器人右側(cè)與墻的距離 d（如圖所示）來計(jì)算實(shí)時(shí)的 1v ， rv 。范圍內(nèi)的點(diǎn)，再經(jīng)過篩選，我們選擇距離為 50mm~1200mm 之間的點(diǎn)進(jìn)行擬合 ，見式（ 36）。 當(dāng)機(jī)器人靠近墻時(shí)， 我們需要的是落在 0176。 圖 沿墻走擬合示意圖 寧波大學(xué) 信息科學(xué)與工程學(xué)院 本科畢業(yè)設(shè)計(jì)（論文） 20 機(jī)器人沿墻走巡視的特殊情況 機(jī)器人在內(nèi)室進(jìn)行巡視時(shí)，經(jīng)常會(huì)出現(xiàn)機(jī)器人走到直角地帶，如圖 所示。由于程序中的坐標(biāo)軸與數(shù)學(xué)定義上的坐標(biāo)軸不一樣，因此我們要對(duì)使用的角度進(jìn)行轉(zhuǎn)換，見公式（ 39） 。 ~90176。的區(qū)別。 寧波大學(xué) 信息科學(xué)與工程學(xué)院 本科畢業(yè)設(shè)計(jì)（論文） 3 a rc ta nD? ? （ 38） 因?yàn)閿M合出來的直線斜率有正負(fù)之分，所以計(jì)算出來的角度，如果按數(shù)學(xué)上定義，則 ?有大于 90176。 然后根據(jù)最小二乘原理和直線擬合的公式 （ 34）計(jì)算出斜率 B，截距 A，而 C=A,D=B。再 設(shè) 1v ， rv 分別為機(jī)器人左、右輪速 ，根據(jù)機(jī)器人右側(cè)與墻的距離 d（如圖所示）來計(jì)算實(shí)時(shí)的 1v ， rv 。然后 通過極坐標(biāo)的轉(zhuǎn)換 將篩選出來的點(diǎn)轉(zhuǎn)換 成 對(duì)應(yīng) 平面 坐標(biāo) ， 極坐標(biāo)轉(zhuǎn)換公式 見式（ 37）。范圍內(nèi)的點(diǎn)，再經(jīng)過篩選，我們選擇距離為 50mm~1200mm 之間的點(diǎn)進(jìn)行擬合。（ 2）當(dāng)激光打在墻上的個(gè)數(shù)大于 3 時(shí)（ n＞ 3），與墻的距離小于 8000mm（ d＜ 8000mm）時(shí)，此時(shí)就要進(jìn)行擬合； 寧波大學(xué) 信息科學(xué)與工程學(xué)院 本科畢業(yè)設(shè)計(jì)（論文） 18 圖 機(jī)器人沿墻走巡視的實(shí)現(xiàn) 設(shè)定機(jī)器人的右側(cè)始終靠近墻 ， 當(dāng)機(jī)器人靠近墻時(shí)， 為了分析方便，現(xiàn)我們 取激光打在墻上的 A1~A10 十個(gè)點(diǎn) ， 如圖 所示， 而我們需要的是落在 0176。 圖 沿墻走激光 束分布 示意圖 （ 2）沿墻走的判斷 設(shè)激光打在墻上的個(gè)數(shù)為 n，機(jī)器人與墻的距離為 d。 這 360 份用極坐標(biāo)表示，見式（ 36）。的半圓分成 0~360 共 361 份，寧波大學(xué) 信息科學(xué)與工程學(xué)院 本科畢業(yè)設(shè)計(jì)（論文） 3 其角度分辨率為 176。 圖 沿墻走仿真示意圖 （ 1）沿墻走的 激光束分布 本文中 所用的 仿真 激光傳感器是按照德國 SICK 公司生產(chǎn)的 LMS200 設(shè)計(jì)的，它的掃描范圍最大可以到達(dá) 8m，掃 描角度為 180176。 直線擬合 假定所求實(shí)驗(yàn)數(shù)據(jù)的最佳擬合直線方程為： 寧波大學(xué) 信息科學(xué)與工程學(xué)院 本科畢業(yè)設(shè)計(jì)（論文） 16 Y A BX?? （ 33） 式中， A 為直線的截距； B 為其斜率。 圖 沿墻走示意圖 最小二乘原理 設(shè)有一獨(dú)立等精度的測(cè)量列 i? （ i=1， 2， 3， ， n），其殘差（或偏差）為 i? = i??? ，殘差的平方和為： n n n222i i ii = 1 i = 1 i == = n? ? ? ? ??? ? ? 21（ ） （ ） （ 31） 式中， n ii=1= n?? ? 。 寧波大學(xué) 信息科學(xué)與工程學(xué)院 本科畢業(yè)設(shè)計(jì)（論文） 3 研究的重點(diǎn)不是路徑規(guī)劃，而是室內(nèi)巡視機(jī)器人的設(shè)計(jì)，所以考慮的到執(zhí)行的難以程度 ，所到達(dá)的效果（由于機(jī)器人要進(jìn)行巡視，本身帶有攝像 頭，那么沿墻走就可以到達(dá)看得到室內(nèi)的一切這個(gè)效果）等，選擇沿墻走這種方法就可以到達(dá)要求的效果，因此選擇沿墻走的路徑規(guī)劃方法。 } } Activate((false, _dateTimePort, UpdateLaser))。 ((200), _dateTimePort)。 LogInfo(Obstacle Avoid!)。 i 2 * laserCount / 3。 機(jī)器人沿墻巡視的 設(shè)計(jì) 本文所采用的路徑規(guī)劃方法是沿墻走，即機(jī)器人沿著室內(nèi)的墻壁進(jìn)行巡視。 它所要達(dá)到的目的是使機(jī)器人 在存在障礙物的環(huán)境中， 按照 一定的標(biāo)準(zhǔn) ， 尋找到 一條從 起點(diǎn) 到 終點(diǎn) 的最優(yōu)或近似最優(yōu)的無障礙路徑 。 在這里，我們用到了差分驅(qū)動(dòng)輪的消息端口 _simulatedDifferentialDriveServicePort，并根據(jù)激光數(shù)據(jù)向其發(fā)送驅(qū)動(dòng)參數(shù)。范圍內(nèi)有障礙物且只 小于 500 的安全距離 時(shí)，機(jī)器人向右轉(zhuǎn)的避障程序如下圖所示。 （ 2）當(dāng)激光檢測(cè)到 90176。 } 寧波大學(xué) 信息科學(xué)與工程學(xué)院 本科畢業(yè)設(shè)計(jì)（論文） 14 圖 這兩條程序是用來刷新激光數(shù)據(jù)，這里我們?cè)O(shè)定的定時(shí)器定時(shí)時(shí)間是 200ms，即 。 ((200), _dateTimePort)。 LogInfo(Obstacle Avoid!)。 } } int laserCount = 。 ((200), _dateTimePort)。 LogInfo(Obstacle Avoid!)。 i laserCount / 2。i++ int laserCount = 。 int i = laserCount/3。 的 范圍的數(shù)據(jù)。 圖 這條語句是實(shí)現(xiàn)取 激光數(shù)據(jù) 60176。范圍內(nèi)有障礙物且只 小于 500mm 的安全距離 時(shí)，機(jī)器人向左轉(zhuǎn)的避障程序如下圖所示。 （ 1）當(dāng)激光檢測(cè)到 60176。 ((600), _dateTimePort)。這樣就 實(shí)現(xiàn) 了機(jī)器人的 避障 功能 。 ~120176。 ~90176。）。和 90176。（即 60176。 為了更好的實(shí)現(xiàn)避障功能，使避障時(shí)機(jī)器人的轉(zhuǎn)向與下面將要講到的沿墻走 路徑規(guī)劃有效的結(jié)合起來，以達(dá)到巡視時(shí)的最佳效果，現(xiàn)我們將這 60176。 范圍的，我們?nèi)≈虚g的 60176。 本文所采用的避障方法： 獲取激光數(shù)據(jù)后，我們可以根據(jù)獲取的數(shù)據(jù)編寫一個(gè)避障的控制程序，即當(dāng)激光檢測(cè)到的前方障礙物距離小于 500mm，就轉(zhuǎn)個(gè)方向。 這樣只能執(zhí)行一次，為了使 UpdateLaser 任務(wù)不斷重復(fù)執(zhí)行，我們?cè)?UpdateLaser 任務(wù)函數(shù)的結(jié)尾再加上上述兩句，這樣的話 UpdateLaser 就能不斷地重復(fù)了。 第二句啟動(dòng)一個(gè)延時(shí)計(jì)數(shù)，當(dāng)經(jīng)過 600 毫秒后，就往 _dateTimePort 發(fā)送一個(gè)消息。 寧波大學(xué) 信息科學(xué)與工程學(xué)院 本科畢業(yè)設(shè)計(jì)（論文） 3 圖 啟動(dòng)計(jì)時(shí)器程序 第一句告訴 DSS，當(dāng) _dateTimePort 消息來時(shí)，執(zhí)行 UpdateLaser 任務(wù)，第一個(gè)參數(shù) false表示只執(zhí)行一次。 yield break。 } Activate((false, _dateTimePort, UpdateLaser))。 if (!HasError(sensorOrFault)) { sensorState = ()sensorOrFault。 zm