在机器人开发中，四轮转向（Four-Wheel Steering，简称4WS）广泛应用于提高车辆的操控性能和运动灵活性。本文将详细介绍如何通过C++代码实现一个四轮转向控制器，并结合里程计数据提供精准的车辆位置信息反馈。
一、头文件与命名空间
我们需要包括几个关键头文件来处理数学计算、四轮转向控制、TF变换和URDF解析：
#include <cmath>
#include <four_wheel_steering_controller/four_wheel_steering_controller.h>
#include <pluginlib/class_list_macros.hpp>
#include <tf/transform_datatypes.h>
#include <urdf_geometry_parser/urdf_geometry_parser.h>
namespace four_wheel_steering_controller {
二、FourWheelSteeringController 类简介
FourWheelSteeringController 类是一个集成了四轮转向控制和里程计数据计算的控制器。它管理并控制四轮转向车辆的各个部分，包括转向联动、速度控制、里程计更新等。
1. 构造函数
构造函数初始化了一些重要参数和标志位：
FourWheelSteeringController::FourWheelSteeringController()
: command_struct_twist_()
, command_struct_four_wheel_steering_()
, track_(0.0)
, wheel_steering_y_offset_(0.0)
, wheel_radius_(0.0)
, wheel_base_(0.0)
, cmd_vel_timeout_(0.5)
, base_frame_id_("base_link")
, enable_odom_tf_(true)
, enable_twist_cmd_(false)
{
}
2. 初始化函数
初始化函数配置了一些必要参数、获取关节信息和设置里程计参数等：
bool FourWheelSteeringController::init(hardware_interface::RobotHW* robot_hw,
ros::NodeHandle& root_nh,
ros::NodeHandle& controller_nh)
{
// 处理命名空间和控制器名称
const std::string complete_ns = controller_nh.getNamespace();
std::size_t id = complete_ns.find_last_of("/");
name_ = complete_ns.substr(id + 1);
// 获取前后轮的名称
std::vector<std::string> front_wheel_names, rear_wheel_names;
if (!getWheelNames(controller_nh, "front_wheel", front_wheel_names) ||
!getWheelNames(controller_nh, "rear_wheel", rear_wheel_names))
{
return false;
}
// 检查车轮数量是否匹配且为2个
if (front_wheel_names.size() != rear_wheel_names.size() || front_wheel_names.size() != 2)
{
ROS_ERROR_STREAM_NAMED(name_, "前轮和后轮数量不匹配或不为2个。");
return false;
}
else
{
front_wheel_joints_.resize(front_wheel_names.size());
rear_wheel_joints_.resize(front_wheel_names.size());
}
// 获取前后转向关节名称
std::vector<std::string> front_steering_names, rear_steering_names;
if (!getWheelNames(controller_nh, "front_steering", front_steering_names) ||
!getWheelNames(controller_nh, "rear_steering", rear_steering_names))
{
return false;
}
// 检查转向关节数量是否匹配且为2个
if (front_steering_names.size() != rear_steering_names.size() || front_steering_names.size() != 2)
{
ROS_ERROR_STREAM_NAMED(name_, "前后转向关节数量不匹配或不为2个。");
return false;
}
else
{
front_steering_joints_.resize(front_steering_names.size());
rear_steering_joints_.resize(front_steering_names.size());
}
// 初始化里程计更新速率及其他参数
double publish_rate;
controller_nh.param("publish_rate", publish_rate, 50.0);
publish_period_ = ros::Duration(1.0 / publish_rate);
// 固定参数获取和里程计对象初始化
int velocity_rolling_window_size = 10;
controller_nh.param("velocity_rolling_window_size", velocity_rolling_window_size, velocity_rolling_window_size);
odometry_.setVelocityRollingWindowSize(velocity_rolling_window_size);
// 获取并设置命令超时时间及基础帧ID
controller_nh.param("cmd_vel_timeout", cmd_vel_timeout_, cmd_vel_timeout_);
controller_nh.param("base_frame_id", base_frame_id_, base_frame_id_);
controller_nh.param("enable_odom_tf", enable_odom_tf_, enable_odom_tf_);
// 获取车辆几何参数
bool lookup_track = !controller_nh.getParam("track", track_);
bool lookup_wheel_radius = !controller_nh.getParam("wheel_radius", wheel_radius_);
bool lookup_wheel_base = !controller_nh.getParam("wheel_base", wheel_base_);
urdf_geometry_parser::UrdfGeometryParser uvk(root_nh, base_frame_id_);
if (lookup_track)
if (!uvk.getDistanceBetweenJoints(front_wheel_names[0], front_wheel_names[1], track_))
return false;
else
controller_nh.setParam("track", track_);
if (!uvk.getDistanceBetweenJoints(front_steering_names[0], front_wheel_names[0], wheel_steering_y_offset_))
return false;
else
controller_nh.setParam("wheel_steering_y_offset", wheel_steering_y_offset_);
if (lookup_wheel_radius)
if (!uvk.getJointRadius(front_wheel_names[0], wheel_radius_))
return false;
else
controller_nh.setParam("wheel_radius", wheel_radius_);
if (lookup_wheel_base)
if (!uvk.getDistanceBetweenJoints(front_wheel_names[0], rear_wheel_names[0], wheel_base_))
return false;
else
controller_nh.setParam("wheel_base", wheel_base_);
// 设置里程计参数
odometry_.setWheelParams(track_ - 2 * wheel_steering_y_offset_, wheel_steering_y_offset_, wheel_radius_, wheel_base_);
// 其他初始化代码...
return true;
}
三、核心功能实现
1. 更新函数
update 函数是控制器的主要循环，调用 updateOdometry 和 updateCommand 分别更新里程计数据和执行控制指令：
void FourWheelSteeringController::update(const ros::Time& time, const ros::Duration& period)
{
updateOdometry(time);
updateCommand(time, period);
}
2. 更新里程计数据
updateOdometry 函数负责计算并发布车辆的里程计数据：
void FourWheelSteeringController::updateOdometry(const ros::Time& time)
{
const double fl_speed = front_wheel_joints_[0].getVelocity();
const double fr_speed = front_wheel_joints_[1].getVelocity();
const double rl_speed = rear_wheel_joints_[0].getVelocity();
const double rr_speed = rear_wheel_joints_[1].getVelocity();
if (std::isnan(fl_speed) || std::isnan(fr_speed) || std::isnan(rl_speed) || std::isnan(rr_speed))
return;
const double fl_steering = front_steering_joints_[0].getPosition();
const double fr_steering = front_steering_joints_[1].getPosition();
const double rl_steering = rear_steering_joints_[0].getPosition();
const double rr_steering = rear_steering_joints_[1].getPosition();
if (std::isnan(fl_steering) || std::isnan(fr_steering) || std::isnan(rl_steering) || std::isnan(rr_steering))
return;
double front_steering_pos = atan(2 * tan(fl_steering) * tan(fr_steering) / (tan(fl_steering) + tan(fr_steering)));
double rear_steering_pos = atan(2 * tan(rl_steering) * tan(rr_steering) / (tan(rl_steering) + tan(rr_steering)));
// 计算并更新里程计数据
odometry_.update(fl_speed, fr_speed, rl_speed, rr_speed, front_steering_pos, rear_steering_pos, time);
// 发布里程计消息
if (last_state_publish_time_ + publish_period_ < time)
{
last_state_publish_time_ += publish_period_;
// 计算并存储方向信息
geometry_msgs::Quaternion orientation(tf::createQuaternionMsgFromYaw(odometry_.getHeading()));
// 填充 odom 消息并发布
if (odom_pub_->trylock())
{
odom_pub_->msg_.header.stamp = time;
odom_pub_->msg_.pose.pose.position.x = odometry_.getX();
odom_pub_->msg_.pose.pose.position.y = odometry_.getY();
odom_pub_->msg_.pose.pose.orientation = orientation;
odom_pub_->msg_.twist.twist.linear.x = odometry_.getLinearX();
odom_pub_->msg_.twist.twist.linear.y = odometry_.getLinearY();
odom_pub_->msg_.twist.twist.angular.z = odometry_.getAngular();
odom_pub_->unlockAndPublish();
}
// 发布 tf /odom 帧
if (enable_odom_tf_ && tf_odom_pub_->trylock())
{
auto& odom_frame = tf_odom_pub_->msg_.transforms[0];
odom_frame.header.stamp = time;
odom_frame.transform.translation.x = odometry_.getX();
odom_frame.transform.translation.y = odometry_.getY();
odom_frame.transform.rotation = orientation;
tf_odom_pub_->unlockAndPublish();
}
}
}
3. 更新控制指令
updateCommand 函数根据收到的速度或转向指令更新车辆状态：
void FourWheelSteeringController::updateCommand(const ros::Time& time, const ros::Duration& period)
{
Command* cmd;
CommandTwist curr_cmd_twist = *(command_twist_.readFromRT());
Command4ws curr_cmd_4ws = *(command_four_wheel_steering_.readFromRT());
if (curr_cmd_4ws.stamp >= curr_cmd_twist.stamp)
{
cmd = &curr_cmd_4ws;
enable_twist_cmd_ = false;
}
else
{
cmd = &curr_cmd_twist;
enable_twist_cmd_ = true;
}
const double dt = (time - cmd->stamp).toSec();
if (dt > cmd_vel_timeout_)
{
curr_cmd_twist.lin_x = 0.0;
curr_cmd_twist.lin_y = 0.0;
curr_cmd_twist.ang = 0.0;
curr_cmd_4ws.lin = 0.0;
curr_cmd_4ws.front_steering = 0.0;
curr_cmd_4ws.rear_steering = 0.0;
}
double cmd_dt = period.toSec();
// 更新速度和转向指令
// 复杂的控制逻辑省略...
// 设置车轮和转向角度命令
if (front_wheel_joints_.size() == 2 && rear_wheel_joints_.size() == 2)
{
front_wheel_joints_[0].setCommand(vel_left_front);
front_wheel_joints_[1].setCommand(vel_right_front);
rear_wheel_joints_[0].setCommand(vel_left_rear);
rear_wheel_joints_[1].setCommand(vel_right_rear);
}
if (front_steering_joints_.size() == 2 && rear_steering_joints_.size() == 2)
{
front_steering_joints_[0].setCommand(front_left_steering);
front_steering_joints_[1].setCommand(front_right_steering);
rear_steering_joints_[0].setCommand(rear_left_steering);
rear_steering_joints_[1].setCommand(rear_right_steering);
}
}
四、总结
通过本文的介绍，我们了解了如何通过 C++ 代码实现一个四轮转向控制器，并结合里程计数据提供准确的车辆位置信息反馈。这个控制器不仅能够处理车辆的基本运动控制，还能实时更新并发布里程计数据，使得车辆在复杂环境中能够获得精确的定位信息。
四轮转向控制器的核心在于合理的参数初始化、精确的里程计数据计算以及实时的控制指令更新。希望本文能够对从事机器人和自动驾驶研究的工程师们有所帮助。
扫一扫
770
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
