， ，，，

(東北大學(xué)機(jī)械工程與自動(dòng)化學(xué)院，遼寧 沈陽(yáng)110819)

0 引言

Pendubot是一種典型的欠驅(qū)動(dòng)非穩(wěn)定機(jī)器人[1]，由兩個(gè)機(jī)械臂組成，第一個(gè)機(jī)械臂(肩部關(guān)節(jié))由電機(jī)驅(qū)動(dòng)，為主動(dòng)機(jī)械臂，第二個(gè)機(jī)械臂(肘部關(guān)節(jié))可以自由活動(dòng)，為欠驅(qū)動(dòng)臂。Pendubot可以作為欠驅(qū)動(dòng)基準(zhǔn)系統(tǒng)，用于驗(yàn)證各種非線性控制算法。Pendubot系統(tǒng)的控制分為搖起控制和平衡控制兩部分。搖起控制是從穩(wěn)定的下垂位置(Down-Down)運(yùn)動(dòng)到最高不穩(wěn)定平衡位置(Up-Up)；平衡控制是使系統(tǒng)穩(wěn)定在最高不穩(wěn)定平衡位置。通常，搖起控制和平衡控制分別設(shè)計(jì)，并通過(guò)控制轉(zhuǎn)換裝置進(jìn)行切換。

早在1995年美國(guó)的Spong等就對(duì)Pendubot系統(tǒng)的可控性進(jìn)行了證明[2]。Pendubot系統(tǒng)的搖起控制主要方法主要有部分反饋線性化方法[2]、能量控制方法[3-4]、混雜控制方法[5]、滑膜控制方法[6-7]和智能控制方法[8]等；平衡控制的主要方法有線性二次型調(diào)節(jié)器(LQR)[2]、極點(diǎn)配置法、模糊控制[9-10]、角動(dòng)量控制[11]和神經(jīng)網(wǎng)絡(luò)控制[12]等。在Pendubot系統(tǒng)的控制中，LQR控制器是應(yīng)用最廣泛的平衡控制器。在已有的文獻(xiàn)中，采用LQR控制器進(jìn)行Pendubot系統(tǒng)的平衡控制時(shí)，兩個(gè)機(jī)械臂均處于豎直位置，平衡狀態(tài)下主動(dòng)臂的力矩為0。對(duì)Pendubot系統(tǒng)處于非最高不穩(wěn)定平衡位置時(shí)的平衡控制進(jìn)行研究，平衡狀態(tài)下主動(dòng)臂的力矩不為0。在詳細(xì)分析Pendubot系統(tǒng)的動(dòng)力學(xué)方程的基礎(chǔ)上，對(duì)LQR控制器進(jìn)行了改進(jìn)。建立Pendubot系統(tǒng)的仿真模型，對(duì)該方法進(jìn)行了驗(yàn)證。

1 Pendubot系統(tǒng)的動(dòng)力學(xué)模型

通過(guò)拉格朗日方程來(lái)建立Pendubot系統(tǒng)的動(dòng)力學(xué)模型(不考慮摩擦力的影響)為:

(1)

圖1 Pendubot系統(tǒng)的模型

在圖1中，m1和m2分別表示主動(dòng)臂和欠驅(qū)動(dòng)臂的質(zhì)量，q1表示主動(dòng)臂相對(duì)于坐標(biāo)軸x的夾角，q2表示欠驅(qū)動(dòng)臂相對(duì)于主動(dòng)臂的夾角，l1和l2分別表示主動(dòng)臂和欠驅(qū)動(dòng)臂的長(zhǎng)度，lc1和lc2分別表示主動(dòng)臂和欠驅(qū)動(dòng)臂相對(duì)于質(zhì)心的距離，I1和I2分別表示主動(dòng)臂和欠驅(qū)動(dòng)臂的轉(zhuǎn)動(dòng)慣量。下面將Pendubot系統(tǒng)的動(dòng)力學(xué)方程中的參數(shù)變?yōu)橐韵碌?個(gè)新參數(shù)。

(2)

將式(2)中的參數(shù)帶入到Pendubot系統(tǒng)的動(dòng)力學(xué)方程中，可以得到

(3)

(4)

(5)

假設(shè)τ1/θ4g≤1，可以求解出q1和q2為：

其中n=1,3,5,…

(6)

(7)

(8)

2 LQR控制器

線性二次型調(diào)節(jié)器(LQR)屬于現(xiàn)代控制理論，其性能指標(biāo)易于分析、處理和計(jì)算，并具有很好的魯棒性。LQR的目的是在一定的性能指標(biāo)下，使系統(tǒng)的控制效果最佳，即利用最少的控制量來(lái)達(dá)到最小的狀態(tài)誤差。根據(jù)Pendubot系統(tǒng)在平衡位置進(jìn)行線性化后的狀態(tài)方程，定義如下的性能指標(biāo)

(9)

矩陣Q是正定或半正定實(shí)對(duì)稱矩陣，即Q≥0，矩陣R是正定實(shí)對(duì)稱矩陣，即R>0。矩陣Q和R用來(lái)平衡狀態(tài)變量和輸入向量的權(quán)重。使 J達(dá)到最小值的最優(yōu)控制律為：

τ1=-R-1BTPx= -Kx

(10)

其中，參數(shù)K=R-1BTP為最優(yōu)狀態(tài)反饋矩陣，參數(shù)P是下面Riccati方程的解。

ATP+PA-PBR-1BTP+Q=0

(11)

3 改進(jìn)的LQR進(jìn)行平衡控制

如果將Pendubot系統(tǒng)穩(wěn)定在狀態(tài)變量xi=[π/2 0 0 0]T的不穩(wěn)定平衡位置，由于平衡狀態(tài)下主動(dòng)臂的驅(qū)動(dòng)力矩為0，因此可以直接采用式(10)進(jìn)行平衡控制。本文的控制目標(biāo)是將Pendubot系統(tǒng)穩(wěn)定在狀態(tài)變量為xq=[3π/8 π/8 0 0]T的不穩(wěn)定平衡位置，即主動(dòng)臂的角度為3π/8，欠驅(qū)動(dòng)臂為豎直位置。對(duì)于位置xq，由于驅(qū)動(dòng)力矩大于0，不能直接采用式(10)進(jìn)行平衡控制。為了使Pendubot系統(tǒng)穩(wěn)定在平衡位置xq，需要對(duì)LQR控制器進(jìn)行改進(jìn)。

首先求解平衡位置xq處的狀態(tài)反饋矩陣K。將狀態(tài)變量xq帶入到式(8)中，得到矩陣A和B分別為：

在MATLAB軟件中采用函數(shù)lqr()進(jìn)行LQR控制器的設(shè)計(jì)，該函數(shù)的調(diào)用格式為K=lqr(A,B,Q,R)，其中參數(shù)Q為4階單位矩陣，參數(shù)R=1。在平衡位置xq，得到的狀態(tài)反饋矩陣為K=[-71.22 -56.78 -12.76 -8.31]。

為了使Pendubot系統(tǒng)穩(wěn)定在平衡位置xq，在式(10)中，將狀態(tài)變量修改為xqΔ=[3π/8+Δπ/8-Δ0 0]T，即

τ1=-KxqΔ

(12)

參數(shù)Δ為補(bǔ)償角度值。下面介紹如何求解Δ。

Pendubot系統(tǒng)處于平衡位置xq時(shí)，驅(qū)動(dòng)力矩為恒定值，并滿足式(5)，則驅(qū)動(dòng)力矩為：

τ1=θ4gcos(q1)+θ5gcos(q1+q2)

=0.783 1 N·m

(13)

將力矩τ1，狀態(tài)變量xqΔ和反饋矩陣K帶入公式(12)，可以求得 Δ=0.054 3。

3 仿真分析

在仿真中采用的Pendubot系統(tǒng)[3]模型參數(shù)為θ1=0.030 8 kg·m2，θ2=0.010 6 kg·m2，θ3=0.009 5 kg·m2，θ4=0.208 6 kg·m，θ5=0.063 0 kg·m。

采用MATLAB/Simulink建立Pendubot的系統(tǒng)模型，系統(tǒng)的仿真時(shí)間為3 s，初始狀態(tài)的狀態(tài)變量為xi=[π/2 0 0 0]T，仿真步長(zhǎng)為1 ms。

將Pendubot系統(tǒng)從初始狀態(tài)穩(wěn)定到狀態(tài)變量為xq=[3π/8 π/8 0 0]T的不穩(wěn)定平衡位置。采用LQR控制器和改進(jìn)的LQR控制器的結(jié)果如圖2和圖3所示。在圖2中，采用LQR控制器時(shí)，系統(tǒng)穩(wěn)定時(shí)主動(dòng)臂的角度q1=1.226 0，大于設(shè)定的角度值3π/8；欠驅(qū)動(dòng)臂的角度q2=0.344 8，小于設(shè)定的角度值π/8。采用改進(jìn)的LQR控制器時(shí)，系統(tǒng)穩(wěn)定時(shí)主動(dòng)臂的角度q1=3π/8，欠驅(qū)動(dòng)臂的角度為q2=π/8，即穩(wěn)定在位置xq。

圖2 Pendubot系統(tǒng)的輸出角度

在圖3中，采用LQR控制器時(shí)，穩(wěn)定狀態(tài)下的驅(qū)動(dòng)力矩為0.691 7 N·m；采用改進(jìn)的LQR控制器時(shí)，穩(wěn)定狀態(tài)下的驅(qū)動(dòng)力矩為0.783 1 N·m，和式(13)的計(jì)算值相等。采用LQR控制器和改進(jìn)的LQR控制器都能使Pendubot系統(tǒng)穩(wěn)定。直接采用LQR控制器時(shí)，Pendubot系統(tǒng)并沒(méi)有穩(wěn)定在設(shè)置的平衡位置xq，但是改進(jìn)的LQR控制器能夠使系統(tǒng)穩(wěn)定在設(shè)置的位置xq。

圖3 輸入力矩

4 結(jié)束語(yǔ)

在利用LQR進(jìn)行Pendubot系統(tǒng)的平衡控制中，當(dāng)主動(dòng)臂和欠驅(qū)動(dòng)臂都處于豎直位置時(shí)，在穩(wěn)定狀態(tài)下主動(dòng)臂的輸入力矩為0，可以直接采用LQR進(jìn)行平衡控制。利用LQR對(duì)Pendubot系統(tǒng)的平衡位置進(jìn)行控制時(shí)，主動(dòng)臂不在豎直位置，只有欠驅(qū)動(dòng)臂在豎直位置。如果直接采用LQR進(jìn)行控制，由于平衡位置的力矩大于0，Pendubot系統(tǒng)并不能穩(wěn)定在設(shè)置的位置。根據(jù)設(shè)定的平衡位置，以及平衡位置處的力矩值，設(shè)計(jì)了一個(gè)補(bǔ)償參數(shù)，并給出了補(bǔ)償參數(shù)的詳細(xì)計(jì)算方法。通過(guò)改進(jìn)的LQR控制器，使得Pendubot系統(tǒng)能夠穩(wěn)定在設(shè)置的平衡位置。

[1] LIU Y, YU H. A survey of underactuated mechanical systems[J]. IET Control Theory and Applications, 2013, 7(7):1-15.

[2] SPONG M W, BLOCK D J. The pendubot: a mechatronic system for control research and education[C]//Proceedings of the 34th Conference on Decision & Control, 1995, 555-556.

[3] FANTONI I, LOAZNO R, SPONG M W. Energy based control of the pendubot[J]. IEEE Transactions on Automatic Control, 2000, 45(4):725-729.

[4] XIN X, TANAKA S, SHE J, et al. New analytical results of energy-based swing-up control for the Pendubot [J]. International Journal of Non-linear Mechanics, 2013, 52(3):110-118.

[5] ZHANG M, TARN T J. Hybrid control of the pendubot[J]. IEEE/ASME Transactions on Mechatronics, 2002, 7(1):79-86.

[6] 王偉，易建強(qiáng)，趙冬斌,等. Pendubot的一種分層滑膜控制方法[J]. 控制理論與應(yīng)用, 2005, 22(3):417-422.

[7] 牛瑞燕，許午嘯，劉金琨. 欠驅(qū)動(dòng)機(jī)械臂滑膜控制與實(shí)驗(yàn)研究[J]. 儀器儀表學(xué)報(bào), 2016, 37(2):348-355.

[8] EOM M, CHEA D. Robust swing-up and balancing control using a nonlinear disturbance observer for the pendubot system with dynamic friction[J]. IEEE Trans. Robotics, 2015, 31(2):331-343.

[9] LI W, TANAKA K, WANG H O. Acrobatic control of a Pendubot[J]. IEEE Transactions on Fuzzy Systems, 2004, 12(4):549-552.

[10] Sanchez E, Flores V. Real-time underactuated robot swing-up via fuzzy PI+PD control[J]. Journal of Intelligent & Fuzzy Systems, 2006, 17(1):1-13.

[11] AZAD M, FEATHERSTONE R. Angular momentum based balance controller for an under-actuated planar robot [J]. Autonomous Robots, 2015, 40(1):1-15.

[12] XIA D, WANG L, CHAI T. Neural-network-friction compensation based energy swing-up control of Pendubot[J]. IEEE Transactions on Industrial Electronics, 2014, 61(3):1411-1423.