CN109108936A - 一种多源传感器数据融合的自平衡机器人控制系统和控制方法 - Google Patents

Abstract

本发明公开了一种基于多源传感器数据融合的自平衡机器人控制系统和控制方法，包括以下步骤：获取加速度计数据和磁力计数据，然后根据加速度计数据和磁力计数据计算初步姿态；再获取陀螺仪数据并转换为旋转矢量，然后与初步姿态融合，得到准确的机器人姿态，最后通过准确的机器人姿态和电机编码器反馈的速度控制电机的转动以控制机器人姿态。本发明通过姿态角信息进行滤波降噪之后再进行融合和电机编码器反馈的信息控制电机的转动实现对机器人姿态的控制从而实现机器人的自平衡以及运动，得到结合自平衡机器人物理特性、电机响应特性的改进型PID控制方法，能够更准确更稳定的对自平衡机器人实现运动控制。

Description

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

技术领域

本发明属于移动机器人技术领域，具体涉及一种多源传感器数据融合的自平衡机器人控制系统和控制方法。

背景技术

移动机器人目前已经越来越广泛的应用于各个行业中，具有广阔的应用前景。自平衡机器人是一种特殊的轮式移动机器人，其概念最早由日本东京电信大学自动化系的山藤一雄教授于二十世纪八十年代提出。双轮自平衡机器人具有体积小、转弯半径为零、车身灵活等优点，因此适用于以拥挤的城市环境为代表的多种狭窄场景。平衡机器人的技术关键在于能持续保持平衡，并能在平衡中前进、后退、转弯。因此其运动控制技术为自平衡机器人技术的核心，对自平衡机器人运动控制技术的研究有着重要的意义，也有着极大的工程应用价值。

近年来，随着机器人的热潮以及科学技术的发展，机器人的运动控制技术也有了很大的提高。现有技术中运动控制技术多为反馈控制，首先通过传感器的数据获得机器人的姿态信息，之后根据姿态信息进行反馈控制。其中主要使用的是传感器的数据处理技术和PID控制技术，现有技术中一般为将传感器数据进行简易的加权平均计算，并将加权平均后的值作为融合后的最终结果，该方法运算速度快，但不能准确并最大效率利用各传感器数据。现有技术中还有一种方式是采用将惯性/磁力传感器与单目视觉信息融合的SLAM方法对数据进行处理，该方法能够较为准确的获得机器人的姿态信息，但由于传感器数量多且体积较大，数据处理与计算需要工控机之类体积较大的设备，因此不适用于自平衡机器人。PID控制方法是经典控制算法中的典型代表，由美国N.Minorsky在1922年提出，PID控制器由比例单元(P)、积分单元(I)和微分单元(D)组成。其输入e(t)与输出u(t)的关系为u(t)＝kp[e(t)+1/TI∫e(t)dt+TD*de(t)/dt]。其中比例参数kp可以加快系统的响应速度，但比例控制有个缺点，会产生余差，因此引入积分作用，可以消除累积误差。微分作用主要用来克服对象的滞后性。

由PID控制算法衍生出了专家PID、模糊PID、神经网络PID、遗传算法PID、自适应PID控制方法。传统的PID控制算法虽然稳定余度不小，但具有良好的动态品质余度不大，当处在动态环境下时，传统PID算法并不适用。而衍生出的其他几种PID算法则由于计算量大，算法复杂度高，并不适用于自平衡机器人这样体积小，只可以搭载小重量硬件的平台。

发明内容

本发明的目的在于：解决上述现有技术中的不足，提供一种多源传感器数据融合的自平衡机器人控制系统和控制方法，将磁力计、陀螺仪、加速度计、编码器所获得的数据融合，得到结合自平衡机器人物理特性、电机响应特性的改进型PID控制方法，能够更准确更稳定的对自平衡机器人实现运动控制。

为了实现上述目的，本发明采用的技术方案为：

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

一种基于多源传感器数据融合的自平衡机器人控制方法，应用上述的基于多源传感器数据融合的自平衡机器人控制系统，包括以下步骤：

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

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

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

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

进一步的，上述的步骤二还包括：将加速度计数据进行滑动权值滤波处理，计算公式为：

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

进一步的，上述的步骤二具体为：

步骤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]，ψ₁为融合磁力计后算出的航位角。

进一步的，上述的步骤三具体为：

步骤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：根据旋转矢量通过捷联矩阵计算姿态角，计算公式为：

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

进一步的，上述的步骤三中与初步姿态融合的计算公式为：

Z(n)＝a*X(n)+(1-a)*Y(n)

其中，X为加速度计输入，Y为陀螺仪输入，Z为输出，a为滤波系数，a为远小于1的一个数，即低通滤波器，(1-a)为高通滤波器。

进一步的，上述的步骤四具体为：

将准确的机器人姿态解算并进行直立控制，同时根据反馈的自平衡机器人速度通过速度闭环控制器进行速度控制，计算公式为：

a＝k_p*tan(θ)+k_d*[tan(θ)]'-[k_p1*e(k)+k_i1*∑e(k)]

其中，k_p为角度控制器比例系数，k_d为角度控制器微分系数，k_p1为速度控制比例系数，k_i1为速度控制器积分系数

进一步的，上述的直立控制采用PD控制，速度控制为PI控制。

由于采用了上述技术方案，本发明的有益效果是：

本发明的基于多源传感器数据融合的自平衡机器人控制方法通过加速度计、陀螺仪以及外接的磁力计获得姿态角信息，然后将姿态角信息进行滤波降噪之后再进行融合，得到自平衡机器人的姿态信息，并通过电机编码器反馈的信息控制电机的转动实现对机器人姿态的控制从而实现机器人的自平衡以及运动，得到结合自平衡机器人物理特性、电机响应特性的改进型PID控制方法，能够更准确更稳定的对自平衡机器人实现运动控制。

附图说明

为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。

图1为本发明的自平衡机器人结构示意图。

图2为本发明的数据融合流程示意图。

图3为本发明的数据融合细节流程示意图。

图4为本发明的自平衡机器人受力示意图。

图5为本发明的电机响应曲线示意图。

图6为本发明的自平衡机器人控制流程示意图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。

因此，以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

参照附图1-6，对本发明的实施方式做具体的说明。

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

一种基于多源传感器数据融合的自平衡机器人控制方法，应用上述的基于多源传感器数据融合的自平衡机器人控制系统，包括以下步骤：

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

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

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

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

进一步的，上述的步骤二还包括：将加速度计数据进行滑动权值滤波处理，计算公式为：

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

进一步的，上述的步骤二具体为：

步骤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]，ψ₁为融合磁力计后算出的航位角。

进一步的，上述的步骤三具体为：

步骤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：根据旋转矢量通过捷联矩阵计算姿态角，计算公式为：

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

进一步的，上述的步骤三中与初步姿态融合的计算公式为：

Z(n)＝a*X(n)+(1-a)*Y(n)

其中，X为加速度计输入，Y为陀螺仪输入，Z为输出，a为滤波系数，a为远小于1的一个数，即低通滤波器，(1-a)为高通滤波器。

进一步的，上述的步骤四具体为：

将准确的机器人姿态解算并进行直立控制，同时根据反馈的自平衡机器人速度通过速度闭环控制器进行速度控制，计算公式为：

a＝k_p*tan(θ)+k_d*[tan(θ)]'-[k_p1*e(k)+k_i1*∑e(k)]

其中，k_p为角度控制器比例系数，k_d为角度控制器微分系数，k_p1为速度控制比例系数，k_i1为速度控制器积分系数

进一步的，上述的直立控制采用PD控制，速度控制为PI控制。

在本发明的其中一个实施例中，如图1所示，自平衡机器人结构模型图中示出了机体坐标系及机器人姿态角。其中，绕X轴转动时机器人产生的倾角称为俯仰角(Pitch)，绕Y轴转动时机器人产生的倾角称为横滚角(Roll)，绕Z轴转动时机器人产生的倾角称为航向角(Yaw)。

自平衡机器人包括了MPU6050姿态传感器1，电池2，电机3，车轮4，底盘5和主控电路板6，通过一对驱动模块实现前进、后退、转弯等动作，通过MPU6050模块内置的加速度计、陀螺仪以及外接的磁力计获得姿态角信息，本实施例中针对加速度计、陀螺仪以及外接的磁力计获得的姿态角信息进行滤波降噪之后再进行融合，得到自平衡机器人的姿态信息，并通过电机编码器反馈的信息控制电机的转动实现对机器人姿态的控制从而实现机器人的自平衡以及运动。

如图2所示，自平衡机器人使用了三种传感器计算姿态，分别为：陀螺仪、磁力计、加速度计。陀螺仪是一种动态特性较好，在较短时间内响应较快的传感器。陀螺仪所返回的数据为角速度。但陀螺仪主要存在着两种噪声，一种是角速率随机游走，另一种是零漂。陀螺仪零漂是一种随时间流逝的在0轴之外偏移的零偏不稳定性。磁力计是一种长期静态性能稳定的传感器，但动态性能较差，它通过磁场强度在机体坐标系各个轴的分量来计算角度。加速度计具有较好的长期静态性能，是一种长期稳定的惯性传感器。加速度计利用的是重力进行测量，因此它无法获得世界坐标系Z轴的角度即航位角。而磁力计利用的是地球上的磁场强度，地球上的磁场强度由南恒指向北方，磁力计的强项在于测量机器人的方向。因此三种传感器各有优缺点，通过数据融合将它们的优势互补，进而可以更为精确地获得机器人姿态。

先将加速度计数据进行滑动权值滤波获得较为稳定的三轴加速度，再结合磁力计数据初步算出机器人的姿态。由于加速度计和磁力计动态特性不好，此时算出的机器人姿态并不准确，我们再利用陀螺仪返回的角速度数据积分后利用卡尔曼滤波或者互补滤波进行数据融合(若处理器性能较好则使用卡尔曼滤波，否则使用互补滤波)。

本实施例中的数据融合包括以下步骤：

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

首先，将加速度计数据进行滑动权值滤波处理，计算公式为：

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

利用捷联矩阵，我们可以将加速度计的数据转化为姿态，捷联矩阵如公式2所示：

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

加速度计的坐标转换如公式3所示，其中a_n＝(0,0,g)，a_b＝(a_x,a_y,a_z)：

根据这个公式2与公式3，我们可以初步求出俯仰角和横滚角，如公式4，公式5所示。

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

航位角受限于加速度计的性质，无法通过加速度计求出航位角，因此需要融合磁力计解算出航位角。磁力计获得的数据为m_b＝(m_x,m_y,m_z)，绝对坐标系下磁力计的磁场强度为m_n＝[0，m_N,m_D]，按照同样的原理，可初步求出航位角。如公式6所示：

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

本实施例中以上步骤已经利用加速度计和磁力计初步得到了自平衡机器人的姿态。但由于加速度计和磁力计动态特性不好，因此本实施例通过陀螺仪补偿以上数据，由于陀螺仪数据更新较快，用四元数的方式计算更快。

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

具体为：根据四元数公式将原始的陀螺仪数据转换为旋转矢量，刚体定点转动理论中，参考坐标系绕矢量旋转一个角度α＝(α₁,α₂,α₃)，就可与机体坐标系重合，那么四元数Q可表示为：

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的矢量形式的分量；

四元数表达式求出之后，利用四元数表示的捷联矩阵求出姿态角。此时的姿态角即为利用陀螺仪所求出的姿态角。

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

此时，已经利用加速度计和磁力计得到一组姿态角数据，已经利用陀螺仪得到了一组姿态角数据，而陀螺仪的动态特性较好，长期效果不好。加速度计和磁力计长期效果好，动态特性差。因此我们将两者的优势互补。处理信号常见的方式有高通滤波器与低通滤波器，高通滤波器可以过滤掉长期变化，保留短暂变化，低通滤波器可以过滤掉短期变化，保留长期变化。我们将基本的滤波算法的公式列出：

将其转化为一阶RC数字滤波的基本算法，X为加速度计输入，Y为陀螺仪输入，Z为输出。如公式15所示，其中，a为滤波系数，通常为远小于1的一个数，相当于低通滤波器，(1-a)相当于高通滤波器。

Z(n)＝a*X(n)+(1-a)*Y(n)

其中，X为加速度计输入，Y为陀螺仪输入，Z为输出，a为滤波系数，a为远小于1的一个数，即低通滤波器，(1-a)为高通滤波器。

得出准确的机器人姿态之后，由于传统的PID算法的PID参数是固定的，并不能随着机器人运行环境的变化而变化，因此适应性不好。本实施例还提供了一种基于自平衡机器人姿态变化的自适应PID控制算法，具体步骤如下：

如图4-6所示，自平衡机器人的核心问题是其平衡，本文针对这一点对算法进行改进，由上图可知，为了使机器人自平衡，应使电机驱动力的分力克服重力向斜下方的分力。由此可推导出应给的电机控制力：

F＝mgtan(θ)

电机的驱动力由加速度表示，如图2电机响应曲线所示，自平衡阶段时，电机为加速阶段，由图5可知，其加速度与电机驱动电压基本成正比。因此可以得出

U＝Kmgtan(θ)

自平衡控制即直立控制，除此之外，还需对自平衡机器人进行速度控制，由于直立控制需要响应速度较快，因此使用PD控制，速度控制由于消除稳态误差更为重要，因此使用PI控制。

如图6所示，该控制流程对应的公式为：

a＝kp*tan(θ)+kd*[tan(θ)]'-[kp₁*e(k)+ki₁*∑e(k)]。

Claims (8)

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：根据旋转矢量通过捷联矩阵计算姿态角，计算公式为：

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

6.根据权利要求5所述的基于多源传感器数据融合的自平衡机器人控制方法，其特征在于：所述的步骤三中与初步姿态融合的计算公式为：

Z(n)＝a*X(n)+(1-a)*Y(n)

其中，X为加速度计输入，Y为陀螺仪输入，Z为输出，a为滤波系数，a为远小于1的一个数，即低通滤波器，(1-a)为高通滤波器。

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

将准确的机器人姿态解算并进行直立控制，同时根据反馈的自平衡机器人速度通过速度闭环控制器进行速度控制，计算公式为：

a＝k_p*tan(θ)+k_d*[tan(θ)]'-[k_p1*e(k)+k_i1*∑e(k)]

其中，k_p为角度控制器比例系数，k_d为角度控制器微分系数，k_p1为速度控制比例系数，k_i1为速度控制器积分系数。

8.根据权利要求7所述的基于多源传感器数据融合的自平衡机器人控制方法，其特征在于：所述的直立控制采用PD控制，所述的速度控制为PI控制。