CN108437972B - 一种基于位置偏差的轨迹跟踪方法和装置 - Google Patents

Abstract

基于位置偏差的轨迹跟踪方法和装置，应用于车路协同环境下设置有路侧设备的轨迹跟踪区域，包括：与路侧设备建立通信连接时，从建立通信连接的路侧设备上获取车辆当前所在区域的局部导航信息；根据车辆的位置信息和获得的局部导航信息确定车辆当前的位置偏差。本发明实施例通过路侧设备获取局部导航信息，提升了车辆轨迹跟踪的实时性；进一步的，基于实时的位置偏差进行方向盘转角的计算，提高了车辆轨迹跟踪的准确性。

Description

一种基于位置偏差的轨迹跟踪方法和装置

技术领域

本文涉及但不限于车路协同环境下的自动驾驶技术，尤指一种基于位置偏差的轨迹跟踪方法和装置。

背景技术

车辆的轨迹跟踪和控制是实现自动驾驶功能的重要组成部分。

目前，车辆的轨迹跟踪和控制包括基于高精度地图和高精度车载传感器实现的车辆轨迹的跟踪和控制；上述方案进行轨迹跟踪和控制时，需要获取全局的交通数据，信息量大、传输时间长、影响轨迹跟踪的实时性；由于实时性受到影响，用于车辆控制的方向盘转角的准确度也受到影响；此外，上述方案运算精度过度依赖车载传感器的精度，而高精度车载传感器成本过高，无法在单体车辆上进行推广，实用性较差。

发明内容

以下是对本文详细描述的主题的概述。本概述并非是为了限制权利要求的保护范围。

本发明实施例提供一种基于位置偏差的轨迹跟踪方法和装置，能够提升车辆轨迹跟踪的实时性和车辆控制的准确性。

本发明实施例提供了一种基于位置偏差的轨迹跟踪方法，应用于设置有路侧设备的轨迹跟踪区域，包括：

与路侧设备建立通信连接时，从建立通信连接的路侧设备上获取车辆当前所在区域的局部导航信息；

根据车辆的位置信息和获得的局部导航信息确定车辆当前的位置偏差。

可选的，所述局部导航信息包括：

由当前建立通信连接的所述路侧设备生成的：基于在所述轨迹跟踪区域预先设置的道路坐标点，以所述车辆当前所在区域内包含的所有道路坐标点作为组成元素，构建生成的坐标序列。

可选的，所述根据车辆的位置信息和获得的局部导航信息确定车辆当前的位置偏差包括：

根据车辆的所述位置信息及所述局部导航信息，根据预瞄距离d确定位置偏差相关信息；

根据确定的位置偏差相关信息和所述位置信息确定车辆当前的位置偏差。

可选的，所述根据车辆的位置信息和获得的局部导航信息确定车辆当前的位置偏差包括：

根据所述位置信息确定沿车辆运行方向与车身中心距离为预瞄距离d的参考点P(P_x,P_y)；确定所述局部导航信息中距离所述车身中心最近的道路坐标点为就近路点A(A_x,A_y)；在所述局部导航信息中，确定沿车辆运行方向与所述就近路点的距离最接近所述预瞄距离d的道路坐标点为预瞄点B(B_x,B_y)；

根据车辆的所述位置信息、确定的所述参考点P(P_x,P_y)和所述预瞄点B(B_x,B_y)，计算确定车辆当前的所述位置偏差。

可选的，所述参考点P(P_x,P_y)的坐标通过以下公式计算获得：

P_x＝V_x+d·sin(θ·π/180)；P_y＝V_y+d·cos(θ·π/180)；

其中，所述V_x、V_y和θ分别为所述位置信息中包含的横坐标、纵坐标和航向角。

可选的，所述位置偏差根据以下公式计算获得：

位置偏差；

其中，

为预瞄点B到参考点P的向量；

为位置坐标V到参考点P的向量；

为参考点P到预瞄点B的向量；所述位置坐标V为由所述位置信息中包含的车辆位置的坐标信息。

可选的，所述确定车辆的位置偏差后，所述轨迹跟踪方法还包括：

根据当前的位置偏差和前一次获得的位置偏差，确定进行车辆控制的控制信息。

可选的，所述确定车辆的位置偏差后，所述轨迹跟踪方法还包括：

将当前获得的所述位置偏差和在前一次获得的所述位置偏差作为模糊比例微分PD控制器的输入；

由模糊PD控制器根据下述公式计算输出进行车辆控制的方向盘转角：

方向盘转角＝K_P·E(1)+K_d·(E(1)-E(2))；

其中，参数K_p和参数K_d的隶属度函数通过高斯函数确定；E(1)-E(2)表示位置偏差率。

可选的，所述确定车辆的位置偏差之前，所述轨迹跟踪方法还包括：

与路侧设备建立通信连接时，从建立通信连接的路侧设备上获取车辆的所述位置信息。

另一方面，本发明实施例还提供一种基于位置偏差的轨迹跟踪装置，应用于设置有路侧设备的轨迹跟踪区域，包括：获取单元和确定偏差单元；其中，

获取单元用于：与路侧设备建立通信连接时，从建立通信连接的路侧设备上获取车辆当前所在区域的局部导航信息；

确定偏差单元用于：根据车辆的位置信息和获得的局部导航信息确定车辆当前的位置偏差。

可选的，所述局部导航信息包括：

由当前建立通信连接的所述路侧设备生成的：基于在所述轨迹跟踪区域预先设置的道路坐标点，以所述车辆当前所在区域内包含的所有道路坐标点作为组成元素，构建生成的坐标序列。

可选的，所述确定偏差单元具体用于：

根据车辆的所述位置信息及所述局部导航信息，根据预瞄距离d确定位置偏差相关信息；

根据确定的位置偏差相关信息和所述位置信息确定车辆当前的位置偏差。

可选的，所述确定偏差单元具体用于：

根据所述位置信息确定沿车辆运行方向与车身中心距离为预瞄距离d的参考点P(P_x,P_y)；确定所述局部导航信息中距离所述车身中心最近的道路坐标点为就近路点A(A_x,A_y)；在所述局部导航信息中，确定沿车辆运行方向与所述就近路点的距离最接近所述预瞄距离d的道路坐标点为预瞄点B(B_x,B_y)；

根据车辆的所述位置信息、确定的所述参考点P(P_x,P_y)和所述预瞄点B(B_x,B_y)，计算确定车辆当前的所述位置偏差。

可选的，所述确定偏差单元通过以下公式计算获得所述参考点P(P_x,P_y)的坐标：

P_x＝V_x+d·sin(θ·π/180)；P_y＝V_y+d·cos(θ·π/180)；

其中，所述V_x、V_y和θ分别为所述位置信息中包含的横坐标、纵坐标和航向角。

可选的，所述确定偏差单元通过以下公式计算获得所述位置偏差：

位置偏差；

其中，

为预瞄点B到参考点P的向量；

为位置坐标V到参考点P的向量；

为参考点P到预瞄点B的向量；所述位置坐标V为由所述位置信息中包含的车辆位置的坐标信息。

可选的，所述轨迹跟踪装置还包括控制单元，用于：

根据当前的位置偏差和前一次获得的位置偏差，确定进行车辆控制的控制信息。

可选的，所述控制单元具体用于：

将当前获得的所述位置偏差和在前一次获得的所述位置偏差作为模糊比例微分PD控制器的输入；

由模糊PD控制器根据下述公式计算输出进行车辆控制的方向盘转角：

方向盘转角＝K_p·E(1)+K_d·(E(1)-E(2))；

其中，参数K_p和参数K_d的隶属度函数通过高斯函数确定；E(1)-E(2)表示位置偏差率。

可选的，所述获取单元还用于：

与路侧设备建立通信连接时，从建立通信连接的路侧设备上获取车辆的所述位置信息。

再一方面，本发明实施例还提供一种计算机存储介质，所述计算机存储介质中存储有计算机可执行指令，所述计算机可执行指令用于执行上述轨迹跟踪方法。

还一方面，本发明实施例还提供一种终端，包括：存储器和处理器；其中，

处理器被配置为执行存储器中的程序指令；

程序指令在处理器读取执行以下操作：

与路侧设备建立通信连接时，从建立通信连接的路侧设备上获取车辆当前所在区域的局部导航信息；

根据车辆的位置信息和获得的局部导航信息确定车辆当前的位置偏差。

还一方面，本发明实施例还提供一种基于位置偏差的轨迹跟踪方法，包括：

与车辆建立通信连接；

与车辆建立通信连接时，向车辆发送车辆当前所在区域的局部导航信息。

可选的，所述向车辆发送车辆当前所在区域的局部导航信息之前，所述轨迹跟踪方法还包括：

基于在所述轨迹跟踪区域预先设置的道路坐标点，以所述车辆当前所在区域内包含的所有道路坐标点作为组成元素，构建生成作为所述局部导航信息的坐标序列。

还一方面，本发明实施例还提供一种基于位置偏差的轨迹跟踪装置，包括：连接单元和发送单元；其中，

连接单元用于：与车辆建立通信连接；

发送单元用于：与车辆建立通信连接时，向车辆发送车辆当前所在区域的局部导航信息。

可选的，所述轨迹跟踪装置还包括生成单元，用于：

以所述车辆当前所在区域内包含的所有预先设置的道路坐标点作为导航的基础坐标，构建生成的路点坐标序列。

还一方面，本发明实施例还提供一种计算机存储介质，所述计算机存储介质中存储有计算机可执行指令，所述计算机可执行指令用于执行上述轨迹跟踪方法。

还一方面，本发明实施例还提供一种终端，包括：存储器和处理器；其中，

处理器被配置为执行存储器中的程序指令；

程序指令在处理器读取执行以下操作：

与车辆建立通信连接；

与车辆建立通信连接时，向车辆发送车辆当前所在区域的局部导航信息。

与相关技术相比，本申请技术方案应用于设置有路侧设备的轨迹跟踪区域，包括：与路侧设备建立通信连接时，从建立通信连接的路侧设备上获取车辆当前所在区域的局部导航信息；根据车辆的位置信息和获得的局部导航信息确定车辆当前的位置偏差。本发明实施例通过路侧设备获取局部导航信息，提升了车辆轨迹跟踪的实时性；进一步的，基于实时的位置偏差进行方向盘转角的计算，提高了车辆轨迹跟踪的准确性。

本发明的其它特征和优点将在随后的说明书中阐述，并且，部分地从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点可通过在说明书、权利要求书以及附图中所特别指出的结构来实现和获得。

附图说明

附图用来提供对本发明技术方案的进一步理解，并且构成说明书的一部分，与本申请的实施例一起用于解释本发明的技术方案，并不构成对本发明技术方案的限制。

图1为本发明实施例基于位置偏差的轨迹跟踪方法的流程图；

图2为本发明实施例基于位置偏差的轨迹跟踪装置的结构框图；

图3为本发明另一实施例基于位置偏差的轨迹跟踪方法的流程图；

图4为本发明另一实施例基于位置偏差的轨迹跟踪装置的结构框图；

图5为本发明应用示例的方法流程图；

图6为本发明应用示例车辆行驶过程中位置偏差的示意图；

图7为本发明应用示例车辆行驶过程中另一位置偏差的示意图；

图8为本发明应用示例轨迹跟踪效果示意图；

图9为本发明应用示例跟踪误差的示意图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明白，下文中将结合附图对本发明的实施例进行详细说明。需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互任意组合。

在附图的流程图示出的步骤可以在诸如一组计算机可执行指令的计算机系统中执行。并且，虽然在流程图中示出了逻辑顺序，但是在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤。

本发明实施例轨迹跟踪方法可以应用于车路协同环境下设置有一个或一个以上路侧设备的轨迹跟踪区域；图1为本发明实施例基于位置偏差的轨迹跟踪方法的流程图，如图1所示，包括：

步骤101、与路侧设备建立通信连接时，从建立通信连接的路侧设备上获取车辆当前所在区域的局部导航信息；

需要说明的是，本发明实施例上述车辆包括：进行自动驾驶的车辆；路侧设备可以包括参照智能车路协同系统(i-VICS，Intelligent Vehicle InfrastructureCooperation System)体系架构，设置在车辆运行环境中的设备；其中，车路协同系统采用先进的无线通信和新一代互联网等技术，可以实现车车、车路动态实时信息的交互；在动态交通信息采集与融合的基础上开展车路协同安全和道路主动控制，能够充分实现人、车、路的有效协同，保证交通安全，提高通行效率，构建具备安全、高效和环保的道路交通系统。

可选的，本发明实施例局部导航信息包括：

由当前建立通信连接的所述路侧设备生成的：基于在所述轨迹跟踪区域预先设置的道路坐标点，以所述车辆当前所在区域内包含的所有道路坐标点作为组成元素，构建生成的坐标序列。

需要说明的是，本发明实施例路侧设备之间的间距一般为几百米，因此，每一次与车辆建立通信连接的路侧设备一般只有一个；与路侧设备建立通信连接时，可以及时的从路侧设备获取局部导航信息；车辆当前所在区域可以由本领域技术人员根据路侧设备的间距和分布进行设定；例如、设定车辆运行前方的两个路点到车辆运行后方的一个路点所覆盖的区域为车辆当前所在区域；本发明实施例轨迹跟踪方法可以应用在车路协同环境下设置有多个路侧设备的轨迹跟踪区域，路侧设备的设置分布可以由本领域技术人员进行分析确定。

本发明实施例路车设备可以采用包括：专用短程通信技术(DSRC，DedicatedShort Range Communications)、第四代移动通信技术(4G)、车联网(LTE-V)等通信方式实现信息交互。

本发明实施例通过路侧设备实现了车路动态实时信息交互，实现了车路之间的有效协同，提升了自动驾驶过程的通信效率，为安全、高效的道路交通提供技术基础。

步骤102、根据车辆的位置信息和获得的局部导航信息确定车辆当前的位置偏差。

可选的，本发明实施例根据车辆的位置信息和获得的局部导航信息确定车辆当前的位置偏差包括：

根据车辆的所述位置信息及所述局部导航信息，根据预瞄距离d确定位置偏差相关信息；

根据确定的位置偏差相关信息和所述位置信息确定车辆当前的位置偏差。

可选的，本发明实施例根据车辆的位置信息和获得的局部导航信息确定车辆当前的位置偏差包括：

根据所述位置信息确定沿车辆运行方向与车身中心距离为预瞄距离d的参考点P(P_x,P_y)；确定所述局部导航信息中距离所述车身中心最近的道路坐标点为就近路点A(A_x,A_y)；在所述局部导航信息中，确定沿车辆运行方向与所述就近路点的距离最接近所述预瞄距离d的道路坐标点为预瞄点B(B_x,B_y)；

根据车辆的位置信息、确定的所述参考点P(P_x,P_y)和所述预瞄点B(B_x,B_y)，计算确定车辆当前的所述位置偏差。

可选的，本发明实施例参考点P(P_x,P_y)的坐标通过以下公式计算获得：

P_x＝V_x+d·sin(θ·π/180)；P_y＝V_y+d·cos(θ·π/180)；

其中，V_x、V_y和θ分别为所述位置信息中包含的横坐标、纵坐标和航向角。

可选的，本发明实施例位置偏差根据以下公式计算获得：

位置偏差；

其中，

为预瞄点B到参考点P的向量；

为位置坐标V到参考点P的向量；

为参考点P到预瞄点B的向量；所述位置坐标V为由所述位置信息中包含的车辆位置的坐标信息。可选的，确定车辆的位置偏差后，本发明实施例轨迹跟踪方法还包括：

根据当前的位置偏差和前一次获得的位置偏差，确定进行车辆控制的控制信息。

可选的，确定车辆的位置偏差后，本发明实施例轨迹跟踪方法还包括：

将当前获得的位置偏差和在前一次获得的位置偏差作为模糊比例微分(PD)控制器的输入；

由模糊PD控制器根据下述公式计算输出进行车辆控制的方向盘转角：

方向盘转角＝K_p·E(1)+K_d·(E(1)-E(2))；

其中，参数K_p和参数K_d的隶属度函数通过高斯函数确定；E(1)-E(2)表示位置偏差率。

需要说明的是，本发明实施例上述模糊PD控制器包含PD控制器和模糊控制器；其中，PD控制器根据E(1)和E(1)-E(2)的值，在设定合适的参数K_p和参数K_d时计算方向盘转角；而在运算时，设定合适的参数K_p和参数K_d比较困难，因此采用模糊控制器来动态调节参数K_p和参数K_d。本发明实施例模糊控制器的输入也是E(1)和E(1)-E(2)的值，输出为参数K_p和参数K_d的值。模糊PD控制器只是本发明用于计算输出方向盘转角的一个可选实施例，本发明实施例可以采用其他方法基于位置偏差计算获得方向盘转角。

另外，本发明实施例方向盘转角信息可以通过车内局域网络(CAN)总线进行接收，通过接收的方向盘转角信息可以实现车辆的控制。

可选的，所述确定车辆的位置偏差之前，本发明实施例轨迹跟踪方法还包括：

与路侧设备建立通信连接时，从建立通信连接的路侧设备上获取车辆的所述位置信息。

需要说明的是，本发明实施例通过路侧设备提高了车辆的定位的实时性和定位精度；本发明实施例也可以采用其他相关技术确定车辆的位置信息。

本发明实施例在不依赖高精度传感器的基础上，实现了车辆运行轨迹的实时获取，提升了车辆轨迹跟踪的准确性。

与相关技术相比，本申请技术方案应用于设置有路侧设备的轨迹跟踪区域，包括：与路侧设备建立通信连接时，从建立通信连接的路侧设备上获取车辆当前所在区域的局部导航信息；根据车辆的位置信息和获得的局部导航信息确定车辆当前的位置偏差。本发明实施例通过路侧设备获取局部导航信息，提升了车辆轨迹跟踪的实时性；进一步的，基于实时的位置偏差进行方向盘转角的计算，提高了车辆轨迹跟踪的准确性。

图2为本发明实施例基于位置偏差的轨迹跟踪装置的结构框图，如图2所示，包括：获取单元和确定偏差单元；其中，

获取单元用于：与路侧设备建立通信连接时，从建立通信连接的路侧设备上获取车辆当前所在区域的局部导航信息；

可选的，本发明实施例局部导航信息包括：

由当前建立通信连接的所述路侧设备生成的：基于在所述轨迹跟踪区域预先设置的道路坐标点，以所述车辆当前所在区域内包含的所有道路坐标点作为组成元素，构建生成的坐标序列。

确定偏差单元用于：根据车辆的位置信息和获得的局部导航信息确定车辆当前的位置偏差。

本发明实施例轨迹跟踪装置可以应用于设置有路侧设备的轨迹跟踪区域。

需要说明的是，本发明实施例上述车辆包括：进行自动驾驶的车辆；路侧设备可以包括参照车路协同(V2X)技术，设置在车辆运行环境中的设备。本发明实施例轨迹跟踪装置可以内置于自动驾驶的车辆上，也可以通过有线或无线的方式与自动驾驶车辆进行通信连接。

可选的，本发明实施例确定偏差单元具体用于：

根据车辆的所述位置信息及所述局部导航信息，根据预瞄距离d确定位置偏差相关信息；

根据确定的位置偏差相关信息和所述位置信息确定车辆当前的位置偏差。

可选的，本发明实施例确定偏差单元具体用于：

根据所述位置信息确定沿车辆运行方向与车身中心距离为预瞄距离d的参考点P(P_x,P_y)；确定所述局部导航信息中距离所述车身中心最近的道路坐标点为就近路点A(A_x,A_y)；在所述局部导航信息中，确定沿车辆运行方向与所述就近路点的距离最接近所述预瞄距离d的道路坐标点为预瞄点B(B_x,B_y)；

根据车辆的所述位置信息、确定的所述参考点P(P_x,P_y)和所述预瞄点B(B_x,B_y)，计算确定车辆当前的所述位置偏差。

本发明实施例参考点P(P_x,P_y)和预瞄点B(B_x,B_y)为根据预瞄距离d确定的位置偏差相关信息。

可选的，所述确定偏差单元通过以下公式计算获得所述参考点P(P_x,P_y)的坐标：

P_x＝V_x+d·sin(θ·π/180)；P_y＝V_y+d·cos(θ·π/180)；

其中，所述V_x、V_y和θ分别为所述位置信息中包含的横坐标、纵坐标和航向角。

可选的，所述确定偏差单元通过以下公式计算获得所述位置偏差：

位置偏差

其中，

为预瞄点B到参考点P的向量；

为位置坐标V到参考点P的向量；

为参考点P到预瞄点B的向量；所述位置坐标V为由所述位置信息中包含的车辆位置的坐标信息。可选的，本发明实施例轨迹跟踪装置还包括控制单元，用于：

根据当前的位置偏差和前一次获得的位置偏差，确定进行车辆控制的控制信息。

可选的，本发明实施例所述控制单元具体用于：

将当前获得的所述位置偏差和在前一次获得的所述位置偏差作为模糊比例微分PD控制器的输入；

由模糊PD控制器根据下述公式计算输出进行车辆控制的方向盘转角：

方向盘转角＝K_p·E(1)+K_d·(E(1)-E(2))；

其中，参数Kp和参数Kd的隶属度函数通过高斯函数确定；E(1)-E(2)表示位置偏差率。

可选的，本发明实施例获取单元还用于：与路侧设备建立通信连接时，获取车辆的所述位置信息。

本发明实施例还提供一种计算机存储介质，所述计算机存储介质中存储有计算机可执行指令，所述计算机可执行指令用于执行上述基于位置偏差的轨迹跟踪方法。

本发明实施例还提供一种终端，包括：存储器和处理器；其中，

处理器被配置为执行存储器中的程序指令；

程序指令在处理器读取执行以下操作：

与路侧设备建立通信连接时，从建立通信连接的路侧设备上获取车辆当前所在区域的局部导航信息；

根据车辆的位置信息和获得的局部导航信息确定车辆当前的位置偏差。

图3为本发明另一实施例基于位置偏差的轨迹跟踪方法的流程图，如图3所示，包括：

步骤301、与车辆建立通信连接；

步骤302、与车辆建立通信连接时，向车辆发送车辆当前所在区域的局部导航信息。

需要说明的是，本发明实施例局部导航信息包含用于确定车辆当前的位置偏差的道路点坐标。

可选的，向车辆发送车辆当前所在区域的局部导航信息之前，本发明实施例轨迹跟踪方法还包括：

基于在所述轨迹跟踪区域预先设置的道路坐标点，以所述车辆当前所在区域内包含的所有道路坐标点作为组成元素，构建生成作为所述局部导航信息的坐标序列。

图4为本发明另一实施例基于位置偏差的轨迹跟踪装置的结构框图，如图4所示，包括：连接单元和发送单元；其中，

连接单元用于：与车辆建立通信连接；

发送单元用于：与车辆建立通信连接时，向车辆发送车辆当前所在区域的局部导航信息。

可选的，本发明实施例轨迹跟踪装置还包括生成单元，用于：

以所述车辆当前所在区域内包含的所有预先设置的道路坐标点作为导航的基础坐标，构建生成的路点坐标序列。

本发明实施例上述轨迹跟踪装置可以设置于路侧设备，也可以通过相关技术中的通信方式实现路侧设备与轨迹跟踪装置的连接。

本发明实施例还提供一种计算机存储介质，所述计算机存储介质中存储有计算机可执行指令，所述计算机可执行指令用于执行上述轨迹跟踪方法。

本发明实施例还提供一种终端，包括：存储器和处理器；其中，

处理器被配置为执行存储器中的程序指令；

程序指令在处理器读取执行以下操作：

与车辆建立通信连接；

与车辆建立通信连接时，向车辆发送车辆当前所在区域的局部导航信息。

以下通过应用示例对本发明实施例方法进行清楚详细的说明，应用示例仅用于陈述本发明，并不用于限定本发明的保护范围。

应用示例

本应用示例以具备自动驾驶功能的车辆为例，对车辆进行90度弯道轨迹的跟踪控制进行说明。本应用示例车辆与路侧设备均设置有保证车辆和路侧设备实时通信的车与网(V2I)通信设备和实现车辆定位的定位系统(例如、GPS定位系统)；

本应用示例车辆的控制器通过车内局域网络(CAN)总线接收采用本发明实施例计算获得的方向盘转角度数信息，通过接收到的方向盘转角度数信息可以实现对车辆的转向控制；

图5为本发明应用示例的方法流程图，如图5所示，包括：

步骤501、车辆向路侧设备发送目的地址；

步骤502、接收到目的地址的路侧设备，基于初始地址到目的地址生成车辆当前所在区域的局部导航信息；

这里，初始地址可以是当前地址，也可以是预先输入的地址。

本应用示例可以采用一个n×3的数组来存放局部导航信息，即导航信息中的n个道路坐标点的坐标信息；

步骤503、车辆从建立通信连接的路侧设备获取局部导航信息；

步骤504、车辆根据自身位置的位置信息和获得的局部导航信息确定车辆的位置偏差。

本应用示例确定车辆的位置偏差包括：

根据所述位置信息确定沿车辆运行方向与车身中心距离为预瞄距离d的参考点P(P_x,P_y)；确定所述局部导航信息中距离所述车身中心最近的路点为就近路点A(A_x,A_y)；在所述局部导航信息中，确定沿车辆运行方向与所述就近路点的距离最接近所述预瞄距离d的路点为预瞄点B(B_x,B_y)；其中，预瞄距离可以由本领域技术人员经验设定为固定值或动态值。

根据确定的所述参考点P(P_x,P_y)、所述就近路点A(A_x,A_y)和所述预瞄点B(B_x,B_y)，计算获得所述位置偏差。

本应用示例预瞄距离d可以由本领域技术人员经验设定为固定值或动态值。

本应用示例，参考点P(P_x,P_y)的坐标通过以下公式计算获得：

P_x＝V_x+d·sin(θ·π/180)；P_y＝V_y+d·cos(θ·π/180)；

其中，V_x、V_y和θ分别为所述位置信息中包含的横坐标、纵坐标和航向角。

图6为本发明应用示例车辆行驶过程中位置偏差的示意图，在图6中显示出了参考点P(P_x,P_y)、就近路点A(A_x,A_y)和预瞄点B(B_x,B_y)的相对位置，E为位置偏差；图7为本发明应用示例车辆行驶过程中另一位置偏差的示意图，在图7中显示出了车辆移动后参考点P(P_x,P_y)、就近路点A(A_x,A_y)和预瞄点B(B_x,B_y)的相对位置，E为位置偏差；

本发明应用示例B点与P点的距离即为位置偏差的大小，偏差的方向可由

确定，

为正时，车辆需要左转，偏差为正；

为负时，车辆需要右转，偏差为负；

步骤505、根据位置偏差E计算偏差变化率，将位置偏差和计算获得的偏差变化率EC作为模糊PD控制器的输入；

本发明应用示例模糊PD控制器中位置偏差、偏差变化率、Kp参数以及Kd参数的隶属度函数均可以采用高斯函数来确定；本发明实施例可以选择7种语言变量，即PL(正大)、PM(正中)、PS(正小)、ZO(零)、NS(负小)、NM(负中)、NL(负大)，设置模糊逻辑规则，模糊PD控制器动态输出PD参数；

步骤506、模糊PD控制器计算输出方向盘转角；具体的，可以根据下述公式计算输出进行车辆控制的方向盘转角：

方向盘转角＝K_p·E(1)+K_d·(E(1)-E(2))；

其中，参数Kp和参数Kd的隶属度函数通过高斯函数确定；E(1)-E(2)表示位置偏差率。

步骤507、根据方向盘转角进行车辆的控制。

较相关技术使用的高精度传感器；本发明应用示例采用基于V2I技术的通讯设备实现了轨迹跟踪和控制；相对相关技术，在降低成本的同时，通过路侧设备提升了轨迹跟踪的实时性。图8为本发明应用示例轨迹跟踪效果示意图，如图8所示，对于90度、转弯半径约12m的弯道路段，本发明应用示例自动驾驶车辆真实行驶轨迹(通过实线表示)与参考轨迹(通过虚线表示)重合度达到较佳状态理想。依据车辆的真实行驶轨迹和参考轨迹，本应用示例可计算出车辆在各个时刻的跟踪误差，图9为本发明应用示例跟踪误差的示意图，如图9所示，本发明应用示例跟踪误差较小，满足自动驾驶车辆的轨迹跟踪和控制要求。

本领域普通技术人员可以理解上述方法中的全部或部分步骤可通过程序来指令相关硬件(例如处理器)完成，所述程序可以存储于计算机可读存储介质中，如只读存储器、磁盘或光盘等。可选地，上述实施例的全部或部分步骤也可以使用一个或多个集成电路来实现。相应地，上述实施例中的每个模块/单元可以采用硬件的形式实现，例如通过集成电路来实现其相应功能，也可以采用软件功能模块的形式实现，例如通过处理器执行存储于存储器中的程序/指令来实现其相应功能。本发明不限制于任何特定形式的硬件和软件的结合。

虽然本发明所揭露的实施方式如上，但所述的内容仅为便于理解本发明而采用的实施方式，并非用以限定本发明。任何本发明所属领域内的技术人员，在不脱离本发明所揭露的精神和范围的前提下，可以在实施的形式及细节上进行任何的修改与变化，但本发明的专利保护范围，仍须以所附的权利要求书所界定的范围为准。

Claims (20)

1.一种基于位置偏差的轨迹跟踪方法，应用于设置有路侧设备的轨迹跟踪区域，其特征在于，包括：

与路侧设备建立通信连接时，从建立通信连接的路侧设备上获取车辆当前所在区域的局部导航信息；

根据车辆的位置信息和获得的局部导航信息确定车辆当前的位置偏差；

其中，所述位置偏差根据以下公式计算获得：

位置偏差

所述P为根据所述位置信息确定的沿车辆运行方向与车身中心距离为预瞄距离d的参考点；所述B为所述局部导航信息中，确定沿车辆运行方向与就近路点A的距离最接近所述预瞄距离d的道路坐标点；A为所述局部导航信息中距离车身中心最近的道路坐标点；位置坐标V为由所述位置信息中包含的车辆位置的坐标信息；

为预瞄点B到参考点P的向量；

为位置坐标V到参考点P的向量；

为参考点P到预瞄点B的向量。

2.根据权利要求1所述的轨迹跟踪方法，其特征在于，所述局部导航信息包括：

由当前建立通信连接的所述路侧设备生成的：基于在所述轨迹跟踪区域预先设置的道路坐标点，以所述车辆当前所在区域内包含的所有道路坐标点作为组成元素，构建生成的坐标序列。

3.根据权利要求2所述的轨迹跟踪方法，其特征在于，所述参考点P(P_x,P_y)的坐标通过以下公式计算获得：

P_x＝V_x+d·sin(θ·π/180)；P_y＝V_y+d·cos(θ·π/180)；

其中，所述V_x、V_y和θ分别为所述位置信息中包含的横坐标、纵坐标和航向角。

4.根据权利要求1～3任一项所述的轨迹跟踪方法，其特征在于，所述确定车辆的位置偏差后，所述轨迹跟踪方法还包括：

根据当前的位置偏差和前一次获得的位置偏差，确定进行车辆控制的控制信息。

5.根据权利要求4所述的轨迹跟踪方法，其特征在于，所述确定车辆的位置偏差后，所述轨迹跟踪方法还包括：

将当前获得的所述位置偏差和在前一次获得的所述位置偏差作为模糊比例微分PD控制器的输入；

由模糊PD控制器根据下述公式计算输出进行车辆控制的方向盘转角：

方向盘转角＝K_p·E(1)+K_d·(E(1)-E(2))；

其中，参数K_p和参数K_d的隶属度函数通过高斯函数确定；E(1)-E(2)表示位置偏差率。

6.根据权利要求1～3任一项所述的轨迹跟踪方法，其特征在于，所述确定车辆的位置偏差之前，所述轨迹跟踪方法还包括：

与路侧设备建立通信连接时，从建立通信连接的路侧设备上获取车辆的所述位置信息。

7.一种基于位置偏差的轨迹跟踪装置，应用于设置有路侧设备的轨迹跟踪区域，其特征在于，包括：获取单元和确定偏差单元；其中，

获取单元用于：与路侧设备建立通信连接时，从建立通信连接的路侧设备上获取车辆当前所在区域的局部导航信息；

确定偏差单元用于：根据车辆的位置信息和获得的局部导航信息确定车辆当前的位置偏差；

其中，所述位置偏差根据以下公式计算获得：

位置偏差

所述P为根据所述位置信息确定的沿车辆运行方向与车身中心距离为预瞄距离d的参考点；所述B为所述局部导航信息中，确定沿车辆运行方向与就近路点A的距离最接近所述预瞄距离d的道路坐标点；A为所述局部导航信息中距离车身中心最近的道路坐标点；位置坐标V为由所述位置信息中包含的车辆位置的坐标信息；

为预瞄点B到参考点P的向量；

为位置坐标V到参考点P的向量；

为参考点P到预瞄点B的向量。

8.根据权利要求7所述的轨迹跟踪装置，其特征在于，所述局部导航信息包括：

由当前建立通信连接的所述路侧设备生成的：基于在所述轨迹跟踪区域预先设置的道路坐标点，以所述车辆当前所在区域内包含的所有道路坐标点作为组成元素，构建生成的坐标序列。

9.根据权利要求8所述的轨迹跟踪装置，其特征在于，所述确定偏差单元通过以下公式计算获得所述参考点P(P_x,P_y)的坐标：

P_x＝V_x+d·sin(θ·π/180)；P_y＝V_y+d·cos(θ·π/180)；

其中，所述V_x、V_y和θ分别为所述位置信息中包含的横坐标、纵坐标和航向角。

10.根据权利要求7～9任一项所述的轨迹跟踪装置，其特征在于，所述轨迹跟踪装置还包括控制单元，用于：

根据当前的位置偏差和前一次获得的位置偏差，确定进行车辆控制的控制信息。

11.根据权利要求10所述的轨迹跟踪装置，其特征在于，所述控制单元具体用于：

将当前获得的所述位置偏差和在前一次获得的所述位置偏差作为模糊比例微分PD控制器的输入；

由模糊PD控制器根据下述公式计算输出进行车辆控制的方向盘转角：

方向盘转角＝K_p·E(1)+K_d·(E(1)-E(2))；

其中，参数K_p和参数K_d的隶属度函数通过高斯函数确定；E(1)-E(2)表示位置偏差率。

12.根据权利要求7～9任一项所述的轨迹跟踪装置，其特征在于，所述获取单元还用于：

与路侧设备建立通信连接时，从建立通信连接的路侧设备上获取车辆的所述位置信息。

13.一种基于位置偏差的轨迹跟踪方法，其特征在于，包括：

与车辆建立通信连接；

与车辆建立通信连接时，向车辆发送车辆当前所在区域的局部导航信息；

其中，所述局部导航信息用于结合车辆的位置信息确定车辆当前的位置偏差；所述位置偏差根据以下公式计算获得：

位置偏差

所述P为根据所述位置信息确定的沿车辆运行方向与车身中心距离为预瞄距离d的参考点；所述B为所述局部导航信息中，确定沿车辆运行方向与就近路点A的距离最接近所述预瞄距离d的道路坐标点；A为所述局部导航信息中距离车身中心最近的道路坐标点；位置坐标V为由所述位置信息中包含的车辆位置的坐标信息；

为预瞄点B到参考点P的向量；

为位置坐标V到参考点P的向量；

为参考点P到预瞄点B的向量。

14.根据权利要求13所述的轨迹跟踪方法，其特征在于，所述向车辆发送车辆当前所在区域的局部导航信息之前，所述轨迹跟踪方法还包括：

基于在所述轨迹跟踪区域预先设置的道路坐标点，以所述车辆当前所在区域内包含的所有道路坐标点作为组成元素，构建生成作为所述局部导航信息的坐标序列。

15.一种基于位置偏差的轨迹跟踪装置，其特征在于，包括：连接单元和发送单元；其中，

连接单元用于：与车辆建立通信连接；

发送单元用于：与车辆建立通信连接时，向车辆发送车辆当前所在区域的局部导航信息；

其中，所述局部导航信息用于结合车辆的位置信息确定车辆当前的位置偏差；所述位置偏差根据以下公式计算获得：

位置偏差

所述P为根据所述位置信息确定的沿车辆运行方向与车身中心距离为预瞄距离d的参考点；所述B为所述局部导航信息中，确定沿车辆运行方向与就近路点A的距离最接近所述预瞄距离d的道路坐标点；A为所述局部导航信息中距离车身中心最近的道路坐标点；位置坐标V为由所述位置信息中包含的车辆位置的坐标信息；

为预瞄点B到参考点P的向量；

为位置坐标V到参考点P的向量；

为参考点P到预瞄点B的向量。

16.根据权利要求15所述的轨迹跟踪装置，其特征在于，所述轨迹跟踪装置还包括生成单元，用于：

以所述车辆当前所在区域内包含的所有预先设置的道路坐标点作为导航的基础坐标，构建生成的路点坐标序列。

17.一种计算机存储介质，所述计算机存储介质中存储有计算机可执行指令，所述计算机可执行指令用于执行权利要求1～6中任一项所述的轨迹跟踪方法。

18.一种计算机存储介质，所述计算机存储介质中存储有计算机可执行指令，所述计算机可执行指令用于执行权利要求13或14所述的轨迹跟踪方法。

19.一种终端，包括：存储器和处理器；其中，

处理器被配置为执行存储器中的程序指令；

程序指令在处理器读取执行以下操作：

与路侧设备建立通信连接时，从建立通信连接的路侧设备上获取车辆当前所在区域的局部导航信息；

根据车辆的位置信息和获得的局部导航信息确定车辆当前的位置偏差；

其中，所述位置偏差根据以下公式计算获得：

位置偏差

所述P为根据所述位置信息确定的沿车辆运行方向与车身中心距离为预瞄距离d的参考点；所述B为所述局部导航信息中，确定沿车辆运行方向与就近路点A的距离最接近所述预瞄距离d的道路坐标点；A为所述局部导航信息中距离车身中心最近的道路坐标点；位置坐标V为由所述位置信息中包含的车辆位置的坐标信息；

为预瞄点B到参考点P的向量；

为位置坐标V到参考点P的向量；

为参考点P到预瞄点B的向量。

20.一种终端，包括：存储器和处理器；其中，

处理器被配置为执行存储器中的程序指令；

程序指令在处理器读取执行以下操作：

与车辆建立通信连接；

与车辆建立通信连接时，向车辆发送车辆当前所在区域的局部导航信息；

其中，所述局部导航信息用于结合车辆的位置信息确定车辆当前的位置偏差；所述位置偏差根据以下公式计算获得：

位置偏差

所述P为根据所述位置信息确定的沿车辆运行方向与车身中心距离为预瞄距离d的参考点；所述B为所述局部导航信息中，确定沿车辆运行方向与就近路点A的距离最接近所述预瞄距离d的道路坐标点；A为所述局部导航信息中距离车身中心最近的道路坐标点；位置坐标V为由所述位置信息中包含的车辆位置的坐标信息；

为预瞄点B到参考点P的向量；

为位置坐标V到参考点P的向量；

为参考点P到预瞄点B的向量。

Images