4WD-4WS型床椅一体化机器人室内定位与点镇定控制研究

李秀智,杨 宇,贾 桐,张祥银

(1.北京工业大学 信息学部,北京 100124； 2.数字社区教育部工程研究中心,北京 100124)

0 引言

智能床椅一体化机器人(后文简称床椅机器人)是一种新兴的智能化康复护理系统。其特点是通过一系列动作自动实现轮椅-床姿态的双向变形，且具有导航、避障、自主对接与运动控制等智能行为。其中，点镇定控制作为运动控制的首要问题，通常指控制机器人到达期望位置，并在该位置保持稳定状态，为后续机器人在某一固定位姿下实现相应的功能提供可能。

由于室内狭小空间限制以及横向床-椅对接的需要，客观要求床椅机器人具有全向运动能力。麦克纳姆轮移动平台可通过调节四轮速度就可以完成全方位移动，从而实现点镇定控制[1]。虽便于控制，但由于车轮辊子接地点不连续导致车体振动，显著降低了乘坐的舒适性。

四轮驱动及四轮转向(four-wheel driving, four-wheel steering, 4WD-4WS)型驱动设计是一种可行的替代方案，在动力学参数未知的情况下，首先考虑其运动学点镇定控制。通过阿克曼转向[2]、合成轴转向[3]、滑移转向[4]等模型计算各电机的控制量。

由于此类非完整约束移动机器人平台不满足Brokett所提出的光滑反馈镇定的必要条件，无法选取合适的连续光滑反馈控制律将移动机器人稳定到期望位姿[5]。有学者提出了鲁棒状态反馈法[6]、基于齐次理论、平均系统理论的鲁棒指数法[7]等不连续控制方法，但是在实际应用中无法得到很多理想的不连续特性，因此导致在实际中难以应用[8]。

由于不连续控制中的坐标变换能避免Brockett必要条件的限制，同时变换后的系统能够通过Laypunov函数法构造出直观的连续光滑反馈控制律且其连续性有利于在实际中应用[9-10]。因此，本文自主研发设计了一种全向床椅机器人样机，并设计了基于CAN总线与ROS分布式运动控制系统，在此基础上针对床椅机器人样机的点镇定控制问题，采用不连续坐标变换和Laypunov函数的方法来实现连续光滑反馈控制律的设计；为了提供控制器中的位置反馈，设计了超宽带(ultra wide band, UWB)与惯性测量单元(inertial measurement unit, IMU)多源信息融合的定位方案；通过实验验证了整体控制方案的有效性。

1 床椅机器人样机开发与运动学建模

1.1 机械与电气系统设计

设计研制了4WD-4WS驱动模式的全向床椅机器人样机，机器人样机包括机械结构的设计与运动控制系统的设计，两部分均采用模块化设计，提了床椅机器人的可扩展性。

各部分执行机构在系统指令的控制下完成相应动作，使机器人调整至工作中所需要的不同位姿状态，因此机器人底盘结构设计将影响其运动性能，同时也将决定运动学模型的分析与建立和运动控制系统的设计。通过Solidworks完成对床椅机器人的结构设计，其中底盘结构是重点设计部分。床椅机器人底盘主体包含四个轮组，每个轮组包括一个轮毂电机、一个直流无刷转向电机以及悬架减振机构，如图1所示。轮毂电机为主动轮提供行进的驱动力，直流无刷转向电机用于机器人的转向控制。该方式避免了四驱麦轮由于接地点不连续导致的车体振动，在保留全向运动的同时，显著提高了乘坐的舒适性。

图1 机器人底盘结构图

为提高复杂机器人系统的响应速度及可靠性，采用CAN总线与串口通信收发传感器数据和控制指令，具有较好的扩展性。软件上，机器人操作系统(robot operating system, ROS)是一种基于分布式和模块化的操作系统，有助于提高控制器数据计算与程序执行的效率[11]。鉴于上述系统和通讯方式的优点，设计了基于CAN总线通信与ROS的分布式运动控制系统，设计方案如图2所示。

图2 机器人控制系统方案图

该控制系统采用模块化设计，主要包括一下几部分：

1)主控制模块。该模块包括工控机与USB-CAN设备。床椅机器人的控制算法均在工控机的ROS操作系统下完成。ROS系统中执行节点包括算法解算节点、UWB节点、IMU节点、串口节点、驱动电机控制节点。各节点单独控制设备数据的收发，使得各控制节点具备低耦合的特点。

2)驱动电机模块。该四组伺服控制器与轮毂电机组成，轮毂电机与伺服控制器连接，并配有增量式编码器，伺服控制器通过USB-CAN设备与工控机连接。为得到稳定控制速度，通过速度闭环PID控制算法实现控制。

3)推杆电机与转向电机模块。该模块硬件设备主要包括一个STM32单片机、四组转向电机和四组推杆电机。单片机通过串口与工控机和推杆电机通信，通过CAN总线与转向电机通信。为了实现对转向电机精确的位置控制，采用双闭环PID控制，即速度闭环与位置闭环。

4)电源模块。电源模块主要为床椅机器人各个传感器以及设备提供电能，包括充电锂电池、电压显示模块、降压模块(24 V、12 V以及19 V)以及隔离开关。供电电源采用48 V锂电池，经各降压模块为各类控制器及电机提供可靠稳定的电源。

5)导航定位模块。该控制模块主要是组合导航定位系统，为床椅机器人的自主移动功能提供实时定位坐标。硬件设备包括IMU与UWB，均通过串口通信与工控机连接，并对采集的数据完成优化处理，同时将两种设备固定于床椅机器人中心处。

1.2 运动学模型建立

阿克曼转向原理[12]为4WD-4WS型运动学控制提供了设计依据。其优势在于，每个车轮均可沿自然路线运动，车轮处于纯滚动的状态且无滑移现象或者滑移的趋势[13]。根据该原理，机器人可采用前轮转向、后轮转向以及全转向等多种形式。由于单一的前轮或后轮转向形式易产生床椅机器人的重心偏移，导致侧翻、侧滑等事故，而前后轮反向偏转全轮驱动形式具备运动灵活、转弯半径小等特点，因此采用前后轮反向偏转全轮驱动形式并对其进行分析[14]。

在转向瞬间，床椅机器人的运动可看作相对于瞬时旋转中心(instantaneous center of rotation, ICR)的瞬时旋转运动，并且这个瞬时旋转中心是时变的，也就是床椅机器人的速度方向垂直于机器人中心点与ICR的连线，且每个车轮中心点同ICR的连线与每个车轮的线速度方向垂直。当ICR位于床椅机器人横向对称轴的延长线上时，旋转半径为最小，因此内侧两轮转向角大小相等，外侧两轮转向角大小相等[15]。因此通过几何关系转换，4WD-4WS模型中驱动电机与转向电机的控制输入量为式(1)～(4)中所示。

(1)

(2)

(3)

(4)

式中，r为床椅机器人的旋转半径，rl为床椅机器人的车轮半径，2L为前后两车轮的轮距，2W为左右两车轮的轮距。vin、vout为内、外侧车轮的线速度，如图3中所示线速度v1,3、v2,4；ωin、ωout为内、外侧车轮的角速度，如图2中所示角速度ω1,3、ω2,4。

图3 前后轮反向偏转示意图

2 控制器设计及分析

2.1 极坐标系下控制模型

本文设计床椅机器人系统是非完整约束系统，为了实现室内环境下，在无障碍物时能够快速镇定至期望位姿，并保持稳定，需要设计合适的控制器。若将目标姿态表示为Pg=[xgygθg]T(xg、yg、θg均为指定参数)，则在全局坐标系下，床椅机器人当前位姿与期望位姿之间的误差可以表示为:

(5)

式中，P=[xyθ]T为机器人的当前位姿，x、y和θ分别为床椅机器人当前坐标及偏航角。

如前文所述，床椅机器人作为非完整约束系统，不满足Brockett定理中提出的存在反馈控制律的必要条件，但是极坐标变换作为不连续变换方法之一，变换后的模型中非完整约束条件不再显示出现，可以通过设计连续光滑反馈控制律完成镇定控制[16]。因此在本文点镇定控制方法中，为设计出相应的控制模型与全局速度控制律，采用极坐标形式表示出在全局坐标系下机器人当前位姿与期望位姿的位姿差[7,17]。现以床椅机器人中心为坐标原点建立机器人坐标系Or-xryr，如图3所示，xr轴的正向为床椅机器人的正前方。由于点镇定过程中期望位姿点可能位于当前位置的任意方向，为便于对误差模型总体描述，定义符号函数:

(6)

式中，d为当前位置与目标位置之间的距离；α为当前时刻机器人坐标系x轴与当前位置和目标位置连线的夹角；Ψ为机器人目标偏航角与当前偏航角之间的差值，即θe。由此可见d、α符号仅代表位姿调整方向。

2.2 控制器设计及稳定性分析

为得到控制律一般形式，忽略期望位姿点所在位置的影响，选取合适的误差变量描述点镇定控制模型，使β=α-Ψ，代入式(6)可得到如下公式。

(7)

若要实现床椅机器人的点镇定控制目标，应满足式(7)中三个状态量分别收敛至0。

对式(7)两边求导可得到床椅机器人的位姿误差微分方程如式(8)所示。

(8)

为保证式(7)中三个变量均收敛到0，采用Lyapunov函数法设计控制律[18-19]，选择的Lyapunov函数如下所示。

V=exp(d2+2(1-cosα)+2(1-cosβ)+ε)

对其微分求导，并将方程式(8)代入可得：

V′=

(9)

根据以上等式设计如下控制律：

式中，ε=0.1,k1,k2,k3> 0。

将控制律代入等式(9)可以得到:

V′=-k1d2cos2α-k2k3sin2α≤0

由李雅普诺夫稳定性第二定律可得，对k1、k2、k3取合理的正值，则V’非正恒成立，即V正定，V’为负定[20]。在控制律的作用下，机器人的位姿误差将会稳定地收敛至0，从而实现床椅机器人镇定控制。由式(1)～(4)可得到驱动电机与转向电机的输入控制律，如下所示。

ωin=

ωout=

式中，ξ1、ξ2为转角系数，b1、b2、b3、b4为驱动轮转动系数，K是车体物理参数且K=L2+W2，驱动轮的转动方向与期望位姿点的所在位置相关。

3 基于卡尔曼滤波的组合定位

通常情况下，UWB定位精度约为0.1 m，且无定位累计误差，但是在室内环境下无法保证在UWB定位时，设备间无任何信号遮挡，而造成非视距(not line of sight, NLOS)情况的出现，导致定位精度严重下降，甚至出现UWB数据丢失现象[21]。因此在多形态特征的室内环境下，采用单一的定位方案，难以获取精确地实时定位坐标。由于IMU能够在不借助外界其他设备的情况下，在短时间内完成定位导航。鉴于上述UWB与IMU的定位优点，为了解决床椅机器人运动控制过程中实时准确定位的问题，在卡尔曼滤波框架下完成多源信息的融合，以实现床椅机器人位置的最优估计[21]，技术方案如图4所示。

图4 组合导航技术方案

在本方案组合导航算法中，由IMU数据的航迹推演提供一步状态预测，作为组合导航的状态方程。通过TOA定位思想，将优化后的UWB数据采用两次加权三边定位法完成坐标解算，并作为观测量，同时对预测量进行不断修正，得到最小方差意义下的最优位置估计。若出现UWB观测信息缺失的情况，为保证定位的连续性，以IMU航位推算的先验估计作为估值。现定义系统状态量为[xwywvxwvyw]T(各数值均为UWB全局坐标系w系下的数值，xw、yw为当前时刻坐标，vxwvyw分别为x轴方向、y轴方向的速度)，控制输入量为[axrayr]T(各量为机器人在机器人坐标系r系下的加速度分量)，基于卡尔曼滤波组合定位系统的状态方程与观测方程具体分别为:

Xk=AXk-1+BRk,k-1Uk-1+W

(10)

Zk=HXk+V

(11)

式中，k时刻的系统状态矢量Xk为:

状态转移矩阵A及控制输入矩阵B分别为:

其中:T为采样周期。k-1时刻到k时刻的坐标转换矩阵Rk,k-1为:

k-1时刻输入矩阵Uk-1为:

k时刻观测矢量Zk及测量矩阵H为:

V为观测噪声，V～N(0,R)；W为系统噪声，W～N(0,Q)。

4 实验结果与分析

4.1 仿真及分析

定义仿真实验中的全局坐标系为Os-xsys，且与初始状态的机器人坐标系重合，即机器人初始位姿为(0, 0, 0)。在该坐标系下，分别选取不同象限内姿态相同且位置不同的4个位姿，即(2, 2, π/4)、(-2, 2, π/4)、(2, -2, π/4)和(-2, -2, π/4)为目标位姿。考虑实际的物理约束，设置机器人最大线速度与角速度分别为vmax=1 m/s，ωmax=1 rad/s。

1)第一象限内点镇定仿真实验如图5所示。

图5 第一现象实验结果图

2)第四象限内点镇定仿真实验如图6所示。

图6 第二现象实验结果图

经仿真实验发现，在仿真实验的全局坐标系Os-xsys下，一、三象限与二、四象限的点镇定轨迹图像大致呈中心对称，现以一、四象限结果为例进行分析。如实验结果所示，点镇定轨迹图中箭头表示机器人在期望位姿与初始位姿时的姿态方向，在镇定过程中机器人的运动轨迹平滑且无明显震荡现象，且各速度及误差变量能够快速收敛至0，验证了镇定控制算法的有效性。

4.2 实验及分析

床椅一体化机器人(如图7所示)的机械部分除了上文所述的运动底盘，还包括用于变形的背部连杆机构、腿部连杆机构、扶手升降机构等。运动控制系统中主控制器为一台研华工控机，Intel i7内核；驱动轮选用5寸轮毂电机，转向电机选用大疆M3508直流无刷电机；IMU包含三轴加速度计和三轴角加速度计，UWB由四个基站和一个标签组成。经测试计算，各种传感器及电机均满足尺寸、功率和扭矩等实验需要。

图7 床椅一体化机器人整体实物图

将UWB四个基站配置为矩形分布，并建立UWB全局坐标参考系Ow-xwyw。基站A0为坐标原点Ow，基站A0指向A1的水平方向为xw轴正向，基站A0指向A2的水平方向为yw轴正向。因此基站的坐标分别为A0(0, 0)、A1(4.7, 0)、A2(0, 5.8)和A3(4.7, 5.8)。将UWB标签与IMU同时固定于床椅一体化机器人的中心处，实时获取组合导航定位坐标，具体实验场景如图8所示。设定全局坐标系下期望位姿的真值为(2.55, 3.49, π/3)，本实验设定初始状态下全局坐标系与机器人坐标系x与y轴对应平行，因此初始状态下，期望位姿在床椅机器人坐标系中为(2, 2, π/3)。

图8 点镇定实验场景图

为验证设计方案有效性，对同一期望位姿做出多组实验。图9、10、11中的(a)图为床椅机器人点镇定仿真运动轨迹与实况下运动轨迹采样点的对比图。为方便对比真实轨迹点与理想轨迹，将真实环境下点镇定控制的定位采样坐标由全局坐标系转换至机器人坐标系下。通过对比看出，理想运动轨迹与真实运动轨迹采样点基本重合，控制精度较为理想，最终使机器人镇定到期望位姿范围内。如图9、10、11中的(b)图所示，在现场镇定实验过程中，机器人当前位姿与期望位姿的x轴误差、y轴误差以及偏航角的误差均在最后收敛至同一水平。

图9 A组点镇定实验结果

图10 B组点镇定实验结果

图11 C组点镇定实验结果

图12为C组实验中床椅机器人点镇定控制达到的稳定状态，并对其做出分析。图中叉子标志为期望位姿真值位置，十字标志为床椅机器人到达稳定状态质心的真实坐标。经过测量机器人到达稳定状态质心的真实坐标为(2.49, 3.44, 0.96)。与期望位姿相比较x轴误差为0.06 m，y轴误差为0.05 m，偏航角误差为0.08 rad，该误差均为可接受的范围内，相比较UWB定位方案0.1 m的定位精度而言，本文设计组合定位系统具有更高的定位精度与实时性，同时多组实验也验证了本文所设计的点镇定控制器在真实环境下的有效性。

图12 床椅机器人稳定状态示意图

5 结束语

针对全驱麦轮床椅机器人所存在的问题，设计实现了一种具备一定减震功能的4WD-4WS全向移动床椅机器人，并设计搭建了基于CAN总线通信与ROS的分布式床椅机器人控制系统。通过分析其转向形式与驱动形式，建立了基于阿克曼转向原理的床椅机器人运动学模型。经后续运动控制实验证明所设计床椅机器人样机具有良好的稳定性。针对床椅机器人室内定位问题，设计实现了一种基于卡尔曼滤波的UWB/IMU多源信息融合的导航定位系统，同时解决了单一传感器在复杂环境下定位受限的问题。通过实验表明组合导航定位系统具有良好的实时性与较高的精确度。针对床椅机器人点镇定控制问题，通过不连续坐标变换法，采用极坐标表示出误差模型，并选用合适的变量建立出控制模型。继而，设计出基于位置闭环的反馈控制器，实现了床椅机器人的点镇定控制。多组MATLAB仿真与现场实验表明点镇定控制器具有可行性与良好的有效性。

在组合导航系统的配合下，本文设计的点镇定控制方案，能够实现床椅机器人无障碍物时的快速镇定。但是运动路径上存在障碍物时，如何在本文所设计点镇控制方案中加入避障策略，实现快速本文点镇定是下一个重点工作。