賈利想 曹東興 魯建軍 范柳彬 李燦

摘要 爬樓輪椅的爬樓控制是研究的重點(diǎn)，而前腿是實(shí)現(xiàn)爬樓的重要輔助機(jī)構(gòu)，為了準(zhǔn)確穩(wěn)定地控制前腿的運(yùn)動(dòng)，文章建立了前腿的末端位置方程，并計(jì)算其工作空間，在工作空間內(nèi)對(duì)其進(jìn)行軌跡規(guī)劃并求末端位置方程的逆解，得到前腿的控制量。然后設(shè)計(jì)前腿控制系統(tǒng)，計(jì)算系統(tǒng)各部分的參數(shù)，設(shè)計(jì)一種模糊控制算法控制前腿按規(guī)劃的軌跡運(yùn)動(dòng)。把前腿的運(yùn)行數(shù)據(jù)通過(guò)傳感器測(cè)量下來(lái)并通過(guò)串口通信傳遞到上位PC機(jī)，上位機(jī)對(duì)數(shù)據(jù)進(jìn)行處理，分框顯示在上位機(jī)界面上，并根據(jù)已有數(shù)據(jù)，通過(guò)VC++6.0的繪圖功能，把前腿爬樓的過(guò)程動(dòng)態(tài)的繪制在上位機(jī)界面上。通過(guò)實(shí)驗(yàn)可知，本控制方法可以控制前腿按規(guī)劃的軌跡運(yùn)動(dòng)。

關(guān) 鍵 詞 爬樓輪椅;軌跡規(guī)劃;模糊控制;上位機(jī)顯示;動(dòng)態(tài)繪圖

中圖分類號(hào) TP311? ? ?文獻(xiàn)標(biāo)志碼 A

Abstract The control of stair climbing wheelchair is the focus of the study， and the the front leg is an important auxiliary mechanism to realize stair climbing. In order to control the movement of the forelegs accurately and steadily， this paper established the end position equation of the front leg， and calculated the working space in which the trajectory planning was used to get the inverse solution for the terminal position equation and the control quantity of the front leg was obtained; then the front leg control system was designed， and the parameters of each part of the system were calculated to work out a fuzzy control algorithm to control the front leg to move according to the planned trajectory. The operation data of the front leg was measured by sensors and transmitted to the host PC through serial communication. The host computer processed the data， and displayed it on the upper computer interface. According to the existing data， the front leg climbing process was dynamically drawn on the PC interface through the VC++6.0 drawing function. The experiment shows that this control method can control the movement of the front leg according to the planned trajectory.

Key words stair climbing wheelchair; path planning; fuzzy control; data display; dynamic drawing

0 引言

近來(lái)，科技的發(fā)展日新月異并逐漸接近普通人的生活，因?yàn)榭萍急揪蛻?yīng)該為人服務(wù)。尤其是在醫(yī)療方面，科技的應(yīng)用十分廣泛。本項(xiàng)目就是基于傷殘人士和老年人口的逐年增長(zhǎng)而提出并申請(qǐng)的。目前，對(duì)于爬樓輪椅的研究非常多，按照其工作原理和實(shí)現(xiàn)方式，可分為輪足式、行星輪式和履帶式3種爬樓輪椅裝置。本項(xiàng)目綜合各種方式爬樓輪椅的優(yōu)缺點(diǎn)，并根據(jù)自己的一些想法，提出一種可重構(gòu)八足輪腿式輪椅車，它可以實(shí)現(xiàn)平地行走、上下樓梯、上下坡和過(guò)溝等功能。而實(shí)現(xiàn)這一系列功能的重要保障就是前腿機(jī)構(gòu)。前腿機(jī)構(gòu)安置在輪椅前端的左右兩側(cè)，起到支撐和提供動(dòng)力的作用，在輪椅工作過(guò)程中，通過(guò)不斷調(diào)整前腿的位姿，以實(shí)現(xiàn)輪椅越障功能。而如何根據(jù)輪椅的工作要求控制前腿位姿變化是我們研究的重點(diǎn)。因此，本文首先對(duì)前腿的運(yùn)動(dòng)進(jìn)行軌跡規(guī)劃，然后根據(jù)前腿控制系統(tǒng)的構(gòu)成及其各部分的參數(shù)設(shè)計(jì)模糊控制算法及控制程序控制前腿按規(guī)劃的軌跡運(yùn)動(dòng)，最后通過(guò)試驗(yàn)，分析控制效果。

1 前腿軌跡規(guī)劃

1.1 前腿末端位置方程

近似的把前腿看成機(jī)械手，求機(jī)械手末端位置方程的方法很多，如D-H表示法，由于前腿機(jī)構(gòu)運(yùn)動(dòng)副較少，可以直接對(duì)前腿求末端位置方程。

前腿機(jī)構(gòu)可以簡(jiǎn)化為如圖1所示的連桿機(jī)構(gòu)，在C點(diǎn)建立動(dòng)坐標(biāo)系[X′O′Y′]，其中連桿CE和連桿CD是主動(dòng)桿，連桿EG為從動(dòng)桿，電機(jī)通過(guò)驅(qū)動(dòng)連桿CD使滑塊運(yùn)動(dòng)進(jìn)而使θ2發(fā)生變化從而調(diào)節(jié)連桿EG的位姿，電機(jī)還可通過(guò)驅(qū)動(dòng)連桿CE運(yùn)動(dòng)來(lái)改變?chǔ)?從而直接調(diào)節(jié)連桿EG的位姿，通過(guò)連桿CD與連桿CE的協(xié)調(diào)工作即可實(shí)現(xiàn)前腿的位姿調(diào)整，即通過(guò)改變?chǔ)?、θ3即可使θ2發(fā)生變化從而調(diào)節(jié)前腿位姿。根據(jù)封閉矢量法可知

式中：θ1為連桿CE與[Y′]軸反向夾角;θ2為連桿EG與[X′]軸正向夾角;θ3為連桿CD與[Y′]軸反向夾角。解得

則G點(diǎn)在動(dòng)坐標(biāo)系[X′O′Y′]中的位置方程為

在樓梯O點(diǎn)處建立靜坐標(biāo)系[XOY]，設(shè)C點(diǎn)在靜坐標(biāo)系XOY中的坐標(biāo)為（XC，YC），動(dòng)坐標(biāo)系[X′O′Y′]相對(duì)于靜坐標(biāo)系的位置可用平移矩陣T表示，則G點(diǎn)在靜坐標(biāo)系[XOY]中的位置用矩陣表示為

1.2 前腿工作空間

前腿機(jī)構(gòu)主要進(jìn)行輔助輪椅爬樓的工作，其結(jié)構(gòu)特性決定其工作范圍，即前腿的活動(dòng)范圍是由其關(guān)節(jié)變量的活動(dòng)范圍決定的，而前腿末端位置的最大活動(dòng)范圍就是其工作空間。

由前腿的機(jī)械結(jié)構(gòu)可知θ1的范圍為0° ～ 80°，又可知d的范圍為20 ～ 140 mm，則根據(jù)三角關(guān)系可知θ3-θ1的范圍是2° ～ 60°，又由前腿機(jī)械結(jié)構(gòu)的限制可知θ3的范圍為0° ～ 90°，所以可知θ3的范圍受到θ1的限制，由此，對(duì)θ1的范圍分2種情況討論：

1）當(dāng)[θ1∈0°，30°]時(shí)，[θ3∈θ1+2°，θ1+60°];

2）當(dāng)[θ1∈30°，80°]時(shí)，[θ3∈θ1+2°，90°]。

根據(jù)上述2種情況，結(jié)合公式（3）可知θ2的范圍，并根據(jù)前面得到的前腿末端位置方程，通過(guò)MATLAB工具，可知前腿末端位置的工作空間，如圖2所示，圖中坐標(biāo)原點(diǎn)為圖1中的動(dòng)坐標(biāo)系原點(diǎn)O′。得到前腿的工作空間可以為前腿末端軌跡規(guī)劃提供參考。

1.3 求位置方程逆解

前面已知前腿末端位置方程，根據(jù)爬樓輪椅爬樓需要及前腿工作空間要求，規(guī)劃爬樓過(guò)程中前腿末端運(yùn)動(dòng)軌跡的關(guān)節(jié)點(diǎn)，已知關(guān)節(jié)點(diǎn)對(duì)應(yīng)的前腿C點(diǎn)位置，通過(guò)求位置方程逆解的方法，求出前腿對(duì)應(yīng)關(guān)節(jié)點(diǎn)處的角度θ1、θ2，通過(guò)θ1、θ2的值可以唯一缺定θ3。已知初始階段時(shí)C點(diǎn)的坐標(biāo)（-320，176），末端G點(diǎn)坐標(biāo)（-170，50），代入末端位置方程可得θ1=0°，θ2=0°。初始階段結(jié)束時(shí)C點(diǎn)的坐標(biāo)（-191，176），末端G點(diǎn)坐標(biāo)（0，200），且根據(jù)前腿工作空間，且依據(jù)位姿調(diào)整階段時(shí)間應(yīng)盡量短的原則（因前腿桿1與桿2運(yùn)行速度相近，因此桿1與桿2變化角度相差最小時(shí)為位姿調(diào)整階段時(shí)間最短的情況），通過(guò)最小二乘法選擇θ1=46°，代入方程可得θ2=48°。在工作階段，根據(jù)輪椅整機(jī)運(yùn)動(dòng)要求，可知YC應(yīng)提高的最低高度，并可知前腿桿l3應(yīng)變化的最小角度，由此可得工作階段前腿各桿的變化。同理可得其它階段的控制量如表一所示。觀察[（X′G，Y′G）]在各階段的坐標(biāo)值并與前腿工作空間圖進(jìn)行對(duì)比，發(fā)現(xiàn)各點(diǎn)均在工作空間內(nèi)。

2 前腿控制算法

2.1 前腿控制系統(tǒng)結(jié)構(gòu)

如圖3所示，前腿控制系統(tǒng)主要由控制器部分、電機(jī)驅(qū)動(dòng)及離合器部分、機(jī)械傳動(dòng)部分和測(cè)量單元組成。

2.2 驅(qū)動(dòng)環(huán)節(jié)與機(jī)械傳動(dòng)部分

爬樓輪椅前腿采用欠驅(qū)動(dòng)方式，即獨(dú)立控制變量個(gè)數(shù)小于系統(tǒng)自由度個(gè)數(shù)，對(duì)于前腿來(lái)說(shuō)就是1個(gè)驅(qū)動(dòng)，2個(gè)自由度。本設(shè)計(jì)采用壓嵌式離合器DLY0，前腿共安裝有4個(gè)壓嵌式離合器，離合器1和離合器2控制前腿桿l1 、l2是否轉(zhuǎn)動(dòng)，離合器3和離合器4分別控制前腿桿l1 、l2正轉(zhuǎn)和反轉(zhuǎn)。如圖3所示，驅(qū)動(dòng)電機(jī)持續(xù)工作，帶動(dòng)齒輪減速器運(yùn)動(dòng)，齒輪減速器末齒輪與離合器主動(dòng)部分同軸，因此帶動(dòng)離合器主動(dòng)部分轉(zhuǎn)動(dòng)，當(dāng)離合器吸合時(shí)，離合器主動(dòng)部分與從動(dòng)部分吸合，帶動(dòng)傳動(dòng)輪系轉(zhuǎn)動(dòng)以使前腿運(yùn)動(dòng);當(dāng)離合器斷開(kāi)時(shí)，離合器主動(dòng)部分與從動(dòng)部分分開(kāi)，前腿停止運(yùn)動(dòng)。所以通過(guò)協(xié)調(diào)控制離合器工作狀態(tài)就能控制前腿的運(yùn)動(dòng)狀態(tài)，在已知電機(jī)轉(zhuǎn)速與傳動(dòng)部分傳動(dòng)比的情況下，控制離合器工作時(shí)間就能控制前腿桿的運(yùn)動(dòng)角度。

機(jī)械傳動(dòng)部分由齒輪傳動(dòng)、絲杠螺母?jìng)鲃?dòng)和齒輪齒條傳動(dòng)組成。在這部分中，已知電機(jī)主軸轉(zhuǎn)速，欲求前腿桿l1和桿l2的轉(zhuǎn)速，可通過(guò)齒輪輪系關(guān)系求得主軸和前腿桿的傳動(dòng)比。齒輪系為空間定軸輪系，根據(jù)空間定軸齒輪系傳動(dòng)比的計(jì)算公式可知

已知絲杠螺母?jìng)鲃?dòng)中絲杠的螺距為4 mm，即絲杠每轉(zhuǎn)一圈，螺母移動(dòng)4 mm，且知螺母移動(dòng)速度即是齒條移動(dòng)速度，而齒條移動(dòng)速度就是嚙合齒輪線速度，由此可知前腿桿的轉(zhuǎn)速。

2.3 測(cè)量環(huán)節(jié)

角度傳感器放置在桿l3上，用來(lái)測(cè)量桿與垂直方向的角度α，距離傳感器放置在桿l3末端，用來(lái)測(cè)量滑塊與桿末端的距離d。由圖1可知θ1、θ3與角度α、距離d的關(guān)系

2.4 模糊控制器設(shè)計(jì)

為了穩(wěn)定準(zhǔn)確的實(shí)現(xiàn)規(guī)劃的軌跡，對(duì)前腿的控制分為2個(gè)階段完成，第1階段是對(duì)比傳感器測(cè)量數(shù)據(jù)與目標(biāo)數(shù)據(jù)，進(jìn)而控制離合器的工作狀態(tài)，控制前腿向目標(biāo)位置運(yùn)動(dòng)，此階段為普通離合器控制;當(dāng)當(dāng)前角度與目標(biāo)角度的差值在一定范圍（稱為閥值）時(shí)，開(kāi)始第2階段，即啟動(dòng)模糊控制，對(duì)前腿進(jìn)行精確控制，考慮到傳感器測(cè)量數(shù)據(jù)與目標(biāo)數(shù)據(jù)對(duì)比的時(shí)間間隔及電機(jī)轉(zhuǎn)速，模糊控制與普通控制的閥值為[-10，10]，即根據(jù)誤差信號(hào)是否達(dá)到閥值，自適應(yīng)的在普通離合器控制與模糊控制之間切換。

模糊控制器的設(shè)計(jì)主要包括以下幾部分：

1）對(duì)輸入量和輸出量進(jìn)行模糊化

由上述可知，定義目標(biāo)角度值為θ目，實(shí)際測(cè)得的當(dāng)前角度值為θ當(dāng)，選擇角度偏差E=Δθ=θ目-θ當(dāng)為輸入量，為了將控制規(guī)則中角度偏差E對(duì)應(yīng)的語(yǔ)言變量表示成fuzzy集，把它分成7個(gè)模糊子集：{NB，NM，NS，ZE，PS，PM，PB}，分別表示：負(fù)大，負(fù)中，負(fù)小，零，正小，正中，正大。已知，模糊控制與普通控制間的閥值[-10，10]，即為角度偏差的基本論域，為保證控制精度，將E分為13個(gè)模糊檔，即[-6，+6]。則可知量化因子aE=1.67。輸出量U的基本論域?yàn)閇-1，1]，語(yǔ)言變量?。簕NB，NM，NS，ZE，PS，PM，PB}，分為13個(gè)模糊檔，則可知比例因子kU=0.17。模糊量E′與U′的隸屬函數(shù)表如表2、表3所示。

2）設(shè)計(jì)模糊規(guī)則，即設(shè)計(jì)模糊控制算法

設(shè)計(jì)1組模糊控制規(guī)則，以實(shí)現(xiàn)模糊推理。模糊控制規(guī)則設(shè)計(jì)的基本思想如下：

①若角度偏差為正且偏差較大，則控制量應(yīng)增大。

②若角度偏差為正且偏差較小，則控制量應(yīng)減小。

③若角度偏差為負(fù)且偏差較大，則控制量應(yīng)增大。

④若角度偏差為負(fù)且偏差較小，則控制量應(yīng)減小。

通常情況下模糊控制器一旦完成，語(yǔ)言規(guī)則與模糊推理是不可調(diào)整的，所以在設(shè)計(jì)模糊控制規(guī)則時(shí)要結(jié)合操作經(jīng)驗(yàn)，并不斷對(duì)控制規(guī)則進(jìn)行調(diào)整，直到得到滿意的控制結(jié)果。根據(jù)上面的設(shè)計(jì)思想并結(jié)合試驗(yàn)得到如表4所示的模糊控制規(guī)則表。

3）輸出量的反模糊化

根據(jù)實(shí)際的控制要求，在模糊控制時(shí)對(duì)精確控制量劃分為7個(gè)檔，分別是-0.8 s，-0.4 s，-0.1 s，0 s，0.1 s，0.4 s，0.8 s。其中正負(fù)號(hào)代表前腿桿的正反轉(zhuǎn)，數(shù)值代表離合器吸合時(shí)間，分別對(duì)應(yīng)模糊控制量的7個(gè)模糊子集。

本次設(shè)計(jì)控制程序主要在上位PC機(jī)中實(shí)現(xiàn)，上位PC機(jī)與單片機(jī)之間采用串口通信。圖4為控制系統(tǒng)的程序流程圖，其具體的控制步驟如下：

①查表一可知各階段的控制量θ1，θ3的值，向單片機(jī)發(fā)送地址指令，單片機(jī)接收到指令立即向上位PC機(jī)發(fā)送傳感器測(cè)量的數(shù)據(jù)，根據(jù)公式（7）、（8）可得當(dāng)前的θ1，θ3的值，由此得到角度偏差E;

②判斷角度偏差E是否達(dá)到模糊控制的閥值，如果沒(méi)有達(dá)到閥值，則按普通控制方法向單片機(jī)發(fā)送離合器控制指令;

③如果角度偏差E達(dá)到模糊控制閥值，則判斷當(dāng)前角度是否達(dá)到目標(biāo)角度，如果沒(méi)有達(dá)到，則啟用模糊控制，查詢模糊規(guī)則表，向單片機(jī)發(fā)送控制指令;

④如果當(dāng)前角度達(dá)到目標(biāo)角度，則判斷是否還有新的目標(biāo)值，如果有，則重復(fù)以上過(guò)程，否則關(guān)閉串口。

3 上位機(jī)顯示驗(yàn)證

在VC++6.0環(huán)境下，利用MFC模塊進(jìn)行上位機(jī)系統(tǒng)設(shè)計(jì)，其主要包括界面設(shè)計(jì)部分、WinAPI通信部分、控制程序部分和實(shí)時(shí)繪圖部分。其中上位機(jī)與單片機(jī)之間采用串口通信，通過(guò)WinAPI方式編程實(shí)現(xiàn);控制程序部分按上述控制器設(shè)計(jì)的原理來(lái)實(shí)現(xiàn);實(shí)時(shí)繪圖部分可以觀察控制過(guò)程中前腿的位姿變化，并能實(shí)現(xiàn)前腿爬樓過(guò)程的動(dòng)態(tài)仿真。

3.1 試驗(yàn)結(jié)果

在爬樓輪椅前腿界面下，分框?qū)崟r(shí)顯示4個(gè)傳感器的檢測(cè)數(shù)據(jù)，在前腿爬樓過(guò)程中，前腿位姿不斷變化，傳感器檢測(cè)的數(shù)據(jù)也不斷發(fā)生變化，數(shù)據(jù)實(shí)時(shí)在前腿界面顯示，并繪制實(shí)時(shí)動(dòng)態(tài)的前腿位姿圖。前腿機(jī)構(gòu)數(shù)據(jù)顯示及繪圖界面如圖5所示。圖5中a）表示爬樓初始階段結(jié)束時(shí)的狀態(tài)，b）表示爬第2個(gè)臺(tái)階前的準(zhǔn)備階段的某一瞬間的狀態(tài)。圖6為有無(wú)模糊控制前腿角度變化對(duì)比圖，通過(guò)對(duì)比可知模糊控制效果更佳。

4 結(jié)論

本文的目的是控制爬樓輪椅的前腿按規(guī)劃的軌跡運(yùn)動(dòng)，根據(jù)輪椅爬樓的實(shí)際情況及前腿機(jī)構(gòu)的結(jié)構(gòu)限制規(guī)劃出前腿運(yùn)動(dòng)軌跡，然后設(shè)計(jì)模糊控制方法，控制前腿運(yùn)動(dòng)，最后設(shè)計(jì)爬樓輪椅前腿上位機(jī)界面，并在上位機(jī)與單片機(jī)之間進(jìn)行串口通信，把傳感器測(cè)量的數(shù)據(jù)在上位機(jī)上顯示，并通過(guò)繪圖模塊對(duì)前腿爬樓過(guò)程動(dòng)態(tài)仿真。

結(jié)果表明，本控制方法可以實(shí)現(xiàn)預(yù)定軌跡，并具有速度快，控制穩(wěn)定的優(yōu)點(diǎn)。

參考文獻(xiàn)：

[1]? ? 劉劍飛，韓長(zhǎng)宇. 基于模糊自適應(yīng)PID算法的三軸穩(wěn)定器研究[J]. 河北工業(yè)大學(xué)學(xué)報(bào)，2017，46（3）：13-17.

[2]? ? 蔡琴. 全方位移動(dòng)避障實(shí)時(shí)軌跡控制算法的研究[J]. 四川大學(xué)學(xué)報(bào)，2017，54（1）：76-80.

[3]? ? 楊小菊，張偉，高宏偉，等. 基于模糊控制的移動(dòng)機(jī)器人避障研究[J]. 傳感器與微系統(tǒng)，2017，36（3）：51-54.

[4]? ? 田敏，鄭瑤，李江全. Visual C++數(shù)據(jù)采集與串口通信測(cè)控應(yīng)用實(shí)戰(zhàn)[M]. 北京：人民郵電出版社，2010.

[5]? ? 鄒箏，康曉林，袁建洲. Visual C++ 6. 0實(shí)用教程[M]. 北京：電子工業(yè)出版社，2008

[6]? ? 馮正，韓焱，王黎明. 多線程串口通信技術(shù)在GPS導(dǎo)航中的應(yīng)用[J]. 現(xiàn)代電子技術(shù)，2009，32（5）：28-30.

[7]? ? 王中訓(xùn)，徐超，王德法. 基于VC++6. 0的多串口通信方法[J]. 計(jì)算機(jī)應(yīng)用，2008，28（增刊）：254-256.

[8]? ? 段軍棋，蔣丹. 遠(yuǎn)程視頻監(jiān)控系統(tǒng)的設(shè)計(jì)與實(shí)現(xiàn)[J]. 電子科技大學(xué)學(xué)報(bào)，2002，31（5）：523-528.

[9]? ? 劉書(shū)智. Visual C++串口通信與工程應(yīng)用實(shí)踐[M]. 北京：中國(guó)鐵道出版社，2011.

[10]? 陳家俊，鄭滔. 程序設(shè)計(jì)教程用C++語(yǔ)言編程[M]. 3版. 北京：機(jī)械工業(yè)出版社，2015.

[責(zé)任編輯 楊 屹]