[发明专利]操纵器关节极限处理算法

权利要求书

1.一种用于控制自动机械设备中的多个关节的方法，所述关节中的至少一部分具有相应的关节移动极限，所述方法包括：

判断所述自动机械设备的所需移动命令是否将导致所述自动机械设备的至少第一关节移动极限被违犯；

使用所述自动机械设备的当前状态和所述所需移动命令，为所述自动机械设备构建修改的移动命令，所述修改的移动命令避免所述至少第一关节移动极限被违犯，其中，所述修改的移动命令不与所述所需移动命令相同，并且当被所述自动机械设备处理时导致所述自动机械设备的移动；以及

使用所述修改的移动命令代替所述所需移动命令，以导致所述自动机械设备的相应的移动。

2.如权利要求1所述的方法，其中，为所述自动机械设备构建修改的移动命令进一步包括使用加权因数来加权所述关节的相应的移动。

3.如权利要求2所述的方法，其中，所述自动机械设备包括所述自动机械设备的末端执行器的m维任务空间的n个关节，其中，所述所需移动命令将导致所述n个关节中的L个违犯它们的相应的关节移动极限，并且其中，为所述自动机械设备构建所述修改的关节移动命令包括求解下列公式：

q·mod=Jmod(JmodTW2Jmod)-1JmodTW2x·cmd,]]>

其中，是当前没有被限制的关节的(n-L)×1关节速度命令，J_mod是m×(n-L)矩阵，是J_mod的转置矩阵，W是包含所述加权因数的m×m矩阵，并且是所述末端执行器的所需移动命令。

4.如权利要求3所述的方法，其中，为所述自动机械设备构建修改的关节移动命令进一步包括求解下列公式：

q·new=re(q·mod),]]>

其中，是n×1新关节速度命令，并且re()是将L个有效限制的关节中的每一个的相应的预定义的值插入到中以生成的函数。

5.如权利要求3所述的方法，进一步包括使用下列步骤来生成m×(n-L)矩阵J_mod：

使用所述自动机械设备的当前状态来确定所述自动机械设备的m×n雅各布矩阵J，所述自动机械设备的所述当前状态包括所述n个关节中的每一个的当前位移；以及

使用所述雅各布矩阵J来生成所述矩阵J_mod，其中，J_mod包括J中的除对应于所述L个有效限制的关节的那些列之外的所有列。