机器人走直线的控制方法和芯片及机器人

摘要

本发明涉及一种机器人走直线的控制方法和芯片及机器人，通过陀螺仪和里程计来确定待行走的直线的初始位置信息和机器人行走过程中具体的当前位置信息，再由初始位置信息和当前位置信息中的角度的差值和当前位置偏离待行走直线的垂直距离，来判断机器人的行走是否偏离了待行走直线。如果出现偏离，则通过调整机器人的驱动轮的速度，使机器人回到待行走直线上。这种机器人走直线的控制方法和芯片，仅需要借助陀螺仪和里程计的检测数据，可以有效地控制机器人保持较好的直线行走效果，成本很低，同时，进行控制的数据运算处理也比较简单，不需要高性能的处理器，进一步减小了系统运算资源和处理器的硬件成本。

说明书

技术领域

本发明涉及机器人领域，具体涉及一种机器人走直线的控制方法和芯片及机器人。

背景技术

现有的机器人在路面行走过程中，由于地面对左右驱动轮的摩擦力不一样，而且路面情况的平坦程度不同，对驱动轮的影响也会有所不同，所以要让机器人保持沿着某一条直线行走，实现起来比较困难，特别是随着行走距离的增大，所产生的偏差也会越大。有的机器人通过采用激光或者视觉等辅助手段对走直线过程中出现的偏差进行纠正，以此保证机器人的直线行走效果。但是，采用这些辅助手段需要价格比较昂贵的激光传感器或者摄像头，导致机器人的成本提高，此外还需要配合比较复杂的算法，对机器人处理器的性能要求也比较高，不适于推广应用。

发明内容

本发明提供了一种机器人走直线的控制方法和芯片及机器人，只需要陀螺仪和里程计的配合就可以纠正机器人走直线的偏差，以较低的成本保证机器人走直线的效果。本发明的具体技术方案如下：

一种机器人走直线的控制方法，包括如下步骤：步骤S1：确定机器人待行走的直线的初始位置为（X1，Y1，θ1）。步骤S2：基于里程计和陀螺仪实时检测的数据，确定机器人的当前位置为（X2，Y2，θ2）。步骤S3：判断θ2是否等于θ1或者所述当前位置距所述直线的垂直距离是否等于0；如果θ2等于θ1，且所述当前位置距所述直线的垂直距离等于0，则回到步骤S2；如果θ2不等于θ1，或者所述当前位置距所述直线的垂直距离不等于0，则进入步骤S4。步骤S4：调整机器人的驱动轮的速度，直到θ2等于θ1，且所述当前位置距所述直线的垂直距离等于0。

进一步地，步骤S3中所述的判断所述当前位置距所述直线的垂直距离是否等于0，包括如下步骤：确定所述初始位置到所述当前位置的直线距离为L，L=。确定所述初始位置与所述当前位置的角度差值为θ，θ=θ1-θ2。确定所述当前位置距所述直线的垂直距离为H，H=L*sinθ。判断H是否等于0。

进一步地，所述步骤S4包括如下步骤：步骤S41：确定机器人的左驱动轮和右驱动轮的控制速度为V。步骤S42：确定机器人的行走误差为T，T=P1*H+P2*（θ1-θ2）；其中，P1和P2为常数；步骤S43：判断θ是否大于0。如果是，则进入步骤S44，如果否，则进入步骤S45。步骤S44：调整右驱动轮的速度为V1，直到θ2等于θ1，且所述当前位置距所述直线的垂直距离等于0；其中，V1的数值为控制速度的数值与行走误差的数值的绝对值之间的差值。步骤S45：调整左驱动轮的速度为V2，直到θ2等于θ1，且所述当前位置距所述直线的垂直距离等于0；其中，V2的数值为控制速度的数值与行走误差的数值的绝对值之间的差值。

进一步地，所述P1=100。所述P2=200。

进一步地，步骤S44中所述的调整右驱动轮的速度为V1，包括如下步骤：确定t-2时刻，所述右驱动轮的控制速度和采样速度之间的速度偏差为E（t-2）。确定t-1时刻，所述右驱动轮的控制速度和采样速度之间的速度偏差为E（t-1）。确定t时刻，所述右驱动轮的控制速度和采样速度之间的速度偏差为E（t）。确定PID控制的比例值为P，积分值为I，微分值为D。则t时刻，输出至右驱动轮以调整右驱动轮的速度为V1的电压值为U（t），U（t）=P*[ E（t）-E（t-1）]+I* E（t）+D*[ E（t）-2* E（t-1）+ E（t-2）]。

进一步地，步骤S45中所述的调整左驱动轮的速度为V2，包括如下步骤：确定t-2时刻，所述左驱动轮的控制速度和采样速度之间的速度偏差为E（t-2）。确定t-1时刻，所述左驱动轮的控制速度和采样速度之间的速度偏差为E（t-1）。确定t时刻，所述左驱动轮的控制速度和采样速度之间的速度偏差为E（t）。确定PID控制的比例值为P，积分值为I，微分值为D。则t时刻，输出至左驱动轮以调整左驱动轮的速度为V2的电压值为U（t），U（t）=P*[ E（t）-E（t-1）]+I* E（t）+D*[ E（t）-2* E（t-1）+ E（t-2）]。

进一步地，所述P=0.4。所述I=0.8。所述D=0.5。

进一步地，所述确定PID控制的比例值为P，积分值为I，微分值为D，包括如下步骤：先将积分值和微分值设置为0，由小到大逐步改变比例值，当输出的电压值满足第一预设条件，则确定当前的比例值为P。再将比例值设置为P，微分值设置为0，由小到大逐步改变积分值，当输出的电压值满足第二预设条件，则确定当前的积分值为I。最后将比例值设置为P，积分值设置为I，由小到大逐步改变微分值，当输出的电压值满足第三预设条件，则确定当前的微分值为D。

一种芯片，用于存储程序，所述程序用于控制机器人执行上述的机器人走直线的控制方法。

一种机器人，包括控制芯片，所述控制芯片为上述的芯片。

本发明的有益效果在于：通过陀螺仪和里程计来确定待行走的直线的初始位置信息和机器人行走过程中具体的当前位置信息，再由初始位置信息和当前位置信息中的角度的差值和当前位置偏离待行走直线的垂直距离，来判断机器人的行走是否偏离了待行走直线。如果出现偏离，则通过调整机器人的驱动轮的速度，使机器人回到待行走直线上。这种机器人走直线的控制方法和芯片，仅需要借助陀螺仪和里程计的检测数据，可以有效地控制机器人保持较好的直线行走效果，成本很低，同时，进行控制的数据运算处理也比较简单，不需要高性能的处理器，进一步减小了系统运算资源和处理器的硬件成本。

附图说明

图1为本发明所述的机器人走直线的控制方法的流程图。

图2为本发明所述的机器人走直线的分析示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行详细描述。应当理解，下面所描述的具体实施例仅用于解释本发明，并不用于限定本发明。

本发明所述的机器人可以是一种智能移动家用电器，比如清洁机器人或者监控机器人等，也可以是一种商用的智能移动设备，比如餐厅服务机器人或者大堂服务机器人等。这些机器人能凭借一定的人工智能，自动在某些场合自动进行行走。机器人的机体上设有各种传感器，可检测行走距离、行走角度、机体状态和障碍物等，如碰到墙壁或其他障碍物，会自行转弯，并依不同的设定，而走不同的路线，有规划地行走，还会根据行走过程中检测到的各种数据构建栅格地图。本发明所述的机器人包括如下结构：带有左驱动轮和右驱动轮的能够自主行走的机器人机体，机体上设有人机交互界面，机体上设有障碍检测单元。机体内部设置有惯性传感器，所述惯性传感器包括加速度计和陀螺仪等，两个驱动轮上都设有用于检测驱动轮的行走距离的里程计（一般是码盘），还设有能够处理相关传感器的参数，并能够输出控制信号到执行部件的控制模块。

本发明所述的机器人走直线的控制方法，如图1所示，包括如下步骤：步骤S1：确定机器人待行走的直线的初始位置为（X1，Y1，θ1）；步骤S2：基于里程计和陀螺仪实时检测的数据，确定机器人的当前位置为（X2，Y2，θ2）；步骤S3：判断θ2是否等于θ1或者所述当前位置距所述直线的垂直距离是否等于0；如果θ2等于θ1，且所述当前位置距所述直线的垂直距离等于0，则回到步骤S2；如果θ2不等于θ1，或者所述当前位置距所述直线的垂直距离不等于0，则进入步骤S4；步骤S4：调整机器人的驱动轮的速度，直到θ2等于θ1，且所述当前位置距所述直线的垂直距离等于0。如图2所示的XY轴坐标系中，直线AB为机器人待行走的直线，A点为该直线的初始位置，坐标为（X1，Y1）。从A指向B的方向为机器人想要沿该直线行走的方向，用θ1表示该直线的方向，即直线AB与竖直线的夹角。C点为机器人当前的位置，坐标为（X2，Y2），从A指向C的方向为机器人的当前方向（即陀螺仪在C点时检测到的角度值），用θ2表示当前方向，即直线AC与竖直线的夹角。直线CD为垂直于直线AB的线段，该线段的长度表示机器人偏离直线AB的垂直距离。所述方法通过陀螺仪和里程计来确定待行走的直线的初始位置信息（X1，Y1，θ1）和机器人行走过程中具体的当前位置信息（X2，Y2，θ2），再由初始位置信息中的角度θ1与当前位置信息中的角度θ2的差值以及当前位置偏离待行走直线的垂直距离CD，来判断机器人的行走是否偏离了待行走直线。如果出现偏离，则通过调整机器人的驱动轮的速度，使机器人回到待行走直线AB上。这种机器人走直线的控制方法，仅需要借助陀螺仪和里程计的检测数据，就可以判断机器人行走是否出现偏差，并根据偏差的数值可以有效地控制机器人纠正偏差，从而保持较好的直线行走效果，成本相对较低。同时，关于偏差的检测和纠正的相关数据比较简明，数据运算处理也比较简易，不需要高性能的处理器，进一步减小了系统运算资源和处理器的硬件成本。

作为其中一种实施方式，步骤S3中所述的判断所述当前位置距所述直线的垂直距离是否等于0，包括如下步骤：确定所述初始位置到所述当前位置的直线距离为L，L=；确定所述初始位置与所述当前位置的角度差值为θ，θ=θ1-θ2；确定所述当前位置距所述直线的垂直距离为H，H=L*sinθ；判断H是否等于0。如图2所示，由于初始位置A点的坐标为（X1，Y1），当前位置C点的坐标为（X2，Y2），所以，可以先求得AC之间的距离为。然后再通过AB的角度θ1和AC的角度θ2来确定AB与AC之间的角度为θ1-θ2。接着就可以确定机器人在C点时距直线AB的垂直距离CD为*sin（θ1-θ2）。最后机器人就可以通过计算CD的值是否等于0，来判断当前位置距所述直线的垂直距离是否等于0了。通过这种判断当前位置距直线的垂直距离的方法，需要检测和处理的数据不多，机器人可以快速简易地得知自己是否偏离待行走的直线以及具体的偏离数值，还能为后续的偏差纠正提供参考数据，十分高效。

作为其中一种实施方式，所述步骤S4包括如下步骤：步骤S41：确定机器人的左驱动轮和右驱动轮的控制速度为V；步骤S42：确定机器人的行走误差为T，T=P1*H+P2*（θ1-θ2），其中，P1和P2为常数；步骤S43：判断θ是否大于0；如果是，则进入步骤S44，如果否，则进入步骤S45；步骤S44：调整右驱动轮的速度为V1，直到θ2等于θ1，且所述当前位置距所述直线的垂直距离等于0，其中，V1的数值为控制速度的数值与行走误差的数值的绝对值之间的差值；步骤S45：调整左驱动轮的速度为V2，直到θ2等于θ1，且所述当前位置距所述直线的垂直距离等于0，其中，V2的数值为控制速度的数值与行走误差的数值的绝对值之间的差值。在机器人对行走偏差进行纠正时，需要先确定机器人的行走误差，而影响行走误差的因素主要为当前位置距所述直线的垂直距离和当前位置与初始位置的角度差值，分别对这两个因素乘以适当的误差系数P1和P2，就可以得出行走误差的值。其中，误差系数P1和P2是通过对机器人进行有限次的实验、测试和总结而得到的，分别为一个常数。在确定了行走误差的值之后，还需要判断机器人到达是往所述直线的哪一侧偏离了。如图2所示，以Y轴方向为参考方向，向Y轴右侧偏移的方向角度为正角度，向Y轴左侧偏移的方向角度为负角度。由于AB和AC都是往Y轴的右侧偏移，所以，θ1和θ2都为正角度值，当θ1和θ2的差值小于0，表明θ1小于θ2，当前位置C点位于直线AB的右侧，机器人向直线的右侧偏离。同理，当θ1和θ2的差值大于0，表明θ1大于θ2，当前位置C点位于直线AB的左侧，机器人向直线的左侧偏离。所以，通过比较分析θ1和θ2的差值，就可以得出机器人相对于待行走直线AB的偏离方向。最后，再根据偏离的方向调整不同的驱动轮的速度，可以使机器人回到待行走直线AB上。理论上，机器人想要沿直线行走，输出到两个驱动轮的控制速度应该是相同的，但是，由于驱动轮打滑、越障等原因，会导致机器人的左驱动轮和右驱动轮的实际行走速度出现偏差，从而导致机器人会向待行走直线的一侧偏离。如果机器人向待行走直线AB的右侧偏离时，为了尽量保持机器人当前的行走速度，所以，需要降低左驱动轮的行走速度，这样机器人就会逐渐向左转向，当机器人判断θ2等于θ1，且所述当前位置距所述直线的垂直距离等于0时，机器人就已经回到了待行走直线AB上，然后机器人就会保持左驱动轮和右驱动轮的控制速度一致，继续沿待行走直线AB方向继续行走。其中，机器人在降低左驱动轮的行走速度时，需要调整左驱动轮的速度为控制速度的数值与行走误差的数值的绝对值之间的差值，这样，机器人就可以随着行走误差的减小而不断地调整输出的控制速度，最终当行走误差为0时，机器人调整输出的速度恢复到沿待行走直线AB行走时的控制速度，从而保持左驱动轮的速度和右驱动轮的速度一致，机器人继续沿待行走直线AB方向继续行走。同理，如果机器人向待行走直线AB的左侧偏离时，需要降低右驱动轮的行走速度，这样机器人就会逐渐向右转向，具体调整方法与上述左驱动轮速度的调整方法相同，在此不再赘述。本实施例所述的方法需要检测和调整的参数比较少，控制简易，大大地减少了机器人系统的数据处理资源，适于推广应用。

作为其中一种实施方式，所述P1=100，所述P2=200。这两个数值是针对一般的室内扫地机器人在35cm/s左右的速度下走直线时的最佳数值，能达到相对较好的走直线效果。

作为其中一种实施方式，步骤S44中所述的调整右驱动轮的速度为V1，包括如下步骤：确定t-2时刻，所述右驱动轮的控制速度和采样速度之间的速度偏差为E（t-2）；确定t-1时刻，所述右驱动轮的控制速度和采样速度之间的速度偏差为E（t-1）；确定t时刻，所述右驱动轮的控制速度和采样速度之间的速度偏差为E（t）；确定PID控制的比例值为P，积分值为I，微分值为D；则t时刻，输出至右驱动轮以调整右驱动轮的速度为V1的电压值为U（t），U（t）=P*[ E（t）- E（t-1）]+I* E（t）+D*[ E（t）-2* E（t-1）+ E（t-2）]。由于，机器人要调整右驱动轮的速度，就需要向右驱动轮的电机输入不同的PWM驱动信号，所以，基于PID控制算法，为了达到较好的调整效果，需要预先获取当前时刻和邻近当前时刻的前两个时刻的机器人的右驱动轮的控制速度和采样速度。比如，获取到当前时刻和当前时刻前1秒以及当前时刻前2秒的控制速度和采样速度。其中，控制速度是系统输出数据，而采样速度则是通过里程计检测到的数据。由于外界环境的影响，实际的采样速度与系统输出的控制速度是有所偏差的。所以，在确定了不同时刻两者的误差数据之后，再结合PID的数值，就可以准确的得到输出至右驱动轮以调整右驱动轮的速度的电压值。其中，P、I和D的值是通过对机器人进行有限次的实验、测试和总结而得到的，分别为一个常数。本实施例所述的方法，通过PID控制算法，可以准确得到输出至右驱动轮的电机的电压值，控制精度高，控制效果较好。

作为其中一种实施方式，步骤S45中所述的调整左驱动轮的速度为V2，包括如下步骤：确定t-2时刻，所述左驱动轮的控制速度和采样速度之间的速度偏差为E（t-2）；确定t-1时刻，所述左驱动轮的控制速度和采样速度之间的速度偏差为E（t-1）；确定t时刻，所述左驱动轮的控制速度和采样速度之间的速度偏差为E（t）；确定PID控制的比例值为P，积分值为I，微分值为D；则t时刻，输出至左驱动轮以调整左驱动轮的速度为V2的电压值为U（t），U（t）=P*[ E（t）- E（t-1）]+I* E（t）+D*[ E（t）-2* E（t-1）+ E（t-2）]。由于，机器人要调整左驱动轮的速度，就需要向左驱动轮的电机输入不同的PWM驱动信号，所以，基于PID控制算法，为了达到较好的调整效果，需要预先获取当前时刻和邻近当前时刻的前两个时刻的机器人的左驱动轮的控制速度和采样速度。比如，获取到当前时刻和当前时刻前1秒以及当前时刻前2秒的控制速度和采样速度。其中，控制速度是系统输出数据，而采样速度则是通过里程计检测到的数据。由于外界环境的影响，实际的采样速度与系统输出的控制速度是有所偏差的。所以，在确定了不同时刻两者的误差数据之后，再结合PID的数值，就可以准确的得到输出至左驱动轮以调整左驱动轮的速度的电压值。其中，P、I和D的值是通过对机器人进行有限次的实验、测试和总结而得到的，分别为一个常数。本实施例所述的方法，通过PID控制算法，可以准确得到输出至左驱动轮的电机的电压值，控制精度高，控制效果较好。

作为其中一种实施方式，所述P=0.4，所述I=0.8，所述D=0.5。这三个数值是针对一般的室内扫地机器人在35cm/s左右的速度下走直线时的最佳数值，能达到相对较好的控制机器人走直线的效果。

作为其中一种实施方式，所述确定PID控制的比例值为P，积分值为I，微分值为D，包括如下步骤：将积分值和微分值设置为0，由小到大逐步改变比例值，当输出的电压值满足第一预设条件，则确定当前的比例值为P；将比例值设置为P，微分值设置为0，由小到大逐步改变积分值，当输出的电压值满足第二预设条件，则确定当前的积分值为I；将比例值设置为P，积分值设置为I，由小到大逐步改变微分值，当输出的电压值满足第三预设条件，则确定当前的微分值为D。所述方法将P、I和D逐个进行实验测试和调整，先将积分值和微分值设置为0，由小到大逐步改变比例值，并观察输出的电压波形，当输出的电压波形与理想的电压波形误差较小（即当输出的电压值满足第一预设条件）时，则把当前的比例值确定为适合的数值。然后再将比例值设置为所确定的适合的数值，微分值设置为0，由小到大逐步改变积分值，并观察输出的电压波形，当输出的电压波形与理想的电压波形误差较小（即当输出的电压值满足第二预设条件）时，则把当前的积分值确定为适合的数值。最后，将比例值设置为所确定的合适的数值，积分值也设置为所确定的合适的数值，由小到大逐步改变微分值，并观察输出的电压波形，当输出的电压波形与理想的电压波形误差较小（即当输出的电压值满足第三预设条件）时，则把当前的微分值确定为合适的数值。这些所确定的合适的数值都是常数值，可以较好地调整系统输出的电压值。本实施例所述的方法，可以较快较准确地得出所需要的P、I和D的数值，操作简单，得出的数值也比较准确和有效。

本领域普通技术人员可以理解：实现上述各方法实施例的全部或部分步骤可以通过程序指令相关的硬件来完成。这些程序可以存储于计算机可读取存储介质（比如ROM、RAM、磁碟或者光盘等各种可以存储程序代码的介质）中。该程序在执行时，执行包括上述各方法实施例的步骤。

一种芯片，用于存储程序，所述程序用于控制机器人执行上述的机器人走直线的控制方法。所述芯片通过陀螺仪和里程计来确定待行走的直线的初始位置信息（X1，Y1，θ1）和机器人行走过程中具体的当前位置信息（X2，Y2，θ2），再由初始位置信息中的角度θ1与当前位置信息中的角度θ2的差值以及当前位置偏离待行走直线的垂直距离CD，来判断机器人的行走是否偏离了待行走直线。如果出现偏离，则通过调整机器人的驱动轮的速度，使机器人回到待行走直线AB上。

装配所述芯片作为控制芯片的机器人，仅需要借助陀螺仪和里程计的检测数据，就可以判断机器人行走是否出现偏差，并根据偏差的数值可以有效地控制机器人纠正偏差，从而保持较好的直线行走效果，成本相对较低。同时，关于偏差的检测和纠正的相关数据比较简明，数据运算处理也比较简易，不需要高性能的处理器，进一步减小了系统运算资源和机器人的硬件成本。

最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。