孫 平， 劉 博， 楊德國

(沈陽工業(yè)大學(xué) a. 信息科學(xué)與工程學(xué)院， b. 軟件學(xué)院， 沈陽 110870)

全方向康復(fù)步行訓(xùn)練機器人的跟蹤控制*

孫 平a， 劉 博a， 楊德國b

(沈陽工業(yè)大學(xué) a. 信息科學(xué)與工程學(xué)院， b. 軟件學(xué)院， 沈陽 110870)

全方向康復(fù)步行訓(xùn)練機器人； 動力學(xué)模型； 運動學(xué)模型； 模型解析； 非線性反饋控制律； 輸入輸出線性化； 速度跟蹤； 軌跡跟蹤

機器人控制是一項具有挑戰(zhàn)性的任務(wù)，作為機器人控制研究的一個重要問題，軌跡跟蹤近年來得到了廣泛的研究.為了滿足高性能跟蹤的要求，多種控制方法被提出，如自適應(yīng)跟蹤控制方法[1]、反饋線性化跟蹤控制方法[2]和自適應(yīng)模糊跟蹤控制方法[3]等，然而，上述研究中只考慮了軌跡跟蹤，忽略了速度跟蹤這一重要因素.速度跟蹤在康復(fù)機器人跟蹤過程中起著至關(guān)重要的作用，如果速度不能達到醫(yī)生指定的跟蹤要求，將會影響康復(fù)效果并對康復(fù)者的安全造成威脅，因此康復(fù)機器人同時實現(xiàn)軌跡跟蹤和速度跟蹤具有重要意義.

近年來，由于人口老齡化及交通事故等原因，步行障礙的患者越來越多，使康復(fù)步行訓(xùn)練機器人得到了迅速發(fā)展.醫(yī)生將康復(fù)訓(xùn)練路徑存儲在機器人內(nèi)，患者只需跟隨機器人便可進行康復(fù)訓(xùn)練，因此，康復(fù)機器人精確跟蹤醫(yī)生指定的訓(xùn)練路徑非常重要.然而在軌跡跟蹤過程中難免會出現(xiàn)誤差，機器人為了消除誤差就會加快運動速度，而運動速度超過康復(fù)者的承受能力，就會影響康復(fù)者的安全，因此，康復(fù)機器人同時實現(xiàn)速度跟蹤和軌跡跟蹤是一個值得研究的問題.

全方向康復(fù)步行訓(xùn)練機器人(omnidirectional rehabilitative training walker，簡稱ODW)能對患者進行各個方向的康復(fù)訓(xùn)練，改變了傳統(tǒng)康復(fù)機器人固定位置的訓(xùn)練方式[4].文獻[5-6]采用魯棒控制方法實現(xiàn)了ODW的軌跡跟蹤；文獻[7-8]采用自適應(yīng)控制方法實現(xiàn)了ODW的軌跡跟蹤；文獻[9]采用保性能控制方法實現(xiàn)了ODW的軌跡跟蹤.然而上述方法僅考慮了軌跡跟蹤控制，而沒有考慮速度跟蹤，鑒于此，本文主要解決ODW同時實現(xiàn)速度和軌跡跟蹤問題，完成的主要工作如下：

1) 基于輸入輸出線性化方法，利用ODW運動學(xué)模型和動力學(xué)模型建立了四輪轉(zhuǎn)速與驅(qū)動力矩間的解耦狀態(tài)方程；

2) 針對速度解耦狀態(tài)方程，提出了速度跟蹤控制器設(shè)計方法，并使速度跟蹤達到漸近穩(wěn)定，進一步利用速度跟蹤控制器和非線性反饋控制律，通過ODW動力學(xué)模型實現(xiàn)了軌跡跟蹤；

3) 通過仿真算例表明，ODW同時實現(xiàn)了速度跟蹤和軌跡跟蹤，驗證了文中提出輸入輸出線性化解耦方法和控制方法的有效性.

1 ODW數(shù)學(xué)模型

全方向康復(fù)步行訓(xùn)練機器人結(jié)構(gòu)坐標如圖1所示，圖中，∑(x，y，o)為全局坐標系；∑(x′，y′，c)為局部坐標系；v為ODW的速度；vi為ODW每個輪子的速度，i=1，2，3，4；fi為ODW每個輪子的輸入力，i=1，2，3，4；G為ODW的重心；r0為ODW中心到重心的距離；L為ODW中心與全向輪之間的距離；α為x′軸方向與速度v之間的夾角；θi為x′軸方向與每個輪子對應(yīng)L之間的夾角.

圖1 全方向康復(fù)步行訓(xùn)練機器人結(jié)構(gòu)坐標Fig.1 Structure coordinates for omnidirectional rehabilitative training walker

1.1 ODW運動學(xué)模型

ODW的運動學(xué)模型[10]可以描述為

(1)

(2)

由式(1)得到速度約束方程為

v1+v2=v3+v4

(3)

運動學(xué)模型可以表示為

(4)

式中：

1.2 ODW的動力學(xué)模型

ODW的動力學(xué)模型[10]可以描述為

(5)

(6)

式中：

2 輸入輸出反饋線性化解耦模型

(7)

由式(7)可以得到

(8)

f2=f4

(9)

整理后的動力學(xué)模型為

(10)

式中：

可以得到ODW系統(tǒng)的數(shù)學(xué)模型為

(11)

將式(8)進行微分運算得

(12)

將式(12)代入式(10)可以得到

(13)

將式(8)和式(13)結(jié)合，可以得到

(14)

由模型式(14)可得系統(tǒng)的狀態(tài)方程為

(15)

選取ODW的位置X和角速度w作為系統(tǒng)的狀態(tài)變量，即

(16)

則可以得到仿真非線性系統(tǒng)方程

(17)

設(shè)計非線性反饋控制律為

(18)

于是可得

(19)

系統(tǒng)數(shù)學(xué)模型為

(20)

定義系統(tǒng)的輸出為

y=h(q)=[xc，yc，θ，w1，w2，w3]T

(21)

完整的仿真非線性系統(tǒng)模型為

(22)

對輸出y進行微分可以得到

(23)

從而有

(24)

由于v1+v2=v3+v4，則可以得到

w4=w1+w2-w3

(25)

式(24)表明已經(jīng)實現(xiàn)了ODW非線性輸入輸出解耦，可見得到的閉環(huán)系統(tǒng)的輸入輸出行為就是一個線性系統(tǒng)行為，它由三個解耦的單輸入單輸出子系統(tǒng)構(gòu)成.

3 控制器設(shè)計與穩(wěn)定性分析

ODW的角速度跟蹤問題就是要求設(shè)計反饋控制器，使得系統(tǒng)的響應(yīng)漸近地趨于期望軌跡.為了使得ODW的輸出跟蹤任意參考信號wd=[w1d，w2d，w3d]T，對解耦后的系統(tǒng)線性子系統(tǒng)設(shè)計比例控制器.

定義跟蹤的角速度誤差為

ei=wi-wid(i=1，2，3)

(26)

則系統(tǒng)的角速度跟蹤控制問題就是要求設(shè)計反饋控制器，使得ODW的響應(yīng)漸近地趨于期望軌跡，即

(27)

由式(24)和(26)可得角速度和控制之間的線性關(guān)系，因此設(shè)計控制器為

(28)

式中：wid為參考角速度；Kpi為常數(shù).

將式(24)代入到式(28)可得

(29)

將式(29)進一步整理可以得到

(30)

將式(29)中的Kpi取負值，t→∞情況下，誤差ei→0，則可以說明速度跟蹤系統(tǒng)漸近穩(wěn)定.由此可得系統(tǒng)的比例控制器為

(31)

利用控制式(29)和(19)，通過動力學(xué)模型可以實現(xiàn)軌跡跟蹤.

4 仿真研究

為了驗證文中提出控制器設(shè)計方法的有效性，基于ODW運動學(xué)模型和動力學(xué)模型，對醫(yī)生指定的直線軌跡進行了跟蹤，直線方程描述為

圖2 w1軌跡跟蹤Fig.2 Trajectory tracking of w1

圖3 w2軌跡跟蹤Fig.3 Trajectory tracking of w2

根據(jù)圖2～5可知：利用輸入輸出線性化方法所設(shè)計的控制器能夠?qū)χ付ㄜ壽E的角速度進行跟蹤，并且在很短的時間內(nèi)達到穩(wěn)定.由式(28)可知控制器是一種有差控制，但角速度跟蹤過程是沒有誤差的.角速度跟蹤誤差曲線如圖6～9所示.

圖4 w3軌跡跟蹤Fig.4 Trajectory tracking of w3

圖5 w4軌跡跟蹤Fig.5 Trajectory tracking of w4

圖6 w1跟蹤誤差Fig.6 Tracking error of w1

圖7 w2跟蹤誤差Fig.7 Tracking error of w2

由圖6～9可知，通過控制器不斷修正，消除誤差，實現(xiàn)角速度的漸近跟蹤.軌跡跟蹤仿真曲線如圖10～13所示.

通過圖10～13可知，所設(shè)計控制器對于全局坐標系內(nèi)三個位置量和直線軌跡都可以進行跟蹤，雖然方向角跟蹤在開始存在一定的誤差，但在控制器的作用下，誤差逐漸減小，最后趨于零.

圖8 w3跟蹤誤差Fig.8 Tracking error of w3

圖9 w4跟蹤誤差Fig.9 Tracking error of w4

圖10 x軸軌跡跟蹤Fig.10 Trajectory tracking of x axis

圖11 y軸軌跡跟蹤Fig.11 Trajectory tracking of y axis

圖14為不規(guī)則菱形軌跡跟蹤曲線，由圖14可以直觀看出ODW對不規(guī)則菱形運動路徑也能實現(xiàn)軌跡跟蹤.仿真結(jié)果表明，經(jīng)過輸入輸出線性化的ODW系統(tǒng)在控制器的作用下可以同時實現(xiàn)速度跟蹤和軌跡跟蹤.

圖12 方向角跟蹤Fig.12 Trajectory tracking of direction angle

圖13 直線軌跡跟蹤Fig.13 Trajectory tracking of straight line

圖14 不規(guī)則菱形軌跡跟蹤Fig.14 Trajectory tracking of irregular lozenge

5 結(jié) 論

本文研究了全方向康復(fù)步行訓(xùn)練機器人的跟蹤控制問題，提出了基于輸入輸出線性化方法的跟蹤控制器設(shè)計，建立了四輪輪速和其驅(qū)動力間的解耦模型，通過跟蹤控制和反饋控制實現(xiàn)了速度和軌跡的同時跟蹤.本文提出方法不僅適用于全方向康復(fù)步行訓(xùn)練機器人，還適用于其他輪式機器人.

[1]李會來，李小民，陳靜華.非完整移動機器人軌跡跟蹤自適應(yīng)控制器設(shè)計 [J].傳感器與微系統(tǒng)，2011，30(5)：104-106.

(LI Hui-lai，LI Xiao-min，CHEN Jing-hua.Design of adaptive trajectory tracking controller for nonholono-mic mobile robots [J].Transducer and Microsystem Technologies，2011，30(5)：104-106.)

[2]劉子龍，蔣辰飛，劉潔，等.非完整移動機器人級聯(lián)反饋線性化跟蹤控制 [C]//中國控制協(xié)會第32次會議.中國，西安，2013：4232-4237.

(LIU Zi-long，JIANG Chen-fei，LIU Jie，et al.Cascaded feedback linearization tracking control of nonhonomic mobile robots [C]//Proceedings of the 32nd Chinese Control Conference.Xi’an，China，2013：4232-4237.)

[3]弓洪瑋，鄭維.機器人軌跡跟蹤的自適應(yīng)模糊神經(jīng)網(wǎng)絡(luò)控制 [J].計算機仿真，2010，27(8)：145-149.

(GONG Hong-wei，ZHENG Wei.Adaptive fuzzy-neuro control for trajectory tracking of robot [J].Computer Simulation，2010，27(8)：145-149.)

[4]Wang S Y，Inoue H，Kawata K，et al.Developing the omnidirectional mobile walker and verifying its effect of increase in the muscle power [C]//JSME Symposium on Walfare Engineering.Tokyo，Japan，2007：176-177.

[5]楊俊友，白殿春，王碩玉，等.全方向輪式下肢康復(fù)訓(xùn)練機器人軌跡跟蹤控制 [J].機器人，2011，33(3)：314-318.

(YANG Jun-you，BAI Dian-chun，WANG Shuo-yu，et al.Trajectory tracking control of omnidirectional wheeled robot for lower limbs rehabilitative training [J].Robot，2011，33(3)：314-318.)

[6]孫平，彭萬平.全方向步行康復(fù)訓(xùn)練器的魯棒容錯跟蹤控制 [J].沈陽工業(yè)大學(xué)學(xué)報，2014，36(2)：218-223.

(SUN Ping，PENG Wan-ping.Robust fault-tolerant tracking control for omni-directional rehabilitative training walker [J].Journal of Shenyang University of Technology，2014，36(2)：218-223.)

[7]Tan R P，Wang S Y，Jiang Y L.Adaptive control method for path-tracking control of an omnidirectional walker compensating for center-of-gravity shifts and load changes [J].International Journal of Innovative Computing Information and Control，2011，7(7)：4423-4434.

[8]Tan R P，Wang S Y，Jiang Y L，et al.Nonlinear adaptive controller for omnidirectional walker：dynamic model improvement and experiment [J].ICIC Express Letters，2012，6(3)：611-615.

[9]Sun P，Wang S Y.Redundant input guaranteed cost switched training control for omnidirectional rehabilitative training walker [J].International Journal of Innovative Computing，Information and Control，2014，10(3)：883-895.

[10]Tan R P，Wang S Y，Jiang Y L.Adaptive controller for omnidirectional walker [C]//Proceedings of the 2010 IEEE International Conference on Mechatronics and Automation.Xi’an，China，2010：156-161.

[11]Duan Z S，Huang L，Yang Y.The effects of redundant control inputs in optimal control [J].Information Sciences，2009，52(11)：1973-1981.

(責任編輯：景 勇 英文審校：尹淑英)

Tracking control of omnidirectional rehabilitative training walker

SUN Pinga, LIU Boa, YANG De-guob

(a. School of Information Science and Engineering, b. School of Software, Shenyang University of Technology, Shenyang 110870, China)

In order to solve the strong coupling between thexaxis,yaxis and direction angle trajectory of omnidirectional rehabilitative training walker in the process of tracking movement, a speed tracking controller design method was proposed, so that the ideal tracking effect in each direction for the omnidirectional rehabilitative training walker could be realized at the same time. Based on the dynamic model and kinematic model of omnidirectional rehabilitative training walker, the input and output linearization method was adopted, and the decoupling state equation between four-wheel speed and its driving force was derived through the model anlysis. In addition, the speed tracking controller was designed, and the decoupling control of speed was realized. The speed controller was combined with the nonlinear feedback control law, which could realize the tracking of movement trajectory. The results of simulation experiments show that the proposed mehod has certain effectiveness, and can solve the probem that the movement velocity and trajectory in the traditional tracking process can not be tracked simultaneously.

omnidirectional rehabilitative training walker; dynamic model; kinematic model; model analysis; nonlinear feedback control law; input and output linearization; speed tracking; trajectory tracking

2015-10-20.

遼寧省高等學(xué)校優(yōu)秀人才支持計劃資助項目(2014013)； 遼寧省自然科學(xué)基金資助項目(2015020066).

孫 平(1974-)，女，遼寧營口人，副教授，博士，主要從事康復(fù)機器人控制等方面的研究.

14∶01在中國知網(wǎng)優(yōu)先數(shù)字出版.

http：∥www.cnki.net/kcms/detail/21.1189.T.20160512.1401.022.html

10.7688/j.issn.1000-1646.2017.01.17

TP 273

A

1000-1646(2017)01-0088-06