首页 > 专利信息

基于三线阵CCD 的室内导航方法研究

第45卷第4期

燕山大学学报

Vol.45No.4

2021年7月

Journal of Yanshan University

July 2021

㊀㊀文章编号:1007-791X (2021)04-0312-09

基于三线阵CCD 的室内导航方法研究

王㊀闯1,2，屠晓伟1,∗罗口袜

(1.上海大学机电工程与自动化学院，上海200444;

2.许昌学院信息工程学院，河南许昌461000)

㊀㊀收稿日期:2020-03-10㊀㊀责任编辑：温茂森㊀㊀基金项目：国家自然科学基金资助项目(51775325,61903241)；国家商用飞机制造工程技术研究中心创新基金资助项目(COMAC-SFGS-2017-36741)

㊀㊀作者简介：王闯(1983-)，男，河南许昌人，博士研究生，主要研究方向为室内定位㊁无线传感器网络时间同步;∗通信作者：屠晓伟(1959-)，男，加拿大人，博士，教授，博士生导师，主要研究方向为空间定位㊁机器视觉和图像处理,Email:tuxiaowei@shu.edu㊂

摘㊀要：针对目前室内移动导航定位精度低㊁累积误差大㊁并且经常需要多个传感器分别估测移动目标位置和姿态的问题，提出了一种新型的位置姿态传感器用于室内导航㊂该传感器由3个线阵CCD 和3个柱面透镜组成，并能够估测其视域范围内布置在天花板上的红外LED 锚节点的三维坐标㊂在依次估测到至少3个锚节点的三维坐标之后，使用Rodrigues 矩阵坐标转换算法估测出传感器在室内的位置和姿态㊂同时，移动目标位置和姿态的估测是在接收端，因此该系统不存在轨迹暴露的问题㊂本文提出的方法部署成本低，易于室内大范围扩展使用㊂最后，仿真结果表明该方法能够可靠㊁精确地进行位置和姿态的估测，为结构化室内环境下的导航提供了一种行之有效的解决方法㊂

关键词：室内定位；罗德里格矩阵；坐标转换；姿态；线阵CCD；柱面透镜

中图分类号:V249.1㊀㊀文献标识码:A㊀㊀DOI :10.3969/j.issn.1007-791X.2021.04.003

0㊀引言

室内移动智能体广泛应用在商业㊁物流㊁自动化生产㊁军事等领域㊂移动智能体的高精度位置和姿态信息是完成用户指定运行任务的基础，同时也是实现精准运动控制和导航路径规划的前提条件㊂

室内移动智能体位置和姿态的测量方法主要

分为两类：相对定位和绝对定位[1]㊂相对定位技术通过估测移动智能体相对于起始位置的方向和距离来推断出当前的位置信息㊂航迹推算法是相对定位技术中的一种经典方法，通过惯性测量单元或测量里程计的方法而实现的航迹推算法既不需要外部信息，也不需要向外发射任何信息，它能够独立工作并且连续不断地为移动智能体提供定位服务㊂然而，航迹推算法的测量误差会随着时间积累，所以该方法仅满足短时间内的状态估计，无法实现复杂环境下的长时间定位[2]㊂同时航迹

推算法还需要引入初始航向角与位置信息，在实际场景中较难实现㊂因此，移动智能体位置和姿态的估测更多地依靠绝对定位方法㊂

在过去几十年，绝对定位受到相关研究者的关注，并取得了重大进展㊂绝对定位需要测量多个已知位置的参考点来获取移动智能体的位置信息㊂室外绝对定位技术的典型代表是全球定位系统(Global Positioning System,GPS)，自从GPS 投

入使用后，室外定位的应用越来越广泛㊂然而，在城市地区和建筑物内部，建筑物的墙壁和屋顶阻挡了GPS 信号传播，使得GPS 系统不能在室内正常工作，因此室内绝对定位方法的研究进入了一个快速发展的时期[3]㊂

在室内绝对定位技术中，基于锚节点的测量

方法为室内移动智能体的定位和导航提供了多种选择，例如使用可见光[4]㊁超声波[5]㊁Wi-Fi [6]㊁Ultra

Wide Band (UWB)[7]等无线定位技术㊂然而，这些定位技术往往受到多径效应等因素的干扰，而

第4期王㊀闯等㊀基于三线阵CCD的室内导航方法研究313

㊀

且绝大多数都不能提供三维的位置和姿态服务,

在室内移动智能体的导航应用中存在很大的

缺陷㊂

最近几十年来，光电测量技术发展迅速，相关

研究者提出了很多可行的方法㊂激光雷达主要采

用红外激光束以径向视野扫描周围环境，它以其

测量精度高㊁灵活性强㊁实时性好等特点，已在多

种室内环境场合中广泛使用[8]㊂但是在某些场合下，如在飞机大部件的高精度装配和对接程序中,

激光雷达技术很难同时获得移动平台的位置和方

向[9]㊂文献[10]采用基于激光的光电扫描方式测量锚节点的三维坐标，并使用坐标转换算法实现了室内位置和姿态的测量㊂但该方法需要使用复杂的旋转激光扫描仪器测量室内锚节点的三维坐标，并且激光设备价格昂贵，不便于在室内多个移动智能体情况下大规模使用㊂

基于电荷耦合器件(CCD)或CMOS的视觉传

感器采用非接触的测量方法，可以在不干扰系统

的情况下估测出室内移动物体运动参数㊂视觉传

感器具有结构简单㊁安装方式多元化㊁探测信息丰

富㊁自身不受外界电磁干扰等优点㊂文献[11]使

用面阵CCD相机来估测室内位置和姿态，但该方

案需要分别考虑二维或三维工作情况，从而决定

锚节点的使用个数㊂文献[12]使用面阵相机观测

矩形约束信标，然后获得机器人位置和姿态㊂然

而，该系统要求相机的光轴总是指向目标中心点,

这使得该方法复杂而笨拙，不适合室内较大工作

范围的使用㊂目前有一些商业现成的设备通过计

算机视觉的方法来测量目标的位置和姿态信

息[13]，但是这种设备不能提供360ʎ方向的测量，不适合不断变化的室内背景，并且这些设备价格昂贵，更大的测量空间需要更多的相机测量设备，导致整个系统成本开销的增加㊂

现有的室内位置和姿态的视觉测量方法主要

采用面阵CCD或CMOS，较少采用线阵CCD来实

现室内移动智能体的位置和姿态的测量㊂文献[14]使用线阵CCD测量室内移动物体的位置和姿态，然而该方法需要使用多套线阵CCD测量设备，并且只能在较小的室内空间测量位置和姿态㊂文献[15]提出了一种3D运动跟踪系统，该系统使用多个线性光学传感器阵列，辅以惯性测量单元，在位置和姿态跟踪方面取得了良好的性能㊂但是该方法需要使用线阵CCD和惯性测量单元分别测量位置和姿态，并且也只能在小范围的室内空间内实现位置和姿态测量㊂

针对移动智能体在室内导航过程中的位置和姿态问题，本文使用3个线阵CCD和3个柱面透镜设计了一种新型的室内位置姿态传感器，通过三平面相交于空间一点的方法，即可在该传感器上建立传感器坐标系㊂通过在室内屋顶上布置多个红外LED锚节点，根据位置姿态传感器在室内移动的位置，测量传感器视域范围内锚节点的三维坐标值，获取到至少3个锚节点在传感器坐标系下的三维坐标值后，通过罗德里格矩阵坐标转换算法，即可估测出移动智能体在室内的位置和姿态信息㊂该方法简单实用，精度高，成本低，适合于室内移动智能体的大规模应用㊂

1㊀测量原理

线阵CCD在计算机视觉测量中应用广泛，与面阵CCD或CMOS相比，线阵CCD具有高分辨率和快速的帧速率，在精确坐标测量和动态位置跟踪方面具有突出的优势[16]㊂

1.1㊀系统组成和工作原理

一维线阵相机由两个部分组成，一个线阵CCD和一个柱面透镜㊂柱面透镜用于实现从三维空间到一维成像的投影映射㊂根据光学的基本原理，来自柱面透镜一侧物点的光线将在另一侧形成线性图像[17]，如图1所示

㊂

图1㊀一维成像单元投影模型Fig.1㊀Projection model of the one-dimentional imaging unit ㊀㊀在图1中，线阵CCD和柱面透镜构成一维成像单元(One Dimensional Imaging Unit,ODIU)，来自LED发光点的光线通过柱面透镜将聚焦在焦

314㊀燕山大学学报2021

平面上㊂为了检测由于LED 发光点移动而引起的变化，将线阵CCD 图像传感器放置在柱面透镜的焦平面㊂线阵CCD 垂直于柱面透镜的光轴放置，用来检测和标记LED 移动时投影线的变化㊂线阵CCD 的中心与柱面透镜光轴的中心上下对照，它们之间的距离是柱面透镜的焦距㊂LED 的投影线垂直于线阵CCD 并在CCD 上形成脉冲峰值点，经过图像处理后可计算出LED 的一维图像坐标㊂如果线阵CCD 上的成像点和柱面透镜的光轴是已知的，则可以计算出包含LED 位置的平面㊂

一条直线和一个不在这条线上的点可以定义一个平面㊂为了在传感器坐标系下测量LED 锚节点的三维坐标，至少需要3个ODIU 来存储LED 的3个一维图像位置坐标，以重建传感器坐标系下LED 的坐标值㊂如图2所示,3个ODIU 的线阵CCD 在同一水平面布置,LED 的三维坐标可以通过3个平面的相交点来估测㊂当LED 位于传感器的视域范围时候,LED 穿过相应的柱面透镜并形成分别垂直于3个线阵CCD 的3条投影线，投影线在相应的焦平面上与3个线阵CCD 相交形成3个投影点，以此来计算投影线的位置信息㊂如果确定了每个ODIU 中的投影线的交点位置，则就确定了3个投影面，这3个投影面的相交点就是LED 锚节点在传感器坐标系的位置㊂因此，在传感器坐标系下的LED 锚节点的三维坐标可以由3个虚拟平面唯一地解析

㊂

图2㊀位置姿态传感器原理图

Fig.2㊀Schematic of the position and attitude sensor

㊀㊀同时，将3个线阵CCD 感光区域的延长线交点设置为位置姿态传感器坐标系的原点，每个线

阵CCD 的感光区域都位于传感器坐标系的XY 平面上㊂其中线阵CCD3位于Y 轴的负半轴上，而CCD1和CCD2分别位于第一象限和第二象限㊂

1.2㊀位置姿态传感器视域

图2中的位置姿态传感器只能测量其视域范围内LED 锚节点的三维坐标㊂同时在室内导航中，传感器的视域对于天花板上的锚节点布局非常重要，传感器坐标系在XY 平面上的投影图如图

3所示

㊂

图3㊀传感器在XY 平面投影图

Fig.3㊀Projection drawing of the sensor on XY plane

㊀㊀在图3中，线阵CCD 光敏元件的感光长度L

都等于30mm㊂线阵CCD3位于Y 轴的负半轴上，将CCD3顺时针旋转120ʎ移动到CCD2的位置，同时将CCD3逆时针旋转120ʎ移动到CCD1的位置㊂CCD1_In,CCD2_In 和CCD3_In 是线阵CCD 靠近坐标原点的内边缘，它们到原点的距离d 都等于60mm;CCD1_Out,CCD2_Out,CCD3_Out 是线阵

CCD 的外边缘，它们到原点的距离都等于90mm㊂a 1和b 1,a 2和b 2,a 3和b 3分别是3个柱面透镜的光轴在XY 平面上的投影线，柱面透镜的焦距为50mm，仿真参数如表1所示㊂

㊀㊀根据表1，在3个线阵CCD 的光敏区域每隔

1mm 选择投影点，由于一般室内距离大部分在

6m 以内，因此只考虑室内空间的Z 坐标值在[0,

6000mm]范围的情况㊂图4是根据表1中的参

数仿真得到的传感器的视域图㊂根据图4中的仿真结果，传感器最近的测量距离为300mm，同时

可测量视域随着距离的增加而变大㊂

第4期王㊀闯等㊀基于三线阵CCD的室内导航方法研究315

㊀

表1㊀仿真参数

Tab.1㊀Simulation parameters

设计参数数值

CCD1_In/mm[51.96,30.00,0.00]

CCD1_Out/mm[77.94,45.00,0.00]

CCD2_In/mm[-51.96,30.00,0.00]

CCD2_Out/mm[-77.94,45.00,0.00]

CCD3_In/mm[0.00,-60.00,0.00]

CCD3_Out/mm[0.00,-90.00,0.00]

a1/mm[49.95,63.48,50.00]

b1/mm[79.95,11.52,50.00]

a2/mm[-79.95,11.52,50.00]

b2/mm[-49.95,63.48,50.00]

a3/mm[30.00,-75.00,50.00]

b3/mm[-30.00,-75.00,50.00]

焦距/mm50.00

距离d/mm60.00

感光区域长度L/mm30.00

线阵CCD夹角/(ʎ)120

图4㊀传感器视域图

Fig.4㊀The vision field of the sensor

2㊀位置和姿态测量算法

在室内位置和姿态测量系统中，有两个不同的坐标系统㊂一个是室内坐标系，它的坐标原点设在屋内的墙角处㊂另一个是位置姿态传感器坐标系，它的坐标原点设定在传感器上，如图2所示㊂2.1㊀数学模型

设定3个锚节点A1,A2,A3固定在天花板上，它们在室内坐标系下的坐标分别为A1(X1,Y1, Z1),A2(X2,Y2,Z2)和A3(X3,Y3,Z3)㊂同时设定这3个锚节点在传感器坐标系下的坐标值分别为C1(x c1,y c1,z c1),C2(x c2,y c2,z c2)和C2(x c2,y c2,z c2)，根据锚节点在室内坐标系和传感器坐标系下的三维坐标值，列出坐标转换方程来实现从传感器坐标系到室内坐标系的转换，即

+T,(1)

式(1)建立了锚节点在两个坐标系之间的关系，其中[X㊀Y㊀Z]T表示锚节点在室内坐标系下的坐标值,[x㊀y㊀z]T表示锚节点在传感器坐标系下的坐标值,R是3ˑ3旋转矩阵,T=[x0㊀y0㊀z0]T 是平移向量㊂在坐标转换方程中，平移向量T表示传感器坐标系的原点在室内坐标系的位置，旋转矩阵R里面含有传感器坐标系相对于室内坐标系的旋转角度，这些旋转角就是要求的姿态角㊂假设传感器坐标系的x轴,y轴和z轴在室内坐标系下的旋转角分别是α㊁β和γ㊂设定α为俯仰角,β为滚转角,γ为偏航角㊂同时定义顺时针方向的旋转角为正，逆时针方向的旋转角为负㊂根据旋转角，可以得到绕x轴,y轴和z轴转动的旋转矩阵为

x(α)=

100

0cosαsinα

第一联动

0-sinαcosα

y(β)=

cosβ0-sinβ

010

sinβ0cosβ

z(γ)=

cosγsinγ0

-sinγcosγ0

001

定义旋转矩阵R=R y(β)R x(α)R z(γ)，其中α= arcsin(R[2,3]),β=-arctan(R[1,3]/R[3,3]),γ=-arctan(R[2,1]/R[2,2]),R的计算结果为

总线上的音频设备

机房环控R=cosβcosγ-sinαsinβsinγcosβsinγ+sinαsinβcosγ-cosαsinβ-cosαsinγcosαcosγsinαsinβcosγ+cosβsinαsinγsinβsinγ-cosβsinαcosγcosαcosβé

㊂

316㊀燕山大学学报2021

2.2㊀罗德里格矩阵坐标转换算法

为了求解式(1)中T 和R ，至少需要3个锚节点㊂使用罗德里格矩阵坐标转换算法来求解R 和T ㊂在式(1)中，根据转换算法，首先要计算R ，然后计算T [18]㊂

设反对称矩阵S 为

S =0-c -b c

0-a b

a 0éëêêêù

úúú,(2)

其中,a ㊁b ㊁c 是独立参数,R 由反对称矩阵S 和3ˑ3单位矩阵I 组成，即

R =(I -S )-1(I +S )㊂

(3)

由式(1)可以列出3个方程，第j 个锚节点的

方程分别减去第i 个锚节点的方程，可以消除平移向量，然后得到

X j -X i Y j -Y i Z j -Z i éëêêêêùûúúúú=R x j -x i y j -y i z j -z i éëêêêêùû

úúúú,(4)

把式(3)代入(4)，可得到

(I -S )X j -X i Y j -Y i Z j

-Z i

éëêêêêùûúúúú=(I +S )x j -x i y j -y i z j

-z i

éëêêêêùû

úúú,

(5)

将式(2)代入式(5)，然后提取a ,b 和c 并表示成

0-z ij -Z ij -y ij -Y ij -z ij -Z ij 0x ij +X ij y ij +Y ij x ij +X ij 0é

ëêêêêùûúúúúa b c éëêêêùû

úúú=X ij -x ij Y ij -y ij Z ij -z ij éëêêêêùû

úúú,(6)其中,X ij =X j -X i ,Y ij =Y j -Y i ,Z ij =Z j -Z i ,x ij =x j -x i ,y ij =y j -y i ,z ij =z j -z i ㊂

在式(6)中，左边的系数矩阵是一个奇异矩阵，在只有两个独立方程的情况下不能求解a ㊁b 和c 这3个参数，再使用锚节点k 和锚节点i 可以得到

0-z ik -Z ik -y ik -Y ik -z ik -Z ik 0

x ik +X ik y ik +Y ik x ik +X ik

0éë

êê

êùû

úúúúa b c éëêêêù

ûú

úú=

X ik -x ik Y ik -y ik Z ik -z ik éëêêêêùû

úúúú,(7)

然后结合式(6)和(7)，得到0-z ij -Z ij -y ij -Y ij -z ij -Z ij 0

x ij +X ij y ij +Y ij x ij +X ij

00-z ik -Z ik -y ik -Y ik -z ik

-Z

0x ik +X ik y ik +Y ik x ik +X ik

0éë

êê

êêêù

úúúúúúú㊃a b c éëêêêùûú

úú

X ij -x ij Y ij -y ij Z ij -z ij X ik -x ik Y ik -y ik

Z ik -z ik éëêêêêêêêêêù

û丝锥磨床

úúúú

úúú

úú㊂

(8)由式(8)可以计算出a ㊁b ㊁c 三个参数，再根据

式(2)和(3)可求出旋转矩阵R ，然后把R 代入式

(1)可以计算出平移向量T ㊂经过上面的步骤，就可以求出平移向量T 和旋转矩阵R ，这两个参数分别包含有位置姿态传感器在室内坐标系下的位置和姿态角㊂

2.3㊀多个公共点情况的坐标转换算法

如果位置姿态传感器在其视域范围内检测到

3个以上的锚节点(n >3)，由多个锚节点估测旋转矩阵和平移矩阵的问题就可以转换为最小二乘问题㊂当两个坐标系中存在有多个公共点，根据式(8)，列出误差方程为

V 3(n -1)ˑ1=A 3(n -1)ˑ3㊃X 3ˑ1-L 3(n -1)ˑ1,(9)

式中,X 3ˑ1=[a b c ]T ，矩阵A 3(n -1)ˑ3和L 3(n -1)ˑ1为

A 3(n -1)ˑ3=0-z ij -Z ij

-y ij -Y ij -z ij -Z ij 0x ij +X ij y ij +Y ij x ij +X ij 0︙

︙︙0-z in -Z in -y in -Y in -z in -Z in 0x in +X in y in +Y in

x in +X in 0éëê

êê

温度远程监控êêê

êêêù

úú

úú,L 3(n -1)ˑ1=[X ij -x ij ㊀

Y ij -y ij ㊀Z ij -z ij ㊀

X in -x in ㊀Y in -y in ㊀Z in -z in ]T ㊂

根据最小二乘原理，可得到式(9)的最优解为X =(A T ㊃A )-1㊃A T ㊃L ㊂

根据最优解，即可求出在多个公共点情况下,

罗德里格矩阵坐标转换算法中的3个独立参数a ㊁

本文发布于:2024-09-22 09:29:25，感谢您对本站的认可！

本文链接：https://www.17tex.com/tex/2/298991.html

上一篇：omnic标峰

下一篇：机器人智能化系统中的定位技术研究

标签：位置传感器姿态测量移动坐标坐标系方法

留言与评论（共有 0 条评论）