对应文章地址：Vision-encoder-based Payload State Estimation for Autonomous MAV
With a Suspended Payload
摘要
负载状态估计的鲁棒性和在线性很重要，但具有挑战性，特别是在户外环境中。本文开发了一种新型的实时有效载荷位置估计系统;该系统由单目鱼眼摄像机和一种基于编码器的新型装置组成。提出了一种基于高斯融合的负载状态估计算法。在负载位置估计鲁棒的基础上，提出了一种有效负载控制器，以保证可靠的快速轨迹跟踪性能。
引言
实现负载状态估计的方法有很多。
例如，Aggressive Flight With Suspended Payloads Using Vision-Based Control | IEEE Journals & Magazine | IEEE Xplore使用视觉方法来估计载荷的位置。该方法要求负载上有特定的人工标记(如白色圆形标签)，而视频流的低频和高延迟使其难以准确估计负载的状态。
Autonomous Swing-Angle Estimation for Stable Slung-Load Flight of Multi-Rotor UAVs 使用惯性测量单元(IMU)来估计负载的位置(这里有误，其实是通过负载的受力估计来估计负载位置);但IMU作为积分型传感器，在长时间工作后可能会产生累积误差。
本文贡献分为三部分：
首先，针对无外界观测的实际场景（无法使用动作捕捉），设计了一种结合编码器和鱼眼摄像机的负载估计系统。通过对编码器和视觉信息的融合，进一步提出了一种基于高斯融合的估计算法，以获得精确和鲁棒的性能。
其次，提出了一种负载控制器，该控制器能够保证可靠的快速轨迹的跟踪性能。提高视觉-编码器估计系统检测缆绳是否绷紧，并自动切换控制策略。
最后，通过实验验证了整个估计系统对复杂飞行环境和极端负载变化的鲁棒性。
估计器设计和建模
A 系统中的坐标系定义以及变量定义
B 编码器设计和角度定义
为了使载荷可控，载荷与微型飞行器之间的缆绳应保持拉紧。如果缆绳没有拉紧，MAV-载荷系统退化为普通MAV系统。因此，负载的工作空间一般为两个自由度可控的球壳。
通过考虑估计器坐标系中的笛卡尔坐标，将负载向量投影到xOz, yOz平面上。分别定义角$θ_1$和$θ_2$，如上图所示。负载在估计器笛卡尔坐标系中的位置为
$$\left\{\begin{array}{l}x^{2}+y^{2}+z^{2}=l^{2} \\ \frac{x}{z}=\tan \theta_{1} \\ \frac{y}{z}=\tan \theta_{2}\end{array}\right. \tag{1}$$其中当绳子拉紧时，l为常量。
为了测量$θ_1$和$θ_2$以上值，设计了一种新型的测量系统。设计的系统机构如图1所示。选用AS5047P磁旋转位置传感器作为角度传感器。AS5047P是一种高分辨率旋转位置传感器，可在360度范围内进行高速(高达28krpm)角度测量，几乎没有延迟。AS5047P具有强大的设计，可以抑制任何均匀外部杂散磁场的影响。标准的4线SPI串行接口允许主机微控制器从AS5047P读取14位绝对角度位置数据，无需专用编程器即可进行非易失性设置。如图2所示，设计了一种两轴平移机构来测量图3中提到的两个角度。为了减少轴之间的摩擦，高速轴承被采用并嵌入3d打印的支持。支架可进一步采用金属材料替代，可在承载要求较高的现场使用。传感器模型的详细介绍如下:
C 相机模型
装备一个向下的鱼眼摄像机来估计负载的状态，如图1所示。我们假定缆绳的末端与基于编码器的估计器的原点重合。编码器坐标系的原点距离摄像机光学中心的距离定义为$p^C_\mathcal{E}$。根据针孔相机模型，将相机到负载的单位矢量定义为
$$\mathbf{q}^{\mathcal{C}}=\frac{\left(\frac{\Delta u}{f_{x}}, \frac{\Delta v}{f_{y}}, 1\right)^{T}}{\left\|\left(\frac{\Delta u}{f_{x}}, \frac{\Delta v}{f_{y}}, 1\right)\right\|},$$其中 $\Delta u$ 和 $\Delta v$ 有效载荷到图像中心的像素坐标; $f_{x}$ 和$f_{y}$ 通过相机标定获得相机模型的内在参数A Generic Camera Model and Calibration Method for Conventional, Wide-Angle, and Fish-Eye Lenses (researchgate.net)。 在世界坐标系下的负载位置由如下公式得到
$$\begin{gathered} \theta=\arccos \left(\mathbf{q}^{\mathcal{C}} \cdot \mathbf{p}_{\mathcal{E}}^{\mathcal{C}}\right), \\ \bar{l}=\left\|\mathbf{p}_{\mathcal{E}}^{\mathcal{C}}\right\| \cos \theta+\sqrt{\left\|\mathbf{p}_{\mathcal{E}}^{\mathcal{c}}\right\|^{2} \cos ^{2} \theta-\left\|\mathbf{p}_{\mathcal{E}}^{\mathcal{C}}\right\|^{2}+l^{2},} \\ \mathbf{x}_{L}^{\mathcal{E}}=\mathbf{R}_{\mathcal{C}}^{\mathcal{E}} \overline{l}{\mathbf{q}}^{\mathcal{C}}+\mathbf{p}_{\mathcal{C}}^{\mathcal{E}}, \\ \mathbf{x}_{L}^{\mathcal{W}}=\mathbf{x}_{Q}^{\mathcal{W}}+\mathbf{R}_{\mathcal{B}}^{\mathcal{W}} \mathbf{R}_{\mathcal{E}}^{\mathcal{B}} \mathbf{x}_{L}^{\mathcal{E}}, \mathbf{q}^{\mathcal{W}}=\mathbf{x}_{L}^{\mathcal{W}} /\left\|\mathbf{x}_{L}^{\mathcal{W}}\right\|, \end{gathered}$$其中$\theta$为 $\mathbf{q}^{\mathcal{C}}$与 $\mathbf{p}_{\mathcal{E}}^{\mathcal{C}} $之间的夹角， $ \bar{l}$ 是相机到有效载荷的估计距离。
在下面几节中我们省略了世界坐标系$\mathcal{W}$的上标，即q表示$q^\mathcal{W}$, x表示$x^\mathcal{W}$ ,R表示$R^\mathcal{W}_B$，从而简化表示。
D 无人机-负载系统动力学
负载位置和拉紧状态估计
本节介绍了基于视觉编码器的载荷估计器的算法框架，如下图所示。编码器和鱼眼摄像机的估计结果与高斯滤波器相融合，产生高频、实时且鲁棒的估计负载的位置以及缆绳的拉紧状态。
A 基于编码器的估计器
对于每个磁力编码器，角度数据通过模数转换获得，总量程$n_{total} = 2^{14}$。假设整个编码器系统的测量受到白噪声的干扰，则测量角度可以表示为
$\hat{\theta}=2 \pi \frac{n_{a d c}}{n_{\text {total }}}+w_{n},$其中$n_{\text {adc }} \in\left[0, n_{\text {total }}\right]$为测量的原始角度, 而 $w_{n} \sim \mathcal{N}\left(0, \sigma_{e}^{2}\right)$。
根据编码器估计器系统的机械设计，测量值可以表示为
$$\begin{aligned} \mathbf{x}_{L}^{\mathcal{E}} &=h(\hat{\theta}) \\ &=h(\theta)+\eta, \end{aligned}$$其中函数 $h(\cdot)$ 是公式(1)所示测量函数，$\eta \in \mathbb{R}^{3}$ 是变换后的噪声方差。噪声由零均值高斯噪声描述，其协方差矩阵为 $R$。对于测量值 $\theta$， 条件概率密度为:
$$\begin{aligned} p\left(\mathbf{x}_{L}^{\mathcal{E}} \mid \theta\right) &=\mathcal{N}\left(\mathbf{x}_{L}^{\mathcal{\varepsilon}} ; h(\theta), R\right) \\ &=\frac{1}{\sqrt{|2 \pi R|}} \exp \left(-\frac{1}{2}\left\|h(\theta)-\mathbf{x}_{L}^{\mathcal{\varepsilon}}\right\|_{R}^{2}\right) . \end{aligned}$$B 基于视觉的估计器
值得注意的是，如果缆绳处于松弛状态，基于编码器的估计器不能提供有效载荷位置的鲁棒估计;这将导致MAV飞行控制失败。为了解决这个问题，我们进一步利用了单目鱼眼相机;摄像机不仅用于检测缆绳的绷紧状态，还提供负载的位置信息。
为了实现鲁棒检测，采用STAPLE（Staple: Complementary Learners for Real-Time Tracking | IEEE Conference Publication | IEEE Xplore）跟踪负载，利用相关性过滤器(使用HOG特征)和全局颜色直方图的线性组合，构造如下的评分函数：
$$f(u, v)=\gamma_{t p} f_{t p}(u, v)+\gamma_{h t} f_{h t}(u, v),$$其中 $f_{t p}(u, v)$ 为由相关过滤器计算的模板分数; $f_{h t}(u, v)$ 是直方图分数; $\gamma_{t p}$ 和 $\gamma_{h t}$ 为评分函数中的权重。 STAPLE对运动模糊、光照变化、物体变形和复杂背景等具有挑战性的情况具有很强的鲁棒性。有了这些特征，我们的视觉系统在室内和室外实验中显示出了很强的鲁棒性。
此外，得益于基于编码器的估计器，我们赋予STAPLE检索丢失的跟踪对象的能力。在原始STAPLE中存有一个低概率事件，在重新跟踪对象时，对象是在视野之外。在我们的系统中，如果目标跟踪在基于视觉的估计器中失败，它将由基于编码器的估计器估计的位置重新初始化。
为了判断电缆的绷紧状态，计算基于编码器的估计器和基于视觉的估计器之间的估计位置差 $\Delta \mathbf{x}_{L}=\left\|\mathbf{x}_{L_{\text {vision }}}-\mathbf{x}_{L_{\text {encoder }}}\right\|_{2}$ 。
定义负载外接圆半径为r，当上述估计位置差小于r时，缆绳为拉紧，反之为松弛。
C 基于视觉和编码器的传感器融合
如A、B节所述，编码器和基于视觉的估计器都提供了负载的位置估计。由于这两种估计都受到噪声干扰，因此有必要将两种传感器的测量结果进行融合以提供精确的结果。不失一般性，假设整个视觉-编码器系统得到的位置估计量$\mathbf{x}_{L_{e s t}}^{B}$服从高斯分布：
$$P\left(\mathbf{x}_{L_{e s t}}^{B}\right)=\mathcal{N}\left(\mu, \sigma^{2}\right) .\tag{2}$$视觉和编码器的观测结果也服从高斯分布
$$P\left(\mathbf{x}_{L_{o b x}}^{\mathcal{B}}\right)=\mathcal{N}\left(\mu_{o b s}, \sigma_{o b s}^{2}\right) .$$根据高斯分布的乘积，(2)中的参数计算为
$$\begin{gathered} \mu_{\text {est }}=\frac{\sigma_{\text {vision }}^{2} \mu_{\text {encoder }}+\sigma_{\text {vision }}^{2} \mu_{\text {encoder }}}{\sigma_{\text {vision }}^{2}+\sigma_{\text {encoder }}^{2}}, \\ \sigma_{\text {est }}^{2}=\frac{\sigma_{\text {encoder }}^{2} \sigma_{\text {vision }}^{2}}{\sigma_{\text {vision }}^{2}+\sigma_{\text {encoder }}^{2}} . \end{gathered}$$利用公式(1)，实时计算得到融合的负载位置估计。
传感器融合实际上就是直接对两个传感器测得的同一个对象的测量值取平均数，从而降低测量值中的噪声，提高精确度。了解传感器融合和跟踪 （全6P）MATLAB&Simulink
对于上标B表示机体坐标系，但前文编码器中的负载位置在估计器坐标系中表示，不对应有问题。
轨迹生成与控制
A 轨迹生成
这部分内容与Aggressive Flight With Suspended Payloads Using Vision-Based Control | IEEE Journals & Magazine | IEEE Xplore类似，本文负载位置最高阶导数为4阶，而Aggressive Flight With Suspended Payloads Using Vision-Based Control | IEEE Journals & Magazine | IEEE Xplore中变量对应的负载位置最高阶导数为6阶。
B 控制设计和控制器切换策略
与Geometric control and differential flatness of a quadrotor UAV with a cable-suspended load | IEEE Conference Publication | IEEE Xplore的控制器一样，分为绳子有张力和无张力处理。有张力时为一个负载-无人机系统。无张力时，系统退化为无人机和负载两个系统。
实验
无人机重量为1.96kg，负载重量为0.168kg。
