从之前使用gazebo仿真小车的过程中，明白了流程，本片文章不考虑你会不会真正的编程，你只需要会AI即可，但是还是需要提醒，AI虽然好，但是不能完全依赖，还是需要掌握AI输出的知识。

概述

其实通过ROS编写机器人自主导航的第一步就是掌握需要掌握的内容然后使用这个流程，在每一次的使用中慢慢积累。了解整个树干，就知道每个树枝的作用了。

本次主要是项目要求在制作的自动驾驶工程车，主要用于地下。在任务开始之前呢我会放一张思维导图用来明确的指引方向。其他次关于知识的补充大多数来自于b站的小虎哥哥还有机器人阿杰，鱼香ros这几位作者。小虎哥哥是实战派，他的精品课很不错。

思维导图

第一步构建工作空间

1.构建工作空间主要是一个项目的根，没有工作空间，就不能运行项目。工作空间中一般分为三个文件：

src、build、devel目录的含义

在软件开发或系统管理中，src、build、devel是常见的目录名称，通常用于组织和管理项目文件。以下是它们的详细解释：

src

src是source的缩写，表示源代码目录。这个目录存放项目的所有源代码文件，包括头文件、源文件、配置文件等。它是开发过程中最核心的目录，包含程序的原始实现。

• 存放语言相关的源代码（如.c、.cpp、.py、.java等）。
• 可能包含子目录以模块化组织代码。
• 通常不包含编译生成的中间文件或二进制文件。

build

build是编译目录，用于存放编译过程中生成的中间文件和最终的可执行文件。它是一个临时目录，内容通常可以通过清理操作删除并重新生成。

• 存放编译器生成的中间文件（如.o、.obj）。
• 包含最终链接的可执行文件或库文件。
• 通常通过cmake、make等工具自动生成。
• 支持分离源代码和编译产物，保持src目录的纯净。

devel

devel是development的缩写，常见于ROS（机器人操作系统）或其他大型项目中，用于存放开发环境所需的文件。

• 包含开发时需要的符号链接、环境变量配置等。
• 存放未完全安装的中间产物（如ROS中的setup.bash）。
• 便于开发者测试和调试，无需全局安装。
• 通常通过工具链（如catkin_make）自动生成。

总结

• src：存放原始代码，是开发的基础。
• build：存放编译生成的临时文件和最终二进制文件。
• devel：提供开发环境所需的配置和中间状态文件。

第二步构建功能包

概述：工作空间中有多个功能包，每个功能包存放不同的功能，通过多个功能的配合以及功能包的话题交流配合。

构建功能包

创建ROS功能包及标准目录结构

以下脚本实现了ROS功能包的一键创建和标准目录初始化，同时包含编译说明：

function create_ros_pkg() {
    if [ $# -lt 1 ]; then
        echo "用法：create_ros_pkg <包名> [依赖1] [依赖2] ..."
        return 1
    fi
    pkg_name=$1
    shift
    catkin_create_pkg $pkg_name $@
    cd $pkg_name
    mkdir -p launch config scripts msg srv action
    echo "已创建文件夹：launch, config, scripts, msg, srv, action"
    cd ..
}

比如说想在多加一个文件夹用来编写修改日记的，就可以在mkdir中的后面空格 “readme”

功能包依赖管理

创建时可以指定依赖项：

create_ros_pkg my_pkg roscpp rospy std_msgs

1.关于这个指定的依赖，不能随便定义，会混乱，会需要什么就定义什么。

2.关于功能包的名字，不能使用大写，会出现警告。

编译新创建的功能包

在功能包创建完成后，执行以下编译步骤：

cd ~/catkin_ws
catkin_make
source devel/setup.bash

目录结构说明

标准ROS功能包包含以下目录：

• launch: 存放启动文件
• config: 配置文件目录
• scripts: Python脚本存放位置
• msg: 自定义消息定义
• srv: 服务定义文件
• action: Action定义文件

编译后验证

使用以下命令验证功能包是否成功编译：

rospack find <包名>
rosmsg list | grep <包名>

在这里我构建的功能包有：1.传感器类功能包（imu、镭神c16雷达（由于是事物，用官方驱动、编码器）2.自适应蒙特卡洛EKF、3.AMCL定位、4.move_base3d、5.通信

第三步配置FAST-LIO算法复现与雷达驱动

关于FAST-LIO的复现

这里主要使用FAST-LIO香港大学郑博开源的算法。后面可能会根据相机去修改算法使用FAST-livo，再次对香港大学的slam研究室表示感谢，为slam领域提供了很多帮助！

关于复现这里主要参考：非常详细的fastlio配置https://blog.csdn.net/qq_53004665/article/details/143211245?spm=1001.2101.3001.6650.8&utm_medium=distribute.pc_relevant.none-task-blog-2%7Edefault%7EBlogCommendFromBaidu%7ERate-8-143211245-blog-140084806.235%5Ev43%5Econtrol&depth_1-utm_source=distribute.pc_relevant.none-task-blog-2%7Edefault%7EBlogCommendFromBaidu%7ERate-8-143211245-blog-140084806.235%5Ev43%5Econtrol&utm_relevant_index=101.关于FAST-LIO
其包都好下，就是这个kd树不好找，这里我插一嘴

如果卡住了一直下载不下来就使用下面的镜像：

git clone https://gitee.com/XYZ_Galaxy/ikd-Tree.git

然后回到工作空间src之前的文件夹，编译就好。

编译完成之后就下载包去运行尝试以下FAST_LIO是否好用，具体方法在上面的复现教程里面。

这里提供方针包的下载，来源于复现带有方针数据包的https://blog.51cto.com/u_15699099/7871150找到这个位置，点进去下载下载这个包就行：

进入你下载这个包的文件夹内右键打开终端，然后运行之前查看所需要的话题：

rosbag info demo01_velodyne.bag

把你包里的雷达话题放在FASTLIO的话题中，保存启动包和FAST-LIO的Velodyne

启动：
j

镭神c16雷达配置

简介

雷达的包这里不给提供，无权提供。找购买雷达的克服或在经历拿出你们购买的发票发给他们就会给你资料，这里展示一下如何将驱动放入工作空间和IP配置：

步骤：

1.确认雷达连接成功，ip灯在闪烁。

2.确认雷达的ip，抓包通过wireshark的混合模式。

3.使用udp连接的所以要用端口（端口在.h文件中）

4.通过抓包wireshark找到雷达的ip 192.192.192.200，电脑ip 192.192.192.102

5.根据找到的ip设置电脑的ipv4 然后ping 通雷达的ip、

6.进入雷达的启动文件，修改ip，把192.168.1.200改成192.192.192.102

电脑的ip ping一下，这个其实找雷达技术帮你解决就行，就是比较麻烦。

上位机验证

1.通过上面的抓包知道自己的IP地址

2.通过打开上位机软件

如果显示搜索不到，1.检测是否能ping通，2.检查ipv4是否正确，3.使用wireshark检查一下是否有数据，4.检查防火墙是否关闭，5.npcap是否下载

FSAT-LIO的输出

通过算法，融合imu与雷达输出的叫做激光里程计，在狭长无变化的环境种容易退化。一般会有imu的补偿。

第四步陀螺仪的配置

产品概述：

关于imu我购买了亚博智能家的，不得不说还不错。就是不给提供imu的滤波算法（推荐EKF紧耦合，就是之间调取imu原始数据）。

我手里这款YJ901b的IMU是维特智能家的，如果需要自己设计imu使用他们家的imu就行，但是亚博家的带imu带板子的外围电路直接type-c数据传输供电，需要168元。维特单就一个imu130元。亚博提供教程，他们俩都提供上位机。

关于imu 使用：

imu的传输

配置好传感器的波特率（每秒传输的信号单元（符号或脉冲）数量，单位为“波特”（Baud））一般是9600，但是IMU需要高频率，所以尽量波特率高一点，频率也高一点（下面是配置）

imu的校准

硬件校准

根据上面图片去校准，关于磁场校准，旋转球形拟合发去校准

第一步：拿出手机找出指南针，找到正北方向。

第二步：按照xy平面垂直北方旋转一道两圈（没有反正）

图片来源于亚博智能

按照xz平面垂直与正北方时旋转

图片来源于亚博智能

按照zy平面垂直正北方旋转一到两圈。

图片来源于亚博智能

当校准完成就是这个样子。他会自动补偿，imu嘎嘎稳定。找到亚博智能的imu的ROS启动按照步骤配置一下就可以使用

亚博智能10轴IMUhttps://www.yahboom.com/study/IMU

图片来源于亚博智能

确认之后完成配置：这里要注意的时波特率要匹配，在luanch中波特率需要修改。

编译启动在rviz中查看，在rviz中使用的插件叫imu-tool，这是这个包里自带的，如果不启动这个包就没有，如果要自己下载看这一个：imu-tool下载https://blog.csdn.net/learning_tortosie/article/details/103189118/?ops_request_misc=&request_id=&biz_id=102&utm_term=imu%20tools&utm_medium=distribute.pc_search_result.none-task-blog-2~all~sobaiduweb~default-3-103189118.142^v102^pc_search_result_base6&spm=1018.2226.3001.4187

经过imu校准之后挺稳定，只要远离干扰就会非常稳定。

软件校准

由于系统误差，还有其他干扰，

imu的使用

imu原理概述：

IMU（Inertial Measurement Unit，惯性测量单元）是一种用于测量物体三轴姿态角（或角速率）以及加速度的电子设备。它通常由加速度计、陀螺仪和磁力计（9轴，带气压计为10轴）组成，能够提供物体的运动状态信息。

imu的数据如何处理，接收到四元数如何发送给ekf或者其他算法。

1.由于我购买的是亚博智能家的imu板子，板子上是维特智能的芯片，使用上位机也是维特家的，维特的imu可以配置输出是否需要四元数、欧拉角、加速度、磁力计之类的，需要的勾选上，在ros上启动imu，使用  rosrun echo /imu/data 读取详细参数，就会看见这些信息是在一直给ros端发送的。

2.由于小车是空间移动（三维雷达，有上坡）所以必须要使用磁力计，使用磁力计的时候除了需要硬件校准之外，还需要软件的系统性校准。比如计算协方差矩阵。这里我使用脚本自动帮我获取10000组数据，在静态的时候校准协方差，并且融入ekf算法。
        不过由于FAST-LIO使用的是imu+lidar的配置，内部自带高度吻合的ekf算法，使用紧耦合，是需要把雷达的数据和imu的原始数据话题放在配置文件中，就行，自动输出odom到baselink的数据话题。下面是采集程序脚本。

#!/usr/bin/env python3
import rospy
from sensor_msgs.msg import Imu
from sensor_msgs.msg import MagneticField
import numpy as np

N = 10000 # 采集样本数
imu_samples = []
mag_samples = []

def imu_callback(msg):
    if len(imu_samples) < N:
        imu_samples.append([
            msg.linear_acceleration.x,
            msg.linear_acceleration.y,
            msg.linear_acceleration.z,
            msg.angular_velocity.x,
            msg.angular_velocity.y,
            msg.angular_velocity.z
        ])
        if len(imu_samples) == 1:
            print("[IMU采集] 开始采集IMU数据，总计样本数: {}".format(N))
        if len(imu_samples) % 50 == 0 or len(imu_samples) == N:
            print(f"[IMU采集] 已采集: {len(imu_samples)}/{N}")
    check_and_finish()

def mag_callback(msg):
    if len(mag_samples) < N:
        mag_samples.append([
            msg.magnetic_field.x,
            msg.magnetic_field.y,
            msg.magnetic_field.z
        ])
        if len(mag_samples) == 1:
            print("[磁力计采集] 开始采集磁力计数据，总计样本数: {}".format(N))
        if len(mag_samples) % 50 == 0 or len(mag_samples) == N:
            print(f"[磁力计采集] 已采集: {len(mag_samples)}/{N}")
    check_and_finish()

def check_and_finish():
    if len(imu_samples) == N and len(mag_samples) == N:
        arr_imu = np.array(imu_samples)
        arr_mag = np.array(mag_samples)
        acc_mean = arr_imu[:, 0:3].mean(axis=0)
        acc_var = arr_imu[:, 0:3].var(axis=0)
        gyro_mean = arr_imu[:, 3:6].mean(axis=0)
        gyro_var = arr_imu[:, 3:6].var(axis=0)
        mag_mean = arr_mag.mean(axis=0)
        mag_var = arr_mag.var(axis=0)

        print("\n================= 采集完成，结果如下 =================")
        print("【IMU】加速度计零偏: [{:.6f}, {:.6f}, {:.6f}]".format(*acc_mean))
        print("【IMU】加速度计方差: [{:.6e}, {:.6e}, {:.6e}]".format(*acc_var))
        print("【IMU】陀螺仪零偏:   [{:.6f}, {:.6f}, {:.6f}]".format(*gyro_mean))
        print("【IMU】陀螺仪方差:   [{:.6e}, {:.6e}, {:.6e}]".format(*gyro_var))
        print("【磁力计】零偏:      [{:.6f}, {:.6f}, {:.6f}]".format(*mag_mean))
        print("【磁力计】方差:      [{:.6e}, {:.6e}, {:.6e}]".format(*mag_var))

        print("\n================= EKF配置建议 =================")
        print("# 1. process_noise_covariance（过程噪声协方差矩阵，按状态向量顺序填写）")
        print("#   [x, y, z, roll, pitch, yaw, vx, vy, vz, wx, wy, wz, ax, ay, az]")
        print("process_noise_covariance: [建议位置相关可设0.05, 姿态相关可设0.05, 速度相关0.05, ")
        print("  {:.6e}, {:.6e}, {:.6e}, {:.6e}, {:.6e}, {:.6e}]".format(
            gyro_var[0], gyro_var[1], gyro_var[2], acc_var[0], acc_var[1], acc_var[2]))
        print("#   其中 wx,wy,wz 用陀螺仪方差，ax,ay,az 用加速度计方差")

        print("# 2. initial_estimate_covariance（初始估计协方差矩阵，建议可与过程噪声同量级或更大）")
        print("initial_estimate_covariance: [1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, "
              "{:.6e}, {:.6e}, {:.6e}, {:.6e}, {:.6e}, {:.6e}]".format(
            gyro_var[0], gyro_var[1], gyro_var[2], acc_var[0], acc_var[1], acc_var[2]))

        print("# 3. imu0_orientation_noise: 可根据IMU姿态噪声经验值或适当设置，如0.1")
        print("imu0_orientation_noise: 0.1")

        print("# 4. mag0_magnetic_field_noise: 用磁力计方差")
        print("mag0_magnetic_field_noise: {:.6e}".format(np.mean(mag_var)))
        print("#   或分别设置x/y/z方向噪声")
        print("#   mag0_magnetic_field_noise_x: {:.6e}".format(mag_var[0]))
        print("#   mag0_magnetic_field_noise_y: {:.6e}".format(mag_var[1]))
        print("#   mag0_magnetic_field_noise_z: {:.6e}".format(mag_var[2]))

        print("\n【操作建议】请在IMU和磁力计静止放置时采集数据，避免运动干扰零偏和噪声统计。采集完成后，将上述建议参数填入EKF配置文件对应位置。")
        print("====================================================\n")
        rospy.signal_shutdown("采集完成")

if __name__ == "__main__":
    imu_topic = "/imu/data"
    mag_topic = "/wit/mag"
    rospy.init_node("imu_mag_bias_variance")
    print(f"[IMU采集] 节点已启动，订阅IMU话题: {imu_topic}")
    print(f"[磁力计采集] 节点已启动，订阅磁力计话题: {mag_topic}")
    rospy.Subscriber(imu_topic, Imu, imu_callback)
    rospy.Subscriber(mag_topic, MagneticField, mag_callback)
    rospy.spin()

第五步编码器的配置

概述：

关于编码器，我这辆车没用，用的是伺服点击，我可以根据伺服点击给的数据来判断车的移动距离，但是在FAST-LIO中编码器的作用不大，主要是imu惯性导航和雷达的激光里程计。

第六步生成urdf

详细内容在我这篇文章里：关于从solid works中使用udrf导出机器人小车与gazebo地图模型在gazebo导中做三维导航的仿真_csdn solid works机器人-CSDN博客

这里做一下补充，生存urdf的注意事项

1.取保z轴朝上，x轴朝前

2.小车水平于上视图（俯视图）放置

3.原始坐标通过移动让坐标在下车下面

4.之前是原点为基准底面，现在是baselink为基准地面，导致在gazebo中轮子埋入地下弹飞小车。在启动gazebo时添加这句话就好。

args="-file $(find urdf包名字)/urdf/urdf包名字.urdf -urdf -model urdf包名字"    
改成下面那个。
args="-file $(find urdf包名字)/urdf/urdf包名字.urdf -urdf -model urdf包名字 -x 0.0 -y 0.0 -z 0.9"
提高小车在z轴的高度m

urdf在gezebo中乱动的问题

  生成的urdf模型由于物理参数不对，可能从silodworks中导出的，然后所有的模型都是使用铁质材料配置的，导致有些物体的整理不对，关于小车的总重量是可以修改的，这里修改一下总重即可，重力压在轮胎上到地面上，地面之上的重量还是以总重为主。

 1. 修改重力配置如下：

<link
    name="base_link">
    <inertial>
      <origin
        xyz="0.60525 0.052718 0.24445"
        rpy="0 0 0" />
      <mass
        value="592.83" />  #修改重量的位置，根据车辆的自身重量修改即可
      <inertia
        ixx="14.739"
        ixy="-0.013401"
        ixz="0.28545"
        iyy="59.583"
        iyz="0.00026819"
        izz="57.275" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://zzmxcby/meshes/base_link.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="0.2 0.6 0.9 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://zzmxcby/meshes/base_link.STL" />
      </geometry>
    </collision>
  </link>
  <link

2.关于轮子的阻尼摩擦力配置：

2.1轮子的阻尼

由于已进入gazebo就会乱动，所以需要给关节与关节之间施加摩擦力也就是阻尼，来克服轮子随意扭动的问题

<joint
    name="front_left_turn_joint"
    type="continuous">
    <dynamics damping="3.0"/> #阻尼配置
    <origin
      xyz="0.729573125178064 0.439499999999814 -0.0123921356236931"
      rpy="0 0 0" />
    <parent
      link="base_link" />
    <child
      link="front_left_turn_link" />
    <axis
      xyz="0 0 1" />
  </joint>

2.2轮子的摩擦力

摩擦力主要是轮子与地面的摩擦力，轮子与联轴器之间的摩擦力叫阻尼。

<link
    name="front_left_wheel_right_link">
    <gazebo reference="front_left_wheel_right_link">
      <mu1>10.0</mu1>#摩擦力
      <mu2>10.0</mu2>#摩擦力
    </gazebo>
    <inertial>
      <origin
        xyz="2.2423E-06 0.0004067 0.0013447"
        rpy="0 0 0" />
      <mass
        value="11.756" />
      <inertia
        ixx="0.24955"
        ixy="-6.7848E-07"
        ixz="-5.962E-07"
        iyy="0.39129"
        iyz="-5.2713E-06"
        izz="0.24954" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://zzmxcby/meshes/front_left_wheel_right_link.STL" />
      </geometry>
      <material
        name="">
        <color
          rgba="0.25 0.25 0.25 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://zzmxcby/meshes/front_left_wheel_right_link.STL" />
      </geometry>
    </collision>
  </link>

urdf插件与urdf中传感器插件调节

关于urdf的插件可以去网上寻找，主要是你需要的型号的雷达话题，或者是匹配FASTLIO的雷达话题，不单单是雷达，包括imu、摄像头之类的都需要去找。并且还需要根据urdf在gazebo中的应用进行配置。

关于urdf的插件我要被ai坑惨了，终于知道ai不是万能，如果我自己检测对比修改的话可能1天就解决了，但是我相信ai的话，整整修改了接近三个星期，我想骂人，草。就是因为urdf中的传感器插件，传感器插件放在了link中，怎么启动都不显示传感器插件的问题，所以我必须要在此总结关于传感器模型的问题。

1.原始urdf

一开始urdf都是在solidwork中生成的，通过修改不断完善到达最终版本

2.关于修改urdf

2.1由于urdf刚生成就会来回晃动，于是我就增加了轮子与地面的摩擦力，还增加了关节直接的摩擦力，效果显而易见（在上一步重力与阻尼中有讲解）

2.2添加了前后雷达的插件其实插件只要放进其中就完全能行，不要放在link中。

 <!-- 前激光雷达Gazebo插件（Velodyne VLP-16） -->
  <gazebo reference="front_lidar_link">
    <sensor type="ray" name="front_lidar_sensor">
      <pose>0 0 0 0 0 0</pose>
      <visualize>true</visualize>
      <update_rate>10</update_rate>
      <ray>
        <scan>
          <horizontal>
            <samples>360</samples>
            <resolution>1</resolution>
            <min_angle>-3.14159265358</min_angle>
            <max_angle>3.14159265358</max_angle>
          </horizontal>
          <vertical>
            <samples>16</samples>
            <resolution>1</resolution>
            <min_angle>-0.2618</min_angle>
            <max_angle>0.2618</max_angle>
          </vertical>
        </scan>
        <range>
          <min>0.1</min>
          <max>30.0</max>
          <resolution>0.01</resolution>
        </range>
      </ray>
      <plugin name="front_lidar" filename="libgazebo_ros_velodyne_laser.so">
        <topicName>/front_lidar/scan</topicName>
        <frameName>front_lidar_link</frameName>
        <min_range>0.1</min_range>
        <max_range>30.0</max_range>
        <gaussianNoise>0.0</gaussianNoise>
      </plugin>
    </sensor>
  </gazebo>
<!-- 后激光雷达Gazebo插件（Velodyne VLP-16） -->
  <gazebo reference="back_lidar_link">
    <sensor type="ray" name="back_lidar_sensor">
      <pose>0 0 0 0 0 0</pose>
      <visualize>true</visualize>
      <update_rate>10</update_rate>
      <ray>
        <scan>
          <horizontal>
            <samples>360</samples>
            <resolution>1</resolution>
            <min_angle>-2.70526034</min_angle>
            <max_angle>2.70526034</max_angle>
          </horizontal>
          <vertical>
            <samples>16</samples>
            <resolution>1</resolution>
            <min_angle>-0.2618</min_angle>
            <max_angle>0.2618</max_angle>
          </vertical>
        </scan>
        <range>
          <min>0.1</min>
          <max>30.0</max>
          <resolution>0.01</resolution>
        </range>
      </ray>
      <plugin name="back_lidar" filename="libgazebo_ros_velodyne_laser.so">
        <topicName>/back_lidar/scan</topicName>
        <frameName>back_lidar_link</frameName>
        <min_range>0.1</min_range>
        <max_range>30.0</max_range>
        <gaussianNoise>0.0</gaussianNoise>
      </plugin>
    </sensor>
  </gazebo>

2.3添加了十轴imu的插件其实插件只要放进其中就完全能行，不要放在link中。

  <!-- IMU（十轴：9轴+磁力计）Gazebo插件 -->
  <gazebo reference="imu_link">
    <sensor type="imu" name="imu_sensor_10axis">
      <always_on>true</always_on>
      <update_rate>200</update_rate>
      <imu>
        <noise>
          <type>gaussian</type>
          <rate>
            <mean>0.0</mean>
            <stddev>0.00017</stddev>
            <bias_mean>0.0</bias_mean>
            <bias_stddev>0.0000087</bias_stddev>
          </rate>
          <accel>
            <mean>0.0</mean>
            <stddev>0.002</stddev>
            <bias_mean>0.0</bias_mean>
            <bias_stddev>0.0001</bias_stddev>
          </accel>
        </noise>
      </imu>
      <plugin name="imu_plugin" filename="libgazebo_ros_imu_sensor.so">
        <topicName>/imu/data_raw</topicName>
        <bodyName>imu_link</bodyName>
        <updateRateHZ>200.0</updateRateHZ>
        <gaussianNoise>0.00017</gaussianNoise>
        <xyzOffset>0 0 0</xyzOffset>
        <rpyOffset>0 0 0</rpyOffset>
        <frameName>imu_link</frameName>
        <initialOrientationAsReference>false</initialOrientationAsReference>
      </plugin>
    </sensor>
  </gazebo>
  <!-- 磁力计Gazebo插件 -->
  <gazebo reference="imu_link">
    <sensor type="magnetometer" name="mag_sensor">
      <always_on>true</always_on>
      <update_rate>100</update_rate>
      <plugin name="mag_plugin" filename="libgazebo_ros_magnetometer.so">
        <topicName>/imu/mag</topicName>
        <bodyName>imu_link</bodyName>
        <updateRateHZ>100.0</updateRateHZ>
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.0005</stddev>
        </noise>
        <frameName>imu_link</frameName>
        <referenceHeading>0.0</referenceHeading>
        <declination>0.0</declination>
      </plugin>
    </sensor>
  </gazebo>

使用urdf在gazebo中成功生成小车

1. 仿真环境启动阶段

启动Gazebo服务器和客户端
创建基础物理仿真世界

  <!-- 1. 启动Gazebo（空世界，可替换为自定义世界） -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <!-- 使用Gazebo自带的空世界启动文件 empty_world.launch -->
    <!-- $(find gazebo_ros) 会找到 gazebo_ros 包的路径 -->
    <arg name="world_name" default="worlds/empty.world"/>
    <!-- 指定要加载的世界文件，默认是空世界，可以替换为自定义的 .world 文件 -->
    <arg name="paused" default="false"/>
    <!-- Gazebo 是否在启动时暂停仿真，false表示启动后立即运行 -->
    <arg name="use_sim_time" default="true"/>
    <!-- 使用仿真时间 /clock，ROS 节点订阅 /clock 话题同步仿真时间 -->
    <arg name="gui" default="true"/>
    <!-- 是否显示Gazebo GUI界面，true表示可看到仿真窗口 -->
  </include>

2. urdf预配置阶段

将urdf文件加载到参数服务器中。

<!-- 2. 将once的URDF文件加载到参数服务器 -->
  <param name="robot_description" command="$(find xacro)/xacro '$(find zzmxcby)/urdf/zzmxcby.urdf'"/>
  <!-- 
    作用：将机器人URDF模型加载到ROS参数服务器，参数名为 robot_description
    command 部分调用 xacro 解析 URDF 文件，即使是普通URDF也能保证兼容xacro功能
    $(find zzmxcby) 会找到 zzmxcby 包的路径
    后续 spawn_model 节点或 robot_state_publisher 会读取 robot_description 参数
  -->

3. 机器人模型加载阶段

将URDF模型加载到参数服务器
在Gazebo中实例化机器人模型
设置初始位置防止碰撞

<!-- 3. 在Gazebo中生成机器人模型 -->
  <node name="spawn_robot" pkg="gazebo_ros" type="spawn_model" output="screen"
        args="-urdf -param robot_description -model zzmxcby -x 0 -y 0 -z 0.5"/>
  <!-- 
    节点说明：
    - name="spawn_robot" 节点名字
    - pkg="gazebo_ros" 所在ROS包
    - type="spawn_model" 使用 gazebo_ros 提供的 spawn_model 可将URDF加载到Gazebo
    - output="screen" 表示日志输出到终端

    args 参数解析：
    - -urdf 指明使用 URDF 格式
    - -param robot_description 使用参数服务器中的 robot_description
    - -model zzmxcby 指定模型名称
    - -x 0 -y 0 -z 0.5 设置机器人初始在世界坐标的位置（0,0,0.5）
  -->

4. 机器人模型加载阶段

将URDF模型加载到参数服务器
在Gazebo中实例化机器人模型
设置初始位置防止碰撞

  <!-- 4. 启动关节状态发布和机器人状态发布节点 -->
  <node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" output="screen"/>
  <!-- 
    joint_state_publisher_gui：
    - 用于发布机器人关节状态话题 /joint_states
    - 可以手动通过GUI调整关节角度，方便调试
    - 对于没有物理驱动的仿真或者只做可视化时非常有用
  -->

  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen">
    <param name="use_sim_time" value="true"/>
  </node>
  <!-- 
    robot_state_publisher：
    - 根据机器人URDF模型和 /joint_states 数据计算 TF 变换树
    - 将 base_link 到各个 link 的坐标关系广播到 /tf 话题
    - use_sim_time=true 表示同步Gazebo仿真时间
  -->

5. 状态发布阶段与可选控制器加载阶段

启动机器人状态发布器
建立TF坐标变换体系
根据参数决定是否加载运动控制器
延迟确保Gazebo稳定运行

  <!-- 5. 加载控制器配置文件（需手动创建zzmxcby_control.yaml，参考suxian的格式） -->
  <rosparam file="$(find zzmxcby)/config/zzmxcby_control.yaml" command="load"/>
  <!-- 
    rosparam：
    - 加载 YAML 文件内容到参数服务器
    - 用于 ros_control 控制器参数配置
    - zzmxcby_control.yaml 包含控制器类型、PID参数、硬件接口绑定等
    - command="load" 表示将文件内容加载到参数服务器
  -->

6.影响因素

声明：影响到正常生成urdf小车在gazebo应用的因素有很多，发现会列上的。

1.关节聚集

在启动时经常发现关节聚集问题，这个车忽然轮子就聚集在baselink点上了，是因为初始化错误导致，主要原因是因为使用了ros的控制器，ros的控制器需要配置初始化，如果使用自制控制器或者想解决掉这个问题直接去urdf中找到这一句话。

在urdf中注释掉或者删掉这句话，这句话主要左右是用ros自带的控制器就是他会导致关节聚集，删掉使用键盘控制毫无影响，关于导航还有待研究。（可以私信提醒我）

<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so"/>

7.构建urdf世界

关于世界的构建我在这篇中已经讲述了的很明确了。urdf勾线小车地图https://blog.csdn.net/sjdksshdbdb/article/details/154201190?utm_source%20=%20uc_fansmsg

7.1地图大小问题

有的地图大，有的地图小，有的gazebo能放的住，有的放不住这都是可以调节的。修改地图地面大小就在world中设置。（后续可以用配置文件来配置地图大小）

<!-- 碰撞尺寸 -->
<collision name='collision'>
  <geometry>
    <plane>
      <normal>0 0 1</normal>
      <size>10000 10000</size>  <!-- 改为10000x10000 -->
    </plane>
  </geometry>
  ...
</collision>

<!-- 视觉尺寸 -->
<visual name='visual'>
  <geometry>
    <plane>
      <normal>0 0 1</normal>
      <size>10000 10000</size>  <!-- 改为10000x10000 -->
    </plane>
  </geometry>
  ...
</visual>

7.2地图起飞问题

地图质量，地图质量不要太大，不然地图容易起飞

<model name='suidao'>
      <link name='base_link'>
        <inertial>
          <pose>-304.281 57.4764 348.095 0 -0 0</pose>
          <mass>3.7708e+09</mass>  《这个地方质量太大了，改小一点》
          <inertia>
            <ixx>3.67611e+13</ixx>
            <ixy>4.23344e+12</ixy>
            <ixz>1.65966e+13</ixz>
            <iyy>6.23934e+13</iyy>
            <iyz>-5.83951e+12</iyz>
            <izz>4.00841e+13</izz>
          </inertia>
        </inertial>
        <collision name='base_link_collision

7.3启动地图

如何启动自己的地图呢？

  <!-- 1. 启动Gazebo（空世界，可替换为自定义世界） -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
{
    <arg name="world_name" default="worlds/empty.world"/>
改为
    <arg name="world_name" value="$(find 自己的功能包)/功能包中的文件夹worlds/自己的地图名字.world"/>
}
//其他
    <arg name="paused" default="false"/>
    <arg name="use_sim_time" default="true"/>
    <arg name="gui" default="true"/>
  </include>

7.4地图加载失败

我在仿真的时候发现，地图总是不显示，也不加载，就感觉好像没启动一样，苦恼了很久，焯！就是world出现了问题，因为我的地图是从SolidWorks中导出来的urdf，使用urdf中自带的gazebo.luanch打开地图将地图另存格式为world。但是上一次地图出现的问题我不知道是什么情况发生的，问ai也没用，焯！！。于是我重新构建了一遍。关于构建urdf地图的流程在“关于从solid works中使用udrf导出机器人小车与gazebo地图模型在gazebo导中做三维导航的仿真https://blog.csdn.net/sjdksshdbdb/article/details/154201190?spm=1001.2014.3001.5501中，还望各位小白指出问题。

更新urdf中的错误

[ERROR] [1767522337.408421118, 0.001000000]: No p gain specified for pid.  Namespace: /gazebo_ros_control/pid_gains/front_left_wheel_right_joint
[WARN] [1767522337.408432803, 0.001000000]: Deprecated syntax, please prepend 'hardware_interface/' to 'VelocityJointInterface' within the <hardwareInterface> tag in joint 'front_right_wheel_left_joint'.
[ERROR] [1767522337.408686588, 0.001000000]: No p gain specified for pid.  Namespace: 

一般报错这种问题，基本上是没有pid，但是我有pid啊，那就是没有正确加载，一下是解决办法

<hardwareInterface>EffortJointInterface</hardwareInterface>
改成
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
 

关于kdl的问题

在运行的时候会包一个警告，尤其是刚建立小车gazebo的时候如下所示

[WARN] [1767574028.274794027]: The root link base_link has an inertia specified in the URDF, but KDL does not support a root link with an inertia.  As a workaround, you can add an extra dummy link to your URDF.

这个警告主要是说你的kdl加载初始化失败，没有kdl会有什么问题呢？比如说TF树，导航，rviz，等依赖tf树的组件无法工作。kdl的伦理形态是惯性为0的理想值，但是符合惯性才是现实物理学，所有想到的办法就是添加一个虚拟的连杆，就是在baselink的父级在加一个虚拟的，但是我发现没有什么用。不知道各位怎么看待，没加也照样用，后续再更新吧，麻烦知道的小伙伴帮帮博主，后续博主回去深入研究的。

第七步控制小车的键盘系统

关于键盘的想法与碎碎念

        关于小车键盘控制有很多问题，比如是否使用ros官方自带的控制，还是自己编写控制脚本，控制脚本是用c++还是python，我不知那种好。另外我在想编写一个万能键盘控制，只需要输入关节的名称，或者旋转关节，还是转动轮子，只需要填入对应名称就可以切换控制。无论小车的型号，不管是三个轮子的还是四个轮子的任务，任何的控制方式都可以使用，只需要填入对应的关节之类的，万能要控制，不知道能不能实现。目前我不能实现，但是我会整理每一步，在下次编写的时候我希望我能用的上。说句实在话，键盘控制只是小车的一个辅助问题，并不需要太多的投入。

1.项目概述

本次主要使用了一个八轮万向的小车，有四个可以360度自由旋转方向的旋转轴，每一个旋转轴有两个机械差速的轮子，总共八个轮子，注主要使用AGV作为正常使用方式。主要使用python脚本以及udp通信完成控制。

2.模块化设计原则

单一职责原则：每个模块负责特定功能
低耦合高内聚：模块间通过明确接口通信
可扩展性：便于添加新功能和模式

3. 核心模块组成
主控制模块（main.py）：系统入口，整合各模块

配置模块（config.py）：集中管理控制参数
运动控制模块（motion_controller.py）：核心运动算法实现
模式管理模块（mode_manager.py）：控制模式切换与状态管理
键盘输入模块（keyboard_reader.py）：非阻塞键盘输入处理
ROS通信模块（ros_publisher.py）：ROS消息发布
硬件通信模块（udp_communicator.py）：与STM32的UDP通信
自动导航模块（auto_navigator.py）：自动导航接口

4. 多模式控制

正常模式：基础前进、后退、转向控制
原地旋转模式：全向轮原地旋转实现
阿克曼转向模式：基于阿克曼几何的转向控制
自动导航模式：预设路径的自动导航

5. 运动控制算法

速度平滑过渡（加速度/减速度控制）
转向角度归一化与限制
阿克曼几何角度计算
转向自动回正机制
速度衰减与紧急停止

6. 硬件通信

UDP协议设计与实现
控制命令与反馈数据格式
校验机制（CRC16)
非阻塞通信与多线程处理

1.配置小车的关节

无论是重新换一个小车的urdf也好还是更换关节组件也好，如果更换了urdf以及相关关节的名字，都需要重新配置小车。

关于小车需要哪些配置这里说一下：有的小车可能靠差速驱动，那就单独使用差速就行，有的就是阿克曼那就直接使用阿克曼就行，像我使用的是可以使用360度旋转方向的转向轴，所以有很多种控制方式，不单单是阿克曼转向，还有比如说平移以及原地旋转模式都可以。但是一般前进如果是前后对称的轮子除了麦克纳姆轮之外都用阿克曼转向或者AGV转向都可以。

关于我的小车主要也是轮子多控制复杂，但是目前我有自己的一套完美的控制方案。

我编写上面说的5个绿色模块，用来使用键盘控制小车，通过配置-->运动控制-->模式管理控制-->键盘输入-->最后到ROS接收指令。

我这里是单关节驱动，顾名思义，就是单独控制一个关节，用函数串起来，一起控制，虽然复杂，但是确实好用，比较键盘控制只是一个小测试，不要浪费太多的时间。

1.1驱动与关节使用

分别在驱动话题中发布urdf中对应的关节名，分为转向发布话题和驱动发布话题

        # 转向发布器话题 - 按URDF实际关节名
        self.steering_topics = [
            '/front_left_turn_joint_controller/command',
            '/front_right_turn_joint_controller/command',
            '/back_left_turn_joint_controller/command',
            '/back_right_turn_joint_controller/command'
        ]

        # 驱动发布器话题 - 按URDF实际关节名
        self.drive_topics = [
            '/front_left_wheel_left_joint_controller/command',
            '/front_left_wheel_right_joint_controller/command',
            '/front_right_wheel_left_joint_controller/command',
            '/front_right_wheel_right_joint_controller/command',
            '/back_left_wheel_left_joint_controller/command',
            '/back_left_wheel_right_joint_controller/command',
            '/back_right_wheel_left_joint_controller/command',
            '/back_right_wheel_right_joint_controller/command'
        ]

1.2配置pid

关于小车的驱动肯定是要用转动驱动控制，由于是单关节所以每一个都有，这些关机主要还是与urdf中相关联。它们是同一个控制链的不同环节，必须保持“一致的名字 + 硬件接口匹配 + 加载顺序”才能工作。并且还要加载Gazebo ROS Control 插件以及<!-- 驱动关节 Velocity 控制 -->

所以再整理一下：

config：的分为转向发布话题和驱动发布话题
launch：controller_spawner
urdf：驱动关节 Velocity 控制
ymal：control 配置pid
urdf：Gazebo ROS Control 插件

先修改这些吧，如果后面再修改的话就再继续补充。

 第八步使用FAST-LIO构建仿真中的点云地图
第九步解决lidar退化问题

第十步EKF的介绍和使用

EKF（扩展卡尔曼滤波）简介

扩展卡尔曼滤波（EKF）是一种非线性状态估计方法，通过线性化非线性模型来处理状态预测和观测更新。EKF的核心步骤包括：

• 预测步骤：基于系统模型预测状态和协方差矩阵。
• 更新步骤：利用传感器观测值修正预测结果，通过卡尔曼增益权衡预测与观测的置信度。

EKF适用于机器人定位（如SLAM）、传感器融合等场景，尤其在ROS中常用于多传感器数据融合（如IMU、里程计、GPS）。

第十一步AMCL定位