一种基于fpga的七自由度空间机械臂运动学实时解算方法
【专利摘要】本发明公开了一种基于FPGA的七自由度空间机械臂运动学实时解算方法。该运动学解算方法分为五个功能模块，分别是求解正余弦模块、正解模块、求解雅克比矩阵模块、求逆模块以及求解关节角速度模块。由正余弦模块计算出机械臂各关节角度的正余弦值，输入给正解模块求解齐次变换矩阵，然后在雅克比矩阵模块中，通过齐次变换矩阵计算相对于末端坐标系的雅克比矩阵的每一列，经过计算可以得到相对于基坐标系的雅克比矩阵和一个满秩方阵，输出满秩方阵给求逆模块进行求逆，当求逆运算完成以后输出给求解关节角速度模块与相对于基坐标系的雅克比矩阵的转置相乘，得到雅克比矩阵的伪逆，最后乘上末端速度得到各个关节的角速度，完成整个运动学解算。
【专利说明】—种基于FPGA的七自由度空间机械臂运动学实时解算方法
【技术领域】
[0001]本发明涉及机器人【技术领域】，特别涉及一种基于FPGA的七自由度空间机械臂运动学实时解算方法。
【背景技术】
[0002]在机器人控制【技术领域】，机器人的控制问题一直是控制领域众多学者的研究课题，在机器人控制系统上，尝试过几乎所有由现代控制理论所提供的方法。
[0003]随着空间技术的不断发展，空间机械臂在人类探索空间活动中发挥着不可替代的作用。空间机械臂可以代替宇航员执行大量的空间在轨作业任务，如大型空间站的建造，维护及服务，卫星的回收、空间设备的维修以及星球的表面探索等，大大降低了宇航员的舱外作业风险，因此空间机械臂技术的发展受到国内外广泛的重视。具有冗余度的空间机械臂，可以利用在任务中满足一些附加条件，来获取更佳的得运动学和动力学性能，但是冗余度空间机械臂的运动控制中要进行复杂的运动学解算，在轨操作过程当中又需要满足很高的实时性要求，因此为了保证在轨操作任务的顺利完成，必须对空间机械臂运动学解算方法展开深入的研究。
[0004]然而空间机械臂的主要工作环境在太空，太空环境极端恶劣。与地面表现出很大不同，主要包括:高辐射，高低温快速交变，高速微尘碰撞等等。传统的航天器控制系统中，多采用具有抗辐照加固能力的宇航级CPU，系统任务被转换为指令顺序执行，因而解算能力主要由频率决定，但是太空环境限制了系统只能在较低频率下运行，同时已发表的文献中具有强大解算能力的宇航级CPU鲜有耳闻。
【发明内容】
[0005]本发明的目的是为了克服上述已有技术的不足，提出一种基于FPGA的七自由度空间机械臂运动学实时解算方法，该方法全部采用硬件描述语言实现，大幅度提升了机械臂运动学解算的速度，能够满足空间机械臂运动控制的实时性要求。
[0006]本发明采用如下技术方案:
[0007]一种基于FPGA的七自由度空间机械臂运动学实时解算方法，包括求解正余弦模块、正解模块、求解雅克比矩阵模块、求逆模块以及求解关节角速度模块。由正余弦模块计算出机械臂各关节角度的正余弦值，输入给正解模块求解各齐次变换矩阵，然后在雅克比矩阵模块中，通过齐次变换矩阵计算相对于末端坐标系的雅克比矩阵的每一列，经过计算可以得到相对于基坐标系的雅克比矩阵和一个满秩方阵，输出满秩方阵给求逆模块进行求逆，当求逆运算完成以后输出给求解关节角速度模块与相对于基坐标系的雅克比矩阵的转置相乘，得到雅克比矩阵的伪逆，最后乘上末端速度得到各个关节的角速度，实现整个运动学解算，所有模块中的数据均采用定点数表示。
[0008]上述空间机械臂运动学实时解算方法中，所述求解正余弦模块主要包含CORDIC核，多路选择器，计数器和逻辑控制单元。CORDIC核负责计算每个关节角度的正余弦值。多路选择器的作用是依据控制单元的指令，选择输入给CORDIC核的关节角，顺序依次为关节
7、关节6、关节5、节4、关节3、关节2和关节1，每当输出一个关节角正余弦值并收到应答信号，计数器的值减一，控制单元依据计数器的数值使多路选择器更新当前关节角。控制单元由有限状态机构成，负责整个模块的运行调度。
[0009]上述空间机械臂运动学实时解算方法中，所述正解模块串行接收各个关节角的正余弦值，依次计算各个关节的齐次变换矩阵并输出到求解雅克比矩阵模块，主要由一个多路选择器，一个四阶方阵相乘子模块和逻辑控制单元构成。四阶方阵相乘子模块由乘累加单元(MAC)构成，将矩阵相乘化运算简化为乘加运算，即可实现基本模块，在基本模块上进行了封装，添加一些复位，使能等控制信号，组成四阶方阵相乘子模块。四阶方阵相乘子模块得到结果后，输出齐次矩阵给求解雅克比矩阵模块，同时输出到自身模块的输入端，等待与下一组齐次矩阵进行相乘计算。
[0010]上述空间机械臂运动学实时解算方法中，所述求解雅克比矩阵模块，将七个关节的齐次变换矩阵中前六个进行运算得到相对于末端坐标系的雅克比矩阵的每一列，第七个齐次变换矩阵转换成末端到基坐标的旋转矩阵，通过旋转矩阵乘上相对于末端坐标系的雅克比矩阵得到相对于基坐标系的雅克比矩阵，然后用相对于基坐标系的雅克比矩阵乘上自身的转置，得到一个6X6的满秩方阵。该模块包含有矩阵相乘子模块和向量相乘子模块，两个子模块同样是乘累加单元(MAC)基本模块构成，向量相乘子模块用于计算相对于末端坐标系的雅克比矩阵每一列，矩阵相乘用于计算旋转矩阵乘上相对于末端坐标系的雅克比矩阵和相对于基坐标系的雅克比矩阵乘上自身的转置。
[0011]上述空间机械臂运动学实时解算方法中，所述求逆模块采用分块矩阵求逆算法，将6 X 6的满秩方阵划分为四块3 X 3方阵，通过进行矩阵相乘和矩阵求逆运算，依次求出逆矩阵的四块。该模块包括:三阶矩阵相乘子模块、三阶矩阵求逆子模块和逻辑控制单元。三阶矩阵相乘子模块由乘累加单元(MAC)基本模块构成。三阶矩阵求逆子模块由除法器和乘累加单元(MAC)基本模块构成。控制单元由有限状态机构成，负责调度矩阵求逆子模块和矩阵相乘子模块，依次计算逆矩阵的四块。
[0012]上述空间机械臂运动学实时解算方法中，所述求解关节角速度模块用乘累加模块(MAC)构成一个7X6X6X I的矩阵-向量相乘子模块，将矩阵相乘运算化简为矩阵-向量相乘运算，求雅克比矩阵伪逆运算和求关节角度运算复用该子模块，通过有限状态机进行调度，先将雅克比矩阵转置和方阵的逆矩阵相乘得到雅克比矩阵伪逆，然后再将雅克比矩阵伪逆乘上末端速度得到七个关节角速度，最后输出结果。
[0013]本发明相对于现有技术如下优点及技术效果:
[0014]1、与传统的串行指令操作相比，本发明采用硬件描述语言实现的并行计算方式保证了运算的快速性和可靠性，提高数据处理速度，满足运动控制的实时性要求。
[0015]2、本发明针对冗余度机械臂运动学解算搭建的处理结构，各个模块分工合作，可以在不改变各自模块的硬件结构下，在此平台上完成不同自由度的机械臂运动学解算，这种结构具有很好的移植性和扩展性。
[0016]3、本发明采用分块矩阵求逆算法求解雅克比矩阵的伪逆，相比其它的方式极大的简化了求逆运算的复杂度。[0017]4、本发明是基于FPGA芯片的，FPGA芯片具有体积小、能耗低、设计灵活等优点，能够进行重配置，这种设计可以降低成本，缩短开发周期。
【专利附图】
【附图说明】
[0018]图1是本发明提出的机械臂运动学实时解算方法系统框图。
[0019]图2是本发明提出的求解正余弦模块结构框图。
[0020]图3是本发明提出的正解模块结构框图。
[0021]图4是本发明提出的四阶方阵相乘子模块结构框图。
[0022]图5是本发明提出的求解雅克比矩阵模块结构框图。
[0023]图6是本发明提出的矩阵求逆模块结构框图。
[0024]图7是本发明提出的求解关节角速度模块结构框图。
【具体实施方式】
[0025]以下结合参考附图更详细的说明了本发明的【具体实施方式】。
[0026]如图1所示为本发明所述的空间机械臂运动学实时解算方法，包括求解正余弦模块、正解模块、求解雅克比矩阵模块、求逆模块以及求解关节角速度模块。由正余弦模块计算出机械臂各关节角度的正余弦值，输入给正解模块求解各齐次变换矩阵，然后在雅克比矩阵模块中，通过齐次变换矩阵计算相对于末端坐标系的雅克比矩阵的每一列，经过计算可以得到相对于基坐标系的雅克比矩阵和一个满秩方阵，输出满秩方阵给求逆模块进行求逆，当求逆运算完成以后输出给求解关节角速度模块与相对于基坐标系的雅克比矩阵的转置相乘，得到雅克比矩阵的伪逆，最后乘上末端速度得到各个关节的角速度，实现整个运动学解算，所有模块中的数据均采用定点数表示。其中整个模块的输入数据是机械臂七个关节角度和规划出的末端速度，输出数据是末端的齐次变换矩阵和七个关节角速度。
[0027]如图2所示为本发明所述的求解正余弦模块，主要包含CORDIC核，多路选择器，计数器和逻辑控制单元。CORDIC采用圆周系统下的旋转模式，负责计算每个关节角度的正余弦值。多路选择器的作用是依据控制单元的指令，选择输入给CORDIC核的关节角，顺序依次为关节7、关节6、关节5、节4、关节3、关节2和关节1，每当输出一个关节角正余弦值并收到应答信号，计数器的值减一，控制单元依据计数器的数值使多路选择器更新当前关节角。控制单元由有限状态机构成，负责整个模块的运行调度，当七个关节角的正余弦值全部运算完毕，状态机回到初始状态，等待进行下一轮运动学解算。
[0028]如图3所示为本发明所述的正解模块，该模块串行接收各个关节角的正余弦值，依次计算各个关节的齐次变换矩阵并输出到求解雅克比矩阵模块，主要由一个多路选择器，一个四阶方阵相乘子模块和逻辑控制单元构成。四阶方阵相乘子模块由乘累加单元(MAC)构成，将矩阵相乘化运算简化为乘加运算，即可实现基本模块，在基本模块上进行了封装，添加一些复位，使能等控制信号，组成四阶方阵相乘子模块。四阶方阵相乘子模块得到结果后，输出齐次矩阵给求解雅克比矩阵模块，同时输出到自身模块的输入端，等待与下一组齐次矩阵进行相乘计算。
[0029]如图4所示为本发明所述的四阶方阵相乘子模块。该模块由四个乘累加单元构成，先输入a矩阵的第一行和b矩阵的1、2、3、4列，得到c矩阵的第一行。经过四次循环计算，最终输出C矩阵。
[0030]在七自由度空间机械臂上建立坐标系，则末端位姿的齐次变换矩阵T可写成:
【权利要求】
1.一种基于FPGA的七自由度空间机械臂运动学实时解算方法，其特征在于，包括求解正余弦模块、正解模块、求解雅克比矩阵模块、求逆模块以及求解关节角速度模块。由正余弦模块计算出机械臂各关节角度的正余弦值，输入给正解模块求解各齐次变换矩阵，然后在雅克比矩阵模块中，通过齐次变换矩阵计算相对于末端坐标系的雅克比矩阵的每一列，经过计算可以得到相对于基坐标系的雅克比矩阵和一个满秩方阵，输出满秩方阵给求逆模块进行求逆，当求逆运算完成以后输出给求解关节角速度模块与相对于基坐标系的雅克比矩阵的转置相乘，得到雅克比矩阵的伪逆，最后乘上末端速度得到各个关节的角速度，实现整个运动学解算，所有模块中的数据均采用定点数表示。
2.依据权利要求1所述的空间机械臂运动学实时解算方法，其特征在于，求解正余弦模块主要包含CORDIC核，多路选择器，计数器和逻辑控制单元。CORDIC核负责计算每个关节角度的正余弦值。多路选择器的作用是依据控制单元的指令，选择输入给CORDIC核的关节角，顺序依次为关节7、关节6、关节5、节4、关节3、关节2和关节1，每当输出一个关节角正余弦值并收到应答信号，计数器的值减一，控制单元依据计数器的数值使多路选择器更新当前关节角。控制单元由有限状态机构成，负责整个模块的运行调度。
3.依据权利要求1所述的空间机械臂运动学实时解算方法，其特征在于，正解模块串行接收各个关节角的正余弦值，依次计算各个关节的齐次变换矩阵并输出到求解雅克比矩阵模块，主要由一个多路选择器，一个四阶方阵相乘子模块和逻辑控制单元构成。四阶方阵相乘子模块由乘累加单元(MAC)构成，将矩阵相乘化运算简化为乘加运算，即可实现基本模块，在基本模块上进行了封装，添加一些复位，使能等控制信号，组成四阶方阵相乘子模块。四阶方阵相乘子模块得到结果后，输出齐次矩阵给求解雅克比矩阵模块，同时输出到自身模块的输入端，等待与下一组齐次矩阵进行相乘计算。
4.依据权利要求1所述的空间机械臂运动学实时解算方法，其特征在于，求解雅克比矩阵模块，将七个关节的齐次变换矩阵中前六个进行运算得到相对于末端坐标系的雅克比矩阵的每一列，第七个齐次变换 矩阵转换成末端到基坐标的旋转矩阵，通过旋转矩阵乘上相对于末端坐标系的雅克比矩阵得到相对于基坐标系的雅克比矩阵，然后用相对于基坐标系的雅克比矩阵乘上自身的转置，得到一个6X6的满秩方阵。该模块包含有矩阵相乘子模块和向量相乘子模块，两个子模块同样是乘累加单元(MAC)基本模块构成，向量相乘子模块用于计算相对于末端坐标系的雅克比矩阵每一列，矩阵相乘用于计算旋转矩阵乘上相对于末端坐标系的雅克比矩阵和相对于基坐标系的雅克比矩阵乘上自身的转置。
5.依据权利要求1所述的空间机械臂运动学实时解算方法，其特征在于，求逆模块采用分块矩阵求逆算法，将6X6的满秩方阵划分为四块3X3方阵，通过进行矩阵相乘和矩阵求逆运算，依次求出逆矩阵的四块。该模块包括:三阶矩阵相乘子模块、三阶矩阵求逆子模块和逻辑控制单元。三阶矩阵相乘子模块由乘累加单元(MAC)基本模块构成。三阶矩阵求逆子模块由除法器和乘累加单元(MAC)基本模块构成。控制单元由有限状态机构成，负责调度矩阵求逆子模块和矩阵相乘子模块，依次计算逆矩阵的四块。
6.依据权利要求1所述的空间机械臂运动学实时解算方法，其特征在于，求解关节角速度模块用乘累加模块(MAC)构成一个7X6X6X1的矩阵-向量相乘子模块，将矩阵相乘运算化简为矩阵-向量相乘运算，求雅克比矩阵伪逆运算和求关节角度运算复用该子模块，通过有限状态机进行调度，先将雅克比矩阵转置和方阵的逆矩阵相乘得到雅克比矩阵伪逆，然后再将雅克比矩阵伪逆乘上末`端速度得到七个关节角速度，最后输出结果。
【文档编号】G05B13/04GK103499922SQ201310419763
【公开日】2014年1月8日 申请日期:2013年9月16日 优先权日:2013年9月16日
【发明者】叶平, 韩亮亮, 孙汉旭, 贾庆轩, 吉雪 申请人:北京邮电大学