爬樓輪椅前腿的軌跡規劃及控制策略研究

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

摘要 爬樓輪椅的爬樓控制是研究的重點，而前腿是實現爬樓的重要輔助機構，為了準確穩定地控制前腿的運動，文章建立了前腿的末端位置方程，并計算其工作空間，在工作空間內對其進行軌跡規劃并求末端位置方程的逆解，得到前腿的控制量。然后設計前腿控制系統，計算系統各部分的參數，設計一種模糊控制算法控制前腿按規劃的軌跡運動。把前腿的運行數據通過傳感器測量下來并通過串口通信傳遞到上位PC機，上位機對數據進行處理，分框顯示在上位機界面上，并根據已有數據，通過VC++6.0的繪圖功能，把前腿爬樓的過程動態的繪制在上位機界面上。通過實驗可知，本控制方法可以控制前腿按規劃的軌跡運動。

關 鍵 詞 爬樓輪椅;軌跡規劃;模糊控制;上位機顯示;動態繪圖

中圖分類號 TP311? ? ?文獻標志碼 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 引言

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

1 前腿軌跡規劃

1.1 前腿末端位置方程

近似的把前腿看成機械手，求機械手末端位置方程的方法很多，如D-H表示法，由于前腿機構運動副較少，可以直接對前腿求末端位置方程。

前腿機構可以簡化為如圖1所示的連桿機構，在C點建立動坐標系[X′O′Y′]，其中連桿CE和連桿CD是主動桿，連桿EG為從動桿，電機通過驅動連桿CD使滑塊運動進而使θ2發生變化從而調節連桿EG的位姿，電機還可通過驅動連桿CE運動來改變θ2從而直接調節連桿EG的位姿，通過連桿CD與連桿CE的協調工作即可實現前腿的位姿調整，即通過改變θ1、θ3即可使θ2發生變化從而調節前腿位姿。根據封閉矢量法可知

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

則G點在動坐標系[X′O′Y′]中的位置方程為

在樓梯O點處建立靜坐標系[XOY]，設C點在靜坐標系XOY中的坐標為（XC，YC），動坐標系[X′O′Y′]相對于靜坐標系的位置可用平移矩陣T表示，則G點在靜坐標系[XOY]中的位置用矩陣表示為

1.2 前腿工作空間

前腿機構主要進行輔助輪椅爬樓的工作，其結構特性決定其工作范圍，即前腿的活動范圍是由其關節變量的活動范圍決定的，而前腿末端位置的最大活動范圍就是其工作空間。

由前腿的機械結構可知θ1的范圍為0° ～ 80°，又可知d的范圍為20 ～ 140 mm，則根據三角關系可知θ3-θ1的范圍是2° ～ 60°，又由前腿機械結構的限制可知θ3的范圍為0° ～ 90°，所以可知θ3的范圍受到θ1的限制，由此，對θ1的范圍分2種情況討論：

1）當[θ1∈0°，30°]時，[θ3∈θ1+2°，θ1+60°];

2）當[θ1∈30°，80°]時，[θ3∈θ1+2°，90°]。

根據上述2種情況，結合公式（3）可知θ2的范圍，并根據前面得到的前腿末端位置方程，通過MATLAB工具，可知前腿末端位置的工作空間，如圖2所示，圖中坐標原點為圖1中的動坐標系原點O′。得到前腿的工作空間可以為前腿末端軌跡規劃提供參考。

1.3 求位置方程逆解

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

2 前腿控制算法

2.1 前腿控制系統結構

如圖3所示，前腿控制系統主要由控制器部分、電機驅動及離合器部分、機械傳動部分和測量單元組成。

2.2 驅動環節與機械傳動部分

爬樓輪椅前腿采用欠驅動方式，即獨立控制變量個數小于系統自由度個數，對于前腿來說就是1個驅動，2個自由度。本設計采用壓嵌式離合器DLY0，前腿共安裝有4個壓嵌式離合器，離合器1和離合器2控制前腿桿l1 、l2是否轉動，離合器3和離合器4分別控制前腿桿l1 、l2正轉和反轉。如圖3所示，驅動電機持續工作，帶動齒輪減速器運動，齒輪減速器末齒輪與離合器主動部分同軸，因此帶動離合器主動部分轉動，當離合器吸合時，離合器主動部分與從動部分吸合，帶動傳動輪系轉動以使前腿運動;當離合器斷開時，離合器主動部分與從動部分分開，前腿停止運動。所以通過協調控制離合器工作狀態就能控制前腿的運動狀態，在已知電機轉速與傳動部分傳動比的情況下，控制離合器工作時間就能控制前腿桿的運動角度。

機械傳動部分由齒輪傳動、絲杠螺母傳動和齒輪齒條傳動組成。在這部分中，已知電機主軸轉速，欲求前腿桿l1和桿l2的轉速，可通過齒輪輪系關系求得主軸和前腿桿的傳動比。齒輪系為空間定軸輪系，根據空間定軸齒輪系傳動比的計算公式可知

已知絲杠螺母傳動中絲杠的螺距為4 mm，即絲杠每轉一圈，螺母移動4 mm，且知螺母移動速度即是齒條移動速度，而齒條移動速度就是嚙合齒輪線速度，由此可知前腿桿的轉速。

2.3 測量環節

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

2.4 模糊控制器設計

為了穩定準確的實現規劃的軌跡，對前腿的控制分為2個階段完成，第1階段是對比傳感器測量數據與目標數據，進而控制離合器的工作狀態，控制前腿向目標位置運動，此階段為普通離合器控制;當當前角度與目標角度的差值在一定范圍（稱為閥值）時，開始第2階段，即啟動模糊控制，對前腿進行精確控制，考慮到傳感器測量數據與目標數據對比的時間間隔及電機轉速，模糊控制與普通控制的閥值為[-10，10]，即根據誤差信號是否達到閥值，自適應的在普通離合器控制與模糊控制之間切換。

模糊控制器的設計主要包括以下幾部分：

1）對輸入量和輸出量進行模糊化

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

2）設計模糊規則，即設計模糊控制算法

設計1組模糊控制規則，以實現模糊推理。模糊控制規則設計的基本思想如下：

①若角度偏差為正且偏差較大，則控制量應增大。

②若角度偏差為正且偏差較小，則控制量應減小。

③若角度偏差為負且偏差較大，則控制量應增大。

④若角度偏差為負且偏差較小，則控制量應減小。

通常情況下模糊控制器一旦完成，語言規則與模糊推理是不可調整的，所以在設計模糊控制規則時要結合操作經驗，并不斷對控制規則進行調整，直到得到滿意的控制結果。根據上面的設計思想并結合試驗得到如表4所示的模糊控制規則表。

3）輸出量的反模糊化

根據實際的控制要求，在模糊控制時對精確控制量劃分為7個檔，分別是-0.8 s，-0.4 s，-0.1 s，0 s，0.1 s，0.4 s，0.8 s。其中正負號代表前腿桿的正反轉，數值代表離合器吸合時間，分別對應模糊控制量的7個模糊子集。

本次設計控制程序主要在上位PC機中實現，上位PC機與單片機之間采用串口通信。圖4為控制系統的程序流程圖，其具體的控制步驟如下：

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

②判斷角度偏差E是否達到模糊控制的閥值，如果沒有達到閥值，則按普通控制方法向單片機發送離合器控制指令;

③如果角度偏差E達到模糊控制閥值，則判斷當前角度是否達到目標角度，如果沒有達到，則啟用模糊控制，查詢模糊規則表，向單片機發送控制指令;

④如果當前角度達到目標角度，則判斷是否還有新的目標值，如果有，則重復以上過程，否則關閉串口。

3 上位機顯示驗證

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

3.1 試驗結果

在爬樓輪椅前腿界面下，分框實時顯示4個傳感器的檢測數據，在前腿爬樓過程中，前腿位姿不斷變化，傳感器檢測的數據也不斷發生變化，數據實時在前腿界面顯示，并繪制實時動態的前腿位姿圖。前腿機構數據顯示及繪圖界面如圖5所示。圖5中a）表示爬樓初始階段結束時的狀態，b）表示爬第2個臺階前的準備階段的某一瞬間的狀態。圖6為有無模糊控制前腿角度變化對比圖，通過對比可知模糊控制效果更佳。

4 結論

本文的目的是控制爬樓輪椅的前腿按規劃的軌跡運動，根據輪椅爬樓的實際情況及前腿機構的結構限制規劃出前腿運動軌跡，然后設計模糊控制方法，控制前腿運動，最后設計爬樓輪椅前腿上位機界面，并在上位機與單片機之間進行串口通信，把傳感器測量的數據在上位機上顯示，并通過繪圖模塊對前腿爬樓過程動態仿真。

結果表明，本控制方法可以實現預定軌跡，并具有速度快，控制穩定的優點。

參考文獻：

[1]? ? 劉劍飛，韓長宇. 基于模糊自適應PID算法的三軸穩定器研究[J]. 河北工業大學學報，2017，46（3）：13-17.

[2]? ? 蔡琴. 全方位移動避障實時軌跡控制算法的研究[J]. 四川大學學報，2017，54（1）：76-80.

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

[4]? ? 田敏，鄭瑤，李江全. Visual C++數據采集與串口通信測控應用實戰[M]. 北京：人民郵電出版社，2010.

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

[6]? ? 馮正，韓焱，王黎明. 多線程串口通信技術在GPS導航中的應用[J]. 現代電子技術，2009，32（5）：28-30.

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

[8]? ? 段軍棋，蔣丹. 遠程視頻監控系統的設計與實現[J]. 電子科技大學學報，2002，31（5）：523-528.

[9]? ? 劉書智. Visual C++串口通信與工程應用實踐[M]. 北京：中國鐵道出版社，2011.

[10]? 陳家俊，鄭滔. 程序設計教程用C++語言編程[M]. 3版. 北京：機械工業出版社，2015.

[責任編輯 楊 屹]