[发明专利]一种多源传感器数据融合的自平衡机器人控制系统和控制方法

权利要求书

1.一种基于多源传感器数据融合的自平衡机器人控制系统，其特征在于：包括MPU6050模块和磁力计，所述MPU6050模块包括加速度计和陀螺仪,所述MPU6050模块和磁力计与电机通讯连接。

2.一种基于多源传感器数据融合的自平衡机器人控制方法，应用权利要求1所述的基于多源传感器数据融合的自平衡机器人控制系统，其特征在于包括以下步骤：

步骤一：获取加速度计数据和磁力计数据；

步骤二：根据加速度计数据和磁力计数据计算初步姿态；

步骤三：获取陀螺仪数据并转换为旋转矢量，然后与初步姿态融合，得到准确的机器人姿态；

步骤四：通过准确的机器人姿态和电机编码器反馈的速度控制电机的转动以控制机器人姿态。

3.根据权利要求2所述的基于多源传感器数据融合的自平衡机器人控制方法，其特征在于：所述的步骤二还包括：将加速度计数据进行滑动权值滤波处理，计算公式为：

其中，Acc(i)为加速度计原始数据，单位为m/s²，ACC(i)为融合后的加速度计数据，单位为m/s²，i为时间单位，由处理器采集传感器数据的速率决定。

4.根据权利要求2或3所述的基于多源传感器数据融合的自平衡机器人控制方法，其特征在于：所述的步骤二具体为：

步骤201：通过加速度计的数据求出捷联矩阵，根据捷联矩阵转换加速度计数据的坐标，计算公式为：

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

其中，为机体坐标转换为世界坐标系的矩阵，a_n为重力加速度在世界坐标系下的表达方式，a_b为重力加速度在机体坐标系下的表达方式，a_x,a_y,a_z为重力加速度在机体坐标系下三个轴的分量；

步骤202：根据转换后的加速计数据坐标和粗略姿态计算俯仰角和横滚角，计算公式为：

其中，θ为俯仰角，γ为横滚角，ψ为航位角；θ₁，γ₁为通过加速度计算出的俯仰角和横滚角；

步骤203：融合磁力计数据计算航位角，计算公式为：

其中，磁力计获得的数据为m_b＝(m_x,m_y,m_z)，绝对坐标系下磁力计的磁场强度为m_n＝[0，m_N,m_D]，ψ₁为融合磁力计后算出的航位角。

5.根据权利要求4所述的基于多源传感器数据融合的自平衡机器人控制方法，其特征在于：所述的步骤三具体为：

步骤301：根据四元数公式将原始的陀螺仪数据转换为旋转矢量，计算公式为：

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

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

其中，α＝(α₁,α₂,α₃)为参考坐标系绕矢量旋转后与机体坐标系重合的旋转角度，Q为四元数，q₀,q₁,q₂,q₃为四元数四个分量，i,j,k为Q的矢量形式的分量；

步骤302：根据旋转矢量通过捷联矩阵计算姿态角，计算公式为：

其中，θ₂，γ₂，ψ₂为融合陀螺仪数据之后计算出的俯仰角，横滚角，航位角。