CN105058388B - 用于获取机器人关节位置反馈信息的传感器数据融合方法 - Google Patents

Abstract

用于获取机器人关节位置反馈信息的传感器数据融合方法，属于机械臂关节位置控制技术领域。本发明是为了解决轻型机械臂关节驱动控制技术中，由于关节端位置传感器检测精度低而无法获得精确的关节角度信息反馈，从而影响关节位置控制精度的问题。它包括三个步骤：步骤一：获得传感器数据标定表格；步骤二：获得关节初始绝对位置；步骤三：获得机器人关节位置实时反馈信息；将关节绝对位置角度θ作为反馈信息传递给关节控制器。本发明用于获取机器人关节位置反馈信息。

Description

用于获取机器人关节位置反馈信息的传感器数据融合方法

技术领域

本发明涉及用于获取机器人关节位置反馈信息的传感器数据融合方法，属于机械臂关节位置控制技术领域。

背景技术

轻型机械臂关节一般采用直流无刷电机驱动，电机轴与关节轴通过减速装置连接。机械臂关节中需要集成位置检测元件以实现关节位置控制，同时直流无刷电机也需要集成位置检测元件，检测转子磁极位置以实现电机的换相控制。如果能够将关节端与电机端的位置传感器信息通过适当的算法融合，将极大的提高关节位置信息检测的精度。因此，如何利用关节端位置传感器和电机端位置传感器反馈，获得高精度的关节位置信息，从而实现精确的位置控制，是当前亟待解决的问题。

发明内容

本发明目的是为了解决轻型机械臂关节驱动控制技术中，由于关节端位置传感器检测精度低而无法获得精确的关节角度信息反馈，从而影响关节位置控制精度的问题，提供了一种用于获取机器人关节位置反馈信息的传感器数据融合方法。

本发明所述用于获取机器人关节位置反馈信息的传感器数据融合方法，它包括以下步骤：

步骤一：获得传感器数据标定表格：

记录关节从一侧机械限位运动到另一侧机械限位过程中，在电机位置传感器换圈时刻对应的关节位置传感器输出值p_j，j＝0,1,2...l-1；将关节位置传感器输出值p_j整理成传感器数据标定表格Table[l]，存入关节控制器；l为关节位置传感器输出值p_j的个数；

步骤二：获得关节初始绝对位置：

启动关节控制器，在关节运动初始时刻，根据关节位置传感器采集获得的关节角度θ_j、电机位置传感器采集获得的电机轴角度θ_m及步骤一中获得的传感器数据标定表格Table[l]，计算得到相对于预置点p₀的关节初始绝对位置角度θ_init；

步骤三：获得机器人关节位置实时反馈信息：

在关节控制器的每一个控制周期内，实时读取电机轴角度θ_m，基于关节初始绝对位置角度θ_init，采用累加的方式获得实时的关节绝对位置角度θ；所述关节绝对位置角度θ作为反馈信息传递给关节控制器。

关节初始绝对位置角度θ_init的获得方法分为如下两种情况：

一)关节位置传感器与电机位置传感器数值增大方向相同：

若θ_j≤Table[0]，则θ_init＝θ_m/η-△θ；

若θ_j≥Table[l-1]，θ_init＝△θ*(l-1)+θ_m/η；

否则，采用二分法查询传感器数据标定表格Table[l]，获得θ_j对应的N，则：

θ_init＝△θ*N+θ_m/η；

式中η为关节减速装置的减速比；

△θ为电机转动一圈过程中对应的关节角度变化量，△θ＝2π/η；

N为相对于预置点p₀，电机轴转过的完整圈数；

二)关节位置传感器与电机位置传感器数值增大方向相反：

若θ_j≤Table[0]，则θ_init＝-θ_m/η；

若θ_j≥Table[l-1]，θ_init＝△θ*(l-1)+△θ-θ_m/η

否则，采用二分法查询传感器数据标定表格Table[l]，获得θ_j对应的N，则：

θ_init＝△θ*N+△θ-θ_m/η。

当关节初始绝对位置角度θ_init满足如下条件时，对其补偿如下：

若θ_init-θ_j>△θ/2，使θ_init＝θ_init-△θ；

若θ_init-θ_j<-△θ/2，使θ_init＝θ_init+△θ。

实时的关节绝对位置角度θ的获得方法分为如下两种情况：

一)关节位置传感器与电机位置传感器数值增大方向相同：

若|θ_m-θ_{m_last}|<π，则θ＝θ+(θ_m-θ_{m_last})/η，

若θ_m-θ_{m_last}≥π，则θ＝θ+(θ_m-θ_{m_last}-2π)/η，

若θ_m-θ_{m_last}≤-π，则θ＝θ+(θ_m-θ_{m_last}+2π)/η；

二)关节位置传感器与电机位置传感器数值增大方向相反：

若|θ_m-θ_{m_last}|<π，则θ＝θ-(θ_m-θ_{m_last})/η；

若θ_m-θ_{m_last}≥π，则θ＝θ-(θ_m-θ_{m_last}-2π)/η；

若θ_m-θ_{m_last}≤-π，则θ＝θ-(θ_m-θ_{m_last}+2π)/η；

式中θ_{m_last}为相邻上一个控制周期的电机位置传感器读数；

最后，在当前控制周期结束时，更新θ_{m_last}，使θ_{m_last}＝θ_m。

本发明的优点：本发明方法不需搭建专门的标定平台，关节安装完成后，在整个机械范围内运转一圈，记录数据，即可获得传感器数据标定表格；本发明可以采用低精度的关节位置传感器，如电位计，获得高精度的关节位置信息反馈，从而有效提高机器人关节位置感知精度，解决了关节位置传感器精度不高引起的机械臂运动控制精度差的问题，能够为精确的关节位置闭环控制奠定基础；同时，本发明方法具有算法简单，计算量小，易于实现的优点。

附图说明

图1是关节位置传感器与电机位置传感器数值增大方向相同时，本发明的融合方法示意图；图中A为电机位置传感器输出数据，B为关节位置传感器输出数据；

图2是关节位置传感器与电机位置传感器数值增大方向相反时，本发明的融合方法示意图。

具体实施方式

具体实施方式一：下面结合图1和图2说明本实施方式，本实施方式所述用于获取机器人关节位置反馈信息的传感器数据融合方法，它包括以下步骤：

步骤一：获得传感器数据标定表格：

记录关节从一侧机械限位运动到另一侧机械限位过程中，在电机位置传感器换圈时刻对应的关节位置传感器输出值p_j，j＝0,1,2...l-1；将关节位置传感器输出值p_j整理成传感器数据标定表格Table[l]，存入关节控制器；l为关节位置传感器输出值p_j的个数；

步骤二：获得关节初始绝对位置：

启动关节控制器，在关节运动初始时刻，根据关节位置传感器采集获得的关节角度θ_j、电机位置传感器采集获得的电机轴角度θ_m及步骤一中获得的传感器数据标定表格Table[l]，计算得到相对于预置点p₀的关节初始绝对位置角度θ_init；

步骤三：获得机器人关节位置实时反馈信息：

在关节控制器的每一个控制周期内，实时读取电机轴角度θ_m，基于关节初始绝对位置角度θ_init，采用累加的方式获得实时的关节绝对位置角度θ；所述关节绝对位置角度θ作为反馈信息传递给关节控制器。

所述电机位置传感器换圈时刻是指传感器读数从最小值变化到最大值或从最大值变化到最小值所对应的时刻。其中l等于标定表格的长度，因为关节的运动范围小于360°，所以标定表格的长度l小于关节减速装置的减速比。例如采用减速比η＝100的谐波减速器，则l<100。

具体实施方式二：下面结合图1和图2说明本实施方式，本实施方式对实施方式一作进一步说明，关节初始绝对位置角度θ_init的获得方法分为如下两种情况：

一)关节位置传感器与电机位置传感器数值增大方向相同：

若θ_j≤Table[0]，则θ_init＝θ_m/η-△θ；

若θ_j≥Table[l-1]，θ_init＝△θ*(l-1)+θ_m/η；

否则，采用二分法查询传感器数据标定表格Table[l]，获得θ_j对应的N，则：

θ_init＝△θ*N+θ_m/η；

式中η为关节减速装置的减速比；

△θ为电机转动一圈过程中对应的关节角度变化量，△θ＝2π/η；

N为相对于预置点p₀，电机轴转过的完整圈数；

二)关节位置传感器与电机位置传感器数值增大方向相反：

若θ_j≤Table[0]，则θ_init＝-θ_m/η；

若θ_j≥Table[l-1]，θ_init＝△θ*(l-1)+△θ-θ_m/η

否则，采用二分法查询传感器数据标定表格Table[l]，获得θ_j对应的N，则：θ_init＝△θ*N+△θ-θ_m/η。

本实施方式中，采用二分法查询在步骤一中得到的标定表格，可以迅速确定N的数值。关节角度θ_j和电机轴角度θ_m的测量可以在关节运动范围内的任意位置。

上述过程中，将关节0度定义在电机轴位置传感器第一次换圈处，实际应用中，根据实际要求加上一个固定的角度偏差即可。

具体实施方式三：下面结合图1和图2说明本实施方式，本实施方式对实施方式二作进一步说明，当关节初始绝对位置角度θ_init满足如下条件时，对其补偿如下：

若θ_init-θ_j>△θ/2，使θ_init＝θ_init-△θ；

若θ_init-θ_j<-△θ/2，使θ_init＝θ_init+△θ。

本实施方式用于补偿由于关节端和电机端位置传感器读数微小变化引入的误差。对关节初始绝对位置角度θ_init进行补偿即相当对其进行重新赋值，以实时更新当前值。

由于关节位置传感器和电机位置传感器都存在误差，当关节位置靠近标定表格记录的标定点位置时，关节端传感器和电机端传感器读数的微小变化都会使得融合后的关节角度增加或减少△θ，当关节位置传感器的测量精度高于△θ/2时，可以引入补偿措施。

具体实施方式四：下面结合图1和图2说明本实施方式，本实施方式对实施方式三作进一步说明，实时的关节绝对位置角度θ的获得方法分为如下两种情况：

一)关节位置传感器与电机位置传感器数值增大方向相同：

若|θ_m-θ_{m_last}|<π，则θ＝θ+(θ_m-θ_{m_last})/η，

若θ_m-θ_{m_last}≥π，则θ＝θ+(θ_m-θ_{m_last}-2π)/η，

若θ_m-θ_{m_last}≤-π，则θ＝θ+(θ_m-θ_{m_last}+2π)/η；

二)关节位置传感器与电机位置传感器数值增大方向相反：

若|θ_m-θ_{m_last}|<π，则θ＝θ-(θ_m-θ_{m_last})/η；

若θ_m-θ_{m_last}≥π，则θ＝θ-(θ_m-θ_{m_last}-2π)/η；

若θ_m-θ_{m_last}≤-π，则θ＝θ-(θ_m-θ_{m_last}+2π)/η；

式中θ_{m_last}为相邻上一个控制周期的电机位置传感器读数；

最后，在当前控制周期结束时，更新θ_{m_last}，使θ_{m_last}＝θ_m。

本实施方式采用电机位置信息变化累加的方式获得关节绝对位置信息。在关节控制器上电的初始时刻，采用步骤二的方法计算一次关节初始位置信息，在每一个控制周期实时读取电机位置传感器测量的电机轴角度θ_m，通过位置信息变化量的累加，计算关节角度θ。

实际上，在关节运动控制过程中，每一时刻的关节位置信息都可以通过步骤二的公式计算获得，但是这种方法需要在每一个控制周期查询步骤一中得到的表格，比较耗时，因此本发明采用电机位置信息变化累加的方式获得关节绝对位置信息。采用这种方法，只需要在关节控制器上电的初始时刻，通过步骤二的公式计算一次关节初始位置信息即可。获得关节初始位置信息后，实时读取电机位置传感器测量的电机轴角度θ_m，通过位置信息变化量的累加，计算关节角度θ。由于只查询一次标定表格，极大的缩短了运算时间。

在机械臂运动控制过程中，需要实时反馈各个关节的位置信息。轻型机械臂常用的位置检测元件有电位计、光电编码器、磁式编码器、旋转变压器等。其中，电位计具有质量轻、体积小、成本低、易于***集成的优点，如果不考虑其测量精度，是最适合应用在机械臂关节端作为位置检测元件的。但对比其他位置传感器，电位计的线性度差，检测精度低，很难满足高精度位置闭环控制的需要，极大地限制了其在关节位置检测中的应用。本发明方法利用关节端测量的关节轴位置信息进行粗定位，用电机端测量的电机轴位置信息进行精确定位，在采用低精度的关节端位置传感器的情况下获得精确的关节绝对位置反馈。

Claims (3)

1.一种用于获取机器人关节位置反馈信息的传感器数据融合方法，它包括以下步骤：

步骤一：获得传感器数据标定表格：

记录关节从一侧机械限位运动到另一侧机械限位过程中，在电机位置传感器换圈时刻对应的关节位置传感器输出值p_j，j＝0,1,2...l-1；将关节位置传感器输出值p_j整理成传感器数据标定表格Table[l]，存入关节控制器；l为关节位置传感器输出值p_j的个数；

步骤二：获得关节初始绝对位置：

启动关节控制器，在关节运动初始时刻，根据关节位置传感器采集获得的关节角度θ_j、电机位置传感器采集获得的电机轴角度θ_m及步骤一中获得的传感器数据标定表格Table[l]，计算得到相对于预置点p₀的关节初始绝对位置角度θ_init；

步骤三：获得机器人关节位置实时反馈信息：

在关节控制器的每一个控制周期内，实时读取电机轴角度θ_m，基于关节初始绝对位置角度θ_init，采用累加的方式获得实时的关节绝对位置角度θ；所述关节绝对位置角度θ作为反馈信息传递给关节控制器；

其特征在于，关节初始绝对位置角度θ_init的获得方法分为如下两种情况：

一)关节位置传感器与电机位置传感器数值增大方向相同：

若θ_j≤Table[0]，则θ_init＝θ_m/η-△θ；

若θ_j≥Table[l-1]，θ_init＝△θ*(l-1)+θ_m/η；

否则，采用二分法查询传感器数据标定表格Table[l]，获得θ_j对应的N，则：

θ_init＝△θ*N+θ_m/η；

式中η为关节减速装置的减速比；

△θ为电机转动一圈过程中对应的关节角度变化量，△θ＝2π/η；

N为相对于预置点p₀，电机轴转过的完整圈数；

二)关节位置传感器与电机位置传感器数值增大方向相反：

若θ_j≤Table[0]，则θ_init＝-θ_m/η；

若θ_j≥Table[l-1]，θ_init＝△θ*(l-1)+△θ-θ_m/η

否则，采用二分法查询传感器数据标定表格Table[l]，获得θ_j对应的N，则：

θ_init＝△θ*N+△θ-θ_m/η。

2.根据权利要求1所述的用于获取机器人关节位置反馈信息的传感器数据融合方法，其特征在于，当关节初始绝对位置角度θ_init满足如下条件时，对其补偿如下：

若θ_init-θ_j>△θ/2，使θ_init＝θ_init-△θ；

若θ_init-θ_j<-△θ/2，使θ_init＝θ_init+△θ。

3.根据权利要求2所述的用于获取机器人关节位置反馈信息的传感器数据融合方法，其特征在于，实时的关节绝对位置角度θ的获得方法分为如下两种情况：

一)关节位置传感器与电机位置传感器数值增大方向相同：

若|θ_m-θ_{m_last}|<π，则θ＝θ+(θ_m-θ_{m_last})/η，

若θ_m-θ_{m_last}≥π，则θ＝θ+(θ_m-θ_{m_last}-2π)/η，

若θ_m-θ_{m_last}≤-π，则θ＝θ+(θ_m-θ_{m_last}+2π)/η；

二)关节位置传感器与电机位置传感器数值增大方向相反：

若|θ_m-θ_{m_last}|<π，则θ＝θ-(θ_m-θ_{m_last})/η；

若θ_m-θ_{m_last}≥π，则θ＝θ-(θ_m-θ_{m_last}-2π)/η；

若θ_m-θ_{m_last}≤-π，则θ＝θ-(θ_m-θ_{m_last}+2π)/η；

式中θ_{m_last}为相邻上一个控制周期的电机位置传感器读数；

最后，在当前控制周期结束时，更新θ_{m_last}，使θ_{m_last}＝θ_m。