0
  • 聊天消息
  • 系统消息
  • 评论与回复
登录后你可以
  • 下载海量资料
  • 学习在线课程
  • 观看技术视频
  • 写文章/发帖/加入社区
会员中心
创作中心

完善资料让更多小伙伴认识你,还能领取20积分哦,立即完善>

3天内不再提示

反光柱导航开发与实验

3D视觉工坊 来源:CSDN博主一叶执念 2023-07-14 15:37 次阅读

VEnus算法对于反光柱导航的基本思路,其主要分为了高反点提取、高反点聚类查找中心、高反点与已知反光柱位姿匹配以及调用ceres库进行位姿优化等步骤。

然后在这个算法的基础上,再进行一定的开发达到一个比较稳定且可视化的版本。


使用:

ffmpeg -f x11grab -framerate 25 -video_size 1920*1080 -i :0.0 out.mp4

这里使用的是自己录制的一个包,其中包含了激光数据以及里程计信息odom。为了使结果看起来更舒适,主要进行了以下几个部分的改进:

1、发布map到odom变换关系

为了在rviz中进行点云以及反光柱的显示,需要绑定一个固定TF,一开始使用的是以odom作为固定坐标系,但是由于odom本身存在比较大的累积误差,所以后面发现效果并不好。

所以这里需要重新计算一下从map到odom的累计漂移发布到TF树中,以形成一个较为稳定的TF关系。

tf::Quaternion quat1;
  tf::quaternionMsgToTF(new_odom.pose.pose.orientation, quat1);
  double roll1, pitch1, yaw1;//定义存储rpy的容器
  tf::Matrix3x3(quat1).getRPY(roll1, pitch1, yaw1);//进行转换
  //上一时刻机器人的角度
  double angel2 = last_pose.theta();
  //yaw1为机器人在odom下的角度,相减得到两个坐标系之间的角度差
  double angel = yaw1-angel2;
  odom_trans.header.frame_id = "map";
 odom_trans.child_frame_id = "odom";
  odom_trans.header.stamp = ros::now();
 odom_trans.transform.translation.x = (last_pose.x() - new_odom.pose.pose.position.x * cos(-angel) + new_odom.pose.pose.position.y * sin(-angel));
 odom_trans.transform.translation.y = (last_pose.y() - new_odom.pose.pose.position.x * sin(-angel) - new_odom.pose.pose.position.y * cos(-angel));
 odom_trans.transform.translation.z = 0;
 odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(-angel);
  odom_trans.header.stamp = ros::now();
  ROS_INFO("tf map_to_odom: x = %f,y = %f,theta = %f",new_odom.pose.pose.position.x - last_pose.x(),new_odom.pose.pose.position.y - last_pose.y(),angel);
  broadcaster.sendTransform(odom_trans);

通过已知机器人在map坐标系下的pose以及odom下的位姿,反求odom到map的TF。先计算两个坐标系下的位姿的角度差,这个代表了两个TF之间的旋转分量。

在已知旋转分量的情况下再计算两个TF之间的平移分量。最后再将具体的TF关系通过broadcaster发布出来。

2、添加前端可视化插件

为了能够在rviz中更加方便的查看机器人的运动以及匹配情况,需要在rviz中添加一下反光柱与机器人的一些可视化消息。

对于反光柱的消息,分为两类:已经确定的以及当前的。已经确定的是指那些被放到feature中我们认为其已经代表一个正常的反光柱的信息的那些点。

当前的则是指当前时间下我通过TF关系将当前坐标系下的点变换到map坐标系下后的点。对于这两种点。我们使用markarray来进行可视化操作,同时以不同的颜色(红色/绿色)来区分它们,类似于这样:

std::unordered_map>::iterator ite;
  for(ite=feature_points.begin();ite!=feature_points.end();ite++)
  {
    cylinder_arrow.ns = "cylinder";  //命名空间namespace                                     
    cylinder_arrow.type = visualization_msgs::CYLINDER;   //类型
    cylinder_arrow.action = visualization_msgs::ADD;
    cylinder_arrow.lifetime = ros::Duration(); //(sec,nsec),0 forever
    cylinder_arrow.header.frame_id = "map";
    cylinder_arrow.header.stamp = ros::now();
    cylinder_arrow.id = ite->first;
    cylinder_arrow.pose.position.x = ite->second.first;
    cylinder_arrow.pose.position.y = ite->second.second;
    cylinder_arrow.pose.position.z = 0;
    cylinder_arrow.pose.orientation.w = 1.0;
    cylinder_arrow.pose.orientation.x = 0;
    cylinder_arrow.pose.orientation.y = 0;
    cylinder_arrow.pose.orientation.z = 0;
    //设置标记尺寸
    cylinder_arrow.scale.x = 0.09; //m
    cylinder_arrow.scale.y = 0.09;
    cylinder_arrow.scale.z = 0.50;


     ///设置标记颜色
    cylinder_arrow.color.a = 1.0; // Don't forget to set the alpha!
    cylinder_arrow.color.r = 1.0;
    cylinder_arrow.color.g = 0.0;
    cylinder_arrow.color.b = 0.0;
    cylinder_Array.markers.push_back(cylinder_arrow);
  }
  mark_info_pub.publish(cylinder_Array);
  cylinder_Array.markers.clear();

这里只是feature_point的可视化发布,另外一个类似就不摘抄了。最后得到的就是类似于下图这样子的对应关系:

0ebd74b8-21d2-11ee-962d-dac502259ad0.png

3、优化反光柱位姿匹配问题

在上一章中分析源代码的过程我们大致了解了算法的原理主要是基于对不同反光柱点的距离匹配的。例如对于我现有的反光柱ABCD。

A到B的距离为5,A到C的距离为8,A到D的距离为6,B到C的距离为7,B到D的距离为4,C到D的距离为3。然后当前帧检测到几个反光柱点A1,B1,C1。A1到B1的距离为5,A1到C1的距离为4。

则其分别与BA、BD两条边对应,所以A1与B点对应。这种情况适合大部分反光柱,但是会存在一种不适合的情况:当全局反光柱中存在A、D两个反光柱其到周围其他反光柱之间的距离都相等的时候。

那么新检测到的反光柱可能会匹配到A也可能会匹配到D。这种情况下就会存在比较大的误匹配问题。

对于这个问题,我们在算法中的位姿匹配部分新添加了一个新的约束关系:在检测与匹配当前时刻的反光柱与全局反光柱的时候,我们不仅对其存在的边进行约束关系的判断。

还对这两个匹配上的点之间进行一定距离上的约束。如果当前帧下的投影点在全局坐标系下的位姿与对应匹配上的点的全局坐标差别较大的话,我们认为这是可能存在问题的。

舍弃这一个匹配结果继续寻找其他可能存在的匹配点。这样子可以保证基本每次匹配上的点都是比较正确的。类似于这样子:

0ec70ed8-21d2-11ee-962d-dac502259ad0.png

测试时在终端打印出了对应匹配点的坐标关系,point1是map坐标系下的反光柱的位姿,而point2是当前时刻下检测到的高反点对应在map坐标系下的坐标。整个匹配结果基本上是正确的。

4、优化位姿匹配错误的跳点问题

跳点问题是指当反光柱匹配错误是,导致后面优化后的位姿突然一下子出现在离上一时刻很远的地方。

这个在没有优化匹配之前还是较容易出现的,但是匹配结果正确的话基本上不会出现。只是作为一个保险的手段还是在这里添加一下。

根据运动逻辑我们知道机器人运动过程不可能一下子出现在离上一时刻很远的地方。所以在优化完位姿后,算法会在更新位姿之前对新的位姿进行一次判断。

如果位姿与前一时刻想差较大,例如一下子跳到2-3米远。时间间隔才0.1S,那肯定是有问题的。这个位姿不应该给到其他地方进行运动计算。所以这时候需要舍弃这个结果重新计算。

一般场景中反光柱数量够多的话是不会出现这个问题的,测试时基本位姿也都是正确的。

5、优化同一位置出现多个反光柱的问题

在上面关于rviz中的图片中存在一个BUG,就是在同一个位置出现了两个不同的红色反光柱。

这个反光柱其实是代表的同一个反光柱,出现这个问题的原因是再于:在进行位姿匹配的过程中,当前帧的反光柱点与现有的反光柱点之间的距离超过了阈值,所以这两个点没有被匹配为一个点。

于是在求完新的位姿后,这个点作为一个新的反光柱点被添加到了feature_point里面。所以在这个位置就出现了一个新的反光柱。这个问题有两种解决方案:第一种比较简单就是增大范围判断的阈值。

这样子同一个位置的反光柱就不会出现第二个了。另外一种是在新增反光柱的时候判断一下这个点周围多少范围内是否存在反光柱,如果有一个的话就不用新增了。

0ed27444-21d2-11ee-962d-dac502259ad0.png


审核编辑:汤梓红

声明:本文内容及配图由入驻作者撰写或者入驻合作网站授权转载。文章观点仅代表作者本人,不代表电子发烧友网立场。文章及其配图仅供工程师学习之用,如有内容侵权或者其他违规问题,请联系本站处理。 举报投诉
  • 算法
    +关注

    关注

    23

    文章

    4607

    浏览量

    92840
  • 导航
    +关注

    关注

    7

    文章

    528

    浏览量

    42405
  • SLAM
    +关注

    关注

    23

    文章

    423

    浏览量

    31822
  • Venus
    +关注

    关注

    0

    文章

    7

    浏览量

    2671

原文标题:反光板导航SLAM-反光柱导航开发与实验

文章出处:【微信号:3D视觉工坊,微信公众号:3D视觉工坊】欢迎添加关注!文章转载请注明出处。

收藏 人收藏

    评论

    相关推荐

    反光板的选用

    推荐一款用在智能小车上红外测距传感器的反光板吧{:19:}
    发表于 07-19 12:57

    《FPGA开发实例导航

    本帖最后由 eehome 于 2013-1-5 09:47 编辑 《FPGA开发实例导航
    发表于 08-20 19:39

    【OK210申请】GPRS导航系统

    申请理由:本人是嵌入式爱好者,学嵌入式开发已有三年经历,想开发一款gprs导航系统,需要用到ok210系统板。项目描述:项目名称:GPS导航系统本GPS
    发表于 07-27 13:03

    激光导航AGV控制系统解决方案(无反光板非工控机)

    传感器配合使用,就可以实现精准的激光无轨导航功能。LNM-v1.0(工业级)无轨导航目前,苏州智伟达机器人科技有限公司与各大公司合作已经实现了LNM模块无轨导航功能的应用。实验证明,直
    发表于 06-10 14:07

    请问有LED光柱驱动电路图吗?

    LED光柱驱动电路
    发表于 10-11 08:15

    有没有办法用VL53L0X 区分反光板和其他反光材料?

    如您所知,无论反光片表面角度如何,反光片都非常明亮。然而,其他反光材料,如镜子,只有在表面角度垂直于 VL53L0X 时才会亮。在这种情况下,仅凭亮度的差异是很难区分的。 有没有办法用 VL53L0X 区分
    发表于 12-27 06:24

    关于AGV控制系统使用中的应用笔记(二)——追加地图操作

    。地图追加常用于以下工况场景:①在激光反射板导航地图或激光自动导航地图发生变动的情况下,将变动场景部分更新至 AGV 运行地图。②在原有激光自动导航地图上叠加反射板导航定位所需的
    发表于 03-13 17:10

    LED 光柱式仪表的串行动态显示

    嵌入式工业自动化仪表中常常需要显示系统的工作状态或测量结果。显示方法有很多种,如液晶显示屏、LED数码管等。近年来,光柱模拟加数字混合显示的仪表很受欢迎。光柱模拟
    发表于 05-14 13:20 28次下载

    基于VxWorks的导航系统人机交互界面的开发

    本文介绍了嵌入式实时操作系统VxWorks 及其图形开发组件WindML,阐述了车辆导航系统的组成,提出了导航系统中人机交互界面开发的相关技术。经
    发表于 08-03 11:07 16次下载

    48N系列光柱显示器

    高可视性3毫米宽光柱显示 IP65级防尘防水 全机种符合CE标准 新增蓝色LED光柱显示 深度缩40%,重量减30%的小型设计
    发表于 04-19 10:52 16次下载

    太阳能LED光柱系统的设计制作

    本文设计的太阳能LED光柱系统属于小型太阳能独立光伏系统.从小型太阳能发电系统的研究和整体设计出发,设计出由太阳能电池板供电,LED发出彩色光的太阳能LED光柱.太阳能LED光柱
    发表于 04-21 16:58 140次下载
    太阳能LED<b class='flag-5'>光柱</b>系统的设计制作

    LED光柱驱动电路

    LED光柱 驱动电路 作为一个指标,S1选择直接输入或一个1k(高阻抗)音频输入。 R2是灵敏度控制。
    发表于 11-03 16:36 2721次阅读

    基于手机app的室内导航系统的设计与开发

    综合考虑常用室内导航系统精确度差、室内平面结构集纳度低、高校公共基础实验室错综复杂等现状,为了方便实验者快速、便捷的到达指定实验室位置,本文以南京航空航天大学物理
    发表于 12-21 13:54 1次下载
    基于手机app的室内<b class='flag-5'>导航</b>系统的设计与<b class='flag-5'>开发</b>

    LED光柱显示器的基本结构

    关键词:LED , 光柱显示器 , 基本结构 如图所示为LED光柱显示器的基本结构。在这种结构中,将高亮度发光二极管芯片排列成一条直线,将100个芯片分成10组,每组10个芯片。每组共用一个阴极
    发表于 10-23 00:03 535次阅读

    反光导航SLAM:VEnus代码浅析

    通过研究具体的代码我们可以简单了解VEnus中对于反光柱定位的具体流程。
    的头像 发表于 03-14 10:54 1467次阅读