示教机械臂姿态解算改进方法仿真研究

黄 洋，姜文刚

江苏科技大学 电子信息学院，江苏 镇江 212003

1 引言

基于惯性传感器的机械臂示教系统是通过采集固定于人体手臂各个位置的惯性传感器数据，经过姿态解算与空间位置计算得到机械臂姿态运动轨迹，其核心在于惯性传感器解算姿态角度的精确性。Rehbinder和Hu在其研究姿态解算算法的时候，采用了基于MEMS（微机电传感器）技术的陀螺仪和加速度计传感器组合的姿态解算算法，俯仰角和横滚角得到了一定的校正，但是航向角容易发生漂移。Bachmann在进行三维空间姿态测量的研究中加入了磁力计，由磁力计数据对航向角进行校正。由于姿态测量环境中的随机干扰对MEMS传感器测量精度的影响较大，同时受制于其本身的精度问题，导致经过误差标定的MEMS传感器在采集数据以及之后的姿态解算过程中不可避免的引起姿态角较大的偏差，最终导致姿态数据的漂移。

针对上述问题，参考捷联惯导相关算法[1-2]，设计了一种采用组合传感器测量[3]和组合滤波的姿态解算方法。使用六个惯性传感器单元对载体坐标系的三轴正负轴向分别测量加速度，磁场强度和角速度数据，采用四元数法对姿态角进行求取。将磁罗盘算法和四元数估计法得到的两组姿态角的误差分别作为模糊调节器和PI调节器的输入参数，使用互补滤波[4-5]分别修正两组角速度的输出数据，通过卡尔曼滤波器[6]减小误差，得到更精确的姿态角数据。该方法结合了模糊调节的快速性和PI调节的精确性，通过互补滤波来抑制陀螺仪角速度漂移，保证姿态解算系统的稳定性，在姿态解算过程中通过卡尔曼滤波能有效抑制噪声[7]，通过迭代估计提高姿态角解算的精度。该方法相较于普通互补滤波解算姿态的方法更适合于示教机器人的姿态解算系统。

2 人体姿态解算方法原理

人体姿态变化即人体坐标系相对于基坐标系的空间转动。假设人体运动为刚体运动，在惯性导航系统中用俯仰角θ，横滚角γ和航向角ψ作为姿态角的表示方法。取人体坐标系为b系，参考坐标系为n系。xnynzn为参考坐标系的东北地方向，初始xbybzb与参考坐标系三轴重合。经过旋转后参考坐标系与人体坐标系有如下关系[8]：

其中Cbn以欧拉角形式表示为：器之间存在的耦合等因素，常规使用单组MEMS传感器进行姿态解算的方法难以保证示教机械臂姿态角解算的精度和稳定性。而采用六组传感器可以有效减小微传感器之间的耦合所产生的量测误差，组合调节能够结合模糊调节的快速性和PI调节的精确性，经过互补滤波对正负两组三轴角速度数据同时调节来抑制陀螺仪漂移，得到更精确的角速度数据，EKF（拓展卡尔曼滤波）滤波器可以根据调节后的角速度数据进行姿态估计并抑制噪声，能够满足系统精确性和稳定性的要求，因此设计了一种采用模糊和PI组合调节，互补滤波和EKF组合滤波的多组传感器姿态解算方法。

3 算法结构及分析过程

3.1 算法结构

设计的算法总体结构如图1所示。载体坐标系中按坐标轴固定的六组传感器分别测得各个轴向上加速度，磁场强度和角速度数据，可归为正负两组数据。将传感器测量数据与四元数法估计数据的向量积分别作为模糊调节器和PI调节器的输入，分别与两组角速度数据进行互补滤波，然后通过EKF滤波进行四元数求解，进而转化为姿态角。

由此可以计算出欧拉角方式下姿态角数据，即：

图1 算法总体结构框图

式（3）即为示教机器人姿态解算的欧拉角方程。在示教机器人匀速运动的过程中，传统磁罗盘算法可以通过各传感器数据得出姿态角。载体坐标系b下加速度计测得加速度ab=[]T，磁力计测得磁场强度mb=[]T，考虑东向磁场强度近似为0，参考坐标系下加速度与磁场强度an=[0 0 g]T，mn=[0]T，根据坐标变换矩阵可以得到姿态角数据，即：

其中角速度调节部分是根据传感器量测误差，利用模糊调节和PI调节，根据互补滤波原理可以调节角速度原始数据，能够快速并精确抑制陀螺仪在运动过程中的漂移，减小角速度误差，提高系统稳定性。EKF滤波部分是根据调节后得到更精确的角速度数据来构建状态方程，传感器量测数据构建观测方程，利用EKF滤波器进行姿态估计并抑制噪声。在循环迭代的过程中，得到更精确的四元数值，能够有效地减小传感器量测误差对姿态解算精度的影响，提高系统精度。

3.2 传感器组合测量

由于低成本MEMS传感器本身的精度局限，陀螺仪的漂移，外界的随机干扰和同一传感器单元中微传感

设计的算法采用按坐标轴固定的六组传感器分别测得各个轴向的传感器数据，安装位置如图2所示。

图2 传感器安装位置示意图

由示意图所示，传感器1～3可以测得人体加速度a1=[]T，磁场m1=[]T，角速度ω1=[]T，传感器4～6可以测得符号相反的加速度，磁力和角速度数据。根据坐标变换原理可得：

Δax,Δay,Δaz为加速度计测量噪声。由式（7）、（8）可以求出俯仰角θ与横滚角γ。

与上述方法一致，考虑东向磁场近似为0，可以得到航向角ψ：

Δmx,Δmy,Δmz为磁力计测量噪声。根据组合方式解算出的欧拉角可以通过推导得到四元数[9-10]初值，即：

3.3 角速度调节

在实际应用过程中，由于加速度计和磁力计静态特性较好，在解算过程中不会产生累计误差，航时间的稳定性较好，但是短时间内精度较差。陀螺仪动态特性较好，短时间内精度较高，但是在积分过程中会产生累计误差，长时间工作会引起漂移，最终导致解算方法失效。因此，可以采用互补滤波的方法对传感器信息进行融合[11]。互补滤波公式变换可得：

转换到时域为：

由公式（14）可知互补滤波为通过磁罗盘算法和四元数估计法得到的姿态角差值作为输入量调节陀螺仪角速度输出[12]。由于对陀螺仪角速度的校正中误差的计算通常采用的梯度下降法比较复杂且计算量大，因此在低速小范围运动中采用向量积法代替来计算误差量[13-14]，即：

其中，eθγ,eψ为加速度计和磁力计的测量向量与四元数下的观测向量的叉乘，an,hn为参考坐标系下加速度和磁力单位向量，,为载体坐标系下传感器测量值单位向量。加速度计1～3得到，加速度计4～6得到，磁力计1～3得到，磁力计4～6得到，e1=+，e2=+，通过采用模糊调节器[15]对陀螺仪1～3输出的角速度进行调节，误差e1和其在采样周期内的积分为输入量，u1为输出量，则：

通过PI调节器对陀螺仪4～6输出的角速度进行调节，误差e2为输入量，u2为输出量，则：

由于陀螺仪1～3与陀螺仪4～6安装方向相反，则载体角速度可由均值得到：

3.4 EKF姿态解算

由四元数作为状态向量，根据四元数微分方程可以建立EKF的状态方程，即：

其中

其中ω(k)为根据式（20）得到的调节后的角速度数据，W(k -1)近似为白噪声矩阵，Γ(k -1)为噪声驱动矩阵。

以传感器1～3测量的加速度和磁力数据的单位向量为观测量，建立观测方程：

在小范围手臂示教运动情况下，可近似认为有害加速度趋近于0，磁场无变化，即：

则可以得到

4 仿真验证

为了验证所设计的姿态解算方法，MATLAB中设定六组传感器两两相差90°，固定于手臂同一位置，其中加速度计随机误差为1×10-4g，陀螺仪随机误差为0.1°/h，磁力计随机误差为50 nt，各传感器误差相互独立，量测过程中互不影响。人体手臂运动属于低速小范围运动，频率一般低于15 Hz，则当截止频率 fc为15 Hz时，K为0.8，采样周期ts为0.1 s，仿真时长30 s。在陀螺仪角速度校正部分，模糊调节器设定误差，误差积分和输出量的量化因子分别为3，1，1。论域分别为[- 6 6]，[-3 3]，[- 9 9]。模糊化过程采用三角形隶属度函数，模糊规则表根据经验设计。PI调节器设定P为0.8，I为0.02。卡尔曼滤波器中，根据文献[12]的方法取各协方差矩阵[16]为 Q=0.000 1I3×3，R=0.001I6×6，P0=0.001I4×4。分别采用文献[11]中PI调节方法和设计的组合调节方法对姿态角静态和动态情况下姿态角变化做仿真，得到姿态角误差曲线图，静态仿真图如图3～5所示。

静态情况下，调节前后的姿态角误差如图3～5所示。图3为使用单组传感器无调节姿态角误差，图4为加入PI调节后的姿态角误差，图5为使用多组传感器，并采用模糊PI组合调节的姿态角误差，误差参数见表1，range表示误差范围，std表示误差标准差。

图3 无调节静态姿态角误差

图4 PI调节静态姿态角误差

图5 模糊PI组合调节静态姿态角误差

表1 静态姿态角误差参数比较

由表1中可以看出，图4中采用文献[11]中PI调节的方法，姿态角误差的标准差从0.507°、0.643°、0.519°降低到0.136°、0.157°、0.206°。图5中PI调节与模糊调节进行组合后，姿态角标准差进一步变为0.096°、0.098°、0.142°，姿态角误差标准差有进一步减小。表明在静态情况下，设计的算法相比文献[11]中的算法能够减小姿态角误差和提高系统稳定性的效果。

在动态仿真的过程中，假设航向角由0°匀速变化为－90°，同时适当增大各传感器的随机误差，动态仿真图如图6所示。

图6 模糊PI组合调节航向角变化曲线

图6 为航向角变化的曲线，即从0°匀速变化为－90°。从图中可以看出，虽然航向角在变化过程中存在微小的误差波动，但是曲线整体变化近似于真实曲线。

动态情况下，调节前后的姿态角误差如图7～9所示。图7为使用单组传感器无调节姿态角误差，图8为加入PI调节后的姿态角误差，图9为使用多组传感器，并采用模糊PI组合调节的姿态角误差，误差参数见表2。

图7 无调节动态姿态角误差

图8 PI调节动态姿态角误差

图9 模糊PI组合调节动态姿态角误差

表2 动态姿态角误差参数比较

从表2中可以看出，图8中采用文献[11]中PI调节的方法，动态姿态角误差的标准差分别从0.792°、0.997°、1.135°降低为0.298°、0.275°、0.608°。图9中采用模糊PI组合调节方法后，姿态角误差的标准差进一步减小为0.193°、0.188°、0.308°，这表明在航向角动态变化过程中采用模糊PI组合调节的方法相比在单一PI调节能进一步减小姿态角误差，并且提高系统稳定性，尤其对变化的航向角有更好的效果。

组合了模糊调节和PI调节，融合了互补滤波和卡尔曼滤波的多组传感器姿态解算方法对静态和动态情况下姿态角的误差消除都有比较好的效果，尤其是在航向角变化的情况下相比使用单组传感器PI调节的姿态角的精度和稳定性有进一步的提高，使得航向角变化接近于真值。因此，设计的算法在示教机械臂姿态解算中能有效提高姿态解算的精度，提高系统的稳定性。

5 结束语

在传统加速度计，磁力计和陀螺仪信息融合算法的基础上，将传感器组增加到六个，使用六轴向安装的MEMS传感器分别测得各自轴向的传感器数据。以该方法得到的传感器量测向量与四元数法得到的估计向量的两个向量积分别作为模糊调节器和PI调节器的输入量，输出量经过互补滤波分别调节两组角速度向量，取均值后输入卡尔曼滤波器中，得到更精确的四元数估计值，再经过变换得到姿态角。仿真结果表明，设计的算法在静态和动态情况下均能有效提高姿态角解算精度，并且对变化的航向角精度和稳定性提高有比较好的效果。