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

孫 平， 劉 博， 楊德國(guó)

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

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

孫 平a， 劉 博a， 楊德國(guó)b

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

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

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

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

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

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

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

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

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

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

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

1.1 ODW運(yùn)動(dòng)學(xué)模型

ODW的運(yùn)動(dòng)學(xué)模型[10]可以描述為

(1)

(2)

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

v1+v2=v3+v4

(3)

運(yùn)動(dòng)學(xué)模型可以表示為

(4)

式中：

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

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

(5)

(6)

式中：

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

(7)

由式(7)可以得到

(8)

f2=f4

(9)

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

(10)

式中：

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

(11)

將式(8)進(jìn)行微分運(yùn)算得

(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è)計(jì)非線性反饋控制律為

(18)

于是可得

(19)

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

(20)

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

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

(21)

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

(22)

對(duì)輸出y進(jìn)行微分可以得到

(23)

從而有

(24)

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

w4=w1+w2-w3

(25)

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

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

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

定義跟蹤的角速度誤差為

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

(26)

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

(27)

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

(28)

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

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

(29)

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

(30)

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

(31)

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

4 仿真研究

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

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

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

根據(jù)圖2～5可知：利用輸入輸出線性化方法所設(shè)計(jì)的控制器能夠?qū)χ付ㄜ壽E的角速度進(jìn)行跟蹤，并且在很短的時(shí)間內(nèi)達(dá)到穩(wěn)定.由式(28)可知控制器是一種有差控制，但角速度跟蹤過(guò)程是沒(méi)有誤差的.角速度跟蹤誤差曲線如圖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可知，通過(guò)控制器不斷修正，消除誤差，實(shí)現(xiàn)角速度的漸近跟蹤.軌跡跟蹤仿真曲線如圖10～13所示.

通過(guò)圖10～13可知，所設(shè)計(jì)控制器對(duì)于全局坐標(biāo)系內(nèi)三個(gè)位置量和直線軌跡都可以進(jìn)行跟蹤，雖然方向角跟蹤在開(kā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對(duì)不規(guī)則菱形運(yùn)動(dòng)路徑也能實(shí)現(xiàn)軌跡跟蹤.仿真結(jié)果表明，經(jīng)過(guò)輸入輸出線性化的ODW系統(tǒng)在控制器的作用下可以同時(shí)實(shí)現(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)練機(jī)器人的跟蹤控制問(wèn)題，提出了基于輸入輸出線性化方法的跟蹤控制器設(shè)計(jì)，建立了四輪輪速和其驅(qū)動(dòng)力間的解耦模型，通過(guò)跟蹤控制和反饋控制實(shí)現(xiàn)了速度和軌跡的同時(shí)跟蹤.本文提出方法不僅適用于全方向康復(fù)步行訓(xùn)練機(jī)器人，還適用于其他輪式機(jī)器人.

[1]李會(huì)來(lái)，李小民，陳靜華.非完整移動(dòng)機(jī)器人軌跡跟蹤自適應(yīng)控制器設(shè)計(jì) [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]劉子龍，蔣辰飛，劉潔，等.非完整移動(dòng)機(jī)器人級(jí)聯(lián)反饋線性化跟蹤控制 [C]//中國(guó)控制協(xié)會(huì)第32次會(huì)議.中國(guó)，西安，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]弓洪瑋，鄭維.機(jī)器人軌跡跟蹤的自適應(yīng)模糊神經(jīng)網(wǎng)絡(luò)控制 [J].計(jì)算機(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ī)器人軌跡跟蹤控制 [J].機(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]孫平，彭萬(wàn)平.全方向步行康復(fù)訓(xùn)練器的魯棒容錯(cuò)跟蹤控制 [J].沈陽(yáng)工業(yè)大學(xué)學(xué)報(bào)，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.

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

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)秀人才支持計(jì)劃資助項(xiàng)目(2014013)； 遼寧省自然科學(xué)基金資助項(xiàng)目(2015020066).

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

14∶01在中國(guó)知網(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