CN105404296A - 一种两轮自平衡智能车姿态控制方法 - Google Patents

Abstract

本发明公开了一种两轮自平衡智能车姿态控制方法，属于惯性导航技术领域，本发明利用与智能车刚性连接的惯性传感器陀螺仪和加速度计获取原始姿态倾角信息，并在其基础之上应用卡尔曼滤波对陀螺仪和加速度计进行数据融合，精确地获得对智能车运动状态进行全面的描述的姿态倾角信息。本发明方法抑制了由动态加速度引起的加速度计干扰噪声，克服了陀螺仪对时间积分所引起的滞后问题，同时也适用于两轮自平衡机器人、四轴飞行器等系统的姿态解算。本发明通过两轮自平衡智能车平台予以验证，具有较好的解算精度和响应速度。

Description

一种两轮自平衡智能车姿态控制方法

技术领域

本发明属于惯性导航领域，尤其涉及一种两轮自平衡智能车姿态控制方法。

背景技术

两轮自平衡智能车是一种典型的非线性欠驱动系统，是一种由两个电机以差分方式驱动两个同轴相连的车轮运动的具有自平衡能力的智能车。具有体积小，机动性强的特点，可实现零转弯半径及狭窄通道通过的能力。为了实现两轮自平衡智能车在运动过程中处于自平衡状态，需要对其姿态信息进行准确的实时测量。常用构成姿态测量系统的惯性传感器的受温度、动态加速度噪声等因素影响，往往不能够对姿态信息进行全面精确地实时测量，与此同时，测量误差还会随着时间的增大，最终导致系统无法正常工作。姿态解算的精度对两轮自平衡智能车的姿态控制的动态特性和控制品质有着较大的影响。因此，针对两轮自平衡智能车运动特点并结合相关惯性传感器的特性进行准确的姿态解算十分重要。

近年来，国内外诸多学者应用Largrange法和牛顿-欧拉法建立了两轮自平衡智能车的模型，并运用了多种不同控制理论完成了控制器的设计。在进行控制器设计仿真实验时往往将智能车的姿态信息进行理想化处理，没有结合实际两轮自平衡智能车的系统特点以及惯性传感器的固有属性进行研究。例如：惯性传感器陀螺仪具有较好的动态特性，但是由诸多因素所引起的累积漂移误差，且进行姿态解算获取姿态倾角的过程需要角速度对时间积分，因此会存在一定的姿态解算滞后；加速度计传感器具有灵敏度高，相对陀螺仪漂移误差小的固有属性，但是由运动引起的动态加速度高频噪声的特性决定了其不适合跟踪过动态角度运动，因此单独的应用加速度计传感器进行姿态解算并不合适。

发明内容

本发明主要解决的技术问题是针对上述背景技术的不足，根据两轮自平衡智能车运行对姿态解算精度和姿态控制的需求提出了一种两轮自平衡智能车姿态控制方法，达到了提高两轮自平衡智能车姿态解算精度的目的。惯性传感器陀螺仪具有动态性能良好，不受动态加速度变化的影响的特点，但是存在由温度、摩擦力和不稳定力矩引起的漂移误差；惯性传感器加速度计在静态条件下具有较好的检测效果，但是在动态条件下受动态加速度影响较大。本发明的目的是为了克服现有单一惯性传感器固有属性的不足，提出了一种两轮自平衡智能车姿态控制方法。

本发明公开了一种两轮自平衡智能车姿态控制方法，包括如下步骤：

步骤1，应用动力学方程建立两轮自平衡智能车的运动模型；

步骤2，在运动模型中引入单位负反馈和串联PD控制器以调整相应参数；

步骤3，应用与智能车刚性连接的惯性传感器陀螺仪和加速度计构建两轮自平衡智能车的姿态测量系统；

步骤4，根据惯性传感器陀螺仪和加速度计的输出模型，分别对两轮自平衡智能车进行静态和动态条件下特性分析，进而得到各自包含误差的数据输出模型；

步骤5，应用滤波算法对惯性传感器陀螺仪和加速度计的数据进行最优估计，从而实现对两轮自平衡智能车的姿态解算，并完成对两轮自平衡智能车的姿态控制。

步骤1中所述动力学方程为：

$L\frac{d^2\mathrm{\θ}\left(t\right)}{{\mathrm{dt}}^2}=gsin\[\mathrm{\θ}\left(t\right)\]-a\left(t\right)cos\[\mathrm{\θ}\left(t\right)\]+Lx\left(t\right),$

其中，L为两轮自平衡智能车的摆杆质心到两个同轴车轮轴线的距离；g为重力加速度9.8m/s²；θ(t)为t时刻两轮自平衡智能车与竖直方向的夹角即姿态倾角；x(t)为t时刻由外界扰动所引起的两轮自平衡智能车角加速度；a(t)为t时刻车轮运动产生的加速度。

当两轮自平衡智能车处于自平衡状态时，在姿态控制的作用下，姿态倾角变化范围较小(当保持自平衡状态时变化范围是比较小的，这是一种常用的对非线性方程或系统进行线性化处理的方法)。因此在姿态倾角θ(t)＝0处通过泰勒级数展开进行线性化处理，忽略高阶项则有sinθ(t)＝θ(t)，cosθ(t)＝1。与此同时，当智能车处于自平衡状态时，运行状态应为静止状态或匀速直线运动状态，则有智能车车轮运动产生的加速度a(t)＝0。此时对应的动力学方程为：

$L\frac{d^2\mathrm{\θ}\left(t\right)}{{\mathrm{dt}}^2}=g\mathrm{\θ}\left(t\right)+Lx\left(t\right),$

对上式进行拉氏变换得到两轮自平衡智能车在自平衡状态下s域内(s域是一种公知的概念，应用于将微分方程转换为代数方程的方法)姿态倾角Θ(s)与外界扰动所引起的角加速度X(s)之间的传递函数H(s)：

$H\left(s\right)=\frac{\mathrm{\Θ}\left(s\right)}{X\left(s\right)}=\frac{1}{s^2-\frac{g}{L}}.$

步骤2中，对两轮自平衡智能车系统进行拉式变换后可知：系统在复平面内存在两个极点，即其中一个位于复平面的右侧，此时系统的自由模态发散，系统不稳定。为改善系统特性使自由模态收敛，对运动模型引入单位负反馈和串联PD控制器后传递函数H(s)为：

$H\left(s\right)=\frac{\mathrm{\Θ}\left(s\right)}{X\left(s\right)}=\frac{K_P+K_{D}s}{s^2+K_{D}s+K_P-\frac{g}{L}},$

其中，K_P、K_D分别为PD控制器的比例系数和微分系数，K_P、K_D取值范围为实数。对上式使用劳斯判据进行分析，当满足下列条件时，两轮自平衡智能车姿态的自由模态收敛并趋向于零从而达到稳定：

$\left\{\begin{array}{c}{K}_P>\frac{g}{L}\\ K_D>0\end{array}\right..$

步骤4中，惯性传感器陀螺仪的输出模型为：

ω_gyro＝ω_real+ε_g+ε_n，

其中ω_gyro为惯性传感器陀螺仪输出的角速度，ω_real为两轮自平衡智能车真实的角速度，ε_g为慢时变漂移误差，ε_n由温度、摩擦力和不稳定力矩构成的随机误差；ε_g和ε_n的取值范围为实数。

惯性传感器加速度计输出模型为：

θ_acce＝θ_real+ε_a，

其中θ_acce表示惯性传感器加速度计输出的姿态倾角，θ_real为两轮自平衡智能车真实角度，ε_a为两轮自平衡智能车运动时产生的动态加速度。惯性传感器加速度计可以测量运动和静态的加速度。静态加速度的一个特殊例子是重力加速度，当加速度传感器静止时(也就是侧面和垂直方向没有加速度作用)，那么作用在它上面的只有重力加速度。重力(垂直)和加速度传感器灵敏轴之间的夹角就是倾斜角，即可将加速度计用作倾角计。因为角度由灵敏轴和重力矢量组成的垂直平面决定,倾斜可以从各种初始的惯性加速度传感器位置测得。

步骤5中，采用如下公式对惯性传感器陀螺仪和加速度计的输出数据进行最优估计：

θ(k+1|k)＝θ(k|k)+ω_gyro(k)T，

P(k+1|k)＝P(k|k)+Q，

$K_g(k+1)=\frac{P(k+1|k)}{P(k+1|k)+R},$

θ(k+1|k+1)＝θ(k+1|k)+K_g(k+1)(θ_acce(k+1)-θ(k+1|k))_，

P(k+1|k+1)＝(1-K_g(k+1))P(k+1|k)，

其中：

θ(k+1|k)：k+1时刻预估姿态倾角；

θ(k|k)：k时刻的最优估计姿态倾角；

ω_gyro(k)：k时刻输出角速度；

T：积分时间；

P(k+1|k)：θ(k+1|k)对应的协方差；

P(k|k)：θ(k|k)对应的协方差；

K_g(k+1)：k+1时刻卡尔曼增益；

θ_acce(k+1)：当前时刻姿态倾角；θ(k+1|k+1)：k+1时刻姿态倾角的最优估计；

Q：系统过程噪声的协方差；

R：系统测量噪声协方差；

对以上公式进行递归运算即可得到姿态倾角的最优估计，从而实现对两轮自平衡智能车的姿态解算。

有益效果：本发明适用于两轮自平衡智能车的姿态解算，同时也适用于两轮自平衡机器人、四轴飞行器等系统的姿态解算。

本发明在建立了两轮自平衡智能车系统的运动模型并得到了系统稳定的条件。针对两轮自平衡智能车的动态条件下的运行特点，结合惯性传感器陀螺仪和加速度计的固有属性特点，建立了二者的输出模型并应用卡尔曼滤波器提高姿态解算精度。通过在两轮自平衡智能车平台进行实验，对比单独应用陀螺仪和加速度计的数据和应用卡尔曼滤波器进行滤波融合后的数据，可知本发明对两轮自平衡智能车的姿态解算精度有着明显的提高。

附图说明

图1两轮自平衡智能车结构示意图；

图2卡尔曼滤波器数据融合输入输出关系图；

图3卡尔曼滤波器递归运算流程图；

图4单独应用陀螺仪的姿态解算漂移示意图；

图5a为启动实验卡尔曼滤波数据融合、单独应用加速度计和单独应用陀螺仪进行姿态解算对比曲线；

图5b为通过路障实验卡尔曼滤波数据融合、单独应用加速度计和单独应用陀螺仪进行姿态解算对比曲线；

图5c为停止实验卡尔曼滤波数据融合、单独应用加速度计和单独应用陀螺仪进行姿态解算对比曲线。

具体实施方式

本发明公布了一种两轮自平衡智能车姿态解算方法，同时也适用于两轮自平衡机器人。

本发明公开了一种两轮自平衡智能车姿态控制方法，包括如下步骤：

步骤1，应用动力学方程建立两轮自平衡智能车的运动模型；

步骤2，在运动模型中引入单位负反馈和串联PD控制器以调整相应参数；

步骤3，应用与智能车刚性连接的惯性传感器陀螺仪和加速度计构建两轮自平衡智能车的姿态测量系统；

步骤4，根据惯性传感器陀螺仪和加速度计的输出模型，分别对两轮自平衡智能车进行静态和动态条件下特性分析，进而得到各自包含误差的数据输出模型；

步骤5，应用滤波算法对惯性传感器陀螺仪和加速度计的数据进行最优估计，从而实现对两轮自平衡智能车的姿态解算，并完成对两轮自平衡智能车的姿态控制。

应用牛顿力学方法建立两轮自平衡智能车的运动模型，两轮自平衡智能车结构示意图如图1所示，根据系统特点设计控制器进而得到系统的控制方法。

步骤1中所述动力学方程为：

$L\frac{d^2\mathrm{\θ}\left(t\right)}{{\mathrm{dt}}^2}=gsin\[\mathrm{\θ}\left(t\right)\]-a\left(t\right)cos\[\mathrm{\θ}\left(t\right)\]+Lx\left(t\right),$

其中，L为两轮自平衡智能车的摆杆质心到两个同轴车轮轴线的距离，g为重力加速度，θ(t)为t时刻两轮自平衡智能车与竖直方向的夹角即姿态倾角，x(t)为t时刻由外界扰动所引起的两轮自平衡智能车角加速度，a(t)为t时刻车轮运动产生的加速度。

当两轮自平衡智能车处于自平衡状态时，在姿态倾角θ(t)＝0处通过泰勒级数展开进行线性化处理，则有sinθ(t)＝θ(t)，cosθ(t)＝1，当智能车处于自平衡状态时，运行状态为静止状态或匀速直线运动状态，则有智能车车轮运动产生的加速度a(t)＝0，此时对应的动力学方程为：

$L\frac{d^2\mathrm{\θ}\left(t\right)}{{\mathrm{dt}}^2}=g\mathrm{\θ}\left(t\right)+Lx\left(t\right),$

引入负反馈和串联PD控制器后，对上式进行拉氏变换得到两轮自平衡智能车在自平衡状态下s域内姿态倾角Θ(s)与外界扰动所引起的角加速度X(s)之间的传递函数

H(s)：

$H\left(s\right)=\frac{\mathrm{\Θ}\left(s\right)}{X\left(s\right)}=\frac{1}{s^2-\frac{g}{L}}.$

步骤2中，对运动模型引入单位负反馈和串联PD控制器后传递函数H(s)为：

$H\left(s\right)=\frac{\mathrm{\Θ}\left(s\right)}{X\left(s\right)}=\frac{K_P+K_{D}s}{s^2+K_{D}s+K_P-\frac{g}{L}},$

其中，K_P、K_D分别为PD控制器的比例系数和微分系数，对上式使用劳斯判据进行分析，当满足以下条件时，两轮自平衡智能车姿态的自由模态收敛并趋向于零从而达到稳定：

$\left\{\begin{array}{c}{K}_P>\frac{g}{L}\\ K_D>0\end{array}\right..$

结合惯性传感器陀螺仪加速度计的固有属性，对动态条件下的数据输出分别进行分析并得到各自的特征模型。

步骤4中，惯性传感器陀螺仪的输出模型为：

ω_gyro＝ω_real+ε_g+ε_n，

其中ω_gyro为惯性传感器陀螺仪输出的角速度，ω_real为两轮自平衡智能车真实的角速度，ε_g为慢时变漂移误差，ε_n由温度、摩擦力和不稳定力矩构成的随机误差，可以近似的认为是白噪声；

惯性传感器加速度计输出模型为：

θ_acce＝θ_real+θ_g，

其中θ_acce表示惯性传感器加速度计输出的姿态倾角，θ_real为两轮自平衡智能车真实角度，ε_a为两轮自平衡智能车运动时产生的动态加速度。

如图2所示，在两轮自平衡智能车硬件平台基础上，应用改进的卡尔曼滤波器对姿态测量系统的原始数据ω_gyro、θ_acce进行滤波融合，从而实现对两轮自平衡智能车的姿态解算，并完成对两轮自平衡智能车的姿态控制。

如图3所示，步骤5中，采用如下公式对陀螺仪加速度计数据进行最优估计，包括步骤3-1～步骤3-7：

步骤3-1，对惯性传感器陀螺仪和加速度计数据进行采样，更新陀螺仪角速度ω_gyro(k)和加速度计倾角θ_acce(k+1)。

步骤3-2，根据k时刻的最优估计姿态倾角θ(k|k)、k时刻输出角速度ω_gyro(k)和姿态测量系统采样时间T，完成k+1时刻姿态倾角的估计：

θ(k+1|k)＝θ(k|k)+ω_gyro(k)T，

惯性传感器陀螺仪积分倾角和真实倾角对比图如图4所示；

步骤3-3，根据k时刻姿态倾角θ(k|k)对应的协方差P(k|k)和系统过程噪声的协方差Q，更新k+1时刻姿态倾角θ(k+1|k)的协方差：

P(k+1|k)＝P(k|k)+Q，

步骤3-4，根据k+1时刻姿态倾角θ(k+1|k)的协方差P(k+1|k)和系统测量噪声协方差R，计算k+1时刻的卡尔曼增益K_g(k+1)：

$K_g(k+1)=\frac{P(k+1|k)}{P(k+1|k)+R},$

步骤3-5，根据当前时刻姿态倾角θ_acce(k+1)、姿态倾角估计θ(k+1|k)和卡尔曼增益K_g(k+1)计算k+1时刻最优估计姿态倾角：

θ(k+1|k+1)＝θ(k+1|k)+K_g(k+1)(θ_acce(k+1)-θ(k+1|k))，

步骤3-6，根据k+1时刻的卡尔曼增益K_g(k+1)、θ(k+1|k)对应的协方差P(k+1|k)，更新k+1时刻最优估计姿态倾角的协方差：

P(k+1|k+1)＝(1-K_g(k+1))P(k+1|k)，

步骤3-7，根据步骤3-1～步骤3-6进行递归运算即可得到姿态倾角的最优估计，从而实现对两轮自平衡智能车的姿态解算。

实施例：

应用自行设计的两轮自平衡智能车平台对姿态解算方法予以实施。姿态控制应用PD控制，MPU6050型数字陀螺仪和MMA7361型模拟加速度计构成了两轮自平衡智能车的姿态测量系统并与智能车刚性连接。智能车通过IIC接口获取陀螺仪输出的包含由温度、摩擦力和不稳定力矩所引起的漂移的姿态信息，通过ADC接口获取加速度计输出的包含高频动态加速度噪声的姿态信息。将上述姿态信息单位归一化后，应用权利要求3所述的解算方法进行姿态解算获取姿态倾角的最优估计，其初始状态(即k＝0时)相关参数如下：

积分时间T：0.005；

θ(k|k)对应的协方差P(k|k)：0.000001；

系统过程噪声的协方差Q：0.000001；

系统测量噪声协方差R：0.35

两轮自平衡智能车运动过程中存在着多种运动状态，其中加速、减速以及通过路障时最具有普遍性和代表性，也对姿态解算的解算精度和响应速度要求最高。因此应用蓝牙模块将上述三个运动状态的相关姿态信息及姿态解算结果发送至MATLAB上位机进行观测。其具体实验方法和结果如下：

(1)启动实验：即加速实验，两轮自平衡智能车由静止状态加速到某一速度后保持自平衡状态匀速运行，获得如图5a的对比曲线。由对比曲线可知，应用卡尔曼滤波融合进行姿态解算后得到的姿态倾角相对其他解算方法在智能车启动状态时具有跟踪迅速解算精确的特点。在图5a中具体的表现为：同一时刻，通过卡尔曼滤波融合进行姿态解算的方法较陀螺仪输出角速度对时间积分后得到的姿态倾角对真实的姿态倾角跟踪更为迅速；应用卡尔曼滤波融合进行姿态解算后的姿态倾角，克服了运动过程中加速度计输出的干扰噪声，提高了解算精度。

(2)通过路障实验：两轮自平衡智能车在处于自平衡匀速运行状态通过一个路障减速带，获得如图5b的对比曲线。由对比曲线可知，应用卡尔曼滤波融合进行姿态解算后得到的姿态倾角在智能车通过路障状态时具有跟踪迅速解算精确的特点；在图5b中具体的表现为：同一时刻，通过卡尔曼滤波融合进行姿态解算的方法较陀螺仪输出角速度对时间积分后得到的姿态倾角对真实的姿态倾角跟踪更为迅速；应用卡尔曼滤波融合进行姿态解算后的姿态倾角，克服了运动过程中加速度计输出的干扰噪声，提高了解算精度。

(3)停止实验：即减速实验，两轮自平衡智能车由运动状态到静止状态获得如图5c的对比曲线。由对比曲线可知，应用卡尔曼滤波融合进行姿态解算后得到的姿态倾角相对其他解算方法相对其他解算方法在智能车停止状态时具有跟踪迅速解算精确的特点。在图5c中具体的表现为：同一时刻，通过卡尔曼滤波融合进行姿态解算的方法较陀螺仪输出角速度对时间积分后得到的姿态倾角对真实的姿态倾角跟踪更为迅速；应用卡尔曼滤波融合进行姿态解算后的姿态倾角，克服了运动过程中加速度计输出的干扰噪声，提高了解算精度。

综合上述三组实验的对比曲线进行分析：加速度计测量得到的姿态倾角虽然响应速度较快，但存在着较大的干扰噪声。陀螺仪角速度对时间积分得到的姿态倾角虽然不受动态加速度影响，但存在着一定的漂移和相对较大的滞后。应用卡尔曼滤波融合进行姿态解算后得到的姿态倾角最优估计，有效的抑制了动态加速度引起的干扰噪声，克服了陀螺仪角速度对时间积分引起的滞后问题，有效的提高了两轮自平衡智能车的姿态解算的精度和响应速度。

本发明提供了一种两轮自平衡智能车姿态控制方法，具体实现该技术方案的方法和途径很多，以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。本实施例中未明确的各组成部分均可用现有技术加以实现。

Claims (5)

1.一种两轮自平衡智能车姿态控制方法，其特征在于，包括如下步骤：

步骤1，应用动力学方程建立两轮自平衡智能车的运动模型；

步骤2，在运动模型中引入单位负反馈和串联PD控制器以调整相应参数；

步骤3，应用与智能车刚性连接的惯性传感器陀螺仪和加速度计构建两轮自平衡智能车的姿态测量系统；

步骤4，根据惯性传感器陀螺仪和加速度计的输出模型，分别对两轮自平衡智能车进行静态和动态条件下特性分析，进而得到各自包含误差的数据输出运动模型；

步骤5，应用滤波算法对惯性传感器陀螺仪和加速度计的数据进行最优估计，从而实现对两轮自平衡智能车的姿态解算，并完成对两轮自平衡智能车的姿态控制。

2.根据权利要求1所述的一种两轮自平衡智能车姿态控制方法，其特征在于，步骤1中所述动力学方程为：

$L\frac{d^2\mathrm{\θ}\left(t\right)}{{\mathrm{dt}}^2}=gsin\[\mathrm{\θ}\left(t\right)\]-a\left(t\right)cos\[\mathrm{\θ}\left(t\right)\]+Lx\left(t\right),$

其中，L为两轮自平衡智能车的摆杆质心到两个同轴车轮轴线的距离，g为重力加速度，θ(t)为t时刻两轮自平衡智能车与竖直方向的夹角即姿态倾角，x(t)为t时刻由外界扰动所引起的两轮自平衡智能车角加速度，a(t)为t时刻车轮运动产生的加速度；

当两轮自平衡智能车处于自平衡状态时，在姿态倾角θ(t)＝0处通过泰勒级数展开进行线性化处理，则有sinθ(t)＝θ(t)，cosθ(t)＝1，当智能车处于自平衡状态时，运行状态为静止状态或匀速直线运动状态，则有智能车车轮运动产生的加速度a(t)＝0，此时对应的动力学方程为：

$L\frac{d^2\mathrm{\θ}\left(t\right)}{{\mathrm{dt}}^2}=g\mathrm{\θ}\left(t\right)+Lx\left(t\right),$

对上式进行拉氏变换，得到两轮自平衡智能车在自平衡状态下s域内姿态倾角Θ(s)与外界扰动所引起的角加速度X(s)之间的传递函数H(s)：

$H\left(s\right)=\frac{\mathrm{\Θ}\left(s\right)}{X\left(s\right)}=\frac{1}{s^2-\frac{g}{L}}.$

3.根据权利要求2所述的一种两轮自平衡智能车姿态控制方法，其特征在于，步骤2中，对运动模型引入单位负反馈和串联PD控制器后传递函数H(s)为：

$H\left(s\right)=\frac{\mathrm{\Θ}\left(s\right)}{X\left(s\right)}=\frac{K_P+K_{D}s}{s^2+K_{D}s+K_P-\frac{g}{L}},$

其中，K_P、K_D分别为PD控制器的比例系数和微分系数，对上式使用劳斯判据进行分析，当满足下列条件时，两轮自平衡智能车姿态的自由模态收敛并趋向于零从而达到稳定：

$\left\{\begin{array}{c}{K}_P>\frac{g}{L}\\ K_D>0\end{array}\right..$

4.根据权利要求3所述的一种两轮自平衡智能车姿态解算的设计方法，其特征在于，步骤4中，惯性传感器陀螺仪的输出模型为：

ω_gyro＝ω_real+ε_g+ε_n，

其中ω_gyro为惯性传感器陀螺仪输出的角速度，ω_real为两轮自平衡智能车真实的角速度，ε_g为慢时变漂移误差，ε_n由温度、摩擦力和不稳定力矩构成的随机误差；

惯性传感器加速度计输出模型为：

θ_acce＝θ_real+ε_a，

其中θ_acce表示惯性传感器加速度计输出的姿态倾角，θ_real为两轮自平衡智能车真实角度，ε_a为两轮自平衡智能车运动时产生的动态加速度。

5.根据权利要求4所述的一种两轮自平衡智能车姿态控制方法，其特征在于，步骤5中，采用如下公式对惯性传感器陀螺仪和加速度计的输出数据进行最优估计：

θ(k+1|k)＝θ(k|k)+ω_gyro(k)T，

P(k+1|k)＝P(k|k)+Q，

$K_g(k+1)=\frac{P(k+1|k)}{P(k+1|k)+R},$

θ(k+1|k+1)＝θ(k+1|k)+K_g(k+1)(θ_acce(k+1)-θ(k+1|k))，

P(k+1|k+1)＝(1-K_g(k+1))P(k+1|k)，

其中：

θ(k+1|k)：k+1时刻预估姿态倾角；

θ(k|k)：k时刻的最优估计姿态倾角；

ω_gyro(k)：k时刻输出角速度；

T：积分时间；

P(k+1|k)：θ(k+1|k)对应的协方差；

P(k|k)：θ(k|k)对应的协方差；

K_g(k+1)：k+1时刻卡尔曼增益；

θ_acce(k+1)：当前时刻姿态倾角；

θ(k+1|k+1)：k+1时刻姿态倾角的最优估计；

Q：系统过程噪声的协方差；

R：系统测量噪声协方差；

对以上公式进行递归运算即可得到姿态倾角的最优估计，从而实现对两轮自平衡智能车的姿态解算。