一种基于UWB阵列的定位导航方法与终端技术

本发明专利技术公开了一种基于UWB阵列的定位导航方法与终端；本发明专利技术在预设的UWB阵列中选择预设的UWB基站作为原点坐标建立直角坐标系，并根据预设的各个所述UWB基站间的距离确定各个所述UWB基站的坐标，根据预设时间间隔实时获取车辆与各个所述UWB基站间的距离，并根据与车辆距离最近的四个UWB基站的坐标计算得到车辆的坐标，以两次连续获取的车辆坐标，计算车辆的航向角，从而根据坐标和航向角对车辆进行定位导航；定位基于距离最近的四个UWB基站，能够使定位更加准确，同时不需要依靠GPS或北斗的定位导航，且UWB模块的带宽大，抗干扰性强，信号更加稳定。

【技术实现步骤摘要】
一种基于UWB阵列的定位导航方法与终端
本专利技术涉及无人驾驶
，特别涉及一种基于UWB阵列的定位导航方法与终端。
技术介绍
无人驾驶汽车是汽车领域今后发展的主要趋势，在无人驾驶中，给无人车提供精准的定位和导航是很重要的环节。目前无人驾驶采用的定位导航技术大都是GPS或北斗等，但当这些信号被屏蔽时，或者信号弱的情况下，车辆就难以实现定位和导航，从而影响无人驾驶任务的进行。
技术实现思路
本专利技术所要解决的技术问题是：提供一种基于UWB阵列的定位导航方法与终端，实现不需要依靠GPS或北斗的定位导航。为了解决上述技术问题，本专利技术采用的技术方案为：一种基于UWB阵列的定位导航方法，包括：S1、在预设的UWB阵列中选择预设的UWB基站作为原点坐标建立直角坐标系，并根据预设的各个所述UWB基站间的距离确定各个所述UWB基站的坐标，所述UWB阵列为由多个UWB基站架设的矩形阵列；S2、根据预设时间间隔实时获取车辆与各个所述UWB基站间的距离，并结合距离最近的四个所述UWB基站的坐标计算所述车辆的当前位置在所建立的所述直角坐标系中的第一坐标；S3、根据所述第一坐标以及所述车辆在预设时间间隔前的所在位置的第二坐标，计算所述车辆的航向角，所述航向角为所述车辆的行驶方向与所述直角坐标系的Y轴正向的夹角；S4、根据所述车辆的第一坐标和航向角在所述直角坐标系中对所述车辆进行定位和导航。为了解决上述技术问题，本专利技术采用的另一种技术方案为：一种基于UWB阵列的定位导航终端，包括处理器、存储器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现以下步骤：S1、在预设的UWB阵列中选择预设的UWB基站作为原点坐标建立直角坐标系，并根据预设的各个所述UWB基站间的距离确定各个所述UWB基站的坐标，所述UWB阵列为由多个UWB基站架设的矩形阵列；S2、根据预设时间间隔实时获取车辆与各个所述UWB基站间的距离，并结合距离最近的四个所述UWB基站的坐标计算所述车辆的当前位置在所建立的所述直角坐标系中的第一坐标；S3、根据所述第一坐标以及所述车辆在预设时间间隔前的所在位置的第二坐标，计算所述车辆的航向角，所述航向角为所述车辆的行驶方向与所述直角坐标系的Y轴正向的夹角；S4、根据所述车辆的第一坐标和航向角在所述直角坐标系中对所述车辆进行定位和导航。本专利技术的有益效果在于：本专利技术通过多个UWB基站架设成矩形阵列，并以此建立直角坐标系，根据车载的UWB模块与UWB基站间的距离，以及基站间的固定间距，从而实现对车辆实时的快速定位，同时定位基于距离最近的四个UWB基站，能够使定位更加准确，也能够实时获取车辆的航向角，实现不需要依靠GPS或北斗的定位导航，且UWB模块的带宽大，抗干扰性强，信号更加稳定。附图说明图1为本专利技术实施例的一种基于UWB阵列的定位导航方法的流程图；图2为本专利技术实施例的一种基于UWB阵列的定位导航终端的结构图；图3为本专利技术实施例的一种基于UWB阵列的定位导航方法的坐标展示图；标号说明：1、一种基于UWB阵列的定位导航终端；2、处理器；3、存储器。具体实施方式为详细说明本专利技术的
技术实现思路
、所实现目的及效果，以下结合实施方式并配合附图予以说明。请参照图1以及图3，一种基于UWB阵列的定位导航方法，包括：S1、在预设的UWB阵列中选择预设的UWB基站作为原点坐标建立直角坐标系，并根据预设的各个所述UWB基站间的距离确定各个所述UWB基站的坐标，所述UWB阵列为由多个UWB基站架设的矩形阵列；S2、根据预设时间间隔实时获取车辆与各个所述UWB基站间的距离，并结合距离最近的四个所述UWB基站的坐标计算所述车辆的当前位置在所建立的所述直角坐标系中的第一坐标；S3、根据所述第一坐标以及所述车辆在预设时间间隔前的所在位置的第二坐标，计算所述车辆的航向角，所述航向角为所述车辆的行驶方向与所述直角坐标系的Y轴正向的夹角；S4、根据所述车辆的第一坐标和航向角在所述直角坐标系中对所述车辆进行定位和导航。从上述描述可知，本专利技术的有益效果在于：本专利技术的有益效果在于：本专利技术通过多个UWB基站架设成矩形阵列，并以此建立直角坐标系，根据车载的UWB模块与UWB基站间的距离，以及基站间的固定间距，从而实现对车辆实时的快速定位，同时定位基于距离最近的四个UWB基站，能够使定位更加准确，也能够实时获取车辆的航向角，实现不需要依靠GPS或北斗的定位导航，且UWB模块的带宽大，抗干扰性强，信号更加稳定。进一步地，所述步骤S2具体为：根据预设时间间隔获取车辆与各个所述UWB基站间的距离，获取距离最近的四个所述UWB基站的基站坐标，选择其中纵坐标小的两个所述基站坐标作为第一基站坐标和第二基站坐标，其中所述第一基站坐标的横坐标小于第二基站坐标的横坐标，将所述车辆与所述第一基站坐标的第一距离a、所述车辆与所述第二基站坐标的第二距离c、所述第一基站坐标和所述第二基站坐标间的第三距离b代入以下公式：p＝(a+b+c)/2；S＝√[p*(p-a)*(p-b)*(p-c)]S＝h*b/2a2＝h2+t2计算出所述车辆的当前位置的第一坐标相对于第一基站坐标(x，y)的横向差值t，纵向差值h，从而得到所述第一坐标(x+t，y+h)。由上述描述可知，本申请通过获取车辆与各个所述UWB基站间的距离，选择最近的四个基站中的两个获取其坐标进行当前车辆位置坐标的计算，较之距离远的UWB基站，距离近的UWB基站于车辆间的距离能够更加准确，且因为基于UWB矩阵进行定位，每次车辆的定位坐标均位于四个UWB基站围成的矩形范围中，能够快速地确定车辆定位范围，并选择其中两个坐标从而组成三角形进而更方便快捷地进行计算出车辆的定位坐标，提高了车辆坐标获取的准确性和方便性。进一步地，所述步骤S2还包括：所述第一距离与所述第二距离之和与所述第三距离的差值小于预设的距离误差值，则所述车辆当前位置的第一坐标位于所述第一基站坐标与第二基站坐标之间，根据第一基站坐标与所述第一距离计算出所述车辆的当前位置的第一坐标。由上述描述可知，通过第一距离、第二距离和第三距离的关系，能够判断能出车辆是否位于两个基站间的连线上，此时能够根据车辆与基站的距离以及基站的坐标更加快速地对车辆进行定位。进一步的，所述步骤S2还包括：判断所述第一距离以及所述第二距离与所述第三距离的差值是否都小于预设的距离误差值，若是则所述第一坐标为所述第一基站坐标。由上述描述可知，通过第一距离、第二距离和第三距离的关系，能够判断出车辆是否位于基站处，此时基站的坐标即车辆的坐标。进一步地，所述步骤S3具体为：根据车辆的所述第一坐标(x1，y1)以及预设时间间隔前的所在位置的第二坐标(x0，y0)，计算车辆行驶向量Vc＝(x1本文档来自技高网...

【技术保护点】
1.一种基于UWB阵列的定位导航方法，其特征在于，包括：/nS1、在预设的UWB阵列中选择预设的UWB基站作为原点坐标建立直角坐标系，并根据预设的各个所述UWB基站间的距离确定各个所述UWB基站的坐标，所述UWB阵列为由多个UWB基站架设的矩形阵列；/nS2、根据预设时间间隔实时获取车辆与各个所述UWB基站间的距离，并结合距离最近的四个所述UWB基站的坐标计算所述车辆的当前位置在所建立的所述直角坐标系中的第一坐标；/nS3、根据所述第一坐标以及所述车辆在预设时间间隔前的所在位置的第二坐标，计算所述车辆的航向角，所述航向角为所述车辆的行驶方向与所述直角坐标系的Y轴正向的夹角；/nS4、根据所述车辆的第一坐标和航向角在所述直角坐标系中对所述车辆进行定位和导航。/n

【技术特征摘要】
1.一种基于UWB阵列的定位导航方法，其特征在于，包括：
S1、在预设的UWB阵列中选择预设的UWB基站作为原点坐标建立直角坐标系，并根据预设的各个所述UWB基站间的距离确定各个所述UWB基站的坐标，所述UWB阵列为由多个UWB基站架设的矩形阵列；
S2、根据预设时间间隔实时获取车辆与各个所述UWB基站间的距离，并结合距离最近的四个所述UWB基站的坐标计算所述车辆的当前位置在所建立的所述直角坐标系中的第一坐标；
S3、根据所述第一坐标以及所述车辆在预设时间间隔前的所在位置的第二坐标，计算所述车辆的航向角，所述航向角为所述车辆的行驶方向与所述直角坐标系的Y轴正向的夹角；
S4、根据所述车辆的第一坐标和航向角在所述直角坐标系中对所述车辆进行定位和导航。


2.根据权利要求1所述的一种基于UWB阵列的定位导航方法，其特征在于，所述步骤S2具体为：
根据预设时间间隔获取车辆与各个所述UWB基站间的距离，获取距离最近的四个所述UWB基站的基站坐标，选择其中纵坐标小的两个所述基站坐标作为第一基站坐标和第二基站坐标，其中所述第一基站坐标的横坐标小于第二基站坐标的横坐标，将所述车辆与所述第一基站坐标的第一距离a、所述车辆与所述第二基站坐标的第二距离c、所述第一基站坐标和所述第二基站坐标间的第三距离b代入以下公式：
p＝(a+b+c)/2；
S＝√[p*(p-a)*(p-b)*(p-c)]
S＝h*b/2
a2＝h2+t2
计算出所述车辆的当前位置的第一坐标相对于第一基站坐标(x，y)的横向差值t，纵向差值h，从而得到所述第一坐标(x+t，y+h)。


3.根据权利要求2所述的一种基于UWB阵列的定位导航方法，其特征在于，所述步骤S2还包括：
所述第一距离与所述第二距离之和与所述第三距离的差值小于预设的距离误差值，则所述车辆当前位置的第一坐标位于所述第一基站坐标与第二基站坐标之间，根据第一基站坐标与所述第一距离计算出所述车辆的当前位置的第一坐标。


4.根据权利要求3所述的一种基于UWB阵列的定位导航方法，其特征在于，所述步骤S2还包括：
判断所述第一距离以及所述第二距离与所述第三距离的差值是否都小于预设的距离误差值，若是则所述第一坐标为所述第一基站坐标。


5.根据权利要求1所述的一种基于UWB阵列的定位导航方法，其特征在于，所述步骤S3具体为：
根据车辆的所述第一坐标(x1，y1)以及预设时间间隔前的所在位置的第二坐标(x0，y0)，计算车辆行驶向量Vc＝(x1-x0，y1-y0)；并由Y轴的正向向量Vy＝(0,1)根据以下公式：
θ＝arccos(Vc*Vy/(|Vc|*|Vy|)
计算出夹角θ，若x1-x0>0，则航向角为θ，若x1-x0<0，则航向角为π-θ，若x1-x0＝0且y1-y0>＝0，则航向角为0，若x1-x0＝0且y1-y0<0，则航向角为π。


6.一种基于UWB阵列的定位导航终端，其特征在于，包括处理器、存储器及存...

【专利技术属性】
技术研发人员：梁煜，林立言，
申请(专利权)人：福建盛海智能科技有限公司，
类型：发明
国别省市：福建;35