CN107132839A - 一种单片机机器人直线导航方法 - Google Patents

一种单片机机器人直线导航方法 Download PDF

Info

Publication number
CN107132839A
CN107132839A CN201710156788.9A CN201710156788A CN107132839A CN 107132839 A CN107132839 A CN 107132839A CN 201710156788 A CN201710156788 A CN 201710156788A CN 107132839 A CN107132839 A CN 107132839A
Authority
CN
China
Prior art keywords
machine
speed
zero
chip microcomputer
deviation
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN201710156788.9A
Other languages
English (en)
Inventor
沙炼
陈江
李昀达
徐烨
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Zhejiang University of Technology ZJUT
Original Assignee
Zhejiang University of Technology ZJUT
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Zhejiang University of Technology ZJUT filed Critical Zhejiang University of Technology ZJUT
Priority to CN201710156788.9A priority Critical patent/CN107132839A/zh
Publication of CN107132839A publication Critical patent/CN107132839A/zh
Pending legal-status Critical Current

Links

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Navigation (AREA)

Abstract

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

Description

一种单片机机器人直线导航方法
技术领域
本发明设计机器人技术领域，具体而言，涉及一种单片机机器人直线导航方法。
技术背景
目前，随着信息技术的快速发展，机器人被应用在各个领域，在工业上，机器人可以代替人力搬运货物；在生活中，机器人也可以代替人类执勤巡检等。
当前，工业机器人搬运货物和机器人巡检等，机器人的直线运行是必不可少的，一般机器人都是用的磁条或激光来导航，但是磁条导航运行速度有限，而且需要在地面贴上磁条；而激光导航又成本太高，且易受空中物流或障碍物的影响。
发明内容
本发明要克服现有技术的上述缺点，提供一种单片机机器人直线导航方法，实现通过惯导传感器和初始导轨确定机器所处姿态，并最终通过惯导传感器与速度采集模块共同作用来保证机器的直行。
一种单片机机器人直线导航方法，包括如下步骤：
步骤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共同作用来保证机器的直行。提高了机器直线行走的准确性，减小了偏离的误差，并且可以控制机器更快地到达目的地。
本说明书实施例所述的内容仅仅是对发明构思的实现形式的列举，本发明的保护范围不应当被视为仅限于实施例所陈述的具体形式，本发明的保护范围也及于本领域技术人员根据本发明构思所能够想到的等同技术手段。

Claims (8)

1.一种单片机机器人直线导航方法，包括如下步骤：
步骤1，将机器在初始位置校零，
步骤2，获取惯导传感器数值,
步骤3，滤波，
步骤4，得到较为准确的数值，并代入方向环；
步骤5，将方向环与速度环结合起来调节机器直线前进
步骤6，待机器运行一周，检测是否发生零飘，若没有，则转入步骤2；若有，则转入步骤1，以此循环，通过惯导传感器和初始导轨确定机器所处姿态，并最终通过惯导传感器与速度采集模块共同作用来保证机器的直行。
2.根据权利要求1所述的单片机机器人直线导航方法，其特征在于：步骤1所述的机器在初始位置校零是机械校零或按键校零；具体包括：
11,在初始位置有一段机械导轨，方便机器步入导轨机械校零，避免机器在初始位置前进方向就存在偏差；
12，待机器姿态调正后按键复位校零。
3.根据权利要求1所述的单片机机器人直线导航方法，其特征在于：步骤2所述的获取惯导传感器值，具体包括：
步骤S1021：采取定时中断处理，为数据采集和处理提供时间。
步骤S1022：通过集成电路总线(IIC)获取绕Z轴的角度和角速度,IIC总线是双向、两线(SCL、SDA)、串行、多主控接口标准，具有总线仲裁机制；
步骤S1023：等待数据进一步处理，为滤波做好准备。
4.根据权利要求1所述的单片机机器人直线导航方法，其特征在于：步骤3所述的滤波是所述软件滤波，具体包括：
31.先连续取N个采样值看成一个队列；
32.每次采样到一个新的数据放入队尾，并扔掉原来队首的一次数据；
33.最后把队列中的N个数据进行算术平均运算。
5.根据权利要求1所述的单片机机器人直线导航方法，其特征在于，步骤4所述的方向环包括：
41.先设置机器的角度目标值，采集滤波结束后的角度和角速度；
42.再计算角度的偏差；通过整定参数Kp1和参数Kp2,从而进一步调节机器的姿态。
6.根据权利要求1所述的单片机机器人直线导航方法，其特征在于：步骤5所述的速度环包括：
51.先设置机器的速度目标值，采集机器此刻的运行速度；
52.再计算此次的速度偏差；同时将此次的偏差与上次的偏差做差；
53.通过调节参数Kp2和参数Ki2，从而得出速度的增量；
54.最后将此次的速度偏差值赋给上次的偏差，最终达到控制机器速度的目的。
7.根据权利要求1所述的单片机机器人直线导航方法，其特征在于：步骤5所述的方向环和速度环一起调节机器直线运行包括：
S51.用第一获取模块采集机器直线运行的角度值和角速度值；
S52.用第二获取模块获取机器的速度值。
8.根据权利要求1所述的单片机机器人直线导航方法，其特征在于：步骤6所述的待机器运行一周检测是否发生零飘包括：用确定是否零飘模块判断运行一周是否发生零飘和校零。
CN201710156788.9A 2017-03-16 2017-03-16 一种单片机机器人直线导航方法 Pending CN107132839A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710156788.9A CN107132839A (zh) 2017-03-16 2017-03-16 一种单片机机器人直线导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710156788.9A CN107132839A (zh) 2017-03-16 2017-03-16 一种单片机机器人直线导航方法

Publications (1)

Publication Number Publication Date
CN107132839A true CN107132839A (zh) 2017-09-05

Family

ID=59721868

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710156788.9A Pending CN107132839A (zh) 2017-03-16 2017-03-16 一种单片机机器人直线导航方法

Country Status (1)

Country Link
CN (1) CN107132839A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107544574A (zh) * 2017-09-07 2018-01-05 上海电力学院 基于嵌入式arm和组合滤波的巡检机器人轮速控制方法

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN201993646U (zh) * 2011-01-27 2011-09-28 合肥工业大学 工业型激光导引agv的双闭环控制系统
CN103217157A (zh) * 2012-01-18 2013-07-24 北京自动化控制设备研究所 一种惯导/里程计自主组合导航方法
CN103543746A (zh) * 2012-07-09 2014-01-29 迪尔公司 用于自动割草机的导航系统和方法
CN105180934A (zh) * 2015-09-16 2015-12-23 成都四威高科技产业园有限公司 一种agv惯性导航方法
CN105180930A (zh) * 2015-09-16 2015-12-23 成都四威高科技产业园有限公司 一种agv惯性导航系统
CN105347181A (zh) * 2015-12-07 2016-02-24 南开大学 一种自动吊钩系统
CN106125742A (zh) * 2016-08-30 2016-11-16 宁波菜鸟智能科技有限公司 机器人的行走控制方法、装置及系统
CN106292664A (zh) * 2016-08-17 2017-01-04 国网山东省电力公司电力科学研究院 一种变电站巡检机器人导航控制系统及方法
CN106325275A (zh) * 2016-09-14 2017-01-11 广州今甲智能科技有限公司 一种机器人导航的系统、方法及装置
CN106325287A (zh) * 2015-06-14 2017-01-11 复旦大学 一种基于惯性/磁传感器marg姿态检测的智能割草机直线行走控制系统

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN201993646U (zh) * 2011-01-27 2011-09-28 合肥工业大学 工业型激光导引agv的双闭环控制系统
CN103217157A (zh) * 2012-01-18 2013-07-24 北京自动化控制设备研究所 一种惯导/里程计自主组合导航方法
CN103543746A (zh) * 2012-07-09 2014-01-29 迪尔公司 用于自动割草机的导航系统和方法
CN106325287A (zh) * 2015-06-14 2017-01-11 复旦大学 一种基于惯性/磁传感器marg姿态检测的智能割草机直线行走控制系统
CN105180934A (zh) * 2015-09-16 2015-12-23 成都四威高科技产业园有限公司 一种agv惯性导航方法
CN105180930A (zh) * 2015-09-16 2015-12-23 成都四威高科技产业园有限公司 一种agv惯性导航系统
CN105347181A (zh) * 2015-12-07 2016-02-24 南开大学 一种自动吊钩系统
CN106292664A (zh) * 2016-08-17 2017-01-04 国网山东省电力公司电力科学研究院 一种变电站巡检机器人导航控制系统及方法
CN106125742A (zh) * 2016-08-30 2016-11-16 宁波菜鸟智能科技有限公司 机器人的行走控制方法、装置及系统
CN106325275A (zh) * 2016-09-14 2017-01-11 广州今甲智能科技有限公司 一种机器人导航的系统、方法及装置

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
岳明等: "双闭环策略下非完整轮式机器人鲁棒自适应运动/力协调控制(英文)", 《控制理论与应用》 *
张弛: "采用惯性测量单元的移动机器人轨迹跟踪方法研究(英文)", 《控制理论与应用》 *
陈昌雄等: "基于差速驱动的移动机器人路径控制", 《装备制造技术》 *
魏娟: "一种差速驱动机器人路径控制方法", 《制造业自动化》 *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107544574A (zh) * 2017-09-07 2018-01-05 上海电力学院 基于嵌入式arm和组合滤波的巡检机器人轮速控制方法
CN107544574B (zh) * 2017-09-07 2020-12-22 上海电力学院 基于嵌入式arm和组合滤波的巡检机器人轮速控制方法

Similar Documents

Publication Publication Date Title
US20180273173A1 (en) Autonomous inspection of elongated structures using unmanned aerial vehicles
CN105352495B (zh) 加速度与光流传感器数据融合无人机水平速度控制方法
CN110220529B (zh) 一种路侧自动驾驶车辆的定位方法
CN103863948B (zh) 一种起重机协同载运的导航控制装置与导航控制的方法
CN104597905B (zh) 磁导航agv的路径跟踪方法
CN107111313A (zh) 用无人驾驶飞行器检查太阳能电池板
CN104197934B (zh) 一种基于地磁的定位方法、装置及系统
CN111442759A (zh) 一种综采工作面设备位姿统一监测系统
CN108469817B (zh) 基于fpga和信息融合的无人船避障控制系统
CN109178305A (zh) 一种水文监测水陆两栖无人机以及水文监测方法
CN109074638B (zh) 融合建图方法、相关装置及计算机可读存储介质
CN105468033A (zh) 一种基于多摄像头机器视觉的医疗吊臂自动避障控制方法
CN106707123A (zh) 一种基于无人机飞行平台的配电架空线路局放检测装置
CN101158581A (zh) 陀螺罗盘多位置自主定向寻北装置
CN106774387A (zh) 一种无人机避障方法及避障系统
CN106774374B (zh) 一种无人机自动巡检方法及系统
CN110162036A (zh) 一种掘进机自主导航定位系统及其方法
CN107132839A (zh) 一种单片机机器人直线导航方法
CN106919179A (zh) 一种四旋翼飞行器控制系统及控制方法
CN105511486A (zh) 一种基于异带航拍影像的电力弧垂检测方法及系统
CN107422727A (zh) 一种物流机器人横偏控制方法及物流机器人
CN110381253A (zh) 一种质量检测的控制方法、装置及系统
CN103317513A (zh) 一种基于多cpu的网络化机器人控制系统
CN102034244B (zh) 基于运动背景的绝缘子实时目标识别和跟踪装置
CN117093002A (zh) 无人驾驶的智能推耙机系统

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
RJ01 Rejection of invention patent application after publication
RJ01 Rejection of invention patent application after publication

Application publication date: 20170905