張佳樂，趙睿英?，馮艷麗，楊皓，武琳琳

1) 長安大學工程機械學院公路養(yǎng)護裝備國家工程研究中心，西安 710064 2) 西北工業(yè)大學航天學院，西安 710068

自行車是一種使用歷史悠久的代步交通工具，具有結構簡單、環(huán)保、經(jīng)濟、用途廣泛等優(yōu)點，其歷史可大約追溯到第二次工業(yè)革命時期[1].進入21 世紀，隨著計算和傳感技術的飛速發(fā)展，使得自動駕駛成為一個熱門而重要的研究課題.自行車的廣闊市場為無人駕駛自行車創(chuàng)造了巨大的機會[2].無人駕駛自行車機器人具有運動靈活、車身狹小等特點，從而能夠廣泛應用到災區(qū)救援、娛樂表演、物流運輸?shù)葓鼍爸衃3]，因此得到了眾多學者對該類型自行車的研究與關注.

近年來，圍繞著自行車機器人的穩(wěn)定平衡控制策略問題，國內(nèi)外的學者開展了深入的研究.其中，Tanaka 和Murakami[4]通過控制轉(zhuǎn)向角或利用離心力來控制平衡.然而，該方法無法實現(xiàn)自行車靜止狀態(tài)的平衡控制.Lee 和Ham[5]則是通過改變質(zhì)心的位置來控制機器人的平衡，但該方法增加了自行車的重量，導致系統(tǒng)的響應時間較慢.Schwab 和Meijaard[6]，以及Vu 和Nguyen[7]則指出通過轉(zhuǎn)向車把來平衡低速（靜止）狀態(tài)的自行車困難較大，在這種情況下，可采用角動量輪進行側(cè)向平衡控制，相比之下，該方法可以實現(xiàn)在靜止狀態(tài)下的車體平衡控制.目前，已有一些使用角動量輪進行機器人自平衡的線性控制方法被提出.其中，Lam[8]和Lee等[9]對自行車的橫滾角度進行實時測量，采用PD 控制算法與PID 控制算法，實現(xiàn)了兩自由度自行車機器人的平衡控制.Chen等[10]將自行車動態(tài)模型進行線性化和離散化，設計了模型預測控制器來實現(xiàn)自行車的平衡控制，使用了兩個旋轉(zhuǎn)方向相反的飛輪來抵消自行車偏航動力學中的反作用力矩，對于每個飛輪，需要兩個執(zhí)行器來調(diào)節(jié)圍繞兩個正交軸的角速度，但會使平衡設備的復雜性增加.Sprya 和Girard[11]對動力學模型進行了線性化處理，并采用極點置零法設計了平衡控制器.上述方法大多需要對模型進行線性化處理，但自行車機器人是一個復雜的非線性系統(tǒng)，模型簡化會降低控制系統(tǒng)精度.針對這些問題，Cui等[12]提出了一種基于互聯(lián)和阻尼分配的無源控制方法（IDA-PBC）的非線性控制器，為簡化輔助平衡設備僅使用一個飛輪，將與飛輪旋轉(zhuǎn)方向相反的扭矩用于自行車的平衡控制.Zhang等[13]設計了一種非線性控制器，通過控制陀螺儀進行自行車的平衡控制.此外，也有許多其他兩輪自行車的非線性平衡控制方法被提出，Beznos等[14]的非線性控制以及Chen 和Dao[15]的模糊控制方法等，這些控制方法考慮了自行車機器人系統(tǒng)的非線性因素，利用反饋信息設計機器人平衡控制器，有效解決了模型簡化所產(chǎn)生的控制系統(tǒng)精度受限的問題，成為了自行車機器人控制領域的研究熱點.

與上述傳統(tǒng)的非線性控制方法不同，張新榮等[16]，Udwadia 和Kalaba[17]以及Chen[18]基于Udwadia?Kalaba（U?K）理論從一個新的角度出發(fā)，將控制目標視為受控對象的伺服約束條件，并通過產(chǎn)生主動伺服約束力使受控系統(tǒng)實現(xiàn)控制目標，即利用主動控制的思想來解決機械系統(tǒng)的控制問題.該方法是專門針對機械系統(tǒng)的一種新非線性控制方法，并被應用至不同領域中，其中：趙韓等[19]在機械臂位置控制方面應用了該方法，能夠較好地解決伺服約束控制方面問題.Zhao等[20]考慮了系統(tǒng)的關節(jié)摩擦以及不確定性的影響，將該方法應用到非線性Delta 并聯(lián)機器人的魯棒控制中，并驗證了系統(tǒng)穩(wěn)定性與有效性.Chen等[21]針對欠驅(qū)動移動機器人的平衡控制問題，提出了一種基于U?K 理論的自適應律的控制方法，該自適應律可根據(jù)系統(tǒng)存在的不確定性不斷進行跟蹤誤差的調(diào)整.韓江等[22]將一種基于U?K 理論的軌跡跟蹤控制方法應用到六軸協(xié)作機器人上，使系統(tǒng)具有較好的穩(wěn)定性.Yin等[23]在解決車輛橫向和偏航運動控制問題中使用了該方法，通過數(shù)值仿真結果驗證了該方法的有效性.此外，董方方和趙曉敏[24]將該方法應用到柔性機器人控制中，通過理論分析與仿真結果，驗證了該控制方法可以完成系統(tǒng)的軌跡跟蹤控制任務.然而該方法在自行車機器人領域的應用尚未涉及，論文利用該方法對自行車機器人側(cè)向平衡控制進行研究，具有一定的理論意義和應用價值.

綜上所述，本文基于U-K 理論，針對自行車機器人側(cè)向自平衡問題，提出了一種滿足系統(tǒng)平衡要求的主動控制方法.相比于傳統(tǒng)的反饋控制方法，該方法從一個新的角度來解決自行車機器人的平衡控制問題，克服了初始偏差的干擾，實現(xiàn)了不同初始橫滾角速度下自行車機器人的側(cè)向平衡約束跟隨控制，并借助MATLAB 軟件對該控制方法進行了數(shù)值仿真，驗證了該系統(tǒng)的穩(wěn)定性和有效性，可實現(xiàn)自行車機器人系統(tǒng)的側(cè)向自平衡控制.

1 Udwadia?Kalaba 方程

Udwadia?Kalaba 方程是一類用于描述受約束系統(tǒng)動力學問題的方程.考慮一個機械系統(tǒng)包含n個質(zhì)點，整個系統(tǒng)在任意時刻t的位形可由廣義坐標向量q∈Rn表示，系統(tǒng)的廣義速度向量為∈Rn，廣義加速度向量為∈Rn.無約束條件下，機械系統(tǒng)的運動方程可以表述為：

其中：t為時間；M(q,t)=MT(q,t)∈Rn×n為質(zhì)量矩陣（或慣性矩陣）；F(,q,t)∈Rn包括重力、外力和離心力/科式力[25].

如果該機械系統(tǒng)受到一組約束（完整約束或非完整約束），約束方程為：

則，

將上式用矩陣形式表示為：

其中，b(·)=[b1b2···bl]T.假定機械系統(tǒng)處于理想情況下，則做出以下假設：

假設1：對于任意(q,t)∈Rn×R，系統(tǒng)慣性矩陣M?1(q,t)>0.

假設2：對于所有q,t)∈Rn×R，rank[A(q,t)]≥1.

假設3：約束方程是相容的，對于任意A(q,t)和b(,q,t)，至少存在一個滿足約束方程.

定理1：滿足假設1 至假設3，受約束的機械系統(tǒng)運動方程，即Udwadia?Kalaba 方程[17]：

其中，F(xiàn)c(,q,t)為約束力，(A(q,t)M(q,t)?1/2)+為A(q,t)M(q,t)?1/2的逆矩陣[26].

當系統(tǒng)受到一個伺服約束，且不存在初始偏差和不確定性時，控制器可以基于模型設計為τ=Fc(,q,t)，可使系統(tǒng)滿足約束方程（9），實現(xiàn)控制要求.對于欠驅(qū)動系統(tǒng)，其獨立的控制輸入變量個數(shù)少于系統(tǒng)的自由度個數(shù)，因此，常用于全驅(qū)動機器人的控制方法通常難以直接應用到自行車機器人控制系統(tǒng)中，則控制器可以基于模型架構成B(q,t)τ形式，來實現(xiàn)系統(tǒng)的控制要求.其中B(q,t)∈Rn×u為系統(tǒng)的輸入矩陣，τ ∈Ru×1為系統(tǒng)的控制扭矩，且n>u[24].

2 自行車機器人動力學建模和控制

2.1 機器人動力學模型

自行車機器人結構簡圖如圖1 所示.O?XYZ代表全局慣性參考坐標系，其中O為后輪與地面接觸點，OX是機器人行走方向，OZ是垂直地面向上的方向，θ為橫滾角（機器人相對于垂直面的傾斜角度），θ1為角動量輪旋轉(zhuǎn)角度，整個機器人的質(zhì)心用點P表示，m1和m2分別表示機器人（包括前后輪）和角動量輪的質(zhì)量，L1和L2分別表示從地面到機器人質(zhì)心和角動量輪質(zhì)心的距離，除角動量輪外的機器人繞X軸的轉(zhuǎn)動慣量為I1，角動量輪的轉(zhuǎn)動慣量為I2，g表示重力加速度.

圖1 自行車機器人結構簡圖Fig.1 Simplified structure diagram of bicycle robot

利用歐拉-拉格朗日方程建立機器人的無約束動力學模型[27]：

其中：W為拉格朗日量；T為系統(tǒng)動能；U為系統(tǒng)勢能；Qi為系統(tǒng)所受外力;q=[θ,θ1]T.系統(tǒng)的運動可分為兩部分：平動和轉(zhuǎn)動.對于平動運動，自行車機器人質(zhì)心和角動量輪質(zhì)心的速度分別為:

其中：vy1為自行車機器人Y方向的線速度；vz1為自行車機器人Z方向的線速度；vy2為角動量輪Y方向的線速度；vz2為角動量輪Z方向的線速度.

機器人和角動量輪的轉(zhuǎn)動速度（ω1和ω2）為：

因此結合式（13）～（15），系統(tǒng)的動能可表示為：

其中：T1為系統(tǒng)平動動能；T2為系統(tǒng)轉(zhuǎn)動動能.

系統(tǒng)的勢能表示為：

結合式（12），自行車機器人系統(tǒng)靜止時刻的動力學方程可表示為：

其中，B=[0;1]為系統(tǒng)的輸入矩陣；τ ∈R1×1為角動量輪提供的控制扭矩.

2.2 控制器設計

當自行車機器人存在一定的初始橫滾角速度[t,∞)→Rn，且qt連續(xù)，則機器人系統(tǒng)的期望速度，期望加速度.而后將系統(tǒng)的位置誤差為零看作系統(tǒng)的一個完整約束，即q?qt=0.定義系統(tǒng)位置誤差：

為使系統(tǒng)位置能夠完全到達期望位置，需要滿足以下約束：

其中，K1與K2為常數(shù)，且K1，K2均大于0.

將式（25）重新表達為矩陣形式：

其中：A1=[1 0]；b1=?K2θ；qt(θ=0).

根據(jù)定理1 可以得出維持機器人側(cè)向平衡的約束力模型，且該模型是實現(xiàn)平衡約束所需扭矩的最小值[28]，根據(jù)約束力的解析模型，設計系統(tǒng)的控制扭矩 τ[29]為：

其中：h∈Rn為任意向量；E為單位矩陣.

該控制器設計可以消除初始位置誤差的影響，令自行車機器人橫滾角 θ快速收斂到平衡位置，使系統(tǒng)達到穩(wěn)定狀態(tài).其控制器設計步驟總結為：首先根據(jù)側(cè)向平衡條件，構造出自行車機器人平衡控制的運動學約束；然后將該約束重新表達為二階矩陣形式；最后根據(jù)定理1，設計出滿足式(26)平衡約束條件的控制扭矩 τ.其流程如圖2所示.

圖2 自平衡控制器設計流程圖Fig.2 Flow chart of controller design

3 仿真及結果分析

假設自行車機器人仿真參數(shù)如表1 所示[30]，利用MATLAB ODE45 對自行車機器人進行數(shù)值仿真.初始值設定如下：q=[10 0]，=[1 0]，仿真結果如圖3～圖5 所示.

表1 自行車機器人參數(shù)表Table 1 Bicycle robot parameters

圖3 橫滾角曲線圖Fig.3 Angle graph

圖4 扭矩曲線圖Fig.4 Torque graph

圖5 誤差曲線圖Fig.5 Error graph

圖3 與圖4 分別表示自行車機器人橫滾角 θ與角動量輪提供的控制扭矩值 τ隨時間t變化的曲線圖.當初始橫滾角 θ為10°時，在角動量輪所提供控制扭矩的作用下，系統(tǒng)的橫滾角 θ約5 s 后收斂于平衡位置.

圖5 所示為系統(tǒng)的位置誤差值e與速度誤差值隨時間t變化的曲線圖.圖6 呈現(xiàn)了角動量輪提供的控制扭矩值τ、時間t與位置誤差值e的關系圖，隨著時間t的增大或位置誤差值e的減小，控制扭矩值 τ逐漸減小.

圖6 扭矩角度誤差曲線圖Fig.6 Torque and angle error graph

圖7 橫滾角度曲線圖Fig.7 Angle curves

圖7 所示為自行車機器人橫滾角 θ在不同的初始橫滾角速度 θ˙的情況下隨時間t變化的曲線圖，可知橫滾角 θ由初始給定的10°開始衰減，在2.4 s左右減至?0.4°，約5 s 后穩(wěn)定收斂于期望值0°.圖8所示為自行車機器人橫滾角速度在不同的初始橫滾角速度的情況下隨時間t變化的曲線圖，可知橫滾角速度隨著初始偏差干擾的增大，其超調(diào)量的絕對值逐漸增大.圖9 所示為角動量輪的旋轉(zhuǎn)角速度在不同的初始橫滾角速度的情況下隨時間t變化的曲線圖.圖10 所示為角動量輪提供的控制扭矩值 τ在不同的初始橫滾角速度的情況下隨時間t變化的曲線圖，從局部放大圖可知，仿真伊始隨著初始橫滾角速度 θ˙的增大，所需控制扭矩值 τ隨之增大.

圖8 橫滾角速度曲線圖Fig.8 Angular velocity curves

圖9 角動量輪角速度曲線圖Fig.9 Angular momentum wheel angular velocity curves

圖10 扭矩曲線圖Fig.10 Torque graph

為了與本文提出的約束跟隨控制器進行控比較，我們提出了以下針對自行車機器人自平衡系統(tǒng)的傳統(tǒng)PD 控制器[31]：

其中：τ′為PD 控制器產(chǎn)生的控制扭矩；HD為微分系數(shù)；HP為比例系數(shù).

圖11 為PD 控制器simulink 環(huán)境下的系統(tǒng)框架圖，圖12 為傳統(tǒng)PD 控制器與本文約束跟隨控制器仿真對比曲線圖，從仿真結果可知本文采用的控制方法較于傳統(tǒng)的PD 控制方法，具有系統(tǒng)響應速度快、超調(diào)量小以及系統(tǒng)震蕩次數(shù)顯著減少等特點.

圖11 PD 控制器simulink 框架圖Fig.11 Simiulink frame diagram of PD controller

圖12 PD 控制器和約束跟隨控制器對比曲線圖Fig.12 Comparison of PD controller and constraint following controller

4 結論

(1) 本文將Udwadia?Kalaba 理論應用至裝有角動量輪的自行車機器人靜止狀態(tài)平衡控制中，闡述了該方法的原理與應用條件，并依據(jù)側(cè)向平衡條件，構造出機器人平衡控制的運動學約束，建立了滿足運動約束的扭矩解析模型，該解析模型可以實現(xiàn)所需平衡控制的最小扭矩.

(2) 通過仿真分析的方式，說明了所設計的約束跟隨控制器可在不同初始橫滾角速度下，實現(xiàn)自行車機器人系統(tǒng)的自平衡控制，保證了該控制系統(tǒng)的穩(wěn)定性和有效性.相比于傳統(tǒng)的PD 反饋控制方法，該方法具有系統(tǒng)響應時間快、超調(diào)量小等優(yōu)點.因此，該方法可為無人駕駛自行車機器人的側(cè)向平衡問題提供一種新的控制方法.