劉佩佩,周亞麗,張奇志,徐鑫鑫
(1.北京信息科技大學(xué)自動化學(xué)院,北京 100192;2.北京沃普拉斯科技有限公司,北京 100086)
平衡控制在航空航天、機(jī)器人直立控制、科研教學(xué)、控制理論研究及驗證等方面有著廣泛應(yīng)用[1]。與無人駕駛汽車相比,無人駕駛自行車具有靜態(tài)不穩(wěn)定性和動態(tài)可穩(wěn)定性,因此對自行車的運動特性和平衡控制的研究已經(jīng)成為一個重要的研究領(lǐng)域[2]。1899年Whipple[3]首次運用數(shù)學(xué)方程描述自行車的運動行為,他將自行車簡化為前輪、后輪、車架和車體前叉四部分,應(yīng)用倒立擺模型導(dǎo)出了自行車圍繞直立垂直平衡點的線性化運動方程,并提出了自穩(wěn)定的概念。隨著計算機(jī)時代的到來,越來越多的研究者借用計算機(jī)仿真等工具對復(fù)雜問題進(jìn)行仿真和分析,由此開始了對無人駕駛自行車的深入研究。從國內(nèi)外研究現(xiàn)狀可以看出,學(xué)者們對無人駕駛自行車系統(tǒng)的研究主要集中在各種形式的配重調(diào)節(jié)器設(shè)計、車體結(jié)構(gòu)分析和運動過程中車身平衡穩(wěn)定性控制等相關(guān)方面,并取得了豐碩的成果。例如,一些學(xué)者[4-8]著重研究了增加配重或改變傳統(tǒng)自行車原有結(jié)構(gòu)對無人駕駛自行車平衡運動的影響,結(jié)果表明,增加配重調(diào)節(jié)器或者改變自行車原有結(jié)構(gòu)可以擴(kuò)大系統(tǒng)傾角的調(diào)整范圍。而另有學(xué)者[9-13]則認(rèn)為無人駕駛自行車無需添加配重調(diào)節(jié)裝置,僅靠車把和車輪運動即可保持穩(wěn)定平衡。此種方案驅(qū)動自由度較少,控制容易,但能否達(dá)到前者的傾角調(diào)整范圍和效果還需進(jìn)一步驗證[14]。
本文將以上兩種主流思路進(jìn)行結(jié)合,各取所長,設(shè)計開發(fā)了一種由曲柄滑塊機(jī)構(gòu)控制的無人駕駛自行車系統(tǒng)。通過曲柄滑塊機(jī)構(gòu)控制車體的運動方向,同時配合配重塊調(diào)節(jié)車體重心,最終實現(xiàn)直立平衡運動。本文所設(shè)計的無人駕駛自行車實驗平臺不僅有利于培養(yǎng)學(xué)生解決復(fù)雜工程問題能力,更具有廣闊的實際應(yīng)用價值。
本文設(shè)計一種由滾珠絲杠滑臺、曲柄和連桿組成的曲柄滑塊機(jī)構(gòu)來控制車體的運動方向。圖1所示,滾珠絲桿滑臺由螺桿、螺母、滾珠和滑塊組成,是將回轉(zhuǎn)運動轉(zhuǎn)化為直線運動,或?qū)⒅本€運動轉(zhuǎn)化為回轉(zhuǎn)運動的理想產(chǎn)品。由于摩擦阻力很小,同時具有慢速無爬行、高速無顫動的特點,滾珠絲杠被廣泛應(yīng)用于各種工業(yè)設(shè)備和精密儀器。可在高負(fù)載的情況下實現(xiàn)高精度的直線運動。
圖1 滾珠絲杠滑臺機(jī)構(gòu)圖
曲柄滑塊機(jī)構(gòu)模型簡圖如圖2所示。記曲柄OA的長為r,連桿AB的長為l,當(dāng)曲柄繞固定點O以角速度w旋轉(zhuǎn)時,由連桿帶動滑塊B在水平槽內(nèi)作往復(fù)直線運動,整個曲柄滑塊機(jī)構(gòu)通過滾珠絲杠實現(xiàn)進(jìn)給運動。
圖2 曲柄滑塊機(jī)構(gòu)模型簡圖
假設(shè)初始時刻曲柄的端點A位于水平線段OB上,曲柄從初始位置起轉(zhuǎn)動的角度為θ,連桿AB與OB的銳夾角為β(稱為擺角)。
取O點為坐標(biāo)原點,OB方向為x軸上的坐標(biāo)軸,用x表示滑塊的位移,即OB邊的長度。得到:
對模型進(jìn)行近似化:因為(1+ε)a=1+aε+…,|ε|<1。一般而言,r2/l2是遠(yuǎn)比1小的數(shù),則滑塊位移的近似模型為
圖3所示為無人駕駛自行車系統(tǒng)三維模型圖。整個系統(tǒng)可簡化為由前輪、后輪、曲柄滑塊機(jī)構(gòu)以及車身框架組成。在當(dāng)前研究階段,配重塊調(diào)整到一個合適的位置之后將其固定,可看成車體的一部分。曲柄滑塊機(jī)構(gòu)裝配在車體橫梁和前車把轉(zhuǎn)軸上,通過伺服電動機(jī)驅(qū)動滑塊在滑臺上的移動控制自行車車把轉(zhuǎn)動;附加配重塊調(diào)節(jié)機(jī)構(gòu)及控制電路安裝在后車座上,可通過配重調(diào)節(jié)裝置控制自行車重心平衡。
圖3 系統(tǒng)三維模型圖
圖4所示為無人駕駛自行車系統(tǒng)結(jié)構(gòu)模型圖。自行車的前后輪與地面的接觸點Q1Q2形成一條接觸線,接觸線與x軸的夾角為俯仰角Ψ,逆時針方向為正方向。W為自行車的輪間距,假設(shè)系統(tǒng)所有質(zhì)量集中在質(zhì)心上,定義質(zhì)心高度為h,質(zhì)心在水平面上的投影點A與Q2的距離定為b。車身與z軸的夾角為車體滾轉(zhuǎn)角φ,順時針方向為正方向[15]。通過滑塊位置控制前輪轉(zhuǎn)過的角度定義為方向角δ,如圖5所示。定義車體滾轉(zhuǎn)角和前車把轉(zhuǎn)角均為零時的滑塊位置為起始位置;起始位置時曲柄滑塊機(jī)構(gòu)可看成是一個直角三角形,定義直角短邊為r(始終與前車把保持平行共面),直角長邊為l,這兩條邊的長度為定值、可測量;定義滑塊位置邊長度為x。
圖4 無人駕駛自行車系統(tǒng)結(jié)構(gòu)模型
圖5 無人駕駛自行車系統(tǒng)結(jié)構(gòu)俯視圖
由圖4 可知,δ=90°-θ,代入式(3),得:2
當(dāng)車體處于直立狀態(tài)時,式中各角度均為極小量,則cos δ≈1,sin δ≈δ。
將式(4)進(jìn)行線性化:
由式(5)得:
根據(jù)質(zhì)點系相對于動軸的動量矩定理,可得系統(tǒng)線性傾斜動力學(xué)模型[16]:
式中:J為系統(tǒng)相對于自行車輪間距的轉(zhuǎn)動慣量,F(xiàn)為系統(tǒng)所受牽引慣性力,
v為車體運動速度;R為車輪半徑。
將式(8)、(9)代入式(7),得到以轉(zhuǎn)向角δ為輸入的自行車動力學(xué)模型:
將式(6)代入式(10)得:
最終得到以曲柄滑塊位置x為輸入,以車體傾角φ為輸出的單輸入單輸出無人駕駛自行車動力學(xué)系統(tǒng)模型。
如圖6所示為無人駕駛自行車電控系統(tǒng)圖,主要包括:上位機(jī)(Intel NUC6i7KYK小型PC主機(jī));下位機(jī)(控制單元STM32);伺服驅(qū)動單元(3個伺服電動機(jī)及伺服驅(qū)動器);姿態(tài)傳感器(MPU-6050);OLED 顯示屏;48V@14AH鋰電池組。
圖6 無人駕駛自行車電控系統(tǒng)圖
小型PC主機(jī)型號為Intel NUC6i7KYK,采用第6代Intel i7-6770HQ 的CPU,2.6~2.5 GHz四核六兆緩存,機(jī)身輕薄,易于攜帶和安裝,可以滿足無人駕駛自行車系統(tǒng)正常運行所需。NUC6i7作為上位機(jī)是整個系統(tǒng)的控制算法和信息處理核心,主要作用是將下位機(jī)發(fā)送過來的姿態(tài)信息(俯仰角、滾轉(zhuǎn)角、偏航角、三軸陀螺的原始數(shù)據(jù))進(jìn)行幀協(xié)議解析,并將解析完的各電動機(jī)模塊控制信息發(fā)送給下位機(jī)。
下位機(jī)以STM32作為控制核心單元,通過隔離USB轉(zhuǎn)UART與上位機(jī)通信,通信速度1 Mb/s,物理線纜為一根USB2.0 A口線,直接插接到PC機(jī)USB口即可。通過I2C接口搭載MPU-6050姿態(tài)傳感器、通過RS-485接口與伺服驅(qū)動單元通信、通過SPI接口外接一個OLED顯示屏,可以動態(tài)顯示程序運行狀態(tài)和數(shù)據(jù)變量。
電控系統(tǒng)下位機(jī)的主要作用是接收上位機(jī)解析完成發(fā)送過來的各電動機(jī)模塊控制信息,同時不斷采集各電動機(jī)模塊的編碼器信息。
無人駕駛自行車系統(tǒng)驅(qū)動單元包括3個伺服電動機(jī)及伺服驅(qū)動器,分別是:控制自行車轉(zhuǎn)速的轉(zhuǎn)速電動機(jī)模塊、控制自行車重心的平衡電動機(jī)模塊、控制自行車車把的轉(zhuǎn)向電動機(jī)模塊。
平衡電動機(jī)和轉(zhuǎn)向電動機(jī)型號為雷賽ACM4005V24H-B5,電動機(jī)內(nèi)部集成有絕對式編碼器,工作在位置模式,額定功率為50 W,額定轉(zhuǎn)速為3 000 r/min,額定力矩為0.16 N·m;轉(zhuǎn)速電動機(jī)為車體自帶輪轂電動機(jī),工作在速度模式,在電動機(jī)外部安裝一個增量式編碼器。3個電動機(jī)配套自行設(shè)計的Su7020三相交流伺服驅(qū)動器。
由于3個伺服電動機(jī)模塊控制信號均通過RS-485總線通信,故需要對3個電動機(jī)模塊進(jìn)行編號,規(guī)定為:轉(zhuǎn)速電動機(jī)模塊ID=01,平衡電動機(jī)模塊ID=02,轉(zhuǎn)向電動機(jī)模塊ID=03。
本系統(tǒng)采用InvenSense公司的MPU-6050高性能的三軸加速度計、三軸陀螺儀芯片來確定姿態(tài)位置,結(jié)構(gòu)設(shè)計上要求控制核心板和系統(tǒng)平行安裝,獲取俯仰信息即為機(jī)構(gòu)的俯仰信息[17]。MPU-6050集成了3軸MEMS陀螺儀、3軸MEMS加速度計,以及一個可擴(kuò)展的數(shù)字運動處理器DMP(Digital Motion Processor),可用I2C接口連接一個第3方的數(shù)字傳感器,比如磁力計或壓力傳感器。MPU-6050對陀螺儀和加速度計分別用了3個16位的ADC,將其測量的模擬量轉(zhuǎn)化為可輸出的數(shù)字量。為了精確跟蹤快速和慢速的運動,傳感器的測量范圍都是用戶可控的,陀螺儀可測范圍為±250,±500,±1 000,±2 000°/s,加速度計可測范圍為±2,±4,±8,±16 g。本系統(tǒng)參數(shù)設(shè)定為:陀螺量程:±2 000°/s,加速度計量程:±16 g。
無人駕駛自行車系統(tǒng)姿態(tài)傳感器的功能為采集系統(tǒng)運動時的姿態(tài)信息,傳感器輸出信息主要包括:俯仰角、滾轉(zhuǎn)角、偏航角、三軸陀螺的原始數(shù)據(jù)(傳感器內(nèi)部16 bit ADC采集數(shù)據(jù))。
整個系統(tǒng)使用48V@14AH鋰電池組供電,伺服驅(qū)動模塊的供電為電池組電源48 V,并且通過急??刂乒虘B(tài)繼電器方式輸出,摁下急停后所有電機(jī)會掉電卸載。上位機(jī)供電電壓為19 V,所以需要DC48~19V模塊轉(zhuǎn)19 V輸出,并在電源模塊輸入前級設(shè)計按鈕開關(guān)。下位機(jī)電源使用24 V供電,與上位機(jī)電源并聯(lián),均使用19 V電源輸入,故整個系統(tǒng)同時上電、掉電。
系統(tǒng)界面如圖7所示,用于監(jiān)控系統(tǒng)的運行狀態(tài)。界面左邊分別為打開端口、關(guān)閉端口和顯示接收數(shù)據(jù),中間可顯示采集并解析完成的3個電動機(jī)的速度、位置等運動信息,界面右端可通過勾選“記錄數(shù)據(jù)”將系統(tǒng)運動的各項數(shù)據(jù)記錄下來并生成.text文件保存到指定路徑,以便后需進(jìn)行數(shù)據(jù)分析。
圖7 無人駕駛自行車系統(tǒng)操作界面
圖8、9分別為無人駕駛自行車系統(tǒng)軟件數(shù)據(jù)流程圖及雙閉環(huán)控制框圖。
圖8 系統(tǒng)軟件數(shù)據(jù)流程圖
圖9 無人駕駛自行車系統(tǒng)雙閉環(huán)控制框圖
當(dāng)系統(tǒng)啟動運行后,上位機(jī)根據(jù)MPU-6050姿態(tài)傳感器傳過來的實時姿態(tài)信息(俯仰角、滾轉(zhuǎn)角、偏航角以及三軸陀螺的原始數(shù)據(jù))進(jìn)行幀協(xié)議解析,解析后得到的各伺服電動機(jī)的速度和位置信息作為內(nèi)環(huán)的期望輸入值;伺服驅(qū)動模塊根據(jù)上位機(jī)發(fā)送過來的各電動機(jī)速度和位置信息,經(jīng)反饋閉環(huán)調(diào)節(jié),分別控制3個電動機(jī)去執(zhí)行相應(yīng)任務(wù)。同時不斷采集調(diào)整后的姿態(tài)信息判斷整個系統(tǒng)是否達(dá)到了平衡狀態(tài)。
圖10為曲柄滑塊機(jī)構(gòu)實體裝配圖,軸承選用進(jìn)口IKO魚眼接頭軸承,此曲柄滑塊機(jī)械結(jié)構(gòu)具有控制精確,力矩大和操作簡單的特性。圖11和圖12分別為樣機(jī)左視圖和俯視圖,配重調(diào)節(jié)裝置由滾珠絲杠滑臺和5個配重塊組成,每個配重塊0.98 kg。曲柄滑塊機(jī)構(gòu)和配重調(diào)節(jié)裝置的滑臺總長均為38 cm,絲杠螺距0.37
cm。系統(tǒng)的核心控制電路安裝在配重調(diào)節(jié)裝置后方的車架上,樣機(jī)總質(zhì)量為38.95 kg,無人駕駛自行車系統(tǒng)的相關(guān)物理參數(shù)為:曲柄連桿直角短邊r=102.8 mm,曲柄連桿直角長邊l=272.2 mm,滑臺絲杠螺距s=3.7 mm,自行車車輪半徑R=0.185 m,自行車前后輪間距W=0.94 m,質(zhì)心投影到后輪距離b=0.37 m,車體質(zhì)心高度h =0.5 m,車體前向速度v=0.8 m/s,重力加速度g=9.8 m/s2,系統(tǒng)總質(zhì)量m =38.95 kg。
圖10 曲柄滑塊機(jī)構(gòu)實體裝配圖
圖11 樣機(jī)左視圖
圖12 樣機(jī)俯視圖
圖13所示為樣機(jī)試驗部分視頻連拍圖,每張圖片的右下角為拍攝時間,為展示控制細(xì)節(jié),每隔1 s采集2幀畫面。
圖13 樣機(jī)試驗視頻連拍
首先在系統(tǒng)操作界面左邊點擊“端口設(shè)置”里的“打開”按鈕,等待界面顯示“成功打開自行車端口”之后,打開自行車樣機(jī)上的啟動按鈕,完成所有準(zhǔn)備工作之后將系統(tǒng)在無外力干預(yù)情況下自由釋放。
圖14所示為樣機(jī)試驗中測量得到的車體方向角δ和滾轉(zhuǎn)角φ隨時間變化的曲線。從圖14可以看出,將自行車由方向角為0.075 rad(4.27°)、滾轉(zhuǎn)角為0.02 rad(1.15°)的初始狀態(tài)自由釋放,在大約1 s時,車體方向角達(dá)到最大值0.54 rad(30°);自行車自主行走大約2 s后,曲柄滑塊控制著車把開始在小幅度范圍內(nèi)調(diào)整;大約3 s時,因路面稍有不平,系統(tǒng)出現(xiàn)微微震蕩,但0.2 s后迅速調(diào)整回穩(wěn)定狀態(tài),之后系統(tǒng)便維持動態(tài)平衡運行。
圖14 樣機(jī)試驗結(jié)果
樣機(jī)試驗結(jié)果表明,本文所設(shè)計的由曲柄滑塊機(jī)構(gòu)控制的無人駕駛自行車系統(tǒng)可以在水平路面上實現(xiàn)穩(wěn)定直線運動。
本文設(shè)計開發(fā)了一種由曲柄滑塊機(jī)構(gòu)控制的無人駕駛自行車系統(tǒng)。依次進(jìn)行了動力學(xué)建模、電控系統(tǒng)設(shè)計和軟件系統(tǒng)設(shè)計,采用雙閉環(huán)控制思想進(jìn)行了系統(tǒng)控制器的設(shè)計,并進(jìn)行了樣機(jī)試驗驗證。多次樣機(jī)試驗結(jié)果表明,曲柄滑塊機(jī)構(gòu)對前車把方向的控制精確、快速且操作簡便,采用傳統(tǒng)簡單的雙閉環(huán)控制算法即可實現(xiàn)試驗樣機(jī)在水平路面上的穩(wěn)定直線平衡運動。