本发明属于环境建图领域，具体涉及一种基于平面栅格模型的机器人三维建图方法及系统。
背景技术：
环境建图对于移动机器人的应用有着重要的作用，其可为移动机器人的应用带来如环境探测，实时定位、路径规划等功能。随着机器人技术的发展，其在机器人应用具有广阔的发展前途。
目前，公知的机器人自动建图的方法有二维和三维建图，但二维建图提供的环境信息量少，难以构造复杂的环境模型，使机器人工作在复杂的环境时有一定安全隐患。三维建图方法有点云模型和栅格模型，点云模型的缺点是计算机内存消耗巨大，对处理器的性能要求太高，从而使处理器的体积也受到限制，无法使用体积小巧的处理系统。栅格模型是目前应用最广的建图方法，但现有的方法中绝大部分采用等尺寸的体素模型，内存消耗仍然较大，少量的方法采用变尺寸栅格，但建模的效率不高。同时现有的建图系统实现一般采用通用计算机系统，其体积较大，嵌入应用受到限制，移植性和移动性均较差。
技术实现要素：
本发明的目的在于针对现有技术的缺陷，提供一种基于平面栅格模型的机器人三维建图方法及系统，可以实现移动机器人更高效更精确地对工作环境进行三维建图，同时减小了系统体积，提高系统的移植性。
本发明的技术方案如下：
一种基于平面栅格模型的机器人三维建图方法，包括如下步骤:
S1、移动机器人通过两个摄像头获取周围环境的两路视频信息，以向数字信号处理系统传输所述两路视频信息；
S2、所述数字信号处理系统接收所述两路视频信息，并通过数字信号处理器对两路视频信息进行特征提取，得到特征点；
S3、所述数字信号处理系统利用立体视觉匹配算法对提取出的特征点进行演算，得到视觉特征点，并将所述视觉特征点储存至数据储存单元；
S4、所述数字信号处理器使用Multi-RANSAC算法对所述视觉特征点进行迭代运算，估算出所述视觉特征点所满足的多个特征平面的参数；
S5、所述数字信号处理器利用所述特征平面将所述视觉特征点划分成平面点云数据集和非平面点云数据集；
S6、所述数字信号处理器对所述平面点云数据集和非平面点云数据集分别进行四叉树结构划分至预设的尺寸，形成平面栅格；
S7、所述数字信号处理器将所述平面栅格沿平面的法向延伸预设的高度形成三维栅格，得到周围环境的三维模型。
进一步的，所述立体视觉匹配算法为动态规划算法。
进一步的，所述步骤S4的具体步骤为：
S41、所述数字信号处理器使用Multi-RANSAC算法，随机选取部分所述视觉特征点作为样本计算出多个特征平面模型，将全部所述视觉特征点分别与任一特征平面模型进行匹配；
S42、在所述视觉特征点与所述任一特征平面模型匹配，且匹配数量达到预设数据阈值的情况下，得到对应的特征平面；
S43、按照预设的执行次数重复步骤S41～S42。
进一步的，所述步骤S5的具体步骤为：
S51、所述数字信号处理器分别计算全部所述视觉特征点到任一特征平面的距离；
S52、当所述视觉特征点到所述任一特征平面的距离小于预设的距离阈值时，将所述视觉特征点归类为所述任一特征平面的平面点云数据集；否则，将所述视觉特征点归类为所述非平面点云数据集。
进一步的，所述步骤S6中所述数字信号处理器通过如下步骤将所述平面点云数据集内的视觉特征点点云数据形成平面栅格：
S61、对所述平面点云数据进行四叉树结构划分形成平面栅格
S62、将所述平面点云数据集内的所述视觉特征点点云数据形成的平面栅格合并，组成大尺寸的平面栅格。
本发明同时公开了一种基于平面栅格模型的机器人三维建图系统，包括移动机器人和数字信号处理系统；所述移动机器人包括：
通讯模块，用于与所述数字信号处理系统建立连接并传输数据及指令；
马达驱动器，用于驱动移动机器人移动；
两个摄像头，对周围环境进行摄取而得到两路视频信息；
所述数字信号处理系统，包括：
通讯模块，用于与所述移动机器人建立连接并传输数据及指令；
数字信号处理器，用于对所述两路视频信息进行特征提取，并将提取出的特征点进行立体匹配，从而得到视觉特征点，并处理所述视觉特征点形成环境三维模型；
数据缓冲单元，用于储存所述视觉特征点。
进一步的，所述数字信号处理器使用Multi-RANSAC算法对所述视觉特征点进行迭代运算，估算出所述视觉特征点所满足的多个特征平面的参数；利用所述特征平面将所述视觉特征点划分成平面点云数据集和非平面点云数据集；对所述平面点云数据集和非平面点云数据集分别进行四叉树结构划分至预设的尺寸，形成平面栅格；将所述平面栅格沿平面的法向延伸预设的高度形成三维栅格，得到周围环境的三维模型。
进一步的，所述数字信号处理器估算出所述视觉特征点所满足的多个特征平面的参数的具体方式为：
S41、所述数字信号处理器使用Multi-RANSAC算法，随机选取部分所述视觉特征点作为样本计算出多个特征平面模型，将全部所述视觉特征点分别与任一特征平面模型进行匹配；
S42、在所述视觉特征点与所述任一特征平面模型匹配，且匹配数量达到预设数据阈值的情况下，得到对应的特征平面；
S43、按照预设的执行次数重复步骤S41～S42。
进一步的，所述数字信号处理器利用所述特征平面将所述视觉特征点划分成平面点云数据集和非平面点云数据集的具体方式为：
所述数字信号处理器分别计算全部所述视觉特征点到任一特征平面的距离；
当所述视觉特征点到所述任一特征平面的距离小于预设的距离阈值时，将所述视觉特征点归类为所述任一特征平面的平面点云数据集；否则，将所述视觉特征点归类为所述非平面点云数据集。
进一步的，所述数字信号处理器通过如下步骤将所述平面点云数据集内的视觉特征点进行四叉树结构划分至预设的尺寸，形成平面栅格：
所述数字信号处理器对所述平面点云数据进行四叉树结构划分形成平面栅格
所述数字信号处理器将所述平面点云数据集内的所述视觉特征点点云数据形成的平面栅格合并，组成大尺寸的平面栅格。
与现有技术相比，本发明的有益技术效果如下：
本发明提供了一种基于平面栅格模型的机器人三维建图方法及系统，采用基于平面栅格模型的三维建模方法，结合嵌入式系统，实现移动机器人更高效更精确地对工作环境进行三维建图，同时由于系统尺寸较小且易嵌入其它系统，增加了其使用的多样性及简便性。
附图说明
图1是本发明实施例1的一种基于平面栅格模型的机器人三维建图方法的流程示意图。
图2是本发明实施例2的一种基于平面栅格模型的机器人三维建图系统的结构示意图。
具体实施方式
为了充分地了解本发明的目的、特征和效果，以下将结合附图对本发明的几种优选的实施方式进行说明。
实施例1
如图1所示，本实施例1公开了一种基于平面栅格模型的机器人三维建图方法，包括如下步骤:
S1、移动机器人通过两个摄像头获取周围环境的两路视频信息，以向数字信号处理系统传输两路视频信息；
S2、数字信号处理系统接收两路视频信息，并通过数字信号处理器对两路视频信息进行特征提取，得到特征点；
S3、数字信号处理系统利用立体视觉匹配算法对提取出的特征点进行演算，得到视觉特征点，并将视觉特征点储存至数据储存单元；
具体的，步骤S3中的立体视觉匹配算法为动态规划算法。
S4、数字信号处理器使用Multi-RANSAC算法对视觉特征点进行迭代运算，估算出视觉特征点所满足的多个特征平面的参数；
具体的，步骤S4的具体步骤为：
S41、数字信号处理器使用Multi-RANSAC算法，随机选取部分视觉特征点作为样本计算出多个特征平面模型，将全部视觉特征点分别与任一特征平面模型进行匹配；
S42、在视觉特征点与任一特征平面模型匹配，且匹配数量达到预设数据阈值的情况下，得到对应的特征平面；
S43、按照预设的执行次数重复步骤S41～S42。
具体的，特征平面模型的参数方程为：
fi(x,y,z)＝aix+biy+ciz+di；其中ai,bi,ci,di分别为第i个平面的参数；
其中θi分别为第i个平面与地平面的夹角；
S5、数字信号处理器利用S4得到的特征平面将视觉特征点划分成平面点云数据集和非平面点云数据集；
具体的，步骤S5的具体步骤为：
S51、数字信号处理器分别计算全部视觉特征点到任一特征平面的距离；
S52、当视觉特征点到任一特征平面的距离小于预设的距离阈值时，将视觉特征点归类为任一特征平面的平面点云数据集；否则，将视觉特征点归类为非平面点云数据集。
具体的，数字信号处理器计算视觉特征点到特征平面的距离所用的公式为：
其中d为距离阀值；
S6、数字信号处理器对平面点云数据集和非平面点云数据集分别进行四叉树结构划分至预设的尺寸，形成平面栅格；
具体的，步骤S6中数字信号处理器通过如下步骤将平面点云数据集内的视觉特征点点云数据形成平面栅格：
S61、对平面点云数据进行四叉树结构划分形成平面栅格
S62、将平面点云数据集内的视觉特征点点云数据形成的平面栅格合并，组成大尺寸的平面栅格。
S7、数字信号处理器将平面栅格沿平面的法向延伸预设的高度形成三维栅格，得到周围环境的三维模型。
实施例2
如图2所示，本实施例公开了一种基于平面栅格模型的机器人三维建图系统，其包括移动机器人和数字信号处理系统；移动机器人包括：
通讯模块1，用于与数字信号处理系统建立连接并传输数据及指令；
马达驱动器2，用于驱动移动机器人移动；
两个摄像头3，对周围环境进行摄取而得到两路视频信息；
数字信号处理系统为嵌入式系统，包括：
通讯模块4，用于与移动机器人建立连接并传输数据及指令；
数字信号处理器5，用于对两路视频信息进行特征提取，并将提取出的特征点进行立体匹配，从而得到视觉特征点，并处理视觉特征点形成环境三维模型；
数据缓冲单元6，用于储存视觉特征点。
具体的，数字信号处理器5使用Multi-RANSAC算法对视觉特征点进行迭代运算，估算出视觉特征点所满足的多个特征平面的参数；利用特征平面将视觉特征点划分成平面点云数据集和非平面点云数据集；对平面点云数据集和非平面点云数据集分别进行四叉树结构划分至预设的尺寸，形成平面栅格；将平面栅格沿平面的法向延伸预设的高度形成三维栅格，得到周围环境的三维模型。
具体的，数字信号处理器5估算出视觉特征点所满足的多个特征平面的参数的具体方式为：
S41、数字信号处理器使用Multi-RANSAC算法，随机选取部分视觉特征点作为样本计算出多个特征平面模型，将全部视觉特征点分别与任一特征平面模型进行匹配；
S42、在视觉特征点与任一特征平面模型匹配，且匹配数量达到预设数据阈值的情况下，得到对应的特征平面；
S43、按照预设的执行次数重复步骤S41～S42。
具体的，特征平面模型的参数方程为：
fi(x,y,z)＝aix+biy+ciz+di；其中ai,bi,ci,di分别为第i个平面的参数；
其中θi分别为第i个平面与地平面的夹角；
具体的，数字信号处理器5利用特征平面将视觉特征点划分成平面点云数据集和非平面点云数据集的具体方式为：
数字信号处理器分别计算全部视觉特征点到任一特征平面的距离；
当视觉特征点到任一特征平面的距离小于预设的距离阈值时，将视觉特征点归类为任一特征平面的平面点云数据集；否则，将视觉特征点归类为非平面点云数据集。
具体的，数字信号处理器计算视觉特征点到特征平面的距离所用的公式为：
其中d为距离阀值；
具体的，数字信号处理器5通过如下步骤将平面点云数据集内的视觉特征点进行四叉树结构划分至预设的尺寸，形成平面栅格：
数字信号处理器对平面点云数据进行四叉树结构划分形成平面栅格
数字信号处理器将平面点云数据集内的视觉特征点点云数据形成的平面栅格合并，组成大尺寸的平面栅格。
通过本发明提供的一种基于平面栅格模型的机器人三维建图方法及系统，采用基于平面栅格模型的三维建模方法，结合嵌入式系统，可实现移动机器人更高效更精确地对工作环境进行三维建图，同时由于系统尺寸较小且易嵌入其它系统，增加了其使用的多样性及简便性。
以上详细描述了本发明的较佳具体实施例，应当理解，本领域的普通技术人员无需创造性劳动就可以根据本发明的构思作出诸多修改和变化。因此，凡本技术领域中技术人员依本发明构思在现有技术基础上通过逻辑分析、推理或者根据有限的实验可以得到的技术方案，均应该在由本权利要求书所确定的保护范围之中。