CN103753534B - 一种移动机器人控制器及其控制方法 - Google Patents

Abstract

本发明公开了一种移动机器人控制器及控制方法，包括遥控器、无线收发模块、工控机、接收机、传感器、传感器无线传输系统、比例阀、开关阀；通过遥控器采集板采集遥控器面板的控制信息，以指令形式发送给主工控机，同时工控机采集各关节绝对值编码器的信息，经主工控机进行运动学算法处理后，将控制指令发送给接收机，完成移动装配机器人的运动控制。本发明利用联接在其末端专用工具，采用主从加自主控制方式代替人的手臂完成频率高汽车装配任务，一方面可减轻作业人员的劳动强度，另一方面也为作业人员的安全提供了保证的移动装配机器人控制器及控制方法。

Description

一种移动机器人控制器及其控制方法

技术领域

本发明涉及一种机器人技术，尤其是一种移动机器人控制器及其控制方法。

背景技术

装配机器人是柔性自动化装配系统的核心设备，由机器人操作机、控制器、末端执行器和传感系统组成。其中操作机的结构类型有水平关节型、直角坐标型、多关节型和圆柱坐标型等；控制器一般采用多CPU或多级计算机系统，实现运动控制和运动编程；末端执行器为适应不同的装配对象而设计成各种手爪和手腕等；传感系统又来获取装配机器人与环境和装配对象之间相互作用的信息。常用的装配机器人主要有可编程通用装配操作手即PUMA机器人（最早出现于1978年，工业机器人的祖始）和平面双关节型机器人即SCARA机器人两种类型。与一般工业机器人相比，装配机器人具有精度高、柔顺性好、工作范围小、能与其他系统配套使用等特点，主要用于各种电器的制造行业。

目前的装配机器人都为固定机械臂,将机械臂安装在移动机器人之上，因此与固定的机械臂相比，具有更大的工作空间和更高的灵活性，可以得到更为广泛的应用。但是移动装配机器人比固定操作臂的结构更加复杂，所以对它末端规划、控制等操作也更为困难。

发明内容

本发明的目的是为克服上述现有技术的不足，提供一种移动装配机器人控制器，利用联接在其末端专用工具，采用主从加自主控制方式代替人的手臂完成汽车的装配作业，一方面可减轻作业人员的劳动强度，另一方面也为作业人员的安全提供了保证的移动装配机器人控制器及控制方法。

为实现上述目的，本发明采用下述技术方案。

一种移动机器人控制器，包括遥控器、无线收发模块、工控机、接收机、传感器、传感器无线传输系统、比例阀、开关阀。

所述遥控器通过无线模块与工控机通信，工控机连接接收机，接收机与比例阀、开关阀连接，传感器设于机器人上，且通过传感器无线传输系统连接工控机，所述遥控器的指令，通过无线模块下发给工控机，工控机通过接收机下发速度指令、开关驱动指令给机器人；所述遥控器包括模拟量采集模块、数字量采集模块、磁藕隔离模块I、磁藕隔离模块II、磁藕隔离模块III、复位电路、JTAG电路、微处理器电路、串口驱动电路。模拟量采集模块、数字量采集模块分别通过磁藕隔离模块I、磁藕隔离模块II与微处理器连接，微处理器、磁藕隔离模块III、串口驱动电路依次连接，复位电路、JTAG电路分别与微处理器连接，模拟量采集模块采集遥控器模拟摇杆的数据，数字量采集模块采集带锁按键、自复位按键的数据，并经过微处理器处理后，经过串口驱动电路、无线模块，发送给工控机。所述遥控器可以带有7个模拟摇杆，即发动机启停、油门大小、一键还原、示教、再现、世界坐标系运动按键、工具控制。

所述微处理器采用32位TMS320F2812DSP芯片，主频可达150MHz。

所述磁藕隔离模块选用ADI公司的ADuM1400/1/2四通道数字隔离器。两端工作电压2.7V～5.5V，支持低电压工作并能实现电平转换。

所述机器人上设有机械臂，所述机械臂是5自由度液压机械臂；位置传感器测量机械臂每个轴的角度或长度；所述机械臂可以由液压驱动，液压源设有电磁阀用来启动、停止发动机、控制油门的大小。

车体移动控制具备前进、后退、调速、左右转向安全监控等功能。

升降机构运动控制具备单关节控制、单关节位置反馈及安全监控等功能。

发动机及电源动力监控模块具备发动机启停、调速等控制和发动机状态及电源电压的监视等功能。

所述工控机采用ARK-5260小型工控机，主频1.66GHz，带有2个网口，支持4路232/422/485接口，自带2路PCI扩展槽，12-24V直流电源供电。

所述接收机输出7路模拟量信号，电压范围3～9V，6V代表中位；输出3路开关量信号，驱动电流10A，12V。

所述角度位移传感器采用BE1822S58,总精度单圈13位,RS485自由协议输出,无需找零,具备起点标定和置位功能。

所述无线传输模块采用SZ02-ZIGBEE无线通信模块，具有通讯距离远、抗干扰能力强、组网灵活等优点和特性，可实现一点对多点及多点对多点之间的设备间数据的透明传输。

所述液压比例阀采用丹佛斯阀，供电电压12V，输入控制信号3-9V，6V是零位。

本发明还公开了一种移动机器人控制方法，包括如下步骤：

输入机器人运动的初始点P₀(x₀,y₀,z₀)和终点P_f(x_f,y_f,z_f)运动速度P_v，加减速时间T_a和插补周期T_c，运行时间T；

基本参数的确定和插补点的求解方法；在进行插补运动前，确定P_v是否满足加减速要求；方法如下：

由P₀(x₀,y₀,z₀)和P_f(x_f,y_f,z_f)得到实际运动距离P_d＝|P₀P_f|；由P_v和T_a计算出加减速段所需距离若C_d≥P_d，则实际运动速度否则C_v＝P_v；由时间T_a和插补时间T_c得出加速步数S_a；由P₀(x₀,y₀,z₀)和P_f(x_f,y_f,z_f),可得空间直线参数方程

$\left\{\begin{array}{c}x=k(x_f-x_0)+x_0\\ y=k(y_f-y_0)+y_0\\ z=k(z_f-z_0)+z_0\end{array}\right.$ 其中k为比例因子，0≤k≤1(1)

因此由式(1)，可得各插补点P_i(x_i,y_i,z_i)到P₀的距离为

$C_{\mathrm{Sd}\left(i\right)}=\left|P_i{P}_0\right|=\sqrt{{(x_i-x_0)}^2+{(y_i-y_0)}^2+{(z_i-z_0)}^2}=k{P}_d$ （C_Sd(i)表示P_i(x_i,y_i,z_i)到P₀的距离，P_d＝|P₀P_f|）(2)

i为各插补点的步号，在0与之间的所有正整数；

令第n插补段运动距离为S_d(n)，n＝1,...,i，得点P_i到P₀的距离

$C_{\mathrm{Sd}\left(i\right)}=\underset{n=1}{\overset{i}{\mathrm{\Σ}}}{S}_{d\left(n\right)}=C_{\mathrm{Sd}(i-1)}+S_{d\left(i\right)},$ C_Sd(i-1)表示P_i-1(x_i-1,y_i-1,z_i-1)到P₀的距离，

S_d(i)是第i插补段运动距离，故由式(1)和(2)得到各插补点比例因子k的计算公式如下：

$k=\frac{C_{\mathrm{Sd}\left(i\right)}}{P_d}=\frac{\underset{n=1}{\overset{i}{\mathrm{\Σ}}}{S}_{d\left(n\right)}}{P_d}=\frac{C_{\mathrm{Sd}(i-1)}+S_{d\left(i\right)}}{P_d}$ 其中k为比例因子，0≤k≤1(3)

由式(3)求出k，并得到插补点直角坐标。

所述加速运动段运动距离S_d(i)确定方法如下：

由实际运动速度C_v和加减速时间T_a求得加速度因此加速度段上第i个插补点的速度S_cv(i)＝iT_c·a，得到

$S_{d\left(i\right)}=\frac{1}{2}(S_{\mathrm{cv}\left(i\right)}+S_{\mathrm{cv}(i-1)})\·T_c=\frac{1}{2}(2i-1)a{T_c}^2---\left(4\right)$

S_cv(i-1)表示第i-1个插补点的速度。

所述匀速运动段运动距离S_d(i)确定方法如下：

匀速段各插补段运动距离S_d(i)＝C_v·T_c。

所述减速运动段运动距离S_d(i)确定方法如下：

经过前面i-1个插补点后，得所剩距离L_d(i)＝P_d-C_Sd(i-1)，因此可得减速段加速度则减速度段上第m个插补点的速度S_cv(m)＝C_v+mT_c·a，即可得到

$S_{d\left(m\right)}=\frac{1}{2}(S_{\mathrm{cv}\left(m\right)}+S_{\mathrm{cv}(m-1)})\·T_c=\frac{1}{2}[2C_v+a\·(2m-1)\·T_c]---\left(5\right)$

本发明的移动装配机器人控制器的工作原理是：通过遥控器采集板采集遥控器面板的控制信息，以指令形式发送给主工控机，同时工控机采集各关节绝对值编码器的信息，经主工控机进行运动学算法处理后，将控制指令发送给接收机，完成移动装配机器人的运动控制。

采用上述方案，本发明具有以下优点，一是自行设计的移动装配机器人控制系统经实验验证，代替人的手臂完成作业频率较高汽车装配任务；二是嵌入式机械臂自重轻、尺寸小、控制系统功耗低和尺寸小，适合移动装配机器人的应用需要。三是移动装配机器人可以实现复杂的一键还原、示教再现、直线插补运动；四是系统采用模块化设计，具有开放性、可读性、可扩展性、可维护性，以便持续开发。五是主控制器采用主工控机，主工控机实现运动学正反解、插补算法，可以实现复杂的运动控制，处理速度快。六是控制器带有位置传感器接口、电位器接口，功能齐全，位置精度高。

附图说明

图1是本发明结构原理总框图；

图2是本发明遥控器采集板原理框图；

图3是本发明车载机械臂结构图；

图4是本发明遥控器面板图；

图5是本发明一键还原原理图；

图6是本发明世界坐标系轨迹规划流程图；

其中，1、遥控器；2、无线发送模块；3、无线接收模块；4、工控机；5、接收机；6、位置传感器；7、485总线；8、比例阀；9、开关阀；10、模拟量采集模块；11、数字量采集模块；12、磁藕隔离模块I；13、磁藕隔离模块II；14、磁藕隔离模块III；15、复位电路；16、JTAG电路；17、微处理器；18、串口驱动电路；19、晶振电路。

具体实施方式

下面结合附图和实施例对本发明进一步说明。

参见图1，一种移动装配机器人控制器，包括遥控器1、无线发送模块2、无线接收模块3、工控机4、接收机5、位置传感器6、485总线7、比例阀8、开关阀9。

遥控器1控制面板的输出接数据采集板的输入，数据采集板的输出接无线发送模块2；无线接收模块3的输出通过串口接工控机4的输入，工控机4的输出通过串口接接收机5的输入，接收机5的输出接比例阀8和开关阀9；位置传感器6通过485总线7接工控机4的输入。所述遥控器1的指令，通过无线发送模块2和无线接收模块3下发给工控机4，工控机4通过接收机5下发速度指令、开关驱动指令给液压系统。

参见图2，所述遥控器1采集板包括模拟量采集模块10、数字量采集模块11、磁藕隔离模块I12、磁藕隔离模块II13、磁藕隔离模块III14、复位电路15、JTAG电路16、微处理器17、串口驱动电路18、晶振电路19。模拟量采集模块10采集7路模拟摇杆的数据，数字量采集模块11采集带锁按键、自复位按键的数据，并经过微处理器17处理后，经过串口驱动电路18、无线发送模块2，发送给无线接收模块3。

参见图3，所述机械臂为5自由度，机械结构包括基座，基座上安装有支撑臂，支撑臂上端为肩关节，小臂安装在肩关节上，小臂的前端与平台连接。平台包括平台俯仰轴和平台旋转轴。

参见图4，遥控器面板示意图，模拟摇杆包括7个，控制左轮前后、右轮前后、大臂左右、大臂俯仰、小臂伸缩、平台俯仰、平台左右；带锁按键包括工具、选择、XYZ世界坐标系、油门大小的控制；自恢复按键包括复位、发动机启停、示教、再现、启动的控制；指示灯包括电源、无线通信、报警指示灯。

参见图5，移动装配机器人处于自动运动方式时，按照事先编写好的运动控制程序自动循环执行，直到按下控制面板上的Stop按键。

参见图6，本实施例采用空间直线插补的方式运动，可分为以下几步完成:

输入机器人运动的初始点P₀(x₀,y₀,z₀)和终点P_f(x_f,y_f,z_f)(f是final的缩写)运动速度P_v，加减速时间T_a和插补周期T_c，运行时间T；

基本参数的确定和插补点的求解方法。由于机器人空间直线运动需经过加减速和匀速运动段，因此在进行插补运动前，应确定P_v是否满足加减速要求。方法如下：

由P₀(x₀,y₀,z₀)和P_f(x_f,y_f,z_f)得到实际运动距离P_d＝|P₀P_f|；由P_v和T_a可计算出加减速段所需距离若C_d≥P_d，则实际运动速度否则C_v＝P_v；由时间T_a和插补时间T_c得出加速步数S_a。由P₀(x₀,y₀,z₀)和P_f(x_f,y_f,z_f),可得空间直线参数方程

$\left\{\begin{array}{c}x=k(x_f-x_0){+x}_0\\ y=k(y_f-y_0)+y_0\\ z=k(z_f-z_0)+z_0\end{array}\right.$ 其中k为比例因子(0≤k≤1)(1)

因此由式(1)，可得各插补点P_i(x_i,y_i,z_i)(i为各插补点的步号，在0与之间的所有正整数)到P₀的距离为

$C_{\mathrm{Sd}\left(i\right)}=\left|P_i{P}_0\right|=\sqrt{{(x_i-x_0)}^2+{(y_i-y_0)}^2+{(z_i-z_0)}^2}=k{P}_d$ （C_Sd(i)表示P_i(x_i,y_i,z_i)到P₀的距离，P_d＝|P₀P_f|）(2)

令第n插补段运动距离为S_d(n)(n＝1,...,i)(n是1到i的所有正整数)，(i为各插补点的步号，在0与之间的所有正整数)可得点P_i到P₀的距离(C_Sd(i-1)表示P_i-1(x_i-1,y_i-1,z_i-1)到P₀的距离，S_d(i)是第i插补段运动距离)，故由式(1)和(2)得到各插补点比例因子k的计算公式如下：

$k=\frac{C_{\mathrm{Sd}\left(i\right)}}{P_d}=\frac{\underset{n=1}{\overset{i}{\mathrm{\Σ}}}{S}_{d\left(n\right)}}{P_d}=\frac{C_{\mathrm{Sd}(i-1)}+S_{d\left(i\right)}}{P_d}$ 其中k为比例因子(0≤k≤1)(3)

由式(3)就可求出k，并得到插补点直角坐标。因此空间直线插补算法关键在于确定各插补段运动距离S_d(i)。下面介绍运动各段求取S_d(i)方法：

加速运动段。由于本发明设计的机器人加速段为匀加速运动，故由实际运动速度C_v和加减速时间T_a求得加速度(单位是m/s^2)，因此加速度段上第i个插补点的速度S_cv(i)＝iT_c·a，可得到

$S_{d\left(i\right)}=\frac{1}{2}(S_{\mathrm{cv}\left(i\right)}+S_{\mathrm{cv}(i-1)})\·T_c=\frac{1}{2}(2i-1)a{T_c}^2$ (S_cv(i-1)表示第i-1个插补点的速度)(4)

匀速运动段。由于本发明设计的机器人要求必须经过减速段，而且插补运算为“边算边走”，故每次进行匀速运动段开始前，必须计算所剩距离能否满足系统减速要求。匀速段各插补段运动距离S_d(i)＝C_v·T_c

减速运动段。由于在求取加速步数S_a时进行取整计算，因此不能简单的将加速段加速度取反后规划加速段，这样会引入误差，故减速段加速度应重新计算。经过前面i-1个插补点后，可得所剩距离L_d(i)＝P_d-C_Sd(i-1)，因此可得减速段加速度则减速度段上第m个插补点的速度S_cv(m)＝C_v+mT_c·a，即可得到

$S_{d\left(m\right)}=\frac{1}{2}(S_{\mathrm{cv}\left(m\right)}+S_{\mathrm{cv}(m-1)})\·T_c=\frac{1}{2}[2C_v+a\·(2m-1)\·T_c]---\left(5\right)$

上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。

Claims (5)

1.一种移动机器人控制器，其特征是，包括遥控器、无线收发模块、工控机、接收机、传感器、传感器无线传输系统、比例阀、开关阀；所述遥控器通过无线收发模块与工控机通信，工控机连接接收机，接收机与比例阀、开关阀连接，传感器设于机器人上，且通过传感器无线传输系统连接工控机，所述遥控器的指令，通过无线收发模块下发给工控机，工控机通过接收机下发速度指令、开关驱动指令给机器人；所述遥控器包括模拟量采集模块、数字量采集模块、磁藕隔离模块I、磁藕隔离模块II、磁藕隔离模块III、复位电路、JTAG电路、微处理器电路、串口驱动电路；模拟量采集模块、数字量采集模块分别通过磁藕隔离模块I、磁藕隔离模块II与微处理器连接，微处理器、磁藕隔离模块III、串口驱动电路依次连接，复位电路、JTAG电路分别与微处理器连接，模拟量采集模块采集遥控器模拟摇杆的数据，数字量采集模块采集带锁按键、自复位按键的数据，并经过微处理器处理后，经过串口驱动电路、无线收发模块，发送给工控机。

2.一种具有如权利要求1所述的移动机器人控制器的移动机器人控制方法，其特征是，包括如下步骤:

输入机器人运动的初始点P₀(x₀,y₀,z₀)和终点P_f(x_f,y_f,z_f)运动速度P_v，加减速时间T_a和插补周期T_c，运行时间T；

基本参数的确定和插补点的求解方法；在进行插补运动前，确定P_v是否满足加减速要求；方法如下：

由P₀(x₀,y₀,z₀)和P_f(x_f,y_f,z_f)得到实际运动距离P_d＝|P₀P_f|；由P_v和T_a计算出加减速段所需距离若C_d≥P_d，则实际运动速度否则C_v＝P_v；由时间T_a和插补时间T_c得出加速步数S_a；由P₀(x₀,y₀,z₀)和P_f(x_f,y_f,z_f)，得空间直线参数方程

$\left\{\begin{array}{c}x=k(x_f-x_0)+x_0\\ y=k(y_f-y_0)+y_0\\ z=k(z_f-z_0)+z_0\end{array}\right.$ 其中k为比例因子，0≤k≤1(1)

因此由式(1)，得各插补点P_i(x_i,y_i,z_i)到P₀的距离为

$C_{Sd\left(i\right)}=\left|P_i{P}_0\right|=\sqrt{{(x_i-x_0)}^2+{(y_i-y_0)}^2+{(z_i-z_0)}^2}={\mathrm{kP}}_d,$ C_Sd(i)表示P_i(x_i,y_i,z_i)到P₀的距离，P_d＝|P₀P_f|，(2)

i为各插补点的步号，在0与之间的所有正整数；

令第n插补段运动距离为S_d(n)，n＝1,...,i，得点P_i到P₀的距离

C_Sd(i-1)表示P_i-1(x_i-1,y_i-1,z_i-1)到P₀的距离，

S_d(i)是第i插补段运动距离，故由式(1)和(2)得到各插补点比例因子k的计算公式如下：

其中k为比例因子，0≤k≤1，(3)

由式(3)求出k，并得到插补点直角坐标。

3.如权利要求2所述的移动机器人控制方法，其特征是，加速运动段运动距离S_d(i)确定方法如下：

由实际运动速度C_v和加减速时间T_a求得加速度因此加速度段上第i个插补点的速度S_cv(i)＝iT_c·a，得到

$S_{d\left(i\right)}=\frac{1}{2}(S_{cv\left(i\right)}+S_{cv(i-1)})\·T_c=\frac{1}{2}(2i-1){{\mathrm{aT}}_c}^2---\left(4\right)$

S_cv(i-1)表示第i-1个插补点的速度。

4.如权利要求2所述的移动机器人控制方法，其特征是，匀速运动段运动距离S_d(i)确定方法如下：

匀速段各插补段运动距离S_d(i)＝C_v·T_c。

5.如权利要求2所述的移动机器人控制方法，其特征是，减速运动段运动距离S_d(i)确定方法如下：

经过前面i-1个插补点后，得所剩距离L_d(i)＝P_d-C_Sd(i-1)，得减速段加速度则减速度段上第m个插补点的速度S_cv(m)＝C_v+mT_c·a，即得到

$S_{d\left(m\right)}=\frac{1}{2}(S_{cv\left(m\right)}+S_{cv(m-1)})\·T_c=\frac{1}{2}\[2C_v+a\·(2m-1)\·T_c\]---\left(5\right).$