一种多构型绳索驱动并联机器人及其空间位姿求解方法与流程

本发明涉及一种绳索驱动并联机器人领域，特别是关于一种多构型绳索驱动并联机器人及其空间解算位姿解算方法。

背景技术：

并联机器人具有工作空间大、负载能力强、快速可重构、模块化设计等优点，绳索驱动的优点使得目标工作平台的工作范围更大、运动速度更快，正在被广泛运用到各种操作机构中，通过控制绳索的输出长度来控制动平台在工作空间中的位置姿态；在驱动过程中不仅需要精确控制绳索的输出长度，同时需要检测绳索上拉力；现有的绳索驱动的并联机构的驱动装置中多不能集成测量装置，传统测量方法是通过将绳索与拉力传感器串联来进行测量。但是应用范围窄，测量效果差，影响线路布局。当绳索数目大于动平台自由度时，其运动学模型方程为冗余的非线性方程，不能得到显式的空间位姿正反解，空间位姿解算非常复杂，且难以获得实时且精确的位姿解。

国内专利申请号为201410146203.1的专利公开了一种具有张紧力测量功能的高精度卷索装置，该装置由设置在滑台上的驱动装置和固定在平台上的测量装置组成，其测量装置结构复杂，并且需要通过另外一台电机来控制移动卷筒，造价成本更高。其自身不具有绳索导引功能，需要依赖于其他导引装置来引导绳索牵引方向；

国内专利申请号为201110026500.9的专利公开了一种能实现快速重构的绳索驱动并联机器人，该装置有多组驱动模块、换向装置和输出装置组成，但是由于没有实时测量拉力，所以控制精度较低，且结构复杂，安装维护困难，造价成本更高。

技术实现要素：

本发明为了解决上述现有技术存在的不足之处，提供一种多构型绳索驱动并联机器人及其空间位姿求解方法，以期能利用空间姿态解算方法求解得到驱动绳索长度与动平台在工作空间中的位置姿态，从而精确控制动平台在绳索驱动并联机器人工作空间中的位姿，进而提高绳索驱动并联机器人的动平台控制精度，完成在工作空间内的工作任务。

为解决上述技术问题，本发明采用如下技术方案：

本发明一种多构型绳索驱动并联机器人的特点包括：动平台、若干个绳索驱动装置、主体机架和若干绳索；

根据任务需要在所述主体机架的任意位置上设置有所述绳索驱动装置；所述绳索驱动装置通过所述柔性绳索与所述动平台相连，并利用空间位姿求解方法获得所述动平台在所述主体机架的内部空间中的位姿，从而利用所述柔性绳索牵引所述动平台达到所述主体机架内的相应位置，并改变所述动平台的相应姿态。

本发明所述的多构型绳索驱动并联机器人的特点也在于，所述动平台包括：若干个安装平板、若干个型材支架和绳索固定装置；

在任意一个安装平板上表面和下表面上均匀分布有若干个带有螺纹孔的圆形凹槽；任意两个安装平板之间通过所述型材支架进行连接固定；在所述圆形凹槽上设置有所述绳索固定装置；

所述绳索固定装置包括：可旋转圆环、外壳、轴承、旋转底座、固定底座；

所述可旋转圆环固定在所述旋转底座的顶部；所述旋转底座的外侧通过所述轴承与所述外壳相连接；所述外壳的直径与所述安装平板上圆形凹槽的直径相匹配；在所述旋转底座的底部设置有所述固定底座；在所述固定底座的中间位置上设置有螺纹通孔，所述螺纹通孔与所述圆形凹槽上的螺纹孔相对应。

所述绳索驱动装置包括：一对主体支架、一对支撑杆、绳索、绳索导引机构、万向导出机构、实时拉力测量机构和伺服驱动装置；

所述绳索导引机构包括：一对支撑光轴、刻槽卷筒、传动丝杠、移动滑台、直线轴承、同步带、同步带从动轮与同步带主动轮；

所述实时拉力测量机构包括：滑轮、拉压力测量传感器、滑轮支架、拉压力传感器支架以及旋转支架；

所述伺服驱动装置包括：减速机和伺服电机；

在所述一对主体支架之间的中心位置处设置有所述刻槽卷筒，在所述刻槽卷筒的下方设置有所述一对支撑杆；在所述刻槽卷筒的端轴上设置有所述同步带主动轮；在所述刻槽卷筒的上方设置有一对支撑光轴；在所述一对支撑光轴的上方之间设置有所述传动丝杠；在所述传动丝杠的端轴上设置有同步带从动轮；所述同步带从动轮与所述同步带主动轮之间通过所述同步带连接，并位于所述一对主体支架的前端支架上；在所述传动丝杠和一对支撑光轴上设置有移动滑台；在所述移动滑台的两侧通过直线轴承与所述一对支撑光轴相连接；

在所述移动滑台上通过所述拉压力传感器支架设置有拉压力测量传感器，且所述拉压力测量传感器的水平面与所述移动滑台的安装平面平行；在所述移动滑台的前端面，且与所述安装平面垂直的位置上设置有旋转支架；所述旋转支架与所述滑轮支架之间通过轴销连接；在所述滑轮支架的上端处设置有上通孔，所述上通孔通过轴销与所述拉压力测量传感器相连；在所述滑轮支架的下端处设置有下光孔，所述下通孔通过轴销与所述滑轮相连；在所述刻槽卷筒上缠绕有所述绳索，所述绳索垂直向上绕过所述滑轮后水平延伸至所述一对主体支架的前端支架外侧；

在所述一对主体支架的后端支架外侧设置有减速机；所述减速机的输出轴通过轴键与所述刻槽卷筒的输入轴相连；所述减速机的输入轴通过联轴器与所述伺服电机相连；

所述伺服电机驱动所述减速机顺时针或逆时针运动，并一起带动所述刻槽卷筒同步旋转，使得所述同步带主动轮通过同步带带动所述传动丝杠同步旋转，由所述传动丝杠驱动所述移动滑台沿着所述一对支撑光轴轴向前后运动，从而利用所述滑轮实现所述绳索能在所述刻槽卷筒上进行缠绕或释放。

在所述一对主体支架的前端支架上设置有万向导出机构；所述万向导出机构包括：万向旋转支架、轴销和导向滑轮，

在所述万向旋转支架上通过轴销与所述导向滑轮相连接；

在所述万向旋转支架的底部设置有旋转轴，所述旋转轴的中心设有绳索导入孔；所述绳索从所述绳索导入孔中穿出并经过所述导向滑轮实现万向导出。

本发明一种利用所述的多构型绳索驱动并联机器人的空间位姿求解方法的特点是按如下步骤进行：

步骤1、利用式(1)建立所述动平台的空间位姿和绳长的运动学模型：

^bli＝^bbi-^bx-^brp^ppi(1)

式(1)中，^bli表示主体机架坐标系b中连接第i个绳索驱动装置与所述动平台上第i个绳索固定装置的柔性绳索的长度；^bbi＝[^bbi,x^bbi,y^bbi,z]^t表示主体机架坐标系b中第i个绳索驱动装置的位置坐标，^bx＝[^pxi,x^pxi,y^pxi,z^pxi,α^pxi,β^pxi,γ]^t表示主体机架坐标系b中所述动平台在内部空间中的空间位置和姿态，^brp表示主体机架坐标系b中所述动平台相对于主体机架的空间旋转矩阵，^ppi＝[^ppi,x^ppi,y^ppi,z]^t表示动平台局部坐标系p中第i个绳索固定装置的位置坐标；并有：^bpi＝^brp^ppi，表示动平台局部坐标系p中第i个绳索固定装置在主体机架坐标系b中的位置；且^bpi＝[^bpi,x^bpi,y^bpi,z]^t；i＝1…m；m表示绳索驱动装置的总数；

步骤2、根据所述动平台在所述主体机架坐标系b中的空间位置和姿态^bx对所述运动学模型进行反解，得到第i个绳索驱动装置与相应的所述动平台上第i个绳索固定装置的柔性绳索的长度li；

步骤2.1、利用式(2)得到所述第i个导向滑轮绕万向旋转支架旋转轴旋转的夹角为θi：

步骤2.2、利用式(3)得到所述第i个导向滑轮的旋转中心点在主体机架上的位姿^bmi＝[^bmi,x^bmi,y^bmi,z]^t：

式(3)中，表示第i个导向滑轮绕万向旋转支架上z轴旋转θi角时的旋转矩阵；ρ表示所述导向滑轮的半径；^bex表示主体机架坐标系b中x轴的单位向量；

步骤2.3、利用式(4)得到第i个绳索在第i个导出滑轮上的缠绕角度αi：

αi＝π-(αi,1+αi,2)(4)

式(4)中，αi,1表示第i个导向滑轮绕万向旋转支架上z轴与动平台的夹角，αi,2表示第i个绳索与动平台的夹角，并有：

步骤2.4、利用式(6)得到主体机架坐标系b中第i个导向滑轮的绳索引出位置^bsi：

步骤2.5、利用式(7)得到第i个绳索驱动装置与所述动平台上第i个绳索固定装置的柔性绳索的长度li：

li＝ρ·(π-αi)+||^bsi-^bpi||2(7)

步骤3、根据所述第i个绳索驱动装置与所述动平台上第i个绳索固定装置的柔性绳索的长度li对所述运动学模型进行正解，得到所述主体机架坐标系b中所述动平台在内部空间中的空间位置和姿态^bx；

步骤3.1、对所述运动学模型进行求导，得到绳长变化的雅克比矩阵jinv(l(t))：

式(8)中，l表示绳长向量，并有l＝[^bl1^bl2…^bli…^blm]；l(t)表示绳长向量l对时间t的函数；^bp＝^bx表示主体机架坐标系b中动平台在内部空间中的空间位置和姿态；并有：动平台的位姿变化的雅克比矩阵为jforw(^bp(t))＝jinv(l(t))^-1；^bp(t)表示空间位置和姿态对时间t的函数；

步骤3.2、利用式(9)得到第i个柔性绳索长度li的运动学正解方程：

步骤3.3对所述运动学正解方程进行优化求解，得到第k次迭代下主体机架坐标系b中动平台在内部空间中的空间位置和姿态^bpk：

步骤3.3.1、定义收敛参数为β、σ和μ0，并初始化β,σ∈(0,1)，μ0＞0；

定义迭代次数为k；并初始化k＝0；

定义容许误差为ε，且0＝ε＝1；

定义初始位姿点为

步骤3.3.2、根据式(8)计算第k次迭代下动平台的位姿变化的雅克比矩阵jforw(^bpk(t))，并判断||jforw(pk(t))||≤ε是否成立，若成立，则停止计算，返回第k次迭代下主体机架坐标系b中动平台在内部空间中的空间位置和姿态^bpk作为位姿的正解点，否则执行步骤3.3.3；

步骤3.3.3、求解式(10)，得到第k次迭代的步长dk：

(jforw(pk(t))^tjforw(pk(t))+μki)dk＝-jforw(pk(t))^tfuc(^bpk)(10)

式(10)中，μk表示第k次迭代的收敛参数；i表示对角矩阵；

步骤3.3.4、令第k次迭代的最小非负整数mk满足式(11)：

步骤3.3.5、将赋值给ωk后，得到^bpk+1＝^bpk+ωkdk；

步骤3.3.6、利用式(12)得到第k+1次迭代的收敛参数μk+1后，返回步骤3.3.2：

式(12)中，rk表示收敛参数μk取值的判定条件，τ1表示判定条件rk的下限；τ2表示判定条件rk的上限；δ1表示收敛参数μk的第一比例因子；δ2表示收敛参数μk的第一比例因子；τ1∈(0,0.5)，τ2＝1-τ1；δ1∈(0,1)；δ2＝100δ1；并有：

与已有技术相比，本发明的有益效果体现在：

1、本发明的多构型绳索驱动并联机器人，采用模块化设计的高精度绳索驱动装置、动平台以及主体机架，模块化设计的高精度绳索驱动装置具有实时绳索输出拉力测量装置，结合万向导出装置可以根据任务需求在主体机架上任何位置布置绳索输出装置，重新构造新的绳索驱动机器人构型，从而提高了绳索驱动机器人的利用率以及经济效益，同时，提供的空间位姿解算方法解决了现有绳索驱动并联机器人不能实时求解空间位姿的难题，保证了绳索驱动机器人的位姿控制精度。

2、本发明采用模块化设计的高精度绳索驱动装置和动平台能够根据实际任务需要来进行布置安装，绳索驱动装置上设置有绳索导引机构，能够将绳索顺序且均匀地缠绕在卷筒上或者从卷筒上释放，保证了绳索的输出或者回收的速度与电机速度恒定，提高了绳索的控制精度。

3、本发明绳索驱动装置的绳索导引机构上设置有根据杠杆原理设计的实时拉力测量装置，该装置实施方便，结构简单；绳索在卷筒缠绕或者释放时实时拉力测量装置辅助绳索导引机构将绳索在刻槽卷筒上缠绕或者释放，不会与绳索脱落，实时反馈绳索上拉力；

4、本发明绳索驱动装置前端输出位置设置有万向导出装置，万向导出装置的滑轮引导绳索绕自身轴旋转并导向空间任意位置，不需要其他辅助导引机构来设置绳索的输出位置，提高了机器人的可重构性和绳索输出的精度；

5、本发明提供了空间位姿求解方法包括机器人运动学模型的空间位姿正解和空间位姿反解方法，空间位姿反解方法是基于本发明使用的高精度绳索驱动装置，有效提高了绳索的输出控制精度；

6、本发明利用优化算法来对运动学正解方程进行求解，保证了能够获得工作空间中的全局最优解和高精度的位姿解；并提高了模型的精度，降低了方程的复杂度。

附图说明

图1为本发明多构型绳索驱动并联机器人示意图；

图2为本发明交叉连接构型绳索驱动并联机器人的示意图；

图3为本发明顺序连接构型动平台示意图；

图4为本发明交叉连接构型动平台示意图；

图5为本发明多构型绳索驱动并联机器人的动平台安装示意图；

图6为本发明多构型绳索驱动并联机器人的高精度绳索驱动装置示意图；

图7为本发明多构型绳索驱动并联机器人的运动学模型坐标系示意图；

图8a为本发明多构型绳索驱动并联机器人的万向导出滑轮局部坐标系左视图；

图8b为本发明多构型绳索驱动并联机器人的万向导出滑轮局部坐标系上视图；

图中标号：安装平板1200、型材支架1100、可旋转圆环1301、外壳1302、轴承1303、旋转底座1304、固定底座1305、支架2101、支撑杆2102、绳索2103、同步带主动轮2201、同步带2202、同步带从动轮2203、支撑光轴2204、刻槽卷筒2205、直线轴承2206、移动滑台2207、传动丝杠2208、滑轮2301、滑轮支架2302、旋转支架2303、拉压力传感器2304、轴销2305、拉压力传感器支架2306、减速机2401、伺服电机2402、万向旋转支架2501、轴销2502、万向导出滑轮2503、主体机架3000；

具体实施方式

本实施例中，一种多构型绳索驱动并联机器人包括：动平台1000、若干个绳索驱动装置2000、主体机架3000和若干绳索4000；

根据任务需要在所述主体机架3000的任意位置上设置有所述绳索驱动装置2000；如图1所示，动平台1000位于主体机架3000的内部空间中，可以在内部空间中任意改变位置姿态；绳索驱动装置2000通过所述柔性绳索4000与所述动平台1000相连，并利用空间位姿求解方法获得所述动平台1000在所述主体机架3000的内部空间中的位姿，从而利用所述柔性绳索4000牵引所述动平台1000达到所述主体机架3000内的相应位置，并改变所述动平台1000的相应姿态。柔性绳索4000与动平台1000的连接方式可以根据任务需求进行调整，如图2所示的是一种交叉连接构型的绳索驱动并联机器人；绳索驱动装置的数目也可以根据实际任务需要进行调整，绳索驱动装置利用自身绳索4000牵引动平台在机体框架3000内的空间任意位置上运动；动平台上可以附加通用操作设备，通过移动操作动平台来移动操作设备完成工作任务。如图3所示，绳索固定装置1300可以安装在安装平板1200反面，构成反向交叉形式；

如图4所示，多构型绳索驱动并联机器人的动平台1000包括：若干个安装平板1200、若干个型材支架1100和绳索固定装置1300；

在任意一个安装平板1200上表面和下表面上均匀分布有若干个带有螺纹孔的圆形凹槽；这些凹槽的位置不固定，可以根据实际需要进行配置；任意两个安装平板之间通过型材支架1100进行连接固定，型材支架的尺寸可以调整动平台的尺寸来获得需要的运动性能；在圆形凹槽上设置有绳索固定装置1300；

如图5所示，绳索固定装置包括：可旋转圆环1301、外壳1302、轴承1303、旋转底座1304、固定底座1305；

可旋转圆环1301固定在旋转底座1304的顶部；旋转底座1304的外侧通过轴承1303与外壳1302相连接；外壳1302的直径与安装平板1200上圆形凹槽的直径相匹配，可以提高固定底座1305的定位精度；外壳上也可以设置螺纹，同时在安装平板上设置螺纹，这样就可以省去在固定底座1305设置固定用的螺纹；在旋转底座1304的底部设置有固定底座1305；在固定底座1305的中间位置上设置有螺纹通孔，螺纹通孔与圆形凹槽上的螺纹孔相对应；固定底座设置的固定螺纹数目根据实际需要进行调整，以满足不同任务负载的要求；

如图6所示，多构型绳索驱动并联机器人的绳索驱动装置2000包括：一对主体支架2101、一对支撑杆2102、绳索2103、绳索导引机构2200、万向导出机构2500、实时拉力测量机构2300和伺服驱动装置2400；主体支架2101底部设置有螺纹安装孔，可以配合主体机架3000上的标准型材安装凹槽进行固定安装；固定安装的位置和姿势不固定，可以按照如图1或者图2的方式进行固定，也可以将绳索驱动装置以垂直姿态固定在主体机架3000上；具体的安装方式可以根据实际任务需要进行调整；

绳索导引机构2200包括：一对支撑光轴2204、刻槽卷筒2205、传动丝杠2208、移动滑台2207、直线轴承2206、同步带2202、同步带从动轮2203与同步带主动轮2201；支撑光轴2204和传动丝杠2208具有负载支撑的作用，同时可以牵引移动滑台2207做水平方向的移动；支撑光轴2204和传动丝杠2208的数目可以根据实际任务负载需求进行调整，也可以采用直线导轨来替代支撑光轴2204或者传动丝杠2208；

实时拉力测量机构包括：滑轮2301、拉压力测量传感器2304、滑轮支架2302、拉压力传感器支架2306以及旋转支架2303；实时拉力测量机构的工作原理是利用杠杆原理，将水平方向和竖直方向的绳索拉力通过杠杆作用传递到拉压力传感器；

万向导出机构包括：万向旋转支架2501、万向导出滑轮2503与轴销2502；

伺服驱动装置包括：减速机2401和伺服电机2402；伺服电机采用带高精度编码器的电机，可以反馈电机的转速和转动角度，并转换成绳索运动的长度反馈给上位机控制系统；减速机这里采用行星齿轮减速机，行星齿轮减速机具有减速比大，效率高，尺寸小的优点，也可以根据任务需要来设置其他减速机；

在一对主体支架2101之间的中心位置处设置有刻槽卷筒2205，刻槽卷筒上按照顺时针螺旋线铣刻了标准的圆形凹槽或者梯形凹槽，凹槽的尺寸根据实际使用的绳索直接来一一对应；刻槽卷筒可以采用经过表面硬化处理的铝合金材料制作加工，降低卷筒的转动惯量，提高能量利用效率；也可以用冶金陶瓷制作，陶瓷制作的卷筒具有表面光洁度高，摩擦力小的优点，相对于铝合金材料能够承载更大的负载；在刻槽卷筒2205的下方设置有一对支撑杆2102；在刻槽卷筒2205的端轴上设置有同步带主动轮2201；同步带传动装置主要用来传递卷筒上的力矩至移动滑台，驱动移动滑台做水平方向运动；可以采用齿轮或者其他链条或者带传动方式，但是要保证绳索在刻槽卷筒上的移动和移动滑台的移动速度同步；在刻槽卷筒2205的上方设置有一对支撑光轴2204；在一对支撑光轴2204的上方之间设置有传动丝杠2208；在传动丝杠2208的端轴上设置有同步带从动轮2203；同步带从动轮2203与同步带主动轮2201之间通过同步带2202连接，并位于一对主体支架2101的前端支架上；在传动丝杠2208和一对支撑光轴2204上设置有移动滑台2207；在移动滑台2207的两侧通过直线轴承2206与一对支撑光轴2204相连接；

在移动滑台2207上通过拉压力传感器支架2306设置有拉压力测量传感器2304，且拉压力测量传感器2304的水平面与移动滑台2207的安装平面平行；在移动滑台207的前端面，且与安装平面垂直的位置上设置有旋转支架2303；旋转支架2303与滑轮支架2302之间通过轴销连接；在滑轮支架2302的上端处设置有上通孔，上通孔通过轴销与拉压力测量传感器2304相连；在滑轮支架2302的下端处设置有下通孔，下通孔通过轴销与滑轮2301相连；在刻槽卷筒2205上缠绕有绳索2103，绳索2103垂直向上绕过滑轮2301后水平延伸至一对主体支架2101的前端支架外侧；实时拉力测量装置2300是利用杠杆原理进行测量绳索拉力，这里将旋转支架2303中间位置设置旋转用的固定通孔，上通孔连接拉压力传感器2304，下通孔连接滑轮，两个通孔距离中间位置的旋转通孔相等的距离，这样滑轮上所受的绳索拉力就等于拉压力传感器上测量得到压力；也可以根据需要改变这个杠杆力臂的长度比值，来调整拉压力传感器上所受力大小；

如图6所示，在一对主体支架2101的后端支架外侧设置有减速机2401；通过减速机2401的输出轴的轴键与刻槽卷筒2205的输入轴相连，并传递电机的扭矩，也可以通过联轴器连接；减速机2401的输入轴通过联轴器与伺服电机2402相连；

伺服电机2402驱动减速机2401顺时针或逆时针运动，并一起带动刻槽卷筒2205同步旋转，使得同步带主动轮2201通过同步带2202带动传动丝杠2208同步旋转，由传动丝杠2208驱动移动滑台2207沿着一对支撑光轴2204轴向前后运动，带动滑轮2301前后运动，从而利用滑轮2301实现绳索2103能在刻槽卷筒2205上进行顺序缠绕或释放，避免绳索直接在卷筒上缠绕时绳索之间干扰，干扰电机编码器对绳索输出长度的计算。

在一对主体支架2101的前端支架上设置有万向导出机构2500；万向导出机构包括万向旋转支架2501、轴销2502和导向滑轮2503；

在万向旋转支架2501上通过轴销2502与导向滑轮2503相连接；

在万向旋转支架2501的底部设置有旋转轴，旋转轴的中心设有绳索导入孔；绳索2103从绳索导入孔中穿出并经过导向滑轮2503实现万向导出。绳索从万向导出装置出发连接至动平台的可旋转固定环，万向导出装置可以任意改变导出方向，同时固定圆环可以绕自身轴旋转，保证了动平台位于工作空间任意位置时绳索的运动距离是可以通过空间位姿解算方法进行计算。

本实施例中，一种多构型绳索驱动并联机器人的空间位姿求解方法是按如下步骤进行：

步骤1、建立如图7所示坐标系，其中主体机架坐标系b可以建立在任意位置，表示为世界坐标系；平台局部坐标系p可以建立在动平台重心，也可以建立在动平台上的任意位置，一般选择建立在动平台的一个绳索固定点上，方便计算；利用式(1)建立动平台的空间位姿和绳长的运动学模型：

^bli＝^bbi-^bx-^brp^ppi(1)

式(1)中，^bli表示主体机架坐标系b中连接第i个绳索驱动装置与动平台上第i个绳索固定装置的柔性绳索的长度；^bbi＝[^bbi,x^bbi,y^bbi,z]^t表示主体机架坐标系b中第i个绳索驱动装置的位置坐标，^bx＝[^pxi,x^pxi,y^pxi,z^pxi,α^pxi,β^pxi,γ]^t表示主体机架坐标系b中动平台在内部空间中的空间位置和姿态，^brp表示主体机架坐标系b中动平台相对于主体机架的空间旋转矩阵，^ppi＝[^ppi,x^ppi,y^ppi,z]^t表示动平台局部坐标系p中第i个绳索固定装置的位置坐标；并有：^bpi＝^brp^ppi，表示动平台局部坐标系p中第i个绳索固定装置在主体机架坐标系b中的位置；且^bpi＝[^bpi,x^bpi,y^bpi,z]^t；i＝1…m；m表示绳索驱动装置的总数；

步骤2、根据动平台在主体机架坐标系b中的空间位置和姿态^bx对运动学模型进行反解，得到第i个绳索驱动装置与相应的动平台上第i个绳索固定装置的柔性绳索的长度li；

步骤2.1、如图8所示，为了示意准确，将3维坐标系分别投影到二维坐标空间中；滑轮绕万向导出支架旋转轴逆时针旋转为正方向，利用式(2)得到第i个导向滑轮绕万向旋转支架旋转轴旋转的夹角为θi：

步骤2.2、建立如图8(a)和如图8(b)所示的局部坐标系，利用式(3)得到第i个万向导出滑轮的中心点在主体机架上的位置^bmi＝[^bmi,x^bmi,y^bmi,z]^t：

式(3)中，^bmi表示万向导出滑轮的中心点，即滑轮自身旋转轴的中心点位置；滑轮不仅绕万向导出支架旋转轴运动，还可以绕自身旋转轴转动，其自身旋转轴轴心点跟随绳索方向改变，可通过式(3)求得；如图8所示，表示第i个导向滑轮的轮绕万向旋转支架上局部坐标系z轴旋转θi角时的旋转矩阵，即为万向导出滑轮的局部坐标系转换到主体机架坐标系的转换矩阵；ρ表示所述导向滑轮的半径；^bex表示主体机架坐标系b中x轴的单位向量；

步骤2.3、利用式(4)得到第i个绳索在第i个导出滑轮上的缠绕角度αi：

αi＝π-(αi,1+αi,2)(4)

式(4)中，αi,1表示第i个导向滑轮的局部坐标系z轴与动平台的夹角，αi,2表示第i个绳索与动平台的夹角，并有：

步骤2.4、利用式(6)得到主体机架坐标系b中第i个导向滑轮的绳索引出位置^bsi：

步骤2.5、综合以上各式，并带入运动学模型式(1)得到式(7)，从而利用式(7)得到第i个绳索驱动装置与动平台上第i个绳索固定装置的柔性绳索的长度li：

li＝ρ·(π-αi)+||^bsi-^bpi||2(7)

步骤3、根据第i个绳索驱动装置与所述动平台上第i个绳索固定装置的柔性绳索的长度li对运动学模型进行正解，得到主体机架坐标系b中动平台在内部空间中的空间位置和姿态^bx；

步骤3.1、对运动学模型进行求导，得到绳长变化的雅克比矩阵jinv(l(t))：

式(8)中，l表示绳长向量，并有l＝[^bl1^bl2…^bli…^blm]；l(t)表示绳长向量l对时间t的函数；^bp表示主体机架坐标系b中动平台在内部空间中的空间位置和姿态；并有：动平台的位姿变化的雅克比矩阵为jforw(^bp(t))＝jinv(l(t))^-1；^bp(t)表示空间位置和姿态对时间t的函数；

步骤3.2、将运动学正解的求解过程看作是优化过程，则可以将运动学正解方程写作优化目标，即得到式(9)，利用式(9)得到第i个柔性绳索长度li的运动学正解方程：

步骤3.3、根据第i个绳索驱动装置与所述动平台上第i个绳索固定装置的柔性绳索的长度li，对运动学正解方程进行优化求解，得到第k次迭代下主体机架坐标系b中动平台在内部空间中的空间位置和姿态^bpk：

步骤3.3.1、定义收敛参数为β、σ和μ0，并初始化β,σ∈(0,1)，μ0＞0；

定义迭代次数为k；并初始化k＝0；为了保证优化算法在最坏的情况下跳出迭代过程，需要给迭代次数设置一个最大上限，最大上限可以根据实际需要进行设定；在本算法求解过程中，一般不超过20次迭代就可以收敛得到非常准确的优化结果；

定义容许误差为ε，且0＝ε＝1；容许误差体现了算法结果的精度，容许误差越小，结果精度越高；如果容许误差太小，则会使迭代次数增加，算法计算时间增长；如果容许误差太大，则会使得计算结果不准确；所以在设定容许误差初值时，需要根据实际场合确定；如果需要进行实时计算运动学正解，则可以适当增大容许误差；

定义初始位姿点为在本算法中，初始位姿点可以任意选取在绳索驱动并联机器人的工作空间中；但是初始位姿点的选取会影响算法迭代的收敛速度，初始位姿点与待求解的位姿点距离较近时，可以加快算法的收敛速度；

步骤3.3.2、根据式(8)计算第k次迭代下动平台的位姿变化的雅克比矩阵jforw(^bpk(t))，并判断||jforw(pk(t))||≤ε是否成立，若成立，则停止计算，返回第k次迭代下主体机架坐标系b中动平台在内部空间中的空间位置和姿态^bpk作为位姿的正解点，否则执行步骤3.3.3；

步骤3.3.3、求解式(10)，得到第k次迭代的步长dk：

(jforw(pk(t))^tjforw(pk(t))+μki)dk＝-jforw(pk(t))^tfuc(^bpk)(10)

式(10)中，μk表示第k次迭代的收敛参数；i表示对角矩阵；

步骤3.3.4、令第k次迭代的最小非负整数mk满足式(11)：

步骤3.3.5、将赋值给ωk后，得到^bpk+1＝^bpk+ωkdk；

步骤3.3.6、利用式(12)得到第k+1次迭代的收敛参数μk+1后，返回步骤3.3.2：

式(12)中，rk表示收敛参数μk取值的判定条件，τ1表示判定条件rk的下限；τ2表示判定条件rk的上限；δ1表示收敛参数μk的第一比例因子；δ2表示收敛参数μk的第一比例因子；τ1∈(0,0.5)，τ2＝1-τ1；δ1∈(0,1)；δ2＝100δ1；判定条件并有：