近距定位系统

1、中南民族大学计算机科学学院设计报告题目：PID算法恒速控制电机姓名： 汪 立 富、陈一福、董国发学号：201321095146、201321095149、指导老师：张志俊 2015年 5 月5日一、总体方案 采用4个收发一体的超声波对目标物体分别分时测距，将测量的数据进行一系列计算后对目标物体进行定位，二、方案选择与论证设计主要有以下几个模块组成：电源模块控制处理模块：驱动直流电机模块：显示模块速度测量模块。2.1处理器根据设计要求，控制器主要用于产生占空比受数字PID算法控制的PWM脉冲，并对电机当前速度进行采集处理，根据算法得出当前所需输出的占空比脉冲。对于控制器的选择有以下两种方案。 方

2、案一：采用ARM作为系统的控制器ARM可以实现各种复杂的逻辑功能，模块大，密度高，它将所有器件集成在一块芯片上，减少了体积，提高了稳定性，并且可应用软件在线仿真、调试，易于进行功能控制。ARM采用并行的输入输出方式，提高了系统的处理速度，适合作为大规模实时系统的控制核心。通过输入模块将参数输入给ARM,ARM通过程序设计控制PWM脉冲的占空比，但是由于本次设计对数据处理的时间要求不高，ARM的高速处理的优势得不到充分体现，并且由于其集成度高，使其成本偏高，同时由于芯片的引脚较多，实物硬件电路板布线复杂，加重了电路设计和实际焊接的工作。 方案二：采用传统的STC89C52单片机作为运动物体的控制

3、中心。具有软件编程灵活、体积小、成本低,使用简单等特点，在各个领域中应用广泛。 综合上两种方案比较，采用ASTC89C52作为控制器处理输入的数据并控制电机运动较为简单，可以满足设计要求，还可以减少外围电路的复杂度，价格廉价，因此在本次设计选用方案二。程序部分2. 2 测距传感器2.3 显示模块3、 原理分析3.1最小系统3.2 超声波测距原理3.3电源部分3.4温差补偿4、 软件部分 4.1软件总体流程 在本软件系统中，首先对对定时器、液晶屏的各个参数进行初始化，再进入while循环语句进行重复性的测量并且更新目标物体的数据，四个超声波的测距流程图在图已经给出，这里不加论述，当四个距离都测出

4、后，进入计算子程序calculator（）计算各个参数进行定位，最后再调用子程序显示函数，把计算出的参数数据经过液晶屏显示出来，为使显示的数据直观观察，需要延时1s，在本软件设计中，循环一次的时间大约为1.2s。 开始 初始化定时器2调用测距程序调用定位计算子程序 调用显示函数更新显示输出延时1ms 图4.1 总体流程4.2测距算法原理 4.2.1 单个测距原理本系统采用的是超声波传感器测距，超声波发射器向某一方向发射超声波，在发射时刻的同时开始计时，超声波在空气中传播，途中碰到障碍物就立即返回来，超声波接收器收到反射波就立即停止计时。超声波在空气中的传播速度为340m/s，而外界的温度的大小

5、会影响的传播速度，在本系统的设计中，在前面已经论述过，这里不加论证，根据计时器记录的时间t，就可以计算出发射点距障碍物的距离(s)，这就是所谓的时间差测距法。距离计算公式 L=（t*340)/2 (0) 发送大于40KHz的脉冲方波，在本次的验证试验中，f为58Hz最为准确，当超声波发射的同时，定时器2开始定时使能，当接收到反射波时，定时器失能，定时器2停止计时，时间记为t，由（0）公式即可计算出距离长度。发射58Hz的脉冲波是否接收到回波？ N4.2.2 四个超声波测距 一个超声波只能测距，要定位一个目标物体，则最少需要3个超声波测量不同的距离，在本系统中，为了方便计算，增加系统的可靠性，本

6、次设计采用4个收发一天的超声波分别测距，距离分别记为X1,X2，X3，X4如图所示，记1,3号超声波探头的连线方向为正北方向，于是就可以定位目标物体的具体位置了。 测量X1的距离 Y X1的距离是否过大？ Y N 测量X2的距离 X2的距离是否过大？ Y 测量X3的距离 N X3的距离是否过大？ 测量X4的距离 N Y X4的距离是否过大？4.3定位算法原理分析 在本系统中，定位算法占主要作用，在整个的测距过程中， 图4.3.1四个探头的分布 如图所示，在测距过程中，可等效为一个底面长为40cm的四棱锥，测量的距离分别记为，探头1到被测物的距离记为X1，2号测距探头测得的距离记为X2，3号测距

7、探头测得的距离记为X3，4号测距探头测得的距离记为X4，探头1和探头3的连线方向为正北方向，如图所示，底面为边长为30Cm的正方形，中点O为定位参考点，当X1，X2，X3和X4的距离求出后，四棱锥为一个固定图像，被测物相对于O点的位置就可以确定， 图4.3.2 等效图1 图4.3.3等效四棱锥图2 计算算法，在计算被测物的位置时，可以运用两种方法计算，分别为直接计算距离法和向量法，由于在本次直接计算法难度较大，而向量法计算简单，因此，利用向量求图法计算，设O点的坐标为（0,0,0），被测物的坐标为（x,y,z），1点的坐标为（-a,a,0），2点的坐标为（a,a,0），3点的坐标为（-a,-a

8、,0），4点的坐标为（a,-a,0），在线段1Q中，向量1Q的长度可以表示为坐标1减去坐标Q，再开方，则X1的长度计算公式为： X1=sqrt(x+a)*(x+a)+(y-a)*(y-a)+z*z); （1） 同理便可算出X2，X3和X4的长度： X2=sqrt(x-a)*(x-a)+(y-a)*(y-a)+z*z); （2） X3=sqrt(x+a)*(x+a)+(y+a)*(y+a)+z*z); （3） X4=sqrt(x-a)*(x-a)+(y+a)*(y+a)+z*z); （4） 其中，a为常量，sqrt（）函数表示对括号里面的数进行开方，设x1和y1，x2和y2分别为三棱锥Q123计

9、算出的Q点的坐标和三棱锥Q234计算出的Q点坐标，在则结合（1）（2）（3）（4）可以得出x1，y1，x2和y2，计算公式为： x1=(X 32*X3-X4*X4)/(4*a); （5） y1=(X4*X4-X2*X2)/(4*a); （6） x2=(X1*X1-X2*X2)/(4*a); （7） y2=(X3*X3-X1*X1)/(4*a); （8） z1=sqrt(X1*X1-(x+a)*(x+a)-(y-a)*(y-a); （9） z2=sqrt(X2*X2-(x-a)*(x-a)-(y-a)*(y-a); （10） z3=sqrt(X3*X3-(x+a)*(x+a)-(y+a)*(y+

10、a); （11） z4=sqrt(X4*X4-(x-a)*(x-a)-(y+a)*(y+a); （12） 为了提高精度， 对各个方向上的值求平均值，由（5）（6）（7）（8）（9）（10）（11）（12） 可以算出得出平均值，最后记的平均值坐标为（coordinate_x，coordinate_y， coordinate_z），则可以对各个方向坐标求平均值，则计算公式为： coordinate_x=(x1+x2)/2; coordinate_y=(y1+y2)/2; coordinate_z=(z1+z2+z3+z4)/4； 已知Q点相对于O点的坐标后，便可以求QO的距离长度了，记OQ得长度为

11、L，Q点对地的的距离记为d，计算公式为：d=sqrt(coordinate_x*coordinate_x+coordinate_y*coordinate_y);L=sqrt(coordinate_x*coordinate_x+coordinate_y*coordinate_y+coordinate_z*coordinate_z); 记coordinate_y/d为角度A的余弦值，d/L的值记为角度B的余弦值，其中计算公式如下：cosA=(coordinate_y/d);cosB=(d/L);调用acos（）余弦计算函数，将角度A和B计算出来：pi_angle=acos(cosA); / /an

12、gle=pi_angle*180/3.14159;/pi_angle1=acos(cosB);/?angle1=pi_angle1*180/3.14159;/?在角度数据的读取中，往往角度在90度以内，因此必须进过一下换算：if(angle>90) angle=180-angle;if(angle1>90) angle1=180-angle1; 最后，所有参数计算完毕。五、测试结果及其误分析总结7、 附件7.1 完整电路图7.2全部程序清单#include "stm32f10x.h"#include "stm32f10x_fsmc.h"#in

13、clude "bsp_ili9341_lcd.h"#include "bsp_sdfs_app.h"#include "math.h"#include "stm32f10x_tim.h"/#include "usart1.h"#include "bsp_usart1.h"#include "TIM2.h"#include "UltrasonicWave.h"#include "math.h" void DelayTi

14、me_ms(int Time) /1us延时函数 unsigned int i; for ( ; Time>0; Time-) for ( i = 0; i <720; i+ ); void LCD_SHOW(void)LCD_DispEnCh(25, 15, (uint8_t *)"作者:wang",RED); LCD_DispEnCh(25, 35, (uint8_t *)"单位:中南民族大学", RED); LCD_DispEnCh(90, 80, (uint8_t *)"室内定位系统", RED); LCD_Dis

15、pEnCh(5, 120, (uint8_t *)"被称测物离基站距离为: cm", RED);LCD_DispEnCh(5, 150, (uint8_t *)"被测物离地距离为: cm", RED);LCD_DispEnCh(2, 180, (uint8_t *)"被测物具体坐标为( , , )", RED);LCD_DispEnCh(5, 210, (uint8_t *)"基站观察的仰角为: 度", RED);LCD_DispEnCh(5, 270, (uint8_t *)"被测物对地离基站距离为:

16、cm", RED);int main(void)/USART1_Config();LCD_Init();LCD_Clear(0, 0, 240, 320, BACKGROUND);/初始化sd卡文件系统，因为汉字的字库放在了sd卡里面 Sd_fs_init(); SystemInit();NVIC_Configuration();TIM2_Configuration();UltrasonicWave_Configuration(); LCD_SHOW(); while(1) UltrasonicWave_StartMeasure(); DelayTime_ms(10000);LCD_

17、Clear(0, 0, 240, 320, BACKGROUND);#include "UltrasonicWave.h"#include "bsp_usart1.h"#include "TIM2.h"#include "stm32f10x_tim.h"#include "bsp_ili9341_lcd.h"#include "bsp_sdfs_app.h"#include "math.h"#defineTRIG_PORT GPIOC/TRIG #defi

18、neECHO_PORT GPIOC/ECHO #defineTRIG_PIN GPIO_Pin_8 /TRIG #defineECHO_PIN GPIO_Pin_9/ECHO#defineTRIG_PORT1 GPIOA/TRIG #defineECHO_PORT1 GPIOA/ECHO #defineTRIG_PIN1 GPIO_Pin_1 /TRIG #defineECHO_PIN2 GPIO_Pin_2/ECHO #defineTRIG_PIN3 GPIO_Pin_3 /TRIG #defineECHO_PIN4 GPIO_Pin_4/ECHO #defineTRIG_PIN5 GPIO

19、_Pin_5 /TRIG #defineECHO_PIN6 GPIO_Pin_6/ECHO #defineTRIG_PIN7 GPIO_Pin_7 /TRIG #defineECHO_PIN8 GPIO_Pin_8/ECHO /unsigned short intint UltrasonicWave_Distance; /计算出的距离 int UltrasonicWave_Distance1; /计算出的距离 int UltrasonicWave_Distance2; /计算出的距离 int UltrasonicWave_Distance3; /计算出的距离 int UltrasonicWav

20、e_Distance4; /计算出的距离 /* * 函数名：DelayTime_us * 描述 ：1us延时函数 * 输入 ：Time 延时的时间 US * 输出 ：无 */ float angle, pi_angle, angle1, pi_angle1, cosA, cosB, d,a=40;float X1,X2,X3,X4;float L, x,y,z, coordinate_x,coordinate_y,coordinate_z, y1,x1,z1, y2,x2,z2, z3,z4;void calculator (void) / /算法计算函数/*X1=sqrt(x+a)*(x+a

21、)+(y-a)*(y-a)+z*z);X2=sqrt(x-a)*(x-a)+(y-a)*(y-a)+z*z); X3=sqrt(x+a)*(x+a)+(y+a)*(y+a)+z*z); X4=sqrt(x-a)*(x-a)+(y+a)*(y+a)+z*z);*/x1=(X3*X3-X4*X4)/(4*a);y1=(X4*X4-X2*X2)/(4*a);x2=(X1*X1-X2*X2)/(4*a);y2=(X3*X3-X1*X1)/(4*a); coordinate_x=(x1+x2)/2; coordinate_y=(y1+y2)/2;z1=sqrt(X1*X1-(x+a)*(x+a)-(y-

22、a)*(y-a); z2=sqrt(X2*X2-(x-a)*(x-a)-(y-a)*(y-a);z3=sqrt(X3*X3-(x+a)*(x+a)-(y+a)*(y+a);z4=sqrt(X4*X4-(x-a)*(x-a)-(y+a)*(y+a); coordinate_z=(z1+z2+z3+z4)/4; /取平均值d=sqrt(coordinate_x*coordinate_x+coordinate_y*coordinate_y);L=sqrt(coordinate_x*coordinate_x+coordinate_y*coordinate_y+coordinate_z*coordina

23、te_z);cosA=(coordinate_y/d);cosB=(d/L); LCD_DisNum(1, 232, x1, WHITE);LCD_DisNum(15, 232, x2, WHITE);LCD_DisNum(37, 232, y1, WHITE);LCD_DisNum(57, 232, y2, WHITE);LCD_DisNum(67, 232, x, WHITE);LCD_DisNum(87, 232, y, WHITE);LCD_DisNum(107, 232, z, WHITE);LCD_DisNum(127, 232, z1, WHITE);LCD_DisNum(157

24、, 232, z2, WHITE);*/pi_angle=acos(cosA);/?angle=pi_angle*180/3.14159;/?pi_angle1=acos(cosB);/?angle1=pi_angle1*180/3.14159;/?if(angle>90) angle=180-angle;if(angle1>90) angle1=180-angle1;LCD_DisNum(159, 122, L, WHITE);LCD_DisNum(148, 152, coordinate_z, WHITE);LCD_DisNum(139, 182, coordinate_x,

25、WHITE);LCD_DisNum(165, 182, coordinate_y, WHITE); LCD_DisNum(197, 182, coordinate_z, WHITE);LCD_DisNum(149, 212, angle1, WHITE);LCD_DisNum(192, 272, d, WHITE);if(coordinate_x>0&coordinate_y>0)LCD_DispEnCh(5, 240, (uint8_t *)"被测物位于基站的北偏东 度", RED); LCD_DisNum(186, 242, angle, WHITE

26、);if(coordinate_x=0&coordinate_y=0)LCD_DispEnCh(5, 240, (uint8_t *)"被测物位于基站的正上方 度", RED); LCD_DisNum(186, 242, angle, WHITE);if(coordinate_x<0&coordinate_y>0)LCD_DispEnCh(5, 240, (uint8_t *)"被测物位于基站的北偏西 度", RED); LCD_DisNum(186, 242, angle, WHITE);if(coordinate_x<

27、;0&coordinate_y<0)LCD_DispEnCh(5, 240, (uint8_t *)"被测物位于基站的南偏西 度", RED); LCD_DisNum(186, 242, angle, WHITE);if(coordinate_x>0&coordinate_y<0)LCD_DispEnCh(5, 240, (uint8_t *)"被测物位于基站的南偏东 度", RED); LCD_DisNum(186, 242, angle, WHITE);if(coordinate_x=0&coordinate

28、_y>0)LCD_DispEnCh(5, 240, (uint8_t *)"被测物位于基站正北方向", RED); LCD_DisNum(186,242, angle, WHITE);if(coordinate_x=0&coordinate_y<0)LCD_DispEnCh(5, 240, (uint8_t *)"被测物位于基站正南方向", RED); LCD_DisNum(186, 242, angle, WHITE);if(coordinate_x>0&coordinate_y=0)LCD_DispEnCh(5, 2

29、40, (uint8_t *)"被测物位于基站正东方向", RED); LCD_DisNum(186, 242, angle, WHITE);if(coordinate_x<0&coordinate_y=0)LCD_DispEnCh(50, 240, (uint8_t *)"被测物位于基站正西方向", RED); LCD_DisNum(186,242, angle, WHITE);void DelayTime_us(int Time) unsigned char i; for ( ; Time>0; Time-) for ( i =

30、0; i < 72; i+ );/* * 函数名：UltrasonicWave_Configuration * 描述 ：超声波模块的初始化 * 输入 ：无 * 输出 ：无 */void UltrasonicWave_Configuration(void) GPIO_InitTypeDef GPIO_InitStructure; / RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE); RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); /*GPIO_InitStructure.G

31、PIO_Pin = TRIG_PIN; /PC8接TRIG GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; /设为推挽输出模式 GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_Init(TRIG_PORT, &GPIO_InitStructure);/初始化外设GPIO */ GPIO_InitStructure.GPIO_Pin = TRIG_PIN1; /PA1接TRIG GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; /

32、设为推挽输出模式 GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; GPIO_Init(TRIG_PORT1, &GPIO_InitStructure);GPIO_InitStructure.GPIO_Pin = TRIG_PIN3; /PA3接TRIG GPIO_Init(TRIG_PORT1, &GPIO_InitStructure);GPIO_InitStructure.GPIO_Pin = TRIG_PIN5; /PA5接TRIG GPIO_Init(TRIG_PORT1, &GPIO_InitStructure

33、);GPIO_InitStructure.GPIO_Pin = TRIG_PIN7; /PA7接TRIG GPIO_Init(TRIG_PORT1, &GPIO_InitStructure); /*GPIO_InitStructure.GPIO_Pin = ECHO_PIN; /PC9接ECH0 GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; /设为输入 GPIO_Init(ECHO_PORT1,&GPIO_InitStructure); /初始化GPIOA*/GPIO_InitStructure.GPIO_Pin =

34、 ECHO_PIN2; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;/PC9接ECH0 GPIO_Init(ECHO_PORT1,&GPIO_InitStructure); /初始化GPIOAGPIO_InitStructure.GPIO_Pin = ECHO_PIN4; /PC9接ECH0 GPIO_Init(ECHO_PORT1,&GPIO_InitStructure); /初始化GPIOAGPIO_InitStructure.GPIO_Pin = ECHO_PIN6; /PC9接ECH0 GPIO_Init(E

35、CHO_PORT1,&GPIO_InitStructure); /初始化GPIOAGPIO_InitStructure.GPIO_Pin = ECHO_PIN8; /PC9接ECH0 GPIO_Init(ECHO_PORT1,&GPIO_InitStructure); /初始化GPIOAGPIO_InitStructure.GPIO_Pin = ECHO_PIN; /PC9接ECH0 GPIO_Init(ECHO_PORT1,&GPIO_InitStructure); /初始化GPIOA/* * 函数名：UltrasonicWave_CalculateTime * 描述

36、 ：计算距离 * 输入 ：无 * 输出 ：无void UltrasonicWave_CalculateTime(void) UltrasonicWave_Distance=TIM_GetCounter(TIM2)*5*34/2000; /0.05*340*100/200000UltrasonicWave_Distance1=TIM_GetCounter(TIM2)*5*34/2000; /0.05*340*100/200000UltrasonicWave_Distance1=TIM_GetCounter(TIM2)*5*34/2000; /0.05*340*100/200000Ultrason

37、icWave_Distance1=TIM_GetCounter(TIM2)*5*34/2000; /0.05*340*100/200000UltrasonicWave_Distance1=TIM_GetCounter(TIM2)*5*34/2000; /0.05*340*100/200000 * 函数名：UltrasonicWave_StartMeasure * 描述 ：开始测距，发送一个>10us的脉冲，然后测量返回的高电平时间 * 输入 ：无 * 输出 ：无 */void UltrasonicWave_StartMeasure(void) /* GPIO_SetBits(TRIG_P

38、ORT,TRIG_PIN); /送>10US的高电平 DelayTime_us(20); /延时20US GPIO_ResetBits(TRIG_PORT,TRIG_PIN); while(!GPIO_ReadInputDataBit(ECHO_PORT,ECHO_PIN); /等待高电平 TIM_Cmd(TIM2, ENABLE); /开启时钟 while(GPIO_ReadInputDataBit(ECHO_PORT,ECHO_PIN); /等待低电平 TIM_Cmd(TIM2, DISABLE); /定时器2失能 /UltrasonicWave_CalculateTime(); /

39、计算距离 TIM_SetCounter(TIM2,0);/printf("rndistance:%d%d cmrn",UltrasonicWave_Distance/256,UltrasonicWave_Distance%256); LCD_DisNum(157,122,UltrasonicWave_Distance,WHITE);*/ GPIO_SetBits(TRIG_PORT1,TRIG_PIN1); /送>10US的高电平 DelayTime_us(25); /延时20US GPIO_ResetBits(TRIG_PORT1,TRIG_PIN1); while

40、(!GPIO_ReadInputDataBit(ECHO_PORT1,ECHO_PIN2); /等待高电平 TIM_Cmd(TIM2, ENABLE); /开启时钟 while(GPIO_ReadInputDataBit(ECHO_PORT1,ECHO_PIN2); /等待低电平 TIM_Cmd(TIM2, DISABLE); /定时器2失能UltrasonicWave_Distance1=TIM_GetCounter(TIM2)*5*34/2000; /0.05*340*100/200000 /计算距离 UltrasonicWave_Distance1=UltrasonicWave_Dist

41、ance1; X1=UltrasonicWave_Distance1; TIM_SetCounter(TIM2,0);GPIO_SetBits(TRIG_PORT,TRIG_PIN3); /送>10US的高电平 DelayTime_us(25); /延时20US GPIO_ResetBits(TRIG_PORT,TRIG_PIN3); while(!GPIO_ReadInputDataBit(ECHO_PORT1,ECHO_PIN4); /等待高电平 TIM_Cmd(TIM2, ENABLE); /开启时钟 while(GPIO_ReadInputDataBit(ECHO_PORT1,E

42、CHO_PIN4); /等待低电平 TIM_Cmd(TIM2, DISABLE); /定时器2失能 UltrasonicWave_Distance2=TIM_GetCounter(TIM2)*5*34/2000; /0.05*340*100/200000 /计算距离 X2=UltrasonicWave_Distance2; / X2=TIM_GetCounter(TIM2)*5*34/2000; /0.05*340*100/200000 TIM_SetCounter(TIM2,0);/printf("rndistance:%d%d cmrn",UltrasonicWave_

43、Distance/256,UltrasonicWave_Distance%256); / LCD_DisNum(157,122,UltrasonicWave_Distance,WHITE); GPIO_SetBits(TRIG_PORT1,TRIG_PIN5); /送>10US的高电平 DelayTime_us(25); /延时20US GPIO_ResetBits(TRIG_PORT1,TRIG_PIN5); while(!GPIO_ReadInputDataBit(ECHO_PORT1,ECHO_PIN6); /等待高电平 TIM_Cmd(TIM2, ENABLE); /开启时钟 w

44、hile(GPIO_ReadInputDataBit(ECHO_PORT1,ECHO_PIN6); /等待低电平 TIM_Cmd(TIM2, DISABLE); /定时器2失能 UltrasonicWave_Distance3=TIM_GetCounter(TIM2)*5*34/2000; /0.05*340*100/200000 /计算距离 X3=UltrasonicWave_Distance3; TIM_SetCounter(TIM2,0);/printf("rndistance:%d%d cmrn",UltrasonicWave_Distance/256,UltrasonicWave_Distance%256); /