四轮转向（4WS, Four-Wheel Steering）与阿克曼转向（Ackerman Steering）在结构和设计理念上有显著区别，以下是它们的主要不同点：
四轮转向是一种车辆转向系统，允许前后轮都能进行转向。它通过机械、液压或电子控制系统，使后轮根据前轮的转向角度和车辆速度进行相应的角度调整。4WS 的目标是提升车辆在不同速度下的操控性、低速时的灵活性和高速时的稳定性。
Ackerman（阿克曼转向）：
阿克曼转向是一种几何设计，主要应用于前轮转向车辆。它通过特定的连杆机构（通常是梯形连杆）确保在转弯时，内侧前轮的转向角度大于外侧前轮，以适应内外轮不同的转弯半径，从而减少轮胎侧滑和磨损。阿克曼设计最初是为低速转向优化的。
为了对比两种结构在路径跟踪效果，通过实现以下几部分功能对比两种结构的控制效果；
该代码的主要功能是实现阿克曼（Ackermann）和四轮四转（Four-Wheel Steering）两种车辆模型的路径跟踪仿真，并对比它们的性能。参考以下论文实现两种结构的控制：Steering Control Strategies for a Four-Wheel-Independent-Steering Bin Managing Robot - ScienceDirect
参考代码：
#include <iostream>
#include <vector>
#include <cmath>
#include <algorithm>
#include <map>
#include "matplotlibcpp.h"
namespace plt = matplotlibcpp;
// 参数定义
const double k = 0.1; // 前视距离增益，用于计算前视距离 Lf = k * v + Lfc
const double Lfc = 2.0; // 基础前视距离（单位：米）
const double Kp = 1.0; // PID 控制的比例增益
const double dt = 0.1; // 时间步长（单位：秒）
const double WB = 2.0; // 车辆轴距（单位：米）
const double max_lat_acc = 1.5; // 最大横向加速度（单位：m/s²）
const double initial_target_speed = 10.0 / 3.6; // 初始目标速度（单位：m/s，10 km/h 转换为 m/s）
const bool show_animation = true; // 是否显示动画
// 阿克曼模型状态类
class AckermannState {
public:
double x, y, yaw, v, rear_x, rear_y; // x, y: 车辆中心位置；yaw: 航向角；v: 速度；rear_x, rear_y: 后轮位置
AckermannState(double x_ = 0.0, double y_ = 0.0, double yaw_ = 0.0, double v_ = 0.0)
: x(x_), y(y_), yaw(yaw_), v(v_) {
updateRear(); // 初始化时更新后轮位置
}
// 更新车辆状态（运动学模型）
void update(double a, double delta) {
// 运动学公式：
// x(t+dt) = x(t) + v * cos(yaw) * dt
// y(t+dt) = y(t) + v * sin(yaw) * dt
// yaw(t+dt) = yaw(t) + (v / WB) * tan(delta) * dt
// v(t+dt) = v(t) + a * dt
x += v * std::cos(yaw) * dt; // 更新 X 坐标
y += v * std::sin(yaw) * dt; // 更新 Y 坐标
yaw += v / WB * std::tan(delta) * dt; // 更新航向角
v += a * dt; // 更新速度
updateRear(); // 更新后轮位置
}
// 计算后轮到某点的欧几里得距离
double calcDistance(double point_x, double point_y) const {
double dx = rear_x - point_x;
double dy = rear_y - point_y;
return std::hypot(dx, dy); // 距离公式：sqrt(dx² + dy²)
}
private:
// 更新后轮位置
void updateRear() {
// 后轮位置公式：
// rear_x = x - (WB / 2) * cos(yaw)
// rear_y = y - (WB / 2) * sin(yaw)
rear_x = x - (WB / 2) * std::cos(yaw);
rear_y = y - (WB / 2) * std::sin(yaw);
}
};
// 四轮四转模型状态类
class FourWheelState {
public:
double x, y, yaw, v, rear_x, rear_y; // 同上
double delta_f, delta_r; // 前轮转向角和后轮转向角
FourWheelState(double x_ = 0.0, double y_ = 0.0, double yaw_ = 0.0, double v_ = 0.0)
: x(x_), y(y_), yaw(yaw_), v(v_), delta_f(0.0), delta_r(0.0) {
updateRear();
}
// 更新车辆状态（四轮四转运动学模型）
void update(double a, double delta_f_new, double delta_r_new) {
delta_f = delta_f_new; // 更新前轮转向角
delta_r = delta_r_new; // 更新后轮转向角
// 运动学公式：
// x(t+dt) = x(t) + v * cos(yaw) * dt
// y(t+dt) = y(t) + v * sin(yaw) * dt
// yaw(t+dt) = yaw(t) + (v / WB) * (tan(delta_f) - tan(delta_r)) * dt
// v(t+dt) = v(t) + a * dt
x += v * std::cos(yaw) * dt; // 更新 X 坐标
y += v * std::sin(yaw) * dt; // 更新 Y 坐标
yaw += v / WB * (std::tan(delta_f) - std::tan(delta_r)) * dt; // 更新航向角
v += a * dt; // 更新速度
updateRear(); // 更新后轮位置
}
// 计算后轮到某点的欧几里得距离
double calcDistance(double point_x, double point_y) const {
double dx = rear_x - point_x;
double dy = rear_y - point_y;
return std::hypot(dx, dy); // 距离公式：sqrt(dx² + dy²)
}
private:
// 更新后轮位置
void updateRear() {
rear_x = x - (WB / 2) * std::cos(yaw);
rear_y = y - (WB / 2) * std::sin(yaw);
}
};
// 状态历史记录类（包含横向误差）
class States {
public:
std::vector<double> x, y, yaw, v, t, lateral_error; // 记录位置、航向、速度、时间和横向误差
// 添加阿克曼模型状态
void append(double time, const AckermannState& state, double error) {
x.push_back(state.x);
y.push_back(state.y);
yaw.push_back(state.yaw);
v.push_back(state.v);
t.push_back(time);
lateral_error.push_back(error);
}
// 添加四轮四转模型状态
void append(double time, const FourWheelState& state, double error) {
x.push_back(state.x);
y.push_back(state.y);
yaw.push_back(state.yaw);
v.push_back(state.v);
t.push_back(time);
lateral_error.push_back(error);
}
};
// PID速度控制（比例控制）
double proportionalControl(double target, double current) {
// 比例控制公式：a = Kp * (target - current)
return Kp * (target - current);
}
// 阿克曼模型最大速度
double calcMaxSpeedAckermann(double delta) {
if (std::fabs(delta) < 1e-5) return initial_target_speed; // 转向角接近 0 时返回初始速度
// 转弯半径公式：R = WB / tan(delta)
// 最大速度公式：v_max = sqrt(max_lat_acc * |R|)
double R = WB / std::tan(delta);
return std::sqrt(max_lat_acc * std::fabs(R));
}
// 四轮四转模型最大速度
double calcMaxSpeedFourWheel(double delta_f, double delta_r) {
if (std::fabs(delta_f) < 1e-5 && std::fabs(delta_r) < 1e-5) return initial_target_speed;
// 转弯半径公式：R = WB / (tan(delta_f) - tan(delta_r))
// 最大速度公式：v_max = sqrt(max_lat_acc * |R|)
double R = WB / (std::tan(delta_f) - std::tan(delta_r));
return std::sqrt(max_lat_acc * std::fabs(R));
}
// 计算横向误差（车辆中心到路径的垂直距离）
double calcLateralError(double center_x, double center_y, const std::vector<double>& cx, const std::vector<double>& cy, int nearest_ind) {
if (nearest_ind < 0 || nearest_ind >= static_cast<int>(cx.size())) return 0.0;
// 处理边界情况
int prev_ind = std::max(0, nearest_ind - 1); // 前一个点索引
int next_ind = std::min(static_cast<int>(cx.size()) - 1, nearest_ind + 1); // 后一个点索引
// 计算路径段的切线方向（由前后两点确定）
double dx = cx[next_ind] - cx[prev_ind];
double dy = cy[next_ind] - cy[prev_ind];
double path_length = std::hypot(dx, dy); // 路径段长度：sqrt(dx² + dy²)
if (path_length < 1e-6) return 0.0; // 避免除零
// 路径切线的单位向量
double tx = dx / path_length; // tx = dx / path_length
double ty = dy / path_length; // ty = dy / path_length
// 车辆中心到最近点的向量
double vx = center_x - cx[nearest_ind]; // vx = center_x - cx[nearest_ind]
double vy = center_y - cy[nearest_ind]; // vy = center_y - cy[nearest_ind]
// 计算横向误差（点到直线的距离）
// 叉积公式：cross_product = vx * ty - vy * tx
double cross_product = vx * ty - vy * tx; // 带符号的垂直距离
return cross_product; // 正值表示右侧偏差，负值表示左侧偏差
}
// 目标路径类
class TargetCourse {
public:
std::vector<double> cx, cy; // 目标路径的 X 和 Y 坐标
int old_nearest_point_index; // 上一次最近点的索引
TargetCourse(const std::vector<double>& cx_, const std::vector<double>& cy_)
: cx(cx_), cy(cy_), old_nearest_point_index(-1) {}
// 搜索目标点索引（阿克曼模型）
std::pair<int, double> searchTargetIndex(const AckermannState& state) {
int ind;
if (old_nearest_point_index == -1) { // 初次搜索最近点
std::vector<double> d;
for (size_t i = 0; i < cx.size(); ++i) {
double dx = state.rear_x - cx[i];
double dy = state.rear_y - cy[i];
d.push_back(std::hypot(dx, dy)); // 计算后轮到各点的距离
}
ind = std::min_element(d.begin(), d.end()) - d.begin(); // 找到最近点索引
old_nearest_point_index = ind;
} else {
ind = old_nearest_point_index;
double distance_this_index = state.calcDistance(cx[ind], cy[ind]);
while (true) {
if (ind + 1 >= static_cast<int>(cx.size())) break;
double distance_next_index = state.calcDistance(cx[ind + 1], cy[ind + 1]);
if (distance_this_index < distance_next_index) break; // 找到局部最近点
ind++;
distance_this_index = distance_next_index;
}
old_nearest_point_index = ind;
}
// 计算前视距离：Lf = k * v + Lfc
double Lf = k * state.v + Lfc;
int target_ind = ind;
while (Lf > state.calcDistance(cx[target_ind], cy[target_ind])) { // 找到前视点
if (target_ind + 1 >= static_cast<int>(cx.size())) break;
target_ind++;
}
return {target_ind, Lf}; // 返回目标点索引和前视距离
}
// 搜索目标点索引（四轮四转模型）
std::pair<int, double> searchTargetIndex(const FourWheelState& state) {
int ind;
if (old_nearest_point_index == -1) {
std::vector<double> d;
for (size_t i = 0; i < cx.size(); ++i) {
double dx = state.rear_x - cx[i];
double dy = state.rear_y - cy[i];
d.push_back(std::hypot(dx, dy));
}
ind = std::min_element(d.begin(), d.end()) - d.begin();
old_nearest_point_index = ind;
} else {
ind = old_nearest_point_index;
double distance_this_index = state.calcDistance(cx[ind], cy[ind]);
while (true) {
if (ind + 1 >= static_cast<int>(cx.size())) break;
double distance_next_index = state.calcDistance(cx[ind + 1], cy[ind + 1]);
if (distance_this_index < distance_next_index) break;
ind++;
distance_this_index = distance_next_index;
}
old_nearest_point_index = ind;
}
double Lf = k * state.v + Lfc;
int target_ind = ind;
while (Lf > state.calcDistance(cx[target_ind], cy[target_ind])) {
if (target_ind + 1 >= static_cast<int>(cx.size())) break;
target_ind++;
}
return {target_ind, Lf};
}
int getNearestIndex(const AckermannState& state) {
return old_nearest_point_index; // 获取最近点索引
}
int getNearestIndex(const FourWheelState& state) {
return old_nearest_point_index;
}
};
// 阿克曼模型Pure Pursuit控制
std::pair<double, int> purePursuitSteerControlAckermann(const AckermannState& state, TargetCourse& trajectory, int pind) {
std::pair<int, double> target_info = trajectory.searchTargetIndex(state);
int ind = target_info.first;
double Lf = target_info.second;
if (pind >= ind) ind = pind; // 确保目标点不会回退
double tx = (ind < static_cast<int>(trajectory.cx.size())) ? trajectory.cx[ind] : trajectory.cx.back(); // 目标点 X 坐标
double ty = (ind < static_cast<int>(trajectory.cy.size())) ? trajectory.cy[ind] : trajectory.cy.back(); // 目标点 Y 坐标
// 计算偏差角：alpha = atan2(ty - rear_y, tx - rear_x) - yaw
double alpha = std::atan2(ty - state.rear_y, tx - state.rear_x) - state.yaw;
// Pure Pursuit 转向角公式：delta = atan2(2 * WB * sin(alpha) / Lf, 1)
double delta = std::atan2(2.0 * WB * std::sin(alpha) / Lf, 1.0);
return {delta, ind}; // 返回转向角和目标点索引
}
// 四轮四转模型Pure Pursuit控制
std::tuple<double, double, int> purePursuitSteerControlFourWheel(const FourWheelState& state, TargetCourse& trajectory, int pind) {
std::pair<int, double> target_info = trajectory.searchTargetIndex(state);
int ind = target_info.first;
double Lf = target_info.second;
if (pind >= ind) ind = pind;
double tx = (ind < static_cast<int>(trajectory.cx.size())) ? trajectory.cx[ind] : trajectory.cx.back();
double ty = (ind < static_cast<int>(trajectory.cy.size())) ? trajectory.cy[ind] : trajectory.cy.back();
double alpha = std::atan2(ty - state.rear_y, tx - state.rear_x) - state.yaw;
double delta = std::atan2(2.0 * WB * std::sin(alpha) / Lf, 1.0);
// 前后轮转向角分配（绝对值相等，反向）
double delta_f = delta; // 前轮转向角
double delta_r = -delta; // 后轮转向角
return {delta_f, delta_r, ind}; // 返回前轮转向角、后轮转向角和目标点索引
};
// 绘制车辆箭头
void plotArrow(double x, double y, double yaw, double length = 1.0, double width = 0.5) {
std::vector<double> x_vec = {x, x + length * std::cos(yaw)}; // 箭头 X 坐标
std::vector<double> y_vec = {y, y + length * std::sin(yaw)}; // 箭头 Y 坐标
plt::plot(x_vec, y_vec, "r-"); // 绘制箭身
plt::plot({x}, {y}, "ro"); // 绘制箭头起点
}
// 主模拟函数
void mainSimulation() {
std::vector<double> cx, cy;
// 生成目标路径（正弦曲线）
for (double i = 0; i < 50; i += 0.5) {
cx.push_back(i);
cy.push_back(std::sin(i / 5.0) * i / 2.0); // y = sin(x/5) * x/2
}
// 阿克曼模型初始化
AckermannState ackermann_state(-0.0, -3.0, 0.0, 0.0); // 初始位置 (-0, -3)，航向 0，速度 0
States ackermann_states;
TargetCourse ackermann_course(cx, cy);
int ackermann_target_ind = ackermann_course.searchTargetIndex(ackermann_state).first;
double ackermann_error = calcLateralError(ackermann_state.x, ackermann_state.y, cx, cy, ackermann_course.getNearestIndex(ackermann_state));
ackermann_states.append(0.0, ackermann_state, ackermann_error);
// 四轮四转模型初始化
FourWheelState fourwheel_state(-0.0, -3.0, 0.0, 0.0);
States fourwheel_states;
TargetCourse fourwheel_course(cx, cy);
int fourwheel_target_ind = fourwheel_course.searchTargetIndex(fourwheel_state).first;
double fourwheel_error = calcLateralError(fourwheel_state.x, fourwheel_state.y, cx, cy, fourwheel_course.getNearestIndex(fourwheel_state));
fourwheel_states.append(0.0, fourwheel_state, fourwheel_error);
int lastIndex = cx.size() - 1; // 路径最后一个点索引
double time = 0.0; // 仿真时间
const double T = 100.0; // 最大仿真时间
// 主仿真循环
while (T >= time && (lastIndex > ackermann_target_ind || lastIndex > fourwheel_target_ind)) {
// 阿克曼模型仿真
if (lastIndex > ackermann_target_ind) {
auto [delta, new_target_ind] = purePursuitSteerControlAckermann(ackermann_state, ackermann_course, ackermann_target_ind);
ackermann_target_ind = new_target_ind;
double max_speed = calcMaxSpeedAckermann(delta); // 计算最大允许速度
double target_speed = std::min(initial_target_speed, max_speed); // 选择较小值作为目标速度
double ai = proportionalControl(target_speed, ackermann_state.v); // 计算加速度
ackermann_state.update(ai, delta); // 更新状态
double error = calcLateralError(ackermann_state.x, ackermann_state.y, cx, cy, ackermann_course.getNearestIndex(ackermann_state));
ackermann_states.append(time, ackermann_state, error); // 记录状态
}
// 四轮四转模型仿真
if (lastIndex > fourwheel_target_ind) {
auto [delta_f, delta_r, new_target_ind] = purePursuitSteerControlFourWheel(fourwheel_state, fourwheel_course, fourwheel_target_ind);
fourwheel_target_ind = new_target_ind;
double max_speed = calcMaxSpeedFourWheel(delta_f, delta_r);
double target_speed = std::min(initial_target_speed, max_speed);
double ai = proportionalControl(target_speed, fourwheel_state.v);
fourwheel_state.update(ai, delta_f, delta_r);
double error = calcLateralError(fourwheel_state.x, fourwheel_state.y, cx, cy, fourwheel_course.getNearestIndex(fourwheel_state));
fourwheel_states.append(time, fourwheel_state, error);
}
time += dt; // 时间递增
// 实时动画
if (show_animation) {
plt::cla(); // 清除当前图形
plotArrow(ackermann_state.x, ackermann_state.y, ackermann_state.yaw); // 绘制阿克曼车辆
plotArrow(fourwheel_state.x, fourwheel_state.y, fourwheel_state.yaw); // 绘制四轮四转车辆
plt::plot(cx, cy, {{"label", "course"}, {"color", "red"}, {"linestyle", "-"}}); // 绘制目标路径
plt::plot(ackermann_states.x, ackermann_states.y, {{"label", "Ackermann"}, {"color", "blue"}, {"linestyle", "-"}}); // 绘制阿克曼轨迹
plt::plot(fourwheel_states.x, fourwheel_states.y, {{"label", "FourWheel"}, {"color", "green"}, {"linestyle", "-"}}); // 绘制四轮四转轨迹
plt::plot({cx[ackermann_target_ind]}, {cy[ackermann_target_ind]}, {{"label", "target"}, {"marker", "x"}, {"color", "black"}}); // 绘制目标点
plt::axis("equal"); // 坐标轴等比例
plt::grid(true); // 显示网格
plt::legend(); // 显示图例
plt::title("Ackermann Speed[km/h]: " + std::to_string(ackermann_state.v * 3.6).substr(0, 4) +
" | FourWheel Speed[km/h]: " + std::to_string(fourwheel_state.v * 3.6).substr(0, 4)); // 设置标题
plt::pause(0.001); // 暂停以显示动画
}
}
// 最终结果对比
if (show_animation) {
// 轨迹对比图
plt::figure();
plt::plot(cx, cy, {{"label", "course"}, {"color", "red"}, {"marker", "."}});
plt::plot(ackermann_states.x, ackermann_states.y, {{"label", "Ackermann"}, {"color", "blue"}, {"linestyle", "-"}});
plt::plot(fourwheel_states.x, fourwheel_states.y, {{"label", "FourWheel"}, {"color", "green"}, {"linestyle", "-"}});
plt::legend();
plt::xlabel("x[m]");
plt::ylabel("y[m]");
plt::axis("equal");
plt::grid(true);
plt::title("Trajectory Comparison");
// 速度对比图
plt::figure();
std::vector<double> ackermann_speed_kmh(ackermann_states.v.size());
std::vector<double> fourwheel_speed_kmh(fourwheel_states.v.size());
for (size_t i = 0; i < ackermann_states.v.size(); ++i) ackermann_speed_kmh[i] = ackermann_states.v[i] * 3.6; // 转换为 km/h
for (size_t i = 0; i < fourwheel_states.v.size(); ++i) fourwheel_speed_kmh[i] = fourwheel_states.v[i] * 3.6;
plt::plot(ackermann_states.t, ackermann_speed_kmh, {{"label", "Ackermann"}, {"color", "blue"}, {"linestyle", "-"}});
plt::plot(fourwheel_states.t, fourwheel_speed_kmh, {{"label", "FourWheel"}, {"color", "green"}, {"linestyle", "-"}});
plt::legend();
plt::xlabel("Time[s]");
plt::ylabel("Speed[km/h]");
plt::grid(true);
plt::title("Speed Comparison");
// 横向误差对比图
plt::figure();
plt::plot(ackermann_states.t, ackermann_states.lateral_error, {{"label", "Ackermann"}, {"color", "blue"}, {"linestyle", "-"}});
plt::plot(fourwheel_states.t, fourwheel_states.lateral_error, {{"label", "FourWheel"}, {"color", "green"}, {"linestyle", "-"}});
plt::legend();
plt::xlabel("Time[s]");
plt::ylabel("Lateral Error[m] (Perpendicular Distance)");
plt::grid(true);
plt::title("Lateral Error Comparison (Center to Path)");
plt::show(); // 显示所有图形
}
}
int main() {
std::cout << "Ackermann vs Four-wheel steering path tracking simulation with perpendicular lateral error start" << std::endl;
mainSimulation();
return 0;
}对比结果：
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
