CN101060196A - 基于索长/索力的巨型索系并联机器人绳索调整方法 - Google Patents

Abstract

本发明公开了一种基于索长/索力的巨型索系并联机器人绳索调整方法，主要解决大跨度索系并联机器人的精确定位问题。该方法首先通过测量仪器，标定站点坐标和塔顶坐标，进而确定出馈源舱的理论姿态角，并将绳索空间形状视为悬链线，解出每个离散点所在位置的绳索理论长度；然后通过绳索张力传感器测量出绳索张力，并计算出绳索当前的实际长度，用该绳索的实际值与理论值进行比较，得到绳索需要的调整量；最后将该调整量送入电机执行机构进行绳索的收放，牵引馈源舱运动。本发明馈源舱在2cm/s空间扫描速度下，通过对绳索长度的调整可使馈源舱的空间定位精度小于3厘米，适用于馈源舱的不同运行轨迹，特别适用于巨型索系并联机器人绳索的调整。

Description

基于索长/索力的巨型索系并联机器人绳索调整方法

                           技术领域

本发明属于机械工程，涉及工程部件，具体地说是一种并联机器人绳索长度的调整方法，可用于指导大跨度索系并联机器人高精度定位。

                           背景技术

随着科技的不断进步，天文学家需要建造有效接收面积达一平方公里的射电望远镜，将其命名为新一代大射电望远镜LT。众所周知，反射面口径直接影响着射电望远镜的接收能力，但由于地面环境因素对天线反射面精度的影响，目前全可动抛物面天线的最大口径只能做到100米左右。美国于20世纪70年代初在波多黎格建造了目前世界上最大的口径为305米的Arecibo球反射面射电天文望远镜，与传统天线不同，它的反射面铺设在KARST洼地中，天线反射面是固定不动的，而馈源则在钢索悬吊的固定背架结构上做跟踪射电源的运动。但不足之处在于：馈源的方位和俯仰运动为纯机械方式，跟踪精度不高；馈源背架结构重达1000多吨，自重大、造价高；反射面为球面，电磁波经球反射面反射汇聚于一根线，采用线馈源接受信号，因而接收带宽窄。Arecibo的接收面积仅为预计建造LT的三十分之一，如果LT工程方案采用Arecibo的设计方案，其高昂的造价是不可被接受的。为此，西安电子科技大学段宝岩教授结合现代的计算机、自动控制与激光检测技术，提出了全新的LT机电光一体化设计方案。在该设计方案中，原Arecibo大射电望远镜中重达800多吨的、固定的背架支撑结构被仅重达20吨左右的、由六根绳索拖动的馈源系统所代替，通过激光检测装置实时检测馈源系统的空间位置姿态，由计算机主动控制绳索的收放来实时调整馈源系统的位姿，以满足馈源轨迹跟踪高精度要求，如图1所示。该LT机电光一体化的设计方案同Arecibo望远镜相比：1、馈源支撑结构的重量从1000吨降到约30吨，造价随之大幅度降低，从根本上解决了Arecibo大射电望远镜设计方案造价太高的难题；2、馈源具有更大范围的运动空间，这使望远镜的观测天顶角从±20°提高到±60°。从操作功能来看，LT舱索系统属于索系并联机器人。

所谓索系并联机器人是用可动的绳索或钢丝将上下两平台通过球铰或虎克铰联接而成的、在三维空间按要求实现任意方向的移动和绕任何方向、位置的轴线转动的一类机构。由于绳索具有轻质的特点，克服了传统刚性并联机器人工作空间较小的弱点，得到众多研究者的关注，同时由于绳索是索系并联机器人的执行元件，绳索调整量合适与否，直接影响了馈源舱的定位精度，因此绳索调整方法是LT中的关键技术。

国外研究现状：

自从80年代末期起，在日本，绳牵引并联机构的概念逐渐流行起来。Ming等人首先提出了完全约束定位机构的基本分类，指出不完全约束定位机构需要另外的力，如重力来保持稳定性(Ming A，Higuchi T.Study on multiple degree of freedompositioning mechanisms using wires(Part 2)：development of a planarcompletely restrained positioning mechanism.International Journal of theJapan Society for Precision Engineering，1994，28(3)：235-242)；另外，还指出，由于绳索只能承受拉力，不能承受压力，故绳牵引并联机构一般应采用冗余驱动，即n个自由度的绳牵引并联机构至少要由n+1根绳来牵引(Ming A，Kajitani M，HiguchiT.On the design of wire parallel mechanism.International Journal of the JapanSociety for Precision Engineering，1995，29(4)：337-342)。Takeda和Funabashi提出了一种由七根绳牵引的力传递性能好、工作空间大的六自由度并联机构，并首次对该类型的绳牵引并联机构进行了力传递性能分析(Takeda Y，Funabashi H.Kinematic synthesis of spatial in-parallel wire-driven mechanism with sixdegrees of freedom with high force transmissibility.In：Proceedings of theASME-DETC2000，Baltimore，Maryland，2000)。Kino等人开始对绳杆混合并联机构进行了研究(Kino H，Kawamura S.Development of a serial link structure/parallel wire system for a force display.In：Proceedings of IEEE InternationalConference on Robotics and Automation，Washington D.C.，May，MPI-6.3，2002)。Yamamoto等人则研究了3根绳索牵引6自由度不完全约束并联机构的力学及轨迹跟踪控制问题(Yamamoto M，Yanai N，Mohri A.trajectory control of incompletelyrestrained parallel-wire-suspended mechanism based on inverse dynamics.IEEETransactions on Robotics，2004，20(5)：840-850)。

德国学者Maier等人针对带有串联子系统的绳牵引机构进行了逆解、动力学和控制问题的早期研究工作之后(Maier T，Woernle C.Kinematic control of cablesuspension robots.In：Proceedings of NATO-ASI Workshop on ComputationalMethods in Mechanisms，Varna，Bulgaria，1997：421-430，NATO)，在Rostock大学建立了一个研究中心，主要针对冗余约束定位机构进行研究，并研制出一种样机。

意大利Melchiorri等人则以绳索驱动并联机器人系统为原型，开发了一种触觉装置，并对相关的控制技术、运动学等问题进行了研究(Melchiorri C，Vassura G..Development and application of wire-actuated haptic interfaces.Journal ofRobotics System，2001，18(12)：755-768)。

加拿大Gosselin等人引入动力学工作空间的概念，给出了绳牵引平面并联机构工作空间边界的解析表达式，成功研制了9根绳牵引的具有超大平动工作空间的并联机构的样机(Gouttefarde M，Gosselin C M.Analysis of the wrench-closureworkspace of planar parallel cable-driven mechanisms.IEEE Transactions onRobotics，2006，22(3)：434-445)。McGill大学和加拿大国家研究委员会自1998年起开始研制利用绳牵并联机构调整大型射电望远镜的馈源位置(Dewdney P，Nahon M，Veidt B.The large adaptive reflector：a giant radio telescope with an aerotwist.Canadian Aeronautics and Space Journal，2002，48(4)：239-250.)。Behzadipour和Khajepour则研究了3自由度绳杆混合机构的时间最优轨迹规划(Behzadipour S，Khajepour A.Time-optimal trajectory planning in cable-basedmanipulators，IEEE Transactions on Robotics，2006，22(3)：559-563)。

国内研究现状：

东北大学和中科院沈阳自动化研究所隋春平等人合作对4根绳索驱动3自由度并联机构的力学分析及控制进行了研究(隋春平，张波，赵明.一种3自由度并联柔索驱动柔性操作臂的建模与控制.机械工程学报，2005，41(6)：60-65)。清华大学汪劲松教授等人在考虑绳索质量的情况下，对3自由度及6自由度绳牵引并联机构进行了静力学分析(]Kozak K，Zhou Q，Wang J S.Static analysis of cable-drivenmanipulators with non-negligible cable mass.IEEE Transactions on Robotics，2006，22(3)：425-433.)。华侨大学郑亚青博士针对绳牵引并联机构的若干关键技术做了深入的研究，特别是对绳牵引并联机构的构型、拉力分布优化、工作空间、刚度、冗余性、轨迹规划做了深入的理论研究与大量的数值仿真研究工作(郑亚青，刘雄伟.绳牵引并联机构拉力分布优化.机械工程学报，2005，41(9)：140-145.)；另外，郑亚青博士和厦门大学林麟教授还合作对低速风洞绳牵引并联支撑系统的机构与模型姿态控制方案进行了研究(郑亚青，林麟，刘雄伟.低速风洞绳牵引并联支撑系统的机构与模型姿态控制方案设计.航空学报，2005，26(6)：774-778)。

上述各种并联绳索系统的结构虽然各不相同，但基本原理都是通过调节多根绳索的长度实现物体的空间运动，其基本思想与大射电望远镜机电光一体化方案相似，然而这些并联绳索系统由于绳索跨度小、自重轻、动平台质量大，在设计计算中完全忽略了绳索自重引起的变形，将绳索简化为直线来考虑，从而使系统线性化。根据所述相关文献提供的数据，可以计算出上述并联机器人绳索单位长度的质量为1.786×10-3kg/m，绳索质量与悬吊物体质量之比约为1∶10，而绳索馈源系统中绳索单位长度的质量设计为6.524kg/m，绳索质量与动平台质量之比可达到1∶4。当绳索跨度300米时，如不容许虚牵，绳索挠曲线比两端点直线要长出1.6m多，造成舱索系统空间定位误差大于粗调系统50cm的指标要求，求绳索长度已不能近似为直线。因此上述理论成果并不适于LT，不能通过调整绳索长度实现而对馈源舱的精度定位。

发明的内容

本发明的目的是提供一种基于力传感器巨型索系并联机器人绳索长度调整方法，以实现通过调整绳索长度对馈源舱进行精度定位。

本发明的目的是这样实现的：

本发明采用现代高精度测量仪器、结合分析计算模型和计算机强大的数据处理能力，对绳索长度或者张力进行调整，实现对大跨度绳索牵引系统的高精度定位，具体过程如下：

(1)架设测量仪器，标定站点坐标和塔顶坐标；

(2)输入馈源舱被跟踪星体的赤纬角，确定馈源舱运动轨迹上的离散点坐标，即

式中，是地理纬度；δ是赤纬角；H是时角；ρ是反射面的曲率半径；R是反射面所在球面的半径；

(3)由所述的离散坐标得出馈源舱的理论姿态，利用悬链线方程解出每个离散点所在位置的绳索理论长度；

(4)启动激光全站仪测量机，自动跟踪馈源舱上的棱镜获得棱镜在馈源舱上的整体坐标系，并将该坐标系的坐标值传输给测量计算机，通过刚体坐标转化，得到绳索与馈源舱连接点的整体坐标值；

(5)通过绳索力传感器测量出绳索张力，并将该绳索张力的实测数据与馈源舱连接点的坐标值通过数据线传输给主控计算机，利用悬链线方程计算出绳索当前的实际长度，用该绳索实际长度与理论绳索长度进行比较，得出绳索理论长度和实际长度的差值；

(6)将绳索理论长度和实际长度的差值通过张力与位置的混合控制算法，计算出绳索需要的调整量，即 $U(k+1)=U\left(k\right)+\frac{k_2\widehat{\mathrm{\θ}}(k+1)}{r_2+{\left|\right|\widehat{\mathrm{\θ}}(k+1)\left|\right|}^2}(Y(k+1)-Y\left(k\right))$

k₂是跟踪修正因子，r₂是稳定修正因子；θ是输入和输出关系矩阵，Y(n)表示系统实际输出，U为输入，k表示调整次数；Y(k+1)表示理想的输出。

(7)将计算出的绳索调整量，送入电机执行机构进行绳索的收放，牵引馈源舱运动；

(8)反复运行步骤(5)-(7)，对绳索进行精确调整，最终完成馈源舱的空间扫描运动。

上述巨型索系并联机器人绳索长度调整方法，其中步骤(4)所述的计算每个离散点绳索理论的长度，按如下步骤进行：

1)根据跟踪的星体，利用地球自转方程，通过如下公式求出局部坐标系与整体坐标系下的理想姿态角；

$\mathrm{\α}=\mathrm{arctg}\frac{y^{*}-y_{o1}^d}{x^{*}-x_{o1}^d}$

$\mathrm{\γ}=\arccos \frac{z^{*}-z_{o1}^d}{r^d}$

式中，x_o1 ^d、y_o1 ^d、z_o1 ^d是馈源舱中心点理论坐标值，x^*，y^*，z^*是被跟踪星体坐标值，r^d为馈源舱和被跟踪星体之间的距离，α表示馈源舱理论方位角，γ表示馈源舱理论俯仰角；

2)根据整体坐标系下的坐标值和星体的坐标值，由欧拉公式求出馈源舱局部坐标系与整体坐标系下的姿态角，并通过刚体坐标转换公式，得到馈源舱和绳索连接点在整体坐标系的坐标值，即

$\left\{\begin{array}{c}{x}_i=x_{O1}^d+x^i\sin \mathrm{\α}+y^i\cos \mathrm{\α}\cos \mathrm{\γ}+z^i\cos \mathrm{\α}\sin \mathrm{\γ}\\ y_i=y_{O1}^d-x^i\sin \mathrm{\α}+y^i\sin \mathrm{\α}\cos \mathrm{\γ}+z^i\sin \mathrm{\α}\sin \mathrm{\γ}\\ z_i=z_{O1}^d-y^i\sin \mathrm{\γ}+z^i\cos \mathrm{\γ}\end{array}\right\}$

式中，(x_i，y_i，z_i)和(xⁱ，yⁱ，zⁱ)分别表示馈源舱和绳索第i个连接点在整体坐标和馈源舱局部坐标下的数值；(x_O1 ^d，y_O1 ^d，z_O1 ^d)表示馈源舱中心点在整体坐标系的理论坐标值；

3)根据馈源舱和绳索连接点在整体坐标系的坐标值与塔顶的坐标，将绳索对馈源舱体的作用力分解为，F_i＝[H_icosβ_i H_isinβ_i V_i]^T，

其中H_i和V_i分别为绳索张力F_i的水平和铅垂分量；β_i表示第i根绳索水平张力和整体坐标的夹角；

4)根据给定馈源舱体相对馈源舱体局部坐标系O₁z₁轴的转角θ，建立馈源舱的六个空间力系平衡方程组：

$\left.\begin{array}{c}\underset{i=1}{\overset{6}{\mathrm{\Σ}}}{H}_i\cos {\mathrm{\β}}_i=0\\ \underset{i=1}{\overset{6}{\mathrm{\Σ}}}{H}_i\sin {\mathrm{\β}}_i=0\\ \stackrel{}{\underset{i=1}{\overset{6}{\mathrm{\Σ}}}{V}_i-W=0}\\ \underset{i=1}{\overset{6}{\mathrm{\Σ}}}(y_i{V}_i-z_i{H}_i\sin {\mathrm{\β}}_i)-W\·y_d=0\\ \underset{i=1}{\overset{6}{\mathrm{\Σ}}}(z_i{H}_i\cos {\mathrm{\β}}_i-x_i{V}_i)+W\·x_d=0\\ \underset{i=1}{\overset{6}{\mathrm{\Σ}}}(x_i{H}_i\sin {\mathrm{\β}}_i-y_i{H}_i\cos {\mathrm{\β}}_i)=0\end{array}\right\}$

式中，W是馈源舱重力，x_d，y_d是馈源舱重心坐标，(x_i，y_i，z_i)是绳索与馈源舱连接点的全局坐标；

5)建立单根绳索的静力平衡方程，

$H_i{h}_i-V_i{l}_i-\underset{0}{\overset{l_1}{\∫}}{\mathrm{qx}}_i\sqrt{1+{{\stackrel{.}{z}}_i}^2}{\mathrm{dx}}_i=0(i=1,\·\·\·,6)$

式中，h_i(i＝1，…，6)是绳索上端点与下端点的高度差，l_i(i＝1，…，6)是绳索在总体坐标系下水平面上的投影；q是绳索单位长度的重量； $z\left(x\right)=-\mathrm{kch}(\frac{x}{k}+c2)+c1$ 是绳索悬链线的方程， $k=\frac{H}{q},$ H为悬索在x处的水平张力。c1、c2两个待定系

$\left\{\begin{array}{c}c1-\mathrm{kch}\left(c2\right)=0\\ c1=\mathrm{kch}(\frac{l}{k}+c2)+h\end{array}\right.$

6)将馈源舱的六个空间力系平衡方程与单根绳索的静力平衡方程进行运算，得出第i根绳索的H_i，将H_i带入第i根绳索的悬链线方程z_i，对z_i求积分，得到理论绳索的长度，即 $L_i^d=\underset{0}{\overset{l_1}{\∫}}\sqrt{1+{\stackrel{.}{z}}_i^2\left(x\right)}\mathrm{dx}.$

上述巨型索系并联机器人绳索长度调整方法，其中步骤(5)所述的计算绳索当前的实际长度，包括如下步骤：

1)由均匀分布在馈源舱底圆上三个棱镜b₁，b₂，b₃，算出三个棱镜组成平面中心的直角坐标值：

$\left\{\begin{array}{c}{b}_{\mathrm{ox}}=(b_{1x}+b_{2x}+b_{3x})/3\\ b_{\mathrm{oy}}=(b_{1y}+b_{2y}+b_{3y})/3\\ b_{\mathrm{oz}}=(b_{1z}+b_{2z}+b_{3z})/3\end{array}\right.$

式中，(b_ix，b_iy，b_iz)表示第i(i＝1…3)个棱镜在整体坐标系的坐标值，

2)由三个棱镜的直角坐标值算出馈源舱局部坐标Z′轴在整体坐标系下的矢量表达式：

$m_1=\frac{\stackrel{\‾}{b_1{b}_2}\×\stackrel{\‾}{b_2{b}_3}}{|\stackrel{\‾}{b_1{b}_2}\×\stackrel{\‾}{b_2{b}_3}|};$

3)由所述的矢量m₁，得到馈源舱底面中心r_o1和馈源舱顶端点r_o2在整体坐标系下的坐标为

          r_o1＝[r_o1x，r_o1y，r_o1z]^T＝R₁k₁+[b_ox，b_oy，b_oz]^T

          r_o2＝[r_o2x，r_o2y，r_o2z]^T＝R₂k₁+[b_ox，b_oy，b_oz]^T

式中，R₁为馈源舱底面中心与三个棱镜组成平面中心的距离；

R₂为馈源舱顶端点与棱镜组成三角形中心的距离；

(r_o1x，r_o1y，r_o1z)为馈源舱底面中心r_o1在整体坐标系下的三个坐标分量；

(r_o2x，r_o2y，r_o2z)为馈源舱顶端点r_o2在整体坐标系下的三个坐标分量；

4)用(r_o1x，r_o1y，r_o1z)和(r_o2x，r_o2y，r_o2z)分别代替所述馈源舱理想姿态公式中的(x_o1 ^d，y_o1 ^d，z_o1 ^d)和(x^*，y^*，z^*)，即可以得到实际馈源舱姿态角，

${\mathrm{\α}}^{\′}=\mathrm{arctg}\frac{r_{o2y}-r_{o1y}}{r_{o2x}-x_{o1x}}$

${\mathrm{\γ}}^{\′}=\arccos \frac{r_{o2y}-r_{o1y}}{R_2-R_1}$

式中，α′表示馈源舱实际方位角；

γ′表示馈源舱实际俯仰角

5)利用馈源舱的实际姿态角，即方位角α′和俯仰角γ′，进行刚体坐标转换，得到馈源舱和绳索第i个连接点在整体坐标系下的实际位置坐标值，即

$\left\{\begin{array}{c}{x}_i^{\′}=b_{\mathrm{ox}}+x^i\sin {\mathrm{\α}}^{\′}+y^i\cos {\mathrm{\α}}^{\′}\cos {\mathrm{\γ}}^{\′}+z^i\cos {\mathrm{\α}}^{\′}\sin {\mathrm{\γ}}^{\′}\\ y_i^{\′}=b_{\mathrm{oy}}-x^i\sin {\mathrm{\α}}^{\′}+y^i\sin {\mathrm{\α}}^{\′}\cos {\mathrm{\γ}}^{\′}+z^i\sin {\mathrm{\α}}^{\′}\sin {\mathrm{\γ}}^{\′}\\ z_i^{\′}==b_{\mathrm{oz}}-y^i\sin {\mathrm{\γ}}^{\′}+z^i\cos {\mathrm{\γ}}^{\′}\end{array}\right\}$

6)根据绳索张力传感器测量的绳索张力，通过如下公式求出绳索的实际长度L，

$L^2=\frac{l^2}{{\mathrm{\η}}^2}\sin h^2\mathrm{\η}+h^2$

其中，h是绳索两端的高度差，

$\mathrm{\η}=-\frac{q_{0}l}{2F_x},$ l是其在水平平面上投影的长度，q₀是绳索的线密度，F_x是绳索张力在水平方向的分量。

上述巨型索系并联机器人绳索长度调整方法，其中步骤(6)所述的将绳索的差值通过力与位置的混合控制算法，计算出绳索需要的调整量，包括如下步骤：

1)将绳索理论长度和实际长度的差值或绳索理论张力和实际张力的差值，代入如下公式，得到绳索输入和输出的关系矩阵：

$\widehat{\mathrm{\θ}}(k+1)=\widehat{\mathrm{\θ}}\left(k\right)+\frac{k_{1}U\left(k\right)}{r_1+U{\left(k\right)}^{T}U\left(k\right)}(Y^T(k+1)-{\left(\widehat{\mathrm{\θ}}\left(k\right)U\left(k\right)\right)}^T)$

式中，

和

分别表示上一时刻和当前时刻输入和输出的关系矩阵，

k₁表示权重因子，r₁表示遗忘因子，

Y^T(k+1)表示当前绳索理论长度和实际长度或力的差值，

U(k)为上一个时刻输出的绳索调整量或力，k为调整次数；

2)根据馈源舱理想位置(r_O1，y_O1，z_O1)和馈源舱实际位置(r_o1x，r_o1y，r_o1z)的位置差值，对绳索输入和输出的关系矩阵准确性进行判断，

如果该位置差值小于设定的值，则按如下公式直接计算出绳索的调整量或力，

$U(k+1)=U\left(k\right)+\frac{k_2\widehat{\mathrm{\θ}}(k+1)}{r_2+{\left|\right|\widehat{\mathrm{\θ}}(k+1)\left|\right|}^2}(Y(k+1)-Y\left(k\right)),$

如果该位置差值大于设定的值，按照如下公式重新确定所述关系矩阵，即

$\widehat{\mathrm{\θ}}\left(n\right)=\widehat{\mathrm{\θ}}(n-1)+S\left(n\right)[Y(n+1)-U{\left(n\right)}^T\widehat{\mathrm{\θ}}(n-1)]$

其中， $S\left(n\right)=\frac{P(n-1)U\left(n\right)}{c+U{\left(n\right)}^{T}P(n-1)U\left(n\right)}$

$P\left(n\right)=\frac{1}{c}[P(n-1)-S\left(n\right)U{\left(n\right)}^{T}P(n-1)]$

式中，S(n)为卡尔曼增益矢量，P(n)为理想输出索长和实际输入索长误差的协方差矩阵，n为迭代的次数，c为更新因子；

3)用重新确定的矩阵

替代绳索输入和输出的关系矩阵

即 $\widehat{\mathrm{\θ}}(k+1)=\widehat{\mathrm{\θ}}\left(n\right),$ 再通过公式： $U(k+1)=U\left(k\right)+\frac{k_2\widehat{\mathrm{\θ}}(k+1)}{r_2+{\left|\right|\widehat{\mathrm{\θ}}(k+1)\left|\right|}^2}(Y(k+1)-Y\left(k\right))$ 计算出绳索长度的调整量或绳索张力的调整量，当Y^T(k+1)表示当前绳索理论长度和实际长度的差值，求出的U(k+1)表示绳索长度调整量，当Y^T(k+1)表示当前绳索理论张力和实际绳索张力的差值时，求出的U(k+1)表示绳索张力调整量。

上述巨系并联机器人绳索长度调整方法，其中步骤(5)所述的通过绳索力传感器测量出绳索张力的方法，是将绳索力传感器固定在两个设定的定向轮之间，每个定向轮上开设有绳索槽，将绳索嵌在槽内，以保证绳索在两个定向轮之间的形状不变，并在力传感器的屏蔽传输线上增设RC虑波器，并通过该屏蔽传输线将张力传感器测量的模拟信号送入计算机内数据采集卡，采用差分方式进行A/D转换，测量出绳索张力的数值。

发明具有如下优点：

(1)本发明由于设计了安装力传感器的结构，使得索力的测量精度小于0.3％，满足LT工程力控制的需要。

(2)本发明由于对力传感器采用了电磁屏蔽的措施，消除了因传感器信号线和馈源舱内仪器工作的动力线距离非常近所造成的信号干扰，解决了LT工程的实际问题。

(3)本发明由于采用了理论规划绳索长度的方法，解决了LT工程中大跨度索系并联机器人中的绳索不能近似为直线的计算问题，且该方法以绳索张力变化均匀为目标，可使馈源舱运行平稳且节约能源。

(4)本发明由于采用了基于张力传感器的实际索长求解方法，与现有技术相比极大地减小了计算量和时间消耗，可满足舱索系统控制时时性的要求。

(5)本发明由于采用了基于力/索长的混合控制算法，不仅避免了由绳索虚牵造成舱索系统结构对馈源舱定位精度的影响，而且实现了在计算机控制下，大跨度绳索牵引系统的空间高精度定位，

模型实验表明，在绳索跨度为30余米，馈源舱重700公斤，馈源舱2cm/s空间扫描速度下，通过对绳索长度的调整可使馈源舱的空间定位精度小于3厘米，解决了国家大科学计划“新一代大射电望远镜”的技术难点。

以下结合附图和实施方式对本发明的目的、特征作进一步详细描述。

                              附图说明

图1是本发明绳索调整方法的流程图；

图2是本发明力传感器安装结构示意图；

图3是本发明力传感器的信号线滤波电路示意图；

图4是本发明大射电望远镜舱索结构示意图；

图5是本发明绳索对馈源舱的受力分析图；

图6是本发明绳索结构的受力分析图；

图7是本发明计算绳索调整量的过程示意图；

图8是本发明力/绳索长度混合控制器控制过程示意图。

                             具体实施方式

参照图1，本发明的绳索调整过程如下：

一.架设测量仪器，标定站点坐标和塔顶坐标

测量仪器包括3台激光全站仪和6台力传感器，3台激光全站仪分别固定在3个支架上，经过校准后，3个镜头分别对准馈源舱底面上安装的3个棱镜。该6台张力传感器分别固定在6根绳索的悬塔上，力传感器的两端分别设有安装定向轮2，该两个定向轮固定在悬塔上，每一个定向轮上开有一圈凹槽4，被测绳索3穿过力传感器放置在该凹槽4内，以保证绳索力传感器所测的本段绳索形状不变，如图2所示。为了消除动力线和信号线之间的电磁干扰，本发明对力传感器的传输线采用了如图3所示的屏蔽措施，即将力传感器上传输线采用屏蔽线，并在屏蔽线上连接有RC滤波电路，张力传感器将测出的模拟信号通过屏蔽线送入计算机内数据采集卡，采用差分方式进行A/D转换，得出绳索张力的测量数值。

激光全站仪通过测量实验场内至少3个已知固定点坐标，通过后方交汇的原理标定出激光全站仪站点的坐标。激光全站仪测量出在塔顶贴有的激光反射靶标处的坐标值，并通过反射靶标和塔顶坐标的固定关系，得出塔顶在整体坐标系下的坐标值。

二.规划绳索的理论长度

本发明绳索的理论长度规划可参照图4进行描述。图4所示的大射电望远镜舱索结构包括6根悬索塔、6条绳索和馈源舱，每根悬索塔的分别上端固定有力传感器和定向轮组件，馈源舱与每根悬索塔之间分别连接有绳索。悬索塔与绳索的连接点分别为A1、A2、A3、A4、A5、A6，绳索与馈源舱的连接点分别为B1、B2、B3、B4、B5、B6，馈源舱底面中心为O₁，馈源舱顶点为O₂，Ox，Oy，Oz为整体坐标系三个坐标轴，O₁x₁，O₁y₁，O₁z₁为馈源舱局部坐标系三个坐标轴。α是馈源舱局部坐标系与整体坐标系的方位角，γ表示馈源舱局部坐标系与整体坐标系的俯仰角。

(一)确定离散点坐标和馈源舱理论的姿态角

当被跟踪的星体确定后，通过如下公式确定馈源舱做扫描运动中心点在整体坐标系下的理论坐标值，

式中，是地理纬度；δ是赤纬角；H是时角；ρ是反射面的曲率半径；R是反射面所在球体的半径。

根据跟踪的星体，利用地球自转方程，通过如下公式求出馈源舱局部坐标系与整体坐标系下的理想姿态角；

$\mathrm{\α}=\mathrm{arctg}\frac{y^{*}-y_{O1}^d}{x^{*}-x_{O1}^d}$

$\mathrm{\γ}=\arccos \frac{z^{*}-z_{o1}^d}{r^d}$

式中，x^*，y^*，z^*是被跟踪星体坐标值，r^d为馈源舱和被跟踪星体之间的距离。

(二)确定馈源舱与绳索连接点的理论坐标

将馈源舱局部坐标系与整体坐标系下的理想姿态角，通过刚体坐标转换公式，得到馈源舱和绳索连接点在整体坐标系的坐标值，即

$\left\{\begin{array}{c}{x}_i=x_{O1}^d+x^i\sin \mathrm{\α}+y^i\cos \mathrm{\α}\cos \mathrm{\γ}+z^i\cos \mathrm{\α}\sin \mathrm{\γ}\\ y_i=y_{O1}^d-x^i\sin \mathrm{\α}+y^i\sin \mathrm{\α}\cos \mathrm{\γ}+z^i\sin \mathrm{\α}\sin \mathrm{\γ}\\ z_i=z_{O1}^d-y^i\sin \mathrm{\γ}+z^i\cos \mathrm{\γ}\end{array}\right\}$

式中，(x_i，y_i，z_i)表示馈源舱和绳索第i个连接点在整体坐标，(xⁱ，yⁱ，zⁱ)表示馈源舱和绳索第i个连接点在馈源舱局部坐标下的坐标。

(三)确定绳索的理论的长度

1.根据馈源舱和绳索连接点在整体坐标系的坐标值与塔顶的坐标，将绳索对馈源舱体的作用力分解为，F_i＝[H_icosβ_i H_isinβ_i V_i]^T，如图5所示。

其中，H_i和V_i分别为绳索张力F_i的水平和铅垂分量；β_i表示第i根绳索H_i与坐标系y轴的夹角；

2.根据给定馈源舱体相对馈源舱体局部坐标系O₁z₁轴的转角θ，建立馈源舱的六个空间力系平衡方程组：

$\left.\begin{array}{c}\underset{i=1}{\overset{6}{\mathrm{\Σ}}}{H}_i\cos {\mathrm{\β}}_i=0\\ \underset{i=1}{\overset{6}{\mathrm{\Σ}}}{H}_i\sin {\mathrm{\β}}_i=0\\ \stackrel{}{\underset{i=1}{\overset{6}{\mathrm{\Σ}}}{V}_i-W=0}\\ \underset{i=1}{\overset{6}{\mathrm{\Σ}}}(y_i{V}_i-z_i{H}_i\sin {\mathrm{\β}}_i)-W\·y_d=0\\ \underset{i=1}{\overset{6}{\mathrm{\Σ}}}(z_i{H}_i\cos {\mathrm{\β}}_i-x_i{V}_i)+W\·x_d=0\\ \underset{i=1}{\overset{6}{\mathrm{\Σ}}}(x_i{H}_i\sin {\mathrm{\β}}_i-y_i{H}_i\cos {\mathrm{\β}}_i)=0\end{array}\right\}$

式中，W是馈源舱重力，x_d，y_d是馈源舱重心坐标，H_i，V_i为待求变量；

3.建立单根绳索的静力平衡方程，

参照图6，Axz是绳索的局部坐标系，Ax是绳索坐标系的x轴，Ay是绳索坐标系的y轴，q(x)是绳索单位长度的重量，h是绳索上端点与下端点的高度差，l是绳索在Ax轴的投影。由图6的受力分析，得到第i根绳索力矩静平衡方程为：

$H_i{h}_i-V_i{l}_i-\underset{0}{\overset{l_1}{\∫}}{\mathrm{qx}}_i\sqrt{1+{{\stackrel{.}{z}}_i}^2}{\mathrm{dx}}_i=0(i=1,\·\·\·,6)$

式中，h_i(i＝1，…，6)是绳索上端点与下端点的高度差，l_i(i＝1，…，6)是绳索在总体坐标系下水平面上的投影；q是绳索单位长度的重量； $z\left(x\right)=-\mathrm{kch}(\frac{x}{k}+c2)+c1$ 是绳索悬的链线方程，z_i是第i根绳索的悬链线方程， $k=\frac{H}{q},$ H为悬索在x处的水平张力，c1、c2为两个待定系数

$\left\{\begin{array}{c}c1-\mathrm{kch}\left(c2\right)=0\\ c1=\mathrm{kch}(\frac{l}{k}+c2)+h\end{array}\right.;$

4.将馈源舱的六个空间力系平衡方程与单根绳索的静力平衡方程进行运算，得出第i根绳索的H_i，将H_i带入第i根绳索的悬链线方程z_i得到z_i方程的非线形表达式，对z_i求积分，得到理论绳索的长度，即

$L_i^d=\underset{0}{\overset{l_1}{\∫}}\sqrt{1+{\stackrel{.}{z}}_i^2\left(x\right)}\mathrm{dx}.$

三.确定绳索实际索长

(一)确定棱镜平面中心的直角坐标值

由激光全站仪测出均匀分布在馈源舱底圆上三个棱镜b₁，b₂，b₃在整体坐标系下的坐标值，算出三个棱镜组成平面中心的直角坐标值：

$\left\{\begin{array}{c}{b}_{\mathrm{ox}}=(b_{1x}+b_{2x}+b_{3x})/3\\ b_{\mathrm{oy}}=(b_{1y}+b_{2y}+b_{3y})/3\\ b_{\mathrm{oz}}=(b_{1z}+b_{2z}+b_{3z})/3\end{array}\right.$

式中，(b_ix，b_iy，b_iz)表示第i(i＝1…3)个棱镜在整体坐标系的坐标值。

(二)给出馈源舱在整体坐标系下的矢量表达式

由激光全站仪测量三个棱镜的坐标，得到三个棱镜组成三角形边长

和

在整体坐标下的矢量，即

$\stackrel{\‾}{b_1{b}_2}=\left[\begin{array}{ccc}{b}_{1x}-b_{2x}& b_{1y}-b_{2y}& b_{1z}-b_{2z}\end{array}\right]$

$\stackrel{\‾}{b_2{b}_3}=\left[\begin{array}{ccc}{b}_{2x}-b_{3x}& b_{2y}-b_{3y}& b_{2z}-b_{3z}\end{array}\right]$

由

和

在整体坐标下的矢量，得到馈源舱局部坐标O₁z₁轴在整体坐标系下的矢量表达式为：

$m_1=\frac{\stackrel{\‾}{b_1{b}_2}\×\stackrel{\‾}{b_2{b}_3}}{|\stackrel{\‾}{b_1{b}_2}\×\stackrel{\‾}{b_2{b}_3}|}.$

(三)算出馈源舱底面中心r_o1和馈源舱顶端点r_o2在整体坐标系下的坐标

由三个棱镜组成平面中心的直角坐标值和馈源舱局部坐标O₁z₁轴在整体坐标系下的矢量k₁，根据馈源舱局部坐标系和整体坐标系的关系，得到馈源舱底面中心r_o1和馈源舱顶端点r_o2在整体坐标系下的坐标为：

              r_o1＝[r_o1x，r_o1y，r_o1z]^T＝R₁m₁+[b_ox，b_oy，b_oz]^T

              r_o2＝[r_o2x，r_o2y，r_o2z]^T＝R₂m₁+[b_ox，b_oy，b_oz]^T

R₁为馈源舱底面中心与三个棱镜组成平面中心的距离；R₂为馈源舱顶端点与棱镜组成三角形中心的距离；(r_o1x，r_o1y，r_o1z)和(r_o2x，r_o2y，r_o2z)分别是馈源舱底面中心r_o1和馈源舱顶端点r_o2在整体坐标系下的三个坐标分量。

(四)确定馈源舱实际姿态角

用所述(r_o1x，r_o1y，r_o1z)和(r_o2x，r_o2y，r_o2z)分别代替所述馈源舱理想姿态公式中的(x_o1 ^d，y_o1 ^d，z_o1 ^d)和(x^*，y^*，z^*)，即可以得到馈源舱的实际姿态角为：

${\mathrm{\α}}^{\′}=\mathrm{arctg}\frac{r_{o2y}-r_{o1y}}{r_{o2x}-x_{o1x}}$

${\mathrm{\γ}}^{\′}=\arccos \frac{r_{o2y}-r_{o1y}}{R_2-R_1}$

式中，α′表示馈源舱实际方位角，γ′表示馈源舱实际俯仰角。

(五)确定馈源舱和绳索第i个连接点在整体坐标系下的实际位置坐标值

利用际馈源舱姿态角，并通过刚体坐标转换公式，得到馈源舱和绳索第i个连接点在整体坐标系下的实际位置坐标值为：

$\left\{\begin{array}{c}{x}_i^{\′}=b_{\mathrm{ox}}+x^i\sin {\mathrm{\α}}^{\′}+y^i\cos {\mathrm{\α}}^{\′}\cos {\mathrm{\γ}}^{\′}+z^i\cos {\mathrm{\α}}^{\′}\sin {\mathrm{\γ}}^{\′}\\ y_i^{\′}=b_{\mathrm{oy}}-x^i\sin {\mathrm{\α}}^{\′}+y^i\sin {\mathrm{\α}}^{\′}\cos {\mathrm{\γ}}^{\′}+z^i\sin {\mathrm{\α}}^{\′}\sin {\mathrm{\γ}}^{\′}\\ z_i^{\′}==b_{\mathrm{oz}}-y^i\sin {\mathrm{\γ}}^{\′}+z^i\cos {\mathrm{\γ}}^{\′}\end{array}\right\}$

(六)确定绳索的实际长度L

通过上述步骤，绳索两端实际坐标均为以知值。设绳索在自重作用下保持平衡，其弹性模量为E，横截面积为A，受力后的长度为L，绳索的初始长度为L_u。绳索投影与索端张力之间的关系为，

$l=-F_x[\frac{L_u}{\mathrm{EA}}+\frac{1}{q_0}\ln \frac{F_{2z}+T_2^c}{T_1^c-F_z}]$

$h=\frac{1}{2\mathrm{EA}{q}_0}(T_2^{c^2}-T_1^{c^2})+\frac{T_2^c-T_1^c}{q_0}$

其中， $F_1^c={\{F_x,F_z\}}^T,F_2^c={\{F_{2x},F_{2z}\}}^T,T_1^c=\left|\right|F_1^c\left|\right|=\sqrt{F_x^2+F_z^2}$ 为绳索对馈源舱的作用力， $T_2^c=\left|\right|F_2^c\left|\right|=\sqrt{F_{2x}^2+F_{2z}^2}$ 为绳索在塔端的作用力，T₂ ^c值由绳索张力传感器测量获得。并且F_x＝-F_2x，F_z+F_2z＝q₀L_u，q₀是绳索的线密度，F_x是绳索张力在水平方向的分量，h是绳索两端的高度差，联立求解上面两个非线性方程，求得F_x的数值。

由如下公式求解出绳索的实际长度为：

$L^2=\frac{l^2}{{\mathrm{\η}}^2}{\sinh }^2\mathrm{\η}+h^2$

其中， $\mathrm{\η}=-\frac{q_{0}l}{2F_x},$ l是其在水平平面上投影的长度。

四.确定绳索需要的调整量

参照图7，本发明确定绳索需要的调整量过程如下：

(一)确定绳索输入和输出的关系矩阵：

将绳索理论长度和实际长度进行比较，得出两者之间的差值，再将该差值代入如下公式，得到绳索输入和输出的关系矩阵：

$\widehat{\mathrm{\θ}}(k+1)=\widehat{\mathrm{\θ}}\left(k\right)+\frac{k_{1}U\left(k\right)}{r_1+U{\left(k\right)}^{T}U\left(k\right)}(Y^T(k+1)-{\left(\widehat{\mathrm{\θ}}\left(k\right)U\left(k\right)\right)}^T)$

式中，

和 分别表示上一时刻和当前时刻输入和输出的关系矩阵，k₁表示权重因子，r₁表示遗忘因子，Y^T(k+1)表示当前绳索理论长度和实际长度或力的差值，U(k)为上一个时刻输出的绳索调整量或力，k为调整次数；

(二)判断绳索输入和输出的关系矩阵准确性

根据馈源舱理想位置(x_O1，y_O1，z_O1)和馈源舱实际位置(r_o1x，r_o1y，r_o1z)的位置差值，对绳索输入和输出的关系矩阵准确性进行判断，

如果该位置差值小于设定的值，则按如下公式直接计算出绳索的长度调整量或张力调整量，

$U(k+1)=U\left(k\right)+\frac{k_2\widehat{\mathrm{\θ}}(k+1)}{r_2+{\left|\right|\widehat{\mathrm{\θ}}(k+1)\left|\right|}^2}(Y(k+1)-Y\left(k\right)),$

如果该位置差值大于设定的值，按照如下公式重新确定所述的关系矩阵，即

$\widehat{\mathrm{\θ}}\left(n\right)=\widehat{\mathrm{\θ}}(n-1)+S\left(n\right)[Y(n+1)-U{\left(n\right)}^T\widehat{\mathrm{\θ}}(n-1)]$

其中， $S\left(n\right)=\frac{P(n-1)U\left(n\right)}{c+U{\left(n\right)}^{T}P(n-1)U\left(n\right)}$

$P\left(n\right)=\frac{1}{c}[P(n-1)-S\left(n\right)U{\left(n\right)}^{T}P(n-1)]$

式中，S(n)为卡尔曼增益矢量，P(n)为理想输出索长和实际输入索长误差的协方差矩阵，n为迭代的次数，c为更新因子；

(三)确定最终的绳索调整量

用重新确定的矩阵

替代绳索输入和输出的关系矩阵

即 $\widehat{\mathrm{\θ}}(k+1)=\widehat{\mathrm{\θ}}\left(n\right),$ 再通过公式； $U(k+1)=U\left(k\right)+\frac{k_2\widehat{\mathrm{\θ}}(k+1)}{r_2+{\left|\right|\widehat{\mathrm{\θ}}(k+1)\left|\right|}^2}(Y(k+1)-Y\left(k\right))$ 计算出绳索的调整量。

绳索的调整量可以是绳索的长度，也可以是绳索的张力，如图8所示。

图8中S_i ^T，S_i ^R分别表示理论索长和实际索长，T_i ^T，T_i ^R分别表示绳索的理论张力和实际测量的张力。馈源舱的空间运动是通过六根大跨度绳索的牵引完成的，如果绳索的张力过小时，将引起虚牵，具体表现为该绳索不参与馈源舱位置和姿态的调整，导致舱索结构的刚度降低，抗干扰能力减弱，造成舱索系统的不稳定。此外，当绳索张力较大时，绳索张力变化很大而绳索长度变化却非常小，这说明在该位置舱索系统的调整对绳索张力非常敏感，而对绳索长度并不敏感。针对上述特点，设计了绳索的张力/长度混合控制的方案：

如果绳索的实际测量张力T_i ^R在给定的绳索理论张力T_i ^T范围内，将绳索的理论长度与绳索实际长度之差 $\mathrm{\Δ}{S}_i=S_i^T-S_i^R$ 送入绳索力/位置混合控制算法，计算出每一根绳索需要调整的绳索长度，输送给馈源舱的电机执行机构拉动馈源舱运动，完成馈源舱第一次扫描运动调整。

如果绳索的实际张力T_i ^R在给定的绳索理论张力T_i ^T范围内外，将绳索的理论张力与绳索实际张力之差 $\mathrm{\Δ}{T}_i=T_i^T-T_i^R$ 送入绳索力/位置混合控制算法，计算出每一根绳索需要调整的绳索张力，输送给馈源舱的电机执行机构拉动馈源舱运动，完成馈源舱第一次扫描运动调整。

馈源舱在调整后的绳索牵引下，得到新的位置和姿态，再通过激光全站仪对馈源舱底面上的棱镜进行跟踪测量，进而求得馈源舱和绳索连接点在整体坐标系下新的坐标值，结合绳索张力传感器测量新的绳索张力，得到新位置下绳索的实际长度，并通过对绳索实际张力值大小进行安全判断，如果测量出绳索的张力值大于等于理论计算的绳索所能承受的最大张力值，断电保护，如果测量出绳索的张力值小于理论计算的绳索所能承受的最大张力，则将对应位置理论绳索的长度和张力进行比较，根据绳索力/位置混合控制方案，确定绳索调整的方式，并通过混合控制方法，计算出下一步绳索的调整量，反复运行上述步骤，完成馈源舱空间的最终扫描运动。

实验结果：

将本发明应用于LT50米室外模型，该模型硬件组成如下：实验塔均匀分布在直径为50m的圆周上，执行机构由六套绞盘组成，每一套包括松下交流伺服电机及其驱动器，卷筒，钢丝绳，减速箱，馈源舱的位姿通过三台激光全站仪，分别自动跟踪测量安装在馈源舱底面上了三个360度棱镜，仪器的测量精度是1mm+2ppm，采样频率约200毫秒，满足控制周期800毫秒的要求。馈源舱重720kg，绳索直径为15mm，线密度为8.027N/m，假设跟踪星体位于(0，0，50)m处。从馈源舱实验曲线可见：馈源舱直线运行的长度在4米以上，其跟踪误差在1cm以内；馈源舱水平圆弧长度在8米以上，其跟踪误差在2cm以内；馈源舱观测曲线的运行长度在8米以上，其跟踪误差在3cm以内。本发明适用于馈源舱的不同运行轨迹，特别适用于巨型索系并联机器人绳索的调整。

Claims (5)

1.一种基于索长/索力的巨型索系并联机器人绳索调整方法，包括如下步骤：

(1)架设测量仪器，标定站点坐标和塔顶坐标；

(2)输入馈源舱被跟踪星体的赤纬角，确定馈源舱运动轨迹上的离散点坐标，即

式中，是地理纬度；δ是赤纬角；H是时角；ρ是反射面的曲率半径；

(3)由所述的离散坐标得出馈源舱的理论姿态，利用悬链线方程解出每个离散点所在位置的绳索理论值；

(4)启动激光全站仪测量机，自动跟踪馈源舱上的棱镜获得棱镜在馈源舱上的整体坐标系，并将该坐标系的坐标值传输给测量计算机，通过刚体坐标转化，得到绳索与馈源舱连接点的整体坐标值；

(5)通过绳索力传感器测量出绳索张力，并将该绳索张力的实测数据与馈源舱连接点的坐标值通过数据线传输给主控计算机，利用悬链线方程计算出绳索当前的实际长度，用该绳索实际长度与理论绳索长度进行比较，得出绳索理论长度和实际长度的差值；

(6)将绳索理论长度和实际长度的差值通过张力与位置的混合控制算法，计算出绳索需要的调整量，即 $U(k+1)=U\left(k\right)+\frac{k_2\widehat{\mathrm{\θ}}(k+1)}{r_2+{\left|\right|\widehat{\mathrm{\θ}}(k+1)\left|\right|}^2}(Y(k+1)-Y\left(k\right))$ k₂是跟踪修正因子，r₂是稳定修正因子；θ是输入和输出关系矩阵，Y(n)表示系统实际输出，U为输入，k表示调整次数；Y(k+1)表示理想的输出；

(7)将计算出的绳索调整量，送入电机执行机构进行绳索的收放，牵引馈源舱运动；

(8)反复运行步骤(5)-(7)，对绳索进行精确调整，最终完成馈源舱的空间扫描运动。

2.根据权利求1所述的方法，其中步骤(3)所述的计算每个离散点绳索理论的长度，按如下步骤进行：

1)根据跟踪的星体，利用地球自转方程，通过如下公式求出局部坐标系与整体坐标系下的理想姿态角；

$\mathrm{\α}=\mathrm{arctg}\frac{y^{*}-y_{o1}^d}{x^{*}-x_{o1}^d}$

$\mathrm{\γ}=\arccos \frac{z^{*}-z_{o1}^d}{r^d}$

式中，x_o1 ^d、y_o1 ^d、z_o1 ^d是馈源舱中心点理论坐标值，

x^*，y^*，z^*是被跟踪星体坐标值，

r^d为馈源舱和被跟踪星体之间的距离，

α表示馈源舱理论方位角，

γ表示馈源舱理论俯仰角；

2)根据整体坐标系下的坐标值和星体的坐标值，由欧拉公式求出馈源舱局部坐标系与整体坐标系下的姿态角，并通过刚体坐标转换公式，得到馈源舱和绳索连接点在整体坐标系的坐标值，即

$\left\{\begin{array}{c}{x}_i=x_{O1}^d+x^i\sin \mathrm{\α}+y^i\cos \mathrm{\α}\cos \mathrm{\γ}+z^i\cos \mathrm{\α}\sin \mathrm{\γ}\\ y_i=y_{O1}^d-x^i\sin \mathrm{\α}+y^i\sin \mathrm{\α}\cos \mathrm{\γ}+z^i\sin \mathrm{\α}\sin \mathrm{\γ}\\ z_i=z_{O1}^d-y^i\sin \mathrm{\γ}+z^i\cos \mathrm{\γ}\end{array}\right\}$

式中，(x_i，y_i，z_i)表示馈源舱和绳索第i个连接点在整体坐标系下的坐标，

(xⁱ，yⁱ，zⁱ)表示馈源舱局部坐标下的数值，

(x_O1 ^d，y_O1 ^d，z_O1 ^d)表示馈源舱中心点在整体坐标系的理论坐标值；

3)根据馈源舱和绳索连接点在整体坐标系的坐标值与塔顶的坐标，将绳索对馈源舱体的作用力分解为，F_i＝[H_icosβ_i H_isinβ_i V_i]^T，式中，H_i和V_i分别为绳索张力F_i的水平和铅垂分量，

β_i表示第i根绳索水平张力和整体坐标的夹角；

4)根据给定馈源舱体相对馈源舱体局部坐标系O₁z₁轴的转角θ，建立馈源舱的六个空间力系平衡方程组：

$\left.\begin{array}{c}\underset{i=1}{\overset{6}{\mathrm{\Σ}}}{H}_i\cos {\mathrm{\β}}_i=0\\ \underset{i=1}{\overset{6}{\mathrm{\Σ}}}{H}_i\sin {\mathrm{\β}}_i=0\\ \underset{i=1}{\overset{6}{\mathrm{\Σ}}}{V}_i-W=0\\ \underset{i=1}{\overset{6}{\mathrm{\Σ}}}(y_i{V}_i-z_i{H}_i\sin {\mathrm{\β}}_i)-W\·y_d=0\\ \underset{i=1}{\overset{6}{\mathrm{\Σ}}}(z_i{H}_i\cos {\mathrm{\β}}_i-x_i{V}_i)+W\·x_d=0\\ \underset{i=1}{\overset{6}{\mathrm{\Σ}}}(x_i{H}_i\sin {\mathrm{\β}}_i-y_i{H}_i\cos {\mathrm{\β}}_i)=0\end{array}\right\}$

式中，W是馈源舱重力，

(x_d，y_d)是馈源舱重心坐标，

(x_i，y_i，z_i)是绳索与馈源舱连接点的全局坐标；

5)建立单根绳索的静力平衡方程，

$H_i{h}_i-V_i{l}_i-\underset{0}{\overset{l_i}{\∫}}{\mathrm{qx}}_i\sqrt{1+{{\stackrel{\·}{z}}_i}^2}d{x}_i=0,(i=1,\·\·\·,6)$

式中，h_i(i＝1，…，6)是绳索上端点与下端点的高度差，

l_i(i＝1，…，6)是绳索在总体坐标系下水平面上的投影，

q是绳索单位长度的重量，

$z\left(x\right)=-\mathrm{kch}(\frac{x}{k}+c2)+c1$ 是绳索悬链线的方程， $k=\frac{H}{q},$ H为悬索在x处的水平张力，c1、c2为两个待定系数

$\left\{\begin{array}{c}c1-\mathrm{kch}\left(c2\right)=0\\ c1=\mathrm{kch}(\frac{l}{k}+c2)+h\end{array}\right.;$

6)将馈源舱的六个空间力系平衡方程与单根绳索的静力平衡方程进行运算，得出第i根绳索的H_i，将H_i带入第i根绳索的悬链线方程z_i，对z_i求积分，得到理论绳索的长度，即

$L_i^d=\underset{0}{\overset{l_i}{\∫}}\sqrt{1+{\stackrel{\·}{z}}_i^2\left(x\right)}\mathrm{dx}.$

3.根据权利求1所述的方法，其中步骤(5)所述的计算出绳索当前的实际长度，包括如下步骤：

1)由均匀分布在馈源舱底圆上三个棱镜b₁，b₂，b₃，算出三个棱镜组成平面中心的直角坐标值：

$\left\{\begin{array}{c}{b}_{\mathrm{ox}}=(b_{1x}+b_{2x}+b_{3x})/3\\ b_{\mathrm{oy}}=(b_{1y}+b_{2y}+b_{3y})/3\\ b_{\mathrm{oz}}=(b_{1z}+b_{2z}+b_{3z})/3\end{array}\right.$

式中，(b_ix，b_iy，b_iz)表示第i(i＝1…3)个棱镜在整体坐标系的坐标值，

2)由三个棱镜的直角坐标值算出馈源舱局部坐标O₁z₁轴在整体坐标系下的矢量表达式，

$m_1=\frac{\stackrel{\‾}{b_1{b}_2}\×\stackrel{\‾}{b_2{b}_3}}{|\stackrel{\‾}{b_1{b}_2}\×\stackrel{\‾}{b_2{b}_3}|};$

3)由所述的矢量m₁，得到馈源舱底面中心r_o1和馈源舱顶端点r_o2在整体坐标系下的坐标为

r_o1＝[r_o1x，r_o1y，r_o1z]^T＝R₁m₁+[b_ox，b_oy，b_oz]^T

r_o2＝[r_o2x，r_o2y，r_o2z]^T＝R₂m₁+[b_ox，b_oy，b_oz]^T

式中，R₁为馈源舱底面中心与三个棱镜组成平面中心的距离，

R₂为馈源舱顶端点与棱镜组成三角形中心的距离，

(r_o1x，r_o1y，r_o1z)是馈源舱底面中心r_o1在整体坐标系下的三个坐标分量，

(r_o2x，r_o2y，r_o2z)是馈源舱顶端点r_o2在整体坐标系下的三个坐标分量；

4)用(r_o1x，r_o1y，r_o1z)和(r_o2x，r_o2y，r_o2z)分别代替所述馈源舱理想姿态公式中的(x_o1 ^d，y_o1 ^d，z_o1 ^d)和(x^*，y^*，z^*)，即可以得到馈源舱的实际方位角α′和馈源舱实际俯仰角γ′为：

${\mathrm{\α}}^{\′}=\mathrm{arctg}\frac{r_{o2y}-r_{o1y}}{r_{o2x}-r_{o1x}}$

${\mathrm{\γ}}^{\′}=\arccos \frac{r_{o2y}-r_{o1y}}{R_2-R_1};$

5)利用馈源舱的实际姿态角，即方位角α′和俯仰角γ′，进行刚体坐标转换，得到馈源舱和绳索第i个连接点在整体坐标系下的实际位置坐标信，即

$\left\{\begin{array}{c}{x}_i^{\′}=b_{\mathrm{ox}}+x^i\sin {\mathrm{\α}}^{\′}+y^i\cos {\mathrm{\α}}^{\′}\cos {\mathrm{\γ}}^{\′}+z^i\cos {\mathrm{\α}}^{\′}\sin {\mathrm{\γ}}^{\′}\\ y_i^{\′}=b_{\mathrm{oy}}-x^i\sin {\mathrm{\α}}^{\′}+y^i\sin {\mathrm{\α}}^{\′}\cos {\mathrm{\γ}}^{\′}+z^i\sin {\mathrm{\α}}^{\′}\sin {\mathrm{\γ}}^{\′}\\ z_i^{\′}==b_{\mathrm{oz}}-y^i\sin {\mathrm{\γ}}^{\′}+z^i\cos {\mathrm{\γ}}^{\′}\end{array}\right\}$

6)根据绳索张力传感器测量的绳索张力，通过如下公式求出绳索的实际长度L，

$L^2=\frac{l^2}{{\mathrm{\η}}^2}\sin h^2\mathrm{\η}+h^2$

其中，h是绳索两端的高度差， $\mathrm{\η}=-\frac{q_{0}l}{2F_x},$

l是其在水平平面上投影的长度，

q₀是绳索的线密度，

F_x是绳索张力在水平方向的分量。

4.根据权利求1所述的方法，其中步骤(6)所述的将绳索的差值通过力与位置的混合控制算法，计算出绳索需要的调整量，包括如下步骤：

1)将绳索理论长度和实际长度的差值或绳索理论张力和实际张力的差值，代入如下公式，得到绳索输入和输出的关系矩阵：

$\widehat{\mathrm{\θ}}(k+1)=\widehat{\mathrm{\θ}}\left(k\right)+\frac{k_{1}U\left(k\right)}{r_1+U{\left(k\right)}^{T}U\left(k\right)}(Y^T(k+1)-{\left(\widehat{\mathrm{\θ}}\left(k\right)U\left(k\right)\right)}^T)$

式中，

和

分别表示上一时刻和当前时刻输入和输出的关系矩阵，k₁表示权重因子，r₁表示遗忘因子，

Y^T(k+1)表示当前绳索理论长度和实际长度或张力的差值，

U(k)为上一个时刻输出的绳索调整量或力，k为调整次数；

2)根据馈源舱理想位置(x_O1，y_O1，z_O1)和馈源舱实际位置(r_o1x，r_o1y，r_o1z)的位置差值，对绳索输入和输出的关系矩阵准确性进行判断，

如果该位置差值小于设定的值，则按如下公式直接计算出绳索的长度调整量或张力调整量，

$U(k+1)=U\left(k\right)+\frac{k_2\widehat{\mathrm{\θ}}(k+1)}{r_2+{\left|\right|\widehat{\mathrm{\θ}}(k+1)\left|\right|}^2}(Y(k+1)-Y\left(k\right)),$

如果该位置差值大于设定的值，按照如下公式重新确定所述关系矩阵，即

$\widehat{\mathrm{\θ}}\left(n\right)=\widehat{\mathrm{\θ}}(n-1)+S\left(n\right)[Y(n+1)-U{\left(n\right)}^T\widehat{\mathrm{\θ}}(n-1)]$

其中， $S\left(n\right)=\frac{P(n-1)U\left(n\right)}{c+U{\left(n\right)}^{T}P(n-1)U\left(n\right)},$

$P\left(n\right)=\frac{1}{c}[P(n-1)-S\left(n\right)U{\left(n\right)}^{T}P(n-1)]$

S(n)为卡尔曼增益矢量，

P(n)为理想输出索长和实际输入索长误差的协方差矩阵，n为迭代的次数，c为更新因子；

3)用重新确定的矩阵

替代绳索输入和输出的关系矩阵

即 $\widehat{\mathrm{\θ}}(k+1)=\widehat{\mathrm{\θ}}\left(n\right),$ 再通过公式： $U(k+1)=U\left(k\right)+\frac{k_2\widehat{\mathrm{\θ}}(k+1)}{r_2+{\left|\right|\widehat{\mathrm{\θ}}(k+1)\left|\right|}^2}(Y(k+1)-Y\left(k\right))$ 计算出绳索长度的调整量或绳索张力的调整量，当Y^T(k+1)表示当前绳索理论长度和实际长度的差值，求出的U(k+1)表示绳索长度调整量，当Y^T(k+1)表示当前绳索理论张力和实际绳索张力的差值时，求出的U(k+1)表示绳索张力调整量。

5.根据权利求1所述的方法，其中(5)所述的通过绳索力传感器测量出绳索张力的方法，是将绳索力传感器固定在两个设定的定向轮之间，每个定向轮上开设有绳索槽，将绳索嵌在槽内，以保证绳索在两个定向轮之间的形状不变，并在力传感器的屏蔽传输线上增设RC，通过该屏蔽传输线将张力传感器测量的模拟信号送入计算机内数据采集卡，采用差分方式进行A/D转换，测量出绳索张力的数值。

Images