CN112394733A - 基于uwb及超声波的智能车自主跟随避障方法 - Google Patents

基于uwb及超声波的智能车自主跟随避障方法 Download PDF

Info

Publication number
CN112394733A
CN112394733A CN202011347642.0A CN202011347642A CN112394733A CN 112394733 A CN112394733 A CN 112394733A CN 202011347642 A CN202011347642 A CN 202011347642A CN 112394733 A CN112394733 A CN 112394733A
Authority
CN
China
Prior art keywords
uwb
base station
intelligent vehicle
uwb base
ultrasonic sensor
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202011347642.0A
Other languages
English (en)
Inventor
陈万忠
张润哲
韩志武
李明阳
张涛
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Jilin University
Original Assignee
Jilin University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Jilin University filed Critical Jilin University
Priority to CN202011347642.0A priority Critical patent/CN112394733A/zh
Publication of CN112394733A publication Critical patent/CN112394733A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0255—Control of position or course in two dimensions specially adapted to land vehicles using acoustic signals, e.g. ultra-sonic singals
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
    • G05D1/0285—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle using signals transmitted via a public communication network, e.g. GSM network
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/02—Services making use of location information
    • H04W4/023—Services making use of location information using mutual or relative location information between multiple location based services [LBS] targets or of distance thresholds
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/30—Services specially adapted for particular environments, situations or purposes
    • H04W4/40—Services specially adapted for particular environments, situations or purposes for vehicles, e.g. vehicle-to-pedestrians [V2P]
    • H04W4/44—Services specially adapted for particular environments, situations or purposes for vehicles, e.g. vehicle-to-pedestrians [V2P] for communication between vehicles and infrastructures, e.g. vehicle-to-cloud [V2C] or vehicle-to-home [V2H]
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W64/00—Locating users or terminals or network equipment for network management purposes, e.g. mobility management

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Signal Processing (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Acoustics & Sound (AREA)
  • Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了基于UWB及超声波的智能车自主跟随避障方法，克服了存在的跟随精度不足及跟随时无法避障问题，其步骤：1)开始：智能车、UWB标签(8)上电，1号UWB基站(1)至3号UWB基站(3)时间同步；2)上位机设置所需的跟随的距离L′以及跟随的角度θ′，并通过蓝牙装置(6)将预设的跟随的距离L′以及跟随的角度θ′传送到单片机(7)；3)UWB基站测距并采集数据；4)单片机(7)对串口数据信息进行解析；5)通过三边算法得到UWB标签(9)与智能车实时相对位置(L，θ)；6)1号超声波传感(4)与2号超声波传感器(5)检测到障碍物；7)1号超声波传感(4)与2号超声波传感器(5)没检测到障碍物。

Description

基于UWB及超声波的智能车自主跟随避障方法
技术领域
本发明涉及一种跟随避障方法，更确切的说，本发明涉及一种基于UWB定位以及超声波避障相结合的智能车自主跟随避障方法。
背景技术
随着科学技术和社会的迅速发展，无人驾驶、无人机、以及自主机器人的研究和进展非常迅速，以很快的速度在我们的日常生活中普及起来。而这些技术最大的一个特点就是自主作业，自主作业的前提就是进行定位，而在接近目标的过程中难免会遇到障碍物，则要进行避障。所以定位跟随以及避障是自主作业的一个重点以及前提。
现有定位技术有GPS、蓝牙技术、红外线技术、无线局域网络等，其中GPS是目前应用最广泛的室外定位技术，其优势是利用卫星定位，其信号有效覆盖范围大，但接收机在室内工作时，由于GPS的定位精度在3m左右，而且信号受建筑物的影响而大大衰减，致使定位精度更低，所以其不适用于室内更不适用于对精度较高的自主作业的无人车或者无人机器人；对于蓝牙定位虽然受环境干扰较小，但作用距离短，通信能力不强，不便于整合到其它系统中；红外线技术有较高的定位精度，但是由于光线不能穿过障碍物，使得红外射线仅能视距传播，容易受其他灯光干扰，并且红外线的传输距离较短，使其室内定位的效果很差，当移动设备放置在口袋里或者被墙壁遮挡时，就不能正常工作，需要在每个房间、走廊安装接收天线，导致总体造价较高；无线局域网以网络节点(无线接入点)的位置信息为基础和前提，采用经验测试和信号传播模型相结合的方式，对已接入的移动设备进行位置定位，最高精确度大约在1米至20米之间。
UWB(Ultra Wideband)是一种无载波通信技术，利用纳秒至微秒级的非正弦波窄脉冲传输数据。UWB与传统通信技术的定位方法有较大差异，它不需要使用传统通信体制中的载波，而是通过发送和接收具有纳秒或纳秒级以下的极窄脉冲来传输数据，UWB中的超宽带系统与传统的窄带系统相比，具有穿透力强、功耗低、抗干扰效果好、安全性高、系统复杂度低、能够提高精确定位精度等优点，通常用于室内移动物体的定位跟踪或导航。
发明内容
本发明所要解决的技术问题是克服了现有技术存在的跟随精度不足以及跟随时无法实时避障问题，提供了一种基于UWB及超声波的智能车自主跟随避障方法。
为解决上述技术问题，本发明是采用如下技术方案实现的：所述的基于UWB及超声波的智能车自主跟随避障方法的步骤如下；
1)开始
智能车上电，UWB标签上电，1号UWB基站、2号UWB基站、3号UWB基站时间同步；
2)上位机设定跟随距离以及跟随角度
上位机设置所需的跟随的距离L′以及跟随的角度θ′，并通过蓝牙装置将预设的跟随的距离L′以及跟随的角度θ′传送到型号为STM32F103C8T6的单片机；
3)UWB基站测距并采集数据；
4)单片机对串口数据信息进行解析；
5)通过三边算法得到UWB标签与智能车实时相对位置(L，θ)；
6)1号超声波传感与2号超声波传感器检测到障碍物；
7)1号超声波传感与2号超声波传感器没检测到障碍物。
技术方案中所述的技术方案中所述的UWB基站测距并采集数据是指：
1)1号UWB基站、2号UWB基站、3号UWB基站分别同时发送给UWB标签一个无载波脉冲信号，UWB标签接收到信号后记录下此时的时间T1，同时UWB标签分别返还给1号UWB基站、2号UWB基站、3号UWB基站一个信号；
2)1号UWB基站、2号UWB基站、3号UWB基站接收到信号后记录下此时的时间T2，于是由d＝(T2-T1)×c，c为光速，计算出各个基站与UWB标签之间的距离d，依次计算出1号UWB基站、2号UWB基站、3号UWB基站与UWB标签之间的距离d1、d2、d3；
3)由串口数据格式可知，1号UWB基站、2号UWB基站、3号UWB基站与UWB标签之间的距离分别是以RANGE0、RANGE1以及RANGE2来表示，RANGE0、RANGE1以及RANGE2是d1、d2、d3的16进制表示；1号UWB基站、2号UWB基站、3号UWB基站内部组织网络，将RANGE0、RANGE1以及RANGE2通过1号UWB基站的TXD引脚将其按串口数据格式排列并传到型号为STM32F103C8T6的单片机的直接存储器DMA通道中。
技术方案中所述的单片机对串口数据信息进行解析是指：
单片机对串口数据信息进行解析，判断数据帧头MID是否为mc，判断MSAK数据位是否为07，判断正确则将串口数据信息中的RANGE0、RANGE1、RANGE2数据位截取，并将16进制的RANGE0、RANGE1、RANGE2数转化为10进制数据，其转化后分别对应1号UWB基站、2号UWB基站、3号UWB基站到UWB标签的距离d1、d2、d3。
技术方案中所述的通过三边算法得到UWB标签与智能车实时相对位置(L，θ)是指：
1)以1号UWB基站、2号UWB基站、3号UWB基站相对其呈等边三角形的几何中心点为坐标原点，几何中心点与基站1连线方向延长线为y轴正方向、以y轴正方向绕几何中心点顺时针旋转90度为x轴正方向建立直角坐标系；
2)设1号UWB基站、2号UWB基站、3号UWB基站相对坐标原点的距离位置为(x1,y1),(x2,y2),(x3,y3),设UWB标签的坐标为(x,y),由三边定位算法有
d1^2＝(x-x1)^2+(y-y1)^2+(z-z1)^2；
d2^2＝(x-x2)^2+(y-y2)^2+(z-z2)^2；
d3^2＝(x-x3)^2+(y-y3)^2+(z-z3)^2；
由此可以计算出UWB标签的位置坐标(x,y,z)；
式中：d1、d2、d3为1号UWB基站、2号UWB基站、3号UWB基站到UWB标签的距离；
3)得到UWB标签的具体坐标后，可以计算出基于智能车的车平面XY坐标系中UWB标签在智能车平面垂直投影点与智能车中心点的垂直距离为L＝(x^2+y^2)^0.5；
UWB标签向智能车平面垂直投影点与智能车中心点连线与智能车平面X轴正方向方向夹角θ＝arccos(x/(x^2+y^2)^0.5)，(L，θ)为UWB标签与智能车相对位置。
技术方案中所述的1号超声波传感与2号超声波传感器检测到障碍物是指：将避障的优先级设置高于跟随的优先级，如果在智能车前方50cm范围内检测到障碍物则进行避障，避障的具体过程如下：
(1)左前侧的1号超声波传感器检测到障碍物时智能车右转避障；
(2)右前侧的2号超声波传感器检测到障碍物时则智能车左转避障；
(3)1号超声波传感与2号超声波传感器同时检测到障碍物时则选择左拐从而实现避障。
技术方案中所述的1号超声波传感与2号超声波传感器没检测到障碍物是指：如果在智能车前方50cm范围内没有检测到障碍物：
1)则通过算法根据实时的UWB标签与智能车相对位置(L,θ)与预设值之间的差值去调整智能车左右电机转速：
(1)当智能车中心点与UWB标签在智能车平面垂直投影点距离L与预设值L′的差值ΔL在10cm的范围内波动时保持匀速；
(2)当智能车中心点与UWB标签在智能车平面垂直投影点距离L与预设值L′的差值ΔL绝对值超过10cm时，按照ΔL/L′的比例去调整电机转速，当ΔL>0时按ΔL/L′的比例加速，当ΔL<0时则按ΔL/L′的比例减速；
(3)当智能车中心点与UWB标签的投影点连线与水平方向夹角θ与预设值θ′的差值Δθ在5度的范围内波动则不用调整左右电机的转速差；
(4)当智能车中心点与UWB标签的投影点连线与水平方向夹角θ与预设值θ′的差值Δθ的绝对值超过5度时，按照Δθ/θ′的比例去调整电机转速，当Δθ>0时按Δθ/θ′的比例加速右侧电机，当Δθ<0时则按|Δθ/θ′|的比例加速左侧电机转速；
2)1号超声波传感与2号超声波传感器在跟随的过程中如若检测到障碍物则中断跟随过程进行避障，当1号超声波传感与2号超声波传感器检测智能车前方50cm以内无障碍物时避障过程结束，重新进入到跟随过程，依次实现了跟随过程中实时避障。
与现有技术相比本发明的有益效果是：
1.本发明所述的基于UWB及超声波的智能车自主跟随避障方法与传统定位跟随方法相比具有更高的定位精度；
2.本发明所述的基于UWB及超声波的智能车自主跟随避障方法可以设定跟随者与被跟随者的相对位置；
3.本发明所述的基于UWB及超声波的智能车自主跟随避障方法在跟随的过程中能够实时的避障；
附图说明
下面结合附图对本发明作进一步的说明：
图1是本发明所述的基于UWB及超声波的智能车结构组成的轴测投影视图；
图2是本发明所述的基于UWB及超声波的智能车跟随的目标即UWB标签的示意图；
图3是本发明所述的基于UWB及超声波的智能车的单片机接线简图
图4是本发明所述的基于UWB及超声波的智能车中型号为STM32F103C8T6的单片机与1号UWB基站接线示意图；
图5是本发明所述的基于UWB及超声波的智能车的UWB串口数据图；
图6是本发明所述的基于UWB及超声波的智能车中型号为STM32F103C8T6的单片机和左侧1号超声波传感器与右侧2号超声波传感器接线示意图；
图7是本发明所述的基于UWB及超声波的智能车自主跟随避障方法中UWB定位算法原理图；
图8是本发明所述的基于UWB及超声波的智能车的1号超声波传感器与2号超声波传感器安装及工作原理示意图；
图9是本发明所述的实现基于UWB及超声波的智能车自主跟随避障方法的整体系统示意图；
图10是本发明所述的基于UWB及超声波的智能车自主跟随避障方法的流程框图。
图中：1.1号UWB基站，2.2号UWB基站，3.3号UWB基站，4.1号超声波传感器，5.2号超声波传感器，6.蓝牙装置，7.单片机，8.智能车底盘系统，9.UWB标签。
具体实施方式
下面结合附图对本发明作详细的描述：
参阅图1，本发明所述的基于UWB及超声波的智能车包括单片机7、1号UWB基站1、2号UWB基站2、3号UWB基站3、1号超声波传感器4、2号超声波传感器5、蓝牙装置6与智能车底盘系统8。
参阅图2，图中所示的UWB标签9是基于UWB及超声波的智能车跟随的目标。
所述的1号UWB基站1、2号UWB基站2、3号UWB基站3以及UWB标签9采用的是型号为UWB Mini 3s的UWB基站与UWB标签，型号为UWB Mini 3s的1号UWB基站1、2号UWB基站2、3号UWB基站3、UWB标签9在无遮挡的情况下通讯距离长达80m，定位精度能控制在10cm以内，具有很高的定位精度，1号UWB基站1、2号UWB基站2、3号UWB基站3在车底座上摆放位置之间的连线为等边三角形；
所述的1号超声波传感器4与2号超声波传感器5采用型号为HC-SR04超声波传感器，该型号的超声波传感器可以提供2cm-400cm的非接触式距离感测功能，测量精度可达到3mm，测量角度为15度；
所述的蓝牙装置6则采用型号为HC-05的主从一体蓝牙装置；
所述的单片机7采用的是型号为STM32F103C8T6的单片机，型号为STM32F103C8T6的单片机7包括有10个定时器、两个12位数模转换器、5个USART接口、12条直接存储器DMA(Direct memory access)通道，在数据的传输处理方面带来很大的便利。
所述的智能车底盘系统8包括车底座、电机驱动装置、4个智能车车轮、2个可调速电机、5V供电电源；
参阅图3，本发明所述的基于UWB及超声波的智能车的型号为STM32F103C8T6的单片机7和UWB基站(包括1号UWB基站1、2号UWB基站2、3号UWB基站3)、蓝牙装置6、1号超声波传感器4、2号超声波传感器5与智能车底盘系统8中的电机驱动装置直连。
所述的电机驱动装置采用型号为L293D的电机驱动装置，输出驱动电流为1000mA,可以很方便的驱动两个变速直流电机，可以通过PWM电机调速算法来调节电压占空比从而改变电机转速让智能车前进、后退、停止和拐弯。
参阅图4，1号UWB基站1的VCC、GND、RXD及TXD引脚分别与型号为STM32F103C8T6的单片机7的VCC、GND、TXD及RXD引脚连接。1号UWB基站1通过TXD引脚将所有基站的位置信息发送到型号为STM32F103C8T6的单片机7的RXD引脚，型号为STM32F103C8T6的单片机7通过RXD引脚接收到3个基站相对标签9的相对位置信息并放入到直接存储器DMA进行串口数据解析以及数据处理，2号UWB基站2以及3号UWB基站3只需要VCC和GND引脚分别与型号为STM32F103C8T6的单片机7的VCC和GND引脚连接即可。
参阅图5，1号UWB基站1通过TXD引脚按照图5数据格式传入型号为STM32F103C8T6的单片机7直接存储器DMA中。其中：
MID为数据帧的帧头，占用2个字节，有3种模式分别为ma，mr，mc，在此定位过程中采用mc模式，mc模式表示数据位RANGE0、RANGE1、RANGE2内容为标签分别到1号基站1、2号基站2、3号基站3的距离；
MASK为数据帧中的控制段，占用2个字节，表示RANGE0、RANGE1、RANGE2、RANGE3中哪几个数据位有效，而本专利只采用3个基站测距，则MASK应为07，表示前3个数据位有效；
RANGE0、RANGE1、RANGE2、RANGE3是数据区分别占用8个字节，当MID选用mc、MASK设置为07时，RANGE0、RANGE1、RANGE2内容为UWB标签9分别到1号基站1、2号基站2、3号基站3的距离，而RANGE3为无效数据位；
NRANGES以及RSEQ分别为原始数据计数器以及数据帧计数器分别占用4个和2个字节；
DEBUG占用8个字节当选用mc模式时不会产生串口天线延迟；
aT:A为数据帧的尾帧占用4个字节，T用来表示标签ID，A是基站ID。
所以串口解析过程中MID选取mc模式，MASK设为07，再将分别从RANGE0、RANGE1、RANGE2读取到的16进制数据转换为10进制数据，其分别表示1号基站1、2号基站2、3号基站3到UWB标签9的距离。
参阅图6，本发明所述的基于UWB及超声波的智能车中1号超声波传感器4的VCC、GND、TRIG及ECHO引脚分别与型号为STM32F103C8T6的单片机7的VCC、GND、PA1及PA2引脚连接，2号超声波传感器5的VCC、GND、TRIG及ECHO引脚分别与型号为STM32F103C8T6的单片机7的VCC、GND、PA9及PA10引脚连接，PA1、PA2、PA9、PA10引脚为型号为STM32F103C8T6的单片机7的IO口负责数据的收发。当TRIG引脚受到型号为STM32F103C8T6的单片机7的IO口高电平脉冲时，超声波传感器自动发送8个40khz的方波并自动检测是否有信号返回；有信号返回时，通过ECHO给型号为STM32F103C8T6的单片机7的IO口输入一个高电平，高电平持续的时间是超声波从发射到返回的时间。若已知高电平持续的时间为t，即可计算出超声波发射点与障碍物之间的距离s，计算公式为s＝t×340(m/s)/2，其中340(m/s)为声波在空气中传播的速度。
参阅图7，1号UWB基站1、2号UWB基站2、3号UWB基站3分别同时发送给UWB标签9一个无载波脉冲信号，UWB标签9接收到信号后记录下此时的时间T1，同时UWB标签9分别返还给1号UWB基站1、2号UWB基站2、3号UWB基站3一个信号，1号UWB基站1、2号UWB基站2、3号UWB基站3接收到信号后记录下此时的时间T2，于是就可以由d＝(T2-T1)×c(c为光速)计算出各个基站与UWB标签9之间的距离d，依次可以计算出1号UWB基站1、2号UWB基站2、3号UWB基站3与UWB标签9之间的距离d1，d2，d3；以1号UWB基站1、2号UWB基站2、3号UWB基站3相对其呈等边三角形的几何中心为坐标原点，几何中心点与基站1连线方向延长线为y轴正方向、以y轴正方向绕几何中心点顺时针旋转90度为x轴正方向建立直角坐标系；设1号UWB基站1、2号UWB基站2、3号UWB基站3相对原点的的距离位置为(x1,y1),(x2,y2),(x3,y3),设UWB标签9的坐标为(x,y),由三边定位算法有
d1^2＝(x-x1)^2+(y-y1)^2+(z-z1)^2；
d2^2＝(x-x2)^2+(y-y2)^2+(z-z2)^2；
d3^2＝(x-x3)^2+(y-y3)^2+(z-z3)^2；
由此可以计算出UWB标签9的坐标(x,y,z)。得到目标的具体坐标点后，可以计算出基于智能车的车平面XY坐标系中UWB标签9的坐标投影点与智能车的垂直距离为L＝(x^2+y^2)^0.5，投影点与智能车中心坐标连线与水平方向夹角θ＝arccos(x/(x^2+y^2)^0.5),得到d0与θ两个数值后可以通过设置这两个值来控制智能车跟随目标的垂直距离以及角度。
参阅图8，1号超声波传感4和2号超声波传感器5分别以45°水平地固定在智能车的左前端和右前端，1号超声波传感器4与2号超声波传感器5分别检测其前方15度范围内2cm-400cm距离的障碍物，设定障碍物判定的距离为50cm以内。当左前侧的1号超声波传感器4检测到障碍物时智能车右转避障；当右前侧的2号超声波传感器5检测到障碍物时则智能车左转避障；当1号超声波传感4和2号超声波传感器5同时检测到障碍物时则选择左拐从而实现避障。
参阅图9，图9是实现基于UWB及超声波的智能车自主跟随避障方法的整体系统示意图；整体系统由上位机(具有蓝牙传输功能的笔记本电脑均可)、图1所示的智能车以及携带有UWB标签9移动的目标。实现基于UWB及超声波的智能车自主跟随避障方法的整体流程如下：
智能车上电初始化、UWB标签9上电，上位机设定智能车与目标的距离L以及角度θ并通过蓝牙通信发送给蓝牙装置6，型号为STM32F103C8T6的单片机7通过蓝牙装置6接收到上位机传输的数据后，将预设值修改并将1号UWB基站1、2号UWB基站2、3号UWB基站3的时间同步。1号UWB基站1、2号UWB基站2、3号UWB基站3开始向UWB标签9发送无载波脉冲信号，当UWB标签9分别接收到1号UWB基站1、2号UWB基站2、3号UWB基站3发来的脉冲信号立即分别向1号UWB基站1、2号UWB基站2、3号UWB基站3发送信号反馈，各个基站采用到达时间TOA(Timeof Arrival)算法分别测得每个基站与UWB标签9之间的相对位置，1号UWB基站1、2号UWB基站2、3号UWB基站3内部组织网络，通过1号UWB基站1将测得的位置信息传到型号为STM32F103C8T6的单片机7的直接存储器DMA通道中，型号为STM32F103C8T6的单片机7将当前智能车与目标的距离L以及角度θ与预设值作比对，通过算法调节智能车电机转速从而实现目标跟随；而当1号超声波传感4与2号超声波传感器5检测到有障碍物时，中断跟随过程实行避障，避障结束后再次进入到跟随过程。
参阅图10，是本发明所述的基于UWB及超声波的智能车自主跟随避障方法的步骤如下
1.开始
智能车上电，UWB标签9上电，1号UWB基站1、2号UWB基站2、3号UWB基站3时间同步；
2.上位机设定跟随距离以及跟随角度
上位机设置所需的跟随的距离L′以及跟随的角度θ′，并通过蓝牙装置6将预设的跟随的距离L′以及跟随的角度θ′传送到型号为STM32F103C8T6的单片机7；
3.UWB基站采集数据
1)1号UWB基站1、2号UWB基站2、3号UWB基站3分别同时发送给UWB标签9一个无载波脉冲信号，UWB标签9接收到信号后记录下此时的时间T1，同时UWB标签9分别返还给1号UWB基站1、2号UWB基站2、3号UWB基站3一个信号；
2)1号UWB基站1、2号UWB基站2、3号UWB基站3接收到信号后记录下此时的时间T2，于是就可以由d＝(T2-T1)×c，c为光速；计算出各个基站与UWB标签9之间的距离d，依次可以计算出1号UWB基站1、2号UWB基站2、3号UWB基站3与UWB标签9之间的距离d1、d2、d3；
3)由串口数据格式可知，1号UWB基站1、2号UWB基站2、3号UWB基站3与UWB标签9之间的距离分别是以RANGE0、RANGE1以及RANGE2来表示，RANGE0、RANGE1以及RANGE2是d1、d2、d3的16进制表示；1号UWB基站1、2号UWB基站2、3号UWB基站3内部组织网络，将RANGE0、RANGE1以及RANGE2通过1号UWB基站1的TXD引脚将其按串口数据格式排列并传到型号为STM32F103C8T6的单片机7的直接存储器DMA通道中；
4.单片机7对串口数据信息进行解析
型号为STM32F103C8T6的单片机7对串口数据信息进行解析，判断数据帧头MID是否为mc，判断MSAK数据位是否为07，判断正确则将串口数据信息中的RANGE0、RANGE1、RANGE2数据位截取，并将16进制的RANGE0、RANGE1、RANGE2数转化为10进制数据，其转化后的10进制数据分别对应1号UWB基站1、2号UWB基站2、3号UWB基站3到UWB标签9的距离d1、d2、d3；
5.通过三边算法得到UWB标签9与智能车实时相对位置(L，θ)
1)以1号UWB基站1、2号UWB基站2、3号UWB基站3相对其呈等边三角形的几何中心点为坐标原点，几何中心点与基站1连线方向延长线为y轴正方向、以y轴正方向绕几何中心点顺时针旋转90度为x轴正方向建立直角坐标系；
2)设1号UWB基站1、2号UWB基站2、3号UWB基站3相对坐标原点的距离位置为(x1,y1),(x2,y2),(x3,y3),设UWB标签9的坐标为(x,y),由三边定位算法有
d1^2＝(x-x1)^2+(y-y1)^2+(z-z1)^2；
d2^2＝(x-x2)^2+(y-y2)^2+(z-z2)^2；
d3^2＝(x-x3)^2+(y-y3)^2+(z-z3)^2；
由此可以计算出UWB标签9的位置坐标(x,y,z)；
式中：d1、d2、d3为1号UWB基站1、2号UWB基站2、3号UWB基站3到UWB标签9的距离；
3)得到UWB标签9的具体坐标点后，可以计算出基于智能车的车平面XY坐标系中UWB标签9在智能车平面垂直投影点与智能车的垂直距离为L＝(x^2+y^2)^0.5，智能车中心与UWB标签9的投影点连线与水平方向夹角θ＝arccos(x/(x^2+y^2)^0.5)，(L，θ)为UWB标签9与智能车相对位置；
6.1号超声波传感4与2号超声波传感器5检测到障碍物
将智能车避障的优先级设置高于智能车跟随的优先级，如果在智能车前方50cm范围内检测到障碍物则进行避障，避障的具体过程如下：
(1)左前侧的1号超声波传感器4检测到障碍物时智能车右转避障；
(2)右前侧的2号超声波传感器5检测到障碍物时则智能车左转避障；
(3)1号超声波传感4与2号超声波传感器5同时检测到障碍物时则选择左拐从而实现避障；
7.1号超声波传感4与2号超声波传感器5没检测到障碍物
如果在智能车前方50cm范围内没有检测到障碍物：
1)则根据实时的UWB标签9和智能车相对位置(L,θ)与预设值之间的差值去调整智能车左右电机转速：
(1)当智能车中心点与UWB标签9垂直距离L与预设值L′的差值ΔL在10cm的范围内波动时保持匀速；
(2)当智能车中心点与UWB标签9垂直距离L与预设值L′的差值ΔL绝对值超过10cm时，按照ΔL/L′的比例去调整电机转速，当ΔL>0时按ΔL/L′的比例加速，当ΔL<0时则按ΔL/L′的比例减速；
(3)当智能车中心点与UWB标签9的投影点连线与水平方向夹角θ与预设值θ′的差值Δθ在5度的范围内波动则不用调整左右电机的转速差；
(4)当智能车中心点与UWB标签9的投影点连线与水平方向夹角θ与预设值θ′的差值Δθ的绝对值超过5度时，按照Δθ/θ′的比例去调整电机转速，当Δθ>0时按Δθ/θ′的比例加速右侧电机，当Δθ<0时则按|Δθ/θ′|的比例加速左侧电机转速；
2)1号超声波传感4与2号超声波传感器5在跟随的过程中如若检测到障碍物则中断跟随过程进行避障，当1号超声波传感4与2号超声波传感器5检测智能车前方50cm以内无障碍物时避障过程结束，重新进入到跟随过程，依次实现了跟随过程中实时避障。

Claims (6)

1.一种基于UWB及超声波的智能车自主跟随避障方法，其特征在于，所述的基于UWB及超声波的智能车自主跟随避障方法的步骤如下；
1)开始
智能车上电，UWB标签(8)上电，1号UWB基站(1)、2号UWB基站(2)、3号UWB基站(3)时间同步；
2)上位机设定跟随距离以及跟随角度
上位机设置所需的跟随的距离L′以及跟随的角度θ′，并通过蓝牙装置(6)将预设的跟随的距离L′以及跟随的角度θ′传送到型号为STM32F103C8T6的单片机(7)；
3)UWB基站测距并采集数据；
4)单片机(7)对串口数据信息进行解析；
5)通过三边算法得到UWB标签(9)与智能车实时相对位置(L，θ)；
6)1号超声波传感(4)与2号超声波传感器(5)检测到障碍物；
7)1号超声波传感(4)与2号超声波传感器(5)没检测到障碍物。
2.按照权利要求1所述的基于UWB及超声波的智能车自主跟随避障方法，其特征在于，所述的UWB基站测距并采集数据是指：
1)1号UWB基站(1)、2号UWB基站(2)、3号UWB基站(3)分别同时发送给UWB标签(9)一个无载波脉冲信号，UWB标签(9)接收到信号后记录下此时的时间T1，同时UWB标签(9)分别返还给1号UWB基站(1)、2号UWB基站(2)、3号UWB基站(3)一个信号；
2)1号UWB基站(1)、2号UWB基站(2)、3号UWB基站(3)接收到信号后记录下此时的时间T2，于是由d＝(T2-T1)×c，c为光速，计算出各个基站与UWB标签(9)之间的距离d，依次计算出1号UWB基站(1)、2号UWB基站(2)、3号UWB基站(3)与UWB标签(9)之间的距离d1、d2、d3；
3)由串口数据格式可知，1号UWB基站(1)、2号UWB基站(2)、3号UWB基站(3)与UWB标签(9)之间的距离分别是以RANGE0、RANGE1以及RANGE2来表示，RANGE0、RANGE1以及RANGE2是d1、d2、d3的16进制表示；1号UWB基站(1)、2号UWB基站(2)、3号UWB基站(3)内部组织网络，将RANGE0、RANGE1以及RANGE2通过1号UWB基站(1)的TXD引脚将其按串口数据格式排列并传到型号为STM32F103C8T6的单片机(7)的直接存储器DMA通道中。
3.按照权利要求1所述的基于UWB及超声波的智能车自主跟随避障方法，其特征在于，所述的单片机(7)对串口数据信息进行解析是指：
单片机(7)对串口数据信息进行解析，判断数据帧头MID是否为mc，判断MSAK数据位是否为07，判断正确则将串口数据信息中的RANGE0、RANGE1、RANGE2数据位截取，并将16进制的RANGE0、RANGE1、RANGE2数转化为10进制数据，其转化后分别对应1号UWB基站(1)、2号UWB基站(2)、3号UWB基站(3)到UWB标签(9)的距离d1、d2、d3。
4.按照权利要求1所述的基于UWB及超声波的智能车自主跟随避障方法，其特征在于，所述的通过三边算法得到UWB标签(9)与智能车实时相对位置(L，θ)是指：
1)以1号UWB基站1、2号UWB基站2、3号UWB基站3相对其呈等边三角形的几何中心点为坐标原点，几何中心点与基站1连线方向延长线为y轴正方向、以y轴正方向绕几何中心点顺时针旋转90度为x轴正方向建立直角坐标系；
2)设1号UWB基站(1)、2号UWB基站(2)、3号UWB基站(3)相对坐标原点的距离位置为(x1,y1),(x2,y2),(x3,y3),设UWB标签(9)的坐标为(x,y),由三边定位算法有
d1^2＝(x-x1)^2+(y-y1)^2+(z-z1)^2；
d2^2＝(x-x2)^2+(y-y2)^2+(z-z2)^2；
d3^2＝(x-x3)^2+(y-y3)^2+(z-z3)^2；
由此可以计算出UWB标签(9)的位置坐标(x,y,z)；
式中：d1、d2、d3为1号UWB基站(1)、2号UWB基站(2)、3号UWB基站(3)到UWB标签(9)的距离；
3)得到UWB标签(9)的具体坐标后，可以计算出基于智能车的车平面XY坐标系中UWB标签(9)在智能车平面垂直投影点与智能车中心点的直线距离为L＝(x^2+y^2)^0.5；
UWB标签(9)向智能车平面垂直投影点与智能车中心点连线与智能车平面X轴正方向夹角θ＝arccos(x/(x^2+y^2)^0.5)，(L,θ)为UWB标签(9)与智能车相对位置。
5.按照权利要求1所述的基于UWB及超声波的智能车自主跟随避障方法，其特征在于，所述的1号超声波传感(4)与2号超声波传感器(5)检测到障碍物是指：
将智能车避障的优先级设置高于智能车跟随的优先级，如果在智能车前方50cm范围内检测到障碍物则进行避障，避障的具体过程如下：
(1)左前侧的1号超声波传感器(4)检测到障碍物时智能车右转避障；
(2)右前侧的2号超声波传感器(5)检测到障碍物时则智能车左转避障；
(3)1号超声波传感(4)与2号超声波传感器(5)同时检测到障碍物时则选择左拐从而实现避障。
6.按照权利要求1所述的基于UWB及超声波的智能车自主跟随避障方法，其特征在于，所述的1号超声波传感(4)与2号超声波传感器(5)没检测到障碍物是指：如果在智能车前方50cm范围内没有检测到障碍物：
1)根据实时的UWB标签(9)与智能车相对位置(L，θ)和预设值之间的差值去调整智能车左右电机转速：
(1)当智能车中心点与UWB标签(9)在智能车平面垂直投影点距离L与预设值L′的差值ΔL在10cm的范围内波动时保持匀速；
(2)当智能车中心点与UWB标签(9)在智能车平面垂直投影点距离L与预设值L′的差值ΔL绝对值超过10cm时，按照ΔL/L′的比例去调整电机转速，当ΔL>0时按ΔL/L′的比例加速，当ΔL<0时则按ΔL/L′的比例减速；
(3)当智能车中心点与UWB标签(9)的投影点连线与水平方向夹角θ与预设值θ′的差值Δθ在5度的范围内波动则不用调整左右电机的转速差；
(4)当智能车中心点与UWB标签(9)的投影点连线与水平方向夹角θ与预设值θ′的差值Δθ的绝对值超过5度时，按照Δθ/θ′的比例去调整电机转速，当Δθ>0时按Δθ/θ′的比例加速右侧电机，当Δθ<0时则按|Δθ/θ′|的比例加速左侧电机转速；
2)1号超声波传感(4)与2号超声波传感器(5)在跟随的过程中如若检测到障碍物则中断跟随过程进行避障，当1号超声波传感(4)与2号超声波传感器(5)检测智能车前方50cm以内无障碍物时避障过程结束，重新进入到跟随过程，依次实现了跟随过程中实时避障。
CN202011347642.0A 2020-11-26 2020-11-26 基于uwb及超声波的智能车自主跟随避障方法 Pending CN112394733A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011347642.0A CN112394733A (zh) 2020-11-26 2020-11-26 基于uwb及超声波的智能车自主跟随避障方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011347642.0A CN112394733A (zh) 2020-11-26 2020-11-26 基于uwb及超声波的智能车自主跟随避障方法

Publications (1)

Publication Number Publication Date
CN112394733A true CN112394733A (zh) 2021-02-23

Family

ID=74605264

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011347642.0A Pending CN112394733A (zh) 2020-11-26 2020-11-26 基于uwb及超声波的智能车自主跟随避障方法

Country Status (1)

Country Link
CN (1) CN112394733A (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113115223A (zh) * 2021-03-30 2021-07-13 长城汽车股份有限公司 载物车自动跟随控制方法及控制装置
CN113885510A (zh) * 2021-10-21 2022-01-04 齐鲁工业大学 一种四足机器人避障及领航员跟随方法及系统
CN114390439A (zh) * 2022-01-11 2022-04-22 厦门大学 一种基于aoa的定位方法及介质
CN114679778A (zh) * 2022-03-21 2022-06-28 Pfu上海计算机有限公司 自动跟随方法、系统、电子设备及计算机可读存储介质

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20160363933A1 (en) * 2014-12-15 2016-12-15 Irobot Corporation Robot Lawnmower Mapping
CN107713230A (zh) * 2017-09-30 2018-02-23 合肥学院 一种智能跟随代步箱包及其控制方法
CN107765220A (zh) * 2017-09-20 2018-03-06 武汉木神机器人有限责任公司 基于uwb和激光雷达混合定位的行人跟随系统及方法
CN108013563A (zh) * 2018-01-03 2018-05-11 山东农业大学 一种基于uwb的自动调速跟随与避障的旅行箱及跟随算法
CN108128392A (zh) * 2017-11-20 2018-06-08 上海理工大学 一种可折叠式自动跟随电动车
CN208061024U (zh) * 2018-02-08 2018-11-06 苏州纳秒机器人有限公司 一种电商分拣自动跟随机器人
CN109348426A (zh) * 2018-11-26 2019-02-15 黎国溥 一种基于uwb的跟随系统及方法
CN209198940U (zh) * 2018-12-17 2019-08-02 东北大学秦皇岛分校 图书馆自动跟随车

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20160363933A1 (en) * 2014-12-15 2016-12-15 Irobot Corporation Robot Lawnmower Mapping
CN107765220A (zh) * 2017-09-20 2018-03-06 武汉木神机器人有限责任公司 基于uwb和激光雷达混合定位的行人跟随系统及方法
CN107713230A (zh) * 2017-09-30 2018-02-23 合肥学院 一种智能跟随代步箱包及其控制方法
CN108128392A (zh) * 2017-11-20 2018-06-08 上海理工大学 一种可折叠式自动跟随电动车
CN108013563A (zh) * 2018-01-03 2018-05-11 山东农业大学 一种基于uwb的自动调速跟随与避障的旅行箱及跟随算法
CN208061024U (zh) * 2018-02-08 2018-11-06 苏州纳秒机器人有限公司 一种电商分拣自动跟随机器人
CN109348426A (zh) * 2018-11-26 2019-02-15 黎国溥 一种基于uwb的跟随系统及方法
CN209198940U (zh) * 2018-12-17 2019-08-02 东北大学秦皇岛分校 图书馆自动跟随车

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113115223A (zh) * 2021-03-30 2021-07-13 长城汽车股份有限公司 载物车自动跟随控制方法及控制装置
CN113115223B (zh) * 2021-03-30 2023-04-07 长城汽车股份有限公司 载物车自动跟随控制方法及控制装置
CN113885510A (zh) * 2021-10-21 2022-01-04 齐鲁工业大学 一种四足机器人避障及领航员跟随方法及系统
CN113885510B (zh) * 2021-10-21 2023-11-10 齐鲁工业大学 一种四足机器人避障及领航员跟随方法及系统
CN114390439A (zh) * 2022-01-11 2022-04-22 厦门大学 一种基于aoa的定位方法及介质
CN114679778A (zh) * 2022-03-21 2022-06-28 Pfu上海计算机有限公司 自动跟随方法、系统、电子设备及计算机可读存储介质

Similar Documents

Publication Publication Date Title
CN112394733A (zh) 基于uwb及超声波的智能车自主跟随避障方法
CN108196549A (zh) 一种用于潜入式agv激光红外避障系统
CN201266322Y (zh) 超声波目标定位与跟踪装置
CN208126197U (zh) 一种无人小车控制系统及无人小车
CN103941231A (zh) 超声射频信号联合处理的室内定位系统及定位方法
CN110631576A (zh) 一种基于uwb和imu的抗nlos的室内定位系统及其定位方法
CN104953709A (zh) 一种变电站智能巡视机器人
CN210835734U (zh) 无人机搜救系统
CN111364539A (zh) 一种纯电驱动无人驾驶推煤机
CN203838560U (zh) 一种用于辐射环境的机械手载体agv小车
CN110177337A (zh) 室内定位方法及室内定位购物车系统
WO2018228254A1 (zh) 一种移动电子设备以及该移动电子设备中的方法
CN112346008A (zh) 基于uwb的agv定位导航方法及系统装置
CN111722623A (zh) 一种基于蓝牙阵列天线定位的自动跟随系统
WO2023015251A1 (en) Assisted parking of electrical vehicle at charging station
CN211765598U (zh) 基于地铁列车精确定位的追踪系统
CN104281160A (zh) 一种近距离自动跟随系统
CN213689923U (zh) 一种基于uwb的agv定位导航装置
CN110161515A (zh) 一种基于激光测距的房屋测量系统
CN114545918A (zh) 一种可接入移动终端的机器人巡检系统及巡检方法
CN212388632U (zh) 一种纯电驱动无人驾驶推煤机
CN211263763U (zh) 基于uwb测距和gps定位的流动机械防撞定位装置
CN204423152U (zh) 空中障碍物检测装置及其自动导引运输车
CN203650526U (zh) 一种基于嵌入式的适于自动追踪与循迹的机器人
CN104199040A (zh) 一种倒车入库检测系统及方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
RJ01 Rejection of invention patent application after publication
RJ01 Rejection of invention patent application after publication

Application publication date: 20210223