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

权利要求书

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的矩阵-向量相乘子模块，将矩阵相乘运算化简为矩阵-向量相乘运算，求雅克比矩阵伪逆运算和求关节角度运算复用该子模块，通过有限状态机进行调度，先将雅克比矩阵转置和方阵的逆矩阵相乘得到雅克比矩阵伪逆，然后再将雅克比矩阵伪逆乘上末端速度得到七个关节角速度，最后输出结果。