Simple_navigation

本项目基于：

• RTK
• IMU
• 单线激光雷达
• 闭环控制akm底盘

实现基于多传感器数据融合的导航算法。

---

1. akm底盘控制

akm小车采用坐标系：

1.1 上位机串口控制

ROS主控发布控制话题：/cmd_vel，消息类型：geometry_msgs::Twist，采用串口通讯进行控制，串口数据：

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

值得注意的是：

• 小车采用的西北天坐标系。

• akm小车只需要进行X轴速度以及Z轴转角控制。

---

1.2 下位机指令解算

对ROS发出的指令，下位机主控需要解算到对应电机及转向舵机上。

后轮驱动电机解算：

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

转向角度：

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

由舵盘转角与转向角度的关系，进行二次曲线拟合得到关系式，再采用PWM控制方式。

---

2. 坐标系

2.1 地理坐标系

2.1.1 ECEF坐标系

ECEF是一个三维直角坐标系，用于描述地球表面上任何一点的三维坐标：

• 坐标系原点: 地球的质心
• X轴：指向赤道平面与本初子午线的交点，即经度为0°，纬度为0°的点
• Y轴:：位于赤道平面内，指向经度为90°的点
• Z轴：指向地理北极

ECEF坐标系使用三维直角坐标(X, Y, Z)来表示地球表面上的任意点位置。

---

2.1.2 wgs84坐标系

wgs84是导航常用坐标系，其定义了地球的形状、大小、重力场以及其在空间中的位置：

• 椭球模型:：wgs84定义了地球的形状为一个椭球，具有特定的长半轴和扁率
• 坐标系:：wgs84使用地理坐标（纬度、经度和椭球高度）来表示地球表面上的点
  • 纬度： 相对于地球赤道平面的角度，表示南北方向的位置
  • 经度： 相对于本初子午线的角度，表示东西方向的位置
  • 高度： 高度是相对于wgs84参考椭球面的垂直距离

我们经常说的LLA就是指的：经度、维度、海拔

---

2.1.3 ENU坐标系

这是两种常用的导航局部坐标系：东北天（ENU）、东北地（END）

ENU：

• X轴：E，朝向地理东
• Y轴：N，朝向地理北
• Z轴：U，朝向天
• 欧拉角顺序：Z-X-Y

往往ENU和END坐标系是wgs84坐标给定原点位置。

---

2.2 欧拉角与四元数

欧拉角：用三个角描述物体姿态，一般取自绕三个固定轴的旋转角，假如在END坐标系下：

• Yaw（偏航角）
• Pitch（俯仰角）
• Roll（滚转角）

欧拉角的正负遵循右手定则：伸出右手，大拇指指向旋转轴的正方向，四指弯曲的方向就是旋转的正方向。

欧拉角的旋转顺序很重要，上述坐标系的旋转顺序：Z-Y-X，即先绕Z轴旋转Yaw，再绕Y轴旋转Pitch，最后绕X轴旋转Roll

具有不连续、万向锁的问题。

四元数：一种基于超复数的数学结构，用于表示旋转

q=w+xi+yj+zkq = w + xi + yj + zkq=w+xi+yj+zk

通常表示为一个四元组(w, x, y, z)，其中w是实部，x、y、z是虚部。四元数也可以表示为一个旋转轴和一个旋转角度。

之间的转换：

#include <ros/ros.h>
#include <tf/transform_datatypes.h>

tf::Quaternion quaternion = tf::createQuaternionFromRPY(roll, pitch, yaw); // 欧拉角转四元数
tf::Matrix3x3(quaternion).getRPY(roll, pitch, yaw); // 四元数转欧拉角

tf::Quaternion quaternion(0.707, 0.707, 0.0, 0.0); // 定义一个四元数
double norm = quaternion.length(); // 计算模长
quaternion /= norm; // 归一化

---

3. RTK的使用

3.1 固定设备号

由于串口设备拔插后端口号会改变，故需要对RTK设备（一般是转USB）进行起别名。

查看串口信息：lsusb，得到设备识别码：

新建shell脚本：touch rtk_udev.sh，内容如下：

# 设置变量
UDEV_RULES_FILE="/etc/udev/rules.d/RTK_SERIAL.rules"
VENDOR_ID="0403"
PRODUCT_ID="6001"
SYMLINK_NAME="RTK_SERIAL"

# 检查脚本是否以 root 用户运行
if [ "$(id -u)" -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 "完成。"

添加可执行属性：sudo chmod +x rtk_udev.sh，

重新拔插设备，执行：sudo ./rtk_udev.sh：

---

3.2 RTK驱动

RTK设备一般都支持nmea0184数据格式，需要以下报文信息：

• GPGGA：包含位置信息， AT+GPGGA=5\r\n
• GPVTG：包含速度信息，AT+GPVTG= 1\r\n
• GPHDT：包含定向的朝向信息，AT+GPHDT= 1\r\n

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

编辑启动文件：sudo nano /opt/ros/noetic/share/nmea_navsat_driver/launch/nmea_serial_driver.launch

<launch>
  <arg name="port" default="/dev/RTK_SERIAL" />
  <arg name="baud" default="115200" />
  <arg name="frame_id" default="gps" />
  <arg name="use_GNSS_time" default="False" />
  <arg name="time_ref_source" default="gps" />
  <arg name="useRMC" default="False" />

  <node name="nmea_serial_driver_node" pkg="nmea_navsat_driver" type="nmea_serial_driver" output="screen">
    <param name="port" value="$(arg port)"/>
    <param name="baud" value="$(arg baud)" />
    <param name="frame_id" value="$(arg frame_id)" />
    <param name="use_GNSS_time" value="$(arg use_GNSS_time)" />
    <param name="time_ref_source" value="$(arg time_ref_source)" />
    <param name="useRMC" value="$(arg useRMC)" />
  </node>
</launch>

执行：roslaunch nmea_navsat_driver nmea_serial_driver.launch

此时发布的/fix话题为sensor_msgs/NavSatFix类型，其中gps信号为wgs84坐标系。

---

3.3 gps_common

将sensor_msgs/NavSatFix 转为 sensor_msgs/Odometry

安装：

git clone -b foxy-eol --single-branch https://github.com/swri-robotics/gps_umd.git

---

4. IMU的使用

imu：FDISYSTEMS九轴AHRS姿态传感器

4.1 固定设备号

首先，如果IMU与RTK使用的同一通讯芯片，如：CP2102，需要更改设备序列号。

新建shell文件：imu_udev.sh，内容如下：

# 设置变量
UDEV_RULES_FILE="/etc/udev/rules.d/IMU_SERIAL.rules"
VENDOR_ID="10c4"
PRODUCT_ID="ea60"
SYMLINK_NAME="IMU_SERIAL"

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

# 创建 udev 规则
echo "Creating udev rule for USB device..."
echo "KERNEL==\"ttyUSB*\", ATTRS{idVendor}==\"$VENDOR_ID\",ATTRS{serial}==\"0003\", 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 "完成。"

重新拔插：

---

4.2 imu驱动

驱动采用官方驱动包：fdilink_ahrs，启动文件：roslaunch fdilink_ahrs ahrs_data.launch：

<launch>
  <node pkg="fdilink_ahrs" name="ahrs_driver" type="ahrs_driver" output="screen" >
    <param name="debug"  value="false"/>
    <param name="port"  value="/dev/IMU_SERIAL"/>
    <param name="baud"  value="921600"/>
    <param name="imu_topic"  value="imu"/>
    <param name="imu_frame"  value="imu_link"/>

    <param name="mag_pose_2d_topic"  value="/mag_pose_2d"/> <!-- 磁力计航向 -->
    <param name="Euler_angles_pub_"  value="/euler_angles"/> <!-- 欧拉角 -->
    <param name="Magnetic_pub_"  value="/magnetic"/> <!-- 磁力计发布 -->

    <!-- 0：XYZ（北东地） 1： XYZ（北西天）-->
    <param name="device_type"  value="1"/>
  </node>
</launch> 

编译后，执行：

如果想要实现imu输出ENU下数据，魔改代码：

// NED->ENU : x=y y=x z=z
imu_data.orientation.w = ahrs_frame_.frame.data.data_pack.Qw;
imu_data.orientation.x = ahrs_frame_.frame.data.data_pack.Qy;
imu_data.orientation.y = ahrs_frame_.frame.data.data_pack.Qx;
imu_data.orientation.z = ahrs_frame_.frame.data.data_pack.Qz;

imu_data.angular_velocity.x = imu_frame_.frame.data.data_pack.gyroscope_y; // 注意磁力计解包顺序
imu_data.angular_velocity.y = imu_frame_.frame.data.data_pack.gyroscope_x;
imu_data.angular_velocity.z = -imu_frame_.frame.data.data_pack.gyroscope_z;
imu_data.linear_acceleration.x = imu_frame_.frame.data.data_pack.accelerometer_y;
imu_data.linear_acceleration.y = imu_frame_.frame.data.data_pack.accelerometer_x;
imu_data.linear_acceleration.z = -imu_frame_.frame.data.data_pack.accelerometer_z;

也可以用tf静态发布：

<node pkg="tf" type="static_transform_publisher" name="tf_imu_ned_enu" args="0 0 0 1.5708 0 3.1416 imu_link_ned imu_link 10" output="screen"/>

使用转换后的数据：

tf2_.reset(new tf2_ros::Buffer()); // 创建tf2_ros::Buffer对象，储存在tf2_中
tf2_listener_.reset(new tf2_ros::TransformListener(*tf2_)); // 创建监听器
tf2_->transform(*imu_in, imu_out, target_frame_); // 将输入的imu_in原始数据转为target_frame_，保存在imu_out中

注意：

• 该imu自带九轴滤波融合
• 注意绝对航向和相对航向问题
• 数据漂移，请校准

---

4.3 imu_transformer

作用：

• 创建imu数据从一坐标系到另一坐标系的静态发布
• 计算转换后的坐标信息

下载：

git clone -b master --single-branch https://github.com/ros-perception/imu_pipeline.git

假设imu固定坐标系为NED，转为ENU，则父坐标为NED，子坐标为ENU，创建和计算如下：

<node pkg="tf" type="static_transform_publisher" name="tf_imu_ned_enu" args="0 0 0 1.5708 0 3.1416 imu_link_ned imu_link 10" output="screen"/>

<node pkg="nodelet" type="nodelet" name="imu_data_transformer" args="load imu_transformer/imu_transformer_nodelet imu_manager">
  <remap from="imu_in/data" to="imu"/> <!-- 数据类型：sensor_msgs/Imu-->
  <remap from="imu_in/mag" to="imu/mag"/> <!-- 数据类型：(sensor_msgs/MagneticField 或 geometry_msgs::Vector3Stamped-->
  <param name="target_frame" value="base_link"/>
</node>

注意mag（磁力计）消息类型要求。

---

4.4 imu_tools

其提供三个节点：

• imu_filter_madgwick：融合imu角速度、线加速度、磁力计（可选）生成带orientation的imu数据
• imu_complementary_filter：采用补偿融合的方式生成带orientation的imu数据
• rviz_imu_plugin：imu Rviz可视化

下载：

git clone -b noetic --single-branch https://github.com/CCNYRoboticsLab/imu_tools.git

上述imu自带多轴数据融合，不再进行融合。

---

5. multi-sensor fusion

5.1 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

观察其逻辑框图：

用于融合：轮式里程计、imu、gps

编辑启动文件，robot_localization.launch:

<launch>
  <!-- 1.加载参数配置文件 -->
  <rosparam command="load" file="$(find robot_localization)/params/dual_ekf_navsat_example.yaml" />
  
  <!-- 2.第一个EKF实例，融合轮式里程计与imu，实现odom到base的局部变换 -->
  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_se_odom" clear_params="true"/>
  
  <!-- 3.第二个EKF实例，融合(2)相同数据，并结合(4)的gps数据，实现map到odom的变换 -->
  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_se_map" clear_params="true">
    <remap from="odometry/filtered" to="odometry/filtered_map"/>
  </node>

  <!-- 4.将gps数据转为机器人世界坐标系下，即map -->
  <node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform" clear_params="true">
    <remap from="odometry/filtered" to="odometry/filtered_map"/>
    <remap from="gps/fix" to="/fix" />
	<remap from="imu/data" to="imu" />
  </node>
  
</launch>

编辑配置参数，dual_ekf_navsat_example.yaml：

# imu、轮式里程计融合
ekf_se_odom:
  frequency: 30 # 节点频率
  sensor_timeout: 0.5 # 传感器数据超时设置，可视为EKF最小频率
  two_d_mode: true # true：不使用3d运动模型预测，假如在平面环境中，希望忽略imu检测地面的微小变化
  transform_time_offset: 0.0 # 延时tf发布
  transform_timeout: 0.5 # tf监听器等待时间
  print_diagnostics: true # 打印参数适配情况
  debug: false # 调试模式
  publish_tf: true # tf发布使能

  odom_frame: odom
  base_link_frame: base_link
  world_frame: odom
  
  odom0: /odom # 轮式里程计
  # 选择性融合数据
  # 顺序：
  #		x     , y     , z,
  #		roll  , pitch , yaw,
  #		vx    , vy    , vz,
  #		vroll , vpitch, vyaw,
  #		ax    , ay    , az
  odom0_config: [false, false, false,
                 false, false, false,
                 true,  true,  false,
                 false, false, true,
                 false, false, false]
  odom0_queue_size: 10 # 消息队列
  odom0_differential: false
  odom0_relative: false # true：将该传感器第一个值作为后续数据的零点
  odom0_nodelay: false # 数据量较大消息引起的bug调试，true：禁用Nagle算法

  imu0: /imu # imu
  imu0_config: [false, false, false,
                true,  true,  false,
                false, false, false,
                true,  true,  true,
                true,  true,  true]
                
  imu0_differential: false
  imu0_relative: false # 这儿的设置直接影响航向的选取
  imu0_queue_size: 10
  imu0_remove_gravitational_acceleration: true # imu自动滤除重力影响
  imu0_nodelay: false

  use_control: false # 利用控制指令模拟加速度，当输入提供加速度测量值时，忽略
	
  # 过程噪声，过程噪声协方差矩阵，当运动模型与系统匹配度越高，值越小，根据值得收敛情况更改
  # 值的顺序为：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]

# gps、imu、轮式里程计融合
ekf_se_map:
  frequency: 30
  sensor_timeout: 0.5
  two_d_mode: true
  transform_time_offset: 0.0
  transform_timeout: 0.5
  print_diagnostics: true
  debug: false
  publish_tf: true

  map_frame: map
  odom_frame: odom
  base_link_frame: base_link
  world_frame: map
  
  odom0: /odom
  odom0_config: [false, false, false,
                 false, false, false,
                 true,  true,  false,
                 false, false, true,
                 false, false, false]
  odom0_queue_size: 10
  odom0_differential: false
  odom0_relative: false
  odom0_nodelay: false
  
  odom1: odometry/gps
  odom1_config: [true,  true,  false,
                 false, false, false,
                 false, false, false,
                 false, false, false,
                 false, false, false]
  odom1_queue_size: 10
  odom1_differential: false
  odom1_relative: false
  odom1_nodelay: false

  imu0: /imu
  imu0_config: [false, false, false,
                true,  true,  false,
                false, false, false,
                true,  true,  true,
                true,  true,  true]
  imu0_differential: false
  imu0_relative: false
  imu0_nodelay: false
  imu0_queue_size: 10
  imu0_remove_gravitational_acceleration: true

  use_control: false

  process_noise_covariance: [1.0,  0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                             0,    1.0,  0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                             0,    0,    1e-3, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                             0,    0,    0,    0.3,  0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                             0,    0,    0,    0,    0.3,  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.5,   0,     0,    0,    0,    0,    0,    0,    0,
                             0,    0,    0,    0,    0,    0,    0,     0.5,   0,    0,    0,    0,    0,    0,    0,
                             0,    0,    0,    0,    0,    0,    0,     0,     0.1,  0,    0,    0,    0,    0,    0,
                             0,    0,    0,    0,    0,    0,    0,     0,     0,    0.3,  0,    0,    0,    0,    0,
                             0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.3,  0,    0,    0,    0,
                             0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.3,  0,    0,    0,
                             0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0.3,  0,    0,
                             0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0.3,  0,
                             0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.3]

  initial_estimate_covariance: [1.0,  0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                0,    1.0,  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,    1.0,  0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                0,    0,    0,    0,    1.0,  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,    1.0,  0,    0,    0,     0,     0,     0,    0,    0,
                                0,    0,    0,    0,    0,    0,    0,    1.0,  0,    0,     0,     0,     0,    0,    0,
                                0,    0,    0,    0,    0,    0,    0,    0,    1.0,  0,     0,     0,     0,    0,    0,
                                0,    0,    0,    0,    0,    0,    0,    0,    0,    1.0,   0,     0,     0,    0,    0,
                                0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     1.0,   0,     0,    0,    0,
                                0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     1.0,   0,    0,    0,
                                0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     1.0,  0,    0,
                                0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    1.0,  0,
                                0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1.0]

navsat_transform:
  frequency: 30
  delay: 3.0
  magnetic_declination_radians: 0.0  # 位置磁偏角
  yaw_offset: 0.0  # imu的yaw偏移量，当面向东时，yaw应该为0

  zero_altitude: true
  broadcast_cartesian_transform: false # 发布
  publish_filtered_gps: false  
  use_odometry_yaw: true # true：该节点忽略imu数据，获取航向信息从另一个EKF状态节点，当EKF提供的航向是基于速度积分得到的，置为false
  wait_for_datum: true # true：加载datum参数
  datum: [55.944904, -3.186693, 0.0] # 手动设置原点，经度、维度、航向

---

5.2 imu_gnss_eskf

该项目旨在实现一个误差状态卡尔曼滤波器（ESKF）算法，用于融合 IMU 和 GNSS 数据。

安装依赖项：

sudo apt-get install libeigen3-dev 
sudo apt-get install ros-noetic-geographic-* geographiclib-* libgeographic-*
sudo apt-get install ros-noetic-nav-msgs ros-noetic-eigen-conversions ros-noetic-nmea-navsat-driver

查询路径下cmake版本号，然后替换下面的**：

sudo ln -s /usr/share/cmake/geographiclib/FindGeographicLib.cmake /usr/share/cmake-3**/Modules/

sudo ln -s：创建符号链接，sudo ln -s [目标文件或目录] [链接名称]

修改imu_gnss_eskf_node.cpp文件，新增参数，更改父坐标系与子坐标系，并添加动态广播。

...
private:
   tf::TransformBroadcaster tf_broadcaster_;
   std::string odom_frame_id, robot_frame_id;
...
    nh.param<std::string>("odom_frame_id", odom_frame_id, "odom");
    nh.param<std::string>("robot_frame_id", robot_frame_id, "base_link");
...
    odom_msg.header.frame_id = odom_frame_id;
    odom_msg.child_frame_id = robot_frame_id;
...
    geometry_msgs::TransformStamped odom_trans;
    odom_trans.header.stamp = odom_msg.header.stamp;
    odom_trans.header.frame_id = odom_msg.header.frame_id;  // "odom"
    odom_trans.child_frame_id = odom_msg.child_frame_id;    // "base_link"

    odom_trans.transform.translation.x = T_wb.translation().x();
    odom_trans.transform.translation.y = T_wb.translation().y();
    odom_trans.transform.translation.z = T_wb.translation().z();

    // normalize 归一化
    odom_trans.transform.rotation = odom_msg.pose.pose.orientation;
    tf2::Quaternion quat_tf;
    tf2::convert(odom_trans.transform.rotation, quat_tf);
    quat_tf.normalize();
    odom_trans.transform.rotation.x = quat_tf.x();
    odom_trans.transform.rotation.y = quat_tf.y();
    odom_trans.transform.rotation.z = quat_tf.z();
    odom_trans.transform.rotation.w = quat_tf.w();
    
    tf_broadcaster_.sendTransform(odom_trans);

编译通过，修改启动文件：roslaunch imu_gnss_eskf imu_gnss_eskf.launch：

<launch>

  <!-- 打开RTK -->
  <include file="$(find nmea_navsat_driver)/launch/nmea_serial_driver.launch" />

  <!-- 打开imu -->
  <include file="$(find fdilink_ahrs)/launch/ahrs_data.launch.launch" />
  
  <!-- ekf融合 -->
  <node name="imu_gnss_eskf_node" pkg="imu_gnss_eskf" type="imu_gnss_eskf_node" output="screen">
    <param name="acc_noise"       type="double" value="1e-2" />
    <param name="gyr_noise"       type="double" value="1e-4" />
    <param name="acc_bias_noise"  type="double" value="1e-6" />
    <param name="gyr_bias_noise"  type="double" value="1e-8" />
    <param name="p_I_GNSS_x"      type="double" value="0."/>
    <param name="p_I_GNSS_y"      type="double" value="0."/>
    <param name="p_I_GNSS_z"      type="double" value="0."/>
    <param name="odom_frame_id"   type="string" value="odom"/>
    <param name="robot_frame_id"  type="string" value="base_link"/>
    
    <remap from="/imu/data" to="/imu"/>
    <remap from="/nav_odom" to="/odom"/>
  </node>
</launch>

其产生的话题：

/fix ## 接收RTK话题，sensor_msgs/NavSatFix
/imu/data ## 接收imu话题，sensor_msgs/Imu
/nav_odom ## 发布融合里程计话题，nav_msgs/Odometry
/nav_path ## 发布运动路径信息，nav_msgs/Path

实际融合：

---

5.3 robot_pose_ekf

这也是一个多传感器融合包：

• 轮式里程计
• imu
• gps

下载：

git clone https://github.com/udacity/robot_pose_ekf.git

其订阅的话题：

对于轮式里程计不作介绍，一般由机器人底盘上的电机编码器得到，对于imu可以直接使用上述imu。

而对于gps传感器而言，上述RTK提供的gps为sensor_msgs/NavSatFix类型，需要将其转换。

5.4 imu_gps_localization

该包维护位置、速度、方向、加速度和陀螺的bias，实现：将wgs84坐标系下的gps转为ENU坐标系下与imu进行融合。

下载：

git clone https://github.com/ydsf16/imu_gps_localization.git

依赖：

sudo apt-get install libgoogle-glog-dev
# find package 中添加 Eigen

查看话题：

这个包显然只是将gps与imu融合得到轨迹跟踪数据。

5.5 rtk_odom

基于带航向信息的RTK里程计设计：

开启nmea数据流：

• GPGGA
• GPVTG

nmea驱动包：nmea_navsat_driver

• 话题：/fix，消息类型：sensor_msgs/NavSatFix
• 话题：/heading，消息类型：geometry_msgs/QuaternionStamped

新建rtk_odom包，代码如下：

#include <ros/ros.h>
#include <sensor_msgs/NavSatFix.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/QuaternionStamped.h>
#include <nav_msgs/Odometry.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <eigen3/Eigen/Dense>
#include <mutex>
#include <vector>
#include <cmath>

class RTKOdom {
public:
    RTKOdom(ros::NodeHandle& nh) {
        // 从参数服务器获取话题名称
        std::string rtk_topic, heading_topic, odom_topic, init_fix_topic;
        nh.param<std::string>("rtk_topic", rtk_topic, "/fix");
        nh.param<std::string>("heading_topic", heading_topic, "/heading");
        nh.param<std::string>("odom_topic", odom_topic, "/rtk_odom");
        nh.param<std::string>("init_fix_topic", init_fix_topic, "/rtk_init_fix");

        // 订阅和发布话题
        rtk_sub_ = nh.subscribe(rtk_topic, 10, &RTKOdom::rtkCallback, this);
        heading_sub_ = nh.subscribe(heading_topic, 10, &RTKOdom::rtkHeadingCallback, this);
        odom_pub_ = nh.advertise<nav_msgs::Odometry>(odom_topic, 10);
        init_lla_pub_ = nh.advertise<geometry_msgs::PointStamped>(init_fix_topic, 10);

        frame_count_ = 0;
        initial_llat_ = Eigen::Vector3f::Zero(); // 初始LLA位置信息
        initial_ecef_ = Eigen::Vector3f::Zero(); // 初始ECEF位置信息
    }

    // rtk信号的回调函数
    void rtkCallback(const sensor_msgs::NavSatFix::ConstPtr& msg) {
        if (msg->status.status != 2) {
            ROS_WARN("RTK status is not good."); // 不是差分gps定位
            return;
        }

        int average_num = 1; // 设置初始位置计算次数

        if (frame_count_ < average_num) {
            frame_count_++;
            initial_llat_ += Eigen::Vector3f(msg->latitude, msg->longitude, msg->altitude);
            ROS_INFO("Accumulating average..."); // 积累平均值
            return;
        }

        // 完成初始化位置标定
        if (frame_count_ == average_num) {
            initial_llat_ /= frame_count_; // 初始lla位置
            initial_ecef_ = llaToECEF(initial_llat_); // 初始ecef位置
            frame_count_++;

            geometry_msgs::PointStamped init_fix_msg;
            init_fix_msg.header.stamp = ros::Time::now();
            init_fix_msg.point.x = initial_llat_(0);
            init_fix_msg.point.y = initial_llat_(1);
            init_fix_msg.point.z = initial_llat_(2);
            init_lla_pub_.publish(init_fix_msg);

            ROS_INFO("Initial RTK position set.");
            return;
        }

        // 获取上次的四元数
        Eigen::Quaternionf quat(last_heading_msg_.quaternion.w, last_heading_msg_.quaternion.x, last_heading_msg_.quaternion.y, last_heading_msg_.quaternion.z);
        Eigen::Vector3f euler = quat.toRotationMatrix().eulerAngles(2, 0, 1); // 转欧拉角，2 0 1 为Z X Y，即yaw、roll、pitch
        quat = Eigen::AngleAxisf(-euler(0) + M_PI, Eigen::Vector3f::UnitZ()); // 调整方向，取反绕Z轴，与坐标系对其

        Eigen::Vector3f current_rtk_xyz = llaToENU(msg->latitude, msg->longitude, msg->altitude); // 当前位置
        Eigen::Quaternionf rtk_heading_quat = getCurrentRTKOrientation(msg->header.stamp.toSec()); // 航向信息
        Eigen::Vector3f velocity(0, 0, 0); // 航速

        if (rtk_heading_quat.coeffs().size() == 4) {
            publishOdom(current_rtk_xyz, velocity, rtk_heading_quat, msg->header.frame_id); // 发布RTK里程计
        }
    }

    // heading话题回调
    void rtkHeadingCallback(const geometry_msgs::QuaternionStamped::ConstPtr& msg) {
        std::lock_guard<std::mutex> lock(rtk_heading_mutex_);
        if (rtk_heading_msgs_.size() > 100) {
            rtk_heading_msgs_.erase(rtk_heading_msgs_.begin()); // 航向信息动态数组
        }
        rtk_heading_msgs_.push_back(*msg); // 压入最新航向信息
        last_heading_msg_ = *msg;
    }

private:
    ros::Subscriber rtk_sub_;
    ros::Subscriber heading_sub_;
    ros::Publisher odom_pub_;
    ros::Publisher init_lla_pub_;

    int frame_count_;
    Eigen::Vector3f initial_llat_;
    Eigen::Vector3f initial_ecef_;

    std::mutex rtk_heading_mutex_;
    geometry_msgs::QuaternionStamped last_heading_msg_;
    std::vector<geometry_msgs::QuaternionStamped> rtk_heading_msgs_;

    // 获取当前rtk航向信息
    Eigen::Quaternionf getCurrentRTKOrientation(double current_time) {
        std::lock_guard<std::mutex> lock(rtk_heading_mutex_); // 互斥锁，确保不被其它进程修改
        if (rtk_heading_msgs_.empty()) {
            return Eigen::Quaternionf::Identity();
        }

        std::vector<double> time_bias;
        for (const auto& msg : rtk_heading_msgs_) {
            time_bias.push_back(std::abs(msg.header.stamp.toSec() - current_time)); // 遍历时间偏差
        }

        auto min_time_iter = std::min_element(time_bias.begin(), time_bias.end());
        int min_time_index = std::distance(time_bias.begin(), min_time_iter); // 最小时间偏差索引

        auto min_time_msg = rtk_heading_msgs_[min_time_index]; // 提取最近的消息
        rtk_heading_msgs_.erase(rtk_heading_msgs_.begin(), rtk_heading_msgs_.begin() + min_time_index); // 清除之前的旧消息

        Eigen::Quaternionf quat(min_time_msg.quaternion.w, min_time_msg.quaternion.x, min_time_msg.quaternion.y, min_time_msg.quaternion.z); // 转为四元数并调整方向
        Eigen::Vector3f euler = quat.toRotationMatrix().eulerAngles(2, 0, 1);
        quat = Eigen::AngleAxisf(-euler(0) + M_PI, Eigen::Vector3f::UnitZ());

        return quat;
    }

    // lla转ecef坐标系
    Eigen::Vector3f llaToECEF(const Eigen::Vector3f& lla) {
        const double equatorial_radius = 6378137.0;
        const double polar_radius = 6356752.31424518;
        const double square_ratio = std::pow(polar_radius, 2) / std::pow(equatorial_radius, 2);

        double lat_rad = lla(0) * M_PI / 180.0;
        double lon_rad = lla(1) * M_PI / 180.0;
        double alt = lla(2);

        double N = equatorial_radius / std::sqrt(1 - (1 - square_ratio) * std::pow(std::sin(lat_rad), 2));
        double x = (N + alt) * std::cos(lat_rad) * std::cos(lon_rad);
        double y = (N + alt) * std::cos(lat_rad) * std::sin(lon_rad);
        double z = (square_ratio * N + alt) * std::sin(lat_rad);

        return Eigen::Vector3f(x, y, z);
    }

    // lla转enu坐标系
    Eigen::Vector3f llaToENU(double lat, double lon, double alt) {
        Eigen::Vector3f cur_ecef = llaToECEF(Eigen::Vector3f(lat, lon, alt));
        Eigen::Vector3f r_ecef = cur_ecef - initial_ecef_;

        double phi = initial_llat_(0) * M_PI / 180.0;
        double lam = initial_llat_(1) * M_PI / 180.0;

        Eigen::Matrix3f R;
        R << -std::sin(lam), std::cos(lam), 0,
            -std::cos(lam) * std::sin(phi), -std::sin(lam) * std::sin(phi), std::cos(phi),
            std::cos(lam) * std::cos(phi), std::sin(lam) * std::cos(phi), std::sin(phi);

        return R * r_ecef;
    }

    // 发布rtk里程计
    void publishOdom(const Eigen::Vector3f& P, const Eigen::Vector3f& V, const Eigen::Quaternionf& Q, const std::string& frame_id) {
        nav_msgs::Odometry odom_msg;
        odom_msg.header.stamp = ros::Time::now();
        odom_msg.header.frame_id = "map";
        odom_msg.child_frame_id = frame_id;

        odom_msg.pose.pose.position.x = P(0);
        odom_msg.pose.pose.position.y = P(1);
        odom_msg.pose.pose.position.z = P(2);

        odom_msg.pose.pose.orientation.x = Q.x();
        odom_msg.pose.pose.orientation.y = Q.y();
        odom_msg.pose.pose.orientation.z = Q.z();
        odom_msg.pose.pose.orientation.w = Q.w();

        odom_msg.twist.twist.linear.x = V(0);
        odom_msg.twist.twist.linear.y = V(1);
        odom_msg.twist.twist.linear.z = V(2);

        odom_msg.twist.twist.angular.x = 0;
        odom_msg.twist.twist.angular.y = 0;
        odom_msg.twist.twist.angular.z = 0;

        odom_pub_.publish(odom_msg);
    }
};

int main(int argc, char** argv) {
    ros::init(argc, argv, "rtk_odom");
    ros::NodeHandle nh("~");

    RTKOdom odom_node(nh);

    ros::spin();

    return 0;
}

编辑launch文件：

<launch>
  <!-- 初始化RTK -->
 <node pkg="rtk_init" type="rtk_init_node" name="rtk_init_node" output="screen">
    <param name="serial_port" type="string" value="/dev/RTK_SERIAL"/>
    <param name="baud_rate" type="int" value="115200"/>
    <param name="use_binary_stream" type="bool" value="false"/>
    <param name="use_nmea_stream" type="bool" value="true"/>
    <param name="use_rtk_physics_config" type="bool" value="false"/>
    <param name="use_GPGGA" type="bool" value="true"/>
    <param name="use_GPHDT" type="bool" value="false"/>
    <param name="use_GPVTG" type="bool" value="true"/>
    <param name="use_GPRMC" type="bool" value="false"/>
    <param name="use_GPZDA" type="bool" value="false"/>
    <param name="use_GPATT" type="bool" value="false"/>
  </node>
  
  <!-- 打开RTK -->
  <include file="$(find nmea_navsat_driver)/launch/nmea_serial_driver.launch" />

  <!-- 开始融合 -->
  <node name="rtk_odom" pkg="rtk_odom" type="rtk_odom_node" output="screen">
      <param name="rtk_topic" value="/fix"/>
      <param name="heading_topic" value="/heading"/>
      <param name="odom_topic" value="/rtk_odom"/>
      <param name="init_fix_topic" value="/rtk_init_fix"/>
  </node>
</launch>

查看效果：

不推荐这种，RTK航向信息太不稳定了。

---

6. rviz查看卫星地图

下面介绍两种卫星地图的查看方式。

6.1 rviz_satellite

安装：

git clone -b ros1 --single-branch https://github.com/nobleo/rviz_satellite.git

打开launch目录下的demo.gps文件，修改经纬度，填入起始经纬度信息。

运行：roslaunch rviz_satellite demo.launch

修改AerialMapDisplay：

选项	含义
Topic	接受gps话题，消息类型：sensor_msgs/NavSatFix
Robot frame	机器人坐标系
Dynamically reload	当机器人移出中心块时，将导致图像重新加载，这仅在 TF 正确指定机器人框架时才有效
Alpha	透明度
Draw Under	将地图显示在所有其他几何体下方
Zoom	地图缩放级别，推荐16-19，最大22
Blocks	加载的相邻块数，将加载中心块，以及中心周围的许多块，最大值8
Frame Convention	X/Y 轴的约定，默认XYZ 映射到 ENU
Object URI	地图API接口

简单URL：

https://maps.wikimedia.org/osm-intl/{z}/{x}/{y}.png # 最简陋 已经停用了
https://tile.openstreetmap.org/{z}/{x}/{y}.png # 稍好点

付费：

TomTom: https://api.tomtom.com/map/1/tile/basic/main/{z}/{x}/{y}.png?tileSize=512&key=[TOKEN]
Mapbox: https://api.mapbox.com/styles/v1/mapbox/satellite-v9/tiles/256/{z}/{x}/{y}?access_token=[TOKEN]
GoogleMaps: https://maps.googleapis.com/maps/api/staticmap?maptype=satellite&center={lat},{lon}&zoom={z}&size=256x256&key=[TOKEN]

查看效果：

---

6.2 mapviz

快速安装：

sudo apt-get install ros-noetic-mapviz ros-noetic-mapviz-plugins ros-noetic-tile-map ros-noetic-multires-image

下载：

git clone -b melodic-eol --single-branch https://github.com/swri-robotics/mapviz.git

没发现noetic版本的，但melodic版本在noetic下编译通过。

安装依赖：

rosdep install --from-paths src --ignore-src

修改mapviz.launch中的经纬度，地图将以该中心加载地图。

<launch>
  <node pkg="mapviz" type="mapviz" name="mapviz"></node>
  <node pkg="swri_transform_util" type="initialize_origin.py" name="initialize_origin" >
    <param name="local_xy_frame" value="/map"/>
    <param name="local_xy_origin" value="back_40"/>
    <rosparam param="local_xy_origins">
      [{ name: swri,
         latitude: 30.777693,
         longitude: 103.960831,
         altitude: 233.719,
         heading: 0.0},
         
       { name: back_40,
         latitude: 29.447507,
         longitude: -98.629367,
         altitude: 200.0,
         heading: 0.0}]
    </rosparam>
  </node>
  <node pkg="tf" type="static_transform_publisher" name="swri_transform" args="0 0 0 0 0 0 /map /origin 100"  />
</launch>

启动：

roslaunch mapviz mapviz.launch

天地网申请URL：

http://t0.tianditu.gov.cn/img_w/wmts?SERVICE=WMTS&REQUEST=GetTile&VERSION=1.0.0&LAYER=img&STYLE=default&TILEMATRIXSET=w&FORMAT=tiles&TILEMATRIX={level}&TILEROW={y}&TILECOL={x}&tk=f6c42d6c77ed827abe977452a23c2732

查看效果：

---

7. 导航

整个导航是基于ROS Navigation Stack的，安装：

git clone https://github.com/ros-planning/navigation.git

默认版本：noetic，melodic：git clone -b melodic-devel --single-branch https://github.com/ros-planning/navigation.git。

安装依赖项：

sudo apt-get install ros-noetic-navigation*

最好不要用rosdep，rosdep安装与其它包存在冲突。

编译：catkin_make。

基于ROS Navigation Stack进行导航，大致思路：

• map_server加载全局地图，以map为全局坐标系
• 将odom原点与map重合，odom子坐标系为base_link，base_link与laser_link坐标系为静态坐标系
• 启动雷达，得到点云信息
• 启动move_base
• 局部规划由move_base完成
• 发送导航路径
• ok

7.1 创建地图

新建一个ros包：pgm_generator，代码如下：

#include <iostream>
#include <fstream>
#include <cmath>
#include <ros/ros.h>
#include <ros/package.h>

int main(int argc, char** argv) {
    ros::init(argc, argv, "pgm_generator");
    ros::NodeHandle nh("~"); // 使用私有命名空间，以便从 launch 文件中获取参数

    double size;
    double resolution;

    // 从参数服务器获取参数
    nh.param("size", size, 100.0);  // 默认值为100.0
    nh.param("resolution", resolution, 0.05);  // 默认值为0.05

    int w = std::round(size / resolution);
    uint8_t *image_data = new uint8_t[w * w];

    // 初始化图像数据为255
    std::fill_n(image_data, w * w, 255);

    // 获取当前包的路径
    std::string package_path = ros::package::getPath("pgm_generator");
    std::string output_path = package_path + "/map/blank_map.pgm";

    std::ofstream file(output_path, std::ios::binary);
    if (file.is_open()) {
        file << "P5\n" << w << " " << w << "\n255\n"; // map head param
        file.write(reinterpret_cast<char*>(image_data), w * w);
        file.close();
        ROS_INFO("PGM has created.");
        ROS_INFO("Map size: %f m.", size);
        ROS_INFO("Map resolution: %f m/p.", resolution);
    } else {
        ROS_ERROR("Failed to create.");
    }

    delete[] image_data;
    return 0;
}

这个文件依赖 roslib包，但不能targetlink。

编写launch文件：

<launch>
  <!-- 定义参数 -->
  <arg name="map_size" value="100.0" />
  <arg name="resolution" value="0.05" />

  <!-- 启动节点 -->
  <node name="pgm_generator" pkg="pgm_generator" type="pgm_generator_node" output="screen">
    <param name="map_size" value="$(arg map_size)" />
    <param name="resolution" value="$(arg resolution)" />
  </node>
</launch>

该地图还可以使用ps进行修改，后续进阶可以使用雷达、视觉建图生成。

---

7.2 map_server

map_server作用：

• 加载地图文件，转为地图话题信息发布。
• 保存地图。

这里只用前者，使用map_server加载地图文件，需要配置地图描述文件，新建_map_config.yaml文件：

image: /home/.../blank_map.pgm ## 地图文件
resolution: 0.050000 ## 地图的分辨率，单位是米/像素
origin: [-50.000000, -50.000000, 0.000000] ## 地图原点，地图坐标系相对世界坐标系的平移和旋转
occupied_thresh: 0.65 ## 占用栅格的阈值，大于该值的像素表示被占用
free_thresh: 0.196 ## 空闲栅格的阈值，小于该值的像素表示空闲
negate: 0 ## 0表示地图中白色为自由空间，黑色为占用空间；1表示相反

一般map_server的使用结合整个launch文件设计：

<arg name="map_file" default="$(find simple_navigation)/map/_map_config.yaml" />
<node name="map_server_for_test" pkg="map_server" type="map_server" args="$(arg map_file)" />

map_server：

• 发布话题：/map
  • 消息类型：nav_msgs/OccupancyGrid
• 发布话题：/map_metadata
  • 消息类型：nav_msgs/MapMetaData

查看效果：

---

7.3 雷达的使用

7.3.1 单线雷达

lidar：M10 Navigation & Obstacle Avoidance LiDAR | Leishen 2D LiDAR Supplier (lslidar.com)

雷达固定ip：192.168.1.102，端口：2368

安装依赖：

sudo apt-get install ros-$ROS_DISTRO-pcl-ros ros-$ROS_DISTRO-pluginlib  ros-$ROS_DISTRO-pcl-conversions  ros-$ROS_DISTRO-diagnostic-updater

sudo apt-get install libpcap-dev 
sudo apt-get install libboost${BOOST_VERSION}-dev   #选择适合的版本

移植驱动，编译：

catkin_make -DCATKIN_WHITELIST_PACKAGES="lslidar_msgs"
catkin_make -DCATKIN_WHITELIST_PACKAGES="lslidar_driver"

运行：roslaunch lslidar_driver lslidar_net.launch

<launch>
  <node pkg="lslidar_driver" type="lslidar_driver_node" name="lslidar_driver_node" output="screen">
    <param name="frame_id" value="laser_link"/>          #激光坐标
    <param name="device_ip" value="192.168.1.200"/>      #雷达源IP
    <param name="device_port" value="2368"/>             #雷达目的端口号
    <param name="device_ip_difop" value="192.168.1.102"/>#雷达目的ip
    <param name="difop_port" value="2369"/>              #雷达源端口号
    <param name="lidar_name" value="M10_P"/>               #雷达选择:M10 M10_P M10_PLUS M10_GPS N10 N10_PLUS L10
    <param name="interface_selection" value="net"/>      #接口选择:net 为网口,serial 为串口
    <param name="add_multicast" value="false"/>          #是否启动组播。
    <param name="group_ip" value="224.1.1.2"/>           #组播ip。
    <param name="scan_topic" value="scan"/>              #设置激光数据topic名称
    <param name="angle_disable_min" value="0.0"/>        #角度裁剪开始值
    <param name="angle_disable_max" value="0.0"/>        #角度裁剪结束值
    <param name="min_range" value="0"/>                  #雷达接收距离最小值
    <param name="max_range" value="100.0"/>              #雷达接收距离最大值
    <param name="use_gps_ts" value="false"/>             #雷达是否使用GPS授时
    <param name="compensation" value="false"/>           #雷达是否使用角度补偿功能
    <param name="pubScan" value="true"/>                 #雷达是否发布scan话题
    <param name="pubPointCloud2" value="false"/>         #雷达是否发布pointcloud2话题
    <param name="high_reflection" value="false"/>        #M10_P雷达需填写该值,若不确定，请联系技术支持。
    <!--param name="pcap" value="$(find lslidar_driver)/pcap/xxx.pcap"/-->     #雷达是否使用pcap包读取功能
  </node>
</launch>

查看话题：

• /scan：雷达数据
• /lslidar_order：雷达控制

对于雷达控制：

#M10 M10_GPS M10_PLUS N10 N10_PLUS 雷达
rostopic pub -1 /lslidar_order std_msgs/Int8 "data: 0"		#雷达停止旋转
rostopic pub -1 /lslidar_order std_msgs/Int8 "data: 1"		#雷达开始旋转

#M10 M10_GPS M10_PLUS 雷达
rostopic pub -1 /lslidar_order std_msgs/Int8 "data: 2"		#雷达点云不滤波
rostopic pub -1 /lslidar_order std_msgs/Int8 "data: 3"		#雷达点云正常滤波
rostopic pub -1 /lslidar_order std_msgs/Int8 "data: 4"		#雷达近距离滤波
rostopic pub -1 /lslidar_order std_msgs/Int8 "data: 100"    #雷达发设备包

---

7.3.2 多线雷达

雷达：32/16-Line Mechanical LiDAR | Leishen Intelligent System (lslidar.com)

雷达固定ip：192.168.1.102，端口：2368

安装依赖：

sudo apt-get install ros-$ROS_DISTRO-pcl-ros ros-$ROS_DISTRO-pluginlib  ros-$ROS_DISTRO-pcl-conversions 

sudo apt-get install libpcap-dev
sudo apt-get install libboost${BOOST_VERSION}-dev   #选择适合的版本

移植驱动，编译：

catkin_make -DCATKIN_WHITELIST_PACKAGES="lslidar_msgs"
catkin_make -DCATKIN_WHITELIST_PACKAGES="lslidar_cx_driver"

运行：roslaunch lslidar_cx_driver lslidar_c16.launch

<launch>
  <arg name="device_ip" default="192.168.1.200" />  //雷达ip
  <arg name="msop_port" default="2368"/>   //数据包目的端口
  <arg name="difop_port" default="2369"/>   //设备包目的端口
  <arg name="use_gps_ts" default="false" />  //雷达是否使用gps或ptp授时，使用改为true
  <arg name="pcl_type" default="false" />   //点云类型，默认false点云中的点为xyzirt字段。改为true，点云中的点为xyzi字段。
  <arg name="lidar_type" default="c16"/>   //选择雷达型号,若为c32雷达改为c32
  <arg name="packet_size" default="1206"/>  //udp包长1206或1212,若为1212字节雷达改为1212
  <arg name="c16_type" default="c16_2"/> //c16_2表示16线垂直角度分辨率为2度的雷达，c16_1表示16线垂直角度分辨率为1.33度的雷达
  <arg name="c32_type" default="c32_2"/> //c32_2表示32线垂直角度分辨率为1度的雷达，c32_1表示32线垂直角度分辨率为0.33度的雷达
  <arg name = "c32_fpga_type" default="3"/> //3表示32线fpga为\2.8\3.0的版本的雷达，2表示32线fpga为2.6的版本的雷达

  <node pkg="lslidar_driver" type="lslidar_driver_node" name="lslidar_driver_node" output="screen">
    <!--param name="pcap" value="$(find lslidar_driver)/pcap/123.pcap" /--> //启用离线pcap模式
    <param name="device_ip" value="$(arg device_ip)" />
    <param name="msop_port" value="$(arg msop_port)" />
    <param name="difop_port" value="$(arg difop_port)"/>
    <param name="pcl_type" value="$(arg pcl_type)"/>
    <param name="lidar_type" value="$(arg lidar_type)"/>
    <param name="packet_size" value="$(arg packet_size)"/>
    <param name="c16_type" value="$(arg c16_type)"/>
    <param name="c32_type" value="$(arg c32_type)"/>
    <param name="c32_fpga_type" value="$(arg c32_fpga_type)"/>
    <param name="add_multicast" value="false"/>  // 是否开启组播模式。
    <param name="group_ip" value="224.1.1.2"/>  //组播ip地址
    <param name="use_gps_ts" value="$(arg use_gps_ts)"/> 
    <param name="min_range" value="0.15"/>   //单位，米。雷达盲区最小值，小于此值的点被过滤
    <param name="max_range" value="150.0"/>  //单位，米。雷达盲区最大值 ，大于此值的点被过滤
    <param name="horizontal_angle_resolution" value="0.2"/>  //雷达水平角度分辨率
    <param name="frame_id" value="laser_link"/>  //坐标系id
    <param name="distance_unit" value="0.25"/>   //雷达距离分辨率
    <param name="angle_disable_min" value="0"/>   //雷达裁剪角度开始值 ，单位0.01°
    <param name="angle_disable_max" value="0"/>  //雷达裁剪角度结束值，单位0.01°
    <param name="packet_rate" value="840.0"/>    //离线播放时参数，c16雷达为840.0（双回波1680.0），c32雷达1700.0（双回波3400.0）
    <param name="scan_num" value="10"/>          //laserscan线号
    <param name="read_once" value="false"/>      //pcap离线模式下,是否播放一次
    <param name="publish_scan" value="true"/>  //是否发布laserscan话题，发布改为true
    <param name="pointcloud_topic" value="lslidar_point_cloud"/> //点云话题名称，可修改
    <param name="coordinate_opt" value="false"/> //默认false 雷达零度角对应点云y轴,true雷达零度角对应点云x轴
  </node>
 </launch>

查看话题：

• /lslidar_point_cloud：点云话题
• /laserscan：指定线束的数据，默认10

雷达控制：

source devel/setup.bash  
rosservice call /lslidarcontrol 0 # 下电
rosservice call /lslidarcontrol 1 # 上电

虽然该雷达提供了可选laserscan话题，但：

• pointcloud_to_laserscan提取3d点云信息中与地面平行的那一层，用作下位机导航和避障。

pcl点云转scan：

下载：

git clone -b foxy --single-branc https://github.com/ros-perception/pointcloud_to_laserscan.git

调整对应启动文件：pointcloud_scan.launch

<?xml version="1.0"?>
<launch>
    <!-- 启动 pointcloud_to_laserscan 节点 -->
    <node pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node" name="pointcloud_to_laserscan">
        <!-- 将输入的点云话题重映射为 /lslidar_point_cloud -->
        <remap from="cloud_in" to="/lslidar_point_cloud"/>
        <!-- 将输出的激光扫描话题重映射为 /scan -->
        <remap from="scan" to="/scan"/>
        <rosparam>
            <!-- 时间同步误差，单位：秒 -->
            transform_tolerance: 0.01

            <!-- 高度范围内的点云数据用于生成2D激光扫描 -->
            min_height: -0.4
            max_height: 1.0

            <!-- 扫描角度范围，单位：弧度 -->
            angle_min: -3.1415926  # -π
            angle_max: 3.1415926   # π

            <!-- 每个测量点之间的角度增量，单位：弧度 -->
            angle_increment: 0.003 # 约 0.17 度

            <!-- 生成一帧激光扫描数据的时间，单位：秒 -->
            scan_time: 0.1

            <!-- 激光扫描的最小和最大检测距离，单位：米 -->
            range_min: 0.2
            range_max: 100

            <!-- 是否使用无穷大表示超出最大检测范围的距离 -->
            use_inf: true

            <!-- 无穷大的精度偏差 -->
            inf_epsilon: 1.0

            <!-- 
            # 并发级别，影响处理点云的队列数量和使用的线程数量
            # 0 : 自动检测核心数
            # 1 : 单线程处理
            # 2->inf : 并行级别
            -->
            concurrency_level: 1
        </rosparam>
    </node>
</launch>

---

7.3.3 laser_filters

用来滤除sensor_msgs/LaserScan类型的雷达数据，可支持：

• Range Filter：范围滤波
• Angular Filter：角度滤波
• Intensity Filter：强度滤波
• Median Filter：中值滤波
• Shadow Filter：阴影滤波
• Bounding Box Filter：界限滤波
• Jump Distance Filter：跳点滤波
• Scan to Scan Filter Chain：复合滤波

下载：

git clone -b noetic-devel --single-branch https://github.com/ros-perception/laser_filters.git

本处使用：Bounding Box Filter

scan_filter_chain:
- name: box
  type: LaserScanBoxFilter
  params:
    box_frame: base_link
    min_x: -0.495
    max_x: 0.495
    min_y: -0.335
    max_y: 0.335
    min_z: -0.130
    max_z: 0.790

原文：laser_filters - ROS Wiki

---

7.3.4 gampping

这是一种建图算法。

下载：

git clone https://github.com/ros-perception/openslam_gmapping
git clone https://github.com/ros-perception/slam_gmapping.git

---

7.4 odom

7.4.1 坐标系转换

ROS中坐标系的转换：

<!-- 静态TF1 -->
<node pkg="tf" type="static_transform_publisher" name="map_to_odom" args="0 0 0 0 0 0 /map /odom 10" />
<!-- 静态TF2 -->
<node pkg="tf" type="static_transform_publisher" name="base_link_to_lslidar" args="0 0 0 0 0 0 /base_link /laser_link 10" 

参数含义：x偏移量 y偏移量 z偏移量 z偏航角度 y俯仰角度 x翻滚角度 父级坐标系 子级坐标系 发布频率

---

7.4.2 urdf

在rviz中显示车体外型：

<!-- URDF 标准化机器人描述格式 -->
<param name = "robot_description" textfile = "$(find turn_on_robot)/urdf/akm_robot.urdf" /> 

<!-- 读取urdf信息(robot_description) 发布话题：/joint_states -->
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />

<!-- 订阅话题：/joint_states，发布小车TF信息 -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />

   <node pkg="tf" type="static_transform_publisher" name="base_to_laser" args="0.26 0.00 0.228 3.14 0 0  base_footprint laser 100" />
   <node pkg="tf" type="static_transform_publisher" name="base_to_camera" args="0.34 0.00 0.178 0 0 0   base_footprint camera_link 100" />
   <node pkg="tf" type="static_transform_publisher" name="base_to_gyro" args="0 0 0 0 0 0 base_footprint gyro_link 100" />

其中 akm_robot.urdf ：

<?xml version="1.0" ?>
<robot name="akm_robot">

    <link name="base_link">
        <visual>
            <origin xyz=" 0.15 0 0.065" rpy="0 0 0" />
            <geometry>
                <box size="0.39 0.24 0.06"/>
            </geometry>
            <material name="white">
                <color rgba="1 1 1 1"/>
            </material>
        </visual>
    </link>   

    <joint name="left_wheel_joint" type="continuous">
        <origin xyz="0 0.162 0.05" rpy="1.57 0 0"/>
        <parent link="base_link"/>
        <child link="left_wheel_link"/>
        <axis xyz="0 1 0"/>
    </joint>

    <link name="left_wheel_link">
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <geometry>
                <cylinder radius="0.05" length = "0.04"/>
            </geometry>
            <material name="black">
                <color rgba="0 0 0 1"/>
            </material>
        </visual>
    </link>

    <joint name="right_wheel_joint" type="continuous">
        <origin xyz="0 -0.162 0.05" rpy="1.57 0 0"/>
        <parent link="base_link"/>
        <child link="right_wheel_link"/>
        <axis xyz="0 1 0"/>
    </joint>

    <link name="right_wheel_link">
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <geometry>
                <cylinder radius="0.05" length = "0.04"/>
            </geometry>
            <material name="black">
                <color rgba="0 0 0 1"/>
            </material>
        </visual>
    </link>

      <joint name="left_front_joint" type="continuous">
        <origin xyz="0.31 0.162 0.05" rpy="1.57 0 0"/>
        <parent link="base_link"/>
        <child link="left_front_link"/>
        <axis xyz="0 1 0"/>
    </joint>

    <link name="left_front_link">
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <geometry>
                <cylinder radius="0.05" length = "0.04"/>
            </geometry>
            <material name="black">
                <color rgba="0 0 0 1"/>
            </material>
        </visual>
    </link>

    <joint name="right_front_joint" type="continuous">
        <origin xyz="0.31 -0.162 0.05" rpy="1.57 0 0"/>
        <parent link="base_link"/>
        <child link="right_front_link"/>
        <axis xyz="0 1 0"/>
    </joint>

    <link name="right_front_link">
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <geometry>
                <cylinder radius="0.05" length = "0.04"/>
            </geometry>
            <material name="black">
                <color rgba="0 0 0 1"/>
            </material>
        </visual>
    </link>
    
</robot>

---

7.4.3 nav里程计

---

7.5 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	提供机器人在地图中的初始位置，用于初始化定位模块

发布话题：

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

7.5.1 launch

选择使用动态避障效果更好的teb局部规划器。

安装：

sudo apt update
sudo apt install ros-noetic-teb-local-planner

move_base核心，参数文件加载：

<!-- move_base核心 -->
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
  <rosparam file="$(find akm_robot)/param/move_base_params.yaml"  command="load" />
  
  <rosparam file="$(find akm_robot)/param/local_costmap_params.yaml"  command="load" />
  <rosparam file="$(find akm_robot)/param/global_costmap_params.yaml"  command="load" />
  
  <rosparam file="$(find akm_robot)/param/local_planner_param.yaml"  command="load" />
  <rosparam file="$(find akm_robot)/param/global_planner_param.yaml"  command="load" />
  
  <rosparam file="$(find akm_robot)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
  <rosparam file="$(find akm_robot)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
</node>

下面进行move_base的参数文件加载，参数文件有：

• move_base_params.yaml
• local_costmap_params.yaml
• global_costmap_params.yaml
• local_planner_param.yaml
• global_planner_param.yaml
• costmap_common_params.yaml

---

7.5.2 move_base_params

move_base_params.yaml：负责move_base基本设置

#设置全局路径规划器
#base_global_planner: "carrot_planner/CarrotPlanner"
#base_global_planner: "navfn/NavfnROS" 
base_global_planner: "global_planner/GlobalPlanner"

#设置局部路径规划器
#base_local_planner: "dwa_local_planner/DWAPlannerROS"
base_local_planner: "teb_local_planner/TebLocalPlannerROS"

#发布机器人速度控制命令话题cmd_vel的频率，单位：Hz
controller_frequency: 10.0 

#全局路径规划器的规划频率，如果设置为0，仅在设置目标点位置时执行一次，单位：Hz
planner_frequency: 1.0  

#路径规划失败后，尝试重新规划几次后才执行恢复行为，如果设置为-1，代表无限重试
max_planning_retries: 1  

#当move_base在不活动状态时,是否关掉代价地图的更新
shutdown_costmaps: false  

#配置恢复行为
recovery_behaviors:  
  #自定义恢复行为名称
  - name: 'recovery_behavior_1'  
  #选择恢复行为类型
    type: 'clear_costmap_recovery/ClearCostmapRecovery'  
  #自定义恢复行为名称
  - name: 'recovery_behavior_2'  
  #选择恢复行为类型
    type: 'rotate_recovery/RotateRecovery'  
  #自定义恢复行为名称
  - name: 'recovery_behavior_3' 
  #选择恢复行为类型 
    type: 'move_slow_and_clear/MoveSlowAndClear'  
 
#是否开启恢复行为，这里选择不开启，因为经测试恢复行为用处不大，开启后比较浪费时间。
recovery_behavior_enabled: true 
#是否开启恢复行为中控制机器人旋转的恢复行为。注意，此参数仅在move_base使用默认恢复行为时使用。
clearing_rotation_allowed: true 

#执行恢复行为时，与机器人距离3米外的障碍物会被清除，单位：s。注意，此参数仅在move_base使用默认恢复行为时使用。
conservative_reset_dist: 3.0

#路径规划无法成功多长时间后，执行恢复行为，单位：s
planner_patience: 3.0  
#没有接收到有些控制命令多长时间后，执行恢复行为，单位：s
controller_patience: 3.0 

#当机器人在运动，但是运动幅度不大于多少时，认为机器人处于振荡状态，单位：m
oscillation_distance: 0.02  
#机器人处于振荡状态多久后，执行恢复行为，单位：s
oscillation_timeout: 10.0 

#设置恢复行为参数
recovery_behavior_1:
  #与机器人距离5米外的障碍物会被清除
  reset_distance: 1.0  
recovery_behavior_2:
  #与机器人距离3米外的障碍物会被清除
  reset_distance: 3.0  
recovery_behavior_3:
  #与机器人距离1米外的障碍物会被清除
  clearing_distance: 5.0
  #限制恢复行为时机器人的线速度，单位：m/s
  limited_trans_speed: 0.1  
  #限制恢复行为时机器人的角速度，单位：rad/s
  limited_rot_speed: 0.4  
  #完成该恢复行为机器人必须移动的距离，单位：m
  limited_distance: 0.3  
  #对应的局部路径规划器的名称

---

7.5.3 local_costmap_params

local_costmap_params.yaml：特定局部代价地图参数设置

#局部代价地图参数命名空间
local_costmap:
  #代价地图的TF参考坐标系
  global_frame: odom # map
  #机器人的TF坐标名称
  robot_base_frame: base_link # base_footprint
  #global_frame和robot_base_frame间的TF坐标停止发布多久后，控制机器人停止，单位：s
  transform_tolerance: 0.5
  #代价地图刷新频率，单位：Hz
  update_frequency: 5.0
  #代价地图的可视化话题发布频率，单位：Hz
  publish_frequency: 3.0

  #是否直接使用静态地图生成代价地图
  #static_map: false #使用plugins手动配置代价地图时，该参数无效
  #代价地图是否跟随机器人移动，static_map为true时该参数必须为false
  rolling_window: true
  #代价地图宽度，单位：m
  width: 4.0 
  #代价地图高度，单位：m
  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】内进行设置

---

7.5.4 global_costmap_params

global_costmap_params.yaml：特定全局代价地图参数设置

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

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

---

---

7.5.5 local_planner_param

local_planner_param.yaml：局部规划器参数设置

#局部路径规划器TebLocalPlannerROS命名空间
TebLocalPlannerROS:
  odom_topic: odom #订阅的里程计话题
  map_frame: map #代价地图的TF参考坐标系

  #机器人参数
  max_vel_x: 0.5 #最大x前向速度，单位：m/s
  max_vel_y: 0.0 #最大y前向速度，单位：m/s，非全向移动小车需要设置为0
  max_vel_x_backwards: 0.5 #最大后退速度，单位：m/s
  max_vel_theta: 1.5  #最大转向角速度，单位：rad/s
  acc_lim_x: 0.2  #最大x向加速度，单位：m/s^2
  acc_lim_y: 0.0  #最大y向加速度，，单位：m/s^2,非全向移动小车需要设置为0
  acc_lim_theta: 0.3  #最大角加速度，单位：rad/s^2

  #阿克曼小车参数，非阿克曼小车设置为0
  min_turning_radius: 0.750 #机器人最小转弯半径
  wheelbase: 0.322 #机器人轴距，前轮与后轮的距离
  cmd_angle_instead_rotvel: False #true则cmd_vel/angular/z内的数据是舵机角度
                                  #无论是不是阿克曼小车都设置为false，因为我们的阿克曼机器人内部进行了速度转换

  #用于局部路径规划的机器人外形
  #机器人外形的类型可以为：point、circular、two_circles、line、polygon，默认为point类型
  footprint_model:    
    #type: point #point类型不需要设置其它参数

    #type: circular #圆形类型，需要设置圆的半径
    #radius: 0.3

    #type: two_circles #两个圆类型，需要设置两个圆的位置和半径
    #front_offset: 0.2 #前面的圆的位置，相对机器人中心
    #front_rasius: 0.2 #前面的圆的半径
    #rear_offset : 0.2 #后面的圆的位置，相对机器人中心
    #rear_rasius : 0.2 #前面的圆的半径

    #type: line #两条线类型，需要设置两条线的位置
    #line_start: [0.00, 0.0]
    #line_end: [0.7, 0.0]
    
    #type: polygon #多边形类型，需要设置各顶点的坐标值
    #vertices: [[-0.133, -0.125],[-0.133, 0.125],[0.133,0.125],[0.133, -0.125]]

    type: polygon
    vertices: [[-0.09, -0.185], [-0.09, 0.185], [0.4,0.185],  [0.4, -0.185]]   #机器人形状 for senior_akm

  #障碍物参数
  min_obstacle_dist: 0.1 #和障碍物最小距离，直接影响机器人避障效果
  include_costmap_obstacles: True #是否将动态障碍物预测为速度模型，
  costmap_obstacles_behind_robot_dist: 1.5 #限制机器人后方规划时考虑的局部成本地图障碍物
  obstacle_poses_affected: 15  #障碍物姿态受影响0~30
  costmap_converter_plugin: ""
  costmap_converter_spin_thread: True
  costmap_converter_rate: 5
  include_dynamic_obstacles: True 
  dynamic_obstacle_inflation_dist: 0.6

  #目标点误差允许值
  xy_goal_tolerance: 0.2  #机器人到达目标点时附近时的弧度偏差允许量，在该偏差内认为已经到达目标点，单位为：m
  yaw_goal_tolerance: 0.1 #机器人到达目标点时附近时的弧度偏差允许量，在该偏差内认为已经到达目标点单位为：rad
  free_goal_vel: False    #允许机器人以最大速度驶向目的地

  #轨道配置参数
  teb_autosize: True #优化期间允许改变轨迹的时域长度
  dt_ref: 0.45 #局部路径规划的解析度# minimum 0.01
  dt_hysteresis: 0.1 #允许改变的时域解析度的浮动范围， 一般为 dt_ref 的 10% 左右 minimum0.002
  global_plan_overwrite_orientation: False #覆盖全局路径中局部路径点的朝向
  max_global_plan_lookahead_dist: 3.0 #考虑优化的全局计划子集的最大长度
  feasibility_check_no_poses: 5 #检测位姿可到达的时间间隔 minimum 0

  #轨迹优化参数
  no_inner_iterations: 5
  no_outer_iterations: 4
  optimization_activate: True
  optimization_verbose: False
  penalty_epsilon: 0.1
  obstacle_cost_exponent: 4
  weight_max_vel_x: 2
  weight_max_vel_theta: 1
  weight_acc_lim_x: 1
  weight_acc_lim_theta: 1
  weight_kinematics_nh: 1000
  weight_kinematics_forward_drive: 1
  weight_kinematics_turning_radius: 1
  weight_optimaltime: 1 #必须大于0
  weight_shortest_path: 0
  weight_obstacle: 100
  weight_inflation: 0.2
  weight_dynamic_obstacle: 10 
  weight_dynamic_obstacle_inflation: 0.2
  weight_viapoint: 1
  weight_adapt_factor: 2

  #不同拓扑中的并行规划
  enable_homotopy_class_planning: False
  enable_multithreading: True
  max_number_classes: 4
  selection_cost_hysteresis: 1.0
  selection_prefer_initial_plan: 0.95
  selection_obst_cost_scale: 1.0
  selection_alternative_time_cost: False
  roadmap_graph_no_samples: 15
  roadmap_graph_area_width: 5
  roadmap_graph_area_length_scale: 1.0
  h_signature_prescaler: 0.5
  h_signature_threshold: 0.1
  obstacle_heading_threshold: 0.45
  switching_blocking_period: 0.0
  viapoints_all_candidates: True
  delete_detours_backwards: True
  max_ratio_detours_duration_best_duration: 3.0
  visualize_hc_graph: False
  visualize_with_time_as_z_axis_scale: False

  #恢复行为
  shrink_horizon_backup: True
  shrink_horizon_min_duration: 10
  oscillation_recovery: False
  oscillation_v_eps: 0.1
  oscillation_omega_eps: 0.1
  oscillation_recovery_min_duration: 10
  oscillation_filter_duration: 10

---

7.5.6 global_planner_param

global_planner_param.yaml：全局规划器参数设置

#全局路径规划器GlobalPlanner命名空间
GlobalPlanner:  
  #是否使用dijkstra算法进行路径规划，false则选择A*算法进行路径规划
  use_dijkstra: true

  #是否使用二次逼近法进行计算，false则使用更简单的计算方法
  use_quadratic: true

  #如果出于某种原因，您想要global_planner精确地反映navfn的行为，则将其设置为true(并为其他布尔参数使用默认值)                                
  old_navfn_behavior: false

  #是否发布Potential代价地图
  publish_potential: true

  #如果为true，创建一条遵循网格边界的路径。false，则使用梯度下降法。
  use_grid_path: false

  #是否允许路径穿过代价地图的未知区域
  allow_unknown:  true  

  #允许的创建的路径规划终点与设置目标点的偏差为多少，单位：m
  default_tolerance: 0.0

  #是否对通过PointCloud2计算出来的potential area进行可视化
  visualize_potential: false

  lethal_cost: 253
  neutral_cost: 50
  cost_factor: 3.0
  #How to set the orientation of each point 
  orientation_mode: 0
  #What window to use to determine the orientation based on the position derivative specified by the orientation mode
  orientation_window_size: 1
  #用完全占用方格勾勒出全局代价地图。对于“非static_map”(rolling_window)的全局代价地图，需要将其设置为false
  outline_map: true

---

7.5.7 costmap_common_params

costmap_common_params.yaml：代价地图通用参数设置

# 圆形机器人的外形设置，直接设置其外形半径
# robot_radius: 0.4
# 这里依次设置的是机器人的右下角、左下角、左上角、右上角的顶点坐标
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

---

7.6 多点导航

move_base提供了MoveBaseAction消息接口可实现多点导航。

7.6.1 巡线打点

导航点可以来自多个方面，下面给出巡线自动记录点位和姿态的代码，并保存在nav_data/map/nav_goals.csv中。

7.6.2 多点导航

新建pub_goals包：

#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <geometry_msgs/PoseArray.h>
#include <fstream>
#include <string>
#include <vector>
#include <sstream>

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

void simple_move(float x, float y, float w, float z)
{
    MoveBaseClient ac("move_base", true);

    // Wait for the action server to come up
    ac.wait_for_server();

    move_base_msgs::MoveBaseGoal goal;

    // Set up the frame parameters
    goal.target_pose.header.frame_id = "map";
    goal.target_pose.header.stamp = ros::Time::now();

    // Define a position and orientation for the robot to reach
    goal.target_pose.pose.position.x = x;
    goal.target_pose.pose.position.y = y;
    goal.target_pose.pose.orientation.w = w;
    goal.target_pose.pose.orientation.z = z;

    // Send the goal position and orientation for the robot to reach
    ROS_INFO("Sending goal: x: %f, y: %f, w: %f, z: %f", x, y, w, z);
    ac.send_goal(goal);

    // Wait for the result
    ac.wait_for_result();

    if (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
        ROS_INFO("Hooray, the base moved to the goal");
    else
        ROS_INFO("The base failed to move to the goal for some reason");
}

void talker(std::vector<std::vector<float>> goals)
{
    ros::NodeHandle nh;
    ros::Publisher pub = nh.advertise<geometry_msgs::PoseArray>("simpleNavPoses", 100);
    geometry_msgs::PoseArray array;

    array.header.frame_id = "map";
    array.header.stamp = ros::Time::now();

    for (const auto &goal : goals)
    {
        geometry_msgs::Pose pose;
        pose.position.x = goal[0];
        pose.position.y = goal[1];
        pose.orientation.w = goal[2];
        pose.orientation.z = goal[3];
        array.poses.push_back(pose);
    }

    ros::Rate rate(1);
    int count = 0;
    while (count < 1)
    {
        pub.publish(array);
        rate.sleep();
        count++;
    }
}

std::vector<std::vector<float>> load_goals_from_csv(const std::string &filename)
{
    std::ifstream file(filename);
    std::string line;
    std::vector<std::vector<float>> goals;

    while (std::getline(file, line))
    {
        std::stringstream ss(line);
        std::string token;
        std::vector<float> goal;
        while (std::getline(ss, token, ','))
        {
            goal.push_back(std::stof(token));
        }
        goals.push_back(goal);
    }

    return goals;
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "simple_move");

    try
    {
        std::string filename = ros::package::getPath("nav_data") + "/map/nav_goals.csv";
        std::vector<std::vector<float>> goals = load_goals_from_csv("nav_goals.csv");

        for (const auto &goal : goals)
        {
            simple_move(goal[0], goal[1], goal[2], goal[3]);
            talker(goals);
            ROS_INFO("Goal reached");
            ros::Duration(10.0).sleep();
        }
    }
    catch (const std::exception &e)
    {
        ROS_ERROR("Exception: %s", e.what());
    }

    return 0;
}

---

7.7 导航

新建nav.launch文件：

<launch>

	<!-- 1. 空白地图的地图加载 -->
    <arg name="map_config" default="$(find nav_data)/map/_map_config.yaml"/>
    <node name ="map_server" pkg="map_server" type="map_server" output="screen" respawn="false" args="$(arg map_config)"/>

	<!-- 2. rtk_imu 融合里程计 -->
	<include file="$(find imu_gnss_eskf )/launch/imu_gnss_eskf.launch" />

	<!-- 3. 启动雷达 -->
	<include file="$(find lslidar_driver)/launch/lslidar_net.launch" />

	<!-- 4. 导航核心 -->
	<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
		<rosparam file="$(find nav_data)/param/move_base_params.yaml"  command="load" />
		<rosparam file="$(find nav_data)/param/local_costmap_params.yaml"  command="load" />
		<rosparam file="$(find nav_data)/param/global_costmap_params.yaml"  command="load" />
		<rosparam file="$(find nav_data)/param/local_planner_param.yaml"  command="load" />
		<rosparam file="$(find nav_data)/param/global_planner_param.yaml"  command="load" />
		<rosparam file="$(find nav_data)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
		<rosparam file="$(find nav_data)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
	</node>

	<!-- 静态TF1 -->
	<node pkg="tf" type="static_transform_publisher" name="map_to_odom" args="0 0 0 0 0 0 /map /odom   10" />
	<!-- 静态TF2 -->
	<node pkg="tf" type="static_transform_publisher" name="base_link_to_lslidar" args="0 0 0 0 0 0 /base_link /laser_link   10" />

	<!-- 在rviz中显示-->
	<!-- <node name="rviz" pkg="rviz" type="rviz" args="-d $(find nav_data)/config/rviz.rviz" required="true" /> -->
</launch>