发明名称 一种面向室内移动机器人的光流场视觉/INS组合导航方法
摘要 一种面向室内移动机器人的光流场视觉导航/INS组合导航方法,其采用基于光流场的视觉导航和INS两种导航方式,移动机器人通过车载摄像头采集车辆的动态视频,在视频中获取视频光流特征,根据特征计算移动机器人在载体坐标系下的横向速度(V<sub>x</sub>)、纵向速度(V<sub>y</sub>),并与INS测量得到的航向角计算机器人的东向速度(V<sub>E</sub>)和北向速度(V<sub>N</sub>)。将计算得到的V<sub>E</sub>、V<sub>N</sub>和INS测量得到的航向角变化量ω<sub>Z</sub>共同输入到扩展卡尔曼滤波器中进行数据融合,得到比基于光流场导航和INS两种导航方式精度更高的移动机器人的东向和北向的速度和加速度的最优预估。通过本发明的方法可为移动机器人提供更加精确的导航信息,扩大了室内机器人定位的范围。
申请公布号 CN103411621B 申请公布日期 2016.02.10
申请号 CN201310369354.9 申请日期 2013.08.09
申请人 东南大学 发明人 李庆华;陈熙源;高金鹏;徐元
分类号 G01C21/28(2006.01)I 主分类号 G01C21/28(2006.01)I
代理机构 江苏永衡昭辉律师事务所 32250 代理人 王斌
主权项 一种面向室内移动机器人的光流场视觉/INS组合导航方法,其包括下列步骤:(1)车载摄像头垂直向下放置,从车载摄像头拍摄的图像帧中选取一个矩形区域作为感兴趣区域ROI,后续的处理均在ROI中进行;(2)在上述ROI中根据Shi‑Tomasi角点检测方法提取N个角点,记录所有角点在图像坐标系下的坐标;(3)采用图像金字塔Lucas‑Kanade光流法对选取的步骤(2)中选取的角点进行跟踪,获取所有角点在下一帧中的图像坐标系中的坐标;(4)采用随机抽样一致性算法RANSAC剔除跟踪异常的角点,只对跟踪正常的角点进行数据处理;(5)根据所有跟踪正常的角点在相邻两帧图像中对应的坐标,计算所有正常角点在横向和纵向的位置变化(Δx,Δy);(6)假设车载摄像头的投影中心距离地面的垂直距离为Z<sub>R</sub>,焦距为f<sub>1</sub>,f<sub>2</sub>,主光轴点坐标(c<sub>1</sub>,c<sub>2</sub>),车载摄像头在水平面上的速度分量分别为V<sub>x</sub>,V<sub>y</sub>,镜头畸变系数为[k<sub>1</sub>,k<sub>2</sub>,k<sub>3</sub>,k<sub>4</sub>,k<sub>5</sub>],其包括径向畸变系数和切向畸变系数,所述车载摄像头模型的归一化映射关系如下:<maths num="0001" id="cmaths0001"><math><![CDATA[<mrow><mfenced open = "[" close = "]"><mtable><mtr><mtd><msub><mi>x</mi><mi>n</mi></msub></mtd></mtr><mtr><mtd><msub><mi>y</mi><mi>n</mi></msub></mtd></mtr></mtable></mfenced><mo>=</mo><mfenced open = "[" close = "]"><mtable><mtr><mtd><mfrac><msub><mi>X</mi><mi>R</mi></msub><msub><mi>Z</mi><mi>R</mi></msub></mfrac></mtd></mtr><mtr><mtd><mfrac><msub><mi>Y</mi><mi>R</mi></msub><msub><mi>Z</mi><mi>R</mi></msub></mfrac></mtd></mtr></mtable></mfenced><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>1</mn><mo>)</mo></mrow></mrow>]]></math><img file="FDA0000801709340000011.GIF" wi="469" he="382" /></maths>其中,<maths num="0002" id="cmaths0002"><math><![CDATA[<mfenced open = "[" close = "]"><mtable><mtr><mtd><msub><mi>x</mi><mi>n</mi></msub></mtd></mtr><mtr><mtd><msub><mi>y</mi><mi>n</mi></msub></mtd></mtr></mtable></mfenced>]]></math><img file="FDA0000801709340000012.GIF" wi="109" he="159" /></maths>为车载摄像头模型的归一化映射后的图像坐标系上跟踪正常的角点坐标,(X<sub>R</sub>,Y<sub>R</sub>,Z<sub>R</sub>)为与图像坐标系上跟踪正常的角点相对应的地面上的真实点在相机坐标系中的坐标,考虑镜头畸变系数,则相应的图像坐标系上的点可以表示为:<maths num="0003" id="cmaths0003"><math><![CDATA[<mrow><mfenced open = "[" close = "]"><mtable><mtr><mtd><msub><mi>x</mi><mi>d</mi></msub></mtd></mtr><mtr><mtd><msub><mi>y</mi><mi>d</mi></msub></mtd></mtr></mtable></mfenced><mo>=</mo><mrow><mo>(</mo><mn>1</mn><mo>+</mo><msub><mi>k</mi><mn>1</mn></msub><msup><mi>r</mi><mn>2</mn></msup><mo>+</mo><msub><mi>k</mi><mn>2</mn></msub><msup><mi>r</mi><mn>4</mn></msup><mo>+</mo><msub><mi>k</mi><mn>5</mn></msub><msup><mi>r</mi><mn>6</mn></msup><mo>)</mo></mrow><mfenced open = "[" close = "]"><mtable><mtr><mtd><msub><mi>x</mi><mi>n</mi></msub></mtd></mtr><mtr><mtd><msub><mi>y</mi><mi>n</mi></msub></mtd></mtr></mtable></mfenced><mo>+</mo><msub><mi>t</mi><mi>d</mi></msub><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>2</mn><mo>)</mo></mrow></mrow>]]></math><img file="FDA0000801709340000021.GIF" wi="894" he="159" /></maths>其中<img file="FDA0000801709340000022.GIF" wi="267" he="94" />为车载摄像头模型的归一化映射后的图像坐标系上跟踪正常的角点坐标相对于图像中心的半径,切向畸变向量<maths num="0004" id="cmaths0004"><math><![CDATA[<mrow><msub><mi>t</mi><mi>d</mi></msub><mo>=</mo><mfenced open = "[" close = "]"><mtable><mtr><mtd><mn>2</mn><msub><mi>k</mi><mn>3</mn></msub><msub><mi>x</mi><mi>n</mi></msub><msub><mi>y</mi><mi>n</mi></msub><mo>+</mo><msub><mi>k</mi><mn>4</mn></msub><mo>(</mo><msup><mi>r</mi><mn>2</mn></msup><mo>+</mo><mn>2</mn><msubsup><mi>x</mi><mi>n</mi><mn>2</mn></msubsup><mo>)</mo></mtd></mtr><mtr><mtd><msub><mi>k</mi><mn>3</mn></msub><mo>(</mo><msup><mi>r</mi><mn>2</mn></msup><mo>+</mo><mn>2</mn><msubsup><mi>y</mi><mi>n</mi><mn>2</mn></msubsup><mo>)</mo><mo>+</mo><mn>2</mn><msub><mi>k</mi><mn>4</mn></msub><msub><mi>x</mi><mi>n</mi></msub><msub><mi>y</mi><mi>n</mi></msub></mtd></mtr></mtable></mfenced><mo>,</mo></mrow>]]></math><img file="FDA0000801709340000023.GIF" wi="766" he="198" /></maths>点(c<sub>1</sub>,c<sub>2</sub>)为车载摄像头光轴与图像平面的交点,则像素点的最终坐标为:<maths num="0005" id="cmaths0005"><math><![CDATA[<mrow><mfenced open = "[" close = "]"><mtable><mtr><mtd><msub><mi>x</mi><mi>c</mi></msub></mtd></mtr><mtr><mtd><msub><mi>y</mi><mi>c</mi></msub></mtd></mtr></mtable></mfenced><mo>=</mo><mfenced open = "[" close = "]"><mtable><mtr><mtd><msub><mi>f</mi><mn>1</mn></msub></mtd><mtd><mn>0</mn></mtd></mtr><mtr><mtd><mn>0</mn></mtd><mtd><msub><mi>f</mi><mn>2</mn></msub></mtd></mtr></mtable></mfenced><mfenced open = "[" close = "]"><mtable><mtr><mtd><msub><mi>x</mi><mi>d</mi></msub></mtd></mtr><mtr><mtd><msub><mi>y</mi><mi>d</mi></msub></mtd></mtr></mtable></mfenced><mo>+</mo><mfenced open = "[" close = "]"><mtable><mtr><mtd><msub><mi>c</mi><mn>1</mn></msub></mtd></mtr><mtr><mtd><msub><mi>c</mi><mn>2</mn></msub></mtd></mtr></mtable></mfenced><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>3</mn><mo>)</mo></mrow></mrow>]]></math><img file="FDA0000801709340000024.GIF" wi="676" he="151" /></maths>假设镜头的畸变系数[k<sub>1</sub>,k<sub>2</sub>,k<sub>3</sub>,k<sub>4</sub>,k<sub>5</sub>]可忽略,均为零,则<maths num="0006" id="cmaths0006"><math><![CDATA[<mrow><mfenced open = "[" close = "]"><mtable><mtr><mtd><msub><mi>x</mi><mi>c</mi></msub></mtd></mtr><mtr><mtd><msub><mi>y</mi><mi>c</mi></msub></mtd></mtr></mtable></mfenced><mo>=</mo><mfenced open = "[" close = "]"><mtable><mtr><mtd><msub><mi>f</mi><mn>1</mn></msub><mfrac><msub><mi>X</mi><mi>R</mi></msub><msub><mi>Z</mi><mi>R</mi></msub></mfrac></mtd></mtr><mtr><mtd><mrow><msub><mi>f</mi><mn>2</mn></msub><mfrac><msub><mi>Y</mi><mi>R</mi></msub><msub><mi>Z</mi><mi>R</mi></msub></mfrac></mrow></mtd></mtr></mtable></mfenced><mo>+</mo><mfenced open = "[" close = "]"><mtable><mtr><mtd><msub><mi>c</mi><mn>1</mn></msub></mtd></mtr><mtr><mtd><msub><mi>c</mi><mn>2</mn></msub></mtd></mtr></mtable></mfenced><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>4</mn><mo>)</mo></mrow></mrow>]]></math><img file="FDA0000801709340000025.GIF" wi="558" he="294" /></maths>(4)式对时间微分得:<maths num="0007" id="cmaths0007"><math><![CDATA[<mrow><mfenced open = "[" close = "]"><mtable><mtr><mtd><msub><mi>V</mi><mi>x</mi></msub></mtd></mtr><mtr><mtd><msub><mi>V</mi><mi>y</mi></msub></mtd></mtr></mtable></mfenced><mo>=</mo><mfenced open = "[" close = "]"><mtable><mtr><mtd><mfrac><mrow><msub><mi>Z</mi><mi>R</mi></msub><msub><mi>v</mi><mi>x</mi></msub></mrow><msub><mi>f</mi><mn>1</mn></msub></mfrac><mo>+</mo><mfrac><mrow><msub><mi>X</mi><mi>R</mi></msub><msub><mi>V</mi><mi>z</mi></msub></mrow><msub><mi>Z</mi><mi>R</mi></msub></mfrac></mtd></mtr><mtr><mtd><mrow><mfrac><mrow><msub><mi>Z</mi><mi>R</mi></msub><msub><mi>v</mi><mi>y</mi></msub></mrow><msub><mi>f</mi><mn>2</mn></msub></mfrac><mo>+</mo><mfrac><mrow><msub><mi>Y</mi><mi>R</mi></msub><msub><mi>V</mi><mi>z</mi></msub></mrow><msub><mi>Z</mi><mi>R</mi></msub></mfrac></mrow></mtd></mtr></mtable></mfenced><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>5</mn><mo>)</mo></mrow></mrow>]]></math><img file="FDA0000801709340000026.GIF" wi="574" he="303" /></maths>式中V<sub>x</sub>为X<sub>R</sub>对时间的微分即<img file="FDA0000801709340000027.GIF" wi="198" he="92" />V<sub>y</sub>为Y<sub>R</sub>对时间的微分即<img file="FDA0000801709340000028.GIF" wi="174" he="94" />V<sub>Z</sub>为Z<sub>R</sub>对时间的微分即<img file="FDA0000801709340000029.GIF" wi="189" he="93" />v<sub>x</sub>为x<sub>c</sub>对时间的微分即<img file="FDA00008017093400000210.GIF" wi="173" he="78" />v<sub>y</sub>为y<sub>c</sub>对时间的微分即<maths num="0008" id="cmaths0008"><math><![CDATA[<mrow><msub><mi>v</mi><mi>y</mi></msub><mo>=</mo><msub><mover><mi>y</mi><mo>&CenterDot;</mo></mover><mi>c</mi></msub><mo>;</mo></mrow>]]></math><img file="FDA00008017093400000211.GIF" wi="179" he="86" /></maths>考虑地面水平则Z<sub>R</sub>为常值,对时间导数为0则有:<maths num="0009" id="cmaths0009"><math><![CDATA[<mrow><mfenced open = "[" close = "]"><mtable><mtr><mtd><msub><mi>V</mi><mi>x</mi></msub></mtd></mtr><mtr><mtd><msub><mi>V</mi><mi>y</mi></msub></mtd></mtr></mtable></mfenced><mo>=</mo><mfenced open = "[" close = "]"><mtable><mtr><mtd><mfrac><mrow><msub><mi>Z</mi><mi>R</mi></msub><msub><mi>v</mi><mi>x</mi></msub></mrow><msub><mi>f</mi><mn>1</mn></msub></mfrac></mtd></mtr><mtr><mtd><mfrac><mrow><msub><mi>Z</mi><mi>R</mi></msub><msub><mi>v</mi><mi>y</mi></msub></mrow><msub><mi>f</mi><mn>2</mn></msub></mfrac></mtd></mtr></mtable></mfenced><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>6</mn><mo>)</mo></mrow></mrow>]]></math><img file="FDA00008017093400000212.GIF" wi="418" he="302" /></maths>由(4)式带入得:<maths num="0010" id="cmaths0010"><math><![CDATA[<mrow><mfenced open = "[" close = "]"><mtable><mtr><mtd><msub><mi>V</mi><mi>x</mi></msub></mtd></mtr><mtr><mtd><msub><mi>V</mi><mi>y</mi></msub></mtd></mtr></mtable></mfenced><mo>=</mo><mfenced open = "[" close = "]"><mtable><mtr><mtd><mfrac><mrow><msub><mi>Z</mi><mi>R</mi></msub><msub><mi>v</mi><mi>x</mi></msub></mrow><msub><mi>f</mi><mn>1</mn></msub></mfrac><mo>+</mo><mfrac><mn>1</mn><msub><mi>f</mi><mn>1</mn></msub></mfrac><mo>(</mo><msub><mi>x</mi><mi>c</mi></msub><mo>-</mo><msub><mi>c</mi><mn>1</mn></msub><mo>)</mo><msub><mi>V</mi><mi>z</mi></msub></mtd></mtr><mtr><mtd><mrow><mfrac><mrow><msub><mi>Z</mi><mi>R</mi></msub><msub><mi>v</mi><mi>y</mi></msub></mrow><msub><mi>f</mi><mn>2</mn></msub></mfrac><mo>+</mo><mfrac><mn>1</mn><msub><mi>f</mi><mn>2</mn></msub></mfrac><mrow><mo>(</mo><msub><mi>y</mi><mi>c</mi></msub><mo>-</mo><msub><mi>c</mi><mn>2</mn></msub><mo>)</mo></mrow><msub><mi>V</mi><mi>z</mi></msub></mrow></mtd></mtr></mtable></mfenced><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>7</mn><mo>)</mo></mrow></mrow>]]></math><img file="FDA0000801709340000031.GIF" wi="733" he="302" /></maths>考虑地面水平则Z<sub>R</sub>为常值,对时间导数为0则有:<maths num="0011" id="cmaths0011"><math><![CDATA[<mrow><mfenced open = "[" close = "]"><mtable><mtr><mtd><msub><mi>V</mi><mi>x</mi></msub></mtd></mtr><mtr><mtd><msub><mi>V</mi><mi>y</mi></msub></mtd></mtr></mtable></mfenced><mo>=</mo><mfenced open = "[" close = "]"><mtable><mtr><mtd><mfrac><mrow><msub><mi>Z</mi><mi>R</mi></msub><msub><mi>v</mi><mi>x</mi></msub></mrow><msub><mi>f</mi><mn>1</mn></msub></mfrac></mtd></mtr><mtr><mtd><mfrac><mrow><msub><mi>Z</mi><mi>R</mi></msub><msub><mi>v</mi><mi>y</mi></msub></mrow><msub><mi>f</mi><mn>2</mn></msub></mfrac></mtd></mtr></mtable></mfenced><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>8</mn><mo>)</mo></mrow></mrow>]]></math><img file="FDA0000801709340000032.GIF" wi="421" he="311" /></maths>(7)与惯性导航系统(INS)测量得到的航向角<img file="FDA0000801709340000033.GIF" wi="70" he="54" />计算机器人的东向速度V<sub>E</sub>和北向速度V<sub>N</sub>,计算公式如下:<img file="FDA0000801709340000034.GIF" wi="597" he="87" /><img file="FDA0000801709340000035.GIF" wi="662" he="86" />(8)将计算得到的V<sub>E</sub>、V<sub>N</sub>和INS测量得到的航向角变化量ω<sub>Z</sub>共同输入到卡尔曼滤波器中进行数据融合;(9)扩展卡尔曼滤波器的系统方程以每一时刻视觉测量得到V<sub>E</sub>和V<sub>N</sub>以及INS每一时刻东向和北向的加速度作为状态变量,以每一时刻视觉测量得到的V<sub>E</sub>、V<sub>N</sub>和INS测量得到的航向角变化量ω<sub>Z</sub>作为观测量,滤波器的系统方程如式(11)所示:<img file="FDA0000801709340000036.GIF" wi="1462" he="566" />其中<img file="FDA0000801709340000037.GIF" wi="179" he="163" />为系统噪声,观测方程如式(12)所示:<img file="FDA0000801709340000041.GIF" wi="1325" he="565" />其中υ<sub>3×1</sub>为观测方程噪声,V<sub>E,k</sub>,V<sub>N,k</sub>,Acc<sub>E,k</sub>,Acc<sub>N,k</sub>分别代表k时刻的东向速度、北向速度、东向加速度和北向加速度;V<sub>E,k+1</sub>,V<sub>N,k+1</sub>Acc<sub>E,k+1</sub>,Acc<sub>N,k+1</sub>分别代表k+1时刻的东向速度、北向速度、东向加速度和北向加速度;<img file="FDA0000801709340000042.GIF" wi="259" he="101" />分别为所观测到的东向速度、北向速度和航向角变化量;T为相邻时刻间隔周期。
地址 210096 江苏省南京市四牌楼2号