机器人状态感知系统.pdf

一种机器人状态感知系统，其包括一感测装置、一数据接收装置、一数据处理器、一状态判断装置、一执行装置、一定位装置以及一误差校正装置。所述感测装置测量与机器人运动的加速度信号和角速度信号并经数据接收装置滤波和数字化处理后由所述数据处理器计算出与机器人运动的各种运动参量。所述状态判断装置根据机器人的运动参量判断机器人所处的运动状态并控制所述执行装置发出对应的警报声。所述定位装置沿机器人运动路径按均等间距分布于机器人的运动区域内并提供与其所处位置相关的校正信息。所述误差校正装置感测所述定位装置提供的校

1.  一种机器人状态感知系统，其包括：
感测装置，用于测量机器人运动的加速度信号和角速度信号；
数据接收装置，与所述感测装置连接，用于对所述感测装置测得的加速度信号和角速度信号进行滤波和数字化处理；
数据处理器，与所述数据接收装置连接，用于根据所述数据接收装置滤波和数字化处理后的加速度信号和角速度信号计算机器人运动的加速度、角速度和位移；
状态判断装置，与所述数据接收装置和数据处理器连接，用于根据所述数据接收装置滤波和数字化处理后的加速度信号和所述数据处理器计算出的加速度判断机器人所处的运动状态；
执行装置，与所述状态判断装置连接，用于根据机器人所处的状态发出相对应的警报声，
其特征在于，所述机器人状态感知系统进一步包括：
定位装置，其沿机器人运动路径按均等间距分布，所述定位装置上设置有数码标签，所述数码标签上的内容为与所述定位装置所处位置相关的校正信息；
误差校正装置，其与所述数据处理器相连接，用于读取所述数码标签上的校正信息并根据所述校正信息对所述数据处理器计算出来的机器人运动的位移进行校正。

2.  如权利要求1所述的机器人状态感知系统，其特征在于，所述数据接收装置包括一截止频率为10赫兹的高通滤波器、一截止频率为12.5赫兹的低通滤波器及一截止频率为50赫兹的低通滤波器，所述高通滤波器和低通滤波器对感测装置测得的加速度模拟信号和角速度模拟信号进行滤波处理并分别转换为加速度数字信号和角速度数字信号。

3.  如权利要求1所述的机器人状态感知系统，其特征在于，所述感测装置包括一加速度计和一陀螺仪，所述加速度计包括感测轴方向相互垂直的三个加速度感测器，所述陀螺仪包括感测轴方向与所述加速度感测器的感测轴方向相对应的三个角速度感测器，以所述加速度感测器和角速度感测器的感测轴方向为坐标轴定义一感测活动直角坐标系，将机器人初始状态时的感测活动直角坐标系定义为起始参考直角坐标系。

4.  如权利要求3所述的机器人状态感知系统，其特征在于，所述数据处理器根据经截止频率为50赫兹的低通滤波器处理后的角速度数字信号计算出所述感测活动直角坐标系与起始参考直角坐标系之间的坐标转换矩阵。

5.  如权利要求4所述的机器人状态感知系统，其特征在于，所述数据处理器根据经截止频率为12.5赫兹的低通滤波器处理后的加速度数字信号和所述感测活动直角坐标系与起始参考直角坐标系之间的坐标转换矩阵计算出机器人相对于起始参考直角坐标系的加速度。

6.  如权利要求5所述的机器人状态感知系统，其特征在于，所述状态判断装置根据机器人相对于起始参考直角坐标系的加速度与所述起始参考直角坐标系三个坐标轴之间的倾斜角以判断机器人是否处于倾斜状态。

7.  如权利要求6所述的机器人状态感知系统，其特征在于，如果所述状态判断装置判断机器人处于倾斜状态则向所述执行装置发出倾斜警报指令，所述执行装置接收到所述倾斜警报指令后发出倾斜警报声。

8.  如权利要求5所述的机器人状态感知系统，其特征在于，所述数据处理器通过对所述机器人相对于起始参考直角坐标系的加速度进行积分运算得到所述机器人相对于起始参考直角坐标系的速度。

9.  如权利要求8所述的机器人状态感知系统，其特征在于，所述数据处理器通过对所述机器人相对于起始参考直角坐标系的速度进行积分运算得到所述机器人相对于起始参考直角坐标系的位移。

10.  如权利要求2所述的机器人状态感知系统，其特征在于，所述状态判断装置检测经截止频率为10赫兹的高通滤波器处理后的加速度数字信号，若检测到持续时间超过一预设摇晃时间阈值且加速度值呈正负相间变化的加速度数字信号时，所述状态判断装置判断机器人处于被摇晃的状态。

11.  如权利要求10所述的机器人状态感知系统，其特征在于，如果所述状态判断装置判断机器人处于被摇晃的状态则向所述执行装置发出摇晃警报指令，所述执行装置在接收到摇晃警报指令后发出摇晃警报声。

12.  如权利要求1所述的机器人状态感知系统，其特征在于，所述定位装置包括一初始定位器和至少一个辅助定位器，所述初始定位器设置于机器人的运动起点，用于对机器人进行初始化设定，所述辅助定位器从初始定位器开始沿机器人运动路径按均等间距分布。

13.  如权利要求12所述的机器人状态感知系统，其特征在于，每一个所述辅助定位器相对于初始定位器的位移定义为校正位移，所述每一个辅助定位器上均设置有一与所述校正位移相对应的数码标签。

14.  如权利要求13所述的机器人状态感知系统，其特征在于，所述误差校正装置包括一读取器，在机器人每次经过所述辅助定位器时，所述读取器读取与数码标签相对应的校正位移信息，并根据所述校正位移校正由所述数据处理器计算出来的机器人运动的位移。

15.  如权利要求12所述的机器人状态感知系统，其特征在于，所述辅助定位器的个数取决于所述感测装置的精密度，若所述感测装置的精密度越高则在相同运动路径内所需要的辅助定位器的个数越少。

机器人状态感知系统
技术领域
本发明涉及一种机器人状态感知系统。
背景技术
随着机械制造技术和自动控制技术的飞速发展，各种各样的机器人已经进入人类生活和生产的相关领域，以代替或模仿人类执行各种任务。在控制机器人活动的各项技术中，最为关键的技术之一便是机器人的状态感知技术。
现有机器人的状态感知主要是通过全球定位系统(Global Positioning System，GPS)、红外线导航系统或惯性导航系统对机器人的相关运动参量进行测量和计算来获取机器人运动状态信息。但GPS系统在室内或有强电磁干扰的环境下会无法锁定目标从而出现信号中断的情况，且使用成本较高，而红外线导航系统对障碍物敏感，只能在较小范围内满足感测精度的要求。由加速度计和陀螺仪等运动感测器件所构成的惯性导航系统具有短时间内较高的精度且价格便宜但因其需要采用积分运算来推测被感测物的位置信息，所产生的误差会随时间增加而累积，无法长时间使用。
发明内容
有鉴于此，有必要提供一种可长时间准确感知机器人状态的机器人状态感知系统。
一种机器人状态感知系统，其包括：
感测装置，用于测量机器人运动的加速度信号和角速度信号。
数据接收装置，与所述感测装置连接，用于对所述感测装置测得的加速度信号和角速度信号进行滤波和数字化处理。
数据处理器，与所述数据接收装置连接，用于根据所述数据接收装置滤波和数字化处理后的加速度信号和角速度信号计算机器人运动的加速度、角速度和位移。
状态判断装置，与所述数据接收装置和数据处理器连接，用于根据所述数据接收装置滤波和数字化处理后的加速度信号和所述数据处理器计算出的加速度判断机器人所处的运动状态。
执行装置，与所述状态判断装置连接，用于根据机器人所处的运动状态发出相对应的警报声。
定位装置，其沿机器人运动路径按均等间距分布。所述定位装置上设置有数码标签，所述数码标签上的内容为与所述定位装置所处位置相关的校正信息。
误差校正装置，其与所述数据处理器相连接，用于读取所述数码标签上的校正信息并根据所述校正信息对所述数据处理器所计算出来的机器人运动的位移进行校正。
相对于现有技术，本发明所提供的机器人状态感知系统利用感测装置测量机器人运动过程中的各种运动参量，并通过计算和分析来判断机器人所处的运动状态，以简单的结构和较低的成本实现了机器人的状态感知。其次，在机器人运动路径上设置间距相等的定位装置以提供与所述定位装置所处位置相关的校正信息，并通过误差校正装置来感测所述定位装置上的校正信息从而对由数据处理器计算出来的机器人运动位移进行校正以克服机器人状态感知系统在长时间使用过程中所产生的累积误差。
附图说明
图1是本发明提供的机器人状态感知系统的硬件架构图。
图2是图1的机器人状态感知系统的感测装置结构示意图。
图3是感测活动直角坐标系XYZ与起始参考直角坐标系X_oY_oZ_o之间的转动关系示意图。
具体实施方式
如图1所示，本发明提供的机器人状态感知系统2包括一感测装置22、一与所述感测装置22相连接的数据接收装置24、一与所述数据接收装置24相连接的数据处理器25、一与所述数据接收装置24和所述数据处理器25相连接的状态判断装置26、一与所述状态判断装置26相连接的执行装置28、一沿机器人运动路径按均等间距分布的定位装置210以及一与所述数据处理器25相连接的误差校正装置212。
如图2所示，所述感测装置22包括一加速度计221和一陀螺仪222。所述加速度计221和陀螺仪222利用微机电技术(Micro Electro-Mechanical System，MEMS)固装在同一平台220上。所述加速度计221包括感测轴方向相互垂直的X轴加速度感测器221a、Y轴加速度感测器221b及Z轴加速度感测器221c。所述X轴、Y轴、Z轴加速度感测器221a、221b和221c分别用于测量机器人在X轴、Y轴和Z轴方向上的加速度。所述陀螺仪222包括感测轴方向分别与所述加速度感测器221a、221b和221c的感测轴方向相对应的X轴角速度感测器222a、Y轴角速度感测器222b及Z轴角速度感测器222c。所述X轴、Y轴、Z轴角速度感测器222a、222b和222c分别用于测量机器人在X轴、Y轴和Z轴方向上的角速度。以所述X轴、Y轴、Z轴加速度感测器221a、221b、和221c以及X轴、Y轴、Z轴角速度感测器222a、222b和222c的感测轴方向为坐标轴可定义一随机器人一起移动的感测活动直角坐标系XYZ，将机器人初始状态时的感测活动直角坐标系XYZ定义为一起始参考直角坐标系X_oY_oZ_o。所述加速度计221直接测量的是反映机器人相对于感测活动直角坐标系XYZ的加速度变化的加速度模拟信号。所述陀螺仪222直接测量的是反映机器人相对于感测活动直角坐标系XYZ的角速度变化的角速度模拟信号。
如图1所示，所述数据接收装置24包括一截止频率为10赫兹的高通滤波器241、一截止频率为12.5赫兹的低通滤波器242及一截止频率为50赫兹的低通滤波器243。
所述截止频率为10赫兹的高通滤波器241与所述加速度计221电连接，用于对由所述加速度计221测得的加速度模拟信号中的低频干扰成分进行过滤并将其转换成加速度数字信号后传输至所述状态判断装置26。
所述截止频率为12.5赫兹的低通滤波器242与所述加速度计221电连接，用于对由所述加速度计221测得的加速度模拟信号中的高频干扰成分进行过滤并将其转换成加速度数字信号后传输至所述数据处理器25。
所述截止频率为50赫兹的低通滤波器243与所述陀螺仪222电连接，用于对由所述陀螺仪222测得的角速度模拟信号中的干扰成分进行过滤并将其转换成角速度数字信号后传输至所述数据处理器25。
所述数据处理器25根据经低通滤波器242处理后的加速度数字信号计算出机器人相对于感测活动直角坐标系XYZ的加速度a。所述加速度a包括加速度值的大小a和加速度的方向，即加速度与感测活动直角坐标系XYZ三个坐标轴的夹角：θ、φ、γ。
所述数据处理器25根据经低通滤波器243处理后的角速度数字信号计算出机器人相对于感测活动直角坐标系XYZ的角速度ψ。将所述角速度ψ沿感测活动直角坐标系XYZ的X轴、Y轴、Z轴分解得到角速度ψ沿X轴、Y轴、Z轴的分量：α、β、ω。
因为感测装置22直接测得的是机器人相对于感测活动直角坐标系XYZ的运动参量，若要得到机器人相对于起始参考直角坐标系X_oY_oZ_o的运动参量必须先求得感测活动直角坐标系XYZ与起始参考直角坐标系X_oY_oZ_o之间的转换矩阵C。所述转换矩阵C可通过对角速度分量的积分和矩阵运算求得。
如图3所示，感测活动直角坐标系XYZ在初始状态时与起始参考直角坐标系X_oY_oZ_o相同，经过取样时间ΔT后转动为新的感测活动直角坐标系X″′Y″′Z″′。所述角速度沿三个坐标轴的分量：α′、β′、ω′对取样时间ΔT积分可得到在取样时间ΔT内三个坐标轴所转动的角度为α、β、ω。将所述感测活动直角坐标系XYZ的转动过程依次分解为绕X轴、Y轴、Z轴的三次转动。首先将感测活动直角坐标系XYZ绕Z轴依逆时针方向转动α角得到第一次分解转动后的感测活动直角坐标系X′Y′Z′，则坐标转换关系为：
x′y′z′=Cαxyz]]>
其中，
转换矩阵
接着将第一次分解转动后的感测活动直角坐标系X′Y′Z′绕X′轴依逆时针方向转动β角得到第二次分解转动后的感测活动直角坐标系X″Y″Z″，则坐标转换关系为：
x′′y′′z′′=Cβx′y′z′]]>
其中，
转换矩阵
最后将第二次分解转动后的感测活动直角坐标系X″Y″Z″绕Y″轴依逆时针方向转动ω角得到第三次分解转动后的感测活动直角坐标系X″′Y″′Z″′，则坐标转换关系为：
x′′′y′′′z′′′=Cωx′′y′′z′′]]>
其中，
转换矩阵
综上所述，起始参考直角坐标系X_oY_oZ_o与转动后的感测活动直角坐标系X″′Y″′Z″′之间的坐标转换关系为：
x′′′y′′′z′′′=Cxyz=CαCβCωxyz]]>
其中，转换矩阵：
C=CαCβCω=1000cosα-sinα0sinαcosαcosβ0sinβ010-sinβ0cosβ]]>
cosω-sinω0sinωcosω0001]]>
在经过n个取样时间ΔT后只需要在上一个转换矩阵C_n-1的基础上乘上最后一次转动所对应的转换矩阵C则可得到感测活动直角坐标系XYZ与起始参考直角坐标系X_oY_oZ_o之间的转换矩阵C_n。
所述数据处理器25将机器人相对于感测活动直角坐标系XYZ的加速度a通过直角坐标系转换后得到机器人相对于起始参考直角坐标系X_oY_oZ_o的加速度a_o。所述加速度a_o与起始参考直角坐标系X_oY_oZ_o三个坐标轴的夹角分别为θ_o、φ_o、γ_o，所述加速度值大小为a_o。将所述加速度a_o对取样时间ΔT积分可得到在取样时间ΔT内机器人相对于起台参考直角坐标系X_oY_oZ_o的运动速度V_o。将所述运动速度V_o对取样时间ΔT积分可得到在取样时间ΔT内机器人相对于起始参考直角坐标系X_oY_oZ_o的位移S。从初始状态开始累积计算每一个取样时间ΔT内机器人相对于起始参考直角坐标系X_oY_oZ_o的位移S便可得到机器人相对起始参考直角坐标系X_oY_oZ_o的总位移S_o，同时可以描绘出机器人的运动轨迹。
所述状态判断装置26内预设有用于判断机器人是否被摇晃的摇晃时间阈值H。所述状态判断装置26检测经高通滤波器241处理后的加速度数字信号。因高通滤波器241已将低频振动引起的干扰信号滤除，若状态判断装置26检测到信号持续时间超过所述摇晃时间阈值H，且加速度值呈正负相间交替变化的加速度数字信号时则判断机器人处于被摇晃状态并向所述执行装置28发出摇晃警报指令。
所述状态判断装置26内预设有用于判断机器人倾斜状态的倾斜角范围(θ₁，θ₂)、(φ₁，φ₂)、(γ₁，γ₂)。所述状态判断装置26接收由数据处理器25计算得到的机器人相对于起始参考直角坐标系X_oY_oZ_o的加速度a，并分别检测所述加速度a_o与起始参考直角坐标系X_oY_oZ_o的X_o轴、Y_o轴、Z_o轴的夹角：θ_o、φ_o、γ_o是否在所述倾斜角范围(θ₁，θ₂)、(φ₁，φ₂)、(γ₁，γ₂)内。如果所述加速度夹角θ_o、φ_o、γ_o超出所述倾斜角范围则判断机器人处于倾斜状态中并向所述执行装置28发出倾斜警报指令。
所述执行装置28包括一警报器281。若所述执行装置28接收状态判断装置26发出的摇晃控制指令则通过所述警报器281发出摇晃警报声。若所述执行装置28接收到来自状态判断装置26发出的倾斜警报指令则通过所述警报器281发出倾斜警报声。
因为所述加速度计221和陀螺仪222的测量误差随着感测时间的延长上升得很快，而所述数据处理器25是通过对所述加速度计221和陀螺仪222所感测到的加速度a_o进行积分来推算出机器人运动的速度值和位移值。所以所述陀螺仪222和加速度计221的测量误差会进入积分公式进行累积从而导致所计算出来的速度值和位移值的误差越来越大。因此，每当机器人运动一段距离后就必须通过设置在运动路径上的定位装置210来对机器人所感测到的相对于起始参考直角坐标系X_oY_oZ_o的总位移S_o进行校正。
如图1所示，所述定位装置210包括一初始定位器2101和至少一个辅助定位器2102。所述初始定位器2101设置在机器人的运动起点，即所述起始参考直角坐标系X_oY_oZ_o的原点处。在机器人每次重新开始运动之前，所述初始定位器2101对机器人进行初始化设定，清除数据处理器25内关于机器人前一次运动的运动参量。
机器人每次从运动起点出发之前都已设置好即将运行的运动路径。所述辅助定位器2102从初始定位器2101开始沿设置好的机器人运动路径按均等间距分布。因为所述每一个辅助定位器2102相对于所述初始定位器2101具有确定的位移S′，将所述位移S′定义为对应的辅助定位器2102的校正位移。所述每一个辅助定位器2102上均设置有一与所述校正位移S′相对应的数码标签2103。
所述误差校正装置212包括一读取器2120。在机器人每经过一辅助定位器2102时，所述读取器从辅助定位器2102的数码标签2103上读取对应的校正位移S′。所述误差校正装置212将所获取的校正位移S′与所述数据处理器25所计算出来的机器人相对于起始参考直角坐标系X_oY_oZ_o的总位移S_o相减得到位移校正差值ΔS。在经过下一个辅助定位器2102之前，所述数据处理器25根据所述位移校正差值ΔS对其计算出来的机器人相对于起始参考直角坐标系X_oY_oZ_o的总位移S_o进行校正。此后运动过程中，机器人将不断地到达新的辅助定位器2102以更新所述位移校正差值ΔS来对由所述数据处理器25计算出来的相对于起始参考直角坐标系X_oY_oZ_o的总位移S_o进行校正，从而确保所得到的总位移S_o的精确度。可以理解的是，需要设置的辅助定位器2102的个数由机器人运动路径的长短和所述感测装置22的精确度所决定。所述感测装置22的精密度越高则在相同的运动路径内所需要的辅助定位器的个数越少。
与现有技术相比，本发明所提供的机器人状态感知系统利用加速度计和陀螺仪测量机器人运动过程中的各种运动参量，并通过计算和分析来判断机器人所处的运动状态，以简单的结构和较低的成本实现了机器人的状态感知。其次，在机器人运动路径上设置间距相等的定位装置以提供与所述定位装置所处位置相关的校正信息，并通过误差校正装置来感测所述定位装置上的校正信息从而对由数据处理器计算出来的机器人运动位移进行校正以克服机器人状态感知系统在长时间使用过程中所产生的累积误差。
最后应说明的是，以上实施方式仅用以说明本发明的技术方案而非限制，尽管参照较佳实施方式对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或等同替换，而不脱离本发明技术方案的精神和范围。