简介：在上一节内容中，我们估算出了车辆姿态，主要是位置偏差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 python3import rospy
import numpy as np
import timefrom std_msgs.msg import Float32,Float32MultiArray, Bool, Header
from duckietown_msgs.msg import Twist2DStamped, LanePose, ButtonEvent, WheelsCmdStamped
from sensor_msgs.msg import Rangeclass 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.0self.last_s = Noneself.action = np.array([0.0, 0.0])#停车线检测结果--检测到为trueself.red_line_sotp_falg = False#红绿灯检测结果--红灯为trueself.traffic_light_flag = False#距离雷达检测结果--距离小于限值为trueself.tof_stop_flag = False#按键检测，状态反向切换self.button_flag = Falseself.tof_count = 0self.wheels_cmd_executed = WheelsCmdStamped()self.wheels_cmd_executed.vel_left = 0.2self.wheels_cmd_executed.vel_right = 0.2rospy.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_threswheels_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_ffself.last_s = current_saction_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 = 0action_msg.omega = 0else:action_msg.v = self.action[0]action_msg.omega = self.action[1]header = msg.headerheader.frame_id = 'car_control_frame'action_msg.header = headerself.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.rangeif(dist>0 and dist<0.15):self.tof_count += 1if self.tof_count>=5:self.tof_stop_flag = Trueelse:self.tof_count = 0self.tof_stop_flag = False#按钮事件处理，按一次启动，再按一次停止def cd_button_event(self, msg):if self.button_flag:self.button_flag = Falseelse: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_errself.prev_phi_err = phi_errreturn [self.v_bar, omega]#横向位置和航向角误差进行积分计算def integrate_errors(self, d_err, phi_err, dt):self.d_I += d_err * dtself.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 = 0if np.sign(phi_err) != np.sign(self.prev_phi_err):self.phi_I = 0if wheels_cmd_exec[0] == 0 and wheels_cmd_exec[1] == 0:self.d_I = 0self.phi_I = 0#调整积分误差，使其保持在定义的范围内，如果误差小于误差估计的分辨率，则将其取消@staticmethoddef 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 = 0return integralif __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

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

若有收获，就点个赞吧

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

无人驾驶虚拟仿真（二）--ROS系统安装
简介:在ubuntu20.04系统中安装ROS noetic版本 1.安装源 $ sudo sh -c 'echo "deb http://packages.ros.org/ros/ubun ...
无人驾驶虚拟仿真（一）--环境搭建
简介:安装部署无人驾驶虚拟仿真平台,通过键盘控制仿真小车移动,简单介绍仿真环境切换以及自制方法基础环境:ubuntu20.04,python3.8.10 1.安装git.pip3 $ sudo ap ...
无人驾驶虚拟仿真（十三）--图像处理之交通标志牌识别1
简介:在实际交通地图环境中,我们预设了交通标志牌,可以通过交通标志牌对小车进行更加细致丰富的控制,标志牌形式如下: 每个交通标志牌都是由功能图片+编码图片结合的方式生成 ...
无人驾驶虚拟仿真（五）--图像处理之色彩平衡
简介:同样的物体在不同的光照环境中会出现色彩偏差,为了减少色彩偏差对系统检测造成的影响,在对图像进行识别前,需要先评估图像明暗度,计算出明暗阈值,通过对图像的色彩平衡处理,可以校正图像色偏,过饱和或饱 ...
无人驾驶虚拟仿真（十二）--图像处理之红绿灯识别
简介:在交通地图中有红绿灯组件,一般放置在T形或者十字路口处,车辆行驶过程中,需要检测红绿灯信号来控制车辆启停,以符合基本的交通规则.红绿灯识别基本分为图像截取.斑点检测.颜色识别三步来实现. 1.新 ...
无人驾驶虚拟仿真（八）--图像处理之车道线识别3
简介:在上一节中,我们完成了单张图片车道线的检测,在最终的结果中,可以比较明确的观察出车辆相对车道线的位置,接下来,我们要把这一功能移植到ROS系统中,将小车抓取到的图像都处理成车道线数据,并发布到下 ...
基于PanoSim5.0虚拟仿真平台的自主代客泊车AVP系统开发教程
一.Panosim5仿真平台简介 PanoSim是由国内一家创业公司联合吉大.北航等高校资源开发的一款智能驾驶汽车仿真软件平台.软件以智能驾驶汽车全栈仿真为开发目标,具有完整的场景模型.传感器模型和车 ...
博诺杯工业机器人比赛2019_关于举办第三届“汇博-博诺杯”全国高职院校工业机器人虚拟仿真大赛的通知...
随着<中国制造2025>国家战略的全面推进,工业机器人已经是目前最热门的发展领域之一.2016年5月,工信部.发改委.财政部联合印发<机器人产业发展规划(2016-2020年)> ...
Ubuntu18.04配置搭建基于Gazebo的虚拟仿真平台(Px4)：无人机(UAV)、无人车等模拟实验平台
这篇我觉得是写得比较详细完整的摘自:https://dgzc.ganahe.top/ganahe/2021/uavgazebomoni.html Ubuntu18.04配置搭建基于Gazebo的虚拟 ...

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