已解决(一分钟)gazebo生成urdf小车模型后出现原地打转&x、y轴方向偏移问题的解决参考方案

<1>问题描述:

b站赵虚左老师的ros教程中,gazebo生成小车模型后,出现原地缓慢打转和x、y轴方向缓慢移动问题。

细节如下图所示

初始生成的小车

 过一会就能明显发现小车原地逆时针旋转,查看里程计位姿发生偏移

<2>分析:查看/odom位姿信息,发现出现不该出现的线速度和角速度,因为/cmd_vel没有发布方

网上搜了好久没有适合的解决方案,最后从小车xacro文件尝试修改参数最终成功解决。

<3>亲测解决的参考方案:修改小车驱动和万向轮质量mass参数

具体做法如下:

首先打开   demo05_car_base.urdf.xacro   

然后底盘、驱动轮、万向轮mass在教程中分别对应为:2、0.05、0.01修改为3.5、0.1、0.05

效果如下图模型生成800s过后小车基本还在原地,里程计/odom话题下的位姿信息坐标值、四元数以及 x、y方向线速度与绕z轴的偏航角角速度几乎为0,问题解决!

最后上述参数可以自己微微调整,思路就是微调大模型底盘和轮子质量,具体原理与其他解决方案欢迎同学们评论区讨论,如果方案有效请给个点赞或关注谢谢O(∩_∩)O

最后附demo05_car_base.urdf.xacro   代码

<robot name="mycar" xmlns:xacro="http://wiki.ros.org/xacro">
    
    <!-- 添加 base_footprint -->
    <!-- 属性 -->
    <xacro:property name="footprint_radius" value="0.001"/>
    <link name="base_footprint">
        <visual>
            <geometry>
                <sphere radius="${footprint_radius}"/>
            </geometry>
        </visual>
    </link>

    <!-- 2.添加底盘 -->
    <!--  
        形状: 圆柱
        半径:0.1m
        高度: 0.08m
        离地: 0.015m
    -->
    <!-- 属性封装 -->
    <xacro:property name="base_radius" value="0.1" />
    <xacro:property name="base_length" value="0.08" />
    <xacro:property name="base_mass" value="3.5" />
    <xacro:property name="lidi" value="0.015" />
    <xacro:property name="base_joint_z" value="${base_length / 2 + lidi}" />
    <!-- 2-1.link -->
    <link name="base_link">
        <visual>
            <geometry>
                <cylinder radius="${base_radius}" length="${base_length}" />
            </geometry>

            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>

            <material name="baselink_color">
                <color rgba="0.1 0.5 0.2 0.7"/>
            </material>            
        </visual>

        <collision>
            <geometry>
                <cylinder radius="${base_radius}" length="${base_length}" />
            </geometry>

            <contact>
                <ode>
                    <min_depth>0.001</min_depth>
                </ode>
            </contact>

            <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
        </collision>
        
        <!-- 调用惯性矩阵函数 -->
        <xacro:cylinder_inertial_matrix m="${base_mass}" r="${base_radius}" h="${base_length}" />
    </link>

        <!-- gezebo 有自己的颜色设置标签 -->
        <gazebo reference="base_link">
            <material>Gazebo/Yellow</material>
        </gazebo>

    <!-- 2-1.joint -->
    <joint name="link2footprint" type="fixed">
        <parent link="base_footprint"/>
        <child link="base_link"/>
        <!-- 关节z上的设置 = 车体高度/2 + 离地间距 -->
        <origin xyz="0 0 ${base_joint_z} " rpy="0 0 0"/>
        <axis xyz="0 0 0"/>
    </joint>


   <!-- 3.添加驱动轮 -->
    <!--  
        形状: 圆柱
        半径: 0.0325m
        厚度: 0.015m
    -->
    <!-- 属性封装 -->
    <xacro:property name="wheel_radius" value="0.0325" />
    <xacro:property name="wheel_length" value="0.015" />
    <xacro:property name="wheel_mass" value="0.1" />
    <xacro:property name="PI" value="3.1415927" />
    <!-- 注意: 结果是负数 -->
    <xacro:property name="wheel_joint_z" value="${ (base_length / 2 + lidi -wheel_radius) * -1 }" />
    
    <!-- 函数封装(方便调用) 底盘就一个不需要调用 而驱动论有两个要调用两次 -->
    <!--  
        wheel_name: left 或 right
        flag: 1 或 -1
    -->

    <xacro:macro name="wheel_func" params="wheel_name flag" >

        <!-- 3-1.link -->
        <link name="${wheel_name}_wheel">
            <visual>
                <geometry>
                    <cylinder radius="${wheel_radius}" length="${wheel_length}"/>
                </geometry>

                <origin xyz="0.0 0.0 0.0" rpy="${PI / 2} 0.0 0.0"/>

                <material name="wheel_color">
                    <color rgba="0 0 0 0.7"/>
                </material>            
            </visual>
            <collision>
                <geometry>
                    <cylinder radius="${wheel_radius}" length="${wheel_length}"/>
                </geometry>

                <origin xyz="0.0 0.0 0.0" rpy="${PI / 2} 0.0 0.0"/>
            </collision>
            <xacro:cylinder_inertial_matrix m="${wheel_mass}" r="${wheel_radius}" h="${wheel_length}" />
        </link>
        <gazebo reference="${wheel_name}_wheel">
            <material>Gazebo/Red</material>
        </gazebo>
            
        <!-- 3-2.joint -->
        <joint name="${wheel_name}2link" type="continuous">
            <parent link="base_link"/>
            <child link="${wheel_name}_wheel"/>
            <!-- 
                x 无偏移
                y 车体半径
                z z = 车体高度/2 + 离地间距 - 车轮半径 记得加负号是负值
            -->
            <origin xyz="0 ${0.1 * flag} ${wheel_joint_z} " rpy="0.0 0.0 0.0"/>
            <axis xyz="0 1 0"/>
        </joint>

    </xacro:macro >
    <!-- 调用宏 -->
    <xacro:wheel_func wheel_name="left" flag="1" />
    <xacro:wheel_func wheel_name="right" flag="-1" />

    <!--  4.添加万向轮 -->
    <!--  
        形状: 球
        半径:0.0075m
    -->
    <!-- 属性封装 -->
    <xacro:property name="small_wheel" value="0.0075" />
    <xacro:property name="small_wheel_mass" value="0.05" />
    <xacro:property name="small_joint_z" value="${ (base_length / 2 + lidi - small_wheel) * -1 }" />

    <xacro:macro name="small_wheel_func" params="small_wheel_name flag" >
        <!-- 4-1.link -->
        <link name="${small_wheel_name}_wheel">
            <visual>
                <geometry>
                    <sphere radius="${small_wheel}" />
                </geometry>

                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>

                <material name="wheel_color">
                    <color rgba="0 0 0 0.7"/>
                </material>            
            </visual>
            <collision>
                <geometry>
                    <sphere radius="${small_wheel}" />
                </geometry>

                <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
            </collision>
            <xacro:sphere_inertial_matrix m="${small_wheel_mass}" r="${small_wheel}" />
        </link>
        <gazebo reference="${small_wheel_name}_wheel">
            <material>Gazebo/Red</material>
        </gazebo>
        
        <!-- 4-2.joint -->
        <joint name="${small_wheel_name}2link" type="continuous">
            <parent link="base_link"/>
            <child link="${small_wheel_name}_wheel"/>
            <!-- 
                x 相对底盘中心0.08
                y 无偏移
                z z = 车体高度/2 + 离地间距 - 万向车轮半径 记得加负号是负值
            -->
            <origin xyz="${0.08 * flag} 0 ${small_joint_z}" rpy="0.0 0.0 0.0"/>
            <axis xyz="0 1 0"/>
        </joint>

    </xacro:macro >
    <!-- 调用宏 -->
    <xacro:small_wheel_func small_wheel_name="front" flag="1" />
    <xacro:small_wheel_func small_wheel_name="back" flag="-1" />
    
        
</robot>

  • 6
    点赞
  • 24
    收藏
    觉得还不错? 一键收藏
  • 8
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 8
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值