专利名称：一种多自由度机器人运动规划控制器的制作方法
技术领域：
本发明属于机器人控制技术领域。具体涉及一种基于网络的多自由度机器人运动规划控制器。
背景技术：
在多自由度机器人运动规划中，需要进行大量的数学计算如坐标变换、正逆运动学方程求解、高次插补运算等，这一系列的运算均需采用高速的处理器才能满足运动规划控制的实时性要求。“基于分布式控制的即插即用机械臂系统”(CN1586829)专利技术，采用8位单片机来完成运动规划控制，运算能力有限，难以满足高速的实时运动规划控制要求。
在多自由度机器人运动规划中，机器人控制器与各自由度伺服驱动器之间需要进行大量的数据交换。机器人及其控制系统(周志红，周国容.PR机器人及其控制系统.机电一体化.2004第1期)，由一个DSP控制器对机器人的4个关节进行插补运算和伺服控制，控制器与各关节之间采用点对点的传输方式，其传输速度和传输效率有待进一步提高，同时控制器与每个关节之间连接都需要设计接口电路和数条电缆，使得整个控制系统结构较为复杂。
另外，随着机器人应用领域的扩展，如危险环境下机器人作业，本地控制已无法满足其控制要求，需要机器人系统具有远程操作能力。采用自下而上的设计思想，设计了基于PC、TMS320VC5410DSP和TMS320LF240DSP的三层控制系统(王沫楠，孙立宁.多运动方式移动机器人控制系统设计.电机与控制学报.2005第9卷第4期)，其结构较为合理，但由于PC与DSP5410之间采用串口通讯方式，最大波特率为115200bps，系统的实时规划性能受到一定的限制，尤其是在系统网络化和多机器人系统扩展时，通讯速度以及控制的灵活性有待进一步提升。以ARM和FPGA为核心的机器人分布式CAN总线控制系统体系(刘淼等.一种机器人嵌入式网络化控制系统体系结构的研究.机器人.2006第28卷第2期)便具有一定的网络控制功能，但是其用于完成运动规划算法的控制器主频为67.5MHz，运算速度较不高，限制了系统运动规划的实时性。
发明内容
本发明的目的是提供一种结构简单、体积小、运算能力强、可远程操作、具有运动实时规划能力及快速数据交换能力的多自由度机器人运动规划控制器。该控制器可用于工业机器人、机械臂、空间机器人、类人机器人等具有多个自由度模块的机器人控制系统。
为了实现上述目的，本发明所采用的技术方案是本发明包括电源转换模块、DSP芯片、以太网控制器、以太网连接器(内置以太网变压器)、CAN总线连接器、CAN总线接口芯片。外部直流电源与电源转换模块的输入端连接，电源转换模块的输出端分别与DSP芯片、以太网控制器、以太网连接器、CAN总线接口芯片连接，DSP芯片分别与以太网控制器、CAN总线接口芯片连接，以太网控制器与以太网连接器连接，CAN总线接口芯片与CAN总线连接器连接。多自由度机器人运动规划控制软件集成在DSP芯片中。其中电源转换模块包括开关电源模块、双路输出电压调整器、三极管。电源转换模块的输入端为24V、COM，输出端为5V、RESET、1.9V、3.3V。开关电源模块的VIN+、VIN-引脚分别与24V、COM输入端连接，开关电源模块的VOUT+引脚分别与双路输出电压调整器的1IN、2IN引脚连接，开关电源模块的VOUT+引脚与5V输出端连接，开关电源模块的VOUT+引脚通过电阻R1与双路输出电压调整器的1EN引脚连接，开关电源模块的VOUT-引脚接地。双路输出电压调整器的1EN引脚与三极管的集电极连接，三极管的基极通过电阻R5与双路输出电压调整器的2OUT引脚连接，三极管的发射极与双路输出电压调整器的2EN引脚分别接地。双路输出电压调整器的1RESET、2RESET引脚分别与RESET输出端连接，双路输出电压调整器的1OUT引脚与1.9V输出端连接，双路输出电压调整器的1FB/SENSE、1OUT引脚间通过电阻R3连接，双路输出电压调整器的1FB/SENSE引脚通过R4接地，双路输出电压调整器的2OUT引脚与3.3V输出端连接，双路输出电压调整器的2OUT、1RESET引脚通过电阻R2连接。上述电阻R1、R2、R3、R4、R5的阻值分别为5～15KΩ、5～15KΩ、15～25KΩ、25～35KΩ、5～15KΩ。
电源转换模块的24V、COM输入端与直流电源连接，电源转换模块的3.3V输出端分别与DSP芯片的VDDIO引脚、以太网控制器的VDD引脚、以太网连接器的TX_CT引脚连接。电源转换模块的RESET、1.9V输出端分别与DSP芯片的XRS、VDD引脚连接，电源转换模块的5V输出端与CAN总线接口芯片的VCC引脚连接。
DSP芯片的XCLKOUT、GPIOB7、XINT1、XINT2、SPISIMO、SPISOMI、SPICLK引脚分别与以太网控制器的OSC1、CS、INT、WOL、SI、SO、SCK引脚连接；DSP芯片的CANTX、CANRX引脚分别与CAN总线接口芯片的TXD、RXD引脚连接。
以太网控制器的TPOUT+、TPOUT-、TPIN+、TPIN-引脚分别与以太网连接器的TP_OUT+、TP_OUT-、TP_IN+、TP-IN-引脚连接，以太网连接器的J1、J2、J3、J4、J5、J6、J7、J8输出端通过双绞线与上位机连接。
CAN总线接口芯片的CANH、CANL引脚与CAN总线连接器连接。
本发明的多自由度机器人运动规划控制软件的主流程为首先初始化DSP控制器片内各功能模块，包括设置系统控制寄存器、初始化中断向量表、配置CAN控制器模块、配置SPI控制器模块。再初始化以太网控制器，包括初始化设备标识符、数据传输标准、协议、波特率及其它相关参数。然后进入判断是否接收到上位机指令的循环，若未接收到指令则继续循环，若接收到指令则首先读取CAN总线反馈信息，再计算出机器人当前位姿。然后结合接收到的上位机指令进行轨迹规划，完成轨迹规划后，先发送指令至CAN总线，再发送反馈信息至以太网，然后判断是否有以太网接收信息标志，有则返回至判断是否接收到上位机指令，没有则返回至读取CAN总线反馈信息。
轨迹规划的流程为首先判断是否为连续路径模式，是连续路径模式则先进行位置插补计算，再进行姿态插补计算，然后计算插补点目标位姿，计算完毕启动多点路径模式；不是连续路径模式则判断是否为示教模式，是示教模式则进入判断是否为期望位姿的循环，不是期望位姿则继续循环，是期望位姿则计算目标点位姿，计算完毕后启动多点路径模式；不是示教模式则直接启动多点路径模式；启动多点路径模式后，首先进行逆运动学计算，再判断是否运动学方程有解，运动学方程有解则进行可行解、最优解计算，然后进行多项式轨迹规划，规划完毕发送各自由度运动控制指令，程序结束；运动学方程无解则提示目标点错误，程序结束。
由于采用上述技术方案，本发明采用DSP芯片作为核心处理器，它是一款高速的数字信号处理芯片，不仅具有数字信号处理器的优点，同时又具有微控制器的特点，无论在运算精度和数据的处理能力上都可以满足运动规划控制的高实时性要求，为完成复杂的实时轨迹规划算法提供了可靠的平台。
本发明与上位机通讯采用以太网方式，不但解决了传统串行通讯方式存在的传输速率低、通讯距离短和组网能力差等问题，且具有信息量大、兼容性强、编址灵活方便等优点。还可以通过以太网通讯模块将系统接入局域网或者因特网，从而实现系统的远程控制，提高了系统控制的灵活性和使用价值。
本发明与多自由度机器人的各个自由度模块通讯采用CAN总线方式，CAN总线通讯具有多主机方式、传输距离远、传输速度快、抗干扰能力强、应用灵活等诸多优点，解决了传统控制器点对点数据传输方式中的传输速率低、接口复杂等问题。
本发明主要由一个电源模块和三个芯片组成，结构简单、体积小、运算能力强、可远程操作、具有实时规划能力和快速数据交换能力，可满足多自由度机器人运动规划控制的要求。
本发明可广泛用于工业机器人、机械臂、空间机器人、类人机器人等具有多个自由度模块的机器人控制系统。
图1是本发明的一种电路连接示意图；图2是图1中电源转换模块1的电路连接示意图；图3是本发明的多自由度机器人运动规划控制软件的主流程图；图4是图3中轨迹规划的流程图。
具体实施例方式下面结合附图对本发明做进一步的描述一种多自由度机器人运动规划控制器，其硬件部分如图1所示，包括电源转换模块1、DSP芯片2、以太网控制器3、以太网连接器(内置以太网变压器)4、CAN总线连接器5、CAN总线接口芯片6。外部直流电源7与电源转换模块1的输入端连接，电源转换模块1的输出端分别与DSP芯片2、以太网控制器3、以太网连接器4、CAN总线接口芯片6连接，DSP芯片2分别与以太网控制器3、CAN总线接口芯片6连接，以太网控制器3与以太网连接器4连接，CAN总线接口芯片6与CAN总线连接器5连接；多自由度机器人运动规划控制软件集成在DSP芯片2中。在本实施例中电源转换模块1如图2所示，包括开关电源模块8、双路输出电压调整器9、三极管10，电源转换模块1的输入端为24V、COM，输出端为5V、RESET、1.9V、3.3V；开关电源模块8的VIN+、VIN-引脚分别与24V、COM输入端连接，开关电源模块8的VOUT+引脚分别与双路输出电压调整器9的1IN、2IN引脚连接，开关电源模块8的VOUT+引脚与5V输出端连接，开关电源模块8的VOUT+引脚通过电阻R1与双路输出电压调整器9的1EN引脚连接，开关电源模块8的VOUT-引脚接地，双路输出电压调整器9的1EN引脚与三极管10的集电极连接，三极管10的基极通过电阻R5与双路输出电压调整器9的2OUT引脚连接，三极管10的发射极与双路输出电压调整器9的2EN引脚分别接地；双路输出电压调整器9的1RESET、2RESET引脚分别与RESET输出端连接，双路输出电压调整器9的1OUT引脚与1.9V输出端连接，双路输出电压调整器9的1FB/SENSE、1OUT引脚通过电阻R3连接，双路输出电压调整器9的1FB/SENSE引脚通过R4接地，双路输出电压调整器9的2OUT引脚与3.3V输出端连接，双路输出电压调整器9的2OUT、1RESET引脚间通过电阻R2连接；上述的电阻R1、R2、R3、R4、R5的阻值分别为10KΩ、10KΩ、18.2KΩ、30.1KΩ、10KΩ。
电源转换模块1的24V、COM输入端与直流电源7连接，电源转换模块1的3.3V输出端分别与DSP芯片2的VDDIO引脚、以太网控制器3的VDD引脚、以太网连接器4的TX_CT引脚连接；电源转换模块1的RESET、1.9V输出端分别与DSP芯片2的XRS、VDD引脚连接；电源转换模块1的5V输出端与CAN总线接口芯片6的VCC引脚连接。
如图1所示，DSP芯片2的XCLKOUT、GPIOB7、XINT1、XINT2、SPISIMO、SPISOMI、SPICLK引脚分别与以太网控制器3的OSC1、CS、INT、WOL、SI、SO、SCK引脚连接；DSP芯片2的CANTX、CANRX引脚分别与CAN总线接口芯片6的TXD、RXD引脚连接。
以太网控制器3的TPOUT+、TPOUT-、TPIN+、TPIN-引脚分别与以太网连接器4的TP_OUT+、TP_OUT-、TP_IN+、TP_IN-引脚连接；以太网连接器4的J1、J2、J3、J4、J5、J6、J7、J8输出端通过双绞线与上位机连接。
CAN总线接口芯片6的CANH、CANL引脚与CAN总线连接器5连接。
本实施例的多自由度机器人运动规划控制软件的主流程如图3所示首先初始化DSP控制器，再初始化以太网控制器，然后进入循环，判断是否接收到上位机指令，若未接收到指令则继续循环，若接收到指令则进入下一步；首先读取CAN总线反馈信息，再计算出机器人当前位姿，然后结合接收到的上位机指令进行轨迹规划；完成轨迹规划后，先发送指令至CAN总线，再发送反馈信息至以太网，然后判断是否有以太网接收信息标志，有则返回至判断是否接收到上位机指令，没有则返回至读取CAN总线反馈信息。
轨迹规划的流程如图4所示首先判断是否为连续路径模式，是连续路径模式则先进行位置插补计算，再进行姿态插补计算，然后计算插补点目标位姿，计算完毕启动多点路径模式；不是连续路径模式则判断是否为示教模式，是示教模式则进入判断是否为期望位姿的循环，不是期望位姿则继续循环，是期望位姿则计算目标点位姿，计算完毕后启动多点路径模式；不是示教模式则直接启动多点路径模式；启动多点路径模式后，首先进行逆运动学计算，再判断是否运动学方程有解，运动学方程有解则进行可行解、最优解计算，然后进行多项式轨迹规划，规划完毕发送各自由度运动控制指令，程序结束；运动学方程无解则提示目标点错误，程序结束。
本实施例采用TI公司的DSP芯片TMS320F2812作为核心处理器，它是一款高速的数字信号处理芯片，不仅具有数字信号处理的优点，同时又具有微控制器的特点，无论在运算精度和数据的处理能力上都可以满足运动规划控制的高实时性要求，为完成复杂的实时轨迹规划算法提供了可靠的平台。
本实施例与上位机通讯采用以太网方式，以太网控制器芯片采用MicrochipTechnology(美国微芯科技公司)近期推出的28引脚独立以太网控制器ENC28J60，它采用标准的SPI串行接口，只需4根导线即可实现与TMS320F2812连接，有利于在系统上实现以太网功能。采用以太网通讯方式不但解决了传统串行通讯方式存在的传输速率低、通讯距离短和组网能力差等问题，具有信息量大、兼容性强、编址灵活方便等优点，而且可以通过以太网通讯模块将系统接入局域网或者因特网，从而实现系统的远程控制，提高了系统控制的灵活性和使用价值。
本实施例与多自由度机器人的各个自由度模块通讯采用CAN总线方式，CAN总线通讯具有多主机方式、传输距离远、传输速度快、抗干扰能力强、应用灵活等诸多优点，解决了传统控制器点对点数据传输方式中的传输速率低、接口复杂等问题。
本实施例主要由一个电源模块和三个芯片组成，结构简单、体积小、运算能力强、可远程操作、具有实时规划能力和快速数据交换能力，可满足多自由度机器人运动规划控制的要求。
本装置可广泛用于工业机器人、机械臂、空间机器人、类人机器人等具有多个自由度模块的机器人控制系统。
权利要求
1.一种多自由度机器人运动规划控制器，其特征在于电源转换模块1的输入端与外部直流电源7连接，电源转换模块1的输出端分别与DSP芯片2、以太网控制器3、以太网连接器4、CAN总线接口芯片6连接，DSP芯片2分别与以太网控制器3、CAN总线接口芯片6连接，以太网控制器3与以太网连接器4连接，CAN总线接口芯片6与CAN总线连接器5连接；多自由度机器人运动规划控制软件集成在DSP芯片2中。
2.根据权利要求1所述的多自由度机器人运动规划控制器，其特征在于所述的电源转换模块1包括开关电源模块8、双路输出电压调整器9、三极管10，电源转换模块1的输入端为24V、COM，输出端为5V、RESET、1.9V、3.3V；开关电源模块8的VIN+、VIN-引脚分别与24V、COM输入端连接，开关电源模块8的VOUT+引脚分别与双路输出电压调整器9的1IN、2IN引脚连接，开关电源模块8的VOUT+引脚与5V输出端连接，开关电源模块8的VOUT+引脚通过电阻R1与双路输出电压调整器9的1EN引脚连接，开关电源模块8的VOUT-引脚接地，双路输出电压调整器9的1EN引脚与三极管10的集电极连接，三极管10的基极通过电阻R5与双路输出电压调整器9的2OUT引脚连接，三极管10的发射极与双路输出电压调整器9的2EN引脚分别接地；双路输出电压调整器9的1RESET、2RESET引脚分别与RESET输出端连接，双路输出电压调整器9的1OUT引脚与1.9V输出端连接，双路输出电压调整器9的1FB/SENSE、1OUT引脚间通过电阻R3连接，双路输出电压调整器9的1FB/SENSE引脚通过R4接地，双路输出电压调整器9的2OUT引脚与3.3V输出端连接，双路输出电压调整器9的2OUT、1RESET引脚间通过电阻R2连接；电源转换模块1的24V、COM输入端与直流电源7连接，电源转换模块1的3.3V输出端分别与DSP芯片2的VDDIO引脚、以太网控制器3的VDD引脚、以太网连接器4的TX_CT引脚连接；电源转换模块1的RESET、1.9V输出端分别与DSP芯片2的XRS、VDD引脚连接；电源转换模块1的5V输出端与CAN总线接口芯片6的VCC引脚连接。
3.根据权利要求2所述的多自由度机器人运动规划控制器，其特征在于所述的电阻R1、R2、R3、R4、R5的阻值分别为5～15KΩ、5～15KΩ、15～25KΩ、25～35KΩ、5～15KΩ。
4.根据权利要求1所述的多自由度机器人运动规划控制器，其特征在于所述的DSP芯片2的XCLKOUT、GPIOB7、XINT1、XINT2、SPISIMO、SPISOMI、SPICLK引脚分别与以太网控制器3的OSC1、CS、INT、WOL、SI、SO、SCK引脚连接；DSP芯片2的CANTX、CANRX引脚分别与CAN总线接口芯片6的TXD、RXD引脚连接。
5.根据权利要求1所述的多自由度机器人运动规划控制器，其特征在于所述的以太网控制器3的TPOUT+、TPOUT-、TPIN+、TPIN-引脚分别与以太网连接器4的TP_OUT+、TP_OUT-、TP_IN+、TP_IN-引脚连接；以太网连接器4的J1、J2、J3、J4、J5、J6、J7、J8输出端通过双绞线与上位机连接。
6.根据权利要求1所述的多自由度机器人运动规划控制器，其特征在于所述的CAN总线接口芯片6的CANH、CANL引脚与CAN总线连接器5连接。
7.根据权利要求1所述的多自由度机器人运动规划控制器，其特征在于所述的多自由度机器人运动规划控制软件的主流程为首先初始化DSP控制器，再初始化以太网控制器，然后进入循环，判断是否接收到上位机指令，若未接收到指令则继续循环，若接收到指令则进入下一步；先读取CAN总线反馈信息，再计算机器人当前位姿，然后结合接收到的上位机指令进行轨迹规划；完成轨迹规划后，发送指令至CAN总线，再发送反馈信息至以太网，然后判断是否有以太网接收信息标志，有则返回至判断是否接收到上位机指令，没有则返回至读取CAN总线反馈信息。
8.根据权利要求7所述的多自由度机器人运动规划控制器，其特征在于所述的轨迹规划的流程为首先判断是否为连续路径模式，是连续路径模式则先进行位置插补计算，再进行姿态插补计算，然后计算插补点目标位姿，计算完毕启动多点路径模式；不是连续路径模式则判断是否为示教模式，是示教模式则进入判断是否为期望位姿的循环，不是期望位姿则继续循环，是期望位姿则计算目标点位姿，计算完毕后启动多点路径模式；不是示教模式则直接启动多点路径模式；启动多点路径模式后，首先进行逆运动学计算，再判断是否运动学方程有解，运动学方程有解则进行可行解、最优解计算，然后进行多项式轨迹规划，规划完毕发送各自由度运动控制指令，程序结束；运动学方程无解则提示目标点错误，程序结束。
全文摘要
本发明涉及一种多自由度机器人运动规划控制器。其技术方案是外部电源7与电源转换模块1的输入端连接，电源转换模块1的输出端分别与DSP芯片2、以太网控制器3、以太网连接器4、CAN总线接口芯片6连接，DSP芯片2分别与以太网控制器3、CAN总线接口芯片6连接，以太网控制器3与以太网连接器4连接，CAN总线接口芯片6与CAN总线连接器5连接；多自由度机器人运动规划控制软件集成在DSP芯片2中。本发明主要由一个电源模块和三个芯片组成，结构简单、体积小、运算能力强、可远程操作、具有实时规划能力和快速数据交换能力，可满足多自由度机器人运动规划控制的要求。
文档编号B25J13/00GK101045298SQ20071005184
公开日2007年10月3日 申请日期2007年4月12日 优先权日2007年4月12日
发明者吴怀宇, 方恒, 程磊, 许春山, 赵伟, 张巍, 胡琳萍, 刘亮 申请人:武汉科技大学