Motion Control of Tracked Vehicle based on Contact Force Model

最新推荐文章于 2023-03-06 20:18:39 发布

Vic_Hao

最新推荐文章于 2023-03-06 20:18:39 发布

阅读量796

点赞数

分类专栏：机器人论文阅读

本文链接：https://blog.csdn.net/weixin_42018112/article/details/94364090

版权

机器人论文阅读专栏收录该内容

33 篇文章 4 订阅

订阅专栏

introduction

This paper propose an navigation method for tracked vehicles based on a contact force model that enables the robot to compensate for collisions with obstacles.

这篇文章的意思是collision是不能避免的due to slips and sensing limits。因此，the driving force should be controlled counting on collision force when the tracked vehicle’s side surfaces collide with walls while the vehicle is autonomously navigating its path.

The proposed method has three features:

passive wheels are attached to the side surfaces of the tracked vehicle
the tracked vehicle is controlled based on a dynamic model that uses driving force and collision force
this method can be applied to robots that do not have accurate force sensors attached to the active flippers, as the method only requires the simple information of whether the robot is colliding or not

Construction of dynamic model

在这里插入图片描述
像上图中这种情况，有一点特殊: 碰撞点A和旋转中心O点的连线与墙面垂直。这是因为A点一直在墙上，所以A点的速度是沿着墙面的。

整体的Dynamic Model:
$\ddot{x} = F_{1} + F_{2} + F_{5}sin\theta + F_{6}cos\theta$ $\ddot{y} = - F_{3} - F_{4} + F_{5}cos\theta - F_{6}sin\theta$ $\ddot{\theta} = F_{1}(L_{y} + d) + F_{2}(L_{y} - d) - (F_{3} + F_{4})L_{x} + F_{6} \sqrt{(L_{y} + l_{1})^{2} + (L_{x} + l_{2})^{2}}$

同时还有一个Geometrical Constraint：
$\frac{l_{2} + L_{x}}{l_{1} + L_{y}} = tan \theta$

Analysis of collision on side surface of the vehicle body

上面分析的这种情况只是tracked vehicle的一个点与墙面collision。下面我们再分析一下side surface of the vehicle和墙碰撞时候的Dynamic Model, 其实就是往上面的model继续添加约束:

这里的情况是a caster wheel on a flipper is colliding with a wall, 所以和wall的摩擦力可以忽略
$F_{6} = 0$
在加速过程中，两条履带的加速度假定是一样并且是不变的， $L_{y}$ 也是不变的。Considering $t$ as the acceleration time:
$\frac{v_{r}}{\dot{v_{r}}} = \frac{v_{l}}{\dot{v_{l}}} = t$
Based on the kinematics of a differential-drive wheeled robot:
$L_{y} = \frac{(v_{r} + v_{l})d}{v_{r} - v_{l}}$ $\dot{x} = \frac{v_{r} + v_{l}}{2}$ 把上式对时间求导，并在两边乘以 $M$ ，可以得到式子：
$\ddot{x} = \frac{1}{2}M \dot{v_{r}} + \frac{1}{2}M \dot{v_{l}}$ 把上式和动力学模型中的式子进行比较，因为 $F_{6} = 0$ 并且 $\theta$ 特别小，所以可以得到式子：
$F_{1} = \frac{1}{2}M \dot{v_{r}}$ $F_{2} = \frac{1}{2} M \dot{v_{l}}$
The friction force can’t be measured directly. 因此，这里假设the friction force is equivalent to the maximum static friction force just before the beginning of motion：
$F_{3} + F_{4} = \mu_{g}Mg$

综上，我们可以得到：
$\ddot{\theta} = \frac{Md}{t}(\frac{v_{r}^{2} + v_{l}^{2}}{v_{r} - v_{l}}) + \mu_{g}Mg \left [ (L_{y} + l_{1})tan \theta - l_{2}\right ]$

仔细分析上式，上式包含两个部分，一项driving force，一项friction force。在这个式子中，只有driving force是可控的。
Therefore, the tracked vehicle moves in the commanded direction when the term of friction becomes smaller than that of driving force. 也就是：

找到使得 $J(\ddot{\theta}) > 0$ 的command velocity，这个区域就是controllable.

Path Following Control Based on Contact Force Model

control method for collision on side surface of sub-tracks
In this method, velocity $v$ is constant; angular velocity $w$ is decided based on distance to the path, angle toward the path direction and current angular velocity. 表达式如下：
$w_{c} - (K_{1} \triangle \theta + K_{2}w_{c} + K_{3} \triangle d) \triangle t$ $v = c o n s t$
在没有collision的情况下，上式可以很好的follow path。如果发生collision的话，就不适用了。下面分析一下：
Basic idea is to select combination of left and right track speeds that located in the 上面图中的controllable area.
就用下面的表达式：
$w_{ref} = \left\{\begin{matrix} w&(if \ no\ collision) \\ w&(else\ if\ \left | w\right | < w_{max}) \\ sig(w)w_{max}&(else) \end{matrix}\right.$
$w_{ref} = \left\{\begin{matrix} v&(if \ no\ collision) \\ v + K_{p}(v - v_{c}) + K_{i} \int(v - v_{c})dt&\\ \end{matrix}\right.$
角标带c的就是current的 $v$ 和 $w$ 。
The collision is detected by using differenced of the angular velocity $w_{ref} - w_{c})$
control method for collision on tracked surface of sub-tracks
这种情况指的就是履带的前面和wall发生collision的情况。碰撞是通过measuring torque applied on flipper’s driving motor来检测的.
左边track撞到的话就原地顺时针旋转，右边track撞到的话就原地逆时针旋转。