【《两轮自平衡小车系统的硬件和软件设计案例》8800字】_第1页
【《两轮自平衡小车系统的硬件和软件设计案例》8800字】_第2页
【《两轮自平衡小车系统的硬件和软件设计案例》8800字】_第3页
【《两轮自平衡小车系统的硬件和软件设计案例》8800字】_第4页
【《两轮自平衡小车系统的硬件和软件设计案例》8800字】_第5页
已阅读5页,还剩16页未读, 继续免费阅读

下载本文档

版权说明:本文档由用户提供并上传,收益归属内容提供方,若内容存在侵权,请进行举报或认领

文档简介

两轮自平衡小车系统的硬件和软件设计案例目录TOC\o"1-3"\h\u176两轮自平衡小车系统的硬件和软件设计案例 117962第1章系统硬件设计 1280921.1电源电路 145311.2按键检测电路 235101.3电压检测电路 3312701.4主控电路 327831.5电机驱动电路 4123581.5显示电路 6277291.6蓝牙模块 6232001.7本章小结 729394第2章系统软件设计 8249722.1单片机编程软件介绍 834262.2程序功能实现思路 820512.3程序设计流程 8110432.4小车平衡控制 983002.2.1运动分析 1075572.2.2平衡控制(直立环) 10257422.2.3速度控制(速度环) 1153472.2.4转向控制(转向环) 12292522.2.5定时器PWM输出 13212342.2.6六轴传感器获取角度 15150182.5卡尔曼滤波算法 16188592.6OLED显示模块 19245392.7无线控制模块 20第1章系统硬件设计本章依据第2章选择硬件模块完成平衡小车系统的硬件电路的设计,简述了模块和单片机之间的连线方式,并说芯片的各引脚和模块怎么连接,也对各模块硬件进行了功能简介。1.1电源电路JGA25-370电机的工作电压是12V,所以电源选择的是三节共12V锂电池,电机驱动模块需要给电机内的AB编码器提供电压同时也需要给STM32F103芯片供电,所以就需要通过LM2596降压模块进行降压到5V。依据LM2596降压模块的数据手册中给出的引脚图如图3-1,进行电路设计。图3-1LM2596引脚图降压模块中电压转换的原理图如图3-2所示。其中IN作为电路的输入电压,电压范围在11V~16V之前,锂电池为IN口的提供了12V电压。GND和ON/OFF都接地线,out是线路的输出电压,其数值是5V。为了使降压电路的输出电压稳定,就需要使用电容进行充放电,然后进行去耦滤波来提高输出电压的稳定,也可以通过调节R2电阻是输出电压固定为5V。图3-2电压转换原理图输出电压计算公式为:VCC=VREF(RP1R8+1) VREF1.2按键检测电路该电路主要是用于检测开关是否按下。当开关按下,小车通电,立即开始检测小车当前姿态,然后控制电机运转。开关需要接上拉电阻,如图3-3所示,开关按下时,芯片的PA5脚接GND口,此时引脚为低电平。如果开关未打开,PA5接上拉电阻,此时为高电平。图3-3按键电路原理图1.3电压检测电路在平衡小车正常行驶时,为了避免出现小车电池电量不足,而导致出现的摔倒等现象,防止小车硬件损坏,所以对电池电压的检测就必不可少。因此我们就必选设置电压检测电路,在电池电量低时,进行报警提醒用户充电。电压检测电路中,可以通过检测电池两端的电压大小来判断电量是否过低,而检测两端电压就需要使用到STM32F103芯片中PA6脚中的AD功能。由此可以设置出如图3-4所示的电池电压检测电路。可以用两个电阻R1、R2将电池电压进行分压,其中A6端口的输出电压就是电阻R1两端的电压,这里电路中可以加一个电容,让电容充放电来稳定电压的变化。由图3-4可以得出,电池电压的1/11大约就是分压电路中电阻的电压,由STM32F103芯片手册中可知,ADC功能的参考电压是1.3V,并且要精确到12位。这时可以算出电池电压的计算公式是:V=V图3-4电池检测电路原理图1.4主控电路由STM32F103芯片手册中可得,两轮自平衡小车的主控电路原理图如图3-5所示:图3-5主控电路原理图两轮自平衡小车的最小系统是由晶振电路、电源电路、复位电路和一些调试电路组成。本设计中的选用芯片为STM32F103C8T6作为主控芯片,这一款芯片在成本较低,芯片有很多外接接口,可以方便在硬件设计中使用,芯片在出厂时封装了许多片上外设,在设计中使用起来相对比较方便。最小系统中的晶振电路主要还是用来提供设计中所需要的时钟频率。调试使用的接口电路有两种,一种是JTAG和SW,在使用的时候需要根据具体使用什么调试方法,来选取。但在使用的时候需要注意只能使用其中一种,需要进行配置,将其中一种开启,把不需要的关闭。程序烧写的时候使用USB转串口来实现。1.5电机驱动电路电机驱动模块选用TB6612FNG作为内置控制芯片,TB6612FNG能够同时控制两个520直流电机,其最大输入电压是15V,可以满足电池12V的电压,平均电流是1.2A,最大电流是1.2A。通过STM32F103芯片的IO口控制模块中的PWMA、PWMB来控制电机的转速,AIN1、AIN2、BIN1、BIN2来控制电机的正反转,AO1、AO2、BO1、BO2分别连接在电机的正、负极上,即直流电机编码器的正负极,从而实现直流电机的速度调整和小车的换向功能。编码器一共六根线,从左往右看,黄线连接着电机的正极,红线是编码器的电源线,绿线是编码器A相输出,白线是B相输出,可以通过AB相输出判断直流电机的正反转,黑线为编码器的地线,最右边的棕线连接着电机的负极。如图3-6是TB6612的连接测试图与编码器:图3-6TB6612的连接测试图与编码器图VM直接与供电电池相连,VCC为内部逻辑供电,可以接1.3V或者5V,GND接地,STBY为使能信号,接高电平正常工作,以PWMA、AIN1、AIN2、AO1、AO2为例介绍内部逻辑。一般PWMA接10KHz的PWM波,当AIN1、AIN2全为0时,电机停止;当AIN1为1,AIN2为0时,电机正转;当AIN1为0,AIN2为1时,电机反方向转。具体控制原理如图3-7所示:图3-7驱动电路原理图1.5显示电路本次设计中使用了OLED显示屏来进行平衡车参数显示,可以方便对平衡车状态,以及小车两个轮自运行速度。OLED显示屏在显示时使用较多,它有两种驱动方法,分别是IIC和SPI通信。这两种通信都可以实现OLED正常显示操作,它们之间主要不同的是IIC它通信的时候需要严格按照时序图来设计数据传输格式,每传输一次都需要进行一次起始条件才可以数据传输,而SPI就不需要那么多格式,它只需要设置传输数据的位数就可以,使用起来更加方便。OLED显示屏在本次设计中选用的是SPI模式进行通信。OLED显示屏在使用时需要接6根线,分别还是电源线、地线、SPI通信的四根线片选CS,MOSI、MISO、CLK。在显示的时候需要先录入字库信息,录入才能显示文字信息。在显示是可以参考手册,找到读写命令,在使用SPI通信将命令先发送过去,在传输数据。OLED电路图如图3-8和3-9所示:图3-8OLED硬件图图3-9显示电路原理图1.6蓝牙模块在本次设计中使用了蓝牙模块来与APP建立通信,蓝牙模块连接通信相对较简单,蓝牙模块的工作原理就是用户将单片机与手机之间建立蓝牙通信之后,就可以将手机上的数据原模原样的发送到单片机中,或者单片机将数据原模原样的传输到手机中。在建立连接的时候也比较方便,蓝牙模块在厂家出厂的时候已经设置好了一些功能,用户在使用的时候可以根据要实现的功能,选择相应的AT指令进行通信建立。蓝牙通信的时候使用的通信方式的是USART通信模式,因为它需要通过发送AT指令来控制单片机与APP之间建立通信,使用USART通信可以电脑作为中介,先通过电脑发送指令和蓝牙模块通信,验证模块是否能正常通信,然后在使用单片机来发送。这样就避免了出错误,无法查验。蓝牙模块接线正常的接电源和地线,然后把蓝牙模块的TX和单片机的RX连接。把蓝牙模块的RX和单片机的TX连接就可以实现模块的通信,然后进行程序设计即可。蓝牙模块的参数蓝牙模块的工作电压和其他模块一样使用的是1.3V电压,它的工作频率为2.4GHz,它的模式使用AT指令来进行设置,一般采用透传模式进行传输。模块的模块实物图和原理图如图3-10和3-11所示:图3-10蓝牙模块硬件图图3-11蓝牙模块原理图1.7本章小结平衡小车的控制系统包括了硬件控制系统和软件控制系统,而本章节主要介绍了两轮平衡小车的硬件系统的设计。小车硬件系统就是由构成小车的物理器件,即各模块的实物组成。在选择模块硬件时,需要确定小车要实现的功能,初步估算小车各功能需要用到的引脚数量,在进行对应模块的选择。由于有些功能实现的硬件极易损坏,因此选择了一些封装好了的模块,如蓝牙模块。小车硬件的设计也需要考虑到小车的平衡性,所以小车实物的线路不能太繁杂,所以主控电路的线路用了PCB板,这使小车平衡的实现更加容易,也让小车模型看起来更清爽。

第2章系统软件设计本章主要根据上一章硬件系统的设计方案进行软件系统的设计,其中程序的编写需要使用到Keil5这个软件,本章需要列出流程图进行程序的编写。2.1单片机编程软件介绍Keil5是一款集C语音编译器、宏汇编、链接器、库函数管理和仿真调试器等在内为一体的编写软件,可以通过一个集各模块代码将这些部分组合在一起。本设计主要使用C语言编写程序。C语言的主要特点是可以模块化编写和程序可移植,它的数据类型执行效率高、使用灵活方便、自由度大、可直接将程序烧录进实物。本次设计要设计单片机和各模块之间的数据交互。在Keil5中创建工程的步骤,首先安装STM32F103系列芯片的驱动包,然后在设置里面找到Device,点进去选择STM32F103C8驱动,再建立一个文件,保存成.c文件形式,然后将它添加到项目文件夹的USER中,在界面里面找到Opinionsfortarget,里面找到C/C++页面,将.h文件包含进去,最后选择弄好的文件保存为main文件。2.2程序功能实现思路两轮自平衡小车要实现的功能需要在程序中体现出来。(1)维持小车自平衡。首先由STM32读取MPU-6050六轴传感器上各个轴上的数据,并上传至程序进行整理计算,估算出需要改正的角度,再由卡尔曼滤波算法算出最佳的姿态角,并得到适合的PWM波,最后发送的电机进行调整使小车得到平衡控制。(2)实时检测小车状态并显示在OLED屏上。当小车开关打开通上电时,平衡小车各模块进行初始化设置。电压检测电路开始检测小车电池电压,并判断小车电量是否充足,如果电量低,小车开始报警。同时进行姿态角检测,并显示在OLED屏上。其他模块都同步进行。当开关按下几秒后,小车自动进入平衡状态。(3)与手机进行数据控制。用户打开蓝牙与小车蓝牙进行连接,通过按钮进行小车的前后左右和加速控制。2.3程序设计流程两轮自平衡小车控制系统的程序流程图如图4-1所示:图4-1程序流程图平衡小车通电后,首先各模块程序开始初始化设置,然后开始状态检测,状态检测的过程是一个循环函数,不断循环检测小车的状态,并将数据发送至OLED显示屏,显示屏不断刷新,实时更新数据,然后再发送至蓝牙,由用户操控。程序中需要使用中断,中断的过程就是小车实现平衡的过程。首先需要将小车姿态角的检测结果发送到PID算法中,在进行计算,最后发送到电机,这是一个周期。其中周期时间由MPU-6050六轴传感器中设置提供。这个周期是10ms,给过一个周期就会将各个轴上的数据发送到算法进行计算,在发出控制命令,由小车执行平衡。2.4小车平衡控制两轮自平衡小车实现平衡控制主要由两部分组成,一部分由姿态角传感器不断进行数据检测,另一部分是由PID控制算法进行最佳角度计算,再控制电机。获取小车的速度和角度这两组数据就是实现小车自平衡的前提,这些数值在stm32的中断控制中结合PID算法使用,输出运算后的数值赋值给PWM寄存器控制电机,从而实现小车的自平衡;而遥控数据则是小车转向控制才需要用的,因此可以先不关注遥控部分,在小车能够站起来后再去关注。2.2.1运动分析平衡小车的运动控制可以分为平衡控制、速度控制和方向控制,即直立环、速度环和转向环,其中平衡控制是最关键的部分。平衡小车作为一个控制对象,可以简化地把它看做两个电机的转动速度和转动方向控制,小车的直立控制和方向控制是直接通过控制小车两个驱动电机完成的,小车的速度则是通过调节小车到的角度来完成的,不同的倾角会引起小车的加减速。这里需要关注这两个关键词:速度和角度,这两个数据是通过检测装置从传感器获的的。2.2.2平衡控制(直立环)小车的平衡控制是通过负反馈来实现的。反馈又称回馈,是控制论的基本概念,指将系统的输出返回到输入端并以某种方式改变输入,进而影响系统功能的过程;而使输出起到与输入相反的作用,使系统输出与系统目标的误差减小,系统趋于稳定的反馈称为负反馈。平衡小车的两个轮子着地,车体只会在轮子滚动的方向上发生倾斜,因此可以通过控制轮子转动,抵消在一个维度上倾斜的趋势便可以保持车体平衡,即如果车体向前倾斜就向前加速运动,向后倾斜就向后加速运动。保持小车稳定平衡的条件是:能够精确测量小车倾角θ的大小和角速度θ的大小以及可以控制车轮的加速度。直立控制主要是保证小车在运行过程中保持在一个理想的角度,这个理想的角度一般由小车的机械零点决定。这个点在实际中大多指的是一个范围。本人在单纯调试直立环的时候调了很多组参数小车的平衡效果还是不好,后来尝试在直立控制的程序中加上或者减去一个小一些的数值,发现效果还行。单纯直立环作用如图4-2所示,在调好参数的情况下小车也是可以保持一定范围和时间的平衡的,但是对外界干扰特别敏感,不稳定,轻轻一推小车就失衡了,因此仅仅使用平衡控制是不足以实现我们想要的平衡效果的,要实现稳定效果,还需要加上速度环。图4-2直立控制原理图直立环代码如图4-3所示:图4-3直立环代码2.2.3速度控制(速度环)小车的速度控制是通过正反馈来实现的。如图4-4所示使输出起到与输入相同的作用,使系统输出与系统目标的误差增大,系统趋于不稳定的反馈称为正反馈;即小车要向前倾斜,就往前更快地加速,以达到再次平衡。举个简单例子进行分析。假设车模开始保持静止,然后增加给定速度,为此需要小车往前倾斜以便获得加速度。在小车直立控制下,为了能够有一个往前的倾斜角度,车轮需要往后运动,这样会引起车轮速度下降,因为车轮往负方向运动。由于负反馈,使得小车往前倾角需要更大。如此循环,小车很快就会倾倒。原本利用负反馈进行速度控制反而成了“正”反馈。由于存在安装误差,传感器实际测量的角度与小车角度有偏差,因此小车实际不是保持与地面垂直,而是存在一个倾角,在重力的作用下,小车就会朝倾斜的方向加速前进。平衡小车的速度是通过调节小车倾角来完成的,平衡小车不同的倾角会引起小车的加减速,从而达到对于速度的控制。图4-4速度控制原理图要调节小车的倾角,首先需要获取小车电机的转动速度,一般都是通过编码器来获取。大多数平衡小车的电机都是带有编码器的,引出AB两相编码器,因为stm32的定时器有编码器模式,因此只需要再特定的GPIO口可以直接连接电机上的编码器接口,就可以读取脉冲获得速度值。直立环与速度环的共同作用,就可以实现小车的平衡了,参数调好了即使推歪小车,小车也会迅速回到平衡位置。速度环代码如图4-5所示:图4-5速度环代码2.2.4转向控制(转向环)实现平衡小车的转向控制的前提是平衡控制和速度控制已经能够实现较好地平衡效果,要是还没调好这两个环,就先不要调转向环。转向控制原理图如图4-6所示,转向控制比较简单,就是给予两个电机差速,例如上位机需要控制小车左转,那么在最终赋值给电机的时候让左轮比右轮慢就行了,即左轮最终赋值减去转向输入值,右轮最终赋值加上转向输入值,两个值大小相同,符号相反。图4-6角速度控制原理图转向控制代码如图4-7所示:图4-7转向控制代码2.2.5定时器PWM输出因为我们在驱动电机的时候是希望它按照一定的速度行驶的,如果直接赋值高低电平来控制,则达不到我们要的控制效果。利用定时器输出PWM波则可以实现上述要求。STM32的定时器除了除了基本定时器TIM6和TIM7外,其他的定时器都可以用来产生PWM输出。其中高级定时器TIM1和TIM8可以同时产生多达7路的PWM输出。而通用定时器也能同时产生多达4路的PWM输出。编码器模式是通用定时器和高级定时器的功能之一。这里要着重注意:stm32定时器只有TIMx_CH1和TIMx_CH2支持编码器模式,如图4-8所示:图4-8通用定时器框图选择编码器接口模式的方法是:如果计数器只在TI2的边沿计数,则置TIMx_SMCR寄存器中的SMS=001;如果只在TI1边沿计数,则置SMS=010;如果计数器同时在TI1和TI2边沿计数,则置SMS=011。通过设置TIMx_CCER寄存器中的CC1P和CC2P位,可以选择TI1和TI2极性;如果需要,还可以对输入滤波器编程。两个输入TI1和TI2被用来作为增量编码器的接口。假定计数器已经启动,即TIMx_CR1寄存器中的CEN=‘1’,计数器由每次在TI1FP1或TI2FP2上的有效跳变驱动。TI1FP1和TI2FP2是TI1和TI2在通过输入滤波器和极性控制后的信号;如果没有滤波和变相,TI1FP1=TI1,TI2FP2=TI2。根据两个输入信号的跳变顺序,产生了计数脉冲和方向信号。依据两个输入信号的跳变顺序,计数器向上或向下计数,同时硬件对TIMx_CR1寄存器的DIR位进行相应的设置。不管计数器是依靠TI1计数、依靠TI2计数或者同时依靠TI1和TI2计数。在任一输入端(TI1或者TI2)的跳变都会重新计算DIR位。编码器接口模式基本上相当于使用了一个带有方向选择的外部时钟。这意味着计数器只在0到TIMx_ARR寄存器的自动装载值之间连续计数,要么是0到ARR计数,要么是ARR到0计数。所以在开始计数之前必须配置TIMx_ARR;同样,捕获器、比较器、预分频器、触发输出特性等仍工作如常。在这个模式下,计数器依照增量编码器的速度和方向被自动的修改,因此计数器的内容始终指示着编码器的位置。计数方向与相连的传感器旋转的方向对应。如图4-9所示:图4-9编码器下计数器的操作当定时器配置成编码器接口模式时,提供传感器当前位置的信息。使用第二个配置在捕获模式的定时器,可以测量两个编码器事件的间隔,获得动态的信息如速度、加速度、减速度。指示机械零点的编码器输出可被用做此目的。根据两个事件间的间隔,可以按照固定的时间读出计数器。如果可能的话,可以把计数器的值锁存到第三个输入捕获寄存器,但是捕获信号必须是周期的并且可以由另一个定时器产生,也可以通过一个由实时时钟产生的DMA请求来读取它的值。PWM的代码如图4-10所示:图4-10PWM代码2.2.6六轴传感器获取角度平衡小车制作的核心部分是倾角和加速的的获取,想要使小车站立和运动,这两种参数的获取是非常关键的,前者是控制小车直立的关键参数,后者是速度环和转向环的关键参数。平衡小车使用MPU-6050六轴传感器模块来获取角度和加速度值,MPU-6050六轴传感器模块的本质是三轴陀螺仪倾角输出+三轴加速度计速度输出,并带有温度传感器。该模块的通信方式是IIC通信,芯片内置16字节AD转换器,能输出16位数据。得到输出的数据后,可利用自带的数字运动处理器硬件加速引擎,通过主IIC接口,向应用端输出姿态解算后的数据。SCL和SDA是连接STM32F103芯片的IIC接口,MCU通过这个IIC接口来控制MPU6050。VLOGIC是IO口电压,该引脚最低可以到1.8V,一般直接接VDD即可。AD0是从IIC接口接MCU控制引脚,该引脚控制IIC地址的最低位。如果接GND,则MPU6050的IIC地址是:0X68,如果接VDD,则是0X69。2.5卡尔曼滤波算法陀螺仪具精确但有零点漂移特性,其测量误差会随着时间的累加而不断的累积,从而影响测量精度。因此,短时间测量应信任陀螺仪。加速度计一直受到平衡车振动的影响,混叠额外的高频振动量干扰,但是漂移小。因此,长时间测量应信任加速度计。所以单一的传感器测量难以得到精确的姿态角度。需采用多传感器信号融合的方法,来获得准确的姿态角度量。卡尔曼滤波,可以设置分配信任权重,解决陀螺仪零漂、滤除加速度计高频振动,递归自学习,滤波平滑、跟随快。虽然算法较复杂,占用较多线程时间,但是我们选用的freescaleK60处理器完全可以承担这样的计算,所以决定使用该滤波算法。最后,就是卡尔曼滤波算法的应用了。代码如下:floatKal_Gyro;floatgyro;int16_tmmax,mmaz;floatangle,angleSpeed;floatAngle_Kalman;int16_tPre_Angle_Kalman;floatangleSpeedIntegral;floatQ_bias,Angle_err;floatPCt_0,PCt_1,E;floatK_0,K_1,t_0,t_1;floatPdot[4]={0,0,0,0};floatPP[2][2]={{1,0},{0,1}};staticfloatKalmanFilter(floatAccel,floatGyro) {staticfloatAngle=0,Gyro_y=0; Angle+=(Gyro-Q_bias)*dt;//先验估计 Pdot[0]=Q_angle-PP[0][1]-PP[1][0];//Pk-先验估计误差协方差的微分 Pdot[1]=-PP[1][1]; Pdot[2]=-PP[1][1]; Pdot[3]=Q_gyro; PP[0][0]+=Pdot[0]*dt;//Pk-先验估计误差协方差微分的积分 PP[0][1]+=Pdot[1]*dt;//先验估计误差协方差 PP[1][0]+=Pdot[2]*dt; PP[1][1]+=Pdot[3]*dt; Angle_err=Accel-Angle; //zk-先验估计 PCt_0=C_0*PP[0][0]; PCt_1=C_0*PP[1][0]; E=R_angle+C_0*PCt_0; K_0=PCt_0/E; K_1=PCt_1/E; t_0=PCt_0; t_1=C_0*PP[0][1]; PP[0][0]-=K_0*t_0; //后验估计误差协方差 PP[0][1]-=K_0*t_1; PP[1][0]-=K_1*t_0; PP[1][1]-=K_1*t_1; Angle +=K_0*Angle_err; //后验估计 Q_bias +=K_1*Angle_err; //后验估计 Gyro_y=Gyro-Q_bias; //输出值(后验估计)的微分=角速度 returnAngle;}由此可得卡尔曼滤波的5个方程。

先验估计:X(k|k-1)=AX(k-1|k-1)+BU(k)协方差矩阵的预测:P(k|k-1)=AP(k-1|k-1)A’+Q计算卡尔曼增益:Kg(k)=P(k|k-1)H’/(HP(k|k-1)H’+R)通过卡尔曼增益进行修正:X(k|k)=X(k|k-1)+Kg(k)(Z(k)-HX(k|k-1))更新协方差阵:P(k|k)=(I-Kg(k)HP(k|k-1)(1)根据角度微分等于时间的微分乘以角速度,k时刻角度可近似认为是k-1时刻的角度值加上k-1时刻陀螺仪测得的角加速度值乘以时间,同时要减去陀螺仪的静态漂移。angle_kalman+=(angle_speed_kal-Kal_Gyro_Zero)*Kalman_Sample_Time;Kalman_Sample_Time是卡尔曼滤波采样时间参量,应调节到使滤波平滑,跟随快滞后少,此处为0.0055。对应卡尔曼滤波第一个方程,先验估计得:X(k|k-1)=AX(k-1|k-1)+BU(k)(2)预测方差阵的预测值,噪声就是数据的方差值#define

Q_angle

0.001

//角度噪声#define

Q_gyro

0.0005//漂移噪声Pdot[0]=Q_angle-Pk[0][1]-Pk[1][0];Pdot[1]=-Pk[1][1];Pdot[2]=-Pk[1][1];Pdot[3]=Q_gyro;Pk[0][0]+=Pdot[0]*Kalman_Sample_Time;Pk[0][1]+=Pdot[1]*Kalman_Sample_Time;Pk[1][0]+=Pdot[2]*Kalman_Sample_Time;Pk[1][1]+=Pdot[3]*Kalman_Sample_Time;对应卡尔曼滤波第二个方程,协方差矩阵的预测:P(k|k-1)=AP(k-1|k-1)A’+Q(3)计算卡尔曼增益K_0,K_1#define

R_angle

0.05

//角度测量噪声值

angle_err=angle_kal-angle_kalman;PCt_0=C_0*Pk[0][0];PCt_1=C_0*Pk[1][0];E=R_angle+C_0*PCt_0;K_0=PCt_0/E;K_1=PCt_1/E;对应卡尔曼滤波第三个方程,计算卡尔曼增益得:Kg(k)=P(k|k-1)H’/(HP(k|k-1)H’+R)(4)对矩阵Pk进行更新t_0=PCt_0;t_1=C_0*Pk[0][1];Pk[0][0]-=K_0*t_0;Pk[0][1]-=K_0*t_1;Pk[1][0]-=K_1*t_0;Pk[1][1]-=K_1*t_1;对应卡尔曼滤波第五个方程,更新协方差阵得:P(k|k)=(I-Kg(k)HP(k|k-1)(5)通过卡尔曼增益修正当前的角度、陀螺仪零点,算出当前角速度angle_kalman+=K_0*angle_err;Kal_Gyro_Zero+=K_1*angle_err;Kal_Gy

温馨提示

  • 1. 本站所有资源如无特殊说明,都需要本地电脑安装OFFICE2007和PDF阅读器。图纸软件为CAD,CAXA,PROE,UG,SolidWorks等.压缩文件请下载最新的WinRAR软件解压。
  • 2. 本站的文档不包含任何第三方提供的附件图纸等,如果需要附件,请联系上传者。文件的所有权益归上传用户所有。
  • 3. 本站RAR压缩包中若带图纸,网页内容里面会有图纸预览,若没有图纸预览就没有图纸。
  • 4. 未经权益所有人同意不得将文件中的内容挪作商业或盈利用途。
  • 5. 人人文库网仅提供信息存储空间,仅对用户上传内容的表现方式做保护处理,对用户上传分享的文档内容本身不做任何修改或编辑,并不能对任何下载内容负责。
  • 6. 下载文件中如有侵权或不适当内容,请与我们联系,我们立即纠正。
  • 7. 本站不保证下载资源的准确性、安全性和完整性, 同时也不承担用户因使用这些下载资源对自己和他人造成任何形式的伤害或损失。

评论

0/150

提交评论