一种面向规模化养殖场的CAD预建图辅助定位与建图方法

本发明属于定位与建图技术领域，公开了一种面向规模化养殖场的CAD预建图辅助定位与建图方法。首先对养殖场二维CAD布局图进行结构解析和统一标定；随后构建占据栅格地图和连续梯度几何约束代价场并进行坐标关联与融合，形成先验拓扑与几何地图。机器人运行过程中采集多源传感器数据，结合先验拓扑与几何地图完成前端匹配，获得当前位姿估计结果，并根据前端匹配残差确定场景几何退化程度及退化方向。构建包含多源传感器约束和CAD几何约束的因子图，动态调整CAD几何约束信息权重，获得优化后全局位姿序列；对不同时刻激光雷达点云数据进行坐标变换和融合更新，获得与养殖场固定结构保持全局一致的全局地图及机器人实时位姿估计结果。