基于伪距离的混凝土泵车臂架避障运动控制研究

黄毅　胡勇　范佳源　胡明　汤麒英　熊英涛

摘要：针对混凝土泵车臂架末端避障和臂架关节限制问题，提出一种基于伪距离的泵车臂架避障方法。运用超二次曲面函数表示空间障碍物，采用伪距离作为臂架与空间障碍物的接近程度指标。基于最小伪距离判别指标，对泵车臂架赋予一个避障速度，并对传统梯度投影算法进行改进，在避免关节运动超过关节极限位置的情况下实现泵车臂架的安全避障，同时保证避障过程的平稳性和臂架末端的轨迹精度。仿真及半物理实验验证了该算法在混凝土泵车臂架避障过程中的有效性，且能完成空间多障碍物避障。

关键词：梯度投影法；关节避障极限；伪距离；避障；冗余度臂架；混凝土泵车

中图分类号：TH6

DOI：10.3969/j.issn.1004-132X.2023.12.012

Study on Obstacle Avoidance Control of Concrete Pump Truck Booms Based on Pseudo-distance Method

HUANG Yi HU Yong FAN Jiayuan HU Ming TANG Qiying Xiong Yingtao

Abstract： Aiming at the problems of end obstacle avoidance and joint limitation of concrete pump truck boom， an obstacle avoidance method of pump truck boom was proposed based on pseudo-distance. Superquadratic function was used to represent space obstacle， and pseudo-distance was used as the index of proximity between boom and space obstacle. Based on the minimum pseudo-distance discriminant index， an obstacle avoidance speed was assigned to the boom of the pump truck， and the traditional gradient projection algorithm was improved to realize the safe obstacle avoidance of the boom of the pump truck under the condition of avoiding the joint movement exceeding the joint limit position， while ensuring the stability of the obstacle avoidance processes and the trajectory accuracy of the boom end. Simulation and semi-physical experiments verified the effectiveness of the algorithm in concrete pump truck boom obstacle avoidance processes， which may complete the space multi-obstacle avoidances.

Key words： gradient projection method; joint avoidance limit; pseudo-distance; obstacle avoidance; redundant boom； concrete pump truck

0 引言

混凝土泵车是一种通过泵车臂架将混凝土连续均匀地输送到浇筑点的专用工程机械。随着对工程机械的要求越来越智能化、无人化，人们不但要求混凝土泵车臂架能够在施工现场实现自动浇筑，还要求能够对遇到的障碍物实现自动安全避障［1］。泵车臂架可以看作是一种由液压驱动的超长冗余度机械臂，冗余度機械臂利用其冗余度的特性可在不影响末端执行任务的前提下完成其他任务，在奇异处理、避障、关节避障极限、提高可操作性等方面具有显著优势［2-4］。

避障是冗余度机械臂研究的一个重要内容，冗余度机械臂避障方法主要有基于优化准则的梯度投影法和人工势场法。此外，根据应用场景的不同避障方法分为离线方法和在线方法［5-6］，离线避障方法是对周围环境提前进行建模，当周围环境改变时可能导致避障失败，因此应用更多的是在线避障方法，根据传感器获取的信息实时规划运动路径实现在线避障。PERDEREAU等［7］提出了一种冗余度机械臂实时避碰方案，该方案将冗余作为一个无约束优化问题，冗余与路径跟踪和避障约束集成到一个增强目标函数中最终实现避障。YOSHIDA等［8］提出了一种基于迭代规划方案的复杂环境下三维无碰撞运动生成的实用规划框架。BENZAOUI等［9］利用扩展雅可比矩阵实现了平面三连杆机械臂避障控制。GUAN等［10］提出了一种适用于多移动障碍物的避障算法，基于广义逆方法将逃逸速度引入其中，得到机械臂的关节速度，从而完成避障轨迹规划。HU等［11］利用后向二次搜索算法，结合阻尼最小二乘法、加权最小范数法和梯度投影法，对可能发生的从末端到基座的障碍物碰撞进行向后搜索，并成功实现避障。DE等［12］基于人工势场法中的斥力场提出了Pivot算法，将排斥力作用于机械臂末端，使机械臂末端能够避开动态障碍物。随着机械臂的自由度和障碍物数目的增加，获取机械臂与障碍物之间的最小欧氏距离变得越来越困难，因此，众多学者对检测障碍物到机械臂之间的距离检测也进行了研究。另外，对障碍物进行建模是实时避障的关键，为了减小计算量和提高效率，通常使用简单的几何单元来表示障碍物。CHEN等［13］对障碍物和冗余机器人连杆用最接近其体积的椭球建模，基于两者之间的最小距离采用虚拟力进行避障。WANG等［14］基于人工势场方法，将机器人避障简化成线段与球之间的避障。ZHAO等［15］提出将超冗余度机械臂和工作空间中的障碍物作为边界椭圆进行建模，实现了超冗余度机械臂的灵活避障。CHOI等［16］分别用球体和椭球代表障碍物和机械臂，将机械臂与障碍物之间的碰撞距离定义为碰撞度量指标，通过最小化碰撞度量指标来实现避障任务。HWANG等［17］也将工作空间中的障碍物建模为椭球体以简化数学表示，并减小碰撞检测的计算复杂度。PATEL等［18］利用对偶向量和角度的概念提出了一种紧凑的圆柱体-圆柱体碰撞检测和距离计算方法。然而，上述这些方法对障碍物与机械臂之间的最小距离很难获得精确的解析式。BRICE等［19］运用超曲面对障碍物进行建模，提出了一种冗余度机械臂自适应控制方案。MU等［20-21］将障碍物表面采用超二次曲面函数表示，采用伪距离来评估机械臂与障碍物之间的接近程度，并提出了一种伪距离归一化处理方法，成功完成了冗余度机械臂的避障任务。

本文针对混凝土泵车臂架末端的避障问题，同时考虑泵车臂架结构的关节极限限制，结合伪距离概念，提出一种基于伪距离的改进梯度投影避障算法。对障碍物表面进行超二次函数表示，采用具有解析式的伪距离来评估臂架与障碍物的接近程度，并对传统梯度投影法冗余度臂架避障方法进行改进，完成对臂架避障过程中各臂节关节转角在关节极限位置的限定。基于最小伪距离判别指标对泵车臂架赋予一个避障速度，实现对空间障碍物的避障。以中联重科38 m混凝土泵车为研究对象，开展混凝土泵车臂架避障运动控制的仿真和半物理实验，验证该方法在臂架避障过程中的可行性。

1 泵车臂架避障模型建立及伪距离计算

1.1 泵车臂架系统运动学分析

混凝土泵车臂架系统是一种冗余自由度结构系统，五节臂混凝土泵车臂架系统是一个六自由度冗余系统，将泵车臂架系统简化为一个刚性结构系统，便可以运用机器人学理论进行运动学分析。采用D-H法对泵车臂架系统建立正向运动学方程，其臂架系统简图见图1，D-H参数如表1所示，坐标系定义为：（X0，Y0，Z0）为基坐标系，（Xi，Yi，Zi）为转台和各臂架坐标系，X轴方向指向臂架离开的方向。

运用D-H法可求得臂架末端轨迹方程和雅可比矩阵：

2 基于伪距离的泵车臂架避障算法建模

2.1 关节避障极限

由于泵车臂架结构、液压等因素限制，泵车每节臂架都有运动范围，存在关节运动极限，无法直接引用传统梯度投影法。针对这一情况，对传统梯度投影法冗余度机械臂避障方法进行改进，实现对各臂节关节转角极限的限定。

3 仿真分析与半物理仿真实验验证

3.1 仿真分析

为验证本文的基于伪距离的混凝土泵车臂架三维避障算法的有效性，以中联重科38 m混凝土泵车臂架为研究对象，进行本避障算法仿真，仿真环境为MATLAB R2021a，验证臂架避障运动控制过程中臂架转角的极限情况和避障效果。

混凝土泵车臂架结构参数分别为： d2=0.77 m，d5=0.48 m，l1=7.136 m， l2=5.479 m， l3=5.21 m， l4=5.466 m， l5=5.32 m。转台及各臂架转动角度范围为：θ1∈［－π，π］，θ2∈［0，π/2］，θ3∈［－π，0］，θ4∈［－π，0］，θ5∈［－13π/36，π］，θ6∈［－π，π/18］。转台及各臂架初始位姿角度为：θ=（0，4π/9，－π/3，－π/6，－π/12，－π/5），对应末端臂架位置为（19.051，-0.48，1.803） m。

设定障碍物R=0.5 m、H=2 m，障碍物中心为（16.5，-0.48，1） m的障碍物，臂架拟合圆柱体半径r=0.25 m，臂架末端以0.25 m/s的水平速度向左运动，运动中臂架会检测到障碍物，实时计算末节臂架与障碍物的伪距离，并根据计算出来的伪距离与设定的伪距离阈值Darm进行比较并执行自动避障动作。伪距离阈值的设定影响到避障时臂架与障碍物之间留下的安全裕度空间，初步将伪距离阈值设定为Darm=2 m，最大避障速度vm=5 m/s，闭环反馈参数Ke=diag（10，10，10），仿真时间为20 s。

图3、图4所示为未加避障算法时泵车臂架与障碍物之间的最小伪距离和泵车臂架运动姿态轨迹过程。从图3中可以看出，未加避障算法时，从t=7.21 s开始臂架与障碍物之间的最小伪距离值开始小于0，到t=13.21 s后臂架与障碍物之间的最小伪距离值开始大于0；同时从图4中可以看到，t∈［7.21，13.21］ s时泵车臂架与障碍物发生碰撞。以上说明未加避障算法时，臂架与障碍物发生碰撞，无法自动躲避障碍物，同时也可说明泵车臂架与障碍物之间的最小伪距离指标能够反映出泵车臂架与障碍物之间的接近关系。

使用未加关节避障极限方法时，虽然也实现了避障的目标，但是在泵车臂架运动大约10.09 s左右时一臂角度位移超出了其角度位移的最大限定范围（θ1∈［0，π/2］），无法满足实际工程条件。而在使用本文避障算法后，如图6b所示，可以很明显地看出泵车臂架各臂节转角很好地限制在限定范围内。同时可以看到相较未加关节避障极限方法，使用本文避障算法时，泵车臂架一臂转角变动范围更小一些，而末节臂转角变动范围更大一些，使得一臂运动得更少些，运动更多的是转动末端几节臂，这也更加符合实际泵车正常工作时尽量少动一臂的要求，使泵车臂架运动得更加平稳。

由图5c可知，在使用未加关节避障极限方法时，由于未对泵车臂架关节进行限制，泵车臂架一臂运动范围较大，一臂关节角速度变化幅值也更大，最大幅值差达到0.06 rad/s，这样会造成泵车整体臂架运动更加抖动、不平稳。而使用本文避障算法时，如图6c所示，由于对泵车臂架关节进行了限制，泵车臂架一臂运动范围更小，一臂关节角加速度变化幅值变小，最大幅值差降为0.02 rad/s。将较大的运动放在末节臂，且由于末节臂相较于其他几节臂尺寸更小、质量更小、转动惯量更小，虽然末节臂的关节角速度幅值变大了，但是泵车臂架整体运行得却更加平稳顺滑。

图6d所示为使用本文避障算法的泵车臂架与障碍物之间的最小伪距离变化曲线。在初始阶段，泵车臂架与障碍物之间的最小伪距离值大于设定的伪距离阈值Darm，泵车臂架保持水平运动。当t=5.03 s时泵车臂架与障碍物之间的最小伪距离值开始小于设定的伪距离阈值Darm，此时给泵车臂架添加了一个避障速度，如图6e所示，泵车臂架运动进入到避障任务阶段。由于避障速度的加入，使得t∈［5.03，15.08］ s泵车臂架与障碍物之间的最小伪距离值都保持在伪距离阈值Darm左右并大于零，从而保证泵车臂架一直处在一个安全状态不与障碍物发生碰撞并留有足够的安全裕度空间。到t=15.08 s后，泵车臂架与障碍物之间的最小伪距离值开始大于设定的伪距离阈值Darm，避障速度为零，泵车臂架避障阶段结束，开始保持原来的水平运动。由图6f可知，在t∈［0，5.03］ s阶段，泵车臂架末端误差几乎为零，泵车臂架处于水平运动阶段，在t∈［5.03，16.58］ s阶段，泵车臂架处于避障阶段，为了远離障碍物从而出现了较大的轨迹误差，但在t=15.08 s后泵车臂架结束避障运动，由于闭环控制的存在，泵车臂架开始回归规定的轨迹并在t=16.58 s后回到规定轨迹，泵车臂架末端误差接近于零，保持了很好的目标轨迹跟随。

本文算法還可适用于解决多个障碍物同时存在的避障问题，将仿真条件设置为障碍物1和障碍物2大小相同（R=0.25 m， H=2 m），障碍物相同平面分布时障碍物中心坐标分别为（16.5，-0.48，1）m、（14.5，-0.48，1）m，障碍物不同平面分布时障碍物中心坐标分别为（16.5，0，1）m、（14.5，-0.48，1）m，仿真时间设定为30 s，其他仿真条件与之前仿真条件一样，仿真结果如图7所示。由仿真结果可以看出本文避障算法能够实现多障碍物同时存在时的三维避障问题。

3.2 半物理仿真实验验证

为进一步验证在考虑混凝土泵车实际物理结构下本文避障算法的有效性，进行半物理仿真实验。半物理仿真实验装置如图8 所示，物理部分由混凝土泵车臂架遥控器、遥控器信号接收器、USBCAN信号分析仪等组成，虚拟部分是在MATLAB R2021a＼＼Simulink中搭建的混凝土泵车Simulink物理模型。使用实际的混凝土泵车遥控器对泵车臂架末端进行速度和方向控制，观察记录泵车臂架运动及避障情况，开展半实物仿真实验。

混凝土泵车Simulink物理模型中的泵车臂架结构参数、初始臂架位姿角度、初始臂架位置和障碍物位置及大小设置与前文3.1节中相同。伪距离阈值设定为Darm=2 m，最大避障速度vm=5 m/s，闭环反馈参数Ke=diag（10 000，10 000，10 000），采样频率为10 Hz，仿真时间为20 s。物理硬件之间通信参数波特率为250 kb/s，臂架末端速度方向和大小由遥控器上万向比例推杆控制，速度最大值为0.25 m/s。

图9所示为在Simulink仿真环境中混凝土泵车臂架避障的运动过程，可以看出，在t=12 s左右时，臂架末端跨过障碍物最高点，整个过程臂架末端未与障碍物发生接触，表明本避障算法能够有效地实现泵车臂架避障。

图10所示为泵车臂架半物理仿真实验结果，可以看出，从t=0.7 s输入开始，泵车臂架即开始水平运动，到t=2.4 s达到最大速度0.25 m/s，并从t=17.7 s开始减速直到t=19 s时减到零，泵车臂架停止水平运动。由图10b可以看出，在t∈［6.6，16.8］ s时，臂架末端X方向和Z方向的避障速度不为零，这是因为从t=6.6 s开始泵车臂架与障碍物之间的最小伪距离小于阈值Darm，泵车臂架进入避障运动，到t=16.8 s时最小伪距离开始大于阈值Darm，泵车臂架结束避障运动，如图10f所示。t∈［6.6，16.8］ s的整个过程中，最小伪距离都大于零，说明泵车臂架不与障碍物发生接触，有效地实现了安全避障。

同时由图10b可以看到，在整个避障过程中Z方向的避障速度恒大于零，而在X方向避障速度先大于零后小于零，这是因为在Z方向，臂架需要跨过障碍物，所以Z方向避障速度需要一直大于零。而在X方向臂架末端先是不断向障碍物靠近，当t=12 s左右时经过障碍物最高点后，臂架末端逐渐远离障碍物，所以X方向的避障速度先是大于零，阻碍臂架靠近障碍物，然后再小于零，让臂架更快远离障碍物，这样保证在整个避障过程中避障速度总是起到躲避障碍物的作用，从而达到避障的目的。

由图10c、图10d 可以看出，在整个实验过程中，泵车臂架关节角度和角速度变化都保持着连续、平稳，各关节角度都限制在关节极限内，并且第一节臂架的关节角度和关节角速度都变化得很小，这有利于泵车臂架整体的稳定性。在t=6.6 s，由于激活了泵车臂架的避障运动，出现了一个较大的角速度变化，但仍然保持着连续性，并未发生严重突变。从图10e中可以看出，在t=6.6 s之前，臂架末端保持着很高的位置精度，在t=6.6 s后，泵车臂架末端位置误差开始加大，并且在t=12 s时Z方向上位置误差达到最大值，这是因为从t=6.6 s开始泵车臂架进行避障，需要牺牲位置精度去进行避障运动，并在t=12 s到达障碍物的最高点处，此时位置误差达到最大，在t=16.8 s后结束避障运动，泵车臂架执行水平轨迹跟随运动，臂架各方向位置误差接近于零，保证了避障结束后能够保持较高的精度。

综上可以看出，半物理仿真实验验证了本文避障算法能够有效地实现泵车臂架避障，并且能够有效地将各关节角度限制在关节极限内，整个避障过程平稳、连续，同时保证了在避障完成后回到原来轨迹上并保持较小的位置误差。

4 结论

（1）本文提出了一种基于伪距离的混凝土泵车臂架避障方法，采用超二次曲面函数描述空间障碍物，将伪距离作为评价泵车臂架与障碍物接近程度的指标，有效地避免了欧氏距离的复杂运算，提高了计算效率和实时性，成功实现了泵车臂架的避障。

（2）结合工程实际，考虑到混凝土泵车臂架结构的关节限制，在实现泵车臂架避障的同时保证了各臂架关节极限的限制和运动的平稳性。

（3）通过仿真和半物理实验，验证了本文提出算法的有效性，同时该算法适用于多空间障碍物避障。

参考文献：

［1］KRAMER M， ROSMANN C， JOHN F. Time-optimal Unfolding of a Mobile Truck-mounted Concrete Pump under Constraints［J］. Forschung im Ingenieurwesen-Engineering Research， 2018， 82（1）：45-57.

［2］BOTTIN M， ROSATI G. Trajectory Optimization of a Redundant Serial Robot Using Cartesian via Points and Kinematic Decoupling［J］. Robotics， 2019， 8（4）：101.

［3］MENASRI R， NAKI A， DA A B， et al. A Trajectory Planning of Redundant Manipulators Based on Bilevel Optimization［J］. Applied Mathematics & Computation， 2015， 250：934-947.

［4］WOOLFREY J， LU W， LIU D. A Control Method for Joint Torque Minimization of Redundant Manipulators Handling Large External Forces［J］. Journal of Intelligent and Robotic Systems， 2019， 96（1）：3-16.

［5］PATEL R V， SHADPEY F， RANJBARAN F， et al. A Collision Avoidance Scheme for Redundant Manipulators：Theory and Experiments［J］. Journal of Robotic Systems， 2010， 22（12）：737-757.

［6］DAI Y， LEE S G. Formation Control of Mobile Robots with Obstacle Avoidance Based on GOACM Using Onboard Sensors［J］. International Journal of Control Automation & Systems， 2014， 12（5）：1077-1089.

［7］PERDEREAU V， PASSI C， DROUIN M. Real-time Control of Redundant Robotic Manipulators for Mobile Obstacle Avoidance［J］. Robotics & Autonomous Systems， 2002， 41（1）：41-59.

［8］YOSHIDA E， ESTEVES C， BELOUSOV I， et al. Planning 3-D Collision-free Dynamic Robotic Motion through Iterative Reshaping［J］. IEEE Transactions on Robotics， 2008， 24（5）：1186-1198.

［9］BENZAOUI M， CHEKIREB H， TADJINE M. Redundant Robot Manipulator Control with Obstacle Avoidance Using Extended Jacobian Method［C］∥ 18th Mediterranean Conference on Control and Automation.Marrakech：IEEE， 2010：11474038.

［10］GUAN X Q， LIANG G H， NIU K. A Redundant DOFs Manipulator Motion Obstacle Avoidance Algorithm［C］∥International Conference on Automation， Mechanical and Electrical Engineering. Phuket， 2015：242-250.

［11］HU Tianjian， WANG Tianshu， LI Junfeng， et al. Obstacle Avoidance for Redundant Manipulators Utilizing a Backward Quadratic Search Algorithm［J］. International Journal of Advanced Robotic Systems， 2016， 13（119）：353-341.

［12］DE L， AIESSANDRO， KHATIB， et al. A Depth Space Approach for Evaluating Distance to Objects with Application to Human-Robot Collision Avoidance［J］. Journal of Intelligent & Robotic Systems Theory & Application， 2015， 80：7-22.

［13］CHEN Y J， JU M Y， HWANG K S. A Virtual Torque Based Approach to Kinematic Control of Redundant Manipulators［J］. IEEE Transactions on Industrial Electronics， 2016， 64（2）：2334829.

［14］WANG Wenrui， GU Jinlin， ZHU Mingchao. et al. An Obstacle Avoidance Method for Redundant Manipulators Based on Artificial Potential Field［C］∥2018 IEEE International Conference on Mechatronics and Automation（ICMA）. Changchun：IEEE， 2018， 2151-2156.

［15］ZHAO Liangliang， JIANG Zainan， SUN Yongjun， et al. Collision-free Kinematics for Hyper-redundant Manipulators in Dynamic Scenes Using Optimal Velocity Obstacles［J］. International Journal of Advanced Robotic Systems， 2021， 18（1）：185-190.

［16］CHOI S I， KIM B K. Obstacle Avoidance Control for Redundant Manipulators Using Collidability Measure［J］. Robotica， 2000， 18（2）：143-151.

［17］HWANG K S， Ju M Y. 3D Collision-free Motion Based on Collision Index［J］. Journal of Intelligent and Robotic Systems， 2002， 33（1）：45-60.

［18］PATEL R V， SHADPEY F， RANJBARAN F， et al. A Collision-avoidance Scheme for Redundant Manipulators Theory and Experiments［J］. Journal of Robotic Systems， 2005， 22（12）：737-757.

［19］BRICE L B， MAAROUF S， VAHE N. Modeling and Adaptive Control of Redundant Robots［J］. Mathematics and Computers in Simulation， 2006， 71（4/6）：395-403.

［20］MU Zonggao， XU Wenfu， LIANG Bin. Avoidance of Multiple Moving Obstacles during Active Debris Removal Using a Redundant Space Manipulator［J］. International Journal of Control Automation and Systems， 2017， 15（2）：1-12.

［21］牟宗高. 面向狹小空间作业的超冗余机器人轨迹规划及控制研究［D］. 哈尔滨：哈尔滨工业大学，2017.

MU Zonggao. Research on Trajectory Planning and Control of Hyper-redundant Robots for Confined-space Operation［D］. Harbin：Harbin Institute of Technology， 2017.

（编辑 王艳丽）

作者简介：

黄 毅，男，1978年生，博士、教授。研究方向为工程机械动力学及控制。 E-mail：tjhuangyi110@163.com。

收稿日期：2022-09-01

基金项目：国家自然科学基金（51875048）