视觉惯性融合与自适应残差控制的充电枪插接方法及系统

摘要: 本发明涉及充电枪,尤其涉及一种视觉惯性融合与自适应残差控制的充电枪插接方法及系统。背景技术:1、随着电动车无人化补能和充电站自动化需求提升,自动插拔充电枪技术逐...
视觉惯性融合与自适应残差控制的充电枪插接方法及系统

本发明涉及充电枪,尤其涉及一种视觉惯性融合与自适应残差控制的充电枪插接方法及系统。

背景技术:

1、随着电动车无人化补能和充电站自动化需求提升,自动插拔充电枪技术逐渐应用于停车场、换电站、自动充电站和移动充电机器人等场景。现有自动充电方案通常依赖视觉识别、机械臂路径规划、末端柔顺机构或力/力矩传感器来完成充电枪与车端充电口之间的定位和插接。对于国标直流充电口而言,充电枪插头具有明确的中心轴、导向结构和防呆结构,自动插接过程对插入轴角度、横向偏差、绕轴姿态、插入速度以及插入过程中的冲击和卡滞均较为敏感。

2、在实际自动插枪过程中,视觉系统能够在插入前获得车端充电口的位置和孔轴方向,但当视觉完成孔轴定位并进入实际插入阶段后,仍可能受到机械臂微振动、线缆拖拽、枪体局部偏摆、充电枪自重、车端充电口装配误差、视觉短时遮挡或视觉刷新延迟等因素影响。传统规则控制器虽然能够基于横向偏差和轴向方向执行基础修正,但在不同车辆、不同充电座姿态、不同线缆受力和不同视觉误差条件下,固定阈值或固定补偿量难以持续适配真实插接过程。

3、若仅依赖机械臂关节编码器或末端法兰位姿进行判断,控制器难以直接观测充电枪本体尤其是枪头附近的短时姿态变化;若采用力/力矩传感器进行接触后柔顺控制,则系统成本、结构复杂度和调试难度增加。因此,需要一种在保留传统插枪控制安全性的基础上,能够利用真实插枪数据持续修正控制残差,并结合充电枪的惯性运动数据识别冲击、卡滞和姿态异常,将冲击特征作为安全约束限幅和残差接入系数动态调节的依据,在检测到冲击时自动降低残差接入权重或触发速度限幅,实现基于物理感知的安全联动。

4、公开号为cn118269095a的现有方案利用强化学习策略生成机械臂多自由度控制动作,策略输出直接参与机械臂整体运动控制;另有复合控制方案采用固定权重叠加传统控制动作和强化学习动作,难以随插入深度、接触冲击特征及几何误差实时调整学习动作的参与程度。

5、为了解决现有技术中传统规则控制器难以持续适配真实插接过程,以及依赖机械臂关节编码器反馈而无法感知充电枪本体姿态变化的问题,本发明提供一种视觉惯性融合与自适应残差控制的充电枪插接方法及系统。

技术实现思路

1、本发明的目的是为了解决现有技术中传统规则控制器难以持续适配真实插接过程,以及依赖机械臂关节编码器反馈而无法感知充电枪本体姿态变化的问题。

2、为解决以上技术问题,本发明的技术方案为:

3、一种视觉惯性融合与自适应残差控制的充电枪插接方法,用于通过机械臂带动充电枪移动,从而完成车端充电口与充电枪的插接,包括

4、s1.获取车端充电口的位姿数据、充电枪的实时位姿、速度和惯性运动数据以及上一周期最终插枪动作;基于车端充电口的位姿数据和充电枪的位姿数据,计算插枪引导所需的几何偏差和插接深度,并根据插接深度确定当前插接状态;

5、s2.基于几何偏差、插接深度和插接状态构建基础控制状态,并输入机械臂的基础控制器,生成基础插枪动作;

6、s3.基于充电枪实时惯性运动数据,得到冲击特征,结合基础控制状态、实时惯性运动数据、基础插枪动作和历史插枪动作构建残差输入特征,并输入预先训练的残差学习网络,输出残差补偿量;

7、s4.基于基础插枪动作和残差补偿量,生成指令动作;

8、s5.基于预设的安全约束条件修正指令动作,生成最终插枪动作。

9、进一步地,计算插枪引导所需的几何偏差,具体为:

10、车端充电口的位姿数据包括车端充电口的孔轴方向和孔中心位置数据;

11、充电枪的位姿数据包括当前枪头tcp位置和枪轴方向数据;

12、几何偏差包括横向位置偏差、枪轴-孔轴夹角偏差和绕孔轴方向偏差;

13、将车端充电口的位姿数据、充电枪的位姿数据统一为机器人基坐标系数据;

14、横向位置偏差:

15、

16、其中,为单位矩;为归一化后的锁定孔轴单位向量;为孔中心在机器人基坐标系下的位置向量;为充电枪枪头tcp在机器人基坐标系下的位置向量;

17、枪轴-孔轴夹角偏差:

18、

19、其中,为机器人基坐标系中的枪轴单位向量;为机器人基坐标系中的锁定孔轴单位向量;为将输入值限制在负1至1之间;

20、绕孔轴方向偏差:

21、

22、其中,为投影到垂直于锁定孔轴平面并归一化后的枪头x轴单位向量;为投影到垂直于锁定孔轴平面并归一化后的充电口x轴单位向量;atan2为四象限反正切函数,用于确定方向偏差的符号和大小。

23、进一步地,插入深度的计算为:

24、

25、其中,为将输入值限制在负1至1之间;为枪头tcp位置在机器人基坐标系b下沿孔轴方向的投影;为当前控制周期t枪头tcp在机器人基坐标系b中的位置;为插入深度为0时的枪头tcp在机器人基坐标系b中的参考位置;为预设的最大插入深度;

26、根据插入深度确定当前插接状态具体为:

27、当插接深度不为0时,当前处于已插接状态;当插接深度为0时,当前处于未插接状态。

28、进一步地,得到冲击特征,具体为:

29、充电枪惯性运动数据包括枪体角速度和枪体加速度,冲击特征:

30、

31、其中,为第t个控制周期的冲击特征;为加速度的非负权重;为角速度的非负权重;为第t个控制周期枪体坐标系下的的加速度;为机械臂控制充电枪处于静止状态时在预设的时间窗内采集的加速度均值;为第t个控制周期枪体坐标系下的角速度。

32、进一步地,残差输入特征为:

33、

34、其中,为基础控制状态,,和为枪头tcp相对于孔中心的横向位置偏差,为枪轴-孔轴夹角偏差,为绕孔轴方向偏差,为插入深度,为插接状态;

35、为充电枪的速度,即tcp速度;

36、和分别为枪体坐标系下的枪体角速度和加速度;

37、为冲击特征;为基础插枪动作,为上一控制周期的最终插枪动作。

38、进一步地,残差学习网络采用强化学习算法,其训练目标为最大化累计折扣奖励与策略熵的加权和;

39、目标函数为:

40、

41、其中,为残差策略的目标函数值;为期望算子;t为控制周期;为控制周期t的折扣因子;为是控制周期t的单步奖励函数值;为熵正则系数;为策略熵;为残差输入特征;

42、其中,奖励函数由正向奖励与惩罚项构成:

43、

44、其中,为正向奖励项;为惩罚项;

45、

46、其中,为误差收敛奖励;为插入推进权重;为控制周期t插入深度推进量;为插接成功奖励;为控制周期t插接成功指示量;

47、

48、其中,为控制周期t的横向位置偏差模长;为控制周期t+1的横向位置偏差模长;为横向位置偏差权重;为枪轴-孔轴夹角偏差权重;为控制周期t的枪轴-孔轴夹角偏差;为控制周期t+1的枪轴-孔轴夹角偏差;为绕孔轴方向偏差权重;为控制周期t的绕孔轴方向偏差绝对值;为控制周期t+1的绕孔轴方向偏差绝对值;

49、

50、其中,为控制周期t残差动作惩罚;为冲击特征惩罚权重;为控制周期t冲击特征;为插接失败惩罚;为插接失败指示量;

51、

52、其中,为动作幅值惩罚权重;为控制周期t残差补偿量;为控制周期t-1残差补偿量;为动作变化惩罚权重。

53、进一步地,基于基础插枪动作和残差补偿量,生成指令动作,具体为:

54、

55、其中,为控制周期t的指令动作;为基础插枪动作;为接入系数;为残差补偿量;

56、

57、其中,为基准接入系数;为插入深度调节因子;为冲击特征调节因子;

58、为几何偏差调节因子;其中,为横向位置偏差模长,为枪轴-孔轴夹角偏差,为绕孔轴方向偏差的绝对值。

59、进一步地,预设的安全约束条件包括

60、第t个控制周期的横向位移分量小于等于预设的最大允许横向位移增量:

61、;

62、第t个控制周期的枪轴-孔轴夹角分量小于等于预设的最大允许角度变化量:

63、;

64、第t个控制周期的插入速度大于等于0且小于等于预设的最大允许插入速度:

65、;

66、当第t个控制周期的横向位移分量小于等于预设的最大允许横向位移增量时,则将第t个控制周期的横向位移修正量作为最终横向位移分量;

67、当第t个控制周期的横向位移分量超过预设的最大允许横向位移增量时,则将预设的最大允许横向位移增量作为最终横向位移分量;

68、当第t个控制周期的枪轴-孔轴夹角分量小于等于预设的最大允许角度变化量时,则将第t个控制周期的枪轴-孔轴夹角分量作为最终角度变化分量;

69、当第t个控制周期的枪轴-孔轴夹角分量超过预设的最大允许角度变化量时,则将预设的最大允许角度变化量作为最终角度变化分量;

70、当第t个控制周期的插入速度大于等于0且小于等于预设的最大允许插入速度时,则将第t个控制周期的插入速度作为最终插入速度;若第t个控制周期的插入速度小于0时,最终插入速度为0;若第t个控制周期的插入速度大于最大允许插入速度时,则将最大允许插入速度作为最终插入速度。

71、本发明还提供一种视觉惯性融合与自适应残差控制的充电枪插接系统,用于执行任一项所述的一种视觉惯性融合与自适应残差控制的充电枪插接方法,包括机械臂、充电枪、视觉感知模块、imu模块和控制器;

72、充电枪和视觉感知模块安装在机械臂末端,机械臂带动充电枪和视觉感知模块移动;

73、视觉感知模块用于采集车端充电口的图像,经视觉位姿解算后得到位姿数据;

74、imu模块,安装在充电枪本体的手柄段,获取imu原始加速度和原始角速度;

75、控制器安装在充电枪本体上,分别与机械臂、视觉感知模块和imu模块通信连接,用于获取各模块和机械臂数据,并生成最终插枪动作指令下发至机械臂执行。

76、进一步地,控制器包括

77、数据获取单元,用于获取视觉感知模块输出的车端充电口位姿数据、通过机械臂获取充电枪的实时位姿数据、获取imu原始加速度和原始角速度,以及获取上一周期最终插枪动作;

78、数据处理单元,基于车端充电口位姿数据和充电枪的实时位姿数据,计算插枪引导所需的几何偏差和插接深度,并根据插接深度得出当前插接状态;以及基于获取imu原始加速度和原始角速度获得实时惯性运动数据并提取冲击特征;

79、基础控制单元,用于基于几何偏差、插接深度和插接状态构建基础控制状态,并调用机械臂内部的基础控制器生成基础插枪动作;

80、特征构建单元,用于构建残差输入特征,残差输入特征包括基础控制状态、实时惯性运动数据、基础插枪动作和上一周期最终插枪动作,并将当前控制周期的残差输入特征输出至残差补偿单元和数据存储单元;

81、残差补偿单元,内置有预先训练的残差学习网络,接收残差输入特征,并输入预先训练的残差学习网络,输出残差补偿量;

82、动作合成单元,获取基础插枪动作和残差补偿量,生成指令动作;

83、安全约束单元,用于基于预设的安全约束条件修正指令动作,生成当前控制周期的最终插枪动作,并将最终插枪动作下发至机械臂执行,并将当前控制周期的最终插枪动作输出至数据存储单元;

84、数据存储单元,用于存储历史数据,历史数据包括多个历史控制周期的基础插枪动作、残差输入特征、残差补偿量以及最终插枪动作。

85、本发明具有如下有益效果:

86、本发明通过车端充电口位姿数据和充电枪实时位姿数据,计算横向位置偏差、枪轴-孔轴夹角偏差和绕孔轴方向偏差等几何偏差,基于计算出的插接深度,并根据插接深度确定当前插接状态,为后续基础控制和残差补偿提供误差依据和插接状态判断基础。

87、利用机械臂内部的基础控制器基于几何偏差、插接深度和插接状态生成基础插枪动作,保证常规工况下系统能够独立完成插接,同时实时采集充电枪的加速度和角速度等惯性运动数据,据此提取冲击特征,并将基础控制状态、实时惯性运动数据、基础插枪动作和上一周期最终插枪动作共同构建为残差输入特征,输入预先训练的残差学习网络输出残差补偿量;其中,基础插枪动作的引入使残差学习网络获知基础控制器输出的当前控制意图,实时惯性运动数据的引入使残差学习网络获知充电枪本体的实际运动状态, 从而使残差策略能够在基础控制意图的基础上学习增量修正,避免强化学习策略直接承担全部控制任务所带来的不确定性。

88、本发明通过基础插枪动作与残差补偿量叠加生成指令动作,其中残差补偿量的叠加程度由插入深度、冲击特征和几何偏差共同决定:插入深度浅时允许较大叠加以加速纠偏,冲击特征增大时自动减小叠加以抑制扰动,几何偏差趋零时逐步退出以防止末端抖动;叠加后的指令动作经预设的安全约束条件修正后输出最终插枪动作,将最终插枪动作限制在安全范围内,相比固定权重叠加方案,残差策略的参与程度始终与实际物理状态实时耦合,使补偿量的介入时机和强度随插接进程自适应变化,提高了控制的适应性和平稳性。

89、本发明的残差学习网络采用强化学习算法,其训练目标为最大化累计折扣奖励与策略熵的加权和;其中奖励函数通过误差收敛、插入推进和插接成功提供正向激励,通过动作幅值惩罚、变化惩罚、冲击惩罚和失败惩罚抑制不当动作,从而使残差学习网络的输出的残差补偿量能够实现精准、平稳、安全地完成插接。