一種球形輪移動(dòng)機(jī)器人及其測(cè)速方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明屬于自動(dòng)控制技術(shù)領(lǐng)域，尤其涉及一種球形輪移動(dòng)機(jī)器人及其測(cè)速方法。
【背景技術(shù)】
[0002] 2014年的專利一一球形輪移動(dòng)機(jī)器人的測(cè)速方法和控制方法，申請(qǐng)?zhí)?201410345088.0,對(duì)傳統(tǒng)球形輪移動(dòng)機(jī)器人的機(jī)械結(jié)構(gòu)進(jìn)行了改進(jìn)，并設(shè)計(jì)了復(fù)雜環(huán)境下 的運(yùn)動(dòng)控制方法。
[0003] 該專利中的球形輪移動(dòng)機(jī)器人是通過(guò)萬(wàn)向驅(qū)動(dòng)輪的驅(qū)動(dòng)電機(jī)編碼器數(shù)據(jù)解算球 形輪的速度。由于驅(qū)動(dòng)輪的作用是主動(dòng)驅(qū)動(dòng)，在驅(qū)動(dòng)過(guò)程中，球形輪處于被動(dòng)狀態(tài)，驅(qū)動(dòng)輪 處于主動(dòng)狀態(tài)。當(dāng)驅(qū)動(dòng)輪突然受到一個(gè)大扭矩驅(qū)動(dòng)指令而快速響應(yīng)時(shí)，球形輪因?yàn)槭艿捷^ 大阻力或者因?yàn)閼T性來(lái)不及響應(yīng)驅(qū)動(dòng)輪的作用，驅(qū)動(dòng)輪與球形輪之間就會(huì)存在相對(duì)滑動(dòng)。 此時(shí)驅(qū)動(dòng)電機(jī)的編碼器數(shù)據(jù)就不能很好地反映球形輪的速度，從而使得球速的測(cè)量存在較 大誤差，進(jìn)而影響控制效果，甚至使得機(jī)器人失穩(wěn)。

【發(fā)明內(nèi)容】

[0004] 為解決上述問(wèn)題，本發(fā)明提供一種球形輪移動(dòng)機(jī)器人及其測(cè)速方法，提高了機(jī)器 人行走時(shí)球形輪球速的測(cè)量精度和可靠性。
[0005] 本發(fā)明的球形輪移動(dòng)機(jī)器人，其包括：球形輪、根據(jù)控制信號(hào)驅(qū)動(dòng)球形輪運(yùn)動(dòng)的驅(qū) 動(dòng)機(jī)構(gòu)、承載各結(jié)構(gòu)的機(jī)身、控制模塊以及獲取機(jī)身姿態(tài)數(shù)據(jù)的慣性傳感器；其特征在于， 還包括：
[0006] 用于獲取球形輪相對(duì)于機(jī)身速度的測(cè)速機(jī)構(gòu)；
[0007] 所述控制模塊根據(jù)測(cè)速機(jī)構(gòu)獲取的球形輪相對(duì)于機(jī)身速度和慣性傳感器獲取的 機(jī)身姿態(tài)數(shù)據(jù)利用基于運(yùn)動(dòng)學(xué)約束關(guān)系的線性方程得到球形輪的求解速度《，然后利用球 形輪的求解速度和傳感器的機(jī)身姿態(tài)數(shù)據(jù)結(jié)合運(yùn)動(dòng)控制律求得球形輪移動(dòng)機(jī)器人運(yùn)動(dòng)的 控制信號(hào)，并將該控制信號(hào)發(fā)送至驅(qū)動(dòng)機(jī)構(gòu)；
[0008] 其中：測(cè)速機(jī)構(gòu)的數(shù)量為三個(gè)，每個(gè)測(cè)速機(jī)構(gòu)均包括：固定座、弧形支架、編碼器 固定件、全向測(cè)速輪以及編碼器；
[0009] 固定座與機(jī)身的底盤固定，固定座與弧形支架鉸接，編碼器固定件與編碼器固定， 弧形支架與編碼器固定件鉸接，且該鉸鏈的軸的中心線通過(guò)全向測(cè)速輪的中心；弧形支架 將全向測(cè)速輪壓向球形輪，使得全向測(cè)速輪與球形輪緊密貼合，全向測(cè)速輪與編碼器輸出 軸同軸固連。
[0010] 進(jìn)一步的，弧形支架依次包括水平段、大圓弧段、小圓弧段三段，水平段與固定座 連接，連接方式為活動(dòng)鉸鏈，活動(dòng)鉸鏈的轉(zhuǎn)軸上裝有扭矩彈簧，該扭矩彈簧產(chǎn)生的扭矩將弧 形支架壓向球形輪；大圓弧段所在圓弧與球形輪球心共圓心；小圓弧段的圓心在大圓弧段 的弧面中心線上，小圓弧段與全向測(cè)速輪共圓心；小圓弧段通過(guò)鉸鏈與編碼器固定件連接。
[0011] 進(jìn)一步的，大圓弧段采用可伸縮結(jié)構(gòu)，從而改變?nèi)驕y(cè)速輪與球形輪的接觸位置； 小圓弧段通過(guò)鉸鏈與編碼器固定件連接，從而改變?nèi)驕y(cè)速輪對(duì)球形輪的有效測(cè)速方向。
[0012] 進(jìn)一步的，編碼器固定件包括圓環(huán)部分和直角部分，直角部分內(nèi)表面與弧形支架 小圓弧段外表面通過(guò)鉸鏈連接，該鉸鏈的軸的中心線通過(guò)全向測(cè)速輪的中心；圓環(huán)部分的 環(huán)面上設(shè)有與編碼器螺孔匹配的螺孔，編碼器通過(guò)螺孔與編碼器固定件的環(huán)形連接部分同 軸心固連。
[0013] 進(jìn)一步的，驅(qū)動(dòng)機(jī)構(gòu)的數(shù)量為三個(gè)，對(duì)稱固定于機(jī)身的底盤上，每個(gè)驅(qū)動(dòng)機(jī)構(gòu)包 括：驅(qū)動(dòng)電機(jī)編碼器、驅(qū)動(dòng)電機(jī)、萬(wàn)向驅(qū)動(dòng)輪；驅(qū)動(dòng)電機(jī)輸出軸一端同軸安裝萬(wàn)向驅(qū)動(dòng)輪， 另一端安裝驅(qū)動(dòng)電機(jī)編碼器；驅(qū)動(dòng)電機(jī)與機(jī)身底盤固定；萬(wàn)向驅(qū)動(dòng)輪與球形輪相切。
[0014] 進(jìn)一步的，設(shè)角度y為連線與球形輪的垂線之間的夾角，所述連線為全向測(cè)速輪 的中心與球形輪的球心的連線；該角度Y為90° ;
[0015] 設(shè)夾角P為全向測(cè)速輪所在平面與球形輪經(jīng)線所在平面的夾角，該夾角P為 45。。
[0016] 進(jìn)一步的，獲得球形輪移動(dòng)機(jī)器人運(yùn)動(dòng)的控制信號(hào)的步驟具體包括：
[0017] 步驟1，控制模塊分別解算每組測(cè)速機(jī)構(gòu)的編碼器的數(shù)據(jù)nn、n12、n13，利用公式
徑；
[0018] 上式中所有矢量均處于球形輪坐標(biāo)系中：
[0019] vn、v12、V13分別表示三個(gè)測(cè)速機(jī)構(gòu)與球輪接觸點(diǎn)處的線速度矢量，且
球形輪移動(dòng)機(jī)器人整體處于鉛垂?fàn)顟B(tài)時(shí)的測(cè)量點(diǎn)位置矢量，夾角Y預(yù)先設(shè)定，R表示球輪 的半徑e ,為慣性傳感器測(cè)得的三個(gè)測(cè)速機(jī)構(gòu)相對(duì)于鉛垂?fàn)顟B(tài)的旋轉(zhuǎn)矩陣；
表示三個(gè)測(cè)速機(jī)構(gòu)的全向測(cè)速輪在球形輪坐標(biāo)系的夾角0下測(cè)得的有效測(cè)速方向矢量， 預(yù)先設(shè)定；
[0022] 步驟2,控制模塊分別解算每個(gè)驅(qū)動(dòng)機(jī)構(gòu)的驅(qū)動(dòng)電機(jī)編碼器的數(shù)據(jù)n21、n22、n23，利
驅(qū)動(dòng)輪的半徑；
[0023] 上式中所有矢量均處于球形輪坐標(biāo)系中：
[0024] V21、v22、V23分別表示三個(gè)驅(qū)動(dòng)機(jī)構(gòu)與球輪接觸點(diǎn)處的線速度矢量，且
機(jī)構(gòu)在球形輪移動(dòng)機(jī)器人整體處于鉛垂?fàn)顟B(tài)時(shí)的位置矢量；Le ,為慣性傳感器測(cè)得的三 個(gè)驅(qū)動(dòng)機(jī)構(gòu)相對(duì)于鉛垂?fàn)顟B(tài)的旋轉(zhuǎn)矩陣；
坐標(biāo)系下的有效測(cè)速方向矢量；
[0026] 步驟3,控制模塊對(duì)第一角速度和第二角速度《 進(jìn)行數(shù)據(jù)融合，利用濾波公 式CO1= ?n+Hc^-coJ，計(jì)算出球形輪的相對(duì)速度然后將球形輪的相對(duì)速度^與 慣性傳感器的姿態(tài)數(shù)據(jù)中的陀螺數(shù)據(jù)結(jié)合，得到球形輪的解算速度《 = 其 中，k為卡爾曼濾波系數(shù)；
[0027] 步驟4,利用解算速度co和慣性傳感器的數(shù)據(jù)在周期T內(nèi)計(jì)算控制量，該控制量即 為球形輪移動(dòng)機(jī)器人運(yùn)動(dòng)的控制信號(hào)。
[0028] 有益效果：
[0029] (1)本發(fā)明設(shè)計(jì)的球輪測(cè)速機(jī)構(gòu)，能夠緊貼球形輪表面；
[0030] (2)全向測(cè)速輪能夠?qū)y(cè)速點(diǎn)的線速度分解為測(cè)量速度和自由速度，在有效測(cè)速 的同時(shí)不影響球輪的運(yùn)動(dòng)；
[0031] (3)測(cè)速輪的測(cè)量位置和測(cè)速方位可調(diào)，能夠方便、準(zhǔn)確地測(cè)量球形輪表面（除與 地面接觸和球形輪頂部附近區(qū)域外）任意位置、任意方位的線速度。
[0032] (4)本發(fā)明設(shè)計(jì)的三組對(duì)稱球輪測(cè)速機(jī)構(gòu)，能夠快速、精確地測(cè)量球形輪的轉(zhuǎn)動(dòng)速 度，解算速度快、測(cè)量精度高、可靠性高；
[0033] (5)本發(fā)明設(shè)計(jì)的測(cè)速算法，能夠在球輪低速和高速時(shí)均有效、準(zhǔn)確地測(cè)量球速， 對(duì)以本發(fā)明為基礎(chǔ)運(yùn)動(dòng)平臺(tái)在復(fù)雜環(huán)境下的應(yīng)用打下了堅(jiān)實(shí)的基礎(chǔ)。
【附圖說(shuō)明】
[0034] 圖1為本發(fā)明的結(jié)構(gòu)示意圖；
[0035] 圖2為本發(fā)明中球形輪測(cè)速機(jī)構(gòu)的主視圖；
[0036] 圖3為本發(fā)明中球形輪測(cè)速機(jī)構(gòu)的單體側(cè)視圖；
[0037] 圖4為本發(fā)明中球形輪測(cè)速機(jī)構(gòu)的單體任意角視圖；
[0038] 圖5為本發(fā)明中球形輪測(cè)速機(jī)構(gòu)電機(jī)固定件的任意角視圖。
【具體實(shí)施方式】
[0039] 如圖1所示，球形輪移動(dòng)機(jī)器人包括：球形輪1、三個(gè)相同的驅(qū)動(dòng)機(jī)構(gòu)2、機(jī)身3、控 制模塊4、三個(gè)相同的測(cè)速機(jī)構(gòu)5以及慣性傳感器6 ;
[0040] 驅(qū)動(dòng)機(jī)構(gòu)2包括：驅(qū)動(dòng)電機(jī)編碼器21、驅(qū)動(dòng)電機(jī)22、萬(wàn)向驅(qū)動(dòng)輪23 ;
[0041] 控制模塊4和慣性傳感器6安裝于機(jī)身3內(nèi)；三個(gè)相同的驅(qū)動(dòng)機(jī)構(gòu)2對(duì)稱固定于 機(jī)身3底盤24上，并均與球形輪1相切，如圖1中所示；
[0042] 測(cè)速機(jī)構(gòu)5,檢測(cè)球形輪1相對(duì)于機(jī)身3的第一角速度coy
[0043] 慣性傳感器6,檢測(cè)機(jī)身3的姿態(tài)數(shù)據(jù)，該姿態(tài)數(shù)據(jù)主要包括：相對(duì)于地面的轉(zhuǎn)動(dòng) 速度和旋轉(zhuǎn)矩陣R^ ;
[0044] 控制模塊4,分析球形輪移動(dòng)機(jī)器人姿態(tài)和球形輪1速度得到球形輪1的解算速度 ?，根據(jù)球形輪1的解算速度及姿態(tài)數(shù)據(jù)得出控制策略輸出控制信號(hào)；
[0045] 驅(qū)動(dòng)機(jī)構(gòu)2,根據(jù)控制信號(hào)驅(qū)動(dòng)球形輪1，或者檢測(cè)球形輪1相對(duì)于機(jī)身3的第二 角速度
[0046] 參見圖2、3、4、5,每個(gè)測(cè)速機(jī)構(gòu)5的包括：固定座7、弧形支架8、編碼器固定件9、 全向測(cè)速輪10以及編碼器11。
[0047] 固定座7與機(jī)身3的底盤24固定，固定座7和弧形支架8鉸接，編碼器固定件9 固定編碼器11，弧形支架8和編碼器固定件9鉸接，且該鉸鏈的軸的中心線通過(guò)全向測(cè)速輪 10的中心，如附圖3所示；
[0048] 圖3中，弧形支架8包括水平段、大圓弧段、小圓弧段三段，水平段與固定座7連 接，連接方式為活動(dòng)鉸鏈，活動(dòng)鉸鏈的轉(zhuǎn)軸上裝有扭矩彈簧，扭矩彈簧產(chǎn)生的扭矩25將弧 形支架8壓向球形輪1，從而使得全向測(cè)速輪10與球形輪1緊貼；水平段之后的過(guò)渡連接 段是大圓弧段，大圓弧段所在圓弧與球形輪1的經(jīng)線共圓心，大圓弧部分采用可伸縮結(jié)構(gòu)， 大圓弧的長(zhǎng)度可調(diào)，即可改變?nèi)驕y(cè)速輪10與球形輪1的接觸位置，該位置由圖2中的角 度13決定，角度13為連線與球形輪1的垂線之間的夾角，所述連線為全向測(cè)速輪10的中 心與球形輪1的球心的連線；大圓弧段連接小圓弧段，且小圓弧的圓心在大圓弧的弧面中 心線上，小圓弧與全向測(cè)速輪10共圓心；小圓弧段通過(guò)鉸鏈與編碼器固定件9連接。
[0049] 編碼器固定件9如圖5所示，包括圓環(huán)部分和直角部分。在附圖4中，編碼器固定 件9的直角部分與弧形支架8通過(guò)鉸鏈連接，編碼器固定件9直角部分內(nèi)表面與弧形支架 8的小圓弧段外表面鉸接，鉸鏈的軸的中心線通過(guò)全向測(cè)速輪10的中心，如附圖3所示；鉸 鏈的軸為蝶形螺栓，加緊蝶形螺栓可以將鉸鏈變?yōu)楣潭ㄣq鏈，使全向測(cè)速輪10所在平面與 球形輪經(jīng)線所在平面的夾角12固定，如附圖2所示；編碼器固定件9的環(huán)形連接部分有螺 孔與編碼器11的螺孔匹配，編碼器10通過(guò)螺孔與編碼器固定件9的環(huán)形連接部分同軸心 固連，如附圖4所示。
[0050] 全向測(cè)速輪10與編碼器11輸出軸同軸固連，全向測(cè)速輪10外緣的滾輪保持與球 形輪1的表面相切；全向測(cè)速輪10的外邊緣的滾輪可繞自身軸自由轉(zhuǎn)動(dòng)，并且同時(shí)隨全向 測(cè)速輪10轉(zhuǎn)動(dòng)；滾輪將球形輪接觸點(diǎn)處的線速度分解為繞自身轉(zhuǎn)軸的自由速度和繞編碼 器輸出軸的測(cè)量速度。
[0051] 驅(qū)動(dòng)機(jī)構(gòu)2包括：驅(qū)動(dòng)電機(jī)編碼器21、驅(qū)動(dòng)電機(jī)22、萬(wàn)向驅(qū)動(dòng)輪23 ;驅(qū)動(dòng)電機(jī)22 輸出軸一端同軸安裝萬(wàn)向驅(qū)動(dòng)輪23,另一端安裝驅(qū)動(dòng)電機(jī)編碼器21 ;驅(qū)動(dòng)電機(jī)22與機(jī)身3 底盤24固定。