python机器人编程——用python实现一个写字机器人

一、前言

本篇我们构建一个可以跟人一样写字的机器人python软件。实现如下功能：打开一个写字板，人类在屏幕上写或画出任意形状，机器人同步在纸面上画出和人类一样的形状，就好像人类在远程操控机械臂，又或是机械臂是人的另一只手。这个软件是可以扩展的，如果连上互联网，可以实现人在北京写了一行字，在上海的机器臂同时也在自己的本子上写了一行字。

二、整体框架

2.1 系统构成

如上图，写字机器人硬件部分主要为三轴机械臂和主机构成，软件部分为python编写用于控制机械臂末端的轨迹。

2.2 硬件介绍

2.2.1主要组成部分

硬件的组成如下图所示：

2.2.2机械结构

写字机器人的机械臂部分采用一台开源的优秀的三轴机械臂，如图所示：

其中各转轴齿轮传动部分改成了皮带传动，夹具采用了自制的夹具，用于按照写字用笔。

2.2.3驱动及控制主板

机械臂的控制器可采用如下主板，作为机械臂三个轴的驱动和控制板，也可采用其它成熟的控制板。板子内安装了控制电机的驱动控制代码，提供通过串口通信规约（如G代码）与上位机进行所有必要的通讯，如电机的旋转角度、真反正转、停止等，此部分软硬件产品解决方案已经非常成熟，不再赘述。

如图所示，主板装有电机驱动模块，用于对机械臂电机进行驱动，本质上是控制板通过串口接收，将上位机程序要求的电机目标旋转角度转化为脉冲信号，输入驱动模块，驱动模块驱动电机旋转相应角度，就完成了控制。

PS电机驱动原理简介:

两相四线步进电机（如42步进电机）以8拍方式工作，则每个脉冲旋转0.9°，即每旋转一圈需要400个脉冲信号来励磁。步进电机的正、反转由励磁脉冲产生的顺序来控制，它通过外部周期脉冲信号来相应改变4个线圈电极的上电数量和顺序，来周期改变定子的磁场，带动永磁转子向指定方向指定速度旋转如图所示：

2.2.4其余部分

其余部分，包括了电源变送器及连接线部分，不再赘述。

2.3 机器人python程序框架

我们重点完成写字机器人上位系统功能，如下图所示：

由上图可见，机器人上位操作软件设计了几大模块，主要包括：

2.3.1通信服务模块

通信模块主要有两部分：
1. 串口通信部分
此部分主要使用python的串口通信库（如pyserial）实现，用于主机与机械臂主板的串行双向通信。以下为一个简单的串口示例代码：

import serial

# 打开串口
ser = serial.Serial('COM1', 115200)  # 根据实际情况修改串口号和波特率

# 发送G代码至下位机
gcode = "G90 G0 X0 Y0 Z0\r"  # G代码指令，根据实际情况修改
ser.write(gcode.encode())

# 关闭串口
ser.close()

如上示例所述，首先，使用了serial库导入serial模块以进行串口通信。然后，你可以通过调用serial.Serial()函数打开串口。在此函数中，你需要指定串口号和波特率。根据实际情况，修改’COM1’为正确的串口号（例如linux格式为’/dev/ttyUSB0’，windows格式为‘COM3’）和波特率（例如 9600）。接下来，你可以将要发送的G代码赋值给变量gcode（在示例中是"G90 G0 X0 Y0 Z0\r"）。最后，使用ser.write()函数将G代码发送到下位机，使用.encode()将字符串编码为字节数据。最后，通过调用ser.close()关闭串口。

2.web通信部分
此部分主要是建立B/S的写字机器人架构，用于支持远程写字操作功能。把写字板作为写字机器人的客户端，在写字机器人的核心模块中设置一个写字板服务器端。可以通过WebSocket协议进行远程通信。
Python 支持多个用于 WebSocket 通信的库。以下是一些常用的库：

• websocket：websocket 是一个纯 Python 实现的 WebSocket 客户端和服务器端库，提供了基本的
  WebSocket 功能。
• websockets：websockets 是一个支持异步的 WebSocket 客户端和服务器端库，使用起来简单方便。
• tornado.websocket：Tornado 是一个强大的 Python Web 框架，其中包含了用于 WebSocket 通信的模块。 autobahn：autobahn 是一个基于 Twisted 的 WebSocket 库，提供了客户端和服务器端的功能，支持高级特性，如发布-订阅和RPC模式。
• socket.io：socket.io 是一个跨平台的实时通信引擎，支持 WebSocket 和其他实时传输协议，有 Python 的客户端和服务器端实现。

2.3.2消息处理模块

该模块用于对上下通信模块和应用模块之间产生的数据交互进行有效处理。有利于模块化建模。采用多线程消息的发布订阅机制，实现各功能模块间的协同顺序工作（关于消息系统详见博客相关章节）。以下为一个消息流示意：

2.3.3轨迹解析模块

轨迹解析模块作用是将来自写字板的写字字迹信息转化为序列的机械臂末端执行坐标[（x0、y0、z0）,（x1、y1、z1）,（x2、y2、z2）…（xn、yn、zn）]，并安顺序发布至消息中心。

2.3.4机械臂逆解模块

机械臂逆解模块是实现末端坐标点（xn、yn、zn）转化为机械臂各关节电机的角度及电机控制指令，并发布至消息中心。本模块为机械臂控制的核心模块之一，涉及机械臂的建模和逆解。在后面节中描述。

2.3.5写字板模块

写字板模块可以是一个Web前端程序，功能是捕捉人的手写字轨迹，并对轨迹进行离散化采样，形成轨迹序列离散点，周期或打包发送至写字服务器。写字板如下：

三、机械臂的建模

3.1 机械臂机构几何分析

如下图所示，写字机械臂是一个有三个旋转轴的机械臂，这样的机械臂在工业制造领域有着非常广泛的应用，常用于产品的码垛、搬运等:

下面我们对写字机械臂进行几何解析，首先看一张俯视图：

3.1.1 俯视——水平面的运动投影

在vrep环境里面，所定义的坐标系为机械臂初始位置俯视投影在x轴轴上，机械臂的左侧为y轴正方向。其它轴不变，只是运动轴J1，则机械臂在x-y平面内做旋转运动，如果改变J2、J3的角度，在俯视图看来，投影的臂长L或增加或缩短，简化如下图所示：

3.1.2 侧视——垂直平面的运动投影

这里所说的“侧视”投影面，是需要想象出来的，它是指机械臂左侧正对着L-Z平面看到的机械臂的投影，如下图：

我们的下面的算法将在这个面进行解析，我们通过绘制机械臂的主要机构，对这个机械臂的运动特点进行分析，画出如下稍微简单的结构：

上图可以看到，其实这个机械臂在侧视平面内的运动，是通过改变两根红色臂杆L1、Lq的旋转带动末端（或抓具）运动的，两根驱动的杆子又分别是两个电机J2、J3来驱动的。如下图：

上图把机械臂主要的部分“扣”了出来，左上角画出了只转动臂杆L1时出现的状态，可见，在一定的驱动杆Lq位置固定下，运动是有位置限制的（边界的），我们在python实现的时候要注意这一点，以免超出边界还再给电机发送指令导致电机过载（仿真环境还好，在实际机械臂中容易发生），运动L1后，机械臂的前后运动为主方向，变化比较大，相对地，上下运动为次要方向，变化幅度小。；同样的，右边是固定L1时，改变Lq，可以使机械臂末端主要进行上下运动，同样也存在边界状态。
以上是机械臂的运动特点，需要注意的是，它还有一个连杆机构，如下图（红圈），其目的是使机械臂的末端抓具始终垂直于水平面，这是一个隐含条件：

最后，我们可以将机械臂侧视投影简化为（下图所示）：由6跟杆组成包括（Lg、L0、L1、L2、L3、L4）、三个转轴（或关节，包括J1、J2、J3）的一个连杆结构，同时受到隐形的约束，抓具必须始终保持垂直状态。
Ps：此处去掉了对轴J4的考虑，因为我们用的是吸头，它的角度改变实际是没有用的。

3.1.3 机械臂逆解公式推导

问题描述：
已知目标物体中心相对机械臂的位置坐标（x,y,z），求抓取该物体，所需的机械臂各旋转关节的角度各是多少（J1，J2，J3）？
求解问题得流程如下两步：


至此，当我们知道目标物体得坐标时（x,y,z），我们就可以通过以上的简单几何知识计算出控制机械臂的三个轴的角度值（J1，J2，J3）从而将机械臂的抓手送至目标位置，实现准确的抓取动作。

3.2 机械臂软件定义

3.2.1 机械臂结构的软件表达

对应上面的几何连杆结构，我们可以用python的程序表达如下：

class uarm:
    #以下定义uarm臂的物理参数，对上篇python机器人编程——四轴UARM机械臂的运动控制（逆解）原理及python实现（上）
    #转轴1
    J1=0
    #转轴2
    J2=0
    #转轴1
    J3=0
    #固定基座杆lg 单位：mm
    Lg=22.1
    #固定基座杆l0 单位：mm
    L0=104.9
    #驱动臂杆L1 单位：mm
    L1=148
    #驱动臂杆L2 单位：mm
    L2=160
    #从动臂杆L3
    L3=34.5
    #吸头长度
    L4=59.3
    #臂架投影
    L=100

3.2.2 逆解算法的python表达

（1） 俯视图根据目标点（x,y）计算J1，L

如下图俯视，已知坐标（x,y）计算旋转角度，和投影臂长L：

对应的python函数，属于uarm类的方法，如下：

    def J1AndLBy(self,x,y):
    	J1=round(np.arctan(y/x)/np.pi*180,2)
    	L=round(x/np.cos(self.J1),2)
        return True,J1,L
       

（2）侧视图根据目标点（J1,L,z）计算剩余角度J2，J3

我们根据3.1节（2）中的推导公式编写python程序，作为一个机器人类的方法：

    def J2AndJ3By(self,J1,L,z=0):
 
        try:
            
            (XXm,YYm)=(-(L-self.Lg),z-self.L0)
           
            (XXw,YYw)=(-(L-self.Lg-self.L3),self.L4+z-self.L0)
            
            LL=np.sqrt(XXw**2+YYw**2)
            
            res ,gama=self.sove_angle(self.L1,LL,self.L2)
            if res:
                res,alpha=self.sove_angle(LL, self.L2, self.L1)
                if res:               
                    
                    (XXd,YYd)=(0,-self.L0)
                    
                    Lv=np.sqrt((XXd-XXw)**2+(YYd-YYw)**2)
                    
                    print(LL,Lv)
                    res,beta=self.sove_angle(LL, Lv, self.L0)
                    if res:                        
                        
                        JJ2=round(360-beta-alpha-90,2)
                        JJ3=round(JJ2-gama)
     
                        return True,JJ2,JJ3
                    else:
                        print("超出作用区域")
                        return False,None,None                
                        
                else:
                    print("超出作用区域")
                    return False,None,None
                    
            else:
                print("超出作用区域")
                return False,None,None
        except Exception as e:
             print("计算J1，L异常：",e)  
             return False,None,None

（3） 绘制侧视图

为了直观的能够看到程序运行是否正确，我们可以简单的绘制一下侧面的图，效果如下：

通过构建一个内部方法，实现根据（x,y,z）计算出机械臂的各个关节的角度，并画出侧视图，对应python代码如下：

    def drawUarm(self,x,y,z):
        backimg=np.zeros((500,500,3),np.uint8)
        points=[]
        res,J1,L=self.J1AndLBy(x,y)
        if res:
            points.append((-(L-self.Lg),z-self.L0))
            points.append((-(L-self.Lg),self.L4+z-self.L0))
            points.append((-(L-self.Lg-self.L3),self.L4+z-self.L0))
            res,J2,J3=self.J2AndJ3By(J1,L,z)
            if res:
                p = cmath.rect(self.L1, math.radians(J2))  
                points.append((round(p.real,2),round(p.imag,2)))
                points.append((0,0))
                points.append((0,-self.L0))
                points.append((self.Lg,-self.L0))
                dx=300
                dy=300
                colors=[(0,255,255),(255,0,0),(255,255,255),(0,255,0),(255,255,0),(255,200,100)]
                for i in range(len(points)-1):
                    p1=(points[i][0]+dx,dy-points[i][1])
                    p2=(points[i+1][0]+dx,dy-points[i+1][1])
                    cv2.line(backimg,(int(p1[0]),int(p1[1])),(int(p2[0]),int(p2[1])),colors[i],3)
                cv2.imshow('',backimg) 
                cv2.waitKey()
                cv2.destroyAllWindows()

四、笔迹规划

根据写字的流程，可以把写字划分为以下过程，如图所示：

如上图所示，可以将写字划分为以笔划为最小单位，一个字由诺干个笔划安装时间序列组成，写字只要根据笔划序列依次循环执行以上流程：首选是准备写字，将机械臂末端笔尖定位在写字板的准备位置，一般是写字板的中间；其次是根据一笔的起点坐标，将末端移动到起点位置；接着，降低笔尖至写字板面，根据笔划的序列采样点坐标进行笔尖推送；最后完成一笔后，笔尖抬起。部分python程序如下：

	#以下为机械臂类的成员函数
	#写字准备姿势
    def waitfor_draworder(self):
        self.ready2draw(185,0)  #移动到固定位置      
    #移动到笔划的起点  
    def ready2draw(self,armx0,army0):
        """
        根据新来的一笔，快速移动到开始位置
        """
        if armx0>=self.X_range[0] and armx0<=self.X_range[1] and army0>=self.Y_range[0] and army0<=self.Y_range[1]:
            ppx=[armx0,army0]
            if ppx!=None:                
                self.p0=(-ppx[1],ppx[0])
                res,J1,R=self.cp_J1_R_oring(ppx[0],ppx[1])
                if res!=True:
                    print("out of workspace")
                    self.p0=(None,None)
                    #self.J[0],self.R=0,200
                    print('超机械臂工作范围')
                    return False
                else:   
                    self.J[0],self.R=J1,R
                    self.Z=self.Z_range[1]
                    #print("定位结果J1，R,Z：",[self.J[0],self.R,self.Z])
                    res,J2,J3,J4,J5=self.cp_J2_3_4_5(self.Z,self.R)
                    if res:
                        J1=self.J[0]
                        self.J[1]=J2
                        self.J[2]=J3
                        self.J[3]=J4
                        self.J[4]=J5
                        self.fire_cmm(J1,J2,J3)
                        return True
            else:
                print('无效点')
                return False
        else:
            print('超写字工作范围')
            return False

    #低头开始写字    
    def down_draw(self,armx0,army0):
        """
        根据新来的一笔，移动到头部后，低头到起始点
        """
        if self.jobcancel:
            print("job cancelled signal")
            return False
        else:            
            if armx0>=self.X_range[0] and armx0<=self.X_range[1] and army0>=self.Y_range[0] and army0<=self.Y_range[1]:
                res,J1,R=self.cp_J1_R_oring(armx0,army0)
                self.J[0],self.R=J1,R
                self.Z=self.Z_range[0]               
                res,J2,J3,J4,J5=self.cp_J2_3_4_5(self.Z,self.R)
                if res:
                    J1=self.J[0]
                    self.J[1]=J2
                    self.J[2]=J3
                    self.J[3]=J4
                    self.J[4]=J5
                    self.fire_cmm_faster(J1,J2,J3)
                    return True
                else:
                    return False
    #写完抬起
    def holdon_afdraw(self):
        """
        一连笔完成后，手臂抬起
        """
        self.Z=self.Z_range[1]
        res,J2,J3,J4,J5=self.cp_J2_3_4_5(self.Z,self.R)       
        if res:
            J1=self.J[0]
            self.J[1]=J2
            self.J[2]=J3
            self.J[3]=J4
            self.J[4]=J5
            self.fire_cmm(J1,J2,J3)
            return True               

五、写字web服务器模块

此模块用于开启一个web服务，可接收来自网络内的客户端的笔迹信息序列。使用web服务的好处是可以通过TCP/IP网实现局域网或广域网的远程控制，可以不受地理限制。本模块采用python库websockets实现。在运行写字机机器人主程序时，开启websocket服务，部分代码如下：

	#以下为写字机器人类的成员函数，启动新线程开启websokect服务
	def RunWebServer(self):
        if islinux(): 
            try:                
                self.son_loop = asyncio.get_event_loop()
                localip=getosip()                
                self.WEBSOCKET = websockets.serve(self.main_logic, localip, self.exval["port"])
                self.son_loop.run_until_complete(self.WEBSOCKET)
                print("arm_webserver run at:"+localip+":",self.exval["port"])
                self.son_loop.run_until_complete(self.tasklist())             
            except KeyboardInterrupt as e:
                print(asyncio.Task.all_tasks())
                print(asyncio.gather(*asyncio.Task.all_tasks()).cancel())  # 取消task
                self.son_loop.stop()  # 停止事件循环，配合run_forever()退出事件循环
                self.son_loop.run_forever()
            finally:
                self.son_loop.close()  # 关闭事件循环
        else:            
            self.son_loop=asyncio.new_event_loop()            
            t0 = threading.Thread(target=self.runtasks,args=(self.son_loop,),name="arm_server") 
            self.WebThreads.append(t0)
            t0.setDaemon(True)  
            t0.start() 
            
    def runtasks(self,lp):
        try:
            asyncio.set_event_loop(lp)
            self.WEBSOCKET = websockets.serve(self.main_logic, "localhost", self.exval["port"])        
            lp.run_until_complete(self.WEBSOCKET)   
            print("arm_webserver run at:"+"localhost"+":",self.exval["port"])               
            lp.run_forever()
        except KeyboardInterrupt as e:           
            print(asyncio.gather(*asyncio.Task.all_tasks()).cancel())  # 取消task
            lp.stop()  # 停止事退出事件循环
            lp.run_forever()
        finally:
            lp.close()  # 关闭事件循环
            
    async def recv_msg(self,websocket):
        """  
        接收来自客户端的笔迹数据，并放入写字任务列表              
        """
        while True:
            recv_text = await websocket.recv()
            print("receive from clinet: ")
            try:
                redict=json.loads(recv_text)
                #print(redict)
                self.JobsList.append(redict)
                self.JobsListN.append("jobson")
            except Exception as e:
                    return False
                    logging.error("转换文件异常---：",e) 
    
    async def main_logic(self,websocket, path):
        """
        服务器端主逻辑        
        """
        await self.recv_msg(websocket)

六、写字板客户端模块

由于开启了websocket服务，写字板客户端可以采用HTML语言进行开发。主要功能实现如下：

如上所示，实现了一个简易的画面客户端，可以按笔划进行写字，具有撤销重做等功能，可以通过客户端连接至服务器端，并按发送键将画板上的文字或图画笔迹按顺序序列发送至服务器端。

八、应用演示

Setp1.连接写字机器人串口并启动，进入写字模式

Setp2.在写字服务器界面中点击在线执行模式

Setp3.在写字板中点击连接服务

Setp4.等待连接完成

Setp5.在写字板上任意写字或绘画，点击send发送执行

九、示例代码

本示例采用我们提供的写字机器人库，和录制好的笔迹文件完成写字功能，代码如下：

import inspect
import ctypes
import threading
import time
import logging
import base64
import numpy as np
import cv2
import json
import pyttsx3
import subprocess
import os
import platform
import datetime
import sys
import asyncio
import math
import cmath
import serial
import websockets
import pupil_apriltags as apriltag
from heads import *
import PySimpleGUI as sg 
from collections import deque
import configparser

from minibot_arm_pi import Mini_Arm 

if __name__ == '__main__':
    
    a=Mini_Arm()  #创建一个手臂
    path="drawfile/draw2022-0-2-16-53-8.json"#指定笔迹文件
    a.drawbyfile(path) #读取笔迹文件并写字