[发明专利]一种电驱式农药喷洒机械臂及控制方法

权利要求书

1.一种电驱式农药喷洒机械臂的控制方法，其特征在于：电驱式农药喷洒机械臂包括底座(1)、底座电机(2)、主电流传感器、主速度传感器、主位置传感器、控制模块、喷枪(3)和至少一组电驱臂杆机构；其中，底座电机(2)固定设置于底座(1)表面，且底座电机(2)驱动端的旋转路径所在面与底座电机(2)对应底座(1)的所设面相平行；主速度传感器和主位置传感器分别设置于底座电机(2)的驱动端上，分别用于检测底座电机(2)驱动端的移动速度、空间位置，主电流传感器用于检测底座电机(2)上的驱动电流；

各组电驱臂杆机构分别均包括臂杆本体(5)、关节位置电机(4)、次电流传感器、次速度传感器、次位置传感器、视觉传感器、测距仪；各组电驱臂杆机构中，臂杆本体(5)的其中一端与对应关节位置电机(4)的驱动端相连接，次速度传感器和次位置传感器分别设置于对应关节位置电机(4)的驱动端上，分别用于检测对应关节位置电机(4)驱动端的移动速度、空间位置，次电流传感器用于检测对应关节位置电机(4)上的驱动电流，视觉传感器和测距仪均设置于对应臂杆本体(5)上远离其对应关节位置电机(4)的一端，视觉传感器的图像捕获方向与测距仪的测距方向同向；各组电驱臂杆机构中的视觉传感器、测距仪分别对接控制模块，由控制模块分别为各视觉传感器、测距仪进行供电，并结合视觉传感器的实时图像捕获结果、以及测距仪的实时测距结果作为依据，分别针对底座电机(2)、以及各组电驱臂杆机构中的关节位置电机(4)进行控制；

各组电驱臂杆机构彼此之间依次相连，后一组电驱臂杆机构中臂杆本体(5)上远离其对应关节位置电机(4)的一端与前一组电驱臂杆机构中关节位置电机(4)的底座相固定连接，各组电驱臂杆机构中关节位置电机(4)上驱动端的旋转路径所在面彼此相共面；队尾位置电驱臂杆机构中关节位置电机(4)的底座与底座电机(2)的驱动端相固定对接，且队尾位置电驱臂杆机构中关节位置电机(4)驱动端的旋转路径所在面与底座电机(2)驱动端的旋转路径所在面相垂直；喷枪(3)固定设置于队首位置电驱臂杆机构中臂杆本体(5)的前端，喷枪(3)的喷嘴方向与队首位置电驱臂杆机构中臂杆本体(5)前端的指向相同；

底座电机(2)、主电流传感器、主速度传感器、主位置传感器、以及各组电驱臂杆机构中的关节位置电机(4)、次电流传感器、次速度传感器、次位置传感器分别与控制模块相连接，底座(1)固定设置于指定机器上，控制模块对接指定机器的供电端进行取电、分别为与之相连的各个用电装置进行供电，同时，控制模块接收来自指定机器对其的控制指令，并根据来自主电流传感器、主速度传感器、主位置传感器、以及各组电驱臂杆机构中次电流传感器、次速度传感器、次位置传感器的实时检测结果，分别针对底座电机(2)、以及各组电驱臂杆机构中的关节位置电机(4)进行控制；

控制模块接收来自指定机器对其的控制指令，并计算获得对应于底座电机(2)的驱动控制指令、以及分别对应于各组电驱臂杆机构中关节位置电机(4)的驱动控制指令，然后，控制模块分别针对底座电机(2)、以及各组电驱臂杆机构中的关节位置电机(4)，均按如下步骤进行控制，实现队尾位置电驱臂杆机构中关节位置电机(4)相对底座电机(2)的位置变化，以及各电驱臂杆机构中臂杆本体(5)的移动；

步骤A.控制模块通过位置传感器获得电机驱动端的空间位置，并与对应电机的驱动控制指令进行PID位置比较，更新电机的驱动控制指令，然后进入步骤B；

步骤B.控制模块针对电机的驱动控制指令，结合预设限制电路指标进行比较，更新电机的驱动控制指令，然后进入步骤C；

步骤C.控制模块通过速度传感器获得电机驱动端的移动速度，并与对应电机的驱动控制指令进行PID速度比较，更新电机的驱动控制指令，然后进入步骤D；

步骤D.控制模块通过电流传感器获得电机的驱动电流，并与对应电机的驱动控制指令进行PID速度比较，更新电机的驱动控制指令，然后进入步骤E；

步骤E.控制模块将驱动控制指令应用于电机上，控制电机的驱动端进行转动；

所述控制模块分别针对底座电机(2)、以及各组电驱臂杆机构中的关节位置电机(4)，执行步骤A至步骤D的同时，控制模块实时根据各组电驱臂杆机构中视觉传感器的实时图像捕获结果、以及测距仪的实时测距结果，针对电机的驱动控制指令进行更新，实现避障控制。

2.根据权利要求1所述一种电驱式农药喷洒机械臂的控制方法，其特征在于：所述底座电机(2)为交流伺服电机。