[發(fā)明專利]一種多源傳感器數(shù)據(jù)融合的自平衡機器人控制系統(tǒng)和控制方法

在審

• 申請?zhí)枺?/td>: 201811244545.1
• 申請日: 2018-10-24
• 公開（公告）號: CN109108936A
• 公開（公告）日: 2019-01-01
• 發(fā)明（設(shè)計）人: 彭倍;徐源正;代小林;于慧君;魏敦文
• 申請（專利權(quán)）人: 電子科技大學(xué)
• 主分類號: B25J5/00
• 分類號: B25J5/00;B25J9/16;B25J13/00
• 代理公司: 成都睿道專利代理事務(wù)所(普通合伙) 51217
• 代理人: 薛波

1.一種基于多源傳感器數(shù)據(jù)融合的自平衡機器人控制系統(tǒng)，其特征在于：包括MPU6050模塊和磁力計，所述MPU6050模塊包括加速度計和陀螺儀,所述MPU6050模塊和磁力計與電機通訊連接。

2.一種基于多源傳感器數(shù)據(jù)融合的自平衡機器人控制方法，應(yīng)用權(quán)利要求1所述的基于多源傳感器數(shù)據(jù)融合的自平衡機器人控制系統(tǒng)，其特征在于包括以下步驟：

步驟一：獲取加速度計數(shù)據(jù)和磁力計數(shù)據(jù)；

步驟二：根據(jù)加速度計數(shù)據(jù)和磁力計數(shù)據(jù)計算初步姿態(tài)；

步驟三：獲取陀螺儀數(shù)據(jù)并轉(zhuǎn)換為旋轉(zhuǎn)矢量，然后與初步姿態(tài)融合，得到準(zhǔn)確的機器人姿態(tài)；

步驟四：通過準(zhǔn)確的機器人姿態(tài)和電機編碼器反饋的速度控制電機的轉(zhuǎn)動以控制機器人姿態(tài)。

3.根據(jù)權(quán)利要求2所述的基于多源傳感器數(shù)據(jù)融合的自平衡機器人控制方法，其特征在于：所述的步驟二還包括：將加速度計數(shù)據(jù)進行滑動權(quán)值濾波處理，計算公式為：

其中，Acc(i)為加速度計原始數(shù)據(jù)，單位為m/s²，ACC(i)為融合后的加速度計數(shù)據(jù)，單位為m/s²，i為時間單位，由處理器采集傳感器數(shù)據(jù)的速率決定。

4.根據(jù)權(quán)利要求2或3所述的基于多源傳感器數(shù)據(jù)融合的自平衡機器人控制方法，其特征在于：所述的步驟二具體為：

步驟201：通過加速度計的數(shù)據(jù)求出捷聯(lián)矩陣，根據(jù)捷聯(lián)矩陣轉(zhuǎn)換加速度計數(shù)據(jù)的坐標(biāo)，計算公式為：

a_n＝(0,0,g)，a_b＝(a_x,a_y,a_z)：

其中，為機體坐標(biāo)轉(zhuǎn)換為世界坐標(biāo)系的矩陣，a_n為重力加速度在世界坐標(biāo)系下的表達方式，a_b為重力加速度在機體坐標(biāo)系下的表達方式，a_x,a_y,a_z為重力加速度在機體坐標(biāo)系下三個軸的分量；

步驟202：根據(jù)轉(zhuǎn)換后的加速計數(shù)據(jù)坐標(biāo)和粗略姿態(tài)計算俯仰角和橫滾角，計算公式為：

其中，θ為俯仰角，γ為橫滾角，ψ為航位角；θ₁，γ₁為通過加速度計算出的俯仰角和橫滾角；

步驟203：融合磁力計數(shù)據(jù)計算航位角，計算公式為：

其中，磁力計獲得的數(shù)據(jù)為m_b＝(m_x,m_y,m_z)，絕對坐標(biāo)系下磁力計的磁場強度為m_n＝[0，m_N,m_D]，ψ₁為融合磁力計后算出的航位角。

5.根據(jù)權(quán)利要求4所述的基于多源傳感器數(shù)據(jù)融合的自平衡機器人控制方法，其特征在于：所述的步驟三具體為：

步驟301：根據(jù)四元數(shù)公式將原始的陀螺儀數(shù)據(jù)轉(zhuǎn)換為旋轉(zhuǎn)矢量，計算公式為：

Q＝q₀+q₁i+q₂j+q₃k

Q＝cos(α/2)+sin(α/2)i+sin(α/2)j+sin(α/2)k

其中，α＝(α₁,α₂,α₃)為參考坐標(biāo)系繞矢量旋轉(zhuǎn)后與機體坐標(biāo)系重合的旋轉(zhuǎn)角度，Q為四元數(shù)，q₀,q₁,q₂,q₃為四元數(shù)四個分量，i,j,k為Q的矢量形式的分量；

步驟302：根據(jù)旋轉(zhuǎn)矢量通過捷聯(lián)矩陣計算姿態(tài)角，計算公式為：

其中，θ₂，γ₂，ψ₂為融合陀螺儀數(shù)據(jù)之后計算出的俯仰角，橫滾角，航位角。