嵌入式轻型机械臂控制器及其控制方法

摘要

本发明涉及一种嵌入式轻型机械臂控制器，包括相连接的示教盒控制器和嵌入式主从DSP控制器，所述示教盒控制器包括微处理器I，微处理器I分别与人机接口单元和嵌入式主从DSP控制器相连；所述嵌入式主从DSP控制器包括微处理器II和微处理器III，所述微处理器II分别与微处理器I和双口RAM连接，双口RAM与微处理器III相连，微处理器III分别与运动控制芯片I和运动控制芯片II相连，运动控制芯片I和运动控制芯片II分别与电机驱动器、原点开关和限位开关相连，电机驱动器与电机相连，电机输出轴与机械臂相连。本发明同时公开了一种嵌入式轻型机械臂的控制方法。本发明重量轻、处理速度快、成本低、稳定性好、功能易扩展。

权利要求

1.一种嵌入式轻型机械臂控制器，其特征在于：包括相连接的示教盒控制器和嵌入式主从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的数据总线、地址总线、控制总线与双口RAM的左数据总线、地址总线、控制总线相连，微处理器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所述的嵌入式轻型机械臂控制器的控制嵌入式轻型机械臂的方法，其特征在于，包括如下步骤：

1).对每个杆件在关节轴处建立一个正规的笛卡儿坐标系(xi，yi，zi)，其中，i是1到

6之间的所有正整数，i为自由度数目，再加上基座坐标系(x0，y0，z0)，在基座上的位置和方向可任选，只要z0轴沿着第一关节轴即可；

2).为每个杆件在关节轴处的笛卡儿坐标系建立4×4奇次变换矩阵，表示与前一个杆件在关节轴处的笛卡儿坐标系的关系；

3).采用“边算边走”的定时插补算法，计算插补点的位置和姿态；

4).采用公式法计算每个关节轴的运动学反解，得出插补周期内的每个关节轴的运动角度；

5).得出的每个关节轴的运动角度输出到微处理器III，微处理器III的位置指令输出到运动控制芯片I和运动控制芯片II，来控制每个关节轴的插补运动。

9.根据权利要求8所述的方法，其特征在于，所述步骤1)中确定和建立每个坐标系应采用下面三条规则：每个关节i的运动都绕着zi轴运动；xi轴垂直zi-1轴并指向离开zi-1轴的方向；yi轴按右手坐标系的要求建立；其中i是1到6之间的所有正整数，i为自由度数目。

说明书

嵌入式轻型机械臂控制器及其控制方法

技术领域

[0001] 本发明涉及一种机器人技术，尤其是一种嵌入式轻型机械臂控制器及其控制方法。

背景技术

[0002] 机械臂是在自动化生产过程中使用的一种具有抓取和移动工件功能的自动化装置，它是在机械化、自动化生产过程中发展起来的一种新型装置。机械臂能代替人类完成危险、重复枯燥的工作，减轻人类劳动强度，提高劳动生产率。机械臂越来越广泛地得到了应用，在机械行业中它可用于零部件组装，加工工件的搬运、装卸，特别是在自动化数控机床、组合机床上使用更普遍。

[0003] 目前机械臂控制系统一般分为两类：一类采用工控机和控制卡，使用WINDOWS操作系统，控制系统重量较重，机械臂算法处理速度慢，价格较高且系统不稳定；另一类采用80系列单片机，控制系统硬件过于简单，功能难以扩展。

发明内容

[0004] 本发明的目的是为克服上述现有技术的不足，提供一种重量轻、处理速度快、成本低、稳定性好、功能易扩展的嵌入式轻型机械臂控制器及其控制方法。

[0005] 为实现上述目的，本发明采用下述技术方案：

[0006] 一种嵌入式轻型机械臂控制器，其特征在于：包括相连接的示教盒控制器和嵌入式主从DSP(数字信号处理器)控制器，所述示教盒控制器包括微处理器I，微处理器I分别与人机接口单元和嵌入式主从DSP(数字信号处理器)控制器相连；所述嵌入式主从DSP(数字信号处理器)控制器包括微处理器II和微处理器III，所述微处理器II分别与微处理器I和双口RAM连接，双口RAM与微处理器III相连，微处理器III分别与运动控制芯片I和运动控制芯片II相连，运动控制芯片I和运动控制芯片II分别与电机驱动器、原点开关和限位开关相连，电机驱动器与电机相连，电机输出轴与机械臂相连。

[0007] 所述人机接口单元包括显示器和键盘。

[0008] 所述微处理器I与微处理器II通过串口通信连接。

[0009] 所述微处理器I、微处理器II和微处理器III均采用TMS320F2812芯片。

[0010] 所述微处理器II的数据总线、地址总线、控制总线与双口RAM的左数据总线、地址总线、控制总线相连，微处理器III的数据总线、地址总线、控制总线与双口RAM的右数据总线、地址总线、控制总线相连。

[0011] 所述微处理器III的数据总线、地址总线、控制总线分别与运动控制芯片I和运动控制芯片II的数据总线、地址总线、控制总线相连。

[0012] 所述运动控制芯片I、运动控制芯片II均采用MCX314芯片。

[0013] 所述运动控制芯片I的脉冲输出口1-4与步进电机驱动器的输入口1-4相连，运动控制芯片I原点信号采集口与原点开关的输出口1-4相连，运动控制芯片I限位开关采集口与限位开关的输出口1-8相连；运动控制芯片II的脉冲输出口1-3与步进电机驱动器的输入口5-7相连，运动控制芯片II原点信号采集口与原点开关的输出口5-7相连，运动控制芯片II限位开关采集口与限位开关输出口9-12相连。

[0014] 所述电机驱动器的输出采用正负脉冲形式，电机采用两相混合式步进电机。

[0015] 一种嵌入式轻型机械臂控制方法，其特征在于，包括如下步骤：

[0016] 1).对每个杆件在关节轴处可建立一个正规的笛卡儿坐标系(xi，yi，zi)(i是1到6之间的所有正整数，6为自由度数目)，再加上基座坐标系(x0，y0，z0)(在机座上的位置和方向可任选，只要z0轴沿着第一关节运动轴即可)；

[0017] 2).为每个关节处的杆件坐标系建立4×4奇次变换矩阵，表示与前一个杆件坐标系的关系；

[0018] 3).采用“边算边走”的定时插补算法，计算插补点的位置和姿态；“边算边走”是指将各插补点进行逆运动学变换后得到的关节位置不用存储，而直接再按这些关节位置开始运动；

[0019] 4).采用公式法计算每个轴的运动学反解(运动学反解是指已知末端位置和姿态求每个关节的角度)，得出插补周期内的每个轴的运动角度；

[0020] 5).得出的每个轴的运动角度输出到微处理器III，微处理器III的位置指令输出到运动控制芯片I和运动控制芯片II，来控制每个轴的插补运动。

[0021] 所述步骤中1)确定和建立每个坐标系应采用下面三条规则：每个关节i(i是1到6之间的所有正整数，6为自由度数目)的运动都绕着zi轴运动；xi轴垂直zi-1轴并指向离开zi-1轴的方向；yi轴按右手坐标系得要求建立。

[0022] 本发明的机械臂控制系统的结构采用主从式微处理器进行控制，微处理器II作为主机，它担当系统管理、机械臂语言编译和人机接口功能，同时也利用它的运算能力完成坐标变换、轨迹插补、运动学正解、运动学反解，并定时地把运算结果作为关节运动的增量送到公共内存，供微处理器III读取它。微处理器III完成全部关节位置数字控制。它从公共内存读给定值，也把各关节实际位置送回公共内存中，微处理器II使用。公共内存是由容量为8KB的双口RAM。这类系统的控制速率快，一般可达10ms。

[0023] 采用上述方案，本发明具有以下优点，一是自行设计的机械臂运动控器经实验验证，可以满足机械臂控制要求，运行可靠、成本低廉可以作为机械臂运动控制器应用和销售；二是嵌入式轻型机械臂自重轻、尺寸小、控制系统功耗低和尺寸小，适合移动操作操作机器人的应用需要。三是嵌入式轻型机械臂可以实现复杂的直线插补、圆弧插补运动；四是系统采用模块化设计，具有开放性、可读性、可扩展性、可维护性，以便持续开发。五是机械臂运动控制器采用主从式微处理器，微处理器II实现运动学正反解、插补算法，微处理器III实现运动控制，处理速度快。六是控制器带有驱动器控制接口、原点开关采集口，功能齐全，位置精度高。

附图说明

[0024] 图1是本发明总框图；

[0025] 图2是本发明示教盒硬件电路接口连接图；

[0026] 图3是本发明机械臂主控制板硬件接口连接图；

[0027] 图4是本发明机械臂从控制板硬件接口连接图；

[0028] 图5是本发明键盘示意图；

[0029] 图6是本发明机械臂示意图；

[0030] 图7是本发明原点搜索程序流程图；

[0031] 图8是本发明关节坐标系运动程序流程图；

[0032] 图9是本发明单轴运动子程序流程图；

[0033] 图10是本发明直角坐标系运动程序流程图；

[0034] 图11是本发明定时直角插补运动示意图；

[0035] 图12是本发明工具坐标系运动程序流程图；

[0036] 图13是本发明圆柱坐标系运动程序流程图；

[0037] 图14是本发明运动学反解示意图。

具体实施方式

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

[0039] 参见图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相连。

[0040] 人机接口单元包括键盘和液晶显示，它们分别与微处理器I连接。

[0041] 微处理器I与微处理器II通过串口通信。

[0042] 微处理器I、微处理器II和微处理器III均采用TMS320F2812芯片。

[0043] 微处理器I采集键盘的数据指令，通过串口通信下发给微处理器II，运动速度、下发指令、机械臂的位置通过液压模块显示。

[0044] 微处理器II与微处理器III通过双口RAM进行数据交互。微处理器II的数据总线、地址总线、控制总线与双口RAM的左数据总线、地址总线、控制总线相连，微处理器III的数据总线、地址总线、控制总线与双口RAM的右数据总线、地址总线、控制总线相连。

[0045] 微处理器III的数据总线、地址总线、控制总线与运动控制芯片I、运动控制芯片II的数据总线、地址总线、控制总线相连。

[0046] 运动控制芯片I、运动控制芯片II均采用MCX314芯片。

[0047] 运动控制芯片I输出的正、负脉冲信号控制步进电机驱动器1-4，运动控制芯片I原点信号采集口与原点开关的输出口1-4相连，运动控制芯片I限位开关采集口与限位开关的输出口1-8相连。运动控制芯片II输出的正、负脉冲信号控制步进电机驱动器5-7，运动控制芯片II原点信号采集口与原点开关的输出口5-7相连，运动控制芯片II限位开关采集口与限位开关输出口9-12相连。

[0048] 驱动器的输出采用正负脉冲形式，电机采用两相混合式步进电机。

[0049] 机械臂控制系统的结构采用主从式微处理器进行控制，微处理器II作为主机，它担当系统管理、机械臂语言编译和人机接口功能，同时也利用它的运算能力完成坐标变换、轨迹插补、运动学正解、运动学反解，并定时地把运算结果作为关节运动的增量送到公共内存，供微处理器III读取它。微处理器III完成全部关节位置数字控制。它从公共内存读给定值，也把各关节实际位置送回公共内存中，微处理器II使用。公共内存是由容量为8KB的双口RAM。这类系统的控制速率快，一般可达10ms。

[0050] 参见图2，示教盒控制器由微处理器I、液晶模块、逻辑电平转换器、键盘管理模块、键盘、稳压芯片I、稳压芯片II、串口接收发送器、串口组成。稳压芯片I、稳压芯片I给微处理器I供电。微处理器的GIPIOB1与ADG3308的2脚连接，GPIOB5与5脚连接，XINT2与6脚连接。GPIOA0-7与液晶模块的DB0-7连接，GPIOB0与REQ连接，GPIOB2与CS连接，液晶模块5V供电。ADG3308的16脚与HD7279的DATA脚连接，15与KEY脚连接。微处理器I的GPIOB3与HD7279的CS脚连接，GPIOB4与CLK脚连接。键盘的输出接HD7279的DIG0-7、DP-SG。微处理器I的SCITXDA接MAX3232的11脚，SCIRXDA接12脚，MAX3232的
13、14脚接到串口。参见图3，嵌入式主控制器包括双口RAM、微处理器II、串口接收发送器、串口、从控制器接口。微处理器II的XD0-15接双口RAM的IO0-15L、/XRD接/OEL、/XWE接R//WL、/XZCS2接/CEL、XA0-11接A0-11L。双口RAM的M//S接3.3V，设置成主模式。双口RAM的IO0-15R、/OER、R//WR、/CER、A0-11R接从控制器接口。微处理器II的SCITXDA接MAX3232的11脚，SCIRXDA接12脚，MAX3232的13、14脚接到串口。

[0051] 参见图4，嵌入式从控制板包括双口RAM接口、微处理器III、16M有源晶振、运动控制芯片1、运动控制芯片2、光耦隔离、驱动器接口、原点开关接口、限位开关接口。双口RAM接口的IO0-15R、/OER、R//WR、/CER、A0-11R接微处理器III的XD0-15、/XRD、/XWE、/XZCS2、XA0-11。微处理器III的XD0-15、/XRD、/XWE、XA14、XA0-2分别接运动控制芯片I的D0-15、RDN、WRN、CSN、A0-2。微处理器III的XD0-15、/XRD、/XWE、XA13、XA0-2分别接运动控制芯片II的D0-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的35、36脚分别接驱动器5的正脉冲、负脉冲输入口；运动控制芯片II的38、39脚分别接驱动器6的正脉冲、负脉冲输入口；运动控制芯片II的40、41脚分别接驱动器7的正脉冲、负脉冲输入口。

[0052] 参见图5，键盘示意图，S+表示机械臂关节坐标系第一轴的正运动、直角坐标系X+运动、工具坐标系X+运动、圆柱坐标系θ+运动，S-表示关节坐标系第一轴的负运动、直角坐标系X-运动、工具坐标系X-运动、圆柱坐标系θ-运动；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-表示减小速度；按下原点搜索键执行原点搜索运动；按坐标系切换键时，坐标系以下列顺序变化：关节-直角-工具-圆柱。

[0053] 一种嵌入式轻型机械臂控制方法，包括如下步骤：

[0054] 1)对每个杆件在关节轴处可建立一个正规的笛卡儿坐标系(xi，yi，zi)(i是1到6之间的所有正整数，6为自由度数目)，再加上基座坐标系(x0，y0，z0)(在机座上的位置和方向可任选，只要z0轴沿着第一关节运动轴即可)；

[0055] 2).为每个关节处的杆件坐标系建立4×4奇次变换矩阵，表示与前一个杆件坐标系的关系；

[0056] 3).采用“边算边走”(“边算边走”是指将各插补点进行逆运动学变换后得到的关节位置不用存储，而直接再按这些关节位置开始运动)的定时插补算法，计算插补点的位置和姿态；

[0057] 4).采用公式法计算每个轴的运动学反解(运动学反解是指已知末端位置和姿态求每个关节的角度)，得出插补周期内的每个轴的运动角度；

[0058] 5).得出的每个轴的运动角度输出到微处理器III，微处理器III的位置指令输出到运动控制芯片I和运动控制芯片II，来控制每个轴的插补运动。

[0059] 参见图6，对每个杆件在关节轴处可建立一个正规的笛卡儿坐标系(xi，yi，zi)(i是1到6之间的所有正整数，6为自由度数目)，再加上基座坐标系(x0，y0，z0)(在机座上的位置和方向可任选，只要z0轴沿着第一关节运动轴即可)。该发明确定和建立每个坐标系应根据下面三条规则：每个关节i(i是1到6之间的所有正整数，6为自由度数目)的运动都绕着zi轴运动；xi轴垂直zi-1轴并指向离开zi-1轴的方向；yi轴按右手坐标系得要求建立。

[0060] 参见图7，回机械原点的过程是：设置加减速度、速度等参数；关闭原点开关采集口；各轴正方向运动活动空间的一半；打开原点开关采集口；执行反方向运动；直到搜索到原点开关，执行相应轴的减速停止子程序。

[0061] 参见图8、9，关节坐标系运动，首先输入运动的加减速度、速度值，通过读取端口的状态判断特定键的按下或松开，特定键按下后，系统对指定轴按照设定的参数进行加速连续驱动；当按键松开时，系统发出减速停止命令。

[0062] 参见图10、11、12、13，直角坐标系运动，首先输入运动的加减速度、速度值，通过读取端口的状态判断特定键的按下或松开，特定键按下后，系统对指定轴进行正反解算法、直线插补或圆弧插补运动；当按键松开时，系统发出减速停止命令。

[0063] 该发明空间直线插补可分为以下几步完成：

[0064] 输入机器人运动的初始点P0(x0，y0，z0)和终点Pf(xf，yf，zf)(f是final的缩写)运动速度Pv，加减速时间Ta和插补周期Tc，运行时间T；

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

[0066] 由P0(x0，y0，z0)和Pf(xf，yf，zf)得到实际运动距离Pd＝|P0Pf|；由Pv和Ta可计算出加减速段所需距离 若Cd≥Pd，则实际运动速度 否则Cv＝Pv；由时间Ta和插补时间Tc得出加速步数Sa。由P0(x0，y0，z0)和Pf(xf，yf，zf)，可得空间直线参数方程

[0067]

[0068] 因此由式(1)，可得各插补点Pi(xi，yi，zi)(i为各插补点的步号，在0与 之间的所有正整数)到P0的距离为

[0069] 表示Pi(xi，yi，zi)到P0的距离，

[0070] Pd＝|P0Pf|) (2)

[0071] 令第n插补段运动距离为Sd(n)(n＝1，...，i)(n是1到i的所有正整数)，(i为各插补点的步号，在0与 之间的所有正整数)可得点Pi到P0的距离
(CSd(i-1)表示Pi-1(xi-1，yi-1，zi-1)到P0的距离，Sd(i)是第i插补
段运动距离)，故由式(1)和(2)得到各插补点比例因子k的计算公式如下：

[0072] 其 中 k 为 比 例 因 子 (0 ≤ k ≤ 1)(3)

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

[0074] 加速运动段。由于本文设计的机器人加速段为匀加速运动，故由实际运动速度Cv和加减速时间Ta求得加速度 (单位是m/s^2)，因此加速度段上第i个插补点的速度Scv(i)＝iTc·a，可得到

[0075] 表示第i-1个插补点的速度)(4)

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

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

[0078]

[0079] 该发明采用公式法进行运动学反解(运动学反解是指已知末端位置和姿态求每个关节的角度，如图14所示)：

[0080] px

[0081] py

[0082] Pz------表示机械臂末端在世界坐标系中的位置；

[0083] nx ox ax

[0084] ny oy ay

[0085] nz oz az------表示机械臂末端在世界坐标系中的姿态；

[0086] θ1，...，θ6------表示每个轴运动的角度；

[0087] Ai∈R4×4(i＝1，2，…，6)------是根据D-H坐标系建立的各连杆上坐标系间的转换矩阵。

[0088] si-----表示sinθi；

[0089] ci------表示cosθi；

[0090] sij------表示sin(θi+θj)；

[0091] cij------- 表 示 cos(θi+θj) 表 示 坐 标系1和坐标系0的齐次变换矩阵 表示坐标系2和
坐标系1的齐次变换矩阵 表示坐标系3和坐标
系2的齐次变换矩阵 表示坐标系4和坐标系3的
齐次变换矩阵 表示坐标系5和坐标系4的齐次变
换矩阵 表示坐标系6和坐标系5的齐次变换矩阵
它确定了操作机端部相对于机座坐标系的位置和姿态。

[0092] n-手的法向矢量

[0093] s-手的滑动矢量

[0094] a-手的接近矢量

[0095] p-手的位置矢量 (6)

[0096] 坐标系Oi(i是正整数i＝0，1，......，6)则是在操作臂上建立的D-H坐标系；a2，d4∈R分别表示机械臂对应连杆的长度。可以将操作臂末端坐标系O6在基座坐标系O0下0 0
的位姿写成如下表达式：T6＝A1A2A3A4A5A6。求解运动方程时，从 T6开始求解关节位置。使
0 0
T6的符号表达式的各元素等于 T6的一般形式，并据此确定θ1。一旦求得θ1之后，可由-1 0
A1 左乘 T6的一般形式，得

[0097] A-110T6＝1T6 (7)

[0098] 此式可用来求解其他各关节变量。不断地用A的逆矩阵左乘(7)，可得下列另四个矩阵方程式：

[0099] A2-1A1-10T6＝2T6 (8)

[0100] A3-1A2-1A1-10T6＝3T6 (9)

[0101] A4-1A3-1A2-1A1-10T6＝4T6 (10)

[0102] A5-1A4-1A3-1A2-1A1-10T6＝5T6 (11)

[0103] 上式各方程的左式为0T6和前(i-1)个关节变量的函数，可用这些方程来确定各关节的位置：

[0104] θ1＝atan2(py，px)(-3.1415≤θ1≤3.1415) (12)

[0105]

[0106]

[0107] 注意：式中，正、负号对应θ3的两种可能解。

[0108]

[0109] θ2＝atan2(s23，c23)-θ3(-1.5707≤θ1≤1.5707) (14)

[0110] θ4＝atan2(-axs1+ayc1，-axc1c23-ays1c23+azs23)(-3.1415≤θ1≤3.1415) (15)[0111] 注意：当s5＝0时，机械臂处于奇异形位。此时，关节轴4和6重合，只能解出θ4与θ6的和或差。奇异形位可以由式(15)中atan2的两个变量是否都接近零来判断。

[0112] θ5＝atan2(-ax(c1c23c4+s1s4)-ay(s1c23c4-c1s4)+azs23c4，-axc1s23-ays23s1-azc23)(-3.9268≤θ1≤0.7853) (16)

[0113] k1＝-nx(c1c23s4-s1c4)-ny(s1c23s4+c1c4)+nzs23s4

[0114] k2＝nx((c1c23c4+s1s4)c5-c1s23s5)+ny((s1c23c4-c1s4)c5-s1s23s5)-nz(s23c4c5+c23s5)[0115] θ6＝atan2(k1，k2)(-3.1415≤θ1≤3.1415) (17)。