直立平衡机器人的制作方法
【专利摘要】本发明公开了一种直立平衡机器人，包括机身，机身的内部设有加速度计、陀螺仪以及集成步进电机驱动单元的控制模块，机身下端的两侧设有两个滚轮，机身内部设有两组步进电机，机身的两侧设有活动臂，控制模块基于加速度计和陀螺仪的检测量经过滤波、计算后得到角度值和角速度值，利用角度值和角速度值进行PD控制产生两个用于控制电机转动的输出量，通过步进电机驱动单元控制步进电机工作。本发明通过控制两个电机的转动能够精确地控制机器人保持直立平衡；其整体为人形造型，具有仿生外形效果；控制模块集成有步进电机驱动单元，不需要设置传统的大电机驱动器，减小了机器人本身的体积，降低功耗；此机器人能够进一步配置使得具有娱乐功能。
【专利说明】
直立平衡机器人
技术领域
[0001]本发明涉及电子技术领域，具体涉及一种可供教学使用的直立平衡机器人。
【背景技术】
[0002]现有技术中并无一种专门针对教学使用的专用直立机器人。因此，有必要开发一种相应的机器人产品，以供学生快速学习机器人技术使用。
【发明内容】
[0003]为了实现以上目的，本发明提供一种直立平衡机器人，其能够同时满足教学和娱乐的使用要求。
[0004]本发明是通过以下技术方案实现的:
直立平衡机器人，包括整体成直立状态的机身，机身的内部设有用于检测角度值的加速度计、用于检测角速度值的陀螺仪以及集成步进电机驱动单元的控制模块，机身下端的两侧设有用于与地面接触的两个滚轮，机身内部设有分别单独驱动两个滚轮转动的两组步进电机，所述机身的两侧设有受控制模块控制活动的活动臂，所述控制模块基于加速度计和陀螺仪的检测量经过滤波、计算后得到角度值和角速度值，利用角度值和角速度值进行F1D控制产生两个用于控制电机转动的输出量，通过步进电机驱动单元控制步进电机工作。
[0005]作为优选的实施方式，所述用于与外部通讯的无线数据传输模块，无线数据传输模块与控制模块电气连接。
[0006]作为优选的实施方式，所述机身的外部设有温度传感器、湿度传感器、压力传感器和声音传感器，温度传感器、湿度传感器、压力传感器和声音传感器的输出端与控制模块连接。
[0007]作为优选的实施方式，所述机身的外部设有用于感应其他机器人的感受传感器，感受传感器的输出端与控制模块连接。进一步，所述活动臂上安装有声光枪，机身正面感应到其他机器人时声光枪点亮。
[0008]本发明的有益效果是:本发明通过控制两个电机的转动能够精确地控制机器人保持直立平衡;其整体为人形造型，具有仿生外形效果;控制模块集成有步进电机驱动单元，不需要设置传统的大电机驱动器，减小了机器人本身的体积，降低功耗;此机器人能够进一步配置使得具有娱乐功能。
【附图说明】
[0009]图1为本发明的整体外观结构图。
[0010]图2为本发明的系统组成框图。
[0011 ] 符号说明:1-机身，2-滚轮，3-步进电机，4-活动臂，5-加速度计，6-陀螺仪，7_控制模块，8-无线数据传输模块，9-环境传感器，I O-感受传感器。
【具体实施方式】
[0012]下面参照附图并结合实施例对本发明作进一步的描述。
[0013]参照图1，本发明的直立平衡机器人整体为人形造型，外观上包括机身1、两个滚轮2以及两组分别用于控制滚轮2转动的步进电机3。机身I整体成直立状态。工作时，步进电机3控制两个滚轮2不同的转速实现机器人直立平衡移动和转动。另外，机身I的两侧还设有可活动的活动臂4，在控制下活动臂4能够实现各种动作。
[0014]如图2所示，机器人内设有加速度计5、陀螺仪6、控制模块7等。加速度计5用于检测机身的角度值，陀螺仪6用于检测机身的角速度值。控制模块7内集成有步进电机驱动单元，因此控制模块7能够直接驱动步进电机3工作，不需要设置额外的驱动器。工作时，控制模块7基于加速度计5和陀螺仪6的检测量经过滤波、计算后得到角度值和角速度值，利用角度值和角速度值进行H)控制产生两个用于控制电机转动的输出量，通过步进电机驱动单元控制步进电机工作。
[0015]实际机器人中使用的角度和角速度计算方法如下: 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;
}
其中，函数形参为传感器测得元数据(经过单位变换),Angle和Angle_dot为最终滤波输出结果，分别为角度和角速度。dt为滤波器采样时间，其余均为中间变量。
[0016]在得到了具体的角度和角速度信息后，采用典型的H)控制产生输出量。
[0017]duty_PD=Kp_AngIe *AngIe_Err+Kd_AngIe * Angle_V;
其中Angle_Err为角度偏差，Angle_V为角速度。
[0018]再仔细调整Kp_Angle和Kd_Angle，机器人便可以实现直立。
[0019]本发明的机器人可以通过内置更多的模块以实现更好的教学和娱乐功能。
[0020]例如，机身内可以设有用于与外部通讯的无线数据传输模块8，无线数据传输模块8与控制模块7电气连接。利用此无线数据传输模块8可以方便地与外部进行通讯，外部可以向机器人传输控制命令，控制机器人动作。
[0021]又例如，机身的外部可以设有温度传感器、湿度传感器、压力传感器和声音传感器等环境传感器9，温度传感器、湿度传感器、压力传感器和声音传感器的输出端与控制模块7连接。利用上述各种传感器可以对机器人所处环境进行检测，检测的数据送到控制模块7内，从而控制机器人实现各种动作。另外，这些数据也可以通过无线数据传输模块8向外传输，起到利用机器人探测环境的作用。
[0022]再例如，机身的外部可以设有用于感应其他机器人的感受传感器10，感受传感器1的输出端与控制模块7连接。当机器人感应到其他机器人时可以做出相应的相应动作。如活动臂上安装有声光枪，机身正面感应到其他机器人时声光枪点亮，以实现模拟对战的功會K。
[0023]以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
【主权项】
1.直立平衡机器人，其特征在于:包括整体成直立状态的机身，机身的内部设有用于检测角度值的加速度计、用于检测角速度值的陀螺仪以及集成步进电机驱动单元的控制模块，机身下端的两侧设有用于与地面接触的两个滚轮，机身内部设有分别单独驱动两个滚轮转动的两组步进电机，所述机身的两侧设有受控制模块控制活动的活动臂，所述控制模块基于加速度计和陀螺仪的检测量经过滤波、计算后得到角度值和角速度值，利用角度值和角速度值进行ro控制产生两个用于控制电机转动的输出量，通过步进电机驱动单元控制步进电机工作。2.根据权利要求1所述的直立平衡机器人，其特征在于:所述用于与外部通讯的无线数据传输模块，无线数据传输模块与控制模块电气连接。3.根据权利要求1所述的直立平衡机器人，其特征在于:所述机身的外部设有温度传感器、湿度传感器、压力传感器和声音传感器，温度传感器、湿度传感器、压力传感器和声音传感器的输出端与控制模块连接。4.根据权利要求1所述的直立平衡机器人，其特征在于:所述机身的外部设有用于感应其他机器人的感受传感器，感受传感器的输出端与控制模块连接。5.根据权利要求4所述的直立平衡机器人，其特征在于:所述活动臂上安装有声光枪，机身正面感应到其他机器人时声光枪点亮。
【文档编号】G05D1/02GK106054885SQ201610470008
【公开日】2016年10月26日
【申请日】2016年6月26日
【发明人】李阳
【申请人】合肥仁德电子科技有限公司