专利名称：六自由度机器人运动学cordic算法协处理器的制作方法
技术领域：
本发明涉及一种在空间机器人控制中用于进行高速计算的协处理 器，特别是一种基于CORDIC算法对空间机器人运动学进行正反解高速计算 的协处理器，属于空间技术领域。
背景技术：
空间机器人作为一种通用机械、电气、计算机控制系统和人工智能 的综合体，可以感知外界环境，并进行推理、判断和决策，能够在未知 的环境中完成各种各样的任务，完全胜任空间作业的要求。未来空间机 器人需要完成的任务越来越复杂，这就要求其中央控制计算机具有足够 的处理能力。
目前地面机器人的主控计算机通常采用地面高速嵌入式系统、工控 机甚至高性能的通用计算机实现，虽然地面机器人主控计算机的运算能 力较高，但是很难适应空间强辐射、大温差、高真空以及发射时的振动 要求，因此不可能作为空间机器人中央控制器。目前也有利用较成熟的 空间用计算机作为空间机器人的中央控制器，但目前空间用计算机(如
X386， 80C51系列)由于系统的抗单粒子设计、降额设计，使其运算能 力一般都在10MIPS以下，很难满足空间机械手实时控制的高处理能力要 求。
在国际航天领域，空间机器人控制器目前主要有两种设计方法，第 一种是纯粹采用抗辐照技术和策略制造的系统，这样的系统采用抗辐照 的微处理器(如Lockheed的PowerPC结构的RAD750，欧空局SPARC V7结 构的TSC695系列处理器，SPARC V8结构的LEON系列处理器，这些处理 器都采用了抗辐照设计，但速度相对商用微处理器较慢)、存储器、电路 板等等，为了避免辐照带来的负面效应，电路设计中会采用各种复杂的 硬件检错纠错机制和硬件冗余容错策略；芯片布线不能太密，走线不能 太细，否则也会由于辐照产生单粒子效应或者加快总辐照剂量效应带来 破坏，因此也限制了芯片速度和系统性能的提高。
另一种方法种是采用商用组件技术搭建的系统， 一般使用商用微处理器(例如Atmel的ARM系列，Motorola的PowerPC 603e， IBM的 PowerPC 750FXTM)。此类系统虽微处理器速度快、性能较高，但由于 商业器件不同于专用的星载芯片，没有专门的防护和容错机制，所以必 须采用一套比较全面的策略来缓解和减轻辐射对系统带来的扰动。太空 空间这样一个特殊环境，星载计算机既要增加很多容错策略来提高运行 可靠性，又要完成复杂的空间任务，因此星载计算机的处理速度还远不 能做到现今通用计算机那样高的程度，尤其是我们国家星上计算机性能 相对国外更是有较大差距。这种情况下此类星载计算机芯片应用于空间 机器人运动控制运算时，其运算速度就更显得捉襟见肘。这对于实时性 要求极高的空间任务的完成有着不可回避的矛盾。
另外，在空间机器人运动学正反解的过程中，包含很多超越函数的运算， 并且在各个关节角的计算中很少包含相同的计算式。因此在通常的只使用一 个主处理器的空间机器人中央控制计算机上进行运动学正反解会占用较多的 控制时间和系统资源，影响系统的实时性。如何解决正反解中大量超越函数 的快速计算问题，使用何种适合运动学算式及系统硬件特点的算法，成为了 提高中央控制计算机运动学计算速度的关键问题之一。
发明内容
本发明的发明目的在于解决现有空间机器人技术中存在的问题，提 供一种基于CORDIC算法的机器人运动学正反解高速计算的协处理器。 本发明的发明目的是通过下述技术方案予以实现的 六自由度机器人运动学CORDIC算法协处理器，用以协助主处理器
完成数据计算，其特征在于包括输入输出逻辑接口、算法运算模块；
所述算法运算模块中包括运算参数寄存器、命令字寄存器、控制
逻辑模块、状态字寄存器、输入FIFO、输出FIFO、运算处理模块；
所述输入输出逻辑接口分别与所述运算参数寄存器、命令字寄存器
和输入FIFO相连；所述运算参数寄存器、命令字寄存器将所存参数输
入至所述控制逻辑模块；所述控制逻辑模块控制所述状态字寄存器中的 状态参数；该状态字寄存器与输入输出逻辑接口相连；
所述输入FIFO、运算处理模块、输出FIFO顺序串联，并且受所述 控制逻辑模块控制；所述输出FIFO与输入输出逻辑接口相连。
该协处理包括三个结构相同的所述算法运算模块；该协处理还设有 三模输出比较器、错误处理模块、配置更新模块；所述三个的算法运算模块并联于所述输入输出逻辑接口和三模输 出比较器之间；该三模输出比较器的输出端与所述错误处理模块、输入 输出逻辑接口相连；该错误处理模块的输出端通过输入输出逻辑接口与 主处理器相连；该配置更新模块的输入端通过输入输出逻辑接口与主处 理器相连；该配置更新模块的输出端与所述三个算法运算模块相连。
所述运算处理模块中存储有机器人运动学正解的CORDIC算法流 水结构和六个关节角度的运动学反解的CORDIC算法流水结构；所述机 器人运动学正解的CORDIC算法流水结构和六个关节角度的运动学反解 的CORDIC算法流水结构分别由若干CORDIC算法的基本模块构成；所 述控制逻辑模块通过控制运算处理模块选择进行不同的算法运算。
所述第一关节角度的运动学反解的CORDIC算法流水结构由一个基 本模块(2)构成。
所述第二关节角度的运动学反解的CORDIC算法流水结构包括八个 基本模块(3)、 一个基本模块(1)、 一个基本模块(2)和一个基本模块(6); 所述基本模块(1)的输出端分为并行的两路； 一路依次串联有第一至第 四基本模块(3)和基本模块(6);另一路依次串联有第五至第七基本模 块(3)、第一基本模块(2)和第八基本模块(3);所述基本模块(6) 和第八基本模块(3)的输出端分别连接至基本模块(2)的输入端，由 基本模块(2)计算第二关节角度。
所述第三关节角度的运动学反解的CORDIC算法流水结构包括四个 基本模块(3)、 一个基本模块(2)、 一个基本模块(6);所述第一基本模块
(3)、第二基本模块(3)、第三基本模块(3)串联；该第三基本模块(3) 的输出端分别与所述基本模块(6)和第四基本模块(3)的输入端相连；所 述基本模块(6)和第四基本模块3的输出端连接于基本模块(2)的输入端， 由基本模块(2)计算第三关节角度。
所述第四关节角度的运动学反解的CORDIC算法流水结构由顺序串 联的第一基本模块(1)、基本模块(3)、第二基本模块(1)、基本模块(2) 构成；由所述基本模块(2)输出第四关节角度。
所述第五关节角度的运动学反解的CORDIC算法流水结构包括三个 基本模块(1)、两个基本模块(3)和一个基本模块(2);所述第一基本模块
(1)和第一基本模块(3)串联，所述第二基本模块(1)和第二基本模块(3) 串联；该第一基本模块(3)和第二基本模块(3)的输出端连接于第三基本模块(1)输入端；所述第三基本模块(1)的输出信号输入基本模块(2)， 由基本模块(2)计算第五关节角度。
所述第六关节角度的运动学反解的CORDIC算法流水结构包括四个 基本模块(l)、两个基本模块(3)和一个基本模块(2);所述流水结构中包 括两路并行的输入线路； 一路由该第一基本模块(1)、第一基本模块(3)、 第二基本模块(1)顺序串联而成；另一路由该第三基本模块(1)、第二基本 模块(3)、第四基本模(1)顺序串联而成；该第二基本模块(1)和第四基 本模块(1)的输出端与基本模块(2)的输入端相连，由基本模块2计算第 六关节角度。
本发明的有益效果是本发明所设计的CORDIC算法的协处理器通过 采用CORDIC算法替代了传统的通过计算大量超越方程完成机器人运动 学正反解计算，使大量的超越方程计算过程简化为只需要通过加法、减 法、和移位等这些硬件便于实现的操作，从而大大提高了协处理器的计 算速度。
图l为六自由度空间机械臂模型简图； 图2为第一次矩阵相乘的CORDIC算法；
图3为六自由度空间机械臂运动学正解的CORDIC计算流水线； 图4为关节角《的CORDIC计算流程；
图5为关节角《的CORDIC计算流程；
图6为关节角《的CORDIC计算流程；
图7为关节角《的CORDIC计算流程；
图8为关节角《的CORDIC计算流程；
图9为关节角《的CORDIC计算流程；
图10为运动学CORDIC算法的协处理器的基本结构图； 图11为三冗余结构的CORDIC算法的协处理器结构图； 图12为CORDIC算法基本模块图表。
具体实施方式
下面结合附图和实施例对本发明作进一步描述。
由前述可知，空间机器人不同于一般的地面机器人，由于处于太 空空间这样一个特殊环境，对于系统的可靠性和稳定性都有了较高的 要求。另一方面，空间机器人运动学FF反解的过程中，包含很多超越函数 的运算，并且在各个关节角的计算中很少包含相同的计算式，这对处理器的 处理能力提出了较高的要求。基于这种情况，使用一种有效的求解析解的算 法并开发与此算法紧密相关的专用协处理器来加速中央控制计算机的运动控 制计算能力，成为了解决空间机器人运动控制计算需求和星载计算机性能瓶 颈矛盾的较好途径。本发明即是基于这一设计思想，设计了一种专门针对空 间机器人运动学正反解高速计算的协处理器。
六自由度机器人正反解
六自由度机器人是目前空间技术中比较常见的机械结构。下面即 以六自由度空间机械臂为例，分析六自由度机器人运动学正反解的解 析表达式。图1为六自由度空间机械臂模型简图。该六自由度机械臂 由三个二自由度双关节P,、 P2、 P3，两根碳纤维臂杆， 一个手爪Pg和一 个安装在手爪上的手眼视觉构成。其末端三个关节的轴线相交于一点 P3。该空间机械臂可以安装在空间平台上来完成某些空间任务，如抓取、 维修、操作等。
这个六转动关节机械臂的运动学正解方程可以用六个关节角角度来表示。 六个关节角角度《'从关节位置传感器反馈。
根据Denav i t-Har tenberg方法，该机械臂末端手爪相对于基坐标系的位姿 矩阵^可以由6个4 x 4的齐次变换矩阵顺序相乘得至l」，见式(1 )。这里，' 是坐标系W相对于坐标系"-1}的变换矩阵。
rn;r…〖r 式m
通过手眼视觉可以得到目标体相对于手爪坐标系的位置，并且构成其4x4
的齐次变换矩阵y。
<formula>formula see original document page 8</formula>式(2)为了得到手爪捕获目标的笛卡儿轨迹上的期望节点，需要将^变换到基 坐标下的^ :
<formula>formula see original document page 9</formula>式(3)
图l中，^是机械臂的腕关节，并且水端3个关节的轴线交于腕关节的中 心，故机械臂术端的位姿可以分丌求解。前3个关节用来决定其位置，后三 关节用来决定其姿势。
若机械臂木端定义在基座标下的位姿矩阵为'°7:
<formula>formula see original document page 9</formula>则腕关节位置坐标^ = d ， P"，尺:)可以由下式表示<formula>formula see original document page 9</formula>
各关节的转角用e-(《，《，…，《)表示，若知道腕关节的位置坐标，则前3
个关节的关节角可被求出。采用其次变换求得的解析解如下<formula>formula see original document page 9</formula>得出前3个关节角后，后3个关节的关节角可作如下求解
《=一咖(~^--)
式(7)
6 4 = to"—1(
式(8)
(95 =—
C4(C23(CI", + 54C^"t
式(9)
其中、分另ij是sin^ 、 cos《的缩写形式。、=sin《("=l，2，'''，6); c,, = cos(9"(w = 1，2，..，，6). s23 = sin((92 +《)c23 = cos(<92+《)
从上面的求解式可以发现，在正反解得过程中，包含很多超越函数的运 算，并且在各个关节角的计算中很少包含相同的计算式。因此，如何解决正 反解中大量超越函数的快速计算问题，使用何种适合运动学算式及系统硬件 特点的算法，是提高中央控制计算机运动学计算速度的关键问题。
CORDIC算法
CORDIC算法是一种坐标旋转数值计算方法，将任意坐标的某一角
度的旋转移动通过离散地反复旋转直至收敛而得到，所用到的操作只有
加法、减法、和移位。这些操作用硬件能够很快的实现，从而提高了计
算速度。经典的CORDIC算法共有三种旋转系统，分别为圆周(circular)
系统、线性(linear)系统与双曲(hyperbolic)系统，算法使用了线性，圆周
和双曲坐标下的二维向量旋转。
由于CORDIC算法的推到过程复杂，且为现代信号处理领域经常使 用的成熟算法，在此仅给出其结论性的计算公式。三种旋转系统下， CORDIC算法的基本公式
、+1
z,+i =—《e,
圆周坐标系旋转// = l,e,=tan-'2-线性坐标系旋转A = 0，e,=2-' 双曲坐标系旋转= -二tanh—'2一'
在CRODIC算法的旋转和向量计算模式中，选择不同的迭代约束就 可以得到如图12的CRODIC算法基本模块表。由图12我们可以知道， 通过选择CRODIC算法的不同计算模式和旋转系统，我们可以通过 CRODIC算法得至lj cosz 、 sinz、 tan"y、乘法、除法、cosh z、 sinh z、 tanh-ly
等函数计算方法。按图12中标记所示，我们将CRODIC算法的六种不 同计算模块称为基本模块1至基本模块6。每个基本模块都具有三路输 入和对应的三路输出，我们将这三路分别称为x、 y、 z。
通过观察发现，六自由度机器人运动学正反解解析表达式中的代数式结 构与CORDIC算法基本模块有很大相似度。故，可以将六自由度机器人运动 学正反解解析表达式与CORDIC算法相结合，得到一种新的算法。而由于 CORDIC算法所用到的操作只有加法、减法、和移位。这些操作用硬件 能够很快的实现，从而提高了处理器计算速度。这是本发明的设计核心 所在。
运动学正解的CORDIC算法
为了便于采用CORDIC算法，式(3)可以被改写成以下形式 ^ = ( 7^.,7>^ = (^..〗7>7^ = (^..〗7> 457^ = =^><^式(10)
这里，只需进行六次矩阵相乘就可以计算出》。
假设，=fA， 7， ^'力(1),为定义在手抓坐标系下的变换矩阵^中的第/ 个向量。则正解的第一次矩阵相乘可以由式(11)表示
<formula>formula see original document page 11</formula>
根据图12，式(11)的计算可以用C0RDIC算法的基本模块1和一个加法器 來实现，如图2所示。
故，由6个C0RDIC算法基本模块组成的串行流水线结构可以有效地计算式 (11)，如图3所示。流水线的输出即为^的第/个向量，—化,UZ,，0(l)f 。
CLR信号是复用器MUX的输出选择信号。当CLR置高时，所有的复用器输出全部置0。
对于该机械臂模型的运动学正解计算，通过使用CORDIC算法的流水结 构，将常规解法的245次各种复杂的超越函数的计算变成如图3所示的36次 CORDIC计算操作，大大降低了运算复杂度和难度。
运动学反解的CORDIC算法
运动学反解的CORDIC计算的基本思想是将运动学反解解析解转变为坐标 旋转和正反切等基本运算。为了使运动学反解高速化，必须在复杂的反解解 析解式(4)至(式9)中找出与CORDIC算法基本模块输出共同的计算式，并 同iH解一样由CORDIC的算法基本模块组成流水线结构来实现反解解析式的计算。
观察六自由度空间机器人运动学反解解析解式式(4)至式(9)，可以看 出，在所有的解析表达式中，主要由旋转、反正切、开方、乘法等计算形式 构成。这些恰好可以用图12中基本模块1、 2、 6、 3来计算。以此可以得出所 有6个关节角的运动学反解解析解的C0RDIC算法。
六自由度机械臂的运动学反解解析解的计算也可以使用运动学正解求解 的流水结构来处理。下面对6个关节角的反解计算分别进行讨论，并给出其各 自的CORDIC流水线算法结构(参见图4至图9)。
(1) 关节角l的反解解析式为
由前述可以得出，其计算流程结构可以用图4表示。
《运动学反解的C0RDIC算法流水结构由一个基本模块2构成。该基本模块2
的X路输入P.,， Y路输入P.y， Z路输入0，该Z路的输出即为《。
(2) 关节角3的反解解析式为
<formula>formula see original document page 12</formula>
由前述可以得出，其计算流程结构可以用图5表示。
《运动学反解的C0RDIC算法流水结构包括四个基本模块3、 一个基本模块 2、 一个基本模块6。其中，第一基本模块3、第二基本模块3、第三基本模块3 顺序串联。第一基本模块3的X路和Z路输入P.,, Y路输入V(1);第二基本模块3的X路和Z路输入P^ Y路输入与第一基本模块3的Y路输出相连；第三基本模块 3的X路和Z路输入V (2) ， Y路输入与第二基本模块3的Y路输出相连。第三基 本模块3的Y路输出一路直接输入基本模块6的Y路输入端， 一路经V(4)输入第 四基本模块3的X路输入端。基本模块6的X路和Z路输入V(3)，其X路输出经V(5) 输入基本模块2的Y路输入端。第四基本模块3的Y路输入0， Z路输入V(6),其Y 路输出至基本模块2的X路输入。该基本模块2的Z路输入0，其Z路输出即为《。 3巾
<formula>formula see original document page 13</formula>
(3)关节角2的反解解析式为
《=咖-'(^—— A )
由前述可以得出，其计算流程结构可以用图6表示。
《运动学反解的C0RDIC算法流水结构包括八个基本模块3、 一个基本模块
1、两个基本模块2和一个基本模块6。其中，基本模块1的X路输入0， Y路输入
l'， Z路输入前述计算出的《。该基本模块l的输出分为两路，其X路输出依
次串联有第一至第四基本模块3和基本模块6，其Y路输出依次串联有第五
至第七基本模块3、第一基本模块2和第八基本模块3。最终由第四基本模
块3的X路输出至第二基本模块2的X路，第八基本模块3的Z路输出经V(12)
输出至第二基本模块2的Z路，第二基本模块2的Y路输入V(11)，该第二基
本模块2的Z路输出即为《。 g中 F(7) = l/^
F(8) = /2+/"3<formula>formula see original document page 14</formula>(4) 关节角6的反解解析式为
<formula>formula see original document page 14</formula>由前述可以得出，其计算流程结构可以用图7表示。
《运动学反解的C0RDIC算法流水结构包括四个基本模块1、两个基本模块3 和一个基本模块2。其中，该流水结构中包括两路并行的输入线路， 一路由第 一基本模块l、第一基本模块3、第二基本模块i顺序串联而成；另一路由第三 基本模块l、第二基本模块3、第四基本模块l顺序串联而成。该第二基本模块 1的X路输出至基本模块2的X路输入，第四基本模块1的X路输出至基本模块2的 Y路输入，该基本模块2的Z路输出即为-《。
其巾
<formula>formula see original document page 14</formula>(5) 关节角4的反解解析式为
<formula>formula see original document page 14</formula>由前述可以得出，其计算流程结构可以用图8表示。
《运动学反解的C0RDIC算法流水结构由顺序串联的第一基本模块1、基本 模块3、第二基本模块l、基本模块2构成。该第一基本模块1的X路输入《，Y 路输入^.， Z路输入前述计算出的《，其X路输出至基本模块3的X路，Y路输出 经V(15)至基本模块2的X路。基本模块3的Y路输入0， Z路输入V(7)，其Y路输 出至第二基本模块1的Y路。第二基本模块1的X路输入A, Z路输入《3，其Y路 输出至基本模块2的Y路，则基本模块2的Z路输出即为《。
其中<formula>formula see original document page 15</formula>
(6)关节角5的反解解析式为
<formula>formula see original document page 15</formula>
由前述可以得出，其计算流程结构可以用图9表示。
《运动学反解的C0RDIC算法流水结构包括三个基本模块1、两个基本模块3 和一个基本模块2。其中，该流水结构中包括两路并行的输入线路， 一路由第 一基本模块1和第一基本模块3串联而成，另一路由第二基本模块l和第二基本 模块3串联而成。第一基本模块3的Y路输出至第三基本模块1的X路，第二基本 模块3的Y路输出至第三基本模块1的Y路，该第三基本模块1的Y路输出至基本 模块2的X路。基本模块2的Y路输入前述V(14)， Z路输入0，其Z路输出即为-《。
其中<formula>formula see original document page 15</formula>
<formula>formula see original document page 15</formula>
至此，我们已经得到了六自由度空间机器人运动学正反解的全部 CORDIC计算方法和算法模型。在这些计算过程中，所有的超越函数的计 算我们都是用CORDIC算法来处理的。对于超越函数的计算， 一般的算法 是比较复杂和耗费计算资源的，但是使用CORDIC流水结构来计算这些有 大量超越函数组成的表达式却具有很好的效果。这些运算只需要加法和 移位操作就能实现，这两种操作在二进制硬件系统中的实现是快速和简 单的。
另外，由于C0RDIC算法的3个输入的计算是并行的，所以它的数 据处理都是以三路全并行的形式进行的，这就大大提高了处理速度，縮 短了计算时间。计算过程在空间机器人控制周期的占用上减少到了较低 的水平。
图10即为基于上述得出的运动学正反解CORDIC算法的协处理器 的基本结构图。该协处理器包括输入输出逻辑接口、算法运算模块。 其中，算法运算模块中包括运算参数寄存器、命令字寄存器、控制逻辑模块、状态字寄存器、输入FIFO、输出FIFO、运算处理模块。
该输入输出逻辑接口的输入信号分别输入运算参数寄存器、命令字
寄存器和输入FIFO。运算参数寄存器、命令字寄存器中所存参数输入控
制逻辑模块，再由控制逻辑模块控制状态字寄存器中的状态参数。该状 态字寄存器中所存状态参数输出至主处理器，以表明当前算法运算模块
的运算状态。由命令字寄存器、运算参数寄存器、控制逻辑模块、状态 字寄存器所形成的串联线路构成了算法运算模块的控制信息路径。
该输入FIFO、运算处理模块、输出FIFO顺序串联构成了算法运算 模块的数据信息路径。这三部分根据控制逻辑模块的控制命令进行相应 的运算操作。运算处理模块处理后的运算结果由输出FIF0输出。
下面对其中各个模块的作用作进一步说明
控制逻辑模块是算法运算模块中的控制核心，依据命令字寄存器、 状态字寄存器输入的参数确定运算处理模块所需使用的运动学正解和反 解的C0RDIC算法结构，并控制FIFO、运算处理模块、输出FIFO进行
相应操作。
运算处理模块包含有前述得出的各种运动学正反解C0RDIC算法流 水结构。该模块为整个协处理器的核心运算模块，可进行直接运动学正 反解计算，超越函数计算以及定点数的基本数学运算。该模块每次计算 使用哪条流水结构根据控制逻辑模块的控制指令决定。
运动参数寄存器中存储有机械手的结构参数，可由主处理器发出进 行参数的更新，满足不同构型机械手的需要。
命令字寄存器由主处理器发出可变更控制逻辑模块进行不同的控 制操作。该命令字寄存器是一个8位寄存器。
状态字寄存器用来记录算法运算模块的各阶段的运行状态，反映了 协处理器的当前状态。状态字寄存器为16位寄存器。高8位为一级状态 字，指示所进行的运算功能；低8位为二级状态字，指示所进行的某项 功能中的哪一步操作。
输入输出逻辑接口用以实现协处理器与主处理器的数据传送和控 制协调。
当主处理器需要协处理器完成运动学中的某项计算时，将该运算相 应的命令字发送至协处理器。协处理器将该命令字寄存在命令字寄存器 中供控制逻辑模块解析。控制逻辑模块完成对主处理器命令的解析后，将所对应的算法流程准备好，同时配置好运算参数。当运动学计算数据 压入输入FIF0后，控制逻辑模块控制数据进入算法流程进行计算。计算 过程中，数据计算进行到流程的哪一步，会将当前状态以状态字的形式 写入状态字寄存器，控制逻辑模块根据当前状态字来决定下一步的计算。
最终计算出的结果存入输出FIF0，完成此次计算。
图10所示的协处理器结构仅为一个基本结构。由于该协处理器使 用在特殊的太空环境下，对协处理器的抗干扰能力和容错能力都有着较 高的要求。因此，我们有必要再图IO所示结构的基础上进一步增加一定 的冗余和差错控制结构。
图11给出了具有三冗余结构设计的协处理器。该协处理具有三个 结构相同的算法运算模块。该协处理另外还设有三模输出比较器、错误
处理模块、配置更新模块。所述三个结构相同的算法运算模块与前述图 10中所示的算法运算模块的结构一致。三个算法运算模块并联于输入输 出逻辑接口和三模输出比较器之间。该三模输出比较器的输出端与错误 处理模块和输入输出逻辑接口相连。该错误处理模块的输出端通过输入
输出逻辑接口与主处理器相连。配置更新模块的输入端通过输入输出逻 辑接口与主处理器相连。配置更新模块的输出端与三个算法运算模块相连。
三模输出比较器主要对三个算法运算模块的输出数据进行三取二 的多数表决纠错，避免单粒子翻转的影响。
错误处理模块通过监控三模输出比较器，诊断出有故障的算法运算 模块，通知主处理器进行协处理器内部逻辑配置的修正和更新操作。
配置更新模块主要配合主处理器，对三个算法运算模块进行逻辑配 置的修正和更新操作。
权利要求
1. 六自由度机器人运动学CORDIC算法协处理器，用以协助主处理器完成数据计算，其特征在于包括输入输出逻辑接口、算法运算模块；所述算法运算模块中包括运算参数寄存器、命令字寄存器、控制逻辑模块、状态字寄存器、输入FIFO、输出FIFO、运算处理模块；所述输入输出逻辑接口分别与所述运算参数寄存器、命令字寄存器和输入FIFO相连；所述运算参数寄存器、命令字寄存器将所存参数输入至所述控制逻辑模块；所述控制逻辑模块控制所述状态字寄存器中的状态参数；该状态字寄存器与输入输出逻辑接口相连；所述输 FIFO、运算处理模块、输出FIFO顺序串联，并且受所述控制逻辑模块控制；所述输出FIFO与输入输出逻辑接口相连。
2、 如权利要求1所述的协处理器，其特征在于该协处理包括三 个结构相同的所述算法运算模块；该协处理还设有三模输出比较器、错 误处理模块、配置更新模块；所述三个的算法运算模块并联于所述输入输出逻辑接口和三模输 出比较器之间；该三模输出比较器的输出端与所述错误处理模块、输入 输出逻辑接口相连；该错误处理模块的输出端通过输入输出逻辑接口与 主处理器相连；该配置更新模块的输入端通过输入输出逻辑接口与主处 理器相连；该配置更新模块的输出端与所述三个算法运算模块相连。
3、 如权利要求1或2所述的协处理器，其特征在于所述运算处 理模块中存储有机器人运动学正解的CORDIC算法流水结构和六个关节 角度的运动学反解的CORDIC算法流水结构；所述机器人运动学正解的 CORDIC算法流水结构和六个关节角度的运动学反解的CORDIC算法流 水结构分别由若干CORDIC算法的基本模块构成；所述控制逻辑模块通 过控制运算处理模块选择进行不同的算法运算。
4、 如权利要求3所述的协处理器，其特征在于所述第一关节角 度的运动学反解的CORDIC算法流水结构由一个基本模块(2)构成。
5、 如权利要求3所述的协处理器，其特征在于所述第二关节角 度的运动学反解的CORDIC算法流水结构包括八个基本模块(3)、 一个基本模块(1)、 一个基本模块(2)和一个基本模块(6);所述基本模块(1) 的输出端分为并行的两路； 一路依次串联有第一至第四基本模块(3)和 基本模块(6);另一路依次串联有第五至第七基本模块(3)、第一基本 模块(2)和第八基本模块(3);所述基本模块(6)和第八基本模块(3) 的输出端分别连接至基本模块(2)的输入端，由基本模块(2)计算第 二关节角度。
6、 如权利要求3所述的协处理器，其特征在于所述第三关节角度的运动学反解的CORDIC算法流水结构包括四个基本模块(3)、 一个基 本模块(2)、 一个基本模块(6);所述第一基本模块(3)、第二基本模块(3)、 第三基本模块(3)串联；该第三基本模块(3)的输出端分别与所述基本模 块(6)和第四基本模块(3)的输入端相连；所述基本模块(6)和第四基本 模块3的输出端连接于基本模块(2)的输入端，由基本模块(2)计算第三 关节角度。
7、 如权利要求3所述的协处理器，其特征在于所述第四关节角 度的运动学反解的CORDIC算法流水结构由顺序串联的第一基本模块(l)、 基本模块(3)、第二基本模块(1)、基本模块(2)构成；由所述基本模块(2) 输出第四关节角度。
8、 如权利要求3所述的协处理器，其特征在于所述第五关节角 度的运动学反解的CORDIC算法流水结构包括三个基本模块(1)、两个基 本模块(3)和一个基本模块(2);所述第一基本模块(1)和第一基本模块(3)串联，所述第二基本模块(1)和第二基本模块(3)串联；该第一基本 模块(3)和第二基本模块(3)的输出端连接于第三基本模块(1)输入端； 所述第三基本模块(1)的输出信号输入基本模块(2)，由基本模块(2) 计算第五关节角度。
9、 如权利要求3所述的协处理器，其特征在于所述第六关节角 度的运动学反解的CORDIC算法流水结构包括四个基本模块(1)、两个基本模块(3)和一个基本模块(2);所述流水结构中包括两路并行的输入线路；一路由该第一基本模块(1)、第一基本模块(3)、第二基本模块(1)顺序串 联而成；另一路由该第三基本模块(1)、第二基本模块(3)、第四基本模(1) 顺序串联而成；该第二基本模块(1)和第四基本模块(1)的输出端与基本 模块(2)的输入端相连，由基本模块2计算第六关节角度。
全文摘要
本发明所设计的六自由度机器人运动学CORDIC算法的协处理器通过采用CORDIC算法替代了传统的通过计算大量超越方程才能完成机器人运动学正反解计算，使大量的超越方程计算过程简化为只需要通过加法、减法、和移位等这些硬件便于实现的操作，从而大大提高了协处理器的计算速度。
文档编号B25J9/16GK101286115SQ20081010644
公开日2008年10月15日 申请日期2008年5月13日 优先权日2008年5月13日
发明者史国振, 孙汉旭, 贾庆轩, 郑一力, 尧 陈 申请人:北京邮电大学