《自主移动机器人 》课件 第2章 预备知识

第二讲预备知识熊蓉浙江大学控制科学与工程学院移动机器人框架移动机器人的运动学、规划、定位和地图构建等问题都围绕移动机器人的状态展开状态是对移动机器人自身信息的抽象描述，运动是对状态的改变，而观测是对状态的测量状态运动观测随机性机器人在实际世界中运行，机器人感知实际世界通过传感器传感器必然存在随机噪声，所以需要用概率理论描述状态和传感比如机器人当前的温度需要通过温度传感器测量，和障碍物的距离需要通过测距传感器测量随机变量，数字化随机试验

随机变量，数字化随机试验

离散随机变量概率

连续随机变量概率

连续随机变量概率

连续随机变量概率

期望和方差描述一个概率密度函数的性质，可以用期望和方差，前者描述平均水平，后者描述与均值的偏差水平期望定义为方差定义为

高斯分布

高斯分布对于二维高斯随机变量，可以通过椭圆进行描述，其中椭圆的中心为期望，椭圆的长短轴表示二维向量两个元素各自的方差，椭圆的旋转程度则反映两个元素的相关性标准高斯分布方差主对角线为1，非对角线0.3多变量概率分布很多情况下，不止关心一个随机变量，还关心多个随机变量之间的关系。这时候就需要多变量概率分布，如变量X和Y联合分布函数联合密度函数

边际分布和条件分布

条件分布案例当有一个激光传感器，测量50cm外的物体时，测量误差的方差较大；测量50cm以内的物体时，测量误差的方差较小建模随机变量Y=1表示50cm内，Y=2为外，误差建模为X，可得三维世界通常机器人在三维空间中运动，对于平面移动的机器人，可以采用二维空间来描述。但对于飞行、水下等机器人，一般采用三维。通用期间，介绍三维世界下的机器人运动描述对于一个三维空间中的机器人，可以用位置和姿态进行建模位移、旋转和位姿

位移、旋转和位姿

位移、旋转和位姿

位移、旋转和位姿

位移、旋转和位姿如果B相对W，只有一个轴的旋转，可以导出三个旋转矩阵

位移、旋转和位姿三轴的旋转可以通过对单轴的旋转逐次得到，因此存在将这个分解一般化，机器人也可以通过任意多次三轴旋转得到新的三轴旋转

位移、旋转和位姿更进一步，可以用多次位移和旋转的连乘，获得连续运动中刚体上一点的位置

位移、旋转和位姿

静态世界中的点除了刚体上的点，对于静态世界中的点，也需要表示到移动机器人自身的坐标系下这个操作因为位姿矩阵非奇异，存在逆，容易定义也可以推广到连乘

位姿其他问题旋转矩阵是正交阵，位姿矩阵是非奇异阵位姿矩阵的求逆，对应到旋转矩阵的部分是转置位姿矩阵和旋转矩阵的连乘，都满足矩阵的乘法，因此满足矩阵乘法的结合律但不满足交换律，交换旋转的顺序，得不到相同的变换结果速度与角速度速度与角速度要考虑相对关系，也有考虑表示的坐标系。但不同于位置，其坐标系变换仅和旋转有关速度角速度通过位姿和旋转微分导出，速度很直观，旋转矩阵的导数定义如下可以理解为B坐标系每个坐标轴因旋转形成的线速度

刚体运动针对刚体上一点，可以用位姿联系其在不同坐标系的表示对上式做微分，得到刚体上一点速度和角速度的关系

刚体运动引入反对称矩阵表示叉乘刚体上点的运动可以进一步写为

刚体运动进一步微分，可以得到加速度的关系该式给出了机器人上一点的加速度，和机器人自身的角速度，角加速度，以及线加速度均有关系位置，速度和加速度之间的关系，构建机器人运动的状态系统

状态空间系统方程

运动测量旋转的线性化由于旋转的存在，机器人的系统方程通常是非线性的。为了分析简单，常见的思路是将方程线性化。在介绍旋转的线性化前，首先介绍罗德里格斯公式

罗德里格斯公式旋转的线性化

旋转的线性化

旋转的概率表达

机器人运动小结机器人的运动可以利用刚体上点的运动进行描述机器人的状态通常被建模成位姿和速度，由于旋转的存在，机器人的运动和观测都是非线性模型通过引入旋转的线性化，可以将非线性模型线性化同时引入旋转的概率密度函数，使旋转的随机性可以用概率理论描述至此，我们有了完整的机器人状态系统建模，有了对状态的线性化方法，以及用概率描述因传感噪声导致的系统随机性随机系统相比于前述的状态空间系统，随机系统利用噪声将随机性引入到方程中这种建模方式也可以将运动和观测方程中没有建模的部分也一并考虑进来，并且处理较为简单

随机系统将系统在时间上离散化引入条件概率密度对离散后的随机系统进行描述，形成对机器人包含随机性的状态空间建模

马尔科夫过程建模

估计

最大后验估计最大后验估计，顾名思义就是找使后验概率分布最大的状态对其取最大，并对数变换可得

最大后验估计对于大多数非线性系统和概率分布，上式无法在可行时间内求解。为了导出可行的求解形式，采用高斯分布建模加性噪声其中

最大后验估计回代到MAP的公式，可以得到具体形式其中c表示高斯分布中的常数项取对数，当做MAP时，常数项不会对结果有影响，因此可以忽略

最大后验估计导出MAP的最终形式等价于求解一个非线性方程方程组，通常称为最小二乘，简化起见写成矩阵形式

求解非线性最小二乘

转化MAP为对线性化点和真值点间偏差量的线性最小二乘求解非线性最小二乘

矩阵简写求解线性最小二乘得到偏差量在线性化点上增加偏差量，获得对状态的估计问题求解完毕？求解非线性最小二乘

线性化点如何获得？随机，全0，离真值点较远时结果很差其他取得初值的手段，比如先解控制量部分的方程线性化点一次迭代后是否真的是真值？不是，因为非线性方程线性化只是局部的近似可以用更新后的点，作为新的初始化点，重新进行上述步骤，迭代求解非线性最小二乘该方法称为高斯-牛顿最小二乘求解法通过不断的用二次函数拟合线性化点附近的误差曲面，逐渐逼近解但只能保证局部最优性，因此和第一个线性化点的高度相关求解非线性最小二乘MAP小结基于随机系统理论建模机器人的整个序列将状态测量问题转化为随机变量的估计问题通过加性高斯噪声假设，将MAP具体化为非线性最小二乘通过线性化迭代，求解非线性最小二乘问题滤波为什么要滤波？因为MAP估计机器人在整个运行序列中的状态，这就意味着随着机器人运行序列变长，可能求解越来越慢，因此适合离线在线运行时，机器人更关心当前状态。因此，是否存在一种以过去状态不估计，只求解当前状态，但可以在线的方法？滤波是一个答案滤波导出关于当前状态的MAP与全状态的MAP相比，滤波操作更复杂，但形成了递推关系，适合在线处理

滤波由于递推，所以不能像MAP那样，只关注状态的估计值，还需要导出方差，从而满足下一个时间的计算同样采用和MAP时一样的假设，即加性高斯噪声。首先介绍高斯分布的性质，假设X和Y为高斯随机变量，滤波联合高斯分布存在条件分布存在边际分布

联合高斯分布后验分布边际分布滤波：第一个观测

滤波：第一个观测引入线性化高斯分布的线性变换仍满足高斯分布

滤波：第一个观测构造出联合高斯分布利用联合高斯的条件分布公式，导出后验

滤波：第一个观测

滤波：第一个运动

滤波：第一个运动

滤波：下一个观测

卡尔曼滤波通过高斯假设，可以发现滤波的每一步信息都可以导出新的高斯分布，从而使整个过程可解析计算一般化的卡尔曼增益一般化的后验这组公式被称为卡尔曼滤波

滤波小结相比于MAP，滤波采用单步后验形成递推形式，从而使计算从离线变成在线，相应的也丢弃了过去的状态通过假设高斯分布，以及线性化后，系统的线性传递满足高斯分布，因此此时的滤波转化为高斯分布的参数递推求解问题，使得滤波过程可快速求解卡尔曼滤波在非线性形式下，由于线性化误差的引入，缺少理论的严格保证，通常比MAP的精度要低本章小结概率理论能够提供建立实际世界中随机性的理论工具三维世界的运动理论提供了