[发明专利]一种基于偏航角的双电机速度同步及平衡控制方法

权利要求书

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°。