专利名称：嵌入式轻型机械臂控制器及其控制方法
技术领域：
本发明涉及一种机器人技术，尤其是一种嵌入式轻型机械臂控制器及其控制方法。
背景技术：
机械臂是在自动化生产过程中使用的一种具有抓取和移动工件功能的自动化装 置，它是在机械化、自动化生产过程中发展起来的一种新型装置。机械臂能代替人类完成危 险、重复枯燥的工作，减轻人类劳动强度，提高劳动生产率。机械臂越来越广泛地得到了应 用，在机械行业中它可用于零部件组装，加工工件的搬运、装卸，特别是在自动化数控机床、 组合机床上使用更普遍。目前机械臂控制系统一般分为两类一类采用工控机和控制卡，使用WINDOWS操 作系统，控制系统重量较重，机械臂算法处理速度慢，价格较高且系统不稳定；另一类采用 80系列单片机，控制系统硬件过于简单，功能难以扩展。
发明内容
本发明的目的是为克服上述现有技术的不足，提供一种重量轻、处理速度快、成本 低、稳定性好、功能易扩展的嵌入式轻型机械臂控制器及其控制方法。为实现上述目的，本发明采用下述技术方案一种嵌入式轻型机械臂控制器，其特征在于包括相连接的示教盒控制器和嵌入 式主从DSP (数字信号处理器)控制器，所述示教盒控制器包括微处理器I，微处理器I分 别与人机接口单元和嵌入式主从DSP(数字信号处理器)控制器相连；所述嵌入式主从 DSP(数字信号处理器)控制器包括微处理器II和微处理器III，所述微处理器II分别与 微处理器I和双口 RAM连接，双口 RAM与微处理器III相连，微处理器III分别与运动控制 芯片I和运动控制芯片II相连，运动控制芯片I和运动控制芯片II分别与电机驱动器、原 点开关和限位开关相连，电机驱动器与电机相连，电机输出轴与机械臂相连。所述人机接口单元包括显示器和键盘。所述微处理器I与微处理器II通过串口通信连接。所述微处理器I、微处理器II和微处理器III均采用TMS320F2812芯片。所述微处理器II的数据总线、地址总线、控制总线与双口 RAM的左数据总线、地址 总线、控制总线相连，微处理器III的数据总线、地址总线、控制总线与双口 RAM的右数据总 线、地址总线、控制总线相连。所述微处理器III的数据总线、地址总线、控制总线分别与运动控制芯片I和运动 控制芯片II的数据总线、地址总线、控制总线相连。所述运动控制芯片I、运动控制芯片II均采用MCX314芯片。所述运动控制芯片I的脉冲输出口 1-4与步进电机驱动器的输入口 1-4相连，运 动控制芯片I原点信号采集口与原点开关的输出口 1-4相连，运动控制芯片I限位开关采集口与限位开关的输出口 1-8相连；运动控制芯片II的脉冲输出口 1-3与步进电机驱动器 的输入口 5-7相连，运动控制芯片II原点信号采集口与原点开关的输出口 5-7相连，运动 控制芯片II限位开关采集口与限位开关输出口 9-12相连。所述电机驱动器的输出采用正负脉冲形式，电机采用两相混合式步进电机。一种嵌入式轻型机械臂控制方法，其特征在于，包括如下步骤1).对每个杆件在关节轴处可建立一个正规的笛卡儿坐标系(Xi，yi，Zi) (i是1到 6之间的所有正整数，6为自由度数目)，再加上基座坐标系(X(l，y0, z0)(在机座上的位置和 方向可任选，只要Ztl轴沿着第一关节运动轴即可)；2).为每个关节处的杆件坐标系建立4X4奇次变换矩阵，表示与前一个杆件坐标 系的关系；3).采用“边算边走”的定时插补算法，计算插补点的位置和姿态；“边算边走”是 指将各插补点进行逆运动学变换后得到的关节位置不用存储，而直接再按这些关节位置开 始运动；4).采用公式法计算每个轴的运动学反解(运动学反解是指已知末端位置和姿态 求每个关节的角度)，得出插补周期内的每个轴的运动角度；5).得出的每个轴的运动角度输出到微处理器III，微处理器III的位置指令输出 到运动控制芯片I和运动控制芯片II，来控制每个轴的插补运动。所述步骤中1)确定和建立每个坐标系应采用下面三条规则每个关节i (i是1到 6之间的所有正整数，6为自由度数目)的运动都绕着Zi轴运动；Xi轴垂直Zh轴并指向离 开Zh轴的方向…轴按右手坐标系得要求建立。本发明的机械臂控制系统的结构采用主从式微处理器进行控制，微处理器II作 为主机，它担当系统管理、机械臂语言编译和人机接口功能，同时也利用它的运算能力完成 坐标变换、轨迹插补、运动学正解、运动学反解，并定时地把运算结果作为关节运动的增量 送到公共内存，供微处理器III读取它。微处理器III完成全部关节位置数字控制。它从 公共内存读给定值，也把各关节实际位置送回公共内存中，微处理器II使用。公共内存是 由容量为8KB的双口 RAM。这类系统的控制速率快，一般可达10ms。采用上述方案，本发明具有以下优点，一是自行设计的机械臂运动控器经实验验 证，可以满足机械臂控制要求，运行可靠、成本低廉可以作为机械臂运动控制器应用和销 售；二是嵌入式轻型机械臂自重轻、尺寸小、控制系统功耗低和尺寸小，适合移动操作操作 机器人的应用需要。三是嵌入式轻型机械臂可以实现复杂的直线插补、圆弧插补运动；四是 系统采用模块化设计，具有开放性、可读性、可扩展性、可维护性，以便持续开发。五是机械 臂运动控制器采用主从式微处理器，微处理器II实现运动学正反解、插补算法，微处理器 III实现运动控制，处理速度快。六是控制器带有驱动器控制接口、原点开关采集口，功能齐 全，位置精度高。
图1是本发明总框图；图2是本发明示教盒硬件电路接口连接图；图3是本发明机械臂主控制板硬件接口连接图4是本发明机械臂从控制板硬件接口连接图；图5是本发明键盘示意图；图6是本发明机械臂示意图；图7是本发明原点搜索程序流程图；图8是本发明关节坐标系运动程序流程图；图9是本发明单轴运动子程序流程图；图10是本发明直角坐标系运动程序流程图；图11是本发明定时直角插补运动示意图；图12是本发明工具坐标系运动程序流程图；图13是本发明圆柱坐标系运动程序流程图；图14是本发明运动学反解示意图。
具体实施例方式下面结合附图和实施例对本发明进一步说明。参见图1，一种嵌入式轻型机械臂的控制器，包括示教盒控制器和嵌入式主从DSP 控制器两部分。示教盒控制器由键盘模块、液晶显示模块、串口通信模块、微处理器I组成。 控制键盘的输出接微处理器I的输入，微处理器I的输入输出接液晶显示的输入输出，微处 理器I的串口与机械臂微处理器II的串口通信。嵌入式主从DSP控制器由微处理器II、 双口 RAM、微处理器III、运动控制芯片I、运动控制芯片II、光电开关处理电路、限位开关处 理电路、步进电机驱动模块、抱闸松开控制等组成。微处理器II的数据总线、地址总线、控 制总线与双口 RAM的左数据总线、地址总线、控制总线相连，双口 RAM的右数据总线、地址总 线、控制总线与微处理器III相连，微处理器III的数据总线、地址总线、控制总线与运动控 制芯片I、运动控制芯片II的数据总线、地址总线、控制总线相连，运动控制芯片I的脉冲输 出口 1-4与步进电机驱动器的输入口 1-4相连，运动控制芯片I原点信号采集口与原点开 关的输出口 1-4相连，运动控制芯片I限位开关采集口与限位开关的输出口 1-8相连，运动 控制芯片II的脉冲输出口 1-3与步进电机驱动器的输入口 5-7相连，运动控制芯片II原 点信号采集口与原点开关的输出口 5-7相连，运动控制芯片II限位开关采集口与限位开关 输出口 9-12相连。人机接口单元包括键盘和液晶显示，它们分别与微处理器I连接。微处理器I与微处理器II通过串口通信。微处理器I、微处理器II和微处理器III均采用TMS320F2812芯片。微处理器I采集键盘的数据指令，通过串口通信下发给微处理器II，运动速度、下 发指令、机械臂的位置通过液压模块显示。微处理器II与微处理器III通过双口 RAM进行数据交互。微处理器II的数据总 线、地址总线、控制总线与双口 RAM的左数据总线、地址总线、控制总线相连，微处理器III 的数据总线、地址总线、控制总线与双口 RAM的右数据总线、地址总线、控制总线相连。微处理器III的数据总线、地址总线、控制总线与运动控制芯片I、运动控制芯片 II的数据总线、地址总线、控制总线相连。运动控制芯片I、运动控制芯片II均采用MCX314芯片。
运动控制芯片I输出的正、负脉冲信号控制步进电机驱动器1-4，运动控制芯片I 原点信号采集口与原点开关的输出口 1-4相连，运动控制芯片I限位开关采集口与限位开 关的输出口 1-8相连。运动控制芯片II输出的正、负脉冲信号控制步进电机驱动器5-7，运 动控制芯片II原点信号采集口与原点开关的输出口 5-7相连，运动控制芯片II限位开关 采集口与限位开关输出口 9-12相连。驱动器的输出采用正负脉冲形式，电机采用两相混合式步进电机。机械臂控制系统的结构采用主从式微处理器进行控制，微处理器II作为主机，它 担当系统管理、机械臂语言编译和人机接口功能，同时也利用它的运算能力完成坐标变换、 轨迹插补、运动学正解、运动学反解，并定时地把运算结果作为关节运动的增量送到公共内 存，供微处理器III读取它。微处理器III完成全部关节位置数字控制。它从公共内存读 给定值，也把各关节实际位置送回公共内存中，微处理器II使用。公共内存是由容量为8KB 的双口 RAM。这类系统的控制速率快，一般可达10ms。参见图2，示教盒控制器由微处理器I、液晶模块、逻辑电平转换器、键盘管理模 块、键盘、稳压芯片I、稳压芯片II、串口接收发送器、串口组成。稳压芯片I、稳压芯片I给 微处理器I供电。微处理器的GIPI0B1与ADG3308的2脚连接，GPI0B5与5脚连接，XINT2 与6脚连接。GPI0A0-7与液晶模块的DB0-7连接，GPIOBO与REQ连接，GPI0B2与CS连接， 液晶模块5V供电。ADG3308的16脚与HD7279的DATA脚连接，15与KEY脚连接。微处理 器I的GPI0B3与HD7279的CS脚连接，GPI0B4与CLK脚连接。键盘的输出接HD7279的 DIGO-7、DP-SG。微处理器 I 的 SCITXDA 接 MAX3232 的 11 脚，SCIRXDA 接 12 脚，MAX3232 的 13、14脚接到串口。参见图3，嵌入式主控制器包括双口 RAM、微处理器II、串口接收发送器、 串口、从控制器接口。微处理器II的XDO-15接双口 RAM的I00_15L、/XRD接/OEL、/XWE接 R//WL./XZCS2 接 /CEL、XA0-11 接 A0-11L。双口 RAM 的 M//S 接 3. 3V，设置成主模式。双口 RAM 的 I00-15R、/OER、R//WR、/CER、A0-11R 接从控制器接口。微处理器 II 的 SCITXDA 接 MAX3232 的 11 脚，SCIRXDA 接 12 脚，MAX3232 的 13、14 脚接到串口。参见图4，嵌入式从控制板包括双口 RAM接口、微处理器III、16M有源晶振、运动 控制芯片1、运动控制芯片2、光耦隔离、驱动器接口、原点开关接口、限位开关接口。双口 RAM 接口的 I00-15R、/OER, R//WR、/CER, A0-11R 接微处理器 III 的 XDO-15、/XRD, /XffE, / XZCS2、XAO-Il0 微处理器 III 的 XDO-15、/XRD, /XWE, XA14、XA0-2 分别接运动控制芯片 I 的 DO-15、RDN、WRN、CSN、A0-2。微处理器 III 的 XDO-15、/XRD, /XWE, XA13、XA0-2 分别接 运动控制芯片II的DO-15、RDN、WRN、CSN、A0_2。16M有源晶振的输出口接运动控制芯片 I、II的53脚。正限位开关1-4的输出接口经过光耦隔离分别接运动控制芯片I的69、87、 97、116脚；负限位开关5-8的输出接口经过光耦隔离分别接运动控制芯片I的70、88、98、 117脚；原点开关1、2、3、4的输出口经过光耦隔离分别接运动控制芯片I的73、93、101、120 脚；运动控制芯片I的35、36脚分别接驱动器1的正脉冲、负脉冲输入口 ；运动控制芯片I 的38、39脚分别接驱动器2的正脉冲、负脉冲输入口 ；运动控制芯片I的40、41脚分别接驱 动器3的正脉冲、负脉冲输入口 ；运动控制芯片I的42、43脚分别接驱动器4的正脉冲、负 脉冲输入口。原点开关5、6、7的输出口经过光耦隔离分别接运动控制芯片II的73、93、101 脚；正限位开关9-10的输出接口经过光耦隔离分别接运动控制芯片I的69、87脚；负限位 开关11-12的输出接口经过光耦隔离分别接运动控制芯片I的70、88脚；运动控制芯片II
7的35、36脚分别接驱动器5的正脉冲、负脉冲输入口 ；运动控制芯片II的38、39脚分别接 驱动器6的正脉冲、负脉冲输入口 ；运动控制芯片II的40、41脚分别接驱动器7的正脉冲、 负脉冲输入口。参见图5，键盘示意图，S+表示机械臂关节坐标系第一轴的正运动、直角坐标系X+ 运动、工具坐标系X+运动、圆柱坐标系θ +运动，S-表示关节坐标系第一轴的负运动、直角 坐标系χ-运动、工具坐标系χ-运动、圆柱坐标系θ -运动；L+表示机械臂关节坐标系第二 轴的正运动、直角坐标系Y+运动、工具坐标系Y+运动、圆柱坐标系r+运动，L-表示关节坐 标系第二轴的负运动、直角坐标系Y-运动、工具坐标系Y-运动、圆柱坐标系r-运动；U+表 示关节坐标系第三轴的正运动、直角坐标系Z+运动、工具坐标系Z+运动、圆柱坐标系Z+运 动，U-表示关节坐标系第三轴的负运动、直角坐标系Z-运动、工具坐标系Z-运动、圆柱坐 标系Z-运动；R+表示关节坐标系第四轴的正运动，R-表示关节坐标系第四轴的负运动；B+ 表示关节坐标系第五轴的正运动，B-表示关节坐标系第五轴的负运动；T+表示关节坐标系 第六轴的正运动，T-表示关节坐标系第六轴的负运动；M+表示手爪的开，M-表示手爪的合； V+表示增加速度，V-表示减小速度；按下原点搜索键执行原点搜索运动；按坐标系切换键 时，坐标系以下列顺序变化关节_直角-工具-圆柱。一种嵌入式轻型机械臂控制方法，包括如下步骤1)对每个杆件在关节轴处可建立一个正规的笛卡儿坐标系(Xi，yi; Zi) (i是1到 6之间的所有正整数，6为自由度数目)，再加上基座坐标系(X(l，y0, z0)(在机座上的位置和 方向可任选，只要Ztl轴沿着第一关节运动轴即可)；2).为每个关节处的杆件坐标系建立4X4奇次变换矩阵，表示与前一个杆件坐标 系的关系；3).采用“边算边走”(“边算边走”是指将各插补点进行逆运动学变换后得到的 关节位置不用存储，而直接再按这些关节位置开始运动)的定时插补算法，计算插补点的 位置和姿态；4).采用公式法计算每个轴的运动学反解(运动学反解是指已知末端位置和姿态 求每个关节的角度)，得出插补周期内的每个轴的运动角度；5).得出的每个轴的运动角度输出到微处理器III，微处理器III的位置指令输出 到运动控制芯片I和运动控制芯片II，来控制每个轴的插补运动。参见图6，对每个杆件在关节轴处可建立一个正规的笛卡儿坐标系(Xi，Yi, Zi) (i 是1到6之间的所有正整数，6为自由度数目)，再加上基座坐标系(X(l，y0, z0)(在机座上的 位置和方向可任选，只要Ztl轴沿着第一关节运动轴即可)。该发明确定和建立每个坐标系 应根据下面三条规则每个关节i (i是1到6之间的所有正整数，6为自由度数目)的运动 都绕着Zi轴运动；Xi轴垂直Zh轴并指向离开ZH轴的方向轴按右手坐标系得要求建 立。参见图7，回机械原点的过程是设置加减速度、速度等参数；关闭原点开关采集 口 ；各轴正方向运动活动空间的一半；打开原点开关采集口 ；执行反方向运动；直到搜索到 原点开关，执行相应轴的减速停止子程序。参见图8、9，关节坐标系运动，首先输入运动的加减速度、速度值，通过读取端口的 状态判断特定键的按下或松开，特定键按下后，系统对指定轴按照设定的参数进行加速连续驱动；当按键松开时，系统发出减速停止命令。参见图10、11、12、13，直角坐标系运动，首先输入运动的加减速度、速度值，通过读 取端口的状态判断特定键的按下或松开，特定键按下后，系统对指定轴进行正反解算法、直 线插补或圆弧插补运动；当按键松开时，系统发出减速停止命令。该发明空间直线插补可分为以下几步完成输入机器人运动的初始点Ptl (x0, y0, Z0)和终点Pf (xf，yf, zf) (f是final的缩写) 运动速度Pv，加减速时间Ta和插补周期T。，运行时间T ；基本参数的确定和插补点的求解方法。由于机器人空间直线运动需经过加减速和 勻速运动段，因此在进行插补运动前，应确定Pv是否满足加减速要求。方法如下由P0 (x0, y。，z0)和Pf (xf, yf，zf)得到实际运动距离Pd = | P0Pf | ；由Pv和Ta可计算
出加减速段所需距离Crf =丄乃7；，若Cd彡Pd,则实际运动速度6 =^-Pv，否则Cv = Pv;由时
间Ta和插补时间Tc得出加速步数Sa0由P0(x0, y0, z0)和Pf (xf, yf，zf)，可得空间直线参数
方程
(1)
因此由式(1)，可得各插补点PiUi, y,, Zi) (i为各插补点的步号，在0与f之间的 所有正整数)到Ptl的距离为
(Cmo表示Pi(UpZi)到P0 的距离，
(2)令第η插补段运动距离为Sd(n) (η = 1，...，i) (η是1到i的所有正整 数)，(i为各插补点的步号，在0与f之间的所有正整数)可得点Pi到Ptl的距离
表示Pp1 (Xh，y^, Zi^1)到Ptl的距离，Sd(i)是第i插补
/1=1
段运动距离)，故由式⑴和⑵得到各插补点比例因子k的计算公式如下
/
(3)由式(3)就可求出k，并得到插补点直角坐标。因此空间直线插补算法关键在于确 定各插补段运动距离Sd(i)。下面介绍运动各段求取Sd(i)方法加速运动段。由于本文设计的机器人加速段为勻加速运动，故由实际运动速度Cv和加减速时间Ta求得加速度《 = ·(单位是m/s~2)，因此加速度段上第i个插补点的速度
Ta
Sct⑴=iT。*a，可得到 Sd^=Us^+Sc<i_x)}Tc^U2i-\)aTc2 (5"CT(M)表示第 i_l 个插补点的速度)
(4)勻速运动段。由于本文设计的机器人要求必须经过减速段，而且插补运算为“边算 边走”，故每次进行勻速运动段开始前，必须计算所剩距离能否满足系统减速要求。勻速段 各插补段运动距离sd(i) = Cv . Tc减速运动段。由于在求取加速步数Sa时进行取整计算，因此不能简单的将加速段 加速度取反后规划加速段，这样会引入误差，故减速段加速度应重新计算。经过前面i-Ι个
插补点后，可得所剩距离Ld(i) = Pd-Csd(H)，因此可得减速段加速度《 =，则减速度段
上第m个插补点的速度Scv(m) = Cv+mTc · a,即可得到 该发明采用公式法进行运动学反解(运动学反解是指已知末端位置和姿态求每 个关节的角度，如图14所示)
⑶
转换矩阵
ny oy
-表示机械臂末端在世界坐标系中的位置；
nz oz az------表示机械臂末端在世界坐标系中的姿态；
Q1,..., θ 6------表示每个轴运动的角度；
Ai e R4x4(i = 1,2,…，6)------是根据D-H坐标系建立的各连杆上坐标系间的
表示sin θ j ； -表不 COS θ J ；
一表示 sinOi+ep ； ——表示cos ( θ广θ ) A1 =
表示坐标 系1和坐标系0的齐次变换矩阵A7 =
表示坐标系2和坐标系1的齐次变换矩阵
表示坐标系3和坐标系2的齐次变换矩阵
表示坐标系4和坐标系3的阵 矩 换 变 次 齐
表示坐标系5和坐标系4的齐次变阵 矩 换
表示坐标系6和坐标系5的齐次变换矩阵
它确定了操作机端部相对于机座坐标系的位置和姿态。η-手的法向矢量S-手的滑动矢量a_手的接近矢量P-手的位置矢量(6)坐标系Oi (i是正整数i = 0,1,......，6)则是在操作臂上建立的D-H坐标系；a2，
d4 e R分别表示机械臂对应连杆的长度。可以将操作臂末端坐标系O6在基座坐标系Otl下 的位姿写成如下表达式= A1A2A3AAA60求解运动方程时，从tlT6开始求解关节位置。使 tlT6的符号表达式的各元素等于tlT6的一般形式，并据此确定θ1() —旦求得Q1之后，可由 ΑΓ1左乘°Τ6的一般形式，得A^110T6 = 1T6(7)此式可用来求解其他各关节变量。不断地用A的逆矩阵左乘(7)，可得下列另四个 矩阵方程式 A2 1A1 10Ts一’Ts(8)[oloo] A3 1A2 1A1 10Ts一’Ts(9)[o101] A4 1A3 1A2 1A1 10Ts一‘Ts(10)
A5 1A4 1A3 1A2 1A1 10Ts一‘Ts (11)[o103] 上式各方程的左式为‘T。和前(i—1)个关节变量的函数，可用这些方程来确定各关节的位置θ j = atan2 (py, ρχ) (_3· 1415 彡 6^3. 1415)(12) 注意式中，正、负号对应θ 3的两种可能解。
rmoo1
Pz2 + (ClPx + SlPy)Pz2 + (C1Px + SlPy )θ 2 = atan2 (s23, c23) - θ 3 (-1. 5707 彡 θ j ^ 1. 5707) (14)θ 4 = atan2 (-BxS^ayC1, -a^c^-a^^+a^) (-3. 1415 ( θ 1 ^ 3. 1415) (15)注意当S5 = O时，机械臂处于奇异形位。此时，关节轴4和6重合，只能解出θ4 与θ 6的和或差。奇异形位可以由式(15)中atan2的两个变量是否都接近零来判断。θ 5 = atan2 (_ax (C1C23C^S1S4) _ay (S1C23C4-C1S4) +azs23c4, -BxC1S23-ByS23S1-BzC23) (_3· 9268 ^θ j ^ 0. 7853)(16) k2 = Ilx((C1C23C^S1S4)C5-C1S23S5)+Ily((S1C23C4-C1S4)C5-S1S23S5)-Ilz(S23C4CdC23S5)θ 6 = atar^Q^，k2) (-3. 1415 彡 θ i 彡 3. 1415) (17)
权利要求
一种嵌入式轻型机械臂控制器，其特征在于包括相连接的示教盒控制器和嵌入式主从DSP控制器，所述示教盒控制器包括微处理器I，微处理器I分别与人机接口单元和嵌入式主从DSP控制器相连；所述嵌入式主从DSP控制器包括微处理器II和微处理器III，所述微处理器II分别与微处理器I和双口RAM连接，双口RAM与微处理器III相连，微处理器III分别与运动控制芯片I和运动控制芯片II相连，运动控制芯片I和运动控制芯片II分别与电机驱动器、原点开关和限位开关相连，电机驱动器与电机相连，电机输出轴与机械臂相连。
2.根据权利要求1所述的嵌入式轻型机械臂控制器，其特征在于所述人机接口单元 包括显示器和键盘。
3.根据权利要求1所述的嵌入式轻型机械臂控制器，其特征在于所述微处理器I与 微处理器II通过串口通信连接。
4.根据权利要求1所述的嵌入式轻型机械臂控制器，其特征在于所述微处理器I、微 处理器II和微处理器III均采用TMS320F2812芯片。运动控制芯片I、运动控制芯片II均 采用MCX314芯片。
5.根据权利要求1所述的嵌入式轻型机械臂控制器，其特征在于所述微处理器II的 数据总线、地址总线、控制总线与双口 MM的左数据总线、地址总线、控制总线相连，微处理 器III的数据总线、地址总线、控制总线与双口 RAM的右数据总线、地址总线、控制总线相连。
6.根据权利要求1所述的嵌入式轻型机械臂控制器，其特征在于所述微处理器III 的数据总线、地址总线、控制总线分别与运动控制芯片I和运动控制芯片II的数据总线、地 址总线、控制总线相连。
7.根据权利要求1所述的嵌入式轻型机械臂控制器，其特征在于所述运动控制芯片I 的脉冲输出口 1-4与步进电机驱动器的输入口 1-4相连，运动控制芯片I原点信号采集口 与原点开关的输出口 1-4相连，运动控制芯片I限位开关采集口与限位开关的输出口 1-8 相连；运动控制芯片II的脉冲输出口 1-3与步进电机驱动器的输入口 5-7相连，运动控制 芯片II原点信号采集口与原点开关的输出口 5-7相连，运动控制芯片II限位开关采集口 与限位开关输出口 9-12相连。
8.一种嵌入式轻型机械臂控制方法，其特征在于，包括如下步骤1).对每个杆件在关节轴处可建立一个正规的笛卡儿坐标系(Xi，Ii,Zi)，其中，i是1 到6之间的所有正整数，6为自由度数目，再加上基座坐标系(X(l，y0, z0)，在机座上的位置和 方向可任选，只要Ztl轴沿着第一关节运动轴即可；2).为每个关节处的杆件坐标系建立4X4奇次变换矩阵，表示与前一个杆件坐标系的 关系；3).采用“边算边走”的定时插补算法，计算插补点的位置和姿态；4).采用公式法计算每个轴的运动学反解，得出插补周期内的每个轴的运动角度；5).得出的每个轴的运动角度输出到微处理器III，微处理器III的位置指令输出到运 动控制芯片I和运动控制芯片II，来控制每个轴的插补运动。
9.所述步骤中1)确定和建立每个坐标系应采用下面三条规则每个关节i的运动都 绕着Zi轴运动；Xi轴垂直Zh轴并指向离开ZH轴的方向…轴按右手坐标系得要求建立；其中i是1到6之间的所有正整数，6为自由度数目。
全文摘要
本发明涉及一种嵌入式轻型机械臂控制器，包括相连接的示教盒控制器和嵌入式主从DSP控制器，所述示教盒控制器包括微处理器I，微处理器I分别与人机接口单元和嵌入式主从DSP控制器相连；所述嵌入式主从DSP控制器包括微处理器II和微处理器III，所述微处理器II分别与微处理器I和双口RAM连接，双口RAM与微处理器III相连，微处理器III分别与运动控制芯片I和运动控制芯片II相连，运动控制芯片I和运动控制芯片II分别与电机驱动器、原点开关和限位开关相连，电机驱动器与电机相连，电机输出轴与机械臂相连。本发明同时公开了一种嵌入式轻型机械臂的控制方法。本发明重量轻、处理速度快、成本低、稳定性好、功能易扩展。
文档编号B25J13/00GK101913149SQ201010234678
公开日2010年12月15日 申请日期2010年7月23日 优先权日2010年7月23日
发明者戚晖, 李健, 赵玉良, 陈凡明 申请人:山东电力研究院