面向动态环境的机械臂避障方法_刘银冬.pdf

107工 艺 与 装 备面向动态环境的机械臂避障方法刘银冬王家海*（同济大学 机械与能源工程学院，上海 201804）摘要：针对动态环境避障存在的问题，提出一种轨迹实时规划方法。首先，通过视觉传感器实时监控作业环境，获得动态障碍物的位置信息后，计算得到障碍物的三维坐标。其次，采用卡尔曼滤波处理三维坐标数据，得到当前位置的最优估计值及下一位置的预测值。再次，结合人工势场法，在机械臂末端形成由一个引力和两个斥力组成的人工势场，得到机械臂末端的运动位置。最后，在笛卡尔空间规划轨迹，在 CoppeliaSim 中建立仿真平台，仿真结果验证了该方法的有效性。关键词：动态环境；机械臂；动态避障Obstacle Avoidance Method of Mechanical Arm for Dynamic EnvironmentLIU Yindong,WANG Jiahai*(School of Mechanical and Energy Engineering,Tongji University,Shanghai 201804)Abstract:Aiming at the problem of obstacle avoidance in dynamic environment,a real-time trajectory planning method is proposed.Firstly,the visual sensor is used to monitor the operating environment in real time,obtain the position information of dynamic obstacles,and calculate the three-dimensional coordinates of obstacles.Secondly,Kalman filter is used to process the three-dimensional coordinate data to obtain the optimal estimate of the current position and the predicted value of the next position.Thirdly,combined with the artificial potential field method,an artificial potential field composed of one gravitational force and two repulsive forces was formed at the end of the manipulator,and the moving position of the end of the manipulator was obtained.Finally,the trajectory is planned in Cartesian space,and the simulation platform is established in CoppeliaSim.The simulation results verify the effectiveness of the proposed method.Keywords:dynamic environment;mechanical arm;dynamic obstacle avoidance1主要算法介绍在工业 4.0 的大背景下，机械臂产业及相关技术应用成为现代化生产制造的重要支撑，机械臂也用于更加复杂的生产环境。机械臂需要在动态障碍物中运动，并使用来自环境噪声干扰的传感器。因此，为了能够在环境中安全作业，机械臂的轨迹规划必须同时考虑传感器的不确定性和运动的不稳定性。目前，针对机械臂避障路径规划问题，主要有以下 3 类算法。第一，Stentz 基于启发式算法 A*设计了 D*算法，引导机器人向目标方向移动，减少无用节点的扩展，节省了搜索时间1。该类算法广泛应用于二维路径规划，随着维数的增加，规划时间也会呈指数延长。第二，随机采样方法在高维空间的路径规划上有着广泛的应用，有研究者针对静态障碍物以 RRT*生成初始路径，然后以局部偏置优化路径最终得到全局最优路径。Rose 等针对二维动态障碍物在 RRT*算法的基础上，通过定义机械臂的位置、航向、速度与时间，提出新的实时规划算法 Real-time RRT*，将所有操作的持续时间设置为某个恒定的毫秒数来离散时间，减少解决问题所需的状态和动作空间2。随机采样的方法虽然能够在高维系统得到最优解，但在动态环境中需要重复采样，最优解得出的速度较慢。第三，人工势场法是局部规划中较为典型的一种，谢龙等提出一种改进的人工势场法，将目标点的速度信息引入函数，避免出现目标附近吸引力为 0 的情况3。人工势场法只针对局部障碍物，忽视全局的其他环境信息，得到的路径与最优路径存在偏差。对于动态时变的障碍物，钟碧良通过机器人与障碍物相遇速度的几何变化计算可能出现的避障速度，实现对规划路径上潜在障碍物位置的预测4。何炎祥等采用神经网络训练的方式模拟障碍物的行为，结合人工势场法不断进行滚动在线规划5。有研究者通过在机械臂与障碍物平台上比较 4 种障碍物的预测模型，表明对障碍物进行运动预测能够显著提高工业作业环境的安全性。基金项目：同济西门子校企合作项目“数控机床生产过程的碳排放建模和预测”（kh0100020184510）。*通信作者：王家海。DOI:10.16107/ki.mmte.2023.0160现代制造技术与装备1082023 年第 3 期总第 316 期针对动态环境下对障碍物位置识别的准确性与避障的实时性要求较高的问题，采用卡尔曼滤波方法结合传感器的观测值预测动态障碍物的下一帧轨迹。根据预测的结果，通过局部规划人工势场法实时规划一条远离动态障碍物的路线，使得生产环境始终处于安全状态。2动态环境下的障碍物处理2.1卡尔曼滤波原理在实际生产环境中，障碍物观测数据的获取会受到系统中噪声的干扰影响，而卡尔曼滤波可以利用目标的动态信息排除噪声干扰，得到一个当前位置的最优估计。同时，卡尔曼滤波在预测时只需知道上一状态预测结果的误差协方差矩阵即可，因此计算速度快，适用于动态环境避障。结合卡尔曼滤波的特性，可以估计障碍物的当前位置并预测未来位置。通过建立运动预测方程和观测方程，滤波实现过程如下。k时刻预测状态估计矩阵为xk=Axk-1+Buk-1（1）式中：A为状态转移矩阵；B为控制矩阵；uk-1为状态控制向量。k时刻误差协方差矩阵为Pk=AP(k-1)AT+Q（2）式中：Q为预测模型中处理噪声矩阵。k时刻卡尔曼增益为Kk=PkHT(HPkHT+R)-1（3）式中：R为测量噪声矩阵；H为状态变量到观测的转换矩阵。k时刻状态变量的最优估计值为xk=xk+Kk(zk-Hxk)（4）式中：zk为观测矩阵。更新后的误差协方差矩阵为Pk=(I-KkH)Pk（5）式中：I为主对角线元素为 1 且其他位置元素为0 时的单位矩阵。结合k-1时刻预测的k时刻状态值与最优估计值，通过不断更新迭代使得下一时刻的预测值更加准确。2.2障碍物轨迹预测模型本次所应用的系统主要由视觉传感器、工业 7 自由度机械臂以及障碍物组成，为验证本文所述方法对于动态环境下机械臂避障的有效性，假设障碍物为匀加速运动。在 Vrep 环境中进行系统仿真，通过视觉传感器实时检测障碍物的红-绿-蓝（Red-Green-Blue，RGB）图像和深度图像，利用公式将它们转化成以视觉传感器为基准的障碍物位置pobs和速度obs，再将障碍物的信息作为观测数据输入卡尔曼滤波。通过卡尔曼滤波的处理，对障碍物进行状态估计。基于卡尔曼滤波原理的状态空间方程，结合障碍物实际运动情况，设定状态空间方程的初始化参数，如表 1 所示。表 1卡尔曼滤波状态空间方程初始化参数参数含义参数值初始状态量XX=-1.050,-0.375,1.450T状态转移矩阵A 210.501001|=|tttA控制矩阵BB=zeros(9,9)运动状态噪声协方差矩阵QQ=diagones(1,9)测量噪声矩阵RR=2.5107diag(1,1,1)状态变量到观测的转换矩阵H100 000 000=000 100 000000 000 100|H采用卡尔曼滤波器进行障碍物k(Xk,Yk,Zk)的位置估计和k+1(Xk+1,Yk+1,Zk+1)的位置预测，障碍物运动时的真实值轨迹、观测值轨迹、最优估计轨迹以及预测轨迹，如图 1 所示。从图 1 中可以看出：在不断迭代下，经过滤波后的最优估计和预测值都较接近真实值；并且在环境噪声比较大导致观测数据和真实值偏差较大时，估计值和预测值向真实值靠近，能够减少干扰，为下一步采用人工势场法进行避障提供了比较准确的数据。图 1卡尔曼滤波后的预测误差曲线3基于障碍物轨迹预测的机械臂避障在存在动态障碍物的环境下，机械臂需要实时感109工 艺 与 装 备知障碍物的运动状态变化，确保环境干扰下数据的准确性。通过将卡尔曼滤波与人工势场法结合，减少环境噪声带来的干扰，也能达到预测避障的目的，保证动态环境下机械臂避障的鲁棒性与高效性。3.1引力和斥力人工势场法会分别在障碍物和目标点周围构建斥力场和引力场，被控制对象同时受到两个势场作用的斥力和引力，在合力作用下局部规划出一条无碰撞路径。在人工势场应用的局部规划中，建立机械臂末端运动的引力势场Uatt和斥力势场Urep的位置函数分别为()2att12fUqqq=（6）式中：|q-qf|为机械臂末端初始位置到目标位置的相对距离；为势场的比例系数。()()()()20rep00111,20,qUqqq|=|（7）式中：为势场的比例系数；0为障碍物的影响距离；(q)为机械臂末端到障碍物的距离。通过对势场函数进行微分得到引力Fatt和斥力Frep的计算公式，分别为Fatt(q)=-(q-qf)（8）()()()()rep20111Fqqqq=|（9）式中：()q为从障碍物指向机械臂末端的单位向量。3.2合力在动态避障环境中，为了保证机械臂避开障碍物的实时性与鲁棒性，将障碍物的当前最优估计位置与预测位置都作为影响机械臂末端运动的斥力场，如图 2 所示，得到当前斥力Fcrep和预测斥力Fnrep。机械臂根据斥力与引力建立人工势场后，使用梯度下降法求解机械臂末端的轨迹。从初始位置出发，沿着势场的负梯度方向从势场较高的地方移动到势场较低的安全地点，在得到机械臂末端的下一个运动位置后，重复使用梯度下降法，直至到达目标位置。引力与斥力根据障碍影响距离分条件进行计算，每次通过一个引力与两个斥力的作用，为因障碍物位置获取的误差与噪声造成的机械臂避障不准确以及人工势场法计算的复杂性提供位置修正与时间缓冲，充分保证作业环境的安全性。图 2人工势场法的合力将所有引力与斥力叠加作用在机械臂末端，得到合力Fsum(q)的计算公式为Fsum(q)=Fatt(q)+Fcrep(q)+Fnrep(q)（10）在得到合力并计算出机械臂末端下一个运动位置后，需要对机械臂进行轨迹规划。机械臂轨迹规划的方法可以分为关节空间规划和笛卡尔空间规划两类6。关节空间规划通过确定各个关节转角的变化序列，确定机械臂末端的空间运动轨迹，这种方法不能精确地控制机械臂末端的位姿变化，也无法较好地解决机械臂末端的避障问题。因此，采用笛卡尔空间直线规划，通过定时插补算出t时间间隔内的距离d以及插补数N，即可得到相邻插补点各轴增量与各轴插补点坐标值(Xi+1,Yi+1,Zi+1)，从而完成对机械臂末端的轨迹规划。笛卡尔坐标系下两点坐标(xf,yf,zf)和(x0,y0,z0)之间的距离表达式为()()()222000=+fffLxxyyzz（11）t时间间隔内的距离为 dt=（12）式中：为沿直线的距离。插补数N的表达式为 1=+LNd（13）式中：L为总距离；d为间隔距离。相邻插补