CN100587664C - 六自由度机器人运动学cordic算法协处理器 - Google Patents

Abstract

本发明所设计的六自由度机器人运动学CORDIC算法的协处理器通过采用CORDIC算法替代了传统的通过计算大量超越方程才能完成机器人运动学正反解计算，使大量的超越方程计算过程简化为只需要通过加法、减法、和移位等这些硬件便于实现的操作，从而大大提高了协处理器的计算速度。

Description

六自由度机器人运动学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算法流水结构包括四个基本模块(1)、两个基本模块(3)和一个基本模块(2)；所述流水结构中包括两路并行的输入线路；一路由该第一基本模块(1)、第一基本模块(3)、第二基本模块(1)顺序串联而成；另一路由该第三基本模块(1)、第二基本模块(3)、第四基本模(1)顺序串联而成；该第二基本模块(1)和第四基本模块(1)的输出端与基本模块(2)的输入端相连，由基本模块2计算第六关节角度。

本发明的有益效果是：本发明所设计的CORDIC算法的协处理器通过采用CORDIC算法替代了传统的通过计算大量超越方程完成机器人运动学正反解计算，使大量的超越方程计算过程简化为只需要通过加法、减法、和移位等这些硬件便于实现的操作，从而大大提高了协处理器的计算速度。

附图说明

图1为六自由度空间机械臂模型简图；

图2为第一次矩阵相乘的CORDIC算法；

图3为六自由度空间机械臂运动学正解的CORDIC计算流水线；

图4为关节角θ₁的CORDIC计算流程；

图5为关节角θ₃的CORDIC计算流程；

图6为关节角θ₂的CORDIC计算流程；

图7为关节角θ₆的CORDIC计算流程；

图8为关节角θ₄的CORDIC计算流程；

图9为关节角θ₅的CORDIC计算流程；

图10为运动学CORDIC算法的协处理器的基本结构图；

图11为三冗余结构的CORDIC算法的协处理器结构图；

图12为CORDIC算法基本模块图表。

具体实施方式

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

由前述可知，空间机器人不同于一般的地面机器人，由于处于太空空间这样一个特殊环境，对于系统的可靠性和稳定性都有了较高的要求。另一方面，空间机器人运动学正反解的过程中，包含很多超越函数的运算，并且在各个关节角的计算中很少包含相同的计算式，这对处理器的处理能力提出了较高的要求。基于这种情况，使用一种有效的求解析解的算法并开发与此算法紧密相关的专用协处理器来加速中央控制计算机的运动控制计算能力，成为了解决空间机器人运动控制计算需求和星载计算机性能瓶颈矛盾的较好途径。本发明即是基于这一设计思想，设计了一种专门针对空间机器人运动学正反解高速计算的协处理器。

六自由度机器人正反解

六自由度机器人是目前空间技术中比较常见的机械结构。下面即以六自由度空间机械臂为例，分析六自由度机器人运动学正反解的解析表达式。图1为六自由度空间机械臂模型简图。该六自由度机械臂由三个二自由度双关节P₁、P₂、P₃，两根碳纤维臂杆，一个手爪P_g和一个安装在手爪上的手眼视觉构成。其末端三个关节的轴线相交于一点P₃。该空间机械臂可以安装在空间平台上来完成某些空间任务，如抓取、维修、操作等。

这个六转动关节机械臂的运动学正解方程可以用六个关节角角度来表示。六个关节角角度θ_i从关节位置传感器反馈。

根据Denavit-Hartenberg方法，该机械臂末端手爪相对于基坐标系的位姿矩阵_n ⁰T可以由6个4×4的齐次变换矩阵_i ^i-1T顺序相乘得到，见式(1)。这里，_i ^i-1T是坐标系{i}相对于坐标系{i-1}的变换矩阵。

$T_T^0=T_6^0=T_1^{0}T_2^1...T_6^5$ 式(1)

通过手眼视觉可以得到目标体相对于手爪坐标系的位置，并且构成其4×4的齐次变换矩阵_G ^TT。

$T_G^T=\left(\begin{array}{cccc}n_x^T& o_x^T& a_x^T& p_x^T\\ n_y^T& o_y^T& a_y^T& p_x^T\\ n_z^T& o_z^T& a_z^T& p_x^T\\ 0& 0& 0& 1\end{array}\right)=\left(\begin{array}{cccc}V_1^T& V_2^T& V_3^T& V_4^T\end{array}\right)$ 式(2)

为了得到手爪捕获目标的笛卡儿轨迹上的期望节点，需要将_G ^TT变换到基坐标下的_G ⁰T：

$T_G^0=T_T^0\×T_G^T=(T_1^{0}T_2^1...T_6^5)\×T_G^T=$

式(3)

图1中，P_w是机械臂的腕关节，并且末端3个关节的轴线交于腕关节的中心，故机械臂末端的位姿可以分开求解。前3个关节用来决定其位置，后三个关节用来决定其姿势。

若机械臂末端定义在基座标下的位姿矩阵为_G ⁰T：

$T_G^0=\left(\begin{array}{cccc}{n}_x& o_x& a_x& p_x\\ n_y& o_y& a_y& p_y\\ n_z& o_z& a_z& p_z\\ 0& 0& 0& 1\end{array}\right)$

则腕关节位置坐标P_w＝(P_wx，P_wy，P_wz)可以由下式表示：

p_wx＝p_x-l₄a_x

p_wy＝p_y-l₄a_y

p_wz＝p_z-l₄a_z

各关节的转角用θ＝(θ₁，θ₂，…，θ₆)表示，若知道腕关节的位置坐标，则前3个关节的关节角可被求出。采用其次变换求得的解析解如下：

${\mathrm{\θ}}_1={\tan }^{-1}\left(\frac{p_{\mathrm{wy}}}{p_{\mathrm{wx}}}\right)$ 式(4)

${\mathrm{\θ}}_3={\tan }^{-1}\left(\frac{\sqrt{{\left({2l}_2{l}_3\right)}^2-{(p_{\mathrm{wx}}^2+p_{\mathrm{wy}}^2+{(p_{\mathrm{wz}}-l_1)}^2-(l_2^2+l_3^2))}^2}}{p_{\mathrm{wx}}^2+p_{\mathrm{wy}}^2+{(p_{\mathrm{wz}}-l_1)}^2-(l_2^2+l_3^2)}\right)$ 式(5)

${\mathrm{\θ}}_2={\tan }^{-1}\left(\frac{p_{\mathrm{wz}}-l_1}{\sqrt{{(l_2+l_3{s}_3)}^2+{\left(l_3{c}_3\right)}^2-{(p_{\mathrm{wz}}-l_1)}^2}}\right)-{\tan }^{-1}\left(\frac{l_3{c}_3}{l_2+l_3{s}_3}\right)$ 式(6)

得出前3个关节角后，后3个关节的关节角可作如下求解：

${\mathrm{\θ}}_6=-{\tan }^{-1}\left(\frac{s_{23}(c_1{o}_x+s_1{o}_y)+c_{23}{o}_z}{s_{23}(c_1{n}_x+s_1{n}_y)+c_{23}{n}_z}\right)$ 式(7)

${\mathrm{\θ}}_4={\tan }^{-1}\left(\frac{c_{23}(c_1{a}_x+s_1{a}_y)-s_{23}{a}_z}{-s_1{a}_x+c_1{a}_y}\right)$ 式(8)

${\mathrm{\θ}}_5=-{\tan }^{-1}\left(\frac{s_{23}(c_1{n}_x+s_1{n}_y)+c_{23}{n}_z}{c_4(c_{23}(c_1{n}_x+s_1{n}_y)+s_{23}{n}_z)-s_4(s_1{n}_x+c_1{n}_y)}\right)$ 式(9)

其中：sθ、cθ分别是sinθ、cosθ的缩写形式。s_n＝sinθ_n(n＝1，2，…，6)；

c_n＝cosθ_n(n＝1，2，…，6)；s₂₃＝sin(θ₂+θ₃)；c₂₃＝cos(θ₂+θ₃)。

从上面的求解式可以发现，在正反解得过程中，包含很多超越函数的运算，并且在各个关节角的计算中很少包含相同的计算式。因此，如何解决正反解中大量超越函数的快速计算问题，使用何种适合运动学算式及系统硬件特点的算法，是提高中央控制计算机运动学计算速度的关键问题。

CORDIC算法

CORDIC算法是一种坐标旋转数值计算方法，将任意坐标的某一角度的旋转移动通过离散地反复旋转直至收敛而得到，所用到的操作只有加法、减法、和移位。这些操作用硬件能够很快的实现，从而提高了计算速度。经典的CORDIC算法共有三种旋转系统，分别为：圆周(circular)系统、线性(linear)系统与双曲(hyperbolic)系统，算法使用了线性，圆周和双曲坐标下的二维向量旋转。

由于CORDIC算法的推到过程复杂，且为现代信号处理领域经常使用的成熟算法，在此仅给出其结论性的计算公式。三种旋转系统下，CORDIC算法的基本公式：

x_i+1＝x_i-μd_iy_i2^-i

y_i+1＝y_i+d_ix_i2^-i

z_i+1＝z_i-d_ie_i

圆周坐标系旋转：μ＝l，e_i＝tan^-12^-i

线性坐标系旋转：μ＝0，e_i＝2^-i

双曲坐标系旋转：μ＝-1，e_i＝tanh^-12^-i

在CRODIC算法的旋转和向量计算模式中，选择不同的迭代约束就可以得到如图12的CRODIC算法基本模块表。由图12我们可以知道，通过选择CRODIC算法的不同计算模式和旋转系统，我们可以通过CRODIC算法得到cosz、sinz、tan^-1y、乘法、除法、cosh z、sinh z、tanh^-1y等函数计算方法。按图12中标记所示，我们将CRODIC算法的六种不同计算模块称为基本模块1至基本模块6。每个基本模块都具有三路输入和对应的三路输出，我们将这三路分别称为x、y、z。

通过观察发现，六自由度机器人运动学正反解解析表达式中的代数式结构与CORDIC算法基本模块有很大相似度。故，可以将六自由度机器人运动学正反解解析表达式与CORDIC算法相结合，得到一种新的算法。而由于CORDIC算法所用到的操作只有加法、减法、和移位。这些操作用硬件能够很快的实现，从而提高了处理器计算速度。这是本发明的设计核心所在。

运动学正解的CORDIC算法

为了便于采用CORDIC算法，式(3)可以被改写成以下形式：

$T_G^0=(T_1^{0}T_2^1...T_6^5)\×T_G^T=(T_1^{0}T_2^1...T_5^4)\×T_6^{5}T_G^T=(T_1^{0}T_2^1...T_4^3)\×T_5^{4}T_G^5=...=T_1^0\×T_G^1$ 式(10)

这里，只需进行六次矩阵相乘就可以计算出_G ⁰T。

假设^TV_i＝(^TX_i，^TY_i，^TZ_i，0(1))^T为定义在手抓坐标系下的变换矩阵_G ^TT中的第i个向量。则正解的第一次矩阵相乘可以由式(11)表示：

$\left(\begin{array}{c}X_i^5\\ Y_i^5\\ Z_i^5\\ 1\end{array}\right)=\left(\begin{array}{cccc}{\mathrm{c\θ}}_6& -s{\mathrm{\θ}}_6& 0& 0\\ 0& 0& 1& l4\\ -{\mathrm{s\θ}}_6& -c{\mathrm{\θ}}_6& 0& 0\\ 0& 0& 0& 1\end{array}\right)\left(\begin{array}{c}X_i^T\\ Y_i^T\\ Z_i^T\\ 0\left(1\right)\end{array}\right)=\left(\begin{array}{c}{\mathrm{c\θ}}_{6}X_i^T-{\mathrm{s\θ}}_{6}Y_i^T\\ Z_i^T+0\left(l4\right)\\ -({\mathrm{c\θ}}_{6}Y_i^T+{\mathrm{s\θ}}_{6}X_i^T)\\ 0\left(1\right)\end{array}\right)$ 式(11)

根据图12，式(11)的计算可以用CORDIC算法的基本模块1和一个加法器来实现，如图2所示。

故，由6个CORDIC算法基本模块组成的串行流水线结构可以有效地计算式(11)，如图3所示。流水线的输出即为_G ⁰T的第i个向量⁰V_i＝(⁰X_i，⁰Y_i，⁰Z_i，0(1))^T。CLR信号是复用器MUX的输出选择信号。当CLR置高时，所有的复用器输出全部置0。

对于该机械臂模型的运动学正解计算，通过使用CORDIC算法的流水结构，将常规解法的245次各种复杂的超越函数的计算变成如图3所示的36次CORDIC计算操作，大大降低了运算复杂度和难度。

运动学反解的CORDIC算法

运动学反解的CORDIC计算的基本思想是将运动学反解解析解转变为坐标旋转和正反切等基本运算。为了使运动学反解高速化，必须在复杂的反解解析解式(4)至(式9)中找出与CORDIC算法基本模块输出共同的计算式，并同正解一样由CORDIC的算法基本模块组成流水线结构来实现反解解析式的计算。

观察六自由度空间机器人运动学反解解析解式式(4)至式(9)，可以看出，在所有的解析表达式中，主要由旋转、反正切、开方、乘法等计算形式构成。这些恰好可以用图12中基本模块1、2、6、3来计算。以此可以得出所有6个关节角的运动学反解解析解的CORDIC算法。

六自由度机械臂的运动学反解解析解的计算也可以使用运动学正解求解的流水结构来处理。下面对6个关节角的反解计算分别进行讨论，并给出其各自的CORDIC流水线算法结构(参见图4至图9)。

(1)关节角1的反解解析式为：

${\mathrm{\θ}}_1={\tan }^{-1}\left(\frac{p_{\mathrm{wy}}}{p_{\mathrm{wx}}}\right)$

由前述可以得出，其计算流程结构可以用图4表示。

θ₁运动学反解的CORDIC算法流水结构由一个基本模块2构成。该基本模块2的X路输入P_wx，Y路输入P_wy，Z路输入0，该Z路的输出即为θ₁。

(2)关节角3的反解解析式为：

${\mathrm{\θ}}_3={\tan }^{-1}\left(\frac{\sqrt{{\left({2l}_2{l}_3\right)}^2-{(p_{\mathrm{wx}}^2+p_{\mathrm{wy}}^2+{(p_{\mathrm{wz}}-l_1)}^2-(l_2^2+l_3^2))}^2}}{p_{\mathrm{wx}}^2+p_{\mathrm{wy}}^2+{(p_{\mathrm{wz}}-l_1)}^2-(l_2^2+l_3^2)}\right)$

由前述可以得出，其计算流程结构可以用图5表示。

θ₃运动学反解的CORDIC算法流水结构包括四个基本模块3、一个基本模块2、一个基本模块6。其中，第一基本模块3、第二基本模块3、第三基本模块3顺序串联。第一基本模块3的X路和Z路输入P_wx，Y路输入V(1)；第二基本模块3的X路和Z路输入P_wx，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路输出即为θ₃。

其中：

$V\left(1\right)=-(l_2^2+l_3^2)$

V(2)＝p_wz-l₁

V(3)＝2l₂l₃

$V\left(4\right)=p_{\mathrm{wx}}^2+p_{\mathrm{wy}}^2+{(p_{\mathrm{wz}}-l_1)}^2-(l_2^2+l_3^2)$

$V\left(5\right)=K^{*}\sqrt{{\left({2l}_2{l}_3\right)}^2-{(p_{\mathrm{wx}}^2+p_{\mathrm{wy}}^2+{(p_{\mathrm{wz}}-l_1)}^2-(l_2^2+l_3^2))}^2}$

V(6)＝K^*

(3)关节角2的反解解析式为：

${\mathrm{\θ}}_2={\tan }^{-1}\left(\frac{p_{\mathrm{wz}}-l_1}{\sqrt{{(l_2+l_3{s}_3)}^2+{\left(l_3{c}_3\right)}^2-{(p_{\mathrm{wz}}-l_1)}^2}}\right)-{\tan }^{-1}\left(\frac{l_3{c}_3}{l_2+l_3{s}_3}\right)$

由前述可以得出，其计算流程结构可以用图6表示。

θ₂运动学反解的CORDIC算法流水结构包括八个基本模块3、一个基本模块1、两个基本模块2和一个基本模块6。其中，基本模块1的X路输入0，Y路输入l₃，Z路输入前述计算出的θ₃。该基本模块1的输出分为两路，其X路输出依次串联有第一至第四基本模块3和基本模块6，其Y路输出依次串联有第五至第七基本模块3、第一基本模块2和第八基本模块3。最终由第四基本模块3的X路输出至第二基本模块2的X路，第八基本模块3的Z路输出经V(12)输出至第二基本模块2的Z路，第二基本模块2的Y路输入V(11)，该第二基本模块2的Z路输出即为θ₂。

其中：

V(7)＝1/K

V(8)＝l₂+l₃s₃

V(9)＝l₃c₃

V(10)＝K^*(p_wz-l₁)

V(11)＝p_wz-l₁

$V\left(12\right)=-{\tan }^{-1}\left(\frac{l_3{c}_3}{l_2+l_3{s}_3}\right)$

(4)关节角6的反解解析式为：

${\mathrm{\θ}}_6=-{\tan }^{-1}\left(\frac{s_{23}(c_1{o}_x+s_1{o}_y)+c_{23}{o}_z}{s_{23}(c_1{n}_x+s_1{n}_y)+c_{23}{n}_z}\right)$

由前述可以得出，其计算流程结构可以用图7表示。

θ6运动学反解的CORDIC算法流水结构包括四个基本模块1、两个基本模块3和一个基本模块2。其中，该流水结构中包括两路并行的输入线路，一路由第一基本模块1、第一基本模块3、第二基本模块1顺序串联而成；另一路由第三基本模块1、第二基本模块3、第四基本模块1顺序串联而成。该第二基本模块1的X路输出至基本模块2的X路输入，第四基本模块1的X路输出至基本模块2的Y路输入，该基本模块2的Z路输出即为-θ₆。

其中：

V(7)＝1/K

V(13)＝c₁n_x+s₁n_y

V(14)＝K(s₂₃(c₁n_x+s₁n_y)+c₂₃n_z)

(5)关节角4的反解解析式为：

${\mathrm{\θ}}_4={\tan }^{-1}\left(\frac{c_{23}(c_1{a}_x+s_1{a}_y)-s_{23}{a}_z}{-s_1{a}_x+c_1{a}_y}\right)$

由前述可以得出，其计算流程结构可以用图8表示。

θ₄运动学反解的CORDIC算法流水结构由顺序串联的第一基本模块1、基本模块3、第二基本模块1、基本模块2构成。该第一基本模块1的X路输入a_x，Y路输入a_y，Z路输入前述计算出的θ₁，其X路输出至基本模块3的X路，Y路输出经V(15)至基本模块2的X路。基本模块3的Y路输入0，Z路输入V(7)，其Y路输出至第二基本模块1的Y路。第二基本模块1的X路输入a_z，Z路输入θ₂₃，其Y路输出至基本模块2的Y路，则基本模块2的Z路输出即为θ₄。

其中：

V(15)＝K(-s₁a_x+c₁a_y)

(6)关节角5的反解解析式为：

${\mathrm{\θ}}_5=-{\tan }^{-1}\left(\frac{s_{23}(c_1{n}_x+s_1{n}_y)+c_{23}{n}_z}{c_4(c_{23}(c_1{n}_x+s_1{n}_y)+s_{23}{n}_z)-s_4(s_1{n}_x+c_1{n}_y)}\right)$

由前述可以得出，其计算流程结构可以用图9表示。

θ₅运动学反解的CORDIC算法流水结构包括三个基本模块1、两个基本模块3和一个基本模块2。其中，该流水结构中包括两路并行的输入线路，一路由第一基本模块1和第一基本模块3串联而成，另一路由第二基本模块1和第二基本模块3串联而成。第一基本模块3的Y路输出至第三基本模块1的X路，第二基本模块3的Y路输出至第三基本模块1的Y路，该第三基本模块1的Y路输出至基本模块2的X路。基本模块2的Y路输入前述V(14)，Z路输入0，其Z路输出即为-θ₅。

其中：

V(7)＝1/K

V(13)＝c₁n_x+s₁n_y

V(14)＝K(s₂₃(c₁n_x+s₁n_y)+c₂₃n_z)

V(15)＝s₁n_x+c₁n_y

至此，我们已经得到了六自由度空间机器人运动学正反解的全部CORDIC计算方法和算法模型。在这些计算过程中，所有的超越函数的计算我们都是用CORDIC算法来处理的。对于超越函数的计算，一般的算法是比较复杂和耗费计算资源的，但是使用CORDIC流水结构来计算这些有大量超越函数组成的表达式却具有很好的效果。这些运算只需要加法和移位操作就能实现，这两种操作在二进制硬件系统中的实现是快速和简单的。

另外，由于CORDIC算法的3个输入的计算是并行的，所以它的数据处理都是以三路全并行的形式进行的，这就大大提高了处理速度，缩短了计算时间。计算过程在空间机器人控制周期的占用上减少到了较低的水平。

图10即为基于上述得出的运动学正反解CORDIC算法的协处理器的基本结构图。该协处理器包括：输入输出逻辑接口、算法运算模块。其中，算法运算模块中包括：运算参数寄存器、命令字寄存器、控制逻辑模块、状态字寄存器、输入FIFO、输出FIFO、运算处理模块。

该输入输出逻辑接口的输入信号分别输入运算参数寄存器、命令字寄存器和输入FIFO。运算参数寄存器、命令字寄存器中所存参数输入控制逻辑模块，再由控制逻辑模块控制状态字寄存器中的状态参数。该状态字寄存器中所存状态参数输出至主处理器，以表明当前算法运算模块的运算状态。由命令字寄存器、运算参数寄存器、控制逻辑模块、状态字寄存器所形成的串联线路构成了算法运算模块的控制信息路径。

该输入FIFO、运算处理模块、输出FIFO顺序串联构成了算法运算模块的数据信息路径。这三部分根据控制逻辑模块的控制命令进行相应的运算操作。运算处理模块处理后的运算结果由输出FIFO输出。

下面对其中各个模块的作用作进一步说明：

控制逻辑模块是算法运算模块中的控制核心，依据命令字寄存器、状态字寄存器输入的参数确定运算处理模块所需使用的运动学正解和反解的CORDIC算法结构，并控制FIFO、运算处理模块、输出FIFO进行相应操作。

运算处理模块包含有前述得出的各种运动学正反解CORDIC算法流水结构。该模块为整个协处理器的核心运算模块，可进行直接运动学正反解计算，超越函数计算以及定点数的基本数学运算。该模块每次计算使用哪条流水结构根据控制逻辑模块的控制指令决定。

运动参数寄存器中存储有机械手的结构参数，可由主处理器发出进行参数的更新，满足不同构型机械手的需要。

命令字寄存器由主处理器发出可变更控制逻辑模块进行不同的控制操作。该命令字寄存器是一个8位寄存器。

状态字寄存器用来记录算法运算模块的各阶段的运行状态，反映了协处理器的当前状态。状态字寄存器为16位寄存器。高8位为一级状态字，指示所进行的运算功能；低8位为二级状态字，指示所进行的某项功能中的哪一步操作。

输入输出逻辑接口用以实现协处理器与主处理器的数据传送和控制协调。

当主处理器需要协处理器完成运动学中的某项计算时，将该运算相应的命令字发送至协处理器。协处理器将该命令字寄存在命令字寄存器中供控制逻辑模块解析。控制逻辑模块完成对主处理器命令的解析后，将所对应的算法流程准备好，同时配置好运算参数。当运动学计算数据压入输入FIFO后，控制逻辑模块控制数据进入算法流程进行计算。计算过程中，数据计算进行到流程的哪一步，会将当前状态以状态字的形式写入状态字寄存器，控制逻辑模块根据当前状态字来决定下一步的计算。最终计算出的结果存入输出FIFO，完成此次计算。

图10所示的协处理器结构仅为一个基本结构。由于该协处理器使用在特殊的太空环境下，对协处理器的抗干扰能力和容错能力都有着较高的要求。因此，我们有必要再图10所示结构的基础上进一步增加一定的冗余和差错控制结构。

图11给出了具有三冗余结构设计的协处理器。该协处理具有三个结构相同的算法运算模块。该协处理另外还设有三模输出比较器、错误处理模块、配置更新模块。所述三个结构相同的算法运算模块与前述图10中所示的算法运算模块的结构一致。三个算法运算模块并联于输入输出逻辑接口和三模输出比较器之间。该三模输出比较器的输出端与错误处理模块和输入输出逻辑接口相连。该错误处理模块的输出端通过输入输出逻辑接口与主处理器相连。配置更新模块的输入端通过输入输出逻辑接口与主处理器相连。配置更新模块的输出端与三个算法运算模块相连。

三模输出比较器主要对三个算法运算模块的输出数据进行三取二的多数表决纠错，避免单粒子翻转的影响。

错误处理模块通过监控三模输出比较器，诊断出有故障的算法运算模块，通知主处理器进行协处理器内部逻辑配置的修正和更新操作。

配置更新模块主要配合主处理器，对三个算法运算模块进行逻辑配置的修正和更新操作。

Claims (9)

1、一种六自由度机器人运动学CORDIC算法协处理器，用以协助主处理器完成数据计算，其特征在于：包括输入输出逻辑接口、算法运算模块；

所述算法运算模块中包括：运算参数寄存器、命令字寄存器、控制逻辑模块、状态字寄存器、输入FIFO、输出FIFO、运算处理模块；

所述输入输出逻辑接口分别与所述运算参数寄存器、命令字寄存器和输入FIFO相连；所述运算参数寄存器、命令字寄存器将所存参数输入至所述控制逻辑模块；所述控制逻辑模块控制所述状态字寄存器中的状态参数；该状态字寄存器与输入输出逻辑接口相连；

所述输入FIFO、运算处理模块、输出FIFO顺序串联，并且受所述控制逻辑模块控制；所述输出FIFO与输入输出逻辑接口相连；

所述控制逻辑模块，用于解析寄存于命令字寄存器中的主处理器发送的命令字，将对应的运算处理模块所需使用的运动学正解和反解的CORDIC算法流水结构准备好，配置好运算参数，当运动学计算数据压入输入FIF0后，控制逻辑模块控制数据进入CORDIC算法流水结构进行计算，根据状态字寄存器的当前状态字来决定所述运算处理模块下一步的计算，并将计算出的结果存入输出FIFO；

所述运算处理模块，用于存储机器人运动学正解的CORDIC算法流水结构和六个关节角度的运动学反解的CORDIC算法流水结构，以及进行直接运动学正反解计算、超越函数计算和定点数的基本数学运算，其中，所述机器人运动学正解的CORDIC算法流水结构和六个关节角度的运动学反解的CORDIC算法流水结构分别由若干个CORDIC算法的基本模块构成，所述运算处理模块每次计算使用哪条流水结构根据所述控制逻辑模块的控制指令决定；

所述运算参数寄存器，用于寄存机械手的结构参数；

所述命令字寄存器，用于寄存由主处理器发出变更控制逻辑模块进行不同的控制操作的命令字；

所述状态字寄存器，用于寄存所述运算处理模块的各阶段的运行状态，描述协处理器的当前状态；

所述输入输出逻辑接口，用于实现协处理器与主处理器的数据传送和控制协调。

2、如权利要求1所述的协处理器，其特征在于：该协处理器包括三个结构相同的所述算法运算模块；该协处理器还设有三模输出比较器、错误处理模块、配置更新模块；

所述三个算法运算模块并联于所述输入输出逻辑接口和三模输出比较器之间；该三模输出比较器的输出端与所述错误处理模块、输入输出逻辑接口相连；该错误处理模块的输出端通过输入输出逻辑接口与主处理器相连；该配置更新模块的输入端通过输入输出逻辑接口与主处理器相连；该配置更新模块的输出端与所述三个算法运算模块相连。

3、如权利要求1所述的协处理器，其特征在于：所述CORDIC算法的基本模块都具有三路输入和对应的三路输出，三路输入、输出分别为x、y、z，其中基本模块1计算cosz和sinz，基本模块2计算tan^-1y，基本模块3计算乘法，基本模块4计算除法，基本模块5计算cosh z和sinh z，基本模块6计算tanh^-1y。

4、如权利要求3所述的协处理器，其特征在于：第一关节角度的运动学反解的CORDIC算法流水结构由一个基本模块2构成，用于计算第一关节角度

${\mathrm{\θ}}_1={\tan }^{-1}\left(\frac{p_{\mathrm{wy}}}{p_{\mathrm{wx}}}\right),$

其中，Pwx、Pwy分别为机械臂的腕关节的X、Y轴位置坐标。

5、如权利要求3所述的协处理器，其特征在于：第二关节角度的运动学反解的CORDIC算法流水结构包括八个基本模块3、一个基本模块1、二个基本模块2和一个基本模块6；所述基本模块1的输出端分为并行的两路；一路依次串联有第一至第四基本模块3和基本模块6；另一路依次串联有第五至第七基本模块3、第一基本模块2和第八基本模块3；所述基本模块6和第八基本模块3的输出端分别连接至第二基本模块2的输入端，用于计算第二关节角度

${\mathrm{\θ}}_2={\tan }^{-1}\left(\frac{p_{\mathrm{wz}}-l_1}{\sqrt{{(l_2+l_3{s}_3)}^2+{\left(l_3{c}_3\right)}^2-{(p_{\mathrm{wz}}-l_1)}^2}}\right)-{\tan }^{-1}\left(\frac{l_3{c}_{23}}{l_2+l_3{s}_3}\right),$

其中，P_WZ为机械臂的腕关节的Z轴位置坐标，c_n＝cosθ_n(n＝1，2，…，6)，s_n＝sinθ_n(n＝1，2，…，6)，θ_n(n＝1，2，...，6)为第n关节角度，sθ、cθ分别是sinθ、cosθ的缩写形式，l_n(n＝1，2，3)取自于目标体相对于手爪坐标系位置的4×4的齐次变换矩阵_G ^TT所变换成的基坐标下的矩阵_G ⁰T：

$T=T_T^0\×T_G^T_G^0=(T_1^{0}T_2^1\·\·\·T_6^5)\×T_G^T=$

$\left(\begin{array}{cccc}c{\mathrm{\θ}}_1& -s{\mathrm{\θ}}_1& 0& 0\\ s{\mathrm{\θ}}_1& c{\mathrm{\θ}}_1& 0& 0\\ 0& 0& 1& -l1\\ 0& 0& 0& 1\end{array}\right)\left(\begin{array}{cccc}c{\mathrm{\θ}}_2& -s{\mathrm{\θ}}_2& 0& 0\\ 0& 0& 1& 0\\ -s{\mathrm{\θ}}_2& -c{\mathrm{\θ}}_2& 0& 0\\ 0& 0& 0& 1\end{array}\right)\left(\begin{array}{cccc}c{\mathrm{\θ}}_3& -s{\mathrm{\θ}}_3& 0& -l2\\ s{\mathrm{\θ}}_3& c{\mathrm{\θ}}_3& 0& 0\\ 0& 0& -1& 0\\ 0& 0& 0& 1\end{array}\right)$

$\begin{array}{c}\left(\begin{array}{cccc}c{\mathrm{\θ}}_4& -s{\mathrm{\θ}}_4& 0& 0\\ 0& 0& 1& l3\\ -s{\mathrm{\θ}}_4& -c{\mathrm{\θ}}_4& 0& 0\\ 0& 0& 0& 1\end{array}\right)\left(\begin{array}{cccc}c{\mathrm{\θ}}_5& -s{\mathrm{\θ}}_5& 0& 0\\ 0& 0& -1& 0\\ s{\mathrm{\θ}}_5& c{\mathrm{\θ}}_5& 0& 0\\ 0& 0& 0& 1\end{array}\right)\left(\begin{array}{cccc}c{\mathrm{\θ}}_6& -s{\mathrm{\θ}}_6& 0& 0\\ 0& 0& 1& l4\\ -s{\mathrm{\θ}}_6& -c{\mathrm{\θ}}_6& 0& 0\\ 0& 0& 0& 1\end{array}\right)\\ \begin{array}{cccc}V_1^T& V_2^T& V_3^T& V_4^T\end{array}\end{array}$

6、如权利要求3所述的协处理器，其特征在于：第三关节角度的运动学反解的CORDIC算法流水结构包括四个基本模块3、一个基本模块2、一个基本模块6；所述第一基本模块3、第二基本模块3、第三基本模块3串联；该第三基本模块3的输出端分别与所述基本模块6和第四基本模块3的输入端相连：所述基本模块6和第四基本模块3的输出端连接于基本模块2的输入端，用于计算第三关节角度

${\mathrm{\θ}}_3={\tan }^{-1}\left(\frac{\sqrt{{\left(2l_2{l}_3\right)}^2-{(p_{\mathrm{wx}}^2+p_{\mathrm{xy}}^2+{(p_{\mathrm{wz}}-l_1)}^2-(l_2^2+l_3^2))}^2}}{p_{\mathrm{wx}}^2+p_{\mathrm{wy}}^2+{(p_{\mathrm{wz}}-l_1)}^2+(l_2^2+l_3^2)}\right),$

其中，P_WX、P_WY、P_WZ分别为机械臂的腕关节的X、Y、Z轴位置坐标，sθ、cθ分别是sinθ、cosθ的缩写形式，l_n(n＝1，2，3)取自于目标体相对于手爪坐标系位置的4×4的齐次变换矩阵_G ^TT所变换成的基坐标下的矩阵_G ⁰T：

$T=T_T^0\×T_G^T_G^0=(T_1^{0}T_2^1\·\·\·T_6^5)\×T_G^T=$

$\left(\begin{array}{cccc}c{\mathrm{\θ}}_1& -s{\mathrm{\θ}}_1& 0& 0\\ s{\mathrm{\θ}}_1& c{\mathrm{\θ}}_1& 0& 0\\ 0& 0& 1& -l1\\ 0& 0& 0& 1\end{array}\right)\left(\begin{array}{cccc}c{\mathrm{\θ}}_2& -s{\mathrm{\θ}}_2& 0& 0\\ 0& 0& 1& 0\\ -s{\mathrm{\θ}}_2& -c{\mathrm{\θ}}_2& 0& 0\\ 0& 0& 0& 1\end{array}\right)\left(\begin{array}{cccc}c{\mathrm{\θ}}_3& -s{\mathrm{\θ}}_3& 0& -l2\\ s{\mathrm{\θ}}_3& c{\mathrm{\θ}}_3& 0& 0\\ 0& 0& -1& 0\\ 0& 0& 0& 1\end{array}\right)$

$\begin{array}{c}\left(\begin{array}{cccc}c{\mathrm{\θ}}_4& -s{\mathrm{\θ}}_4& 0& 0\\ 0& 0& 1& l3\\ -s{\mathrm{\θ}}_4& -c{\mathrm{\θ}}_4& 0& 0\\ 0& 0& 0& 1\end{array}\right)\left(\begin{array}{cccc}c{\mathrm{\θ}}_5& -s{\mathrm{\θ}}_5& 0& 0\\ 0& 0& -1& 0\\ s{\mathrm{\θ}}_5& c{\mathrm{\θ}}_5& 0& 0\\ 0& 0& 0& 1\end{array}\right)\left(\begin{array}{cccc}c{\mathrm{\θ}}_6& -s{\mathrm{\θ}}_6& 0& 0\\ 0& 0& 1& l4\\ -s{\mathrm{\θ}}_6& -c{\mathrm{\θ}}_6& 0& 0\\ 0& 0& 0& 1\end{array}\right)\\ \begin{array}{cccc}V_1^T& V_2^T& V_3^T& V_4^T\end{array}\end{array}$

7、如权利要求3所述的协处理器，其特征在于：第四关节角度的运动学反解的CORDIC算法流水结构由顺序串联的第一基本模块1、基本模块3、第二基本模块1、基本模块2构成，用于计算第四关节角度

${\mathrm{\θ}}_4={\tan }^{-1}\left(\frac{c_{23}(c_1{a}_x+s_1{a}_y)+s_{23}{a}_z}{-s_1{a}_x+c_1{a}_y}\right),$

其中，c₂₃＝cos(θ₂+θ₃)，c_n＝cosθ_n(n＝1，2，…，6)，s_n＝sinθ_n(n＝1，2，…，6)，s₂₃＝sin(θ₂+θ₃)，θ_n(n＝1，2，...，6)为第n关节角度，a_x，a_y，a_z取自于机械臂末端定义在基座标下的位姿矩阵_G ⁰T：

$T_G^0=\left(\begin{array}{cccc}{n}_x& o_x& a_x& p_x\\ n_y& o_y& a_y& p_y\\ n_z& o_z& a_z& p_z\\ 0& 0& 0& 1\end{array}\right).$

8、如权利要求3所述的协处理器，其特征在于：第五关节角度的运动学反解的CORDIC算法流水结构包括三个基本模块1、两个基本模块3和一个基本模块2；所述第一基本模块1和第一基本模块3串联，所述第二基本模块1和第二基本模块3串联；该第一基本模块3和第二基本模块3的输出端连接于第三基本模块1输入端；所述第三基本模块1的输出信号输入基本模块2，用于计算第五关节角度

${\mathrm{\θ}}_5=-{\tan }^{-1}\left(\frac{s_{23}(c_1{n}_x+s_1{n}_y)+c_{23}{n}_z}{c_4(c_{23}(c_1{n}_x+s_1{n}_y)+s_{23}{n}_z)-s_4(s_1{n}_x+c_1{n}_y)}\right),$

其中，c₂₃＝cos(θ₂+θ₃)，c_n＝cosθ_n(n＝1，2，…，6)，s_n＝sinθ_n(n＝1，2，…，6)，s₂₃＝sin(θ₂+θ₃)，θ_n(n＝1，2，...，6)为第n关节角度，n_x，n_y，n_z取自于机械臂末端定义在基座标下的位姿矩阵_G ⁰T：

$T_G^0=\left(\begin{array}{cccc}{n}_x& o_x& a_x& p_x\\ n_y& o_y& a_y& p_y\\ n_z& o_z& a_z& p_z\\ 0& 0& 0& 1\end{array}\right).$

9、如权利要求3所述的协处理器，其特征在于：第六关节角度的运动学反解的CORDIC算法流水结构包括四个基本模块1、两个基本模块3和一个基本模块2；所述第六关节角度的流水结构中包括两路并行的输入线路；一路由该第一基本模块1、第一基本模块3、第二基本模块1顺序串联而成；另一路由该第三基本模块1、第二基本模块3、第四基本模块1顺序串联而成；该第二基本模块1和第四基本模块1的输出端与基本模块2的输入端相连，用于计算第六关节角度

${\mathrm{\θ}}_6=-{\tan }^{-1}\left(\frac{s_{23}(c_1{o}_x+s_1{o}_y)+c_{23}{o}_z}{s_{23}(c_1{n}_x+s_1{n}_y)+c_{23}{n}_z}\right),$

其中，c₂₃＝cos(θ₂+θ₃)，c_n＝cosθ_n(n＝1，2，…，6)，s_n＝sinθ_n(n＝1，2，…，6)，s₂₃＝sin(θ₂+θ₃)，θ_n(n＝1，2，...，6)为第n关节角度，n_x，n_y，n_z，o_x，o_y，o_z取自于机械臂末端定义在基座标下的位姿矩阵_G ⁰T：

$T_G^0=\left(\begin{array}{cccc}{n}_x& o_x& a_x& p_x\\ n_y& o_y& a_y& p_y\\ n_z& o_z& a_z& p_z\\ 0& 0& 0& 1\end{array}\right).$

Images