一种单片机机器人直线导航方法与流程

本发明设计机器人技术领域，具体而言，涉及一种单片机机器人直线导航方法。

技术背景

目前，随着信息技术的快速发展，机器人被应用在各个领域，在工业上，机器人可以代替人力搬运货物；在生活中，机器人也可以代替人类执勤巡检等。

当前，工业机器人搬运货物和机器人巡检等，机器人的直线运行是必不可少的，一般机器人都是用的磁条或激光来导航，但是磁条导航运行速度有限，而且需要在地面贴上磁条；而激光导航又成本太高，且易受空中物流或障碍物的影响。

技术实现要素：

本发明要克服现有技术的上述缺点，提供一种单片机机器人直线导航方法，实现通过惯导传感器和初始导轨确定机器所处姿态，并最终通过惯导传感器与速度采集模块共同作用来保证机器的直行。

一种单片机机器人直线导航方法，包括如下步骤：

步骤1，将机器在初始位置校零，

步骤2，获取惯导传感器数值,

步骤3，滤波，

步骤4，得到较为准确的数值，并代入方向环；

步骤5，将方向环与速度环结合起来调节机器直线前进

步骤6，待机器运行一周，检测是否发生零飘，若没有，则转入步骤2；若有，则转入步骤1，以此循环，通过惯导传感器和初始导轨确定机器所处姿态，并最终通过惯导传感器与速度采集模块共同作用来保证机器的直行。

步骤1所述的机器在初始位置校零是机械校零或按键校零；具体包括：

11,在初始位置有一段机械导轨，方便机器步入导轨机械校零，避免机器在初始位置前进方向就存在偏差；

12，待机器姿态调正后按键复位校零。

步骤2所述的获取惯导传感器值，具体包括：

步骤s1021：采取定时中断处理，为数据采集和处理提供时间。

步骤s1022：通过集成电路总线(iic)获取绕z轴的角度和角速度,iic总线是双向、两线(scl、sda)、串行、多主控接口标准，具有总线仲裁机制；

步骤s1023：等待数据进一步处理，为滤波做好准备。

步骤3所述的滤波是所述软件滤波，具体包括：

31.先连续取n个采样值看成一个队列；

32.每次采样到一个新的数据放入队尾，并扔掉原来队首的一次数据；

33.最后把队列中的n个数据进行算术平均运算。

步骤4所述的方向环包括：

41.先设置机器的角度目标值，采集滤波结束后的角度和角速度；

42.再计算角度的偏差；通过整定参数kp1和参数kp2,从而进一步调节机器的姿态。

步骤5所述的速度环包括：

51.先设置机器的速度目标值，采集机器此刻的运行速度；

52.再计算此次的速度偏差；同时将此次的偏差与上次的偏差做差；

53.通过调节参数kp2和参数ki2，从而得出速度的增量；

54.最后将此次的速度偏差值赋给上次的偏差，最终达到控制机器速度的目的。

步骤5所述的方向环和速度环一起调节机器直线运行包括：

s51.用第一获取模块采集机器直线运行的角度值和角速度值；

s52.用第二获取模块获取机器的速度值；

步骤6所述的待机器运行一周检测是否发生零飘包括：用确定是否零飘模块判断运行一周是否发生零飘和校零。

第一方面，本发明提供了一种单片机机器人直线导航方法，所述方法包括：先将机器在初始位置校零，获取惯导传感器数值后，经滤波得到较为准确的数值，并代入方向环；紧接着将方向环与速度环结合起来调节机器直线前进。待机器运行一周，检测是否发生零飘，若没有，则继续运行并继续读取惯导传感器的数值；若有，则重新校完零再继续往下，以此循环。通过惯导传感器和初始导轨确定机器所处姿态，并最终通过惯导传感器与速度采集模块共同作用来保证机器更快的直行运动。

结合第一方面，其中，所述在初始位置校零包括：采取机械校零和按键校零相结合。在初始位置布有一段机械导轨，方便机器步入导轨机械校零，避免机器在初始位置前进方向就存在偏差；待机器姿态调正后可以按键复位校零。

结合第一方面，其特征在于，所述获取惯导传感器值，包括：进入中断后，通过iic(集成电路总线)获取绕z轴的角度和角速度；待获取完数值后，对数值进行滤波处理。

结合第一方面，所述滤波包括：硬件滤波和软件滤波。本发明实例所采取的传感器内部自带卡尔曼滤波，因此只需考虑软件滤波。所述软件滤波方法包括：先连续取n个采样值看成一个队列；每次采样到一个新的数据放入队尾，并扔掉原来队首的一次数据；最后把队列中的n个数据进行算术平均运算。

结合第一方面，所述方向环包括：先设置机器的角度目标值，采集滤波结束后的角度和角速度；再计算角度的偏差；通过整定参数kp1和参数kp2,从而进一步调节机器的姿态。

结合第一方面，所述速度环包括：先设置机器的速度目标值，采集机器此刻的运行速度；再计算此次的速度偏差；同时将此次的偏差与上次的做差；通过调节参数kp2和参数ki2，从而得出速度的增量；最后将此次的速度偏差值赋给上次的偏差。同时利用定时中断重复以上动作。

第二方面，本发明提供了一种单片机机器人直线导航方法，所述方向环和速度环一起调节机器直线运行包括：第一获取模块，用于采集机器直线运行的角度值和角速度值；第二获取模块，用于获取机器的速度值；以及确定是否零飘模块，用于判断运行一周是否发生零飘和校零。

结合第二方面，所述第一获取模块包括：惯导传感器，用于检测机器所处角度与运行的角速度；

滤波单元，用于获得较为精准的数值代入后续的方向环。

结合第二方面，所述第二获取模块包括：脉冲采集器(编码器)，用于采集机器运行的脉冲值，从而得出速度值。电机单元，用于控制速度输出。

结合第二方面，所述的确定是否零飘模块包括：惯导传感器，用于采集运行一周后，机器重新回到初始地点时的角度值。

初始导轨，用于保证机器运行一周后重新回到初始地点时与出发时的位置保持一致，从而方便判断传感器是否零飘。

本发明的优点是：提高了机器直线行走的准确性，减小了偏离的误差，从而可以控制机器更快的到达目的地。

附图说明

图1示出了本发明方法的流程图。

图2示出了获取惯导传感器值的流程图。

图3示出了软件滤波的流程图。

图4示出了本发明提供的机器人直线导航方法中方向环的流程图。

图5示出了本发明提供的机器人直线导航方法中速度环的流程图。

图6示出了本发明提供的机器人直线导航方法中，方向环和速度环一起调节机器直线运行的示意图。

图7示出了本发明提供的机器人直线导航方法中，确定是否零飘模块的示意图。

具体实施方法

下面将结合本发明中的附图，对本发明进行清楚完整地描述。

考虑到现有技术中，磁条导航运行速度有限，而且需要在地面贴上磁条；而激光导航又成本太高，且易受空中物流或障碍物的影响。基于此，本发明提供了一种单片机机器人直线导航方法，实现通过惯导传感器和初始导轨确定机器所处姿态，并最终通过惯导传感器与速度采集模块共同作用来保证机器的直行。提高了机器直线行走的准确性，减小了偏离的误差，从而可以控制机器更快地到达目的地。

参见图1，步骤s101：初始校零。采取机械校零和按键校零相结合。在初始位置布有一段机械导轨，方便机器步入导轨机械校零，避免机器在初始位置前进方向就存在偏差；待机器姿态调正后可以按键复位校零。

步骤s102：获取惯导传感器值。由于获取的方法有很多，且数据的获取和处理也需要时间，本发明中根据以下s1021-s1023实现获取惯导传感器值。

步骤s1021：采取定时中断处理，为数据采集和处理提供时间。

步骤s1022：通过iic(集成电路总线)获取绕z轴的角度和角速度,iic总线是双向、两线(scl、sda)、串行、多主控接口标准，具有总线仲裁机制，非常适合在器件之间进行近距离的数据通信。在它的协议体系中，传输数据时都会带上目的设备的设备地址，因此可以方便后期的设备组网。

步骤s1023：等待数据进一步处理，为滤波做好准备。

步骤s103:滤波。滤波包括：硬件滤波和软件滤波。本发明所采取的传感器内部自带卡尔曼滤波，因此只需考虑软件滤波。所述软件滤波方法包括s1031-s1033：先连续取n个采样值看成一个队列；每次采样到一个新的数据放入队尾，并扔掉原来队首的一次数据；最后把队列中的n个数据进行算术平均运算。此滤波方法对周期性干扰有良好的抑制作用，平滑度高。

步骤s104：将获得的较为准确值代入方向环。经滤波处理后所得到的角度值和角速度值准确度都将进一步提高，而所述的方向环又如图4所示。先设置机器的角度目标值target_angle，采集滤波结束后的角度angle和角速度gyro；再计算角度的偏差ε＝target_angle-angle；将kp1*ε+kd1*gyro的计算结果赋予a，通过整定参数kp1和参数kp2,输出a，为后续和速度环一起调节直行做准备，从而进一步调节机器的姿态。

所述的速度环如图5所示：先设置机器的速度目标值target_speed和初值ε(k-1)＝ε(k)＝0，采集机器此刻的运行速度speed；再计算此次的速度偏差ε(k)＝target_speed-speed；同时将此次的偏差与上次的做差ε(k)-ε(k-1)；紧接着计算增量△u＝kp2*(ε(k)-ε(k-1))+ki2*ε(k)，通过调节参数kp2和参数ki2，从而得出速度的增量△u；然后将△u+b重新赋给b,最后将此次的速度偏差值赋给上次的偏差。b是最终输出的速度。

步骤s105：方向环和速度环一起调节机器直线运行。整个方案涉及到了：第一获取模块s1，用于采集机器直线运行的角度值和角速度值；第二获取模块s2，用于获取机器的速度值；以及确定是否零飘模块s3，用于判断运行一周是否发生零飘和校零。

所述第一获取模块s1包括：惯导传感器，用于检测机器所处角度与运行的角速度；滤波单元，用于获得较为精准的数值代入后续的方向环。

所述第二获取模块s2包括：脉冲采集器(编码器)，用于采集机器运行的脉冲值，从而得出速度值。电机单元，用于控制速度的输出。

步骤s106，待机器运行一周，是否发生零飘。所述的确定是否零飘模块s3包括：第一获取模块s1，用于采集运行一周后，机器重新回到初始地点时的角度值。

确定单元s31：初始导轨。用于保证机器运行一周后重新回到初始地点时与出发时的位置保持一致，从而方便判断传感器是否零飘。初始导轨上可以布置卡槽等机构，机器运行一周后重新回到初始位置，确保和初始校零时所处角度和位置一致，此时只需观察第一获取模块s1的数值就可以判断是否发生零飘。

本发明实现通过第一获取模块s1,第二获取模块s2,确定是否零飘模块s3共同作用来保证机器的直行。提高了机器直线行走的准确性，减小了偏离的误差，并且可以控制机器更快地到达目的地。

本说明书实施例所述的内容仅仅是对发明构思的实现形式的列举，本发明的保护范围不应当被视为仅限于实施例所陈述的具体形式，本发明的保护范围也及于本领域技术人员根据本发明构思所能够想到的等同技术手段。