两轮自平衡车主动避障轨迹规划与稳定跟踪控制方法

摘要

本发明涉及一种两轮自平衡车主动避障轨迹规划与稳定跟踪控制方法，包括基于圆弧和过渡曲线的避障轨迹规划方法与基于非奇异终端滑模和嵌套饱和算法的稳定跟踪控制方法。所述局部轨迹规划方法所规划的轨迹包括换道轨迹、超越轨迹和并道轨迹。所述非奇异终端滑模算法控制两轮自平衡车的全驱动转向子系统，所述嵌套饱和算法控制两轮自平衡车的欠驱动前向子系统。本发明所规划的避障轨迹，具有轨迹曲率变化平缓，便于欠驱动两轮自平衡车稳定跟踪的特点；所提出的稳定跟踪控制算法，能够使两轮自平衡车在较大车身倾角初值下，稳定跟踪所规划的避障轨迹，同时保持车身稳定，从而实现两轮自平衡车的主动避障。

说明书

技术领域

本发明属于两轮自平衡车控制领域，具体而言是一种两轮自平衡车主动避障轨迹规划与稳定跟踪控制方法。

背景技术

两轮自平衡车作为单级倒立摆系统的扩展，不仅是验证相关控制理论的理想实验平台，而且在休闲娱乐、医疗器械、空间探测等诸多方面具有重要的实际应用价值。能否主动规避行进过程中遇到的障碍物，是两轮自平衡车智能化程度高低的重要标志之一。

目前，关于避障控制的研究主要针对全驱动机器人，而对于欠驱动两轮自平衡车主动避障方面的研究鲜有报道。例如，申请号为201610118977.2的中国专利，提出了一种“自平衡两轮平衡车及控制方法”，该平衡车主要包括左右车轮、横梁和移动平台，通过平衡驱动电机自动调节移动平台，使自平衡车自动调整重心，保持平衡，并未涉及两轮自平衡车的避障控制。申请号为201610573390.0的中国专利，提出了一种“移动机器人路径规划和避障方法及系统”，根据障碍物信息建立二维栅格地图，采用跳点搜索算法获取最短路径，从而实现移动机器人的自主导航。但该方法主要针对全驱动机器人，欠驱动两轮自平衡车难以稳定跟踪此方法规划出的轨迹。申请号为201410448977.X的中国专利提出了一种“基于模糊控制的两轮自平衡机器人避障系统及控制方法”，其系统包括姿态检测模块、运动控制模块和运动执行模块，通过遥控器按键向两轮自平衡机器人发送控制指令，实现其前进、后退、自旋、定点平衡。机器人根据障碍物距离信息和模糊规则，来做出对应的转向，实现避障。该专利的实施需要借助遥控器，无法实现两轮自平衡机器人的主动避障。

综上，现有控制方法难以实现两轮自平衡车的主动避障控制。

发明内容

针对上述问题，本发明的目的是提供一种两轮自平衡车主动避障轨迹规划与稳定跟踪控制方法，使得两轮自平衡车在行进过程中遇到障碍物时，可以无碰撞的主动避开障碍物，并回到既定行驶路线上，同时保持车身稳定。

为实现上述目的，本发明采取以下技术方案：两轮自平衡车主动避障轨迹规划与稳定跟踪控制方法，包括：基于圆弧和过渡曲线的避障轨迹规划方法、基于非奇异终端滑模和嵌套饱和算法的稳定跟踪控制方法；所述避障轨迹规划方法规划出的轨迹包括换道、超越和并道三部分；所述稳定跟踪控制方法，首先将两轮自平衡车实际位姿对于避障轨迹所确定的参考位姿的跟踪,在考虑非完整约束的情况下，转化为实际前向速度和转向角速度对于期望前向速度和转向角速度的跟踪，然后将整个两轮自平衡车系统解耦为全驱动转向子系统和欠驱动前向子系统分别进行控制。

进一步的，换道轨迹，包括第1过渡轨迹、第1圆弧轨迹、第2过渡轨迹、第2圆弧轨迹、第3过渡轨迹；所述第1过渡轨迹分别与避障开始前的直线行驶轨迹和第1圆弧轨迹连接，其表达式为x(t)＝v₀t，以避障开始点位置为大地坐标系原点，以避障开始时刻为零时刻，其中，v₀为两轮自平衡车的前向速度，0≤t≤t₁，R为第1圆弧轨迹的曲率半径，J为两轮自平衡车所允许的最大横向加加速度；所述第2圆弧轨迹与第1圆弧轨迹的曲率半径R及所对应的圆心角ψ大小相等，所述第2圆弧轨迹与第1圆弧轨迹的凹向相反，其轨迹表达式根据匀速圆周运动的参数方程确定；所述第2过渡轨迹分别与第1圆弧轨迹和第2圆弧轨迹连接，其表达式为

t₁+t₂＜t≤t₁+t₂+t₃

其中，t₁、t₂、t₃分别为第1过渡轨迹、第1圆弧轨迹和第2过渡轨迹所用时间，ψ根据确定，δ为安全余量，W_b和L_v分别为为两轮自平衡车和障碍物的尺寸参数；

所述第3过渡轨迹分别与第2圆弧轨迹和超越轨迹连接，其轨迹表达式为

x(t)＝L+2Rsinψ+2Lcosψ+v₀(t-t₁-t₂-t₃-t₄)

t₁+t₂+t₃+t₄＜t≤t₁+t₂+t₃+t₄+t₅

其中，t₄、t₅分别为第2圆弧轨迹和第3过渡轨迹所用时间；

超越轨迹为一直线，其轨迹表达式根据匀速直线运动方程确定；

并道轨迹与换道轨迹关于超越轨迹的中垂线对称。

进一步的，所述稳定跟踪控制方法，首先将两轮自平衡车实际位姿对于规划出的避障轨迹所确定的参考位姿的跟踪，在考虑非完整约束的情况下，转化为实际前向速度v和转向角速度ω对于期望前向速度v_d和转向角速度ω_d的跟踪；其中，x、y为两轮自平衡车在大地坐标系下的坐标，为转向角，为x的一阶导，为y的一阶导，

v_d＝v_r>3+λ₁tanh>1

其中，x_r、y_r为避障轨迹在大地坐标系下的坐标，为避障轨迹所确定的参考转向角，为x_r的一阶导，为y_r的一阶导，λ₁、λ₂和λ₃均为正的设计参数；当e₃＝0时，

进一步的，非奇异终端滑模算法控制两轮自平衡车的全驱动转向子系统，其控制律由确定；其中和分别为期望角速度ω_d的积分、ω_d和ω_d的一阶导，β＞0，η＞0，p、q均为正奇数，且m_w为车轮质量，r为车轮半径，d为轮距，I₁为车轮绕其直径的转动惯量，I₂为车轮绕其轮轴的转动惯量，I₃为底盘绕过其质心的竖直线的转动惯量，I₅为车身绕过其质心的竖直线的转动惯量，τ_L、τ_R分别为左右电机的输出力矩；函数sat(x)满足如下定义：当时，当时，sat(x)＝1；当时，sat(x)＝-1，φ为正常数。

进一步的，所述嵌套饱和算法控制两轮自平衡车的欠驱动前向子系统，其控制律由v_z＝-σ₄(y₄+σ₃(y₃+σ₂(y₂+σ₁(y₁))))确定，其中

ξ₁＝θ，z₃＝tanξ₁，z₄＝(1+tan²ξ₁)ξ₂，x_vd、和分别为v_d的积分、v_d和v_d的一阶导；θ为车身倾角，θ_d、分别为θ、的期望值，均为零；m_c为底盘质量，m_b为车身质量，L为车身质心到轮轴中心的距离，I₄为底盘绕轮轴的转动惯量，I₆为车身绕轮轴的转动惯量；线性饱和函数σ(x)：R→R是连续非降函数,且满足如下条件：当|x|＜A时，σ(x)＝x；当x≥A时，σ(x)＝A；当x≤-A时，σ(x)＝-A；A称为σ的幅值。

更进一步的，第2过渡轨迹所用时间t₃＝2t₁，第3过渡轨迹所用时间t₅＝t₁；第2圆弧轨迹所用时间t₄与第1圆弧轨迹所用时间t₂相等，且

更进一步的，当L_b≤2(x_E-S₀)时，超越轨迹长度为零；否则，超越轨迹长度为L_b-2(x_E-S₀)；其中，x_E为换道结束时的两轮自平衡车的水平位移，L_b为障碍物尺寸参数，S₀为避障开始时两轮自平衡车与障碍物的距离。

更进一步的，避障过程中，在车体坐标系下，两轮自平衡车的横向加速度随时间变化为正反梯形，前向加速度近似为零。

作为更进一步的，避障开始点与障碍物的最小距离其中，x_c为两轮自平衡车恰好与障碍物接触时的水平位移，α为此时轨迹切线与水平方向的夹角，L_v为两轮自平衡车尺寸参数。

本发明的有益效果是：：1、所提出的基于圆弧和过渡曲线的避障轨迹规划方法，在车体坐标系下，两轮自平衡车的横向加速度随时间变化为正反梯形，可以保证轨迹曲率的连续变化；而前向加速度近似为零，这个特点使欠驱动两轮自平衡车可以相对容易的实现对所规划轨迹的稳定跟踪。2、提出了一种基于非奇异终端滑模技术和嵌套饱和方法的两轮自平衡车稳定轨迹跟踪控制方案，能够实现两轮自平衡车在较大车身倾角初始值的情况下，稳定的跟踪所规划出的避障轨迹。

附图说明

本发明共有附图9幅：

图1是本发明中所涉及两轮自平衡车的结构图；

图2是避障前两轮自平衡车与障碍物的位置关系图；

图3是本发明所规划出的避障轨迹；

图4是避障过程中两轮自平衡车在车体坐标系下的横向加速度与时间的关系；

图5是避障过程中两轮自平衡车恰好与障碍物接触时的位置关系；

图6是本发明的控制原理框图；

图7是车身初始倾角为0.1时实施例的仿真结果；

图8是车身初始倾角为0.2时实施例的仿真结果；

图9是车身初始倾角为0.3时实施例的仿真结果；

具体实施方式

为了使本发明的目的、技术方案和优点更加清楚，下面结合附图和具体实施例对本发明进行详细描述。

下面结合附图和实施例对本发明进行详细的描述。

如图1所示，本发明所涉及的两轮自平衡车主要包括车轮1、底盘2和车身3等部件。如图2所示，假设两轮自平衡车4沿直道一侧以匀速v₀向右行驶，正前方有一障碍物5，道路足够宽。障碍物的尺寸用其在水平面投影的外接矩形的尺寸表示，其上侧与两轮自平衡车初始直线轨迹的距离为W_b，长度为L_b。两轮车在水平面上的投影也看作矩形，其长为L_v，宽为W_v。

一、推导两轮自平衡车所受非完整约束、运动学模型和动力学模型，分别如下：

其中，x、y为两轮自平衡车在大地坐标系中的坐标，为转向角，θ为车身倾角，m_w为车轮质量，m_c为底盘质量，m_b为车身质量，r为车轮半径，d为轮距，L为车身质心到轮轴中心的距离，I₁为车轮绕其直径的转动惯量，I₂为车轮绕其轮轴的转动惯量，I₃为底盘绕过其质心的竖直线的转动惯量，I₄为底盘绕轮轴的转动惯量，I₅为车身绕过其质心的竖直线的转动惯量，I₆为车身绕轮轴的转动惯量，τ_L、τ_R分别为左右电机的输出力矩。

二、根据传感器测得的障碍物形位信息，规划避障轨迹。以开始换道的位置为原点建立大地坐标系xOy，以开始换道的时刻为零时刻,如图3所示，所述避障轨迹主要包括三部分：换道轨迹OE、超越轨迹EF和并道轨迹FK。

所述换道轨迹，主要包括第1过渡轨迹OA、第1圆弧轨迹AB、第2过渡轨迹BC、第2圆弧轨迹CD、第3过渡轨迹DE。第1圆弧轨迹和第2圆弧轨迹所对应的圆心角均为ψ，曲率半径均为R。W为避障过程中的最大横向位移，可由下式确定：

其中，δ为安全余量。

OA段的轨迹表达式为

其中，J为两轮自平衡车所允许的最大横向加加速度。

根据(5)式求得前向速度为横向加速度为若取R＝1000，v₀＝2，J＝2(均采用国际单位，下同)，可得t₁＝0.002，y_r(t)∈[0,0.0000000027]≈0，v_r(t)∈[2,2.000000000004]≈v₀，

显然，两轮自平衡车在此过渡曲线上，近似沿x轴做匀速直线运动；同时，通过此过渡曲线，可以使横向加速度从零平滑过渡到轨迹曲率从零平滑过渡到1/R。

BC段的轨迹表达式为

t₁+t₂＜t≤t₁+t₂+t₃；其中，t₁、t₂、t₃分别为OA、AB和BC段所用时间，t₃＝2t₁，ψ根据确定。

类似于对OA段轨迹表达式的分析，可以发现，两轮自平衡车在此过渡曲线上，近似沿两圆弧的公切线做匀速直线运动；同时，通过此过渡曲线，可以使横向加速度从平滑过渡到对应轨迹曲率从1/R平滑过渡到-1/R。

DE段轨迹表达式可类比OA段轨迹表达式确定，DE段所用时间t₅＝t₁，AB段和CD段的轨迹表达式可根据匀速圆周运动的参数方程确定，CD段所用时间t₄＝t₂。从而，整个换道轨迹表达式都得以确定。

设开始避障时，两轮自平衡车与障碍物左边沿的距离为S₀。若障碍物的长度L_b满足L_b≤2(x_E-S₀)，超越轨迹长度为零；否则，超越轨迹不为零，即两轮自平衡车沿直线水平向右匀速移动一段距离，设此过程结束时刻为t_F，则相应的轨迹表达式为

其中，x_E为换道结束时的水平位移，t_E为整个换道过程所用时间。

并道轨迹与换道轨迹关于超越轨迹的中垂线对称，其轨迹表达式类比换道轨迹表达式确定。

整个避障过程中，在车体坐标系下，两轮自平衡车的横向加速度a_n随时间变化为正反梯形，如图4所示；而前向加速度近似为零。

三、确定最小安全距离S_min，也即避障开始时与障碍物左边沿的最小距离。

如图5所示，设在换道过程中，t_c时刻两轮车右侧恰好与障碍物前左上角接触，此时轨迹切线与水平方向的夹角为α，两轮自平衡车沿x轴和y轴的位移分别为x_c和y_c。

根据图5中的几何关系，可得：

当时，接触点在避障轨迹的第一段圆弧上，此时

当时，接触点在两段圆弧之间的过渡曲线上，此时

α＝ψ(10)

当时，接触点在第二段圆弧上，根据式(8)、(9)和(10)以及避障轨迹表达式，即可求得x_c和α的值。

最终可以得到最小安全距离为

四、稳定跟踪控制。

所规划出的避障轨迹确定了两轮自平衡车的参考位姿p_r＝[x_r,y_r,φ_r]^T，设两轮车的当前位姿为p＝[x,y,φ]^T，其与参考位姿的误差为p_e＝[e₁,e₂,e₃]^T，其中

设避障轨迹所确定的前向速度参考值为v_r，转向角速度参考值为ω_r。利用李雅普诺夫直接法容易证明，若两轮自平衡车的前向速度v和转向角速度ω对应取(13)式确定的v_d和ω_d，则对于任意的初始位姿误差p_e，有

其中，λ₁、λ₂和λ₃均为正的设计参数；当e₃＝0时，

1.转向子系统控制

根据动力学模型(3)，可得二阶转向子系统方程为

其中，τ_ω＝τ_R-τ_L。

设和分别是由(13)式确定的ω_d的积分、ω_d和ω_d的一阶导，定义取非奇异终端滑模面和控制律分别为

其中，β＞0，η＞0，p、q均为正奇数，且函数sat(x)满足如下定义：当时，当时，sat(x)＝1；当时，sat(x)＝-1。φ为正常数。

利用李雅普诺夫直接法并结合相平面分析，容易证明，采用控制律(16)，可以使和在有限时间内收敛到零。

2.前向子系统控制

根据动力学模型(3)可得欠驱动前向子系统方程为

其中，x_v为车体坐标系下两轮自平衡车的前向位移，g为重力加速度。

令

z₃＝tanξ₁，z₄＝(1+tan²ξ₁)ξ₂。其中x_vd、和分别是由(13)式确定的v_d的积分、v_d和v_d的一阶导；θ、的期望值θ_d、均为零；

则定义偏差变量e_z1＝z₁-z_1d，e_z2＝z₂-z_2d，e_z3＝z₃，e_z4＝z₄，经过推导，可得偏差系统

其中，

由于在规划出的避障轨迹上v_r为定值，而且非奇异终端滑模控制可以使转向角误差e₃快速收敛于零，tanhe₁∈(-1,1)，故当λ₁取为很小值时，从(13)式可以看出v_d≈v_r，进而有式(19)变为如下前馈系统：

利用李雅普诺夫直接法容易证明：存在线性变换y_i:R⁴→R和幅值为A_i(A_i足够小)的线性饱和函数σ_i，其中i＝1,2,3,4，采用如下嵌套饱和控制律

v_z＝-σ₄(y₄+σ₃(y₃+σ₂(y₂+σ₁(y₁))))(21)

可以使系统(20)全局渐近稳定到原点。

其中，y₄＝e_z4。

线性饱和函数σ：R→R是连续非降函数,且满足如下条件：当|x|＜A时，σ(x)＝x；当x≥A时，σ(x)＝A；当x≤-A时，σ(x)＝-A。A称为σ的幅值。

综上，所述基于非奇异终端滑模和嵌套饱和算法的控制方法可以使两轮自平衡车稳定跟踪所规划出的避障轨迹，整个系统的控制框图如图7所示。

五、仿真

选取如下仿真参数：R＝1000,L_b＝40，W_b＝3，L_v＝0.7，W_v＝0.3，δ＝0.2，J＝2,v₀＝2，m_w＝1，m_c＝5，m_b＝3，d＝0.6，L＝1，r＝0.15，I₁＝1,I₂＝1.5,I₃＝1,I₄＝4,I₅＝0.5,I₆＝1，g＝9.8；考虑驱动电机的输出饱和，两个车轮的驱动电机的最大扭矩均取为35，也即|τ_v|≤70，|τ_ω|≤70；两轮自平衡车的位姿误差初值e₁＝0、e₂＝0、e₃＝0.1，车身倾角角速度初值根据以上参数，求得ψ＝0.0596，x_E＝119.216S_min＝99.119，考虑一定的安全余量，当传感器检测到两轮车距离障碍物为S₀＝105时开始执行避障。又L_b＝40＞2(x_E-S₀)＝28.432，故超越轨迹长度不为零。车身倾角初值分别为θ＝0.1、θ＝0.2和θ＝0.3时的仿真结果如图7、图8和图9所示。可以看出，所规划出的避障轨迹(x_r,y_r)曲率光滑，能够安全的避开障碍物，并回到既定行驶路线上；所采用的稳定跟踪控制方法能够使两轮自平衡车在较大车身倾角的情况下，实际轨迹(x,y)稳定跟踪所规划出的避障轨迹(x_r,y_r),各状态变量的跟踪误差e_x,e_y,θ,均能在短时间内收敛于零。从而验证了所提出的两轮自平衡车主动避障轨迹规划与稳定跟踪控制方法的有效性。

本发明所规划的避障轨迹，在车体坐标系下，横向加速度随时间变化为正反梯形，前向加速度为零，具有轨迹曲率变化平缓，便于欠驱动两轮自平衡车稳定跟踪的特点；所提出的稳定跟踪控制算法，能够使两轮自平衡车在较大车身倾角初值下，稳定跟踪所规划的避障轨迹，同时保持车身稳定，从而实现两轮自平衡车的主动避障。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明披露的技术范围内，根据本发明的技术方案及其发明构思加以等同替换或改变，都应涵盖在本发明的保护范围之内。