CN106970641B - 无人机环绕物体外形轮廓飞行的控制方法 - Google Patents

Abstract

本发明提供的是一种无人机环绕物体外形轮廓飞行的控制方法。无人机开机自检；起飞，使待环绕目标进入目标可检测范围，开启目标环绕模式；利用激光雷达探测信息解算无人机机头方向与目标中心方向的夹角即转差角及无人机与目标的距离即目标距离；将转差角输入转向PID控制器，控制器输出量用于控制无人机锁定目标；将目标距离输入定距PID控制器，控制器输出量用于控制无人机与目标维持在设定距离；控制无人机环绕目标外形轮廓飞行。本发明的无人机环绕物体外形轮廓飞行的控制方法能控制机头方向始终对着环绕目标，并可以控制无人机沿着目标外形轮廓定距飞行，大大降低手动控制无人机环绕目标飞行的难度。

Description

无人机环绕物体外形轮廓飞行的控制方法

技术领域

本发明涉及的是一种无人机控制方法，具体地说是一种无人机环绕物体外形轮廓飞行的控制方法。

背景技术

无人机具有使用成本低，无直接人员伤亡风险，可搭载多种检测仪器等特点。无人机在航拍、搜救、野生动物考察、大气研究、电力巡线、桥梁检测等领域发挥重要作用。

桥梁支座是桥梁易损坏部位，是桥梁检测的重点检查对象，无人机可搭载相机对支座病害进行拍照记录，人为控制无人机环绕桥墩飞行检查桥梁支座健康状态对无人机操控者操控技术要求较高，采用无人机定距环绕桥墩飞行控制技术可大幅度降低无人机操控难度，同时提高桥梁检测效率。

名称为“无人机定点环绕飞行的控制方法”的专利文件中，通过遥控器或地面站设定环绕目标点和环绕半径，利用GPS和机载传感器对无人机进行定位，控制无人机环绕目标点飞行，该方法不能沿物体外形轮廓飞行，在无GPS信号情况下该方法不能使用。双目视觉可构建环境三维立体影像，可以获得环境大量信息，在目标跟踪中有广泛应用，双目视觉可识别环绕目标的方位和无人机与环绕目标的距离，也可用于环绕飞行控制。视觉传感器获取的环境的信息量大，特征提取算法复杂，计算量大，当环境光线较暗或者较强时或环境特征不明显时会影响特征提取效果，进而影响无人机环绕目标飞行的控制效果。

发明内容

本发明的目的在于提供一种计算复杂度低、易于操控的无人机环绕物体外形轮廓飞行的控制方法。

本发明的目的是这样实现的：

步骤A，无人机开机自检，检查系统中各传感器工作状态，当有故障时通过提示装置发出警报；

步骤B，遥控无人机起飞，使环绕目标进入目标可检测范围，开启目标环绕模式，通过提示装置发出环绕模式开启成功或开启失败的提示信息；

步骤C，利用激光雷达探测信息解算无人机机头与目标中心方向的夹角即转差角及无人机与目标的距离即目标距离；

步骤D，将转差角输入转向PID控制器，转向PID控制器输出量用于控制无人机锁定目标；

步骤E，将目标距离输入定距PID控制器，定距PID控制器输出量用于控制无人机与目标维持在设定距离，环绕模式下通过遥控器俯仰通道改变定距距离；

步骤F，控制无人机环绕目标飞行。

本发明还可以包括：

1、步骤C具体包括：将激光雷达探测信息变换到水平面内，在目标可检测范围搜索连续体，合并可以合并的连续体，寻找探测点数最多连续体，确定连续体边界，计算转差角和目标距离；

步骤C1，利用陀螺仪和加速度计信息计算无人机的姿态，包括横滚角、俯仰角和航向角，激光雷达与无人机具有相同的姿态角；

载体坐标系到水平坐标系的坐标变换矩阵为：

其中α为横滚角，β为俯仰角，利用坐标变换矩阵将激光雷达探测信息变换到水平面内；

步骤C2，所述连续体是指根据判定规则确定的激光雷达探测点的集合；连续体均由一个激光雷达探测点逐步扩充得到，连续体扩充判定规则为：

其中r和γ分别为激光雷达探测点的探测距离和方位角，下标表示探测点编号，min表示小括号中取二者中较小值，dDis为相邻激光雷达探测点的探测距离差，arcDis为相邻探测点以其中最小探测距离为半径所夹圆弧长度，FACD为设定的判定因数，使用连续体扩充判定规则确定连续体；

步骤C3，使用连续体扩充判定规则对间隔探测点数小于a的连续体边界探测点进行判定，合并可以合并的连续体，a为正整数；

步骤C4，根据连续体距离在[minDis,maxDis]范围内寻找点数最多的连续体即最大连续体，其中minDis和maxDis为设定的搜索最大连续体的最小和最大的环绕目标与无人机的距离；

对于包含b个探测点的连续体其连续体距离oDis计算公式为：

oDis＝(r₁+r₂+…+r_b)/b

步骤C5，当最大连续体左侧边界小于等于右侧边界时，从右侧边界向内搜寻探测点作为新边界，将新边界与左侧边界比较，若新边界距离小于左侧边界距离或新边界与左侧边界夹角小于χ度，停止继续向内搜寻，将新边界作为最大连续体的新右侧边界，χ为可选设定夹角值；

当最大连续体右侧边界小于左侧边界时，从左侧边界向内搜寻探测点作为新边界，将新边界与右侧边界比较，若新边界距离小于右侧边界距离或新边界与右侧边界夹角小于χ度，停止继续向内搜寻，将新边界作为最大连续体的新左侧边界；

将左右两侧边界所确定的连续体作为环绕目标；

步骤C6，目标中心方向为：

γ_o＝(γ_l+γ_r)/2

其中γ_o为目标中心方向，γ_l为左侧边界方位角，γ_r为右侧边界方位角；

目标方向与机头方向的角度差简称转差角为

其中为转差角，γ_u为机头在激光雷达探测区域对应方位角；

无人机与环绕目标距离简称目标距离为

r_o＝(r₁+…+r_m+r_m+1+…+r_2m)/(2*m)

其中r_o为目标距离，r₁,r₂…r_m为目标中心方向左侧与目标中心方向邻近的连续m个激光雷达探测点，r_m+1,r_m+2…r_2m为目标中心方向右侧与目标中心方向邻近的连续m个激光雷达探测点，m为可选正整数值。

2、步骤D具体包括：转向控制在每个转差角输出周期确定机头方向设定值为

其中为机头设定方向，为采样时刻机头方向，为转差角；

机头方向通过与无人机相对固定的陀螺仪和加速度传感器数据解算得到，或者通过与无人机相对固定的磁力计传感器数据解算得到；

在每个转向控制周期，转向PID控制器输出为：

其中c_r为转向PID控制器的输出量用于转向控制，P_r、I_r和D_r为转向PID控制器参数，E_intR为误差积分。

3、定距PID控制器为：

其中c_d为定距PID控制器的输出量用于定距控制，P_d、I_d和D_d为定距PID控制器参数，E_intD为误差积分，r_t为采样时刻无人机与环绕目标的距离，r_s为设定的定矩距离，r_s取为固定的距离值r_sd或进入环绕模式时采样到的目标距离r_so。

4、所述通过遥控器俯仰通道改变定距距离是指：设定距离通过俯仰通道调整为

其中r_sa为调整后的设定距离，R_p为遥控器俯仰通道值，R_pmin、R_pmid、R_pmax分别为遥控器俯仰通道最小值、中间值和最大值，r_smin为允许的最小设定距离，r_smax为允许的最大设定距离。

5、所述控制无人机环绕目标飞行包括通过手动方式或设定环绕速度方式，

所述通过手动方式是指将遥控器的横滚信号直接作为无人机环绕目标飞行的环绕角速率的控制量；

所述设定环绕速度方式是指通过遥控器横滚通道信号量设定无人机环绕目标飞行的角速率，设定的环绕角速率为：

ω_s＝(R_r-R_rmid)/(R_rmax-R_rmin)*ω_max

其中ω_s为设定环绕角速率，R_r为遥控器俯仰通道值，R_rmin、R_rmid、R_rmax分别为遥控器横滚通道最小值、中间值和最大值，ω_max为允许的最大环绕角速率；

环绕角速率PID控制器为：

其中c_ω为环绕角速率PID控制器的输出量，用于控制无人机环绕目标飞行的环绕角速率，P_ω、I_ω和D_ω为环绕角速率PID控制器参数，E_intω为误差积分，ω_t为无人机在水平坐标系内z轴方向旋转角速率，ω_s为设定环绕角速率。

所述无人机开机自检是指无人机系统启动后，系统检测陀螺仪传感器、加速度计传感器，激光雷达等通信和传感器输出数据是否正常，当有异常时通过提示装置发出警报。

所述提示装置至少包括以下方式中的一种：

无人机上的LED灯；

具有显示页面的控制终端；

能发出异常提示音或语音的可穿戴设备。

所述警报方式至少包括以下方式中的一种：

控制所述无人机上的指示灯闪烁；

控制所述控制终端显示异常信息；

控制所述可穿戴式设备发出异常提示音或语音。

所述目标进入目标可检测范围是指在激光雷达探测角度范围内设定的机头两边正负ψ角度范围及激光雷达有效探测距离所限定的扇形区域。

环绕飞行模式开关开启后，无人机系统根据传感器检测信息判定是否符合进入环绕飞行模式条件，并能通过提示装置提示环绕模式开启失败或成功的消息。

环绕飞行模式开关开启后，当转差角过大、目标距离过小或过大等因素，使无人机不能稳定继续环绕物体轮廓飞行，应退出环绕飞行模式并进入其他飞行模式如手动控制模式，并通过所述提示装置发出警报。

本发明提供了一种无人机环绕目标外形轮廓飞行的控制方法，使用激光雷达确定目标方位及无人机与目标外形轮廓的距离，本发明的有益效果为：

1.使用该方法可控制无人机环绕目标外形轮廓定距飞行，降低无人机操控难度；

2.使用机载传感器即可完成环绕飞行控制，不依赖于卫星导航等外界信息；

3.该方法计算量小，易于在低性能的处理器上实现；

4.激光雷达为主动发光原件，可在暗光环境下工作；

5.设置了提示装置向操控者提示系统工作状态。

附图说明

图1是本发明实施例的无人机环绕物体外形轮廓飞行控制方法的流程示意图。

图2是本发明实施例的连续体扩充判定规则示意图。

图3是本发明实施例的无人机环绕柱状物体外形轮廓定距飞行示意图。

具体实施方式

下面结合附图举例对本发明做详细描述，本发明的实施例是对本发明的的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

图1是本发明实施例的一种无人机环绕物体外形轮廓飞行控制方法的流程示意图。应说明的是，本发明实施例的所述方法并不限于图所示的流程图中的步骤及顺序。根据不同的实施例，图所示的流程图中的步骤可以增加、移除、或者改变顺序。在本实施方式中，无人机环绕物体外形轮廓飞行控制包括如下步骤：

步骤S101，无人机开机自检，发出故障警告。

步骤S102，遥控无人机使环绕目标进入目标可检测范围，开启目标环绕模式。

步骤S103，利用激光雷达探测信息解算转差角和目标距离。

步骤S104，将转差角输入转向PID控制器，控制器输出量用于控制无人机机头锁定环绕目标。

步骤S105，将目标距离输入定距PID控制器，控制器输出量用于控制无人机与环绕目标距离维持在定距设定值。

步骤S106，手动方式或设定环绕速度方式控制无人机环绕目标飞行。

在步骤S101中，无人机系统启动后，系统检测陀螺仪传感器、加速度计传感器，激光雷达是否可由控制器完成初始化设置，检测传感器输出数据是否正常，当有异常时通过无人机操控人员端的可穿戴设备发出语音警报。

在步骤S102中，由无人机操控人员控制无人机起飞，使目标进入目标可检测范围，如使被环绕目标在无人机机头两边正负60度范围内可被检测到且无人机与目标外形轮廓在1～3米之间。进入上述区域通过无人机操控人员手中遥控器开启环绕飞行控制开关，无人机可成功进入环绕飞行模式，若在上述区域之外将不能开启环绕飞行模式。当环绕飞行模式控制开关开启后，无人机系统可以通过无人机操控人员端的可穿戴设备发出环绕飞行模式开启成功或失败的提示语音。

步骤S103利用激光雷达信息解算转差角和目标距离步骤包括：将激光雷达探测信息变换到水平面内，在目标可检测范围搜索连续体，合并可以合并的连续体，寻找探测点数最多连续体，确定连续体边界，计算转差角和目标距离。

(1)利用陀螺仪和加速度计信息计算无人机的姿态包括横滚角、俯仰角和航向角，由于激光雷达与无人机相对固定的，二者具有相同的姿态角。

使用欧拉角法由载体坐标系到水平坐标系的坐标变换矩阵为：

其中α为横滚角，β为俯仰角，利用坐标变换矩阵将激光雷达探测信息变换到水平面内。

(2)所述连续体是指根据一定判定规则确定的激光雷达探测点的集合。

连续体均由一个激光雷达探测点逐步扩充得到，连续体扩充判定规则示意图如图2所示，连续体扩充判定规则为：

其中r和γ分别为激光雷达探测点的探测距离和方位角，下标表示探测点编号，min表示小括号中取二者中较小值，dDis为相邻激光雷达探测点的探测距离差，arcDis为相邻探测点以其中最小探测距离为半径所夹圆弧，FACD为人为设定的判定因数。

使用连续体扩充判定规则确定连续体。

(3)使用连续体扩充判定规则对间隔探测点数小于3点的连续体的边界探测点进行判定，合并可以合并的邻近连续体。

(4)根据连续体距离在0.5～4米范围内寻找点数最多的连续体即最大连续体，对于包含b个探测点的连续体其连续体距离oDis计算公式为：

oDis＝(r₁+r₂+…+r_b)/b

(5)以无人机环绕柱状物体为例来说明转差角和无人机与目标距离计算方法，图3为无人机环绕柱状物体外形轮廓定距飞行示意图。

连续体左侧边界是指在机头方向从无人机上方看去，逆时针方向一侧的连续体边缘探测点，连续体左侧边界是指在机头方向从无人机上方看去，顺时针方向一侧的连续体边缘探测点。

在图3中，左侧边界小于右侧边界，从右侧边界向连续体内部寻找新边界，向内寻找的第一个激光雷达探测点，探测距离小于左侧边界距离，满足新右侧边界的条件，可以将该探测点作为新右侧边界，将左侧边界和新右侧边界所确定的连续体作为环绕目标。

(6)目标中心方向为：

γ_o＝(γ_l+γ_r)/2

其中γ_o为目标中心方向，γ_l为左侧边界方向，γ_r为右侧边界方向。

目标方向与机头方向的角度差即转差角为：

其中为转差角，γ_u为机头在激光雷达探测区域对应方向。

取m＝3，求取无人机与环绕目标距离的2m个激光雷达探测点如图3所示。

r_o＝(r₁+r₂+r₃+r₄+r₅+r₆)/6

其中r_o为目标距离，r₁,r₂,r₃为目标中心方向左侧与目标中心方向邻近的连续3个激光雷达探测点，r₄,r₅,r₆为目标中心方向右侧与目标中心方向邻近的连续3个激光雷达探测点。

步骤S104，所述转向控制在每个转差角输出周期确定机头设定转向

其中为机头设定方向，为采样时刻机头方向。

其中机头方向通过与无人机相对固定的陀螺仪和加速度传感器数据解算得到。

在每个转向控制周期，所述转向PID控制器输出为：

其中c_r为PID控制器的输出量用于转向控制，P_r、I_r和D_r为转向控制PID参数，E_intR为误差积分，Gyroz为有陀螺仪数据计算得到的无人机在水平坐标系内z轴方向旋转角速度。

步骤S105，所述定距PID控制器为：

其中c_d为定距PID控制器的输出量用于定距控制，P_d、I_d和D_d为定距控制PID参数，E_intD为误差积分，r_s为设定的定矩距离，r_t为采样时刻测得的目标距离。

在成功开启环绕模式后采样的目标距离作为设定的定距距离，可通过遥控器俯仰通道改变定距距离，设定距离通过俯仰通道调整为

其中r_sa为调整后的设定距离，R_p为遥控器俯仰通道值，R_pmin、R_pmid、R_pmax分别为遥控器俯仰通道最小值、中间值和最大值，r_smin为允许的最小设定距离，r_smax为允许的最大设定距离。

步骤S106，无人机操控者通过遥控器的俯仰通道，直接控制无人机横滚角大小，间接控制无人机沿环绕目标外形轮廓环绕速度。

当环绕飞行模式开关开启后，环绕飞行模式开关开启后，当转差角过大、目标距离过小或过大等因素，使无人机不能稳定继续环绕物体轮廓飞行，应退出环绕飞行模式并进入其他飞行模式如手动控制模式，并通过所述提示装置发出警报。

最后应说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或等同替换，而不脱离本发明技术方案的精神和范围。

Claims (5)

1.一种无人机环绕物体外形轮廓飞行的控制方法，包括如下步骤：

步骤A，无人机开机自检，检查系统中各传感器工作状态，当有故障时通过提示装置发出警报；

步骤B，遥控无人机起飞，使环绕目标进入目标可检测范围，开启目标环绕模式，通过提示装置发出环绕模式开启成功或开启失败的提示信息；

步骤C，利用激光雷达探测信息解算无人机机头与目标中心方向的夹角即转差角及无人机与目标的距离即目标距离；

步骤D，将转差角输入转向PID控制器，转向PID控制器输出量用于控制无人机锁定目标；

步骤E，将目标距离输入定距PID控制器，定距PID控制器输出量用于控制无人机与目标维持在设定距离，环绕模式下通过遥控器俯仰通道改变定距距离；

步骤F，控制无人机环绕目标飞行；

其特征是步骤C具体包括：将激光雷达探测信息变换到水平面内，在目标可检测范围搜索连续体，合并可以合并的连续体，寻找探测点数最多连续体，确定连续体边界，计算转差角和目标距离；

步骤C1，利用陀螺仪和加速度计信息计算无人机的姿态，包括横滚角、俯仰角和航向角，激光雷达与无人机具有相同的姿态角；

载体坐标系到水平坐标系的坐标变换矩阵为：

其中α为横滚角，β为俯仰角，利用坐标变换矩阵将激光雷达探测信息变换到水平面内；

步骤C2，所述连续体是指根据判定规则确定的激光雷达探测点的集合；连续体均由一个激光雷达探测点逐步扩充得到，连续体扩充判定规则为：

其中r和γ分别为激光雷达探测点的探测距离和方位角，下标表示激光雷达探测点编号，min表示小括号中取二者中较小值，dDis为相邻激光雷达探测点的探测距离差，arcDis为相邻探测点以其中最小探测距离为半径所夹圆弧长度，FACD为设定的判定因数，使用连续体扩充判定规则确定连续体；

步骤C3，使用连续体扩充判定规则对间隔探测点数小于a的连续体边界探测点进行判定，合并可以合并的连续体，a为正整数；

步骤C4，根据连续体距离在[minDis,maxDis]范围内寻找点数最多的连续体即最大连续体，其中minDis和maxDis为设定的搜索最大连续体的最小和最大的环绕目标与无人机的距离；

对于包含b个探测点的连续体其连续体距离oDis计算公式为：

oDis＝(r₁+r₂+···+r_b)/b

步骤C5，当最大连续体左侧边界小于等于右侧边界时，从右侧边界向内搜寻探测点作为新边界，将新边界与左侧边界比较，若新边界距离小于左侧边界距离或新边界与左侧边界夹角小于χ度，停止继续向内搜寻，将新边界作为最大连续体的新右侧边界，χ为可选设定夹角值；

当最大连续体右侧边界小于左侧边界时，从左侧边界向内搜寻探测点作为新边界，将新边界与右侧边界比较，若新边界距离小于右侧边界距离或新边界与右侧边界夹角小于χ度，停止继续向内搜寻，将新边界作为最大连续体的新左侧边界；

将左右两侧边界所确定的连续体作为环绕目标；

步骤C6，目标中心方向为：

γ_o＝(γ_l+γ_r)/2

其中γ_o为目标中心方向，γ_l为左侧边界方位角，γ_r为右侧边界方位角；

目标方向与机头方向的角度差简称转差角为

其中为转差角，γ_u为机头在激光雷达探测区域对应方位角；

无人机与环绕目标距离简称目标距离为

r_o＝(r₁+···+r_m+r_m+1+···+r_2m)/(2*m)

其中r_o为目标距离，r₁,r₂···r_m为目标中心方向左侧与目标中心方向邻近的连续m个激光雷达探测点，r_m+1,r_m+2···r_2m为目标中心方向右侧与目标中心方向邻近的连续m个激光雷达探测点，m为可选正整数值。

2.根据权利要求1所述的无人机环绕物体外形轮廓飞行的控制方法，其特征是步骤D具体包括：转向控制在每个转差角输出周期确定机头方向设定值为

其中为机头设定方向，为采样时刻机头方向，为转差角；

机头方向通过与无人机相对固定的陀螺仪和加速度传感器数据解算得到，或者通过与无人机相对固定的磁力计传感器数据解算得到；

在每个转向控制周期，转向PID控制器输出为：

其中c_r为转向PID控制器的输出量用于转向控制，P_r、I_r和D_r为转向PID控制器参数，E_intR为误差积分。

3.根据权利要求1所述的无人机环绕物体外形轮廓飞行的控制方法，其特征是定距PID控制器为：

其中c_d为定距PID控制器的输出量用于定距控制，P_d、I_d和D_d为定距PID控制器参数，E_intD为误差积分，r_t为采样时刻无人机与环绕目标的距离，r_s为设定的定矩距离，r_s取为固定的距离值r_sd或进入环绕模式时采样到的目标距离r_so。

4.根据权利要求3所述的无人机环绕物体外形轮廓飞行的控制方法，其特征是所述通过遥控器俯仰通道改变定距距离是指：设定距离通过俯仰通道调整为

其中r_sa为调整后的设定距离，R_p为遥控器俯仰通道值，R_pmin、R_pmid、R_pmax分别为遥控器俯仰通道最小值、中间值和最大值，r_smin为允许的最小设定距离，r_smax为允许的最大设定距离。

5.根据权利要求1所述的无人机环绕物体外形轮廓飞行的控制方法，其特征是所述控制无人机环绕目标飞行包括通过手动方式或设定环绕速度方式，

所述通过手动方式是指将遥控器的横滚信号直接作为无人机环绕目标飞行的环绕角速率的控制量；

所述设定环绕速度方式是指通过遥控器横滚通道信号量设定无人机环绕目标飞行的角速率，设定的环绕角速率为：

ω_s＝(R_r-R_rmid)/(R_rmax-R_rmin)*ω_max

其中ω_s为设定环绕角速率，R_r为遥控器俯仰通道值，R_rmin、R_rmid、R_rmax分别为遥控器横滚通道最小值、中间值和最大值，ω_max为允许的最大环绕角速率；

环绕角速率PID控制器为：

其中c_ω为环绕角速率PID控制器的输出量，用于控制无人机环绕目标飞行的环绕角速率，P_ω、I_ω和D_ω为环绕角速率PID控制器参数，E_intω为误差积分，ω_t为无人机在水平坐标系内z轴方向旋转角速率，ω_s为设定环绕角速率。