Viobot定位用于导航

这篇具有很好参考价值的文章主要介绍了Viobot定位用于导航。希望对大家有所帮助。如果存在错误或未考虑完全的地方,请大家不吝赐教,您也可以点击"举报违法"按钮提交疑问。

注:此教程以轮式机器人作为一个样例,具体的应用还需要用户自己去做更深入的开发。当然,着并不是唯一的方法,有更好的方法也欢迎大家讲一下自己的思路,有什么说错的地方也欢迎大家批评指正。

Viobot定位用于导航,Viobot使用教程,机器人

路径规划部分我们选用轮式机器人比较常用的move_base。

整个架构就是Viobot使用stereo2算法提供机器人的当前位姿,输入到move_base作为规划起点,再给定目标终点,move_base会输出一个/cmd_vel的话题使机器人运动起来。至于/cmd_vel到控制机器人底盘运动(也就是base controller这部分)的实现就要用户自己去实现了,我们先默认都已经可以实现base controller了。

Viobot定位用于导航,Viobot使用教程,机器人

下面是我整理的一张整体的框架图

Viobot定位用于导航,Viobot使用教程,机器人

接下来就是例程的粗略讲解

1.viobot输出信息处理

接收stereo2的位姿和点云,使用TF转换,构建TF树。

1)接收消息,注册回调函数
std::string point_clound_topic;
//点云话题配置接口,可以通过launch文件配置点云输入,默认为TOF
nh_private.param<std::string>("point_clound_topic", point_clound_topic, "/pr_loop/tof_points");

sub_odom = nh.subscribe("/pr_loop/odometry_rect", 50, &VioOdomNodelet::loop_pose_callback, this);
sub_car_odom = nh.subscribe("/odom", 50, &VioOdomNodelet::car_odom_callback, this);
sub_pointcloud = nh.subscribe(point_clound_topic, 50, &VioOdomNodelet::loop_pointclound_callback, this);
2)回调函数处理位姿信息

自己去维护了一个从base_linkvio_odom的动态TF变换

void VioOdomNodelet::loop_pose_callback(const nav_msgs::OdometryPtr &msg){
    static tf::TransformBroadcaster odom_broadcaster;//定义tf 对象
    geometry_msgs::TransformStamped odom_trans;//定义tf发布时需要的类型消息
    nav_msgs::Odometry odom;

    odom.header.stamp = msg->header.stamp;
    odom.header.frame_id = "vio_odom";
    odom.child_frame_id = "base_link";
    Eigen::Vector3d position_in_vio(msg->pose.pose.position.x, msg->pose.pose.position.y,msg->pose.pose.position.z);
    
    tf::Quaternion original_quat(msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z, msg->pose.pose.orientation.w);
    
    //选转到跟base_link方向一致
    tf::Quaternion quat_rotate;
    quat_rotate.setRPY(0, -M_PI/2, M_PI/2);
    // quat_rotate.setRPY(0,M_PI/2, -M_PI/2);

    // Apply the rotations
    tf::Quaternion final_quat = original_quat * quat_rotate;
    
    // Convert the final quaternion back to a geometry_msgs Quaternion
    geometry_msgs::Quaternion geo_q;
    tf::quaternionTFToMsg(final_quat, geo_q);

    if(height_charge){//一个将Z轴归零的策略,可根据自己的实际情况配置
        frame_z = msg->pose.pose.position.z;
        position_in_vio.z() = 0;
        odom.pose.pose.position.z = 0;//z轴归零
        //这里为了把位姿朝向规整到水平面
        vio_quat = geo_q;
        // printf("--------------------\npose:\nx:%lf\ny:%lf\nz:%lf\nquat:\nx:%lf\ny:%lf\nz:%lf\nw:%lf\n",msg->pose.pose.position.x,msg->pose.pose.position.y,
        //                                                             msg->pose.pose.position.z,msg->pose.pose.orientation.x,msg->pose.pose.orientation.y,
        //                                                             msg->pose.pose.orientation.z,msg->pose.pose.orientation.w);
        double yaw = tf::getYaw(vio_quat);
        // geometry_msgs::Quaternion geo_q1 = tf::createQuaternionMsgFromYaw(yaw + M_PI / 2);
        geometry_msgs::Quaternion geo_q1 = tf::createQuaternionMsgFromYaw(yaw);

        odom.pose.pose.orientation = geo_q1;
    }
    else{
        odom.pose.pose.position.z = msg->pose.pose.position.z;
        odom.pose.pose.orientation = geo_q; 
    } 

    position_in_vio = position_in_vio + t_vio_base_link;

    // Coordinate transformation from VIO to base_link
    // odom.pose.pose.position.x = position_in_vio.x();
    // odom.pose.pose.position.y = position_in_vio.y();
    // odom.pose.pose.position.z = position_in_vio.z();
    odom.pose.pose.position.x = msg->pose.pose.position.x;
    odom.pose.pose.position.y = msg->pose.pose.position.y;

    //这里odom的速度可选stereo2输出的速度,也可以选择底盘论速计的速度
    // odom.twist.twist.linear.x = msg->twist.twist.linear.x;
    // odom.twist.twist.linear.y = msg->twist.twist.linear.y;
    odom.twist.twist.linear.x = linear_vx;
    odom.twist.twist.linear.y = linear_vy;
    odom.twist.twist.linear.z = 0.0;
    odom.twist.twist.angular.x = 0.0;
    odom.twist.twist.angular.y = 0.0;
    odom.twist.twist.angular.z = angular_vz;
    // odom.twist.twist.angular.z = msg->twist.twist.angular.z;

    odom_trans.header.stamp = msg->header.stamp;
    odom_trans.header.frame_id = "vio_odom";
    odom_trans.child_frame_id = "base_link";
    odom_trans.transform.translation.x = position_in_vio.x();//x坐标
    odom_trans.transform.translation.y = position_in_vio.y();//y坐标
    odom_trans.transform.translation.z = position_in_vio.z();//z坐标        
    odom_trans.transform.rotation = odom.pose.pose.orientation;//偏航角
    odom_broadcaster.sendTransform(odom_trans);

    //区分静止和运动时的协方差
    if(msg->twist.twist.linear.y == 0 && msg->twist.twist.linear.x == 0 && msg->twist.twist.angular.z == 0){
        memcpy(&odom.pose.covariance, odom_pose_covariance2, sizeof(odom_pose_covariance2));
        memcpy(&odom.twist.covariance, odom_twist_covariance2, sizeof(odom_twist_covariance2));
    }
    else{
        memcpy(&odom.pose.covariance, odom_pose_covariance, sizeof(odom_pose_covariance));
        memcpy(&odom.twist.covariance, odom_twist_covariance, sizeof(odom_twist_covariance));
    }
    pub_odom.publish(odom);
}
3)回调函数处理点云信息

把点云划到vio_odom上面,它会根据上面的base_linkvio_odom的动态TF变换变换

void VioOdomNodelet::loop_pointclound_callback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg_in){
    sensor_msgs::PointCloud2 cloud_msg_out = *cloud_msg_in;

    // Set the frame_id of the output pointcloud to base_link
    cloud_msg_out.header.frame_id = "vio_odom";

    if(height_charge){
        Eigen::Quaterniond q(vio_quat.w, vio_quat.x, vio_quat.y, vio_quat.z);
        Eigen::Matrix3d rotation_matrix = q.normalized().toRotationMatrix();

        //将Eigen::Matrix3d 手动转换为 tf::Matrix3x3
        tf::Matrix3x3 tf_rotation_matrix(rotation_matrix(0, 0), rotation_matrix(0, 1), rotation_matrix(0, 2),
                                        rotation_matrix(1, 0), rotation_matrix(1, 1), rotation_matrix(1, 2),
                                        rotation_matrix(2, 0), rotation_matrix(2, 1), rotation_matrix(2, 2));

        double yaw, pitch, roll;
        tf_rotation_matrix.getRPY(roll, pitch, yaw);

        //创建pitch和roll的逆旋转
        Eigen::Matrix3d pitch_matrix_inv = Eigen::AngleAxisd(-pitch, Eigen::Vector3d::UnitY()).toRotationMatrix();
        Eigen::Matrix3d roll_matrix_inv = Eigen::AngleAxisd(-roll, Eigen::Vector3d::UnitX()).toRotationMatrix();

        Eigen::Matrix3d final_rotation_matrix;
        // 应用逆旋转到原始四元数上
        if(inv){
            final_rotation_matrix = pitch_matrix_inv * roll_matrix_inv;
        }
        else{
            final_rotation_matrix << 1,0,0,0,1,0,0,0,1;
        }

        sensor_msgs::PointCloud pointcl1;
        sensor_msgs::convertPointCloud2ToPointCloud(cloud_msg_out,pointcl1);
        for(int i = 0;i < pointcl1.points.size();i++){
            Eigen::Vector3d point_eigen(pointcl1.points[i].x, pointcl1.points[i].y, pointcl1.points[i].z);
            point_eigen = final_rotation_matrix * point_eigen;
            pointcl1.points[i].x = point_eigen.x();
            pointcl1.points[i].y = point_eigen.y();
            pointcl1.points[i].z = point_eigen.z() - frame_z;
            // printf("frame_z = %lf\n",frame_z);
        }
        sensor_msgs::convertPointCloudToPointCloud2(pointcl1,cloud_msg_out);  
    }
    pub_pointcloud.publish(cloud_msg_out);
}

2.pointcloud转laserscan

其实就是根据原有的开源代码做了一下修改,原有的代码配置了use_inf为false时,当点云的距离大于range_max就不显示了,这样会造成一些刷新上面的困难,所以再输出/scan话题之前加了1.5m的距离,让/scan话题初始就是一个range_max+1.5的扇形,根据障碍物来刷新。

3.move_base配置

1)launch文件
主要是启动map_server加载地图文件和启动move_base并加载了配置文件。
<launch>
    <!-- Run the map server -->
    <node name="map_server" pkg="map_server" type="map_server" args="$(find nav_robot)/maps/map_office.yaml"/>
    <!-- <node name="map_server" pkg="map_server" type="map_server" args="$(find nav_robot)/maps/blank_map.yaml"/> -->


    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
        <rosparam file="$(find nav_robot)/config/mark_1/costmap_common_params.yaml" command="load" ns="global_costmap" />
        <rosparam file="$(find nav_robot)/config/mark_1/costmap_common_params.yaml" command="load" ns="local_costmap" />
        <rosparam file="$(find nav_robot)/config/mark_1/local_costmap_params.yaml" command="load" />
        <rosparam file="$(find nav_robot)/config/mark_1/global_costmap_params.yaml" command="load" />
        <rosparam file="$(find nav_robot)/config/mark_1/move_base_teb_params.yaml" command="load" />
        <rosparam file="$(find nav_robot)/config/mark_1/teb_local_planner_params.yaml" command="load" />
    </node>
    <!-- <node name="rviz" pkg="rviz" type="rviz" args="-d $(find nav_robot)/rviz_cfg/nav_test.rviz" /> -->

</launch>
2)地图文件
这个需要用户把自己的场景先建里一个地图先验,可以是使用雷达等设备,也可以使用viobot(这个建图要单独开一篇来讲)。
3)move_base配置文件

每个文件的参数都有详细注释,用户可以自行查看参数的意义和选择配置。

在文件costmap_common_params.yaml有一part需要重点说明的:

障碍物层输入是/scan ,不使用pointcloud是因为move_base底层代码逻辑,避障是使用世界系的点云的,其实避障使用body系点云应该是更合理的,所以我们把一定高度范围的pointcloud转成了/scan;其次是obstacle_range和raytrace_range两个参数,它会跟踪raytrace_range范围内的障碍物,但是只有在obstacle_range范围内的障碍物点才会被加到代价地图,所以我们前面在转/scan的出话题的时候加了那个1.5m就是为了把大部分点定到obstacle_range和raytrace_range中间,使得障碍物能够快速被刷新。

Viobot定位用于导航,Viobot使用教程,机器人文章来源地址https://www.toymoban.com/news/detail-684099.html

到了这里,关于Viobot定位用于导航的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请点击违法举报进行投诉反馈,一经查实,立即删除!

领支付宝红包 赞助服务器费用

相关文章

  • ROS2下使用TurtleBot3-->SLAM导航(仿真)RVIZ加载不出机器人模型

    在使用台式机进行仿真时,大部分例程很顺利,但在SLAM导航时,在RVIZ中却一直加载不出机器人模型,点击Navigation2 Goal选择目标点进行导航时,无响应。 启动后在RVIZ2和终端看到一个错误 按照官网的指令试了多次,一直无法加载,在网上赵的解决方案都是修改RVIZ里的各种设

    2024年02月09日
    浏览(52)
  • 机器人制作开源方案 | 全自动导航分拣机器人

    作者:孙国峰 董阳 张鑫源 单位:山东科技大学 机械电子工程学院 指导老师:张永超 贝广霞 1.1 研究背景       在工业生产中,机器人在解决企业的劳动力不足,提高企业劳动生产率,提高产品质量和降低生产成本方面有着显著的意义。随着计算机自动化技术和社会的发展

    2024年01月23日
    浏览(54)
  • 使用贝叶斯滤波器通过运动模型和嘈杂的墙壁传感器定位机器人研究(Matlab代码实现)

     💥💥💞💞 欢迎来到本博客 ❤️❤️💥💥   🏆博主优势: 🌞🌞🌞 博客内容尽量做到思维缜密,逻辑清晰,为了方便读者。 ⛳️ 座右铭: 行百里者,半于九十。 📋📋📋 本文目录如下: 🎁🎁🎁 目录 💥1 概述 📚2 运行结果 🎉3 参考文献 🌈4 Matlab代码实现 使用

    2024年02月14日
    浏览(46)
  • 使用环境中的视觉地标和扩展卡尔曼滤波器定位移动机器人研究(Matlab代码实现)

     💥💥💞💞 欢迎来到本博客 ❤️❤️💥💥 🏆博主优势: 🌞🌞🌞 博客内容尽量做到思维缜密,逻辑清晰,为了方便读者。 ⛳️ 座右铭: 行百里者,半于九十。 📋📋📋 本文目录如下: 🎁🎁🎁 目录 💥1 概述 📚2 运行结果 🎉3 参考文献 🌈4 Matlab代码及文章 本文

    2024年02月10日
    浏览(52)
  • 机器人SLAM与自主导航

    机器人技术的迅猛发展,促使机器人逐渐走进了人们的生活,服务型室内移动机器人更是获得了广泛的关注。但室内机器人的普及还存在许多亟待解决的问题,定位与导航就是其中的关键问题之一。在这类问题的研究中,需要把握三个重点:一是地图精确建模;二是机器人准

    2024年02月08日
    浏览(36)
  • 【ROS】—— 机器人导航(仿真)—导航实现(十八)[重要][重要][重要]

    📢本系列将依托赵虚左老师的ROS课程,写下自己的一些心得与笔记。 📢课程链接:https://www.bilibili.com/video/BV1Ci4y1L7ZZ 📢讲义链接:http://www.autolabor.com.cn/book/ROSTutorials/index.html 📢 文章可能存在疏漏的地方,恳请大家指出。 安装相应功能包: 安装 gmapping 包(用于构建地图): sudo a

    2024年02月01日
    浏览(49)
  • 导航机器人硬件配置及其常用功能

    提示:这里可以添加系列文章的所有文章的目录,目录需要自己手动添加 TODO:写完再整理

    2024年02月11日
    浏览(44)
  • 使用mid360从0开始搭建实物机器人入门级导航系统,基于Fast_Lio,Move_Base

    本文原本只是自己在拿到mid360后,开始进行开发过程的一些问题和学习的记录。毕竟实物和仿真还是有很多不同,且由于碰到的问题也比较多,READEME也越来越详细,所以就干脆整合起来,做成了一篇使用mid360的搭建入门的导航系统全流程分享。里面用到的都是主流的开源的框

    2024年02月05日
    浏览(52)
  • 《机器人SLAM导航核心技术与实战》第1季:第5章_机器人主机

    《机器人SLAM导航核心技术与实战》第1季:第5章_机器人主机 视频讲解 【第1季】5.第5章_机器人主机-视频讲解 【第1季】5.1.第5章_机器人主机_X86与ARM主机对比-视频讲解 【第1季】5.2.第5章_机器人主机_ARM主机树莓派3B+-视频讲解 【第1季】5.3.第5章_机器人主机_ARM主机RK3399-视频讲

    2024年02月08日
    浏览(45)
  • 无人驾驶动态避障策略调研 | 机器人动态避障策略 | 行人轨迹预测 | 机器人导航

    最近在研究机器人协同路径规划策略,发现现有paper中的obstacle都是静态的,但是在实际场景中,常有动态障碍的情形,如走动的行人等等。 为了更好的了解相关技术,我开始调研无人驾驶领域中的动态避障策略: 无人驾驶技术是多个技术的集成,包括了传感器、定位与深度

    2023年04月08日
    浏览(34)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包,优惠每天领

二维码1

领取红包

二维码2

领红包