迷你无人车 Navigation 导航（3）

自己实现了对于迷你无人车关节的控制，由于原本的关节布置仅支持阿克曼转向，因此先进行阿克曼转向的控制

修改 URDF 文件

添加 transmission 标签，定义关节的驱动

<transmission name="right_rear_joint_trans"><type>transmission_interface/SimpleTransmission</type><joint name="right_rear_joint"><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></joint><actuator name="right_rear_joint_motor"><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface><mechanicalReduction>1</mechanicalReduction></actuator></transmission>

<transmission name="front_steer_right_joint_trans"><type>transmission_interface/SimpleTransmission</type><joint name="front_steer_right_joint"><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface></joint><actuator name="front_steer_right_joint_motor"><hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface><mechanicalReduction>1</mechanicalReduction></actuator>
</transmission>

注意位置控制关节中 limit 的设置，一开始均设置为 0，无法进行位置控制

<joint name="front_steer_right_joint" type="revolute"><origin rpy="0 0 0"  xyz="0.17 -0.21 0.102" /><parent link="base_link" /><child link="front_steer_right_link" /><axis xyz="0 0 1" />     <!-- charge steer direction default is 0 0 1--><limit lower="-0.69" upper="0.69" effort="2.0" velocity="1.0" /></joint>

配置 ros_control 插件

<gazebo><plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so"><!-- <robotSimType>steer_bot_hardware_gazebo/SteerBotHardwareGazebo</robotSimType><legacyModeNS>false</legacyModeNS> --><robotNamespace>/steer_neor_mini</robotNamespace><robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType><legacyModeNS>true</legacyModeNS></plugin></gazebo>

注意命名空间的设置

配置 yaml 文件

在该文件中添加各关节控制器类型（速度控制/位置控制），以及控制器的 PID 系数

steer_neor_mini:joint_state_controller:type: joint_state_controller/JointStateControllerpublish_rate: 50# Velocity Controllers ----速度控制器---------------------left_rear_wheel_velocity_controller:type: effort_controllers/JointVelocityControllerjoint: left_rear_jointpid: {p: 1.0, i: 0.0, d: 0.0, i_clamp: 0.0}right_rear_wheel_velocity_controller:type: effort_controllers/JointVelocityControllerjoint: right_rear_jointpid: {p: 1.0, i: 0.0, d: 0.0, i_clamp: 0.0}left_front_wheel_velocity_controller:type: effort_controllers/JointVelocityControllerjoint: front_left_wheel_jointpid: {p: 1.0, i: 0.0, d: 0.0, i_clamp: 0.0}right_front_wheel_velocity_controller:type: effort_controllers/JointVelocityControllerjoint: front_right_wheel_jointpid: {p: 1.0, i: 0.0, d: 0.0, i_clamp: 0.0}# Position Controllers ---位置控制器-----------------------left_steering_hinge_position_controller:joint: front_steer_left_jointtype: effort_controllers/JointPositionControllerpid: {p: 0.5, i: 0.0, d: 0.0}right_steering_hinge_position_controller:joint: front_steer_right_jointtype: effort_controllers/JointPositionControllerpid: {p: 0.5, i: 0.0, d: 0.0}

配置 launch 文件

<?xml version="1.0"?>
<launch><arg name="x" default="0.0"/><arg name="y" default="0.0"/><arg name="z" default="0.0" /><arg name="roll" default="0.0"/><arg name="pitch" default="0.0"/><arg name="yaw" default="0.0"/><arg name="controllers" default="joint_state_controllerleft_rear_wheel_velocity_controller right_rear_wheel_velocity_controllerleft_front_wheel_velocity_controller right_front_wheel_velocity_controllerleft_steering_hinge_position_controller right_steering_hinge_position_controller"/><!-- Gazebo  --><!--Load the surrounding environment into Gazebo--><include file="$(find gazebo_ros)/launch/empty_world.launch" ><arg name="world_name" value="$(find neor_mini)/worlds/cooneo_office.world"/></include><!-- Load the robot description --><param name="robot_description" command="cat $(find neor_mini)/urdf/neor_mini_gazebo_sensors.urdf"/><!-- Load ros_controllers configuration parameters --><!-- <rosparam file="$(find steer_mini_gazebo)/mini_control/config/ctrl_ackermann_steering_controller.yaml" command="load"   /> --><!-- <rosparam file="$(find steer_mini_gazebo)/mini_control/config/ctrl_gains.yaml" command="load"   /> --><rosparam file="$(find steer_mini_gazebo)/mini_control/config/ctrl_joint_state_publisher.yaml" command="load"   /><!-- <rosparam file="$(find steer_mini_gazebo)/mini_control/config/ctrl_steer_bot_hardware_gazebo.yaml" command="load"   /> --><rosparam file="$(find steer_mini_gazebo)/mini_control/config/my_controller.yaml" command="load"/><!-- Spawn the controllers --><!-- <node pkg="controller_manager" type="spawner" name="controller_spawner"  args="joint_state_publisher ackermann_steering_controller" output="screen" respawn="false" /> --><node pkg="controller_manager" type="spawner" name="controller_spawner" output="screen" respawn="false" ns="/steer_neor_mini" args="$(arg controllers)"/><!-- Launch  the robot state publisher --><node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"><param name="publish_frequency" value="50.0"/><remap from="/joint_states" to="/steer_neor_mini/joint_states"/></node><!-- Launch a rqt steering GUI for publishing to /steer_bot/steer_drive_controller/cmd_vel --><!-- <node pkg="rqt_robot_steering" type="rqt_robot_steering" name="rqt_robot_steering" > --><!-- <param name="default_topic" value="ackermann_steering_controller/cmd_vel"/>                         default velocity control topic name --><!-- <param name="default_vx_max" value="1.0"/>                        linear velocity max value    m/s --><!-- <param name="default_vx_min" value="-1.0"/>                       linear velocity min value    m/s --><!-- <param name="default_vw_max" value="0.69"/>                    angular velocity max value  rad/s (adaptor for urdf joint limit) --><!-- <param name="default_vw_min" value="-0.69"/>                   angular velocity min value  rad/s (adaptor for urdf joint limit) --><!-- </node> --><!-- Start the Gazebo node and configure the location and posture of the accident when the model is loaded --><node name="spawn_vehicle" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model neor_mini -gazebo_namespace /gazebo -x $(arg x) -y $(arg y) -z $(arg z)-R $(arg roll) -P $(arg pitch) -Y $(arg yaw)"respawn="false" output="screen" /><!-- ******************************************************************************************************************************************* --><node name="laser_to_base_link" pkg="tf" type="static_transform_publisher" args="0.0 0.0 0.135 0 0 0 base_link laser_link 40 " /><node name="imu_to_base_link" pkg="tf" type="static_transform_publisher" args="-0.10 0.0 0.02 0 0 0 base_link imu_link 40 " /><node name="camera_to_base_link" pkg="tf" type="static_transform_publisher" args="0.12 0.0 0.12 0 0 0 base_link camera_link 40 " /></launch>

💡 注意各文件中命名空间的一致性

车轮速度解算

沿用四轮车的解算代码，更新车长、车宽以及车轮直径参数，目前只进行阿克曼转向的解算控制

#include <cmd_to_neor/wheelparams.h>
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <std_msgs/Float64.h>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <cmath>
#include <iostream>
#include <mutex>
#include "myTools.hpp"
using namespace std;
using namespace Eigen;ros::Publisher pub_vel_left_rear_wheel;
ros::Publisher pub_vel_right_rear_wheel;
ros::Publisher pub_vel_left_front_wheel;
ros::Publisher pub_vel_right_front_wheel;
ros::Publisher pub_pos_left_steering_hinge;
ros::Publisher pub_pos_right_steering_hinge;
cmd_to_neor::wheelparams w_params;
AidTool::Ptr aidtool;Matrix<double, 8, 3> coff_mat; // 机器人模型参数矩阵
Matrix<double, 3, 1> xyw;      // 机器人Vx， Vy， W向量
Matrix<double, 8, 1> p_xyw;// 机器人长的一半
const double half_len = 0.3492 / 2;
// 机器人宽的一半
const double half_wid = 0.36 / 2;
const double WHELL_DIA = 0.0625;
const double REDUCER_RATIO = 1.0;mutex mut;void computeWheelVel_pivotSteer();
void computeWheelVel_ackermannSteer(double linear_vel, double front, double rear);
double computeAckermannRatio(double vel);std_msgs::Float64 vel_left_rear_wheel;
std_msgs::Float64 vel_right_rear_wheel;
std_msgs::Float64 vel_left_front_wheel;
std_msgs::Float64 vel_right_front_wheel;
std_msgs::Float64 pos_left_steering_hinge;
std_msgs::Float64 pos_right_steering_hinge;double toRealVel(const double vel){return vel / (M_PI * WHELL_DIA) * REDUCER_RATIO;
}double toReaPos(const double pos){return pos * 180 / M_PI * REDUCER_RATIO;
}void robot_callback(const geometry_msgs::Twist::ConstPtr &msg)
{memset(&w_params, 0, sizeof(w_params));doublelinear_vel = msg->linear.x;double front_angle = msg->angular.z;computeWheelVel_ackermannSteer(linear_vel, front_angle, 0);vel_left_rear_wheel.data = toRealVel(w_params.lb_v);vel_right_rear_wheel.data = toRealVel(w_params.rb_v);vel_left_front_wheel.data = toRealVel(w_params.lf_v);vel_right_front_wheel.data = toRealVel(w_params.rf_v);pos_left_steering_hinge.data = w_params.lf_p;pos_right_steering_hinge.data = w_params.rf_p;cout << "lr_v:" << vel_left_rear_wheel.data << ";rr_v:" << vel_right_rear_wheel.data << \";lf_v:" << vel_left_front_wheel.data << ";rf_v:" << vel_right_front_wheel.data << endl;cout << "lf_p:" << pos_left_steering_hinge.data << ";rf_p:" << pos_right_steering_hinge.data << endl;cout << "--------------" << endl; lock_guard<mutex> lock(mut);pub_vel_left_rear_wheel.publish(vel_left_rear_wheel);pub_vel_right_rear_wheel.publish(vel_right_rear_wheel);pub_vel_left_front_wheel.publish(vel_left_front_wheel);pub_vel_right_front_wheel.publish(vel_right_front_wheel);pub_pos_left_steering_hinge.publish(pos_left_steering_hinge);pub_pos_right_steering_hinge.publish(pos_right_steering_hinge);mut.unlock();
}int main(int argc, char **argv)
{coff_mat << 1, 0, -half_wid,0, 1, half_len,1, 0, -half_wid,0, 1, -half_len,1, 0, half_wid,0, 1, -half_len,1, 0, half_wid,0, 1, half_len;const string vel_left_rear_wheel_topic = "/steer_neor_mini/left_rear_wheel_velocity_controller/command";const string vel_right_rear_wheel_topic = "/steer_neor_mini/right_rear_wheel_velocity_controller/command";const string vel_left_front_wheel_topic = "/steer_neor_mini/left_front_wheel_velocity_controller/command";const string vel_right_front_wheel_topic = "/steer_neor_mini/right_front_wheel_velocity_controller/command";const string pos_left_steering_hinge_topic = "/steer_neor_mini/left_steering_hinge_position_controller/command";const string pos_right_steering_hinge_topic = "/steer_neor_mini/right_steering_hinge_position_controller/command";aidtool = make_shared<AidTool>();ros::init(argc, argv, "cmd_to_neor");ros::NodeHandle nh;pub_vel_left_rear_wheel = nh.advertise<std_msgs::Float64>(vel_left_rear_wheel_topic, 1);pub_vel_right_rear_wheel = nh.advertise<std_msgs::Float64>(vel_right_rear_wheel_topic, 1);pub_vel_left_front_wheel = nh.advertise<std_msgs::Float64>(vel_left_front_wheel_topic, 1);pub_vel_right_front_wheel = nh.advertise<std_msgs::Float64>(vel_right_front_wheel_topic, 1);pub_pos_left_steering_hinge = nh.advertise<std_msgs::Float64>(pos_left_steering_hinge_topic, 1);pub_pos_right_steering_hinge = nh.advertise<std_msgs::Float64>(pos_right_steering_hinge_topic, 1);ros::Subscriber sub = nh.subscribe("/cmd_vel", 1, robot_callback);ros::spin();return 0;
}void computeWheelVel_pivotSteer()
{lock_guard<mutex> lock(mut);// 左前if (p_xyw(0, 0) == 0){if (p_xyw(1, 0) > 0){w_params.lf_p = M_PI_2;}else if (p_xyw(1, 0) < 0){w_params.lf_p = -M_PI_2;}else{w_params.lf_p = 0;}}else{w_params.lf_p = atan(p_xyw(1, 0) / p_xyw(0, 0));}if (p_xyw(0, 0) == 0.0 && p_xyw(1, 0) == 0.0){w_params.lf_v = 0.0;}else if (p_xyw(0, 0) == 0){w_params.lf_v = p_xyw(1, 0) / sin(w_params.lf_p);}else{w_params.lf_v = p_xyw(0, 0) / cos(w_params.lf_p);}// 左后if (p_xyw(2, 0) == 0){if (p_xyw(3, 0) > 0){w_params.lb_p = M_PI_2;}else if (p_xyw(3, 0) < 0){w_params.lb_p = -M_PI_2;}else{w_params.lb_p = 0;}}else{w_params.lb_p = atan(p_xyw(3, 0) / p_xyw(2, 0));}if (p_xyw(2, 0) == 0.0 && p_xyw(3, 0) == 0.0){w_params.lb_v = 0.0;}else if (p_xyw(2, 0) == 0){w_params.lb_v = p_xyw(3, 0) / sin(w_params.lb_p);}else{w_params.lb_v = p_xyw(2, 0) / cos(w_params.lb_p);}// 右后if (p_xyw(4, 0) == 0){if (p_xyw(5, 0) > 0){w_params.rb_p = M_PI_2;}else if (p_xyw(5, 0) < 0){w_params.rb_p = -M_PI_2;}else{w_params.rb_p = 0;}}else{w_params.rb_p = atan(p_xyw(5, 0) / p_xyw(4, 0));}if (p_xyw(4, 0) == 0.0 && p_xyw(5, 0) == 0.0){w_params.rb_v = 0.0;}else if (p_xyw(4, 0) == 0){w_params.rb_v = p_xyw(5, 0) / sin(w_params.rb_p);}else{w_params.rb_v = p_xyw(4, 0) / cos(w_params.rb_p);}// 右前if (p_xyw(6, 0) == 0){if (p_xyw(7, 0) > 0){w_params.rf_p = M_PI_2;}else if (p_xyw(7, 0) < 0){w_params.rf_p = -M_PI_2;}else{w_params.rf_p = 0;}}else{w_params.rf_p = atan(p_xyw(7, 0) / p_xyw(6, 0));}if (p_xyw(6, 0) == 0.0 && p_xyw(7, 0) == 0.0){w_params.rf_v = 0.0;}else if (p_xyw(6, 0) == 0){w_params.rf_v = p_xyw(7, 0) / sin(w_params.rf_p);}else{w_params.rf_v = p_xyw(6, 0) / cos(w_params.rf_p);}mut.unlock();
}void computeWheelVel_ackermannSteer(double linear_vel, double front, double rear)
{lock_guard<mutex> lock(mut);// cout << "front_angle: " << aidtool->rad2deg(front) << "\t rear_angle: " << aidtool->rad2deg(rear) << endl;double d_vertical = 0.0, l_up = half_len;if (front != 0){d_vertical = 2 * half_len * cos(front) * cos(rear) / sin(front - rear);l_up = 2 * half_len * sin(front) * cos(rear) / sin(front - rear);// cout << "d_vertical: " << d_vertical << "\tl_up: " << l_up << endl;w_params.lf_p = atan(2 * half_len * sin(front) * cos(rear) / (2 * half_len * cos(front) * cos(rear) - half_wid * sin(front - rear)));w_params.rf_p = atan(2 * half_len * sin(front) * cos(rear) / (2 * half_len * cos(front) * cos(rear) + half_wid * sin(front - rear)));w_params.lb_p = atan(2 * half_len * cos(front) * sin(rear) / (2 * half_len * cos(front) * cos(rear) - half_wid * sin(front - rear)));w_params.rb_p = atan(2 * half_len * cos(front) * sin(rear) / (2 * half_len * cos(front) * cos(rear) + half_wid * sin(front - rear)));}double Rc = sqrt(pow(half_len - l_up, 2) + pow(d_vertical, 2));if (Rc == 0){w_params.lf_v = linear_vel;w_params.rf_v = linear_vel;w_params.lb_v = linear_vel;w_params.rb_v = linear_vel;}else{double Rlf = sqrt(pow(l_up, 2) + pow(d_vertical - half_wid, 2));double Rrf = sqrt(pow(l_up, 2) + pow(d_vertical + half_wid, 2));double Rlr = sqrt(pow(2 * half_len - l_up, 2) + pow(d_vertical - half_wid, 2));double Rrr = sqrt(pow(2 * half_len - l_up, 2) + pow(d_vertical + half_wid, 2));double angular_vel = linear_vel / Rc;w_params.lf_v = angular_vel * Rlf;w_params.rf_v = angular_vel * Rrf;w_params.lb_v = angular_vel * Rlr;w_params.rb_v = angular_vel * Rrr;}mut.unlock();
}double computeAckermannRatio(double vel)
{double corner_stiffness = -100000.0;double mass = 250;lock_guard<mutex> lock(mut);double ratio = 1 - 4 * half_len * corner_stiffness / (2 * half_len * corner_stiffness - mass * pow(vel, 2));mut.unlock();return ratio;
}