复合移动机器人运动学建模+协同控制

扬之水，白石凿凿

已于 2022-10-12 22:15:24 修改

阅读量2.3k

点赞数

分类专栏：论文阅读分享文章标签：算法自动驾驶

于 2022-10-11 14:06:20 首次发布

本文链接：https://blog.csdn.net/qq_45494729/article/details/127243663

版权

论文阅读分享专栏收录该内容

1 篇文章 1 订阅

订阅专栏

本博客主要内容是论文阅读和翻译理解

论文地址：A New Performance Index for the Repetitive Motion of Mobile Manipulators | IEEE Journals & Magazine | IEEE Xplorehttps://ieeexplore.ieee.org/document/6514557

摘要：

移动机械手是一种由移动平台和固定在平台上的固定机械手组成的机器人装置。为了实现移动机械手的重复运动控制，移动平台和机械手必须同时实现重复运动。为此，本文首次设计并提出了一种新的二次型性能指标，并采用神经动力学方法对其有效性进行了分析。在此基础上，结合移动机械手的准则、物理约束和综合运动学方程，提出了一种重复运动方案，并将其转化为具有等式和约束的二次规划(QP)。此外，建立了两个重要的桥定理，证明了这样的QP可以等价地转化为线性变分不等式，然后等价地转化为分段线性投影方程。提出了一种基于PLPE的实时数值算法，并将其应用于得到的QP的在线求解。两个跟踪路径任务验证了重复运动方案的有效性和准确性。此外，非重复运动和重复运动的比较进一步验证了所提方案的优越性和新颖性。

复合移动机器人的模型建立

复合移动机器人由轮式移动平台和六自由度空间机械手组成。该移动平台包括两个独立驱动轮和两个全向被动支撑轮。在本文中，只考虑了末端执行器的位置，因此机械手成为了一个功能冗余的机械手。

P0:双驱动轮轴中点;其在世界坐标系中的坐标为(x0, y0, z0);

PC:机械手与移动平台的连接点;其在世界坐标系中的坐标为 $\left ( x_c,y_c,z_c \right ),z_c=0$

d: $P_0$ 和 $P_c$ 之间的距离

b:驱动轮和 $P_0$ 之间的距离

r:驱动轮的半径

$\phi$ ：小车移动平台的航向角，它的导数是航向角的角速度

$Q$ ：移动平台围绕其旋转的假想点;

R：假想点 $Q$ 到小车左驱动轮的距离R

$w$ :移动平台绕点Q旋转时的转速;数值上 $w=\phi \dot{}$

$\dot{\varphi _1},\dot{\varphi _r}$ :左右轮的转动速度

值得指出的是，移动平台和机械臂的各个环节都是刚性的，移动平台的运动仅在 $XOY$ 平面内，在平台不发生侧滑的假设下，左右轮的速度都严格的垂直于驱动轮轴

$\dot{\phi}=\frac{r \dot{\varphi}_1}{R}=\frac{\dot{x}_0 \cos \phi+\dot{y}_0 \sin \phi}{R+b}=\frac{r \dot{\varphi}_{\mathrm{r}}}{R+2 b}$ （1）

可以发现线速度XC轴方向P0和Pc是一致的

${\dot{x}_0 \cos \phi+\dot{y}_0 \sin \phi}={\dot{x}_C \cos \phi+\dot{y}_C \sin \phi}$

上述公式是在XOY平面的计算，当我们把坐标系迁移到 $X_CP_CY_C$ ,根据前面的假设在移动平台不打滑的情况下， $Y_C$ 方向上的速度为0

得到：

${\dot{x}_C \sin \phi-\dot{y}_C \cos \phi}+d\dot{\phi }=0$

结合

$\begin{aligned} &\dot{\phi}=\frac{r}{2 b}\left(\dot{\varphi}_{\mathrm{r}}-\dot{\varphi}_{\mathrm{I}}\right) \\ &\dot{x}_{\mathrm{C}}=\left(\frac{r}{2} \cos \phi+\frac{d r}{2 b} \sin \phi\right) \dot{\varphi}_{\mathrm{I}}+\left(\frac{r}{2} \cos \phi-\frac{d r}{2 b} \sin \phi\right) \dot{\varphi}_{\mathrm{r}} \\ &\dot{y}_{\mathrm{C}}=\left(\frac{r}{2} \sin \phi-\frac{d r}{2 b} \cos \phi\right) \dot{\varphi}_{\mathrm{I}}+\left(\frac{r}{2} \sin \phi+\frac{d r}{2 b} \cos \phi\right) \dot{\varphi}_{\mathrm{r}} \end{aligned}$

其中 $\dot{x_C}$ 和 $\dot{y_C}$ 分别表示xC和yC的时间导数。因此，将上述方程改写为一个紧凑的矩阵形式，得到移动平台的数学模型如下:

$\\A\dot{\varphi }=\dot{\phi }\\B\dot{\varphi }=\dot{p}_C$

其中 $\dot{\varphi }=[\dot{\varphi _{1}},\dot{\varphi _{r}}]^{T}$ 同时 $\dot{p}_C=[\dot{x_C},\dot{y_C}]^{T}$ 另外A和B的矩阵为：

$A=\frac{r}{2 b}\left[\begin{array}{c} -1 \\ 1 \end{array}\right]^{\mathrm{T}}, B=\frac{r}{2}\left[\begin{array}{cc} \cos \phi & -\sin \phi \\ \sin \phi & \cos \phi \end{array}\right]\left[\begin{array}{cc} 1 & 1 \\ -d / b & d / b \end{array}\right] .$

该篇论文中移动平台的r=0.1025m,b=0.32m,d=0.1m

六自由度机械手的运动学研究

对于固定坐标系而言，正运动学方程（关节角 $\theta$ 到末端tcp的 $r_b$ 的位姿变换）可以直接相对极坐标系XCYCZC坐标系建立

$f(\theta )=r_b$

对于一个已知的机械臂构型，上述的f()函数已知，首先对于机械臂建立DH模型，可以求得从基座标系到末端坐标系的变换矩阵T

移动机械手的综合运动学

根据上述移动平台和机械臂的运动学方程建立综合运动学模型。

对于移动机械手，其驱动器的运动是相对于世界坐标系的，而不是相对于基础坐标系的。因此，采用从基础坐标系到世界坐标系的变换矩阵，可以得到一个新的关于世界坐标系的移动机械手的运动学积分方程，

$r_{\mathrm{w}}=\left[\begin{array}{cccc} \cos \phi & -\sin \phi & 0 & x_{\mathrm{C}} \\ \sin \phi & \cos \phi & 0 & y_{\mathrm{C}} \\ 0 & 0 & 1 & 0 \end{array}\right]\left[\begin{array}{c} f(\theta) \\ 1 \end{array}\right]:=\left[\begin{array}{c} x_{\mathrm{C}} \\ y_{\mathrm{C}} \\ 0 \end{array}\right]+g(\theta, \phi)$

上述方程是移动操纵器相对于世界坐标系在位置层面的运动学模型。

对上式进行微分处理，就可以得到移动操纵器在速度层面的综合运动学

$\dot{r}_{\mathrm{w}}=\left[\begin{array}{c} \dot{p}_{\mathrm{C}} \\ 0 \end{array}\right]+J(\theta, \phi)\left[\begin{array}{l} \dot{\phi} \\ \dot{\theta} \end{array}\right]$

其中雅可比矩阵为： $J(\theta, \phi)=\partial g(\theta ,\phi )/\partial w\in R^{m\times (n+1))}$ 其中 $w=[\phi ;\theta ]\in R^{n+1}$ （m=3,n=6）；

对上式进行简便处理可以得到：

$\dot{r}_{\mathrm{w}}=\left[\begin{array}{c} B\dot{\varphi } \\ 0 \end{array}\right]+J(\theta, \phi)\left[\begin{array}{l} A\dot{\varphi} \\ \dot{\theta} \end{array}\right]$

$\dot{r}_{\mathrm{w}}=\left[\begin{array}{cc} B & 0 \\ 0 & 0 \end{array}\right]\left[\begin{array}{l} \dot{\varphi} \\ \dot{\theta} \end{array}\right]+J(\theta, \phi)\left[\begin{array}{cc} A & 0 \\ 0 & I \end{array}\right]\left[\begin{array}{c} \dot{\varphi} \\ \dot{\theta} \end{array}\right]:=K \dot{q}$

其中K：

$K=\left[\begin{array}{ll} B & 0 \\ 0 & 0 \end{array}\right]+J(\theta, \phi)\left[\begin{array}{cc} A & 0 \\ 0 & I \end{array}\right] \in R^{m \times(n+2)}$ （该式存在问题，其中雅可比矩阵应该也对 $\dot{\varphi}$ 求导变成 $m \times(n+2)$ 的矩阵。

重复运动方案设计：

如前所述，对于移动机器人，我们必须考虑移动平台对逆运动学算法的重复性的影响。因此，前面提到的用于固定机械臂的重复运动方案可能对移动机械臂的重复运动控制并不有效。此外，还必须考虑移动机械臂的物理约束。基于以上几点，我们设计并提出了一种新型的移动机械臂重复运动方案。这种方案不仅可以解决关节角漂移问题，还可以使移动平台返回到初始位置。也就是说，在完成一个封闭路径的任务后，移动操纵器的最终状态与它的初始状态是重复的。

论文对于考虑到物理约束的移动机械手，重复运动方案被设计为

$\begin{array}{ll} \operatorname{minimize} & (D \dot{q}+z)^{\mathrm{T}}(D \dot{q}+z) / 2 \\ \text { subject to } & K \dot{q}=\dot{r}_{\mathrm{w}} \\ & q^{-} \leqslant q \leqslant q^{+} \\ & \dot{q}^{-} \leqslant \dot{q} \leqslant \dot{q}^{+} \end{array}$

其实把上述公示拆分之后发现一个很奇怪的事情，为什么用 $(D \dot{q}+z)^{\mathrm{T}}(D \dot{q}+z) / 2$ 作为我们二次型的最小化函数，拆解后 $D \dot{q}$ 为一个关于 $\dot{\phi },\dot{\theta},\dot{x},\dot{y},$ 的九维导数，而z为变量的加权变化值。

理论推导：

为了实现移动机器人的重复运动控制，让我们考虑移动机器人的三个因素，即操纵器的关节角θ，平台的旋转角φ，以及操纵器在移动平台上的位置pC = [xC, yC]T。可见，当且仅当这三个变量回到它们的初始位置时，当移动机器人的控制器执行一个封闭的轨迹时，移动控制器可以返回到原始状态。也就是说，移动控制器的重复运动等同于变量θ、φ和pC的重复运动。因此，按照Zhang等人的设计方法[32], [33]，θ、φ和pC的重复运动可以通过以下设计步骤实现。

第一步：建立一个误差函数 $E(t)$ (指数类型，保证可以趋近于0）
第二步：对时间的导数函数 $\dot{E(t)}$ 用下式表示，其中 $\lambda$ 为正数
$\dot{E(t)}=-\lambda E(t)$
对于上式的求解可以参考以下图片：
微分方程求解
第三步：我们可以用具体的误差函数来展开上述设计公式，从而得到所需的动力学方程。
这篇论文定义了三种误差函数：
$\begin{aligned} E(t) &:=\theta-\theta(0) \\ E(t) &:=\sin \phi-\sin \phi(0) \\ E(t) &:=p_{\mathrm{C}}-p_{\mathrm{C}}(0) \end{aligned}$

其中 $\theta(0),\phi (0),P_{C}(0)$ 描述的都是复合移动机器人在初始位置的 $\theta,\phi ,P_{C}$

对于第二种误差函数为什么这样设置，考虑一种情况：当移动平台做圆周运动时（即航向角φ做360度旋转），航向角可以返回到初始位置。那么航向角的值就是 $\phi =\phi (0)+2k\pi (K=\pm 1,\pm 2\cdots )$ 所以论文采用了第二种误差函数的表示方法，用 $E(t) :=\sin \phi-\sin \phi(0)$ 来代替，模型的建立会更加方便。

利用 $\dot{E(t)}+\lambda E(t)=0$ 可以得到：

$\begin{aligned} &\dot{\theta}+\lambda_1[\theta-\theta(0)]=0 \\ &\dot{\phi} \cos \phi+\lambda_2[\sin \phi-\sin \phi(0)]=0 \\ &\dot{p}_{\mathrm{C}}+\lambda_3\left[p_{\mathrm{C}}-p_{\mathrm{C}}(0)\right]=0 \end{aligned}$

其中 $\lambda _{1}>0,\lambda _{2}>0,\lambda_{3}>0$ 并且根据上述的微分方程的求解我们可以得到当 $t\rightarrow \infty$ 时，θ、sin φ和pC可以全局地、指数地收敛到其初始状态。

此外模型的速度级运动学模型只有 $\dot{\theta },\dot{\varphi }$ ,所以我们要做的是把刚刚得到的三个误差公式进行参数的转换。所以结合

$\\A\dot{\varphi }=\dot{\phi }\\B\dot{\varphi }=\dot{p}_C$

可以得到：

$\left[\begin{array}{c} B \\ A \cos \phi \end{array}\right] \dot{\varphi}+\left[\begin{array}{c} \lambda_3\left(p_{\mathrm{C}}-p_{\mathrm{C}}(0)\right) \\ \lambda_2(\sin \phi-\sin \phi(0)) \end{array}\right]=0$

当(22)被求解时，结果的解决方案可以实现移动机械臂的重复运动控制。但是，由于必须考虑末端执行器的运动轨迹要求，而且移动机械臂中始终存在物理约束，所以采用之前提到的QP问题进行求解而不是直接利用式（22）直接求解。

推导过程中可以发现，三个误差函数的微分方程组的求解保证了时间趋于无穷时，θ、sin φ和pC可以全局地、指数地收敛到其初始状态。也就是说，在完成一个封闭路径的任务后，移动操纵器的最终状态与它的初始状态是重复的。另一方面考虑：机械臂的重复运动主要是θ、sin φ和pC的函数确定，

这也就实解释了为什么用之前为什么突然使用该函数和约束作为复合移动机器人可以进行重复运动规划和控制的判定条件。

但是因为上述的目标函数是速度层面，但是约束还有 $q^{-} \leqslant q \leqslant q^{+}$ 约束，所以需要想办法把该约束转换成速度级别的约束，转换后如下图所示，

$\mu\left(q^{-}-q\right) \leqslant \dot{q} \leqslant \mu\left(q^{+}-q\right)$

记得有个转换的证明公式，但是一时想不起来，后期补上。

$\xi_i^{-}=\max \left\{\mu\left(q_i^{-}-q_i\right), \dot{q}_i^{-}\right\} \text {and } \xi_i^{+}=\min \left\{\mu\left(q_i^{+}-q_i\right), \dot{q}_i^{+}\right\}$

约束更改后，把上述目标函数和约束变成标准的二次型求解的问题。(Dx+z)T(Dx+z)/2 = xTDTDx/2 + zTDx + zTz/2。

$\begin{array}{ll} \operatorname{minimize} & \frac{1}{2} x^{\mathrm{T}} W x+h^{\mathrm{T}} x \\ \text { subject to } & K x=\dot{r}_{\mathrm{w}} \\ & \xi^{-} \leqslant x \leqslant \xi^{+} \end{array}$

上述的目标函数和约束条件中所有的系数都是时变的和随状态变化的，这就要求对上述时变QP问题有一个有效的求解器。

往下的求解是一种基于LVI和PLPE的方法，其中对于LVI之前查到了一篇关于LVI的对偶神经网络的QP问题的求解，但是本文提到的PLPE还是没有头绪。经过查询后发现作者除了上述的神经网络的介绍还提及到了一篇论文

基于LVI的数值方法（E47算法）求解二次规划问题

https://ieeexplore.ieee.org/document/6024697https://ieeexplore.ieee.org/document/6024697

该论文主要基于线性变分不等式（LVI）的数值方法（称为E47算法）来解决同时受到线性平等、不平等和约束的二次规划（QP）问题。需要注意的是，这种受约束的QP问题可以等同于线性变分不等式，然后等同于片断-线性投影方程（PLPE）。然后，E47算法被调整为解决结果的PLPE，从而得到QP问题的最佳数值解。此外，这种E47算法的全局线性收敛性被证明。进一步提供了这种E47算法和主动集算法的数值比较结果。证实了所提出的E47算法在解决QP方面的有效性和优越性。

$\begin{array}{ll} \operatorname{minimize} & x^{\mathrm{T}} W x / 2+q^{\mathrm{T}} x, \\ \text { subject to } & J x=d \\ & A x \leqslant b \\ & \xi^{-} \leqslant x \leqslant \xi^{+} \end{array}$