小车tf如何配置

方小生–

于 2024-08-19 09:08:54 发布

阅读量497

点赞数 17

分类专栏： ROS 文章标签： urdf tf ros

本文链接：https://blog.csdn.net/qq2650326396/article/details/141310622

版权

ROS 专栏收录该内容

14 篇文章 7 订阅

订阅专栏

小车tf如何配置

原因：小车导航跑不好小车位置漂移很严重，修改导航参数无法解决，最后检查发现是tf变化的问题。

tf配置

小车tf变化就好像告诉小车的手和脚在那里。

首先确定确定小车主坐标系：即base_footprint(0,0,0,0,0,0)—原点坐标—使用右手坐标系。

手就是小车x正方向即小车的正反向，然后基于这个坐标系确定自己的传感器的位置：

在这里插入图片描述

这个是我的urdf的设置：主要修改了imu和激光雷达和base_link的位置，其他轮子的无关紧要的没有用到。

urdf代码

<?xml version="1.0" ?>
<robot name="diff_car">
    <link name="base_footprint"/>
    <link name="camera_link"/>
    <link name="imu_link"/>
    <link name="laser_link"/>
    <joint name="base_joint" type="fixed">   
        <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/> 
        <parent link="base_footprint"/>  
        <child link="base_link"/> 
    </joint>
    <joint name="base_camera" type="fixed">   
        <origin xyz="0.1 0.0 0.37" rpy="0.0 0.0 0.0"/> 
        <parent link="base_link"/>  
        <child link="camera_link"/> 
    </joint>

    <joint name="base_laser" type="fixed">   
        <origin xyz="0.15 0.0 0.1" rpy="0.0 0.0 3.1415926535"/> 
        <parent link="base_link"/>  
        <child link="laser_link"/> 
    </joint>

    <joint name="base_imu" type="fixed">
        <parent link="base_link"/>
        <child link="imu_link"/>
        <origin xyz="-0.1 0.1 0.1" rpy="0 0 0"/>
    </joint>
    <link name="base_link">
    <!--base_car-->
        <visual> 
            <origin xyz="0.0 0.0 0.1" rpy="0.0 0.0 0.0"/> 
            <geometry>  
                <cylinder radius="0.20" length="0.16"/>             
            </geometry>
            <material name="yellow"> 
                <color rgba="1 0.4 0 1"/>  
            </material>
        </visual>
    </link>

<!--left_wheel-->
    <joint name="left_wheel_joint" type="continuous">   
        <origin xyz="0.0 0.19 -0.05" rpy="0.0 0.0 0.0"/> 
        <parent link="base_link"/>  
        <child link="left_wheel_link"/> 
        <axis xyz="0.0 1.0 0.0"/>  
    </joint>

    <link name="left_wheel_link">
        <visual>
            <origin xyz="0.0 0.0 0.0" rpy="1.5707 0.0 0.0"/>
            <geometry>
                <cylinder radius="0.06" length="0.025"/>
            </geometry>
            <material name="white">
                <color rgba="1 1 1 0.9"/>
            </material>
        </visual>
    </link>
 
<!--right_wheel-->
    <joint name="right_wheel_joint" type="continuous">
        <origin xyz="0.0 -0.19 -0.05"/>
        <parent link="base_link"/>
        <child link="right_wheel_link"/>
        <axis xyz="0.0 1.0 0.0"/>
    </joint>

    <link name="right_wheel_link">
        <visual>
            <origin xyz="0.0 0.0 0.0" rpy="1.5707 0.0 0.0"/>
            <geometry>
                <cylinder radius="0.06" length="0.025"/>
            </geometry>
            <material name="white">
                <color rgba="1 1.0 1.0 0.9"/>
            </material>
        </visual>
    </link>

<!--front_caster-->
    <joint name="front_caster_joint" type="continuous">
        <origin xyz="0.18 0.0 -0.095" rpy="0.0 0.0 0.0"/>
        <parent link="base_link"/>
        <child link="front_caster_link"/>
        <axis xyz="0.0 1.0 0.0"/>
    </joint>

    <link name="front_caster_link">
        <visual>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
                <geometry>
                    <sphere radius="0.015"/>
                </geometry>
                <material name="black">
                    <color rgba="0.0 0.0 0.0 0.95"/>
                </material>
        </visual>
    </link>

<!--back_caster-->
    <joint name="back_caster_joint" type="continuous">
        <origin xyz="-0.18 0.0 -0.095" rpy="0.0 0.0 0.0"/>
        <parent link="base_link"/>
        <child link="back_caster_link"/>
        <axis xyz="0.0 1.0 0.0"/>
    </joint>

    <link name="back_caster_link">
        <visual>
            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
                <geometry>
                    <sphere radius="0.015"/>
                </geometry>
            <material name="black">
                <color rgba="0.0 0.0 0.0 0.95"/>
            </material>
        </visual>
    </link>
</robot>

检查urdf语法问题：

check_urdf my.urdf

生成可视化pdf

urdf_to_graphiz my.urdf

在这里插入图片描述

这里后面发现一个问题就是导航时候发现激光雷达位置不对发现是激光雷达的位置配置不正确，我使用的是思岚的A2：

这个要看你激光雷达安装的位置和方向，然后再tf配置上将激光雷达的位置设置成和上面的tf坐标的反向一样还要位置关系即xyz和rpy（位姿）。

在这里插入图片描述

由右手的坐标系可以知道，要绕Z轴顺时针旋转180度激光雷达的位置才正确。得到激光雷达的位姿为（0.15，0.0，0.1）（0.0，0.0，3.1415926535）

launch配置

<launch>
	<!-- 设置机器人模型路径参数 -->
	<param name="robot_description" textfile="$(find hg_bringup)/urdf/four_drive.urdf" />
	<!-- 运行robot_state_publisher节点，发布tf  -->
	<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
</launch>

启动程序检查tf

启动上面的launch不用启动底盘

然后启动rviz添加tf

查看tf的方向和位置是不是和实车的一致

红色是X+;绿色是Y+;蓝色是Z+;

在这里插入图片描述

ok,下一步

启动并导航

发现许多问题出现底盘的问题，基于这个教程底盘里程计发布的tf是：base_footprit

然后就可以正常导航

关于底盘定位使用了imu_filter_madgwick和robot_localization：

底盘定位

imu_filter_madgwick：

用于过滤和融合来自 IMU 设备的原始数据。它将来自通用 IMU 设备的角速度、加速度和（可选）磁读数融合到定向四元数中，并在 IMU/data 主题上发布融合数据。

ros地址

算法原理解析

配置教程参考

robot_localization：

**基于卡尔曼滤波在ROS系统上比较成熟、应用比较广泛的一个机器人动态定位软件包。**robot_localization ，但是它具备几个不可替代的优势：

它有专门的逻辑融合GPS定位信息，可以支持户外定位
它能够融合多种传感器数据，支持3D空间定位
与ROS系统的集成由来已久，深得人心，普及率挺好。
由于这些原因，robot_localization软件包迄今为止从代码到文档都得到了很好的维护，对ROS乃至ROS2的各个发布版本都有专门的支持。

ros地址

算法原理解析

配置教程参考

bringup.launch

<launch>
    <arg name="use_gui" default="false"/>
    <arg name="use_rviz" default="false"/>

    <!-- 发布静态坐标系转换 -->
    <!-- Post static coordinate system conversion -->
    <include file="$(find hg_bringup)/launch/urdf.launch"/>
      <!-- <param name="car_type" value="$(env CAR_TYPE)"/> -->
    <!-- 底层驱动节点 -->
    <!-- Low-level driver node -->
    <node pkg="hg_bringup" type="car_control.py" name="driver_node" required="true" output="screen">
        <param name="car_type" value="$(env CAR_TYPE)"/>  
        <param name="imu_link" type="string" value="imu_link"/>
        <remap from="/pub_vel" to="/vel_raw"/>
        <remap from="/pub_imu" to="/imu/imu_raw"/>
        <remap from="/pub_mag" to="/mag/mag_raw"/>
    </node>

    <!-- 发布里程计数据 -->
    <!-- Publish odometer data -->
    <node pkg="hg_bringup" type="base_node" name="odometry_publisher">
        <param name="odom_frame" type="string" value="odom"/>
        <param name="base_link" type="string" value="base_footprint"/>
        <param name="linear_scale_x" type="double" value="1"/>
        <param name="linear_scale_y" type="double" value="1"/>
        <remap from="/sub_vel" to="/vel_raw"/>
        <remap from="/pub_odom" to="/odom_raw"/>
    </node>

    <!-- 过滤和融合imu数据 -->
    <!-- Filter and fuse imu data -->
    <node pkg="imu_filter_madgwick" type="imu_filter_node" name="imu_filter_madgwick" output="screen">
        <param name="fixed_frame" value="base_link"/>
        <param name="use_mag" value="false"/>
        <param name="publish_tf" value="false"/>
        <param name="use_magnetic_field_msg" value="false"/>
        <param name="world_frame" value="enu"/>
        <param name="orientation_stddev" value="0.05"/>
        <param name="angular_scale" value="0.3"/>
        <remap from="/sub_imu" to="/imu/imu_raw"/>
        <remap from="/sub_mag" to="/mag/mag_raw"/>
        <remap from="/pub_imu" to="/imu/imu_data"/>
        <remap from="/pub_mag" to="/mag/mag_field"/>
    </node>

    <!-- 扩展卡尔曼数据融合 -->
    <!-- Extended Kalman data fusion -->
    <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization" output="screen">
        <remap from="odometry/filtered" to="odom"/>
        <remap from="/imu0" to="/imu/imu_data"/>
        <remap from="/odom0" to="odom_raw"/>
        <param name="odom_frame" value="/odom"/>
        <param name="world_frame" value="/odom"/>
        <param name="base_link_frame" value="/base_footprint"/>
        <rosparam command="load" file="$(find hg_bringup)/param/robot_localization.yaml"/>
    </node>

    <node pkg="rviz" type="rviz" name="odom_rviz" required="true"
          args="-d $(find hg_bringup)/rviz/odom.rviz" if="$(arg use_rviz)"/>
</launch>

ok，结束

续写：

传感器的坐标系

IMU的坐标系

测试得到IMU的坐标系应该是这样的：下位机的中心到电源输入的方向是X轴正方向，Z正方向正放，Y正方向是电源开关的方向。

激光雷达的坐标系

如图所示，以接线端子位置作为判断依据

在这里插入图片描述