多机编队—(3)Fast_planner无人机模型替换为Turtlebot3模型实现无地图的轨迹规划

文章目录

前言
一、模型替换
二、Riz可视化
三、坐标变换
四、轨迹规划
最后

前言

前段时间已经成功将Fast_planner配置到ubuntu机器人中，这段时间将Fast_planner中的无人机模型替换为了Turtlebot3_waffle模型，机器人识别到环境中的三维障碍物信息，并完成无地图的轨迹规划。

一、模型替换

先把turtlebot3功能包 拷贝到配置成功的Fast_planner工作空间 下，turtlebot3中含有turtlebot3_description功能包，里面包含了turtlebot3所有的模型文件。

普通在gazebo中加载turtlebot3的模型文件如图所示：

但Fast_planner需要环境中的三维信息，以进行路径的规划，故这里需要将2D激光雷达替换为3D激光雷达，如下所示在turtlebot3_waffle.urdf.xacro文件中加入VLP-16激光雷达，以获取环境中的三维信息。

cpp 复制代码

<?xml version="1.0" ?>
<robot name="turtlebot3_waffle" xmlns:xacro="http://ros.org/wiki/xacro">

  <xacro:include filename="$(find turtlebot3_description)/urdf/common_properties.xacro"/>
  <xacro:include filename="$(find turtlebot3_description)/urdf/turtlebot3_waffle.gazebo.xacro"/>

  <xacro:property name="support_length" value="0.3" />      <!--0.3-->

   <xacro:arg name="gpu" default="false"/>
    <xacro:property name="gpu" value="$(arg gpu)" />
    <xacro:arg name="organize_cloud" default="true"/>
    <xacro:property name="organize_cloud" value="$(arg organize_cloud)" />

   <xacro:property name="base_z_size" value="0.122" />        <!--0.122-->
   <xacro:include filename="$(find turtlebot3_description)/urdf/sensors/inertial.xacro" />
   <xacro:include filename="$(find turtlebot3_description)/urdf/sensors/laser_support.xacro" />
    <xacro:include filename="$(find velodyne_description)/urdf/VLP-16.urdf.xacro"/>
    <xacro:VLP-16 parent="support" name="velodyne" topic="/velodyne_points" organize_cloud="${organize_cloud}" hz="10" samples="400" gpu="${gpu}">
          <origin xyz="0 0 ${support_length/4}" rpy="0 0 0" /> 
    </xacro:VLP-16>

    <xacro:include filename="$(find turtlebot3_description)/urdf/sensors/imu.xacro"/>
    <xacro:imu sensor_name="imu" parent_link="base_link">
    <origin rpy="0 0 0" xyz="-0.064 0 ${base_z_size/2}"/> 
    </xacro:imu>

其中的关键点为（上面代码也已展现出）：需要将相关的模型加载文件放在设置的路径下。

然后在gazebo中加载新模型：

二、Riz可视化

cpp 复制代码

<launch>
  <node name="rvizvisualisation" pkg="rviz" type="rviz" output="log" args="-d $(find plan_manage)/config/traj.rviz" />
</launch>

三、坐标变换

在模型加装VLP-16激光雷达时设置了雷达的话题：topic="/velodyne_points"，为使雷达能得到地图中障碍物信息。

写下面的一段代码实现订阅激光雷达（如Velodyne）产生的点云数据，并将这些点云从base_link 的坐标系转换到odom坐标系。转换后的点云数据再发布出去，以供其他节点使用。

c 复制代码

#include <ros/ros.h>
#include <sensor_msgs/PointCloud.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
#include <tf2/transform_datatypes.h>

ros::Publisher points_pub;
tf2_ros::Buffer tfBuffer;

void pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr &msg) {

    try {
        geometry_msgs::TransformStamped transformStamped = tfBuffer.lookupTransform("odom", "base_link", ros::Time(0));

        sensor_msgs::PointCloud2 transformed_cloud;
        tf2::doTransform(*msg, transformed_cloud, transformStamped);

        transformed_cloud.header.frame_id = "odom";
        points_pub.publish(transformed_cloud);
    } catch(tf2::TransformException &e) {
        ROS_WARN("Failed to transform point cloud: %s", e.what());
    }

}

int main(int argc, char **argv) 
{
    ros::init(argc, argv, "point_cloud_transform_node");
    ros::NodeHandle nh;
    ros::Subscriber points_sub = nh.subscribe("/velodyne_points", 10, pointCloudCallback);

    points_pub = nh.advertise<sensor_msgs::PointCloud2>("point_cloud_map", 10);

    tf2_ros::TransformListener tfListener(tfBuffer);
    ros::spin();

    return 0;
}

四、轨迹规划

该部分借助Fast_planner来实现，前端通过混合A*生成初始路径，后端在对其进行B样条优化和重规划。

为使Turtlebot3使用下面是配置的接口文件：

c 复制代码

<launch>
  <!-- size of map, change the size in x, y, z according to your application -->
  <arg name="map_size_x" value="40.0"/>
  <arg name="map_size_y" value="40.0"/>
  <arg name="map_size_z" value="0.5 "/>

  <!-- topic of your odometry such as VIO or LIO -->
  <arg name="odom_topic" value="/odom" />
  <!-- main algorithm params -->
  <include file="$(find plan_manage)/launch/lidar.xml">
    <arg name="map_size_x_" value="$(arg map_size_x)"/>
    <arg name="map_size_y_" value="$(arg map_size_y)"/>
    <arg name="map_size_z_" value="$(arg map_size_z)"/>
    <arg name="odometry_topic" value="$(arg odom_topic)"/>

    <!-- camera pose: transform of camera frame in the world frame -->
    <!-- depth topic: depth image, 640x480 by default -->
    <!-- don't set cloud_topic if you already set these ones! -->
    <arg name="camera_pose_topic" value="/pcl_render_node/camera_pose"/><!--geometry_msgs::PoseStamped-->
    <arg name="depth_topic" value="/pcl_render_node/depth"/>

    <!-- topic of point cloud measurement, such as from LIDAR  -->
    <!-- don't set camera pose and depth, if you already set this one! -->
    <arg name="cloud_topic" value="/point_cloud_map"/>

    <!-- intrinsic params of the depth camera -->
    <arg name="cx" value="321.04638671875"/>
    <arg name="cy" value="243.44969177246094"/>
    <arg name="fx" value="387.229248046875"/>
    <arg name="fy" value="387.229248046875"/>

    <!-- maximum velocity and acceleration the drone will reach -->
    <arg name="max_vel" value="3" />
    <arg name="max_acc" value="2" />

    <!-- 1: use 2D Nav Goal to select goal  -->
    <!-- 2: use global waypoints below  -->
    <arg name="flight_type" value="1" />
    
    <!-- global waypoints -->
    <!-- If flight_type is set to 2, the drone will travel these waypoints one by one -->
    <arg name="point_num" value="3" />

    <arg name="point0_x" value="2" />
    <arg name="point0_y" value="0" />
    <arg name="point0_z" value="0" />

    <!-- set more waypoints if you need -->
    <arg name="point1_x" value="2" />
    <arg name="point1_y" value="-5" />
    <arg name="point1_z" value="0" />

    <arg name="point2_x" value="5.0" />
    <arg name="point2_y" value="-10.0" />
    <arg name="point2_z" value="0.0" />
    
  </include>

  <!-- trajectory server -->
  <node pkg="plan_manage" name="traj_server" type="traj_server" output="screen">
    <remap from="/position_cmd" to="/planning/pos_cmd"/>

    <remap from="/odom_world" to="$(arg odom_topic)"/>
    <param name="traj_server/time_forward" value="1.5" type="double"/>
  </node>

  <node pkg="waypoint_generator" name="waypoint_generator" type="waypoint_generator" output="screen">
    <remap from="~odom" to="$(arg odom_topic)"/>        
    <remap from="~goal" to="/move_base_simple/goal"/>
    <remap from="~traj_start_trigger" to="/traj_start_trigger" />
    <param name="waypoint_type" value="manual-lonely-waypoint"/>    
  </node>
 
</launch>

lidar.xml

cpp 复制代码

<launch>
  <arg name="map_size_x_"/>
  <arg name="map_size_y_"/>
  <arg name="map_size_z_"/>

  <arg name="odometry_topic"/>
  <arg name="camera_pose_topic"/>
  <arg name="depth_topic"/>
  <arg name="cloud_topic"/>

  <arg name="cx"/>
  <arg name="cy"/>
  <arg name="fx"/>
  <arg name="fy"/>

  <arg name="max_vel"/>
  <arg name="max_acc"/>

  <arg name="point_num"/>
  <arg name="point0_x"/>
  <arg name="point0_y"/>
  <arg name="point0_z"/>
  <arg name="point1_x"/>
  <arg name="point1_y"/>
  <arg name="point1_z"/>
  <arg name="point2_x"/>
  <arg name="point2_y"/>
  <arg name="point2_z"/>

  <arg name="flight_type"/>

  <!-- main node -->
  <node pkg="plan_manage" name="fast_planner_node" type="fast_planner_node" output="screen">
    <remap from="/odom_world" to="$(arg odometry_topic)"/>
    <remap from="/sdf_map/odom" to="$(arg odometry_topic)"/>
    <remap from="/sdf_map/cloud" to="$(arg cloud_topic)"/>
    <remap from = "/sdf_map/pose"   to = "$(arg camera_pose_topic)"/> 
    <remap from = "/sdf_map/depth" to = "$(arg depth_topic)"/>

    <!-- replanning method -->
    <param name="planner_node/planner" value="1" type="int"/>

    <!-- planning fsm -->
    <param name="fsm/flight_type" value="$(arg flight_type)" type="int"/>
    <param name="fsm/thresh_replan" value="1.5" type="double"/>
    <param name="fsm/thresh_no_replan" value="1.0" type="double"/>

    <param name="fsm/waypoint_num" value="$(arg point_num)" type="int"/>
    <param name="fsm/waypoint0_x" value="$(arg point0_x)" type="double"/>
    <param name="fsm/waypoint0_y" value="$(arg point0_y)" type="double"/>
    <param name="fsm/waypoint0_z" value="$(arg point0_z)" type="double"/>
    <param name="fsm/waypoint1_x" value="$(arg point1_x)" type="double"/>
    <param name="fsm/waypoint1_y" value="$(arg point1_y)" type="double"/>
    <param name="fsm/waypoint1_z" value="$(arg point1_z)" type="double"/>
    <param name="fsm/waypoint2_x" value="$(arg point2_x)" type="double"/>
    <param name="fsm/waypoint2_y" value="$(arg point2_y)" type="double"/>
    <param name="fsm/waypoint2_z" value="$(arg point2_z)" type="double"/>

    <param name="sdf_map/resolution"      value="0.15" /> 
    <param name="sdf_map/map_size_x"   value="$(arg map_size_x_)" /> 
    <param name="sdf_map/map_size_y"   value="$(arg map_size_y_)" /> 
    <param name="sdf_map/map_size_z"   value="$(arg map_size_z_)" /> 
    <param name="sdf_map/local_update_range_x"  value="5.5" /> 
    <param name="sdf_map/local_update_range_y"  value="5.5" /> 
    <param name="sdf_map/local_update_range_z"  value="4.5" /> 
    <param name="sdf_map/obstacles_inflation"     value="0.3" /> <!--<膨胀的半径  单位米>-->
    <param name="sdf_map/local_bound_inflate"    value="0.0"/>
    <param name="sdf_map/local_map_margin" value="10"/>
    <param name="sdf_map/ground_height"        value="0"/>
    <!-- camera parameter -->
    <param name="sdf_map/cx" value="$(arg cx)"/>
    <param name="sdf_map/cy" value="$(arg cy)"/>
    <param name="sdf_map/fx" value="$(arg fx)"/>
    <param name="sdf_map/fy" value="$(arg fy)"/>
    <!-- depth filter -->
    <param name="sdf_map/use_depth_filter" value="false"/>
    <param name="sdf_map/depth_filter_tolerance" value="0.15"/>
    <param name="sdf_map/depth_filter_maxdist"   value="5.0"/><!--5.0-->
    <param name="sdf_map/depth_filter_mindist"   value="0.2"/>
    <param name="sdf_map/depth_filter_margin"    value="2"/>
    <param name="sdf_map/k_depth_scaling_factor" value="1000.0"/>
    <param name="sdf_map/skip_pixel" value="2"/>
    <!-- local fusion -->
    <param name="sdf_map/p_hit"  value="0.65"/>
    <param name="sdf_map/p_miss" value="0.35"/>
    <param name="sdf_map/p_min"  value="0.12"/>
    <param name="sdf_map/p_max"  value="0.90"/>
    <param name="sdf_map/p_occ"  value="0.80"/>
    <param name="sdf_map/min_ray_length" value="0.3"/>
    <param name="sdf_map/max_ray_length" value="5.0"/>

    <param name="sdf_map/esdf_slice_height" value="0.3"/>
    <param name="sdf_map/visualization_truncate_height"   value="2.49"/>
    <param name="sdf_map/virtual_ceil_height"   value="2.5"/>
    <param name="sdf_map/show_occ_time"  value="false"/>
    <param name="sdf_map/show_esdf_time" value="false"/>
    <param name="sdf_map/pose_type"     value="1"/>  
    <param name="sdf_map/frame_id"      value="odom"/>

  <!-- planner manager -->
    <param name="manager/max_vel" value="$(arg max_vel)" type="double"/>
    <param name="manager/max_acc" value="$(arg max_acc)" type="double"/>
    <param name="manager/max_jerk" value="4" type="double"/>
    <param name="manager/dynamic_environment" value="0" type="int"/>
    <param name="manager/local_segment_length" value="12.0" type="double"/>
    <param name="manager/clearance_threshold" value="0.2" type="double"/>
    <param name="manager/control_points_distance" value="0.5" type="double"/>

    <param name="manager/use_geometric_path" value="false" type="bool"/>
    <param name="manager/use_kinodynamic_path" value="true" type="bool"/>
    <param name="manager/use_topo_path" value="false" type="bool"/>
    <param name="manager/use_optimization" value="true" type="bool"/>

  <!-- kinodynamic path searching -->
    <param name="search/max_tau" value="1.0" type="double"/><!--如果考虑对时间维度进行划分才设置-->
    <param name="search/init_max_tau" value="0.8" type="double"/>
    <param name="search/max_vel" value="$(arg max_vel)" type="double"/>
    <param name="search/max_acc" value="$(arg max_acc)" type="double"/>
    <param name="search/w_time" value="10.0" type="double"/>
    <param name="search/horizon" value="5.0" type="double"/><!--限制全局规划的距离，保证实时性-->
    <param name="search/lambda_heu" value="4.0" type="double"/><!--启发函数权重-->
    <param name="search/resolution_astar" value="0.1" type="double"/><!--空间分辨率-->
    <param name="search/time_resolution" value="0.8" type="double"/><!--时间维度分辨率-->
    <param name="search/margin" value="0.2" type="double"/><!--检测碰撞-->
    <param name="search/allocate_num" value="100000" type="int"/><!--最大节点数目-->
    <param name="search/check_num" value="5" type="int"/><!--对中间状态安全检查-->

  <!-- trajectory optimization -->
    <param name="optimization/lambda1" value="10.0" type="double"/>
    <param name="optimization/lambda2" value="5.0" type="double"/>
    <param name="optimization/lambda3" value="0.00001" type="double"/>
    <param name="optimization/lambda4" value="0.01" type="double"/>
    <param name="optimization/lambda7" value="100.0" type="double"/><!-- reduces 'optimization/lambda7' to make the yaw changes slower-->
    <param name="optimization/dist0" value="0.4" type="double"/>
    <param name="optimization/max_vel" value="$(arg max_vel)" type="double"/>
    <param name="optimization/max_acc" value="$(arg max_acc)" type="double"/>

    <param name="optimization/algorithm1" value="15" type="int"/>
    <param name="optimization/algorithm2" value="11" type="int"/>

    <param name="optimization/max_iteration_num1" value="2" type="int"/>
    <param name="optimization/max_iteration_num2" value="300" type="int"/>
    <param name="optimization/max_iteration_num3" value="200" type="int"/>
    <param name="optimization/max_iteration_num4" value="200" type="int"/>

    <param name="optimization/max_iteration_time1" value="0.0001" type="double"/>
    <param name="optimization/max_iteration_time2" value="0.005" type="double"/>
    <param name="optimization/max_iteration_time3" value="0.003" type="double"/>
    <param name="optimization/max_iteration_time4" value="0.003" type="double"/>

    <param name="optimization/order" value="3" type="int"/>

    <param name="bspline/limit_vel" value="$(arg max_vel)" type="double"/>
    <param name="bspline/limit_acc" value="$(arg max_acc)" type="double"/>
    <param name="bspline/limit_ratio" value="1.1" type="double"/>

    <param name="bspline/limit_vel" value="$(arg max_vel)" type="double"/>
    <param name="bspline/limit_acc" value="$(arg max_acc)" type="double"/>
    <param name="bspline/limit_ratio" value="1.1" type="double"/>

  </node>
</launch>

可能会出现规划出的路径实际位置与目标期望位置存在偏差的情况：

Fast_planner规划期望位置与实际位置偏差-解决前

这是由于OneShot中检查边界出现问题，令其返回了false，即认为最后寻找的目标点是不可达的，上一节点可达，故此时规划目标位置接近目标点但不是目标点。
sdf_map.cpp

cpp 复制代码

  node_.param("sdf_map/map_size_x", x_size, -1.0);
  node_.param("sdf_map/map_size_y", y_size, -1.0);
  node_.param("sdf_map/map_size_z", z_size, -1.0);

kinodynamic_astar.cpp

cpp 复制代码

if (coord(0) < origin_(0) || coord(0) >= map_size_3d_(0) || coord(1) < origin_(1) || coord(1) >= map_size_3d_(1) 
     || coord(2) < origin_(2)  || coord(2) >= map_size_3d_(2))
    {
      return false;
    }

在进行OneShot时边界约束判断当前点是否存在一条路经直到终点，因为coord(2)小车z轴信息为-0.000981506， origin(2)为0，小车z值为负一直小于origin(2)，则会一直满足判断条件返回false，故无法到目标点。

如果仍有问题，请检查其他的约束条件。

由当前点寻找到终点的最优路径简单证明思路如下（感兴趣可以自己推一下）：

最后

Fast_planner规划期望目标位置与实际位置-解决后