一种连续体机械臂的多丝杆电机总线型嵌入式控制系统的制作方法

1.本发明主要涉及机械臂电机控制技术领域，具体涉及一种连续体机械臂的多丝杆电机总线型嵌入式控制系统。


背景技术：

2.在连续体机械臂领域，机械臂需要完成行进、到达目标位置、进行抓取或者剪切等一系列任务。这些任务的完成都离不开电机驱动系统，且对系统的尺寸、功耗、控制精度等有着较高的要求。而线驱动的连续体机械臂，一般由3根线或者4根线控制每一段机械臂的运动，多段机械臂的运动控制则需要多个电机同时对连续体机械臂进行驱动，这对多个电机驱动的同步性有着较高要求，同时由于电机数量较多，每个驱动器的体积也不宜过大。
3.现有的连续体机械臂电机控制系统，多采用体积较大、功率较大的单电机驱动器，由于连续体机械臂每一段需要多个电机进行驱动，大体积的电机驱动器必然会使驱动系统过于庞大；且现有连续体机械臂电机控制系统多采用微控制器的i/o口或者pwm波方式给驱动器发送控制信号，存在着多电机驱动同步性差、响应速度慢、抗干扰能力差等缺陷。


技术实现要素：

4.本发明要解决的技术问题就在于：针对现有技术存在的技术问题，本发明提供一种结构简单、控制精度高的连续体机械臂的多丝杆电机总线型嵌入式控制系统。
5.为解决上述技术问题，本发明提出的技术方案为：
6.一种连续体机械臂的多丝杆电机总线型嵌入式控制系统，包括上位机、控制模块和多个电机驱动模块，所述上位机通过以太网与所述控制模块通讯连接，所述控制模块通过can总线与多个所述电机驱动模块通讯连接；所述上位机通过以太网将电机的标准位置和标准速度发送至控制模块，所述控制模块根据标准位置和标准速度输出控制指令至电机驱动模块，所述电机驱动模块根据控制指令驱动电机的运行，并将所述电机的实时位置和实时转速发送至控制模块以进行双闭环控制。
7.优选地，所述控制模块包括微控制器u1、分别与所述微控制器u1连接的最小系统电路、usb接口、uart接口、tf卡接口、两个can接口、ethernet接口以及rs422接口。
8.优选地，所述微控制器u1包括stm32h743vit6，所述微控制器u1的8脚、9脚作为rtc时钟的外部晶振接入端osc32_in、osc32_out，12脚、13脚作为主时钟的外部晶振接入端osc_in、osc_out；72脚、76脚分别作为下载调试信号jtms_swdio、jtck_swclk；70脚、71脚分别作为usb驱动信号usb_d-、usb_d ；81脚、82脚作为串口的收发信号uart4_rx、uart4_tx；37脚、38脚作为串口的收发信号uart7_rx、uart7_tx；65脚、66脚、78脚、79脚、80脚、83脚则作为tf卡的sdio驱动信号sdio_d0、sdio_d1、sdio_d2、sdio_d3、sdio_sck、sdio_cmd；95脚、96脚作为can收发器1#的收发信号can1_rx、can1_tx；91脚、92脚作为can收发器2#的收发信号can2_rx、can2_tx；15脚、16脚、23脚、24脚、31脚、32脚、33脚、47脚、51脚、52脚分别作为lan8720a与微控制器通信的rmii接口信号eth_reset、eth_mdc、rmii_ref_clk、eth_mdio、
rmii_crs_dv、rmii_rxd0、rmii_rxd1、rmii_tx_en、rmii_txd0、rmii_txd1；11脚、27脚、50脚、75脚、100脚均接直流电源vcc3.3；10脚、26脚、49脚、74脚、99脚均接gnd；48脚、73脚各通过一个2.2uf电容c27、c28接到gnd。
9.优选地，所述最小系统电路用于为微控制器提供其正常工作所必需的电源、时钟、复位和下载调试接口；所述微控制器的外部电源为vcc5，采用电压转换芯片ams1117-3.3将vcc5转换得到vcc3.3；微控制器u1的vbat引脚用于给其后备区域供电，采用外部电源vcc和纽扣电池cr1220混合供电的方式；在有外部电源vcc3.3时，cr1220不给vbat供电；在外部电源断开时，则由cr1220供电，保证vbat总是有电以防止寄存器内容丢失。
10.优选地，还包括用于提供微控制器工作时钟的晶振电路，所述晶振电路包括分别给主时钟和rtc提供时钟的两组外部晶振，主时钟的晶振y1连接在u1的osc_in和osc_out引脚之间，且晶振两端通过电容接地，并且并联电阻r5；rtc时钟的晶振y2连接在u1的osc32_in和osc32_out引脚之间，且晶振两端通过电容接地。
11.优选地，还包括用于下载驱动电机运行程序的下载调试电路，所述下载调试电路采用swd方式，通过引出插件p1作为下载接口，p1的引脚1连接gnd，引脚4连接vcc3.3，2、3引脚分别连接u1的下载调试信号jtck_swclk、jtms_swdio，引脚5连接u1的reset引脚；u1的reset引脚为低电平时复位，复位电路的设计遵循低电平复位原则，复位按钮未按下时，直流电源vcc3.3经电阻r1接到reset脚，使其保持高电平；复位按钮被按下时，reset引脚和电源地短接，变为低电平，微控制器复位。
12.优选地，所述usb接口与上位机进行连接，用于为微控制器提供电源和进行固件更新；所述usb接口选用micro usb接口，其2脚d-、3脚d 分别经电阻r2、r3与微控制器u1的usb驱动信号usb_d-、usb_d 连接，5-8脚全部接gnd，其1脚vbus引出直流电源vcc5；
13.所述uart接口作为备用通信接口，引出一个外部插件p2，其1脚接gnd，2、3脚分别接微控制器u1的串口4#的收发信号uart4_rx、uart4_tx；
14.tf卡接口用于存储电机的位置和速度信息，以及各个can总线接口步进电机驱动器的can通讯地址；tf卡槽的7脚、8脚、1脚、2脚、3脚分别接微控制器u1的sdio驱动信号sdio_d0、sdio_d1、sdio_d2、sdio_d3、sdio_cmd，并分别接上拉电阻r10、r11、r7、r8、r9，其5脚连接u1的sdio_sck，4脚和6脚分别接vcc3.3和gnd。
15.优选地，所述can接口数量有两个，两个can接口电路相同；每个can接口包括数字隔离器件u3、can收发器u4和电源隔离器件u5；数字隔离器u3用于保护can收发信号不受干扰，其2脚、3脚分别接微控制器u1的can收发信号can1_rx、can1_tx,其1脚、4脚和8脚、5脚这两组电源引脚分别接两组不同的电源vcc3.3、gnd和vo、gnd2，其6脚和7脚与can收发器u4的1脚和4脚连接；can收发器u4用于将微控制器的can控制数据转换为电信号并通过总线发送出去，同时也接收总线数据，并将数据传回微控制器，其1脚、4脚连接来自u3隔离后的收发信号，其7脚和6脚则引出作为can接口的can_h和can_l，并在两者之间并联电阻，其2脚、3脚则分别接gnd2、vo，其8脚接下拉电阻r13；电源隔离器件u5采用小功率dc/dc电源隔离器件，用于得到两组相互隔离的电源供数字隔离器u3使用，其输入端连接vcc3.3和gnd，然后输出一组隔离的电源vo和gnd2。
16.优选地，所述电机驱动模块包括多个can总线型驱动器和多台丝杆电机，所述can总线型驱动器与丝杆电机一一对应；所述can总线型驱动器用于对can总线发送的控制指令
进行解析，转换为相应的控制脉冲对丝杆电机进行驱动。
17.优选地，所述can总线型驱动器安装于所述丝杆电机的底部。
18.与现有技术相比，本发明的优点在于：
19.本发明的多丝杆电机总线型嵌入式控制系统，控制模块接收到上位机通过以太网发送的电机标准位置和标准速度，以及接收到电机的实时位置和实时速度，输出精准的控制指令至电机驱动模块，对电机的速度和位置进行双闭环反馈控制以实现电机的精准控制，保证电机到达期望位置和速度；本发明采用can总线实现控制模块和电机驱动模块通信，可以同时驱动多台电机驱动模块，能够保证多台电机控制的同步性，提高控制电机的效率。
附图说明
20.图1为本发明的多丝杆电机总线型嵌入式控制系统在实施例的方框图。
21.图2为本发明中控制模块和电机驱动模块在实施例的通信结构示意图。
22.图3为本发明中控制模块在实施例的结构示意图。
23.图4为本发明中电机位置和速度在实施例的双闭环控制框图。
24.图5为本发明中微控制器在实施例的电路原理图。
25.图6为本发明中微控制器最小系统在实施例的电路原理图。
26.图7为本发明中usb接口在实施例的电路原理图。
27.图8为本发明中uart接口在实施例的电路原理图。
28.图9为本发明中tf卡接口在实施例的电路原理图。
29.图10为本发明中can接口在实施例的电路原理图。
30.图11为本发明中ethernet接口在实施例的电路原理图。
31.图12为本发明中rs422接口在实施例的电路原理图。
32.图13为本发明中每组驱动器和电机在实施例的结构三维图。
具体实施方式
33.以下结合说明书附图和具体实施例对本发明作进一步描述。
34.如图1所示，本发明实施例提供了一种多丝杆电机总线型嵌入式控制系统，包括上位机、控制模块以及电机驱动模块三层架构，上位机与控制模块采用ethernet进行通信连接，控制模块与电机驱动模块采用can总线进行通信连接；其中上位机属于人机交互层，控制模块属于控制层，电机驱动模块属于执行层，三级结构能够保障系统安全可靠的运行；上位机通过以太网将电机的标准位置和标准速度发送至控制模块，控制模块根据标准位置和标准速度输出控制指令至电机驱动模块，电机驱动模块根据控制指令驱动电机的运行，并将电机的实时位置和实时转速发送至控制模块以进行双闭环控制。
35.本发明的多丝杆电机总线型嵌入式控制系统，控制模块接收到上位机通过以太网发送的电机标准位置和标准速度，以及接收到电机的实时位置和实时速度，输出精准的控制指令至电机驱动模块，对电机的速度和位置进行双闭环反馈控制以实现电机的精准控制，保证电机到达期望位置和速度；本发明采用can总线实现控制模块和电机驱动模块通信，可以同时驱动多台电机驱动模块，能够保证多台电机控制的同步性，提高控制电机的效
率。
36.在一具体实施例中，上位机完成的主要任务是人机交互、控制程序的编写以及将控制任务发送给控制模块。人机交互功能的完成，基于qt完成用户图形界面的生成，同时利用上位机外接的键盘鼠标完成输入功能，显示器进行电机目标位置、速度以及电机当前位置、速度信息的显示。上位机安装有keil或者iar软件，用于进行控制程序的编写、编译和下载到微控制器。用户输入电机期望位置和速度后，上位机通过ethernet(以太网)，基于tcp/ip协议，将电机的位置和速度控制任务发送到控制模块。
37.在一具体实施例中，控制模块主要任务是根据上位机的对每个电机位置和速度的控制任务，通过can总线向每个电机驱动模块发送控制指令。如图3所示，控制模块包括微控制器u1、分别与所述微控制器u1连接的最小系统电路、usb接口、uart接口、tf卡接口、两个can接口、ethernet接口以及rs422接口。其中微控制器u1采用stm32h743vit6，其功能为根据上位机发送的电机期望位置、速度信息以及电机驱动模块反馈的当前电机位置、速度信息，对电机的位置和速度进行双闭环控制，位置环采用绝对式pid控制器，速度环采用增量式pid控制器，控制框图如图4所示。
38.为了实现上述功能，微控制器需要依赖许多外围接口电路，如图5所示，微控制器u1的8脚、9脚作为rtc时钟的外部晶振接入端osc32_in、osc32_out，其12脚、13脚作为主时钟的外部晶振接入端osc_in、osc_out；u1的72脚、76脚分别作为下载调试信号jtms_swdio、jtck_swclk；u1的70脚、71脚分别作为usb驱动信号usb_d-、usb_d ；u1的81脚、82脚作为串口4的收发信号uart4_rx、uart4_tx，其37脚、38脚作为串口7的收发信号uart7_rx、uart7_tx；u1的65、66、78、79、80、83脚则作为tf卡的sdio驱动信号sdio_d0、sdio_d1、sdio_d2、sdio_d3、sdio_sck、sdio_cmd；u1的95脚、96脚作为can收发器1的收发信号can1_rx、can1_tx，91脚、92脚作为can收发器2的收发信号can2_rx、can2_tx；u1的15、16、23、24、31、32、33、47、51、52脚分别作为lan8720a与微控制器通信的rmii接口信号eth_reset、eth_mdc、rmii_ref_clk、eth_mdio、rmii_crs_dv、rmii_rxd0、rmii_rxd1、rmii_tx_en、rmii_txd0、rmii_txd1；u1的11、27、50、75、100脚均接直流电源vcc3.3，其10、26、49、74、99脚均接gnd，其48、73脚各通过一个2.2uf电容c27、c28接到gnd。
39.在一具体实施例中，最小系统电路主要用于为微控制器提供其正常工作所必需的电源、时钟、复位和下载调试接口。如图6所示，微控制器的外部电源为vcc5(来源于usb接口的vbus)，采用电压转换芯片ams1117-3.3(即u2)可以将vcc5转换得到vcc3.3；微控制器u1的vbat引脚用于给其后备区域供电，采用外部电源vcc和纽扣电池cr1220混合供电的方式，在有外部电源vcc3.3的时候，cr1220不给vbat供电，而在外部电源断开的时候，则由cr1220给其供电，保证vbat总是有电，防止寄存器内容丢失。晶振电路用于提供微控制器工作时钟，包括分别给主时钟和rtc提供时钟的两组外部晶振，主时钟的晶振y1选用25mhz，其连接在u1的osc_in和osc_out引脚之间，且晶振两端通过电容接地，并且并联1mω电阻r5；rtc时钟的晶振y2选用32.768khz，其连接在u1的osc32_in和osc32_out引脚之间，且晶振两端也通过电容接地。下载调试电路用于下载驱动电机运行的程序，采用swd方式，通过引出插件p1作为下载接口，p1的引脚1连接gnd，引脚4连接vcc3.3，2、3引脚分别连接u1的下载调试信号jtck_swclk、jtms_swdio，引脚5连接u1的reset引脚。由于u1的reset引脚为低电平时复位，故复位电路的设计遵循低电平复位原则，复位按钮未按下时，直流电源vcc3.3经电阻r1
接到reset脚，使其保持高电平；复位按钮被按下时，reset引脚和电源地短接，变为低电平，微控制器便复位。上述最小系统电路结构简单且易于实现。
40.在一具体实施例中，usb接口与上位机进行连接，用于为微控制器提供电源和进行固件更新。如图7所示，选用micro usb接口，其2脚d-、3脚d 分别经电阻r2、r3与微控制器u1的usb驱动信号usb_d-、usb_d 连接，5-8脚全部接gnd，其1脚vbus引出直流电源vcc5。上述usb接口结构简单且易于实现。
41.uart接口作为备用通信接口。如图8所示，引出一个外部插件p2，其引脚1接gnd，2、3脚分别接微控制器u1的串口4的收发信号uart4_rx、uart4_tx。上述uart接口结构简单且易于实现。
42.tf卡接口用于存储电机的位置和速度信息，以及各个can总线接口步进电机驱动器的can通讯地址等。如图9所示，tf卡槽的7、8、1、2、3脚分别接微控制器u1的sdio驱动信号sdio_d0、sdio_d1、sdio_d2、sdio_d3、sdio_cmd，并分别接上拉电阻r10、r11、r7、r8、r9，其5脚连接u1的sdio_sck，4脚和6脚分别接vcc3.3和gnd。上述tf卡接口结构简单且易于实现。
43.can接口有两个，两者电路相同，用于微控制器向电机驱动模块发送控制指令以及电机驱动模块向微控制器反馈电机当前位置和速度信息。如图10所示，每个can接口主要包括数字隔离器件u3、can收发器u4和电源隔离器件u5。数字隔离器u3选用adum1201，用于保护can收发信号不受干扰，其2、3脚分别接微控制器u1的can收发信号can1_rx、can1_tx,其1、4和8、5这两组电源引脚分别接两组不同的电源vcc3.3、gnd和vo、gnd2，其6脚和7脚与can收发器u4的1脚和4脚连接。can收发器u4选用tja1050，用于将微控制器的can控制数据转换为电信号并通过总线发送出去，同时也接收总线数据，并将数据传回微控制器，其1、4脚连接来自u3隔离后的收发信号，其7脚和6脚则引出作为can接口的can_h和can_l，并在二者之间并联120ω电阻，其2、3脚则分别接gnd2、vo，其8脚接下拉电阻r13。电源隔离器件u5采用小功率dc/dc电源隔离器件，用于得到两组相互隔离的电源供数字隔离器u3使用，其输入端连接vcc3.3和gnd，然后输出一组隔离的电源vo和gnd2。上述can接口结构简单且易于实现。
44.ethernet接口用于微控制器与上位机之间的通信，用于上位机向微控制器发送电机期望位置和速度信息，以及微控制器向上位机反馈电机当前位置和速度。如图11所示，主要包括一个phy芯片u6、一个rj45头和电感线圈l1。phy芯片u6采用lan8720，与微控制器u1的通信采用rmii接口，其12脚经电阻r14与微控制器u1的eth_mdio连接，其13、17、18、16、8、7、11、14、15脚分别接u1的eth_mdc、rmii_txd0、rmii_txd1、rmii_tx_en、rmii_rxd0、rmii_rxd1、rmii_crs_dv、rmii_ref_clk、eth_reset，其24脚接下拉电阻r15，其21、20、23、22脚分别作为rj45的信号tptx 、tptx-、tprx 、tprx-且各接一个49.9ω上拉电阻r16-r19，其3、2脚分别作为rj45的信号link_led、speed_led且各接一个49.9ω下拉电阻r20、r21，其4、5脚之间并联25mhz晶振y3，且其19、1、9、25脚分别接vcc3.3e、vcc3.3e、vcc、gnd，各电源引脚与地之间用电容隔断。rj45头则为控制模块的以太网口，型号为hr91105a，其1、3、4、6脚分别接来自u6的信号tptx 、tptx-、tprx 、tprx-且各通过串接一个电容c20-c23连接到gnd，其8、9脚则分别接来自u6的信号link_led、speed_led，其10、11脚分别接下拉电阻r24、r23，12-14脚则均接地。电感线圈l1用于将电源vcc3.3转换成vcc3.3e。上述ethernet接口结构简单且易于实现。
45.rs422接口作为通信备用接口，如图12所示，主要包括一个rs422收发器u7和一个
db9接口p5。rs422收发器u7选用max3490，用于将串口的收发信号转换为rs422的收发信号，其2、3脚连接来自微控制器u1的串口7#收发信号uart7_rx、uart7_tx,其1、4脚分别接vcc3.3、gnd，其5-8脚则对应rs422_rx 、rs422_rx-、rs422_tx 、rs422_tx，然后分别连接到db9接口p5的3、4、2、1引脚，p5的5脚接gnd；u7的2、3、5、8引脚还分别接上拉电阻r26-r28，其6、7脚则接下拉电阻r29、r30，且7脚和8脚之间并联120ω电阻r31，5-8脚与gnd之间各串接一个tvs管d1-d4。上述rs422接口结构简单且易于实现。
46.在一具体实施例中，电机驱动模块主要任务是根据控制模块的控制指令驱动电机的运行，并反馈电机的位置和速度信息给控制模块，具体包括多个can总线接口步进电机驱动器(can总线型驱动器1)和多台丝杆电机。每台电机对应一个驱动器(以两段连续体机械臂为例，则需要8个驱动器和8台电机)，且驱动器安装在电机底部，能够极大节省整个驱动装置的空间，如图13所示。
47.如图13所示，can总线型驱动器1用于对can总线发送的控制指令进行解析，转换为相应的控制脉冲对丝杆电机进行驱动；其型号为7tcsm4210q，具有s型加减速曲线，能够保证电机运行的平稳性，支持定位模式、正反转模式、速度模式、到位模式四种工作模式，能够完成不同任务需求；其尺寸大小仅为42.2mm
×
42.2mm
×
14.5mm，通过螺钉安装在丝杆电机的底面，极大缩小电机驱动模块的整体体积；其最多可进行32细分，能极大提高控制精度；采用can2.0a协议，可多个驱动器进行组网进行同步驱动。
48.如图13所示，丝杆电机由电机2、丝杆3和螺母4组成。电机2采用42系列电机，其步距角为1.8
°
；丝杆的长度为350mm，导程2mm，即电机2转动一周丝杆螺母4仅移动2mm，配合驱动器1最高32细分能够达到很高的精度。连续体机械臂的驱动绳5固连在螺母4上，电机2带动螺母4在丝杆3上进行移动，进而使驱动绳5进行收缩或拉伸，每四根驱动绳5控制一段机械臂的运动。以两段连续体机械臂为例，一共需要八个驱动器和八台丝杆电机，通过can总线对多台丝杆电机进行同步控制。
49.以上仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，应视为本发明的保护范围。