CN104182614A - 六自由度机械臂姿态监测系统及方法 - Google Patents

Abstract

本发明公开了六自由度机械臂姿态监测系统及方法，包括机械臂整体系统的若干个监测节点，每个监测节点处安装有监测装置，监测装置有监控终端通过Zigbee无线通信模块控制启停，Zigbee无线通信模块与中央处理单元通信，中央处理单元与监测装置通信，用于接收监测装置所监测的监测节点的信息。利用无线传感网络对机械臂整体系统进行姿态监测，通过监测终端和若干监测节点实现同时对多个节点的监测，克服了传统只对单个点姿态监测的缺陷。

Description

六自由度机械臂姿态监测系统及方法

技术领域

本发明涉及六自由度机械臂姿态监测系统及方法。

背景技术

机械臂在工程机械技术领域具有重要作用，机械臂姿态不精确的问题会导致机械臂的控制难以满足实际需求。对机械臂整体系统的姿态监测可以使机械臂操作精准，以使机械臂达到预定位置、实施预定动作。由此机械臂整体系统的姿态监测的正常工作对具有机械臂的工程机械的正常工作同样具有重要作用。

传统的机械臂姿态监测是根据机械臂的结构特点和运动学约束，对机械臂的运动学进行分析。采用Denavit-Hartenberg(D-H)法对机械臂操作空间进行描述，在考虑机械臂运动学约束的基础上，得到以关节角度为变量的正运动学模型。通过分析正运动学模型的可解性，采用矩阵逆乘的解析法求解机械臂的正运动学模型，进而得到了机械臂逆运动学的解。利用D-H方法对机械臂进行分析建模：首先，给每个关节指定一个参考坐标系；然后，确定如何实现任意2个相邻坐标系之间的变换；最后，写出机械臂的总变换矩阵。

综上所述，传统方法的不足可以总结为传统的机械臂整体系统的姿态监测方法是通过将机械臂各个关节进行D-H建模，然后通过各个轴之间复杂的矩阵变换得出总的变换矩阵从而推导出末端的姿态。这种方式存在计算量大，计算过程容易出错，降低姿态监测的精度，且工作复杂效率低等问题。

发明内容

为解决现有技术存在的不足，本发明公开了六自由度机械臂姿态监测系统及方法，本发明从机械臂整体结构为出发点，在机械臂各个关节之间协调工作时，求取某个臂的姿态坐标时不需要经过复杂的坐标变换得出，可以直接经过姿态监测模块测量计算得出。本发明将复杂的矩阵变换转化为三角函数的计算，直接通过角度相关计算推导出末端的姿态。简化了计算过程，提高了测量精度。同时姿态监测模块可直接安装在多自由度机械臂的关节处，不需要改变机械臂原有的安装来进行复杂的安装过程，并且其结构简单、抗干扰性强。

为实现上述目的，本发明的具体方案如下：

六自由度机械臂姿态监测系统，包括布置在机械臂整体系统的若干个监测节点，每个监测节点处安装有监测装置，每个监测装置有监控终端通过Zigbee无线通信模块控制启停，Zigbee无线通信模块与中央处理单元通信，中央处理单元与监测装置通信，中央处理单元接收监测装置所监测的监测节点的信息。

所述监测装置包括用于测量机械臂关节的加速度值的三轴加速度传感器，用于测量机械臂关节的地磁感应强度分量的三轴地磁传感器及用于测量械臂关节的角速度值的三轴陀螺仪。

所述若干个监测节点包括机械臂基座的第一监测节点、机械臂关节处的第二监测节点、机械臂关节处的第三监测节点、机械臂关节处的第四监测节点、机械臂关节处的第五监测节点、机械臂关节处的第六监测节点及机械臂关节处的第七监测节点。

六自由度机械臂姿态监测方法，包括以下步骤：

步骤一：中央处理单元读取三轴地磁传感器测量出的机械臂各关节的三维地磁感应强度分量值m_x、m_y、m_z、三轴加速度传感器测量出的机械臂各关节的三维加速度分量值α_x、α_y、α_z及三轴陀螺仪测量出的机械臂各关节的角速度；

步骤二：对读取的各关节的三维地磁感应强度分量、各关节的三维加速度分量值利用四元数算法进行归一化处理，对读取的陀螺仪角速度进行偏差补偿后的四元数向量进行归一化处理；

步骤三：将步骤二归一化处理后所得四元数转换成欧拉角即旋转角；将各部分进行多次测量测得的多个旋转角取其均值；

步骤四：根据步骤三中的旋转角、机械臂的臂长及各关节坐标系通过三角函数计算得出机械臂最终姿态。

所述步骤一中还包括测量得到三维加速度值分量α_x、α_y、α_z及三维地磁感应分量m_x、m_y、m_z，对分量进行校准，校准的目的是为了去除由外界环境和测量过程中造成的误差，即实验前先分别对多次测得的加速度值和地磁感应三个分量取平均值，以此平均值作为标定值校准当次测量值的准确性。计算加速度参考值acc和地磁感应强度参考值mag；

其中： $\mathrm{acc}=\sqrt{a_x^2+a_y^2+a_z^2};\mathrm{mag}=\sqrt{m_x^2+m_y^2+m_z^2}.$

所述步骤二中具体包括：

当加速度测量值和参考值acc的偏差小于设定阈值时，加速度的四元数表示通过归一化处理得到： $A_t^s=\frac{\mathrm{\α}_t^m{}_s}{\left|\right|\mathrm{\α}_t^m{}_s\left|\right|}=\left[\begin{array}{cccc}0& {\mathrm{\α}}_{x,t}& {\mathrm{\α}}_{y,t}& {\mathrm{\α}}_{z,t}\end{array}\right],$ 否则，由对地坐标系中的加速度通过四元数转换得到： $A_t^s=\hat{q}_{t-\mathrm{\Δt}}^{*}{}_e{}^s\⊗z_a^e\⊗\hat{q}_{t-\mathrm{\Δt}}^e{}_s;$

其中各式中参数含义如下：

加速度测量值其中^sa_t＝^sa_b,t+^sg_t，^sa_t是经过误差补偿的线性加速度，^sa_b,t是重力补偿后物体的线性加速度，b_a,t是由低频偏移引起的偏差，e_a,t为零均值白噪声，^sg_t的取值取决于传感器定位；^sA_t为加速度测量值的归一化表示即加速度的四元数表示，为加速度测量值的幅值，α_x,t、α_y,t、α_z,t分别为时间t时的三个方向的加速度分量，为在测量时间内的单位四元数表示，为的共轭，^ez_a＝[0 0 0 1]。

当地磁感应强度测量值和参考值mag的偏差小于设定阈值时，地磁感应强度的四元数表示通过归一化处理得到： $M_t^s=\frac{m_t^m{}_s}{\left|\right|m_t^m{}_s\left|\right|}=\left[\begin{array}{cccc}0& m_{x,t}& m_{y,t}& m_{z,t}\end{array}\right],$ 否则，由对地坐标系中的地磁通过四元数转换得到： $z_{m,t}^e=\hat{q}_{t-\mathrm{\Δt}}^e{}_s\⊗M_{t-\mathrm{\Δt}}^s\⊗\hat{q}_{t-\mathrm{\Δt}}^{*}{}_s{}^s,M_t^s=\hat{q}_t^{*}{}_e{}^s\⊗z_{m,t}^e\⊗\hat{q}_t^e{}_s;$

其中，地磁感应强度测量值其中，^sm_t为地磁场的磁感应强度，d_m,t为磁场干扰，b_m,t为磁场偏差，e_m,t为噪声，m_x,t、m_y,t、m_z,t分别为时间t时的三个方向的地磁感应强度分量，^sM_t为时间t时的地磁感应强度测量值的归一化表示即地磁感应强度的四元数表示，^sM_t-Δt为测量时间内的地磁感应强度， $z_{m,t}^e=\left[\begin{array}{cccc}0& \stackrel{\‾}{m_x}& \stackrel{\‾}{m_y}& \stackrel{\‾}{m_z}\end{array}\right],$ 为在时间t时的单位四元数表示，为的共轭。

根据高斯牛顿法则，寻找 $\hat{q}_t^e{}_s=\left[\begin{array}{cccc}\widehat{q_o}& \widehat{q_x}& \widehat{q_y}& \widehat{q_z}\end{array}\right],$ 使 $f\left(\widehat{q_t^e{}_s}\right)=\frac{1}{2}\mathrm{\ϵ}{\left(\widehat{q_t^e{}_s}\right)}^T\mathrm{\ϵ}\left(\widehat{q_t^e{}_s}\right)$ 取最小值，以便于求取使陀螺仪角速度误差最小的其中为误差函数；

求解陀螺仪偏差漂移补偿四元数梯度向量，并对四元数梯度向量进行归一化处理： $\stackrel{\·}{q}_{\mathrm{\ϵ},t+\mathrm{\Δt}}^e{}_s=\frac{\mathrm{\Δ}\hat{q}}{\left|\right|\mathrm{\Δ}\hat{q}\left|\right|};$

其中为传感器坐标系相对于地坐标系的陀螺仪方向的四元数的变化率，为单位四元数的变化。

所述对读取的陀螺仪角速度进行偏差补偿，角度误差角度偏差补偿后的传感器坐标系下的角度向量

其中，^sω_e,t+Δt为陀螺仪轴中补偿后的角误差，表示互补滤波融合后被估计方向的四元数表示的共轭，被用作陀螺仪偏差漂移补偿的四元数表示变化率，^sω_b,t+Δt为角偏移，为抵消非零均值陀螺仪测量误差的收敛率，^sω_t+Δt为角速度。

计算由陀螺仪数据估算的四元数梯度向量：其中为陀螺仪测量的四元数变化率，为陀螺仪的被估计方向的单位四元数表示；

计算经过互补滤波器对陀螺仪偏差漂移进行的补偿，最终得到的角速度四元数向量 $\hat{q}_{f,t+\mathrm{\Δt}}^e{}_s=\hat{q}_{f,t}^e{}_s+\stackrel{\·}{q}_{f,t+\mathrm{\Δt}}^e{}_s\mathrm{\Δt},$ 其中 $\stackrel{\·}{q}_{f,t+\mathrm{\Δt}}^e{}_s=\stackrel{\·}{q}_{\mathrm{\ω},t+\mathrm{\Δt}}^e{}_s-\mathrm{\β}\stackrel{\·}{q}_{\mathrm{\ϵ},t+\mathrm{\Δt}}^e{}_s,$ 并进行归一化处理；

其中互补滤波器融合的被估计方向的四元数表示，是互补滤波融合后被估计方向的四元数变化率，为陀螺仪测量的四元数变化率，β为滤波器增益；

所述步骤三中将步骤二所得四元数转换成欧拉角。由于机械臂每个部分只能进行单一运动方式如只能是俯仰、旋转、摆动中的一种，所以求取的旋转角只能是由相应的运动产生的横摆角、俯仰角、滚摆角中的一种角度。其中，ψ、θ、分别为绕Z轴、Y轴、X轴的旋转角度对应横摆角、俯仰角、滚摆角。

四元数与欧拉角的转换关系如下：

若四元数q＝[w x y z]^T，则旋转角度可由下式得出：

所述步骤四中，将各部分进行多次测量测得的多个旋转角取其均值，以确保测量的准确性。

在每个关节处最终的旋转角其中α为已求取的旋转角，n为测量的次数。

所述步骤四的具体步骤为：通过旋转角和臂长在相应坐标系中的投影测得各部分的姿态后，通过各部分的叠加得到机械臂最终的姿态；

在机械臂基座的监测节点100处建坐标系xyz，作为机械臂整体系统的基准，其中a₀、a₁、a₂、a₃、a₄、a₅分别为相邻两个关节之间的臂长；

在机械臂关节处的第一监测节点100处建坐标系x₀y₀z₀，作为基准，第一监测节点对应的位姿；

$\left\{\begin{array}{c}{x}_0=0\\ y_0=0\\ z_0=0\end{array}\right.$

在机械臂关节处的第二监测节点101处建坐标系x₁y₁z₁，第一关节旋转α₀，第二监测节点对应的位姿；

$\left\{\begin{array}{c}{x}_1=0\\ y_1=0\\ z_1=a_0\end{array}\right..$

在机械臂关节处的第三监测节点102处建坐标系x₂y₂z₂，第二关节旋转α₁，第三监测节点对应的位姿；

$\left\{\begin{array}{c}{x}_2=a_1\cos {\mathrm{\α}}_1\sin {\mathrm{\α}}_0+x_1\\ y_2=a_1\cos {\mathrm{\α}}_1\cos {\mathrm{\α}}_0+y_1\\ z_2=a_1\sin {\mathrm{\α}}_1+z_1\end{array}\right..$

在机械臂关节处的第四监测节点103处建坐标系x₃y₃z₃，第三关节旋转α₂，第四监测节点对应的位姿；

$\left\{\begin{array}{c}{x}_3=a_2\sin {\mathrm{\α}}_0\cos ({\mathrm{\α}}_1+{\mathrm{\α}}_2)+x_2\\ y_3=a_2\cos {\mathrm{\α}}_0\cos ({\mathrm{\α}}_1+{\mathrm{\α}}_2)+y_2\\ z_3=a_2\sin ({\mathrm{\α}}_1+{\mathrm{\α}}_2)+z_2\end{array}\right..$

在机械臂关节处的第五监测节点104处建坐标系x₄y₄z₄，第四关节旋转α₃，第五监测节点对应的位姿；

$\left\{\begin{array}{c}{x}_4=a_3\sin {\mathrm{\α}}_0\cos ({\mathrm{\α}}_1+{\mathrm{\α}}_2+{\mathrm{\α}}_3)+x_3\\ y_4=a_3\cos {\mathrm{\α}}_0\cos ({\mathrm{\α}}_1+{\mathrm{\α}}_2+{\mathrm{\α}}_3)+y_3\\ z_4=a_3\sin ({\mathrm{\α}}_1+{\mathrm{\α}}_2+{\mathrm{\α}}_3)+z_3\end{array}\right..$

在机械臂关节处的第六监测节点105处建坐标系x₅y₅z₅，第五关节旋转α₄，第六监测节点对应的位姿；

$\left\{\begin{array}{c}{x}_5=a_4\sin ({\mathrm{\α}}_0+{\mathrm{\α}}_4)\cos ({\mathrm{\α}}_1+{\mathrm{\α}}_2+{\mathrm{\α}}_3)+x_4\\ y_5=a_4\cos ({\mathrm{\α}}_0+{\mathrm{\α}}_4)\cos ({\mathrm{\α}}_1+{\mathrm{\α}}_2+{\mathrm{\α}}_3)+y_4\\ z_5=a_4\sin ({\mathrm{\α}}_1+{\mathrm{\α}}_2+{\mathrm{\α}}_3)+z_4\end{array}\right..$

在机械臂关节处的第七监测节点106处建坐标系x₆y₆z₆，第六关节旋转α₅，第七监测节点对应的位姿；

$\left\{\begin{array}{c}{x}_6=a_5\sin ({\mathrm{\α}}_0+{\mathrm{\α}}_4)\cos ({\mathrm{\α}}_1+{\mathrm{\α}}_2+{\mathrm{\α}}_3+{\mathrm{\α}}_5)+x_5\\ y_6=a_5\cos ({\mathrm{\α}}_0+{\mathrm{\α}}_4)\cos ({\mathrm{\α}}_1+{\mathrm{\α}}_2+{\mathrm{\α}}_3+{\mathrm{\α}}_5)+y_5\\ z_6=a_5\sin ({\mathrm{\α}}_1+{\mathrm{\α}}_2+{\mathrm{\α}}_3+{\mathrm{\α}}_5)+z_5\end{array}\right.$

将各个关节的姿态累加得到机械臂的最终姿态为

$\left\{\begin{array}{c}x=a_1\sin {\mathrm{\α}}_0\cos {\mathrm{\α}}_1+a_2\sin {\mathrm{\α}}_0\cos ({\mathrm{\α}}_1+{\mathrm{\α}}_2)+a_3\sin {\mathrm{\α}}_0\cos ({\mathrm{\α}}_1+{\mathrm{\α}}_2+{\mathrm{\α}}_3)+a_4\sin ({\mathrm{\α}}_0+{\mathrm{\α}}_4)\cos ({\mathrm{\α}}_1+{\mathrm{\α}}_2+{\mathrm{\α}}_3)\\ +a_5\sin ({\mathrm{\α}}_0+{\mathrm{\α}}_4)\cos ({\mathrm{\α}}_1+{\mathrm{\α}}_2+{\mathrm{\α}}_3+{\mathrm{\α}}_5)\\ y=a_1\cos {\mathrm{\α}}_0\cos {\mathrm{\α}}_1+a_2\cos {\mathrm{\α}}_0\cos ({\mathrm{\α}}_1+{\mathrm{\α}}_2)+a_3\cos {\mathrm{\α}}_0\cos ({\mathrm{\α}}_1+{\mathrm{\α}}_2+{\mathrm{\α}}_3)+a_4\cos ({\mathrm{\α}}_0+{\mathrm{\α}}_4)\cos ({\mathrm{\α}}_1+{\mathrm{\α}}_2+{\mathrm{\α}}_3)\\ +a_5\cos ({\mathrm{\α}}_0+{\mathrm{\α}}_4)\cos ({\mathrm{\α}}_1+{\mathrm{\α}}_2+{\mathrm{\α}}_3+{\mathrm{\α}}_5)\\ z=a_0+a_1\sin {\mathrm{\α}}_1+a_2\sin ({\mathrm{\α}}_1+{\mathrm{\α}}_2)+a_3\sin ({\mathrm{\α}}_1+{\mathrm{\α}}_2+{\mathrm{\α}}_3)+a_4\sin ({\mathrm{\α}}_1+{\mathrm{\α}}_2+{\mathrm{\α}}_3)+a_5\sin ({\mathrm{\α}}_1+{\mathrm{\α}}_2+{\mathrm{\α}}_3+{\mathrm{\α}}_5)\end{array}\right.$

本发明的有益效果：

1)使用多个数字式传感器和高速中央处理单元建立姿态监测模块优化算法，将复杂的矩阵变换转化为三角函数的计算，简化了计算过程，提高了测量精度。

2)姿态监测模块可直接安装在多自由度机械臂的关节处，不需要改变机械臂原有的安装来进行复杂的安装过程，并且其结构简单、抗干扰性强。

3)利用无线传感网络对机械臂整体系统进行姿态监测，通过监测终端和若干监测节点实现同时对多个节点的监测，克服了传统只对单个点姿态监测的缺陷。

附图说明

图1为根据本发明实施例所述的安装监测装置的机械臂；

图2为根据本发明实施例所述的监测装置系统结构图；

图3为根据本发明实施例所述的监测方法流程示意图；

图4为根据本发明实施例所述的机械臂的坐标系图；

图5为根据本发明实施例所述的地坐标和传感器坐标及其基于单位四元数的转换图；

图中，201中央处理单元、202加速度传感器、203地磁传感器、204陀螺仪、205Zigbee通信模块、206监测终端、100机械臂基座的第一监测节点、101机械臂关节处的第二监测节点、102机械臂关节处的第三监测节点、103机械臂关节处的第四监测节点、104机械臂关节处的第五监测节点、105机械臂关节处的第六监测节点、106机械臂关节处的第七监测节点。

具体实施方式：

下面结合附图对本发明进行详细说明：

如图1所示，本发明具体实施例包括中央处理单元201、加速度传感器202、地磁传感器203、陀螺仪204、Zigbee通信模块205和监测终端206，如附图2所示。上述各模块均集成在一小型电路板上，各个电路板固定在被测机械臂上，如附图1所示。在由小型电路板测得机械臂各关节姿态角后结合臂长通过建立坐标系，如附图4所示，经过计算得出姿态坐标。本申请的机械臂是美国craft公司的产品，型号是grips。

中央处理单元201选择TI公司的低功耗单片机MSP430F149，其连接简单外围电路构成最小系统，用于采集和处理加速度传感器202、地磁传感器203和陀螺仪204的数据，最终通过相关算法，转换成机械臂的姿态数据。加速度传感器202采用ADI公司的三轴加速度计ADXL345，具有体积小、重量轻、功耗低等优点，通过SPI接口和主控制器通信。地磁传感器203选择Honeywell公司的三轴地磁传感器HMC5843，具有如下优势：可以实现X、Y、Z三轴磁场同时测试，测量范围更广，体积更小，集成度更高。通过I²C接口和中央处理单元相连接。陀螺仪204选用三轴数字式传感器ITG3205，其通过I²C接口和中央处理单元104相连接。Zigbee通信模块204选择CC2530。具有低功耗，组网方便等特点。

系统工作过程如下：系统监测终端发送的控制指令后，读取三轴加速度传感器测量出的机械臂各关节的三维加速度值α_x、α_y、α_z，读取三轴地磁传感器测量出的机械臂各关节的三维地磁感应强度分量m_x、m_y、m_z，结合四元数算法进行归一化处理，同时将进行陀螺仪角速度偏差补偿后的四元数向量进行归一化处理，最后将所得四元数转换成欧拉角，各个部分的旋转角经过多次测量后取其均值，以确保测量的准确性。再结合臂长根据各关节坐标系通过三角函数相关计算得出机械臂最终姿态。最终通过Zigbee无线通信模块将各个节点的数据传输至监测终端进行数据的的实时显示与存储，中央处理单元接收监测装置所监测的监测节点的信息并进行数据的处理。在已知旋转角和臂长的情况下计算得出机械臂的位姿。

如附图3所示，姿态监测模块计算旋转角度包括以下步骤：

根据装置初始状态，测量得到三维加速度值分量α_x、α_y、α_z及三维地磁感应分量m_x、m_y、m_z，对分量进行校准，校准的目的是为了去除由外界环境和测量过程中造成的误差，即实验前先分别对多次测得的加速度值和地磁感应三个分量取平均值，以此平均值作为标定值校准当次测量值的准确性，消除误差对测量值的影响。然后计算加速度参考值acc和地磁感应强度参考值mag；其中： $\mathrm{acc}=\sqrt{a_x^2+a_y^2+a_z^2};\mathrm{mag}=\sqrt{m_x^2+m_y^2+m_z^2}.$

当加速度测量值和参考值acc的偏差小于设定阈值时，加速度的四元数表示通过归一化处理得到： $A_t^s=\frac{\mathrm{\α}_t^m{}_s}{\left|\right|\mathrm{\α}_t^m{}_s\left|\right|}=\left[\begin{array}{cccc}0& {\mathrm{\α}}_{x,t}& {\mathrm{\α}}_{y,t}& {\mathrm{\α}}_{z,t}\end{array}\right],$ 否则，由对地坐标系中的加速度通过四元数转换得到： $A_t^s=\hat{q}_{t-\mathrm{\Δt}}^{*}{}_e{}^s\⊗z_a^e\⊗\hat{q}_{t-\mathrm{\Δt}}^e{}_s;$

其中各式中参数含义如下：

如图5所示，加速度测量值其中^sa_t＝^sa_b,t+^sg_t，^sa_t是经过误差补偿的线性加速度，^sa_b,t是重力补偿后物体的线性加速度，b_a,t是由低频偏移引起的偏差，e_a,t为零均值白噪声，^sg_t的取值取决于传感器定位；^sA_t为加速度测量值的归一化表示即加速度的四元数表示，为值的幅值，α_x,t、α_y,t、α_z,t分别为时间t时的三个方向的加速度分量，为在测量时间内的单位四元数表示，为的共轭，^ez_a＝[0 0 0 1]。

当地磁感应强度测量值和参考值mag的偏差小于设定阈值时，地磁感应强度的四元数表示通过归一化处理得到： $M_t^s=\frac{m_t^m{}_s}{\left|\right|m_t^m{}_s\left|\right|}=\left[\begin{array}{cccc}0& m_{x,t}& m_{y,t}& m_{z,t}\end{array}\right],$ 否则，由对地坐标系中的地磁通过四元数转换得到： $z_{m,t}^e=\hat{q}_{t-\mathrm{\Δt}}^e{}_s\⊗M_{t-\mathrm{\Δt}}^s\⊗\hat{q}_{t-\mathrm{\Δt}}^{*}{}_s{}^s,M_t^s=\hat{q}_t^{*}{}_e{}^s\⊗z_{m,t}^e\⊗\hat{q}_t^e{}_s;$

其中，地磁感应强度测量值其中，^sm_t为地磁场的磁感应强度，d_m,t为磁场干扰，b_m,t为磁场偏差，e_m,t为噪声，m_x,t、m_y,t、m_z,t分别为时间t时的三个方向的地磁感应强度分量，^sM_t为时间t时的地磁感应强度测量值的归一化表示即地磁感应强度的四元数表示，^sM_t-Δt为测量时间内的地磁感应强度， 为在时间t时的单位四元数表示，为的共轭。

根据高斯牛顿法则，寻找 $\hat{q}_t^e{}_s=\left[\begin{array}{cccc}\widehat{q_o}& \widehat{q_x}& \widehat{q_y}& \widehat{q_z}\end{array}\right],$ 使 $f\left(\widehat{q_t^e{}_s}\right)=\frac{1}{2}\mathrm{\ϵ}{\left(\widehat{q_t^e{}_s}\right)}^T\mathrm{\ϵ}\left(\widehat{q_t^e{}_s}\right)$ 取最小值，以便于求取使陀螺仪角速度误差最小的其中为误差函数；

求解陀螺仪偏差漂移补偿四元数梯度向量，并对四元数梯度向量进行归一化处理： $\stackrel{\·}{q}_{\mathrm{\ϵ},t+\mathrm{\Δt}}^e{}_s=\frac{\mathrm{\Δ}\hat{q}}{\left|\right|\mathrm{\Δ}\hat{q}\left|\right|};$

其中为传感器坐标系相对于地坐标系的陀螺仪方向的四元数的变化率，为单位四元数的变化。

所述对读取的陀螺仪角速度进行偏差补偿，角度误差角度偏差补偿后的传感器坐标系下的角度向量

其中，^sω_e,t+Δt为陀螺仪轴中补偿后的角误差，表示互补滤波融合后被估计方向的四元数表示的共轭，被用作陀螺仪偏差漂移补偿的四元数表示变化率，^sω_b,t+Δt为角偏移，为抵消非零均值陀螺仪测量误差的收敛率，^sω_t+Δt为角速度。

计算由陀螺仪数据估算的四元数梯度向量：其中为陀螺仪测量的四元数变化率，为陀螺仪的被估计方向的单位四元数表示；

计算经过互补滤波器对陀螺仪偏差漂移进行的补偿，最终得到的角速度四元数向量 $\hat{q}_{f,t+\mathrm{\Δt}}^e{}_s=\hat{q}_{f,t}^e{}_s+\stackrel{\·}{q}_{f,t+\mathrm{\Δt}}^e{}_s\mathrm{\Δt},$ 其中 $\stackrel{\·}{q}_{f,t+\mathrm{\Δt}}^e{}_s=\stackrel{\·}{q}_{\mathrm{\ω},t+\mathrm{\Δt}}^e{}_s-\mathrm{\β}\stackrel{\·}{q}_{\mathrm{\ϵ},t+\mathrm{\Δt}}^e{}_s,$ 并进行归一化处理；

其中互补滤波器融合的被估计方向的四元数表示，是互补滤波融合后被估计方向的四元数变化率，为陀螺仪测量的四元数变化率，β为滤波器增益；

最后，将所得四元数转换成欧拉角。由于机械臂每个部分只能进行单一运动方式如只能是俯仰、旋转、摆动中的一种，所以求取的选转角只能是由相应的运动产生的横摆角、俯仰角、滚摆角中的一种角度。其中，ψ、θ、分别为绕Z轴、Y轴、X轴的旋转角度。若q＝[w x y z]^T，则旋转角度可由下式得出：

将各部分进行多次测量测得的多个旋转角取其均值，以确保测量的准确性。

在每个关节处最终的旋转角其中α为已求取的旋转角，n为测量的次数。

在参考相关算法得到欧拉角的情况下结合各部分的臂长，各部分建立相应的坐标系，利用三角函数相关计算得出机械臂各关节的姿态。具体算法如下:

选择相应坐标系通过旋转角和臂长在相应坐标系中的投影测得各部分的姿态后，通过各部分的叠加得到机械臂最终的姿态。

在机械臂基座的监测节点100处建坐标系x0y0z0，作为机械臂整体系统的基准，其中a₀、a₁、a₂、a₃、a₄、a₅分别为相邻两个关节之间的臂长；

在机械臂关节处的第一监测节点100处建坐标系x₀y₀z₀，作为基准，第一监测节点对应的位姿；

$\left\{\begin{array}{c}{x}_0=0\\ y_0=0\\ z_0=0\end{array}\right.$

在机械臂关节处的第二监测节点101处建坐标系x₁y₁z₁，第一关节旋转α₀，第二监测节点对应的位姿；

$\left\{\begin{array}{c}{x}_1=0\\ y_1=0\\ z_1=a_0\end{array}\right..$

在机械臂关节处的第三监测节点102处建坐标系x₂y₂z₂，第二关节旋转α₁，第三监测节点对应的位姿；

$\left\{\begin{array}{c}{x}_2=a_1\cos {\mathrm{\α}}_1\sin {\mathrm{\α}}_0+x_1\\ y_2=a_1\cos {\mathrm{\α}}_1\cos {\mathrm{\α}}_0+y_1\\ z_2=a_1\sin {\mathrm{\α}}_1+z_1\end{array}\right..$

在机械臂关节处的第四监测节点103处建坐标系x₃y₃z₃，第三关节旋转α₂，第四监测节点对应的位姿；

$\left\{\begin{array}{c}{x}_3=a_2\sin {\mathrm{\α}}_0\cos ({\mathrm{\α}}_1+{\mathrm{\α}}_2)+x_2\\ y_3=a_2\cos {\mathrm{\α}}_0\cos ({\mathrm{\α}}_1+{\mathrm{\α}}_2)+y_2\\ z_3=a_2\sin ({\mathrm{\α}}_1+{\mathrm{\α}}_2)+z_2\end{array}\right..$

在机械臂关节处的第五监测节点104处建坐标系x₄y₄z₄，第四关节旋转α₃，第五监测节点对应的位姿；

$\left\{\begin{array}{c}{x}_4=a_3\sin {\mathrm{\α}}_0\cos ({\mathrm{\α}}_1+{\mathrm{\α}}_2+{\mathrm{\α}}_3)+x_3\\ y_4=a_3\cos {\mathrm{\α}}_0\cos ({\mathrm{\α}}_1+{\mathrm{\α}}_2+{\mathrm{\α}}_3)+y_3\\ z_4=a_3\sin ({\mathrm{\α}}_1+{\mathrm{\α}}_2+{\mathrm{\α}}_3)+z_3\end{array}\right..$

在机械臂关节处的第六监测节点105处建坐标系x₅y₅z₅，第五关节旋转α₄，第六监测节点对应的位姿；

$\left\{\begin{array}{c}{x}_5=a_4\sin ({\mathrm{\α}}_0+{\mathrm{\α}}_4)\cos ({\mathrm{\α}}_1+{\mathrm{\α}}_2+{\mathrm{\α}}_3)+x_4\\ y_5=a_4\cos ({\mathrm{\α}}_0+{\mathrm{\α}}_4)\cos ({\mathrm{\α}}_1+{\mathrm{\α}}_2+{\mathrm{\α}}_3)+y_4\\ z_5=a_4\sin ({\mathrm{\α}}_1+{\mathrm{\α}}_2+{\mathrm{\α}}_3)+z_4\end{array}\right..$

在机械臂关节处的第七监测节点106处建坐标系x₆y₆z₆，第六关节旋转α₅，第七监测节点对应的位姿；

$\left\{\begin{array}{c}{x}_6=a_5\sin ({\mathrm{\α}}_0+{\mathrm{\α}}_4)\cos ({\mathrm{\α}}_1+{\mathrm{\α}}_2+{\mathrm{\α}}_3+{\mathrm{\α}}_5)+x_5\\ y_6=a_5\cos ({\mathrm{\α}}_0+{\mathrm{\α}}_4)\cos ({\mathrm{\α}}_1+{\mathrm{\α}}_2+{\mathrm{\α}}_3+{\mathrm{\α}}_5)+y_5\\ z_6=a_5\sin ({\mathrm{\α}}_1+{\mathrm{\α}}_2+{\mathrm{\α}}_3+{\mathrm{\α}}_5)+z_5\end{array}\right.$

将各个关节的姿态累加得到机械臂的最终姿态为

$\left\{\begin{array}{c}x=a_1\sin {\mathrm{\α}}_0\cos {\mathrm{\α}}_1+a_2\sin {\mathrm{\α}}_0\cos ({\mathrm{\α}}_1+{\mathrm{\α}}_2)+a_3\sin {\mathrm{\α}}_0\cos ({\mathrm{\α}}_1+{\mathrm{\α}}_2+{\mathrm{\α}}_3)+a_4\sin ({\mathrm{\α}}_0+{\mathrm{\α}}_4)\cos ({\mathrm{\α}}_1+{\mathrm{\α}}_2+{\mathrm{\α}}_3)\\ +a_5\sin ({\mathrm{\α}}_0+{\mathrm{\α}}_4)\cos ({\mathrm{\α}}_1+{\mathrm{\α}}_2+{\mathrm{\α}}_3+{\mathrm{\α}}_5)\\ y=a_1\cos {\mathrm{\α}}_0\cos {\mathrm{\α}}_1+a_2\cos {\mathrm{\α}}_0\cos ({\mathrm{\α}}_1+{\mathrm{\α}}_2)+a_3\cos {\mathrm{\α}}_0\cos ({\mathrm{\α}}_1+{\mathrm{\α}}_2+{\mathrm{\α}}_3)+a_4\cos ({\mathrm{\α}}_0+{\mathrm{\α}}_4)\cos ({\mathrm{\α}}_1+{\mathrm{\α}}_2+{\mathrm{\α}}_3)\\ +a_5\cos ({\mathrm{\α}}_0+{\mathrm{\α}}_4)\cos ({\mathrm{\α}}_1+{\mathrm{\α}}_2+{\mathrm{\α}}_3+{\mathrm{\α}}_5)\\ z=a_0+a_1\sin {\mathrm{\α}}_1+a_2\sin ({\mathrm{\α}}_1+{\mathrm{\α}}_2)+a_3\sin ({\mathrm{\α}}_1+{\mathrm{\α}}_2+{\mathrm{\α}}_3)+a_4\sin ({\mathrm{\α}}_1+{\mathrm{\α}}_2+{\mathrm{\α}}_3)+a_5\sin ({\mathrm{\α}}_1+{\mathrm{\α}}_2+{\mathrm{\α}}_3+{\mathrm{\α}}_5)\end{array}\right..$

Claims (10)

1.六自由度机械臂姿态监测系统，其特征是，包括布置在机械臂整体系统的若干个监测节点，每个监测节点处安装有监测装置，每个监测装置有监控终端通过Zigbee无线通信模块控制启停，Zigbee无线通信模块与中央处理单元通信，中央处理单元与监测装置通信，中央处理单元接收监测装置所监测的监测节点的信息。

2.如权利要求1所述的六自由度机械臂姿态监测系统，其特征是，所述监测装置包括用于测量机械臂关节的加速度值的三轴加速度传感器，用于测量机械臂关节的地磁感应强度分量的三轴地磁传感器及用于测量械臂关节的角速度值的三轴陀螺仪。

3.如权利要求1所述的六自由度机械臂姿态监测系统，其特征是，所述若干个监测节点包括机械臂基座的第一监测节点、机械臂关节处的第二监测节点、机械臂关节处的第三监测节点、机械臂关节处的第四监测节点、机械臂关节处的第五监测节点、机械臂关节处的第六监测节点及机械臂关节处的第七监测节点。

4.如权利要求1所述的六自由度机械臂姿态监测系统的监测方法，其特征是，包括以下步骤：

步骤一：中央处理单元读取三轴地磁传感器测量出的机械臂各关节的三维地磁感应强度分量值m_x、m_y、m_z、三轴加速度传感器测量出的机械臂各关节的三维加速度分量值α_x、α_y、α_z及三轴陀螺仪测量出的机械臂各关节的角速度；

步骤二：对读取的各关节的三维地磁感应强度分量、各关节的三维加速度分量值利用四元数算法进行归一化处理，对读取的陀螺仪角速度进行偏差补偿后的四元数向量进行归一化处理；

步骤三：将步骤二归一化处理后所得四元数转换成欧拉角即旋转角；将各部分进行多次测量测得的多个旋转角取其均值；

步骤四：根据步骤三中的旋转角、机械臂的臂长及各关节坐标系通过三角函数计算得出机械臂最终姿态。

5.如权利要求4所述的监测方法，其特征是，所述步骤一中还包括测量得到三维加速度值分量α_x、α_y、α_z及三维地磁感应分量m_x、m_y、m_z，对分量进行校准，校准的目的是为了去除由外界环境和测量过程中造成的误差，即实验前先分别对多次测得的加速度值和地磁感应三个分量取平均值，以此平均值作为标定值校准当次测量值的准确性；计算加速度参考值acc和地磁感应强度参考值mag；

其中： $\mathrm{acc}=\sqrt{a_x^2+a_y^2+a_z^2};\mathrm{mag}=\sqrt{m_x^2+m_y^2+m_z^2}.$

6.如权利要求4所述的监测方法，其特征是，所述步骤二中具体包括：

当加速度测量值和参考值acc的偏差小于设定阈值时，加速度的四元数表示通过归一化处理得到： $A_t^s=\frac{\mathrm{\α}_t^m{}_s}{\left|\right|\mathrm{\α}_t^m{}_s\left|\right|}=\left[\begin{array}{cccc}0& {\mathrm{\α}}_{x,t}& {\mathrm{\α}}_{y,t}& {\mathrm{\α}}_{z,t}\end{array}\right],$ 否则，由对地坐标系中的加速度通过四元数转换得到： $A_t^s=\hat{q}_{t-\mathrm{\Δt}}^{*}{}_e{}^s\⊗z_a^e\⊗\hat{q}_{t-\mathrm{\Δt}}^e{}_s;$

其中各式中参数含义如下：

加速度测量值其中^sa_t＝^sa_b,t+^sg_t，^sa_t是经过误差补偿的线性加速度，^sa_b,t是重力补偿后物体的线性加速度，b_a,t是由低频偏移引起的偏差，e_a,t为零均值白噪声，^sg_t的取值取决于传感器定位；^sA_t为加速度测量值的归一化表示即加速度的四元数表示，为值的幅值，α_x,t、α_y,t、α_z,t分别为时间t时的三个方向的加速度分量，为在测量时间内的单位四元数表示，为的共轭，^ez_a＝[0 0 0 1]。

7.如权利要求4所述的监测方法，其特征是，所述步骤二中还包括：

当地磁感应强度测量值和参考值mag的偏差小于设定阈值时，地磁感应强度的四元数表示通过归一化处理得到： $M_t^s=\frac{m_t^m{}_s}{\left|\right|m_t^m{}_s\left|\right|}=\left[\begin{array}{cccc}0& m_{x,t}& m_{y,t}& m_{z,t}\end{array}\right],$ 否则，由对地坐标系中的地磁通过四元数转换得到： $z_{m,t}^e=\hat{q}_{t-\mathrm{\Δt}}^e{}_s\⊗M_{t-\mathrm{\Δt}}^s\⊗\hat{q}_{t-\mathrm{\Δt}}^{*}{}_s{}^s,M_t^s=\hat{q}_t^{*}{}_e{}^s\⊗z_{m,t}^e\⊗\hat{q}_t^e{}_s;$

其中，地磁感应强度测量值其中，^sm_t为地磁场的磁感应强度，d_m,t为磁场干扰，b_m,t为磁场偏差，e_m,t为噪声，m_x,t、m_y,t、m_z,t分别为时间t时的三个方向的地磁感应强度分量，^sM_t为时间t时的地磁感应强度测量值的归一化表示即地磁感应强度的四元数表示，^sM_t-Δt为测量时间内的地磁感应强度， $z_{m,t}^e=\left[\begin{array}{cccc}0& \stackrel{\‾}{m_x}& \stackrel{\‾}{m_y}& \stackrel{\‾}{m_z}\end{array}\right],$ 为在时间t时的单位四元数表示，为的共轭；

根据高斯牛顿法则，寻找 $\hat{q}_t^e{}_s=\left[\begin{array}{cccc}\widehat{q_o}& \widehat{q_x}& \widehat{q_y}& \widehat{q_z}\end{array}\right],$ 使 $f\left(\widehat{q_t^e{}_s}\right)=\frac{1}{2}\mathrm{\ϵ}{\left(\widehat{q_t^e{}_s}\right)}^T\mathrm{\ϵ}\left(\widehat{q_t^e{}_s}\right)$ 取最小值，以便于求取使陀螺仪角速度误差最小的其中为误差函数；

求解陀螺仪偏差漂移补偿四元数梯度向量，并对四元数梯度向量进行归一化处理： $\stackrel{\·}{q}_{\mathrm{\ϵ},t+\mathrm{\Δt}}^e{}_s=\frac{\mathrm{\Δ}\hat{q}}{\left|\right|\mathrm{\Δ}\hat{q}\left|\right|};$

其中为传感器坐标系相对于地坐标系的陀螺仪方向的四元数的变化率，为单位四元数的变化。

8.如权利要求4所述的监测方法，其特征是，所述对读取的陀螺仪角速度进行偏差补偿，角度误差 $\mathrm{\ω}_{e,t+\mathrm{\Δt}}^s=2\hat{q}_{f,t+\mathrm{\Δt}}^{*}{}_e{}^s\⊗\stackrel{\·}{q}_{\mathrm{\ϵ},t+\mathrm{\Δt}}^e{}_s,$ 角度偏差补偿后的传感器坐标系下的角度向量

其中，^sω_e,t+Δt为陀螺仪轴中补偿后的角误差，表示互补滤波融合后被估计方向的四元数表示的共轭，被用作陀螺仪偏差漂移补偿的四元数表示变化率，^sω_b,t+Δt为角偏移，为抵消非零均值陀螺仪测量误差的收敛率，^sω_t+Δt为角速度；

计算由陀螺仪数据估算的四元数梯度向量：其中为陀螺仪测量的四元数变化率，为陀螺仪的被估计方向的单位四元数表示；

计算经过互补滤波器对陀螺仪偏差漂移进行的补偿，最终得到的角速度四元数向量 $\hat{q}_{f,t+\mathrm{\Δt}}^e{}_s=\hat{q}_{f,t}^e{}_s+\stackrel{\·}{q}_{f,t+\mathrm{\Δt}}^e{}_s\mathrm{\Δt},$ 其中 $\stackrel{\·}{q}_{f,t+\mathrm{\Δt}}^e{}_s=\stackrel{\·}{q}_{\mathrm{\ω},t+\mathrm{\Δt}}^e{}_s-\mathrm{\β}\stackrel{\·}{q}_{\mathrm{\ϵ},t+\mathrm{\Δt}}^e{}_s,$ 并进行归一化处理；

其中互补滤波器融合的被估计方向的四元数表示，是互补滤波融合后被估计方向的四元数变化率，为陀螺仪测量的四元数变化率，β为滤波器增益。

9.如权利要求4所述的监测方法，其特征是，所述步骤三中将步骤二所得四元数转换成欧拉角，由于机械臂每个部分只能进行单一运动方式如只能是俯仰、旋转、摆动中的一种，所以求取的旋转角只能是由相应的运动产生的横摆角、俯仰角、滚摆角中的一种角度；四元数与欧拉角的转换关系如下：

若四元数q＝[w x y z]^T，则旋转角度可由下式得出：

所述步骤三中，将各部分进行多次测量测得的多个旋转角取其均值，以确保测量的准确性；在每个关节处最终的旋转角其中α为已求取的旋转角，n为测量的次数。

10.如权利要求4所述的监测方法，其特征是，所述步骤四的具体步骤为：通过旋转角和臂长在相应坐标系中的投影测得各部分的姿态后，通过各部分的叠加得到机械臂最终的姿态。