丘陵山地移动机械臂协同运动控制方法技术

本发明专利技术公开了一种丘陵山地移动机械臂协同运动控制方法，包括以下步骤：步骤一，设计局部路径规划器；步骤二，构建移动机械臂运动学模型；步骤三，设计状态估计器；步骤四，设计车臂协同控制器。本发明专利技术一种丘陵山地移动机械臂协同运动控制方法，该方法用于用材林立木修剪作业，在丘陵山地中的人工林环境中实现对修枝机的运载和移动。在得到某一林区的移动机器人全局规划路径的前提下，该方法首先可以针对工作区域内的障碍物进行局部路径规划，规划出移动底盘和机械臂的轨迹，协同控制底盘和机械臂跟踪各自的轨迹进行全自动作业。完成对修枝机的运载和移动任务，实现无人作业，大大提升了机器人的自动化程度。机器人的自动化程度。机器人的自动化程度。

【技术实现步骤摘要】
丘陵山地移动机械臂协同运动控制方法


[0001]本专利技术涉及林业修剪设备
，特别涉及一种丘陵山地移动机械臂协同运动控制方法。

技术介绍

[0002]近年来，随着农林业现代化水平不断提高，农林业的生产逐渐由劳动密集型转变为技术密集型，用于农林业的作业机器人得到较大的发展。然而我国很多林业领域尤其是生态建设领域还停留在半机械化水平，还未完全达到机械化水平。由于林区的环境复杂，移动式机器人尚未大量地应用，对于用材林的修剪作业还是以人工为主，工作效率低。
[0003]目前用材林的修剪作业主要依靠人工修剪或使用爬树修枝机进行修枝作业，我国对于用材林修枝机械装备的研究尚处于测试试验阶段，较少有可应用的机型。爬树修枝机缺点是需要作业人员进行人工辅助操作，由人工将修枝机放置于立木上遥控修枝机进行修枝作业。这种工作模式智能化、自动化程度有所欠缺。
[0004]公开于该
技术介绍
部分的信息仅仅旨在增加对本专利技术的总体背景的理解，而不应当被视为承认或以任何形式暗示该信息构成已为本领域一般技术人员所公知的现有技术。

技术实现思路

...

【技术保护点】

【技术特征摘要】
1.一种丘陵山地移动机械臂协同运动控制方法，其特征在于，包括以下步骤：步骤一，设计局部路径规划器：由全局参考路径和障碍物信息，建立障碍物模型，建立人工势场，分别对移动底盘和机械臂进行局部路径规划，最后将局部路径规划输出给车臂协同控制器；步骤二，构建移动机械臂运动学模型：由移动底盘和机械臂的运动变量构建移动机械臂的运动学模型；步骤三，设计状态估计器：采用扩展卡尔曼滤波器对底盘的侧向滑移和纵向滑移进行实时最优估计；步骤四，设计车臂协同控制器：车臂协同控制器接收步骤一的局部路径规划器输出的参考轨迹，用非线性模型预测控制方法对底盘和机械臂进行协同控制；将移动机械臂的运动学模型离散化，构建约束条件和目标函数；在每一控制周期内求解优化问题，将控制输入增量作用于移动机械臂，实现对移动底盘和机械臂的轨迹跟踪控制。2.根据权利要求1所述的丘陵山地移动机械臂协同运动控制方法，其特征在于，步骤一设计局部路径规划器的方法包括：(1)建立障碍物模型：由多线激光雷达检测障碍物，得到障碍物的点云，随后使用AABB包围盒技术对障碍物以不同形状的圆柱体进行包络，以此代替不规则障碍物进行局部路径规划；(2)设计避障函数：避障功能采用人工势场法实现，建立如下引力场函数：式中，k
a
为引力系数；d
goal
为控制点到目标点的距离；引力场分为引力场和变力场，d
L
为引力场的变力场半径，在变力场内引力与d
goal
相关；在恒力场内，引力大小不变；建立如下斥力场函数：式中，k
r
为引力系数；d
obs
为控制点到障碍点的距离；d
r
为斥力场半径。(3)局部路径规划。3.根据权利要求2所述的丘陵山地移动机械臂协同运动控制方法，其特征在于，步骤(3)局部路径规划的包括以下步骤：1)将全局参考路径等距离地取若干个点，作为局部路径规划的目标点集；2)对于移动底盘，以底盘运动中心的内切圆和外接圆半径为尺寸对障碍物模型进行膨胀处理，剔除处于障碍物模型内的目标点；随后采用梯度下降算法，从当前位置开始，沿着负梯度方向不断前进，直至达到梯度为零的位置，输出规划点序列；3)对于机械臂，以修枝机尺寸半径对障碍物模型进行膨胀处理，剔除处于障碍物模型内的目标点；先计算当前各关节的关节空间位姿，随后寻找合势能最小的一组关节角，判断
该关节角是否存在碰撞，若机械臂连杆存在碰撞则重新寻找合适的关节角，机械臂沿各关节合势能最小的方向移动，直至达到目标点的位置，输出规划点序列；4)若规划点陷入势场极值的状态而产生振荡时，在局部极值处添加随机扰动，以跳出局部极值。4.根据权利要求1所述的丘陵山地移动机械臂协同运动控制方法，其特征在于，步骤二构建移动机械臂运动学模型的方法包括：(1)移动底盘运动学建模：利用底盘转向系统和速度控制的延迟和动态特性，使用一阶滞后加时间延迟对底盘转向系统和速度控制器进行建模：式中s为一阶滞后的时间常数；L为时间延迟；以底盘后轴的中心作为其坐标系原点，则移动底盘的运动学模型如下：式中，(x,y)为底盘后轴中心坐标，为底盘的航向角，δ为前轮转向角，v为底盘后轴中心的速度，l为底盘前后轴的轴距，τ
v
、τ
α
分别为底盘速度和转向角的时间常数；(2)机械臂运动学建模：机械臂为六自由度机械臂，安装在移动底盘的中轴线上，六个关节均为回转关节，其关节变量为[θ1θ2θ3θ4θ5θ6]，采用D
‑
H参数法对其建模，机械臂末端在世界坐标系下的坐标P
tip
如下：x
tip
＝x+d6[c5s1+cos(θ2+θ3+θ4)c1s5]
‑
d4s1+l3c1cos(θ2+θ3)+l2c1c2+d5c1sin(θ2+θ3+θ4)+d
bb
y
tip
＝y+d4c1+l3s1cos(θ2+θ3)
‑
d6c1c5+l2c2s1+d5sin(θ2+θ3+θ4)s1+d6cos(θ2+θ3+θ4)s1s5z
tip
＝z+d1‑
d5[sin(θ2+θ3)s4‑
cos(θ2+θ3)c4]
‑
l3sin(θ2+θ3)
‑
l2s2‑
d6s5[cos(θ2+θ3)s4+sin(θ2+θ3)c4]+h
bb
式中，c
i
、s
i
分别为cosθ
i
和sinθ
i
的缩写；l
i
为机械臂第i个连杆的长度；d
i
为连杆偏移；(x,y,z)为底盘后轴中心坐标，d
bb
机械臂底座与底盘后轴的距离，h
bb
机械臂底座与底盘后轴的高度差；计算机械臂末端速度V
tip
如下：5.根据权利要求1所述的丘陵山地移动机械臂协同运动控制方法，其特征在于，步骤三设计状态估计器包括用移动机械臂的传感系统获取数据，结合加速度计的测量信号，采用
扩展卡尔曼滤波器对底盘状态进行实时最优估计，同...

【专利技术属性】
技术研发人员：蒙丽雯，陈世锋，韦锦，蒙艳玫，朱纪洪，陈贵，陈泉成，董振，周志标，毛星宇，
申请(专利权)人：广西大学，
类型：发明
国别省市：