[发明专利]一种基于UWB阵列的定位导航方法与终端

权利要求书

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

a²＝h²+t²

计算出所述车辆的当前位置的第一坐标相对于第一基站坐标(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-x00，则航向角为θ，若x1-x00，则航向角为π-θ，若x1-x0＝0且y1-y0＝0，则航向角为0，若x1-x0＝0且y1-y00，则航向角为π。

6.一种基于UWB阵列的定位导航终端，其特征在于，包括处理器、存储器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现以下步骤：

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

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

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

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