Vrep+Python实现小车运动仿真_v-rep pro edu v3.5.0搭建agv仿真

环境安装以及配置：

本文用来记录自己使用Python远程控制Vrep进行两轮小车，进行差速运动仿真。模拟小车沿二维码运动

Vrep配置

       Vrep官网下载速度比较慢这里推荐网盘下载coppeliasim/vrep官网软件安装包（免费百度网盘链接）-CSDN博客

我ubuntu系统版本用的18.04，这里下载的是4.0版本的，经过测试最新的4.5版本在进行多台AGV控制的时候，出现了无法获取句柄。而且网上资料也多是旧版本的。

下载完之后解压，并配置环境

echo 'export VREP_ROOT="$HOME/V-REP_PRO_EDU_V3_5_0_Linux"' >> ~/.bashrc 
source ~/.bashrc

ubuntu使用命令vrep打开软件

python配置：

需要在安装目录导出3个文件

如果Vrep3.6版本

1.找到 vrep.py和 vrepConst.py两个文件

安装目录/programming/remoteApiBindings/python/python

2.找到编译文件

如果是ubuntu系统找到 remoteApi.so

在 安装目录/programming/remoteApiBindings/lib/lib/Linux/64Bit

如果是window系统找到 remoteApi.dll

在 安装目录/programming/remoteApiBindings/lib/lib/Windows/64Bit

如果Vrep4.0版本

1.找到 sim.py 和 simConst.py 两个文件

 安装目录/programming/legacyRemoteApi/remoteApiBindings/python/python

2.找到编译文件

如果是ubuntu系统找到 remoteApi.so

在 安装目录/programming/legacyRemoteApi/remoteApiBindings/lib/lib/Ubuntu18_04

如果是window系统找到 remoteApi.dll

在 安装目录/programming/legacyRemoteApi/remoteApiBindings/lib/lib/Windows

将3个文件和你的脚本放在一起

Vrep软件操作：

打开软件后在左下角可以拉取模型

建模过程不做演示了，具体建模可以在B站看视频学习，我这里弄了12辆小车，地上的红色方块是建立的平面，用来模拟地面贴的二维码

在信息栏中可以看到小车的属性，python是用过这些属性对小车进行句柄控制

1.设置连接端口

class VREP():
    def __init__(self) -> None:
        self.lock = threading.Lock()
        self.linear_velocity = 1
        self.speed = 0.026781272888183594  # m/s
        try:
            vrep.simxFinish(-1)  # 关掉之前连接
            self.clientID = vrep.simxStart(
                '127.0.0.1', 19997, True, True, 5000, 5)  # Connect to CoppeliaSim
            if self.clientID != -1:
                print('connect successfully')
            else:
                # Terminar este script
                sys.exit("[Error]:  no se puede conectar")
        except:
            print('[Error]: Check if CoppeliaSim is open')
        self.running = False
        # self.start_sim()
'
运行
运行

2.设置小车属性，多台小车名字需要不同，与代码中的小车名字要一致

每辆小车都有一个脚本，需要将脚本的disable打钩，否则在使用脚本控制的时候会发生冲突，启动仿真的时候小车会自动前进

更改小车轮子属性

在python脚本中使用simxGetObjectHandle函数获取句柄

获取小车和左右轮的句柄

    def creat_agv(self, i):
        """
        return agv = (agv1_handle,leftMotor1_handle, rightMotor1_handle)
        """
        _agv_name = "Pioneer_p3dx#"+str(i)
        _left_motor = "Pioneer_p3dx_leftMotor#"+str(i)
        _right_motor = "Pioneer_p3dx_rightMotor#"+str(i)
 
        agv = (self.get_handle(_agv_name),
               self.get_handle(_left_motor), self.get_handle(_right_motor))
        return agv
'
运行
运行

    def get_handle(self, name):
        with self.lock:
            e, handle = vrep.simxGetObjectHandle(
                self.clientID, name, vrep.simx_opmode_oneshot_wait)
            if e != 0:
                print("[Error]: get {} handle error !!!".format(name))
            return handle
'
运行
运行

至此Vrep相关配置已经完成。

Python接口介绍：

写一个vrep_client.py和导入的3个脚本放在一起

from . import vrep
import threading
import time
import numpy as np
import sys
 
 
class VREP():
    def __init__(self) -> None:
        self.lock = threading.Lock()
        self.linear_velocity = 1
        self.speed = 0.026781272888183594  # m/s
        try:
            vrep.simxFinish(-1)  # 关掉之前连接
            self.clientID = vrep.simxStart(
                '127.0.0.1', 19997, True, True, 5000, 5)  # Connect to CoppeliaSim
            if self.clientID != -1:
                print('connect successfully')
            else:
                # Terminar este script
                sys.exit("[Error]:  no se puede conectar")
        except:
            print('[Error]: Check if CoppeliaSim is open')
        self.running = False
        # self.start_sim()
 
    def __del__(self):
        # if self.clientID:
        # 断开与V-REP仿真的连接
        self.running = False
        vrep.simxStopSimulation(
            self.clientID, vrep.simx_opmode_blocking)  # 关闭仿真
        vrep.simxFinish(self.clientID)
        time.sleep(1)  # 仿真开启延时5s
        vrep.simxFinish(-1)  # 关闭连接
 
    def start_sim(self):
        with self.lock:
            res = vrep.simxStartSimulation(
                self.clientID, vrep.simx_opmode_oneshot_wait)
            if res == vrep.simx_return_ok:
                self.running = True
                print("仿真环境已启动")
            else:
                print("[Error]: 仿真环境启动失败")
 
    def finish_sim(self):
        with self.lock:
            vrep.simxStopSimulation(
                self.clientID, vrep.simx_opmode_blocking)  # 关闭仿真
            self.running = False
 
    def get_handle(self, name):
        with self.lock:
            e, handle = vrep.simxGetObjectHandle(
                self.clientID, name, vrep.simx_opmode_oneshot_wait)
            if e != 0:
                print("[Error]: get {} handle error !!!".format(name))
            return handle
 
    def stop_sim(self):
        with self.lock:
            res = vrep.simxStopSimulation(
                self.clientID, vrep.simx_opmode_oneshot)
            if res == vrep.simx_return_ok:
                print("仿真环境已关闭")
            else:
                print("[Error]: 仿真环境关闭失败")
 
    def get_pose(self, handle):
        """
        agv_name: handle
        return : agv pose [x, y, z]
        """
        with self.lock:
            e, res = vrep.simxGetObjectPosition(
                self.clientID, handle, -1, vrep.simx_opmode_oneshot_wait)
            if e != 0:
                return [None, None, None]
            else:
                return res
 
    def get_ori(self, handle):
        with self.lock:
            e, res = vrep.simxGetObjectOrientation(
                self.clientID, handle, -1, vrep.simx_opmode_blocking)
            if e != 0:
                return [None, None, None]
            else:
                return np.rad2deg(res)
 
    def get_joint(self, handle):
        with self.lock:
            e, res = vrep.simxGetJointPosition(
                self.clientID, handle, vrep.simx_opmode_oneshot_wait)
            if e != 0:
                return [None, None, None]
            else:
                return res
 
    def get_obj_matrix(self, obj_name):
        with self.lock:
            _, obj_handle = vrep.simxGetObjectHandle(
                self.clientID, obj_name, vrep.simx_opmode_oneshot_wait)
            return vrep.getObjectMatrix(obj_handle)
 
    def set_joint_velocity(self, handle, speed):
        with self.lock:
            res = vrep.simxSetJointTargetVelocity(
                self.clientID, handle, speed, vrep.simx_opmode_oneshot)

运动控制：

 两轮小车其实是用的一个差速控制，其实就是通过给两个轮子不同的速度让他进行运动，这里就不进行赘述了，具体可以参照

小鱼的两轮差速驱动运动模型 - 知乎

奔驰的战猪 两轮差速机器人运动学模型_两轮差速运动学模型-CSDN博客

python控制接口，通过函数set_joint_velocity进行速度控制，传入两个轮子不同的句柄。

    def run(self, agv_num, lspeed, rspeed):
        """
        speed:     0.029628699166434153 m/s
                或 0.3070062009269009 弧度/s 
                或 17.59015959745676 角度/s 
        轮子直径： 0.19411166926398632                    
        """
        left_motor_handle = self.agvs[agv_num][1]
        right_motor_handle = self.agvs[agv_num][2]
        r_res = self.set_joint_velocity(right_motor_handle, rspeed)
        l_res = self.set_joint_velocity(left_motor_handle, lspeed)
'
运行
运行

Pid部分：

小车在运动的时候受外界因素的影响，需要进行pid控制

class PIDControl:
    def __init__(self, Kp=0.25, Ki=0.0, Kd=0.1, integral_upper_limit=20, integral_lower_limit=-20):  
        self.Kp = Kp  #影响相应速度
        self.Ki = Ki  #补偿
        self.Kd = Kd  #消除震荡
        self.integral_upper_limit = integral_upper_limit  
        self.integral_lower_limit = integral_lower_limit  
        self.prev_error = 0  
        self.integral = 0  
  
    def update(self, setpoint, actual_position, dt):  
        error = setpoint - actual_position  
        self.integral += error * dt  
        derivative = self.Kd * (error - self.prev_error) / dt  
        self.prev_error = error  
        proportional = self.Kp * error 
        integral = self.Ki * self.clip(self.integral, self.integral_lower_limit, self.integral_upper_limit)
        control_increment = proportional + derivative + integral
        print("[PID]: 比例:{:.3f}, 积分:{:.3f}, 微分:{:.3f}, error:{:.3f}, p: {:.3f}"\
            .format(proportional,integral, derivative,error,control_increment))
        return control_increment  
  
    @staticmethod  
    def clip(value, lower_limit, upper_limit):  
        if value < lower_limit:  
            # print(f"lower_limit:{lower_limit}, upper_limit:{upper_limit}")
            return lower_limit  
        elif value > upper_limit:  
            # print(f"lower_limit:{lower_limit}, upper_limit:{upper_limit}")
            return upper_limit  
        else:  
            return value
    
    def clear(self):  
        self.integral = 0  
        self.prev_error = 0
'
运行
运行

直线运动调用,根据目标的位置和小车的当前角度差进行修正小车方向，

 
    def move_l(self, agv_num, tar_pose, s=5, e=0.1, rate=0.01):
        pid = PIDControl(0.05, 0.02, 0.0, 5,-5)
        print(f"[Dispath]:agv-{agv_num} runing new potint--target:{tar_pose}")
        time_start = time.time()
        distance = 100
        agv_handle = self.agvs[agv_num][0]
        car_pose = self.get_pose(agv_handle)
        car_angle = self.get_ori(agv_handle)[2]
        car_radians = np.radians(car_angle)
        rotation_matrix = np.array([[np.cos(car_radians), -np.sin(car_radians), 0],
                                    [np.sin(car_radians), np.cos(car_radians), 0],
                                    [0, 0, 1]])
        vector_ = np.array(
            [tar_pose[0] - car_pose[0], tar_pose[1] - car_pose[1], 0])
        # 将向量差转换到B坐标系中
        transformed_vector = np.dot(rotation_matrix.T, vector_)
        dif_angle = np.degrees(np.arctan2(
                    transformed_vector[1], transformed_vector[0]))
        while distance > e:
            time.sleep(rate)
            car_pose = self.get_pose(agv_handle)
            car_angle = self.get_ori(agv_handle)[2]
            car_radians = np.radians(car_angle)
            rotation_matrix = np.array([[np.cos(car_radians), -np.sin(car_radians), 0],
                                        [np.sin(car_radians), np.cos(car_radians), 0],
                                        [0, 0, 1]])
            vector_ = np.array(
                [tar_pose[0] - car_pose[0], tar_pose[1] - car_pose[1], 0])
            # 将向量差转换到B坐标系中
            transformed_vector = np.dot(rotation_matrix.T, vector_)
            # 计算旋转角度（方位角度）
            dif_angle = np.degrees(np.arctan2(
                    transformed_vector[1], transformed_vector[0]))
            # TODO use pid
            distance = np.linalg.norm(vector_)
            p = pid.update(0, dif_angle, rate)
            print(f"[AGV] movel====>> pid: {p},tar_angle: {0}, dif_angle: {dif_angle}, distanc:{distance} \n")
            #   减速
            if distance < 0.5:
                print(f"[AGV] movel==== 0.5 distance:{distance}")
                self.run(agv_num, (s+p)/2, (s-p)/2)
            elif distance < 0.1:
                print(f"[AGV] movel==== 0.5 distance:{distance}")
                self.run(agv_num, (s+p)/3, (s-p)/3)
            else:
                self.run(agv_num, s+p, s-p)
        self.run(agv_num, 0, 0)
        # time.sleep(1)
        return time.time() - time_start
'
运行
运行

原地旋转，传参为最终目标角，也就是世界坐标系角度。例如tar_angle=180，将车头转向世界坐标系的180。

    def move_c(self, agv_num, tar_angle, e=0.25, rate=0.01):
        pid = PIDControl(Kp=0.05, Ki=0.0, Kd=0.0005)
        time_start = time.time()
        cur_angle = self.get_ori(self.agvs[agv_num][0])[2]
        tar_matrix = R.from_euler('xyz', [0,0,tar_angle], degrees=True).as_matrix()
        while abs(tar_angle - cur_angle) > e:
            time.sleep(rate)
            cur_angle = self.get_ori(self.agvs[agv_num][0])[2]
            cur_rotation_matrix = R.from_euler('xyz', [0,0,cur_angle], degrees=True).as_matrix()
            C = np.dot(np.linalg.inv(tar_matrix), cur_rotation_matrix)
            yaw = R.from_matrix(C).as_euler('xyz')  
            yaw = np.degrees(yaw)[-1]
            vl = pid.update(0, yaw, rate)
            print("[AGV] movec vl:{:.3f}, 误差角:{:.3f}, tar_angle:{:.3f}, cur_angle:{:.3f}\n"\
                .format(vl, yaw,tar_angle,cur_angle))
            self.run(agv_num, -vl, vl)
        self.run(agv_num, 0, 0)
        pid.clear()
        return time.time() - time_start
'
运行
运行

原地旋转，传参为沿自身偏转角。例如tar_angle=90，将车头逆时针旋转90度。

 
    def move_offset_c(self, agv_num, angle, e=0.5, rate=0.01):
        pid = PIDControl(Kp=0.05, Ki=0.0, Kd=0.0005)
        time_start = time.time()
        cur_angle = self.get_ori(self.agvs[agv_num][0])[2]
        cur_rotation_matrix = R.from_euler('xyz', [0,0,cur_angle], degrees=True).as_matrix()
        angle_matrix = R.from_euler('xyz', [0,0,angle], degrees=True).as_matrix()
        tar_matrix = np.dot(cur_rotation_matrix,angle_matrix)
        tar_angle = R.from_matrix(tar_matrix).as_euler('xyz')[-1]
        yaw = 10
        while abs(yaw) > e:
            time.sleep(rate)
            cur_angle = self.get_ori(self.agvs[agv_num][0])[2]
            cur_rotation_matrix = R.from_euler('xyz', [0,0,cur_angle], degrees=True).as_matrix()
            C = np.dot(np.linalg.inv(tar_matrix), cur_rotation_matrix)
            yaw = R.from_matrix(C).as_euler('xyz')  
            yaw = np.degrees(yaw)[-1]
            vl = pid.update(0, yaw, rate)
            print("[AGV] movec vl:{:.3f}, 误差角:{:.3f}, tar_angle:{:.3f}, cur_angle:{:.3f}\n"\
                .format(vl, yaw,tar_angle,cur_angle))
            self.run(agv_num, -vl, vl)
        self.run(agv_num, 0, 0)
        pid.clear()
        return time.time() - time_start
'
运行
运行

完整代码：

 完整代码放在https://github.com/lizihan-robot/AGVVrepSim.git