CN103499922A - 一种基于fpga的七自由度空间机械臂运动学实时解算方法 - Google Patents

Abstract

本发明公开了一种基于FPGA的七自由度空间机械臂运动学实时解算方法。该运动学解算方法分为五个功能模块，分别是求解正余弦模块、正解模块、求解雅克比矩阵模块、求逆模块以及求解关节角速度模块。由正余弦模块计算出机械臂各关节角度的正余弦值，输入给正解模块求解齐次变换矩阵，然后在雅克比矩阵模块中，通过齐次变换矩阵计算相对于末端坐标系的雅克比矩阵的每一列，经过计算可以得到相对于基坐标系的雅克比矩阵和一个满秩方阵，输出满秩方阵给求逆模块进行求逆，当求逆运算完成以后输出给求解关节角速度模块与相对于基坐标系的雅克比矩阵的转置相乘，得到雅克比矩阵的伪逆，最后乘上末端速度得到各个关节的角速度，完成整个运动学解算。

Description

一种基于FPGA的七自由度空间机械臂运动学实时解算方法

技术领域

本发明涉及机器人技术领域，特别涉及一种基于FPGA的七自由度空间机械臂运动学实时解算方法。 

背景技术

在机器人控制技术领域，机器人的控制问题一直是控制领域众多学者的研究课题，在机器人控制系统上，尝试过几乎所有由现代控制理论所提供的方法。 

随着空间技术的不断发展，空间机械臂在人类探索空间活动中发挥着不可替代的作用。空间机械臂可以代替宇航员执行大量的空间在轨作业任务，如大型空间站的建造，维护及服务，卫星的回收、空间设备的维修以及星球的表面探索等，大大降低了宇航员的舱外作业风险，因此空间机械臂技术的发展受到国内外广泛的重视。具有冗余度的空间机械臂，可以利用在任务中满足一些附加条件，来获取更佳的得运动学和动力学性能，但是冗余度空间机械臂的运动控制中要进行复杂的运动学解算，在轨操作过程当中又需要满足很高的实时性要求，因此为了保证在轨操作任务的顺利完成，必须对空间机械臂运动学解算方法展开深入的研究。 

然而空间机械臂的主要工作环境在太空，太空环境极端恶劣。与地面表现出很大不同，主要包括：高辐射，高低温快速交变，高速微尘碰撞等等。传统的航天器控制系统中，多采用具有抗辐照加固能力的宇航级CPU，系统任务被转换为指令顺序执行，因而解算能力主要由频率决定，但是太空环境限制了系统只能在较低频率下运行，同时已发表的文献中具有强大解算能力的宇航级CPU鲜有耳闻。 

发明内容

本发明的目的是为了克服上述已有技术的不足，提出一种基于FPGA的七自由度空间机械臂运动学实时解算方法，该方法全部采用硬件描述语言实现，大幅度提升了机械臂运动学解算的速度，能够满足空间机械臂运动控制的实时性要求。 

本发明采用如下技术方案： 

一种基于FPGA的七自由度空间机械臂运动学实时解算方法，包括求解正余弦模块、正解模块、求解雅克比矩阵模块、求逆模块以及求解关节角速度模块。由正余弦模块计算出机械臂各关节角度的正余弦值，输入给正解模块求解各齐次变换矩阵，然后在雅克比矩阵模块中，通过齐次变换矩阵计算相对于末端坐标系的雅克比矩阵的每一列，经过计算可以得到相对于基坐标系的雅克比矩阵和一个满秩方阵，输出满秩方阵给求逆模块进行求逆，当求逆运算完成以后输出给求解关节角速度模块与相对于基坐标系的雅克比矩阵的转置相乘，得到雅克比矩阵的伪逆，最后乘上末端速度得到各个关节的角速度，实现整个运动学解算，所有模块中 的数据均采用定点数表示。 

上述空间机械臂运动学实时解算方法中，所述求解正余弦模块主要包含CORDIC核，多路选择器，计数器和逻辑控制单元。CORDIC核负责计算每个关节角度的正余弦值。多路选择器的作用是依据控制单元的指令，选择输入给CORDIC核的关节角，顺序依次为关节7、关节6、关节5、节4、关节3、关节2和关节1，每当输出一个关节角正余弦值并收到应答信号，计数器的值减一，控制单元依据计数器的数值使多路选择器更新当前关节角。控制单元由有限状态机构成，负责整个模块的运行调度。 

上述空间机械臂运动学实时解算方法中，所述正解模块串行接收各个关节角的正余弦值，依次计算各个关节的齐次变换矩阵并输出到求解雅克比矩阵模块，主要由一个多路选择器，一个四阶方阵相乘子模块和逻辑控制单元构成。四阶方阵相乘子模块由乘累加单元(MAC)构成，将矩阵相乘化运算简化为乘加运算，即可实现基本模块，在基本模块上进行了封装，添加一些复位，使能等控制信号，组成四阶方阵相乘子模块。四阶方阵相乘子模块得到结果后，输出齐次矩阵给求解雅克比矩阵模块，同时输出到自身模块的输入端，等待与下一组齐次矩阵进行相乘计算。 

上述空间机械臂运动学实时解算方法中，所述求解雅克比矩阵模块，将七个关节的齐次变换矩阵中前六个进行运算得到相对于末端坐标系的雅克比矩阵的每一列，第七个齐次变换矩阵转换成末端到基坐标的旋转矩阵，通过旋转矩阵乘上相对于末端坐标系的雅克比矩阵得到相对于基坐标系的雅克比矩阵，然后用相对于基坐标系的雅克比矩阵乘上自身的转置，得到一个6×6的满秩方阵。该模块包含有矩阵相乘子模块和向量相乘子模块，两个子模块同样是乘累加单元(MAC)基本模块构成，向量相乘子模块用于计算相对于末端坐标系的雅克比矩阵每一列，矩阵相乘用于计算旋转矩阵乘上相对于末端坐标系的雅克比矩阵和相对于基坐标系的雅克比矩阵乘上自身的转置。 

上述空间机械臂运动学实时解算方法中，所述求逆模块采用分块矩阵求逆算法，将6×6的满秩方阵划分为四块3×3方阵，通过进行矩阵相乘和矩阵求逆运算，依次求出逆矩阵的四块。该模块包括：三阶矩阵相乘子模块、三阶矩阵求逆子模块和逻辑控制单元。三阶矩阵相乘子模块由乘累加单元(MAC)基本模块构成。三阶矩阵求逆子模块由除法器和乘累加单元(MAC)基本模块构成。控制单元由有限状态机构成，负责调度矩阵求逆子模块和矩阵相乘子模块，依次计算逆矩阵的四块。 

上述空间机械臂运动学实时解算方法中，所述求解关节角速度模块用乘累加模块(MAC)构成一个7×6×6×1的矩阵-向量相乘子模块，将矩阵相乘运算化简为矩阵-向量相乘运算，求 雅克比矩阵伪逆运算和求关节角度运算复用该子模块，通过有限状态机进行调度，先将雅克比矩阵转置和方阵的逆矩阵相乘得到雅克比矩阵伪逆，然后再将雅克比矩阵伪逆乘上末端速度得到七个关节角速度，最后输出结果。 

本发明相对于现有技术如下优点及技术效果： 

1、与传统的串行指令操作相比，本发明采用硬件描述语言实现的并行计算方式保证了运算的快速性和可靠性，提高数据处理速度，满足运动控制的实时性要求。 

2、本发明针对冗余度机械臂运动学解算搭建的处理结构，各个模块分工合作，可以在不改变各自模块的硬件结构下，在此平台上完成不同自由度的机械臂运动学解算，这种结构具有很好的移植性和扩展性。 

3、本发明采用分块矩阵求逆算法求解雅克比矩阵的伪逆，相比其它的方式极大的简化了求逆运算的复杂度。 

4、本发明是基于FPGA芯片的，FPGA芯片具有体积小、能耗低、设计灵活等优点，能够进行重配置，这种设计可以降低成本，缩短开发周期。 

附图说明

图1是本发明提出的机械臂运动学实时解算方法系统框图。 

图2是本发明提出的求解正余弦模块结构框图。 

图3是本发明提出的正解模块结构框图。 

图4是本发明提出的四阶方阵相乘子模块结构框图。 

图5是本发明提出的求解雅克比矩阵模块结构框图。 

图6是本发明提出的矩阵求逆模块结构框图。 

图7是本发明提出的求解关节角速度模块结构框图。 

具体实施方式

以下结合参考附图更详细的说明了本发明的具体实施方式。 

如图1所示为本发明所述的空间机械臂运动学实时解算方法，包括求解正余弦模块、正解模块、求解雅克比矩阵模块、求逆模块以及求解关节角速度模块。由正余弦模块计算出机械臂各关节角度的正余弦值，输入给正解模块求解各齐次变换矩阵，然后在雅克比矩阵模块中，通过齐次变换矩阵计算相对于末端坐标系的雅克比矩阵的每一列，经过计算可以得到相对于基坐标系的雅克比矩阵和一个满秩方阵，输出满秩方阵给求逆模块进行求逆，当求逆运算完成以后输出给求解关节角速度模块与相对于基坐标系的雅克比矩阵的转置相乘，得到雅克比矩阵的伪逆，最后乘上末端速度得到各个关节的角速度，实现整个运动学解算，所有模块中的数据均采用定点数表示。其中整个模块的输入数据是机械臂七个关节角度和规划出的 末端速度，输出数据是末端的齐次变换矩阵和七个关节角速度。 

如图2所示为本发明所述的求解正余弦模块，主要包含CORDIC核，多路选择器，计数器和逻辑控制单元。CORDIC采用圆周系统下的旋转模式，负责计算每个关节角度的正余弦值。多路选择器的作用是依据控制单元的指令，选择输入给CORDIC核的关节角，顺序依次为关节7、关节6、关节5、节4、关节3、关节2和关节1，每当输出一个关节角正余弦值并收到应答信号，计数器的值减一，控制单元依据计数器的数值使多路选择器更新当前关节角。控制单元由有限状态机构成，负责整个模块的运行调度，当七个关节角的正余弦值全部运算完毕，状态机回到初始状态，等待进行下一轮运动学解算。 

如图3所示为本发明所述的正解模块，该模块串行接收各个关节角的正余弦值，依次计算各个关节的齐次变换矩阵并输出到求解雅克比矩阵模块，主要由一个多路选择器，一个四阶方阵相乘子模块和逻辑控制单元构成。四阶方阵相乘子模块由乘累加单元(MAC)构成，将矩阵相乘化运算简化为乘加运算，即可实现基本模块，在基本模块上进行了封装，添加一些复位，使能等控制信号，组成四阶方阵相乘子模块。四阶方阵相乘子模块得到结果后，输出齐次矩阵给求解雅克比矩阵模块，同时输出到自身模块的输入端，等待与下一组齐次矩阵进行相乘计算。 

如图4所示为本发明所述的四阶方阵相乘子模块。该模块由四个乘累加单元构成，先输入a矩阵的第一行和b矩阵的1、2、3、4列，得到c矩阵的第一行。经过四次循环计算，最终输出c矩阵。 

在七自由度空间机械臂上建立坐标系，则末端位姿的齐次变换矩阵T可写成： 

T^0，8＝T^0，1·T^1，2·T^2，3·T^3，4·T^4，5·T^5，6·T^6，7·T^7，8

其中，T^i-1，i表示两相邻坐标系之间的齐次变换矩阵，表达式为： 

$T^{i-1,i}=\left[\begin{array}{cc}{R}^{i-1,i}& p^{i-1,i}\\ \begin{array}{ccc}0& 0& 0\end{array}& 1\end{array}\right]$

p^i-1，i，R^i-1，i表示两相邻坐标系间的平移及旋转变化。在计算过程齐次变换矩阵T^0，8的过程中，p^i-1，i作为已知输入，R^i-1，i根据输入的关节角的角度值代入公式计算得到，各齐次变换矩阵的表达式如下： 

$T^{\mathrm{0,1}}=\left[\begin{array}{cccc}0& 0& -1& -a0\\ -\cos {\mathrm{\θ}}_1& {\sin \mathrm{\θ}}_1& 0& 0\\ {\sin \mathrm{\θ}}_1& {\cos \mathrm{\θ}}_1& 0& 0\\ 0& 0& 0& 1\end{array}\right]$

$T^{\mathrm{1,2}}=\left[\begin{array}{cccc}-{\sin \mathrm{\θ}}_2& -\cos {\mathrm{\θ}}_2& 0& 0\\ 0& 0& -1& -a1\\ {\cos \mathrm{\θ}}_2& -{\sin \mathrm{\θ}}_2& 0& 0\\ 0& 0& 0& 1\end{array}\right]$

$T^{3,4}=\left[\begin{array}{cccc}{\cos \mathrm{\θ}}_4& -\sin {\mathrm{\θ}}_4& 0& a3\\ {\sin \mathrm{\θ}}_4& {\cos \mathrm{\θ}}_4& 0& 0\\ 0& 0& 1& a4\\ 0& 0& 0& 1\end{array}\right]$

$T^{4,5}=\left[\begin{array}{cccc}{\cos \mathrm{\θ}}_5& -\sin {\mathrm{\θ}}_5& 0& a5\\ {\sin \mathrm{\θ}}_5& {\cos \mathrm{\θ}}_5& 0& 0\\ 0& 0& 1& a6\\ 0& 0& 0& 1\end{array}\right]$

$T^{5,6}=\left[\begin{array}{cccc}{\sin \mathrm{\θ}}_6& \cos {\mathrm{\θ}}_6& 0& 0\\ 0& 0& -1& -a7\\ -{\cos \mathrm{\θ}}_6& {\sin \mathrm{\θ}}_6& 0& 0\\ 0& 0& 0& 1\end{array}\right]$

$T^{6,7}=\left[\begin{array}{cccc}{\cos \mathrm{\θ}}_7& -\sin {\mathrm{\θ}}_7& 0& 0\\ 0& 0& 1& a8\\ -{\sin \mathrm{\θ}}_7& -{\cos \mathrm{\θ}}_7& 0& 0\\ 0& 0& 0& 1\end{array}\right]$

$T^{\mathrm{7,8}}=\left[\begin{array}{cccc}0& -1& 0& 0\\ 0& 0& -1& 0\\ 1& 0& 0& 0\\ 0& 0& 0& 1\end{array}\right]$

机械臂的末端位姿可由矩阵T^0，8求解，p^0，８表示平移变化，分别对应x，y，z的坐标值，R^0，8表示旋转变化，可以求解末端的姿态，得到姿态角α，β，γ。 

如图5所示为本发明所述的求解雅克比矩阵模块，该模块将七个关节的齐次变换矩阵中前六个进行运算得到相对于末端坐标系的雅克比矩阵的每一列，第七个齐次变换矩阵转换成末端到基坐标的旋转矩阵，通过旋转矩阵乘上相对于末端坐标系的雅克比矩阵得到相对于基坐标系的雅克比矩阵，然后用相对于基坐标系的雅克比矩阵乘上自身的转置，得到一个6×6的满秩方阵。该模块包含有矩阵相乘子模块和向量相乘子模块，两个子模块同样是乘累加单元(MAC)基本模块构成，向量相乘子模块用于计算相对于末端坐标系的雅克比矩阵每一列，矩阵相乘用于计算旋转矩阵乘上相对于末端坐标系的雅克比矩阵和相对于基坐标系的雅克比矩阵乘上自身的转置。 

利用微分变换求解相对于末端坐标系的雅克比矩阵^TJ： 

$J_T=\left[\begin{array}{ccccccc}J_1^T& J_2^T& J_3^T& J_4^T& J_5^T& J_6^T& J_7^T\end{array}\right]$

根据机器人学可知，相对于末端坐标系的雅克比矩阵^TJ的第i列可按下式计算，即： 

$J_i^T=\left[\begin{array}{c}-n_x{p}_y+n_y{p}_x\\ -o_x{p}_y+o_y{p}_x\\ -a_x{p}_y+a_y{p}_x\\ n_z\\ o_z\\ a_z\end{array}\right]$

其中，n，o，a，p等元素可由相应的齐次变换矩阵T得到，对应关系如下所示： 

$T=\left[\begin{array}{cccc}{n}_x& o_x& a_x& p_x\\ n_y& o_y& a_y& p_x\\ n_z& o_z& a_z& p_x\\ 0& 0& 0& 1\end{array}\right]$

相对于末端坐标系的雅可比矩阵各列与各齐次变换矩阵的的对应关系如下： 

$J_1^T\⇔T^{\mathrm{1,8}}=T^{\mathrm{1,2}}\·T^{\mathrm{2,3}}\·T^{\mathrm{3,4}}\·T^{\mathrm{4,5}}\·T^{\mathrm{5,6}}\·T^{\mathrm{6,7}}\·T^{\mathrm{7,8}}$

$J_2^T\⇔T^{\mathrm{2,8}}=T^{\mathrm{2,3}}\·T^{\mathrm{3,4}}\·T^{\mathrm{4,5}}\·T^{\mathrm{5,6}}\·T^{\mathrm{6,7}}\·T^{\mathrm{7,8}}$

$J_3^T\⇔T^{\mathrm{3,8}}=T^{\mathrm{3,4}}\·T^{\mathrm{4,5}}\·T^{\mathrm{5,6}}\·T^{\mathrm{6,7}}\·T^{\mathrm{7,8}}$

$J_4^T\⇔T^{\mathrm{4,8}}=T^{\mathrm{4,5}}\·T^{\mathrm{5,6}}\·T^{\mathrm{6,7}}\·T^{\mathrm{7,8}}$

$J_5^T\⇔T^{\mathrm{5,8}}=T^{\mathrm{5,6}}\·T^{\mathrm{6,7}}\·T^{\mathrm{7,8}}$

$J_6^T\⇔T^{\mathrm{6,8}}=T^{\mathrm{6,7}}\·T^{\mathrm{7,8}}$

$J_7^T\⇔T^{\mathrm{7,8}}$

通过微分变换法求的的雅克比矩阵是相对于末端坐标系的，通过乘上旋转矩阵求出相对于基坐标的雅克比矩阵。 

$J={\left[\begin{array}{cc}{R}^{\mathrm{0,8}}& 0\\ 0& R^{\mathrm{0,8}}\end{array}\right]}^{-1}J_T$

求解雅可比矩阵的伪逆J⁺公式如下： 

J⁺=J^T(JJ^T)^-1

如图6所示为本发明所述的求逆模块，该模块采用分块矩阵求逆算法，将6×6的满秩方阵划分为四块3×3方阵，通过进行矩阵相乘和矩阵求逆运算，依次求出逆矩阵的四块。该模块包括：三阶矩阵相乘子模块、三阶矩阵求逆子模块和逻辑控制单元。三阶矩阵相乘子模块由乘累加单元(MAc)基本模块构成。三阶矩阵求逆子模块由除法器和乘累加单元(MAc)基本模块构成。控制单元由有限状态机构成，负责调度矩阵求逆子模块和矩阵相乘子模块，依次计算逆矩阵的四块。 

在非奇异条件下，JJ^T是一个6×6满秩方阵，常用的矩阵求逆方法多采用QR分解，本文 采用分块矩阵求逆的方法来计算(JJ^T)^-1。设n阶可逆矩阵： 

$R=\left[\begin{array}{cc}{A}_1& A_2\\ A_3& A_4\end{array}\right]$

其中A₁，A₂分别是k，n-k阶可逆矩阵，设： 

$R^{-1}=\left[\begin{array}{cc}E& F\\ G& H\end{array}\right]$

根据逆矩阵的定义RR^-1=I，有： 

$\begin{array}{c}\left[\begin{array}{cc}{A}_1& A_2\\ A_3& A_4\end{array}\right]\left[\begin{array}{cc}E& F\\ G& H\end{array}\right]=\\ \left[\begin{array}{cc}{A}_{1}E+A_{2}G& A_{1}F+A_{2}H\\ A_{3}E+A_{4}G& A_{3}F+A_{4}H\end{array}\right]=\left[\begin{array}{cc}{I}_k& 0\\ 0& I_{n-k}\end{array}\right]\end{array}$

解方程组可以得到： 

$R^{-1}=\left[\begin{array}{cc}{(A_1-A_2{A}_4^{-1}{A}_3)}^{-1}& {-A}_1^{-1}{A}_2{(A_4-A_3{A}_1^{-1}{A}_2)}^{-1}\\ {-A}_4^{-1}{A}_3{(A_1-A_2{A}_4^{-1}{A}_3)}^{-1}& {(A_4-A_3{A}_1^{-1}{A}_2)}^{-1}\end{array}\right]$

由于JJ^T矩阵是6×6的方阵所以可以划分为4个3×3的矩阵，而3×3的矩阵求逆有公式可以直接套用，避免了常用方法如QR分解时，繁琐的迭代计算。但是分块矩阵求逆必须要求A₁，A₄矩阵可逆，这种方法适用范围小，并且当矩阵维数增加时，分块后的矩阵依然很大，这种方法不再适用。然而冗余机械臂的JJ^T总是6阶方阵，当雅克比矩阵非奇异时，显然有xJJ^Tx＞0，所以JJ^T是正定矩阵，设： 

$x=\left[\begin{array}{c}{x}_1\\ 0\end{array}\right],{\mathrm{JJ}}^T=A=\left[\begin{array}{cc}{A}_1& A_2\\ A_3& A_4\end{array}\right]$

$x^T\mathrm{Ax}=\left[\begin{array}{cc}{x_1}^T& 0\end{array}\right]\left[\begin{array}{cc}{A}_1& A_3\\ A_2& A_4\end{array}\right]\left[\begin{array}{c}{x}_1\\ 0\end{array}\right]={x_1}^T{A}_1{x}_1>0$

所以A₁正定，即可逆，同理推出A₄可逆，满足分块矩阵求逆条件，所以可以利用分块矩阵求逆计算(JJ^T)^-1，再通过矩阵相乘从而求得J⁺，证明了分块矩阵求逆适用于空间机械臂的运动学解算。 

如图7所示为本发明所述的求解关节角速度模块，该模块用乘累加模块(MAc)构成一个7×6×6×1的矩阵一向量相乘子模块，将矩阵相乘运算化简为矩阵一向量相乘运算，求雅克比矩阵伪逆运算和求关节角度运算复用该子模块，通过有限状态机进行调度，先将雅克比矩阵转置和方阵的逆矩阵相乘，通过子模块依次算出

得到雅克比矩阵伪逆J⁺，然后再将雅 克比矩阵伪逆乘上末端速度v得到七个关节角速度，最后输出结果，求解关节角速度公式如下所示： 

$\stackrel{\·}{\mathrm{\θ}}=J^{+}v=J^T{\left({\mathrm{JJ}}^T\right)}^{-1}v$ 。

Claims (6)

1.一种基于FPGA的七自由度空间机械臂运动学实时解算方法，其特征在于，包括求解正余弦模块、正解模块、求解雅克比矩阵模块、求逆模块以及求解关节角速度模块。由正余弦模块计算出机械臂各关节角度的正余弦值，输入给正解模块求解各齐次变换矩阵，然后在雅克比矩阵模块中，通过齐次变换矩阵计算相对于末端坐标系的雅克比矩阵的每一列，经过计算可以得到相对于基坐标系的雅克比矩阵和一个满秩方阵，输出满秩方阵给求逆模块进行求逆，当求逆运算完成以后输出给求解关节角速度模块与相对于基坐标系的雅克比矩阵的转置相乘，得到雅克比矩阵的伪逆，最后乘上末端速度得到各个关节的角速度，实现整个运动学解算，所有模块中的数据均采用定点数表示。 

2.依据权利要求1所述的空间机械臂运动学实时解算方法，其特征在于，求解正余弦模块主要包含CORDIC核，多路选择器，计数器和逻辑控制单元。CORDIC核负责计算每个关节角度的正余弦值。多路选择器的作用是依据控制单元的指令，选择输入给CORDIC核的关节角，顺序依次为关节7、关节6、关节5、节4、关节3、关节2和关节1，每当输出一个关节角正余弦值并收到应答信号，计数器的值减一，控制单元依据计数器的数值使多路选择器更新当前关节角。控制单元由有限状态机构成，负责整个模块的运行调度。 

3.依据权利要求1所述的空间机械臂运动学实时解算方法，其特征在于，正解模块串行接收各个关节角的正余弦值，依次计算各个关节的齐次变换矩阵并输出到求解雅克比矩阵模块，主要由一个多路选择器，一个四阶方阵相乘子模块和逻辑控制单元构成。四阶方阵相乘子模块由乘累加单元(MAC)构成，将矩阵相乘化运算简化为乘加运算，即可实现基本模块，在基本模块上进行了封装，添加一些复位，使能等控制信号，组成四阶方阵相乘子模块。四阶方阵相乘子模块得到结果后，输出齐次矩阵给求解雅克比矩阵模块，同时输出到自身模块的输入端，等待与下一组齐次矩阵进行相乘计算。 

4.依据权利要求1所述的空间机械臂运动学实时解算方法，其特征在于，求解雅克比矩阵模块，将七个关节的齐次变换矩阵中前六个进行运算得到相对于末端坐标系的雅克比矩阵的每一列，第七个齐次变换矩阵转换成末端到基坐标的旋转矩阵，通过旋转矩阵乘上相对于末端坐标系的雅克比矩阵得到相对于基坐标系的雅克比矩阵，然后用相对于基坐标系的雅克比矩阵乘上自身的转置，得到一个6×6的满秩方阵。该模块包含有矩阵相乘子模块和向量相乘子模块，两个子模块同样是乘累加单元(MAC)基本模块构成，向量相乘子模块用于计算相对于末端坐标系的雅克比矩阵每一列，矩阵相乘用于计算旋转矩阵乘上相对于末端坐标系的雅克比矩阵和相对于基坐标系的雅克比矩阵乘上自身的转置。 

5.依据权利要求1所述的空间机械臂运动学实时解算方法，其特征在于，求逆模块采用分块 矩阵求逆算法，将6×6的满秩方阵划分为四块3×3方阵，通过进行矩阵相乘和矩阵求逆运算，依次求出逆矩阵的四块。该模块包括：三阶矩阵相乘子模块、三阶矩阵求逆子模块和逻辑控制单元。三阶矩阵相乘子模块由乘累加单元(MAC)基本模块构成。三阶矩阵求逆子模块由除法器和乘累加单元(MAC)基本模块构成。控制单元由有限状态机构成，负责调度矩阵求逆子模块和矩阵相乘子模块，依次计算逆矩阵的四块。 

6.依据权利要求1所述的空间机械臂运动学实时解算方法，其特征在于，求解关节角速度模块用乘累加模块(MAC)构成一个7×6×6×1的矩阵-向量相乘子模块，将矩阵相乘运算化简为矩阵-向量相乘运算，求雅克比矩阵伪逆运算和求关节角度运算复用该子模块，通过有限状态机进行调度，先将雅克比矩阵转置和方阵的逆矩阵相乘得到雅克比矩阵伪逆，然后再将雅克比矩阵伪逆乘上末端速度得到七个关节角速度，最后输出结果。 

Images