一種基于力矩控制的管道自動清洗機器人的控制方法
【技術領域】
[0001]本發明涉及管道清洗運動控制技術及其控制系統設計，具體涉及一種基于力矩控制的管道自動清洗機器人的控制方法。
【背景技術】
[0002]煙道是賓館、酒樓、飯店廚房的基礎設施，極易藏污納垢，是清理的死角。同時煙道是消防、衛生、防疫部門檢查的重點區域。同時，煙罩內壁易寄生老鼠、蟑螂，不符合國家衛生標準。油膩膠狀物長期扒附在金屬表面，還會腐蝕金屬材料，縮短煙道使用壽命。油煙管道有很多油漬長時間殘留在煙機上和煙道表面上形成油垢，這樣就會影響油煙機、煙道的使用，使煙流不易排除。廚房排煙不暢有80%左右的原因，都是由于長時間沒有對油煙凈化器的內置過濾網進行清潔，油污堵塞過濾網而造成的。煙道、煙罩常年處于高溫工作環境，油煙氣化形成積油、積炭或油膩的膠狀物，如果不及時對煙道及煙罩進行清洗，非常容易引起火災。
[0003]現在，繼中央空調風道清洗機器人的成功運用，又出現了管道自動清洗機器人成功解決了油煙管道自動清洗這一難題，使用控制系統操控的管道自動清洗機器人通過自身攜帶的攝像頭，清楚掌握管道內的污染情況，可實現對管道油煙的自動清洗，但是現有的管道自動清洗機器人在清洗管道的過程中，一方面，因噴桿和噴嘴與管道壁的干涉會導致控制電機的堵轉從而燒毀電機，從而導致管道自動清洗機器人中途停止清洗工作；另一方面，現有的機器人清洗噴嘴與管道壁貼合不緊，導致管道自動清洗效果不佳。
【發明內容】
[0004]本發明的目的旨在解決現有的管道自動清洗機器人清洗管道的過程中容易燒壞電機停止工作以及對管道的清洗效果不佳的技術問題，提供一種基于力矩控制的管道自動清洗機器人的控制方法。
[0005]本發明提供一種基于力矩控制的管道自動清洗機器人的控制方法，所述機器人包括噴桿及與所述噴桿相連接的噴嘴及安裝在機器人上的監控攝像頭，所述控制方法包括以下步驟:
讀取機器人參數配置文件，并開啟監控攝像頭；
獲取管道自動清洗信號為圓形管道或近似圓形管道，并根據所述管道自動清洗信號設定所述機器人的清洗參數；
輸出對用于控制噴桿繞水平旋轉軸水平旋轉的第一伺服電機和用于控制噴桿繞豎直旋轉軸豎直旋轉的第二伺服電機的第一控制信號以帶動所述機器人的噴嘴移動到管道自動清洗周期的運動原點；
輸出對第一伺服電機和第二伺服電機的第二控制信號以控制所述噴桿的豎直旋轉軸和所述噴桿的水平旋轉軸的運行；
判斷接收到噴桿水平旋轉的返回速度是否為零且持續一定時間T ;如果是，輸出對第一伺服電機和第二伺服電機的第三控制信號以控制所述噴桿的豎直旋轉軸和所述噴桿的水平旋轉軸的運行；
判斷接收到噴桿豎直旋轉的返回速度是否為零且持續一定時間T ;
如果是，輸出對第一伺服電機和第二伺服電機的第四控制信號以控制所述噴桿的豎直旋轉軸和所述噴桿的水平旋轉軸的運行；
判斷接收到噴桿水平旋轉的返回速度是否為零且持續一定時間T ;
如果是，輸出對第一伺服電機和第二伺服電機的第五控制信號以控制所述噴桿的豎直旋轉軸和所述噴桿的水平旋轉軸的運行；
判斷接收到噴桿豎直旋轉的返回速度是否為零且持續一定時間T;
如果是，所述機器人沿當前管道移動預設的行進距離；
判斷機器人已經移動的距離是否大于等于設定的總距離，如果是，終止清洗工作，如果否，則進入下個周期的清洗步驟。
[0006]進一步地，所述獲取管道自動清洗信號為圓形管道或近似圓形管道，并根據所述管道自動清洗信號設定所述機器人的清洗參數的步驟，具體為：
設定清洗管道總長度L、每個清洗周期內的行進距離A、清洗阻力F，清洗動力S，以及主動軸的運行速度V ;
根據監控攝像頭采集的管道視頻信號設定清洗模式為輕油污清洗模式或重油污清洗模式；
清油污清洗模式下，控制噴嘴與噴桿的夾角為0 ;
重油污清洗模式下，控制噴嘴與噴桿的夾角為設定角度a，使噴桿與噴嘴所在的平面始終與所清洗的管道的垂直。
[0007]進一步地，所述輸出對用于控制噴桿水平旋轉的第一伺服電機和用于控制噴桿豎直旋轉的第二伺服電機的第一控制信號以帶動所述機器人的噴嘴移動到管道自動清洗周期的運動原點的步驟，具體為：
設定所述噴桿豎直旋轉軸和噴桿水平旋轉軸的力矩大小為空走力矩，所述噴桿豎直旋轉軸和噴桿水平旋轉軸的最大速度設定為空走速度；
開啟對第一伺服電機和第二伺服電機的使能信號以帶動所述機器人的噴嘴移動到管道自動清洗周期的運動原點；
其中，所述運動原點位于圓形管道或近似圓形管道的內壁面最高點。
[0008]進一步地，所述輸出對第一伺服電機和第二伺服電機的第二控制信號以控制所述噴桿的豎直旋轉軸和所述噴桿的水平旋轉軸的運行的步驟，具體為：
設定所述豎直旋轉軸力矩為清洗動力S的力矩與噴桿重力G的力矩之差，設定所述水平旋轉軸的力矩為清洗阻力F的力矩，設定豎直旋轉軸為主動軸，水平旋轉軸的速度為豎直旋轉軸的四倍；
開啟對第一伺服電機和第二伺服電機的使能信號以帶動所述機器人的噴嘴開始清洗管道的左上壁面。
[0009]進一步地，所述輸出對第一伺服電機和第二伺服電機的第三控制信號以控制所述噴桿的豎直旋轉軸和所述噴桿的水平旋轉軸運行的步驟，具體為：
設定所述豎直旋轉軸力矩為清洗阻力F的力矩與噴桿重力G的力矩之差，設定所述水平旋轉軸的力矩為清洗動力S的力矩，設定水平旋轉軸的為主動軸，豎直旋轉軸的速度為水平旋轉軸的四倍；
開啟對第一伺服電機和第二伺服電機的使能信號以帶動所述機器人的噴嘴開始清洗管道的左下壁面。
[0010]進一步地，所述輸出對第一伺服電機和第二伺服電機的第四控制信號以控制所述噴桿的豎直旋轉軸和所述噴桿的水平旋轉軸運行的步驟，具體為:
設定所述豎直旋轉軸力矩為清洗動力S的力矩與噴桿重力G的力矩之和，設定所述水平旋轉軸的力矩為清洗阻力F的力矩，設定豎直旋轉軸為主動軸，水平旋轉軸的速度為豎直旋轉軸的四倍；
開啟對第一伺服電機和第二伺服電機的使能信號以帶動所述機器人的噴嘴開始清洗管道的右下壁面。
[0011]進一步地，所述輸出對第一伺服電機和第二伺服電機的第五控制信號以控制所述噴桿的豎直旋轉軸和所述噴桿的水平旋轉軸運行的步驟，具體為:
設定所述豎直旋轉軸力矩為清洗阻力F的力矩與噴桿重力G的力矩之和，設定所述水平旋轉軸的力矩為清洗動力S的力矩，設定水平旋轉軸為主動軸，豎直旋轉軸的速度為水平旋轉軸的四倍；
開啟對第一伺服電機和第二伺服電機的使能信號以帶動所述機器人的噴嘴開始清洗管道的右上壁面。
[0012]從上述系統的方案可以看出，通過采用第一伺服電機控制噴桿繞水平旋轉軸轉動，并采用第二伺服電機控制噴桿繞豎直旋轉軸轉動，兩個伺服電機分別都工作在轉矩模式下，可以有效保證噴桿遇到管道壁面的干涉時，伺服電機不易被燒毀，從而有效降低了管道自動清洗機器人在管道中作業時驅動電機的故障率；同時，采用伺服電機的力矩模式，可以有效保證噴嘴始終緊貼所清洗的管道壁，有效提高了對管道油污的清洗效果。
【附圖說明】
[0013]圖1為本發明管道自動清洗機器人的一種實施例的結構示意圖；
圖2為本發明圓形管道一種實施例的清洗區域劃分示意圖；
圖3為本發明管道自動清洗機器人控制方法的一種實施例的流程圖。
【具體實施方式】
[0014]為了使本發明所解決的技術問題、技術方案及有益效果更加清楚明白，以下結合附圖及實施例，對本發明進行進一步詳細說明。應當理解，此處所描述的具體實施例僅僅用以解釋本發明，并不用于限定本發明。
[0015]本發明提供一種實施例的基于力矩控制的管道自動清洗機器人的控制方法，所述管道自動清洗機器人，如圖1所示，包括噴桿I及與噴桿相連接的噴嘴2，其上還設有水平旋轉軸3及豎直旋轉軸4，所述噴桿I可繞水平旋轉軸3在水平面上轉動，從而帶動噴嘴2沿著水平面轉動，所述噴桿I可繞豎直旋轉軸4在豎直面上轉動，從而帶動噴嘴2沿著水平面轉動。利用其內設置的控制裝置及驅動裝置(圖中未示出)控制驅動所述水平旋轉軸及豎直旋轉軸的轉動，從而帶動噴桿和噴嘴完成對管道的清洗工作。本實施例中，優選采用第一伺服電機驅動所述水平旋轉軸3轉動，第二伺服電機驅動所述豎直旋轉軸4轉動。
[0016]對于交流伺服電機，其可以分別運行在三種不同模式下:位置模式、速度模式以及轉矩模式。本實施例中，我們使用它的轉矩模式，在轉矩模式下，電機的轉動轉矩為可設定量，其設定范圍為0-3倍額定轉矩。理論上，若阻力矩一直小于設定力矩，電機將會一直保持加速狀態，最終導致轉速過快，實際中，受電機功率限制，轉速不會達到無限快，但轉速也