0 引言
随着机器人技术的迅速发展和广泛应用,轮式机器人在工业、交通、医疗、服务等领域中被广泛采用。然而,在机器人的实际运行过程中,由于传感器精度、环境变化和安装误差等多种因素的影响,机器人的定位精度往往存在一定的偏差。为了提高机器人的定位精度并减小这些因素对机器人定位的影响,位置误差标定技术应运而生。位置误差标定技术能够提高机器人的定位精度,使其能够更准确地执行任务,并为机器人在其他领域的应用提供技术支持[1]。
乔贵方等[2]首先建立基于M-DH的轮式机器人位置误差模型,并利用差分进化优化算法对模型的参数展开辨识;其次引入RBF神经网络对机器人的剩余误差实施拟合处理,降低轮式机器人的综合位置误差,融合误差模型和RBF神经网络完成机器人位置两级误差标定。该方法仅以里程误差为标定参数,无法充分考虑到其他环境因素对机器人定位精度的影响。赵磊等[3]利用空间矢量法和DH法建立轮式机器人的运动学模型,建立了偏微分理论推导的误差模型;采用激光跟踪仪对不同移动轨迹下的机器人空间位置数据实施采集;最后使用基于等步距搜索策略的改进遗传算法完成机器人位置误差的标定和补偿。在理论上扩展了位置误差的模型,但如果仅仅以里程误差作为位置标定参数,仍然无法充分考虑到其他误差源对定位精度的影响。Chen等[4]将机器人终端位置误差从3个方面进行来源分析,建立了机器人位置误差模型,推导了机器人末端位置误差与工业机器人模型参数误差之间的关系。通过求解这些方程来识别和补偿模型参数误差。当前的标定方式放弃了对随机环境地图的参考,并仅以里程误差为位置标定参数,无法充分考虑到随机环境地图的变化和其他环境因素对机器人定位精度的影响。苏成志等[5]在标定过程中,利用PSD设备测量机器人末端激光器发出的空间异面光线之间的距离d,在机器人系下计算对应的空间异面光线之间的距离d,两者构成距离约束,利用PSO算法对机器人系统参数误差进行辨识,完成标定。但该方法在大空间范围的应用中,基于光线测量的方法易受外界因素干扰,导致测量误差较大,影响最终的标定结果。Luo等[6]通过对偶四元数的新的正运动学求解和修正的误差建模过程,得到无量纲误差映射矩阵(error mapping matrixes,EMMs)。在此基础上,设计了迭代辨识程序,完成误差标定。但该方法在应用中,设置的迭代辨识程序存在过拟合问题,影响标定结果的准确性。
为了解决上述方法中存在的问题,提出基于视觉SLAM算法的轮式机器人位置误差标定方法。使用深度相机进行数据采集,综合考虑里程和视觉采集误差,获得准确、稳定的标定结果,以降低机器人的位置误差。实验结果表明,所提方法在标定精度和鲁棒性方面均表现优越。
1 轮式机器人运动区域随机地图建立
1.1 运动区域的图像数据获取
通过获取运动区域的图像数据,轮式机器人能够有效地感知和理解周围环境,为轮式机器人运动区域随机地图构建提供重要的视觉感知信息。这样的视觉感知能力使机器人能够更好地适应复杂和变化的环境,并执行更精准和更智能任务。
在轮式机器人移动过程中,采用Kinect深度相机[7-8]采集轮式机器人的随机运动环境数据。该相机可以获取到轮式机器人运动区域的彩色信息和深度信息。Kinect相机的采集原理是通过针孔相机模型将轮式机器人运动场景中的三维路标点映射到二维图像平面,见图1。
图1 针孔相机成像模型
Fig.1 Imaging model of pinhole camera
图1中,P为相机坐标系下的一点[X,Y,Z]T,该点在经过O投影后映射到物理成像平面上的点P′,其坐标为 [X′,Y′,Z′]T。完成成像后,通过三角形相似关系得到该点各坐标的关系方程为
用于表示相机的缩放倍数;此时在该平面上,点P′的像素坐标为[u,v]T,且
与
结合得:
(1)
式(1)中: cx、cy表示像素的坐标偏移量; fx、 fy为两轴焦距。将相机的当前位姿(包括旋转矩阵R和平移向量t)转换为其对象的世界坐标系,图像坐标系之间的坐标转换关系为
(2)
通过Kinect的针孔成像模型实现轮式机器人运动区域图像(包含彩色和深度图像)的扫描采集[9],用于后续视觉SLAM研究。表达式为
(3)
这些采集的信息提供了关于场景中物体的视觉外观和空间位置的数据,为机器人在运动区域的环境感知和理解提供了重要的视觉感知信息。
1.2 基于Kinect的视觉SLAM环境随机地图建立
视觉SLAM是实现轮式机器人智能化的关键技术之一。基于深度Kinect的视觉SLAM算法流程分为前端和后端2部分,前端为视觉里程估计,通过上述所采集获得Kinect深度图像信息的相邻两帧图像信息求出机器人的运动信息[10];后端用于接收不同时刻前端提供的机器人位姿信息,以获取全局轨迹,建立环境随机地图,为后续的机器人位置误差标定提供了准确定位的基础。
1.2.1 前端设计
1) 图像特征选择
首先,利用SIFT算法[11]选择Kinect深度相机采集的轮式机器人运动区域图像中针对性较强的特征点,具体步骤为:
尺度空间极值点监测。为了保证SIFT特征的不变性,在空间坐标系(x,y)下建立尺度坐标σ,利用式(4)求出机器人移动场景下运动区域图像尺度空间L(x,y,σ)和尺度可变二维高斯函数卷积G(x,y,σ),
(4)
式(4)中: I(u,v)表示Kinect深度相机采集的轮式机器人运动区域图像的灰度值。
备选特征点筛选。比较尺度空间中每一个特征点的对比度,设定一个对比度阈值,小于阈值则删除,反之保留。对比度较低的点删除,对比度较高的点保留。
特征点方向分配。通过式(5)计算点(x,y)的梯度大小和方向:
(5)
式(5)中:L表示每个特征点的尺度。
特征描述。建立以4×4种子点为主的特征向量关键点,关键点中的每个种子点均存在8个不同方向上的位置特征向量,利用这些关键点最终构成128维SIFT特征向量,以便描述。
2) 特征点匹配
设获取的运动区域图像前后两帧的特征点集合为X、Y,根据欧几里得距离得出相似度度量指标
值越小,相似度越大[12]。
3) 错误匹配点剔除
为减少上述处理出现的一些错误匹配点,将SIFT算法和GTM算法相结合剔除这些点,步骤如下:
建立初始图形。将特征点集合作为初始图形的N个顶点,用Ui(i=1,2,…,N)描述,在满足
条件下,图中任意2个顶点Ui、Uj相连,其中的
代表图形中两两距离均值,以此获取初始图形。
错误匹配点搜索剔除。将图形建立过程中出现的差异点所在的连接线去除。差异性搜索方法是:
建立2个图对应的矩阵,初始值为零。若i和j满足
则赋值给矩阵第i行第j列为1,得到如下矩阵:
(6)
将2个矩阵作差,剔除矩阵中的非零元素,即错误匹配点,效果图如图2所示。
图2 错误匹配点剔除结果
Fig.2 Error matching point removal result
1.2.2 后端设计
视觉SLAM算法的后端部分采用迭代最近点算法(iterative closest point,ICP)[13]实施机器人的位姿估计和三维地图创建,假设通过前后两帧轮式机器人运动区域图像的特征选择、匹配等操作分别获取的三维特征点集为P={pi,i=0,1,2,…,k}和U={ui,i=0,1,2,…,n},选用ICP算法获取2个点集中的旋转、平移变换矩阵,由此展开环境随机地图创建过程:
1) 寻找集合P中距U中各点最近的对应点,将其组成新点集,用Q={qi, i=0,1,2,…,n}表示。
2) 通过最小均方根法求出U和Q间的配准,获取变换矩阵R和T。
3) 对于集合U,利用R和T展开坐标转换,得到新的点集U1=R·U+T。
4) 设一个极限值ε,将其与U1和Q之间的均方根误差对比,若均方根误差小于ε则结束计算,大于则用U1替换U后继续计算。通过该算法求出最后的R和T后,当轮式机器人继续行驶获取到第三帧运动区域图像后,在地图中描述机器人前两帧时刻的位姿信息,在此基础上与前两帧配准实施拼接,并继续上述操作后,实现与第三帧图像的拼接并更新环境随机地图,以此循环操作,获取轮式机器人未知环境的完整三维地图,得到机器人的运动轨迹和位置信息。
2 机器人位置误差标定方法设计
通常情况下,轮式机器人配有实现物体移动距离计算的里程本体感知传感器和视觉传感器,将两者获取的真实位置数据与视觉SLAM算法构建的随机环境地图中获取的机器人位置数据对比,计算两者之间的误差,即可实现机器人的位置误差标定。
2.1 里程误差标定
在实际生产和使用中,可能会存在制造误差、装配误差、测量误差等,导致轮子的直径不完全相等,测得的轮距不准确,而不相等的轮直径和不确定的轮距是影响轮式机器人里程估计精度的主要因素[14]。根据这2种误差来源Wd和Wb,可以建立一个误差模型来描述里程估计的误差:
(7)
式(7)中:DL、DR表示轮式机器人左、右驱动轮的真实直径;baxturl、bnominal分别表示真实轮距以及标称轮距。
定义误差模型后,使用UMBmark测试方法对里程误差实施离线标定[15],根据测试结果,计算里程估计定位误差∈x,∈y,∈θ:
(8)
式(8)中: xabx、yabs、yabs用于描述通过环境随机地图得到的机器人终点绝对位置信息;xcal、ycal、θcal则代表所记录的机器人所在终点位置信息。
结合式(8),对里程误差展开标定,用Wmax,sysr表示标定结果,公式如下:
(9)
式(9)中: xc.g.,cw/ccw、yc.g.,cw/ccw用于表示UMBmark测试中轮式机器人按照顺时针和逆时针方向移动时,其位置误差的横坐标值以及纵坐标值,其中下标cw和ccw分别表示顺时针和逆时针方向;rc.g.,cw/ccw代表弧线曲率半径;以此展开UMBmark实验,即可实现轮式机器人里程误差的离线标定。
2.2 视觉采集误差标定
通过视觉采集误差标定,可以精确估计视觉传感器引入的误差模型和相关参数,以便在实际运行中对数据进行校正和处理。这有助于提高机器人对环境的感知能力,提高目标检测和跟踪的准确性,以及提高机器人的导航和定位精度。
根据建立的里程模型,先预设轮式机器人在世界坐标系下k时刻的位姿qk=[qxk,qyk,qθk]T,根据航迹推算原理,推导轮式机器人在k时刻到k-1时刻的运动学位姿变化规律,即qk=qk-1+δk。当视觉传感器的安装位置l=[lx,ly,lθ]T在与机器人平台同平面的附近位置时,其中lx、ly、lθ分别代表视觉传感器的横纵安装坐标以及视觉传感器与机器人坐标系x轴之间产生的夹角;若不在附近位置时,lx、ly、lθ分别代表视觉传感器相对于机器人坐标系的横纵向偏移量以及视觉传感器相对于机器人坐标系x轴之间产生旋转误差。由此,可以获取k时刻与k-1时刻的世界坐标系下视觉传感器安装的位置信息:
(10)
式(10)中:⊕为Censi运算符,用来计算视觉传感器测量值与机器人状态之间的差异。轮式机器人在世界坐标系下k-1时刻的位姿信息为
(11)
由此获取视觉传感器的状态方程sk=sk-1-L+δk⊕l。通过建立状态方程,可以将视觉传感器的位姿坐标和时间相关联,并利用控制输入量对状态进行预测和修正。
为了在线标定视觉采集误差,采用扩展卡尔曼滤波(extended kalman filter,EKF)[16-17],根据状态方程确定其状态向量x=[sx,sy,sθ,δxlx,δyly,δθ,lθ]T,其中(sx,sy,sθ)为视觉传感器在世界坐标系下的位姿坐标,(δxlx,δyly,δθ,lθ)为安装位置真实值,加入均值为vk、hk,协方差为Rk的高斯噪声,利用式(12)确定视觉传感器的观测方程:
(12)
观测方程可以准确地将视觉传感器的测量值转化为机器人的位姿坐标,从而提高机器人的位置定位精度和鲁棒性。用
表示当前时刻的状态变量,
为k-1时刻的最优估计,在此基础上采用EKF公式,求解状态预测过程,如式(13)所示。
(13)
利用式(13)获取在k-1时刻的状态预测协方差矩阵[18]:
(14)
式(14)中,Fk-1、Hk-1分别表示函数f针对视觉传感器状态变量及控制输入量所建立的雅可比矩阵。随着时间的不断更新,可以根据k时刻的卡尔曼增益、后验状态及协方差不断对结果展开更新修正以达到最优状态。
根据上述步骤,将里程的标定结果应用于扩展卡尔曼滤波的在线标定过程中。通过不断使用观测值对预测值实施修正,以达到视觉采集误差参数逼近真值的目的。
3 实验与分析
为了验证基于视觉SLAM算法的轮式机器人位置误差标定的整体有效性,选择一个含有各种不规则障碍物的室内环境作为测试环境,令所研究的轮式机器人在该环境内按照预设的轨迹前进,利用所提方法采集该图像中机器人的行驶路线,得到所提方法采用基于深度Kinect的视觉SLAM算法建立的环境随机地图,并与机器人自身激光传感器采集到的地图对比,结果如图3所示。
图3 所提方法环境随机地图建立测试
Fig.3 The proposed method is tested for establishing a random map in the environment
对比图3(c)和图3(d)可以看出,左图激光扫描地图对于真实测试环境中的玩具、架子等障碍物未能完全扫描出来,这将会影响后续机器人位置误差的标定结果,而右图中所提方法能够较为精确地将测试环境中的各种障碍物的相对位置都在地图上很好地表现出来,说明所提方法的地图建立效果是清晰并准确的。
将里程和视觉传感器安装在轮式机器人的顶部和底部位置测量其移动距离和方向,并确保其能正确地扫描周围环境,获取里程误差参数(左轮、右轮半径及轮距)和视觉采集误差参数(安装x、y轴位置及z方向安装位置)的仿真真值分别为∈x=1.05,∈y=0.98,∈θ=1.05和sx=1.1,sy=0.8,sθ=-1,以此利用所提方法对两者展开同时标定,将3次 标定后的结果和真值实施对比,以此验证所提方法的轮式机器人位置误差标定有效性和可靠性,如图4所示。
图4 所提方法标定结果
Fig.4 Calibration results of the proposed method
从图4中的结果可以看出,经过3次标定的迭代更新,第2次的标定曲线与第1次标定曲线区别不是特别明显,第3次标定结果显示,轮式机器人的里程误差标定结果、视觉采集误差标定结果均接近于各自真值,可以说明所提方法具有较好的收敛效果。所研究方法的优势在于使用深度Kinect相机对轮式机器人的运动环境数据进行采集,可以获得更准确和全面的环境感知信息,为后续的位置误差标定提供了更可靠的输入数据,从而加速了整个过程的收敛。
为了衡量所提方法的性能,引入文献[2]方法和文献[3]方法,设定轮式机器人按照直线轨迹和曲线轨迹前进,每条轨迹上均采集20个位置点,采用上述3种方法对2种情况下机器人的位置误差实施标定,直至3种方法达到各自认为的最终标定状态,获取如图5所示的标定前后对比结果。其中,误差为轮式机器人在曲线轨迹下的位置数据与标定真实位置数据的差。
图5 不同轨迹下3种方法标定结果对比
Fig.5 Comparison of calibration results of three methods under different trajectories
根据图5可知,在直线轨迹下,所提方法能够有效降低轮式机器人的标定误差,与轮式机器人的位置误差真实结果相接近,而文献[2]方法的标定结果与真实位置点之间相差较大,误差最大处达到了45 mm,说明其标定效果不佳;文献[3]方法虽然比文献[2]方法的标定精度高一些,但出现了个别未能被跟踪到的位置点,同时误差最大处达到了50 mm以上;在曲线轨迹下,仅所提方法的标定结果较为稳定,控制后的误差最大控制在10 mm以内,其他2种传统方法均出现了较大的误差。所提方法的优势在于充分考虑了里程和视觉采集误差之间的关系,使标定结果更为准确和稳定。
4 结论
此次研究提出基于视觉SLAM算法的轮式机器人位置误差标定方法,将机器人自身的里程和视觉采集误差进行共同标定,以减少机器人位置误差的影响。得出以下结论:
1) 相比于其他环境地图建立方法,所研究方法的地图建立效果是清晰且准确的,并且能够在较少次数的迭代后获得稳定的结果。
2) 在不同轨迹下所提方法的标定结果较为稳定,控制后的误差最大不超过10 mm,因此可以说明所研究方法能够在直线与曲线轨迹2种情况下,达到理想的控制效果。
[1] 韩耀廷,赵志梅,郝晓宇,等.变电站巡检机器人路径规划智能算法优化[J].内蒙古电力技术,2021,39(6):58-61.HAN Yaoting,ZHAO Zhimei,HAO Xiaoyu,et al.Intelligent algorithm optimization for path planning of substation inspection robots[J].Inner Mongolia Electric Power Technology,2021,39(6):58-61.
[2] 乔贵方,田荣佳,张颖,等.串联工业机器人两级定位误差标定方法研究[J].中国测试,2022,48(7):134-139.QIAO Guifang,TIAN Rongjia,ZHANG Ying,et al.Research on double-stage positional error calibration method of serial industrial robot[J].China Measurement &Test,2022,48(7):134-139.
[3] 赵磊,闫照方,栾倩倩,等.大空间运动3-RRRU并联机器人运动学标定与误差分析[J].农业机械学报,2021,52(11):411-420.ZHAO Lei,YAN Zhaofang,LUAN Qianqian,et al.Kinematic calibration and error analysis of 3-RRRU parallel robot in large overall motion[J].Transactions of the Chinese Society for Agricultural Machinery,2021,52(11):411-420.
[4] CHEN T,LIN J,WU D,et al.Research of calibration method for industrial robot based on error model of position[J].Applied Sciences,2021,11(3):1287-1296.
[5] 苏成志,吴文华,王浪,等.多PSD距离约束的机器人定位精度快速标定[J].组合机床与自动化加工技术,2022(9):7-10.SU Chengzhi,WU Wenhua,WANG Lang,et al.Fast calibration of robot positioning accuracy with multiple PSD distance constraints[J].Modular Machine Tool &Automatic Manufacturing Technique,2022(9):7-10.
[6] LUO X,XIE F,LIU X J,et al.Kinematic calibration of a 5-axis parallel machining robot based on dimensionless error mapping matrix[J].Robotics and Computer-Integrated Manufacturing,2021,70(5):102115-102127.
[7] KAMBLE T U,MAHAJAN S P.3D vision using multiple structured light-based kinect depth cameras[J].International Journal of Image and Graphics,2022,40(4):548-550.
[8] MART
NEZ M,PEDRAZA M,DAZ F J,et al.Indoor 3D video monitoring using multiple kinect depth-cameras[J].SSRN Electronic Journal,2021,65(7):157-161.
[9] 贾晓雪,赵冬青,张乐添,等.基于自适应惯导辅助特征匹配的视觉SLAM算法[J].光学精密工程,2023,31(5):621-630.JIA Xiaoxue,ZHAO Dongqing,ZHANG Letian,et al.A visual SLAM algorithm based on adaptive inertial navigation assistant feature matching[J].Optics and Precision Engineering,2023,31(5):621-630.
[10] 冯一博,张小俊,王金刚.适用于室内动态场景的视觉SLAM算法研究[J].燕山大学学报,2022,46(4):319-326.FENG Yibo,ZHANG Xiaojun,WANG Jingang.Research on visual SLAM algorithm suitable for indoor dynamic scenes[J].Journal of Yanshan University,2022,46(4):319-326.
[11] 周宇.基于相位一致和稳定非线性SIFT算法的SAR图像校准[J].现代雷达,2022,44(6):49-54.ZHOU Yu.SAR image calibration based on phase-consistent and stable nonlinear SIFT algorithm[J].Modern Radar,2022,44(6):49-54.
[12] HUI Y U,TANG J Y,KUN Y E,et al.Feature fusion-based feature point extraction and matching of the three-dimensional human vertebrae model[J].Journal of mechanics in medicine and biology,2022,22(3):2240015.
[13] HUANG L,WANG C,YUN J,et al.Object pose estimation based on stereo vision with improved K-D tree ICP algorithm[J].Concurrency and computation:practice and experience,2023,35(21):1-16.
[14] 张良安,唐锴,李鹏飞,等.基于复合摆线轨迹的四足机器人稳定性分析[J].江苏大学学报(自然科学版),2022,43(1):62-66,74.ZHANG Liang’an,TANG Kai,LI Pengfei,et al.Stability analysis of quadruped robot based on compound cycloid trajectory[J].Journal of Jiangsu University:Natural Science Editiont,2022,43(1):62-66,74.
[15] 于国栋,王春阳,兰孝野,等.基于离线标定的快速图像拼接算法[J].液晶与显示,2021,36(10):1430-1436.YU Guodong,WANG Chunyang,LAN Xiaoye,et al.Fast image mosaic algorithm based on offline calibration[J].Chinese Journal of Liquid Crystals and Displays,2021,36(10):1430-1436.
[16] 陈龙,刘巧斌,陶磊.基于GS-EKF滤波方法的车辆状态参数估计[J].东北大学学报(自然科学版),2023,44(8):1144-1151.CHEN Long,LIU Qiaobin,TAO Lei.Vehicle state parameter estimation based on GS-EKF filtering method[J].Journal of Northeastern University(Natural Science Edition),2023,44(8):1144-1151.
[17] PATTNAIK M,BADONI M,TATTE Y.Application of gaussian-sequential-probabilistic-inference concept based kalman filter for accurate estimation of state of charge of battery[J].Journal of Energy Storage,2022,54(10):1612-1624.
[18] 李锐洋,霍伟博,马巍,等.基于逆高斯纹理分布的协方差矩阵估计方法[J].系统工程与电子技术,2021,43(9):2470-2475.LI Ruiyang,HUO Weibo,MA Wei,et al.Covariancematrix estimation method based on inverse Gaussian texturedistribution[J].Systems Engineering and Electronics,2021,43(9):2470-2475.