林木联合采育机自主作业的虚拟仿真系统1)

葛桃桃 郑一力 刘晋浩

(北京林业大学，北京，100083)

林木联合采育机自主作业的虚拟仿真系统1)

葛桃桃 郑一力 刘晋浩

(北京林业大学，北京，100083)

针对林木联合采育机自主作业路径规划与控制的研究需求，采用SolidWorks软件对林木联合采育机进行了三维实体建模，将模型导入到图形开发引擎OpenSceneGraph(OSG)，结合Visual C++编程语言，完成了一套林木联合采育机作业三维虚拟仿真系统。阐述了该虚拟仿真系统的架构、功能以及系统的开发流程。在对机械臂运动学建模的基础上，给出了伐木头直线、圆弧和激光反馈的路径规划和控制方法。在三维虚拟仿真系统中通过采伐虚拟立木实验，验证了自主作业路径规划与控制方法的有效性，该研究为实现林木联合采育机的无人自主采伐作业打下基础。

林木联合采育机；自主作业；虚拟仿真

For the path planning and control simulation for the autonomous task of a logging harvester, we built three-dimensional solid models of all parts of the logging harvester via SolidWorks. Introducing these models to the OpenSceneGraph (OSG) by Visual C++ programming language, we performed the virtual simulation system. We introduced the system architecture of the virtual simulation system, together with its function, and system process. Kinematics modeling of the hydraulic boom was obtained for the control purpose. The straight, circular and the laser measurement feedback path planning and control methods were given for the harvesting head to capture trunks. The felling trunks experiments in the virtual simulation system showed the validity of the proposed path planning and control method.

林木联合采育机是一种高性能现代林业装备，可大大提高林木采伐的效率和安全性，但将伐木头对准和捕获目标立木的环节，由于视线被挡以及车底盘和机械臂的振动，需要操作员做出大量的观测判断和反复手动对准，造成不必要的时间和燃料，有必要对机械臂的自主作业路径规划与控制方法进行研究。

针对林木联合采育机这种大型和复杂机电系统，对其自主作业进行研究的前期，由于存在危险性和费用高等问题，必须要对关键控制技术进行虚拟仿真验证[1]。

在农林机械虚拟仿真方面，芬兰Mevea公司[2]和美国JohnDeere公司[3]开发的林业训练模拟机，对林木联合采育机和集材机的液压装置、机身、变速器以及伐木环境等精确的仿真，可以完成采伐、打枝、去皮、造材、集材等连续作业，并通过显示屏从不同的视角观察伐木点周围环境；U. Mettin等[4]针对集材机液压臂的路径规划与控制问题，综合考虑了液压臂的动作时间、关节速度、最大角度和动力学等约束，通过OSG对机械臂进行三维路径规划，提出了时间运动分解的运动控制算法，实现了集材机液压臂抓取原木的自主规划和控制。贺继林等[5]针对液压挖掘机机械臂的轨迹控制，将基于SOPC的轨迹控制器与基于OpenGL的仿真软件共同组成虚拟仿真环境，使液压挖掘机的机械臂模型按照设定轨迹运动，达到了良好控制效果；赵满全等[6]、张红彦等[7]应用SolidWorks和ADAMS对双圆盘割草机进行仿真和动力学分析，研究其运动轨迹，计算重割率；苏明明[8]采用SolidWorks和Creator建立了挖掘机的三维模型和虚拟场景，用Lynx Prime对仿真系统进行配置，在VC++平台上调用Vega Prime实现交互功能，用键盘控制挖掘机的挖掘动作。

三维实体建模软件SolidWorks具有仿真效果逼真、实时交互性好等诸多优点，图形开发引擎OSG具有可扩展性、可移植性和快速开发性。本文选用SolidWorks为三维建模软件，在Visual C++开发环境中调用OSG进行虚拟仿真系统的研发，对机械臂的自主作业路径规划与控制方法进行研究。

1 仿真系统的建立

虚拟仿真系统包含林木联合采育机、林地背景和目标立木的虚拟模型和数学模型，可输入控制参数和指令，并实时显示和保存运动信息，进行林木联合采育机自主作业路径规划与控制的参数化仿真与同步动态虚拟显示。

1.1 仿真系统的架构

针对于林木联合采育机的虚拟仿真任务需求，本系统分为任务管理、数据管理、路径规划和界面显示4个模块，系统架构见图1。

图1 系统架构

任务管理模块：完成对林木采育作业任务的管理，以及任务运行过程中对数据管理模块和路径规划模块的协调与同步。

数据管理模块：管理路径规划数据和状态数据，实现数据共享和同步更新。林木联合采育机的运动控制需要处理大量的路径规划数据和状态数据，且存在多个模块处理相同数据的情况，故将整个系统所需要处理的数据分离出来形成一个独立的模块。

路径规划模块：调用相关路径规划库函数，根据作业任务需求完成林木联合采育机作业的直线、圆弧和自主路径规划与控制算法计算，并更新数据管理模块中的相关内容。

界面显示模块：由仿真模型显示模块和信息更新模块两部分构成。仿真模型显示模块可实时显示林木联合采育机的运动及其操作环境的仿真模型。信息更新模块将数据管理模块和路径规划模块传输的数据进行实时更新和实现，有数据变动时，发送更新信号，视图类窗口即会读取对应的数据进行更新，确保仿真图像的实时性。

1.2 林木联合采育机三维实体建模

采用SolidWorks软件，以CFJ30型林木联合采育机为原型，根据其真实尺寸，对各零件进行三维实体建模，如图2所示。

1.后推油缸;2.底座;3.主油缸;4.立柱;5.副油缸;6.连杆;7.大臂;8.小臂;9.伸缩臂。

图2 林木联合采育机结构示意图

林木联合采育机的模型由轮式底盘、伐木头和多自由度机械臂组成。轮式底盘为所有部件提供支撑，为车辆行驶、机械臂和伐木头的运动提供动力；伐木头安装在机械臂的末端，用来完成伐木、打枝、去皮、造材等一系列工序；机械臂是林木联合采育机的主要执行机构，决定了其作业能力和范围，主要由立柱、连杆、大臂、小臂、伸缩臂等组成。后推油缸伸缩控制底座小角度摆动，最大倾角为20°，底座的回转机构使机械臂可相对于底座做150°转动，大臂和小臂通过主油缸和副油缸的伸缩进行变幅和升降运动，伸缩臂从小臂中伸出，小臂内部有伸缩油缸控制伸缩臂的伸出量[9]，本文系统中设定伸缩臂的最大伸长量为2 m。

机械臂各机构之间通过旋转关节和滑动关节连接，共有5个自由度：立柱俯仰旋转和朝向旋转，大臂俯仰旋转，小臂屈伸和伸缩臂伸缩。伐木头通过销轴安装在机械臂末端，呈悬挂状态。

1.3 三维场景建立

林木联合采育机作业在三维场景中完成，以增强虚拟仿真的沉浸性。本文三维场景主要包括林木联合采育机本体、林地背景图和目标立木。林地背景图和目标立木通过贴图导入到OSG中，将SolidWorks建立的林木联合采育机实体模型，通过stl格式导入到OSG中，完成各虚拟零件的组装，并添加纹理、颜色。完成林木联合采育机虚拟仿真系统界面(见图3)。本系统中目标立木的方位数据来源于项目先前在白杨树林中做的激光测量实验[10]。

图3 虚拟仿真系统界面

本系统在旋转轴位置建立了Denavit-Hartenberg(DH)坐标系[11](如图4)，以坐标轴为参考，通过一系列数据的指令驱动各坐标系使实体产生运动。同时，林木联合采育机组装过程中，使DH坐标系与零件本身位姿坐标系重合，可以精确快速的完成组装。

2 运动学建模分析

运动学建模分析是对林木联合采育机机械臂的自主作业路径规划和控制方法进行研究的基础。运动学正解是机械臂关节空间到末端操作空间的映射，在已知机械臂各关节角时，可求得机械臂末端相对于基坐标系的位姿；运动学逆解是位姿空间向关节空间的转换，通过液压缸运动可以实现对机械臂末端执行器位姿的控制。

图4 机械臂的DH坐标系

2.1 坐标系建立

林木联合采育机的机械臂可看成是具有多个关节的空间结构，为了描述机械臂末端执行器在空间的位置，利用坐标系之间的关系来描述机械臂末端执行器的位置。林木联合采育机的机械臂有4个旋转关节和一个平动关节。每一个关节对应一个坐标系{i}(i=1，2，…，5)，其对应的DH参数见表1。

表1 机械臂的DH参数

注：表中“-”表示机械臂逆时针转动。

2.2 运动学正解

则机械臂末端的位姿为：

(1)

2.3 运动学逆解

(2)

(2)式中的雅克比矩阵J不能直接变换，用5×6的广义逆矩阵J+=JT(JJT)-1表示[13]。机械臂的运动学逆解为：

(3)

通过(3)式可以实时计算出各关节转动的角速度。

3 路径规划与控制方法

机械臂的路径规划是指给定机械臂的初始位姿及机械手末端的目标位姿，根据作业任务的要求，计算出预期的运动轨迹[14]。本文主要研究3种路径规划与控制方法：直线路径规划、圆弧路径规划和基于激光反馈的自主路径规划。

3.1 直线路径规划

机械臂末端按直线运动规划时，机械臂末端执行器的线速度和角速度都采用按照带抛物线过度圆弧梯形法规划。设起点和终点时刻为0和tz，加速段(或减速段)时间为ta，匀速段时间为ts，加速段和减速段曲线的过渡点时刻为ta/3、2ta/3、tz-2ta/3和tz-ta/3，在过渡点时刻ta/3或(tz-ta/3)、2ta/3或(tz-2ta/3)处的速度为v1和v2，最大速度为vm，终点对应的位移为df，其位移-时间关系如图5所示。

a=ta/3,b=2ta/3,c=ta,d=ta+ts,e=tz-2ta/3,f=tz-ta/3,g=tz。

3.2 圆弧路径规划

林木联合采育机的圆弧路径规划是将圆弧路径当做空间圆弧曲线处理。空间圆弧规划一般分两步处理，第一步把三维问题转化为二维问题，即在圆弧平面内规划，第二步利用二维平面插补算法，求出插补点坐标，然后再把这个点坐标值转变成基坐标系下的值。同样采用按照带抛物线过度圆弧梯形法的线性插值方法，与直线路径规划不同的是，这里规划的是末端执行器沿圆周的旋转角度和角速度。

空间圆弧规划坐标系如图6所示。已知点p1、p2、p3，设机械臂末端执行器沿着从始点p1将中间点p2到终点p3的圆弧运动[15]。

图6 空间圆弧规划坐标系

3.3 基于激光反馈的自主路径规划

自主控制时，激光测量系统参与运动控制，激光测量系统实时计算出目标立木相对伐木头的位姿，进行路径规划与控制。

设基坐标系为I(OI-XIYIZI)，机械臂末端坐标系为T(OT-XTYTZT)，目标点坐标系为W(OW-XWYWZW)，自主路径规划的坐标系如图7所示。具体步骤如下：

1)确定跟踪目标点W相对于机械臂末端T和基坐标系I的当前位姿；

2)考虑到直接跟踪目标物，可能会发生机械臂末端和目标物碰撞的问题，因此先以与目标物固连的坐标系的X轴上距离目标物ds(设ds=0.1 m)的点作为跟踪中间点pW；

3)计算pW的位姿坐标，然后以pW为目标物，计算pW相对于机械臂末端T在基坐标系I下的位姿坐标差D，并计算机械臂末端与pW之间的距离dv；

以实际目标物W为跟踪对象进行精确跟踪。其规划跟踪方法是重复前4个步骤。

图7 自主路径规划坐标系

3.4 路径规划的虚拟仿真流程

4 实验验证

本系统采用林木联合采育机伐木头连续捕捉并采伐目标立木的实验，验证自主作业路径规划与控制方法的有效性。

图8 程序的架构简图

在图9(a～h)中，白线代表伐木头的运动轨迹。图9(a)为车体先运行到一个通过机械臂可以捕获目标立木的位置；图9(b)伐木头从A点到B点进行自主路径规划，并在B点将伐木头对准捕获目标立木；图9(c)伐倒目标立木；图9(d)将目标立木沿直线路径B点拖至C点进行打枝、造材；接着如图9(e～f)伐木头通过圆弧轨迹到达靠近下一个目标立木的D点；在图9(g)中，从D点到E点是自主路径规划，并将伐木头对准捕获下一棵目标立木；图9(h)将目标立木伐倒。

表2 各关节角的运动范围约束

注：表中“-”表示关节角逆时针运动。

图9 实时动态仿真系统

将图9实验过程的数据导出并分析，在此过程中，俯仰液压缸保持静止，即关节1没有发生变化，角速度为0，关节2～5位移的数据随时间变化如图10～13。

图中纵坐标负值表示转动装置逆时针运动。

图中纵坐标负值表示转动装置逆时针运动。

在图10～13中，时间0-7.9 s是基于激光反馈的自主路径规划方法捕获目标立木过程，对应图9(b)；时间7.9-11.1 s，各关节位移没有变化，对应图9(c)，伐木头伐倒目标立木；时间11.1-18.7 s对应图9(d)，将目标立木从B点拖至C点；时间18.7-29.2 s是伐木头进行打枝、造材作业；时间29.2-42.2 s对应图9(e～f)，伐木头运动到靠近下一个目标立木的D点；时间42.2-49.7 s伐木头捕获下一个目标立木，对应图9(g)；第49.7 s开始，再次对该目标立木伐倒，对应图9(h)。

图中纵坐标负值表示机械装置逆时针转动。

图13 关节5的位移

实验过程中，关节2的位移-33.8°～0°，最大速度为8.5°/s(<20°/s)；关节3的位移-23.0°～0°，最大速度为3.6°/s(<15°/s)；关节4的位移-6.1°～29.8°，最大速度为8.0°/s(<15°/s)；关节5的位移0～1.16 m，最大速度为0.15 m/s(<0.2 m/s)。各关节运动范围没有超出约束设置，最大速度没有超过设定的最大速度，且运动曲线平滑，实验结果显示本文的自主作业路径规划和控制方法有效。

5 结束语

本文采用三维实体建模软件SolidWorks和图形开发引擎OSG，结合VC++编程语言，建立了林木联合采育机参数化可视化虚拟仿真系统。完成了对林木联合采育机三维实体建模，构建了包括林木联合采育机模型及其周围环境的仿真平台，并对林木联合采育机的机械臂建立了运动学模型，对其进行直线、圆弧、自主路径规划与控制。同时对机械臂作业路径规划进行仿真实验，实现了仿真数据的实时传输和图形界面的实时显示，验证了自主作业路径规划与控制方法的有效性。

林木联合采育机无人自主采伐作业的研究可进一步提高其作业效率和安全性，同时采用虚拟仿真技术可节省实验成本，保证实验安全，也可用于林木联合采育机驾驶员的培训。

[1] 臧宇，朱忠祥，宋正河，等.农业装备虚拟试验系统平台的建立[J].农业机械学报，2010，41(9)：70-74.

[2] LAPOINTE J F, ROBERT J M. Using VR for efficient training of forestry machine operators[J]. Education and Information Technologies,2000,5(4):237-250.

[3] BAHARUDIN M E, ROUVINEN A, KORKEALAAKSO P, et al. Real-time multibody application for tree harvester truck simulator[J]. Proceedings of the Institution of Mechanical Engineers Part K Journal of Multi-body Dynamics,2014,228(2):182-198.

[4] METTIN U, LA HERA P X, MORALES D O, et al. Path-constrained trajectory planning and time-independent motion control: Application to a forestry crane[C]//ICAR. 2009 14th International Conference on Advanced Robotics (ICAR) Proceedings, Munich: Germany IEEE,2009.

[5] 贺继林，刘鹏飞，彭灿，等.液压挖掘机轨迹控制策略的半实物仿真研究[J].华中科技大学学报(自然科学版)，2008，36(S1)：19-22.

[6] 赵满全，张宁，杨铁军，等.双圆盘割草机切割器虚拟样机设计与试验[J].农业机械学报，2014，45(8)：101-105.

[7] 张红彦，于长志，赵丁选，等.基于虚拟现实的液压挖掘机视景仿真[J].华中科技大学学报(自然科学版)，2013，41(3)：87-91.

[8] 苏明明.基于Multigen Creator/Vega prime的液压挖掘机的可视化虚拟仿真[D].成都：西南交通大学，2011.

[9] 郭秀丽.采伐联合机机械手运动分析与控制系统研究[D].哈尔滨：东北林业大学，2011.

[10] ZHENG Y, LIU J, WANG D, et al. Laser scanning measurements on trees for logging harvesting operations[J]. Sensors-Basel,2012,12(7):9273-9285.

[11] DENAVIT J, HARTENBERG R S. A kinematic notation for lower-pair mechanisms based on matrices[J]. Trans of the ASME Journal of Applied Mechanics,1955,22(2):215-221.

[12] CRAIG J J. Introduction to robotics[M]. Beijing: China Machine Press,Pearson/Prentice Hall,2005.

[13] ZHENG YILI, GE TAOTAO, LIU JINHAO. Kinematics modeling and control simulation for a logging harvester in virtual environments[J]. Advances in Mechanical Engineering,2015,7(10):1-10.

[14] 熊有伦，丁汉，刘恩沧.机器人学[M].北京：机械农业出版社，1993.

[15] 任秉银，梁兆东，孔民秀.机械手空间圆弧位姿轨迹规划算法的实现[J].哈尔滨工业大学学报，2012，44(7)：27-31.

Autonomous Task of A Logging Harvester in Virtual Simulation System//

Ge Taotao, Zheng Yili, Liu Jinhao

(Beijing Forestry University, Beijing 100083, P. R. China)//Journal of Northeast Forestry University，2017,45(1)：71-76.

Logging harvester； Autonomous task； Virtual simulation

1)国家自然科学基金项目(31300596)。

葛桃桃，女，1990年12月生，北京林业大学工学院，硕士研究生。E-mail：getaotao@bjfu.edu.cn。

郑一力，北京林业大学工学院，副教授。E-mail：zhengyili@bjfu.edu.cn。

2016年8月2日。

S78

责任编辑：王广建。