高精度差分车回充功能控制方法与流程

本发明涉及移动机器人运动控制领域，具体是一种高精度差分车回充功能控制方法。

背景技术：

1、近年来，移动机器人与自动驾驶领域技术兴起并取得长足发展。市面上销售的轮式移动机器人或者复合机器人中，按照轮驱动方式动力来源主要分为差速模型，阿克曼模型和全向模型三种。其中差速模型由于其驱动方式简单，且运动方式灵活，被广泛应用到各种商业场景中，例如，无人仓储，医疗轮椅，3c，室内外清洁等。

2、移动机器人涉及的基本技术有传感器技术、信息处理技术、自动化控制技术、导航技术。其中的导航控制模块要根据不同的使用方式，不同的地面路面以及不同的障碍物情况调整控制方式以及精度要求。自动回充功能作为产品功能模块中比较重要的一部分更是对控制方式和精度有着非常高的要求，其中的对桩运动控制算法要在满足够精度的情况下尽量使得运动路径精简。

3、普遍的双轮差速移动机器人回充对桩系统主要有以下两种：

4、1.在全局定位精度足够高的情况下直接使用通用导航局部规划算法实现对桩，其对于全局精度要求非常高。而在全局定位精度不高的情况下，一般的导航控制算法会因为定位跳动而导致对桩精度不足，或者在终点处反复尝试导致过程不够简洁。

5、2.利用多组红外传感器施行实时定位，从而控制机器人精准定位到充电桩的位置。但这种方法对传感器稳定性要求过高。

技术实现思路

1、本发明为了解决现有技术的问题，提供了一种满足了在传感器精度不是很高的情况下实现了对桩的精准控制。

2、本发明采用如下控制装置，在充电桩点位上面部署一个二维码或者提供一个固定几何形状的信息如v型槽等，同时机器人上面要配有相应的感知传感器如相机，二维码等，通过相机或者激光雷达传感器实时获取二维码或者几何形状的位置信息。

3、本发明包括以下步骤：

4、1)识别：识别出终点坐标在世界坐标系下的坐标；

5、2)滤波：对步骤1)识别到的坐标进行滤波处理，消除误差；

6、所述滤波过程参照扩展卡尔曼滤波，具体有以下几个步骤：

7、2.1)建立第k次迭代的先验状态向量第k次迭代的输入向量uk，状态转移矩阵fk，输入转移矩阵bk和观测转移矩阵h：

8、uk＝[v ω]t

9、

10、

11、h＝i3

12、2.2)给出过程噪声的协方差矩阵qk和观测噪声的协方差矩阵rk：

13、

14、2.3)状态转换关系更新下一步预测值：

15、

16、pk＝fkpk-1fkt+qk

17、其中，为第k+1次迭代的后验状态向量，pk为第k时刻的后验估计协方差矩阵；

18、2.4)根据观测计算卡尔曼增益kk：

19、

20、2.5)应用观测值修正预测值，并将其中的状态修正值作为滤波的输出并作为下一次迭代的输入进行下一次迭代：

21、

22、pk'＝(i-kkh)pk

23、其中，为第k+1次迭代的后验状态向量，zk为第k次迭代的观测向量，由传感器给出，pk'为第k时刻的后验估计协方差矩阵。

24、3)运动控制：根据实时的目标点位计算出机器人应当发给底层控制的线速度v和角速度ω，直到车成功到终点，具体方法如下：

25、3.1)根据机器人距离目标沿线的横向误差计算跟随距离；

26、ld＝|yrg|+lcontrol

27、其中，id为前视距离，yrg为目标点在世界坐标系下的y坐标，lcontrol为调整参数，通过调整lcontrol以控制跟随的效果；

28、3.2)根据跟随距离计算出机器人中心点与追踪点之间的连线在世界坐标系下的朝向δ；

29、

30、3.3)根据δ和机器人在世界坐标系下的朝向θwr计算出机器人中心点与追踪点之间的连线与机器人朝向之间的夹角β；

31、β＝δ+θwr

32、3.4)根据β计算出车的控制点的速度朝向，使得当控制点朝着此方向前进一段时间后，机器人的中心带刚好经过预瞄点，即机器人最前方或者最后方的正中心点；

33、

34、其中，θ为机器人控制点的速度的朝向在机器人坐标系下的表示，l为控制点距离中心的长度，v为车中心点在世界坐标系下的速度，kcontrol为控制强度；

35、3.5)在得到θ后根据几何关系计算出车的角速度ω，旋转半径r，然后计算中心点速度v：

36、

37、

38、v＝ωr

39、其中，vc为控制点在世界坐标系下的速度。

40、本发明有益效果在于：

41、1.本发明提出了一种新的运动控制方法可以弥补全局定位不足而导致的导航对桩不精准，从而降低了对做全局定位的传感器的精度要求。

42、2.基于本方法可以在看不到特征的时候依然保持正确的行进方向。

43、3.本方法的运动控制可以兼容前进和后退两种运动方式。

44、4.本方法兼备了运动方式的简洁，运动高精度以及运动对空间的低要求。

技术特征：

1.一种高精度差分车回充功能控制方法，其特征在于包括以下步骤：

2.根据权利要求1所述的高精度差分车回充功能控制方法，其特征在于：步骤2)所述滤波过程参照扩展卡尔曼滤波，具体有以下几个步骤：

3.根据权利要求1所述的高精度差分车回充功能控制方法，其特征在于：步骤1)所述识别过程中，在充电桩点位上面部署识别设备，机器人上配置有与识别设备相应的感知设备。

4.根据权利要求3所述的高精度差分车回充功能控制方法，其特征在于：所述识别设备为二维码或者一个固定几何形状的信息。

5.根据权利要求4所述的高精度差分车回充功能控制方法，其特征在于：所述固定几何形状的信息为v型槽。

6.根据权利要求3所述的高精度差分车回充功能控制方法，其特征在于：所述感知设备为相机。

技术总结
本发明提供了一种高精度差分车回充功能控制方法，首先识别出终点坐标在世界坐标系下的坐标，然后对识别到的坐标进行滤波处理，消除误差，最后根据实时的目标点位计算出机器人应当发给底层控制的线速度v和角速度ω，直到车成功到终点。本发明提出了一种新的运动控制方法可以弥补全局定位不足而导致的导航对桩不精准，从而降低了对做全局定位的传感器的精度要求，兼备了运动方式的简洁，运动高精度以及运动对空间的低要求。

技术研发人员：张鸿博
受保护的技术使用者：深圳亿嘉和科技研发有限公司
技术研发日：
技术公布日：2024/1/15