位姿控制方法、装置、存储介质及控制器

摘要

本公开涉及一种位姿控制方法、装置、存储介质及控制器。该方法应用于引导架，所述引导架包括动平台和用于驱动所述动平台的位姿变化的驱动装置，该方法包括：确定所述动平台的当前位姿；在所述动平台的所述当前位姿与目标位姿不相同的情况下，确定用于控制所述驱动装置的目标控制量；根据所述目标控制量驱动所述驱动装置，以使所述动平台从所述当前位姿到达所述目标位姿。本公开这种通过引导架的主动驱动来控制引导架上的动平台的位姿的方式，提升了位姿控制的安全性。

权利要求

1.一种位姿控制方法，其特征在于，所述方法应用于引导架，所述引导架包括动平台和用于驱动所述动平台的位姿变化的驱动装置，所述方法包括：确定所述动平台的当前位姿，其中所述引导架还包括万向节，所述万向节包括球体，所述当前位姿包括当前姿态矩阵，所述确定所述动平台的当前位姿，包括：确定所述球体在驱动装置坐标系下的第一坐标；

确定所述球体在动平台坐标系下的第二参数坐标；

根据所述驱动装置和所述动平台之间的部分连接关系，确定目标节点；

根据所述目标节点的旋转角度参数构建所述驱动装置坐标系和所述动平台坐标系之间的转换矩阵，所述转换矩阵包括各所述目标节点的旋转角度参数的三角函数；

根据所述第一坐标、所述第二参数坐标、以及所述转换矩阵之间的关系，确定所述旋转角度参数对应的旋转角度大小；

根据所述目标节点的所述旋转角度大小确定所述当前姿态矩阵；

在所述动平台的所述当前位姿与目标位姿不相同的情况下，确定用于控制所述驱动装置的目标控制量；

根据所述目标控制量驱动所述驱动装置，以使所述动平台从所述当前位姿到达所述目标位姿。

2.根据权利要求1所述的方法，其特征在于，所述驱动装置包括底座和第一驱动组件，所述第一驱动组件包括第一末端连杆和两个第一直线驱动器，所述球体固连于所述第一末端连杆；

所述确定所述球体在驱动装置坐标系下的第一坐标，包括：根据两个所述第一直线驱动器分别与所述底座铰接的第一铰接位置、以及两个所述第一直线驱动器通过所述第一末端连杆铰接的第二铰接位置构建第一平面参考三角形；

根据所述第一平面参考三角形中两个所述第一铰接位置的第一铰接坐标和每一所述第一直线驱动器的当前长度，确定所述第二铰接位置的第二铰接坐标；

根据所述第二铰接坐标、所述球体与所述第二铰接位置之间的距离、以及左侧第一直线驱动器与所述第一末端连杆之间的夹角大小确定所述第一坐标，其中，所述左侧第一直线驱动器是两个所述第一直线驱动器中的与所述第一末端连杆固连的所述第一直线驱动器。

3.根据权利要求1所述的方法，其特征在于，所述万向节还包括球套，所述球套固连于所述动平台的连杆的第一端，所述连杆的第二端可转动地连接于所述动平台的安装板的第一连接位，所述驱动装置的第二驱动组件的输出端可转动地连接于所述安装板的第二连接位；所述方法还包括：以所述第二连接位为原点构建所述动平台坐标系；

所述确定所述球体在动平台坐标系下的第二参数坐标，包括：根据所述第一连接位、所述第二连接位以及所述球体确定第二平面参考三角形；

根据所述第二平面参考三角形的几何关系确定所述第二参数坐标。

4.根据权利要求3所述的方法，其特征在于，所述第二驱动组件包括第二末端连杆、连接件以及两个第二直线驱动器，所述连接件的第一端可转动地连接于所述第二末端连杆，所述第二驱动组件的输出端表征所述连接件的第二端；

所述根据所述驱动装置和所述动平台之间的部分连接关系，确定目标节点，包括：将两个所述第二直线驱动器通过所述第二末端连杆铰接的第三铰接位置确定为第一目标节点；

将所述连接件的第一端与所述第二末端连杆连接的第一连接位置确定为第二目标节点；

将所述第二驱动组件的输出端与所述安装板连接的第二连接位置确定为第三目标节点。

5.根据权利要求4所述的方法，其特征在于，所述根据所述目标节点的旋转角度参数构建所述驱动装置坐标系和所述动平台坐标系之间的转换矩阵，包括：分别以所述第一目标节点、所述第二目标节点以及所述第三目标节点为原点构建坐标系，得到串联坐标系；

根据所述串联坐标系构建所述转换矩阵。

6.根据权利要求2所述的方法，其特征在于，所述在所述动平台的所述当前位姿与目标位姿不相同的情况下，确定用于控制所述驱动装置的目标控制量，包括：根据所述目标位姿中的目标姿态矩阵、所述球体在所述动平台坐标系下的第一目标坐标确定所述球体在所述驱动装置坐标系下的第二目标坐标；

根据所述第二目标坐标、两个所述第一铰接坐标、以及所述左侧第一直线驱动器与所述第一末端连杆之间的夹角大小确定所述左侧第一直线驱动器的第一目标长度；

根据所述左侧第一直线驱动器的当前长度和所述第一目标长度，确定所述左侧第一直线驱动器的第一目标控制量。

7.根据权利要求6所述的方法，其特征在于，所述在所述动平台的所述当前位姿与目标位姿不相同的情况下，确定用于控制所述驱动装置的目标控制量，包括：根据所述左侧第一直线驱动器的所述第一目标长度、两个所述第一铰接坐标、以及所述球体与所述第二铰接位置之间的距离确定右侧第一直线驱动器的第二目标长度；

根据所述右侧第一直线驱动器的当前长度和所述第二目标长度，确定所述右侧第一直线驱动器的第二目标控制量。

8.根据权利要求2所述的方法，其特征在于，在根据所述目标节点的所述旋转角度大小确定所述当前姿态矩阵之前，还包括：基于根据所述目标节点的所述旋转角度大小判断所述球体的当前轴线与所述球体的参考轴线之间的夹角是否小于预设阈值；

所述根据所述目标节点的所述旋转角度大小确定所述当前姿态矩阵，包括：在所述夹角小于所述预设阈值的情况下，根据所述目标节点的所述旋转角度大小确定所述当前姿态矩阵。

9.一种位姿控制装置，其特征在于，所述装置应用于引导架，所述引导架包括动平台和用于驱动所述动平台的位姿变化的驱动装置，所述装置包括：第一确定模块，用于确定所述动平台的当前位姿，其中所述引导架还包括万向节，所述万向节包括球体，所述当前位姿包括当前姿态矩阵，所述第一确定模块，包括：第一确定子模块，用于确定所述球体在驱动装置坐标系下的第一坐标；

第二确定子模块，用于确定所述球体在动平台坐标系下的第二参数坐标；

第三确定子模块，用于根据所述驱动装置和所述动平台之间的部分连接关系，确定目标节点；

构建子模块，用于根据所述目标节点的旋转角度参数构建所述驱动装置坐标系和所述动平台坐标系之间的转换矩阵，所述转换矩阵包括各所述目标节点的旋转角度参数的三角函数；

第四确定子模块，用于根据所述第一坐标、所述第二参数坐标、以及所述转换矩阵之间的关系，确定所述旋转角度参数对应的旋转角度大小；

第五确定子模块，用于根据所述目标节点的所述旋转角度大小确定所述当前姿态矩阵；

第二确定模块，用于在所述动平台的所述当前位姿与目标位姿不相同的情况下，确定用于控制所述驱动装置的目标控制量；

驱动模块，用于根据所述目标控制量驱动所述驱动装置，以使所述动平台从所述当前位姿到达所述目标位姿。

10.一种非临时性计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现权利要求1‑8中任一项所述方法的步骤。

11.一种控制器，其特征在于，包括：

存储器，其上存储有计算机程序；

处理器，用于执行所述存储器中的所述计算机程序，以实现权利要求1‑8中任一项所述方法的步骤。

说明书

位姿控制方法、装置、存储介质及控制器

技术领域

[0001] 本公开涉及计算机技术领域，具体地，涉及一种位姿控制方法、装置、存储介质及控制器。

背景技术

[0002] 手术机器人是一种先进的医疗设备，借助微创伤手术及相关底层技术的发展而发明。机器人辅助手术与开放手术及传统微创伤手术相比，具以下优势：1、减少手术伤口、术后恢复快及较少术后并发症；2、灵活的机械臂与高度复杂的手术兼容；3、手术的精准度及手术结果的稳定性；4、降低外科医生疲惫及缩短学习曲线；5、减少辐射暴露等。

[0003] 相关技术中，手术机器人包括机械臂和引导架，引导架连接于机械臂的输出端，以通过控制机械臂动作来将引导架移动至目标位置。但是，因手术室环境复杂，机械臂的主动驱动方式无法感知周围环境变化，如医生的移动、其他医疗设备的位置等，而容易在驱动过程中造成磕碰引发事故，安全性低。

发明内容

[0004] 本公开的目的是提供一种位姿控制方法、装置、存储介质及控制器，以提升位姿控制的安全性。

[0005] 为了实现上述目的，本公开实施例的第一部分提供一种位姿控制方法，所述方法应用于引导架，所述引导架包括动平台和用于驱动所述动平台的位姿变化的驱动装置，所述方法包括：

[0006] 确定所述动平台的当前位姿；

[0007] 在所述动平台的所述当前位姿与目标位姿不相同的情况下，确定用于控制所述驱动装置的目标控制量；

[0008] 根据所述目标控制量驱动所述驱动装置，以使所述动平台从所述当前位姿到达所述目标位姿。

[0009] 可选地，所述引导架还包括万向节，所述万向节包括球体；

[0010] 所述当前位姿包括当前姿态矩阵，所述确定所述动平台的当前位姿，包括：

[0011] 确定所述球体在驱动装置坐标系下的第一坐标；

[0012] 确定所述球体在动平台坐标系下的第二参数坐标；

[0013] 根据所述驱动装置和所述动平台之间的部分连接关系，确定目标节点；

[0014] 根据所述目标节点的旋转角度参数构建所述驱动装置坐标系和所述动平台坐标系之间的转换矩阵，所述转换矩阵包括各所述目标节点的旋转角度参数的三角函数；

[0015] 根据所述第一坐标、所述第二参数坐标、以及所述转换矩阵之间的关系，确定所述旋转角度参数对应的旋转角度大小；

[0016] 根据所述目标节点的所述旋转角度大小确定所述当前姿态矩阵。

[0017] 可选地，所述驱动装置包括底座和第一驱动组件，所述第一驱动组件包括第一末端连杆和两个第一直线驱动器，所述球体固连于所述第一末端连杆；

[0018] 所述确定所述球体在驱动装置坐标系下的第一坐标，包括：

[0019] 根据两个所述第一直线驱动器分别与所述底座铰接的第一铰接位置、以及两个所述第一直线驱动器通过所述第一末端连杆铰接的第二铰接位置构建第一平面参考三角形；

[0020] 根据所述第一平面参考三角形中两个所述第一铰接位置的第一铰接坐标和每一所述第一直线驱动器的当前长度，确定所述第二铰接位置的第二铰接坐标；

[0021] 根据所述第二铰接坐标、所述球体与所述第二铰接位置之间的距离、以及左侧第一直线驱动器与所述第一末端连杆之间的夹角大小确定所述第一坐标，其中，所述左侧第一直线驱动器是两个所述第一直线驱动器中的与所述第一末端连杆固连的所述第一直线驱动器。

[0022] 可选地，所述万向节还包括球套，所述球套固连于所述动平台的连杆的第一端，所述连杆的第二端可转动地连接于所述动平台的安装板的第一连接位，所述驱动装置的第二驱动组件的输出端可转动地连接于所述安装板的第二连接位；所述方法还包括：

[0023] 以所述第二连接位为原点构建所述动平台坐标系；

[0024] 所述确定所述球体在动平台坐标系下的第二参数坐标，包括：

[0025] 根据所述第一连接位、所述第二连接位以及所述球体确定第二平面参考三角形；

[0026] 根据所述第二平面参考三角形的几何关系确定所述第二参数坐标。

[0027] 可选地，所述第二驱动组件包括第二末端连杆、连接件以及两个第二直线驱动器，所述连接件的第一端可转动地连接于所述第二末端连杆(421)，所述第二驱动组件的输出端表征所述连接件的第二端；

[0028] 所述根据所述驱动装置和所述动平台之间的部分连接关系，确定目标节点，包括：

[0029] 将两个所述第二直线驱动器通过所述第二末端连杆铰接的第三铰接位置确定为第一目标节点；

[0030] 将所述连接件的第一端与所述第二末端连杆连接的第一连接位置确定为第二目标节点；

[0031] 将所述第二驱动组件的输出端与所述安装板连接的第二连接位置确定为第三目标节点。

[0032] 可选地，所述根据所述目标节点的旋转角度参数构建所述驱动装置坐标系和所述动平台坐标系之间的转换矩阵，包括：

[0033] 分别以所述第一目标节点、所述第二目标节点以及所述第三目标节点为原点构建坐标系，得到串联坐标系；

[0034] 根据所述串联坐标系构建所述转换矩阵。

[0035] 可选地，所述在所述动平台的所述当前位姿与目标位姿不相同的情况下，确定用于控制所述驱动装置的目标控制量，包括：

[0036] 根据所述目标位姿中的目标姿态矩阵、所述球体在所述动平台坐标系下的第一目标坐标确定所述球体在所述驱动装置坐标系下的第二目标坐标；

[0037] 根据所述第二目标坐标、两个所述第一铰接坐标、以及所述左侧第一直线驱动器与所述第一末端连杆之间的夹角大小确定所述左侧第一直线驱动器的第一目标长度；

[0038] 根据所述左侧第一直线驱动器的当前长度和所述第一目标长度，确定所述左侧第一直线驱动器的第一目标控制量。

[0039] 可选地，所述在所述动平台的所述当前位姿与目标位姿不相同的情况下，确定用于控制所述驱动装置的目标控制量，包括：

[0040] 根据所述左侧第一直线驱动器的所述第一目标长度、两个所述第一铰接坐标、以及所述球体与所述第二铰接位置之间的距离确定所述右侧第一直线驱动器的第二目标长度；

[0041] 根据所述右侧第一直线驱动器的当前长度和所述第二目标长度，确定所述右侧第一直线驱动器的第二目标控制量。

[0042] 可选地，所述万向节包括固连于所述球体的连接柱，所述球体通过所述连接柱连接于所述第一末端连杆，所述万向节的球套包括用于容纳所述球体的容纳槽和供所述连接柱贯穿的开口，所述开口的边缘形成能够抵接所述连接柱的限位部；

[0043] 在根据所述目标节点的所述旋转角度大小确定所述当前姿态矩阵之前，还包括：

[0044] 基于根据所述目标节点的所述旋转角度大小判断所述球体的当前轴线与所述球体的参考轴线之间的夹角是否小于预设阈值；

[0045] 所述根据所述目标节点的所述旋转角度大小确定所述当前姿态矩阵，包括：

[0046] 在所述夹角小于所述预设阈值的情况下，根据所述目标节点的所述旋转角度大小确定所述当前姿态矩阵。

[0047] 本公开实施例的第二部分提供一种位姿控制装置，所述装置应用于引导架，所述引导架包括动平台和用于驱动所述动平台的位姿变化的驱动装置，所述装置包括：

[0048] 第一确定模块，用于确定所述动平台的当前位姿；

[0049] 第二确定模块，用于在所述动平台的所述当前位姿与目标位姿不相同的情况下，确定用于控制所述驱动装置的目标控制量；

[0050] 驱动模块，用于根据所述目标控制量驱动所述驱动装置，以使所述动平台从所述当前位姿到达所述目标位姿。

[0051] 可选地，所述引导架还包括万向节，所述万向节包括球体；所述当前位姿包括当前姿态矩阵，所述第一确定模块，包括：

[0052] 第一确定子模块，用于确定所述球体在驱动装置坐标系下的第一坐标；

[0053] 第二确定子模块，用于确定所述球体在动平台坐标系下的第二参数坐标；

[0054] 第三确定子模块，用于根据所述驱动装置和所述动平台之间的部分连接关系，确定目标节点；

[0055] 构建子模块，用于根据所述目标节点的旋转角度参数构建所述驱动装置坐标系和所述动平台坐标系之间的转换矩阵，所述转换矩阵包括各所述目标节点的旋转角度参数的三角函数；

[0056] 第四确定子模块，用于根据所述第一坐标、所述第二参数坐标、以及所述转换矩阵之间的关系，确定所述旋转角度参数对应的旋转角度大小；

[0057] 第五确定子模块，用于根据所述目标节点的所述旋转角度大小确定所述当前姿态矩阵。

[0058] 可选地，所述驱动装置包括底座和第一驱动组件，所述第一驱动组件包括第一末端连杆和两个第一直线驱动器，所述球体固连于所述第一末端连杆；

[0059] 所述第一确定子模块，用于：根据两个所述第一直线驱动器分别与所述底座铰接的第一铰接位置、以及两个所述第一直线驱动器通过所述第一末端连杆铰接的第二铰接位置构建第一平面参考三角形；根据所述第一平面参考三角形中两个所述第一铰接位置的第一铰接坐标和每一所述第一直线驱动器的当前长度，确定所述第二铰接位置的第二铰接坐标；根据所述第二铰接坐标、所述球体与所述第二铰接位置之间的距离、以及左侧第一直线驱动器与所述第一末端连杆之间的夹角大小确定所述第一坐标，其中，所述左侧第一直线驱动器是两个所述第一直线驱动器中的与所述第一末端连杆固连的所述第一直线驱动器。

[0060] 可选地，所述万向节还包括球套，所述球套固连于所述动平台的连杆的第一端，所述连杆的第二端可转动地连接于所述动平台的安装板的第一连接位，所述驱动装置的第二驱动组件的输出端可转动地连接于所述安装板的第二连接位；所述装置还包括：

[0061] 构建模块，用于以所述第二连接位为原点构建所述动平台坐标系；

[0062] 所述第二确定子模块，用于：根据所述第一连接位、所述第二连接位以及所述球体确定第二平面参考三角形；根据所述第二平面参考三角形的几何关系确定所述第二参数坐标。

[0063] 可选地，所述第二驱动组件包括第二末端连杆、连接件以及两个第二直线驱动器，所述连接件的第一端可转动地连接于所述第二末端连杆，所述第二驱动组件的输出端表征所述连接件的第二端；

[0064] 所述第三确定子模块，用于：将两个所述第二直线驱动器通过所述第二末端连杆铰接的第三铰接位置确定为第一目标节点；将所述连接件的第一端与所述第二末端连杆连接的第一连接位置确定为第二目标节点；将所述第二驱动组件的输出端与所述安装板连接的第二连接位置确定为第三目标节点。

[0065] 可选地，所述构建子模块，用于：分别以所述第一目标节点、所述第二目标节点以及所述第三目标节点为原点构建坐标系，得到串联坐标系；根据所述串联坐标系构建所述转换矩阵。

[0066] 可选地，所述第二确定模块，包括：

[0067] 第一执行模块，用于根据所述目标位姿中的目标姿态矩阵、所述球体在所述动平台坐标系下的第一目标坐标确定所述球体在所述驱动装置坐标系下的第二目标坐标；

[0068] 第二执行模块，用于根据所述第二目标坐标、两个所述第一铰接坐标、以及所述左侧第一直线驱动器与所述第一末端连杆之间的夹角大小确定所述左侧第一直线驱动器的第一目标长度；

[0069] 第三执行模块，用于根据所述左侧第一直线驱动器的当前长度和所述第一目标长度，确定所述左侧第一直线驱动器的第一目标控制量。

[0070] 可选地，所述第二确定模块，包括：

[0071] 第四执行模块，用于根据所述左侧第一直线驱动器的所述第一目标长度、两个所述第一铰接坐标、以及所述球体与所述第二铰接位置之间的距离确定所述右侧第一直线驱动器的第二目标长度；

[0072] 第五执行模块，用于根据所述右侧第一直线驱动器的当前长度和所述第二目标长度，确定所述右侧第一直线驱动器的第二目标控制量。

[0073] 可选地，所述万向节包括固连于所述球体的连接柱，所述球体通过所述连接柱连接于所述第一末端连杆，所述万向节的球套包括用于容纳所述球体的容纳槽和供所述连接柱贯穿的开口，所述开口的边缘形成能够抵接所述连接柱的限位部；

[0074] 所述装置还包括：判断模块，用于在根据所述目标节点的所述旋转角度大小确定所述当前姿态矩阵之前，基于根据所述目标节点的所述旋转角度大小判断所述球体的当前轴线与所述球体的参考轴线之间的夹角是否小于预设阈值；

[0075] 所述第五确定子模块，用于：在所述夹角小于所述预设阈值的情况下，根据所述目标节点的所述旋转角度大小确定所述当前姿态矩阵。

[0076] 本公开实施例的第三部分提供一种非临时性计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现第一部分中任一项所述方法的步骤。

[0077] 本公开实施例的第四部分提供一种控制器，包括：

[0078] 存储器，其上存储有计算机程序；

[0079] 处理器，用于执行所述存储器中的所述计算机程序，以实现第一部分中任一项所述方法的步骤。

[0080] 采用上述技术方案，至少可以达到如下的有益技术效果：

[0081] 通过确定引导架的动平台的当前位姿，并在动平台的当前位姿与目标位姿不相同的情况下，确定用于控制引导架的驱动装置的目标控制量。根据目标控制量驱动该驱动装置，以使引导架的动平台从当前位姿到达目标位姿。本公开这种通过引导架的主动驱动来控制引导架上的动平台位姿的方式，与相关技术中的以机械臂的主动驱动来控制引导架移动至目标位置的方式相比，本公开这种方式因引导架的主动驱动范围较小而不容易在驱动过程中造成磕碰，所以本公开这种位姿控制的方式更加安全。

[0082] 本公开的其他特征和优点将在随后的具体实施方式部分予以详细说明。

附图说明

[0083] 附图是用来提供对本公开的进一步理解，并且构成说明书的一部分，与下面的具体实施方式一起用于解释本公开，但并不构成对本公开的限制。在附图中：

[0084] 图1是本公开示例性实施方式中提供的引导架的结构示意图。

[0085] 图2是本公开示例性实施方式中提供的引导架的移除防护壳的结构示意图。

[0086] 图3是本公开示例性实施方式中提供的引导架的移除底座后的结构示意图。

[0087] 图4是本公开示例性实施方式中提供的动平台与第一驱动组件的配合示意图。

[0088] 图5是本公开示例性实施方式中提供的第一驱动组件与球体的配合示意图。

[0089] 图6是本公开示例性实施方式中提供的万向节的剖面图。

[0090] 图7是本公开示例性实施方式中提供的动平台与第二驱动组件的爆炸图。

[0091] 图8是根据本公开一示例性实施例示出的一种位姿控制方法的流程图。

[0092] 图9是根据本公开一示例性实施例示出的一种第一平面参考三角形示意图。

[0093] 图10是根据本公开一示例性实施例示出的一种三角形抽象示意图。

[0094] 图11是根据本公开一示例性实施例示出的另一种三角形抽象示意图。

[0095] 图12是根据本公开一示例性实施例示出的一种驱动装置坐标系示意图。

[0096] 图13是根据本公开一示例性实施例示出的一种第二平面参考三角形示意图。

[0097] 图14是根据本公开一示例性实施例示出的一种串联坐标系示意图。

[0098] 图15是根据本公开一示例性实施例示出的一种串联坐标系抽象示意图。

[0099] 图16是根据本公开一示例性实施例示出的一种第三平面参考三角形示意图。

[0100] 图17是根据本公开一示例性实施例示出的几何示意图。

[0101] 图18是根据本公开一示例性实施例示出的另一种三角形抽象示意图。

[0102] 图19是根据本公开一示例性实施例示出的一种位姿控制装置的框图。

[0103] 图20是根据本公开一示例性实施例示出的一种控制器的框图。

[0104] 附图标记说明

[0105] 1‑底座；110‑限位架；111‑条形孔；120‑限位孔；2‑动平台；210‑安装板；211‑第一连接位；212‑第二连接位；213‑连接板；220‑连杆；221‑安装座；222‑第三板体；223‑第一枢转轴；3‑万向节；310‑球体；320‑球套；330‑连接柱；4‑驱动组件；410‑第一驱动组件；411‑第一末端连杆；412‑第一直线驱动器；413‑第二连接轴；420‑第二驱动组件；421‑第二末端连杆；422‑连接件；4221‑U形板；4222‑转轴；4223‑第二枢转轴；423‑第二直线驱动器；424‑第一连接轴。

具体实施方式

[0106] 以下结合附图对本公开的具体实施方式进行详细说明。应当理解的是，此处所描述的具体实施方式仅用于说明和解释本公开，并不用于限制本公开。

[0107] 在本公开中，在未作相反说明的情况下，使用的方位词如“内、外”是指相应零部件的本身轮廓而言的，此外，本公开中使用的术语“第一”、“第二”等是为了区别一个要素和另一个要素，不具有顺序性和重要性。下面的描述涉及附图时，除非另有表示，不同附图中的相同数字表示相同或相似的要素。

[0108] 需要说明的是，本公开中所有获取信号、信息或数据的动作都是在遵照所在地国家相应的数据保护法规政策的前提下，并获得由相应装置所有者给予授权的情况下进行的。

[0109] 为了便于本领域普通技术人员更加容易理解本公开的技术方案，下面先对可应用本公开的位姿控制方法的一种引导架进行举例说明。参见图1‑7，该引导架包括底座1、动平台2以及驱动组件4。

[0110] 动平台2包括安装板210和跟踪器，安装板210上设置有用于定位手术器械的定位部，跟踪器设置在安装板210上，以供导航系统识别并获取定位部以及安装板210在导航系统中的位置，例如，可以将动平台2的三维模型预先导入导航系统中，跟踪器用于被导航系统的导航相机识别，以在导航系统中构建动平台的三维模型的图像和姿态，以在导航系统的显示器中显示定位部的实时位置的影像。

[0111] 底座1可以通过驱动组件4直接或间接地驱动动平台2移动，以使得定位部移动至目标位置。

[0112] 由此，通过驱动组件4的主动控制和引导可以实现定位部的精确移动和定位，以便于手术过程的开展，且相比于机械臂的复杂的主动驱动结构，显然本公开提供的具有主动控制功能的引导架的成本更低。

[0113] 在一些实施方式中，参考图1所示，动平台2还包括连杆220，连杆220的第一端可绕第一枢转轴线转动地连接于安装板210的第一连接位211。引导架还包括万向节3，该万向节3包括球体310和活动套接在球体310上的球套320，球套320固连于连杆220的与第一端相对的第二端。驱动组件4设置在底座1上且分别与球体310和安装板210的第二连接位212连接，第一连接位211、第二连接位212以及球体310分别位于第一假想三角形的三个端点，驱动组件4用于驱动球体310朝向相对于底座1的第一位置移动，和/或用于驱动第二连接位212朝向相对于底座1的第二位置移动，以间接调整动平台2的空间位置并使得定位部移动至目标位置。

[0114] 驱动组件通过万向节和连杆连接于动平台，能够实现动平台的多姿态的调整，以便于将定位部精确定位至目标位置。

[0115] 驱动组件4可以根据实际设计需求以任意合适的方式构造，例如，参考图1至图3所示，驱动组件4可以包括第一驱动组件410和第二驱动组件420，球体310固连于第一驱动组件410的输出端，第二驱动组件420的输出端可绕平行于第一枢转轴线的第二枢转轴线转动地连接于第二连接位212。由此，可以通过第一驱动组件410和第二驱动组件420分别驱动球体310和第二连接位212的空间位置变换，以实现对球体310和第二连接位212的空间位置的精确控制。

[0116] 在一些实施方式中，参考图4所示，第一驱动组件410可以包括第一末端连杆411和两个第一直线驱动器412，两个第一直线驱动器412的两个固定端分别铰接于底座1，其中一个第一直线驱动器412的驱动端与第一末端连杆411固连，另一个第一直线驱动器412的驱动端铰接于第一末端连杆411，以使得第一末端连杆411在两个第一直线驱动器412所在的第一平面内移动，其中，球体310固连于第一末端连杆411。

[0117] 其中，参考图4和图5所示，上述另一个第一直线驱动器412的驱动端可以通过第二连接轴413铰接于第一末端连杆411，由此，两个第一直线驱动器412的两个固定端以及第二连接轴可以在第一平面形成第二假想三角形的三个端点。由此，由于两个固定端之间的距离一定且已知，通过控制两个第一直线驱动器412的伸缩长度，可以精确控制球体310在第一平面内的位置变换，从而带动动平台2的位置姿态的调整。

[0118] 在一些实施方式中，参考图3和图7所示，第二驱动组件420可以包括第二末端连杆421、连接件422以及两个第二直线驱动器423，两个第二直线驱动器423的两个固定端分别铰接于底座1，其中一个第二直线驱动器423的驱动端与第二末端连杆421固连，另一个第二直线驱动器423的驱动端铰接于第二末端连杆421，以使得第二末端连杆421在两个第二直线驱动器423所在的第二平面内移动，连接件422的一端可绕平行于第二平面的第三枢转轴线转动地连接于第二末端连杆421，连接件422的另一端为第二驱动组件420的输出端。

[0119] 其中，上述另一个第二直线驱动器423的驱动端可以通过例如第一连接轴424铰接于第二末端连杆421，而连接件422的一端可绕平行于第二平面的第三枢转轴线转动地连接于第二末端连杆421，连接件423的另一端可绕第二枢转轴线转动地连接于第二连接位，由此可以在第一连接轴424处、连接件423与第二末端连杆421的连接处以及连接件423与第二连接位212的连接处形成具有三个自由度的三个关节位置，以实现动平台2的多姿态调整，防止卡塞等。此外，两个第二直线驱动器423的两个固定端以及第一连接轴424可以在第二平面内形成第三假想三角形的三个端点。由此，由于该两个固定端之间的距离一定且已知，通过控制两个第二直线驱动器423的伸缩长度，可以精确控制第一连接轴424或第二末端连杆421在第二平面内的位置变化，从而带动动平台2的位置姿态的调整。

[0120] 在一些具体的实施方式中，参考图7所示，连接件422可以包括U形板4221和转轴4222，U形板4221包括相对布置的两个第一板体和连接在两个第一板体之间的第二板体，两个第一板体通过第二枢转轴4223可绕第二枢转轴线转动地连接于安装板210，转轴4222的一端固连于第二末端连杆421，另一端可绕第三枢转轴线转动地连接于第二板体。其中，安装板210可以包括连接板213，第二连接位212可以为形成在连接板213的第一通孔，在安装连接件422时，可以将连接板213部分插入U形板4221的开口内，随后通过第二枢转轴4223贯穿两个第一板体和第二连接位212即可。

[0121] 此外，参考图4所示，连杆220可以包括安装座221和间隔设置在安装座221上的两个第三板体222，两个第三板体222通过第一枢转轴223可绕第一枢转轴线转动地连接于安装板210，安装座221上设置有用于固定连接球套320的安装槽。具体地，第一连接位211可以为形成在连接板213上的第二通孔，在安装时，可以将连接板213部分插入两个第三板体222之间，随后通过第一枢转轴223贯穿两个第三板体222和第一连接位即可。

[0122] 在一些实施方式中，参考图5和图6所示，万向节3可以包括固连于球体310的连接柱330，球体310通过连接柱330连接于驱动组件4，球套320包括用于容纳球体310的容纳槽和供连接柱330贯穿的开口321，开口321的边缘形成能够抵接连接柱330的限位部。这样，可以通过开口321的边缘能够限制球套320相对于球体310的摆角范围，例如，可以根据实际应用需求将球套320相对于球体310的摆角范围限定在0～25°之间。

[0123] 在一些实施方式中，两个第一直线驱动器412所在的第一平面平行于两个第二直线驱动器423所在的第二平面。由此，可以简化驱动组件驱动动平台位姿变化的计算过程。

[0124] 此外，第一直线驱动器412和/或第二直线驱动器423可以为电动推杆或伸缩缸等。通过电动推杆可以精准控制其行程，以实现各直线驱动器的伸缩长度的控制。在采用伸缩缸时，例如液压缸或气缸，为了实现伸缩长度的控制，也可以在底座上设置例如磁栅式传感器等位移传感器，以实时监测和反馈伸缩缸的行程。

[0125] 在一些实施方式中，第一直线驱动器412的驱动行程可以大于第二直线驱动器423的驱动行程，以实现动平台更灵活的位姿调整。

[0126] 在一些实施方式中，底座1上设置有限位架110，限位架110具有平行于第一平面或第二平面延伸的条形孔111，条形孔111用于供两个第一直线驱动器412或两个第二直线驱动器423贯穿，以限制两个第一直线驱动器412或两个第二直线驱动器423沿倾斜于第一平面或第二平面的方向移动。

[0127] 图2示例性地示出了限位架110上具有供两个第一直线驱动器412贯穿的条形孔111的实施方式，以便于将两个第一直线驱动器412限制在第一平面上。

[0128] 或者，也可以的是，底座1上设置有限位孔120，限位孔120用于供两个第一直线驱动器412或两个第二直线驱动器423贯穿，以限制两个第一直线驱动器412或两个第二直线驱动器423沿倾斜于第一平面或第二平面的方向移动。

[0129] 图2示例性地示出了在底座1上设置供两个第二直线驱动器423贯穿的限位孔120的实施方式，以便于将两个第二直线驱动器423限制在第二平面上。

[0130] 在一些实施方式中，参考图2所示，底座1可以构造为空心壳体，限位孔120形成在底座1的侧壁上，两个第二直线驱动器423部分位于该空心壳体内部且驱动端延伸至空心壳体外部以与第二末端连杆421相连，限位架110设置在该空心壳体的外壁上，两个第一直线驱动器412设置在该空心壳体的外壁上且贯穿条形孔111。通过此种布置方式，可以将两个第二直线驱动器423和两个第一直线驱动器412分隔开，以便于引导架的装配及维护。

[0131] 此外，参考图1所示，底座1上设置有围绕两个所述第一直线驱动器412布置的防护壳体。

[0132] 下面结合图1‑7所示的一种引导架例子来详细说明本公开的位姿控制方法。

[0133] 图8是根据本公开一示例性实施例示出的一种位姿控制方法的流程图。该位姿控制方法应用于引导架，引导架包括动平台和用于驱动动平台的位姿变化的驱动装置。如图8所示，该位姿控制方法包括以下步骤：

[0134] S11、确定所述动平台的当前位姿；

[0135] S12、在所述动平台的所述当前位姿与目标位姿不相同的情况下，确定用于控制所述驱动装置的目标控制量；

[0136] S13、根据所述目标控制量驱动所述驱动装置，以使所述动平台从所述当前位姿到达所述目标位姿。

[0137] 在一些实施方式中，动平台可用于承载手术器械。如此，通过本公开的位姿控制方法，可通过控制动平台的位姿来实现控制手术器械的位姿，进而实现机器人辅助手术的目的。

[0138] 在另一些实施方式中，动平台上可设置引导孔，如用于引导穿刺路径的引导孔。如此，通过本公开的位姿控制方法，可通过控制动平台的位姿来实现引导孔的位姿控制，进而实现机器人辅助手术的目的。

[0139] 采用上述方法，通过确定引导架的动平台的当前位姿，并在动平台的当前位姿与目标位姿不相同的情况下，确定用于控制引导架的驱动装置的目标控制量。根据目标控制量驱动该驱动装置，以使引导架的动平台从当前位姿到达目标位姿。本公开这种通过引导架的主动驱动来控制引导架上的动平台位姿的方式，与相关技术中的以机械臂的主动驱动来控制引导架移动至目标位置的方式相比，本公开这种方式因引导架的主动驱动范围较小而不容易在驱动过程中造成磕碰，所以本公开这种位姿控制的方式更加安全。

[0140] 由于本公开的这种方式实现了引导架主动驱动以控制动平台的位姿的目的，因此应说明的是，本公开的引导架不仅可装配在机械臂上进行使用，还可以装配在其他的机械底座上。对此本公开不作具体限定。

[0141] 可选地，所述引导架还包括万向节，所述万向节包括球体；所述当前位姿包括当前姿态矩阵，所述确定所述动平台的当前位姿，包括：

[0142] 确定所述球体在驱动装置坐标系下的第一坐标；确定所述球体在动平台坐标系下的第二参数坐标；根据所述驱动装置和所述动平台之间的部分连接关系，确定目标节点；根据所述目标节点的旋转角度参数构建所述驱动装置坐标系和所述动平台坐标系之间的转换矩阵，所述转换矩阵包括各所述目标节点的旋转角度参数的三角函数；根据所述第一坐标、所述第二参数坐标、以及所述转换矩阵之间的关系，确定所述旋转角度参数对应的旋转角度大小；根据所述目标节点的所述旋转角度大小确定所述当前姿态矩阵。

[0143] 其中，确定球体在驱动装置坐标系下的第一坐标的一种实施方式为，所述驱动装置包括底座和第一驱动组件，所述第一驱动组件包括第一末端连杆和两个第一直线驱动器，所述球体固连于所述第一末端连杆；所述确定所述球体在驱动装置坐标系下的第一坐标，包括：

[0144] 根据两个所述第一直线驱动器分别与所述底座铰接的第一铰接位置、以及两个所述第一直线驱动器通过所述第一末端连杆铰接的第二铰接位置构建第一平面参考三角形；根据所述第一平面参考三角形中两个所述第一铰接位置的第一铰接坐标和每一所述第一直线驱动器的当前长度，确定所述第二铰接位置的第二铰接坐标；根据所述第二铰接坐标、所述球体与所述第二铰接位置之间的距离、以及左侧第一直线驱动器与所述第一末端连杆之间的夹角大小确定所述第一坐标，其中，所述左侧第一直线驱动器是两个所述第一直线驱动器中的与所述第一末端连杆固连的所述第一直线驱动器。

[0145] 根据各部件之间的连接关系、连接方式可知，两个第一直线驱动器分别与底座铰接的第一铰接位置，该两个第一铰接位置在驱动装置坐标系下的第一铰接坐标为已知坐标。每一第一直线驱动器的当前长度为已知长度。球体与第二铰接位置之间的距离为已知距离。左侧第一直线驱动器与第一末端连杆之间的夹角大小为已知角度大小如160°。

[0146] 根据两个第一铰接位置和两个第一直线驱动器通过第一末端连杆铰接的第二铰接位置构建第一平面参考三角形。示例地，如图9所示，以两个第一铰接位置A1和B1，以及第二铰接位置C1构建第一平面参考三角形(A1、B1、C1)。

[0147] 根据第一平面参考三角形中两个第一铰接位置的第一铰接坐标和每一第一直线驱动器的当前长度，通过三角形的几何运算，可确定第二铰接位置在驱动装置坐标系下的第二铰接坐标。

[0148] 此处应说明的是，已知三角形三边长和两个点坐标，可求第三个顶点坐标，如图9中的C1点坐标。原理推导过程如下所示：

[0149] 首先，参见图10创建xoy、x’o’y’、x”o”y”三个坐标系。

[0150] 接着，从图10可看出C点相对于以x’oy’组成的坐标系的坐标为(acosθ，asinθ)。

[0151] 接着，求解x’oy’坐标系的规范正交基，得到线性变化矩阵T。解法1为：确定x’的单→ → → → →位向量 设b ＝(m,n)，a ⊥b ，则a *b ＝0，则可得出
其中， 根据情况取舍m值，即 线性变化矩
阵T则为

[0152] 解法2：根据旋转角度α的旋转矩阵为 如果α为0°，则其中 那么

[0153] 假设到xoy坐标系的平移向量为A，那么根据T求得xoy坐标系下的C点坐标为[0154] 继续参见图9，根据得到的第二铰接坐标C1、球体(如图9中的D1)与第二铰接位置之间的距离、以及左侧第一直线驱动器与第一末端连杆之间的夹角大小160°可确定球体在驱动装置坐标系下的第一坐标。例如，参见图11，球体(如图11中的D)在驱动装置坐标系下的第一坐标为 其中，

[0155] 可选地，所述万向节还包括球套，所述球套固连于所述动平台的连杆的第一端，所述连杆的第二端可转动地连接于所述动平台的安装板的第一连接位，所述驱动装置的第二驱动组件的输出端可转动地连接于所述安装板的第二连接位；所述方法还包括：

[0156] 以所述第二连接位为原点构建所述动平台坐标系。

[0157] 参见图12，以第二连接位(图12中的C点)为原点O3构建动平台坐标系X3Y3Z3。

[0158] 所述确定所述球体在动平台坐标系下的第二参数坐标，包括：

[0159] 根据所述第一连接位、所述第二连接位以及所述球体确定第二平面参考三角形；根据所述第二平面参考三角形的几何关系确定所述第二参数坐标。

[0160] 参见图13，根据第一连接位Pt3、第二连接位Pt2以及球体Pt1确定第二平面参考三角形(Pt3、Pt2、Pt1)，根据第二平面参考三角形的几何关系确定第二参数坐标为[0161] 可选地，所述第二驱动组件包括第二末端连杆、连接件以及两个第二直线驱动器，所述连接件的第一端可转动地连接于所述第二末端连杆，所述第二驱动组件的输出端表征所述连接件的第二端；

[0162] 所述根据所述驱动装置和所述动平台之间的部分连接关系，确定目标节点，包括：

[0163] 将两个所述第二直线驱动器通过所述第二末端连杆铰接的第三铰接位置确定为第一目标节点；将所述连接件的第一端与所述第二末端连杆连接的第一连接位置确定为第二目标节点；将所述第二驱动组件的输出端与所述安装板连接的第二连接位置确定为第三目标节点。

[0164] 可选地，所述根据所述目标节点的旋转角度参数构建所述驱动装置坐标系和所述动平台坐标系之间的转换矩阵，包括：分别以所述第一目标节点、所述第二目标节点以及所述第三目标节点为原点构建坐标系，得到串联坐标系；根据所述串联坐标系构建所述转换矩阵。

[0165] 由于每组驱动组件中因左侧直线驱动器与末端连杆固连、右侧直线驱动器通过末端连杆与左侧直线驱动器铰接，所以直线驱动器长度变化会导致直线驱动器的铰接位置处夹角的角度变化，因此，本公开实施例中，将第一目标节点处作为一旋转轴(x1，y1，z1)参与坐标变换过程计算。

[0166] 示例地，参见图14，以第一目标节点为原点的坐标系为图14中的坐标系x0y0z0，以第二目标节点为原点的坐标系为图14中的坐标系x2y2z2，以第三目标节点为原点的坐标系为图14中的坐标系x3y3z3。

[0167] 一种实施方式，参见图12和图14，以第二驱动组件所在的平面向第一驱动组件所在的平面的垂直方向为视角，将驱动装置坐标系即图12中的xyz坐标系、旋转轴坐标系x1y1z1、以第一目标节点为原点构建的坐标系x0y0z0、以第二目标节点为原点构建的坐标系x2y2z2、以及以第三目标节点为原点构建的坐标系x3y3z3映射为如图15所示的坐标系。

[0168] 根据图15所示的串联坐标系，可通过如下过程构建出驱动装置坐标系和动平台坐标系之间的转换矩阵T。

[0169] θ1、θ2、θ3分别为第一目标节点的旋转角度、第二目标节点的旋转角度、第三目标节点的旋转角度，T＝Tx*T1*T2*T3，其中，

[0170] 那么

[0171] 根据 第一坐 标 第 二参 数坐标 以及 转换矩阵之间的关系即
T*P＝D，确定旋转角度参数对应的旋转角度大小。具体过程为：

[0172] 根据T*P＝D，得到根据 得到
2 2
设 根据sinθ3+cosθ3 ＝1，考虑到
应用反三角函数求解角度值，θ3的取值范围为 固可将sinθ3设为x，而不是cosθ
3。那么 根据求根公式 计算x，
根据 情 况取 值 为 将
带入公式①，可求得θ2，θ2的初始值为90°。θ1由三角形两边
长及夹角已经确定可知，θ1＝90‑20‑θ＝70‑θ，其中，

[0173] 将求得的θ1、θ2、θ3带入得到转换矩阵。

[0174] 此处需说明的是，驱动装置坐标系和动平台坐标系之间的平移向量可通过对图16所示的第三平面参考三角形进行求解得到，求解过程类似于对第一平面参考三角形进行求解的过程，此处不再赘述。

[0175] 可选地，所述在所述动平台的所述当前位姿与目标位姿不相同的情况下，确定用于控制所述驱动装置的目标控制量，包括：

[0176] 根据所述目标位姿中的目标姿态矩阵、所述球体在所述动平台坐标系下的第一目标坐标确定所述球体在所述驱动装置坐标系下的第二目标坐标；根据所述第二目标坐标、两个所述第一铰接坐标、以及所述左侧第一直线驱动器与所述第一末端连杆之间的夹角大小确定所述左侧第一直线驱动器的第一目标长度；根据所述左侧第一直线驱动器的当前长度和所述第一目标长度，确定所述左侧第一直线驱动器的第一目标控制量。

[0177] 示例地，目标姿态矩阵T(3*3) 和位置向量(3*1) V，满足方程其中x表征第一驱动组件所在平面与第二驱动组件所在平
面之间的距离，该距离为已知值如36单位长度。可得到 该方程对
应的几何图形如图17所示。通过解方程可知道m和n的值，进而知道球体在驱动装置坐标系下的第二目标坐标。

[0178] 进一步地，参见图18，根据第二目标坐标、两个第一铰接坐标、以及左侧第一直线驱动器与第一末端连杆之间的夹角大小如160°可确定左侧第一直线驱动器的第一目标长度。计算原理如下：

[0179] 参 见图1 8 ，已 知e 、d 长度 和a ‑d 夹角 16 0 °，解 三 角形 ，可 得即得到左侧第一直线驱动器的第一目标长度。根据左侧第一直线驱动器的当前长度和其第一目标长度，可确定左侧第一直线驱动器的第一目标控制量。

[0180] 可选地，所述在所述动平台的所述当前位姿与目标位姿不相同的情况下，确定用于控制所述驱动装置的目标控制量，包括：

[0181] 根据所述左侧第一直线驱动器的所述第一目标长度、两个所述第一铰接坐标、以及所述球体与所述第二铰接位置之间的距离确定所述右侧第一直线驱动器的第二目标长度；根据所述右侧第一直线驱动器的当前长度和所述第二目标长度，确定所述右侧第一直线驱动器的第二目标控制量。

[0182] 示例地，继续参见图18，可知设
2 2 2
则 根据sinθ +cosθ ＝1，将cosθ设为x，整理得到x ‑
2 2
2kcosβ·x+k‑s＝0，
即得到右侧第一直线驱动器的第二目标长度。根据右侧第一
直线驱动器的当前长度和其第二目标长度，确定右侧第一直线驱动器的第二目标控制量。

[0183] 可选地，所述万向节包括固连于所述球体的连接柱，所述球体通过所述连接柱连接于所述第一末端连杆，所述万向节的球套包括用于容纳所述球体的容纳槽和供所述连接柱贯穿的开口，所述开口的边缘形成能够抵接所述连接柱的限位部；

[0184] 在根据所述目标节点的所述旋转角度大小确定所述当前姿态矩阵之前，还包括：

[0185] 基于根据所述目标节点的所述旋转角度大小判断所述球体的当前轴线与所述球体的参考轴线之间的夹角是否小于预设阈值。其中，参考轴线为图6所示的连接柱的对称轴线。

[0186] 所述根据所述目标节点的所述旋转角度大小确定所述当前姿态矩阵，包括：在所述夹角小于所述预设阈值的情况下，根据所述目标节点的所述旋转角度大小确定所述当前姿态矩阵。其中预设阈值为25°等经验值。

[0187] 本公开的上述位姿控制方法，充分体现和整合了每个引导架节点由运动过程带来的位移和角度偏差，通过严格的参数标定，可达到预期的运动学位姿高精度要求。并且一方面能方便的对关节机械运动范围限制条件进行预先控制，如控制球体的当前轴线与球体的参考轴线之间的夹角小于或等于25°，这避免了因正向运动学由直线驱动器的任意长度计算出的关节旋转角度超出装置机械旋转临界值如25°临界值而导致的设备部件碰撞、咬合甚至损坏设备机械及电气部件的问题，另一方面还简化数据处理过程，避免多解情况。

[0188] 图19是根据本公开一示例性实施例示出的一种位姿控制装置的框图，该装置应用于引导架，所述引导架包括动平台和用于驱动所述动平台的位姿变化的驱动装置。如图19所示，所述装置包括：

[0189] 第一确定模块1901，用于确定所述动平台的当前位姿；

[0190] 第二确定模块1902，用于在所述动平台的所述当前位姿与目标位姿不相同的情况下，确定用于控制所述驱动装置的目标控制量；

[0191] 驱动模块1903，用于根据所述目标控制量驱动所述驱动装置，以使所述动平台从所述当前位姿到达所述目标位姿。

[0192] 采用这种装置，通过确定引导架的动平台的当前位姿，并在动平台的当前位姿与目标位姿不相同的情况下，确定用于控制引导架的驱动装置的目标控制量。根据目标控制量驱动该驱动装置，以使引导架的动平台从当前位姿到达目标位姿。本公开这种通过引导架的主动驱动来控制引导架上的动平台位姿的方式，与相关技术中的以机械臂的主动驱动来控制引导架移动至目标位置的方式相比，本公开这种方式因引导架的主动驱动范围较小而不容易在驱动过程中造成磕碰，所以本公开这种位姿控制的方式更加安全。

[0193] 可选地，所述引导架还包括万向节，所述万向节包括球体；所述当前位姿包括当前姿态矩阵，所述第一确定模块1901，包括：

[0194] 第一确定子模块，用于确定所述球体在驱动装置坐标系下的第一坐标；

[0195] 第二确定子模块，用于确定所述球体在动平台坐标系下的第二参数坐标；

[0196] 第三确定子模块，用于根据所述驱动装置和所述动平台之间的部分连接关系，确定目标节点；

[0197] 构建子模块，用于根据所述目标节点的旋转角度参数构建所述驱动装置坐标系和所述动平台坐标系之间的转换矩阵，所述转换矩阵包括各所述目标节点的旋转角度参数的三角函数；

[0198] 第四确定子模块，用于根据所述第一坐标、所述第二参数坐标、以及所述转换矩阵之间的关系，确定所述旋转角度参数对应的旋转角度大小；

[0199] 第五确定子模块，用于根据所述目标节点的所述旋转角度大小确定所述当前姿态矩阵。

[0200] 可选地，所述驱动装置包括底座和第一驱动组件，所述第一驱动组件包括第一末端连杆和两个第一直线驱动器，所述球体固连于所述第一末端连杆；

[0201] 所述第一确定子模块，用于：根据两个所述第一直线驱动器分别与所述底座铰接的第一铰接位置、以及两个所述第一直线驱动器通过所述第一末端连杆铰接的第二铰接位置构建第一平面参考三角形；根据所述第一平面参考三角形中两个所述第一铰接位置的第一铰接坐标和每一所述第一直线驱动器的当前长度，确定所述第二铰接位置的第二铰接坐标；根据所述第二铰接坐标、所述球体与所述第二铰接位置之间的距离、以及左侧第一直线驱动器与所述第一末端连杆之间的夹角大小确定所述第一坐标，其中，所述左侧第一直线驱动器是两个所述第一直线驱动器中的与所述第一末端连杆固连的所述第一直线驱动器。

[0202] 可选地，所述万向节还包括球套，所述球套固连于所述动平台的连杆的第一端，所述连杆的第二端可转动地连接于所述动平台的安装板的第一连接位，所述驱动装置的第二驱动组件的输出端可转动地连接于所述安装板的第二连接位；所述装置还包括：

[0203] 构建模块，用于以所述第二连接位为原点构建所述动平台坐标系；

[0204] 所述第二确定子模块，用于：根据所述第一连接位、所述第二连接位以及所述球体确定第二平面参考三角形；根据所述第二平面参考三角形的几何关系确定所述第二参数坐标。

[0205] 可选地，所述第二驱动组件包括第二末端连杆、连接件以及两个第二直线驱动器，所述连接件的第一端可转动地连接于所述第二末端连杆，所述第二驱动组件的输出端表征所述连接件的第二端；

[0206] 所述第三确定子模块，用于：将两个所述第二直线驱动器通过所述第二末端连杆铰接的第三铰接位置确定为第一目标节点；将所述连接件的第一端与所述第二末端连杆连接的第一连接位置确定为第二目标节点；将所述第二驱动组件的输出端与所述安装板连接的第二连接位置确定为第三目标节点。

[0207] 可选地，所述构建子模块，用于：分别以所述第一目标节点、所述第二目标节点以及所述第三目标节点为原点构建坐标系，得到串联坐标系；根据所述串联坐标系构建所述转换矩阵。

[0208] 可选地，所述第二确定模块1902，包括：

[0209] 第一执行模块，用于根据所述目标位姿中的目标姿态矩阵、所述球体在所述动平台坐标系下的第一目标坐标确定所述球体在所述驱动装置坐标系下的第二目标坐标；

[0210] 第二执行模块，用于根据所述第二目标坐标、两个所述第一铰接坐标、以及所述左侧第一直线驱动器与所述第一末端连杆之间的夹角大小确定所述左侧第一直线驱动器的第一目标长度；

[0211] 第三执行模块，用于根据所述左侧第一直线驱动器的当前长度和所述第一目标长度，确定所述左侧第一直线驱动器的第一目标控制量。

[0212] 可选地，所述第二确定模块1902，包括：

[0213] 第四执行模块，用于根据所述左侧第一直线驱动器的所述第一目标长度、两个所述第一铰接坐标、以及所述球体与所述第二铰接位置之间的距离确定所述右侧第一直线驱动器的第二目标长度；

[0214] 第五执行模块，用于根据所述右侧第一直线驱动器的当前长度和所述第二目标长度，确定所述右侧第一直线驱动器的第二目标控制量。

[0215] 可选地，所述万向节包括固连于所述球体的连接柱，所述球体通过所述连接柱连接于所述第一末端连杆，所述万向节的球套包括用于容纳所述球体的容纳槽和供所述连接柱贯穿的开口，所述开口的边缘形成能够抵接所述连接柱的限位部；

[0216] 所述装置还包括：判断模块，用于在根据所述目标节点的所述旋转角度大小确定所述当前姿态矩阵之前，基于根据所述目标节点的所述旋转角度大小判断所述球体的当前轴线与所述球体的参考轴线之间的夹角是否小于预设阈值；

[0217] 所述第五确定子模块，用于：在所述夹角小于所述预设阈值的情况下，根据所述目标节点的所述旋转角度大小确定所述当前姿态矩阵。

[0218] 关于上述实施例中的装置，其中各个模块执行操作的具体方式已经在有关该方法的实施例中进行了详细描述，此处将不做详细阐述说明。

[0219] 图20是根据本公开一示例性实施例示出的一种控制器700的框图。如图20所示，该控制器700可以包括：处理器701，存储器702。该控制器700还可以包括多媒体组件703，输入/输出(I/O)接口704，以及通信组件705中的一者或多者。

[0220] 其中，处理器701用于控制该控制器700的整体操作，以完成上述的位姿控制方法中的全部或部分步骤。存储器702用于存储各种类型的数据以支持在该控制器700的操作，这些数据例如可以包括用于在该控制器700上操作的任何应用程序或方法的指令，以及应用程序相关的数据，例如联系人数据、收发的消息、图片、音频、视频等等。该存储器702可以由任何类型的易失性或非易失性存储设备或者它们的组合实现，例如静态随机存取存储器(Static Random Access Memory，简称SRAM)，电可擦除可编程只读存储器(Electrically Erasable Programmable Read‑Only Memory，简称EEPROM)，可擦除可编程只读存储器(Erasable Programmable Read‑Only Memory，简称EPROM)，可编程只读存储器(Programmable Read‑Only Memory，简称PROM)，只读存储器(Read‑Only Memory，简称ROM)，磁存储器，快闪存储器，磁盘或光盘。多媒体组件703可以包括屏幕和音频组件。其中屏幕例如可以是触摸屏，音频组件用于输出和/或输入音频信号。例如，音频组件可以包括一个麦克风，麦克风用于接收外部音频信号。所接收的音频信号可以被进一步存储在存储器702或通过通信组件705发送。音频组件还包括至少一个扬声器，用于输出音频信号。I/O接口704为处理器701和其他接口模块之间提供接口，上述其他接口模块可以是键盘，鼠标，按钮等。这些按钮可以是虚拟按钮或者实体按钮。通信组件705用于该控制器700与其他设备之间进行有线或无线通信。无线通信，例如Wi‑Fi，蓝牙，近场通信(Near Field Communication，简称NFC)，2G、3G、4G、NB‑IOT、eMTC、或其他5G等等，或它们中的一种或几种的组合，在此不做限定。因此相应的该通信组件705可以包括：Wi‑Fi模块，蓝牙模块，NFC模块等等。

[0221] 在一示例性实施例中，控制器700可以被一个或多个应用专用集成电路(Application Specific Integrated Circuit，简称ASIC)、数字信号处理器(Digital Signal Processor，简称DSP)、数字信号处理设备(Digital Signal Processing Device，简称DSPD)、可编程逻辑器件(Programmable Logic Device，简称PLD)、现场可编程门阵列(Field Programmable Gate Array，简称FPGA)、控制器、微控制器、微处理器或其他电子元件实现，用于执行上述的位姿控制方法。

[0222] 在另一示例性实施例中，还提供了一种包括程序指令的计算机可读存储介质，该程序指令被处理器执行时实现上述的位姿控制方法的步骤。例如，该计算机可读存储介质可以为上述包括程序指令的存储器702，上述程序指令可由控制器700的处理器701执行以完成上述的位姿控制方法。

[0223] 在另一示例性实施例中，还提供一种计算机程序产品，该计算机程序产品包含能够由可编程的装置执行的计算机程序，该计算机程序具有当由该可编程的装置执行时用于执行上述的位姿控制方法的代码部分。

[0224] 以上结合附图详细描述了本公开的优选实施方式，但是，本公开并不限于上述实施方式中的具体细节，在本公开的技术构思范围内，可以对本公开的技术方案进行多种简单变型，这些简单变型均属于本公开的保护范围。

[0225] 另外需要说明的是，在上述具体实施方式中所描述的各个具体技术特征，在不矛盾的情况下，可以通过任何合适的方式进行组合。为了避免不必要的重复，本公开对各种可能的组合方式不再另行说明。

[0226] 此外，本公开的各种不同的实施方式之间也可以进行任意组合，只要其不违背本公开的思想，其同样应当视为本公开所公开的内容。