机器人导航系统的制作方法

本发明关于一种导航系统，特别是，一种结合导航参考器和编码器且适用于机器人的机器人导航系统。
背景技术：
：近来随着传感器的进步和控制技术的成熟，机器人不再局限于在工厂加工处理组件而逐渐从工厂迈进日常生活中，以协助人们处理日常生活中无聊的重复性工作，例如打扫或搬运。另外，由于各种电子零件的成本下降，进而降低机器人的制造成本而开始商品化，越来越受到业界重视。机器人中相当重要的部分为自主导航，亦即，机器人自己行走而不需使用者控制，而通常其为透过传感器和处理器来辅助机器人行进，然而，机器人在行进时总会伴随摩擦力的产生，摩擦力则会造成传感器的误差，因此，如何精准演算出机器人的路径且考虑到摩擦力产生的误差，遂成为一个重要的议题。综观前文所述，本发明的发明者思索并设计一种机器人导航系统，以期针对公知技术的缺失加以改善，进而增进产业上的实施利用。申请内容有鉴于上述现有的问题，本发明的目的在于提供一种机器人导航系统，用以解决现有技术中所面临的问题。基于上述目的，本发明提供一种机器人导航系统，其包括机器人、状态判断器、导航参考器以及误差补偿滤波器。机器人包括三轴式加速度计、三轴式陀螺仪以及轮部，轮部包括左轮、右轮以及编码器，三轴式加速度计侦测机器人于各轴的加速度并整合及演算出推进力信息，三轴式陀螺仪侦测机器人于各轴的角速度并整合成角速度信息，编码器依据左轮的旋转角度和右轮的旋转角度演算出第一方向角信息，并计算出左轮的角速度和右轮的角速度以整合成轮部角速度信息；状态判断器连接三轴式加速度计、三轴式陀螺仪以及编码器，以取得推进力信息、角速度信息以及第一方向角信息，并据此判断机器人为静态状态或动态状态；导航参考器连接三轴式加速度计以及三轴式陀螺仪以取得推进力信息以及角速度信息，并据此演算出第二方向角信息；误差补偿滤波器连接导航参考器和编码器，以取得第一方向角信息和第二方向角信息。当状态判断器判断机器人为动态状态，误差补偿滤波器据此演算出误差预估信息，并将误差预估信息回传至编码器，根据误差预估信息加以调整第一方向角信息，进而调整机器人的位置、速度以及方向角，让机器人准确地行进。优选地，状态判断器包括编码器模块以及惯性模块，编码器模块判断第一方向角信息的轮部编码讯号值，惯性模块比较推进力信息的数值减去重力的数值和推进力临界值以及比较角速度信息的数值和角速度临界值。优选地，编码器模块判断第一方向角信息的轮部编码讯号值为零，惯性模块判断推进力信息的数值减去重力的数值小于推进力临界值以及角速度信息的数值小于角速度临界值，机器人则为静态状态。优选地，编码器模块判断第一方向角信息的轮部编码讯号值为零，惯性模块判断推进力信息的数值减去重力的数值大于推进力临界值以及角速度信息的数值大于角速度临界值，机器人则从动态状态恢复至静态状态。优选地，编码器模块判断第一方向角信息的轮部编码讯号值不等于零，惯性模块判断推进力信息的数值减去重力的数值小于推进力临界值以及角速度信息的数值小于角速度临界值，机器人则为所述静态状态，而左轮和右轮正在自转。优选地，编码器模块判断第一方向角信息的轮部编码讯号值不等于零，惯性模块判断推进力信息的数值减去重力的数值大于推进力临界值以及角速度信息的数值大于角速度临界值，机器人则为动态状态。优选地，编码器通过方向推测技术演算出第一方向角信息。优选地，误差补偿滤波器进一步包含比例积分微分控制组件和模糊规则增益调整组件，比例积分微分控制组件具有比例参数和积分参数以調整误差预估信息的數值。优选地，誤差补偿滤波器依據第一方向角信息的第一方向角数值和第二方向角信息的第二方向角數值演算出误差预估信息，並回傳误差预估信息至编码器和導航參考器，進而調整第一方向角信息和第二方向角信息。优选地，比例积分微分控制组件依据模糊规则调整比例参数和积分参数，模糊规则增益调整组件依据左轮的角速度和右轮的角速度来规定模糊规则。承上所述，本发明的机器人导航系统，可具有下述优点：(1)本发明的机器人导航系统，通过编码器和导航参考器准确地计算出误差预估信息，据此加以调整轮部的第一方向角信息，进而调整机器人的位置、速度以及方向角，让机器人更为准确地行进。(2)本发明的机器人导航系统，通过状态判断器判断机器人的状态，得知机器人是否在前进或停止。附图说明图1为本发明的第一实施例的整体结构图；图2为本发明的第一实施例的状态判断器的判断流程图；图3为本发明的第一实施例的流程图；图4为本发明的第一实施例的误差补偿滤波器的组件图；图5为本发明的第一实施例的左轮角速度及右轮角速度和归属度的关系图。其中的标记为：10：机器人11：三轴式加速度计12：三轴式陀螺仪13：轮部20：导航参考器30：状态判断器31：编码器模块32：惯性模块40：误差补偿滤波器41：比例积分微分控制组件42：模糊规则增益调整组件131：左轮132：右轮133：编码器af：推进力信息ar：角速度信息a1：第一方向角信息a2：第二方向角信息er：误差预估信息fa：推进力信息的数值ft：推进力临界值g：重力s1、s2、s3、s4：流程ull：左轮低速值ulm：左轮中速值ulh：左轮高速值url：右轮低速值urm：右轮中速值urh：右轮高速值vr：角速度信息的数值vt：角速度临界值war：轮部角速度信息ya1：轮部编码讯号值yh1：第一方向角数值yh2：第二方向角数值具体实施方式本发明的优点、特征以及达到的技术方法将参照例示性实施例及附图进行更详细地描述而更容易理解，且本发明可以不同形式来实现，故不应被理解仅限于此处所陈述的实施例，相反地，对所属
技术领域：
的普通技术人员而言，所提供的实施例将使本揭露更加透彻与全面且完整地传达本发明的保护范围，且本发明将仅替所附加的权利要求书所定义。如图1所示，其为本发明的机器人导航系统的第一实施例的整体结构图。在本实施例中，本发明的机器人导航系统包括机器人10、状态判断器30、导航参考器20以及误差补偿滤波器40。机器人10包括三轴式加速度计11、三轴式陀螺仪12以及轮部13，轮部13包括左轮131、右轮132以及编码器133，三轴式加速度计11侦测机器人10于各轴的加速度并整合及演算出推进力信息af，三轴式陀螺仪12侦测机器人10于各轴的角速度并整合成角速度信息ar，编码器133依据左轮131的旋转角度和右轮132的旋转角度并通过方向推测技术，而演算出第一方向角信息a1，并计算出左轮131的角速度ul和右轮132的角速度ur以整合成轮部角速度信息war；状态判断器30连接三轴式加速度计11、三轴式陀螺仪12以及编码器133，以取得推进力信息af、角速度信息ar以及第一方向角信息a1，并包括编码器模块31以及惯性模块32，编码器模块31判断第一方向角信息a1的数值ya1，惯性模块32比较推进力信息af的数值fa减去重力g的数值和推进力临界值ft以及比较角速度信息ar的数值vr和角速度临界值vt，据此判断机器人10为静态状态或动态状态；导航参考器20连接三轴式加速度计11以及三轴式陀螺仪12以取得推进力信息af以及角速度信息ar，并据此演算出第二方向角信息a2；误差补偿滤波器40连接导航参考器20和编码器133，以取得第一方向角信息a1和第二方向角信息a2，且连接状态判断器30以得知机器人10为静态状态或动态状态。需说明的是，第一方向角信息a1包含第一方向角数值yh1，第二方向角信息a2包含第二方向角数值yh2，误差补偿滤波器40乃依据第一方向角数值yh1和第二方向角数值yh2进行演算，以取得误差预估信息er。具体而言，三轴式加速度计11、三轴式陀螺仪12以及编码器133分别根据其表达式子演算推进力信息af、角速度信息ar以及第一方向角信息a1，三轴式加速度计11、三轴式陀螺仪12以及编码器133的表达式子分别如下：fa＝f+g+ea+wavr＝v+er+wrfa为推进力信息af的数值，f为目前的推进力，g为重力，ea为三轴式加速度计11的误差，wa为三轴式加速度计11的白噪声；vr为角速度信息ar的数值，v为目前三轴式陀螺仪12的角速度，er为三轴式陀螺仪12的误差，wr为三轴式陀螺仪12的白噪声；yal为左轮131的编码讯号值，为左轮131的旋转角度，yar为右轮132的编码讯号值，为右轮132的旋转角度，c为常数，第一方向角信息a1的数值包括轮部编码讯号值ya1，轮部编码讯号值ya1包括yal和yar。请参阅附图2，其为本发明的机器人导航系统的第一实施例的状态判断器的判断流程图。在此，结合附图详细说明状态判断器30的判断流程如下：(1)编码器模块31判断第一方向角信息a1的轮部编码讯号值ya1为零，惯性模块32判断推进力信息af的数值fa减去重力g的数值小于推进力临界值ft以及角速度信息ar的数值vr小于角速度临界值vt，得知机器人10的左轮131和右轮132为停止状态且未加速前进，据此判断机器人10为静态状态。(2)编码器模块31判断第一方向角信息a1的轮部编码讯号值ya1为零，惯性模块32判断推进力信息af的数值fa减去重力g的数值大于推进力临界值ft以及角速度信息ar的数值vr大于角速度临界值vt，得知机器人10的左轮131和右轮132为停止状态，但三轴式加速度计11和三轴式陀螺仪12仍在运作，据此判断机器人10从动态状态恢复至静态状态。(3)编码器模块31判断第一方向角信息a1的轮部编码讯号值ya1不等于零，惯性模块32判断推进力信息af的数值fa减去重力g的数值小于推进力临界值ft以及角速度信息ar的数值vr小于角速度临界值vt，得知机器人10未前进但左轮131和右轮132正在运行，据此判断机器人10则为静态状态，而左轮131和右轮132正在自转。(4)编码器模块31判断第一方向角信息a1的轮部编码讯号值ya1不等于零，惯性模块32判断推进力信息af的数值fa减去重力g的数值大于推进力临界值ft以及角速度信息ar的数值vr大于角速度临界值vt，得知机器机器人10正在前进或后退，据此判断机器人10为动态状态。另外，当状态判断器30判断机器人10为静态状态时，因为机器人10并未前进，所以并未有前进误差，开始估计三轴式加速度计11本身的误差以及编码器133因为环境所产生的误差，环境所产生的误差主要来自于左轮131和右轮132与地面的摩擦力所造成的，三轴式加速度计11本身的误差传至导航参考器20，编码器133由于环境所产生的误差会传至编码器133，以准确地演算第一方向角信息a1和第二方向角信息a2。请参阅附图3，其为本发明的机器人导航系统的第一实施例的流程图。在此，结合附图说明误差补偿滤波器40的运作机制如下：(1)s1：状态判断器30判断机器人10为动态状态，并传至误差补偿滤波器40以让其运作。(2)s2：误差补偿滤波器40接收第一方向角信息a1的第一方向角数值yh1和第二方向角信息a2的第二方向角数值yh2，并依据模糊规则演算出误差预估信息er。(3)s3：误差补偿滤波器40将误差预估信息er回传至编码器133和导航参考器20，编码器133根据误差预估信息er加以调整第一方向角信息a1的第一方向角数值yh1，导航参考器20据此调整而让第二方向角信息a2的第二方向角数值yh2更为准确(4)s4：机器人10则据此调整其位置、速度以及方向角，让机器人10准确地行进。请参阅附图4，其为本发明的机器人导航系统的第一实施例的误差补偿滤波器的组件图。如图所示，误差补偿滤波器40包含比例积分微分控制组件41以及模糊规则增益调整组件42，模糊规则增益调整组件42则根据左轮131的角速度ul和右轮132的角速度ur来规定模糊规则，比例积分微分控制组件41依据模糊规则调整比例参数kp、积分参数ki，比例积分微分控制组件41接受第一方向角信息a1的第一方向角数值yh1以及第二方向角信息a2的第二方向角数值yh2，则根据运算公式演算出误差预估信息er的数值δ，运算公式如下：δ＝kp(yh2-yh1)+(ki(yh2-yh1))/s此运算公式也考虑到之前的编码器133和导航参考器20产生的误差，因此误差估算会越来越精准，机器人10的移动精准度也随之提高。请参阅附图5，其为本发明的机器人导航系统的第一实施例的左轮角速度及右轮角速度和归属度的关系图。在此，先搭配附图5将左轮131的角速度ul和右轮132的角速度ur分类为低速区、中速区以及高速区，并具体叙明如下:(1)左轮131的角速度ul处于低速区时为左轮131在低速运动，当左轮131的角速度ul为左轮低速值ull，归属度为1及其代表左轮131确实处于低速状态；左轮131的角速度ul处于中速区时为左轮131在中速运动，当左轮131的角速度ul为左轮中速值ulm，归属度为1及其代表左轮131确实处于中速状态；左轮131的角速度ul处于高速区时为左轮131在高速运动，当左轮131的角速度ul为左轮高速值ulh，归属度为1及其代表左轮131确实处于高速状态。(2)右轮132的角速度ur处于低速区时为右轮132在低速运动，当右轮132的角速度ur为右轮低速值url，归属度为1及其代表右轮132确实处于低速状态；右轮132的角速度ur处于中速区时为右轮132在中速运动，当右轮132的角速度ur为右轮中速值urm，归属度为1及其代表右轮132确实处于中速状态；右轮132的角速度ul处于高速区时为右轮132在高速运动，当右轮132的角速度ur为右轮高速值urh，归属度为1及其代表右轮132确实处于高速状态。续言之，搭配附图5和表1说明左轮131的角速度ul和右轮132的角速度ur所规定的模糊规则如下：(1)当左轮131的角速度ul和右轮132的角速度位于相同速度区间时，亦即左轮131和右轮132皆位于低速区、左轮131和右轮132皆位于中速区或左轮131和右轮132皆位于高速区，机器人10正在直线运动，此时的比例参数kp设定为cps和积分参数ki设定为cis。(2)当左轮131的角速度ul和右轮132的角速度ur相差不大时，亦即左轮131位于低速区和右轮132位于中速区、左轮131位于中速区和右轮132位于低速区、左轮131位于高速区和右轮132位于中速区或左轮131位于中速区和右轮132位于高速区，机器人10正在例如小转弯之低速旋转运动，此时的比例参数kp设定为cpl和积分参数ki设定为cil。(3)当左轮131的角速度ul和右轮132的角速度ur相差甚巨时，亦即左轮131位于高速区和右轮132位于低速区或左轮131位于低速区和右轮132位于高速区，机器人10正在例如大转弯的高速旋转运动，此时的比例参数kp设定为cph和积分参数ki设定为cih。表1左轮位于低速区左轮位于中速区左轮位于高速区右轮位于低速区直线运动低速旋转运动高速旋转运动右轮位于中速区低速旋转运动直线运动低速旋转运动右轮位于高速区高速旋转运动低速旋转运动直线运动其中，cps、cis、cpl、cil、cph和cih皆为常数且各个相异，其大小顺序如下：cph>cpl>>cps，cih>cil>>cis。cps、cis、cpl、cil、cph和cih的数值大小为根据机器人10的实际运作状况而定，而未加以限制其实际数值大小。再者，可将本发明应用于扫地机器人或其他智能机器人。举例来说，应用本发明于自主导航机器人上，通过三轴式加速度计11和三轴式陀螺仪12设置，测量自主导航机器人于三轴的加速度和角速度，得出自主导航机器人的推进力信息af和角速度信息ar并传至导航参考器20，进而取得第二方向角信息a2，误差补偿滤波器40比较第二方向角信息a2和编码器133演算出的第一方向角信息a1而取得误差预估信息er，误差预估信息er回传至导航参考器20和自主导航机器人，而加以调整自主导航机器人的行进方向和速度，导航参考器20据此演算出更为精准的第二方向角信息a2，自主导航机器人的行进更为精准。综上所述，本发明的机器人导航系统，利用三轴式加速度计11、三轴式陀螺仪12和导航参考器20取得第二方向角信息a2，再搭配编码器133演算出的第一方向角信息a1，误差补偿滤波器40比较第二方向角信息a2和第一方向角信息a1而演算出误差预估信息er，并将误差预估信息er回传至导航参考器20和机器人10，进而让机器人10的行进更为精准，而不会脱离正常轨迹。本发明的机器人导航系统，利用导航参考器20和编码器133的搭配，让机器人10能顺利地运行前进。以上所述仅为举例性，而非为限制性者。任何未脱离本发明之精神与范围，而对其进行的等效修改或改动，均应包含于所附的权利要求书。当前第1页1 2 3