CN106956270A - 一种电动汽车自动充电桩用六自由度机械臂及其控制方法 - Google Patents

Abstract

本发明公开了一种电动汽车自动充电桩用六自由度机械臂及其控制方法，包含底盘旋转舵机、旋转底盘、大臂前倾舵机、中臂前倾舵机、小臂前倾舵机、第一支架、第二支架、充电枪旋转舵机、充电枪、充电枪支架和充电枪俯仰舵机；通过六自由度机械臂的联动控制实现自动充电臂上充电枪的空间运动，通过线性霍尔传感器感应电动汽车受电接口中心处磁铁的磁场，进行简单定位，之后再通过电动汽车上的位置传感器PSD感应充电臂上的激光准直发射器发出的激光，进行精确定位，最终锁定电动汽车受电接口的位置，使充电枪顺利插入电动汽车上的受电接口。

Description

一种电动汽车自动充电桩用六自由度机械臂及其控制方法

技术领域

本发明涉及一种机械臂及其控制方法，特别是一种电动汽车自动充电桩用六自由度机械臂及其控制方法。

背景技术

电动汽车是汽车工业的发展趋势，充电桩作为电动汽车的必不可少的附属设施，具有极其广阔的应用前景。但现有的充电桩均为手动型，由于充电枪连接着沉重的电线，整体质量较大，因此手动充电存在诸多不便，对一些力气比较小的人士，把充电枪插进电动汽车上的受电接口存在较大的困难。同时，随着科技的进步，立体车库以及无人自助充电站相应而生，这些都对充电桩能够给电动汽车自动充电提出了需求。

发明内容

本发明所要解决的技术问题是提供一种电动汽车自动充电桩用六自由度机械臂及其控制方法，它能够自动将充电枪插入电动汽车上的受电接口。

为解决上述技术问题，本发明所采用的技术方案是：

一种电动汽车自动充电桩用六自由度机械臂，其特征在于：包含底盘旋转舵机、旋转底盘、大臂前倾舵机、中臂前倾舵机、小臂前倾舵机、第一支架、第二支架、充电枪旋转舵机、充电枪、充电枪支架和充电枪俯仰舵机，旋转底盘转动设置在充电桩侧面并且由底盘旋转舵机驱动，大臂前倾舵机固定在旋转底盘外侧，第一支架一端设置在大臂前倾舵机上由大臂前倾舵机驱动，第一支架另一端设置在中臂前倾舵机上由中臂前倾舵机驱动，第二支架一端固定在中臂前倾舵机端部，第二支架另一端设置在小臂前倾舵机上由小臂前倾舵机驱动，充电枪旋转舵机固定在小臂前倾舵机端部，充电枪支架设置在充电枪旋转舵机上由充电枪旋转舵机驱动，充电枪转动设置在充电枪支架上并且充电枪与充电枪俯仰舵机连接由充电枪俯仰舵机驱动。

进一步地，所述第一支架由两个U型支架固定构成，第二支架为U型支架。

进一步地，所述底盘旋转舵机、大臂前倾舵机、中臂前倾舵机、小臂前倾舵机、充电枪旋转舵机和充电枪俯仰舵机均与自动充电臂控制系统连接由自动充电臂控制系统控制驱动。

进一步地，所述充电枪支架上设置有充电枪定位装置。

进一步地，所述充电枪定位装置包含无线通信模块1、无线通信模块2、PSD、激光准直发射器、受电接口中心强磁铁和线性霍尔传感器，无线通信模块1、PSD和受电接口中心强磁铁设置在电动汽车尾部，无线通信模块2、激光准直发射器和线性霍尔传感器设置在充电枪支架上。

进一步地，电动汽车尾部无线通信模块1与车主智能手机无线连接由智能手机内置APP控制操作。

一种电动汽车自动充电桩用六自由度机械臂的控制方法，其特征在于包含以下步骤：

步骤一：车主将电动汽车开到车位上后，在手机APP上按下自动充电按钮；

步骤二：自动充电臂收到指令后，使充电臂顶部的充电枪上的线性霍尔传感器逐渐接近电动汽车受电接口中心处的强磁铁的磁场，线性霍尔传感器通过感知电动汽车尾部位于受电接口中心处的强磁铁的磁感应强度，对充电枪的位置进行定位；

步骤三：通过线性霍尔传感器定位将充电枪移动到受电接口中心处附近；

步骤四：通过激光准直发射器和PSD进一步进行充电枪精确定位，并将充电枪移动到受电接口中心位置。

进一步地，所述步骤二中线性霍尔传感器控制充电枪定位的具体过程为，线性霍尔传感器在磁场中的位置(x₀，y₀)与线性霍尔传感器两端的电压U₀的关系式为

其中，U为霍尔传感器输出电压，DSP首先控制充电臂使线性霍尔传感器向右移动0.5mm，此时线性霍尔传感器两端电压变化为U₀₁，接着DSP控制充电臂使线性霍尔传感器向上移动0.5mm，此时电压变化为U₀₂，将U₀₁和U₀₂代入关系式(1)，即可得到充电枪当前在磁场中的位置坐标(x₀，y₀)；由于磁场粗略定位模式下充电枪枪口与电动汽车受电接口之间的距离z₀已设定好，即此时z₀为已知值，因此充电枪在空间中的位置坐标为(x₀，y₀，z₀)，其中z₀为已知值。

进一步地，所述步骤三中充电枪移动控制过程为，

充电臂充电枪的位置坐标为(x₀，y₀，z₀)，磁场中心即电动汽车上的受电接口中心的坐标为(0，0，0)，因此可以得到充电枪相对受电接口的位置关系为(x₀，y₀，z₀)；

将充电臂的固定基原点定位绝对坐标系1(x₁，y₁，z₁)，其原点位置在底盘中心线与舵机转动轴的交点上；充电臂的大臂绕底盘旋转舵机的转动轴前倾，其舵机坐标系2表示为(x₂，y₂，z₂)；充电臂的中臂绕大臂舵机的旋转轴前倾，其舵机坐标系3表示为(x₃，y₃，z₃)；充电臂的小臂绕中臂舵机的旋转轴前倾，其舵机坐标系4表示为(x₄，y₄，z₄)；充电枪旋转舵机控制充电枪绕小臂的垂直方向转动，其坐标5表示为(x₅，y₅，z₅)；充电枪俯仰舵机控制充电枪在小臂的水平方向旋转，其坐标6表示为(x₆，y₆，z₆)；充电枪中心点P即为充电枪在坐标系6中的位置点；

建立相邻两坐标系i-1与i之间的相对关系，此坐标系变换通式如式(2)所示：

表示由Z轴平移和Z轴旋转，X轴平移和X轴旋转的四种运动变换矩阵的结果；其中，α_i-1为第i坐标系绕第i-1坐标系的X_i-1轴的转角；a_i-1为第i坐标系绕第i-1坐标系的X_i-1轴的移动距离；θ_i为第i坐标系绕第Z_i轴的转角；d_i为第i坐标系绕第Z_i轴的移动距离；

根据自动充电臂的坐标变化通式和充电臂臂长D—H参数表，可以得到各连杆变化矩阵，将从底盘基座到大臂、中臂、小臂直至最后的充电枪所在的关节的所有变换结合起来，就得到了自动充电机械臂的总变换矩阵，即充电枪相对于底盘固定基1的变换矩阵：

得到总变换矩阵之后，就可以进行自动充电臂的6个舵机的运动参数的计算，控制自动充电臂上的充电枪运动到电动汽车上的受电接口附近。

进一步地，所述步骤四激光准直发射器和PSD进一步进行充电枪精确定位过程为，自动充电臂上的充电枪运动到电动汽车上的受电接口附近时，激光准直发射器启动并射出激光对PSD进行照射，然后从PSD传出的电流信号经位置信号处理电路进行反相、放大、保持后送入位置计算主控芯片，由位置计算主控芯片根据式结合PSD的规格和引脚分布，将相应引脚的光电流进行加减与除法运算，得到光敏面上光斑的坐标(x₇，y₇)，从而得到充电枪与电动汽车受电接口之间精确的位置关系(x₇，y₇，z₀)，其中z₀为充电枪枪口与电动汽车受电接口之间的距离，z₀为已知值。

本发明与现有技术相比，具有以下优点和效果：本发明通过六自由度机械臂的联动控制实现自动充电臂上充电枪的空间运动，通过线性霍尔传感器感应电动汽车受电接口中心处磁铁的磁场，进行简单定位，之后再通过电动汽车上的位置传感器PSD感应充电臂上的激光准直发射器发出的激光，进行精确定位，最终锁定电动汽车受电接口的位置，使充电枪顺利插入电动汽车上的受电接口。

附图说明

图1是本发明的一种电动汽车自动充电桩用六自由度机械臂的示意图。

图2是本发明的一种电动汽车自动充电桩用六自由度机械臂的使用状态图。

图3是本发明的一种电动汽车自动充电桩用六自由度机械臂的模块连接图。

图4是线性霍尔感应器特性曲线图。

图5是本发明的磁感应线分布示意图。

图6是本发明的机械臂初始状态对应的D-H参数表。

图7是本发明的激光准直光斑位置检测系统框图。

图8是本发明一种电动汽车自动充电桩用六自由度机械臂的控制系统的流程图。

具体实施方式

下面结合附图并通过实施例对本发明作进一步的详细说明，以下实施例是对本发明的解释而本发明并不局限于以下实施例。

如图1所示，本发明的一种电动汽车自动充电桩用六自由度机械臂，包含底盘旋转舵机1、旋转底盘2、大臂前倾舵机3、中臂前倾舵机4、小臂前倾舵机5、第一支架6、第二支架7、充电枪旋转舵机8、充电枪9、充电枪支架10和充电枪俯仰舵机11，旋转底盘2转动设置在充电桩侧面并且由底盘旋转舵机1驱动，大臂前倾舵机3固定在旋转底盘2外侧，第一支架6一端设置在大臂前倾舵机3上由大臂前倾舵机3驱动，第一支架6另一端设置在中臂前倾舵机4上由中臂前倾舵机4驱动，第二支架7一端固定在中臂前倾舵机4端部，第二支架7另一端设置在小臂前倾舵机5上由小臂前倾舵机5驱动，充电枪旋转舵机8固定在小臂前倾舵机5端部，充电枪支架10设置在充电枪旋转舵机8上由充电枪旋转舵机8驱动，充电枪9转动设置在充电枪支架10上并且充电枪9与充电枪俯仰舵机11连接由充电枪俯仰舵机11驱动。

第一支架由两个U型支架固定构成，第二支架为U型支架。底盘旋转舵机、大臂前倾舵机、中臂前倾舵机、小臂前倾舵机、充电枪旋转舵机和充电枪俯仰舵机均与自动充电臂控制系统连接由自动充电臂控制系统控制驱动。

充电枪支架上设置有充电枪定位装置。如图3所示，充电枪定位装置包含无线通信模块1、无线通信模块2、PSD、激光准直发射器、受电接口中心强磁铁和线性霍尔传感器，无线通信模块1、PSD和受电接口中心强磁铁设置在电动汽车尾部，无线通信模块2、激光准直发射器和线性霍尔传感器设置在充电枪支架上。电动汽车尾部无线通信模块1与车主智能手机无线连接由智能手机内置APP控制操作。

如图7所示，激光准直光斑位置检测系统包含PSD、位置信号处理电路、数据采集模块、接口电路、A/D转换模块和位置计算主控芯片。自动充电臂上的充电枪运动到电动汽车上的受电接口附近时，激光准直发射器启动并射出激光对PSD进行照射，然后从PSD传出的电流信号经位置信号处理电路进行反相、放大、保持后送入位置计算主控芯片，由位置计算主控芯片将相应引脚的光电流进行加减与除法运算，得到光敏面上光斑的坐标，从而得到充电枪与电动汽车受电接口之间精确的位置关系。

车主将电动汽车开到车位上后，在手机APP上按下自动充电按钮。自动充电臂收到指令后，使充电臂由图1所示的初始状态逐渐运动到图2所示的状态，使充电臂顶部的充电枪上的线性霍尔传感器逐渐接近电动汽车受电接口中心处的强磁铁的磁场，线性型霍尔集成传感器的输出电压与外加磁场强度呈线性关系。如图4所示，在磁感应强度±0.25T的范围内，线性霍尔传感器输出的电压U₀与磁感应强度成正比。

如图8所示，一种电动汽车自动充电桩用六自由度机械臂的控制方法，其特征在于包含以下步骤：

步骤一：车主将电动汽车开到车位上后，在手机APP上按下自动充电按钮。

步骤二：自动充电臂收到指令后，使充电臂顶部的充电枪上的线性霍尔传感器逐渐接近电动汽车受电接口中心处的强磁铁的磁场，线性霍尔传感器通过感知电动汽车尾部位于受电接口中心处的强磁铁的磁感应强度，对充电枪的位置进行定位。

线性霍尔传感器通过感知电动汽车尾部位于受电接口中心处的强磁铁的磁感应强度，对充电枪的位置进行定位。强磁铁磁感应强度分布示意简图如图5所示，由图5可以看出，磁感应线分布模型磁场各点的磁场强度不同，因此相同位移下，由上一个运动点到下一个运动点的磁场强度差值不同，线性霍尔传感器可以感应到磁场的细微变化，并表现为不同的输出电压，通过放大电路，可以将这种电压差值进行放大。经推导，磁场强度的分布通式如式(1)所示，线性霍尔传感器在磁场中的位置(x₀，y₀)与线性霍尔传感器两端的电压U₀的关系式为：

其中U为霍尔传感器输出电压。如图5所示，DSP首先控制充电臂使线性霍尔传感器向右移动0.5mm，此时线性霍尔传感器两端电压变化为U₀₁，接着DSP控制充电臂使线性霍尔传感器向上移动0.5mm，此时电压变化为U₀₂，将U₀₁和U₀₂代入式(1)，即可得到充电枪当前在磁场中的位置坐标(x₀，y₀)。由于磁场粗略定位模式下充电枪枪口与电动汽车受电接口之间的距离z₀已设定好，即此时z₀为已知值，因此充电枪在空间中的位置坐标为(x₀，y₀，z₀)，其中z₀为已知值。

步骤三：通过线性霍尔传感器定位将充电枪移动到受电接口中心处附近。

自动充电臂有底盘、大臂、中臂、小臂、充电枪旋转和充电枪俯仰六个自由度，自动充电臂控制系统通过控制6个舵机，可以实现充电枪的空间运动。自动充电臂控制系统根据磁场模型和扫描控制算法计算出充电臂充电枪的位置坐标(x₀，y₀，z₀)后，由于磁场中心即电动汽车上的受电接口中心的坐标为(0，0，0)，因此可以得到充电枪相对受电接口的位置关系(x₀，y₀，z₀)，之后开始进行六个舵机的运动参数的计算。

将充电臂的固定基原点定位绝对坐标系1(x₁，y₁，z₁)，其原点位置在底盘中心线与舵机转动轴的交点上。充电臂的大臂绕底盘旋转舵机的转动轴前倾，其舵机坐标系2表示为(x₂，y₂，z₂)。充电臂的中臂绕大臂舵机的旋转轴前倾，其舵机坐标系3表示为(x₃，y₃，z₃)。充电臂的小臂绕中臂舵机的旋转轴前倾，其舵机坐标系4表示为(x₄，y₄，z₄)。充电枪旋转舵机控制充电枪绕小臂的垂直方向转动，其坐标5表示为(x₅，y₅，z₅)。充电枪俯仰舵机控制充电枪在小臂的水平方向旋转，其坐标6表示为(x₆，y₆，z₆)。充电枪中心点P即为充电枪在坐标系6中的位置点。对全部舵机规定坐标系后，可建立相邻两坐标系i-1与i之间的相对关系，这种关系可由表示坐标系i-1与i相对位置的四个齐次变换来描述，此坐标系变换通式表示如式(2)所示：

式(2)表示表示由Z轴平移和Z轴旋转，X轴平移和X轴旋转的四种运动变换矩阵的结果；公式中：a_i-1为第i坐标系绕第i-1坐标系的X_i-1轴的转角；a_i-1为第i坐标系绕第i-1坐标系的X_i-1轴的移动距离；θ_i为第i坐标系绕第Z_i轴的转角；d_i为第i坐标系绕第Z_i轴的移动距离。

当自动充电机械臂各连杆的坐标系即(x₁，y₁，z₁)到(x₆，y₆，z₆)被规定之后，就能够列出各连杆的常量参数。矩阵就成为关节变量θ(底盘旋转舵机、充电枪旋转舵机、充电枪俯仰舵机运动参数)的函数或变量d(大臂前倾舵机、中臂前倾舵机、小臂前倾舵机运动参数)的函数。本发明设计的自动充电臂的初始状态对应的D-H参数如图6的表1所示。

根据自动充电臂的坐标系变化通式和充电臂臂长D—H参数表，可以得到各臂的变化矩阵，将从底盘基座到大臂、中臂、小臂直至最后的充电枪所在的关节的所有变换结合起来，就得到了自动充电机械臂的总变换矩阵，即充电枪相对于底盘固定基1的变换矩阵：

得到总变换矩阵之后，就可以进行自动充电臂的6个舵机的运动参数的计算。自动充电臂控制系统根据磁场模型和扫描控制算法计算出充电臂充电枪的位置坐标(x₀，y₀)后，通过式(2)和式(3)所示的自动充电臂的运动方程，计算出六个舵机的运动参数(该参数里面包含了各个舵机的运动参数)，控制自动充电臂上的充电枪运动到电动汽车上的受电接口附近。

步骤四：通过激光准直发射器和PSD进一步进行充电枪精确定位，并将充电枪移动到受电接口中心位置。

线性霍尔传感器感应中心磁铁的磁场，自动充电臂控制系统根据这一数据控制充电臂运动到受电接口中心处，但由于充电臂运动过程中的累积误差的影响，充电枪不能精确地移动到正对电动汽车受电接口的位置，为实现充电枪与受电接口的精确定位，本发明使用光电传感器对受电接口进行精确定位，通过无线模块反馈受电接口的位置信息，从而构成闭环跟踪控制系统。

光电传感器主要由激光准直发射器和二维PSD位置传感器组成，激光准直发射器发出的激光照射在PSD上时在PSD上产生一个光斑，二维PSD具有2个方向垂直且相互独立的光敏层，能够分别感知光斑在X，Y两个方向上的位置。PSD光敏面上光斑的坐标(x₇，y₇)与电极输出的光电流之间的关系如式(4)所示：

激光准直光斑位置检测系统框图如图7所示，自动充电臂上的充电枪运动到电动汽车上的受电接口附近时，激光准直发射器启动并射出激光对PSD进行照射，然后从PSD传出的电流信号经位置信号处理电路进行反相、放大、保持后送入位置计算主控芯片，由位置计算主控芯片根据式(4)，结合PSD的规格和引脚分布，将相应引脚的光电流进行加减与除法运算，得到光敏面上光斑的坐标(x₇，y₇)，从而得到充电枪与电动汽车受电接口之间精确的位置关系(x₇，y₇，z₀)，其中z₀为充电枪枪口与电动汽车受电接口之间的距离，z₀为已知值。

获得充电枪与电动汽车受电接口之间精确的位置关系(x₇，y₇，z₀)之后，汽车尾部的无线通信模块将这一坐标发送给自动充电臂上的无线通信模块。自动充电臂主控芯片得到坐标(x₇，y₇，z₀)之后，结合式(2)和式(3)所示的六自由度运动方程计算出舵机的运动参数使充电枪运动到受电接口正前方，之后自动充电臂主控系统启动充电枪与受电接口对接程序，控制充电枪前移与受电接口进行对接。当充电完成或者接收到停止充电信号后，充电枪与受电接口断开，自动充电臂自动回到初始位置。

本说明书中所描述的以上内容仅仅是对本发明所作的举例说明。本发明所属技术领域的技术人员可以对所描述的具体实施例做各种修改或补充或采用类似的方式替代，只要不偏离本发明说明书的内容或者超越本权利要求书所定义的范围，均应属于本发明的保护范围。

Claims (10)

1.一种电动汽车自动充电桩用六自由度机械臂，其特征在于：包含底盘旋转舵机、旋转底盘、大臂前倾舵机、中臂前倾舵机、小臂前倾舵机、第一支架、第二支架、充电枪旋转舵机、充电枪、充电枪支架和充电枪俯仰舵机，旋转底盘转动设置在充电桩侧面并且由底盘旋转舵机驱动，大臂前倾舵机固定在旋转底盘外侧，第一支架一端设置在大臂前倾舵机上由大臂前倾舵机驱动，第一支架另一端设置在中臂前倾舵机上由中臂前倾舵机驱动，第二支架一端固定在中臂前倾舵机端部，第二支架另一端设置在小臂前倾舵机上由小臂前倾舵机驱动，充电枪旋转舵机固定在小臂前倾舵机端部，充电枪支架设置在充电枪旋转舵机上由充电枪旋转舵机驱动，充电枪转动设置在充电枪支架上并且充电枪与充电枪俯仰舵机连接由充电枪俯仰舵机驱动。

2.按照权利要求1所述的一种电动汽车自动充电桩用六自由度机械臂，其特征在于：所述第一支架由两个U型支架固定构成，第二支架为U型支架。

3.按照权利要求1所述的一种电动汽车自动充电桩用六自由度机械臂，其特征在于：所述底盘旋转舵机、大臂前倾舵机、中臂前倾舵机、小臂前倾舵机、充电枪旋转舵机和充电枪俯仰舵机均与自动充电臂控制系统连接由自动充电臂控制系统控制驱动。

4.按照权利要求3所述的一种电动汽车自动充电桩用六自由度机械臂，其特征在于：所述充电枪支架上设置有充电枪定位装置。

5.按照权利要求4所述的一种电动汽车自动充电桩用六自由度机械臂，其特征在于：所述充电枪定位装置包含无线通信模块1、无线通信模块2、PSD、激光准直发射器、受电接口中心强磁铁和线性霍尔传感器，无线通信模块1、PSD和受电接口中心强磁铁设置在电动汽车尾部，无线通信模块2、激光准直发射器和线性霍尔传感器设置在充电枪支架上。

6.按照权利要求5所述的一种电动汽车自动充电桩用六自由度机械臂，其特征在于：电动汽车尾部无线通信模块1与车主智能手机无线连接由智能手机内置APP控制操作。

7.一种权利要求1-6任一项所述的电动汽车自动充电桩用六自由度机械臂的控制方法，其特征在于包含以下步骤：

步骤一：车主将电动汽车开到车位上后，在手机APP上按下自动充电按钮；

步骤二：自动充电臂收到指令后，使充电臂顶部的充电枪上的线性霍尔传感器逐渐接近电动汽车受电接口中心处的强磁铁的磁场，线性霍尔传感器通过感知电动汽车尾部位于受电接口中心处的强磁铁的磁感应强度，对充电枪的位置进行定位；

步骤三：通过线性霍尔传感器定位将充电枪移动到受电接口中心处附近；

步骤四：通过激光准直发射器和PSD进一步进行充电枪精确定位，并将充电枪移动到受电接口中心位置。

8.按照权利要求7所述的一种电动汽车自动充电桩用六自由度机械臂的控制方法，其特征在于：所述步骤二中线性霍尔传感器控制充电枪定位的具体过程为，线性霍尔传感器在磁场中的位置(x₀，y₀)与线性霍尔传感器两端的电压U₀的关系式为

${\left(\frac{x_0-1.479U_0-0.027}{0.565U+0.025}\right)}^2+{\left(\frac{y_0+0.042/(U_0+0.031)+0.2661}{5.21}\right)}^2=1---\left(1\right)$

其中，U为霍尔传感器输出电压，DSP首先控制充电臂使线性霍尔传感器向右移动0.5mm，此时线性霍尔传感器两端电压变化为U₀₁，接着DSP控制充电臂使线性霍尔传感器向上移动0.5mm，此时电压变化为U₀₂，将U₀₁和U₀₂代入关系式(1)，即可得到充电枪当前在磁场中的位置坐标(x₀，y₀)；由于磁场粗略定位模式下充电枪枪口与电动汽车受电接口之间的距离z₀已设定好，即此时z₀为已知值，因此充电枪在空间中的位置坐标为(x₀，y₀，z₀)，其中z₀为已知值。

9.按照权利要求7所述的一种电动汽车自动充电桩用六自由度机械臂的控制方法，其特征在于：所述步骤三中充电枪移动控制过程为，

充电臂充电枪的位置坐标为(x₀，y₀，z₀)，磁场中心即电动汽车上的受电接口中心的坐标为(0，0，0)，因此可以得到充电枪相对受电接口的位置关系为(x₀，y₀，z₀)；

将充电臂的固定基原点定位绝对坐标系1(x₁，y₁，z₁)，其原点位置在底盘中心线与舵机转动轴的交点上；充电臂的大臂绕底盘旋转舵机的转动轴前倾，其舵机坐标系2表示为(x₂，y₂，z₂)；充电臂的中臂绕大臂舵机的旋转轴前倾，其舵机坐标系3表示为(x₃，y₃，z₃)；充电臂的小臂绕中臂舵机的旋转轴前倾，其舵机坐标系4表示为(x₄，y₄，z₄)；充电枪旋转舵机控制充电枪绕小臂的垂直方向转动，其坐标5表示为(x₅，y₅，z₅)；充电枪俯仰舵机控制充电枪在小臂的水平方向旋转，其坐标6表示为(x₆，y₆，z₆)；充电枪中心点P即为充电枪在坐标系6中的位置点；

建立相邻两坐标系i-1与i之间的相对关系，此坐标系变换通式如式(2)所示：

$\begin{array}{c}{T}_i^{i-1}=Rot\left(x,{\mathrm{\α}}_{i-1}\right)trans\left(x,a_{i-1}\right)Rot\left(z,{\mathrm{\θ}}_i\right)trans\left(z,d_i\right)\\ =\left[\begin{array}{cccc}{\mathrm{cos\θ}}_i& -\sin \mathrm{\θ}i& 0& a_{i-1}\\ {\mathrm{sin\θ}}_i{\mathrm{cos\α}}_{i-1}& {\mathrm{cos\θ}}_i{\mathrm{cos\α}}_{i-1}& -{\mathrm{sin\α}}_{i-1}& -d_i{\mathrm{sin\α}}_{i-1}\\ {\mathrm{sin\θ}}_i{\mathrm{cos\α}}_{i-1}& {\mathrm{cos\θ}}_i{\mathrm{sin\α}}_{i-1}& {\mathrm{cos\α}}_{i-1}& d_i{\mathrm{cos\α}}_{i-1}\\ 0& 0& 0& 1\end{array}\right]\end{array}---\left(2\right)$

表示由Z轴平移和Z轴旋转，X轴平移和X轴旋转的四种运动变换矩阵的结果；其中，a_i-1为第i坐标系绕第i-1坐标系的X_i-1轴的转角；a_i-1为第i坐标系绕第i-1坐标系的X_i-1轴的移动距离；θ_i为第i坐标系绕第Z_i轴的转角；d_i为第i坐标系绕第Z_i轴的移动距离；

根据自动充电臂的坐标系变化通式和充电臂臂长D—H参数表，可以得到各连杆变化矩阵，将从底盘基座到大臂、中臂、小臂直至最后的充电枪所在的关节的所有变换结合起来，就得到了自动充电机械臂的总变换矩阵，即充电枪相对于底盘固定基1的变换矩阵：

$T_6^1=T_2^1{T}_3^2{T}_4^3{T}_5^4{T}_6^5---\left(3\right)$

得到总变换矩阵之后，就可以进行自动充电臂的6个舵机的运动参数的计算，控制自动充电臂上的充电枪运动到电动汽车上的受电接口附近。

10.按照权利要求7所述的一种电动汽车自动充电桩用六自由度机械臂的控制方法，其特征在于：所述步骤四激光准直发射器和PSD进一步进行充电枪精确定位过程为，自动充电臂上的充电枪运动到电动汽车上的受电接口附近时，激光准直发射器启动并射出激光对PSD进行照射，然后从PSD传出的电流信号经位置信号处理电路进行反相、放大、保持后送入位置计算主控芯片，由位置计算主控芯片根据式结合PSD的规格和引脚分布，将相应引脚的光电流进行加减与除法运算，得到光敏面上光斑的坐标(x₇，y₇)，从而得到充电枪与电动汽车受电接口之间精确的位置关系(x₇，y₇，z₀)，其中z₀为充电枪枪口与电动汽车受电接口之间的距离，z₀为已知值。