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

(19)中华人民共和国国家知识产权局(12)发明专利申请(10)申请公布号CN105404296A(43)申请公布日2016.03.16(21)申请号201510714330.1(22)申请日2015.10.28(71)申请人南京师范大学地址210046江苏省南京市板仓街78号申请人南京宝岩自动化有限公司(72)发明人曹楠刘益剑杨继全李娜夏俊程继红(74)专利代理机构江苏圣典律师事务所32237代理人胡建华(51)Int.Cl.G05D1/02(2006.01)权利要求书2页说明书7页附图4页(54)发明名称一种两轮自平衡智能车姿态控制方法(57)摘要本发明公开了一种两轮自平衡智能车姿态控制方法，属于惯性导航技术领域，本发明利用与智能车刚性连接的惯性传感器陀螺仪和加速度计获取原始姿态倾角信息，并在其基础之上应用卡尔曼滤波对陀螺仪和加速度计进行数据融合，精确地获得对智能车运动状态进行全面的描述的姿态倾角信息。本发明方法抑制了由动态加速度引起的加速度计干扰噪声，克服了陀螺仪对时间积分所引起的滞后问题，同时也适用于两轮自平衡机器人、四轴飞行器等系统的姿态解算。本发明通过两轮自平衡智能车平台予以验证，具有较好的解算精度和响应速度。CN105404296ACN105404296A权利要求书1/2页1.一种两轮自平衡智能车姿态控制方法，其特征在于，包括如下步骤：步骤1，应用动力学方程建立两轮自平衡智能车的运动模型；步骤2，在运动模型中引入单位负反馈和串联PD控制器以调整相应参数；步骤3，应用与智能车刚性连接的惯性传感器陀螺仪和加速度计构建两轮自平衡智能车的姿态测量系统；步骤4，根据惯性传感器陀螺仪和加速度计的输出模型，分别对两轮自平衡智能车进行静态和动态条件下特性分析，进而得到各自包含误差的数据输出运动模型；步骤5，应用滤波算法对惯性传感器陀螺仪和加速度计的数据进行最优估计，从而实现对两轮自平衡智能车的姿态解算，并完成对两轮自平衡智能车的姿态控制。2.根据权利要求1所述的一种两轮自平衡智能车姿态控制方法，其特征在于，步骤1中所述动力学方程为：其中，L为两轮自平衡智能车的摆杆质心到两个同轴车轮轴线的距离，g为重力加速度，θ(t)为t时刻两轮自平衡智能车与竖直方向的夹角即姿态倾角，x(t)为t时刻由外界扰动所引起的两轮自平衡智能车角加速度，a(t)为t时刻车轮运动产生的加速度；当两轮自平衡智能车处于自平衡状态时，在姿态倾角θ(t)＝0处通过泰勒级数展开进行线性化处理，则有sinθ(t)＝θ(t)，cosθ(t)＝1，当智能车处于自平衡状态时，运行状态为静止状态或匀速直线运动状态，则有智能车车轮运动产生的加速度a(t)＝0，此时对应的动力学方程为：对上式进行拉氏变换，得到两轮自平衡智能车在自平衡状态下s域内姿态倾角Θ(s)与外界扰动所引起的角加速度X(s)之间的传递函数H(s)：3.根据权利要求2所述的一种两轮自平衡智能车姿态控制方法，其特征在于，步骤2中，对运动模型引入单位负反馈和串联PD控制器后传递函数H(s)为：其中，KP、KD分别为PD控制器的比例系数和微分系数，对上式使用劳斯判据进行分析，当满足下列条件时，两轮自平衡智能车姿态的自由模态收敛并趋向于零从而达到稳定：4.根据权利要求3所述的一种两轮自平衡智能车姿态解算的设计方法，其特征在于，2CN105404296A权利要求书2/2页步骤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+1|k+1)＝θ(k+1|k)+Kg(k+1)(θacce(k+1)-θ(k+1|k))，P(k+1|k+1)＝(1-Kg(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)对应的协方差；Kg(k+1)：k+1时刻卡尔曼增益；θacce(k+1)：当前时刻姿态倾角；θ(k+1|k+1)：k+1时刻姿态倾角的最优估计；