一种移动机器人3D定位系统的制作方法

本发明涉及机器人标定的技术领域，特别是一种移动机器人3d定位系统。

背景技术

移动机器人技术是目前机器人行业的研究热点，随着移动机器人的成本普及，其应用必将呈现爆发式增长。由于应用场景和领域不同，不同移动机器人的硬件设备、驱动方式和控制系统均有较大差别，算法通用性和代码复用性受到限制。

定位是导航的基本环节，实时精确的定位是提高移动机器人导航性能的关键。目前，能够装载于移动机器人设备上的传感器种类众多，广泛采用的传感器有里程计、惯性导航模块、gps、激光雷达、kinect、路由器等，这些传感器的数据和使用方法不尽相同，能够提供的移动机器人的运动状态信息也有较大差别。此外，由于单一传感器能够提供的信息准确度和可信度不高，多传感器信息融合成为移动机器人定位发展的大趋势。

移动机器人的运动信息，不仅可以由陀螺仪等测量运动状态的传感器测量，还可以由kinect等传感器的数据经过处理之后得到的视觉里程计信息进行估计；类似的，激光雷达可以类比gps提供移动机器人的全局位姿估计，相当于一个里程计。此外，不同平台采用的算法各有优劣和适用环境，且目前多机分布控制和分布式计算逐渐取得越来越广泛的应用，除了对多种类型的传感器信息进行融合外，如果能够融合不同的算法结果进行移动机器人的定位，显然效果会更上一层。

技术实现要素：

本部分的目的在于概述本发明的实施例的一些方面以及简要介绍一些较佳实施例。在本部分以及本申请的说明书摘要和发明名称中可能会做些简化或省略以避免使本部分、说明书摘要和发明名称的目的模糊，而这种简化或省略不能用于限制本发明的范围。

鉴于上述和/或现有技术中存在的缺陷，提出了本发明。

因此，本发明其中的一个目的是提供一种移动机器人3d定位系统，其不仅能够融合各种传感器数据，还能进行不同算法融合的定位系统框架。

为解决上述技术问题，本发明提供如下技术方案：一种移动机器人3d定位系统，包括，同步定位建图单元和导航单元，所述同步定位建图单元和所述导航单元之间可以切换，所述同步定位建图单元是所述导航单元的2d位姿，所述导航单元显示所述同步定位建图单元的角度和初始位姿；所述导航单元包括算法，所述算法进行信息融合，所述信息包括多种传感器的输入和所述同步定位建图单元算法结果融合；所述算法结果融合，先将移动机器人在空间所处的状态用15维向量x进行描述：

x＝[x,y,z,θr,θp,θy,vx,vy,vz,ωr,ωp,ωy,ax,ay,az]^t

其中，x，y，z分别代表三维位置，θr,θp,θy表示各个位置的角度，vx，vy，vz分别代表各个位置的线速度，wr，wp，wy分别代表各个位置的角速度，ax，ay，az分别代表各个位置的加速度。

作为本发明移动机器人3d定位系统的一种优选实施方案，其中：所述同步定位建图单元包括移动机器人的定位和坐标设置；所述定位，通过全局定位对移动机器人的当前位置进行准确估计，再通过相对定位方式对其运动状态进行预测，并不断修正；所述坐标，其包括世界坐标系、里程计坐标系、移动机器人中心坐标系、四个驱动轮的坐标系和各个传感器坐标系，所述里程计坐标系由安装在所述四个驱动轮上的编码器计算里程计数据，从而确定其相对于世界坐标系的相对关系。

作为本发明移动机器人3d定位系统的一种优选实施方案，其中：所述移动机器人以所述移动机器人中心坐标系为参考系，所述移动机器人在所述世界坐标系中与其当前θr,θp,θy和当前所处平面有关；其中，以s表示函数sin，c表示函数cos，r、p、y分别表示所述移动机器人中心坐标系相对于所述世界坐标系绕x、y、z轴旋转的角度；若通过传感器配置和相关算法，已经获得15维向量x的所有成员，将移动机器人在所述移动机器人中心坐标系中各个方向上的运动向世界坐标系进行投影，在采样时间为δt时，可得融合后的预测。

作为本发明移动机器人3d定位系统的一种优选实施方案，其中：确定系统的模型，所述系统的模型是将融合后的预测的式子整理成矩阵形式得到。

作为本发明移动机器人3d定位系统的一种优选实施方案，其中：所述系统的模型求偏导数，得到算法迭代所需雅各比矩阵并得到十五维空间上，传感器和算法结果的融合过程。

作为本发明移动机器人3d定位系统的一种优选实施方案，其中：所述四个驱动轮的坐标系包括，后右轮链接、后左轮链接、前右轮链接和前左轮链接。

作为本发明移动机器人3d定位系统的一种优选实施方案，其中：所述同步定位建图单元还包括数据处理模块、扫描匹配模块和构图。

作为本发明移动机器人3d定位系统的一种优选实施方案，其中：所述移动机器人采用多机分布式控制和分布式计算。

作为本发明移动机器人3d定位系统的一种优选实施方案，其中：所述分布式计算包括第一处理器、第二处理器、传感器、供电系统和驱动执行件；其中，所述供电系统给所述驱动执行件、第二处理器和传感器供电，所述第二处理器进行硬件驱动、信息采集以及数据转换后输送至驱动执行件和传感器，以及第一处理器，所述传感器进行激光测距、姿态测量和速度检测，所述第一处理器进行人机交互、运动控制、数据处理和规划决策。

作为本发明移动机器人3d定位系统的一种优选实施方案，其中：所述分布式控制包括微控制器、嵌入式平台和pc决策计算。

本发明的有益效果：本发明在进行信息融合时，不仅能够灵活配置传感器，还能灵活配置采用的算法，进行信息融合时，不仅能够灵活配置传感器，还能灵活配置采用的算法，能够进行3d位姿估计，适应三维环境。

附图说明

为了更清楚地说明本发明实施例的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其它的附图。其中：

图1为本发明移动机器人3d定位系统一个实施例中定位系统框架整体结构示意图；

图2为本发明移动机器人3d定位系统一个实施例中所述移动机器人的定位分类的结构图；

图3为本发明移动机器人3d定位系统一个实施例中所述分布式计算的整体结构框架图；

图4为本发明移动机器人3d定位系统一个实施例中所述分布式控制的整体结构框架图。

具体实施方式

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合说明书附图对本发明的具体实施方式做详细的说明。

在下面的描述中阐述了很多具体细节以便于充分理解本发明，但是本发明还可以采用其他不同于在此描述的其它方式来实施，本领域技术人员可以在不违背本发明内涵的情况下做类似推广，因此本发明不受下面公开的具体实施例的限制。

其次，此处所称的“一个实施例”或“实施例”是指可包含于本发明至少一个实现方式中的特定特征、结构或特性。在本说明书中不同地方出现的“在一个实施例中”并非均指同一个实施例，也不是单独的或选择性的与其他实施例互相排斥的实施例。

本发明移动机器人3d定位系统提供的一个实施例，参照图1，该实施例的主体包括同步定位建图单元100和导航单元200，同步定位建图单元100和导航单元200之间可以切换，同步定位建图单元100是导航单元200的2d位姿，导航单元200显示同步定位建图单元100的角度和初始位姿。

导航单元200包括算法201和3d位姿信息202，算法201进行信息融合，所述信息包括多种传感器的输入和所述同步定位建图单元100算法结果融合，并将融合后的结果反馈给3d位姿信息202。

需要说明的是，这里所述的算法结果融合，先将移动机器人在空间所处的状态用15维向量x进行描述：

x＝[x,y,z,θr,θp,θy,vx,vy,vz,ωr,ωp,ωy,ax,ay,az]^t

其中，x，y，z分别代表三维位置，θr,θp,θy表示各个位置的角度，vx，vy，vz分别代表各个位置的线速度，wr，wp，wy分别代表各个位置的角速度，ax，ay，az分别代表各个位置的加速度。虽然移动机器人传感器类型众多，不同的传感器和算法描述的移动机器人状态也不尽相同，但它们提供的位姿估计与导航所需信息都在上述向量之内。

较佳的，同步定位建图单元100包括移动机器人的定位和坐标设置；所述定位，通常，可以将移动机器人的位姿计算方法分为两大类，参照图2，一类是移动机器人的初始位置信息已知，通过惯性导航和测程法等进行其相对位置的推算，我们称之为相对定位方法。这类的算法具有显而易见的缺点，那就是由于误差的累积，随着时间的推移，其准确度越来越低，甚至不再可用。但是如果对移动机器人的实时位置能够有所了解的话，采用该方法可以对其运动状态达到相对精确的估计，从而实现较好的定位效果。全局定位的出现则是为了解决移动机器人实时位置如何确定这个问题的。称移动机器人初始位置未知的情况下，通过外部传感器的数据进行定位的这类定位方式为全局定位。毋庸置疑，任何定位方式都有其优缺点和适用场合。虽然全局定位能够解决相对定位的一些问题，但是全局定位由于运算效率以及数据跳跃性大等问题，也有其适用范围和局限性。

组合定位逐渐成为移动机器人进行定位的主要方式，组合定位，就是将上述两种定位方式结合起来进行定位的一种方法。通过全局定位对移动机器人的当前位置进行准确估计，再通过相对定位方式对其运动状态进行预测，并不断进行修正，从而使移动机器人运动模型简化，达到其最优估计。

坐标变换是移动机器人进行定位时的重要模块。在本实施例中，移动机器人的坐标设置采用四轮差分驱动，其包括世界坐标系(map)、里程计坐标系(odom)、移动机器人中心坐标系(baselink)、四个驱动轮的坐标系和各个传感器坐标系，例如：激光雷达(laser)、九轴imu模块(imu)。

需要说明的是，所述四个驱动轮的坐标系包括，后右轮链接(rearrightwheellink)、后左轮链接(rearleftwheellink)、前右轮链接(frontrightwheellink)和前左轮链接(frontleftwheellink)。

需要说明的是，所述里程计坐标系由安装在所述四个驱动轮上的编码器计算里程计数据，从而确定其相对于世界坐标系的相对关系。由于滑移的存在，里程计信息与移动机器人实际所处位置往往有较大差异，这一部分差异由里程计坐标系到车体中心坐标系的变换进行估计和维持。根据全局传感器的信息和内部传感器的惯性导航推算里程计坐标系到车体中心坐标系变换的过程称为航迹推算，参照图3。

在本实施例中，移动机器人是典型的非线性系统，采用算法对其进行位置和姿态的估计，优选的，在本实施例中，采用ekf算法进行位置和姿态的估计。ekf(extendedkalmanfilter)即扩展卡尔曼滤波器，一种高效率的递归滤波器(自回归滤波器)，它能够从一系列的不完全包含噪声的测量中，估计动态系统的状态。ekf的基本思想是将非线性系统线性化，然后进行卡尔曼滤波，因此ekf是一种次优滤波。其后，多种二阶广义卡尔曼滤波方法的提出及应用进一步提高了卡尔曼滤波对非线性系统的估计性能。二阶滤波方法考虑了taylor级数展开的二次项，因此减少了由于线性化所引起的估计误差，但大大增加了运算量，因此在实际中反而没有一阶ekf应用广泛。

本实施例中，在15维向量x上进行移动机器人的信息融合，因此必须考虑移动机器人相对于水平面的翻转角和俯仰角对其运动造成的影响。移动机器人以所述移动机器人中心坐标系为参考系，所述移动机器人在所述世界坐标系中与其当前θr,θp,θy和当前所处平面有关，以s表示函数sin，c表示函数cos，r、p、y分别表示所述移动机器人中心坐标系相对于所述世界坐标系绕x、y、z轴旋转的角度；若通过传感器配置和相关算法，已经获得15维向量x的所有成员，将移动机器人在所述移动机器人中心坐标系中各个方向上的运动向世界坐标系进行投影，在采样时间为δt时，可得融合后的预测；

将三式整理成矩阵形式，得到系统的模型：

其中，

对系统的模型求偏导数，得到ekf算法迭代所需雅各比矩阵移动机器人3d位姿融合过程下：

至此，在十五维空间上进行传感器和算法结果的融合就完成了。

较佳的，同步定位建图单元100还包括数据处理模块101、扫描匹配模块102和构图103。因为目前移动机器人平台众多，传感器配置千差万别，能够动态的配置传感器输入是算法通用的基本要求，而环境地图构建依赖于激光雷达，因此激光雷达的数据处理模块也必不可少。参照图1，由激光雷达先给出信号到数据处理模块101，通过数据处理模块101处理好后到扫描匹配模块102，到构图103，且构图103和匹配模块102是双向传输的。

导航单元200采用ekf算法融合2d位姿和多种传感器输入，得到移动机器人的3d位姿估计。显然对于任何能提供3d位姿估计的算法，都可以接入本平台参与数据融合，而不必拘泥于算法种类，且任何能提供运动信息的传感器，均能参与数据融合。

本实施例中的激光雷达由于功能限制，只能进行2d位姿估计，2d位姿在2d同步定位建图单元100中进行计算，可以采用任何能够提供位姿估计信息并且可以进行地图构建的算法。对三维激光雷达，则可以将2d位姿估计算法更换为相应的三维算法。

较佳的，算法201进行融合的信息还包括imu、gps、里程计和其他传感器和算法。

较佳的，移动机器人采用多机分布式控制和分布式计算。参照图3，分布式计算包括第一处理器301、第二处理器302、传感器303、供电系统304和驱动执行件305。其中，供电系统304给驱动执行件305、第二处理器302和传感器303供电，第二处理器302进行硬件驱动、信息采集以及数据转换后输送至驱动执行件305、传感器303和第一处理器301，传感器303进行激光测距、姿态测量和速度检测，第一处理器301进行人机交互、运动控制、数据处理和规划决策，驱动执行件305内设有电机控制器、电机驱动器和驱动电机。

较佳的，参照图4，分布式控制包括微控制器、嵌入式平台和pc决策计算。微控制器根据上位机给出的指令对电机进行伺服闭环控制，并按一定频率采集编码器，并反馈信息给上位机。嵌入式平台根据pc机的驱动信息将指令发送给微控制器，并按一定频率获取微控制器反馈的编码器数值转发给pc机，通过总线和imu模块进行通讯，采集相应信息发送给pc机，同时通过串口速去传感器的数据发送给pc机。pc机主要包括机器人的运动学控制、运动学逆解以及姿态解算等基本模块和滤波、地图构建与导航等复杂算法模块，也包括人机交互界面。

pc决策计算外接wifi模块、rj-45接口和usb2.0口。嵌入式平台外接wifi模块、rj-45接口、usb2.0口、rs232接口和iic总线，其中i²c总线连接加速度计、陀螺仪和电子罗盘。微控制器外接rs232接口、编码器结构、pwm输出io和普通io，微控制器还与旋转编码器和电机驱动器相连接，其中电机驱动器、电机和旋转编码器形成串联。pc决策计算与嵌入式平台连接时，通过wifi模块或者rj-45接口，微控制器和嵌入式平台通过rs232接口或者ttl相连接。

需要说明的是，在本实施例中，还设有电源，电源分别给嵌入式平台、微控制器和电机驱动器供电。

应说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的精神和范围，其均应涵盖在本发明的权利要求范围当中。