差速轮时间最优控制

差速轮机器人时间最优控制

   最优控制做为现代控制理论一个非常重要的部分，相比于其它控制算法，直接对性能指标进行优化，同时可以添加各种各样的约束条件等优点，在自动化工程中有比较多的应用。但受限于当前的计算水平，还没有传统控制算法普遍应用于实际生产、生活中。
   本文将通过离散化的方式求解差速轮机器人从点A（$x$,$y$,$\theta$）,运行到点B的时间最短，在运行过程中，有速度、加速度的约束。

差速轮机器人运动学

   由于最优控制需要预测差速轮机器人的状态，因此需要得到差速轮机器人的运动学模型，其中微分形式的状态方程如式（1）所示。
$$\begin{array}{l}\dot{x}=vcos\theta \\ \dot{y}=vsin\theta \\ \dot{\theta }=w\\ \dot{v}=a_v\\ \dot{w}=a_w\end{array}\text{\hspace{1em}(1)}$$
   式（1）中，$(x,y,\theta )$为机器人在平面上的位姿，$(v,w)$分别为机器人的线速度和角速度，$(a_v,a_w)$分别为机器人的线速度加速度和角速度加速度，也就是需要控制的变量。由于本文不使用极小值或动态规划的方法求解该问题（通常无解），而是采用离散化近似的方法求解该问题，因此需要根据式（1）得到离散化形式的状态方程，如式（2）所示。
$$\begin{array}{l}{x}_n=x_{n-1}+v_{n}Tcos({\theta }_n)\\ y_n=y_{n-1}+v_{n}Tsin({\theta }_n)\\ {\theta }_n={\theta }_{n-1}+w_{n}T\\ v_n=v_{n-1}+a_{vn}T\\ w_n=w_{n-1}+a_{wn}T\end{array}\text{\hspace{1em}(2)}$$

目标函数

  在定义目标函数前，需要确定优化的目标，比如当机器人到达终点时，机器人和目标点的位姿偏差尽可能小，同时速度和加速度在约束范围内。由此可定义目标函数如式（3）所示。
$$\begin{array}{l}J=nT+(x_n-x_g{)}^2+(y_n-y_g{)}^2+({\theta }_n-{\theta }_g{)}^2\\ x_0=x_s,y_0=y_s,{\theta }_0={\theta }_s,v_0=0,w_0=0\\ v_n=0,w_n=0\\ 0\le v_k\le v_{max},k=0,1,...,n\\ -a_{vmax}\le a_{vn}\le a_{vmax}\\ -a_{wmax}\le a_{wn}\le a_{wmax}\end{array}\text{\hspace{1em}(3)}$$
  式（3）中，$(x_s,y_s,{\theta }_s)$ 为起始点位姿，$(x_g,y_g,{\theta }_g)$为终点位姿。当将$(a_{v0},a_{w0},a_{v1},a_{w1},...,a_{vn},a_{wn},T)$作为变量时，可以式（2）计算相关的状态，也就得到非线性离散状态的目标函数。

实例求解

由于方程（3）是一个多变量非线性优化的问题，本文将使用casadi求解器进行求解，并用ROS当中的rviz进行可视化显示，实际程序如下所示，其中以（-1,1,0）为起点，（0,0,0）为终点。

# -*- coding: utf-8 -*-
import numpy
import casadi as ca
import matplotlib.pyplot as plt
import math
import rospy
import numpy as np
import time
from geometry_msgs.msg import Twist
from sensor_msgs.msg import PointCloud
from geometry_msgs.msg import Point32
from geometry_msgs.msg import PoseWithCovarianceStamped
from geometry_msgs.msg import PoseStamped

fig = plt.figure()
ax1 = fig.add_subplot(111)
#ax1.set_title('Fig')
plt.xlabel('x(m)')
plt.ylabel('y(m)')

g_pub_clothoid = rospy.Publisher('/trajectory', PointCloud, queue_size = 2)

def solveProblem():
    print_time = rospy.Time().now().to_sec()

    N = 51
    U = ca.SX.sym('U', N)
    x0 = ca.SX.sym('x0', 5)    #初始条件
    x0[0] = -1
    x0[1] = 1
    x0[2] = 0
    x0[3] = 0
    x0[4] = 0
    xf = ca.SX.sym('xf', 5)    #终端状态
    xf[0] = x0[0]
    xf[1] = x0[1]
    xf[2] = x0[2]
    xf[3] = x0[3]
    xf[4] = x0[4]
    obj = U[-1]
    g = []
    det_t = ca.SX.sym('det_t', 1)
    det_t = U[-1] / ((N - 1) / 2)
    for i in range((N - 1) / 2):
        xf[4] += det_t * U[2 * i + 1]
        xf[3] += det_t * U[2 * i]
        xf[2] += det_t * xf[4]
        xf[1] += det_t * xf[3] * ca.sin(xf[2])
        xf[0] += det_t * xf[3] * ca.cos(xf[2])
    g.append(xf[0])
    g.append(xf[1])
    g.append(xf[2])
    g.append(xf[3])
    g.append(xf[4])
    nlp_prob = {'f': obj, 'x': U, 'g': ca.vertcat(*g)}
    opts_setting = {'ipopt.max_iter':100, 'ipopt.print_level':0, 'print_time':0, 'ipopt.acceptable_tol':1e-8, 'ipopt.acceptable_obj_change_tol':1e-6}
    solver = ca.nlpsol('solver', 'ipopt', nlp_prob, opts_setting)
    lbx = []
    ubx = []
    lbg = []
    ubg = []
    for i in range((N - 1) / 2):
        lbx.append(-1)
        ubx.append(1)
        lbx.append(-3)
        ubx.append(3)
    lbx.append(0)
    ubx.append(100)
    for i in range(5):
        lbg.append(0)    #终端约束
        ubg.append(0)    #终端约束
    res = solver(lbx = lbx, ubx = ubx, lbg = lbg, ubg = ubg)

    print('cost time of casadi opt problem: ', rospy.Time().now().to_sec() - print_time)
    print(res['f'], res['x'])
    #print(xf)
    UU = res['x']

    points = PointCloud()
    points.header.frame_id = 'map'
    xf[0] = x0[0]
    xf[1] = x0[1]
    xf[2] = x0[2]
    xf[3] = x0[3]
    xf[4] = x0[4]
    points.points.append(Point32(xf[0], xf[1], 0))
    det_t = ca.SX.sym('det_t', 1)
    det_t = UU[-1] / ((N - 1) / 2)
    for i in range((N - 1) / 2):
        xf[4] += det_t * UU[2 * i + 1]
        xf[3] += det_t * UU[2 * i]
        xf[2] += det_t * xf[4]
        xf[1] += det_t * xf[3] * ca.sin(xf[2])
        xf[0] += det_t * xf[3] * ca.cos(xf[2])
        points.points.append(Point32(xf[0], xf[1], 0))
    g_pub_clothoid.publish(points)
    # #while not rospy.is_shutdown():
    for i in range(100000):
        g_pub_clothoid.publish(points)
        #rospy.sleep(0.1)
    print('end pub')
    

if __name__ == '__main__':
    print('opt control test.')
    rospy.init_node("opt_control_test", anonymous = True)
    solveProblem()
    rospy.spin()

实验效果如下图所示：

注意事项

1. 本文只提供一个近似的解；
2. 变量的个数越多，求解越经精确，但计算量也就越大；
3. 需要在ROS环境下运行；
4. 需要安装casadi库；
5. 当前计算实时性非常差，不适合做为路径跟踪控制器；