CN105680736B - 一种基于偏航角的双电机速度同步及平衡控制方法 - Google Patents

Abstract

本发明涉及一种基于偏航角的双电机速度同步及平衡控制方法，用到一种包括微控制器、两个电机、电机调速器、姿态传感器和PID控制器的双电机控制***，***上电后姿态传感器电机的角速度信号和加速度信号并输出pitch角度和yaw角度两个姿态角度信号至微控制器，微控制器对pitch角度和yaw角度进行标幺化处理，将pitch角度标幺值输出至PID控制器，将对前后两个yaw角度比较所得偏离值输出至PID控制器，pitch角度标幺值和偏离值通过PID控制器运算，输出给电机调速器转换成PWM信号，进而对两个电机进行速度同步和平衡控制。本发明对双电机的速度同步控制简单高效，可避免使用电子罗盘，成本低。

Description

一种基于偏航角的双电机速度同步及平衡控制方法

技术领域

本发明涉及一种双轮电动平衡车中两个驱动电机速度同步的实现方法，具体地说是一种基于姿态传感器偏航角的双电机速度同步及平衡方法，属于电动平衡车控制技术领域。

背景技术

双轮电动平衡车是目前新兴的一种短途交通工具，它主要是基于车体内部姿态传感器(包括陀螺仪和加速度传感器等)所检测到的车体姿态变化，通过控制***控制两个驱动电机来实现车体运动中的两轮速度同步和车体平衡。通常，为了使得双轮电动平衡车在运动时的航向不发生偏转，需要保持两轮的驱动电机在速度上同步，现有的速度控制方法是对速度低的一边车轮上的驱动电机采用适当的PID参数进行增大输出调整，然而这种控制方法无法克服双电机速度做同步时航向偏移的已然发生性以及在低速度情况下测量电机速度不准所导致的控制***整体稳定性的问题。

姿态传感器包含陀螺仪、加速度计和电子罗盘等运动传感器，通过内嵌的低功耗ARM处理器得到经过温度补偿的三维姿态与方位等数据，利用基于四元数的三维算法和特殊数据融合技术，实时输出以四元数或欧拉角或旋转矩阵表示的零漂移三维姿态方位数据。姿态传感器通过配置数字运动处理单元(DMP)并经过四元数转换最终可输出包括有俯仰角(pitch)、偏航角(yaw)和滚转角(roll)的欧拉角，欧拉角作为姿态传感器输出的姿态角能够直观的描述物体的空间姿态，在现有的电动平衡车中用到其中的pitch角度，如绕pitch角度为正则x轴指向下，为了使pitch角度为零，则给输出方向(右手定则)旋转即可。

发明内容

本发明的目的在于提供一种基于偏航角的双电机速度同步及平衡控制方法，在现有的速度同步控制***中采用姿态传感器输出的欧拉角结合PID控制对两个电机进行速度同步及平衡控制，具有控制简单高效的特点，同时减少了电子罗盘(即磁力计)的使用，降低了成本。

为了实现上述目的，本发明采用的技术方案为，一种基于偏航角的双电机速度同步及平衡控制方法，用到一种双电机控制***，该***包括微控制器、两个电机、电机调速器、姿态传感器和PID控制器，所述姿态传感器上整合有陀螺仪和加速度计；在***上电后，所述姿态传感器高速A/D采样当前信号并经四元数转换后实时输出pitch角度和yaw角度两个姿态角度信号至微控制器，微控制器对pitch角度和yaw角度进行标幺化处理，并将pitch角度标幺值输出至PID控制器，微控制器对前后两个yaw角度标幺值进行比较后输出一个偏离值θ，并将偏离值θ输出至PID控制器，pitch角度标幺值和偏离值θ通过PID控制器运算，输出给电机调速器转换成PWM信号，进而对两个电机进行速度同步和平衡控制。

其中，所述微控制器将第一次接收到的yaw角度经过标幺化处理后的结果记载为A，将第二次及以后接收到的yaw角度经过标幺化处理后的结果与A进行比较得到偏离值θ，并将该偏离值θ与预先设定的角度极小量ε做比较，当所得偏离值θ大于该角度极小量ε时微控制器向PID控制器输送当前的偏离值θ，而当所得偏离值θ不大于该角度极小量ε时，微控制器一方面向PID控制器输送数值零，另一方面将当前的yaw角度赋值于A。

所述PID控制器包括第一PID模块、第二PID模块和第三PID模块，将两个电机分别记为A电机和B电机，在第一PID模块的输入端设有第一比较器，而在第一PID模块的输出端设有第二比较器，在第二PID模块的输入端设有第三比较器，而在第二PID模块的输出端设有第四比较器，在第三PID模块的输入端设有第五比较器，所述第一、三、五比较器的输入端连接微控制器的输出端，所述第二比较器的输出端连接A电机的电机调速器(记为A电机调速器)，所述第四比较器的输出端连接B电机的电机调速器(记为B电机调速器)，所述A电机调速器的输出端连接第一比较器的输入端，所述B电机调速器的输出端连接第三比较器的输入端，所述第三PID模块的输出端连接第二、四、五比较器的输入端。

因此，上述控制方法具体包括以下步骤：

a、***上电后，所述姿态传感器采集A电机和B电机的当前信号并经过四元数转换后输出pitch角度和yaw角度两个姿态角度信号至微控制器；

b、所述微控制器对pitch角度和yaw角度进行标幺化处理后将pitch角度标幺值输出至第一、三比较器，将对yaw角度进行判断比较所得的偏离值θ或数值零输出至第五比较器；

c、所述第一比较器将pitch角度标幺值与从A电机调速器反馈回来的A电机速度值进行比较处理后将所得pitch角度偏差值传送给第一PID模块进行相应的PID转换输出一个数值至第二比较器；所述第三比较器将pitch角度标幺值与从B电机调速器反馈回来的B电机速度值进行比较处理后将所得pitch角度偏差值传送给第二PID模块进行相应的PID转换输出一个数值至第四比较器；

d、所述第五比较器将偏离值θ或数值零与从第三PID模块反馈回来的电信号进行比较处理后将所得结果传送给第三PID模块进行相应的PID转换输出一个电信号至第二、四比较器；

e、所述第二比较器将比较结果输出至A电机调速器转换成PWM信号传送给A电机，所述第四比较器将比较结果输出至B电机调速器转换成PWM信号传送给B电机，进而对A电机和B电机的速度进行同步和平衡控制。

作为本发明的一种改进，所述角度极小量ε设为0.005°。

相对于现有技术，本发明的优点如下，本方法是在现有的双电机控制***中通过姿态传感器输出的pitch角度和yaw角度两个姿态角对两个电机进行速度同步和航向漂移矫正控制，其控制精度高，算法简单高效，易于实现且成本低，通过高效益地利用姿态传感器，避免了低速情况下电机转速测速不准所导致的相关速度同步的伴随性问题，同时也减少了电子罗盘的使用，不仅降低了成本，而且也避免了因电子罗盘的使用而给电路引入的电磁干扰。

附图说明

图1为本发明中对偏航角yaw进行处理的流程图。

图2为本发明的控制流程图。

具体实施方式

为了加深对本发明的理解和认识，下面结合附图对本发明作进一步描述和介绍。

一种基于偏航角的双电机速度同步及平衡控制方法，该方法用到双轮电动平衡车的双电机速度同步控制***，该双轮电动平衡车包括两个驱动轮、为驱动轮提供驱动力的驱动电机、带微控制器、PID控制器和电机调速器的控制***以及姿态传感器。所述姿态传感器中整合有三轴陀螺仪和三轴加速度计；双轮电动平衡车上电后，姿态传感器采集驱动轮的速度信号并经过四元数转换实时输出pitch角度和yaw角度两个姿态角度信号至微控制器，微控制器对pitch角度和yaw角度进行标幺化处理，并将pitch角度标幺值输出至PID控制器，微控制器对前后两个yaw角度标幺值进行比较后输出一个偏离值θ，并将偏离值θ输出至PID控制器，pitch角度标幺值和偏离值θ通过PID控制器运算，输出给电机调速器转换成PWM信号，进而对两个电机进行速度同步和平衡控制。

如图1所示，所述微控制器将第一次接收到的yaw角度经过标幺化处理后的结果记载为A，将第二次及以后接收到的yaw角度经过标幺化处理后的结果与A进行比较得到偏离值θ。并将该偏离值θ与预先设定的角度极小量ε＝0.005°做比较，当所得偏离值θ大于该角度极小量ε时微控制器向PID控制器输送当前的偏离值θ，而当所得偏离值θ不大于该角度极小量ε时，微控制器一方面向PID控制器输送数值零，另一方面将当前的yaw角度赋值于A。当双轮电动平衡车需要转向时，通过改变上电时的yaw角度即可；由于所采用的姿态传感器中缺少磁力计，双轮电动平衡车在运动过程中的偏航角存在漂移，即yaw角度会随着时间持续增大或减小，因此，在控制***中预设一个角度极小量ε，并在当偏离值θ小于该角度极小量ε时，则定义偏离值θ为0度；与此同时将A伴随性的定义到漂移后的yaw角度。

如图2所示，所述PID控制器包括第一PID模块PID1、第二PID模块PID2和第三PID模块PID3，其中，第一、二PID模块为速度调节PID模块，第三PID模块为航向调节PID模块。将两个驱动电机分别记为A电机和B电机，在第一PID模块的输入端设有第一比较器C1，在第一PID模块的输出端设有第二比较器C2，在第二PID模块的输入端设有第三比较器C3，在第二PID模块的输出端设有第四比较器C4，在第三PID模块的输入端设有第五比较器C5，所述第一、三、五比较器的输入端连接微控制器的输出端，所述第二比较器的输出端连接A电机调速器，所述第四比较器的输出端连接B电机调速器，所述A电机调速器的输出端连接第一比较器的输入端，所述B电机调速器的输出端连接第三比较器的输入端，所述第三PID模块的输出端连接第二、四、五比较器的输入端。

因此，因此，上述控制方法具体可包括以下步骤：

a、***上电后，所述姿态传感器采集当前信号并经过四元数转换后输出pitch角度和yaw角度两个姿态角度信号至微控制器，其中yaw则定义为初始A；

b、所述微控制器对pitch角度和yaw角度进行标幺化处理后将pitch角度标幺值输出至第一、三比较器，将对yaw角度进行判断比较所得的偏离值θ或数值零输出至第五比较器；

c、所述第一比较器将pitch角度标幺值与从A电机调速器反馈回来的A电机速度信号进行比较处理后将所得pitch角度偏差值传送给第一PID模块进行相应的PID转换输出一个数值至第二比较器；所述第三比较器将pitch角度标幺值与从B电机调速器反馈回来的B电机速度信号进行比较处理后将所得pitch角度偏差值传送给第二PID模块进行相应的PID转换输出一个数值至第四比较器；

d、所述第五比较器将偏离值θ或数值零与从第三PID模块反馈回来的电信号进行比较处理后将所得结果传送给第三PID模块进行相应的PID转换输出一个电信号至第二、四比较器；

e、所述第二比较器将比较结果输出至A电机调速器转换成PWM信号传送给A电机，所述第四比较器将比较结果输出至B电机调速器转换成PWM信号传送给B电机，进而对A电机和B电机的速度进行同步和平衡控制，确保两驱动轮的速度同步，进一步保证电动平衡车的左右驱动轮的平衡。

需要说明的是上述具体实施方式，并非用来限定本发明的保护范围，在上述技术方案的基础上所作出的等同变换或替代均落入本发明权利要求所保护的范围。在权利要求中，不应将位于括号之间的任何参考符号构造成对权利要求的限制。单词“包含”不排除存在未列在权利要求中的元件或步骤。单词第一、第二以及第三等的使用不表示任何顺序，可将这些单词解释为名称。

Claims (5)

1.一种基于偏航角的双电机速度同步及平衡控制方法，其特征在于：该方法用到一种双电机控制***，该***包括微控制器、两个电机、电机调速器、姿态传感器和PID控制器，所述姿态传感器上整合有陀螺仪和加速度计；在***上电后，所述姿态传感器高速A/D采样电机的角速度信号和加速度信号并经四元数转换整合后实时输出pitch角度和yaw角度两个姿态角度信号至微控制器，微控制器对pitch角度和yaw角度进行标幺化处理，并将pitch角度标幺值输出至PID控制器，微控制器对前后两个yaw角度标幺值进行比较后输出一个偏离值θ，并将偏离值θ输出至PID控制器，pitch角度标幺值和偏离值θ通过PID控制器运算，输出给电机调速器转换成PWM信号，进而对两个电机进行速度同步和平衡控制。

2.如权利要求1所述的一种基于偏航角的双电机速度同步及平衡控制方法，其特征在于，所述微控制器将第一次接收到的yaw角度经过标幺化处理后的结果记载为A，将第二次及以后接收到的yaw角度经过标幺化处理后的结果与A进行比较得到偏离值θ，并将该偏离值θ与预先设定的角度极小量ε做比较，当所得偏离值θ大于该角度极小量ε时微控制器向PID控制器输送当前的偏离值θ，而当所得偏离值θ不大于该角度极小量ε时，微控制器一方面向PID控制器输送数值零，另一方面将当前的yaw角度赋值于A。

3.如权利要求2所述的一种基于偏航角的双电机速度同步及平衡控制方法，其特征在于，所述PID控制器包括第一PID模块、第二PID模块和第三PID模块，将两个电机分别记为A电机和B电机，在第一PID模块的输入端设有第一比较器，而在第一PID模块的输出端设有第二比较器，在第二PID模块的输入端设有第三比较器，而在第二PID模块的输出端设有第四比较器，在第三PID模块的输入端设有第五比较器，所述第一、三、五比较器的输入端连接微控制器的输出端，所述第二比较器的输出端连接A电机的电机调速器，所述第四比较器的输出端连接B电机的电机调速器，A电机的电机调速器的输出端连接第一比较器的输入端，B电机的电机调速器的输出端连接第三比较器的输入端，所述第三PID模块的输出端连接第二、四、五比较器的输入端。

4.如权利要求3所述的一种基于偏航角的双电机速度同步及平衡控制方法，其特征在于，该控制方法具体包括以下步骤：

a、***上电后，所述姿态传感器采集A电机和B电机的输出速度信号并经过四元数转换后输出pitch角度和yaw角度两个姿态角度信号至微控制器；

b、所述微控制器对pitch角度和yaw角度进行标幺化处理后将pitch角度标幺值输出至第一、三比较器，将对yaw角度进行判断比较所得的偏离值θ或数值零输出至第五比较器；

c、所述第一比较器将pitch角度标幺值与从A电机调速器反馈回来的A电机速度信号进行比较处理后将所得pitch角度偏差值传送给第一PID模块进行相应的PID转换输出一个数值至第二比较器；所述第三比较器将pitch角度标幺值与从B电机调速器反馈回来的B电机速度信号进行比较处理后将所得pitch角度偏差值传送给第二PID模块进行相应的PID转换输出一个数值至第四比较器；

d、所述第五比较器将偏离值θ或数值零与从第三PID模块反馈回来的电信号进行比较处理后将所得结果传送给第三PID模块进行相应的PID转换输出一个电信号至第二、四比较器；

e、所述第二比较器将比较结果输出至A电机调速器转换成PWM信号传送给A电机，所述第四比较器将比较结果输出至B电机调速器转换成PWM信号传送给B电机，进而对A电机和B电机的速度进行同步和平衡控制。

5.如权利要求1或2所述的一种基于偏航角的双电机速度同步及平衡控制方法，其特征在于，所述角度极小量ε设为0.005°。