本发明公开了一种基于投影几何法的单关节故障机械臂模型重构方法。其核心包括：基于旋量坐标表征机械臂的运动学模型，由于不同故障关节在基础坐标系中的轴向不同，其故障角度影响各关节旋量坐标的不同参数，因此将故障关节按其轴向进行分类，针对不同类关节，采用投影法在其轴线的公垂面构造投影平面，获得运动学模型平面投影图；根据各故障关节的平面投影图，通过几何分析，建立故障角度、连杆长度与各关节旋量坐标的关系式，完成单关节故障机械臂模型重构。本发明解决了机械臂单关节故障后，原有运动学模型失效，无法进行运动控制的问题，同时其以旋量理论作为基础，相比D‑H参数方法，具有效率高、通用性强，适用于不同构型机械臂的特点。
1.一种基于投影几何法的单关节故障机械臂模型重构方法，其特征在于该方法包括以下步骤：
1)基于旋量坐标表征机械臂的运动学模型，根据故障关节在基础坐标系下的轴向不同，其故障角度影响各关节旋量坐标的不同参数，将故障关节沿轴向进行分类，针对具有不同轴向的故障关节，采用投影法在其轴线的公垂面构造投影平面，获得运动学模型平面投影图,具体包括以下步骤：
11)针对关节在基础坐标系中的轴向不同，其故障角度对各关节旋量坐标的影响不同，将故障关节进行分类，即X轴关节、Y轴关节、Z轴关节；
12)基于旋量坐标表征机械臂的运动学模型，规定除故障关节Jf锁定在故障角度以外，其余关节固定于 (i＝1,...,m且i≠f)，其中，表示第i个关节的关节角度，m表示机械臂总自由度数，根据故障关节Jf的轴向确定其运动学模型的二维投影平面：X轴关节对应基础坐标系的YZ平面投影图、Y轴关节对应XZ平面投影图、Z轴关节对应XY平面投影图，二维图可用于确定各关节除故障关节轴向方向的旋量坐标，沿故障关节轴向方向的旋量坐标需由重构的运动学模型获得；
2)基于平面投影图，采用几何分析法确定机械臂的旋量坐标，针对不同类关节发生故障的情况，分析故障关节锁定在固定角度后，基础坐标系与工具坐标系的变换矩阵，以及重构运动学模型中各关节的旋量坐标与故障角度的关系，实现单关节故障机械臂运动学模型重构，具体包括以下步骤：
21)若故障关节为X轴关节，关节故障后，基础坐标系与工具坐标系的变化关系为：其中，θf表示故障关节锁定的角度，Σlx表示沿X轴方向连杆的长度之和，Σlyr和Σlzr分别表示发生故障的关节之前的Y轴和Z轴方向连杆的长度之和，Σlyf和Σlzf表示发生故障的关节之后的Y轴和Z轴方向连杆的长度之和；
故障关节标号前的关节的旋量坐标保持原有值不变，标号后的关节的旋量坐标为：
其中， 分别表示Y轴关节及Z轴关节的运动旋量轴线方向上的单位矢量，(xi,yi,zi)表示故障前轴线上的点,xi,yi,zi分别表示点的x,y,z坐标；
22)若故障关节为Y轴关节，关节故障后，基础坐标系与工具坐标系的变化关系为：其中，Σly表示沿Y轴方向连杆的长度之和，Σlxr表示发生故障的关节之前的X轴方向连杆的长度之和，Σlxf表示发生故障的关节之后的X轴方向连杆的长度之和；
故障关节标号前的关节的旋量坐标保持原有值不变，标号后的关节的旋量坐标为：
其中， 表示X轴关节的运动旋量轴线方向上的单位矢量；
23)若故障关节为Z轴关节，关节故障后，基础坐标系与工具坐标系的变化为：
故障关节标号前的关节的旋量坐标保持原有值不变，标号后的关节的旋量坐标为：
一种基于投影几何法的单关节故障机械臂模型重构方法
技术领域
[0001] 本发明涉及一种基于投影几何法的单关节故障机械臂模型重构方法，属于机械臂容错技术领域。
背景技术
[0002] 由于太空环境具有超真空、高温差、强辐射的特点，综合考虑空间机械臂工作环境的恶劣性以及关节内部的结构复杂性，在其长期服役过程中极有可能出现关节失效现象。空间机械臂一旦失效，通常无法进行及时修复，这将导致空间任务的推迟甚至延误，从而影响到整个航天计划的实施。目前，针对空间机械臂在轨操作过程中发生单一关节突发失效的情形，锁定故障关节是一种简单而有效的处理方法。关节锁定后由于运动模型的损坏，机械臂无法按照原规划路径完成在轨操作任务，从而导致任务的失败。本发明为一种基于投影几何法的单关节故障机械臂模型重构方法，能够满足在机械臂单关节损坏状态下，最大限度的完成在轨任务的需求，对于未来进行空间探索，特别是空间机械臂在轨应用具有十分重要的理论意义和现实意义。
[0003] 目前，针对单关节故障的机械臂模型重构方法研究成果较少，主流的研究成果为基于D-H参数方法的SSRMS型机械臂的运动学模型重构，解决了特殊构型机械臂任一关节锁定时的模型重构问题，但其D-H坐标系的重建过程复杂，针对每一关节失效情况的重建方法均不相同，因此计算量较大，同时针对不同构型机械臂的解决方法没有通用的表达式，因此通用性低。为了解决上述问题，本发明以旋量理论为基础，提供了一种基于投影几何法的单关节故障机械臂模型重构方法，该方法具有效率高、通用性强、适用于不同构型机械臂的优点。
发明内容
[0004] 本发明的目的是针对上述模型重构方法的不足，提供一种基于投影几何法的单关节故障机械臂模型重构方法，解决了单关节故障状态下机械臂运动模型发生改变，无法进行运动控制的问题。
[0005] 本发明所采用的技术方案是：一种基于投影几何法的单关节故障机械臂模型重构方法，步骤如下：
[0006] 1)基于旋量坐标表征机械臂的运动学模型，根据故障关节在基础坐标系下的轴向不同，其故障角度影响各关节旋量坐标的不同参数，将故障关节沿轴向进行分类，针对具有不同轴向的故障关节，采用投影法在其轴线的公垂面构造投影平面，获得运动学模型平面投影图；
[0007] 2)基于平面投影图，采用几何分析法确定机械臂的旋量坐标，针对不同类关节发生故障的情况，分析故障关节锁定在固定角度后，基础坐标系与工具坐标系的变换矩阵，以及重构运动学模型中各关节的旋量坐标与故障角度的关系，实现单关节故障机械臂运动学模型重构。
[0008] 步骤1)投影法对机械臂运动学模型进行二维投影包括以下步骤：
[0009] 11)规定关节故障状态下的标号规则，当关节Jf锁定时，除关节Jf外，其余关节的相关符号均增加“～”，且其后关节标号在原基础上减一，即 且机械臂自由度数退化为m＝n-1；
[0010] 12)针对关节在基础坐标系中的轴向不同，其故障角度对各关节旋量坐标的影响不同，将故障关节进行分类，即X轴关节、Y轴关节、Z轴关节；
[0011] 13)基于旋量坐标表征机械臂的运动学模型，规定除故障关节Jf锁定在故障角度以外，其余关节固定于 根据故障关节Jf的轴向确定其运动学模型的二维投影平面：X轴关节对应基础坐标系的YZ平面投影图、Y轴关节对应XZ平面投影图、Z轴关节对应XY平面投影图，二维图可用于确定各关节除故障关节轴向方向的旋量坐标，沿故障关节轴向方向的旋量坐标需由重构的运动学模型获得。
[0012] 步骤2)几何分析法确定旋量坐标包括以下步骤：
[0013] 21)若故障关节为X轴关节，关节故障后，基础坐标系与工具坐标系的变换关系为：
[0014]
[0015] 其中，θf表示故障关节锁定的角度，Σlx表示沿X轴方向连杆的长度之和，Σlyr和Σlzr分别表示发生故障的关节之前的Y轴和Z轴方向连杆的长度之和，Σlyf和Σlzf分别表示发生故障的关节之后的Y轴和Z轴方向连杆的长度之和。
[0016] 故障关节标号前的关节的旋量坐标保持原有值不变，标号后的关节的旋量坐标为：
[0017]
[0018]
[0019]
[0020]
[0021] 其中， 分别表示Y轴关节及Z轴关节的运动旋量轴线方向上的单位矢量，(xi,yi,zi)表示故障前轴线上的点,xi,yi,zi分别表示点的x,y,z坐标，表示关节i的运动旋量；
[0022] 22)若故障关节为Y轴关节，关节故障后，基础坐标系与工具坐标系的变换关系为：
[0023]
[0024] 其中，Σly表示沿Y轴方向连杆的长度之和，Σlxr表示发生故障的关节之前的X轴方向连杆的长度之和，Σlxf表示发生故障的关节之后的X轴方向连杆的长度之和。
[0025] 故障关节标号前的关节的旋量坐标保持原有值不变，标号后的关节的旋量坐标为：
[0026]
[0027]
[0028]
[0029]
[0030] 其中， 表示X轴关节的运动旋量轴线方向上的单位矢量。
[0031] 23)若故障关节为Z轴关节，关节故障后，基础坐标系与工具坐标系的变换关系为：
[0032]
[0033] 故障关节标号前的关节的旋量坐标保持原有值不变，标号后的关节的旋量坐标为：
[0034]
[0035]
[0036]
[0037]
[0038] 本发明与现有技术相比具有以下优点：
[0039] (1)本发明通过旋量坐标表征机械臂的运动学模型，实现了不同构型机械臂单关节失效后运动学模型重构的通用性，并且在机械臂构型复杂、自由度高时，其通用性尤为明显。
[0040] (2)本发明采用投影法在故障关节轴线的公垂面构造投影平面，能够有效地分析关节锁定在故障角度后，基础坐标系与工具坐标系的变换矩阵，以及重构运动学模型中各关节的旋量坐标与故障角度的关系。
[0041] (3)本发明采用几何法推导出各类关节发生故障时的旋量坐标与故障关节角度及连杆长度之间的数学表达式，在已知常态下机械臂旋量坐标及故障关节锁定角度的基础上，可直接获得重构的旋量坐标，提高了运动学模型重构的效率，能够满足在轨应用的实时性需求。
附图说明
[0042] 图1是本发明方法流程图。
[0043] 图2是常态下旋量坐标系。
[0044] 图3是关节三锁定后的重建旋量系。
[0045] 图4是关节三锁定后YZ平面投影图。
[0046] 图5是关节二锁定后的重建旋量系。
[0047] 图6是关节二锁定后XZ平面投影图。
[0048] 图7是关节一锁定后的重建旋量系。
[0049] 图8是关节一锁定后XY平面投影图。
具体实施方式
[0050] 1、投影法对单关节失效机械臂运动学模型进行二维投影
[0051] 如图1所示为本发明方法流程图。本发明以空间七自由度机械臂作为研究对象，各杆件长度如表1所示，机械臂的构型如图2所示。对于每一个关节Ji(i＝1,...,7)，构造其运动旋量，旋量坐标如表2所示。
[0052] 表1连杆长度
[0053]
[0054] 表2常态下旋量坐标
[0055]
[0056] X轴关节J3锁定在30°时，J3之后的关节标记为 重构的旋量系如图3所示，YZ方向投影图如图4所示。Y轴关节J2锁定在30°时，J2之后的关节标记为 重构的旋量系如图5所示，XZ方向投影图如图6所示。Z轴关节J1锁定在30°时，J1之后的关节标记为重构的旋量系如图7所示，XY方向投影图如图8所示。
[0057] 2、几何分析法确定旋量坐标
[0058] X轴关节J3锁定时，产生变化的旋量参数为 和 均为θ3的函数，重构后的旋量参数如表3所示，旋量坐标如表4所示。基础坐标系S保持原有方向，工具坐标系T的方向发生变化，基础坐标系与工具坐标系的变换关系为：
[0059]
[0060] 表3三关节锁定后重构旋量参数
[0061]
[0062] 表4三关节锁定后重构旋量坐标
[0063]
[0064] Y轴关节J2锁定时，产生变化的旋量参数为 和 均为θ2的函数。重构后的旋量参数如表5所示，旋量坐标如表6所示。基础坐标系S保持原有方向，工具坐标系T的方向发生变化，基础坐标系与工具坐标系的变换关系为：
[0065]
[0066] 表5二关节锁定后重构旋量参数
[0067]
[0068] Z轴关节J1锁定时，产生变化的旋量参数为 和 均为θ1的函数。重构后的旋量参数如表7所示，旋量坐标如表8所示。基础坐标系S保持原有方向，工具坐标系T的方向发生变化，基础坐标系与工具坐标系的变换关系为：
[0069]
[0070] 表6二关节锁定后重构旋量坐标
[0071]
[0072] 表7一关节锁定后重构旋量参数
[0073]
[0074] 表8一关节锁定后重构旋量坐标
[0075]
[0076] 本发明说明书中未作详细描述的内容属本领域技术人员的公知技术。
