CN112440273A - 一种三机械臂运动控制系统及方法 - Google Patents

Abstract

本发明涉及充电设备技术领域，特别涉及一种三机械臂运动控制系统及方法，包括充电装置、坐标定位单元、坐标输入单元、路线计算单元、运动装置驱动单元，充电装置为并联式三机械臂，通过步进电机驱动在导轨上运动；坐标定位单元对车载连接器进行定位并将坐标传递给路线计算单元；路线计算单元根据坐标定位单元传递过来的坐标确定充电装置是否可以达到，如果不能达到，则反馈给坐标输入单元，重新发送坐标；如果可以达到，则计算结果转换为相对应的PWM波形；运动装置驱动单元根据PWM波形驱动步进电机。本发明的机械臂采用并联式，不存在多级的累计误差，利用机械臂本身与目标位置的坐标关系，直接求解机械臂运动的距离，运动算法简单。

Description

一种三机械臂运动控制系统及方法

技术领域

本发明涉及充电设备技术领域，特别涉及一种三机械臂运动控制系统及方法。

背景技术

随着自动驾驶及自动泊车技术的发展，无人值守的自动充电成为电动汽车分支运行发展的必然趋势。在公路两侧等停车空间有限、适合离散充电桩的场所，针对电动汽车自动充电过程完全无人化、自主充电特点，以及自主充电过程中车辆停靠情况不同时充电口位置的离散化分成情况，本着降低充电设施成本的原则，结合机器人技术进行电动汽车自动插接自动充电具有重大意义。

中国专利公开号CN206954035U，公开日2018年02月02日，提出了一种滑轨式多自由度机械臂自动充电装置，用滑轨方式实现多自由度机械臂的自动充电，多滑轨串联方式实现机械臂空间运动，但是存在串联控制精度累积误差大和控制相关关联性大的缺点。

德国大众公司推出基于7轴KUKALBR iiwa库卡机器人和一系列感知设备的E-Smart Connect电动汽车自动快速充电系统。该机器人由7个传动轴组成，通过配备的感应摄像头可以捕捉停靠汽车上的充电插座位置，并转换到机器人坐标下，进行插接充电命令指挥，实现充电电缆与汽车充电口之间精确可靠的自动插拔效果，该充电系统可以实现电动汽车充电口在目标范围20cm*20cm区域内的自动充电工作，其缺点在于使用串联方式，控制精度要求高，存在每个轴的累积误差，控制算法要增加补偿计算，较为复杂。

因此，如何控制单机械臂精准地完成充电接口的移动和对接动作，是整个侧方充电运动的重中之重。一旦运动出现问题，很容易触碰车辆侧壁同时由于未移动到位电机还会持续工作，导致车辆的损伤甚至是电机的失转。因此设计一种侧方充电的机械臂运动控制系统是必不可少的。

发明内容

本发明解决了相关技术中串联控制精度累积误差大和控制算法较为复杂的问题，提出一种三机械臂运动控制系统及方法，采用并联式机械臂，不存在多级的累计误差，利用机械臂本身与目标位置的坐标关系，直接求解机械臂运动的距离，运动算法方法简单。

为了解决上述技术问题，本发明是通过以下技术方案实现的：一种三机械臂运动控制系统，包括：

充电装置，所述充电装置包括并三个并联式机械臂，所述机械臂通过步进电机驱动在导轨上运动；

坐标定位单元，所述坐标定位单元与坐标输入单元相连；

坐标输入单元，所述坐标输入单元用于输入坐标定位单元所定位的坐标；

路线计算单元，所述路线计算单元分别与坐标定位单元和运动装置驱动单元相连；

运动装置驱动单元，所述运动装置驱动单元分别与路线计算单元、电机相连。

作为优选方案，所述充电装置包括充电基架，所述充电基架上安装有三副导轨，各所述导轨上均安装有步进电机，所述步进电机上安装有万向座，所述万向座上分别连接有机械臂，各所述机械臂顶端连接在一起并连接有充电接口。

作为优选方案，所述导轨包括导轨I、导轨Ⅱ和导轨Ⅲ，所述导轨I、导轨Ⅱ和导轨Ⅲ之间平行设置，且所述导轨Ⅱ和导轨Ⅲ位于同一水平线上。

作为优选方案，所述机械臂包括机械臂I、机械臂Ⅱ和机械臂Ⅲ，所述机械臂I安装于导轨I上且为四连杆，所述机械臂Ⅱ安装于导轨Ⅱ上且为万向杆，所述机械臂Ⅲ安装于导轨Ⅲ上。

作为优选方案，所述坐标定位单元采用超声波传感器对车载连接器进行定位。

作为优选方案，所述路线计算单元为单片机。

作为优选方案，所述运动装置驱动单元为电机驱动器。

一种三机械臂运动系统控制方法，包括以下步骤：

S1、坐标定位单元对车载连接器进行定位并将坐标传递给路线计算单元；

S2、路线计算单元接收到坐标后计算出来机械臂所要移动的距离并转换成相对应的PWM波形；

S3、运动装置驱动单元根据路线计算单元传输过来的PWM波形，驱动步进电机从而带动机械臂运动到指定位置。

作为优选方案，在步骤S1和步骤S2之间增加如下判定步骤：

所述路线计算单元根据坐标定位单元传递过来的坐标确定机械臂是否可以达到，如果不能达到，则返回步骤S1；如果能到达，则执行步骤S2。

作为优选方案，步骤S2中的计算过程如下：

设导轨Ⅱ和导轨Ⅲ的上端连线的中心为原点，建立三维坐标系；

设导轨长度为d，导轨Ⅱ上端和导轨Ⅲ上端到原点的距离为a，导轨I下端距离原点的距离为b，则导轨I上下两端点坐标为(0,b+d,0)和(0,b,0)，导轨Ⅱ上下两端点坐标为(-a,0,0)和(-a,-d,0)，导轨Ⅲ上下两端点坐标为(a,0,0)和(a,-d,0)；

设每个机械臂长度为L，在导轨上移动的机械臂端为A端，连接充电接口的机械臂端称为B端，设机械臂I的A端坐标(0,y₁,0)，机械臂Ⅱ的A端坐标(-a,y₂,0)，机械臂Ⅲ的A端坐标(a,y₃,0)；

设充电接口的坐标为(x,y,z)，根据机械臂长度恒等于L，则需要满足如下公式：

x²+(y-y₁)²+z²＝L²

(x+a)²+(y-y₂)²+z²＝L²

(x-a)²+(y-y₃)²+z²＝L²

求得三个机械臂A端坐标为：

由于导轨长度固定，机械臂A端移动的范围可以确定下来，则A端坐标范围进一步减小为：

b≤y₁≤b+d

-d≤y₂≤0

-d≤y₃≤0

分别计算端点和初始位置的差值，选择差值小的端点作为机械臂运动的最后位置。

与现有技术相比，本发明的有益效果是：本发明的机械臂采用三机械臂并联式，不存在多级的累计误差，利用机械臂本身与目标位置的坐标关系，直接求解机械臂运动的距离，运动算法方法简单。

附图说明

图1是本发明充电装置结构示意图；

图2是本发明控制系统框架图；

图3是本发明的逻辑流程图；

图4是本发明xyz坐标建立的示意图；

图5是本发明中机械臂在xyz坐标中的示意图；

图6是本发明机械臂在xyz坐标中运动状态的示意图。

图中：

1、充电基架，2、步进电机，301、导轨I，302、导轨Ⅱ，303、导轨Ⅲ，4、充电接口，5、机械臂I，6、机械臂Ⅱ，7、万向座，8、机械臂Ⅲ。

具体实施方式

下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。

如图1至3所示，一种三机械臂运动控制系统，包括充电装置、坐标输入单元、坐标定位单元、路线计算单元、运动装置驱动单元。

充电装置包括充电基架1，充电基架1上安装有三副导轨，分别为导轨I301、导轨Ⅱ302和导轨Ⅲ303，导轨I301、导轨Ⅱ302和导轨Ⅲ303之间平行设置，且导轨Ⅱ302和导轨Ⅲ303位于同一水平线上；各导轨上均安装有步进电机2，步进电机2上安装有万向座7和减速器，减速器的作用是将步进电机2的速度减慢，同时提高力矩(比如电机现在能带动10kg的物体，带上减速器，那么速度减半，能带动20kg的物体)；各万向座7上分别连接有长度相等的机械臂I5、机械臂Ⅱ6和机械臂Ⅲ8，机械臂I5、机械臂Ⅱ6和机械臂Ⅲ8的顶端相连并连接有充电接口4；机械臂I5安装于导轨I301上且为四连杆，机械臂Ⅱ6安装于导轨Ⅱ302上且为万向杆，机械臂Ⅲ8安装于导轨Ⅲ303上。

坐标定位单元与坐标输入单元相连，坐标定位单元采用超声波传感器对车载连接器进行定位进行坐标输入并将坐标传递给路线计算单元。

坐标输入单元用于输入坐标定位单元所定位的坐标。

路线计算单元为单片机(可选用型号STM32H743)，路线计算单元分别与坐标定位单元和运动装置驱动单元相连；所述路线计算单元根据坐标定位单元传递过来的坐标确定机械臂是否可以达到，如果不能达到，则反馈给坐标输入单元，重新发送坐标；如果可以达到，则计算结果并转换为相对应的PWM波形。

运动装置驱动单元为电机驱动器，根据路线计算单元传输过来的PWM波形，驱动步进电机3从而带动机械臂运动到指定位置即可进行充电。

一种三机械臂运动系统控制方法，包括以下步骤：

S1、坐标定位单元对车载连接器进行定位并将坐标传递给路线计算单元；

S2、路线计算单元接收到坐标后计算出来机械臂所要移动的距离并转换成相对应的PWM波形；

S3、运动装置驱动单元根据路线计算单元传输过来的PWM波形，驱动步进电机从而带动机械臂运动到指定位置。

其中，在步骤S1和步骤S2之间增加如下判定步骤：

路线计算单元根据坐标定位单元传递过来的坐标确定机械臂是否可以达到，如果不能达到，则返回步骤S1；如果能到达，则执行步骤S2。

路线计算单元计算机械臂运动的原理如下：

路线计算单元将机械臂的运动分为两个阶段。

第一阶段，计算机械臂是否可以达到坐标定位单元传递过来的坐标，如果不能达到，则反馈给坐标输入单元，重新发送坐标(因为车在移动，超声波传感器在实时定位，所以每次送过来的位置信息是不同的，一旦位置满足，就可以停车并且不用再发送了)；如果能够达到，则计算出具体需要移动的距离，具体计算过程如下：

如图4所示，三个导轨按照先上下，后左右的方式，分别命名为导轨I303、导轨Ⅱ302、导轨Ⅲ303，设导轨Ⅱ302和导轨Ⅲ303的上端连线的中心为原点，建立三维坐标系，设导轨长度为d，导轨Ⅱ302上端和导轨Ⅲ303上端到原点的距离为a，导轨I301下端距离原点的距离为b，则导轨I301上下两端点坐标为(0,b+d,0)和(0,b,0)，导轨Ⅱ302上下两端点坐标为(-a,0,0)和(-a,-d,0)，导轨Ⅲ303上下两端点坐标为(a,0,0)和(a,-d,0)。

如图5所示，在各个导轨上移动的机械臂分别为机械臂I5、机械臂Ⅱ6、机械臂Ⅲ8，设每个机械臂长度为L，在导轨上移动的机械臂端为A端，连接充电接口4的机械臂端称为B端。设机械臂I5的A端坐标(0,y₁,0)，机械臂Ⅱ6的A端坐标(-a,y₂,0)，机械臂Ⅲ8的A端坐标(a,y₃,0)。

设充电接口4的坐标为(x,y,z)，根据机械臂长度恒等于L，则需要满足如下公式：

x²+(y-y₁)²+z²＝L²

(x+a)²+(y-y₂)²+z²＝L²

(x-a)²+(y-y₃)²+z²＝L²

求得三个机械臂A端坐标为：

由于导轨长度固定，机械臂A端移动的范围可以确定下来，则A端坐标范围进一步减小为：

b≤y₁≤b+d

-d≤y₂≤0

-d≤y₃≤0

由以上可知，即便各个机械臂A端的坐标加以限定，但依然还是有可能出现坐标两解的情况。如图6所示，在xy平面上，对于同一个目标坐标，导轨Ⅱ302和导轨Ⅲ303上，出现两个关于对称轴对称的机械臂Ⅱ6和机械臂Ⅲ8的轨迹可以满足如上所有条件，即同一个机械臂有两个A端满足条件。对于上述同一个机械臂上均满足条件的两个A端的情况，分别计算端点和初始位置的差值，选择差值小的端点作为机械臂运动的最后位置。

第二阶段，控制机械臂运动流程，将上述计算出来的y₁,y₂,y₃，根据步进电机2以及减速器的特性转换为对应的PWM波形(步进电机2是用PWM波形控制的，一个PWM波形，步进电机2可转1.8度，步进电机2旋转360度，机械臂移动的距离为2mm)。

以上为本发明较佳的实施方式，本发明所属领域的技术人员还能够对上述实施方式进行变更和修改，因此，本发明并不局限于上述的具体实施方式，凡是本领域技术人员在本发明的基础上所作的任何显而易见的改进、替换或变型均属于本发明的保护范围。

Claims (10)

1.一种三机械臂运动控制系统，其特征在于，包括：

充电装置，所述充电装置包括并三个并联式机械臂，所述机械臂通过步进电机(3)驱动在导轨上运动；

坐标定位单元，所述坐标定位单元与坐标输入单元相连；

坐标输入单元，所述坐标输入单元用于输入坐标定位单元所定位的坐标；

路线计算单元，所述路线计算单元分别与坐标定位单元和运动装置驱动单元相连；

运动装置驱动单元，所述运动装置驱动单元分别与路线计算单元、电机(3)相连。

2.根据权利要求1所述的三机械臂运动控制系统，其特征在于：所述充电装置包括充电基架(1)，所述充电基架(1)上安装有三副导轨，各所述导轨上均安装有步进电机(2)，所述步进电机(2)上安装有万向座(7)，所述万向座(7)上分别连接有机械臂，各所述机械臂顶端连接在一起并连接有充电接口(4)。

3.根据权利要求2所述的三机械臂运动控制系统，其特征在于：所述导轨包括导轨I(301)、导轨Ⅱ(302)和导轨Ⅲ(303)，所述导轨I(301)、导轨Ⅱ(302)和导轨Ⅲ(303)之间平行设置，且所述导轨Ⅱ(302)和导轨Ⅲ(303)位于同一水平线上。

4.根据权利要求3所述的三机械臂运动控制系统，其特征在于：所述机械臂包括机械臂I(5)、机械臂Ⅱ(6)和机械臂Ⅲ(8)，所述机械臂I(5)安装于导轨I(301)上且为四连杆，所述机械臂Ⅱ(6)安装于导轨Ⅱ(302)上且为万向杆，所述机械臂Ⅲ(8)安装于导轨Ⅲ(303)上。

5.根据权利要求1所述的三机械臂运动控制系统，其特征在于：所述坐标定位单元采用超声波传感器对车载连接器进行定位。

6.根据权利要求1所述的三机械臂运动控制系统，其特征在于：所述路线计算单元为单片机。

7.根据权利要求1所述的三机械臂运动控制系统，其特征在于：所述运动装置驱动单元为电机驱动器。

8.一种三机械臂运动系统控制方法，其特征在于，包括以下步骤：

S1、坐标定位单元对车载连接器进行定位并将坐标传递给路线计算单元；

S2、路线计算单元接收到坐标后计算出来机械臂所要移动的距离并转换成相对应的PWM波形；

S3、运动装置驱动单元根据路线计算单元传输过来的PWM波形，驱动步进电机(3)从而带动机械臂运动到指定位置。

9.根据权利要求8所述的三机械臂运动系统控制方法，其特征在于，在步骤S1和步骤S2之间增加如下判定步骤：

路线计算单元根据坐标定位单元传递过来的坐标确定机械臂是否可以达到，如果不能达到，则返回步骤S1；如果能到达，则执行步骤S2。

10.根据权利要求8所述的三机械臂运动系统控制方法，其特征在于，步骤S2中的计算过程如下：

设导轨Ⅱ(302)和导轨Ⅲ(303)的上端连线的中心为原点，建立三维坐标系；

设导轨长度为d，导轨Ⅱ(302)上端和导轨Ⅲ(303)上端到原点的距离为a，导轨I(301)下端距离原点的距离为b，则导轨I(301)上下两端点坐标为(0,b+d,0)和(0,b,0)，导轨Ⅱ(302)上下两端点坐标为(-a,0,0)和(-a,-d,0)，导轨Ⅲ(303)上下两端点坐标为(a,0,0)和(a,-d,0)；

设每个机械臂长度为L，在导轨上移动的机械臂端为A端，连接充电接口(4)的机械臂端称为B端，设机械臂I(5)的A端坐标(0,y₁,0)，机械臂Ⅱ(6)的A端坐标(-a,y₂,0)，机械臂Ⅲ(8)的A端坐标(a,y₃,0)；

设充电接口(4)的坐标为(x,y,z)，根据机械臂长度恒等于L，则需要满足如下公式：

x²+(y-y₁)²+z²＝L²

(x+a)²+(y-y₂)²+z²＝L²

(x-a)²+(y-y₃)²+z²＝L²

求得三个机械臂A端坐标为：

由于导轨长度固定，机械臂A端移动的范围可以确定下来，则A端坐标范围进一步减小为：

b≤y₁≤b+d

-d≤y₂≤0

-d≤y₃≤0

分别计算端点和初始位置的差值，选择差值小的端点作为机械臂运动的最后位置。

Images