近距定位系统.docx
- 文档编号:24093928
- 上传时间:2023-05-24
- 格式:DOCX
- 页数:25
- 大小:83.73KB
近距定位系统.docx
《近距定位系统.docx》由会员分享,可在线阅读,更多相关《近距定位系统.docx(25页珍藏版)》请在冰豆网上搜索。
近距定位系统
中南民族大学
计算机科学学院
设计报告
题目:
PID算法恒速控制电机
姓名:
汪立富、陈一福、董国发
学号:
201821095146、201821095149、
指导老师:
张志俊
2018年5月5日
一、总体方案
采用4个收发一体的超声波对目标物体分别分时测距,将测量的数据进行一系列计算后对目标物体进行定位,
二、方案选择与论证
设计主要有以下几个模块组成:
电源模块控制处理模块:
驱动直流电机模块:
显示模块速度测量模块。
2.1处理器
根据设计要求,控制器主要用于产生占空比受数字PID算法控制的PWM脉冲,并对电机当前速度进行采集处理,根据算法得出当前所需输出的占空比脉冲。
对于控制器的选择有以下两种方案。
b5E2RGbCAP
方案一:
采用ARM作为系统的控制器ARM可以实现各种复杂的逻辑功能,模块大,密度高,它将所有器件集成在一块芯片上,减少了体积,提高了稳定性,并且可应用软件在线仿真、调试,易于进行功能控制。
ARM采用并行的输入输出方式,提高了系统的处理速度,适合作为大规模实时系统的控制核心。
通过输入模块将参数输入给ARM,ARM通过程序设计控制PWM脉冲的占空比,但是由于本次设计对数据处理的时间要求不高,ARM的高速处理的优势得不到充分体现,并且由于其集成度高,使其成本偏高,同时由于芯片的引脚较多,实物硬件电路板布线复杂,加重了电路设计和实际焊接的工作。
p1EanqFDPw
方案二:
采用传统的STC89C52单片机作为运动物体的控制中心。
具有软件编程灵活、体积小、成本低,使用简单等特点,在各个领域中应用广泛。
DXDiTa9E3d
综合上两种方案比较,采用ASTC89C52作为控制器处理输入的数据并控制电机运动较为简单,可以满足设计要求,还可以减少外围电路的复杂度,价格廉价,因此在本次设计选用方案二。
RTCrpUDGiT
程序部分
2.2测距传感器
2.3显示模块
3、原理分析
3.1最小系统
3.2超声波测距原理
3.3电源部分
3.4温差补偿
4、软件部分
4.1软件总体流程
在本软件系统中,首先对对定时器、液晶屏的各个参数进行初始化,再进入while循环语句进行重复性的测量并且更新目标物体的数据,四个超声波的测距流程图在图已经给出,这里不加论述,当四个距离都测出后,进入计算子程序calculator<)计算各个参数进行定位,最后再调用子程序显示函数,把计算出的参数数据经过液晶屏显示出来,为使显示的数据直观观察,需要延时1s,在本软件设计中,循环一次的时间大约为1.2s。
5PCzVD7HxA
图4.1总体流程
4.2测距算法原理
4.2.1单个测距原理
本系统采用的是超声波传感器测距,超声波发射器向某一方向发射超声波,在发射时刻的同时开始计时,超声波在空气中传播,途中碰到障碍物就立即返回来,超声波接收器收到反射波就立即停止计时。
超声波在空气中的传播速度为340m/s,而外界的温度的大小会影响的传播速度,在本系统的设计中,在前面已经论述过,这里不加论证,根据计时器记录的时间t,就可以计算出发射点距障碍物的距离(s>,这就是所谓的时间差测距法。
jLBHrnAILg
距离计算公式
L=
发送大于40KHz的脉冲方波,在本次的验证实验中,f为58Hz最为准确,当超声波发射的同时,定时器2开始定时使能,当接收到反射波时,定时器失能,定时器2停止计时,时间记为t,由<0)公式即可计算出距离长度。
xHAQX74J0X
NLDAYtRyKfE
图4.2.1
4.2.2四个超声波测距
一个超声波只能测距,要定位一个目标物体,则最少需要3个超声波测量不同的距离,在本系统中,为了方便计算,增加系统的可靠性,本次设计采用4个收发一天的超声波分别测距,距离分别记为X1,X2,X3,X4如图所示,记1,3号超声波探头的连线方向为正北方向,于是就可以定位目标物体的具体位置了。
Zzz6ZB2Ltk
YdvzfvkwMI1
Y
N
Y
N
N
Y
图4.2.2
4.3定位算法原理分析
在本系统中,定位算法占主要作用,在整个的测距过程中,
图4.3.1四个探头的分布
如图所示,在测距过程中,可等效为一个底面长为40cm的四棱锥,测量的距离分别记为,探头1到被测物的距离记为X1,2号测距探头测得的距离记为X2,3号测距探头测得的距离记为X3,4号测距探头测得的距离记为X4,探头1和探头3的连线方向为正北方向,如图所示,底面为边长为30Cm的正方形,中点O为定位参考点,当X1,X2,X3和X4的距离求出后,四棱锥为一个固定图像,被测物相对于O点的位置就可以确定,rqyn14ZNXI
图4.3.2等效图1
图4.3.3等效四棱锥图2
计算算法,在计算被测物的位置时,可以运用两种方法计算,分别为直接计算距离法和向量法,由于在本次直接计算法难度较大,而向量法计算简单,因此,利用向量求图法计算,设O点的坐标为<0,0,0),被测物的坐标为 EmxvxOtOco X1=sqrt((x+a>*(x+a>+(y-a>*(y-a>+z*z>。 <1)SixE2yXPq5 同理便可算出X2,X3和X4的长度: X2=sqrt((x-a>*(x-a>+(y-a>*(y-a>+z*z>。 <2)6ewMyirQFL X3=sqrt((x+a>*(x+a>+(y+a>*(y+a>+z*z>。 <3)kavU42VRUs X4=sqrt((x-a>*(x-a>+(y+a>*(y+a>+z*z>。 <4)y6v3ALoS89 其中,a为常量,sqrt<)函数表示对括号里面的数进行开方,设x1和y1,x2和y2分别为三棱锥Q123计算出的Q点的坐标和三棱锥Q234计算出的Q点坐标,在则结合<1)<2)<3)<4)可以得出x1,y1,x2和y2,计算公式为: M2ub6vSTnP x1=(X32*X3-X4*X4>/(4*a>。 <5)0YujCfmUCw y1=(X4*X4-X2*X2>/(4*a>。 <6)eUts8ZQVRd x2=(X1*X1-X2*X2>/(4*a>。 <7)sQsAEJkW5T y2=(X3*X3-X1*X1>/(4*a>。 <8)GMsIasNXkA z1=sqrt(X1*X1-(x+a>*(x+a>-(y-a>*(y-a>>。 <9)TIrRGchYzg z2=sqrt(X2*X2-(x-a>*(x-a>-(y-a>*(y-a>>。 <10)7EqZcWLZNX z3=sqrt(X3*X3-(x+a>*(x+a>-(y+a>*(y+a>>。 <11)lzq7IGf02E z4=sqrt(X4*X4-(x-a>*(x-a>-(y+a>*(y+a>>。 <12)zvpgeqJ1hk 为了提高精度,对各个方向上的值求平均值,由<5)<6)<7)<8)<9)<10)<11)<12)可以算出得出平均值,最后记的平均值坐标为 NrpoJac3v1 coordinate_x=(x1+x2>/2。 coordinate_y=(y1+y2>/2。 coordinate_z=(z1+z2+z3+z4>/4; 已知Q点相对于O点的坐标后,便可以求QO的距离长度了,记OQ得长度为L,Q点对地的的距离记为d,计算公式为: 1nowfTG4KI d=sqrt(coordinate_x*coordinate_x+coordinate_y*coordinate_y>。 fjnFLDa5Zo L=sqrt(coordinate_x*coordinate_x+coordinate_y*coordinate_y+coordinate_z*coordinate_z>。 tfnNhnE6e5 记coordinate_y/d为角度A的余弦值,d/L的值记为角度B的余弦值,其中计算公式如下: cosA=(coordinate_y/d>。 cosB=(d/L>。 调用acos<)余弦计算函数,将角度A和B计算出来: pi_angle=acos(cosA>。 // angle=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" #include"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" voidDelayTime_ms(intTime>//1us延时函数 { unsignedinti。 for(。 Time>0。 Time--> for(i=0。 i<720。 i++>。 } voidLCD_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_DispEnCh(5,120,(uint8_t*>"被称测物离基站距离为: cm",RED>。 HbmVN777sL LCD_DispEnCh(5,150,(uint8_t*>"被测物离地距离为: cm",RED>。 V7l4jRB8Hs LCD_DispEnCh(2,180,(uint8_t*>"被测物具体坐标为(,,>",RED>。 83lcPA59W9 LCD_DispEnCh(5,210,(uint8_t*>"基站观察的仰角为: 度",RED>。 mZkklkzaaP LCD_DispEnCh(5,270,(uint8_t*>"被测物对地离基站距离为: cm",RED>。 AVktR43bpw } intmain(void> { //USART1_Config(>。 LCD_Init(>。 LCD_Clear(0,0,240,320,BACKGROUND>。 //初始化sd卡文件系统,因为汉字的字库放在了sd卡里面ORjBnOwcEd Sd_fs_init(>。 SystemInit(>。 NVIC_Configuration(>。 TIM2_Configuration(>。 UltrasonicWave_Configuration(>。 LCD_SHOW(>。 while(1> { UltrasonicWave_StartMeasure(>。 DelayTime_ms(10000>。 LCD_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_PORTGPIOC//TRIG #defineECHO_PORTGPIOC//ECHO #defineTRIG_PINGPIO_Pin_8//TRIG #defineECHO_PINGPIO_Pin_9//ECHO #defineTRIG_PORT1GPIOA//TRIG #defineECHO_PORT1GPIOA//ECHO #defineTRIG_PIN1GPIO_Pin_1//TRIG #defineECHO_PIN2GPIO_Pin_2//ECHO #defineTRIG_PIN3GPIO_Pin_3//TRIG #defineECHO_PIN4GPIO_Pin_4//ECHO #defineTRIG_PIN5GPIO_Pin_5//TRIG #defineECHO_PIN6GPIO_Pin_6//ECHO #defineTRIG_PIN7GPIO_Pin_7//TRIG #defineECHO_PIN8GPIO_Pin_8//ECHO //unsignedshortint intUltrasonicWave_Distance。 //计算出的距离 intUltrasonicWave_Distance1。 //计算出的距离 intUltrasonicWave_Distance2。 //计算出的距离 intUltrasonicWave_Distance3。 //计算出的距离 intUltrasonicWave_Distance4。 //计算出的距离 /* *函数名: DelayTime_us *描述: 1us延时函数 *输入: Time延时的时间US *输出: 无 */ floatangle,pi_angle,angle1,pi_angle1,cosA,cosB,d,a=40。 2MiJTy0dTT floatX1,X2,X3,X4。 floatL,x,y,z,coordinate_x,coordinate_y,coordinate_z,y1,x1,z1,y2,x2,z2,z3,z4。 gIiSpiue7A voidcalculator(void>//算法计算函数 { /* X1=sqrt((x+a>*(x+a>+(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-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>。 uEh0U1Yfmh L=sqrt(coordinate_x*coordinate_x+coordinate_y*coordinate_y+coordinate_z*coordinate_z>。 IAg9qLsgBX 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,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,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>。 WwghWvVhPE LCD_DisNum(186,242,angle,WHITE>。 } if(coordinate_x==0&coordinate_y==0> { LCD_DispEnCh(5,240,(uint8_t*>"被测物位于基站的正上方度",RED>。 asfpsfpi4k LCD_DisNum(186,242,angle,WHITE>。 } if(coordinate_x<0&coordinate_y>0> { LCD_DispEnCh(5,240,(uint8_t*>"被测物位于基站的北偏西度",RED>。 ooeyYZTjj1 LCD_DisNum(186,242,angle,WHITE>。 } if(coordinate_x<0&coordinate_y<0> { LCD_DispEnCh(5,240,(uint8_t*>"被测物位于基站的南偏西度",RED>。 BkeGuInkxI LCD_DisNum(186,242,angle,WHITE>。 } if(coordinate_x>0&coordinate_y<0> { LCD_DispEnCh(5,240,(uint8_t*>"被测物位于基站的南偏东度",RED>。 PgdO0sRlMo LCD_DisNum(186,242,angle,WHITE>。 } if(coordinate_x==0&coordinate_y>0> { LCD_DispEnCh(5,240,(uint8_t*>"被测物位于基站正北方向",RED>。 3cdXwckm15 LCD_DisNum(186,242,angle,WHITE>。 } if(coordinate_x==0&coordinate_y<0> { LCD_DispEnCh(5,240,(uint8_t*>"被测物位于基站正南方向",RED>。 h8c52WOngM LCD_DisNum(186,242,angle,WHITE>。 } if(coordinate_x>0&coordinate_y==0> { LCD_DispEnCh(5,240,(uint8_t*>"被测物位于基站正东方向",RED>。 v4bdyGious LCD_DisNum(186,242,angle,WHITE>。 } if(coordinate_x<0&coordinate_y==0> { LCD_DispEnCh(50,240,(uint8_t*>"被测物位于基站正西方向",RED>。 J0bm4qMpJ9 LCD_DisNum(186,242,angle,WHITE>。 } } voidDelayTime_us(intTime> { unsignedchari。 for(。 Time>0。 Time--> for(i=0。 i<72。 i++>。 } /* *函数名: UltrasonicWave_Configuration *描述: 超声波模块的初始化 *输入: 无 *输出: 无 */ voidUltrasonicWave_Configuration(void> { GPIO_InitTypeDefGPIO_InitStructure。 //RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC,ENABLE>。 XVauA9grYP RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE>。 bR9C6TJscw /*GPIO_InitStructure.GPIO_Pin=TRIG_PIN。 //PC8接TRIGpN9LBDdtrd GPIO_InitStructure.GPIO_Mode=GPIO_Mode_Out_PP。 //设为推挽输出模式DJ8T7nHuGT GPIO_InitStructure.GP
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- 近距 定位 系统