[发明专利]一种基于位置偏差的轨迹跟踪方法和装置

权利要求书

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的向量。