一种基于多传感器设备对接方法
阅读说明：本技术 一种基于多传感器设备对接方法 (Multi-sensor-based equipment docking method ) 是由 李越 赖志林 于 2021-07-21 设计创作，主要内容包括：本发明公开了一种基于多传感器设备对接方法,利用机器人上的摄像头扫描对接设备上的二维码,通过aruco算法得到二维码中心坐标Q,依据机器人中心相对于地图的位置、摄像头在机器人上的安装位置,得到二维码相对于机器人的坐标Q1,依据二维码在对接设备上的位置,得到对接设备相对于机器人的坐标点P2,坐标点P2为目标对接点,已知机器人的当前坐标R0、目标对接点的坐标P2,在对接设备的对接方向上取过渡点P3,利用Bezier一阶曲线规划P2至P3路线为路径path1,利用Bezier二阶曲线规划P3至R0路线为路径Path2,将路径path1和路径path2合并为路径path3,控制机器人沿路径path3移动,使机器人的当前坐标R0和目标对接点P2重合。本发明可防止与对接设备的侧边发生干涉,提高对接精度。(The invention discloses a multi-sensor equipment docking method, which utilizes a camera on a robot to scan a two-dimensional code on docking equipment, obtaining a center coordinate Q of the two-dimensional code through an aruco algorithm, obtaining a coordinate Q1 of the two-dimensional code relative to the robot according to the position of the center of the robot relative to a map and the installation position of a camera on the robot, obtaining a coordinate point P2 of the docking equipment relative to the robot according to the position of the two-dimensional code on the docking equipment, wherein the coordinate point P2 is a target docking point, and knowing a current coordinate R0 of the robot and a coordinate P2 of the target docking point, taking a transition point P3 in the docking direction of the docking equipment, taking a Path from P2 to P3 as a Path1 by using Bezier first-order curve planning, taking a Path from P3 to R0 as a Path2 by using Bezier second-order curve planning, combining the Path1 and the Path2 into a Path3, and controlling the robot to move along the Path3 so that the current coordinate R0 of the robot is superposed with the target docking point P2. The invention can prevent the interference with the side edge of the butt joint equipment and improve the butt joint precision.)
技术领域
本发明涉及移动机器人设备对接
技术领域
，具体为一种基于多传感器设备对接方法。背景技术
目前，在智能机器人领域，机器人对接充电、货架等辅助配件的时候，存在定位效率低、速度慢、稳定性不高的问题。现有的一般利用二维码定位、激光或者视觉等传感器定位，然后边规划边控制，但是在机器人移动过程中数据会存在不稳定性的问题，导致定位精度降低、效率不高、稳定性差。
发明内容
本发明的目的是克服上述现有技术的缺点，提供一种可防止与对接设备的侧边发生干涉，避免对接失败，提高体验效果和对接精度，通用性较强的基于多传感器设备对接方法。
本发明是通过以下技术方案来实现的：一种基于多传感器设备对接方法，包括以下步骤：
S1.机器人通过内置的导航系统前往对接设备附近的预设对接点P1(x1，y1，yaw1)，P1(x1，y1，yaw1)为相对于地图原点的坐标；
S2.获取机器人的当前坐标R0(x0，y0，yaw0)，R0(x0，y0，yaw0)为相对于地图原点的坐标；
S3.机器人上的摄像头扫描对接设备上的二维码，通过aruco算法计算得到二维码中心坐标Q(x0,y0,yaw0)；
S4.依据机器人中心相对于地图的位置、以及摄像头在机器人上的安装位置，计算得到二维码相对于机器人的坐标Q1(x1,y1,yaw1)；
S5.依据二维码在对接设备上的位置，通过坐标转换，计算得到对接设备相对于机器人的坐标P2(x2,y2,yaw2)，坐标P2(x2,y2,yaw2)为目标对接点；
S6.已知机器人的当前坐标R0(x0，y0，yaw0)、目标对接点的坐标P2(x2,y2,yaw2)，在对接设备的对接方向上取过渡点P3(x3,y3,yaw3)，过渡点P3(x3,y3,yaw3)为相对于机器人的坐标点；
S7.利用Bezier一阶曲线规划P2至P3路线为直线路径path1，利用Bezier二阶曲线规划P3至R0路线为路径path2，将路径path1和路径path2合并为路径path3；
S8.采用Pure Pursuit路线跟踪算法控制机器人沿规划的路径path3移动，使机器人的当前坐标R0(x0，y0，yaw0)和目标对接点P2(x2,y2,yaw2)重合。
进一步地：步骤S6中所述在对接方向上取过渡点P3的方法为：
S61.设对接目标点P2沿对接方向与对接设备边界的交点设为a1；
S62.设对接目标点P2与a1之间的距离为a，a1到过度点P3的距离为S1；
S63.若机器人为圆形，机器人的半径设为R，则S1>R，若机器人为长方形，机器人的长度设为L，则S1>L/2，目标对接点P2到过渡点P3的距离设为S2，则S2≥a+L/2，Y方向的距离为S3；
S64.设过渡点P3(x3,y3,yaw3)和目标对接点P2(x2,y2,yaw2)方向一致，以目标对接点P2建立坐标系，通过公式：
X3＝X2+S2*cos(yaw2)-S3*sin(yaw2)、y3＝y2+S2*sin(yaw2)+S3*cos(yaw2)、yaw3＝yaw2，得到过渡点P3(x3,y3,yaw3)。
进一步地：步骤S2和步骤S3之间还包括，通过机器人内置的IMU传感器校准机器人角度，使机器人的当前坐标R0的yaw0值与预设对接点P1的yaw1值的差小于等于预设阈值。
进一步地：所述预设阈值为1°。
进一步地：步骤8中控制机器人沿规划的路径path3移动时，将再次识别的二维码相对于机器人的坐标Q1(x1,y1,yaw1)和初次识别的二维码中心坐标Q(x0,y0,yaw0)进行对比，计算两者的差值，并判断两者的差值是否超过阈值，若是，则重复执行步骤S4，若否，则继续沿路径path3移动。
进一步地：计算再次识别的二维码相对于机器人的坐标Q1(x1,y1,yaw1)和初次识别的二维码中心坐标Q(x0,y0,yaw0)两者的差值的方法为：
Dx＝x0-x1、Dy＝y0-y1、Yaw＝yaw0-yaw1，若Dx<0.01,并且Dy<0.01,Yaw<1，则继续沿路径path3移动，反之，则重复执行步骤S4。
本发明的有益效果：
与现有技术相比，本发明通过机器人本身自带的摄像头扫描对接设备上的二维码，通过aruco算法计算得到二维码中心坐标Q(x0,y0,yaw0)，并依据机器人中心相对于地图的位置、以及摄像头在机器人上的安装位置，计算得到二维码相对于机器人的坐标Q1(x1,y1,yaw1)，依据二维码在对接设备上的位置，通过坐标转换，计算得到对接设备相对于机器人的坐标点P2，坐标点P2为目标对接点，然后依据机器人的当前坐标R0、目标对接点的坐标P2，在对接设备的对接方向上取过渡点P3，过渡点P3为相对于机器人的坐标点，从而把目标对接点P2、过渡点P3、机器人的当前坐标R0转换至机器人地图坐标系，且过渡点P3距离对接目标点P2沿对接方向与对接设备的交点大于机器人或其他设备的半径或大于机器人或其他设备自身长度的二分之一，再利用Bezier一阶曲线规划P2至P3路线为直线路径path1，利用Bezier二阶曲线规划P3至R0路线为路径Path2，将路径path1和路径path2合并为路径path3，采用Pure Pursuit路线跟踪算法控制机器人沿规划的路径path3移动，使机器人的当前坐标R0(x0，y0，yaw0)和目标对接点P2(x2,y2,yaw2)重合。
机器人沿规划的路径path3移动时采用过渡点P3进行约束，在机器人与对接设备对接之前，先使机器人中心垂直于对接设备平面中心点，因此，取过渡点P3时考虑机器人或其他设备的旋转半径，在机器人还没移动到使其中心垂直于对接设备平面中心点前，可防止机器人或其他设备和对接设备的侧边发生干涉，避免对接失败，提高体验效果和对接精度，且定位方法可采用二维码、激光雷达、红外传感器、超声波传感器等已知现有的定位传感器和定位算法均可实现，通用性较强。
附图说明
图1为本发明基于多传感器设备对接方法流程图；
图2为本发明机器人和对接设备对接的示意图。
附图标记说明：1-机器人，2-对接设备。
具体实施方式
参照图1，本发明一种基于多传感器设备对接方法，包括以下步骤：
S1.机器人通过内置的导航系统前往对接设备附近的预设对接点P1(x1，y1，yaw1)，P1(x1，y1，yaw1)为相对于地图原点的坐标。
S2.获取机器人的当前坐标R0(x0，y0，yaw0)，R0(x0，y0，yaw0)为相对于地图原点的坐标。
S3.机器人上的摄像头扫描对接设备上的二维码，通过aruco算法计算得到二维码中心坐标Q(x0,y0,yaw0)。
S4.依据机器人中心相对于地图的位置、以及摄像头在机器人上的安装位置，计算得到二维码相对于机器人的坐标Q1(x1,y1,yaw1)。
S5.依据二维码在对接设备上的位置，通过坐标转换，计算得到对接设备相对于机器人的坐标P2(x2,y2,yaw2)，坐标P2(x2,y2,yaw2)为目标对接点。
S6.已知机器人的当前坐标R0(x0，y0，yaw0)、目标对接点的坐标P2(x2,y2,yaw2)，在对接设备的对接方向上取过渡点P3(x3,y3,yaw3)，过渡点P3(x3,y3,yaw3)为相对于机器人的坐标点。
参照图2，具体的，在对接方向上取过渡点P3的方法为：
S61.设对接目标点P2沿对接方向与对接设备边界的交点设为a1。S62.设对接目标点P2与a1之间的距离为a，a1到过度点P3的距离为S1。
S63.若机器人为圆形，机器人的半径设为R，则S1>R，若机器人为长方形，机器人的长度设为L，则S1>L/2，目标对接点P2到过渡点P3的距离设为S2，则S2≥a+L/2，Y方向的距离为S3。
S64.设过渡点P3(x3,y3,yaw3)和目标对接点P2(x2,y2,yaw2)方向一致，以目标对接点P2建立坐标系，通过公式：
X3＝X2+S2*cos(yaw2)-S3*sin(yaw2)、y3＝y2+S2*sin(yaw2)+S3*cos(yaw2)、yaw3＝yaw2，得到过渡点P3(x3,y3,yaw3)。
S7.利用Bezier一阶曲线规划P2至P3路线为直线路径path1，利用Bezier二阶曲线规划P3至R0路线为路径Path2，将路径path1和路径path2合并为路径path3；
S8.采用Pure Pursuit路线跟踪算法控制机器人沿规划的路径path3移动，使机器人的当前坐标R0(x0，y0，yaw0)和目标对接点P2(x2,y2,yaw2)重合。
具体的，控制机器人沿规划的路径path3移动时，将再次识别的二维码相对于机器人的坐标Q1(x1,y1,yaw1)和初次识别的二维码中心坐标Q(x0,y0,yaw0)进行对比，计算两者的差值，并判断两者的差值是否超过阈值，若是，则重复执行步骤S4，若否，则继续沿路径path3移动。
计算再次识别的二维码相对于机器人的坐标Q1(x1,y1,yaw1)和初次识别的二维码中心坐标Q(x0,y0,yaw0)两者的差值的方法为：
Dx＝x0-x1、Dy＝y0-y1、Yaw＝yaw0-yaw1，若Dx<0.01,并且Dy<0.01,Yaw<1，则继续沿路径path3移动，反之，则重复执行步骤S4。
步骤S2和步骤S3之间还包括，通过机器人内置的IMU传感器校准机器人角度，使机器人的当前坐标R0的yaw0值与预设对接点P1的yaw1值的差小于等于预设阈值。
具体的，所述预设阈值为1°。
本发明基于多传感器设备对接方法可应用于较大型的对接设备，包括机器人、无人驾驶清扫车充电、无人驾驶清扫车入库、无人驾驶清扫车倒垃圾、无人驾驶清扫车加水、无人驾驶货车对接充电等对接方案。
上列详细说明是针对本发明可行实施例的具体说明，该实施例并非用以限制本发明的专利范围，凡未脱离本发明所为的等效实施或变更，均应包含于本案的专利范围中。
- 上一篇：一种医用注射器针头装配设备
- 下一篇：一种机器人移动控制方法、装置、设备及存储介质