基于激光雷达的四旋翼无人机自主定位及控制方法

摘要

本发明属于四旋翼无人机自主飞行控制研究的技术领域，为提出一套新的基于二维激光雷达的自主定位方法，及基于此定位方法及其它机载MEMS芯片的四旋翼无人机控制系统设计方法，为此，本发明采用的技术方案是，基于激光雷达的四旋翼无人机自主定位及控制方法，首先利用二维激光雷达进行无人机水平方向的初步定位，利用机载气压计得到无人机的高度方向的初步位置值；之后利用互补滤波算法，结合机载加速度计芯片，获得更高频率的无人机位置信息；最后基于此位置信息，应用于无GPS信号下的无人机控制系统。本发明主要应用于无人机自主飞行控制装置的设计制造。

主权项

一种基于激光雷达的四旋翼无人机自主定位及控制方法，其特征是，首先利用二维激光雷达进行无人机水平方向的初步定位，利用机载气压计得到无人机的高度方向的初步位置值；之后利用互补滤波算法，结合机载加速度计芯片，获得更高频率的无人机位置信息；最后基于此位置信息，应用于无GPS信号下的无人机控制系统；基于激光雷达的水平方向初步定位具体步骤为：1)三维环境模型向二维平面投影利用机器人运动学坐标变换方法，并结合机载惯性导航单元得到的欧拉角，将激光雷达扫描得到的参考图像及当前图像投影到水平面上，在这一平面上，再利用二维平面匹配方法进行位移及角度测量，从而得到无人机在水平方向上精确的位置信息，具体步骤如下，设激光雷达在时刻t，按逆时针顺序第i条激光束在三维空间中扫描测到点P_i^l，其在激光雷达坐标系下定义为P_i^l＝[cos(θ_i)r_i,sin(θ_i)r_i,0]^T,其中θ_i代表该条激光束所在角度，r_i代表该条激光束测得的距离，与此同时根据机载惯性导航单元，测得激光雷达所处滚转角为γ、俯仰角为β，则通过(1)式，将P_i^l投影至世界坐标系中：P_i3d＝R_pitch×R_roll×P_i^l              (1)其中最后将P_i3d的Z轴分量置为0，从而得到采集图像中一个三维点P_i^l在二维平面上的投影点P_i2d，这里由所有投影点组成的图像即为三维环境模型在二维平面上的投影图像；2)迭代式无人机平面位移及转角初步估计方法利用1)中投影方法将无人机在不同时刻，不同角度下采集到的图像投影到同一平面上；而后利用迭代式方法对无人机平面位移及转角进行估计，因此需要选取两幅处于同一平面的图像进行比较，将其中一幅图像选定为当前时刻采集到的投影图像，另一幅参考图像根据换图策略进行选取；依据换图策略获得两幅比较图像后，需要利用迭代估计方法对无人机平面位移及转角进行估计；这里将所估计的参考图像到当前图像的变换关系定义为q，其包含由平面偏航角θ决定的旋转变换R(θ)及平移变换t，定义p_i为激光雷达采集到的当前投影图像中一个点，定义为利用所估计的位移及转角将点p_i投影到参考图像中的点，定义及为参考图像中距离最近的相邻点；设当前迭代次数为k且k>0，迭代直至收敛或陷入迭代环，其每一迭代步描述如下：第一步，设置迭代初值：若第一次进入迭代程序，则其变换猜测初值q₀定义为q₀＝(t₀,θ₀)，利用机载惯性导航元件，通过测得参考时刻及当前时刻间偏航角的差值作为θ₀，并设初始时间t₀为0，之后每次迭代中，迭代初值q_k＝(t_k,θ_k)设定为上一步计算出的结果，θ_k、t_k分别为第k次迭代时的偏航角的差值及时刻；第二步，确定匹配关系：将选取的迭代初值q_k应用于当前图像点p_i上，得到该点在参考图像上的坐标对于每一个在参考图像所有点中寻找距离最近的两个相邻点以及其中下角标j₁、j₂代表点以及点分别由激光雷达沿逆时针方向的第j₁及j₂条激光束采集得到，而上脚标i代表点及点与当前图像中的点p_i相匹配，继而可将该匹配关系简化为<i，j₁ⁱ，j₂ⁱ>，定义该匹配关系为<i，j₁ⁱ，j₂ⁱ>,即当前图像中点i匹配于参考图像中线段此方法需要首先确定参考图像中满足(2)式的点${j_1}^i=\mathrm{argmin}\left|\right|p_j-p_i^w|{|}^2---\left(2\right)$这里首先需要根据激光束分辨率设置初始搜索点，如式3所示；(∠P_i^w‑ψ₀)×[nrays/(θmax‑θmin)]                  (3)其中ψ₀为初始扫描激光束角度，nrays为激光束总数，θmax‑θmin为起始及终止激光束角度差值，之后比较起始点前后两点与的距离差，取距离较小的点所在方向开始搜索；如果当前点与的距离已经超过了之前获得的最短距离，则终止此方向的搜索并转向起始点另一方向继续搜索；此后，当两个方向的搜索均达到终止条件时，得到最短匹配最后选取前后两点中，与距离更近的那个点作为这样便可得到有效匹配对<i，j₁ⁱ，j₂ⁱ>及其匹配距离；第三步，裁剪部分匹配对：首先裁剪掉重复匹配参考图中一点的部分匹配对，建立一个数组存储所有与有关的匹配对，通过遍历这些匹配对应的匹配距离，取出其中最小的距离值，之后裁剪掉大于该距离一定倍数的匹配对<i，j₁ⁱ，j₂ⁱ>，其后将剩下的所有匹配对按匹配距离长短排序，再按预设比例裁剪掉其中距离较长的匹配对；第四步，定义式(4)为误差函数J，$J(q_{k+1},C_k)=\underset{i}{\Sigma }\left({n_i}^T\right[R\left({\theta }_{k+1}\right)P_i+t_{k+1}-{P_{j_1}}^i\left]\right)---\left(4\right)$其中n_i为垂直于线段的法向量，C_k为当前第k步所有有效匹配关系，该式可描述为求取变换关系_qk+1＝(t_k+1,θ_k+1)，使得未被裁剪的所有匹配对中，点到与其匹配的线段的距离总和最短，对(4)式这一非线性方程求取极值，计算出无人机水平方向位置及转角的变化估计q_k+1＝(t_k+1,θ_k+1)，若未满足迭代终止条件，则将此值设置为迭代初值，至第一步继续迭代运算，直至满足预设终止迭代条件。