[發明專利]一種基于偏航角的雙電機速度同步及平衡控制方法

有效

• 申請號: 201610141538.3
• 申請日: 2016-03-11
• 公開（公告）號: CN105680736B
• 公開（公告）日: 2018-04-13
• 發明（設計）人: 劉峰;張濤;薛祖播
• 申請（專利權）人: 南京快輪智能科技有限公司
• 主分類號: H02P5/56
• 分類號: H02P5/56;B62K11/00
• 代理公司: 南京眾聯專利代理有限公司32206
• 代理人: 顧進

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