# fanguangban_workspace **Repository Path**: yongwangzhiqiankai/fanguangban_workspace ## Basic Information - **Project Name**: fanguangban_workspace - **Description**: 反光柱定位算法 作者： 苏凯 微信号：yongwangzhiqiankai 微信昵称：一往无前 邮箱： 422168787@qq.com qq交流群号: 742679602,也欢迎企业hr入群招聘 b站演示：https://www.bilibili.com/video/BV1CdtjemE4D/?spm_id_from=333.1387.upload.video_card.click - **Primary Language**: Unknown - **License**: Zlib - **Default Branch**: master - **Homepage**: None - **GVP Project**: No ## Statistics - **Stars**: 19 - **Forks**: 18 - **Created**: 2025-02-08 - **Last Updated**: 2025-08-30 ## Categories & Tags **Categories**: Uncategorized **Tags**: None ## README 反光柱定位算法 作者： 苏凯 微信号：yongwangzhiqiankai 微信昵称：一往无前 邮箱： 422168787@qq.com qq交流群号: 742679602,也欢迎企业hr入群招聘 项目仅供学习交流，企业使用需联系作者 b站演示： https://www.bilibili.com/video/BV1CdtjemE4D/?spm_id_from=333.1387.upload.video_card.click 系统环境： 系统：ubuntu20.04 ros1版本： noetic 雷达: sick TM581 说明： 此版本是针对长走廊环境设计的一款反光柱定位产品，它需要在无人的情况下，而且雷达能够扫描到两边时定位效果比较好。 如果不能保证雷达扫描到墙体就需要重新设计的方式的定位策略。 雷达匹与周边特征的配情况： rosrun fanguangbantrilateration fanguangbantrilateration_matchCostmapWithScan_node 长走廊环境必须启动，非长走廊环境需要自己改一下代码，不要使用这个节点给的置信度数据。 远程开发： 把本机时间同步到工控机中，使得两设备时间一致，方便远程开发，远程编译调试 /home/sukai/workspace/workspace_ros_car_noetic/src/fanguangbantrilateration/src/sync_time.cpp sync_time_password.sh 文件： scanIntensities.txt : 强度值标定文件 allpoint.txt 部署在环境中的反光柱数据 calculate.txt 反光柱特征数据 调试： 数据包仿真调试： 读录制数据: roslaunch fanguangbantrilateration rosbag_trilateration_play.launch 注意数据源路径改成自己的路径 雷达匹与周边特征的配情况： rosrun fanguangbantrilateration fanguangbantrilateration_matchCostmapWithScan_node 必须启动。 注意： 为了方便调试在 trilateration.launch 中把已经包含 fanguangbantrilateration_matchCostmapWithScan_node 节点 注释掉了，所以需要按顺序单独启动。 反光柱定位算法: roslaunch fanguangbantrilateration trilateration.launch 注意文件路径改成自己的路径 日志打印： rosrun fanguangbantrilateration fanguangbantrilateration_read_time_log_node 启动动态参数服务器： rosrun rqt_reconfigure rqt_reconfigure 勾选： is_trilateration 开启反光板定位 参数： time_log_fig 控制日志数据内容 勾选： is_start_place_rotation_ 开启原地旋转不会更新定位数据 参数： rotate_in_place_variance_multiplier_ 在原地旋转时放大定位精度，从而减少定位的抖动带来的移动机器人反复调整位置. 勾选： is_use_scan_points_map_size 使用所有雷达点数量作为计算条件，如果使用所有雷达点数量作为计算条件只能适合在长走廊环境，需要满足所有点都能打到墙壁，不适合在宽阔且雷达打不到墙的环境中使用。不勾选 可以尝试在宽阔且雷达打不到墙的环境中使用，需要满足一部分雷达点能打到墙壁。 参数： matchingcostma_radius_ 雷达匹配情况->雷达点的与墙的搜索半径 参数： matchingcostma_threshold_ 雷达匹配情况->比例阀值，超过这个比例认为雷达与墙匹配的挺好 使用： 部署反光柱： 启动动态参数服务器： rosrun rqt_reconfigure rqt_reconfigure 勾选： is_trilateration 非勾选状态 rviz图形化中有 /clicked_point 话题，并指定回调函数，依次取地图反光板点位。 启动rosservice客户端服务： rosrun rqt_service_caller rqt_service_caller 选择： /trilateration_service 参数 request_type 输入： handleSavePosint 保存取到的地图反光板点位 参数 request_type 输入： handleCalculatePosintAllFeature 反光柱点位部署完成后就计算所有点位特征，自动保存成 calculate.txt 文件 ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------- 参数服务器调整部署在地图中的所有反光柱坐标： 第一步取消勾选 is_trilateration_ 关闭反光板功能定位 第二步输入需要调整的 反光柱id 第三步点击 isslectPoint 查询 第四步点击上下左右移动反光柱点 第五步点击 isupdate_point 会更改 allpoint.txt 文件的数据 第六步重启或者重新加载数据 第七步重新计算点位特征 启动： rosrun rqt_service_caller rqt_service_caller 选择： /trilateration_service 参数 request_type 输入： handleCalculatePosintAllFeature 特征数据输出 calculate.txt ============================================================================================================================================================================ rosservice客户端服务： 服务名： /trilateration_service 服务消息： trilateration_srvs.srv string SHAPE_CYLINDER=CYLINDER 圆柱形 string SHAPE_CUBE=CUBE 立方体 string request_type bool is_trilateration 定位标志，是否使用反光板定位，false：暂停定位 string posint_id 点位唯一标示 string posint_id2 string shape 多边形类型 float64 posint_x float64 posint_y float64 posint_z --- string result string resultTxt int32 savepointnum int32 numPoints bool success request_type： handleSaveMapTbaselinkPose handleSavePosint 保存单个也可以是多个点位,部署时鼠标获取的点，取几个就保存几个。依次储存每一个反光板在map下的坐标数据到json文件中 handleDeletePosint 删除单个点位，部署时鼠标获取的点。 handleDeletePosintAll 实现删除所有缓存点位的逻辑，部署时鼠标获取的点。 handleSavePosintAndCalculatePosintAllFeatureAndStartLocation 部署反光板的同时进行定位 依次储存每一个反光板在 map 下的坐标数据到 json文件中，并创建 ReflectorPanelPoint对象 handleDeletePosintWithJsonFile 删除单个json中的数据，删除单个数据后会全部重新计算特征数据 handleUpdatePosintWithJsonFile 更新单个json中的数据 handleDeleteMatchokReflectorPanelPointEnd_Unordered_map 删除已经 MatchokReflectorPanelPointEnd_Unordered_map 匹配上的点 handleReloadJSONPosintAll 重新获取的JSON中的反光板单个点位 ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------- handleCalculatePosintAllFeature 计算所有点位特征，并保存成json文件 handleDeletePosintAllWithSavePointFilePath 实现删除所有json文件中的点位pointAllMaps handleDeleteCalculatePosint 删除计算所有点位的特征数据 handleReloadJSONDataTxt 重新加载 ，所有点位的json特征文件 handleFindPosintById 查询数据 用1个反光板坐标id查1个点的数据 handleFindPosintByIds 查询数据 用2个反光板坐标id查一条线的前后2个点的数据 handleStartLocation 开启反光板定位 handleEndLocation 停止反光板定位 handleTrilateration ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------- handleSaveScanIntensitiesFromJSONDataTxt 保存雷达强度表格数据，scanIntensities.txt： {"scan_intensities":[[1,1600],[3,1500],[6,1400],[10,1300]]} handleReloadScanIntensitiesFromJSONDataTxt 实现重新加载雷达强度数据json表格 handleDeleteScanIntensitiesFromJSONDataTxt delete 雷达强度表格数据 json ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------- handlePrintReflectorPanelPoints_vector 控制台打印所有已经转为ReflectorPanel反光板，并且排序好的反光板数据 handlePrintReflectorPanelPointAllUnordered_maps 控制台打印所有已经转为ReflectorPanel反光板 handlePrintMatchokReflectorPanelPointEnd_Unordered_map 打印 储存匹配上的点位，map下的反光板全局坐标 handlePrintSavePosint 打印鼠标点击地图得到的反光板缓存的数据 handlePrintPosintAllMap 打印加载json中反光板的数据 handlePrintPrintIndices handlePrintUnorderedEdgeMapNew_IdKey 所有点线特征-反光板的数据 handlePrintUnorderedEdgeMapNew 所有点线特征-反光板的数据 handleShowAllPointsMarker 显示地图中的所有反光柱，反光板 handleShowScanMaxIntensities 显示雷達最大強度值数据 handleShowScanMaxIntensitiesWithFront 显示雷達正前方的雷达强度值 ============================================================================================================================================================================ 输出的话题： /clicked_point 话题，并指定回调函数，取地图反光板点位 /reflectormarker_marker 发布匹配上的反光板，与匹配过在8米范围内的反光板 /trilateration_marker 发布雷达扫当前描到的反光板 /trilateration_all_marker 发布地图中的所有反光板 【需要匹配上后才会一直显示】 /initialpose 重定位 /trilateration_circle_points 强度点云数据 ============================================================================================================================================================================ 日志： 发布性能评估数据 rosrun fanguangbantrilateration trilateration_read_time_log_node time_log_pub_ = nh.advertise("/trilateration_time_log", 10); trilateration::trilateration_read_time_log string result_type string className 类名 string classLine 行号 string result_chinese_message 中文消息 string result_EN_message 英文消息 string result_message int32 num_points 处理的数据量 string execution_time 执行的时间 选择性能评估数据 参数： time_log_fig_=-1; -1:全部关闭，0：全部打开 ， -2:只打开 "1：FindCircle拟定圆心坐标，6：匹配算法，9：位姿推算" 3个性能评估数据。 *1：FindCircle: 通过雷达强度数据，使用三角函数拟定圆心坐标，并存入反光板数据缓存中 2：匹配数据: 通过得到的圆心坐标，在已经匹配成功的反光板数据缓存中匹配数据 3：匹配数据: 通过得到的圆心坐标，在储存所有的反光板数据缓存中匹配数据 4：匹配数据: 通过得到的圆心坐标，在反光板数据缓存中匹配数据 5：匹配数据: 匹配走广度优先算法 或 匹配走广度优先算法 + 优先使用匹配好的反光板数据 *6：匹配算法：从查找圆心到匹配完成需要的总执行时间 7：位姿推算: 基于之前 （ 已经存在 || 不存 ） 在匹配上的反光柱数据及当前匹配的反光柱数量小于3个时的移动机器人位置坐标计算 8：位姿推算: 经典三边定位最小二乘法，推算位姿 *9：位姿推算: 基于之前已经匹配上了反光板 + 经典三边定位最小二乘法，推算位姿 10：位姿推算: 所有使用默认的 tf_last_odom_to_map_transform_ 数据 发布的移动机器人坐标 11：匹配算法: 反光柱匹配算法：只用特征匹配算法 需要匹配的边长数量 12：位姿推算: 上一帧的位姿odomTbaselink与当前帧的位姿odomTbaselink之间的差异 13：位姿推算: 未计算出有效的定位数据导致超时 14:位姿推算: 未计算出有效的定位数据导致超时,通过检查不同条件来决定是否需要重新开始定位过程 [超时重新计算 (trilateration_s>targetTrilaterationTimeOut_)] 15:位姿推算: 未计算出有效的定位数据导致超时,通过检查不同条件来决定是否需要重新开始定位过程 [判断当前机器人的定位数据是否不满足条件，移动机器人不在地图的自由区域中(result_Mapvalue_!=free_cell_)，不满足则重新计算] 16:位姿推算: 未计算出有效的定位数据导致超时,通过检查不同条件来决定是否需要重新开始定位过程 [超时重新计算,雷达与地图轮廓的结合率 bool is_ratio = (ratio >= matchingcostma_threshold_)，不满足则重新计算] 17:位姿推算: 未计算出有效的定位数据导致超时,通过检查不同条件来决定是否需要重新开始定位过程 [超时重新计算,进入这个函数 evaluateTrilaterationTimeout，分别判断 14，15，16] 18:匹配算法: 不满足条件移动机器人不在地图的自由区域中或者雷达与地图轮廓的结合率不满足阀值 【( max_ratio -> 分别计算 AB 向量在map中的姿态YAW，与 ab 向量在 baselink 中的姿态yaw。 baselink在map中的姿态Yaw=YAW-yaw。 （法二） 通过雷达扫描到的反光柱的距离数据，移动机器人中心点baselink坐标以及相对的雷达的安装坐标，我们可计算得到反光柱到baselink的距离，通过着两个距离 分别用三边定位最小二乘法计算出 移动机器人中心点baselink在map中的坐标以及雷达laser在map中的安装坐标。 通过着两个坐标可以拟合一条向量，通过这条向量计算得到姿态。当然我么也可以使用移动机器人身上的其它点为来计算，因为机器人是刚性的，已知机器人身上的任意一个坐标，我们就能计算得到其他任意坐标 ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------- 进阶版工程化本： 优化一：显著提升了定位数据发布的实时性以及匹配上的反光柱显示实时性，让定位更加及时准确，显示更加清晰直观。 优化二：进一步优化了系统稳定性，为可靠运行提供了坚实保障。 优化三：增加了相同特征与镜像特征处理策略，有效避免了定位跳变。在窄长走廊中，由于空间限制只能一维部署反光柱，难以具备唯一特征，此策略的重要性不言而喻。 优化四：新算法采用直接法替换原最小二乘三边定位算法。运用 Ceres 优化框架动态计算反光柱与机器人 base_link 之间的距离，并结合里程计数据实时优化。 通过每一帧数据计算反光柱位置，自动微分和残差优化结合 CauchyLoss 函数处理噪声，从而获得更精确的位姿。 与最小二乘三边测量法相比，新算法在动态调整、优化过程和鲁棒性方面优势明显，尤其适合动态环境下的机器人定位，而最小二乘三边测量法在动态复杂环境中精度相对较低。 ============================================================================================================================================================================ 开发版本，草稿demo /home/sukai/workspace/workspace_ros_car_noetic/src/trilateration test41.cpp - trilateration_read_time_log.cpp test52.cpp - matchCostmapWithScan.cpp test58.cpp，test57.cpp - trilateration.cpp ============================================================================================================================================================================