一种基于四足机器人的3D点云定位的制作方法

一种基于四足机器人的3d点云定位
技术领域
1.本发明涉及机器人技术领域，具体涉及一种基于四足机器人的3d点云定位。


背景技术：

2.现在市面上绝大多数的机器人定位算法都是基于2d机器人(例如扫地机器人等)或基于自动驾驶的定位导航算法。虽然自动驾驶的定位算法可以满足几乎所有机器人的需求但是它们通常需要较多的资源。另外，2d定位算法虽然消耗的资源较少和具有较为成熟技术。但是并无法满足四足机器人领域的应用(例，无法支持四足机器人跨越障碍物，上下坡等)市面上许多四足机器人的活动范围与普通家用扫地机器人没有差别，仅仅外观上从轮式变成了四足，在使用上完全应用了2d机器人技术。
3.2d扫地机器人并无法支持台阶等有起伏的地形。四足机器人无法简单的使用扫地机器人的定位算法，因为二维定位无法为机器人提供高度上定位。本发明可以为机器人领域发展提供一个更为精准的三维定位算法。


技术实现要素：

4.本发明致力于更好的解决四足机器人在复杂地形的定位问题。解决了2d定位在台阶，草地以及上下坡的使用中具有较高的局限性。
5.为解决上述技术问题，本发明所采用的技术方案是：一种基于四足机器人的3d点云定位，包括一下步骤：
6.s1：对机器人配备16线激光雷达，imu，以及作业地区的3d点云图；
7.s2：在机器人作业前需要在图形界面标注机器人的初始位置或者在源码中给定初始位置的坐标；
8.s3：再给定初始位置后便可以自由移动机器人，并实时定位可以在图形界面中实时观察到机器人的相对位置。
9.本发明技术方案的进一步改进在于：所述s1包括s1.1：设置地图slam：
10.设xk为节点k，令zk和ωk为与xk有关的约束的均值和信息矩阵。目标函数定义为：
11.f(x)＝∑ek(xk,zk)
t
ω
kek
(xk,zk)
12.其中，ek(xk,zk)是参数xk和约束zk之间的误差函数。
13.通过使用gauss-newton或levenberg-marquardt算法进行线性化和最小化。
14.本发明技术方案的进一步改进在于：所述s1.1包括s1.1-a：在s1.1基础上，如果部分参数出现非欧几里得空间(如姿势参数)，算法可能会出现不可预料的结果或者陷入死循环中。因此在此基础上需要增加流形优化。流形是参数的最小表示并在本地充当欧几里得空间。算法将引入了一个算子它在流形上转换一个局部变化δx。通常，在3dslam中，节点xk的传感器姿态参数位于k(平移向量tk和四元数qk)。四元数的一个流形qk＝[qw,qx,qy,qz]t可以表示为[qx,qy,qz]t，算子描述为：(注q0*＝q*-δq*)
[0015][0016]
本发明技术方案的进一步改进在于：所述s3包括s3.1：采用迭代应用扫描匹配算法来估计传感器的自我运动。
[0017]
本发明技术方案的进一步改进在于：所述s3.1包括s3.1-a：当帧之间的传感器运动变得非常大，并且由于大的位移，扫描匹配可能会错误地估计传感器的自我运动时，采用无迹卡尔曼滤波器将ndt扫描匹配与3d雷达提供的角速度数据相结合，传感器速度假设为：
[0018][0019]
其中，p是位置，q是旋转四元数，v是速度，bat是传感器在时间t的角速度偏差。假设传感器运动模型的平移速度恒定，角速度传感器的偏差恒定，预测状态的系统方程定义为：
[0020][0021]
其中，δ是t和t-1之间的持续时间，δq是δt期间由偏置补偿角速度引起的旋转。
[0022][0023]
由于采用了上述技术方案，本发明相对现有技术来说，取得的技术进步是：
[0024]
本发明提供一种基于四足机器人的3d点云定位，通过搭配16线激光雷达，通过机器人的位置以及雷达的fov(视野角)来计算机器人与各个障碍物的距离。例如本发明测试使用的velodyne雷达将提供30度的垂直视场角以及100米的探测长度。在得到宽广的探测范围后，本发明算法将使用分割地面与障碍物后的数据对自身进行定位。同时，在机器人应用中，可以根据自身移动后数据的变化得出物体的大概高度(即是否允许机器人通过得高度，可用于三维自动导航算法)；本发明可以为机器人领域发展提供一个更为精准的三维定位算法，致力于更好的解决四足机器人在复杂地形的定位问题。解决了2d定位在台阶，草地以及上下坡的使用中具有较高的局限性。
附图说明
[0025]
图1为本发明地图slam示意图；
[0026]
图2为本发明避障算法结构示意图。
具体实施方式
[0027]
下面结合实施例对本发明做进一步详细说明：
[0028]
实施例1
[0029]
如图1-2所示，本发明提供了一种基于四足机器人的3d点云定位，包括一下步骤：s1：对机器人配备16线激光雷达，imu，以及作业地区的3d点云图；s2：在机器人作业前需要在图形界面标注机器人的初始位置或者在源码中给定初始位置的坐标；s3：再给定初始位置后便可以自由移动机器人，并实时定位可以在图形界面中实时观察到机器人的相对位置。
[0030]
在本实施例中，通过搭配16线激光雷达，通过机器人的位置以及雷达的fov(视野
角)来计算机器人与各个障碍物的距离。例如本发明测试使用的velodyne雷达将提供30度的垂直视场角以及100米的探测长度。在得到宽广的探测范围后，本发明算法将使用分割地面与障碍物后的数据对自身进行定位。同时，在机器人应用中，可以根据自身移动后数据的变化得出物体的大概高度(即是否允许机器人通过得高度，可用于三维自动导航算法)；本发明可以为机器人领域发展提供一个更为精准的三维定位算法，致力于更好的解决四足机器人在复杂地形的定位问题。解决了2d定位在台阶，草地以及上下坡的使用中具有较高的局限性。
[0031]
实施例2
[0032]
如图1-2所示，在实施例1的基础上，本发明提供一种技术方案：优选的，设置地图slam：设xk为节点k，令zk和ωk为与xk有关的约束的均值和信息矩阵。目标函数定义为：
[0033]
f(x)＝∑ek(xk,zk)
t
ω
kek
(xk,zk)
[0034]
其中，ek(xk,zk)是参数xk和约束zk之间的误差函数。
[0035]
通过使用gauss-newton或levenberg-marquardt算法进行线性化和最小化。如果部分参数出现非欧几里得空间(如姿势参数)，算法可能会出现不可预料的结果或者陷入死循环中。因此在此基础上需要增加流形优化。流形是参数的最小表示并在本地充当欧几里得空间。算法将引入了一个算子它在流形上转换一个局部变化δx。通常，在3dslam中，节点xk的传感器姿态参数位于k(平移向量tk和四元数qk)。四元数的一个流形qk＝[qw,qx,qy,qz]t可以表示为[qx,qy,qz]t，算子描述为：(注q0*＝q*-δq*)
[0036][0037]
在本实施例中，我们首先在连续帧之间迭代应用ndt(正态分布变换)来扫描匹配和估计传感器(机器人)轨迹。对于3dlidar，ndt显示出比其他扫描匹配算法更好的性能，例如迭代最近点在可靠性和处理速度方面。设pt是t处的传感器位姿，由平移向量t和四元数q组成，rt,t+1是通过扫描匹配估计的t和t+1之间的相对传感器位姿。我们将它们作为节点[p0,
····
,pn]和边[r0,1,
···
,rn-1,n]添加到姿势图中。然后，我们在轨迹中找到循环并将它们作为边添加到图中(即循环闭合)，以纠正与算法匹配的扫描累积误差。同时，在环路检测算法中，我们根据平移距离和节点之间的轨迹长度检测循环候选。然后，为了验证循环候选者，在每个候选者的节点之间应用扫描匹配算法(ndt)。如果适应度分数低于阈值(例如，0.2)，我们将循环作为节点之间的边。每次找到循环时，都会更新位姿图。我们利用g2o，一个超图优化的通用框架，用于位姿图优化。
[0038]
实施例3
[0039]
如图1-2所示，在实施例1的基础上，本发明提供一种技术方案：优选的，采用迭代应用扫描匹配算法来估计传感器的自我运动。当帧之间的传感器运动变得非常大，并且由于大的位移，扫描匹配可能会错误地估计传感器的自我运动时，采用无迹卡尔曼滤波器将ndt扫描匹配与3d雷达提供的角速度数据相结合，传感器速度假设为：
[0040][0041]
其中，p是位置，q是旋转四元数，v是速度，bat是传感器在时间t的角速度偏差。假设传感器运动模型的平移速度恒定，角速度传感器的偏差恒定，预测状态的系统方程定义
为：
[0042][0043]
其中，δ是t和t-1之间的持续时间，δq是δt期间由偏置补偿角速度引起的旋转。
[0044][0045]
在本实施例中，系统通过使用unscented kalman滤波器预测传感器位姿，然后应用ndt将观察到的点云与全局地图进行匹配，并将估计的xt和qt作为传感器位姿的初始猜测。然后，系统利用扫描匹配z
t
＝[p
′
t
,q'
t
]
t
,估计的传感器姿态来校正传感器状态。我们在每个预测和校正步骤之后对状态向量中的四元数进行归一化，以防止其范数由于无迹变换和累积计算误差而改变。其中还实现了将加速度考虑在内的姿态预测。
[0046]
综上所述，通过机器人的位置以及雷达的fov(视野角)来计算机器人与各个障碍物的距离。在得到宽广的探测范围后，本发明算法将使用分割地面与障碍物后的数据对自身进行定位。同时，在机器人应用中，可以根据自身移动后数据的变化得出物体的大概高度(即是否允许机器人通过得高度，可用于三维自动导航算法)；本发明可以为机器人领域发展提供一个更为精准的三维定位算法，致力于更好的解决四足机器人在复杂地形的定位问题。解决了2d定位的局限性。
[0047]
下面具体说一下该基于四足机器人的3d点云定位的工作原理。
[0048]
如图1-2所示，首先，对机器人配备16线激光雷达，imu，以及作业地区的3d点云图；然后，在机器人作业前需要在图形界面标注机器人的初始位置或者在源码中给定初始位置的坐标；最后再给定初始位置后便可以自由移动机器人，并实时定位可以在图形界面中实时观察到机器人的相对位置。
[0049]
上文一般性的对本发明做了详尽的描述，但在本发明基础上，可以对之做一些修改或改进，这对于技术领域的一般技术人员是显而易见的。因此，在不脱离本发明思想精神的修改或改进，均在本发明的保护范围之内。