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

简介：在上一节中我们检测到了车道线线段，并发布出来，在这一节内容中，我们要根据车道线来估算车辆位置，为什么说估算而不是计算，是因为我们无法根据车道线位置准确的计算出车辆姿态，每一条车道线线段都可以计算出一个车辆姿态数据，我们只能选取其中重合度较高的数据做为最终参考数据，为了让数据更加可靠，哦我们还需要用上一次的数据做参考，通过车辆速度和偏向角预测当前车辆姿态做为参考，再加入实时数据一起计算出当前车辆姿态。因为数据需要多次迭代，所以不方便分步来讲解，本节以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("lane_filter_node", anonymous=False)
        #多元正态随机变量参数
        self.mean_d_0 = rospy.get_param("~mean_d_0", default=0)
        self.mean_phi_0 = rospy.get_param("~mean_phi_0", default=0)
        self.sigma_d_0 = rospy.get_param("~sigma_d_0", default=0.1)
        self.sigma_phi_0 = rospy.get_param("~sigma_phi_0", default=0.1)
        #车辆姿态取值范围及精度
        self.delta_d = rospy.get_param("~delta_d", default=0.02)
        self.delta_phi = rospy.get_param("~delta_phi", default=0.1)
        self.d_max = rospy.get_param("~d_max", default=0.3)
        self.d_min = rospy.get_param("~d_min", default=-0.15)
        self.phi_min = rospy.get_param("~phi_min", default=-1.5)
        self.phi_max = rospy.get_param("~phi_max", default=1.5)
        self.cov_v = rospy.get_param("~cov_v", default=0.5)
        #车道线宽度参数
        self.linewidth_white = rospy.get_param("~linewidth_white", default=0.05)
        self.linewidth_yellow = rospy.get_param("~linewidth_yellow", default=0.025)
        self.lanewidth = rospy.get_param("~lanewidth", default=0.22)
        #
        self.min_max = rospy.get_param("~min_max", default=0.1)
        self.sigma_d_mask = rospy.get_param("~sigma_d_mask", default=1.0)
        self.sigma_phi_mask = rospy.get_param("~sigma_phi_mask", default=2.0)
        #参与车辆姿态计算的点的距离车距离范围设定
        self.range_min = rospy.get_param("~range_min", default=0.21)
        self.range_est = rospy.get_param("~range_est", default=0.33)
        self.range_max = rospy.get_param("~range_max", default=0.6)
        
        #最新的车辆控制参数
        self.currentVelocity = None
        #最新数据更新时间
        self.t_last_update = 0
        #初始化车道线距离误差表和航向角误差表
        self.d, self.phi = np.mgrid[
            self.d_min : self.d_max : self.delta_d, self.phi_min : self.phi_max : self.delta_phi
        ]
        #初始化置信度矩阵
        self.belief = np.empty(self.d.shape)
        #多元正态随机变量平均值
        self.mean_0 = [self.mean_d_0, self.mean_phi_0]
        #多元正态随机变量协方差矩阵
        self.cov_0 = [[self.sigma_d_0, 0], [0, self.sigma_phi_0]]
        #高斯滤波掩模
        self.cov_mask = [self.sigma_d_mask, self.sigma_phi_mask]

        #红线是否按白线处理
        self.red_to_white = False
        #是否使用黄线，False的话计算姿态只依据检测到的白线
        self.use_yellow = True
        self.range_est_min = 0

        self.initialize()
        rospy.Subscriber('~segments', SegmentList, self.cb_segments)
        rospy.Subscriber("~car_cmd", Twist2DStamped, self.car_cmd_callback)
        self.pub_err = rospy.Publisher("~err", LanePose, queue_size=10)

    #初始化置信度矩阵
    def initialize(self):
        pos = np.empty(self.d.shape + (2,))
        pos[:, :, 0] = self.d
        pos[:, :, 1] = self.phi
        #生成一个多元正态随机变量。
        RV = multivariate_normal(self.mean_0, self.cov_0)
        #使用概率密度函数对pos进行处理，得到满足设置的mean和cov的值,使其分布满足高斯分布
        self.belief = RV.pdf(pos)
    
    #车道线数据处理函数
    def cb_segments(self, msg):
        current_time = time.time()
        #t_last_update不等于0，说明不是第一条数据，根据之前的数据对车辆姿态进行预估
        if self.t_last_update!=0:
            dt = current_time - self.t_last_update
            self.predict(dt=dt, v=self.currentVelocity.v, w=self.currentVelocity.omega)
        #self.t_last_update更新在本节因为收不到控制反馈，不更新，设置为0，在下一节中需要更新为current_time
        self.t_last_update = 0
        #self.t_last_update = current_time   
        self.update(msg.segments)
        [d_max, phi_max] = self.getEstimate()
        err_msg = LanePose()
        header = msg.header
        header.frame_id = 'lane_filter_frame'
        err_msg.header = header
        err_msg.d = d_max
        err_msg.phi = phi_max
        self.pub_err.publish(err_msg)
    #车辆控制结果回调函数
    def car_cmd_callback(self, msg):
        self.currentVelocity = msg

    #车辆姿态预估， dt：时间间隔，v：上一次车辆速度，w：上一次车辆航向角
    def predict(self, dt, v, w):
        delta_t = dt
        #预估车辆位置
        d_t = self.d + v * delta_t * np.sin(self.phi)
        #预估车辆航向角
        phi_t = self.phi + w * delta_t
        #初始化置信度矩阵
        p_belief = np.zeros(self.belief.shape)

        #遍历置信度矩阵，过滤无效点
        for i in range(self.belief.shape[0]):
            for j in range(self.belief.shape[1]):
                if self.belief[i, j] > 0:
                    #移除超出范围的点
                    if (
                        d_t[i, j] > self.d_max
                        or d_t[i, j] < self.d_min
                        or phi_t[i, j] < self.phi_min
                        or phi_t[i, j] > self.phi_max
                    ):
                        continue
                    
                    i_new = int(floor((d_t[i, j] - self.d_min) / self.delta_d))
                    j_new = int(floor((phi_t[i, j] - self.phi_min) / self.delta_phi))

                    p_belief[i_new, j_new] += self.belief[i, j]

        s_belief = np.zeros(self.belief.shape)
        #多维高斯滤波
        gaussian_filter(p_belief, self.cov_mask, output=s_belief, mode="constant")

        if np.sum(s_belief) == 0:
            return
        self.belief = s_belief / np.sum(s_belief)

    #处理车道线数据，为置信度矩阵计算做准备
    def prepareSegments(self, segments):
        segmentsArray = []
        for segment in segments:
            #如果红线当白线用，改变segment颜色值
            if self.red_to_white and segment.color == 2:
                segment.color = 0
            #如果计算不参考黄线，则跳过，相当于删除黄线数据
            if not self.use_yellow and segment.color == 1:
                continue
            #对于车辆姿态估计，红线没有意义，跳过，相当于删除红线数据
            if segment.color != 0 and segment.color != 1:
                continue
            #车道线坐标x小于0的表示该线在车辆后方，不参与计算
            if segment.points[0].x < 0 or segment.points[1].x < 0:
                continue
            #仅考虑在一定范围内的点用来对车辆姿态进行计算，计算点到车辆的距离，超出范围的点
            point_range = self.getSegmentDistance(segment)
            if self.range_est > point_range > self.range_est_min:
                segmentsArray.append(segment)            
        return segmentsArray
    #更新车道线数据
    def update(self, segments):
        #过滤无用车道线数据
        segmentsArray = self.prepareSegments(segments)
        #计算车辆位置可能性矩阵
        measurement_likelihood = self.generate_measurement_likelihood(segmentsArray)
        #根据可能性矩阵重新计算置信度矩阵
        if measurement_likelihood is not None:
             #矩阵对应元素位置相乘
            self.belief = np.multiply(self.belief, measurement_likelihood)
            if np.sum(self.belief) == 0:
                self.belief = measurement_likelihood
            else:
                self.belief /= np.sum(self.belief)

    def generate_measurement_likelihood(self, segments):
        #初始化位置可能性矩阵
        measurement_likelihood = np.zeros(self.d.shape)

        for segment in segments:
            #根据单条车道线线段，计算车辆姿态（d_i:偏移距离，phi_i:偏移角度）
            d_i, phi_i = self.generateVote(segment)

            #根据车道线计算出的车辆位置不在正常范围内的车道线剔除
            if d_i > self.d_max or d_i < self.d_min or phi_i < self.phi_min or phi_i > self.phi_max:
                continue
            #正常范围内的车道线以偏移距离和偏移角度为基础计算坐标值，可能性矩阵对应坐标值+1
            i = int(floor((d_i - self.d_min) / self.delta_d))
            j = int(floor((phi_i - self.phi_min) / self.delta_phi))
            measurement_likelihood[i, j] += 1

        if np.linalg.norm(measurement_likelihood) == 0:
            return None
        #归一化处理
        measurement_likelihood /= np.sum(measurement_likelihood)
        return measurement_likelihood

    def getEstimate(self):
        #取置信度矩阵中最大值索引
        maxids = np.unravel_index(self.belief.argmax(), self.belief.shape)
        #根据索引反向计算偏移距离和偏移角度
        d_max = self.d_min + (maxids[0] + 0.5) * self.delta_d
        phi_max = self.phi_min + (maxids[1] + 0.5) * self.delta_phi
        return [d_max, phi_max]

    #计算车道线可提供的参数，包括车辆位置偏移，角度偏移等
    def generateVote(self, segment):
        p1 = np.array([segment.points[0].x, segment.points[0].y])
        p2 = np.array([segment.points[1].x, segment.points[1].y])
        #计算该车道线偏向角度三角函数
        #t_hat[0] = (p2[0]-p1[0])/((p2[0]-p1[0])**2+(p2[1]-p1[1])**2)**(1/2)  -cos(phi)
        #t_hat[1] = (p2[1]-p1[1])/((p2[0]-p1[0])**2+(p2[1]-p1[1])**2)**(1/2)  sin(phi)
        t_hat = (p2 - p1) / np.linalg.norm(p2 - p1)
        #n_hat = [-sin(phi), -cos(phi)]
        n_hat = np.array([-t_hat[1], t_hat[0]])
        #计算车道线端点距车头(原点)的距离
        d1 = np.inner(n_hat, p1)
        d2 = np.inner(n_hat, p2)
        d_i = (d1 + d2) / 2
        
        #计算偏向角
        phi_i = np.arcsin(t_hat[1])
        if segment.color == 0:  #白线应该在车辆右侧
            if p1[0] > p2[0]:  # right edge of white lane
                d_i -= self.linewidth_white
            else:  # left edge of white lane
                d_i = -d_i
                phi_i = -phi_i
            d_i -= self.lanewidth / 2

        elif segment.color == 1:  #黄线应该在车辆左侧
            if p2[0] > p1[0]:  # left edge of yellow lane
                d_i -= self.linewidth_yellow
                phi_i = -phi_i
            else:  # right edge of yellow lane
                d_i = -d_i
            d_i = self.lanewidth / 2 - d_i
        return d_i, phi_i

    #计算点到车辆的距离
    def getSegmentDistance(self, segment):
        x_c = (segment.points[0].x + segment.points[1].x) / 2
        y_c = (segment.points[0].y + segment.points[1].y) / 2
        return sqrt(x_c ** 2 + y_c ** 2)

if __name__=='__main__':
    node = LaneFilterNode()
    rospy.spin()

---

4、编辑启动脚本

$ gedit lane_filter/launch/start.launch

<launch>
  <arg name="veh"/>
  <arg name="pkg_name" value="lane_filter"/>
  <arg name="node_name" value="lane_filter_node"/>
  <arg name="param_file_name" default="default" doc="Specify a param file. ex:megaman"/>
  <arg name="required" default="false" />

  <group ns="$(arg veh)">
    <remap from="lane_filter_node/segments" to="line_detect_node/segments"/>
    <remap from="lane_filter_node/car_cmd" to="duckiebot_node/car_cmd"/>
    <node name="$(arg node_name)" pkg="$(arg pkg_name)" type="$(arg node_name).py" respawn="true" respawn_delay="10" output="screen" required="$(arg required)">
      <rosparam command="load" file="$(find lane_filter)/config/$(arg node_name)/$(arg param_file_name).yaml"/>
    </node>
  </group>
</launch>

---

5、编译

$ cd ~/myros/catkin_ws

$ catkin_make

---

6、修改多节点启动文件

$ gedit start.launch

<launch>
  <arg name="veh" default="duckiebot"/>
  <group>
    <include file="$(find duckiebot)/launch/duckiebot.launch">
      <arg name="veh" value="$(arg veh)"/>
    </include>
    <include file="$(find anti_instagram)/launch/start.launch">
      <arg name="veh" value="$(arg veh)"/>
    </include>
    <include file="$(find line_detect)/launch/start.launch">
      <arg name="veh" value="$(arg veh)"/>
    </include>
    <include file="$(find lane_filter)/launch/start.launch">
      <arg name="veh" value="$(arg veh)"/>
    </include>
  </group>
</launch>

---

7、启动测试

$ source devel/setup.bash

$ roslaunch start.launch

​

若有收获，就点个赞吧