[發明專利]一種自平衡機器人的位置控制方法

在審

• 申請號: 201710515331.2
• 申請日: 2017-06-29
• 公開（公告）號: CN109202883A
• 公開（公告）日: 2019-01-15
• 發明（設計）人: 鄒風山;祁跡;劉世昌;宋吉來;趙金野;張彥超
• 申請（專利權）人: 沈陽新松機器人自動化股份有限公司
• 主分類號: B25J9/16
• 分類號: B25J9/16;B25J13/08
• 代理公司: 沈陽科苑專利商標代理有限公司 21002
• 代理人: 許宗富

1.一種基于電機編碼的自平衡機器人的位置控制方法，其特征在于，所述方法包括：

步驟一：對自平衡機器人的各個模塊進行初始化操作；

步驟二：獲取自平衡機器人的傳感數據并且進行姿態解算；

步驟三：將步驟二中姿態解算的姿態信息以及自平衡機器人的位置信息作為自平衡位置控制器的輸入信息，進行自平衡機器人的平衡狀態判斷，從而確定是否更改控制策略；

步驟四：執行模塊接收并且執行步驟三中自平衡位置控制器所輸出的控制策略；所述執行模塊包括電機、驅動輪、以及用于實時反饋自平衡機器人的位置信息的編碼器，所述電機連接所述驅動輪，所述編碼器連接所述電機，所述編碼器將所述電機旋轉產生的角度變化轉變成自身的碼數變化，進而產生電信號的變化，所述電信號的變化作為自平衡機器人的位置信息反饋給所述自平衡位置控制器。

2.根據權利要求1所述的一種基于電機編碼的自平衡機器人的位置控制方法，其特征在于，步驟一中所述的模塊初始化包括控制板卡初始化、傳感器初始化、自平衡機器人重心初始化、執行模塊初始化。

3.根據權利要求2述的一種基于電機編碼的自平衡機器人的位置控制方法，其特征在于，所述控制板卡初始化包括主控時鐘初始化、中斷系統初始化、各個通信模塊初始化。

4.根據權利要求1所述的一種基于電機編碼的自平衡機器人的位置控制方法，其特征在于，用于獲取自平衡機器人的傳感數據的傳感器為微機電六軸運動傳感器，包括加速度計和陀螺儀。

5.根據權利要求4述的一種基于電機編碼的自平衡機器人的位置控制方法，其特征在于，對所述六軸運動傳感器所測的傳感數據進行卡爾曼濾波進行姿態解算。

6.根據權利要求4述的一種基于電機編碼的自平衡機器人的位置控制方法，其特征在于，所述姿態解算的步驟包括利用陀螺儀所獲取的角速度構建卡爾曼狀態方程，利用加速度計獲取各軸加速度分量并計算獲取角度而構建卡爾曼測量方程，建立離散化卡爾曼系統方程，計算獲得姿態解算結果。

7.根據權利要求1所述的一種基于電機編碼的自平衡機器人的位置控制方法，其特征在于，所述自平衡控制器采用閉環PID控制。

8.根據權利要求7所述的一種基于電機編碼的自平衡機器人的位置控制方法，其特征在于，將所述編碼器所反應自平衡機器人的實時位置信息作為自平衡控制器的反饋量，通過位置迭代的方式得到自平衡控制的控制量，將控制量和反饋量的差作為閉環PID控制的輸入量。

9.根據權利要求8所述的一種基于電機編碼的自平衡機器人的位置控制方法，其特征在于，所述位置迭代的方式包括：將當前位置和變化位置作和后迭代更新為下一次的當前位置，所述變化位置包括位置前進、位置后退和位置靜止。

10.根據權利要求1所述的一種基于電機編碼的自平衡機器人的位置控制方法，其特征在于，所述編碼器的碼數變化與所述自平衡機器人的位置變化的對應計算公式為：

其中，

自平衡機器人在初始位置時編碼器的碼數為A，電機旋轉α角度且帶動驅動輪運動L_AB距離后，編碼器的碼數變為B；其中R為驅動輪半徑，M為碼盤一圈總數。