一种高空作业机械臂实时环境感知与自主定位方法

本发明涉及一种高空作业机械臂实时环境感知与自主定位方法。

背景技术：

1、高空作业平台主要由地面移动平台、回转变幅机构及平台框架组成，平台框架能够被举升，使得工作人员能够在平台框架中进行高空作业。高空作业机械臂是在高空作业平台基础上改进而来，其是将平台框架调整为机械臂以替代人工作业。

2、高空作业机械臂在高空环境下作业需要感知实时的三维信息，slam技术可以进行三维建图，但是slam三维建图依赖于3d激光雷达，而3d激光雷达价格高昂、设备笨重，不便于搭载在体型较小的高空作业机械臂上。目前，高空作业机械臂主要由人工引导作业，无法实现自主的环境感知。具体来说，是采用多传感器融合、三维扫描仪等方式预先进行工作场景的全景扫描以获取先验地图，然后将高空作业机械臂置于先验地图中进行作业。预先建图工作量较大，且需要借助人工完成，效率低下，影响高空作业机械臂的环境适应性、灵活性。

技术实现思路

1、发明目的：本发明的目的是提供一种高空作业机械臂实时环境感知与自主定位方法，能够实现自主的环境感知，实现边作业、边建图。

2、技术方案：本发明所述的高空作业机械臂实时环境感知与自主定位方法，包括：

3、(1)初始地图建立

4、在地面移动平台上设置2d激光雷达，在机械臂上设置3d超声波传感器；

5、地面移动平台移动到指定工作地点，机械臂和回转变幅机构保持固定姿势，通过2d激光雷达获取2d点云，将2d点云传入2d slam系统中，通过粒子滤波方法，得到地面移动平台中心的位置，该位置信息传入八叉树数据结构中；通过2d网格建图得到2d地图；

6、通过3d超声波传感器获取3d点云，对3d点云进行预处理，将球坐标系转化为笛卡尔坐标系，再通过机械臂逆解的方法获取3d超声波传感器与2d激光雷达的相对距离，将此位移量融入3d点云中，获得3d点云与2d激光雷达的相对位置坐标；

7、2d点云通过逆传感器模型和贝叶斯滤波处理成2d占据概率地图，3d点云通过逆传感器模型和贝叶斯滤波处理成3d占据概率地图，2d、3d占据概率地图存入八叉树数据结构；

8、通过八叉树数据结构建图，得到融合的3d地图，将八叉树数据结构中3d占据概率地图投影到二维平面上，经过数据融合得到融合的2d地图；

9、(2)工作过程中地图更新

10、超声波周期计时模块记录3d超声波传感器的发射周期，用于后续误差处理；

11、初始地图建立完成后，机械臂开始抬升或下降，进行高空定位；3d超声波传感器从一个周期初时开始扫描周围物体，同时回转变幅机构与机械臂开始运动；

12、3d超声波传感器在一个周期末时，将采集到的三维点云传入三维坐标叠加模块中；回转变幅机构与机械臂的位置在周期初与周期末存在相对位移与角度变化，相对坐标逆解模块通过机械臂位姿逆解方法，得到3d超声波传感器与2d激光雷达的位置变化量，然后将机械臂的位移变化量与角度变化量传入误差缩减模块中；

13、一个周期内的相对位置变化逆解得到后，超声波周期计时模块自增一个周期，开始下一个周期的位置变化逆解与三维点云获取；同时，误差缩减模块通过获取采集各点云的时间，按与一个周期之间位移与角度变化的比例关系，并假设极短周期内的机械臂位移与旋转为匀速，得到各个点云假设下的相对位移与角度变化量，然后将其传入三维坐标叠加模块，三维坐标叠加模块将位移变化量增加到三维点云中，然后根据角度变化量旋转三维点云，使得到的新的三维点云的方向与初始定位获得的三维地图方向一致；

14、将新的三维点云传入八叉树数据结构进行建图，在初始获得的3d地图上拓展更广阔的3d地图，由此得到一个3d超声波传感器周期内的三维地图；重复上述的过程，实现工作过程中的地图更新。

15、进一步地，融合3d地图的生成策略为：

16、先设定融合时地图单元状态改变的阈值，阈值取值范围为0～1，单元状态有占据、空闲、未知三种，再将2d和3d地图在新地图上同一位置的单元格或体素的两个概率值通过数学运算，得到新地图上这一位置体素的融合概率，若该体素的概率达到阈值，则改变体素状态，以生成新地图上不同单元的不同状态，最终得到的新地图就是融合的3d地图。

17、进一步地，单元占据阈值设为0.7，空闲阈值设为0.4，单元概率p≥0.7是视为占据状态，p≤0.4时视为空闲状态，0.4＜p＜0.7时视为未知状态；p1设为3d地图体素的概率值，p2为2d地图单元格的概率值，设p为p1和p2数学运算后的值；p1≥0.7，无论p2为多少，p＝p1；当p1＜0.4时，则p＝p1+p2；当0.4≤p1＜0.7时，则p＝p2。

18、进一步地，融合2d地图的生成策略为：与融合3d地图的生成策略类似地，先设定融合时地图单元状态改变的阈值，再将高度上距离2d地图较近的3d地图体素投影到一个新的2d地图中，新的2d地图和激光雷达生成的2d地图中同一位置的单元格的两个概率值通过数学运算，将得到的融合概率结果覆盖新的2d地图中单元格的概率，若单元格概率达到阈值，则改变单元格状态，将所有单元格融合完成后，得到的新地图就是融合的2d地图。

19、进一步地，2d点云处理成2d占据概率地图的过程包括：

20、p(mi∣sj)定义为逆传感器模型的一个单元格的占据概率值，mi表示第i个单元格，sj表示第j个传感器测量数据；

21、扇形区域为传感器一次测量的面积，包括白色区域a1和灰色区域a2，扇形角度为δα，j表示传感器一次测量中获得的一个具体点，位于灰色区域a2中心，到灰色区域a2弧形边的距离为l，j点与原点o的连线位于扇形的角平分线上；αj是极坐标系下j点与原点o的连线与极轴的夹角；rj是极坐标系下j点距离原点o的距离；

22、点i为某个单元格中心点，距离原点o的距离为di，βi是极坐标系下i点与原点o的连线与极轴的夹角；

23、在灰色区域a2计算点i单元格的占据概率，di大于白色区域a1半径：

24、先计算距离概率or和角度概率oα：

25、

26、

27、其中kr与kα取值为0～1；

28、然后，根据p(mi∣sj)＝oroα，求得灰色区域a2中单元格i的占据概率，距离点j越近，概率值越大；

29、当点i为白色区域a1中的单元格时，di小于白色区域a1半径：

30、先计算距离概率er和角度概率eα：

31、

32、

33、然后，根据p(mi|sj)＝1-ereα，求得点i为白色区域a1中的单元格时的占据概率。

34、进一步地，当p(mi∣sj)超出占据概率阈值或低于空闲概率阈值时，更新单元格占据概率：

35、将得到的逆传感器模型值p(mi∣sj)代入下面的贝叶斯滤波式，通过迭代计算推测该单元格在下一个数据的概率值：

36、

37、li,n表示第n个数据下的i单元格占据概率的对数奇数表示，li,0＝0；

38、将得到的li,n的值带入下式，用来更新单元格占据概率，得到第i个栅格单元在第n个数据下的占据概率：

39、

40、当p(mi|s1,...,sn)值大于等于占据概率阈值时，将该单元格视为物体存在，单元格的占据概率重设为占据概率阈值，小于等于空闲概率阈值时，将该单元格视为没有物体，单元格的占据概率重设为空闲概率阈值。

41、进一步地，再将各个栅格单元的被占概率相乘，得到局部栅格地图占用概率：

42、

43、m表示组成大单元格的单元格个数。

44、进一步地，3d点云处理成3d占据概率地图的过程包括：

45、采用相同的方法计算三维单元格的占据概率，将3d点云划分为立体单元格，然后在逆传感器模型取值公式中增加一个高度参数，即p(mi∣sj)＝oroαoh与p(mi|sj)＝1-ereαeh，之后计算方法与2d单元格占据概率计算一致，最后立体单元格的占据概率保持原有计算结果，即最终结果即使超出占据概率阈值或低于空闲概率阈值，也不用占据概率阈值与空闲概率阈值覆盖最终结果。

46、进一步地，3d超声波传感器从第i个周期初时记为tia，3d超声波传感器在第i个周期末时记为tib；δxi、δyi、δzi、δαi、δβi、δγi为tia时刻到tib时刻一个周期内3d超声波传感器的位移与角度相对位置变化，其中δxi为沿x轴的位移变化量，δαi为绕x轴的角度变化量，δyi为沿y轴的位移变化量，δβi为绕y轴的角度变化量，δzi为沿z轴的位移变化量，δγi为绕z轴的角度变化量。

47、进一步地，误差缩减模块通过三维点云，获得体素与3d超声波传感器坐标系原点的距离l，依据同一介质中超声波波速不变原理，得到速度v，通过2l与v的比值得到超声波击中该体素现实中的物体并返回到传感器的时间t＝2l/v，t＜t；

48、由于在tia到tib用时为t，超声波传感器相对上一个位置运动距离为δxi，假设极短周期内的机械臂运动为匀速，则在t秒内，实际运动距离δxi为：

49、

50、经误差缩减模块处理后的点云实际相对位移即δxi，比一个周期末tib时刻的相对位移要小；依据比例计算出其他五个相对变量的值，然后将所有的三维点云进行误差缩减处理。

51、有益效果：本发明与现有技术相比，具有如下显著优点：

52、本发明基于2d激光雷达和3d超声波传感器获取点云信息，然后将2d点云和3d点云转化为概率地图并进行融合获取初始地图，机械臂在建图过程中即已实现自主定位(在环境中的位置)；再进行工作过程中地图的更新和误差缩减；能够实现高空作业机械臂自主的环境感知，且具备较高的精度，能够实现边作业、边建图，不再需要预先建立全景地图，因此效率更高，环境适应性、灵活性更好。