迷你无人车 Navigation 导航（3）

迷你无人车 Navigation 导航（3）

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

修改 URDF 文件

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

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

<transmission name="front_steer_right_joint_trans">
  <type>transmission_interface/SimpleTransmissiontype>
  <joint name="front_steer_right_joint">
    <hardwareInterface>hardware_interface/EffortJointInterfacehardwareInterface>
  joint>
  <actuator name="front_steer_right_joint_motor">
    <hardwareInterface>hardware_interface/EffortJointInterfacehardwareInterface>
    <mechanicalReduction>1mechanicalReduction>
  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" />     
    <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">
      
      <robotNamespace>/steer_neor_minirobotNamespace>
      <robotSimType>gazebo_ros_control/DefaultRobotHWSimrobotSimType>
      <legacyModeNS>truelegacyModeNS>
    plugin>
  gazebo>

注意命名空间的设置

配置 yaml 文件

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

steer_neor_mini:
  joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50

 # Velocity Controllers ----速度控制器---------------------
  left_rear_wheel_velocity_controller:
    type: effort_controllers/JointVelocityController
    joint: left_rear_joint
    pid: {p: 1.0, i: 0.0, d: 0.0, i_clamp: 0.0}
  right_rear_wheel_velocity_controller:
    type: effort_controllers/JointVelocityController
    joint: right_rear_joint
    pid: {p: 1.0, i: 0.0, d: 0.0, i_clamp: 0.0}
  left_front_wheel_velocity_controller:
    type: effort_controllers/JointVelocityController
    joint: front_left_wheel_joint
    pid: {p: 1.0, i: 0.0, d: 0.0, i_clamp: 0.0}
  right_front_wheel_velocity_controller:
    type: effort_controllers/JointVelocityController
    joint: front_right_wheel_joint
    pid: {p: 1.0, i: 0.0, d: 0.0, i_clamp: 0.0}

  # Position Controllers ---位置控制器-----------------------
  left_steering_hinge_position_controller:
    joint: front_steer_left_joint
    type: effort_controllers/JointPositionController
    pid: {p: 0.5, i: 0.0, d: 0.0}
  right_steering_hinge_position_controller:
    joint: front_steer_right_joint
    type: effort_controllers/JointPositionController
    pid: {p: 0.5, i: 0.0, d: 0.0}

配置 launch 文件

<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_controller
                                    left_rear_wheel_velocity_controller right_rear_wheel_velocity_controller
                                    left_front_wheel_velocity_controller right_front_wheel_velocity_controller
                                    left_steering_hinge_position_controller right_steering_hinge_position_controller"/>

    
    
    <include file="$(find gazebo_ros)/launch/empty_world.launch" >
        <arg name="world_name" value="$(find neor_mini)/worlds/cooneo_office.world"/>
    include>

    
    <param name="robot_description" command="cat $(find neor_mini)/urdf/neor_mini_gazebo_sensors.urdf"/>

    
    
    
    <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/my_controller.yaml" command="load"/>

    
    
    <node pkg="controller_manager" type="spawner" name="controller_spawner" output="screen" respawn="false" 
        ns="/steer_neor_mini" args="$(arg controllers)"/>

    
    <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>

    
    
        
        
        
        
        
    

    
    <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 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#include 
#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;
}

话题组织

完整的话题组织如下，可以通过 Logitech 手柄控制 Gazebo 中的迷你无人车以阿克曼转向方式运动

实际效果如下

下一步计划修改 URDF 模型进行四轮转向和原地转向控制

参考

阿克曼结构移动机器人的gazebo仿真（五）

阿克曼结构移动机器人的gazebo仿真（六）

https://wiki.ros.org/urdf/XML/Transmission

https://wiki.ros.org/urdf/XML/joint

探究–gazebo里 关节是如何动起来的____实现默认插件joint控制