1.本发明涉及轮式移动机器人控制技术领域，特别是涉及含姿态规划的轮式移动机器人位置控制方法、系统及设备。
背景技术：
2.在现有的分层控制系统中,采用atan2可以得到动点的辅角，但由于atan2(0,-1)＝
±
π，动点穿过x轴左半轴时，辅角函数会短时间内在-π和π之间剧烈震动，这使得分层控制系统相应的控制器中出现杂乱的脉冲信号，从而出现稳定性降低的现象，因而难以实现精准的控制效果。
技术实现要素：
3.本发明实施例的目的是提供一种含姿态规划的轮式移动机器人位置控制方法、系统及设备，避免了传统求幅角主值方法产生脉冲导数的计算灾难，改善了系统的瞬态性能，实现了跟踪误差的指数收敛性，提高了系统抗外界干扰的能力，提高了机器人位置的控制精度。
4.为实现上述目的，本发明实施例提供了如下方案：
5.一种含姿态规划的轮式移动机器人位置控制方法，包括：
6.获取机器人质心坐标历史数据集、偏航角历史数据集、前进速度历史数据集和转向角速度历史数据集；实时获取机器人质心坐标、偏航角、前进速度和转向角速度；
7.采用动力学分析方法对所述机器人质心坐标历史数据集、机器人偏航角历史数据集、机器人前进速度历史数据集和机器人转向角速度历史数据集进行运动学计算、车轮力学计算和车体力学计算，得到非完整机器人动力学模型；
8.采用切换和坐标变换的方法对机器人移动过程中的质心动点坐标进行设计，并首次给出计算动点幅角的光滑函数模型atan3(y,x)；
9.根据所述非完整机器人动力学模型和幅角的光滑函数模型atan3(y,x)，结合期望控制构建光滑姿态规划器，得到期望的幅角和幅值；对所述期望的幅角和幅值进行滤波，得到滤波后的期望幅角和期望幅值；
10.对所述滤波后的期望幅角和期望幅值进行位置计算和姿态计算，得到位置控制外环设计结果和姿态控制内环设计结果；
11.根据所述位置控制外环设计结果和姿态控制内环设计结果，构建含姿态规划的轮式移动机器人位置控制模型；将所述机器人质心坐标、偏航角、前进速度和转向角速度输入所述含姿态规划的轮式移动机器人位置控制模型，得到含姿态规划的机器人位置控制结果。
12.可选地，采用动力学分析方法对所述机器人质心坐标历史数据集、机器人偏航角历史数据集、机器人前进速度历史数据集和机器人转向角速度历史数据集进行运动学计算、车轮力学计算和车体力学计算，得到非完整机器人动力学模型，具体包括：
13.对所述机器人质心坐标历史数据集、偏航角历史数据集、前进速度历史数据集和转向角速度历史数据集进行运动学计算，得到运动学模型；具体公式如下：
[0014][0015]
其中，(x,y)为所述机器人质心坐标，ψ为机器人偏航角，v为机器人前进速度，ω为机器人转向角速度，为x的一阶导数，为y的一阶导数，为ψ的一阶导数；
[0016]
对所述运动学模型进行车轮力学计算，得到车轮力学模型；具体公式如下：
[0017][0018][0019]
其中，表示地面对左轮前向的约束反力，表示地面对右轮前向的约束反力；j1为左轮的转动惯量，j2为右轮的转动惯量，j1＝j2；φ1为左轮的角位移，φ2为右轮的角位移；τ1为左轮电机提供的力矩，τ2为右轮电机提供的力矩；r为每个轮的直径，l为机器人质心坐标到左轮面或右轮面的距离；为左轮的角位移的一阶导数，为右轮的角位移的一阶导数；为左轮的角位移的二阶导数，为右轮的角位移的二阶导数；
[0020]
对所述运动学模型进行车体力学计算，得到车体力学模型；具体公式如下：
[0021][0022]
其中，k1为平动阻尼系数，k2转动阻尼系数，m为机器人车体的质量，表示地面对左轮和右轮侧向的约束反力的合力；为x的二阶导数，为y的二阶导数，为ψ的二阶导数；j＝j1＝j2；
[0023]
采用动力学分析方法对所述运动学模型、车轮力学模型和车体力学模型进行计算，得到所述非完整机器人动力学模型；具体公式如下：
[0024]
[0025]
其中，为v的一阶导数，为ω的一阶导数，r＝(x,y)
t
；f为两轮的平均力矩，γ为半力矩差，p1,p2,k1,k2为与轮式移动机器人参数相关的常系数。
[0026]
可选地，采用切换和坐标变换的方法对机器人移动过程中的质心动点坐标进行设计，首次给出计算动点幅角的光滑函数模型atan3(y,x)；根据所述非完整机器人动力学模型和幅角的光滑函数模型atan3(y,x)，结合期望控制构建光滑姿态规划器，得到期望的幅角和幅值，具体包括：
[0027]
根据值域在[-π,π)的函数atan2(y,x)得到幅角主值；具体公式如下：
[0028][0029]
atan2(0,0)＝0；
[0030]
建立坐标系xoy，机器人移动过程中的任一质心a的坐标为(x(t),y(t))；预设活跃区间[-2π/3,2π/3]，在xoy中利用atan2计算点(-1,0)的幅角主值；根据所述幅角主值得到xoy坐标系中的活跃窗口；
[0031][0032]
第一次离开d的时间为：
[0033]
τ1＝inf{t≥τ0:a(t)∈dc},τ0＝0；dc表示集合d的补集；
[0034]
在时刻τ1，将坐标系xoy绕o点旋转180
°
，得到坐标系x
′
oy
′
；机器人移动过程中的任一质心a(t)的坐标为为a
′
(t)＝(x
′
(t),y
′
t))＝-(x(t),y(t))；
[0035]
第一次离开d
′
的时间为：
[0036]
τ2＝inf{t≥τ1:a
′
(t)∈d
′c}；
[0037]
其中d
′
为x
′
oy
′
坐标系中的活跃窗口；
[0038][0039]
根据任意一个机器人车体的质量m，得到机器人移动过程中的任一质心坐标a在t时刻在坐标系xoy和坐标系x
′
oy
′
的复合坐标系中幅角主值的计算公式:
[0040]
τ
2m-1
＝inf{t≥τ
2m-2
:a(t)∈dc},
[0041]
τ
2m
＝inf{t≥τ
2m-1
:a
′
(t)∈d
′c},
[0042][0043]
预设采样时间的步长等于δ，tk＝kδ，第一计数器σ(t)具体为:
[0044][0045]
其中，σ(t)＝1对应坐标系xoy，σ(t)＝-1对应坐标系x
′
oy
′
，i为示性函数，φ
(t)＝atan2[(y(t),x(t))]，φ-(t)＝atan2[(y(t),x(t))*(-1)]；
[0046]
预设第二计数器s(t)；计算移动质心坐标穿出[-2π/3,2π/3]的次数的代数和；具体公式如下：
[0047][0048]
s(t)具体为：
[0049][0050]
首次给出计算机器人移动过程中质心动点坐标幅角的光滑函数模型atan3(y,x)，具体公式如下：
[0051]
ψ(t)＝atan3[y(t),x(t)]＝φ(t) s(t)π；
[0052]
其中，atan3中的3表示继atan2之后对atan的再次推广；
[0053]
根据所述非完整机器人动力学模型和幅角的光滑函数模型atan3(y,x)，结合期望控制构建光滑姿态规划器，得到期望的幅角和幅值，具体包括：
[0054]
根据非完整机器人动力学模型，通过引入虚拟位置控制μ，得到位置运动模型；具体公式如下：
[0055][0056]
μ＝vr(ψ)；
[0057][0058]
其中，为r的一阶导数，r(ψ)＝(cosψ,sinψ)
t
，取决于和其中表示全体实数集；根据虚拟位置控制，得到期望位置控制向量
[0059]
μd＝vdr(ψd)；
[0060]
其中，ψd为待设计的偏航角，vd为期望的平动速度，μd的分量计算公式如下：
[0061]
μ
dx
＝v
d cosψd,μ
dy
＝v
d sinψd；
[0062]
其中，μ
dx
,μ
dy
是向量μd的分量；
[0063]
根据(μ
dx
,μ
dy
)计算期望的平动速度向量vd和转动方向向量ψd，得到期望速度模型；
[0064][0065]
根据所述幅角的光滑函数模型atan3(y,x)和期望位置控制向量，得到光滑姿态规划器；
[0066]
ψd(t)＝atan3(μ
dx
(t),μ
dy
(t))。
[0067]
可选地，对所述期望的幅角和期望幅值进行滤波，得到滤波后的期望幅角和期望幅值，具体包括：
[0068][0069][0070]
其中，为的一阶导数，为滤波后的期望幅角，为的一阶导数，为滤波后的期望幅值；滤波时间常数κ1＞0,κ2＞0。
[0071]
可选地，对所述滤波后的期望幅角和期望幅值进行位置计算和姿态计算，得到位置控制外环设计结果和姿态控制内环设计结果，具体包括：
[0072]
根据滤波后的期望幅值和期望位置r
*
(k)得到位置误差和速度误差
[0073][0074][0075]
其中，
[0076][0077][0078][0079]
其中，为位置误差的一阶导数，为滤波后的期望位置控制向量；
[0080]
定义位置误差李雅普诺夫函数vr，根据链式求导法则，得到其时间导数；具体公式如下：
[0081][0082][0083][0084]
其中，为vr的一阶导数；
[0085]
根据李雅普诺夫稳定性定理，得到期望位置控制律为：
[0086][0087]
其中，cr和d为正的设计参数；
[0088]
定义平动系统李雅普诺夫函数v1，根据链式求导法则，得到其时间导数；具体公式如下：
[0089][0090][0091]
根据李雅普诺夫稳定性定理取平动控制，得到位置控制外环设计结果；
[0092][0093]
其中，p1，cv和k1为正的设计参数；
[0094]
根据滤波后的期望幅角和期望角速度ωd，得到姿态误差和角速度误差
[0095][0096][0097]
定义姿态误差李雅普诺夫函数v
ψ
，根据链式求导法则，得到其时间导数；具体公式如下：
[0098][0099][0100]
根据李雅普诺夫稳定性定理，得到期望角速度为：
[0101][0102]
其中，c
ψ
为正的设计参数；
[0103]
定义转动系统李雅普诺夫函数v2，根据链式求导法则，得到其时间导数；具体公式如下：
[0104][0105][0106]
根据李雅普诺夫稳定性定理取转动控制，得到姿态控制内环设计结果；
[0107][0108]
其中c
ω
和p2为正的设计参数。
[0109]
可选地，根据所述位置控制外环设计结果和姿态控制内环设计结果，构建含姿态规划的轮式移动机器人位置控制模型；将所述机器人质心坐标、偏航角、前进速度和转向角速度输入所述含姿态规划的轮式移动机器人位置控制模型，得到含姿态规划的机器人位置控制结果，具体包括：
[0110][0111]
其中d，cr，p1，p2，c
ψ
，cv均为正的设计参数；
[0112]
根据所述位置控制外环设计结果和姿态控制内环设计结果，构建稳定性分析闭环误差系统；具体公式如下：
[0113][0114]
计算所述稳定性分析闭环误差系统的lyapunov函数，得到所述含姿态规划的机器人位置控制模型；将所述机器人质心坐标、偏航角、前进速度和转向角速度输入所述含姿态规划的机器人位置控制模型，得到含姿态规划的机器人位置控制结果v；
[0115]
v＝v1 v2；
[0116][0117]
一种含姿态规划的轮式移动机器人位置控制系统，包括：
[0118]
数据获取模块，用于获取机器人质心坐标历史数据集、偏航角历史数据集、前进速度历史数据集和转向角速度历史数据集；实时获取机器人质心坐标、偏航角、前进速度和转向角速度；
[0119]
非完整机器人动力学模块，与所述数据获取模块连接，用于采用动力学分析方法对所述机器人质心坐标历史数据集、机器人偏航角历史数据集、机器人前进速度历史数据集和机器人转向角速度历史数据集进行运动学计算、车轮力学计算和车体力学计算，得到非完整机器人动力学模型；
[0120]
幅角模块，分别与所述数据获取模块和所述非完整机器人动力学模块连接，用于采用切换和坐标变换的方法对机器人移动过程中的质心动点坐标进行设计，并首次给出计算动点幅角的光滑函数模型atan3(y,x)；
[0121]
光滑姿态规划器，与所述非完整机器人动力学模块和所述幅角模块连接，用于根据所述非完整机器人动力学模型和幅角的光滑函数模型atan3(y,x)，结合期望控制构建光滑姿态规划器，得到期望的幅角和幅值；
[0122]
滤波器，与所述光滑姿态规划器连接，用于对所述期望的幅角和幅值进行滤波，得到滤波后的期望幅角和期望幅值；
[0123]
计算模块，与所述滤波器连接，用于对所述滤波后的期望幅角和期望幅值进行位置计算和姿态计算，得到位置控制外环设计结果和姿态控制内环设计结果；
[0124]
输出模块，与所述计算模块连接，用于根据所述位置控制外环设计结果和姿态控制内环设计结果，构建含姿态规划的轮式移动机器人位置控制模型；将所述机器人质心坐标、偏航角、前进速度和转向角速度输入所述含姿态规划的机器人位置控制模型，得到含姿态规划的轮式移动机器人位置控制结果。
[0125]
一种电子设备，包括存储器、处理器以及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现所述的含姿态规划的轮式移动机器人位置控制方法。
[0126]
一种非暂态计算机可读存储介质，其上存储有计算机程序，所述计算机程序被执行时实现所述的含姿态规划的轮式移动机器人位置控制方法。
[0127]
在本发明实施例中，根据非完整机器人动力学模型和机器人质心坐标得到机器人移动过程中的质心坐标的幅角模型，基于质心坐标旋转和切换的求解移动质点的幅角代替函数atan2，消除震荡，实现控制器排除外界信号干扰。
[0128]
根据非完整机器人动力学模型和幅角模型，构建光滑姿态规划器，得到具有良好解析性质的光滑姿态规划器。光滑姿态规划器可以切换，弥补了brockett限制条件所带来的不足。
[0129]
光滑姿态规划器提高了控制器的机动性，将非完整移动机器人的控制问题转化为关于位置的全驱控制问题，也就是不直接控制姿态。与传统的结合lasalle不变集原理与零状态可检测性质的欠驱动控制器相比，含姿态规划的机器人位置控制结果具有较强的鲁棒性。
[0130]
由于位置控制外环设计结果(位置控制器)和姿态控制内环设计结果(姿态控制器)都利用了典型的反推方法，控制器设计易于与自适应观测器相结合，能够解决更复杂的控制问题，以提高了机器人位置的控制精度。
附图说明
[0131]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
[0132]
图1为本发明实施例提供的含姿态规划的轮式移动机器人位置控制方法的流程示意图；
[0133]
图2为本发明实施例提供的含姿态规划的轮式移动机器人位置控制系统的详细结构图；
[0134]
图3为本发明实施例提供的非完整移动机器人示意图；
[0135]
图4为本发明实施例提供的辐角主值示意图；a为xoy坐标系，b为坐标系x
′
oy
′
；
[0136]
图5为本发明实施例提供的的幅角与辐角主值示意图；a为质心坐标，b为xoy中的
幅角主值c为x'oy'中的幅角主值d为切换后的混合坐标系中的幅角主值e为质心移动的真实幅角σ(t)，f为质心移动的真实幅角σ(t)；
[0137]
图6为本发明实施例提供的控制器示意图；
[0138]
图7为本发明实施例提供的无人车在圆形场地的自动驾驶示意图；
[0139]
图8为本发明实施例提供的人车在圆形场地的仿真结果示意图；a为x,xd仿真结果，b为y,yd仿真结果，c为(x,y)仿真结果，d为ψ,ψd仿真结果，e为f仿真结果，f为γ仿真结果；
[0140]
图9为本发明实施例提供的无人车在方形场地的自动驾驶示意图；
[0141]
图10为本发明实施例提供的无人车在方形场地的仿真结果示意图；a为x,xd仿真结果，b为y,yd仿真结果，c为(x,y)仿真结果，d为ψ,ψd仿真结果，e为f仿真结果，f为γ仿真结果。
[0142]
符号说明：
[0143]
数据获取模块-1，非完整机器人动力学模块-2，幅角模块-3，光滑姿态规划器-4，滤波器-5，计算模块-6，输出模块-7。
具体实施方式
[0144]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0145]
本发明的目的是提供一种含姿态规划的轮式移动机器人位置控制方法、系统及设备，以解决现有的控制器存在杂乱的脉冲信号干扰，机器人位置的控制精度低的问题。
[0146]
对于受非完整约束的轮式移动机器人系统，设计带有姿态规划器的内外环反馈控制，既可实现轨迹跟踪，也可用于定点镇定，同时具有较强的鲁棒性，且能够完成由路径跟踪和定点镇定混合构成的控制目标。
[0147]
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
[0148]
图1示出了上述一种含姿态规划的轮式移动机器人位置控制方法的一种示例性流程。下面对各步骤进行详细介绍。
[0149]
步骤1：获取轮式移动机器人质心坐标历史数据集、偏航角历史数据集、前进速度历史数据集和转向角速度历史数据集；实时获取机器人质心坐标、偏航角、前进速度和转向角速度；
[0150]
步骤2：采用动力学分析方法对所述机器人质心坐标历史数据集、机器人偏航角历史数据集、机器人前进速度历史数据集和机器人转向角速度历史数据集进行运动学计算、车轮力学计算和车体力学计算，得到非完整机器人动力学模型；具体包括：
[0151]
步骤21：对所述机器人质心坐标历史数据集、偏航角历史数据集、前进速度历史数据集和转向角速度历史数据集进行运动学计算，得到运动学模型；具体公式如下：
[0152][0153]
请参见图3，其中，(x,y)为所述机器人质心坐标，ψ为机器人偏航角，v为机器人前进速度，ω为机器人转向角速度，为x的一阶导数，为y的一阶导数，为ψ的一阶导数；
[0154]
步骤22：对所述运动学模型进行车轮力学计算，得到车轮力学模型；具体公式如下：
[0155][0156]
其中，表示地面对左轮前向的约束反力，表示地面对右轮前向的约束反力；j1为左轮的转动惯量，j2为右轮的转动惯量，j1＝j2；φ1为左轮的角位移，φ2为右轮的角位移；τ1为左轮电机提供的力矩，τ2为右轮电机提供的力矩；r为每个轮的直径，l为机器人质心坐标到左轮面或右轮面的距离；为左轮的角位移的一阶导数，为右轮的角位移的一阶导数；为左轮的角位移的二阶导数，为右轮的角位移的二阶导数；
[0157]
步骤23：对所述运动学模型进行车体力学计算，得到车体力学模型；具体公式如下：
[0158][0159]
其中，k1为平动阻尼系数，k2转动阻尼系数，m为机器人车体的质量，表示地面对左轮和右轮侧向的约束反力的合力；为x的二阶导数，为y的二阶导数，为ψ的二阶导数；j＝j1＝j2；
[0160]
步骤24：采用动力学分析方法对所述运动学模型、车轮力学模型和车体力学模型进行计算，得到所述非完整机器人动力学模型；具体公式如下：
[0161][0162]
其中，为v的一阶导数，为ω的一阶导数，r＝(x,y)
t
；f为两轮的平均力矩，γ为半力矩差，p1,p2,k1,k2为与轮式移动机器人参数相关的常系数。
[0163]
从机器人移动过程中的质心坐标(x(t),y(t))得到幅角ψ(t)。
[0164]
对于xoy平面上点(x,y)，利用值域为(-π/2,π/2)的反正切函数atan(y/x)可以得到幅角主值，但x＝0处会出现零除现象。
[0165]
为了克服零除现象这个困难，可以利用值域为[-π,π)的atan2(y,x)得到幅角主值。两个参数的函数atan2是正切函数的一个变型。
[0166]
步骤3：采用切换和坐标变换的方法对机器人移动过程中的质心动点坐标进行设计，首次给出计算动点幅角的光滑函数模型atan3(y,x)；具体包括：
[0167]
步骤31：根据[-π,π)的atan2(y,x)得到幅角主值；具体公式如下：
[0168][0169]
atan2(0,0)＝0；
[0170]
在一个示例中，利用函数atan2(y,x)计算随时间移动的质心坐标(x(t),y(t))在时刻t对应的辐角面临挑战。一方面，幅角的取值范围是(-∞, ∞)而不是[-π,π)，相同的坐标(x,y)可能对应不同的幅角ψ(x,y)。另一方面，直接利用atan2会带来困难，这是由于(x,y)在(-1,0)处是奇点，其函数满足：
[0171][0172]
这表明，对于x-y平面中随时间移动的质心坐标(x(t),y(t))，当该质心坐标穿过x的左半轴时，函数atan2(y(t),x(t))出现一个阶跃，其导数相应地出现一次脉冲。对于在x负半轴附近震动的点(-1,ε(t))的幅角主值atan2(ε(t),-1)会在短时间内在-π和π间剧烈震动，这与真实的物理运动过程的幅角只有微弱的变化不符。
[0173]
请参见图4中的a，步骤32：建立坐标系xoy，机器人移动过程中的任一质心a的坐标为(x(t),y(t))；预设活跃区间[-2π/3,2π/3](其实真含于[-π,π]的任意真包含[-π/2,π/2]的[-a,a]都可以)，在xoy中利用atan2计算点(-1,0)的幅角主值；为了避免在xoy中利用atan2计算点(-1,0)的幅角主值，在[-2π/3,2π/3]为一个活跃区间，根据所述幅角主值得到
xoy坐标系中的活跃窗口；
[0174][0175]
第一次离开d的时间为：
[0176]
τ1＝inf{t≥τ0:a(t)∈dc},τ0＝0；dc为集合d的补集；
[0177]
请参见图4中的b，步骤33：在时刻τ1，将坐标系xoy绕o点旋转180
°
(逆时针或顺时针)，得到坐标系x
′
oy
′
；机器人移动过程中的任一质心a(t)的坐标为为a
′
(t)＝(x
′
(t),y
′
t))＝-(x(t),y(t))；
[0178]
第一次离开d
′
的时间为：
[0179]
τ2＝inf{t≥τ1:a
′
(t)∈d
′c}；
[0180]
其中d
′
为x
′
oy
′
坐标系中的活跃窗口；
[0181][0182]
步骤34：根据任意一个机器人车体的质量m＝1,2...，得到机器人移动过程中的任一质心坐标a在t时刻在坐标系xoy和坐标系x
′
oy
′
的复合坐标系中幅角主值的计算公式:
[0183][0184][0185]
步骤35：预设采样时间的步长等于δ，tk＝kδ，第一计数器σ(t)具体为:
[0186][0187]
其中，σ(t)＝1对应坐标系xoy，σ(t)＝-1对应坐标系x
′
oy
′
，i为示性函数，φ
(t)＝atan2[(y(t),x(t))]，φ-(t)＝atan2[(y(t),x(t))*(-1)]；
[0188]
步骤36：预设第二计数器s(t)；计算移动质心坐标穿出[-2π/3,2π/3]的次数的代数和，移动质心穿出[-2π/3,2π/3]的次数(规定从上沿穿出[-2π/3,2π/3]为正，从下沿穿出为负)的代数和；具体公式如下：
[0189][0190]
s(t)具体为：
[0191][0192]
在上述步骤中，ψ(t)涉及到切换运算。切换的引入是为了消除atan2引起的抖震现象。
[0193]
步骤37：机器人移动过程中质心动点坐标的幅角的光滑函数模型atan3(y,x)，具体公式如下：
[0194]
ψ(t)＝atan3[y(t),x(t)]＝φ(t) s(t)π；
ꢀꢀꢀ
1.8
[0195]
其中，atan3中的3表示继atan2之后对atan的再次推广，为本发明实施例首次命名并给出详细计算方法。
[0196]
在一个示例中，对于光滑移动的质心坐标(x(t),y(t))，因为这个计算过程实际是对光滑物理过程的幅角实现和模拟，尽管ψ(t)依赖于(x(t),y(t))，但函数值与(x,y)的关系并非函数关系，ψ(t)与时间t是函数关系，并且具有良好的解析性质，因此ψ(t)是个关于t的光滑函数。可以通过以下例子更好地理解这一点。
[0197]
在图5中，由(x(t),y(t))＝(2 cos t,sin t)描述的点是沿椭圆移动的点。函数φ
(t)表示质心坐标在坐标系xoy中的幅角主值，φ-(t)是质心坐标在坐标系x
′
oy
′
中的幅角主值，它们都是不连续的函数(间断点的阶跃都为2π)。在φ
(t)与φ-(t)之间按切换σ(t)可以得到φ，也是一个不连续的函数(间断点的阶跃为π)。而s(t)π是一个阶梯函数，每次阶跃为π。幅角ψ(t)是φ(t)与s(t)π的和，是连续且光滑的函数。
[0198]
考虑幅角ψ(t)在点t处的解析性质，由于ψ(t)与atan2(y(t),x(t))仅相差π的整数倍，atan2(y(t),x(t))与atan(y(t)/x(t))也相差π的整数倍，ψ(t)与atan2(y(t),x(t))具有相同的导数，即：
[0199][0200]
需要注意的是，尽管引入了函数atan3，但平面上的(0,0)点依然是个奇点，当质心坐标在无限接近该奇点处的微小移动，会引起幅角剧烈的变化。
[0201]
步骤4：根据所述非完整机器人动力学模型和幅角的光滑函数模型atan3(y,x)，结合期望控制构建光滑姿态规划器，得到期望的幅角和幅值；具体包括：
[0202]
步骤41：根据非完整机器人动力学模型，通过引入虚拟位置控制μ，得到位置运动模型；
[0203][0204]
μ＝vr(ψ)；
ꢀꢀꢀ
1.11
[0205][0206]
其中，为r的一阶导数，r(ψ)＝(cosψ,sinψ)
t
，取决于和其中表示全体实数集；μ为虚拟位置控制；可以设计全驱控制。
[0207]
步骤42：根据虚拟位置控制，得到期望位置控制向量
[0208]
μd＝vdr(ψd)；
ꢀꢀꢀ
1.13
[0209]
其中，ψd为待设计的偏航角，vd为期望的平动速度，μd的分量计算公式如下：
[0210]
μ
dx
＝v
d cosψd,μ
dy
＝v
d sinψd；
ꢀꢀꢀ
1.14
[0211]
其中，μ
dx
,μ
dy
是向量μd的分量；
[0212]
步骤43：根据(μ
dx
,μ
dy
)计算期望的平动速度向量vd和转动方向向量ψd，得到期望速度模型；
[0213][0214]
步骤44：根据所述幅角的光滑函数模型atan3(y,x)和期望位置控制向量，得到光滑姿态规划器；
[0215]
ψd(t)=atan3(μ
dx
(t),μ
dy
(t))。
ꢀꢀꢀ
1.16
[0216]
在一个示例中，μ
dx
,μ
dy
是待设计的。在xoy平面，除原点外，速度和姿态都具有良好的解析性质。位置r*是由用户任意指定的，但是姿态则无需指定，而是由机器人自己利用光滑姿态规划器生成，表明移动机器人具有更好的机动性。
[0217]
步骤5：对所述期望的幅角和期望幅值进行滤波，得到滤波后的期望幅角和期望幅值；具体包括：
[0218]
步骤51：步骤51：
[0219]
其中，为的一阶导数，为滤波后的期望幅角，为的一阶导数，为滤波后的期望幅值；滤波时间常数κ1＞0,κ2＞0。
[0220]
在一个示例中，尽管在xoy平面，除原点外速度和姿态都具有良好的解析性质，但在零点附近的对解析性质的讨论是必不可少的，而且也是困难的。为了减少理论上的困难，同时考虑应用上的方便，为ψd设计滤波器。
[0221]
这两个滤波器并非高增益的滤波器，因为κ1＞0,κ2＞0，仅影响动态性能，而不影响系统的稳定性。
[0222]
对所述滤波后的期望幅角和期望幅值进行位置计算和姿态计算，得到位置控制外环设计结果和姿态控制内环设计结果；具体包括：
[0223]
步骤52：根据滤波后的期望幅值和期望位置r
*
(k)得到位置误差和速度误差
[0224][0225]
将(1.19)的第一式关于时间求导得到
[0226]
其中，
[0227][0228][0229][0230]
其中，为位置误差的一阶导数，为滤波后的期望位置控制向量；
[0231]
步骤53：定义位置误差李雅普诺夫函数vr，根据链式求导法则，得到其时间导数；具体公式如下：
[0232][0233][0234][0235]
其中为vr的一阶导数；
[0236]
步骤54：根据李雅普诺夫稳定性定理，得到第一虚拟控制律为：
[0237][0238]
其中，cr和d为正的设计参数；
[0239]
步骤55：定义平动系统李雅普诺夫函数v1，根据链式求导法则，得到其时间导数；具体公式如下：
[0240][0241][0242]
步骤56：根据李雅普诺夫稳定性定理取平动控制，得到位置控制外环设计结果；
[0243][0244]
其中，p1，cv和k1为正的设计参数；
[0245]
步骤57：根据滤波后的期望幅角和期望角速度ωd，得到姿态误差和角速度误差
[0246][0247]
步骤58：定义姿态误差李雅普诺夫函数v
ψ
，根据链式求导法则，得到其时间导数；
具体公式如下：
[0248][0249][0250]
根据李雅普诺夫稳定性定理，得到期望角速度为：
[0251][0252]
其中，c
ψ
为正的设计参数；
[0253]
步骤59：定义转动系统李雅普诺夫函数v2，根据链式求导法则，得到其时间导数；具体公式如下：
[0254][0255][0256]
步骤510：根据李雅普诺夫稳定性定理取转动控制，得到姿态控制内环设计结果；
[0257][0258]
其中c
ω
和p2为正的设计参数。
[0259]
在一个示例中，因为期望角度ψd是方向子系统的输出，所以不能立即分配或提供它们。因此，它们被视为内环控制器的参考轨迹。
[0260]
步骤6：根据所述位置控制外环设计结果和姿态控制内环设计结果，构建含姿态规划的轮式移动机器人位置控制模型；将所述机器人质心坐标、偏航角、前进速度和转向角速度输入所述含姿态规划的轮式移动机器人位置控制模型，得到含姿态规划的机器人位置控制结果。具体包括：
[0261][0262][0263]
其中，d，cr，p1，p2，c
ψ
，cv均为正的设计参数；
[0264]
步骤61：根据所述位置控制外环设计结果和姿态控制内环设计结果，构建稳定性分析闭环误差系统；具体公式如下：
[0265][0266]
步骤62：请参见图6，计算所述稳定性分析闭环误差系统的lyapunov函数，得到所述含姿态规划的轮式移动机器人位置控制模型；将所述轮式移动机器人质心坐标、偏航角、前进速度和转向角速度输入所述含姿态规划的轮式移动机器人位置控制模型，得到含姿态规划的轮式移动机器人位置控制结果v；
[0267]
v＝v1 v2；
ꢀꢀꢀ
1.39
[0268][0269]
综上所述，在本发明实施例中，根据非完整轮式移动机器人动力学模型和机器人质心坐标得到轮式移动机器人移动过程中的质心坐标的幅角模型，基于质心坐标旋转和切换的求解移动质点的幅角代替函数atan2，消除震荡，实现控制器排除外界噪声信号干扰。
[0270]
根据非完整轮式移动机器人动力学模型和幅角模型，构建光滑姿态规划器，得到具有良好解析性质的光滑姿态规划器。光滑姿态规划器可以切换，弥补了brockett限制条件所带来的不足。
[0271]
光滑姿态规划器提高了控制器的机动性，将非完整轮式移动机器人的控制问题转化为关于位置的全驱控制问题，也就是不直接控制姿态。与传统的结合lasalle不变集原理与零状态可检测性质的欠驱动控制器相比，含姿态规划的轮式移动机器人位置控制结果具有较强的鲁棒性。
[0272]
由于位置控制外环设计结果(位置控制器)和姿态控制内环设计结果(姿态控制器)都利用了典型的反推方法，控制器设计易于与自适应观测器相结合，能够解决更复杂的控制问题，以提高了机器人位置的控制精度。
[0273]
在本发明其他实施例中：
[0274]
(1)对于镇定问题，即控制公式(1.36)使得闭环系统公式(1.38)指数稳定，其余所有信号有界。
[0275]
(2)对于跟踪问题，若控制公式(1.36)使得闭环系统公式(1.38)指数稳定，其余所有信号有界。
[0276]
结合公式(1.39)和公式(1.40)，得到指数收敛到零。对于
[0277][0278]
保证的有界性，由
[0279][0280]
得到
[0281][0282]
其中，以及
[0283][0284]
π1,π2的有界性是显然的。由得知存在lv≥0使得≥0使得所以
[0285]
表明
[0286]
其中|π3|的有界性转化为ξ0的有界性。最后，可以得到
[0287][0288]
对于镇定问题，上式恒有界。对于跟踪问题，由于指数收敛，存在k3和k4使得因此
[0289]
[0290]
根据假设则存在足够小的cr和足够大的d使得表明跟踪情形下，ξ0 π4的有界性可以通过选取控制器参数得到保证。
[0291]
对于
[0292]
为了完成证明，关键在于如何保证的有界性。
[0293]
可以求得这表明
[0294]
实施例1仿真模拟：第一个仿真：无人车在圆形场地行驶。如图7所示，建立坐标系xoy。汽车从a开始在直道上以速度vs行驶t1秒到b，先以线速度vs按半径ra右转角度θ1到c，其中co与ox的夹角θ2，同时切入环岛，然后以相同的线速度绕半径为rb的环岛旋转近θ3后到d点，以半径rc右转θ4到e，然后沿切线进入直道长度为l的ef，最后利用t4镇定到f。
[0295]
t1＝1,θ1＝π/3,θ2＝2π/3,ra＝13,rb＝20,rc＝30,θ3＝0.92*5*π/3,θ4＝0.4*π/3,l＝20；
[0296][0297]
故
[0298]
系统参数:p1＝10,p2＝100；
[0299]
控制器参数:cr＝0.1,cv＝10,c
ψ
＝1,c
ω
＝10,κ1＝κ2＝10,d＝1.
[0300]
移动机器人的初始状态:x0＝40,y0＝-3.5,v
x0
＝0,v
y0
＝0,ω0＝0。仿真结果如图8所示，由此可以验证针对移动机器人设计的控制器的有效性。
[0301]
实施例2第二个仿真：无人车在方形场地行驶，如图9所示。
[0302]
建立坐标系xoy。汽车o沿x轴到a，然后转弯到b，沿依次经过c，d，e，f，g，h到达i转弯半径为r，角度都是90度，线速度都是速度vs。oa，ab，bc，cd，de，ef，fg，gh，hi的行驶时间分别为t1到t9。
[0303]
仿真时，取vs＝5，r＝4.2，t1＝t3＝t5＝10.1，t7＝6.262，t9＝7.1可以求得转弯的时间系统参数:p1＝10,p2＝100；
[0304]
控制器参数:cr＝0.1,cv＝10,c
ψ
＝1,c
ω
＝10,κ1＝κ2＝10,d＝1.
[0305]
移动机器人的初始状态:x0＝0,y0＝0,v
x0
＝0,v
y0
＝0,ψ0＝0,ω0＝0。仿真结果如图10所示。
[0306]
为实现上述目的，本发明实施例还提供了如下方案：
[0307]
一种含姿态规划的轮式移动机器人位置控制系统，请参见图2，包括：
[0308]
数据获取模块1用于获取机器人质心坐标历史数据集、偏航角历史数据集、前进速度历史数据集和转向角速度历史数据集；实时获取机器人质心坐标、偏航角、前进速度和转向角速度；
[0309]
非完整机器人动力学模块2与所述数据获取模块1连接，非完整机器人动力学模块2用于采用动力学分析方法对所述机器人质心坐标历史数据集、机器人偏航角历史数据集、机器人前进速度历史数据集和机器人转向角速度历史数据集进行运动学计算、车轮力学计算和车体力学计算，得到非完整机器人动力学模型；
[0310]
幅角模块3分别与所述数据获取模块1和所述非完整机器人动力学模块2连接，幅角模块3用于采用切换和坐标变换的方法对机器人移动过程中的质心动点坐标进行设计，并首次给出计算动点幅角的光滑函数模型atan3(y,x)；
[0311]
光滑姿态规划器4与所述非完整机器人动力学模型2和所述幅角模型3连接，光滑姿态规划器4用于根据所述非完整机器人动力学模型和幅角的光滑函数模型atan3(y,x)，结合期望控制构建光滑姿态规划器，得到期望的幅角和幅值；
[0312]
滤波器5与所述光滑姿态规划器4连接，滤波器5用于对所述期望的幅角和幅值进行滤波，得到滤波后的期望幅角和期望幅值；
[0313]
计算模块6与所述滤波器5连接，计算模块6用于对所述滤波后的期望幅角和期望幅值进行位置计算和姿态计算，得到位置控制外环设计结果和姿态控制内环设计结果；
[0314]
输出模块7与所述计算模块6连接，输出模块7用于根据所述位置控制外环设计结果和姿态控制内环设计结果，构建含姿态规划的轮式移动机器人位置控制模型；将所述机器人质心坐标、偏航角、前进速度和转向角速度输入所述含姿态规划的机器人位置控制模型，得到含姿态规划的轮式移动机器人位置控制结果。
[0315]
进一步地，本发明还提供一种电子设备，该电子设备可以包括：处理器、通信接口、存储器和通信总线。其中，处理器、通信接口、存储器通过通信总线完成相互间的通信。处理器可以调用存储器中的计算机程序，以执行所述处理器执行所述计算机程序时实现所述的含姿态规划的轮式移动机器人位置控制方法。
[0316]
此外，上述的存储器中的计算机程序通过软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机、服务器或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：u盘、移动硬盘、只读存储器、随机存取存储器、磁碟或者光盘等各种可以存储程序代码的介质。
[0317]
进一步地，本发明还提供一种非暂态计算机可读存储介质，其上存储有计算机程序，所述计算机程序被执行时实现所述的含姿态规划的轮式移动机器人位置控制方法。
[0318]
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
[0319]
本文中应用了具体个例对本发明实施例的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明实施例的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明实施例的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明实施例的限制。