CN104483967A - 一种基于节能考虑的轮式移动机器人轨迹跟踪控制方法 - Google Patents

Abstract

本发明涉及一种基于节能考虑的轮式移动机器人轨迹跟踪控制方法，属于机器人控制领域。其步骤为：根据轮式移动机器人的能耗特点，重点考虑机器人巡航时的驱动电机能耗优化，构建电机能耗模型；根据轮式移动机器人轨迹跟踪的特点，建立运动学模型及跟踪误差模型，设计运动学跟踪子控制器；根据运动学模型和电机能耗模型之间的内在关联机制，构建其关联模型；最后得到节能子控制器，从而获得一种基于节能考虑的轮式移动机器人轨迹跟踪控制方法。本发明能够实现轮式移动机器人整个过程的高精度、低能耗控制及系统全局稳定性，使它在进行精确轨迹跟踪的同时实现能量最优化。

Description

一种基于节能考虑的轮式移动机器人轨迹跟踪控制方法

技术领域

本发明主要涉及移动机器人的控制领域，尤其涉及一种适用于轮式移动机器人的轨迹跟踪控制方法。

背景技术

机器人能力的大小与规模是一个国家科技水平的象征，因此世界各国的研究机构都非常重视这方面的研究工作。移动机器人是机器人学的一个重要分支，随着计算机、人工智能、机械电子以及自动化等技术的飞速发展，移动机器人的研究进入了一个崭新的阶段。常见的移动机器人有轮式、履带式、腿式和蜿蜒式等几种类型，其中轮式移动机器人由于其悠久的历史与成熟的设计，在实际应用中最为常见。

轨迹跟踪控制是轮式移动机器人研究的基础性问题，是智能化技术的核心，因此提高轮式移动机器人的轨迹跟踪控制性能对于提高机器人自动化水平具有重要的理论意义和实用价值；另一方面，目前大多数的轮式移动机器人都使用蓄电池提供能量，由于电池技术的局限，机器人使用时间十分有限。因此，如何有效地使用有限的电池能量，延长其运行时间成为一个至关重要的课题。虽然目前有关轮式移动机器人轨迹跟踪控制的研究前人已取得了许多重要研究成果，控制方法涵盖了反演控制、滑模控制、自适应控制、神经网络控制、模糊控制等。然而，基于节能考虑同时又能实现轮式移动机器人对既定轨迹的准确跟踪的控制问题至今仍未能得到很好的解决。

发明内容

本发明要解决的技术问题在于：针对现有技术存在的不足之处，本发明提供一种基于节能考虑的轮式移动机器人轨迹跟踪控制方法，主要考虑轮式移动机器人的机械能耗，建立能耗模型并将其与轮式移动机器人运动学模型有效融合，建立关联模型，设计一个有效的速度控制策略，从而有效减少能耗损耗，提高能量利用效率，实现基于节能考虑的轮式移动机器人轨迹跟踪控制。

本发明采用以下技术方案：

一种基于节能考虑的轮式移动机器人轨迹跟踪控制方法，其步骤为：

(1)根据轮式移动机器人的能耗特点，重点考虑机器人巡航时的驱动电机能耗优化，构建电机能耗模型；

(2)根据轮式移动机器人轨迹跟踪的特点，建立运动学模型及跟踪误差模型，设计运动学跟踪子控制器；

(3)根据运动学模型和电机能耗模型之间的内在关联机制，构建其关联模型；

(4)设计节能子控制器，获得一种基于节能考虑的轮式移动机器人轨迹跟踪控制策略。

具体实现过程为：

(a)由路径规划算法给出当前时刻的轮式移动机器人参考位姿p_r＝[x_r y_r θ_r]^T和参考速度q_r＝[v_r ω_r]^T；由轮式移动机器人定位模块反馈得到当前时刻的轮式移动机器人当前实际位姿p_c＝[x y θ]^T；比较p_r与p_c可得到轮式移动机器人在全局坐标系{X-O-Y}下的位姿误差矢量，再经过坐标转换矩阵T转换得到局部坐标系{X_0-C-Y₀}下的位姿误差矢量p_e＝[x_e y_e θ_e]^T；

(b)上步中的误差矢量p_e和参考速度q_r一起作为轮式移动机器人运动学跟踪子控制器的输入，输出期望速度控制率q_d＝[v_d ω_d]^T；

(c)进入一个节能子控制器进行能量优化处理。由于轮式移动机器人的巡游过程是机器人能耗的关键，故本发明仅考虑前进线速度v_d的节能优化，通过一个节能子控制器寻找一个考虑节能的最优跟踪速度v_d ^*，使对于任意初始误差p_e(t＝0)，系统在该最优控制输入q_d ^*＝[v_d ^*ω_d]^T的作用下实现误差矢量p_e＝[x_e y_e θ_e]^T有界且同时系统机械能耗有最小值，即找到解析解v_d ^*最小化节能目标函数。因此本发明最终提供的基于节能考虑的轮式移动机器人轨迹跟踪控制律为q_d ^*＝[v_d ^* ω_d]^T。

作为本发明的进一步改进：

所述轮式移动机器人采用一种三轮式结构，其前轮既是转向轮又是驱动轮，分别由一个转向电机和一个驱动电机实现。x、y和θ分别表示轮式移动机器人位置坐标和车体方位角；轮式移动机器人的当前实际位姿表示为p_c(t)＝[x(t) y(t) θ(t)]^T，由路径规划算法给出的参考位姿表示为p_r(t)＝[x_r(t) y_r(t) θ_r(t)]^T；位姿误差为p_e(t)＝[x_e(t) y_e(t) θ_e(t)]^T；v和ω表示车体前进线速度和旋转角转速，因此当前时刻车体实际速度表示为q(t)＝[v(t) ω(t)]^T，参考速度为q_r(t)＝[v_r(t) ω_r(t)]^T。此外，前轮半径、前轮转向角、前轮角速度及前轮驱动电机角速度分别用r、β、ω_w及ω_m表示。

所述电机能耗模型为：

设轮式移动机器人的前轮驱动电机具有电枢电阻为R_a，反电势常数K_b，扭矩常数K_t，磁滞摩擦系数f_v，齿轮减速比i，电池电压为V_s，设U_a为电机输入电压，u为控制输入且u＝U_a/V_s，ω_w与ω_m关系可表示为ω_m＝iω_w。由于电路响应比机械响应快很多，因此本发明忽略电枢电路的电感，从而简化动力学方程。

电枢等效电路电压平衡方程式为：

U_a＝I_aR_a+E_b   (1)

式中U_a＝V_su为电机输入电压，E_b＝K_biω_w为反电动势，I_a为电枢电流。

对于时间间隔t∈[t₀ t_f]可知电机能耗方程为：

$E={\∫}_{t_0}^{t_f}{U}_a{I}_a\mathrm{dt}---\left(2\right)$

电机损失转矩为：

$T_{\mathrm{loss}}=f_{v}i{\mathrm{\ω}}_w+\frac{{I_a}^2{R}_a}{i{\mathrm{\ω}}_w}---\left(3\right)$

电机损失功率为：

P_loss＝f_vi²ω_w ²+I_a ²R_a   (4)

电机损失功率的百分比可表示为：

${\mathrm{\η}}_{\mathrm{loss}}=\frac{P_{\mathrm{loss}}}{P_{\mathrm{input}}}=\frac{f_v{{\mathrm{\ω}}_m}^2+{I_a}^2{R}_a}{U_a{I}_a}---\left(5\right)$

从等效电路电压平衡方程式(1)得并代入式(5)，得：

${\mathrm{\η}}_{\mathrm{loss}}=\frac{f_v{i}^2{{\mathrm{\ω}}_w}^2+\frac{{U_a}^2-2K_b{U}_{a}i{\mathrm{\ω}}_w+{K_b}^2{i}^2{{\mathrm{\ω}}_w}^2}{R_a}}{\frac{{U_a}^2-K_b{U}_{a}i{\mathrm{\ω}}_w}{R_a}}=\frac{(R_a{f}_v+{K_a}^2)i^2{{\mathrm{\ω}}_w}^2-2K_b{U}_{a}i{\mathrm{\ω}}_w+{U_a}^2}{{U_a}^2-K_b{U}_{a}i{\mathrm{\ω}}_w}---\left(6\right)$

最后将U_a＝V_su代入上式得到关于能量效率函数作为节能目标函数：

${\mathrm{\η}}_{\mathrm{loss}}=\frac{i^2(R_a{f}_v+{K_b}^2){{\mathrm{\ω}}_w}^2-2i{K}_b{V}_{s}u{\mathrm{\ω}}_w+{V_s}^2{u}^2}{{V_s}^2{u}^2-i{K}_b{V}_{s}u{\mathrm{\ω}}_w}---\left(7\right)$

此外，轮式移动机器人系统的控制约束为：

$-1\≤u=\frac{U_a}{V_s}\≤1---\left(8\right)$

电机电枢等效电路电压平衡方程式和转矩方程式组成如下方程组：

$\left\{\begin{array}{c}{I}_a{R}_a=V_{s}u-K_{b}i{\mathrm{\ω}}_w\\ J\frac{{\mathrm{d\ω}}_w}{\mathrm{dt}}+f_v{\mathrm{\ω}}_w=K_{t}i{I}_a\end{array}\right.---\left(9\right)$

式中J为电机转动惯量。

联立方程组(9)可消去I_a，得：

${\stackrel{\·}{\mathrm{\ω}}}_w+J^{-1}(f_v+\frac{K_t{K}_b{i}^2}{R_a}){\mathrm{\ω}}_w=J^{-1}\left(\frac{K_t{V}_{s}i}{R_a}\right)u---\left(10\right)$

设式(10)中的则系统状态方程(10)可改写成：

${\stackrel{\·}{\mathrm{\ω}}}_w+{\mathrm{\λ}}_1{\mathrm{\ω}}_w={\mathrm{\λ}}_{2}u---\left(11\right)$

所述轮式移动机器人的运动学模型及关联模型为：

轮式移动机器人的运动学方程为：

$\left[\begin{array}{c}\stackrel{\·}{x}\\ \stackrel{\·}{y}\\ \stackrel{\·}{\mathrm{\θ}}\end{array}\right]=\left[\begin{array}{cc}\cos \mathrm{\θ}& 0\\ \sin \mathrm{\θ}& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}v\\ \mathrm{\ω}\end{array}\right]=S\·q---\left(12\right)$

式中S是一个雅克比矩阵。

轮式移动机器人的位姿误差为：

$p_e=\left[\begin{array}{c}{x}_e\\ y_e\\ {\mathrm{\θ}}_e\end{array}\right]=\left[\begin{array}{ccc}\cos \mathrm{\θ}& \sin \mathrm{\θ}& 0\\ -\sin \mathrm{\θ}& \cos \mathrm{\θ}& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_r-x\\ y_r-y\\ {\mathrm{\θ}}_r-\mathrm{\θ}\end{array}\right]=T(p_r-p_c)---\left(13\right)$

式中T是一个坐标转换矩阵，将轮式移动机器人在笛卡尔坐标系{X-O-Y}的速度转换成局部坐标系{X₀-C-Y₀}中的速度。

轮式移动机器人的位姿误差微分方程为：

${\stackrel{\·}{p}}_e=\left[\begin{array}{c}{\stackrel{\·}{x}}_e\\ {\stackrel{\·}{y}}_e\\ {\stackrel{\·}{\mathrm{\θ}}}_e\end{array}\right]=\left[\begin{array}{cc}{y}_e& -1\\ {-x}_e& 0\\ -1& 0\end{array}\right]\left[\begin{array}{c}\mathrm{\ω}\\ v\end{array}\right]+\left[\begin{array}{c}{v}_r\cos {\mathrm{\θ}}_e\\ v_r\sin {\mathrm{\θ}}_e\\ {\mathrm{\ω}}_r\end{array}\right]---\left(14\right)$

对轮式移动机器人位姿误差微分方程(14)构造Lyapunov函数：

$V=\frac{1}{2}({x_e}^2+{y_e}^2)+\frac{1}{k_2}(1-\cos {\mathrm{\θ}}_e)---\left(15\right)$

式中k₂为正常数。

对式(15)中的V求时间导数可得：

$\begin{array}{c}\stackrel{\·}{V}=x_e{\stackrel{\·}{x}}_e+y_e{\stackrel{\·}{y}}_e+\frac{1}{k_2}{\stackrel{\·}{\mathrm{\θ}}}_e\sin {\mathrm{\θ}}_e\\ =x_e(v_r\cos {\mathrm{\θ}}_e-v+y_e\mathrm{\ω})+y_e(v_r\sin {\mathrm{\θ}}_e-x_e\mathrm{\ω})+\frac{1}{k_2}({\mathrm{\ω}}_r-\mathrm{\ω})\sin {\mathrm{\θ}}_e\end{array}---\left(16\right)$

相对于转向电机，驱动电机的耗能占总耗能的绝大部分，因此本发明所设计的控制方法旨在使驱动电机耗能最小而暂不考虑转向电机。

设计期望速度控制率如下：

$q_d=\left[\begin{array}{c}{v}_d\\ {\mathrm{\ω}}_d\end{array}\right]=\left[\begin{array}{c}{v_d}^{*}\\ {\mathrm{\ω}}_r+k_2{v}_r{y}_e+k_3\sin {\mathrm{\θ}}_e\end{array}\right]---\left(17\right)$

式中k₃为可正常数，v_d ^*为待定线速度控制率。

根据Barbalat引理，为了使系统误差p_e＝[x_e y_e θ_e]^T一致有界且渐近收敛于零，则要求式(16)中的

将式(17)代入(16)得x_e(v_r cosθ_e-v_d)≤0，又根据轮式移动机器人模型可知其车体速度v_d与前驱动轮相对于车体的旋转角转速ω_w之间的关系式为：

v_d＝ω_wr cosβ   (18)

故要使轮式移动机器人能准确跟踪参考轨迹则必须满足关于ω_w的状态约束：

x_e(v_rcosθ_e-ω_wrcosβ)≤0   (19)

所述的基于节能考虑的轮式移动机器人轨迹跟踪控制策略为：

该控制问题本质上属于解决有约束的多变量优化问题。整理上述节能目标函数(7)和控制约束(8)、系统状态方程(11)及状态约束(19)，得到如下优化模型：

${\min \mathrm{\η}}_{\mathrm{loss}}=\frac{i^2(R_a{f}_v+{K_b}^2){{\mathrm{\ω}}_w}^2-2i{K}_b{V}_{s}u{\mathrm{\ω}}_w+{V_s}^2{u}^2}{{V_s}^2{u}^2-{\mathrm{iK}}_b{V}_{s}u{\mathrm{\ω}}_w}$

$s.t.\left\{\begin{array}{c}{\stackrel{\·}{\mathrm{\ω}}}_w+{\mathrm{\λ}}_1{\mathrm{\ω}}_w={\mathrm{\λ}}_{2}u\\ x_e(v_r\cos {\mathrm{\θ}}_e-{\mathrm{\ω}}_{w}r\cos \mathrm{\β})\≤0\\ -1\≤u\≤1\end{array}\right.---\left(20\right)$

式中， ${\mathrm{\λ}}_1=J^{-1}(f_v+\frac{K_t{K}_b{i}^2}{R_a});{\mathrm{\λ}}_2=J^{-1}\left(\frac{K_t{V}_{s}i}{\mathrm{Ra}}\right).$

能耗最小化的获得可以表示为一个最小值的搜索问题，可用遗传算法来解决。设搜索出的最优驱动轮转速为ω_w ^*，则能量最优车体线速度为v_d ^*＝ω_w ^*r cosβ。结合式(17)中运动控制器设计出的车体角速度ω_d，最后得到考虑节能的轨迹跟踪最优控制速度率为：

${q_d}^{*}=\left[\begin{array}{c}{v_d}^{*}\\ {\mathrm{\ω}}_d\end{array}\right]=\left[\begin{array}{c}{{\mathrm{\ω}}_w}^{*}r\cos \mathrm{\β}\\ {\mathrm{\ω}}_r+k_2{v}_r{y}_e+k_3\sin {\mathrm{\θ}}_e\end{array}\right]---\left(21\right)$

与现有技术相比，本发明的有益效果在于：

(1)本发明的控制方法将节能理念融入到传统的轮式移动机器人轨迹跟踪控制方法中，使其在准确实现轨迹跟踪的同时达到能耗最优化，实现跟踪与节能两者的统一控制；

(2)本发明具有模块化程度高和兼容性强等特点，能够很好地满足当前轮式移动机器人的各类应用需求。

附图说明

图1为本发明实施例中所应用的轮式移动机器人轨迹跟踪示意图；

图2为本发明实施例中所应用的轮式移动机器人控制策略流程图。

具体实施方式

以下将结合说明书附图和具体实施例对本发明作进一步详细说明。

如图1所示，本发明实施例中采用一种三轮式轮式移动机器人，其前轮既是转向轮又是驱动轮，分别由一个转向电机和一个驱动电机实现。在图1所示的轨迹跟踪示意图中，x、y表示轮式移动机器人位置坐标，θ表示车体方位角；轮式移动机器人的当前实际位姿表示为p_c(t)＝[x(t) y(t) θ(t)]^T，由路径规划算法给出的参考位姿表示为p_r(t)＝[x_r(t) y_r(t) θ_r(t)^T；位姿误差为p_e(t)＝[x_e(t) y_e(t) θ_e(t)]^T；当前时刻车体实际速度为q(t)＝[v(t) ω(t)]^T，参考速度为q_r(t)＝[v_r(t)ω_r(t)]^T，其中v表示前进线速度，ω为旋转角速度。此外，图1中β表示前驱动轮转向角。

如图2所示，本发明所提供的一种基于节能考虑的轮式移动机器人轨迹跟踪控制方法，其步骤为：

(1)根据轮式移动机器人的能耗特点，重点考虑机器人巡航时的驱动电机能耗优化，构建电机能耗模型；

(2)根据轮式移动机器人轨迹跟踪的特点，建立运动学模型及跟踪误差模型，设计运动学跟踪子控制器；

(3)根据运动学模型和电机能耗模型之间的内在关联机制，构建其关联模型；

(4)设计节能子控制器，获得一种基于节能考虑的轮式移动机器人轨迹跟踪控制策略。

具体实现过程为：

(a)由路径规划算法给出当前时刻的轮式移动机器人参考位姿p_r＝[x_r y_r θ_r]^T和参考速度q_r＝[v_r ω_r]^T；由轮式移动机器人定位模块反馈得到当前时刻的轮式移动机器人当前实际位姿p_c＝[x y θ]^T；比较p_r与p_c可得到轮式移动机器人在全局坐标系{X-O-Y}下的位姿误差矢量，再经过坐标转换矩阵T转换得到局部坐标系{X₀-C-Y₀}下的位姿误差矢量p_e＝[x_e y_e θ_e]^T；

(b)上步中的误差矢量p_e和参考速度q_r一起作为轮式移动机器人运动学跟踪子控制器的输入，输出期望速度控制率q_d＝[v_d ω_d]^T；

(c)进入一个节能子控制器进行能量优化处理。由于轮式移动机器人的巡游过程是机器人能耗的关键，故本发明实施例仅考虑前进线速度v_d的节能优化，通过一个节能子控制器寻找一个考虑节能的最优跟踪速度v_d ^*，使对于任意初始误差p_e(t＝0)，系统在该最优控制输入q_d ^*＝[v_d ^* ω_d]^T的作用下实现误差矢量p_e＝[x_e y_e θ_e]^T有界且同时系统机械能耗有最小值，即找到解析解v_d ^*最小化节能目标函数。因此本发明实施例最终提供的基于节能考虑的轮式移动机器人轨迹跟踪控制律为q_d ^*＝[v_d ^* ω_d]^T。

在本发明实施例中，电机能耗模型为：

设轮式移动机器人的前轮驱动电机具有电枢电阻为R_a，反电势常数K_b，扭矩常数K_t，磁滞摩擦系数f_v，齿轮减速比i，电池电压为V_s，设U_a为电机输入电压，u为控制输入且u＝U_a/V_s，ω_w与ω_m关系可表示为ω_m＝iω_w。由于电路响应比机械响应快很多，因此本发明忽略电枢电路的电感，从而简化动力学方程。

电枢等效电路电压平衡方程式为：

U_a＝I_aR_a+E_b   (1)

式中U_a＝V_su为电机输入电压，E_b＝K_biω_w为反电动势，I_a为电枢电流。

对于时间间隔t∈[t₀ t_f]可知电机能耗方程为：

$E={\∫}_{t_0}^{t_f}{U}_a{I}_a\mathrm{dt}---\left(2\right)$

电机损失转矩为：

$T_{\mathrm{loss}}=f_{v}i{\mathrm{\ω}}_w+\frac{{I_a}^2{R}_a}{i{\mathrm{\ω}}_w}---\left(3\right)$

电机损失功率为：

P_loss＝f_vi²ω_w ²+I_a ²R_a          (4)

电机损失功率的百分比可表示为：

${\mathrm{\η}}_{\mathrm{loss}}=\frac{P_{\mathrm{loss}}}{P_{\mathrm{input}}}=\frac{f_v{{\mathrm{\ω}}_m}^2+{I_a}^2{R}_a}{U_a{I}_a}---\left(5\right)$

从等效电路电压平衡方程式(1)得并代入式(5)，得：

${\mathrm{\η}}_{\mathrm{loss}}=\frac{f_v{i}^2{{\mathrm{\ω}}_w}^2+\frac{{U_a}^2-2K_b{U}_{a}i{\mathrm{\ω}}_w+{K_b}^2{i}^2{{\mathrm{\ω}}_w}^2}{R_a}}{\frac{{U_a}^2-K_b{U}_{a}i{\mathrm{\ω}}_w}{R_a}}=\frac{(R_a{f}_v+{K_a}^2)i^2{{\mathrm{\ω}}_w}^2-2K_b{U}_{a}i{\mathrm{\ω}}_w+{U_a}^2}{{U_a}^2-K_b{U}_{a}i{\mathrm{\ω}}_w}---\left(6\right)$

最后将U_a＝V_su代入上式得到关于能量效率函数作为节能目标函数：

${\mathrm{\η}}_{\mathrm{loss}}=\frac{i^2(R_a{f}_v+{K_b}^2){{\mathrm{\ω}}_w}^2-2i{K}_b{V}_{s}u{\mathrm{\ω}}_w+{V_s}^2{u}^2}{{V_s}^2{u}^2-i{K}_b{V}_{s}u{\mathrm{\ω}}_w}---\left(7\right)$

此外，轮式移动机器人系统的控制约束为：

$-1\≤u=\frac{U_a}{V_s}\≤1---\left(8\right)$

电机电枢等效电路电压平衡方程式和转矩方程式组成如下方程组：

$\left\{\begin{array}{c}{I}_a{R}_a=V_{s}u-K_{b}i{\mathrm{\ω}}_w\\ J\frac{{\mathrm{d\ω}}_w}{\mathrm{dt}}+f_v{\mathrm{\ω}}_w=K_{t}i{I}_a\end{array}\right.---\left(9\right)$

式中J为电机转动惯量。

联立方程组(9)可消去I_a，得：

${\stackrel{\·}{\mathrm{\ω}}}_w+J^{-1}(f_v+\frac{K_t{K}_b{i}^2}{R_a}){\mathrm{\ω}}_w=J^{-1}\left(\frac{K_t{V}_{s}i}{R_a}\right)u---\left(10\right)$

设式(10)中的则系统状态方程(10)可改写成：

${\stackrel{\·}{\mathrm{\ω}}}_w+{\mathrm{\λ}}_1{\mathrm{\ω}}_w={\mathrm{\λ}}_{2}u---\left(11\right)$

在本发明实施例中，轮式移动机器人的运动学模型及关联模型为：

轮式移动机器人的运动学方程为：

$\left[\begin{array}{c}\stackrel{\·}{x}\\ \stackrel{\·}{y}\\ \stackrel{\·}{\mathrm{\θ}}\end{array}\right]=\left[\begin{array}{cc}\cos \mathrm{\θ}& 0\\ \sin \mathrm{\θ}& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}v\\ \mathrm{\ω}\end{array}\right]=S\·q---\left(12\right)$

式中S是一个雅克比矩阵。

轮式移动机器人的位姿误差为：

$p_e=\left[\begin{array}{c}{x}_e\\ y_e\\ {\mathrm{\θ}}_e\end{array}\right]=\left[\begin{array}{ccc}\cos \mathrm{\θ}& \sin \mathrm{\θ}& 0\\ -\sin \mathrm{\θ}& \cos \mathrm{\θ}& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_r-x\\ y_r-y\\ {\mathrm{\θ}}_r-\mathrm{\θ}\end{array}\right]=T(p_r-p_c)---\left(13\right)$

式中T是一个坐标转换矩阵，将轮式移动机器人在笛卡尔坐标系{X-O-Y}的速度转换成局部坐标系{X₀-C-Y₀}中的速度。

轮式移动机器人的位姿误差微分方程为：

${\stackrel{\·}{p}}_e=\left[\begin{array}{c}{\stackrel{\·}{x}}_e\\ {\stackrel{\·}{y}}_e\\ {\stackrel{\·}{\mathrm{\θ}}}_e\end{array}\right]=\left[\begin{array}{cc}{y}_e& -1\\ {-x}_e& 0\\ -1& 0\end{array}\right]\left[\begin{array}{c}\mathrm{\ω}\\ v\end{array}\right]+\left[\begin{array}{c}{v}_r\cos {\mathrm{\θ}}_e\\ v_r\sin {\mathrm{\θ}}_e\\ {\mathrm{\ω}}_r\end{array}\right]---\left(14\right)$

对轮式移动机器人位姿误差微分方程(14)构造Lyapunov函数：

$V=\frac{1}{2}({x_e}^2+{y_e}^2)+\frac{1}{k_2}(1-\cos {\mathrm{\θ}}_e)---\left(15\right)$

式中k₂为正常数。

对式(15)中的V求时间导数可得：

$\begin{array}{c}\stackrel{\·}{V}=x_e{\stackrel{\·}{x}}_e+y_e{\stackrel{\·}{y}}_e+\frac{1}{k_2}{\stackrel{\·}{\mathrm{\θ}}}_e\sin {\mathrm{\θ}}_e\\ =x_e(v_r\cos {\mathrm{\θ}}_e-v+y_e\mathrm{\ω})+y_e(v_r\sin {\mathrm{\θ}}_e-x_e\mathrm{\ω})+\frac{1}{k_2}({\mathrm{\ω}}_r-\mathrm{\ω})\sin {\mathrm{\θ}}_e\end{array}---\left(16\right)$

相对于转向电机，驱动电机的耗能占总耗能的绝大部分，因此本发明所设计的控制方法旨在使驱动电机耗能最小而暂不考虑转向电机。

设计期望速度控制率如下：

$q_d=\left[\begin{array}{c}{v}_d\\ {\mathrm{\ω}}_d\end{array}\right]=\left[\begin{array}{c}{v_d}^{*}\\ {\mathrm{\ω}}_r+k_2{v}_r{y}_e+k_3\sin {\mathrm{\θ}}_e\end{array}\right]---\left(17\right)$

式中k₃为可正常数，v_d ^*为待定线速度控制率。

根据Barbalat引理，为了使系统误差p_e＝[x_e y_e θ_e]^T一致有界且渐近收敛于零，则要求式(16)中的

将式(17)代入(16)得x_e(v_r cosθ_e-v_d)≤0，又根据轮式移动机器人模型可知其车体速度v_d与前驱动轮相对于车体的旋转角转速ω_w之间的关系式为：

v_d＝ω_w r cosβ   (18)

故要使轮式移动机器人能准确跟踪参考轨迹则必须满足关于ω_w的状态约束：

x_e(v_r cosθ_e-ω_wrcosβ)≤0   (19)

在本发明实施例中，基于节能考虑的轮式移动机器人轨迹跟踪控制策略为：

该控制问题本质上属于解决有约束的多变量优化问题。整理上述节能目标函数(7)和控制约束(8)、系统状态方程(11)及状态约束(19)，得到如下优化模型：

${\min \mathrm{\η}}_{\mathrm{loss}}=\frac{i^2(R_a{f}_v+{K_b}^2){{\mathrm{\ω}}_w}^2-2i{K}_b{V}_{s}u{\mathrm{\ω}}_w+{V_s}^2{u}^2}{{V_s}^2{u}^2-{\mathrm{iK}}_b{V}_{s}u{\mathrm{\ω}}_w}$

$s.t.\left\{\begin{array}{c}{\stackrel{\·}{\mathrm{\ω}}}_w+{\mathrm{\λ}}_1{\mathrm{\ω}}_w={\mathrm{\λ}}_{2}u\\ x_e(v_r\cos {\mathrm{\θ}}_e-{\mathrm{\ω}}_{w}r\cos \mathrm{\β})\≤0\\ -1\≤u\≤1\end{array}\right.---\left(20\right)$

式中， ${\mathrm{\λ}}_1=J^{-1}(f_v+\frac{K_t{K}_b{i}^2}{R_a});{\mathrm{\λ}}_2=J^{-1}\left(\frac{K_t{V}_{s}i}{\mathrm{Ra}}\right).$

能耗最小化的获得可以表示为一个最小值的搜索问题，可用遗传算法来解决。设搜索出的最优驱动轮转速为ω_w ^*，则能量最优车体线速度为v_d ^*＝ω_w ^*r cosβ。结合式(17)中运动控制器设计出的车体角速度ω_d，最后得到考虑节能的轨迹跟踪最优控制速度率为：

${q_d}^{*}=\left[\begin{array}{c}{v_d}^{*}\\ {\mathrm{\ω}}_d\end{array}\right]=\left[\begin{array}{c}{{\mathrm{\ω}}_w}^{*}r\cos \mathrm{\β}\\ {\mathrm{\ω}}_r+k_2{v}_r{y}_e+k_3\sin {\mathrm{\θ}}_e\end{array}\right]---\left(21\right)$

以上所述的实施例仅为本发明的最佳实施例，但并非用以限制本发明。在不背离本发明原理的情况下，熟悉本领域的技术人员可根据本发明做出各种相应的改进和润饰，但这些相应的改进和润饰都应属于本发明所属的权利要求范围之内。

Claims (5)

1.一种基于节能考虑的轮式移动机器人轨迹跟踪控制方法，其特征在于，步骤为：

(1)根据轮式移动机器人的能耗特点，重点考虑机器人巡航时的驱动电机能耗优化，构建电机能耗模型；

(2)根据轮式移动机器人轨迹跟踪的特点，建立运动学模型及跟踪误差模型，设计运动学跟踪子控制器；

(3)根据运动学模型和电机能耗模型之间的内在关联机制，构建其关联模型；

(4)设计节能子控制器，获得一种基于节能考虑的轮式移动机器人轨迹跟踪控制策略。

2.根据权利要求1所述的基于节能考虑的轮式移动机器人轨迹跟踪控制方法，其特征在于，具体实现过程为：

(a)由路径规划算法给出当前时刻的轮式移动机器人参考位姿p_r＝[x_r y_r θ_r]^T和参考速度q_r＝[v_r ω_r]^T；由轮式移动机器人定位模块反馈得到当前时刻的轮式移动机器人当前实际位姿p_c＝[x y θ]^T；比较p_r与p_c可得到轮式移动机器人在全局坐标系{X-O-Y}下的位姿误差矢量，再经过坐标转换矩阵T转换得到局部坐标系{X₀-C-Y₀}下的位姿误差矢量p_e＝[x_e y_e θ_e]^T；

(b)上步中的误差矢量p_e和参考速度q_r一起作为轮式移动机器人运动学跟踪子控制器的输入，输出期望速度控制率q_d＝[v_d ω_d]^T；

(c)进入一个节能子控制器进行能量优化处理。由于轮式移动机器人的巡游过程是机器人能耗的关键，故本发明仅考虑前进线速度v_d的节能优化，通过一个节能子控制器寻找一个考虑节能的最优跟踪速度v_d ^*，使对于任意初始误差p_e(t＝0)，系统在该最优控制输入q_d ^*＝[v_d ^*ω_d]^T的作用下实现误差矢量p_e＝[x_e y_e θ_e]^T有界且同时系统机械能耗有最小值，即找到解析解v_d ^*最小化节能目标函数。因此本发明最终提供的基于节能考虑的轮式移动机器人轨迹跟踪控制律为q_d ^*＝[v_d ^* ω_d]^T。

3.根据权利要求1所述的基于节能考虑的轮式移动机器人轨迹跟踪控制方法，其特征在于，所述电机能耗模型为：

设轮式移动机器人的前轮驱动电机具有电枢电阻为R_a，反电势常数K_b，扭矩常数K_t，磁滞摩擦系数f_v，齿轮减速比i，电池电压为V_s，设U_a为电机输入电压，u为控制输入且u＝U_a/V_s，ω_w与ω_m关系可表示为ω_m＝iω_w。由于电路响应比机械响应快很多，因此本发明忽略电枢电路的电感，从而简化动力学方程。

电枢等效电路电压平衡方程式为：

U_a＝I_aR_a+E_b                (1)

式中U_a＝V_su为电机输入电压，E_b＝K_biω_w为反电动势，I_a为电枢电流。

对于时间间隔t∈[t₀ t_f]可知电机能耗方程为：

$E={\∫}_{t_0}^{t_f}{U}_a{I}_a\mathrm{dt}---\left(2\right)$

电机损失转矩为：

$T_{\mathrm{loss}}=f_{v}i{\mathrm{\ω}}_w+\frac{{I_a}^2{R}_a}{i{\mathrm{\ω}}_w}---\left(3\right)$

电机损失功率为：

$P_{\mathrm{loss}}=f_v{i}^2{{\mathrm{\ω}}_w}^2+{I_a}^2{R}_a---\left(4\right)$

电机损失功率的百分比可表示为：

${\mathrm{\η}}_{\mathrm{loss}}=\frac{P_{\mathrm{loss}}}{P_{\mathrm{input}}}=\frac{f_v{{\mathrm{\ω}}_m}^2+{I_a}^2{R}_a}{U_a{I}_a}---\left(5\right)$

从等效电路电压平衡方程式(1)得并代入式(5)，得：

${\mathrm{\η}}_{\mathrm{loss}}=\frac{f_v{i}^2{{\mathrm{\ω}}_w}^2+\frac{{U_a}^2-2K_b{U}_{a}i{\mathrm{\ω}}_w+{K_b}^2{i}^2{{\mathrm{\ω}}_w}^2}{R_a}}{\frac{{U_a}^2-K_b{U}_{a}i{\mathrm{\ω}}_w}{R_a}}=\frac{(R_a{f}_v+{K_b}^2)i^2{{\mathrm{\ω}}_w}^2-2K_b{U}_{a}i{\mathrm{\ω}}_w+{U_a}^2}{{U_a}^2-K_b{U}_{a}i{\mathrm{\ω}}_w}---\left(6\right)$

最后将U_a＝V_su代入上式得到关于能量效率函数作为节能目标函数：

${\mathrm{\η}}_{\mathrm{loss}}=\frac{i^2(R_a{f}_v+{K_b}^2){{\mathrm{\ω}}_w}^2-2i{K}_b{V}_{s}u{\mathrm{\ω}}_w+{V_s}^2{u}^2}{{V_s}^2{u}^2-i{K}_b{V}_{s}u{\mathrm{\ω}}_w}---\left(7\right)$

此外，轮式移动机器人系统的控制约束为：

$-1\≤u=\frac{U_a}{V_s}\≤1---\left(8\right)$

电机电枢等效电路电压平衡方程式和转矩方程式组成如下方程组：

$\left\{\begin{array}{c}{I}_a{R}_a=V_{s}u-K_{b}i{\mathrm{\ω}}_w\\ J\frac{{\mathrm{d\ω}}_w}{\mathrm{dt}}+f_v{\mathrm{\ω}}_w=K_{t}i{I}_a\end{array}\right.---\left(9\right)$

式中J为电机转动惯量。

联立方程组(9)可消去I_a，得：

${\stackrel{.}{\mathrm{\ω}}}_w+J^{-1}(f_v+\frac{K_t{K}_b{i}^2}{R_a}){\mathrm{\ω}}_w=J^{-1}\left(\frac{K_t{V}_{s}i}{R_a}\right)u---\left(10\right)$

设式(10)中的则系统状态方程(10)可改写成：

${\stackrel{.}{\mathrm{\ω}}}_w+{\mathrm{\λ}}_1{\mathrm{\ω}}_w={\mathrm{\λ}}_{2}u---\left(11\right)$

4.根据权利要求1所述的基于节能考虑的轮式移动机器人轨迹跟踪控制方法，其特征在于，所述轮式移动机器人的运动学模型及关联模型为：

轮式移动机器人的运动学方程为：

$\left[\begin{array}{c}\stackrel{.}{x}\\ \stackrel{.}{y}\\ \stackrel{.}{\mathrm{\θ}}\end{array}\right]=\left[\begin{array}{cc}\cos \mathrm{\θ}& 0\\ \sin \mathrm{\θ}& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}v\\ \mathrm{\ω}\end{array}\right]=S\·q---\left(12\right)$

式中S是一个雅克比矩阵。

轮式移动机器人的位姿误差为：

$p_e=\left[\begin{array}{c}{x}_e\\ y_e\\ {\mathrm{\θ}}_e\end{array}\right]=\left[\begin{array}{ccc}\cos \mathrm{\θ}& \sin \mathrm{\θ}& 0\\ -\sin \mathrm{\θ}& \cos \mathrm{\θ}& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_r-x\\ y_r-y\\ {\mathrm{\θ}}_r-\mathrm{\θ}\end{array}\right]=T(p_r-p_c)---\left(13\right)$

式中T是一个坐标转换矩阵，将轮式移动机器人在笛卡尔坐标系{X-O-Y}的速度转换成局部坐标系{X₀-C-Y₀}中的速度。

轮式移动机器人的位姿误差微分方程为：

${\stackrel{.}{p}}_e=\left[\begin{array}{c}{\stackrel{.}{x}}_e\\ {\stackrel{.}{y}}_e\\ {\stackrel{.}{\mathrm{\θ}}}_e\end{array}\right]=\left[\begin{array}{cc}{y}_e& -1\\ {-x}_e& 0\\ -1& 0\end{array}\right]\left[\begin{array}{c}\mathrm{\ω}\\ v\end{array}\right]+\left[\begin{array}{c}{v}_r\cos {\mathrm{\θ}}_e\\ v_r\sin {\mathrm{\θ}}_e\\ {\mathrm{\ω}}_r\end{array}\right]---\left(14\right)$

对轮式移动机器人位姿误差微分方程(14)构造Lyapunov函数：

$V=\frac{1}{2}({x_e}^2+{y_e}^2)+\frac{1}{k_2}(1-\cos {\mathrm{\θ}}_e)---\left(15\right)$

式中k₂为正常数。

对式(15)中的V求时间导数可得：

$\begin{array}{c}\stackrel{.}{V}=x_e{\stackrel{.}{x}}_e+y_e{\stackrel{.}{y}}_e+\frac{1}{k_2}{\stackrel{.}{\mathrm{\θ}}}_e\sin {\mathrm{\θ}}_e\\ =x_e(v_r\cos {\mathrm{\θ}}_e-v+y_e\mathrm{\ω})+y_e(v_r\sin {\mathrm{\θ}}_e-x_e\mathrm{\ω})+\frac{1}{k_2}({\mathrm{\ω}}_r-\mathrm{\ω})\sin {\mathrm{\θ}}_e\end{array}---\left(16\right)$

相对于转向电机，驱动电机的耗能占总耗能的绝大部分，因此本发明所设计的控制方法旨在使驱动电机耗能最小而暂不考虑转向电机。

设计期望速度控制率如下：

$q_d=\left[\begin{array}{c}{v}_d\\ {\mathrm{\ω}}_d\end{array}\right]=\left[\begin{array}{c}{v_d}^{*}\\ {\mathrm{\ω}}_r+k_2{v}_r{y}_e+k_3\sin {\mathrm{\θ}}_e\end{array}\right]---\left(17\right)$

式中k₃为可正常数，v_d ^*为待定线速度控制率。

根据Barbalat引理，为了使系统误差p_e＝[x_e y_e θ_e]^T一致有界且渐近收敛于零，则要求式(16)中的 $\stackrel{.}{V}\≤0.$

将式(17)代入(16)得x_e(v_r cosθ_e-v_d)≤0，又根据轮式移动机器人模型可知其车体速度v_d与前驱动轮相对于车体的旋转角转速ω_w之间的关系式为：

v_d＝ω_wr cosβ           (18)

故要使轮式移动机器人能准确跟踪参考轨迹则必须满足关于ω_w的状态约束：

x_e(v_r cosθ_e-ω_wr cosβ)≤0        (19)

5.根据权利要求1所述的基于节能考虑的轮式移动机器人轨迹跟踪控制方法，其特征在于，所述基于节能考虑的轮式移动机器人轨迹跟踪控制策略为：

该控制问题本质上属于解决有约束的多变量优化问题。整理上述节能目标函数(7)和控制约束(8)、系统状态方程(11)及状态约束(19)，得到如下优化模型：

$\min {\mathrm{\η}}_{\mathrm{loss}}=\frac{i^2(R_a{f}_v+{K_b}^2){{\mathrm{\ω}}_w}^2-2i{K}_b{V}_{s}u{\mathrm{\ω}}_w+{V_s}^2{u}^2}{{V_s}^2{u}^2-i{K}_b{V}_{s}u{\mathrm{\ω}}_w}$

$s.t.\left\{\begin{array}{c}{\stackrel{.}{\mathrm{\ω}}}_w+{\mathrm{\λ}}_1{\mathrm{\ω}}_w={\mathrm{\λ}}_{2}u\\ x_e(v_r\cos {\mathrm{\θ}}_e-{\mathrm{\ω}}_{w}r\cos \mathrm{\β})\≤0\\ -1\≤u\≤1\end{array}\right.---\left(20\right)$

式中， ${\mathrm{\λ}}_1=J^{-1}(f_v+\frac{K_t{K}_b{i}^2}{R_a});{\mathrm{\λ}}_2=J^{-1}\left(\frac{K_t{V}_{s}i}{R_a}\right).$

能耗最小化的获得可以表示为一个最小值的搜索问题，可用遗传算法来解决。设搜索出的最优驱动轮转速为ω_w ^*，则能量最优车体线速度为v_d ^*＝ω_w ^*r cosβ。结合式(17)中运动控制器设计出的车体角速度ω_d，最后得到考虑节能的轨迹跟踪最优控制速度率为：

${q_d}^{*}=\left[\begin{array}{c}{v_d}^{*}\\ {\mathrm{\ω}}_d\end{array}\right]=\left[\begin{array}{c}{{\mathrm{\ω}}_w}^{*}r\cos \mathrm{\β}\\ {\mathrm{\ω}}_r+k_2{v}_r{y}_e+k_3\sin {\mathrm{\θ}}_e\end{array}\right]---\left(21\right)$