二维码以及用于机器人的视觉-惯性组合导航系统及方法

摘要

本发明实施例提供了一种二维码以及用于机器人的视觉‑惯性组合导航系统及方法。二维码的外围带有封闭的辅助边框，该辅助边框和二维码均用于视觉导航。用于机器人的视觉‑惯性组合导航系统采用前述二维码。用于机器人的视觉‑惯性组合导航方法包括：在地面上铺设多个外围带有封闭的辅助边框的二维码；在机器人行进过程中，成像设备拍摄图像；获取成像设备的绝对位置和绝对方向角，获得二维码的绝对坐标，成像设备的绝对位置和绝对方向角；确定机器人相对其当前起始点的相对位置和相对其当前起始方向角的相对方向角；得到机器人的绝对位置，利用该绝对位置作为下一起始点；得到机器人的绝对方向角，利用该绝对方向角作为下一起始方向角。

权利要求

1.一种用于机器人的视觉‑惯性组合导航方法，其特征在于，包括：控制设置在机器人上的成像设备拍摄机器人在行进路线上经过铺设在地面上的外围带有辅助边框的二维码，以获得二维码的图像；

针对拍摄的二维码的图像进行边缘提取，以获得边缘图像；

针对边缘图像进行筛选，以获得封闭的轮廓曲线；

针对封闭的轮廓曲线进行多边形拟合，并将辅助边框的轮廓在尺寸和形状均相同的封闭的轮廓曲线确定为辅助边框；

基于辅助边框确定辅助边框内的区域为二维码区域；

基于确定的辅助边框和确定的二维码区域，计算成像设备相对于二维码区域的相对位置和相对方向角；

利用二维码扫描程序对二维码的图像在二维码区域内进行扫描，以扫描二维码，并基于二维码编码规则对扫描后的二维码进行解码和校验，以获得二维码的绝对坐标；

基于计算出的成像设备的相对位置和相对方向角以及获得的二维码的绝对坐标，经坐标系转换，获得成像设备的绝对位置和绝对方向角，作为惯性导航中修正机器人位置的视觉导航数据；

获得机器人相对机器人的当前起始点的相对位置以及机器人相对机器人的当前起始方向角的相对方向角；机器人的相对位置和相对方向角是由设置在机器人上的编码器和惯性导航系统确定的；

对成像设备的绝对位置和机器人的相对位置进行计算，以得到机器人的绝对位置，并利用得到的机器人的绝对位置作为惯性导航中确定机器人的下一起始位置，以及，对成像设备的绝对方向角和机器人的相对方向角进行估计，以得到机器人的绝对方向角，并利用得到的机器人的绝对方向角作为惯性导航中确定机器人的下一起始方向角。

2.根据权利要求1所述的方法，其特征在于，所述基于确定的辅助边框和确定的二维码区域，计算成像设备相对于二维码区域的相对位置和相对方向角，包括：根据确定的辅助边框的内周或外周的顶点的图像坐标，计算出成像设备的光心相对于二维码区域的中心的相对位置和相对方向角，作为成像设备相对于二维码区域的相对位置和相对方向角。

3.根据权利要求2所述的方法，其特征在于，所述根据确定的辅助边框的内周或外周的顶点的图像坐标，计算出成像设备的光心相对于二维码区域的中心的相对位置和相对方向角，包括：

根据辅助边框的内周或外周的顶点的图像坐标，计算得到辅助边框中心的图像像素坐标，将图像像素坐标乘以比例因子即为成像设备的光心相对于二维码区域的中心的相对位置；其中，比例因子k＝行长度/行像素个数；

由辅助边框中心点与二维码的图像中心点组成一条直线，计算直线与竖直方向的夹角，为成像设备的光心相对于二维码区域的中心的相对方位角。

4.根据权利要求1所述的方法，其特征在于，所述基于计算出的成像设备的相对位置和相对方向角以及获得的二维码的绝对坐标，经坐标系转换，获得成像设备的绝对位置和绝对方向角，包括：

设二维码的绝对坐标为(x1，y1)，绝对方向角为θ，设成像设备的相对位置为(x1’，y1’)，绝对方向角为θ’，则成像设备的绝对位置为(x1+x1’，y1+y1’)，绝对方向角为θ+θ’。

5.根据权利要求1所述的方法，其特征在于，所述二维码为QR码，QR码包含有三个小正方形，所述三个小正方形为QR本身的位置探测图形。

6.根据权利要求5所述的方法，其特征在于，在基于辅助边框确定辅助边框内的区域为二维码区域后，还包括：

针对封闭的轮廓曲线，如果存在有三个封闭的轮廓曲线与三个小正方形的轮廓在尺寸和形状上均相同，二维码区域确定正确。

7.根据权利要求1所述的方法，其特征在于，在基于确定的辅助边框和确定的二维码区域，计算成像设备相对于二维码区域的相对位置和相对方向角，且在利用二维码扫描程序对二维码的图像在二维码区域内进行扫描之前，还包括：将包含二维码区域的辅助边框的顶点对应到一个正多边形区域得到单应性矩阵；

根据单应性矩阵进行透视变换得到正的二维码区域。

8.一种用于视觉‑惯性组合导航的二维码区域筛选方法，所述视觉‑惯性组合导航运用在机器人上，其特征在于，包括：控制设置在机器人上的成像设备拍摄机器人在行进路线上经过铺设在地面上的外围带有辅助边框的二维码，以获得二维码的图像；

针对拍摄的二维码的图像进行边缘提取，以获得边缘图像；

针对边缘图像进行筛选，以获得封闭的轮廓曲线；

针对封闭的轮廓曲线进行多边形拟合，并将辅助边框的轮廓在尺寸和形状均相同的封闭的轮廓曲线确定为辅助边框；

基于辅助边框确定辅助边框内的区域为二维码区域。

9.根据权利要求8所述的方法，其特征在于，所述二维码为QR码，QR码包含有三个小正方形，所述三个小正方形为QR本身的位置探测图形。

10.根据权利要求9所述的方法，其特征在于，还包括：针对封闭的轮廓曲线，如果存在有三个封闭的轮廓曲线与三个小正方形的轮廓在尺寸和形状上均相同，二维码区域确定正确。

11.根据权利要求8所述的方法，其特征在于，还包括：基于确定的辅助边框和确定的二维码区域，计算成像设备相对于二维码区域的相对位置和相对方向角；

利用二维码扫描程序对二维码的图像在二维码区域内进行扫描，以扫描二维码，并基于二维码编码规则对扫描后的二维码进行解码和校验，以获得二维码的绝对坐标；

基于计算出的成像设备的相对位置和相对方向角以及获得的二维码的绝对坐标，经坐标系转换，获得成像设备的绝对位置和绝对方向角，作为惯性导航中修正机器人位置的视觉导航数据。

12.一种机器人，其特征在于，包括存储器和处理器；其中，所述存储器用于存储一条或多条计算机指令，所述一条或多条计算机被所述处理器执行如权利要求1‑7中任一项所述的方法。

13.一种机器人，其特征在于，包括存储器和处理器；其中，所述存储器用于存储一条或多条计算机指令，所述一条或多条计算机被所述处理器执行如权利要求8‑11中任一项所述的方法。

14.一种用于机器人的视觉‑惯性组合导航系统，其特征在于，包括二维码以及如权利要求12所述的机器人，或者包括二维码以及如权利要求13所述的机器人；多个所述二维码铺设在地面上，所述二维码的外围带有封闭的辅助边框，该辅助边框和所述二维码均用于视觉导航。

说明书

二维码以及用于机器人的视觉‑惯性组合导航系统及方法

[0001] 相关申请的交叉引用

[0002] 本申请为2015年6月1日提交的中国专利申请号为“201510293436.9”的分案申请，其全部内容作为整体并入本申请中。

技术领域

[0003] 本发明涉及导航领域，具体而言涉及一种二维码以及用于机器人的视觉‑惯性组合导航系统及方法。

背景技术

[0004] 视觉‑惯性组合导航凭借其良好的互补性和自主性逐渐成为导航领域的重要发展方向和极具发展前景的导航技术。惯性导航是一种不依赖外界信息的自主式导航系统，具
有实时性好、抗干扰性强等优点，但其精度误差会造成累积漂移误差,在较长时间内难以达
到定位的要求。因此，在视觉/惯性组合导航中，视觉导航用以辅助定位修正惯性导航的漂
移,从而提供一种高精度的组合定位方式。从工程应用角度看，视觉导航的精确性、鲁棒性
和实时性是影响视觉/惯性组合导航性能的重要因素。

发明内容

[0005] 鉴于背景技术中存在的问题，本发明的目的在于提供一种二维码以及用于机器人的视觉‑惯性组合导航系统及方法，其能有效加快二维码区域的筛选效率、成像设备的绝对
位置和绝对方向角的计算效率，实时地修正惯性导航的漂移，以更可靠地以视觉/惯性组合
的方式实现机器人的高精度实时导航。

[0006] 为了实现上述目的，在第一方面，本发明实施例提供了一种二维码，其中所述二维码的外围带有封闭的辅助边框，该辅助边框和所述二维码均用于视觉导航。

[0007] 为了实现上述目的，在第二方面，本发明实施例提供了一种用于机器人的视觉‑惯性组合导航方法，包括：

[0008] 控制设置在机器人上的成像设备拍摄机器人在行进路线上经过铺设在地面上的外围带有辅助边框的二维码，以获得二维码的图像；

[0009] 针对拍摄的二维码的图像进行边缘提取，以获得边缘图像；

[0010] 针对边缘图像进行筛选，以获得封闭的轮廓曲线；

[0011] 针对封闭的轮廓曲线进行多边形拟合，并将辅助边框的轮廓在尺寸和形状均相同的封闭的轮廓曲线确定为辅助边框；

[0012] 基于辅助边框确定辅助边框内的区域为二维码区域；

[0013] 基于确定的辅助边框和确定的二维码区域，计算成像设备相对于二维码区域的相对位置和相对方向角；

[0014] 利用二维码扫描程序对二维码的图像在二维码区域内进行扫描，以扫描二维码，并基于二维码编码规则对扫描后的二维码进行解码和校验，以获得二维码的绝对坐标；

[0015] 基于计算出的成像设备的相对位置和相对方向角以及获得的二维码的绝对坐标，经坐标系转换，获得成像设备的绝对位置和绝对方向角，作为惯性导航中修正机器人位置
的视觉导航数据；

[0016] 获得机器人相对机器人的当前起始点的相对位置以及机器人相对机器人的当前起始方向角的相对方向角；机器人的相对位置和相对方向角是由设置在机器人上的编码器
和惯性导航系统确定的；

[0017] 对成像设备的绝对位置和机器人的相对位置进行计算，以得到机器人的绝对位置，并利用得到的机器人的绝对位置作为惯性导航中确定机器人的下一起始位置，以及，对
成像设备的绝对方向角和机器人的相对方向角进行估计，以得到机器人的绝对方向角，并
利用得到的机器人的绝对方向角作为惯性导航中确定机器人的下一起始方向角。

[0018] 为了实现上述目的，在第三方面，本发明实施例提供了一种用于视觉‑惯性组合导航的二维码区域筛选方法，所述视觉‑惯性组合导航运用在机器人上，该方法包括：

[0019] 控制设置在机器人上的成像设备拍摄机器人在行进路线上经过铺设在地面上的外围带有辅助边框的二维码，以获得二维码的图像；

[0020] 针对拍摄的二维码的图像进行边缘提取，以获得边缘图像；

[0021] 针对边缘图像进行筛选，以获得封闭的轮廓曲线；

[0022] 针对封闭的轮廓曲线进行多边形拟合，并将辅助边框的轮廓在尺寸和形状均相同的封闭的轮廓曲线确定为辅助边框；

[0023] 基于辅助边框确定辅助边框内的区域为二维码区域。

[0024] 为了实现上述目的，在第六方面，本发明实施例提供了一种用于机器人的视觉‑惯性组合导航系统，包括本发明实施例的第一方面所述的二维码以及机器人，或者包括本发
明实施例的第一方面所述的二维码以及机器人；多个所述二维码铺设在地面上。

[0025] 本发明实施例的有益效果如下：

[0026] 在本发明的二维码以及用于机器人的视觉‑惯性组合导航系统及方法中，采用外围带有封闭的辅助边框的二维码，能有效加快二维码区域的筛选效率、成像设备的绝对位
置和绝对方向角的计算效率；由于在地面上铺设多个外围带有封闭的辅助边框的二维码，
在机器人行进过程中，通过设置在机器人上的成像设备拍摄机器人在其行进路线上经过的
铺设在地面上的外围带有辅助边框的二维码的图像，之后经过计算得到机器人的绝对位置
和绝对方向角，并利用得到机器人的该绝对位置和绝对方向角作为惯性导航系统确定机器
人的下一起始点和下一起始方向角，从而可以在机器人行进过程中每拍摄到经过的外围带
有辅助边框的二维码的图像，就进行如此处理，实时地修正惯性导航的漂移，以更可靠地以
视觉/惯性组合的方式实现机器人的高精度实时导航。

附图说明

[0027] 图1示出根据本发明的二维码；

[0028] 图2示出在地面上铺设多个外围带有封闭的辅助边框的二维码的示意图；

[0029] 图3为在根据本发明的用于机器人的视觉‑惯性组合导航方法中的步骤S4中确定机器人的当前起始点的相对位置的计算示意图。

具体实施方式

[0030] 下面参照附图来说明本发明的二维码以及用于机器人的视觉‑惯性组合导航系统及方法。

[0031] 首先说明根据本发明第一方面的二维码。

[0032] 图1示出根据本发明第一方面的二维码，如图1所示，所述二维码的外围带有封闭的辅助边框，该辅助边框和所述二维码均用于视觉导航。在图1中，黑色最外的边框为辅助
边框，辅助边框的色彩无限制，只要与二维码的背景色足以区分即可。此外，该辅助边框和
所述二维码均用于视觉导航，本发明的辅助边框不起修饰作用。

[0033] 在根据本发明第一方面所述的二维码中，辅助边框可为方形。由于目前的二维码的轮廓都是方形，采用方形的辅助边框，包络二维码的轮廓的辅助边框最小，从而在识别时
容易且最快。但是如果采用其他形状，例如三角形，包络线非常大，也不容易判断。当然不限
于此，如果二维码的轮廓变化，也可采用与二维码的轮廓几何相似的辅助边框。

[0034] 在根据本发明第一方面所述的二维码中，二维码可为QR码。但不限于此，可以选用任何合适的二维码。

[0035] 其次说明根据本发明第二方面的用于机器人的视觉‑惯性组合导航系统。

[0036] 根据本发明第二方面的用于机器人的视觉‑惯性组合导航系统采用本发明第一方面所述的二维码，且在地面上铺设多个外围带有封闭的辅助边框的二维码(如图2所示)。图
2示出在地面上铺设多个外围带有封闭的辅助边框的二维码的示意图，其只是一个示意，多
个外围带有封闭的辅助边框的二维码在地面上的铺设可以视实际情况来布置。

[0037] 最后说明根据本发明第三方面的用于机器人的视觉‑惯性组合导航方法。

[0038] 根据本发明第三方面的用于机器人的视觉‑惯性组合导航方法包括步骤：步骤S1，在地面上铺设多个外围带有封闭的辅助边框的二维码；步骤S2，在机器人行进过程中，通过
设置在机器人上的成像设备拍摄机器人在其行进路线上经过的铺设在地面上的外围带有
辅助边框的二维码的图像；步骤S3，当拍摄到一个铺设在地面上的外围带有辅助边框的二
维码的图像时，基于拍摄的该一个图像获取成像设备的绝对位置和绝对方向角；步骤S4，利
用设置在机器人上的编码器和惯性导航系统确定机器人相对机器人的当前起始点的相对
位置和机器人相对机器人的当前起始方向角的相对方向角；步骤S5，对成像设备的绝对位
置和机器人的相对位置进行计算，以得到机器人的绝对位置，并利用得到机器人的该绝对
位置作为惯性导航系统确定机器人的下一起始点；以及步骤S6，对成像设备的绝对方向角
和机器人的相对方向角进行估计，以得到机器人的绝对方向角，并利用得到机器人的该绝
对方向角作为惯性导航系统确定机器人的下一起始方向角。其中，步骤S3包括子步骤：子步
骤S31，针对拍摄的图像进行边缘提取，以获得边缘图像；子步骤S32，针对边缘图像进行筛
选，以获得封闭的轮廓曲线；子步骤S33，对封闭的轮廓曲线进行多边形拟合，并将与辅助边
框的轮廓在尺寸和形状均相同的封闭的轮廓曲线确定为辅助边框；子步骤S34，基于辅助边
框确定辅助边框内的区域为二维码区域；子步骤S35，基于确定的辅助边框以及确定的二维
码区域，计算成像设备相对于二维码区域的相对位置和相对方向角；子步骤S36，利用二维
码扫描程序对拍摄的图像在二维码区域内进行扫描，以扫描二维码，并基于二维码编码规
则对扫描后的二维码进行解码和校验，以获得二维码的绝对坐标；以及子步骤S37，基于子
步骤S35计算出的成像设备的相对位置和相对方向角和子步骤S36获得的二维码的绝对坐
标，经坐标系转换，获得成像设备的绝对位置和绝对方向角，作为修正机器人位置的视觉导
航数据。

[0039] 在根据本发明第三方面的用于机器人的视觉‑惯性组合导航方法中，在步骤S1中，辅助边框可为方形。

[0040] 在根据本发明第三方面的用于机器人的视觉‑惯性组合导航方法中，在步骤S2中，成像设备可为摄像机，当然不限于此，可以采用任何具有拍摄功能的设备。

[0041] 在根据本发明第三方面的用于机器人的视觉‑惯性组合导航方法中，在步骤S2中，成像设备设置在机器人的底部且镜头的轴线垂直于地面，以使成像设备正对铺设在地面上
的外围带有辅助边框的二维码进行拍摄，从而获得垂直拍摄的图像。

[0042] 在根据本发明第三方面的用于机器人的视觉‑惯性组合导航方法中，在子步骤S31中，使用canny算子对图像进行卷积得到边缘灰度图(其中，canny算子参见：http://
baike.baidu.com/link？url＝UEQx23cOWV2HEMdSxRF8Ndzns98piUlmawtPCVECgpm2VfcdNX
ipCdfg_3_UyMCtZGlm8g7cxcJES3e41erbRq)，再根据规定的阈值对边缘灰度图进行二值化，
从而得到二值化的边缘图像；在子步骤S32中，针对二值化的边缘图像，进行轮廓提取，得到
封闭的轮廓，并将得到的封闭轮廓存储；在子步骤S33中，使用Ramer–Douglas–Peucker算法
对轮廓曲线进行多边形拟合，以确定辅助边框；在子步骤S35中，辅助边框的内周或外周的
顶点的图像坐标，计算出成像设备的光心相对于二维码区域的中心的相对位置和相对方位
角，以作为成像设备的相对位置和相对方位角，计算过程为：根据辅助边框的内周或外周的
顶点的图像坐标计算得到辅助边框中心的图像像素坐标，该图像像素坐标乘以比例因子即
为成像设备的光心相对于二维码区域的中心的相对位置，其中比例因子为k＝行长度/行像
素个数；由辅助边框中心点与图像中心点组成一条直线，计算该直线与竖直方向的夹角，即
为成像设备的光心相对于二维码区域的中心的相对方位角。

[0043] 其中，轮廓提取参见文献参见：铃木智，基于边界跟踪的二值化图像的拓扑结构分析，《计算机视觉、图形与图像处理》，30(1)，1985，32‑46(Suzuki,Satoshi."Topological 
structural analysis of digitized binary images by border following."Computer 
Vision,Graphics,and Image Processing 30,no.1(1985):32‑46)；Ramer–Douglas–
Peucker算法，参见：http://en.wikipedia.org/wiki/Ramer％E2％80％93Douglas％E2％
80％93Peucker_algorithm。

[0044] 在根据本发明第三方面的用于机器人的视觉‑惯性组合导航方法中，在一实施例中，在子步骤S37中，坐标系转换为：设二维码的绝对位置为(x1，y1)，绝对方向角为θ，设成
像设备的相对位置数据(x1’，y1’)，绝对方向角为θ’，则成像设备的绝对位置为(x1+x1’，y1
+y1’)，绝对方向角为θ+θ’。

[0045] 在根据本发明第三方面的用于机器人的视觉‑惯性组合导航方法中，在一实施例中，在步骤S1中，辅助边框为方形；在步骤S1中，二维码为QR码，QR码包含有三个小正方形，
所述三个小正方形为QR本身的位置探测图形；在子步骤S34中，还利用二维码本身的位置探
测图形进行二维码区域的校验：在子步骤S34中，基于辅助边框确定辅助边框内的区域为二
维码区域后，再利用子步骤S33中获得封闭的轮廓曲线，当存在有三个封闭的轮廓曲线与三
个小正方形的轮廓在尺寸和形状上均相同时，校验该二维码区域的确定正确。

[0046] 在根据本发明第三方面的用于机器人的视觉‑惯性组合导航方法中，在一实施例中，在子步骤S35和子步骤S36之间还可包括子步骤：基于确定的二维码区域并通过透视变
换获得正的二维码图像。在一实施例中，透视变换为：将包含二维码区域的辅助边框的顶点
对应到一个正多边形区域得到单应性矩阵，之后再根据这个单应性矩阵进行透视变换得到
正的二维码图像，从而采用透视变换将二维码的图像转换成正的形状。

[0047] 在根据本发明第三方面的用于机器人的视觉‑惯性组合导航方法中，在一实施例中，在一实施例中，在步骤S4中，利用设置在机器人上的编码器提供的编码器信息和惯性导
航系统的陀螺仪提供的陀螺仪信息，确定机器人相对机器人的当前起始点的相对位置和相
对机器人的当前起始方向角的相对方向角，机器人的相对方向角用θd表示：

[0048] 1)从编码器估计机器人方向角

[0049] 用θe(k)和θe(k‑1)分别表示在k时刻和(k‑1)时刻从编码器信息估计的机器人角度值；dθr(k)和dθl(k)分别表示右驱动轮编码器和左驱动轮编码器的角度增量，θe(k)可由
下式计算：

[0050]

[0051] 其中ne(k)为编码器角度测量误差，由对编码器脉冲数计数误差引起，是一个零均值高斯白噪声；Rd为驱动轮半径；b为驱动轮之间沿轴线的距离；r为电机减速比；

[0052] 2)从陀螺仪估计机器人方向角

[0053] 陀螺仪是角速度传感器，通过对陀螺仪数据的积分，得到机器人相对于初始位置所转过的角度，用θg(k)及θg(k‑1)分别表示k时刻和(k‑1)时刻从陀螺仪数据积分得到的机
器人方向角， 表示陀螺仪的角速度，T为积分周期，则从θg(k‑1)到θg(k)的一步更新公
式为：

[0054]

[0055] 其中ng(k)为陀螺仪角度估计中的随机误差，由陀螺仪的随机漂移引起；

[0056] 3)相对角度的确定

[0057] 基于从编码器估计的机器人方向角θe(k)和从陀螺仪估计的机器人方向角θg(k)，确定机器人相对机器人的当前起始点的相对方向角，假设零均值高斯白噪声过程ne(k)和ng
(k)的协方差分别为σe和σg，则：

[0058]

[0059] 在根据本发明第三方面的用于机器人的视觉‑惯性组合导航方法中，在一实施例中，在一实施例中，在步骤S4中，航位推测方法融合机器人的相对方向角和里程信息，从机
器人的初始位置开始推算出机器人相对机器人的当前起始点的相对位置，对机器人定位系
统做如下约定：

[0060] 1)机器人在绝对坐标系的位置和方向表示为状态向量(x,y,θ)；

[0061] 2)机器人两驱动轮的轴线中心点代表机器人所在的位置；

[0062] 3)机器人车头方向代表机器人的正方向；

[0063] 为了得到机器人相对机器人的当前起始点的相对位置，且便于数据处理，使用微元累加的方式，将机器人的行动曲线视为多段微小的直线组成的，从机器人的初始位置不
断累加；

[0064] 机器人由一个向量表示(参照图3)，显示了从在(k‑1)时刻的点A(x(k‑1),y(k‑1))行走到k时刻的点A′(x(k),y(k))，点A(x(k‑1),y(k‑1))定义为机器人的当前起始点，角度
从θ(k‑1)增加到θ(k)的状态变化，Δx,Δy,Δθ分别表示在惯性导航的一个程序循环时间
周期T内机器人的横、纵坐标和方向角的增加量；Δl为点A到A′的直线距离；Δs为机器人从
点A到A′行走的实际距离，可以从驱动轮编码器的脉冲增量转换而来，由图3所知，Δx,Δy
可由如下公式计算得到：

[0065]

[0066]

[0067] 由于从点A到A′的时间间隔T很短，Δl与Δs可以近似相等，则：

[0068]

[0069]

[0070] 这样，从机器人的初始位置的坐标(x(0)，y(0))开始，机器人的惯性导航的每个程序循环时间周期T都在上一个周期的机器人坐标(x(k‑1),y(k‑1))的基础上，计算一次坐标
更新(x(k),y(k))，(x(k),y(k))即为机器人相对机器人的当前起始点的相对位置，而(x(k‑
1),y(k‑1))的计算需要从机器人的初始位置的坐标(x(0)，y(0))开始，其中机器人的初始
坐标(x(0)，y(0))是指在机器人加电后开始工作的初始时刻的绝对坐标位置，程序循环时
间周期T是指惯性导航每隔一个固定的时间T进行一次惯性导航计算，惯性导航计算的过程
是一个等时间间隔的无限循环过程。

[0071] 在根据本发明第三方面的用于机器人的视觉‑惯性组合导航方法中，在一实施例中，在步骤S5中，规定k为离散化时间变量，Xa(k)为k时刻子步骤S37获得的成像设备的绝对
位置的坐标，Xd(k)为k时刻步骤S4确定的机器人相对机器人的当前起始点的相对位置的坐
标，经过绝对位置和相对位置融合后得到的机器人坐标为X(k)，采用卡尔曼滤波方法(参见
http://baike.haosou.com/doc/3054305‑3219642.html)进行数据融合，计算步骤如下：

[0072] 1)计算一步最优估计值 其为通过航位推测得到的相对位置Xd(k)，即：

[0073]

[0074] 一步最优估计值 的协方差矩阵 可以通过下面的递推公式计算：

[0075]

[0076] 其中 为k‑1时刻的最优估计 的协方差矩阵，Q(k‑1)为过程噪声的协方差矩阵，为一个对角矩阵；

[0077] 2)计算误差增益K(k)

[0078]

[0079] 其中R(k)为二维码视觉测量噪声的对角协方差矩阵，在步骤S36中校验二维码的过程中，通过统计的方法来确定；

[0080] 3)机器人的位置融合计算

[0081]

[0082] 更新误差增益矩阵

[0083]

[0084] 其中Xa(k)为k时刻子步骤S37获得的成像设备的绝对位置的坐标，即Xa(k)＝(xa(k),ya(k))，I为单位矩阵；

[0085] 令 即得到经过绝对位置和相对位置融合后的机器人坐标，并使以消除步骤S4中机器人相对起始点的相对位置的累计误差。

[0086] 在根据本发明第三方面的用于机器人的视觉‑惯性组合导航方法中，在一实施例中，在步骤S6中，对成像设备的绝对方向角和机器人的相对方向角进行估计以得到机器人
的绝对方向角的计算步骤如下：

[0087] 假设对应当前起始点的k时刻，机器人的绝对方向角用θ表示，在步骤S4中通过编码器和惯性导航系统得到机器人相对机器人的当前起始点的相对方向角为θr(k)，在步骤
S37中成像设备的绝对方向角为θa(k)，θr(k)和θa(k)的误差模型分别为零均值高斯白噪声
过程ne(k)和ng(k)，ne(k)和ng(k)的协方差分别为σe和σg，则：

[0088]

[0089] 在本发明的二维码以及用于机器人的视觉‑惯性组合导航系统及方法中，采用外围带有封闭的辅助边框的二维码，能有效加快二维码区域的筛选效率、成像设备的绝对位
置和绝对方向角的计算效率；由于在地面上铺设多个外围带有封闭的辅助边框的二维码，
在机器人行进过程中，通过设置在机器人上的成像设备拍摄机器人在其行进路线上经过的
铺设在地面上的外围带有辅助边框的二维码的图像，之后经过计算得到机器人的绝对位置
和绝对方向角，并利用得到机器人的该绝对位置和绝对方向角作为惯性导航系统确定机器
人的下一起始点和下一起始方向角，从而可以在机器人行进过程中每拍摄到经过的外围带
有辅助边框的二维码的图像，就进行如此处理，实时地修正惯性导航的漂移，以更可靠地以
视觉/惯性组合的方式实现机器人的高精度实时导航。