一种无人机球体感知与捕捉装置及方法

1.本发明属于无人机捕捉飞行物技术领域，尤其涉及一种无人机球体感知与捕捉装置及方法。


背景技术：

2.四旋翼机具有很强的灵活性，通常具有较高的推质比，拥有较大的角加速度，这使他们能够执行复杂且高机动性的任务，例如空中杂技、拦截飞行物等。其中，使用无人机接球、打球极具观赏性。目前，苏黎世联邦理工学院研发的无人机接球系统是世界上较为知名的无人机杂耍系统，它能够快速识别球体并捕捉。但是，此类系统的感知依赖于外部动作捕捉设备，外部设备必须花费较大成本部署在固定场地下，无人机本体不具备定位与感知能力，因而很难在其他场景下实现球体捕捉，设备迁移成本也较高。


技术实现要素：

3.为了解决现有技术中存在的无人机接球系统依赖于外部动捕设备因而无法在其他场景实现球体感知与捕捉的问题，提出了一种无人机球体感知与捕捉装置及方法，其特征在于无人机具有自主感知能力，仅依靠自身搭载的传感器即可实现球体的识别、轨迹预测与无人机的轨迹规划，使得无人机可在任何场景下实现快速感知与捕捉球体。具体的技术方案如下：
4.一种无人机球体感知与捕捉装置，包括机载主机，机载主机为十字形支架结构，十字形支架中间上表面具有飞控和深度相机，所述飞控通过串口与机载主机相连，每个支架末端具有电调，所述电调的信号输入线与飞控连接，每个支架末端延伸连接有无刷电机，所述电调的输出端通过导线与无刷电机连接，所述无刷电机的转子轴上安装螺旋桨，网兜架固定安装在机架上，所述机载主机获取深度相机的传感器信息并计算，将感知与捕捉球体的智能方法计算所得的无人机的控制量信息通过串口发送至飞控，由飞控控制电调，电调控制无刷电机的位置和转速从而控制无人机运动。
5.进一步的，所述机载主机包括三大模块：球体识别模块，球体轨迹预测模块，无人机轨迹规划模块。
6.本发明还公开了一种无人机球体感知与捕捉方法，包括如下步骤：
7.步骤1：球体识别；
8.步骤2：球体轨迹预测；
9.步骤3：无人机轨迹规划。
10.进一步地，所述步骤1包括如下步骤：
11.步骤20，获取深度相机的rgb图像及深度信息；
12.步骤21，对rgb图像进行预处理；
13.步骤22，hsv彩色图像分割；
14.步骤23，形态学滤波；
15.步骤24，二值化处理，轮廓检测，并通过位与运算提取图像中轮廓选定的区域；
16.步骤25，对图像进行霍夫圆检测，同时提取步骤24中检测到的每一个轮廓的最小覆盖圆，如检测到的霍夫圆与最小覆盖圆面积差不超过阈值，且霍夫圆圆心在该轮廓内，则取该霍夫圆为目标球体的外轮廓，其圆心记为(xh,yh)，半径记为rh；如不满足，则跳过并进入下一个循环；
17.步骤26，从深度相机的深度信息中获取圆心(xh,yh)处的深度值z；
18.步骤27，对圆心(xh,yh)进行相机内参转换，获取相机坐标系下的目标球体平面中心坐标为(x,y)；
19.步骤28，将识别到的目标球体中心坐标观测值(x,y,z)及发出消息的时间t发送至球体轨迹预测模块；
20.重复执行步骤20-28即可实现球体的实时识别。
21.进一步地，所述步骤2包括如下步骤：
22.步骤30：构建球体动力学模型；在自由落体方程中加入风阻，并优化动力学方程；
23.步骤31：球体初速度获取；利用视觉采样的前后两帧图像中球体的位置进求球体初速度；
24.步骤32：基于卡尔曼滤波的球体位姿估计；利用卡尔曼滤波估计球体姿态；
25.步骤33：基于物理模型的球体轨迹预测；根据球体动力学模型计算出在未来t时刻球体的位置；
26.步骤34：阻力系数优化；在基于卡尔曼滤波的球体位姿估计和基于物理模型的球体轨迹预测的同时，用非线性最小二乘法优化阻力系数；
27.步骤32、33、34在球体识别的一帧至下一帧采样之间完成，每一次循环过程中优化读取的球体坐标点以及阻力系数，当误差值小于阈值时，即得到最后预测的t时刻的小球坐标pf＝(xf,yf,zf)
t
，将其发送给无人机轨迹规划模块。
28.进一步地，所述步骤3包括如下步骤：
29.步骤40：构建无人机动力学模型；根据目前无人机的通用建模方法，将无人机建模为具有六个自由度的刚体；
30.步骤41：将无人机轨迹规划构建为最优控制问题；使用加加速度来描述无人机关于时间t的轨迹x(t)，并将无人机轨迹规划构建为最优边界值问题，即obvp问题；
31.步骤42：求解最优控制问题；利用庞特里亚金极小值原理求解obvp问题；
32.步骤43：将最优控制问题转化为多项式求极值问题并求解；
33.步骤44：执行最优控制量；将计算得到的控制量利用串口通讯下发至飞控，飞控执行，从而实现无人机捕捉球体。
34.进一步地，所述步骤1包括如下具体步骤：
35.步骤20，对rgb图像进行预处理包括：高斯滤波、色彩空间转换：bgr格式转换至hsv格式；
36.步骤22，根据设定好的所需目标球体的颜色范围，进行hsv彩色图像分割；
37.步骤23，对分割后的图像进行形态学滤波，包括腐蚀、膨胀、腐蚀三步操作；
38.步骤24，对形态学滤波后的图像进行二值化处理，对处理后的二值图像进行轮廓
检测，并通过位与运算提取图像中轮廓选定的区域；
39.进一步地，所述步骤2包括如下具体步骤：
40.步骤30：构建球体动力学模型：设三维空间下球体状态为x＝(p
x
,py,pz,v
x
,vy,vz)
t
，不考虑球体自旋的情况下，受风阻影响时球体的动力学模型为：
[0041][0042]
公式(a)中，g为重力加速度，kd为阻力系数，满足其中m为球体质量，cd为空气阻力系数，ρa为空气密度，a为球体横截面积，将一段连续飞行中||v||看作一个与该段轨迹初始运动速度相关的固定值，即令：
[0043]
k＝f(v0)＝kd||v||
ꢀꢀ
(b)
[0044]
由于对式(a)做了线性简化处理，这里通过增加k变化的自由度降低对||v||变化简化的误差，对于x,y,z三个方向区别处理，以k
x
,ky,kz代表，由此，球体的动力学模型进一步简化为：
[0045][0046]
式(c)是球体动力学模型最终优化结果，为在三个方向独立解耦的一阶线性微分形式，若给出球的初始状态x0＝(p
x0
,p
y0
,p
z0
,v
x0
,v
y0
,v
z0
)
t
，则可求解球体的位置和速度随时间t变化的连续运动模型，如下式：
[0047][0048][0049]
通过式(d)和式(e)可获得球体离散运动模型为：
[0050][0051][0052]
式(f)和(g)中，p(i)为球体运动到第i个点的位置，v(i)为球体运动到第i个点的速度，δt为视觉观测采样周期；
[0053]
步骤31：球体初速度获取：由公式(d)至(g)可知，利用球体离散运动模型和连续运动模型进行位姿估计和轨迹预测时需要已知球体初速度，设视觉采样的第一帧图像中球体位置为p1＝(x1,y1,z1)
t
，第二帧图像中球体位置为p2＝(x2,y2,z2)
t
，则初速度为：
[0054][0055]
式(h)中，取v0为位置p2的速度，即以(p2,v0)作为球体的初始状态(p0,v0)；步骤32：基于卡尔曼滤波的球体位姿估计：由公式(f)和(g)可知，球体的离散运动模型可改写为：
[0056][0057]
根据式(i)状态转移方程，采用卡尔曼滤波对球体识别步骤28中给出的球体中心坐标观测值(x,y,z)进行位姿估计，即得到最优估计位置pm＝(xm,ym,zm)t，并将最优估计位置用于下一步的球体轨迹预测；
[0058]
步骤33：基于物理模型的球体轨迹预测：根据公式(f)和(g)，可直接通过球体的连续运动模型获取t时刻的球体坐标pf＝(xf,yf,zf)
t
，该坐标位于实际场地坐标系中；
[0059]
步骤34：阻力系数优化：在基于卡尔曼滤波的球体位姿估计和基于物理模型的球体轨迹预测的同时，用非线性最小二乘法优化阻力系数k
x
,ky,kz，构建xyz方向的位置误差的平方项，记为：
[0060][0061]
其中，i为球体处于的第i个坐标点，n代表选取用于非线性最小二乘法的坐标点个数。每一次循环时阻力系数优化目标函数如下：
[0062][0063]
采用ceres非线性优化库，调用非线性最小二乘求解器对上式求解。
[0064]
进一步地，所述步骤3包括如下具体步骤：
[0065]
步骤40：构建无人机动力学模型：根据目前无人机的通用建模方法，将无人机建模为具有六个自由度的刚体，分别为三维笛卡尔坐标系上的位置(x1,x2,x3)和角速度(w1,w2,
w3)，则其动力学方程为：
[0066][0067][0068]
其中，g为重力加速度，r为旋转矩阵，e3＝(0,0,1)，为角速度的斜对称矩阵形式：
[0069][0070]
步骤41：将无人机轨迹规划构建为最优控制问题：使用加加速度来描述无人机关于时间t的轨迹x(t)，那么控制系统输入的力f为：
[0071][0072]
对式(l)求导并代入(m)，得到式(o)，对式(n)求导并代入式(l)，得到式(p)；
[0073][0074][0075]
由式(0)和式(p)可推导得：
[0076][0077]
式(n)和式(q)说明了可以通过无人机的加加速度j计算得到无人机的控制量力f以及角速度w，可将最优控制问题的目标函数表示为:
[0078][0079]
其中i表示坐标系的轴数，为了方便计算，将无人机三个轴的变量分别计算，后述轴数i也均省略，该控制系统的状态量为s＝(p,v,a)，即包含位置p、速度v、加速度a，无人机初始时刻静止，初始状态为s0＝(p0,v0,a0)＝(0,0,0)，无人机要实现捕捉球体，需要满足球体在某时刻的位置与无人机的位置相同，根据球体轨迹预测模块得到t时刻的小球位置pf，则无人机运动的终端状态sf＝(pf,vf,af)，至此，将无人机轨迹规划构建为了最优边界值问题：obvp问题；
[0080]
步骤42：求解最优控制问题；求解obvp问题有成熟的数学方法，首先引入协态λ＝(λ1,λ2,λ3)，定义哈密尔顿方程：
[0081]
h(s,j,λ)＝j2+λ
t
f(s,j)＝j2+λ1v+λ2a+λ3j
ꢀꢀ
(s)
[0082]
根据极小值原理得：
[0083][0084]
其中星号*表示最优量。引入常数系数α,β,γ，可得：
[0085][0086]
最优输入为：
[0087][0088]
对式(v)求积分得：
[0089][0090]
则从初始状态经过时间t后，终端状态的最优状态为s
*
(t)＝sf，结合极小值原理的约束条件解得：
[0091][0092]
步骤43：将最优控制问题转化为多项式求极值问题并求解，将式(v)代入式(o)，得：
[0093][0094]
将式(x)代入式(y)，由式(x)得知α,β,γ的取值仅与t相关，则式(y)中j为仅与t相关的函数，利用费拉里法求得式(y)的极值以及对应的t；
[0095]
步骤44：执行最优控制量：利用上述t，代入式(x)可求得α,β,γ，代入式(v)可求得最优加加速度关于时间t的函数，根据式(n)和式(q)可计算得控制量力f以及角速度w，将其利用串口通讯下发至飞控，飞控执行，从而实现无人机捕捉球体。
[0096]
本发明的有益效果是：
[0097]
(1)本发明不依赖于外部动捕设备，仅依靠无人机自身搭载的传感器和机载计算单元即可实现球体的识别、轨迹预测与无人机的轨迹规划。
[0098]
(2)本发明能够实现对传感器误差的补偿，并能够实时优化球体的风阻系数，从而减少不确定性对球体识别与轨迹预测带来的影响。
[0099]
(3)本发明能够在轨迹规划时考虑无人机动力学，从而快速准确地捕捉球体。
附图说明
[0100]
图1是本发明的无人机球体感知与捕捉装置示意图。
[0101]
图2是本发明的无人机球体感知与捕捉方法总体流程示意图。
[0102]
图3是本发明的球体识别流程示意图。
[0103]
图4是本发明的球体轨迹预测流程示意图。
[0104]
图5是本发明的无人机轨迹规划流程示意图。
[0105]
图中标记说明：1、螺旋桨；2、无刷电机；3、电调；4、飞控；5、机载主机；6、深度相机；7、网兜架；8、机架；10、球体识别模块；11、球体轨迹预测模块；12、无人机轨迹规划模块。
具体实施方式
[0106]
为了使本发明的目的、技术方案和优点更加清楚，下面结合附图和具体实施步骤
来对本专利的技术方案进一步详细的说明。
[0107]
如图1所示，本发明的无人机球体感知与捕捉装置，包括机载主机5，机载主机5为十字形支架结构，十字形支架中间上表面具有飞控4和深度相机6，深度相机6用于实时传输图像和点云数据。飞控4通过串口与机载主机5相连，每个支架末端具有电调3，电调3的信号输入线与飞控4连接。每个支架末端延伸连接有无刷电机2，电调3的输出端通过导线与无刷电机2连接。无刷电机2的转子轴上安装螺旋桨1。网兜架7固定安装在机架8上。机载主机5获取深度相机6的传感器信息并计算，将感知与捕捉球体的智能方法计算所得的无人机的控制量信息通过串口发送至飞控4，由飞控4控制电调3，电调3控制无刷电机2的位置和转速从而控制无人机运动。
[0108]
如图2所示，本发明的无人机球体感知与捕捉方法，包括三大模块：球体识别模块10，球体轨迹预测模块11，无人机轨迹规划模块12，均部署在机载主机5中。
[0109]
实施例1：启动无人机，使无人机悬停于空中。配合者正对无人机深度相机前方3-5米处，使得配合者进入深度相机视野范围内。配合者手持球体，向前方抛出。
[0110]
机载主机5获取深度相机6的传感器信息并计算，首先，进行球体识别10，如图3所示，包含以下计算方法：
[0111]
步骤20，获取深度相机6的rgb图像及深度信息；
[0112]
步骤21，对rgb图像进行预处理，包括：高斯滤波、色彩空间转换(bgr格式转换至hsv格式)；
[0113]
步骤22，根据设定好的所需目标球体的颜色范围，进行hsv彩色图像分割；
[0114]
步骤23，对分割后的图像进行形态学滤波，包括腐蚀、膨胀、腐蚀三步操作；
[0115]
步骤24，对形态学滤波后的图像进行二值化处理，对处理后的二值图像进行轮廓检测，并通过位与运算提取图像中轮廓选定的区域；
[0116]
步骤25，对图像进行霍夫圆检测，同时提取步骤24中检测到的每一个轮廓的最小覆盖圆，如检测到的霍夫圆与最小覆盖圆面积差不超过阈值，且霍夫圆圆心在该轮廓内，则取该霍夫圆为目标球体的外轮廓，其圆心记为(xh,yh)，半径记为rh；如不满足，则跳过并进入下一个循环；
[0117]
步骤26，从深度相机6的深度信息中获取圆心(xh,yh)处的深度值z；
[0118]
步骤27，对圆心(xh,yh)进行相机内参转换，获取相机坐标系下的目标球体平面中心坐标为(x,y)；
[0119]
步骤28，将识别到的目标球体中心坐标观测值(x,y,z)及发出消息的时间t发送至球体轨迹预测11模块。
[0120]
重复执行步骤20-28即可实现球体的实时识别。
[0121]
然后，进行球体轨迹预测11，在考虑风阻等外界干扰的情况下，能更准确地估计从球体识别10模块获取的实时球体位置，并预测球体未来的轨迹。如图4所示，该轨迹预测方法主要包含以下计算方法：
[0122]
步骤30：构建球体动力学模型。设三维空间下球体状态为x＝(p
x
,py,pz,v
x
,vy,vz)
t
，不考虑球体自旋的情况下，受风阻影响时球体的动力学模型为：
[0123][0124]
公式(a)中，g为重力加速度，kd为阻力系数，满足其中m为球体质量，cd为空气阻力系数，ρa为空气密度，a为球体横截面积。为了减小由于||v||带来的非线性和各向耦合影响，将一段连续飞行中||v||看作一个与该段轨迹初始运动速度相关的固定值，即令：
[0125]
k＝f(v0)＝kd||v||
ꢀꢀ
(b)
[0126]
由于对式(a)做了线性简化处理，这里通过增加k变化的自由度降低对||v||变化简化的误差，对于x,y,z三个方向区别处理，以k
x
,ky,kz代表，由此，球体的动力学模型进一步简化为：
[0127][0128]
式(c)是球体动力学模型最终优化结果，为在三个方向独立解耦的一阶线性微分形式。若给出球的初始状态x0＝(p
x0
,p
y0
,p
z0
,v
x0
,v
y0
,v
z0
)
t
，则可求解球体的位置和速度随时间t变化的连续运动模型，如下式：
[0129][0130][0131]
通过式(d)和式(e)可获得球体离散运动模型为：
[0132][0133][0134]
式(f)和(g)中，p(i)为球体运动到第i个点的位置，v(i)为球体运动到第i个点的速度，δt为视觉观测采样周期。
[0135]
步骤31：球体初速度获取。由公式(d)至(g)可知，利用球体离散运动模型和连续运动模型进行位姿估计和轨迹预测时需要已知球体初速度，设视觉采样的第一帧图像中球体位置为p1＝(x1,y1,z1)
t
，第二帧图像中球体位置为p2＝(x2,y2,z2)
t
，则初速度为：
[0136][0137]
式(h)中，取v0为位置p2的速度，即以(p2,v0)作为球体的初始状态(p0,v0)。
[0138]
步骤32：基于卡尔曼滤波的球体位姿估计。由公式(f)和(g)可知，球体的离散运动模型可改写为：
[0139][0140]
为了减少传感器误差，获取更为精确的球体位置，根据式(i)状态转移方程，采用卡尔曼滤波对球体识别10步骤28中给出的球体中心坐标观测值(x,y,z)进行位姿估计，即得到最优估计位置pm＝(xm,ym,zm)
t
，并将最优估计位置用于下一步的球体轨迹预测。
[0141]
步骤33：基于物理模型的球体轨迹预测。根据公式(f)和(g)，可直接通过球体的连续运动模型获取t时刻的球体坐标pf＝(xf,yf,zf)
t
，该坐标位于实际场地坐标系中。
[0142]
步骤34：阻力系数优化。在基于卡尔曼滤波的球体位姿估计32和基于物理模型的球体轨迹预测33的同时，用非线性最小二乘法优化阻力系数k
x
,ky,kz，构建xyz方向的位置误差的平方项，记为：
[0143][0144]
其中，i为球体处于的第i个坐标点，n代表选取用于非线性最小二乘法的坐标点个数。每一次循环时阻力系数优化目标函数如下：
[0145][0146]
采用ceres非线性优化库，调用非线性最小二乘求解器可对上式求解。
[0147]
步骤32、33、34在球体识别10的一帧至下一帧采样之间完成，每一次循环过程中优化了读取的球体坐标点以及阻力系数，能够较精确地预测球体的运动轨迹。当式(j)中的误差值小于阈值时，即得到最后预测的t时刻的小球坐标pf＝(xf,yf,zf)
t
，将其发送给无人机轨迹规划12模块。
[0148]
最后，进行无人机轨迹规划12并执行控制量。将球体轨迹预测11模块得到的球体坐标作为无人机运动规划的终端状态。如图5所示，其包含以下计算方法：
[0149]
步骤40：构建无人机动力学模型。根据目前无人机的通用建模方法，将无人机建模为具有六个自由度的刚体，分别为三维笛卡尔坐标系上的位置(x1,x2,x3)和角速度(w1,w2,w3)，则其动力学方程为：
[0150]
[0151][0152]
其中，g为重力加速度，r为旋转矩阵，e3＝(0,0,1)，为角速度的斜对称矩阵形式：
[0153][0154]
步骤41：将无人机轨迹规划构建为最优控制问题。使用加加速度来描述无人机关于时间t的轨迹x(t)，那么控制系统输入的力f为：
[0155][0156]
对式(l)求导并代入(m)，得到式(o)。对式(n)求导并代入式(l)，得到式(p)。
[0157][0158][0159]
由式(0)和式(p)可推导得：
[0160][0161]
式(n)和式(q)说明了可以通过无人机的加加速度j计算得到无人机的控制量力f以及角速度w。可将最优控制问题的目标函数表示为:
[0162][0163]
其中i表示坐标系的轴数。为了方便计算，将无人机三个轴的变量分别计算，后述轴数i也均省略。该控制系统的状态量为s＝(p,v,a)，即包含位置p、速度v、加速度a。无人机初始时刻静止，初始状态为s0＝(p0,v0,a0)＝(0,0,0)。无人机要实现捕捉球体，需要满足球体在某时刻的位置与无人机的位置相同。根据球体轨迹预测(11)模块得到t时刻的小球位置pf，则无人机运动的终端状态sf＝(pf,vf,af)。至此，将无人机轨迹规划构建为了最优边界值问题(obvp问题)。
[0164]
步骤42：求解最优控制问题。求解obvp问题有成熟的数学方法，首先引入协态λ＝(λ1,λ2,λ3)，定义哈密尔顿方程：
[0165]
h(s,j,λ)＝j2+λ
t
f(s,j)＝j2+λ1v+λ2a+λ3j
ꢀꢀ
(s)
[0166]
根据极小值原理得：
[0167][0168]
其中星号*表示最优量。引入常数系数α,β,γ，可得：
[0169][0170]
最优输入为：
[0171]
[0172]
对式(v)求积分得：
[0173][0174]
则从初始状态经过时间t后，终端状态的最优状态为s
*
(t)＝sf，结合极小值原理的约束条件解得：
[0175][0176]
步骤43：将最优控制问题转化为多项式求极值问题并求解。将式(v)代入式(o)，得：
[0177][0178]
将式(x)代入式(y)，由式(x)得知α,β,γ的取值仅与t相关，则式(y)中j为仅与t相关的函数，利用费拉里法即可求得式(y)的极值以及对应的t。
[0179]
步骤44：执行最优控制量。利用上述t，代入式(x)可求得α,β,γ，代入式(v)可求得最优加加速度关于时间t的函数，根据式(n)和式(q)可计算得控制量力f以及角速度w，将其利用串口通讯下发至飞控4，飞控4执行，从而实现无人机捕捉球体。
[0180]
本发明并不限于上述实施例，在本领域技术人员所具有的知识范围内，还可以在不脱离本发明宗旨的前提下做出各种变化。比如：单独使用球体识别、球体轨迹预测、无人机运动规划中某一模块至其它任务中。