专利名称：机器人状态感知系统的制作方法
技术领域：
本发明涉及一种机器人状态感知系统。
背景技术：
随着机械制造技术和自动控制技术的飞速发展，各种各样的机器人已经进入人类生活和 生产的相关领域，以代替或模仿人类执行各种任务。在控制机器人活动的各项技术中，最为 关键的技术之一便是机器人的状态感知技术。
现有机器人的状态感知主要是通过全球定位系统(Global Positioning System, GPS) 、红外线导航系统或惯性导航系统对机器人的相关运动参量进行测量和计算来获取机器人运 动状态信息。但GPS系统在室内或有强电磁干扰的环境下会无法锁定目标从而出现信号中断 的情况，且使用成本较高，而红外线导航系统对障碍物敏感，只能在较小范围内满足感测精 度的要求。由加速度计和陀螺仪等运动感测器件所构成的惯性导航系统具有短时间内较高的 精度且价格便宜但因其需要采用积分运算来推测被感测物的位置信息，所产生的误差会随时 间增加而累积，无法长时间使用。
发明内容
有鉴于此，有必要提供一种可长时间准确感知机器人状态的机器人状态感知系统。 一种机器人状态感知系统，其包括
感测装置，用于测量机器人运动的加速度信号和角速度信号。
数据接收装置，与所述感测装置连接，用于对所述感测装置测得的加速度信号和角速度 信号进行滤波和数字化处理。
数据处理器，与所述数据接收装置连接，用于根据所述数据接收装置滤波和数字化处理 后的加速度信号和角速度信号计算机器人运动的加速度、角速度和位移。
状态判断装置，与所述数据接收装置和数据处理器连接，用于根据所述数据接收装置滤 波和数字化处理后的加速度信号和所述数据处理器计算出的加速度判断机器人所处的运动状 态。
执行装置，与所述状态判断装置连接，用于根据机器人所处的运动状态发出相对应的警 报声。
定位装置，其沿机器人运动路径按均等间距分布。所述定位装置上设置有数码标签，所述数码标签上的内容为与所述定位装置所处位置相关的校正信息。
误差校正装置，其与所述数据处理器相连接，用于读取所述数码标签上的校正信息并根 据所述校正信息对所述数据处理器所计算出来的机器人运动的位移进行校正。
相对于现有技术，本发明所提供的机器人状态感知系统利用感测装置测量机器人运动过 程中的各种运动参量，并通过计算和分析来判断机器人所处的运动状态，以简单的结构和较 低的成本实现了机器人的状态感知。其次，在机器人运动路径上设置间距相等的定位装置以 提供与所述定位装置所处位置相关的校正信息，并通过误差校正装置来感测所述定位装置上 的校正信息从而对由数据处理器计算出来的机器人运动位移进行校正以克服机器人状态感知 系统在长时间使用过程中所产生的累积误差。
图l是本发明提供的机器人状态感知系统的硬件架构图。 图2是图1的机器人状态感知系统的感测装置结构示意图。
图3是感测活动直角坐标系与起始参考直角坐标系^^KZ。之间的转动关系示意图。
具体实施例方式
如图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包括感测轴方向分别与所述加 速度感测器221 a、 22lb和221 c的感测轴方向相对应的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的感测轴方向为坐标轴可
定义一随机器人一起移动的感测活动直角坐标系，将机器人初始状态时的感测活动直
6角坐标系^^Z定义为一起始参考直角坐标系^^KZ。。所述加速度计221直接测量的是反映机器人相对于感测活动直角坐标系的加速度变化的加速度模拟信号。所述陀螺仪222直接测量的是反映机器人相对于感测活动直角坐标系的角速度变化的角速度模拟信号。
如图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处理后的加速度数字信号计算出机器人相对于
感测活动直角坐标系的加速度fl 。所述加速度fl包括加速度值的大小"和加速度的
方向，即加速度与感测活动直角坐标系"^2三个坐标轴的夹角^、 ^、 ^。
所述数据处理器25根据经低通滤波器243处理后的角速度数字信号计算出机器人相对于
感测活动直角坐标系"^2的角速度^ 。将所述角速度^沿感测活动直角坐标系"^^的X轴、Y轴、Z轴分解得到角速度^沿X轴、Y轴、Z轴的分量^ 、 ^、 ^。
因为感测装置22直接测得的是机器人相对于感测活动直角坐标系的运动参量，若
要得到机器人相对于起始参考直角坐标系a的运动参量必须先求得感测活动直角坐标系与起始参考直角坐标系^^KZ之间的转换矩阵c。所述转换矩阵c可通过对角速度分量的积分和矩阵运算求得。
如图3所示，感测活动直角坐标系"^72在初始状态时与起始参考直角坐标系
^^KZ。相同，经过取样时间AT后转动为新的感测活动直角坐标系T，^Z"。所述角速度沿三个坐标轴的分量"'、"、^'对取样时间AT积分可得到在取样时间Ar内三个坐标轴所转动的角度为"、^、 w。将所述感测活动直角坐标系皿的转动过程依次分解为绕x轴、Y轴、z轴的三次转动。首先将感测活动直角坐标系^^Z绕z轴依逆时针方向转动"角得到第一次分解转动后的感测活动直角坐标系z'^z'，则坐标转换关系为
其中，<formula>formula see original document page 8</formula>
转换矩阵
接着将第一次分解转动后的感测活动直角坐标系Z'^^^'绕T'轴依逆时针方向转动
^角得到第二次分解转动后的感测活动直角坐标系z'r^"，则坐标转换关系为
<formula>formula see original document page 8</formula>
其中，
转换矩阵- = L
最后将第二次分解转动后的感测活动直角坐标系TT'Z"绕y"轴依逆时针方向转动
W角得到第三次分解转动后的感测活动直角坐标系Z^^^Z"7 ，则坐标转换关系为
<formula>formula see original document page 8</formula>
其中，
转换矩阵综上月<formula>formula see original document page 8</formula>
综上所述，起始参考直角坐标系 与转动后的感测活动直角坐标系
之间的坐标转换关系为=c
zrz
其中，转换矩阵
cosw — sinw 000 1
1 0 00 cos a —sin a0 sin acos a
cos/0 sin/
0 1 0—sin/0 cos/
在经过n个取样时间后只需要在上一个转换矩阵 1的基础上乘上最后一次转动
所对应的转换矩阵C则可得到感测活动直角坐标系与起始参考直角坐标系
J^kz。之间的转换矩阵
所述数据处理器25将机器人相对于感测活动直角坐标系的加速度fl通过直角坐
标系转换后得到机器人相对于起始参考直角坐标系A 的加速度。所述加速度
"。 x丄z。
与起始参考直角坐标系 三个坐标轴的夹角分别为
j。 ^ 「
值大小为"° 。将所述加速度"°对取样时间AJT
对于起始参考直角坐标系 的运动速度°。将所述运动速度。对取样时间AJ
分可得到在取样时间AJT内机器人相对于起始参考直角坐标系^"'kz。的位移S
，所述加速度
积分可得到在取样时间内机器人相
A、
从初
尔系
始状态开始累积计算每一个取样时间AJT内机器人相对于起始参考直角坐标系^^kz。的
位移°便可得到机器人相对起始参考直角坐标系 的总位移°，同时可以描绘出机器人的运动轨迹。
所述状态判断装置26内预设有用于判断机器人是否被摇晃的摇晃时间阈值H。所述状态判断装置26检测经高通滤波器241处理后的加速度数字信号。因高通滤波器241已将低频振动引起的干扰信号滤除，若状态判断装置26检测到信号持续时间超过所述摇晃时间阈值H，且加速度值呈正负相间交替变化的加速度数字信号时则判断机器人处于被摇晃状态并向所述执行装置28发出摇晃警报指令。
9/9 /9
所述状态判断装置26内预设有用于判断机器人倾斜状态的倾斜角范围("2)、(A， ^2)、 ("1， "2 )。所述状态判断装置26接收由数据处理器25计算得到的机器人
相对于起始参考直角坐标系 的加速度"，并分别检测所述加速度与起始参考
直角坐标系 的 轴、轴、 轴的夹角°、 ^、 ^是否在所述倾斜角范
围(S， S)、(A，-2)、("1， 内。如果所述加速度夹角汉、^、 "°超
出所述倾斜角范围则判断机器人处于倾斜状态中并向所述执行装置28发出倾斜警报指令。
所述执行装置28包括一警报器281。若所述执行装置28接收状态判断装置26发出的摇晃控制指令则通过所述警报器281发出摇晃警报声。若所述执行装置28接收到来自状态判断装置26发出的倾斜警报指令则通过所述警报器281发出倾斜警报声。
因为所述加速度计221和陀螺仪222的测量误差随着感测时间的延长上升得很快，而所述"。
数据处理器25是通过对所述加速度计221和陀螺仪222所感测到的加速度 进行积分来推算出机器人运动的速度值和位移值。所以所述陀螺仪222和加速度计221的测量误差会进入积分公式进行累积从而导致所计算出来的速度值和位移值的误差越来越大。因此，每当机器人运动一段距离后就必须通过设置在运动路径上的定位装置210来对机器人所感测到的相对于起
始参考直角坐标系^^KZ。的总位移S。进行校正。
如图1所示，所述定位装置210包括一初始定位器2101和至少一个辅助定位器2102。所述初始定位器2101设置在机器人的运动起点，即所述起始参考直角坐标系 的原点处。在机器人每次重新开始运动之前，所述初始定位器2101对机器人进行初始化设定，清除数据处理器25内关于机器人前一次运动的运动参量。
机器人每次从运动起点出发之前都已设置好即将运行的运动路径。所述辅助定位器2102从初始定位器2101开始沿设置好的机器人运动路径按均等间距分布。因为所述每一个辅
助定位器2102相对于所述初始定位器2101具有确定的位移S ，将所述位移S定义为对应
的辅助定位器2102的校正位移。所述每一个辅助定位器2102上均设置有一与所述校正位移《
O相对应的数码标签2103。
所述误差校正装置212包括一读取器2120。在机器人每经过一辅助定位器2102时，所述读取器从辅助定位器2102的数码标签2103上读取对应的校正位移S 。所述误差校正装置212将所获取的校正位移S与所述数据处理器25所计算出来的机器人相对于起始参考直角
坐标系 的总位移。相减得到位移校正差值^ 。在经过下一个辅助定位器2102
之前，所述数据处理器25根据所述位移校正差值^对其计算出来的机器人相对于起始参考直角坐标系 的总位移°进行校正。此后运动过程中，机器人将不断地到达新的
辅助定位器2102以更新所述位移校正差值^来对由所述数据处理器25计算出来的相对于
起始参考直角坐标系的总位移^^。进行校正，从而确保所得到的总位移^^。的精确度。可以理解的是，需要设置的辅助定位器2102的个数由机器人运动路径的长短和所述感测装置22的精确度所决定。所述感测装置22的精密度越高则在相同的运动路径内所需要的辅助定位器的个数越少。
与现有技术相比，本发明所提供的机器人状态感知系统利用加速度计和陀螺仪测量机器人运动过程中的各种运动参量，并通过计算和分析来判断机器人所处的运动状态，以简单的结构和较低的成本实现了机器人的状态感知。其次，在机器人运动路径上设置间距相等的定位装置以提供与所述定位装置所处位置相关的校正信息，并通过误差校正装置来感测所述定位装置上的校正信息从而对由数据处理器计算出来的机器人运动位移进行校正以克服机器人状态感知系统在长时间使用过程中所产生的累积误差。
最后应说明的是，以上实施方式仅用以说明本发明的技术方案而非限制，尽管参照较佳实施方式对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或等同替换，而不脱离本发明技术方案的精神和范围。
权利要求
1.一种机器人状态感知系统，其包括感测装置，用于测量机器人运动的加速度信号和角速度信号；数据接收装置，与所述感测装置连接，用于对所述感测装置测得的加速度信号和角速度信号进行滤波和数字化处理；数据处理器，与所述数据接收装置连接，用于根据所述数据接收装置滤波和数字化处理后的加速度信号和角速度信号计算机器人运动的加速度、角速度和位移；状态判断装置，与所述数据接收装置和数据处理器连接，用于根据所述数据接收装置滤波和数字化处理后的加速度信号和所述数据处理器计算出的加速度判断机器人所处的运动状态；执行装置，与所述状态判断装置连接，用于根据机器人所处的状态发出相对应的警报声，其特征在于，所述机器人状态感知系统进一步包括定位装置，其沿机器人运动路径按均等间距分布，所述定位装置上设置有数码标签，所述数码标签上的内容为与所述定位装置所处位置相关的校正信息；误差校正装置，其与所述数据处理器相连接，用于读取所述数码标签上的校正信息并根据所述校正信息对所述数据处理器计算出来的机器人运动的位移进行校正。
2.如权利要求l所述的机器人状态感知系统，其特征在于，所述数 据接收装置包括一截止频率为10赫兹的高通滤波器、 一截止频率为12. 5赫兹的低通滤波器及 一截止频率为50赫兹的低通滤波器，所述高通滤波器和低通滤波器对感测装置测得的加速度 模拟信号和角速度模拟信号进行滤波处理并分别转换为加速度数字信号和角速度数字信号。
3.如权利要求l所述的机器人状态感知系统，其特征在于，所述感 测装置包括一加速度计和一陀螺仪，所述加速度计包括感测轴方向相互垂直的三个加速度感 测器，所述陀螺仪包括感测轴方向与所述加速度感测器的感测轴方向相对应的三个角速度感 测器，以所述加速度感测器和角速度感测器的感测轴方向为坐标轴定义一感测活动直角坐标系，将机器人初始状态时的感测活动直角坐标系定义为起始参考直角坐标系。
4.如权利要求3所述的机器人状态感知系统，其特征在于，所述数据处理器根据经截止频率为50赫兹的低通滤波器处理后的角速度数字信号计算出所述感测活动直角坐标系与起始参考直角坐标系之间的坐标转换矩阵。
5.如权利要求4所述的机器人状态感知系统，其特征在于，所述数据处理器根据经截止频率为12.5赫兹的低通滤波器处理后的加速度数字信号和所述感测活动直角坐标系与起始参考直角坐标系之间的坐标转换矩阵计算出机器人相对于起始参考直角坐标系的加速度。
6.如权利要求5所述的机器人状态感知系统，其特征在于，所述状 态判断装置根据机器人相对于起始参考直角坐标系的加速度与所述起始参考直角坐标系三个 坐标轴之间的倾斜角以判断机器人是否处于倾斜状态。
7.如权利要求6所述的机器人状态感知系统，其特征在于，如果所述 状态判断装置判断机器人处于倾斜状态则向所述执行装置发出倾斜警报指令，所述执行装置 接收到所述倾斜警报指令后发出倾斜警报声。
8.如权利要求5所述的机器人状态感知系统，其特征在于，所述数 据处理器通过对所述机器人相对于起始参考直角坐标系的加速度进行积分运算得到所述机器 人相对于起始参考直角坐标系的速度。
9.如权利要求8所述的机器人状态感知系统，其特征在于，所述数 据处理器通过对所述机器人相对于起始参考直角坐标系的速度进行积分运算得到所述机器人 相对于起始参考直角坐标系的位移。
10.如权利要求2所述的机器人状态感知系统，其特征在于，所述状 态判断装置检测经截止频率为10赫兹的高通滤波器处理后的加速度数字信号，若检测到持续 时间超过一预设摇晃时间阈值且加速度值呈正负相间变化的加速度数字信号时，所述状态判 断装置判断机器人处于被摇晃的状态。
11.如权利要求10所述的机器人状态感知系统，其特征在于，如果 所述状态判断装置判断机器人处于被摇晃的状态则向所述执行装置发出摇晃警报指令，所述 执行装置在接收到摇晃警报指令后发出摇晃警报声。
12.如权利要求l所述的机器人状态感知系统，其特征在于，所述定 位装置包括一初始定位器和至少一个辅助定位器，所述初始定位器设置于机器人的运动起点 ，用于对机器人进行初始化设定，所述辅助定位器从初始定位器开始沿机器人运动路径按均等间距分布。
13 如权利要求12所述的机器人状态感知系统，其特征在于，每一 个所述辅助定位器相对于初始定位器的位移定义为校正位移，所述每一个辅助定位器上均设 置有一与所述校正位移相对应的数码标签。
14 如权利要求13所述的机器人状态感知系统，其特征在于，所述 误差校正装置包括一读取器，在机器人每次经过所述辅助定位器时，所述读取器读取与数码 标签相对应的校正位移信息，并根据所述校正位移校正由所述数据处理器计算出来的机器人 运动的位移。
15 如权利要求12所述的机器人状态感知系统，其特征在于，所述 辅助定位器的个数取决于所述感测装置的精密度，若所述感测装置的精密度越高则在相同运 动路径内所需要的辅助定位器的个数越少。
全文摘要
一种机器人状态感知系统，其包括一感测装置、一数据接收装置、一数据处理器、一状态判断装置、一执行装置、一定位装置以及一误差校正装置。所述感测装置测量与机器人运动的加速度信号和角速度信号并经数据接收装置滤波和数字化处理后由所述数据处理器计算出与机器人运动的各种运动参量。所述状态判断装置根据机器人的运动参量判断机器人所处的运动状态并控制所述执行装置发出对应的警报声。所述定位装置沿机器人运动路径按均等间距分布于机器人的运动区域内并提供与其所处位置相关的校正信息。所述误差校正装置感测所述定位装置提供的校正信息并据此对数据处理器计算出来的机器人的运动位移进行校正，以确保所感知的机器人运动状态更加精确。
文档编号G01C21/26GK101685308SQ200810304600
公开日2010年3月31日 申请日期2008年9月22日 优先权日2008年9月22日
发明者文 舒 申请人:鸿富锦精密工业(深圳)有限公司;鸿海精密工业股份有限公司