一、AEB算法
1.1 TTC算法
碰撞时间算法TTC是指从当前时刻开始,两车保持当前车速行驶直到发生碰撞所需的时间,两车前后行驶在路上时,只有当后车车速大于前车车车速时才会进入TTC算法。为了避免碰撞,前后车应满足以下关系式:
式中:v1、a1表示前车速度、加速度,v2、a2表示后车速度、加速度,Drel表示两车相对距离,d0表示安全停车距离(一般为2mor3m)。由上述公式得到如下解:
式中:vrel、arel表示两车相对速度、相对加速度。
然后是TTC的触发逻辑:
在TTC算法中设定时间阈值如下:TTC(预警危险)=2.6s,TTC(部分制动)=1.6s,TTC(全力制动)=0.6s。具体的控制逻辑如下:
1)TTC>2.6s:AEB系统不触发;
2)1.6s<TTC≤2.6s:AEB系统触发→发出警报;
3)0.6s<TTC≤1.6s:AEB系统触发→40%制动;
4)TTC≤0.6s:AEB系统触发→100%制动。
1.2 SeungwukMoon安全距离模型
SeungwukMoon安全距离模型计算公式如下:
式中:dbr为危险制动距离,Tdelay为系统延迟时间,f(μ)为制动因数,amax为最大制动减速度。参考文献,取Tdelay=1.2s、 f(μ)=0.2、amax=6m/s2。
触发逻辑也很简单,当相对车距小于等于dbr后AEB触发。
以下是实现思路:
-
输入和输出:
- 输入:前车速度和加速度(v1,a1),后车速度和加速度(v2,a2),两车相对距离(Drel)
- 输出:是否触发AEB,触发类型(警报、部分制动、全力制动)
-
步骤:
- 计算相对速度(vrel)和相对加速度(arel)
- 根据公式计算TTC
- 根据TTC值决定AEB触发类型
- 计算危险制动距离dbr
- 根据Seungwuk Moon模型触发AEB
下面是具体的C++代码实现:
cpp
复制代码
//3.cpp
#include <ros/ros.h>
#include <common_msgs/CICV_Location.h>
#include <common_msgs/Lane.h>
#include <common_msgs/Lanes.h>
#include <common_msgs/Perceptionobjects.h>
#include <common_msgs/Perceptionobject.h>
#include <std_msgs/String.h>
#include <common_msgs/Control_Test.h>
#include <cmath>
#include <thread>
class SensorData {
public:
SensorData(ros::NodeHandle& nh) {
// 初始化订阅器
pose_subscriber = nh.subscribe("/cicv_location", 10, &SensorData::poseStationCallback, this);
lane_detection_subscriber = nh.subscribe("/LaneDetection", 10, &SensorData::detectionLanesCallback, this);
perception_subscriber = nh.subscribe("/tpperception", 10, &SensorData::obstacleCallback, this);
}
common_msgs::CICV_Location getCurrentPose() const {
return current_pose;
}
common_msgs::Perceptionobjects getObstacles() const {
return obstacles;
}
private:
ros::Subscriber pose_subscriber;
ros::Subscriber lane_detection_subscriber;
ros::Subscriber perception_subscriber;
common_msgs::CICV_Location current_pose;
common_msgs::Perceptionobjects obstacles;
// 处理车辆位置信息的回调函数
void poseStationCallback(const common_msgs::CICV_Location::ConstPtr& msg) {
current_pose = *msg;
double x, y, xv, vy, ax, ay, phi, yaw;
//ROS_INFO("Header: %d, Frame ID: %s", current_pose.header.seq, current_pose.header.frame_id.c_str());
x = current_pose.Position_x;
y = current_pose.Position_y;
xv = current_pose.Velocity_x;
vy = current_pose.Velocity_y;
ax = current_pose.Accel_x;
ay = current_pose.Accel_y;
phi = current_pose.Angular_velocity_z;
yaw = current_pose.Yaw;
// ROS_INFO("Received pose: x=%8.8f, y=%8.8f, vx=%8.8f, vy=%8.8f, ax=%8.8f, ay=%8.8f, phi=%8.8f, yaw=%8.8f",
// current_pose.Position_x, current_pose.Position_y, current_pose.Velocity_x,
// current_pose.Velocity_y, current_pose.Accel_x, current_pose.Accel_y,
// current_pose.Angular_velocity_z, current_pose.Yaw);
}
// 处理车道信息的回调函数
void detectionLanesCallback(const common_msgs::Lanes::ConstPtr& msg) {
common_msgs::Lanes lanes = *msg;
// ROS_INFO("Header: %d, Frame ID: %s", lanes.header.seq, lanes.header.frame_id.c_str());
// ROS_INFO("number of lanes:%d", lanes.num);
double lane_idx, c_0, c_1, c_2, c_3;
for (size_t i = 0; i < lanes.lanes.size(); ++i) {
const common_msgs::Lane& lane = lanes.lanes[i];
// ROS_INFO("lane_idx:%d, CO:%.8f, C1:%.8f, C2:%.8f, C3:%.8f",
// lane.lane_idx, lane.c_0, lane.c_1, lane.c_2, lane.c_3);
lane_idx = lane.lane_idx;
c_0 = lane.c_0;
c_1 = lane.c_1;
c_2 = lane.c_2;
c_3 = lane.c_3;
}
}
// 处理障碍物信息的回调函数
void obstacleCallback(const common_msgs::Perceptionobjects::ConstPtr& msg) {
obstacles = *msg;
double object_id , object_x , object_y, object_vx , object_ax, object_vy, object_ay, object_heading, object_length, object_width, object_height;
// ROS_INFO("Header: %d, Frame ID: %s", obstacles.header.seq, obstacles.header.frame_id.c_str());
// ROS_INFO("number of obstacles: %d", obstacles.num);
for (size_t i = 0; i < obstacles.Perceptionobjects.size(); ++i) {
const common_msgs::Perceptionobject& object = obstacles.Perceptionobjects[i];
object_id = object.ID;
object_x = object.x;
object_y = object.y;
object_vx = object.v_x;
object_ax = object.a_x;
object_vy = object.v_y;
object_ay = object.a_y;
object_heading = object.heading;
object_length = object.length;
object_width = object.width;
object_height = object.height;
// ROS_INFO("Object ID: %d, X: %.8f, Y: %.8f, vx: %.8f, vy: %.8f, ax:%.8f, ay:%.8f , xg:%.8f, yg:%.8f,"
// "vxg:%.8f, vyg:%.8f, heading:%.8f, length: %.8f, width: %.8f, height: %.8f ",
// object.ID, object.x, object.y, object.v_x, object.v_y, object.a_x, object.a_y ,object.xg, object.yg,
// object.v_xg, object.v_yg, object.heading, object.length, object.width, object.height);
}
}
};
class Control {
public:
Control(ros::NodeHandle& nh) {
// 控制信息发布
control_pub_ = nh.advertise<common_msgs::Control_Test>("/control_test", 10);
}
void sendControlCommand(double throttle, double brake, double steer_angle, double gear = 4) {
common_msgs::Control_Test control_msg;
// ROS_INFO("Header: %d, Frame ID: %s", control_msg.header.seq, control_msg.header.frame_id.c_str());
// control_msg.header.stamp = ros::Time::now();
control_msg.ThrottlePedal = throttle;
control_msg.BrakePedal = brake;
control_msg.SteeringAngle = steer_angle;
control_msg.Gear = gear;
control_pub_.publish(control_msg);
}
private:
ros::Publisher control_pub_;
};
bool braking_ = false;
class AEBSystem {
public:
AEBSystem(ros::NodeHandle& nh, SensorData& sensor_data, Control& control) : sensor_data_(sensor_data), control_(control)
{
// 初始化发布器
aeb_pub_ = nh.advertise<std_msgs::String>("aeb_status", 10);
}
void checkAEB()
{
const common_msgs::CICV_Location& current_pose = sensor_data_.getCurrentPose();
const common_msgs::Perceptionobjects& obstacles = sensor_data_.getObstacles();
if (obstacles.Perceptionobjects.empty())
{
ROS_WARN("No perception objects available");
return;
}
const common_msgs::Perceptionobject& closest_object = obstacles.Perceptionobjects[0]; // 假设第一个是最近的物体
double phi1 = closest_object.heading*(180/M_PI);
// double phi1 = closest_object.heading;
double phi2 = current_pose.Yaw;
double v1 = -closest_object.v_xg/cos(phi1); // 前车速度(车辆坐标)
double a1 = closest_object.a_x; // 假设前车加速度未知或为0
double v2 = current_pose.Velocity_x/cos(phi2); // 自车速度(大地坐标转车辆坐标)
double a2 = current_pose.Accel_x/cos(phi2); // 自车加速度(大地坐标转车辆坐标)
double x1 = closest_object.xg;
double y1 = closest_object.yg;
double x2 = current_pose.Position_x;
double y2 = current_pose.Position_y;
double vel = -closest_object.v_xrel; // vel为目标物x方向绝对速度减自车x方向绝对速度,单位:m/s
double Drel = sqrt(pow(x1 - x2, 2) + pow(y1 - y2, 2)); // 相对距离
double TTC = calculateTTC(v1, a1, v2, a2, Drel);
double dbr = calculateSafetyDistance(v2,v1);
ROS_INFO("TTC:%f, x1:%f, x2:%f, y1:%f, y2:%f, v1:%f, v2:%f, a2:%f, Drel:%f, dbr:%f ", TTC, x1, x2, y1, y1, v1, v2, a2, Drel, dbr);
if (TTC < 2.6 && TTC >= 1.6)
{
std::thread([this]()
{
control_.sendControlCommand(0.0, 0.4, 0.0);
ROS_INFO("AEB triggered: Warning");
std::this_thread::sleep_for(std::chrono::seconds(3));
}).detach();
}
else if (TTC < 1.6 && TTC >= 0.6) {
braking_ = true;
std::thread([this]() {
control_.sendControlCommand(0.0, 0.5, 0.0);
ROS_INFO("AEB triggered: 40%% braking");
std::this_thread::sleep_for(std::chrono::seconds(5));
}).detach();
}
else if (TTC < 0.6)
{
std::thread([this]() {
control_.sendControlCommand(0.0, 1.0, 0.0);
ROS_INFO("AEB triggered: 100%% braking");
std::this_thread::sleep_for(std::chrono::seconds(5));
}).detach();
}
// if (TTC<2.6&&TTC>=1.6)
// {
// control_.sendControlCommand(0.0, 0.4, 0.0);
// ROS_INFO("AEB triggered: Warning");
// //ROS_INFO("TTC: %f, phi1:%f, phi2:%f, v1:%f, a1:%f, v2:%f, a2:%f, Drel:%f, dbr:%f",TTC, phi1, phi2, v1, a1, v2, a2, Drel, dbr);
// }
// else if (TTC<1.6&&TTC>=0.6)
// {
// control_.sendControlCommand(0.0, 0.5, 0.0);
// ROS_INFO("AEB triggered: 40%% braking");
// //ROS_INFO("TTC: %f, phi1:%f, phi2:%f, v1:%f, a1:%f, v2:%f, a2:%f, Drel:%f, dbr:%f",TTC, phi1, phi2, v1, a1, v2, a2, Drel, dbr);
// }
// else if (TTC<0.6)
// {
// control_.sendControlCommand(0.0, 1.0, 0.0);
// ROS_INFO("AEB triggered: 100%% braking");
// //ROS_INFO("TTC: %f, phi1:%f, phi2:%f, v1:%f, a1:%f, v2:%f, a2:%f, Drel:%f, dbr:%f",TTC, phi1, phi2, v1, a1, v2, a2, Drel, dbr);
// ROS_INFO("TTC: %f, phi1:%f, phi2:%f, v1:%f, a1:%f, v2:%f, a2:%f, Drel:%f, dbr:%f",TTC, phi1, phi2, v1, a1, v2, a2, Drel, dbr);
// }
}
// private:
ros::Publisher aeb_pub_;
// ros::Publisher aeb_control_pub_;
SensorData& sensor_data_;
Control& control_;
const double d0 = 3.0; // 安全停车距离,一般取2m或3m
bool braking_; // 用于跟踪当前制动状态
// 计算TTC的函数
double calculateTTC(double v1, double a1, double v2, double a2, double Drel)
{
//double vrel = -closest_object.v_xrel; //直接获取
// double vrel = v2 - v1;
const common_msgs::CICV_Location& current_pose = sensor_data_.getCurrentPose();
const common_msgs::Perceptionobjects& obstacles = sensor_data_.getObstacles();
if (obstacles.Perceptionobjects.empty())
{
ROS_WARN("No perception objects available");
return 0;
}
const common_msgs::Perceptionobject& closest_object = obstacles.Perceptionobjects[0]; // 假设第一个是最近的物体
double vrel = -closest_object.v_x; //
double arel = a2 - a1;
double TTC;
if (arel == 0)
{
if (vrel == 0)
{
vrel = 0.000000001;
}
else
{
vrel = -closest_object.v_x;
}
TTC = (Drel-d0) / vrel;
TTC = std::abs(TTC);
return TTC;
}
else
{
if (vrel == 0)
{
vrel = 0.000000001;
}
else
{
vrel = -closest_object.v_x;
}
TTC = (vrel - sqrt(vrel * vrel + 2 * arel * (Drel - d0))) / arel;
TTC = std::abs(TTC);
return TTC;
}
// std::cout<<"这里计算TTC"<<std::endl;
// ROS_INFO("TTC: %f",TTC);
}
// // AEB系统触发逻辑
// std::string AEBSystemLogic(double TTC) {
// if (TTC > 2.6) {
// return "AEB not triggered";
// ROS_INFO("AEB not triggered");
// } else if (TTC > 1.6&&TTC < 2.6) {
// return "AEB triggered: Warning";
// ROS_INFO("AEB triggered: Warning");
// } else if (TTC > 0.6&&TTC < 1.6) {
// return "AEB triggered: 40% braking";
// ROS_INFO("AEB triggered: 40% braking");
// } else {
// return "AEB triggered: 100% braking";
// ROS_INFO("AEB triggered: 100% braking");
// }
// }
// Seungwuk Moon模型
double calculateSafetyDistance(double v2,double v1)
{
const double Tdelay = 1.2;
const double f_mu = 0.2;
const double amax = 6.0;
const common_msgs::CICV_Location& current_pose = sensor_data_.getCurrentPose();
const common_msgs::Perceptionobjects& obstacles = sensor_data_.getObstacles();
if (obstacles.Perceptionobjects.empty())
{
ROS_WARN("No perception objects available");
return 0;
}
const common_msgs::Perceptionobject& closest_object = obstacles.Perceptionobjects[0]; // 假设第一个是最近的物体
double vel = -closest_object.v_x;
double dbr = vel * Tdelay + f_mu *((2*v2 - vel) / (2 * amax)); //安全模型计算公式
// double dbr = (v2-v1) * Tdelay + f_mu *((2*v2 - (v2-v1)) / (2 * amax)); //安全模型计算公式
return dbr;
}
void maintainBraking(Control& control, double throttle, double brake, double steer_angle, double gear, int duration) {
ros::Rate rate(10);
for (int i = 0; i < duration * 10; ++i) {
control.sendControlCommand(throttle, brake, steer_angle, gear);
rate.sleep();
}
}
};
int main(int argc, char **argv) {
ros::init(argc, argv, "aeb_system_node");
ros::NodeHandle nh;
SensorData sensor_data(nh);
Control control(nh);
AEBSystem aeb_system(nh, sensor_data,control);
std::cout << "ok,begin" << std::endl;
ros::Rate rate(10); // 10 Hz
while (ros::ok()) {
ros::spinOnce(); // 处理回调函数
aeb_system.checkAEB(); // 检查AEB触发逻辑
rate.sleep();
}
return 0;
}
说明:
calculateTTC
函数计算碰撞时间TTC。AEBSystem
函数根据TTC值决定AEB触发类型。calculateSafetyDistance
函数根据Seungwuk Moon模型计算安全距离。main
函数初始化ROS节点并处理传感器输入数据,通过计算TTC和安全距离来触发相应的AEB动作。
这个程序是一个基本框架,你可能需要根据实际数据格式和ROS消息类型做相应调整。
#include <ros/ros.h>
#include <common_msgs/CICV_Location.h>
#include <common_msgs/Lane.h>
#include <common_msgs/Lanes.h>
#include <common_msgs/Perceptionobjects.h>
#include <common_msgs/Perceptionobject.h>
#include <std_msgs/String.h>
#include <cmath>
class SensorData {
public:
SensorData(ros::NodeHandle& nh) {
// 初始化订阅器
pose_subscriber = nh.subscribe("/cicv_location", 10, &SensorData::poseStationCallback, this);
lane_detection_subscriber = nh.subscribe("/LaneDetection", 10, &SensorData::detectionLanesCallback, this);
perception_subscriber = nh.subscribe("/tpperception", 10, &SensorData::obstacleCallback, this);
}
common_msgs::CICV_Location getCurrentPose() const {
return current_pose;
}
common_msgs::Perceptionobjects getObstacles() const {
return obstacles;
}
private:
ros::Subscriber pose_subscriber;
ros::Subscriber lane_detection_subscriber;
ros::Subscriber perception_subscriber;
common_msgs::CICV_Location current_pose;
common_msgs::Perceptionobjects obstacles;
// 处理车辆位置信息的回调函数
void poseStationCallback(const common_msgs::CICV_Location::ConstPtr& msg) {
current_pose = *msg;
ROS_INFO("Header: %d, Frame ID: %s", current_pose.header.seq, current_pose.header.frame_id.c_str());
ROS_INFO("Received pose: x=%8.8f, y=%8.8f, vx=%8.8f, vy=%8.8f, ax=%8.8f, ay=%8.8f, phi=%8.8f, yaw=%8.8f",
current_pose.Position_x, current_pose.Position_y, current_pose.Velocity_x,
current_pose.Velocity_y, current_pose.Accel_x, current_pose.Accel_y,
current_pose.Angular_velocity_z, current_pose.Yaw);
}
// 处理车道信息的回调函数
void detectionLanesCallback(const common_msgs::Lanes::ConstPtr& msg) {
common_msgs::Lanes lanes = *msg;
ROS_INFO("Header: %d, Frame ID: %s", lanes.header.seq, lanes.header.frame_id.c_str());
ROS_INFO("number of lanes:%d", lanes.num);
for (size_t i = 0; i < lanes.lanes.size(); ++i) {
const common_msgs::Lane& lane = lanes.lanes[i];
ROS_INFO("lane_idx:%d, CO:%.8f, C1:%.8f, C2:%.8f, C3:%.8f",
lane.lane_idx, lane.c_0, lane.c_1, lane.c_2, lane.c_3);
}
}
// 处理障碍物信息的回调函数
void obstacleCallback(const common_msgs::Perceptionobjects::ConstPtr& msg) {
obstacles = *msg;
ROS_INFO("Header: %d, Frame ID: %s", obstacles.header.seq, obstacles.header.frame_id.c_str());
ROS_INFO("number of obstacles: %d", obstacles.num);
for (size_t i = 0; i < obstacles.Perceptionobjects.size(); ++i) {
const common_msgs::Perceptionobject& object = obstacles.Perceptionobjects[i];
ROS_INFO("Object ID: %d, X: %.8f, Y: %.8f, vx: %.8f, vy: %.8f, xg:%.8f, yg:%.8f,"
"vxg:%.8f, vyg:%.8f, heading:%.8f, length: %.8f, width: %.8f, height: %.8f ",
object.ID, object.x, object.y, object.v_x, object.v_y, object.xg, object.yg,
object.v_xg, object.v_yg, object.heading, object.length, object.width, object.height);
}
}
};
class AEBSystem {
public:
AEBSystem(ros::NodeHandle& nh, SensorData& sensor_data) : sensor_data_(sensor_data) {
// 初始化发布器
aeb_pub_ = nh.advertise<std_msgs::String>("aeb_status", 10);
}
void checkAEB() {
const common_msgs::CICV_Location& current_pose = sensor_data_.getCurrentPose();
const common_msgs::Perceptionobjects& obstacles = sensor_data_.getObstacles();
if (obstacles.Perceptionobjects.size() > 0) {
const common_msgs::Perceptionobject& closest_object = obstacles.Perceptionobjects[0]; // 假设第一个是最近的物体
double v1 = current_pose.Velocity_x; // 自车速度
double a1 = current_pose.Accel_x; // 自车加速度
double v2 = closest_object.v_x; // 前车速度
double a2 = 0; // 假设前车加速度未知或为0
double Drel = sqrt(pow(closest_object.x - current_pose.Position_x, 2) +
pow(closest_object.y - current_pose.Position_y, 2)); // 相对距离
double TTC = calculateTTC(v1, a1, v2, a2, Drel);
std_msgs::String status_msg;
status_msg.data = AEBSystemLogic(TTC);
aeb_pub_.publish(status_msg);
double safety_distance = calculateSafetyDistance(v2);
if (Drel <= safety_distance) {
status_msg.data = "AEB triggered: Safety Distance Breached";
aeb_pub_.publish(status_msg);
}
}
}
private:
ros::Publisher aeb_pub_;
SensorData& sensor_data_;
const double d0 = 3.0; // 安全停车距离,一般取2m或3m
// 计算TTC的函数
double calculateTTC(double v1, double a1, double v2, double a2, double Drel) {
double vrel = v2 - v1;
double arel = a2 - a1;
if (arel == 0) {
return Drel / vrel;
} else {
return (vrel - sqrt(vrel * vrel - 2 * arel * (Drel - d0))) / arel;
}
}
// AEB系统触发逻辑
std::string AEBSystemLogic(double TTC) {
if (TTC > 2.6) {
return "AEB not triggered";
} else if (TTC > 1.6) {
return "AEB triggered: Warning";
} else if (TTC > 0.6) {
return "AEB triggered: 40% braking";
} else {
return "AEB triggered: 100% braking";
}
}
// Seungwuk Moon模型
double calculateSafetyDistance(double v2) {
const double Tdelay = 1.2;
const double f_mu = 0.2;
const double amax = 6.0;
return v2 * Tdelay + (v2 * v2) / (2 * f_mu * amax);
}
};
int main(int argc, char **argv) {
ros::init(argc, argv, "aeb_system_node");
ros::NodeHandle nh;
SensorData sensor_data(nh);
AEBSystem aeb_system(nh, sensor_data);
ros::Rate rate(10); // 10 Hz
while (ros::ok()) {
ros::spinOnce(); // 处理回调函数
aeb_system.checkAEB(); // 检查AEB触发逻辑
rate.sleep();
}
return 0;
}