清远桥梁检测车出租， 云浮桥梁检测车出租， 顺德桥梁检测车出租    基于数据融合的点云桥梁检测车姿态矫正方法？     三维激光雷达固连在桥梁检测车上，当桥梁检测车发生姿态变化时，三维激光雷达也会随之产生姿态变化，采集到的点云无法反映真实路面信息。通常使用惯性测量单元测量车身的姿态变化，但是惯性测量单元测量角度的原理是将角速度进行积分求得，会产生累积误差，而因三维激光雷达的姿态变化产生的点云误差非常大，三维激光雷达的俯仰角产生1°的变化，将引起点云三维坐标的巨大变化。为得到三维激光雷达和车身准确的位姿变化，使用扩展卡尔曼滤波融合相机计算的位姿变化，对由车身变化引起的点云误差进行消除。

      

       1 基于视觉的位姿变化计算： 视觉产生的图像的本质是由像素值组成的像素矩阵，将图像中的特殊点进行提取，称之为特征点，在相机连续采集图像的过程中，在相邻的几帧图像中总会存在相同的特征点，将这些特征点进行匹配后计算其位姿变化，即可计算出相机的位姿变化，而相机与车身，三维激光雷达固连，可求得三维激光雷达的位姿变化。提取特征点的经典算法有尺度不变特征变换，此算法充分考虑图像采集中出现的光照变化，尺度变化，旋转变化等，具有较强的鲁棒性，识别精度很高，但是计算量极大，而位姿变化要求实时性，故不能采用这类算法。为适当提高计算速度，可适当降低精度和鲁棒性。ORB（Oriented FAST and Rotated BRIEF）算法可快速提取图像特征，根据算法作者的测试，在同一图像中提取相同数量的特征点，ORB 需要15.3ms。而其他算法需消耗上百毫秒，特别的，尺度不变特征变化算法需要消耗5228.7ms，故本系统采用ORB 算法进行特征提取。ORB 算法提取的特征点被称为ORB 特征点，其由关键点（Key Point）和描述子（Descriptor）构成，关键点表示该特征点在图像里的位置，ORB 关键点还包括了尺度和旋转的描述；描述子用向量表示，描述了对应关键点周围的像素信息。ORB 关键点的提取过程如下：（1）设图像中的像素p 的亮度为pI ；（2）设置亮度阈值；（3）以像素p 为中心，选取半径为3 的圆上的16 个像素点；（4）如果选取的圆上的像素点的亮度值有连续n（n 通常取12，也可取9或11）个点大于t Ip或小于t Ip，则像素点p 是关键点；（5）为像素点p 添加尺度和旋转，即让像素点p 在尺度和方向改变时依然为关键点。尺度不变性用构建图像金字塔法检测，即对图像进行不同层次的降采样，以获得不同分辨率的图像，并在这些图像上检测关键点；旋转使用亮度质心法（Intensity Centroid）实现，质心指的是图像块以亮度值作为权重的中心，假设关键点偏离亮度的质心，则其向量可用于表示方向。ORB 算法使用改进的BRIEF 描述子，BRIEF 是一种二进制的描述子，用于比较关键点附近的两个像素点像素的大小，大的记为1，小的记为0,。在进行特征提取时，在关键点附近随机选取N 个像素进行比较，组成N 维向量空间。ORB算法的描述子使用了关键点的尺度与旋转信息。进行匹配时，使用快速近似最近邻FLANN算法对各个特征点的描述子进行比较，当其向量空间距离相近时，认为两个特征点是同一特征点。当所有的特征点匹配完后，进行相机运动求解。由于立体相机的图像具有深度信息，故可使用P3P（Perspective-Three-Point）算法估计相机的位姿。三个特征点A,B,C 在相机成像平面投影分别为a,b,c，S 为相机光心，由深度计算的A,B,C 的坐标为其在世界坐标系下的坐标，通过计算得到他们在相机坐标系下的坐标，就可以解算其转换矩阵。随着相机的运动而变化，利用消元法求得三个特征点在相机坐标系下坐标，并用第四个点进行验证，即可使用ICP 算法求解相机的位姿变化。

        清远桥梁检测车出租， 云浮桥梁检测车出租，  顺德桥梁检测车出租 www.shundeludengchechuzu.com/

        2   基于卡尔曼滤波器的数据融合：卡尔曼滤波器用于在含有任何不确定信息中提取有用信息，进而进行状态估计的算法。卡尔曼滤波器分为两部分，一部分为自身系统的预测，另一部分为根据传感器的观测信息对自身系统预测的更新。卡尔曼滤波器对所有信息的不确定性都进行了考虑，并假设其均为线性高斯分布。对于状态x，可认为是任何形式的状态，包括位置，速度，甚至是温度，体积，而对于当前时刻的状态tx的估计量txˆ，在对自身系统的预测中可由上一时刻的状态量1进行估计。 当前时刻系统内的各个状态量的相关性可由协方差矩阵tP 表示，并考虑到系统外部扰动，假设扰动为高斯分布，用扰动协方差tQ 表示，同样的，当前时刻的协方差矩阵由上一时刻的协方差矩阵决定。 其中，状态转移矩阵，由系统的属性决定；对于一些有控制影响的系统，考虑到由控制对系统产生的影响，引入控制向量，其中控制矩阵，由系统的控制系统决定。由式3-7 与式3-8 组成卡尔曼滤波器的预测部分。当使用传感器数据对系统状态进行估计时，由于传感器读取的数据的单位和尺度有可能与要跟踪的状态的单位和尺度不一样，传感器的模型用矩阵tH 表示，传感器带有的误差用协方差矩阵tR 表示，传感器的测量数值用向量kz表示，根据系统当前的状态，传感器对系统状态的估计应服从高斯分布，而传感器本身的测量结果服从高斯分布。为了得到上述两种事件的概率分布，即传感器对系统预测正确和传感器本身测量准确，两种事件均发生时的概率分布，将其概率密度相乘，其结果也服从高斯分布：T称为卡尔曼滤波器对系统预测的更新。当使用卡尔曼滤波器进行传感器信息融合时，设有l 个传感器参与观测，则其对系统的状态估计可表示至此，可将多个传感器的信息进行融合作为状态估计的观测值来更新系统对自身的预测。

         3 姿态矫正:   对于本系统，需要进行估计的状态量有姿态角和里程计，姿态角用于矫正由桥梁检测车行驶时的姿态变化而引起的点云误差，里程计用于下一节中点云的递归叠加。在ROS 的消息机制中，里程计消息既包含了姿态角又包含了位置信息。 其中姿态角用四元数进行表示，四元数为紧凑的不带奇异性的姿态角表达方式。四元数是一种扩展的复数，拥有一个实部三个虚部：表示三维坐标值。设置状态相关性协方差tP 和系统状态扰动协方差tQ 为单位阵，根据惯性测量单元和立体相机的测量精度设置传感器测量误差协方差矩阵tR 。将惯性测量单元的姿态角原始数据作为状态估计观测值的姿态角，将线加速度进行两次积分计算出当前位置的三维坐标；将立体相机计算出的姿态角和三维坐标根据式3-11 至3-13 与惯性测量单元的状态估计观测值进行融合，得到两个传感器的相关性协方差和各自的权重，将融合后的传感器对系统状态的估计带入式3-10，结合式3-7 进行迭代，得到当前时刻的状态估计值。将当前时刻的四元数转化为转换矩阵，首先将四元数转化为旋转向量，再根据罗德里格公式转换为矩阵，其转换关系为：设当前点云集合S 经过姿态矫正后完成了三维激光雷达的数据姿态矫正。

           清远桥梁检测车出租， 云浮桥梁检测车出租，  顺德桥梁检测车出租