專利名稱：平衡控制方法、平衡控制裝置及機器人的制作方法
技術領域：
本發(fā)明涉及機器人控制技術，尤其涉及用于維持兩輪機器人平衡的平衡控制方 法、平衡控制裝置及機器人。
背景技術：
兩輪非穩(wěn)定移動服務機器人具有體積小、重量輕、移動靈活等特點，可廣泛應用于 酒店、機場、會議中心等場所。目前，常見的兩輪直立式機器人如圖1所示，該機器人分移動 平臺(5、6、7、8)和任務機構(1、2、3、4)，此類機器人屬于固定質心系統(tǒng)，無法在負載變動時 對系統(tǒng)的控制參數(shù)進行調整，無法對系統(tǒng)的重心變動進行估計，并依據(jù)估計結果對系統(tǒng)的 控制參數(shù)進行必要的修正，只能在一定的系統(tǒng)質量下使系統(tǒng)維持平衡?，F(xiàn)有的針對兩輪直立式機器人本體的自平衡方法大多存在如下缺陷①地面摩擦 系數(shù)不易獲得，無法準確獲得車輪摩擦力，以根據(jù)該摩擦力維持機器人的平衡；②系統(tǒng)負載 變化時機器人的可控參數(shù)無法自主調整。因此，需要用于維持兩輪機器人平衡的平衡控制方法、平衡控制裝置及機器人，解 決在負載動態(tài)變化以及地面摩擦系數(shù)無法獲得的情況下不能維持動態(tài)平衡的問題。

發(fā)明內容
有鑒于此，本發(fā)明提供了一種用于維持兩輪機器人平衡的平衡控制方法、平衡控 制裝置及具有該平衡控制裝置的機器人，可以使機器人在負載動態(tài)變化及地面摩擦系數(shù)無 法獲得的情況下維持動態(tài)平衡。本發(fā)明提供了一種用于維持兩輪機器人平衡的平衡控制方法，包括步驟202，實 時測量兩輪機器人的車體的質量、兩輪機器人的每個車輪相對于車體的旋轉速度、以及車 體與豎直方向的傾斜角度和傾斜角速度，其中，該車體包括所述兩輪機器人的負載；步驟 204，根據(jù)車體的質量、傾斜角度、傾斜角速度和旋轉速度，對每個車輪的電樞電壓進行調 節(jié)，以使兩輪機器人維持平衡。在上述技術方案中，優(yōu)選地，步驟204包括通過預置的平衡關系來調節(jié)兩輪機器 人的每個車輪的輸出扭矩，并根據(jù)每個車輪的輸出扭矩來得到每個車輪的電樞電壓，該平 衡關系通過以下公式限定xm(M + 2m + 2J(0/R2) + (θcosΘ-Θ2 sin 0)ML = (Tr +T1)/R
OJp =M(g-Lesm0-L cos ΘΘ2 )L sin θ — [xmM + (θ cos Θ-Θ2 sin 0)ML]L cos O-(Tr^Tl)其中，M為車體的質量，m為每個車輪的質量，Jw為每個車輪相對于兩輪機器人的 輪軸的轉動慣量，Jp為車體相對于輪軸的轉動慣量，R為每個車輪的半徑，L為輪軸到車體
的重心的距離，通過車體的質量M結合車體的高度得到，θ為傾斜角度…為傾斜角速度，&
為傾斜角速度的一階微分，、為輪軸的中心相對于地面的速度、的一階微分，、通過旋轉速度結合傾斜角速度得到， ；為兩輪機器人的右車輪的輸出扭矩，T1為兩輪機器人的左車輪 的輸出扭矩。在上述技術方案中，優(yōu)選地，兩輪機器人接收指令，該步驟204還包括根據(jù)平衡 關系及指令，使用狀態(tài)反饋控制方法得到每個車輪的電樞電壓，其中，以傾斜角度θ、傾斜 角速度&輪軸的中心相對于地面的速度i為狀態(tài)變量，以每個車輪的電樞電壓為控制量。在上述技術方案中，優(yōu)選地，通過傾角儀測量傾斜角度，通過陀螺儀測量傾斜角速 度，通過增量碼盤測量旋轉速度，通過稱重儀測量車體的質量。通過上述技術方案，實時監(jiān)測機器人的負載變化、行走速度及傾斜變化，根據(jù)機 器人的這些現(xiàn)場情況來調節(jié)驅動機器人的車輪的電壓，以使機器人保持平衡，該方法考慮 到機器人的負載動態(tài)變化以及地面摩擦系數(shù)無法獲得的情況，使得機器人可以用于服務應 用，更加智能。本發(fā)明還提供了一種維持兩輪機器人平衡的平衡控制裝置，包括測量模塊，實時 測量兩輪機器人的車體的質量、兩輪機器人的每個車輪相對于車體的旋轉速度、以及車體 與豎直方向的傾斜角度和傾斜角速度，其中，車體包括兩輪機器人的負載；計算模塊，根據(jù) 車體的質量、傾斜角度、傾斜角速度和旋轉速度，對每個車輪的電樞電壓進行調節(jié)，以使兩 輪機器人維持平衡。在上述技術方案中，優(yōu)選地，該計算模塊通過預置的平衡關系來調節(jié)兩輪機器人 的每個車輪的輸出扭矩，并根據(jù)每個車輪的輸出扭矩來得到每個車輪的電樞電壓，該平衡 關系通過以下公式限定xm(M +2m + 2JmlR2) + (θcosΘ-Θ2 sin0)ML = (Tr +T1)/R
0JP =M(g-L0sin0-L cos θθ2)1 sin θ — [xmM + (θ cos Θ-Θ2 sin 0)ML]L cos 0-(Tr+Tt)其中，M為車體的質量，m為每個車輪的質量，Jw為每個車輪相對于兩輪機器人的 輪軸的轉動慣量，Jp為車體相對于輪軸的轉動慣量，R為每個車輪的半徑，L為輪軸到車體 的重心的距離，通過車體的質量M結合車體的高度得到，θ為傾斜角度，々為傾斜角速度，力 為傾斜角速度的一階微分，元為輪軸的中心相對于地面的速度、的一階微分，、通過旋轉速 度結合傾斜角速度得到， ；為兩輪機器人的右車輪的輸出扭矩，T1為兩輪機器人的左車輪 的輸出扭矩。在上述技術方案中，優(yōu)選地，還包括無線收發(fā)模塊，用于接收指令，該計算模塊 根據(jù)平衡關系和指令，使用狀態(tài)反饋控制方法得到每個車輪的電樞電壓，其中，以傾斜角度
θ、傾斜角速度々輪軸的中心相對于地面的速度4為狀態(tài)變量，以每個車輪的電樞電壓為 、 .
控制量。在上述技術方案中，優(yōu)選地，該計算模塊供用戶調整預置的平衡關系。在上述技術方案中，優(yōu)選地，該測量模塊通過傾角儀測量傾斜角度，通過陀螺儀測 量傾斜角速度，通過增量碼盤測量旋轉速度，通過稱重儀測量車體的質量。通過上述技術方案，實時監(jiān)測機器人的負載變化、行走速度及傾斜變化，根據(jù)機器 人的這些現(xiàn)場情況來調節(jié)驅動機器人的車輪的電壓，以使機器人保持平衡，該平衡控制裝 置考慮到機器人的負載動態(tài)變化以及地面摩擦系數(shù)無法獲得的情況，使得機器人可以用于服務應用，更加智能。本發(fā)明還提供了一種自行維持平衡的機器人，包括以上所述的平衡控制裝置。通過以上技術方案，可以實現(xiàn)一種維持兩輪機器人平衡的平衡控制方法、裝置，以 及一種自行維持平衡的機器人，能夠使得兩輪機器人在負載變化的情況仍能夠保持平衡， 并且不需依賴地面的摩擦系數(shù)來調節(jié)兩輪機器人的平衡。


圖1示出了現(xiàn)有技術的兩輪移動服務機器人的本體的示意圖；圖2示出了根據(jù)本發(fā)明的實施例的用于維持兩輪機器人平衡的平衡控制方法的 流程圖；圖3示出了根據(jù)本發(fā)明的實施例的維持兩輪機器人平衡的平衡控制裝置的框圖；圖4示出了根據(jù)本發(fā)明的實施例的自行維持平衡的機器人的示意圖；圖5示出了根據(jù)本發(fā)明的又一實施例的機器人的示意圖；圖6示出了根據(jù)本發(fā)明的又一實施例的機器人的平衡控制系統(tǒng)的結構框圖；圖7示出了根據(jù)本發(fā)明的又一實施例的機器人的控制系統(tǒng)的原理圖；圖8示出了根據(jù)本發(fā)明的實施例的機器人的主控系統(tǒng)的結構框圖；圖9示出了根據(jù)本發(fā)明的實施例的機器人的重心位置獲取裝置結構圖；圖10示出了根據(jù)本發(fā)明的實施例的平衡控制方法的動力學分析圖；以及圖11示出了根據(jù)本發(fā)明的實施例的平衡控制方法中的直流它勵電機的結構圖。
具體實施例方式為了能夠更清楚地理解本發(fā)明的上述目的、特征和優(yōu)點，下面結合附圖和具體實 施方式對本發(fā)明進行進一步的詳細描述。在下面的描述中闡述了很多具體細節(jié)以便于充分理解本發(fā)明，但是，本發(fā)明還可 以采用其他不同于在此描述的其他方式來實施，因此，本發(fā)明并不限于下面公開的具體實 施例的限制。圖2示出了根據(jù)本發(fā)明的實施例的用于維持兩輪機器人平衡的平衡控制方法的 流程圖。如圖2所示，根據(jù)本發(fā)明的實施例的用于維持兩輪機器人平衡的平衡控制方法， 包括步驟202，實時測量兩輪機器人的車體的質量、兩輪機器人的每個車輪相對于車體的 旋轉速度、以及車體與豎直方向的傾斜角度和傾斜角速度，其中，該車體包括所述兩輪機器 人的負載；步驟204，根據(jù)車體的質量、傾斜角度、傾斜角速度和旋轉速度，對每個車輪的電 樞電壓進行調節(jié)，以使兩輪機器人維持平衡。在上述技術方案中，優(yōu)選地，步驟204包括通過預置的平衡關系來調節(jié)兩輪機器 人的每個車輪的輸出扭矩，并根據(jù)每個車輪的輸出扭矩來得到每個車輪的電樞電壓，該平 衡關系通過以下公式限定 xm(M +2m + 2Ja/ R2) + (θ cos Θ-Θ2 sin 0)ML =(Jr +T1)/R
0J,, ^M(g-L0sin0-L cos 002)L sin θ - [xmM + (Θ cos Θ-Θ2 sin 0)ML]L cos θ-(^+T1)
其中，M為車體的質量，m為每個車輪的質量，Jw為每個車輪相對于兩輪機器人的 輪軸的轉動慣量，Jp為車體相對于輪軸的轉動慣量，R為每個車輪的半徑，L為輪軸到車體 的重心的距離，通過車體的質量M結合車體的高度得到，θ為傾斜角度j為傾斜角速度， 鄉(xiāng)為傾斜角速度的一階微分，之為輪軸的中心相對于地面的速度、的一階微分，、通過旋轉 速度結合傾斜角速度得到， ；為兩輪機器人的右車輪的輸出扭矩，T1為兩輪機器人的左車 輪的輸出扭矩。在上述技術方案中，優(yōu)選地，兩輪機器人接收指令，該步驟204還包括根據(jù)平衡 關系及指令，使用狀態(tài)反饋控制方法得到每個車輪的電樞電壓，其中，以傾斜角度θ、傾斜 角速度々、輪軸的中心相對于地面的速度、為狀態(tài)變量，以每個車輪的電樞電壓為控制量。在上述技術方案中，優(yōu)選地，通過傾角儀測量傾斜角度，通過陀螺儀測量傾斜角速 度，通過增量碼盤測量旋轉速度，通過稱重儀測量車體的質量。通過上述技術方案，實時監(jiān)測機器人的負載變化、行走速度及傾斜變化，根據(jù)機器 人的這些現(xiàn)場情況來調節(jié)驅動機器人的車輪的電壓，以使機器人保持平衡，該方法考慮到 機器人的負載動態(tài)變化或受到外部沖擊干擾的情況，使得機器人可以用于服務應用，更加智能。圖3示出了根據(jù)本發(fā)明的實施例的維持兩輪機器人平衡的平衡控制裝置的框圖。如圖3所示，根據(jù)本發(fā)明的實施例的維持兩輪機器人平衡的平衡控制裝置300包 括測量模塊302，實時測量兩輪機器人的車體的質量、兩輪機器人的每個車輪相對于車體 的旋轉速度、以及車體與豎直方向的傾斜角度和傾斜角速度，其中，車體包括兩輪機器人的 負載；計算模塊304，根據(jù)車體的質量、傾斜角度、傾斜角速度和旋轉速度，對每個車輪的電 樞電壓進行調節(jié)，以使兩輪機器人維持平衡。在上述技術方案中，優(yōu)選地，該計算模塊304通過預置的平衡關系來調節(jié)兩輪機 器人的每個車輪的輸出扭矩，并根據(jù)每個車輪的輸出扭矩來得到每個車輪的電樞電壓，該 平衡關系通過以下公式限定xm(M + 2m + 2Ja/R2) + (0cos0-θ2 sin0)ML 二、Tr+Τ) R
OJp =M(g- W sin 0-L cos ΘΘ1 )L sin θ - [xmM + (θ cos Θ-Θ2 sin 0)ML]L cos θ-(Tr +T1)其中，M為車體的質量，m為每個車輪的質量，Jw為每個車輪相對于兩輪機器人的 輪軸的轉動慣量，Jp為車體相對于輪軸的轉動慣量，R為每個車輪的半徑，L為輪軸到車體 的重心的距離，通過車體的質量M結合車體的高度得到，θ為傾斜角度d為傾斜角速度， 々為傾斜角速度的一階微分，之為輪軸的中心相對于地面的速度、的一階微分，、通過旋轉 速度結合傾斜角速度得到， ；為兩輪機器人的右車輪的輸出扭矩，T1為兩輪機器人的左車 輪的輸出扭矩。在上述技術方案中，優(yōu)選地，還包括無線收發(fā)模塊306，用于接收指令，該計算模 塊304根據(jù)平衡關系和指令，使用狀態(tài)反饋控制方法得到每個車輪的電樞電壓，其中，以傾
斜角度θ、傾斜角速度々輪軸的中心相對于地面的速度弋為狀態(tài)變量，以每個車輪的電樞
、
電壓為控制量。在上述技術方案中，優(yōu)選地，該計算模塊304供用戶調整預置的平衡關系。
在上述技術方案中，優(yōu)選地，該測量模塊302通過傾角儀30 測量傾斜角度，通過 陀螺儀30 測量傾斜角速度，通過增量碼盤3022測量旋轉速度，通過稱重儀30M測量車 體的質量。通過上述技術方案，實時監(jiān)測機器人的負載變化、行走速度及傾斜變化，根據(jù)機器 人的這些現(xiàn)場情況來調節(jié)驅動機器人的車輪的電壓，以使機器人保持平衡，該平衡控制裝 置考慮到機器人的負載動態(tài)變化或受到外部沖擊干擾的情況，使得機器人可以用于服務應 用，更加智能。圖4示出了根據(jù)本發(fā)明的實施例的自行維持平衡的機器人的示意圖。如圖4所示，根據(jù)本發(fā)明的實施例的自行維持平衡的機器人400包括以上所述的 平衡控制裝置300。下面參考圖5至圖11來詳細說明本發(fā)明的又一實施例。圖5示出了根據(jù)本發(fā)明的又一實施例的機器人的示意圖，如圖5所示，為本實施例 中的適用的兩輪機器人，其中可控任務托盤502，用于容納機器人的負載；擺桿504 ；右驅 動輪506、右驅動器及減速直流電機508，右驅動器及減速直流電機508用來控制右驅動輪 506的旋轉；陀螺儀510，測量機器人擺桿的傾斜角速度；鋰電池組及電源變換模塊512，用 于為左右車輪的驅動器及減速直流電機供電；主控制器與濾波器514，控制器負責調整輸 入到左右車輪的驅動器以及減速電機的電壓，其中還可包括對控制器的輸入輸出進行模數(shù) 轉換的轉換板卡；左驅動器及減速直流電機516、左驅動輪518，左驅動器及減速電機516用 來控制左驅動輪518的旋轉；機器人本體520 ；稱重儀522，用來測量機器人車體(包括負 載)的質量；無線收發(fā)器524，接收兩輪機器人的行動指令。陀螺儀510、稱重儀522、增量碼 盤(未示出)、傾角儀(未示出)構成了圖3所示實施例的平衡控制裝置的測量模塊302， 無線收發(fā)器5M即圖3所示實施例的平衡控制裝置的無線收發(fā)模塊306，主控制器和濾波器 514中的控制器即計算模塊304。在此，本領域內的技術人員應該理解，根據(jù)本發(fā)明的技術方案的機器人包括但不 限于圖5所示的機器人，例如任務托盤502可以是方形或者立方形。圖6示出了根據(jù)本發(fā)明的又一實施例的機器人的平衡控制系統(tǒng)的結構框圖。如圖 6所示，圖中箭頭表示信號的傳遞方向，該平衡控制系統(tǒng)由鋰電池組602供電，由模擬信號 隔離放大器604將系統(tǒng)的動力信號和控制信號進行隔離，在隔離放大器604的左側為系統(tǒng) 的控制信號，右側為動力信號。傾角傳感器606為單軸加速度傳感器，即傾角儀，角速度傳 感器608為陀螺儀，轉向傳感器610為電阻式角度位置傳感器，即增量碼盤，稱重傳感器612 為霍爾力傳感器，即稱重儀。模擬濾波器614、模數(shù)輸入板卡616、輸出板卡6 及PC104控 制器618組合成為系統(tǒng)的主控制器系統(tǒng)，PC104控制器即為圖3所示的計算模塊304。無線 通信模塊620為465MHZ的串行數(shù)據(jù)傳輸模塊，即為圖3所示的無線收發(fā)模塊306。電機驅 動622及電機6 均為MAXON公司的配套產品。隔離穩(wěn)壓模塊6 為大功率DC-DC變換模 塊。其中，傾角傳感器606、角速度傳感器608、轉向傳感器610和稱重傳感器612構成 了如圖3所示實施例的測量模塊，模數(shù)輸入卡616將經模擬濾波器614過濾的模擬信號轉 變?yōu)閿?shù)字信號，然后傳送給P0104控制器618按照預置的算法進行處理，經過處理的數(shù)字信 號傳送給數(shù)模輸出卡628，將該處理過的數(shù)字信號轉變成模擬信號，該數(shù)字信號、處理過的數(shù)字信號及模擬信號即為控制信號，數(shù)模輸出卡6 輸出的模擬信號經過模擬隔離信號放 大器604變成動力信號，控制左右電機，隔離穩(wěn)壓模塊6 對鋰電池組602提供的電壓進行 調節(jié)，并輸出給左右電機驅動和左右電機。圖7示出了根據(jù)本發(fā)明的又一實施例的機器人的控制系統(tǒng)的原理圖。如圖7所 示，本實施例中的機器人的控制系統(tǒng)由兩個基本組成部分，基站部分和機載部分?；静?分由操作輸入702、基站PC主機704、基站監(jiān)視器、GSM無線收發(fā)模塊706、控制軟件708組 成?；静糠钟糜诎l(fā)送指令給機載部分，可以通過無線網絡進行傳送指令給機載部分，即基 站PC704根據(jù)通過操作輸入702的指令通過無線發(fā)射器706發(fā)送給機載部分，機載部分即 機器人執(zhí)行該指令。機載部分由超聲探測器710、攝像機712、左驅動電機碼盤A714、右驅動電機碼盤 B716、陀螺儀718、GSM無線收發(fā)器720、左輪電機驅動722A、右輪電機驅動器724B、機載監(jiān) 視器726、任務托盤7 及機載控制核心PC104控制器730組成。左驅動電機碼盤A714和 右驅動電機碼盤B716(即增量碼盤)、陀螺儀716、傾角儀(未示出)、稱重儀(未示出)實 時采集的數(shù)據(jù)輸入PC104控制器730，PC104根據(jù)上述數(shù)據(jù)并結合無線收發(fā)器720 (無線收 發(fā)模塊)接收的命令，調節(jié)至左輪電機驅動722A、右輪電機驅動器724B的電壓，以維持機器 人的平衡。超聲探測器、攝像機712用于根據(jù)采集的信號，對無線收發(fā)器720接收的指令進 行修正，監(jiān)視器7 用于顯示機器人當前的狀態(tài)數(shù)據(jù)。圖8示出了根據(jù)本發(fā)明的實施例的機器人的主控系統(tǒng)的結構框圖。該機器人的主 控系統(tǒng)包括控制器802、陀螺儀804、傾角儀806、左增量碼盤L808、右增量碼盤R810、稱重 儀812、濾波器814、左驅動器L816、右驅動器R818、模數(shù)轉換模塊820、數(shù)模轉換模塊822、 數(shù)據(jù)存儲模塊824、碼盤數(shù)據(jù)采集模塊826、無線收發(fā)模塊828、電源模塊830、鋰電池組832， 具體連接關系如圖中箭頭所表示，陀螺儀804、傾角儀806、左增量碼盤L808、右增量碼盤 R810和稱重儀812構成了如圖3所示的測量模塊302，控制器802構成了圖3所示的計算 模塊304。其中，控制器802的主要功能是對陀螺儀804、傾角儀806、左增量碼盤L808、右 增量碼盤R810和稱重儀812采集的狀態(tài)信息進行處理，根據(jù)內嵌的平衡控制算法獲得系統(tǒng) 的輸出量；陀螺儀804的主要功能是對機器人的傾斜角速度進行監(jiān)測，提供機器人的傾斜 角速度信息；傾角儀806的主要功能是對機器人的傾角角度進行監(jiān)測，提供機械人的傾角角
度{曰息。增量碼盤L808、增量碼盤R810的主要功能是對機器人相對與本體(輪軸)的旋 轉速度信息進行監(jiān)測，結合陀螺儀804獲得機器人相對與地面的直線速度信息；稱重儀812獲得機器人的車體(包括負載)質量信息；驅動器L816、驅動器L818接收來自控制器802的控制電壓信號，并將其轉變?yōu)轵?動器L816、驅動器R818的電壓信號；模數(shù)轉換模塊820將經過濾波器814進行濾波處理的模擬信號轉變?yōu)閿?shù)字信號， 供控制器802調用；數(shù)模轉換模塊822將控制器802的數(shù)字信號轉變?yōu)槟M控制信號；
數(shù)據(jù)存儲模塊8M為系統(tǒng)提供數(shù)據(jù)、程序存儲及交換功能；濾波器814對陀螺儀804、稱重儀812、傾角儀806的信號進行濾波處理；碼盤數(shù)據(jù)采集模塊擬6將增量碼盤L808、增量碼盤R810采集的串行數(shù)據(jù)流轉變?yōu)?并行數(shù)據(jù)流；無線收發(fā)模塊8 接收和發(fā)送命令數(shù)據(jù)及機器人的狀態(tài)信息；電源模塊830將機器人及系統(tǒng)的電源電壓進行變換，匹配各用電模塊；鋰電池組832提供機器人運行以及系統(tǒng)所需的運行能量。在此，本領域的技術人員應該理解，該控制器802包括但不限于嵌入式計算機、 數(shù)字信號處理設備、單片計算機。接下來詳細說明機器人利用上述主控系統(tǒng)維持平衡的流程。第1步使機器人靜止且處于豎直狀態(tài)；第2步打開機器人的開關，給主控系統(tǒng)加電；第3步控制系統(tǒng)對電源830、陀螺儀804、傾角儀806、稱重儀812、無線收發(fā)模塊 824的狀態(tài)進行檢測，如果上述設備正常，則顯示工作正常信號，執(zhí)行第4步；否則，回到第2
步；第4步用戶向機器人的托盤上放置物品；稱重儀812對車體質量(包括托盤中負 載的質量)進行測量；第5步控制器802通過重心高度估計方法估計機器人的重心高度；第6步控制器802根據(jù)托盤7 及車體質量和機器人的重心高度，設置初始狀態(tài) 反饋增益系數(shù)(控制器802采用狀態(tài)反饋控制方法控制機器人的平衡)，并應用直線方向的 狀態(tài)反饋平衡控制方法得到對驅動器L816、驅動器R818的控制量；第7步控制器802將第6步所述控制量(即輸出至驅動器的電壓信號)送往驅 動器，驅動器L816、驅動器R818執(zhí)行控制量，使機器人保持平衡狀態(tài)；第8步陀螺儀804、增量碼盤L808、增量碼盤R810、模數(shù)轉換模塊820、碼盤數(shù)據(jù) 采集模塊擬6實時采集機器人的狀態(tài)數(shù)據(jù)，控制器802反復執(zhí)行第5步至第7步的操作，直 到獲得期望的狀態(tài)反饋增益系數(shù)；第9步控制器802通過無線收發(fā)模塊擬8接收行動指令，并按照行動指令運動；其中，判斷重心高度估計是否準確的方法如果控制器802根據(jù)上述行動指令與 機器人實際行動狀態(tài)的偏差所產生的控制量在連續(xù)3個控制周期(對應于狀態(tài)反饋控制方 法)內都能使機器人的行動狀態(tài)朝期望方向變動，則認為機器人的負載變化不大，機器人 的重心的估計是恰當?shù)?，當前的狀態(tài)反饋增益系數(shù)是合適的，不需要調整。如果連續(xù)兩個控 制周期內機器人的行動狀態(tài)均沒有朝期望的行動狀態(tài)變化，則認為機器人重心的估計不成 功，機器人的狀態(tài)反饋增益系數(shù)需要調整，此時要輸出電壓控制量先將機器人速度減為原 值的一半，并在接下來的第三個控制周期內將狀態(tài)反饋增益系數(shù)增大25%，如此循環(huán)直到 機器人的行動狀態(tài)連續(xù)三個周期朝期望的方向改變。其中，重心高度計算過程如下所述稱重儀的結構如圖9所示。包括四個部分，由902組成的基板，四個稱重傳感器組 成稱重單元904，由上板908組成的站立平臺以及由軟連接帶906組成的浮動連接構成。稱 重單元904測量隨時檢測位于上板上的質量K (即車體質量)，質量K為四個稱重傳感器的代數(shù)和。根據(jù)標準體重及身高關系，設置如下的計算公式L = 75+w/0. 75L為機器人的高度(米)，W為機器人的車體質量(千克)，即為上述的質量K。根據(jù)獲得的高度信息，帶入下面的公式獲得機器人的重心位置CG = 0. 66 □ 1通過機器人的重心位置得到機器人的重心高度?？刂破?02中采用的直線平衡控制方法為使用全狀態(tài)反饋控制方法建立機器人 的平衡模型，根據(jù)陀螺儀804、傾角儀806、稱重儀812以及增量碼盤L808、增量碼盤R810采 集的數(shù)據(jù)作為狀態(tài)量，以輸出至驅動器L816、驅動器R818的電壓為控制量，該方法只針對 機器人直線行走的情況。直線行走時，控制器802根據(jù)機器人當前行動狀態(tài)與接收指令間 的誤差產生電壓控制量，控制量根據(jù)直流電機的電壓一扭矩特性轉變?yōu)榫S持系統(tǒng)平衡需要 的扭矩量?？刂破?02對沖擊擾動的控制方法為首先測得機器人的自由振動頻率，然后以 此頻率為截止頻率設計濾波器812，對高于此頻率的信號進行最大幅度的衰減，達到消除干 擾、平抑電機輸出的效果，并最終消除沖擊信號對機器人平衡的影響，這樣，機器人可以在 外界沖擊干擾的情況下保持動態(tài)平衡。圖10示出了根據(jù)本發(fā)明的實施例的平衡控制方法的動力學分析圖，圖11示出了 根據(jù)本發(fā)明的實施例的平衡控制方法中的直流它勵電機的結構圖。首先簡單介紹機器人動力學模型里用到的參數(shù)及其定義，所有量的單位均為國際 單位m:單個車輪的質量；M 機器人除兩個車輪外的質量，即車體質量；Jw 車輪相對于輪軸(ζ軸)的轉動慣量；Jp 機器人本體對ζ軸的轉動慣量J0 機器人本體對Y軸的轉動慣量；R 驅動輪半徑；D 兩驅動輪之間的距離；L :z軸到機器人重心的距離；θ :機器人的擺桿相對于Y軸的夾角；ω 機器人的擺桿的相對于Y軸角速度；δ 機器人的偏航角；Xp X1 左右驅動輪的位移；fr, fx 左右驅動輪與地面的靜摩擦力；Hr, H1 左右驅動輪與機器人車體相互作用力沿水平方的分量；vr, V1 左右驅動輪與機器人車體相互作用力沿豎直方向分量； ；，T1:左右輪驅動電機的輸出扭矩；xm 兩輪軸線中點的位移；xp, yp 機器人擺桿重心位移；
根據(jù)牛頓定律分析對于左輪有^im = fi 'hI ( 1 )Xt JaIR = T1-frR (2)對于右輪有Xrm^ fr~Hr (3)(JJR = Tr-fr R (4)對于擺桿有^pM =Hr+H1 (5)0Jp =(Vl^Vr)Lsine-(Hl+Hr)Lco^-(Tl+Tr) (6)=V^V1-Mg ( )SJs=(Hl-Hr)D/! (8)以上的物理量間存在如下關系xm = (xr+xi) /2，2^m =Xr+^l =K+^i ( 9 )xp = xm+Lsin θ ,xp =xm +L0cos0 xp =xm + cos0-L^2sin^ ( )yp = Lcos θ ~l,yp =-LOsm θ yp =-LOsin θ-LO2 cos θ (工工)δ · D = X1-Xr(12)根據(jù)以上的方程，可以得到系統(tǒng)的微分非線性方程(M + 2m + /2) + (6> cos6> - 02 sin0)ML = (T^T1)IR (工3 )
0JP =M(g-Lesm0-Lcosee2)L^ine~ [xmM + {θ cos θ-θ2 sin 0)ML ]L cos θ-(Jr+T1")δ (Dm+ DJ J R2+ 2/, / D) = (Tr-T1) / R ( 15 )下面描述對于直流電機的數(shù)學模型，如圖11所示，該實施例中將直流它勵電機簡 化為受到負載影響的阻性和感性元件的組合體。
0145]首先對電機模型的參數(shù)的定義0146]Ua直流電機電樞電壓(V)0147]Ra直流電機電樞電阻(Ω)0148]Ia電樞電流(A)0149]Ea電機電樞反電動勢(V)0150]Ke直流電機反電動勢系數(shù)(V · s/rad)0151]ωm:直流電機轉動角速度(rad/s)0152]Tm直流電機輸出轉矩(Ν·πι)0153]Km直流電機電磁轉矩系數(shù)(N · m/A)0154]T減速器輸出扭矩0155]ω減速器輸出角速度0156]N:減速器的減速比
電機的數(shù)學模型為Tffl = Kffl (Ua-Ea)/Ra(16)Ea = Ke. ωω(17)于是有Tm = Km (Ua-Ke · ω m)/Ra(18)加裝減速機驅動模塊后，輸出扭矩T與電機輸出扭矩間存在如下關系T = Tm · N (19)角速度關系為GJm= ω ·Ν(20)根據(jù)以上公式，電機加裝減速機驅動模塊后的數(shù)學模型為
權利要求
1.一種用于維持兩輪機器人平衡的平衡控制方法，其特征在于，包括步驟202，實時測量所述兩輪機器人的車體的質量、所述兩輪機器人的每個車輪相對于 所述車體的旋轉速度、以及所述車體與豎直方向的傾斜角度和傾斜角速度，其中，所述車體 包括所述兩輪機器人的負載；步驟204，根據(jù)所述車體的質量、所述傾斜角度、所述傾斜角速度和所述旋轉速度，對所 述每個車輪的電樞電壓進行調節(jié)，以使所述兩輪機器人維持平衡。
2.根據(jù)權利要求1所述的平衡控制方法，其特征在于，所述步驟204包括通過預置的平衡關系來調節(jié)所述兩輪機器人的每個車輪的輸出扭矩，并根據(jù)所述每個 車輪的輸出扭矩來得到所述每個車輪的電樞電壓，所述平衡關系通過以下公式限定xm(M +2m + 2Jm/R2) + (θ cos Θ-Θ2 sin 0)ML = {Tr+Tt)/R0Jp=M(g- Ld sm0-L cos ΘΘ2 )L sin θ - [xmM + (Θ cos Θ-Θ2 sin 0)ML]L cos θ-(Tr +T1)其中，M為所述車體的質量，m為所述每個車輪的質量，Jw為所述每個車輪相對于所述 兩輪機器人的輪軸的轉動慣量，Jp為所述車體相對于所述輪軸的轉動慣量，R為所述每個車 輪的半徑，L為所述輪軸到所述車體的重心的距離，通過所述車體的質量M結合所述車體的 高度得到，θ為所述傾斜角度j為所述傾斜角速度，鄉(xiāng)為所述傾斜角速度的一階微分，禮為 所述輪軸的中心相對于地面的速度的一階微分，、通過所述旋轉速度結合所述傾斜角速 度得到， ；為所述兩輪機器人的右車輪的輸出扭矩，T1為所述兩輪機器人的左車輪的輸出 扭矩。
3.根據(jù)權利要求2所述的平衡控制方法，其特征在于，所述兩輪機器人接收指令，所述 步驟204還包括根據(jù)所述平衡關系及所述指令，使用狀態(tài)反饋控制方法得到所述每個車輪的電樞電 壓，其中，以所述傾斜角度θ、所述傾斜角速度々所述輪軸的中心相對于地面的速度i為狀態(tài)變量，以所述每個車輪的電樞電壓為控制量。
4.根據(jù)權利要求1至3中任一項所述的平衡控制方法，其特征在于，通過傾角儀測量所 述傾斜角度，通過陀螺儀測量所述傾斜角速度，通過增量碼盤測量所述旋轉速度，通過稱重 儀測量所述車體的質量。
5.一種維持兩輪機器人平衡的平衡控制裝置，其特征在于，包括測量模塊，實時測量所述兩輪機器人的車體的質量、所述兩輪機器人的每個車輪相對 于所述車體的旋轉速度、以及所述車體與豎直方向的傾斜角度和傾斜角速度，其中，所述車 體包括所述兩輪機器人的負載；計算模塊，根據(jù)所述車體的質量、所述傾斜角度、所述傾斜角速度和所述旋轉速度，對 所述每個車輪的電樞電壓進行調節(jié)，以使所述兩輪機器人維持平衡。
6.根據(jù)權利要求5所述的平衡控制裝置，其特征在于，所述計算模塊通過預置的平衡 關系來調節(jié)所述兩輪機器人的每個車輪的輸出扭矩，并根據(jù)所述每個車輪的輸出扭矩來得 到所述每個車輪的電樞電壓，所述平衡關系通過以下公式限定xm(M +2m + 2J0J R2) + (θ cos θ-θ2 sin 0)ML =(T^T1)IRθ] p =M(g-Ld -L cos ΘΘ2 )L sin θ - [xmM + (Θ cos Θ-Θ2 sin 0)ML]L cos O-(T^T1)其中，M為所述車體的質量，m為所述每個車輪的質量，Jw為所述每個車輪相對于所述 兩輪機器人的輪軸的轉動慣量，Jp為所述車體相對于所述輪軸的轉動慣量，R為所述每個車 輪的半徑，L為所述輪軸到所述車體的重心的距離，通過所述車體的質量M結合所述車體的 高度得到，θ為所述傾斜角度j為所述傾斜角速度j為所述傾斜角速度的一階微分，之為 所述輪軸的中心相對于地面的速度.、的一階微分，、通過所述旋轉速度結合所述傾斜角速 度得到， ；為所述兩輪機器人的右車輪的輸出扭矩，T1為所述兩輪機器人的左車輪的輸出 扭矩。
7.根據(jù)權利要求6所述的平衡控制裝置，其特征在于，還包括無線收發(fā)模塊，用于接收指令，所述計算模塊根據(jù)所述平衡關系和所述指令，使用狀態(tài) 反饋控制方法得到所述每個車輪的電樞電壓，其中，以所述傾斜角度θ、所述傾斜角速度θ所述輪軸的中心相對于地面的速度、為狀態(tài)變量，以所述每個車輪的電樞電壓為控制 、量。
8.根據(jù)權利要求5所述的平衡控制裝置，其特征在于，所述計算模塊供用戶調整預置 的平衡關系。
9.根據(jù)權利要求5至8中任一項所述的平衡控制裝置，其特征在于，所述測量模塊通過 傾角儀測量所述傾斜角度，通過陀螺儀測量所述傾斜角速度，通過增量碼盤測量所述旋轉 速度，通過稱重儀測量所述車體的質量。
10.一種自行維持平衡的機器人，其特征在于，包括 權利要求5至9中任一項所述的平衡控制裝置。
全文摘要
本發(fā)明提供了一種用于維持兩輪機器人平衡的平衡控制方法，包括步驟202，實時測量所述兩輪機器人的車體的質量、所述兩輪機器人的每個車輪相對于所述車體的旋轉速度、以及所述車體與豎直方向的傾斜角度和傾斜角速度，其中，所述車體包括所述兩輪機器人的負載；步驟204，根據(jù)所述車體的質量、所述傾斜角度、所述傾斜角速度和所述旋轉速度，對所述每個車輪的電樞電壓進行調節(jié)，以使所述兩輪機器人維持平衡。本發(fā)明還提供了一種維持兩輪機器人平衡的平衡控制裝置及具有該平衡控制裝置的機器人。根據(jù)本發(fā)明的技術方案，可以使機器人在負載動態(tài)變化的情況下維持動態(tài)平衡。
文檔編號G05D1/08GK102141814SQ201010593908
公開日2011年8月3日 申請日期2010年12月9日 優(yōu)先權日2010年12月9日
發(fā)明者夏雪蛟, 戴福全, 李潮全, 李科杰, 王會彬, 邵潔, 高學山, 黃強 申請人:北京理工大學