本发明涉及一种两轮自平衡车直线定点运动的最优时间轨迹规划方法，包括基于拟凸优化技术的最优离散轨迹规划方法和基于自适应曲线细化的B样条插值算法。所述最优离散轨迹规划方法，在充分考虑状态和控制输入约束的情况下，通过系统离散化，将最优时间轨迹规划问题转化为标准的拟凸优化问题，并求解得到时间最优离散轨迹；所述B样条插值算法，通过识别所得最优离散轨迹中的特征点获得初始插值点，并进而得到初始B样条插值曲线，然后通过自适应的方式不断细化插值曲线，直到所得解析轨迹满足给定精度。该方法大大提高了工作效率，也为两轮自平衡车的控制提供了一个最高效率指标的参考。
1.两轮自平衡车直线定点运动的最优时间轨迹规划方法，其特征在于，包括基于拟凸优化技术的最优离散轨迹规划方法和基于自适应曲线细化的B样条插值算法；
所述基于拟凸优化技术的最优离散轨迹规划方法，具体为：基于两轮自平衡车纵向运动与车身平衡的运动耦合关系推导状态空间模型，通过引入状态反馈控制律来保证系统的稳定；然后，通过系统离散化，将两轮自平衡车直线定点运动的最优时间轨迹规划问题转化为标准的拟凸优化问题，利用二分法并结合求解一系列凸可行性问题得到最优离散轨迹；
所述基于自适应曲线细化的B样条插值算法，具体为：通过识别所得最优离散轨迹中的特征点获得初始插值点；然后，通过B样条插值技术获得初始插值曲线；接着，计算最优离散轨迹上各点与初始B样条插值曲线上各对应点的偏差，并将偏差最大的点增加到插值点中，重新生成插值曲线；如此循环，以自适应的方式不断细化插值曲线，直到所得解析轨迹表达式满足给定精度；
J2＝I4/2+I6/2+mbL2/2；mb为车身质量，g为重力加速度，L为车身质心到轮轴的距离，I4为底盘绕轮轴的转动惯量，I6为车身绕轮轴的转动惯量；xvr为参考纵向位移， 为参考纵向速度，θr为参考车身倾角， 为参考车身倾角速度。
2.根据权利要求1所述两轮自平衡车直线定点运动的最优时间轨迹规划方法，其特征在于，状态反馈控制律为：δ＝Kp+u
其中，K＝[k1,k2,k3,k4]为状态反馈系数矩阵，u为参考控制输入，则引入状态反馈的闭环系统表达为：其中，A＝G+HK,B＝H。
3.根据权利要求2所述两轮自平衡车直线定点运动的最优时间轨迹规划方法，其特征在于，式(6)的解表达为其中，t0为初始时间；
然后，令t0＝kT，t＝(k+1)T，其中，k＝0,1,2,...，T为采样周期；假设u在采样周期内保持不变，可得基于以上推导，式(6)的精确离散化形式表达为
p(k+1)＝C(T)p(k)+D(T)u(k) (9)其中，C(T)＝eAT, p(k)和u(k)分别为第k个采样时间的系统状态和控制输入；
通过式(10)看出，优化控制序列u0,u1,...,uk-1，即可实现两轮自平衡车的最优时间控制。
4.根据权利要求3所述两轮自平衡车直线定点运动的最优时间轨迹规划方法，其特征在于，控制序列应该保证系统能够最终到达目标状态pf，因此需要满足如下等式约束：为了确保两轮自平衡车的稳定运行，车身倾角和倾角速度需要满足|θr(k)|≤θmax (12)
其中，θmax和 为所允许的最大车身倾角和倾角速度；
|uj|≤umax,j＝0,1,2,...,k-1 (15)其中，vmax和umax分别为所允许的最大速度和最大参考控制输入；
其中，Eθ＝[0,0,1,0]T， Ev＝[0,1,0,0]T；
这样，约束式(12)-式(14)转化为关于优化变量u0,u1,...,uk-1的线性不等式约束，如下
5.根据权利要求4所述两轮自平衡车直线定点运动的最优时间轨迹规划方法，其特征在于，假设系统在t＝knT时到达目标状态pf，其中kn为足够大的正整数，并满足如下关系其中xf为终点位置的横坐标、yf为终点位置的纵坐标；
其中， 即为优化问题的目标函数，km为最优时间指数；
对于i＝0,1,...,km-1，ui满足-umax≤ui≤umax；对于i＝km,km+
6.根据权利要求4或5所述两轮自平衡车直线定点运动的最优时间轨迹规划方法，其特征在于，对于求解优化问题式(30)所得的有序点序列 点Qi(i＝1,
2,...,km-2)处的斜率通过中心差分法求得，如下：通过式(31)求得各点处的斜率后，通过如下方式选取大斜率点：给定一个正的阈值δ，当各点的斜率满足 时，该点被选择为大斜率点；
对于求解优化问题式(30)所得的有序点列 点Q′i(i＝1,
2,...,km-2)处的曲率通过过Q′i-1，Q′i和Q′i+1这三点的圆弧的曲率近似代替，并通过如下公式进行计算：其中， 是由Q′i-1，Q′i和Q′i+1这三点组成的有符号三角形的面积；ψi为线段和线段 之间的夹角；Mi，Mi+1和Ni分别为线段 和 的长度；
通过式(32)计算出相应的曲率ρi(i＝1,2,...,km-2)后，选取满足如下条件的点为曲率极值点:ρi＞ρi-1,ρi＞ρi+1或ρi＜ρi-1,ρi＜ρi+1，其中，
7.根据权利要求1所述两轮自平衡车直线定点运动的最优时间轨迹规划方法，其特征在于，B样条曲线表达为其中，Ci(i＝0,1,2,...,n)为控制点，Ni,m(x)(i＝0,1,2,...,n)为定义在非降节点序列X＝[x0,x1,...,xn+m+1]上的m次B样条基函数，满足xi≤xi+1,i＝0,1,...,n+m；
根据德布尔-考克斯递推公式，并约定0/0＝0，则m次B样条基函数给定如下从递推公式(34)得出，计算基函数Ni,m(x)，需要m+2个节点xi,xi+1,...,xi+m+1，也即基函数Ni,m(x)的支承空间为[xi,xi+m+1]；因此，在区间x∈[xi,xi+1]上至多有m+1个非零的m次B样条基函数Nj,m(x)(j＝i-m,i-m+1,...,i)，则式(33)进一步写作m次B样条曲线的导数通过下式进行计算
L[j]＝x-xi+1-j,R[j]＝xi+j-x,j＝0,1,2,... (37)然后，根据式(34)和式(37)，m+1个m次B样条基函数通过如下公式进行计算其中，Ni-m,m-1(x)＝0,Ni+1,m-1(x)＝0,Ni,0(x)＝1,x∈[xi,xi+1]。
8.根据权利要求6所述两轮自平衡车直线定点运动的最优时间轨迹规划方法，其特征在于，根据识别出的特征点集 反算出控制顶点；为此，采用向心参数化法对 进行参数化：
则节点序列X＝[x0,x1,...,xr+2m]通过以下公式进行计算在此基础上，令B样条曲线恰好插值于X中各节点处，这就意味着，在任意插值点xi(i＝m,m+1,...,m+r)处最多有m个非零的m次B样条基函数Nj,m(x)(j＝i-m,i-m+1,...,i-1)；令m＝3，则根据式(35)和特征点集 构建如下(r+1)×(r+3)的方程组：其中， 根据式(37)和式
给定两个端点处的一阶导数P(1)(x3)和P(1)(xr+3)来获得两个辅助方程，其中，P(1)(x3)和P(1)(xr+3)的方向给定为水平方向，而模长给定为d/2；这样，根据式(36)获得如下两个辅助方程：基于式(41)-式(43)，计算出r+3个控制点 然后根据式(35)获得B样条插值曲线。
9.根据权利要求8所述两轮自平衡车直线定点运动的最优时间轨迹规划方法，其特征在于，考虑到 P(x)＝(t(x),u(x))，因此，根据式(35)得如下方程
定义离散点列Qi和对应的插值曲线上的点P(xi)之间的偏差的绝对值为拟合误差，即ei＝|ui-u(ti)|,i＝0,1,...,km-1 (46)其中，u(ti)表示由式(45)计算出的u(x*)，x*根据式(44)并结合t(x)＝ti反算得到；
如果ei的最大值比允许误差ε大，也即， 则相应的最大误差点就被添加到特征点集 中，并重新生成插值曲线；如此循环，直到所得B样条插值曲线满足给定的拟合误差要求。
两轮自平衡车直线定点运动的最优时间轨迹规划方法
技术领域
[0001] 本发明属于两轮自平衡车控制领域，具体而言是一种两轮自平衡车直线定点运动的最优时间轨迹规划方法。
背景技术
[0002] 两轮自平衡车以其结构紧凑、运动灵活和耗能低等诸多优点，在工业生产、交通运输和家居服务等领域具有重要的应用价值，而有效的运动控制是两轮自平衡车系统完成相关任务的技术基础。两轮自平衡车的运动控制一般可分为上层的轨迹规划和下层的轨迹跟踪，而合理的轨迹规划对于简化跟踪控制器设计，提高两轮自平衡车运动控制效果具有重要意义。
[0003] 申请号为201810540583.5的中国专利为了实现两轮自平衡车的平面点对点自主运动控制，在考虑两轮自平衡车运动耦合关系的基础上，提出了一种纵向直行轨迹规划方法，其充分考虑了纵向运动与车身倾角之间的运动耦合关系，使所规划的纵向运动轨迹不仅能够保证两轮自平衡车到达目标位置，同时能够保持车身稳定。然而该方法难以直接考虑状态和控制输入约束，且只能实现渐近收敛而无法实现时间最优控制。
[0004] 发表在《IEEE Transactions on Industrial Electronics》2014年61卷12期上的论文“Minimum-time trajectory planning for underactuated overhead crane systems with state and control constraints”提出了一种针对欠驱动桥式吊车的的最小时间轨迹规划方法，其通过系统离散化和扩张，并充分考虑各种状态和控制输入约束，将最小时间轨迹规划问题成功转化为拟凸优化问题，并采用二分法求得了全局时间最优轨迹。然而，桥式吊车是一种本质稳定的欠驱动系统，相关方法难以应用于本质不稳定的两轮自平衡车；另外，该方法得到的时间最优轨迹是离散形式，对后续跟踪控制器的设计造成了障碍。
发明内容
[0005] 针对上述问题，本发明提供了一种两轮自平衡车直线定点运动的最优时间轨迹规划方法，为实现两轮自平衡车的最优时间控制奠定了基础。
[0006] 为实现上述目的，本发明采取以下技术方案：两轮自平衡车直线定点运动的最优时间轨迹规划方法，包括基于拟凸优化技术的最优离散轨迹规划方法和基于自适应曲线细化的B样条插值算法；
[0007] 所述基于拟凸优化技术的最优离散轨迹规划方法，具体为：基于两轮自平衡车纵向运动与车身平衡的运动耦合关系推导状态空间模型，通过引入状态反馈控制律来解决系统的本质不稳定问题；然后，通过系统离散化，将两轮自平衡车直线定点运动的最优时间轨迹规划问题转化为标准的拟凸优化问题，利用二分法并结合求解一系列凸可行性问题得到最优离散轨迹；
[0008] 所述基于自适应曲线细化的B样条插值算法，具体为：通过识别所得最优离散轨迹中的特征点获得初始插值点；然后，通过B样条插值技术获得初始插值曲线；接着，计算最优离散轨迹上各点与初始B样条插值曲线上各对应点的偏差，并将偏差最大的点增加到插值点中，重新生成插值曲线；如此循环，以自适应的方式不断细化插值曲线，直到所得解析轨迹满足给定精度。
[0009] 进一步的，在将最优时间轨迹规划问题转化为拟凸优化问题的过程中，充分考虑了参考控制输入约束、纵向速度约束、车身倾角约束、车身倾角速度约束，从而纵向加速度约束也可以得到保证。
[0010] 进一步的，在将最优时间轨迹规划问题转化为拟凸优化问题的过程中，并未提前指定具体的轨迹形式，因此所得最优离散轨迹为全局最优。
[0011] 进一步的，在选择特征点时，选择两个端点和大斜率点作为参考控制输入的特征点；选择两个端点和曲率极值点作为纵向速度和纵向位移的特征点。
[0012] 进一步的，根据基于自适应曲线细化的B样条插值算法，所得插值曲线可以给出横、纵坐标之间的直接关系，即给定实时时间，就可以求出相应的最优参考控制输入、最优纵向速度和最优纵向位移，进而可求得最优纵向加速度。
[0013] 本发明的有益效果是：在考虑两轮自平衡车状态和控制输入约束的条件下，可得到时间最优的、具有解析表达式的直线定点运动轨迹，在此基础上，设计相应的轨迹跟踪控制器即可实现两轮自平衡车的时间最优控制，从而大大提高工作效率；另外，由于本发明所得最优轨迹为全局最优，因此所得结果也为两轮自平衡车的控制提供了一个最高效率指标的参考。
附图说明
[0014] 本发明共有附图6幅：
[0015] 图1是本发明中所涉及两轮自平衡车的结构图；
[0016] 图2是所得时间最优轨迹；
[0017] 图3是参考控制输入时间最优离散轨迹中的特征点；
[0018] 图4是离散曲率计算示意图；
[0019] 图5是纵向速度和位移时间最优离散轨迹中的特征点；
[0020] 图6是自适应曲线细化过程中的最大拟合误差。
[0021] 图中序号说明：1、车轮，2、底盘，3、车身。
具体实施方式
[0022] 为了使本发明的目的、技术方案和优点更加清楚，下面结合附图和实施例对本发明进行详细描述。
[0023] 如图1所示，本发明所涉及的两轮自平衡车主要包括车轮1、底盘2和车身3等部件，其动力学模型可以表达为
[0024]
[0025]
[0026]
[0027] 其中， τv＝τr+τl，τω＝τr-τl，mv＝I2/r2+mω+mc/2+mb/2,J1＝I1+I3/2+I5/2+(mω+I2/r2)d2/4,J2＝I4/2+I6/2+mbL2/2,x和y分别为大地坐标系下的横、纵坐标，为转向角，θ为车身倾角，mω为车轮质量，mc为底盘质量，mb为车身质量，r为车轮半径，d为轮距，L为车身质心到轮轴的距离，I1为车轮绕其直径的转动惯量，I2为车轮绕其轮轴的转动惯量，I3为底盘绕过其质心的竖直线的转动惯量，I4为底盘绕轮轴的转动惯量，I5为车身绕过其质心的竖直线的转动惯量，I6为车身绕轮轴的转动惯量，τl、τr分别为左右电机的输出力矩，相关结构参数的取值具体参见文献“基于IMU/UWB的两轮自平衡车轨迹跟踪控制器设计与实现，控制与决策，DOI:10.13195/j.kzyjc.2018.0363”。
[0028] 基于运动耦合关系(3)，可得如下状态空间模型：
[0029]
[0030] 其中， θr为参考车身倾角，为参考纵向加速度。
[0031] 由于系统(4)的能控性矩阵M＝(H,GH,G2H,G3H)是非奇异的，也即系统(4)是可控的，因此，必然存在一些可行的控制序列使系统从初始状态p0到达目标状态pf。然而，可以计算出系统矩阵G的特征值为8.9393，-8.9393,0和0，正特征值8.9393的存在体现了系统的本质不稳定特性，这给寻找可行控制轨迹造成了巨大障碍，也就更难找到时间最优控制轨迹。
[0032] 为此，引入如下状态反馈控制律
[0033] δ＝Kp+u (5)
[0034] 其中，K＝[k1,k2,k3,k4]为状态反馈系数矩阵，u为参考控制输入，则引入状态反馈的闭环系统可以表达为
[0035]
[0036] 其中，A＝G+HK,B＝H。
[0037] 通过合理的选择K可以消除系统中正的特征值，本发明中取K＝[0,0,10,1]，可计算出A的特征值为-0.2051，-7.9491,0和0。这样，最优时间轨迹规划问题就可以基于闭环系统(6)进行。
[0038] (1)优化变量
[0039] 方程(6)的解可以表达为
[0040]
[0041] 其中，t0为初始时间。
[0042] 然后，令t0＝kT，t＝(k+1)T，其中，k＝0,1,2,...，T为采样周期。假设u在采样周期内保持不变，可得
[0043]
[0044] 基于以上推导，式(6)的精确离散化形式可以表达为
[0045] p(k+1)＝C(T)p(k)+D(T)u(k) (9)
[0046] 其中，C(T)＝eAT, p(k)和u(k)分别为第k个采样时间的系统状态和控制输入，为方便起见，后面将分别用pk和uk代替p(k)和u(k)。
[0047] 进一步地，根据式(9)可得
[0048]
[0049] 通过式(10)可以看出，通过优化控制序列u0,u1,...,uk-1，即可实现两轮自平衡车的最优时间控制。
[0050] (2)约束条件
[0051] 首先，控制序列应该保证系统能够最终到达目标状态pf，因此需要满足如下等式约束
[0052]
[0053] 接下来，为了确保两轮自平衡车的稳定运行，车身倾角和倾角速度需要满足[0054] |θr(k)|≤θmax (12)
[0055]
[0056] 其中，θmax和 为所允许的最大车身倾角和倾角速度。
[0057] 另外，考虑电机的性能约束，还需考虑如下约束
[0058]
[0059] |uj|≤umax,j＝0,1,2,...,k-1 (15)
[0060] 其中，vmax和umax分别为所允许的最大速度和最大参考控制输入。
[0061] 进一步地，将θr(k)， 和 可以写为如下形式
[0062]
[0063]
[0064]
[0065] 其中，Eθ＝[0,0,1,0]T， Ev＝[0,1,0,0]T。
[0066] 将式(10)代入(16)-(18)，可得
[0067]
[0068]
[0069]
[0070] 这样，约束(12)-(14)可以转化为关于优化变量u0,u1,...,uk-1的线性不等式约束，如下
[0071]
[0072]
[0073]
[0074]
[0075]
[0076]
[0077] (3)拟凸优化
[0078] 假设系统在t＝knT时到达目标状态pf，其中kn为足够大的正整数，并满足如下关系[0079]
[0080] 每个可行控制序列对应的运动时间ft可定义如下
[0081]
[0082] 其中， 即为优化问题的目标函数，km为最优时间指数。
[0083] 综上，优化问题可以推导如下
[0084]
[0085] 其中，
[0086]
[0087] 对于i＝0,1,...,km-1，ui满足-umax≤ui≤umax；对于i＝km,km+1,...,kn，ui满足ui＝0。
[0088] 因为目标函数是拟凸的，而相关约束是凸的，故上述优化问题是标准的拟凸优化问题，其最优解可通过二分法并结合求解一系列凸可行性问题求得。本发明中给定θmax＝0.1， vmax＝0.15,umax＝0.1,T＝0.05,q0＝[0,0,0,0]T,qf＝[3,3,π/4,0]T,kn＝
650,则可求得最优参考控制输入如图2(a)中的点线所示，对应的最优时间指数为km＝586。
另外，一旦最优参考控制输入u被求出，对应的状态轨迹，如纵向位移、纵向速度即可通过式(10)求出，如图2(b)和2(c)中的点线所示。
[0089] 由于上面计算得到的最优参考控制输入是一系列离散点，而不具有解析表达式，这给后续跟踪控制器的设计造成了障碍；另外，如果以离散点的形式存储最优轨迹，将给一般的嵌入式系统造成显著的存储负担，特别是当T取较小值或xvf取较大值时。为此，本发明在特征点识别的基础上，提出了一种具有自适应细化特性的B样条插值算法，以最少的插值点获得光滑的、具有解析表达式的最优时间轨迹。
[0090] 理论上，只要已知参考控制输入u，即可根据式(5)求得参考加速度 而参考速度和位移可通过对 进行一次和两次积分获得。然而，通过B样条自适应插值算法获得的解析表达式必然存在一定的误差，对 进行积分将放大这种误差，从而导致明显的运动控制误差。因此，为了确保控制精度，参考位移和参考速度轨迹也需要通过上述B样条自适应插值算法独立获得。
[0091] (1)特征点识别
[0092] 特征点，如端点、局部曲率极值点、拐点等，在表征曲线形状方面具有重要作用，这为从一系列稠密点中选取少量点插值得到解析表达式提供了方便。从图2(a)可以看出，两个端点和一些大斜率点对参考控制输入轨迹的形状具有重要影响；从图2(b)和图2(c)可以看出，参考速度和位移轨迹的形状主要受两个端点和一些曲率极值点影响。因此，这些特征点将被选出作为B样条曲线的初始插值点。由于两个端点可以很容易的获得，故下面只介绍大斜率点和曲率极值点的获取方法。
[0093] 1)大斜率点的获取方法：对于求解优化问题(30)所得的有序点序列点Qi(i＝1,2,...,km-2)处的斜率可以通过中心差分法求得，如下：
[0094]
[0095] 通过式(31)求得各点处的斜率后，则可通过如下方式选取大斜率点：给定一个正的阈值δ，当各点的斜率满足 时，该点被选择为大斜率点。利用上述方法求得的各点斜率以及识别出的特征点如图3所示。
[0096] 2)曲率极值点的获取方法：以位移时间最优离散轨迹为例，对于求解优化问题(30)所得的有序点列 点Qi′(i＝1,2,...,km-2)处的曲率可以通过过Qi′-1，Qi′和Qi′+1这三点的圆弧的曲率近似代替，并通过如下公式进行计算：
[0097]
[0098] 其中， 是由Qi′-1，Qi′和Qi′+1这三点组成的有符号三角形的面积；ψi为线段和线段 之间的夹角；Mi，Mi+1和Ni分别为线段 和 的长度。如图4所示，当Qi′-1，Qi′和Qi′+1这三点逆时针分布时， 的符号为正；否则，其符号为负。
[0099] 通过式(32)计算出相应的曲率ρi(i＝1,2,...,km-2)后，选取满足如下条件的点为曲率极值点:ρi＞ρi-1,ρi＞ρi+1或ρi＜ρi-1,ρi＜ρi+1，其中， 利用上述方法求得的曲率极值点如图5(b)所示，类似地，也可求出速度时间最优离散轨迹中的曲率极值点，如图5(a)所示。
[0100] (2)B样条插值曲线
[0101] B样条曲线可以表达为
[0102]
[0103] 其中，Ci(i＝0,1,2,...,n)为控制点，Ni,m(x)(i＝0,1,2,...,n)为定义在非降节点序列X＝[x0,x1,...,xn+m+1]上的m次B样条基函数，满足xi≤xi+1,i＝0,1,...,n+m。
[0104] 根据德布尔-考克斯递推公式，并约定0/0＝0，则m次B样条基函数可以给定如下[0105]
[0106] 从递推公式(34)可以看出，计算基函数Ni,m(x)，需要m+2个节点xi,xi+1,...,xi+m+1，也即基函数Ni,m(x)的支承空间为[xi,xi+m+1]。因此，在区间x∈[xi,xi+1]上至多有m+1个非零的m次B样条基函数Nj,m(x)(j＝i-m,i-m+1,...,i)，则式(33)可以进一步写作[0107]
[0108] 进一步地，m次B样条曲线的导数可以通过下式进行计算
[0109]
[0110] 为了方便B样条基函数的计算，引入如下符号：
[0111] L[j]＝x-xi+1-j,R[j]＝xi+j-x,j＝0,1,2,... (37)
[0112] 然后，根据式(34)和式(37)，m+1个m次B样条基函数可通过如下公式进行计算[0113]
[0114] 其中，Ni-m,m-1(x)＝0,Ni+1,m-1(x)＝0,Ni,0(x)＝1,x∈[xi,xi+1]。
[0115] 接下来，根据前面识别出的特征点集 可以反算出控制顶点。为此，采用向心参数化法对 进行参数化：
[0116]
[0117] 其中，
[0118] 则节点序列X＝[x0,x1,...,xr+2m]可通过以下公式进行计算
[0119]
[0120] 在此基础上，令B样条曲线恰好插值于X中各节点处，这就意味着，在任意插值点xi(i＝m,m+1,...,m+r)处最多有m个非零的m次B样条基函数Nj,m(x)(j＝i-m,i-m+1,...,i-1)。本发明中，令m＝3，则根据式(35)和特征点集 可构建如下(r+1)×(r+3)的方程组：
[0121]
[0122] 其中， k＝0,1,2,...,r，根据式(37)和(38)，相关基函数可通过下式进行计算：
[0123]
[0124]
[0125]
[0126]
[0127] 其中，i＝3,4,...,r+3。
[0128] 然而，需要说明的是，在式(41)中，有r+3个未知控制顶点，却只有r+1个方程，这对反算控制顶点造成了障碍。为此，给定两个端点处的一阶导数P(1)(x3)和P(1)(xr+3)来获得两个辅助方程，其中，P(1)(x3)和P(1)(xr+3)的方向给定为水平方向，而模长给定为d/2。这样，就可以根据式(36)获得如下两个辅助方程：
[0129]
[0130] 基于式(41)-(43)，就可以计算出r+3个控制点 然后根据式(35)就可以顺利的获得B样条插值曲线。
[0131] (3)自适应曲线细化
[0132] 初始插值曲线如图2(a)-2(c)中的点画线所示，虽然其可以反映相应最优轨迹的一些特征，但明显存在较大误差。因此，为了确保B样条曲线可以更好的逼近 中除插值点 外的其它点，初始B样条插值曲线还需要通过将最大误差点加入特征点集的方法不断细化。
[0133] 考虑到 因此，根据式(35)可得如下方程
[0134]
[0135]
[0136] 定义离散点列Qi和对应的插值曲线上的点P(xi)之间的偏差的绝对值为拟合误差，即
[0137] ei＝|ui-u(ti)|,i＝0,1,...,km-1 (46)
[0138] 其中，u(ti)表示由式(45)计算出的u(x*)，x*可根据式(44)并结合t(x)＝ti反算得到。
[0139] 如果ei的最大值比允许误差ε大，也即， 则相应的最大误差点就被添加到特征点集 中，并重新生成插值曲线。如此循环，直到所得B样条插值曲线满足给定的拟合误差要求。
[0140] 需要说明的是，用上述方法同样可得到满足给定精度的参考速度和位移的时间最优解析轨迹，这里不再赘述。
[0141] 本发明中给定ε＝0.003，则最大拟合误差κ随细化次数η的变化如图6所示。从图中可以看出，最大拟合误差随细化次数的增加而快速减小，直到满足给定精度。最终的插值曲线，也就是具有解析表达式的时间最优轨迹如图2(a)-(c)中的实线所示，从图中可以看出，所生成的插值曲线与相应的最优离散点列能够很好的吻合，从而证明了所提出的B样条自适应插值算法的有效性。
[0142] 在获得具有解析表达式的纵向加速度、速度和位移的时间最优参考轨迹后，采用常用的PID控制器或设计其它性能更加优良的轨迹跟踪控制器，使两轮自平衡车的纵向加速度、速度和位移跟踪相应的时间最优参考值，即可实现两轮自平衡车的直线定点运动的最优时间控制。
[0143] 以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明披露的技术范围内，根据本发明的技术方案及其发明构思加以等同替换或改变，都应涵盖在本发明的保护范围之内。
