【《智能车循迹控制设计硬件系统分析》12000字】

智能车循迹控制设计硬件系统分析目录TOC\o"1-3"\h\u6432智能车循迹控制设计硬件系统分析 1298351.1电机驱动电路 2246031.1.1TB6612FNG介绍 2216071.1.2电机驱动电路 23181.2电机正反转和速度控制程序设计 244681.2.1电机的正反转实现分析 3165911.2.2电机运转方式的测试方法 3261641.2.3电机调节运转速度的原理分析 385341.2.4电机速度控制程序设计 4237941.3避障电路设计 4220851.1.1HC-SR04的介绍 4195261.1.2HC-SR04驱动电路 5207381.1.3超声波避障程序设计 5103741.1.4超声波模块工作原理分析 6253491.1.5模块使用思路 6229971.1.6超声波模块使用说明 79561.4红外循迹电路设计 893391.4.1ITR20001-T的介绍 8210981.4.2红外循迹模块电路 8170981.4.3红外循迹模块程序设计 9171661.4.4红外循迹模块工作原理分析 11200431.4.5红外循迹模块检测电路的设计 11184071.5自平衡系统设计 1227571.5.1MPU-6050介绍 12281651.5.3传感器程序设计 14204571.5.4MPU-6050两种传感器原理分析 1566001.5.5融合加速度传感器和陀螺仪的数据 22302661.5.6消除累计误差 23226891.5.7PID控制原理 2462441.5.8电机速度闭环内环PID控制 24311441.6.1速度闭环PID控制程序设计 25132861.6.3直立平衡角度环程序设计 261.1电机驱动电路利用前面介绍的驱动模块的选择，本研究中使用的是TB6612FNG驱动模块，两个直流电动机。1.1.1TB6612FNG介绍TB6612FNG通过相比对于l298N的额定热耗性和外围电源二极管的额定续流工作电流,TB6612FNG能够直接承受的直流负载工作电流较大,而且我们可以不用额定的电流外加散热片,直接通过使用一个外界电源滤波器和电容器来作为直流电源就这样能够直接使得主发电机正常工作运行，因此，电机驱动模块可以设计得很小巧。1.1.2电机驱动电路图3-1电机驱动电路原理图1.2电机正反转和速度控制程序设计电机的前进后退是基于对TB6612FNG驱动系统的研究得出的。1.2.1电机的正反转实现分析图3-2TB6612引脚图红框部分标注的端口如：AO1、AO2、PWMA、，AN1、AN2都是用来控制一号电机，蓝框部分用于控制二号电机，接线方式很简单，PWM对应单片机的PWM输出端口即可，下面是电机转动的真值表，以1号电机为例：表3-1一号电机真值表AIN1010AIN2001转动方式停止正转反转1.2.2电机运转方式的测试方法按照以下方式接线：VM直接接电池正极（注意：电压<15v）；如果控制电机运行在满占空比正转状态，使得AIN1接5V，GND接AIN2,同理反转的话，AIN1接GND即可。可以简单得出电机运转方式。1.2.3电机调节运转速度的原理分析单片机IO口需要发送高电平的数字信号,IO口只能同时发送高电平和低电平,假设高电平为5V,低电平为0V,则必须通过改变IO口输出的波形方向频率占空比,得到模拟信号和数字电压信号,我们可以知道在一个重复的脉冲序列中产生的电压与LED灯、直流电机、接触器都是直流电源的输出，分离直流电源相同，通过控制触点断开时间，即模拟电压不超过最大值ADDINEN.CITE<EndNote><Cite><Author>Blaabjerg</Author><Year>2006</Year><RecNum>11</RecNum><DisplayText>[11]</DisplayText><record><rec-number>11</rec-number><foreign-keys><keyapp="EN"db-id="wvxfdfpx6xwp5he2sf6p9svsvae90atdv29v"timestamp="1620308810">11</key></foreign-keys><ref-typename="JournalArticle">17</ref-type><contributors><authors><author>Blaabjerg,F.</author><author>Teodorescu,R.</author><author>Liserre,M.</author><author>Timbus,A.V.</author></authors></contributors><titles><title>Overviewofcontrolandgridsynchronizationfordistributedpowergenerationsystems</title><secondary-title>IeeeTransactionsonIndustrialElectronics</secondary-title></titles><periodical><full-title>IeeeTransactionsonIndustrialElectronics</full-title></periodical><pages>1398-1409</pages><volume>53</volume><number>5</number><dates><year>2006</year><pub-dates><date>Oct</date></pub-dates></dates><isbn>0278-0046</isbn><accession-num>WOS:000241050400002</accession-num><urls><related-urls><url><GotoISI>://WOS:000241050400002</url></related-urls></urls><electronic-resource-num>10.1109/tie.2006.881997</electronic-resource-num></record></Cite></EndNote>[11]。举个例子,假设每个高电平的持续时间和低电平持续时间相同都只能是一半，那么在固定的频率下，假设电源电压为5V，则模拟电压为2.5V。图3-3PWM调节电压原理图PWM调节功能是基于占空比控制，输出功率越大，开关电阻越小，占空比越低，平均电压输出信号，通过控制线圈电压，改变直流电压大小，实现不同工作周期下的平均电压降，在给定频率下获得不同的模拟输出电压，从而实现直流电机控制ADDINEN.CITE<EndNote><Cite><Author>Rodriguez</Author><Year>2002</Year><RecNum>13</RecNum><DisplayText>[12]</DisplayText><record><rec-number>13</rec-number><foreign-keys><keyapp="EN"db-id="wvxfdfpx6xwp5he2sf6p9svsvae90atdv29v"timestamp="1620309110">13</key></foreign-keys><ref-typename="JournalArticle">17</ref-type><contributors><authors><author>Rodriguez,J.</author><author>Lai,J.S.</author><author>Peng,F.Z.</author></authors></contributors><titles><title>Multilevelinverters:Asurveyoftopologies,controls,andapplications</title><secondary-title>IeeeTransactionsonIndustrialElectronics</secondary-title></titles><periodical><full-title>IeeeTransactionsonIndustrialElectronics</full-title></periodical><pages>724-738</pages><volume>49</volume><number>4</number><dates><year>2002</year><pub-dates><date>Aug</date></pub-dates></dates><isbn>0278-0046</isbn><accession-num>WOS:000177191000002</accession-num><urls><related-urls><url><GotoISI>://WOS:000177191000002</url></related-urls></urls><electronic-resource-num>10.1109/tie.2002.801052</electronic-resource-num></record></Cite></EndNote>[12]。1.2.4电机速度控制程序设计对电机进行初始化设置代码如下：GPIO_InitStructure.GPIO_Pin=GPIO_Pin_6|GPIO_Pin_7;GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF_PP;//GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;电机正反转程序MoterFord()MoterReverse(),见附录1.3避障电路设计使用HC-SR04超声波模块实现避障功能。1.1.1HC-SR04的介绍避障模块设计选择的HC-SR04超声波测距模块，可提供2cm-400cm的非接触式距离感测功能，拥有测距精度可高达3mm的非接触式距离感测功能，模块包括超声波发射器、接收器与控制电路。1.1.2HC-SR04驱动电路图3-4HC-SR04驱动电路原理图由原理图可以看到，驱动电路由一个四路放大运算器LMC6034IM、单片机EM78PI53、MAX232构成。上图R3可以调节最大测试距离，R3的电阻为392欧姆，探测距离为4.5m左右，探测角度小于15°；R3电阻为472欧姆，其探测距离为7m左右，探测角度小于30°。电阻的位置和大小决定了,接受信号的是否有收益,电阻越高,检测的距离就越大,但同时被检测的角度也就会逐渐变大,容易被旁边的其他物体影响。1.1.3超声波避障程序设计图3-5超声波传感器引脚接线图SEM32的PA11引脚点解HC-SR04的Echo引脚，PA2引脚连接Trig引脚。其中，PA11引脚作为高级定时器TIM1的CH4通道使用。打开Keil5，配置PA2的引脚为GPIO_Output模式，并将UserLabel改为Trig。然后，配置高级定时器1（TIM1）CH4通道。Channel4配置方法为InputCapturedirectmode，在ParameterSettings中配置了相关参数，Prescaler预分频系数设定为71，计数时钟频率就是72MHz/(71+1)=1MHz，CounterMode的计数模式被设定为向上计数Up，此时1us进行计数一次自动加载的最大值被设定为16bits最大值0xFFFF。PolaritySelection在这里预设为RisingEdge上升沿触发。点击GENERATECODE生成代码。生成代码后，我们先在main函数中开启定时器，代码如下：HAL_TIM_IC_Start_IT(&htim1,TIM_CHANNEL_4);//自动打开TIM1的一个捕获信息通道并且自动打新的捕获通道中断HAL_TIM_ENABLE_IT(&htim1,TIM_IT_UPDATE);//使能更新中断之后我们要了解两个输出中断伺服函数,第一个伺服函数指的是通过输入来捕获一个中断回调函数,当其具有一个变化线路沿的位置时候就可以直接进入这个函数：//voidHAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef*htim)//（捕捉到中断事件在此执行）另外一个函数采用满周期回调中断函数,当计数器的计数满时,进入回调函数,进入回调函数的途径由两种:一种没有通过电源来触发上升沿或者是下降路线进入回调中断,另一种就是计数器的电源来计满而进入中断。部分代码如下:if((Distance>=0)&&(Distance<=20))//右转750个脉冲，转弯角度约为90度Steer(5,0);g_iLeftTurnRoundCnt=750;g_iRightTurnRoundCnt=-750;1.1.4超声波模块工作原理分析超声波发射器向某一方向发射超声波，在发射的同时开始计时，超声波在空气中传播，途中碰到障碍物就立即返回来，超声波接收器收到反射波就立即停止计时。已知声波在空气中的传播速度为340m/s，根据计时器记录的时间t，就可以计算出发射点距障碍物的距离s，即：s=340m/s*t/2s=340m/s∗t/2。1.1.5模块使用思路使用模块时，首先要将ECHO和TRIG初始化，也就是置0，首先向给TRIG发送至少10us的高电平脉冲（模块自动向外发送8个40KHz的方波），然后在等待,捕捉一个上升路程的同时,定时器就会开始进行计时,随后紧接着在等待一个下降路程的捕捉,如果这个下降路程被捕捉到那么定时器的运动时间也就是声波来回两个路程的运动时间,显而易见测试距离=（时间*340m/s）/2。捕捉上升沿和下降沿需要用到STM32的输入捕获功能ADDINEN.CITE<EndNote><Cite><Author>张艳</Author><Year>2016</Year><RecNum>14</RecNum><DisplayText>[13]</DisplayText><record><rec-number>14</rec-number><foreign-keys><keyapp="EN"db-id="wvxfdfpx6xwp5he2sf6p9svsvae90atdv29v"timestamp="1620309404">14</key></foreign-keys><ref-typename="JournalArticle">17</ref-type><contributors><authors><author>张艳</author><author>贾应炜</author></authors></contributors><auth-address>陕西工业职业技术学院;</auth-address><titles><title>基于HC-RS04模块的超声波测距系统设计%J自动化技术与应用</title></titles><pages>101-104+109</pages><volume>35</volume><number>03</number><keywords><keyword>超声波测距</keyword><keyword>单片机STC89C52RD</keyword><keyword>HC-RS04模块</keyword><keyword>LCD1602</keyword></keywords><dates><year>2016</year></dates><isbn>1003-7241</isbn><call-num>23-1474/TP</call-num><urls></urls><remote-database-provider>Cnki</remote-database-provider></record></Cite></EndNote>[13]。图3-6超声波波形图图3-6示意图用①②③④⑤进行了标注，分别对应：①先通过设置的输入被捕获作为一个上升线的检测；②记录发生上升沿时TIMx_CNT(计数器)的值；③配置的捕获信号是下降路线的捕获,当下降路线到来的情况下发生了捕获；④记录此时的TIMx_CN(计数器)T的值；⑤一个高电平的脉宽值就是一个计数器先后两次的最大值的相差,同时对于计数器进行时间的测量可以按照计数频率来得到,捕获一次开始,计数器的最大值会由cpu自动复制出来送到寄存器中,然后再由计数器开始进行计数,再次被捕获是，比较寄存器会用之后的值减去之前的值。1.1.6超声波模块使用说明1.采用IO口TRIG触发测距，给至少10us的高电平信号；2.该控制模块自动向每个40khz的方波信号发送一个固定频率,自动快速判断其中是否已经存在一个射频信号源并进行频率返回；1.有信号返回，通过IO口ECHO输出一个高电平，高电平持续的时间就是超声波从发射到返回的时间。测试距离=(高电平时间*声速)/2;（注：超声波在空气中速度为340m/s）超声波时序图，如图3-7所示。图3-7超声波时序图由一个超声波的时序曲线图可知,触发超声波信号的要求是10us以上，HC-SR04超声波模块内部将发出8个40KHz周期电平并检测回波。一旦检测到有回波信号输出回响信号。回响信号脉冲宽度与所测的距离成正比。公式：us/58=厘米或者us/148=英寸；或者距离=高电平时间*声速（340M/S）/2。测量周期为60ms以上，以防止发射信号对回响信号的影响ADDINEN.CITE<EndNote><CiteExcludeAuth="1"ExcludeYear="1"><Author>苏红艳</Author><RecNum>15</RecNum><DisplayText>[14]</DisplayText><record><rec-number>15</rec-number><foreign-keys><keyapp="EN"db-id="wvxfdfpx6xwp5he2sf6p9svsvae90atdv29v"timestamp="1620311039">15</key></foreign-keys><ref-typename="Patent">25</ref-type><contributors><authors><author>苏红艳</author></authors></contributors><auth-address>223003江苏省淮安市高教园区枚乘东路3号</auth-address><titles><title>一种基于超声波距离检测的无人驾驶汽车避障方法</title></titles><dates><pub-dates><date>2021-04-16</date></pub-dates></dates><isbn>CN112666950A</isbn><work-type>发明公开</work-type><urls></urls><remote-database-name>中国专利PageCount-页码12</remote-database-name><remote-database-provider>Cnki</remote-database-provider></record></Cite></EndNote>[14]。1.4红外循迹电路设计红外寻迹模块由四个ITR20001-T红外发射接受管与一个LM339电压比较器构成，之所以选用LM339电压比较器，是因为该电压比较器失调电压小，典型值为2mV；电源电压范围宽，单电源为2-36V，双共模电源上的电压限制范围一般为0~±1v~±18v;对于这种比较性共模信号来说,其内部相对电流小和阻力小的限制更广;对于共模信号区间的电压范围很大,为0~(UCC-1.5v)ADDINEN.CITE<EndNote><Cite><Author>苏晓峰</Author><Year>2021</Year><RecNum>16</RecNum><DisplayText>[15]</DisplayText><record><rec-number>16</rec-number><foreign-keys><keyapp="EN"db-id="wvxfdfpx6xwp5he2sf6p9svsvae90atdv29v"timestamp="1620311219">16</key></foreign-keys><ref-typename="JournalArticle">17</ref-type><contributors><authors><author>苏晓峰</author><author>王海涛</author><author>程一平</author><author>杨嵛媛</author><author>肖又文</author><author>黄旻璇</author></authors></contributors><auth-address>西安建筑科技大学机电工程学院;</auth-address><titles><title>自动循迹避障灭火智能小车设计%J工业控制计算机</title></titles><pages>134-135+143</pages><volume>34</volume><number>04</number><keywords><keyword>单片机</keyword><keyword>循迹</keyword><keyword>避障</keyword><keyword>灭火</keyword><keyword>智能小车</keyword></keywords><dates><year>2021</year></dates><isbn>1001-182X</isbn><call-num>32-1764/TP</call-num><urls></urls><remote-database-provider>Cnki</remote-database-provider></record></Cite></EndNote>[15]。1.4.1ITR20001-T的介绍红外循迹模块选用的传感器是ITR20001-T，其优点是便宜，容易购买，不易被其他物体干扰，简单便捷。探测器的原理就是发射接收红外线，然后将新号传递给电压比较器，经过触发器整合输出成低电平。1.4.2红外循迹模块电路红外循迹模块由四个IRT2000-T红外发射接收管组成加上一个LM339电压比较器构成，原理图如图3-8。图3-8红外循迹模块电路图1.4.3红外循迹模块程序设计图3-9红外循迹模块引脚接线图从原理图中可以看到，四路红外模块的Ra、Rb、La、Lb引脚分别连接STM32的PA15、PA8、PB5、PB3引脚。由于红外模块是反馈高低电平（黑线为0，反之为1），我们只要设置引脚为GPIO，读取到红外实时电平，就能判断线在哪一边。打开STM32CubeMX工程。如果之前我们在SYS的DEBUG（仿真）选项中选择了JTAG(4PIN)，那么PA15就会被占用了。在这里，我们需要把PA15分配给红外使用，而核心板中只是预留了SWD接口，所以把DEBUG（仿真）选项中设为SerialWire。接着，把PA15、PA8、PB5、PB3这四个引脚设置为GPIO_Input模式，并分别命名为Ra、Rb、La、Lb，同时设置为上拉模式Pull-up。如图3-10所示。图3-10STM32CubeMX设置列举部分代码：#ifINFRARE_DEBUG_EN>0 charbuff[32]; #endif charresult; floatdirect=0; floatspeed=0;result=InfraredDetect(); if(result&infrared_channel_Lc) direct=-10; elseif(result&infrared_channel_Lb) direct=-6; elseif(result&infrared_channel_La) direct=-4; elseif(result&infrared_channel_Rc) direct=10; elseif(result&infrared_channel_Rb) direct=6; elseif(result&infrared_channel_Ra) direct=4; else direct=0.0;speed=3;Steer(direct,speed);1.4.4红外循迹模块工作原理分析红外光电管总共由两种，发射管是透明LED，工作过程中可以发出红外光，还有一个是黑色接收管，会根据光强来变化阻止。另外无论是分体式的还是一体的，它们的检测原理都是相同的，根据光学原理，由于黑色吸收任何颜色的光线，所以本研究中，我们设计的是识别黑线而不是其他颜色的线，另外为了确保实验的准确性，采用的是白底黑线的路面，因为白色的底板会反射所有的光线，所以选择白色的底板。从电子元器件角度来讲：由于黑色会吸收所有的光源，当红外发射管发出红光时，红光会被吸收，所以就无法被接收管接受，所以表现在电信号上，电阻大，反之，当白光遇到红光，由于白色会反射大部分光源，所以表现在电信号上就是高电平。黑白两种地面会出现电平高低，再将信号传给单片机ADDINEN.CITE<EndNote><Cite><Author>林永杰</Author><RecNum>17</RecNum><DisplayText>[16]</DisplayText><record><rec-number>17</rec-number><foreign-keys><keyapp="EN"db-id="wvxfdfpx6xwp5he2sf6p9svsvae90atdv29v"timestamp="1620311381">17</key></foreign-keys><ref-typename="Patent">25</ref-type><contributors><authors><author>林永杰</author><author>黄紫林</author><author>许伦辉</author></authors></contributors><auth-address>510640广东省广州市天河区五山路381号</auth-address><titles><title>一种基于机器视觉与红外传感器的无人驾驶小车避障系统</title></titles><dates><pub-dates><date>2020-12-25</date></pub-dates></dates><isbn>CN212229485U</isbn><work-type>实用新型</work-type><urls></urls><remote-database-name>中国专利PageCount-页码10</remote-database-name><remote-database-provider>Cnki</remote-database-provider></record></Cite></EndNote>[16]。1.4.5红外循迹模块检测电路的设计介绍完红外探测器的原理，这一节介绍外接电路，除了发射与接受外，必须要有外接电路才能将电讯号传递给STM32主控。使用的LM339是一个集成在内部的思路比较器，由于其内部结构基本相同，所以我们举一个图3-10具体说明：图3-11集成电路图其使用原理可以分为四点：比较器输出端由于结构类似于不接集电极的电阻，所以必须要串联上拉电阻（3k-15k）此时输出端的电压就由电阻大小决定了，这里我们一般选10k就可以如图所示R3为分压电阻，其作用是给电压比较器提供比较电压，但是电压的设定是由R2电阻上的电压所决定的，R3的主要作用是用来根据不同道路，不同地面的需要来调节比较电压图中R1和R2的阻抗数值和大小均是由红外发射管的功耗数值所决定,其被称为一个限流式的电阻,R与功率P成正正反比,R1越小,发射红外光的功率就会变得更加巨大,但多个限流式电阻并联会直接导致小车的功耗也在不断地增加，好处是这样扩大了光电管的探测范围。红外接收管的内部电阻，正常设计时，可以增添一个发光二极管，根据发光二极管的亮灭情况来判断电阻选择是否合适。最后，在设计时，4与5端口输入可以互换，可以将传感器的新号从比较器的正端输入，只需要注意接在不同输入端时，输出新号不相同。1.5自平衡系统设计小车的自平衡系统设计包括MPU-6050传感单元，对MPU-6050传感单元导出的数据的加速度g，速度，角速度，进行函数处理，用PD控制来保持小车的平衡，PI控制来控制小车的速度，对于角度的获取方面，应用了DMP算法直接输出角度ADDINEN.CITE<EndNote><Cite><Author>贾树林</Author><Year>2020</Year><RecNum>21</RecNum><DisplayText>[17]</DisplayText><record><rec-number>21</rec-number><foreign-keys><keyapp="EN"db-id="wvxfdfpx6xwp5he2sf6p9svsvae90atdv29v"timestamp="1620804700">21</key></foreign-keys><ref-typename="JournalArticle">17</ref-type><contributors><authors><author>贾树林</author><author>马双宝</author></authors></contributors><auth-address>武汉纺织大学机械工程与自动化学院;</auth-address><titles><title>基于STM32F429两轮平衡车的研制%J武汉纺织大学学报</title></titles><pages>57-63</pages><volume>33</volume><number>05</number><keywords><keyword>STM32F429IGT6</keyword><keyword>两轮小车</keyword><keyword>自平衡</keyword><keyword>传感器MPU6050</keyword></keywords><dates><year>2020</year></dates><isbn>2095-414X</isbn><call-num>42-1818/Z</call-num><urls></urls><remote-database-provider>Cnki</remote-database-provider></record></Cite></EndNote>[17]。1.5.1MPU-6050介绍MPU-6050是目前世界上第一个将6轴集成Motiontracking的六轴集成式驱动装置。它由三轴运动陀螺仪、S三轴运动加速度计和三轴运动信号处理器DMP等等构成,内部的元件I2CP可用来与主机外接运动传感器。其主要特点包括：1.将传感器的数据以旋转矩阵、欧拉角格式输出2.探测距离广，敏感度高达131LSB/°/sec1.程序可以控制集成，范围从2g到16g4.将振动加速器和振动陀螺仪之间的振动灵敏度干扰消除,使其对进行设定时所需要给予的振动影响和相机飞行飘逸被大大减少5.其内置的DMP可以简化演算数据、传感器同化的复合6.增加了磁力感测器技术，降低客户的使用难度，避免额外的校正7.内置温度传感器，可以将温度转化成数字8.带有数字式的数码输入视频同步引脚(Syncpin)可以支持数字视频和图像电子数码摄像机的输入影相性和稳定性相关技术和技术GPS9.MPU-6050支持对物体的姿势识别与判断、其可以将画面随意的放大缩小，并且拥有可以通过程序控制的中断，能够实现快速中断，使用者可以通过触摸感应，晃动物体感应，甚至当物体保持不动时也能做到零动作感应10.可以使用5mA的电流来使陀螺仪正常工作，为了节约电量，传感器待机时的电流仅为5uA，装备的省电模式，自动开启时，仅需40uA。11.采用的新型FIFO，给系统降低了功耗12.高达400Khz的IIC通信接口11.封装尺寸小：4x4x0.9mm（QFN）管脚表如下：表3-2管脚表管脚名称说明VCC1.3-5V(内部有稳压芯片）GND地线SCLMPU-6050作为从机时IIC时钟线SDAMPU-6050作为从机时IIC时钟线XCLMPU-6050作为主机时IIC时钟线XDAMPU-6050作为主机时IIC时钟线AD0决定IIC地址的最后一位将外部MCU的IIC主接口与SCL与SDA,MCU也就是可以直接通过外部MCU主机的接口进行控制MPU-6050,外部MCU也就是传感器,它们也可以直接通过XCL与XDA从外部进行连接，组成多轴传感器。AD0作为地址管脚，将其接GND，那么地址就是0X68，将VDD与之连接，则是0X69，因为GND默认接AD0，所以决定了MPU-6050D的地址是0X68。1.5.2传感器模块电路设计传感器模块部分主要是一个LED灯、一个MPU-6050传感器、电源构成，LED灯主要用于判断MPU-6050是否处于工作状态。图3-12MPU-6050传感模块接线图1.5.3传感器程序设计通过3-11原理图得知，两轮自平衡小车通过STM32的I2C1接口与MPU-6050传感器进行通信。这里使用的是轮询模式，只需要配置PB8：I2C1_SCL、PB9：I2C1_SDA。如果使用中断模式，则还要使用一个GPIO引脚连接MPU-6050模块的INT引脚。打开Keil工程。在左侧Pinout&Configuration界面中的Connectivity下拉中点击I2C1，然后在I2C1ModeandConfiguration的Mode中将I2C选择为I2C。而其他地方，比如Configuration中的ParameterSetting选项卡全部保持默认参数（标准模式，100k速率）即可。然后生成代码，找到已经用STM32HAL库封装好的MPU6050代码文件：mpu-6050.c、mpu-6050.h，这两个文件主要就是使用HAL库里的I2C函数进行改写，可以直接调用。将mpu-6050.c复制到Scr文件中即可，将mpu-6050.h放入inc文件中。打开MDK-ARM软件，在左侧目录中双击Application/User，将Src文件夹的mpu6050.c加入工程。打开main.h头文件，加入mpu6050.h文件打开main.c文件，在/*USERCODEBEGIN1*/和/*USERCODEEND1*/之间定义以下新变量，并且将上个实验的变量注释掉：图3-13设置三轴参数在/*USERCODEBEGIN2*/和/*USERCODEEND2*/之间敲入以下新代码，注释掉上个实验留下的代码：图3-14设置MPU-6050引脚参数在MDK-ARM中重新编译代码，把代码烧录进小车的核心板，并用数据线连接核心板和电脑，打开串口助手软件，对小车进行复位，能够看到小车发送MPU-6050InitSuccessfully到串口助手上，代表MPU6050初始化成功，而且每隔0.5秒就将加速度和陀螺仪数据发送上去，我们转动小车可以看到数据随之变化。1.5.4MPU-6050两种传感器原理分析陀螺仪、加速度计都是惯性测量元件的一种。而MPU-6050传感器的内部同时集成了陀螺仪和加速度传感器两种惯性测量元件。加速器传感器：图3-15模拟方盒图我们可以把加速度传感器想象为一个盒子模型——一个小球在一个方盒子中。如果我们把这个盒子放在真空中的话，在真空中，一切物体处于无重力状态，所以X,Y,Z轴方向上ɡ=0，小球也不例外将悬浮在盒子的中间。图中,你可以观察到由6面墙被平均分配给了每个轴，但是由于为了清晰的观察，这里我们不展示Y+这一面。假设每一面墙都是弹性的，可以通过某些仪器检测到压力。研究开始：我们将盒子向左突然运动，那么由于惯性的原理，球会撞上X-X−墙。这时通过仪器检测墙面的压力得到X轴输出值为-1g。3-16模拟动态图惯性力根据定义来讲，他与运动的实际方向是相反的，如果我们用加速度传感器来测量力的话，那么这个物体自身运动时其加速度的方向势必会与我们用传感器测量到的力方向是相反的。换句话说，用方盒模型，其目的就是通过惯性力的方法耿直观地让研究者明白，加速度传感器的原理就是通过测量物体对墙面的作用来测量，我们在生活中同样也会采用这样的转换法，比如测量物体的重力，我们是通过弹簧测力计的弹力来间接反映，这个力是由重力加速度引起的，但是接下来的例子不是这样子的。众所周知地球上的物体都会受到重力g=9.8m/s^2的作用。地球上任意取一个圆球，这时圆球不再是悬浮的，显而易见的是这个圆球对它下方的Z-面造成了一个1g的压力（方向忽略）。3-17模拟动态图在这样的情况下,箱子不再进行移动,但我们依旧能够准确地读取z轴上所有-1g的值。因为当球体在墙上所施加的压力都是由于重力。从理论上看,它可能是不同形式和类型的导向力。例如,你可以很好地想象一个盒子里的一个小圆球都是带有铁质的,将一个小巨大的磁铁球直接摆在盒子旁边,小球就有可能会直接撞上另一面墙，这时圆球受到的是磁场力。引用这个实例仅仅是为了阐述一种方法来说明一个加速度传感器在其本质上是一种检测力量而不是一种加速度。只是由于加速度引起的惯性应变力正好可以通过对于加速度传感器中的检查设备进行捕捉。尽管这个盒式化的模型不是一个MEMS传感器的现实建立和构造,但它被用于解决一些与加速度传感器密切相关的问题是一个很有效的方法。实际上有些像素式传感器中含有很多金属小球,它们可以被称为倾角式的开关,但是它们的特点比较薄弱,仅仅能够检测出设备在某种范围内是否发生了倾斜,却无法得到其倾斜的大小。迄今为止,我们分析了一种单轴加速度传感器的输出。三轴式加速度传感器真正的价值是因为它们可以检测三条轴的惯性运动。这样就让我们重新回归到了盒子的模型,并将一个箱子左右方向旋转45度。盒子模型依然是放在地球上。现在这个球会与两个面相互接触:Z-和X-，见图3-18。3-18z-x模拟图其中，0.71g这个数字实际上是不是随意写入上去的，它们实际上是QUOTE1212​的一种近似。这一点是依据勾股定理进行计算得出来的,具体在计算模型时我们在向下面介绍加速度传感器模型的下一步计算时会比较清楚。在它的上一个箱子模型中,我们已经成功引入了它的重力并高速地使它旋转了一个大的箱子。在最后两个电子实验室的案例中我们对一个箱体重力进行了矢量分析,其中的重力向量和矢量场的维数始终保持恒定。虽然它仅仅可以用来帮助我们准确理解坐在加速度上的传感器坐标是怎样和外部的矢量力相互作用而由此产生的,但如果我们把这个坐标系替换成了等于加速度的三条矢量轴并且我们能够准确想象到等于加速度的三个矢量力正在周围地面上旋转,那么就可能会更加简单地对它进行数学计算。图3-19x-y-z轴其其中,Rx、Ry、Rz分别认为是三个三股向量投影R分别映射位于它的x、y、z时间轴上的一个三股投影,Rxz分别认为是三个三股向量映射R在X和XZ三个平面上的一个三股投影;Ryz分别是一个二股向量映射R在X和YZ平面上的一个二股投影;AXZ分别是一个Rz和对的Rxz之间的一个二股夹角;Ayz分别是一个Rz和对的Ryz之间的一个二股夹角；根据欧拉勾勒二股夹角定理，求得：QUOTEtan(Axz)tan(Axz)=QUOTERxRyRxRy,QUOTEtan(Ayz)tan(Ayz)=QUOTERyRzRyRz,那么通过反正切函数可以求得Axy=QUOTEtan-1RxRztan-1RxRz,Ayz=QUOTEtan-1RyRztan-1RyRz,需要的角度是Axz、Ayz。请看看在上面的模型,我们只是保留了主轴的形状和颜色,以便我们的思维能够比较好地从上一个模型改变到新的模型中。设想出新模型中的每一条轴都会分别竖向于原来模型中不同位置的墙体。矢量R为加速度传感器检测到的向量（其中有可能由重力或上述例子中的惯性力组合）。Rx，Ry，Rz分别为向量R在X，Y，Z三个轴上的投影。根据传感器的三维空间中勾股定理，可以得到下列关系：QUOTER2R2=QUOTERx2Rx2+QUOTERy2Ry2+QUOTERz2Rz2,回到MPU-6050传感器中，已经分别从两个轴上读取了加速度数据，但是读取到的加速度数字量的单位还不是g（9.8m/QUOTES2S2）最后的转换，我们还需要引入加速度传感器的灵敏度（Sensitivity），单位通常是LSB/g，比方说，加速度传感器的灵敏度Sensitivity=16384LSB/g,灵敏度值可以在加速度传感器规格书中找到。要获得最后的单位为

g

的加速度，我们使用下列公式计算QUOTERxRx=QUOTER比如：当设置MPU-6050加速度传感器的灵敏度为16384LSB/g，读取MPU-6050的X、Z轴加速度数据为1122、16674时，则可以计算出：Rx=1122LSB/(16384LSB/g)=0.068g，Rz=16674LSB/(16384LSB/g)=1.018gAxz=atan(0.068g/1.018g)=0.589rad，Axz=atan(0.068g/1.018g)=0.589rad注意，这时求出来的角度单位为

弧度rad，和我们日常习惯用的单位角度之间有条转换公式：Angle=Radian/π∗180，那么，Axz=0.589rad/π∗180=31.7°陀螺仪介绍：图3-20陀螺仪图陀螺仪在每个运动路径上都不需要自动检测齿轮到一个中心轴的高速转动。例如,一个2轴的自动陀螺仪系统可以同时检测一个环绕旋转X传动轴和旋转Y两个传动轴的转矩。为了以一种数字化的形式应用来清晰描述和准确表达这些物体运动的方向旋转,我们首先从其中分别引入一些数字标记。首先,我们同样可以进行定义：QUOTERxzRxz惯性运动力的同轴矢量QUOTERR在QUOTEXZXZ平面上的同轴投影，QUOTERyzRyz惯性运动力的同轴矢量R在YZ平面上的同轴投影，在由QUOTERxzRxz和QUOTERzRz结合组成的直角三角形中，运用公式勾股夹角定理可得QUOTERxz2Rxz2=QUOTERx2Rx2+QUOTERz2Rz2，同样QUOTERyz2Ryz2=QUOTERy2Ry2+QUOTERz2Rz2，同时我们还是要特别注意QUOTER2R2=QUOTERxz2Rxz2+QUOTERy2Ry2，这条夹角公式既可以从夹角公式1和上面的两种夹角公式推导得出，也有人认为可由QUOTERR和QUOTERyzRyz所结合组成的两种直角三角形中来进行公式推导得得的出来,不用这些勾夹角公式，但是知道这些夹角公式有助于理解，相反我们按照一些如下的计算方法我们去重新定义了QUOTEZZ轴和QUOTERyzRyz、QUOTERxzRxz向量之间所组成的两种夹角：QUOTEAxzAxz--QUOTERxzRxz(同轴矢量和和r在z点xz两轴平面的同轴投影）与Z轴所成的夹角，QUOTEAyzAyz--QUOTERyzRyz（同轴矢量和和r在z点yz平面的同轴投影）和在Z点的轴所要组成的两种夹角夹角，现在我们距离同轴陀螺仪还是需要对轴进行夹角测量的这件事情就更加的接近了一步。陀螺仪通常是用来测量上述公式定义的陀螺角度与其变动量的比率。换言之,它就是一个会从其中角度来输出一个和上面这些不同角度的频率变化称为频率值的线性函数有关的频率数值。我们已经再次检验并得到一个绕y-轴进行旋转的径向速度，测得绕Y轴旋转的角度（注意即也就是说是QUOTEAxzAxz），定义这个表达式假设为QUOTEAxz0Axz0，之后再次定义假设在这个t1时刻我们再次已经检验到这个轴的速度，得到QUOTEAxz1Axz1。角度的大小变化以及频率按下列计算方法可以进行精确计算：RateQUOTEAxz1Axz1=(QUOTEAxz1Axz1-QUOTEAxz0Axz0)/(QUOTEt1t1-QUOTEt0t0）如果我们使用度量方法来表达角度,秒数量方法来代表时间,那这个值的计算单位也就是度/秒。这便是陀螺仪所要检测到的事情。在实践和运用中,陀螺仪一般都不会直接地给你输出一个以度/秒为基准数量的值(除非它们只是个特殊数字陀螺仪)。在MPU-6050传感器中,就像你在读取一个加速度的数据一样,你会获得一个经过内置ADC转换后得到的数字量,单位为LSB,参考对加速度数据的处理,我们同样得到：RateAxz=(GyroY−ZeroRate)/Sensitivity

RateAyz=(GyroX−ZeroRate)/Sensitivity其中,GyroX,GyroY-这两个数值都是陀螺仪的数据,它们分别是代表了一个向矢量R的旋转投影在XZ和YZ平面内里的一个转角,也就是我们可以用一个等价的方式来说,旋转运动可以被分解成为单独围绕Y和X轴的一个运动。ZeroRate–陀螺仪零偏值，换句话说，它是陀螺仪静止不动时的输出值。理论上，陀螺仪静止不动时，应该输出为0的值，但是受制造工艺、外部因素的影响，陀螺仪静止不动时输出一个比较小的随机的不为0的值，我们称之为零偏值。Sensitivity-陀螺仪的运动灵敏度,单位为LSB/(deg/s),其意思也就是假设一个陀螺仪的旋转速度每增加1°/s,那么这个陀螺仪的运动量和输出也就可能会有多少LSB。举个例子，假设我们读取到MPU-6050传感器的X轴数据：GyroX=12345若此时设置陀螺仪的灵敏度为16.4LSB/deg/s，并认为陀螺仪零偏值为0，代入公式3，得到：RateAyz=(12345-0)/(16.4LSB/deg/s)=751deg/sRateAyz=(12345−0)/(16.4LSB/deg/s)=751deg/s换句话说,传感器围绕X轴(或者是XZ平面内)以751°/s的旋转速度运动。请您特别注意,因为陀螺仪的输出值是有正负关系之分,负号代表该传感器沿着相反方向转动。一定要注意方向。1.5.5融合加速度传感器和陀螺仪的数据惯性检测元件主要有多种传感器,比如倾角仪、加速度计、陀螺仪、罗盘等传感器。每种惯性测量元件的性能都不尽相同，对比分析表格如表3-4所示：表3-4性能分析表测量量优点缺点倾角仪角度静态性能好，精度高，无累积误差动态响应速度慢加速度传感器加速度静态性能好，精度高，跟新频率快存在累积漂移误差陀螺仪角速度动态响应好，频带宽，更新频率快存在温漂和零漂罗盘角度静态性能好，精度高，无漂移误差动态响应速度慢，受环境磁场影响大倾角仪是用来测量和重力运动方向密切相关的一个姿态偏角,输出信号频率较低,实时性也比较差,而且它所输出的信号很容易被噪声所污染。对相频性质进行了分析表明,倾角仪的相位运动存在一定的滞后。加速度测量传感器主要测量与物体惯性旋转相关的移动加速度,包括惯性旋转、重力和其他线性移动加速度,对一次测量的惯性数据结果进行一次积分后就同样可以直接获得一个线性移动加速度,二次测量积分后也同样可以直接获得一个线性移动加速度,但是二次积分完成后所可能产生的时间漂移和移动误差可能会因为随着测量时间的不断累积而变得没有任何限制地逐渐变大,从而可以导致二次测量积分完成后所可以获得的测量数据不会失真。加速度式速率传感器经过了三角速率函数的数学运算原理即可准确获取速度倾角的数值,但是其输出的高频信号很容易就会遭到高频噪声。所以当一个反向加速度测量传感器在角度垂直于一个俯仰角的角度情况下进行安装时,通过反正和相切倾角函数的形式运算就已经可以直接用来把它作为一个角度倾角仪使它在空中仅当使用时可以进行360°的整个角度全方位倾角测量,并且这样也不会对它造成任何线性化的测量误差。陀螺仪在测量瞬时的旋转角和速度,由于温度的变化、摩擦力、不稳定的力矩等多种因素,会使陀螺仪产生一些漂移的误差。对陀螺仪所测量的数据进行积分后就可以获取与竖向方位相关的倾角信息,动态反馈响应迅速,但漂移后的误差会随着时间的累积而减小或者无穷。陀螺仪有一个足够的带宽,动态精度高,静态输入受到漂移和误差的影响很大。罗盘可以通过它来测量相对地球和磁场的绝对运动量和方向,提供一个绝对地磁位置的运动姿态例如夹角等的信息。由于室内以及储气室中普遍存在的励磁电源线和其他钢结构构件造成了超强磁场而因此产生了磁畸变，罗盘一般不适于室内工作。在上面的加速度传感器部分，我们似乎只要读取到加速度数据，通过反正切函数atan()就能求出我们想要的角度，再对此信号进行微分便可以获得角速度。但是在实际运用环境中，比如两轮自平衡小车，由于小车本身的摆动所产生的加速度会产生很大的干扰信号，它叠加在上述加速度数据量上使得该数据无法准确反映小车的倾角。一个自动陀螺仪就已经能够被广泛用来帮助计算和精确测量一个特定物体在某种角度方向上的速度旋转和倾角。由于这种高速陀螺仪在多个传感器中同时输出的噪声信号都通常是一辆大型小车的行驶角度和速度,不会直接影响受到一辆小车高速行驶时的小车运动噪声性能从而产生直接影响,因此在该输出信号中小车产生的运动噪声很小。小车的横向倾角也是通过根据相应的小车对角率和小车速度系数进行倾角积分来获得，这可进一步平滑信号，从而使得角度信号更加平稳。因此，小车控制所需要的角度和角速度可以使用陀螺仪所得到的数据量。由于从陀螺仪角速度获得角度信息，需要经过积分运算。如果角速度信号存在微小的偏差和漂移，经过积分运算之后，变化形成积累误差。这个误差会随着时间延长逐步增加，最终导致电路饱和，无法形成正确的角度信号。1.5.6消除累计误差简易方法是利用数据融合算法。将上面的加速度传感器来获得的角度数据信息进行了校正。通过将积分计算所得的的加速度和重力为相同的角度和陀螺仪在相同的位置下,利用这两个角度之间的偏移来改变陀螺仪的输出,从而将计算所积分的角度逐渐追溯至加速度的传感器计算所得的的相同角度ADDINEN.CITE<EndNote><Cite><Author>韩帅</Author><Year>2020</Year><RecNum>23</RecNum><DisplayText>[18]</DisplayText><record><rec-number>23</rec-number><foreign-keys><keyapp="EN"db-id="wvxfdfpx6xwp5he2sf6p9svsvae90atdv29v"timestamp="1620804700">23</key></foreign-keys><ref-typename="JournalArticle">17</ref-type><contributors><authors><author>韩帅</author><author>刘满禄</author><author>张俊俊</author><author>张华</author></authors></contributors><auth-address>西南科技大学特殊环境机器人技术四川省重点实验室;</auth-address><titles><title>基于两轮自平衡小车的模糊自适应补偿算法研究%J机械设计与制造</title></titles><pages>197-200</pages><number>09</number><keywords><keyword>自平衡小车</keyword><keyword>模糊自适应</keyword><keyword>跟踪实验</keyword><keyword>实物验证</keyword></keywords><dates><year>2020</year></dates><isbn>1001-3997</isbn><call-num>21-1140/TH</call-num><urls></urls><remote-database-provider>Cnki</remote-database-provider></record></Cite></EndNote>[18]。数据融合算法的种类很多,目前主要有两种:卡尔曼滤波和互补滤波。数据滤波器是一种通过噪声来还原真正数据的数据而进行处理的一种方法。任何的数据融合都是为了把多种渠道的数据进行中的噪音滤波,以便于达到尽量接近真正值的目标ADDINEN.CITE<EndNote><Cite><Author>丛玉华</Author><Year>2020</Year><RecNum>22</RecNum><DisplayText>[19]</DisplayText><record><rec-number>22</rec-number><foreign-keys><keyapp="EN"db-id="wvxfdfpx6xwp5he2sf6p9svsvae90atdv29v"timestamp="1620804700">22</key></foreign-keys><ref-typename="JournalArticle">17</ref-type><contributors><authors><author>丛玉华</author><author>吴洪佳</author><author>朱惠娟</author><author>朱娴</author><author>谢玲</author></authors></contributors><auth-address>南京理工大学紫金学院;</auth-address><titles><title>两轮自平衡小车的设计及实现%J电子技术与软件工程</title></titles><pages>106-109</pages><number>19</number><keywords><keyword>嵌入式技术</keyword><keyword>自控原理</keyword><keyword>自平衡小车</keyword></keywords><dates><year>2020</year></dates><isbn>2095-5650</isbn><call-num>10-1108/TP</call-num><urls></urls><remote-database-provider>Cnki</remote-database-provider></record></Cite></EndNote>[19]。在加速度传感器部分，我们通过反正切函数求出了角度，这里用Acc表示；在陀螺仪部分，我们得到了角速度，这里用Gyro表示。互补滤波的核心公式：Angle=0.98∗(Angle+Gyro∗dt)+0.02∗Acc，在这里面Angle是互补滤波后得到的角度，Gyro是陀螺仪计算得到的旋转角速度，滤波器的运行周期是dt，而0.98和0.02为加权系数a和（1-a）。在程序里面dt的运行周期我们设定为5ms，时间常数QUOTEττ的设定要看滤波需求调整，如果QUOTEττ的取值越大，就很难跟上加速度传感器，如果QUOTEττ越小，就会有更多的噪声。这里QUOTEττ=0.49。互补滤波的缺陷也很明显，首先初始化不能跟随实际角度，另外对于零漂问题，陀螺仪在理论上，静止时候，读出的数据应该为0，但是上电后，读出的值不为0。1.5.7PID控制原理控制器里最常见的控制规律就是PID控制，下面是一个简单的直流电机调速框图。图3-21直流电机调速图误差值e（t）在经过PID控制器后输出成u（t），再经过功率放大器的放大，驱动直流电机运转。本次研究中主要使用的算法就是一种增量式PID算法,增量式PID所指的也就是一个执行器的输出仅仅是一个控制器的增量,如果一个执行器需要的也就是一个增量的话，那我们就可以采用增量式PID算法ADDINEN.CITE<EndNote><Cite><Author>李昊伦</Author><Year>2021</Year><RecNum>24</RecNum><DisplayText>[20]</DisplayText><record><rec-number>24</rec-number><foreign-keys><keyapp="EN"db-id="wvxfdfpx6xwp5he2sf6p9svsvae90atdv29v"timestamp="1620804884">24</key></foreign-keys><ref-typename="Thesis">32</ref-type><contributors>