雙擺雜技機器人DD2UD的多模態(tài)混合控制

但遠宏

（重慶理工大學 人工智能系統(tǒng)研究所，重慶 400054）

雙擺雜技機器人的原型是旋轉(zhuǎn)二級倒立擺，是典型的非線性、多變量、強耦合欠驅(qū)動控制系統(tǒng)，能直觀地反映出各項控制特征（快速性、穩(wěn)定性等），是驗證控制理論與算法的經(jīng)典平臺。相關(guān)控制研究可分為倒立點的穩(wěn)定控制，從一個平衡點轉(zhuǎn)換到另一個平衡點的大范圍非線性控制。對于穩(wěn)定控制，目前已經(jīng)實現(xiàn)了從一級到四級倒立擺的仿真與實時控制，相關(guān)控制方法包括滑模控制[1]、云控制[2]、變論域模糊控制[3]等。而平衡點間的大范圍非線性轉(zhuǎn)換控制，則主要集中在從下垂平衡點擺起到倒立平衡點的擺起倒立控制，由于難度很大，目前僅實現(xiàn)了一級倒立擺和二級倒立擺的擺起倒立實時控制，相關(guān)控制方法包括逆控制[4]、基于能量的耗散系統(tǒng)控制[5]、仿人智能控制[6]等。對于所有平衡態(tài)之間任意切換控制，目前僅文獻[7]進行了較為全面系統(tǒng)的論述，并以Down-Up到Up-Down的轉(zhuǎn)換控制為例介紹了控制器的設(shè)計過程。本文則主要討論雙擺雜技機器人從Down-Down平衡點轉(zhuǎn)換到Up-Down平衡點的大范圍非線性控制（DD2UD）。

在現(xiàn)代高層房建工程施工過程中，鉆孔灌注樁技術(shù)應(yīng)用比較廣泛，屬于最為基礎(chǔ)的施工技術(shù)，可以有效控制房建工程的質(zhì)量。為了進一步提升房建工程的質(zhì)量水平，施工企業(yè)應(yīng)深入研究鉆孔灌注樁技術(shù)，并采取有效的質(zhì)量控制措施，確保基礎(chǔ)工程的質(zhì)量，這樣才可以真正提高整個房建工程的質(zhì)量。

1 數(shù)學模型

雙擺雜技機器人設(shè)備如圖1所示，包含機器人本體、內(nèi)桿、外桿。每個擺桿有Down和Up 2個平衡點，2個擺桿可構(gòu)成4個平衡點，如圖2所示。除了Down-Down為自然穩(wěn)定平衡點外，其余3個都是臨界穩(wěn)定平衡點。

“曹孟德占天時兵多將廣，領(lǐng)人馬下江南兵扎在長江。孫仲謀無決策難以抵擋，東吳的臣武將要戰(zhàn)文官要降。”樓下的老陳喜歡聽京戲，《打漁殺家》《貴妃醉酒》，經(jīng)常是反復聽，今天他聽的是馬連良的《借東風》。老陳坐在一張?zhí)梢紊希扑撬Ｔ鹤永锏哪强檬磷訕洌叩脦缀醭^了五樓。

圖1 雙擺雜技機器人Fig.1 Device of double pendulum robot

圖2 雙擺雜技機器人的4個平衡點Fig.2 Four equilibrium point of double pendulum robot

定義廣義坐標 Θ=［θ0，θ1，θ2］， 應(yīng)用拉格朗日建模方法，并以角加速度作為控制輸入，可得雙擺雜技機器人的動力學模型為

式中：M1＝（m1l1+m2L1+mbL1）L0cosθ1；M2=J1+m1l12+m2L12+mbL12；M3=m2l2L0cos（θ1-θ2）；M4=（m2l2L1sinθ2-M2sinθ1）θ˙0cosθ1；M5=m2l2L1θ˙2sin（θ1-θ2）-c2；M6=（m2l2L1sinθ1-（J2+m2l22）sinθ2）θ˙0cosθ2；M7=-m2l2L1θ˙1sin（θ1-θ2）-c2；M8=（m1l1+m2L1+mbL1）gsinθ1。

式中，U0為一個 bang-bang常量，式（2）的激活條件如下所示：

表1 雙擺雜技機器人的物理參數(shù)說明Tab.1 Physical parameters of double pendulum robot

2 控制任務(wù)分析與階段劃分

式中：k1，k2，k3，k4分別為水平桿和內(nèi)桿的比例和微分（PD）系數(shù)，用于將水平桿穩(wěn)定在零點，將內(nèi)桿穩(wěn)定在 Up 位置；eθ0和e˙θ0分別為水平桿的角度誤差和角速度誤差；eθU1和 e˙θU1分別為內(nèi)桿以 Up 位置為零點時的角度誤差和角速度誤差；U4為bang-bang控制常量，其作用方向與外桿旋轉(zhuǎn)方向一致，以實現(xiàn)吸收外桿多余能量的目的。式（8）的激活條件如下所示：

式中：U1為bang-bang控制常量；θD1為內(nèi)桿以垂直向下位置（Down）為零點時的角度；θc1為一個給定角度常量，用來表示本階段擺桿所需達到的角度，當內(nèi)桿角度大于θc1時將進入下一控制階段。

根據(jù)上述分析，可將DD2UD控制過程分為5個階段：初始擾動階段、雙桿起擺階段、推拉擺起階段、外桿調(diào)整階段、Up-Down穩(wěn)定控制階段，如圖3所示。下面分別闡述每個控制階段的詳細設(shè)計過程。

圖3 DD2UD控制流程與控制階段劃分Fig.3 Control phases for DD2UD

3 控制器設(shè)計

3.1 初始擾動

在特征分類方面,本文選用了高斯混合模型(GMM)對訓練樣本進行訓練,并劃分樣本空間,完成分類器的設(shè)計。其實現(xiàn)過程如圖3所示。

各個變量的物理含義如表1所示。

式中：θc0為指定角度常量，用于確定式（2）的作用持續(xù)時間；step為用于指示當前所處控制階段的狀態(tài)標志變量。

3.2 雙桿起擺控制

3 年來，我們先后推出了一系列高質(zhì)量專題班，深受集團領(lǐng)導和廣大學員的好評。在十九大召開的第一周，即在線建設(shè)“十九大精神解讀專題班”，短時間內(nèi)吸引33 家單位3 萬人報名學習，學習完成率超過80%；“黨務(wù)干部網(wǎng)絡(luò)專題培訓班”獲得20家單位5000 多人報名學習；“創(chuàng)新驅(qū)動發(fā)展戰(zhàn)略網(wǎng)絡(luò)專題班”獲得11 家單位1000 人報名學習；“深化三項制度改革網(wǎng)絡(luò)專題班”獲得30 家單位1000人報名學習；為配合學院案例開發(fā)工作，快速設(shè)計專題項目“案例匯編”，實現(xiàn)全院教職工的案例開發(fā)學習。

經(jīng)過簡單的動力學分析不難發(fā)現(xiàn)，內(nèi)桿位于水平線下方時，當水平桿逆時針加速旋轉(zhuǎn)，內(nèi)桿將產(chǎn)生順時針旋轉(zhuǎn)加速度；當水平桿順時針加速旋轉(zhuǎn)，內(nèi)桿將產(chǎn)生逆時針旋轉(zhuǎn)加速度。因此只需要根據(jù)當前內(nèi)桿的旋轉(zhuǎn)方向施加對應(yīng)方向的控制量，使得新產(chǎn)生的旋轉(zhuǎn)趨勢與當前運動方向一致，便可實現(xiàn)高效的內(nèi)桿能量泵入。本階段的控制律可設(shè)計如下，式（4）為本階段的運動圖式（子控制器），式（5）為本階段的激活條件。

由于外桿通過鉸鏈連接在內(nèi)桿末端，當內(nèi)桿從水平桿下端擺起到水平桿上端時，外桿也必然從水平桿下端擺起到水平桿上端；且外桿目標位置為自然穩(wěn)定的垂直向下位置（Down），即便不特意控制，也會在重力和轉(zhuǎn)動摩擦的作用下逐漸趨于穩(wěn)定。內(nèi)桿則正好相反，其目標位置為臨界穩(wěn)定的垂直向上位置（Up），對姿態(tài)與能量有較高精度要求。因此本階段的控制主要考慮內(nèi)桿。

本階段的目標是打破擺桿在Down-Down平衡點的穩(wěn)定狀態(tài)，因此本階段僅需對水平桿施加一個較小的控制常量即可，控制律可表示為

雙擺雜技機器人剛開始處于Down-Down的自然穩(wěn)定狀態(tài)，需要施加一個控制作用來打破這種穩(wěn)定狀態(tài)，以便進行內(nèi)桿與外桿的擺起控制。由于實際控制時電機的力矩受限，并且控制量對內(nèi)桿與外桿的耦合傳導作用是依次遞減的，一次性將內(nèi)桿與外桿擺起到Up-Down位置非常困難，需要多次來回震蕩來實現(xiàn)內(nèi)桿與外桿的能量累積。而當擺桿接近水平線附近時，由簡單的動力學分析可知，施加在旋轉(zhuǎn)臂上的力矩對擺桿運動的影響顯著減小，因此當擺桿角度大到一定幅值時，應(yīng)該以較大的控制作用一次性向擺桿注入足夠能量，使得擺桿以慣性從水平線下方運動到水平線上方，以避開水平線附近的低可控區(qū)域。由于Up-Down屬于臨界穩(wěn)定點，對進入Up-Down穩(wěn)定控制時的擺桿能量和姿態(tài)有較高要求，為增大控制器的穩(wěn)定性，在進行穩(wěn)定控制前先對擺桿姿態(tài)進行調(diào)整，使得擺桿進入穩(wěn)定區(qū)間時的狀態(tài)更好。

3.3 推拉擺起控制

經(jīng)過上一階段的控制，擺桿在水平線下方已獲得了部分能量，為避免擺桿在水平線位置的低可控區(qū)域，此時需要以較大的控制量一次性向擺桿注入足夠能量，從而使擺桿能夠依靠慣性從水平線下方旋轉(zhuǎn)到水平線上方。因此本階段的控制律可設(shè)計為

1）主控芯片串口通過發(fā)送AT+CSCON?查詢是否連接到網(wǎng)絡(luò)，當返回1時為CONNECT狀態(tài)，當返回0時為IDLE狀態(tài)。

式中：U2，U3為bang-bang控制常量，它們的數(shù)值較大且符號相反，分別作用于水平線下方和水平線上方，使得擺桿在短時間內(nèi)快速擺起；θD2為外桿以垂直向下（Down）為零點時的角度；θc2，θc3為給定的角度特征閾值，分別用于激活水平線下方和水平線上方bang-bang控制。本階段的激活條件為

1.2.1 試驗設(shè)計 設(shè)置4個光照強度，分別為：一層遮蔭(P1，47.3%透光率)，二層遮蔭(P2，15.1%透光率)，三層遮蔭(P3，7.3%透光率)，全光照(CK，100%透光率)。每個處理4株，3次重復，共48株。試驗于2016年5月1日開始，共進行180 d，在試驗期間每個處理進行相同的水肥管護。

式中：θU1為內(nèi)桿以垂直向上（Up）為零點時的角度；θc4為給定的角度特征閾值，當內(nèi)桿角度小于θc4進入姿態(tài)調(diào)整階段。

3.4 外桿調(diào)整階段

經(jīng)過前面的控制內(nèi)桿與外桿已從水平線下方擺起到水平線上方，前階段在設(shè)計控制器時，主要以內(nèi)桿為控制目標，導致外桿的在水平線上方的角度和角速度與目標值存在較大偏差，因此在進行Up-Down穩(wěn)定控制前，有必要先對外桿狀態(tài)進行調(diào)整。本階段的控制目標有2個：（1）吸收掉外桿多余的能量使其接近Down目標平衡點；（2）將內(nèi)桿穩(wěn)定在Up平衡點附近。為達成這2個控制目標，本階段控制律設(shè)計如下：

雙擺雜技機器人的初始狀態(tài)為兩擺桿垂直向下的自然穩(wěn)定狀態(tài)，目標狀態(tài)為內(nèi)擺桿垂直向上外擺桿垂直向下的臨界穩(wěn)定狀態(tài)。控制器的任務(wù)即是通過控制水平旋轉(zhuǎn)桿的來回轉(zhuǎn)動，使得內(nèi)桿與外擺桿從垂直向下的位置擺起到水平旋轉(zhuǎn)桿上方位置，且在內(nèi)桿垂直向上（Up）外桿垂直向下（Down）位置穩(wěn)定。在控制過程中有3個目標需要同時兼顧：水平旋轉(zhuǎn)桿穩(wěn)定在零點位置；內(nèi)桿從Down位置擺起到Up位置；外桿從水平桿下方的Down位置擺起到水平桿上方的Down位置。

3.5 Up-Down穩(wěn)定控制

當兩擺桿經(jīng)歷前4個階段的控制，到達Up-Down位置附近時，將進入穩(wěn)擺階段。本階段的動力學控制目標是Up-Down這個新的自不穩(wěn)定的動平衡狀態(tài)，它的平衡由如下施加在雙擺機器人本體上的控制律實現(xiàn)：

我刊將以此次評選為契機，繼續(xù)沿著創(chuàng)建世界一流期刊的道路，不斷改進工作，提升質(zhì)量，開拓創(chuàng)新，為廣大讀者、作者提供更加優(yōu)質(zhì)的服務(wù)。

式中：k5，k6，k7，k8，k9，k10分別為水平桿、內(nèi)桿、外桿的比例與微分系數(shù)，實現(xiàn)Up-Down的穩(wěn)定控制；eθD2與e˙θD2分別是外桿以Down位置為零點時的角度誤差和角速度誤差。式（11）為式（10）的激活條件。

綜合式（2）～式（11），可得 DD2UD 的總體動覺智能圖式（仿人智能控制器）如下所示：

4 實時控制實驗

將表2所示的控制器參數(shù)和特征閾值參數(shù)代入式（12）所示的控制器中，并在如圖4所示的雙擺雜技機器人裝置上進行實時控制實驗。實驗環(huán)境：編譯環(huán)境Visual C++6.0；操作系統(tǒng)Windows XP；運動控制卡（深圳固高科技有限公司GT400）；編碼器（擺桿角度編碼器600線，電機編碼器2500線）；伺服電機與伺服驅(qū)動器（松下200 W交流同步伺服電機）。

表2 DD2UD的控制參數(shù)與特征閾值Tab.2 Control parameters and characteristic threshold

圖4 雙擺雜技機器人實驗平臺Fig.4 Experiment platform of double pendulum robot

圖5～圖7所示分別為水平桿、內(nèi)桿、外桿的角度和角速度響應(yīng)曲線，圖8所示為DD2UD的控制輸出曲線，圖9所示為DD2UD實時控制視頻的時序截圖。

圖5 水平旋轉(zhuǎn)桿響應(yīng)曲線Fig.5 Horizontal rotating rod response curve

圖6 內(nèi)桿響應(yīng)曲線Fig.6 Inner rod response curve

圖7 外桿響應(yīng)曲線Fig.7 External rod response curve

圖8 DD2UD實時控制量Fig.8 DD2UD real-time quantity

圖9 DD2UD實時控制視頻截圖Fig.9 DD2UD real-time control video screenshot

5 結(jié)語

本文首先介紹了雙擺雜技機器人的物理結(jié)構(gòu)與動力學模型，針對雙擺雜技機器人的DD2UD雜技動作，模仿人的控制行為并根據(jù)動力學分析，將DD2UD控制過程進行了子任務(wù)劃分。按照每個子任務(wù)的控制目標設(shè)計出了各個階段的控制律，并構(gòu)建出具有多模態(tài)結(jié)構(gòu)的混合控制器。最后將該控制器應(yīng)用于實際雙擺雜技機器人設(shè)備，成功進行了實時控制實驗，驗證了本文控制方法的有效性。

[1]王鵬虎，李小兵，付國慶，等.三級倒立擺系統(tǒng)全程滑模變結(jié)構(gòu)控制[J].機械設(shè)計與制造，2009（7）：127-129.

[2]李德毅.三級倒立擺的云控制及動平衡模式[J].中國工程科學，1999，1（2）：41-46.

[3]李洪興，苗志宏，王加銀.四級倒立擺的變論域自適應(yīng)模糊控制[J].中國科學：E 輯，2002，32（1）：65-75.

[4]Devasia S，Chen D，Paden B.Nonlinear inversio-n-based output tracking[J].IEEE Transactions on Automatic Control，1996，41（7）：930-942.

[5]Wei Z，Helmut R.Energy and passivity based control of the double inverted pendulum on a cart[C]//Proc of IEEE InternationalConferenceon ControlApplication，Mecico，2001：896-901.

[6]Li Z S，Wang Y X，Tan Z，et al.The swing-up and handstand control and its implementation of cat double pendulum[C]//Proc of the 5th world congress on inelligent control and automation，Hangzhou，2004：2360-2364.

[7]李祖樞，但遠宏，張小川，等.雙擺機器人擺桿平衡態(tài)任意轉(zhuǎn)換運動控制的實現(xiàn)[J].自動化學報，2010，36（12）：86-97.