CN101870110B - 一种机械铰接臂的控制方法及控制装置 - Google Patents

Abstract

本发明涉及一种机械铰接臂控制方法，在铰接臂的不同位置上至少设置两个倾角传感器，该方法包括：标定铰接臂未发生弹性变形时倾角传感器的零点位置，设铰接臂弹性变形前、后末端位置分别为P点和P’点，在铰接臂上选取一R点；利用倾角传感器检测弹性变形前、后铰接臂两个不同位置的角度值，获得铰接臂因弹性变形产生的角度偏移量Δθ，结合OR和RP’的长度值，计算铰接臂变形后的长度参数L_a和角度参数θ_a；根据长度参数L_a和角度参数θ_a到P’点的位置参数X′_p和Y′_p，依据位置参数X′_p和Y′_p控制铰接臂动作。本发明还涉及一种机械铰接臂控制装置。本发明实现对铰接臂姿态的更加准确可靠的判断，增强对铰接臂的姿态的可测性与可控性，提高控制精度。

Description

一种机械铰接臂的控制方法及控制装置

技术领域

本发明涉机械铰接臂定位领域，特别是涉及一种机械铰接臂的控制方法控制及控制装置。

背景技术

众所周知，机械臂系统的传统控制方式是采用人工操作的手动模式，通过操作手操作不同的手柄以实现机械臂的多个关节协调运动，以达到目标位置，但该方式存在多种的缺陷。例如，人工操作时，特别是在需要多节机械臂联动协调的情况下，人工操作的效率与精准性往往比较低；人工操作对操作人员的操作的熟练程度要求比较高，且在操作过程中要求操作人员时刻关注铰接臂的移动情况，稍有不慎就有将施工人员甩下楼层的危险，因此存在安全性差，劳动强度大的缺陷。

随着社会与科学日新月异的进步，如何为劳动者提供更好的工作环境，如何最大程度的减轻劳动者的劳动强度，一直是机械臂系统研发工作的焦点与热点。1993年普茨迈斯特公司在US5640996A专利中提出通过遥控器手柄的单个调节实现多节机械臂的协调运动，使各机械臂能互不影响、相互独立的进行伸缩、旋转和升降运动，并在1994年申请的US5823218专利首次揭示了末端软管随动功能，即通过由操作人员引导末端软管移动至混凝土浇注点，实现机械臂随动功能。

尽管国内外一直都在尝试实现机械臂的机器人化，然而截至目前为止，其应用效果仍然未能充分满足工程化的要求，其实现的难点主要体现在控制的精准性方面。

参见图1，示出两节铰接臂简图，由图可知，二轴机械臂可绕关节O₁和O₂旋转，已知O₁、O₂长度为l₁，O₂A长度为l₂，由几何分析法，根据各关节转角θ₁、θ₂可建立机械臂的运动方程：

x＝l₁cosθ₁+l₂cos(θ₁+θ₂)                  式1

y＝l₁sinθ₁+l₂sin(θ₁+θ₂)                  式2

其中，(x，y)为末端A的坐标。

进一步，在机械臂控制及轨迹规划中，往往需要在已知点要达到的空间位置的情况下，求出各关节运动量，以驱动各关节的运动，使端点的位置得到满足。

将式1、式2化简，得：

$x^2+y^2=l_1^2+l_2^2+2l_1{l}_2[\cos {\mathrm{\θ}}_1\cos ({\mathrm{\θ}}_1+{\mathrm{\θ}}_2)+\sin {\mathrm{\θ}}_1\sin ({\mathrm{\θ}}_1+{\mathrm{\θ}}_2)]$

$=l_1^2+l_2^2+2l_1{l}_2\cos {\mathrm{\θ}}_2$                             式3

${\mathrm{\θ}}_2=\arccos \left(\frac{x^2+y^2-l_1^2-l_2^2}{2l_1{l}_2}\right)$                             式4

${\mathrm{\θ}}_1=\arctan \left(\frac{y}{x}\right)-\arccos \left(\frac{x^2+y^2+l_1^2-l_2^2}{2l_1\sqrt{x^2+y^2}}\right)$                             式5

这样，在要到达的位置A(X，Y)已知情况下，可得到所需的各关节转角θ₁、θ₂，驱动液压油缸控制机械臂转动，使得机械臂末端到达目标位置A(X，Y)。但是，机械臂运动因其自身的柔性可分解为刚性运动与柔性运动，上述计算方式忽略了机械架的柔性，则将导致最终的计算与规划结果与实际情况存在较大的偏差，影响控制精准性。

发明内容

本发明所要解决的技术问题是提供一种机械铰接臂的控制方法，该方法增强对铰接臂姿态的可测性与可控性，提高控制精度。

本发明一种机械铰接臂控制方法，在铰接臂的不同位置上至少设置两个倾角传感器，该方法包括：标定铰接臂未发生弹性变形时倾角传感器的零点位置，设铰接臂弹性变形前、后末端位置分别为P点和P’点，在铰接臂上选取一R点；利用倾角传感器检测弹性变形前、后铰接臂两个不同位置的角度值，获得铰接臂因弹性变形产生的角度偏移量Δθ，结合OR和RP’的长度值，计算铰接臂变形后的长度参数L_a和角度参数θ_a；其中，O点为所述铰接臂首端位置；根据长度参数L_a和角度参数θ_a得到P’点的位置参数X′_p和Y′_p，依据位置参数X′_p和Y′_p以及控制目标控制铰接臂动作。

优选的，在铰接臂不同位置上至少设置两个倾角传感器为：在每节铰接臂的首末两端各设有一个倾角传感器。

优选的，在铰接臂上选取一R点为：将铰接臂弹性变形后弧线的中间位置设置为R点，该中间位置到O点和P点的距离相等。

优选的，在铰接臂上选取一R点为：将铰接臂弹性变形后距离等效刚性铰接臂最远点。

优选的，利用倾角传感器检测弹性变形前、后铰接臂两个不同位置的角度值，获得铰接臂因弹性变形产生的角度偏移量Δθ包括以下步骤：设铰接臂弹性变形前首、末端的倾角传感器检测的角度值分别为θ_a0和θ_b0，因倾角传感器安装位置不与铰接臂轴线方向平行导致的角度差为Δθ₀，则：

Δθ₀＝θ_b0-θ_a0

设铰接臂首端位置为O点，利用倾角传感器分别检测O点的角度值与P’点的角度值，分别计作θ_a1与θ_b1，计算角度偏移量Δθ：

Δθ＝θ_b1-θ_a1-Δθ₀。

优选的，计算铰接臂弹性变形后长度参数L_a和角度参数θ_a包括以下步骤：

设∠ROP’和∠RP’O的角度值分别为∠1和∠2，则有；

Δθ＝∠1+∠2；

根据三角定理得到：

$\frac{\sin \∠1}{\left|\mathrm{OR}\right|}=\frac{\sin \∠2}{\left|{\mathrm{RP}}^{\′}\right|}=\frac{\sin (\mathrm{\π}-\mathrm{\Δ\θ})}{\left|{\mathrm{OP}}^{\′}\right|}$

将Δθ＝∠1+∠2代入得： $\sin (\mathrm{\Δ\θ}-\∠2)=\frac{\left|\mathrm{OR}\right|}{\left|{\mathrm{RP}}^{\′}\right|}\sin \∠2$

由此获得铰接臂的角度θ_a与长度L_a参数为： $\left\{\begin{array}{c}{\mathrm{\θ}}_a={\mathrm{\θ}}_{a1}-\∠1\\ L_a=\left|{\mathrm{OP}}^{\′}\right|=\frac{\left|{\mathrm{RP}}^{\′}\right|\sin \mathrm{\Δ\θ}}{\sin \∠2}\end{array}\right..$

优选的，根据长度参数L_a和角度参数θ_a得到P’点的位置参数X′_p和Y′_p具体为：

$\left\{\begin{array}{c}{X}_p^{\′}=L_a\cos {\mathrm{\θ}}_a\\ Y_p^{\′}=L_a\sin {\mathrm{\θ}}_a\end{array}\right..$

本发明还提供一种机械铰接臂的控制装置，该装置增强对铰接臂姿态的可测性与可控性，提高控制精度。

本发明一种机械铰接臂控制装置，包括铰接臂，还包括设置在铰接臂不同位置上的至少两个倾角传感器，与所述倾角传感器连接的控制器；所述控制器包括计算单元和驱动单元：所述计算单元，用于标定铰接臂未发生弹性变形时倾角传感器的零点位置，设铰接臂弹性变形前、后末端位置分别为P点和P’点，在铰接臂上选取一R点；利用倾角传感器检测弹性变形前、后铰接臂两个不同位置的角度值，获得铰接臂因弹性变形产生的角度偏移量Δθ，结合OR和RP’的长度值，计算铰接臂变形后的长度参数L_a和角度参数θ_a；根据长度参数L_a和角度参数θ_a得到P’点的位置参数X′_p和Y′_p，依据位置参数X′_p和Y′_p计算驱动量；其中，O点为所述铰接臂首端位置；所述驱动单元，用于依据驱动量控制铰接臂动作。

优选的，在铰接臂不同位置上至少设置两个倾角传感器为：在每节铰接臂的首末两端各设有一个倾角传感器。

优选的，在铰接臂上选取一R点为：将铰接臂弹性变形后弧线的中间位置设置为R点，该中间位置到O点和P点的距离相等。优选的，在铰接臂上选取一R点为：将铰接臂弹性变形后距离等效刚性铰接臂最远点。

与现有技术相比，本发明具有以下优点：

本发明充分考虑铰接臂在运动过程中的弹性变形，利用机械臂变形前后的角度变化值进行推理与运算，实现对铰接臂姿态的更加准确可靠的判断，增强对铰接臂的姿态的可测性与可控性，提高控制精度。

附图说明

图1为现有两节铰接臂示意图；

图2为本发明机械铰接臂控制方法流程图；

图3为本发明铰接臂弹性变形后第一示意图；

图4为本发明铰接臂弹性变形后第二示意图；

图5为本发明铰接臂弹性变形后第三示意图；

图6为本发明五节铰接臂弹性变形后示意图；

图7为本发明铰接臂控制装置示意图。

具体实施方式

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

本发明旨在提供一种通过在一个机械臂的不同位置(如：首端与末端)安装两个或两个以上的倾角传感器，然后利用倾角传感器测量出的角度值来获取机械臂的弹性变形后姿态参数，通过数学运算获取弹性变形后的末端坐标位置参数，再根据该末端位置参数控制机械动作。

参见图2，示出本发明机械铰接臂控制方法，具体步骤如下。

步骤S201、在机械铰接臂的首末两端各安装一个倾角传感器。当然，也可以根据机械铰接臂的长度及材料特性确定倾角传感器的安装位置，例如在铰接臂的L/8，L/4，L/2处。

步骤S202、标定铰接臂未发生弹性变形时倾角传感器的零点位置，设铰接臂弹性变形前、后末端位置分别为P点和P’点，在铰接臂上选取一R点。零点位置是指将铰接臂至于一个具备完备支撑结构的支架上，确保铰接臂处于没有弹性变形的姿态时铰接臂的位置，该支撑结构可以是水平的，也可以是沿着铰接臂伸展方向倾斜的，只需要取保铰接臂可以被完备支撑，没有发生弹性变形即可。

步骤S203、利用倾角传感器检测弹性变形前、后铰接臂两个不同位置的角度值，获得铰接臂因弹性变形产生的角度偏移量Δθ，结合OR和RP’的长度值，计算铰接臂变形后的长度参数L_a和角度参数θ_a；

步骤S204、根据长度参数L_a和角度参数θ_a得到P’点的位置参数X′_p和Y′_p。

步骤S205、依据P’点的位置参数X′_p和Y′_p控制铰接臂动作。

本发明充分考虑铰接臂在运动过程中的弹性变形，利用机械臂变形前后的角度变化值进行推理与运算，实现对铰接臂姿态的更加准确可靠的判断，增强对铰接臂的姿态的可测性与可控性，提高控制精度。

下面以一节铰接臂为例，详细说明本发明计算过程。如图3所示，假设铰接臂不变形的方向为OP向量方向，铰接臂变形后则沿ORP弧线方向。设定R点为铰接臂变形后的弧线的中点位置(该中点位置到O点与P点的直线距离相等)。当然在铰接臂变形分布情况已知或可测的情况下，也可将R点近似设定为铰接臂上距离变形后的等效刚性铰接臂(OP’段)距离最远点(如下图4所示)。在R点人为确定的情况下，则OR与RP的长度也是可测的，使得折线ORP可以更加近似的描述变形后的沿ORP弧线伸展的铰接臂的姿态。

更进一步，为了更好的近似铰接臂的变形情况，可以在铰接臂的不同位置如：L/8，L/4，L/2处各设置一个采样点M点，N点，R点，则折线OMNRP可以更加近似的描述铰接臂OP的变形情况，其实质是一种折线近似曲线的办法，其采样点数越多，精确程度也会越大。实际应用时，采样点的个数的选择一方面需要考虑采样器件的成本因素，另一方面也需要考虑采样器件的采样精度问题，综合以上两方面的因素，根据应用精度需求而选择适合自己的方案。

由图3可知，变形后的铰接臂ORP可等效刚性铰接臂OP′，若可求出OP′的长度与角度参数，则可将柔性铰接臂的问题转化为刚性铰接臂的正运动与逆运动的求解问题。如下介绍如何利用在机械铰接臂的至少两个不同的位置，安装至少两个倾角传感器所测量到的角度差值，计算铰接臂变形后的精确坐标参数。

在铰接臂仅存在微小变形时，倾角传感器的优选安装位置为铰接臂的首端与末端，如图3中的O位置与B位置(铰接臂不发生变形时的位置)或B’位置(铰接臂发生弹性变形后的位置)，其中的倾角传感器所在的B位置坐标已知，该坐标参数可以用OP的长度(即铰接臂的长度参数)及OP的角度参数加以描述，忽略铰接臂铰接点的变形，可近似认为OP的角度参数即为A点倾角传感器A测量出的角度值θ_a0(见图5)。

若将铰接臂OP置于一个具备完备支撑结构的支架上，使得铰接臂处于未发生弹性变形的姿态，则可认为OP的角度参数同样可以用B点的倾角传感器B测量出的角度值描述θ_b0，此时，当倾角传感器A与倾角传感器B的安装位置与铰接臂轴线方向完全平衡时，则存在如下关系

θ_a0＝θ_b0                                   式6

由于实际应用时倾角传感器A的安装位置与倾角传感器B的安装位置，可能存在不完全平行于铰接臂的轴线方向，由此可能导致两个倾角传感器的测量值存在有起始偏差，即

θ_a0≠θ_b0                               式7

将铰接臂未发生弹性变形的情况下倾角传感器A和倾角传感器B的测量值记为零点位置值，分别记忆为θ_a0，θ_b0

令：

Δθ₀＝θ_b0-θ_a0

即：

θ_b0＝θ_a0+Δθ₀                      式8

如图4所示，铰接臂发生变形后，倾角传感器A测得其相对零点位置的角度值为θ_a1，倾角传感器B测得的其相对零点的角度值为θ_b1，铰接臂变形后的偏移角度值为Δθ，综上所述有如下关系成立。

Δθ＝θ_b1-θ_a1-Δθ₀                 式9

由图4可知，在R点选择满足OR可近似为铰接臂弹性变形前方向的条件下，对于ΔORP′有如下关系成立

Δθ＝∠1+∠2                                     式10

忽略铰接臂变形时铰接臂的长度参数的变化，假设已知铰接臂长|OP|＝L，则有：

|OR|+|RP′|＝L                                    式11

由三角定理可知道

$\frac{\sin \∠1}{\left|\mathrm{OR}\right|}=\frac{\sin \∠2}{\left|{\mathrm{RP}}^{\′}\right|}=\frac{\sin (\mathrm{\π}-\mathrm{\Δ\θ})}{\left|{\mathrm{OP}}^{\′}\right|}$                                             式12

即： $\left\{\begin{array}{c}\sin \∠1=\frac{\left|\mathrm{OR}\right|}{\left|{\mathrm{RP}}^{\′}\right|}\sin \∠2\\ \left|{\mathrm{OP}}^{\′}\right|=\frac{\left|{\mathrm{RP}}^{\′}\right|\sin \mathrm{\Δ\θ}}{\sin \∠2}\end{array}\right.$                                         式13

将Δθ＝∠1+∠2代入上式可得

$\sin (\mathrm{\Δ\θ}-\∠2)=\frac{\left|\mathrm{OR}\right|}{\left|{\mathrm{RP}}^{\′}\right|}\sin \∠2$                                               式14

展开后： $\sin \mathrm{\Δ\θ}\cos \∠2+\cos \mathrm{\Δ\θ}\sin \∠2=\frac{\left|\mathrm{OR}\right|}{\left|{\mathrm{RP}}^{\′}\right|}\sin \∠2$                                       式15

即： $\cos \∠2=(\frac{\left|\mathrm{OR}\right|}{\left|{\mathrm{RP}}^{\′}\right|}-\cos \mathrm{\Δ\θ})\sin \∠2/\sin \mathrm{\Δ\θ}$                                           式16

令 $k=(\frac{\left|\mathrm{OR}\right|}{\left|{\mathrm{RP}}^{\′}\right|}-\cos \mathrm{\Δ\θ})/\sin \mathrm{\Δ\θ},$

则有：cos∠2＝ksin∠2                               式17

又：cos²∠2+sin²∠2＝1

综上可得： $\left\{\begin{array}{c}\∠2=\arcsin \left(\sqrt{1/1+k^2}\right)\\ \∠1=\mathrm{\Δ\θ}-\∠2\\ {\mathrm{OP}}^{\′}=\frac{\left|{\mathrm{RP}}^{\′}\right|\sin \mathrm{\Δ\θ}}{\sin \∠2}\end{array}\right.$                                         式18

由此可以获得铰接臂的角度(θ_a)与长度(L_a)参数为：

$\left\{\begin{array}{c}{\mathrm{\θ}}_a={\mathrm{\θ}}_{a1}-\∠1\\ L_a=\left|{\mathrm{OP}}^{\′}\right|=\frac{\left|{\mathrm{RP}}^{\′}\right|\sin \mathrm{\Δ\θ}}{\sin \∠2}\end{array}\right.$

铰接臂形变后末端的位置为：

$\left\{\begin{array}{c}{X}_p^{\′}=L_a\cos {\mathrm{\θ}}_a\\ Y_p^{\′}=L_a\sin {\mathrm{\θ}}_a\end{array}\right.$

这样，就可以利用铰接臂形变后末端位置控制铰接臂动作，该控制方式充分考虑了铰接臂的弹性变形，提高控制的精确性。

存在两节铰接臂或者两节以上铰接臂时，只须将与前面一节铰接臂的铰接点看作是上述坐标的O点，则其求解方法可依照如上所述式7到式18的方法执行。

如图6所示，以五节铰接臂的求解为例子，参照如上方法，可依次得出第二节铰接臂2#，第三节铰接臂3#，第四节铰接臂4#，第五节铰接臂5#的角度和长度参数θ_b，θ_c，θ_d，θ_e，L_b，L_c，L_d，L_e。

由此计算出的铰接臂变形后末端的坐标参数为：

$\left\{\begin{array}{c}{X}_e=L_a\cos {\mathrm{\θ}}_a+L_b\cos {\mathrm{\θ}}_b+L_c\cos {\mathrm{\θ}}_c+L_d\cos {\mathrm{\θ}}_d+L_e\cos {\mathrm{\θ}}_e\\ Y_e=L_a\sin {\mathrm{\θ}}_a+L_b\sin {\mathrm{\θ}}_b+L_c\sin {\mathrm{\θ}}_c+L_d\sin {\mathrm{\θ}}_d+L_e\sin {\mathrm{\θ}}_e\end{array}\right.$

基于上述机械铰接臂控制方法，本发明还提供一种机械铰接臂控制装置，见图7，包括铰接臂11，还包括设置在铰接臂11上的倾角传感器12和倾角传感器13，与倾角传感器12和倾角传感器13的控制器14，控制器14包括计算单元141和驱动单元142。

计算单元141所述计算单元，用于标定铰接臂未发生弹性变形时倾角传感器的零点位置，设铰接臂弹性变形前、后末端位置分别为P点和P’点，在铰接臂上选取一R点；利用倾角传感器检测弹性变形前、后铰接臂两个不同位置的角度值，获得铰接臂因弹性变形产生的角度偏移量Δθ，结合OR和RP’的长度值，计算铰接臂变形后的长度参数L_a和角度参数θ_a；根据长度参数L_a和角度参数θ_a得到P’点的位置参数X′_p和Y′_p，依据位置参数X′_p和Y′_p计算驱动量；

驱动单元142依据位置参数X′_p和Y′_p控制铰接臂11动作。

以上对本发明所提供的机械铰接臂的控制方法和控制装置，进行了详细介绍，本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。

Claims (11)

1.一种机械铰接臂控制方法，其特征在于，在铰接臂的不同位置上至少设置两个倾角传感器，该方法包括：

标定铰接臂未发生弹性变形时倾角传感器的零点位置，设铰接臂弹性变形前、后末端位置分别为P点和P’点，在铰接臂上选取一R点；

利用倾角传感器检测弹性变形前、后铰接臂两个不同位置的角度值，获得铰接臂因弹性变形产生的角度偏移量Δθ，结合OR和RP’的长度值，计算铰接臂变形后的长度参数L_a和角度参数θ_a；其中，O点为所述铰接臂首端位置；

根据长度参数L_a和角度参数θ_a得到P’点的位置参数X′_p和Y′_p，依据位置参数X′_p和Y′_p控制铰接臂动作。

2.如权利要求1所述的机械铰接臂控制方法，其特征在于，在铰接臂不同位置上至少设置两个倾角传感器为：

在每节铰接臂的首末两端各设有一个倾角传感器。

3.如权利要求2所述的机械铰接臂控制方法，其特征在于，在铰接臂上选取一R点为：

将铰接臂弹性变形后弧线的中间位置设置为R点，该中间位置到O点和P点的距离相等。

4.如权利要求2所述的机械铰接臂控制方法，其特征在于，在铰接臂上选取一R点为：

将铰接臂弹性变形后距离等效刚性铰接臂最远点。

5.如权利要求2、3或4所述的机械铰接臂控制方法，其特征在于，利用倾角传感器检测弹性变形前、后铰接臂两个不同位置的角度值，获得铰接臂因弹性变形产生的角度偏移量Δθ包括以下步骤：

设铰接臂弹性变形前首、末端的倾角传感器检测的角度值分别为θ_a0和θ_b0，因倾角传感器安装位置不与铰接臂轴线方向平行导致的角度差为Δθ₀，则：

Δθ₀＝θ_b0-θ_a0

利用倾角传感器分别检测O点的角度值与P’点的角度值，分别计作θ_a1与θ_b1，计算角度偏移量Δθ：

Δθ＝θ_b1-θ_a1-Δθ₀。

6.如权利要求5所述的机械铰接臂控制方法，其特征在于，计算铰接臂变形后的长度参数L_a和角度参数θ_a包括以下步骤：

设∠ROP’和∠RP’O的角度值分别为∠1和∠2，则有；

Δθ＝∠1+∠2；

根据三角定理得到：

$\frac{\sin \∠1}{\left|\mathrm{OR}\right|}=\frac{\sin \∠2}{\left|{\mathrm{RP}}^{\′}\right|}=\frac{\sin (\mathrm{\π}-\mathrm{\Δ\θ})}{\left|{\mathrm{OP}}^{\′}\right|}$

将Δθ＝∠1+∠2代入得： $\sin (\mathrm{\Δ\θ}-\∠2)=\frac{\left|\mathrm{OR}\right|}{\left|{\mathrm{RP}}^{\′}\right|}\sin \∠2$

由此获得铰接臂的角度θ_a与长度L_a参数为： $\left\{\begin{array}{c}{\mathrm{\θ}}_a={\mathrm{\θ}}_{a1}-\∠1\\ L_a=\left|{\mathrm{OP}}^{\′}\right|=\frac{\left|{\mathrm{RP}}^{\′}\right|\sin \mathrm{\Δ\θ}}{\sin \∠2}\end{array}\right..$

7.如权利要求6所述的机械铰接臂控制方法，其特征在于，根据长度参数L_a和角度参数θ_a得到P’点的位置参数X′_p和Y′_p具体为：

$\left\{\begin{array}{c}{X}_p^{\′}=L_a\cos {\mathrm{\θ}}_a\\ Y_p^{\′}=L_a\sin {\mathrm{\θ}}_a\end{array}\right..$

8.一种机械铰接臂控制装置，包括铰接臂，其特征在于，还包括设置在铰接臂不同位置上的至少两个倾角传感器，与所述倾角传感器连接的控制器；所述控制器包括计算单元和驱动单元：

所述计算单元，用于标定铰接臂未发生弹性变形时倾角传感器的零点位置，设铰接臂弹性变形前、后末端位置分别为P点和P’点，在铰接臂上选取一R点；利用倾角传感器检测弹性变形前、后铰接臂两个不同位置的角度值，获得铰接臂因弹性变形产生的角度偏移量Δθ，结合OR和RP’的长度值，计算铰接臂变形后的长度参数L_a和角度参数θ_a；根据长度参数L_a和角度参数θ_a得到P’点的位置参数X′_p和Y′_p，依据位置参数X′_p和Y′_p计算驱动量；其中，O点为所述铰接臂首端位置；

所述驱动单元，用于依据驱动量控制铰接臂动作。

9.如权利要求8所述的机械铰接臂控制装置，其特征在于，在铰接臂不同位置上至少设置两个倾角传感器为：

在每节铰接臂的首末两端各设有一个倾角传感器。

10.如权利要求8或9所述的机械铰接臂控制装置，其特征在于，在铰接臂上选取一R点为：

将铰接臂弹性变形后弧线的中间位置设置为R点，该中间位置到O点和P点的距离相等。

11.如权利要求8或9所述的机械铰接臂控制装置，其特征在于，在铰接臂上选取一R点为：

将铰接臂弹性变形后距离等效刚性铰接臂最远点。

Images