一种两轮机器人自平衡控制方法和机器人

1.本发明实施例涉及机器人技术领域，尤其涉及一种两轮机器人自平衡控制方法和机器人。


背景技术：

2.随着自动控制技术的发展，倒立摆形态的平衡车开始出现。平衡车具有运动灵活，体积较小、地形适应性较强等特点，可以实现跟踪。因此，现在已经开始出现了平衡车构型的两轮自平衡机器人。
3.现有的二轮平衡车大多数采用的都是经典理论的pid算法，虽然简单且上手难度低，但其调参过程漫长，且多依靠经验进行参数的调整。我们团队研发的自平衡机器人底盘使用了lqr最优控制算法，该控制基于物理建模得到状态空间方程，并依靠这个状态空间方程计算得到付出代价最小的控制方式，一定程度上缩短了调参的过程。
4.在车身上，现有的平衡车构型因为需要依靠车身的倾斜来实现运动，在运动时车身无法保持水平状态，因此对于其用途有一定限制。


技术实现要素：

5.本发明实施例提供一种两轮机器人自平衡控制方法和机器人，以解决现有技术中两轮机器人需要依靠车身的倾斜来实现运动，在运动时车身无法保持水平状态的问题。
6.第一方面，本发明实施例提供一种两轮机器人自平衡控制方法，所述两轮机器人包括底盘和车体，所述底盘包括两个驱动轮，所述车体上设有云台；所述自平衡控制方法包括：
7.基于两个驱动轮的相对位置及受力状态对底盘进行物理建模，得到底盘运动的状态空间模型；对所述状态空间模型进行lqr解算，得到控制矩阵，基于所述控制矩阵和lqr解算的反馈量确定底盘电机的扭矩输出量；
8.基于pid控制方法对云台的pitch轴角度和yaw轴角度进行反馈控制。
9.作为优选的，基于两个驱动轮的相对位置及受力状态对底盘进行物理建模，得到底盘运动的状态空间模型，具体包括：
10.基于驱动轮的质量、转动路程、受车体水平作用力、摩擦力、转动惯量、半径和力矩，构建驱动轮状态模型；
11.基于车体与竖直方向的角度差，车体重量、车体相对于转轴转动的转动惯量、车体质心距离转动轴的距离、车体水平方向位移、车身偏转角度、驱动轮受车体竖直作用力大小、车身宽度确定车体状态模型；
12.基于所述驱动轮状态模型和所述车体状态模型确定两轮机器人的正向运动方程；基于两个驱动轮的驱动轮状态模型确定两轮机器人的旋转运动方程。
13.作为优选的，基于驱动轮的质量、转动路程、受车体水平作用力、摩擦力、转动惯量、半径和力矩，构建驱动轮状态模型，具体包括：
14.获取单个驱动轮的质量mw、转动路程x
x
、受车体水平作用力n
x
、摩擦力nf，其中，x
x
和n
x
中，x为l时表示左轮，x为r时表示右轮；基于牛顿经典力学获得：
[0015][0016]
由平衡转动定理得：
[0017][0018]
上式中，r为驱动轮的半径，iw为单个驱动轮的转动惯量，t
x
为电机对驱动轮产生的力矩；分别对两个驱动轮联立方程，得到左驱动轮的驱动轮状态模型为：
[0019][0020]
右驱动轮的驱动轮状态模型为：
[0021][0022]
作为优选的，基于车体与竖直方向的角度差，车体重量、车体相对于转轴转动的转动惯量、车体质心距离转动轴的距离、车体水平方向位移、车身偏转角度、驱动轮受车体竖直作用力大小、车身宽度确定车体状态模型，具体包括：
[0023]
获取车体与竖直方向的角度差θ，车体重量m
p
、车体相对于转轴转动的转动惯量i
p
、车体质心距离转动轴的距离l、车体水平方向位移x、车身偏转角度δ、驱动轮受车体竖直作用力大小p、车身宽度d；基于经典力学得到：
[0024][0025][0026]
上式中，t为时间；g为重力加速度；基于平衡轴转动定理得到车体状态模型：
[0027][0028]
上式中，t
l
为电机对左驱动轮产生的力矩，tr为电机对右驱动轮产生的力矩。
[0029]
作为优选的，基于所述驱动轮状态模型和所述车体状态模型确定两轮机器人的正向运动方程，具体包括：
[0030]
联立左驱动轮和右驱动轮的状态模型得到：
[0031][0032]
上式中，t＝t
l
+tr，n＝n
l
+nr；
[0033]
[0034][0035]
令sinθ＝1，cosθ＝0，线性优化得到正向运动方程：
[0036][0037]
作为优选的，基于两个驱动轮的驱动轮状态模型确定两轮机器人的旋转运动方程，具体包括；
[0038]
基于左驱动轮和右驱动轮的驱动轮状态模型及刚体定轴转动定律确定旋转运动方程：
[0039][0040][0041]
上式中，i
δ
为车体直立时在水平面上绕中心转动的转动惯量。作为优选的，对所述状态空间模型进行lqr解算，具体包括：建立线性模型：
[0042][0043]
令：
[0044][0045]
则有：
[0046][0047]
其中：
[0048][0049][0050][0051][0052][0053][0054]
基于lqr设定反馈控制器u＝-kx，以使代价函数最小，其中，代价函数为：
[0055][0056]
上式中，q为n
×
n维半正定的状态加权矩阵，r为n
×
n维正定的控制加权矩阵；
[0057]
其中，反馈增益满足：k＝r-1bt
p；
[0058]
p满足riccati方程：a
t
p+pa+q-prb-1bt
p＝0。
[0059]
第二方面，本发明实施例提供一种两轮机器人，所述两轮机器人包括控制器，所述控制器采用如本发明第一方面实施例所述两轮机器人自平衡控制方法。
[0060]
第三方面，本发明实施例提供一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现如本发明第一方面实施例所述两轮机器人自平衡控制方法的步骤。
[0061]
第四方面，本发明实施例提供一种非暂态计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现如本发明第一方面实施例所述两轮机器人自平衡控制方法的步骤。
[0062]
本发明实施例提供的一种两轮机器人自平衡控制方法和机器人，可以有更多的自由度以实现更复杂的运动需求，可以控制其独立运动，这一部分称之为云台，位于车身上部。运用陀螺仪等传感器，可以实现车的正常运动下云台的绝对水平；结合视觉计算，可以实现云台对特定特征目标的识别以及跟随。自由度的增加使得二轮平衡机器人的运动受限程度明显降低，可以胜任更多方面的工作。其次，该平衡机器人使用lqr控制，基于物理建模得到控制系统，相对于一般的pid控制，可以简化繁杂的调参过程，并且提高控制效率。并且由于加入了yaw轴和pitch轴这两个自由度，车身上部可以实现更精准的位置控制，可以加
入视觉功能实现瞄准目标、跟随等高级功能。
附图说明
[0063]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
[0064]
图1为根据本发明实施例的两轮机器人自平衡控制方法流程框图；
[0065]
图2为根据本发明实施例的云台控制算法架构图；
[0066]
图3为本发明提供的电子设备的结构示意图。
具体实施方式
[0067]
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0068]
本技术实施例中术语“和/或”，仅仅是一种描述关联对象的关联关系，表示可以存在三种关系，例如，a和/或b，可以表示：单独存在a，同时存在a和b，单独存在b这三种情况。
[0069]
本技术实施例中的术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。本技术的描述中，术语“包括”和“具有”以及它们任何变形，意图在于覆盖不排他的包含。例如包含了一系列部件或单元的系统、产品或设备没有限定于已列出的部件或单元，而是可选地还包括没有列出的部件或单元，或可选地还包括对于这些产品或设备固有的其它部件或单元。本技术的描述中，“多个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。
[0070]
在本文中提及“实施例”意味着，结合实施例描述的特定特征、结构或特性可以包含在本技术的至少一个实施例中。在说明书中的各个位置出现该短语并不一定均是指相同的实施例，也不是与其它实施例互斥的独立的或备选的实施例。本领域技术人员显式地和隐式地理解的是，本文所描述的实施例可以与其它实施例相结合。
[0071]
现有的二轮平衡车大多数采用的都是经典理论的pid算法，虽然简单且上手难度低，但其调参过程漫长，且多依靠经验进行参数的调整。本发明实施例提供的自平衡机器人底盘使用了lqr最优控制算法，该控制基于物理建模得到状态空间方程，并依靠这个状态空间方程计算得到付出代价最小的控制方式，一定程度上缩短了调参的过程。
[0072]
在车身上，现有的平衡车构型因为需要依靠车身的倾斜来实现运动，在运动时车身无法保持水平状态，因此对于其用途有一定限制。如果可以加上一个可以活动并控制的机构，则可以拓展此类机器人的用途。
[0073]
因此，本发明实施例提供一种两轮机器人自平衡控制方法和机器人，在底盘的运动的基础上，可以有更多的自由度以实现更复杂的运动需求，可以控制其独立运动，这一部分称之为云台，位于车身上部。运用陀螺仪等传感器，可以实现车的正常运动下云台的绝对
水平；结合视觉计算，可以实现云台对特定特征目标的识别以及跟随。下面结合附图描述本发明实施例的一种两轮机器人自平衡控制方法和机器人。
[0074]
图1为根据本发明实施例提供一种两轮机器人自平衡控制方法，所述两轮机器人包括底盘和车体，所述底盘包括两个驱动轮，所述车体上设有云台；本发明实施例的控制方案主要分为两部分，一部分是控制机器人整体运动的底盘，另一部分是有两个自由度的云台。底盘是控制机器人整体进行直线运动与转动的部分，这一部分的控制机构包括底盘电机及其编码器、陀螺仪、控制核心(单片机)；底盘上的陀螺仪需要获取各个方向的加速度和角速度，并通过滤波解算出车身的姿态角；电机编码器获取底盘驱动轮的转速。底盘控制使用了经典力学理论对底盘进行物理建模，得到了其运动的状态空间模型。基于已建立的状态空间模型使用lqr进行计算，得到控制矩阵，该矩阵与反馈量的矩阵相乘即可得到电机的扭矩输出大小。所述自平衡控制方法包括：
[0075]
基于两个驱动轮的相对位置及受力状态对底盘进行物理建模，得到底盘运动的状态空间模型；对所述状态空间模型进行lqr解算，得到控制矩阵，基于所述控制矩阵和lqr解算的反馈量确定底盘电机的扭矩输出量；
[0076]
其中，基于两个驱动轮的相对位置及受力状态对底盘进行物理建模，得到底盘运动的状态空间模型，具体包括：
[0077]
基于驱动轮的质量、转动路程、受车体水平作用力、摩擦力、转动惯量、半径和力矩，构建驱动轮状态模型；获取单个驱动轮的质量mw、转动路程x
x
、受车体水平作用力n
x
、摩擦力nf，其中，x
x
和n
x
中，x为l时表示左轮，x为r时表示右轮；基于牛顿经典力学获得：
[0078][0079]
由平衡转动定理得：
[0080][0081]
上式中，r为驱动轮的半径，iw为单个驱动轮的转动惯量，t
x
为电机对驱动轮产生的力矩；分别对两个驱动轮联立方程，得到左驱动轮的驱动轮状态模型为：
[0082][0083]
右驱动轮的驱动轮状态模型为：
[0084][0085]
基于车体与竖直方向的角度差，车体重量、车体相对于转轴转动的转动惯量、车体质心距离转动轴的距离、车体水平方向位移、车身偏转角度、驱动轮受车体竖直作用力大小、车身宽度确定车体状态模型；
[0086]
对平衡机器人轮子以外的车身进行分析，获取车体与竖直方向的角度差θ，车体重量m
p
、车体相对于转轴转动的转动惯量i
p
、车体质心距离转动轴的距离l、车体水平方向位移x、车身偏转角度δ、驱动轮受车体竖直作用力大小p、车身宽度d；基于经典力学得到：
[0087][0088][0089]
上式中，t为时间；g为重力加速度；基于平衡轴转动定理得到车体状态模型：
[0090][0091]
上式中，t
l
为电机对左驱动轮产生的力矩，tr为电机对右驱动轮产生的力矩。
[0092]
基于所述驱动轮状态模型和所述车体状态模型确定两轮机器人的正向运动方程；联立左驱动轮和右驱动轮的状态模型(1.1)、(1.2)得到：
[0093][0094]
上式中，t＝t
l
+tr，n＝n
l
+nr；联立式(2.1)(3.1)得到：
[0095][0096]
联立上式(2.1)、(2.2)、(2.3)得到：
[0097][0098]
考虑到车身倾斜角度都比较小，在较小的倾角时可以进行如下线性化：令sinθ＝1，cosθ＝0，线性优化得到正向运动方程：
[0099][0100][0101]
该二方程(3.6)、(3.7)即为正向运动的方程。
[0102]
基于两个驱动轮的驱动轮状态模型确定两轮机器人的旋转运动方程，根据刚体定轴转动定律：
[0103][0104]
δ为车身转动的角度，(1.1)与(1.2)相减得到：
[0105][0106]
左驱动轮和右驱动轮速度不相等时，会发生转向：
[0107]
[0108]
联立(3.8)、(3.9)、(3.10)得到：
[0109][0110]
上式中，i
δ
为车体直立时在水平面上绕中心转动的转动惯量，至此，物理建模完成。
[0111]
对所述状态空间模型进行lqr解算，具体包括：
[0112]
建立线性模型：
[0113][0114]
令：
[0115][0116]
则有：
[0117][0118]
其中：
[0119][0120][0121][0122]
[0123][0124][0125]
基于lqr设定反馈控制器u＝-kx，以使代价函数最小，其中，代价函数为：
[0126][0127]
上式中，q为n
×
n维半正定的状态加权矩阵，q为n
×
n维正定的控制加权矩阵；通过改动q和q的值，可以修改各个参数在控制系统中所占权重，从而使控制符合实际需求。
[0128]
其中，反馈增益满足：k＝r-1bt
p；
[0129]
p满足riccati方程：a
t
p+pa+q-prb-1bt
p＝0。
[0130]
基于pid控制方法对云台的pitch轴角度和yaw轴角度进行反馈控制。
[0131]
如图2中所示，云台有两个自由度，分别是yaw轴和pitch轴。云台上所搭载的陀螺仪可以获取yaw轴和pitch轴的角度，设定期望角度并使用pid计算得到电机的输出值。云台上的pid基于角度误差进行反馈控制。yaw轴和pitch轴的系统都可视作siso(单输入单输出)系统，其控制较为简单，因此需要调整的参数较少，使用pid可以很容易地搭建一个控制系统。同时因为pitch轴会受重力影响，加入了前馈控制，在pitch轴物理特性不变的情况下，可以将重力视作稳定的扰动进行补偿，以提高控制精度。
[0132]
而因为云台电机控制相对于底盘控制应该更加精准，可以实现瞄准目标等功能，加入摄像头以及运算平台，就可以实现对具有一定特征的目标进行识别和跟踪。
[0133]
而因为底盘和云台运动是相对独立的，可以实现云台转动而底盘不动、或者底盘跟随云台转动的效果。
[0134]
本发明实施例提供一种两轮机器人，所述两轮机器人包括控制器，所述控制器采用如本发明上述实施例所述两轮机器人自平衡控制方法。
[0135]
基于相同的构思，图3示例了一种电子设备的实体结构示意图，如图3所示，该电子设备可以包括：处理器(processor)310、通信接口(communications interface)320、存储器(memory)330和通信总线340，其中，处理器310，通信接口320，存储器330通过通信总线340完成相互间的通信。处理器310可以调用存储器330中的逻辑指令，以执行两轮机器人自平衡控制方法，该方法包括：
[0136]
基于两个驱动轮的相对位置及受力状态对底盘进行物理建模，得到底盘运动的状态空间模型；对所述状态空间模型进行lqr解算，得到控制矩阵，基于所述控制矩阵和lqr解算的反馈量确定底盘电机的扭矩输出量；
[0137]
基于pid控制方法对云台的pitch轴角度和yaw轴角度进行反馈控制。
[0138]
此外，上述的存储器330中的逻辑指令可以通过软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施
例所述方法的全部或部分步骤。而前述的存储介质包括：u盘、移动硬盘、只读存储器(rom，read-onlymemory)、随机存取存储器(ram，randomaccess memory)、磁碟或者光盘等各种可以存储程序代码的介质。
[0139]
基于相同的构思，本发明实施例还提供一种非暂态计算机可读存储介质，该计算机可读存储介质存储有计算机程序，该计算机程序包含至少一段代码，该至少一段代码可由主控设备执行，以控制主控设备用以实现如上述各实施例所述两轮机器人自平衡控制方法的步骤。例如包括：
[0140]
基于两个驱动轮的相对位置及受力状态对底盘进行物理建模，得到底盘运动的状态空间模型；对所述状态空间模型进行lqr解算，得到控制矩阵，基于所述控制矩阵和lqr解算的反馈量确定底盘电机的扭矩输出量；
[0141]
基于pid控制方法对云台的pitch轴角度和yaw轴角度进行反馈控制。
[0142]
基于相同的技术构思，本技术实施例还提供一种计算机程序，当该计算机程序被主控设备执行时，用以实现上述方法实施例。
[0143]
所述程序可以全部或者部分存储在与处理器封装在一起的存储介质上，也可以部分或者全部存储在不与处理器封装在一起的存储器上。
[0144]
基于相同的技术构思，本技术实施例还提供一种处理器，该处理器用以实现上述方法实施例。上述处理器可以为芯片。
[0145]
综上所述，本发明实施例提供的一种两轮机器人自平衡控制方法和机器人，可以有更多的自由度以实现更复杂的运动需求，可以控制其独立运动，这一部分称之为云台，位于车身上部。运用陀螺仪等传感器，可以实现车的正常运动下云台的绝对水平；结合视觉计算，可以实现云台对特定特征目标的识别以及跟随。自由度的增加使得二轮平衡机器人的运动受限程度明显降低，可以胜任更多方面的工作。其次，该平衡机器人使用lqr控制，基于物理建模得到控制系统，相对于一般的pid控制，可以简化繁杂的调参过程，并且提高控制效率。并且由于加入了yaw轴和pitch轴这两个自由度，车身上部可以实现更精准的位置控制，可以加入视觉功能实现瞄准目标、跟随等高级功能。
[0146]
本发明的各实施方式可以任意进行组合，以实现不同的技术效果。
[0147]
在上述实施例中，可以全部或部分地通过软件、硬件、固件或者其任意组合来实现。当使用软件实现时，可以全部或部分地以计算机程序产品的形式实现。所述计算机程序产品包括一个或多个计算机指令。在计算机上加载和执行所述计算机程序指令时，全部或部分地产生按照本技术所述的流程或功能。所述计算机可以是通用计算机、专用计算机、计算机网络、或者其他可编程装置。所述计算机指令可以存储在计算机可读存储介质中，或者从一个计算机可读存储介质向另一个计算机可读存储介质传输，例如，所述计算机指令可以从一个网站站点、计算机、服务器或数据中心通过有线(例如同轴电缆、光纤、数字用户线)或无线(例如红外、无线、微波等)方式向另一个网站站点、计算机、服务器或数据中心进行传输。所述计算机可读存储介质可以是计算机能够存取的任何可用介质或者是包含一个或多个可用介质集成的服务器、数据中心等数据存储设备。所述可用介质可以是磁性介质，(例如，软盘、硬盘、磁带)、光介质(例如，dvd)、或者半导体介质(例如固态硬盘solidstatedisk)等。
[0148]
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，该流程
可以由计算机程序来指令相关的硬件完成，该程序可存储于计算机可读取存储介质中，该程序在执行时，可包括如上述各方法实施例的流程。而前述的存储介质包括：rom或随机存储记忆体ram、磁碟或者光盘等各种可存储程序代码的介质。
[0149]
最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

技术特征：
1.一种两轮机器人自平衡控制方法，所述两轮机器人包括底盘和车体，所述底盘包括两个驱动轮，所述车体上设有云台；其特征在于，所述自平衡控制方法包括：基于两个驱动轮的相对位置及受力状态对底盘进行物理建模，得到底盘运动的状态空间模型；对所述状态空间模型进行lqr解算，得到控制矩阵，基于所述控制矩阵和lqr解算的反馈量确定底盘电机的扭矩输出量；基于pid控制方法对云台的pitch轴角度和yaw轴角度进行反馈控制。2.根据权利要求1所述的两轮机器人自平衡控制方法，其特征在于，基于两个驱动轮的相对位置及受力状态对底盘进行物理建模，得到底盘运动的状态空间模型，具体包括：基于驱动轮的质量、转动路程、受车体水平作用力、摩擦力、转动惯量、半径和力矩，构建驱动轮状态模型；基于车体与竖直方向的角度差，车体重量、车体相对于转轴转动的转动惯量、车体质心距离转动轴的距离、车体水平方向位移、车身偏转角度、驱动轮受车体竖直作用力大小、车身宽度确定车体状态模型；基于所述驱动轮状态模型和所述车体状态模型确定两轮机器人的正向运动方程；基于两个驱动轮的驱动轮状态模型确定两轮机器人的旋转运动方程。3.根据权利要求2所述的两轮机器人自平衡控制方法，其特征在于，基于驱动轮的质量、转动路程、受车体水平作用力、摩擦力、转动惯量、半径和力矩，构建驱动轮状态模型，具体包括：获取单个驱动轮的质量m
w
、转动路程x
x
、受车体水平作用力n
x
、摩擦力n
f
，其中，x
x
和n
x
中，x为l时表示左轮，x为r时表示右轮；基于牛顿经典力学获得：由平衡转动定理得：上式中，r为驱动轮的半径，i
w
为单个驱动轮的转动惯量，t
x
为电机对驱动轮产生的力矩；分别对两个驱动轮联立方程，得到左驱动轮的驱动轮状态模型为：右驱动轮的驱动轮状态模型为：4.根据权利要求3所述的两轮机器人自平衡控制方法，其特征在于，基于车体与竖直方向的角度差，车体重量、车体相对于转轴转动的转动惯量、车体质心距离转动轴的距离、车体水平方向位移、车身偏转角度、驱动轮受车体竖直作用力大小、车身宽度确定车体状态模型，具体包括：获取车体与竖直方向的角度差θ，车体重量m
p
、车体相对于转轴转动的转动惯量i
p
、车体质心距离转动轴的距离l、车体水平方向位移x、车身偏转角度δ、驱动轮受车体竖直作用力
大小p、车身宽度d；基于经典力学得到：大小p、车身宽度d；基于经典力学得到：上式中，t为时间；g为重力加速度；基于平衡轴转动定理得到车体状态模型：上式中，t
l
为电机对左驱动轮产生的力矩，t
r
为电机对右驱动轮产生的力矩。5.根据权利要求3所述的两轮机器人自平衡控制方法，其特征在于，基于所述驱动轮状态模型和所述车体状态模型确定两轮机器人的正向运动方程，具体包括：联立左驱动轮和右驱动轮的状态模型得到：上式中，t＝t
l
+t
r
，n＝n
l
+n
r
；；令sinθ＝1，cosθ＝0，线性优化得到正向运动方程：6.根据权利要求5所述的两轮机器人自平衡控制方法，其特征在于，基于两个驱动轮的驱动轮状态模型确定两轮机器人的旋转运动方程，具体包括；基于左驱动轮和右驱动轮的驱动轮状态模型及刚体定轴转动定律确定旋转运动方程：基于左驱动轮和右驱动轮的驱动轮状态模型及刚体定轴转动定律确定旋转运动方程：上式中，i
δ
为车体直立时在水平面上绕中心转动的转动惯量。7.根据权利要求6所述的两轮机器人自平衡控制方法，其特征在于，对所述状态空间模型进行lqr解算，具体包括：建立线性模型：
令：则有：其中：其中：其中：其中：其中：其中：基于lqr设定反馈控制器u＝-kx，以使代价函数最小，其中，代价函数为：
上式中，q为n
×
n维半正定的状态加权矩阵，r为n
×
n维正定的控制加权矩阵；其中，反馈增益满足：k＝r-1
b
t
p；p满足riccati方程：a
t
p+pa+q-prb-1
b
t
p＝0。8.一种两轮机器人，其特征在于，包括底盘和车体，所述底盘包括两个驱动轮，所述车体上设有云台；其特征在于，还包括控制器，所述控制器采用如权利要求1至7任一项所述两轮机器人自平衡控制方法。9.一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现如权利要求1至7任一项所述两轮机器人自平衡控制方法的步骤。10.一种非暂态计算机可读存储介质，其上存储有计算机程序，其特征在于，该计算机程序被处理器执行时实现如权利要求1至7任一项所述两轮机器人自平衡控制方法的步骤。

技术总结
本发明实施例提供一种两轮机器人自平衡控制方法和机器人，可以有更多的自由度以实现更复杂的运动需求，可以控制其独立运动，这一部分称之为云台，位于车身上部。运用陀螺仪等传感器，可以实现车的正常运动下云台的绝对水平；结合视觉计算，可以实现云台对特定特征目标的识别以及跟随。自由度的增加使得二轮平衡机器人的运动受限程度明显降低，可以胜任更多方面的工作。其次，该平衡机器人使用LQR控制，基于物理建模得到控制系统，相对于一般的PID控制，可以简化繁杂的调参过程，并且提高控制效率。并且由于加入了yaw轴和pitch轴这两个自由度，车身上部可以实现更精准的位置控制，可以加入视觉功能实现瞄准目标、跟随等高级功能。能。能。


技术研发人员：张准 方驰 陈静楠 李志凯 余文致 陆韵
受保护的技术使用者：华南师范大学
技术研发日：2023.03.24
技术公布日：2023/7/7