一种基于双目技术的室内可见光自定位系统与方法与流程
本发明涉及一种基于双目技术的室内可见光自定位系统与方法，属于图像处理和可见光定位技术领域。
背景技术：
随着各种综合体类建筑(如大型购物中心、大型交通中转站等)的出现，人们活动空间越来越庞大和复杂，对定位和导航需求日趋强烈，同时精准营销、智能仓储、机器人、疗养护理等行业也需要利用定位技术获取特定对象的室内位置信息，目前应用最为广泛的室外无线电定位系统是gps或北斗全球定位系统，它是通过在移动终端内置卫星的位置信号接收模块，利用无线电信号实现室外的高精度定位。但在人们活动较为频繁的家庭、办公室、大型商场等室内环境以及隧道、地下停车场等特殊环境，由于对空间无线电信号的阻挡及屏蔽效应导致gps等传统卫星定位系统难以适用，据统计，现代社会人们80％的时间是在室内活动，并且随着城市化的发展，各类大型建筑物越来越多，因此室内定位需求日益迫切。
现有的针对该种室内定位需求的定位装置或方法有较多公开内容，例如：中国发明专利“双目非标定空间定位方法”(cn102622767b)是通过背景差分法获取运动目标中心点的位置数据；通过摄像机成像原理，计算出运动目标在场景中相对于当前分析画面的拍摄光轴的角度、方向数据，得出运动点与拍摄点的坐标，其不足之处是：使用背景差分法获取运动目标中心点的位置数据，易受到光线变化、环境变化、背景变化、场景中物体运动的干扰，且用运动物体相对于拍摄光轴角度、方向数据来计算其坐标，并不能实现对自身的定位；发明专利“基于fpga实现的双目视觉定位装置”(cn201310651555)是通过两个相机采集图像信息，利用两片sdram分别对两幅图像进行缓存，最后在lcd上实时显示两幅图像。在fpga内部实现两幅图像的并行处理，经过图像滤波、特征提取等数字图像处理方法获得特定目标图像位置信息后，利用bp神经网络标定方法确定目标物体的笛卡尔空间位置信息，虽然定位装置利用bp神经网络标定方法实现二维信息到三维信息之间的非线性变换，无需确定摄像机的内、外参数，同时可降低摄像机镜头各种畸变及外界噪声的影响，但其实现的仅是对目标物体的定位，并不能对自身进行定位；发明专利“基于logo的双目视觉室内定位方法”(cn106228538a)通过建立logo图像采集的视觉地图数据库等五个步骤可实现定位，但该种方法存在离线阶段需要在室内长走廊中布置较多的logo图像，并在光线较好时对logo进行拍照，因此实施难度大，另一方面：摄像机距logo的距离是3.5m时，定位误差将达到0.22m，可见其定位误差较大；发明专利“基于双目相机的空间定位方法及装置”(cn107705333a)只能获得对自身的定位，不能得到其姿态，且该专利未写出详细的定位算法；发明专利“一种鱼眼双目视觉立体匹配空间定位方法”(cn107909543a)是用鱼眼相机采集图像，并直接对未经校正的图像进行立体匹配，但由于实现室内定位需要的采集的图像范围不大，因此对相机的视角要求不高，普通的相机即可满足要求，同样还存在只能对目标物体的定位，而不是自定位问题；发明专利“一种基于uwb和双目视觉的导航定位方法”(cn108012325a)通过通过uwb基站及定位标签来确定叉车的位置，所采用的双目技术仅用于确定得到货架托盘相对叉车的距离信息和姿态信息，用以调整叉车使其正对货架托盘，没有通过固定的led和双目技术确定设备的位置；发明专利“一种基于双目视觉的机械臂目标定位抓取方法”(cn107767423a)是采用双目技术获取目标物体的三维空间坐标及机械臂末端的状态信息，无法实现对安装有双目相机机械臂的定位，还需要判断目标物体所在象限；发明专利“一种室内可见光定位系统”(cn107703485a)的核心为通过采集led的虚拟坐标与其真实坐标值进行转换，以确定相机位于真实坐标系下的坐标，从而实现相机的真实定位；发明专利“一种使用相机的室内可见光异步定位方法”(cn105306141a)通过led异步发送信号，利用智能终端的相机进行室内可见光定位。
综上所述，现有的室内可见光定位普遍存在如下两方面的问题：
⑴定位效率不高，无法实时更新在室内的定位：现有的可见光系统和算法设计过于复杂，无法快速地得到设备的定位。
⑵获得信息受限，无法获得设备的姿态：这些室内可见光定位方法只能设备的位置信息，无法判断设备是否平衡，无法得到设备的状态，获得的信息较单一，对可见光的资源利用率低。
⑶通用性不强，需要布设多个led灯：这些基于室内可见光实现的定位方法绝大部分需要专门的接收模块接收led发送的号，在具体实施中设备复杂、可拓展性差，商业应用性不强。
基于此，一种具有节能降耗、健康安全、布设简单、成本低廉、无电磁干扰等突出优点的室内可见光自定位系统与方法成为必需，它不仅能实现设备的自定位，还能获得设备的实时姿态，因此在室内无线定位领域具有良好的应用前景。
技术实现要素：
本发明针对背景技术所述问题，设计一种基于双目技术的室内可见光自定位系统与方法，可实现安装有双目相机的设备在室内、车内等的快速定位，并获得设备的倾角。基于最小均方算法的定位由于其计算复杂度低，计算速度快，因此可以对设备进行实时定位。本发明所述基于双目技术的室内可见光定位系统与方法具有成本低、定位精度高、计算规则明确等的综合优势。
为达到上述目的，本发明采用技术方案：
一种基于双目技术的室内可见光自定位系统，包括相互协同作业的图像分析系统和自定位系统；所述图像分析系统包括：信标模块、图像采集模块、相机自标定模块、图像处理模块、立体匹配模块；所述自定位系统包括：坐标及倾角计算模块；其特征在于：
所述信标模块包括三个固定在上方的led灯，三个led灯分布在非同一直线上，且各led灯在空间参考坐标系的坐标已知，各led灯分别工作在设定的频率中；
所述图像采集模块包括安装在手持设备上由左右二个相机组成的双目相机，所述双目相机用于从两个不同角度同时获得同一物体的图像信息；
所述相机自标定模块用于获得双目相机的内外参数，包括：图像中心、焦距f、镜头畸变和相机图像坐标系的旋转矩阵和平移矩阵；
所述图像处理模块用于降低噪声对图像质量的影响，增加图像对比度以增强边缘特性，提高后续两幅图像之间立体匹配的精确度；
所述立体匹配模块用于存储从各led灯的ook(on-offkeying)编码到坐标的映射关系，通过解码图像中各led灯的编码信息，找到空间内各led灯在两幅图像中的匹配点，并获得左右两幅图像上匹配点在图像坐标系中的坐标，设定所述坐标分别为(a,b)和(a',b')；
所述坐标及倾角计算模块用于对获得的坐标进行处理，得到左右相机在三维空间坐标系的坐标，并计算手持设备的倾角；
进一步地，所述双目相机为两个内外参数相同的cmos相机，双目相机在拍照过程中对图像像素进行逐行扫描，从而识别并提取图像所中包含的信息。
进一步地，所述坐标及倾角计算模块在工作中具体实现过程包括以下步骤：
第1步，图像采集：
在设定空间内固定安装三个坐标不同且已知具体位置的led灯，分别为led1、led2和led3，各led灯在空间直角参考坐标系内的坐标分别为l1(m1,n1,q1)、l2(m2,n2,q2)和l3(m3,n3,q3)；以ook编码i1、i2、i3对led1、led2和led3进行调制，再对一定的载波频率f0对编码后的led进行调制，使得led的发射频率大于1khz以保证其变化无法被人眼察觉，并可以有效地保护视力。若cmos相机的扫描频率为fs，则led的载波频率f0为fs/n(n为任意正整数)，同时将led的发射频率与坐标之间的映射关系存储到立体匹配模块中。在待定位设备上安装两个内外参数相同的相机，使其构成一对双目相机。相机在同一时刻从不同角度拍摄天花板，得到两张图像分别为左图像和右图像；
第2步，获得相机内外参数和匹配点坐标：
双目相机经过自标定模块的自标定，得到双目相机光轴之间的距离d、相机的焦距f，左右相机的旋转矩阵r、r'，左右相机的平移矩阵t、t'等参数。图像处理模块对图像进行处理，增加图像的对比度以增强边缘特性。立体匹配模块将两幅图像转化成灰度图并对其模糊化处理，然后使其通过一个单边otsu滤波器，找到每个led灯在图像上的对应子区域并从中解码得到ledi(i＝1,2,3)在左右两幅图像中的匹配点的坐标分别为pi(ui，vi)、pi'(ui'，vi')；
第3步，计算左右相机的坐标：
设定：左右相机在世界坐标系的坐标分别为s＝(x，y，z)、s'＝(x'，y'，z')，由抽象出的双目相机与各led灯之间的几何关系，列出包含未知左右相机坐标的方程组分别为：
公式(1)和公式(2)中：
其中：a、b、a'、b'是由实际参数确定的已知向量，x、x'为需要求解的未知向量；
通过最小均方算法(leastmeansquare，lms)或最小二乘法(leastsquaremethod，ls)求解该矩阵方程，进而得到左右相机的坐标；
第4步，计算手持设备的倾角。
进一步地，第3步中，用lms算法求解左相机坐标的具体步骤包括：
s1，x(n)表示n次迭代后的估计系数x；
s2，设置步长μ，μ的取值范围为0<μ<2/λmax；其中：λmax是a的自相关矩阵的最大特征值；
s3，设置初始估计系数x(0)为任意值；
s4，判断是否满足误差项e(n)＝b-ax(n)是否为0；若是，则所得的即为左相机的横纵坐标，根据已求得的横纵坐标可以推算出左相机的z轴坐标
s5，更新系数x(n+1)＝x(n)+μahe(n)并循环s4，直到满足最小误差条件；所述ah表示矩阵a的共轭转置。
进一步地，第3步中，用lms算法求解右相机坐标的具体步骤包括：
u1，x'(n)表示n次迭代后的估计系数x'；
u2，设置步长μ'，μ'的取值范围为0<μ'<2/λ'max。其中，λ'max是a'的自相关矩阵的最大特征值；
u3，设置初始估计系数x'(0)为任意值；
u4，判断是否满足误差项e'(n)＝b'-a'x'(n)是否为0；若是，则所得的即为左相机的横纵坐标，根据已求得的横纵坐标可以推算出右相机的z轴坐标
u5，更新系数x'(n+1)＝x'(n)+μ'a'he'(n)并循环u5，直到满足最小误差条件；所述a'h表示矩阵a'的共轭转置。
进一步地，第3步中，用ls求解左相机坐标的具体步骤包括：
q1，残差值e＝b-ax，残差的平方f(x)表示为：
f(x)＝e2＝(b-ax)2＝(b-ax)t(b-ax)；
q2，对f(x)求导并令其等于零，即
q3，得到x的解x＝(ata)-1atb，根据已求得的横纵坐标可以推算出左相机的z轴坐标
进一步地，第3步中，用ls求解右相机坐标的具体步骤包括：
w1，残差值e'＝b'-a'x',残差的平方：
f(x')＝e'2＝(b'-a'x)2＝(b'-a'x')t(b'-a'x')；
w2，对f(x')求导并令其等于零，即：
w3，得到x的解x'＝(a'ta')-1a'tb'，根据已求得的横纵坐标可以推算出右相机的z轴坐标
进一步地，第4步中，计算相机倾角的具体步骤包括：
v1，令n1＝[0,a,0]，将其转换成世界坐标系的坐标n1，由[n2,1]＝[n1,1]*w计算得到，其中转换矩阵
v2,计算设备的倾角
与现有技术相比，本发明所描述的一种基于双目技术的室内可见光自定位系统与方法，具有以下优点：
(1)定位效率高，算法简单，可以在室内实现快速实时定位，对设备的硬件要求不高；
(2)通过定位得到的双目相机的坐标计算其倾角，获得设备的姿态信息；
(3)利用双目技术实现高精度的定位，led的布施简单，仅需要三个led就可实现自定位。
附图说明
图1显示为本发明中的基于双目技术的室内可见光自定位系统与方法的模块图；
图2显示为本发明中的基于双目技术的室内可见光自定位系统与方法的自定位流程图；
图3显示为本发明中的基于双目技术的室内可见光自定位系统与方法的左相机定位算法流程图；
图4显示为本发明中的基于双目技术的室内可见光自定位系统与方法的右相机定位算法流程图；
图5显示为本发明实施例中的室内结构示意图，图中led1、led2、led3分别安装在天花板上的不同位置的led灯，且各led灯具体坐标点已预先测得，smc表示安装有双目相机的设备。
具体实施方式
以下通过特定的具体实例说明本发明的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。
本发明的一种基于双目技术的室内可见光自定位系统与方法的核心技术思想是基于最小均方算法实现双目相机的自定位和倾角的计算。为使本发明的技术方案更为清晰，下面将结合附图进一步的对本发明作详细描述。
图1是本发明中一种基于双目技术的室内可见光自定位系统与方法的模块图，其功能为通过双目相机采集图像信号，经过相机的自标定、图像处理技术和立体匹配得到匹配点坐标，计算此双目相机在空间直角参考坐标系的坐标并最终得到设备的定位和倾角。具体地，该基于双目技术的室内可见光自定位系统与方法包括：图像采集模块、自标定模块、图像处理模块、立体匹配模块、坐标及倾角计算模块等。
图2是本发明中的一种基于双目技术的室内可见光自定位系统与方法中的自定位流程图，实现装有双目相机的设备的自定位和倾角计算，具体实现过程包括以下步骤：
第1步，图像采集：
在设定空间内固定安装三个坐标不同且已知具体位置的led灯，分别为led1、led2和led3，各led灯在空间直角参考坐标系内的坐标分别为l1(m1,n1,q1)、l2(m2,n2,q2)和l3(m3,n3,q3)。以ook(on-offkeying)编码i1、i2、i3对led1、led2和led3进行调制，再用一定的载波频率f0对调制后的led进行调制，使得led的发射频率大于1khz以保证其变化无法被人眼察觉，并可以有效地保护视力。若cmos相机的扫描频率为fs，则led的载波频率f0为fs/n(n为任意正整数)，同时将led的ook编码与坐标之间的映射关系存储到立体匹配模块中。在待定位设备上安装两个内外参数相同的相机，使其构成一对双目相机。相机在同一时刻从不同角度拍摄天花板，得到两张图像分别为左图像和右图像；
第2步，获得摄像机内外参数和匹配点坐标：
双目相机经过自标定模块的自标定，得到双目相机光轴之间的距离d、相机的焦距f，左右相机的旋转矩阵r、r'，左右相机的平移矩阵t、t'等参数。图像处理模块对图像进行处理，增加图像的对比度以增强边缘特性。立体匹配模块将两幅图像转化成灰度图并对其模糊化处理，然后使其通过一个单边otsu滤波器，找到每个led在图像上的对应子区域并从中解码得到ledi(i＝1,2,3)在左右两幅图像中的匹配点的坐标分别为pi(ui，vi)、pi'(ui'，vi')；
第3步，计算相机的坐标：
假设左右相机在空间直角参考坐标系的坐标分别为s＝(x，y，z)、s'＝(x'，y'，z')。由抽象出的双目相机与led之间的几何关系列出包含未知左、右相机坐标的方程组分别为：
其中
其中，a、b、a'和b'是由实际参数确定的已知向量，x、x'为需要求解的未知向量。并通过最小均方算法(leastmeansquare，lms)或最小二乘法(leastsquaremethod，ls)求解矩阵方程，进而得到左右相机的坐标。
第4步，计算设备的倾角。
进一步地，第3步中，用lms算法求解左相机坐标的具体步骤包括：
s1，x(n)表示n次迭代后的估计系数x；
s2，设置步长μ，μ的取值范围为0<μ<2/λmax。其中，λmax是a的自相关矩阵的最大特征值；
s3，设置初始估计系数x(0)为任意值；
s4，判断是否满足误差项e(n)＝b-ax(n)是否为0；若是，则所得的即为左相机的横纵坐标，根据已求得的横纵坐标可以推算出左相机的z轴坐标
s5，更新系数x(n+1)＝x(n)+μahe(n)并循环s4，直到满足最小误差条件，其中ah表示矩阵a的共轭转置；
进一步地，第3步中，用lms算法求解右相机坐标的具体步骤包括：
u1，x'(n)表示n次迭代后的估计系数x'；
u2，设置步长μ'，μ'的取值范围为0<μ'<2/λ'max。其中，λ'max是a'的自相关矩阵的最大特征值；
u3，设置初始估计系数x'(0)为任意值；
u4，判断是否满足误差项e'(n)＝b'-a'x'(n)是否为0；若是，则所得的即为左相机的横纵坐标，根据已求得的横纵坐标可以推算出右相机的z轴坐标
u5，更新系数x'(n+1)＝x'(n)+μ'a'he'(n)并循环u5，直到满足最小误差条件。其中a'h表示矩阵a'的共轭转置；
进一步地，第3步中，用ls求解左相机坐标的具体步骤包括：
q1，残差值e＝b-ax,残差的平方
f(x)＝e2＝(b-ax)2＝(b-ax)t(b-ax)；
q2，对f(x)求导并令其等于零，即
q3，得到x的解x＝(ata)-1atb,根据已求得的横纵坐标可以推算出左相机的z轴坐标
进一步地，第3步中，用ls求解右相机坐标的具体步骤包括：
w1，残差值e'＝b'-a'x',残差的平方
f(x')＝e'2＝(b'-a'x)2＝(b'-a'x')t(b'-a'x')；
w2，对f(x')求导并令其等于零，即
w3，得到x的解x'＝(a'ta')-1a'tb',根据已求得的横纵坐标可以推算出右相机的z轴坐标
进一步地，第4步中，计算设备倾角的具体步骤包括：
v1，令n1＝[0,a,0]，将其转换成世界坐标系的坐标n1，由[n2,1]＝[n1,1]*w计算得到，其中转换矩阵
v2,设备的倾角
下面通过具体的实施例对本发明的基于双目技术的室内可见光自定位系统与方法进行说明。
在本实施例中，将用简化的实验案例来验证本发明的定位计算方法及倾角计算方法。在设备(smc)上安装两个同型号的相机，其内外参数相同，并构成一对双目相机。图5是本发明实施例中的室内结构示意图，选取室内一点作为坐标原点，天花板上安装有三个坐标已知的led灯分别为led1、led2和led3，各led灯在空间直角参考坐标系内的坐标分别为l1(0.21,2.35,2.80)、l2(2.50,3.75,2.80)和l3(1.30,1.50,2.80)，分别以ook(on-offkeying)编码0100、1101、1010对led1、led2和led3进行调制。设定cmos相机的扫描频率fs＝3khz，再用1.5khz的载波频率对调制后的led进行调制。并将led频率与坐标之间的映射关系存储到立体匹配模块中。相机在同一时刻从不同角度拍摄天花板，得到两张图像分别为左图像和右图像；
将相机光心连线的中点坐标视为设备的坐标。令设备做无规则运动。定位装置获得设备在第j秒(j＝1,2,3,4,5,6,7,8,9,10)时的定位dj：d1＝(0.1700,0.3621,0.3648)，d2＝(0.1976,0.2815,0.4265)，d3＝(0.2013,0.3984,0.3940)，d4＝(0.2322,0.4551,0.4603)，d5＝(0.3762,0.4876,0.3724)，d6＝(0.3072,0.5293,0.5351)，d7＝(0.4054,0.5632,0.6290)，d8＝(0.4012,0.5868,0.6423)，d9＝(0.4646,0.6038,0.7100)，d10＝(0.4790,0.6353,0.7182)，同时双目相机捕获图像；
第2步，获得摄像机内外参数和匹配点坐标：
双目相机经过自标定模块的自标定，得到双目相机光轴之间的距离d＝0.1、相机的焦距f＝0.028,第js的左右相机的旋转矩阵rj、rj'和平移矩阵tj、tj'等参数，
t1＝[0.16800.31650.3565]t，t1'＝[-0.1680-0.4165-0.3565]t，
t2＝[0.20260.23380.4233]t，t2'＝[-0.2026-0.3338-0.4233]t，
t3＝[-0.10460.53760.2805]t,t3'＝[-0.1046-0.63760.2805]t，
t4＝[0.0904-0.1373-0.6555]t,t4'＝[0.09040.2373-0.6555]t，
t5＝[0.56900.30630.2592]t,t5'＝[0.5690-0.40630.2592]t，
t6＝[0.20470.31340.6977]t,t6'＝[0.2047-0.41340.6977]t，
t7＝[-0.17860.81090.3226]t,t7'＝[-0.1786-0.91090.3226]t，
t8＝[-0.19330.24840.8897]t,t8'＝[-0.1933-0.34840.8897]t，
t9＝[0.41170.90120.1016]t,t9'＝[0.4117-1.00120.1016]t，
t10＝[-0.14030.53180.8892]t,t10'＝[-0.1403-0.63180.8892]t，
图像处理模块对图像进行处理，增加图像的对比度以增强边缘特性。立体匹配模块将两幅图像转化成灰度图并对其模糊化处理，然后使其通过一个单边的otsu滤波器，找到每个led在图像上的对应子区域并从中解码得到ledi(i＝1,2,3)在左右两幅图像中的匹配点的坐标分别为pi(ui，vi)、pi'(ui'，vi')；将第j秒(j＝1,2,3,4,5,6,7,8,9,10)时ledi在左右图像上的匹配点坐标分别记作pi,i(ui，vi)、pi,i'(ui'，vi')；
p1,1＝(0.0003,0.0251)，p1,1'＝(0.0003,-0.0239)，
p2,1＝(0.0125,0.0142)，p2,1'＝(0.0125,-0.0130)，
p3,1＝(0.0275,0.0401)，p3,1'＝(0.0275,-0.0389)，
p1,2＝(-0.0008,0.0266)，p1,2'＝(0.0009,-0.0254)，
p2,2＝(0.0125,0.0150)，p2,2'＝(0.0130,-0.0138)，
p3,2＝(0.0293,0.0410)，p3,2'＝(0.0293,-0.0398)，
p1,3＝(-0.0831,0.1891)，p1,3'＝(-0.0831,-0.1824)，
p2,3＝(-0.0188,0.0368)，p2,3'＝(-0.0188,-0.0351)，
p3,3＝(0.0038,0.0546)，p3,3'＝(0.0038,-0.0533)，
p1,4＝(-0.0128,0.0184)，p1,4'＝(-0.0831,-0.0172)，
p2,4＝(-0.0117,0.0021)，p2,4'＝(-0.0117,-0.0010)，
p3,4＝(0.0023,0.0057)，p3,4'＝(0.0023,-0.0057)，
p1,5＝(0.0235,0.0745)，p1,5'＝(-0.0831,-0.0719)，
p2,5＝(0.0169,-0.0256)，p2,5'＝(-0.0117,-0.0010)，
p3,5＝(0.0654,0.0480)，p3,5'＝(0.0654,-0.0462)，
p1,6＝(-0.0028,0.0276)，p1,6'＝(-0.0028,-0.0263)，
p2,6＝(-0.0008,0.0084)，p2,6'＝(-0.0008,-0.0073)，
p3,6＝(0.0152,0.0142)，p3,6'＝(0.0152,-0.0135)，
p1,7＝(0.1579,-0.3009)，p1,7'＝(-0.0028,-0.0263)，
p2,7＝(-0.0310,0.05447)，p2,7'＝(-0.0310,-0.0518)，
p3,7＝(0.0019,0.0945)，p3,7'＝(0.0019,-0.0923)，
p1,8＝(-0.0205,0.02790)，p1,8'＝(-0.0205,-0.0263)，
p2,8＝(-0.0153,0.0058)，p2,8'＝(-0.0153,-0.0045)，
p3,8＝(0.0004,0.0109)，p3,8'＝(0.0004,-0.0102)，
p1,9＝(-0.1656,1.1663)，p1,9'＝([-0.1656,-1.1239)，
p2,9＝(0.0199,0.0726)，p2,9'＝(0.0199,-0.0694)，
p3,9＝(-0.0931,-0.1781)，p3,9'＝(-0.0931,0.1734)，
p1,10＝(-0.0278,0.0526)，p1,10'＝([-0.1656,-0.0502)，
p2,10＝(-0.0149,0.0153)，p2,10'＝(-0.0149,-0.0138)，
p3,10＝(0.0029,0.0224)，p3,10'＝(0.0029,-0.0215)，
第3步，计算相机的坐标：
假设第j秒(j＝1,2,3,4,5,6,7,8,9,10)时左右相机的坐标分别为slj＝(xlj,ylj,zlj)、srj＝(xrj,yrj,zrj)。由抽象出的双目相机与led之间的几何关系并通过最小均方算法(leastmeansquare，lms)求解左右相机的坐标。由抽象出的双目相机与led之间的几何关系列出第j秒(j＝1,2,3,4,5,6,7,8,9,10)时包含未知左相机坐标的方程组分别为：
列出第j秒(j＝1,2,3,4,5,6,7,8,9,10)时包含未知右相机坐标的方程组分别为：
将第j秒时左、右方程组分别整理成ajxj＝bj和aj'xj'＝bj'的矩阵形式，则有
其中，aj、aj'、bj、bj'是由实际参数确定的已知向量，xj、xj'为需要求解的未知向量。
进一步地，第3步中，用lms算法求解左相机坐标的具体步骤包括：
s1，xj(n)表示n次迭代后的估计系数xj；
s2，设置步长μj＝0.1；
s3，设置初始估计系数xj(j＝1,2,3,4,5,6,7,8,9,10)为[01]t；
s4，判断是否满足误差项ej(n)＝bj-ajxj(n)是否为0；若是，则所得的即为左相机的横纵坐标，根据已求得的横纵坐标可以推算出左相机的z轴坐标
s5，更新系数xj(n+1)＝xj(n)+μajhej(n)并循环s4，直到满足最小误差条件；
至此，左相机坐标的计算完成：
s1＝(0.1725，0.3171，0.3603)，s2＝(0.2010，0.2316，0.4247)，
s3＝(0.2335，0.3800，0.4239)，s4＝(0.2676，0.4200，0.4568)，
s5＝(0.4000，0.4561，0.3417)，s6＝(0.3359，0.4900，0.5235)，
s7＝(0.3941，0.5200，0.6065)，s8＝(0.4300，0.5431，0.6383)，
s9＝(0.4624，0.5700，0.6732)，s10＝(0.4965，0.5900，0.7061)。
进一步地，第3步中，用lms算法求解右相机坐标的具体步骤包括：
u1，xj'(n)表示n次迭代后的估计系数xj'；
u2，设置步长μj'＝0.1(j＝1,2,3,4,5,6,7,8,9,10)；
u3，设置初始估计系数xj'(0)＝[01]t,(j＝1,2,3,4,5,6,7,8,9,10)；
u4，判断是否满足误差项ej'(n)＝bj'-aj'xj'(n)，(j＝1,2,3,4,5,6,7,8,9,10)是否为0；若是，则所得的即为左相机的横纵坐标，根据已经求得的横纵坐标可以推算出右相机的z轴坐标
u5，更新系xj'(n+1)＝xj'(n)+μj'aj'hej'(n)数并循环u4，直到满足最小误差条件；
至此，右相机坐标的计算完成：
s1′＝(0.1683，0.4120，0.3666)，s2′＝(0.1942，0.3313，0.4282)，
s3′＝(0.1691，0.4169，0.3641)，s4′＝(0.1967，0.4902，0.4638)，
s5′＝(0.3524，0.5191，0.4030)，s6′＝(0.2785，0.56860.5466)，
s7′＝(0.4166，0.6064，0.6514)，s8′＝(0.3724，0.6275，0.6464)，
s9′＝(0.4669，0.6376，0.7468)，s10′＝(0.4616，0.6805，0.303)。
由第j秒时左相机的坐标sj和右相机的坐标sj'得到设备的坐标hj:
h1＝(0.1700,0.3621,0.3648)，h2＝(0.1976,0.2815,0.4265)，h3＝(0.2013，0.3984，0.3940)，h4＝(0.2322，0.4551，0.4603)，h5＝(0.3762，0.4876，0.3724)，h6＝(0.3072，0.5293，0.5351)，h7＝(0.4054，0.5632，0.6290)，h8＝(0.4012，0.5868，0.6423)，h9＝(0.4646，0.6038，0.7100)，h10＝(0.4790，0.6353，0.7182)。
进一步地，第3步中，用ls求解左相机坐标的具体步骤包括：
q1，残差值e＝b-ax,残差的平方
f(x)＝e2＝(b-ax)2＝(b-ax)t(b-ax)；
q2，对f(x)求导并令其等于零，即
q3，得到x的解x＝(ata)-1atb,根据已求得的横纵坐标可以推算出左相机的z轴坐标
s1＝(0.1725，0.3171，0.3603)，s2＝(0.2010，0.2316，0.4247)，
s3＝(0.2335，0.3800，0.4239)，s4＝(0.2676，0.4200，0.4568)，
s5＝(0.4000，0.4561，0.3417)，s6＝(0.3359，0.4900，0.5235)，
s7＝(0.3941，0.5200，0.6065)，s8＝(0.4300，0.5431，0.6383)，
s9＝(0.4624，0.5700，0.6732)，s10＝(0.4965，0.5900，0.7061)。
进一步地，第3步中，用ls求解右相机坐标的具体步骤包括：
w1，残差值e'＝b'-a'x',残差的平方
f(x')＝e'2＝(b'-a'x)2＝(b'-a'x')t(b'-a'x')；
w2，对f(x')求导并令其等于零，即
w3，得到x的解x'＝(a'ta')-1a'tb',根据已求得的横纵坐标可以推算出右相机的z轴坐标
s1′＝(0.1683，0.4120，0.3666)，s2′＝(0.1942，0.3313，0.4282)，
s3′＝(0.1691，0.4169，0.3641)，s4′＝(0.1967，0.4902，0.4638)，
s5′＝(0.3524，0.5191，0.4030)，s6′＝(0.2785，0.56860.5466)，
s7′＝(0.4166，0.6064，0.6514)，s8′＝(0.3724，0.6275，0.6464)，
s9′＝(0.4669，0.6376，0.7468)，s10′＝(0.4616，0.6805，0.303)。
此时得到设备的坐标hj':
h1'＝(0.1700,0.3621,0.3648)，h2'＝(0.1976,0.2815,0.4265)，h3'＝(0.2013，0.3984，0.3940)，h4'＝(0.2322，0.4551，0.4603)，h5'＝(0.3762，0.4876，0.3724)，h6'＝(0.3072，0.5293，0.5351)，h7'＝(0.4054，0.5632，0.6290)，h8'＝(0.4012，0.5868，0.6423)，h9'＝(0.4646，0.6038，0.7100)，h10'＝(0.4790，0.6353，0.7182)。
进一步地，第4步中，计算设备倾角的具体步骤包括：
v1，令t1＝[0,1,0]，将其分别转换成js时在世界坐标系的坐标gj，由[gj,1]＝[t1,1]*wj计算得到，其中转换矩阵
v2，计算设备在第j秒的倾角
尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在不脱离本发明的原理和宗旨的情况下在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。本发明的范围由所附权利要求及其等同限定。
- 还没有人留言评论。精彩留言会获得点赞！