CN109459039B - 一种医药搬运机器人的激光定位导航系统及其方法 - Google Patents
一种医药搬运机器人的激光定位导航系统及其方法 Download PDFInfo
- Publication number
- CN109459039B CN109459039B CN201910015647.4A CN201910015647A CN109459039B CN 109459039 B CN109459039 B CN 109459039B CN 201910015647 A CN201910015647 A CN 201910015647A CN 109459039 B CN109459039 B CN 109459039B
- Authority
- CN
- China
- Prior art keywords
- robot
- medicine
- laser
- handling robot
- dimensional space
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
- G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Manipulator (AREA)
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时刻陀螺仪的零偏值,Wk-1为过程激励噪声,dt为时间变量Δt;
S102、所述激光传感器采集所述医药搬运机器人的实时环境信息数据并对所采集的信息数据进行投影,得到真实的激光平面数据,根据激光模型[ρ,θ]极坐标变换出障碍物相对于激光平面的坐标Px,Py,Pz:
式(2)中,θx,θy分别为绕x,y轴旋转的角度;
S103、根据所述医药搬运机器人绕轴旋转的角度θ变换信息和初始的位置,由式(2)可得到所述医药搬运机器人的角度θ发生变化后的位置[XYZ]T:
式(3)中,Rx,Ry,Rz为绕x,y,z轴旋转的角度θ所对应的旋转矩阵,根据已知的障碍物位置信息,由式(3)计算出所述障碍物与水平平面的投影,投影后的所述障碍物激光距离可表示为:
ρ'2=X2+Y2 (4)。
作为上述技术的优选方案,所述S2具体步骤为:
S201、通过所述激光传感器获取扫描帧数据,所述中央处理器将获取的扫描帧数据变换为子地图坐标:
式(5)中,p是激光传感器中激光光束端点的位置,ξx,ξy,ξθ为医药搬运机器人的三维空间位姿信息变化,Tξ为变换矩阵;
S202、所述中央处理器应用ceres-Solver库构建最优的环境栅格子地图,其公式为:
式(6)中,ha为所述激光传感器扫描点的集合,A为所述激光传感器360°扫描的数据量,Msmooth为栅格子地图的概率值;
S203、保存所述S1中所述医药搬运机器人的三维空间位姿信息数据以及与该三维空间位姿信息数据对应的最优环境栅格子地图扫描数据。
作为上述技术的优选方案,所述S3具体步骤为:
S301、所述中央处理器采用分支界定法寻找与所述激光传感器获取的扫描帧相对应的历史帧并进行匹配,消除建图过程中累积的误差;
S302、所述中央处理器应用ceres-Solver库选择所述S301中的相应数据,并将相应数据加入到全局环境栅格地图中进行SPA优化,其优化公式可表示为:
式(7)中,是每一子地图的位姿,Em是的集合,是相对应的激光点集的位姿,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时刻陀螺仪的零偏值,Wk-1为过程激励噪声,dt为时间变量Δt,θk-1为k-1时刻加速度计的角度,ωk-1为k-1时刻陀螺仪角速度,ωmk-1为k-1时刻陀螺仪的测量值,ωbk-1为k-1时刻陀螺仪的零偏值,
本实施例中,所述陀螺仪零偏值ωbk的状态方程可构造为:
ωbk=ωbk-1+Wk-1 (10)
由式(10)得到
根据式(11),可计算出系统输出的角度zk为:
式(12)中,νk为观测噪声;
本实施例中,由于所述激光传感器11存在噪声影响,为了消除所述激光传感器11对扫描数据的影响,采用卡尔曼滤波对所述中央处理器中的惯性导航单元获取加速度计和陀螺仪的相应数据进行校正补偿,避免了因非平坦地面而造成的所述激光传感器扫描的数据偏差和里程计信息的失准,通过卡尔曼滤波融合处理所述惯性导航单元获取的数据,从而得到所述医药搬运机器人的三维空间位姿信息。
S102、所述激光传感器采集所述医药搬运机器人的实时环境信息数据,并对所采集的信息数据进行投影,得到真实的激光平面数据,根据激光模型[ρ,θ]极坐标变换出障碍物相对于激光平面的坐标Px,Py,Pz:
式(2)中,θx,θy分别为绕x,y轴旋转的角度;
本实施例中,考虑到非结构化场景,所述激光传感器11扫描平面会发生倾斜,通过对采集的信息数据做投影处理进而得到证实的激光平面数据。
S103、根据所述医药搬运机器人绕轴旋转的角度θ变换信息和初始的位置,由式(2)可得到所述医药搬运机器人的角度θ发生变化后的位置[XYZ]T:
式(3)中,Rx,Ry,Rz为绕x,y,z轴旋转的角度θ所对应的旋转矩阵,根据已知的障碍物位置信息,由式(3)计算出所述障碍物与水平平面的投影,投影后的所述障碍物激光距离可表示为:
ρ'2=X2+Y2 (4)。
如图4所示,所述S2具体步骤为:
S201、通过所述激光传感器获取所述医药搬运机器人的扫描帧数据,所述中央处理器将获取的扫描帧数据变换为子地图坐标:
式(5)中,p是激光传感器中激光光束端点的位置,ξx,ξy,ξθ为医药搬运机器人的三维空间位姿信息变化,Tξ为变换矩阵;
本实施例中,通过所述步骤S201实现了所述医药搬运机器人从扫描坐标系到子地图坐标系的变换;
S202、所述中央处理器应用ceres-Solver库构建最优的环境栅格子地图,其公式为:
式(6)中,hn为所述激光传感器扫描点的集合,N为所述激光传感器360°扫描的数据量,Msmooth为栅格子地图的概率值;
S203、保存所述S1中所述医药搬运机器人的三维空间位姿信息数据以及与该三维空间位姿信息数据对应的最优环境栅格子地图扫描数据。
如图3所示,所述的S3具体步骤为:
S301、所述中央处理器采用分支界定法寻找与所述激光传感器获取的扫描帧相对应的历史帧并进行匹配,消除建图过程中累积的误差;
S302、所述中央处理器应用ceres-Solver库选择所述S301中的相应数据,并将相应数据加入到全局环境栅格地图中进行SPA优化,其优化公式可表示为:
式(7)中,是每一子地图的位姿,Em是的集合,是相对应的激光点集的位姿,Es是的集合,∑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 (6)
1.一种医药搬运机器人的激光定位导航方法,其特征在于,所述医药搬运机器人的激光定位导航方法包括以下步骤:
S1:激光传感器采集所述医药搬运机器人实时环境信息数据,并通过中央处理器融合处理以获取所述医药搬运机器人的三维空间位姿信息数据,具体包括:
S101、通过卡尔曼滤波融合处理所述中央处理器中的惯性导航单元中加速度计和陀螺仪的相应数据,得到所述医药搬运机器人的三维空间姿态信息,其中所述惯性导航单元中加速度计和陀螺仪的模型表示为:
式(1)中,θk为k时刻加速度计的角度,ωk为k时刻陀螺仪角速度,ωmk为k时刻陀螺仪的测量值,ωbk为k时刻陀螺仪的零偏值,Wk-1为k-1时刻过程激励噪声,dt为时间变量Δt;
S102、激光传感器采集所述医药搬运机器人的实时环境信息数据,并对所采集的信息数据进行投影,得到真实的激光平面数据,根据激光模型[ρ,θ]极坐标变换出障碍物相对于激光平面的坐标Px,Py,Pz,其中,ρ表示投影后的所述障碍物激光距离,θ表示医药搬运机器人绕轴旋转的角度:
式(2)中,θx,θy分别为绕x,y轴旋转的角度;
S103、根据所述医药搬运机器人绕轴旋转的角度θ变换信息和初始的位置,由式(2)可得到所述医药搬运机器人的角度θ发生变化后的位置[XYZ]T:
式(3)中,Rx,Ry,Rz为绕x,y,z轴旋转的角度θ所对应的旋转矩阵,根据已知的障碍物位置信息,由式(3)计算出所述障碍物与水平平面的投影,投影后的所述障碍物激光距离ρ'可表示为:
ρ'2=X2+Y2 (4);
S2:通过所述激光传感器获取所述医药搬运机器人的扫描帧数据,所述中央处理器通过对比多帧扫描数据构建局部最优环境子地图,并保存所述S1中所述医药搬运机器人的三维空间位姿信息数据以及与该三维空间位姿信息对应的局部最优环境子地图扫描数据;
S3:重复所述S1和所述S2,直至所述激光传感器完成对所述医药搬运机器人全部工作范围的扫描,实现全局环境栅格地图的构建;
S4:在人机交互模块设置所述医药搬运机器人的初始三维空间位姿信息;
S5:通过所述中央处理器将所述S1中所述激光传感器采集的所述医药搬运机器人的实时环境信息数据与所述S3中构建的全局环境栅格地图进行匹配定位;
S6:所述中央处理器根据所述S5的匹配定位信息,寻找所述医药搬运机器人上下料的最佳路径。
2.如权利要求1所述的医药搬运机器人的激光定位导航方法,其特征在于,所述S2具体步骤为:
S201、通过所述激光传感器获取所述医药搬运机器人的扫描帧数据,所述中央处理器将获取的扫描帧数据变换为子地图坐标:
式(5)中,p是激光传感器中激光光束端点的位置,ξx,ξy,ξθ为医药搬运机器人的三维空间位姿信息变化,Tξ为变换矩阵;
S202、所述中央处理器应用ceres-Solver库构建最优的环境栅格子地图,其公式为:
式(6)中,ha为所述激光传感器扫描点的集合,A为所述激光传感器360°扫描的数据量,Msmooth为栅格子地图的概率值;
S203、保存所述S1中所述医药搬运机器人的三维空间位姿信息数据以及与该三维空间位姿信息数据对应的最优环境栅格子地图扫描数据。
3.如权利要求2所述的医药搬运机器人的激光定位导航方法,其特征在于,所述的S3具体步骤为:
S301、所述中央处理器采用分支界定法寻找与所述激光传感器获取的扫描帧相对应的历史帧并进行匹配,消除建图过程中累积的误差;
S302、所述中央处理器应用ceres-Solver库选择所述S301中的相应数据,并将相应数据加入到全局环境栅格地图中进行优化,其优化公式可表示为:
式(7)中,是每一子地图的位姿,Em是的集合,即Em表示子地图的位姿集合;是相对应的激光点集的位姿,Es是的集合,即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,对所述医药搬运机器人工作范围内的所有未知环境进行扫描,完成全局环境栅格地图的构建。
4.如权利要求3所述的医药搬运机器人的激光定位导航方法,其特征在于,所述S5中的匹配定位采用蒙特卡洛定位方法进行预测和更新,具体步骤为:
S501、根据所述医药搬运机器人的运动状态,计算所述医药搬运机器人的先验概率密度,然后对所述医药搬运机器人的三维空间位姿信息进行预测;
S502、根据所述S303构建的全局环境栅格地图,比较所述医药搬运机器人处于所述S501预测三维空间位姿信息的理想状态下所述激光传感器扫描数据与该预测三维空间位姿信息所述激光传感器实际的扫描数据,根据所述医药搬运机器人预测三维空间位姿信息在理想状态和实际状态下比较的误差大小对所述预测三维空间位姿信息进行判断;
S503、缩小所述S502中比较的误差值,直至所预测三维空间位姿信息与真实三维空间位姿信息之间的误差值≤5cm,即完成所述医药搬运机器人的定位。
5.如权利要求4所述的医药搬运机器人的激光定位导航方法,其特征在于,所述的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(υ,ω)表示当前速度大小,系数α,β,γ表示对其进行归一化处理。
6.一种医药搬运机器人的激光定位导航系统,其特征在于,采用权利要求1-5任一项所述的医药搬运机器人的激光定位导航方法进行激光定位导航,其包括:
建图模块(1),所述建图模块(1)包括激光传感器(11)和中央处理器(12),所述激光传感器(11)用于采集所述医药搬运机器人的实时环境信息数据,所述中央处理器(12)用于对所述激光传感器(11)采集的信息数据进行融合处理以获取所述医药搬运机器人的三维空间位姿信息,并构建所述医药搬运机器人工作范围内的环境栅格地图,所述激光传感器(11)与所述中央处理器(12)电连接;
导航模块(2),与所述中央处理器(12)电连接,用于确定所述医药搬运机器人的三维空间位姿信息,并将所述激光传感器(11)扫描的所述医药搬运机器人实时环境信息与所述中央处理器(12)构建的环境栅格地图进行匹配定位,寻找所述医药搬运机器人工作的最佳路径;
人机交互模块(3),与所述中央处理器(12)和所述导航模块(2)均电连接,用于可视化显示所述中央处理器(12)构建的环境栅格地图和所述医药搬运机器人所采集的信息数据,以及所述医药搬运机器人工作的最佳路径。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910015647.4A CN109459039B (zh) | 2019-01-08 | 2019-01-08 | 一种医药搬运机器人的激光定位导航系统及其方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910015647.4A CN109459039B (zh) | 2019-01-08 | 2019-01-08 | 一种医药搬运机器人的激光定位导航系统及其方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN109459039A CN109459039A (zh) | 2019-03-12 |
CN109459039B true CN109459039B (zh) | 2022-06-21 |
Family
ID=65616273
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910015647.4A Active CN109459039B (zh) | 2019-01-08 | 2019-01-08 | 一种医药搬运机器人的激光定位导航系统及其方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109459039B (zh) |
Families Citing this family (13)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
DE102019203751A1 (de) * | 2019-03-19 | 2020-09-24 | Robert Bosch Gmbh | Verfahren und Vorrichtung zum Bewerten einer Trajektorie für ein technisches System |
CN110189366B (zh) * | 2019-04-17 | 2021-07-06 | 北京迈格威科技有限公司 | 一种激光粗配准方法、装置、移动终端及存储介质 |
CN110440811B (zh) * | 2019-08-29 | 2021-05-14 | 湖北三江航天红峰控制有限公司 | 一种通用型自主导航控制方法、装置及设备终端 |
CN111273128B (zh) * | 2020-02-28 | 2022-07-05 | 广东工业大学 | 一种用于检测地下电缆故障的管道机器人 |
CN111360829B (zh) * | 2020-03-13 | 2023-12-05 | 苏州三百亿科技有限公司 | 人工智能大数据下医疗用品运送机器人及其控制方法 |
CN111754566A (zh) * | 2020-05-12 | 2020-10-09 | 哈尔滨工业大学(深圳)(哈尔滨工业大学深圳科技创新研究院) | 机器人场景定位方法和施工操作方法 |
CN111839926B (zh) * | 2020-06-18 | 2022-04-12 | 南京邮电大学 | 一种头姿交互控制与自主学习控制共享的轮椅控制方法及系统 |
CN112162294B (zh) * | 2020-10-10 | 2023-12-15 | 北京布科思科技有限公司 | 一种基于激光传感器的机器人结构检测方法 |
CN112506972B (zh) * | 2020-12-15 | 2023-06-13 | 中国联合网络通信集团有限公司 | 用户常驻区域定位方法、装置、电子设备以及存储介质 |
CN112750161B (zh) * | 2020-12-22 | 2023-11-03 | 苏州大学 | 用于移动机器人的地图更新方法 |
CN112762928B (zh) * | 2020-12-23 | 2022-07-15 | 重庆邮电大学 | 含有激光slam的odom与dm地标组合移动机器人及导航方法 |
CN112729321A (zh) * | 2020-12-28 | 2021-04-30 | 上海有个机器人有限公司 | 一种机器人的扫图方法、装置、存储介质和机器人 |
CN114609646A (zh) * | 2022-03-16 | 2022-06-10 | 上海擎朗智能科技有限公司 | 激光建图方法、装置、介质及电子设备 |
Family Cites Families (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US6526352B1 (en) * | 2001-07-19 | 2003-02-25 | Intelligent Technologies International, Inc. | Method and arrangement for mapping a road |
CN101769754B (zh) * | 2010-01-19 | 2012-04-25 | 湖南大学 | 一种基于类三维地图的移动机器人全局路径规划方法 |
CN103808316A (zh) * | 2012-11-12 | 2014-05-21 | 哈尔滨恒誉名翔科技有限公司 | 室内飞行智能体惯性系统与激光测距仪组合导航改进方法 |
CN104503449A (zh) * | 2014-11-24 | 2015-04-08 | 杭州申昊科技股份有限公司 | 一种基于环境直线特征的定位方法 |
US10046869B2 (en) * | 2015-01-28 | 2018-08-14 | Kieran A. Carroll | Inertial sensing augmentation for navigation of spacecraft |
CN104914865B (zh) * | 2015-05-29 | 2017-10-24 | 国网山东省电力公司电力科学研究院 | 变电站巡检机器人定位导航系统及方法 |
CN105354875B (zh) * | 2015-09-25 | 2018-01-23 | 厦门大学 | 一种室内环境二维与三维联合模型的构建方法和系统 |
CN106772517A (zh) * | 2016-12-29 | 2017-05-31 | 华南农业大学 | 基于双天线gnss接收机/陀螺仪信息融合的农机横滚角测试方法 |
CN107608364A (zh) * | 2017-11-01 | 2018-01-19 | 广州供电局有限公司 | 一种用于数据中心物理设备上下架的智能机器人 |
CN107702722A (zh) * | 2017-11-07 | 2018-02-16 | 云南昆船智能装备有限公司 | 一种激光导引agv自然导航定位方法 |
CN107909614B (zh) * | 2017-11-13 | 2021-02-26 | 中国矿业大学 | 一种gps失效环境下巡检机器人定位方法 |
CN108919238B (zh) * | 2018-07-18 | 2023-10-27 | 浙江大学 | 一种基于惯性测量单元的旋转式激光雷达数据校正方法及系统 |
-
2019
- 2019-01-08 CN CN201910015647.4A patent/CN109459039B/zh active Active
Also Published As
Publication number | Publication date |
---|---|
CN109459039A (zh) | 2019-03-12 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109459039B (zh) | 一种医药搬运机器人的激光定位导航系统及其方法 | |
US11567502B2 (en) | Autonomous exploration framework for indoor mobile robotics using reduced approximated generalized Voronoi graph | |
CN109211251B (zh) | 一种基于激光和二维码融合的即时定位与地图构建方法 | |
Nieuwenhuisen et al. | Autonomous navigation for micro aerial vehicles in complex GNSS-denied environments | |
Li et al. | Collaborative mapping and autonomous parking for multi-story parking garage | |
WO2010038353A1 (ja) | 自律移動装置 | |
Karimi et al. | LoLa-SLAM: low-latency LiDAR SLAM using continuous scan slicing | |
Schmid et al. | A unified approach for autonomous volumetric exploration of large scale environments under severe odometry drift | |
CN112444246B (zh) | 高精度的数字孪生场景中的激光融合定位方法 | |
CN112904358B (zh) | 基于几何信息的激光定位方法 | |
CN111487960A (zh) | 一种基于定位能力估计的移动机器人路径规划方法 | |
Li et al. | Hybrid filtering framework based robust localization for industrial vehicles | |
CN115855062A (zh) | 一种室内移动机器人自主建图与路径规划方法 | |
US20230111122A1 (en) | Multi-sensor-fusion-based autonomous mobile robot indoor and outdoor positioning method and robot | |
Karam et al. | Integrating a low-cost mems imu into a laser-based slam for indoor mobile mapping | |
Choi et al. | Cellular Communication-Based Autonomous UAV Navigation with Obstacle Avoidance for Unknown Indoor Environments. | |
Yilmaz et al. | A precise scan matching based localization method for an autonomously guided vehicle in smart factories | |
Shoukat et al. | Cognitive robotics: Deep learning approaches for trajectory and motion control in complex environment | |
JP2010112836A (ja) | 自己位置同定装置および該自己位置同定装置を備えた移動ロボット | |
Emter et al. | Stochastic cloning for robust fusion of multiple relative and absolute measurements | |
Samarakoon et al. | Impact of the Trajectory on the Performance of RGB-D SLAM Executed by a UAV in a Subterranean Environment | |
Montes-Grova et al. | Multi-Sensor System for Pipe Inspection using an Autonomous Hybrid Aerial Robot | |
Liu et al. | Error modelling and optimal estimation of laser scanning aided inertial navigation system in GNSS-denied environments | |
Kozlov et al. | Development of an Autonomous Robotic System Using the Graph-based SPLAM Algorithm | |
Mitka et al. | Autonomous mobile platform with simultaneous localisation and mapping system for patrolling purposes |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |