一种控制双轮机器人直线行走的方法和系统与流程

本发明涉及双轮机器人控制策略领域，具体涉及一种控制双轮机器人直线行走的方法和系统。

背景技术：

移动机器人因为在各行各业有着广阔的应用前景，已经成为机器人研究领域的一个重要分支。按其移动方式分类，有论事、履带式、腿式、推进式等，其中轮式移动机器人具有成本低、结构和控制简单、能量利用率高等特点而得到广泛应用。双轮机器人是一种特殊的轮式移动机器人，可以自我平衡，拓展了它适应地形变化的能力，运动灵活、行驶速度和倾斜角度可以调节。双轮机器人的左右轮分别由一个电机驱动,依靠差速实现转向,转向灵活。

随着轮式机器人技术的不断发展，两轮呈左右对称布置的两轮移动机器人被广泛的应用，其优点是：左右轮分别由一个电机驱动,依靠差速实现转向,转向灵活。

双轮自平衡机器人在运动过程中除了要解决平衡问题，还要解决双轮机器人的机械系统存在的误差，因为在双轮机器人一直运行的情况下，双轮机器人的积累误差会越来越明显，从而使机器人无法直线行进。尤其在负重不同的情况下，需要根据不同负重自动采取不同的控制策略，否则容易造成不同负重情况下过调或者欠调导致的无法直线行走问题。

技术实现要素：

本发明提供了一种控制双轮机器人直线行走的方法和系统，以解决现有的双轮机器人机械系统误差以及不同负重导致的双轮机器人无法直线行走等问题。

根据本发明的一个方面，提供了一种控制双轮机器人直线行走的方法，所述双轮机器人内设置有压力传感器、陀螺仪和左右电机，所述方法包括：利用所述陀螺仪获取所述双轮机器人绕z轴旋转的角速度值，并根据所述角速度值计算所述双轮机器人绕z轴旋转的角度值；

通过所述压力传感器获取所述双轮机器人的负重值，并根据所述负重值计算所述双轮机器人进行pd直线控制的比例控制参数p值和微分控制参数d值；

将所述双轮机器人绕z轴旋转的角度值与预存的零角度目标值之差进行所述p值的比例控制，将所述双轮机器人绕z轴旋转的角速度值进行所述d值的微分控制，计算所述双轮机器人进行pd直线控制的输出量；

将所述输出量输出至所述左右电机，根据所述输出量控制所述左右电机的步进数，以实现对所述双轮机器人直线行走的控制。

根据本发明的另一个方面，提供了一种控制双轮机器人直线行走的系统，所述系统包括陀螺仪、压力传感器、左右电机和处理器；

所述陀螺仪，用于获取所述双轮机器人的绕z轴旋转的角速度值，并将获取到的所述角速度值发送给所述处理器；

所述压力传感器，用于获取所述双轮机器人的负重值，并将所述负重值发送至所述处理器；

所述处理器，用于接收来自所述陀螺仪的角速度值，根据所述角速度值计算双轮机器人绕z轴旋转的角度值；以及用于根据来自所述压力传感器的负重值计算所述双轮机器人进行pd直线控制的比例控制参数p值和微分控制参数d值；将所述双轮机器人绕z轴旋转的角度值与预存的零角度目标值之差进行所述p值的比例控制，将所述双轮机器人绕z轴旋转的角速度值进行所述d值的微分控制，计算所述双轮机器人进行pd直线控制的输出量；并将所述输出量输出至所述左右电机；

所述左右电机，用于根据来自所述处理器的输出量控制自身的步进数，实现双轮机器人的直线行走。

本发明的有益效果是：本发明的技术方案实现了一种双轮机器人直线行走的控制策略，利用陀螺仪获取双轮机器人绕z轴旋转的角速度数据，并根据角速度值计算双轮机器人绕z轴旋转的角度值；通过压力传感器测得双轮机器人的外加负重大小，并根据外加负重大小计算双轮机器人进行pd直线控制的比例控制参数p值和微分控制参数d值以优化pd直线控制；将双轮机器人绕z轴旋转的角度值与预存的零角度目标值之差进行p值的比例控制，将双轮机器人绕z轴旋转的角速度值进行d值的微分控制，计算双轮机器人进行pd直线控制的输出量；将输出量输出至左右电机，根据输出量控制左右电机的步进数，消除了双轮机器人机械系统中存在的误差，并使得双轮机器人可以在不同负重情况下直线行走。

附图说明

图1是本发明一个实施例的一种控制双轮机器人直线行走的方法的流程图；

图2是本发明一个实施例的一种控制双轮机器人直线行走的平滑处理过程的示意图；

图3是本发明一个实施例的一种控制控制双轮机器人直线行走的系统的示意图。

具体实施方式

本发明的设计构思是：为了消除了双轮机器人机械系统中存在的误差，并使得双轮机器人可以在不同负重情况下直线行走，利用陀螺仪获取双轮机器人绕z轴旋转的角速度数据，并计算双轮机器人绕z轴旋转的角度值；通过压力传感器测得双轮机器人的外加负重大小，自动调节双轮机器人进行pd直线控制的比例控制参数p值和微分控制参数d值；将双轮机器人绕z轴旋转的角度值与预存的零角度目标值之差进行p值的比例控制，将双轮机器人绕z轴旋转的角速度值进行d值的微分控制，计算双轮机器人进行pd直线控制的输出量；将输出量输出至左右电机，控制左右电机的步进数。

实施例一

图1是本发明一个实施例的一种控制双轮机器人直线行走的方法的流程图，如图1所示，一种控制双轮机器人直线行走的方法，所述双轮机器人内设置有压力传感器、陀螺仪和左右电机，所述方法包括：

步骤s110，利用所述陀螺仪获取所述双轮机器人绕z轴旋转的角速度值，并根据所述角速度值计算所述双轮机器人绕z轴旋转的角度值；

在本发明的一个实施例中，陀螺仪为6轴角速度感测器(mpu6050)。mpu6050内部嵌入了一个运动控制引擎(dmp)，通过使用dmp提供的驱动库，可以直接得到四元数，利用输出的四元数来计算双轮机器人绕z轴旋转的角度值。利用mpu6050内部的运动引擎直接输出四元数，减轻了外围微处理器的工作负担且避免了繁琐的滤波和数据融合。

步骤s120，通过所述压力传感器获取所述双轮机器人的负重值，并根据所述负重值计算所述双轮机器人进行pd直线控制的比例控制参数p值和微分控制参数d值；

由于单一的控制参数无法满足不同的负重情况下的调节需求，即较小的负重情况下的调节相对于较大负重情况下的调节过小，无法很好的调节到直线的效果；而较大负重情况下的调节参数相对于较小负重情况来说调节力度过大，容易造成过调。因此需要根据负重情况自动调整控制策略，即调整pd直线控制参数。在本发明的一个实施例中，当负重不为零时，为了简化计算减少系统运算资源。根据公式pout＝kp*weight+p0计算p值，其中，pout为双轮机器人适应当前负重的p值，kp为根据双轮机器人的结构和本身重量通过试验标定的系数，weight为所述压力传感器获得的当前所述双轮机器人的负重值，p0为双轮机器人的负重为零时的p值。

根据公式dout＝kd*weight+d0计算d值，其中，dout为双轮机器人适应当前负重的d值，kd为根据双轮机器人的结构和本身重量通过试验标定的系数，weight为所述压力传感器获得的当前所述双轮机器人的负重值，d0为双轮机器人的负重为零时的d值。

需要说明的是，p值和d值这两个参数有正负、大小之分，可以通过调试得到负重为零时的最佳参数值p0、d0。通过上述这种方法，实现了根据不同负重情况自动调整pd直线控制参数，进而保证了双轮机器人在任何负重情况下都可以直线行走。

步骤s130，将所述双轮机器人绕z轴旋转的角度值与预存的零角度目标值之差进行所述p值的比例控制，将所述双轮机器人绕z轴旋转的角速度值进行所述d值的微分控制，计算所述双轮机器人进行pd直线控制的输出量；

在本发明的一个实施例中，由于当对双轮机器人进行pd直线控制时，只要使双轮机器人绕z轴旋转的角度为0，则双轮机器人一直直线行走，因此将绕z轴旋转的角度与设定的角度目标值之差进行p比例控制。为了保持双轮机器人行进时走直线，所以将角度目标值设置为0，另外，将绕z轴旋转的角速度进行d微分控制，微分控制可以实现对干扰做出迅速的响应。

根据公式straightcontrolout＝p*(yaw-car_zero)+d*gyro[2]计算进行pd直线控制的输出量，其中，straightcontrolout为pd直线控制的输出量；yaw为所述双轮机器人绕z轴旋转的角度值；car_zero_angle为预存的零角度目标值；gyro[2]为陀螺仪测得的双轮机器人在行走过程中绕z轴旋转的角速度值；p为根据所述负重值计算得到的比例控制参数值，d为根据所述负重值计算得到的微分控制参数值。

步骤s140，将所述输出量输出至所述左右电机，根据所述输出量控制所述左右电机的步进数，以实现对所述双轮机器人直线行走的控制。

通过图1所示的方法可知，本发明的技术方案利用陀螺仪获取双轮机器人绕z轴旋转的角速度数据，并根据角速度值计算双轮机器人绕z轴旋转的角度值；通过压力传感器测得双轮机器人的外加负重大小，并根据外加负重大小计算双轮机器人进行pd直线控制的比例控制参数p值和微分控制参数d值以优化pd直线控制；将双轮机器人绕z轴旋转的角度值与预存的零角度目标值之差进行p值的比例控制，将双轮机器人绕z轴旋转的角速度值进行d值的微分控制，计算双轮机器人进行pd直线控制的输出量；将输出量输出至左右电机，根据输出量控制左右电机的步进数，消除了双轮机器人机械系统中存在的误差，并使得双轮机器人可以在不同负重情况下直线行走。

当双轮机器人行走在不平坦路面时，导致左右轮的负重不同，为了使得双轮机器人在不平坦里面依旧能够直线行走。在本发明的一个实施例中，当双轮机器人内设置有两个压力传感器时，将两个压力传感器分别对应设置在双轮机器人的左右轮。

利用步骤s110中的方法获取双轮机器人绕z轴旋转的角速度值和角度值；

然后通过两个压力传感器分别获取双轮机器人的左右轮的负重值weight1和weight2，并根据左右轮的负重值分别计算左右轮进行pd直线控制的比例控制参数p1＝kp*weight1+p0、p2＝kp*weight2+p0和微分控制参数d1＝kd*weight1+d0、d2＝kd*weight2+d0；

对于双轮机器人的左轮，分别将双轮机器人绕z轴旋转的角度值与预存的零角度目标值之差进行对应p1值的比例控制，将双轮机器人绕z轴旋转的角速度值进行对应d1值的微分控制，则双轮机器人左轮的pd直线控制的输出量为：

straightcontrolout1＝p1*(yaw-car_zero)+d1*gyro[2]

同理则双轮机器人右轮的pd直线控制的输出量为：

straightcontrolout2＝p2*(yaw-car_zero)+d2*gyro[2]

将straightcontrolout1输出至双轮机器人的左轮，控制左电机的步进数，将straightcontrolout2输出至双轮机器人的右轮，控制右电机的步进数，实现对双轮机器人直线行走的控制。

图2是本发明一个实施例的一种控制双轮机器人直线行走的平滑处理过程的示意图，如图2所示，

步骤s210，根据负重值计算直线控制的p值和d值。采用步骤s120中的方法计算双轮机器人负重时p值和d值的实际有效值pou和dout。

步骤s220，计算负重前后p值和d值的差值。根据公式δp＝pout-p0计算所述双轮机器人负重前后进行pd直线控制的p值的差值；其中，δp为所述双轮机器人负重前后进行pd直线控制的p值的差值，pout为双轮机器人适应当前负重的p值；p0为双轮机器人的负重为零时的p值；同理，根据公式δd＝dout-d0计算所述双轮机器人负重前后进行pd直线控制的d值的差值。

步骤s230，将负重前后p和d值的差值与最佳步进值的比值累加到p0值。

根据公式p＝p0+δp/stepp，将所述双轮机器人负重前后进行pd直线控制的p值的差值与预存的相应负重时左右电机的最佳步进数的比值累加到双轮机器人的负重为零时的p值，得到平滑处理后的p值；其中，p为平滑处理后的p值，p0为双轮机器人的负重为零时的p值,δp为所述双轮机器人负重前后进行pd直线控制的p值的差值，stepp为预存的相应负重时左右电机的最佳步进数；同理，根据公式d＝d0+δd/stepd计算平滑处理后的d值。

步骤s240，平滑处理后pd直线控制输出量计算。

根据公式bst-fstraightcontrolout＝p*(yaw-car_zero)+d*gyro[2]计算平滑处理后的直线控制输出量。

步骤s250，输出平滑后的直线控制量。将平滑处理后的直线控制量输出至双轮机器人的左右电机，控制双轮机器人左右电机的步进数。

步骤s250，判断平滑后的直线控制量对应的步进数是否达到最佳步进数的上限？也就是说，判断根据平滑后的输出量控制得到的左右电机的步进数是否达到预存的相应负重时左右电机的最佳步进数。需要说明的是，最佳步进数是根据实际试验测得的在相应负重情况下双轮机器人直线行走时步进电机的步进数，最佳步进数是预存在控制系统中的。

步骤s260，若判断为是，停止p值和d值的累加。若判断为否，则重复上述步骤s220到步骤s260的操作。

为了使本方案清楚，以p值的平滑处理为例，若p0＝5，stepp＝10，pou＝15，则计算出来的δp＝pou-p0＝15-5＝10，则p＝p0+δp/stepp＝5+10/10＝6。以d值的平滑处理为例，若d0＝5，stepp＝10，dou＝25，则计算出来的δd＝dout-d0＝25-5＝20，则d＝d0+δd/stepd＝5+20/10＝7；由此可见，平滑处理后的p值是按照一定的调用周期根据步进数为1的计数机制进行平滑；平滑处理后的d值是按照一定的调用周期根据步进数为2的计数机制进行平滑；由于p值和d值是渐增的，则bst_fstraightcontrolout也是渐增的，则每一次叠加到电机上的输入量是缓慢增加的，直到达到最佳步进数的上限停止p值和d值的平滑处理。利用渐增的bst_fstraightcontrolout来取代突变的直线控制量straightcontrolout叠加到电机上，优化了双轮机器人的直线行走控制，并有效缓解了负重突然增大时导致的瞬间卡顿问题。

实施例二

图3是本发明一个实施例的一种控制控制双轮机器人直线行走的系统的示意图，如图3所示，一种控制双轮机器人直线行走的系统300，所述系统300包括陀螺仪320、压力传感器310、左右电机340和处理器330；

所述陀螺仪320，用于获取所述双轮机器人的绕z轴旋转的角速度值，并将获取到的所述角速度值发送给所述处理器；

所述压力传感器310，用于获取所述双轮机器人的负重值，并将所述负重值发送至所述处理器330；

所述处理器330，用于接收来自所述陀螺仪的角速度值，根据所述角速度值计算双轮机器人绕z轴旋转的角度值；以及用于根据来自所述压力传感器310的负重值计算所述双轮机器人进行pd直线控制的比例控制参数p值和微分控制参数d值；将所述双轮机器人绕z轴旋转的角度值与预存的零角度目标值之差进行所述p值的比例控制，将所述双轮机器人绕z轴旋转的角速度值进行所述d值的微分控制，计算所述双轮机器人进行pd直线控制的输出量；并将所述输出量输出至所述左右电机340；

所述左右电机340，用于根据来自所述处理器330的输出量控制自身的步进数，实现双轮机器人的直线行走。

通过图3所示的方法可知，本发明的技术方案利用陀螺仪320获取双轮机器人绕z轴旋转的角速度数据，并根据角速度值计算双轮机器人绕z轴旋转的角度值；通过压力传感器310测得双轮机器人的外加负重大小，并根据外加负重大小计算双轮机器人进行pd直线控制的比例控制参数p值和微分控制参数d值以优化pd直线控制；将双轮机器人绕z轴旋转的角度值与预存的零角度目标值之差进行p值的比例控制，将双轮机器人绕z轴旋转的角速度值进行d值的微分控制，计算双轮机器人进行pd直线控制的输出量；将输出量输出至左右电机340，根据输出量控制左右电机340的步进数，消除了双轮机器人机械系统中存在的误差，并使得双轮机器人可以在不同负重情况下直线行走。

当双轮机器人行走在不平坦路面时，导致左右轮的负重不同，为了使得双轮机器人在不平坦里面依旧能够直线行走。在本发明的一个实施例中，所述压力传感器310的数量为两个，分别对应所述双轮机器人的左右轮设置；

所述两个压力传感器310，用于分别获取所述双轮机器人的左右轮的负重值，并将获取的负重值分别传送至所述处理器330，

所述处理器330，具体用于根据来自所述两个压力传感器310的左右轮的负重值分别计算左右轮进行pd直线控制的比例控制参数p值和微分控制参数d值；对于所述双轮机器人的左右轮340，分别将所述双轮机器人绕z轴旋转的角度值与预存的零角度目标值之差进行对应所述p值的比例控制，将所述双轮机器人绕z轴旋转的角速度值进行对应所述d值的微分控制，分别计算所述双轮机器人左右轮340的pd直线控制的输出量；并将对应左右轮的两个所述输出量分别输出至所述左右电机340；

所述左右电机340，用于根据对应的所述输出量分别控制自身的步进数，实现所述双轮机器人的直线行走。

在本发明的一个实施例中，由于单一的控制参数无法满足不同的负重情况下的调节需求，即较小的负重情况下的调节相对于较大负重情况下的调节过小，无法很好的调节到直线的效果；而较大负重情况下的调节参数相对于较小负重情况来说调节力度过大，容易造成过调。因此需要根据负重情况自动调整控制策略，即调整pd直线控制参数。

所述处理器330，具体用于根据公式pout＝kp*weight+p0计算p值，其中，pout为双轮机器人适应当前负重的p值，kp为根据双轮机器人的结构和本身重量通过试验标定的系数，weight为所述压力传感器获得的当前所述双轮机器人的负重值，p0为双轮机器人的负重为零时的p值；

根据公式dout＝kd*weight+d0计算d值，其中，dout为双轮机器人适应当前负重的d值，kd为根据双轮机器人的结构和本身重量通过试验标定的系数，weight为所述压力传感器获得的当前所述双轮机器人的负重值，d0为双轮机器人的负重为零时的d值；

以及，

所述处理器330，具体用于根据公式

straightcontrolout＝p*(yaw-car_zero)+d*gyro[2]计算进行pd直线控制的输出量，其中，straightcontrolout为pd直线控制的输出量；yaw为所述双轮机器人绕z轴旋转的角度值；car_zero_angle为预存的零角度目标值；gyro[2]为陀螺仪测得的双轮机器人在行走过程中绕z轴旋转的角速度值；p为根据所述负重值计算得到的比例控制参数值，d为根据所述负重值计算得到的微分控制参数值。由于当对双轮机器人进行pd直线控制时，只要使双轮机器人绕z轴旋转的角度为0，则双轮机器人一直直线行走，因此将绕z轴旋转的角度与设定的角度目标值之差进行p比例控制。为了保持双轮机器人行进时走直线，所以将角度目标值设置为0，另外，将绕z轴旋转的角速度进行d微分控制，微分控制可以实现对干扰做出迅速的响应。

在本发明的一个实施例中，所述处理器330，具体用于根据预存的相应负重时左右电机的最佳步进数对计算出的p值和d值进行平滑处理；根据平滑处理后的p值和d值计算进行pd直线控制的平滑后的输出量；将计算出的所述平滑后的输出量输出至所述左右电机；判断根据所述平滑后的输出量控制得到的所述左右电机的步进数是否达到预存的相应负重时所述左右电机的最佳步进数；若是，则停止所述p值和d值的平滑处理；若否，则重复上述操作；

以及，

所述处理器330，具体用于根据公式δp＝pout-p0计算所述双轮机器人负重前后进行pd直线控制的p值的差值；其中，δp为所述双轮机器人负重前后进行pd直线控制的p值的差值，pout为双轮机器人适应当前负重的p值；p0为双轮机器人的负重为零时的p值；

根据公式p＝p0+δp/stepp，将所述双轮机器人负重前后进行pd直线控制的p值的差值与预存的相应负重时左右电机的最佳步进数的比值累加到双轮机器人的负重为零时的p值，得到平滑处理后的p值；其中，p为平滑处理后的p值，p0为双轮机器人的负重为零时的p值,δp为所述双轮机器人负重前后进行pd直线控制的p值的差值，stepp为预存的相应负重时左右电机的最佳步进数；

同理，根据公式δd＝dout-d0和d＝d0+δd/stepd计算平滑处理后的d值。由此可知，采用平滑处理机制，优化了双轮机器人的直线行走控制，并有效缓解了负重突然增大时导致的瞬间卡顿问题。

综上所述，本发明的技术方案实现了一种双轮机器人直线行走的控制策略，利用陀螺仪获取双轮机器人绕z轴旋转的角速度数据，并根据角速度值计算双轮机器人绕z轴旋转的角度值；通过压力传感器测得双轮机器人的外加负重大小，并根据外加负重大小计算双轮机器人进行pd直线控制的比例控制参数p值和微分控制参数d值以优化pd直线控制；将双轮机器人绕z轴旋转的角度值与预存的零角度目标值之差进行p值的比例控制，将双轮机器人绕z轴旋转的角速度值进行d值的微分控制，计算双轮机器人进行pd直线控制的输出量；将输出量输出至左右电机，根据输出量控制左右电机的步进数，消除了双轮机器人机械系统中存在的误差，并使得双轮机器人可以在不同负重情况下直线行走。

以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内所作的任何修改、等同替换、改进等，均包含在本发明的保护范围内。