(19)中华人民共和国国家知识产权局 (12)发明专利申请 ▣ (10)申请公布号CN104881045A (43)申请公布日2015.09.02 (21)申请号201510337549.4 (22)申请日2015.06.17 (71)申请人中国科学院自动化研究所 地址100080北京市海淀区中关村东路95 勿 (72)发明人喻俊志孙飞虎徐德赵鹏 (74)专利代理机构北京博维知识产权代理事务 所（特殊普通合伙）11486 代理人方振昌 (51)nt.Cl. 605D1/12(2006.01) 权利要求书3页说明书9页附图4页 (54)发明名称 嵌入式视觉引导下仿生机器鱼三维追踪控制 方法 (57)摘要 本发明公开了一种嵌入式视觉引导下仿生机 角度测量 器鱼三维追踪控制方法，首先，步骤1，设置人工 定向 控制器 地标，通过嵌入式视觉系统自主识别人工地标，结 佰高被 开关 合人工地标的尺寸信息，获取目标所在的三维位 模 定深 控制器 置信息：步骤2，获取所述目标所在三维位置信息 的深度，利用基于模糊滑模的控制方法，引导仿生 深度量 机器鱼游动到目标深度并保持在目标深度游动： 视觉翡最 步骤3，所述仿生机器鱼在目标深度保持游动时， 通过多阶段的定向控制，保证仿生机器鱼对目标 的追踪。本发明在机器鱼运动机动灵活的基础上 实现了准确的控制任务。 S50188501
CN104881045A 权利要求书 1/3页 1.一种嵌入式视觉引导下仿生机器鱼三维追踪控制方法，其特征在于，包括以下步 骤： 步骤1，通过仿生机器鱼的嵌入式视觉系统识别目标，并计算目标中心位置的深度信 息： 步骤2，依据深度信息，基于模糊滑模控制的方法控制仿生机器鱼游动到目标中心所在 深度，并保持仿生机器鱼在目标中心所在的深度游动： 步骤3，在仿生机器鱼保持在目标中心位置所在的深度游动时，通过仿生机器鱼的嵌入 式视觉系统计算偏航角，并采用多阶段的定向控制方法进行目标的追踪。 2.根据权利要求1所述的嵌入式视觉引导下仿生机器鱼三维追踪控制方法，其特征在 于，所述的目标为具有按照一定规律排列的色块的圆形的人工地标：所述人工地标的中心 位置通过单一颜色的识别及不同颜色的严格拓扑关系进行确定。 3.根据权利要求2所述的嵌入式视觉引导下仿生机器鱼三维追踪控制方法，其特征在 于，所述人工地标中色块为扇形区域，该扇形区域以人工地标的圆心为圆心、以人工地标的 半径为半径，色块的圆心角之和为360°，不同颜色的色块均与间隔分布。 4.根据权利要求3所述的嵌入式视觉引导下仿生机器鱼三维追踪控制方法，其特征 在于，以仿生机器鱼起始时刻嵌入式视觉系统的摄像机的镜头光心的位置作为坐标原点建 立惯性坐标系0XYZ,其中0Y轴垂直于水面向下，0Z轴为摄像机的镜头光轴沿仿生机器鱼 头部向前，OX轴由右手定则确定：当仿生机器鱼与人工地标的中心位置不在同一深度时， 仿生机器鱼通过嵌入式视觉系统采集到的人工地标为椭圆形图像，根据色块的拓扑关系 识别出所述椭圆形图像的长轴和短轴：计算长轴两个端点的水平位置坐标分别为(x1,y1)、 (x2,y2),并计算长轴的长度1； 根据小孔成像模型按照如下步骤进行人工地标的中心位置所在深度的计算： 步骤1.1，计算视觉给定的偏航角A: tan(4)=4-4 步骤1.2，计算视觉给定的俯仰角P: tan(p)=-va 其中，A为视觉给定的偏航角，P为视觉给定的俯仰角，(u,v)为人工地标的中心位置 坐标，(u,ⅴ)为嵌入式视觉系统的摄像机镜头光心的图像坐标，(f,f,)为成像平面到相机 坐标系的放大系数： 步骤1.3，计算人工地标的中心位置的深度D: d=1.R= R -x)/2+(0-)f Da=d·tan(P) 其中，d为目标离光心的距离，f为焦距，R为圆形人工地标的直径，D为目标中心所在 深度。 5.根据权利要求4所述的嵌入式视觉引导下仿生机器鱼三维追踪控制方法，其特征在 2
CN104881045A 权利要求书 2/3页 于，所述基于模糊滑模控制的方法为：每个采样周期计算人工地标的中心位置的深度D,与 当前的测量深度D.的差值作为深度误差e,依据深度误差e,计算切换函数值s、切换函数 的变化量$，将切换函数值S、切换函数的变化量5输入模糊控制器得到胸鳍攻角的变化率 ,进而通过鱼体波模型调整仿真机器鱼的下潜动作。 6.根据权利要求5所述的嵌入式视觉引导下仿生机器鱼三维追踪控制方法，其特征在 于，切换函数值s和切换函数的变化量5的计算步骤为： 步骤2.1.1，计算滑模面的输入： e(k)=D(k)-D(k) e()=S()e(-1) Ts (6)=9)-ek-1 Ts 其中，Ts是采样周期，￠为深度误差e,的一阶导数，已为深度误差e,的二阶导数： 步骤2.1.2，计算切换函数值s和切换函数的变化量： [s(k)=ce(k)+c(k)+c(k) (k)=s(k)-s(k-1) 其中，c1、c2与c3为常量调节因子。 7.根据权利要求6所述的嵌入式视觉引导下仿生机器鱼三维追踪控制方法，其特征在 于，所述模糊控制器的设计及胸鳍攻角的变化率“，的计算方法包括如下步骤： 步骤2.2.1，模糊控制器隶属度函数设计：切换函数值s、切换函数的变化量，5、胸鳍攻 角的变化率i分别使用五个、七个、七个模糊语言值，分别用模糊语言值NB、NM、NS、ZE、PS、 PM以及PB代表负大、负中、负小、零、正小、正中以及正大，隶属度函数选用正态分布的函 数： 步骤2.2.2，模糊控制器规则库设计：规则库的设计以满足s对<0的条件，且当s、5模 糊语言值均为NB或PB时，的模糊语言值与s、S的模糊语言值相同，以加快：s心变化效率； 步骤2.2.3，利用重心解模糊方法解算模糊输出： 步骤2.2.4，计算胸鳍的攻角u1(k): 4(k)=4(k-1)+4(k)T。 8.根据权利要求7所述的嵌入式视觉引导下仿生机器鱼三维追踪控制方法，其特征在 于，所述多阶段定向控制方法包括： 定向运动控制中模糊控制器的设计：分别包括隶属度设计、规则库的设计以及解模糊 方法，其中，隶属度函数采用标准三角函数：解模糊方法采用重心解模糊方法： 采用陀螺仪测量的偏航角度作为负反馈，用以保证控制系统的稳定性；偏航误差的 变化为ec,其公式如下： 3
CN104881045A 权利要求书 3/3页 ec(k)=(k)-e(k-1) Ts 9.根据权利要求1-8任一项所述的嵌入式视觉引导下仿生机器鱼三维追踪控制方法， 其特征在于，所述步骤3中的多阶段定向控制过程，包括偏航阶段和直游阶段的控制过程： 所述偏航阶段控制过程，包括如下步骤： 步骤3.1.1，设计鱼体波模型，进而从所述鱼体波模型上增强仿生机器鱼头部的稳定 性； 步骤3.1.2，基于牛顿-欧拉方法对仿生机器鱼的水动力学进行建模： 步骤3.1.3，基于水动力学模型，采用遗传算法进行参数优化以满足机器鱼的头部最小 摆动； 所述直游阶段的控制过程为：以正常游动参数控制鱼体游动，保证直游的快速性。 10.根据权利要求9所述的嵌入式视觉引导下仿生机器鱼三维追踪控制方法，其特征 在于，所述鱼体波模型为： ax）=时1）-gs =(cx+cax2)sin(kx-wt)+c c xsin(wt) 其中：y为鱼体的横向位移，x为鱼体沿头尾轴的位置，c,为线性鱼体波的振幅包络 线系数，C2为鱼体波的二次项系数，k为波数，o为鱼体波的频率，c3为比例因子： 离散化输入到各个关节的鱼体波信号为： 0=4sin(o+g)+i=1,2,,6 其中，0：表示t时刻输入到第i个舵机的角位移，A:表示第1个振荡器的振幅，9：是 第i个振荡器相对于基准谐波的相位差，i=1,2,.·,4表示尾部舵机，1=5,6代表胸鳍舵 机，并且，51=52=53=ξ4=u2,55=56=u1
CN104881045A 说明书 1/9页 嵌入式视觉引导下仿生机器鱼三维追踪控制方法 技术领域 [0001] 本发明涉及视觉控制领域，尤其是一种嵌入式视觉引导下仿生机器鱼三维追踪控 制方法。 背景技术 [oo02]水下自主追踪，是水下机器人(Autonomous underwater vehicles,AUVs)的重要 任务，具有广泛的应用前景，包括海洋资源勘探、海洋地图构建以及海洋救援等。虽然基于 视觉的追踪容易受到水中光线不均、成像质量低的影响，但是基于视觉的追踪具有成本低 廉、精度较高的优点，仍然引起广泛的关注。 [0003]基于视觉的水下追踪任务，包括构建地图的水下追踪与无地图的水下追踪。构 建地图的方法，适用于精度要求高、环境未知的场合，但是地图构建是计算复杂、资源消耗 大的方案。相关技术人员构建了一种实时的视觉SLAM(simultaneous localization and mapping)地图，克服了水下成像视野受限、成像质量不高的缺点，并且利用视觉显著性方 法实现舰船的检测；技术人员利用SC-PHD(Single cluster probability hypothesis density)滤波方法进行视觉SLAM地图的构建，为水下机器人的导航奠定了基础。无地图的 视觉追踪，适用于环境已知的场合，具有简单、高效的特点。Rodr'iguez-Telles等利用改 进的SLIC(Simple Linear Iterative Clustering)方法进行图像分割，并采用最小邻域分 类器获取目标位置，然后设计鲁棒性的控制策略指引水下机器人的运动：并计了按一定规 律分布的水下光斑以指引水下机器人的入仓。 [0004] 仿生机器鱼是自治水下机器人高机动性运动的典范。通过对真实鱼类游动的模 仿，仿生机器鱼具有高超的游动性能和高效的效率；在机器鱼上增加视觉追踪，推动了自治 水下机器人应对复杂环境、精准控制的任务要求。但是，目前仿生机器鱼上的视觉应用还处 于初级阶段。例如，在一条仿斑点箱鲀(Spotted boxfish)的机器鱼上实现基于视觉的平 面跟随：又如，在一条多控制面的仿生机器鱼上实现了平面内的视觉追踪控制：再如设计 了单关节的小型机器鱼，实现了基于视觉信息的位置预测。 发明内容 [0005]本发明的目的是设计一种嵌入式视觉引导下仿生机器鱼三维追踪控制方法，在己 知环境中实现仿生机器鱼对目标的自主识别与追踪。 [0006] 本发明采用的技术方案为：一种嵌入式视觉引导下仿生机器鱼三维追踪控制方 法，包括以下步骤： [0007] 步骤1，通过仿生机器鱼的嵌入式视觉系统识别目标，并计算目标中心位置的深度 信息： [0008] 步骤2，依据深度信息，基于模糊滑模控制的方法控制仿生机器鱼游动到目标中心 所在深度，并保持仿生机器鱼在目标中心所在的深度游动： [0009] 步骤3，在仿生机器鱼保持在目标中心位置所在的深度游动时，通过仿生机器鱼的 5
CN104881045A 说明书 2/9页 嵌入式视觉系统计算偏航角，并采用多阶段的定向控制方法进行目标的追踪。 [0010]进一步地，所述的目标为具有按照一定规律排列的色块的圆形的人工地标：所述 人工地标的中心位置通过单一颜色的识别及不同颜色的严格拓扑关系进行确定 [0011]进一步地，所述人工地标中色块为扇形区域，该扇形区域以人工地标的圆心为圆 心、以人工地标的半径为半径，色块的圆心角之和为360°，不同颜色的色块均与间隔分布。 [0012]进一步地，以仿生机器鱼起始时刻嵌入式视觉系统的摄像机的镜头光心的位置作 为坐标原点建立惯性坐标系0XYZ,其中OY轴垂直于水面向下，0z轴为摄像机的镜头光轴沿 仿生机器鱼头部向前，0X轴由右手定则确定：当仿生机器鱼与人工地标的中心位置不在同 深度时，仿生机器鱼通过嵌入式视觉系统采集到的人工地标为椭圆形图像，根据色块的 拓扑关系识别出所述椭圆形图像的长轴和短轴：计算长轴两个端点的水平位置坐标分别为 (x1,y)、(x2,y2),并计算长轴的长度1： [0013] 根据小孔成像模型按照如下步骤进行人工地标的中心位置所在深度的计算： [0014] 步骤1.1，计算视觉给定的偏航角A [0015] tan(4)=4-e f [0016] 步骤1.2，计算视觉给定的俯仰角P: [0017] tan(P)=v-va [0018] 其中，A为视觉给定的偏航角，P为视觉给定的俯仰角，(u,v)为人工地标的中心 位置坐标，(u,)为嵌入式视觉系统的摄像机镜头光心的图像坐标，(f,f,)为成像平面到 相机坐标系的放大系数： [0019] 步骤1.3，计算人工地标的中心位置的深度Da: R [0020] d= ·R= Vx-x)子/f2+0%-2)/f2 [0021] Da=d·tan(P) [0022] 其中，d为目标离光心的距离，f为焦距，R为圆形人工地标的直径，D为目标中心 所在深度。 [0023]进一步地，所述基于模糊滑模控制的方法为：每个采样周期计算人工地标的中心 位置的深度D,与当前的测量深度D.的差值作为深度误差e1,依据深度误差e计算切换函 数值s、切换函数的变化量，将切换函数值s、切换函数的变化量5输入模糊控制器得到胸 鳍攻角的变化率41，进而通过鱼体波模型调整仿真机器鱼的下潜动作。 [0024]进一步地，切换函数值s和切换函数的变化量的计算步骤为： [0025] 步骤2.1.1，计算滑模面的输入： 6
CN104881045A 说明书 3/9页 e(k)=D(k)-D.(k) [0026] )=5-s(k-1) T ξ(K)=)-g-1) Ts [0027] 其中，Ts是采样周期，乌为深度误差e的一阶导数，莒为深度误差e的二阶导数； [0028] 步骤2.1.2，计算切换函数值s和切换函数的变化量$： s(k)=ce(k)+c(k)+c(k) [0029] (k)=s(k)-s(k-I) [0030] 其中，c1、c2与c3为常量调节因子。 [0031] 进一步地，所述模糊控制器的设计及胸鳍攻角的变化率的计算方法包括如下步 骤： [0032] 步骤2.2.1，模糊控制器隶属度函数设计：切换函数值s、切换函数的变化量5、胸 鳍攻角的变化率a分别使用五个、七个、七个模糊语言值，分别用模糊语言值B、NM、NS、ZE、 PS、PM以及PB代表负大、负中、负小、零、正小、正中以及正大，隶属度函数选用正态分布的 函数： [0033] 步骤2.2.2，模糊控制器规则库设计：规则库的设计以满足s<0的条件，且当s、 s模糊语言值均为NB或PB时，4的模糊语言值与s、5的模糊语言值相同，以加快ss变化效 率； [0034] 步骤2.2.3，利用重心解模糊方法解算模糊输出； [0035] 步骤2.2.4，计算胸鳍的攻角u1(k): [0036] 4(k)=4(k-1)+4(k)。 [0037] 进一步地，所述多阶段定向控制方法包括： [0038] 定向运动控制中模糊控制器的设计：分别包括隶属度设计、规则库的设计以及解 模糊方法，其中，隶属度函数采用标准三角函数：解模糊方法采用重心解模糊方法： [0039] 采用陀螺仪测量的偏航角度作为负反馈，用以保证控制系统的稳定性：偏航误差 e的变化为ec,其公式如下： [0040] ec(k)=0(k)-e:(k-1) Ts 0 [0041] 进一步地，所述步骤3中的多阶段定向控制过程，包括偏航阶段和直游阶段的控 制过程； [0042] 所述偏航阶段控制过程，包括如下步骤： [0043] 步骤3.1.1，设计鱼体波模型，进而从所述鱼体波模型上增强仿生机器鱼头部的稳 定性： [0044] 步骤3.1.2，基于牛顿-欧拉方法对仿生机器鱼的水动力学进行建模： 7
CN104881045A 说 明书 4/9页 [0045] 步骤3.1.3，基于水动力学模型，采用遗传算法进行参数优化以满足仿生机器鱼的 头部最小摆动： [0046] 所述直游阶段的控制过程为：以正常游动参数控制鱼体游动，保证直游的快速性。 [0047] 进一步地，所述鱼体波模型为： 元aty(x,）=o(x,t）-cx9 (x,t) x-0 [0048] =(cx+czx2)sin(kx-wt)+c c x sin(wt) [0049] 其中：y为鱼体的横向位移，x为鱼体沿头尾轴的位置，c1为线性鱼体波的振幅 包络线系数，c2为鱼体波的二次项系数，k为波数，o为鱼体波的频率，c3为比例因子： [0050] 离散化输入到各个关节的鱼体波信号为： [0051] 0=A,sin(+g)+5i=1,2,,6 [0052] 其中，表示t时刻输入到第i个舵机的角位移，A,表示第个振荡器的振幅， 9:是第i个振荡器相对于基准谐波的相位差，i=1,2,.·,4表示尾部舵机，i=5,6代表 胸鳍舵机，并且，号，=52=53=54=u2,55=56=U1。 [0053]采用上述技术方案，产生的技术效果有：本发明针对基于嵌入式视觉的仿生机器 鱼的三维追踪控制，在机器鱼运动机动灵活的基础上实现准确的控制任务。提出了基于视 觉的三维追踪控制系统，将三维追踪划分为定深控制与定向控制以尽量减小控制量的耦 合，保证控制的平滑稳定。利用了模糊滑模控方法，实现无精确模型指导、干扰较大环境 下的深度控制：多阶段的定向控制中，将连续的基于图像伺服控制转化为多阶段的偏航反 馈控制，降低控制的复杂度。并且，通过对深度控制的评估控制定向控制的引入，实现深度 控制与定向控制的切换。 附图说明 [0054] 图1是基于嵌入式视觉的仿生机器鱼三维追踪示意图： [0055] 图2是基于嵌入式视觉的三维追踪的控制系统框图； [0056] 图3是三维定位的人工地标示意图； [0057] 图4是基于视觉的三维定位示意图： [0058] 图5是基于模糊滑模的定深运动控制框图： [0059] 图6a是模糊控制的输入s的隶属度函数； [0060] 图6b是模糊控制的输入的隶属度函数： [0061] 图6c是模糊控制的输出41的隶属度函数： [0062] 图7是基于模糊的定向运动控制框图。 具体实施方式 [0063]为使本发明的目的、技术方案和优点更加清楚明白，以下结合具体实施例，并参照 附图，对本发明进一步详细说明。 [0064] 如图1所示，以仿生机器鱼起始时刻的位置作为坐标原点建立惯性坐标系OXYZ, 8
CN104881045A 说明书 5/9页 其中0Y轴垂直于水面向下，0Z轴沿着仿生机器鱼起始时刻的尾头轴向前，0X轴由右手定则 确定。整个追踪控制分为视觉定位、定深控制、定向控制。首先，通过嵌入式视觉系统计算 出目标（图示为Entrance)的三维信息：然后，在视觉指引下，仿生机器鱼游动向目标所在 的深度（到达A点）并保持：最后，机器鱼调整航向完成定向控制（到达B点），使得鱼体 对准入口目标中心，快速游动向目标处。 [0065]仿生机器鱼通过下潜运动到达目标中心所在深度，需要胸鳍产生攻角，并且鱼体 达到一定游速才能实现。因此，在定深控制的过程中，无法采集到稳定的图像，实时的图像 数据反馈是不可靠的。为了解决这一问题，采用视觉给定深度信息，利用压力传感器反馈深 度信息，在这个过程中不进行基于视觉的深度反馈。另一方面，到达指定深度后，机器鱼调 整航向完成定向控制需要较高的精度，这需要基于视觉的多阶段航向给定：同时，需要兼顾 机器鱼游动向入口的快速性。但是，保证采集图像的稳定与机器鱼游动的灵活快速是矛盾 的。因此，均衡控制精度和运动的灵活快速需要多阶段定向控制的支持。 [0066]基于以上的分析设计了本发明基于嵌入式视觉的三维追踪的控制系统，如图2所 示，图中，设计了定向控制的启动开关，开关的闭合受定深控制器的控制。整个控制过程，定 深控制持续执行，以保持机器鱼在入口中心所在深度：定向控制则受开关控制在深度保持 较好的情况下进入。 [0067]图中，e是深度误差，可以表示为式(1)：同样，偏航误差e2可以表示为式(2)。 [0068] e(k)=Aa(k)-A.(k) (1) [0069]e2(k)=Da(k)-D.(k) (2) [0070] 其中，A(k)、Da(k)分别表示k时刻视觉给定的偏航角与深度，A。(k)、D。(k)分别表 示k时刻测量得到偏航角与深度。另外，u1、u2分别表示定深控制器与定向控制器的输出。 [007]本发明中机器鱼的运动模型选用鱼体波模型。传统的鱼体波模型基本形式如式 (3)所示： [0072]yhody (x,t)=(c:x+c2x2)sin(kx-0t) (3) [0073] 其中，y,表示鱼体的横向位移，X表示鱼体沿头尾轴的位置，c代表线性鱼体波 的振幅包络线系数，℃代表了鱼体波的二次项系数，k是波数，⊙是鱼体波的频率，t代表 时间。 [0074] 对传统的鱼体波模型进行改进，以保证头部的平稳性为目的，改进后的鱼体波模 型如式(4)所示： (4) [0075] c小=儿s（-Gr =(cx+cx2)sin(kx-wt)+c;cxsin(wt) [0076] 其中，℃是比例因子，用于调整鱼体波包络线的幅度，使其满足机器鱼游动时的振 幅特点。其它参数同式(3)。离散化输入到各个关节的鱼体波信号是一种简谐运动的振荡 信号，可以表示如式（⑤）。 [0077] 0=4sin(ot+9)+5i=l,2,,6 (5) [0078] 其中，0：表示t时刻输入到第i个舵机的角位移：A,表示第i个振荡器的振幅： 9
CN104881045A 说明书 6/9页 9是第i个振荡器相对于基准谐波的相位差，i=1,2,..,4表示尾部舵机：i=5,6代表胸 鳍舵机，并且，51=52=ξ3=54=u2:55=号6=u1 [0079] 本发明所提出的嵌入式视觉引导下仿生机器鱼三维追踪控制方法，具体分为基于 嵌入式视觉的自主识别与定位、基于模糊滑模的定深运动控制以及多阶段的定向运动控 制。 [0080] 所述追踪控制方法具体包括以下步骤： [0081] 步骤1，通过仿生机器鱼的嵌入式视觉系统识别目标，并计算目标中心位置的深度 信息； [0082] 水下目标的识别与连续定位，是具有挑战的任务：在嵌入式系统资源有限的前提 下，基于人工地标的视觉识别与定位是可靠、有效的方法。在保证实时性的同时，为提高算 法的鲁棒性，设计了色块按照一定规律排列的人工地标。并且，结合地标的己知尺寸信息， 可以解算出目标的三维位置信息。 [0083]如图3所示，本发明的目标为具有按照一定规律排列的色块的圆形的人工地标： 所述人工地标的中心位置通过单一颜色的识别及不同颜色的严格拓扑关系进行确定。图 中，R区域代表红色区域，B区域代表蓝色区域。本发明的人工地标中色块为扇形区域，该扇 形区域以人工地标的圆心为圆心、以人工地标的半径为半径，色块的圆心角之和为360°， 不同颜色的色块均与间隔分布。对于单一颜色的识别，关注于其主分量，使其满足宽松的阈 值识别（弱识别）：通过不同颜色的严格拓扑关系（强识别），最终确定人工地标的中心位 置。 [0084]以仿生机器鱼起始时刻嵌入式视觉系统的摄像机的镜头光心的位置作为坐标原 点建立惯性坐标系OXYZ,其中OY轴垂直于水面向下，0Z轴为摄像机的镜头光轴沿仿生机 器鱼头部向前，0X轴由右手定则确定：当仿生机器鱼与人工地标的中心位置不在同一深度 时，仿生机器鱼通过嵌入式视觉系统采集到的人工地标为椭圆形图像，根据几何关系，椭圆 的长轴与短轴相互垂直且相交于确定的中心。因此，分别向垂直中心位置的方向出发，根据 色块的拓扑关系识别出所述椭圆形图像的长轴和短轴：计算长轴两个端点的水平位置坐标 分别为(x,y)、(x,y2),并计算长轴的长度1： [0085] 根据小孔成像模型按照如下步骤进行人工地标的中心位置所在深度的计算： [0086] 图4描述了小孔成像的进行三维定位的基本原理。其中摄像机坐标系OXYZ的坐 标原点设为摄像机的镜头光心，Z轴为光轴。图像坐标系0，UV的坐标原点位于光轴中心。 [0087] 忽略摄像头的镜头畸变，摄像机内参数矩阵可以表述为： f [0088] 0 f (6) 0 0 [0089] 其中，(u,V)是摄像机镜头光心的图像坐标：（任，f,)表示成像平面到相机坐标系 的放大系数，可以由式(7)表示： [0090] f=fls (7) f,=f/s, [0091] 其中，f代表焦距，(s,S,)分别表示表示图像中x方向、y方向上单位象素的长度。 10