CN107132844B - 一种移动机器人基于姿态检测模块和特殊地标的运动自矫正方法 - Google Patents

Abstract

本发明公开了一种移动机器人基于姿态检测模块和特殊地标的运动自矫正方法，包括设置特殊地标，并在机器人本体上设置包括控制器、姿态检测模块、特殊地标识别模块等；所述的特殊地标包括含有当地坐标信息的二维码，以及在二维码四边中部垂直向外延伸的四条黑色巡迹线；所述的特殊地标识别模块包括二维码识别模块和光电传感器，二维码识别模块用于识别特殊地标中的二维码坐标，光电传感器用于读取巡迹线，当机器人检测到特殊地标时，利用巡迹线结合模糊PID控制进行位置矫正，当机器人离开特殊地标后，基于姿态检测模块结合常规PID控制进行位置矫正。采用本发明方法可使得室内移动机器人在定位和运动中更加精准，且成本低、不会有惯性误差。

Description

一种移动机器人基于姿态检测模块和特殊地标的运动自矫正 方法

技术领域

本发明涉及一种移动机器人基于姿态检测模块和特殊地标的运动自矫正方法，该移动机器人可以基于模糊PID控制和姿态检测模块、特殊地标进行运动自矫正。

背景技术

当下移动机器人的应用越来越广泛,几乎渗透到了所有领域，尤其是室内移动机器人，越来越能够取代人们完成一些室内的工作。其中移动机器人的定位系统，大都采用二维码作为定位方法，其具体方法是：在移动机器人上面装有一个读取二维码的模块，且在地面上设有很多二维码作为坐标，在移动机器人移动过程中读取二维码坐标，然后进行路径规划，有的二维码获取模块还带有拍摄功能，将二维码拍摄下来，然后用二维码的对边和机器人的前进方向所构成的夹角以及夹角的两条线之间的线性距离作为小车角度偏离的判别标准。但是上述移动机器人存在的问题是：在小车拍摄二维码并获取坐标进行路径规划选择的时候，小车仍然按照惯性会向前走一点，此时小车已经偏离出本该走的路径了，再加上图像处理速度过慢等因素，左右拐控制时，机器人会偏离轨迹，最终无法走到下一个二维码处。在机器人偏离出原轨迹后，需要再矫正回正常轨迹以保证能够读取到后面的二维码，保证机器人运动过程的稳定，这个矫正其实大多是凭借着经验法的，因为一旦脱离了二维码，这个矫正是没有反馈的，是开环的不知道能否矫正成功，这样会给系统带来很大的不稳定性，产生很大误差。

综上所述，目前室内移动机器人技术的问题是惯性运动的矫正难，无法知道是否矫正成功，无法保证能否读取下一个二维码。

发明内容

本发明的目的在于针对市场的需求和现有技术的不足，提供一种移动机器人基于姿态检测模块和特殊地标的运动自矫正方法。该移动机器人可基于模糊PID控制和姿态检测以及特殊地标的识别进行运动的自矫正。

本发明的移动机器人基于姿态检测模块和特殊地标的运动自矫正方法，包括设置特殊地标，并在机器人本体上设置包括控制器、姿态检测模块、特殊地标识别模块、执行电机、电机驱动模块；所述的特殊地标包括含有当地坐标信息的二维码，以及在二维码四边中部垂直向外延伸的四条黑色巡迹线；所述的特殊地标识别模块包括二维码识别模块和光电传感器，二维码识别模块用于识别特殊地标中的二维码坐标，光电传感器用于读取巡迹线，当机器人检测到特殊地标时，利用巡迹线结合模糊PID控制进行位置矫正，当机器人离开特殊地标后，基于姿态检测模块结合常规PID控制进行位置矫正。

上述技术方案中，所述的移动机器人当检测到特殊地标时，读取二维码所含的坐标信息，根据目标位置坐标进行路径规划，路径规划原则以使机器人转向次数最小。

所述的特殊地标识别模块与控制器的串口接受端设为中断模式，当特殊地标识别模块检测到特殊地标时通过串口将地标数据发送给控制器，进入中断进行矫正，否则不进入中断或退出中断。

所述的模糊PID控制，其输入为期望光电传感器检测到的巡迹线的位置，将该位置与光电传感器实际检测的巡迹线位置做偏差，通过模糊PID控制使得最终偏差为零，使得巡迹线保持在移动机器人前进方向的中心位置。

其中路径规划与运动控制部分：

本移动机器人的定位方法为在经过各个特殊地标时读取各个地标中包含的坐标信息，在之后本移动机器人进行路径规划，并根据路径规划结果进行运动控制。其路径规划算法思路如下：设本移动机器人所在的位置坐标为(x₀,y₀)，通过通讯模块得知自己的作业目的地坐标为(x_d,y_d)。则我们设定规则：机器人优先向横坐标靠近再向纵坐标靠近，且当转向时，只能选择左转九十度或者右转九十度，在路径不需要转弯时，移动机器人保持惯性向前。

其过程如下：移动机器人由(x₀,y₀)->(x_d,y₀)->(x_d,y_d)，这样可以使得转向数量最小(一般为1-2次)，减少矫正的工作，使得机器人效率高、稳定。

如果出现多机器人的协作工作下，可以利用通讯模块使得移动机器人之间互相通讯，互相共享下一个目的地坐标，如果出现冲突情况，则其中一个移动机器人停下等待，避免冲突。

其中移动机器人自矫正控制部分：

在移动机器人运动过程中，所以设计的自矫正控制系统主要分为两部分，一部分是利用特殊地标的短循迹线进行矫正，主要对移动机器人在特殊地标附近时进行位置矫正，另一部分是利用姿态模块检测的偏航角进行矫正，主要用于离开地标后的惯性运动下的移动机器人进行运动矫正。

其中在特殊地标附近的矫正是基于地标短循迹线的，使用模糊PID控制：

本矫正控制系统，对于特殊地标附近的矫正：当移动机器人通过光电CCD检测模块检测到了特殊地标时，开始矫正。采用模糊PID进行控制，因为这个过程里，循迹线很短，需要快速的进行控制，且整个过程中分多种情况：如转向后的矫正、刚检测到特殊地标转向前的矫正、检测到特殊地标不用转向的矫正、各种仓库干扰情况下的矫正情况。这些情况要想快速的完成矫正，若用常规的PID控制，就需要不同的P、I、D参数，为了让控制更精简和自适应效果好，采用模糊PID控制。

本模糊PID控制系统的输入是光电传感器的黑色循迹线期望中心位置，将检测到的位置与期望位置的偏差以及其微分输入到模糊PID控制器中，输出为驱动轮差速实现机器人运动，然后经过执行机构(电机)输出即可。

其中模糊PID控制器结构如图5所示,其中e为期望中心点位置的偏差，ec为其微分，ΔV为移动机器人两轮的差速，整个控制器包括一个模糊控制器和一个PID控制器，模糊控制器输出ΔK_P,ΔK_D,ΔK_I给PID控制器进行P、I、D参数整定，再进行控制。其中的PID控制器如图6，PID得到不同的P,I,D参数，进行自适应控制。最终将其特殊地标的中心保持在机器人运动轨迹的中间，保证运动轨迹正确，如图7。

其中的模糊控制器设计如下：

其中模糊控制器如图5，将运动位置与循迹线中心位置的期望偏差和偏差的微分作为输入，P,I,D参数的变化量作为输出。

依据模糊PID控制器的控制规律以及经典PID的控制方法，同时兼顾控制精度。我们将输入的误差(e)和误差微分(ec)分为7个模糊集：NB(负大)，NM(负中)，NS(负小)，ZO(零)，PS(正小)，PM(正中)，PB(正大)。即e,ec的模糊子集为{NB,NM,NS,ZO,PS,PM,PB}。控制器将输出ΔK_P,ΔK_D,ΔK_I分为7个模糊集：NB(负大)，NM(负中)，NS(负小)，ZO(零)，PS(正小)，PM(正中)，PB(正大)。ΔK_P,ΔK_D,ΔK_I的模糊糊子集：{NB,NM,NS,ZO,PS,PM,PB}。

考虑到三角形隶属度函数的简易性，计算快，对于e,ec,ΔK_P,ΔK_D,ΔK_I的隶属度函数都选取三角形隶属度函数。本发明选用Mamdani型的模糊推理办法，且兼顾模糊PID系统的要求，利用重心平均法进行解模糊操作。

应用模糊合成推理PID参数的整定算法。第k个采样时间的整定为(式中K_po,K_do,K_io为经典PID控制器的初始参数)：

K_P(k)＝K_P0+ΔK_P(k) K_I(k)＝K_I0+ΔK_I(k) K_D(k)＝K_D0+ΔK_D(k)

为了便于系统输入，输出出参数映射到论域内。根据工程经验和相关文献，确定模糊化因子为：ke,kec；解模糊因子为：K₁,K₂,K₃。(ke,kec,K₁,K₂,K₃也可根据实际情况修改,在控制框图中不单独列出)即e和ec分别乘上K_e和K_ec后输入模糊控制器，而模糊控制器输出ΔK_P,ΔK_D,ΔK_I也经过处理：K_P＝K_p+K₁*ΔK_P，K_D＝K_D+K₂*ΔK_D，K_I＝K_I+K₃*ΔK_I后输入到PID控制器中。

其模糊控制规则表：

根据常规PID控制的运动经验总结如表1、2、3的模糊规则：

表1

表2

表3

模糊规则的设计主要根据控制经验以及考虑到以下几个原则：

⑴当偏差较大时，为了加快系统的响应速度，并防止开始时偏差的瞬间变大可能引起的微分过饱和而使控制作用超出许可范围，应取较大的K_P和较小的K_D。另外为防止积分饱和，避免系统响应较大的超调，K_I值要小，一般取K_I＝0。

⑵当偏差和变化率为中等大小时，为了使系统响应的超调量减小和保证一定的响应速度，K_P应取小些。在这种情况下K_D的取值对系统影响很大，应取小一些，K_I的取值要适当。

⑶当偏差变化较小时，为了使系统具有较好的稳态性能，应增大K_P、K_I值，同时为避免输出响应在设定值附近振荡，以及考虑系统的抗干扰能力，应适当选取K_D。原则是：当偏差变化率较小时，K_D取大一些；当偏差变化率较大时，K_D取较小的值，通常为中等大小。

其中在远离特殊地标的矫正是基于姿态检测模块的，使用常规PID控制：

当在第一部分矫正完成后，离开特殊地标后，移动机器人的惯性运动矫正则比较简单，利用常规的PID矫正即可，其为当下比较常见的控制模型，其控制器结构如图8：其输入为当下偏航角度与期望偏航角0的偏差，输出为小车差速，在运动控制中，单片机将信息计算结果给驱动模块，驱动控制电机进行差速运动控制，通过控制左右轮转速差来控制移动机器人的运动，保持运动偏航角为0，实现运动矫正。

本发明的方法可以精准地矫正机器人的运动，且成本低，不会有惯性误差。

附图说明

图1特殊地标示意图；

图2移动机器人结构示意图；

图3自矫正控制系统示意图；

图4模糊PID控制系统框图；

图5模糊PID控制器内部结构；

图6模糊PID控制器中的PID控制器内部结构；

图7特殊地标部分在机器人运动矫正位置示意图；

图8PID控制图。

具体实施方式

本发明的移动机器人基于姿态检测模块和特殊地标的运动自矫正方法，包括设置特殊地标，并在机器人本体上设置包括控制器单片机STM32、姿态检测模块MPU9250AHRS、特殊地标识别模块、执行电机、电机驱动模块；所述的特殊地标包括含有当地坐标信息的二维码，以及在二维码四边中部垂直向外延伸的四条黑色巡迹线；所述的特殊地标识别模块包括二维码识别模块HoneywellN5680和光电传感器光电线性CCD传感器TSL1401CL模块，二维码识别模块用于识别特殊地标中的二维码坐标，光电传感器用于读取巡迹线，当机器人检测到特殊地标时，利用巡迹线结合模糊PID控制进行位置矫正，当机器人离开特殊地标后，基于姿态检测模块结合常规PID控制进行位置矫正。

其中路径规划与运动控制部分：

本移动机器人的定位方法为在经过各个特殊地标时读取各个地标中包含的坐标信息，在之后本移动机器人进行路径规划，并根据路径规划结果进行运动控制。其路径规划算法思路如下：设本移动机器人所在的位置坐标为(x₀,y₀)，通过通讯模块得知自己的作业目的地坐标为(x_d,y_d)。则我们设定规则：机器人优先向横坐标靠近再向纵坐标靠近，且当转向时，只能选择左转九十度或者右转九十度，在路径不需要转弯时，移动机器人保持惯性向前。

其过程如下：移动机器人由(x₀,y₀)->(x_d,y₀)->(x_d,y_d)，这样可以使得转向数量最小(一般为1-2次)，减少矫正的工作，使得机器人效率高、稳定。例如：此时机器人位置(0,1),目的地坐标(3，3)，则移动机器人先直走(0，1)->(0,2)->(0,3)，再右转，继续直走(1,3)->(2,3)->(3,3)最终到达目的地。

如果出现多机器人的协作工作下，可以利用通讯模块使得移动机器人之间互相通讯，互相共享下一个目的地坐标，如果出现冲突情况，则其中一个移动机器人停下等待，避免冲突。

其中移动机器人自矫正控制部分：

在移动机器人运动过程中，我们设计的自矫正控制系统主要分为两部分，一部分是利用特殊地标的短循迹线进行矫正，主要对移动机器人在特殊地标附近时进行位置矫正，另一部分是利用姿态模块检测的偏航角进行矫正，主要用于离开地标后的惯性运动下的移动机器人进行运动矫正。整个矫正系统示意图，如图3。

STM32单片机，将移动机器人的特殊地标获取模块的串口接受端设为中断模式，当检测到地标时通过串口将地标数据发送给单片机，进入中断进行第一部分矫正，通过模糊PID进行矫正控制，检测不到则串口里没东西，不进入中断或退出中断。

第一部分矫正控制系统，对于特殊地标附近的矫正：当移动机器人通过光电CCD检测模块检测到了我们特殊地标时，开始矫正。本模糊PID控制系统的输入是光电传感器的黑色循迹线期望中心位置，以现实中光电传感器检测结果为例，其一般是位置在0-125的各点灰度值，其中我们期望粗线中心位置在62处。检测的灰度值最大的点就为黑色循迹线，将检测到的位置与期望位置的偏差以及其微分输入到模糊PID控制器中，输出为驱动轮差速实现机器人运动，然后经过执行机构(电机)输出即可。

其中模糊PID控制器结构如图5所示,其中e为期望中心点位置的偏差，ec为其微分，ΔV为移动机器人两轮的差速，整个控制器包括一个模糊控制器和一个PID控制器，模糊控制器输出ΔK_P,ΔK_D,ΔK_I给PID控制器进行P、I、D参数整定，再进行控制。其中的PID控制器如图6，PID得到不同的P,I,D参数，进行自适应控制。最终将其特殊地标的中心保持在机器人运动轨迹的中间，保证运动轨迹正确，如图7。

其中的模糊控制器设计相关：

其中模糊控制器如图6，将循迹线期望偏差和偏差的微分作为输入，P,I,D参数的变化量作为输出。本控制器为双输入、三输出结构:考虑实际情况和控制经验，设计模糊控制器时，e的论域为[-125,125],ec的论域为[-125,125],ΔK_P,ΔK_D,ΔK_I的论域为[-30,30]。

依据模糊PID控制器的控制规律以及经典PID的控制方法，同时兼顾控制精度。我们将输入的误差(e)和误差微分(ec)分为7个模糊集：NB(负大)，NM(负中)，NS(负小)，ZO(零)，PS(正小)，PM(正中)，PB(正大)。即e,ec的模糊子集为{NB,NM,NS,ZO,PS,PM,PB}。控制器将输出ΔK_P,ΔK_D,ΔK_I分为7个模糊集：NB(负大)，NM(负中)，NS(负小)，ZO(零)，PS(正小)，PM(正中)，PB(正大)。ΔK_P,ΔK_D,ΔK_I的模糊糊子集：{NB,NM,NS,ZO,PS,PM,PB}。

应用模糊合成推理PID参数的整定算法。第k个采样时间的整定为(式中K_po,K_do,K_io为经典PID控制器的初始参数)，在实际问题中我们的K_po,K_do,K_io可以根据情况修改：

K_P(k)＝K_P0+ΔK_P(k)

K_I(k)＝K_I0+ΔK_I(k)

K_D(k)＝K_D0+ΔK_D(k)

为了便于系统输入，输出出参数映射到论域内。根据工程经验和相关文献，确定模糊化因子为：ke＝kec＝0.01；解模糊因子为：K₁＝0.5,K₂＝K₃＝0.01。(ke,kec,K₁,K₂,K₃也可根据实际情况修改,在控制框图中不单独列出)即e和ec分别乘上K_e和K_ec后输入模糊控制器，而模糊控制器输出ΔK_P,ΔK_D,ΔK_I也经过处理：K_P＝K_p+K₁*ΔK_P，K_D＝K_D+K₂*ΔK_D，K_I＝K_I+K₃*ΔK_I后输入到PID控制器中。

最后，考虑到三角形隶属度函数的简易性，计算快，对于e,ec,ΔK_P,ΔK_D,ΔK_I的隶属度函数都选取三角形隶属度函数。

权衡PID控制自身的诸多特点。例如，它的控制规则形式符合人们的思维和语言表达的习惯，控制策略能够方便地表达，控制算法简单等。本处选用的是Mamdani型的模糊推理办法。且兼顾模糊PID系统的要求，此系统利用重心平均法进行解模糊操作。

模糊控制规则表：

根据常规PID控制的运动经验总结如表1、2、3的模糊规则：

表1

表2

表3

其中在远离特殊地标的矫正是基于姿态检测模块的，使用常规PID控制：

当在第一部分矫正完成后，离开特殊地标后，移动机器人的惯性运动矫正则比较简单，利用常规的PID矫正即可，其为当下比较常见的控制模型，其控制器结构如图8：其输入为当下偏航角度与期望偏航角0的偏差，输出为小车差速，在运动控制中，单片机将信息计算结果给驱动模块，驱动控制电机进行差速运动控制，通过控制左右轮转速差来控制移动机器人的运动，保持运动偏航角为0，实现运动矫正。

Claims (3)

1.一种移动机器人基于姿态检测模块和特殊地标的运动自矫正方法，其特征在于，包括设置特殊地标，并在机器人本体上设置包括控制器、姿态检测模块、特殊地标识别模块、执行电机、电机驱动模块；所述的特殊地标包括含有当地坐标信息的二维码，以及在二维码四边中部垂直向外延伸的四条黑色巡迹线；所述的特殊地标识别模块包括二维码识别模块和光电传感器，二维码识别模块用于识别特殊地标中的二维码坐标，光电传感器用于读取巡迹线；

所述的自矫正方法包括两部分：

当机器人检测到特殊地标时，利用巡迹线结合模糊PID控制进行位置矫正，其输入为期望光电传感器检测到的巡迹线的位置，将该位置与光电传感器实际检测的巡迹线位置做偏差，通过模糊PID控制使得最终偏差为零，使得巡迹线保持在移动机器人前进方向的中心位置，输出为驱动轮差速实现机器人运动，然后经执行电机输出；

当机器人离开特殊地标后，基于姿态检测模块结合常规PID控制进行位置矫正，其输入为当下偏航角度与期望偏航角的偏差，输出为小车差速，在运动控制中，姿态检测模块将信息计算结果给电机驱动模块，驱动执行电机进行差速运动控制，通过控制左右轮转速差来控制移动机器人的运动，保持运动偏航角为0，实现运动矫正。

2.根据权利要求1所述的移动机器人基于姿态检测模块和特殊地标的运动自矫正方法，其特征在于，所述的移动机器人当检测到特殊地标时，读取二维码所含的坐标信息，根据目标位置坐标进行路径规划，路径规划原则以使机器人转向次数最小。

3.根据权利要求1所述的移动机器人基于姿态检测模块和特殊地标的运动自矫正方法，其特征在于，所述的特殊地标识别模块与控制器的串口接受端设为中断模式，当特殊地标识别模块检测到特殊地标时通过串口将地标数据发送给控制器，进入中断进行矫正，否则不进入中断或退出中断。