一种基于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/2

a²＝h²+t²

计算出所述车辆的当前位置的第一坐标相对于第一基站坐标(x，y)的横向差值t，纵向差值h，从而得到所述第一坐标(x+t，y+h)。

由上述描述可知，本申请通过获取车辆与各个所述uwb基站间的距离，选择最近的四个基站中的两个获取其坐标进行当前车辆位置坐标的计算，较之距离远的uwb基站，距离近的uwb基站于车辆间的距离能够更加准确，且因为基于uwb矩阵进行定位，每次车辆的定位坐标均位于四个uwb基站围成的矩形范围中，能够快速地确定车辆定位范围，并选择其中两个坐标从而组成三角形进而更方便快捷地进行计算出车辆的定位坐标，提高了车辆坐标获取的准确性和方便性。

进一步地，所述步骤s2还包括：

所述第一距离与所述第二距离之和与所述第三距离的差值小于预设的距离误差值，则所述车辆当前位置的第一坐标位于所述第一基站坐标与第二基站坐标之间，根据第一基站坐标与所述第一距离计算出所述车辆的当前位置的第一坐标。

由上述描述可知，通过第一距离、第二距离和第三距离的关系，能够判断能出车辆是否位于两个基站间的连线上，此时能够根据车辆与基站的距离以及基站的坐标更加快速地对车辆进行定位。

进一步的，所述步骤s2还包括：

判断所述第一距离以及所述第二距离与所述第三距离的差值是否都小于预设的距离误差值，若是则所述第一坐标为所述第一基站坐标。

由上述描述可知，通过第一距离、第二距离和第三距离的关系，能够判断出车辆是否位于基站处，此时基站的坐标即车辆的坐标。

进一步地，所述步骤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，则航向角为π。

由上述描述可知，以车辆行驶方向与y轴正方向的夹角作为航向角，能够形成很好的参照，同时便于计算，通过车辆两次定位坐标得到车辆的行驶向量从而能够依照y轴正向的向量计算出车辆的航向角，计算更加合理准确。

请参照图2，一种基于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/2

a²＝h²+t²

计算出所述车辆的当前位置的第一坐标相对于第一基站坐标(x，y)的横向差值t，纵向差值h，从而得到所述第一坐标(x+t，y+h)。

由上述描述可知，本申请通过获取车辆与各个所述uwb基站间的距离，选择最近的四个基站中的两个获取其坐标进行当前车辆位置坐标的计算，较之距离远的uwb基站，距离近的uwb基站于车辆间的距离能够更加准确，且因为基于uwb矩阵进行定位，每次车辆的定位坐标均位于四个uwb基站围成的矩形范围中，能够快速地确定车辆定位范围，并选择其中两个坐标从而组成三角形进而更方便快捷地进行计算出车辆的定位坐标，提高了车辆坐标获取的准确性和方便性。

进一步地，所述处理器执行所述计算机程序时，所述步骤s2还包括：

所述第一距离与所述第二距离之和与所述第三距离的差值小于预设的距离误差值，则所述车辆当前位置的第一坐标位于所述第一基站坐标与第二基站坐标之间，根据第一基站坐标与所述第一距离计算出所述车辆的当前位置的第一坐标。

由上述描述可知，通过第一距离、第二距离和第三距离的关系，能够判断能出车辆是否位于两个基站间的连线上，此时能够根据车辆与基站的距离以及基站的坐标更加快速地对车辆进行定位。

进一步的，所述处理器执行所述计算机程序时，所述步骤s2还包括：

判断所述第一距离以及所述第二距离与所述第三距离的差值是否都小于预设的距离误差值，若是则所述第一坐标为所述第一基站坐标。

由上述描述可知，通过第一距离、第二距离和第三距离的关系，能够判断出车辆是否位于基站处，此时基站的坐标即车辆的坐标。

进一步地，所述步骤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，则航向角为π。

由上述描述可知，以车辆行驶方向与y轴正方向的夹角作为航向角，能够形成很好的参照，同时便于计算，通过车辆两次定位坐标得到车辆的行驶向量从而能够依照y轴正向的向量计算出车辆的航向角，计算更加合理准确。

请参照图1和图3，本发明的实施例一为：

一种基于uwb阵列的定位导航方法，包括：

s1、在预设的uwb阵列中选择预设的uwb基站作为原点坐标建立直角坐标系，并根据预设的各个所述uwb基站间的距离确定各个所述uwb基站的坐标，所述uwb阵列为由多个uwb基站架设的矩形阵列；

本实施例中，如图3所示，预先通过9个uwb基站在预设区域建立了3*3的uwb阵列，基站间的间距为b(b>0)，以其中一个基站的位置为原点建立直角坐标系，计算出各个基站的基站坐标。

s2、根据预设时间间隔实时获取车辆与各个所述uwb基站间的距离，并结合距离最近的四个所述uwb基站的坐标计算所述车辆的当前位置在所建立的所述直角坐标系中的第一坐标；

所述步骤s2具体为：

根据预设时间间隔获取车辆与各个所述uwb基站间的距离，获取距离最近的四个所述uwb基站的基站坐标，选择其中纵坐标小的两个所述基站坐标作为第一基站坐标和第二基站坐标，其中所述第一基站坐标的横坐标小于第二基站坐标的横坐标，将所述车辆与所述第一基站坐标的第一距离a、所述车辆与所述第二基站坐标的第二距离c、所述第一基站坐标和所述第二基站坐标间的第三距离b代入以下公式：

p＝(a+b+c)/2；

s＝√[p*(p-a)*(p-b)*(p-c)]

s＝h*b/2

a²＝h²+t²

计算出所述车辆的当前位置的第一坐标相对于第一基站坐标(x，y)的横向差值t，纵向差值h，从而得到所述第一坐标(x+t，y+h)。

此外，当所述第一距离与所述第二距离之和与所述第三距离的差值小于预设的距离误差值，则所述车辆当前位置的第一坐标位于所述第一基站坐标与第二基站坐标之间，根据第一基站坐标与所述第一距离计算出所述车辆的当前位置的第一坐标；

当所述第一距离以及所述第二距离与所述第三距离的差值是否都小于预设的距离误差值，即认为车辆位于第一基站的位置，所述第一坐标为所述第一基站坐标；

本实施例中，如图3所示，四个基站坐标为(0，0)、(b，0)、(b，0)和(b，b)，可知车辆位于以上四个基站围成的矩形区域中，以(0，0)为第一基站坐标，(b，0)为第二基站坐标，根据所述车辆与所述第一基站坐标的第一距离a、所述车辆与所述第二基站坐标的第二距离c、第一基站坐标和第二基站坐标间的第三距离b，代入公式：

p＝(a+b+c)/2；

s＝√[p*(p-a)*(p-b)*(p-c)]；

s＝h*b/2

a²＝h²+t²

进而可以计算出：

h＝2s/b；

t＝√(a²-h²)；

得到车辆相对第一基站坐标(0，0)的横坐标差值t和纵坐标差值h，可以得到车辆的第一坐标为(t，h)。

s3、根据所述第一坐标以及所述车辆在预设时间间隔前的所在位置的第二坐标，计算所述车辆的航向角，所述航向角为所述车辆的行驶方向与所述直角坐标系的y轴正向的夹角；

所述步骤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，则航向角为π。

本实施例中，车辆的第一坐标为(t，h)，预设时间间隔前的第二坐标为(0，0)，因此车辆形式向量vc＝(t，h)，与vy＝(0，1)代入公式：

θ＝arccos(vc*vy/(|vc|*|vy|)

可得θ＝arccos[h/√(h²+t²)]＝arccos(h/a)。

s4、根据所述车辆的第一坐标和航向角在所述直角坐标系中对所述车辆进行定位和导航。

请参照图2，本发明的实施例二为：

一种基于uwb阵列的定位导航终端1，包括处理器2、存储器3及存储在存储器3上并可在处理器2上运行的计算机程序，所述处理器2执行所述计算机程序时实现以上实施例一中步骤。

综上所述，本发明提供的一种基于uwb阵列的定位导航方法与终端，本发明通过多个uwb基站架设成矩形阵列，并以此建立直角坐标系，根据车载的uwb模块与uwb基站间的距离，以及基站间的固定间距，从而实现对车辆实时的快速定位，同时定位基于距离最近的四个uwb基站，能够使定位更加准确，也能够实时获取车辆的航向角，实现不需要依靠gps或北斗的定位导航，且uwb模块的带宽大，抗干扰性强，信号更加稳定。

以上所述仅为本发明的实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等同变换，或直接或间接运用在相关的技术领域，均同理包括在本发明的专利保护范围内。