机器人Rviz+matlab仿真详细步骤，从solidworks到urdf以及机器人编程控制，以ER50-C20为例

运动学建模

ER50-C20是一款工业机器人，通常用于制造业中的自动化任务。它具有六个自由度，适用于中小型的搬运、焊接、组装等应用。ER50-C20的特点包括高精度、高负载能力和灵活的操作范围，能够在复杂的工作环境中执行多样化的任务。我们就以该机器人为例建立机器人模型并利用编程语言进行控制。

模型坐标系建立

利用Modified Denavit–Hartenberg也即修正DH法建立其DH坐标系建立该机器人的末端姿态和关节转角的关系。

首先建立机器人坐标系方便建立MDH坐标系，在建系的过程当中发现利用MDH很难直接列出其MDH，直接原因是当我们将转轴设置为z轴后部分坐标系通过MDH的坐标转化是不可能实现的，因此最直接的解决方法是改变机器人的初始姿态：

MDH的每一个坐标系变化表示公式为：

其中表示的含义是从i-1坐标系转化到i坐标系的过程是先从i-1坐标系的x轴转动α角度，然后再在变化后的坐标系下以x方向移动a距离，转化后现在i-1的z轴应该已经和i的z轴重合了，因此可以在此基础上转动θ，也即机器人的关节转角，一般是以初始位置来算就是0，最后再向着现在的z轴移动d距离。

公式当中Trotx代表绕着x方向转动的刚体变换矩阵，Trotz代表绕着z方向转动的刚体变换矩阵，transl函数代表平移刚体变换矩阵，具体详见：空间描述和变换

由于该转换只设计x和z方向的变化，为方便建立坐标系设置的初始姿态x方向不变，z随着机器人关节转动方向变化，建立的MDH坐标系草稿如下（其中将4坐标系向上移动到和5重合确保MDH转换能够顺利进行）：

根据机器人结构发现关节3这里有一个偏置，因此最终表示出来的MDH坐标系表示如下：

可以表示的MDH表格为：

ER50-C20机器人MDH

θ/rad	d/mm	a/mm	α/rad
θ1	603	0	0
θ2	0	220	-pi/2
θ3	0	900	0
θ4	1004	-160	pi/2
θ5	0	0	-pi/2
θ6	219.5	0	pi/2

matlab机器人工具箱验证

为了验证我们的MDH是否准确描述该机器人，可以在matlab当中建立简单几何机器人结构对比一下。首先先安装matlab机器人工具箱：

前往Robotics Toolbox - Peter Corke网址下载机器人工具箱并解压到matlab目录当中，网址显示下载文件如下

同时也可以在win系统终端（Powershell）或者cmd（Command Prompt）当中输入下列指令下载：

git clone https://github.com/petercorke/robotics-toolbox-matlab rtb

git clone https://github.com/petercorke/spatial-math smtb

git clone https://github.com/petercorke/toolbox-common-matlab common

同时也可以在该网址下载对应的pdf文档以方便深入学习机器人工具箱的用法和对应函数的意义：

matlab当中可以通过mltbx通过Adds-on下载安装详见：matlab-robotics-toolbox安装方法，同样也可以直接解压压缩包找到rvctools文件夹下的startup_rvc.m文件在matlab界面运行

在matlab当中通过上述建立的MDH编写下列简单代码用于验证：

%%
% This is a demo script to establish ER50 kinematic model
% Dedicated to research for robotics
% Written by ZSTU-ZHC
%%

clear; clc;
startup_rvc
L_modified = [
    Link([0,          603,           0,          0], 'modified');   
    Link([0,            0,         220,      -pi/2], 'modified');
    Link([0,            0,         900,          0], 'modified');
    Link([0,         1004,        -160,       pi/2], 'modified');
    Link([0,            0,           0,      -pi/2], 'modified');
    Link([0,        219.5,           0,       pi/2], 'modified')
];

ER50 = SerialLink(L_modified, 'name', 'ER50C20');

figure;
view(3)
ER50.teach([0 0 0 0 0 0 ]);
title('Modified DH Parameter Robot Model');

代码解释如下：

• clear; clc;代表清除工作区和命令面板的所有数据防止程序数据影响；
• startup_rvc代表运行 Robotics Toolbox（Peter Corke 的 Robotics Toolbox）中的初始化脚本，设置相关路径和工具；
• L_modified代表定义一个包含多个关节链接的数组 L_modified，其中每个关节用一个 Link 对象表示；
• ER50 = SerialLink代表创建一个串联机器人模型 ER50，使用 L_modified 中定义的关节链接；
• figure;view(3)代表创建一打开一个新的图形窗口并使用三维视角；
• 打开一个交互式窗口，用于控制和可视化机器人并设置表示各个关节的初始角度为零。

运行成功后跳出如下界面：

通过调整左侧的关节调节滑动条观察机器人的运动情况：

验证发现和solidworks模型的关节结构相同，可以建立机械臂末端执行器到关节角度的映射关系。

模型导出为urdf

导出urdf可以通过自行编写urdf文件以及将模型转化为stl文件根据坐标系建立，但是该操作需要操作者对于URDF有一定的理解经验，具体可以前往github的ROS教学pdf当中学习：ROS-1/ROS机器人开发实践https://www.bing.com/ck/a?!&&p=f930984304912f5aJmltdHM9MTcyNDg4OTYwMCZpZ3VpZD0xYmNkN2ViZi1iMmQ2LTY5ZWItMWYyZS02ZDBmYjNiMDY4ZjgmaW5zaWQ9NTE4Ng&ptn=3&ver=2&hsh=3&fclid=1bcd7ebf-b2d6-69eb-1f2e-6d0fb3b068f8&psq=github+ROS1%E5%AD%A6%E4%B9%A0pdf&u=a1aHR0cHM6Ly9naXRodWIuY29tL3poYW5namllZmVuZy9ST1MtMS9ibG9iL21hc3Rlci9ST1MlRTYlOUMlQkElRTUlOTklQTglRTQlQkElQkElRTUlQkMlODAlRTUlOEYlOTElRTUlQUUlOUUlRTglQjclQjVfJUU4JTgzJUExJUU2JTk4JUE1JUU2JTk3JUFELnBkZg&ntb=1

安装urdf工具

如果认为自行建立较为繁杂可以通过urdf转化工具辅助转化，下载网址：sw_urdf_exporter - ROS Wikihttps://www.bing.com/ck/a?!&&p=74df685a424bce11JmltdHM9MTcyNDg4OTYwMCZpZ3VpZD0xYmNkN2ViZi1iMmQ2LTY5ZWItMWYyZS02ZDBmYjNiMDY4ZjgmaW5zaWQ9NTIyMQ&ptn=3&ver=2&hsh=3&fclid=1bcd7ebf-b2d6-69eb-1f2e-6d0fb3b068f8&psq=solidworks+urdf&u=a1aHR0cDovL3dpa2kucm9zLm9yZy9zd191cmRmX2V4cG9ydGVy&ntb=1

网址打开界面如下，点击download下载：

点击后会跳到github网址：https://github.com/ros/solidworks_urdf_exporter/releases

直接下载第一个exe文件直接安装即可

SOLIDWORKS转URDF

solidworks安装urdf转化工具，直接点击安装后solidworks界面会多出如下界面：

在solidworks的工具（Tools）界面会有导出为URDF的选项（Export as URDF）

我们通过参考几何当中的坐标系选项结合上述的MDH机器人坐标系建立机器人的坐标系后可以导出urdf。

由于在solidworks建立坐标系，并在urdf转化插件当中建立参考转轴和输入参数是一个繁琐的过程，为了简化步骤此处直接让urdf转化插件自动生成整个urdf文件通过后期修改urdf的内容来完成操作：

在solidworks当中选择工具再选择导出urdf：

通过添加子链接添加6个子链接

并且由于该机器人是串联机器人，因此需要一个挨着一个建立，都从上一个link作为自己的parent link，设置每一个link名字是l*（*代表对应的数值），关节是j*，最后建立如下：

选择每一条link对应的物理模型并点击preview and export，该步骤会让机器人自动设置坐标系和转轴等信息：

在接下来的窗口任意选择一个转角输入下列参数：

其余的关节限制等参数基本是一致的，但是手动输入较为繁琐我们直接导出等会在urdf当中修改，因此此处直接点击下一步next

这里主要是设置动力学需要的惯性参数，需要真实模拟的需要输入准确参数，如果只考虑运动控制不需要管这些参数，同时右侧还有颜色RGB选项，可以自行修改，点击export URDF and Meshes导出

修改URDF参数

如果是严格按照MDH建立坐标系并准确输入参数输出模型的不需要查看此步骤

由于我们只输入了其中一个关节的限制参数，可以将有的那个复制<joint ... </joint>内部的数据转贴到其他关节当中，同时将运动类别改为revolute而不是continuous。

同时sw_urdf直接转化的urdf文件内部转轴可能是反的，需要根据步骤“模型坐标系建立”当中的matlab得到的转动方向和此处的转动方向对比，如果是反的只需要在urdf文件当中将：例如某个关节：<axisxyz="0 1 0" />当中有1的部分1改为-1，如果是-1改为1即可。

ROS1工作空间

安装ROS

在x86_64或者ARM架构下的Ubuntu安装ROS1建议直接使用鱼香ROS。在安装选项当中选择ROS1当中的noetic或者melodic即可

鱼香ROS安装终端指令：

wget http://fishros.com/install -O fishros && . fishros

通过在终端输入roscore验证是否安装成功，成功界面如下：

安装后为启动Rviz验证关节转角是否准确建议安装关节控制，如果安装的是melodic执行下列指令

sudo apt-get install ros-melodic-joint-state-publisher-gui

如果是noetic执行下列指令：

​sudo apt-get install ros-noetic-joint-state-publisher-gui

此案例使用的是：

• ubuntu18.04
• python3.10.7
• ros1-melodic-1.14.13

创建工作空间

在终端运行下列指令将会在/home/*你的系统名字*/目录下建立一个名字叫做robotics_ws的工作空间并直接编译：

mkdir -p ~/robotics_ws/src
cd ~/robotics_ws/src
catkin_init_workspace
cd ..
catkin_make

由于编译完成后每次都要输入

source ~/robotics_ws/devel/setup.bash

在终端输入下列指令以方便下次不再需要source这个工作空间的编译结果:

如果安装了gedit（默认都是有的）

sudo gedit ~/.bashrc

如果没有gedit输入下列指令安装：

sudo apt-get update
sudo apt-get install gedit

然后在弹出的窗口当中在最后添加source ~/robotics_ws/devel/setup.bash，也就是我们要输入的指令，这样每次系统会自动索引。

将solidworks生成的urdf包复制到工作空间目录下，也即~/robotics_ws/src目录下。

然后在robotics_ws文件夹下打开一个终端输入catkin_make进行编译

将solidworks生成的文件夹直接放到工作空间的src文件夹内并编译（此处我用的是WSL2因此是在windows界面显示的文件）例如导出的文件名字是ER50，那么只需要将ER50放入文件夹~/robotics_ws/src下即可

然后在robotics_ws界面打开终端输入：

catkin_make

如果没有在.bashrc文件内部添加机器人编译源则需要在终端输入：

source ~/robotics_ws/devel/setup.bash

然后在该终端窗口输入：

roslaunch ER50 display.launch

直接打开的界面是空的，打开后在该界面选择base_link当中固定坐标系

再在左下角选择添加（Add）内部选择RobotModel点击OK

在上述操作后可以得到下列界面：

通过调节关节调节器可以控制机器人关节运动：

运动控制

机器人工具箱roboticstoolbox-python

安装机器人工具箱

安装python的机器人工具箱指令如下，使用清华源在国内速度较快，如果速度还是很慢请去除“-i”以及后面的指令或者将“-i”后的的pip源换成其他源，详见pip 使用国内镜像源：

pip3 install roboticstoolbox-python -i https://pypi.tuna.tsinghua.edu.cn/simple/

如果采用机器人工具箱，由于使用ROS1的ubuntu系统最高版本只支持到20.04，再高只能自己手动安装编译ROS1，较为麻烦。同时ubuntu20.04系统下的python安装机器人工具箱可能会导致各种问题，即使安装成功有可能部分库无法使用.

使用下列指令验证工具箱是否安装成功：

首先输入：

python3

然后在跳出的界面内输入：

from roboticstoolbox import DHRobot, RevoluteMDH
from spatialmath import SE3

如果直接换行并没有其他报错表示安装成功。

安装Python3.10.7（Optional）

如果你的系统安装了python3并成功安装了机器人工具箱那么则忽略下列步骤，如果没有则利用下列代码安装python3.10.7

wget https://www.python.org/ftp/python/3.10.7/Python-3.10.7.tgz
tar -xf Python-3.10.7.tgz
cd Python-3.10.7
./configure --enable-optimizations --prefix=$HOME/python3.10
make -j 8 
make install

该指令将会自动从python官网下载python3.10.7并解压编译安装，根据系统资源情况自行选择编译的并行个数，如果设备较差可以将make -j 8 当中的8改为4，较好可以改为12。总体过程耗时会较长，可以先挂着休息一下。

安装完成后会发现在ubuntu系统主目录下会多出python3.10和Python3.10.7两个目录，其中Python3.10.7是解压的目录，python3.10是已经编译好的目录。空间不够的可以删除Python3.10.7目录，因为后期也用不到了，删除指令：

sudo rm -rf ~/Python3.10.7

输入下列指令验证python3.10.7是否安装成功：

~/python3.10/bin/python3.10

如果它显示如下则表示安装成功：

利用工具箱编写控制代码

逆运动学程序

如果利用工具箱那么则必须要建立这个机器人的DH模型或者MDH模型

此处我们用建立的MDH编写逆运动学代码：

import numpy as np
from roboticstoolbox import DHRobot, RevoluteMDH
from spatialmath import SE3

pi = np.pi

def ik_ER50(target_pose):
    enable = True
    l0 = RevoluteMDH(d=603  , a=     0, alpha=0    ,qlim=[-3.0  ,   3.0])
    l1 = RevoluteMDH(d=0    , a=   220, alpha=-pi/2,qlim=[-np.pi,     0])
    l2 = RevoluteMDH(d=0    , a=   900, alpha=0    ,qlim=[   1.5,4.4124])
    l3 = RevoluteMDH(d=1004 , a=  -160, alpha= pi/2,qlim=[-np.pi, np.pi])
    l4 = RevoluteMDH(d=0    , a=     0, alpha=-pi/2,qlim=[    -2,     2])
    l5 = RevoluteMDH(d=219.5, a=     0, alpha= pi/2,qlim=[-np.pi, np.pi])

    robot = DHRobot([l0, l1, l2, l3, l4, l5], name='ER50')

    if enable == True:
        xyz = target_pose[:3]  
        rpy = target_pose[3:]  
        T = SE3(xyz) * SE3.RPY(rpy, order='zyx')

        result = robot.ikine_LM(T)

        q = result.q  
        success = result.success  
        err = result.residual  
            
    print("Joint Angles:", q)
    print("Success:", success)
    print("Error:", err)

    return q, err

该代码简单解释如下:

• 建立了ik_ER50函数用于求解机器人逆运动学并在内部输入了MDH；
• 由于求解需要在解空间内寻找解，因此需要设置关节限制，qlim内部参数代表每一个关节的限制角度表示了上下限；
• 利用DHRobot将上述MDH建立机器人数学模型；
• xyz和rpy分别代码机器人末端的位置和姿态，q代表求解的关节角度，success表示是否求解成功，err表示和实际位姿的偏差；
• 该函数输入机器人的末端姿态xyzrpy返回机器人的各个关节数值q和对应的损失err。

建立该函数并命名为ik_er50.py，然后编写一个利用该逆运动学的控制程序：

GUI关节运动控制

在此前需要下载对应的库以支持其运行：

pip3 install rospkg -i https://pypi.tuna.tsinghua.edu.cn/simple/
sudo apt-get install python3-tk

import tkinter as tk
from tkinter import messagebox
import numpy as np
import rospy
import subprocess
from sensor_msgs.msg import JointState
from ik_er50 import ik_ER50 # 导入我们编写的逆运动学程序

rospy.init_node('robot_gui_control', anonymous=True)
pub = rospy.Publisher('joint_states', JointState, queue_size=10)
rate = rospy.Rate(25)

def kill_rosnode(node_name): # 杀死关节控制程序防止其影响该程序运行的函数
    try:
        subprocess.run(['rosnode', 'kill', node_name], check=True)
        print(f"Successfully killed ROS node: {node_name}")
    except subprocess.CalledProcessError as e:
        print(f"Failed to kill ROS node: {node_name}")
        print(f"Error: {str(e)}")

def shortest_path_interpolation(start_angles, end_angles, steps):# 设置从一个初始关节角度到目标关节角度的一个最短的线性变化矩阵
    interpolated_angles = np.zeros((steps, len(start_angles)))

    for i in range(len(start_angles)):
        start_angle = start_angles[i]
        end_angle = end_angles[i]
        
        # 计算角度差
        delta_angle = end_angle - start_angle
        
        # 确定最短路径的方向
        if delta_angle > np.pi:
            delta_angle -= 2 * np.pi
        elif delta_angle < -np.pi:
            delta_angle += 2 * np.pi

        # 生成中间角度
        for j in range(steps):
            interpolated_angles[j, i] = start_angle + (delta_angle * j / (steps - 1))
    
    return interpolated_angles

def calculate_and_publish():
    try:
        pi = np.pi
        
        x = float(entry_x.get()) # 从tk-gui界面当中读取输入的xyzrpy数值
        y = float(entry_y.get())
        z = float(entry_z.get())
        roll = float(entry_roll.get())
        pitch = float(entry_pitch.get())
        yaw = float(entry_yaw.get())
        
        target_pose = np.array([x, y, z, roll, pitch, yaw]) 
        # target_pose = np.array([1575.6, 0, 276.677, -pi, 0, -pi]) 
        
        joint_angles,error = ik_ER50(target_pose) # 进行逆运动学反解
        
        print(joint_angles,error)
        
        current_angles = np.array([0,-pi/2,pi,0,pi/2,0]) # 设置该关节角度为初始姿态
        # current_angles = np.array([0,0,0,0,0,0]) 
        steps = 200  # 变化步长200
        interpolated_angles = shortest_path_interpolation(current_angles, joint_angles, steps)
        # print(interpolated_angles)
        
        for i in range(steps):
            joint_state = JointState() # 发布给ROS节点控制机器人运动
            joint_state.name = ['j1', 'j2', 'j3', 'j4', 'j5', 'j6']
            joint_state.position = interpolated_angles[i]
            joint_state.header.stamp = rospy.Time.now()
            pub.publish(joint_state)
            rate.sleep()
        
        messagebox.showinfo("Success", "Pose reached!")
    except Exception as e:
        messagebox.showerror("Error", str(e))

kill_rosnode('/joint_state_publisher_gui') # 杀死关节控制节点
root = tk.Tk() # 下面是python TK GUI界面
root.title("UR3 Robot Controller")

tk.Label(root, text="X:").grid(row=0, column=0)
entry_x = tk.Entry(root)
entry_x.grid(row=0, column=1)

tk.Label(root, text="Y:").grid(row=1, column=0)
entry_y = tk.Entry(root)
entry_y.grid(row=1, column=1)

tk.Label(root, text="Z:").grid(row=2, column=0)
entry_z = tk.Entry(root)
entry_z.grid(row=2, column=1)

tk.Label(root, text="Roll (rad):").grid(row=3, column=0)
entry_roll = tk.Entry(root)
entry_roll.grid(row=3, column=1)

tk.Label(root, text="Pitch (rad):").grid(row=4, column=0)
entry_pitch = tk.Entry(root)
entry_pitch.grid(row=4, column=1)

tk.Label(root, text="Yaw (rad):").grid(row=5, column=0)
entry_yaw = tk.Entry(root)
entry_yaw.grid(row=5, column=1)

button = tk.Button(root, text="Calculate and Move", command=calculate_and_publish)
button.grid(row=6, column=0, columnspan=2)

root.mainloop()

将上述代码命名为move_GUI_xd3.py并且和ik_er50.py放在同一个文件夹下在该文件夹内打开一个终端运行下列指令：

如果是成功安装机器人工具箱的

python3 move_GUI_xd3.py

如果是安装了python3.10.7的先安装机器人工具箱：

~/python3.10/bin/python3.10 -m pip config set global.index-url https://pypi.tuna.tsinghua.edu.cn/simple/
~/python3.10/bin/python3.10 -m pip install roboticstoolbox-python 

然后再在ik_er50.py和move_GUI_xd3.py的文件夹内打开终端执行下列指令运行该程序：

~/python3.10/bin/python3.10 move_GUI_xd3.py

将得到一个GUI界面：

输入一个机器人末端的位置和姿态：

上述代码是通过关节控制来控制机器人运动，实际只是利用机器人逆运动学计算了目标位置的关节解然后让机器人从设置的初始位置线性变化的目标位置。

机器人轨迹控制案例（以圆形为例）

利用逆运动学编写一个圆形轨迹然后将这个轨迹当中的点交给机器人逆运动学算法当中运算出每一个点对应的关节角度，由于机器人普遍存在多解，因此编写了compute_smooth_ik函数用于控制每一个求解出来的解的连续性：

import numpy as np
import subprocess
import rospy
from sensor_msgs.msg import JointState
from spatialmath import SE3
from roboticstoolbox import DHRobot, RevoluteDH
from ik_er50 import ik_ER50, compute_smooth_ik


def kill_rosnode(node_name):
    try:
        subprocess.run(['rosnode', 'kill', node_name], check=True)
        print(f"Successfully killed ROS node: {node_name}")
    except subprocess.CalledProcessError as e:
        print(f"Failed to kill ROS node: {node_name}")
        print(f"Error: {str(e)}")

def interpolate(start, end, steps):
    return np.linspace(start, end, steps)

rospy.init_node('robot_control', anonymous=True)
pub = rospy.Publisher('joint_states', JointState, queue_size=10)
rate = rospy.Rate(25)

def execute_multi_segment_trajectory_with_variable_steps(points, steps):
    last_q = None

    for j in range(len(points) - 1):
        start_pose = points[j]
        end_pose = points[j+1]
        segment_steps = steps[j]

        xs, ys, zs = interpolate(start_pose[0], end_pose[0], segment_steps), interpolate(start_pose[1], end_pose[1], segment_steps), interpolate(start_pose[2], end_pose[2], segment_steps)
        rolls, pitches, yaws = interpolate(start_pose[3], end_pose[3], segment_steps), interpolate(start_pose[4], end_pose[4], segment_steps), interpolate(start_pose[5], end_pose[5], segment_steps)

        for i in range(segment_steps):
            target_pose = [xs[i], ys[i], zs[i], rolls[i], pitches[i], yaws[i]]
            if i == 0 and j == 0:
                joint_angles, _ = compute_ik_ur10(target_pose)
            else:
                joint_angles = compute_smooth_ik(target_pose, last_q)

            last_q = joint_angles

            # Publish joint state
            joint_state = JointState()
            joint_state.header.stamp = rospy.Time.now()
            joint_state.name = ['j1', 'j2', 'j3', 'j4', 'j5', 'j6', 'j7']
            joint_state.position = joint_angles
            pub.publish(joint_state)
            rate.sleep()


def circle_interpolate_xoy(center_pose, radius, num_points):
    x_center, y_center, z_center, roll, pitch, yaw = center_pose
    angles = np.linspace(0, 2 * np.pi, num_points, endpoint=False)
    x_points = x_center + radius * np.cos(angles)
    y_points = y_center + radius * np.sin(angles)
    z_points = np.full_like(x_points, z_center)
    roll_points = np.full_like(x_points, roll)
    pitch_points = np.full_like(x_points, pitch)
    yaw_points = np.full_like(x_points, yaw)
    circle_poses = np.vstack((x_points, y_points, z_points, roll_points, pitch_points, yaw_points)).T
    return circle_poses

def execute_trajectory(poses, last_q=None):
    for pose in poses:
        if last_q is None:
            joint_angles, _ = ik_ER50(pose)
        else:
            joint_angles = compute_smooth_ik(pose, last_q)
        last_q = joint_angles
        joint_state = JointState()
        joint_state.header.stamp = rospy.Time.now()
        joint_state.name = ['j1', 'j2', 'j3', 'j4', 'j5', 'j6']
        joint_state.position = joint_angles
        pub.publish(joint_state)
        rate.sleep()
    return last_q

rospy.init_node('robot_control', anonymous=True)
pub = rospy.Publisher('joint_states', JointState, queue_size=10)
rate = rospy.Rate(25)

if __name__ == '__main__':
    pi = np.pi
    kill_rosnode('/joint_state_publisher_gui')

    # execute_multi_segment_trajectory_with_variable_steps(points, steps)
    # Circle trajectory
    center_pose = [1300, 600, 0, -pi, 0, -pi]
    radius = 200
    num_points = 200
    circle_poses = circle_interpolate_xoy(center_pose, radius, num_points)
    execute_trajectory(circle_poses, None)

其中的控制连续性的逆运动学函数如下：

import numpy as np
from roboticstoolbox import DHRobot, RevoluteMDH
from spatialmath import SE3

pi = np.pi

def compute_smooth_ik(target_pose, last_q):
    robot = DHRobot([
        RevoluteMDH(d=603  , a=     0, alpha=0    ),
        RevoluteMDH(d=0    , a=   220, alpha=-pi/2),
        RevoluteMDH(d=0    , a=   900, alpha=0    ),
        RevoluteMDH(d=1004 , a=  -160, alpha= pi/2),
        RevoluteMDH(d=0    , a=     0, alpha=-pi/2),
        RevoluteMDH(d=219.5, a=     0, alpha= pi/2),
    ], name='ER50')

    xyz = target_pose[:3]  
    rpy = target_pose[3:]  
    T = SE3(xyz) * SE3.RPY(rpy, order='zyx')
    result = robot.ikine_LM(T, q0=last_q)  

    if result.success:
        delta_q = np.sum((result.q - last_q)**2)
        if delta_q < 0.05:
            return result.q  
        else:
            print("Transition not smooth, delta_q:", delta_q)
    else:
        print("IK failed for pose:", target_pose)

    return last_q  

该函数和逆运动学求解无异，主要核心部分在于delta_q = np.sum((result.q - last_q)**2)用于计算当前和上一次的解的差值，只有当差值损失在一定范围内才返回该解，如果你在多次运行某一段轨迹发现其一直返回Transition not smooth，可以适当将delta_q < 0.05的数值改大提高容错率。

运行结果如下：

连续轨迹控制

由于机器人刚开始运行是直接有速度的，不符合物理学定律，现实当中速度以及加速度变化应该是连续的，否则会导致机器人速度突变产生冲击，可以利用五次函数让机器人运动的速度和加速度以及加加速度都连续变化的轨迹，该轨迹和时间建立关系，其函数可以表示为：

def five_polynomial(start, end, t, T):
    a0 = start
    a1 = 0
    a2 = 0
    a3 = 10 * (end - start) / T**3
    a4 = -15 * (end - start) / T**4
    a5 = 6 * (end - start) / T**5
    return a0 + a1*t + a2*t**2 + a3*t**3 + a4*t**4 + a5*t**5

多段线轨迹控制（以直线为例）

同时还可以通过多段线拼合形成自定义的空间轨迹，这里以直线为例编写下列函数：

def execute_multi_segment_trajectory_with_variable_steps(points, steps):
    last_q = None

    for j in range(len(points) - 1):
        start_pose = points[j]
        end_pose = points[j+1]
        segment_steps = steps[j]

        xs, ys, zs = interpolate(start_pose[0], end_pose[0], segment_steps), interpolate(start_pose[1], end_pose[1], segment_steps), interpolate(start_pose[2], end_pose[2], segment_steps)
        rolls, pitches, yaws = interpolate(start_pose[3], end_pose[3], segment_steps), interpolate(start_pose[4], end_pose[4], segment_steps), interpolate(start_pose[5], end_pose[5], segment_steps)

        for i in range(segment_steps):
            target_pose = [xs[i], ys[i], zs[i], rolls[i], pitches[i], yaws[i]]
            if i == 0 and j == 0:
                joint_angles, _ = ik_ER50(target_pose)
            else:
                joint_angles = compute_smooth_ik(target_pose, last_q)

            last_q = joint_angles

            # Publish joint state
            joint_state = JointState()
            joint_state.header.stamp = rospy.Time.now()
            joint_state.name = ['j1', 'j2', 'j3', 'j4', 'j5', 'j6']
            joint_state.position = joint_angles
            pub.publish(joint_state)
            rate.sleep()

多段线运行结果：

科学运算库SciPy

求解6自由度机器人逆运动学实质上是在一个六维解空间范围内求解的过程，利用scipy库并给定一个初始解空间向量不断通过正运动学计算和目标姿态比对反馈后的迭代可以将这个初始解不断逼近到目标解向量当中。

通常来说迭代次数越多这个解的精度也会越高，但是同一个姿态机器人存在多解并且迭代可能陷入局部最优的情况会可能导致求解偏差很大。

利用Scipy库和numpy建立方程求解在一定程度上简化了自身代码的编写量。

from scipy.spatial.transform import Rotation as R
import scipy.optimize as opt
import numpy as np
import math

pi = math.pi

def transl(x,y,z):
    matrix1 = np.array([
    [1, 0, 0, x],
    [0, 1, 0, y],
    [0, 0, 1, z],
    [0, 0, 0, 1]])
    return matrix1

def rotx(theta):
    c, s = np.cos(theta), np.sin(theta)
    return np.array([
        [1, 0, 0, 0],
        [0, c, -s, 0],
        [0, s, c, 0],
        [0, 0, 0, 1]])

def roty(theta):
    c, s = np.cos(theta), np.sin(theta)
    return np.array([
        [c, 0, s, 0],
        [0, 1, 0, 0],
        [-s, 0, c, 0],
        [0, 0, 0, 1]])

def rotz(theta):
    c, s = np.cos(theta), np.sin(theta)
    return np.array([
        [c, -s, 0, 0],
        [s, c, 0, 0],
        [0, 0, 1, 0],
        [0, 0, 0, 1]])

def Forward6(qs, xd):
    h = 0.01
    l = 1
    T1_1 = np.dot(transl(h + 2 * l, 0, 0), rotz(qs[0]))
    T1_2 = np.dot(roty(-qs[1]), transl(h + l, 0, 0))
    T1 = np.dot(T1_1, T1_2)
    T2_1 = ... # 此处编写对应机器人的任意坐标转化，只要能够准确描述基座到机器人末端执行器的转化关系即可
    T4 = np.dot(T4_1, T4_2)
    T_homo1 = np.dot(T1, T2)
    T_homo2 = np.dot(T3, T4)
    T_homo = np.dot(T_homo1, T_homo2)# T_homo用于表示最终的齐次矩阵

    Rm = T_homo[0:3, 0:3]
    rotation = R.from_matrix(Rm)
    euler_angles = rotation.as_euler('ZYX', degrees=False)

    print(T_homo[0, 3])
    f = np.power(xd[0] - T_homo[0, 3], 2) + np.power(xd[1] - T_homo[1, 3], 2) + \
        np.power(xd[2] - T_homo[2, 3], 2) + np.power(xd[3] - euler_angles[0], 2) + \
        np.power(xd[4] - euler_angles[1], 2) + np.power(xd[5] - euler_angles[2], 2)
    return f


def InverseKinematic(xd, q0):

    print('Iteration initialized!')

    ub = np.full(6,   np.pi)  # Upper bounds
    lb = np.full(6,  -np.pi) # Lower bounds
    bounds = opt.Bounds(lb, ub)  # Create bounds object

    objective = lambda q: Forward6(q, xd)

    result = opt.minimize(objective, q0, bounds=bounds, method='SLSQP')

    q = result.x
    y = result.fun
    return q, y

通过上述代码可以建立自定义的坐标系转化函数并求解逆解，不过需要输入初始关节猜测角度通过迭代求解并以此代替机器人工具箱，坐标系建立也不再依赖于DH。

文章简单总结

1. 上述步骤可以作为案例实现任意串联机器人的运动学控制（并联机构难以在solidworks当中建立模型树）；
2. 结合了matlab、python、ROS、ubuntu、solidworks；
3. 运动控制部分同样可以使用moveit方便运动控制，本文主要使用工具箱以及自定义的函数控制；

附录

该文章案例当中的urdf模型以及控制代码打包免费下载：

【免费】机器人ROSRviz+matlab仿真的urdf模型文件资源免费分享资源-CSDN文库