CN109459039A - 一种医药搬运机器人的激光定位导航系统及其方法 - Google Patents

Abstract

本发明公开了一种医药搬运机器人的激光定位导航系统及其方法，其定位导航系统包括相互电连接的建图模块、导航模块和人机交互模块，其定位导航方法为：通过建图模块扫描医药搬运机器人实时环境信息数据，构建医药搬运机器人工作范围内的环境栅格地图；然后通过设置医药搬运机器人初始三维空间位姿信息，快速准确完成建图模块扫描的实时环境信息与构建的环境栅格地图匹配定位；最后通过导航模块完成医药搬运机器人上下料的工作。本发明不受场地平整度、环境光照、卫星信号等因素的影响，能够在非结构化环境中获取移动平台的三维空间位姿信息，构建对未知环境的栅格地图，并进行精确定位导航，完成医药搬运机器人的上下料工作。

Description

一种医药搬运机器人的激光定位导航系统及其方法

技术领域

本发明涉及激光定位和导航技术领域，尤其涉及一种医药搬运机器人的激光定位导航系统及其方法。

背景技术

目前，仍大量存在依赖人工进行医药制药的现象，GMP标准要求在医药制药方面必须实现无菌化、无人化和自动化，而依赖人工制药导致其生产效率低、安全性差以及产品质量差等问题，特别是在生产过程中，不可或缺地需要进行物料的搬运操作，为了减少人为带来的污染，节约生产成本，现有技术中，采用具有无人、无菌、高自由度特性的机器人取代人工进行物料搬运，同时这也就要求了必须对机器人上下料过程中的进行精确定位与导航。

定位与导航系统在机器人等移动平台中扮演着重要的角色，其中涉及环境感知、路径规划、控制等多项技术，是保证机器人实现其系统功能的重要基础，常见的定位导航方式采用磁条导航、电磁导航、二维码导航等，其中磁条导航容易破损，需定期维护，且无法智能避让；电磁导航铺设导引线困难，扩充路径需重新铺设，结构复杂成本昂贵；二维码导航需定期维护，而且对场地平整度有一定要求，对陀螺仪精度要求高；视觉导航计算量大，消耗资源多，受光照条件影响较大。

因此，在非结构化环境中，如何保证机器人能够不受场地平整度、光照环境、多径效应等因素影响，进行稳定精确的定位和导航是目前亟待解决的一个技术问题。

发明内容

本发明的目的是提供一种能够不受场地平整度、光照环境、多径效应等因素影响的医药搬运机器人激光定位导航系统及其方法。

为了解决上述技术问题，本发明提供一种医药搬运机器人的激光定位导航系统，包括：

建图模块，所述建图模块包括激光传感器和中央处理器，所述激光传感器用于采集所述医药搬运机器人的实时环境信息数据，所述中央处理器用于对所述激光传感器采集的信息数据进行融合处理以获取所述医药搬运机器人的三维空间位姿信息，并构建所述医药搬运机器人工作范围内的环境栅格地图，所述激光传感器与所述中央处理器电连接；

导航模块，与所述中央处理器电连接，用于确定所述医药搬运机器人的三维空间位姿信息，并将所述激光传感器扫描的所述医药搬运机器人实时环境信息与所述中央处理器构建的环境栅格地图进行匹配定位，寻找所述医药搬运机器人工作的最佳路径；

人机交互模块，与所述中央处理器和所述导航模块均电连接，用于可视化显示所述中央处理器构建的环境栅格地图和所述医药搬运机器人所采集的信息数据，以及所述医药搬运机器人工作的最佳路径。

一种医药搬运机器人的激光定位导航方法，采用上述的医药搬运机器人的激光定位导航系统，所述医药搬运机器人的激光定位导航方法包括以下步骤：

S1：所述激光传感器采集所述医药搬运机器人实时环境信息数据，并通过所述中央处理器融合处理以获取所述医药搬运机器人的三维空间位姿信息数据；

S2：通过所述激光传感器获取所述医药搬运机器人的扫描帧数据，所述中央处理器通过对比多帧扫描数据构建局部最优环境子地图，并保存所述S1中所述医药搬运机器人的三维空间位姿信息数据以及与该三维空间位姿信息对应的局部最优环境子地图扫描数据；

S3：重复所述S1和所述S2，直至所述激光传感器完成对所述医药搬运机器人全部工作范围的扫描，实现全局环境栅格地图的构建；

S4：在所述人机交互模块设置所述医药搬运机器人的初始三维空间位姿信息；

S5：通过所述中央处理器将所述S1中所述激光传感器采集的所述医药搬运机器人的实时环境信息数据与所述S3中构建的全局环境栅格地图进行匹配定位；

S6：所述中央处理器根据所述S5的匹配定位信息，寻找所述医药搬运机器人上下料的最佳路径。

作为上述技术的优选方案，所述的S1通过所述中央处理器融合处理以获取所述医药搬运机器人的三维空间位姿信息数据的具体步骤为：

S101、通过卡尔曼滤波融合处理所述中央处理器中的惯性导航单元中加速度计和陀螺仪的相应数据，得到所述医药搬运机器人的三维空间姿态信息，其中所述惯性导航单元中加速度计和陀螺仪的模型表示为：

式(1)中，θ_k为k时刻加速度计的角度，ω_k为k时刻陀螺仪角速度，ω_mk为k时刻陀螺仪的测量值，ω_bk为k时刻陀螺仪的零偏值，W_k-1为过程激励噪声，dt为时间变量Δt；

S102、所述激光传感器采集所述医药搬运机器人的实时环境信息数据并对所采集的信息数据进行投影，得到真实的激光平面数据，根据激光模型[ρ,θ]极坐标变换出障碍物相对于激光平面的坐标P_x，P_y，P_z：

式(2)中，θ_x，θ_y分别为绕x，y轴旋转的角度；

S103、根据所述医药搬运机器人绕轴旋转的角度θ变换信息和初始的位置，由式(2)可得到所述医药搬运机器人的角度θ发生变化后的位置[XYZ]^T：

式(3)中，R_x，R_y，R_z为绕x，y，z轴旋转的角度θ所对应的旋转矩阵，根据已知的障碍物位置信息，由式(3)计算出所述障碍物与水平平面的投影，投影后的所述障碍物激光距离可表示为：

ρ'²＝X²+Y² (4)。

作为上述技术的优选方案，所述S2具体步骤为：

S201、通过所述激光传感器获取扫描帧数据，所述中央处理器将获取的扫描帧数据变换为子地图坐标：

式(5)中，p是激光传感器中激光光束端点的位置，ξ_x，ξ_y，ξ_θ为医药搬运机器人的三维空间位姿信息变化，T_ξ为变换矩阵；

S202、所述中央处理器应用ceres-Solver库构建最优的环境栅格子地图，其公式为：

式(6)中，h_a为所述激光传感器扫描点的集合，A为所述激光传感器360°扫描的数据量，M_smooth为栅格子地图的概率值；

S203、保存所述S1中所述医药搬运机器人的三维空间位姿信息数据以及与该三维空间位姿信息数据对应的最优环境栅格子地图扫描数据。

作为上述技术的优选方案，所述S3具体步骤为：

S301、所述中央处理器采用分支界定法寻找与所述激光传感器获取的扫描帧相对应的历史帧并进行匹配，消除建图过程中累积的误差；

S302、所述中央处理器应用ceres-Solver库选择所述S301中的相应数据，并将相应数据加入到全局环境栅格地图中进行SPA优化，其优化公式可表示为：

式(7)中，是每一子地图的位姿，E^m是的集合，是相对应的激光点集的位姿，Es是的集合，∑_ij是协方差矩阵，ξ_ij是形成子地图与激光点集约束，是在以为原点的坐标系下的偏移，m表示子地图节点，i是第i个子地图节点(i＝1,2,3...m)，s表示扫描帧节点，j是第j个扫描帧节点(j＝1,2,3,....s)，其中 式中表示子地图节点的位置，表示扫描帧节点的位置，是2*2的旋转矩阵，e表示误差项，表示的角度，表示的角度，表示两者之间的角度偏移约束，表示范数意义下误差项e的平方。。

S303、重复所述S301和所述S302，对所述医药搬运机器人工作范围内的所有未知环境进行扫描，完成全局环境栅格地图的构建。

作为上述技术的优选方案，所述S5中的匹配定位采用自适应蒙特卡洛定位方法进行预测和更新，具体步骤为：

S501、根据所述医药搬运机器人的运动状态，计算所述医药搬运机器人的先验概率密度，然后对所述医药搬运机器人的三维空间位姿信息进行预测；

S502、根据所述S303构建的全局环境栅格地图，比较所述医药搬运机器人处于所述S501预测三维空间位姿信息的理想状态下所述激光传感器扫描数据与该预测三维空间位姿信息所述激光传感器实际的扫描数据，根据所述医药搬运机器人预测三维空间位姿信息在理想状态和实际状态下比较的误差大小对所述预测三维空间位姿信息进行判断；

S503、缩小所述S502中比较的误差值，直至所预测三维空间位姿信息与真实三维空间位姿信息之间的误差值≤5cm，即完成所述医药搬运机器人的定位。

作为上述技术的优选方案，所述的S6中寻找并导航所述医药搬运机器人上下料的最佳路径的具体步骤为：

S601、采用A*方法寻找所述医药搬运机器人上下料的最佳路径，通过已知的起点值和终点值，设定合适的启发函数，全面评估和扩展搜索节点的代价值，并通过比较各扩展节点代价值的大小，选择最有希望的点加以扩展，直到找到目标节点为止，其估价函数公式如下：

F(n)＝G(n)+H(n) (8)

式(8)中，n表示医药搬运机器人的当前节点，G(n)表示从起点到当前节点的距离值，H(n)表示从当前节点到终点的距离值；

S602、采用DWA动态窗口方法选取局部避障路径，通过在速度空间[v,w]中对线速度v和角速度w进行多组采样，在采样速度的限制条件下，对一定时间内的所述医药搬运机器人运行轨迹进行预测，然后引入评价函数对所预测的所述医药搬运机器人运行轨迹进行评估，选取所述医药搬运机器人的最优速度和运行轨迹，其评价函数公式如下：

G(υ,ω)＝α·Gdist(υ,ω)+β·Pdist(υ,ω)+γ·vel(υ,ω) (9)

式(9)中，Gdist(ν,ω)表示所述医药搬运机器人航迹推演末端与目标方向的距离，Pdist(ν,ω)表示所述医药搬运机器人航迹推演与全局路径之间的距离，vel(ν,ω)表示当前速度大小，系数α，β，γ表示对其进行归一化处理。

与现有技术比较，本发明通过所述激光传感器扫描获取所述医药搬运机器人周围的实时环境信息数据，构建环境栅格地图，然后通过设置所述医药搬运机器人的初始三维空间位姿信息，将获取的所述医药搬运机器人实时环境信息与构建的环境栅格地图进行匹配定位，最后对所述医药搬运机器人的上下料行程进行导航，本发明通过所述激光传感器中的惯性导航单元获取所述医药搬运机器人的三维空间姿态信息数据，采用卡尔曼滤波对所述激光传感器扫描的数据进行了校正补偿，避免了非平坦地面造成的扫描数据偏差和里程计信息的失准；同时在建图过程中引入了闭环检测，消除了累积误差，提高了建图的精度。本发明的激光定位导航系统及其方法具有精确、快速且计算量小的特点，能够不受场地平整度、环境光照、卫星信号等因素的影响，能够在非结构化环境中获取移动平台的三维空间位姿信息，完成对未知环境的建图，进行精确定位与导航，以便完成医药搬运机器人的上下料工作。

附图说明

图1是本发明中激光定位导航系统结构框图，

图2是本发明中激光定位导航方法流程图，

图3是本发明中获取三维空间位姿信息数据的方法流程图，

图4是本发明中构建局部最优环境子地图的方法流程图，

图5是本发明中构建全局环境栅格地图的方法流程图，

图6是本发明中对医药搬运机器人的匹配定位进行预测更新的流程图，

图7是本发明中导航医药搬运机器人上下料的最佳路径方法流程图。

图中：1.建图模块，11.激光传感器，12.中央处理器，2.导航模块，3.人机交互模块。

具体实施方式

为了使本技术领域的人员更好地理解本发明的技术方案，下面结合附图对本发明作进一步的详细说明。

如图1所示，一种医药搬运机器人的激光定位导航系统，包括：

建图模块1，所述建图模块包括激光传感器11和中央处理器12，所述激光传感器11用于采集所述医药搬运机器人的实时环境信息数据，所述中央处理器12用于对所述激光传感器11采集的信息数据进行融合处理，并构建所述医药搬运机器人工作范围内的环境栅格地图，所述激光传感器11与所述中央处理器12电连接；

导航模块2，与所述中央处理器12电连接，用于确定所述医药搬运机器人的三维空间位姿信息，并将所述激光传感器11扫描的所述医药搬运机器人实时环境信息与所述中央处理器12构建的环境栅格地图进行匹配定位，寻找所述医药搬运机器人工作的最佳路径；

人机交互模块3，与所述中央处理器12和所述导航模块2均电连接，用于可视化显示所述中央处理器12构建的环境栅格地图以及所述医药搬运机器人所采集的信息数据，以及所述医药搬运机器人工作的最佳路径。

本实施例中，所述激光传感器11扫描所述医药搬运机器人周围的实时环境信息数据，并通过所述中央处理器12构建环境栅格地图，所述导航模块2通过将所述医药搬运机器人所处的实时环境信息与所述中央处理器12构建的环境栅格地图进行匹配定位，用于寻找所述医药搬运机器人的最优行程并对其进行导航，而且还可通过所述人机交互模块3对所述中央处理器12构建的环境栅格地图以及所述医药搬运机器人所采集的实时环境信息数据和导航行程进行可视化显示，本实施例中的所述激光定位导航系统能够实现在非结构化环境中获取移动平台的三维空间位姿信息，完成对未知环境的建图，进行精确定位与导航，以便完成医药搬运机器人的上下料工作。

如图2所示，一种医药搬运机器人的激光定位导航方法，采用所述医药搬运机器人的激光定位导航系统，所述医药搬运机器人的激光定位导航方法包括以下步骤：

S1：所述激光传感器采集所述医药搬运机器人的实时环境信息数据，并通过所述中央处理器融合处理以获取所述医药搬运机器人的三维空间位姿信息数据；

S2：通过所述激光传感器获取所述医药搬运机器人的扫描帧数据，所述中央处理器通过对比多帧扫描数据构建局部的最优环境子地图，并保存所述S1中所述医药搬运机器人的三维空间位姿信息数据以及与该三维空间位姿信息对应的最优环境子地图扫描数据；

S3：重复所述S1和所述S2，直至所述激光传感器完成对所述医药搬运机器人全部工作范围的扫描，实现全局环境栅格地图的构建；

S4：在所述人机交互模块设置所述医药搬运机器人的初始三维空间位姿信息；

S5：通过所述中央处理器将所述S1中所述激光传感器采集的所述医药搬运机器人的实时环境信息数据与所述S3中构建的全局环境栅格地图进行匹配定位；

S6：所述中央处理器根据所述S5的匹配定位信息，寻找所述医药搬运机器人上下料的最佳路径。

如图2所示，所述的S1具体步骤为：

S101、通过卡尔曼滤波融合处理所述中央处理器中的惯性导航单元中加速度计和陀螺仪的相应数据，得到所述医药搬运机器人的三维空间姿态信息，其中所述惯性导航单元中加速度计和陀螺仪的模型表示为：

式(1)中，θ_k为k时刻加速度计的角度，ω_k为k时刻陀螺仪角速度，ω_mk为k时刻陀螺仪的测量值，ω_bk为k时刻陀螺仪的零偏值，W_k-1为过程激励噪声，dt为时间变量Δt，θ_k-1为k-1时刻加速度计的角度，ω_k-1为k-1时刻陀螺仪角速度，ω_mk-1为k-1时刻陀螺仪的测量值，ω_bk-1为k-1时刻陀螺仪的零偏值，

本实施例中，所述陀螺仪零偏值ω_bk的状态方程可构造为：

ω_bk＝ω_bk-1+W_k-1 (10)

由式(10)得到

式(11)中，x_k为状态向量

根据式(11)，可计算出系统输出的角度z_k为：

式(12)中，ν_k为观测噪声；

本实施例中，由于所述激光传感器11存在噪声影响，为了消除所述激光传感器11对扫描数据的影响，采用卡尔曼滤波对所述中央处理器中的惯性导航单元获取加速度计和陀螺仪的相应数据进行校正补偿，避免了因非平坦地面而造成的所述激光传感器扫描的数据偏差和里程计信息的失准，通过卡尔曼滤波融合处理所述惯性导航单元获取的数据，从而得到所述医药搬运机器人的三维空间位姿信息。

S102、所述激光传感器采集所述医药搬运机器人的实时环境信息数据，并对所采集的信息数据进行投影，得到真实的激光平面数据，根据激光模型[ρ,θ]极坐标变换出障碍物相对于激光平面的坐标P_x，P_y，P_z：

式(2)中，θ_x，θ_y分别为绕x，y轴旋转的角度；

本实施例中，考虑到非结构化场景，所述激光传感器11扫描平面会发生倾斜，通过对采集的信息数据做投影处理进而得到证实的激光平面数据。

S103、根据所述医药搬运机器人绕轴旋转的角度θ变换信息和初始的位置，由式(2)可得到所述医药搬运机器人的角度θ发生变化后的位置[XYZ]^T：

式(3)中，R_x，R_y，R_z为绕x，y，z轴旋转的角度θ所对应的旋转矩阵，根据已知的障碍物位置信息，由式(3)计算出所述障碍物与水平平面的投影，投影后的所述障碍物激光距离可表示为：

ρ'²＝X²+Y² (4)。

如图4所示，所述S2具体步骤为：

S201、通过所述激光传感器获取所述医药搬运机器人的扫描帧数据，所述中央处理器将获取的扫描帧数据变换为子地图坐标：

式(5)中，p是激光传感器中激光光束端点的位置，ξ_x，ξ_y，ξ_θ为医药搬运机器人的三维空间位姿信息变化，T_ξ为变换矩阵；

本实施例中，通过所述步骤S201实现了所述医药搬运机器人从扫描坐标系到子地图坐标系的变换；

S202、所述中央处理器应用ceres-Solver库构建最优的环境栅格子地图，其公式为：

式(6)中，h_n为所述激光传感器扫描点的集合，N为所述激光传感器360°扫描的数据量，M_smooth为栅格子地图的概率值；

S203、保存所述S1中所述医药搬运机器人的三维空间位姿信息数据以及与该三维空间位姿信息数据对应的最优环境栅格子地图扫描数据。

如图3所示，所述的S3具体步骤为：

S301、所述中央处理器采用分支界定法寻找与所述激光传感器获取的扫描帧相对应的历史帧并进行匹配，消除建图过程中累积的误差；

S302、所述中央处理器应用ceres-Solver库选择所述S301中的相应数据，并将相应数据加入到全局环境栅格地图中进行SPA优化，其优化公式可表示为：

式(7)中，是每一子地图的位姿，E_m是的集合，是相对应的激光点集的位姿，E^s是的集合，∑_ij是协方差矩阵，ξ_ij是形成子地图与激光点集约束，是在以为原点的坐标系下的偏移，m表示子地图节点，i是第i个子地图节点(i＝1,2,3...m)，s表示扫描帧节点，j是第j个扫描帧节点(j＝1,2,3,....s)，其中 式中表示子地图节点的位置 表示扫描帧节点的位置 是2*2的旋转矩阵，e表示误差项，表示的角度，表示的角度，ξ_ij是(x,y,θ)的3*1向量(前两维表示的是偏移，后一维表示的是角度)，表示两者之间的角度偏移约束，表示范数意义下误差项e的平方。

S303、重复所述S301和所述S302，对所述医药搬运机器人工作范围内的所有未知环境进行扫描，完成全局环境栅格地图的构建。

本实施例中，当所述激光传感器获取一扫描帧后，该帧开始采用分支界定法寻找相对应的历史帧进行匹配，首先利用DFS方法进行节点选择，然后利用通用的分支方法将解空间分割成越来越小的子集，同时对目标子集计算相对应的界限，当可行解集的目标值无法达到界定值时，将该子集舍去，即剪枝，最后获得相匹配的结果，然后通过所述中央处理器应用ceres-Solver库并选择SPA优化方式对相匹配的结果进行优化，进而完成全局环境栅格地图的构建。本实施例中，所述步骤S302中的子地图节点m和扫描帧节点s分别为所述步骤S2中最优环境栅格子地图的节点和激光传感器获取的扫描帧节点。

如图3所示，所述S5中的匹配定位采用自适应蒙特卡洛定位方法进行预测和更新，具体步骤为：

S501、根据所述医药搬运机器人的运动状态，计算所述医药搬运机器人的先验概率密度，然后对所述医药搬运机器人的三维空间位姿信息进行预测；

S502、根据所述S303构建的全局环境栅格地图，比较所述医药搬运机器人处于所述S501预测三维空间位姿信息的理想状态下所述激光传感器扫描数据与该预测三维空间位姿信息所述激光传感器实际的扫描数据，根据所述医药搬运机器人预测三维空间位姿信息在理想状态和实际状态下比较的误差大小对所述预测三维空间位姿信息进行判断；

S503、缩小所述S502中比较的误差值，直至所预测三维空间位姿信息与真实三维空间位姿信息之间的误差值≤5cm，即完成所述医药搬运机器人的定位。

本实施例中，预测的三维空间位姿信息与真实的三维空间位姿信息之间的误差值可在根据需要达到的精度进行设置调整，本实施例中设置为5cm。

如图4所示，所述的S6中寻找并导航所述医药搬运机器人上下料的最佳路径的具体步骤为：

S601、采用A*方法寻找所述医药搬运机器人上下料的最佳路径，通过已知的起点值和终点值，设定合适的启发函数，全面评估和扩展搜索节点的代价值，并通过比较各扩展节点代价值的大小，选择最有希望的点加以扩展，直到找到目标节点为止，其估价函数公式如下：

F(n)＝G(n)+H(n) (8)

式(8)中，n表示医药搬运机器人的当前节点，G(n)表示从起点到当前节点的距离值，H(n)表示从当前节点到终点的距离值；

S602、采用DWA动态窗口方法选取局部避障路径，通过在速度空间[v,w]中对线速度v和角速度w进行多组采样，在采样速度的限制条件下，对一定时间内的所述医药搬运机器人运行轨迹进行预测，然后引入评价函数对所预测的所述医药搬运机器人运行轨迹进行评估，选取所述医药搬运机器人的最优速度和运行轨迹，其评价函数公式如下：

G(υ,ω)＝α·Gdist(υ,ω)+β·Pdist(υ,ω)+γ·vel(υ,ω) (9)

式(9)中，Gdist(ν,ω)表示所述医药搬运机器人航迹推演末端与目标方向的距离，Pdist(ν,ω)表示所述医药搬运机器人航迹推演与全局路径之间的距离，vel(ν,ω)表示当前速度大小，通过对其系数α，β，γ进行归一化处理，如判断所述医药搬运机器人的航迹中是否存在障碍物，若判断航迹中存在障碍物，则舍弃该组航迹。

本实施例中，通过所述激光传感器11扫描获取所述医药搬运机器人周围的实时环境信息数据，构建环境栅格地图，然后通过设置所述医药搬运机器人的初始三维空间位姿信息，将获取的所述医药搬运机器人实时环境信息与构建的环境栅格地图进行匹配定位，最后对所述医药搬运机器人的上下料行程进行导航，所述激光定位导航方法中，通过所述激光传感器中的惯性导航单元获取所述医药搬运机器人的三维空间姿态信息数据，采用卡尔曼滤波对所述激光传感器扫描的数据进行了校正补偿，避免了非平坦地面造成的扫描数据偏差和里程计信息的失准；同时在建图过程中引入了闭环检测，消除了累积误差，提高了建图的精度，具有精确、快速且计算量小的特点，能够不受场地平整度、环境光照、卫星信号等因素的影响，能够在非结构化环境中获取移动平台的三维空间位姿信息，完成对未知环境的建图，进行精确定位与导航，以便完成医药搬运机器人的上下料工作。本实施例中，通过设置所述医药搬运机器人的初始三维空间位姿信息，能够帮助所述医药搬运机器人完成快速的匹配定位，提高其准确度。

以上对本发明所提供的一种医药搬运机器人激光定位导航系统及其方法进行了详细介绍。本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的核心思想。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以对本发明进行若干改进和修饰，这些改进和修饰也落入本发明权利要求的保护范围内。

Claims (7)

1.一种医药搬运机器人的激光定位导航系统，其特征在于，包括：

建图模块(1)，所述建图模块(1)包括激光传感器(11)和中央处理器(12)，所述激光传感器(11)用于采集所述医药搬运机器人的实时环境信息数据，所述中央处理器(12)用于对所述激光传感器(11)采集的信息数据进行融合处理以获取所述医药搬运机器人的三维空间位姿信息，并构建所述医药搬运机器人工作范围内的环境栅格地图，所述激光传感器(11)与所述中央处理器(12)电连接；

导航模块(2)，与所述中央处理器(12)电连接，用于确定所述医药搬运机器人的三维空间位姿信息，并将所述激光传感器(11)扫描的所述医药搬运机器人实时环境信息与所述中央处理器(12)构建的环境栅格地图进行匹配定位，寻找所述医药搬运机器人工作的最佳路径；

人机交互模块(3)，与所述中央处理器(12)和所述导航模块(2)均电连接，用于可视化显示所述中央处理器(12)构建的环境栅格地图和所述医药搬运机器人所采集的信息数据，以及所述医药搬运机器人工作的最佳路径。

2.一种医药搬运机器人的激光定位导航方法，其特征在于，采用权利要求1所述的医药搬运机器人的激光定位导航系统，所述医药搬运机器人的激光定位导航方法包括以下步骤：

S1：所述激光传感器采集所述医药搬运机器人实时环境信息数据，并通过所述中央处理器融合处理以获取所述医药搬运机器人的三维空间位姿信息数据；

S2：通过所述激光传感器获取所述医药搬运机器人的扫描帧数据，所述中央处理器通过对比多帧扫描数据构建局部最优环境子地图，并保存所述S1中所述医药搬运机器人的三维空间位姿信息数据以及与该三维空间位姿信息对应的局部最优环境子地图扫描数据；

S3：重复所述S1和所述S2，直至所述激光传感器完成对所述医药搬运机器人全部工作范围的扫描，实现全局环境栅格地图的构建；

S4：在所述人机交互模块设置所述医药搬运机器人的初始三维空间位姿信息；

S5：通过所述中央处理器将所述S1中所述激光传感器采集的所述医药搬运机器人的实时环境信息数据与所述S3中构建的全局环境栅格地图进行匹配定位；

S6：所述中央处理器根据所述S5的匹配定位信息，寻找所述医药搬运机器人上下料的最佳路径。

3.如权利要求2所述的医药搬运机器人的激光定位导航方法，其特征在于，所述S1具体步骤为：

S101、通过卡尔曼滤波融合处理所述中央处理器中的惯性导航单元中加速度计和陀螺仪的相应数据，得到所述医药搬运机器人的三维空间姿态信息，其中所述惯性导航单元中加速度计和陀螺仪的模型表示为：

式(1)中，θ_k为k时刻加速度计的角度，ω_k为k时刻陀螺仪角速度，ω_mk为k时刻陀螺仪的测量值，ω_bk为k时刻陀螺仪的零偏值，W_k-1为k-1时刻过程激励噪声，dt为时间变量Δt；

S102、所述激光传感器采集所述医药搬运机器人的实时环境信息数据，并对所采集的信息数据进行投影，得到真实的激光平面数据，根据激光模型[ρ，θ]极坐标变换出障碍物相对于激光平面的坐标P_x，P_y，P_z：

式(2)中，θ_x，θ_y分别为绕x，y轴旋转的角度；

S103、根据所述医药搬运机器人绕轴旋转的角度θ变换信息和初始的位置，由式(2)可得到所述医药搬运机器人的角度θ发生变化后的位置[XYZ]^T：

式(3)中，R_x，R_y，R_z为绕x，y，z轴旋转的角度θ所对应的旋转矩阵，根据已知的障碍物位置信息，由式(3)计算出所述障碍物与水平平面的投影，投影后的所述障碍物激光距离ρ'可表示为：

ρ'²＝X²+Y² (4)。

4.如权利要求3所述的医药搬运机器人的激光定位导航方法，其特征在于，所述S2具体步骤为：

S201、通过所述激光传感器获取所述医药搬运机器人的扫描帧数据，所述中央处理器将获取的扫描帧数据变换为子地图坐标：

式(5)中，p是激光传感器中激光光束端点的位置，ξ_x，ξ_y，ξ_θ为医药搬运机器人的三维空间位姿信息变化，T_ξ为变换矩阵；

S202、所述中央处理器应用ceres-Solver库构建最优的环境栅格子地图，其公式为：

式(6)中，h_a为所述激光传感器扫描点的集合，A为所述激光传感器360°扫描的数据量，M_smooth为栅格子地图的概率值；

S203、保存所述S1中所述医药搬运机器人的三维空间位姿信息数据以及与该三维空间位姿信息数据对应的最优环境栅格子地图扫描数据。

5.如权利要求4所述的医药搬运机器人的激光定位导航方法，其特征在于，所述的S3具体步骤为：

S301、所述中央处理器采用分支界定法寻找与所述激光传感器获取的扫描帧相对应的历史帧并进行匹配，消除建图过程中累积的误差；

S302、所述中央处理器应用ceres-Solver库选择所述S301中的相应数据，并将相应数据加入到全局环境栅格地图中进行优化，其优化公式可表示为：

式(7)中，ξ_i ^m是每一子地图的位姿，E_m是ξ_i ^m的集合，是相对应的激光点集的位姿，E^s是ξ_i ^s的集合，∑_ij是协方差矩阵，ξ_ij是形成子地图与激光点集约束，是在以为原点的坐标系下的偏移，m表示子地图节点，i是第i个子地图节点(i＝1,2,3...m)，s表示扫描帧节点，j是第j个扫描帧节点(j＝1,2,3,....s)，其中 式中表示子地图节点的位置，表示扫描帧节点的位置，是2*2的旋转矩阵，e表示误差项，表示ξ_i ^m的角度，表示的角度，表示两者之间的角度偏移约束，表示范数意义下误差项e的平方。

S303、重复所述S301和所述S302，对所述医药搬运机器人工作范围内的所有未知环境进行扫描，完成全局环境栅格地图的构建。

6.如权利要求5述的医药搬运机器人的激光定位导航方法，其特征在于，所述S5中的匹配定位采用蒙特卡洛定位方法进行预测和更新，具体步骤为：

S501、根据所述医药搬运机器人的运动状态，计算所述医药搬运机器人的先验概率密度，然后对所述医药搬运机器人的三维空间位姿信息进行预测；

S502、根据所述S303构建的全局环境栅格地图，比较所述医药搬运机器人处于所述S501预测三维空间位姿信息的理想状态下所述激光传感器扫描数据与该预测三维空间位姿信息所述激光传感器实际的扫描数据，根据所述医药搬运机器人预测三维空间位姿信息在理想状态和实际状态下比较的误差大小对所述预测三维空间位姿信息进行判断；

S503、缩小所述S502中比较的误差值，直至所预测三维空间位姿信息与真实三维空间位姿信息之间的误差值≤5cm，即完成所述医药搬运机器人的定位。

7.如权利要求6所述的医药搬运机器人的激光定位导航方法，其特征在于，所述的S6具体步骤为：

S601、采用A*方法寻找所述医药搬运机器人上下料的最佳路径，通过已知的起点值和终点值，设定合适的启发函数，全面评估和扩展搜索节点的代价值，并通过比较各扩展节点代价值的大小，选择最有希望的点加以扩展，直到找到目标节点为止，其估价函数公式F(n)如下：

F(n)＝G(n)+H(n) (8)

式(8)中，n表示医药搬运机器人的当前节点，G(n)表示从起点到当前节点的距离值，H(n)表示从当前节点到终点的距离值；

S602、采用DWA动态窗口方法选取局部避障路径，通过在速度空间[v,w]中对线速度v和角速度w进行多组采样，在采样速度的限制条件下，对一定时间内的所述医药搬运机器人运行轨迹进行预测，然后引入评价函数对所预测的所述医药搬运机器人运行轨迹进行评估，选取所述医药搬运机器人的最优速度和运行轨迹，其评价函数公式如下：

G(υ,ω)＝α·Gdist(υ,ω)+β·Pdist(υ,ω)+γ·vel(υ,ω) (9)

式(9)中，Gdist(ν,ω)表示机器人航迹推演末端与目标方向的距离，Pdist(ν,ω)表示机器人航迹推演与全局路径之间的距离，vel(ν,ω)表示当前速度大小，系数α，β，γ表示对其进行归一化处理。