基于Udwadia?Kalaba 理論的自行車機器人平衡控制方法

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

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

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

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

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

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

1 Udwadia?Kalaba 方程

Udwadia?Kalaba 方程是一類用于描述受約束系統(tǒng)動力學(xué)問題的方程.考慮一個機械系統(tǒng)包含n個質(zhì)點，整個系統(tǒng)在任意時刻t的位形可由廣義坐標(biāo)向量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)處于理想情況下，則做出以下假設(shè)：

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

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

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

定理1：滿足假設(shè)1 至假設(shè)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].

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

2 自行車機器人動力學(xué)建模和控制

2.1 機器人動力學(xué)模型

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

圖1 自行車機器人結(jié)構(gòu)簡圖Fig.1 Simplified structure diagram of bicycle robot

利用歐拉-拉格朗日方程建立機器人的無約束動力學(xué)模型[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）為：

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

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

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

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

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

2.2 控制器設(shè)計

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

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

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

將式（25）重新表達(dá)為矩陣形式：

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

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

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

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

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

3 仿真及結(jié)果分析

假設(shè)自行車機器人仿真參數(shù)如表1 所示[30]，利用MATLAB ODE45 對自行車機器人進(jìn)行數(shù)值仿真.初始值設(shè)定如下：q=[10 0]，=[1 0]，仿真結(jié)果如圖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變化的曲線圖.當(dāng)初始橫滾角 θ為10°時，在角動量輪所提供控制扭矩的作用下，系統(tǒng)的橫滾角 θ約5 s 后收斂于平衡位置.

圖5 所示為系統(tǒng)的位置誤差值e與速度誤差值隨時間t變化的曲線圖.圖6 呈現(xiàn)了角動量輪提供的控制扭矩值τ、時間t與位置誤差值e的關(guān)系圖，隨著時間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

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

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

圖11 為PD 控制器simulink 環(huán)境下的系統(tǒng)框架圖，圖12 為傳統(tǒng)PD 控制器與本文約束跟隨控制器仿真對比曲線圖，從仿真結(jié)果可知本文采用的控制方法較于傳統(tǒng)的PD 控制方法，具有系統(tǒng)響應(yī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 結(jié)論

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

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