(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210897756.5 (22)申请日 2022.07.28 (71)申请人 电子科技大 学 地址 611731 四川省成 都市高新区 （西区） 西源大道 2006号 (72)发明人 施孟佶　杨森　王梓豪　林伯先　 李维豪　苟蕊　秦开宇　 (74)专利代理 机构 成都虹盛汇泉专利代理有限 公司 51268 专利代理师 王伟 (51)Int.Cl. G06T 7/80(2017.01) G06T 7/13(2017.01) G06T 7/12(2017.01) G06V 10/44(2022.01) (54)发明名称 基于目标几何约束的多形状目标快速定位 方法 (57)摘要 本发明公开了一种基于目标几何约束的多 形状目标快速定位方法， 包括以下步骤： 步骤1、 采集图像序列 S， 求取目标的形状类型Type； 步骤 2、 选择相应的 图像分割及特征点提取方法， 获得 目标特征点在像素坐标系下的坐标； 步骤3、 根据 相机投影方程和目标特征点在像素坐标系下的 坐标， 结合目标几何约束信息建立约束方程组； 步骤4、 通过改进的牛顿迭代方法解算约束方程 组获得特征点的深度值； 步骤5、 融合单目SLA M获 得的相机位姿， 得到特征点在世界坐标系下的坐 标。 本发明仅需要单目图像和目标的几何信息即 可完成对多种形状的目标的三维空间定位， 所需 的硬件成本和计算成本都较低， 系统简单， 计算 量小， 且能够实现较准确的目标定位效果。 权利要求书3页 说明书8页 附图3页 CN 115187675 A 2022.10.14 CN 115187675 A 1.基于目标几何约束的多形状目标 快速定位方法， 其特 征在于， 包括以下步骤： 步骤1、 利用移动机器人平台搭载的视觉传感器采集图像， 按时间顺序排列， 组成图像 序列S， 通过目标检测算法求取图像中目标的形状 类型Type； 步骤2、 根据目标的形状类型Type， 选择相应的图像分割及特征点提取方法， 获得目标 特征点在像素坐标系下的坐标； 步骤3、 根据相机投影方程和目标特征点在像素坐标系下的坐标， 结合目标几何约束信 息建立约束方程组； 步骤4、 通过改进的牛 顿迭代方法解 算约束方程组， 获得 特征点的深度值； 步骤5、 融合单目SLAM获得的相机位姿[R|t]得到特征点在世界坐标系下的坐标， 实现 对多形状目标的快速 定位， R和t分别为描述相机位姿变化的旋转矩阵和平 移向量。 2.根据权利要求1所述的基于目标几何约束的多形状目标快速定位方法， 其特征在于， 所述目标的形状 类型Type包括平行四边形、 圆形或三角形。 3.根据权利要求2所述的基于目标几何约束的多形状目标快速定位方法， 其特征在于， 所述步骤2具体实现方法为： 步骤21、 对检测到图像进行阈值处 理， 获得二 值图像； 步骤22、 通过自适应Can ny边缘提取算法获得图像中目标的边 缘信息； 步骤23、 使用Hough直线变换提取平行四边形或三角形边缘图像中的直线特征， 使用 Hough圆变换提取圆形边 缘图像中的圆弧特 征； 步骤24、 对于直线特征， 通过Harris角点检测提取特征角点； 对于圆弧特征， 则任取圆 弧边缘上 的两点， 分别取两点与圆心连线与对侧圆弧的交点为另外两个特征角点， 若连线 不能够与已知的圆弧段产生交点， 则重新选点； 步骤25、 根据目标类别Type和已知的目标尺度信息剔除异常特 征角点。 4.根据权利要求2所述的基于目标几何约束的多形状目标快速定位方法， 其特征在于， 所述步骤3具体实现方法为： A、 使用平行四边形的距离约束和平行约束建立平行四边形图像的几何约束方程组， 具 体方法为： 取平行四边形四个角点作为特征点描述 目标空间位置， 四个点在世界坐标系 下 的坐标为Pi＝[Xi,Yi,Zi]T,i＝1,2,3,4； 在相机坐标系下的坐标为Pwi＝[Pwxi,Pwyi,Pwzi]T； 同 时在归一化平面中， 目标的归一化投影点坐标表示为Pci＝[pcxi,pcyi,1]T； 目标在像素坐标 系下的坐标为Puvi＝[ui,vi,1]T； dij表示角点 i和j之间连线的距离； 建立6个距离约束方程： 根据平行四边形对边平行 条件 建立4个平行约束方程： 权　利　要　求　书 1/3 页 2 CN 115187675 A 2对于平行四边形目标建立距离约束加平行约束共10个方程； 方程中四个未知量为平行 四边形门框四个角点坐标在 Z轴上的分量pwzi； 选择距离约束中的任意两个方程加上描述一 组对边平行的两个方程组成适定方程组， 解算出平行四边形目标四个角点像素坐标的深度 值； B、 圆的投影是圆形或者椭圆形， 圆心的投影仍然是投影之后图形的对称中心； 通过该 中心的直线与图像边缘的交点之间的连线均为直径； 取过圆心的两条直线与图像边缘的交 点， 得到四个点pc1,pc2,pc3,pc4， 其坐标为Pci＝[pcxi,pcyi,1]T,i＝1,2,3,4， 四个点在世界坐 标系下的坐标为Pi＝[Xi,Yi,Zi]T， 在相机坐标系下的坐标为Pwi＝[Pwxi,Pwyi,Pwzi]T， 在像素 坐标系下的坐标为Puvi＝[ui,vi,1]T； 四个点依次连线能够组成一个平行四边形； 对于圆形 目标， 仅将对角线距离作为距离约束， 建立如下距离约束方程： 然后采用与A中平行四边形对边平行 条件一致的方式建立 4个约束方程； 对于圆形目标， 距离约束加平行约束一共6个方程， 选择距离约束的两个方程加上任意 一组描述对边平行的两个方程组成适定方程组， 解算出圆形目标四个特征点像素坐标的深 度值； C、 对于三角形目标， 已知三角形的边长为dij； 首先根据距离约束建立 三个方程式： 若三角形为等边三角形， 根据等边三角形的性质知道， 任意一顶点与对边中点的连线， 与对边垂直； 向量垂直关系在笛卡尔坐标系下的数 学表达为： 对于等边三角形目标， 距离约束加 垂直约束一共4个方程， 选择距离约束中的任意两个 方程加上描述向量垂 直关系的方程组成适定方程组， 解算出等边三角形目标三个特征点像 素坐标的深度值； 若三角形为非等边三角形， 则通过距离约束建立的三个方程式解算三个特征点的深度 值， 确定三角形目标三维空间位置 。 5.根据权利要求2所述的基于目标几何约束的多形状目标快速定位方法， 其特征在于， 所述步骤4具体实现方法为： 改进的牛顿迭代法的输入是步骤3构建的多元非线性方程组，权　利　要　求　书 2/3 页 3 CN 115187675 A 3