Master-slave Interactive Control and Implementation for Minimally Invasive Vascular Interventional Robots
-
摘要: 微创血管介入手术是治疗冠心病的重要治疗手段, 手术中要求对导管、导丝等介入器械高精度递送.得益于机器人技术的高精度、可远程操作等特点, 血管介入手术机器人的研制受到了极大关注.在微创血管介入手术中, 按导管或导丝远端所处血管部位, 将介入器械递送过程分为三个阶段: 1) 主动脉阶段:导丝或导管远端位于主动脉, 需快速前送, 以减少X射线和造影剂的使用; 2) 冠脉入口阶段:导丝或导管远端进入冠脉, 此时需选择相应的病变冠脉分支; 3) 冠脉病变阶段:导丝或导管远端位于狭窄病变部位, 需要高精度操作才能使导丝或导管穿过狭窄病变.针对器械递送的不同阶段, 提出具有运动缩放的主从控制方法, 通过改变血管介入手术机器人主端和从端之间的运动缩放关系, 实现机器人从端对主端操作的放大、缩小或等比例复现.通过在微创血管介入手术机器人平台实验, 验证了机器人从端对主端操作缩放的可行性和有效性.通过操作时间和操作精度对比分析得到:在缩放因子为4时操作时间减少39.9%;在缩放因子为1/4时, 平移和旋转操作精度分别提高72.9%和77.1%.Abstract: Minimally invasive vascular intervention is an important treatment for the coronary heart disease. High-precision catheter and guidewire delivery are required in the operation. Benefited from the high accuracy and remote operation of the robot technology, the development of vascular interventional robot has attracted increasing attention. During the intervention, according to the distal end position of the guidewire/catheter, the procedure of device delivery could be divided into three stages. 1) Aorta stage: the distal end of the guidewire/catheter is in the aorta. High delivery speed is required in this stage to minimize the dosage of the X-ray and the contrast medium. 2) Coronary artery ostium stage: the distal end of the guidewire/catheter approaches the ostium of the coronary artery, and the guidewire is rotated to enter the specific artery. 3) Stenotic coronary artery stage: the distal end of the guidewire is at the stenosis, high accuracy manipulation is required. Motion scaling based master-slave control is presented for different stages of the device delivery. By changing the motion scaling factor between the master side and the slave side of the vascular interventional robot, the manipulation of the master side is magnified, minified or equally repeated at the slave side. Experiments of the minimally invasive vascular interventional robot has validated the feasibility and effectiveness of the scaling. The manipulation time is reduced by 39.9% when the scaling factor is 4, and the accuracies of translation and rotation are improved by 72.9% and 77.1%, respectively under scaling factor 1/4.
-
Key words:
- Medical robot /
- vascular interventional surgery /
- motion scaling /
- master-slave control
-
统计显示, 90%以上的交通事故都是由驾驶员的失误操作引起.近年来, 随着Google、百度等公司在无人驾驶方面的研发推动, 使得人们热切期待利用无人车辆来彻底解决交通事故问题.欲完全取代人类驾驶员, 无人车辆的自动驾驶系统需足够智能化, 以应对复杂的道路交通场景.特别地, 无人车辆需在全局路径规划得到的起点与终点之间的最优路径基础上, 根据动态交通环境以及自车运动状态, 进行局部的轨迹规划, 以实现动态避障.轨迹规划在机器人领域已被广泛研究并用于障碍物的规避[1].无人车可看作是一种轮式机器人, 因此可借鉴一些机器人领域的轨迹规划方法.当然两者的轨迹规划也有很大的不同, 例如, 无人车的运行速度相比于普通机器人来说要快得多, 行车环境也是动态变化的, 无人车的轨迹规划还需要考虑道路的结构信息以及一些交通规则.另外, 无人车在轨迹规划的过程中要考虑车辆的运动学、动力学特性以及满足轮胎等因素的约束.目前已经有很多学者对无人车的轨迹规划进行了研究[2-5], 常见的无人车轨迹规划算法包括基于特定函数的轨迹规划方法[2]、基于搜索的轨迹规划方法(随机搜索法[3]、栅格法)、基于优化的轨迹规划方法(模型预测方法[4]、人工势场法等).
人工势场是最具吸引力的路径规划算法之一, 它最早被用于机器人的路径规划[6].通过为障碍物、道路结构以及目标点分配合理的势场函数, 人工势场可为车辆规划出一条通往目标点且无碰撞的路径.人工势场的主要优势在于结构简单、可用不同的势场函数比较精确地描述各类障碍物、道路结构等影响因素.目前, 已经有许多学者将人工势场用于自动驾驶车辆的轨迹规划. Wang等[7]基于势场理论提出了一种驾驶安全势场模型, 包括静止障碍物(静止的车辆、道路边界)势场、运动障碍物势场、驾驶员行为势场, 最后根据驾驶安全势场模型可以实现车辆的轨迹规划. Wolf等[8]提出了相应的势场函数来描述行车环境中的各类影响因素, 然后用梯度下降法来进行轨迹规划. Cao等[9]根据障碍物、道路边界、目标点的特征设计了谐波函数形式的势场模型, 通过梯度下降法得到最优的轨迹.基于这种架构, Ji等[10]先通过构建环境势场, 分别用三角函数和指数函数形式来描述道路边界和障碍物, 再进行轨迹规划, 最后通过模型预测控制实现轨迹跟踪.然而, 这些方法在产生最优轨迹时, 并未考虑车辆的一些特性, 这可能导致规划出的轨迹并不适合于车辆进行跟随.
模型预测控制也可用来进行轨迹规划. Abbas等[11]基于障碍物与车辆的间距建立了避障目标函数, 然后把它引入到模型预测算法的目标函数中, 以实现障碍物的规避. Park等[12]把障碍物信息以视差的形式引入到模型预测控制的框架中进行避障. Mousavi等[13]把障碍物和道路结构以约束条件的形式引入到模型预测控制中实现障碍物规避.从上述文献可知, 基于模型预测控制的路径规划方法通常把障碍物、道路等影响因素当作是一种约束或用单一的目标函数来描述所有这些因素.当前的无人车辆或机器人的轨迹规划与跟踪控制通常分为两个独立的单元, 即采用人工势场等方法实现轨迹规划, 再通过鲁棒控制、最优控制等方法实现对规划出的轨迹进行跟踪. Huang等[14]首次提出了一种基于人工势场的模型预测控制器, 兼具轨迹规划与跟踪控制的功能.然而, 该研究采用较为简单的车辆运动学模型, 其跟踪控制精度有待进一步提高.
为此, 本文基于车辆动力学模型, 利用人工势场与模型预测控制相结合, 通过模型预测控制中的优化算法来实现最优轨迹选择, 提出一种针对轨迹规划与跟踪的统一建模方法, 即在无人车辆轨迹规划的同时, 实现对无人车辆的纵横向耦合控制.
本文的结构如下:第1节介绍无人车辆轨迹规划的框架, 主要根据车道线、环境车、目标点等多影响因素建立势场函数; 第2节利用车辆动力学建模, 并基于模型预测控制方法设计轨迹跟踪控制器, 并实现对最优局部轨迹的规划与跟踪; 第3节通过不同的交通场景, 对无人车辆轨迹规划与跟踪控制器进行验证分析; 第4节分对全文进行总结.
1. 基于人工势场的行车环境建模
行车环境建模主要是对无人车辆在行驶过程中可能遭遇的静态和动态交通环境进行建模, 以量化交通环境对无人车辆构成的风险.在正常情况下, 车辆应沿着车道中心线以一个稳定的速度前行, 当遭遇本车道前方慢行的障碍车辆时, 应执行换道超车动作, 或在遭遇他车道的干扰车辆时伺机寻找换道时机.在这个过程中, 车辆始终必须保证其既不能同其他的环境车(包括障碍车辆和干扰车辆)发生碰撞, 也不能驶出道路边界线.
假设被控车行驶的路线足够长, 那么在行车过程中经历的环境主要包括车道线(包括分道线和道路边界线)、环境车和目标点。因此, 可将行车环境对被控车的总势场$U$定义为分道线势场$U_{\rm lane}$, 道路边界线势场$U_{\rm road}$, 环境车势场$U_{\rm car}$和目标点势场$U_{\rm goal}$等各类势场的总和.
$ \begin{align} U=U_{\rm lane}+U_{\rm road}+U_{\rm car}+U_{\rm goal} \end{align} $
(1) 1.1 车道线势场
车道线包括分道线和车道边界线两类.其中, 分道线势场的作用是确保车辆沿着在车道内行驶, 避免不必要的换道.所以在分道线位置势场取最大值, 往两边减小.同时, 分道线的势场应该足够小, 以确保被控车在需要换道避障时能跨越.本文采用类高斯函数[8]进行建模.
$ \begin{align} U_{\rm lane}\left(i \right)=A_{\rm lane}\exp \left( -\frac{\left(y-y_{{\rm lane}, i} \right)^2 }{2{\sigma_{\rm lane}^2} } \right) \end{align} $
(2) 其中, $A_{\rm lane}$是分道线势场系数, $y_{{\rm lane}, i}$表示第$i$条分道线在$Y$方向上的(位置$X$为行车方向, $Y$为垂直方向, 且两者相较于道路最右侧车道的中心线上), $\sigma_{\rm lane}$表示分道线势场的收敛系数, 由车道宽度决定.图 1是道路势场示意图.图 1 (a)表示一个三车道道路结构的分道线势场示意图, 设车道宽为3 m, 则两条分道线在$Y$方向的位置分别为$y=1.5$ m和$y$ $=$ $4.5$ m.
车道边界线的作用是防止车辆驶出道路, 是严格不可逾越的, 因而被控车在靠近车道边界线时, 势场值应迅速增大, 在道路边界线达到最大值.根据这个特征, 采用一个常用于机器人轨迹规划的势场函数, 道路边界的具体势场函数形式下:
$ \begin{align} U_{\rm road}\left(j \right)=A_{\rm road}\frac{1}{\left(y-y_{{\rm road}, j} \right)^2} \end{align} $
(3) 其中, $A_{\rm road}$为道路边界势场系数, $j\in\left\{1, 2\right\} $, $y_{{\rm road}, j}$为第$j$条道路边界线的位置.两条道路边界线在$Y$方向的位置分别为$y=-1.5$ m和$y=7.5$ m, 图 1 (b)是道路边界线势场的示意图.
因此, 车道线的总势场表示为分道线与道路边界线势场的和, 如图 1 (c)所示.
1.2 环境车势场
环境车势场的建立, 可保证被控车与其周围的环境车保持相对安全, 且能够引导被控车从环境车后方换道.对环境车而言, 附近区域的危险程度的纵横向分布并不均匀, 因此横向和纵向的势场分布也有很大的差异.一般地, 在横向上, 被控车可距其1米甚至更近, 但在纵向上这个距离却非常危险.在纵向上, 除相对距离因素外, 环境车与被控车的相对速度也是影响环境车势场的一个重要因素.
以车尾中心为原点建立局部坐标系(如图 2所示), 对环境车的纵向势场进行分析.为了更安全地引导后方被控车进行换道, 在环境车后方增加一个三角形区域, 在其前方增加一个矩形区域, 这两个区域跟车身所在区域一样, 也是被控车不可逾越的区域.车身所在区域, 即$x\in[0, L]$, 纵向势场取最大值$A_{\rm car}$.
车辆前方, 即$x\in[L, L+s]$, 这个势场主要是为了防止相邻车道的环境车在换至本车道时与被控车过于靠近.距离$s$与当前车辆的速度相关, 速度越快, 距离应该相应增大, 所以$s$的计算公式为$s=\rho$ $\times$ $V+S_{\rm min}$, 在这个范围内的纵向势场值也取最大值$A_{\rm car}$, 其中$V$表示当前环境车的速度, $\rho$是比例系数, $S_{\rm min}$表示设定的最小安全距离.
车辆后方的纵向势场比较特殊, 分两部分考虑.
1) $x\in[-S, 0]$, 这部分的纵向势场值也取最大值, 作为一个不可靠近的区域, $S$的计算方式如下:
$ \begin{align} S=V_{r}\times\Delta T+S_{\rm min} \end{align} $
(4) 其中, $V_{r}$表示相对速度, $\Delta T$表示延迟时间.
2) $x <-S$, 当环境车的速度比被控车快, 即相对速度小于0时, 该势场为0;当相对速度大于0时, 势场值应该与到$(-S, 0)$的距离成反比, 距离越近势场值越大, 势场函数采用汤川势形式[8], 这种形式的函数在远距离的时候, 随着距离的减小上升比较平缓, 近距离的时候则上升迅速, 符合环境车与被控车之间的交互, 具体如下:
$ \begin{align} U_{\rm car, long}=A_{\rm car, long}\frac{{\rm e}^{-\lambda K}} {K} \qquad \end{align} $
(5) 其中, $A_{\rm car, long}$是纵向势场系数, $K$表示到$(-S, 0)$的距离.在计算$K$之前, 对被控车的横坐标进行一个转化, $x^*=x\times\tau$, 其中$\tau\in(0, 1]$, 使被控车与环境车的距离比实际更近一点, 这样可以在很安全的距离引导被控车进行换道. $\tau$的值由相对速度决定, 相对速度越大, $\tau$值越小.图 3是不同$\tau$值下的纵向势场值变化曲线.实验发现, 当相对速度小于5 m/s时, $\tau$取0.5左右比较合适.
图 3 环境车的纵向势场值随距离变化图Fig. 3 Longitudinal potential of the environment vehicle with respect to the relative distance环境车的整体势场可在其纵向势场的基础上向外延伸, 并采用一个类高斯函数[14]的形式进行计算.因此, 环境车的总势场计算方式如下:
$ \begin{align} U_{\rm car}=U_{\rm car, long} \exp\left(-\frac{d^2}{2\sigma_{\rm car}^2}\right) \end{align} $
(6) 其中, $\sigma_{\rm car}$表示环境车势场的收敛系数, 决定了横向的影响范围, 根据车道宽来决定. $d$表示横向距离, 如图 2所示, 当计算点处于区域1, 2, 3时, $d$值分别取$d_{1}$, $d_{2}$, $d_{3}$.图 4为环境车周围势场的分布示意图, 该环境车在坐标(50 m, 0)点处, 与被控车之间的相对速度为3 m/s.
1.3 目标点势场
目标点势场也称为方向势场, 该势场的作用是保证被控车朝车道前方行驶, 即车辆前方的势场值要低于后方的势场值, 势场点离被控车越远, 势场值就越小, 势场值与该距离成反比, 但总是正值.采用的势场函数如下:
$ \begin{align} U_{\rm goal}=\varepsilon-\kappa\times\left(x-x_{\rm car} \right) \end{align} $
(7) 其中, $\varepsilon$是正的势场常数, $\kappa$是较小的势场系数, 目标点势场可使被控车保持向前行驶的趋势.
2. 基于模型预测控制的轨迹跟踪控制
行车环境的人工势场建模为无人车辆规避风险提供了可供选择的轨迹, 在此基础上, 本文通过模型预测控制器中的优化算法对轨迹进行决策, 以规划出最优轨迹, 并进行控制输出, 实现无人车辆轨迹规划与跟踪的统一建模.
2.1 车辆动力学建模
在设计模型预测控制器之前, 首先选择单轨模型来进行车辆动力学建模, 具体如下:
$ \begin{align} \begin{cases} m\times a_{x}=\sum F_{x}\\ m\left(\dot{v}+u\times r \right)=\sum F_{y} \\ I_{z} \dot{r}=\sum M_{z}\\ \dot{\varphi}=r \\ \dot{X}=u\cos\varphi-v \sin\varphi \\ \dot{Y}=u \sin\varphi+v \cos\varphi \end{cases} \end{align} $
(8) 其中, $m$为车辆质量, $u$, $v$, $r$分别表示纵向速度、横向速度和横摆角速度. $I_{z}$表示车辆绕$z$轴的转动惯量, $\sum F_{x}$, $\sum F_{y}$, $\sum M_{z}$分别表示车辆所受的纵向力、横向力、横摆力矩. $X$, $Y$表示车辆在大地坐标系下的位置, $\varphi$表示车辆的横摆角.
假设车辆为前轮转向, 并且考虑到车辆的前轮转角比较小, 可得
$ \sum F_{y}=F_{cf}+F_{cr} $
(9) $ \begin{align} \sum M_{z}=aF_{cf}-bF_{cr} \end{align} $
(10) 其中, $F_{cf}$, $F_{cr}$分别表示前后轮受到的侧向力; $a$, $b$分别表示前、后轴距.轮胎的纵向力和侧向力可分别表示为以轮胎侧偏角、滑移率、路面附着系数与垂向载荷为参数的函数.若路面附着系数已知, 垂向载荷为
$ \begin{align} F_{zf}=\frac{bmg}{2\left(a+b \right)}, ~~~ F_{zr}=\frac{amg}{2\left(a+b \right)} \end{align} $
(11) 由于前轮转角较小, 根据线性轮胎模型可得
$ \begin{align} F_{cf}=C_{f}\times\alpha_{f}, ~~~F_{cr}=C_{r}\times\alpha_{r} \end{align} $
(12) 其中, $C_{f}$, $C_{r}$分别为前后轮侧偏刚度, $\alpha_{f}$, $\alpha_{r}$分别为前后轮侧偏角.侧偏角的计算如下:
$ \begin{align} \begin{cases} \beta=\dfrac{v}{u}\\ \alpha_{f}=\beta +\dfrac{ar}{u}-\delta \\ \alpha_{r}=\beta -\dfrac{br}{u} \end{cases} \end{align} $
(13) 其中, $\beta$为质心侧偏角.侧偏刚度可由轮胎侧向力与侧偏角的关系曲线获得, 在侧偏角比较小的时候, 两者近似为线性关系.
综上所述, 可得车辆动力学模型如下:
$ \begin{align} \begin{cases} \dot{X}=u \cos\varphi-v \sin\varphi \\ \dot{Y}=u \sin\varphi+v \cos\varphi \\ \dot{\varphi}=r \\ \dot{u}=a_{x} \\ \dot{v}=Av+Br+C\delta \\ \dot{r}={A}^{'}v+{B}^{'}r+{C}^{'}\delta \end{cases} \end{align} $
(14) 其中,
$ \begin{align*} & A=-\frac{{C_{f}+C_{r}}}{{m\times u}}\\ & B=-u+\frac{{b\times C_{r}-a\times C_{f}}}{{m\times u}}, ~~~C=\frac{{C_{f}}}{{m}}\\ &{A}{'}=\frac{b\times C_{r}-a\times C_{f}}{I_{z}\times u}\\ &{B}{'}=-\frac{a^2\times C_{f}+b^2\times C_{r}}{I_{z}\times u}, ~~~~~{C}{'}=\frac{a\times C_{f}}{I_{z}} \end{align*} $
在该状态空间系统中, 状态量选取为$[X, Y, \varphi$, $u$, $v$, $r]^{\rm T}$, 控制量选取为$[a_{x}, \delta]^{\rm T}$.
2.2 模型预测控制器设计
一般地, 模型预测控制器主要包括预测模型的建立、目标函数的设计和约束条件的确定三部分.其中, 预测模型是基础, 主要作用是根据对象的历史信息和未来的控制输入序列, 预测出系统未来的输出.为了能够预测车辆在未来一个时域内的状态, 需要将连续的车辆动力学模型进行离散化, 得到离散化后的预测模型.
$ x\left(t+1 \right)=f\left(x\left(t \right), u\left(t \right) \right) $
(15) $ \begin{align} u\left(t \right)=u\left(t-1 \right)+\Delta u\left(t \right) \end{align} $
(16) 其中, 控制量是车辆的纵向加速度和前轮转角.为实现无人车辆局部路径规划与跟踪的统一建模, 本文在考虑车速跟踪、控制增量的同时, 将行车环境势场作为一部分增加到模型预测控制器的目标函数中, 利用其优化算法实现轨迹规划.该目标函数的形式如下:
$ \begin{align} &J\left(x\left(t \right), u\left(t-1 \right), \Delta \pmb U\left(t \right) \right)= \nonumber\\ &\qquad\sum\limits_{i=1}^{N_{p}}\parallel U_{APF}\left(t+i| t \right)\parallel^2_{Q}\, + \nonumber\\ &\qquad\sum\limits_{i=1}^{N_{p}}\parallel V\left(t+i| t \right)-V_{des}\parallel^2_{R}\, + \nonumber\\ &\qquad\sum\limits_{i=1}^{N_{c}}\parallel \Delta u\left(t+i-1| t \right)\parallel^2_{S} \end{align} $
(17) 其中, $\Delta \pmb U(t)$表示$t$时刻下的优化控制输入, $x(t)$表示当前时刻$t$下的状态量, $u(t-1)$表示上一时刻的控制量, $(t+i|t)$表示在当前时刻$t$之后$i$步的预测值, $N_{p}$表示预测时域, $N_{c}$表示控制时域, $V_{des}$表示车辆在无障碍下的一个期望速度, $Q$, $R$, $S$分别代表各部分的权重.目标函数主要包括三部分: 1)环境势场, 通过建立的车辆动力学模型结合势场函数计算出预测时域内被控车所受的势场值, 势场值越大, 则行车风险越大, 局部路径的规划主要取决于这一部分; 2)为了保证车辆在没有干扰的情况下, 能够维持当前的速度行驶; 3)控制增量, 控制增量最小, 可避免车辆运动状态出现大幅变化.
对于约束条件的设计, 首先考虑对控制过程中的控制量和控制增量进行约束.根据现有汽车理论及相关文献, 纵向加速度一般取值为$-$0.4 g $\sim$ 0.2 g, 前轮转角一般取值在$-$25$^\circ$ $\sim$ 25$^\circ$之间.由于在目标函数中采用了控制增量, 因而也须对其进行约束, 加速度变化率一般在$-$2 g/s $\sim$ 2 g/s之间, 前轮转角速度一般为$-$9.4$^\circ$/s $\sim$ 9.4$^\circ$/s.
同时, 考虑到当车辆行驶在附着系数较低的路面时, 须确保操纵稳定性.车辆的质心侧偏角或轮胎侧偏角是衡量车辆操纵稳定性的关键之一.文献[15]分析了质心侧偏角对车辆稳定性的影响, 给出了判断稳定性的准则.车辆在极限工况下之所以会失稳, 主要是因为轮胎与地面产生的轮胎力达到饱和.轮胎力饱和与轮胎侧偏角存在一定关系, 因而可根据路面附着条件来约束轮胎侧偏角.轮胎侧偏角的计算方法如式(13), 轮胎侧偏角的约束如下:
$ \begin{align} \alpha_{\rm min}\leq\alpha_{f, r}\leq \alpha_{\rm max} \end{align} $
(18) 其中, $\alpha_{\rm min}$和$\alpha_{\rm max}$分别为轮胎侧偏角的下限和上限.根据轮胎模型, 轮胎力饱和时轮胎侧偏角与路面附着条件有关.一般地, 低附着路面条件下的轮胎侧偏角极限值应限制在$[-2^{\circ}, 2^\circ]$.
因此, 无人车辆轨迹规划与跟踪可统一描述为如下优化问题:
$ \begin{align} &\min\limits_{\Delta \pmb U(t)}\left\{J\left(x\left(t \right), u\left(t-1 \right), \Delta \pmb U\left(t \right) \right) \right\} \nonumber\\ & \ {\rm s.t.}\quad x\left(k+1 \right)=f\left(x\left(k \right), u\left(k \right) \right) \nonumber\\ &\qquad\ \ u\left(k \right)=u\left(k-1 \right)+\Delta u\left(k \right) \nonumber\\ &\qquad\ \ u_{\rm min}\left(k \right)\leq u_{\rm min}\left(k \right)\leq u_{\rm max}\left(k \right) \nonumber\\ &\qquad\ \ \Delta u_{\rm min}\left(k \right)\leq \Delta u_{\rm min}\left(k \right)\leq \Delta u_{\rm max}\left(k \right) \nonumber\\ &\qquad\ \ \alpha_{\rm min}\leq\alpha_{f, r}\leq \alpha_{\rm max} \end{align} $
(19) 在每个控制周期内完成求解后, 得到控制增量序列如下:
$ \begin{align*} \Delta \pmb U^{*}_{t}=\left[\Delta u^{*}_{t}, \Delta u^{*}_{t+1}, \cdots, \Delta u^{*}_{t+N_{C}-1}\right]^{\rm T} \end{align*} $
将上述控制增量序列的第1个元素, 作为实际的控制输出增量作用于系统.
3. 实验与验证
3.1 实验场景设计
无人车辆的行车环境动态多变, 为简化对本文涉及的模型预测控制器进行验证, 选取了3种典型交通场景, 包括普通超车、相邻车道有干扰的超车、跟车.如图 5所示, 第1个场景假设超车时相邻车道空闲, 车辆可保持当前车速对前方慢速车辆进行超越; 第2个场景假设车辆在超车时相邻车道有其他环境车辆干扰, 此时被控车须降速行驶并等待超车时机; 第3个场景假设前方各车道都有慢速车辆, 此时被控车只能减速并跟随前方的慢速车辆.这三个场景都在较好的路面附着条件下完成.
此外, 进行车辆的操纵稳定性实验.即在不同的路面附着条件下进行双移线实验.
3.2 实验结果及分析
通过车辆动力学仿真软件CarSim与MATLAB/Simulink的联合仿真进行验证, 模型预测控制器及仿真车辆的参数如表 1所示.
表 1 控制器参数Table 1 Controller parameters参数 值 单位 参数 值 单位 $\sigma_{\rm lane}$, $A_{\rm lane}$ 0.8 $-$ $I_{z}$ 2 031 ${\rm kg}\cdot {\rm m}^{2}$ $\sigma_{\rm car}$ 0.53 $-$ $m$ 1 231 ${\rm kg}$ $A_{\rm road}$ 1 $-$ $a$, $b$ 1.04, 1.56 $\rm m$ $A_{\rm car}$ 15 $-$ $\varepsilon$, $\kappa$ 2, 0.01 $-$ $S_{\rm min}$ 3 ${\rm m}$ $C_{f}$ 61 224 $\rm N/rad$ $\rho$ 0.3 $-$ $C_{r}$ 42 500 ${\rm N/rad}$ $\lambda$ 0.5 $-$ $N_{p}$ 25 $-$ $A_{\rm car, long}$ 10 $-$ $N_{c}$ 2 $-$ 值得注意的是, 对于无人车辆的纵向控制, 控制器输出的是车辆纵向加速度.实际过程中, 应将纵向加速度转化为纵向合力, 根据车辆传动系和制动系的逆模型, 计算得到对应的节气门开度和制动压力.本文直接将纵向加速度输出给CarSim软件, 并由该软件自带的速度调节器来调节车速.
图 6是各场景关键时刻势场分布图, 图 7是各场景被控车速度变化图.场景1为普通超车, 前方有两辆慢速行驶的环境车分处两个车道.在此场景中, 被控车在遇到慢速环境车进行换道时, 相邻车道一直存在足够换道空间. 图 6 (a)为首次换道时的势场分布, 实心点表示当前被控车的位置, 星号为预测轨迹.可见, 预测轨迹开始偏向相邻车道, 被控车将要进行换道.图 7 (a)是被控车的纵向速度随时间变化图, 整个过程中的被控车速度始终维持在预定速度25m/s左右, 只在两次换道过程中有略微下降.
图 7 各场景的被控车速度变化图Fig. 7 Speed variation diagram of the subject vehicle in several scenarios场景2是在相邻车道存在干扰的超车.在此场景中, 除前方有慢速行驶的环境车外, 被控车的侧方也有环境车干扰.由于受到侧方环境车的影响, 被控车在靠近前方慢速车辆时无法立即进行换道, 只能先减速跟随, 等待相邻车道有足够安全的换道空间.图 6 (b)为被控车周边的势场分布, 从中可知, 由于侧方环境车2的势场影响, 预测轨迹并未偏向相邻车道.当被控车减速行驶一段时间后, 即相邻车道有足够安全空间时, 被控车才加速并进行换道.此刻势场分布与场景1类似, 预测轨迹会偏向相邻车道.图 7 (b)是整个过程中被控车的纵向速度响应, 被控车的纵向速度在经历一段时间下降后, 会在换道时机出现时加速, 直至换道完成便上升至预定的车速.
场景3为普通跟车.在此场景中, 本车道和相邻车道的前方均有慢速行驶的环境车, 因而被控车无法实施换道超车, 只能降速并保持跟前方慢速行驶的环境车一致.如图 6 (c)所示, 由于相邻车道环境车2的势场影响, 被控车在此时的预测轨迹也未偏向相邻车道.图 7 (c)是在该场景中的被控车纵向速度响应, 在其行驶一段时间后, 速度开始下降, 直至与前方环境车1的车速一致, 并匀速行驶.
图 8是控制量纵向加速度和前轮转角的变化情况, 左侧从上到下依次是场景1、场景2、场景3中被控车控制量的变化情况, 右侧是控制增量变化情况.由图 8可知, 控制量以及控制增量的变化范围都在约束范围之内.
图 8 各场景的控制增量以及控制量变化图Fig. 8 Responses of the control increment and the control quantity variation in several scenarios在仿真过程中, 被控车的其他运动状态还包括轨迹、侧向加速度、横摆角与横摆加速度, 如图 9所示.图 9 (a)是3个场景中的被控车轨迹图, 场景1中的被控车进行了两次换道; 场景2中的被控车进行了一次换道, 且换道时间相比场景1的首次换道要晚, 这是因为受侧方环境车2的影响, 一开始没有足够的安全换道空间; 场景3中的被控车则一直在本车道行驶.
图 9 仿真场景中被控车的轨迹、侧向加速度、横摆角、横摆角速度的响应图Fig. 9 Responses of the trajectory, lateral acceleration, yaw angle and yaw rate of the controlled vehicle in the simulation scenarios同样, 被控车在3个场景中的运动变化也反映在侧向加速度、横摆角与横摆角速度等车辆运动状态的响应中(如图 9所示), 即场景1中被控车的运动状态会有两次变化, 这两次变化的趋势大致对称, 方向正好相反; 场景2中被控车的运动状态出现1次变化, 时间较场景1中第1次变化晚; 场景3中被控车的侧向加速度、横摆角、横摆角速度则基本维持在0左右, 表明被控车始终未进行换道.
场景4是不同附着系数路面的双移线实验.选取高附着路面(附着系数$\mu=0.9$)以及低附着路面(附着系数$\mu=0.3$)分别进行实验, 对比结果如图 10所示.
图 10 场景4的行车轨迹和轮胎侧偏角响应图Fig. 10 Responses of the trajectory and the slip angle in Scenario 4在高附着条件下, 轮胎侧偏角最大值未超过4$^\circ$, 轨迹比较平顺.但是, 在低附着路面且未引入轮胎侧偏角约束的条件下, 车辆的行车轨迹在140米处有明显抖动, 且轮胎侧偏角最大值超过了6$^\circ$; 当引入轮胎侧偏角约束后, 其值始终限定在$-2^\circ$ $\sim$ $2^\circ$, 且车辆的双移线行驶轨迹相比无约束情况明显减少了抖动, 有效防止出现轮胎力饱和的情况, 提高了车辆行驶的稳定性.
4. 结束语
本文提出一种针对无人车辆轨迹规划与跟踪控制的统一建模方法, 通过人工势场来描述行车环境, 并将行车环境势场引入到模型预测控制的目标函数中, 以实现轨迹规划与跟踪的统一建模.由于在进行模型预测控制器设计时, 采用了纵横向耦合的车辆动力学模型, 因而可以在无人车辆路径规划的过程中实现车辆的纵横向控制.
-
图 1 微创血管介入手术机器人活体动物实验场景图
Fig. 1 In vivo animal trial setup of minimally invasive vascular interventional robot
图 2 血管介入机器人的主从控制框图
Fig. 2 Master-slave control diagram of vascular interventional robot
图 4 导引导丝、支架导管及电磁跟踪传感器
Fig. 4 Experimental guidewire, catheter and electromagnetic tracking sensors
图 6 不同比例因子下主端和导丝的平移距离
Fig. 6 The distances of the master side and the guidewire in different scaling factors
图 8 不同比例因子下操作时间与误差对比
Fig. 8 The comparison of time expense and error in different scaling factors
表 2 不同递送阶段比例缩放策略
Table 2 Motion scaling method for different delivery stage
阶段 运动缩放因子 缩放效果 第一阶段(主动脉) µ > 1, ψ > 1 放大 第二阶段(冠脉入口) µ = 1, ψ = 1 等比例 第三阶段(狭窄病变) µ < 1, ψ < 1 缩小 
下载: 导出CSV
-
[1] [2] [3] [4] [5] [6] [7] [8] [9] [10] [11] [12] [13] [14] [15] [16] [17] [18] [19] [20] [21] [22] [23] [24] [25] [26] [27] [28] 期刊类型引用(14)
1. 李辉，周浩，赵岩，张建华，张天晶. 血管介入机器人导管递送力实时检测系统研究. 生物医学工程学杂志. 2022(02): 359-369 . 
百度学术2. Guang-Yao ZHAI，Zheng CHEN，Rui-Fang LIU，Yong-He GUO，Jian-Long WANG，Tie-Nan SUN，Jing XIE，Tao HUANG，Yu-Jie ZHOU. First-in-human evaluation of an independently developed Chinese robot-assisted system for percutaneous coronary intervention. Journal of Geriatric Cardiology. 2022(10): 743-752 . 必应学术3. 张亮，李攀，李博，张必利. 机器人辅助冠脉介入治疗的动物实验研究. 中国急救复苏与灾害医学杂志. 2022(12): 1557-1559 . 百度学术4. 张俊峰，胡陟，齐金龙，崔国华. 基于滑模控制和广义预测控制的主从控制系统设计. 传感器与微系统. 2021(03): 81-83+89 . 百度学术5. 吴敏，徐兴雷，尚猛. 基于双种群鲨鱼优化算法的自抗扰机械臂轨迹跟踪. 计算机应用与软件. 2021(06): 231-237+270 . 百度学术6. 马旭，周金鹏，张旭，周琪. PID神经网络控制的手术机器人导管操作系统. 天津理工大学学报. 2020(03): 18-22+29 . 百度学术7. 陆清声. 血管腔内介入手术机器人的特点和未来发展方向. 机器人外科学杂志(中英文). 2020(04): 231-235 . 百度学术8. 王洪波，关博，闫勇敢，王晔，王旭升. 微创血管介入手术机器人的力反馈与导丝定位精度分析. 中国科技论文. 2020(11): 1260-1269 . 百度学术9. 赵含霖，谢晓亮，奉振球，刘市祺，周小虎，侯增广. 血管介入手术机器人系统综述. 中国医疗设备. 2020(12): 11-16 . 百度学术10. 陈世明，严伟玮，韩世鹏，张一. 微创介入机器人系统设计与精度分析. 测控技术. 2019(01): 49-53 . 百度学术11. 王国帅，王利，张一，严伟玮，欧方亮，韩世鹏. 基于仿生手指的血管介入手术机器人设计与分析. 机床与液压. 2019(09): 81-84 . 百度学术12. 陈果，刘金根，韩世鹏. 基于导管机器人的变论域模糊PID控制研究. 计算机应用研究. 2018(02): 471-474+482 . 百度学术13. 陆清声. 血管腔内介入手术机器人的探讨. 中国血管外科杂志(电子版). 2018(04): 237-239 . 百度学术14. 郑炬炬，孙志峻，闫鹤，杨建林，郭语，钱丰. 运用中空超声电机的血管介入手术机器人系统. 振动.测试与诊断. 2021(05): 976-983+1037-1038 . 百度学术其他类型引用(43)
-
下载:
计量
- 文章访问数: 2745
- HTML全文浏览量: 642
- PDF下载量: 1650
- 被引次数: 57
