CN112180944A - 一种绳系轮式移动机器人运动控制***及方法 - Google Patents

Abstract

本发明公开了一种绳系轮式移动机器人运动控制***及方法，所述***包括控制器、左轮驱动器、右轮驱动器、系绳绞盘驱动器、系绳张力传感器、系绳、绳系轮式移动机构和导航定位装置。本发明充分考虑了极端地形局部地形变化等干扰因素，主动补偿干扰；综合考虑了车轮的驱动力矩和系绳拉力的有机配合，降低***功耗；使机器人保持系绳张力约束的同时，沿着预定的轨迹进行运动。

Description

一种绳系轮式移动机器人运动控制***及方法

技术领域

本发明属于移动机器人运动控制领域，特别涉及了一种绳系轮式移动机器人运动控制***及方法。

背景技术

行星表面很多具有探索价值的区域往往处于陡峭斜坡、沟壑等类型的极端地形之中。现有大多机器人只能在小于30度斜坡、相对平坦的岩石地形上运行和作业，不适用于探索前述极端地形区域。陡峭斜坡会削弱巡视器的牵引力，极大地降低地形的可通过性；传统的摇臂转向架结构，在陡峭斜坡上的滑动会显著增加，并且根本无法进入某些区域；沟壑对巡视器来说非常危险，在崎岖不平的地形上，巡视器还有可能跌落。

为了克服上述极端地形区域所带来的限制，目前针对陡峭斜坡、沟壑等地形设计的巡视器一类为绳系移动机器人，通过一端固定在母机器人或者锚点的系绳提供拉力或者支撑力，绳系移动机器人能够到达普通轮式机器人无法通过、进入或者定点固定探测的陡峭斜坡、沟壑等，使得对这些极端地形区域进行探测成为可能。

由于能够探索极端地形，除了行星表面探测，绳系移动机器人还可以应用到许多领域。能够自主远程探索高风险地形区域，例如接近火山的区域。此外绳系移动机器人不需要地形表面提供支撑下降，因此能够下降到坑洞中从而可以用于矿山事故中危险区域的搜索与营救任务中。

虽然现在已经提出了许多针对普通轮式移动机器人运动控制的方法，但是基本上都是针对非绳系机器人的。随着绳系轮式移动机器人的提出，亟需相应的合理的运动控制技术，合理控制车轮的驱动力矩和系绳拉力，从而使机器人能够通过普通轮式移动机器人无法穿越的障碍，按照期望的轨迹运动；同时能够快速响应极端地形局部变化造成的干扰以及滑动和跌落，降低***功耗。

发明内容

为了解决上述背景技术提到的技术问题，本发明提出了一种绳系轮式移动机器人运动控制***及方法。

为了实现上述技术目的，本发明的技术方案为：

一种绳系轮式移动机器人运动控制***，包括控制器、左轮驱动器、右轮驱动器、系绳绞盘驱动器、系绳张力传感器、系绳、绳系轮式移动机构和导航定位装置；所述绳系轮式移动机构包括左驱动轮、右驱动轮、绞盘、中心轴体和脚轮臂；左驱动轮和右驱动轮通过轴承连接于中心轴体的左右两侧，脚轮臂与中心轴体固定连接，绞盘通过轴承安装于中心轴体中央，系绳缠绕在绞盘上，系绳张力传感器固定于绞盘上或者安装在脚轮臂上，系绳穿过系绳张力传感器，系绳张力传感器测量系绳张力并将其转换为电信号后发送给控制器，导航定位装置检测机器人速度和位姿信息并发送给控制器，所述控制器包括运动学控制模块、动力学控制模块、干扰观测器模块和系绳张力控制模块，控制器对系绳张力传感器和导航定位装置发送的信息进行处理得到左轮驱动器、右轮驱动器的期望力矩以及系绳绞盘驱动器的期望系绳收放长度，左轮驱动器和右轮驱动器根据期望力矩控制左驱动轮和右驱动轮运动，系绳绞盘驱动器根据期望系绳收放长度控制绞盘收放系绳，从而实现机器人的运动控制。

基于上述绳系轮式移动机器人运动控制***的控制方法，包括以下步骤：

(1)比较上位轨迹规划***输出的期望位姿与绳系轮式移动机器人实际位姿之间的差值，将其转换到机器人本体坐标系中得到位姿误差；

(2)根据位姿误差，由运动学控制模块通过运动学控制律计算得到期望的速度和方位角速度；

(3)比较期望的速度和方位角速度与绳系轮式移动机器人实际的速度和方位角速度之间的差值，得到速度偏差和方位角速度偏差；

(4)根据干扰观测器模块估计的干扰、速度偏差和方位角速度偏差和动力学控制律，由动力学控制模块基于能耗最小原则计算最优系绳张力F^*，并通过动力学控制律计算得到期望的左轮控制力矩和右轮控制力矩，并分别传送给左轮驱动器和右轮驱动器进行双轮控制；

(5)根据绳系轮式移动机器人实际的速度和方位角速度、双轮控制力矩和最优系绳张力F^*，干扰观测器模块估计***的不确定性和干扰并输出到动力学控制模块进行补偿；

(6)根据最优系绳张力F^*和系绳张力传感器测量的系绳张力，系绳张力控制模块计算期望的系绳收放长度并传送给系绳绞盘驱动器进行绞盘控制。

进一步地，在步骤(2)中，所述运动学控制律如下：

v_c＝k₁x_e+v_dcosθ_e

上式中，k₀,k₁,k₂为控制增益，且有k₀＞0,k₁＞0,k₂＞0；v_c、ω_c分别为期望的速度和方位角速度，v_d、ω_d分别为机器人本体坐标系中期望的速度和方位角速度；x_e、y_e、θ_e分别为位姿误差中的横坐标误差、纵坐标误差和方位角误差。

进一步地，在步骤(4)中，所述动力学控制律如下：

上式中，τ_lr为双轮控制力矩，τ_lr＝[τ_l τ_r]^T，τ_l和τ_r分别为左轮控制力矩和右轮控制力矩，上标T表示转置；

M(q)为***动力学模型中的正定惯性矩阵，S为S(q)的简写形式，q＝[x y θ]^T，x,y表示机器人当前的位置，θ表示机器人当前的方位角；

G(q)为***动力学模型中的重力项；

字母上方一点表示该参数的一阶求导；K_τ为正定控制增益矩阵；u_c＝[v_c ω_c]^T，u＝[v ω]^T，v,ω为机器人实际的速度和方位角速度；

为***不确定性和干扰的估计值；

其中，r为驱动轮半径，L为两驱动轮间距的一半，a为脚轮臂长度，α为系绳与脚轮臂杆轴线之间的夹角。

进一步地，在步骤(4)中，所述最优系绳张力F^*如下：

上式中，y₁、y₂、z₁、z₂定义：

进一步地，在步骤(5)中，所述干扰观测器模块的表达式如下：

上式中，K₃和K₄为观测器增益调节矩阵；

为x的估计值，

为速度跟踪误差，

B(q)τ为***所受广义力：

采用上述技术方案带来的有益效果：

本发明能够实现极端地形沿着预定的轨迹进行运动；通过综合考虑了双轮力矩和系绳张力的能量消耗最小优化方法，降低了能量消耗；在极端地形局部地形变化等干扰因素下，通过干扰观测器主动补偿干扰，实现稳定轨迹跟踪；同时保持系绳张力在合适的范围内，降低系绳的磨损，提高其寿命。可进一步应用于通过一端固定在母机器人/锚定位置的系绳提供支撑力的绳系移动机器人中，这类机器人可以用于行星表面探测、救灾抢险等领域，因此具有广阔的应用前景。

附图说明

图1是本发明的***组成图；

图2是本发明的运动控制框图；

图3是双轮差分驱动移动机器人位姿描述示意图；

图4是位姿误差定义示意图；

图5是绳系轮式移动机器人受力示意图。

标号说明：1、控制器；2、左轮驱动器；3、右轮驱动器；4、系绳绞盘驱动器；5、系绳张力传感器；6、系绳；7、绳系轮式移动机构；8、导航定位装置；9、左驱动轮；10、右驱动轮；11、绞盘；12、中心轴体；13、脚轮臂。

具体实施方式

以下将结合附图，对本发明的技术方案进行详细说明。

如图1所示，一种绳系轮式移动机器人运动控制***，包括：控制器1、左轮驱动器2、右轮驱动器3、系绳绞盘驱动器4、系绳张力传感器5、系绳6、绳系轮式移动机构7、导航定位装置8。绳系轮式移动机构7包括左驱动轮9、右驱动轮10、绞盘11和中心轴体12、脚轮臂13，左驱动轮9和右驱动轮10通过轴承连接于中心轴体12的左右两边，脚轮臂13与中心轴体12固定连接。绞盘11通过轴承安装于中心轴体12中央，系绳6一端固定于绞盘11上，系绳6的另一端通过脚轮臂中心轴孔固定于母机器人或者一个固定锚点上。在部署前，系,6缠绕在绞盘11上，通过旋转绞盘11，可以实现系绳6的收放和缠绕。系绳张力传感器5固定于绞盘11上或者安装在脚轮臂13上，系绳6穿过系绳张力传感器5，系绳张力传感器5测量系绳6张力并且将其变换为电信号发送给控制器1。导航定位装置8可以确定机器人速度和机器人位姿，发送给控制器1统一进行处理实现控制功能。

控制器1包括运动学控制模块、动力学控制模块、干扰观测器模块、系绳张力控制模块。控制器1接收来自绳系轮式移动机器人的系绳张力F、机器人速度以及机器人位姿信息。其中系绳张力由系绳张力传感器5测得，机器人速度以及机器人位姿信息可由导航定位装置确定。动力学控制模块输出的期望力矩输出到左轮驱动器2和右轮驱动器3，左轮和右轮驱动器包括电机以及相关的电机控制等组件，根据控制器1输入的期望力矩驱动左右轮运动；同时控制器1发送期望的系绳收放长度Δx到系绳绞盘驱动器4，使绞盘11按照期望的方向旋转，通过收放系绳6来控制系绳张力，与左、右驱动轮配合，实现期望的运动和系绳张力控制。其中系绳绞盘驱动器4包括电机以及相关的电机控制等组件。

如图2所示，基于上述绳系轮式移动机器人运动控制***的控制方法，步骤如下：

步骤1，由比较器比较上位轨迹规划***输出的期望位姿x_d,y_d,θ_d与绳系轮式移动机器人***的实际位姿x,y,θ之间的差别，将其转换到机器人本体坐标系中得到位姿误差x_e,y_e,θ_e；

步骤2，根据位姿误差x_e,y_e,θ_e，由运动学控制模块通过运动学控制律计算得到期望的控制速度v_c,ω_c；

步骤3，由比较器比较期望的控制速度v_c,ω_c与实际绳系轮式移动机器人的速度和方位角速度v,ω之间的差别，得到速度和方位角速度偏差v_e,ω_e；

步骤4，根据干扰观测器模块估计的干扰

速度和方位角速度偏差v_e,ω_e以及动力学控制律，由动力学控制模块基于能耗最小原则计算最优的系绳张力F^*；进一步通过动力学控制律计算得到期望的左轮和右轮控制力矩，控制力矩分别输送给左轮和右轮驱动器对双轮进行控制；

步骤5，根据绳系轮式移动机器人实际的速度和方位角速度v,ω以及动力学控制模块输出的双轮控制力矩和系绳张力F^*，干扰观测器模块估计***的不确定性和干扰

并且输出到动力学控制模块进行补偿；

步骤6，系绳张力控制模块根据最优系绳张力F^*、系绳张力传感器输出的系绳张力计算系绳收放长度，输送给系绳绞盘驱动器控制绞盘，实现系绳张力控制。

其中，控制器中各模块的具体实现如下：

1)运动学控制模块

运行在较为平坦路面的移动机器人一般将路面简化为一个二维平面，具体一个差分驱动双轮机器人的情况如图3所示。此时移动机器人具有三个自由度，位姿描述为q＝[xy θ]^T，其中，x,y表示机器人当前的位置，θ表示机器人当前的方位角，即机器人前进方向与x轴正方向之间的夹角；由于机器人是双轮差分驱动的，所以共有两个自由度的控制输入，记为u＝[v ω]^T，其中，v表示机器人几何中心沿机器人前进方向上的速度，ω表示机器人几何中心绕z轴(垂直于xy平面)的旋转角速度。

此时，移动机器人的运动学方程为：

其中转换矩阵为：

如图4所示，在机器人坐标系中一个轮式移动机器人的位姿误差定义为：

其中，x_d,y_d,θ_d表示上位轨迹规划***输出的期望位姿轨迹，x,y,θ为移动机器人实际位姿,x_e,y_e,θ_e表示位姿误差。

运动学控制律为：

v_c＝k₁x_e+v_dcosθ_e

其中，k₀,k₁,k₂为控制增益，且有k₀＞0,k₁＞0,k₂＞0；v_c,ω_c为期望的控制速度。

2)动力学控制模块

对于一个轮式移动机器人***，其动力学模型为：

其中，M(q)为***动力学模型的正定惯性矩阵；G(q)为重力项；

表示不确定性和干扰集合；

为离心力和哥式力项，A(q)λ表示约束力矢量，其中：

A(q)＝[sinθ -cosθ 0]^T

由于绳系轮式移动机器人在极端地形运动过程中需要保持系绳处于张紧状态，故其除了受到双轮输入力矩之外，还受到系绳张力的作用，如图5所示。

根据图5可得***所受广义力为：

其中，τ_l,τ_r分别为左、右两轮的驱动力矩，F为系绳张力，a为脚轮臂长度，α为系绳与脚轮臂杆轴线之间的夹角，r为轮半径，2L为两驱动轮间距。

根据上式可得：

其中，

为简化书写和更加明了，部分S(q)简写为S。由实际中绳系轮式移动机器人的动力学方程，

因此式中去掉了该项。

对于绳系移动机器人而言，由于系绳的存在，使机器人额外受到一个系绳张力的作用，因此必须综合考虑系绳张力的影响，使张力保持在一个合适的约束范围之内，这里主要通过综合考虑双轮力矩和系绳张力所需的能量消耗最小的方式实现对系绳张力进行合理的控制。现在对广义输入力进行分解如下：

其中，τ_lr＝[τ_l τ_r]^T为2×1维输入力矩，B_τ为2×2维双轮力矩输入矩阵，B_F为2×1维系绳张力输入矩阵，具体为：

设最优系绳张力为F^*，动力学控制模块采用的控制律为：

其中，K_τ为正定控制增益矩阵，

为干扰观测器模块输出的干扰估计值，

另外，考虑到为了使三个控制输入力/力矩达到能耗最低，这里定义能量函数为：

上述动力学控制模块的控制律可以进一步写为：

其中：

将τ_l、τ_r表示为F^*的表达式，代入到能量函数中，由E最小，

可得最优的绳索张力为F^*：

将求得的F^*代入动力学控制模块的控制律中，进一步计算出对应的左右驱动轮期望输入力矩。

3)干扰观测器模块

干扰观测器为：

其中，K₃和K₄为观测器增益矩阵，

为对f的估值，

表示***动力学模型中的不确定性和干扰集合；

为对x的估值，

为速度跟踪误差，定义为：

根据τ和u_c即可得到干扰f的估计，该干扰估计值输入到动力学控制模块，用于干扰主动补偿。

4)系绳张力控制模块

系绳张力控制模块***表达式如下：

其中，K_PF，K_DF为系绳张力PD控制参数。

通过合适的机构参数，可以确定相应的参数矩阵；进一步根据具体的性能指标要求，可以确定相应的控制器参数。下面是具体实施实例一组相关参数设置。

具体所针对的极端地形为陡峭斜坡地形，斜坡相对水平面的倾角为θ_s，一个绳系双轮机器人参数如表1所示，代入下式得到相应动力学方程参数数值：

其中，m为机器人总质量，r为轮半径，2L为轮间距，

为中心轴体绕坐标轴oz的转动惯量，脚轮臂选择轻质细杆，因此质量和转动惯量忽略，

和

分别为车轮绕坐标轴oy和oz的转动惯量。

表1

构型参数	数值	单位
			轮半径	225	mm
轮间距	980	mm
			单轮质量	1.5	kg
中心轴体质量	17	kg
			中心轴体半径	110	mm

具体绳系轮式移动机器人***的控制器由嵌入式单板机、数据采集模块组成；结合电机驱动控制器实现车轮和绳索的控制。

根据上述结构参数，进行了运动学控制模块和动力学控制模块控制参数的调节。根据实际测量系绳锚点坐标为(5.0,4.8)m，θ_s＝45°，运动学控制模块控制参数为k₀＝2.5,k₁＝0.6,k₂＝0.9，动力学控制器参数为K_τ＝diag[20 10]，观测器参数K₃＝diag[5 5]，K₄＝diag[10 10]，系绳张力PD控制参数K_PF＝15，K_DF＝10。采用本发明方法所实现***获得了良好的性能，能够沿着预定的轨迹进行运动；通过综合考虑了双轮力矩和系绳张力的能量消耗最小优化方法，降低了能量消耗；在极端地形局部地形变化等干扰因素下，通过干扰观测器能够主动补偿干扰，实现稳定轨迹跟踪；同时保持系绳张力在合适的范围内，降低系绳的磨损，提高其寿命。

该方法可进一步应用于通过一端固定在母机器人/锚定位置的系绳提供支撑力的绳系移动机器人中，这类机器人可以用于行星表面探测、救灾抢险，因此该实现方法具有广阔的应用前景。

实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。

Claims (6)

1.一种绳系轮式移动机器人运动控制***，其特征在于，包括控制器、左轮驱动器、右轮驱动器、系绳绞盘驱动器、系绳张力传感器、系绳、绳系轮式移动机构和导航定位装置；所述绳系轮式移动机构包括左驱动轮、右驱动轮、绞盘、中心轴体和脚轮臂；左驱动轮和右驱动轮通过轴承连接于中心轴体的左右两侧，脚轮臂与中心轴体固定连接，绞盘通过轴承安装于中心轴体中央，系绳缠绕在绞盘上，系绳张力传感器固定于绞盘上或者安装在脚轮臂上，系绳穿过系绳张力传感器，系绳张力传感器测量系绳张力并将其转换为电信号后发送给控制器，导航定位装置检测机器人速度和位姿信息并发送给控制器，所述控制器包括运动学控制模块、动力学控制模块、干扰观测器模块和系绳张力控制模块，控制器对系绳张力传感器和导航定位装置发送的信息进行处理得到左轮驱动器、右轮驱动器的期望力矩以及系绳绞盘驱动器的期望系绳收放长度，左轮驱动器和右轮驱动器根据期望力矩控制左驱动轮和右驱动轮运动，系绳绞盘驱动器根据期望系绳收放长度控制绞盘收放系绳，从而实现机器人的运动控制。

2.基于权利要求1所述绳系轮式移动机器人运动控制***的控制方法，其特征在于，包括以下步骤：

(1)比较上位轨迹规划***输出的期望位姿与绳系轮式移动机器人实际位姿之间的差值，将其转换到机器人本体坐标系中得到位姿误差；

(2)根据位姿误差，由运动学控制模块通过运动学控制律计算得到期望的速度和方位角速度；

(3)比较期望的速度和方位角速度与绳系轮式移动机器人实际的速度和方位角速度之间的差值，得到速度偏差和方位角速度偏差；

(4)根据干扰观测器模块估计的干扰、速度偏差和方位角速度偏差和动力学控制律，由动力学控制模块基于能耗最小原则计算最优系绳张力F^*，并通过动力学控制律计算得到期望的左轮控制力矩和右轮控制力矩，并分别传送给左轮驱动器和右轮驱动器进行双轮控制；

(5)根据绳系轮式移动机器人实际的速度和方位角速度、双轮控制力矩和最优系绳张力F^*，干扰观测器模块估计***的不确定性和干扰并输出到动力学控制模块进行补偿；

(6)根据最优系绳张力F^*和系绳张力传感器测量的系绳张力，系绳张力控制模块计算期望的系绳收放长度并传送给系绳绞盘驱动器进行绞盘控制。

3.根据权利要求2所述控制方法，其特征在于，在步骤(2)中，所述运动学控制律如下：

v_c＝k₁x_e+v_dcosθ_e

上式中，k₀,k₁,k₂为控制增益，且有k₀＞0,k₁＞0,k₂＞0；v_c、ω_c分别为期望的速度和方位角速度，v_d、ω_d分别为机器人本体坐标系中期望的速度和方位角速度；x_e、y_e、θ_e分别为位姿误差中的横坐标误差、纵坐标误差和方位角误差。

4.根据权利要求3所述控制方法，其特征在于，在步骤(4)中，所述动力学控制律如下：

上式中，τ_lr为双轮控制力矩，τ_lr＝[τ_l τ_r]^T，τ_l和τ_r分别为左轮控制力矩和右轮控制力矩，上标T表示转置；

M(q)为***动力学模型中的正定惯性矩阵，S为S(q)的简写形式，q＝[x y θ]^T，x,y表示机器人当前的位置，θ表示机器人当前的方位角；

G(q)为***动力学模型中的重力项；

字母上方一点表示该参数的一阶求导；K_τ为正定控制增益矩阵；u_c＝[v_c ω_c]^T，u＝[v ω]^T，v,ω为机器人实际的速度和方位角速度；

为***不确定性和干扰的估计值；

其中，r为驱动轮半径，L为两驱动轮间距的一半，a为脚轮臂长度，α为系绳与脚轮臂杆轴线之间的夹角。

5.根据权利要求4所述控制方法，其特征在于，在步骤(4)中，所述最优系绳张力F^*如下：

上式中，y₁、y₂、z₁、z₂定义：

6.根据权利要求4所述控制方法，其特征在于，在步骤(5)中，所述干扰观测器模块的表达式如下：

上式中，K₃和K₄为观测器增益调节矩阵；

为x的估计值，

为速度跟踪误差，

B(q)τ为***所受广义力：

Images