无人驾驶虚拟仿真（十）--车辆姿态控制(PID)

简介：在上一节内容中，我们估算出了车辆姿态，主要是位置偏差d和航向角偏差phi，出现偏差就需要矫正，根据物理常识我们知道不可能马上矫正，需要一个矫正的过程，图像采集处理又有一个时间间隔，在这个时间间隔里还会出现矫正过度，需要反向矫正的情况，再加上车辆本身的动力偏差，道路的影响等，我们无法通过简单计算来让车辆达到误差为0的状态，因此我们引入PID控制算法，目的是实现车辆姿态控制参数自动化调整。

比例积分微分控制（proportional-integral-derivative control），简称PID控制，是最早发展起来的控制策略之一，由于其算法简单、鲁棒性好和可靠性高，被广泛应用于工业过程控制。其原理简单来说，就是根据给定值和实际输出值构成控制偏差，将偏差按比例、积分和微分通过线性组合构成控制量，对被控对象进行控制。

PID控制器各校正环节的作用如下：

比例环节：即时成比例地反应控制系统的偏差信号e(t)，偏差一旦产生，控制器立即产生控制作用以减小误差。当偏差e=0时，控制作用也为0。因此，比例控制是基于偏差进行调节的，即有差调节。

积分环节：能对误差进行记忆，主要用于消除静差，提高系统的无差度，积分作用的强弱取决于积分时间常数Ti，Ti越大，积分作用越弱，反之则越强。

微分环节：能反映偏差信号的变化趋势(变化速率)，并能在偏差信号值变得太大之前，在系统中引入一个有效的早期修正信号，从而加快系统的动作速度，减小调节时间。

从时间的角度讲，比例作用是针对系统当前误差进行控制，积分作用则针对系统误差的历史，而微分作用则反映了系统误差的变化趋势，这三者的组合是“过去、现在、未来”的完美结合。

详细的PID控制原理不再多说，大家可以到网上查看，接下来我们来说明如何在我们的系统中实现车辆姿态控制，由于PID控制也是一个多帧数据迭代计算的过程，不方便单独拆开来看效果，与上一节相似，还是以源码注解的方式来说明：

1、新建功能包

进入工作空间目录：

$ cd ~/myros/catkin_ws/src

创建新功能包：

$ catkin_create_pkg car_control rospy duckietown_msgs std_msgs

创建配置文件：

$ mkdir -p car_control/config/car_control_node

$ touch car_control/config/car_control_node/default.yaml

新建启动脚本：

$ mkdir -p car_control/launch

$ touch car_control/launch/start.launch

新建源码文件

$ touch car_control/src/car_control_node.py

修改编译配置文件

$ gedit car_control/CMakeLists.txt

修改为：

---

2、编辑配置文件

$ gedit car_control/config/car_control_node/default.yaml

v_bar: 0.4
k_d: -10
k_theta: -5
k_Id: 0.3
k_Iphi: 0.1
theta_thres: 0.523
d_thres: 0.2615
d_offset: 0.02
omega_ff: 0
integral_bounds: {'d': {'top': 0.3,'bot': -0.3},'phi': {'top': 1.2, 'bot': -1.2}}
d_resolution: 0.011
phi_resolution: 0.051
stop_line_slowdown: {'start': 0.6,'end': 0.15}

---

3、编辑源码文件

$ gedit car_control/src/car_control_node.py

#!/usr/bin/env python3

import rospy
import numpy as np
import time

from std_msgs.msg import Float32,Float32MultiArray, Bool, Header
from duckietown_msgs.msg import Twist2DStamped, LanePose, ButtonEvent, WheelsCmdStamped
from sensor_msgs.msg import Range

class CarControlNode:
    def __init__(self):
        rospy.init_node("car_control_node", anonymous=False)
        #车辆速度基数
        self.v_bar = rospy.get_param('~v_bar', default=0.4)
        #横向位置误差控制比例参数
        self.k_d = rospy.get_param('~k_d', default=-10)
        #航向角控制比例参数
        self.k_theta = rospy.get_param('~k_theta', default=-5)
        #横向位置误差控制积分参数
        self.k_Id = rospy.get_param('~k_Id', default=0.3)
        #航向角控制积分参数
        self.k_Iphi = rospy.get_param('~k_Iphi', default=0.1)
        #航向角偏差上限
        self.theta_thres = rospy.get_param('~theta_thres', default=0.523)
        #横向位置误差积分偏差上限
        self.d_thres = rospy.get_param('~d_thres', default=0.2615)
        #横向位置偏移量（米）
        self.d_offset = rospy.get_param('~d_offset', default=0.02)
        #角度偏移量
        self.omega_ff = rospy.get_param('~omega_ff', default=0.0)
        #积分项界限
        self.integral_bounds = rospy.get_param('~integral_bounds', default={'d': {'top': 0.3,'bot': -0.3},'phi': {'top': 1.2, 'bot': -1.2}})
        #横向位置估计的分辨率
        self.d_resolution = rospy.get_param('~d_resolution', default=0.011)
        #航向角估计的分辨率
        self.phi_resolution = rospy.get_param('~phi_resolution', default=0.051)
        #停车线处减速的起点和终点距离
        self.stop_line_slowdown = rospy.get_param('~stop_line_slowdown', default={'start': 0.6,'end': 0.15})
        
        #横向位置误差
        self.d_I = 0.0
        #当前航向角误差
        self.phi_I = 0.0
        #上一次横向位置误差
        self.prev_d_err = 0.0
        #上一次航向角误差
        self.prev_phi_err = 0.0
        
        self.last_s = None
        self.action = np.array([0.0, 0.0])
        #停车线检测结果--检测到为true
        self.red_line_sotp_falg = False
        #红绿灯检测结果--红灯为true
        self.traffic_light_flag = False
        #距离雷达检测结果--距离小于限值为true
        self.tof_stop_flag = False
        #按键检测，状态反向切换
        self.button_flag = False
        self.tof_count = 0
        self.wheels_cmd_executed = WheelsCmdStamped()
        self.wheels_cmd_executed.vel_left = 0.2
        self.wheels_cmd_executed.vel_right = 0.2
        
        rospy.Subscriber("~err", LanePose, self.cb_err)
        rospy.Subscriber('~wheels_cmd', WheelsCmdStamped, self.cd_wheels_cmd)
        rospy.Subscriber("~range", Range, self.cb_tof_info)
        rospy.Subscriber("~detected", Bool, self.cb_stop_line)
        rospy.Subscriber("~tlState", Bool, self.cb_traffic_light)
        rospy.Subscriber('~event', ButtonEvent, self.cd_button_event)
        self.pub_action = rospy.Publisher("~car_cmd", Twist2DStamped, queue_size=10)
    
    def cb_err(self, msg):
        current_s = time.time()
        dt = None
        #计算时间差
        if self.last_s is not None:
            dt = (current_s - self.last_s)
        #计算实际横向位置误差
        d_err = msg.d - self.d_offset
        #航向角误差
        phi_err = msg.phi
        #横向积分误差太大时限制一下
        if np.abs(d_err) > self.d_thres:
            print("d_err too large, thresholding it!", 'error')
            d_err = np.sign(d_err) * self.d_thres
        wheels_cmd_exec = [self.wheels_cmd_executed.vel_left, self.wheels_cmd_executed.vel_right]
        #计算控制参数
        self.action = self.compute_control_action(d_err, phi_err, dt, wheels_cmd_exec)
        self.action[1] += self.omega_ff
        self.last_s = current_s
        action_msg = Twist2DStamped()
        #判断是否需要停车
        if((self.red_line_sotp_falg and self.traffic_light_flag) or self.tof_stop_flag or self.button_flag):
            action_msg.v = 0
            action_msg.omega = 0
        else:
            action_msg.v = self.action[0]
            action_msg.omega = self.action[1]
        header = msg.header
        header.frame_id = 'car_control_frame'
        action_msg.header = header
        self.pub_action.publish(action_msg)
    #停车线检测处理
    def cb_stop_line(self, msg):
        self.red_line_sotp_falg = msg.data
    #红绿灯检测处理
    def cb_traffic_light(self, msg):
        self.traffic_light_flag = msg.data
    #距离传感器数据处理，连续5次检测到距离小于0.15米，停车
    def cb_tof_info(self, msg):
        dist = msg.range
        if(dist>0 and dist<0.15):
            self.tof_count += 1
            if self.tof_count>=5:
                self.tof_stop_flag = True
        else:
            self.tof_count = 0
            self.tof_stop_flag = False
    #按钮事件处理，按一次启动，再按一次停止
    def cd_button_event(self, msg):
        if self.button_flag:
            self.button_flag = False
        else:
            self.button_flag = True
    #车轮控制结果反馈
    def cd_wheels_cmd(self, msg):
        self.wheels_cmd_executed = msg
    #计算控制参数
    def compute_control_action(self, d_err, phi_err, dt, wheels_cmd_exec):
        if dt is not None:
            #横向位置和航向角误差进行积分计算
            self.integrate_errors(d_err, phi_err, dt)

        #调整横向位置和航向角积分误差
        self.d_I = self.adjust_integral(d_err, self.d_I, self.integral_bounds["d"], self.d_resolution)
        self.phi_I = self.adjust_integral(phi_err,self.phi_I,self.integral_bounds["phi"],self.phi_resolution,)

        #误差值符号变化或者车辆停止，消除误差积分
        self.reset_if_needed(d_err, phi_err, wheels_cmd_exec)

        #缩放线性参数，使其速度实际值为固定值
        omega = (self.k_d * d_err+ self.k_theta * phi_err+ self.k_Id * self.d_I+ self.k_Iphi * self.phi_I)

        self.prev_d_err = d_err
        self.prev_phi_err = phi_err

        return [self.v_bar, omega]

    #横向位置和航向角误差进行积分计算
    def integrate_errors(self, d_err, phi_err, dt):
        self.d_I += d_err * dt
        self.phi_I += phi_err * dt
    #如果误差正负符号改变或机器人完全停止，则重置“d”和“phi”中的积分错误
    def reset_if_needed(self, d_err, phi_err, wheels_cmd_exec):
        if np.sign(d_err) != np.sign(self.prev_d_err):
            self.d_I = 0
        if np.sign(phi_err) != np.sign(self.prev_phi_err):
            self.phi_I = 0
        if wheels_cmd_exec[0] == 0 and wheels_cmd_exec[1] == 0:
            self.d_I = 0
            self.phi_I = 0

    #调整积分误差，使其保持在定义的范围内，如果误差小于误差估计的分辨率，则将其取消
    @staticmethod
    def adjust_integral(error, integral, bounds, resolution):
        if integral > bounds["top"]:
            integral = bounds["top"]
        elif integral < bounds["bot"]:
            integral = bounds["bot"]
        elif abs(error) < resolution:
            integral = 0
        return integral

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

---

4、编辑启动脚本

$ gedit car_control/launch/start.launch

<launch>
  <arg name="veh"/>
  <arg name="pkg_name" value="car_control"/>
  <arg name="node_name" value="car_control_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="car_control_node/err" to="lane_filter_node/err"/>
    <remap from="car_control_node/stopFlag" to="red_line_node/stopFlag"/>
    <remap from="car_control_node/range" to="front_center_tof_driver_node/range"/>
    <remap from="car_control_node/car_cmd" to="duckiebot_node/car_cmd"/>
    <remap from="car_control_node/detected" to="stop_line_node/detected"/>
    <remap from="car_control_node/tlState" to="traffic_light_node/tlState"/>
    <remap from="car_control_node/event" to="button_driver_node/event"/>
    <node name="$(arg node_name)" pkg="$(arg pkg_name)" type="$(arg node_name).py" respawn="true" respawn_delay="10" output="screen" required="$(arg required)">
    </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>
    <include file="$(find car_control)/launch/start.launch">
      <arg name="veh" value="$(arg veh)"/>
    </include>
  </group>
</launch>

修改duckiebot_node，源码中：

修改为：

配置文件map-name更换loop_empty，这样程序运行时将打开车辆行驶画面。

---

7、启动程序

$ source devel/setup.bash

$ roslaunch start.launch

到此为止，我们完成了无人驾驶虚拟仿真从图像采集到姿态控制所有流程，形成了一个闭环的系统，后续章节我们将在这个闭环控制的基础上，增加一些新的功能。

​

若有收获，就点个赞吧