无人驾驶虚拟仿真（九）--车辆姿态预测及估算

最新推荐文章于 2026-08-18 16:53:10 发布

原创

最新推荐文章于 2026-08-18 16:53:10 发布 · 2.1k 阅读

本文介绍如何利用ROS实现基于车道线的车辆位置估算，通过融合历史数据与实时数据，采用置信度矩阵和可能性矩阵的方法来确定车辆最可能的位置。

简介：在上一节中我们检测到了车道线线段，并发布出来，在这一节内容中，我们要根据车道线来估算车辆位置，为什么说估算而不是计算，是因为我们无法根据车道线位置准确的计算出车辆姿态，每一条车道线线段都可以计算出一个车辆姿态数据，我们只能选取其中重合度较高的数据做为最终参考数据，为了让数据更加可靠，哦我们还需要用上一次的数据做参考，通过车辆速度和偏向角预测当前车辆姿态做为参考，再加入实时数据一起计算出当前车辆姿态。因为数据需要多次迭代，所以不方便分步来讲解，本节以ROS源码注释的方式来讲解。

1、新建功能包

进入工作空间目录：

$ cd ~/myros/catkin_ws/src

创建新的功能包：

$ catkin_create_pkg lane_filter rospy duckietown_msgs

新建配置文件：

$ mkdir -p lane_filter/config/lane_filter_node

$ touch lane_filter/config/lane_filter_node/default.yaml

新建启动脚本：

$ mkdir -p lane_filter/launch

$ touch lane_filter/launch/start.launch

新建源码文件：

$ touch lane_filter/src/lane_filter_node.py

编辑编译配置文件：

$ gedit lane_filter/CMakeLists.txt

修改为：

2、编辑配置文件

$ gedit lane_filter/config/lane_filter_node/default.yaml

mean_d_0: 0
mean_phi_0: 0
sigma_d_0: 0.1
sigma_phi_0: 0.1
delta_d: 0.02
delta_phi: 0.1
d_max: 0.3
d_min: -0.15
phi_min: -1.5
phi_max: 1.5
cov_v: 0.5
linewidth_white: 0.05
linewidth_yellow: 0.025
lanewidth: 0.22
min_max: 0.1
sigma_d_mask: 1.0
sigma_phi_mask: 2.0
range_min: 0.21
range_est: 0.33
range_max: 0.6

3、编辑源码文件

$ gedit lane_filter/src/lane_filter_node.py

功能实现逻辑图：

在姿态估算过程中，我们用到一个变量belief，文中称之为置信度矩阵，是一个23*30的矩阵，矩阵中，每一个行列交叉点实际意义是代表车辆的一种姿态，距离误差范围设定为-0.15~0.3，步长0.02，可分割为23种不同的距离误差，航向角误差范围设定为-1.5~1.5，步长0.1，可分割为30种不同的航向角误差，二者结合形成一个23*30的矩阵，（0，0)就代表距离误差-0.15，航向角误差-1.5，以此类推。

在计算当前姿态过程中，还有一个变量measurement_likelihood，我称之为可能性矩阵，与置信度矩阵相似，初始化为全0矩阵，每一条车道线估算出来的车辆姿态，以固定公式转化为行列坐标，该坐标值+1，全部计算完成后，值最大坐标对应的置信度矩阵种的姿态就是计算出来的车辆可能性最大的姿态数据。

附源码：

#!/usr/bin/env python3

from math import floor, sqrt
import rospy
import numpy as np
import time
from scipy.ndimage.filters import gaussian_filter
from scipy.stats import entropy, multivariate_normal
from duckietown_msgs.msg import Segment, SegmentList, Twist2DStamped,LanePose

class LaneFilterNode():
    def __init__(self):
        rospy.init_node("

标签