22年暑假实习过程中第一个开发任务，基于激光雷达实现三边定位功能

开发平台为ubuntu 18.04 + ros melodic

日期：2022.7.22

本次实现：由于实现三边定位需要先找反光柱的圆心，原来计划是先找出激光雷达获取反光柱反射激光的最强点，该点到激光雷达的距离加反光柱半径近似于圆心，误差在于激光雷达本身精度误差，可以接受。而本次将会采用以下文献中提到的三角函数法，利用多个激光高强度点来拟合反光柱圆心，并在圆心位置生成标记。

基于激光雷达和反光板的移动机器人定位方法研究

（一）原理

（二）代码编写

将原来用来收集激光数据的结构体增加一栏，用于记录角度，并在代码中相应位置做相应变化。

struct Point {long index;float range;float intensity;float angle;//角度float x;float y;
};

根据公式原理编写用三角函数法拟定圆心的函数。

//三角函数拟定圆心
Point trigonometry(std::vector< Point >  &points ) {Point trigon{};float sum = 0,num = 0;for (long i = 0; i < points.size(); i++) {sum = sum + points[i].angle;}sum = sum / points.size();for (long i = 0; i < points.size(); i++) {float os = points[i].angle - sum;float ob =  points[i].range * cos(os) + RADIUS * cos(asin(points[i].range * sin(abs(os)) / RADIUS));num = num + ob;}num = num / points.size();std::cout<<"num::" <<num  <<"sum::"<<sum<<std::endl;       trigon.index = 0;trigon.range = num;trigon.intensity = 0;trigon.angle = sum;trigon.x = num * cos(sum);trigon.y = num * sin(sum);return trigon;
}

将原来查找高强点函数修改为查找高强度点组作为反光柱并确定圆心的函数，主要在于替换原来用最强点确定圆心的函数，并将拟合的圆心集合输出。

//查找高强度点组作为反光柱并确定圆心
std::vector< Point > findCircle (const sensor_msgs::LaserScan &scan) {std::vector< Point > points;                          //储存全部高强点激光信息std::vector< Point > points2;                       //各个反光柱激光信息std::vector< Point > points3;                       //各个反观柱圆心坐标信息Point high{};Point circle{};Point end{2000,0,0,0,0,0};int HighStop = 1;  //从1号反光柱开始for (long i = 0; i < scan.ranges.size(); i++) {        //获取高强度点if (scan.intensities[i] > 2000){                    //强度阀值high.index = i;                                              //记录高强点数据high.range = scan.ranges[i] ;//+ RADIUS;          //查找最大值确定圆心方法需要加半径，三角函数法不需要high.intensity = scan.intensities[i];high.angle = scan.angle_min + scan.angle_increment * high.index;high.x = high.range * cos(high.angle);high.y = high.range * sin(high.angle);points.push_back(high);                          //将强度超过阀值的数据储存}}if(points.size() >2){                                           //预防没有高强度数据输入发生报错long sum = points[0].index - 1;points.push_back(end);                            //结束标志位，最后一个反光柱 指数 需要突变for (long j = 0; j< points.size(); j++) {          //分离各反光柱数据if((points[j].index  - sum)== 1) {                //判断是否连续points2.push_back(points[j]);              //连续点为同一组std::cout<<HighStop <<"号反光柱"<<std::endl;    std::cout<<"指数：" <<points[j].index  <<"，第"<<points2.size()<<"个，强度:"<<points[j].intensity<<std::endl;       }else{if(points2.size() > 2 ) {                                //至少连续三个点强度超过   强度阀值   即默认为反光柱,单独点滤除HighStop++;  std::cout<<"数量：" <<points2.size()<<std::endl;       // circle = findMax(points2);                  //查找各个反光柱最大值确定圆心circle = trigonometry(points2);            //三角函数拟定圆心std::cout<<"圆心点:"<<"距离:"<<circle.range<<"角度:"<<circle.angle<<"x轴:"<<circle.x <<"y轴:"<<circle.y <<std::endl;set_marker_fixed_property(circle.x,circle.y,HighStop);      //绘图  画反光柱标记points3.push_back(circle);                    //储存圆心点数据}points2.clear();}sum =points[j].index;} }else{std::cout<<"无法找到有效数据" <<std::endl;   }points.clear();return points3;
}

在处理激光信息函数中获取圆心数据集合，留作下一步开发。

//处理激光信息
void scan_CB(sensor_msgs::LaserScan scan) {std::vector< Outline > outlines;std::vector< Point > point;for (int i = 0; i < 5; i++) {point = findCircle(scan);            //获取各个反光柱圆心}}

（三）运行结果

（四）问题和下一目标

问题：反光柱距离越远，特征点越少，拟合效果越差，抖动就越是剧烈，目前可靠范围仅为五米以内

目标：先根据目前获取的圆心数据编写三边定位函数，然后再考虑将适用范围扩大

基于激光雷达实现三边定位算法开发记录（五）——三角函数法拟定圆心相关推荐

基于激光雷达实现三边定位算法开发记录（一）——查找激光最强点
22年暑假实习过程中第一个开发任务,基于激光雷达实现三边定位开发功能开发平台为ubuntu 18.04 + ros melodic 日期:2022.7.18 本次实现:由于实现三边定位需要先找反光柱 ...
基于激光雷达实现三边定位算法开发记录（二）——查找多个反光柱数据
22年暑假实习过程中第一个开发任务,基于激光雷达实现三边定位开发功能开发平台为ubuntu 18.04 + ros melodic 日期:2022.7.19 本次实现:由于实现三边定位需要先找反光柱 ...
蒙特卡洛粒子滤波定位算法_蒙特卡罗定位算法(基于粒子滤波的定位算法) ——原理、理解与仿真...
1 算法原理 1.1 机器人定位问题关于机器人定位,有三大问题,它们分别是: (1)"全局定位":指初始位置未知,机器人靠自身运动确定自己在地图中的位姿. (2)"位姿 ...
基于MATLAB的车牌定位算法设计
目前国外车牌定位识别系统已经有很多成熟的产品,以色列Hi-Tech公司的See/CarSystem系列,新加坡optasia公司的IMPS系列都是比较成熟的产品.但是,这些产品基本上只适合于自己国内的 ...
毕业设计 : 基于机器学习的驾驶疲劳检测 - 开发记录
文章目录 0 前言 1 总结 2 基于多视频窗口的疲劳驾驶监测系 3 基于近红外图像的疲劳驾驶检测 4 基于面部特征的全天候疲劳驾驶检测及预警 5 其他方法最后 0 前言最近在帮同一位同学研究基于 ...
基于RSSI三维空间四点定位算法
之前写过基于rssi的三点定位算法,但是那是平面的,下面介绍三维空间的四点定位算法: 基于rssi的原理,这里不再重复,请参考以前的博文: http://blog.csdn.net/u01309067 ...
基于android的考勤定位算法,基于移动定位服务的考勤管理系统
[导读] 考勤管理是企事业单位经营和管理工作的重要环节和保障,主要包括两种方式,一种是手工方式,该方式工作繁杂,很难保证数据的准确与实时,增加了管理成本;另一种是利用专用考勤设备的电脑记录考勤方式,包 ...
EIS（电子稳像，视频防抖）算法开发记录
最近项目中有个小需求,需要对运动视频使用电子稳像算法进行稳像处理.开发从0开始,在此记录一下开发过程和心得,做个备份,期间也走了很多弯路,供大家参考.最终效果个人认为基本可媲美主流手机.先上最终效果: ...
基于YOLOv3的车辆号牌定位算法【文末送书】
注:本文选自中国水利水电出版社出版的<PyTorch深度学习之目标检测>一书,略有改动.经出版社授权刊登于此. 文末有送新书福利! 活动截止时间为12月25日晚19点. 01 OCR原理分 ...

基于激光雷达实现三边定位算法开发记录（五）——三角函数法拟定圆心

（一）原理

（二）代码编写

（三）运行结果

（四）问题和下一目标

基于激光雷达实现三边定位算法开发记录（五）——三角函数法拟定圆心相关推荐

最新文章

热门文章