【正文】 標(biāo)機(jī)器人不直接支持旋轉(zhuǎn)，必須通過(guò)機(jī)器人的左右轉(zhuǎn)動(dòng)，獲得X方向上的旋轉(zhuǎn)分量。，點(diǎn)A、點(diǎn)B代表機(jī)器人的左右輪，可以看出，以上旋轉(zhuǎn)過(guò)程等效于將點(diǎn)A、點(diǎn)B移動(dòng)到點(diǎn)Aˊ和點(diǎn)Bˊ。那么，如果機(jī)器人此刻作右轉(zhuǎn)運(yùn)動(dòng)，對(duì)于點(diǎn)A，會(huì)在右轉(zhuǎn)作用的驅(qū)動(dòng)下沿AC方向移動(dòng)，但是，由于有車輪貼緊管壁這個(gè)前提，點(diǎn)A沿AC方向的運(yùn)動(dòng)只能作為車輪貼壁移動(dòng)的一個(gè)分量存在，（b）所示，當(dāng)點(diǎn)A在水平方向右移到點(diǎn)C時(shí)，其真實(shí)位置已經(jīng)到達(dá)點(diǎn)Aˊ，即貼壁運(yùn)動(dòng)這一性質(zhì)決定了點(diǎn)A在豎直方向必然還有一個(gè)位移AE。點(diǎn)B的運(yùn)動(dòng)也是類似的，（c）所示，在點(diǎn)B完成水平右移運(yùn)動(dòng)BD的同時(shí)，其實(shí)際位置已經(jīng)到達(dá)點(diǎn)Bˊ。這一過(guò)程說(shuō)明，機(jī)器人在管道內(nèi)作右轉(zhuǎn)運(yùn)動(dòng)，就可以得到逆時(shí)針旋轉(zhuǎn)分量，同樣，機(jī)器人在管道內(nèi)作左轉(zhuǎn)運(yùn)動(dòng)，就可以得到順時(shí)針旋轉(zhuǎn)分量。 X方向旋轉(zhuǎn)運(yùn)動(dòng)分解以上過(guò)程實(shí)現(xiàn)了機(jī)器人沿X向的旋轉(zhuǎn)運(yùn)動(dòng)，但是依然存在一個(gè)問(wèn)題，就是貼壁運(yùn)動(dòng)這個(gè)前提并不是絕對(duì)成立的。根據(jù)上面的分析，我們知道X向旋轉(zhuǎn)只是作為機(jī)器人運(yùn)動(dòng)的一個(gè)分量存在，完整的運(yùn)動(dòng)過(guò)程還包括沿管道中心軸的前后運(yùn)動(dòng)和繞重垂線的旋轉(zhuǎn)運(yùn)動(dòng)，其中，軸向前后運(yùn)動(dòng)對(duì)自平衡過(guò)程不構(gòu)成影響，但是繞重垂線的旋轉(zhuǎn)運(yùn)動(dòng)會(huì)直接破壞貼壁移動(dòng)這個(gè)前提，即在某個(gè)極端情況下可能會(huì)出現(xiàn)一個(gè)車輪懸空的問(wèn)題，如果這個(gè)車輪是主動(dòng)輪，可能會(huì)影響自平衡策略的執(zhí)行。針對(duì)這個(gè)問(wèn)題，本文設(shè)計(jì)一個(gè)簡(jiǎn)單的處理方法，即執(zhí)行自平衡策略的過(guò)程中時(shí)刻保持兩個(gè)主動(dòng)輪處于運(yùn)動(dòng)狀態(tài)。可以確定的是，機(jī)器人不可能出現(xiàn)有兩個(gè)或兩個(gè)以上車輪懸空的情況，即至少有三個(gè)車輪是貼壁的，那么這就保證了在自平衡策略執(zhí)行的過(guò)程中，至少有一個(gè)主動(dòng)輪貼壁并處于運(yùn)動(dòng)狀態(tài)，這必然會(huì)改變機(jī)器人當(dāng)前的位姿狀態(tài)，由于三個(gè)車輪貼壁狀態(tài)是非常不穩(wěn)定的，在至少一個(gè)主動(dòng)輪的驅(qū)動(dòng)下必然要回歸四輪貼壁的穩(wěn)定狀態(tài)。這樣，在機(jī)器人處于不穩(wěn)定狀態(tài)的情況下，機(jī)器人會(huì)靠主動(dòng)輪驅(qū)動(dòng)回歸穩(wěn)定狀態(tài)，機(jī)器人在穩(wěn)定狀態(tài)時(shí)，會(huì)執(zhí)行自平衡策略。因此，在機(jī)器人執(zhí)行自平衡策略的過(guò)程中可能會(huì)出現(xiàn)多次從不穩(wěn)定狀態(tài)回歸穩(wěn)定狀態(tài)（貼壁運(yùn)動(dòng)）的過(guò)程，但是最終能夠完成自平衡策略的執(zhí)行過(guò)程。因此，只要保證在自平衡策略的執(zhí)行過(guò)程中兩個(gè)主動(dòng)輪都處于運(yùn)動(dòng)狀態(tài)，那么，就可以認(rèn)為貼壁運(yùn)動(dòng)這一前提是成立的。根據(jù)上文的分析，自平衡過(guò)程是通過(guò)機(jī)器人左右運(yùn)動(dòng)實(shí)現(xiàn)的，而目標(biāo)控制系統(tǒng)通過(guò)調(diào)節(jié)左右輪的速度差來(lái)實(shí)現(xiàn)左右轉(zhuǎn)彎，關(guān)于速度差的取值存在這樣一個(gè)矛盾，如果速度差較大，那么可以盡快地獲得位姿補(bǔ)償，但是由于慣性作用，機(jī)器人可能會(huì)反復(fù)沖過(guò)平衡位置，造成抖動(dòng)。如果速度差較小，雖然能夠保證穩(wěn)定運(yùn)行，但是位姿調(diào)整的時(shí)間較長(zhǎng)。為此，本文使用了兩個(gè)傾角閾值和，同時(shí)，針對(duì)這兩個(gè)傾角閾值設(shè)定兩個(gè)速度差和，其中大于，大于。使用中斷方式啟動(dòng)自平衡過(guò)程時(shí)，當(dāng)目標(biāo)控制系統(tǒng)檢測(cè)到機(jī)器人的傾角大于時(shí)，啟動(dòng)自平衡過(guò)程，并設(shè)定左右輪的速度差為，此時(shí)機(jī)器人以較快的速度回歸平衡，一旦檢測(cè)到傾角小于，控制系統(tǒng)將左右輪的速度差調(diào)至，這樣，圖中是傾斜容忍裕度。 自平衡策略流程圖目標(biāo)控制系統(tǒng)的主要任務(wù)之一就是攜帶放射源專用設(shè)備進(jìn)入管道進(jìn)行探傷拍攝，為了提高拍攝質(zhì)量，要求在拍攝時(shí)機(jī)器人本體保持平衡，那么這就要求在目標(biāo)機(jī)器人到達(dá)探傷點(diǎn)時(shí)手動(dòng)執(zhí)行自平衡過(guò)程，盡管此時(shí)傾角可能并沒(méi)有達(dá)到閾值。，當(dāng)這種自平衡過(guò)程完成后，目標(biāo)機(jī)器人已經(jīng)離開(kāi)探傷點(diǎn)一段距離了。針對(duì)這個(gè)問(wèn)題，本文采用分段執(zhí)行的方法完成手動(dòng)自平衡過(guò)程，手動(dòng)自平衡過(guò)程根據(jù)初始傾角的大小，分為以下兩種情況：（1）如果，那么系統(tǒng)分兩個(gè)階段執(zhí)行自平衡策略，每個(gè)階段完成的傾角補(bǔ)償，只不過(guò)第一階段在前進(jìn)中執(zhí)行自平衡策略，而第二階段在后退過(guò)程中執(zhí)行自平衡策略，整個(gè)過(guò)程以的速度差運(yùn)行。（2）如果，那么系統(tǒng)分四個(gè)階段執(zhí)行自平衡策略，其中第一、二階段以的速度差，分別在前進(jìn)和后退過(guò)程中各完成的傾角補(bǔ)償。而第三、四階段按照情況（1）完成初始傾角為的自平衡過(guò)程。如果按上述過(guò)程執(zhí)行一輪后，仍然達(dá)不到平衡要求，那么系統(tǒng)再次執(zhí)行上述分段自平衡過(guò)程，直到傾角小于容忍閾值。這種分階段自平衡過(guò)程的優(yōu)點(diǎn)是完成平衡調(diào)節(jié)后，機(jī)器人不會(huì)離開(kāi)出發(fā)點(diǎn)太遠(yuǎn)，適用于定點(diǎn)作業(yè)的情況，其根本原理和單向行走的自平衡過(guò)程是完全一致的。 放射源中心定位策略本課題要求目標(biāo)機(jī)器人能夠攜帶放射源進(jìn)入管道進(jìn)行焊縫探傷，為了得到較好的底片成像質(zhì)量，管道機(jī)器人需要將放射源定位到管道中央，因此，本文為目標(biāo)控制系統(tǒng)設(shè)計(jì)了一個(gè)可靠精確的放射源中心定位策略，在放射源定位策略的實(shí)現(xiàn)中需要用到四路超聲波傳感器。圖中以質(zhì)點(diǎn)M為中心的正方形的每一條邊對(duì)應(yīng)一路超聲波傳感器，超聲波的發(fā)射方向與正方形的邊垂直。圖中，第1路超聲波傳感器（對(duì)應(yīng)正方形編號(hào)為1的邊）發(fā)出超聲波，可測(cè)得反射面R的距離為s，而由于每一路超生波傳感器與質(zhì)點(diǎn)M的距離d是固定的（），這樣就可以計(jì)算得到質(zhì)點(diǎn)M到反射面R的距離為。因此，只要在數(shù)據(jù)處理時(shí)為每一路超聲波傳感器的測(cè)得距離補(bǔ)償一個(gè)長(zhǎng)度d，就可以將四路超聲波傳感器的位置等效到正方形的中心M。在本文中，放射源與四路超聲波傳感器的中心沿管道徑向的位置相同，且相對(duì)位置固定，下文將放射源和四路超聲波傳感器都等效為質(zhì)點(diǎn)M來(lái)處理。 四路超聲波傳感器的位置所謂放射源中心定位，就是通過(guò)一定的控制算法，讓放射源從不同的初始位置移動(dòng)到管道中心軸上。 放射源中心定位本文使用的放射源定位策略建立在如下命題成立的基礎(chǔ)上：命題P：在二維空間中，對(duì)于圓O內(nèi)部的一點(diǎn)M和圓周上的三個(gè)點(diǎn)A、B、C，如果滿足條件： （）那么，點(diǎn)M即為圓心O。 命題P的示意圖如果命題P成立，那么只要圓心內(nèi)部找到一個(gè)到圓周上任意3個(gè)點(diǎn)距離相等的點(diǎn)，然后再將放射源移動(dòng)到該點(diǎn)位置，就可以完成放射源中心定位的任務(wù)。，由條件 （）可以知道，、和都是等腰三角形。圖中，點(diǎn)D、E分別為線段AC、BC的中點(diǎn)，由等腰三角形的性質(zhì)（對(duì)于）可以知道，則OD為線段AC的垂直平分線，同理可以知道OE為線段BC的垂直平分線，即點(diǎn)O為AC、BC兩個(gè)線段的垂直平分線的交點(diǎn)。同樣，根據(jù)等腰三角形的性質(zhì)（對(duì)于和），可以知道線點(diǎn)M也為AC、BC兩條線段垂直平分線的交點(diǎn)。因此，可以得到點(diǎn)M與圓心O重疊，即命題P成立。、3路超聲波傳感器用作三向定位，（a）為其等效圖。使用超聲波傳感器測(cè)量距離，要求超聲波的傳播方向與被測(cè)面盡量垂直，如果偏離垂直位置過(guò)大，可能會(huì)出現(xiàn)接收器收不到回波的情況。（b）所示，當(dāng)質(zhì)點(diǎn)M沿方向1發(fā)射超聲波時(shí)，其回波信號(hào)的方向會(huì)發(fā)生嚴(yán)重偏離。而在圓心處，沿任意方向發(fā)生超聲波，都能沿原路返回。不難證明，質(zhì)點(diǎn)M離圓心越近，其回波信號(hào)越容易被接收。但是，對(duì)于目標(biāo)控制系統(tǒng)，要求質(zhì)點(diǎn)M在圓心處發(fā)射超聲波是沒(méi)有意義的，因?yàn)樽罱K控制目的就是要將質(zhì)點(diǎn)M定位到圓心。 超聲波定位示意圖（b）中，從質(zhì)點(diǎn)M發(fā)射的另一束超聲波3，其回波信號(hào)3ˊ可以完全被接收，原因是超聲波3傳播方向所在的直線經(jīng)過(guò)圓心。利用這一性質(zhì)，本文在使用三向定位策略之前先對(duì)放射源進(jìn)行初步定位，初步定位需要用到管道的半徑信息，本文使用第4路超聲波傳感器負(fù)責(zé)對(duì)管道半徑進(jìn)行粗略測(cè)量。，第4路超聲波傳感器與擺桿垂直，而它們的超聲波發(fā)射方向與擺桿方向一致，因此只要保證機(jī)器人擺桿經(jīng)過(guò)圓心，那么第4路傳感器的超聲波發(fā)射方向也必然經(jīng)過(guò)圓心。根據(jù)以上分析，最終本文分四步完成放射源的中心定位任務(wù)：第一步：擺桿位置校正目標(biāo)控制系統(tǒng)中有兩路傾角傳感器用于測(cè)量小車傾角α和擺桿傾角β。調(diào)整擺桿位置，使得擺桿的傾角為α，此時(shí)擺桿與機(jī)器人表面垂直，由于機(jī)器人的截面為矩形，且擺桿端點(diǎn)固定在矩形邊框的中點(diǎn)處，根據(jù)對(duì)稱性原理可以證明，此時(shí)擺桿必然經(jīng)過(guò)圓心。 擺桿位置校正第二步：管道半徑計(jì)算，超聲波傳感器2測(cè)得質(zhì)點(diǎn)M到圓周的距離（線段MC）為，超聲波傳感器4測(cè)得質(zhì)點(diǎn)M到機(jī)器人表面的距離（線段）為，機(jī)器人的寬度為w，高度為h，寬度和高度是特定機(jī)器人的固有屬性，為已知量，可以在程序初始化時(shí)由用戶手動(dòng)輸入。 管道半徑測(cè)量根據(jù)圓形的相交弦定理有： （）即 （）可以求得線段XD的長(zhǎng)度d為： （）那么，半徑r的長(zhǎng)度為： （）第三步：初步定位在第二步中，我們求得了管道半徑r，現(xiàn)在只要讓放射源沿著擺桿軸向進(jìn)行移動(dòng)，使得放射源到管道邊緣的距離等于管道半徑，就可以認(rèn)為放射源處于管道中央。，我們使用第2路超聲波傳感器實(shí)時(shí)測(cè)量線段MC的長(zhǎng)度，當(dāng)檢測(cè)到長(zhǎng)度為時(shí)，停止放射源移動(dòng)即可，其中為一個(gè)很小的容忍裕度，作用是防止在平衡點(diǎn)附近出現(xiàn)抖動(dòng)。第四步：三向定位如果以上操作都是在理想狀態(tài)下進(jìn)行的，那么初步定位就可以滿足放射源定位的精度要求，但是由于以下三方面原因，初步定位的精度并不高：（1）傾角傳感器的測(cè)量誤差。本文使用的傾角傳感器最大測(cè)量誤差為3176。，由于擺桿有杠桿放大作用。（2）超聲波傳感器的測(cè)量誤差。本文使用的超聲波傳感器最大測(cè)量誤差為4%，這個(gè)誤差出現(xiàn)的一個(gè)重要原因是發(fā)射器和接收器不在同一點(diǎn)，導(dǎo)致發(fā)射波與反射波成一定的角度。（3）機(jī)器人尺寸參數(shù)不精確。機(jī)器人的輪胎具有一定的彈性，再加上一定的磨損，輸入尺寸與實(shí)際尺寸存在一定的偏差。三向定位原理使用的是命題P的結(jié)論，跟傾角傳感器精度和機(jī)器人機(jī)械尺寸沒(méi)有任何關(guān)系，其全部參數(shù)都由超聲波傳感器來(lái)測(cè)量。（a）所示，由于超聲波傳感器存在入射角誤差，其測(cè)量的距離為ss2和s3，而實(shí)際距離為ddd3，但是由對(duì)稱性知識(shí)，可以知道有如下的關(guān)系成立： （）即由sss3相等，就可以得出ddd3三者相等，由于三向定位策略就是根據(jù)距離相等條件來(lái)定位，而不關(guān)心距離具體的數(shù)值，因此使用三向定位策略可以消除角度傳感器誤差、尺寸參數(shù)誤差和由入射角造成的傾角傳感器誤差三個(gè)方面的影響，因而定位精度比初步定位高很多。 三向定位原理（b）所示，同樣需要先對(duì)擺桿位置進(jìn)行校正，只不過(guò)這次是根據(jù)第3路超聲波傳感器的采集信號(hào)來(lái)校正，即通過(guò)保證d1與d3相等來(lái)確保擺桿通過(guò)圓心。然后再讓放射源沿?cái)[桿作軸向運(yùn)動(dòng)，使得放射源離管道邊界的距離d2與dd3相等。，圖中的為三向定位的容忍裕度。在該流程圖中，一次三向定位過(guò)程完成后，會(huì)再次判斷dd3的長(zhǎng)度是否滿足裕度條件，如果不滿足，會(huì)再執(zhí)行一次三向定位過(guò)程，其目的是，防止放射源移動(dòng)后，擺桿的杠桿作用將d1與d3的差值放大。 三向定位流程圖 基于MFC的人機(jī)接口層程序設(shè)計(jì)人機(jī)接口層處于控制系統(tǒng)的最高層，其主要任務(wù)是為使用者提供一個(gè)友好的交互界面，以實(shí)現(xiàn)對(duì)管道機(jī)器人的參數(shù)設(shè)定、運(yùn)動(dòng)控制和狀態(tài)監(jiān)控等功能。，我們知道人機(jī)接口層程序采用的是UDP通訊協(xié)議，結(jié)合本文設(shè)計(jì)的查詢重發(fā)指令機(jī)制，可以實(shí)現(xiàn)高效可靠的網(wǎng)絡(luò)通訊。本文使用MFC（Microsoft Foundation Classes）進(jìn)行人機(jī)接口層應(yīng)用程序設(shè)計(jì)。 HMI交互界面設(shè)計(jì) HMI界面是控制系統(tǒng)提供給操作者的重要交互接口，其設(shè)計(jì)工作是人機(jī)接口層程序設(shè)計(jì)的主要任務(wù)之一。為了實(shí)現(xiàn)對(duì)機(jī)器人的統(tǒng)一管理，HMI交互界面需要具備以下功能：（1）參數(shù)設(shè)定機(jī)器人的參數(shù)主要包括寬度、高度、傳動(dòng)比和速度四個(gè)。前三個(gè)參數(shù)是機(jī)器本體的機(jī)械屬性，由于本文對(duì)控制系統(tǒng)有通用性的要求，系統(tǒng)可以在一定程度上適應(yīng)機(jī)械結(jié)構(gòu)的改變，因此在HMI層預(yù)留了機(jī)械本體的參數(shù)設(shè)置接口。寬度和高度參數(shù)用于初步定位時(shí)管道半徑的計(jì)算。本文定義傳動(dòng)比為機(jī)器人行走速度與步進(jìn)電機(jī)轉(zhuǎn)速的比例，單位為米/圈。功能層根據(jù)傳動(dòng)比和機(jī)器人行走速度，確定步進(jìn)電機(jī)的轉(zhuǎn)速。（2）運(yùn)動(dòng)控制HMI界面作為控制系統(tǒng)的主要對(duì)外接口，必須具備運(yùn)動(dòng)控制功能。借助HMI交互界面，操作者除了可以控制機(jī)器人的前、后、左、右等基本運(yùn)動(dòng)，還可以下達(dá)位姿校正、放射源中心定位等復(fù)合性運(yùn)動(dòng)的控制指令。另外，機(jī)器人的初始化、啟動(dòng)、停止和照明開(kāi)關(guān)等操作都可以通過(guò)HMI界面來(lái)實(shí)現(xiàn)。（3）狀態(tài)監(jiān)控操作者需要根據(jù)機(jī)器人當(dāng)前的運(yùn)動(dòng)狀態(tài)決定下達(dá)怎樣的指令，這就要求HMI交互界面能夠?qū)崟r(shí)反饋機(jī)器人的狀態(tài)參數(shù)。機(jī)器人狀態(tài)主要包括左右輪的速度、爬坡角、傾斜角等。此外，控制系統(tǒng)要求HMI交互界面能夠針對(duì)電量不足、通訊故障等緊急狀況發(fā)出警報(bào)。根據(jù)以上需求。圖中共有13個(gè)BUTTON控件，這13個(gè)BUTTON控件結(jié)合對(duì)應(yīng)的EDIT控件可以完成參數(shù)設(shè)定和運(yùn)動(dòng)控制的任務(wù)。圖中的“系統(tǒng)狀態(tài)”組合框中包含多個(gè)STATIC控件，其作用是實(shí)時(shí)顯示機(jī)器人的各個(gè)狀態(tài)參數(shù)。圖中的“緊急事件日志”LISTBOX用于記錄系統(tǒng)緊急事件的時(shí)間和內(nèi)容。另外，在該HMI界面中還包含“發(fā)送指令”和“反饋指令”兩個(gè)LISTBOX控件，這兩個(gè)控件可以記錄HMI應(yīng)用程序發(fā)送和接收的指令，主要用于系統(tǒng)調(diào)試。 HMI交互界面HMI主機(jī)在對(duì)機(jī)器人進(jìn)行管理前，首先要與機(jī)器人建立連接。HMI應(yīng)用程序中連接的概念不同于TCP協(xié)議的連接，在這里，連接由握手指令建立，由循環(huán)發(fā)送的查詢指令維持。HMI主機(jī)與目標(biāo)機(jī)器人建立網(wǎng)絡(luò)連接時(shí)，兩者可以處于同一局域網(wǎng)中，也可以分布在不同的局域網(wǎng)中。對(duì)于HMI主機(jī)與目標(biāo)機(jī)器人分布在不同局域網(wǎng)的情況，操作者必須知道目標(biāo)機(jī)器人的IP地址，并將該IP地址從HMI界面的IP地址控件中輸入給HMI程序，HMI程序根據(jù)輸入的IP地址，向目標(biāo)機(jī)器人發(fā)送握手指令，從而建立連接。對(duì)于HMI主機(jī)與目標(biāo)機(jī)器人處于同一局域網(wǎng)的情況，HMI程序提供了目標(biāo)搜索功能，此時(shí)目標(biāo)機(jī)器人的IP地址可以由HMI程序自動(dòng)獲取，而無(wú)需操作者手動(dòng)輸入。搜索功能利用的是廣播包的二層擴(kuò)散原理，HMI首先向整個(gè)局域網(wǎng)發(fā)送握手指令，而網(wǎng)絡(luò)中只有目標(biāo)機(jī)器人能夠識(shí)別握手指令并給以反饋，HMI程序提取UDP反饋報(bào)文中的IP字段作為目標(biāo)機(jī)器人的IP地址。建立連接后，操作者就可以通過(guò)HMI交互界面控制目標(biāo)機(jī)器人執(zhí)行具體的動(dòng)作。HMI交互界面使用Windows消息機(jī)制檢測(cè)用戶按鍵事件并生成對(duì)應(yīng)的消息，再根據(jù)消息映射調(diào)用相應(yīng)的消息響應(yīng)函數(shù)，在消息響應(yīng)函數(shù)中，應(yīng)用程序借助Windows套接字將指令發(fā)送給目標(biāo)機(jī)器人。同樣，HMI主機(jī)會(huì)實(shí)時(shí)接收到目標(biāo)機(jī)器人的狀態(tài)反饋指令，收到狀態(tài)反饋指令后，HMI應(yīng)用程序根據(jù)通訊協(xié)議提取指令中的有效字段并通過(guò)HMI交互界面展示給操作者。 基于WINSOCKET的UDP通訊