直立平衡機(jī)器人的制作方法
【專利摘要】本發(fā)明公開(kāi)了一種直立平衡機(jī)器人，包括機(jī)身，機(jī)身的內(nèi)部設(shè)有加速度計(jì)、陀螺儀以及集成步進(jìn)電機(jī)驅(qū)動(dòng)單元的控制模塊，機(jī)身下端的兩側(cè)設(shè)有兩個(gè)滾輪，機(jī)身內(nèi)部設(shè)有兩組步進(jìn)電機(jī)，機(jī)身的兩側(cè)設(shè)有活動(dòng)臂，控制模塊基于加速度計(jì)和陀螺儀的檢測(cè)量經(jīng)過(guò)濾波、計(jì)算后得到角度值和角速度值，利用角度值和角速度值進(jìn)行PD控制產(chǎn)生兩個(gè)用于控制電機(jī)轉(zhuǎn)動(dòng)的輸出量，通過(guò)步進(jìn)電機(jī)驅(qū)動(dòng)單元控制步進(jìn)電機(jī)工作。本發(fā)明通過(guò)控制兩個(gè)電機(jī)的轉(zhuǎn)動(dòng)能夠精確地控制機(jī)器人保持直立平衡；其整體為人形造型，具有仿生外形效果；控制模塊集成有步進(jìn)電機(jī)驅(qū)動(dòng)單元，不需要設(shè)置傳統(tǒng)的大電機(jī)驅(qū)動(dòng)器，減小了機(jī)器人本身的體積，降低功耗；此機(jī)器人能夠進(jìn)一步配置使得具有娛樂(lè)功能。
【專利說(shuō)明】
直立平衡機(jī)器人
技術(shù)領(lǐng)域
[0001]本發(fā)明涉及電子技術(shù)領(lǐng)域，具體涉及一種可供教學(xué)使用的直立平衡機(jī)器人。
【背景技術(shù)】
[0002]現(xiàn)有技術(shù)中并無(wú)一種專門針對(duì)教學(xué)使用的專用直立機(jī)器人。因此，有必要開(kāi)發(fā)一種相應(yīng)的機(jī)器人產(chǎn)品，以供學(xué)生快速學(xué)習(xí)機(jī)器人技術(shù)使用。

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

[0003]為了實(shí)現(xiàn)以上目的，本發(fā)明提供一種直立平衡機(jī)器人，其能夠同時(shí)滿足教學(xué)和娛樂(lè)的使用要求。
[0004]本發(fā)明是通過(guò)以下技術(shù)方案實(shí)現(xiàn)的:
直立平衡機(jī)器人，包括整體成直立狀態(tài)的機(jī)身，機(jī)身的內(nèi)部設(shè)有用于檢測(cè)角度值的加速度計(jì)、用于檢測(cè)角速度值的陀螺儀以及集成步進(jìn)電機(jī)驅(qū)動(dòng)單元的控制模塊，機(jī)身下端的兩側(cè)設(shè)有用于與地面接觸的兩個(gè)滾輪，機(jī)身內(nèi)部設(shè)有分別單獨(dú)驅(qū)動(dòng)兩個(gè)滾輪轉(zhuǎn)動(dòng)的兩組步進(jìn)電機(jī)，所述機(jī)身的兩側(cè)設(shè)有受控制模塊控制活動(dòng)的活動(dòng)臂，所述控制模塊基于加速度計(jì)和陀螺儀的檢測(cè)量經(jīng)過(guò)濾波、計(jì)算后得到角度值和角速度值，利用角度值和角速度值進(jìn)行F1D控制產(chǎn)生兩個(gè)用于控制電機(jī)轉(zhuǎn)動(dòng)的輸出量，通過(guò)步進(jìn)電機(jī)驅(qū)動(dòng)單元控制步進(jìn)電機(jī)工作。
[0005]作為優(yōu)選的實(shí)施方式，所述用于與外部通訊的無(wú)線數(shù)據(jù)傳輸模塊，無(wú)線數(shù)據(jù)傳輸模塊與控制模塊電氣連接。
[0006]作為優(yōu)選的實(shí)施方式，所述機(jī)身的外部設(shè)有溫度傳感器、濕度傳感器、壓力傳感器和聲音傳感器，溫度傳感器、濕度傳感器、壓力傳感器和聲音傳感器的輸出端與控制模塊連接。
[0007]作為優(yōu)選的實(shí)施方式，所述機(jī)身的外部設(shè)有用于感應(yīng)其他機(jī)器人的感受傳感器，感受傳感器的輸出端與控制模塊連接。進(jìn)一步，所述活動(dòng)臂上安裝有聲光槍，機(jī)身正面感應(yīng)到其他機(jī)器人時(shí)聲光槍點(diǎn)亮。
[0008]本發(fā)明的有益效果是:本發(fā)明通過(guò)控制兩個(gè)電機(jī)的轉(zhuǎn)動(dòng)能夠精確地控制機(jī)器人保持直立平衡;其整體為人形造型，具有仿生外形效果;控制模塊集成有步進(jìn)電機(jī)驅(qū)動(dòng)單元，不需要設(shè)置傳統(tǒng)的大電機(jī)驅(qū)動(dòng)器，減小了機(jī)器人本身的體積，降低功耗;此機(jī)器人能夠進(jìn)一步配置使得具有娛樂(lè)功能。
【附圖說(shuō)明】
[0009]圖1為本發(fā)明的整體外觀結(jié)構(gòu)圖；
圖2為本發(fā)明的系統(tǒng)組成框圖。
[0010]符號(hào)說(shuō)明:1-機(jī)身，2-滾輪，3-步進(jìn)電機(jī)，4-活動(dòng)臂，5-加速度計(jì)，6-陀螺儀，7_控制模塊，8-無(wú)線數(shù)據(jù)傳輸模塊，9-環(huán)境傳感器，I O-感受傳感器。
【具體實(shí)施方式】
[0011]下面參照附圖并結(jié)合實(shí)施例對(duì)本發(fā)明作進(jìn)一步的描述。
[0012]參照?qǐng)D1，本發(fā)明的直立平衡機(jī)器人整體為人形造型，外觀上包括機(jī)身1、兩個(gè)滾輪2以及兩組分別用于控制滾輪2轉(zhuǎn)動(dòng)的步進(jìn)電機(jī)3。機(jī)身I整體成直立狀態(tài)。工作時(shí)，步進(jìn)電機(jī)3控制兩個(gè)滾輪2不同的轉(zhuǎn)速實(shí)現(xiàn)機(jī)器人直立平衡移動(dòng)和轉(zhuǎn)動(dòng)。另外，機(jī)身I的兩側(cè)還設(shè)有可活動(dòng)的活動(dòng)臂4，在控制下活動(dòng)臂4能夠?qū)崿F(xiàn)各種動(dòng)作。
[0013]如圖2所示，機(jī)器人內(nèi)設(shè)有加速度計(jì)5、陀螺儀6、控制模塊7等。加速度計(jì)5用于檢測(cè)機(jī)身的角度值，陀螺儀6用于檢測(cè)機(jī)身的角速度值。控制模塊7內(nèi)集成有步進(jìn)電機(jī)驅(qū)動(dòng)單元，因此控制模塊7能夠直接驅(qū)動(dòng)步進(jìn)電機(jī)3工作，不需要設(shè)置額外的驅(qū)動(dòng)器。工作時(shí)，控制模塊7基于加速度計(jì)5和陀螺儀6的檢測(cè)量經(jīng)過(guò)濾波、計(jì)算后得到角度值和角速度值，利用角度值和角速度值進(jìn)行H)控制產(chǎn)生兩個(gè)用于控制電機(jī)轉(zhuǎn)動(dòng)的輸出量，通過(guò)步進(jìn)電機(jī)驅(qū)動(dòng)單元控制步進(jìn)電機(jī)工作。
[0014]實(shí)際機(jī)器人中使用的角度和角速度計(jì)算方法如下: float Angle = 0， Angle_dot = O;
float Q—angIe=0.001， Q_gyro=0.003, R—angle=0.67， dt=0.0025;
float P[2][2] ={{ 1， 0 }，{ 0， I }};
char C—0 = I;
float E=O;
float q—bias = 0;
float Angle_err = 0;
float PCt—0 = 0， PCt—I = 0;
float K—0 = 0， K—I = 0;
float t—0 = 0， t—I = 0;
float Pdot[4] =(0,0,0,0};
void filter(f1at angle—m，f1at gyro—m)
{
Angle += (gyro—m_q—bias) * dt;
Pdot[0]=Q—angle - P[0][I] - P[l][0];
Pdot[I]=- P[l][l];
Pdot[2]=- P[l][l];
Pdot[3]=Q—gyro;
P[0][0] += Pdot[0] * dt;
P[0][1] += Pdot[I] * dt;
P[1][0] += Pdot[2] * dt;
P[l][l] += Pdot[3] * dt;
Angle_err = angle—m - Angle;
PCt—0 = C—0 * P[0][0];
PCt—I = C—0 * P[1][0]; E = R_angle + C_0 * PCt_0;
K_0 = PCt_0 / E;
K_1 = PCt_l / E;
t_0 = PCt_0;
t_l = C_0 * P[0][1];
P[0][0] -= K_0 * t_0;
P[0][1] -= K_0 * t_l;
P[1][0] -= K_1 * t_0;
P[l][l] -= K_1 * t_l;
Angle += K_0 * Angle_err; q_bias+= K_1 * Angle_err;
Angle_dot = gyro_m-q_bias;
}
其中，函數(shù)形參為傳感器測(cè)得元數(shù)據(jù)(經(jīng)過(guò)單位變換),Angle和Angle_dot為最終濾波輸出結(jié)果，分別為角度和角速度。dt為濾波器采樣時(shí)間，其余均為中間變量。
[0015]在得到了具體的角度和角速度信息后，采用典型的H)控制產(chǎn)生輸出量。
[0016]duty_PD=Kp_AngIe *AngIe_Err+Kd_AngIe * Angle_V;
其中Angle_Err為角度偏差，Angle_V為角速度。
[0017]再仔細(xì)調(diào)整Kp_Angle和Kd_Angle，機(jī)器人便可以實(shí)現(xiàn)直立。
[0018]本發(fā)明的機(jī)器人可以通過(guò)內(nèi)置更多的模塊以實(shí)現(xiàn)更好的教學(xué)和娛樂(lè)功能。
[0019]例如，機(jī)身內(nèi)可以設(shè)有用于與外部通訊的無(wú)線數(shù)據(jù)傳輸模塊8，無(wú)線數(shù)據(jù)傳輸模塊8與控制模塊7電氣連接。利用此無(wú)線數(shù)據(jù)傳輸模塊8可以方便地與外部進(jìn)行通訊，外部可以向機(jī)器人傳輸控制命令，控制機(jī)器人動(dòng)作。
[0020]又例如，機(jī)身的外部可以設(shè)有溫度傳感器、濕度傳感器、壓力傳感器和聲音傳感器等環(huán)境傳感器9，溫度傳感器、濕度傳感器、壓力傳感器和聲音傳感器的輸出端與控制模塊7連接。利用上述各種傳感器可以對(duì)機(jī)器人所處環(huán)境進(jìn)行檢測(cè)，檢測(cè)的數(shù)據(jù)送到控制模塊7內(nèi)，從而控制機(jī)器人實(shí)現(xiàn)各種動(dòng)作。另外，這些數(shù)據(jù)也可以通過(guò)無(wú)線數(shù)據(jù)傳輸模塊8向外傳輸，起到利用機(jī)器人探測(cè)環(huán)境的作用。
[0021 ]再例如，機(jī)身的外部可以設(shè)有用于感應(yīng)其他機(jī)器人的感受傳感器1，感受傳感器1的輸出端與控制模塊7連接。當(dāng)機(jī)器人感應(yīng)到其他機(jī)器人時(shí)可以做出相應(yīng)的相應(yīng)動(dòng)作。如活動(dòng)臂上安裝有聲光槍，機(jī)身正面感應(yīng)到其他機(jī)器人時(shí)聲光槍點(diǎn)亮，以實(shí)現(xiàn)模擬對(duì)戰(zhàn)的功會(huì)K。
[0022]以上所述僅為本發(fā)明的優(yōu)選實(shí)施例而已，并不用于限制本發(fā)明，對(duì)于本領(lǐng)域的技術(shù)人員來(lái)說(shuō)，本發(fā)明可以有各種更改和變化。凡在本發(fā)明的精神和原則之內(nèi)，所作的任何修改、等同替換、改進(jìn)等，均應(yīng)包含在本發(fā)明的保護(hù)范圍之內(nèi)。
【主權(quán)項(xiàng)】
1.直立平衡機(jī)器人，其特征在于:包括整體成直立狀態(tài)的機(jī)身，機(jī)身的內(nèi)部設(shè)有用于檢測(cè)角度值的加速度計(jì)、用于檢測(cè)角速度值的陀螺儀以及集成步進(jìn)電機(jī)驅(qū)動(dòng)單元的控制模塊，機(jī)身下端的兩側(cè)設(shè)有用于與地面接觸的兩個(gè)滾輪，機(jī)身內(nèi)部設(shè)有分別單獨(dú)驅(qū)動(dòng)兩個(gè)滾輪轉(zhuǎn)動(dòng)的兩組步進(jìn)電機(jī)，所述機(jī)身的兩側(cè)設(shè)有受控制模塊控制活動(dòng)的活動(dòng)臂，所述控制模塊基于加速度計(jì)和陀螺儀的檢測(cè)量經(jīng)過(guò)濾波、計(jì)算后得到角度值和角速度值，利用角度值和角速度值進(jìn)行ro控制產(chǎn)生兩個(gè)用于控制電機(jī)轉(zhuǎn)動(dòng)的輸出量，通過(guò)步進(jìn)電機(jī)驅(qū)動(dòng)單元控制步進(jìn)電機(jī)工作。2.根據(jù)權(quán)利要求1所述的直立平衡機(jī)器人，其特征在于:所述用于與外部通訊的無(wú)線數(shù)據(jù)傳輸模塊，無(wú)線數(shù)據(jù)傳輸模塊與控制模塊電氣連接。3.根據(jù)權(quán)利要求1所述的直立平衡機(jī)器人，其特征在于:所述機(jī)身的外部設(shè)有溫度傳感器、濕度傳感器、壓力傳感器和聲音傳感器，溫度傳感器、濕度傳感器、壓力傳感器和聲音傳感器的輸出端與控制模塊連接。4.根據(jù)權(quán)利要求1所述的直立平衡機(jī)器人，其特征在于:所述機(jī)身的外部設(shè)有用于感應(yīng)其他機(jī)器人的感受傳感器，感受傳感器的輸出端與控制模塊連接。5.根據(jù)權(quán)利要求4所述的直立平衡機(jī)器人，其特征在于:所述活動(dòng)臂上安裝有聲光槍，機(jī)身正面感應(yīng)到其他機(jī)器人時(shí)聲光槍點(diǎn)亮。
【文檔編號(hào)】B25J9/16GK106078739SQ201610446679
【公開(kāi)日】2016年11月9日
【申請(qǐng)日】2016年6月21日
【發(fā)明人】武行峰
【申請(qǐng)人】合肥羿振電力設(shè)備有限公司