摘 要：为解决生活垃圾清理中人力资源成本高、效率低的问题，提出一种垃圾分拣智能车设计方案。该研究对机械臂抓取效率和精度进行提升，优化机械臂的运动学求解算法和结构设计；为解决智能车因轮子打滑导致的定位偏差，通过实测位移数据，使用MATLAB计算拟合曲线四次多项式进行优化；为处理陀螺仪零漂的问题，利用卡尔曼滤波算法进行优化。通过综合改进，使智能车机械臂和车身位姿控制更精准可靠。结果表明，设计的垃圾分拣智能车能获得准确可靠的位姿信息，从而高效完成分拣任务。
关键词：正逆运动学；最小二乘法；优化分析；卡尔曼滤波器
中图分类号：TP241；TP273 文献标识码：A 文章编号：2096-4706（2024）08-0069-06
DOI：10.19850/j.cnki.2096-4706.2024.08.016
收稿日期：2023-08-16
基金项目：2022年安徽理工大学大学生创新创业训练计划项目（2022210361067）
0 引 言
根据《2020年全国大、中城市固体废物污染环境防治年报》数据，2019年全国近200个大、中城市的生活垃圾产生量达23 560万吨，其中，上海市、北京市生活垃圾产量均超过1 000万吨[1]。为积极实施乡村振兴战略[2]，以数字化手段赋予乡村生活垃圾分类处理和回收利用是推进乡村精细化治理、优化人居环境的基本要求，也是促进广大居民养成绿色低碳的生活方式以及建设无废乡村、智能乡村、绿色乡村[3]的重要内容。
以第十七届全国大学生智能汽车竞赛智能视觉组项目为背景，在此项目的基础上进行创新改进，设计出一种基于竞赛用的垃圾分拣智能车，主要通过在智能车车体前加装多自由度机械臂，实现对垃圾的抓取；并通过深度学习进行模型训练，实现对不同种类垃圾的识别与分类。本文重点介绍在机械臂控制和智能车位姿解算上的优化设计。
1 系统组成
本垃圾分拣智能车主要包括三大系统，即识别系统、决策控制系统、动力驱动系统。图像识别采用Openart摄像头；决策控制系统采用恩智浦的MIMX RT1064芯片作为主控芯片，用于处理工作环境信息，做出决策并产生控制信号；动力驱动系统包括电机、舵机以及驱动模块，驱动模块接受控制信号并控制车模运行。在车模运行过程中，通过摄像头实现对不同种类垃圾的识别与分类，主控芯片接收摄像头传来的识别信息，产生控制信号，控制机械臂进行抓取动作，并控制车模移动到对应区域，最终实现垃圾分拣任务。智能车整体系统组成如图1所示。
通过对上述系统组件进行详细界定和构建，设计出了一种智能垃圾分拣车的初步方案。其整体组装示意图如图2所示。总钻风摄像头和Openart摄像头分别固定在具体车底盘中心27.5 cm和7 cm的位置，并位于车身中线位置上。固定支架整体采用碳纤维材质，并在固定支架前端加装补光灯，旨在优化摄像头的成像效果，确保目标对象清晰可见。此外，在智能车尾部加装放置加重块，以抵消机械臂自身质量对车辆前倾的影响，避免后轮滑动和空转等问题。用于连接机械臂与智能车底盘的关节电机采用立式放置，相比于卧式放置，立式布置不仅节省车底空间，还能防止因机械臂增加致使车辆进一步前倾而可能产生的影响。为确保智能车机动性与灵活性，车轮使用麦克纳姆轮，以实现全方位移动。
2 发现问题与分析
按照上述方案对智能车进行设计，经过多次测试，发现智能车对工作场地的摩擦系数要求较高。通过测量不同条件下几种常见工作地面的摩擦系数[4]，得到表1，其中，K1表示干燥环境下的摩擦系数，K2表示潮湿环境下的摩擦系数。
上述摩擦系数的波动区间收到不同品牌的瓷砖和木质地板所采用的材质不同以及水泥和柏油马路的路面处理方法、污垢程度等多种因素影响，最终导致测量结果的变化。
通过在不同摩擦系数的路面进行测试，发现智能车在运行过程中，车轮普遍存在打滑问题，导致定位存在一定偏差。同时，智能车在移动过程中由于陀螺仪存在零漂[5]等问题，随着时间增长，智能车位姿将会产生较大偏差，最终导致无法准确定位，进而无法高效完成垃圾分拣任务。并且在实际测试中发现，机械臂抓取的效率和精度较低，尽管从摄像头检测出了垃圾位置，但在运动学计算方面的耗时较大，无法高效准确完成抓取动作。
针对上述问题，将采取以下措施来进行优化改进。首先，在进行拓扑优化时尝试设置不同的参数，同时对机械臂进行运动学仿真分析。此外，还将对智能车位姿数据进行滤波处理，以获取更加准确的数据。改进方案的具体步骤如图3所示。
3 优化与仿真结果
3.1 机械臂逆运动学设计与优化分析
在初始设计中，智能车采用了二自由度机械臂来实现垃圾抓取功能。为了使机械臂准确高效地完成任务，需要对各个关节进行逆运动学求解[6]。初始设计机械简图如图4所示。
初始方案采用几何求解法对机械臂进行逆运动学求解，并通过建立的二位坐标系可知：
（1）
（2）
（3）
通过上述角度关系计算出如下关系式：
（4）
（5）
（6）
由上述（4）（5）（6）式可知，在平面坐标系中的带入到（1）（2）（3）式中整理得：
（7）
但通过实际测试发现，智能车进行垃圾分拣中，在寻找适合夹起垃圾的合适位姿时，需要不断调整智能车车身位姿从而完成抓取，这导致分拣效率低下。因此采用改进的三自由度机械臂进行抓取[7]，其机械简图如图5所示。
在进行逆运动学求解时，考虑到代数法的通用性强、可处理多解问题、可实时求解等优点，同时考虑到该项目所存在的特点：需要实时求解，并对响应速度有一定要求。因此采用代数法对机械臂位姿进行求解，经仿真研究表明，代数法最终效果较好。
通过在机械臂各个连杆建立对应的坐标系，每个关节的变换矩阵为[8]：
通过上述公式可得到总变换矩阵：
（8）
解得最后结果：
（9）
随后，根据机械臂模型的初始位姿和期望位姿建立标准D-H模型[9]，其位姿数据如表2、表3所示。通过仿真模拟机械臂抓取，并实时分析机械臂连杆处于不同位姿时每个关节的位置、速度、加速度以及关节力矩的图像。
根据表2和表3的D-H参数表，建立得到仿真模型如图6、7所示。
将图6作为机械臂抓取位姿，图7作为机械臂抓取垃圾的终止位姿，通过仿真实时计算每个关节的位置、速度、加速度以及关节力矩等变量，确保机械结构的可行性，防止因机械臂过载导致关节电机烧毁、连杆断裂损坏等情况的出现。其计算分析如图8所示。
其中，q1～q5分别表示关节1～5，并为了使图像更具有普适性，在仿真模拟计算的过程中，又加入了关节5与关节6，以方便后续优化改进。但在上述论述中，并未涉及关节5、关节6相关内容，因此图8中的q5、q6相关部分可忽略。
由图8可知，当前机械臂的结构可以有效避免关节电机因力臂过长而导致电机烧毁的现象[10]，并且通过采用运动学代数法来求解，机械臂抓取阶段的响应速度有明显的改进。
3.2 智能车全向移动优化分析
智能车在运行中麦克纳姆轮会产生打滑的现象，从而导致智能车定位不准，进而导致无法准确抓取目标垃圾[11，12]，为此进行如下改进优化。
首先以车身重心为原点，以车轮方向为Y轴，以垂直于车身方向为X轴建立二维直角坐标系。垃圾相对于智能车的坐标点可通过摄像头实时检测到横纵坐标点X、Y以及角度矢量θ。其建立的坐标系以及角度矢量θ如图9所示。
然后在坐标系X轴以及Y轴方向上，分别测量实际行驶距离与期望行驶距离因轮子打滑所造成的误差。
在X轴方向上测量15组数据，数据内容包括：实际行驶距离DRX，期望行驶距离DEX，通过大量反复实际测量，得到如表4所示的原始数据。
曲线拟合的最优标准是采用最小二乘法原理[13]，所构造的g（x）是一个次数小于插值节点个数的多项式。设测得n个离散数据点（xi，yi）（i=1，2，3，…，n）欲构造一个m（m≤n）次多项式p（x）：
（10）
所谓曲线拟合的最小二乘法原理，就是使上述拟合多项式在各节点的偏差p（xi） = yi的平方和 达到最小值。且在数学上已经证明，上述最小二乘法逼近问题的解是确定的。
通过上述表3的数据采用4次多项式进行曲线拟合，得到拟合曲线如图10所示。
通过使用MATLAB进行计算可得到拟合曲线4次多项式为：
（11）
同样，测量在Y轴方向上测量15组数据，数据内容包括：实际行驶距离DRY，期望行驶距离DEY，通过大量反复实际测量，得到如表5所示的原始数据。
同理使用上述最小二乘法进行拟合，拟合出曲线如图11所示。
通过图11可得到拟合出的4次多项式：
（12）
通过上述在X轴方向、Y轴方向上得到拟合曲线（11）（12）推广到整个二维平面坐标系，得到最终方程式为：
（13）
其中，sθ为sinθ的缩写，cθ为cosθ的缩写，方程式中的乘号全部被省略。
主控系统通过将相应的数据代入得到的方程组（13），求得的最终结果可以有效解决因麦克纳姆轮打滑而导致的车身定位存在较大偏差的问题。
3.3 偏航角算法优化设计
为解决智能车在运行过程中，存在剧烈抖动进而影响车身位姿和机械臂抓取精度的问题。通过结合卡尔曼滤波器算法对陀螺仪传感器得到的数据进行处理，进而可以有效控制智能车自身位姿[14]。卡尔曼滤波器是一种经典的最优化递归数字处理算法，其方程主要包括：
预测方程：
（14）
校正方程：
（15）
其中， 为先验误差协方差，Pk为更新后的误差协方差， 为先验估计值，Xk为后验估计值，H为转换矩阵，Kk为卡尔曼增益（Kalman Gain），R为测量噪声的协方差，Q为过程噪声的协方差。
通过陀螺仪测量的位姿数据，采用卡尔曼滤波器进行处理，仿真结果如图12所示。可知将位姿传感器得到的位姿信息变得更加平滑，可有效解决位姿传感器数据零漂的问题[15]，将降低车身的抖动程度，进而使机械臂抓取更加高效精准。
4 结 论
结合第十七届全国大学生智能汽车竞赛智能视觉组项目，创新设计提出一种智能垃圾分拣车。首先通过实地测试发现问题。针对智能车的机械臂部分，通过进行逆运动学优化分析与对机械结构的优化工作，实现机械臂在分拣过程中进行更加精确和可靠的运动；对全向移动车轮打滑导致的偏差问题进行数据处理和优化，以确保智能车在复杂环境中能够准确定位；对偏航角算法进行了优化改进，从而进一步提高智能车惯导的准确性。通过上述一系列优化方案，成功实现了对智能车机械臂和车身位姿进行更加精准可靠的控制。
参考文献：
[1] 陈瑀琼.城市生活垃圾治理博弈分析 [D].北京：首都经济贸易大学，2020.
[2] 曹索贝.D市农村生活垃圾治理环境绩效审计评价研究 [D].兰州：兰州财经大学，2023.
[3] SANG Z Z，LIU Z H. Feasibility Analysis of Rural Garbage Classification and Reuse [J].Computer Informatization and Mechanical System，2023，6（3）：43-47.
[4] 李钰.摆式摩擦系数测定仪示值误差来源分析及不确定度评定 [J].科学技术创新，2023（18）：13-16.
[5] LIU Y，XIANG G L，GAO Y，et al. Research on the Temperature Compensation Algorithm of Zero Drift in MEMS Gyroscope Based on Wavelet Transform and Improved Grey Theory [J].Sensors amp; Transducers，2014，178（9）：20-27.
[6] WEN Z X，LIU F F，DOU X F，et al. Research on Kinematics Analysis and Trajectory Planning of UR5 Robot [J/OL].Journal of Physics：Conference Series，2022，2396（1）.[2023-07-26].https：//iopscience.iop.org/article/10.1088/1742-6596/2396/1/012046。
[7] 刘育强.面向可维修航天器的空间机械臂在轨维修动力学与控制 [D].哈尔滨：哈尔滨工业大学，2022.
[8] ZHU Y C，YANG C J，WEI Q X，et al. Human–Robot Shared Control for Humanoid Manipulator Trajectory Planning [J].The Industrial Robot，2020，47（3）：395-407.
[9] JIE K，HE W Y，GUI R J. Kinematics Analysis of Lower Limb Rehabilitation Robot Based on D-H method [C]//第41届中国控制会议.合肥：[出版者不详]，2022：231-236.
[10] PHONG D L，CHOI J，LEE W，et al. A novel Method for Estimating External Force： Simulation Study With A 4-DOF Robot Manipulator [J].International Journal of Precision Engineering and Manufacturing，2015，16（4）：755-766.
[11] 邹彦冉，张竹林，阮帅，等.基于自适应容积卡尔曼滤波的车辆状态参数估计与仿真分析 [J].农业装备与车辆工程，2023，61（7）：59-64.
[12] GRYNAL D，ROLAN G，ROSTON M，et al. Wheel Alignment Detection with IoT Embedded System [J].Materials Today：Proceedings，2022，52（P3）：1924-1929.
[13] LUIS ALBERTO C C，CRISTÓBAL V J，SERGIO ISAÍ P R，et al. A Polynomial Fitting Problem： The Orthogonal Distances Method [J].Mathematics，2022，10（23）：4596-4596.
[14] 李鑫，郑铖，杨星，等.卡尔曼滤波与PID的两轮自平衡小车设计 [J].单片机与嵌入式系统应用，2023，23（1）：84-87.
[15] WON J，PARK J W，SONG M H，et al. Robust Vision-Based Displacement Measurement and Acceleration Estimation Using RANSAC and Kalman Filter [J].Earthquake Engineering and Engineering Vibration，2023，22（2）：347-358.
作者简介：章聪聪（2001—），男，汉族，河南焦作人，本科在读，研究方向：自动化专业；张同瓒（2003一），男，汉族，河北廊坊人，本科在读，研究方向：机器人工程。
Optimization Design of Intelligent Vehicle Posture for Garbage Sorting in Competitions
ZHANG Congcong， ZHANG Tongzan
（School of Electrical and Information Engineering， Anhui University of Science and Technology， Huainan 232001， China）
Abstract： In order to solve the problems of high cost and low efficiency of human resources in household garbage cleaning， a design scheme for garbage sorting intelligent vehicle is proposed. This study aims to improve the efficiency and accuracy of robotic arm grasping， optimize the kinematic solution algorithm and structural design of the robotic arm. To solve the positioning deviation caused by wheel slippage in intelligent vehicles， the displacement data is measured and MATLAB is used to calculate the fitting curve quartic polynomial for optimization. To address the issue of zero drift in gyroscopes， the Kalman filtering algorithm is used for optimization. Through comprehensive improvement， the intelligent vehicles robotic arm and body posture control are made more precise and reliable. The results indicate that the designed garbage sorting intelligent vehicle can obtain accurate and reliable posture information， thereby efficiently completing sorting tasks.
Keywords： forward and inverse kinematics; least square method; optimization analysis; Kalman filter
