机械臂目标抓捕学习笔记【1】

基于概率量度的机械臂视觉伺服目标捕获运动规划方法

参考文献：《[1]祁若龙,邵健铎,张珂等.基于概率量度的机械臂视觉伺服目标捕获运动规划方法[J].吉林大学学报(工学版),2020,50(06):2044-2054.DOI:10.13229/j.cnki.jdxbgxb20190720.》

期望轨迹序列

机器人轨迹 Π序列表示为系统预定义状态、控制输入：

其中$l$为整条预定义轨迹的周期数，

状态转移函数

下个周期的系统状态是当前系统状态、指令输入和运动误差的函数：

系统测量值

当机器人运动系统存在过程噪声，希望通过外部测量或机器人自身携带的传感器形成运动反馈时，测量值只与两个因素相关：一个是机器人系统状态；另一个是测量系统本身的观测噪声。写成函数的形式为：

模型线性化

针对离散化周期系统，对模型型（2、3）利用一阶 Taylor展开。

在式（2）的预定义轨迹点（$x_{t-1}^{\ast }$,$u_{t-1}^{\ast }$）处进行线性化，$m_t$在0处展开，则有：

在式（3）的预定义轨迹点（$x_t^{\ast }$）处进行线性化，$n_t$在0处展开，则有：

对上式整理可以得到：

卡尔曼滤波状态估计

代价函数-闭环反馈

闭环反馈的目的是使实际轨迹与预定义轨迹之间的偏移量最小：

线性二次型（LQR）最优解具备求解过程的规范化，可简单地采用状态线性反馈控制律构成闭环最优控制系统，兼顾系统状态和输入指标。求解过程可以表示为：

轨迹误差方差

结合式(8)和(17)，可得：
$$\begin{gathered} u_t=L_t{\tilde{x}}_t+u_t^{\ast } \\ {x}_t={\overline{x}}_t+x_t^{\ast } \end{gathered}$$
进一步可整理为：


通过式（21）~（23）对整个轨迹过程的各中断周期内的系统状态进行估计。

将式(9、17)代入式(12)，得到：

将式(6、7、9)代入式(24)，转化为：

将式（17）代入式（6）得：


系统状态的初始方差 $R_0$ 可以设置为一个较大的初始值，这个初始值会随着系统状态估计迅速收敛到正常的估计值范围。

误差几何化表示

为了得到机械臂关节、末端的误差分布，将关节空间的协方差向笛卡尔空间映射，得到机械臂在笛卡尔空间下的误差特性。



其中$V_k^i$是特征值，$D_k^i$是特征矢量。

到达概率计算

在定义机器人运动的目标位置时会给出到达目的点的允许误差，例如：x_{goal} ± h。其中，x_{goal} 为目标位置，h 为允许的误差范围。

在三维空间中，机器人运动的目标区域范围可以看作是立方体，需要计算立方体的 6 个面与空间方差椭球之间的相关关系。

首先，只考虑概率分布$x_{t|t-1}$与第一个立方体平面约束 S_1 方程 $a_1^{T}x<b_1$范围内的概率分布和方差椭球缩放情况。

平面方程写成向量形式为 $a_1^{T}x=b_1$，其中 $a_1$ 为首个约束平面的法向单位矢量，$b_1$ 为平面方程的常量参数。

为了能将三维概率椭球投影到一维空间进行正态分布概率计算，逐个采用约束平面，对分布为$x_{t|t-1}\sim N({\hat{x}}_{k|k-1},P_{k|k-1})$的概率椭圆（或椭球）进行剖分，并向一维空间投影得到正态分布，从而在一维空间下，进行满足限定约束条件的正态分布的概率计算，其计算方法示意图如图 3 所示。