navigation导航

导航算法及ROS实现

在下面我们将会详细介绍如何使用：基于雷达建图和navigation导航包，将imu、里程计、gps进行数据融合，实现导航和避障。

• navigation讲解
• navigation实战
• navigation进阶

您需要具备的知识：

• ROS基础知识
• 话题通讯、服务通讯、参数服务器、动作通讯
• 串口通讯
• c++基础知识

1. navigation

1.1 简介

navigation包是用于移动机器人路径规划和导航的核心包，它主要包括：

• 路径规划
• 路径跟踪
• 避障
• …

安装navigation包：

sudo apt-get install ros-<your_ros_distro>-navigation

其软件包：

• 定位：

  • amcl： Adaptive Monte Carlo Localization， 是一种基于粒子滤波的自适应定位算法，用于确定移动机器人的位置。它利用激光扫描和地图数据，通过对多个粒子的跟踪和更新，估计机器人在环境中的位置和方向。

• 全局规划器：

  • carrot_planner： 是一个简单的全局路径规划器，生成指向目标点的直线路径。虽然简单，但在某些情况下（例如无障碍环境）可能非常有效。
  • navfn： 是一个基于 Dijkstra 算法的全局路径规划器，用于生成从起点到目标点的最优路径，它使用代价地图来计算路径，适用于静态环境。

• 局部规划器：

  • dwa_local_planner：Dynamic Window Approach， 是一种动态窗口方法的局部路径规划器，它在速度空间中搜索，选择能够在给定时间窗口内避开障碍物并朝向目标的最佳速度命令。
  • teb_local_planner ：是一个更高级的局部路径规划器，基于时间弹性带（Time Elastic Band，TEB）算法。它不仅考虑了机器人的运动约束，还优化了整个路径的时间分布。
  • base_local_planner： 是一个用于局部路径规划的工具。它基于代价地图生成短期路径，并考虑了机器人运动模型和动态避障。该规划器生成速度命令，使机器人在避开障碍物的同时，朝着目标点移动。

• 恢复行为：

  • move_slow_and_clear： 是一种恢复行为，当机器人在导航过程中检测到前方有障碍物时，会减慢速度并尝试清除路径上的障碍物。
  • clear_costmap_recovery：是一种恢复行为，当机器人在导航过程中被困时，它会清除局部和全局代价地图，以帮助机器人重新找到路径。

• 代价地图：

  • costmap_2d： 是用于创建2D代价地图的工具，它整合了激光雷达、声纳等传感器数据，用于路径规划和避障，代价地图表示环境中的占据情况和障碍物。

• 地图服务：

  • map_server：是一个提供静态地图数据的工具，通常用于加载预先构建好的环境地图。它可以发布 /map 话题，供导航系统使用。

• voxel_grid： 是一种三维体素栅格工具，用于表示三维空间中的占据情况。它通常用于处理三维障碍物数据，提供更详细的环境表示。

各软件包调用关系：

这些软件包通常在一起使用，以实现完整的导航功能。以下是一个典型的导航系统工作流程：

1. 定位：使用 amcl 确定机器人的位置。
2. 全局规划：使用 navfn 或 carrot_planner 等全局规划器计算从起点到目标点的路径。
3. 局部规划：使用 base_local_planner 或 dwa_local_planner 生成短期路径和速度命令。
4. 代价地图：使用 costmap_2d 生成局部和全局代价地图，反映环境中的障碍物。
5. 路径跟踪和避障：使用 move_base 接收目标点并协调全局和局部规划器，实现路径跟踪和避障。
6. 恢复行为：当机器人遇到障碍物或被困时，使用 clear_costmap_recovery 和 move_slow_and_clear 进行恢复。

通过合理配置和组合这些软件包，可以实现高效的机器人导航和路径规划。

---

1.2 代价地图

这是一种用于表示环境中不同位置通行难易程度的数据结构，常用于机器人导航和路径规划中。代价地图将环境表示为一个二维网格，每个网格单元（cell）都有一个对应的代价值，用于指示机器人在该位置移动的难易程度或危险程度。

• 代价地图的基本概念

1. 网格表示：代价地图将环境划分为一个个网格单元，每个单元有一个数值代价。网格的分辨率决定了代价地图的精细程度。
2. 代价值：每个网格单元的代价值表示了该位置的通行代价。较低的代价值通常表示该区域容易通行或安全，较高的代价值表示该区域难以通行或存在障碍物。
3. 来源数据：代价地图的数据来源于传感器，如激光雷达、超声波传感器、摄像头等，这些传感器可以检测环境中的障碍物和空旷区域。

• 代价地图的类型

1. 全局代价地图（Global Costmap）：通常用于全局路径规划， 它使用静态地图或构建好的环境地图，并结合传感器数据更新障碍物信息。

# global_costmap_params.yaml
# 全局代价地图参数命名空间
global_costmap:
  global_frame: map # 代价地图的TF参考坐标系
  robot_base_frame: base_footprint # 机器人的TF坐标名称
  transform_tolerance: 1 # global_frame和robot_base_frame间的TF坐标停止发布多久后，控制机器人停止，单位：s
  update_frequency: 1.5 # 代价地图刷新频率，单位：Hz
  publish_frequency: 1.0 # 代价地图的可视化话题发布频率，单位：Hz

  static_map: true # 是否直接使用静态地图生成代价地图，使用plugins手动配置代价地图时，该参数无效
  rolling_window: false # 代价地图是否跟随机器人移动，static_map为true时该参数必须为false
  width: 10.0 # 代价地图宽度，这里会被静态层扩宽，单位：m
  height: 10.0 # 代价地图高度，这里会被静态层扩宽，单位：m
  resolution: 0.05 # 代价地图分辨率（米/单元格）
  
  # 为代价地图设置地图层，这里设置了三层，分别作为静态层、障碍层和膨胀层
  plugins:
    - {name: static_layer,      type: "costmap_2d::StaticLayer"} # 定义地图层的名称，设置地图层的类型
    
    - {name: obstacle_layer,    type: "costmap_2d::VoxelLayer"} # 障碍层可以使用VoxelLayer代替ObstacleLayer
    
    - {name: inflation_layer,   type: "costmap_2d::InflationLayer"}
  # 各地图层的参数，会以地图层名称作为命名空间
  # 各地图层的参数，会在【costmap_common_params.yaml】内进行设置

2. 局部代价地图（Local Costmap）：覆盖机器人周围的一小部分区域，通常用于局部路径规划和动态避障。它实时更新，反映机器人当前周围的环境情况。

# local_costmap_params.yaml
# 局部代价地图参数命名空间
local_costmap:
  global_frame: map
  robot_base_frame: base_footprint
  transform_tolerance: 0.5
  update_frequency: 5.0
  publish_frequency: 3.0

  rolling_window: true
  width: 4.0 
  height: 4.0 
  resolution: 0.05 

  # 为代价地图设置地图层，这里设置了两层，分别作为障碍层和膨胀层
  # 局部代价动态要求高刷新率，不使用静态层以节省计算资源
  plugins:
    - {name: obstacle_layer,      type: "costmap_2d::ObstacleLayer"}
    
    - {name: inflation_layer,     type: "costmap_2d::InflationLayer"}
  # 各地图层的参数，会以地图层名称作为命名空间
  # 各地图层的参数，会在【costmap_common_params.yaml】内进行设置

代价地图（Costmap）由多个层（layers）组成，每一层负责处理特定类型的信息，这些层包括：

• 静态层（Static Layer）
• 障碍层（Obstacle Layer）
• 膨胀层（Inflation Layer）

3. costmap_common_params.yaml全局代价地图与局部代价地图共同参数。

# 机器人外形设置参数，直接影响代价地图
# 圆形机器人的外形设置，直接设置其外形半径
# robot_radius: 0.4
# 多边形机器人的外形设置，设置机器人外形各顶点相对机器人旋转中心的坐标
# 坐标系正方向为，X：前进为正，Y：向左为正，坐标点=[x, y]
# 这里依次设置的是机器人的右下角、左下角、左上角、右上角的顶点坐标
footprint: [[-0.09, -0.185], [-0.09, 0.185],[0.4,0.185],[0.4, -0.185]]

# 设置静态层参数
static_layer:
  enabled: true # 是否开启静态层
  map_topic: map # 静态层的订阅的地图话题
  unknown_cost_value: -1 # 地图话题中数据值为多少，会转换为静态层代价地图中的未知区域
  lethal_cost_threshold: 100 # 地图话题中数据值为多少，会转换为静态层代价地图中的完全占用区域
  first_map_only: false # 是否仅把第一次订阅到的地图数据转换为静态层代价地图，无视后续订阅到的地图数据
  subscribe_to_updates: false # 是否订阅话题 “map_topic”+“_updates”
  track_unknown_space: true # 如果设置为false，地图话题中的未知区域在代价地图中会转换为自由区域
  
  #如果设置为true，静态层代价地图只有未知、自由和完全占用三种情况
  #如果设置为false，静态层代价地图可以有不同的占用程度
  trinary_costmap: true


#设置障碍层参数
obstacle_layer:
  enabled: true # 是否开启障碍层
  observation_sources: scan # 设置障碍层的观测源名称，可以一次设置多个观测源observation_sources: scan, scan2, camera
  
  # 设置对应观测源参数
  scan:
    topic: scan # 观测源数据话题名称
    # 观测源的TF坐标名称，如果设置为空，会自动从话题数据寻找TF坐标名称
    # 以下三种数据格式支持自动寻找TF坐标名称
    #sensor_msgs/LaserScan, sensor_msgs/PointCloud, and sensor_msgs/PointCloud2
    sensor_frame: laser
    data_type: LaserScan # 观测源话题的数据格式，可以为LaserScan、PointCloud、PointCloud2
    observation_persistence: 0.0 # 保留多久时间内的全部话题数据作为障碍层输入，设置为0代表只保留最近的一帧数据，单位：s
    expected_update_rate: 0.0 # 读取观测源话题的频率，如果进行设置，频率应该设置的比传感器频率低一些。默认0，代表允许观测源一直不发布话题。单位：Hz
    clearing: true # 是否使用该观测源清除自由空间
    marking: true # 是否使用该观测源添加障碍物
    max_obstacle_height: 2.0 # 高于多少的障碍物不加入观测范围，单位：m
    min_obstacle_height: 0.0 # 低于多少的障碍物不加入观测范围，单位：m
    obstacle_range: 2.5 # 多少范围内障碍物会被加入代价地图，单位：m
    raytrace_range: 3.0 # 多少范围内障碍物会被追踪，单位：m

  #在观测源基础上再次进行设置的参数
  max_obstacle_height:  2.0
  obstacle_range: 2.5
  raytrace_range: 3.0

  #如果设置为true，障碍层代价地图会有未知、自由和完全占用三种情况
  #如果设置为false，障碍层代价地图只有自由和完全占用三种情况
  track_unknown_space: true
  #障碍层如何与其它地图层处理的方法。
  #0：障碍层覆盖其它地图层； 1:障碍物最大化方法，即各层的占用方格会覆盖其它层的自由方格，这是最常用的方法
  #99：不改变其它地图层，应该是使障碍层层无效的方法
  combination_method: 1

  #如果障碍层类型是"costmap_2d::VoxelLayer"，可以对以下参数进行设置
  #origin_z: 0.0 # 代价地图的高度
  #z_resolution: 0.2 # 障碍层的Z轴方格的高度
  #z_voxels: 10 # 障碍层Z轴上有几个方格
  #unknown_threshold: 15 # 被认为是“已知”的列中允许的未知单元格数
  #mark_threshold: 0 # 被认为是“自由”的列中允许的标记单元格数
  #publish_voxel_map: false # 是否发布障碍层的投影地图层话题
  #footprint_clearing_enabled: true # 如果设置为true，机器人将把它所经过的空间标记为自由区域
  

#设置膨胀层参数
#根据obstacle_layer、static_layer和footprint生成代价地图
inflation_layer:
  enabled: true # 是否开启膨胀层
  cost_scaling_factor: 1.0 # 代价地图数值随与障碍物距离下降的比值，越大会导致路径规划越靠近障碍物
  inflation_radius: 0.4 # 机器人膨胀半径，影响路径规划，单位：m

---

1.3 move_base

上面我们介绍了navigation的总体框架，下面我们介绍该导航功能的核心：move_base

查看文件夹结构：

C:.
├─cfg
├─include
│  └─move_base
|		└─move_base.h
└─src
	└─move_base_node.cpp
	└─move_base.cpp

move_base 代码参数解析：

// 读取参数并初始化
std::string global_planner, local_planner; // 选择全局、局部规划器
private_nh.param("base_global_planner", global_planner, std::string("navfn/NavfnROS")); // 全局规划器：Dijkstra，支持A*
// 局部规划器，支持：teb、dwa
private_nh.param("base_local_planner", local_planner, std::string("base_local_planner/TrajectoryPlannerROS"));
private_nh.param("global_costmap/robot_base_frame", robot_base_frame_, std::string("base_link")); // 机器人坐标系
private_nh.param("global_costmap/global_frame", global_frame_, std::string("map")); // 全局坐标系
private_nh.param("planner_frequency", planner_frequency_, 0.0); // 规划频率
private_nh.param("controller_frequency", controller_frequency_, 20.0); // 控制频率
...

当使用系统默认全局规划器，使用dwa局部规划器时，启动文件可以为：

<launch>

  <!-- 加载代价地图参数 -->
  <rosparam file="$(find package)/param_common/local_costmap_params.yaml"      command="load" ns="move_base"/>
  <rosparam file="$(find package)/param_common/global_costmap_params.yaml"     command="load" ns="move_base"/>
  <rosparam file="$(find package)/param_common/move_base_params.yaml"          command="load" ns="move_base"/>
  <rosparam file="$(find package)/param_common/base_global_planner_param.yaml" command="load" ns="move_base"/>
  
  <!-- 选择dwa局部规划器 -->
  <param name="move_base/base_local_planner"    type="string" value="dwa_local_planner/DWAPlannerROS"/>
  <rosparam file="param_common/dwa_local_planner_params.yaml"  command="load" ns="move_base"/>
  
  <!-- 启动move_base -->
  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    <rosparam file="costmap_common_params/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="costmap_common_params/costmap_common_params.yaml" command="load" ns="local_costmap" />
  </node>

</launch>

书接上回，那么我们在选择好规划器、做好传感器数据处理、坐标转换后，怎样使用move_base包呢，那就需要用到actionlib的服务端接口。

• 该包提供actionlib的服务端接口，通过发布move_base_msgs::MoveBaseAction类型的goal，可实现到目标点的导航。

rosmsg show MoveBaseActionGoal  
  
[move_base_msgs/MoveBaseActionGoal]:  
std_msgs/Header header  
  uint32 seq  
  time stamp  
  string frame_id
  
actionlib_msgs/GoalID goal_id  
  time stamp  
  string id
  
move_base_msgs/MoveBaseGoal goal  
  geometry_msgs/PoseStamped target_pose  
    std_msgs/Header header  
      uint32 seq  
      time stamp  
      string frame_id  
    geometry_msgs/Pose pose  
      geometry_msgs/Point position  
        float64 x  
        float64 y  
        float64 z  
      geometry_msgs/Quaternion orientation  
        float64 x  
        float64 y  
        float64 z  
        float64 w

其中最重要的就是位置（position）和方向（orientation）。

使用实例，move_base actionlib客户端：

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <nav_msgs/Odometry.h>
#include <string.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <cmath>

typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseActionClient;

int main(int argc, char** argv)
{
    ros::init(argc, argv, "nav_move_base");
    ros::NodeHandle node;
    MoveBaseActionClient mbclient("move_base",true); // 订阅move_base操作服务器
    
    /*设置目标点*/
    geometry_msgs::Point point;
    geometry_msgs::Quaternion quaternion;
    geometry_msgs::Pose pose;
    point.x = 17;
    point.y = 28;
    point.z = 0.000;
    quaternion.x = 0.000;
    quaternion.y = 0.000;
    quaternion.z = 0.012;
    quaternion.w = 0.999;
    pose.position = point;
    pose.orientation = quaternion;
    ROS_INFO("waiting movebase server.");
    if(!mbclient.waitForServer(ros::Duration(30))) // 等待操作服务器可用
    {
        ROS_INFO("not connect movebase server.");
        return 1;
    }
    ROS_INFO("connected movebase server success.");
    ROS_INFO("start to navigation.");
    move_base_msgs::MoveBaseGoal goal;
    goal.target_pose.header.frame_id = "map"; // 全局坐标系
    goal.target_pose.header.stamp = ros::Time::now(); // 时间戳
    goal.target_pose.pose = pose; // 全局姿态
    
    mbclient.sendGoal(goal); // 将目标位姿发送到MoveBaseAction服务器
    bool finished_within_time = mbclient.waitForResult(ros::Duration(1000)); // 要求机器人在1000s时间内到达目标点
    if(!finished_within_time)
    {
        mbclient.cancelGoal(); // 放弃目标
        ROS_INFO("time out for arrival goal.");
    }
    else
    {
        if(mbclient.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) // 获取状态
        {
            ROS_INFO("succeess to arrival goal!");
        }
        else
        {
            ROS_INFO("other reason to failed to arrival goal.");
        }
    }
    ros::spin(); // 进入回调队列，检查回调函数
    ROS_INFO("movebase kill...");
    return 0;
}

---

1.4 map_server

map_server 主要用于ROS地图数据服务，其提供了两个接口：

• map_server node：将灰度图作为地图信息，并将其转换为话题信息（/map）并发布，用作ROS其余节点提供地图数据
• map_saver node：保存地图数据到地图文件

map_server参数配置：

• 首先，其根据参数配置文件中的阈值信息判断栅格是否可以是可以通过的并发布信息，需要建立map.yaml文件：

image: /home/map/map.pgm ## 需要填写地图图片的绝对地址或相对于yaml文件的相对地址
resolution: 0.050000 ## 地图的分辨率，单位是米/像素
origin: [-5.000000, -15.200000, 0.000000] ## 地图的原点的姿态(x,y,yaw)
negate: 0 ## 是否颠倒障碍栅格与自由栅格
occupied_thresh: 0.65 ## 像素被认为是占用的（有障碍物）的阈值
free_thresh: 0.196 ## 每个栅格的被视为障碍物的阈值

地图加载——roslaunch：

<launch>
    <arg name="map_file" default="$(find my_package)/map/map.yaml"/>
    <node name ="map_server" pkg="map_server" type="map_server" output="screen" respawn="false" args="$(arg map_file)"/>
</launch>

保存地图——roslaunch：

<launch>
	<node pkg="map_server" type="map_saver" name="map_saver_node" args="-f /home/map/map_file" output="screen">	
	</node>
</launch>

---

1.5 acml

AMCL：Adaptive Monte Carlo Localization，自适应蒙特卡洛定位，用于机器人在已知地图中的定位。其主要使用粒子滤波算法来估计机器人的位置和姿态。

ros::Publisher pose_pub_;
ros::Publisher particlecloud_pub_;
ros::ServiceServer global_loc_srv_;
ros::ServiceServer nomotion_update_srv_; //to let amcl update samples without requiring motion
ros::ServiceServer set_map_srv_;
ros::Subscriber initial_pose_sub_old_;
ros::Subscriber map_sub_;

• 订阅

  • 话题名称：/scan
    • 类型: sensor_msgs/LaserScan
    • 描述: 接收激光扫描数据，用于更新位姿估计。
  • 话题名称：/initialpose
    • 类型: geometry_msgs/PoseWithCovarianceStamped
    • 描述: 接收初始化的位姿，用于重新设置机器人位姿估计。
  • 话题名称：/map
    • 类型: nav_msgs/OccupancyGrid
    • 描述: 地图数据，用于定位和路径规划。

• 输出

  • 话题名称：/amcl_pose

    • 数据类型：geometry_msgs/PoseWithCovarianceStamped
    • **描述：**发布机器人在地图中的估计位姿，包含位姿的协方差信息

    Header header
    PoseWithCovariance pose
      Pose pose
        Point position
          float64 x
          float64 y
          float64 z
        Quaternion orientation
          float64 x
          float64 y
          float64 z
          float64 w
      float64[6x6] covariance

  • 话题名称：/particlecloud

    • 数据类型：geometry_msgs/PoseArray
    • **描述：**发布当前粒子滤波器中的粒子分布，用于可视化

    Header header
    Pose[] poses
      Pose pose
        Point position
          float64 x
          float64 y
          float64 z
        Quaternion orientation
          float64 x
          float64 y
          float64 z
          float64 w

• 使用

<!-- 开启用于导航的自适应蒙特卡洛定位amcl-->
<include file="$(find turn_on_wheeltec_robot)/launch/include/amcl.launch" />

amcl.launch：

<launch>
    <!-- 使用map的topic而不是map文件-->
    <arg name="use_map_topic" default="false"/>
    <!-- 不可以使用PointCloud，只能使用LaserScan -->
    <arg name="scan_topic" default="scan"/>
    <arg name="map_topic"  default="/map"/>

    <node pkg="amcl" type="amcl" name="amcl" clear_params="true">
      <param name="use_map_topic" value="$(arg use_map_topic)"/>
      <!-- 使用rtabmap分布的Map，连接TF树 -->
      <remap from="/map" to="/$(arg map_topic)"/>
...
      <remap from="scan" to="$(arg scan_topic)"/>
    </node>
</launch>

详情：ACML参数详解

---

2. 项目实战

首先我们看一个实际工程的整体导航launch文件：

<launch>

 <!-- 开启机器人底层控制 同时开启导航功能 -->
 <include file="$(find turn_on_robot)/launch/turn_on_robot.launch">
    <arg name="navigation"  default="true"/>
 </include>
 
 <!-- 开启雷达 -->
 <include file="$(find turn_on_robot)/launch/lidar.launch" />

 <!-- 设置需要用于导航的地图 -->
 <arg name="map_file" default="$(find turn_on_robot)/map/map.yaml"/>
 <node name="map_server_load" pkg="map_server" type="map_server" args="$(arg map_file)">
 </node>

 <!-- 开启用于导航的自适应蒙特卡洛定位amcl-->
 <include file="$(find turn_on_wheeltec_robot)/launch/include/amcl.launch" />

 <!-- 功能节点 -->
 <node name='send_mark' pkg="turn_on_robot" type="send_mark.py">
 </node>

</launch>

下面我将会按顺序介绍：

• 机器人底层控制
• 导航算法实现流程及规划器选择
• 使用雷达生成地图信息
• 基于扩展卡尔曼滤波的多传感器数据融合定位
• 传感器坐标变换及参数标定
• 启动！

---

2.1 机器人底层控制

对机器人底层的控制无非有两个方面：

• 机器人的运动学控制
• 机器人的运动状态反馈

在ROS环境下的navigation包进行导航时，通常导航算法发布的机器人的运动话题为：/cmd_vel，其消息类型：geometry_msgs::Twist，如下：

geometry_msgs::Twist
{
  geometry_msgs::Vector3 linear
  geometry_msgs::Vector3 angular
}

linear:表示线速度的三维向量，通常以米每秒（m/s）为单位。该向量的分量如下：

• linear.x: 沿机器人的前进方向的线速度。
• linear.y: 沿机器人的侧向（左/右）方向的线速度。
• linear.z: 沿机器人的垂直方向的线速度。

angular: 表示角速度的三维向量，通常以弧度每秒（rad/s）为单位。该向量的分量如下：

• angular.x: 绕机器人的前进方向轴的角速度（滚动）。
• angular.y: 绕机器人的侧向轴的角速度（俯仰）。
• angular.z: 绕机器人的垂直轴的角速度（偏航）。

下面演示怎么将\cmd_vel消息数据怎么封装并发送到串口。

short  transition; //中间变量

Send_Data.tx[0]=FRAME_HEADER; //帧头0X7B
Send_Data.tx[1] = 0; //预留位
Send_Data.tx[2] = 0; //预留位

//机器人x轴的目标线速度
transition=0;
transition = twist_aux.linear.x*1000; //将浮点数放大一千倍，简化传输
Send_Data.tx[4] = transition;     //取数据的低8位
Send_Data.tx[3] = transition>>8;  //取数据的高8位

//机器人y轴的目标线速度
transition=0;
transition = twist_aux.linear.y*1000;
Send_Data.tx[6] = transition;
Send_Data.tx[5] = transition>>8;

//机器人z轴的目标角速度
transition=0;
transition = twist_aux.angular.z*1000;
Send_Data.tx[8] = transition;
Send_Data.tx[7] = transition>>8;

Send_Data.tx[9]=Check_Sum(9,SEND_DATA_CHECK); //BCC校验位，规则参见Check_Sum函数
Send_Data.tx[10]=FRAME_TAIL;//帧尾0X7D

虽然发送有Y轴数据，但是实际在下位机解算过程中，并不会解算 Y 轴数据。

可以归结为对机器人三轴速度及转角的控制，而针对不同的运动模型，速度及转角的解算也不同。

---

2.1.1 AKM运动学模型

akm运动模型就是现代汽车的转向结构，下面进行运动学模型分析。

显然akm小车的三轴速度解算，实际上是对 X 轴线速度以及 Z 轴转角的控制，也就是计算出：

• 前轮偏角：Angle~R~、Angle~L~
• 左、右驱动轮的目标速度：V~L~、V~R~

首先得到转弯半径与 V~X~ 、w~Z~ 之间的关系，针对简化的二轮模型，有：

• 圆弧AC的长度为对X轴线速度的积分。
• 前轮转角为对Z轴角速度的积分。

即：

$$R=\frac{L_{AC}}{\alpha}=\frac{\int V_{X}dt}{\int w_{Z}dt}\approx\frac{V_{X}t}{w_{Z}t}=\frac{V_{X}}{w_{Z}}$$

其次，求出左、右驱动轮的速度，需知道轮距 W 、轴距 H ，如图：

已知转向时机器人整体在绕 O 点做旋转运动，则易知左右后轮的速度：

$$V_{L}=V_{X}\frac{R-0.5W}{R}\\ V_{R}=V_{X}\frac{R+0.5W}{R}$$

由几何关系知，前轮偏角：

$$Angle_{L}=\tan^{-1}{(\frac{H}{R-0.5W})}\\ Angle_{R}=\tan^{-1}{(\frac{H}{R+0.5W})}\\ R=\frac{V_{X}}{w_{Z}}$$

运动学正解算得到了，下面进行运动学逆解：

$$V_{L}+V_{R}=V_{X}\frac{R-0.5W}{R}+V_{X}\frac{R+0.5W}{R}\\V_{L}+V_{R}=V_{X}\frac{R+R+0.5W-0.5W}{R}\\ V_{L}+V_{R}=2V_{X}\\ V_{X}=\frac{V_{L}+V_{R}}{2}$$

得到V~X~，下面计算w~Z~：

$$V_R-V_L=V_X\frac{R+0.5W}{R}-V_X\frac{R-0.5W}{R} w_{Z}=\frac{V_{R}-V_{L}}{W}$$

总结：

$$R=\frac{V_{X}}{w_{Z}}\quad\\V_{L}=V_{X}\frac{R-0.5W}{R}, V_{R}=V_{X}\frac{R+0.5W}{R}\\Angle_{L}=\tan^{-1}{(\frac{H}{R-0.5W})}, Angle_{R}=\tan^{-1}{(\frac{H}{R+0.5W})}$$

2.1.2 基于C语言的akm运动解算

正解：

void Drive_Motor(float Vx, float Vz)
{
	float R, AngleL, AngleR, Min_Turn_Radius;
    Min_Turn_Radius=SENIOR_AKM_MIN_TURN_RADIUS; // 设置最小转弯半径，过小影响控制效果
	
    // 计算转向轮偏角
    if(wz!=0 && Vx!=0)
    {
        // 如果目标速度要求的转弯半径小于最小转弯半径
        if(float_abs(Vx/wz)<=Min_Turn_Radius)
        {
            // 降低目标角速度，配合前进速度，提高转弯半径到最小转弯半径
            if(Vz>0)
                wz= Vx/(Min_Turn_Radius);
            else
                wz=-Vx/(Min_Turn_Radius);
        } 
    R=Vx/wz; // 转弯半径
    AngleL=atan(Axle_spacing/(R-0.5*Wheel_spacing)); // 计算出左前轮的偏角
    AngleR=atan(Axle_spacing/(R+0.5*Wheel_spacing)); // 计算出右前轮的偏角
    }
    else
    {
        AngleR=0; // 不需要转向时
    }
    
    // 运动学逆解
    if(AngleR!=0)
    {
        MOTOR_A.Target = Vx*(R-0.5*Wheel_spacing)/R; // 计算出左轮的目标速度
        MOTOR_B.Target = Vx*(R+0.5*Wheel_spacing)/R; // 计算出右轮的目标速度
    }
    else
    {
        MOTOR_A.Target = Vx;
        MOTOR_B.Target = Vx;
    }
}       

逆解：

void Positive_solution(float V_L, float V_R)
{
    Vx=(V_L+V_R)/2;
    Vz=(V_R-V_L)/Wheel_spacing;
}

那么前轮偏角怎么控制呢？

假设采用舵机 + 曲柄摇杆的方式进行控制，可采用舵机舵盘的偏角和前轮偏角之间的关系，很遗憾这并不是线性关系，需要进行曲线拟合，拟合后可以得到某种关系。

Angle_Servo = -0.2137*pow(AngleR, 2) + 1.439*AngleR + 0.009599;

对于上述电机的控制多采用PID控制。

---

2.1.3 履带式差速小车运动学模型

2.1.4 基于C语言的履带式差速运动学解算

2.1.5 下位机与ROS通讯设置

显然整个项目涉及到下位机与ROS主控之间的通讯问题，通常采用串口通讯。

串口通讯一般有对应的串口芯片进行通讯适配，例如常见的：CH341、cp2102。

在ROS中使用串口通讯会涉及以下问题：

串口设备号更改：

当ROS与多个同串口芯片进行通讯时，需要分配串口芯片设备号，例如cp2102有对应的上位机：CP21xx Customization Utility .exe，更改 serial 序号。

创建串口设备别名：

为什么需要用到串口别名？

答：在ROS中，串口设备插拔都会导致端口号/ttyUSB0发生改变，故需要起别名用于固定端口号。

查看端口号：ll /dev/ttyUSB*

固定设备号，新建shell脚本：touch meta_udev.sh，内容如下：

# stm32下位机
echo  'KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60",ATTRS{serial}=="0002", MODE:="0777", GROUP:="dialout", SYMLINK+="base_controller"' >/etc/udev/rules.d/base_controller.rules


# 单线雷达
echo  'KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60",ATTRS{serial}=="0001", MODE:="0777", GROUP:="dialout", SYMLINK+="meta_lidar"' >/etc/udev/rules.d/meta_lidar.rules


# FSS RTK_IMU 组合导航
echo  'KERNEL=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6001", MODE:="0777", GROUP:="dialout", SYMLINK+="FSS_RTK_IMU"' >/etc/udev/rules.d/FSS_RTK_IMU.rules

# Astra_Gemini深度相机
echo  'SUBSYSTEM=="video4linux",ATTR{name}=="USB 2.0 Camera",ATTR{index}=="0",MODE:="0777",SYMLINK+="Astra_Gemini"' >>/etc/udev/rules.d/camera.rules

# 重载 udev 规则
echo "Reloading udev rules..."
udevadm control --reload-rules
udevadm trigger

===========================================================================================
# 案例
# 设置变量
UDEV_RULES_FILE="/etc/udev/rules.d/99-usb-serial.rules"
VENDOR_ID="0403"
PRODUCT_ID="6001"
SYMLINK_NAME="RTK_SERIAL"

# 检查脚本是否以 root 用户运行
if [ "$EUID" -ne 0 ]; then
  echo "请以 root 用户权限运行此脚本。"
  exit 1
fi

# 创建 udev 规则
echo "Creating udev rule for USB device..."
echo "KERNEL==\"ttyUSB*\", ATTRS{idVendor}==\"$VENDOR_ID\", ATTRS{idProduct}==\"$PRODUCT_ID\", MODE:=\"0777\", GROUP:=\"dialout\", SYMLINK+=\"$SYMLINK_NAME\"" > $UDEV_RULES_FILE

# 重载 udev 规则
echo "Reloading udev rules..."
udevadm control --reload-rules
udevadm trigger
echo "符号链接已创建为 /dev/$SYMLINK_NAME"

# 检查符号链接
if [ -L "/dev/$SYMLINK_NAME" ]; then
  echo "符号链接 /dev/$SYMLINK_NAME 创建成功。"
else
  echo "符号链接 /dev/$SYMLINK_NAME 创建失败，请检查设备连接和 udev 规则。"
fi

echo "完成。"

参数解释：

• echo: 将文本输出到标准输出或文件中。

规则	含义
KERNEL==“ttyUSB*”	匹配所有与 ttyUSB 相关的设备（通常是 USB 到串口转换器）
ATTRS{idVendor}==“10c4”	匹配 USB 设备的供应商 ID 为 10c4
ATTRS{idProduct}==“ea60”	匹配 USB 设备的产品 ID 为 ea60
ATTRS{serial}==“0002”	匹配设备的序列号为 0002
MODE:=“0777”	将设备文件的权限设置为 0777，即所有用户都拥有读、写、执行权限
GROUP:=“dialout”	将设备文件的组设置为 dialout，这是一种常见的串口设备用户组
SYMLINK+=“base_controller”	创建符号链接指向该设备，则可以通过 /dev/base_controller 来访问设备，而不用担心设备节点的动态变化

对于相机设备：

• echo: 将文本输出到标准输出或文件中。

规则	含义
SUBSYSTEM==“video4linux”	匹配属于 video4linux 子系统的设备。video4linux 是用于视频设备的内核子系统，例如摄像头
ATTR{name}==“USB 2.0 Camera”	匹配设备名称为 USB 2.0 Camera 的设备
ATTR{index}==“0”	匹配设备索引为 0 的设备
MODE:=“0777”	将设备文件的权限设置为 0777，即所有用户都拥有读、写、执行权限
SYMLINK+=“Astra_Gemini”	同上

对于其它设备，例如，FSS RTK_IMU组合设备，采用的是232转USB口，使用lsusb指令得到：

则可以得到：

• 设备名称: FT232 Serial (UART) IC
• 供应商 ID (idVendor): 0403
• 产品 ID (idProduct): 6001

则对应shell脚本如上，其它设备也可以通过类似方式进行端口固定。

在编辑完脚本后，记得：sudo chmod +x meta.sh，赋予可执行属性，最后重新拔插设备：sudo ./meta.sh。

---

2.2 导航实现框架及规划器选择

2.2.1 导航实现框架

没错，整个导航都是以move_base为核心，上面我们介绍了move_base的使用，这里侧重介绍执行流程。

但是相信还是有很多人看不懂，其实整个过程可总结为：

从上图来看，我们只需要设定好中间框图（全局、局部规划器、代价地图参数），然后提供以下接口数据就行了：

话题名称	消息类型	作用
/move_base_simple/goal	geometry_msgs/PoseStamped	提供目标点
/odom	nav_msgs/Odometry	参考位移
/scan	sensor_msgs/LaserScan or sensor_msgs/PointCloud	检测障碍物，进行避障
/map	nav_msgs/OccupancyGrid	map_server加载地图，并发布话题
/tf	tf2_msgs/TFMessage	跟踪和广播不同坐标系变换关系
/initialpose	geometry_msgs/PoseWithCovarianceStamped	提供机器人在地图中的初始位置，用于初始化定位模块

在设置好以上话题输出后，move_base将会发布：

• 话题名称：/cmd_vel
  • 消息类型：geometry_msgs/Twist
  • 作用：导航栈输出的速度指令，用于控制机器人的运动

---

2.2.2 路径规划器选择

一般来说全局规划器采用默认的Dijkstra算法，选择的为局部规划器，局部规划器有：

• TEB（Timed Elastic Band）
  • 优化导向：TEB是一种优化导向的算法，通过在运动轨迹中插入一系列离散点，并对这些点进行时间和空间上的优化，来生成平滑且高效的路径。
  • 考虑时间因素：TEB在路径规划中显式地考虑了时间维度，使其能够动态调整速度和加速度，生成时间最优路径。
  • 全局约束：能够很好地处理全局约束，如最大速度、最大加速度、障碍物距离等。
• DWA（Dynamic Window Approach）
  • 采样导向：DWA通过在机器人当前状态下采样一组可能的速度（线速度和角速度）组合，然后评估这些组合在未来短时间内的效果。
  • 局部规划：主要关注在短时间窗口内的局部规划，不直接优化时间，而是选择一组速度使机器人能够在局部环境中安全行驶。
  • 动态约束：直接考虑机器人动态特性（如加速度和减速度），选择适合的速度组合。

	TEB	DWA
优点	生成路径更平滑、动态障碍物及全局约束适应性好	计算效率高、实时性好
缺点	计算机量大、环境响应慢	路径不如teb顺滑、路径可能不是最优路径
适用场合	路径频繁调整、空间狭小、障碍物密集	实时性要求好、障碍物稀疏

选择好规划器后，可直接将整个导航的启动文件按如下书写：dwa_local_planner.launch

<launch>

  <!-- 加载代价地图参数 -->
  <rosparam file="$(find package)/param_common/local_costmap_params.yaml"      command="load" ns="move_base"/>
  <rosparam file="$(find package)/param_common/global_costmap_params.yaml"     command="load" ns="move_base"/>
  <rosparam file="$(find package)/param_common/move_base_params.yaml"          command="load" ns="move_base"/>
  <rosparam file="$(find package)/param_common/base_global_planner_param.yaml" command="load" ns="move_base"/>
  
  <!-- 选择dwa局部规划器 -->
  <param name="move_base/base_local_planner"    type="string" value="dwa_local_planner/DWAPlannerROS"/>
  <rosparam file="param_common/dwa_local_planner_params.yaml"  command="load" ns="move_base"/>
  
  <!-- 启动move_base -->
  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    <rosparam file="costmap_common_params/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="costmap_common_params/costmap_common_params.yaml" command="load" ns="local_costmap" />
  </node>

</launch>

看到这里，可能会有人比较懵，这里仅仅熟悉了导航框架的执行以及规划器的选择，还需要进行以下工作：

• 地图信息：由雷达进行建图，生成全局地图信息，可用PS进行地图特征修改。
• 适配传感器话题发布：在导航框架中介绍的navigation依赖的传感器数据，需要完成对应驱动设计及话题发布。
• 由于采用了多种传感器提供定位数据，故需要采用算法将其融合，提高适应性、精度和稳定性。
• 参数设定及坐标系转换：代价地图参数设定、传感器参数标定、坐标系tf转换及广播
• 然后就是实验、移植和总结了
• 最后进行优化，也就是进阶过程。

---

2.3 激光雷达

在整个导航项目中，雷达主要负责：建图、障碍物信息检测。

2.3.1 2d建图

常见的2d建图算法为：

算法	依赖项	优点	缺点
Cartographer	雷达、里程计、imu	支持 2D 和 3D 建图，具有出色的全局一致性校正能力，适合复杂和大规模环境	-
Karto	雷达、里程计	通过图优化实现高精度地图构建，适合多传感器融合，适合大地图构建	地图更新缓慢
Gmapping	雷达、里程计	能够处理传感器噪声和非线性运动模型	没有回环检测、地图容易错位
Lago	雷达、里程计	通过解决线性约束问题进行图优化，处理里程计和激光数据，以实现大规模地图的构建和优化。	-
Core	雷达	使用简单的扫描匹配技术，以最小化资源消耗实现环境建图	-
Hector	雷达	非常适合高速移动的机器人以及没有里程计的场景，能够在动态环境中快速响应	效果不好、易受转向速度影响

算法实现见：

在建立好地图信息后，配置导航栈：

• 确保 move_base 和其他导航节点已正确配置，以利用新的 2D 地图

• local_costmap 和 global_costmap 配置文件需要根据生成的地图调整参数。

• 确保 costmap_common_params.yaml 文件中的地图参数与实际地图分辨率和尺寸匹配。

确定传感器与坐标系对齐：

• 使用 tf 来定义各传感器和机器人基座之间的坐标变换。

• 确保所有相关的 tf 广播正确，且传感器数据与地图数据在同一坐标系下对齐。

---

2.3.2 3d建图（可选）

算法	依赖项	优点	缺点
Cartographer	雷达、里程计、imu	支持 2D 和 3D 建图，具有出色的全局一致性校正能力，适合复杂和大规模环境	-
LIO-SAM	雷达、imu	无需传统里程计、实时性强、闭环检测、因子图优化	-
LEGO-LOAM	雷达	轻量级、地面优化、分离处理	对于不平坦地面适应性差

具体实现见：

---

2.3.3 3d地图转2d栅格地图

对于3d建图生成的地图信息实现导航，需要将其转换为2d栅格地图，再进行其它操作，可用的方法：

• 投影法：将 3D 点云投影到水平面上，生成 2D 占据栅格地图。可以使用 pcl_ros 包中的工具实现。
• octomap_server：如果使用 OctoMap 进行 3D 建图，可以利用 octomap_server 来生成 2D 投影地图。

使用代码：

rosrun octomap_server octomap_server_node octomap_file.ot
rosrun map_server map_saver -f map_name

---

2.4 基于EKF的多数据融合

由于单独使用imu、里程计、gps中一种，都具局限性，故采用数据融合的方式，通常采用：扩展卡尔曼滤波。

ROS集成了扩展卡尔曼滤波的多传感器数据融合包，常用的有：robot_pose_ekf、robot-localization，下面仅介绍robot_pose_ekf。

安装：

cd 
git clone https://github.com/ros-planning/robot_pose_ekf.git

在 ROS navigation 栈中都主要用于位姿估计，在多个环节中起到关键作用：

1. 定位（Localization）：
   • 为导航栈的定位模块（如 AMCL）提供精确的初始位置和姿态估计。这有助于 AMCL 更快地收敛到正确的位姿。
2. 路径规划（Path Planning）：
   • 提供准确的机器人当前位姿，使得局部和全局路径规划器能够基于更可靠的起始状态进行路径计算。
   • 改善机器人在动态环境中的避障能力，通过更精准的姿态信息，导航算法能够更好地调整和规划路径。
3. 运动控制（Motion Control）：
   • 通过提供实时更新的机器人位姿信息，move_base 等控制模块能够更好地执行速度控制和路径跟踪任务。

下面我将介绍常用的imu、里程计、RTK三种传感器的使用。

---

2.4.1 imu

一般来说imu的使用分为两种方式：

• 直接与ROS主控连接
• 通过stm32采集传感器数据，并通过串口发送到ROS主控

针对以上两种方式，需要适当调整程序设计，一般而言，购买imu模块时，厂家都会提供对应的驱动程序，自己适当修改就行。

无论采用哪种方式，最终都需要在ROS主控上进行imu话题的发布：

void turn_on_robot::Publish_ImuSensor()
{
  sensor_msgs::Imu Imu_Data_Pub; // 实例化IMU话题数据
  Imu_Data_Pub.header.stamp = ros::Time::now(); 
  Imu_Data_Pub.header.frame_id = gyro_frame_id; // IMU对应TF坐标，使用robot_pose_ekf功能包需要设置此项
  Imu_Data_Pub.orientation.x = Mpu6050.orientation.x; // 四元数表达三轴姿态
  Imu_Data_Pub.orientation.y = Mpu6050.orientation.y; 
  Imu_Data_Pub.orientation.z = Mpu6050.orientation.z;
  Imu_Data_Pub.orientation.w = Mpu6050.orientation.w;
  Imu_Data_Pub.orientation_covariance[0] = 1e6; // 三轴姿态协方差矩阵
  Imu_Data_Pub.orientation_covariance[4] = 1e6;
  Imu_Data_Pub.orientation_covariance[8] = 1e-6;
  Imu_Data_Pub.angular_velocity.x = Mpu6050.angular_velocity.x; // 三轴角速度
  Imu_Data_Pub.angular_velocity.y = Mpu6050.angular_velocity.y;
  Imu_Data_Pub.angular_velocity.z = Mpu6050.angular_velocity.z;
  Imu_Data_Pub.angular_velocity_covariance[0] = 1e6; // 三轴角速度协方差矩阵
  Imu_Data_Pub.angular_velocity_covariance[4] = 1e6;
  Imu_Data_Pub.angular_velocity_covariance[8] = 1e-6;
  Imu_Data_Pub.linear_acceleration.x = Mpu6050.linear_acceleration.x; // 三轴线性加速度
  Imu_Data_Pub.linear_acceleration.y = Mpu6050.linear_acceleration.y; 
  Imu_Data_Pub.linear_acceleration.z = Mpu6050.linear_acceleration.z;  
  imu_publisher.publish(Imu_Data_Pub); // 发布IMU话题
}

详细的imu处理见：

---

2.4.2 里程计

对于里程计，这个词具有多重含义，在ROS中里程计分为：

• 轮式里程计
• 视觉里程计
• 软件里程计

我们平时说的里程计一般指的是：采用电机编码器数据处理，得到的关于机器人位移变化。

但是随着技术的发展，里程计也不一定需要以上结构来记录机器人的位移变化，例如后两者，这里不再赘述。

下面介绍第一种里程计的使用，跟上一小节同理，其ROS主控里程计话题发布：

void turn_on_robot::Publish_Odom()
{
    geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(Robot_Pos.Z); // 把Z轴转角转换为四元数进行表达

    nav_msgs::Odometry odom; // 实例化里程计话题数据
    odom.header.stamp = ros::Time::now(); 
    odom.header.frame_id = odom_frame_id; // 里程计TF父坐标
    odom.pose.pose.position.x = Robot_Pos.X; // 位置
    odom.pose.pose.position.y = Robot_Pos.Y;
    odom.pose.pose.position.z = Robot_Pos.Z;
    odom.pose.pose.orientation = odom_quat; // 姿态，通过Z轴转角转换的四元数

    odom.child_frame_id = robot_frame_id; // 里程计TF子坐标
    odom.twist.twist.linear.x =  Robot_Vel.X; // X方向速度
    odom.twist.twist.linear.y =  Robot_Vel.Y; // Y方向速度
    odom.twist.twist.angular.z = Robot_Vel.Z; // 绕Z轴角速度 

    // 这个矩阵有两种，分别在机器人静止和运动的时候使用。扩展卡尔曼滤波官方提供的2个矩阵，用于robot_pose_ekf功能包
    if(Robot_Vel.X== 0&&Robot_Vel.Y== 0&&Robot_Vel.Z== 0)
      // 如果velocity是零，说明编码器的误差会比较小，认为编码器数据更可靠
      memcpy(&odom.pose.covariance, odom_pose_covariance2, sizeof(odom_pose_covariance2)),
      memcpy(&odom.twist.covariance, odom_twist_covariance2, sizeof(odom_twist_covariance2));
    else
      // 如果小车velocity非零，考虑到运动中编码器可能带来的滑动误差，认为imu的数据更可靠
      memcpy(&odom.pose.covariance, odom_pose_covariance, sizeof(odom_pose_covariance)),
      memcpy(&odom.twist.covariance, odom_twist_covariance, sizeof(odom_twist_covariance));       
    odom_publisher.publish(odom); // 发布里程计话题
}

---

2.4.3 RTK

RTK可以说是GPS的升级版，具有更高精度，详情看：RTK的使用 | lancit の blog

下面将会介绍:

• 如何在ROS中读取RTK数据流
• 将RTK数据流转为标准话题并发布
• gps数据的坐标系转换
• 适配robot_pose_ekf包

ROS读取RTK数据流：

一般RTK设备都支持 NMEA 0183数据格式输出，那么可以直接采用ROS包：nmea-navsat-driver

在线安装：sudo apt-get install ros-noetic-nmea-navsat-driver

卸载包：sudo apt-get remove ros-melodic-nmea-navsat-driver, sudo apt-get remove ros-melodic-nmea-msgs

离线下载：git clone https://github.com/ros-drivers/nmea_navsat_driver.git

其提供四个节点：

节点名	订阅及发布话题
nmea_serial_driver	直接从串口读取，发布话题：/fix，/vel，/time_reference
nmea_topic_driver	此订阅话题nmea_sentence，发布话题为：/fix，/vel，/time_reference、/heading
nmea_socket_driver	用于通过UDP/IP网络连接接收NMEA数据，并解析
nmea_topic_serial_reader	此节点从串口读入RTK数据，然后封装为nmea_msgs/Sentence数据格式，发布话题nmea_sentence

我们这里使用的是：nmea_serial_driver

编译：catkin_make -DCATKIN_WHITELIST_PACKAGES="nmea_navsat_driver"

发现错误： Could NOT find catkin_virtualenv (missing: catkin_virtualenv_DIR)，缺失catkin_virtualenv依赖项。

排除错误：sudo apt-get install ros-melodic-catkin-virtualenv

---

2.4.4 robot_pose_ekf

robot_pose_ekf 旨在通过融合 IMU、里程计和 GPS 数据来估计机器人的位姿，在 2D 环境中进行稳健状态估计的应用。

EKF-node 的输入参数：

// 输出组合位姿.消息类型：geometry_msgs/PoseWithCovarianceStamped
nh_private.param("output_frame", output_frame_, std::string("odom_combined")); 
nh_private.param("base_footprint_frame", base_footprint_frame_, std::string("base_footprint")); // 车体参考坐标系
nh_private.param("sensor_timeout", timeout_, 1.0); // 等待传感器最大时间，单位：s
nh_private.param("odom_used", odom_used_, true); // 里程计，订阅话题：/odom，消息类型：nav_msgs/Odometry
nh_private.param("imu_used",  imu_used_, true); // imu，订阅话题：/imu_data，消息类型：sensor_msgs/Imu
nh_private.param("vo_used",   vo_used_, true); // 视觉里程计，订阅话题：/vo，消息类型：nav_msgs/Odometry

// gps，订阅话题：/gps，消息类型：nav_msgs/Odometry，方法：将gps转为utm再转为对应消息
nh_private.param("gps_used",   gps_used_, false); 

nh_private.param("debug",   debug_, false); // 启用debug
nh_private.param("self_diagnose",  self_diagnose_, false); // 诊断
nh_private.param("freq", freq, 30.0); // 节点执行频率

包启动文件参数配置：

<launch>
  <!-- 选择cartogapher算法时不开启滤波节点 -->
  <arg name="is_cartographer" default="false"/>
  
  <!-- Robot pose ekf 拓展卡尔曼滤波-->
  <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf" output="screen" unless="$(arg is_cartographer)">
    <param name="output_frame" value="odom_combined"/>
    <param name="base_footprint_frame" value="base_footprint"/>
    <param name="freq" value="30.0"/>
    <param name="debug" value="true"/>
    <param name="sensor_timeout" value="2.0"/>
    <param name="odom_used" value="true"/>
    <param name="imu_used" value="true"/>
    <param name="vo_used" value="false"/>
    <param name="gps_used" value="true"/>
    
    <remap from="imu_data" to="imu" /> ## 在启动时，更改订阅主题（此处是订阅/imu_data，但实际发布的是/imu）
  </node>

</launch>

扩展卡尔曼滤波注意事项

• 传感器数据都有自身坐标系，数据随着时间推移都可能出现漂移现象，因此，每个传感器发出来的绝对位姿不能直接对比，应该使用每个传感器的相对位姿差异来更新扩展卡尔曼滤波器。
• 当机器人在四周移动时候，随着时间推移位姿不确定性会变得越来越大，协方差将无限增长。这样一来发布位姿自身协方差没有意义，因此传感器源公布协方差如何随时间变化（例如，速度的协方差）。请注意，使用世界观测（例如测量到已知墙的距离）将减少机器人姿势的不确定性; 然而，这是定位，而不是里程计。
• 假定机器人上次更新位姿滤波器在t0时刻， 该node只有在收到每个传感器测量值（时间戳>t0）之后才会进行下一次的滤波器更新。 例如，在odom topic收到一条消息时间戳（t_1 > t_0）, 且在imu_data topic上也收到一条消息（ 其时间戳t_2 > t_1 > t_0）, 滤波器将被更新到所有传感器信息可用的最新时刻，这个时刻是t_1。 在t_1时刻odom位姿直接给出了，但是imu位姿需要通过在t_0和t_2两时刻之间进行线性插值求得。 在t_0到 t_1时间段，机器人位姿滤波器使用odom和IMU相对位姿进行更新。

---

2.4.5 robot_localization

下载：

git clone -b noetic-devel --single-branch https://github.com/cra-ros-pkg/robot_localization.git

依赖：

sudo apt-get install ros-noetic-roslint

robot_localization支持连续状态估计、任意数量传感器数据融合、自定义传感器输入数据剔除等功能，其提供的状态估计节点有：

• ekf_localization_node
• ukf_localization_node
• navsat_transform_node

观察其逻辑框图：

可以看出该包是十分强大的，可以进行参数配置可完成多种功能。

坐标系遵循规范：REP 105 – Coordinate Frames for Mobile Platforms (ROS.org)

单位使用规范：REP 103 – Standard Units of Measure and Coordinate Conventions (ROS.org)

---

4.1.1 ekf_localization_node

通过使用全向运动模型来预测未来状态，并使用感知到的传感器数据来修正预测估计，适合处理线性和非线性的系统。

该节点配置文件：ekf_template.yaml

# pose估计频率
frequency: 30

silent_tf_failure: false

# 传感器数据超时阈值，单位 s，可视为滤波器的最小滤波频率
sensor_timeout: 0.1

# true：不使用3d运动模型预测，假如在平面环境中，希望忽略imu检测地面变化，可使能
two_d_mode: false

# 输出tf变换时间延时
transform_time_offset: 0.0

# tf监听器可等待时间
transform_timeout: 0.0

# 查看节点与参数设置是否适配
print_diagnostics: true

# 调试设置
debug: false

# 调试文件路径
debug_out_file: /path/to/debug/file.txt

# tf广播使能
publish_tf: true

# 发布加速度状态使能
publish_acceleration: false

# 允许旧值替代新值使能
permit_corrected_publication: false

# 1. 如果系统没有map_frame，将其删除，并将world_frame设置为odom_frame
# 2. 如果你使用的是： odom、vo_odom、imu，将world_frame设置为odom_frame，默认
# 3. 如果你使用的是： odom、gps、imu，将world_frame设置为map_frame，确保有其它节点发布odom->base_link的变换
map_frame: map              # Defaults to "map"
odom_frame: odom            # Defaults to "odom"
base_link_frame: base_link  # Defaults to "base_link"
world_frame: odom           # Defaults to the value of odom_frame

# 滤波器可接受输入消息类型：
# nav_msgs/Odometry、geometry_msgs/PoseWithCovarianceStamped、geometry_msgs/TwistWithCovarianceStamped、sensor_msgs/Imu
# 添加输入消息，消息类型/编号，例如odom0、odom1、imu0
odom0: example/odom

# 对应上面传感器消息，选择性提取数据更新滤波器
# 例如，有一个里程计消息作为输入，但只想使用它的Z位置值，只设置第三条目为true
# 顺序：x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az
odom0_config: [true,  true,  false,
               false, false, false,
               false, false, false,
               false, false, true,
               false, false, false]

# 传感器消息队列数
odom0_queue_size: 2

# 数据量较大消息引起的bug调试，可使能禁用Nagle算法
odom0_nodelay: false

# 对多个位姿传感器测量同一位姿时，增加的微分使能
odom0_differential: false

# true：设置第一个测量值为后面测量值的零点
odom0_relative: false

# 数据易受异常值影响，阈值使能
odom0_pose_rejection_threshold: 5
odom0_twist_rejection_threshold: 1

# 更多的参数设置实例
odom1: example/another_odom
odom1_config: [false, false, true,
               false, false, false,
               false, false, false,
               false, false, true,
               false, false, false]
odom1_differential: false
odom1_relative: true
odom1_queue_size: 2
odom1_pose_rejection_threshold: 2
odom1_twist_rejection_threshold: 0.2
odom1_nodelay: false

pose0: example/pose
pose0_config: [true,  true,  false,
               false, false, false,
               false, false, false,
               false, false, false,
               false, false, false]
pose0_differential: true
pose0_relative: false
pose0_queue_size: 5
pose0_rejection_threshold: 2  # Note the difference in parameter name
pose0_nodelay: false

twist0: example/twist
twist0_config: [false, false, false,
                false, false, false,
                true,  true,  true,
                false, false, false,
                false, false, false]
twist0_queue_size: 3
twist0_rejection_threshold: 2
twist0_nodelay: false

imu0: example/imu
imu0_config: [false, false, false,
              true,  true,  true,
              false, false, false,
              true,  true,  true,
              true,  true,  true]
imu0_nodelay: false
imu0_differential: false
imu0_relative: true
imu0_queue_size: 5
imu0_pose_rejection_threshold: 0.8                 # Note the difference in parameter names
imu0_twist_rejection_threshold: 0.8                #
imu0_linear_acceleration_rejection_threshold: 0.8  #

# 当imu能自动去除重力引起的加速度变化，设置为flase，确保数据符合REP-103，imu为ENU
imu0_remove_gravitational_acceleration: true

# 当输入提供加速度测量值时，忽略
use_control: true

# 当输入为geometry_msgs/Twist、geometry_msgs/TwistStamped时
stamped_control: false

# 控制指令延时
control_timeout: 0.2

# 哪些指令收到控制：vx, vy, vz, vroll, vpitch, vyaw
control_config: [true, false, false, false, false, true]

# 加速限制
acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4]
# 减速限制
deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5]

# 当机器人加速不能立刻达到限制值，允许通过以下增益控制
acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9]
# 当机器人减速不能立刻达到限制值，允许通过以下增益控制
deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0]

# 过程噪声协方差矩阵，当运动模型与系统匹配度越高，值越小，根据值得收敛情况更改
# 值的顺序为：x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az
process_noise_covariance: [0.05, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0.05, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0.06, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0.03, 0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0.03, 0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0.06, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0.025, 0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0.025, 0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0.04, 0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0.01, 0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.01, 0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.02, 0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0.01, 0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0.01, 0,
                           0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.015]

# 状态估计误差协方差矩阵的初始值。将对角值（方差）设置为较大值将导致相关变量的初始测量值快速收敛。
# 值的顺序为：x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az
initial_estimate_covariance: [1e-9, 0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    1e-9, 0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    1e-9, 0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    1e-9, 0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    1e-9, 0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    1e-9, 0,    0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0,    1e-9, 0,    0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,    1e-9, 0,    0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,    0,    1e-9, 0,     0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,    0,    0,    1e-9,  0,     0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1e-9,  0,     0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1e-9,  0,    0,    0,
                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     1e-9, 0,    0,
                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    1e-9, 0,
                              0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1e-9]

4.1.2 ukf_localization_node

无迹卡尔曼滤波器的实现，使用一组精心设计的sigma投影点恢复状态估计和协方差，消除了雅可比矩阵带来的不稳定性，更适合高非线性系统的处理。

同上，多了以下配置：ukf_template.yaml

# alpha和kappa变量控制sigma点的分布，默认0.001
alpha: 0.001
# 如果未指定，默认值为0。
kappa: 0

# beta变量与状态向量的分布有关，默认值为2。
beta: 2

4.1.3 navsat_transform_node

观察节点配置文件：navsat_transform_template.yaml

# main节点循环频率
frequency: 30

# 在计算UTM坐标系到你的世界坐标系的变换之前的延迟时间（以秒为单位）。如果你将use_odometry_yaw设置为true，这一点特别重要。默认值为0。
delay: 3.0

# 本节点使用的imu是基于ENU的，很多imu是END

# 如果imu未涉及磁偏角（磁极北与地理北），请补充在此处，必要参数
magnetic_declination_radians: 0

# 设置好磁偏角后，imu朝向东yaw应该为0，否则输入偏移量，默认0
yaw_offset: 0.0

# 里程计高度置0
zero_altitude: false

# world_frame->utm广播使能
broadcast_utm_transform: false

# utm->world_frame广播使能
broadcast_utm_transform_as_parent_frame: false

# 里程计数据恢复输出经纬度，话题： /gps/filtered
publish_filtered_gps: false

# true：节点忽略imu航向信息，从里程计源获取，此时要求里程计中的航向是基于世界坐标系的，假如是基于速度变化得到的航向信息，则不能使用里程计源
use_odometry_yaw: false

# true：将从datum参数中加载基准点，该参数不存在则等待SetDatum服务发布消息
wait_for_datum: false

# 用户自定义局部坐标系的基准点，[经度, 纬度, 航向]，当以ENU坐标系时，0航向对应东方
datum: [55.944904, -3.186693, 0.0]

从上述文件可知该节点需要知道三个变量：

• 航向信息：必须基于世界坐标系，例如，采用ENU，则0度航向为指向东，可选：
  • imu：一般由imu磁力计给出，不支持磁力计不采用
  • /odometry/filtered/global：融合后的局部里程计提供，use_odometry_yaw参数设置为true，如果状态总是从0航向开始，则不能使用该项
• 位置信息：经度、维度、海拔
  • gps：需要设置datum基准点
  • /odometry/filtered/global：融合后的局部里程计，此时选定合适datum基准点
• /odometry/filtered/global：可选

用户可以随时向“datum”服务发送robot_localization/SetDatum服务消息。这将手动设置经度、纬度、海拔高度和世界参考的航向，并假设一个包含所有零的里程计消息。这将有效地在指定的经度/纬度处设置原点，X轴与东对齐。用户可以通过“datum”服务或通过启动文件设置此基准（datum）。如果wait_for_datum参数设置为true，则节点将尝试使用datum参数。如果未设置该参数，它将等待接收服务调用。

启动，roslaunch robot_localization navsat_transform_template.launch：

<launch>
  <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node" clear_params="true">
    <rosparam command="load" file="$(find robot_localization)/params/navsat_transform_template.yaml" />

      <!-- 重映射话题
      <remap from="imu/data" to=""/>
      <remap from="odometry/filtered" to=""/>
      <remap from="gps/fix" to=""/>
      -->
      
  </node>
</launch>

观察话题输出：

即，输出的/odometry/gps话题就是我们想要的机器人局部坐标系下的里程计话题。